#include "mbed.h"
#include "Serial.h"
#include "mbed2/299/platform/wait_api.h"
#include <ctime>


// Define named constants
//const int PWM_PERIOD_MS = 20;
const int MAX_RETRIES = 5;
const int COMMUNICATION_ERROR_RETRY_DELAY_MS = 1000;
const int SERVO_MIN_ANGLE = 30;
const int SERVO_MAX_ANGLE = 150;
const int SERVO_MID_ANGLE = 90;
const int SENSOR_MEDIUM_THRESHOLD = 800;
const int RETRY_DELAY = 400;
const float PUMPER_STRENGTH = 0.8f;
const float PUMPER_OFF = 0.0f;
const int AUTONOMOUS_MODE_SERVO_SPEED = 10;
const int AUTONOMOUS_MODE_SENSOR_THRESHOLD = 500;
const int AUTONOMOUS_MODE_SERVO_WAIT_TIME = 20;
Timer timer;
unsigned int x = 20;
unsigned int y = 50;
uint32_t last_update_time = 0;



// Define serial communication
Serial bluetooth(PTB2, PTB1);  // TX, RX pins

// Define motor and servo pins
DigitalOut Fspin(D2);           // Pin for motor 1 forward direction (in1)
DigitalOut Rspin(D3);           // Pin for motor 1 backward direction (in2)
PwmOut Speed(D4);               // Pin for motor speed control (ena1)
DigitalOut Pumper(D9);          // Pin for the 9V water pump forward direction
DigitalOut pump(D12);           // Pin for pump control backward direction
PwmOut pumperSTRENGTH(D8);      // Pin for PUMP motor speed (ena2)
AnalogIn Fleft(D7);             // Left sensor
AnalogIn Fmiddle(D6);           // Middle sensor
AnalogIn Fright(D5);            // Right sensor
PwmOut Canon(D10);              // Water sprayer servo
PwmOut Rudder(D11);             // Boat direction servo

// Define global variables
char State = 'M';
int CanonVal; // initial position of Water canon
int RudderVal = 50; // initial directional position of boat
int Boat_SPEED; //initial boat speed
float Speedometer;
float pumperS;
float BoatSPEED;
float LsenseVAL, MsenseVAL, RsenseVAL; //sensor readings
int buttonState = 0;         // the current reading from the input pin
int lastButtonState = 0;     // the previous reading from the input pin
int brakeTime = 500;

// Function prototypes
bool checkCommunicationError();
bool checkServoError(PwmOut& servo, int expectedPosition);
bool CanonPulse(PwmOut& Canon, int angle);
bool RudderPulse(PwmOut& Rudder, int angle);
void forward();
void backward();
void turnLeft();
void turnRight();
void Stop();
void setRudderPulseWidth(int RudderVal);
void moveForward();
void moveBackward();
void moveSharpLeft();
void moveSharpRight();
void moveLeft();
void moveRight();
void moveSTOP();
void controlCanon();
void SPEED();
void MOVEMENT();
void Autonomous();
void readSensorValues(int& RsenseVAL, int& MsenseVAL, int& LsenseVAL);
void handleServoError(int angle, int retries);
bool moveServoToAngle(PwmOut& Canon, int angle, int MAX_RETRIES);
void moveServoOverRange(PwmOut& Canon, int startAngle, int endAngle);
void Control();
void brakeOn();


// Define functions
bool checkCommunicationError() {
    return !(bluetooth.readable() && bluetooth.writeable());
}

bool checkServoError(PwmOut& servo, int expectedPosition) {
    float actualPosition = (servo.read() * 0.18) -180;// Convert to microseconds
    return (abs(actualPosition - expectedPosition) > 90); // 50 microseconds tolerance
}

bool CanonPulse(PwmOut& Canon, int angle) {
    // Calculate the pulse width in microseconds based on the angle
    int pulseWidth = 1000 + (angle * 5.556);
    if (pulseWidth > 2000) {
        pulseWidth = 2000; // Limit pulse width to 2400 microseconds
    } else if (pulseWidth < 1000) {
        pulseWidth = 1000; // Limit pulse width to 500 microseconds
    }
    // Set the pulse width on the servo motor
    Canon.pulsewidth_us(pulseWidth);
    wait_ms(500); 

    if (checkServoError(Canon, angle)) {
        bluetooth.printf("Error: Canon failed to reach position %d\r\n", angle);
        return false;
    }
    return true;
}

