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

// ----- Servos -----
Servo servo1, servo2, servo3, servo4;
const int pot1 = A0;
const int pot2 = A1;
const int pot3 = A2;
const int pot4 = A3;
const int pinServo1 = 3;
const int pinServo2 = 5;
const int pinServo3 = 6;
const int pinServo4 = 9;

// ----- Botões -----
const int btnRecord = 7;
const int btnPlay   = 8;

// ----- LEDs -----
const int ledRecord = 10;  // vermelho
const int ledPlay   = 11;  // verde

// ----- Armazenamento -----
const int MAX_STEPS = 255;  // limitado pela EEPROM
int seq1[MAX_STEPS];
int seq2[MAX_STEPS];
int seq3[MAX_STEPS];
int seq4[MAX_STEPS];
int stepCount = 0;
bool recording = false;
bool playing = false;

// EEPROM
const int EEPROM_START = 0;

void setup() {
  Serial.begin(9600);

  pinMode(btnRecord, INPUT_PULLUP);
  pinMode(btnPlay, INPUT_PULLUP);
  pinMode(ledRecord, OUTPUT);
  pinMode(ledPlay, OUTPUT);

  servo1.attach(pinServo1);
  servo2.attach(pinServo2);
  servo3.attach(pinServo3);
  servo4.attach(pinServo4);

  // Carrega sequência da EEPROM
  Serial.println("Carregando dados da EEPROM...");
  loadFromEEPROM();
  Serial.print("Sequencia carregada: ");
  Serial.print(stepCount);
  Serial.println(" passos.");
  Serial.println("Sistema pronto!");

  digitalWrite(ledRecord, LOW);
  digitalWrite(ledPlay, LOW);
}

void loop() {
  // --- Leitura de botões ---
  if (digitalRead(btnRecord) == LOW) {
    delay(200);
    recording = !recording;
    playing = false;
    digitalWrite(ledPlay, LOW); // garante desligado

    if (recording) {
      stepCount = 0;
      Serial.println("Gravando movimentos...");
    } else {
      Serial.print("Gravacao encerrada. Passos: ");
      Serial.println(stepCount);
      saveToEEPROM();
    }
  }

  if (digitalRead(btnPlay) == LOW) {
    delay(200);
    playing = !playing;
    recording = false;
    digitalWrite(ledRecord, LOW); // garante desligado

    if (playing)
      Serial.println("Executando sequencia...");
    else
      Serial.println("Execucao parada.");
  }

  // --- Estados ---
  if (recording) {
    // pisca LED vermelho
    digitalWrite(ledRecord, millis() / 300 % 2);

    int v1 = map(analogRead(pot1), 0, 1023, 0, 180);
    int v2 = map(analogRead(pot2), 0, 1023, 0, 180);
    int v3 = map(analogRead(pot3), 0, 1023, 0, 180);
    int v4 = map(analogRead(pot4), 0, 1023, 0, 180);

    servo1.write(v1);
    servo2.write(v2);
    servo3.write(v3);
    servo4.write(v4);

    if (stepCount < MAX_STEPS) {
      seq1[stepCount] = v1;
      seq2[stepCount] = v2;
      seq3[stepCount] = v3;
      seq4[stepCount] = v4;
      stepCount++;
    }

    delay(100);
  }

  else if (playing) {
    digitalWrite(ledPlay, HIGH);
    for (int i = 0; i < stepCount; i++) {
      servo1.write(seq1[i]);
      servo2.write(seq2[i]);
      servo3.write(seq3[i]);
      servo4.write(seq4[i]);
      delay(100);
      if (digitalRead(btnPlay) == LOW) { // botão interrompe reprodução
        playing = false;
        break;
      }
    }
  }

  else {
    digitalWrite(ledRecord, LOW);
    digitalWrite(ledPlay, LOW);

    int v1 = map(analogRead(pot1), 0, 1023, 0, 180);
    int v2 = map(analogRead(pot2), 0, 1023, 0, 180);
    int v3 = map(analogRead(pot3), 0, 1023, 0, 180);
    int v4 = map(analogRead(pot4), 0, 1023, 0, 180);

    servo1.write(v1);
    servo2.write(v2);
    servo3.write(v3);
    servo4.write(v4);
    delay(15);
  }
}

// ------------------------------------------------------
// EEPROM
// ------------------------------------------------------
void saveToEEPROM() {
  EEPROM.update(EEPROM_START, stepCount);
  int addr = EEPROM_START + 1;

  for (int i = 0; i < stepCount; i++) {
    EEPROM.update(addr++, seq1[i]);
    EEPROM.update(addr++, seq2[i]);
    EEPROM.update(addr++, seq3[i]);
    EEPROM.update(addr++, seq4[i]);
  }

  Serial.println("Sequencia salva na EEPROM!");
}

void loadFromEEPROM() {
  stepCount = EEPROM.read(EEPROM_START);
  if (stepCount > MAX_STEPS) stepCount = MAX_STEPS;

  int addr = EEPROM_START + 1;
  for (int i = 0; i < stepCount; i++) {
    seq1[i] = EEPROM.read(addr++);
    seq2[i] = EEPROM.read(addr++);
    seq3[i] = EEPROM.read(addr++);
    seq4[i] = EEPROM.read(addr++);
  }
}