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

/********************************************************************\
* File: roadsidesensor2_grid.hs                                      *
* Date: 12/08/1997                                                   *
* Author: Cem Unsal                                                  *
*         Robotics Institute, Carnegie-Mellon University             *
*         unsal@ri.cmu.edu                                           *
*                                                                    *
* Description:                                                       *
* The type RoadSideSensor2_Grid 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.           *
*                                                                    *
* This type is similar to RoadSideSensor2, except the use of the     *
* sensor grid (See grid*.hs and cell.hs for details).                *
*                                                                    *
* External C functions: eucdist                                      *
*                                                                    *
* This file is distributed under the conditions described in the     *
* file 'CONDITIONS' which should accompany this file.                *
\********************************************************************/

#ifndef SAHS_ROADSIDESENSOR2_GRID_HS
#define SAHS_ROADSIDESENSOR2_GIRD_HS

#define PI 3.1415927

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

type RoadSideSensor2_Grid
{
  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
	
        CellCreator mycc;       // Needed for first transition
	Cell mycell;            // Grid cell

  discrete idle, sense {senseflow;};

  setup
    define
    {
      // See documentation for 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);
      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;
      /* Cell Creator */
      mycc := ccc;    // 'ccc' is the global parameter
    };

  flow senseflow
    {
      t' = 1;
    };
    
  transition
    idle -> sense {mycc:finished}
    // To make sure that all cells are created
      define
      {
	set(Cell) candidates := {cc : cc in Cells
				    | x(cc) <= sx and x(cc)+cs(cc) > sx and
			              y(cc) <= sy and y(cc)+cs(cc) > sy};
      }
      do
      {
        mycell := minel c in candidates : 1;
      },

    sense -> sense {} 
    when t >= procspd
      define
      {
        /* Subset of vehicles in mycell and adjoining cells */
        set(set(Vehicle)) vset1 := {vset(k) : k in NC(mycell)};

        /* Set of the vehicles in range
         * Instead of the whole set of vehicles, only the vehicle in mycell
         * and neighboring cells are considered */
        set(Vehicle) inRange 
	    := {x : v1 in vset1, x in v1} + vset(mycell}; 
	/* 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_GRID_HS