bool RudderPulse(PwmOut& Rudder, int angle) {
    int pulseWidth = 1000 + (angle * 5.556);
    if (pulseWidth > 2000) {
        pulseWidth = 2000; // Limit pulse width to 2400 microseconds
    } else if (pulseWidth < 1000) {
        pulseWidth = 1000; // Limit pulse width to 500 microseconds
    }
    Rudder.pulsewidth_us(pulseWidth); 
    wait_ms(500); // Give the servo some time to move
    if (checkServoError(Rudder, angle)) {
        bluetooth.printf("Error: Rudder failed to reach position %d\r\n", angle);
        return false;
    }
    return true;
}

void forward() {
    Fspin = 1;
    Rspin = 0;
    BoatSPEED = 40 / 100.0f;
    Speed.write(BoatSPEED);
    if (RudderVal == 90) {
        RudderPulse(Rudder, RudderVal);
        wait_ms(200);
    }
}

void backward() {
    Fspin = 0;
    Rspin = 1;
    BoatSPEED = 40 / 100.0f;
    Speed.write(BoatSPEED);
    if (RudderVal == 90) {
        RudderPulse(Rudder, RudderVal);
        wait_ms(200);
    }
}

void turnLeft() {
    Fspin = 1;
    Rspin = 0;
    BoatSPEED = 30 / 100.0f;
    Speed.write(BoatSPEED);
    for (RudderVal = 90; RudderVal >= 0; RudderVal -= 10) {
        RudderPulse(Rudder, RudderVal);
        if (!RudderPulse(Rudder, RudderVal)) {
            bluetooth.printf("Error turning left. Stopping turn.\r\n");
            break;
        }
    }
    wait_ms(300);
    for (RudderVal = 0; RudderVal <= 90; RudderVal += 90) {
        RudderPulse(Rudder, RudderVal);
    }
}

void turnRight() {
    Fspin = 1;
    Rspin = 0;
    BoatSPEED = 30 / 100.0f;
    Speed.write(BoatSPEED);
    for (RudderVal = 90; RudderVal <= 180; RudderVal += 10) {
        RudderPulse(Rudder, RudderVal);
        wait_ms(50);
        if (!RudderPulse(Rudder, RudderVal)) {
            bluetooth.printf("Error turning right. Stopping turn.\r\n");
            break;
        }
    }
    wait_ms(300);
    for (RudderVal = 180; RudderVal >= 90; RudderVal -= 90) {
        RudderPulse(Rudder, RudderVal);
    }
}

void Stop() {
    Fspin = 0;
    Rspin = 0;
    BoatSPEED = 0 / 100.0f;
    Speed.write(BoatSPEED);
}


//the BELOW are sub functions for the "VOID MANUAL"
// Function to calculate pulse width
int calculatePulseWidth(int value) {
    int pulseWidth = 1000 + (value * 5.556);
    if (pulseWidth > 2000) {
        pulseWidth = 2000; // Limit pulse width to 2400 microseconds
    } else if (pulseWidth < 1000) {
        pulseWidth = 1000; // Limit pulse width to 500 microseconds
    }
    return pulseWidth;
}

// Function to set rudder pulse width
void setRudderPulseWidth(int RudderVal) {
    int pulseWidth = calculatePulseWidth(RudderVal);
    Rudder.pulsewidth_us(pulseWidth);
    wait_ms(20);
}

// Function to move forward
void moveForward() {
    Fspin = 1;
    Rspin = 0;
    RudderVal = 90;
    setRudderPulseWidth(RudderVal);
    Speed.write(Speedometer);
}

// Function to move backward
void moveBackward() {
    Fspin = 0;
    Rspin = 1;
    Speed.write(Speedometer);
    RudderVal = 90;
    setRudderPulseWidth(RudderVal);
}

// Function to move sharp left
void moveSharpLeft() {
    Fspin = 1;
    Rspin = 0;
    Speed.write(Speedometer);
    for (RudderVal = 90; RudderVal >= 0; RudderVal -= 90) {
        setRudderPulseWidth(RudderVal);
    }
    for (RudderVal = 0; RudderVal <= 90; RudderVal += 90) {
        setRudderPulseWidth(RudderVal);
    }
}

