/*
 * Multi Servo Control via Serial (C-String Version)
 * Includes Record/Playback and Pre-defined Sequence execution.
 *
 * Serial Commands:
 * {S<index>:<angle>,...} : Direct servo control (only when IDLE)
 * RECORD                  : Start recording commands
 * PLAY                    : Play back recorded sequence
 * STOP                    : Stop recording or playback or sequence
 * SEQUENCE                : Execute pre-defined interpolated sequence
 */

#include <Servo.h>
#include <errno.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>
#include <limits.h>
#include <math.h>             // For round() during interpolation
#include <avr/pgmspace.h>     // For PROGMEM functions

// --- User Defined Constants ---
const int SERVO_NUM = 6;
const byte SERVO_MAX[SERVO_NUM] PROGMEM = { 180, 180, 180, 130, 180, 180 }; // Store in Flash
const byte SERVO_MIN[SERVO_NUM] PROGMEM = { 0, 0, 0, 0, 0, 0 };       // Store in Flash
const byte SERVO_PIN[SERVO_NUM] PROGMEM = { 3, 2, 8, 9, 12, 13 };     // Store in Flash

// === Input Buffer ===
const byte MAX_COMMAND_LEN = 100;
char commandInputBuffer[MAX_COMMAND_LEN];
byte commandInputIndex = 0;

// === Recording & Playback Constants & Data ===
// Max steps - Adjust based on RAM. ~280 should be safe with ~1150 bytes free RAM
const int MAX_RECORDED_STEPS = 280;
//const unsigned long RECORDING_TIMEOUT_MS = 30000; // 30 seconds timeout - Optional

struct RecordedStep {
    uint16_t timeOffsetMs; // 2 bytes
    byte servoIndex;       // 1 byte
    byte servoAngle;       // 1 byte
}; // Total 4 bytes per step

RecordedStep recordedSequence[MAX_RECORDED_STEPS]; // ~280 * 4 = 1120 bytes SRAM
int recordedStepsCount = 0;

// === Pre-defined Sequence Constants & Data ===
const byte NUM_SEQUENCE_STATES = 7;
const unsigned long SEQUENCE_INTERPOLATION_TIME = 1000; // 1 second interpolation

// Store sequence data in Flash memory
const byte sequenceData[NUM_SEQUENCE_STATES][SERVO_NUM] PROGMEM = {
  {100, 85, 175, 80, 90, 0},   // State 0
  {100, 85, 175, 80, 90, 78},  // State 1
  {100, 100, 105, 80, 90, 78}, // State 2
  {180, 90, 120, 80, 90, 78},  // State 3
  {180, 90, 130, 80, 90, 78},  // State 4
  {180, 90, 130, 80, 90, 0},   // State 5
  {100, 90, 90, 90, 90, 0}    // State 6
};

// === State Machine & Timing Variables ===
enum State { IDLE, RECORDING, PLAYBACK };
State currentState = IDLE;

unsigned long recordingStartTime = 0;
unsigned long playbackStartTime = 0;
int playbackIndex = 0;

bool isRunningSequence = false;
int currentSequenceStateIndex = 0; // Index of the *target* state
unsigned long sequenceStateStartTime = 0;
int sequenceStartPos[SERVO_NUM];    // Servo positions at the start of an interpolation step

// --- Global Variables ---
Servo myServos[SERVO_NUM];

// --- Helper Function: Trim leading/trailing whitespace (Unchanged) ---
void trimWhitespace(char *str) {
    if (str == NULL || str[0] == '\0') return;
    char *start = str;
    while (isspace((unsigned char)*start)) start++;
    if (*start == '\0') { *str = '\0'; return; }
    char *end = str + strlen(str) - 1;
    while (end > start && isspace((unsigned char)*end)) end--;
    *(end + 1) = '\0';
    if (str != start) memmove(str, start, (end - start) + 2);
}

// --- Helper Function for Robust Integer Parsing from C-string (Unchanged) ---
bool parseCharStrToInt(const char* s, int& value) {
    if (s == NULL || s[0] == '\0' || isspace((unsigned char)s[0])) return false;
    char* endptr; errno = 0;
    long result = strtol(s, &endptr, 10);
    if (errno != 0 || *endptr != '\0' || result < INT_MIN || result > INT_MAX) return false;
    value = (int)result;
    return true;
}

// --- Function to Stop Recording ---
void stopRecording() {
    if (currentState == RECORDING) {
        currentState = IDLE;
        Serial.print(F("\nRecording stopped. ")); // Use F()
        Serial.print(recordedStepsCount);
        Serial.println(F(" steps recorded."));
    }
}

