// ============================================================
//  INTERMISSION OBJECT — Board Test  (rev 2)
//  XIAO ESP32-S3 · Custom PCB
// ============================================================
//
//  GPIO  Label     Function
//  ----  --------  ------------------------------------------
//  1     D0/A0     I2S DIN   → MAX98357A
//  2     D1/A1     I2S LRC   → MAX98357A
//  3     D2/A2     I2S BCLK  → MAX98357A
//  4     D3        LED PWM   → MOSFET gate (Q1)
//  5     D4/SDA    I2C SDA   → TM6605 (0x2A, pullups onboard)
//  6     D5/SCL    I2C SCL   → TM6605
//  7     D8        Motor PWM → MOSFET gate (Q2)
//  8     D9/ADC1   Hall IN   → A3144 OUT  (10kΩ pullup to 3V3)
//
//  Library required for I2S: "AudioTools" by pschatzmann
//    Install via: Arduino IDE → Library Manager → search "AudioTools"
// ============================================================

#include "AudioTools.h"
#include <Wire.h>

// ── Pin definitions ──────────────────────────────────────────
#define PIN_I2S_DIN     1
#define PIN_I2S_LRC     2
#define PIN_I2S_BCLK    3
#define PIN_LED_PWM     4
#define PIN_I2C_SDA     5
#define PIN_I2C_SCL     6
#define PIN_MOTOR_PWM   7
#define PIN_HALL        8

// ── PWM ──────────────────────────────────────────────────────
#define PWM_FREQ_MOTOR  20000   // 20 kHz — inaudible motor whine
#define PWM_FREQ_LED    24989   // prime, above camera flicker
#define PWM_RES         8       // 8-bit → 0–255
#define MOTOR_MEDIUM    140     // ~55 % duty

// ── Audio ─────────────────────────────────────────────────────
#define TONE_HZ         240     // low-mid tone
#define SAMPLE_RATE     44100
#define CHANNELS        2
#define BITS            16
#define TONE_AMPLITUDE  18000   // 0–32767; louder than before

// ── TM6605 (I2C address confirmed 0x2A) ──────────────────────
#define TM6605_ADDR     0x2A

// Register map
#define REG_STATUS      0x00
#define REG_MODE        0x01
#define REG_RTP         0x02    // real-time playback level (0x00–0x7F)
#define REG_LIB         0x03    // waveform library: 0x01 = ERM, 0x06 = LRA
#define REG_WAVSEQ1     0x04    // waveform sequence slot 1
#define REG_WAVSEQ2     0x05    // slot 2 (0x00 = end of sequence)
#define REG_GO          0x0C    // write 0x01 to trigger sequence
#define REG_LRARESON    0x20    // LRA resonance frequency (auto-cal writes here)
#define REG_CTRL3       0x1A    // control register 3
#define REG_FEEDBACK    0x1C    // feedback control (ERM/LRA select bit)

// ── Hall sensor ───────────────────────────────────────────────
#define HALL_DEBOUNCE_US  5000  // 5 ms debounce in ISR

volatile bool     hallPulseFlag  = false;
volatile uint32_t hallLastUs     = 0;
unsigned long     hallLastPrint  = 0;
unsigned long     hallLastPulse  = 0;
int               hallCount      = 0;

void IRAM_ATTR hallISR() {
  uint32_t now = micros();
  if (now - hallLastUs > HALL_DEBOUNCE_US) {
    hallLastUs   = now;
    hallPulseFlag = true;
  }
}

// ── AudioTools objects ────────────────────────────────────────
I2SStream            i2s;
SineWaveGenerator<int16_t> sine;
GeneratedSoundStream<int16_t> sound(sine);
StreamCopy           copier(i2s, sound);
AudioInfo            audioInfo(SAMPLE_RATE, CHANNELS, BITS);

// ────────────────────────────────────────────────────────────
//  TM6605 helpers
// ────────────────────────────────────────────────────────────
void tm_write(uint8_t reg, uint8_t val) {
  Wire.beginTransmission(TM6605_ADDR);
  Wire.write(reg);
  Wire.write(val);
  uint8_t err = Wire.endTransmission();
  if (err) {
    Serial.printf("  [TM6605] I2C error %d on reg 0x%02X\n", err, reg);
  }
}

