// Radar version 3 with mutex
#define ANGLE 1000
#define SPEED 60
#define DOTTED_LINES 2

mutex SinCosMutex;
int   Radar[100];
float Position;

long fx(  float Angle, float Dist)
{   return cos( PI*(99-Angle)/100)*Dist+49; }

long fy(  float Angle, float Dist)
{   return sin( PI*(99-Angle)/100)*Dist; }

///////////////////////////////////////////////////////////////////////////
void Initialize_screen( void)
{  int i;

   ClearScreen();
   for (i=0; i<100; i++)
   {   if (Radar[i]>4 && Radar[i]<64)
          CircleOut( fx(i, Radar[i]),
                     fy(i, Radar[i]),
                     3, DRAW_OPT_NORMAL);
       Radar[i] = 0;

       if (i & DOTTED_LINES)
       {  PointOut( fx(i,16.0), fy(i,16.0));
          PointOut( fx(i,32.0), fy(i,32.0));
          PointOut( fx(i,48.0), fy(i,48.0));
       }
   }
}

///////////////////////////////////////////////////////////////////////////
task Turn_radar()
{ int i;
  ResetTachoCount( OUT_A);
  while (true)
  {
      Acquire( SinCosMutex);
      Initialize_screen( );
      Release( SinCosMutex);
      RotateMotor( OUT_A, +SPEED, ANGLE);
      
      Acquire( SinCosMutex);
      Initialize_screen( );
      Release( SinCosMutex);
      RotateMotor( OUT_A, -SPEED, ANGLE);
  }
}

///////////////////////////////////////////////////////////////////////////
task Radar_unit()
{ int Distance;

   SetSensorLowspeed(IN_4);
   while (true)
   {  Distance = SensorUS( IN_4);
      Position = MotorTachoCount(OUT_A)*100/ANGLE;
      
      if (Position<0) Position = 0;
      Position = (Position>99)? 99 :Position;
      
      Radar[ Position] = Distance;
      
      Acquire( SinCosMutex);
      if (Distance>4 && Distance<64)
         CircleOut( fx(Position, Distance),
                    fy(Position, Distance), 2, DRAW_OPT_FILL_SHAPE);
      Release( SinCosMutex);
      
      LineOut(0,0,Position,0);
      LineOut(Position,0,99,0,DRAW_OPT_CLEAR);
      printf("%5d", Distance);
      Wait( 20);
   }
}

///////////////////////////////////////////////////////////////////////////
task main()
{  Precedes(Turn_radar,Radar_unit);
   ClearScreen();
}