/* Control program for a small car that do a random walk with
   obstacle avoidance.

   Last updated 13.3.01
 */

#include "RCX_String.h"
#include "RCX_Control.h"
#include "H8.h" 
#include "LCD.h"
#include "Battery.h"
#include "Buttons.h"
#include "Speaker.h"

/* BUG Real Time System with process control*/
#include "BugRTS.h" 

/* Sensors */
#include "InputPorts.h"

/* Motors */
#include "RobotBehaviours.h"

#include "Math.h"


/* Global variable for activation/deactivation of control program,
   initialized and used by Main.
 */
static volatile byte Active;

/* This variable local to the Behaviour Process is made global to 
   report on the LCD from the Main Process.
*/

static byte Behaviour;
static byte TakeAction;

/******* Behaviour  Process **************************************/  

void BehaviourProcess( void )
{         

  forever {
   
    while ( ! Active );

    RobotBehavioursInit(); 

    while (   Active ){

      switch ( Behaviour ) {

      case 1: /* One step in a random walk  */
              switch ( RndBits(2) ) {
                case 0: RobotGoForward(8+RndBits(2)); 
		        if (WaitUntil( &TakeAction, 400 )) TakeAction = FALSE;
			break;
                case 1: RobotTurnClockwise(8+RndBits(2));
                        if (WaitUntil( &TakeAction,200 )) TakeAction = FALSE; 
                        break;
                case 2: RobotTurnCounterClockwise(8+RndBits(2)); 
                        if (WaitUntil( &TakeAction,200 )) TakeAction = FALSE;
                        break;
                case 3: RobotBrake(); 
                        if ( WaitUntil( &TakeAction,400 )) TakeAction = FALSE;
                        break;
              }
              break;
      case 2: /* Avoid Front */
              RobotGoBackward(15);         
              if (WaitUntil( &TakeAction,2000 )) TakeAction = FALSE;
              break;
      case 3: /* Avoid Left */
              RobotGoForwardAndTurn(15,1); 
              if (WaitUntil( &TakeAction,2000 )) TakeAction = FALSE; 
              break;
      case 4: /* Avoid Right */
              RobotGoForwardAndTurn(1,15); 
              if ( WaitUntil( &TakeAction,2000 )) TakeAction = FALSE;
              break;

      default:
              break;
      } /* switch ( Behaviour ) */

      RobotRest();

    } /* while ( Active ) */

  } /* Forever */
}

/******* Sample Process ****************************************/  

void SampleProcess () {
  byte   Front,      Left,      Right;
  uint16 FrontValue, LeftValue, RightValue;
  byte NewBehaviour;
  

  forever {

    /* Sample all input ports */
    LeftValue  = Port1Raw(); 
    FrontValue = Port2Raw(); 
    RightValue = Port3Raw();
    
    /* Interprete values as touch sensors */
    Left  = ( LeftValue  > 1000 ) ? FALSE : TRUE;
    Right = ( RightValue > 1000 ) ? FALSE : TRUE;
    Front = ( FrontValue > 1000 ) ? FALSE : TRUE;
      
    /* Select action based on input */
    if ( Front ) NewBehaviour = 2; 
    else if ( Left  ) NewBehaviour = 3; 
    else if ( Right ) NewBehaviour = 4; 
    else NewBehaviour = 1;

    /* Signal if needed */
    if (NewBehaviour != Behaviour) {
      Behaviour = NewBehaviour;
      if (NewBehaviour!=1) TakeAction = TRUE;
    }
  }
}

/******* Main Process ****************************************/  

void _start( void ) 
{ 

  /* Initialize global variables */
  ButtonsInit();
  SpeakerInit();
  RTSInit();
  TimeCountersResetTime();

  while ( ! Running ) {
    lcd_show_int16(BatteryLevel());
    BusyPauseMS(300);
  }

  Active = TRUE;    
  StartProcess( SampleProcess );
  StartProcess( BehaviourProcess );
  
  forever {

    while ( Running ) {
      lcd_show_int16(RealTime); 
      lcd_show_digit(Behaviour);
    }
    SpeakerPlay(10,10);
    Active = FALSE;

    while ( ! Running ) {
      lcd_show_int16(BatteryLevel()); 
      BusyPauseMS(100);
    }
    SpeakerPlay(1,10);
    Active = TRUE;

  } /* Forever */
}