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

/********************************************************************\
* File: roadsidesensor.hs                                            *
* Date: 09/11/1997                                                   *
* Author: Cem Unsal                                                  *
*         Robotics Institute, Carnegie-Mellon University             *
*         unsal@ri.cmu.edu                                           *
*                                                                    *
* Description:                                                       *
* The type RoadSideSensor returns the ID of the closest vehicle as   *
* well as the distance.                                              *
*                                                                    *
* Global positions of the center of gravities of the vehicles and    *
* global position of the sensor are used for calculations.           *
*                                                                    *
* External C functions: eucdist                                      *
*                                                                    *
* This file is distributed under the conditions described in the     *
* file 'CONDITIONS' which should accompany this file.                *
\********************************************************************/

#ifndef SAHS_ROADSIDESENSOR_HS
#define SAHS_ROADSIDESENSOR_HS

#define PI 3.1415927

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

type RoadSideSensor
{
  output continuous number distance;  // Distance
	 Vehicle closest;             // Closest vehicle

  state Segment segment;         // Segment associated with the sensor
        number sx, sy;           // Sensor position on the segment
	number maxrange;         // Maximum range in meters
	number procspd;          // Processing speed for the sensor
	set(Vehicle) inrange;    // Set of the vehicles in range
	continuous number t;     // Time
	number xgs, ygs;         // Global position of the sensor

  discrete sense;

  setup
    define
    {
      // See documentation for the description of these parameters
      number rho := curvature(segment);
      number R := if rho = 0 then 0 else 1/abs(rho);
      number beta := orientation(segment);
      number sxbar := if rho = 0 then sx
			         else R*sin(sx/R)+
			              sy*sin(sx/R)*signum(-rho);

      number sybar := if rho = 0 then sy
			         else -R*(1-cos(sx/R))*signum(-rho)+
			              +sy*cos(sx/R);
    }
    do
    {
      t := 0;
      xgs :=  gxa(segment) + sxbar*cos(beta) - sybar*sin(beta);
      ygs :=  gya(segment) + sxbar*sin(beta) + sybar*cos(beta);
    };

  flow default
    {
      t' = 1;
    };
    
  transition
    sense -> sense {} 
    when t >= procspd
      define
      {
        /* Set of the vehicles in range
         * Instead of the whole set of vehicles, other structures 
         * must/can be used.
         */
        set(Vehicle) inRange 
	    := {z : z in Vehicles 
		  | eucdist(xgs, ygs, gxp(z), gyp(z)) < maxrange };
 
        /* Closest vehicle; may be out of maximum range */
        Vehicle av := if size(inRange) = 0
                      then nil
                      else minel z in inRange 
                      : eucdist(xgs,ygs,gxp(z),gyp(z)); 
      } 
      do
      {
        t := 0;
        inrange := inRange;    // Set of all vehicles in range
	closest := av; 
        /* Distance to the closest vehicle */
        distance := if av = nil 
      		    then maxrange 
		     else eucdist(xgs,ygs,gxp(av),gyp(av));
      };
}

#endif // SAHS_ROADSIDESENSOR_HS