// Function to move sharp right
void moveSharpRight() {
    Fspin = 1;
    Rspin = 0;
    Speed.write(Speedometer);
    for (RudderVal = 90; RudderVal <= 180; RudderVal += 90) {
        setRudderPulseWidth(RudderVal);
    }
    for (RudderVal = 180; RudderVal >= 90; RudderVal -= 90) {
        setRudderPulseWidth(RudderVal);
    }
}

// Function to move right
void moveRight() {
    Fspin = 1;
    Rspin = 0;
    Speed.write(Speedometer);
    for (RudderVal = 90; RudderVal <= 180; RudderVal += 15) {
        setRudderPulseWidth(RudderVal);
    }
    wait_ms(10);
    for (RudderVal = 180; RudderVal >= 90; RudderVal -= 90) {
        setRudderPulseWidth(RudderVal);
    }
}

// Function to move left
void moveLeft() {
    Fspin = 1;
    Rspin = 0;
    Speed.write(Speedometer);
    for (RudderVal = 90; RudderVal >= 0; RudderVal -= 15) {
        setRudderPulseWidth(RudderVal);
    }
    wait_ms(10);
    for (RudderVal = 0; RudderVal <= 90; RudderVal += 90) {
        setRudderPulseWidth(RudderVal);
    }
}

// Function to control canon
void controlCanon() {
    Pumper = 1;
    pumperS = 80 / 100.f;
    pumperSTRENGTH.write(pumperS);
    // Control canon movement
    int CYCLES = 0; // Add a counter for the number of iterations
    const int MAX_CYCLES = 3; // Define the maximum number of iterations
    while ( CYCLES < MAX_CYCLES) {
        for (int angle = 90; angle >= 0; angle -= 10) {
            CanonPulse(Canon, angle);
        }
        wait_ms(500);
        for (int angle = 0; angle <= 180; angle += 10) {
            CanonPulse(Canon, angle);
        }
        wait_ms(500);
        for (int angle = 180; angle >= 0; angle -= 10) {
            CanonPulse(Canon, angle);
        }
        wait_ms(500);
        for (int angle = 0; angle <= 90; angle += 10) {
            CanonPulse(Canon, angle);
        }
        wait_ms(500);

        CYCLES++;
    }
}


//the above are sub functions for the "VOID MOVEMENT"
void SPEED() {
    if (checkCommunicationError()) {
        bluetooth.printf("Communication error. Attempting to reconnect...\r\n");
        wait_ms(COMMUNICATION_ERROR_RETRY_DELAY_MS); // Wait for 1 second before retrying
        return;
    }  
            // Process valid commands (existing code remains the same)
            // ...
            if (State == '0') {
                 Speedometer = 0 / 100.0f;
                 Speed.write(Speedometer);
                }
            else if (State == '1') {
                 Speedometer = 10 / 100.0f;
                 Speed.write(Speedometer);
                }
            else if (State == '2') {
                 Speedometer = 20 / 100.0f;
                 Speed.write(Speedometer);
                }
            else if (State == '3') {
                 Speedometer = 30 / 100.0f;
                 Speed.write(Speedometer);
                }
            else if (State == '4') {
                 Speedometer = 40 / 100.0f;
                 Speed.write(Speedometer);
                }  
            else if (State == '5') {
                 Speedometer = 50 / 100.0f;
                 Speed.write(Speedometer);
                } 
            else if (State == '6') {
                 Speedometer = 60 / 100.0f;
                 Speed.write(Speedometer);
                } 
            else if (State == '7') {
                 Speedometer = 70 / 100.0f;
                 Speed.write(Speedometer);
                }
            else if (State == '8') {
                 Speedometer = 80 / 100.0f;
                 Speed.write(Speedometer);
                }
            else if (State == '9') {
                 Speedometer = 90 / 100.0f;
                 Speed.write(Speedometer);
                } 
           else {
                 bluetooth.printf("Invalid command received: %c\r\n", State);
                 return;
                }
                 wait_ms(20); // Wait for 20ms before checking again 
}

void MANUAL() {
    if (checkCommunicationError()) {
        bluetooth.printf("AUTONOMOUS MODE ERROR\r\n");
        bluetooth.printf("SWITCHING TO MANUAL MODE\r\n");
        return;
    }
             switch (State) {
                case 'F':
                    moveForward();
                    break;
                case 'B':
                    moveBackward();
                    break;
                case 'L':
                    moveSharpLeft();
                    break;
                case 'R':
                    moveSharpRight();
                    break;
                case 'M':
                    moveRight();
                    break;
                case 'N':
                    moveLeft();
                    break;
                case 'G':
                    controlCanon();
                    break;
                case 'V':
                    Speedometer = 99 / 100.0f;
                    Speed.write(Speedometer);
                    break;
                case 'J':
                    brakeOn();
                    break;
                default:
                    bluetooth.printf("Unknown state. Retry.%c\r\n", State);
                    
                    break;
             }
}

