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

#include "stdafx.h"
#include "Simulator.h"
#include "SBumpSensor.h"

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

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

SBumpSensor::SBumpSensor()
{
    for (int ix = 0; ix < MAXBUMPSENSOR; ix++)
    {
        BumpSensors[ix].Valid = false;
        TransformedBumpSensors[ix].Valid = false;
    }
    ClearBumpSensor();

    // load colors and pens    
    Colors[0] = RGB(0,0,0);
    Colors[RED] = RGB(255,0,0);
    ColorPens[0].CreatePen(PS_SOLID,1,Colors[0]);
    ColorPens[RED].CreatePen(PS_SOLID,1,Colors[RED]);
    


}

SBumpSensor::~SBumpSensor()
{

}

/************************************************************************
* 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.  This does it for the bump sensors
* USAGE
* By the model open file
*************************************************************************/
void SBumpSensor::AdjustforCenter(float Left, float Top)
{
    for (int ix = 0; ix < MAXBUMPSENSOR; ix++)
    {
        if (BumpSensors[ix].Valid)
        {
            BumpSensors[ix].DisplayPoint.x -= Left;
            BumpSensors[ix].DisplayPoint.y -= Top;
            BumpSensors[ix].StartRange.x -= Left;
            BumpSensors[ix].StartRange.y -= Top;
            BumpSensors[ix].EndRange.x -= Left;
            BumpSensors[ix].EndRange.y -= Top;
        }
    }

}


/************************************************************************
* Function LoadBumpSensor(CPoint StartRange, CPoint EndRange, CPoint DisplayPoint, int IDNumber)
*
* PURPOSE
* To create a new bump sensors based on the model file
* USAGE
* By the model open file
*************************************************************************/
void SBumpSensor::LoadBumpSensor(CPoint StartRange, CPoint EndRange, CPoint DisplayPoint, int IDNumber)
{
    if (IDNumber < MAXBUMPSENSOR)
    {
        BumpSensors[IDNumber].Valid = true;
        BumpSensors[IDNumber].StartRange = StartRange;
        BumpSensors[IDNumber].EndRange = EndRange;
        BumpSensors[IDNumber].DisplayPoint = DisplayPoint;
    }
}

