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

// Define constants
const int MAX_INPUT_LENGTH = 200; // Define a maximum input length
const int MAX_SERVO_COUNT = 32; // Define maximum number of servos

// Create two instances of the Adafruit_PWMServoDriver for two boards
Adafruit_PWMServoDriver pwm1 = Adafruit_PWMServoDriver(0x40); // Default I2C address for the first board
Adafruit_PWMServoDriver pwm2 = Adafruit_PWMServoDriver(0x41); // Default I2C address for the second board

// Define the minimum and maximum pulse lengths for your servos
#define SERVOMIN  150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  600 // This is the 'maximum' pulse length count (out of 4096)

// Define pins for the A4988 stepper motor driver
#define dirPin 14
#define stepPin 12

// Function to set servo angle
void setServoAngle(int servoNumber, int angle) {
  int pulseLength = map(angle, 0, 180, SERVOMIN, SERVOMAX);
  if (servoNumber <= 16) {
    pwm1.setPWM(servoNumber - 1, 0, pulseLength);
  } else {
    pwm2.setPWM(servoNumber - 17, 0, pulseLength);
  }
}

// Function to execute the command based on the servo number and delay
void executeCommand(int servoNumber, int delayTime) {
  if (servoNumber >= 1 && servoNumber <= MAX_SERVO_COUNT) {
    if (servoNumber % 2 == 0) {  // Even servo
      setServoAngle(servoNumber, 180);
      delay(delayTime);
      setServoAngle(servoNumber, 160);
    } else {  // Odd servo
      setServoAngle(servoNumber, 2);
      delay(delayTime);
      setServoAngle(servoNumber, 20);
    }
  } else {
    Serial.println("Invalid servo number. Please enter a value between 1 and 32.");
  }
}

void executeMultipleServos(String command) {
  int delayIdx = command.lastIndexOf("-(");
  if (delayIdx != -1) {
    Serial.println(delayIdx);
    int delayTime = command.substring(delayIdx + 2, command.length()).toInt();
    String servos = command.substring(0, delayIdx);
    char servoBuffer[servos.length() + 1];
    servos.toCharArray(servoBuffer, servos.length() + 1);
    char* token = strtok(servoBuffer, "-");

    // Move servos to target positions
    while (token != NULL) {
      int servoNumber = String(token).toInt();
      if (servoNumber >= 1 && servoNumber <= MAX_SERVO_COUNT) {
        if (servoNumber % 2 == 0) {
          setServoAngle(servoNumber, 180); // Set even servo to 180 degrees
        } else {
          setServoAngle(servoNumber, 0); // Set odd servo to 0 degrees
        }
      } else {
        Serial.println("Invalid servo number. Please enter a value between 1 and 32.");
      }
      token = strtok(NULL, "-");
    }

    Serial.print("Delay for ");
    Serial.print(delayTime);
    Serial.println(" ms");
    delay(delayTime);

    // Reset the servos
    Serial.println("Resetting servos:");
    servos.toCharArray(servoBuffer, servos.length() + 1); // Reset the buffer
    token = strtok(servoBuffer, "-");
    while (token != NULL) {
      int servoNumber = String(token).toInt();
      Serial.print("Resetting servo ");
      Serial.println(servoNumber);
      if (servoNumber % 2 == 0) {
        setServoAngle(servoNumber, 160); // Reset even servo to 160 degrees
      } else {
        setServoAngle(servoNumber, 20); // Reset odd servo to 20 degrees
      }
      token = strtok(NULL, "-");
    }
  } else {
    Serial.println("Invalid command format.");
  }
}

// Function to control stepper motor
void moveStepperMotor(char direction, int steps) {
  Serial.println(direction);
  // Set direction: LOW for counterclockwise (left), HIGH for clockwise (right)
  digitalWrite(dirPin, direction == 'l' ? LOW : HIGH);
  for (int i = 0; i < steps; i++) {
    // Create a step pulse
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(800); // Adjust delay to control speed, this is a common delay, adjust as needed
    digitalWrite(stepPin, LOW);
    delayMicroseconds(800); // Adjust delay to control speed
  }
}

