#include <Arduino.h>
#include <ESP32Servo.h>
#include <FastLED.h>
#include <FS.h>
#include <SD.h>
#include <SPI.h>

// ===== Pins =====
#define PAN_A_PIN 3
#define TILT_A_PIN 4
#define PAN_B_PIN 5
#define TILT_B_PIN 6

#define JOY1_X    1
#define JOY1_Y    2
#define JOY2_X    7
#define JOY2_Y    8

#define BTN_COLOR_A 9    // cycle Head A colour
#define BTN_COLOR_B 10   // cycle Head B colour
#define BTN_REC     11   // record start/stop
#define BTN_PLAY    12   // play start/stop

#define LED_PIN   21
#define NUM_LEDS  2
#define SD_CS     13

Servo panA, tiltA, panB, tiltB;
CRGB leds[NUM_LEDS];

// ===== Colours =====
#define NUM_COLORS 15
CRGB presetColors[NUM_COLORS] = {
  CRGB::Red, CRGB::Green, CRGB::Blue,
  CRGB::Yellow, CRGB::Purple, CRGB::Cyan,
  CRGB::Orange, CRGB::Pink, CRGB::White,
  CRGB(255,128,0),  CRGB(0,255,128), CRGB(128,0,255),
  CRGB(255,0,128),  CRGB(128,255,0), CRGB(200,200,200)
};
int currentColorIndexA = 0;
int currentColorIndexB = 0;

// ===== Recorder =====
#define MAX_STEPS 1000
struct Step {
  int panA, tiltA, colorA;
  int panB, tiltB, colorB;
  unsigned long t;
};
Step sequence[MAX_STEPS];
int stepCount = 0;

bool recording = false;
bool playing = false;
unsigned long recordStart = 0;
unsigned long playStart = 0;
int playIndex = 0;

String currentShow = "/show1.txt";

// ===== File Save/Load =====
void saveSequence(String fname) {
  if (!fname.startsWith("/")) fname = "/" + fname;
  if (!fname.endsWith(".txt")) fname += ".txt";
  File file = SD.open(fname, FILE_WRITE);
  if (!file) { Serial.println("❌ Save failed"); return; }
  for (int i=0; i<stepCount; i++) {
    file.printf("%d,%d,%d,%d,%d,%d,%lu\n",
      sequence[i].panA, sequence[i].tiltA, sequence[i].colorA,
      sequence[i].panB, sequence[i].tiltB, sequence[i].colorB,
      sequence[i].t);
  }
  file.close();
  Serial.printf("💾 Saved %d steps to %s\n", stepCount, fname.c_str());
  currentShow = fname;
}

void loadSequence(String fname) {
  if (!fname.startsWith("/")) fname = "/" + fname;
  if (!fname.endsWith(".txt")) fname += ".txt";
  File file = SD.open(fname);
  if (!file) { Serial.println("❌ Load failed"); return; }
  stepCount = 0;
  while (file.available() && stepCount < MAX_STEPS) {
    String line = file.readStringUntil('\n');
    int v[7]; int idx=0; int last=0;
    for (int i=0; i<6; i++) {
      int pos=line.indexOf(',',last);
      if (pos<0) break;
      v[idx++]=line.substring(last,pos).toInt();
      last=pos+1;
    }
    v[idx++]=line.substring(last).toInt();
    if (idx==7) {
      sequence[stepCount].panA=v[0]; sequence[stepCount].tiltA=v[1]; sequence[stepCount].colorA=v[2];
      sequence[stepCount].panB=v[3]; sequence[stepCount].tiltB=v[4]; sequence[stepCount].colorB=v[5];
      sequence[stepCount].t=v[6];
      stepCount++;
    }
  }
  file.close();
  Serial.printf("📂 Loaded %d steps from %s\n", stepCount, fname.c_str());
  currentShow = fname;
}