void readSensorValues(float& RsenseVAL, float& MsenseVAL, float& LsenseVAL) {
    RsenseVAL = Fright.read() * 1000;
    MsenseVAL = Fmiddle.read() * 1000;
    LsenseVAL = Fleft.read() * 1000;
}
// Define a function to handle servo motor movement errors
void handleServoError(int angle, int retries) {
    bluetooth.printf("Failed to move servo to angle %d after %d retries\r\n", angle, retries);
}

// Define a function to move the servo motor to a specific angle with retries
bool moveServoToAngle(PwmOut& Canon, int angle, int MAX_RETRIES) {
    int retryCount = 0;
    while (retryCount < MAX_RETRIES) {
        if (CanonPulse(Canon, angle)) {
            wait_ms(AUTONOMOUS_MODE_SERVO_WAIT_TIME);
            return true;
        } else {
            bluetooth.printf("Error moving servo to angle %d\r\n", angle);
            retryCount++;
            wait_ms(RETRY_DELAY);
        }
    }
    handleServoError(angle, MAX_RETRIES);
    return false;
}
void moveServoOverRange(PwmOut& Canon, int startAngle, int endAngle) {
    int step = (startAngle < endAngle) ? AUTONOMOUS_MODE_SERVO_SPEED : -AUTONOMOUS_MODE_SERVO_SPEED;
    for (int angle = startAngle; angle != endAngle + step; angle += step) {
        if (!moveServoToAngle(Canon, angle, MAX_RETRIES)) {
            return;
        }
    }
}

