#include <Servo.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// OLED Display settings
#define SCREEN_WIDTH 128    // OLED display width, in pixels
#define SCREEN_HEIGHT 64    // OLED display height, in pixels
#define OLED_RESET    -1    // Reset pin (not used for most modules)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Servo declarations
Servo servo1, servo2, servo3, servo4, servo5;

// Current position variables
int currentAngle1 = 90, currentAngle2 = 90, currentAngle3 = 90, currentAngle4 = 90, currentAngle5 = 90;
int targetAngle1 = 90, targetAngle2 = 90, targetAngle3 = 90, targetAngle4 = 90, targetAngle5 = 90;

// Timer variables
unsigned long previousMillis_1 = 0; // Store current timer for timer1
unsigned long previousMillis_2 = 0; // Store current timer for timer2
unsigned long previousMillis_3 = 0; // Store current timer for timer3
unsigned long previousMillis_4 = 0; // Store current timer for timer4
unsigned long previousMillis_5 = 0; // Store current timer for timer5

// Interval settings (milliseconds)
int interval_1 = 10; // Default interval for servo1
int interval_2 = 10; // Default interval for servo2
int interval_3 = 10; // Default interval for servo3
int interval_4 = 10; // Default interval for servo4
int interval_5 = 10; // Default interval for servo5

// Joystick 1 pins
const int joystick1VertPin = A0;  // Vertical axis for Joystick 1
const int joystick1HorzPin = A1;  // Horizontal axis for Joystick 1
const int joystick1ButtonPin = 2; // Button for Joystick 1

// Joystick 2 pins
const int joystick2VertPin = A2;  // Vertical axis for Joystick 2
const int joystick2HorzPin = A3;  // Horizontal axis for Joystick 2
const int joystick2ButtonPin = 3; // Button for Joystick 2

// Control mode flags
bool serialControlActive = false; // Flag to determine if we are using Serial control
unsigned long lastSerialInputTime = 0; // Time of the last Serial input
const unsigned long serialTimeout = 10000; // Timeout for switching back to Joystick control (in ms)

void setup() {
  Wire.begin();
  // Attach servos to pins
  servo1.attach(5); // Servo numbers :5-> 11, 4-> 10, 3-> 9, 2-> 6, 1-> 5
  servo2.attach(6);
  servo3.attach(9);
  servo4.attach(10);
  servo5.attach(11);

  // Initialize servos to starting position
  servo1.write(currentAngle1);
  servo2.write(currentAngle2);
  servo3.write(currentAngle3);
  servo4.write(currentAngle4);
  servo5.write(currentAngle5);

  // Set up joystick buttons as input with pull-up resistors
  pinMode(joystick1ButtonPin, INPUT_PULLUP);
  pinMode(joystick2ButtonPin, INPUT_PULLUP);

  // if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Try 0x3C or 0x3D
  //   Serial.println(F("SSD1306 allocation failed"));
  //   for (;;); // Stop execution if initialization fails
  // }
  // Serial.println("OLED initialized successfully!");

  // // Clear the display
  // display.clearDisplay();
  // display.setTextSize(1);
  // display.setTextColor(SSD1306_WHITE);
  // display.setCursor(0, 0);
  // display.println("Servo Control");
  // display.display();

  // Start serial communication
  Serial.begin(115200);
  Serial.println("Enter angles and intervals for servos:");
  Serial.println("Format: angle1,interval1 angle2,interval2 angle3,interval3 angle4,interval4 angle5,interval5");
  Serial.println("Example: 90,15 45,10 180,20 0,5 120,25");
}

