----------------------------------------------------------------------------
--
--                              -*- Mode: Ada -*-
--
-- Filename        : Laser.adb
-- Description     : All you need to read the laser range finder on the B14
-- Author          : Uwe R. Zimmer
-- Created On      : Wed Oct 15 16:04:34 1997
-- Last Modified By:
-- Last Modified On:
-- Update Count    :
-- Status          :
--
----------------------------------------------------------------------------

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

with Ada.Real_Time;  use Ada.Real_Time;
with Interfaces.C;   use Interfaces.C;
with Laser_driver;   use Laser_driver;

----------------------------------------------------------------------------
--
-- Package Laser
--
----------------------------------------------------------------------------

package body Laser is

----------------------------------------------------------------------------
--
-- Variables global for package Laser
--
----------------------------------------------------------------------------

   LaserInitialized  : Boolean := False;

   NoOfClients       : Natural := 0;
   NoOfSuspends      : Natural := 0;

----------------------------------------------------------------------------
--
-- Protected Monitor LaserInterface;
--
----------------------------------------------------------------------------

   protected body LaserInterface is


      procedure InitLaser is

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

      begin
         if not LaserInitialized then

            for LaserIndex in LaserReadingRange loop
               ActualLaserAngles (LaserIndex) :=
                 Degrees (Float (FirstLaserAngle)
                          + Float (LaserIndex - LaserReadingRange'First)
                          * (  Float (LastLaserAngle - FirstLaserAngle)
                               / Float (LaserReadingRange'Last
                                        - LaserReadingRange'First)
                               ));
            end loop;

            LaserDriverInterface.InitLaserDriver;
            LaserMonitor.BlockTask;
            LaserInitialized := True;
         end if;
         NoOfClients := NoOfClients + 1;
      end InitLaser;


      entry ShutdownLaser when LaserInitialized is

      begin
         NoOfClients := NoOfClients - 1;
         if NoOfClients = 0 then
            LaserDriverInterface.ShutdownLaserDriver;
            LaserInitialized := False;
         end if;
      end ShutdownLaser;


      entry SuspendSensing when LaserInitialized is

      begin
         NoOfSuspends := NoOfSuspends + 1;
         if NoOfSuspends >= NoOfClients then
            LaserDriverInterface.SuspendLaserDriver;
         end if;
      end SuspendSensing;


      entry ResumeSensing when LaserInitialized is

      begin
         NoOfSuspends := NoOfSuspends - 1;
         if NoOfSuspends < NoOfClients then
            LaserDriverInterface.ResumeLaserDriver;
         end if;
      end ResumeSensing;


      entry GetCurrentLaserStatus (Readings : out LaserStatus)
      when LaserInitialized is

      begin
         LaserDriverInterface.GetMostRecentLaserScan (Readings);
      end GetCurrentLaserStatus;


      entry GetCurrentLaserTimeStamp (TimeStamp: out Time)
      when LaserInitialized is

      begin
         LaserDriverInterface.GetMostRecentLaserTimeStamp (TimeStamp);
      end GetCurrentLaserTimeStamp;


      entry WaitforNewLaserData when LaserInitialized is

      begin
         requeue LaserMonitor.BlockTask;
      end WaitforNewLaserData;


   end LaserInterface;

end Laser;