#include <Encoder.h>

// Define motor pins
#define MOTOR1_IN1 2
#define MOTOR1_IN2 3
#define MOTOR2_IN1 4
#define MOTOR2_IN2 5
#define MOTOR3_IN1 6
#define MOTOR3_IN2 7
#define MOTOR4_IN1 8
#define MOTOR4_IN2 9

// Define ultrasonic sensor pins
#define TRIG_PIN1 10
#define ECHO_PIN1 11
#define TRIG_PIN2 12
#define ECHO_PIN2 13
#define TRIG_PIN3 A0
#define ECHO_PIN3 A1

// Define encoders
Encoder motor1Encoder(14, 15);  // Motor 1 encoder pins
Encoder motor2Encoder(16, 17);  // Motor 2 encoder pins
Encoder motor3Encoder(18, 19);  // Motor 3 encoder pins
Encoder motor4Encoder(20, 21);  // Motor 4 encoder pins

// Ultrasonic sensor distance variables
long distance1, distance2, distance3;

// Encoder counts and surface detection variables
long prevEncoder1 = 0;
long prevEncoder2 = 0;
long prevEncoder3 = 0;
long prevEncoder4 = 0;
bool surfaceChanged = false;

// PID control variables
float kp = 0.6;
float ki = 0.3;
float kd = 0.05;
float lastError1 = 0, lastError2 = 0, lastError3 = 0, lastError4 = 0;
float iTerm1 = 0, iTerm2 = 0, iTerm3 = 0, iTerm4 = 0;

// Thresholds
int safeDistance = 20; // Ultrasonic sensor threshold (in cm)
int surfaceThreshold = 100;  // Encoder threshold for detecting surface change

// Constants for voltage measurement
#define VOLTAGE_PIN A2 // Analog pin for reading battery voltage
float voltageMultiplier = 11.0; // Based on the voltage divider resistor ratio
float nominalVoltage = 12.0;  // Nominal operating voltage of the robot's battery
float lowVoltageThreshold = 9.0;  // Voltage below which power drops significantly

void setup() {
  // Initialize motor pins
  pinMode(MOTOR1_IN1, OUTPUT);
  pinMode(MOTOR1_IN2, OUTPUT);
  pinMode(MOTOR2_IN1, OUTPUT);
  pinMode(MOTOR2_IN2, OUTPUT);
  pinMode(MOTOR3_IN1, OUTPUT);
  pinMode(MOTOR3_IN2, OUTPUT);
  pinMode(MOTOR4_IN1, OUTPUT);
  pinMode(MOTOR4_IN2, OUTPUT);

  // Initialize ultrasonic sensor pins
  pinMode(TRIG_PIN1, OUTPUT);
  pinMode(ECHO_PIN1, INPUT);
  pinMode(TRIG_PIN2, OUTPUT);
  pinMode(ECHO_PIN2, INPUT);
  pinMode(TRIG_PIN3, OUTPUT);
  pinMode(ECHO_PIN3, INPUT);

  Serial.begin(9600);  // Initialize serial communication

  // Run self-test on power-up
  performSelfTest();
}

void loop() {
  // Get distance readings from the ultrasonic sensors
  distance1 = getUltrasonicDistance(TRIG_PIN1, ECHO_PIN1);
  distance2 = getUltrasonicDistance(TRIG_PIN2, ECHO_PIN2);
  distance3 = getUltrasonicDistance(TRIG_PIN3, ECHO_PIN3);

  // Voltage compensation
  float batteryVoltage = readBatteryVoltage();
  adjustForVoltageDrop(batteryVoltage);

  // Check for obstacles and surface change
  if (distance1 > safeDistance && distance2 > safeDistance) {
    // Use PID for smooth motor control
    controlMotorsPID(200, 200, 200, 200);
  } else if (distance3 < safeDistance) {
    makeLeftTurn();  // Turn left if left-side sensor detects an obstacle
  } else {
    stopMotors();  // Stop if obstacles are detected in front
  }
  
  // Detect surface change using encoder feedback
  detectSurfaceChange();
}

