#include <Servo.h>
// #include <PID_v1.h> // Uncomment if using PID library
// Include other necessary libraries for sensors and gyro

// Define FSM states
enum State {
  STATE_1,
  STATE_2,
  STATE_3,
  STATE_4,
  STATE_5,
  STATE_6,
  STATE_7
};

State currentState = STATE_1;

// Define sensor pins
const int leftUltrasonicTrig = 2;
const int leftUltrasonicEcho = 3;
const int rightUltrasonicTrig = 4;
const int rightUltrasonicEcho = 5;
const int frontUltrasonicTrig = 6;
const int frontUltrasonicEcho = 7;

// Define motor pins
const int motorSpeedPin = 9;       // PWM pin for speed control
const int motorDirPin1 = 10;       // Direction pin 1
const int motorDirPin2 = 11;       // Direction pin 2

// Define servo pin
const int servoPin = 12;
Servo steeringServo;

// PID variables (optional)
double Kp = 1.0, Ki = 0.0, Kd = 0.0;
double input, output, setpoint;
// PID myPID(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

// Define other variables
long leftDistance = 0;
long rightDistance = 0;
long frontDistance = 0;
int steeringAngle = 120; // Default neutral angle
int motorSpeed = 0;
char incomingByte;
bool objDetected = false;
int counter = 0;

// Gyro variables
float currentHeading = 0.0;
float targetHeading = 0.0;

// Timing for movements
unsigned long backStartTime = 0;
const unsigned long backDuration = 500; // 500 ms

// Function prototypes
long readUltrasonicDistance(int trigPin, int echoPin);
void moveMotor(int speed, bool forward);
void stopMotor();
bool isIntercept(int steeringAngle);
void takeUTurn();
float readGyro(); // Implement based on your gyro library

void setup() {
  // Initialize Serial
  Serial.begin(9600);
  
  // Initialize Servo
  steeringServo.attach(servoPin);
  steeringServo.write(steeringAngle); // Set to neutral position
  
  // Initialize Motor Pins
  pinMode(motorSpeedPin, OUTPUT);
  pinMode(motorDirPin1, OUTPUT);
  pinMode(motorDirPin2, OUTPUT);
  
  // Initialize Ultrasonic Pins
  pinMode(leftUltrasonicTrig, OUTPUT);
  pinMode(leftUltrasonicEcho, INPUT);
  pinMode(rightUltrasonicTrig, OUTPUT);
  pinMode(rightUltrasonicEcho, INPUT);
  pinMode(frontUltrasonicTrig, OUTPUT);
  pinMode(frontUltrasonicEcho, INPUT);
  
  // Initialize Gyro
  // Initialize your gyro/magnetometer here
  
  // Initialize PID
  // setpoint = 0;
  // myPID.SetMode(AUTOMATIC);
}

