#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define SERVOMIN  125   // Minimum pulse length count (out of 4096)
#define SERVOMAX  600   // Maximum pulse length count (out of 4096)
#define SERVO_FREQ 60   // Analog servos run at ~50 Hz updates

#define MATRIX_ROWS 4   // Number of rows in the matrix
#define MATRIX_COLS 4   // Number of columns in the matrix
#define SERVO_MIN_ANGLE 0
#define SERVO_MAX_ANGLE 180

unsigned long lastUpdateTime = 0;
const unsigned long updateInterval = 100;  // Time between updates in milliseconds

void setup() {
  Serial.begin(115200);
  pwm.begin();
  pwm.setPWMFreq(SERVO_FREQ);  // Set the frequency to 50 Hz
  waveLeftRight();
}

void loop() {
  unsigned long currentTime = millis();

  if (currentTime - lastUpdateTime >= updateInterval) {
    // waveLeftRight();
    waveTopBottom();

    lastUpdateTime = currentTime;
  }
  delay(100);
}

// Function to control a servo at a specific row and column
void setServoAngle(int row, int col, int angle) {
  int servoNumber = row * MATRIX_COLS + col;  // Calculate servo number (0 to 15)
  int pwmValue = map(angle, 0, 180, SERVOMIN, SERVOMAX); // Map angle to PWM
  Serial.println("Set PWM for Servo#" + String(servoNumber) + ", pwmValue " + String(pwmValue));
  pwm.setPWM(servoNumber, 0, pwmValue);
  // delay(50);
}

// Wave pattern from left to right and back
void waveLeftRight() {
  // Left to Right
  for (int col = 0; col < MATRIX_COLS; col++) {
    for (int row = 0; row < MATRIX_ROWS; row++) {
      setServoAngle(row, col, SERVO_MIN_ANGLE); // Move to 0 degrees
    }
    delay(200);  // Delay between columns
  }
  delay(100);
  // Right to Left
  for (int col = MATRIX_COLS - 1; col >= 0; col--) {
    for (int row = 0; row < MATRIX_ROWS; row++) {
      setServoAngle(row, col, SERVO_MAX_ANGLE); // Move to 180 degrees
    }
    delay(200); // Delay between columns
  }
  delay(100); 
}

// Wave pattern from top to bottom and back
void waveTopBottom() {
  // Top to Bottom
  for (int row = 0; row < MATRIX_ROWS; row++) {
    for (int col = 0; col < MATRIX_COLS; col++) {
      setServoAngle(row, col, SERVO_MIN_ANGLE); // Move to 0 degrees
    }
    delay(200);  // Delay between rows
  }
  delay(100);
  // Bottom to Top
  for (int row = MATRIX_ROWS - 1; row >= 0; row--) {
    for (int col = 0; col < MATRIX_COLS; col++) {
      setServoAngle(row, col, SERVO_MAX_ANGLE); // Move to 180 degrees
    }
    delay(200);  // Delay between rows
  }
  delay(100);
}