/* -*- Mode: Text -*- */

/********************************************************************\
* File: leftsensor2.hs                                               *
* Date: 12/15/1997                                                   *
* Author: Cem Unsal                                                  *
*         Robotics Institute, Carnegie-Mellon University             *
*         unsal@ri.cmu.edu                                           *
*                                                                    *
* Description:                                                       *
* Left Side Sensor for Human Driver Models (HDM)                     *
*                                                                    *
* This sensor returns the COG-to-COG distances to the vehicles in    *
* the left lane. Longitudinal distances to:                          *
*                                                                    *
* - closest vehicle in the back and                                  *
* - two closest vehicles in the front                                *
*                                                                    *
* are returned.                                                      *
*                                                                    *
* This module may require VREP for synchronization. Sensor           *
* parameters can be set in this file, or the type 'creating' the     *
* sensor.                                                            *
*                                                                    *
* External C functions: eucdist                                      *
*                                                                    *
* This file is distributed under the conditions described in the     *
* file 'CONDITIONS' which should accompany this file.                *
\********************************************************************/

#ifndef SAHS_LEFTSENSOR2_HS
#define SAHS_LEFTSENSOR2_HS

/* External functions */
function eucdist(number gxp1, gyp1, gxp2, gyp2) -> number;

type LeftSensor2
{
  output Vehicle fclosest, 
                 fclosest2,
                 bclosest;               // Closest vehicles
         continuous number fdistance, 
                           fdistance2, 
                           bdistance;    // Longitudinal distances

  input number rxp, gxp, gyp;    // Position of the sensor
        Lane lane;               // Sensor lane
        Section section;         // Sensor section

  state Vehicle vehicle;                // Self vehicle            
        number frange, brange;          // Sensor ranges
        number sr;                      // Sampling rate
        continuous number t;            // Time
        Driver mydriver;                // Associated driver for 
                                        // synchronization; alternative is vrep

  setup
    do
    {
      t := sr-0.1;
    };

  discrete idle, sample;

  export timed, forced;

  flow default
    {
      t' = 1; 
    }; 
 
  transition  
    idle -> sample {timed}
    when t >= sr
    ,

    idle -> sample {mydriver:new_lane, forced}
    ,

    sample -> idle {}
      define
      {
        /* Sets of up and down lanes for the current left lane */ 
        set(Lane) lanesDownLeft :=
                  if laneLeft(lane) = nil
                  then nil
                  else {laneDown(laneLeft(lane))[k]
                        : k in [0 .. size(laneDown(laneLeft(lane)))-1]}; 
        set(Lane) lanesUpLeft := 
                  if laneLeft(lane) = nil
                  then nil
                  else {laneUp(laneLeft(lane))[k]
                        : k in [0 .. size(laneUp(laneLeft(lane)))-1]};

        /* Number of  Up/Down Sections */
        number secUp :=0;
        number secDown :=0;

        /* This assumes that there is only one up/down section for each
         * section. General subroutine must have something similar to:
         *
         * set(number) nofSecUp := {k : k in [0 .. size(sectionUp(section))-1]};
         * 
         * and then below:
         *
         * exists i  in nofSecUp : section(vrep(z)) = sectionDown(section)[i]
         */

        /* Set of vehicles in the left lane, front */
        set(Vehicle) inrangef
            := if laneLeft(lane) = nil
               then nil
               else {z : z in Vehicles
                       | z /= vehicle
                         and (
                              (lane(vrep(z)) = laneLeft(lane)
                               and
                               rxp(vrep(z)) > rxp and rxp(vrep(z))-rxp < frange
                               and
                               section(vrep(z)) = section
                              )
                              or
                              (size(lanesDownLeft-{lane(vrep(z))}) 
                                                        = size(lanesDownLeft)-1
                               and
                               rxp(vrep(z))+length(section)-rxp < frange
                               and
                               section(vrep(z)) = sectionDown(section)[secDown]
                              )
                             )
                    };

        /* Set of vehicles in the left lane, back */
        set(Vehicle) inrangeb
           := if laneLeft(lane) = nil
              then nil
              else {z : z in Vehicles
                      | z /= vehicle
                        and (
                             (lane(vrep(z)) = laneLeft(lane)
                              and
                              rxp(vrep(z)) < rxp and -rxp(vrep(z))+rxp < brange
                              and
                              section(vrep(z)) = section
                             )
                             or
                             (size(lanesUpLeft-{lane(vrep(z))}) 
                                                           = size(lanesUpLeft)-1
                              and
                              rxp+length(section(vrep(z)))-rxp(vrep(z)) < brange
                              and
                              section(vrep(z)) = sectionUp(section)[secUp]
                             )
                            )
                   };

        /* Closest vehicle in the left lane, front */
        Vehicle virf
           := if laneLeft(lane) = nil
              then nil
              else if size(inrangef) = 0
                   then nil
                   else minel v in inrangef 
                        : eucdist(gxp(vrep(v)),gyp(vrep(v)),gxp,gyp);
             
        /* Set of vehicles in the left lane, front, minus the closest vehicle */
        set(Vehicle) inrangef2 := inragef - {virf};

        /* Second closest vehicle in the left lane, front */
        Vehicle virf2
              := if laneLeft(lane) = nil
                 then nil
                 else if size(inrangef2) = 0
                      then nil
                      else minel v in inrangef2 
                           : eucdist(gxp(vrep(v)),gyp(vrep(v)),gxp,gyp);    

        /* Closest vehicle in the left lane, back */
        Vehicle virb
              := if laneLeft(lane) = nil
                 then nil
                 else if size(inrangeb) = 0
                      then nil
                      else minel v in inrangeb 
                           : eucdist(gxp(vrep(v)),gyp(vrep(v)),gxp,gyp);
      }
      do
      {
        t := 0;
        fclosest := virf;
        flocsest2 := virf2;
        bclosest := virb;
        /* Distances */
        fdistance := if laneLeft(lane) = nil
                     then -2
                     else if virf = nil
                          then -1
                          else if lane(vrep(virf)) = laneLeft(lane)
                               then rxp(vrep(virf))-rxp
                               else rxp(vrep(virf))+length(section)-rxp;
        fdistance2 := if laneLeft(lane) = nil
                      then -2
                      else if virf2 = nil
                           then -1
                           else if lane(vrep(virf2)) = laneLeft(lane)
                                then rxp(vrep(virf2))-rxp
                                else rxp(vrep(virf2))+length(section)-rxp;
        bdistance := 
                 if laneLeft(lane) = nil
                    then -2
                    else if virb = nil
                         then -1
                         else if lane(vrep(virb)) = laneLeft(lane)
                              then rxp-rxp(vrep(virb))
                              else rxp+length(section(vrep(virb)))-rxp(vrep(virb));
      };
} 

#endif // SAHS_LEFTSENSOR2_HS