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

/********************************************************************\
* File:   humandrivers.hs                                            *
* Date:   12/17/1997                                                 *
* Author: Cem Unsal                                                  *
*         Robotics Institute, Carnegie-Mellon University             *
*         unsal@ri.cmu.edu                                           *
*                                                                    *
* Description:                                                       *
* Four different driver models                                       *
*                                                                    *
* Driver0 : Driver   Follows of predefined trajectory, keeps headway *
* DriverA : Driver   Human driver model A. Keeps headway, and        *
*                    takes random actions every actiontime sec.      *
* DriverB : Driver   Human driver model B. Makes decisions for lane  *
*                    changes. Complex headway algorithm.             *
* DriverC : Driver   Human driver model C. Make decisions for lane   *
*                    changes. Complex headway algorithm, lane        *
*                    changes based on timed flags.                   *
*                                                                    *
* Model parameters can be set in this file, or the type 'creating'   *
* the driver type.                                                   *
*                                                                    *
* External C functions: <none>                                       *
*                                                                    *
* This file is distributed under the conditions described in the     *
* file 'CONDITIONS' which should accompany this file.                *
\********************************************************************/

#ifndef SAHS_DRIVER_HS
#define SAHS_DRIVER_HS

type Driver
{
  input number current_spd;           // Current vehicle speed
        number current_hdwy;          // Current headway
        number sensor_lf, sensor_lb;  // Side sensor values
        number sensor_rf, sensor_rb;

  output continuous number target_spd;  // Computed target speed
         continuous number target_lyp;  // Computed lateral lane command
}

#endif // SAHS_DRIVER_HS


/********************************************************************\
* Driver0                                                            *
*                                                                    *
* Description:                                                       *
* Simple human driver following a predefined trajectory.             *
*                                                                    *
* Comes to a stop during a predefined time interval. Used to test    *
* other driver models. Other trajectory definitions are easily       *
* obtainable with minor changes.                                     *
\********************************************************************/

#ifndef SAHS_DRIVER_0_HS
#define SAHS_DRIVER_0_HS

type Driver0 : Driver
{
  input number current_spd;
        number current_hdwy;
        number sensor_lf, sensor_lb;
        number sensor_rf, sensor_rb;

  output continuous number target_spd;
	 continuous number target_lyp :=0; 

  state VREP myvrep;               // Associated VREP
        /* Speed-related */
        number ad;                 // Rate of change of target speed
        number amean, adev, freq;  // Mean, std. dev. and freq. for acc. profile
        /* Headway-related */
        number hlim;               // Limiting value (headway) for transition
        /* Timers */
        number t1, t2;             // Time interval for stop
        /* Other */
        number PI := 3.1415927;    
        number ge := 9.81;
        
  setup
    define
    {
      number v := 18;
    }
    do
    {
      target_spd := v;
      amean := 0 * ge; adev := 0.1 * ge;
      freq := 4;
      hlim := 50 + 5; ad := 0.3 * ge;
      t1 := 15; t2 := 15+6;
    };
        
  discrete
    cruise 
    {
      // Sinuziodal accelaration profile
      target_spd' = amean + adev * cos(2* PI /freq * t(time));
    }, 

    detect
    {
      // Slow down if Vehicle in front
      target_spd' = if target_spd > 4 then -ad else 0;
    },
                
    stop 
    ;
        
  transition
    cruise -> detect {}
    when current_hdwy < hlim
    ,
        
    detect -> cruise {}
    when current_hdwy >= hlim
      do
      {
        target_spd := 18;
      },

    detect -> stop {}
    when t(time) > t1 and t(time) < t2
      do
      { 
        target_spd := 0;
      },
 
    cruise -> stop {}
    when t(time) > t1 and t(time) < t2
      do
      {
            target_spd := 0;
      },

    stop -> cruise {}
    when t(time) >= t2
      do
      {
        target_spd := 18;
      };        
}

#endif // SAHS_DRIVER_0_HS


