#include "mbed.h"

#define LED1    (1 << 18)
#define LED2  (1 << 20)
#define LED3  (1 << 21)
#define LED4    (1 << 23)

#define pin21 (1 << 5)
#define pin22 (1 << 4)
#define pin23 (1 << 3)

#define SBIT_WordLength 0x00u
#define SBIT_DLAB       0x07u
#define SBIT_FIFO       0x00u
#define SBIT_RxFIFO     0x01u
#define SBIT_TxFIFO     0x02u
#define SBIT_THRE       0x05u

#define CTRL_REG1 0x2A
#define CTRL_REG1_ACTIVE 0x01
#define UINT14_MAX 16383
#define MMA8452_I2C_ADDRESS (0x1d<<1)

unsigned int* pinsel0           = reinterpret_cast<unsigned int*>(0x4002C000);
unsigned int* pinsel4           = reinterpret_cast<unsigned int*>(0x4002C010);
unsigned int* pinmode0          = reinterpret_cast<unsigned int*>(0x4002C040);
unsigned int* pinmode4          = reinterpret_cast<unsigned int*>(0x4002C050);

unsigned int* fio2dir0          = reinterpret_cast<unsigned int*>(0x2009C040);

unsigned int* io2intenr         = reinterpret_cast<unsigned int*>(0x400280B0);
unsigned int* io2intstatr   = reinterpret_cast<unsigned int*>(0x400280A4);
unsigned int* io2intclr         = reinterpret_cast<unsigned int*>(0x400280AC);

unsigned int* uart1FCR          = reinterpret_cast<unsigned int*>(0x40010008);
unsigned int* uart1LCR          = reinterpret_cast<unsigned int*>(0x4001000C);
unsigned int* uart1DLL          = reinterpret_cast<unsigned int*>(0x40010000);
unsigned int* uart1DLM          = reinterpret_cast<unsigned int*>(0x40010004);
unsigned int* uart1THR          = reinterpret_cast<unsigned int*>(0x40010000);

int cijfer[12] = {63,6,91,79,102,109,124,7,127,103,64,0};

I2C i2c(p28, p27);

int last_display;
int last_brightness = 50;
int _count;
int angles[3];
DigitalIn in(p22);
int ref_angle = 0;

class Data {
    public:
        Data(int pin) : _input(pin) { }
        ~Data() { }

        void push() {
            char tmp = in.read();
            _trame = _trame << 1;
            _trame = _trame | tmp;
        }
        char get_trame() {
            char tmp = _trame;
            _trame= 0x00;
            return tmp;
        }

        bool validate() {
            int parity = _trame & 1;
            int ones =0;
            for (int i =0;i<5;i++){
                if (_trame & 1<< i)
                    ones++;
            }
            if ( ( (parity == 1) && (ones % 2 == 0) ) || ((parity == 0 ) && (ones % 2 != 0)) ){
                return true;
            } else {
                return false;
            }
        }
    private:
        volatile char _trame;
        int _input;
};

void waitDataCleared() {
  while((*uart1LCR & (~(1<<SBIT_THRE))) == 0) { wait(0.000001); }
}

void sendCharUart1(char data) {
  waitDataCleared();
  *uart1THR = data;
}

void sendIntUart1(int data) {
  waitDataCleared();
  *uart1THR = (data & 0xFF);
}

void writeInt(int value) {

    int x1,x2,x3,x4;

    if (value < 0) {
        value = value * -1;

        x1 = value%10;
        x2 = value%100/10;
        x3 = 10;
        x4 = 11;
    }
    else {
        x1 = value%10;
        x2 = value%100/10;
        x3 = 11;
        x4 = 11;
    }

    sendCharUart1(0x7B);
    wait(0.01);
    sendCharUart1(cijfer[x4]);
    wait(0.01);
    sendCharUart1(0x7C);
    wait(0.01);
    sendCharUart1(cijfer[x3]);
    wait(0.01);
    sendCharUart1(0x7D);
    wait(0.01);
    sendCharUart1(cijfer[x2]);
    wait(0.01);
    sendCharUart1(0x7E);
    wait(0.01);
    sendCharUart1(cijfer[x1]);
    wait(0.01);
}

Data _packet(pin22);

