/**
 * BikeAudio — ESP32 DevKitC v4 Bluetooth Audio Relay
 *
 * iPhone --> [ESP32 A2DP SINK] --> [A2DP SOURCE x2] --> JBL Charge 5 + Cardo Packtalk Edge
 *
 * Libraries required (install via Arduino IDE Library Manager or .zip):
 *   - ESP32-A2DP       by Phil Schatzmann  https://github.com/pschatzmann/ESP32-A2DP
 *   - arduino-audio-tools by Phil Schatzmann  https://github.com/pschatzmann/arduino-audio-tools
 *
 * Board: ESP32 Dev Module
 * Partition scheme: Huge APP (3MB No OTA/1MB SPIFFS)  <-- required for BT stack size
 * ESP32 Arduino core: 2.0.17 (do NOT use 3.x — BT stack regression)
 *
 * HOW IT WORKS:
 *   1. ESP32 boots and connects to JBL Charge 5 and Cardo as A2DP sources
 *   2. Then advertises itself as "BikeAudio" for the iPhone to connect to
 *   3. Audio received from iPhone is forwarded to both speakers in real time
 *   4. Auto-reconnect on power cycle — just turn everything on and it finds each other
 *
 * FIRST TIME SETUP:
 *   - Forget JBL and Cardo from your iPhone
 *   - Put JBL in pairing mode (hold Bluetooth button)
 *   - Put Cardo in pairing mode (check Cardo manual — usually hold phone button)
 *   - Flash this sketch, open Serial Monitor at 115200
 *   - ESP32 will find and pair with both devices on first boot
 *   - On iPhone, go to Bluetooth settings and connect to "BikeAudio"
 *   - Done — play audio, both speakers output simultaneously
 */

#include "AudioTools.h"
#include "BluetoothA2DPSink.h"
#include "BluetoothA2DPSource.h"
#include "BluetoothA2DPCommon.h"

// ─── CONFIGURATION ────────────────────────────────────────────────────────────

// Name this device shows to iPhone
#define SINK_NAME        "BikeAudio"

// Exact Bluetooth names of your speakers (must match exactly, case sensitive)
#define JBL_NAME         "JBL Charge 5"
#define CARDO_NAME       "Tangerine EDGE"

// Retry interval if a speaker disconnects (ms)
#define RECONNECT_MS     5000

// Audio buffer size — larger = more stable, slightly more latency
#define BUFFER_SIZE      (4 * 1024)

// ─── GLOBALS ──────────────────────────────────────────────────────────────────

BluetoothA2DPSink   sink;       // receives audio FROM iPhone
BluetoothA2DPSource src_jbl;    // sends audio TO JBL
BluetoothA2DPSource src_cardo;  // sends audio TO Cardo

// Shared ring buffer — sink writes, sources read
RingBuffer<uint8_t> ring_buf(BUFFER_SIZE * 2);

// Connection state
volatile bool jbl_connected   = false;
volatile bool cardo_connected = false;
volatile bool iphone_connected = false;

unsigned long last_reconnect_jbl   = 0;
unsigned long last_reconnect_cardo = 0;

// ─── AUDIO CALLBACK (iPhone → buffer) ────────────────────────────────────────

/**
 * Called by the A2DP sink every time a new audio frame arrives from iPhone.
 * We write raw PCM into the shared ring buffer.
 * Both sources pull from this buffer simultaneously.
 */
void audio_received_cb(const uint8_t *data, uint32_t len) {
    // Write to ring buffer — non-blocking, drop if full (prevents deadlock)
    for (uint32_t i = 0; i < len; i++) {
        if (!ring_buf.isFull()) {
            ring_buf.write(data[i]);
        }
    }
}

// ─── SOURCE DATA CALLBACK (buffer → JBL / Cardo) ─────────────────────────────

/**
 * Called by each A2DP source when it needs audio data to send.
 * Both JBL and Cardo call this — they share the same buffer read pointer
 * via a duplicated/mirrored buffer approach.
 *
 * We use a simple approach: one primary reader (JBL) drains the buffer,
 * Cardo gets the same data via a mirrored write in audio_received_cb.
 */

// Second ring buffer mirroring data for Cardo
RingBuffer<uint8_t> ring_buf_cardo(BUFFER_SIZE * 2);

void audio_received_mirror_cb(const uint8_t *data, uint32_t len) {
    // Write to BOTH ring buffers — JBL gets ring_buf, Cardo gets ring_buf_cardo
    for (uint32_t i = 0; i < len; i++) {
        if (!ring_buf.isFull())       ring_buf.write(data[i]);
        if (!ring_buf_cardo.isFull()) ring_buf_cardo.write(data[i]);
    }
}

int32_t get_audio_for_jbl(uint8_t *data, int32_t len) {
    int32_t bytes_read = 0;
    while (bytes_read < len && !ring_buf.isEmpty()) {
        data[bytes_read++] = ring_buf.read();
    }
    // Pad with silence if buffer underrun
    if (bytes_read < len) {
        memset(data + bytes_read, 0, len - bytes_read);
    }
    return len;
}

