/*

  Simplified control and UI in simulator
  1. LED intensity <-> actuator speed
  2. User button <-> limit switch

  From left to right
  Button group 1:
  blue: press to auto close, right side blue LED on; left side blue LED full intensity
  green: press to auto open, right side green LED on; left side green LED full intensity

  Button group 2:
  blue: press to manual close, right side blue LED on; left side blue LED intensity can be adjusted
  green: press to manual open, right side green LED on; left side green LED intensity can be adjusted

  Button group 3:
  white: press to fully close, right side white LED on
  yellow: press to fully open, right side yellow LED on

  Red Button:
  press to E-Stop, red LED on

  Potentiometer:
  Click on the potentiometer before using these keyboard shortcuts.
  Left / Right - fine movement
  Page Up / Page Down - coarse movement
  Home / End - move to the start (0) or the end (1023) of the range

*/


// Enumerate the states for the system
enum States {
  sys_Hold, sys_ManualOpen, sys_ManualClose, sys_AutoOpen, sys_AutoClose
};

// Define the starting state
States systemState = sys_Hold;

//Linear Actuator setup
const int LA_ext = 4;
const int LA_re = 5;

//sensor setup
const int LS_Open = 2;
const int LS_Close  = 3;

const int V_Pin = A0;
int V_reading = 0;
int V_Value = 0;

//motion control variables
boolean contOpen;
boolean contClose;
double O_sp1, O_sp2, O_sp3, O_t1, O_t2, O_t3;
double C_sp1, C_sp2, C_sp3, C_t1, C_t2, C_t3;

unsigned long current_time;


//buttons setup
const int E_Stop = 21;

const int ManualOpen_Pin = 9;
const int ManualClose_Pin = 10;
const int AutoOpen_Pin = 11;
const int AutoClose_Pin = 12;

//LEDs setup
const int Hold_LED  = 14;
const int Open_LED  = 15;
const int Close_LED  = 16;

const int LA_ext_LED  = 17;
const int LA_re_LED  = 18;

//debounce
static unsigned long last_interrupt_time = 0;
unsigned long interrupt_time;

void setup() {
  Serial.begin(9600);

  //LA
  pinMode(LA_ext, OUTPUT);
  pinMode(LA_re, OUTPUT);

  //dial
  pinMode(V_Pin, INPUT);

  //LEDs
  pinMode(Hold_LED, OUTPUT);
  pinMode(Open_LED, OUTPUT);
  pinMode(Close_LED, OUTPUT);
  pinMode(LA_ext_LED, OUTPUT);
  pinMode(LA_re_LED, OUTPUT);

  //buttons
  pinMode(ManualOpen_Pin, INPUT_PULLUP);
  pinMode(ManualClose_Pin, INPUT_PULLUP);
  pinMode(AutoOpen_Pin, INPUT_PULLUP);
  pinMode(AutoClose_Pin, INPUT_PULLUP);

  //Estop and limit switches
  pinMode(E_Stop, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(E_Stop), stopMotion,  FALLING);


  pinMode(LS_Open, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(LS_Open), EndOpen, FALLING);
  pinMode(LS_Close, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(LS_Close), EndClose, FALLING);

  delay(1000);

}



