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

#include "stdafx.h"
#include "Simulator.h"
#include "SIRSensor.h"

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

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

SIRSensor::SIRSensor()
{

    srand( (unsigned)time( NULL ) ); // randomize the timer

    NonIdealityFactor = 0;


    for (int ix = 0; ix < MAXIRSENSOR; ix++)
    {
        IRSensors[ix].Valid = false;
        TransformedIRSensors[ix].Valid = false;
        TransformedIRSensorsR[ix].Valid = false;

    }

     // 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]);

}

SIRSensor::~SIRSensor()
{

}

/************************************************************************
* Function LoadIRSensor(CPoint DisplayPoint, long MinRange, long MaxRange, long BeamAngle, long ID)
*
* PURPOSE
* To load an IRSensor from a file
* USAGE
* By the Model class, when it is loading the model.
*************************************************************************/
void SIRSensor::LoadIRSensor(CPoint DisplayPoint, long MinRange, long MaxRange, long BeamAngle, long ID)
{
    // check to make sure it is valid
    if ((ID < MAXIRSENSOR) && (ID >= 0))
    {
        IRSensors[ID].Valid = true;
        IRSensors[ID].BeamAngle = (float)BeamAngle;
        IRSensors[ID].DisplayPoint = DisplayPoint;
        IRSensors[ID].MaxRange = MaxRange;
        IRSensors[ID].MinRange = MinRange;
    }
}

/************************************************************************
* Function UpdateModel(SBumpSensor::MODELINFO ModelInfo)
*
* PURPOSE
* The IRSensor 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 SIRSensor::UpdateModel(SIRSensor::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 IR sensors.
* OUTPUT
* Intstead of returning a pointer, it changes TransformedIRSensors based on IRSensors
* an the model
*************************************************************************/
void SIRSensor::TranslateSensors()
{
    FPoint Center;
    Center.x = ModelInfo.x;
    Center.y = ModelInfo.y;

    double Angle = ModelInfo.CurrAngle;

    for (int ix = 0; ix < MAXIRSENSOR; ix++)
    {
        if (IRSensors[ix].Valid)
        {
            TransformedIRSensors[ix].Valid = true;           
            TransformedIRSensors[ix].DisplayPoint.x = ModelInfo.x + IRSensors[ix].DisplayPoint.x - ModelInfo.Center.x;
            TransformedIRSensors[ix].DisplayPoint.y = ModelInfo.y + IRSensors[ix].DisplayPoint.y - ModelInfo.Center.y;

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

            // rotate the beam angle
            TransformedIRSensors[ix].BeamAngle = (float)(IRSensors[ix].BeamAngle + Angle);

            TransformedIRSensors[ix].MinRange = IRSensors[ix].MinRange;
            TransformedIRSensors[ix].MaxRange = IRSensors[ix].MaxRange;
        }
    }  

}

/************************************************************************
* 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 IR sensors.
* OUTPUT
* Intstead of returning a pointer, it changes TransformedIRSensors based on IRSensors
* an the model
*************************************************************************/
void SIRSensor::TranslateSensors(FPoint RenderModelCenter)
{

    double Angle = ModelInfo.CurrAngle;

    for (int ix = 0; ix < MAXIRSENSOR; ix++)
    {
        if (IRSensors[ix].Valid)
        {
            TransformedIRSensorsR[ix].Valid = true;           
            TransformedIRSensorsR[ix].DisplayPoint.x = RenderModelCenter.x + IRSensors[ix].DisplayPoint.x - ModelInfo.Center.x;
            TransformedIRSensorsR[ix].DisplayPoint.y = RenderModelCenter.y + IRSensors[ix].DisplayPoint.y - ModelInfo.Center.y;

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

            // rotate the beam angle
            TransformedIRSensorsR[ix].BeamAngle = (float)(IRSensors[ix].BeamAngle + Angle);

            TransformedIRSensorsR[ix].MinRange = IRSensors[ix].MinRange;
            TransformedIRSensorsR[ix].MaxRange = IRSensors[ix].MaxRange;
        
            // copy over the detection point
            TransformedIRSensorsR[ix].DetectionPoint = TransformedIRSensors[ix].DetectionPoint;
        }
    }  

}