void runCmd(int trame) {

    int cmdBit = trame & 0b10000;
    cmdBit = cmdBit >>4;
    int cmd = trame & 0b01110;
    cmd = cmd >>1;
    int mask = 0b111;
    int dec = 0b000000; //afficheur de decimaux

    if (cmd==5){
        if(cmdBit ==1){
            last_brightness +=10;
        } else {
            last_brightness -= 10;
        }
        sendCharUart1(0x7A);
        wait(0.01);
        sendCharUart1(last_brightness);
        wait(0.01);
        return;
    }
    if (cmdBit) {
        dec = 1<<(cmd);
        dec = last_display | dec;
        last_display = dec;
    } else {
        dec = 1<<(cmd);
        dec = last_display & ~dec;
        last_display = dec;
    }

    writeInt(angles[0] - ref_angle);
    sendCharUart1(0x77);
    wait(0.01);
    sendIntUart1(dec);
    wait(0.01);
}

void EINT3_init (void) {
    *pinsel4 &= ~((1 << 7) | (1 << 6));
    *pinmode4 |= (1 << 7) | (1 << 6);
    *pinsel4 &= ~((1 << 9) | (1 << 8));
    *pinmode4 |= (1 << 9) | (1 << 8);
    *pinsel4 &= ~((1 << 11) | (1 << 10));
    *pinmode4 |= (1 << 11) | (1 << 10);

    *fio2dir0 &= ~pin21;
    *fio2dir0 &= ~pin22;
    *fio2dir0 &= ~pin23;

    *io2intenr |= (pin21);
    *io2intenr |= (pin23);

    NVIC_EnableIRQ(EINT3_IRQn);
}

void updateAngleRef() {
    ref_angle = angles[0];
}

//EINT3 Handler
extern "C" void EINT3_IRQHandler(void) __irq {
    if ((*io2intstatr & pin21) == pin21) {
        *io2intclr = (pin21);

        _count++;
        if (_count >1 && _count <7) {
            _packet.push();
        } else if (_count == 7) {
            _count = 0;
            runCmd(_packet.get_trame());
        }

        return;
    }

    if ((*io2intstatr & pin23) == pin23) {
        *io2intclr = pin23;

        LPC_GPIO1->FIOSET |= LED3;

        updateAngleRef();
        wait(0.01);
        LPC_GPIO1->FIOCLR |= LED3;

        return;
    }
}

void initUart1(unsigned int baudrate) {

  *pinsel0  &= ~0xF0000000;
  *pinsel0  |=  0x40000000;
  *pinmode0 |=  0xC0000000;

  *uart1FCR = (1<<SBIT_FIFO) | (1<<SBIT_RxFIFO) | (1<<SBIT_TxFIFO);
  *uart1LCR = (0x03<<SBIT_WordLength) | (1<<SBIT_DLAB);

  int _baudrate = ((96000000/4) / (16 * baudrate));

  *uart1DLL = _baudrate & 0xFF;
  *uart1DLM = (_baudrate <<  0x08) & 0xFF;

  *uart1LCR &=  ~(1<<SBIT_DLAB);
}

void readRegs(int reg_addr, uint8_t * data, int len)
{
    char t[1] = {reg_addr};
    i2c.write(MMA8452_I2C_ADDRESS, t, 1, true);
    i2c.read(MMA8452_I2C_ADDRESS, (char *)data, len);
}

void writeRegs(uint8_t * data, int len)
{
    i2c.write(MMA8452_I2C_ADDRESS, (char *)data, len);
}

float get_acc_deg(uint8_t * data)
{
    int16_t acc = (data[0] << 6) | (data[1] >> 2);
    if (acc > UINT14_MAX/2)
        acc -= UINT14_MAX;
    float tmp = (float(acc/4096.0));
    float deg = tmp*90;
    return deg;
}

void readAngle(int* angles){
    int reg_addr = 0x01;//read cmd register adress
    uint8_t data[6];

    //Read x,y,z output values
    readRegs(reg_addr, data, 6);

    angles[0] = get_acc_deg(&data[0]);
    angles[1] = get_acc_deg(&data[2]);
    angles[2] = get_acc_deg(&data[4]);
}

int main() {

  LPC_GPIO1->FIODIR |= LED1 | LED2 | LED3 | LED4;
  LPC_GPIO1->FIOSET |= LED4;

  EINT3_init();

  initUart1(9600);
    wait(1);

  sendCharUart1(0x76);
    wait(0.01);
    void readRegs(int reg_addr, uint8_t * data, int len);
    void writeRegs(uint8_t * data, int len);
    float get_acc_deg(uint8_t * data);
    //set full scale selection ACTIVE (reads LSBs)
    uint8_t init[2] = {CTRL_REG1,CTRL_REG1_ACTIVE};
    writeRegs(init,2);
  while(1) {
        readAngle(angles);
        wait(1);
  }
}