// 
// File name: Robot.cpp
//
// Discription: Base classes
//
// Programmer: Vladimir P. Stasevich (SV)
//
// History: 
//    Created 27.01.2000 (SV)
//
#include <vcl.h>
#include <math.h>
#pragma hdrstop

#include "robot.h"
#include "main.h"

TRobot robot;

TRobot::TRobot()
{
   width      = ROBOTDEFAULTWIDTH;
   positionX  = ROBOTDEFAULTPOSX;
   positionY  = ROBOTDEFAULTPOSY;
   angle      = ROBOTDEFAULTANGLE;
   sideLength = ROBOTDEFAULTLEN;
   stepPos    = ROBOTDEFAULTSTEPPOS;
   stepAngle  = ROBOTDEFAULTSTEPANG;

   criticalRadius = ROBOTDEFAULTCRITICALRADIUS;

   touchType = NOTOUCH;
}

TRobot::TRobot
(
   double posX, 
   double posY, 
   double angle 
) :
   angle(angle)
{
   width      = ROBOTDEFAULTWIDTH;
   positionX  = posX;
   positionY  = posY;
//   angle      = angle;
   sideLength = ROBOTDEFAULTLEN;
   stepPos    = ROBOTDEFAULTSTEPPOS;
   stepAngle  = ROBOTDEFAULTSTEPANG;

   criticalRadius = ROBOTDEFAULTCRITICALRADIUS;

   touchType = NOTOUCH;
}

