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

#include "stdafx.h"
#include "Simulator.h"
#include "GenericModel.h"

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

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

SGenericModel::SGenericModel()
{
    _Construct();

    MVReset();
    Debug1 = 0;
    DirectXEnumPtr = NULL;

	SimMultiplier = 1;

	QueryPerformanceCounter(&StartTime);
	QueryPerformanceFrequency(&Frequency);
}

SGenericModel::~SGenericModel()
{
    _Destruct();
}

/************************************************************************
* Function LoadModel(CString FileName)
*
* PURPOSE
* To load the physical parameter of the model
* ALGORITHM
* Currently it just loads lines, without any grouping information.  Code taken from 
* Quadtree fileopenmap.
*************************************************************************/
void SGenericModel::FileLoadModel(CString FileName)
{
    unsigned char buffer[MAXFILESIZE]; // should allow for around 4000 objects to be saved
    unsigned char *bufferptr = buffer;

    // reset the variables
    _Destruct();
    _Construct();

    // O.K now open the file
    CFile NewFile;
    NewFile.Open(FileName,CFile::modeRead|CFile::typeBinary ,NULL);
    long index = NewFile.Read(buffer,200000);
    NewFile.Close();

    CPoint Start,Middle,End;
    long GroupID;

    // first load up the objects
    while ((index > 0) && (*bufferptr != 0xff))
    {
        int color = *bufferptr;  // it handles the conversion correctly without any fancy stuff
        bufferptr++;
        int objecttype = *bufferptr; // it handles the conversion correctly without any fancy stuff
        bufferptr++;
        FileChartoLong(Start.x,bufferptr);
        FileChartoLong(Start.y,bufferptr);
        FileChartoLong(Middle.x,bufferptr);
        FileChartoLong(Middle.y,bufferptr);
        FileChartoLong(End.x,bufferptr);
        FileChartoLong(End.y,bufferptr);
        FileChartoLong(GroupID,bufferptr);

        if (objecttype == LINE)
        {
            // first insert into the object bound list
            OBJECT *ptrObject = new OBJECT;
             #ifdef USEFLOAT
                ptrObject->start.x = (float)Start.x;
                ptrObject->start.y = (float)Start.y;
                ptrObject->end.x = (float)End.x;
                ptrObject->end.y = (float)End.y;
            #else
                ptrObject->start.x = (double)Start.x;
                ptrObject->start.y = (double)Start.y;
                ptrObject->end.x = (double)End.x;
                ptrObject->end.y = (double)End.y;
            #endif    
            
            ptrObject->objectColor = NEUTRALCOLOR;
            ptrObject->renderflag = 0;
            ptrObject->Next = TheModel.PhysicalModel.ObjectBounds;

            // add to list
            TheModel.PhysicalModel.ObjectBounds = ptrObject;

        }
        else if (objecttype == ARCLINE)
        {
            // do nothing, arcs is not support for a model
        }
        else if (objecttype == BUMPSENSOR)
        {
            long SensorID;
            FileChartoLong(SensorID,bufferptr);
            index -= 4;
            TheModel.Sensors.BumpSensors.LoadBumpSensor(Start,End,Middle,SensorID);
        }
        else if (objecttype == IRSENSOR)
        {
            long SensorID;
            FileChartoLong(SensorID,bufferptr);
            index -= 4;
            TheModel.Sensors.IRSensors.LoadIRSensor(Middle,Start.x,Start.y,End.x,SensorID);
        }
        else if (objecttype == SONARSENSOR)
        {            
            TheModel.Sensors.SonarSensor.LoadSonarSensor(Middle);
        }

        index -= OBJECTSIZE;
    }

    index-= 1; // for the 0xff characters
    bufferptr++;

    // o.k recieved 0xff end load model
    // ignore the grouping information that comes next

    // find the models center and adjust all of the other objects accordingly.
    UAdjustforCenter();

    // give the info to the bump sensors
    RSUpdateBumpSensors();
    RSUpdateIRSensors();
    RSUpdateSonarSensor(0);

    char tempo[100];
    sprintf(tempo,"C:\\JPROJECT\\simulator\\LBeammodel.mpp");

    TheModel.Sensors.SonarSensor.LoadSonarBeam(tempo); 
      
}

/************************************************************************
* Function FileChartoLong(long &toRecieve, unsigned char *&ptr)
*
* PURPOSE
* Copies information that is in a byte array to a long
* USAGE
* by the save/load file functions
*************************************************************************/
void SGenericModel::FileChartoLong(long &toRecieve, unsigned char *&ptr)
{
    
    unsigned char *ptr1 = &(unsigned char &)(toRecieve);
    for (int ix = 0; ix < 4; ix++)
    {
        *ptr1 = *ptr;
        ptr++;
        ptr1++;
    }

}

// initilizes the class
void SGenericModel::_Construct()
{
    TheModel.PhysicalModel.ObjectBounds = NULL;
    TheModel.PhysicalModel.Center.x = 0;
    TheModel.PhysicalModel.Center.y = 0;
    TheModel.ModelState.x = 6000;
    TheModel.ModelState.y = 6000;
    TheModel.ModelState.CurrAngle = 90;
    TheModel.ModelState.dx = 0;
    TheModel.ModelState.dy = 0;
    TheModel.ModelState.DAngle = 0;
    TheModel.CollisionPoint.x = NOCOLLISION;    
}

