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

#include "stdafx.h"
#include "Simulator.h"
#include "SSonarSensor.h"

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

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

SSonarSensor::SSonarSensor()
{
    SonarSensor.BeamObjectPrototype = NULL;
    SonarSensor.PreviousTransformedBeam = NULL;
    SonarSensor.CurrentTransformedBeam = NULL;

    SonarSensor.Valid = false;
    SonarSensor.Angle = 0;
  
    BeamSpeed = 34300; // in cm per second

    BeamAngleConst = 30; // 30 degrees

	// should be around 60 rpm, but could be faster for the simulation
    StepperRPM = 180; // 60 rpm

    // sonar command initilization
    SonarCommand.StreamAngle = 30;
    SonarCommand.CurrentAngle = 0;
    SonarCommand.DesiredAngle = 0;
    SonarCommand.RotateState = true;
	SonarCommand.GotoCommand = true;
//	 SonarCommand.SonarCommand = SonarStream;
    SonarCommand.SonarCommand = ScanFowards;
    SonarCommand.ClockWise = false;
    SonarCommand.FireTimeCounter = 0;

    BackBuffer = NULL;
    EnableExtraFlipping = true;

    ResetBeam();    

    Once = false;

    // some rendering options
    MaxIndexSC = 12;
    RenderPastBeams = true;
    RenderPastCollisions = true;

    MaximumDistance = 1000; // 10 meters


	LastDetectionDistance = SNOTREADY;

	TPScanAngle = 3; // 3 degrees scan increment
	TPScanArc = 50; // 30 degree scan arc
	ThreePointEnum = GotoRight;
	TPBeamAngleConst = 3; // only 5 degrees.

}

SSonarSensor::~SSonarSensor()
{
    ResetBeam();    
}

void SSonarSensor::LoadSonarBeam(CString FileName)
{
    unsigned char buffer[MAXFILESIZE]; // should allow for around 4000 objects to be saved
    unsigned char *bufferptr = buffer;

    ResetBeam();

    // O.K now open the file
    CFile NewFile;

    // just in case
    if (NewFile.Open(FileName,CFile::modeRead|CFile::typeBinary ,NULL) == 0)
    {
        AfxMessageBox("Unable to open default Sonar Beam model");
        NewFile.Close();
        return;
    }

    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
            BeamObjectPtr ptrObject = new BeamObject;
            #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 = Start.x;
                ptrObject->Start.y = Start.y;
                ptrObject->End.x = End.x;
                ptrObject->End.y = End.y;      
            #endif  
                 
            ptrObject->Next = SonarSensor.BeamObjectPrototype;

            SonarSensor.BeamObjectPrototype = ptrObject;

        }
        else if (objecttype == ARCLINE)
        {
            // do nothing, arcs is not support for a beam
        }
        else if (objecttype == BUMPSENSOR)
        {
            // do nothing, arcs is not support for a beam
        }
        else if (objecttype == IRSENSOR)
        {
            // do nothing, arcs is not support for a beam
        }
        else if (objecttype == SONARSENSOR)
        {            
            // do nothing, arcs is not support for a beam
        }

        index -= OBJECTSIZE;
    }

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

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

    // now adjust for the center.
    AdjustforCenterBeam();

    SonarSensor.Valid = true;

}

/************************************************************************
* Function LoadSonarSensor(CPoint Center)
*
* PURPOSE
* Loads the Display point of the Sonar from a file
* USAGE
* By genric model on a model load.
*************************************************************************/
void SSonarSensor::LoadSonarSensor(CPoint Center)
{
    SonarSensor.DisplayPoint = Center;
 
}

/************************************************************************
* Function AdjustforCenter(float Left, float Top)
*
* PURPOSE
* After a file has been opened, its center is found, and all other objects are offset based
* on the center.
* USAGE
* By the model open file
*************************************************************************/
void SSonarSensor::AdjustforCenter(float Left, float Top)
{
    SonarSensor.DisplayPoint.x -= Left;
    SonarSensor.DisplayPoint.y -= Top;
}

/************************************************************************
* 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 SSonarSensor::FileChartoLong(long &toRecieve, unsigned char *&ptr)
{
    
    unsigned char *ptr1 = &(unsigned char &)(toRecieve);
    for (int ix = 0; ix < 4; ix++)
    {
        *ptr1 = *ptr;
        ptr++;
        ptr1++;
    }

}

/************************************************************************
* Function ResetBeam()
*
* PURPOSE
* To Reset the Beam object
*************************************************************************/
void SSonarSensor::ResetBeam()
{
    BeamObjectPtr temp;

    while (SonarSensor.BeamObjectPrototype != NULL)
    {
        temp = SonarSensor.BeamObjectPrototype;
        SonarSensor.BeamObjectPrototype = SonarSensor.BeamObjectPrototype->Next;
        delete temp;
        temp = NULL;
    }

    while (BackBuffer != NULL)
    {
        SonarCollisionPtr temp1 = BackBuffer;
        while (temp1->Beam != NULL)
        {
            temp = temp1->Beam;
            temp1->Beam = temp1->Beam->Next;
            delete temp;
            temp = NULL;
        }
        BackBuffer = BackBuffer->Next;
        delete temp1;
        temp1 = NULL;
    }  

    ResetBeam1();
    
}

/************************************************************************
* Function ObjectPtr AdjustforCenterBeam()
*
* 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 SSonarSensor::AdjustforCenterBeam()
{

    BeamObjectPtr TraversePtr = SonarSensor.BeamObjectPrototype;

    #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;
    }    

    // scale it to intially be 5cm wide

    #ifdef USEFLOAT
        float Scale = INITIALBEAMWIDTH/(XR-XL);
    #else
        double Scale = INITIALBEAMWIDTH/(XR-XL);
    #endif  

    SonarSensor.PrototypeBeamCenter.x = ((XR-XL)*Scale)/(float)2.0;
    SonarSensor.PrototypeBeamCenter.y = (YB-YT)*Scale;
    

    // now readjust all of the objects so the center is the center.
    TraversePtr = SonarSensor.BeamObjectPrototype;
    while (TraversePtr != NULL)
    {
        TraversePtr->Start.x = (TraversePtr->Start.x-XL)*Scale;
        TraversePtr->End.x = (TraversePtr->End.x-XL)*Scale;
        TraversePtr->Start.y = (TraversePtr->Start.y-YT)*Scale;
        TraversePtr->End.y = (TraversePtr->End.y-YT)*Scale;

        TraversePtr = TraversePtr->Next;
    }

}

/************************************************************************
* Function UpdateModel(SSonarSensor::MODELINFO ModelInfo)
*
* PURPOSE
* The Sonar Sensor is a "child" of the model so it can't have a model pointer
* So the model has to update the model information every cycle.
* This function does the updating
* USAGE
* By the Model class
*************************************************************************/
void SSonarSensor::UpdateModel(SSonarSensor::MODELINFO ModelInfo)
{
    this->ModelInfo = ModelInfo;
}

