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

/********************************************************************\
* File: encoder.hs                                                  *
* Date: 01/15/1998                                                   *
* Author: Cem Unsal                                                  *
*         Robotics Institute, Carnegie-Mellon University             *
*         unsal@ri.cmu.edu                                           *
*                                                                    *
* Description:                                                       *
* Simple dead reckoning sensor using on-board encoder                *
*                                                                    *
* Distance traveled is calculated by a simple model of an encoder.   *
* Measurements are assumed to have a Gaussian noise distribution.    *
* The input to the model is the actual vehicle speed from vehicle    *
* model.                                                             * 
*                                                                    *
* Sensor parameters can be set in this file, or the type 'creating'  *
* the sensor.                                                        *
*                                                                    *
* External C functions: arcsin                                       *
*                                                                    *
* This file is distributed under the conditions described in the     *
* file 'CONDITIONS' which should accompany this file.                *
\********************************************************************/

#ifndef SAHS_ENCODER_HS
#define SAHS_ENCODER_HS

/* External functions */
function arcsin(continuous number x) -> continuous number;

#include <cmu/sensors/clock.hs>
#include <cmu/sensors/noise.hs>

type DR_Encoder
{
  input continuous number speed;   // Wheel speed from vehicle model
                                   // Measured speed can also be used
                                   // In that case, remove noise below

  output continuous number dist;  // Distance travelled 

  state number re;              // estimated/model wheel radius
        continuous number r;    // actual/current wheel radius 
        number m_noise;         // Measurement noise mean and 
        number v_noise;         // variance

        number arrival_bump;    // Random arrival times for bump/cracks
        number arrival_crack;   // on the road
                                // Alternatively, they can be defined in the
                                // highway description files (in Segment);
                                // this will require rxp input to this module

        number bmin, bmax, cmin, cmax;  // Parameters for arrival times
        number hm, hv, wm, wv;  // Mean and variances for height and widths
 
        Gaussian noise;             // Noise generator
        continuous number n1, n2;   // Noise signals

        continuous number t_b, t_c;    // Time

        continuous number thetadot;    
        // theta = revolution (2*pi*count/(count per 2pi))
        
        number w, h, he, md_c, md_b;  // Parameters used in error measurements 

  setup
    define  
    {
      Gaussian tg := create(Gaussian);
    }
    do
    {
      noise := tg;
      m_noise := 3.1415927 / 180 * 5;  // Error mean = 5 degrees
      v_noise := 3.1415927 / 180 * 2;   // Error variance = 2 degrees
     
      re := 0.30; // Estimated wheel radius in m. 

      bmin := 1; cmin := 2;
      bmax := 5; cmax := 6;

      /* Arrival times */
      t_b := 0; t_c := 0;
      arrival_bump := bmin + random() * (bmax-bmin);
      arrival_crack := cmin + random() * (bmax-bmin);
      // Arrival times better be Poisson. 

      /* Bump and crack sizes (mean and variance)*/
      /* Bump */
      hm := 0.10; hv := 0.004; // m
      /* Crack */
      wm := 0.30; wv := 0.02;   //cm
      /* NOTE :  Non-symmetric distributions required for more 
       *         for more realistic sim.
       */
      
    };

  discrete run;

  export crack, bump;

  flow default
    {
      n1 = sg1(noise);
      n2 = sg2(noise);
      t_b' = 1; t_c' = 1;
      r = re - t(timer) * 0.01 / (1*24*60*60);   // dec. 1 cm. per day
      /* Measurement */
      // Displacement
      thetadot = (speed / r) + sqrt(v_noise) * n1 + m_noise; /* ' */
      dist' = thetadot * re;
    };

  transition
    run -> run {bump}
    when t_b > arrival_bump
      define
      {
        number tdist := dist;
      }
      do
      {
        h := sqrt(hv) * n2 + hm;  // Height of the bump 
        md_b := sqrt((2*r - h)*h);
        dist := tdist + 2*(r* arcsin(md_b/r) - md_b);
        t_b := 0;
        arrival_bump := bmin + random() * (bmax-bmin);
      },

    run -> run {crack}
    when t_c > arrival_crack
      define
      {
        number tdist := dist; 
      }
      do
      {
        w := sqrt(wv) * n2 + wm;        // Width of the crack
        he := r - sqrt(4*r*r - w*w)/2;  // Effective height
        md_c := sqrt((2*r-he)*he);
        dist := tdist + 2*(r * arcsin(md_c/r) - md_c);
        t_c := 0;
        arrival_crack := cmin + random() * (bmax-bmin);
      };
}

#endif // SAHS_ENCODER_HS