#include "USBMIDI_Interface.hpp"

BEGIN_CS_NAMESPACE

void USBMIDI_Interface::update() {
    tud_task();
    MIDI_Interface::updateIncoming(this);
}

MIDIReadEvent USBMIDI_Interface::read() {
    return parser.pull(TinyUSBPuller{cableNum});
}

ChannelMessage USBMIDI_Interface::getChannelMessage() const {
    return parser.getChannelMessage();
}

SysCommonMessage USBMIDI_Interface::getSysCommonMessage() const {
    return parser.getSysCommonMessage();
}

RealTimeMessage USBMIDI_Interface::getRealTimeMessage() const {
    return parser.getRealTimeMessage();
}

SysExMessage USBMIDI_Interface::getSysExMessage() const {
    return parser.getSysExMessage();
}

// Map MIDI status high nibble to USB MIDI Code Index Number
MIDICodeIndexNumber USBMIDI_Interface::CIN(uint8_t status) {
    if (status >= 0x80 && status < 0xF0)
        return static_cast<MIDICodeIndexNumber>((status >> 4) & 0x0F);
    switch (status) {
        case 0xF1: // MTC Quarter Frame
        case 0xF3: // Song Select
            return MIDICodeIndexNumber::SystemCommon2B;
        case 0xF2: // Song Position Pointer
            return MIDICodeIndexNumber::SystemCommon3B;
        case 0xF6: // Tune Request
        case 0xF7: // SysEx End (bare)
            return MIDICodeIndexNumber::SystemCommon1B;
        default:
            if (status >= 0xF8)
                return MIDICodeIndexNumber::SingleByte;
            return MIDICodeIndexNumber::MiscFunctionCodes;
    }
}

void USBMIDI_Interface::writePacket(uint8_t cin, uint8_t b0,
                                     uint8_t b1, uint8_t b2) {
    uint8_t packet[4] = {
        static_cast<uint8_t>((cableNum << 4) | (cin & 0x0F)),
        b0, b1, b2
    };
    tud_midi_n_packet_write(cableNum, packet);
}

void USBMIDI_Interface::sendChannelMessageImpl(ChannelMessage msg) {
    auto cin = CIN(msg.header);
    if (msg.hasTwoDataBytes())
        writePacket(uint8_t(cin), msg.header, msg.data1, msg.data2);
    else
        writePacket(uint8_t(cin), msg.header, msg.data1, 0);
}

void USBMIDI_Interface::sendSysCommonImpl(SysCommonMessage msg) {
    auto cin = CIN(msg.header);
    uint8_t ndata = msg.getNumberOfDataBytes();
    if (ndata == 2)
        writePacket(uint8_t(cin), msg.header, msg.data1, msg.data2);
    else if (ndata == 1)
        writePacket(uint8_t(cin), msg.header, msg.data1, 0);
    else
        writePacket(uint8_t(cin), msg.header, 0, 0);
}

void USBMIDI_Interface::sendSysExImpl(SysExMessage msg) {
    const uint8_t *data = msg.data;
    uint16_t remaining = msg.length;

    while (remaining > 3) {
        writePacket(uint8_t(MIDICodeIndexNumber::SysExStartCont),
                    data[0], data[1], data[2]);
        data += 3;
        remaining -= 3;
    }

    switch (remaining) {
        case 3:
            writePacket(uint8_t(MIDICodeIndexNumber::SysExEnd3B),
                        data[0], data[1], data[2]);
            break;
        case 2:
            writePacket(uint8_t(MIDICodeIndexNumber::SysExEnd2B),
                        data[0], data[1], 0);
            break;
        case 1:
            writePacket(uint8_t(MIDICodeIndexNumber::SysExEnd1B),
                        data[0], 0, 0);
            break;
        default:
            break;
    }
}

void USBMIDI_Interface::sendRealTimeImpl(RealTimeMessage msg) {
    writePacket(uint8_t(MIDICodeIndexNumber::SingleByte),
                msg.message, 0, 0);
}

END_CS_NAMESPACE