---------------------------------------------------------------------------
--
--                              -*- Mode: Ada -*-
-- Filename        : laser_driver.adb
-- Description     : low level routines, including interrupt handler
--
----------------------------------------------------------------------------

----------------------------------------------------------------------------
--
-- Imports
--
----------------------------------------------------------------------------

with Ada.Real_Time;                     use Ada.Real_Time;
with Ada.Interrupts;                    use Ada.Interrupts;
with Ada.Task_Identification;           use Ada.Task_Identification;
with Ada.Exceptions;                    use Ada.Exceptions;
with Interfaces.C;
with System.OS_Interface;               use System.OS_Interface;
with Text_IO;                           use Text_IO;

with Metrics;             use Metrics;
with Interrupt_Handler;   use Interrupt_Handler;

----------------------------------------------------------------------------
--
-- Laser_driver
--
----------------------------------------------------------------------------

package body Laser_driver is

   package Int_Io is new Integer_Io (Integer); use Int_Io;
   package LongInt_Io is new Integer_Io (Long_Integer); use LongInt_Io;
   package Flo_Io is new Float_Io (Float); use Flo_Io;

----------------------------------------------------------------------------
--
-- Imports from os_connection
--
----------------------------------------------------------------------------

   function OpenSerialPort (PortName: Interfaces.C.char_array)
     return Interfaces.C.int;
   pragma Import (C, OpenSerialPort, "open_serial_port");

   procedure CloseSerialPort (FileD: Interfaces.C.int);
   pragma Import (C, CloseSerialPort, "close_serial_port");

   procedure SetupSerialPort (FileD: Interfaces.C.int;
                              PortSpeed: Interfaces.C.unsigned);
   pragma Import (C, SetupSerialPort, "setup_serial_port");

   function ReadSerialPort (FileD: Interfaces.C.int;
                            Buffer: Interfaces.C.char_array) return int;
   pragma Import (C, ReadSerialPort, "read_serial_port");

   procedure WriteSerialPort (FileD: Interfaces.C.int;
                              Buffer: Interfaces.C.char_array;
                              BytesToWrite: Interfaces.C.int);
   pragma Import (C, WriteSerialPort, "write_serial_port");

   procedure EnableSerialPortInterrupt (FileD: Interfaces.C.int);
   pragma Import
     (C, EnableSerialPortInterrupt, "enable_serial_port_interrupt");

----------------------------------------------------------------------------
--
-- Global types
--
----------------------------------------------------------------------------

   MaxTelegramLength : constant Integer := 1024;

   type Raw_8Bit     is mod 2**8;
   type Raw_16Bit    is mod 2**16;
   type ByteArray    is array (Integer range <>) of Raw_8Bit;
   type Telegram     is new ByteArray (1..MaxTelegramLength);
   type TelegramPart is (STX_ACK_NAK, ADR, LENL, LENH, CMD, ATTR, CRCL, CRCH);