/********************************************************************\
* DriverA                                                            *
*                                                                    *
* Description:                                                       *
* Simple human driver model with the following discrete states:      *
*                                                                    *
* - follow:       Adjust target speed to keep desired headway        *
                  distance, a function of the current vehicle speed. *
* - stop:         If headway is less than a certain distance, target *
*                 speed is zero.                                     *
* - cruise:       If current headway distance is greater than a      *
*                 predefined value, adjust speed to desired speed.   *
* - change_left : Based on the randomly chosen action, either        *
* - change_right  (a) change to left lane                            *
*                 (b) change to right lane                           *
*                 (c) randomly change speed                          *
\********************************************************************/


#ifndef SAHS_DRIVER_A_HS
#define SAHS_DRIVER_A_HS

type DriverA : Driver
{
  input number current_spd;
        number current_hdwy;
        number sensor_lf, sensor_lb;
        number sensor_rf, sensor_rb;

  output continuous number target_spd;
         continuous number target_lyp; 
         continuous number des_hdwy;

  state VREP myvrep;
        /* Speed-related */              // Associated VREP
        continuous number speed_change;  // Evaluated change in speed
        continuous number speed_change2; // Input to the limiter
        number max_speed;                // Maximum possible speed
        number aa, dd;                   // Acc/Dec values for follow mode
        number ad;                       // Prop. gain for tracking cruise_speed
        number cruise_speed;             // Desired speed for suise mode
        /* Headway-related */
        number min_hdwy;            // Headway distance limit for transition
        number des_hdwy_limp;       // Parameters defining the comfort region
        number des_hdwy_limn;
        number hlim;                // Limiting value of headway for transition
        /* Other */
        continuous number t;        // Time
        number lwi := 4;            // Lane width
        continuous number action;   // Action ID
        number actiontime;          // Random time interval for actions

  setup
    define
    {
      number tv := 20; // or 19 + (4 * random() - 2);
    }
    do
    {
      target_lyp := 0;
      target_spd := tv;
                
      speed_change := 0; speed_change2 := 0;
      max_speed := 30; /* 108 kmh, 67.5 mph */
                
      des_hdwy := 35+5; des_hdwy_limp := 3; des_hdwy_limn := 3;
      aa := 0.2 * 9.81; dd := 0.5 * 9.81;

      cruise_speed := tv; ad := 0.1 * 9.81;

      hlim :=80+5; min_hdwy := 10+5;

      actiontime := 5 + 4*random(); t := 0;
    };

  export new_lane;

  discrete
    cruise 
    {
      target_spd' = ad * (cruise_speed - target_spd); /* ' */
    }, 
        
    follow 
    {
      des_hdwy = current_spd * 2.5 + 5;
        
      speed_change2 = if current_hdwy > des_hdwy - des_hdwy_limn and
                         current_hdwy < des_hdwy + des_hdwy_limp
                      then 0
                      else if current_hdwy <= des_hdwy - des_hdwy_limn
                           then -dd
                           else aa;
              
       speed_change = if target_spd > max_speed 
                      then min(speed_change2, 0)
                      else if target_spd <= 1 // Approx. zero; 
                                              // Takes delay into account
                           then max(speed_change2, 0)
                           else speed_change2;
        
       target_spd' = speed_change;
    }, 

    stop, 
    
    change_left, change_right;

  flow default 
    {
      t' = 1;
      action = floor (4 * random());
    };

  transition
    cruise -> stop {}
    when current_hdwy <= min_hdwy
      do 
      {
        target_spd := 0;
      },
          
    follow -> stop {}
    when current_hdwy <= min_hdwy
      do
      {
        target_spd := 0;
      },
        
    stop -> follow {}
    when current_hdwy > min_hdwy * 1.5
    ,
        
    cruise -> follow {}
    when current_hdwy < hlim
    ,
        
    follow -> cruise {}
    when current_hdwy >= hlim
    ,
        
    follow -> change_left {}
    when t >= actiontime 
         and
         action < 1
         and
         sensor_lb = -1 and sensor_lf = -1
      do
      {
        target_lyp := -lwi;
        t := 0;            
      },
        
    follow -> change_right {}
    when t >= actiontime
         and
         action >= 1 and action < 2
         and
         sensor_rb = -1 and sensor_rf = -1
      do
      {
        target_lyp := lwi; 
        t := 0;            
      },
        
    cruise -> change_left {}
    when t >= actiontime 
         and
         action < 1 
         and
         sensor_lb = -1 and sensor_lf = -1
      do
      {
        target_lyp := -lwi;
        t := 0;            
      },
        
    cruise -> change_right {}
    when t >= actiontime
         and
         action >= 1 and action < 2
         and
         sensor_rb = -1 and sensor_rf = -1
      do
      {
        target_lyp := lwi; 
        t := 0;            
      },
        
   cruise -> cruise {}
   when t >= actiontime
        and
        action >= 2
     do  
     {
       cruise_speed := min(cruise_speed + (4* random() -2), max_speed);
       t := 0;
     },
        
   change_left -> cruise {myvrep:updateLaneLeft, new_lane}
     do
     {
       target_lyp := 0;
     },
        
   change_right -> cruise {myvrep:updateLaneRight, new_lane}
     do
     {
       target_lyp := 0;
     };
}

