// CREATED AND DESIGNED BY MARVIN QUIZZAGAN, DATE: DECEMBER 7, 2025
// ARDUINO NANO_A - FINAL VERSION WITH RESET ON MODE CHANGE
// D13 HIGH = ENABLE, D13 LOW = DISABLE ALL FUNCTIONS (motors + LED)
// FULL MODE: motors + LED
// LED-ONLY MODE: motors paused, LED fades
// A0 short press: cycle speed levels (1–6)
// A0 long press (>=1s): toggle FULL/LED-only mode, reset speed level to 1, LED-off-first blink
// NEW: At power-on, ears are DEACTIVATED. Long press A0 will activate ears.

#include <Arduino.h>

// ---------- PINS ----------
const int IN1 = A1, IN2 = A2, IN3 = A3, IN4 = A4;
const int IN5 = 5, IN6 = 6, IN7 = 7, IN8 = 8;
const int SPEED_BTN_PIN = A0; // button with pull-up
const int LED_PWM_PIN = 3;    // PWM LED output
const int ENABLE_PIN = 13;    // HIGH = enable, LOW = disable (INPUT_PULLUP)

// ---------- EARS ----------
const uint8_t seq[8][4] = {
  {1,0,0,0},{1,1,0,0},{0,1,0,0},{0,1,1,0},
  {0,0,1,0},{0,0,1,1},{0,0,0,1},{1,0,0,1}
};
const int stepsPerRevolution = 2048;
volatile int angleDegrees = 45;
volatile unsigned long centerDwellMicros = 50000;
unsigned int earStepDelay[6] = {2500, 1500, 1200, 600, 0, 600};

int stepsPerSweep = 0, pos = 0, dir = 1;
unsigned long lastStepMicros = 0;
bool paused = false;

// ---------- LED ----------
const float GAMMA = 1.7f;
unsigned long ledFadePeriod[6] = {6000, 1700, 1000, 500, 1, 1};
unsigned long ledFadeStart = 0;    // tracks when fade started for smooth resume
float ledFadeProgress = 0.0;       // fractional progress saved for seamless resume

// ---------- STATE ----------
int speedIndex = 0;         // 0..5, for full mode
int ledSpeedIndex = 0;      // 0..5, for LED-only mode
bool earsEnabled = false;   // default OFF at power-on
bool lastBtnState = HIGH;
unsigned long lastDebounceTime = 0;
const unsigned long debounceDelay = 50;
bool wasPressed = false;
bool prevEnabledState = true;

// long-press detection
const unsigned long longPressTime = 1000; // 1s
unsigned long btnPressStart = 0;
bool longPressDetected = false;

// ---------- HELPERS ----------
void clearAllPins() {
  digitalWrite(IN1, LOW); digitalWrite(IN2, LOW); digitalWrite(IN3, LOW); digitalWrite(IN4, LOW);
  digitalWrite(IN5, LOW); digitalWrite(IN6, LOW); digitalWrite(IN7, LOW); digitalWrite(IN8, LOW);
}

inline int posToIndex(int p) { return ((p % 8) + 8) % 8; }

void applyBothByPos(int position) {
  int idx1 = posToIndex(position);
  int idx2 = posToIndex(-position);
  digitalWrite(IN1, seq[idx1][0]); digitalWrite(IN2, seq[idx1][1]);
  digitalWrite(IN3, seq[idx1][2]); digitalWrite(IN4, seq[idx1][3]);
  digitalWrite(IN5, seq[idx2][0]); digitalWrite(IN6, seq[idx2][1]);
  digitalWrite(IN7, seq[idx2][2]); digitalWrite(IN8, seq[idx2][3]);
}

int gammaMap(float v) {
  if (v <= 0.0f) return 0;
  if (v >= 1.0f) return 255;
  return (int)(powf(v, GAMMA) * 255.0f + 0.5f);
}

void computeStepsPerSweep() {
  stepsPerSweep = max(1, (int)round((stepsPerRevolution * (double)angleDegrees) / 360.0));
  if (pos > stepsPerSweep) pos = stepsPerSweep;
  if (pos < -stepsPerSweep) pos = -stepsPerSweep;
}

void printStatus(bool enabledByD13) {
  if (!enabledByD13) {
    Serial.println("STATUS: DISABLED BY D13 (LOW) - A0 IGNORED");
    return;
  }
  Serial.print("Mode: "); Serial.print(earsEnabled ? "FULL" : "LED-ONLY");
  Serial.print(" | ");
  int level = earsEnabled ? speedIndex : ledSpeedIndex;
  Serial.print("Speed Level: "); Serial.print(level+1);
  Serial.print(" | ");
  if (earsEnabled) {
    if (speedIndex == 4) Serial.print("EARS: DISABLED | LED: OFF");
    else if (speedIndex == 5) Serial.print("EARS: MAX | LED: FULL ON");
    else Serial.print("EARS: TUNED | LED: TUNED");
  } else {
    if (level == 4) Serial.print("LED: OFF | EARS: DISABLED");
    else if (level == 5) Serial.print("LED: FULL ON | EARS: DISABLED");
    else Serial.print("LED: TUNED | EARS: DISABLED");
  }
  Serial.println();
}

// ---------- LED BLINK FEEDBACK ON LONG PRESS ----------
void blinkFeedback() {
  analogWrite(LED_PWM_PIN, 0); // ensure LED starts OFF
  delay(50);
  for (int i = 0; i < 2; i++) {
    analogWrite(LED_PWM_PIN, 255);
    delay(150);
    analogWrite(LED_PWM_PIN, 0);
    delay(150);
  }
  ledFadeStart = millis(); // resume fade after blink
}