/************************************************************************
* Function GDIRender(CDC &MemDC, CRect Quad, CRect Screen)
*
* PURPOSE
* Draw an IR sensor on the screen using windows GDI rendering
* INPUT
* MemDC, the GDI device context
* Quad, the area to rendering
* Screen, the screen size
* USAGE
* When Rendering
*************************************************************************/
void SSonarSensor::GDIRender(CDC &MemDC, FRect Quad, CRect Screen, float SRScale, FPoint RenderModelCenter)
{
    if (SonarSensor.Valid)
    {
        TranslateSensor(); // for the beams
        TranslateSensor(RenderModelCenter); // for the sensor

        float XScale = (float)Screen.Width()/(float)Quad.Width();
        float YScale = (float)Screen.Height()/(float)Quad.Height();
        float OldXScale = XScale;
        float OldYScale = YScale;

        double Angle = ModelInfo.CurrAngle + SonarSensor.Angle-90;

        float Scale =  SRScale;// to adjust the size
        XScale = XScale/Scale;
        YScale = YScale/Scale;
        float CircleRadius = (float).3; 
        float ConeHeightConst = 2;

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

        CPoint RealCenter;
                             // get the center point (screen coordinates) of which to render
        RealCenter.x = (long)((SonarSensor.TranslatedDisplayPointR.x-XTransform)*OldXScale);
        RealCenter.y = (long)((SonarSensor.TranslatedDisplayPointR.y-YTransform)*OldYScale);                          

        /////////////////////////////////////////////////////////////////////
        // Now Draw the Sonar Object                                       //
        /////////////////////////////////////////////////////////////////////


        CPoint Left,Right,Back;

        // draw the line
        Left.x = RealCenter.x - (long)(3*XScale);
        Left.y = RealCenter.y - (long)(CircleRadius*YScale);

        Right.x = RealCenter.x + (long)(3*XScale);
        Right.y = RealCenter.y - (long)(CircleRadius*YScale);

        Back.x = RealCenter.x;
        Back.y = RealCenter.y + (long)(CircleRadius*YScale);

        Left = LAUtils.RotatePoint(Left,RealCenter,Angle);
        Right = LAUtils.RotatePoint(Right,RealCenter,Angle); 
        Back = LAUtils.RotatePoint(Back,RealCenter,Angle);  

        MemDC.MoveTo(Right);
        MemDC.LineTo(Left); 
        MemDC.LineTo(Back);
        MemDC.LineTo(Right);
        MemDC.MoveTo(Left);

        CBrush BlackBrush;
        BlackBrush.CreateSolidBrush(RGB(0,0,0));

        CBrush *OldBrush = MemDC.SelectObject(&BlackBrush);

        // Draw the circle
        MemDC.Ellipse(RealCenter.x-(long)(CircleRadius*XScale),RealCenter.y-(long)(CircleRadius*YScale),RealCenter.x+(long)(CircleRadius*XScale),RealCenter.y+(long)(CircleRadius*YScale));

        MemDC.SelectObject(OldBrush);

        // Now draw the cone
        CRect Border;

        Border = LAUtils.FormRect(Right,Left);

     //   MemDC.Arc(Border,Right,Left);
       // MemDC.Ellipse(&Border);

        CPoint ArcCenter;
        ArcCenter = RealCenter;
        ArcCenter.y -= (long)(CircleRadius*XScale - 5.2*XScale);
        ArcCenter = LAUtils.RotatePoint(ArcCenter,RealCenter,Angle);  

        float StartAngle = (float)LAUtils.GetAngle(Left,ArcCenter);
        float EndAngle = (float)LAUtils.GetAngle(Right,ArcCenter);
        float MiddleAngle = (StartAngle+EndAngle)/2;
        StartAngle = StartAngle ;//- (StartAngle-MiddleAngle)/70;
        EndAngle = EndAngle;// - (EndAngle-MiddleAngle)/70;
        float DeltaAngle = EndAngle-StartAngle;

        if (DeltaAngle > 0)
        {
            StartAngle = StartAngle - 180;
        }

        if ((StartAngle >= - 180) && (StartAngle <= -120))
        {
            StartAngle = StartAngle - 180;
            DeltaAngle = -(360 - DeltaAngle);
        }


        AngleArc2(MemDC,ArcCenter.x,ArcCenter.y,long(6*XScale),StartAngle,DeltaAngle);

        /////////////////////////////////////////////////////////////////////
        // Now Draw the Beam                                               //
        /////////////////////////////////////////////////////////////////////

        CPen BluePen;
        BluePen.CreatePen(PS_SOLID,1,RGB(0,0,255));
        CPen *OldPen = MemDC.SelectObject(&BluePen);

        for (int ix = 0; ix < NBeams; ix++)
        {
            CPoint Start,End;
            Start.x = (long)((Beam[ix].Start.x-XTransform)*OldXScale);
            Start.y =(long)((Beam[ix].Start.y-YTransform)*OldYScale);
            End.x = (long)((Beam[ix].End.x-XTransform)*OldXScale);
            End.y = (long)((Beam[ix].End.y-YTransform)*OldYScale);
            LAUtils.ClipLine(Screen,Start,End);
			if (Start.x != -1 && End.x != -1)
            {
	            MemDC.MoveTo(Start);
				MemDC.LineTo(End);
			}
        }        

        MemDC.SelectObject(OldPen);

        /////////////////////////////////////////////////////////////////////
        // Now Draw the collision points/ Collsion beams                   //
        /////////////////////////////////////////////////////////////////////

        if (RenderPastCollisions)
        {
            CPen BluePen;
            BluePen.CreatePen(PS_SOLID,1,RGB(0,0,255));
            CPen *OldPen = MemDC.SelectObject(&BluePen);

            for (int ix = 0; ix < DisplayBufferIndex; ix++)
            {
                // double check
                if (DisplayBuffer[ix].Valid)
                {
                    CPoint DCP;
                    DCP.x = long((DisplayBuffer[ix].CollisionPoint.x-XTransform)*OldXScale);
                    DCP.y = long((DisplayBuffer[ix].CollisionPoint.y-YTransform)*OldYScale);

                    // render the old collision marker
                    if (DCP.x > Screen.left && DCP.x < Screen.right &&
                       DCP.y < Screen.bottom && DCP.y > Screen.top)
                    {
                        int R = 3;
                        MemDC.Ellipse(DCP.x-R,DCP.y-R,DCP.x+R,DCP.y+R);
                    }
              

                    if (RenderPastBeams)
                    {
                       
                        for (int iy = 0; iy < DisplayBuffer[ix].NBeams; iy++)
                        { 
                            CPoint Start,End;                            
                            Start.x = (long)((DisplayBuffer[ix].Beam[iy].Start.x-XTransform)*OldXScale);
                            Start.y =(long)((DisplayBuffer[ix].Beam[iy].Start.y-YTransform)*OldYScale);
                            End.x = (long)((DisplayBuffer[ix].Beam[iy].End.x-XTransform)*OldXScale);
                            End.y = (long)((DisplayBuffer[ix].Beam[iy].End.y-YTransform)*OldYScale);
                            LAUtils.ClipLine(Screen,Start,End);
                            // don't render if they are out of bounds
                            if (Start.x != -1 && End.x != -1)
                            {  
                                MemDC.MoveTo(Start);
                                MemDC.LineTo(End);;
                            }
                        }                            
                     

                    }
                }

            }
            MemDC.SelectObject(OldPen);
        

        } // end if

    }


}

/************************************************************************
* Function TranslateSensors()
*
* PURPOSE
* Since the sensors are defined in relative terms with the model sensor, their real
* position has to be computed based on the current position of the model
* USAGE
* When getting the real position of the Sonar sensor.
* OUTPUT
* Alters the real position info in the main struct
*******************************************************/
void SSonarSensor::TranslateSensor()
{
    FPoint Center;
    Center.x = ModelInfo.x;
    Center.y = ModelInfo.y;

    SonarSensor.TranslatedDisplayPoint.x = SonarSensor.DisplayPoint.x + ModelInfo.x - ModelInfo.Center.x;
    SonarSensor.TranslatedDisplayPoint.y = SonarSensor.DisplayPoint.y + ModelInfo.y - ModelInfo.Center.y;

    SonarSensor.TranslatedDisplayPoint = 
        LAUtils.RotatePoint(SonarSensor.TranslatedDisplayPoint,Center,ModelInfo.CurrAngle);

}

/************************************************************************
* Function TranslateSensors()
*
* PURPOSE
* Since the sensors are defined in relative terms with the model sensor, their real
* position has to be computed based on the current position of the model
* USAGE
* When getting the real position of the Sonar sensor.
* OUTPUT
* Alters the real position info in the main struct
* NOTE 
* Same as TranslateSensor() Excepts uses the rendering center, and point
*******************************************************/
void SSonarSensor::TranslateSensor(FPoint RenderModelCenter)
{

    SonarSensor.TranslatedDisplayPointR.x = SonarSensor.DisplayPoint.x + RenderModelCenter.x - ModelInfo.Center.x;
    SonarSensor.TranslatedDisplayPointR.y = SonarSensor.DisplayPoint.y + RenderModelCenter.y - ModelInfo.Center.y;

    SonarSensor.TranslatedDisplayPointR = 
        LAUtils.RotatePoint(SonarSensor.TranslatedDisplayPointR,RenderModelCenter,ModelInfo.CurrAngle);

}

