#include <Arduino.h>
#include <Control_Surface.h>

// ---------------- USB MIDI ----------------
USBMIDI_Interface midi;

// ---------------- UART FROM ESP32 #1 ----------------
HardwareSerial MidiInSerial(1);

// ---------------- MIDI PARSER STATE ----------------
uint8_t statusByte = 0;
uint8_t data1 = 0;
bool waitingForData2 = false;

void setup() {
    Serial.begin(115200);                          // Debug output
    MidiInSerial.begin(1000000, SERIAL_8N1, 16, 17); // RX=16, TX=17
    midi.begin();                                  // Initialize USB MIDI interface
    Serial.println("ESP32 MIDI Receiver started");
}

// ---------------- HANDLE MIDI ----------------
void handleMIDI(uint8_t status, uint8_t d1, uint8_t d2) {
    uint8_t type = status & 0xF0;
    uint8_t channel = status & 0x0F; // 0–15

    // Debug parsed MIDI
    Serial.printf("Parsed MIDI - Status: 0x%02X, Data1: %d, Data2: %d, Channel: %d\n",
                  status, d1, d2, channel);

    switch (type) {
        case 0x90: { // Note On
            MIDIAddress note{d1, Channel(channel)};
            if (d2 == 0) {
                midi.sendNoteOff(note, 0);
                Serial.printf("Note Off (velocity 0) -> note %d\n", d1);
            } else {
                midi.sendNoteOn(note, d2);
                Serial.printf("Note On -> note %d, vel %d\n", d1, d2);
            }
            break;
        }
        case 0x80: { // Note Off
            MIDIAddress note{d1, Channel(channel)};
            midi.sendNoteOff(note, d2);
            Serial.printf("Note Off -> note %d, vel %d\n", d1, d2);
            break;
        }
        case 0xB0: { // Control Change
            MIDIAddress cc{d1, Channel(channel)};
            midi.sendControlChange(cc, d2);
            Serial.printf("Control Change -> CC %d, value %d\n", d1, d2);
            break;
        }
        case 0xE0: { // Pitch Bend
            uint16_t value = (d2 << 7) | d1; // 0–16383
            midi.sendPitchBend(MIDIChannelCable{Channel(channel)}, value);
            Serial.printf("Pitch Bend -> value %d\n", value);
            break;
        }
        default:
            Serial.printf("Other MIDI - Status: 0x%02X, Data1: %d, Data2: %d\n",
                          status, d1, d2);
            break;
    }

    midi.update(); // Send all MIDI messages to USB
}

// ---------------- MAIN LOOP ----------------
void loop() {
    while (MidiInSerial.available()) {
        uint8_t b = MidiInSerial.read();
        Serial.printf("Raw byte: 0x%02X\n", b); // Debug raw RX

        if (b & 0x80) {           // Status byte
            statusByte = b;
            waitingForData2 = false;
        } else if (!waitingForData2) {
            data1 = b;
            waitingForData2 = true;
        } else {
            handleMIDI(statusByte, data1, b);
            waitingForData2 = false;
        }
    }
}
