#include "mbed.h"
#include "ADXL345_I2C.h"
#include "S25FL128K.h"


ADXL345_I2C accelerometer(D14,D15);
S25FL128K s25fl(D11,D12,D13,D10);

Serial pc(USBTX, USBRX);

// stores 1 of 3 different types of data
// data is grouped to minimise wasted space.
union dataUnion {   
    int16_t acceleration[3];
};

// values used to indicate which type of data is being stored
const char AccelData = 1; 

// structure used to store data in S25FL128S
typedef struct {
    char dataType;              // what type of data is in this structure
    time_t dataTime;            // when it was recorded
    union dataUnion dataValue;  // the actual values used.
} dataStore;


const int S25FLSize = 25000; // size of a page in bytes

const int s25flBlockSize = sizeof(dataStore); // size needed to store a data structure


int main()
{
    pc.baud(115200);
   // set_time(1387188323); // Set RTC time to 16 December 2013 10:05:23 UTC
    int readings[3] = {0, 0, 0};

    //pc.printf("Starting ADXL345 test...\n");
    //wait(.001);
    //pc.printf("Device ID is: 0x%02x\n", accelerometer.getDeviceID());
    //wait(.001);

    // These are here to test whether any of the initialization fails. It will print the failure
    if (accelerometer.setPowerControl(0x00)) {
        pc.printf("didn't intitialize power control\n");
        return 0;
    }
    //Full resolution, +/-16g, 4mg/LSB.
    //wait(.001);

    if(accelerometer.setDataFormatControl(0x0B)) {
        pc.printf("didn't set data format\n");
        return 0;
    }
    //wait(.001);

    //3.2kHz data rate.
    if(accelerometer.setDataRate(ADXL345_3200HZ)) {
        pc.printf("didn't set data rate\n");
        return 0;
    }
    //wait(.001);

    //Measurement mode.

    if(accelerometer.setPowerControl(MeasurementMode)) {
        pc.printf("didn't set the power control to measurement\n");
        return 0;
    }

    int recordNumber = 0;

    dataStore dataToWrite;
    s25fl.eraseChip();

    while(1) {

        
        accelerometer.getOutput(readings);

        dataToWrite.dataValue.acceleration[0] = readings[0];
        dataToWrite.dataValue.acceleration[1] = readings[1];
        dataToWrite.dataValue.acceleration[2] = readings[2];
        dataToWrite.dataType = AccelData;

        s25fl.write(recordNumber*s25flBlockSize, (char*)(&dataToWrite), s25flBlockSize);

        recordNumber++;
        if ((recordNumber*s25flBlockSize) >= (S25FLSize - s25flBlockSize)) {
            recordNumber--;
            pc.printf("\r\n PAGE FULL");
            break;
        }

    } // end write loop

    dataStore dataFromS25FL;


// code to read data back
    int readRecord = 0;
    while (readRecord < recordNumber) {

        s25fl.read(readRecord*s25flBlockSize, (char*)(&dataFromS25FL), s25flBlockSize);
        //switch (dataFromS25FL.dataType) {
            //case AccelData:
                printf("\r\n Accelerometer: X: %ig, Y: %ig, Z: %ig", dataFromS25FL.dataValue.acceleration[0], dataFromS25FL.dataValue.acceleration[1],dataFromS25FL.dataValue.acceleration[2]);
              //  break;
        //}
        readRecord++;
    }
}