/*
 * RP2350 + INMP441 -> USB Audio Class 2.0
 * ======================================
 *
 * High-quality Arduino implementation for Raspberry Pi RP2350.
 *
 * Audio path:
 *   INMP441
 *      -> 32-bit I2S slots / 64 BCLK clocks per stereo WS frame
 *      -> RP2350 PIO + DMA via Arduino-Pico I2S
 *      -> select one I2S channel
 *      -> convert left-aligned 32-bit slot to packed signed PCM24
 *      -> USB AudioTools / TinyUSB UAC2
 *      -> standard USB microphone on the host
 *
 * Default USB format:
 *   48,000 Hz / mono / 16-bit PCM (Windows compatibility test)
 *
 * WHY 32-BIT I2S?
 * ----------------
 * INMP441 has 24-bit I2S data but requires 64 SCK clocks per WS frame.
 * The RP2350 Arduino-Pico I2S implementation uses a stereo I2S frame with
 * 'bits' clocks per left/right word. Therefore we run the I2S peripheral at
 * 32 bits per slot, giving the required 32 + 32 = 64 BCLK clocks per WS.
 * The final 8 bits are then discarded when packing the native 24-bit sample
 * for USB.
 *
 * USB:
 *   UAC2, device -> host only (capture / microphone)
 *   48 kHz, mono, 16-bit
 *
 * Arduino environment:
 *   - Arduino-Pico (earlephilhower)
 *   - current 5.x release recommended
 *   - Tools -> USB Stack -> Adafruit TinyUSB
 *   - Arduino AudioTools 1.2.6+
 *
 * Wiring example:
 *   RP2350 GPIO 2  -> INMP441 SCK/BCLK
 *   RP2350 GPIO 3  -> INMP441 WS/LRCLK
 *   RP2350 GPIO 4  <- INMP441 SD
 *   3.3V            -> VDD
 *   GND             -> GND
 *   GND             -> L/R  (select LEFT channel)
 *
 * For RIGHT channel, connect L/R to 3.3V and change MIC_I2S_CHANNEL to 1.
 *
 * IMPORTANT:
 *   This is intended as a prototype/experimental device. AudioTools uses
 *   a prototype VID/PID by default. Assign your own VID/PID before shipping.
 */

#include <Arduino.h>
#include <I2S.h>
#include <Adafruit_TinyUSB.h>

#include "AudioTools.h"
#include "AudioTools/Communication/USB/USBAudioStream.h"

// ============================================================================
// HARDWARE CONFIGURATION
// ============================================================================

// Arduino-Pico requires BCLK and WS to be adjacent for this I2S driver.
static constexpr pin_size_t MIC_BCLK_PIN = 2;
static constexpr pin_size_t MIC_WS_PIN   = 3;   // BCLK + 1
static constexpr pin_size_t MIC_DATA_PIN = 4;

// INMP441 L/R setting:
//   0 = left channel (L/R tied to GND)
//   1 = right channel (L/R tied to 3V3)
static constexpr uint8_t MIC_I2S_CHANNEL = 0;

// ============================================================================
// AUDIO CONFIGURATION
// ============================================================================

static constexpr uint32_t SAMPLE_RATE = 48000;
static constexpr uint8_t  CHANNELS = 1;

// INMP441 is 24-bit, but the physical I2S stereo slot is 32 clocks wide.
static constexpr uint8_t I2S_BITS = 32;
static constexpr uint8_t USB_BITS = 24;

// One millisecond of mono 48 kHz audio = 48 samples.
static constexpr size_t USB_SAMPLES_PER_PACKET = SAMPLE_RATE / 1000;
static constexpr size_t USB_BYTES_PER_SAMPLE = USB_BITS / 8;
static constexpr size_t USB_PACKET_BYTES =
    USB_SAMPLES_PER_PACKET * USB_BYTES_PER_SAMPLE * CHANNELS;