/************************************************************************
* Function AngleArc2(CDC &MemDC, int X, int Y, DWORD dwRadius,
                  float fStartDegrees, float fSweepDegrees)
*
* PURPOSE
* Makes up for the fact that the GDI on 95 based systems don't have this function
* USAGE
* Draws an arc
*******************************************************/
BOOL SSonarSensor::AngleArc2(CDC &MemDC, int X, int Y, DWORD dwRadius,
                  float fStartDegrees, float fSweepDegrees)
   {
     int iXStart, iYStart;  // End point of starting radial line.
     int iXEnd, iYEnd;       // End point of ending radial line.
     float fStartRadians;   // Start angle in radians.
     float fEndRadians;     // End angle in radians.
     BOOL bResult;            // Function result.
     float fTwoPi = 2.0f * 3.141592f;
   
     /* Get the starting and ending angle in radians. */
     if (fSweepDegrees > 0.0f) {
       fStartRadians = ((fStartDegrees / 360.0f) * fTwoPi);
       fEndRadians = (((fStartDegrees + fSweepDegrees) / 360.0f) * fTwoPi);
     }   else {
       fStartRadians = (((fStartDegrees + fSweepDegrees)  / 360.0f) *
                          fTwoPi);
       fEndRadians =  ((fStartDegrees / 360.0f) * fTwoPi);
     }
   
     /* Calculate a point on the starting radial line via */
     /* polar -> cartesian conversion. */
     iXStart = X + (int)((float)dwRadius * (float)cos(fStartRadians));
     iYStart = Y - (int)((float)dwRadius * (float)sin(fStartRadians));
   
     /* Calculate a point on the ending radial line via */
     /* polar -> cartesian conversion. */
     iXEnd = X + (int)((float)dwRadius * (float)cos(fEndRadians));
     iYEnd = Y - (int)((float)dwRadius * (float)sin(fEndRadians));
   
     /* Draw a line to the starting point. */
     if (fSweepDegrees > 0.0f)
      MemDC.LineTo(iXStart, iYStart);
     else
      MemDC.LineTo(iXEnd, iYEnd);

     /* Draw the arc. */
     bResult = MemDC.Arc(X - dwRadius, Y - dwRadius,
                        X + dwRadius, Y + dwRadius,
                        iXStart, iYStart,
                        iXEnd, iYEnd);
   
     /* Move to the ending point. Arc() will not do this and ArcTo() */
     /* will not work on Win32s or Win16. */
     if (fSweepDegrees < 0.0f)
      MemDC.MoveTo(iXStart, iYStart);
     else
      MemDC.MoveTo(iXEnd, iYEnd);
   
     return bResult;
}
   
/************************************************************************
* Function BeamObjectPtr StretchBeam(float TimeElapse, BOOL Collision))
*
* PURPOSE
* The sonar beam expands across its width while it travels.  This function
* returns a list to an stretched beam depending on the time elapse and the
* stretching constant.
* The Beam also travels along the proper x+y displacement and is rotated into its
* proper orientations
* USAGE
* In the routine to check for sonar collisions
* INPUTS
* TimeElapse
* Collision, if this is true then use the previous as current
* ALGORITHM
* 1) Perform a traversal of the old list.
*    a) If it is the first time use the beam prototype
*    b) Otherwise use the previous stretched information
* 2) Stretch out the beam, (use the untranslated beam for this)
* 3) Rotate and then translate is, base the translation on matching up the 
*    Beams centers
* 
* NOTE
* This function has to be called twice before the first collision detection,
* to correctly form both the current and previous list.
* OUTPUT
* Change the internal beam information
* RETURNS
* False if the beam didn't exceed the maximum distance, true otherwise
*******************************************************/
BOOL SSonarSensor::StretchBeam(float &TimeElapse, BOOL Collision)
{
    BOOL tReturn = false;

    BeamObjectPtr Traverse,tPtrT;

    float Angle = SonarSensor.BeamAngle; // rotation angle
    float Angle1 = SonarSensor.BeamAngle + 90; // rotation angle

    FPoint Translation;

    float Distance = BeamSpeed*TimeElapse;

    if (!Collision)
    {
        SonarSensor.CurrentBeamDistance += Distance;
        if (SonarSensor.CurrentBeamDistance >= MaximumDistance)
        {
            // figure out the maximum it could go.   (Previous Distance)
            Distance = MaximumDistance-(SonarSensor.CurrentBeamDistance-Distance);
            // return how much time is left
            TimeElapse = TimeElapse-Distance/BeamSpeed;   

            // return true
            tReturn = true;
        }
    }
   
    if (SonarSensor.CurrentTransformedBeam == NULL)
    {
        // first time using protoype
        Traverse = SonarSensor.BeamObjectPrototype;

        // reset the old 
        OldScale1 = 1;
        OldScale = 1;
       
       #ifdef USEFLOAT
           Translation.x = SonarSensor.CurrBeamOrigin.x + Distance*(float)cos(Angle1*PI/180);
           Translation.y = SonarSensor.CurrBeamOrigin.y - Distance*(float)sin(Angle1*PI/180);
        #else
           Translation.x = SonarSensor.CurrBeamOrigin.x + Distance*cos(Angle1*PI/180);
           Translation.y = SonarSensor.CurrBeamOrigin.y - Distance*sin(Angle1*PI/180);
        #endif    
           
        // copy over the prototype as the previous tranlated beam

          float Scale = 1;

        while (Traverse != NULL)
        {

            tPtrT = new BeamObject;

            FPoint TStart;
            FPoint TEnd;

            // Have to recenter the beam
            TStart.x = Traverse->Start.x*Scale - SonarSensor.PrototypeBeamCenter.x*Scale;
            TStart.y = Traverse->Start.y*Scale  - SonarSensor.PrototypeBeamCenter.y*Scale;
            TEnd.x = Traverse->End.x*Scale  - SonarSensor.PrototypeBeamCenter.x*Scale;
            TEnd.y = Traverse->End.y*Scale  - SonarSensor.PrototypeBeamCenter.y*Scale;

            // rotate about the 0,0 point
            FPoint Rotate;
            Rotate.x = 0;
            Rotate.y = 0;

            // rotate it
            tPtrT->Start = LAUtils.RotatePoint(TStart,Rotate,Angle);
            tPtrT->End = LAUtils.RotatePoint(TEnd,Rotate,Angle);       

            // translate it
            tPtrT->Start.x += SonarSensor.CurrBeamOrigin.x ;
            tPtrT->Start.y += SonarSensor.CurrBeamOrigin.y;
            tPtrT->End.x += SonarSensor.CurrBeamOrigin.x;
            tPtrT->End.y += SonarSensor.CurrBeamOrigin.y;

            // add it to the list.
            tPtrT->Next = SonarSensor.PreviousTransformedBeam;
            SonarSensor.PreviousTransformedBeam = tPtrT;

            Traverse = Traverse->Next;
        }

        // need to do this twice
        Traverse = SonarSensor.BeamObjectPrototype;

    }
    else
    {
        /////////////////////////////////////////////////////////////////////
        // Delete Previous position information                            //
        // Copy Current Variables into Previous position                   //
        /////////////////////////////////////////////////////////////////////
        // Use previous beams dimensions, first copy over the old current into the previous
        if (!Collision)
        {
            SonarSensor.PreviousBeamTransformedCenter = SonarSensor.CurrentBeamTransformedCenter;

            // delete/copy the old linked list, note all lists should be the same length
            while (SonarSensor.PreviousTransformedBeam != NULL)
            {
                Traverse = SonarSensor.PreviousTransformedBeam;
                SonarSensor.PreviousTransformedBeam = SonarSensor.PreviousTransformedBeam->Next;
                delete Traverse;
                Traverse = NULL;
            }
     
            SonarSensor.PreviousTransformedBeam = SonarSensor.CurrentTransformedBeam;

            SonarSensor.CurrentTransformedBeam = NULL;       
        }
        else
        {
            // delete the current and retain the previous
            while (SonarSensor.CurrentTransformedBeam != NULL)
            {
                Traverse = SonarSensor.CurrentTransformedBeam;
                SonarSensor.CurrentTransformedBeam = SonarSensor.CurrentTransformedBeam->Next;
                delete Traverse;
                Traverse = NULL;
            }          
  
            SonarSensor.CurrentTransformedBeam = NULL;  

        }

        /////////////////////////////////////////////////////////////////////
        // Figure out the new center                                       //
        /////////////////////////////////////////////////////////////////////

      
        Traverse = SonarSensor.BeamObjectPrototype;
        
       // SonarSensor.CurrentBeamCenter.x = SonarSensor.PreviousBeamCenter.x*(1+StretchX);
       // SonarSensor.CurrentBeamCenter.y = SonarSensor.PreviousBeamCenter.y*(1+StretchY);

        #ifdef USEFLOAT
           Translation.x = SonarSensor.PreviousBeamTransformedCenter.x + Distance*(float)cos(Angle1*PI/180);
           Translation.y = SonarSensor.PreviousBeamTransformedCenter.y - Distance*(float)sin(Angle1*PI/180);
        #else
           Translation.x = SonarSensor.PreviousBeamTransformedCenter.x + Distance*cos(Angle1*PI/180);
           Translation.y = SonarSensor.PreviousBeamTransformedCenter.y - Distance*sin(Angle1*PI/180);
        #endif  

    }
    SonarSensor.CurrentBeamTransformedCenter.x = Translation.x;
    SonarSensor.CurrentBeamTransformedCenter.y = Translation.y;

    /////////////////////////////////////////////////////////////////////
    // Stretching / Transformation Part                                //
    /////////////////////////////////////////////////////////////////////   
    
    // first figure out how much to stretch by using the beam angle
    float CosDistance = LAUtils.GetDistance(Translation,SonarSensor.CurrBeamOrigin);

    // the total xDistance has to be twice the SinDistance
    float SinDistance = 2*CosDistance*(float)tan(BeamAngleConst*PI/180);

    // figure out how much to scale the beam width by
    float Scale = SinDistance/INITIALBEAMWIDTH;    

    // if it is a collision use the scale of the previous (2 scales back) 
    // current is the one scale back.  Do this because although the distance to
    // the collision point is known using the new scale will skew the results
    if (Collision)
        Scale = OldScale1;
    else
    {
        OldScale1 = OldScale;
        OldScale = Scale;
        
    }
  

    while (Traverse != NULL)
    {

        tPtrT = new BeamObject;

        FPoint TStart;
        FPoint TEnd;

        // Have to recenter the beam
        TStart.x = Traverse->Start.x*Scale - SonarSensor.PrototypeBeamCenter.x*Scale;
        TStart.y = Traverse->Start.y*Scale  - SonarSensor.PrototypeBeamCenter.y*Scale;
        TEnd.x = Traverse->End.x*Scale  - SonarSensor.PrototypeBeamCenter.x*Scale;
        TEnd.y = Traverse->End.y*Scale  - SonarSensor.PrototypeBeamCenter.y*Scale;


        // rotate about the 0,0 point
        FPoint Rotate;
        Rotate.x = 0;
        Rotate.y = 0;

        // rotate it
        tPtrT->Start = LAUtils.RotatePoint(TStart,Rotate,Angle);
        tPtrT->End = LAUtils.RotatePoint(TEnd,Rotate,Angle);       

        // translate it
        tPtrT->Start.x += Translation.x ;
        tPtrT->Start.y += Translation.y;
        tPtrT->End.x += Translation.x;
        tPtrT->End.y += Translation.y;

        // add it to the list.
        tPtrT->Next = SonarSensor.CurrentTransformedBeam;
        SonarSensor.CurrentTransformedBeam = tPtrT;

        Traverse = Traverse->Next;
    }

    // For rendering (use an array not linked list
    FlipDisplayBuffer1();

    return tReturn;
}