// --- Function to Process a Single Command Segment (Modified for Recording) ---
void processSegmentC(char* segment, bool& firstSegmentProcessed) {
    bool segmentIsValid = true;
    const char* errorMessage = "";
    int servoIndex = -1;
    int requestedAngle = 0;
    byte constrainedAngle = 0; // Use byte for angle (0-180)
    byte currentServoMin = 0;
    byte currentServoMax = 180;

    // Basic Format Check
    if (strlen(segment) < 4 || segment[0] != 'S') {
        segmentIsValid = false; errorMessage = "[Invalid Segment Format]";
    } else {
        char* colonPtr = strchr(segment, ':');
        if (colonPtr == NULL || colonPtr == segment + 1 || *(colonPtr + 1) == '\0') {
            segmentIsValid = false; errorMessage = "[Malformed Segment]";
        } else {
            *colonPtr = '\0'; // Temporarily split
            char* indexStr = segment + 1;
            char* angleStr = colonPtr + 1;
            int parsedIndex, parsedAngle;
            bool indexParsedOk = parseCharStrToInt(indexStr, parsedIndex);
            bool angleParsedOk = false;
            bool indexInRange = false;

            if (!indexParsedOk) {
                segmentIsValid = false; errorMessage = "[Invalid Index Num]";
            } else {
                 if (parsedIndex >= 0 && parsedIndex < SERVO_NUM) {
                    indexInRange = true;
                    angleParsedOk = parseCharStrToInt(angleStr, parsedAngle);
                    if (!angleParsedOk) {
                        segmentIsValid = false; errorMessage = "[Invalid Angle Num]";
                    }
                } else {
                    segmentIsValid = false; errorMessage = "[Index Out of Range]";
                }
            }

            if (segmentIsValid) {
                servoIndex = parsedIndex;
                requestedAngle = parsedAngle;
                // Read min/max from PROGMEM for constraining
                currentServoMin = pgm_read_byte(&SERVO_MIN[servoIndex]);
                currentServoMax = pgm_read_byte(&SERVO_MAX[servoIndex]);
                constrainedAngle = constrain(requestedAngle, currentServoMin, currentServoMax);
            }
        }
    }

    // --- Act, Record, and Print Feedback ---
    if (!firstSegmentProcessed) Serial.print(F(", "));

    if (segmentIsValid) {
        myServos[servoIndex].write(constrainedAngle);

        // Record if needed
        if (currentState == RECORDING) {
            if (recordedStepsCount < MAX_RECORDED_STEPS) {
                unsigned long now = millis();
                unsigned long offset = now - recordingStartTime;
                if (offset > 65535) {
                     stopRecording();
                     Serial.println(F("WARN: Recording stopped due to time limit (>65s)."));
                } else {
                   recordedSequence[recordedStepsCount].timeOffsetMs = (uint16_t)offset;
                   recordedSequence[recordedStepsCount].servoIndex = (byte)servoIndex;
                   recordedSequence[recordedStepsCount].servoAngle = constrainedAngle; // Store constrained angle
                   recordedStepsCount++;
                }
            } else {
                stopRecording();
                Serial.println(F("WARN: Recording stopped, memory full."));
            }
        }

        // Feedback
        Serial.print(F("S")); Serial.print(servoIndex); Serial.print(F(":")); Serial.print(constrainedAngle);
        if (requestedAngle != constrainedAngle) {
            Serial.print(F("(<-")); Serial.print(requestedAngle); Serial.print(F(")"));
        }
    } else {
        Serial.print(errorMessage); Serial.print(F(" '")); Serial.print(segment); Serial.print(F("'"));
    }
    firstSegmentProcessed = false;
}

