#include <xc.h>
#include <stdint.h>
#include "system.h"
#include "msghandler.h"

struct msgHeaderT {
        uint8_t size;
        uint8_t flags;
        uint8_t seq;
        uint8_t id;
};

static union {
    uint8_t buf[MD_BUF_LEN];
    struct msgHeaderT header;
    struct {
        struct msgHeaderT header;
        uint16_t streamingOutputMask;
        uint8_t timeStamp;
        uint8_t systemInfo;
        uint16_t dspInfo;
        uint32_t gestureInfo;
        uint32_t touchInfo;
        uint8_t  airWheelCounter;
        uint8_t  airWheelUnused;
        uint16_t posX;
        uint16_t posY;
        uint16_t posZ;
    } sensorData;
} md;

static uint8_t md_idx = 0;
static uint8_t AirWheelActivePrevious = 0;  // AirWheel status in the previous run
static int16_t AirWheelValuePrevious=0;     // AirWheel counter value each time that the flag in the SystemInfo goes from 0 to 1
static int16_t AirWheelDiff = 0;            //a value of +32:a full clockwise circle; a value of -32: a counterclockwise cricle

//Start new MGC3030 message
inline void md_start(void)
{
    i2cMsgFlag = 0;    
    md_idx = 0;
}

//fill the buffer of reception with the message received from the GestIC, byte after byte
inline void md_next(uint8_t c)
 /* HEADER[Size_1B, Flags_1B, Seq_1B, ID_1B]+PAYLOAD[OutputMask_2B, TimeStamp_1B, Systeminfo_1B, Variable_XB]
   Maximum length of the buffer is MD_BUF_LEN bytes*/
{
    if(md_idx < MD_BUF_LEN) {
        md.buf[md_idx++] = c;
    }
}

//Process new message from MGC3030
uint8_t mgcProcMsg(void)
{
    // bits 0 to 4 must be set in the StreamOutputMask to have valid data to read
    uint16_t streamOutRequired = (STREAMOUT_DSPINFO | STREAMOUT_GESTUREINFO | STREAMOUT_TOUCHINFO | STREAMOUT_AIRWHEELINFO | STREAMOUT_XYZPOSITION);
	
    uint8_t retVal = GI_NOGESTURE;

    if(md_idx < 4)
        return GI_NOGESTURE;

    if(md.header.id != MSGID_SENSORDATAOUT)
        return GI_NOGESTURE;

    if(md.header.size < 22 || md_idx < 22)  // message too short for our sensor data configuration
        return GI_NOGESTURE;              //DSP(2)+Gesture(4)+Touch(4)+AirWheel(2)+XYZPosition(6)+NoisePower(4)= 22bytes

    // ok, we got a sensor data message. Check that the 5 first bits of the StreamOutputMask are set to indicate valid data are present
    if((md.sensorData.streamingOutputMask & streamOutRequired) == streamOutRequired) {

        uint32_t TouchInfo = md.sensorData.touchInfo;
        uint8_t AirWheelActive = (md.sensorData.systemInfo & SI_AIRWHEELVALID) != 0; // AirWheel is active and valid if bit1 of SystemInfo is set
        int16_t AirWheelValueNew = md.sensorData.airWheelCounter;

        /* FLICK DETECTION */
	retVal = md.sensorData.gestureInfo & 0xFF; // keep only the first byte (b0..b7) that gives the gesture information (in DECIMAL)

        /* TAP DETECTION */
        if(TouchInfo & 1<<BITSHIFT_TAP_SOUTH ) {
            retVal = GI_TAP_SOUTH;
        }
        else if(TouchInfo & 1<<BITSHIFT_TAP_WEST) {
            retVal = GI_TAP_WEST;
        }
        else if(TouchInfo & 1<<BITSHIFT_TAP_NORTH) {
            retVal = GI_TAP_NORTH;
        }
        else if(TouchInfo & 1<<BITSHIFT_TAP_EAST) {
            retVal = GI_TAP_EAST;
        }
        else if(TouchInfo & 1<<BITSHIFT_TAP_CENTER) {
            retVal = GI_TAP_CENTER;
        }
        else {}

        /* AIRWHEEL DETECTION */
        //store the airwheel counter when the airwheel is started
        if (AirWheelActive && !AirWheelActivePrevious)
        {
            AirWheelValuePrevious = md.sensorData.airWheelCounter;
        }
        else if (AirWheelActive)
        {
            if( AirWheelValuePrevious < 64 && AirWheelValueNew > 192 ){
                    // "crossing zero point" happened
                    AirWheelDiff += ( (AirWheelValueNew-256) - AirWheelValuePrevious);
            }
            else if( AirWheelValuePrevious > 192 && AirWheelValueNew < 64 ){
                    // "crossing zero point" happened
                    AirWheelDiff += ( (AirWheelValueNew+256) - AirWheelValuePrevious);
            }
            else{
                    AirWheelDiff += AirWheelValueNew - AirWheelValuePrevious ;
            }

            if (AirWheelDiff >= 32)
            {
                AirWheelDiff=0;
                retVal = GI_AIRWHEEL_CCW;
            }
            else if (AirWheelDiff <= -32)
            {
                AirWheelDiff=0;
                retVal = GI_AIRWHEEL_CW;
            }
            else{}
        }
        AirWheelActivePrevious = AirWheelActive;    // save the status for the next run
        AirWheelValuePrevious = AirWheelValueNew;
    }
    return retVal;
}