/*
 Last Lock, Inc.
 Key Test aperatus
 
 Last Update: Mar. 2021

*/

#include <AccelStepper.h>

#include <MultiStepper.h>

#include <LiquidCrystal_I2C.h>

// Motor Pins
#define M1_DIR 8 // Yellow
#define M1_PUL 9 // Blue

#define M2_DIR 10 // Yellow
#define M2_PUL 11 // Blue

#define motorInterfaceType 1 // 0 = Function, 1 = Driver, 2 = Full2Wire, .... 8 = Half4Wire

// General Pins
// const int STOP = 18; //INPUT_PULLUP      inturupt pin
const int START = 19; //INPUT_PULLUP     inturupt pin
const int LIMIT_MIN = 2; //INPUT_PULLUP   inturupt pin
const int LIMIT_MAX = 3; //INPUT_PULLUP   inturupt pin
//const int LED_R = 22;
//const int LED_G = 23;

bool active = false;
//bool rotation = false;
int cycleCount = 0;
int phaseState = 0;
int pauseState = 0;

int linearTarget = 0;
int linearMax = 0;
int linearHome = 0;
int linearMin = 0;

int rotationTarget = 0;
int rotationHome = 0;

int delayTime = 0;

String statusMessage = "";
String cycleMessage = "";

/*
  The circuit:
 * LCD RS pin to digital pin 12       not pwm
 * LCD Enable pin to digital pin 11   pwm
 * LCD D4 pin to digital pin 5        pwm
 * LCD D5 pin to digital pin 4        not pwm
 * LCD D6 pin to digital pin 3        pwm
 * LCD D7 pin to digital pin 2        not pwm
 * LCD R/W pin to ground
 * LCD VSS pin to ground
 * LCD VCC pin to 5V
 * 10K resistor:
 * ends to +5V and ground
 * wiper to LCD VO pin (pin 3)
 * */

// initialize the library by associating any needed LCD interface pin
// with the arduino pin number it is connected to
// const int rs = 9, en = 8, db4 = 7, db5 = 6, db6 = 5, db7 = 4;
// LiquidCrystal lcd(rs, en, db4, db5, db6, db7);

LiquidCrystal_I2C lcd(0x27, 20, 4); // set the LCD address to 0x27 for a 16 chars and 2 line display

// Create a new instance of the AccelStepper class:
AccelStepper M1 = AccelStepper(motorInterfaceType, M1_PUL, M1_DIR);
AccelStepper M2 = AccelStepper(motorInterfaceType, M2_PUL, M2_DIR);

void setup() {

  // initialize the serial port:
  Serial.begin(9600);

  // Set the maximum speed and acceleration:
  M1.setMaxSpeed(20000);
  M1.setAcceleration(8000);
  M2.setMaxSpeed(12000);
  M2.setAcceleration(8000);

  linearTarget = 10000;
  linearMax = linearTarget + 50;
  linearMin = linearHome - 50;

  rotationTarget = 3200;


  // Lights
  //pinMode(LED_R, OUTPUT);
  //pinMode(LED_G, OUTPUT);

  // User Controls
  pinMode(START, INPUT_PULLUP);
  // pinMode(STOP, INPUT_PULLUP);
  // attachInterrupt(digitalPinToInterrupt(STOP), Pause_Button, CHANGE);

  // Machine Controls
  pinMode(LIMIT_MIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(LIMIT_MIN), Limit_Home, CHANGE);
  pinMode(LIMIT_MAX, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(LIMIT_MAX), Limit_Maximum, CHANGE);

  // Initialize LED Indicator
  //digitalWrite(LED_R, HIGH);
  //digitalWrite(LED_G, LOW);

  // set up the LCD's number of columns and rows:
  lcd.begin(16, 2);
  // Print a message to the LCD.
  statusMessage = "Last Lock, Inc.";
  Status_Refresh();
  LCD_Cycle_Refresh();
  Serial.println("\nPress Start to Begin\n");
  active = false;

}

/*==========================================
The main(); of Arduino. 
I don't know why I'm explaining this.
==========================================*/