// deletes variables
void SGenericModel::_Destruct()
{
    while (TheModel.PhysicalModel.ObjectBounds != NULL)
    {
        OBJECT *ObjectPtr;
        ObjectPtr = TheModel.PhysicalModel.ObjectBounds;
        TheModel.PhysicalModel.ObjectBounds = TheModel.PhysicalModel.ObjectBounds->Next;
        delete ObjectPtr;
        ObjectPtr = NULL;
    }

    ObjectPtr Prev;

    while (DirectXEnumPtr != NULL)
    {
        Prev = DirectXEnumPtr;
        DirectXEnumPtr = DirectXEnumPtr->Next;
        delete Prev;
        Prev = NULL;
    }
}

/************************************************************************
* Function FileChartoLong(long &toRecieve, unsigned char *&ptr)
*
* PURPOSE
* To render the model
* USAGE
* By the GDIrenderclass
* ALGORITHM
* Traverses the model list and renders all of the objects.
*************************************************************************/
void SGenericModel::RenderGDI(CDC &MemDC, FRect Quad, CRect Screen, float SRScale, FPoint RenderModelCenter)
{      
    
    float XScale = (float)Screen.Width()/(float)Quad.Width();
    float YScale = (float)Screen.Height()/(float)Quad.Height();

    // tranform e.i where to start the lines
    float XTransform = Quad.left;
    float YTransform = Quad.top;

    ObjectPtr object = UGetRotatedBounds(RenderModelCenter);
    ObjectPtr Prev;

    while (object != NULL)
    {   
        // select the right colored pen
        CPen *OldPen = MemDC.SelectObject(&MapPtr->ColorPens[object->objectColor]);                
              
        long XStart = LAUtils.Round((object->start.x-XTransform)*XScale);
        long YStart = LAUtils.Round((object->start.y-YTransform)*YScale);
        long XEnd =  LAUtils.Round((object->end.x-XTransform)*XScale);
        long YEnd =  LAUtils.Round((object->end.y-YTransform)*YScale);
        LAUtils.ClipLine(Screen,XStart,YStart,XEnd,YEnd); 
        MemDC.MoveTo(XStart,YStart);
        MemDC.LineTo(XEnd,YEnd);

        MemDC.SelectObject(OldPen);

        Prev = object;
        object = object->Next;
        delete Prev;
        Prev = NULL;
        
    }

    TheModel.Sensors.BumpSensors.GDIRender(MemDC, Quad, Screen, SRScale, RenderModelCenter);
    TheModel.Sensors.IRSensors.GDIRender(MemDC, Quad, Screen, SRScale, RenderModelCenter);
    TheModel.Sensors.SonarSensor.GDIRender(MemDC, Quad, Screen, SRScale, RenderModelCenter);

}

/************************************************************************
* Function ObjectPtr UGetRotatedBounds()
*
* PURPOSE
* To return the bounds of the model rotates with the current angle
* USAGE
* Various, colision detections, rendersing etc.
* NOTE
* Intial translating is required because the model is always stored at 0 degrees starting at 0,0
*************************************************************************/
SGenericModel::ObjectPtr SGenericModel::UGetRotatedBounds()
{
    ObjectPtr TraversePtr = TheModel.PhysicalModel.ObjectBounds;
    ObjectPtr ReturnPtr,tPtr;

    FPoint Center;

    Center.x = TheModel.ModelState.x;
    Center.y = TheModel.ModelState.y;


    FPoint Start,End;
    double Angle = TheModel.ModelState.CurrAngle;

    ReturnPtr = NULL;


    while (TraversePtr != NULL)
    {
        tPtr = new OBJECT;

        // copy all of the data over
        *tPtr = *TraversePtr;

        Start = TraversePtr->start;
        End = TraversePtr->end;

        // translate the points
        Start.x += Center.x - TheModel.PhysicalModel.Center.x;
        End.x += Center.x - TheModel.PhysicalModel.Center.x;
        Start.y += Center.y - TheModel.PhysicalModel.Center.y;
        End.y += Center.y - TheModel.PhysicalModel.Center.y;

        // rotate and translate the points
        Start = LAUtils.RotatePoint(Start,Center,Angle);
        End = LAUtils.RotatePoint(End,Center,Angle);     

        tPtr->start = Start;
        tPtr->end = End;

        tPtr->RealObject = TraversePtr; // link the rotated ptr to the real one.

        // link it to the list
        tPtr->Next = ReturnPtr;
        ReturnPtr = tPtr;

        TraversePtr = TraversePtr->Next;

    }

    return ReturnPtr;

}

/************************************************************************
* Function ObjectPtr UGetRotatedBounds(FPoint RenderModelCenter)
*
* PURPOSE
* To return the bounds of the model rotates with the current angle
* USAGE
* Various, colision detections, rendersing etc.
* NOTE
* Intial translating is required because the model is always stored at 0 degrees starting at 0,0
* Also same as UGetRotatedBounds(), except uses the Render Center
*************************************************************************/
SGenericModel::ObjectPtr SGenericModel::UGetRotatedBounds(FPoint RenderModelCenter)
{
    ObjectPtr TraversePtr = TheModel.PhysicalModel.ObjectBounds;
    ObjectPtr ReturnPtr,tPtr;

    FPoint Center = RenderModelCenter;



    FPoint Start,End;
    double Angle = TheModel.ModelState.CurrAngle;

    ReturnPtr = NULL;


    while (TraversePtr != NULL)
    {
        tPtr = new OBJECT;

        // copy all of the data over
        *tPtr = *TraversePtr;

        Start = TraversePtr->start;
        End = TraversePtr->end;

        // translate the points
        Start.x += Center.x - TheModel.PhysicalModel.Center.x;
        End.x += Center.x - TheModel.PhysicalModel.Center.x;
        Start.y += Center.y - TheModel.PhysicalModel.Center.y;
        End.y += Center.y - TheModel.PhysicalModel.Center.y;

        // rotate and translate the points
        Start = LAUtils.RotatePoint(Start,Center,Angle);
        End = LAUtils.RotatePoint(End,Center,Angle);     

        tPtr->start = Start;
        tPtr->end = End;

        tPtr->RealObject = TraversePtr; // link the rotated ptr to the real one.

        // link it to the list
        tPtr->Next = ReturnPtr;
        ReturnPtr = tPtr;

        TraversePtr = TraversePtr->Next;

    }

    return ReturnPtr;

}

