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

/********************************************************************\
* File: frontsensor2.hs                                              *
* Date: 12/16/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 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_FRONTSENSOR2_HS
#define SAHS_FRONTSENSOR2_HS

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

type FrontSensor2
{
  output Vehicle closest;       // Closest vehicles
         Vehicle sec_closest;   
         continuous number distance1;   // Headway distances
         continuous number distance2;
                                   
  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;  // for debugging
    };

  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 vir1
              := if size(inrange) = 0 
                 then nil
                 else minel v in inrange 
                      : eucdist(gxp(vrep(v)),gyp(vrep(v)),gxp,gyp);

        /* All vehicles in range minus closest vehicle */
        set(Vehicle) inrange2 := inrange - {vir1};

        /* Second closest vehicle in the current lane, front */
        Vehicle vir2
              := if size(inrange2) = 0 
                 then nil
                 else minel v in inrange2 
                      : eucdist(gxp(vrep(v)),gyp(vrep(v)),gxp,gyp);
      }
      do
      {
        t := 0; 
        closest := vir1;
        sec_closest := vir2;
        /* Distances */
        distance1 := if vir1 = nil 
                     then range
                     else if lane(vrep(vir1)) = lane 
                          then rxp(vrep(vir1))-rxp
                          else rxp(vrep(vir1))+length(section)-rxp;
        distance2 := if vir2 = nil 
                     then range
                     else if lane(vrep(vir2)) = lane 
                          then rxp(vrep(vir2))-rxp
                          else rxp(vrep(vir2))+length(section)-rxp;   
      };
}

#endif // SAHS_FRONTSENSOR2_HS