#include <Servo.h>
#include <EEPROM.h>

Servo servo1;
Servo servo2;

int potPin1 = A0;
int potPin2 = A1;
int recordButtonPin = 2;
int playbackButtonPin = 3; // Neuer Button für die Wiedergabe
int ledAufnahme = 6; 
int ledWiedergabe = 7;
int framerate = 40; // Delay-Wert, je höher, desto geringer die Auflösung

int potValue1 = 0;
int potValue2 = 0;
int servoPos1 = 0;
int servoPos2 = 0;

bool recording = false;
bool playback = false;

int eepromAddress = 0;

void setup() {
  Serial.begin(9600);
  servo1.attach(9);
  servo2.attach(10);

  pinMode(recordButtonPin, INPUT);
  pinMode(playbackButtonPin, INPUT); // Neuer Button als Eingang
  pinMode(ledAufnahme, OUTPUT);
  pinMode(ledWiedergabe, OUTPUT);
}

void loop() {
  if (digitalRead(recordButtonPin) == HIGH) {
    if (!recording && !playback) {
      recording = true;
      digitalWrite(ledAufnahme, HIGH);
      eraseEEPROM();
    } else if (recording) {
      recording = false;
      digitalWrite(ledAufnahme, LOW);
    } else if (playback) {
      playback = false;
    }
    delay(200);
  }

  if (digitalRead(playbackButtonPin) == HIGH && !recording) {
    playback = true;
    digitalWrite(ledWiedergabe,HIGH);
    delay(200);
  }

  if (recording) {
    potValue1 = analogRead(potPin1);
    potValue2 = analogRead(potPin2);
    servoPos1 = map(potValue1, 0, 1023, 0, 180);
    servoPos2 = map(potValue2, 0, 1023, 0, 180);
    servo1.write(servoPos1);
    servo2.write(servoPos2);
    savePositionToEEPROM(servoPos1, servoPos2);
    delay(framerate);    

  } else if (playback) {
    playbackSequence();
  }
}

void savePositionToEEPROM(int pos1, int pos2) {
  if (eepromAddress < 1020){
    EEPROM.write(eepromAddress, pos1);
    eepromAddress++;
    EEPROM.write(eepromAddress, pos2);
    eepromAddress++;  
  } else {
    recording = false;
    digitalWrite(ledAufnahme, LOW);
  }
}

void playbackSequence() {
  int numSteps = eepromAddress / 2;
  for (int step = 0; step < numSteps; step++) {
    int storedPos1 = EEPROM.read(step * 2);
    int storedPos2 = EEPROM.read(step * 2 + 1);
    servo1.write(storedPos1);
    servo2.write(storedPos2);
    delay(framerate);
  }
  playback = false;
  digitalWrite(ledWiedergabe,LOW);
}

void eraseEEPROM() {
  for (int i = 0; i < EEPROM.length(); i++) {
    EEPROM.write(i, 0);
  }
  eepromAddress = 0;
}