//EEPROM Stuff
#include <EEPROMex.h>
#include <EEPROMvar.h>
const int maxAllowedWrites = 20;
const int memBase          = 100;

//Timer
#include "TimerOne.h"


//------------------------ \/ Encoder \/ ------------------------//
#include <Encoder.h>
#define BUTTON 2
#define ENC0 3
#define ENC1 4
Encoder myEnc(ENC0, ENC1);
long oldPosition  = 0;
boolean button = false;
boolean up = false;
boolean down = false;
boolean middle = false;
int16_t last, value;
//------------------------ /\ Encoder /\ ------------------------//

//Random
#include <stdint.h>
bool debug = true;
bool skipCalibration = true;


//------------------------ \/ LCD Stuff \/ ------------------------//
#include <Adafruit_GFX.h>
#include <Adafruit_PCD8544.h>
#include <gfxfont.h>
Adafruit_PCD8544 display = Adafruit_PCD8544 (8, 9, 10, 11, 12);
//------------------------ /\ LCD Stuff /\ ------------------------//



//Runtime Vars
long day = 86400000; // 86400000 milliseconds in a day
long hour = 3600000; // 3600000 milliseconds in an hour
long minute = 60000; // 60000 milliseconds in a minute
long second =  1000; // 1000 milliseconds in a second
volatile long numruns = 0;

unsigned long totalLength = 143000;

volatile long period = 10000;
long runtime = 0;
#define MINRUNTIME 10000


//Stepper Motor 
#define DIR 6
#define STEP 7
#define STEPSMM 60
#define MS1 A0
#define MS2 A1
#define MS3 A2
#define StepperEnable A3
boolean sdir = 0;
boolean oldsdir = !sdir;
boolean timerSet = false;
#define DirLeft 0
#define DirRight 1
#define DirHome 0

//Endstop
int endStop = 5;

#define DEBOUNCE 300
volatile long lastTriggered = 0;

long offset = 0;
long sspeed = 0;
long decimals = 0;

byte oldDays = 0;
byte oldHours = 0;
byte oldMinutes = 0;
byte oldSeconds = 0;

boolean srunning = false;

//------------------------\/ Machine State Driver \/------------------------//
typedef enum State {
  Boot,
  Calibration,
  Menu,
  Settings,
  Running,
  Pause,
  Finished,
  Low_Battery
};
State currentState;

typedef enum Mode {
  LeftStart,
  RightStart,
  Ping_Pong
};
Mode currentMode;

//------------------------/\ Machine State Driver /\------------------------//

void catchButton() {
  if (lastTriggered + DEBOUNCE < millis()) {
    button = true;
    middle = true;
    lastTriggered = millis();
    Serial.print("BUTTON TRIG, DIGITAL STATE: ");
    Serial.println(digitalRead(BUTTON));
  }
}

void setup() {
  if (debug == true){
    Serial.begin(115200);
    Debug();
  }
  
  StepperInit();
  EncoderInit();
  TimerInit();
  
}


void TimerInit (){
  Timer1.initialize(period);         // initialize timer1, and set a 1/2 second period
  Timer1.attachInterrupt(callback);  // attaches callback() as a timer overflow interrupt
  Timer1.stop();
}

void EncoderInit (){
  digitalWrite(BUTTON, HIGH);
  attachInterrupt(digitalPinToInterrupt(BUTTON), catchButton, FALLING);
}

void StepperInit (){
  pinMode(MS1, OUTPUT);
  pinMode(MS2, OUTPUT);
  pinMode(MS3, OUTPUT);
  digitalWrite(MS1, HIGH);
  digitalWrite(MS2, HIGH);
  digitalWrite(MS3, HIGH);

  pinMode(STEP, OUTPUT);
  pinMode(DIR, OUTPUT);
  digitalWrite(DIR, sdir);

  pinMode(StepperEnable, OUTPUT);

  //EndStop
  pinMode(endStop, INPUT_PULLUP);
}


void callback(){
  
  numruns++;
  digitalWrite(STEP, digitalRead(STEP) ^ 1);
  if (currentState != Calibration){

    if (numruns >= totalLength){
      
      if (sdir == DirLeft){
        SetDirection(DirRight);
      }
      else if (sdir == DirRight){
        SetDirection(DirLeft);
      }
      digitalWrite(DIR, sdir);
      numruns = 0;
      srunning = false;
      Timer1.stop();
    }
  }

}

