#include "BricktronicsDriver_wrapper.h"
#include "BricktronicsMotor.h"
#include "BricktronicsMegashield.h"


BricktronicsMotor* M;

extern "C" void* LegoMotor_Init(uint8_T port) {
    //param = port;
    BricktronicsMotor* m;
    switch(port) {
        case 2 :
            m = new BricktronicsMotor(BricktronicsMegashield::MOTOR_2);
            break;
        case 3 :
            m = new BricktronicsMotor(BricktronicsMegashield::MOTOR_3);
            break;
        case 4 :
            m = new BricktronicsMotor(BricktronicsMegashield::MOTOR_4);
            break;
        case 5 :
            m = new BricktronicsMotor(BricktronicsMegashield::MOTOR_5);
            break;
        case 6 :
            m = new BricktronicsMotor(BricktronicsMegashield::MOTOR_6);
            break;
        default :
            m = new BricktronicsMotor(BricktronicsMegashield::MOTOR_1);

    }

// Pokud by slo tohle vyresit inteligentneji nez pres case, bylo by to supr

    m->begin();
    return m;
}
extern "C" void LegoMotor_SetSpeed(void* ptr,int16_T speed) {
    BricktronicsMotor* m = (BricktronicsMotor*) ptr;
    m->setFixedDrive(speed);
}

extern "C" void LegoMotor_Terminate(void* ptr) {
    BricktronicsMotor* m = (BricktronicsMotor*) ptr;
    m->setFixedDrive(0);
    delete m;
}