/************************************************************************
* Function ObjectPtr UGetRotatedBounds()
*
* PURPOSE
* To find the center of a model and adjust all of the other bounding objects to the center
* USAGE
* after the fileloadmodel is done, to adjust the center
* ALGORITHM
* Find the furthest dimensions and then take there midpoint
* WHY?
* Because the internal representations needs a center of movement, and the bounding lines
* shut start at 0,0.
*************************************************************************/
void SGenericModel::UAdjustforCenter()
{
    ObjectPtr TraversePtr = TheModel.PhysicalModel.ObjectBounds;

    #ifdef USEFLOAT
        float XL,XR,YT,YB;
    #else
        double XL,XR,YT,YB;
    #endif         

    // nothing out of the mapper should be less then or greater then these two numbers.
    XL = YT = 1000000000;
    YB = XR = -1;

    // XL X Left
    // XR X Right
    // YT Y Top
    // YB Y Bottom

    // find the center of the model
    while (TraversePtr != NULL)
    {
        // run through all of the cases
        if (TraversePtr->start.x < XL)
        {
            XL = TraversePtr->start.x;
        }
        if (TraversePtr->start.x > XR)
        {
            XR = TraversePtr->start.x;
        }
        if (TraversePtr->end.x < XL)
        {
            XL = TraversePtr->end.x;
        }
        if (TraversePtr->end.x > XR)
        {
            XR = TraversePtr->end.x;
        }
        if (TraversePtr->start.y < YT)
        {
            YT = TraversePtr->start.y;
        }
        if (TraversePtr->start.y > YB)
        {
            YB = TraversePtr->start.y;
        }
        if (TraversePtr->end.y < YT)
        {
            YT = TraversePtr->end.y;
        }
        if (TraversePtr->end.y > YB)
        {
            YB = TraversePtr->end.y;
        }
        TraversePtr = TraversePtr->Next;
    }

    TheModel.PhysicalModel.Center.x = (XR-XL)/(float)2.0;
    TheModel.PhysicalModel.Center.y = (YB-YT)/(float)2.0;

    // now readjust all of the objects so the center is the center.
    TraversePtr = TheModel.PhysicalModel.ObjectBounds;
    while (TraversePtr != NULL)
    {
        TraversePtr->start.x += -XL;
        TraversePtr->end.x += -XL;
        TraversePtr->start.y += -YT;
        TraversePtr->end.y += -YT;

        TraversePtr = TraversePtr->Next;
    }
    // do the same for the bump sensors;
    TheModel.Sensors.BumpSensors.AdjustforCenter((float)XL,(float)YT);
    TheModel.Sensors.IRSensors.AdjustforCenter((float)XL,(float)YT);
    TheModel.Sensors.SonarSensor.AdjustforCenter((float)XL,(float)YT);
    
}

// This function is called to simulate the model
void SGenericModel::RunSimulation(float TimeElapse)
{
    RSUpdatePosition(TimeElapse); 
    RSUpdateBumpSensors();    
    RSUpdateIRSensors();
    RSUpdateSonarSensor(TimeElapse);

}

// garbage functions for debugging.
void SGenericModel::InputfromKeyboard(UINT nChar)
{
    double Speed = 5; // 5cm per second per increment
    double Ratio = 1;
    double DAngle = .2*100;

    double NewDX = TheModel.ModelState.dx;
    double NewDY = TheModel.ModelState.dy;
    double NewDAngle = TheModel.ModelState.DAngle;

    double MaxSpeed = 100;
    double MaxAngle = 2*100;

    if (nChar == 49)
    {
        // num 1        
        NewDX += Speed*Ratio;
        NewDY += Speed*Ratio;
        NewDAngle += -DAngle*Ratio;
    }
    else if (nChar == 50)
    {
        // num 2
        NewDY += Speed;
        NewDX += Speed;
        NewDAngle = 0;
    }
    else if (nChar == 51)
    {
        // num 3
        NewDY += Speed*Ratio;
        NewDX += Speed*Ratio;
        NewDAngle += DAngle*Ratio;
    }
    else if (nChar == 52)
    {
        // num 4
        NewDAngle += -DAngle;
        NewDY = 0;
        NewDX = 0;
    }
    else if (nChar == 53)
    {
        // num 5
        NewDAngle = 0;
        NewDY = 0;
        NewDX = 0;
    }
    else if (nChar == 54)
    {
        // num 6
        NewDAngle += DAngle;
        NewDY = 0;
        NewDX = 0;
    }
    else if (nChar == 55)
    {
        // num 7
        NewDY += -Speed*Ratio;
        NewDX += -Speed*Ratio;
        NewDAngle += DAngle*Ratio;
    }
    else if (nChar == 56)
    {
        // num 8
        NewDY += -Speed;
        NewDX += -Speed;
        NewDAngle = 0;
    }
    else if (nChar == 57)
    {
        // num 9
        NewDY += -Speed*Ratio;
        NewDX += -Speed*Ratio;
        NewDAngle += -DAngle*Ratio;
    } 

    double ABSDangle = NewDAngle;
    double ABSDX = NewDX;
    double ABSDY = NewDY;

    if (ABSDangle < 0)
        ABSDangle = -ABSDangle;

    if (ABSDX < 0)
        ABSDX = -ABSDX;

     if (ABSDY < 0)
        ABSDY = -ABSDY;
    
    if (!(ABSDX > MaxSpeed) && !(ABSDY > MaxSpeed))
    {
        #ifdef USEFLOAT
             TheModel.ModelState.dy = (float)NewDY;
             TheModel.ModelState.dx = (float)NewDX;
        #else
            TheModel.ModelState.dy = NewDY;
            TheModel.ModelState.dx = NewDX;
        #endif     
       
    }
    if (!(ABSDangle > MaxAngle))
    {
        #ifdef USEFLOAT
            TheModel.ModelState.DAngle = (float)NewDAngle;
        #else
            TheModel.ModelState.DAngle = NewDAngle;
        #endif  
        
    }    

}