/************************************************************************
* 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 SIRSensor::GDIRender(CDC &MemDC, FRect Quad, CRect Screen, float SRScale, FPoint RenderModelCenter)
{

    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;// 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 < MAXIRSENSOR; ix++)
      {
            if (TransformedIRSensorsR[ix].Valid)
            {
                CPoint RealCenter;
                // get the center point (screen coordinates) of which to render
                RealCenter.x = (long)((TransformedIRSensorsR[ix].DisplayPoint.x-XTransform)*OldXScale);
                RealCenter.y = (long)((TransformedIRSensorsR[ix].DisplayPoint.y-YTransform)*OldYScale);                          

                float Angle = TransformedIRSensorsR[ix].BeamAngle+90;

                 // draw the border (5 by 2 units)
                CRect Border;

                // create the outline (dx = 5 dy = 2)
                Border.top = RealCenter.y - (long)(YScale*1);
                Border.bottom = RealCenter.y + (long)(YScale*1);
                Border.left = RealCenter.x - (long)(XScale*2.5);
                Border.right = RealCenter.x + (long)(XScale*2.5);

                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;

                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);

                // now draw the two cicles for the emmittor/detector
                CPoint Emitter,Detector,Center10;
                // times 10 so decimal values could occur
                Emitter.x = (long)(RealCenter.x - 1.3*XScale);
                Emitter.y = RealCenter.y;
                Detector.x = (long)(RealCenter.x + 1.3*XScale);
                Detector.y = RealCenter.y;

                // old, just in case *10 is needed to "floatize" the integer arithmetic.
                Center10.x = RealCenter.x;
                Center10.y = RealCenter.y;   

                // now rotate the points about the center
                Emitter = LAUtils.RotatePoint(Emitter,Center10,Angle);
                Detector = LAUtils.RotatePoint(Detector,Center10,Angle);

                // divide
                Emitter.x = Emitter.x;
                Emitter.y = Emitter.y;
                Detector.x = Detector.x;
                Detector.y = Detector.y;

                float CircleRadius = (float)0.8;

                MemDC.Ellipse(Emitter.x-(long)(CircleRadius*XScale),Emitter.y-(long)(CircleRadius*YScale),Emitter.x+(long)(CircleRadius*XScale),Emitter.y+(long)(CircleRadius*YScale));
                MemDC.Ellipse(Detector.x-(long)(CircleRadius*XScale),Detector.y-(long)(CircleRadius*YScale),Detector.x+(long)(CircleRadius*XScale),Detector.y+(long)(CircleRadius*YScale));

                // create little arrow to show which direction the beam goes (foward/backwards?)
                CPoint Point1,Point2,Point3;

                float TriangleSize = CircleRadius/(float)1.5;

                // bottom left
                Point1.x = (long)(RealCenter.x - TriangleSize*XScale);
                Point1.y = (long)(RealCenter.y - TriangleSize*XScale);
                // bottom right
                Point2.x = (long)(RealCenter.x + TriangleSize*XScale);
                Point2.y = (long)(RealCenter.y - TriangleSize*XScale);
                // top middle
                Point3.x = (long)(RealCenter.x);
                Point3.y = (long)(RealCenter.y + TriangleSize*XScale);

                Point1 = LAUtils.RotatePoint(Point1,Center10,Angle);
                Point2 = LAUtils.RotatePoint(Point2,Center10,Angle);
                Point3 = LAUtils.RotatePoint(Point3,Center10,Angle);

                 // draw the triangle
                MemDC.MoveTo(Point1);
                MemDC.LineTo(Point2);
                MemDC.LineTo(Point3);
                MemDC.LineTo(Point1);

                // draw the beam 
                if (TransformedIRSensorsR[ix].DetectionPoint.x != NODETECTION)
                {
                    CPoint ScreenPoint;

                    // translate the real point into a screen point
                    ScreenPoint.x = (long)((TransformedIRSensorsR[ix].DetectionPoint.x - Quad.left) * (float)Screen.Width() /(float)Quad.Width() +  Screen.left);
                    ScreenPoint.y = (long)((TransformedIRSensorsR[ix].DetectionPoint.y - Quad.top) * (float)Screen.Height() / (float)Quad.Height() + Screen.top);

                    CPen *OldPen = MemDC.SelectObject(&ColorPens[RED]);
                       
                    CPoint PlusAngle,MinusAngle;
                    PlusAngle = LAUtils.RotatePoint(ScreenPoint,Point3,GDIBEAMANGLE);
                    MinusAngle = LAUtils.RotatePoint(ScreenPoint,Point3,-GDIBEAMANGLE);

					MemDC.MoveTo(Point3);
                    MemDC.LineTo(PlusAngle);
                    MemDC.LineTo(MinusAngle);
                    MemDC.LineTo(Point3);
                    MemDC.SelectObject(OldPen);
                }
            }
      }  
}

/************************************************************************
* 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 SIRSensor::AdjustforCenter(float Left, float Top)
{
    for (int ix = 0; ix < MAXIRSENSOR; ix++)
    {
        if (IRSensors[ix].Valid)
        {
            IRSensors[ix].DisplayPoint.x -= Left;
            IRSensors[ix].DisplayPoint.y -= Top;
        }
    }

}


/************************************************************************
* Function FindDetection()
*
* PURPOSE
* Checks every IR sensors to see if it has detected anything
* USAGE
* During a simulator run
* Returns
* None, Sets internal values.
*************************************************************************/
void SIRSensor::FindDetection()
{
    // make sure to update sensors
    TranslateSensors();

    for (int ix = 0; ix < MAXIRSENSOR; ix++)
    {
        if (IRSensors[ix].Valid)
        {
            FPoint Detection = MapPtr->DetectCollision(TransformedIRSensors[ix].DisplayPoint,TransformedIRSensors[ix].BeamAngle,TransformedIRSensors[ix].MaxRange);
            if (Detection.x != NODETECTION)
            {
                IRSensors[ix].DetectionPoint = Detection;
                IRSensors[ix].DetectionDistance = LAUtils.Round(LAUtils.GetDistance(Detection,TransformedIRSensors[ix].DisplayPoint));
                RandomizeIR( IRSensors[ix].DetectionDistance);
                TransformedIRSensors[ix].DetectionPoint = Detection;
            }
            else
            {
                IRSensors[ix].DetectionPoint.x = NODETECTION;
                IRSensors[ix].DetectionDistance = NODETECTIOND;
                TransformedIRSensors[ix].DetectionPoint.x = NODETECTION;
            }

        }
    }
}

