#include <stdint.h>
#include <stdarg.h>

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

#include "serial_dma.h"         // AcceleroMeter

//#include "DataFrame.h"

#define CLAMP(x, min, max)     \
        ((x) < (min) ? (min) : \
         (x) > (max) ? (max) : (x))

#define MMA8451_I2C_ADDRESS (0x1d<<1)   // acc sensor address
#define TIME_ACCURACY     0.01

#define ACC_SNS_DEFAULT   0.1   // default collecting interval in seconds
#define MAG_SNS_DEFAULT   1.0
#define LGT_SNS_DEFAULT   1.0
#define TCH_SNS_DEFAULT   1.0
#define A2D_SNS_DEFAULT   1.0
#define LED_OUT_DEFAULT   0.5

#define MIN_RATE          1E-4  // 1 kHz
#define MAX_RATE          10.0  // 0.1 Hz

// Common Frame Information
#define HEADER            0x02
#define TAIL              0x0A  // '\n'

#define MAX_FRAME_SIZE    256

#define SNS_NUM  5
#define TYPE_NUM 4
#define OPER_NUM 3

typedef enum {
    ACC_SNS,
    MAG_SNS,
    LGT_SNS,
    TCH_SNS,
    A2D_SNS,
} sns_t;

typedef enum {
    INT,
    LONG,
    FLOAT,
    DOUBLE,
} type_t;

typedef enum {
    STOP,
    START,
    RATE,
} oper_t;

// Data Frame Information
typedef struct {
    uint8_t header;
    struct {
        uint8_t type : 4;
        uint8_t sns  : 4;
    } bits;
    uint8_t count;
    uint8_t data[];
} __attribute__((__packed__)) state_t;

// Command Frame Information
typedef struct {
    uint8_t header;
    struct {
        uint8_t oper : 4;
        uint8_t sns  : 4;
    } bits;
    float   interval;
} __attribute__((__packed__)) command_t;

// Define Devices & Pins
MMA8451Q   accSensor(PTE25, PTE24, MMA8451_I2C_ADDRESS);
MAG3110    magSensor(PTE25, PTE24);
TSISensor  tchSensor;
AnalogIn   lgtSensor(PTE22);
AnalogIn   a2dSensor(A0);

DigitalOut led1(LED1);
DigitalOut led2(LED2);

Serial     serial(USBTX, USBRX);
Ticker     clock1;

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

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

bool useStr = false;
bool useHex = false;
bool useBin = true;

uint8_t txFrame[MAX_FRAME_SIZE];
uint8_t rxFrame[MAX_FRAME_SIZE];

// Prototypes
void clock1_interrupt(void);
void serialRx_interrupt(void);

void runCommand(command_t *cmd);

void sendAccInfo(void);
void sendMagInfo(void);
void sendLgtInfo(void);
void sendTchInfo(void);
void sendA2dInfo(void);

int  calDataSize(uint8_t);
int  packToFrame(uint8_t *frame, sns_t snsType, type_t dataType, int dataNum, void *data);

void printStr(const char *str, ...);
void printHex(uint8_t *frame, int len);
void printBin(uint8_t *frame, int len);

/********
 * Main *
 ********/

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

    sdma_setup(UART0, 0);

    printStr("\r\n============= Start of the program ============\r\n");

    while(1){
        wait(3);
    }
}

/**********************
 * Interrupt handlers *
 **********************/

void serialRx_interrupt(void) {                     // Byte version
    static int state  = 0;
    static int index  = 0;
    static int length = 0;

    command_t *cmd = (command_t*)rxFrame;

    while (serial.readable()) {
        rxFrame[index++] = serial.getc();
        switch (state) {
            case 0: // Header
                if (cmd->header == HEADER)
                    state = 1;
                else
                    index = 0;
                break;
            case 1: // Bits
                if (cmd->bits.sns  >= SNS_NUM ||
                    cmd->bits.oper >= OPER_NUM) {
                    state = index = 0;
                } else if (cmd->bits.oper == RATE) {
                    length = 4;
                    state  = 2;
                } else {
                    state  = 3;
                }
                break;
            case 2: // Data
                if (--length == 0)
                    state = 3;
                break;
            case 3: // Tail
                if (rxFrame[index-1] == TAIL)
                    runCommand(cmd);
                state = 0;
                index = 0;
                break;
        }
    }
}

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

    accCnt++;
    magCnt++;
    lgtCnt++;
    tchCnt++;
    a2dCnt++;
    ledCnt++;

    // Send data through Serial
    if (accEnable && (accCnt<0 || accCnt>=accTmr/TIME_ACCURACY)){
        sendAccInfo();
        accCnt = 0;
    }
    if (magEnable && (magCnt<0 || magCnt>=magTmr/TIME_ACCURACY)){
        sendMagInfo();
        magCnt = 0;
    }
    if (lgtEnable && (lgtCnt<0 || lgtCnt>=lgtTmr/TIME_ACCURACY)){
        sendLgtInfo();
        lgtCnt = 0;
    }
    if (tchEnable && (tchCnt<0 || tchCnt>=tchTmr/TIME_ACCURACY)){
        sendTchInfo();
        tchCnt = 0;
    }
    if (a2dEnable && (a2dCnt<0 || a2dCnt>=a2dTmr/TIME_ACCURACY)){
        sendA2dInfo();
        a2dCnt = 0;
    }

    // Toggel LED for debugging
    if (ledEnable && (ledCnt<0 || ledCnt>=ledTmr/TIME_ACCURACY)){
        led1   = !led1;
	ledCnt = 0;
    }

    sdma_flush();
}

