#include <IRremote.h>
#include <Servo.h>

#define PIN_RECEIVER 2  // IR receiver pin
#define JOYSTICK_BUTTON 3  // Joystick button pin
#define LED_PIN 4  // LED Indicator

IRrecv receiver(PIN_RECEIVER);

// Joystick and Servo Pins
const int joystickX = A0;
const int joystickY = A1;
const int servoPin1 = 9;
const int servoPin2 = 10;

Servo servo1;
Servo servo2;

int servo1Pos = 90;
int servo2Pos = 90;
bool isLocked = false;
bool ledState = false;
unsigned long lastMoveTime = 0;
int moveInterval = 200; // Default movement speed

void setup() {
  pinMode(JOYSTICK_BUTTON, INPUT_PULLUP);
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, LOW);  // LED starts OFF

  servo1.attach(servoPin1);
  servo2.attach(servoPin2);
  servo1.write(servo1Pos);
  servo2.write(servo2Pos);

  Serial.begin(9600);
  Serial.println("Enter 'reset' to set servos to 90 degrees.");
  Serial.println("Enter 'speed <value>' to set servo speed (1-100).");
  Serial.println("Enter 'angle lock' to lock servos to specific angles.");
  Serial.println("Enter 'unlock' to remove angle locks.");

  receiver.enableIRIn();
}

void loop() {
  if (Serial.available() > 0) {
    processSerialCommand();
  }

  handleJoystick();
  handleIR();
  checkJoystickButton();
}

void handleJoystick() {
  int xPosition = analogRead(joystickX);
  int yPosition = analogRead(joystickY);

  if (millis() - lastMoveTime > moveInterval) {
    lastMoveTime = millis();
    if (!isLocked) {
      if (abs(xPosition - 512) > 50) {
        int targetServo1Pos = map(xPosition, 0, 1023, 0, 180);
        updateServoPosition(servo1, servo1Pos, targetServo1Pos);
      }
      if (abs(yPosition - 512) > 50) {
        int targetServo2Pos = map(yPosition, 0, 1023, 0, 180);
        updateServoPosition(servo2, servo2Pos, targetServo2Pos);
      }
    }
  }
}

void handleIR() {
  if (receiver.decode()) {
    translateIR();
    receiver.resume();
  }
}

void checkJoystickButton() {
  if (digitalRead(JOYSTICK_BUTTON) == LOW) {
    toggleLED();
    delay(300); // Debounce delay
  }
}

void updateServoPosition(Servo &servo, int &currentPos, int targetPos) {
  if (targetPos > currentPos) {
    currentPos += 5;
  } else if (targetPos < currentPos) {
    currentPos -= 5;
  }
  servo.write(currentPos);
}

void processSerialCommand() {
  String command = Serial.readStringUntil('\n');
  command.trim();

  if (command == "reset") {
    resetServos();
  } else if (command.startsWith("speed ")) {
    int newSpeed = command.substring(6).toInt();
    if (newSpeed >= 1 && newSpeed <= 100) {
      moveInterval = map(newSpeed, 1, 100, 500, 10); // Adjust speed dynamically
      Serial.print("Servo speed set to: ");
      Serial.print(newSpeed);
      Serial.println("%");
    } else {
      Serial.println("Invalid speed. Enter a value between 1 and 100.");
    }
  } else if (command == "angle lock") {
    isLocked = true;
    Serial.println("Servos locked.");
  } else if (command == "unlock") {
    isLocked = false;
    Serial.println("Servos unlocked.");
  } else {
    Serial.println("Unknown command.");
  }
}

void translateIR() {
  switch (receiver.decodedIRData.command) {
    case 162:  // POWER button toggles LED
      toggleLED();
      break;
    case 2:  
      if (!isLocked && servo2Pos > 0) servo2Pos -= 5;
      servo2.write(servo2Pos);
      Serial.println("Moving UP");
      break;
    case 224:  
      if (!isLocked && servo1Pos > 0) servo1Pos -= 5;
      servo1.write(servo1Pos);
      Serial.println("Moving LEFT");
      break;
    case 176:  // Number 176 resets servos to 90°
      resetServos();
      break;
    case 168:  // Lock button now toggles lock/unlock
      isLocked = !isLocked;
      Serial.println(isLocked ? "Servos locked." : "Servos unlocked.");
      break;
    case 144:  
      if (!isLocked && servo1Pos < 180) servo1Pos += 5;
      servo1.write(servo1Pos);
      Serial.println("Moving RIGHT");
      break;
    case 152:  
      if (!isLocked && servo2Pos < 180) servo2Pos += 5;
      servo2.write(servo2Pos);
      Serial.println("Moving DOWN");
      break;
    default:
      Serial.print("Unknown IR command: ");
      Serial.println(receiver.decodedIRData.command);
  }
}

void resetServos() {
  servo1Pos = 90;
  servo2Pos = 90;
  servo1.write(90);
  servo2.write(90);
  Serial.println("Servos reset to 90 degrees.");
}

void toggleLED() {
  ledState = !ledState;
  digitalWrite(LED_PIN, ledState ? HIGH : LOW);
  Serial.println(ledState ? "LED ON" : "LED OFF");
}