// --- Function to process the raw command buffer ---
void processCommand(char* cmd) {
    trimWhitespace(cmd);
    int cmdLen = strlen(cmd);
    if (cmdLen == 0) return;

    // --- Handle Control Commands (RECORD, PLAY, STOP, SEQUENCE) FIRST ---
    if (strcmp(cmd, "RECORD") == 0) {
        if (currentState == IDLE && !isRunningSequence) {
            currentState = RECORDING;
            recordedStepsCount = 0;
            recordingStartTime = millis();
            Serial.println(F("Recording started... (Send STOP or wait)"));
        } else {
            Serial.println(F("Error: Cannot start recording now (Busy/Sequence)."));
        }
        return; // Command handled
    } else if (strcmp(cmd, "PLAY") == 0) {
        if (currentState == IDLE && !isRunningSequence) {
            if (recordedStepsCount > 0) {
                currentState = PLAYBACK;
                playbackIndex = 0;
                playbackStartTime = millis();
                Serial.println(F("Playback started..."));
            } else {
                Serial.println(F("Error: Nothing recorded to play."));
            }
        } else {
             Serial.println(F("Error: Cannot start playback now (Busy/Sequence)."));
        }
         return; // Command handled
    } else if (strcmp(cmd, "STOP") == 0) {
        bool stoppedSomething = false;
        if (currentState == RECORDING) {
            stopRecording(); // stopRecording changes state to IDLE
            stoppedSomething = true;
        }
        if (currentState == PLAYBACK) { // Use 'if', not 'else if', in case stopRecording was just called
            currentState = IDLE;
            Serial.println(F("\nPlayback stopped by command."));
            stoppedSomething = true;
        }
        if (isRunningSequence) { // Use 'if', check independently
            isRunningSequence = false; // Stop sequence execution
            currentState = IDLE;      // Ensure state is IDLE
            Serial.println(F("\nSequence stopped by command."));
            stoppedSomething = true;
        }

        if (!stoppedSomething && currentState == IDLE) { // Only print if nothing was actually stopped
             Serial.println(F("Status: IDLE (Nothing to stop)."));
        }
        return; // Command handled
    } else if (strcmp(cmd, "SEQUENCE") == 0) {
        if (currentState == IDLE && !isRunningSequence) {
            isRunningSequence = true;
            currentSequenceStateIndex = 0; // Target the first state
            sequenceStateStartTime = millis();
            // Record starting positions for interpolation
            for (int i = 0; i < SERVO_NUM; i++) {
                sequenceStartPos[i] = myServos[i].read(); // Read current angle
            }
            Serial.println(F("Sequence started..."));
        } else {
             Serial.println(F("Error: Cannot start sequence now (Busy)."));
        }
        return; // Command handled
    }

    // --- Check for Movement Command Format {Sx:y...} ---
    if (cmd[0] == '{' && cmd[cmdLen - 1] == '}') {
        // Command has the potential { } structure for movement.
        // Now, check if the state *allows* direct servo control commands.
        if (currentState == IDLE || currentState == RECORDING) {
            // Process the segments. processSegmentC handles recording IF currentState is RECORDING.
            cmd[cmdLen - 1] = '\0'; // Null-terminate before '}'
            char* content = cmd + 1; // Point after '{'
            trimWhitespace(content);
            if (content[0] == '\0') {
                Serial.println(F("Received empty command: {}"));
                return; // Handled (empty command)
            }

            Serial.print(F("Processing: {"));
            char* segment; char* saveptr; bool firstSegment = true;
            for (segment = strtok_r(content, ",", &saveptr); segment != NULL; segment = strtok_r(NULL, ",", &saveptr))
            {
                trimWhitespace(segment);
                if (segment[0] == '\0') continue; // Skip empty segments like {S1:90,,S2:80}
                processSegmentC(segment, firstSegment); // This function moves the servo AND records if needed
            }
            Serial.println(F("}"));

        } else {
            // Current state is PLAYBACK or SEQUENCE - cannot accept direct movement commands.
            Serial.println(F("Info: Movement command ignored while PLAYBACK or SEQUENCE running. Use STOP first."));
        }
        return; // Command handled (either processed or intentionally ignored based on state)
    }

    // --- If the command wasn't a known control word and didn't match {Sx:y...} format ---
    Serial.print(F("Error: Unknown command or invalid format '")); Serial.print(cmd);
    Serial.println(F("'. Expected {Sx:y,...} or control word."));
}

// --- Arduino Setup Function ---
void setup() {
  Serial.begin(57600);
  unsigned long setupStartTime = millis();
  while (!Serial && (millis() - setupStartTime < 5000)) { delay(10); }

  Serial.println(F("\n--- Multi Servo Control: C-String + Record/Play/Sequence ---"));
  Serial.print(MAX_RECORDED_STEPS); Serial.println(F(" steps max recording."));
  Serial.println(F("Commands: {Sx:y,...} | RECORD | PLAY | STOP | SEQUENCE"));
  Serial.println(F("--------------------------------------------------------"));

  byte pin; // Temporary variable to hold pin from PROGMEM
  for (int i = 0; i < SERVO_NUM; i++) {
    pin = pgm_read_byte(&SERVO_PIN[i]); // Read pin from PROGMEM
    myServos[i].attach(pin);
    // Start servos at midpoint
    myServos[i].write( (pgm_read_byte(&SERVO_MIN[i]) + pgm_read_byte(&SERVO_MAX[i])) / 2 );
    delay(20);
    Serial.print(F("Servo ")); Serial.print(i);
    Serial.print(F(" attached to pin ")); Serial.print(pin);
    Serial.print(F(" | Limits: [")); Serial.print(pgm_read_byte(&SERVO_MIN[i]));
    Serial.print(F(", ")); Serial.print(pgm_read_byte(&SERVO_MAX[i])); Serial.println(F("]"));
  }
  Serial.println(F("--------------------------------------------------------"));
  Serial.println(F("Initialization Complete. State: IDLE"));
}

