#include <Servo.h>

Servo myServo1; // First servo
Servo myServo2; // Second servo

const int servoPin1 = 9;   // First servo connected to D9
const int servoPin2 = 10;  // Second servo connected to D10
const int potPin = A0;     // Potentiometer or accelerometer input connected to A0

float speed = 0;           // Simulated speed in m/s
unsigned long prevTime = 0;
unsigned long maxSpeedTime = 0;
const float speedThreshold = 1.788;  // 4 mph in m/s
const float maxSpeed = 31.2928;      // 70 mph in m/s
float accelSim = 0;        // Simulated acceleration (m/s²)
bool isDecelerating = false;

void setup() {
  Serial.begin(9600);
  myServo1.attach(servoPin1);
  myServo2.attach(servoPin2);

  myServo1.write(90);  // Start first servo at 0 (was 45) degrees
  myServo2.write(90); // Start second servo at 0 (was 135) degrees (opposite position)
  prevTime = millis();
}

void loop() {
  unsigned long currentTime = millis();
  float deltaTime = (currentTime - prevTime) / 1000.0; // Convert ms to seconds
  prevTime = currentTime;

  // Read potentiometer value and simulate acceleration
  int potValue = analogRead(potPin); // Range: 0 to 1023

  // Check if system should decelerate
  if (isDecelerating) {
    speed -= 5 * deltaTime; // Gradually decrease speed
    if (speed <= 0) {
      speed = 0;
      isDecelerating = false; // Stop decelerating
    }
    accelSim = 0; // Ensure no further acceleration during deceleration
  } else {
    // Simulate acceleration based on potentiometer
    accelSim = map(potValue, 0, 1023, 0, 5); // Map to 0 to 5 m/s²

    // Update speed if below max speed
    if (speed < maxSpeed) {
      speed += accelSim * deltaTime; // Speed = v0 + a * t
      if (speed > maxSpeed) {
        speed = maxSpeed; // Cap speed at max
        maxSpeedTime = millis(); // Start timer at max speed
      }
    } else {
      // If at max speed, check how long it's been there
      if (millis() - maxSpeedTime > 5000) { // 5 seconds at max speed
        isDecelerating = true; // Begin deceleration
      }
    }
  }

  // Prevent negative speed
  if (speed < 0) speed = 0;

  // Print simulated values for debugging
  Serial.print("Acceleration: ");
  Serial.print(accelSim);
  Serial.print(" m/s², Speed: ");
  Serial.print(speed);
  Serial.println(" m/s");

  // Check if speed exceeds threshold
  if (speed > speedThreshold) {
    // Move both servos in opposite directions
    for (int pos = 45; pos <= 135; pos += 2) {
      myServo1.write(pos);        // Move first servo up
      myServo2.write(180 - pos); // Move second servo down
      delay(4); // Increased delay to 4 ms
    }
    for (int pos = 135; pos >= 45; pos -= 2) {
      myServo1.write(pos);        // Move first servo down
      myServo2.write(180 - pos); // Move second servo up
      delay(4); // Increased delay to 4 ms
    }
  }
}