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

/********************************************************************\
* File: backsensor.hs                                                *
* Date: 12/15/1997                                                   *
* Author: Cem Unsal                                                  *
*         Robotics Institute, Carnegie-Mellon University             *
*         unsal@ri.cmu.edu                                           *
*                                                                    *
* Description:                                                       *
* Back Sensor for Human Driver Models (HDM)                          *
*                                                                    *
* This sensor returns the COG-to-COG distances to the vehicles in    *
* the same lane. Longitudinal distance to the trailing vehicle       *
* in the same lane is 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_BACKSENSOR_HS
#define SAHS_BACKSENSOR_HS

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

type BackSensor
{
  output Vehicle closest;               // Closest vehicle
         continuous number distance;    // Tailway distance

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

  state Vehicle vehicle;          // Self vehicle
        number range;             // Sensor range
        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
      {
        /* Set of up lanes for the current lane */
        set(Lane) lanesUp := {laneUp(lane)[k]
                               : k in [0 .. size(laneUp(lane))-1]};

        /* Number of  Up Sections */
        number secUp :=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 current lane, back */
        set(Vehicle) inrange 
              := {z : z in Vehicles
                    | z /= vehicle 
                      and (
                           (
                            lane(vrep(z)) = lane
                            and
                            rxp(vrep(z)) < rxp
                            and
                            -rxp(vrep(z))+rxp(vrep(vehicle)) < range
                            and
                            section(vrep(z)) = section
                           )
                           or
                           (
                            size(lanesUp-{lane(vrep(z))})=size(lanesUp)-1
                            and
                            rxp+length(section(vrep(z)))-rxp(vrep(z)) < range
                            and
                            section(vrep(z)) = sectionUp(section)[secUp]
                           )
                          )
                 };

        /* Closest vehicle in the current lane, back */
        Vehicle vir
              := if size(inrange) = 0 
                 then nil
                 else minel v in inrange 
                      : eucdist(gxp(vrep(v)),gyp(vrep(v)),gxp,gyp);  
      }
      do
      {
        t := 0;
        closest := vir;
        distance := if vir = nil 
                    then -1 
                    else if lane(vrep(vir)) = lane
                         then rxp-rxp(vrep(vir))
                         else rxp+length(section(vrep(vir)))-rxp(vrep(vir));
      };
} 

#endif // SAHS_BACKSENSOR_HS