#include "mbed.h"
#include "USBMIDI.h"

USBMIDI midi(false);
DigitalOut  ledGreen(LED1);
DigitalOut  ledBlue(LED2);
DigitalOut  ledRed(LED3);
Mail<MIDIMessage, 64> mboxMIDIMessages;
uint32_t    messageCount;
uint32_t    dropCount;

#define LED_ON  (0)
#define LED_OFF (1)


static MIDIMessage dropBuffer;

void putMIDIMessage() {
    while (midi.readable()) {
        MIDIMessage* midiIn = mboxMIDIMessages.try_alloc();
        if (midiIn != nullptr) {            // success, read and queue midi message
            midi.read(midiIn);
            mboxMIDIMessages.put(midiIn);
        } else {                            // queue full, drop message
            midi.read(&dropBuffer);
            dropCount++;
        }
    }
}

int main()
{
    //sleep_manager_lock_deep_sleep();
    
    printf("Hello from "  MBED_STRINGIFY(TARGET_NAME) "\n");
    printf("Mbed OS version: %d.%d.%d\n\n", MBED_MAJOR_VERSION, MBED_MINOR_VERSION, MBED_PATCH_VERSION);

    midi.connect();
    midi.attach(putMIDIMessage);         // call back for messages received    
    
    MIDIMessage* midiIn;
    while (1) {
        midiIn = mboxMIDIMessages.try_get_for(rtos::Kernel::wait_for_u32_forever);
        if (midiIn == nullptr) {
            continue;
        }
        messageCount++;

        MIDIMessage::MIDIMessageType mt = midiIn->type();
        switch(mt) {
            case MIDIMessage::MIDIMessageType::NoteOnType: {
                printf("#%ld dropped: %ld received NoteOn: key %d\n", messageCount, dropCount, midiIn->key());
                ledGreen = LED_ON;
                break;
            }
            case MIDIMessage::MIDIMessageType::NoteOffType: {
                printf("#%ld received NoteOff: key %d\n", messageCount, midiIn->key());
                ledGreen = LED_OFF;
                break;
            }
            case MIDIMessage::MIDIMessageType::ControlChangeType: {
                printf("#%ld received ControlChangeType: value %d\n", messageCount, midiIn->value());
                ledBlue = !ledBlue;
                break;
            }
            default:
                printf("#%ld received other: type %d\n", messageCount, mt);
                ledRed = !ledRed;
                break;
        }

        mboxMIDIMessages.free(midiIn);
    }
}