void TRobot::Move
(
    TControlMode controlMode,
    TMoveMode    moveMode
)
{
   TTouchType savedTouchType = touchType;
   int setReal = 1;

    // Изменяем движение, если касание
    //
   switch( touchType )
   {
      case LSURFACETOUCH:
         if( controlMode == XYMODE )
         {
            if( moveMode == DOWN )
            {
               moveMode = RIGHT;
               setReal  = 0;
            }
         }
         else
         {
            if( moveMode == DOWN )
            {
               moveMode    = RIGHT;
               controlMode = XYMODE;
               setReal     = 0;
            }
            else if( moveMode == LEFT )
            {
               controlMode = XYMODE;
               setReal     = 0;
            }
         }
         if( moveMode == CLOCKWISE )
             moveMode = NOMOVE;
         break;

      case LFACETTOUCH:
         if( controlMode == XYMODE )
         {
            if( moveMode == DOWN )
            {
               moveMode = RIGHT;
               setReal  = 0;
            }
            else if( moveMode == LEFT )
            {
               moveMode    = UP;
               controlMode = GRIPMODE;
               setReal     = 0;
            }
         }
         else
         {
            if( moveMode == LEFT )
            {
               moveMode = UP;
               setReal  = 0;
            }
            
            if( moveMode == DOWN )
            {
               controlMode = XYMODE;
               moveMode    = RIGHT;
               setReal     = 0;
            }
         }
         break;

      case LONEPOINTTOUCH:
         if( controlMode == XYMODE )
         {
            if( moveMode == LEFT )
            {
               controlMode = GRIPMODE;
               moveMode    = UP;
               setReal     = 0;
            }

            if( moveMode == DOWN )
            {
               controlMode = GRIPMODE;
               setReal     = 0;
            }
         }
         else
         {
            if( moveMode == LEFT )
            {
               moveMode = UP;
               setReal  = 0;
            }
         }

         if( moveMode == CLOCKWISE )
             moveMode = NOMOVE;
         break;

      case LTWOPOINTTOUCH:
         if( controlMode == XYMODE )
         {
            switch( moveMode )
            {
               case RIGHT:
               case CLOCKWISE:
                  moveMode = NOMOVE;
                  break;
                  
               case LEFT:
                  moveMode = UP;
                  setReal  = 0;
                  break;
                  
               case DOWN:
                  Move( controlMode, ANTICLOCKWISE );
//                  Move( controlMode, LEFT );
                  moveMode = DOWN;
                  setReal  = 0;
                  break;
            }
         }
         else
         {
            switch( moveMode )
            {
               case RIGHT:
               case CLOCKWISE:
                  moveMode = NOMOVE;
                  break;
                  
               case LEFT:
                  moveMode = UP;
                  setReal  = 0;
                  break;

               case DOWN:
                  Move( XYMODE, ANTICLOCKWISE );
//                  Move( XYMODE, LEFT );
                  controlMode = XYMODE;
                  moveMode    = DOWN;
                  setReal     = 0;
                  break;
            }
         }
         break;

      case LJAMMINGTOUCH:
         if( moveMode != ANTICLOCKWISE &&
             moveMode != UP               )
            moveMode = NOMOVE;
         break;

      case LFINALTOUCH:
         if( moveMode != UP )
            moveMode = NOMOVE;
         break;

      case RSURFACETOUCH:
         if( controlMode == XYMODE )
         {
            if( moveMode == DOWN )
            {
               moveMode = LEFT;
               setReal  = 0;
            }
         }
         else
         {
            if( moveMode == DOWN )
            {
               moveMode    = LEFT;
               controlMode = XYMODE;
               setReal     = 0;
            }
            else if( moveMode == RIGHT )
            {
               controlMode = XYMODE;
               setReal     = 0;
            }
         }
         if( moveMode == ANTICLOCKWISE )
             moveMode = NOMOVE;
         break;

      case RFACETTOUCH:
         if( controlMode == XYMODE )
         {
            if( moveMode == DOWN )
            {
               moveMode    = LEFT;
               setReal     = 0;
            }
            else if( moveMode == RIGHT )
            {
               moveMode    = UP;
               controlMode = GRIPMODE;
               setReal     = 0;
            }
         }
         else
         {
            if( moveMode == RIGHT )
            {
               moveMode = UP;
               setReal  = 0;
            }
            
            if( moveMode == DOWN )
            {
               controlMode = XYMODE;
               moveMode    = LEFT;
               setReal     = 0;
            }
         }
         break;

      case RONEPOINTTOUCH:
         if( controlMode == XYMODE )
         {
            if( moveMode == RIGHT )
            {
               controlMode = GRIPMODE;
               moveMode    = UP;
               setReal     = 0;
            }

            if( moveMode == DOWN )
            {
               controlMode = GRIPMODE;
               setReal     = 0;
            }
         }
         else
         {
            if( moveMode == RIGHT )
            {
               moveMode = UP;
               setReal  = 0;
            }
         }

         if( moveMode == ANTICLOCKWISE )
             moveMode = NOMOVE;
         break;

      case RTWOPOINTTOUCH:
         if( controlMode == XYMODE )
         {
            switch( moveMode )
            {
               case LEFT:
               case ANTICLOCKWISE:
                  moveMode = NOMOVE;
                  break;
                  
               case RIGHT:
                  moveMode = UP;
                  setReal  = 0;
                  break;
                  
               case DOWN:
                  Move( controlMode, CLOCKWISE );
//                  Move( controlMode, RIGHT );
                  moveMode = DOWN;
                  setReal  = 0;
                  break;
            }
         }
         else
         {
            switch( moveMode )
            {
               case LEFT:
               case ANTICLOCKWISE:
                  moveMode = NOMOVE;
                  break;
                  
               case RIGHT:
                  moveMode = UP;
                  setReal  = 0;
                  break;

               case DOWN:
                  Move( XYMODE, CLOCKWISE );
//                  Move( XYMODE, RIGHT );
                  controlMode = XYMODE;
                  moveMode    = DOWN;
                  setReal     = 0;
                  break;
            }
         }
         break;

      case RJAMMINGTOUCH:
         if( moveMode != CLOCKWISE &&
             moveMode != UP               )
            moveMode = NOMOVE;
         break;

      case RFINALTOUCH:
         if( moveMode != UP )
            moveMode = NOMOVE;
         break;
   }
   
   switch( controlMode )
   {
//      double rightX, rightY;
      
      case XYMODE:

         switch( moveMode )
         {
            case UP:
               positionY -= stepPos;
               break;

            case DOWN:
               positionY += stepPos;
               break;

            case LEFT:
               positionX -= stepPos;
               break;

            case RIGHT:
               positionX += stepPos;
               break;

            case CLOCKWISE:
               if( angle + stepAngle < scene.GetBushFacetAngle()/2 )
                  angle += stepAngle;
               break;

            case ANTICLOCKWISE:
               if( angle - stepAngle > -scene.GetBushFacetAngle()/2 )
                  angle -= stepAngle;
               break;
         }
         break;

      case GRIPMODE:

         switch( moveMode )
         {
            case UP:
               positionX -= stepPos*sin(angle);
               positionY -= stepPos*cos(angle);
               break;

            case DOWN:
//               scene.GetRightBush( rightBushX, rightBushY );
//               scene.GetRightFacet( rightFacetX, rightFacetY );
//               if( //positionY < rightBushY &&
//                   positionX + stepPos*sin(angle) <= rightBushX - width/2  + 2 )
//               {
                  positionX += stepPos*sin(angle);
                  positionY += stepPos*cos(angle);
//               }
//               else
//               {
//                  positionX += stepPos*sin(angle);
//                  positionY += stepPos*cos(angle);
//               }
               break;

            case LEFT:
               positionX -= stepPos*cos(angle);
               positionY += stepPos*sin(angle);
               break;

            case RIGHT:
//               scene.GetRightBush( rightBushX, rightBushY );
//               scene.GetRightFacet( rightFacetX, rightFacetY );
//               if( //positionY < rightBushY &&
//                   positionX + stepPos*cos(angle) <= rightBushX - width/2  + 2 )
//               {
                  positionX += stepPos*cos(angle);
                  positionY -= stepPos*sin(angle);
//               }
//               else
//               {
//                  positionX += stepPos*cos(angle);
//                  positionY -= stepPos*sin(angle);
//               }
               break;

            case CLOCKWISE:
               if( angle + stepAngle < scene.GetBushFacetAngle()/2 )
                  angle += stepAngle;
               break;

            case ANTICLOCKWISE:
               if( angle - stepAngle > -scene.GetBushFacetAngle()/2 )
                  angle -= stepAngle;
               break;
         }
         break;
   }

   GetTouchType( this );

   if( setReal == 1 )
      realTouchType = touchType;
   else if( setReal == 0 )
      realTouchType = savedTouchType;
}