// ===== Setup =====
void setup() {
  Serial.begin(115200);
  pinMode(BTN_COLOR_A, INPUT_PULLUP);
  pinMode(BTN_COLOR_B, INPUT_PULLUP);
  pinMode(BTN_REC, INPUT_PULLUP);
  pinMode(BTN_PLAY, INPUT_PULLUP);

  panA.attach(PAN_A_PIN); tiltA.attach(TILT_A_PIN);
  panB.attach(PAN_B_PIN); tiltB.attach(TILT_B_PIN);

  FastLED.addLeds<WS2812B, LED_PIN, GRB>(leds, NUM_LEDS);
  FastLED.clear(); FastLED.show();

  if (!SD.begin(SD_CS)) Serial.println("⚠️ SD not found, running RAM-only");
  else Serial.println("✅ SD ready");

  Serial.println("🎭 Dual Moving Head Controller with Record/Playback");
}

// ===== Loop =====
void loop() {
  // --- Live joystick control ---
  int panValA  = map(analogRead(JOY1_X),0,4095,0,180);
  int tiltValA = map(analogRead(JOY1_Y),0,4095,0,180);
  int panValB  = map(analogRead(JOY2_X),0,4095,0,180);
  int tiltValB = map(analogRead(JOY2_Y),0,4095,0,180);

  if (!playing) { // during playback, ignore live input
    panA.write(panValA); tiltA.write(tiltValA);
    panB.write(panValB); tiltB.write(tiltValB);
  }

  // --- Colour buttons ---
  static bool lastA=HIGH,lastB=HIGH;
  bool btnA=digitalRead(BTN_COLOR_A), btnB=digitalRead(BTN_COLOR_B);

  if (lastA==HIGH && btnA==LOW) {
    currentColorIndexA=(currentColorIndexA+1)%NUM_COLORS;
    Serial.printf("Head A colour → %d\n",currentColorIndexA);
  }
  if (lastB==HIGH && btnB==LOW) {
    currentColorIndexB=(currentColorIndexB+1)%NUM_COLORS;
    Serial.printf("Head B colour → %d\n",currentColorIndexB);
  }
  lastA=btnA; lastB=btnB;

  // Apply current colours
  leds[0]=presetColors[currentColorIndexA];
  leds[1]=presetColors[currentColorIndexB];
  FastLED.show();

  // --- Record toggle ---
  static bool lastRec=HIGH;
  bool recBtn=digitalRead(BTN_REC);
  if (lastRec==HIGH && recBtn==LOW) {
    if (!recording) {
      stepCount=0; recording=true; recordStart=millis();
      Serial.println("🔴 Recording started");
    } else {
      recording=false; Serial.printf("⏹️ Recording stopped (%d steps)\n",stepCount);
      saveSequence(currentShow);
    }
  }
  lastRec=recBtn;

  // --- Playback toggle ---
  static bool lastPlay=HIGH;
  bool playBtn=digitalRead(BTN_PLAY);
  if (lastPlay==HIGH && playBtn==LOW) {
    if (!playing && stepCount>0) {
      playing=true; playIndex=0; playStart=millis();
      Serial.println("▶️ Playback started");
    } else {
      playing=false; Serial.println("⏹️ Playback stopped");
    }
  }
  lastPlay=playBtn;

  // --- Recording logic ---
  if (recording) {
    unsigned long now=millis()-recordStart;
    if (stepCount==0 || now-sequence[stepCount-1].t>50) {
      sequence[stepCount]={panValA,tiltValA,currentColorIndexA,
                           panValB,tiltValB,currentColorIndexB,now};
      stepCount++;
      if (stepCount>=MAX_STEPS) recording=false;
    }
  }

  // --- Playback logic ---
  if (playing && playIndex<stepCount) {
    unsigned long now=millis()-playStart;
    if (now>=sequence[playIndex].t) {
      panA.write(sequence[playIndex].panA);
      tiltA.write(sequence[playIndex].tiltA);
      panB.write(sequence[playIndex].panB);
      tiltB.write(sequence[playIndex].tiltB);
      leds[0]=presetColors[sequence[playIndex].colorA];
      leds[1]=presetColors[sequence[playIndex].colorB];
      FastLED.show();
      playIndex++;
    }
  }
}