File : sonar_handler.adb

--                              -*- Mode: Ada -*-
-- Filename        : sonar_handler.ads
-- Description     : synchronization for the sonar readings
-- Author          : Christfried Webers
-- Created On      : Mon Nov  8 20:47:28 1999
-- Last Modified By: .
-- Last Modified On: .
-- Update Count    : 0
-- Status          : Experimental
-------------------------------------------------------------------
with Text_IO; use Text_IO;
with Ada.Integer_Text_IO; use Ada.Integer_Text_IO;

with Ada.Numerics.Elementary_Functions; use Ada.Numerics.Elementary_Functions;
with Metrics; use Metrics;

package body Sonar_Handler is

   InvalidSonarData : constant Raw_16Bit := 16#7FFE# ;
   Scale            : constant Float     := 16#05A5.0# / 1000.0 ;

   AllValuesNew : SonarFlags := (others => True);
   AllValuesOld : SonarFlags := (others => False);

   type Orientation is record
      X, Y : Mm;
      Angle : Degrees;
   end record;

   Orientations : array (Sonar.Index) of Orientation :=
     (
      (X => -305, Y =>  150, Angle => 180.0),
      (X => -270, Y =>  180, Angle =>  90.0),
      (X =>   70, Y =>  180, Angle =>  90.0),
      (X =>  110, Y =>  175, Angle =>  75.0),
      (X =>  165, Y =>  175, Angle =>  60.0),
      (X =>  220, Y =>  170, Angle =>  45.0),
      (X =>  290, Y =>  110, Angle =>  30.0),
      (X =>  295, Y =>   55, Angle =>  15.0),
      (X =>  300, Y =>    0, Angle =>   0.0),
      (X =>  295, Y =>  -55, Angle => -15.0),
      (X =>  290, Y => -110, Angle => -30.0),
      (X =>  220, Y => -170, Angle => -45.0),
      (X =>  165, Y => -175, Angle => -60.0),
      (X =>  110, Y => -175, Angle => -75.0),
      (X =>   70, Y => -180, Angle => -90.0),
      (X => -270, Y => -180, Angle => -90.0),
      (X => -305, Y => -150, Angle =>-180.0)
      );

   function ClusterIndexToSonarIndex (ClusterIndex: Raw_8Bit)
     return Sonar.Index is

      First : Integer := Sonar.Index'Pos(Sonar.Index'First);
   begin
      case ClusterIndex is
         when 16#00# => return Sonar.Index'Val(First);
         when 16#01# => return Sonar.Index'Val(First + 1);
         when 16#02# => return Sonar.Index'Val(First + 2);
         when 16#03# => return Sonar.Index'Val(First + 3);
         when 16#04# => return Sonar.Index'Val(First + 4);
         when 16#05# => return Sonar.Index'Val(First + 5);

         when 16#10# => return Sonar.Index'Val(First + 6);
         when 16#11# => return Sonar.Index'Val(First + 7);
         when 16#12# => return Sonar.Index'Val(First + 8);
         when 16#13# => return Sonar.Index'Val(First + 9);
         when 16#14# => return Sonar.Index'Val(First + 10);

         when 16#20# => return Sonar.Index'Val(First + 16);
         when 16#21# => return Sonar.Index'Val(First + 15);
         when 16#22# => return Sonar.Index'Val(First + 14);
         when 16#23# => return Sonar.Index'Val(First + 13);
         when 16#24# => return Sonar.Index'Val(First + 12);
         when 16#25# => return Sonar.Index'Val(First + 11);
         when others =>
            Put("Wrong ClusterIndex in packet!");
            return Sonar.Index'First;
      end case;
   end ClusterIndexToSonarIndex;


   protected body Reader is
      procedure RecordPacket(PacketNumber: Raw_8Bit; Data: ByteArray) is
         -- Data = 00 00 00 02
         --        <4 Byte TimeStamp>
         --        <1 Byte Cluster> <2 Byte Data>
         --        <1 Byte Cluster> <2 Byte Data>
         --        <1 Byte Cluster> <2 Byte Data>
         -- sometimes more clusters

         DataIndex            : Integer;
         ClusterIndex         : Raw_8Bit;
         SonarIndex           : Sonar.Index;
         RawData              : Raw_16Bit;
         DistanceFromSensor   : Float;
         SensorAngle          : Float;
         Xposition, Yposition : Float;
      begin
         DataIndex := Integer(Data'First+8);
         while DataIndex <= Data'Last-2 loop
            ClusterIndex := Data(DataIndex);
            SonarIndex := ClusterIndexToSonarIndex(ClusterIndex);
            RawData :=  TwoBytes_To_Raw_16Bit(Data(DataIndex+1..DataIndex+2));
            if RawData = InvalidSonarData then
               NewReadings(SonarIndex).Valid := False;
            else
               NewReadings(SonarIndex).Valid := True;
               DistanceFromSensor := Float(RawData) / Scale;
               SensorAngle := Float(Orientations(SonarIndex).Angle);
               Xposition := Float(Orientations(SonarIndex).X) +
                 DistanceFromSensor * Cos(SensorAngle, 360.0);
               Yposition := Float(Orientations(SonarIndex).Y) +
                 DistanceFromSensor * Sin(SensorAngle, 360.0);
               NewReadings(SonarIndex).Distance :=
                 Mm(Sqrt(Xposition*Xposition + Yposition * Yposition ));
               NewReadings(SonarIndex).Angle :=
                 Degrees(Arctan(Yposition, Xposition, 360.0));
            end if;

            NewValueFlag(SonarIndex) := True;
            DataIndex := DataIndex + 3;
         end loop;

         if NewValueFlag = AllValuesNew then
            LastStatus.Sonar := NewReadings;
            LastStatus.TimeStamp := Ada.Real_Time.Clock;
            NewDataArrived := True;
            FirstDataArrived := True;
            NewValueFlag := AllValuesOld;
         end if;
      end RecordPacket;


      entry GetCurrentStatus(SonarStatus: out Sonar.Status) when FirstDataArrived is
      begin
         SonarStatus := LastStatus;
      end GetCurrentStatus;


      entry WaitForNewData when NewDataArrived or NonBlockingActive is
      begin
         if WaitForNewData'Count = 0 then
            NewDataArrived := False;
         end if;
      end WaitForNewData;

      procedure NonBlocking is
      begin
         NonBlockingActive := True;
      end NonBlocking;

   end Reader;


   protected body Commander is

      procedure StartReading is
      begin
         Flex_Driver.WriteTask.SendPacket(ByteArray(SonarStartSendingPacket),
                                          SonarStartStopSending);
      end StartReading;

      procedure StopReading is
      begin
         Flex_Driver.WriteTask.SendPacket(ByteArray(SonarStopSendingPacket),
                                          SonarStartStopSending);
      end StopReading;

   end Commander;

end Sonar_Handler;