----------------------------------------------------------------------------
--
-- Global constants
--
----------------------------------------------------------------------------

   SerialPortName : constant String  := "/dev/cur2";
   Resetting_Laser: constant Boolean := False;

   Baudrate9600  : constant Interfaces.C.unsigned := 13;
   Baudrate19200 : constant Interfaces.C.unsigned := 14;
   Baudrate38400 : constant Interfaces.C.unsigned := 15;

   WatchDogTimeOut          : constant Duration := 1.0; -- seconds
   ReadTaskWaitingSleepTime : constant Duration := 0.1; -- seconds

   NoOfStoredTelegrams          : constant Integer := 50;
   TelegramFrameLength          : constant Integer := 6;
   TelegramHeaderLength         : constant Integer := 4;
   TelegramCommandLength        : constant Integer := 1;

   DoNotAppendNull              : constant Boolean := False;
   CharArrayIsNotNullTerminated : constant Boolean := False;

   Byte_Offset   : constant Raw_16Bit := 2**8;
   NullByteArray : constant ByteArray (0..1) := (0,1);

   STX_Offset  : constant Integer := 0;
   ADR_Offset  : constant Integer := 1;
   LENL_Offset : constant Integer := 2;
   LENH_Offset : constant Integer := 3;
   CMD_Offset  : constant Integer := 4;
   CRCL_Offset : constant Integer := 5;
   CRCH_Offset : constant Integer := 6;

   NUL_Byte     : constant Raw_8Bit := 16#00#;
   STX_Byte     : constant Raw_8Bit := 16#02#;
   ACK_Byte     : constant Raw_8Bit := 16#06#;
   DLE_Byte     : constant Raw_8Bit := 16#10#;
   NAK_Byte     : constant Raw_8Bit := 16#15#;
   ADR_Byte     : constant Raw_8Bit := 16#80#;
   LMS_ADR_Byte : constant Raw_8Bit := 16#00#;

   INIT_TGM_Cmd  : constant Raw_8Bit := 16#10#;
   BM_Cmd        : constant Raw_8Bit := 16#20#;
   MWANF_Cmd     : constant Raw_8Bit := 16#30#;
   SSANF_Cmd     : constant Raw_8Bit := 16#31#;
   ERRANF_Cmd    : constant Raw_8Bit := 16#32#;
   MMWANF_Cmd    : constant Raw_8Bit := 16#36#;
   MWPANF_Cmd    : constant Raw_8Bit := 16#37#;
   TYPANF_Cmd    : constant Raw_8Bit := 16#3A#;
   MMWPANF_Cmd   : constant Raw_8Bit := 16#3F#;
   BRPERMDEF_Cmd : constant Raw_8Bit := 16#66#;
   LASER_Cmd     : constant Raw_8Bit := 16#68#;

   PWON_Tgm      : constant Raw_8Bit := 16#90#;
   INIT_ACK_Tgm  : constant Raw_8Bit := 16#91#;
   NACK_Tgm      : constant Raw_8Bit := 16#92#;
   BMACK_Tgm     : constant Raw_8Bit := 16#A0#;
   MW_Tgm        : constant Raw_8Bit := 16#B0#;
   ERR_Tgm       : constant Raw_8Bit := 16#B2#;

   BM_Cmd_Installation  : constant Raw_8Bit := 16#00#;
   BM_Cmd_Calibration   : constant Raw_8Bit := 16#01#;
   BM_Cmd_ContMeasure   : constant Raw_8Bit := 16#24#;
   BM_Cmd_38400         : constant Raw_8Bit := 16#40#;
   BM_Cmd_9600          : constant Raw_8Bit := 16#42#;

   Scan_Tgm_LENL : constant Raw_8Bit := 16#D6#;
   Scan_Tgm_LENH : constant Raw_8Bit := 16#02#;

----------------------------------------------------------------------------
--
-- Global variables
--
----------------------------------------------------------------------------

   FileD : Interfaces.C.Int;

   LaserDriverInitialized : Boolean := False;
   InterruptArrived       : Boolean := False;

   RecentTelegrams            : array (1..NoOfStoredTelegrams) of Telegram;
   RecentTelegramsComplete    : array (1..NoOfStoredTelegrams) of Boolean;
   RecentTelegramsTimeStamp   : array (1..NoOfStoredTelegrams) of Time;
   RecentTelegramsLength      : array (1..NoOfStoredTelegrams) of Integer;
   MostRecentCompleteTelegram : Integer range 1..NoOfStoredTelegrams;
   MostRecentCompleteScan     : Integer range 1..NoOfStoredTelegrams;
   OneOrMoreTelegramsComplete : Boolean                              := False;
   OneOrMoreScansComplete     : Boolean                              := False;
   CurrentTelegram            : Integer range 1..NoOfStoredTelegrams := 1;
   CurrentTelegramByte        : Integer range 1..MaxTelegramLength   := 1;
   CurrentTelegramPart        : TelegramPart
     := STX_ACK_NAK;
   CurrentTelegramLength      : Natural := 0;

   ParseForTelegramsOnly      : Boolean := False;
   ParseForScanTelegramOnly   : Boolean := False;