// Two I2S slots per WS frame. 48 samples * 2 channels = 96 words.
static constexpr size_t I2S_WORDS_PER_PACKET = USB_SAMPLES_PER_PACKET * 2;
static constexpr size_t I2S_BYTES_PER_PACKET =
    I2S_WORDS_PER_PACKET * sizeof(int32_t);

// Arduino-Pico I2S DMA configuration. The values are in 32-bit words.
static constexpr size_t I2S_DMA_BUFFERS = 8;
static constexpr size_t I2S_DMA_WORDS = 256;

// AudioTools USB software FIFO. 32 x 1-ms packets gives ~32 ms of USB-side
// buffering and provides a generous scheduling margin for the RP2350.
static constexpr uint8_t USB_FIFO_PACKETS = 32;

// Keep the microphone data untouched by AudioTools' software volume control.
// Host applications can apply their own recording gain.
static constexpr bool USB_VOLUME_ACTIVE = false;

// Enable periodic diagnostic output. Set false after the hardware is proven.
static constexpr bool DEBUG_STATS = true;

// Startup: discard a short amount of microphone data while the INMP441 and
// I2S pipeline settle.
static constexpr uint16_t STARTUP_DISCARD_MS = 100;

// ============================================================================
// OBJECTS
// ============================================================================

I2S i2s(INPUT);
USBAudioStream usb;

// Raw two-channel 32-bit I2S words for exactly 1 ms.
alignas(4) static int32_t i2sRaw[I2S_WORDS_PER_PACKET];

// Packed USB Audio Class 2.0 PCM24: exactly 3 bytes per mono sample.
alignas(4) static uint8_t usbPacket[USB_PACKET_BYTES];

// ============================================================================
// FATAL ERROR
// ============================================================================

static void fatalError(const char *message)
{
  Serial.println();
  Serial.println(F("=============================================="));
  Serial.println(F("RP2350 INMP441 USB AUDIO - FATAL ERROR"));
  Serial.println(F("=============================================="));
  Serial.println(message);
  Serial.println();

  while (true) {
    delay(1000);
  }
}

// ============================================================================
// EXACT BLOCK READ
// ============================================================================

// I2S::read(buffer, size) is the block-oriented Arduino-Pico API. It may
// return less than requested, so complete the block without ever assuming that
// one call returned the full DMA payload.
static bool readI2SExact(uint8_t *dst, size_t wanted, uint32_t timeoutMs = 20)
{
  size_t total = 0;
  const uint32_t start = millis();

  while (total < wanted) {
    const size_t n = i2s.read(dst + total, wanted - total);

    if (n > 0) {
      total += n;
      continue;
    }

    if ((millis() - start) >= timeoutMs) {
      return false;
    }

    delayMicroseconds(20);
  }

  return true;
}

// ============================================================================
// PACK INMP441 -> PCM24
// ============================================================================

static void packInmp441ToPcm24()
{
  // INMP441 delivers 24 useful bits in the upper 24 bits of the 32-bit I2S
  // slot. UAC2 Type I PCM24 is packed here as 3 bytes/sample, little-endian.
  for (size_t n = 0; n < USB_SAMPLES_PER_PACKET; ++n) {
    const size_t wordIndex = n * 2 + MIC_I2S_CHANNEL;
    const int32_t sample24 = i2sRaw[wordIndex] >> 8;
    const size_t out = n * 3;

    usbPacket[out + 0] = static_cast<uint8_t>(sample24 & 0xFF);
    usbPacket[out + 1] = static_cast<uint8_t>((sample24 >> 8) & 0xFF);
    usbPacket[out + 2] = static_cast<uint8_t>((sample24 >> 16) & 0xFF);
  }
}

// ============================================================================
// I2S INITIALIZATION
// ============================================================================

static bool setupI2S()
{
  i2s.setDATA(MIC_DATA_PIN);
  i2s.setBCLK(MIC_BCLK_PIN);
  i2s.setBitsPerSample(I2S_BITS);
  i2s.setFrequency(SAMPLE_RATE);

  // PIO + DMA buffers. More than the minimum is intentional: USB and serial
  // housekeeping must never make the I2S input overrun under normal load.
  i2s.setBuffers(I2S_DMA_BUFFERS, I2S_DMA_WORDS);

  return i2s.begin();
}

