#include <Wire.h>

#define MOTOR_BOARD_ADDR 0x08  // Motor control board
#define LED_BOARD_ADDR   0x09  // LED board

void setup() {
  Serial.begin(115200);
  Wire.begin();
  Serial.println("Enter commands (e.g., F1000 B2000 L R O C):");
}

void loop() {
  if (Serial.available()) {
    String inputLine = Serial.readStringUntil('\n');
    inputLine.trim();
    if (inputLine.length() == 0) return;

    // Split the input line into separate commands
    int startIdx = 0;
    while (startIdx < inputLine.length()) {
      // Find the next space (command separator)
      int spaceIdx = inputLine.indexOf(' ', startIdx);
      if (spaceIdx == -1) spaceIdx = inputLine.length();

      // Extract one command token
      String token = inputLine.substring(startIdx, spaceIdx);
      token.trim();

      if (token.length() > 0) {
        char command = toupper(token.charAt(0));
        int value = 0;

        if (token.length() > 1) {
          value = token.substring(1).toInt();
        }

        Serial.print("Sending Command: ");
        Serial.print(command);
        Serial.print(" | Value: ");
        Serial.println(value);

        // Send to motor board (always)
        Wire.beginTransmission(MOTOR_BOARD_ADDR);
        Wire.write((uint8_t)command);
        Wire.write((byte *)&value, sizeof(int));
        Wire.endTransmission();
        delay(10);

        // Also send to LED board if it’s an LED command
        if (command == 'L' || command == 'R' || command == 'O' || command == 'C' || command == 'B') {
          Wire.beginTransmission(LED_BOARD_ADDR);
          Wire.write((uint8_t)command);
          Wire.endTransmission();
          delay(10);
        }
      }

      // Move to next token
      startIdx = spaceIdx + 1;
    }

    Serial.println("All commands sent.\n");
  }
}