#include <AccelStepper.h>
#include <queue>
#include <WiFi.h>
#include <vector>
#include <cmath>

#define MOTOR_INTERFACE_TYPE 4

// Define pins for the left motor
#define LEFT_MOTOR_PIN1 13  // IN1 on L293N
#define LEFT_MOTOR_PIN2 12  // IN2 on L293N
#define LEFT_MOTOR_PIN3 14  // IN3 on L293N
#define LEFT_MOTOR_PIN4 27  // IN4 on L293N

// Define pins for the right motor
#define RIGHT_MOTOR_PIN1 26 // IN1 on L293N
#define RIGHT_MOTOR_PIN2 25 // IN2 on L293N
#define RIGHT_MOTOR_PIN3 33 // IN3 on L293N
#define RIGHT_MOTOR_PIN4 32 // IN4 on L293N

// Define pins for the ultrasonic sensors and bumper
#define FRONT_TRIG_PIN 15
#define FRONT_ECHO_PIN 4

#define BUMPER_PIN 21  // Define pin for the bumper button

AccelStepper leftMotor(MOTOR_INTERFACE_TYPE, LEFT_MOTOR_PIN1, LEFT_MOTOR_PIN2, LEFT_MOTOR_PIN3, LEFT_MOTOR_PIN4);
AccelStepper rightMotor(MOTOR_INTERFACE_TYPE, RIGHT_MOTOR_PIN1, RIGHT_MOTOR_PIN2, RIGHT_MOTOR_PIN3, RIGHT_MOTOR_PIN4);

// Structure to store coordinates
struct Coordinate {
  float x;
  float y;
};

std::vector<Coordinate> path; // Store target coordinates
bool isExecuting = false;

float posX = 0.0;
float posY = 0.0;
float heading = 0.0;

float obstacleThreshold = 0.5; // Threshold for obstacle detection in meters

enum RobotState { IDLE, ROTATING, MOVING_FORWARD, BACKWARD, DETOUR_ROTATE, DETOUR_MOVE };
RobotState robotState = IDLE;

void initWiFiConnection();
void initWheelsMotors();
void initUltrasonicCaptors();
void initBumper();
void addInitialTargets();
void processInstructions();
void moveToTarget(float targetX, float targetY);
float calculateDistance(float x1, float y1, float x2, float y2);
float calculateAngle(float x1, float y1, float x2, float y2);
void updatePosition(float distance);
void updateHeading(float angle);
void checkForObstacles();
void checkBumper();
float getDistanceFromSensor(int trigPin, int echoPin);
void recalibratePath();

void setup() {
  Serial.begin(115200);
  Serial.println("Robot Navigation Program Starting...");

  initWiFiConnection();
  initWheelsMotors();
  initUltrasonicCaptors();
  initBumper();
  addInitialTargets();

  if (!path.empty()) {
    moveToTarget(path[0].x, path[0].y); // Move to the first target coordinate
  }
}

void loop() {
  processInstructions();
}

void initWiFiConnection() {
  Serial.print("Connecting to WiFi");
  WiFi.begin("Wokwi-GUEST", "", 6);
  while (WiFi.status() != WL_CONNECTED) {
    delay(100);
    Serial.print(".");
  }
  Serial.println(" Connected!");
}

void initWheelsMotors() {
  leftMotor.setMaxSpeed(1000);  
  leftMotor.setAcceleration(500);
  rightMotor.setMaxSpeed(1000); 
  rightMotor.setAcceleration(500); 
}

void initUltrasonicCaptors() {
  pinMode(FRONT_TRIG_PIN, OUTPUT);
  pinMode(FRONT_ECHO_PIN, INPUT);
}

void initBumper() {
  pinMode(BUMPER_PIN, INPUT_PULLUP); // Set the bumper pin as input with a pull-up resistor
}

void addInitialTargets() {
  // Targets for the robot to move to
  path.push_back({1.0, 2.0});
  path.push_back({2.0, 1.0});
  path.push_back({0.0, 0.0});  // Return to start
}

// Rotate by a specified angle
void rotate(float angle) {
  int steps = angleToSteps(angle);
  
  Serial.print("Rotating by ");
  Serial.print(angle);
  Serial.print(" degrees (");
  Serial.print(steps);
  Serial.println(" steps).");

  updateHeading(angle);

  // Rotate by moving the motors in opposite directions
  leftMotor.move(steps);
  rightMotor.move(-steps);

  robotState = ROTATING; // Set state to rotating
}

void moveForward(float distance) {
  int steps = distanceToSteps(distance);

  Serial.println("Moving forward");
  updatePosition(distance);
  leftMotor.move(steps);
  rightMotor.move(steps);

  robotState = MOVING_FORWARD; // Set state to moving forward
}

void moveBackward(float distance) {
  int steps = distanceToSteps(distance);
  
  Serial.println("Moving backward");
  updatePosition(-distance); // Update position to simulate moving backward
  leftMotor.move(-steps);
  rightMotor.move(-steps);
  
  robotState = BACKWARD; // Set state to moving backward
}

