#include "pico/stdlib.h"
#include "pico/util/queue.h"
#include "hardware/pwm.h"
#include "DmxOutput.h"

DmxOutput dmx_out; 

/* Tell C++ to treat these as C libraries */
extern "C" {
    #include "tusb.h"
}

#define LED_PIN 25
#define RS485_DE_RE_PIN 6
#define DMX_TX_PIN 4
#define FIFO_SIZE 64
#define DMX_UNIVERSE_SIZE 512

typedef struct {
    uint8_t channel;
    uint8_t note;
    uint8_t velocity;
} midi_note_msg_t;

queue_t midi_fifo;
uint8_t dmx_buffer[DMX_UNIVERSE_SIZE + 1]; /* +1 for the Start Code (0x00) */

void setup_hardware() {
    /* Initial GPIO Setup */
    gpio_init(LED_PIN);
    gpio_set_dir(LED_PIN, GPIO_OUT);
    gpio_init(RS485_DE_RE_PIN);
    gpio_set_dir(RS485_DE_RE_PIN, GPIO_OUT);

    /* Set GPIO6 - RS485 DE/RE pin high */
    gpio_put(RS485_DE_RE_PIN, 1);

    /* Perform 3 rapid blinks to signal a fresh boot */
    for (int i = 0; i < 3; i++) {
        gpio_put(LED_PIN, 1);
        sleep_ms(50);
        gpio_put(LED_PIN, 0);
        sleep_ms(50);
    }
  
    sleep_ms(200);

    /* PWM Setup for MIDI Dimming */
    gpio_set_function(LED_PIN, GPIO_FUNC_PWM);
    uint slice_num = pwm_gpio_to_slice_num(LED_PIN);
    pwm_set_wrap(slice_num, 127); 
    pwm_set_enabled(slice_num, true);

    /* Smooth "Welcome" fade-in and out */
    for (int i = 0; i <= 127; i++) {
        pwm_set_gpio_level(LED_PIN, i);
        sleep_ms(3);
    }
    for (int i = 127; i >= 0; i--) {
        pwm_set_gpio_level(LED_PIN, i);
        sleep_ms(3);
    }

    dmx_out.begin(DMX_TX_PIN); // Initialize DMX on GP6 using PIO
    for(int i=0; i<=DMX_UNIVERSE_SIZE; i++) dmx_buffer[i] = 0;

    /* Initialise FIFO */
    queue_init(&midi_fifo, sizeof(midi_note_msg_t), FIFO_SIZE);

    /* Reset DMX */
    dmx_buffer[0x01] = 0xFF; 
    dmx_buffer[0x02] = 0x00;
    dmx_buffer[0x03] = 0x00;
    dmx_buffer[0x04] = 0x00;
    dmx_out.write(dmx_buffer, DMX_UNIVERSE_SIZE + 1);
}

int main() {
    /* Call hardware setup (including splash) before USB init */
    setup_hardware();
    tusb_init();

    while (1) {
        tud_task();

        /* Capture (USB -> PWM LED -> FIFO) */
        if (tud_midi_available()) {
            uint8_t packet[4];
            if (tud_midi_packet_read(packet)) {
                uint8_t note_on_off = packet[1];
                uint8_t note = packet[2];
                uint8_t velocity = packet[3];

                /*If note on... */
                if ((note_on_off & 0xF0) == 0x90) {
                    /* Add to FIFO for DMX processing */
                    midi_note_msg_t msg = {
                        .channel = (uint8_t)((note_on_off & 0x0F) + 1),
                        .note = note,
                        .velocity = velocity
                    };
                    queue_try_add(&midi_fifo, &msg);
                }

                /*If note off... */
                if ((note_on_off & 0xF0) == 0x80) {
                    /* Add to FIFO for DMX processing */
                    midi_note_msg_t msg = {
                        .channel = (uint8_t)((note_on_off & 0x0F) + 1),
                        .note = note,
                        .velocity = 0x00
                    };
                    queue_try_add(&midi_fifo, &msg);
                }
            }
        }

        midi_note_msg_t next_msg;
        if (queue_try_remove(&midi_fifo, &next_msg)) {
            /* Map MIDI note Number to DMX Channel */
            /* Centred around middle C (0x3C) */
            next_msg.note = next_msg.note - 0x3C;

            if (next_msg.note < DMX_UNIVERSE_SIZE) {
                /* MIDI values are 0-127, DMX values are 0-255 */
                /* Shift to scale the brightness */
                dmx_buffer[next_msg.note] = next_msg.velocity << 1; 
            }
        }

        if (!dmx_out.busy()) {
            dmx_out.write(dmx_buffer, DMX_UNIVERSE_SIZE + 1);
        }
    }
    return 0;
}