/************************************************************************
* Function RSUpdatePosition()
*
* PURPOSE
* To move the model into a new position defined by its dx,dy and dangle
* USAGE
* By RunSimulation
* ALGORITHM
* This ones a doosy
* 1) Generate a pointer to the current bounds of the model
* 2) Increment the model state using dx,dy and dangle
* 3) Generate a pointer to the new bounds of the model
* 4) Find out if any of the bounds have a collision
     This is better defined in the subfunctions which are called but here is how it works
     a) If there are any objects in between the old bounds and the new bounds a
        collision occured
     b)  bounds and checking if they are valid.
     This is checked by the quadtree by looking and generating any points that could be between the 
       c) For all bounds 
  5) If one or many collision points have occured find the one with the shortest distance
     and then find out have to move the model right up to this point
  6) Use 5) to form the new model state
  7) Check if the algorithm failed by seeing if any lines intersect the new model, if so roll
     back the movement.  This only happens rarily but there are some failures to the algorith.
*************************************************************************/
void SGenericModel::RSUpdatePosition(float TimeElapse)
{

    BOOL isCollision = false;

    if ((TheModel.ModelState.dx != 0) || (TheModel.ModelState.dy != 0)
        || (TheModel.ModelState.DAngle != 0))
    {

        // generate the old bounds, then the new position then the new bounds
        FPoint DInc;

        FPoint ReverseDelta;       

        ObjectPtr OldBounds = UGetRotatedBounds();

        DInc.x = TheModel.ModelState.dx*TimeElapse;
        DInc.y = TheModel.ModelState.dy*TimeElapse;

        float PrevAngle = TheModel.ModelState.CurrAngle;

        TheModel.ModelState.CurrAngle += TheModel.ModelState.DAngle*TimeElapse;

        // keep it between 0 and 360 for easy of read
        TheModel.ModelState.CurrAngle = LAUtils.Mod360(TheModel.ModelState.CurrAngle);

        #ifdef USEFLOAT
            float Y = (float)sin(PI/180.0*(TheModel.ModelState.CurrAngle));
            float X = (float)cos(PI/180.0*(TheModel.ModelState.CurrAngle));
        #else
            double Y = sin(PI/180.0*(TheModel.ModelState.CurrAngle));
            double X = cos(PI/180.0*(TheModel.ModelState.CurrAngle));
        #endif  
        

        DInc.x = -(DInc.x * X);
        DInc.y = (DInc.y * Y);

        TheModel.ModelState.x += DInc.x;
        TheModel.ModelState.y += DInc.y;

        ObjectPtr NewBounds = UGetRotatedBounds();
        ObjectPtr NewPrev,OldPrev;

        ReverseDelta.x = (float)99999999.0;
        ReverseDelta.y = (float)99999999.0;

        double SavedAbsDelta = 99999999.0;

        TheModel.Sensors.OpticalMouse.IncrementDisplacement(PrevAngle,TheModel.ModelState.DAngle*TimeElapse,
                    DInc.x,DInc.y);

		BOOL Foward;

		if (TheModel.ModelState.dx > 0)		
			Foward = false;		
		else
			Foward = true;

        TheModel.Sensors.ShaftEncoders.IncrementDisplacement(PrevAngle,TheModel.ModelState.DAngle*TimeElapse,
                    DInc.x,DInc.y,Foward);


        // O.K check for the collision in between the two bounds
        while (NewBounds != NULL)
        {   
            float AbsLocalDelta;
            double AbsOldLocalDelta = 99999999.0;
            BOOL FromAbove = false; // because there has to be a global distance, which is invalidates
            // if it is done from above because the second check takes precedence

            FPoint Collision = MapPtr->DetectCollision(OldBounds->start,OldBounds->end,NewBounds->start,NewBounds->end);       
            if (Collision.x == NOCOLLISION)
            {
                NewBounds->RealObject->objectColor = NEUTRALCOLOR;
            }
            else
            {                
                NewBounds->RealObject->objectColor = SELECTCOLOR;
                isCollision = true;

                //////////////////////////////////////////////////////
                // Now we have to find how far to move up the model to make it nearly touch the collision point
                // Algorithm
                // 1)  Find the delta to move the model
                // 2)  To do this you have to translate the collision point into the bounds
                // 3)  Because this is a hard problem I approximate it
                // 4)  Find the closest point that translates from the collision point to the line
                // 5)  This point is either the intersection of the perpindicular line with the bound lines
                //     or one of the end points.
                // 6)  Run a complicated check of checking both the real distance and the perpindicular distance to 
                //     Select the point (also compared to every collision point)
                // 7)  At the end a dx and dy are generated that could be used in the orignal model
                //     (Note DAngle ignored)
                    

                FPoint Point1 = OldBounds->start;
                FPoint Point2 = OldBounds->end;

                // the line which the point needs to be found
                ULinearAlgebra::Matrix2X2 NewLine = LAUtils.PointstoLine(Point1,Point2);
                // a perpindicular line used to find the closest point.
                ULinearAlgebra::Matrix2X2 PerpLine = LAUtils.PointstoPerpLine(Collision,NewLine);

                FPoint Intersection = LAUtils.FindIntersectionF(PerpLine,NewLine);

                // have the point now is it valid?
                BOOL notValid;

                if (((Intersection.x <= Point1.x) && (Intersection.x >= Point2.x))
                    || ((Intersection.x >= Point1.x) && (Intersection.x <= Point2.x)))
                {
                    if (((Intersection.y <= Point1.y) && (Intersection.y >= Point2.y))
                    || ((Intersection.y >= Point1.y) && (Intersection.y <= Point2.y)))
                    {                        
                        notValid = false;
                    }
                    else
                    {
                        notValid = true;
                    }
                }
                else
                {
                    notValid = true;
                }          
                
                // it is valid find out how close it is
                if (!notValid)
                {
                    // O.K figure out the delta
                    FPoint ReverseDeltatemp;
                    ReverseDeltatemp.x = -(Intersection.x - Collision.x);
                    ReverseDeltatemp.y = -(Intersection.y - Collision.y);

                    AbsLocalDelta = MapPtr->DetectCollisionDistance(Intersection,Collision,OldBounds->start,OldBounds->end,NewBounds->start,NewBounds->end);

                    // o.k this is is closer then the last one, so lets use it.
                    if (AbsLocalDelta < AbsOldLocalDelta)
                    {

                         double AbsDelta = sqrt(ReverseDeltatemp.x*ReverseDeltatemp.x +   ReverseDeltatemp.y*  ReverseDeltatemp.y);
                         double AbsOldDelta = sqrt(ReverseDelta.x*ReverseDelta.x +   ReverseDelta.y*  ReverseDelta.y);
                        // check the overall distance, another collision may be shorter.
                        if (AbsDelta < AbsOldDelta)
                        {
                            ReverseDelta = ReverseDeltatemp;
                            TheModel.CollisionPoint = Collision; 
                            SavedAbsDelta = AbsOldDelta;
                            FromAbove = true;
                        }
                        AbsOldLocalDelta = AbsLocalDelta;
                    }  
                 }

                // Have to always check both the start and end points
                float StartD = LAUtils.GetDistance(Collision,Point1);
                float EndD = LAUtils.GetDistance(Collision,Point2);

                if (StartD < EndD)
                {
                    Intersection = Point1;
                }
                else
                {
                    Intersection = Point2;
                }

                 // O.K figure out the delta
                FPoint ReverseDeltatemp;
                ReverseDeltatemp.x = -(Intersection.x - Collision.x);
                ReverseDeltatemp.y = -(Intersection.y - Collision.y);

                AbsLocalDelta = MapPtr->DetectCollisionDistance(Intersection,Collision,OldBounds->start,OldBounds->end,NewBounds->start,NewBounds->end);

                // o.k this is is closer then the last one, so lets use it.
                if (AbsLocalDelta < AbsOldLocalDelta)
                {
                     double AbsDelta = sqrt(ReverseDeltatemp.x*ReverseDeltatemp.x +   ReverseDeltatemp.y*  ReverseDeltatemp.y);
                     double AbsOldDelta = sqrt(ReverseDelta.x*ReverseDelta.x +   ReverseDelta.y*  ReverseDelta.y);
                    // check the overall distance, another collision may be shorter.
                    if ((AbsDelta < AbsOldDelta)|| (FromAbove && (AbsDelta <= SavedAbsDelta)))
                    {
                        ReverseDelta = ReverseDeltatemp;
                        TheModel.CollisionPoint = Collision;                        
                    }
                    AbsOldLocalDelta = AbsLocalDelta;
                }  
            }            

            // clean up the pointers
            NewPrev = NewBounds;
            NewBounds = NewBounds->Next;
            delete NewPrev;
            NewPrev = NULL;

            OldPrev = OldBounds;
            OldBounds = OldBounds->Next;
            delete OldPrev;
            OldPrev = NULL;
        
        }

        if (isCollision)
        {
            // oops can't collide change to prevoius values
            TheModel.ModelState.x -= 1*DInc.x;
            TheModel.ModelState.y -= 1*DInc.y;
            TheModel.ModelState.CurrAngle -= 1*TheModel.ModelState.DAngle*TimeElapse;

            // proposed to improve colliion to have it move up to here, has its flaws
            float XSign,YSign;
            if (ReverseDelta.x > 0)
                XSign = -1;
            else
                XSign = 1;
            if (ReverseDelta.y > 0)
                YSign = -1;
            else
                YSign = 1;

            // if they are not the same sign something screwy is happening so don't increment anything
            // Sometimes DInc is really small and the opposite sign so a zero check is needed too.
            if (((ReverseDelta.x *DInc.x > 0) || LAUtils.IsEqual((float)DInc.x,0,(float)SMALLPLUSMINUS))&& ((ReverseDelta.y * DInc.y > 0) || LAUtils.IsEqual((float)DInc.y,0,(float)SMALLPLUSMINUS)))
            {
               // BOOL SpecialCase = false;
                float SmallPercentage =(float)0.1;

                TheModel.ModelState.x += ReverseDelta.x + SmallPercentage*XSign;
                TheModel.ModelState.y += ReverseDelta.y + SmallPercentage*YSign; 

                // run the safity check
                 NewBounds = UGetRotatedBounds();
                 while (NewBounds != NULL)
                 {
                     if (MapPtr->DetectCollisionSafetyCheck(NewBounds->start,NewBounds->end))
                     {                             
                          //oops made an error, lets reverse it.
                          TheModel.ModelState.x += -ReverseDelta.x - SmallPercentage*XSign;
                          TheModel.ModelState.y += -ReverseDelta.y - SmallPercentage*YSign;                          
                      }
                     // clean up pointers
                     NewPrev = NewBounds;
                     NewBounds = NewBounds->Next;
                     delete NewPrev;
                     NewPrev = NULL;
                 }

            }
            else if (TheModel.ModelState.DAngle != 0)
            {
                // no dx dy, but try the sanity check anyway
                // sanity check
                 NewBounds = UGetRotatedBounds();
                 while (NewBounds != NULL)
                 {
                     if (MapPtr->DetectCollisionSafetyCheck(NewBounds->start,NewBounds->end))
                     {                             
                          //oops made an error, lets reverse it.
                          TheModel.ModelState.CurrAngle -= TheModel.ModelState.DAngle;
                     }
                     // clean up pointers
                     NewPrev = NewBounds;
                     NewBounds = NewBounds->Next;
                     delete NewPrev;
                     NewPrev = NULL;
                 }
            }


            // change all deltas to zero
            TheModel.ModelState.DAngle = 0;
            TheModel.ModelState.dx = 0;
            TheModel.ModelState.dy = 0; 

        }
        else
        {
            TheModel.CollisionPoint.x = NOCOLLISION;
        }


        if (isCollision)
        {
            RSUpdateBumpSensors();
            TheModel.Sensors.BumpSensors.SetBumpSensors(TheModel.CollisionPoint);
        }
        else
        {
            TheModel.Sensors.BumpSensors.ClearBumpSensor();

        }

    }






}

