#include "mbed.h"
#include "SDFileSystem.h"
#include "sx1276_fsk.h"

Serial pc(USBTX, USBRX); // tx, rx


DigitalOut led(LED2);       //PTE29 RED_LED

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

SDFileSystem sd(PTA16, PTA17, PTA15, PTA14, "SD"); // the pinout on the mbed Cool Components workshop board
InterruptIn close(PTC12);     //SW3

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

typedef union 
{
    uint8_t numBuf[4];
    uint32_t num;
    float numFloat;
} convert_t;

void closeSD(void)
{
  //led = !led;
  fileopen = 0;
  fclose(fvibr);  
  fclose(fp);
}

int main(void)
{
    int16_t x, y, z;
    fileopen = 1;
    FSK.enable();
    /**************array ******/
    /********union to convert data back*****/
    convert_t data[4];
    /***************************************/
    
    pc.baud(115200);       
    led = 1;
    close.mode(PullUp);                          
    close.fall(&closeSD);
    mkdir("/SD", 0777);
    if( (fp == NULL) || (fvibr == NULL) )
       {error("Could not open file for write\n");}
    fileopen = 1;
    kom.set_frf_MHz(868.0);       //- default
    FSK.set_tx_fdev_hz(100000);
    FSK.set_bitrate(9600);
    FSK.set_rx_dcc_bw_hz(100000, 0);    // rxbw
    //kom.write_reg(REG_FSK_FEILSB, 0x4E);
    kom.write_reg(REG_FSK_PAYLOADLENGTH, 0x1C);         // set payload to 28 bytes
    FSK.start_rx();
    while (fileopen) 
    {
            while(FSK.service() != SERVICE_READ_FIFO);
            data[0].num = (kom.rx_buf[0] | (kom.rx_buf[1] << 8) | (kom.rx_buf[2] << 16) | (kom.rx_buf[3] << 24));
            for(unsigned i = 1; i < 4; i++)           
                for(int j = 0; j < 4; j++)
                    data[i].numBuf[j] = kom.rx_buf[4*i+j];                                                   
            x = kom.rx_buf[16] | (kom.rx_buf[17] << 8);
            y = kom.rx_buf[18] | (kom.rx_buf[19] << 8);
            z = kom.rx_buf[20] | (kom.rx_buf[21] << 8);      /*numInc*/         /*HarvIn*/        /*HarvOut*/         /*HarvReady*/
            if(fprintf(fvibr, "{%d: %3.3f, %3.3f, %3.3f}, ", data[0].num, data[1].numFloat, data[2].numFloat, data[3].numFloat) < 0)
                break;     
                                                    /*numInc*/          /*x*/          /*y*/            /*z*/
            if(fprintf(fp, "[%d: %f, %f, %f], ", data[0].num, float(x)/4096.0, float(y)/4096.0, float(z)/4096.0) < 0)
            break;       
        deepsleep();
    }        
  fileopen = 0;
  fclose(fvibr);  
  fclose(fp);
}