/************************************************************************
* Function GDIRender(CDC &MemDC, CRect Quad, CRect Screen)
*
* PURPOSE
* Draw a bump 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 SBumpSensor::GDIRender(CDC &MemDC, FRect Quad, CRect Screen, float SRScale, FPoint RenderModelCenter)
{
    // sync with the rendering frame not the simulation frame
    TranslateSensors(RenderModelCenter);
      
    float XScale = (float)Screen.Width()/(float)Quad.Width();
    float YScale = (float)Screen.Height()/(float)Quad.Height();
    float OldXScale = XScale;
    float OldYScale = YScale;

    float Scale = SRScale*6; // to adjust the size
    XScale = XScale/Scale;
    YScale = YScale/Scale;
    float CircleRadius = 1; 

    // tranform e.i where to start the lines
    float XTransform = Quad.left;
    float YTransform = Quad.top;
    
    for (int ix = 0; ix < MAXBUMPSENSOR; ix++)
    {
        if (TransformedBumpSensorsR[ix].Valid)
        {
            CPoint RealCenter;
            // get the center point (screen coordinates) of which to render
            RealCenter.x = LAUtils.Round((TransformedBumpSensorsR[ix].DisplayPoint.x-XTransform)*OldXScale);
            RealCenter.y = LAUtils.Round((TransformedBumpSensorsR[ix].DisplayPoint.y-YTransform)*OldYScale);        

            CPen *OldPen;

            if (TransformedBumpSensorsR[ix].isCollided)
            {
                OldPen = MemDC.SelectObject(&ColorPens[RED]);
            }

            //  CBrush *OldBrush;
    
            CRect Border;
     
            // create the outline
            Border.top = RealCenter.y - (long)(YScale*5);
            Border.bottom = RealCenter.y + (long)(YScale*5);
            Border.left = RealCenter.x - (long)(XScale*10);
            Border.right = RealCenter.x + (long)(XScale*10);

            CPoint TopLeft,TopRight,BottomLeft,BottomRight;

            TopLeft.x = Border.left;
            TopLeft.y = Border.top;

            TopRight.x = Border.right;
            TopRight.y = Border.top;

            BottomLeft.x = Border.left;
            BottomLeft.y = Border.bottom;

            BottomRight.x = Border.right;
            BottomRight.y = Border.bottom;
            

            float Angle = TransformedBumpSensorsR[ix].Angle;

            TopLeft = LAUtils.RotatePoint(TopLeft,RealCenter,Angle);
            TopRight = LAUtils.RotatePoint(TopRight,RealCenter,Angle);
            BottomLeft = LAUtils.RotatePoint(BottomLeft,RealCenter,Angle);
            BottomRight = LAUtils.RotatePoint(BottomRight,RealCenter,Angle);

            // draw the border
            MemDC.MoveTo(TopLeft);
            MemDC.LineTo(TopRight);
            MemDC.LineTo(BottomRight);
            MemDC.LineTo(BottomLeft);
            MemDC.LineTo(TopLeft);

            // get a color brush to fill in the circles
           // OldBrush = MemDC.SelectObject(&ColorBrush[color]);

            // create the switch look.
            // first to the left
            CPoint Point;
            Point.x = RealCenter.x - (long)(XScale*9);
            Point.y = RealCenter.y + (long)(YScale*2); 
            Point = LAUtils.RotatePoint(Point,RealCenter,Angle);
            MemDC.MoveTo(Point);

            Point.x = RealCenter.x - (long)(XScale*3);
            Point.y = RealCenter.y + (long)(YScale*2);   
            Point = LAUtils.RotatePoint(Point,RealCenter,Angle);
            MemDC.Ellipse(Point.x-(long)(CircleRadius*XScale),Point.y-(long)(CircleRadius*YScale),Point.x+(long)(CircleRadius*XScale),Point.y+(long)(CircleRadius*YScale));
            MemDC.LineTo(Point);

            Point.x = RealCenter.x + (long)(XScale*2);
            Point.y = RealCenter.y - (long)(YScale*3);   
            Point = LAUtils.RotatePoint(Point,RealCenter,Angle);
            MemDC.LineTo(Point);

            Point.x = RealCenter.x + (long)(XScale*3);
            Point.y = RealCenter.y + (long)(YScale*2);
            Point = LAUtils.RotatePoint(Point,RealCenter,Angle);
            MemDC.Ellipse(Point.x-(long)(CircleRadius*XScale),Point.y-(long)(CircleRadius*YScale),Point.x+(long)(CircleRadius*XScale),Point.y+(long)(CircleRadius*YScale));
            MemDC.MoveTo(Point);

            Point.x = RealCenter.x + (long)(XScale*9);
            Point.y = RealCenter.y + (long)(YScale*2);
            Point = LAUtils.RotatePoint(Point,RealCenter,Angle);
            MemDC.LineTo(Point);  

            if (TransformedBumpSensorsR[ix].isCollided)
            {
                 MemDC.SelectObject(OldPen);
            }
        }
    }
   //  MemDC.SelectObject(OldBrush);


}


/************************************************************************
* Function GetBumpSensorByte()
* ROBOT API
* PURPOSE
* To simulate the bump sensor byte.  Same as the embedded systems mapping
* NOTE
* bumper 0-7 coorsisponds front left then going around clockwise
*************************************************************************/
unsigned char SBumpSensor::GetBumpSensorByte()
{

    unsigned char BumperArray[] = {FRONTLEFT,FRONTRIGHT,RIGHTFRONT,RIGHTBACK,
                                        REARLEFT,REARRIGHT,LEFTBACK,LEFTFRONT};

    unsigned char BumperByte = 0x00;

    for (int ix = 0; ix < MAXBUMPSENSOR; ix++)
    {
        if (BumpSensors[ix].Valid)
            if (BumpSensors[ix].isCollided)
                BumperByte |= BumperArray[ix];
    } 


    return BumperByte;

}


/************************************************************************
* Function SetBumpSensors(FPoint Collision)
*
* PURPOSE
* When a collision occurs the bump sensors have to be updated.
* The Model class sends over the collision point, and this function evaulates which
* bump sensors are turned on
* USAGE
* By the model class when a collision occurs
* INPUT
* The collision point
*************************************************************************/
void SBumpSensor::SetBumpSensors(FPoint Collision)
{
    TranslateSensors(); // update the bumper locations
     
    for (int ix = 0; ix < MAXBUMPSENSOR; ix++)
    {
        if (TransformedBumpSensors[ix].Valid)
        {
            // finds the bounds of this bump sensor
          //  FRect Bounds = LAUtils.FormRect(TransformedBumpSensors[ix].StartRange,TransformedBumpSensors[ix].EndRange);
            FRect Bounds = FormCollisionRect(ix);

            if (LAUtils.InRect(Collision,Bounds))
            {
                BumpSensors[ix].isCollided = true;
                TransformedBumpSensors[ix].isCollided = true;
            }
            else
            {
                BumpSensors[ix].isCollided = false;
                TransformedBumpSensors[ix].isCollided = false;
            }
        }
    }
   

}

