#include <Servo.h>

Servo myservo1, myservo2, myservo3, myservo4, myservo5, myservo6;

// Arrays of Servo pointers for flexibility
Servo* left_servos[] = {&myservo1, &myservo2, &myservo3};
Servo* right_servos[] = {&myservo4, &myservo5, &myservo6};
Servo* left_und_servos[] = {&myservo1, &myservo2, &myservo6};
Servo* right_und_servos[] = {&myservo3, &myservo4, &myservo5};

void setup() {
  myservo1.attach(11);  
  myservo2.attach(10);  
  myservo3.attach(9);  
  myservo4.attach(6);  
  myservo5.attach(5);  
  myservo6.attach(3);
   
  Serial.begin(9600);
  Serial.println("Waiting for input...");
  Serial.println(" 1st character (A, B, C for Bending modes - Simple, Oscilate, Undulate)");
  Serial.println(" 2nd character (L, R, B for Left, Right, or Both)");
  Serial.println(" 3rd character (1-9 for number of cycles)");
  Serial.println(" Example 1: AR3 means simple bending towards right for 3 times.)");
  Serial.println(" Example 2: BL2 means oscillate 2 times, side (Left/Right) is not used in this case.)");
  Serial.println(" Example 3: CB1 means bend to form undulation pattern towards both left and right side, 1 cycle.)");
}

void loop() {
    if (Serial.available() > 0) {
        String input = Serial.readStringUntil('\n'); // Read input until newline
        input.trim();  // Remove any leading/trailing whitespace

        if (input.length() == 3) {  // Ensure input is exactly 3 characters
            char mode = input[0];  // First character (A, B, C for Bending modes)
            char side = input[1];  // Second character (L, R, B for Left, Right, or Both)
            int cycles = input[2] - '0'; // Convert third character to integer

            if (cycles >= 0 && cycles <= 9) { // Validate the number
                Serial.print("Mode: ");
                Serial.println(mode);
                Serial.print("Side: ");
                Serial.println(side);
                Serial.print("Cycles: ");
                Serial.println(cycles);

                // Call function based on the letter
                switch (mode) {
                    case 'A':
                        if (side == 'L') {
                            Bend_A(cycles, left_servos, 3);
                        }
                        else if (side == 'R') {
                            Bend_A(cycles, right_servos, 3);
                        }
                        else {
                            Serial.println("Insert Correct Side (L/R)");
                        }
                        break;
                    case 'B':
                        Oscilate_B(cycles);
                        break;
                    case 'C':
                        if (side == 'L') {
                            left_und_C(cycles);
                        }
                        else if (side == 'R') {
                            right_und_C(cycles);
                        }
                        else if (side == 'B') {
                            Swim_C(cycles);
                        }
                        else {
                            Serial.println("Insert Correct Side (L/R)");
                        }
                        break;
                    default:
                        Serial.println("Invalid Mode!");
                        break;
                }
            } else {
                Serial.println("Invalid number!");
            }
        } else {
            Serial.println("Invalid input format!");
        }
    }
}

// Function to perform bending motion on a specific set of servos
void Bend_A(int cycles, Servo* servos[], int numServos) {
  for (int i = 0; i < cycles; i++) {
    easeInOut(servos, numServos, 90, 0, 100); // forward stroke
    delay(500);
    easeInOut(servos, numServos, 0, 90, 100); // backward stroke
    delay(1000);
  }
}

// Function to oscillate servos
void Oscilate_B(int cycles) {
  for (int i = 0; i < cycles; i++){
    Bend_A(1, left_servos, 3);
    Bend_A(1, right_servos, 3);
  }
}


void Swim_C(int cycles) {
  for (int i = 0; i < cycles; i++){
    Bend_A(1, left_und_servos, 3);
    Bend_A(1, right_und_servos, 3);
  }
}

void left_und_C(int cycles) {
  Bend_A(cycles, left_und_servos, 3);
}

void right_und_C(int cycles) {
  Bend_A(cycles, right_und_servos, 3);
}





// Function for ease-in and ease-out motion using cubic easing
void easeInOut(Servo* servos[], int numServos, int startPos, int endPos, int duration) {
  int steps = 180;
  float delayTime = (float)duration / steps;

  for (int i = 0; i <= steps; i++) {
    // Calculate easing function
    float progress = (float)i / steps;
    float easeProgress = 3 * progress * progress - 2 * progress * progress * progress;
    
    // Calculate the current position
    int currentPos = startPos + easeProgress * (endPos - startPos);

    // Move only the selected servos
    for (int j = 0; j < numServos; j++) {
        if (servos[j] != nullptr) { // Ensure servo is valid
            servos[j]->write(currentPos);
        }
    }

    Serial.println(currentPos);
    delay(delayTime);
  }
}