#include "mbed.h"
#include "MMA8451Q.h"
#include "SDFileSystem.h"

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


#if   defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
  PinName const SDA = PTE25;
  PinName const SCL = PTE24;
#elif defined (TARGET_KL05Z)
  PinName const SDA = PTB4;
  PinName const SCL = PTB3;
#elif defined (TARGET_K20D50M)
  PinName const SDA = PTB1;
  PinName const SCL = PTB0;
#else
  #error TARGET NOT DEFINED
#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;



SDFileSystem sd(PTA16, PTA17, PTA15, PTA14, "SD"); // the pinout on the mbed Cool Components workshop board
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 get_acc(void)
{ 
    acc.isr();
}

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

int main(void)
{
    int16_t x, y, z;
    pc.baud(115200);       
    led = 1;
    accInt1.fall(&get_acc);     //attach the address of the get_acc function to the falling edge
                                // wait around, interrupts will interrupt this!
    close.mode(PullUp);                          
    close.fall(&closeSD);
    timflag.attach(&ding, 0.1); // the address of the function to be attached (flip) and the interval (2 seconds)
    acc.init_400Hz_logger();
    mkdir("/SD", 0777);
    if( (fp == NULL) || (fvibr == NULL) )
       {error("Could not open file for write\n");}
    fileopen = 1;
    while (fileopen) 
    {
        if(acc.drdy())
            if(fprintf(fvibr, "{%d: %3.3f%%, %3.3f%%, %3.3f%%}, ", timInc, HarvIn.read()*100.0f, HarvOut.read()*100.0f, HarvReady.read()*100.0f) < 0)
                break;     
        if(testX)
        {       
        pc.printf("Neco...");
            acc.writeRegs(REG_CTRL_REG1, 0x00);    
            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;
                    if(fprintf(fp, "[%d, %f, %f, %f], ", timInc, float(x)/4096.0,float(y)/4096.0,float(z)/4096.0)  < 0)
                       { pc.printf("Chyba"); break;}
                //pc.printf("\n\rznacka %d", timInc);
            } 
           // test1 = 0;
                //led = 0;    //light on
        if(fprintf(fvibr, "{%d: %3.3f%%, %3.3f%%, %3.3f%%}, ", timInc, HarvIn.read()*3.6f, HarvOut.read()*3.6f, HarvReady.read()*3.6f) < 0)
            break;       //fvibr ~sdHarv.txt
        testX = 0;
        test1 = 0;      //debug
        
        fprintf(fp, "|");
        acc.writeRegs(REG_CTRL_REG1, 0x09);      //Go to Active Mode
        deepsleep();
        }        
        //pc.printf(".");
    }
  fileopen = 0;
  fclose(fvibr);  
  fclose(fp);
}