/************************************************************************
* Function SimulateSonar(float TimeElapse)
*
* PURPOSE
* This is the function that the is run with the simulater thread to update the sonar
*******************************************************/
void SSonarSensor::SimulateSonar(float TimeElapse)
{
    if (SonarSensor.Valid)
    {
        int Loops = 0;
        float TimeElapseSplit = TimeElapse/SIMULATORSAMPLING;
        // should be a while, but there are lock

        if (TimeElapse > 0)
        {
            for (int ix = 0; ix < SIMULATORSAMPLING; ix++)
            {
                TimeElapse = TimeElapseSplit;

				// check with sonar command first before doing anything
				if (SonarCommand.GotoCommand)
				{
					RunSonarCommand(TimeElapse);					
					//return;
				}
           
                while (TimeElapse > 0)
                {            
                    // rotate the sonar
                    if (SonarCommand.RotateState)
                    {
           
                            TimeElapse = RotateSonar(TimeElapse);

                            if (TimeElapse > 0)
                            {
								// CanFireSonar, lock the sonar in rotating state, to prevent
								// it from pinging more then 100 times a second
                               if (CanFireSonar(TimeElapse))
                               {
								   RunSonarCommand(TimeElapse);                                
                               }

                            }

                    }          
                    if (TimeElapse > 0)
                    {   

                        // true if distance exceeded
                        if (!StretchBeam(TimeElapse, false))
                        {   
                            if (CheckforCollision(TimeElapse))
                            {                     
                                // Something collided, time to rotate the sonar

								RunSonarCommand(TimeElapse);    
                               // RotateSonar();  
                            }
                            else
                            {
                                // no collision just do a safety check if in bounds.
                                TimeElapse  = -1;  

                                // do a check to make sure it is all in bounds
                                BeamObjectPtr Traverse = SonarSensor.CurrentTransformedBeam;
                                while (Traverse != NULL)
                                {
                                    if (!(MapPtr->AddCheckifPointValid((CCPoint)Traverse->Start)
                                        && MapPtr->AddCheckifPointValid((CCPoint)Traverse->End)))
                                    {
                                        // get out up loop, not quite accurate but this case should
                                        // never really happen
                                        TimeElapse  = -1;
										RunSonarCommand(TimeElapse);    
                                       // RotateSonar();
                                        Traverse = NULL;
                                    }
                                    if (Traverse != NULL)
                                    {
                                        Traverse = Traverse->Next;
                                    }
                                }    
                            }      

                        }
                        else
                        {
							// maximum distance exceeded check for a collision                            
							if (!CheckforCollision(TimeElapse))
								RemoveDeadWoodCL(); // run this because it won't be ran otherwise
							LastDetectionDistance = SOUTOFRANGE; // last distance is no detection
							// but the sonar will need to be rotated in any case
							RunSonarCommand(TimeElapse);    
							//RotateSonar();  
                        }  
                    }
                    


                    // this isn't needed anymore but just in case I'll leave it in
                    // to prevent lockups
                    Loops++;
                    if (Loops > 50)
                    {
                        char tempo[30];
                        sprintf(tempo,"Lockup: %8.6f",TimeElapse);
                        AfxMessageBox(tempo);
                    }

                } // end while (might be an if
            }// end extra sampling if
        }// end main elapse time > 0 check
    } // end if valid

}


/************************************************************************
* Function ResetBeam()
*
* PURPOSE
* To Reset the Beam movement objects
*************************************************************************/
void SSonarSensor::ResetBeam1()
{
    BeamObjectPtr temp;
  
    while (SonarSensor.CurrentTransformedBeam != NULL)
    {
        temp = SonarSensor.CurrentTransformedBeam;
        SonarSensor.CurrentTransformedBeam = SonarSensor.CurrentTransformedBeam->Next;
        delete temp;
        temp = NULL;
    }
    while (SonarSensor.PreviousTransformedBeam != NULL)
    {
        temp = SonarSensor.PreviousTransformedBeam;
        SonarSensor.PreviousTransformedBeam = SonarSensor.PreviousTransformedBeam->Next;
        delete temp;
        temp = NULL;
    }

}