// ============================================================================
// USB UAC2 INITIALIZATION
// ============================================================================

static bool setupUSB()
{
  // TX_MODE = device -> host = microphone/capture device.
  auto cfg = usb.defaultConfig(TX_MODE);

  AudioInfo usbInfo(SAMPLE_RATE, CHANNELS, USB_BITS);
  cfg.copyFrom(usbInfo);

  // Device identity shown by the operating system.
  cfg.manufacturer = "Lew Judy";
  cfg.product = "Lew Judy RP2350 INMP441 USB Microphone TEST24";
  cfg.vid = 0xCafe;
  cfg.pid = 0x4005;  // New PID after fixing USB frame sizing.

  // We only advertise one fixed clock: 48 kHz.
  cfg.enable_multi_sample_rate = false;
  cfg.sample_rate = SAMPLE_RATE;

  // USB ISO IN endpoint buffering.
  cfg.fifo_packets = USB_FIFO_PACKETS;

  // IMPORTANT: AudioTools reserves one extra sample in wMaxPacketSize
  // for isochronous headroom. With flow-control enabled, the actual 48 kHz
  // transfer is correctly sized to exactly 48 samples = 1 ms per USB frame.
  // Disabling flow-control would make AudioTools send the full 49-sample
  // endpoint size at 48 kHz, which causes continuous buffer underruns/artifacts.
  cfg.enable_ep_in_flow_control = true;

  // No separate feedback endpoint is necessary for device->host capture.
  // The host consumes the fixed-rate IN stream.
  cfg.enable_feedback_ep = false;

  // No interrupt endpoint needed for the fixed configuration.
  cfg.enable_interrupt_ep = false;

  // Keep audio samples unmodified in the RP2350. Host mixer/recording software
  // remains responsible for gain.
  cfg.volume_active = USB_VOLUME_ACTIVE;

  // Prototype power declaration.
  cfg.self_powered = false;
  cfg.max_power_ma = 100;

  // Diagnostics: tell us whether Windows actually opens the UAC2 capture
  // streaming interface and whether it negotiates the sample rate.
  usb.setStreamingStateCallback([](bool tx, bool rx) {
    Serial.print(F("UAC2 streaming state: TX="));
    Serial.print(tx ? F("ON") : F("OFF"));
    Serial.print(F(" RX="));
    Serial.println(rx ? F("ON") : F("OFF"));
  });

  usb.setSampleRateCallback([](uint32_t rate) {
    Serial.print(F("UAC2 host sample rate: "));
    Serial.println(rate);
  });

  return usb.begin(cfg);
}

// ============================================================================
// STARTUP DRAIN
// ============================================================================

static bool discardStartupAudio()
{
  // We discard complete 1-ms blocks so that the first host-visible audio is
  // not the unstable startup portion of the MEMS microphone/I2S link.
  const uint32_t blocks = STARTUP_DISCARD_MS;

  for (uint32_t n = 0; n < blocks; ++n) {
    if (!readI2SExact(reinterpret_cast<uint8_t *>(i2sRaw),
                      I2S_BYTES_PER_PACKET, 20)) {
      return false;
    }
  }

  return true;
}

// ============================================================================
// SETUP
// ============================================================================

