#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.0005

#define ACC_SNS_DEFAULT   0.01 // default collecting interval in seconds
#define MAG_SNS_DEFAULT   0.1
#define LGT_SNS_DEFAULT   0.1
#define TCH_SNS_DEFAULT   0.1
#define A2D_SNS_DEFAULT   0.1
#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 TYP_NUM           8
#define CMD_NUM           3

typedef enum {
    SNS_ACC,
    SNS_MAG,
    SNS_LGT,
    SNS_TCH,
    SNS_A2D,
} sns_t;

typedef enum {
    TYP_S8,  TYP_S16, TYP_S32,
    TYP_U8,  TYP_U16, TYP_U32,
    TYP_F32, TYP_F64,
} type_t;

typedef enum {
    CMD_STOP,
    CMD_START,
    CMD_RATE,
} cmd_t;

// Data Frame Information
#pragma pack(1)
typedef struct {
    uint8_t header;
    struct {
        uint8_t type : 4;
        uint8_t sns  : 4;
    } bits;
    uint8_t count;
    uint8_t data[];
} state_t;
#pragma pack()

// Command Frame Information
#pragma pack(1)
typedef struct {
    uint8_t header;
    struct {
        uint8_t cmd : 4;
        uint8_t sns : 4;
    } bits;
    float   interval;
} control_t;
#pragma pack()

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

AnalogIn   a2dSensor0(A0);
AnalogIn   a2dSensor1(A1);
AnalogIn   a2dSensor2(A2);
AnalogIn   a2dSensor3(A3);
AnalogIn   a2dSensor4(A4);
AnalogIn   a2dSensor5(A5);

AnalogOut  a2dOutput(PTE30);

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 = false;
bool magEnable = false;
bool lgtEnable = false;
bool tchEnable = false;
bool a2dEnable = true;
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(control_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);
    serial.baud(230400);
    magSensor.begin();

    sdma_setup(UART0, 0);

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

    int ticks = 0, tocks = 0;
    while(1){
    	ticks = (us_ticker_read()*1E-6) / TIME_ACCURACY;
    	if (tocks < ticks) {
		clock1_interrupt();
		tocks++;
	}
    }
}

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

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

    control_t *cmd = (control_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.cmd >= CMD_NUM) {
                    state = index = 0;
                } else if (cmd->bits.cmd == CMD_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;
    static int sinCnt;

    static float sinAmp  = 0.5;  // 0..1
    static float sinFreq = 0.25; // Hz

    // Write A2D output sine wave
    a2dOutput.write(sinAmp*sin(sinCnt * TIME_ACCURACY * (2*PI) * sinFreq)*0.5 + 0.5);

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

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

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

    sdma_flush();
}

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

void runCommand(control_t *cmd)
{
    // Validate interval
    sns_t snsType = (sns_t)cmd->bits.sns;
    cmd_t cmdType = (cmd_t)cmd->bits.cmd;
    float interval = CLAMP(cmd->interval, MIN_RATE, MAX_RATE);

    // Save value to global data
    switch(snsType){
        case SNS_ACC:
            switch(cmdType){
                case CMD_STOP:   accEnable = false; break;
                case CMD_START:  accEnable = true;  break;
                case CMD_RATE:   accTmr = interval; break;
            }
            break;

        case SNS_MAG:
            switch(cmdType){
                case CMD_STOP:   magEnable = false; break;
                case CMD_START:  magEnable = true;  break;
                case CMD_RATE:   magTmr = interval; break;
            }
            break;

        case SNS_LGT:
            switch(cmdType){
                case CMD_STOP:   lgtEnable = false; break;
                case CMD_START:  lgtEnable = true;  break;
                case CMD_RATE:   lgtTmr = interval; break;
            }
            break;

        case SNS_TCH:
            switch(cmdType){
                case CMD_STOP:   tchEnable = false; break;
                case CMD_START:  tchEnable = true;  break;
                case CMD_RATE:   tchTmr = interval; break;
            }
            break;

        case SNS_A2D:
            switch(cmdType){
                case CMD_STOP:   a2dEnable = false; break;
                case CMD_START:  a2dEnable = true;  break;
                case CMD_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, SNS_ACC, TYP_F32, 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){
    // magSensor uses the wrong types,
    // so we have to convert it
    int tmp[3];
    magSensor.getValues(&tmp[0], &tmp[1], &tmp[2]);
    uint16_t magData[3];
    magData[0] = tmp[0];
    magData[1] = tmp[1];
    magData[2] = tmp[2];
    int len = packToFrame(txFrame, SNS_MAG, TYP_S16, 3, magData);

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

void sendLgtInfo(void){
    float lgtData = lgtSensor.read();
    int len = packToFrame(txFrame, SNS_LGT, TYP_F32, 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, SNS_TCH, TYP_F32, 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] = a2dSensor0.read();
    a2dData[1] = a2dSensor1.read();
    a2dData[2] = a2dSensor2.read();
    a2dData[3] = a2dSensor3.read();
    a2dData[4] = a2dSensor4.read();
    a2dData[5] = a2dSensor5.read();
    int len = packToFrame(txFrame, SNS_A2D, TYP_F32, 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 TYP_S8:  return 1;
        case TYP_S16: return 2;
        case TYP_S32: return 4;
        case TYP_U8:  return 1;
        case TYP_U16: return 2;
        case TYP_U32: return 4;
        case TYP_F32: return 4;
        case TYP_F64: 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);
}