/************************************************************************
* Function ClearBumpSensor()
*
* PURPOSE
* Clears the bump sensors collision status
*************************************************************************/
void SBumpSensor::ClearBumpSensor()
{
    for (int ix = 0; ix < MAXBUMPSENSOR; ix++)
    {        
        BumpSensors[ix].isCollided = false;
        TransformedBumpSensors[ix].isCollided = false;

    }
}


/************************************************************************
* Function UpdateModel(SBumpSensor::MODELINFO ModelInfo)
*
* PURPOSE
* The BumpSensor 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 SBumpSensor::UpdateModel(SBumpSensor::MODELINFO ModelInfo)
{
    this->ModelInfo = ModelInfo;
}

/************************************************************************
* 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 bump sensors.
* OUTPUT
* Intstead of returning a pointer, it changes TransformedBumpSensors based on BumpSensors
* an the model
*************************************************************************/
void SBumpSensor::TranslateSensors()
{
    FPoint Center;
    Center.x = ModelInfo.x;
    Center.y = ModelInfo.y;

    double Angle = ModelInfo.CurrAngle;

    for (int ix = 0; ix < MAXBUMPSENSOR; ix++)
    {
        if (BumpSensors[ix].Valid)
        {
            TransformedBumpSensors[ix].Valid = true;
            TransformedBumpSensors[ix].StartRange.x = ModelInfo.x + BumpSensors[ix].StartRange.x - ModelInfo.Center.x;
            TransformedBumpSensors[ix].StartRange.y = ModelInfo.y + BumpSensors[ix].StartRange.y - ModelInfo.Center.y;
            TransformedBumpSensors[ix].EndRange.x = ModelInfo.x + BumpSensors[ix].EndRange.x - ModelInfo.Center.x;
            TransformedBumpSensors[ix].EndRange.y = ModelInfo.y + BumpSensors[ix].EndRange.y - ModelInfo.Center.y;
            TransformedBumpSensors[ix].DisplayPoint.x = ModelInfo.x + BumpSensors[ix].DisplayPoint.x - ModelInfo.Center.x;
            TransformedBumpSensors[ix].DisplayPoint.y = ModelInfo.y + BumpSensors[ix].DisplayPoint.y - ModelInfo.Center.y;

            // rotate and translate the points
            TransformedBumpSensors[ix].StartRange =
                LAUtils.RotatePoint(TransformedBumpSensors[ix].StartRange,Center,Angle);
            TransformedBumpSensors[ix].EndRange =
                LAUtils.RotatePoint(TransformedBumpSensors[ix].EndRange,Center,Angle);  
            TransformedBumpSensors[ix].DisplayPoint =
                LAUtils.RotatePoint(TransformedBumpSensors[ix].DisplayPoint,Center,Angle); 

            // used for display purposes
            double Height = LAUtils.Abs(BumpSensors[ix].EndRange.y - BumpSensors[ix].StartRange.y);
            double Width = LAUtils.Abs(BumpSensors[ix].EndRange.x - BumpSensors[ix].StartRange.x);

            float ExtraAngle;

            if (Height > Width)
                ExtraAngle = 90;
            else
                ExtraAngle = 0;

            TransformedBumpSensors[ix].Angle = (float)Angle + ExtraAngle;
            TransformedBumpSensors[ix].isCollided = BumpSensors[ix].isCollided;
        }
    }  
}