void TRobot::GetLeftUp( double& coordx, double& coordy )
{
   coordx = positionX - width/2*cos(angle) - sideLength*sin(angle);
   coordy = positionY + width/2*sin(angle) - sideLength*cos(angle);
}

void TRobot::GetLeftDown( double& coordx, double& coordy )
{
   coordx = positionX - width/2*cos(angle);
   coordy = positionY + width/2*sin(angle);
}

void TRobot::GetRightUp( double& coordx, double& coordy )
{
   coordx = positionX - width/2*cos(angle) - sideLength*sin(angle) + width*cos(angle);
   coordy = positionY + width/2*sin(angle) - sideLength*cos(angle) - width*sin(angle);
}

void TRobot::GetRightDown( double& coordx, double& coordy )
{
   coordx = positionX + width/2*cos(angle);
   coordy = positionY - width/2*sin(angle);
}
      
TTouchType TRobot::GetTouchType( TRobot *robot )
{
   double leftX,  leftFacetX,  leftBushX,  leftDownX;
   double leftY,  leftFacetY,  leftBushY,  leftDownY;
   double rightX, rightFacetX, rightBushX, rightDownX;
   double rightY, rightFacetY, rightBushY, rightDownY;

   double robotLeftUpX, robotLeftDownX, robotRightUpX, robotRightDownX;
   double robotLeftUpY, robotLeftDownY, robotRightUpY, robotRightDownY;

   double robotCriticalRadius = robot->GetCriticalRadius();

   double bushFacetAngle = scene.GetBushFacetAngle();

//   double sceneCenter = scene.GetWidth();

   robot->GetLeftUp   ( robotLeftUpX,    robotLeftUpY    );
   robot->GetLeftDown ( robotLeftDownX,  robotLeftDownY  );
   robot->GetRightUp  ( robotRightUpX,   robotRightUpY   );
   robot->GetRightDown( robotRightDownX, robotRightDownY );

   scene.GetLeft ( leftX,  leftY  );
   scene.GetRight( rightX, rightY );
   scene.GetLeftFacet ( leftFacetX,  leftFacetY  );
   scene.GetRightFacet( rightFacetX, rightFacetY );
   scene.GetLeftBush ( leftBushX,  leftBushY  );
   scene.GetRightBush( rightBushX, rightBushY );
   scene.GetLeftDown ( leftDownX,  leftDownY  );
   scene.GetRightDown( rightDownX, rightDownY );

     // Проверка на касание поверхности втулки
     //
   if( robotLeftDownX < leftFacetX )
   {
      if( robotLeftDownY + robotCriticalRadius >= leftY )
      {
         robot->SetTouchType( LSURFACETOUCH );
         return LSURFACETOUCH;
      }
   }

   if( robotRightDownX > rightFacetX )
   {
      if( robotRightDownY + robotCriticalRadius >= rightY )
      {
         robot->SetTouchType( RSURFACETOUCH );
         return RSURFACETOUCH;
      }
   }

     // Проверка на касание фаски - LFACETTOUCH
     //

   double deltaY = leftBushY - robotLeftDownY;
   double deltaX = leftBushX - robotLeftDownX;

   if( robotLeftDownX <  leftBushX &&
       robotLeftDownX >= leftFacetX   )
   {
      if( robotLeftDownX - robotCriticalRadius <= 
          leftBushX - deltaY*tan(bushFacetAngle/2) &&
          robotLeftDownY + robotCriticalRadius >=
          leftBushY - deltaX*(1/tan(bushFacetAngle/2)) )
      {
         robot->SetTouchType( LFACETTOUCH );
         return LFACETTOUCH;
      }
   }

   deltaY = rightBushY - robotRightDownY;
   deltaX = robotRightDownX - rightBushX;

   if( robotRightDownX >  rightBushX &&
       robotRightDownX <= rightFacetX   )
   {
      if( robotRightDownX + robotCriticalRadius >= 
          rightBushX - deltaY*tan(bushFacetAngle/2) &&
          robotRightDownY + robotCriticalRadius >=
          rightBushY - deltaX*(1/tan(bushFacetAngle/2)) )
      {
         robot->SetTouchType( RFACETTOUCH );
         return RFACETTOUCH;
      }
   }

   if( robotLeftDownY > leftDownY )
   {
      robot->SetTouchType( LFINALTOUCH );
      return LFINALTOUCH;
   }

   if( robotRightDownY > rightDownY )
   {
      robot->SetTouchType( RFINALTOUCH );
      return RFINALTOUCH;
   }


     // Проверка на LONEPOINTTOUCH, LTWOPOINTTOUCH
     //             RONEPOINTTOUCH, RTWOPOINTTOUCH

   deltaY = robotLeftDownY - leftBushY;
   deltaX = robotLeftDownX - leftBushX;

   if( robotLeftDownX >  leftBushX &&
       robotLeftDownY >= leftBushY    )
   {
      if( leftBushX + robotCriticalRadius >= 
          robotLeftDownX - deltaY*tan(GetAngle()) &&
          leftBushY - robotCriticalRadius <=
          robotLeftDownY - deltaX*(1/tan(GetAngle())))
      {
         if( robotRightDownX + robotCriticalRadius < rightBushX )
         {
            robot->SetTouchType( LONEPOINTTOUCH );
            return LONEPOINTTOUCH;
         }
         else
         {
            double robotAngle = robot->GetAngle();
            
            if( robotAngle > JAMMINGANGLE )
            {
               robot->SetTouchType( LJAMMINGTOUCH );
               return LJAMMINGTOUCH;
            }
            else
            {
               robot->SetTouchType( LTWOPOINTTOUCH );
               return LTWOPOINTTOUCH;
            }
         }
      }
   }

   deltaY = robotRightDownY - rightBushY;
   deltaX = rightBushX - robotRightDownX;

   if( robotRightDownX <  rightBushX &&
       robotRightDownY >= rightBushY    )
   {
      if( rightBushX - robotCriticalRadius <= 
          robotRightDownX - deltaY*tan(GetAngle()) &&
          rightBushY - robotCriticalRadius <=
          robotRightDownY - deltaX*(1/tan(GetAngle())))
      {
         if( robotLeftDownX - robotCriticalRadius > leftBushX )
         {
            robot->SetTouchType( RONEPOINTTOUCH );
            return RONEPOINTTOUCH;
         }
         else
         {
            double robotAngle = robot->GetAngle();
            
            if( -robotAngle > JAMMINGANGLE )
            {
               robot->SetTouchType( RJAMMINGTOUCH );
               return RJAMMINGTOUCH;
            }
            else
            {
               robot->SetTouchType( RTWOPOINTTOUCH );
               return RTWOPOINTTOUCH;
            }
         }
      }
   }

   robot->SetTouchType( NOTOUCH );
   return NOTOUCH;
}

