#include "mbed.h"
#include "midiswitch.h"

SwitchHandler::SwitchHandler(EventQueue * queue, MIDI_CB cb, SwitchConfig sw) :
    queue(queue), write_cb(cb), btn(sw.pin), cfg(sw) {
    btn.mode(PullUp);
    btn.setAssertValue(0);
    btn.attach_asserted(this, &SwitchHandler::handle_pressed);
    btn.attach_deasserted(this, &SwitchHandler::handle_released);
    btn.setSampleFrequency();
};

void SwitchHandler::handle_pressed(void) {
    if (cfg.on_value >= 0) {
        if (make_message(true)) {
            queue->call(write_cb, msg);
        }
    }
};

void SwitchHandler::handle_released(void) {
    if (cfg.off_value >= 0) {
        if (make_message(false)) {
            queue->call(write_cb, msg);
        }
    }
};

bool SwitchHandler::make_message(bool onoff) {
    uint8_t value = (onoff ? cfg.on_value : cfg.off_value) & 0x7F;
    msg.data[0] = CABLE_NUM | cfg.type;
    msg.data[1] = cfg.type;

    switch(cfg.type) {
        case 0xFA:  // START
        case 0xFB:  // CONTINUE
        case 0xFC:  // STOP
            msg.data[2] = 0x00;
            msg.data[3] = 0x00;
            break;
        case 0xC0:  // Program change
        case 0xD0:  // Channel Aftertouch
            msg.data[1] = msg.data[1] | (cfg.channel & 0x0F);
            msg.data[2] = value;
            msg.data[3] = 0x00;
            break;
        case 0x80:  // Note off
        case 0x90:  // Note on
            msg.data[1] = msg.data[1] | (cfg.channel & 0x0F);
            msg.data[3] = value;
            msg.data[2] = cfg.data1;
            break;
        case 0xA0:  // Poly pressure
        case 0xB0:  // Control change
        case 0xE0:  // PitchBend
            msg.data[1] = msg.data[1] | (cfg.channel & 0x0F);
            msg.data[2] = cfg.data1;
            msg.data[3] = value;
            break;
        default:
            return false;
    }
    return true;
};