/************************************************************************
* Function TranslateSensors(FPoint RenderModelCenter)
*
* 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 bump sensors.
* OUTPUT
* Intstead of returning a pointer, it changes TransformedBumpSensors based on BumpSensors
* an the model
* NOTE same as TranslateSensors() except it dumps the output into the renderer array
*************************************************************************/
void SBumpSensor::TranslateSensors(FPoint RenderModelCenter)
{

    double Angle = ModelInfo.CurrAngle;

    for (int ix = 0; ix < MAXBUMPSENSOR; ix++)
    {
        if (BumpSensors[ix].Valid)
        {
            TransformedBumpSensorsR[ix].Valid = true;
            TransformedBumpSensorsR[ix].StartRange.x = RenderModelCenter.x + BumpSensors[ix].StartRange.x - ModelInfo.Center.x;
            TransformedBumpSensorsR[ix].StartRange.y = RenderModelCenter.y + BumpSensors[ix].StartRange.y - ModelInfo.Center.y;
            TransformedBumpSensorsR[ix].EndRange.x = RenderModelCenter.x + BumpSensors[ix].EndRange.x - ModelInfo.Center.x;
            TransformedBumpSensorsR[ix].EndRange.y = RenderModelCenter.y + BumpSensors[ix].EndRange.y - ModelInfo.Center.y;
            TransformedBumpSensorsR[ix].DisplayPoint.x = RenderModelCenter.x + BumpSensors[ix].DisplayPoint.x - ModelInfo.Center.x;
            TransformedBumpSensorsR[ix].DisplayPoint.y = RenderModelCenter.y + BumpSensors[ix].DisplayPoint.y - ModelInfo.Center.y;

            // rotate and translate the points
            TransformedBumpSensorsR[ix].StartRange =
                LAUtils.RotatePoint(TransformedBumpSensorsR[ix].StartRange,RenderModelCenter,Angle);
            TransformedBumpSensorsR[ix].EndRange =
                LAUtils.RotatePoint(TransformedBumpSensorsR[ix].EndRange,RenderModelCenter,Angle);  
            TransformedBumpSensorsR[ix].DisplayPoint =
                LAUtils.RotatePoint(TransformedBumpSensorsR[ix].DisplayPoint,RenderModelCenter,Angle); 

            // used for display purposes
            double Height = LAUtils.Abs(BumpSensors[ix].EndRange.y - BumpSensors[ix].StartRange.y);
            double Width = LAUtils.Abs(BumpSensors[ix].EndRange.x - BumpSensors[ix].StartRange.x);

            float ExtraAngle;

            if (Height > Width)
                ExtraAngle = 90;
            else
                ExtraAngle = 0;

            TransformedBumpSensorsR[ix].Angle = (float)Angle + ExtraAngle;
            TransformedBumpSensorsR[ix].isCollided = BumpSensors[ix].isCollided;
        }
    }  
}

/************************************************************************
* Function FormCollisionRect(int index)
*
* PURPOSE
* To create a rectange of interest to approximate finding if a bumper
* is hit
* USAGE
* To check if a collision point hit a bumper
* OUTPUT
* The rectange in which to look at
* NOTE
* When a bumper is rotates doing a straight rectange such as what this function doesn't
* isn't quite accurate.  A more accurate way of doing it would be to look inside of the
* polygon and not use a rectangle approximation
*************************************************************************/
FRect SBumpSensor::FormCollisionRect(int index)
{
    FRect toReturn;

    toReturn.bottom = -1;
    toReturn.left = -1;
    toReturn.right = -1;
    toReturn.top = -1;

    if (BumpSensors[index].Valid)
    {

         FPoint Center;
         Center.x = ModelInfo.x;
         Center.y = ModelInfo.y;

         double Angle = ModelInfo.CurrAngle;

         FRect UnrotatedBounds = LAUtils.FormRect(BumpSensors[index].StartRange,BumpSensors[index].EndRange);

         UnrotatedBounds.left += ModelInfo.x - ModelInfo.Center.x;
         UnrotatedBounds.right += ModelInfo.x - ModelInfo.Center.x;
         UnrotatedBounds.top += ModelInfo.y - ModelInfo.Center.y;
         UnrotatedBounds.bottom += ModelInfo.y - ModelInfo.Center.y;


         FPoint TopLeft, TopRight, BottomLeft, BottomRight;
         TopLeft.x = UnrotatedBounds.left;
         TopLeft.y = UnrotatedBounds.top;

         TopRight.x = UnrotatedBounds.right;
         TopRight.y = UnrotatedBounds.top;

         BottomLeft.x = UnrotatedBounds.right;
         BottomLeft.y = UnrotatedBounds.bottom;

         BottomRight.x = UnrotatedBounds.left;
         BottomRight.y = UnrotatedBounds.bottom;

         TopLeft = LAUtils.RotatePoint(TopLeft,Center,Angle);
         TopRight = LAUtils.RotatePoint(TopRight,Center,Angle);
         BottomLeft = LAUtils.RotatePoint(BottomLeft,Center,Angle);
         BottomRight = LAUtils.RotatePoint(BottomRight,Center,Angle); 

         toReturn = LAUtils.FormRectF(TopLeft,TopRight,BottomLeft,BottomRight);
    }

    return toReturn;

}