void loop() {
  // Read sensor values
  leftDistance = readUltrasonicDistance(leftUltrasonicTrig, leftUltrasonicEcho);
  rightDistance = readUltrasonicDistance(rightUltrasonicTrig, rightUltrasonicEcho);
  frontDistance = readUltrasonicDistance(frontUltrasonicTrig, frontUltrasonicEcho);
  
  // Read gyro
  currentHeading = readGyro();
  
  // Read serial input
  if (Serial.available() > 0) {
    incomingByte = Serial.read();
    if (currentState == STATE_1 && incomingByte == 'n') {
      objDetected = false;
    } else if (incomingByte == 'd') {
      objDetected = true;
      // Read subsequent characters for angle
      String angleStr = "";
      while (Serial.available() > 0) {
        char c = Serial.read();
        if (c == '\n') break; // Assuming angle sent ends with newline
        angleStr += c;
      }
      steeringAngle = angleStr.toInt();
    } else if (incomingByte == 'i') {
      // Signal 'i' received indicating ideal position reached
      // Handle in relevant state
    }
  }
  
  // FSM Implementation
  switch (currentState) {
    case STATE_1:
      // Conditions:
      // Left and Right distances < d
      // Front distance > d1
      // No obstacle detected (serial 'n')
      if (leftDistance < D && rightDistance < D && frontDistance > D1 && !objDetected) {
        // Actions:
        steeringAngle = 120;
        steeringServo.write(steeringAngle);
        motorSpeed = 180;
        moveMotor(motorSpeed, true);
        
        // PID Control to center
        double pidError = leftDistance - rightDistance;
        // input = pidError;
        // myPID.Compute();
        // Adjust steering based on PID output
        // For simplicity, let's assume PID adjusts the steering angle incrementally
        // steeringServo.write(steeringAngle + (int)output);
        
        // Transition: Remain in STATE_1 until conditions change
        if (objDetected) {
          currentState = STATE_2;
        }
      }
      break;
      
    case STATE_2:
      // Conditions:
      // Left and Right distances < d
      // Object detected
      if (leftDistance < D && rightDistance < D && objDetected) {
        // Expect serial message with 'd' followed by angle has already been handled
        // Set steering angle based on serial input
        steeringServo.write(steeringAngle);
        motorSpeed = 150;
        moveMotor(motorSpeed, true);
        
        // Wait until 'i' signal is received
        // Implemented via serial read above
        if (incomingByte == 'i') {
          counter++;
          currentState = STATE_3;
        }
      }
      break;
      
    case STATE_3:
      // Conditions:
      // Left distance > x
      // Front distance < d1
      // Right distance < d
      // No obstacle detected
      if (leftDistance > X && frontDistance < D1 && rightDistance < D && !objDetected) {
        steeringAngle = 120 - 50; // 70 degrees
        steeringServo.write(steeringAngle);
        motorSpeed = 150;
        moveMotor(motorSpeed, true);
        
        // Set target heading
        targetHeading = currentHeading + 42; // Assuming 42 degrees to the right
        // Handle gyro wrap-around if necessary
        
        // Enter while loop until gyro reading reaches target
        while (abs(currentHeading - targetHeading) < 42) {
          // Continuously update currentHeading
          currentHeading = readGyro();
          
          // Continuously check for obstacle
          if (Serial.available() > 0) {
            char obstacle = Serial.read();
            if (obstacle == 'd') {
              // Read steering angle from serial
              String angleStr = "";
              while (Serial.available() > 0) {
                char c = Serial.read();
                if (c == '\n') break;
                angleStr += c;
              }
              steeringAngle = angleStr.toInt();
              steeringServo.write(steeringAngle);
              currentState = STATE_4;
              break;
            }
          }
        }
        // After while loop
        currentState = STATE_5;
      }
      break;
      
    case STATE_4:
      // Call isIntercept with current steering angle
      if (isIntercept(steeringAngle)) {
        // Move backward at speed 130 for 500 ms
        moveMotor(130, false);
        delay(backDuration);
        stopMotor();
        // Re-evaluate interception
        if (isIntercept(steeringAngle)) {
          // Repeat
          backStartTime = millis();
        } else {
          // Move forward at speed 130
          moveMotor(130, true);
          currentState = STATE_5;
        }
      } else {
        // Move forward at speed 130
        moveMotor(130, true);
        currentState = STATE_5;
      }
      break;
      
    case STATE_5:
      // Transition after State 3 Actions
      if (objDetected) {
        currentState = STATE_2;
      } else {
        currentState = STATE_1;
      }
      // Reset steering to neutral
      steeringServo.write(120);
      break;
      
    case STATE_6:
      // Similar to STATE_3 but for right-side conditions
      // Conditions:
      // Right distance > x
      // Front distance < d1
      // Left distance < d
      // No obstacle detected
      if (rightDistance > X && frontDistance < D1 && leftDistance < D && !objDetected) {
        steeringAngle = 120 + 50; // 170 degrees
        steeringServo.write(steeringAngle);
        motorSpeed = 150;
        moveMotor(motorSpeed, true);
        
        // Set target heading
        targetHeading = currentHeading - 42; // Assuming 42 degrees to the left
        // Handle gyro wrap-around if necessary
        
        // Enter while loop until gyro reading reaches target
        while (abs(currentHeading - targetHeading) < 42) {
          // Continuously update currentHeading
          currentHeading = readGyro();
          
          // Continuously check for obstacle
          if (Serial.available() > 0) {
            char obstacle = Serial.read();
            if (obstacle == 'd') {
              // Read steering angle from serial
              String angleStr = "";
              while (Serial.available() > 0) {
                char c = Serial.read();
                if (c == '\n') break;
                angleStr += c;
              }
              steeringAngle = angleStr.toInt();
              steeringServo.write(steeringAngle);
              currentState = STATE_4;
              break;
            }
          }
        }
        // After while loop
        currentState = STATE_5;
      }
      break;
      
    case STATE_7:
      // If counter > 7, handle U-turn when next object is detected
      if (counter > 7 && objDetected) {
        // Check if green signal from RPI (Assuming 'g' represents green)
        if (incomingByte == 'g') {
          currentState = STATE_2;
        } else {
          takeUTurn();
          // Reset counter after U-turn
          counter = 0;
        }
      }
      break;
      
    default:
      currentState = STATE_1;
      break;
  }
}

// Function to read ultrasonic distance
long readUltrasonicDistance(int trigPin, int echoPin) {
  long duration, distance;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH, 30000); // Timeout after ~30ms
  distance = duration * 0.034 / 2; // Convert to cm
  return distance;
}

// Function to control motor movement
void moveMotor(int speed, bool forward) {
  analogWrite(motorSpeedPin, speed);
  if (forward) {
    digitalWrite(motorDirPin1, HIGH);
    digitalWrite(motorDirPin2, LOW);
  } else {
    digitalWrite(motorDirPin1, LOW);
    digitalWrite(motorDirPin2, HIGH);
  }
}

// Function to stop the motor
void stopMotor() {
  analogWrite(motorSpeedPin, 0);
  digitalWrite(motorDirPin1, LOW);
  digitalWrite(motorDirPin2, LOW);
}

// Placeholder for isIntercept function
bool isIntercept(int steeringAngle) {
  // Implement interception logic based on steering angle
  // Return true if intercept occurs, else false
  // For example purposes, return false
  return false;
}

// Placeholder for takeUTurn function
void takeUTurn() {
  // Implement U-turn logic
  // For example, turn servo to 180 deg, move forward, etc.
  steeringServo.write(180);
  moveMotor(150, true);
  delay(1000); // Move forward for 1 second
  steeringServo.write(60); // Reverse steering
  delay(1000); // Turn for 1 second
  steeringServo.write(120); // Return to neutral
  stopMotor();
  currentState = STATE_1;
}

// Placeholder for reading gyro data
float readGyro() {
  // Implement gyro reading based on your gyro library
  // For example, return simulated heading
  return 0.0;
}
modularize this code better