/* ------------------ */
/* - Include Files. - */
/* ------------------ */

#include <windows.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "mlMouse.h"
#include "mlDataLog.h"

#define CURSOR_UPDATE_GAIN_DRAWING  0.8f
#define CURSOR_UPDATE_GAIN_MOVING   0.8f
#define CURSOR_LEAKAGE_FACTOR       0.1f

tQuatPacket qData;

/**********************************************************************/
/*                                                                    */
/* Function Name: MLQMult                                             */
/*                                                                    */
/*   Description: Quaternion multiplication                           */
/*                                                                    */
/*    Parameters: q1 - multiplicand                                   */
/*                q2 - multiplicator                                  */
/*                qProd - multiplication result                       */
/*                                                                    */
/*       Returns:                                                     */
/*                                                                    */
/**********************************************************************/
void MLQMult(float *q1, float *q2, float *qProd)
{
    qProd[0] = (q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]);
    qProd[1] = (q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]);
    qProd[2] = (q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]);
    qProd[3] = (q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]);
}
/**********************************************************************/
/*                                                                    */
/* Function Name: MLQInvert                                           */
/*                                                                    */
/*   Description: Calculate inverse of Quaternion                     */
/*                                                                    */
/*    Parameters: q - original quaternion                             */
/*                qInverted - inverse of quaternion                   */
/*                                                                    */
/*       Returns:                                                     */
/*                                                                    */
/**********************************************************************/
void MLQInvert(float *q, float *qInverted)
{
    qInverted[0] = q[0];
    qInverted[1] = -q[1];
    qInverted[2] = -q[2];
    qInverted[3] = -q[3];
}
/**********************************************************************/
/*                                                                    */
/* Function Name: IMUquaternionInit                                   */
/*                                                                    */
/*   Description: Data structure initialization                       */
/*                                                                    */
/*    Parameters: qData - data structure pointer                      */
/*                                                                    */
/*       Returns:                                                     */
/*                                                                    */
/**********************************************************************/
void IMUquaternionInit(tQuatPacket *qData)
{
    int i;
    for( i = 0; i < 3; i++ )
    {
        qData->gridLinear[i] = 0;
        qData->gridGain[i] = CURSOR_UPDATE_GAIN_MOVING;
        qData->gridLinearAvg[i] = 0;
    }
    for( i = 0; i < 2; i++ )
    {
        qData->cursor.grid[i] = 0;
    }

    qData->cursor.state = CURSOR_INIT;
    qData->cursor.ButtonPushTimeThreshold = 20; //5;   // delay 700 sample count for cursor display
    qData->cursor.DoubleClickTimeThreshold = 100; //20;   // delay 500 sample count for cursor display
    qData->cursor.PointingDetectTimeThreshold = 100;
    qData->cursor.HandJitterEnterThreshold = 0.003f; // radius
    qData->cursor.HandJitterExitThreshold = 0.0006f; // radius
    qData->cursor.sampleCount = 0;

    qData->buttonTopBit = 0;
    qData->buttonTopBitPrev = 0;

#ifdef ML_DATA_LOG
    mlDataLogInit();
#endif
}
/**********************************************************************/
/*                                                                    */
/* Function Name: GetCursorMovingStatusDuringPointing                 */
/*                                                                    */
/*   Description: Check cursor moving or not on point mode            */
/*                                                                    */
/*    Parameters: qData - data structure pointer                      */
/*                                                                    */
/*       Returns: 1: moving 0: still                                  */
/*                                                                    */
/**********************************************************************/
unsigned char GetCursorMovingStatusDuringPointing(tQuatPacket *qData)
{
    unsigned char status;

    if( (fabs(qData->cursor.output[0]) >= qData->cursor.HandJitterExitThreshold)
        || (fabs(qData->cursor.output[1]) >= qData->cursor.HandJitterExitThreshold) )
    {
        status = 1;
    }
    else
    {
        status = 0;
    }

    return status;
}
/**********************************************************************/
/*                                                                    */
/* Function Name: GetCursorMovingStatusDuringMoving                   */
/*                                                                    */
/*   Description: Check cursor moving or not on moving mode           */
/*                                                                    */
/*    Parameters: qData - data structure pointer                      */
/*                                                                    */
/*       Returns: 1: moving 0: still                                  */
/*                                                                    */
/**********************************************************************/
unsigned char GetCursorMovingStatusDuringMoving(tQuatPacket *qData)
{
    unsigned char status;

    if( (fabs(qData->gridChange[0]) >= qData->cursor.HandJitterEnterThreshold)
        || (fabs(qData->gridChange[1]) >= qData->cursor.HandJitterEnterThreshold) )
    {
        status = 1;
    }
    else
    {
        status = 0;
    }

    return status;
}
/**********************************************************************/
/*                                                                    */
/* Function Name: MouseFrictionProcess                                */
/*                                                                    */
/*   Description: Mouse movement process                              */
/*                                                                    */
/*    Parameters: qData - data structure pointer                      */
/*                                                                    */
/*       Returns:                                                     */
/*                                                                    */
/**********************************************************************/
void MouseFrictionProcess(tQuatPacket *qData)
{
    int i;
    for( i = 0; i < 2; i++ )
    {
        qData->cursor.gridChange[i][0] = qData->gridLinearAvg[i]
                        - qData->cursor.grid[i];
        qData->cursor.output[i] = qData->cursor.gridChange[i][0];
        qData->cursor.gridChange[i][1] = qData->cursor.gridChange[i][0];
        qData->cursor.outFx[i] = 0;
    }

    qData->cursor.sampleCount++;

    switch( qData->cursor.state )
    {
        case CURSOR_INIT:
            if( GetCursorMovingStatusDuringMoving(qData) )
            {
                qData->cursor.state = CURSOR_MOVING;
            }
            else
            {
                qData->cursor.state = CURSOR_POINTING;
            }
            break;
        case CURSOR_POINTING:
            // add some friction
            UpdateMouseCursorPointing(qData, CURSOR_LEAKAGE_FACTOR);
            if( qData->button )
            {
                qData->cursor.state = CURSOR_POINTING_BUTTON;
                qData->cursor.sampleCount = 0;
                qData->cursor.ButtonPrev = qData->button;
            }

            if( GetCursorMovingStatusDuringPointing(qData) )
            {
                qData->cursor.state = CURSOR_MOVING;
            }
            break;
        case CURSOR_POINTING_BUTTON:
            // add some friction
            UpdateMouseCursorPointing(qData, 1.0f);

            if( (qData->button & 0x01) == 0x01 )
            {
                if( qData->cursor.sampleCount
                        >= qData->cursor.ButtonPushTimeThreshold )
                {
                    qData->cursor.state = CURSOR_DRAWING;
                    qData->cursor.sampleCount = 0;
                    for( i = 0; i < 2; i++ )
                    {
                        qData->gridGain[i] = CURSOR_UPDATE_GAIN_DRAWING;
                    }
                }
            }
            else if( qData->button == 0 )
            {
                qData->cursor.state = CURSOR_POINTING_DOUBLE_CLICK;
                qData->cursor.sampleCount = 0;
            }
            break;
        case CURSOR_POINTING_DOUBLE_CLICK:
            // add some friction
            UpdateMouseCursorPointing(qData, 1.0f);
            if( qData->cursor.sampleCount
                    >= qData->cursor.DoubleClickTimeThreshold )
            {
                qData->cursor.state = CURSOR_INIT;
            }
            else
            {
				if( qData->button == qData->cursor.ButtonPrev )
                {
                    qData->cursor.state = CURSOR_INIT;
                    qData->cursor.sampleCount = 0;
                }
            }
            break;
        case CURSOR_MOVING:
            if( !GetCursorMovingStatusDuringMoving(qData) )
            {
                qData->cursor.state = CURSOR_MOVING_READY;
                qData->cursor.sampleCount = 0;

            }
            if( qData->button & 0x01 )
            {
                qData->cursor.state = CURSOR_DRAWING;
                qData->cursor.sampleCount = 0;
                for( i = 0; i < 2; i++ )
                {
                    qData->gridGain[i] = CURSOR_UPDATE_GAIN_DRAWING;
                }

            }
            UpdateMouseCursorDrawing(qData);
            break;
        case CURSOR_MOVING_READY:
            if( GetCursorMovingStatusDuringMoving(qData) )
            {
                qData->cursor.state = CURSOR_MOVING;
            }
            else if( qData->cursor.sampleCount
                         >= qData->cursor.PointingDetectTimeThreshold )
            {
                qData->cursor.state = CURSOR_POINTING;
            }
            if( qData->button )
            {
                qData->cursor.state = CURSOR_POINTING_BUTTON;
                qData->cursor.sampleCount = 0;
            }

            UpdateMouseCursorDrawing(qData);

            break;
        case CURSOR_DRAWING:
            if( (qData->button & 0x01) == 0 )
            {
                for( i = 0; i < 2; i++ )
                {
                    qData->gridGain[i] = CURSOR_UPDATE_GAIN_MOVING;
                    qData->cursor.state = CURSOR_POINTING;
                }
            }
            UpdateMouseCursorDrawing(qData);
            break;
        case CURSOR_IDLE:
            break;
        default:
            qData->cursor.state = CURSOR_INIT;
    }
}
/**********************************************************************/
/*                                                                    */
/* Function Name: CalcLinearGridByEuler                               */
/*                                                                    */
/*   Description: Calculate grid movement by Euler angle              */
/*                                                                    */
/*    Parameters: qData - data structure pointer                      */
/*                                                                    */
/*       Returns:                                                     */
/*                                                                    */
/**********************************************************************/
void CalcLinearGridByEuler(tQuatPacket *qData)
{
    qData->grid[0][0] = qData->angle[0];
    qData->gridChange[0] = qData->grid[0][0] - qData->grid[0][1];
    qData->grid[0][1] = qData->grid[0][0];
    if( qData->gridChange[0] > 6.0 )
    {
        qData->gridLinear[0] += (float) (qData->gridChange[0] - 6.28);
    }
    else if( qData->gridChange[0] < -6.0 )
    {
        qData->gridLinear[0] += (float) (qData->gridChange[0] + 6.28);
    }
    else
    {
        qData->gridLinear[0] += qData->gridChange[0];
    }

    // grid calculation for Pitch
    qData->grid[1][0] = qData->angle[1];
    qData->gridChange[1] = qData->grid[1][0] - qData->grid[1][1];
    qData->grid[1][1] = qData->grid[1][0];
    qData->gridLinear[1] = qData->angle[1];

    // grid calculation for Roll
    qData->grid[2][0] = qData->angle[2];
    qData->gridChange[2] = qData->grid[2][0] - qData->grid[2][1];
    qData->grid[2][1] = qData->grid[2][0];
    if( qData->gridChange[2] > 6.0 )
    {
        qData->gridLinear[2] += (float) (qData->gridChange[2] - 6.28);
    }
    else if( qData->gridChange[1] < -6.0 )
    {
        qData->gridLinear[2] += (float) (qData->gridChange[2] + 6.28);
    }
    else
    {
        qData->gridLinear[2] += qData->gridChange[2];
    }
}
/**********************************************************************/
/*                                                                    */
/* Function Name: CalcLinearGridByQuaternion                          */
/*                                                                    */
/*   Description: Calculate grid movement by Quaternion               */
/*                                                                    */
/*    Parameters: qData - data structure pointer                      */
/*                                                                    */
/*       Returns:                                                     */
/*                                                                    */
/**********************************************************************/
void CalcLinearGridByQuaternion(tQuatPacket *qData)
{
    float quatDelta[4], quatInv[4];

    MLQInvert(qData->quatPrev, quatInv);
    MLQMult(quatInv, qData->quat, quatDelta);

    qData->gridChange[0] = (float) mEULAR_ANGLE_X_MOUSE(quatDelta)
                    - 1.5707963267948966192313216916398f;
    qData->pitchSin = 2* (quatDelta [0]*quatDelta[1]+quatDelta[2]*quatDelta[3]);
    if (qData->pitchSin>1.0f)
	{
        qData->pitchSin=1.0f;
	}
    if (qData->pitchSin<-1.0f)
	{
        qData->pitchSin=-1.0f;
	}

    qData->gridChange[1] = (float)asin(qData->pitchSin);
    qData->gridChange[2] = (float ) mEULAR_ANGLE_Z_MOUSE(quatDelta);

	qData->gridLinear[0] += qData->gridChange[0];
	qData->gridLinear[1] += qData->gridChange[1];
	qData->gridLinear[2] += qData->gridChange[2];
}

