#include "mbed.h"
#include "TSISensor.h"          // Touch Sensor
#include "MAG3110.h"            // Magnetic Sensor
#include "MMA8451Q.h"           // AcceleroMeter
//#include "DataFrame.h"



#define MMA8451_I2C_ADDRESS (0x1d<<1)   // acc sensor address
#define TIME_ACCURACY     0.1
#define LGT_SNS_DEFAULT   2             // default collecting interval in seconds
#define ACC_SNS_DEFAULT   2
#define MAG_SNS_DEFAULT   2
#define TCH_SNS_DEFAULT   2

// Parameters in Data Frame
// TODO: need to sync with the pc program

#define HEADER    0x02

#define SNS_BITS  5
#define LGT_SNS   0x00
#define ACC_SNS   0x01
#define MAG_SNS   0x02
#define TCH_SNS   0x03
#define ADC_SNS_1 0x04

#define INT    0
#define LONG   1
#define FLOAT  2
#define DOUBLE 3
#define TAIL   0x0A             // '\n'

// Command Frame
#define START   0
#define STOP    1
#define SET_INT 2



#define MAX_FRAME_LEN 255
// Define Devices & Pins
MMA8451Q accSensor(PTE25, PTE24, MMA8451_I2C_ADDRESS);
TSISensor touchSensor;
MAG3110 magSensor(PTE25, PTE24);
Serial serial(USBTX, USBRX);
Ticker clock1;
AnalogIn lightSensor(PTE22);


// Global Variables
// Initial interval: in seconds
float lgtTmr = LGT_SNS_DEFAULT;
float accTmr = ACC_SNS_DEFAULT;
float magTmr = MAG_SNS_DEFAULT;
float tchTmr = TCH_SNS_DEFAULT;

bool lgtEnable = false;
bool accEnable = false;
bool magEnable = true;
bool tchEnable = false;

void clock1_interrupt();
void serialRx_interrupt();
void sendLightInfo();
void sendAccInfo();
void sendMagInfo();
void sendTouchInfo();
int packToFrame(char*, char, char, int, void*);
char* floatToByte(float);
float byteToFloat(char*);
int calDataSize(char);
void printHex(char*);
void sendFrame(char*, int);


int main() {
    // Initialization
    // Interruption Declarations
    clock1.attach(&clock1_interrupt, TIME_ACCURACY);    // maximun accuracy be 0.1s
    serial.attach(&serialRx_interrupt, Serial::RxIrq);  // receive interrupt for serialS
    magSensor.begin();

    serial.printf("\r\n============= Start of the program ============\r\n");
    while(1){
        wait(3);
    }
}


/*---------------------------------------------------------------
  ## Receive Interruption of the Serial ##
  -> used to receive & process user command
  -> and configure the board accordingly
  ---------------------------------------------------------------*/
/*void serialRx_interrupt_userFriendlyVersion(){
    clock1.detach();                // close the interrupt
    serial.printf("\r\n");

    // Receive the Serial Input
    float interval;
    char buffer[255];
    char temp[255];
    char ch = serial.getc();
    int i;
    for (i=0 ; ch!='\n' && ch!='\r'; i++){
        serial.putc(ch);
        buffer[i] = ch;
        if (ch==127){                // BackSpace
            i--;
            i--;
        }
        ch = serial.getc();
    }
    buffer[i] = '\0';
    serial.printf("\r\nBUFFER: %s %d\r\n", buffer,i);
    // TODO: buffer -> lower case

    // Process the Serial Input
    // Set-Interval Command
    if (strstr(buffer, "set")!=NULL && strstr(buffer, "int")!= NULL){
        sscanf(buffer, "%*[^0123456789.]%s", temp);       // find the number in buffer
        sscanf(temp, "%f", &interval);                    // translate into float
        if (interval<0.1 || interval>5){
            interval  = 1;
        }
        if (strstr(buffer, "acc")){
            accTmr = interval;
        }
        if (strstr(buffer, "mag")){
            magTmr = interval;
        }
        if (strstr(buffer, "light")){
            lgtTmr = interval;
        }
        if (strstr(buffer, "touch")){
            tchTmr = interval;
        }
    }

    // Stop Command
    else if (strstr(buffer, "stop")!= NULL){
        serial.printf("STOP\r\n");
        if (strstr(buffer, "acc")){
            accEnable = false;
            accTmr = ACC_SNS_DEFAULT;
        }
        if (strstr(buffer, "mag")){
            magEnable = false;
            magTmr = MAG_SNS_DEFAULT;
        }
        if (strstr(buffer, "light")){
            lgtEnable = false;
            lgtTmr = LGT_SNS_DEFAULT;
        }
        if (strstr(buffer, "touch")){
            tchEnable = false;
            tchTmr = TCH_SNS_DEFAULT;
        }
    }

    // Start Command
    else if (strstr(buffer, "start")!=NULL){
        if (strstr(buffer, "acc") && !accEnable){
            accEnable = true;
            accTmr = ACC_SNS_DEFAULT;
        }
        if (strstr(buffer, "mag") && !magEnable){
            magEnable = true;
            magTmr = MAG_SNS_DEFAULT;
        }
        if (strstr(buffer, "light") && !lgtEnable){
            lgtEnable = true;
            lgtTmr = LGT_SNS_DEFAULT;
        }
        if (strstr(buffer, "touch") && !tchEnable){
            tchEnable = true;
            tchTmr = TCH_SNS_DEFAULT;
        }
    }
    clock1.attach(&clock1_interrupt,TIME_ACCURACY);
}
*/