void loop() {


  // Handle Serial input first
  if (Serial.available() > 5 ) {
    handleSerialInput();
    serialControlActive = true; // Switch to Serial control mode
    lastSerialInputTime = millis(); // Update the last Serial input time
  }

  // Check if we should switch back to Joystick control
  if (serialControlActive && (millis() - lastSerialInputTime > serialTimeout)) {
    serialControlActive = false; // Switch back to Joystick control mode
  }

  // Handle Joystick inputs only if not in Serial control mode
  if (!serialControlActive) {
    handleJoystick1();
    handleJoystick2();
  }

  // Handle servo movements with individual timers
  unsigned long currentMillis = millis();

  // Servo 1 control
  if (currentMillis - previousMillis_1 >= interval_1) {
    previousMillis_1 = currentMillis;
    if (currentAngle1 != targetAngle1) {
      if (currentAngle1 < targetAngle1) currentAngle1++;
      else if (currentAngle1 > targetAngle1) currentAngle1--;
      servo1.write(currentAngle1);
    }
  }

  // Servo 2 control
  if (currentMillis - previousMillis_2 >= interval_2) {
    previousMillis_2 = currentMillis;
    if (currentAngle2 != targetAngle2) {
      if (currentAngle2 < targetAngle2) currentAngle2++;
      else if (currentAngle2 > targetAngle2) currentAngle2--;
      servo2.write(currentAngle2);
    }
  }

  // Servo 3 control
  if (currentMillis - previousMillis_3 >= interval_3) {
    previousMillis_3 = currentMillis;
    if (currentAngle3 != targetAngle3) {
      if (currentAngle3 < targetAngle3) currentAngle3++;
      else if (currentAngle3 > targetAngle3) currentAngle3--;
      servo3.write(currentAngle3);
    }
  }

  // Servo 4 control
  if (currentMillis - previousMillis_4 >= interval_4) {
    previousMillis_4 = currentMillis;
    if (currentAngle4 != targetAngle4) {
      if (currentAngle4 < targetAngle4) currentAngle4++;
      else if (currentAngle4 > targetAngle4) currentAngle4--;
      servo4.write(currentAngle4);
    }
  }

  // Servo 5 control
  if (currentMillis - previousMillis_5 >= interval_5) {
    previousMillis_5 = currentMillis;
    if (currentAngle5 != targetAngle5) {
      if (currentAngle5 < targetAngle5) currentAngle5++;
      else if (currentAngle5 > targetAngle5) currentAngle5--;
      servo5.write(currentAngle5);
    }
  }

  //updateOLED();
}

void handleSerialInput() {
  // Parse input for servo 1
  targetAngle1 = Serial.parseInt();
  if (Serial.peek() == ',') {
    Serial.read(); // Consume the comma
    interval_1 = Serial.parseInt();
  }

  // Parse input for servo 2
  targetAngle2 = Serial.parseInt();
  if (Serial.peek() == ',') {
    Serial.read(); // Consume the comma
    interval_2 = Serial.parseInt();
  }

  // Parse input for servo 3
  targetAngle3 = Serial.parseInt();
  if (Serial.peek() == ',') {
    Serial.read(); // Consume the comma
    interval_3 = Serial.parseInt();
  }

  // Parse input for servo 4
  targetAngle4 = Serial.parseInt();
  if (Serial.peek() == ',') {
    Serial.read(); // Consume the comma
    interval_4 = Serial.parseInt();
  }

  // Parse input for servo 5
  targetAngle5 = Serial.parseInt();
  if (Serial.peek() == ',') {
    Serial.read(); // Consume the comma
    interval_5 = Serial.parseInt();
  }

  // Consume any remaining characters until newline
  while (Serial.available() > 0 && Serial.peek() != '\n') {
    Serial.read();
  }

  // Validate angles
  targetAngle1 = constrain(targetAngle1, 0, 180);
  targetAngle2 = constrain(targetAngle2, 0, 180);
  targetAngle3 = constrain(targetAngle3, 0, 180);
  targetAngle4 = constrain(targetAngle4, 0, 180);
  targetAngle5 = constrain(targetAngle5, 0, 180);

  // Validate intervals (minimum 5ms, maximum 100ms)
  interval_1 = constrain(interval_1, 5, 100);
  interval_2 = constrain(interval_2, 5, 100);
  interval_3 = constrain(interval_3, 5, 100);
  interval_4 = constrain(interval_4, 5, 100);
  interval_5 = constrain(interval_5, 5, 100);

  // Print current settings
  Serial.println("Target angles and intervals (Serial Control):");
  Serial.print("Servo 1: "); Serial.print(targetAngle1); Serial.print("° at "); Serial.print(interval_1); Serial.println("ms");
  Serial.print("Servo 2: "); Serial.print(targetAngle2); Serial.print("° at "); Serial.print(interval_2); Serial.println("ms");
  Serial.print("Servo 3: "); Serial.print(targetAngle3); Serial.print("° at "); Serial.print(interval_3); Serial.println("ms");
  Serial.print("Servo 4: "); Serial.print(targetAngle4); Serial.print("° at "); Serial.print(interval_4); Serial.println("ms");
  Serial.print("Servo 5: "); Serial.print(targetAngle5); Serial.print("° at "); Serial.print(interval_5); Serial.println("ms");
}

