#include <LiquidCrystal.h>
#include "SdFat.h"

#define SPI_SPEED SD_SCK_MHZ(4)
#define CS_PIN 53
SdFat sd;
File myLogFile;

const int rs = 37, en = 36, d4 = 35, d5 = 34, d6 = 33, d7 = 32; //pins for lcd display
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);//6 pins used

int startStop = 2;//start stop switch pin
int btnUP = 4;//Up button pin
int btnDN = 6;//Down button pin

int motorF = 8;//relay pin for forward motion (non pwm)
int motorB = 9;//relay pin for backward motion (non pwm)
int motorFpwm = 10;//relay pin for forward motion (with pwm speed control)
int motorBpwm = 11;//relay pin for backward motion (with pwm speed control)

int limitUP = 26;//upper limit switch pin
int limitDN = 28;//lower limit switch pin
int limitreturn = 30;//return (gnd) wire from  limit switches. keep low.

int bzr = 44;//buzzer pin (not currently used)

int IsensePin = A0;//analog input for current sense


int motorSpeed = 10;//motor speed 1(0%) to 10(100%)
long waitInc = 6000;//ammount(ms) to increment waitTime
long waitTime = 30*waitInc;//time to wait between up and down moves
long timeout = 15000;//max move time(ms). will stop test if limit switch is not hit within this time

int IsenseT = 500;//sample interval for current sensor
int IsenseN = 0;//number of samples to be averaged
int cycles = 0;
int runTest = 0;//flag to quit test
int Direction = 0;//current motor direction (Forward: 1, stopped: 0, Reverse: -1)
int nextDirection = 0;//used to schedule next movement after wait period

float Inow = 0;//for current sense value
float Isum = 0;//cumulative sum of current sensor samples
float Ihigh = 0;//max sample in average
float Ilow = 1023;//min sample in average

unsigned long nextMotorStart = 0;//scheduled millis() time to start the next movement
unsigned long lastMotorStart = 0;//timestamp when motor is last started (used for timeout check)
unsigned long limitUPtime = 0;//time stamp for when upper limit was hit
unsigned long limitDNtime = 0;//time stamp for when lower limit was hit
unsigned long testStart = 0;//time of test start
unsigned long IsenseTlast = 0;//time of last current sensor sample

char cyclestr[10];//used to display formatted cycle count
char Inowstr[10];//used to display formatted current
char logHeader [] = "timestamp(ms), Direction, cycle number, cycle T(s), cycle sample qty, cycle Iavg(A), cycle Ihigh(A), cycle Ilow(A)";
char delayStr[10];//used to display cooldown wait time
float delayTmDisp = 0;//used to display delay time