void serialRx_interrupt(){                          // Byte version
    clock1.detach();                                // close the interrupt temporarily
    int i = 0;
    char frame[MAX_FRAME_LEN];
    char ch = serial.getc();
    while (ch!=HEADER){
        if (serial.readable()){
            ch = serial.getc();
        }
        else{
            serial.printf("[ERROR] broken data!\r\n");
            clock1.attach(&clock1_interrupt,TIME_ACCURACY);
            return;
        }
    }
    //TODO: ticker for time out
    while (serial.readable()){
        frame[i] = ch;
        if (ch=='\n'){
            break;
        }
        ch = serial.getc();
        i++;
    }
    frame[++i] = '\0';
    float interval=-1;
    char snsType = frame[1]|0x1F;
    char oper = (frame[1]|0xE0)>>SNS_BITS;              // operation type
    if (oper==SET_INT){
        memcpy(&interval,&frame[2],4);
        if (interval<0.0001||interval>10){
            interval = 1;
        }
    }
    switch(snsType){
        case ACC_SNS:
        {
            switch(oper){
                case START:     accEnable = true;  break;
                case STOP:      accEnable = false; break;
                case SET_INT:   accTmr = interval; break;
            }
        }
        case MAG_SNS:
        {
            switch(oper){
                case START:     magEnable = true;  break;
                case STOP:      magEnable = false; break;
                case SET_INT:   magTmr = interval; break;
            }
        }
        case LGT_SNS:
        {
            switch(oper){
                case START:     lgtEnable = true;  break;
                case STOP:      lgtEnable = false; break;
                case SET_INT:   lgtTmr = interval; break;
            }
        }
        case TCH_SNS:
        {
            switch(oper){
                case START:     tchEnable = true;  break;
                case STOP:      tchEnable = false; break;
                case SET_INT:   tchTmr = interval; break;
            }
        }
    }
    clock1.attach(&clock1_interrupt,TIME_ACCURACY);
}

void clock1_interrupt(){
    static int accCnt;
    static int magCnt;
    static int lgtCnt;
    static int tchCnt;

    accCnt++;
    magCnt++;
    lgtCnt++;
    tchCnt++;

    // TODO: send data through Serial
    if (lgtEnable && (lgtCnt<0 || lgtCnt>=lgtTmr/TIME_ACCURACY)){
        sendLightInfo();
        lgtCnt = 0;
    }
    if (magEnable && (magCnt<0 || magCnt>=magTmr/TIME_ACCURACY)){
        sendMagInfo();
        magCnt = 0;
    }
    if (tchEnable && (tchCnt<0 || tchCnt>=tchTmr/TIME_ACCURACY)){
        sendTouchInfo();
        tchCnt = 0;
    }
    if (accEnable && (accCnt<0 || accCnt>=accTmr/TIME_ACCURACY)){
        sendAccInfo();
        accCnt = 0;
    }
}

void sendLightInfo(){
    char frame[MAX_FRAME_LEN];
    float lightData = lightSensor.read();
    packToFrame(frame,LGT_SNS,FLOAT,1,&lightData);
    serial.printf("[LGT] %s\r\n", frame);
    //printHex(frame);
    //serial.printf("[LGT] Light Intensity=%f\r\n", lightSensor.read());
}


void sendTouchInfo(){
    char frame[MAX_FRAME_LEN];
    float touchData[2];
    touchData[0] = touchSensor.readPercentage();
    touchData[1] = touchSensor.readDistance();
    packToFrame(frame,TCH_SNS,FLOAT,2,touchData);
    serial.printf("[TCH] %s\r\n", frame);
    //printHex(frame);
}

void sendMagInfo(){
    char frame[MAX_FRAME_LEN];
    int magData[3];
    magSensor.getValues(magData, magData+1, magData+2);
    //serial.printf("[MAG] magX=%d magY=%d magZ=%d\r\n",magData[0],magData[1],magData[2]);
    int len = packToFrame(frame,MAG_SNS,INT,3,magData);
    sendFrame(frame, len);
    //memcpy(magData, &frame[3], 2);
    //serial.printf("[MAG] %d\r\n", magData[0]);
    //printHex(frame);
}

void sendAccInfo(){
    // get acc data
    char frame[MAX_FRAME_LEN];
    float accData[3];
    accData[0] = accSensor.getAccX();
    accData[1] = accSensor.getAccY();
    accData[2] = accSensor.getAccZ();
    packToFrame(frame, ACC_SNS,FLOAT,3,accData);
    serial.printf("[ACC] %s\n\r", frame);
    //int num = int(frame[2]);
    //memcpy(&accZ, &frame[3], 4);
    // send acc data
    //printHex(frame);
}
int calDataSize(char dataType){
    switch(dataType){
        case INT:    return 2;
        case LONG:   return 4;
        case FLOAT:  return 4;
        case DOUBLE: return 8;
    }
    return 4;
}

void printHex(char* frame){
    int dataType = frame[1]&0xE0;
    int dataSize = calDataSize(dataType);
    int dataNum = frame[2];
    for (int i=0; i<4+dataSize*dataNum; i++){
        serial.printf("0x%x ",frame[i]);
    }
    serial.printf("\r\n");
}

int packToFrame(char* frame, char snsType,char dataType, int dataNum, void* data){
    int dataSize = calDataSize(dataType);
    frame[0] = HEADER;
    frame[1] = (snsType|(dataType<<SNS_BITS));
    frame[2] = dataNum;
    memcpy(frame+3, data, dataSize*dataNum);
    frame[3+dataNum*dataSize]= TAIL;
    frame[4+dataNum*dataSize]= '\0';
    frame[3] = 0x00;
    return (4+dataNum*dataSize);
}


void sendFrame (char* frame, int len){
    for (int i=0; i<len; i++){
        serial.putc(frame[i]);
    }
}