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

#include "stdafx.h"
#include "Simulator.h"
#include "SShaftEncoders.h"

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

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

SShaftEncoders::SShaftEncoders()
{
    DistanceFromCenter = 9; // in cms
    // the circumfrunce of the wheel is 4.5 cm, with 12 ticks per revolutions
    TicksConst = float(12/(4.5*PI));

    SEReset(true);
}

SShaftEncoders::~SShaftEncoders()
{

}

/************************************************************************
* Function SEReset()
*
* PURPOSE
* Clears the shaft encoders
*************************************************************************/
void SShaftEncoders::SEReset(BOOL Complete)
{
    if (Complete)
    {
        LeftShaftEncoder = 0;
        RightShaftEncoder = 0;  
		LeftShaftEncoder3D = 0;
        RightShaftEncoder3D = 0;  
		TickCounter3 = 0;
        TickCounter2 = 0;
	    TickCounter1 = 0;
    }
    else
    {
        // just reset counters to 0
        LeftShaftEncoder = LeftShaftEncoder - (long)LeftShaftEncoder;
        RightShaftEncoder = RightShaftEncoder - (long)RightShaftEncoder;
    }


}

/************************************************************************
* Function IncrementDisplacement(float DAngle, float DX, float DY)
*
* PURPOSE
* Increments the left and right shaft encoders, calls another version of this function
* which does the real work
*************************************************************************/
void SShaftEncoders::IncrementDisplacement(float PrevAngle, float DAngle, float DX, float DY, BOOL Foward)
{
    float OldLeftShaft = LeftShaftEncoder;
    float OldRightShaft = RightShaftEncoder;

	// Abs the shaft encoder output
    IncrementDisplacement(LeftShaftEncoder,PrevAngle,DAngle,DX,DY,true,true,Foward);
    IncrementDisplacement(RightShaftEncoder,PrevAngle,DAngle,DX,DY,false,true,Foward);

	// do not abs the tread displacement because is could go backwards
    IncrementDisplacement(LeftShaftEncoder3D,PrevAngle,DAngle,DX,DY,true,false,Foward);
    IncrementDisplacement(RightShaftEncoder3D,PrevAngle,DAngle,DX,DY,false,false,Foward);

    float DeltaLeft = LeftShaftEncoder - OldLeftShaft;
    float DeltaRight = RightShaftEncoder - OldRightShaft;

    // add the greater of to the software counters
	// the average is a slightly more accurate model.
    if (DeltaLeft > DeltaRight)
    {
        TickCounter1 += (DeltaLeft+DeltaRight)/2;
        TickCounter2 += (DeltaLeft+DeltaRight)/2;//DeltaLeft;
		TickCounter3 += (DeltaLeft+DeltaRight)/2;		
    }
    else
    {
        TickCounter1 += (DeltaLeft+DeltaRight)/2;
        TickCounter2 += (DeltaLeft+DeltaRight)/2;//DeltaRight;
		TickCounter3 += (DeltaLeft+DeltaRight)/2;
    }

}


