#include <Servo.h>

// Define Arduino pins
const int statePin = 2;         // Green/Red "State" wire from central locking
const int servoPin = 9;         // Arduino PWM pin connected to the servo's signal wire

// Define servo angles for locked and unlocked positions
// !!! YOU WILL NEED TO DETERMINE THESE VALUES BY EXPERIMENTATION !!!
const int unlockedAngle = 0;    // Example: Angle for unlocked state
const int lockedAngle = 90;     // Example: Angle for locked state

Servo tailgateServo; // Create a servo object

// Define lock states
enum LockState {
  UNKNOWN,
  LOCKED,
  UNLOCKED
};
LockState currentCarLockState = UNKNOWN;
LockState previousCarLockState = UNKNOWN;

void setup() {
  Serial.begin(9600); // For debugging

  pinMode(statePin, INPUT_PULLUP); // State wire: LOW when unlocked, HIGH when locked

  tailgateServo.attach(servoPin); // Attach the servo on servoPin to the servo object
  Serial.println("Servo attached.");

  delay(100); // Small delay for stability

  // Read initial state and move servo
  readCentralLockingState();
  previousCarLockState = currentCarLockState; // Set initial previous state

  if (currentCarLockState == LOCKED) {
    tailgateServo.write(lockedAngle);
    Serial.print("Initial state: LOCKED. Servo to: "); Serial.println(lockedAngle);
  } else if (currentCarLockState == UNLOCKED) {
    tailgateServo.write(unlockedAngle);
    Serial.print("Initial state: UNLOCKED. Servo to: "); Serial.println(unlockedAngle);
  } else {
    // Default to one state if unknown, e.g., unlocked
    tailgateServo.write(unlockedAngle);
    Serial.print("Initial state: UNKNOWN. Defaulting servo to: "); Serial.println(unlockedAngle);
  }
  delay(500); // Give servo time to move to initial position
}

void loop() {
  readCentralLockingState();

  if (currentCarLockState != previousCarLockState) {
    Serial.print("Central locking state changed from ");
    printLockState(previousCarLockState);
    Serial.print(" to ");
    printLockState(currentCarLockState);

    if (currentCarLockState == LOCKED) {
      Serial.print("Moving servo to LOCKED position: "); Serial.println(lockedAngle);
      tailgateServo.write(lockedAngle);
    } else if (currentCarLockState == UNLOCKED) {
      Serial.print("Moving servo to UNLOCKED position: "); Serial.println(unlockedAngle);
      tailgateServo.write(unlockedAngle);
    }
    previousCarLockState = currentCarLockState;
    delay(500); // Give servo time to move (adjust as needed)
  }
}

void readCentralLockingState() {
  // Green/Red "state" wire is ground when unlocked (reads LOW),
  // and cut (reads HIGH due to INPUT_PULLUP) when locked.
  if (digitalRead(statePin) == LOW) {
    currentCarLockState = UNLOCKED;
  } else {
    currentCarLockState = LOCKED;
  }
}

void printLockState(LockState state) {
  if (state == LOCKED) {
    Serial.print("LOCKED");
  } else if (state == UNLOCKED) {
    Serial.print("UNLOCKED");
  } else {
    Serial.print("UNKNOWN");
  }
  // No Serial.println() here so it can be used inline in other prints
}