#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <ESP32Servo.h>

#define LED 2 
#define LED2 4 
#define LED3 18 
#define LED4 19 
#define SW1 5 
#define SW2 16 
#define SW3 17 
#define SW4 15
#define SW5 0
#define SW6 12
#define SW7 13
#define SW8 14
#define SW9 33
#define SERVO_LIFT 25 // ESP32 pin GPIO26 connected to servo motor
#define SERVO_PIN 26 // ESP32 pin GPIO26 connected to servo motor

Servo servoMotor;
Servo servoMotor2;
int pos = 0;
int pos2 = 0;

LiquidCrystal_I2C LCD = LiquidCrystal_I2C(0x27, 20, 4);
void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  servoMotor.attach(SERVO_PIN);
  servoMotor2.attach(SERVO_LIFT);
  servoMotor.write(pos);
  servoMotor2.write(pos2);  
  Serial.println("Hello, ESP32!");
  Serial.println("My name is Chaiyan Chanapromma");
  LCD.init();
  LCD.backlight();
  LCD.setCursor(3, 0);
  LCD.print("-->>TONY<<--");
  pinMode(LED, OUTPUT);
  pinMode(LED2, OUTPUT);
  pinMode(LED3, OUTPUT);
  pinMode(LED4, OUTPUT);
  pinMode(SW1, INPUT_PULLUP);
  pinMode(SW2, INPUT_PULLUP);
  pinMode(SW3, INPUT_PULLUP);
  pinMode(SW4, INPUT_PULLUP);
  pinMode(SW5, INPUT_PULLUP);
  pinMode(SW6, INPUT_PULLUP);
  pinMode(SW7, INPUT_PULLUP);
  pinMode(SW8, INPUT_PULLUP);
  pinMode(SW9, INPUT_PULLUP);
 ///////////////////LED1&LED2--> OFF <-- //////////////////
  digitalWrite(LED, LOW);
  digitalWrite(LED2, LOW);
  digitalWrite(LED3, LOW);
  digitalWrite(LED4, LOW);

 ////////////////////////////////////////////////
    delay(2000);
    LCD.clear();
}

void loop() {
  //LCD.setCursor(6, 1);
  //LCD.print("Stanby");

  if (digitalRead(SW1) == LOW){
    function_emergency ();
  }
  if (digitalRead(SW2) == LOW){
    function_Left();
  }
  if (digitalRead(SW3) == LOW){
    function_Right();
  }
  if (digitalRead(SW4) == LOW){
    function_Forward();
  }
  if (digitalRead(SW5) == LOW){

    function_back();
  }
  
  if (digitalRead(SW6) == LOW){
    servo1_FW();
  }
  if (digitalRead(SW7) == LOW){
    servo1_RW();
  }

  if (digitalRead(SW8) == LOW){
    servo2_FW();
  }

    if (digitalRead(SW9) == LOW){
    servo2_RW();
  }

  if ((digitalRead(SW1) == HIGH)&& (digitalRead(SW2) == HIGH) &&(digitalRead(SW3) == HIGH)&&(digitalRead(SW4) == HIGH)&&(digitalRead(SW5) == HIGH)&&(digitalRead(SW6) == HIGH)&&(digitalRead(SW7) == HIGH)&&(digitalRead(SW8) == HIGH)&&(digitalRead(SW9) == HIGH)){
    function_off ();
    //LCD.setCursor(6, 1);
    //LCD.print("      ");
    LCD.setCursor(6, 1);
    LCD.print("Stanby");
  }
   delay(10);
}



void function_emergency (){
  digitalWrite(LED, HIGH);
  LCD.setCursor(0, 0);
  LCD.print("<<-- ");
  digitalWrite(LED2, HIGH);
  LCD.setCursor(17, 0);
  LCD.print("->>");
  Serial.println("Emergency mode");
  LCD.setCursor(6, 0);
  LCD.print("Emergency");
  LCD.setCursor(6, 1);
  LCD.print("      ");
  delay(1000);

  digitalWrite(LED, LOW);
  LCD.setCursor(0, 0);
  LCD.print("     ");
  digitalWrite(LED2, LOW);
  LCD.setCursor(17, 0);
  LCD.print("    ");
  delay(1000);
}