----------------------------------------------------------------------------
--
-- Utility routines
--
----------------------------------------------------------------------------

   function ByteArrayToString
     (Data: in ByteArray; NoOfBytes: in Integer) return String is

      Index      : Integer := 0;
      DataString : String (1 .. NoOfBytes);

   begin

--      Put ("Command: ");

      for Index in 1..NoOfBytes loop
         DataString (Index) :=
           Character'Val (Character'Pos (Character'First)
                          + Data (Data'First+Index-1));

--         Put (Integer (Data (Data'First+Index-1)), 4);

      end loop;
--      New_Line;
      return (DataString);
   end ByteArrayToString;


   function StringToByteArray (Data: String) return ByteArray is

      Index     : Integer := 0;
      DataArray : ByteArray (1..Data'Length);

   begin
      for Index in Data'Range loop
         DataArray (Index) := Character'Pos (Data (Index));
      end loop;
      return (DataArray);
   end StringToByteArray;


   function CRC_16
     (Data: in ByteArray; NoOfBytes: in Integer) return Raw_16Bit is

      Generator_Polynom: constant Raw_16Bit := 2**15 + 2**2 + 2**0;
      Highest_Bit: constant Raw_16Bit := 2**15;
      Byte_Offset: constant Raw_16Bit := 2**8;

      CRC_16: Raw_16Bit := 0;
      Lower, Higher: Raw_8Bit :=0;
      Index: Integer := 0;

   begin
      for Index in 1..NoOfBytes loop

         Higher := Lower;
         Lower := Data (Data'First+Index-1);

         if CRC_16 >= Highest_Bit then
            CRC_16 := (2 * (CRC_16 - Highest_Bit)) xor Generator_polynom;
         else
            CRC_16 := 2 * CRC_16;
         end if;

         CRC_16 := CRC_16 xor
           (Raw_16Bit (Lower) + Byte_Offset * Raw_16Bit (Higher));

      end loop;

      return (CRC_16);
   end CRC_16;


   procedure TelegramToLaserScan
     (ScanTelegram: in Telegram; LaserScan: out LaserStatus) is

      LaserIndex: Positive range FirstLaserReading..LastLaserReading
        := FirstLaserReading;
      ByteIndex: Integer;

      ValueMask : constant Raw_16Bit := 2#1111111111111000#;
      DazzleBit : constant Raw_16Bit := 2#0000000000000100#;
      TelegramElement : Raw_16Bit;

   begin
      ByteIndex := 4 + CMD_Offset;
      for LaserIndex in LaserReadingRange loop

         TelegramElement :=
            Raw_16Bit (ScanTelegram (ByteIndex))
            + (Byte_Offset
               *  Raw_16Bit (ScanTelegram (ByteIndex + 1)));

         LaserScan.Readings (LaserIndex).Distance :=
           mm (TelegramElement and ValueMask);

         LaserScan.Readings (LaserIndex).Dazzled :=
           (TelegramElement and DazzleBit) = 1;

         ByteIndex := ByteIndex + 2;

        end loop;
   end TelegramToLaserScan;


   procedure CopyMostRecentLaserScan (LaserScan: out LaserStatus) is

   begin
      TelegramToLaserScan
        (RecentTelegrams (MostRecentCompleteScan), LaserScan);
      LaserScan.TimeStamp :=
        RecentTelegramsTimeStamp (MostRecentCompleteScan);
   end CopyMostRecentLaserScan;


   procedure CopyAllAvailableLaserScans (LaserScans: out LaserStatusArray) is

      NoOfLaserScans : Natural := 0;
      TelegramIndex  : Integer := 0;
      ScanIndex      : Natural := 1;

   begin
      for TelegramIndex in 1..NoOfStoredTelegrams loop
         if RecentTelegramsComplete (TelegramIndex)
           and then RecentTelegrams (TelegramIndex) (5) = MW_Tgm then
            NoOfLaserScans := NoOfLaserScans + 1;
         end if;
      end loop;

      declare LocalLaserScans: LaserStatusArray (1..NoOfLaserScans); begin

         for TelegramIndex in 1..NoOfStoredTelegrams loop
            if RecentTelegramsComplete (TelegramIndex)
              and then RecentTelegrams (TelegramIndex) (5) = MW_Tgm then
               TelegramToLaserScan
                 (RecentTelegrams (TelegramIndex),
                  LocalLaserScans (ScanIndex));
               ScanIndex := ScanIndex + 1;
            end if;
         end loop;

         LaserScans := LocalLaserScans;
      end;
   end CopyAllAvailableLaserScans;


   procedure Send_Laser_Telegram
     (Cmd: in Raw_8Bit;
      Attr: in ByteArray := NullByteArray;
      NoOfAttrBytes: in Integer := 0) is

      Telegram      : ByteArray (1..1 + TelegramFrameLength + NoOfAttrBytes);
      TelegramLength, Index: Integer := 0;
      CRC: Raw_16Bit;

   begin
      Telegram (1+STX_Offset) := STX_Byte;
      Telegram (1+ADR_Offset) := LMS_ADR_Byte;
      Telegram (1+LENL_Offset) := Raw_8Bit ((1+NoOfAttrBytes) mod 256);
      Telegram (1+LENH_Offset) := Raw_8Bit ((1+NoOfAttrBytes) /   256);
      Telegram (1+CMD_Offset) := Cmd;
      if NoOfAttrBytes > 0 then
         for Index in 1.. NoOfAttrBytes loop
           Telegram (1+CMD_Offset+Index) := Attr (Attr'First+Index-1);
         end loop;
      end if;
      CRC := CRC_16 (Telegram, 1 + CMD_Offset + NoOfAttrBytes);
      Telegram (1+CRCL_Offset+NoOfAttrBytes) := Raw_8Bit (CRC mod 256);
      Telegram (1+CRCH_Offset+NoOfAttrBytes) := Raw_8Bit (CRC /   256);

       WriteSerialPort
         (FileD,
          Interfaces.C.To_C (ByteArrayToString (Telegram,
                                                1 + TelegramFrameLength
                                                + NoOfAttrBytes),
                             DoNotAppendNull),
          Interfaces.C.int (1 + TelegramFrameLength + NoOfAttrBytes));

   end Send_Laser_Telegram;


   procedure SetupLaser is

      MaxAttr : constant Integer := 24;

      LaserReactionTime : constant Duration := 0.01; -- seconds
      Attr : ByteArray (1..MaxAttr);

      LastCompleteTelegram : Integer range 1..NoOfStoredTelegrams := 1;
      TimeStamp : Time;

      function MostRecentResponse return Raw_8Bit is

         TelegramStartByte : Raw_8Bit;

      begin
         if OneOrMoreTelegramsComplete then
            TelegramStartByte :=
              RecentTelegrams (MostRecentCompleteTelegram) (1);
            if TelegramStartByte = ACK_Byte
              or else TelegramStartByte = NAK_Byte
            then
               return (TelegramStartByte);
            elsif TelegramStartByte = STX_Byte then
               return (RecentTelegrams (MostRecentCompleteTelegram) (5));
            else
               return (NUL_Byte);
            end if;
         else
            return (NUL_Byte);
         end if;
      end MostRecentResponse;


   function ResponseTelegramReceived
        (TelegramCmd: Raw_8Bit) return Boolean is

         Trials : Integer := 1;

      begin
         while Trials <= 10
           and then ((LastCompleteTelegram = MostRecentCompleteTelegram)
                     or else MostRecentResponse /= TelegramCmd) loop
            delay (LaserReactionTime);
            Trials := Trials + 1;
         end loop;
         LastCompleteTelegram := MostRecentCompleteTelegram;
         return (MostRecentResponse = TelegramCmd);
      end ResponseTelegramReceived;

   begin

--      Put_Line ("setting up laser start");

      if Resetting_Laser then
        PowerUp_Loop:
         loop
           Reset_Loop:
            loop
               SetupSerialPort (FileD, Baudrate9600);

--               Put_Line ("send INIT 9600 ...");

               Send_Laser_Telegram (INIT_TGM_Cmd);
               exit Reset_Loop when ResponseTelegramReceived (INIT_ACK_Tgm);

               SetupSerialPort (FileD, Baudrate38400);

--               Put_Line ("send INIT 38400 ...");

               Send_Laser_Telegram (INIT_TGM_Cmd);
               exit Reset_Loop when ResponseTelegramReceived (INIT_ACK_Tgm);
            end loop Reset_Loop;

            SetupSerialPort (FileD, Baudrate9600);

            TimeStamp := Clock;

            while (Clock - TimeStamp) < To_Time_Span (Duration (10.0))
              and then not ResponseTelegramReceived (PWON_Tgm) loop null;
            end loop;

            exit PowerUp_Loop
            when (Clock - TimeStamp) < To_Time_Span (Duration (10.0));

         end loop PowerUp_Loop;
      end if;

      Attr (1) := BM_Cmd_Installation;
      Attr (2) := Character'Pos ('S');
      Attr (3) := Character'Pos ('I');
      Attr (4) := Character'Pos ('C');
      Attr (5) := Character'Pos ('K');
      Attr (6) := Character'Pos ('_');
      Attr (7) := Character'Pos ('L');
      Attr (8) := Character'Pos ('M');
      Attr (9) := Character'Pos ('S');

     Init_Loop:
      loop
         SetupSerialPort (FileD, Baudrate9600);
         Send_Laser_Telegram (BM_Cmd, Attr, 9);
         if ResponseTelegramReceived (BMACK_Tgm) then
            Attr (1) := BM_Cmd_38400;
            loop
               Send_Laser_Telegram (BM_Cmd, Attr, 1);
               exit when ResponseTelegramReceived (BMACK_Tgm);
            end loop;
            SetupSerialPort (FileD, Baudrate38400);
            exit Init_Loop;
         end if;

         SetupSerialPort (FileD, Baudrate38400);
         Send_Laser_Telegram (BM_Cmd, Attr, 9);
         exit Init_Loop when ResponseTelegramReceived (BMACK_Tgm);
      end loop Init_Loop;

      ParseForTelegramsOnly := True;
      ParseForScanTelegramOnly := True;

      Attr (1) := BM_Cmd_ContMeasure;
      loop
         Send_Laser_Telegram (BM_Cmd, Attr, 1);
         exit when ResponseTelegramReceived (BMACK_Tgm);
      end loop;

   end SetupLaser;


   procedure ProcessLaserData (NewData: in ByteArray; NewNoOfBytes: Integer) is

      NewDataIndex      : Integer := 1;
      PrintIndex        : Integer := 1;
      ExpectedByteFound : Boolean;
      TelegramComplete  : Boolean := False;

   begin
      while NewDataIndex <= NewNoOfBytes loop
         ExpectedByteFound := False;

         case CurrentTelegramPart is
            when STX_ACK_NAK => begin
               while (NewDataIndex <= NewNoOfBytes)
                 and then (NewData (NewDataIndex) /= STX_Byte)
                 and then (ParseForTelegramsOnly
                           or else
                 ((NewData (NewDataIndex) /= ACK_Byte)
                  and then (NewData (NewDataIndex) /= NAK_Byte)))
               loop
                  NewDataIndex := NewDataIndex + 1;
               end loop;
               if NewDataIndex <= NewNoOfBytes then
                  RecentTelegramsTimeStamp (CurrentTelegram) := Clock;
                  ExpectedByteFound := True;
                  if NewData (NewDataIndex) = STX_Byte then
                     CurrentTelegramPart :=
                       TelegramPart'Succ(CurrentTelegramPart);
                  else
                     TelegramComplete := True;
                  end if;
               end if;
            end;
            when ADR => begin
               if NewData (NewDataIndex) = ADR_Byte then
                  ExpectedByteFound := True;
                  CurrentTelegramPart :=
                    TelegramPart'Succ(CurrentTelegramPart);
               end if;
            end;
            when LENL => begin
               if not ParseForScanTelegramOnly
                 or else NewData (NewDataIndex) = Scan_Tgm_LENL then
                  ExpectedByteFound := True;
                  CurrentTelegramPart :=
                    TelegramPart'Succ(CurrentTelegramPart);
               end if;
            end;
            when LENH => begin
               CurrentTelegramLength :=
                 Natural (RecentTelegrams (CurrentTelegram)
                          (CurrentTelegramByte-1))
                 + (Natural (Byte_Offset)
                    * Natural (NewData (NewDataIndex)));
               if (CurrentTelegramLength
                   <= MaxTelegramLength - TelegramFrameLength)
                 and then (not ParseForScanTelegramOnly
                           or else NewData (NewDataIndex) = Scan_Tgm_LENH)
               then
                  ExpectedByteFound := True;
                  CurrentTelegramPart :=
                    TelegramPart'Succ(CurrentTelegramPart);
               else
                  CurrentTelegramLength := 0;
               end if;
            end;
            when CMD => begin
               ExpectedByteFound := True;
               if CurrentTelegramLength > TelegramCommandLength then
                  CurrentTelegramPart := ATTR;
               else
                  CurrentTelegramPart := CRCL;
               end if;
            end;
            when ATTR => begin
               ExpectedByteFound := True;
               if CurrentTelegramByte
                 = CurrentTelegramLength + TelegramHeaderLength then
                  CurrentTelegramPart := CRCL;
               end if;
            end;
            when CRCL => begin
               ExpectedByteFound := True;
               CurrentTelegramPart :=
                 TelegramPart'Succ(CurrentTelegramPart);
            end;
            when CRCH => begin
               ExpectedByteFound := True;
               TelegramComplete := True;
            end;
         end case;
         if ExpectedByteFound then
            RecentTelegrams (CurrentTelegram) (CurrentTelegramByte)
              := NewData (NewDataIndex);
            CurrentTelegramByte := CurrentTelegramByte + 1;
            NewDataIndex := NewDataIndex + 1;
            if TelegramComplete then
               if CurrentTelegramPart = STX_ACK_NAK or else
                 CRC_16 (ByteArray (RecentTelegrams (CurrentTelegram)),
                         CurrentTelegramLength + TelegramHeaderLength)
                 =
                 Raw_16Bit (RecentTelegrams (CurrentTelegram)
                            (CurrentTelegramLength
                             + TelegramHeaderLength + 1))
                 + (Byte_Offset
                    * Raw_16Bit (RecentTelegrams (CurrentTelegram)
                                 (CurrentTelegramLength
                                  + TelegramHeaderLength + 2)))
               then
                  RecentTelegramsComplete (CurrentTelegram) := True;
                  RecentTelegramsLength   (CurrentTelegram) :=
                    CurrentTelegramByte - 1;
                  MostRecentCompleteTelegram := CurrentTelegram;
                  OneOrMoreTelegramsComplete := True;

                  if RecentTelegrams
                    (MostRecentCompleteTelegram) (5) = MW_Tgm then
                     OneOrMoreScansComplete := True;
                     MostRecentCompleteScan := CurrentTelegram;
                     LaserMonitor.FreeTasks;
--                     Put (".");
                  end if;

--                   Put ("Telegram: ");
--                   for PrintIndex in 1..CurrentTelegramByte - 1 loop
--                      Put (Integer (RecentTelegrams (CurrentTelegram)
--                                    (PrintIndex)), 4);
--                   end loop;
--                   New_Line;

                  if CurrentTelegram < NoOfStoredTelegrams then
                     CurrentTelegram := CurrentTelegram + 1;
                  else
                     CurrentTelegram := 1;
                  end if;
                  RecentTelegramsComplete (CurrentTelegram) := False;

               elsif CurrentTelegramPart /= STX_ACK_NAK then -- wrong CRC

                  Put ("  --->>>> ignoring telegram ");
                  for PrintIndex in 1..5 loop
                     Put (Integer (RecentTelegrams (CurrentTelegram)
                                   (PrintIndex)), 4);
                  end loop;
                  Put (" - (");
                  Put (Integer (CurrentTelegramByte), 4);
                  Put_Line ("bytes), due to wrong CRC");

               end if;

               CurrentTelegramByte := 1;
               CurrentTelegramPart := STX_ACK_NAK;
               TelegramComplete := False;
            end if;
         else
--              Put ("  --->>>> ignoring ");
--              Put (Integer (CurrentTelegramByte), 4);
--              Put_Line ("bytes, due to misexpectation");

            CurrentTelegramByte := 1;
            CurrentTelegramPart := STX_ACK_NAK;
         end if;
      end loop;
   end ProcessLaserData;

----------------------------------------------------------------------------
--
-- LaserReadMonitor
--
----------------------------------------------------------------------------

   protected LaserReadMonitor is

      entry BlockTask;
      entry BlockedTasksQueue;
      procedure IO_Signal_Handler;
--      pragma Attach_Handler (IO_Signal_Handler, SIGIO);
      procedure TriggerRead;
      procedure NonBlocking;

   private

      NewDataArrived     : Boolean := False;
      NonBlockingActive  : Boolean := False;
      TasksWaitingForData: Natural := 0;

   end LaserReadMonitor;

   protected body LaserReadMonitor is

      entry BlockTask when True is

      begin
         TasksWaitingForData := TasksWaitingForData + 1;
         requeue BlockedTasksQueue;
      end BlockTask;


      entry BlockedTasksQueue when NewDataArrived or NonBlockingActive is

      begin

         TasksWaitingForData := TasksWaitingForData - 1;
         if TasksWaitingForData = 0 then
            NewDataArrived := False;
         end if;
      end BlockedTasksQueue;


      procedure IO_Signal_Handler is

      begin

--         Put_Line ("Laser_SIGIO !!!");

         NewDataArrived := True;
         InterruptArrived := True;
      end IO_Signal_Handler;


      procedure TriggerRead is

      begin

         Put_Line ("Laser Serial timeout !!!");

         NewDataArrived := True;
      end TriggerRead;


      procedure NonBlocking is

      begin
         NonBlockingActive := True;
      end NonBlocking;

   end LaserReadMonitor;

----------------------------------------------------------------------------
--
-- SerialWatchDog
--
----------------------------------------------------------------------------

   task SerialWatchDog is

      entry StartWatching;
      entry TerminateWatching;

   end SerialWatchDog;


   task body SerialWatchDog is

      TaskActive    : Boolean := True;

   begin

      accept StartWatching;

      while TaskActive loop

         select
            accept TerminateWatching do
               TaskActive := False;
            end TerminateWatching;
         else
            InterruptArrived := False;
            delay WatchDogTimeOut;
            if not InterruptArrived then
               LaserReadMonitor.TriggerRead;
            end if;
         end select;
      end loop;

      Put_Line (" -> Laser_driver: SerialWatchDog terminated");

   exception
      when E: others =>
         Put_Line (Current_Error,
                   "Task "
                   & Image (Current_Task)
                   & " reports: "
                   & Exception_Name (E));

   end SerialWatchDog;

----------------------------------------------------------------------------
--
-- LaserReadTask
--
----------------------------------------------------------------------------

   task ReadLaser is

      entry StartReading;
      entry StopReading;
      entry TerminateReading;

   end ReadLaser;


   task body ReadLaser is

      MaxBytesPerRead    : constant Integer := 255;
      BytesRead          : Integer;

      DoReading          : Boolean := True;
      TaskActive         : Boolean := True;
      ReadCharArray      : Interfaces.C.char_array
        (Interfaces.C.size_t (1) .. Interfaces.C.size_t (MaxBytesPerRead));
      ReadBuffer         : ByteArray (1..MaxBytesPerRead);

   begin
      accept StartReading;

      while TaskActive loop

         select
            accept StartReading do
               DoReading := True;
            end StartReading;
         or
            accept StopReading do
               DoReading := False;
            end StopReading;
         or
            accept TerminateReading do
               TaskActive := False;
            end TerminateReading;
         else
            if DoReading then
               BytesRead := Integer (ReadSerialPort (FileD, ReadCharArray));
               while BytesRead > 0 loop
                  ProcessLaserData (StringToByteArray
                                    (Interfaces.C.To_Ada
                                     (ReadCharArray,
                                      CharArrayIsNotNullTerminated)),
                                    BytesRead);
                  BytesRead := Integer (ReadSerialPort (FileD, ReadCharArray));
               end loop;
               LaserReadMonitor.BlockTask;
            else
               delay (ReadTaskWaitingSleepTime);
            end if;
         end select;
      end loop;

      Put_Line (" -> ReadLaser terminated");

   exception
      when E: others =>
         Put_Line (Current_Error,
                   "Task "
                   & Image (Current_Task)
                   & " reports: "
                   & Exception_Name (E));

   end ReadLaser;

----------------------------------------------------------------------------
--
-- LaserDriverInterface
--
----------------------------------------------------------------------------

   protected body LaserDriverInterface is

      entry InitLaserDriver when not LaserDriverInitialized is

         Index : Integer := 1;

      begin
         InterruptInterface.InitIntHandler;
         InterruptInterface.AddIntRoutine
           (SignalIO, LaserReadMonitor.IO_Signal_Handler'access);

         FileD := OpenSerialPort (Interfaces.C.To_C (SerialPortName));
         SetupSerialPort (FileD, Baudrate9600);
         EnableSerialPortInterrupt (FileD);

         ReadLaser.StartReading;
         SerialWatchDog.StartWatching;

         for Index in 1..NoOfStoredTelegrams loop
            RecentTelegramsComplete (Index) := False;
         end loop;

         SetupLaser;
         LaserDriverInitialized := True;
      end;


      entry ShutdownLaserDriver when LaserDriverInitialized is

      begin
         InterruptInterface.ShutdownIntHandler;

         LaserReadMonitor.NonBlocking;
         ReadLaser.TerminateReading;
         SerialWatchDog.TerminateWatching;

         CloseSerialPort (FileD);

         LaserDriverInitialized := False;
      end;


      entry SuspendLaserDriver when LaserDriverInitialized is

      begin
         ReadLaser.StopReading;
      end SuspendLaserDriver;


      entry ResumeLaserDriver when LaserDriverInitialized is

      begin
         ReadLaser.StartReading;
      end ResumeLaserDriver;


      entry GetMostRecentLaserScan (LaserScan: out LaserStatus)
      when LaserDriverInitialized is

      begin
         if OneOrMoreScansComplete then
            CopyMostRecentLaserScan (LaserScan);
         end if;
      end GetMostRecentLaserScan;


      entry GetMostRecentLaserTimeStamp (LaserTimeStamp: out Time)
      when LaserDriverInitialized is

      begin
         if OneOrMoreScansComplete then
            LaserTimeStamp :=
              RecentTelegramsTimeStamp (MostRecentCompleteScan);
         end if;
      end GetMostRecentLaserTimeStamp;


      entry GetAllAvailableLaserScans (LaserScans: out LaserStatusArray)
      when LaserDriverInitialized is

      begin
         if OneOrMoreScansComplete then
            CopyAllAvailableLaserScans (LaserScans);
         end if;
      end GetAllAvailableLaserScans;

   end LaserDriverInterface;

----------------------------------------------------------------------------
--
-- Syncronization Monitor
--
----------------------------------------------------------------------------

   protected body LaserMonitor is

      entry BlockTask when True is

      begin
         TasksWaitingForData := TasksWaitingForData + 1;
         requeue BlockedTasksQueue;
      end BlockTask;


      entry BlockedTasksQueue when NewScanArrived is

      begin
         TasksWaitingForData := TasksWaitingForData - 1;
         if TasksWaitingForData = 0 then
            NewScanArrived := False;
         end if;
      end BlockedTasksQueue;


      procedure FreeTasks is

      begin
         NewScanArrived := True;
      end FreeTasks;

   end LaserMonitor;

end Laser_driver;