/*
 * Multi Servo Control via Serial (JSON-like format)
 *
 * Receives commands over Serial monitor to control multiple servos.
 * Expected command format: {S<index>:<angle>,S<index>:<angle>,...}
 * Example: {S0:90,S1:45,S5:180}
 *
 * - Validates the overall { } structure.
 * - Parses comma-separated segments.
 * - Validates each segment format (S<index>:<angle>).
 * - Uses robust integer parsing (strtol) for index and angle.
 * - Checks if servo index is within valid range.
 * - Constrains requested angle to servo's min/max limits.
 * - Controls the servos.
 * - Provides detailed feedback and error messages via Serial.
 *
 * Refactored based on discussions to improve structure, robustness, and readability.
 */

#include <Servo.h>   // Include the Servo library
#include <errno.h>   // For strtol error checking
#include <stdlib.h>  // For strtol
#include <limits.h>  // For INT_MIN, INT_MAX

// --- User Defined Constants ---
const int SERVO_NUM = 6; // Total number of servos

// Maximum angle limits for each servo (adjust as needed)
const int SERVO_MAX[SERVO_NUM] = {
  180, 180, 180, 180, 180, 180
};

// Minimum angle limits for each servo (adjust as needed)
const int SERVO_MIN[SERVO_NUM] = {
  0, 0, 0, 0, 0, 0
};

// Arduino pins connected to each servo's signal line
// Ensure these pins support PWM (usually marked with ~ on Arduino Uno/Nano)
const int SERVO_PIN[SERVO_NUM] = {
  3, 2, 8, 9, 12, 13
};
// --- End Constants ---

// Array to hold the Servo objects
Servo myServos[SERVO_NUM];

// --- Helper Function for Robust Integer Parsing ---
// Attempts to parse the entire String s as an integer.
// Returns true on success, false otherwise.
// Stores the result in 'value' on success.
bool parseStringToInt(const String& s, int& value) {
  if (s.length() == 0) {
    return false; // Empty string is not a valid number
  }
  // Get C-style string for strtol
  const char* s_cstr = s.c_str();
  char* endptr;
  errno = 0; // Reset error number

  long result = strtol(s_cstr, &endptr, 10); // Base 10

  // Check for errors:
  // 1. Did strtol encounter an error (e.g., overflow)?
  // 2. Was the entire string consumed? (endptr should point to the null terminator '\0')
  // 3. Check if the result fits within an int range (strtol returns long)
  if (errno != 0 || *endptr != '\0' || result < INT_MIN || result > INT_MAX) {
    return false; // Conversion failed, didn't consume the whole string, or out of int range
  }

  value = (int)result;
  return true;
}

// --- Function to Process a Single Command Segment (e.g., "S0:90") ---
// Modifies 'firstSegmentProcessed' flag to control comma printing in feedback.
void processSegment(const String& segment, bool& firstSegmentProcessed) {
    // --- Flags for Validation ---
    bool segmentIsValid = true; // Assume valid initially for this segment
    String errorMessage = "";
    int servoIndex = -1;
    int requestedAngle = 0;
    int constrainedAngle = 0;

    // --- Basic Format Check ---
    int colonIndex = segment.indexOf(':');

    if (!(segment.length() > 2 && segment.startsWith("S"))) {
        segmentIsValid = false;
        errorMessage = "[Invalid Segment Format: Doesn't start with 'S' or too short]";
    } else if (!(colonIndex > 1 && colonIndex < segment.length() - 1)) {
        segmentIsValid = false;
        errorMessage = "[Malformed Segment: Missing or misplaced colon]";
    } else {
        // --- Segment format looks okay, parse Index and Angle ---
        String indexStr = segment.substring(1, colonIndex);
        String angleStr = segment.substring(colonIndex + 1);

        int parsedIndex;
        int parsedAngle;

        bool indexParsedOk = parseStringToInt(indexStr, parsedIndex);
        bool angleParsedOk = false; // Parsed only if index is valid
        bool indexInRange = false;  // Check only if index parsing was okay

        if (!indexParsedOk) {
             segmentIsValid = false;
             errorMessage = "[Invalid Servo Index: Not a valid number]";
        } else {
            // Index parsed, now check range
            if (parsedIndex >= 0 && parsedIndex < SERVO_NUM) {
                indexInRange = true;
                // Index is valid and in range, try parsing the angle
                angleParsedOk = parseStringToInt(angleStr, parsedAngle);
                if (!angleParsedOk) {
                   segmentIsValid = false;
                   errorMessage = "[Invalid Angle: Not a valid number]";
                }
            } else {
                 segmentIsValid = false;
                 errorMessage = "[Invalid Servo Index: Out of range (0-" + String(SERVO_NUM - 1) + ")]";
            }
        }

        // If all checks passed so far, store values and constrain angle
        if (segmentIsValid) {
            servoIndex = parsedIndex;
            requestedAngle = parsedAngle;
            // Constrain the angle using servo-specific limits
            constrainedAngle = constrain(requestedAngle, SERVO_MIN[servoIndex], SERVO_MAX[servoIndex]);
        }
    }

    // --- Act based on validation and Print Feedback ---
    // Print comma separator if not the first valid/invalid segment processed
    if (!firstSegmentProcessed) Serial.print(", ");

    if (segmentIsValid) {
        // *** Robot Direct Control is Here ***
        myServos[servoIndex].write(constrainedAngle);

        // Feedback for success
        Serial.print("S"); Serial.print(servoIndex); Serial.print(":"); Serial.print(constrainedAngle);
        if (requestedAngle != constrainedAngle) {
            // Show if the angle was constrained
            Serial.print("(<-"); Serial.print(requestedAngle); Serial.print(")");
        }
    } else {
        // Feedback for error
        Serial.print(errorMessage); Serial.print(" '"); Serial.print(segment); Serial.print("'");
    }
    // Mark that at least one segment (valid or invalid) has been processed and outputted
    firstSegmentProcessed = false;
}