/************************************************************************
* Function unsigned char GetRightIR()
* Robot API
* PURPOSE
* To return the distance of the right IR
*************************************************************************/
unsigned char SIRSensor::GetRightIR()
{
    return (unsigned char)IRSensors[RIGHTIR].DetectionDistance;

}


/************************************************************************
* Function unsigned char GetFrontIR()
* Robot API
* PURPOSE
* To return the distance of the front IR
*************************************************************************/
unsigned char SIRSensor::GetFrontIR()
{
    return (unsigned char)IRSensors[FRONTIR].DetectionDistance;

}

/************************************************************************
* Function void RandomizeIR(unsigned short &IRDistance)
* 
* PURPOSE
* To simulate the random noise that may occur in real life
* ALGORIHTM
* Take 1% of IRDistance, Multiply it by the IRDistance / 20 then multiply it
* By the random factor time a random number from -1 to +1
*************************************************************************/
void SIRSensor::RandomizeIR(unsigned short &IRDistance)
{
    float Error = IRDistance*(float)0.01*IRDistance/20;

    Error = (Error * ((float)rand()/(float)RAND_MAX*2)-1)*NonIdealityFactor;

    IRDistance += (unsigned short)Error;

}

/************************************************************************
* Function void SetIRNonIdeality(int NonIdeality)
* 
* PURPOSE
* Sets the non ideality factor
*************************************************************************/
void SIRSensor::SetIRNonIdeality(int NonIdeality)
{
    NonIdealityFactor = NonIdeality;
}