void moveToTarget(float targetX, float targetY) {
  // Calculate distance and angle to the target
  float distance = calculateDistance(posX, posY, targetX, targetY);
  float angle = calculateAngle(posX, posY, targetX, targetY);

  Serial.print("Moving to target: (");
  Serial.print(targetX);
  Serial.print(", ");
  Serial.print(targetY);
  Serial.println(")");

  rotate(angle); // Start with the rotation first, then move forward
}

void processInstructions() {
  // Always run the motors to make progress toward the target
  leftMotor.run();
  rightMotor.run();

  // Check for bumper collisions only if we are NOT moving backward or detouring
  if (robotState != BACKWARD && robotState != DETOUR_MOVE) {
    checkBumper();
  }

  // Check if the current task (rotation or forward movement) is complete
  if (robotState == ROTATING && leftMotor.distanceToGo() == 0 && rightMotor.distanceToGo() == 0) {
    Serial.println("Rotation completed. Moving forward...");
    
    // Move forward to the target
    float targetX = path[0].x;
    float targetY = path[0].y;
    float distance = calculateDistance(posX, posY, targetX, targetY);
    moveForward(distance);
  }

  if (robotState == MOVING_FORWARD && leftMotor.distanceToGo() == 0 && rightMotor.distanceToGo() == 0) {
    Serial.println("Movement completed. Moving to the next target...");

    // Mark the current instruction as completed
    isExecuting = false;

    if (!path.empty()) {
      path.erase(path.begin());

      if (!path.empty()) {
        moveToTarget(path[0].x, path[0].y);
      } else {
        Serial.println("All tasks completed.");
        robotState = IDLE;
      }
    }
  }

  // After moving backward, rotate for detour
  if (robotState == BACKWARD && leftMotor.distanceToGo() == 0 && rightMotor.distanceToGo() == 0) {
    Serial.println("Backing up completed. Starting detour rotation...");
    rotate(45);  // Rotate by 45 degrees for a detour
    robotState = DETOUR_ROTATE;
  }

  // After detour rotation, move forward slightly to complete the detour
  if (robotState == DETOUR_ROTATE && leftMotor.distanceToGo() == 0 && rightMotor.distanceToGo() == 0) {
    Serial.println("Detour rotation completed. Moving forward slightly for detour...");
    moveForward(0.3);  // Move 0.3 meters forward to clear the obstacle
    robotState = DETOUR_MOVE;
  }

  // After moving forward during the detour, continue with the original target
  if (robotState == DETOUR_MOVE && leftMotor.distanceToGo() == 0 && rightMotor.distanceToGo() == 0) {
    Serial.println("Detour completed. Reattempting original path...");
    
    // Move forward again after detour
    if (!path.empty()) {
      moveToTarget(path[0].x, path[0].y);
    } else {
      Serial.println("All tasks completed.");
      robotState = IDLE;
    }
  }
}

// Function to check if the bumper has been pressed (collision detected)
void checkBumper() {
  if (digitalRead(BUMPER_PIN) == LOW) { // Button pressed (collision)
    Serial.println("Bumper collision detected! Stopping and going backward...");

    // Stop both motors immediately by setting the current position to 0
    leftMotor.setCurrentPosition(0);
    rightMotor.setCurrentPosition(0);

    // Move backward slightly to clear the obstacle
    moveBackward(0.3); // Move 0.3 meters backward
  }
}

float calculateDistance(float x1, float y1, float x2, float y2) {
  return sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
}

float calculateAngle(float x1, float y1, float x2, float y2) {
  return atan2(y2 - y1, x2 - x1) * 180 / PI - heading;
}

void updatePosition(float distance) {
  posX += distance * cos(heading * PI / 180);
  posY += distance * sin(heading * PI / 180);
  Serial.print("Updated position: X = ");
  Serial.print(posX);
  Serial.print(", Y = ");
  Serial.println(posY);
}

void updateHeading(float angle) {
  heading += angle;
  heading = fmod(heading, 360.0);
  Serial.print("Updated heading: ");
  Serial.println(heading);
}

void recalibratePath() {
  // Create a detour by adjusting the path
  float detourX = posX + 0.5;
  float detourY = posY + 0.5;

  Serial.println("Adding detour to avoid collision...");

  // Insert the detour as the next target
  path.insert(path.begin(), {detourX, detourY});
  
  // Reset state machine to move toward the new detour
  if (!path.empty()) {
    moveToTarget(path[0].x, path[0].y);
    robotState = IDLE;
  }
}

int distanceToSteps(float distance) {
  float stepsPerMeter = 1000.0;
  return distance * stepsPerMeter;
}

int angleToSteps(float angle) {
  float stepsPerDegree = 10.0;
  return angle * stepsPerDegree;
}