int32_t get_audio_for_cardo(uint8_t *data, int32_t len) {
    int32_t bytes_read = 0;
    while (bytes_read < len && !ring_buf_cardo.isEmpty()) {
        data[bytes_read++] = ring_buf_cardo.read();
    }
    if (bytes_read < len) {
        memset(data + bytes_read, 0, len - bytes_read);
    }
    return len;
}

// ─── CONNECTION CALLBACKS ─────────────────────────────────────────────────────

void sink_connected_cb(esp_bd_addr_t addr, esp_a2d_connection_state_t state, void *obj) {
    if (state == ESP_A2D_CONNECTION_STATE_CONNECTED) {
        Serial.println("[SINK] iPhone connected");
        iphone_connected = true;
    } else {
        Serial.println("[SINK] iPhone disconnected");
        iphone_connected = false;
    }
}

void jbl_connected_cb(esp_bd_addr_t addr, esp_a2d_connection_state_t state, void *obj) {
    if (state == ESP_A2D_CONNECTION_STATE_CONNECTED) {
        Serial.println("[JBL] Connected");
        jbl_connected = true;
    } else {
        Serial.println("[JBL] Disconnected — will retry");
        jbl_connected = false;
        last_reconnect_jbl = millis();
    }
}

void cardo_connected_cb(esp_bd_addr_t addr, esp_a2d_connection_state_t state, void *obj) {
    if (state == ESP_A2D_CONNECTION_STATE_CONNECTED) {
        Serial.println("[CARDO] Connected");
        cardo_connected = true;
    } else {
        Serial.println("[CARDO] Disconnected — will retry");
        cardo_connected = false;
        last_reconnect_cardo = millis();
    }
}

// ─── SETUP ────────────────────────────────────────────────────────────────────

void setup() {
    Serial.begin(115200);
    delay(500);
    Serial.println("=== BikeAudio Booting ===");

    // ── Step 1: Connect to JBL as A2DP source ─────────────────────────────────
    Serial.println("[JBL] Connecting...");
    src_jbl.set_data_callback(get_audio_for_jbl);
    src_jbl.set_on_connection_state_changed(jbl_connected_cb);
    src_jbl.set_auto_reconnect(true);
    src_jbl.start(JBL_NAME, true);   // true = reconnect if known device

    // Give it time to connect before starting second source
    // (Bluedroid needs sequential connection setup)
    uint32_t t = millis();
    while (!jbl_connected && millis() - t < 10000) {
        delay(100);
    }
    if (jbl_connected) {
        Serial.println("[JBL] Ready");
    } else {
        Serial.println("[JBL] Not found yet — will retry in background");
    }

    // ── Step 2: Connect to Cardo as A2DP source ────────────────────────────────
    Serial.println("[CARDO] Connecting...");
    src_cardo.set_data_callback(get_audio_for_cardo);
    src_cardo.set_on_connection_state_changed(cardo_connected_cb);
    src_cardo.set_auto_reconnect(true);
    src_cardo.start(CARDO_NAME, true);

    t = millis();
    while (!cardo_connected && millis() - t < 10000) {
        delay(100);
    }
    if (cardo_connected) {
        Serial.println("[CARDO] Ready");
    } else {
        Serial.println("[CARDO] Not found yet — will retry in background");
    }

    // ── Step 3: Start sink — advertise "BikeAudio" to iPhone ──────────────────
    Serial.println("[SINK] Advertising as '" SINK_NAME "' ...");
    sink.set_stream_reader(audio_received_mirror_cb);
    sink.set_on_connection_state_changed(sink_connected_cb);
    sink.start(SINK_NAME);
    Serial.println("[SINK] Ready — connect iPhone to 'BikeAudio'");

    Serial.println("=== BikeAudio Ready ===");
    print_status();
}

// ─── LOOP ─────────────────────────────────────────────────────────────────────

void loop() {
    // Auto-reconnect JBL if lost
    if (!jbl_connected && millis() - last_reconnect_jbl > RECONNECT_MS) {
        Serial.println("[JBL] Retrying connection...");
        src_jbl.start(JBL_NAME, true);
        last_reconnect_jbl = millis();
    }

    // Auto-reconnect Cardo if lost
    if (!cardo_connected && millis() - last_reconnect_cardo > RECONNECT_MS) {
        Serial.println("[CARDO] Retrying connection...");
        src_cardo.start(CARDO_NAME, true);
        last_reconnect_cardo = millis();
    }

    // Print status every 10 seconds
    static unsigned long last_status = 0;
    if (millis() - last_status > 10000) {
        print_status();
        last_status = millis();
    }

    delay(100);
}

// ─── HELPERS ──────────────────────────────────────────────────────────────────

void print_status() {
    Serial.println("--- Status ---");
    Serial.printf("  iPhone : %s\n", iphone_connected ? "CONNECTED" : "waiting...");
    Serial.printf("  JBL    : %s\n", jbl_connected    ? "CONNECTED" : "waiting...");
    Serial.printf("  Cardo  : %s\n", cardo_connected  ? "CONNECTED" : "waiting...");
    Serial.printf("  Heap   : %d bytes free\n", ESP.getFreeHeap());
    Serial.println("--------------");
}