#include <Wire.h>
#include <Servo.h>
#include <SoftwareSerial.h>
#include <MPU6050.h>

MPU6050 mpu;

Servo gps_servo;       // Servo controlled by GPS speed, on pin 9
Servo roll_servo_1;    // Servo controlled by roll angle, on pin 10
Servo roll_servo_2;    // Servo controlled by roll angle, on pin 11

int potPin = A0;  // Analog pin for the speed potentiometer
int potPinDeceleration = A1;  // Analog pin for the deceleration potentiometer

float decelerationThreshold = 10.0;  // Threshold for deceleration (adjust as needed)
float currentDeceleration = 0.0;  // Variable to store the current deceleration

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();
  
  gps_servo.attach(9);
  roll_servo_1.attach(10);
  roll_servo_2.attach(11);

  // Randomly set roll servo angles to four positions
  randomSeed(analogRead(0)); // Seed the random number generator
  for (int i = 0; i < 4; ++i) {
    roll_servo_1.write(random(0, 91));  // Set angle between 0 and 90 degrees
    roll_servo_2.write(random(0, 91));  // Set angle between 0 and 90 degrees
    gps_servo.write(random(0, 0));  // Set angle between 0 and 0 degrees
    delay(3000);  // Delay between each random position
  }

  // Set all servos to 0 degrees
  gps_servo.write(0);
  roll_servo_1.write(0);
  roll_servo_2.write(0);
  
  // Wait for a moment to allow the servos to reach their initial positions
  delay(1000);
}

void loop() {
  // Main code here...

  // Simulate GPS data
  float simulatedSpeed = 5.0;  // Manually set speed for demonstration
  float simulatedDeceleration = 2.0;  // Manually set deceleration for demonstration

  // Read the potentiometer value and map it to a speed range
  int potValueSpeed = analogRead(potPin);
  simulatedSpeed = map(potValueSpeed, 0, 1023, 0, 35);  // Map pot value to a speed between 0 and 35 m/s

  // Read the deceleration potentiometer value and map it
  int potValueDeceleration = analogRead(potPinDeceleration);
  simulatedDeceleration = map(potValueDeceleration, 0, 1023, 0, 20.0);  // Map pot value to a deceleration between 0 and 20.0 m/s^2

  int angle_gps, angle_roll_1, angle_roll_2;

  // Read accelerometer data from MPU6050
  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);

  float roll = atan2(ay, az) * 180.0 / PI;

  // Check for deceleration and adjust the angle if necessary
  if (currentDeceleration > decelerationThreshold) {
    angle_gps = 90;
    angle_roll_1 = 90;
    angle_roll_2 = 90;
  } else {
    // Control gps_servo based on GPS speed and roll angle
    if (simulatedSpeed < 11.1 || (roll > 20 || roll < -20)) {
      angle_gps = 0;
    } else if (simulatedSpeed >= 11.1 && simulatedSpeed < 22.2) {
      angle_gps = 25;
    } else {
      angle_gps = 45;
    }

    // Control roll_servo_1 and roll_servo_2 based on speed and roll angle
    if (simulatedSpeed > 11.1 && roll > 20) {
      angle_roll_1 = 45;
      angle_roll_2 = 25;
    } else if (simulatedSpeed > 11.1 && roll < -20) {
      angle_roll_1 = 25;
      angle_roll_2 = 45;
    } else {
      angle_roll_1 = 0;
      angle_roll_2 = 0;
    }
  }

  // Set servo angles
  gps_servo.write(angle_gps);
  roll_servo_1.write(angle_roll_1);
  roll_servo_2.write(angle_roll_2);

  // Update the current deceleration
  currentDeceleration = simulatedDeceleration;

  Serial.print("(Speed): ");
  Serial.print(potValueSpeed);
  Serial.print("\t Speed (m/s): ");
  Serial.print(simulatedSpeed);
  Serial.print("\t (Decel): ");
  Serial.print(potValueDeceleration);
  Serial.print("\t Decel (m/s^2): ");
  Serial.print(simulatedDeceleration);
  Serial.print("\t Roll : ");
  Serial.println(roll);

  delay(200);  // Adjusted delay to 200 milliseconds
}