void Autonomous() {
    if (checkCommunicationError()) {
        bluetooth.printf("AUTONOMOUS MODE ERROR\r\n");
        bluetooth.printf("SWITCHING TO MANUAL MODE\r\n");
        return;
    }

    
    readSensorValues(RsenseVAL, MsenseVAL, LsenseVAL);
    bluetooth.printf("%d", RsenseVAL);
    bluetooth.printf("|");
    bluetooth.printf("%d", MsenseVAL);
    bluetooth.printf("|");
    bluetooth.printf("%d", LsenseVAL);
    bluetooth.printf("\n");
    wait_ms(1000);

    if (RsenseVAL < AUTONOMOUS_MODE_SENSOR_THRESHOLD) {
        Stop();
        Pumper = 1;
        pump = 0;
        pumperSTRENGTH.write(PUMPER_STRENGTH);

        moveServoOverRange(Canon, SERVO_MID_ANGLE, SERVO_MIN_ANGLE);
        moveServoOverRange(Canon, SERVO_MIN_ANGLE, SERVO_MID_ANGLE);

    } else if (MsenseVAL < AUTONOMOUS_MODE_SENSOR_THRESHOLD) {
        Stop();
        Pumper = 1;
        pump = 0;
        pumperSTRENGTH.write(PUMPER_STRENGTH);

        moveServoOverRange(Canon, SERVO_MID_ANGLE, 50);
        moveServoOverRange(Canon, 50, 130);
        moveServoOverRange(Canon, 130, SERVO_MID_ANGLE);

    } else if (LsenseVAL < AUTONOMOUS_MODE_SENSOR_THRESHOLD) {
        Stop();
        Pumper = 1;
        pump = 0;
        pumperSTRENGTH.write(PUMPER_STRENGTH);

        moveServoOverRange(Canon, SERVO_MID_ANGLE, SERVO_MAX_ANGLE);
        moveServoOverRange(Canon, SERVO_MAX_ANGLE, SERVO_MID_ANGLE);

    }else if (RsenseVAL < AUTONOMOUS_MODE_SENSOR_THRESHOLD && MsenseVAL < AUTONOMOUS_MODE_SENSOR_THRESHOLD) {
        // Both right and middle sensors detect something
        Stop();
        Pumper = 1;
        pump = 0;
        pumperSTRENGTH.write(PUMPER_STRENGTH);

        moveServoOverRange(Canon, SERVO_MID_ANGLE, 40);
        moveServoOverRange(Canon, 40, 120);
        moveServoOverRange(Canon, 120, SERVO_MID_ANGLE);

    }else if (MsenseVAL < AUTONOMOUS_MODE_SENSOR_THRESHOLD && LsenseVAL < AUTONOMOUS_MODE_SENSOR_THRESHOLD) {
        // Both middle and left sensors detect something
         Stop();
        Pumper = 1;
        pump = 0;
        pumperSTRENGTH.write(PUMPER_STRENGTH);

        moveServoOverRange(Canon, SERVO_MID_ANGLE, 170);
        moveServoOverRange(Canon, 170, 70);
        moveServoOverRange(Canon, 70, SERVO_MID_ANGLE);

    } else if (RsenseVAL >= AUTONOMOUS_MODE_SENSOR_THRESHOLD && RsenseVAL <= SENSOR_MEDIUM_THRESHOLD) {
        Pumper = 0;
        pump = 0;
        pumperSTRENGTH.write(PUMPER_OFF);
        turnRight();
        forward();
        wait_ms(50);

    } else if (MsenseVAL >= AUTONOMOUS_MODE_SENSOR_THRESHOLD && MsenseVAL <= SENSOR_MEDIUM_THRESHOLD) {
        Pumper = 0;
        pump = 0;
        pumperSTRENGTH.write(PUMPER_OFF);
        forward();
        wait_ms(50);

    } else if (LsenseVAL >= AUTONOMOUS_MODE_SENSOR_THRESHOLD && LsenseVAL <= SENSOR_MEDIUM_THRESHOLD) {
        Pumper = 0;
        pump = 0;
        pumperSTRENGTH.write(PUMPER_OFF);
        turnLeft();
        forward();
        wait_ms(50);

    } else {
        Pumper = 0;
        pump = 0;
        pumperSTRENGTH.write(PUMPER_OFF);
        Stop();
    }
}
void Control() {
    if (timer.read_ms() - last_update_time >= UPDATE_INTERVAL_MS) {
        last_update_time = timer.read_ms(); // Update the last update time

     if (checkCommunicationError()) 
     {
        bluetooth.printf("Communication error. Attempting to reconnect...\r\n");
        wait_ms(COMMUNICATION_ERROR_RETRY_DELAY_MS); // Wait for 1 second before retrying
        return;
     }
    
     if (bluetooth.readable() > 0) 
     {
        State = bluetooth.getc();
        bluetooth.printf("Received: %c\r\n", State);
        if (State >= 'B' && State <= 'z') {
             MANUAL();
        }else if (State >= '0' && State <= '9') {
             SPEED();
        }else if (State == 'A') {
             Autonomous();
        }else {
            switch (state) 
            {
              case 'A':
                Autonomous();
                break;
              default:
                bluetooth.printf("Unknown state. Switching to MANUAL mode.%c\r\n", state);
                break;
            }
             
        }
     }
    
    }

    wait_ms(5);
}
void brakeOn() {
  buttonState = in;
  if (buttonState != lastButtonState) {
    if (buttonState == 'J') {
      if (lastButtonState != buttonState) {
         Fspin = 0;
         Rspin = 0;
        wait_ms(brakeTime);
        moveSTOP();
      }
    }
    lastButtonState = buttonState;
  }
}
void moveSTOP() {
    Fspin = 0;
    Rspin = 0;
    RudderVal = 90;
    setRudderPulseWidth(90);
    CanonPulse(Canon, 90);
    Speedometer = 0 / 100.0f;
    Speed.write(Speedometer);
}
void brakeOff() {
  Fspin = 1;
  Rspin = 1;
}
int main() {
int retryCount = 0;
const int maxRetries = 5; // or any other suitable value
bluetooth.baud(9600);
    // Set PWM period
    Speed.period_ms(20);
    Canon.period_ms(20);
    Rudder.period_ms(20);

    bluetooth.printf("Boat Control System Starting...\r\n");

    while (true) {
        if (checkCommunicationError()) {
            bluetooth.printf("Communication error detected. Retrying...\r\n");
            retryCount++;

            if (retryCount >= maxRetries) {
                bluetooth.printf("Failed to establish connection after %d attempts.check hardware.\r\n", maxRetries);
                break;
            }
            wait_ms(RETRY_DELAY);
        } else {
            retryCount = 0;
            Control();
        }
    }
}