#endif // SAHS_DRIVER_A_HS


/********************************************************************\
* DriverB                                                            *
*                                                                    *
* Description:                                                       *
* Human driver model with the following discrete states:             *
*                                                                    *
* - follow:       Adjust target speed to keep desired headway        *
                  distance, a function of the current vehicle speed. *
* - stop:         If headway is less than a certain distance, target *
*                 speed is zero.                                     *
* - cruise:       If current headway distance is greater than a      *
*                 predefined value, adjust speed to desired speed.   *
* - change_left : Depending on the headway distance and/or its rate  *
* - change_righ   of change, either:                                 *
*                 (a) change to left lane                            *
*                 (b) change to right lane                           *
\********************************************************************/

#ifndef SAHS_DRIVER_B_HS
#define SAHS_DERIVER_B_HS

#include <smart-ahs/delayer.hs>

type DriverB : Driver
{
  input number current_spd;
        number current_hdwy;
        number sensor_lf, sensor_lb;
        number sensor_rf, sensor_rb;

  output continuous number target_spd;
         continuous number target_lyp;
         continuous number des_hdwy;

  state VREP myvrep;                     // Associated VREP
        Delayer delay_hdwy;              // Delayer for headway measurment
        Delayer delay_cmd, delay_cmd2;   // Delayers for speed commands
        /* Speed-related */
        continuous number speed_change;  // Evaluated change in speed
        continuous number speed_change2; // Input to the limiter 
        continuous number target_speed;  // Calculated target speed before delay
        number max_speed;                // Maximum possible speed
        number aa, dd;                   // Acc./Dec. values for follow mode
        number ad;                       // Prop. gain for tracking cruise_speed
        number cruise_speed;             // Desired speed for cruise mode
        number cmd_delay := 0.4;         // Command delay times
        number cmd_delay2 := 0.2;        // for different follow modes         
        /* Headway-related */
        continuous number delayed_hdwy;  // Delayed headway measurement
        continuous number hdwy_rate;     // Rate of change in headway
        number hdwy_delay_t := 0.1;      // Delay time
        number min_hdwy;                 // Headway limits for transitions
        number min_hdwy2;                 
        number des_hdwy_lim1p,des_hdwy_lim2p; // Param.s defining the confort
        number des_hdwy_lim1n,des_hdwy_lim2n; // region around the des_hdwy
        number hlim, hlim_delay;              // Limiting values of headway
                                              // distance for transitions 
        continuous number lane_shift_hdwy;    // parameter changes
        number lane_shift_hrate;              // Limiting rates of change in
        number lane_shift_hrate2;             // headway for lane change
        /* Other */
        number lwi := 4;                      // Lane width

  setup
    define
    {
      Delayer tdh := create(Delayer, 
                            delay := hdwy_delay_t, 
                            sample_period := 0.1, 
                            delayed_variable := 80); 
      // 80 is the initial hdwy for 1st few sec. 
      Delayer tdc := create(Delayer,
                            delay := cmd_delay,
                            sample_period := 0.2,
                            delayed_variable := 15);  
      // 15 is the initial target speed
      Delayer tdc2 := create(Delayer,
                             delay := cmd_delay2,
                             sample_period := 0.2,
                             delayed_variable := 15); 
      // Random desired cruise speed
      number tv := 20 + (3 * random() - 1.5);  
    }
    do
    {
      delay_hdwy := tdh; delay_cmd := tdc; delay_cmd2 := tdc2;
        
      target_lyp := 0; target_spd := tv; target_speed := tv;

      speed_change := 0; speed_change2 := 0;
      max_speed := 30; /* 108kmh, 67.5mph */
              
      des_hdwy := 35+5;
      des_hdwy_lim1p := 3; des_hdwy_lim2p := 3;
      des_hdwy_lim1n := 3; des_hdwy_lim2n := 5;
      aa := 0.3 * 9.81; dd := 0.5 * 9.81;
        
      cruise_speed := tv; ad := 0.2 * 9.81;
        
      hlim :=80+5; hlim_delay := 40+5; min_hdwy2 := 6+5; min_hdwy := 3+5;
        
      lane_shift_hdwy := 15+5; lane_shift_hrate := -3; lane_shift_hrate2 := -8;
    }
    connect
    {
      variable(tdh) <- current_hdwy;
      variable(tdc) <- target_speed;
      variable(tdc2) <- target_speed;
    };
        
  export new_lane;

  discrete 
    cruise 
    {
      target_speed' = ad * (cruise_speed - target_speed);
    }, 
                
  follow 
    {
      des_hdwy = current_spd * 2 + 5;

      speed_change2 = if current_hdwy < des_hdwy + des_hdwy_lim1p and
                         current_hdwy > des_hdwy + des_hdwy_lim1n
                      then 0
                      else if current_hdwy > des_hdwy + des_hdwy_lim2p
                           then aa
                           else if current_hdwy < des_hdwy - des_hdwy_lim2n
                                then -dd
                                else if current_hdwy < des_hdwy + des_hdwy_lim2p
                                        and
                                        current_hdwy > des_hdwy + des_hdwy_lim1p
                                     then if hdwy_rate > 0
                                          then aa
                                          else 0
                                     else  /* if current_hdwy>des_hwdy-lim2p and
                                              current_hdwy<des_hdwy-lim1p */
                                           if hdwy_rate < 0
                                           then -dd
                                           else 0;
              
      speed_change = if target_speed > max_speed 
                     then min(speed_change2, 0)
                     else if target_speed <= 1 // Aapprox. zero;
                                               // Takes delay into account
                          then max(speed_change2, 0)
                          else speed_change2;

      target_speed' = speed_change; /* ' */
    }, 

  followc 
    {
      des_hdwy = current_spd * 2 + 5; // 2 sec headway
         
      speed_change2 = if current_hdwy < des_hdwy + des_hdwy_lim1p and
                         current_hdwy > des_hdwy + des_hdwy_lim1n
                      then 0
                      else if current_hdwy > des_hdwy + des_hdwy_lim2p
                           then aa
                           else if current_hdwy < des_hdwy - des_hdwy_lim2n
                                then -dd
                                else if current_hdwy < des_hdwy + des_hdwy_lim2p
                                        and
                                        current_hdwy > des_hdwy + des_hdwy_lim1p
                                     then if hdwy_rate > 0
                                          then aa
                                          else 0
                                     else  /* if current_hdwy>des_hwdy-lim2p and
                                              current_hdwy<des_hdwy-lim1p */
                                           if hdwy_rate < 0
                                           then -dd
                                           else 0;
              
      speed_change = if target_speed > max_speed 
                     then min(speed_change2, 0)
                     else if target_speed <= 1 // Approx. zero;
                                               // Takes delay into account
                           then max(speed_change2, 0)
                           else speed_change2;
        
      target_speed' = speed_change; /* ' */
    }, 

    stop,

    change_left, change_right
    ;

  flow default
    {
      target_spd = if current_hdwy > hlim_delay
                   then delayed_variable(delay_cmd)
                   else delayed_variable(delay_cmd2);
      lane_shift_hdwy = if current_hdwy < hlim_delay
                        then current_spd/2 + 1 + 5
                        else current_spd + 5;

      delayed_hdwy = delayed_variable(delay_hdwy);
      hdwy_rate = (current_hdwy - delayed_hdwy)/hdwy_delay_t;
    };

  transition
    cruise -> stop {}
    when current_hdwy <= min_hdwy2
      do 
      {
        target_speed := 0;
      },
          
    follow -> stop {}
    when current_hdwy <= min_hdwy
      do
      {
        target_speed := 0;
      },

    followc -> stop {}
    when (current_hdwy <= min_hdwy and hdwy_rate <= 0)
         or current_hdwy <= min_hdwy
      do
      {
        target_speed := 0;
      },

    stop -> followc {}
    when current_hdwy > min_hdwy and hdwy_rate > 0
      do
      {
        aa := 0.2 * 9.81; dd := 1 * 9.81;
      },

    follow -> followc {}
    when current_hdwy <= hlim_delay
      do
      {
        aa := 0.2 * 9.81; dd := 1 * 9.81; 
      },
    
    followc -> follow {}
    when current_hdwy > hlim_delay
      do
      {
        aa := 0.5*9.81; dd := 0.5*9.81; 
      },
        
    cruise -> follow {}    
    when current_hdwy < hlim
    ,

    follow -> cruise {}
    when current_hdwy >= hlim
    ,

    follow -> change_left {}
    when ((current_hdwy < lane_shift_hdwy and hdwy_rate < lane_shift_hrate)
           or
           hdwy_rate < lane_shift_hrate2
          ) and
          sensor_lb = -1 and sensor_lf = -1
      do
      {
        target_lyp := -lwi;            
      },

    follow -> change_right {}
    when ((current_hdwy < lane_shift_hdwy and hdwy_rate < lane_shift_hrate)
          or
          hdwy_rate < lane_shift_hrate2 
         ) and
         sensor_rb = -1 and sensor_rf = -1
      do
      {
        target_lyp := lwi;                     
      },

    followc -> change_left {}
    when ((current_hdwy < lane_shift_hdwy and hdwy_rate < lane_shift_hrate)
          or
          hdwy_rate < lane_shift_hrate2
         ) and
         sensor_lb = -1 and sensor_lf = -1
      do
      {
        target_lyp := -lwi;            
      },

    followc -> change_right {}
    when ((current_hdwy < lane_shift_hdwy and hdwy_rate < lane_shift_hrate)
          or
          hdwy_rate < lane_shift_hrate2 
         ) and
         sensor_rb = -1 and sensor_rf = -1
      do        
      {
        target_lyp := lwi;                    
      },

    change_left -> cruise {myvrep:updateLaneLeft, new_lane}
      do
      {
        target_lyp := 0;
      },
      
    change_right -> cruise {myvrep:updateLaneRight, new_lane}
      do
      {
         target_lyp := 0;
      };
}