/************************************************************************
* Function RSUpdateBumpSensors()
*
* PURPOSE
* To update the bump sensors information about the model
*************************************************************************/
void SGenericModel::RSUpdateBumpSensors()
{
     // update the bump sensor
    SBumpSensor::MODELINFO ModelInfo;
    ModelInfo.x = TheModel.ModelState.x;
    ModelInfo.y = TheModel.ModelState.y;
    ModelInfo.CurrAngle = TheModel.ModelState.CurrAngle;


    ModelInfo.Center.x = TheModel.PhysicalModel.Center.x;
    ModelInfo.Center.y = TheModel.PhysicalModel.Center.y;


    // call the function which updates the bump sensors information
    TheModel.Sensors.BumpSensors.UpdateModel(ModelInfo);

}

/************************************************************************
* Function RSUpdateIRSensors()
*
* PURPOSE
* To update the IR sensors information about the model
*************************************************************************/
void SGenericModel::RSUpdateIRSensors()
{
     // update the bump sensor model information
    SIRSensor::MODELINFO ModelInfo;
    ModelInfo.x = TheModel.ModelState.x;
    ModelInfo.y = TheModel.ModelState.y;
    ModelInfo.CurrAngle = TheModel.ModelState.CurrAngle;


    ModelInfo.Center.x = TheModel.PhysicalModel.Center.x;
    ModelInfo.Center.y = TheModel.PhysicalModel.Center.y;  

    
    // call the function which updates the bump sensors information
    TheModel.Sensors.IRSensors.UpdateModel(ModelInfo);

    // "runs" the IR sensors
    TheModel.Sensors.IRSensors.FindDetection();





}