uint8_t tm_read(uint8_t reg) {
  Wire.beginTransmission(TM6605_ADDR);
  Wire.write(reg);
  Wire.endTransmission(false);
  Wire.requestFrom((uint8_t)TM6605_ADDR, (uint8_t)1);
  return Wire.available() ? Wire.read() : 0xFF;
}

// Configure TM6605 for LRA in RTP (real-time playback) mode
void tm_init_lra_rtp() {
  tm_write(REG_MODE, 0x00);       // out of standby
  delay(5);
  // Set LRA bit in feedback control register (bit 7)
  uint8_t fb = tm_read(REG_FEEDBACK);
  tm_write(REG_FEEDBACK, fb | 0x80);
  // Use LRA open-loop library
  tm_write(REG_LIB, 0x06);
  // Enter RTP mode
  tm_write(REG_MODE, 0x05);
  tm_write(REG_RTP, 0x00);        // silent to start
}

// Soft triangle-envelope pulse in RTP mode
void tm_soft_pulse(uint32_t durationMs) {
  uint32_t start = millis();
  while (millis() - start < durationMs) {
    float t   = (float)(millis() - start) / durationMs;  // 0→1
    float env = (t < 0.5f) ? (t * 2.0f) : (2.0f - t * 2.0f);  // triangle
    tm_write(REG_RTP, (uint8_t)(env * 60.0f));  // 60/127 = gentle
    delay(10);
  }
  tm_write(REG_RTP, 0x00);
}

// ────────────────────────────────────────────────────────────
//  setup()
// ────────────────────────────────────────────────────────────
void setup() {
  Serial.begin(115200);
  AudioToolsLogger.begin(Serial, AudioToolsLogLevel::Warning);
  delay(600);
  Serial.println("\n=== INTERMISSION OBJECT — Board Test (rev 2) ===\n");

  // ── PWM channels ──
  ledcAttach(PIN_MOTOR_PWM, PWM_FREQ_MOTOR, PWM_RES);
  ledcAttach(PIN_LED_PWM,   PWM_FREQ_LED,   PWM_RES);
  ledcWrite(PIN_MOTOR_PWM, 0);
  ledcWrite(PIN_LED_PWM,   0);

  // ── I2C — TM6605 has onboard pullups, no external R needed ──
  Wire.begin(PIN_I2C_SDA, PIN_I2C_SCL);

  // ── I2S via AudioTools ──
  auto cfg       = i2s.defaultConfig(TX_MODE);
  cfg.pin_bck    = PIN_I2S_BCLK;
  cfg.pin_ws     = PIN_I2S_LRC;
  cfg.pin_data   = PIN_I2S_DIN;
  cfg.copyFrom(audioInfo);
  i2s.begin(cfg);

  sine.begin(audioInfo, TONE_HZ);
  sine.setAmplitude(TONE_AMPLITUDE);
  sound.begin(audioInfo);

  // ── Hall sensor — A3144 is active-low / open-collector ──
  // External 10kΩ pullup to 3V3 is on the PCB (R7).
  // Use INPUT (not INPUT_PULLUP) so the internal weak pullup
  // doesn't fight the external one.
  pinMode(PIN_HALL, INPUT);
  attachInterrupt(digitalPinToInterrupt(PIN_HALL), hallISR, FALLING);

  Serial.println("All peripherals initialised. Starting test loop.\n");
}

