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

/********************************************************************\
* File: frontsensor.hs                                               *
* Date: 12/15/1997                                                   *
* Author: Cem Unsal                                                  *
*         Robotics Institute, Carnegie-Mellon University             *
*         unsal@ri.cmu.edu                                           *
*                                                                    *
* Description:                                                       *
* Front Sensor for Human Driver Models (HDM)                         *
*                                                                    *
* This sensor returns the COG-to-COG distances to the vehicles in    *
* the same lane. Longitudinal distances to the closest vehicle       *
* in the front are returned.                                         *
*                                                                    *
* This module may require VREP for synchronization . Sensor          *
* parameters can be set in this file, or in the vehicle 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_FRONTSENSOR_HS
#define SAHS_FRONTSENSOR_HS

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

type FrontSensor
{
  output Vehicle closest;              // Closest vehicle   
         continuous number distance;   // Headway 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;
      distance := 99;   // This is here for debugging purposes
    };

  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 down lanes for the current lane */ 
        set(Lane) lanesDown := {laneDown(lane)[k]
                                : k in [0 .. size(laneDown(lane))-1]};

        /* Number of Down Sections */
        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 current lane, front */
         set(Vehicle) inrange 
               := {z : z in Vehicles
                     | z /= vehicle 
                       and (
                            (
                             lane(vrep(z)) = lane
                             and
                             rxp(vrep(z)) > rxp and rxp(vrep(z))-rxp < range
                             and
                             section(vrep(z)) = section
                            )
                            or
                            (
                             size(lanesDown-{lane(vrep(z))})=size(lanesDown)-1
                             and
                             rxp(vrep(z))+length(section)-rxp < range
                             and
                             section(vrep(z)) = sectionDown(section)[secDown]
                            )
                           )
                   };

         /* Closest vehicle in the current lane, front */
         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 range
                    else if lane(vrep(vir)) = lane 
                         then rxp(vrep(vir))-rxp
                         else rxp(vrep(vir))+length(section)-rxp;
      };
}

#endif // SAHS_FRONTSENSOR_HS