#include <Servo.h>
#include <EEPROM.h>

// Servo objects
Servo baseServo, elbowServo, wristServo, gripServo;

// Joystick pins
const int joystick1X = A0; // Base (X-axis)
const int joystick1Y = A1; // Elbow (Y-axis)
const int joystick2X = A2; // Grip (X-axis)
const int joystick2Y = A3; // Wrist (Y-axis)
const int select1Pin = 7;  // Joystick1 SELECT button
const int select2Pin = 8;  // Joystick2 SELECT button
const int playButtonPin = 10; // Play button

// Servo control pins
const int basePin = 3, elbowPin = 5, wristPin = 6, gripPin = 9;

// Servo angles
int baseAngle = 90, elbowAngle = 90, wristAngle = 90, gripAngle = 90;

// Joystick thresholds
const int deadZone = 10; // Ignore small joystick movements

// Preset data structure
struct Preset {
  int base;
  int elbow;
  int wrist;
  int grip;
};

// Maximum number of presets
const int maxPresets = 5;
Preset presets[maxPresets];
int currentPreset = 0; // Active preset index
bool playMode = false; // Play mode flag

void setup() {
  // Attach servos to pins
  baseServo.attach(basePin);
  elbowServo.attach(elbowPin);
  wristServo.attach(wristPin);
  gripServo.attach(gripPin);

  // Set initial servo positions
  baseServo.write(baseAngle);
  elbowServo.write(elbowAngle);
  wristServo.write(wristAngle);
  gripServo.write(gripAngle);

  // Initialize buttons
  pinMode(select1Pin, INPUT_PULLUP);
  pinMode(select2Pin, INPUT_PULLUP);
  pinMode(playButtonPin, INPUT_PULLUP);

  // Initialize serial monitor
  Serial.begin(9600);

  // Load presets from EEPROM at startup
  loadPresetsFromEEPROM();
}

void loop() {
  // Check for play mode
  if (digitalRead(playButtonPin) == LOW) {
    playMode = true;
    playPresets();
  }

  // If not in play mode, handle joystick control
  if (!playMode) {
    handleJoystickControl();
  }
}

// Smoothly move a servo
void moveServoSmoothly(Servo &servo, int &currentAngle, int targetAngle, int stepDelay) {
  if (currentAngle < targetAngle) {
    for (int pos = currentAngle; pos <= targetAngle; pos++) {
      servo.write(pos);
      delay(stepDelay);
    }
  } else if (currentAngle > targetAngle) {
    for (int pos = currentAngle; pos >= targetAngle; pos--) {
      servo.write(pos);
      delay(stepDelay);
    }
  }
  currentAngle = targetAngle;
}

// Handle joystick control for manual servo adjustments
void handleJoystickControl() {
  int joystick1XValue = analogRead(joystick1X) - 512; // Center at 0
  int joystick1YValue = analogRead(joystick1Y) - 512; // Center at 0
  int joystick2XValue = analogRead(joystick2X) - 512; // Center at 0
  int joystick2YValue = analogRead(joystick2Y) - 512; // Center at 0

  int stepDelay = 5; // Delay for smooth movement

  // Adjust base angle
  if (abs(joystick1XValue) > deadZone) {
    int targetBaseAngle = constrain(baseAngle + (joystick1XValue / 128), 0, 180);
    moveServoSmoothly(baseServo, baseAngle, targetBaseAngle, stepDelay);
  }

  // Adjust elbow angle
  if (abs(joystick1YValue) > deadZone) {
    int targetElbowAngle = constrain(elbowAngle + (joystick1YValue / 128), 0, 180);
    moveServoSmoothly(elbowServo, elbowAngle, targetElbowAngle, stepDelay);
  }

  // Adjust wrist angle
  if (abs(joystick2YValue) > deadZone) {
    int targetWristAngle = constrain(wristAngle + (joystick2YValue / 128), 0, 180);
    moveServoSmoothly(wristServo, wristAngle, targetWristAngle, stepDelay);
  }

  // Adjust grip angle
  if (abs(joystick2XValue) > deadZone) {
    int targetGripAngle = constrain(gripAngle + (joystick2XValue / 128), 0, 180);
    moveServoSmoothly(gripServo, gripAngle, targetGripAngle, stepDelay);
  }

  // Save current angles to a preset with SELECT1
  if (digitalRead(select1Pin) == LOW) {
    presets[currentPreset] = { baseAngle, elbowAngle, wristAngle, gripAngle };
    Serial.print("Preset ");
    Serial.print(currentPreset);
    Serial.println(" saved.");
    delay(300); // Debounce
  }

  // Save all presets to EEPROM with SELECT2
  if (digitalRead(select2Pin) == LOW) {
    savePresetsToEEPROM();
    Serial.println("Presets saved to EEPROM.");
    delay(300); // Debounce
  }
}

// Play presets sequentially
void playPresets() {
  for (int i = 0; i < maxPresets; i++) {
    Serial.print("Playing preset ");
    Serial.println(i);

    moveServoSmoothly(baseServo, baseAngle, presets[i].base, 10);
    moveServoSmoothly(elbowServo, elbowAngle, presets[i].elbow, 10);
    moveServoSmoothly(wristServo, wristAngle, presets[i].wrist, 10);
    moveServoSmoothly(gripServo, gripAngle, presets[i].grip, 10);

    delay(1000); // Pause between presets

    // Exit play mode if the Play button is pressed again
    if (digitalRead(playButtonPin) == LOW) {
      playMode = false;
      Serial.println("Play mode stopped.");
      return;
    }
  }
}

// Save all presets to EEPROM
void savePresetsToEEPROM() {
  for (int i = 0; i < maxPresets; i++) {
    int addr = i * sizeof(Preset);
    EEPROM.put(addr, presets[i]);
  }
}

// Load presets from EEPROM
void loadPresetsFromEEPROM() {
  for (int i = 0; i < maxPresets; i++) {
    int addr = i * sizeof(Preset);
    EEPROM.get(addr, presets[i]);
  }
}