#include <SD.h>

// Pin Definitions
#define CW_BUTTON_PIN 2
#define CCW_BUTTON_PIN 3
#define RECORD_BUTTON_PIN 4
#define PLAYBACK_BUTTON_PIN 5
#define POTENTIOMETER_PIN A3
#define PWM_PIN 6
#define DIRECTION_PIN 7
#define RED_LED_PIN A0
#define GREEN_LED_PIN A1
#define YELLOW_LED_PIN A2
#define SD_CS_PIN 10 // SD Card Chip Select pin

// Global Variables
bool isRecording = false;
bool isPlayback = false;
unsigned long startTime = 0;
unsigned long lastRecordTime = 0;
int lastSpeed = -1;
bool lastDirection = false;
File currentFile;
int lineCount = 0;

// Playback Data Arrays
int timeArray[50];
int speedArray[50];
bool directionArray[50];

void setup() {
  Serial.begin(9600);
  pinMode(CW_BUTTON_PIN, INPUT_PULLUP);
  pinMode(CCW_BUTTON_PIN, INPUT_PULLUP);
  pinMode(RECORD_BUTTON_PIN, INPUT_PULLUP);
  pinMode(PLAYBACK_BUTTON_PIN, INPUT_PULLUP);
  pinMode(PWM_PIN, OUTPUT);
  pinMode(DIRECTION_PIN, OUTPUT);
  pinMode(RED_LED_PIN, OUTPUT);
  pinMode(GREEN_LED_PIN, OUTPUT);
  pinMode(YELLOW_LED_PIN, OUTPUT);

  // Initialize SD Card
  if (!SD.begin(SD_CS_PIN)) {
    digitalWrite(RED_LED_PIN, HIGH); // SD card not detected
    while (true); // Halt execution
  }

  digitalWrite(RED_LED_PIN, LOW); // SD card detected
}

void loop() {
  controlMotor();

  if (digitalRead(RECORD_BUTTON_PIN) == LOW) {
    Serial.println("Start Recording");
    handleRecording();
  }

  if (digitalRead(PLAYBACK_BUTTON_PIN) == LOW) {
    Serial.println("PlayBack");
    handlePlayback();
  }
}

void controlMotor() {
  // Manual motor control
  if (digitalRead(CW_BUTTON_PIN) == LOW) {
    digitalWrite(DIRECTION_PIN, HIGH);
  } else if (digitalRead(CCW_BUTTON_PIN) == LOW) {
    digitalWrite(DIRECTION_PIN, LOW);
  }

  int speed = analogRead(POTENTIOMETER_PIN);
  analogWrite(PWM_PIN, map(speed, 0, 1023, 0, 255));
}

void handleRecording() {
  if (!isRecording) {
    // Start recording
    isRecording = true;
    digitalWrite(YELLOW_LED_PIN, HIGH);
    startTime = millis();
    lastRecordTime = 0;
    lastSpeed = -1;
    lastDirection = false;
    lineCount = 0;

    currentFile = SD.open("recording.txt", FILE_WRITE);
    if (!currentFile) {
      digitalWrite(YELLOW_LED_PIN, LOW);
      isRecording = false;
      return;
    }
  } else {
    // Stop recording
    isRecording = false;
    digitalWrite(YELLOW_LED_PIN, LOW);
    Serial.println("Stop Recording");
    currentFile.close();
    return;
  }

  unsigned long elapsedTime = millis() - startTime;
  int currentSpeed = analogRead(POTENTIOMETER_PIN);
  int pwmValue = map(currentSpeed, 0, 1023, 0, 255);
  bool currentDirection = digitalRead(DIRECTION_PIN);

  // Record only if values change
  if (pwmValue != lastSpeed || currentDirection != lastDirection) {
    currentFile.print(elapsedTime);
    currentFile.print(",");
    currentFile.print(currentDirection);
    currentFile.print(",");
    currentFile.println(pwmValue);
    lineCount++;

    lastSpeed = pwmValue;
    lastDirection = currentDirection;
  }
}

void handlePlayback() {
  if (!isPlayback) {
    // Start playback
    isPlayback = true;
    digitalWrite(GREEN_LED_PIN, HIGH);

    currentFile = SD.open("recording.txt", FILE_READ);
    if (!currentFile) {
      digitalWrite(GREEN_LED_PIN, LOW);
      isPlayback = false;
      return;
    }

    lineCount = 0;

    // Read data into arrays
    while (currentFile.available() && lineCount < 500) {
      String line = currentFile.readStringUntil('\n');
      int commaIndex1 = line.indexOf(',');
      int commaIndex2 = line.lastIndexOf(',');

      timeArray[lineCount] = line.substring(0, commaIndex1).toInt();
      directionArray[lineCount] = line.substring(commaIndex1 + 1, commaIndex2).toInt();
      speedArray[lineCount] = line.substring(commaIndex2 + 1).toInt();
      lineCount++;
    }
    currentFile.close();
  }

  unsigned long playbackStart = millis();
  for (int i = 0; i < lineCount; i++) {
    unsigned long elapsedTime = millis() - playbackStart;

    // Wait until the recorded time for this step
    while (elapsedTime < timeArray[i]) {
      elapsedTime = millis() - playbackStart;

      // Check for manual override
      if (digitalRead(CW_BUTTON_PIN) == LOW || digitalRead(CCW_BUTTON_PIN) == LOW || analogRead(POTENTIOMETER_PIN) > 10) {
        digitalWrite(GREEN_LED_PIN, LOW);
        isPlayback = false;
        return;
      }
    }

    // Set motor state
    digitalWrite(DIRECTION_PIN, directionArray[i]);
    analogWrite(PWM_PIN, speedArray[i]);
  }

  // End playback
  digitalWrite(GREEN_LED_PIN, LOW);
  isPlayback = false;
}