/************************************************************************
* Function IncrementDisplacement1(float DAngle, float DX, float DY)
*
* PURPOSE
* Increments a shaft encoder
* ALGORITHM
* 1) For a shaft encoder, find its center track start and end points based on the angle
*    and model center at the time.
* 1a) Normallize the DY and DY so that the starting center is always 0,0 and DY is always >> then DX
* 2) Use that point to find out the circular distance, which is then the incremenent.
* 3) If the model when in a straight line, as such there is no circular radius so a cutoff
*    is used where DX and DY are used instead
*************************************************************************/
void SShaftEncoders::IncrementDisplacement(float &toIncrement, float PrevAngle, float DAngle, float DX, float DY, BOOL Left, BOOL Abs, BOOL Foward)
{
    float Straight = (float)0.0001;
    float RadiusCutoff = 1000000;

    int Sign;

    if (Left)
        Sign = 1;
    else
        Sign = -1;

    // don't ask, its seems to work better this way
   // if (LAUtils.Mod360(PrevAngle) + DAngle > 180)
    //    DY = -DY;

	FPoint Abser;
	Abser.x = DX;
	Abser.y = DY;

	// normallize displacements to a "0" start
	float Sum = sqrtf(DX*DX+DY*DY);
	DX = Sum*sinf(DAngle*PI/180.0f);
	DY = Sum*cosf(DAngle*PI/180.0f);

	// now add some polarity
	FPoint Zero;
	Zero.x = 0;
	Zero.y = 0;
	float perpAngle = (float)LAUtils.GetAngle(Abser,Zero);

	if (Foward)
		DY = -DY;

    FPoint Start,End;

    Start.x = -Sign*DistanceFromCenter;
    Start.y = 0;
    
    End.x = -Sign*DistanceFromCenter+DX;
    End.y = DY;

    FPoint Center;
    Center.x = DX;
    Center.y = DY;

    End = LAUtils.RotatePoint(End,Center,DAngle);

    // it went straight
    if ((LAUtils.Abs(Start.x - End.x) < Straight) && (LAUtils.Abs(DAngle) < Straight))
    {
		if (Abs)
			toIncrement += LAUtils.Abs(Start.y-End.y);
		else
		{
			// to figure out if we are going fowards or backwards, we take a look
			// at the Dx Dy angle and depending if we have phase lead or lag we are going fowards
			// or backwards

			FPoint Zero;
			Zero.x = 0;
			Zero.y = 0;
			float perpAngle = (float)LAUtils.GetAngle(Abser,Zero);

			// don't ask about the math, it just works this way
		//	if ((LAUtils.IsEqual(PrevAngle,perpAngle,0.1f)) || (LAUtils.IsEqual(PrevAngle+perpAngle,360.0,0.1f)))
				toIncrement += Start.y-End.y;
		//	else				
		//		toIncrement -= Start.y-End.y;

		}

    }
	// this else if is obsolete and never called, but may as well leave it in here
    else if (LAUtils.Abs(Start.y - End.y) < Straight)
    {
		if (Abs)
			toIncrement += LAUtils.Abs(Start.x-End.x);
		else
		{
			FPoint Zero;
			Zero.x = 0;
			Zero.y = 0;
			double perpAngle = LAUtils.GetAngle(Center,Zero) - PrevAngle;
			if (perpAngle > 0)
			{
				// fowards		
				toIncrement += Start.x-End.x;
			}
			else
			{
				// backwards		
				toIncrement -= Start.x-End.x;
			}
		}


    }
    else
    {
        ULinearAlgebra::Matrix2X2 Line1 = LAUtils.PointstoLine(Start,0);
        ULinearAlgebra::Matrix2X2 Line2 = LAUtils.PointstoLine(End,DAngle);      

        FPoint CenterPoint = LAUtils.FindIntersectionF(Line1,Line2);

        float Radius = LAUtils.GetDistance(CenterPoint,Start);     

        if ((Radius > 0) && (Radius < RadiusCutoff))
		{
			if (Abs)
				toIncrement += (float)LAUtils.Abs((Radius*2*PI*DAngle/360));
			else
			{
				if (!Left)
				{
					int asdf;
					asdf = 4;
				}
				if (End.y > Start.y)
					toIncrement -= (float)(Radius*2*PI*LAUtils.Abs(DAngle)/360);
				else
				{
					toIncrement += (float)(Radius*2*PI*LAUtils.Abs(DAngle)/360);
					toIncrement = LAUtils.Abs(toIncrement);
				}
			}

		}
        else
		{
			// this pratically never is neccessary
            toIncrement += LAUtils.GetDistance(Start,End);
		}
    }

}

/************************************************************************
* Function long SEGetLeftTicks()
*
* PURPOSE
* Return the amount of ticks on the left shaft encoder
*************************************************************************/
long SShaftEncoders::SEGetLeftTicks()
{
    return (long)(LeftShaftEncoder*TicksConst);
}

/************************************************************************
* Function long SEGetRightTicks()
*
* PURPOSE
* Return the amount of ticks on the right shaft encoder
*************************************************************************/
long SShaftEncoders::SEGetRightTicks()
{
    return (long)(RightShaftEncoder*TicksConst);
}

/************************************************************************
* Function short SEGetTick1()
* ROBOT API
* PURPOSE
* Return the amount of ticks in the 1 software counter
*************************************************************************/
short SShaftEncoders::SEGetTick1()
{
    return (short)(TickCounter1*TicksConst);
}

/************************************************************************
* Function short SEGetTick2()
* ROBOT API
* PURPOSE
* Return the amount of ticks in the 2 software counter
*************************************************************************/
short SShaftEncoders::SEGetTick2()
{
    return (short)(TickCounter2*TicksConst);

}

/************************************************************************
* Function short SEGetTick3()
* ROBOT API
* PURPOSE
* Return the amount of ticks in the 2 software counter
*************************************************************************/
short SShaftEncoders::SEGetTick3()
{
    return (short)(TickCounter3*TicksConst);

}

/************************************************************************
* Function short SEGetTick1()
* ROBOT API
* PURPOSE
* reset the amount of ticks in the 1 software counter
*************************************************************************/
void SShaftEncoders::SEResetTick1()
{
      TickCounter1 = TickCounter1 - (short)TickCounter1;

}

/************************************************************************
* Function short SEGetTick1()
* ROBOT API
* PURPOSE
* reset the amount of ticks in the 2 software counter
*************************************************************************/
void SShaftEncoders::SEResetTick2()
{
      TickCounter2 = TickCounter2 - (short)TickCounter2;

}

/************************************************************************
* Function short SEGetTick1()
* ROBOT API
* PURPOSE
* reset the amount of ticks in the 3 software counter
*************************************************************************/
void SShaftEncoders::SEResetTick3()
{
      TickCounter2 = TickCounter3 - (short)TickCounter3;

}

/************************************************************************
* Function float GetLeftDelta()
* 
* PURPOSE
* to return the displacement since the last call as a float, for 3D tread rendering
*************************************************************************/
float SShaftEncoders::GetLeftDelta()
{
	float tReturn = (LeftShaftEncoder3D)*10;
	LeftShaftEncoder3D = 0;
	return tReturn;

}

/************************************************************************
* Function float GetRightDelta()
* 
* PURPOSE
* to return the displacement since the last call as a float, for 3D tread rendering
*************************************************************************/
float SShaftEncoders::GetRightDelta()
{
	float tReturn = RightShaftEncoder3D*10;
	RightShaftEncoder3D = 0;
	return tReturn;
	
}
