#include "ble_midi_transport.h"
#include <Arduino.h>
#include <NimBLEDevice.h>
#include <esp_bt.h>
#include <esp_bt_main.h>

#define BLE_MIDI_SERVICE_UUID    "03B80E5A-EDE8-4B33-A751-6CE34EC4C700"
#define BLE_MIDI_CHAR_UUID       "7772E5DB-3868-4112-A1A9-F2669D106BF3"

static BleMidiTransport* instance = nullptr;

static NimBLEServer* ble_server = nullptr;
static NimBLEService* ble_service = nullptr;
static NimBLECharacteristic* ble_char = nullptr;
static bool device_connected = false;

class ServerCallbacks : public NimBLEServerCallbacks {
    void onConnect(NimBLEServer* server) override {
        device_connected = true;
        Serial.println("[BLE] Client connected");
    }
    void onDisconnect(NimBLEServer* server) override {
        device_connected = false;
        Serial.println("[BLE] Client disconnected, restarting advertising");
        NimBLEDevice::startAdvertising();
    }
};

class CharCallbacks : public NimBLECharacteristicCallbacks {
    void onWrite(NimBLECharacteristic* characteristic) override {
        std::string value = characteristic->getValue();
        if (value.length() > 0 && instance) {
            instance->on_receive((const uint8_t*)value.data(), value.length());
        }
    }
};

BleMidiTransport::BleMidiTransport() : initialized(false), client_connected(false) {
    instance = this;
}

BleMidiTransport::~BleMidiTransport() {
    if (instance == this) instance = nullptr;
}

bool BleMidiTransport::begin() {
    Serial.println("[BLE] Initializing BLE MIDI...");

    esp_err_t err;
    err = esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT);
    Serial.printf("[BLE] bt_mem_release: %d (%s)\n", err, esp_err_to_name(err));

    esp_bt_controller_config_t bt_cfg = BT_CONTROLLER_INIT_CONFIG_DEFAULT();
    err = esp_bt_controller_init(&bt_cfg);
    Serial.printf("[BLE] bt_controller_init: %d (%s)\n", err, esp_err_to_name(err));
    if (err != ESP_OK) { initialized = false; return false; }

    err = esp_bt_controller_enable(ESP_BT_MODE_BTDM);
    Serial.printf("[BLE] bt_controller_enable: %d (%s)\n", err, esp_err_to_name(err));
    if (err != ESP_OK) { initialized = false; return false; }

    Serial.println("[BLE] NimBLEDevice init...");
    NimBLEDevice::init("JOC Midi");
    Serial.println("[BLE] NimBLEDevice initialized");

    ble_server = NimBLEDevice::createServer();
    if (!ble_server) {
        Serial.println("[BLE] FAILED to create server");
        return false;
    }
    ble_server->setCallbacks(new ServerCallbacks());

    ble_service = ble_server->createService(BLE_MIDI_SERVICE_UUID);
    if (!ble_service) {
        Serial.println("[BLE] FAILED to create service");
        return false;
    }

    ble_char = ble_service->createCharacteristic(
        BLE_MIDI_CHAR_UUID,
        NIMBLE_PROPERTY::READ |
        NIMBLE_PROPERTY::WRITE_NR |
        NIMBLE_PROPERTY::NOTIFY
    );
    if (!ble_char) {
        Serial.println("[BLE] FAILED to create characteristic");
        return false;
    }
    ble_char->createDescriptor(BLEUUID((uint16_t)0x2902), NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::WRITE);
    ble_char->setCallbacks(new CharCallbacks());

    ble_service->start();
    Serial.println("[BLE] Service started");

    NimBLEAdvertising* adv = NimBLEDevice::getAdvertising();
    adv->addServiceUUID(BLE_MIDI_SERVICE_UUID);
    adv->setScanResponse(true);
    adv->start();

    initialized = true;
    Serial.println("[BLE] BLE MIDI advertising as 'JOC Midi'");
    return true;
}

void BleMidiTransport::update() {
    client_connected = device_connected;
}

void BleMidiTransport::on_midi_receive(std::function<void(const MidiEvent&)> callback) {
    receive_callback = callback;
}