/*******************
 * Command handler *
 *******************/

void runCommand(command_t *cmd)
{
    // Validate interval
    sns_t  snsType  = (sns_t )cmd->bits.sns;
    oper_t operType = (oper_t)cmd->bits.oper;
    float  interval = CLAMP(cmd->interval, MIN_RATE, MAX_RATE);

    // Save value to global data
    switch(snsType){
        case ACC_SNS:
            switch(operType){
                case STOP:   accEnable = false; break;
                case START:  accEnable = true;  break;
                case RATE:   accTmr = interval; break;
            }
            break;

        case MAG_SNS:
            switch(operType){
                case STOP:   magEnable = false; break;
                case START:  magEnable = true;  break;
                case RATE:   magTmr = interval; break;
            }
            break;

        case LGT_SNS:
            switch(operType){
                case STOP:   lgtEnable = false; break;
                case START:  lgtEnable = true;  break;
                case RATE:   lgtTmr = interval; break;
            }
            break;

        case TCH_SNS:
            switch(operType){
                case STOP:   tchEnable = false; break;
                case START:  tchEnable = true;  break;
                case RATE:   tchTmr = interval; break;
            }
            break;

        case A2D_SNS:
            switch(operType){
                case STOP:   a2dEnable = false; break;
                case START:  a2dEnable = true;  break;
                case RATE:   a2dTmr = interval; break;
            }
            break;
    }
}

/*******************
 * Sensors reading *
 *******************/

void sendAccInfo(void){
    float accData[3];
    accData[0] = accSensor.getAccX();
    accData[1] = accSensor.getAccY();
    accData[2] = accSensor.getAccZ();
    int len = packToFrame(txFrame, ACC_SNS, FLOAT, 3, accData);

    printStr("[ACC] accX=%-2.4f accY=%-2.4f accZ=%-2.4f\r\n",
                    accData[0], accData[1], accData[2]);
    printHex(txFrame, len);
    printBin(txFrame, len);
}

void sendMagInfo(void){
    int magData[3];
    magSensor.getValues(&magData[0], &magData[1], &magData[2]);
    int len = packToFrame(txFrame, MAG_SNS, INT, 3, magData);

    printStr("[MAG] magX=%d magY=%d magZ=%d\r\n",
                    magData[0], magData[1], magData[2]);
    printHex(txFrame, len);
    printBin(txFrame, len);
}

void sendLgtInfo(void){
    float lgtData = lgtSensor.read();
    int len = packToFrame(txFrame, LGT_SNS, FLOAT, 1, &lgtData);

    printStr("[LGT] intensity=%f\r\n",
                    lgtSensor.read());
    printHex(txFrame, len);
    printBin(txFrame, len);
}

void sendTchInfo(void){
    float tchData[2];
    tchData[0] = tchSensor.readPercentage();
    tchData[1] = tchSensor.readDistance();
    int len = packToFrame(txFrame, TCH_SNS, FLOAT, 2, tchData);

    printStr("[TCH] force=%0.4f distance=%2.2f\r\n",
                    tchData[0], tchData[1]);
    printHex(txFrame, len);
    printBin(txFrame, len);
}

void sendA2dInfo(void){
    float a2dData[6];
    a2dData[0] = 0;
    a2dData[1] = 0;
    a2dData[2] = 0;
    a2dData[3] = 0;
    a2dData[4] = 0;
    a2dData[5] = 0;
    int len = packToFrame(txFrame, A2D_SNS, FLOAT, 6, a2dData);

    printStr("[A2D] data=%2.2f %2.2f %2.2f %2.2f %2.2f %2.2f\r\n",
                a2dData[0], a2dData[1], a2dData[2],
                a2dData[3], a2dData[4], a2dData[5]);
    printHex(txFrame, len);
    printBin(txFrame, len);
}

/********************
 * Helper functions *
 ********************/

int calDataSize(type_t dataType){
    switch(dataType){
        case INT:    return 2;
        case LONG:   return 4;
        case FLOAT:  return 4;
        case DOUBLE: return 8;
    }
    return 4;
}

/*******************
 * Frame functions *
 *******************/

int packToFrame(uint8_t *frame, sns_t snsType, type_t dataType, int dataNum, void *data){
    //const char pattern[] = "\x80\x81\x82\x83"
    //                       "\x84\x85\x86\x87"
    //                       "\x88\x89\x8A\x8B"
    //                       "\x8C\x8D\x8E\x8F";

    int      size  = dataNum * calDataSize(dataType);
    state_t *state = (state_t*)frame;
    uint8_t *tail  = &state->data[size];

    state->header    = HEADER;
    state->bits.sns  = snsType;
    state->bits.type = dataType;
    state->count     = dataNum;;
    memcpy(&state->data, data, size);
    tail[0]          = TAIL;
    tail[1]          = '\0';

    return (3 + size + 1);
}

/*******************
 * Print functions *
 *******************/

void printHex(uint8_t *frame, int len){
    if (!useHex)
        return;

    sdma_printf("      ");
    for (int i=0; i<len; i++)
        sdma_printf("%02hx ", frame[i]);
    sdma_printf("\r\n");
}

void printStr(const char *fmt, ...){
    if (!useStr)
        return;

    va_list ap;
    va_start(ap, fmt);
    sdma_vprintf(fmt, ap);
    va_end(ap);
}

void printBin(uint8_t *frame, int len){
    if (!useBin)
        return;

    sdma_write(frame, len);
}