/************************************************************************
* Function BOOL CheckforCollision()
*
* PURPOSE
* To check if if the Sonar Beam collided with an object
* USAGE
* When the SimulateSonar Routine is run
* OUTPUT
* True or False
*************************************************************************/
BOOL SSonarSensor::CheckforCollision(float &TimeElapse)
{
    BeamObjectPtr PrevTraverse, CurrTraverse;

    PrevTraverse = SonarSensor.PreviousTransformedBeam;
    CurrTraverse = SonarSensor.CurrentTransformedBeam;

    float Distance;
    float NewTimeElapse = 0;
    float CollisionTime;
    FPoint CollisionPoint;
    float NoCollision = 999999;
    float MinCollisionDistance = NoCollision;


    while (PrevTraverse != NULL)
    {       

        FPoint Collision = MapPtr->DetectCollision(PrevTraverse->Start,PrevTraverse->End,CurrTraverse->Start,CurrTraverse->End);       
        if (!(Collision.x == NOCOLLISION))
        {
            // find out remaining time, and beam collision point

            // get the line with the slope = to the beam angle
            ULinearAlgebra::Matrix2X2 SlopeLine = LAUtils.PointstoLine(Collision,SonarCommand.CurrentAngle+ModelInfo.CurrAngle);

            ULinearAlgebra::Matrix2X2 PrevLine = LAUtils.PointstoLine(PrevTraverse->Start,PrevTraverse->End);
            
            FPoint Intersection = LAUtils.FindIntersectionF(SlopeLine,PrevLine);

            Distance = LAUtils.GetDistance(Intersection,Collision);

            // we want shortest distance to origin.
            float CheckDistance = LAUtils.GetDistance(Collision,SonarSensor.CurrBeamOrigin);                

            if (Distance < MinCollisionDistance)
            {

                // Ideally this should always be true, but for somereason sometimes it isn't
                if (Distance <= BeamSpeed*TimeElapse)
                {
                    // not all of the time is used up.  Figure out how much time is left                    
                    NewTimeElapse = TimeElapse - Distance/BeamSpeed;
                }
                else
                {
                    // so do this as a safety check
                    NewTimeElapse = 0;
                }

                CollisionTime = Distance/BeamSpeed;

                MinCollisionDistance = Distance;

                CollisionPoint = Collision;

            }               

        }

        PrevTraverse = PrevTraverse->Next;
        CurrTraverse = CurrTraverse->Next;
    }

    if (MinCollisionDistance != NoCollision)
    {
          AddToCollisionList(CollisionTime,CollisionPoint);
          TimeElapse = NewTimeElapse;
          return true;
    }

    return false;

}

/************************************************************************
* Function float RotateSonar(float TimeElapse)
*
* PURPOSE
* To rotate the sonar to a desired angle (sonar stream)
* USAGE
* When the SimulateSonar Routine is run
* NOTE
* Not fully tested, for all cases, make sure you change clockwise and counterclockwise to 
* be the shortest route, otherwise it may not work.
* OUTPUT
* -1 if sonar has not reached its desired position
* else it return the time remaining in case there is time to shoot out the sonar beam
*************************************************************************/
float SSonarSensor::RotateSonar(float TimeElapse)
{

    // nothing to do
    if (SonarCommand.CurrentAngle == SonarCommand.DesiredAngle)        
        return TimeElapse;
    else
    {
        if (!SonarCommand.ClockWise)
        {
            // see how much angle is possible to move in this time duration
            float NewAngle = SonarMod(SonarCommand.CurrentAngle + StepperRPM*TimeElapse*6);   

            // need a proper delta, this if statement takes care of 360 overflow problem
            float TempCurr;
            if (SonarCommand.DesiredAngle < SonarCommand.CurrentAngle)
            {
                TempCurr = 360+SonarCommand.DesiredAngle;
            }
            else
            {
                TempCurr = SonarCommand.DesiredAngle;
            }

            // see how much time is left to move to the new location
            float tReturn = TimeElapse-(TempCurr
                              - SonarCommand.CurrentAngle)/(StepperRPM*6);
            
            SonarCommand.CurrentAngle = NewAngle;
            SonarSensor.Angle = SonarCommand.CurrentAngle;
            // if Time is positive that means that it is possible to complete the move,
            // return the time left over.
            if (tReturn > 0)
            {
                SonarCommand.CurrentAngle = SonarCommand.DesiredAngle;
                return tReturn;
            }
            else
            {
                return -1;
            }
        }
        else
        {

            // see how much angle is possible to move in this time duration
            float NewAngle = SonarMod(SonarCommand.CurrentAngle - StepperRPM*TimeElapse*6);   

            // need a proper delta, this if statement takes care of 360 overflow problem
            float TempCurr;
            if (SonarCommand.DesiredAngle > SonarCommand.CurrentAngle)
            {
                TempCurr = 360+SonarCommand.CurrentAngle;
            }
            else
            {
                TempCurr = SonarCommand.CurrentAngle;
            }

             // see how much time is left to move to the new location
            float tReturn = TimeElapse-(TempCurr - SonarCommand.DesiredAngle)/(StepperRPM*6);
            
            SonarCommand.CurrentAngle = NewAngle;
            SonarSensor.Angle = SonarCommand.CurrentAngle;
            // if Time is positive that means that it is possible to complete the move,
            // return the time left over.
            if (tReturn > 0)
            {
                SonarCommand.CurrentAngle = SonarCommand.DesiredAngle;
                return tReturn;
            }
            else
            {
                return -1;
            }
        }
    } 
    return -1;


}

/************************************************************************
* Function float ModSonarAngle(float Angle)
*
* PURPOSE
* Performs a Mod 360 on the sonar
* USAGE
* For the RotateSonar function
*************************************************************************/
float SSonarSensor::SonarMod(float Angle)
{
    float tReturn = Angle;

    while (tReturn > 360)
        tReturn = tReturn - 360;
    while (tReturn < 0)
        tReturn = 360 + tReturn;

    return tReturn;
}

/************************************************************************
* Function void RotateSonarStream()
*
* PURPOSE
* Changes the desired Angle of the sonar (sonar stream)
* USAGE
* Internal, when a rotation is designer
*************************************************************************/
void SSonarSensor::RotateSonarStream()
{
    if (SonarCommand.ClockWise)
    {
        SonarCommand.DesiredAngle = SonarMod(SonarCommand.CurrentAngle - SonarCommand.StreamAngle);            
    }
    else
    {
        SonarCommand.DesiredAngle = SonarMod(SonarCommand.CurrentAngle + SonarCommand.StreamAngle);            
    }
    SonarCommand.RotateState = true;   

}

/************************************************************************
* Function void AddToCollisionList(float TimeElapse, BeamObjectPtr Beam, FPoint CollisionPoint)
*
* PURPOSE
* This is the functions used to add a collision to the list of previous collisions
* USAGE
* Internal when a collision occured
*************************************************************************/
void SSonarSensor::AddToCollisionList(float TimeElapse, FPoint CollisionPoint)
{

    SonarCollisionPtr Traverse;
    SonarCollisionPtr DeletePtr;
    BeamObjectPtr DeletePtr1;
    SonarCollisionPtr Previous = NULL; 
   

     /////////////////////////////////////////////////////////////////////
    // first get rid of the deadwood                                   //
    /////////////////////////////////////////////////////////////////////
     Traverse = BackBuffer;    
    while (Traverse != NULL)
    {
        BOOL DoNotGotoNext = false;
        Traverse->StorageNumber--;
        
        if (Traverse->StorageNumber <= 0)
        {
            // time to go
            DeletePtr  = Traverse;
            Traverse = Traverse->Next;
            
            // nuke the beam linked list
            while (DeletePtr->Beam != NULL)
            {
                DeletePtr1 = DeletePtr->Beam;
                DeletePtr->Beam = DeletePtr->Beam->Next;
                delete DeletePtr1;
                DeletePtr1 = NULL;
            }

            delete DeletePtr;
            DeletePtr = NULL;
            // remove from linked list
            if (Previous == NULL)
            {
                BackBuffer = Traverse;
                // because the list where flipped an
                // extra case happens.
                DoNotGotoNext = true;

            }
            else
            {
                Previous->Next = Traverse;
                DoNotGotoNext = true;
            }
        }
        // could be null if last one in list deleted
        if (Traverse != NULL && !DoNotGotoNext)
        {
            Previous = Traverse;
            Traverse = Traverse->Next;
        }
    }

    /////////////////////////////////////////////////////////////////////
    // Now add the new collision                                       //
    /////////////////////////////////////////////////////////////////////
    SonarCollisionPtr temp = new SonarCollisionStruct;

    temp->CollisionPoint = CollisionPoint;
    temp->StorageNumber = MaxIndexSC;
    temp->Beam = NULL;
	// figure out the last detection distance
	LastDetectionDistance = LAUtils.GetDistance(SonarSensor.CurrBeamOrigin,CollisionPoint);

    // Figure out the collision point beam
    StretchBeam(TimeElapse,true);

    // Now copy the current beam into the linked list
    BeamObjectPtr Traverse1 = SonarSensor.CurrentTransformedBeam;
    while (Traverse1 != NULL)
    {
        BeamObjectPtr NewPtr = new BeamObject;
        NewPtr->Start = Traverse1->Start;
        NewPtr->End = Traverse1->End;
        NewPtr->Next = temp->Beam;
        temp->Beam = NewPtr;
        Traverse1 = Traverse1->Next;
    }
  

    // add to the list.
    temp->Next = BackBuffer;
    BackBuffer = temp;

    // update the display buffer
    FlipDisplayBuffer();
   

}