void parseAndExecuteCommands(String input) {
  input.trim(); // Remove any leading or trailing whitespace

  int index = 0;
  while (index < input.length()) {
    if (input.startsWith("r(")) {
      int startIdx = index + 2; // Skip 'r('
      int endIdx = input.indexOf(')', startIdx);
      if (endIdx != -1) {
        int restTime = input.substring(startIdx, endIdx).toInt();
        Serial.print("Resting for ");
        Serial.print(restTime);
        Serial.println(" ms");
        delay(restTime);
        index = endIdx + 1; // Move past ')'
      } else {
        Serial.println("Invalid rest command format.");
        break;
      }
    } else if (input.startsWith("c(")) {
      int startIdx = index + 2; // Skip 'c('
      int endIdx = input.indexOf(')', startIdx);
      if (endIdx != -1) {
        String command = input.substring(startIdx, endIdx);
        executeMultipleServos(command);
        index = endIdx + 1; // Move past ')'
      } else {
        Serial.println("Invalid command format.");
        break;
      }
    } else if (input.startsWith("m(")) {
      int startIdx = 2; // Skip 'm('
      int endIdx = input.indexOf(')', startIdx);
      if (endIdx != -1) {
        String directionAndSteps = input.substring(startIdx, endIdx);
        char direction = directionAndSteps.charAt(0); // Get 'l' or 'r'
        int steps = directionAndSteps.substring(2).toInt(); // Extract the steps part
        moveStepperMotor(direction, steps);
        index = endIdx + 1; // Move past ')'
      } else {
        Serial.println("Invalid motor command format.");
        break;
      }
    } else {
      int startIdx = index;
      int endIdx = input.indexOf('(', startIdx);
      if (endIdx != -1) {
        int servoNumber = input.substring(startIdx, endIdx).toInt();
        int delayStartIdx = endIdx + 1; // Skip '('
        int delayEndIdx = input.indexOf(')', delayStartIdx);
        if (delayEndIdx != -1) {
          int delayTime = input.substring(delayStartIdx, delayEndIdx).toInt();
          executeCommand(servoNumber, delayTime);
          index = delayEndIdx + 1; // Move past ')'
        } else {
          Serial.println("Invalid command format.");
          break;
        }
      } else {
        Serial.println("Invalid command format.");
        break;
      }
    }
    index++; // Move to the next character
  }
}

void setup() {
  Serial.begin(115200);
  Serial.println("PCA9685 Servo Driver and Stepper Motor Control Test!");

  pwm1.begin();
  pwm2.begin();

  pwm1.setPWMFreq(60);  // Analog servos run at ~60 Hz updates
  pwm2.setPWMFreq(60);  // Analog servos run at ~60 Hz updates

  // Set stepper motor pins as outputs
  pinMode(dirPin, OUTPUT);
  pinMode(stepPin, OUTPUT);

  delay(10);

  // Initialize servos: odd servos to 20 degrees, even servos to 160 degrees
  for (int i = 1; i <= MAX_SERVO_COUNT; i++) {
    int angle = (i % 2 == 0) ? 160 : 20;
    setServoAngle(i, angle);
  }
}

void loop() {
  if (Serial.available()) {
    // Limit the input length to avoid overflow
    char inputBuffer[MAX_INPUT_LENGTH + 1];
    int inputLength = Serial.readBytesUntil('\n', inputBuffer, MAX_INPUT_LENGTH);
    inputBuffer[inputLength] = '\0'; // Null-terminate the string

    if (inputLength == MAX_INPUT_LENGTH) {
      Serial.println("Input too long. Please limit your input.");
      while (Serial.available()) {
        Serial.read(); // Clear the buffer
      }
    } else {
      String input = String(inputBuffer);
      parseAndExecuteCommands(input);
    }
  }
}