// Loopy MIDI Controller - Phase 1
// ESP32-S3 USB MIDI Foot Controller

#include <Arduino.h>
#include <driver/gpio.h>
#include <esp_rom_sys.h>
#include "midi_transport.h"
#include "Adafruit_TinyUSB.h"
#include "pixel_stomp_mux.h"
#include "led_stub.h"
#include "switch_stub.h"
#include "app_task.h"

PixelStompMux mux(12, 10, 11, 9);

DefaultLedStub led_driver;
DefaultSwitchStub switch_driver;
UsbMidiTransport midi_transport;

AppTask controller(&led_driver, &switch_driver, &midi_transport);

TaskHandle_t midi_task_handle = NULL;

void midi_task(void* parameter) {
    Serial.println("[TASK] MIDI task started on core 0");
    
    while (true) {
        midi_transport.update();
        vTaskDelay(1);
    }
}

void handle_serial_command(const String& cmd) {
    if (cmd == "dump" || cmd == "d") {
        mux.dump();
    } else if (cmd == "probe" || cmd == "p") {
        mux.probe();
    } else if (cmd == "ledon") {
        for (int i = 0; i < PixelStompMux::NUM_LEDS; i++) {
            mux.set_led_color(i, 255, 255, 255);
        }
        mux.show();
        Serial.println("[CMD] All LEDs WHITE");
    } else if (cmd == "ledoff") {
        mux.clear_all();
    } else if (cmd == "ledtest") {
        uint32_t colors[] = {0xFF0000, 0x00FF00, 0x0000FF, 0xFFFF00, 0xFF00FF, 0x00FFFF};
        for (int c = 0; c < 6; c++) {
            uint8_t r = (colors[c] >> 16) & 0xFF;
            uint8_t g = (colors[c] >> 8) & 0xFF;
            uint8_t b = colors[c] & 0xFF;
            for (int i = 0; i < PixelStompMux::NUM_LEDS; i++) {
                mux.set_led_color(i, r, g, b);
            }
            mux.show();
            delay(300);
        }
        mux.clear_all();
        Serial.println("[CMD] LED test complete");
    } else if (cmd == "read") {
        uint16_t raw = mux.read_buttons();
        Serial.printf("[CMD] Raw: 0x%04X (", raw);
        for (int i = 15; i >= 0; i--) {
            Serial.print((raw >> i) & 1);
        }
        Serial.println(")");
    } else if (cmd == "red") {
        for (int i = 0; i < PixelStompMux::NUM_LEDS; i++) mux.set_led_color(i, 255, 0, 0);
        mux.show();
    } else if (cmd == "green") {
        for (int i = 0; i < PixelStompMux::NUM_LEDS; i++) mux.set_led_color(i, 0, 255, 0);
        mux.show();
    } else if (cmd == "blue") {
        for (int i = 0; i < PixelStompMux::NUM_LEDS; i++) mux.set_led_color(i, 0, 0, 255);
        mux.show();
    } else if (cmd == "pixel0") {
        mux.set_led_brightness(255);
        mux.set_led_color(0, 255, 255, 255);
        mux.show();
        Serial.println("[CMD] Pixel 0 WHITE (max brightness)");
    } else if (cmd == "pixel1") {
        mux.set_led_brightness(255);
        mux.set_led_color(1, 255, 255, 255);
        mux.show();
        Serial.println("[CMD] Pixel 1 WHITE (max brightness)");
    } else if (cmd == "usb") {
        Serial.printf("[CMD] USB mounted: %s\n", TinyUSBDevice.mounted() ? "YES" : "NO");
        Serial.printf("[CMD] USB ready: %s\n", TinyUSBDevice.ready() ? "YES" : "NO");
    } else if (cmd == "gpiotest") {
        Serial.println("[CMD] === Raw GPIO Test ===");
        uint8_t pins[] = {9, 10, 11, 12};
        const char* names[] = {"DAT(9)", "LD(10)", "CLK(11)", "DI(12)"};
        for (int p = 0; p < 4; p++) {
            uint8_t pin = pins[p];
            pinMode(pin, INPUT_PULLUP);
            delay(10);
            int hi = digitalRead(pin);
            pinMode(pin, INPUT_PULLDOWN);
            delay(10);
            int lo = digitalRead(pin);
            pinMode(pin, INPUT);
            delay(10);
            int fl = digitalRead(pin);
            Serial.printf("  %s: PULLUP=%d PULLDOWN=%d FLOAT=%d\n", names[p], hi, lo, fl);
        }
        Serial.println("  Testing WS2812(12) output toggle...");
        pinMode(12, OUTPUT);
        for (int i = 0; i < 5; i++) {
            digitalWrite(12, HIGH);
            delayMicroseconds(100);
            int readback = digitalRead(9);
            Serial.printf("    HIGH -> readback=%d\n", readback);
            digitalWrite(9, LOW);
            delayMicroseconds(100);
            readback = digitalRead(9);
            Serial.printf("    LOW  -> readback=%d\n", readback);
        }
        Serial.println("[CMD] === GPIO Test Complete ===");
    } else if (cmd == "help") {
        Serial.println("[CMD] Commands:");
        Serial.println("  dump    - show button states");
        Serial.println("  probe   - hardware diagnostic");
        Serial.println("  ledon   - all LEDs white");
        Serial.println("  ledoff  - all LEDs off");
        Serial.println("  ledtest - colour cycle");
        Serial.println("  read    - raw button read");
        Serial.println("  red/green/blue - solid colour");
        Serial.println("  pixel0/pixel1 - single pixel test");
        Serial.println("  usb      - USB connection status and descriptor info");
        Serial.println("  gpiotest - raw GPIO pin diagnostic");
        Serial.println("  rawled   - bit-bang WS2812 (no library)");
        Serial.println("  anim     - re-run startup animation from loop");
    } else if (cmd == "anim") {
        Serial.println("[CMD] Running animation...");
        uint32_t colors[] = {0xFF0000, 0x00FF00, 0x0000FF, 0xFFFF00, 0xFF00FF, 0x00FFFF, 0xFFFFFF};
        for (int c = 0; c < 7; c++) {
            uint8_t r = (colors[c] >> 16) & 0xFF;
            uint8_t g = (colors[c] >> 8) & 0xFF;
            uint8_t b = colors[c] & 0xFF;
            Serial.printf("  Colour %d: #%06X\n", c, colors[c]);
            for (int i = 0; i < PixelStompMux::NUM_LEDS; i++) mux.set_led_color(i, r, g, b);
            mux.show();
            delay(500);
        }
        mux.clear_all();
        Serial.println("[CMD] Animation done");
    } else if (cmd == "rawled") {
        uint8_t pin = 12;
        Serial.println("[CMD] Bit-bang WS2812: pixel 0 = RED on GPIO 9");
        uint8_t colour[] = {0x00, 0xFF, 0x00};
        noInterrupts();
        for (int byte_idx = 0; byte_idx < 3; byte_idx++) {
            uint8_t val = colour[byte_idx];
            for (int bit = 7; bit >= 0; bit--) {
                gpio_set_level((gpio_num_t)pin, 1);
                if (val & (1 << bit)) {
                    esp_rom_delay_us(1);
                }
                gpio_set_level((gpio_num_t)pin, 0);
                if (!(val & (1 << bit))) {
                    esp_rom_delay_us(1);
                }
            }
        }
        gpio_set_level((gpio_num_t)pin, 0);
        esp_rom_delay_us(50);
        interrupts();
        Serial.println("[CMD] Sent. Wait 2 sec...");
        delay(2000);
        Serial.println("[CMD] Done");
    } else {
        Serial.println("[CMD] Unknown command. Type 'help'");
    }
}