void IMUgetQuaternion(tQuatPacket *qData)
{
    int i;
    static unsigned char motionSave = 0xcc;


    if( qData->packetCnt == qData->packetCntPrev )
    {
        return;
	}

    qData->packetCntPrev = qData->packetCnt;

    // Euler angle from quaternion
    qData->angle[0] = (float) mEULAR_ANGLE_X_MOUSE(qData->quat);
    qData->pitchSin = 2* (qData ->quat[0]*qData->quat[1]+qData->quat[2]*qData->quat[3]);
    if (qData->pitchSin>1.0f)
	{
        qData->pitchSin=1.0f;
	}
    if (qData->pitchSin<-1.0f)
	{
        qData->pitchSin=-1.0f;
	}

    qData->angle[1] = (float)asin(qData->pitchSin);
    qData->angle[2] = (float ) mEULAR_ANGLE_Z_MOUSE(qData->quat);

    // grid calculation for Yaw
    if (fabs(qData->angle[1]) <= 1.0f)
    {
        CalcLinearGridByEuler(qData);
    }
    else
    {
        CalcLinearGridByQuaternion(qData);
    }

    for (i=0; i<3; i++)
    {
        qData->gridLinearAvg[i] = (2*qData->gridLinearAvg[i] + qData->gridLinear[i])/3;
    }
    MouseFrictionProcess(qData);
    UpdateMouseButton(qData);
#ifdef ML_DATA_LOG
    mlDataLogProcess();
#endif

#if 0

    printf("state: %2d  x y:%f %f  changes: %f %f\n",
    qData->cursor.state,
    qData->gridLinear[0],
    qData->gridLinear[1],
    qData->gridChange[0],
    qData->gridChange[1]);
#endif

    for (i=0; i<4; i++)
    {
        qData->quatPrev[i] = qData->quat[i];
    }

    if (qData->motion != motionSave)
    {
	motionSave = qData->motion;
	printf("Motion = %02x\n", (unsigned int)motionSave);
    }
}

