/* Maintains some extra sensory variables and interprets more commands. */
/* Copyright 1998, Johuco Ltd., All Rights Reserved */
/* Written by Jon Connell, 2/98, Version 1.3 beta   */

/* ====================================================================== */

/* ifndef needed to prevent loops in pseudo-library */

#include <basal.h> 
#ifndef _VIRTUAL_
#include <virtual.h>
#endif

/* ---------------------------------------------------------------------- */

/* private double buffering variables */

int wleft_max0;
int wright_max0;
int nleft_max0;
int nright_max0;
int top_avg0;
int front_avg0;
int pass_stretch0;


/* ---------------------------------------------------------------------- */

/* private function prototypes */

void default_virtual (void);

void copy_turn_bhv (void);
void swap_turn_bhv (void);
void k_turn_bhv (void);
void copy_drive_bhv (void);
void k_drive_bhv (void);

void wleft_copy_bhv (void);
void wleft_max_bhv (void);
void wright_copy_bhv (void);
void wright_max_bhv (void);
void nleft_copy_bhv (void);
void nleft_max_bhv (void);
void nright_copy_bhv (void);
void nright_max_bhv (void);
void top_avg_bhv (void);
void front_avg_bhv (void);
void pass_copy_bhv (void);
void pass_flywheel_bhv (void);


/* ====================================================================== */

/* initialize averages to reasonable values */

void init_virtual ()
{
  wleft_max    = 0;
  wright_max   = 0;
  nleft_max    = 0;
  wright_max   = 0;
  top_avg      = 50;
  front_avg    = 50;
  pass_stretch = 0;
}


/* double buffer sensor vals and set up default commands */

int next_virtual ()
{
  wleft_max0    = wleft_max;
  wright_max0   = wright_max;
  nleft_max0    = nleft_max;
  wright_max0   = nright_max;
  top_avg0      = top_avg;
  front_avg0    = front_avg;
  pass_stretch0 = pass_stretch;
  default_virtual();
  return 1;
}


/* set up default commands for next cycle */

void default_virtual ()
{
  drive_cmd = 0;	
  turn_cmd  = 0;	
}


/* ---------------------------------------------------------------------- */

/* decompose higher level command into more primitive motions */
/* interprets drive_cmd and turn_cmd                          */

void v_actuator_pol ()
{
  /* choose actual wheel direction */
  copy_turn_bhv();	
  swap_turn_bhv();
  k_turn_bhv();
  
  /* choose actual motion direction */
  copy_drive_bhv();
  k_drive_bhv();
}


/* in addition to basic sensors, compute some time averages */
/* supports wleft_max, wright_max, nleft_max, nright_max,   */
/*   top_avg, front_avg, and passive_avg                    */

void v_sensor_pol ()
{
  wleft_copy_bhv();	
  wleft_max_bhv();	
  
  wright_copy_bhv();	
  wright_max_bhv();	
  
  nleft_copy_bhv();	
  nleft_max_bhv();	

  nright_copy_bhv();	
  nright_max_bhv();	

  top_avg_bhv();

  front_avg_bhv();

  pass_copy_bhv();
  pass_flywheel_bhv();
}


/* ---------------------------------------------------------------------- */

/* default action for turn virtual actuator */

void copy_turn_bhv ()
{
  turn_speed = turn_cmd;
}


/* interpret turn relative to vehicle motion not wheel orientation */
/* this can be used as part of a virtual actuator: use turn_cmd    */

void swap_turn_bhv ()
{
  if (drive_cmd < 0)
    turn_speed = -turn_cmd;
}


/* if turn requested but no translation, then alternately turn wheels */

void k_turn_bhv ()
{
  static int tip_mono;

  if ((drive_cmd == 0) && (turn_cmd != 0))
  { 
    update_mono(tip_mono, 24, tip_mono == 0); 
    if (tip_mono > 12)
      turn_speed = -turn_cmd;	  
    else
      turn_speed = turn_cmd;
  }
  else
    tip_mono = 0;	  
}


/* default action for drive virtual actuator */

void copy_drive_bhv ()
{
  drive_speed = drive_cmd;
}


/* if rotation requested but no translation, then jitter back and forth */

void k_drive_bhv ()
{
  static int jit_mono;

  if ((drive_cmd == 0) && (turn_cmd != 0))
  { 
    update_mono(jit_mono, 24, jit_mono == 0); 
    if (jit_mono > 12)
      drive_speed = -100;	    
    else
      drive_speed = 100;
  }
  else
    jit_mono = 0;	  
}


/* ---------------------------------------------------------------------- */

/* normally just copy value */

void wleft_copy_bhv ()
{
  wleft_max = wleft_val;
}


/* track wide left IR maximum, slow decay to current level */

void wleft_max_bhv ()
{
  if (wleft_val < wleft_max0)	
    wleft_max = (wleft_max0 + wleft_val) >> 1;	  
}


/* normally just copy value */

void wright_copy_bhv ()
{
  wright_max = wright_val;
}


/* track wide right IR maximum, slow decay to current level */

void wright_max_bhv ()
{
  if (wright_val < wright_max0)	
    wright_max = (wright_max0 + wright_val) >> 1;	  
}


/* normally just copy value */

void nleft_copy_bhv ()
{
  nleft_max = nleft_val;
}


/* track narrow left IR maximum, slow decay to current level */

void nleft_max_bhv ()
{
  if (nleft_val < nleft_max0)	
    nleft_max = (nleft_max0 + nleft_val) >> 1;	  
}


/* normally just copy value */

void nright_copy_bhv ()
{
  nright_max = nright_val;
}


/* track narrow right IR maximum, slow decay to current level */

void nright_max_bhv ()
{
  if (nright_val < nright_max0)	
    nright_max = (nright_max0 + nright_val) >> 1;	  
}


/* take moderately long time average of top photocell value */

void top_avg_bhv ()
{
  top_avg = (7 * top_avg0 + top_level) >> 3;	
}


/* take moderately long time average of front photocell value */

void front_avg_bhv ()
{
  front_avg = (7 * front_avg0 + front_level) >> 3;	
}


/* normally just copy the passive IR directional information */

void pass_copy_bhv ()
{
  pass_stretch = passive_vals;
}


/* retain directional information for one cycle for slow pulsers */

void pass_flywheel_bhv ()
{
  static int valid_mono;
  
  update_mono(valid_mono, 2, passive_vals != 0) 
  if (valid_mono && (passive_vals == 0))
    pass_stretch = pass_stretch0;
}


/* ==================================================================== */