void handleJoystick1() {
  // Read Joystick 1 values
  int joystick1Vert = analogRead(joystick1VertPin);
  int joystick1Horz = analogRead(joystick1HorzPin);
  bool joystick1ButtonPressed = digitalRead(joystick1ButtonPin) == LOW;

  // Map joystick values to servo angles (0-180)
  targetAngle2 = map(joystick1Horz, 0, 1023, 0, 180); // Servo 2 controlled by HORZ
  if (!joystick1ButtonPressed) {
    targetAngle1 = map(joystick1Vert, 0, 1023, 0, 180); // Servo 1 controlled by VERT
    targetAngle3 = 90; // Reset Servo 3 when button is not pressed
  } else {
    targetAngle3 = map(joystick1Vert, 0, 1023, 0, 180); // Servo 3 controlled by VERT
    //targetAngle1 = 90; // Reset Servo 1 when button is pressed
  }

  // Ensure angles are within range
  targetAngle1 = constrain(targetAngle1, 0, 180);
  targetAngle2 = constrain(targetAngle2, 0, 180);
  targetAngle3 = constrain(targetAngle3, 0, 180);

  // Serial.print("Servo angles from Joystick 1: ");
  // Serial.print(targetAngle1); Serial.print(" ");
  // Serial.print(targetAngle2); Serial.print(" ");
  // Serial.println(targetAngle3);
  //delay(50);
}

void handleJoystick2() {
  // Read Joystick 2 values
  int joystick2Vert = analogRead(joystick2VertPin);
  int joystick2Horz = analogRead(joystick2HorzPin);
  bool joystick2ButtonPressed = digitalRead(joystick2ButtonPin) == LOW;

  // Map joystick values to servo angles (0-180)
  targetAngle4 = map(joystick2Vert, 0, 1023, 0, 180); // Servo 4 controlled by VERT
  targetAngle5 = map(joystick2Horz, 0, 1023, 0, 180); // Servo 5 controlled by HORZ

  // Ensure angles are within range
  targetAngle4 = constrain(targetAngle4, 0, 180);
  targetAngle5 = constrain(targetAngle5, 0, 180);

  // Serial.print(targetAngle4); Serial.print(" ");
  // Serial.println(targetAngle5);
  //delay(50);
}

// void updateOLED() {
//   // Clear the display
//   display.clearDisplay();

//   // Set text size and color
//   display.setTextSize(1);
//   display.setTextColor(SSD1306_WHITE);

//   // Display current and target angles for each servo
//   display.setCursor(0, 0);
//   display.println("Servo Angles:");

//   display.setCursor(0, 10);
//   display.print("S1: "); display.print(currentAngle1); display.print(" -> "); display.println(targetAngle1);

//   display.setCursor(0, 20);
//   display.print("S2: "); display.print(currentAngle2); display.print(" -> "); display.println(targetAngle2);

//   display.setCursor(0, 30);
//   display.print("S3: "); display.print(currentAngle3); display.print(" -> "); display.println(targetAngle3);

//   display.setCursor(0, 40);
//   display.print("S4: "); display.print(currentAngle4); display.print(" -> "); display.println(targetAngle4);

//   display.setCursor(0, 50);
//   display.print("S5: "); display.print(currentAngle5); display.print(" -> "); display.println(targetAngle5);

//   // Update the display
//   display.display();
// }