#include <cstdio>
#include "pico/stdlib.h"
#include "pico/cyw43_arch.h"
#include "pico/btstack_flash_bank.h"
#include "btstack_tlv_flash_bank.h"
#include "hci.h"
#include "ble/le_device_db_tlv.h"
#include "classic/btstack_link_key_db_tlv.h"

#include <cs_midi.h>
#include "encoder.h"
#include "spp_midi.h"

// Two CC sets, toggled by SW1 button.
// Set A: encoders CC 16-19, buttons CC 20-22 (SW2-SW4)
// Set B: encoders CC 24-27, buttons CC 28-30 (SW2-SW4)
static constexpr uint8_t SET_A_ENC_CC  = 16;
static constexpr uint8_t SET_A_BTN_CC  = 20;
static constexpr uint8_t SET_B_ENC_CC  = 24;
static constexpr uint8_t SET_B_BTN_CC  = 28;
static constexpr uint8_t TOGGLE_CC     = 23;

static uint8_t enc_cc_base = SET_A_ENC_CC;
static uint8_t btn_cc_base = SET_A_BTN_CC;
static bool set_b_active = false;

// LED state
enum LedState { LED_IDLE_BLINK, LED_CONNECT_FLASH, LED_OFF };
static LedState led_state = LED_IDLE_BLINK;
static uint16_t led_counter = 0;
static uint8_t led_flash_count = 0;
static bool was_connected = false;

static void led_set(bool on) {
    cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, on);
}

int main() {
    stdio_init_all();

    if (cyw43_arch_init()) {
        printf("CYW43 init failed\n");
        return 1;
    }
    printf("FractionalLooper: CYW43 initialized\n");

    // Persistent bonding storage — survives power cycles
    static btstack_tlv_flash_bank_t tlv_context;
    const hal_flash_bank_t *flash_bank = pico_flash_bank_instance();
    const btstack_tlv_t *tlv_impl = btstack_tlv_flash_bank_init_instance(
        &tlv_context, flash_bank, NULL);
    btstack_tlv_set_instance(tlv_impl, &tlv_context);
    le_device_db_tlv_configure(tlv_impl, &tlv_context);
    hci_set_link_key_db(btstack_link_key_db_tlv_get_instance(tlv_impl, &tlv_context));

    cs::BluetoothMIDI_Interface ble;
    ble.ble_settings.initiate_security = true;
    ble.begin();
    printf("BLE MIDI started\n");

    SPPStreamMIDI_Interface spp;
    spp.begin();

    cs::BidirectionalMIDI_PipeFactory<2> pipes;
    ble | pipes | spp;

    encoders_init();
    printf("Encoders initialized\n");

    ble.setAsDefault();

    while (true) {
        bool connected = ble.isConnected() || spp.isConnected();
        if (connected && !was_connected) {
            led_state = LED_CONNECT_FLASH;
            led_flash_count = 0;
            led_counter = 0;
        } else if (!connected && was_connected) {
            led_state = LED_IDLE_BLINK;
            led_counter = 0;
        }
        was_connected = connected;

        switch (led_state) {
        case LED_IDLE_BLINK:
            led_set(led_counter < 500);
            if (++led_counter >= 1000) led_counter = 0;
            break;
        case LED_CONNECT_FLASH:
            led_set(led_counter < 50);
            if (++led_counter >= 100) {
                led_counter = 0;
                if (++led_flash_count >= 6) {
                    led_state = LED_OFF;
                    led_set(false);
                }
            }
            break;
        case LED_OFF:
            break;
        }

        for (int i = 0; i < NUM_ENCODERS; i++) {
            int32_t delta = encoder_get_delta(i);
            if (delta != 0) {
                int32_t clamped = delta;
                if (clamped > 63) clamped = 63;
                if (clamped < -63) clamped = -63;
                uint8_t val = (clamped > 0) ? (uint8_t)clamped
                                            : (uint8_t)(128 + clamped);
                ble.sendControlChange({enc_cc_base + i, cs::Channel_1}, val);
            }

            if (button_pressed(i)) {
                if (i == 0) {
                    set_b_active = !set_b_active;
                    enc_cc_base = set_b_active ? SET_B_ENC_CC : SET_A_ENC_CC;
                    btn_cc_base = set_b_active ? SET_B_BTN_CC : SET_A_BTN_CC;
                    ble.sendControlChange({TOGGLE_CC, cs::Channel_1},
                                          set_b_active ? 127 : 0);
                } else {
                    ble.sendControlChange({btn_cc_base + (i - 1), cs::Channel_1}, 127);
                }
            }
        }

        ble.update();
        spp.update();

        sleep_ms(1);
    }
}