//void updateDelay (int,char);
void setup() {
  // put your setup code here, to run once:
  
  pinMode(motorF, OUTPUT);
  pinMode(motorB, OUTPUT);
  pinMode(motorFpwm, OUTPUT);
  pinMode(motorBpwm, OUTPUT);
  pinMode(bzr, OUTPUT);
  pinMode(startStop, INPUT_PULLUP);
  pinMode(limitUP, INPUT_PULLUP);
  pinMode(limitDN, INPUT_PULLUP);
  pinMode(btnUP, INPUT_PULLUP);
  pinMode(btnDN, INPUT_PULLUP);
  pinMode(limitreturn, OUTPUT);
  pinMode(IsensePin, INPUT);
  motor(0,0);
  digitalWrite(limitreturn, LOW);

  lcd.begin(16, 2);// you can now interact with the LCD, e.g. lcd.print("Hello World");
  Serial.begin(115200);
  delay(200);

  if (!sd.begin(CS_PIN, SPI_SPEED)) {
    if (sd.card()->errorCode()) {
      Serial.println("SD initialization failed.");
      lcd.print("SD init Fail");
    } else if (sd.vol()->fatType() == 0) {
      Serial.println("Can't find a valid FAT16/FAT32 partition.");
      lcd.print("SD not formated");
    } else {
      Serial.println("SD card error, Can't determine error type");
      lcd.print("SD error");
    }
    //while(digitalRead(startStop));//wait until start/stop button is manually pressed to start test
    delay(200);
    return;
  }

  Serial.println("Files on card:");
  Serial.println("      Size Name");
  sd.ls(LS_R | LS_SIZE);

  // open the file. note that only one file can be open at a time,
  // so you have to close this one before opening another.
  myLogFile = sd.open("test.txt", FILE_WRITE);
  if (myLogFile) {
    Serial.print("Writing header to test.txt...");
    myLogFile.println(logHeader);
    myLogFile.close();// close the file:
    Serial.println("done.");
  } else {
    Serial.println("error opening test.txt");// if the file didn't open, print an error:
  }
  Serial.println("Files on card:");
  Serial.println("      Size Name");
  sd.ls(LS_R | LS_SIZE);
  
  // Initial LCD display: prompt to set delay time between movements
  lcd.clear();
  lcd.print("Set delay: ");
  updateDelay(0,0,'m');//(int increment, int remainder, char unit)
  //lcd.setCursor(0,1);//(column pos,row pos)
  //lcd.print("set delay");
  //lcd.print(waitTime);
  while(digitalRead(startStop)){//wait until start/stop button is manually pressed to start test
    if(!digitalRead(btnUP)) updateDelay(1,0,'m');
    if(!digitalRead(btnDN)) updateDelay(-1,0,'m');
  }
  delay(1000);

  // prompt to set timemout
  lcd.clear();
  lcd.print("Set timeout: 15s");
  while(digitalRead(startStop)){//wait until start/stop button is manually pressed to start test
    if(!digitalRead(btnUP)) changeTimeout(1);
    if(!digitalRead(btnDN)) changeTimeout(-1);
  }
  delay(1000);


  // move creeper to lowered position at start of test
  testStart = millis();
  runTest = 1;
  limitUPtime = testStart;
  Direction = -1;//set move direction to lower the creeper
  lcd.clear();
  //lcd.setCursor(0,0);//(column pos,row pos)
  lcd.print("C: 00000, d");
  updateDelay(0,0,'s');//(int increment, int remainder, char unit)
  lcd.setCursor(0,1);//(column pos,row pos)
  lcd.print("I:__._A,        ");
  logMsg("Test Started");
  motor(Direction,motorSpeed);
}

