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

/*
 * PCA9685 Servo Controller Code with I2C Scanner
 * Compatible with both Arduino UNO and ESP32.
 *
 * Usage Instructions:
 * - Use the serial monitor to send commands in the following format:
 *   <SPEED:speed_value;ANGLE:channel-angle_pairs;ENABLE:channels;DISABLE:channels>
 *
 * - All settings can be input as a single string.
 * - Parameters:
 *   - SPEED: Set the servo movement speed (delay in ms between steps).
 *     - Example: SPEED:5
 *   - ANGLE: Set the angles of servos on specified channels.
 *     - Provide channel-angle pairs separated by commas.
 *     - Example: ANGLE:0-90,1-45,2-135
 *   - ENABLE: Enable specified channels.
 *     - Provide channels separated by commas.
 *     - Example: ENABLE:0,1,2
 *   - DISABLE: Disable specified channels.
 *     - Provide channels separated by commas.
 *     - Example: DISABLE:3,4,5
 * - Combine multiple parameters using semicolons.
 *   - Example: SPEED:5;ANGLE:0-90,1-45;ENABLE:0,1;DISABLE:2,3
 *
 * Notes:
 * - Channels range from 0 to 15.
 * - Angles range from 0 to 180 degrees.
 * - All parameters are optional; include only what you want to change.
 * - Order of parameters does not matter.
 *
 * Default Behavior:
 * - On startup, only channel 0 is enabled by default; all other channels are disabled.
 * - The servo on channel 0 will automatically sweep back and forth between 0 and 180 degrees.
 */

#ifdef ESP32
  #define SERIAL_BAUDRATE 115200
  #define OE_PIN 27
  #define SDA_PIN 21
  #define SCL_PIN 22
#else
  #define SERIAL_BAUDRATE 115200
#endif

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

// Global variables to store speed and enabled channels
int servoSpeed = 5;           // Default delay between servo movements
bool channelEnabled[16];      // Array to keep track of enabled channels

// PCA9685 I2C address
uint8_t pca9685_address = 0x40;  // Default address

// Create an instance of the PWM servo driver
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(pca9685_address);

// Constants for servo min and max pulse length
#define SERVOMIN 150  // Min pulse length out of 4096
#define SERVOMAX 600  // Max pulse length out of 4096
#define MAX_RETRIES 5

void setup() {
  Serial.begin(SERIAL_BAUDRATE);
  delay(1000);  // Allow time for Serial to initialize
  Serial.println("PCA9685 Servo Controller Initialized");

  // Initialize I2C
  #ifdef ESP32
    Wire.begin(SDA_PIN, SCL_PIN);
    // Set OE_PIN as output and pull low to enable PWM output
    pinMode(OE_PIN, OUTPUT);
    digitalWrite(OE_PIN, LOW);
  #else
    Wire.begin();
  #endif

  // Run I2C Scanner
  scanI2CDevices();

  // Initialize all channels as disabled by default
  for (int i = 0; i < 16; i++) {
    channelEnabled[i] = false;
  }
  // Enable channel 0 by default
  channelEnabled[0] = true;

  // Initialize the PCA9685
  bool initialized = false;
  for (int i = 0; i < MAX_RETRIES; i++) {
    pwm = Adafruit_PWMServoDriver(pca9685_address);
    pwm.begin();
    pwm.setPWMFreq(60);  // Set the PWM frequency to 60 Hz
    delay(10);  // Short delay to allow settings to take effect

    // Read MODE1 register to verify connection
    uint8_t mode1 = readRegister(pca9685_address, 0x00);  // MODE1 register address is 0x00

    if (mode1 != 0xFF) {  // 0xFF indicates a failed read
      initialized = true;
      Serial.println("PCA9685 initialized successfully.");
      break;
    } else {
      Serial.print("Attempt ");
      Serial.print(i + 1);
      Serial.println(": PCA9685 initialization failed! Retrying...");
      delay(500);
    }
  }

  if (!initialized) {
    Serial.println("Error: PCA9685 initialization failed after multiple attempts! Check I2C address and wiring.");
    while (1);
  }

  Serial.println("PWM frequency set to 60 Hz.");
  Serial.println("Enter commands to control the servos in the following format:");
  Serial.println("<SPEED:speed_value;ANGLE:channel-angle_pairs;ENABLE:channels;DISABLE:channels>");
  Serial.println("Example: SPEED:5;ANGLE:0-90,1-45;ENABLE:0,1;DISABLE:2,3");
}

