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

/********************************************************************\
* File: rangesensor2_rate.hs                                         *
* Date: 12/30/1997                                                   *
* Author: Cem Unsal                                                  *
*         Robotics Institute, Carnegie-Mellon University             *
*         unsal@ri.cmu.edu                                           *
*                                                                    *
* Description:                                                       *
* Given the position of the vehicle, the type RangeSensor2_Rate      *
* 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 an extended version of RangeSensor2 which also returns the    *
* rate of change in the distance meaurement                          *
*                                                                    *
* External C functions: eucdist                                      *
*                                                                    *
* This file is distributed under the conditions described in the     *
* file 'CONDITIONS' which should accompany this file.                *
\********************************************************************/

#ifndef SAHS_RANGESENSOR2_RATE_HS
#define SAHS_RANGESENSR2_RATE_HS

#define PI 3.1415927

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

type RangeSensor2_Rate
{
  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) |

  output continuous number distance;   // Distance
         continuous number dist_rate;  // distance rate
         continuous number angle;      // azimuth angle (degrees) and
         continuous number ang_rate;   // angle rate 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
        number prev_dist, prev_angle;   // Previous meaurements

  discrete sense;

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

  flow default
    {
      t' = 1;
    };
    
  transition
    sense -> sense {} 
    when t >= procspd
      define
      {
        /* Previous measurements */
        number t_prev_dist := distance;
        number t_prev_angle := angle;

        /* 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;

        /* Set of the vehicles in range
         * Instead of the whole set of vehicles, SEP or similar 
         * structures must/can be used. */
        set(Vehicle) inRange := {z : z in Vehicles | z /= 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; 
        /* Previous meaurements */
        prev_dist := t_prev_dist;
        prev_angle := t_prev_angle;

        /* 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 */

        /* Rates */
        dist_rate := (distance - prev_dist) / procspd;
        ang_rate := (angle - prev_angle) / procspd; 
      };
}

#endif // SAHS_RANGESENSOR2_RATE_HS