// --- Arduino Setup Function ---
void setup() {
  // Start Serial communication (baud rate should match Serial Monitor setting)
  Serial.begin(57600);
  // Wait for Serial port to connect (needed for native USB like Leonardo, Micro)
  // Add a timeout to prevent blocking forever if Serial isn't connected
  unsigned long setupStartTime = millis();
  while (!Serial && (millis() - setupStartTime < 5000)) { // Wait max 5 seconds
    delay(10); // Small delay while waiting
  }

  Serial.println("\n--- Multi Servo Control via Serial ---");
  Serial.print("Initializing "); Serial.print(SERVO_NUM); Serial.println(" servos...");
  Serial.println("Command format: {S<index>:<angle>,S<index>:<angle>,...}");
  Serial.println("Example: {S0:90,S1:45,S5:180}");
  Serial.println("Valid servo indices: 0 to " + String(SERVO_NUM - 1));
  Serial.println("---------------------------------------");

  // Attach each servo to its pin and set initial position
  for (int i = 0; i < SERVO_NUM; i++) {
    myServos[i].attach(SERVO_PIN[i]);
    // Set servos to a defined starting position (e.g., minimum)
    // Consider if middle (90) or another position is a safer start for your setup
    myServos[i].write( (SERVO_MIN[i]+SERVO_MAX[i])/2 );
    delay(20); // Allow time for servo to potentially reach initial position
    Serial.print("Servo "); Serial.print(i);
    Serial.print(" attached to pin "); Serial.print(SERVO_PIN[i]);
    Serial.print(" | Limits: ["); Serial.print(SERVO_MIN[i]);
    Serial.print(", "); Serial.print(SERVO_MAX[i]); Serial.println("]");
  }
  Serial.println("---------------------------------------");
  Serial.println("Initialization Complete. Ready for commands.");
}

// --- Arduino Main Loop ---
void loop() {
  // Check if data is available to read from Serial
  if (Serial.available() > 0) {
    // Read the incoming string until newline character
    // NOTE: Using the String class extensively can lead to memory fragmentation
    // on memory-constrained boards (like Arduino Uno/Nano/Mega) over long periods.
    // If the device needs to run reliably for extended durations or handles
    // very large commands, consider switching to C-style char arrays.
    String command = Serial.readStringUntil('\n');
    command.trim(); // Remove leading/trailing whitespace & newline chars

    // Ignore empty commands after trimming
    if (command.length() == 0) {
        return; // Nothing to do, wait for next loop iteration
    }

    // --- Validate overall command format FIRST ---
    bool formatIsValid = command.startsWith("{") && command.endsWith("}");

    // --- Guard Clause: If format is NOT valid, print error and exit this iteration ---
    if (!formatIsValid) {
      Serial.print("Error: Invalid command format '");
      Serial.print(command);
      Serial.println("'. Expected {S<index>:<angle>,...}");
      return; // Stop processing this invalid command
    }

    // --- Format IS Valid: Proceed with processing ---

    // Remove the braces to get the content string
    String content = command.substring(1, command.length() - 1);
    content.trim(); // Trim again in case of spaces like { S0:90 }

    // Handle empty content like {}
    if (content.length() == 0) {
        Serial.println("Received empty command: {}");
        return; // Nothing to process
    }

    // Print command start confirmation
    Serial.print("Processing: {");

    // Process each command segment separated by commas
    int startIndex = 0;
    bool firstSegmentOutput = true; // Flag to manage comma printing in feedback

    while (startIndex < content.length()) {
        // Extract the next segment based on comma delimiter
        int commaIndex = content.indexOf(',', startIndex);
        String currentSegment;

        if (commaIndex == -1) {
            // No more commas - this is the last segment
            currentSegment = content.substring(startIndex);
            startIndex = content.length(); // Set index to exit loop after this segment
        } else {
            // Extract segment before the next comma
            currentSegment = content.substring(startIndex, commaIndex);
            startIndex = commaIndex + 1; // Move start index past the comma for next iteration
        }

        currentSegment.trim(); // Clean up the individual segment (remove spaces)

        // Skip empty segments that might result from extra commas (e.g., {S0:90,,S1:90})
        if (currentSegment.length() == 0) {
            continue; // Go to the next iteration of the while loop
        }

        // Process the extracted segment using the helper function
        processSegment(currentSegment, firstSegmentOutput);

    } // End while loop processing segments

    Serial.println("}"); // End the feedback line for the processed command

  } // End if Serial.available()

  // No delay() needed here in most cases. The loop runs again quickly.
  // Other non-blocking code (reading sensors, checking buttons) could go here.
}