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

// Визначення пінів
#define DISTANCE_SENSOR A0
#define LEFT_MOTOR_FORWARD 7
#define LEFT_MOTOR_BACKWARD 8
#define RIGHT_MOTOR_FORWARD 3
#define RIGHT_MOTOR_BACKWARD 4
#define LEFT_MOTOR_SPEED 5
#define RIGHT_MOTOR_SPEED 6
#define SERVO_PIN 9
#define MPU6050_SDA_PIN A4
#define MPU6050_SCL_PIN A5

Servo myServo;
int positionServo = 90;

float Kp = 0.0, Ki = 0.0, Kd = 0.0;
float setpoint = 0;
float output = 0;
float integral = 0;
float lastError = 0;

SoftwareSerial mySerial(10, 11);
bool stringComplete = false;

void setup()
{
   pinMode(DISTANCE_SENSOR, 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);
   mySerial.begin(9600);

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

void loop()
{
   if (stringComplete)
   {
      processCommand(inputString);
      inputString = "";
      stringComplete = false;
   }

   float sensorValue = analogRead(DISTANCE_SENSOR);
   float error = setpoint - sensorValue;
   integral += error;
   float derivative = error - lastError;
   output = Kp * error + Ki * integral + Kd * derivative;

   if (output > 0)
   {
      startMovement(abs(output));
   }
   else
   {
      stopMovement();
   }
}

void processCommand(String command)
{
   command.trim();
   if (command.startsWith("pControl"))
   {
      // Обробка команди пропорційного керування
      float newValue = getValueFromCommand(command);
      Kp = newValue;
      EEPROM.write(0, Kp * 100);
   }
   else if (command.startsWith("pdControl"))
   {
      // Обробка команди пропорційно-диференційного керування
      // Аналогічно
   }
   else if (command.startsWith("piControl"))
   {
      // Обробка команди пропорційно-інтегрального керування
      // Аналогічно
   }
   else if (command.startsWith("pidControl"))
   {
      // Обробка команди ПІД-регулювання
      // Аналогічно
   }
   else if (command.startsWith("start"))
   {
      int speedValue = getValueFromCommand(command);
      startMovement(speedValue);
   }
   else if (command.startsWith("stop"))
   {
      stopMovement();
   }
   else if (command.startsWith("servo"))
   {
      int angle = getValueFromCommand(command);
      myServo.write(angle);
   }
}

void startMovement(int speed)
{
   analogWrite(LEFT_MOTOR_SPEED, speed);
   analogWrite(RIGHT_MOTOR_SPEED, speed);
   digitalWrite(LEFT_MOTOR_FORWARD, HIGH);
   digitalWrite(RIGHT_MOTOR_FORWARD, HIGH);
}

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


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.toFloat();
   }
   return 0;
}

// Функція для обробки даних, які приходять з монітора
void serialEvent()
{
   while (mySerial.available())
   {
      char inChar = (char)mySerial.read();
      inputString += inChar;
      if (inChar == '\n')
      {
         stringComplete = true;
      }
   }
}