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

/********************************************************************\
* File: rangesensor.hs                                               *
* Date: 08/04/1997                                                   *
* Author: Cem Unsal                                                  *
*         Robotics Institute, Carnegie-Mellon University             *
*         unsal@ri.cmu.edu                                           *
*                                                                    *
* Description:                                                       *
* Given the position of the vehicle, the type RangeSensor 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 are    *
* used for calculations. The sensor is assumed to be at the center   *
* of the vehicle.                                                    *
*                                                                    *
* External C functions: eucdist                                      *
*                                                                    *
* This file is distributed under the conditions described in the     *
* file 'CONDITIONS' which should accompany this file.                *
\********************************************************************/

#ifndef SAHS_RANGESENSOR_HS
#define SAHS_RANGESENSOR_HS

#define PI 3.1415927

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

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

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

  state Vehicle vehicle;             // Vehicle associated with the sensor
        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

  discrete sense;

  setup
    do
    {
      t := 0;
    };

  flow default
    {
      t' = 1;
    };
    
  transition
    sense -> sense {} 
    when t >= procspd
      define
      {
        /* Set of the vehicles in range
         * Instead of the whole set of vehicles, SREP or similar structures 
         * must/can be used.
         */
        set(Vehicle) inRange := {z : z in Vehicles | z /= vehicle };
 
        /* Closest vehicle; may be out of maximum range */
        Vehicle av := if size(inRange) = 0
                      then vehicle
                      else minel z in inRange 
                           : eucdist(gxp,gyp,gxp(vrep(z)),gyp(vrep(z))); 

        /* Angle between the line connecting the vehicle COGs and x-axis of
           the vehicle coordinate frame */
        number diff := if av = vehicle 
                       then 0
                       else atan2(gyp(av)-gyp,gxp(av)-gxp) 
                                        - atan2(vgam12, vgam11);
      } 
      do
      {
        t := 0;
        inrange := inRange;    // Set of all vehicles in range
        closest := av; 
        /* Distance to the closest vehicle */
        distance := min(if av = vehicle 
                        then maxrange 
                        else eucdist(gxp,gyp,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_RANGESENSOR_HS