/************************************************************************
* Function SetSpeedofSound(float Speed)
*
* PURPOSE
* Sets the internal variable speed of sound
*************************************************************************/
void SSonarSensor::SetSpeedofSound(float Speed)
{
    BeamSpeed = Speed*100;

}

/************************************************************************
* Function SetMaximumDistance(float MaximumDistance)

*
* PURPOSE
* Sets the internal variable maximum distance 
*************************************************************************/
void SSonarSensor::SetMaximumDistance(float MaximumDistance)
{
    this->MaximumDistance = MaximumDistance;

}

/************************************************************************
* Function SetStreamAngle(float Angle)
*
* PURPOSE
* Sets the internal variable stream angle
*************************************************************************/
void SSonarSensor::SetStreamAngle(float Angle)
{
    SonarCommand.StreamAngle = (int)Angle;

}

/************************************************************************
* Function SetBeamAngle(float Angle)
*
* PURPOSE
* Sets the internal variable beam angle
*************************************************************************/
void SSonarSensor::SetBeamAngle(float Angle)
{
    BeamAngleConst = Angle/2;
}

/************************************************************************
* Function SetBeamAngle(float Angle)
*
* PURPOSE
* Sets the internal variables
*************************************************************************/
void SSonarSensor::SetRenderOptions(BOOL PastCollision, BOOL PastBeams, int NPastBeams, BOOL ExtraFlipping)
{
    MaxIndexSC = NPastBeams;
    RenderPastBeams = PastBeams;
    RenderPastCollisions = PastCollision;
    EnableExtraFlipping = ExtraFlipping;


}

/************************************************************************
* Function BOOL CanFireSonar(float &TimeElapse)
*
* PURPOSE
* To restrict the firing rate of the sonar to a predifined rate
* USAGE
* During the sonar routine.  The problem is if the model faces the wall, the distance
* is very small, then the firing rate could be very high and bog down the computer
*************************************************************************/
BOOL SSonarSensor::CanFireSonar(float &TimeElapse)
{

    if (TimeElapse > SonarCommand.FireTimeCounter)
    {
        SonarCommand.FireTimeCounter = (float)1/MAXFIRINGRATE; // reset variable
        return true;
    }
    else
    {
        SonarCommand.FireTimeCounter -= TimeElapse;
        TimeElapse = 0;
        return false;
    }

}

/************************************************************************
* Function FlipDisplayBuffer()
*
* PURPOSE
* To update the display buffer with the newest contants of the sonar
* USAGE
* Right before a render
* NOTE
* The new code is syncronous to the sonar thread so there are no worrys about the 
* BackBuffer changing
*************************************************************************/
void SSonarSensor::FlipDisplayBuffer()
{

    int index = 0;    
    SonarCollisionPtr Traverse = BackBuffer; 
    BeamObjectPtr Traverse1;

    while (Traverse != NULL && index < MAXDISPLAYBUFFER)
    { 
        DisplayBuffer[index].Angle = Traverse->Angle;
        DisplayBuffer[index].CollisionPoint = Traverse->CollisionPoint;

        int index1 = 0;
        Traverse1 = Traverse->Beam;
        while (Traverse1 != NULL && index1 < MAXBEAM)
        {
            DisplayBuffer[index].Beam[index1].Start = Traverse1->Start;
            DisplayBuffer[index].Beam[index1].End = Traverse1->End;
            index1++;
            Traverse1 = Traverse1->Next;
        }

         DisplayBuffer[index].NBeams = index1;  
         Traverse = Traverse->Next;
         DisplayBuffer[index].Valid = true;
         index++;
    }

    DisplayBufferIndex = index;




    // obsolete linked list code
/*    BeamObjectPtr DeletePtr1;
    SonarCollisionPtr DeletePtr;
    SonarCollisionPtr Previous = NULL;
    SonarCollisionPtr Traverse = DisplayBuffer;  

    // don't change the display buffer if it is being used
    // If it is being used it won't change until the next update.  
    // Rather do this then use a semephore or some triple buffer scheme.
    
     /////////////////////////////////////////////////////////////////////
    // first copy over stuff to the back buffer                        //
    /////////////////////////////////////////////////////////////////////
    // nuke the previous contents of the backbuffer
    while (Traverse != NULL)
    {
        DeletePtr  = Traverse;
        Traverse = Traverse->Next;
    
        // nuke the beam linked list
        while (DeletePtr->Beam != NULL)
        {
            DeletePtr1 = DeletePtr->Beam;
            DeletePtr->Beam = DeletePtr->Beam->Next;
            delete DeletePtr1;
            DeletePtr1 = NULL;
        }

        delete DeletePtr;
        DeletePtr = NULL;

   
    }
    // copy over stuff to the BackBuffer from the DisplayBuffer
    SonarCollisionPtr TraverseBackBuffer;
    Traverse = BackBuffer;
    DisplayBuffer = NULL;
    while (Traverse != NULL)
    {
            TraverseBackBuffer = new  SonarCollisionStruct;
            TraverseBackBuffer->CollisionPoint = Traverse->CollisionPoint;
            TraverseBackBuffer->StorageNumber = Traverse->StorageNumber;
            TraverseBackBuffer->Beam = NULL;

            // now copy over the beam information
            BeamObjectPtr TraverseBeam = Traverse->Beam;
            while (TraverseBeam != NULL)
            {
                BeamObjectPtr tempo = new BeamObject;

                tempo->Start = TraverseBeam->Start;
                tempo->End = TraverseBeam->End;
                tempo->Next = TraverseBackBuffer->Beam;
                TraverseBackBuffer->Beam = tempo;

                TraverseBeam = TraverseBeam->Next;
            }

            TraverseBackBuffer->Next = DisplayBuffer;
            DisplayBuffer = TraverseBackBuffer;

            Traverse = Traverse->Next;
    }*/
}

/************************************************************************
* Function RemoveDeadWoodCL()
*
* PURPOSE
* To update the display previous beam list by one time quata (removes items)
* USAGE
* Whenever one time quanta happens
* NOTE
* This used to be a member of add to collision list but was made independent so
* Items could be removed even if items can't be added
* BackBuffer changing
*************************************************************************/
void SSonarSensor::RemoveDeadWoodCL()
{
    SonarCollisionPtr Traverse;
    SonarCollisionPtr DeletePtr;
    BeamObjectPtr DeletePtr1;
    SonarCollisionPtr Previous = NULL;    

    Traverse = BackBuffer;    
    while (Traverse != NULL)
    {
        BOOL DoNotGotoNext = false;
        Traverse->StorageNumber--;
        
        if (Traverse->StorageNumber <= 0)
        {
            // time to go
            DeletePtr  = Traverse;
            Traverse = Traverse->Next;
            
            // nuke the beam linked list
            while (DeletePtr->Beam != NULL)
            {
                DeletePtr1 = DeletePtr->Beam;
                DeletePtr->Beam = DeletePtr->Beam->Next;
                delete DeletePtr1;
                DeletePtr1 = NULL;
            }

            delete DeletePtr;
            DeletePtr = NULL;
            // remove from linked list
            if (Previous == NULL)
            {
                BackBuffer = Traverse;
                // because the list where flipped an
                // extra case happens.
                DoNotGotoNext = true;

            }
            else
            {
                Previous->Next = Traverse;
                DoNotGotoNext = true;
            }
        }
        // could be null if last one in list deleted
        if (Traverse != NULL && !DoNotGotoNext)
        {
            Previous = Traverse;
            Traverse = Traverse->Next;
        }
    }

    FlipDisplayBuffer();

}


/************************************************************************
* Function FlipDisplayBuffer1()
*
* PURPOSE
* Same as FlipDisplayBuffer() except this one flips the current beam
* USAGE
* Whenver the stretch beam function is called (syncronous)
*************************************************************************/
void SSonarSensor::FlipDisplayBuffer1()
{
    int index = 0;

    BeamObjectPtr Traverse = SonarSensor.CurrentTransformedBeam;

    while (Traverse != NULL)
    {
        Beam[index].Start = Traverse->Start;
        Beam[index].End = Traverse->End;
        Traverse = Traverse->Next;
        index++;
    }

    NBeams = index;
    


}

