#include <digitalWriteFast.h>

#include <MsTimer2.h> //for our timing  to manage time properly or multitasking
#include <LiquidCrystal_I2C.h>
#include <Button2.h>


#include <Wire.h>
#include <EEPROM.h>
///////////////////////////////////////////////////////////


#define MODE_BUTTON_PIN  A0
#define SELECT_BUTTON_PIN  A1

#define ANGLE_PIN1  7
#define ANGLE_PIN2  6
#define ANGLE_PIN3  5

#define MOTOR_PIN1 9
#define MOTOR_PIN2 10
#define MOTOR_PIN3 11

#define ROTATOR_MOTOR1 12
#define ROTATOR_MOTOR2 8
/////////////////////////////////////////////////////////////////
Button2 modeButton, selectButton;

/////////////////////////////////////////////////////////////////

LiquidCrystal_I2C lcd(0x27, 20, 4);

volatile unsigned long timer1, timer2, reverseTimer;
byte cuttingMode, rotatorStep, previousAngle;
byte currentAngle, angle;
byte startFlag1, startFlag2, startFlag3;
byte startFlag;
byte rotationDirection;
void timerIsr() { // this are interrupt service //countdown timers
  //routine to turn provide a count down timer
  //this will decrement every 1 millisecond
  if (timer1)
    timer1-- ; // Only decrement if non-zero !!!!!!!!!!!!
  if (timer2)
    timer2-- ;

  if (reverseTimer)
    reverseTimer--;

}


void setup() {
  //secondsPassed = 72;
  lcd.init();                      // initialize the lcd
  lcd.backlight();
  lcd.clear();

  for (byte x = 19; x--; x > -1) {
    lcd.setCursor(x, 0);
    lcd.print(F(" CHERRY  MAE "));
    delay(10);
    lcd.setCursor(19 - x, 0);
    lcd.print(F(" BITASOLO "));
  }

  delay(3000);
  lcd.clear();

  Serial.begin(115200);

  MsTimer2::set(1, timerIsr); // 1ms period
  MsTimer2::start();

  pinMode(MOTOR_PIN1, OUTPUT);
  pinMode(MOTOR_PIN2, OUTPUT);
  pinMode(MOTOR_PIN3, OUTPUT);

  pinMode(ANGLE_PIN1, INPUT_PULLUP);
  pinMode(ANGLE_PIN2, INPUT_PULLUP);
  pinMode(ANGLE_PIN3, INPUT_PULLUP);

  pinMode(ROTATOR_MOTOR1, OUTPUT);
  pinMode(ROTATOR_MOTOR2, OUTPUT);

  modeButton.begin(MODE_BUTTON_PIN);
  selectButton.begin(SELECT_BUTTON_PIN);


  modeButton.setClickHandler(modeButtonHandler);
  modeButton.setLongClickHandler(modeButtonHandler);       // this will only be called upon release
  modeButton.setDoubleClickHandler(modeButtonHandler);

  selectButton.setClickHandler(selectButtonHandler);
  selectButton.setLongClickHandler(selectButtonHandler);       // this will only be called upon release
  selectButton.setDoubleClickHandler(selectButtonHandler);


}

void rotatorRoutine(const byte position) {

  //if not in the right angle
  byte onAngle1 = !digitalRead(ANGLE_PIN1);
  byte onAngle2 = !digitalRead(ANGLE_PIN2);
  byte onAngle3 = !digitalRead(ANGLE_PIN3);
  angle = position + 1;//offset
  //byte currentAngle;
  if (!onAngle3 && !onAngle2 && !onAngle1) {//convert readings to one number
    currentAngle = 0;
  }
  if (!onAngle3 && !onAngle2 && onAngle1) {
    currentAngle = 1;
  }
  if (!onAngle3 && onAngle2 && !onAngle1) {
    currentAngle = 2;
  }
  if (onAngle3 && !onAngle2 && !onAngle1) {
    currentAngle = 3;
  }

  if (angle != previousAngle) { //state change detection
    rotatorStep = 0;
  }
  previousAngle = angle;


  switch (rotatorStep) {
    case 0:
      if (currentAngle == 0) { //check if not positioned in any of the three
        digitalWriteFast(ROTATOR_MOTOR1, HIGH);
        digitalWriteFast(ROTATOR_MOTOR2, LOW); // rotate counterclockwise
        rotatorStep = 1;
      }
      else {
        rotatorStep = 2; //jump to step 2 so ti will
      }

      break;
    case 1:
      if (currentAngle) {//if positioned on an angle
        // digitalWriteFast(ROTATOR_MOTOR1, LOW);
        // digitalWriteFast(ROTATOR_MOTOR2, LOW); // stop immediately

        rotatorStep = 2;
      }
      break;
    case 2:
      if (currentAngle < angle && currentAngle) { // if less than and more than zero
        digitalWriteFast(ROTATOR_MOTOR1, HIGH);
        digitalWriteFast(ROTATOR_MOTOR2, LOW); // turn counterclockwise

      }

      if (currentAngle > angle && currentAngle) { // if greater than and more than zero
        digitalWriteFast(ROTATOR_MOTOR1, LOW);
        digitalWriteFast(ROTATOR_MOTOR2, HIGH); // turn clockwise

      }
      if (currentAngle == angle) {
        if (digitalRead(ROTATOR_MOTOR1)) {
          digitalWriteFast(ROTATOR_MOTOR1, LOW);
        }
        else {
          digitalWriteFast(ROTATOR_MOTOR1, HIGH);
        }
        if (digitalRead(ROTATOR_MOTOR2)) {
          digitalWriteFast(ROTATOR_MOTOR2, LOW);
        }
        else {
          digitalWriteFast(ROTATOR_MOTOR2, HIGH);
        }
        //digitalWriteFast(ROTATOR_MOTOR1, !digitalRead(ROTATOR_MOTOR1));
        // digitalWriteFast(ROTATOR_MOTOR2, !digitalRead(ROTATOR_MOTOR2));
        Serial.println("reversing2");
        rotatorStep = 4; //exit;
        Serial.print(F("arrived on desired Position"));

        reverseTimer = 200;
      }
      break;
    case 4:
      if (!reverseTimer) {
        digitalWriteFast(ROTATOR_MOTOR1, 0);
        digitalWriteFast(ROTATOR_MOTOR2, 0);
        rotatorStep = 5;
        Serial.println("reverseDone");
      }

      break;


  }
}