/************************************************************************
* Function RSUpdateSonarSensor()
*
* PURPOSE
* To update the Sonar sensors information about the model
*************************************************************************/
void SGenericModel::RSUpdateSonarSensor(float TimeElapse)
{

    // update the bump sensor model information
    SSonarSensor::MODELINFO ModelInfo;
    ModelInfo.x = TheModel.ModelState.x;
    ModelInfo.y = TheModel.ModelState.y;
    ModelInfo.CurrAngle = TheModel.ModelState.CurrAngle;


    ModelInfo.Center.x = TheModel.PhysicalModel.Center.x;
    ModelInfo.Center.y = TheModel.PhysicalModel.Center.y;


    // call the function which updates the bump sensors information
    TheModel.Sensors.SonarSensor.UpdateModel(ModelInfo);

    TheModel.Sensors.SonarSensor.SimulateSonar(TimeElapse);

}

/************************************************************************
* Function MVGoFoward(char Speed)
* Robot API
* PURPOSE
* To move the robot in a foward direction
* Goes backwards for negative values
*************************************************************************/
void SGenericModel::MVGoFoward(char Speed)
{

    char sign = 1;

    if (Speed < 0)
    {
        sign = -1;
        Speed = abs(Speed);
    }

    if (Speed >= MAXSPEEDTABLE)
        Speed = MAXSPEEDTABLE;
    
        

    if ((Speed > 0) && (Speed < MAXSPEEDTABLE))
    {
        TheModel.ModelState.dx = -sign*(float)SpeedTable[Speed];
        TheModel.ModelState.dy = -sign*(float)SpeedTable[Speed];
        TheModel.ModelState.DAngle = 0;
    }
}

