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

/********************************************************************\
* File: rangesensor_pv.hs                                            *
* Date: 08/04/1997                                                   *
* Author: Cem Unsal                                                  *
*         Robotics Institute, Carnegie-Mellon University             *
*         unsal@ri.cmu.edu                                           *
*                                                                    *
* Description:                                                       *
* Given the position and orientation of the vehicle, RangeSensor_PV  *
* returns the distance and the azimuth angle to the closest vehicle  *
* in range by sweeping the pseudo-vertex points.                     *
* Global position and orientations of the sensed vehicles are used   *
* to evaluate the pseudo-vertex point positions.                     *
* Returned values are realistic for long range sensor; the values,   *
* especially the azimuth angle differs from the actual value for     *
* short ranges.                                                      *
*                                                                    *
* External C functions: dist6                                        *
*                                                                    *
* This file is distributed under the conditions described in the     *
* file 'CONDITIONS' which should accompany this file.                *
\********************************************************************/

#ifndef SAHS_RANGESENSOR_PV_HS
#define SAHS_RANGESENSOR_PV_HS

#define PI 3.1415927

/* External functions */
function dist6(number sen_x;
	       number sen_y;
	       number sen_or;
	       array(number) vehx;
	       array(number) vehy;
	       array(number) veh11;
	       array(number) veh12;
	       number n_of_veh;
	       number vl;
	       number vw;
	       number maxrange;
	       number hfov;
	       array(number) out reading) -> number;

type RangeSensor_PV
{
  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 angle;     // and azimuth angle (degrees) to the
	                              // closest vehicle

  state Vehicle vehicle;          // Vehicle associated with the sensor
        number maxrange;          // Maximum range in meters
	number hfov;              // Half of the horizontal field of view
                                  // (in degrees)
        number xs, ys;            // Sensor position in vehicle coord. frame
        number sor;               // Sensor orientation in vehicle coord.
                                  // frame (in degrees)
	number procspd;           // Processing speed for the sensor
	set(Vehicle) inrange;     // Set of the vehicles in range
	continuous number t;      // Time
	number vl :=5, vw :=2;    // Vehicle length and width
				   
  /* NOTE: In case of different types of vehicles, external c-function 'dist6'
   *       will require a minor change: 'vl' and 'vw' must be created as arrrays
   *       using the set of "vehicles in range."
   *
   * NOTE2: The definition of the pseudo-vertices is in the external c-function
   *        calculating the distance (dist6)
   */

  discrete sense;

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

  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 diff := atan2(vgam12,vgam11)-sor;
	number sen_or := if abs(diff) < 2*PI-abs(diff)
                         then diff
	 	         else -(2*PI-abs(diff))*signum(diff);

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

	/* Create arrays (pos. and orientation) for all vhicles in range */
	array(number) vehx := [gxp(x) : x in inRange];
        array(number) vehy := [gyp(y) : y in inRange];
        array(number) veh11 := [vgam11(vrep(t)) : t in inRange];	 
        array(number) veh12 := [vgam12(vrep(u)) : u in inRange];
	 
	/* Find the distance and azimuth angle to the closest vehicle */
	array(number) reading := [0 : i in [0 .. 1]];
	number dum := dist6(sen_x,sen_y,sen_or,
         		    vehx,vehy,veh11,veh12,
                            size(inRange),vl,vw,
			    maxrange, hfov,
			    reading);
      } 
      do
      {
        t := 0;
        inrange := inRange;
	distance := reading[0];
	angle := reading[1]*180/PI;
      };
}

#endif // SAHS_RANGESENSOR_PV_HS