void loop() {


  switch (systemState) {

    case sys_Hold:
      // HOLD state indicators
      Serial.println("System is on HOLD");
      digitalWrite(Hold_LED, HIGH);
      digitalWrite(LA_ext_LED, LOW);
      digitalWrite(LA_re_LED, LOW);


      // make sure Linear actuaor is off - safety redundancy
      analogWrite(LA_ext, 0);
      analogWrite(LA_re, 0);

      contOpen =  false;
      contClose = false;

      //Dwell in hold state until user specifies otherwise
      while (true) {
        // Press manual open button to open door manually with speed adjustment enabled
        if (digitalRead(ManualOpen_Pin) == LOW) {
          contOpen = true;
          systemState = sys_ManualOpen;
          break;
        }
        // Press manual close button to close door manually with speed adjustment enabled
        if (digitalRead(ManualClose_Pin) == LOW) {
          contClose = true;
          systemState = sys_ManualClose;
          break;
        }

        // Press auto open button to open door with predefined speeds
        if (digitalRead(AutoOpen_Pin) == LOW) {
          contOpen = true;
          systemState = sys_AutoOpen;
          break;
        }
        // Press auto close button to close door with predefined speeds
        if (digitalRead(AutoClose_Pin) == LOW) {
          contClose = true;
          systemState = sys_AutoClose;
          break;
        }
      }

      break;


    case sys_ManualOpen:
      // manual open state indicators, pause 3 sec before any motion
      Serial.println("Attention! Door is opening in 2 seconds, please use caution.");
      Serial.println("To adjust with keyboard");
      //Serial.println("Faster: turn clockwise faster.");
      //Serial.println("Slower: turn counter-clockwise.");


      digitalWrite(Hold_LED, LOW);
      digitalWrite(Open_LED, LOW);
      digitalWrite(Close_LED, LOW);
      digitalWrite(LA_ext_LED, HIGH);
      analogWrite(LA_re_LED, LOW);

      delay(2000);

      while (contOpen) {
        //Linear Actuator keeps extending to open the door
        //manual input as LA speed with PWM
        V_reading = analogRead(V_Pin);
        V_Value = map(V_reading, 0, 1023, 0, 255);
        analogWrite(LA_ext, V_Value);

        //debug line
        //Serial.println(V_Value);
        //delay(100);

      }

      systemState = sys_Hold;
      break;


    case sys_ManualClose:
      // manual close state indicators, pause 3 sec before any motion
      Serial.println("Attention! Door is closing in 2 seconds, please use caution.");
      Serial.println("To adjust with keyboard");
      //Serial.println("Faster: turn clockwise faster.");
      //Serial.println("Slower: turn counter-clockwise.");

      digitalWrite(Hold_LED, LOW);
      digitalWrite(Open_LED, LOW);
      digitalWrite(Close_LED, LOW);
      digitalWrite(LA_ext_LED, LOW);
      digitalWrite(LA_re_LED, HIGH);

      delay(2000);

      while (contClose) {
        //Linear Actuator keeps extending to open the door
        //manual input as LA speed with PWM
        V_reading = analogRead(V_Pin);
        V_Value = map(V_reading, 0, 1023, 0, 255);
        analogWrite(LA_re, V_Value);

        //debug line
        //Serial.println("Door is closing with manual control.");


      }

      systemState = sys_Hold;
      break;


    case sys_AutoOpen:
      // auto open state indicators, pause 3 sec before any motion
      Serial.println("Attention! Door is opening in 2 seconds, please use caution.");
      Serial.println("Door will open with set speeds.");

      digitalWrite(Hold_LED, LOW);
      digitalWrite(Open_LED, LOW);
      digitalWrite(Close_LED, LOW);
      digitalWrite(LA_ext_LED, HIGH);
      digitalWrite(LA_re_LED, LOW);

      delay(2000);


      //Linear Actuator extends in 3 stages: 1 rampup, 2 constent, 3 rampdown

      //math for LA speed with PWM : simple low-high-low speed, no acc
      //LA max speed 0.66"/sec
      O_t1 = 3000;
      O_sp1 = 0.3;

      O_t3 = 3000;
      O_sp3 = 0.3;

      O_sp2 = 1;
      O_t2 = (3 - 0.66 * O_sp1 * O_t1 * 0.001 - 0.66 * O_sp3 * O_t3 * 0.001) / (0.66 * O_sp2 * 0.001);

      //debug line
      //Serial.println("Door is opening in auto mode.");

      //3 stage execution
      while (contOpen) {
        current_time = millis();
        while (contOpen && (millis() - current_time) < O_t1 ) {
          analogWrite(LA_ext, O_sp1 * 255);
          Serial.print("");
        }

        current_time = millis();
        while (contOpen && (millis() - current_time) < O_t2 ) {
          analogWrite(LA_ext, O_sp2 * 255);
          Serial.print("");
        }

        current_time = millis();
        while (contOpen && (millis() - current_time) < O_t3) {
          analogWrite(LA_ext, O_sp3 * 255);
          Serial.print("");
        }

        if (contOpen) {
          digitalWrite(Open_LED, HIGH);
          Serial.println("Door fully open.");
        }

        contOpen = false;

        systemState = sys_Hold;
        break;

      }

      break;


    case sys_AutoClose:
      // auto open state indicators, pause 3 sec before any motion
      Serial.println("Attention! Door is closing in 2 seconds, please use caution.");
      Serial.println("Door will close with set speeds.");

      digitalWrite(Hold_LED, LOW);
      digitalWrite(Open_LED, LOW);
      digitalWrite(Close_LED, LOW);
      digitalWrite(LA_ext_LED, LOW);
      digitalWrite(LA_re_LED, HIGH);

      delay(2000);


      //Linear Actuator extends in 3 stages: 1 rampup, 2 constent, 3 rampdown

      //math for LA speed with PWM : simple low-high-low speed, no acc
      //LA max speed 0.66"/sec
      C_t1 = 3000;
      C_sp1 = 0.2;

      C_t3 = 3000;
      C_sp3 = 0.2;

      C_sp2 = 1;
      C_t2 = (3 - 0.66 * 0.001 * O_t1 * O_sp1 - 0.66 * 0.001 * O_t3 * O_sp3) / (0.66 * 0.001 * C_sp2);


      //debug line
      //Serial.println("Door is opening in auto mode.");

      // 3 stage execution
      //3 stage execution
      while (contClose) {
        current_time = millis();
        while (contClose && (millis() - current_time) < C_t1 ) {
          analogWrite(LA_re, C_sp1 * 255);
          Serial.print("");
        }

        current_time = millis();
        while (contClose && (millis() - current_time) < C_t2 ) {
          analogWrite(LA_re, C_sp2 * 255);
          Serial.print("");
        }

        current_time = millis();
        while (contClose && (millis() - current_time) < C_t3) {
          analogWrite(LA_re, C_sp3 * 255);
          Serial.print("");
        }

        if (contClose) {
          digitalWrite(Close_LED, HIGH);
          Serial.println("Door fully closed.");
        }

        contClose = false;

        systemState = sys_Hold;
        break;

      }

      break;

  }

}


