#include <cstdio>
#include "pico/stdlib.h"
#include "pico/cyw43_arch.h"
#include "hardware/adc.h"
#include <cs_midi.h>
#include "spp_midi.h"

using namespace cs;

// SPP constructed first — its begin() registers Classic BT services before BLE starts HCI
SPPStreamMIDI_Interface spp_midi;
BluetoothMIDI_Interface ble_midi;

// State toggle on encoder 1's button — fixed CC, sends 127/0 to indicate active map
CCButtonLatched toggle_btn {1, {80, Channel_1}};

// Encoders (A/B swapped on hardware)
CCRotaryEncoder enc1 {{2, 0},  {16, Channel_1}, 1, 4};
CCRotaryEncoder enc2 {{5, 3},  {17, Channel_1}, 1, 4};
CCRotaryEncoder enc3 {{8, 6},  {18, Channel_1}, 1, 4};
CCRotaryEncoder enc4 {{9, 11}, {19, Channel_1}, 1, 4};

// Remaining encoder push buttons
CCButton btn2 {4,  {20, Channel_1}};
CCButton btn3 {7,  {21, Channel_1}};
CCButton btn4 {10, {22, Channel_1}};

// CC mappings per state
struct CCMap {
    uint8_t enc[4];
    uint8_t btn[3];
};

constexpr CCMap maps[2] = {
    {{16, 17, 18, 19}, {20, 21, 22}},  // state 0
    {{23, 24, 25, 26}, {27, 28, 29}},  // state 1
};

void apply_map(const CCMap &m) {
    enc1.setAddress({m.enc[0], Channel_1});
    enc2.setAddress({m.enc[1], Channel_1});
    enc3.setAddress({m.enc[2], Channel_1});
    enc4.setAddress({m.enc[3], Channel_1});
    btn2.setAddressUnsafe({m.btn[0], Channel_1});
    btn3.setAddressUnsafe({m.btn[1], Channel_1});
    btn4.setAddressUnsafe({m.btn[2], Channel_1});
}

constexpr float BATT_V_MIN = 3.0f;
constexpr float BATT_V_MAX = 4.2f;
constexpr uint32_t BATT_INTERVAL_MS = 60000;
constexpr uint32_t BATT_SETTLE_MS  = 200;
constexpr uint BATT_SINK_PIN = 41;
constexpr uint BATT_ADC_INPUT = 2; // GP42 = ADC2

enum BattState { BATT_IDLE, BATT_SETTLING };
BattState batt_state = BATT_IDLE;
uint32_t  batt_timer = 0;

void batt_sink_enable() {
    gpio_put(BATT_SINK_PIN, 0);
    gpio_set_dir(BATT_SINK_PIN, GPIO_OUT);
}

void batt_sink_float() {
    gpio_set_dir(BATT_SINK_PIN, GPIO_IN);
    gpio_disable_pulls(BATT_SINK_PIN);
}

uint8_t read_battery_percent() {
    adc_select_input(BATT_ADC_INPUT);
    uint16_t raw = adc_read();
    float voltage = raw * (3.3f / 4096.0f) * 2.0f;
    float pct = (voltage - BATT_V_MIN) / (BATT_V_MAX - BATT_V_MIN) * 100.0f;
    if (pct < 0.0f) pct = 0.0f;
    if (pct > 100.0f) pct = 100.0f;
    return static_cast<uint8_t>(pct);
}

int main() {
    stdio_init_all();
    if (cyw43_arch_init()) {
        printf("CYW43 init failed\n");
        return 1;
    }

    adc_init();
    adc_gpio_init(42);
    gpio_init(BATT_SINK_PIN);
    batt_sink_float();

    RelativeCCSender::setMode(relativeCCmode::MACKIE_CONTROL_RELATIVE);
    ble_midi.setName("FractionalLooper");
    Control_Surface.begin();

    bool last_state = false;

    while (true) {
        Control_Surface.loop();

        bool state = toggle_btn.getState();
        if (state != last_state) {
            last_state = state;
            apply_map(maps[state]);
        }

        uint32_t now = to_ms_since_boot(get_absolute_time());
        switch (batt_state) {
        case BATT_IDLE:
            if (now - batt_timer >= BATT_INTERVAL_MS) {
                batt_sink_enable();
                batt_timer = now;
                batt_state = BATT_SETTLING;
            }
            break;
        case BATT_SETTLING:
            if (now - batt_timer >= BATT_SETTLE_MS) {
                setBLEBatteryLevel(read_battery_percent());
                batt_sink_float();
                batt_timer = now;
                batt_state = BATT_IDLE;
            }
            break;
        }

        sleep_ms(1);
    }
}