void loop() {
  // Automatic movement for enabled servos
  static uint16_t angle = 0;
  static int8_t increment = 1;

  // Move enabled servos
  for (uint8_t channel = 0; channel < 16; channel++) {
    if (channelEnabled[channel]) {
      setServoAngle(channel, angle);
    }
  }

  angle += increment;
  if (angle >= 180 || angle <= 0) {
    increment = -increment;
  }

  delay(servoSpeed);

  // Check for serial input
  if (Serial.available() > 0) {
    String input = Serial.readStringUntil('\n');
    input.trim();  // Remove any leading/trailing whitespace
    processCommand(input);
  }
}

// Function to scan I2C devices
void scanI2CDevices() {
  Serial.println("Scanning for I2C devices...");
  uint8_t error, address;
  int nDevices = 0;

  for (address = 1; address < 127; address++) {
    Wire.beginTransmission(address);
    error = Wire.endTransmission();

    if (error == 0) {
      Serial.print("I2C device found at address 0x");
      if (address < 16)
        Serial.print("0");
      Serial.print(address, HEX);
      Serial.println(" !");
      nDevices++;

      // If it's the first device found, assume it's the PCA9685
      if (nDevices == 1) {
        pca9685_address = address;
      }
    } else if (error == 4) {
      Serial.print("Unknown error at address 0x");
      if (address < 16)
        Serial.print("0");
      Serial.println(address, HEX);
    }
  }

  if (nDevices == 0) {
    Serial.println("No I2C devices found. Please check your connections.");
  } else {
    Serial.print(nDevices);
    Serial.println(" I2C devices found.");
    Serial.print("Using PCA9685 at address 0x");
    if (pca9685_address < 16)
      Serial.print("0");
    Serial.println(pca9685_address, HEX);
  }
}

// Function to read a register from the PCA9685
uint8_t readRegister(uint8_t deviceAddress, uint8_t reg) {
  Wire.beginTransmission(deviceAddress);
  Wire.write(reg);
  if (Wire.endTransmission() != 0) {
    return 0xFF;  // Indicate failure to read
  }

  Wire.requestFrom(deviceAddress, (uint8_t)1);
  if (Wire.available()) {
    return Wire.read();
  } else {
    return 0xFF;  // Indicate failure to read
  }
}

// Function to process serial commands
void processCommand(String command) {
  command.toUpperCase();  // Convert command to uppercase for case-insensitive comparison

  // Split the command into parameters using ';' as the delimiter
  int paramStart = 0;
  int paramEnd = command.indexOf(';', paramStart);

  while (paramStart < command.length()) {
    String param;
    if (paramEnd == -1) {
      param = command.substring(paramStart);
      paramEnd = command.length();
    } else {
      param = command.substring(paramStart, paramEnd);
    }

    param.trim();  // Remove whitespace

    // Process each parameter
    if (param.startsWith("SPEED:")) {
      // Set servo speed
      String valueStr = param.substring(6);
      valueStr.trim();
      int value = valueStr.toInt();
      if (value >= 0) {
        servoSpeed = value;
        Serial.print("Servo speed set to ");
        Serial.print(servoSpeed);
        Serial.println(" ms delay between steps.");
      } else {
        Serial.println("Invalid speed value. Please enter a positive integer.");
      }
    } else if (param.startsWith("ANGLE:")) {
      // Set angles for specified channels
      String anglesStr = param.substring(6);
      anglesStr.trim();
      processAngleCommands(anglesStr);
    } else if (param.startsWith("ENABLE:")) {
      // Enable specified channels
      String channelsStr = param.substring(7);
      channelsStr.trim();
      processEnableDisableCommands(channelsStr, true);
    } else if (param.startsWith("DISABLE:")) {
      // Disable specified channels
      String channelsStr = param.substring(8);
      channelsStr.trim();
      processEnableDisableCommands(channelsStr, false);
    } else {
      Serial.print("Unknown parameter: ");
      Serial.println(param);
    }

    paramStart = paramEnd + 1;
    paramEnd = command.indexOf(';', paramStart);
  }
}

