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

/********************************************************************\
* File: rangesensor2_grid.hs                                         *
* Date: 12/04/1997                                                   *
* Author: Cem Unsal                                                  *
*         Robotics Institute, Carnegie-Mellon University             *
*         unsal@ri.cmu.edu                                           *
*                                                                    *
* Description:                                                       *
* Given the position of the vehicle, the type RangeSensor2_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 sensed vehicles *
* are used for calculations. Sensor position, orientation and        *
* field-of view are user-defined parameters.                         *
*                                                                    *
* This version of RangeSensor is for use with SEP and the global     *
* grid; see hardcopy and/or on-line documentation on sensors 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_RANGESENSOR2_GRID_HS
#define SAHS_RANGESENSOR2_GRID_HS

#define PI 3.1415927

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


type RangeSensor2_Grid
{
  input number gxp, gyp, gzp;        // Position of the vehicle
        number vgam11, vgam12,       // Elements of VGAM
               vgam21, vgam22;       // | cos(alpha)  sin(alpha) |
                                     // | sin(alpha) -cos(alpha) |

        Cell currentcell;            // Current cell from SEP

  output continuous number distance;  // Distance and
         continuous number angle;     // azimuth angle (degrees) to the
         Vehicle closest;             // Closest vehicle

  state Vehicle vehicle;             // Vehicle associated with the sensor
        number maxrange;             // Maximum range in meters
        number fov;                  // Angle defining HALF of the field-of-view
        number procspd;              // Processing speed for the sensor
        number xs, ys, sor;          // Sensor position and orientation in
                                     // vehicle coordinate frame
        set(Vehicle) inrange;        // Set of the vehicles in range
        set(Vehicle) infield;        // Set of the vehicles in field         
        continuous number t;         // Time

  discrete sense;

  setup
    do
    {
      t := 0;
      sor := sor*PI/180;   // degrees -> radians
      fov := fov*PI/180;
    };

  flow default
    {
      t' = 1;
    };
    
  transition
    sense -> sense {} 
    when t >= procspd
      define
      {
        /* Sensor position and orientation in global coordinate frame */ 
        number sen_x := gxp+xs*vgam11+ys*vgam21;
        number sen_y := gyp+xs*vgam12+ys*vgam22;
        number sen_or := atan2(vgam12,vgam11)-sor;

        /* Subset of Vehicles from SEP */
        set(set(Vehicle)) vset1 := {vset(k) : k in NC(currentcell)};

        /* Set of the vehicles in range
         * Instead of the whole set of vehicles, only the vehicle in the 
         * current cell and neighboring cells (minus the ego-vehicle) are 
         * considered 
         */
        set(Vehicle) inRange
               := {x : v1 in vset1, x in v1} + vset(currentcell) - {vehicle};
        /* set of vehicles in the field of view */
        set(Vehicle) inField 
               := {z : z in inRange 
                     | abs(atan2(gyp(z)-sen_y,gxp(z)-sen_x)-sen_or) < fov
                       or
                       2*PI-abs(atan2(gyp(z)-sen_y,gxp(z)-sen_x)-sen_or) < fov};

        /* Closest vehicle; may be out of maximum range */
        Vehicle av := if size(inField) = 0
                      then vehicle
                      else minel z in inField
                           : eucdist(gxp(z),gyp(z),sen_x,sen_y);

        /* Angle between the line connecting the vehicle COG and main
           axis of the sensor */
        number diff := if av = vehicle 
                       then 0
                       else atan2(gyp(av)-sen_y,gxp(av)-sen_x) - sen_or;
      } 
      do
      {
        t := 0;
        inrange := inRange;         // Set of vehicles in range
        infield := inField;         // Set of vehicle in fov
        closest := av; 
        /* Distance to the closest vehicle */
        distance := min(if av = vehicle
                        then maxrange 
                        else eucdist(sen_x,sen_y,gxp(av),gyp(av)),
                        maxrange);

        /* Azimuth angle to the closest vehicle */
        angle := if abs(diff) < 2*PI-abs(diff)
                 then diff*(-1)*180/PI
                 else -(2*PI-abs(diff))*signum(diff)*(-1)*180/PI;
        /* This takes care of the problems around PI; the additional '-1'
           is for the global-vehicle frame alignments {positive angle
           definitions also change}. Returned value is in degrees */
      };
}

#endif // SAHS_RANGESENSOR2_GRID_HS