#include <Servo.h>

// Servo objects
Servo servo1Left;  // Servo for left plate (no motion), left motor
Servo servo1Right; // Servo for left plate (no motion), right motor
Servo servo2Left;  // Servo for right plate (motion), left motor
Servo servo2Right; // Servo for right plate (motion), right motor

// Define servo pins
#define servo1LeftPin 5  // No motion plate, left motor
#define servo1RightPin 6 // No motion plate, right motor
#define servo2LeftPin 10 // Motion plate, left motor
#define servo2RightPin 9 // Motion plate, right motor

// Pin definitions for sensors
#define trig_in 7        // Trigger pin for ultrasonic sensor
#define echo_in 8        // Echo pin for ultrasonic sensor
#define pirPin 2         // Digital pin for PIR sensor

// Variable to store the current position of servos
int servo1LeftPos = 90;
int servo1RightPos = 90;
int servo2LeftPos = 90;
int servo2RightPos = 90;

// Base speed and calibration factors
const int baseSpeed = 20;  // Base movement speed
// Calibration factors (adjust these values based on testing)
const float noMotionPlateLeftFactor = 1.0;   // No motion plate, left servo
const float noMotionPlateRightFactor = 1.0;  // No motion plate, right servo
const float motionPlateLeftFactor = 1.0;     // Motion plate, left servo
const float motionPlateRightFactor = 1.0;    // Motion plate, right servo

void setup() {
  Serial.begin(9600);   
  
  // Attach the servos to their respective pins
  servo1Left.attach(servo1LeftPin);
  servo1Right.attach(servo1RightPin);
  servo2Left.attach(servo2LeftPin);
  servo2Right.attach(servo2RightPin);

  // Setup ultrasonic sensor pins
  pinMode(trig_in, OUTPUT);
  pinMode(echo_in, INPUT);

  // Setup PIR sensor pin
  pinMode(pirPin, INPUT);

  // Reset all servos to neutral position (closed state)
  resetServos();

  Serial.println("System initialized and ready.");
}

void loop() {
  float distance = measureDistance();
  
  if (distance < 30) {  // Object detected within 30 cm
    Serial.println("Object detected! Checking for motion...");
    
    unsigned long startTime = millis();
    bool motionDetected = false;
    
    while (millis() - startTime < 10000) {  // Check for motion for 10 seconds
      if (checkMotion()) {
        motionDetected = true;
        break;
      }
      delay(100);  // Small delay to prevent too frequent checks
    }
    
    if (motionDetected) {
      Serial.println("Motion detected! Opening motion panel.");
      openPanel(true);  // Open motion panel
    } else {
      Serial.println("No motion detected. Opening no-motion panel.");
      openPanel(false);  // Open no-motion panel
    }
    
    delay(5000);  // Keep panel open for 5 seconds
    resetServos();  // Close the panel
    
    delay(1000);  // Small delay before next detection cycle
  }
}

void movePlatePair(Servo &servoLeft, Servo &servoRight, 
                   int &leftPos, int &rightPos, 
                   int targetAngle, int speed,
                   float leftFactor, float rightFactor) {
  // Calculate actual positions for normal and reversed servos
  int leftTarget = targetAngle;
  int rightTarget = 180 - targetAngle;  // Reversed for right servo
  
  int leftCurrent = leftPos;
  int rightCurrent = 180 - rightPos;  // Convert stored position to actual position
  
  // Determine the number of steps needed
  int leftSteps = abs(leftTarget - leftCurrent);
  int rightSteps = abs(rightTarget - rightCurrent);
  int maxSteps = max(leftSteps, rightSteps);
  
  // Calculate movement increments for each servo to finish at the same time
  float leftIncrement = leftSteps > 0 ? (leftTarget - leftCurrent) / (float)maxSteps : 0;
  float rightIncrement = rightSteps > 0 ? (rightTarget - rightCurrent) / (float)maxSteps : 0;
  
  // Move both servos incrementally
  float currentLeftPos = leftCurrent;
  float currentRightPos = rightCurrent;
  
  for (int step = 0; step < maxSteps; step++) {
    currentLeftPos += leftIncrement;
    currentRightPos += rightIncrement;
    
    servoLeft.write(round(currentLeftPos));
    servoRight.write(round(currentRightPos));
    
    // Apply calibration factors to delay
    int adjustedDelay = round(speed * max(leftFactor, rightFactor));
    delay(adjustedDelay);
  }
  
  // Ensure final position is exact
  servoLeft.write(leftTarget);
  servoRight.write(rightTarget);
  
  // Small delay at the end to ensure both servos complete their movement
  delay(speed * 2);
  
  // Update stored positions
  leftPos = targetAngle;
  rightPos = targetAngle;
}

void resetServos() {
  Serial.println("Resetting servos to 90 degrees if needed");
  if (servo1LeftPos != 90 || servo1RightPos != 90) {
    movePlatePair(servo1Left, servo1Right, servo1LeftPos, servo1RightPos, 90, baseSpeed,
                  rightPlateLeftFactor, rightPlateRightFactor);
    delay(500); // Add delay between plate movements
  }
  if (servo2LeftPos != 90 || servo2RightPos != 90) {
    movePlatePair(servo2Left, servo2Right, servo2LeftPos, servo2RightPos, 90, baseSpeed,
                  leftPlateLeftFactor, leftPlateRightFactor);
  }
}

void openPanel(bool isMotion) {
  if (isMotion) {
    Serial.println("Opening motion panel to 180 degrees");
    movePlatePair(servo2Left, servo2Right, servo2LeftPos, servo2RightPos, 180, baseSpeed,
                  motionPlateLeftFactor, motionPlateRightFactor);
  } else {
    Serial.println("Opening no-motion panel to 180 degrees");
    movePlatePair(servo1Left, servo1Right, servo1LeftPos, servo1RightPos, 180, baseSpeed,
                  noMotionPlateLeftFactor, noMotionPlateRightFactor);
  }
}

float measureDistance() {
  digitalWrite(trig_in, LOW);
  delayMicroseconds(2);
  digitalWrite(trig_in, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig_in, LOW);
  
  long duration = pulseIn(echo_in, HIGH);
  float distance = duration * 0.034 / 2;  // Calculate distance in cm
  
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");
  
  return distance;
}

bool checkMotion() {
  int motionState = digitalRead(pirPin);
  
  // Debugging
  Serial.print("PIR sensor state: ");
  Serial.println(motionState);

  return motionState == HIGH;  // HIGH means motion detected
}