void TRobot::PaintOnCanvas
( 
   TCanvas*   canvas, 
   TPaintMode mode
)
{
   double leftUpX, leftDownX, rightUpX, rightDownX;
   double leftUpY, leftDownY, rightUpY, rightDownY;

   GetLeftUp   ( leftUpX,    leftUpY    );
   GetLeftDown ( leftDownX,  leftDownY  );
   GetRightUp  ( rightUpX,   rightUpY   );
   GetRightDown( rightDownX, rightDownY );

   if( touchType == LSURFACETOUCH ||
       touchType == LFACETTOUCH   || 
       touchType == RJAMMINGTOUCH ||
       touchType == RTWOPOINTTOUCH   )
      canvas->Pen->Color = clRed;
   else
      canvas->Pen->Color = clGreen;
   canvas->Brush->Color = clBtnFace;

   if( mode == ERASE ) canvas->Pen->Color = clBtnFace;
   
   canvas->Ellipse( leftDownX - criticalRadius, leftDownY - criticalRadius,
                    leftDownX + criticalRadius, leftDownY + criticalRadius );
   canvas->Pen->Color   = clWhite;
   canvas->Brush->Color = clWhite;
   
   if( touchType == RSURFACETOUCH ||
       touchType == RFACETTOUCH   || 
       touchType == LJAMMINGTOUCH  ||
       touchType == LTWOPOINTTOUCH    )
      canvas->Pen->Color = clRed;
   else
      canvas->Pen->Color = clGreen;
   canvas->Brush->Color = clBtnFace;

   if( mode == ERASE ) canvas->Pen->Color = clBtnFace;
   
   canvas->Ellipse( rightDownX - criticalRadius, rightDownY - criticalRadius,
                    rightDownX + criticalRadius, rightDownY + criticalRadius );
   canvas->Pen->Color   = clWhite;
   canvas->Brush->Color = clWhite;
   
   canvas->MoveTo( positionX,  positionY  );
   
   if( touchType == LJAMMINGTOUCH ||
       touchType == RJAMMINGTOUCH    )
      canvas->Pen->Color = clRed;
   else if( touchType == LFINALTOUCH ||
            touchType == RFINALTOUCH    )
      canvas->Pen->Color = clGreen;
   else
      canvas->Pen->Color = clWhite;

   if( mode == ERASE ) canvas->Pen->Color = clBtnFace;
   
   canvas->LineTo( leftDownX,  leftDownY  );
   
   if( touchType == LONEPOINTTOUCH ||
       touchType == LTWOPOINTTOUCH ||
       touchType == LJAMMINGTOUCH  ||
       touchType == RJAMMINGTOUCH     )
      canvas->Pen->Color = clRed;
   else if( touchType == LFINALTOUCH ||
            touchType == RFINALTOUCH    )
      canvas->Pen->Color = clGreen;
   else
      canvas->Pen->Color = clWhite;
   canvas->Brush->Color = clBtnFace;

   if( mode == ERASE ) canvas->Pen->Color = clBtnFace;
   
   canvas->LineTo( leftUpX,    leftUpY    );

   if( touchType == LJAMMINGTOUCH ||
       touchType == RJAMMINGTOUCH    )
      canvas->Pen->Color   = clRed;
   else if( touchType == LFINALTOUCH ||
            touchType == RFINALTOUCH    )
      canvas->Pen->Color = clGreen;
   else
      canvas->Pen->Color   = clWhite;

   if( mode == ERASE ) canvas->Pen->Color = clBtnFace;
   
   canvas->LineTo( rightUpX,   rightUpY   );

   if( touchType == RONEPOINTTOUCH ||
       touchType == RTWOPOINTTOUCH ||
       touchType == LJAMMINGTOUCH ||
       touchType == RJAMMINGTOUCH    )
      canvas->Pen->Color   = clRed;
   else if( touchType == LFINALTOUCH ||
            touchType == RFINALTOUCH    )
      canvas->Pen->Color = clGreen;
   else
      canvas->Pen->Color   = clWhite;

   if( mode == ERASE ) canvas->Pen->Color = clBtnFace;
   
   canvas->LineTo( rightDownX, rightDownY );

   if( touchType == LJAMMINGTOUCH ||
       touchType == RJAMMINGTOUCH    )
      canvas->Pen->Color = clRed;
   else if( touchType == LFINALTOUCH ||
            touchType == RFINALTOUCH   )
      canvas->Pen->Color = clGreen;
   else
      canvas->Pen->Color = clWhite;

   if( mode == ERASE ) canvas->Pen->Color = clBtnFace;
   
   canvas->LineTo( positionX,  positionY  );
}

