#include <SoftwareSerial.h>
#include <Servo.h>
#include <EEPROM.h>

#define LEFT_MOTOR_FORWARD 3
#define LEFT_MOTOR_BACKWARD 4
#define RIGHT_MOTOR_FORWARD 7
#define RIGHT_MOTOR_BACKWARD 8
#define LEFT_MOTOR_SPEED 5
#define RIGHT_MOTOR_SPEED 6

#define SERVO_PIN 9

#define trigerPin 2
#define echoPin 10

Servo myServo;
int positionServo = 90;
String inputString = "";
bool robotStarted = false;
int motorSpeed = 0;

// Define variables for PID control
float Kp = 8.5;  // Proportional gain
float Kd = 0.01; // Derivative gain
float Ki = 0.5;  // Integral gain

bool stringComplete = false;

void setup()
{
   pinMode(trigerPin, OUTPUT);
   pinMode(echoPin, INPUT);
   pinMode(LEFT_MOTOR_FORWARD, OUTPUT);
   pinMode(LEFT_MOTOR_BACKWARD, OUTPUT);
   pinMode(RIGHT_MOTOR_FORWARD, OUTPUT);
   pinMode(RIGHT_MOTOR_BACKWARD, OUTPUT);
   pinMode(LEFT_MOTOR_SPEED, OUTPUT);
   pinMode(RIGHT_MOTOR_SPEED, OUTPUT);
   pinMode(SERVO_PIN, OUTPUT);

   myServo.attach(SERVO_PIN);
   myServo.write(positionServo);

   Serial.begin(9600);
   Serial.println("Initialized");

   EEPROM.get(0, Kp);
   EEPROM.get(sizeof(float), Ki);
   EEPROM.get(2 * sizeof(float), Kd);
}

void parseCommand(String command)
{
   command.trim();
   Serial.print("Received command: ");
   Serial.println(command);
   if (command.startsWith("start"))
   {
      robotStarted = true;
      int speedValue = getValueFromCommand(command);
      motorSpeed = speedValue;
      Serial.println("Starting robot...");
   }
   else if (command.startsWith("stop"))
   {
      robotStarted = false;
      stopMovement();
      Serial.println("Stopping robot...");
   }
   else if (command.startsWith("speed"))
   {
      int speedValue = getValueFromCommand(command);
      motorSpeed = speedValue;
      Serial.println("Speed changed robot...");
   }
   else if (command.startsWith("servo"))
   {
      int angle = getValueFromCommand(command);
      myServo.write(angle);
   }
   else if (command.startsWith("pControl"))
   {
      // Handle proportional control command
   }
   else if (command.startsWith("pdControl"))
   {
      // Handle proportional-derivative control command
   }
   else if (command.startsWith("piControl"))
   {
      // Handle proportional-integral control command
   }
   else if (command.startsWith("pidControl"))
   {
      // Handle PID control command
   }
   else if (command.startsWith("Kp="))
   {
      String kpValue = command.substring(3);
      kpValue.trim();
      Kp = kpValue.toFloat();
      EEPROM.put(0, Kp);
   }
   else if (command.startsWith("Kd="))
   {
      String kdValue = command.substring(3);
      kdValue.trim();
      Kd = kdValue.toFloat();
      EEPROM.put(sizeof(float), Kd);
   }
   else if (command.startsWith("Ki="))
   {
      String kiValue = command.substring(3);
      kiValue.trim();
      Ki = kiValue.toFloat();
      EEPROM.put(2 * sizeof(float), Ki);
   }
}

void moveForward(int motorSpeed)
{
   analogWrite(LEFT_MOTOR_SPEED, motorSpeed);
   analogWrite(RIGHT_MOTOR_SPEED, motorSpeed);
   digitalWrite(LEFT_MOTOR_FORWARD, HIGH);
   digitalWrite(RIGHT_MOTOR_FORWARD, HIGH);
   return motorSpeed;
}

void stopMovement()
{
   analogWrite(LEFT_MOTOR_SPEED, 0);
   analogWrite(RIGHT_MOTOR_SPEED, 0);
   digitalWrite(LEFT_MOTOR_FORWARD, LOW);
   digitalWrite(RIGHT_MOTOR_FORWARD, LOW);
}

void controlServo(int angle)
{
   if (angle < 0)
   {
      angle = 0;
   }
   else if (angle > 180)
   {
      angle = 180;
   }
   myServo.write(angle);
}

float getValueFromCommand(String command)
{
   int pos = command.indexOf("=");
   if (pos != -1)
   {
      String valueStr = command.substring(pos + 1);
      return valueStr.toInt();
   }
   return 0;
}

void serialEvent()
{
   while (Serial.available())
   {
      char inChar = (char)Serial.read();
      inputString += inChar;
      if (inChar == '\n')
      {
         stringComplete = true;
      }
   }
}

float getDistance()
{
   digitalWrite(trigerPin, LOW);
   delayMicroseconds(2);
   digitalWrite(trigerPin, HIGH);
   delayMicroseconds(10);
   digitalWrite(trigerPin, LOW);

   float duration = pulseIn(echoPin, HIGH);
   float distance = duration * 0.034 / 2;
   return distance;
}

void loop()
{
   if (stringComplete)
   {
      parseCommand(inputString);
      inputString = "";
      stringComplete = false;
   }
   float distance = getDistance();
   if (distance > 20 && !robotStarted)
   {
      moveForward(motorSpeed);
   }
   else
   {
      stopMovement();
   }
}