/*
 * mbed GPIO
 */

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#pragma GCC diagnostic ignored "-pedantic"
#include "mbed.h"
#pragma GCC diagnostic pop

extern "C" {
#include "spi.h"
}

static PinName numToPin(uint8_t num) {
    switch (num) {
      case 20:
        return p20;
      case 21:
        return p21;
      case 22:
        return p22;
      case 23:
        return p23;
      case 24:
        return p24;
      case 25:
        return p25;
      case 26:
        return p26;
    }

    return p5;
}

#define GPIO_MAX_COUNT 42
DigitalInOut *gpios[GPIO_MAX_COUNT];

extern "C" {

void gpioInit(void) {
    for (int i = 0; i < GPIO_MAX_COUNT; i++) {
        gpios[i] = NULL;
    }
}

void gpioDir(uint8_t num, uint8_t dir) {
    PinName n = numToPin(num);

    if (gpios[num] == NULL) {
        gpios[num] = new DigitalInOut(n);
    }

    if (dir == DIR_OUTPUT) {
        gpios[num]->output();
    } else {
        gpios[num]->input();
    }
}

void gpioSet(uint8_t num, uint8_t val) {
    PinName n = numToPin(num);

    if (gpios[num] == NULL) {
        gpios[num] = new DigitalInOut(n);
        gpios[num]->output();
    }

    gpios[num]->write(val);
}

uint8_t gpioGet(uint8_t num) {
    PinName n = numToPin(num);

    if (gpios[num] == NULL) {
        gpios[num] = new DigitalInOut(n);
        gpios[num]->input();
    }

    return gpios[num]->read();
}

}