void loop() {
  // put your main code here, to run repeatedly:
  if(!digitalRead(startStop)){//if startstop button pushed, manualy stop or resume test
    delay(50);
    while(!digitalRead(startStop)){//wait until start/stop button is released
    }
    if(runTest){//manually stop test
      motor(0,0);
      lcd.setCursor(0,1);//(column pos,row pos)
      lcd.print("Test Ended     ");
      logMsg("Test Stopped");
      runTest = 0;
      myLogFile = sd.open("test.txt");// re-open the file for reading:
      if (myLogFile) {
        Serial.println("test.txt:");
        while (myLogFile.available()) {// read from the file until there's nothing else in it:
          Serial.write(myLogFile.read());// dump log file to serial output
        }
        myLogFile.close();// close the file
      } else {
        // if the file didn't open, print an error:
        Serial.println("error opening test.txt");
      }
    }else{//resume test
      lcd.setCursor(0,1);//(column pos,row pos)
      lcd.print("Resume?         ");
      updateDelay(0,0,'m');
      while(digitalRead(startStop)){//wait until start/stop button is manually pressed to start test
        if(!digitalRead(btnUP)) updateDelay(1,0,'m');
        if(!digitalRead(btnDN)) updateDelay(-1,0,'m');
      }
      runTest = 1;
      logMsg("Test Resumed");
      lcd.setCursor(0,1);//(column pos,row pos)
      lcd.print("I:__._A,        ");
      delay(1000);
    }//end else resume test
  }//endif startstop
  if(!digitalRead(btnUP)) updateDelay(1,0,'s');
  if(!digitalRead(btnDN)) updateDelay(-1,0,'s');
  if (runTest){
    if(!digitalRead(limitDN)&&!digitalRead(limitUP)){//limit switch error check
      motor(0,0);
      tone(bzr,554,1000);
      //lcd.clear();
      lcd.setCursor(0,1);//(column pos,row pos)
      lcd.print("Error: Limits   ");
      logMsg("Error: both limit switches activated at same time");
      runTest = 0;
      return;//exit loop() iteration
    }//endif limit switch error
    
    if ((Direction != 0 ) && (millis() > (lastMotorStart + timeout))){//timeout error check
      motor(0,0);
      lcd.setCursor(0,1);//(column pos,row pos)
      lcd.print("Error: Timed Out");
      logMsg("Error: Movement timed out");
      runTest = 0;
      return;//exit loop() iteration
    }

    //***********************************************************MAIN TEST CYCLE
    if(!digitalRead(limitDN)&&(Direction==-1)){//At bottom limit
      motor(0,0);//stop motor
      limitDNtime = millis();
      printdata(limitDNtime, -1, cycles, limitUPtime+waitTime, IsenseN, Isum, Ihigh, Ilow);
      //(unsigned long tstamp, int dir, int cn, unsigned long tCStart, int IsenseN, float Isum, float Ihigh, float Ilow){//direction -1 down, 1 up
      //timestamp,direction, cycle, cycleStartTime, IsenseN, Isum, Ihigh, Ilow, 
      nextDirection = 1;// set direction for ntext move to upwards
      nextMotorStart = limitDNtime + waitTime; //schedule next move with time for motor to cool
    }//endif limit down

    if(!digitalRead(limitUP)&&(Direction==1)){//at upper limit
      motor(0,0);//stop motor, set direction to 0
      limitUPtime = millis();
      printdata(limitUPtime, 1, cycles, limitDNtime+waitTime, IsenseN, Isum, Ihigh, Ilow);
      //(unsigned long tstamp, int dir, int cn, unsigned long tCStart, int IsenseN, float Isum, float Ihigh, float Ilow){//direction -1 down, 1 up
      nextDirection = -1;//set direction for next move to downwards
      nextMotorStart = limitUPtime + waitTime; //schedule next move with time for motor to cool
    }//endif limit up

    if(Direction == 0){//during wait time, count down timer then start next cycle
      if(millis() > nextMotorStart){//check if time to start next cycle
        updateDelay(0,0,'m');//(int increment, int remainder, char unit)
        Isum = 0;// reset cumulative data vairables for next cycle
        IsenseN = 0;
        Ihigh = 0;
        Ilow = 1023;
        motor(nextDirection,motorSpeed);//start motor in next direction
        delay(200);
      }//endif next motor start
      else updateDelay (0,1,'s');
    }//endif during wait time

    if((millis()>(IsenseTlast+IsenseT)) && (Direction != 0)){//Measure current during motor run time
      IsenseTlast = millis();
      Inow = map(analogRead(IsensePin), 0, 255, 0, 10);
      Isum += Inow;
      IsenseN++;
      if (Inow>Ihigh) Ihigh = Inow;
      if (Inow<Ilow) Ilow = Inow;
      //update displayed current
      dtostrf(Inow,4,1,Inowstr);//(width,decimals)format to string
      lcd.setCursor(2,1);//(column pos,row pos)
      lcd.print(Inowstr);
      if (Inow < 0.1){//low current error check
        motor(0,0);
        lcd.setCursor(0,1);//(column pos,row pos)
        lcd.print("Error: Low Amps ");
        logMsg("Error: low amps");
        runTest = 0;//stop test
        return;//exit loop() iteration
      }
      if (Inow > 30.0){//over current error check (dont want to exceed 30 amps)
        motor(0,0);
        lcd.setCursor(0,1);//(column pos,row pos)
        lcd.print("Error: ");
        lcd.print(Inow);
        lcd.print("A");
        logMsg("Error: high amps");
        runTest = 0;//stop test
        return;//exit loop() iteration
      }
    }//endif Isense during motor run time
  }//endif runTest
}//loop

void updateDelay (int increment, int remainder, char unit){//increase (increment == 1) or decrease delay (increment == -1) time by 0.1 minute or display current delay (increment == 0)
  if(increment == 1){
    waitTime += waitInc;//increase delay by 0.1 minute (6 seconds)
    nextMotorStart += waitInc;//adjust nextMotorStart accordingly
  } 
  if((increment == -1) && (waitTime >= waitInc)){
    waitTime -= waitInc;//decrease delay by 0.1 minute
    nextMotorStart -= waitInc;//adjust nextMotorStart accordingly
  } 
  if(remainder){//if display remaining wait time for this cycle
    if(unit == 'm'){
      delayTmDisp = double(nextMotorStart - millis())/60000;//waitTime in minutes
      dtostrf(delayTmDisp,4,1,delayStr);//format delay time to 1 decimal precision
      strcat(delayStr, "m");
    } 
    if(unit == 's'){
      delayTmDisp = double(nextMotorStart - millis())/1000;//waitTime left in seconds
      dtostrf(delayTmDisp,4,0,delayStr);//format delay time to 0 decimal precision
      strcat(delayStr, "s");
    }
  }//endif remainder
  else{//if display full wait time
    if(unit == 'm'){
        delayTmDisp = double(waitTime)/60000;//waitTime in minutes
        dtostrf(delayTmDisp,4,1,delayStr);//format delay time to 1 decimal precision
        strcat(delayStr, "m");
      } 
      if(unit == 's'){
        delayTmDisp = double(waitTime)/1000;//waitTime in seconds
        dtostrf(delayTmDisp,4,0,delayStr);//format delay time to 0 decimal precision
        strcat(delayStr, "s");
      }
  }
  lcd.setCursor(11,0);//(col,row) 1st row, 12th character
  lcd.print(delayStr);
  delay(200);
  return;
}