// --- Arduino Main Loop ---
void loop() {

  // === Sequence Execution Logic ===
  if (isRunningSequence) {
      unsigned long now = millis();
      unsigned long elapsed = now - sequenceStateStartTime;

      if (elapsed >= SEQUENCE_INTERPOLATION_TIME) {
          // Time's up for this step, ensure final position is reached
          byte targetAngle;
          for (int i = 0; i < SERVO_NUM; i++) {
              targetAngle = pgm_read_byte(&sequenceData[currentSequenceStateIndex][i]);
              // Use constrain just in case, read limits from PROGMEM
              targetAngle = constrain(targetAngle, pgm_read_byte(&SERVO_MIN[i]), pgm_read_byte(&SERVO_MAX[i]));
              myServos[i].write(targetAngle);
              sequenceStartPos[i] = targetAngle; // Update start pos for next potential step
          }

          // Move to the next state
          currentSequenceStateIndex++;

          // Check if sequence is complete
          if (currentSequenceStateIndex >= NUM_SEQUENCE_STATES) {
              isRunningSequence = false;
              currentState = IDLE; // Ensure state is IDLE
              Serial.println(F("\nSequence finished."));
          } else {
              // Reset timer for the next interpolation step
              sequenceStateStartTime = now; // Use current time to avoid drift
              // No need to update start positions here again, done above after write
               Serial.print(F("Sequence step ")); Serial.print(currentSequenceStateIndex-1); Serial.println(F(" complete. Starting next."));
          }
      } else {
          // --- Interpolate ---
          float fraction = (float)elapsed / SEQUENCE_INTERPOLATION_TIME;
          byte targetAngle;
          int startAngle;
          int interpolatedAngle;

          for (int i = 0; i < SERVO_NUM; i++) {
              // Read target angle for the current destination state from PROGMEM
              targetAngle = pgm_read_byte(&sequenceData[currentSequenceStateIndex][i]);
              startAngle = sequenceStartPos[i];

              // Calculate interpolated angle
              interpolatedAngle = round(startAngle + (targetAngle - startAngle) * fraction);

              // Constrain (safety, though shouldn't be needed if sequenceData is valid)
              interpolatedAngle = constrain(interpolatedAngle, pgm_read_byte(&SERVO_MIN[i]), pgm_read_byte(&SERVO_MAX[i]));

              // Write interpolated position
              // Check if different from last write might save some servo jitter/power? Optional.
              // if (myServos[i].read() != interpolatedAngle) { // read() might be slow
                  myServos[i].write(interpolatedAngle);
              // }
          }
      }
  } // End Sequence Logic

  // === Playback Logic ===
  else if (currentState == PLAYBACK) { // Only run if not running sequence
    if (playbackIndex < recordedStepsCount) {
      RecordedStep& nextStep = recordedSequence[playbackIndex];
      unsigned long targetTime = playbackStartTime + nextStep.timeOffsetMs;
      if (millis() >= targetTime) {
        myServos[nextStep.servoIndex].write(nextStep.servoAngle);
        playbackIndex++;
      }
    } else {
      currentState = IDLE;
      Serial.println(F("\nPlayback finished."));
    }
  } // End Playback Logic

  // === Recording Check (Memory Limit handled in processSegmentC) ===
  // Optional: Timeout check could be re-added here if desired
  // if (currentState == RECORDING && millis() - recordingStartTime >= RECORDING_TIMEOUT_MS) { ... }


  // === Command Processing Logic (Reading from Serial) ===
  while (Serial.available() > 0 && commandInputIndex < MAX_COMMAND_LEN - 1) {
      char receivedChar = Serial.read();
      if (receivedChar == '\n' || receivedChar == '\r') {
          if (commandInputIndex > 0) {
              commandInputBuffer[commandInputIndex] = '\0';
              processCommand(commandInputBuffer);
          }
          commandInputIndex = 0;
          break;
      } else if (isprint(receivedChar)) {
          commandInputBuffer[commandInputIndex++] = receivedChar;
      }
  }
  // Buffer overflow check
  if (commandInputIndex >= MAX_COMMAND_LEN - 1) {
      Serial.println(F("Error: Command buffer overflow. Command discarded."));
      commandInputIndex = 0;
      while(Serial.available() > 0 && Serial.read() != '\n'); // Clear rest of serial line
  }

} // End main loop