/************************************************************************
* Function DirectXRender(VERTEX *&pVertex, FPoint RenderModelCenter, int &VertexCount)
* 
* PURPOSE
* To render the directX version of the IR sensor beams.
*************************************************************************/
void SIRSensor::DirectXRender(VERTEX *&pVertex, FPoint RenderModelCenter, int &VertexCount)
{

      float StartHeight = 9;

      TranslateSensors(RenderModelCenter);

        static int Colorcycle = 128; // used for cycling the collors;
        static int Colorcycle2 = 256;
        static int Colorcycle3 = 256;
		static int Colorcycle4 = 256;
        static int cyclemax = 255;
        static int Sign = 1;
        static int Sign2 = 1;
        static int Sign3 = 1;

     // draw the beam, it is composed of 6 triangles

      for (int ix = 0; ix < MAXIRSENSOR; ix++)
      {
            if (TransformedIRSensorsR[ix].DetectionPoint.x != NODETECTION)
            {
                FPoint PlusAngle,MinusAngle;
                PlusAngle = LAUtils.RotatePoint(TransformedIRSensorsR[ix].DetectionPoint,TransformedIRSensorsR[ix].DisplayPoint,GDIBEAMANGLE);
                MinusAngle = LAUtils.RotatePoint(TransformedIRSensorsR[ix].DetectionPoint,TransformedIRSensorsR[ix].DisplayPoint,-GDIBEAMANGLE);

                float YDistance = LAUtils.GetDistance(PlusAngle,TransformedIRSensorsR[ix].DetectionPoint);

                // first do the four traingles from the base

             
                DWORD BaseColor = 0x00000000 | (Colorcycle << 24); 
                DWORD Shift1 = BaseColor + (Colorcycle3 << 8) + (0 << 16) + (255 << 16) +0;
                DWORD Shift2 = BaseColor + (Colorcycle3 << 8) + (0) + (255 << 16) +Colorcycle2;

                // top
                pVertex[0].position.y = StartHeight;
                pVertex[0].position.x = TransformedIRSensorsR[ix].DisplayPoint.x;
                pVertex[0].position.z = TransformedIRSensorsR[ix].DisplayPoint.y;
                pVertex[0].color = Shift1; 
                
                pVertex[1].position.y = StartHeight+YDistance;
                pVertex[1].position.x = PlusAngle.x;
                pVertex[1].position.z = PlusAngle.y;
                pVertex[1].color = Shift2;
                
                pVertex[2].position.y = StartHeight+YDistance;
                pVertex[2].position.x = MinusAngle.x;
                pVertex[2].position.z = MinusAngle.y;
                pVertex[2].color = Shift2;

                pVertex +=3;
                VertexCount+=1;

                // bottom
                pVertex[0].position.y = StartHeight;
                pVertex[0].position.x = TransformedIRSensorsR[ix].DisplayPoint.x;
                pVertex[0].position.z = TransformedIRSensorsR[ix].DisplayPoint.y;
                pVertex[0].color = Shift1; 
                
                pVertex[1].position.y = StartHeight-YDistance;
                pVertex[1].position.x = PlusAngle.x;
                pVertex[1].position.z = PlusAngle.y;
                pVertex[1].color = Shift2;
                
                pVertex[2].position.y = StartHeight-YDistance;
                pVertex[2].position.x = MinusAngle.x;
                pVertex[2].position.z = MinusAngle.y;
                pVertex[2].color = Shift2;

                pVertex +=3;
                VertexCount+=1;

                // left
                pVertex[0].position.y = StartHeight;
                pVertex[0].position.x = TransformedIRSensorsR[ix].DisplayPoint.x;
                pVertex[0].position.z = TransformedIRSensorsR[ix].DisplayPoint.y;
                pVertex[0].color = Shift1; 
                
                pVertex[1].position.y = StartHeight-YDistance;
                pVertex[1].position.x = MinusAngle.x;
                pVertex[1].position.z = MinusAngle.y;
                pVertex[1].color = Shift2;
                
                pVertex[2].position.y = StartHeight+YDistance;
                pVertex[2].position.x = MinusAngle.x;
                pVertex[2].position.z = MinusAngle.y;
                pVertex[2].color = Shift2;

                pVertex +=3;
                VertexCount+=1;

                // right
                pVertex[0].position.y = StartHeight;
                pVertex[0].position.x = TransformedIRSensorsR[ix].DisplayPoint.x;
                pVertex[0].position.z = TransformedIRSensorsR[ix].DisplayPoint.y;
                pVertex[0].color = Shift1; 
                
                pVertex[1].position.y = StartHeight-YDistance;
                pVertex[1].position.x = PlusAngle.x;
                pVertex[1].position.z = PlusAngle.y;
                pVertex[1].color = Shift2;
                
                pVertex[2].position.y = StartHeight+YDistance;
                pVertex[2].position.x = PlusAngle.x;
                pVertex[2].position.z = PlusAngle.y;
                pVertex[2].color = Shift2;

                pVertex +=3;
                VertexCount+=1;

                // next form the box at the end of the beam (two triangles
                pVertex[0].position.y = StartHeight+YDistance;
                pVertex[0].position.x = PlusAngle.x;
                pVertex[0].position.z = PlusAngle.y;
                pVertex[0].color = Shift2; 
                
                pVertex[1].position.y = StartHeight-YDistance;
                pVertex[1].position.x = PlusAngle.x;
                pVertex[1].position.z = PlusAngle.y;
                pVertex[1].color = Shift2;
                
                pVertex[2].position.y = StartHeight+YDistance;
                pVertex[2].position.x = MinusAngle.x;
                pVertex[2].position.z = MinusAngle.y;
                pVertex[2].color = Shift2;

                pVertex +=3;
                VertexCount+=1;

                // 2cd box triangle
                pVertex[0].position.y = StartHeight+YDistance;
                pVertex[0].position.x = MinusAngle.x;
                pVertex[0].position.z = MinusAngle.y;
                pVertex[0].color = Shift2; 
                
                pVertex[1].position.y = StartHeight-YDistance;
                pVertex[1].position.x = MinusAngle.x;
                pVertex[1].position.z = MinusAngle.y;
                pVertex[1].color = Shift2;
                
                pVertex[2].position.y = StartHeight-YDistance;
                pVertex[2].position.x = PlusAngle.x;
                pVertex[2].position.z = PlusAngle.y;
                pVertex[2].color = Shift2;

                pVertex +=3;
                VertexCount+=1;


                
            }
      }
      Colorcycle += 1*Sign*2; // used for cycling the collors;
      Colorcycle2+= 1*Sign2;
      Colorcycle3+= 1*Sign3*3;

      if (Colorcycle >= cyclemax-100)
          Sign = -1;
      else if (Colorcycle <= 128)
          Sign = 1;
      if (Colorcycle2 >= cyclemax-30)
          Sign2 = -1;
      else if (Colorcycle2 <= 65)
          Sign2 = 1;
      if (Colorcycle3 >= cyclemax-60)
          Sign3 = -1;
      else if (Colorcycle3 <= 34)
          Sign3 = 1;

 

}

