#include <ESP32Servo.h>
#include <cmath> // Include the standard C++ math library

// Define the number of rows and columns in the grid
const int ROWS = 3;
const int COLS = 3;

// Define the servo pins
const int servoPins[ROWS][COLS] = {
  {2, 4, 16},
  {17, 5, 18},
  {33, 32, 25} // Corrected pin 35 to 25, as 35 is input only
};

// Create servo objects
Servo servos[ROWS][COLS];

// Define the minimum and maximum angles for the servos
const int MIN_ANGLE = 30;
const int MAX_ANGLE = 150;

// Define the base delay between each servo movement
const int MIN_SERVO_DELAY = 5;
const int MAX_SERVO_DELAY = 50;

// Define the wave delay between rows
const int MIN_WAVE_DELAY = 50;
const int MAX_WAVE_DELAY = 500;

// Define the delay between columns
const int COLUMN_DELAY = 20;

// Define the potentiometer pins for speed and intensity control
const int SPEED_POT_PIN = 26;
const int INTENSITY_POT_PIN = 27;

// Define an array to store the current angle of each servo
int servoAngles[ROWS][COLS];

// Variables to store potentiometer values
int speedPotValue;
int intensityPotValue;

// Structure to hold servo movement data
typedef struct {
  int row;
  int col;
  float phase;
  unsigned long startTime;
} ServoMovement;

// Array to track active servo movements
ServoMovement activeMovements[ROWS * COLS];
int activeMovementCount = 0;

void setup() {
  Serial.begin(115200);
  // Allow allocation of all timers
  ESP32PWM::allocateTimer(0);
  ESP32PWM::allocateTimer(1);
  ESP32PWM::allocateTimer(2);
  ESP32PWM::allocateTimer(3);
  // Attach each servo to its corresponding pin
  for (int i = 0; i < ROWS; i++) {
    for (int j = 0; j < COLS; j++) {
      servos[i][j].setPeriodHertz(50);
      servos[i][j].attach(servoPins[i][j], 500, 2400);
      servoAngles[i][j] = MIN_ANGLE; // Start at the minimum angle
      servos[i][j].write(servoAngles[i][j]);
    }
  }
}

void loop() {
  // Read potentiometer values
  speedPotValue = analogRead(SPEED_POT_PIN);
  intensityPotValue = analogRead(INTENSITY_POT_PIN);

  // Map potentiometer values to servo delay and wave delay
  int servoDelay = map(speedPotValue, 0, 4095, MAX_SERVO_DELAY, MIN_SERVO_DELAY); // Reverse mapping for speed
  int waveDelay = map(intensityPotValue, 0, 4095, MIN_WAVE_DELAY, MAX_WAVE_DELAY);

  // Initiate new movements
  initiateMovements(waveDelay);

  // Update active movements
  updateMovements(servoDelay);

}

void initiateMovements(int waveDelay) {
  static unsigned long lastRowMovementTime[ROWS] = {0};
  
  unsigned long currentTime = millis();

  for (int i = 0; i < ROWS; i++) {
    if (currentTime - lastRowMovementTime[i] >= waveDelay) {
      for (int j = 0; j < COLS; j++) {
        
          // Check if a movement for this servo is already active
          bool movementAlreadyActive = false;
          for (int k = 0; k < activeMovementCount; k++) {
            if (activeMovements[k].row == i && activeMovements[k].col == j) {
              movementAlreadyActive = true;
              break;
            }
          }

          // If no movement is active for this servo, add a new one
          if (!movementAlreadyActive) {
            // Add the new movement to the array
            if (activeMovementCount < ROWS * COLS) {
              activeMovements[activeMovementCount].row = i;
              activeMovements[activeMovementCount].col = j;
              activeMovements[activeMovementCount].phase = 0.0f;
              activeMovements[activeMovementCount].startTime = currentTime;
              activeMovementCount++;
            
          }
        }
      }
      lastRowMovementTime[i] = currentTime; // Update last row movement time
    }
  }
}

void updateMovements(int servoDelay) {
  unsigned long currentTime = millis();

  for (int i = 0; i < activeMovementCount; i++) {
    ServoMovement* movement = &activeMovements[i];

    if (currentTime - movement->startTime >= servoDelay) {
      int angleOffset = map(intensityPotValue, 0, 4095, 0, (MAX_ANGLE - MIN_ANGLE) / 2);

      // Use the standard sin() function from cmath
      int newAngle = MIN_ANGLE + (MAX_ANGLE - MIN_ANGLE) / 2 + angleOffset * sin(movement->phase + movement->row * 0.5 + movement->col * 0.2);

      int angleVariation = random(-3, 4);
      newAngle = constrain(newAngle + angleVariation, MIN_ANGLE, MAX_ANGLE);

      servos[movement->row][movement->col].write(newAngle);
      servoAngles[movement->row][movement->col] = newAngle;

      movement->phase += 0.05;
      movement->startTime = currentTime;

      if (movement->phase >= 2 * PI) {
        // Remove completed movement
        for (int j = i; j < activeMovementCount - 1; j++) {
          activeMovements[j] = activeMovements[j + 1];
        }
        activeMovementCount--;
        i--; // Decrement i to recheck the current index after removal
      }
    }
  }
}