// ---------- SETUP ----------
void setup() {
  pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT); pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT);
  pinMode(IN5, OUTPUT); pinMode(IN6, OUTPUT); pinMode(IN7, OUTPUT); pinMode(IN8, OUTPUT);
  clearAllPins();

  pinMode(LED_PWM_PIN, OUTPUT);
  pinMode(SPEED_BTN_PIN, INPUT_PULLUP);
  pinMode(ENABLE_PIN, INPUT_PULLUP);

  analogWrite(LED_PWM_PIN, 0);

  Serial.begin(115200);
  computeStepsPerSweep();
  lastStepMicros = micros();
  lastBtnState = digitalRead(SPEED_BTN_PIN);
  prevEnabledState = (digitalRead(ENABLE_PIN) == HIGH);
  Serial.println("=== EARS + LED MANUAL TUNE START ===");

  ledFadeStart = millis();
}

// ---------- LOOP ----------
void loop() {
  bool enabledByD13 = (digitalRead(ENABLE_PIN) == HIGH);

  if (enabledByD13 && !prevEnabledState) {
    wasPressed = false;
    lastDebounceTime = millis();
    lastBtnState = digitalRead(SPEED_BTN_PIN);
  }
  if (!enabledByD13 && prevEnabledState) {
    wasPressed = false;
    lastBtnState = digitalRead(SPEED_BTN_PIN);
  }
  prevEnabledState = enabledByD13;

  if (!enabledByD13) {
    paused = true;
    clearAllPins();
    analogWrite(LED_PWM_PIN, 0);
  }

  // --- READ SPEED BUTTON ---
  if (enabledByD13) {
    int reading = digitalRead(SPEED_BTN_PIN);
    unsigned long now = millis();

    if (reading != lastBtnState) lastDebounceTime = now;

    if ((now - lastDebounceTime) > debounceDelay) {
      if (reading == LOW && !wasPressed) {
        btnPressStart = now;
        longPressDetected = false;
        wasPressed = true;
      }

      if (reading == LOW && wasPressed && !longPressDetected && (now - btnPressStart >= longPressTime)) {
        earsEnabled = !earsEnabled;
        longPressDetected = true;

        // Reset speed levels
        if (earsEnabled) speedIndex = 0;
        else ledSpeedIndex = 0;

        Serial.print("Long press detected -> toggled mode to ");
        Serial.println(earsEnabled ? "FULL" : "LED-ONLY");

        // LED flash twice from OFF
        blinkFeedback();
      }

      if (reading == HIGH && wasPressed) {
        if (!longPressDetected) {
          if (earsEnabled) {
            speedIndex = (speedIndex + 1) % 6;
            switch(speedIndex) {
              case 0: angleDegrees = 45; break;
              case 1: angleDegrees = 45; break;
              case 2: angleDegrees = 45; break;
              case 3: angleDegrees = 20; break;
              case 4: angleDegrees = 0; break;
              case 5: angleDegrees = 45; break;
            }
            computeStepsPerSweep();
          } else {
            ledSpeedIndex = (ledSpeedIndex + 1) % 6;
          }
          Serial.print("Short press -> ");
          Serial.print(earsEnabled ? "FULL mode" : "LED-only mode");
          Serial.print(" new level: ");
          Serial.println(earsEnabled ? speedIndex+1 : ledSpeedIndex+1);
        }
        wasPressed = false;
      }
    }
    lastBtnState = reading;
  }

  // --- HANDLE EARS ---
  if (enabledByD13 && earsEnabled) {
    if (speedIndex == 4) { paused = true; clearAllPins(); }
    else {
      paused = false;
      unsigned long stepDelay = earStepDelay[speedIndex];
      if (!paused && stepDelay > 0) {
        unsigned long t = micros();
        if (t - lastStepMicros >= stepDelay) {
          lastStepMicros = t;
          if ((pos==0 && dir==1) || (pos==0 && dir==-1)) delayMicroseconds(centerDwellMicros);
          pos += dir;
          if (pos >= stepsPerSweep) { pos = stepsPerSweep; dir = -1; }
          else if (pos <= -stepsPerSweep) { pos = -stepsPerSweep; dir = 1; }
          applyBothByPos(pos);
        }
      }
    }
  } else if (enabledByD13 && !earsEnabled) {
    paused = true;
    clearAllPins();
  }

  // --- HANDLE LED ---
  if (enabledByD13) {
    int ledLevel = earsEnabled ? speedIndex : ledSpeedIndex;
    if (ledLevel == 4) analogWrite(LED_PWM_PIN, 0);
    else if (ledLevel == 5) analogWrite(LED_PWM_PIN, 255);
    else {
      unsigned long periodMs = ledFadePeriod[ledLevel];
      if (periodMs <= 1) analogWrite(LED_PWM_PIN, 255);
      else {
        unsigned long t2 = (millis() - ledFadeStart) % periodMs;
        float posVal = t2 < (periodMs/2) ? (float)t2/(periodMs/2) : (float)(periodMs-t2)/(periodMs/2);
        analogWrite(LED_PWM_PIN, gammaMap(posVal));
      }
    }
  }

  // --- STATUS PRINT ---
  static unsigned long lastPrint = 0;
  if (millis() - lastPrint >= 500) {
    lastPrint = millis();
    printStatus(enabledByD13);
  }

  delay(1);
}