// E-stop function
// alert user, disable motion and motion indicating LEDs, bring system back to hold state
void stopMotion() {

  //last_interrupt_time = 0;
  interrupt_time = millis();

  if (interrupt_time - last_interrupt_time > 200) {
    Serial.println("E-stop is pressed.");
    contOpen = false;
    contClose = false;
    analogWrite(LA_ext, 0);
    analogWrite(LA_re, 0);

    digitalWrite(LA_ext_LED, LOW);
    digitalWrite(LA_re_LED, LOW);

    systemState = sys_Hold;
  }
  last_interrupt_time = interrupt_time;
}


//Limit Switch on open side is triggered
void EndOpen() {
  interrupt_time = millis();

  if (interrupt_time - last_interrupt_time > 200) {
    contOpen = false;
    contClose = false;

    analogWrite(LA_ext, 0);
    analogWrite(LA_re, 0);

    digitalWrite(LA_ext_LED, LOW);
    digitalWrite(LA_re_LED, LOW);

    digitalWrite(Open_LED, HIGH);
    Serial.println("Door is detected to be fully open.");
    systemState = sys_Hold;
  }
  last_interrupt_time = interrupt_time;

}

//Limit Switch on close side is triggered
void EndClose() {
  interrupt_time = millis();

  if (interrupt_time - last_interrupt_time > 200) {
    contOpen = false;
    contClose = false;
    analogWrite(LA_ext, 0);
    analogWrite(LA_re, 0);

    digitalWrite(LA_ext_LED, LOW);
    digitalWrite(LA_re_LED, LOW);

    digitalWrite(Close_LED, HIGH);
    Serial.println("Door is detected to be fully closed.");
    systemState = sys_Hold;
  }
  last_interrupt_time = interrupt_time;

}