/**********************************************************************/
/*                                                                    */
/* Function Name: UpdateMouseCursorDrawing                            */
/*                                                                    */
/*   Description: Output mouse cursor movement                        */
/*                                                                    */
/*    Parameters: qData - data structure pointer                      */
/*                                                                    */
/*       Returns:                                                     */
/*                                                                    */
/**********************************************************************/
void UpdateMouseCursorDrawing(tQuatPacket *qData)
{
    int i;

    //Get current mouse position
    POINT tmpPt =
    {
        0, 0
    };
    GetCursorPos(&tmpPt);
    for( i = 0; i < 2; i++ )
    {
        // final output in fixed point grid change format
        qData->cursor.outFx[i] = (short) (qData->cursor.output[i]
                        * qData->gridGain[i] * 2048.0f);
        qData->cursor.grid[i] += ((float) qData->cursor.outFx[i])
                        / (qData->gridGain[i] * 2048.0f);
    }

    SetCursorPos(-qData->cursor.outFx[0] + tmpPt.x, -qData->cursor.outFx[1]
                    + tmpPt.y);
}
/**********************************************************************/
/*                                                                    */
/* Function Name: UpdateMouseCursorPointing                           */
/*                                                                    */
/*   Description: Calculate grid changing on point mode               */
/*                                                                    */
/*    Parameters: qData - data structure pointer                      */
/*                leakFactor - facter to coontrol grid changing       */
/*                                                                    */
/*       Returns:                                                     */
/*                                                                    */
/**********************************************************************/
void UpdateMouseCursorPointing(tQuatPacket *qData, float leakFactor)
{
    int i;

    // saturate the leakFactor
    if( leakFactor > 1.0f )
        leakFactor = 1.0f;
    else if( leakFactor < 0 )
        leakFactor = 0;

    for( i = 0; i < 2; i++ )
    {
        qData->cursor.grid[i] += (float) (qData->cursor.output[i] * leakFactor);
    }

}
/**********************************************************************/
/*                                                                    */
/* Function Name: UpdateMouseButton                                   */
/*                                                                    */
/*   Description: Handle mouse buttons                                */
/*                                                                    */
/*    Parameters: qData - data structure pointer                      */
/*                                                                    */
/*       Returns:                                                     */
/*                                                                    */
/**********************************************************************/
void UpdateMouseButton(tQuatPacket *qData)
{
    static char mlButton[2] =
    {
        0, 0
    };
    POINT tmpPt =
    {
        0, 0
    };
    GetCursorPos(&tmpPt);

    //Button 1
    if( !mlButton[0] && (qData->button & 0x01) )
    {
        // Simulate a left button press
        mouse_event(MOUSEEVENTF_LEFTDOWN, tmpPt.x, tmpPt.y, 0, 0);
        mlButton[0] = 1;

    }
    else if( mlButton[0] && !(qData->button & 0x01) )
    {
        // Simulate a left button release
        mouse_event(MOUSEEVENTF_LEFTUP, tmpPt.x, tmpPt.y, 0, 0);
        mlButton[0] = 0;
    }

    //Button 2
    if( !mlButton[1] && (qData->button & 0x02) )
    {
        // Simulate a right button press
        mouse_event(MOUSEEVENTF_RIGHTDOWN, tmpPt.x, tmpPt.y, 0, 0);
        mlButton[1] = 1;
    }
    else if( mlButton[1] && !(qData->button & 0x02) )
    {
        // Simulate a right button release
        mouse_event(MOUSEEVENTF_RIGHTUP, tmpPt.x, tmpPt.y, 0, 0);
        mlButton[1] = 0;
    }
}