void setup()
{
  // AudioTools' TinyUSB backend expects Adafruit TinyUSB / USE_TINYUSB.
  if (!TinyUSBDevice.isInitialized()) {
    TinyUSBDevice.begin(0);
  }

  Serial.begin(115200);
  delay(20);

  AudioToolsLogger.begin(Serial, AudioToolsLogLevel::Warning);

  Serial.println();
  Serial.println(F("=============================================="));
  Serial.println(F("RP2350 + INMP441 -> USB Audio Class 2.0"));
  Serial.println(F("=============================================="));

  Serial.print(F("I2S: "));
  Serial.print(SAMPLE_RATE);
  Serial.print(F(" Hz / 2-slot x "));
  Serial.print(I2S_BITS);
  Serial.println(F(" bit"));

  Serial.print(F("USB: "));
  Serial.print(SAMPLE_RATE);
  Serial.print(F(" Hz / "));
  Serial.print(CHANNELS);
  Serial.print(F(" ch / "));
  Serial.print(USB_BITS);
  Serial.println(F(" bit PCM"));

  Serial.print(F("I2S GPIO: BCLK="));
  Serial.print(MIC_BCLK_PIN);
  Serial.print(F(" WS="));
  Serial.print(MIC_WS_PIN);
  Serial.print(F(" DATA="));
  Serial.println(MIC_DATA_PIN);

  Serial.print(F("INMP441 channel: "));
  Serial.println(MIC_I2S_CHANNEL == 0 ? F("LEFT") : F("RIGHT"));

  Serial.print(F("USB packet: "));
  Serial.print(USB_PACKET_BYTES);
  Serial.print(F(" bytes / "));
  Serial.print(USB_SAMPLES_PER_PACKET);
  Serial.println(F(" samples (1 ms)"));

  if (MIC_WS_PIN != MIC_BCLK_PIN + 1) {
    fatalError("BCLK and WS must be adjacent GPIOs for Arduino-Pico I2S.");
  }

  if (MIC_I2S_CHANNEL > 1) {
    fatalError("MIC_I2S_CHANNEL must be 0 (LEFT) or 1 (RIGHT).");
  }

  if (!setupI2S()) {
    fatalError("Could not initialize RP2350 I2S/PIO/DMA.");
  }

  // INMP441 startup settling.
  delay(20);

  if (!discardStartupAudio()) {
    fatalError("Could not read the initial I2S data block.");
  }

  if (!setupUSB()) {
    fatalError("Could not initialize USB Audio Class 2.");
  }

  // If TinyUSB was already attached before the UAC2 interface was registered,
  // force a clean host re-enumeration.
  if (TinyUSBDevice.mounted()) {
    TinyUSBDevice.detach();
    delay(10);
    TinyUSBDevice.attach();
  }

  Serial.println(F("USB Audio is initialized."));
  Serial.println(F("Select the USB microphone in the host OS."));
  Serial.println();
}

// ============================================================================
// MAIN AUDIO LOOP
// ============================================================================

void loop()
{
  // Always drain I2S so the DMA ring never overruns. Before the host opens the
  // microphone we simply discard the samples.
  if (!readI2SExact(reinterpret_cast<uint8_t *>(i2sRaw),
                    I2S_BYTES_PER_PACKET, 20)) {
    Serial.println(F("I2S read timeout / underflow"));
    delay(1);
    return;
  }

  if (usb.isStreamingActiveTx()) {
    packInmp441ToPcm24();

    // USB packet contains exactly 48 samples (24-bit PCM) = 1 ms at 48 kHz.
    const size_t written = usb.write(usbPacket, USB_PACKET_BYTES);

    if (DEBUG_STATS && written != USB_PACKET_BYTES) {
      Serial.print(F("USB short write: "));
      Serial.print(written);
      Serial.print(F("/"));
      Serial.println(USB_PACKET_BYTES);
    }
  }

  if (DEBUG_STATS) {
    static uint32_t lastStats = 0;
    const uint32_t now = millis();

    if (now - lastStats >= 1000) {
      lastStats = now;

      Serial.print(F("UAC2 xfer="));
      Serial.print(usb.getTxXferCount());
      Serial.print(F(" fifoRead="));
      Serial.print(usb.getTxFifoReadTotal());
      Serial.print(F(" usbAvail="));
      Serial.print(usb.availableForWrite());
      Serial.print(F(" frame="));
      Serial.print(usb.getTxFrameBytesLast());
      Serial.print(F(" I2S avail="));
      Serial.print(i2s.available());
      Serial.print(F(" I2S overflow="));
      Serial.println(i2s.getOverflow() ? F("YES") : F("no"));
    }
  }
}