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

/********************************************************************\
* File: roadsidesensor2.hs                                           *
* Date: 09/11/1997                                                   *
* Author: Cem Unsal                                                  *
*         Robotics Institute, Carnegie-Mellon University             *
*         unsal@ri.cmu.edu                                           *
*                                                                    *
* Description:                                                       *
* The type RoadSideSensor2 returns the ID of the closest vehicle as  *
* well as the distance and the azimuth angle.                        *
*                                                                    *
* 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_ROADSIDESENSOR2_HS
#define SAHS_ROADSIDESENSOR2_HS\

#define PI 3.1415927

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

type RoadSideSensor2
{
  output continuous number distance;  // Distance
	 continuous number angle;     // Azimuth angle 
         Vehicle closest;             // Closest vehicle

  state Segment segment;        // Segment associated with the sensor
        number sx, sy;          // Sensor position on the segment
	number sorseg;          // Sensor orientation wrt to road segment
			        // (Clockwise direction: +, calculated from
                                //  segment's x-axis at sensor location)
	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
        number sen_or;          // Global prientation of the sensor
	
  discrete sense;

  setup
    define
    {
      // See documentation for decription 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);
      number diff1 := if abs(beta+sx*rho) < 2*PI-abs(beta+sx*rho)
                      then (beta+sx*rho)
                      else -(2*PI-abs(beta+sx*rho))*signum(beta+sx*rho);	
      number diff2 := if abs(sorseg+diff1) < 2*PI-abs(sorseg+diff1)
                      then (sorseg+diff1)
                      else -(2*PI-abs(sorseg+diff1))*signum(sorseg+diff1);	
    }
    do
    {
      t := 0;
      /* Global position */
      xgs :=  gxa(segment) + sxbar*cos(beta) - sybar*sin(beta);
      ygs :=  gya(segment) + sxbar*sin(beta) + sybar*cos(beta);
      /* Global orientation */ 
      sen_or := diff2;
    };

  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));
	/* Angle */
	number diff := if av = nil 
                       then 0
		       else atan2(gyp(av)-ygs,gxp(av)-xgs)-sen_or;
 
      } 
      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));
	/* Angle to the closest vehicle */ 
	angle := if abs(diff) < 2*PI-abs(diff)
                 then diff*180/PI
                 else -(2*PI-abs(diff))*signum(diff)*180/PI;
      };
}

#endif // SAHS_ROADSIDESENSOR2_HS