void loop() {
  // step one revolution  in one direction:
  // lcd.setCursor(0, 0);

  if (digitalRead(START) == LOW && phaseState == 0) {
    statusMessage = "Beginning";
    Status_Refresh();
    phaseState = 1;
    // pauseState = 0;

    linearTarget = 10000;
    rotationTarget = 3200;
    delayTime = 1000;

    //digitalWrite(LED_R, LOW);
    //digitalWrite(LED_G, HIGH);
    //rotation = true;

  } 



    // This is where the cycle tester *does what it is supposed to do*
    //    there will be 4 states that cycle, so that in between steps, it can check for inputs
    //    without me having to rewrite all that stuff 4 times

  switch (phaseState) {

    
    case 1:
      Move_To_Lock();
      phaseState = 2;

      break;
    case 2:
      Turn_Lock_Clockwise();
      phaseState = 3;

      break;
    case 3:
      Turn_Lock_Counterclockwise();
      phaseState = 4;

      break;
    case 4:
      Move_Away_From_Lock();
      phaseState = 1;
      
      cycleCount++;
      LCD_Cycle_Refresh();

      break;
    
    // case 5:
    //   while (phaseState == 5 && digitalRead(START) == HIGH) {
    //     statusMessage = "Paused";
    //     //Serial.println("PAUSED");
    //   }

    //   phaseState = pauseState;

    //   break;
    
    // case 6:

    //   break;
    
    // case 7:

    //   break;

    default:
      phaseState = 0;
      Serial.println("This was tripped somehow");
      break;


    // save this value to eeprom
    // Save_To_EEPROM(); //delete this comment here after the method is actually written out

    // print the number of seconds since reset:
    LCD_Cycle_Refresh();
    //delay(delayTime);



  }
}

/*==========================================
These are the 4 phases of the test cycle. 
==========================================*/

void Move_To_Lock() {
  statusMessage = "Moving to lock";
  
  Status_Refresh();

  // Set the target position:
  M1.moveTo(linearTarget);
  // Run to target position with set speed and acceleration/deceleration:

  M1.runToPosition();

  delay(delayTime);
}

void Turn_Lock_Clockwise() {
  statusMessage = "Rotate right";
  
  Status_Refresh();

  M2.moveTo(rotationTarget);
  // Run to target position with set speed and acceleration/deceleration:
  M2.runToPosition();

  delay(delayTime);
}

void Turn_Lock_Counterclockwise() {
  statusMessage = "Rotate left";
  
  Status_Refresh();
  M2.moveTo(rotationHome);
  M2.runToPosition();

  delay(delayTime);
}

void Move_Away_From_Lock() {
  statusMessage = "Return to start";
  
  Status_Refresh();
  // Move back to zero:
  M1.moveTo(linearHome);
  M1.runToPosition();

  delay(delayTime);
}

/*==========================================
I/O Methods [Input]
==========================================*/

// void Pause_Button() {
//   if (phaseState != 5) {
//   pauseState = phaseState;

//   phaseState = 5;

//   }
//   // statusMessage = "Stop Engaged";
//   // Status_Refresh();

//   // linearTarget = 0;
//   // linearHome = 0;

//   // rotationTarget = 0;
//   // rotationHome = 0;
//   // M1.stop();
//   // M2.stop();

//   // delay(delayTime);
//   // delayTime = 0;
//   //digitalWrite(LED_R, HIGH);
//   //digitalWrite(LED_G, LOW);
// }

void Limit_Home() {
  // statusMessage = "MinLimit Error!";
  // Status_Refresh();

  // linearTarget = 0;
  
  // rotationTarget = 0;
  // rotationHome = 0;
  M1.setCurrentPosition(linearMin);

  phaseState = 1;
  // M2.setCurrentPosition(0);

  // delay(delayTime);
  // delayTime = 0;
  //Pause_Button();

  //digitalWrite(LED_R, HIGH);
  //digitalWrite(LED_G, LOW);
}

void Limit_Maximum() {
  //lcd.setCursor(0, 0);
  //lcd.print("maxlimit Error!!");
  //statusMessage = "MaxLimit Error!";
  //Status_Refresh();
  phaseState = 2;


  // linearTarget = 0;
  // linearHome = 0;

  // rotationTarget = 0;
  // rotationHome = 0;
  M1.setCurrentPosition(linearMax);
  // M1.moveTo(linearTarget - 50);
  // M1.runToPosition();
  // M2.setCurrentPosition(0);

  // delay(delayTime);
  // delayTime = 0;
  //digitalWrite(LED_R, HIGH);
  //digitalWrite(LED_G, LOW);
}

/*==========================================
I/O Methods [Output]
==========================================*/

void Status_Refresh() {
  lcd.setCursor(0, 0);
  lcd.print(statusMessage + "                      ");
  Serial.println("\n" + statusMessage + "\n");
}

void LCD_Cycle_Refresh() {
  String cycleCountString = String(cycleCount);
  lcd.setCursor(0, 1);
  lcd.print("                ");
  lcd.setCursor(0, 1);

  if (cycleCount == 1) {
    cycleMessage = cycleCountString + " Cycle ";
  } else {
    cycleMessage = cycleCountString + " Cycles";
  }
  lcd.print(cycleMessage);
}