\ .............................................................................|
\ todo: Button System in srec ? ; run main loop as terminal 'UserIdle word ?
\ todo: rearrange Utilities
\ SYSTEM           \ if previously downloaded, forget dictionary back to marker
MARKER SYSTEM    \ set a new marker for this download

\ Utilities....................................................................|
: blip ( -- )  0 16388 SOUND_PLAY ;
: .stack  DEPTH U. ." items on stack.  TOS: " DUP . CR ;
: show_escaped ( -- )  LCD_4TH 12294 LCD_HIDE LCD_REFRESH ;  \ show 4th, no man
: check_buttons ( -- code  )  RCX_BUTTON DUP BUTTON_GET @ ;
: ?abort_button  ( code -- )  5 > IF DROP show_escaped ABORT THEN ; \ View+Prgm
: yield ( -- )  check_buttons ?abort_button ; \ check escape buttons
: wait ( ms -- )  0 DO 5 0 DO yield LOOP LOOP ;       \ wait n milliseconds
: set_sensor ( type mode id -- )  DUP ROT SWAP SENSOR_MODE SENSOR_TYPE ;
: get_sensor ( id -- val )  DUP  SENSOR_READ DROP  SENSOR_VALUE ;

\ Button Execution System......................................................|
VARIABLE run_state			\ 0=stop, 1=run
VARIABLE init_vector    \ stores pointer to "init"
VARIABLE scan_vector    \ stores pointer to "scan"
: init_exec  ( -- )  init_vector @ EXECUTE ;  \ execute "init"
: scan_exec  ( -- )  scan_vector @ EXECUTE ;  \ execute "scan"
: show_walk ( -- )  LCD_4TH 12295 LCD_SHOW LCD_REFRESH ;  \ 4th + man
: show_stand ( -- )  LCD_4TH 12294 LCD_SHOW LCD_REFRESH ;
: run_sound ( -- )  3 16388 SOUND_PLAY ;
: stop_sound ( -- )  2 16388 SOUND_PLAY ;
: power_sound ( -- )  1 16388 SOUND_PLAY ;
: stop_motors ( -- )  2 0 DO 0 3 I MOTOR_SET LOOP ; \ stop rather than float
: to_run ( -- )  1 run_state !  show_walk  run_sound  init_exec ;
: to_stop ( -- ) 0 run_state !  stop_motors show_stand stop_sound ;
: show_state ( -- )  run_state @ IF  show_walk  ELSE  show_stand  THEN ;
: setup ( -- )  to_stop ;			\ set initial status
: toggle_run_state ( -- )  run_state @ IF to_stop ELSE to_run THEN 1000 wait ;
: ?run_button ( n -- )  1 = IF  toggle_run_state  THEN ;
: button_actions ( n -- )  DUP ?abort_button ?run_button ;
: check_power_button ( -- n )  RCX_POWER DUP 16384 POWER_GET ;
: power_debounce ( -- )  power_sound 1000 wait ;
: power_offon ( -- )  power_debounce POWER_OFF power_debounce show_state ;
: ?power_button ( n -- )  @ 0= IF  power_offon  THEN ;
: do_power  ( -- )  check_power_button ?power_button ;
: do_buttons ( -- )  do_power check_buttons button_actions ;
: ?runscan ( -- )  run_state @ IF scan_exec THEN ;
: main ( -- )  setup  BEGIN  do_buttons ?runscan  0 UNTIL ;

\ Program......................................................................|
MARKER PROGRAM    \ mark start of program to enable deleting and updating
: calc_angle ( raw -- angle )  128 SWAP DUP 1023 SWAP - */ ;  \ 128*x/(1023-x)
: display ( n -- )  12290 SWAP 12289 LCD_NUMBER LCD_REFRESH ;
: get_angle ( -- angle )  0 get_sensor calc_angle ;
: init ( -- )  0 0 0 set_sensor ;                \ setup sensor 0 as raw
: scan ( -- )  get_angle display ;	\ scan task

\ Register "init" and "scan" for ready for execution...........................|
' init init_vector !
' scan scan_vector !
main

