#include <Adafruit_Sensor.h>
#include <Adafruit_HMC5883_U.h>
#include <Servo.h>



// Servo Pin
const int SERVO_PIN = 9;

// Motor Pins
const int IN1 = 6;
const int IN2 = 7;
const int ENA = 5;

bool isClockwise=true;
bool setDirection=false;

// Variables

Servo servo;
double leftDistance, rightDistance;
double PID_error;
double steering_angle_degrees, prev_err = 0;
const double std_steering_angle = 90.0; // standard steering angle
const double Kp = 0.08, Kd = 0.15, Ki = 0.0;
double sum_err = 0;
int pos = std_steering_angle;
int no_of_turns = 0;
float heading=0.0;

// Variables to track the state after an interrupt

volatile bool isWhite = true;
volatile bool isBlue=true;


// State Machine States
enum State {
  FOLLOW_LINE,
  TURN_LEFT,
  TURN_RIGHT,
  STOP
};

State currentState = FOLLOW_LINE;

// Objects

Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);

// Function Prototypes

double PID(double err);
void updateHeading();

void moveForward(int speed);
void stop();
void turnServo(int angle);


void setup() {
  Serial.begin(115200);
 pinMode(13, OUTPUT);
  // Initialize Servo
  servo.attach(SERVO_PIN);
  turnServo(std_steering_angle);
Serial.println("Initialize Servo");
  // Initialize Motor Pins
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(ENA, OUTPUT);

  // Initialize HMC5883L Compass
  if (!mag.begin()) {
    Serial.println("Failed to initialize HMC5883L");
    while (1);
  }
 // Setup hardware interrupts on pin2 and pin3
  //pinMode(2, INPUT);
  //pinMode(3, INPUT);
  //pinMode(2, INPUT_PULLUP);
  //pinMode(3, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(2), handleNotWhiteInterrupt, RISING);  // Interrupt for pin2 (trigger on rising edge)
attachInterrupt(digitalPinToInterrupt(3), handleNotBlueInterrupt, RISING); 
}

void loop() {
  //digitalWrite(13,LOW);

  if (isWhite==1) 
  {
    currentState=FOLLOW_LINE;
    Serial.println("----------------");
  }
  else 
  {
    detachInterrupt(digitalPinToInterrupt(2));
    detachInterrupt(digitalPinToInterrupt(3));
    if(isBlue==1)
    {
      if(setDirection==false)
      {
        isClockwise=false;
        digitalWrite(13,HIGH);
        setDirection=true;
      }
      currentState=TURN_LEFT;
      Serial.println("+++++++++++++++");
    }
    else
    {
       if(setDirection==false)
       {
        isClockwise=true;
        digitalWrite(13,HIGH);
        setDirection=true;
       }
      currentState=TURN_RIGHT;
      Serial.println("XXXXXXXXXXXXXXX");
    }
  }

    Serial.print("currentState:");
    Serial.println(currentState);
     float startHeading = heading;
  switch (currentState) {
    

   
    case FOLLOW_LINE:
    
      moveForward(120);
      //leftDistance = readUltrasonic(TRIG_L, ECHO_L);
      //rightDistance = readUltrasonic(TRIG_R, ECHO_R);
      PID_error = leftDistance - rightDistance;
      steering_angle_degrees = (30 * PID(PID_error)) + std_steering_angle;
      steering_angle_degrees = constrain(steering_angle_degrees, std_steering_angle - 45, std_steering_angle + 45);
      turnServo(steering_angle_degrees);
      break;

    case TURN_LEFT:
      Serial.print("isClockwise:");
      Serial.println(isClockwise);
      if(isClockwise == false)
      {
      moveForward(130);
      turnServo(std_steering_angle - 40);
      updateHeading();
      startHeading = heading;
      double deltaAngle=0;
      
      while (deltaAngle < 42) {
        deltaAngle=abs(heading - startHeading);
        Serial.println(deltaAngle);
        updateHeading();
      }
      turnServo(std_steering_angle);
      no_of_turns++;
      attachInterrupt(digitalPinToInterrupt(2), handleNotWhiteInterrupt, RISING);  // Interrupt for pin2 (trigger on rising edge)
      attachInterrupt(digitalPinToInterrupt(3), handleNotBlueInterrupt, RISING);
      }
      
      
      currentState = FOLLOW_LINE;
      isWhite=1;
      break;

    case TURN_RIGHT:
    Serial.print("isClockWise=");
    Serial.println(isClockwise);
    if(isClockwise==true)
    {
      moveForward(130);
      turnServo(std_steering_angle + 40);
      updateHeading();
      startHeading = heading;
      while (abs(heading - startHeading) < 42) {
        updateHeading();
      }
      turnServo(std_steering_angle);
      no_of_turns++;
      attachInterrupt(digitalPinToInterrupt(2), handleNotWhiteInterrupt, RISING);  // Interrupt for pin2 (trigger on rising edge)
      attachInterrupt(digitalPinToInterrupt(3), handleNotBlueInterrupt, RISING);
    }
    
      
      currentState = FOLLOW_LINE;
      isWhite=1;
      break;

    case STOP:
      stop();
      while (1);
      break;
  }

  if (no_of_turns >= 12) {
    currentState = STOP;
  }
}






double PID(double err) {
  sum_err += err;
  double result = ((Kp * err) + (Ki * sum_err) + (Kd * (err - prev_err)));
  prev_err = err;
  return result;
}

void updateHeading() {
  sensors_event_t event;
  mag.getEvent(&event);
  heading = atan2(event.magnetic.y, event.magnetic.x) * 180 / PI;
  if (heading < 0) {
    heading += 360;
  }
}
// ISR for not-white detection
void handleNotWhiteInterrupt() {
   isWhite=false; // Set flag in the ISR
  
   //digitalWrite(13,HIGH);
   //Serial.println("Interrupted");
   
}

// ISR for not-blue detection
void handleNotBlueInterrupt() {
  isBlue=false;   // Set flag in the ISR
   //digitalWrite(13,HIGH);
  
}

void moveForward(int speed) {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  analogWrite(ENA, speed);
}

void stop() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  analogWrite(ENA, 0);
}

void turnServo(int angle) {
  angle = constrain(angle, 0, 180);
  if (pos != angle) {
    while (pos != angle) {
      pos += (pos < angle) ? 1 : -1;
      servo.write(pos);
      delay(5);
      }
      }
      }
remove everything related to interrupts and color sensor. new logic - if front ultrasonic reads less than 40 and left reads less than 30 and right reads more than 80, turn right other wise if front reads less than 40 and left reads more than 80 and right reads less than 30 turn left, implement the fsm that way