// CREATED AND DESIGNED BY MARVIN QUIZZAGAN, DATE: FEBRUARY 21, 2026 - 100% WORKING W/ D9 OUTPUT
// ARDUINO NANO_A - FINAL VERSION WITH RESET ON MODE CHANGE
// D13 HIGH = ENABLE, D13 LOW = DISABLE ALL FUNCTIONS (motors + LED)
// FULL MODE: motors + LED (D9 = HIGH)
// LED-ONLY MODE: motors paused, LED fades (D9 = LOW)
// 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.
//
// UPDATE (DEC 19, 2025):
// 1) ADDED D9 MODE OUTPUT INDICATOR:
//    - D13 LOW  -> D9 FORCED LOW
//    - FULL     -> D9 HIGH
//    - LED-ONLY -> D9 LOW
// 2) FULL MODE LEVEL 6:
//    - SAME RUN SPEED AS LEVEL 4 (600us STEADY)
//    - WIDER SWEEP ANGLE (TUNABLE)
//    - WITH STARTUP TORQUE BOOST (PREVENTS HUMMING/STALL)
//
// FEBRUARY 21, 2026
// NEW UPDATE:
// 3) ADDED D11 PRIORITY MIRROR BUTTON (INPUT_PULLUP) = exact same functions as A0
// 4) D11 has priority over A0 when pressed (LOW)
// 5) PRIORITY LOCK option: while D11 is held, ignore A0 completely (even if A0 was already held)

#include <Arduino.h>
#include <math.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  = A0;   // button with pull-up
const int SPEED_BTN_PIN_D11 = 11;   // NEW: priority mirror button with pull-up

const int LED_PWM_PIN   = 3;   // PWM LED output
const int ENABLE_PIN    = 13;  // HIGH = enable, LOW = disable (INPUT_PULLUP)

// NEW: D9 MODE OUTPUT INDICATOR
const int MODE_OUT_PIN  = 9;   // FULL=HIGH, LED-ONLY=LOW (forced LOW when disabled)

// ---------- PRIORITY OPTIONS ----------
const bool PRIORITY_LOCK_ENABLE = true; // true: while D11 held, ignore A0 completely

// ---------- 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;

// Base delays (your working version)
unsigned int earStepDelay[6] = {2500, 1500, 800, 600, 0, 600};

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

// ---- LEVEL 6 WIDE SWEEP TUNING ----
const int LEVEL6_WIDE_ANGLE_DEG = 75;        // <<< TUNE THIS (60..90 recommended)
const unsigned int L6_BOOST_DELAY_US = 900;  // startup torque boost (try 900..1200 if needed)
const int L6_BOOST_STEPS = 160;              // boost duration in steps (try 80..250)
int l6BoostStepsRemaining = 0;

// ---------- 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

// ---------- 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;

// ---------- NEW: D11 PRIORITY/LATCH STATE ----------
bool activeBtnIsD11     = false;
bool prevActiveBtnIsD11 = false;
bool latchedToD11       = false;   // keeps D11 active for the whole press (so release is never missed)

// ---------- 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;
}

// Reset ears to center when entering Level 6 (prevents instant stall)
void resetEarsCenter() {
  pos = 0;
  dir = 1;
  applyBothByPos(0);
  lastStepMicros = micros();
}