void setup() {
    Serial.begin(115200);
    delay(2000);
    
    Serial.println("=================================");
    Serial.println("  Loopy MIDI Controller v0.1");
    Serial.println("  Phase 1: USB MIDI");
    Serial.println("  Board: ESP32-S3-WROOM-1");
    Serial.println("=================================");
    
    Serial.println("[INIT] Initializing PixelStomp MUX...");
    mux.begin();
    
    Serial.println("[INIT] Starting LED startup animation...");
    led_driver.set_mux(&mux);
    led_driver.begin();
    
    Serial.println("[INIT] Initializing switches via MUX...");
    switch_driver.set_mux(&mux);
    switch_driver.begin();
    
    Serial.println("[INIT] Initializing USB MIDI...");
    midi_transport.begin();
    
    Serial.println("[INIT] Registering MIDI callbacks...");
    controller.begin();
    
    xTaskCreatePinnedToCore(
        midi_task,
        "midi_task",
        4096,
        NULL,
        3,
        &midi_task_handle,
        0
    );
    
    Serial.println("=================================");
    Serial.println("  All systems ready");
    Serial.println("  Type 'help' for diagnostics");
    Serial.println("=================================");
    Serial.flush();
}

void loop() {
    controller.update();

    if (Serial.available()) {
        String cmd = Serial.readStringUntil('\n');
        cmd.trim();
        cmd.toLowerCase();
        handle_serial_command(cmd);
    }

    delay(10);
}