void BleMidiTransport::send_midi_packet(const uint8_t* data, uint8_t len) {
    if (!initialized || !client_connected || !ble_char) return;

    // BLE MIDI format: timestamp header + raw MIDI message
    uint16_t timestamp = micros() & 0x3FFF;
    uint8_t packet[16];
    uint8_t idx = 0;
    packet[idx++] = 0x80 | (timestamp >> 7);
    packet[idx++] = timestamp & 0x7F;
    for (uint8_t i = 0; i < len && idx < 16; i++) {
        packet[idx++] = data[i];
    }

    ble_char->setValue(packet, idx);
    ble_char->notify();
}

void BleMidiTransport::send_note_on(uint8_t channel, uint8_t note, uint8_t velocity) {
    uint8_t packet[3] = {(uint8_t)(0x90 | (channel - 1)), note, velocity};
    send_midi_packet(packet, 3);
    Serial.printf("[BLE OUT] Ch:%d NOTE_ON:%d:%d\n", channel, note, velocity);
}

void BleMidiTransport::send_note_off(uint8_t channel, uint8_t note, uint8_t velocity) {
    uint8_t packet[3] = {(uint8_t)(0x80 | (channel - 1)), note, velocity};
    send_midi_packet(packet, 3);
    Serial.printf("[BLE OUT] Ch:%d NOTE_OFF:%d:%d\n", channel, note, velocity);
}

void BleMidiTransport::send_cc(uint8_t channel, uint8_t cc, uint8_t value) {
    uint8_t packet[3] = {(uint8_t)(0xB0 | (channel - 1)), cc, value};
    send_midi_packet(packet, 3);
    Serial.printf("[BLE OUT] Ch:%d CC:%d:%d\n", channel, cc, value);
}

bool BleMidiTransport::is_connected() {
    return initialized && client_connected;
}

void BleMidiTransport::on_receive(const uint8_t* data, size_t len) {
    if (len < 3 || !receive_callback) return;  // need timestamp(2) + status(1)

    // BLE MIDI format: [ts_hi|0x80] [ts_lo] [status ...]
    size_t offset = 2;  // skip timestamp header

    while (offset < len) {
        uint8_t status = data[offset++];
        if (status < 0x80) continue;  // skip non-status bytes

        uint8_t type = status & 0xF0;
        uint8_t channel = (status & 0x0F) + 1;

        MidiEvent event;
        event.channel = channel;
        event.timestamp = millis();
        event.type = MidiEvent::NOTE_ON;  // default

        offset = parse_ble_midi(status, data, len, offset, event);

        if (event.type == MidiEvent::SYSEX) {
            Serial.printf("[BLE IN] Sysex len=%zu\n", len);
        } else {
            Serial.printf("[BLE IN] Ch:%d %s:%d:%d\n",
                event.channel,
                event.type == MidiEvent::NOTE_ON ? "NOTE_ON" :
                event.type == MidiEvent::NOTE_OFF ? "NOTE_OFF" :
                event.type == MidiEvent::CONTROL_CHANGE ? "CC" : "OTHER",
                event.data1, event.data2);
        }

        if (receive_callback) {
            receive_callback(event);
        }
    }
}

size_t BleMidiTransport::parse_ble_midi(uint8_t status, const uint8_t* data, size_t len, size_t offset, MidiEvent& event) {
    uint8_t type = status & 0xF0;
    event.data1 = 0;
    event.data2 = 0;

    if (offset >= len) return offset;

    event.data1 = data[offset++];

    switch (type) {
        case 0x80:
            event.type = MidiEvent::NOTE_OFF;
            if (offset < len) event.data2 = data[offset++];
            break;
        case 0x90:
            event.type = MidiEvent::NOTE_ON;
            if (offset < len) event.data2 = data[offset++];
            if (event.data2 == 0) event.type = MidiEvent::NOTE_OFF;
            break;
        case 0xB0:
            event.type = MidiEvent::CONTROL_CHANGE;
            if (offset < len) event.data2 = data[offset++];
            break;
        case 0xC0:
            event.type = MidiEvent::PROGRAM_CHANGE;
            break;
        case 0xE0:
            event.type = MidiEvent::PITCH_BEND;
            if (offset < len) event.data2 = data[offset++];
            break;
        default:
            break;
    }

    return offset;
}