// Function to process angle commands
void processAngleCommands(String anglesStr) {
  // Split the angles string into channel-angle pairs using ',' as the delimiter
  int pairStart = 0;
  int pairEnd = anglesStr.indexOf(',', pairStart);

  while (pairStart < anglesStr.length()) {
    String pair;
    if (pairEnd == -1) {
      pair = anglesStr.substring(pairStart);
      pairEnd = anglesStr.length();
    } else {
      pair = anglesStr.substring(pairStart, pairEnd);
    }

    pair.trim();  // Remove whitespace

    // Split the pair into channel and angle using '-' as the delimiter
    int dashIndex = pair.indexOf('-');
    if (dashIndex != -1) {
      String channelStr = pair.substring(0, dashIndex);
      String angleStr = pair.substring(dashIndex + 1);
      channelStr.trim();
      angleStr.trim();

      int channel = channelStr.toInt();
      int angle = angleStr.toInt();

      if (channel >= 0 && channel < 16 && angle >= 0 && angle <= 180) {
        setServoAngle(channel, angle);
      } else {
        Serial.print("Invalid channel or angle in pair: ");
        Serial.println(pair);
        Serial.println("Channel: 0-15, Angle: 0-180.");
      }
    } else {
      Serial.print("Invalid channel-angle pair: ");
      Serial.println(pair);
      Serial.println("Use the format channel-angle (e.g., 0-90).");
    }

    pairStart = pairEnd + 1;
    pairEnd = anglesStr.indexOf(',', pairStart);
  }
}

// Function to process enable/disable commands
void processEnableDisableCommands(String channelsStr, bool enable) {
  // Split the channels string into individual channels using ',' as the delimiter
  int chanStart = 0;
  int chanEnd = channelsStr.indexOf(',', chanStart);

  while (chanStart < channelsStr.length()) {
    String chanStr;
    if (chanEnd == -1) {
      chanStr = channelsStr.substring(chanStart);
      chanEnd = channelsStr.length();
    } else {
      chanStr = channelsStr.substring(chanStart, chanEnd);
    }

    chanStr.trim();  // Remove whitespace

    int channel = chanStr.toInt();

    if (channel >= 0 && channel < 16) {
      channelEnabled[channel] = enable;
      if (enable) {
        Serial.print("Channel ");
        Serial.print(channel);
        Serial.println(" enabled.");
      } else {
        pwm.setPWM(channel, 0, 0);  // Turn off PWM signal to the channel
        Serial.print("Channel ");
        Serial.print(channel);
        Serial.println(" disabled.");
      }
    } else {
      Serial.print("Invalid channel: ");
      Serial.println(chanStr);
      Serial.println("Channel must be between 0 and 15.");
    }

    chanStart = chanEnd + 1;
    chanEnd = channelsStr.indexOf(',', chanStart);
  }
}

// Function to set the angle of a specific servo
void setServoAngle(uint8_t n, uint16_t angle) {
  if (n > 15) {
    Serial.print("Error: Invalid channel ");
    Serial.println(n);
    return; // Only allow channels 0-15
  }
  if (!channelEnabled[n]) {
    Serial.print("Channel ");
    Serial.print(n);
    Serial.println(" is disabled. Enable it first.");
    return;
  }
  if (angle > 180) {
    Serial.println("Warning: Angle exceeds 180 degrees. Setting to 180.");
    angle = 180;
  }
  uint16_t pulselen = map(angle, 0, 180, SERVOMIN, SERVOMAX);
  pwm.setPWM(n, 0, pulselen);
}