void loop() {
  EncoderLoop();
}


void EncoderLoop(){
 
  long newPosition = myEnc.read();
  //Serial.println(newPosition);
  if ((newPosition < 0) && (newPosition < offset)) {
    offset = newPosition;
  }
  newPosition = newPosition - offset;

  if (newPosition != oldPosition) {
    oldPosition = newPosition;
    updateRuntime();
    setPeriod();
  }

  if (button) {
    if ((sspeed == 0) && (decimals == 0)) {
      srunning = !srunning;
      setPeriod();
    }
    else {
      srunning = !srunning;
      setPeriod();
    }
    button = false;
  }
}


void updateRuntime() {
  if (oldPosition != 0) {
    runtime = 1000 * oldPosition * oldPosition / 4;
    //runtime = runtime/3;
    int days = runtime / day ;                                //number of days
    int hours = (runtime % day) / hour;                       //the remainder from days division (in milliseconds) divided by hours, this gives the full hours
    int minutes = ((runtime % day) % hour) / minute ;         //and so on...
    int seconds = (((runtime % day) % hour) % minute) / second;
    
    Serial.print("Days: ");
    Serial.print(days);
    Serial.print(" / Hours: ");
    Serial.print(hours);
    Serial.print(" / Minutes: ");
    Serial.print(minutes);
    Serial.print(" / Seconds: ");
    Serial.println(seconds);
  }
}


void setPeriod() {
  Serial.print("Runtime: ");
  Serial.println(runtime);
//  Serial.print(" / sRunning: ");
//  Serial.println(srunning);
//  Serial.print(" / Number of Runs: ");
//  Serial.println(numruns);
  if ((runtime < MINRUNTIME) | ! srunning) {
    Timer1.stop();
  }
  else {
    float ssspeed = 1000000 / ((float) runtime);
    float sps = ssspeed * STEPSMM;
    float pperiod = 500000 / sps;
    Timer1.setPeriod(pperiod);
    //Serial.println(pperiod);
  }
}
//----------------------------------------State Change----------------------------------------//
void ChangeState (State state){
  Serial.print("Changing State from: ");
  Serial.print(currentState);
  Serial.print(" to: ");
  Serial.println(state);
  currentState = state;
}


void ChangeMode (Mode mode){
  Serial.print("Changing Mode from: ");
  Serial.print(currentMode);
  Serial.print(" to: ");
  Serial.println(mode);
  currentMode = mode;
}

void CalibrationStateChange (CalibrationState state){
  Serial.print("Changing Calibration State from: ");
  Serial.print(calibrationState);
  Serial.print(" to: ");
  Serial.println(state);
  calibrationState = state;
}

//----------------------------------------State Change----------------------------------------//

//----------------------------------------\/ Stepper Helpers \/----------------------------------------//
void StopStepper(){
  Timer1.stop();
  srunning = false;
  Serial.println("Stopping Timer");
}

void ZeroPosition (){
  //distanceTravelled = 0;  
  numruns = 0;
  Serial.println("Zeroing Position");
}

void SetPosition (long value){
  distanceTravelled = value;
}

void SetDirection (int value){
  sdir = value;
  digitalWrite(DIR, value);
  Serial.print("Direction set to: ");
  Serial.println(value);
}
//----------------------------------------/\ Stepper Helpers /\----------------------------------------//

//----------------------------------------\/ Debug Stuff \/----------------------------------------//
void Debug(){
    Serial.println("----------------------------------------------");     
    Serial.println("Starting Camera Slider Serial Debug");     
    Serial.println("----------------------------------------------");  

    Serial.println("----------------------------------------------");     
    Serial.print("Checking if we have any remaining distance: ");  
//    if (remainingDistance > 0){
//      Serial.println(remainingDistance);   
//    }
//    else{
//      Serial.println("No remaining distance found. Looks like we are good to go!"); 
//    }
    Serial.println("----------------------------------------------");  
    
}
//----------------------------------------/\ Debug Stuff /\----------------------------------------//