#include "MidiDriver.h"
#include "config.h"

// MIDI UART Pins (GP0/GP1)
#define PIN_MIDI_TX 0
#define PIN_MIDI_RX 1

MidiDriver midi;

MidiDriver::MidiDriver() : lastInputNote(-1), lastInputVelocity(0), _runningStatus(0), _byteIndex(0), _data1(0), _data2(0) {
    memset(activeNotes, 0, sizeof(activeNotes));
}

void MidiDriver::begin() {
    mutex_init(&_mutex);
    Serial1.setTX(PIN_MIDI_TX);
    Serial1.setRX(PIN_MIDI_RX);
    Serial1.begin(31250);
    Serial.println(F("MIDI Serial initialized on GP0/GP1"));
}

void MidiDriver::update() {
    while (Serial1.available()) {
        uint8_t b = Serial1.read();
        Serial1.write(b); // Soft THRU: Merge input with output

        // Realtime messages don't affect running status
        if (b >= 0xF8) continue;

        if (b >= 0x80) {
            _runningStatus = b;
            _byteIndex = 0;
        } else if (_runningStatus) {
            if (_byteIndex == 0) {
                _data1 = b;
                _byteIndex++;
                
                // Handle 2-byte messages (Program Change 0xC0, Channel Pressure 0xD0)
                uint8_t type = _runningStatus & 0xF0;
                if (type == 0xC0 || type == 0xD0) {
                     _byteIndex = 0; // Message complete
                }
            } else if (_byteIndex == 1) {
                _data2 = b;
                _byteIndex = 0; // Message complete
                
                uint8_t channel = (_runningStatus & 0x0F) + 1;
                uint8_t type = _runningStatus & 0xF0;
                if (type == 0x90) {
                    if (_data2 > 0) {
                        // Serial.print(F("Note On CH"));
                        // Serial.print(channel);
                        // Serial.print(F(": "));
                        // Serial.print(_data1);
                        // Serial.print(F(" Vel: "));
                        // Serial.println(_data2);
                        lastInputNote = _data1;
                        lastInputVelocity = _data2;
                    } else {
                        // Serial.print(F("Note Off CH"));
                        // Serial.print(channel);
                        // Serial.print(F(": "));
                        // Serial.println(_data1);
                        if (lastInputNote == _data1) {
                            lastInputNote = -1; // Note On vel 0 is Note Off
                            lastInputVelocity = 0;
                        }
                    }
                } else if (type == 0x80) {
                    // Serial.print(F("Note Off CH"));
                    // Serial.print(channel);
                    // Serial.print(F(": "));
                    // Serial.println(_data1);
                    if (lastInputNote == _data1) {
                        lastInputNote = -1;
                        lastInputVelocity = 0;
                    }
                }
            }
        }
    }
}

void MidiDriver::lock() {
    mutex_enter_blocking(&_mutex);
}

void MidiDriver::unlock() {
    mutex_exit(&_mutex);
}

void MidiDriver::sendNoteOn(uint8_t note, uint8_t velocity, uint8_t channel) {
#ifdef MIDI_DEBUG
    Serial.print(F("["));
    Serial.print(millis());
    Serial.print(F("] "));
    Serial.print(F("MIDI OUT: Note On  CH:"));
    Serial.print(channel);
    Serial.print(F(" Note:"));
    Serial.print(note);
    Serial.print(F(" Vel:"));
    Serial.println(velocity);
#endif
    if (channel >= 1 && channel <= 16 && note < 128) {
        activeNotes[channel - 1][note] = (velocity > 0);
    }
    uint8_t status = 0x90 | (channel - 1);
    Serial1.write(status);
    Serial1.write(note);
    Serial1.write(velocity);
}

void MidiDriver::sendNoteOff(uint8_t note, uint8_t channel) {
#ifdef MIDI_DEBUG
    Serial.print(F("["));
    Serial.print(millis());
    Serial.print(F("] "));
    Serial.print(F("MIDI OUT: Note Off CH:"));
    Serial.print(channel);
    Serial.print(F(" Note:"));
    Serial.println(note);
#endif
    if (channel >= 1 && channel <= 16 && note < 128) {
        activeNotes[channel - 1][note] = false;
    }
    uint8_t status = 0x80 | (channel - 1);
    Serial1.write(status);
    Serial1.write(note);
    Serial1.write((uint8_t)0);
}

void MidiDriver::sendRealtime(uint8_t status) {
#ifdef MIDI_DEBUG
    if (status != 0xF8) {
        Serial.print(F("["));
        Serial.print(millis());
        Serial.print(F("] "));
        Serial.print(F("MIDI OUT: Realtime 0x"));
        Serial.println(status, HEX);
    }
#endif
    Serial1.write(status);
}

void MidiDriver::panic(uint8_t channel) {
#ifdef MIDI_DEBUG
    Serial.print(F("["));
    Serial.print(millis());
    Serial.print(F("] "));
    Serial.print(F("MIDI OUT: Panic    CH:"));
    Serial.println(channel);
#endif
    if (channel >= 1 && channel <= 16) {
        for (int i = 0; i < 128; i++) {
            if (activeNotes[channel - 1][i]) {
                sendNoteOff(i, channel);
            }
        }
    }
    uint8_t status = 0xB0 | (channel - 1);
    Serial1.write(status);
    Serial1.write((uint8_t)123); // All Notes Off
    Serial1.write((uint8_t)0);
}