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


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

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


// Global Variables
// Initial interval: in seconds
float lightTmr = LIGHT_SNS_DEFAULT;
float accTmr = ACC_SNS_DEFAULT;
float magTmr = MAG_SNS_DEFAULT;
float touchTmr = TOUCH_SNS_DEFAULT;


bool lightEnable = true;
bool accEnable = true;
bool magEnable = true;
bool touchEnable = true;

void clock1_interrupt();
void serialRx_interrupt();
void sendLightInfo();
void sendAccInfo();
void sendMagInfo();
void sendTouchInfo();


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

    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(){
    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;
        ch = serial.getc();
    }
    buffer[i] = '\0';
    serial.printf("\r\n");
    // 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;
        }else if (strstr(buffer, "mag")){
            magTmr = interval;
        }else if (strstr(buffer, "light")){
            lightTmr = interval;
        }else if (strstr(buffer, "touch")){
            touchTmr = interval;
        }
    }

    // Stop Command
    else if (strstr(buffer, "stop")!= NULL){
        if (strstr(buffer, "acc")){
            accEnable = false;
            accTmr = ACC_SNS_DEFAULT;
        }else if (strstr(buffer, "mag")){
            magEnable = false;
            magTmr = MAG_SNS_DEFAULT;
        }else if (strstr(buffer, "light")){
            lightEnable = false;
            lightTmr = LIGHT_SNS_DEFAULT;
        }else if (strstr(buffer, "touch")){
            touchEnable = false;
            touchTmr = TOUCH_SNS_DEFAULT;
        }
    }

    // Start Command
    else if (strstr(buffer, "start")!=NULL){
        if (strstr(buffer, "acc") && !accEnable){
            accEnable = true;
            accTmr = ACC_SNS_DEFAULT;
        }else if (strstr(buffer, "mag") && !magEnable){
            magEnable = true;
            magTmr = MAG_SNS_DEFAULT;
        }else if (strstr(buffer, "light") && !lightEnable){
            lightEnable = true;
            lightTmr = LIGHT_SNS_DEFAULT;
        }else if (strstr(buffer, "touch") && !touchEnable){
            touchEnable = true;
            touchTmr = TOUCH_SNS_DEFAULT;
        }
    }
    clock1.attach(&clock1_interrupt,0.1);
}


void clock1_interrupt(){
    static int accCnt;
    static int magCnt;
    static int lightCnt;
    static int touchCnt;

    accCnt++;
    magCnt++;
    lightCnt++;
    touchCnt++;

    // TODO: send data through Serial
    if (lightEnable && (lightCnt<0 || lightCnt>=lightTmr/TIME_ACCURACY)){
        sendLightInfo();
        lightCnt = 0;
    }
    /*if (magEnable){
        sendMagInfo();
    }*/
    if (touchEnable && (touchCnt<0 || touchCnt>=touchTmr/TIME_ACCURACY)){
        sendTouchInfo();
        touchCnt = 0;
    }
    if (accEnable && (accCnt<0 || accCnt>=accTmr/TIME_ACCURACY)){
        sendAccInfo();
        accCnt = 0;
    }

}

void sendLightInfo(){
    serial.printf("[LGT] Light Intensity=%f\r\n", lightSensor.read());
}


void sendAccInfo(){
    // get acc data
    float accX = accSensor.getAccX();
    float accY = accSensor.getAccY();
    float accZ = accSensor.getAccZ();

    // send acc data
    serial.printf("[ACC] accX=%-2.4f accY=%-2.4f accZ=%-2.4f\r\n",accX,accY,accZ);
}

void sendTouchInfo(){
    // get data
    float touchForce = touchSensor.readPercentage();
    float distance = touchSensor.readDistance();

    // send data
    serial.printf("[TCH] Force=%0.4f Distance=%2.2f\r\n", touchForce, distance);
}