void SSonarSensor::DirectXRender(VERTEX* &pVertex, FPoint RenderModelCenter, int &VertexCount)
{
    if (SonarSensor.Valid)
    {
        TranslateSensor(); // for the beams

         /////////////////////////////////////////////////////////////////////
        // Now Draw the Beam                                               //
        /////////////////////////////////////////////////////////////////////


        for (int ix = 0; ix < NBeams; ix++)
        {
            DirectXFormBeam(pVertex,Beam[ix].Start,Beam[ix].End);
            VertexCount += 2;
            pVertex += 6;
        }

        /////////////////////////////////////////////////////////////////////
        // Now Draw the collision points/ Collsion beams                   //
        /////////////////////////////////////////////////////////////////////

        if (RenderPastCollisions)
        {

            for (int ix = 0; ix < DisplayBufferIndex; ix++)
            {
                // double check
                if (DisplayBuffer[ix].Valid)
                {
                  /*  CPoint DCP;
                    DCP.x = long((DisplayBuffer[ix].CollisionPoint.x-XTransform)*OldXScale);
                    DCP.y = long((DisplayBuffer[ix].CollisionPoint.y-YTransform)*OldYScale);

                    // render the old collision marker
                    int R = 3;
                    MemDC.Ellipse(DCP.x-R,DCP.y-R,DCP.x+R,DCP.y+R);*/
              

                    if (RenderPastBeams)
                    {
                       
                        for (int iy = 0; iy < DisplayBuffer[ix].NBeams; iy++)
                        { 
                            DirectXFormBeam(pVertex,DisplayBuffer[ix].Beam[iy].Start,DisplayBuffer[ix].Beam[iy].End);
                            VertexCount += 2;
                            pVertex += 6;

                        }                            
                     

                    }
                }

            }
        }
    }
}

void SSonarSensor::DirectXFormBeam(VERTEX* pVertex, FPoint Point1,FPoint Point2)
{
    float minHeight = 20;
	float height = 50;

    DWORD Color = 0x7fffff00; // yellowish
//	DWORD Color = 0x7f7f7f00; // brownish, looks more seethrough then bright yellow

	pVertex[0].position.x = Point1.x;
	pVertex[0].position.y = minHeight;
	pVertex[0].position.z = Point1.y;
	pVertex[0].color = Color;
//	pVertex[0].tu = 0.0f;
//	pVertex[0].tv = 0.0f;

	pVertex[1].position.x = Point2.x;
	pVertex[1].position.y = minHeight;
	pVertex[1].position.z = Point2.y;
	pVertex[1].color = Color;
//	pVertex[1].tu = 1.0f;
//	pVertex[1].tv = 0.0f;

	pVertex[2].position.x = Point2.x;
	pVertex[2].position.y = height;
	pVertex[2].position.z = Point2.y;
	pVertex[2].color = Color;
//	pVertex[2].tu = 1.0f;
//	pVertex[2].tv = 1.0f;

	pVertex[3].position.x = Point1.x;
	pVertex[3].position.y = minHeight;
	pVertex[3].position.z = Point1.y;
	pVertex[3].color = Color;
//	pVertex[3].tu = 0.0f;
//	pVertex[3].tv = 0.0f;

	pVertex[4].position.x = Point2.x;
	pVertex[4].position.y = height;
	pVertex[4].position.z = Point2.y;
	pVertex[4].color = Color;
//	pVertex[4].tu = 1.0f;
//	pVertex[4].tv = 1.0f;

	pVertex[5].position.x = Point1.x;
	pVertex[5].position.y = height;
	pVertex[5].position.z = Point1.y;
	pVertex[5].color = Color;
//	pVertex[5].tu = 0.0f;
//	pVertex[5].tv = 1.0f;

}

/************************************************************************
* Function SonarCommand(float &TimeElapse)
*
* PURPOSE
* This is the functions used to simulated the various modes of the sonar module
* USAGE
* Called if the SimulateSonar is either done rotating or pinging.  This function
* Sets it next instruction
*************************************************************************/
void SSonarSensor::RunSonarCommand(float &TimeElapse)
{
	if (SonarCommand.SonarCommand == SonarStream)
	{
		// this is not valid for the stream state
		if (SonarCommand.GotoCommand)
			SonarCommand.GotoCommand = false;

		if (SonarCommand.RotateState)
		{
			// Sonar done rotating.  Get ready to beam
            ResetBeam1();
            SonarSensor.CurrBeamOrigin = SonarSensor.TranslatedDisplayPoint;

            SonarSensor.BeamAngle = SonarCommand.CurrentAngle + ModelInfo.CurrAngle-90;  
            SonarCommand.RotateState = false;  
            SonarSensor.CurrentBeamDistance = 0;			

		}
		else
		{
			// rotates the sonar at stream settings
			RotateSonarStream();		

		}
	}
	else if (SonarCommand.SonarCommand == ScanFowards)
	{
		if (SonarCommand.CurrentAngle != 0)
		{
			// make sure sonar is zeroed
			SonarCommand.DesiredAngle = 0;
			SonarCommand.RotateState = true; 		
		}
		else
		{
			// put into normal scan fowards (once zeroed
			SonarCommand.SonarCommand = ScanFowards;
			
			SonarCommand.RotateState = false; 
			// fire off the beams	
			if (CanFireSonar(TimeElapse))
			{
				ResetBeam1();
				SonarSensor.CurrBeamOrigin = SonarSensor.TranslatedDisplayPoint;

				SonarSensor.BeamAngle = SonarCommand.CurrentAngle + ModelInfo.CurrAngle-90;  				 
				SonarSensor.CurrentBeamDistance = 0;

				SonarCommand.GotoCommand = false;				
				//SimulateSonar(TimeElapse);
			}
			else
			{
				TimeElapse = 0;
				SonarCommand.GotoCommand = true;
			}
		}		
	}
	else if (SonarCommand.SonarCommand == ThreePointScan)
	{
		// {GotoRight, ScanRight,GotoFoward, ScanFoward, GotoLeft, ScanLeft};
		if (ThreePointEnum == GotoRight)
		{
			SonarCommand.DesiredAngle = TPTempAngle = 270-TPScanArc/2;
			SonarCommand.GotoCommand = false;
			// won't return until in proper position
			ThreePointEnum = ScanRight;
			SonarCommand.RotateState = true; 
			SonarCommand.ClockWise = true;
			// select the TP beem angle constant
			TPBeamAngleTemp = BeamAngleConst;
			BeamAngleConst = TPBeamAngleConst;
			// init the sonar to 0's
			for (int ix = 0; ix < 3; ix++)
				for (int iy = 0; iy < 30; iy++)
					ThreePointD[ix][iy] = 0;

			SNRArrayIndex = 0;
		}
		else if (ThreePointEnum == ScanRight)
		{			
			SonarCommand.ClockWise = false;
			if (SonarCommand.DesiredAngle == TPTempAngle)
			{
				// shoot out beam
				ResetBeam1();
				SonarSensor.CurrBeamOrigin = SonarSensor.TranslatedDisplayPoint;

				SonarSensor.BeamAngle = SonarCommand.CurrentAngle + ModelInfo.CurrAngle-90;  				 
				SonarSensor.CurrentBeamDistance = 0;

				SonarCommand.RotateState = false;
				SonarCommand.GotoCommand = false;	
				
				TPTempAngle += TPScanAngle;
				if (TPTempAngle > (270+TPScanArc/2))
				{
					// time to go to the next state
					SonarCommand.GotoCommand = true;
					ThreePointEnum = GotoFoward;
					TimeElapse = 0; // so it doesn't shoot out another beam
				}

			}
			else
			{
				// record old value
				ThreePointD[0][SNRArrayIndex] = LastDetectionDistance;
				SNRArrayIndex++;
			
				// rotate
				SonarCommand.DesiredAngle = (float)TPTempAngle;
				SonarCommand.GotoCommand = false;
				SonarCommand.RotateState = true; 
			}
		}
		else if (ThreePointEnum == GotoFoward)
		{
			SonarCommand.DesiredAngle = TPTempAngle = (float)(0-TPScanArc/2);
			SonarCommand.GotoCommand = false;
			// won't return until in proper position
			ThreePointEnum = ScanFoward;
			SonarCommand.RotateState = true; 
			SNRArrayIndex = 0;
		}
		else if (ThreePointEnum == ScanFoward)
		{			
			if (SonarCommand.DesiredAngle == TPTempAngle)
			{
				// shoot out beam
				ResetBeam1();
				SonarSensor.CurrBeamOrigin = SonarSensor.TranslatedDisplayPoint;

				SonarSensor.BeamAngle = SonarCommand.CurrentAngle + ModelInfo.CurrAngle-90;  				 
				SonarSensor.CurrentBeamDistance = 0;

				SonarCommand.RotateState = false;
				SonarCommand.GotoCommand = false;	
				
				TPTempAngle += TPScanAngle;
				if (TPTempAngle > (0 + TPScanArc/2))
				{
					// time to go to the next state
					SonarCommand.GotoCommand = true;
					ThreePointEnum = GotoLeft;
					TimeElapse = 0; // so it doesn't shoot out another beam
				}

			}
			else
			{
					// record old value
				ThreePointD[1][SNRArrayIndex] = LastDetectionDistance;
				SNRArrayIndex++;
				// rotate
				SonarCommand.DesiredAngle = (float)TPTempAngle;
				SonarCommand.GotoCommand = false;
				SonarCommand.RotateState = true; 
			}
		}
		else if (ThreePointEnum == GotoLeft)
		{
			SonarCommand.DesiredAngle = TPTempAngle = (float)(90-TPScanArc/2);
			SonarCommand.GotoCommand = false;
			// won't return until in proper position
			ThreePointEnum = ScanLeft;
			SonarCommand.RotateState = true; 
			SNRArrayIndex = 0;
		}
		else if (ThreePointEnum == ScanLeft)
		{			
			if (SonarCommand.DesiredAngle == TPTempAngle)
			{
				// shoot out beam
				ResetBeam1();
				SonarSensor.CurrBeamOrigin = SonarSensor.TranslatedDisplayPoint;

				SonarSensor.BeamAngle = SonarCommand.CurrentAngle + ModelInfo.CurrAngle-90;  				 
				SonarSensor.CurrentBeamDistance = 0;

				SonarCommand.RotateState = false;
				SonarCommand.GotoCommand = false;	
				
				TPTempAngle += TPScanAngle;
				if (TPTempAngle > (90 + TPScanArc/2))
				{
					// time to go to the next state
					SonarCommand.GotoCommand = true;
					ThreePointEnum = GotoRight;
					SonarCommand.SonarCommand = ScanFowards;
					SonarCommand.ClockWise = true;
					// select back the normal beam constant
					BeamAngleConst = TPBeamAngleTemp;
					TimeElapse = 0; // so it doesn't shoot out another beam
				}
			}
			else
			{
				
				// record old value
				ThreePointD[2][SNRArrayIndex] = LastDetectionDistance;
				SNRArrayIndex++;
				// rotate
				SonarCommand.DesiredAngle = (float)TPTempAngle;
				SonarCommand.GotoCommand = false;
				SonarCommand.RotateState = true; 
			}
		}
	}
}

