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

/********************************************************************\
* File: ExVehicle2.hs                                                *
* Date: 01/26/1998                                                   *
* Author: Cem Unsal                                                  *
*         Robotics Institute, Carnegie-Mellon University             *
*         unsal@ri.cmu.edu                                           *
*                                                                    *
* Description:                                                       *
* Example template for type Vehicle                                  *
* Vehicle with random actions; used to test the grid.                *
*                                                                    *
* The following types are submodules:                                *
*  - DriverA (random lane-changing actions every n sec.)             *
*  - Pursuit-point model                                             *
*  - Vehicle (2-d kinematic model)                                   *
*  - 2DController (2-D vehicle controller)                           *
*  - RangeSensor2_Grid (Similar to RangeSensor2, works with SEP)     *
*  - Sensor Environment Processor SEP                                * 
*                                                                    *  
* External C functions: <none>                                       *
*                                                                    *
* This file is distributed under the conditions described in the     *
* file 'CONDITIONS' which should accompany this file.                *
*                                                                    *
* This file must also be accompanied by the file 'UCBCOPY'.          *
\********************************************************************/
  
#include <cmu/misc/2dkineveh.hs>
#include <cmu/misc/2dkinectrl.hs>
#include <cmu/hdm/humandrivers.hs>
#include <cmu/hdm/pursuit.hs>
#include <cmu/sensors/rangesensor2_grid.hs>
#include <cmu/sensors/sep.hs>

global set(Vehicle) Vehicles;

type AutomatedVehicle
{
  output Vehicle_Kinematics 2dvehicle;
         2DController 2dcontroller;
         Pursuit pursuit;
         DriverA driver;
         RangeSensor2_Grid fs, ls, rs;
         
         Source source;
         VREP vrep;
         SEP  sep;        // Sensor Environment Processor

         continuous number gxp, gyp, gzp :=0;
         continuous number xDot, yDot, zDot :=0;
         continuous number length := 5, width := 2;  // Vehicle size

  discrete run, dummy;

  setup
    define
      {
        Vehicle_Kinematics 2dvehicle := create(Vehicle_Kinematics, 
                                               speed := 20);
        
        VREP tvrep := create(VREP, gxp := gxp(source),
                             gyp := gyp(source),
                             gzp := gzp(source),
                             section := section(source),
                             segment := segment(source),
                             lane := lane(source),
                             rxp := rxp(source),
                             ryp := ryp(source),
                             rzp := rzp(source),
                             lyp := lyp(source),
                             vgam11 := vgam11(source),
                             vgam12 := vgam12(source),
                             vgam13 := vgam13(source),
                             vgam21 := vgam21(source),
                             vgam22 := vgam22(source),
                             vgam23 := vgam23(source),
                             vgam31 := vgam31(source),
                             vgam32 := vgam32(source),
                             vgam33 := vgam33(source),
                             vehicle := self);

        SEP tsep := create(SEP, 
                           mycell := scell(source), 
                           myveh := self);
        // This definition requires new definition of the source; 
        // See file source_grid.hs

        2DController tcontroller := create(2DController);
        DriverA tdriver := create(DriverA);
        Pursuit tpursuit := create(Pursuit);

        // Front, Left and Right Sensors
        RangeSensor2 tfs := create(RangeSensor2,
                                   vehicle := self,
                                   xs := 1,
                                   ys := 0,
                                   sor := 0,
                                   maxrange := 100,
                                   fov := 15,
                                   procspd := 0.2);

        RangeSensor2 trs := create(RangeSensor2,
                                   vehicle := self,
                                   xs := 0,
                                   ys := -1,
                                   sor := -90,
                                   maxrange := 20,
                                   fov := 15,
                                   procspd := 0.2);

        RangeSensor2 tls := create(RangeSensor2,
                                   vehicle := self,
                                   xs := 0,
                                   ys := 1,
                                   sor := 90,
                                   maxrange := 100,
                                   fov := 15,
                                   procspd := 0.2);
      }
    do
      {
        2dvehicle := tvehicle; 
        2dcontroller := tcontroller;
        pursuit := tpursuit;
        driver := tdriver;
        vrep := tvrep;
        sep := tsep;
        fs := tfs; ls := tls; rs := trs;
        Vehicles := Vehicles + {self};
      }
    connect
      {
        steering(tvehicle) <- steering(tcontroller);
        acc(tvehicle) <- long_acc(tcontroller);
        
        followLane(tvrep) <- 0;
        xDot(tvrep) <- xDot(tvehicle);
        yDot(tvrep) <- yDot(tvehicle);
        zDot(tvrep) <- 0.0;
        wx(tvrep) <- 0.0;
        wy(tvrep) <- 0.0;
        wz(tvrep) <- thetaDot(tvehicle);

        spd_act(tcontroller) <- speed(tvehicle);  // Wheel speed 
        spd_des(tcontroller) <- target_spd(tdriver);
        lyp_act(tcontroller) <- lyp(tvrep);
        lyp_des(tcontroller) <- des_lyp_pur(pursuit);
        vgam11(tcontroller) <- vgam11(tvrep);
        vgam12(tcontroller) <- vgam12(tvrep);
        // vgamxy can be replaced with orientation(tvrep) if fixed
        rgam11(tcontroller) <- rgam11(tvrep);
        rgam12(tcontroller) <- rgam12(tvrep);
        curv(tcontroller) <- curvature(segment(tvrep));
        ryp(tcontroller) <- ryp(tvrep);

        target_lyp(tpursuit) <- target_lyp(tdriver);
        current_lyp(tpursuit) <- lyp(tvrep);
        current_lane(tpursuit) <- lane(tvrep);

        current_spd(tdriver) <- speed(tvehicle);
        current_hdwy(tdriver) <- distance(tfs);
        sensor_lf(tdriver) <- fdistance(tls);
        sensor_lb(tdriver) <- bdistance(tls);
        sensor_rf(tdriver) <- fdistance(trs);
        sensor_rb(tdriver) <- bdistance(trs);

        gxp(tfs) <- gxp(tvrep);
        gyp(tfs) <- gyp(tvrep);
        vgam11(tfs) <- vgam11(tvrep);
        vgam12(tfs) <- vgam12(tvrep);
        vgam21(tfs) <- vgam21(tvrep);
        vgam22(tfs) <- vgam22(tvrep);
                  
        gxp(tls) <- gxp(tvrep);
        gyp(tls) <- gyp(tvrep);
        vgam11(tls) <- vgam11(tvrep);
        vgam12(tls) <- vgam12(tvrep);
        vgam21(tls) <- vgam21(tvrep);
        vgam22(tls) <- vgam22(tvrep);

        gxp(trs) <- gxp(tvrep);
        gyp(trs) <- gyp(tvrep);
        vgam11(trs) <- vgam11(tvrep);
        vgam12(trs) <- vgam12(tvrep);
        vgam21(trs) <- vgam21(tvrep);
        vgam22(trs) <- vgam22(tvrep);                        

        // vgam11 and vgam12 for sensors can be replaced with
        // orientation(tvrep) if it is fixed in vrep.hs (atan2!!!)
 
        x(tsep) <- gxp(tvrep);
        y(tsep) <- gyp(tvrep);

        currentcell(tfs) <- mycell(tsep);
        currentcell(trs) <- mycell(tsep);
        currentcell(tls) <- mycell(tsep);

      };
                
  flow default
    {
      gxp = gxp(vrep);
      gyp = gyp(vrep);
      xDot = xDot(vehicle);
      yDot = yDot(vehicle);
    };
 
  transition
  dummy -> dummy {vrep:exiting};  // To prevent VREP from exiting
}