// Function to control motors using PID
void controlMotorsPID(int setpoint1, int setpoint2, int setpoint3, int setpoint4) {
  long encoder1 = motor1Encoder.read();
  long encoder2 = motor2Encoder.read();
  long encoder3 = motor3Encoder.read();
  long encoder4 = motor4Encoder.read();

  // PID calculations for each motor
  float error1 = setpoint1 - encoder1;
  float error2 = setpoint2 - encoder2;
  float error3 = setpoint3 - encoder3;
  float error4 = setpoint4 - encoder4;

  iTerm1 += ki * error1;
  iTerm2 += ki * error2;
  iTerm3 += ki * error3;
  iTerm4 += ki * error4;

  float dTerm1 = kd * (error1 - lastError1);
  float dTerm2 = kd * (error2 - lastError2);
  float dTerm3 = kd * (error3 - lastError3);
  float dTerm4 = kd * (error4 - lastError4);

  float output1 = kp * error1 + iTerm1 + dTerm1;
  float output2 = kp * error2 + iTerm2 + dTerm2;
  float output3 = kp * error3 + iTerm3 + dTerm3;
  float output4 = kp * error4 + iTerm4 + dTerm4;

  // Control motors based on PID output
  setMotorSpeed(output1, MOTOR1_IN1, MOTOR1_IN2);
  setMotorSpeed(output2, MOTOR2_IN1, MOTOR2_IN2);
  setMotorSpeed(output3, MOTOR3_IN1, MOTOR3_IN2);
  setMotorSpeed(output4, MOTOR4_IN1, MOTOR4_IN2);

  lastError1 = error1;
  lastError2 = error2;
  lastError3 = error3;
  lastError4 = error4;
}

// Function to set motor speed
void setMotorSpeed(float speed, int in1, int in2) {
  if (speed > 0) {
    analogWrite(in1, speed);
    analogWrite(in2, 0);
  } else {
    analogWrite(in1, 0);
    analogWrite(in2, -speed);
  }
}

// Surface change detection function
void detectSurfaceChange() {
  long encoder1 = motor1Encoder.read();
  long encoder2 = motor2Encoder.read();
  long encoder3 = motor3Encoder.read();
  long encoder4 = motor4Encoder.read();

  long change1 = abs(encoder1 - prevEncoder1);
  long change2 = abs(encoder2 - prevEncoder2);
  long change3 = abs(encoder3 - prevEncoder3);
  long change4 = abs(encoder4 - prevEncoder4);

  if (change1 > surfaceThreshold || change2 > surfaceThreshold || change3 > surfaceThreshold || change4 > surfaceThreshold) {
    surfaceChanged = true;
    Serial.println("Surface change detected!");
    // Adjust motor behavior or speed for different surfaces here
  }

  prevEncoder1 = encoder1;
  prevEncoder2 = encoder2;
  prevEncoder3 = encoder3;
  prevEncoder4 = encoder4;
}

// Advanced navigation function
void advancedNavigation() {
  // Use ultrasonic sensors and encoder data to adjust speed or path dynamically
  if (distance1 < safeDistance || distance2 < safeDistance) {
    // If an obstacle is detected in front, make a decision
    if (distance3 < safeDistance) {
      makeLeftTurn();  // Turn left if there's space on the left
    } else {
      stopMotors();
      // Implement a decision-making algorithm here for complex navigation
    }
  } else {
    controlMotorsPID(200, 200, 200, 200);  // Continue moving forward if no obstacles
  }
}

// Self-test routine on power-up
void performSelfTest() {
  Serial.println("Starting self-test...");

  // Move forward
  moveForward();
  delay(500);
  stopMotors();

  // Turn left
  makeLeftTurn();
  delay(500);
  stopMotors();

  // Move reverse
  moveBackward();
  delay(500);
  stopMotors();

  // Turn right (opposite of left turn)
  makeRightTurn();
  delay(500);
  stopMotors();

  // Test ultrasonic sensors
  distance1 = getUltrasonicDistance(TRIG_PIN1, ECHO_PIN1);
  distance2 = getUltrasonicDistance(TRIG_PIN2, ECHO_PIN2);
  distance3 = getUltrasonicDistance(TRIG_PIN3, ECHO_PIN3);
  
  Serial.print("Ultrasonic Sensor 1: ");
  Serial.println(distance1);
  Serial.print("Ultrasonic Sensor 2: ");
  Serial.println(distance2);
  Serial.print("Ultrasonic Sensor 3: ");
  Serial.println(distance3);

  // Twitch forward
  moveForward();
  delay(200);
  stopMotors();

  // Twitch backward
  moveBackward();
  delay(200);
  stopMotors();

  Serial.println("Self-test complete.");
}

