#include "mbed.h"

#define LED PF_1
#define ADC_1 PA_0
#define ADC_2 PA_1
#define ADC_6 PA_6
#define ADC_7 PA_7
#define ADC_8 PB_0
#define ADC_9 PB_1

#define DAC_1 PA_4
#define DAC_2 PA_5

DigitalOut led(LED);

BufferedSerial serial(PB_10, PB_11);

I2C i2c(PB_7, PB_6);

AnalogIn adc1(ADC_1);
AnalogIn adc2(ADC_2);
AnalogIn adc6(ADC_6);
AnalogIn adc7(ADC_7);
AnalogIn adc8(ADC_8);
AnalogIn adc9(ADC_9);

AnalogOut dac1(DAC_1);
AnalogOut dac2(DAC_2);

FileHandle *mbed::mbed_override_console(int fd)
{
    return &serial;
}

const int destaddr_write = 0x38;
const int destaddr_read = 0x3A;

const int writecmd = 0x00;
const int readcmd = 0xFF;

Thread pollingThread;
Mutex buttonMutex;
const unsigned char button[4] = {8,4,2,1};
bool buttonPressed[4] = {false};

void keyscan() {
    int rc;
    char cmd[2];
    char data_read;

    while(true) {
        cmd[0] = 0x00;
        rc = i2c.write(destaddr_read, &cmd[0], 1);
        rc = i2c.read(destaddr_read, &data_read, 1);
        if (rc != 0) {
            printf("READ FAILED\r\n");
        }

        for (int i=0; i<4; i++) {
            if ((~data_read & button[i]) == button[i]) {
                buttonPressed[i] = true;
                ThisThread::flags_wait_any(0x01, true);
            }
        }
    }
}

void i2c_init() {
    char cmd[2];

    cmd[0] = 0x03;
    cmd[1] = 0xFF;
    i2c.write(destaddr_read, cmd, 2);

    cmd[0] = 0x02;
    cmd[1] = 0x00;
    i2c.write(destaddr_read, cmd, 2);

    cmd[0] = 0x03;
    cmd[1] = 0x00;
    i2c.write(destaddr_write, cmd, 2);

    cmd[0] = 0x02;
    cmd[1] = 0x00;
    i2c.write(destaddr_write, cmd, 2);
}

int main() {
    int rc;
    char cmd[2];
    char data_write = 1;
    pollingThread.start(keyscan);

    i2c_init();

    while(true) {
        // Cycle Interface Board LED
        data_write = (data_write << 1) % 255;
        cmd[0] = 0x01;
        cmd[1] = data_write;
        rc = i2c.write(destaddr_write, cmd, 2);
        if (rc != 0) {
            printf("WRITE FAILED\r\n");
        }

        ThisThread::sleep_for(500);
    }
}