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

/********************************************************************\
* File: rightsensor.hs                                               *
* Date: 12/15/1997                                                   *
* Author: Cem Unsal                                                  *
*         Robotics Institute, Carnegie-Mellon University             *
*         unsal@ri.cmu.edu                                           *
*                                                                    *
* Description:                                                       *
* Right Side Sensor for Human Driver Models (HDM)                    *
*                                                                    *
* This sensor returns the COG-to-COG distances to the vehicles in    *
* the right lane. Longitudinal distances to the closest vehicle      *
* in the front and the back 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_RIGHTSENSOR_HS
#Define SAHS_RIGHTSENSOR_HS

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

type RightSensor
{
  output Vehicle fclosest, bclosest;              // Closest vehicles
	 continuous number fdistance, bdistance;  // Longitudinal dist.

  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 right lane */ 
        set(Lane) lanesDownRight :=
                  if laneRight(lane) = nil
                  then nil
                  else {laneDown(laneRight(lane))[k]
		    	: k in [0 .. size(laneDown(laneRight(lane)))-1]}; 

        set(Lane) lanesUpRight := 
                  if laneRight(lane) = nil
        	  then nil
		  else {laneUp(laneRight(lane))[k]
			: k in [0 .. size(laneUp(laneRight(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 right lane, front */
        set(Vehicle) inrangef 
              := if laneRight(lane) = nil
                 then nil
                 else {z : z in Vehicles
                         | z /= vehicle
                           and 
                           (
                            (lane(vrep(z)) = laneRight(lane)
                             and
                             rxp(vrep(z)) > rxp and rxp(vrep(z))-rxp < frange
                             and
                             section(vrep(z)) = section
                            )
                            or
                            (size(lanesDownRight-{lane(vrep(z))}) 
                                                     = size(lanesDownRight)-1
                             and
                             rxp(vrep(z))+length(section)-rxp < frange
                             and
                             section(vrep(z))=sectionDown(section)[secDown]
                            )
                           )
                      };

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

        /* Closest vehicle in the right lane, front */
        Vehicle virf
           := if laneRight(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);
	     
        /* Closest vehicle in the right lane, back */
        Vehicle virb
           := if laneRight(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;
        bclosest := virb;
        /* Distances */
        fdistance := if laneRight(lane) = nil
                     then -2
	             else if virf = nil
		          then -1
		          else if lane(vrep(virf)) = laneRight(lane)
		               then rxp(vrep(virf))-rxp
			       else rxp(vrep(virf))+length(section)-rxp;

        bdistance := if laneRight(lane) = nil
                     then -2
                     else if virb = nil
		          then -1
		          else if lane(vrep(virb)) = laneRight(lane)
		               then rxp-rxp(vrep(virb))
			       else rxp+length(section(vrep(virb)))
                                                       -rxp(vrep(virb));
      };
}

#endif // SAHS_RIGHTSENSOR_HS