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

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

/* Interrupt handler for Run button */

static int16 Count;
static int16 Running;

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

  Count = Count + 2;

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

  if (Running) {
    lcd_show_icon(LCD_STANDING);
    Running = 0;
  } 
  else {
    lcd_show_icon(LCD_WALKING);
    Running = 1;
  }
}	

void OnOffBreak (void)
{
  RCX_Reset();
}

void _start(void) 
{ 
  Running = 0;
  lcd_show_int16(BatteryLevel());
  lcd_show_icon(LCD_STANDING);
  while ( ! Run );
  while (   Run );
  lcd_show_icon(LCD_WALKING); 

  Count = 0;
  lcd_show_int16(Count);
  BusyPauseMS(1000);

  RunInt();
  
  /* 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; 
  ISCR = 3;                      
  IER  = 3;

  IRQ1_Interrupt_Handler = OnOffBreak;


  while ( ! View ){
    lcd_show_int16(Count);
    
    if ( Count > 10 ){
      Count = 0;
    }
    
    if (Running) {
      PortA(OnPos);
      PortC(OnPos);
    }
    else {
      PortA(Float);
      PortC(Float);
    }
  }
  
  RCX_Reset();

}