#include "RCX_String.h"
#include "RCX_Control.h"
#include "H8.h" 
#include "Time.h"
#include "LCD.h"
#include "Battery.h"
#include "Buttons.h"
#include "InputPorts.h"

#include "RCX_Interrupts.h"
#include "OutputPorts.h"

#include "RoundRobinScheduler.h"

#define  forever  while (1)

static volatile byte Running;
static byte ProgramNumber;

int16 Touch1( void ) {
  return( Port1Raw() < 800);
}

int16 Touch3( void ) {
  return( Port3Raw() < 800);
}

void ResetProcess( void )
{
  while ( ! OnOff ) ;

  SchedulerClose();
  RCX_Reset();
}

void Process1() {
  forever {

    while( Prgm ) {

      switch ( ProgramNumber ) {

      case 1:
	ProgramNumber = 2;
	break;
      case 2:
	ProgramNumber = 1;
	break;
      }

      lcd_show_digit(ProgramNumber);
      BusyPauseMS(500);
    }
  }
}

void Process2() {
  forever {
    
    while ( ! Running );
    
    while ( Running ) {
      
      switch ( ProgramNumber ) {
	
      case 1:  /* Program 1 */
	PortA(OnNeg); PortC(OnNeg);
	break;
	
      case 2:  /* Program 2 */
	if ( Touch1() ) PortA(Float); else PortA(OnNeg);
	if ( Touch3() ) PortC(Float); else PortC(OnNeg);
	break;
	
      default: break;
      }
    }
    PortA(Float); PortB(Float); PortC(Float); 
  }
}

void OnOffProcess() {
  forever {
    
    while ( Run ) {
      switch ( Running ){
      case 0: 
	Running = 1;
	lcd_show_icon(LCD_WALKING);
	break;
      case 1:
	Running = 0;
	lcd_show_icon(LCD_STANDING);
	break;
      }
      BusyPauseMS(500);
    }
  }
}

void _start(void) {
  
  Running = 0;
  ProgramNumber = 1;
  
  lcd_show_icon(LCD_STANDING);
  lcd_show_digit(ProgramNumber);
  
  SchedulerInit();
  StartProcess( ResetProcess);
  StartProcess( Process1);
  StartProcess( Process2);
  StartProcess( OnOffProcess);
  
  forever {
    lcd_show_int16(Current);
  }
}