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

// Піни для двигунів
#define MOTOR_LEFT_EN 5
#define MOTOR_RIGHT_EN 6
#define MOTOR_LEFT_FORWARD 7
#define MOTOR_LEFT_BACKWARD 8
#define MOTOR_RIGHT_FORWARD 3
#define MOTOR_RIGHT_BACKWARD 4

#define GYROSCOPE_SDA A4
#define GYROSCOPE_SCL A5

#define SERVO_PIN 9

// Піни для Bluetooth
#define BT_RX 10
#define BT_TX 11

// Пін для датчика відстані
#define ECHO_PIN A0
#define TRIG_PIN 2

Servo servo;
MPU6050 gyro;
SoftwareSerial bluetooth(BT_RX, BT_TX);

float Kp = 1.0, Ki = 0, Kd = 0;
int controlMode = 0;

// Змінні для зберігання станів
bool isRunning = false;
String inputString = "";
bool stringComplete = false;

// Таймери
unsigned long previousMillis = 0;
const long interval = 50; // Інтервал часу для оновлення руху

unsigned long servoMoveStartTime = 0;
bool servoTurning = false;
const long servoTurnDuration = 500; // Час на поворот серво

// PID контролер змінні
float setPoint = 0; // Цільовий кут
float input = 0;    // Поточний кут
float output = 0;   // Вихідний сигнал контролера

float integral = 0;
float previousError = 0;

void setup() {
  pinMode(MOTOR_LEFT_EN, OUTPUT);
  pinMode(MOTOR_RIGHT_EN, OUTPUT);
  pinMode(MOTOR_LEFT_FORWARD, OUTPUT);
  pinMode(MOTOR_LEFT_BACKWARD, OUTPUT);
  pinMode(MOTOR_RIGHT_FORWARD, OUTPUT);
  pinMode(MOTOR_RIGHT_BACKWARD, OUTPUT);

  servo.attach(SERVO_PIN);
  bluetooth.begin(9600);

  Wire.begin();
  gyro.initialize();

  // Зчитування PID параметрів з EEPROM
  EEPROM.get(0, Kp);
  EEPROM.get(4, Ki);
  EEPROM.get(8, Kd);
}

void loop() {
  unsigned long currentMillis = millis();
  if (isRunning) {
    if (currentMillis - previousMillis >= interval) {
      previousMillis = currentMillis;

      long distance = getDistance();
      if (distance < 20 && !servoTurning) {
        // Поворот серво
        servo.write(90);
        servoTurning = true;
        // Рух в новому напрямку
        servoMoveStartTime = currentMillis;
      } else if (servoTurning) {
        if (currentMillis - servoMoveStartTime >= servoTurnDuration) {
          servoTurning = false;
          // Рух в новому напрямку
          moveRobot(200, 200);
        }
      } else {
        // Основний рух прямо
        moveRobot(255, 255);
      }
    } else {
      moveRobot(0, 0);
    }
    processBluetoothCommand();
  }
}

// Функція для вимірювання відстані
long getDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  long duration = pulseIn(ECHO_PIN, HIGH);
  long distance = (duration / 2) / 29.1;
  return distance;
}

// Функція для керування рухом робота
void moveRobot(int leftSpeed, int rightSpeed) {
  if (leftSpeed > 0) {
    analogWrite(MOTOR_LEFT_FORWARD, leftSpeed);
    analogWrite(MOTOR_LEFT_BACKWARD, 0);
  } else {
    analogWrite(MOTOR_LEFT_FORWARD, 0);
    analogWrite(MOTOR_LEFT_BACKWARD, -leftSpeed);
  }
  if (rightSpeed > 0) {
    analogWrite(MOTOR_RIGHT_FORWARD, rightSpeed);
    analogWrite(MOTOR_RIGHT_BACKWARD, 0);
  } else {
    analogWrite(MOTOR_RIGHT_FORWARD, 0);
    analogWrite(MOTOR_RIGHT_BACKWARD, -rightSpeed);
  }
}

// Функція для зчитування даних з Bluetooth
void serialEvent() {
  while (bluetooth.available()) {
    char inChar = (char)bluetooth.read();
    inputString += inChar;
    if (inChar == '\n') {
      stringComplete = true;
    }
  }
}

// Функція для обробки Bluetooth команд
void processBluetoothCommand() {
  if (stringComplete) {
    if (inputString.startsWith("start")) {
      isRunning = true;
    } else if (inputString.startsWith("stop")) {
      isRunning = false;
    } else if (inputString.startsWith("pControl")) {
      controlMode = 0;
    } else if (inputString.startsWith("pdControl")) {
      controlMode = 1;
    } else if (inputString.startsWith("piControl")) {
      controlMode = 2;
    } else if (inputString.startsWith("pidControl")) {
      controlMode = 3;
    } else if (inputString.startsWith("Kp=")) {
      Kp = inputString.substring(3).toFloat();
      EEPROM.put(0, Kp);
    } else if (inputString.startsWith("Ki=")) {
      Ki = inputString.substring(3).toFloat();
      EEPROM.put(4, Ki);
    } else if (inputString.startsWith("Kd=")) {
      Kd = inputString.substring(3).toFloat();
      EEPROM.put(8, Kd);
    }
    inputString = "";
    stringComplete = false;
  }
}
float computePID(float input, float setPoint) {
  float error = setPoint - input;
  integral += error * interval / 1000.0;
  float derivative = (error - previousError) / (interval / 1000.0);
  previousError = error;

  float output;
  switch (controlMode) {
    case 0:  // P
      output = Kp * error;
      break;
    case 1:  // PD
      output = Kp * error + Kd * derivative;
      break;
    case 2:  // PI
      output = Kp * error + Ki * integral;
      break;
    case 3:  // PID
      output = Kp * error + Ki * integral + Kd * derivative;
      break;
  }
  return output;
}