#include <Servo.h>

// Define servo objects for the robot arm
Servo baseServo;
Servo shoulderServo;
Servo elbowServo;
Servo gripperServo;

// Joystick pins
const int joyX = A0; // X-axis for base and gripper
const int joyY = A1; // Y-axis for shoulder
const int joyButton = 22; // Button for elbow movement (SW pin on digital pin 22)

// Current position variables (set from current position when powered on)
int basePos;      // Base (0 - 180 degrees)
int shoulderPos;  // Shoulder (0 - 180 degrees)
int elbowPos;     // Elbow (0 - 180 degrees)
int gripperPos;   // Gripper or wrist (0 - 180 degrees)

// Movement speed control
const int moveDelay = 10; // Delay for smooth movement (ms)

// Limits for each servo
const int baseMin = 0, baseMax = 180;
const int shoulderMin = 0, shoulderMax = 180;
const int elbowMin = 0, elbowMax = 180;
const int gripperMin = 0, gripperMax = 180;

// Threshold to avoid small joystick movements from causing jitter
const int joystickThreshold = 10;

// Debounce variables for the joystick button
unsigned long lastDebounceTime = 0;
unsigned long debounceDelay = 50;
int lastButtonState = HIGH;
int buttonState;

// Function to smooth out joystick readings
int smoothInput(int input) {
  int smoothedValue = map(input, 0, 1023, -100, 100);  // Map to a wider range
  if (abs(smoothedValue) < joystickThreshold) {
    return 0; // Ignore small movements
  } else {
    return smoothedValue / 10; // Smoother adjustment rate
  }
}

void setup() {
  // Attach servos to corresponding PWM pins
  baseServo.attach(3);
  shoulderServo.attach(5);
  elbowServo.attach(6);
  gripperServo.attach(9);

  // Initialize joystick button pin
  pinMode(joyButton, INPUT_PULLUP);

  // Initialize servos to their current positions (home position)
  // By not assigning a predefined position, the servos will remain where they are when powered on
  basePos = baseServo.read();       // Read current position of base servo
  shoulderPos = shoulderServo.read(); // Read current position of shoulder servo
  elbowPos = elbowServo.read();      // Read current position of elbow servo
  gripperPos = gripperServo.read();  // Read current position of gripper servo

  Serial.begin(9600); // For debugging
}

void loop() {
  // Read joystick axes and smooth input
  int joyXVal = smoothInput(analogRead(joyX));
  int joyYVal = smoothInput(analogRead(joyY));
  
  // Read and debounce the joystick button
  int reading = digitalRead(joyButton);
  if (reading != lastButtonState) {
    lastDebounceTime = millis(); // Reset the debounce timer
  }
  if ((millis() - lastDebounceTime) > debounceDelay) {
    if (reading != buttonState) {
      buttonState = reading;
      if (buttonState == LOW) {
        // If button pressed, lean the elbow forward
        elbowPos = constrain(elbowPos + 1, elbowMin, elbowMax);
        elbowServo.write(elbowPos);
      }
    }
  }
  lastButtonState = reading;

  // Control Base Servo (X-axis controls base rotation)
  if (joyXVal != 0) {
    basePos = constrain(basePos + joyXVal, baseMin, baseMax);
    baseServo.write(basePos);
    delay(moveDelay); // Smooth movement delay
  }

  // Control Shoulder Servo (Y-axis controls shoulder up/down)
  if (joyYVal != 0) {
    shoulderPos = constrain(shoulderPos + joyYVal, shoulderMin, shoulderMax);
    shoulderServo.write(shoulderPos);
    delay(moveDelay); // Smooth movement delay
  }

  // Control Gripper using joystick X-axis when button is not pressed
  if (buttonState == HIGH) {
    int gripperDirection = smoothInput(analogRead(joyX)); // Re-read X-axis for gripper control
    if (gripperDirection != 0) {
      gripperPos = constrain(gripperPos + gripperDirection, gripperMin, gripperMax);
      gripperServo.write(gripperPos);
      delay(moveDelay); // Smooth movement delay
    }
  }

  // Short delay to allow time for servo movement
  delay(50);
}