// Move functions
void moveForward() {
  digitalWrite(MOTOR1_IN1, HIGH);
  digitalWrite(MOTOR1_IN2, LOW);
  digitalWrite(MOTOR2_IN1, HIGH);
  digitalWrite(MOTOR2_IN2, LOW);
  digitalWrite(MOTOR3_IN1, HIGH);
  digitalWrite(MOTOR3_IN2, LOW);
  digitalWrite(MOTOR4_IN1, HIGH);
  digitalWrite(MOTOR4_IN2, LOW);
}

void moveBackward() {
  digitalWrite(MOTOR1_IN1, LOW);
  digitalWrite(MOTOR1_IN2, HIGH);
  digitalWrite(MOTOR2_IN1, LOW);
  digitalWrite(MOTOR2_IN2, HIGH);
  digitalWrite(MOTOR3_IN1, LOW);
  digitalWrite(MOTOR3_IN2, HIGH);
  digitalWrite(MOTOR4_IN1, LOW);
  digitalWrite(MOTOR4_IN2, HIGH);
}

void makeLeftTurn() {
  digitalWrite(MOTOR1_IN1, LOW);
  digitalWrite(MOTOR1_IN2, HIGH);  // Left motors reverse
  digitalWrite(MOTOR2_IN1, LOW);
  digitalWrite(MOTOR2_IN2, HIGH);
  digitalWrite(MOTOR3_IN1, HIGH);  // Right motors forward
  digitalWrite(MOTOR3_IN2, LOW);
  digitalWrite(MOTOR4_IN1, HIGH);
  digitalWrite(MOTOR4_IN2, LOW);
}

void makeRightTurn() {
  digitalWrite(MOTOR1_IN1, HIGH);
  digitalWrite(MOTOR1_IN2, LOW);   // Left motors forward
  digitalWrite(MOTOR2_IN1, HIGH);
  digitalWrite(MOTOR2_IN2, LOW);
  digitalWrite(MOTOR3_IN1, LOW);   // Right motors reverse
  digitalWrite(MOTOR3_IN2, HIGH);
  digitalWrite(MOTOR4_IN1, LOW);
  digitalWrite(MOTOR4_IN2, HIGH);
}

void stopMotors() {
  digitalWrite(MOTOR1_IN1, LOW);
  digitalWrite(MOTOR1_IN2, LOW);
  digitalWrite(MOTOR2_IN1, LOW);
  digitalWrite(MOTOR2_IN2, LOW);
  digitalWrite(MOTOR3_IN1, LOW);
  digitalWrite(MOTOR3_IN2, LOW);
  digitalWrite(MOTOR4_IN1, LOW);
  digitalWrite(MOTOR4_IN2, LOW);
}

// Function to read the battery voltage using a voltage divider
float readBatteryVoltage() {
  int analogValue = analogRead(VOLTAGE_PIN);  // Read analog value
  float voltage = (analogValue / 1023.0) * 5.0 * voltageMultiplier;  // Convert to actual voltage
  Serial.print("Battery Voltage: ");
  Serial.println(voltage);
  return voltage;
}

// Function to adjust for voltage drop
void adjustForVoltageDrop(float batteryVoltage) {
  if (batteryVoltage < nominalVoltage) {
    float voltageRatio = batteryVoltage / nominalVoltage;

    // Adjust motor power and PID values based on voltage ratio
    kp = kp / voltageRatio;
    ki = ki / voltageRatio;
    kd = kd / voltageRatio;

    Serial.print("Adjusted kp: ");
    Serial.println(kp);
  }
}

// Function to get distance from an ultrasonic sensor
long getUltrasonicDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);  // Clear the trigger
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);  // Set the trigger high
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);  // Read the echo pin
  long distance = duration * 0.034 / 2;  // Calculate the distance in cm
  return distance;
}