// ────────────────────────────────────────────────────────────
//  loop()
// ────────────────────────────────────────────────────────────
void loop() {

  // ══════════════════════════════════════════════════════════
  //  TEST 1 — Motor at medium PWM
  // ══════════════════════════════════════════════════════════
  Serial.println("── TEST 1: Motor (medium PWM) ───────────────────────");
  Serial.printf("   duty=%d/255  freq=%d kHz\n", MOTOR_MEDIUM, PWM_FREQ_MOTOR / 1000);
  ledcWrite(PIN_MOTOR_PWM, MOTOR_MEDIUM);
  delay(3000);
  ledcWrite(PIN_MOTOR_PWM, 0);
  Serial.println("   Motor off.\n");
  delay(500);

  // ══════════════════════════════════════════════════════════
  //  TEST 2 — LED filaments, two slow sine-envelope pulses
  // ══════════════════════════════════════════════════════════
  Serial.println("── TEST 2: LED filaments (soft pulse) ───────────────");
  uint32_t ledStart = millis();
  while (millis() - ledStart < 4000) {
    float   t   = (float)(millis() - ledStart) / 2000.0f;  // 0→2 over 2s
    float   env = 0.5f * (1.0f - cosf(t * PI));            // smooth 0→1→0
    uint8_t pwm = (uint8_t)(env * 220.0f);
    ledcWrite(PIN_LED_PWM, pwm);
    delay(10);
  }
  ledcWrite(PIN_LED_PWM, 0);
  Serial.println("   LED pulse complete.\n");
  delay(500);

  // ══════════════════════════════════════════════════════════
  //  TEST 3 — Speaker: 240 Hz via I2S (AudioTools)
  // ══════════════════════════════════════════════════════════
  Serial.println("── TEST 3: Speaker (I2S, 240 Hz) ────────────────────");
  Serial.printf("   Tone: %d Hz  amplitude: %d  duration: 3 s\n",
                TONE_HZ, TONE_AMPLITUDE);
  uint32_t toneStart = millis();
  while (millis() - toneStart < 3000) {
    copier.copy();
  }
  // Silence the output cleanly
  i2s.end();
  auto cfg2    = i2s.defaultConfig(TX_MODE);
  cfg2.pin_bck  = PIN_I2S_BCLK;
  cfg2.pin_ws   = PIN_I2S_LRC;
  cfg2.pin_data = PIN_I2S_DIN;
  cfg2.copyFrom(audioInfo);
  i2s.begin(cfg2);
  Serial.println("   Tone complete.\n");
  delay(500);

  // ══════════════════════════════════════════════════════════
  //  TEST 4 — LRA haptic: two soft pulses via I2C (TM6605)
  // ══════════════════════════════════════════════════════════
  Serial.println("── TEST 4: LRA haptic (I2C, TM6605 @ 0x2A) ─────────");
  uint8_t status = tm_read(REG_STATUS);
  if (status == 0xFF) {
    Serial.println("   WARNING: no ACK from TM6605 — check wiring");
  } else {
    Serial.printf("   TM6605 status reg: 0x%02X  (device found)\n", status);
    tm_init_lra_rtp();
    for (int i = 0; i < 2; i++) {
      Serial.printf("   Pulse %d/2\n", i + 1);
      tm_soft_pulse(600);
      delay(400);
    }
    // Put device back to standby
    tm_write(REG_MODE, 0x40);
    Serial.println("   Haptic complete.");
  }
  Serial.println();
  delay(500);

  // ══════════════════════════════════════════════════════════
  //  TEST 5 — Hall effect sensor (A3144)
  //  Spin the rotor with magnets past the sensor during this window.
  // ══════════════════════════════════════════════════════════
  Serial.println("── TEST 5: Hall effect sensor (5 s window) ──────────");
  Serial.println("   → Spin the rotor now...");
  Serial.println("   Count | Pulses/sec | RPM (assume 1 magnet)");

  hallCount      = 0;
  hallLastPrint  = millis();
  hallLastPulse  = millis();

  uint32_t hallWindowStart = millis();
  while (millis() - hallWindowStart < 5000) {
    unsigned long now = millis();

    if (hallPulseFlag) {
      hallPulseFlag = false;
      hallCount++;
      hallLastPulse = now;
      Serial.printf("   ★ Magnet pass! count=%d\n", hallCount);
    }

    // Print a rolling 500ms RPM estimate
    if (now - hallLastPrint >= 500) {
      hallLastPrint = now;
      bool   moving = (now - hallLastPulse) < 400;
      float  pps    = hallCount * 2.0f;       // per second from 500ms window
      float  rpm    = pps * 60.0f;            // 1 magnet per revolution
      Serial.printf("   [%4lums]  pps=%.1f  RPM=%.1f  %s\n",
                    now - hallWindowStart, pps, rpm,
                    moving ? "MOVING" : "stopped");
      hallCount = 0;  // reset for next 500ms window
    }

    delay(5);
  }
  Serial.println();

  // ══════════════════════════════════════════════════════════
  Serial.println("══ All tests complete. Repeating in 2 s... ══\n");
  delay(2000);
}