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

/********************************************************************\
* File: rangesensor_r.hs                                             *
* Date: 07/31/1997                                                   *
* Author: Cem Unsal                                                  *
*         Robotics Institute, Carnegie-Mellon University             *
*         unsal@ri.cmu.edu                                           *
*                                                                    *
* Description:                                                       *
* Short range sensor with ray definitions                            *
*                                                                    *
* Given the position and the orientation of the sensor in vehicle    *
* coordinate frame, the position and orientation of the vehicle,     *
* maximum range, maximum field of view, and the ray resolution of    *
* the sensor, the type 'RangeSensor_R' returns the distance and      *
* the azimuth of the closest vehicle in range.                       *
*                                                                    *
* The resolution of the sensor scan is defined by the number of      *
* rays. This functional sensor type is specifically designed for     *
* short range readings: it calculates the intersection of the sensor *
* rays with the lines defining the vehicles in range.                *
*                                                                    *
* External C functions: dist5, min_of_array                          *
*                                                                    *
* This file is distributed under the conditions described in the     *
* file 'CONDITIONS' which should accompany this file.                *
\********************************************************************/

#ifndef SAHS_RANGESENSOR_R_HS
#define SAHS_RANGESENSOR_R_HS

#define PI 3.1415927

/* External functions */
function dist5 (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;
		array(number) rayvec;
		array(number) out distvec) -> number;

function min_of_array (array(number) vec;
		       number vecsize;
		       array(number) out result) -> number;

type RangeSensor_R
{
  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 angle 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 nray;              // Resolution : number of rays
        number xs, ys;            // Sensor position in vehicle coord. frame
        number sor;               // Sensor orientation in vehicle coor. 
			          // frame (in degrees)
	number procspd;           // Processing speed for the sensor
      	array(number) rayvec;     // Definition of ray angles 
	set(Vehicle) inrange;     // Set of the vehicles in range
			          // This is the connection to SEP
	array(number) range;      // Returned readings for all rays
	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."
   */
				   
  discrete sense;

  setup
    do
    {
      /* Slopes of the lines defining the rays (vehicle coord. frame) */
      rayvec := [(hfov-2*hfov/(nray-1)*i)/180*PI : i in [0 .. nray-1]];  
      /* Sensor orientation in radians */
      sor := sor/180*PI;  
      t := 0;
    };

  flow default
    {
      t' = 1;
    };
    
  transition
    sense -> sense {} 
    when t >= procspd
      define
      {
        /* NOTE: If noise to be added to ray definitions, the definition of 
         *       'rayvec' above, must be moved here */

        /* 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 };
          
	/* 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];
           
        /* 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);
 
        /* Create the array for range readings, and then evaluate */
        array(number) raydist := [0 : i in [0 .. nray-1]];
        number dum := dist5(sen_x,sen_y,sen_or,
         		    vehx,vehy,veh11,veh12,
                            size(inRange),vl,vw,
			    rayvec,raydist);
   
        /* Find minimum value and its index */
        array(number) result := [0 : i in [0 .. 1]];
        number dum2 := min_of_array(raydist,nray,result); 
      } 
      do
      {
	t := 0;
        /* NOTE: If noise to be added to range readings, this can be done 
         * here; an alternative method is to add noise in the c-function */
        range := raydist;                        // Range readings for all rays
        distance := result[0];                   // Minimum distance
        inrange := inRange;                      // Set of all vehicles in range
        angle := hfov-2*hfov/(nray-1)*result[1]; 
					    // Azimuth for the ray with minimum 
	                                    // reading (if there are no veh. in 
                                            // range, this doesn't make sense)
	};
}

#endif // SAHS_RANGESENSOR_R_HS