with Lego;
use Lego;

procedure Test is 

   Left_Wheel  : constant Output_Port := Output_A;  
   Right_Wheel : constant Output_Port := Output_C;  
   Left_Rot    : constant Sensor_Port := Sensor_1;  
   Right_Rot   : constant Sensor_Port := Sensor_3;  
   Light       : constant Sensor_Port := Sensor_2;


   procedure Initialize_Robot is 
   begin
      -- Initialize the Wheels
      Output_Power(
         Output => Left_Wheel, 
         Power  => Power_High);
      Output_Power(
         Output => Right_Wheel, 
         Power  => Power_High);
      -- Initialize the Rotation Sensors
      Config_Sensor(
         Sensor => Left_Rot,       
         Config => Config_Rotation);
      Config_Sensor( 
         Sensor => Right_Rot,      
         Config => Config_Rotation);
      -- Initialize the Light Sensor
      Config_Sensor(
         Sensor => Light,       
         Config => Config_Light);
   end Initialize_Robot;


   procedure Go_Forward(Clicks:Integer) is 
   begin
      Output_Power(Left_Wheel,7);
      Output_Power(Right_Wheel,7);
      Clear_Sensor(Left_Rot);
      Clear_Sensor(Right_Rot);
      Output_On_Reverse(Left_Wheel);
      Output_Reverse(Right_Wheel);
      while (((-1*Get_Sensor_Value(Left_Rot))+(-1*Get_Sensor_Value(Right_Rot)))/2<Clicks) loop
         if (-1*Get_Sensor_Value(Left_Rot)>-1*Get_Sensor_Value(Right_Rot)) then
            Output_Float(Left_Wheel);
            Output_On(Right_Wheel);
         else
            Output_Float(Right_Wheel);
            Output_On(Left_Wheel);
         end if;
      end loop;
      Output_Off(Left_Wheel);
      Output_Off(Right_Wheel);
   end Go_Forward;


   procedure Go_Backward(Clicks:Integer) is 
   begin
      Output_Power(Left_Wheel,7);
      Output_Power(Right_Wheel,7);
      Clear_Sensor(Left_Rot);
      Clear_Sensor(Right_Rot);
      Output_On_Forward(Left_Wheel);
      Output_Forward(Right_Wheel);
      while ((Get_Sensor_Value(Left_Rot)+Get_Sensor_Value(Right_Rot))/2<Clicks) loop
         if (Get_Sensor_Value(Left_Rot)>Get_Sensor_Value(Right_Rot)) then
            Output_Float(Left_Wheel);
            Output_On(Right_Wheel);
         else
            Output_Float(Right_Wheel);
            Output_On(Left_Wheel);
         end if;
      end loop;
      Output_Off(Left_Wheel);
      Output_Off(Right_Wheel);
   end Go_Backward;
   

   procedure Turn_Left(Clicks:Integer) is 
   begin
      Output_Power(Left_Wheel,7);
      Output_Power(Right_Wheel,7);
      
      Clear_Sensor(Left_Rot);
      Clear_Sensor(Right_Rot);
      
      Output_On_Forward(Left_Wheel);
      Output_On_Reverse(Right_Wheel);
      
      while ((Get_Sensor_Value(Left_Rot)+(-1*Get_Sensor_Value(Right_Rot)))<Clicks) loop
         if (Get_Sensor_Value(Left_Rot)>-1*Get_Sensor_Value(Right_Rot)) then
            Output_Float(Left_Wheel);
            Output_On(Right_Wheel);
         else
            Output_Float(Right_Wheel);
            Output_On(Left_Wheel);
         end if;
      end loop;
      
      Output_Off(Left_Wheel);
      Output_Off(Right_Wheel);

   end Turn_Left;

   procedure Turn_Left_180 is 
   begin
      Turn_Left(700); -- YOU MIGHT WANT TO TWEAK THIS NUMBER
   end Turn_Left_180;
   
   procedure Turn_Left_90 is 
   begin
Turn_Left(350); -- YOU MIGHT WANT TO TWEAK THIS NUMBER
   end Turn_Left_90;
   
   procedure Turn_Right(Clicks:Integer) is 
   begin
      Output_Power(Left_Wheel,7);
      Output_Power(Right_Wheel,7);
      
      Clear_Sensor(Left_Rot);
      Clear_Sensor(Right_Rot);
      
      Output_On_Reverse(Left_Wheel);
      Output_On_Forward(Right_Wheel);
      
      while ((-1*Get_Sensor_Value(Left_Rot)+Get_Sensor_Value(Right_Rot))<Clicks) loop
         if (-1*Get_Sensor_Value(Left_Rot)>Get_Sensor_Value(Right_Rot)) then
            Output_Float(Left_Wheel);
            Output_On(Right_Wheel);
         else
            Output_Float(Right_Wheel);
            Output_On(Left_Wheel);
         end if;
      end loop;
      
      Output_Off(Left_Wheel);
      Output_Off(Right_Wheel);

   end Turn_Right;
   
   procedure Turn_Right_180 is 
   begin
      Turn_Right(700); -- YOU MIGHT WANT TO TWEAK THIS NUMBER
   end Turn_Right_180;
   
   procedure Turn_Right_90 is
   begin
      Turn_Right(350); -- YOU MIGHT WANT TO TWEAK THIS NUMBER
   end Turn_Right_90;

begin
   -- THE ENDLESS SQUARE!
   loop
      Initialize_Robot;
      Go_Forward(1000);
      Turn_Right_90;
   end loop;
end;