#include <ESP32Servo.h>

#define TX_PIN 17
#define RX_PIN 16

#define MOTOR_1_PIN_1    27
#define MOTOR_1_PIN_2    32
#define MOTOR_2_PIN_1    33
#define MOTOR_2_PIN_2    25


int motorSpeed = 20;

static const int servoPin = 12;
int pushButton = 13;
 int delayTime=10;
Servo servo1;
int angle=75;
String incomingMessage="stop";
String lastmessage = "";


//To move the servomotor with tracking time
int currentAngle = 180;    
int targetAngle = 55;     
unsigned long lastMoveTime = 0; 
int moveInterval = 8;    
const int stepSize = 1;   
bool goingDown = false;        
int a = 75;   


unsigned long lastPrintTime = 0;
const int printInterval = 100; 




void setup() {

  //Use serial 2 for reading camera
  Serial2.begin(115200, SERIAL_8N1, RX_PIN, TX_PIN); 
Serial.begin(115200);
  servo1.attach(servoPin);

   //Pull up pin for button
  pinMode(pushButton, INPUT_PULLUP);


   //Start servo
  servo1.write(75);

 pinMode(MOTOR_1_PIN_1, OUTPUT); 
  pinMode(MOTOR_1_PIN_2, OUTPUT); 
  pinMode(MOTOR_2_PIN_1, OUTPUT); 
  pinMode(MOTOR_2_PIN_2, OUTPUT);


}

void loop() {

//Read the camera
   if (Serial2.available()) {
    incomingMessage = Serial2.readStringUntil('\n'); // Read the incoming data from ESP32 2
    incomingMessage.trim();
    Serial.print("Received command: ");
    Serial.println(incomingMessage); // Print the incoming message
   
  
  }


//Only move motors if a different button was clicked
if(lastmessage != incomingMessage){
processMessage(incomingMessage);
lastmessage=incomingMessage;
}
  

//Read push button and if high (its negated), trigger the servomotor.
//By setting goingDown as true
  int buttonState = digitalRead(pushButton);
 if (millis() - lastPrintTime >= printInterval) {
    Serial.println(buttonState);
    lastPrintTime = millis();
  }

  if (buttonState == 0) {
    goingDown = true;
  }


  //continiouslly move the servomotor, based on a true condition
smoothServoMove(); 

}

void processMessage(String incomingMessage){

  //Move motors based on the message
  
    Serial.print("Processing command");
   Serial.println(incomingMessage); 
 if (incomingMessage.indexOf("Forward") != -1) {
    Serial.println("Forwarding");
    digitalWrite(MOTOR_1_PIN_1, 1);
    digitalWrite(MOTOR_1_PIN_2, 0);
    digitalWrite(MOTOR_2_PIN_1, 0);
    digitalWrite(MOTOR_2_PIN_2, 1);
 
  } else if (incomingMessage.indexOf("Left") != -1) {
    Serial.println("Lefting");
    digitalWrite(MOTOR_1_PIN_1, 1);
    digitalWrite(MOTOR_1_PIN_2, 0);
    digitalWrite(MOTOR_2_PIN_1, 1);
    digitalWrite(MOTOR_2_PIN_2, 0);
    
  } else if (incomingMessage.indexOf("Right") != -1) {
    Serial.println("Righting");
    digitalWrite(MOTOR_1_PIN_1, 0);
    digitalWrite(MOTOR_1_PIN_2, 1);
    digitalWrite(MOTOR_2_PIN_1, 0);
    digitalWrite(MOTOR_2_PIN_2, 1);
    
  } else if (incomingMessage.indexOf("Backward") != -1) {
    Serial.println("Backwarding");
    digitalWrite(MOTOR_1_PIN_1, 0);
    digitalWrite(MOTOR_1_PIN_2, 1);
    digitalWrite(MOTOR_2_PIN_1, 1);
    digitalWrite(MOTOR_2_PIN_2, 0);
    
  } else if (incomingMessage.indexOf("Stop") != -1) {
    Serial.println("Stoping");
    digitalWrite(MOTOR_1_PIN_1, 0);
    digitalWrite(MOTOR_1_PIN_2, 0);
    digitalWrite(MOTOR_2_PIN_1, 0);
    digitalWrite(MOTOR_2_PIN_2, 0);

  }  

}


void smoothServoMove() {
  if (millis() - lastMoveTime >= moveInterval) {
    lastMoveTime = millis(); // Update the last movement time

    // Move servo closer to target position
    if (goingDown) {
      
      currentAngle -= stepSize;
      if (currentAngle <= targetAngle) {
        currentAngle = targetAngle; // Clamp at target
        goingDown = false;          // Start moving up
      }
    } else {
      currentAngle += stepSize;
      if (currentAngle >= 180) {
        currentAngle = 180; // Clamp at 180°
        
      }
    }
    servo1.write(currentAngle); // Update servo position
  }
}