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

/********************************************************************\
* File: ExVehicle.hs                                                 *
* Date: 01/26/1998                                                   *
* Author: Cem Unsal                                                  *
*         Robotics Institute, Carnegie-Mellon University             *
*         unsal@ri.cmu.edu                                           *
*                                                                    *
* Description:                                                       *
* Example template file for type Vehicle                             *
* Vehicle with speed and lane changing decisions, hdm range sensors, *
* GPS and encoder with noise.                                        *
*                                                                    *
* The following types are submodules:                                *
*  - DriverC (Speed and lane-changing decisions)                     *
*  - Pursuit-point model                                             *
*  - Vehicle_Kinematics (2-d kinematic model)                        *
*  - Controller (2-D vehicle controller)                             *
*  - LeftSensor2, RightSensor2 (Side sensors)                        *
*  - FrontSensor_Rate (Front Sensor)                                 *
*  - PositionSensor_GPS (Simple GPS model)                           *
*  - DR_Encoder (Simple dead-reckoning model)                        *  
*                                                                    *  
* 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/leftsensor2.hs>
#include <cmu/sensors/rightsensor2.hs>
#include <cmu/sensors/frontsensor_rate.hs>
#include <cmu/sensors/gps.hs>
#include <cmu/sensors/encoder.hs>

global set(Vehicle) Vehicles;

type Vehicle
{
  output Vehicle_Kinematics 2dvehicle;
         2DController 2dcontroller;
         Pursuit pursuit;
         PositionSensor_GPS psensor_gps;
         DR_Encoder psenqsor_encoder;
         DriverC driver;
         RightSensor2 rsensor;
         LeftSensor2 lsensor;
         FrontSensor_Rate fsensor;

         Source source;
         VREP   vrep;

         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);

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

        RightSensor2 trsensor := create(RightSensor2,
                                        frange := 25,
                                        brange := 25,
                                        sr := 0.2,
                                        vehicle := self,
                                        mydriver := tdriver);

        LeftSensor tlsensor := create(LeftSensor2,
                                      frange := 25,
                                      brange := 25,
                                      sr := 0.2,
                                      vehicle := self,
                                      mydriver := tdriver)

        FrontSensor_Rate tfsensor := create(FrontSensor_Rate,
                                            range := 100,
                                            sr := 0.1,
                                            vehicle := self,
                                            mydriver := tdriver);

        PositionSensor_GPS tgps := create(PositionSensor_GPS);

        DR_Encoder tencoder := create(DR_Encoder);
      }
    do
      {
        2dvehicle := tvehicle; 
        2dcontroller := tcontroller;
        pursuit := tpursuit;
        driver := tdriver;
        vrep := tvrep;
        rsensor := trsensor;
        lsensor := tlsensor;
        fsensor := tfsensor;
        psensor_gps := tgps;
        psensor_encoder := tencoder;
        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 ca be replaced by orientation(vrep) 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) <- xDot(tvehicle);
        current_hdwy(tdriver) <- distance(tfsensor);
        //?(tdriver) <- dist_rate(tfsensor);
        // minor changes in the driver model is required; the
        // "Delayer tdh" can also be removed if this connection
        // is used.

        sensor_lf(tdriver) <- fdistance(tls);
        sensor_lb(tdriver) <- bdistance(tls);
        sensor_rf(tdriver) <- fdistance(trs);
        sensor_rb(tdriver) <- bdistance(trs);      
        
        gxp(tlsensor) <- gxp(tvrep);
        gyp(tlsensor) <- gyp(tvrep);
        rxp(tlsensor) <- rxp(tvrep);
        lane(tlsensor) <- lane(tvrep);
        section(tlsensor) <- section(tvrep);

        gxp(trsensor) <- gxp(tvrep);
        gyp(trsensor) <- gyp(tvrep);
        rxp(trsensor) <- rxp(tvrep);
        lane(trsensor) <- lane(tvrep);
        section(trsensor) <- section(tvrep);

        gxp(tfsensor) <- gxp(tvrep);
        gyp(tfsensor) <- gyp(tvrep);
        rxp(tfsensor) <- rxp(tvrep);
        lane(tfsensor) <- lane(tvrep);
        section(tfsensor) <- section(tvrep); 
      };
                
  flow default
    {
      gxp = gxp(vrep);
      gyp = gyp(vrep);
      xDot = xDot(vehicle);
      yDot = yDot(vehicle);
    };

  transition
  dummy -> dummy {vrep:exiting};  // To prevent VREP from exiting
}