void function_Left(){
  digitalWrite(LED, HIGH);
  LCD.setCursor(0, 0);
  LCD.print("<<-- ");
  LCD.setCursor(6, 0);
  LCD.print("Left     ");
  LCD.setCursor(6, 1);
  LCD.print("      ");
  Serial.println("Left mode");
  delay(1000);

  digitalWrite(LED, LOW);
  LCD.setCursor(0, 0);
  LCD.print("     ");
  delay(1000);
}

void function_Right(){
  digitalWrite(LED2, HIGH);
  LCD.setCursor(17, 0);
  LCD.print("->>");
  LCD.setCursor(6, 0);
  LCD.print("Right    ");
  LCD.setCursor(6, 1);
  LCD.print("      ");
  Serial.println("Right mode");
  delay(1000);

  digitalWrite(LED2, LOW);
  LCD.setCursor(17, 0);
  LCD.print("    ");
  delay(1000);
}

void function_Forward(){
  digitalWrite(LED3, HIGH);
  LCD.setCursor(0, 0);
  LCD.print(" ^ ");
  LCD.setCursor(0, 1);
  LCD.print(" | ");
  LCD.setCursor(0, 2);
  LCD.print(" | ");
  LCD.setCursor(6, 0);
  LCD.print("Forward    ");
  LCD.setCursor(6, 1);
  LCD.print("      ");
  Serial.println("Forward mode");
  delay(1000);

  digitalWrite(LED3, LOW);
  LCD.setCursor(0, 0);
  LCD.print("   ");
  LCD.setCursor(0, 1);
  LCD.print("   ");
  LCD.setCursor(0, 2);
  LCD.print("   ");
  delay(1000);
}

void function_back(){
//LCD.clear();
    
  digitalWrite(LED4, HIGH);
  LCD.setCursor(0, 0);
  LCD.print(" | ");
  LCD.setCursor(0, 1);
  LCD.print(" | ");
  LCD.setCursor(0, 2);
  LCD.print(" V ");
  LCD.setCursor(6, 0);
  LCD.print("Backward    ");
  LCD.setCursor(6, 1);
  LCD.print("      ");
  Serial.println("Backward mode");
  delay(1000);

  digitalWrite(LED4, LOW);
  LCD.setCursor(0, 0);
  LCD.print("   ");
  LCD.setCursor(0, 1);
  LCD.print("   ");
  LCD.setCursor(0, 2);
  LCD.print("   ");
  delay(1000);
}


void function_off (){
 ///////////////////LED1&LED2--> OFF <-- //////////////////
  LCD.setCursor(6, 0);
  LCD.print("         ");
  LCD.setCursor(6, 2);
  LCD.print("         ");
  digitalWrite(LED, LOW);
  digitalWrite(LED2, LOW);
  digitalWrite(LED3, LOW);
  digitalWrite(LED4, LOW);
  //delay(1000);
 ////////////////////////////////////////////////

}

void servo1_FW(){
      if (pos<180) // Check that the position won't go higher than 180°
    {
        pos += 10;
        servoMotor.write(pos);
        Serial.println("servo1_FW");
        Serial.println(pos);
        LCD.setCursor(6, 2);
        LCD.print("Select   ");
        LCD.setCursor(6, 1);
        LCD.print("      ");
        delay(5); // Wait 5ms for the arm to reach the position
    }
}

void servo1_RW(){
  if (pos>0) // Check that the position won't go lower than 0°
    {
        pos -= 10;
        servoMotor.write(pos);
        Serial.println("servo1_RW");
        Serial.println(pos);
        LCD.setCursor(6, 2);
        LCD.print("Unselect  ");
        LCD.setCursor(6, 1);
        LCD.print("      ");
        delay(5); // Wait 5ms for the arm to reach the position
    }
}

void servo2_FW(){
  pos2 = 45;
  servoMotor2.write(pos2);
  Serial.println("LIFT_Up");
  Serial.println(pos2);
  LCD.setCursor(6, 2);
  LCD.print("LIFT_Up  ");
  LCD.setCursor(6, 1);
  LCD.print("      ");
  delay(5); //
}

void servo2_RW(){
  pos2 = 0;
  servoMotor2.write(pos2);
  Serial.println("LIFT_Down");
  Serial.println(pos2);
  LCD.setCursor(6, 2);
  LCD.print("LIFT_Down");
  LCD.setCursor(6, 1);
  LCD.print("      ");
  delay(5); //
}