/* Control program for a small car that do a random walk with
   obstacle avoidance. The behaviours is accompanied by sounds.

   Last updated 16.6.00
 */

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


#include "BugRTS.h" 

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

/* Actuators */
#include "RobotBehaviours.h"
#include "RobotMusic.h"

#include "Math.h"


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

/* A global variable BehaviourBreak is polled to make sure 
   the current behaviour stops when requested
   from the outside control of the bug. 
 */
static  byte                            BehaviourBreak;

/* Global variables for control of control program processes, 
   initialized and used by Action Selection process.
 */
static volatile byte BehaviourActive;
static volatile byte MusicActive;

static byte          Behaviour;    /* Index of Behaviour selected. */
static semaphore     NewBehaviour; /* Signal to Behaviour Process. */

static byte          Music;        /* Index of Music to play.      */
static semaphore     NewMusic;     /* Signal to Music Process.     */

static byte          Avoiding;


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

void BehaviourProcess( void )
{         

  forever {

    while ( ! BehaviourActive ); 

    RobotBehavioursInit(); 

    while (   BehaviourActive ){
      
      Wait( & NewBehaviour ); 
      BehaviourBreak = FALSE; 

      switch ( Behaviour ) {

      case 0: /* No behaviour */
              break;
      case 1: /* Random walk  */
	      while ( ! BehaviourBreak ) {
                switch ( RndBits(2) ) {
                case 0: RobotGoForward(8+RndBits(2)); 
                        break;
                case 1: RobotTurnClockwise(8+RndBits(2)); 
                        break;
                case 2: RobotTurnCounterClockwise(8+RndBits(2)); 
                        break;
                case 3: RobotBrake(); 
                        break;
                }
                WaitUntil( & BehaviourBreak, 128);
              }
              break;
      case 2: /* Avoid Front */
              Avoiding = TRUE;	      
              RobotGoBackward(15);         
              WaitUntil( & BehaviourBreak, 1000);
	      Avoiding = FALSE;
              break;
      case 3: /* Avoid Left */
              Avoiding = TRUE;              
              RobotGoForwardAndTurn(12,2); 
              WaitUntil( & BehaviourBreak, 1000); 
	      Avoiding = FALSE;
              break;
      case 4: /* Avoid Right */
              Avoiding = TRUE;	      
              RobotGoForwardAndTurn(2,12); 
              WaitUntil( & BehaviourBreak, 1000);
	      Avoiding = FALSE;
              break;

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

      RobotRest();

    } /* while ( BehaviourActive ) */
    
    RobotBehavioursClose(); 

  } /* Forever */ 
}

/******* Music Process ***************************************************/  

void MusicProcess (void)
{   

  uint16 times, i;

  forever {

    while ( ! MusicActive );  

    RobotMusicInit();

    while (   MusicActive  ){

      Wait( & NewMusic);
      MusicBreak = FALSE;


      switch ( Music ) {

      case 0: /* No music */
              break;
      case 1: /* Random music  */
	      while ( ! MusicBreak ) {
	         if ( Rnd(29) < 28 ) {
                    Play( fSharp5 + RndBits(12),64,64,2 + RndBits(3));
                 } 
                 else {
                    times = Pick(1,10,1);
                    switch ( Pick(1,5,1) ) {
                    case 1: for ( i=1 ; i < times; i++){
                              Play(c3,32,32,5);
                              Play(dSharp3,32,32,5);
		            }
                            break;
                    case 2: for ( i=1 ; i < times; i++){
                              Play(gSharp3,32,32,5);
                              Play(c4,32,32,5);
		            }
                            break;
                    case 3: for ( i=1 ; i < times; i++){
                              Play(fSharp3,32,32,5);
                              Play(gSharp3,32,32,5);
		            }
                            break;
                    case 4: for ( i=1 ; i < times; i++){
                              Play(cSharp3,32,32,5);
                              Play(dSharp3,32,32,5);
		            }
                            break;
                    case 5: for ( i=1 ; i < times; i++){
                              Play(gSharp2,32,32,5);
                              Play(aSharp2,32,32,5);
		            }
                            break; 
                    } 
                 }
              } /* while */   
              break;
      case 2: /* Avoid Front */
	      PlayDutyGlissade(aSharp5 + RndBits(10),2,30,1000);
              break;
      case 3: /* Avoid Left */
	      PlayFreqGlissade(fSharp5+RndBits(10),fSharp3,2+RndBits(2),1000);
              break;
      case 4: /* Avoid Right */
	      PlayFreqGlissade(fSharp3+RndBits(10),fSharp5,2+RndBits(2),1000);
              break;

      default:
              break;

      } /* switch ( Music ) */
      
      PortSpeakerStopPlaying();

    } /* while ( MusicActive ) */

    RobotMusicClose();

  } /* Forever */ 
}



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

void SelectBehaviour( uint16 behaviour )
{       
  Behaviour = behaviour;
  BehaviourBreak = TRUE;
  Signal( & NewBehaviour );

  MusicBreak = TRUE;
  Music = Behaviour;
  Signal( & NewMusic );
}


void BehaviourSelection( void )
{

  byte   Front,      Left,      Right;
  uint16 FrontValue, LeftValue, RightValue;
  uint16 CurrentBehaviour;

  BehaviourActive = FALSE;            MusicActive = FALSE;
  StartProcess( BehaviourProcess  );  StartProcess( MusicProcess      );

  forever {
   
    while ( ! Active );

    Behaviour       = 0;     Music        = 0;    
    NewBehaviour    = FALSE; NewMusic     = FALSE;
    BehaviourActive = TRUE;  MusicActive  = TRUE;

    Avoiding = FALSE;
    
    while ( Active ){

      CurrentBehaviour = Behaviour;

      /* 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 and state of car */
      if ( Front ) SelectBehaviour(2); 
      else
      if ( Left  ) SelectBehaviour(3); 
      else
      if ( Right ) SelectBehaviour(4); 
      else
      if ( ( ! Avoiding ) && ( CurrentBehaviour != 1 ) ) 
                   SelectBehaviour(1);

      PauseMS(50);

    } /* while ( Active ) */

    BehaviourActive = FALSE;
    BehaviourBreak = TRUE;    
    Behaviour = 0; 
    Signal( & NewBehaviour );

    MusicActive = FALSE;
    MusicBreak = TRUE;
    Music = 0;
    Signal( & NewMusic );

  } /* Forever */
}

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

void _start( void ) 
{ 

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


  RTSInit();
  TimeCountersResetTime();

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

  Active = TRUE;    
  StartProcess( BehaviourSelection );

  forever {

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

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

  } /* Forever */
}