/************************************************************************
* BOOL UDoSafetyCheck()
*
* PURPOSE
* To move the robot to a new position by the mouse
* INPUT
* The new point where the robot is to be moved
* OUTPUT
* If it could be moved there is returns true
* Else it returns false and return the robot to teh previous position
*************************************************************************/
BOOL SGenericModel::UDoSafetyCheck(CPoint NewCoords)
{
    BOOL isSafe = true;

    FPoint OldCoords;

    OldCoords.x = TheModel.ModelState.x;
    OldCoords.y = TheModel.ModelState.y;

    TheModel.ModelState.x = (float)NewCoords.x;
    TheModel.ModelState.y = (float)NewCoords.y;

    // run the safity check
     ObjectPtr NewBounds = UGetRotatedBounds();
     while (NewBounds != NULL)
     {
         if (MapPtr->DetectCollisionSafetyCheck(NewBounds->start,NewBounds->end))
         {        
             isSafe = false;
                               
         }        
 
         if (!MapPtr->AddCheckifPointValid((CCPoint)NewBounds->start) ||
             !MapPtr->AddCheckifPointValid((CCPoint)NewBounds->end))
         {
             isSafe = false;
         }
         // clean up pointers
         ObjectPtr NewPrev = NewBounds;
         NewBounds = NewBounds->Next;
         delete NewPrev;
         NewPrev = NULL;
     }

     if (!isSafe)
     {
           TheModel.ModelState.x = OldCoords.x;
           TheModel.ModelState.y = OldCoords.y;
           return false;
     }
     else
     {
         // why note, updates the sensors
         RunSimulation(0);
     }


     return true;



}

/************************************************************************
* Function MVGoFoward(char Speed)
* Robot API
* PURPOSE
* To stop the robot
*************************************************************************/
void SGenericModel::MVAllStop()
{
//	ASSERT(MVStruct.FowardTicksLeft < 2);

    TheModel.ModelState.dx = 0;
    TheModel.ModelState.dy = 0;
    TheModel.ModelState.DAngle = 0;

	// reset blocking stuff
	MVStruct.MVBlocking = false;
    MVStruct.TurningState = false;
    MVStruct.FowardState = false;
    MVStruct.FowardTicksLeft = 0;
    MVStruct.TurningTicksLeft = 0;

}

/************************************************************************
* Function MVIsBlocking()
* Robot API
* PURPOSE
* To return if an action like move foward a distance or to turn is taking place,
* and to performs check if the action is done
*************************************************************************/
BOOL SGenericModel::MVIsBlocking()
{
    if (MVStruct.TurningState)
    {
        if (TheModel.Sensors.ShaftEncoders.SEGetLeftTicks() >= abs(MVStruct.TurningTicksLeft))
        {
            MVAllStop();
            MVStruct.TurningState = false;
            MVStruct.MVBlocking = false;
            MVStruct.TurningTicksLeft = 0;

        }
    }
    else if (MVStruct.FowardState)
    {
        if (TheModel.Sensors.ShaftEncoders.SEGetLeftTicks() >= abs(MVStruct.FowardTicksLeft))
        {
            MVAllStop();
            MVStruct.FowardState = false;
            MVStruct.MVBlocking = false;
            MVStruct.FowardTicksLeft = 0;
        }
    }
    
    return MVStruct.MVBlocking;

}

/************************************************************************
* Function MVTurn(short ticks)
* Robot API
* PURPOSE
* To rotate the robot on its place.  Each tick coorisponds to one shaftencoder
* or 7.5 degrees
*************************************************************************/
void SGenericModel::MVTurn(short ticks)
{
    MVStruct.TurningState = true;
    MVStruct.MVBlocking = true;
    MVStruct.TurningTicksLeft += ticks;
    TheModel.Sensors.ShaftEncoders.SEReset(false);
	ASSERT(MVStruct.FowardTicksLeft == 0);

    if (MVStruct.FowardState)
    {
        MVStruct.FowardState = false;
        MVStruct.FowardTicksLeft = 0;
    }

    if (MVStruct.TurningTicksLeft > 0)
    {
        TheModel.ModelState.DAngle = 40; // 40 degrees a second?
        TheModel.ModelState.dx = 0;
        TheModel.ModelState.dy = 0;
    }
    else if (MVStruct.TurningTicksLeft < 0)
    {
        TheModel.ModelState.DAngle = -40;
        TheModel.ModelState.dx = 0;
        TheModel.ModelState.dy = 0;
    }
    else
    {
        MVStruct.TurningState = false;
        MVStruct.MVBlocking = false;
    }
}