/************************************************************************
* Function SNRGetFowardDistance(short *Distance)
* ROBOT API
* PURPOSE
* To return the distance of the last detection, returns false if not valid
*************************************************************************/
BOOL SSonarSensor::SNRGetFowardDistanceA(short *Distance)
{
	if (SonarCommand.CurrentAngle != 0)
	{
		*Distance = 999;
		return TRUE;
	}
	else
	{
		if ((LastDetectionDistance == SOUTOFRANGE)
			|| (LastDetectionDistance == SNOTREADY))
			*Distance = 999;
		else
			*Distance = (short)LastDetectionDistance;
		return TRUE;
	}

}
/************************************************************************
* Function void SNRSetFowardScan()
* ROBOT API
* PURPOSE
* Sets the sonar mode to foward scan
*************************************************************************/
void SSonarSensor::SNRSetFowardScanA()
{
	SonarCommand.SonarCommand = ScanFowards;
	SonarCommand.GotoCommand = true;
}

/************************************************************************
* Function void SNRSet3PointScan()
* ROBOT API
* PURPOSE
* Sets the sonar mode to three point scan mode
*************************************************************************/
void SSonarSensor::SNRSet3PointScanA()
{
	SonarCommand.SonarCommand = ThreePointScan;
	SonarCommand.GotoCommand = true;
}

/************************************************************************
* Function void SNRGet3PointDistance(short *distance[])
* ROBOT API
* PURPOSE
* to return the type of obstance in each direction (right, foward, left)
* ALGORITHM
* A wall is defined as not having anything greater then a detection distance of 1 meters
* An unknown area is defined as being between 1-5 meters, and having a stiff cutoff from,
* its peak area to its non peak area in terms of distance
* A hallway is defined as either having a maximum distance greater then 5 meters, or anything that
* is not the above
* In Addition a unknown is classified as having an even backwall.
*************************************************************************/
void SSonarSensor::SNRGet3PointDistanceA(short distance[3])
{
	short index,ix,maxreading,Delta,NumDelta,temp,numberofeven;
	int MaxDistance;

	// temp hacks 
	for (ix = 0; ix < 3; ix++)
	{
		MaxDistance = 0;
		index = 0;
		maxreading = 0;
		numberofeven = 0;
		// find out its maximum reading
		for (index = 0; index < 30; index++)
		{
			if (ThreePointD[ix][index] == SOUTOFRANGE)
				maxreading = 1000;
			else if (maxreading < ThreePointD[ix][index])
				maxreading = (short)ThreePointD[ix][index];
		}
		if (maxreading > 500)
			distance[ix] = SNRHallway;
		else if (maxreading < 100)
			distance[ix] = SNRWall;
		else
		{
			// keep on going, either unknown or hallway
			// to find out some any difference in values over 30 cms. and increment
			// how many items are in the index.  A unknown is going to have a few steep
			// differences, a hallway is going to have many soft differences
			Delta = 0;
			NumDelta = 0;
			for (index = 0; index < 29; index++)
			{
				if (ThreePointD[ix][index+1] != 0)
				{
					if (ThreePointD[ix][index+1] > MaxDistance)
						MaxDistance = ThreePointD[ix][index+1];

					temp = (short)abs((short)ThreePointD[ix][index] - (short)ThreePointD[ix][index+1]);
					if (temp > 20)
					{
						Delta += temp;
						NumDelta++;
						numberofeven = 0;
					}
					else
					{
						numberofeven++;
						if (numberofeven > 5)
						{
							Delta+=5000; // should guarentee an unknow classification;
							numberofeven = 0;

						}
					}
							

				}
			}
			if (NumDelta > 0)
			{
				int tempo = Delta/NumDelta;
			}
			if (NumDelta == 0)
				distance[ix] = SNRHallway;
			else if (Delta/NumDelta < 150)
			{
				distance[ix] = SNRHallway;
				if (MaxDistance < 420)
				{
					distance[ix] = SNRUnknown; //extra check
				}
			}
			else
				distance[ix] = SNRUnknown;

			if ( ix == 0)
			{
			//	ASSERT(distance[0] == SNRHallway);
			}


		}
	}/*
		







	}
	if (ThreePointD[ix] == SOUTOFRANGE)
		distance[ix] = SNRHallway;
	else if (ThreePointD[ix] < 150) // 1.5 meters
		distance[ix] = SNRWall;
	else if (ThreePointD[ix] > 400) // four meters
		distance[ix] = SNRHallway;
	else
		distance[ix] = SNRUnknown;*/
}

/************************************************************************
* Function void SNRIsBlocking()
* ROBOT API
* PURPOSE
* To return if a blocking sonar function is in progress
*************************************************************************/
BOOL SSonarSensor::SNRIsBlockingA()
{
	if (SonarCommand.SonarCommand == ThreePointScan)
		return TRUE;
	else
		return FALSE;
}

/************************************************************************
* Function void SNRReset()
* ROBOT API
* PURPOSE
* To reset the sonar mode
*************************************************************************/
void SSonarSensor::SNRResetA()
{
	SonarCommand.SonarCommand = ScanFowards;
	SonarCommand.GotoCommand = true;
}

/************************************************************************
* Function float DirectXGetAngle()
*
* PURPOSE
* Returns the current angle of the sonar for directX rendering
*************************************************************************/
float SSonarSensor::DirectXGetAngle()
{

	return ModelInfo.CurrAngle + SonarSensor.Angle-90;
}