void loop() {
  selectButton.loop();
  modeButton.loop();
  // put your main code here, to run repeatedly:
  if (!timer1) {
    timer1 = 1000;
    displayRoutine();
    Serial.print(F("currentANgle:"));

    Serial.println(currentAngle);
    Serial.print(F("desiredANlge:"));
    Serial.println(angle);
  }

  rotatorRoutine(cuttingMode);


}

void displayRoutine() {



  switch (cuttingMode) {
    case 0:
      lcd.setCursor(0, 0);
      lcd.print(F("Fine Brunoise Cut"));
      if (startFlag) {
        lcd.setCursor(0, 1);
        lcd.print(F("Cutter Started"));
        digitalWriteFast(MOTOR_PIN1, HIGH);

      }
      else {
        lcd.setCursor(0, 1);
        lcd.print(F("Cutter Stopped"));
        digitalWriteFast(MOTOR_PIN1, LOW);
        if (rotatorStep != 5) {
          lcd.setCursor(0, 2);
          lcd.print(F("Switching Blade..."));
        }
        else {
          lcd.setCursor(0, 2);
          lcd.print(F("                    "));
        }
      }
      break;
    case 1:
      lcd.setCursor(0, 0);
      lcd.print(F("Fine Julienne Cut"));


      if (startFlag) {
        lcd.setCursor(0, 1);
        lcd.print(F("Cutter Started"));
        lcd.setCursor(0, 2);
        lcd.print(F("                    "));
        digitalWriteFast(MOTOR_PIN2, HIGH);
      }
      else {
        lcd.setCursor(0, 1);
        lcd.print(F("Cutter Stopped"));
        digitalWriteFast(MOTOR_PIN2, LOW);
        if (rotatorStep != 5) {
          lcd.setCursor(0, 2);
          lcd.print(F("Switching Blade..."));
        }
        else {
          lcd.setCursor(0, 2);
          lcd.print(F("                    "));
        }
      }
      break;
    case 2:
      lcd.setCursor(0, 0);
      lcd.print(F("Slice"));

      if (startFlag) {
        lcd.setCursor(0, 1);
        lcd.print(F("Cutter Started"));
        lcd.setCursor(0, 2);
        lcd.print(F("                    "));
        digitalWriteFast(MOTOR_PIN3, HIGH);
      }
      else {
        lcd.setCursor(0, 1);
        lcd.print(F("Cutter Stopped"));
        digitalWriteFast(MOTOR_PIN3, LOW);
        if (rotatorStep != 5) {
          lcd.setCursor(0, 2);
          lcd.print(F("Switching Blade..."));
        }
        else {
          lcd.setCursor(0, 2);
          lcd.print(F("                    "));
        }
      }
      break;
  }

}


void modeButtonHandler(Button2 & btn) {
  switch (btn.getType()) {
    case single_click:
      if (!startFlag) {//only switch blade when not running or else
        lcd.clear();
        timer1 = 0;
        cuttingMode++;
        if (cuttingMode > 2) {
          cuttingMode = 0;
        }
      }
      break;
    case double_click:


      Serial.print("2");
      break;
    case triple_click:
      break;
  }
}

void selectButtonHandler(Button2 & btn) {

  switch (btn.getType()) {
    case single_click:
      timer1 = 0;
      lcd.clear();
      if (!startFlag && rotatorStep == 5) {
        startFlag = 1;
      }
      else {
        startFlag = 0;
      }
      break;
    case double_click:


      break;
    case triple_click:


      break;
  }

}