#include <EEPROM.h>
#include <SoftwareSerial.h>
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

const int micPin = A1; //мікрофон

Adafruit_MPU6050 mpu;

unsigned long lastMicrophoneTime = 0;
unsigned long lastAccelerometerTime = 0;

const int motorPin1 = 3; // Вперед лівий
const int motorPin2 = 4; // Назад лівий

const int motorPin3 = 7; // Вперед правий
const int motorPin4 = 8; // Назад правий

const int leftMotorSpeedPin = 12;
const int rightMotorSpeedPin = 13;
bool isRunning = false;
String selectedMode = "None";

float Kp = 0.0;
float Ki = 0.0;
float Kd = 0.0;

void saveCoefficients() {
  EEPROM.put(0, Kp);
  EEPROM.put(sizeof(float), Ki);
  EEPROM.put(2 * sizeof(float), Kd);
}


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

int readMicrophone() {
  return analogRead(micPin);
}

void processMicrophone() {
  static unsigned long lastClapTime = 0;
  static int clapCount = 0;

  int soundLevel = readMicrophone();
  Serial.print("Sound Level: ");
  Serial.println(soundLevel);
  if (soundLevel > 550) { 
    unsigned long currentTime = millis();
    if (currentTime - lastClapTime < 400) { 
      clapCount++;
    } else {
      clapCount = 1;
    }
    lastClapTime = currentTime;
  }

  if (clapCount == 1) {
    moveLeft();
  } else if (clapCount == 2) {
    moveRight();
  }
}

void moveForward() {
  Serial.println("Moving Forward");
  digitalWrite(motorPin1, HIGH);
  digitalWrite(motorPin2, HIGH);
  digitalWrite(motorPin3, HIGH);
  digitalWrite(motorPin4, LOW);
  analogWrite(leftMotorSpeedPin, 255); 
  analogWrite(rightMotorSpeedPin, 255);
}

void moveBackward() {
  Serial.println("Moving Backward");
  digitalWrite(motorPin1, LOW);
  digitalWrite(motorPin2, HIGH);
  digitalWrite(motorPin3, LOW);
  digitalWrite(motorPin4, HIGH);
  analogWrite(leftMotorSpeedPin, 255); 
  analogWrite(rightMotorSpeedPin, 255); 
}

void moveLeft(){
    Serial.println("Moving Left");
  analogWrite(leftMotorSpeedPin, 155); 
  analogWrite(rightMotorSpeedPin, 255); 
}

void moveRight(){
    Serial.println("Moving Right");
  analogWrite(leftMotorSpeedPin, 255); 
  analogWrite(rightMotorSpeedPin, 155); 
}

void stopMotors() {
  Serial.println("Stopping Motors");
  digitalWrite(motorPin1, LOW);
  digitalWrite(motorPin2, LOW);
  digitalWrite(motorPin3, LOW);
  digitalWrite(motorPin4, LOW);
}

