#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 20, 4);
int PWM_Left = 5; // Arduino PWM output pin 5; connect to IBT-2 pin 1 (RPWM)
int PWM_Right = 6; // Arduino PWM output pin 6; connect to IBT-2 pin 2 (LPWM)
int PWM_Up = 9;
int PWM_Down = 10;
int Trimmer = 4;// out for trimmer relay
int autodisplay = 0;
int message = 0;
int cycle = 0;
int Flag1 = 0;
int Flag2 = 0;
int MotorEncoder = 0;
int SlowDown;
unsigned long displayTime = 0;
unsigned long pitch = 0;
const byte StopPin = 3;//e-stop pin
const byte Encoder = 2;// encoder input
int PWMSpeedInput;
int PWMSpeed = 50;
int Speed;
int SpeedInput;
unsigned long SpeedDelay = 0;
int Trimmerswitch = A3;//input pin switch for trimmer
int modeswitch = 1;
int selectorswitch = 7;
int startswitch = 0;
int Left_Up = 8;//switch
int Right_Down = 13;//switch
int RSensor = 12;//input sensor
int LSensor = 11;//input sensor

void setup() {
  lcd.init();  //initialize the lcd
  lcd.backlight();  //turn on backlight
  delay(10);
  lcd.setCursor(0, 0);
  lcd.print("Hedge Dream");
  lcd.setCursor (4, 1);
  lcd.print("Project ");
  lcd.print("2024");
  lcd.setCursor(4, 3);
  lcd.print("Safety First");
  delay (2000);
  lcd.clear();
  pinMode(modeswitch, INPUT_PULLUP);
  pinMode(Left_Up, INPUT_PULLUP);
  pinMode(Right_Down, INPUT_PULLUP);
  pinMode(selectorswitch, INPUT_PULLUP);
  pinMode(Trimmerswitch, INPUT_PULLUP);
  pinMode(startswitch, INPUT_PULLUP);
  pinMode(RSensor, INPUT_PULLUP);// check
  pinMode(LSensor, INPUT_PULLUP);// check
  pinMode(StopPin, INPUT_PULLUP);
  pinMode(Encoder, INPUT_PULLUP);// check
  pinMode(A0, INPUT);
  attachInterrupt(digitalPinToInterrupt(StopPin), blink, FALLING);
  attachInterrupt(digitalPinToInterrupt(Encoder), blink1, FALLING);
  pinMode(PWM_Left, OUTPUT);
  pinMode(PWM_Right, OUTPUT);
  pinMode(PWM_Up, OUTPUT);
  pinMode(PWM_Down, OUTPUT);
  pinMode(Trimmer, OUTPUT);
  analogWrite(PWM_Left, 0);
  analogWrite(PWM_Right, 0);
  analogWrite(PWM_Up, 0);
  analogWrite(PWM_Down, 0);

  lcd.setCursor(7, 0);
  lcd.print("S%     M%");
  lcd.setCursor(14, 1);
  lcd.print("EN");
  lcd.setCursor(0, 3);
  lcd.print("Trimmer");
}
void loop() {

  if (digitalRead (modeswitch) == HIGH) {//Manual Functions

    if (digitalRead(Trimmerswitch) == LOW) {
      digitalWrite(Trimmer, LOW);
      autodisplay = 7;
    }
    else digitalWrite(Trimmer, HIGH), autodisplay = 8;

    if (digitalRead(StopPin) == LOW) {
      MotorEncoder = 0;
    }

    if (digitalRead(selectorswitch) == LOW) { //Jog left and right

      if ((digitalRead(Left_Up) == HIGH) && (digitalRead(Right_Down) == HIGH)) {
        analogWrite(PWM_Right, 0);
        analogWrite(PWM_Left, 0);
        message = 7;
        PWMSpeed = 50;
      }
      if (digitalRead(Left_Up) == LOW) { //CW jog button pushed
        analogWrite(PWM_Right, 0);
        analogWrite(PWM_Left, PWMSpeed);
        message = 1;
      }
      if (digitalRead(Right_Down) == LOW) { //CW jog button pushed
        analogWrite(PWM_Left, 0);
        analogWrite(PWM_Right, PWMSpeed);
        message = 2;
      }
    }
    if (digitalRead(selectorswitch) == HIGH) {//Jog up and down

      if ((digitalRead(Left_Up) == HIGH) && (digitalRead(Right_Down) == HIGH)) {
        analogWrite(PWM_Up, 0);
        analogWrite(PWM_Down, 0);
        message = 6;
        PWMSpeed = 50;
      }
      if (digitalRead(Left_Up) == LOW) {
        analogWrite(PWM_Down, 0);
        analogWrite(PWM_Up, PWMSpeed);
        message = 4;

      }
      if (digitalRead(Right_Down) == LOW) {
        analogWrite(PWM_Up, 0);
        analogWrite(PWM_Down, PWMSpeed);
        message = 5;
      } 
    }
    if ((message < 6) && (millis() > (SpeedDelay + 5))) {
      SpeedDelay = millis();
      if (PWMSpeed < PWMSpeedInput) {
        ++PWMSpeed;
      }
      if (PWMSpeed >  PWMSpeedInput) {
        --PWMSpeed;
      }
    }
    if (digitalRead(LSensor) == LOW) {
      lcd.setCursor(18,3);
      lcd.print("LT");
    }
    if (digitalRead(RSensor) == LOW) {
      lcd.setCursor(18,3);
      lcd.print("RT");
    }
    if ((digitalRead(RSensor) == HIGH)  && (digitalRead(LSensor)==HIGH)){
      lcd.setCursor(18,3);
      lcd.print("  ");
    }
  }
  if (digitalRead (modeswitch) == LOW) {//Auto Sequence
    if ((digitalRead(LSensor) == LOW) && (cycle != 2)) {
      analogWrite(PWM_Right, 0);
      analogWrite(PWM_Left, 100);
      pitch = millis();
      lcd.setCursor (0, 1);
      lcd.print("Lt Correction");
      Flag1 = 1;
    }
    if (digitalRead(RSensor) == LOW) {
      analogWrite(PWM_Left, 0);
      analogWrite(PWM_Right, 100);
      pitch = millis();
      lcd.setCursor (0, 1);
      lcd.print("Rt Correction");
      Flag1 = 1;
    }
    if ((millis() >= pitch + 250) && (Flag1 == 1)) {
      analogWrite(PWM_Right, 0);
      analogWrite(PWM_Left, 0);
      lcd.setCursor (0, 1);
      lcd.print("             ");
      Flag1 = 0;
    }
    if (cycle == 0) {//waiting to start
      autodisplay = 1;
      digitalWrite(Trimmer, HIGH);
      if (digitalRead(startswitch) == LOW) {//starting the cycle
        MotorEncoder = 0;
        cycle = 1;
        Flag2 = 1;
        autodisplay = 2;
        digitalWrite(Trimmer, LOW);
        PWMSpeed = 50;
      }
    }
    if (cycle == 1) { //traveling up

      if ((millis() > (SpeedDelay + 5)) && (MotorEncoder <= SlowDown)) {
        SpeedDelay = millis();
        if (PWMSpeed < PWMSpeedInput) {
          PWMSpeed = PWMSpeed + 1;
        }
        if (PWMSpeed >  PWMSpeedInput) {
          --PWMSpeed;
        }
      }
      if ((PWMSpeed > 300) && (millis() > (SpeedDelay + 1)) && (MotorEncoder > SlowDown)) {
        SpeedDelay = millis();
        PWMSpeed = PWMSpeed - 2;
      }
      analogWrite(PWM_Up, PWMSpeed);
      if (MotorEncoder >= 288 ) {//stop value from encoder
bailout:
        analogWrite(PWM_Up, 0);
        MotorEncoder = 0;
        analogWrite(PWM_Right, 0);
        analogWrite(PWM_Left, 0);
        Flag2 = 0;
        lcd.setCursor (0, 2);
        autodisplay = 4;
        lcd.print("Returning Home");
        lcd.setCursor(17, 1);
        lcd.print("   ");
        analogWrite(PWM_Left, 254);
        delay (2000);
        cycle = 2;
        analogWrite(PWM_Left, 0);
        autodisplay = 3;
        cycle = 2;
        digitalWrite(Trimmer, HIGH);
        PWMSpeed = 50;
      }
    }
    if (cycle == 2) {// down
      if ((millis() > (SpeedDelay + 1)) && (PWMSpeed <= 240) && (MotorEncoder < 200)) {//Down Start
        SpeedDelay = millis();
        PWMSpeed = PWMSpeed + 5;
      } 
      if ((millis() > (SpeedDelay + 20)) && (PWMSpeed >= 20) && (MotorEncoder > 230)) {//Down Stop
        SpeedDelay = millis();
        PWMSpeed = PWMSpeed - 15;
      }
      analogWrite(PWM_Down, PWMSpeed);
      if ((MotorEncoder >= 280) && (Flag2 == 0))  {
bailout1:
        analogWrite(PWM_Down, 0);
        cycle = 0;
      }
    }
    if (cycle == 4) {
      analogWrite(PWM_Up, 0);
      analogWrite(PWM_Down, 0);
      digitalWrite(Trimmer, HIGH);
      analogWrite(PWM_Left, 0);
      analogWrite(PWM_Right, 0);

      if (Flag2 == 1) { //upper end travel
        goto bailout;//jump to off cycle
      }
      if (Flag2 == 0) { //lower end travel
        goto bailout1;
      }
    }
  }
  if (millis() > (displayTime + 750)) {//display squence and delay
    displayTime = millis();
    PWMSpeedInput = (analogRead(A0) / 1023.00) * 255;
    if (PWMSpeedInput < 50) {
      PWMSpeedInput = 50;
    }
    SpeedInput = (PWMSpeedInput * 100) / 255;
    Speed = (PWMSpeed * 100) / 255;
    lcd.setCursor(10, 0);
    lcd.print("    M%    ");
    lcd.setCursor(10, 0);
    lcd.print(SpeedInput);
    lcd.setCursor(17, 0);
    lcd.print(Speed);
    lcd.setCursor(17, 1);
    lcd.print("   ");
    lcd.setCursor(17, 1);
    lcd.print(MotorEncoder);

    if (digitalRead (modeswitch) == HIGH) {
      lcd.setCursor (0, 0);
      lcd.print("Manual");
      lcd.setCursor (0, 2);
      lcd.print("           ");
      lcd.setCursor(0, 1);
      if (message == 1) {
        lcd.print("Pitch Left ");
      }
      if (message == 2) {
        lcd.print("Pitch Right");
      }
      if (message == 7) {
        lcd.print("Pitch        ");
      }
      if (message == 4) {
        lcd.print("UP/DOWN = U  ");
      }
      if (message == 5) {
        lcd.print("UP/DOWN = D");
      }
      if (message == 6) {
        lcd.print("UP/DOWN       ");
      }
      lcd.setCursor(8, 3);
      if (autodisplay == 7) {
        lcd.print("ON ");
      }
      if (autodisplay == 8) {
        lcd.print("OFF");
      }
    }
    if (digitalRead (modeswitch) == LOW) {
      SlowDown = (289 - (analogRead(A0) / 18));
      if (autodisplay == 1) {
        lcd.setCursor(0, 0);
        lcd.print("Auto  ");
        lcd.setCursor (18, 1);
        lcd.print("  ");
        lcd.setCursor(0, 2);
        lcd.print("Press Start   ");
        lcd.setCursor (8, 3);
        lcd.print("OFF");
      }
      if (autodisplay == 2) {
        lcd.setCursor (0, 2);
        lcd.print(" Up        ");
        lcd.setCursor (8, 3);
        lcd.print("ON ");
      }
      if (autodisplay == 3) {
        lcd.setCursor (0, 2);
        lcd.print("  Down        ");
        lcd.setCursor (8, 3);
        lcd.print("OFF");
      }
    }
  }
}
void blink() {//emergency stop intrupt pin 3
  cycle = 4;
}
void blink1() {
  MotorEncoder  = MotorEncoder + 1;
}