/************************************************************************
* Function MVReset()
* Robot API (well mabey)
* PURPOSE
* Initialize MV variables
*************************************************************************/
void SGenericModel::MVReset()
{
	ASSERT(MVStruct.FowardTicksLeft < 2);
    MVStruct.MVBlocking = false;
    MVStruct.TurningState = false;
    MVStruct.FowardState = false;
    MVStruct.FowardTicksLeft = 0;
    MVStruct.TurningTicksLeft = 0;

    for (int ix = 0; ix < MAXSPEEDTABLE; ix++)
    {
        SpeedTable[ix] = 5*(ix+1); // 5cm a second for each index
    }

    
    for (ix = 0; ix < MAXSPEEDTABLE; ix++)
    {
        AngleTable[ix] = 2*(ix+1); // 5 degree a second for each index
    }

}

/************************************************************************
* Function KRNLGetTicks()
* Robot API
* PURPOSE
* returns the tick count from 0 to 999 in milliseconds
*************************************************************************/
short SGenericModel::KRNLGetTicks()
{
    return MVStruct.KRNLTicks;

}

/************************************************************************
* MVGoFowardWA(char speed, char angle)
* Robot API (well mabey)
* PURPOSE
* To move the robot foward or backwards while slowly changing the angle.
*************************************************************************/
void SGenericModel::MVGoFowardWA(char speed, char angle)
{
    char Absangle = abs(angle);


    char sign = 1;

    if (speed < 0)
    {
        sign = -1;
        speed = abs(speed);
    }

    if (Absangle >= MAXSPEEDTABLE)
        Absangle = MAXSPEEDTABLE-1;

    if (speed >= MAXSPEEDTABLE)
        speed = MAXSPEEDTABLE-1;

    if ((speed >= 0) && (speed < MAXSPEEDTABLE)
        && (Absangle >= 0) && (Absangle < MAXSPEEDTABLE))
    {
        MVAllStop();
        TheModel.ModelState.dx = -sign*(float)SpeedTable[speed];
        TheModel.ModelState.dy = -sign*(float)SpeedTable[speed];

        if (angle > 0)
            TheModel.ModelState.DAngle = (float)AngleTable[Absangle];
        else if (angle < 0)
            TheModel.ModelState.DAngle = -(float)AngleTable[Absangle];
		else
			TheModel.ModelState.DAngle = 0;

    }

}

/************************************************************************
* DirectXSetupEnum(FPoint RenderModelCenter)
* 
* PURPOSE
* To set up the directX enum for directX 3D rendering
* USAGE 
* DirectX class
*************************************************************************/
void SGenericModel::DirectXSetupEnum(FPoint RenderModelCenter)
{
    ObjectPtr Prev;

    while (DirectXEnumPtr != NULL)
    {
        Prev = DirectXEnumPtr;
        DirectXEnumPtr = DirectXEnumPtr->Next;
        delete Prev;
        Prev = NULL;
    }

     // Set up the enum
    DirectXEnumPtr = UGetRotatedBounds(RenderModelCenter);
}

/************************************************************************
* BOOL DirectXEnum(SGenericModel::DFPoint &TwoPoints)
* 
* PURPOSE
* To give the directX class all of the model lines for rendering
* USAGE 
* DirectX class
* RETURNS
* True if the point is valid.  false if it isn't and this stops the enum at
* the directX end.
*************************************************************************/
BOOL SGenericModel::DirectXEnum(SGenericModel::DFPoint &TwoPoints)
{
    ObjectPtr Prev;

    if (DirectXEnumPtr == NULL)
        return false;
    else
    {
        TwoPoints.Point1 = DirectXEnumPtr->start;
        TwoPoints.Point2 = DirectXEnumPtr->end;

        Prev = DirectXEnumPtr;
        DirectXEnumPtr = DirectXEnumPtr->Next;
        delete Prev;
        Prev = NULL;
  
        return true;
    }
}


/************************************************************************
* Function MVGoFowardT(short Ticks)
* Robot API
* PURPOSE
* To move the robot in a foward or negative direction
* with the amount of ticks blocking the return
* Each tick is around 1.2 centimeters
* NOTE
* This is a blocking call based on MVisBlocking();
*************************************************************************/
void SGenericModel::MVGoFowardT(char Speed, short Ticks)
{
    MVStruct.FowardState = true;
    MVStruct.MVBlocking = true;
    MVStruct.FowardTicksLeft += Ticks;

    if (MVStruct.TurningState)
    {
        MVStruct.TurningState = false;
        MVStruct.TurningTicksLeft = 0;
    }

    TheModel.Sensors.ShaftEncoders.SEReset(false);

    char sign = 1;

    if (Speed < 0)
    {
        sign = -1;
        Speed = abs(Speed);
    }

    if (Speed >= MAXSPEEDTABLE)
        Speed = MAXSPEEDTABLE;

    if ( MVStruct.FowardTicksLeft == 0)
    {
        MVStruct.FowardState = false;
        MVStruct.MVBlocking = false;
        return;
    }           

    if ((Speed > 0) && (Speed < MAXSPEEDTABLE))
    {
        TheModel.ModelState.dx = -sign*(float)SpeedTable[Speed];
        TheModel.ModelState.dy = -sign*(float)SpeedTable[Speed];
        TheModel.ModelState.DAngle = 0;
    }
}

/************************************************************************
* Function unsigned long KRNLGetTicksL()
* Robot API
* PURPOSE
* To return the number of ticks since the systems started
*************************************************************************/
unsigned long SGenericModel::KRNLGetTicksL()
{
	
    LARGE_INTEGER CurrTime;

	QueryPerformanceCounter(&CurrTime);

	return (unsigned long)(((CurrTime.QuadPart - StartTime.QuadPart)*1000)/Frequency.QuadPart*SimMultiplier);
}