void readAccelerometer() {
  sensors_event_t event;
  mpu.getAccelerometerSensor()->getEvent(&event);
  Serial.print("Acceleration X: ");
  Serial.print(event.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(event.acceleration.y);
  Serial.print(", Z: ");
  Serial.println(event.acceleration.z);
  int threshold = 1000;
  if (event.acceleration.z > threshold) {
    moveForward();
  } else if (event.acceleration.z < -threshold) {
    moveBackward();
  }
}

void readGyroscope() {
  sensors_event_t event;
  mpu.getGyroSensor()->getEvent(&event);
  Serial.print("Gyro X: ");
  Serial.print(event.gyro.x);
  Serial.print(", Y: ");
  Serial.print(event.gyro.y);
  Serial.print(", Z: ");
  Serial.println(event.gyro.z);
}

void setup() {
  Serial.begin(9600);

  loadCoefficients();

  Serial.println("Select an action:");
  Serial.println("start");
  Serial.println("pControl");
  Serial.println("piControl");
  Serial.println("pdControl");
  Serial.println("pidControl");
  Serial.println("Enter the coefficients in the format Kp=..., Ki=..., Kd=...");

  pinMode(motorPin1, OUTPUT);
  pinMode(motorPin2, OUTPUT);
  pinMode(motorPin3, OUTPUT);
  pinMode(motorPin4, OUTPUT);
  pinMode(leftMotorSpeedPin, OUTPUT);
  pinMode(rightMotorSpeedPin, OUTPUT);

  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 chip found");


  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  Serial.println("Gyro range set to 250 degrees per second");


  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  Serial.println("Accel range set to 2G");
}


void loop() {
  if (Serial.available()) {
    String command = Serial.readStringUntil('\n');
    command.trim();
    Serial.print("Received command: ");
    Serial.println(command);
    if (command == "start") {
      isRunning = true;
      Serial.println("Starting...");
    } else if (command == "stop") {
      isRunning = false;
      stopMotors();
      Serial.println("Stopping...");
    } else if (command == "pControl") {
      selectedMode = "pControl";
      Serial.println("Selected pControl");
    } else if (command == "pdControl") {
      selectedMode = "pdControl";
      Serial.println("Selected pdControl");
    } else if (command == "piControl") {
      selectedMode = "piControl";
      Serial.println("Selected piControl");
    } else if (command == "pidControl") {
      selectedMode = "pidControl";
      Serial.println("Selected pidControl");
    } else if (command.startsWith("Kp=")) {
      Kp = command.substring(3).toFloat();
      saveCoefficients();
      Serial.print("Kp set to ");
      Serial.println(Kp);
    } else if (command.startsWith("Ki=")) {
      Ki = command.substring(3).toFloat();
      saveCoefficients();
      Serial.print("Ki set to ");
      Serial.println(Ki);
    } else if (command.startsWith("Kd=")) {
      Kd = command.substring(3).toFloat();
      saveCoefficients();
      Serial.print("Kd set to ");
      Serial.println(Kd);
    }
  }


  if (isRunning) {
    // moveForward();

  unsigned long microphoneInterval = 1000; 
  unsigned long accelerometerInterval = 500; 
  
  unsigned long currentMillis = millis();

  if (currentMillis - lastMicrophoneTime >= microphoneInterval) {
    lastMicrophoneTime = currentMillis;
    processMicrophone();
  }

  if (currentMillis - lastAccelerometerTime >= accelerometerInterval) {
    lastAccelerometerTime = currentMillis;
    readAccelerometer();
  }
  }
}


void pControl() {
  float desiredSpeed = 255; 
  float error = desiredSpeed - analogRead(micPin);
  float motorSpeed = Kp * error; 

  motorSpeed = constrain(motorSpeed, 0, 255);

  analogWrite(leftMotorSpeedPin, motorSpeed);
  analogWrite(rightMotorSpeedPin, motorSpeed);

}

void pdControl() {
  float desiredSpeed = 255; 
  float currentSpeed = analogRead(micPin); 
  static float lastError = 0; 
  float error = desiredSpeed - currentSpeed;
  float dError = error - lastError; 
  float motorSpeed = Kp * error + Kd * dError;

  motorSpeed = constrain(motorSpeed, 0, 255);

  analogWrite(leftMotorSpeedPin, motorSpeed);
  analogWrite(rightMotorSpeedPin, motorSpeed);

  lastError = error;
}

void piControl() {
  float desiredSpeed = 255; 
  float currentSpeed = analogRead(micPin);
  static float totalError = 0;
  float error = desiredSpeed - currentSpeed; 
  totalError += error; 
  float motorSpeed = Kp * error + Ki * totalError; 
  motorSpeed = constrain(motorSpeed, 0, 255);

  analogWrite(leftMotorSpeedPin, motorSpeed);
  analogWrite(rightMotorSpeedPin, motorSpeed);

}

void pidControl() {
  float desiredSpeed = 255; 
  float currentSpeed = analogRead(micPin);
  static float lastError = 0; 
  static float totalError = 0;
  float error = desiredSpeed - currentSpeed;
  totalError += error; 
  float dError = error - lastError; 
  float motorSpeed = Kp * error + Ki * totalError + Kd * dError;

  motorSpeed = constrain(motorSpeed, 0, 255);

  analogWrite(leftMotorSpeedPin, motorSpeed);
  analogWrite(rightMotorSpeedPin, motorSpeed);

  lastError = error;

}