#include <SD.h>
#include <SPI.h>

// Pin Definitions
const int motorPWM = 6;          // PWM pin for motor speed control
const int motorDir1 = 7;         // Direction pin 1 for motor
const int motorDir2 = 7;         // Direction pin 2 for motor
const int potPin = A3;           // Potentiometer for speed control
const int recordButton = 4;      // Button to start/stop recording
const int playbackButton = 5;    // Button to start playback
const int cwButton = 2;          // Button for clockwise rotation
const int ccwButton = 3;         // Button for counterclockwise rotation
const int redLED = A0;           // Red LED for SD card error
const int greenLED = A1;         // Green LED for playback mode
const int yellowLED = A2;        // Yellow LED for recording mode

// Variables
int motorSpeed = 0;              // Motor speed (0-255)
bool motorDirection = true;      // true = CW, false = CCW
bool isRecording = false;        // Recording state
bool isPlaying = false;          // Playback state
unsigned long startTime = 0;     // Timer for recording
int lineCount = 0;               // Number of lines recorded
File dataFile;                   // File object for SD card

// Data structure for recording
struct MotorData {
  unsigned long time;
  int speed;
  bool direction;
};
MotorData recordedData[100];    // Array to store recorded data (2 minutes max)

void setup() {
  // Initialize pins
  pinMode(motorPWM, OUTPUT);
  pinMode(motorDir1, OUTPUT);
  pinMode(motorDir2, OUTPUT);
  pinMode(recordButton, INPUT_PULLUP);
  pinMode(playbackButton, INPUT_PULLUP);
  pinMode(cwButton, INPUT_PULLUP);
  pinMode(ccwButton, INPUT_PULLUP);
  pinMode(redLED, OUTPUT);
  pinMode(greenLED, OUTPUT);
  pinMode(yellowLED, OUTPUT);

  // Initialize Serial for debugging
  Serial.begin(9600);

  // Initialize SD card
  if (!SD.begin(10)) {           // Assuming SD card module is connected to pin 53
    digitalWrite(redLED, HIGH);  // Turn on red LED if SD card is not detected
    Serial.println("SD card initialization failed!");
    return;
  }
  digitalWrite(redLED, LOW);     // Turn off red LED if SD card is detected
  Serial.println("SD card initialized.");
}

void loop() {
  // Read potentiometer for speed control
  motorSpeed = analogRead(potPin) / 4;  // Map 0-1023 to 0-255

  // Manual motor control
  if (!isPlaying && !isPlaying) {
    if (digitalRead(cwButton) == LOW) {
      motorDirection = true;  // Clockwise
      setMotor(motorSpeed, motorDirection);
    } else if (digitalRead(ccwButton) == LOW) {
      motorDirection = false; // Counterclockwise
      setMotor(motorSpeed, motorDirection);
    } else {
      setMotor(motorSpeed, motorDirection);  // Stop motor
    }
  }

  // Recording functionality
  if (digitalRead(recordButton) == LOW) {
    delay(200);  // Debounce
    if (!isRecording) {
      startRecording();
    } else {
      stopRecording();
    }
  }

  // Playback functionality
  if (digitalRead(playbackButton) == LOW) {
    delay(200);  // Debounce
    if (!isPlaying) {
      startPlayback();
    } else {
      stopPlayback();
    }
  }

  // Update motor during playback
  if (isPlaying) {
    playRecordedData();
  }
}

// Function to set motor speed and direction
void setMotor(int speed, bool direction) {
  analogWrite(motorPWM, speed);
  if (direction) {
    digitalWrite(motorDir1, HIGH);
    digitalWrite(motorDir2, LOW);
  } else {
    digitalWrite(motorDir1, LOW);
    digitalWrite(motorDir2, HIGH);
  }
}

// Start recording
void startRecording() {
  isRecording = true;
  isPlaying = false;
  startTime = millis();
  lineCount = 0;
  digitalWrite(yellowLED, HIGH);  // Turn on yellow LED
  digitalWrite(greenLED, LOW);    // Turn off green LED
  Serial.println("Recording started.");
}

// Stop recording
void stopRecording() {
  isRecording = false;
  digitalWrite(yellowLED, LOW);   // Turn off yellow LED
  Serial.println("Recording stopped.");

  // Save recorded data to SD card
  dataFile = SD.open("record.txt", FILE_WRITE);
  if (dataFile) {
    for (int i = 0; i < lineCount; i++) {
      dataFile.print(recordedData[i].time);
      dataFile.print(",");
      dataFile.print(recordedData[i].speed);
      dataFile.print(",");
      dataFile.println(recordedData[i].direction);
    }
    dataFile.close();
    Serial.println("Data saved to SD card.");
  } else {
    Serial.println("Error saving data to SD card!");
  }
}

// Start playback
void startPlayback() {
  isPlaying = true;
  isRecording = false;
  digitalWrite(greenLED, HIGH);   // Turn on green LED
  digitalWrite(yellowLED, LOW);   // Turn off yellow LED
  Serial.println("Playback started.");

  // Load recorded data from SD card
  dataFile = SD.open("record.txt");
  if (dataFile) {
    lineCount = 0;
    while (dataFile.available()) {
      String line = dataFile.readStringUntil('\n');
      recordedData[lineCount].time = line.substring(0, line.indexOf(',')).toInt();
      recordedData[lineCount].speed = line.substring(line.indexOf(',') + 1, line.lastIndexOf(',')).toInt();
      recordedData[lineCount].direction = line.substring(line.lastIndexOf(',') + 1).toInt();
      lineCount++;
    }
    dataFile.close();
    Serial.println("Data loaded from SD card.");
  } else {
    Serial.println("Error loading data from SD card!");
  }
}

// Stop playback
void stopPlayback() {
  isPlaying = false;
  digitalWrite(greenLED, LOW);    // Turn off green LED
  Serial.println("Playback stopped.");
}

// Play recorded data
void playRecordedData() {
  unsigned long currentTime = millis() - startTime;
  for (int i = 0; i < lineCount; i++) {
    if (recordedData[i].time <= currentTime) {
      setMotor(recordedData[i].speed, recordedData[i].direction);
    }
  }

  // Stop playback if manual control is used
  if (digitalRead(cwButton) == LOW || digitalRead(ccwButton) == LOW || analogRead(potPin) / 4 != motorSpeed) {
    stopPlayback();
  }
}