void changeTimeout (int direction){//increase or decrease timeout time by 1 second
  long timeoutTm = 0;//used to display timeout time
  if(direction == 1) timeout += 1000;//increase delay by 1 second
  else if(timeout >= 2000) timeout -= 1000;//decrease delay by 1 second (min value 1 second)
  timeoutTm = timeout/1000;//timeout in seconds
  lcd.setCursor(13,0);//(col,row) 1st row, 11th character
  lcd.print(timeoutTm);
  delay(200);
  return;
}


void motor (int direction, int speed){//set motor direction (1:forward, 0:off, -1:backwards) and speed (1-10)
  speed = map(speed, 0, 10, 0, 255);
  Direction = direction;//set global direction variable for outside reference
  if(direction==0){
    digitalWrite(motorF, LOW);
    digitalWrite(motorB, LOW);
    digitalWrite(motorFpwm, LOW);
    digitalWrite(motorBpwm, LOW);
    lcd.setCursor(15,1);//(column pos,row pos)
    lcd.print(" ");
    lcd.setCursor(2,1);//(column pos,row pos)
    lcd.print("00.0");
  } else if (direction==1){
    digitalWrite(motorB, LOW);
    digitalWrite(motorBpwm, LOW);
    delay(50);//wait for relays to disconnect
    digitalWrite(motorF, HIGH);
    analogWrite(motorFpwm, speed);
    lastMotorStart = millis();
    cycles++;//Increment cycle count
    dtostrf(cycles,5,0,cyclestr);//(width,decimals)format to string
    lcd.setCursor(3,0);//(column pos,row pos)
    lcd.print(cyclestr);//update displayed cycle count
    lcd.setCursor(15,1);//(column pos,row pos)
    lcd.print("F");
  } else if (direction==-1){
    digitalWrite(motorF, LOW);
    digitalWrite(motorFpwm, LOW);
    delay(50);//wait for relays to disconnect
    digitalWrite(motorB, HIGH);
    analogWrite(motorBpwm, speed);
    lastMotorStart = millis();
    lcd.setCursor(15,1);//(column pos,row pos)
    lcd.print("B");
  }
  return;
}

void printdata(unsigned long tstamp, int dir, int cn, unsigned long tCStart, int IsenseN, float Isum, float Ihigh, float Ilow){//direction -1 down, 1 up
  //timestamp, Direction, cycle number, cycle T, cycle sample qty, cycle Iavg, cycle Ihigh, cycle Ilow
  //(limitDNtime, -1, cycles, limitUPtime+waitTime, IsenseN, Isum, Ihigh, Ilow)
  float Iavg = Isum/float(IsenseN);
  //logHeader [] = "timestamp, Direction, cycle number, cycle T, cycle sample qty, cycle Iavg, cycle Ihigh, cycle Ilow";
  myLogFile = sd.open("test.txt", FILE_WRITE);
  myLogFile.print(tstamp);
  myLogFile.print(",");
  if(dir<0) myLogFile.print("down");
  if(dir>0) myLogFile.print("up");
  myLogFile.print(",");
  myLogFile.print(cn);
  myLogFile.print(",");
  myLogFile.print(long(tstamp-tCStart)/1000);
  myLogFile.print(",");
  myLogFile.print(IsenseN);
  myLogFile.print(",");
  myLogFile.print(Iavg);
  myLogFile.print(",");
  myLogFile.print(Ihigh);
  myLogFile.print(",");
  myLogFile.println(Ilow);
  myLogFile.close();// close the file:
  return;
}
void logMsg(String msg){
  myLogFile = sd.open("test.txt", FILE_WRITE);
  myLogFile.print(millis());
  myLogFile.print(",");
  myLogFile.println(msg);
  myLogFile.close();// close the file:
  return;
}