// SOpticalMouse.cpp: implementation of the SOpticalMouse class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "Simulator.h"
#include "SOpticalMouse.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

SOpticalMouse::SOpticalMouse()
{
    TurningRadius = 17;
    TicksperMM = 4;
    ClearDisplacement();

}

SOpticalMouse::~SOpticalMouse()
{

}


/************************************************************************
* Function CPoint GetDisplacement()
*
* PURPOSE
* For any other class to get the values of the X and Y displacement
*************************************************************************/
CPoint SOpticalMouse::GetDisplacement()
{
    CPoint tReturn;
    tReturn.x = (long)XDisplacement;
    tReturn.y = (long)YDisplacement;
    return tReturn;

}

/************************************************************************
* Function ClearDisplacement()
*
* PURPOSE
* Clears the X and Y displacement and the angle
*************************************************************************/
void SOpticalMouse::ClearDisplacement()
{
    XDisplacement = 0;
    YDisplacement = 0;
    CurrentAngle = 0;

}

/************************************************************************
* Function IncrementDisplacement(float DAngle, float DX, float DY)
*
* PURPOSE
* Increments the X and Y Displacment.
* ALGORITHM
* Since I can't take the DX and DY as the step size approaches infinity,
* I'll have to approximate it
* 1) Assume DAngle + PrevAngle is the angle for the entire movement
* 2) calculate the DX and DY relative to that angle.
*************************************************************************/
void SOpticalMouse::IncrementDisplacement(float PrevAngle, float DAngle, float DX, float DY)
{

    CurrentAngle += DAngle;

    if (CurrentAngle > 360)
        CurrentAngle = CurrentAngle-360;
    if (CurrentAngle < 0)
        CurrentAngle = 360+CurrentAngle;

    float NewAngle = PrevAngle+DAngle;

    FPoint StartPoint,EndPoint;

    StartPoint.x = (float)(TurningRadius*cos(PrevAngle*PI/180));
    StartPoint.y = (float)(TurningRadius*sin(PrevAngle*PI/180));

    EndPoint.x = DX + (float)(TurningRadius*cos(NewAngle*PI/180));
    EndPoint.y = DY + (float)(TurningRadius*sin(NewAngle*PI/180));

    float RAngle = NewAngle - (float)LAUtils.GetAngle(EndPoint,StartPoint) - 90;

    float Distance = LAUtils.GetDistance(StartPoint,EndPoint);

    DX = Distance * (float)cos((RAngle)*PI/180)*40;
    DY = Distance * (float)sin((RAngle)*PI/180)*40;

     YDisplacement += DY;
     XDisplacement += DX;

/*

    #ifdef USEFLOAT

         float Delta = (float)(DAngle*2*PI*TurningRadius/360*40);

         FPoint Angle,Zero;
         Zero.x = 0;
         Zero.y = 0;
         Angle.x = DX;
         Angle.y = DY;

         float RAngle = (float)LAUtils.GetAngle(Angle,Zero)-90;

        float Distance =(float)sqrt((double)DX*(double)DX+(double)DY*(double)DY);

        DX = Distance * (float)cos((RAngle-PrevAngle+DAngle)*PI/180)*40;
        DY = Distance * (float)sin((RAngle-PrevAngle+DAngle)*PI/180)*40;
            

        YDisplacement += DY;
        XDisplacement += Delta+ DX;
    #else
         float Delta = (float)(DAngle*2*PI*TurningRadius/360*40);

         FPoint Angle,Zero;
         Zero.x = 0;
         Zero.y = 0;
         Angle.x = DX;
         Angle.y = DY;

         double RAngle = LAUtils.GetAngle(Angle,Zero)-90;

        double Distance =sqrt((double)DX*(double)DX+(double)DY*(double)DY);

        DX = Distance * cos((RAngle-PrevAngle+DAngle)*PI/180);
        DY = Distance * sin((RAngle-PrevAngle+DAngle)*PI/180);
            

        YDisplacement += DY;
        XDisplacement += Delta+ DX;

    #endif*/

    

    

}

/************************************************************************
* Function GetAngle()
*
* PURPOSE
* Returns the Angle
*************************************************************************/
float SOpticalMouse::GetAngle()
{
    return CurrentAngle;

}