/************************************************************************
* Function unsigned char GetRFowardIR()
* Robot API
* PURPOSE
* To return the distance of the Front Right IR
*************************************************************************/
unsigned char SIRSensor::GetRFowardIR()
{
     return (unsigned char)IRSensors[RFOWARDIR].DetectionDistance;
}

/************************************************************************
* Function unsigned char GetLRearIR()
* Robot API
* PURPOSE
* To return the distance of the Front Right IR
*************************************************************************/
unsigned char SIRSensor::GetLRearIR()
{
     return (unsigned char)IRSensors[LEFTREAR].DetectionDistance;
}

/************************************************************************
* Function unsigned char GetRRearIR()
* Robot API
* PURPOSE
* To return the distance of the Front Right IR
*************************************************************************/
unsigned char SIRSensor::GetRRearIR()
{
     return (unsigned char)IRSensors[RIGHTREAR].DetectionDistance;
}


/************************************************************************
* Function unsigned char GetLeftIR()
* Robot API
* PURPOSE
* To return the distance of the Left IR
*************************************************************************/
unsigned char SIRSensor::GetLeftIR()
{
     return (unsigned char)IRSensors[LEFTIR].DetectionDistance;
}

/************************************************************************
* Function unsigned char GetLFowardIR()
* Robot API
* PURPOSE
* To return the distance of the Left Foward IR
*************************************************************************/
unsigned char SIRSensor::GetLFowardIR()
{
     return (unsigned char)IRSensors[LEFTFOWARD].DetectionDistance;
}