void printStatus(bool enabledByD13) {
  if (!enabledByD13) {
    Serial.println("STATUS: DISABLED BY D13 (LOW) - BUTTONS 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);

  if (earsEnabled) {
    Serial.print(" | ANG="); Serial.print(angleDegrees);
    if (speedIndex == 5) {
      Serial.print(" | L6BoostLeft="); Serial.print(l6BoostStepsRemaining);
    }
  }

  Serial.print(" | BTN=");
  Serial.print(activeBtnIsD11 ? "D11(PRIO)" : "A0");

  Serial.print(" | D9=");
  Serial.print(digitalRead(MODE_OUT_PIN) ? "HIGH" : "LOW");
  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
}

// Reset button state machine cleanly (prevents "carry-over" when switching A0<->D11)
void resetButtonStateMachineTo(int currentReading) {
  wasPressed = false;
  longPressDetected = false;
  btnPressStart = 0;
  lastDebounceTime = millis();
  lastBtnState = currentReading;
}

// ---------- 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_A0,  INPUT_PULLUP);
  pinMode(SPEED_BTN_PIN_D11, INPUT_PULLUP);   // NEW

  pinMode(ENABLE_PIN, INPUT_PULLUP);

  // NEW: D9 MODE OUTPUT
  pinMode(MODE_OUT_PIN, OUTPUT);
  digitalWrite(MODE_OUT_PIN, LOW); // power-on default = LED-only

  analogWrite(LED_PWM_PIN, 0);

  Serial.begin(115200);
  computeStepsPerSweep();
  lastStepMicros = micros();

  // default state uses A0
  activeBtnIsD11 = false;
  prevActiveBtnIsD11 = activeBtnIsD11;
  lastBtnState = digitalRead(SPEED_BTN_PIN_A0);

  prevEnabledState = (digitalRead(ENABLE_PIN) == HIGH);

  Serial.println("=== EARS + LED MANUAL TUNE START ===");
  ledFadeStart = millis();
}

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

  // ---------- D9 MODE OUTPUT INDICATOR ----------
  if (!enabledByD13) {
    digitalWrite(MODE_OUT_PIN, LOW);                      // forced LOW when disabled
  } else {
    digitalWrite(MODE_OUT_PIN, earsEnabled ? HIGH : LOW); // FULL=HIGH, LED-only=LOW
  }

  // detect transitions of ENABLE_PIN
  if (enabledByD13 && !prevEnabledState) {
    wasPressed = false;
    lastDebounceTime = millis();
    lastBtnState = digitalRead(SPEED_BTN_PIN_A0);
  }
  if (!enabledByD13 && prevEnabledState) {
    wasPressed = false;
    lastBtnState = digitalRead(SPEED_BTN_PIN_A0);
  }
  prevEnabledState = enabledByD13;

  if (!enabledByD13) {
    paused = true;
    clearAllPins();
    analogWrite(LED_PWM_PIN, 0);
    return; // strict: nothing else runs when disabled
  }

  // ---------- SELECT ACTIVE BUTTON (D11 PRIORITY + LATCH + OPTIONAL LOCK) ----------
  {
    bool d11Low = (digitalRead(SPEED_BTN_PIN_D11) == LOW);

    // Latch to D11 for the whole press so the release event is never missed
    if (d11Low) latchedToD11 = true;

    // Release latch only after D11 is HIGH and we are not currently in a press
    // (wasPressed becomes false after we process the release)
    if (latchedToD11 && !d11Low && !wasPressed) latchedToD11 = false;

    if (PRIORITY_LOCK_ENABLE) {
      // While D11 is held (latched), A0 is ignored completely
      activeBtnIsD11 = latchedToD11;
    } else {
      // Priority-only: D11 wins only while it is physically LOW
      activeBtnIsD11 = d11Low;
    }

    // If active source changed, reset the button logic so A0 can't "continue" an old hold
    if (activeBtnIsD11 != prevActiveBtnIsD11) {
      int newReading = digitalRead(activeBtnIsD11 ? SPEED_BTN_PIN_D11 : SPEED_BTN_PIN_A0);
      resetButtonStateMachineTo(newReading);
      prevActiveBtnIsD11 = activeBtnIsD11;
    }
  }

  // --- READ ACTIVE SPEED BUTTON (A0 OR D11) ---
  {
    int activePin = activeBtnIsD11 ? SPEED_BTN_PIN_D11 : SPEED_BTN_PIN_A0;

    int reading = digitalRead(activePin);
    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;

        // Cancel Level 6 boost on mode change
        l6BoostStepsRemaining = 0;

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

        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 = 65; break;                  // Level 3
              case 3: angleDegrees = 20; break;                  // Level 4
              case 4: angleDegrees = 0;  break;                  // Level 5 OFF
              case 5:
                angleDegrees = LEVEL6_WIDE_ANGLE_DEG;             // Level 6 WIDE
                l6BoostStepsRemaining = L6_BOOST_STEPS;           // startup boost
                resetEarsCenter();                                // start from center
                break;
            }
            computeStepsPerSweep();
          } else {
            ledSpeedIndex = (ledSpeedIndex + 1) % 6;
          }

          Serial.print("Short press (");
          Serial.print(activeBtnIsD11 ? "D11" : "A0");
          Serial.print(") -> ");
          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 (earsEnabled) {
    if (speedIndex == 4 || angleDegrees == 0) {
      paused = true;
      clearAllPins();
    } else {
      paused = false;

      unsigned long stepDelay = earStepDelay[speedIndex];

      // Level 6 startup torque boost (anti-hum)
      if (speedIndex == 5 && l6BoostStepsRemaining > 0) {
        stepDelay = L6_BOOST_DELAY_US;
      }

      if (stepDelay > 0) {
        unsigned long t = micros();
        if (t - lastStepMicros >= stepDelay) {
          lastStepMicros = t;

          if (pos == 0) delayMicroseconds(centerDwellMicros);

          pos += dir;
          if (pos >= stepsPerSweep) { pos = stepsPerSweep; dir = -1; }
          else if (pos <= -stepsPerSweep) { pos = -stepsPerSweep; dir = 1; }

          applyBothByPos(pos);

          if (speedIndex == 5 && l6BoostStepsRemaining > 0) {
            l6BoostStepsRemaining--;
          }
        }
      }
    }
  } else {
    paused = true;
    clearAllPins();
  }

  // --- HANDLE LED ---
  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 half = (float)periodMs / 2.0f;
      float posVal = (t2 < (unsigned long)half) ? (float)t2 / half : (float)(periodMs - t2) / half;
      analogWrite(LED_PWM_PIN, gammaMap(posVal));
    }
  }

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

  delay(1);
}