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

/********************************************************************\
* File: frontsensor_rate.hs                                          *
* Date: 12/30/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 version of FrontSensor also returns the rate of change in the *
* headway reading.                                                   *
*                                                                    *
* 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_FRONTSENSOR_RATE_HS
#define SAHS_FRONTSENSOR_RATE_HS

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

type FrontSensor_Rate
{
  output Vehicle closest;                // Closest vehicle   
         continuous number distance;     // Headway distance
         continuous number dist_rate;    // Headway rate

  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
        number prev_dist;       // Previous measurement

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

  discrete run;

  export sampled;

  flow default
    {
      t' = 1; /* ' */
    }; 

    /* NOTE: This version of FrontSensor doesn't include forced measurement. If 
     * it is to be added, calculation of the headway rate must be defined 
     * carefully for the forced case, using the current value of t, not the
     * fixed value of the sampling rate. 
     */

  transition
    run -> run {sampled}
    when t >= sr
      define
      {
        /* Previous measurement */
        number t_prev_dist := distance;

        /* 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 and distance rate*/
        prev_dist := t_prev_dist;
        distance := if vir = nil 
                    then range
                    else if lane(vrep(vir)) = lane 
                         then rxp(vrep(vir))-rxp
                         else rxp(vrep(vir))+length(section)-rxp;
        dist_rate := (distance - prev_dist) / sr;
      };
}

#endif // SAHS_FRONTSENSOR_RATE_HS