#endif // SAHS_DRIVER_B_HS


/********************************************************************\
* DriverC                                                            *
*                                                                    *
* Description:                                                       *
* Human driver model with the following discrete states:             *
*                                                                    *
* - follow:       Adjust target speed to keep desired headway        *
                  distance, a function of the current vehicle speed. *
* - stop:         If headway is less than a certain distance, target *
*                 speed is zero.                                     *
* - cruise:       If current headway distance is greater than a      *
*                 predefined value, adjust speed to desired speed.   *
* - change_left : Depending on the flag state for lane change,       *
* - change_righ   either:                                            *
*                 (a) change to left lane                            *
*                 (b) change to right lane                           *
\********************************************************************/

#ifndef SAHS_DRIVER_C_HS
#define SAHS_DRIVER_C_HS

#include <smart-ahs/delayer.hs>

type DriverC : Driver
{
  input number current_spd;
        number current_hdwy;
        number sensor_lf, sensor_lb,
               sensor_rf, sensor_rb;

  output continuous number target_spd;
         continuous number target_lyp;
         continuous number des_hdwy; // For info.

  state VREP myvrep;                     // Associated VREP
        Delayer delay_hdwy;              // Delayer for headway measurement
        Delayer delay_cmd, delay_cmd2;   // Delayer for speed commands
        /* Speed-related */
        continuous number target_speed;  // Calculated target speed before delay
        continuous number speed_change;  // Evaluated change in speed
        continuous number speed_change2; // Input to the limiter
        number max_speed;                // Maximum possible speed
        number aa, dd;                   // Acc./Dec. values for follow mode
        number ad;                       // Prop. gain for tracking cruise_speed
        number cruise_speed;             // Desired speed for cruise mode
        number cmd_delay := 0.4;         // Command delay times
        number cmd_delay2 := 0.2;        // for different follow modes
        /* Headway-related */
        continuous number delayed_hdwy;  
        continuous number hdwy_rate;     // Rate of change in headway
        number hdwy_delay_t := 0.1;      // Delay time
        number min_hdwy, min_hdwy2;      // Headway limits for transitions
        number des_hdwy_lim1p,des_hdwy_lim2p;  // Param.s defining the confort
        number  des_hdwy_lim1n,des_hdwy_lim2n; // region around the des_hdwy
        number hlim, hlim_delay;               // Limiting values of headway
                                               // distance for transitions,
        continuous number lane_shift_hdwy;     // parameter changes  
        /* Lane-shift-related */
        number lsflagt, lsflagt2;    // Intervals for lane shift, speed dec.
        continuous number lsft;      // Time since last transiton to 'follow'
        number lsfspeedlim;          // Acceptable diff. beyond which timer runs
        /* Other */
        number lwi := 4;             // Lane width

  setup
    define
    {
      Delayer tdh := create(Delayer, 
                            delay := hdwy_delay_t, 
                            sample_period := 0.1, 
                            delayed_variable := 80);  
      // 80 is the initial hdwy. for 1st few sec. 
      Delayer tdc := create(Delayer,
                            delay := cmd_delay,
                            sample_period := 0.2,
                            delayed_variable := 15);  
      // 15 is the initial target speed
      Delayer tdc2 := create(Delayer,
                             delay := cmd_delay2,
                             sample_period := 0.2,
                             delayed_variable := 15);
      // Ransom desired cruise speed
      number tv := 20 + (3 * random() - 1.5);
    }
    do
    {
      delay_hdwy := tdh; delay_cmd := tdc; delay_cmd2 := tdc2;

      target_lyp := 0; target_spd := tv; target_speed := tv;

      speed_change := 0; speed_change2 := 0;
      max_speed := 30; /* 108kmh, 67.5mph */
      
      des_hdwy := 35+5; /* initial value */
      des_hdwy_lim1p := 3; des_hdwy_lim2p := 3;
      des_hdwy_lim1n := 3; des_hdwy_lim2n := 5;
      aa := 0.3 * 9.81; dd := 0.5 * 9.81;

      cruise_speed := tv; ad := 0.2 * 9.81;

      hlim :=80+5; hlim_delay := 40+5;
      min_hdwy2 := 6+5; min_hdwy := 3+5;

      lane_shift_hdwy := 15+5;

      lsft := 0; lsflagt := 2; lsflagt2 := 5; lsfspeedlim := 2;
    }
    connect
    {
      variable(tdh) <- current_hdwy;
      variable(tdc) <- target_speed;
      variable(tdc2) <- target_speed;
    };

  export new_lane;

  discrete 
    cruise 
    {
      target_speed' = ad * (cruise_speed - target_speed);
    }, 

    follow 
    {
      des_hdwy = if lsft < lsflagt2 
                 then current_spd * 2 + 5
                 else current_spd * 4 + 5; // Forces the vehicle to slow down

      speed_change2 = if current_hdwy < des_hdwy + des_hdwy_lim1p and
                         current_hdwy > des_hdwy + des_hdwy_lim1n
                      then 0
                      else if current_hdwy > des_hdwy + des_hdwy_lim2p
                           then aa
                           else if current_hdwy < des_hdwy - des_hdwy_lim2n
                                then -dd
                                else if current_hdwy < des_hdwy + des_hdwy_lim2p
                                        and
                                        current_hdwy > des_hdwy + des_hdwy_lim1p
                                     then if hdwy_rate > 0
                                          then aa
                                          else 0
                                     else  /* if current_hdwy>des_hwdy-lim2p and
                                              current_hdwy<des_hdwy-lim1p */
                                           if hdwy_rate < 0
                                           then -dd
                                           else 0;
      
      speed_change = if target_speed > max_speed 
                     then min(speed_change2, 0)
                     else if target_speed <= 1 // Approx. zero;
                                               // Take delay into account
                          then max(speed_change2, 0)
                          else speed_change2;

      target_speed' = speed_change;

      lsft' = if target_spd - cruise_speed < - lsfspeedlim 
              then 0.5
              else 0;
      /* Twice as much time for the flag than the "followc" mode '*/
    }, 

 followc 
    {
      des_hdwy = if lsft < lsflagt2 
                 then current_spd * 2 + 5   // 2-sec headway 
                 else current_spd * 4 + 5;  // Forces vehicle to slow down 
 
      speed_change2 = if current_hdwy < des_hdwy + des_hdwy_lim1p and
                         current_hdwy > des_hdwy + des_hdwy_lim1n
                      then 0
                      else if current_hdwy > des_hdwy + des_hdwy_lim2p
                           then aa
                           else if current_hdwy < des_hdwy - des_hdwy_lim2n
                                then -dd
                                else if current_hdwy < des_hdwy + des_hdwy_lim2p
                                        and
                                        current_hdwy > des_hdwy + des_hdwy_lim1p
                                     then if hdwy_rate > 0
                                          then aa
                                          else 0
                                      else  /* if current_hdwy>des_hwdy-lim2p and
                                              current_hdwy<des_hdwy-lim1p */
                                           if hdwy_rate < 0
                                           then -dd
                                           else 0;
      
      speed_change = if target_speed > max_speed 
                     then min(speed_change2, 0)
                     else if target_speed <= 1 // Approx. zero;
                                               // takes delay into account
                          then max(speed_change2, 0)
                          else speed_change2;

      target_speed' = speed_change;

      lsft' = if target_spd - cruise_speed < - lsfspeedlim 
             then 1
             else 0;
    }, 

    stop,

    change_left, change_right
    ;

  flow
    default
    {
      target_spd = if current_hdwy > hlim_delay
                   then delayed_variable(delay_cmd)
                   else delayed_variable(delay_cmd2);
      lane_shift_hdwy = if current_hdwy < hlim_delay
                        then current_spd/2 + 1 + 5
                        else current_spd + 5;
      delayed_hdwy = delayed_variable(delay_hdwy);
      hdwy_rate = (current_hdwy - delayed_hdwy)/hdwy_delay_t;
    };

  transition
    cruise -> stop {}
    when current_hdwy <= min_hdwy2
      do 
      {
        target_speed := 0;
      },
  
    follow -> stop {}
    when current_hdwy <= min_hdwy
      do
      {
        target_speed := 0;
      },

    followc -> stop {}
    when (current_hdwy <= min_hdwy and hdwy_rate <= 0)
         or current_hdwy <= min_hdwy2
      do
      {
        target_speed := 0;
      },

    stop -> followc {}
    when current_hdwy > min_hdwy and hdwy_rate > 0
      do
      {
        aa := 0.2 * 9.81; dd := 1 * 9.81;
      },

    follow -> followc {}
    when current_hdwy <= hlim_delay
      do
      {
        aa := 0.2 * 9.81; dd := 1 * 9.81; 
      },
    
    followc -> follow {}
    when current_hdwy > hlim_delay
      do
      { 
        aa := 0.5*9.81; dd := 0.5*9.81; 
      },

    cruise -> follow {}
    when current_hdwy < hlim
    ,

    follow -> cruise {}
    when current_hdwy >= hlim
    ,

    follow -> change_left {}
    when (lsft >= lsflagt or current_hdwy < lane_shift_hdwy)
         and
         sensor_lb = -1 and sensor_lf = -1
      do
      {
        target_lyp := -lwi;
        lsft := 0;
      },

    follow -> change_right {}
    when (lsft >= lsflagt or current_hdwy < lane_shift_hdwy)
         and
         sensor_rb = -1 and sensor_rf = -1
      do
      {
        target_lyp := lwi; 
        lsft := 0;            
      },

    followc -> change_left {}
    when (lsft >= lsflagt or current_hdwy < lane_shift_hdwy)
         and
         sensor_lb = -1 and sensor_lf = -1
      do
      {
        target_lyp := -lwi;
        lsft := 0;                    
      },

    followc -> change_right {}
    when (lsft >= lsflagt or current_hdwy < lane_shift_hdwy)
         and
         sensor_rb = -1 and sensor_rf = -1
      do
      {
        target_lyp := lwi;
        lsft := 0;            
      },

    change_left -> cruise {myvrep:updateLaneLeft, new_lane}
      do
      {
        target_lyp := 0;
      },

    change_right -> cruise {myvrep:updateLaneRight, new_lane}
      do
      {
        target_lyp := 0;
      };
}

#endif // SAHS_DRIVER_C_HS