#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"

static volatile byte Running;
static byte ProgramNumber;

/* Interrupt handler for Run button */

void RunInt (void) 
{
  asm("push r0
       push r1
       push r2
       push r3
       push r4
       push r5
      ");

  switch ( Running ){
  case 0: 
    Running = 1;
    lcd_show_icon(LCD_WALKING);
    break;
  case 1:
    Running = 0;
    lcd_show_icon(LCD_STANDING);
    break;
  }

  asm("pop  r5
       pop  r4
       pop  r3
       pop  r2
       pop  r1
       pop  r0
      ");
}

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

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

void OnOffBreak (void)
{
  RCX_Reset();
}

void _start(void) {
  
  Running = 0;
  ProgramNumber = 1;
  lcd_show_icon(LCD_STANDING);
  
  lcd_show_digit(ProgramNumber);
  
  /* Make RunInt the interrupt handler for the Run button (IRQ0) */
  /* Generate interrupt by the falling edge of Run button (IRQ0) */
  /* Enable Run button interrupts (IRQ0 interrupts)              */
  
  IRQ0_Interrupt_Handler = RunInt; 
  IRQ1_Interrupt_Handler = OnOffBreak;
  ISCR = 3;                      
  IER  = 3;
  
  while (1) {

    
    PortA(Float); PortB(Float); PortC(Float);
    while ( ! Running ) {    
      
      while ( Prgm ) {
	switch ( ProgramNumber ) {
	case 1:
	  ProgramNumber = 2;
	  break;
	case 2:
	  ProgramNumber = 1;
	  break;
	}	
	BusyPauseMS(500);
	lcd_show_digit(ProgramNumber);
      }
    }

    while ( Running ) {
      
      switch ( ProgramNumber ) {
	
      case 1:  /* Program 1 */
	PortA(OnPos); PortC(OnPos);
	break;
	
      case 2:  /* Program 2 */
	if ( Touch1() ) PortA(Float); else PortA(OnPos);
	if ( Touch3() ) PortC(Float); else PortC(OnPos);
	break;
	
      default: break;
	
      }
    }
  }
}