#include "mbed.h"
#include "MMA8451Q.h"
#include "sx1276_fsk.h"

uint8_t buffer[10][192]; 
uint8_t ind1 = 0, testX = 0;

SX1276 kom(PTD6, PTD7, PTD5, PTD4, PTA6, PTA7, PTC16, PTC13, PTA14);
//SX127x_lora Lora(kom);
SX1276_fsk FSK(kom);


//#if   defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
PinName const SDA = PTE25;
PinName const SCL = PTE24;
//#endif

#define MMA8451_I2C_ADDRESS (0x1d<<1)
#define UINT14_MAX        16383

//const char * INFO =  __DATE__ ", " __TIME__;

Serial pc(USBTX, USBRX); // tx, rx
AnalogIn HarvIn(PTB0);
AnalogIn HarvOut(PTB1);
AnalogIn HarvReady(PTB2);
DigitalOut test(PTC9);
DigitalOut test1(PTC8);
Timer t2;


DigitalOut led(LED2);       //PTE29 RED_LED
Ticker timflag;

MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
InterruptIn accInt1(PTC5);      //PTC5 INT1_ACC
InterruptIn close(PTC12);     //SW3

uint8_t fileopen;
uint32_t timInc = 0;
FILE *fp = fopen("/SD/sdAcc.bin", "ab");
FILE *fvibr = fopen("/SD/sdHarv.bin", "ab");

void ding() 
{
    timInc++;
}

void closePrg(void)
{
  //led = !led;
  fileopen = 0;
}


void get_acc(void)
{ 
    acc.isr();
}

int main(void)
{
    int16_t x, y, z;
    float Hin, Hout, Hrd;
    //**********convert float to uint8_t before writing data to buffer***********/
    uint8_t *data[7];
    ///****************************************************************************/
    close.mode(PullUp);                          
    close.fall(&closePrg);
    pc.baud(115200);       
    FSK.enable();
    led = 1;
    accInt1.fall(&get_acc);     //attach the address of the get_acc function to the falling edge
                                // wait around, interrupts will interrupt this!
    timflag.attach(&ding, 2.0); // the address of the function to be attached (flip) and the interval (2 seconds)
    acc.init_400Hz_logger();
    kom.set_frf_MHz(868.0);       //- default
    FSK.set_tx_fdev_hz(100000);
    FSK.set_bitrate(9600);
    kom.write_reg(REG_FSK_PAYLOADLENGTH, 0x16);         // set payload to 28 bytes
    
    fileopen = 1;
    while (fileopen) 
    { 
        if(testX)
        {     
            acc.writeRegs(REG_CTRL_REG1, 0x00);             //off state (active = 0)
            test1 = 1;
        for(int i = 0; i < 10; i++)
            for(int j = 0; j < 32; j++)
            { 
                    x = ((buffer[i][j*6]<<6) | (buffer[i][j*6+1]>>2));
                    y = (buffer[i][j*6+2]<<6) | (buffer[i][j*6+3]>>2);
                    z = (buffer[i][j*6+4]<<6) | (buffer[i][j*6+5]>>2);
                    if (x > UINT14_MAX/2) x -= UINT14_MAX;
                    if (y > UINT14_MAX/2) y-= UINT14_MAX;
                    if (z > UINT14_MAX/2) z -= UINT14_MAX;           
                    Hin = HarvIn.read()*3.6f;
                    Hout = HarvOut.read()*3.6f;
                    Hrd = HarvReady.read()*3.6f;
                    pc.printf("timInc %d", timInc);
                    kom.tx_buf[0] = timInc; kom.tx_buf[1] = (timInc >> 8); kom.tx_buf[2] = (timInc >> 16); kom.tx_buf[3] = (timInc >> 24); 
                    data[0] = (uint8_t*)(&Hin);
                    data[1] = (uint8_t*)(&Hout);
                    data[2] = (uint8_t*)(&Hrd); 
                    for(unsigned k = 1; k < 4; k++)         // write data to buffer
                        for(unsigned l = 0; l < 4; l++)
                        {
                            kom.tx_buf[4*k+l] = data[k-1][l];   
                        }
                    kom.tx_buf[16] = x;
                    kom.tx_buf[17] = (x >> 8);
                    kom.tx_buf[18] = y;
                    kom.tx_buf[19] = (y >> 8);
                    kom.tx_buf[20] = z;
                    kom.tx_buf[21] = (z >> 8);
                    pc.printf("c%d", i*32+j);    
                    FSK.start_tx(22);                           // 22 B to send 
                    while(FSK.service() != SERVICE_TX_DONE);
                     //pc.printf("OK");
                    wait_ms(1);        // timebreak for receiver
                    
            } 
        test1 = 0;          // for HW DEBUG
        testX = 0;
        acc.writeRegs(REG_CTRL_REG1, 0x09);      //Go to Active Mode
        //deepsleep();
        pc.printf("testOK");
        }        
        //pc.printf(".");
    }
}