Update outer.4th

Update outer.4th

( outer interpreter - reads tokens, compiles headers, literals, ... )
( requires assembler )

( --- register allocation/init ----------------------------------------------- )

          0 const c         ( char last read )
          1 const sp        ( space 32 ASCII )
          2 const tib       ( terminal input buffer )
          3 const len       ( token length )
          4 const len'      ( name length )
          5 const zero      ( constant 0 )
          6 const d         ( dictionary pointer )
          7 const nm        ( match flag for search )
          8 const p         ( pointer )
          9 const c         ( char being compared )
         10 const p'        ( pointer )
         11 const c'        ( char being compared )
         12 const cur       ( cursor )
         13 const lnk       ( link pointer )
         14 const true      ( truth value )
         15 const false     ( false value )
         16 const n         ( parsed number )
         17 const zeroch    ( '0' ASCII )
         18 const base      ( number base )
         19 const s         ( stack pointer )
         20 const nreg      ( register n number )
         21 const ldc       ( ldc instruction [0] )
         22 const call      ( call instruction [27] )
         23 const ret       ( return instruction [29] )
         24 const two       ( constant 2 )
         25 const comp      ( compiling flag )
         26 const x         ( temp )
         27 const y         ( temp )
         28 const z         ( temp - reserved bootstrap )

            32 sp ldc,      ( space ASCII )
        48 zeroch ldc,      ( '0' ASCII )
          -1 true ldc,      ( all bits set [logical/bitwise equivalent] )
          10 base ldc,      ( decimal by default )
          32767 s ldc,      ( top of data stack )
           n nreg ldc,      ( register number of n )
          27 call ldc,      ( call instruction )
           28 ret ldc,      ( ret instruction )
            2 two ldc,      ( constant 2 )

leap,

( --- tokenization ----------------------------------------------------------- )

            label &skipws   ( skip until non-whitespace )
                c in,       ( read char )
     c sp &skipws ble,      ( keep skipping whitespace )
                  ret,

            label &name     ( read input name into buffer )
            c tib st,       ( append char )
          tib tib inc,      ( advance tib )
          len len inc,      ( increment length )
                c in,       ( read char )
       c sp &name bgt,      ( continue until whitespace )
          len tib st,       ( append length )
                  ret,

            label &token    ( read token into buffer )
            d tib cp,       ( end of dictionary as buffer )
         zero len cp,       ( initial zero length )
          &skipws call,     ( skip initial whitespace )
            &name jump,     ( append name )

( --- dictionary search ------------------------------------------------------ )

            label &nomatch
          true nm cp,       ( set no-match flag )
                  ret,

            label &compcs   ( compare chars to input word )
              p c ld,       ( get char of input word )
            p' c' ld,       ( get char of dictionary word )
    c c' &nomatch bne,      ( don't match? )
            p' p' inc,      ( next char of dictionary word )
              p p inc,      ( next char of input word )
    p tib &compcs blt,      ( not end of word? continue... )
                  ret,      ( we have a match! )

            label &nextw    ( advance to next word )
          cur cur ld,       ( follow link address )
                            ( fall through to &comp )

            label &comp
zero cur &nomatch beq,      ( no match if start of dict )
        tib len p sub,      ( point p at start of token )
           cur p' dec,      ( point p' at length field )
          p' len' ld,       ( get length )
  len' len &nextw bne,      ( lengths don't match? )
        p' len p' sub,      ( move to beginning of word )
         false nm cp,       ( reset no-match flag )
          &compcs call,     ( compare characters )
   true nm &nextw beq,      ( if no match, try next word )
                  ret,      ( we have a match! )

            label &find     ( find word [tib within dictionary] )
          lnk cur cp,       ( initialize cursor to last )
            &comp jump,     ( compare with tib )

( --- number processing ------------------------------------------------------ )

            label &digits
              p c ld,       ( get char )
       c zeroch c sub,      ( convert to digit )
         n base n mul,      ( base shift left )
            n c n add,      ( add in one's place )
              p p inc,      ( next char )
    p tib &digits blt,      ( not end? continue... )
                  ret,

            label &parsenum ( parse token as number )
        tib len p sub,      ( point p at start of word )
           zero n cp,       ( init number )
          &digits jump,

            label &pushn    ( push interactive number )
              n s st,       ( store n at stack pointer )
              s s dec,      ( adjust stack pointer )
                  ret,

            label &popn     ( pop number )
              s s inc,      ( adjust stack pointer )
              s n ld,       ( load value )
                  ret,

( --- literals --------------------------------------------------------------- )

: append, d st, d d inc, ;  ( macro: append and advance d )

            label &litn     ( compile literal )
              ldc append,   ( append ldc instruction )
             nreg append,   ( append n register number )
                n append,   ( append value )
             call append,   ( append call instruction )
         &pushn x ldc,      ( load address of &pushn )
                x append,   ( append pushn address )
                  ret,

            label &num      ( process token as number )
        &parsenum call,     ( parse number )
  true comp &litn beq,      ( if compiling, compile literal )
           &pushn jump,     ( else, push literal )

( --- word processing -------------------------------------------------------- )

            label &exec     ( execute word )
        two cur x add,      ( point to code field )
                x exec,     ( exec word )
                  ret,

            label &compw    ( compile word )
            cur x inc,      ( point to immediate flag )
              x y ld,       ( read immediate flag )
     true y &exec beq,      ( execute if immediate )
             call append,   ( append call instruction )
              x x inc,      ( point to code field )
                x append,   ( append code field address )
                  ret,

            label &word     ( process potential word token )
 true comp &compw beq,      ( if compiling, compile word )
            &exec jump,     ( else, execute word )

            label &eval     ( process input tokens )
            &find call,     ( try to find in dictionary )
     true nm &num beq,      ( if not found, assume number )
            &word jump,     ( else, process as a word )

( --- REPL ------------------------------------------------------------------- )

            label &repl     ( loop forever )
           &token call,     ( read a token )
            &eval call,     ( evaluate it )
            &repl jump,     ( forever )

( --- initial dictionary ----------------------------------------------------- )

var link
: header, dup 0 do swap , loop , link @ here link ! , , ;

     0 sym create header,   ( word to create words )
           &token call,     ( read a token )
            tib d cp,       ( move dict ptr to end of name )
              d d inc,      ( move past length field )
            lnk d st,       ( append link address )
            d lnk cp,       ( update link to here )
              d d inc,      ( advance dictionary pointer )
             zero append,   ( append 0 immediate flag )
                  ret,

  0 sym immediate header,   ( set immediate flag )
            lnk x inc,      ( point to immediate flag )
           true x st,       ( set immediate flag )
                  ret,

    0 sym compile header,   ( switch to compiling mode )
        true comp cp,       ( set comp flag )
                  ret,

   0 sym interact header,   ( switch to interactive mode )
            label &interact
       false comp ldc,      ( reset comp flag )
                  ret,

         -1 sym ; header,   ( return )
              ret append,   ( append ret instruction )
        &interact jump,     ( switch out of compiling mode )

     0 sym pushn, header,   ( push number [n] to stack from )
           &pushn jump,     ( jump to push )

      0 sym popn, header,   ( pop number from stack to n )
            &popn jump,     ( jump pop )

          0 sym , header,   ( append value from stack )
            &popn call,     ( pop value from stack )
                n append,   ( append n )
                  ret,

       0 sym find header,   ( find word )
           &token call,     ( read a token )
            &find call,     ( find token )
            cur n cp,       ( prep to push cursor )
          two n n add,      ( address of code field )
           &pushn jump,     ( push cursor )

       0 sym dump header,   ( dump core to boot.bin )
                  dump,     ( TODO: build outside of outer interpreter )

ahead,

( --- set `lnk` to within last header and `d` to just past this code --------- )

link @ lnk ldc,             ( compile-time link ptr to runtime )
here 5 + d ldc,             ( compile-time dict ptr to runtime [advance over init code below] )

&repl jump,                 ( start the REPL )

assemble


Rewording.

Rewording.

\ Simple greeting screen, presenting basic options.
\ XXX This is far too trivial - I don't have time now to think
\ XXX about something more fancy... :-/
\ $Id$

: title
	f_single
	60 11 10 4 box
	29 4 at-xy 15 fg 7 bg
	." Welcome to BootFORTH!"
	me
;

: menu
	2 fg
	20 7 at-xy
	." 1.  Start FreeBSD with /boot/stable.conf."
        20 8 at-xy
        ." 2.  Start FreeBSD with /boot/current.conf."
	20 9 at-xy
	." 3.  Start FreeBSD with standard configuration. "
	20 10 at-xy
	." 4.  Reboot."
	me
;

: tkey	( d -- flag | char )
	seconds +
	begin 1 while
	    dup seconds u< if
		drop
		-1
		exit
	    then
	    key? if
		drop
		key
		exit
	    then
	repeat
;

: prompt
	14 fg
	20 12 at-xy
	." Enter your option (1,2,3,4): "
	10 tkey
	dup 32 = if
	    drop key
	then
	dup 0< if
	    drop 51
	then
	dup emit
	me
;

: help_text
        10 18 at-xy ." * Choose 1 or 2 to run special configuration file."
	10 19 at-xy ." * Choose 3 to proceed with standard bootstrapping."
	12 20 at-xy ." See '?' for available commands, and 'words' for"
	12 21 at-xy ." complete list of Forth words."
	10 22 at-xy ." * Choose 4 in order to warm boot your machine."
;

: (reboot) 0 reboot ;

: main_menu
	begin 1 while
		clear
		f_double
		79 23 1 1 box
		title
		menu
		help_text
		prompt
		cr cr cr
		dup 49 = if
			drop
			1 25 at-xy cr
			." Loading /boot/stable.conf. Please wait..." cr
			s" /boot/stable.conf" read-conf
			boot-conf
		then
		dup 50 = if
			drop
			1 25 at-xy cr
			." Loading /boot/current.conf. Please wait..." cr
			s" /boot/current.conf" read-conf
			boot-conf
		then
		dup 51 = if
			drop
			1 25 at-xy cr
			." Proceeding with standard boot. Please wait..." cr
			boot-conf
		then
		dup 52 = if
			drop
			1 25 at-xy cr
			['] (reboot) catch abort" Error rebooting"
		then
		20 12 at-xy
		." Key " emit ."  is not a valid option!"
		20 13 at-xy
		." Press any key to continue..."
		key drop
	repeat
;

\ Simple greeting screen, presenting basic options.
\ XXX This is far too trivial - I don't have time now to think
\ XXX about something more fancy... :-/
\ $Id$

: title
	f_single
	60 11 10 4 box
	29 4 at-xy 15 fg 7 bg
	." Welcome to BootFORTH!"
	me
;

: menu
	2 fg
	20 7 at-xy
	." 1.  Start FreeBSD /boot/stable.conf."
        20 8 at-xy
        ." 2.  Start FreeBSD /boot/current.conf."
	20 9 at-xy
	." 3.  Start FreeBSD /boot/loader.rc."
	20 10 at-xy
	." 4.  Reboot."
	me
;

: tkey	( d -- flag | char )
	seconds +
	begin 1 while
	    dup seconds u< if
		drop
		-1
		exit
	    then
	    key? if
		drop
		key
		exit
	    then
	repeat
;

: prompt
	14 fg
	20 12 at-xy
	." Enter your option (1,2,3,4): "
	10 tkey
	dup 32 = if
	    drop key
	then
	dup 0< if
	    drop 51
	then
	dup emit
	me
;

: help_text
        10 18 at-xy ." * Choose 1 or 2 to run special configuration file."
	10 19 at-xy ." * Choose 3 to proceed with standard bootstrapping."
	12 20 at-xy ." See '?' for available commands, and 'words' for"
	12 21 at-xy ." complete list of Forth words."
	10 22 at-xy ." * Choose 4 in order to warm boot your machine."
;

: (reboot) 0 reboot ;

: main_menu
	begin 1 while
		clear
		f_double
		79 23 1 1 box
		title
		menu
		help_text
		prompt
		cr cr cr
		dup 49 = if
			drop
			1 25 at-xy cr
			." Loading /boot/stable.conf. Please wait..." cr
			s" /boot/stable.conf" read-conf
			boot-conf
		then
		dup 50 = if
			drop
			1 25 at-xy cr
			." Loading /boot/current.conf. Please wait..." cr
			s" /boot/current.conf" read-conf
			boot-conf
		then
		dup 51 = if
			drop
			1 25 at-xy cr
			." Proceeding with standard boot. Please wait..." cr
			boot-conf
		then
		dup 52 = if
			drop
			1 25 at-xy cr
			['] (reboot) catch abort" Error rebooting"
		then
		20 12 at-xy
		." Key " emit ."  is not a valid option!"
		20 13 at-xy
		." Press any key to continue..."
		key drop
	repeat
;


Added , , , , ,  and rudamentary

Added , , , , ,  and rudamentary

( bootstrap remainder of interpreter )
( load into machine running outer interpreter image )

create : compile create compile ; ( magic! )

( assembler )

: x 1 ; ( shared by outer interpreter )
: d 2 ; ( dictionary pointer - shared by outer interpreter )
: zero 4 ; ( shared by outer interpreter )
: y 30 ;
: z 31 ;

: [ interact ; immediate
: ] compile ;

: cp, 3 , , , ;
: popxy popx [ x y cp, ] popx ;
: pushxy pushx [ y x cp, ] pushx ;

: xor, 15 , , , , ;
: swap popxy [ x y x xor, x y y xor, x y x xor, ] pushxy ;

: ldc,   0 , , , ;
: ld,    1 , , , ;
: st,    2 , , , ;
: in,    4 , , ;
: out,   5 , , ;
: inc,   6 , , , ;
: dec,   7 , , , ;
: add,   8 , , , , ;
: sub,   9 , , swap , , ;
: mul,  10 , , , , ;
: div,  11 , , swap , , ;
: mod,  12 , , swap , , ;
: and,  13 , , , , ;
: or,   14 , , , , ;
: not,  16 , , , ;
: shr,  17 , , swap , , ;
: shl,  18 , , swap , , ;
: beq,  19 , , , , ;
: bne,  20 , , , , ;
: bgt,  21 , , swap , , ;
: bge,  22 , , swap , , ;
: blt,  23 , , swap , , ;
: ble,  24 , , swap , , ;
: exec, 25 , , ;
: jump, 26 , , ;
: call, 27 , , ;
: ret,  28 , ;
: halt, 29 , ;
: dump, 30 , ;

( instruction words )

: +    popxy [ x y x add, ] pushx ;
: -    popxy [ x y x sub, ] pushx ;
: *    popxy [ x y x mul, ] pushx ;
: /    popxy [ x y x div, ] pushx ;
: mod  popxy [ x y x mod, ] pushx ;
: 2/   popxy [ x y x shr, ] pushx ;
: 2*   popxy [ x y x shl, ] pushx ;
: and  popxy [ x y x and, ] pushx ;
: or   popxy [ x y x or,  ] pushx ;
: xor  popxy [ x y x xor, ] pushx ;
: not  popx [ x x not, ] pushx ;
: 1+   popx [ x x inc, ] pushx ;
: 1-   popx [ x x dec, ] pushx ;
: exec popx [ x exec, ] ;
: dump [ dump, ] ;
: exit [ halt, ] ;

( stack manipulation )

: drop ( a- ) popx ;
: 2drop ( ab- ) drop drop ;
: dup ( a-aa ) popx pushx pushx ;
: over ( ab-aba ) popxy pushx pushx [ y x cp, ] pushx swap ;
: 2dup ( ab-abab ) over over ;
: nip ( ab-b ) swap drop ;
: tuck ( ab-bab ) swap over ;
: -rot ( abc-cab ) swap popxy pushx [ y z cp, ] swap [ z x cp, ] pushx ;

( vocabulary )

: true -1 ;
: false 0 ;

: key [ x in, ] pushx ;
: emit popx [ x out, ] ;
: cr 10 emit ;
: space 32 emit ;

: @ popx [ x x ld, ] pushx ;
: ! popxy [ x y st, ] ;

: allot [ d x cp, ] pushx swap popx [ x d d add, ] ;

: here@ [ d x cp, ] pushx ;
: here! popx [ x d cp, ] ;

: >dfa 2 + ;
: ' find >dfa ;

: if [ ' popxy literal ] call, here@ 1+ zero x 0 beq, ; immediate
: else here@ 1+ 0 jump, swap here@ swap ! ; immediate
: then here@ swap ! ; immediate

: = popxy 0 [ x y here@ 6 + bne, ] not ;
: <> popxy 0 [ x y here@ 6 + beq, ] not ;
: > popxy 0 [ x y here@ 6 + ble, ] not ;
: < popxy 0 [ x y here@ 6 + bge, ] not ;
: >= popxy 0 [ x y here@ 6 + blt, ] not ;
: <= popxy 0 [ x y here@ 6 + bgt, ] not ;

: sign 0 < if -1 else 1 then ;
: /mod 2dup / -rot mod ;

: .sign dup sign -1 * 44 + emit ; ( happens 44 +/- 1 is ASCII '-'/'+' )
: .dig 10 /mod swap ;
: .digemit 48 + emit ;  ( 48 is ASCII '0' )
: . .sign .dig .dig .dig .dig .dig drop .digemit .digemit .digemit .digemit .digemit cr ;

( bootstrap remainder of interpreter )
( load into machine running outer interpreter image )

create : compile create compile ; ( magic! )

( assembler )

: x 1 ; ( shared by outer interpreter )
: d 2 ; ( dictionary pointer - shared by outer interpreter )
: zero 4 ; ( shared by outer interpreter )
: y 31 ;

: [ interact ; immediate
: ] compile ;

: cp, 3 , , , ;
: popxy popx [ x y cp, ] popx ;
: pushxy pushx [ y x cp, ] pushx ;

: xor, 15 , , , , ;
: swap popxy [ x y x xor, x y y xor, x y x xor, ] pushxy ;

: ldc,   0 , , , ;
: ld,    1 , , , ;
: st,    2 , , , ;
: in,    4 , , ;
: out,   5 , , ;
: inc,   6 , , , ;
: dec,   7 , , , ;
: add,   8 , , , , ;
: sub,   9 , , swap , , ;
: mul,  10 , , , , ;
: div,  11 , , swap , , ;
: mod,  12 , , swap , , ;
: and,  13 , , , , ;
: or,   14 , , , , ;
: not,  16 , , , ;
: shr,  17 , , swap , , ;
: shl,  18 , , swap , , ;
: beq,  19 , , , , ;
: bne,  20 , , , , ;
: bgt,  21 , , swap , , ;
: bge,  22 , , swap , , ;
: blt,  23 , , swap , , ;
: ble,  24 , , swap , , ;
: exec, 25 , , ;
: jump, 26 , , ;
: call, 27 , , ;
: ret,  28 , ;
: halt, 29 , ;
: dump, 30 , ;

( instruction words )

: +    popxy [ x y x add, ] pushx ;
: -    popxy [ x y x sub, ] pushx ;
: *    popxy [ x y x mul, ] pushx ;
: /    popxy [ x y x div, ] pushx ;
: mod  popxy [ x y x mod, ] pushx ;
: 2/   popxy [ x y x shr, ] pushx ;
: 2*   popxy [ x y x shl, ] pushx ;
: and  popxy [ x y x and, ] pushx ;
: or   popxy [ x y x or,  ] pushx ;
: xor  popxy [ x y x xor, ] pushx ;
: not  popx [ x x not, ] pushx ;
: 1+   popx [ x x inc, ] pushx ;
: 1-   popx [ x x dec, ] pushx ;
: exec popx [ x exec, ] ;
: dump [ dump, ] ;
: exit [ halt, ] ;

( stack manipulation )

: drop popx ;
: dup popx pushx pushx ;

( vocabulary )

: true -1 ;
: false 0 ;

: key [ x in, ] pushx ;
: emit popx [ x out, ] ;
: cr 10 emit ;
: space 32 emit ;

: @ popx [ x x ld, ] pushx ;
: ! popxy [ x y st, ] ;

: allot [ d x cp, ] pushx swap popx [ x d d add, ] ;

: here@ [ d x cp, ] pushx ;
: here! popx [ x d cp, ] ;

: >dfa 2 + ;
: ' find >dfa ;

: if [ ' popx literal ] call, here@ 1+ zero x 0 beq, ; immediate
: else here@ 1+ 0 jump, swap here@ swap ! ; immediate
: then here@ swap ! ; immediate

: = popxy 0 [ x y here@ 6 + bne, ] not ;
: <> popxy 0 [ x y here@ 6 + beq, ] not ;
: > popxy 0 [ x y here@ 6 + ble, ] not ;
: < popxy 0 [ x y here@ 6 + bge, ] not ;
: >= popxy 0 [ x y here@ 6 + blt, ] not ;
: <= popxy 0 [ x y here@ 6 + bgt, ] not ;


LOG2 added

LOG2 added

\ KataDiversion in Forth

\ -- utils

\ empty the stack
: EMPTY
    DEPTH 0 <> IF BEGIN
                    DROP DEPTH 0 =
                  UNTIL
               THEN ;

\ power
: ** ( n1 n2 -- n1_pow_n2 ) 1 SWAP ?DUP IF 0 DO OVER * LOOP THEN NIP ;

\ test if the top is a negative number
: ?NEG ( n -- bool ) DUP 0= IF -1 ELSE DUP ABS <> THEN ;

\ log2 (integer)
: LOG2 ( n -- n log2_n ) DUP 1 < IF 1 ABORT" Log2 need a positive value."
                                 ELSE DUP 1 = IF 0
                                              ELSE
                                                  1 >R
                                                  BEGIN ( n |R: i)
                                                      DUP DUP 2 I ( n n n 2 i |R: i)
                                                      ** ( n n n 2**i )
                                                      - ( n n n-2**i )
                                                      2 * ( n n 2*[n-2**i])
                                                      R> 1 + >R ( … |R: i+1)
                                                      > ( n n>2*[n-2**i] )
                                                  UNTIL
                                                  R> 1 -
                                              THEN
                                 THEN ;

\ -- kata

\ test if the given N has two adjacent 1 bits
\ e.g. : 1011 -> -1
\        1001 -> 0
: ?TWO-ADJACENT-1-BITS ( n -- bool ) ( TODO ) ;

\ return the maximum number which can be made with N (given number) bits
: ?MAX-NB ( n -- m ) DUP ?NEG IF DROP 0 ( 0 )
                              ELSE
                                  DUP IF DUP 2 SWAP ** NIP ( 2**n )
                                  THEN
                              THEN ;

\ return the number of numbers which can be made with N (given number) bits
\ or less, and which have not two adjacent 1 bits.
\ see http://www.codekata.com/2007/01/code_kata_fifte.html
: ?HOW-MANY-NB-NOT-TWO-ADJACENT-1-BITS ( n -- m ) ( TODO ) ;

\ KataDiversion in Forth

\ -- utils

\ empty the stack
: EMPTY
    DEPTH 0 <> IF BEGIN
                    DROP DEPTH 0 =
                  UNTIL
               THEN ;

\ power
: ** ( n1 n2 -- n1_pow_n2 ) 1 SWAP ?DUP IF 0 DO OVER * LOOP THEN NIP ;

\ test if the top is a negative number
: ?NEG ( n -- bool ) DUP 0= IF -1 ELSE DUP ABS <> THEN ;

\ -- kata

\ test if the given N has two adjacent 1 bits
\ e.g. : 1011 -> -1
\        1001 -> 0
: ?TWO-ADJACENT-1-BITS ( n -- bool ) ( TODO ) ;

\ return the maximum number which can be made with N (given number) bits
: ?MAX-NB ( n -- m ) DUP ?NEG IF DROP 0 ( 0 )
                              ELSE
                                  DUP IF DUP 2 SWAP ** NIP ( 2**n )
                                  THEN
                              THEN ;

\ return the number of numbers which can be made with N (given number) bits
\ or less, and which have not two adjacent 1 bits.
\ see http://www.codekata.com/2007/01/code_kata_fifte.html
: ?HOW-MANY-NB-NOT-TWO-ADJACENT-1-BITS ( n -- m ) ( TODO ) ;


Make the time advance code general-purpose.

(( Clock Code ))

\ The gadget that drives it all.   This is rigged for debugging,
\ terminate it by setting clockterm to non-zero.
: CLOCKSTART ( n -- )
  icroot inter.rtcsem 0 over ! \ Reset the free-running counter.
  swap \ addr n --
  0 do
   dup @resetex! ?dup if advance else [asm wfi asm] then
  loop
  drop
;

: CLOCKINIT ( -- )
  #1000 needlemax
  2dup ! 4 + 2dup ! 4 +  !
;

\ Heres where we advance any needles.
: ADVANCE ( n -- )
  pwm0@ + pwm0!
;

\ ----------------------------------------------------------
\ Needle Management
\ ----------------------------------------------------------
udata
create NEEDLEMAX 3 cells allot
cdata

: ++NEEDLE_S ; \ Called ever time.
: ++NEEDLE_M ; \ Every time we roll the seconds.

\ ----------------------------------------------------------
\ PWM/Timer Manipulation
\ ----------------------------------------------------------
_timer0 $34 + equ _PWM0
_timer0 $44 + equ _PWM1
_timer0 $54 + equ _PWM2

: PWM0! ( n -- ) _pwm0 ! ;
: PWM1! ( n -- ) _pwm1 ! ;
: PWM2! ( n -- ) _pwm2 ! ;

: PWM0@ ( -- n ) _pwm0 w@ ;
: PWM1@ ( -- n ) _pwm1 w@ ;
: PWM2@ ( -- n ) _pwm2 w@ ;

: SCALE0 #1000 0 do I pwm0! loop ;
: SCALE1 #1000 0 do I pwm0! [asm wfi asm] loop ;

\ ----------------------------------------------------------
\ Keeping track of the time.
\ ----------------------------------------------------------
struct _HMS
	int hms.subsec
	int hms.s
	int hms.m
	int hms.h
	int hms.maxsubsec \ Max Value for subseconds
	int hms.maxsec   \ Max Value for secs+minutes
	int hms.maxhour   \ Max for hours
	ptr hms.w_s    \ Increment the seconds value
	ptr hms.w_m    \ increment the minutes + hours
end-struct

idata
create HMS
  0 , 0 , 0 , 0 , \ Running Counters
  #16 , #60 , #24 , \ Limits
  ' ++needle_s , ' ++needle_m , \ Words to invoke
create DHMS
  0 , 0 , 0 , 0 , \ Running Counters
  #10 , #100 , #10 , \ Limits
' ++needle_s , ' ++needle_m , \ Words to invoke
cdata

\ Core concept - We update the seconds
\ each time this gets called.
\ If the seconds wrap, we update the minutes
\ and the hours.
\ UNTESTED
: ADVANCETIME ( struct-addr -- )
  dup hms.w_s @ execute \ Invoke the official update word.
  1 over hms.subsec +!  \ Increment
   dup hms.subsec @
   over hms.maxsubsec @ < if drop exit then \ If less then max, we're done
  0 over hms.subsec ! \ Reset the subsecs

  dup hms.w_m @ execute
  1 over hms.s +!
   dup hms.s @ over hms.maxsec @ < if drop exit then
  0 over hms.s ! \ Reset the seconds

  1 over hms.m +!
   dup hms.m @ over hms.maxsec @ < if drop exit then
  0 over hms.m ! \ Reset the minutes

  1 over hms.h +!
   dup hms.h @ over hms.maxhour @ < if drop exit then
  0 over hms.h ! \ Reset the minutes

  drop

;

: ? @ . ;
: .hms ( addr -- )
  dup hms.h ?
  dup hms.m ?
  dup hms.s ?
  hms.subsec ?
  cr
;

\ : foo 0 do hms advancetime loop ;

\ ============================ UNTESTED =======================
\ VVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVV

(( Clock Code ))

\ The gadget that drives it all.   This is rigged for debugging,
\ terminate it by setting clockterm to non-zero.
: CLOCKSTART ( n -- )
  icroot inter.rtcsem 0 over ! \ Reset the free-running counter.
  swap \ addr n --
  0 do
   dup @resetex! ?dup if advance else [asm wfi asm] then
  loop
  drop
;

: CLOCKINIT ( -- )
  #1000 needlemax
  2dup ! 4 + 2dup ! 4 +  !
;

\ Heres where we advance any needles.
: ADVANCE ( n -- )
  pwm0@ + pwm0!
;

\ ----------------------------------------------------------
\ Needle Management
\ ----------------------------------------------------------
udata
create NEEDLEMAX 3 cells allot
cdata

: ++NEEDLE_S ; \ Called ever time.
: ++NEEDLE_M ; \ Every time we roll the seconds.

\ ----------------------------------------------------------
\ PWM/Timer Manipulation
\ ----------------------------------------------------------
_timer0 $34 + equ _PWM0
_timer0 $44 + equ _PWM1
_timer0 $54 + equ _PWM2

: PWM0! ( n -- ) _pwm0 ! ;
: PWM1! ( n -- ) _pwm1 ! ;
: PWM2! ( n -- ) _pwm2 ! ;

: PWM0@ ( -- n ) _pwm0 w@ ;
: PWM1@ ( -- n ) _pwm1 w@ ;
: PWM2@ ( -- n ) _pwm2 w@ ;

: SCALE0 #1000 0 do I pwm0! loop ;
: SCALE1 #1000 0 do I pwm0! [asm wfi asm] loop ;

\ ----------------------------------------------------------
\ Keeping track of the time.
\ ----------------------------------------------------------
struct _HMS
	int hms.subsec
	int hms.s
	int hms.m
	int hms.h
	int hms.maxsubsec \ Max Value for subseconds
	int hms.maxs+m \ Max Value for secs+minutes
	int hms.maxh   \ Max for hours
	ptr hms.w_s    \ Increment the seconds value
	ptr hms.w_mh   \ increment the minutes + hours
end-struct

idata
create HMS
  0 , 0 , 0 , 0 , \ Running Counters
  #16 , #60 , #24 , \ Limits
  ' ++needle_s , ' ++needle_m , \ Words to invoke
create DHMS
  0 , 0 , 0 , 0 , \ Running Counters
  #10 , #100 , #10 , \ Limits
' ++needle_s , ' ++needle_m , \ Words to invoke
cdata

\ Core concept - We update the seconds
\ each time this gets called.
\ If the seconds wrap, we update the minutes
\ and the hours.
: ADVANCETIME ( struct-addr -- )
  ++needle_s  1 over hms.subsec +!
   dup hms.subsec @ #16 < if drop exit then
  0 over hms.subsec ! \ Reset the subsecs

  ++needle_m 1 over hms.s +!
   dup hms.s @ #60 < if drop exit then
  0 over hms.s ! \ Reset the seconds

  1 over hms.m +!
   dup hms.m @ #60 < if drop exit then
  0 over hms.m ! \ Reset the minutes

  1 over hms.h +!
   dup hms.h @ #24 < if drop exit then
  0 over hms.h ! \ Reset the minutes

  drop

;

: ? @ . ;
: .hms ( addr -- )
  dup hms.h ?
  dup hms.m ?
  dup hms.s ?
  hms.subsec ?
  cr
;

\ : foo 0 do hms advancetime loop ;

\ ============================ UNTESTED =======================
\ VVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVV


Checkin some old stuff for working with values.

\ Code to support runtime/dynamic into the C side via SAPI.
\
\ In the MPE environment, VALUEs work very well for this.
\ values are defined at compile time and can be updated at runtime.

\ So the basic logic is - walk the dynamic list, and if you find something
\ that is already defined, assume its a VALUE
\ otherwise, generate a constant.

\ Walk the table and make the constants.
: dy-populate
  dy-first@
  begin
     dup dy.val 0<>
  while
     dup dy-create
     dy-recordlen +
  repeat
  drop
  ;

\ The key bit.  Look for an entry and if its a value, update it.
\ otherwise, add a constant to the dictionary.
: dy-create ( c-addr -- )
        \ Start by looking it up
        DUP dy-cstring \ Make a counted string out of it.

        dy.buf find
        0= IF \ If failure, cleanup and make-const
                DROP
                dup dy.val \ c-addr n
                swap dup dy.name swap dy.namelen  \ n c-addr b
                make-const
        ELSE \ If Success, get the value and stuff it in there.
                SWAP dy.val \ Fetch the value
                SWAP dy-stuff
        THEN
    ;

: dy-first@ ( -- c-addr ) getsharedvars dy-recordlen + ;
: dy-cstring ( c-addr -- ) dy.name OVER dy.namelen dy.buf place ;

\ ************************************************************************
\ ************************************************************************
\ Secondary support functions
\ ************************************************************************
\ ************************************************************************

\ *********************************************
\ Accessors - Tightly tied the data structure
\ *********************************************
: dy.val           @ ;
: dy.size    #4 + w@ ;
: dy.count   #6 + w@ ;
: dy.type    #8 + c@ ;
: dy.namelen #9 + c@ ;
: dy.name   #12 +  @ ;

UDATA \ We need a scratch buffer.
create dy.buf $20 allot
CDATA

\ Return the recordlength.  Its the first thing.
: dy-recordlen getsharedvars @ ;

\ Retrieve the string.
: dy-string ( addr -- caddr n )
  dup dy.name swap dy.namelen
;

\ Create a constant from scratch by laying down some assembly
\ A key trick is that we have to lay down a pointer to
\ the first character of the defintion after we finish
\ See the definition above for the template
: make-const \ n s-addr c --
	here 4+ >r \ This should point to the newly-created header.
	makeheader \

\ This is what a constant looks like after emerging from the compiler.
\ ( 0002:0270 4CF8047D Lx.} )  str r7, [ r12, # $-04 ]!
\ ( 0002:0274 004F .O )        ldr r7, [ PC, # $00 ] ( @$20278=$7D04F84C )
\ ( 0002:0276 7047 pG )        bx LR

	$7D04F84C , \  str r7, [ r12, # $-04 ]!
	$4f00 w,    \  ldr r7, [ PC, # $00 ]
	$4770 w,    \ bx LR
	, 	        \ Lay down the payload

	r> ,        \ lay down the link field for the next word - required
	;

\ Take an XT of a VALUE, and stuff something in there.
: dy-stuff ( n xt -- )  8 + @ ! ;

\ Dump out an entry as a set of constants
: dy-print \ c-addr --
     S" $" type
     DUP dy.val .    \ Fetch the value

     DUP dy.type [CHAR] V = \ check the type

     IF
     	S" VALUE "
     ELSE
     	S" CONSTANT "
     THEN
     TYPE \ Display the result
     DUP dy.size . S" x" DUP dy.count .
     dy-string TYPE
     CR
   ;

\ Take a pointer to a dynamic link record, and add a constant to the dictionary
: dy-create \ addr --
	DUP \ addr addr
	dy.val \ c-addr n
	SWAP dy-string \ n c-addr b
    make-const
    ;

\ Given a record address and a string, figure out
\ if thats the one we're looking for
: dy-compare ( caddr n addr -- n )
  dy-string compare
;

\ Find the record in the list to go with a string
: dy-find ( c-addr n -- addr|0 )
  getsharedvars dy-recordlen + \ Skip over the empty first record
  >R \ Put it onto the return stack
  BEGIN
   R@ dy.val 0<>
  WHILE
   2DUP R@ dy-compare
   \ If we find it, clean the stack and leave the address on R
   0= IF 2DROP R> EXIT THEN
   R> dy-recordlen + >R
  REPEAT
  \ If we fall out, there will be a string caddr/n on the stack
  2DROP R> DROP
  0 \ Leave behind a zero to indicate failure
;

\ Walk the table and make the constants.
: dy-populate
  getsharedvars dy-recordlen + \ Skip over the empty first record
  BEGIN
     dup dy.val 0<>
  WHILE
     dup dy-create
     dy-recordlen +
  REPEAT
  2DROP R> DROP 0
  ;

\ Code to support runtime/dynamic into the C side via SAPI.
\
\ In the MPE environment, VALUEs work very well for this.
\ values are defined at compile time and can be updated at runtime.

\ ************************************************************************
\ ************************************************************************
\ Secondary support functions
\ ************************************************************************
\ ************************************************************************

\ *********************************************
\ Accessors - Tightly tied the data structure
\ *********************************************
: dy.val           @ ;
: dy.size    #4 + w@ ;
: dy.count   #6 + w@ ;
: dy.type    #8 + c@ ;
: dy.namelen #9 + c@ ;
: dy.name   #12 +  @ ;

\ Return the recordlength.  Its the first thing.
: dy-recordlen getsharedvars @ ;

\ Retrieve the string.
: dy-string ( addr -- caddr n )
  dup dy.name swap dy.namelen
;

\ Create a constant from scratch by laying down some assembly
\ A key trick is that we have to lay down a pointer to
\ the first character of the defintion after we finish
\ See the definition above for the template
: make-const \ n s-addr c --
	here 4+ >r \ This should point to the newly-created header.
	makeheader \

\ This is what a constant looks like after emerging from the compiler.
\ ( 0002:0270 4CF8047D Lx.} )  str r7, [ r12, # $-04 ]!
\ ( 0002:0274 004F .O )        ldr r7, [ PC, # $00 ] ( @$20278=$7D04F84C )
\ ( 0002:0276 7047 pG )        bx LR

	$7D04F84C , \  str r7, [ r12, # $-04 ]!
	$4f00 w,    \  ldr r7, [ PC, # $00 ]
	$4770 w,    \ bx LR
	, 	        \ Lay down the payload

	r> ,        \ lay down the link field for the next word - required
	;

\ Dump out an entry as a set of constants
: dy-print \ c-addr --
     S" $" type
     DUP dy.val .    \ Fetch the value

     DUP dy.type [CHAR] V = \ check the type

     IF
     	S" VALUE "
     ELSE
     	S" CONSTANT "
     THEN
     TYPE \ Display the result
     DUP dy.size . S" x" DUP dy.count .
     dy-string TYPE
     CR
   ;

\ Take a pointer to a dynamic link record, and add a constant to the dictionary
: dy-create \ addr --
	DUP \ addr addr
	dy.val \ c-addr n
	SWAP dy-string \ n c-addr b
    make-const
    ;

\ Given a record address and a string, figure out
\ if thats the one we're looking for
: dy-compare ( caddr n addr -- n )
  dy-string compare
;

\ Find the record in the list to go with a string
: dy-find ( c-addr n -- addr|0 )
  getsharedvars dy-recordlen + \ Skip over the empty first record
  >R \ Put it onto the return stack
  BEGIN
   R@ dy.val 0<>
  WHILE
   2DUP R@ dy-compare
   \ If we find it, clean the stack and leave the address on R
   0= IF 2DROP R> EXIT THEN
   R> dy-recordlen + >R
  REPEAT
  \ If we fall out, there will be a string caddr/n on the stack
  2DROP R> DROP
  0 \ Leave behind a zero to indicate failure
;

\ Walk the table and make the constants.
: dy-populate
  getsharedvars dy-recordlen + \ Skip over the empty first record
  BEGIN
     dup dy.val 0<>
  WHILE
     dup dy-create
     dy-recordlen +
  REPEAT
  2DROP R> DROP 0
  ;


use lex vars in (Math::Vector).cross

use lex vars in (Math::Vector).cross

package Math
class Vector
#< Represents a [vector](https://en.wikipedia.org/wiki/Vector_space) of any
#| dimension.

$docOption_instanceName = "v"

#> Creates a vector with the given components.
init {
    want @items: Num...
    if @dimension < 1
        throw Error(:DimensionError, "Need dimension >= 1D")
}

#> dimension of the vector
.dimension -> @items.length

#> human-readable dimension of the vector
.dimensionHR -> @dimension + "D"

#> magnitude of the vector
.magnitude -> @items.map! { -> $_ ^ 2 }.sum.sqrt

#> unit vector in the direction of this vector
.unitVector -> *self / @magnitude

#> returns the unit vector in the direction of the given axis
.axisUnitVector {
    need $axis: VectorAxis #< axis number or letter, starting at 1 or "i"
    -> Vector.axisUnitVector(@dimension, $axis)
}

.x -> *self[0]  #< for a >=1D vector, the first comonent
.y -> *self[1]  #< for a >=2D vector, the second component
.z -> *self[2]  #< for a >=3D vector, the third component

#> for a 2D vector, its direction, measured in radians
.direction {
    if @dimension != 2
        throw Error(:DimensionError, "Direction only exists in 2D")
    -> Math.atan2(@y, @x)
}

#> addition of two vectors
op + {
    need $ehs: Vector
    if $ehs.dimension != @dimension
        throw Error(:DimensionError, "Dimension mismatch $ehs.dimensionHR != @dimensionHR")
    $items = gather for $i in 0..@items.lastIndex {
        take *self[$i] + $ehs[$i]
    }
    -> Vector(items: $items)
}

#> allows you to take the opposite vector `-$u`
op - {
    need $lhs: Num
    if $lhs != 0
        throw(:InvalidOperation, "Unsupported operation")
    -> Vector(items: gather for $x in @items {
        take -$x
    })
}

#> subtraction of a vector from another
op - {
    need $rhs: Vector
    if $rhs.dimension != @dimension
        throw Error(:DimensionError, "Dimension mismatch $rhs.dimensionHR != @dimensionHR")
    $items = gather for $i in 0..@items.lastIndex {
        take *self[$i] - $rhs[$i]
    }
    -> Vector(items: $items)
}

#> scalar multiplication of the vector
op * {
    need $ehs: Num
    -> Vector(items: @items.map! { -> $_ * $ehs })
}

#> scalar division of the vector
op / {
    need $rhs: Num
    -> *self * (1 / $rhs)
}

#> dot product of two vectors
op * {
    need $ehs: Vector
    -> @dot($ehs)
}

#> vector equality
op == {
    need $ehs: Vector
    if $ehs.dimension != @dimension
        -> false
    for $i in 0..@items.lastIndex {
        if *self[$i] == $ehs[$i]
            next
        -> false
    }
    -> true
}

#> dot product of this vector and another of the same dimension
.dot {
    need $ehs: Vector
    if $ehs.dimension != @dimension
        throw Error(:DimensionError, "Dimension mismatch $ehs.dimensionHR != @dimensionHR")
    $dot = 0
    for $i in 0..@items.lastIndex {
        $dot += *self[$i] * $ehs[$i]
    }
    -> $dot
}

#> cross product of two 3D vectors
.cross {
    need $ehs: Vector
    if $ehs.dimension != 3 || @dimension != 3
        throw Error(:DimensionError, "Cross product only exists in 3D")
    $a = *self[1] * $ehs[2] - *self[2] * $ehs[1]
    $b = *self[2] * $ehs[0] - *self[0] * $ehs[2]
    $c = *self[0] * $ehs[1] - *self[1] * $ehs[0]
    -> Vector($a, $b, $c)
}

#> angle between this vector and another of the same dimension, measured in
#| radians
.angleBetween {
    need $ehs: Vector
    # a·b = |a||b|cosθ
    $cosθ = @dot($ehs) / (@magnitude * $ehs.magnitude)
    -> Math.acos($cosθ)
}

#> true if this vector is orthogonal to another of the same dimension
# consider: the dot product is also zero if either is a zero vector. should
# we still -> true in that case?
.orthogonalTo {
    need $ehs: Vector
    -> @dot($ehs) == 0
}

#> true if this vector is parallel to another of the same dimension
.parallelTo {
    need $ehs: Vector
    if $ehs.dimension != @dimension
        throw Error(:DimensionError, "Dimension mismatch $ehs.dimensionHR != @dimensionHR")
    $u1 = @unitVector
    $u2 = $ehs.unitVector
    if $u1 == $u2 || -$u1 == $u2
        -> true
    -> false
}

#> fetches the component at the given index. Allows Vector to conform to
#| IndexedRead such that `$vector[N]` is the N+1th component.
.getValue {
    need $index: Num
    $n = $index + 1
    if @dimension < $n
        throw Error(:DimensionError, "@dimensionHR vector has no component $n")
    -> @items[$index]
}

#> returns a copy of the vector
method copy -> Vector(items: @items.copy!)

.description -> "<" + @items.join(", ") + ">"

#> returns the zero vector in the given dimension
func zeroVector {
    need $dimension: Num
    if $dimension < 1
        throw Error(:DimensionError, "Need dimension >= 1D")
    -> Vector(items: gather for $i in 1..$dimension { take 0 })
}

#> returns the unit vector for the given dimension and axis
func axisUnitVector {
    need $dimension: Num
    need $axis: VectorAxis #< axis number or letter, starting at 1 or "i"
    if $dimension < 1
        throw Error(:DimensionError, "Need dimension >= 1D")
    $items = gather for $i in 1..$dimension {
        if $i == $axis {
            take 1
            next
        }
        take 0
    }
    -> Vector(items: $items)
}

func _axisToNumber {
    need $axis: Num | Char
    if $axis.*isa(Num)
        -> $axis
    $o = $axis.ord
    if $o > 119
        -> $o - 119
    -> $o - 104
}

#> An interface which accepts an axis letter starting at "i" or number starting
#| at 1. For instance, a 3D vector is represented by axes i, j, and k, which
#| correspond to the numbers 1, 2, and 3, respectively.
type VectorAxis {
    #> converts letters starting at 'i' to axis numbers
    transform _axisToNumber($_)
}

package Math
class Vector
#< Represents a [vector](https://en.wikipedia.org/wiki/Vector_space) of any
#| dimension.

$docOption_instanceName = "v"

#> Creates a vector with the given components.
init {
    want @items: Num...
    if @dimension < 1
        throw Error(:DimensionError, "Need dimension >= 1D")
}

#> dimension of the vector
.dimension -> @items.length

#> human-readable dimension of the vector
.dimensionHR -> @dimension + "D"

#> magnitude of the vector
.magnitude -> @items.map! { -> $_ ^ 2 }.sum.sqrt

#> unit vector in the direction of this vector
.unitVector -> *self / @magnitude

#> returns the unit vector in the direction of the given axis
.axisUnitVector {
    need $axis: VectorAxis #< axis number or letter, starting at 1 or "i"
    -> Vector.axisUnitVector(@dimension, $axis)
}

.x -> *self[0]  #< for a >=1D vector, the first comonent
.y -> *self[1]  #< for a >=2D vector, the second component
.z -> *self[2]  #< for a >=3D vector, the third component

#> for a 2D vector, its direction, measured in radians
.direction {
    if @dimension != 2
        throw Error(:DimensionError, "Direction only exists in 2D")
    -> Math.atan2(@y, @x)
}

#> addition of two vectors
op + {
    need $ehs: Vector
    if $ehs.dimension != @dimension
        throw Error(:DimensionError, "Dimension mismatch $ehs.dimensionHR != @dimensionHR")
    $items = gather for $i in 0..@items.lastIndex {
        take *self[$i] + $ehs[$i]
    }
    -> Vector(items: $items)
}

#> allows you to take the opposite vector `-$u`
op - {
    need $lhs: Num
    if $lhs != 0
        throw(:InvalidOperation, "Unsupported operation")
    -> Vector(items: gather for $x in @items {
        take -$x
    })
}

#> subtraction of a vector from another
op - {
    need $rhs: Vector
    if $rhs.dimension != @dimension
        throw Error(:DimensionError, "Dimension mismatch $rhs.dimensionHR != @dimensionHR")
    $items = gather for $i in 0..@items.lastIndex {
        take *self[$i] - $rhs[$i]
    }
    -> Vector(items: $items)
}

#> scalar multiplication of the vector
op * {
    need $ehs: Num
    -> Vector(items: @items.map! { -> $_ * $ehs })
}

#> scalar division of the vector
op / {
    need $rhs: Num
    -> *self * (1 / $rhs)
}

#> dot product of two vectors
op * {
    need $ehs: Vector
    -> @dot($ehs)
}

#> vector equality
op == {
    need $ehs: Vector
    if $ehs.dimension != @dimension
        -> false
    for $i in 0..@items.lastIndex {
        if *self[$i] == $ehs[$i]
            next
        -> false
    }
    -> true
}

#> dot product of this vector and another of the same dimension
.dot {
    need $ehs: Vector
    if $ehs.dimension != @dimension
        throw Error(:DimensionError, "Dimension mismatch $ehs.dimensionHR != @dimensionHR")
    $dot = 0
    for $i in 0..@items.lastIndex {
        $dot += *self[$i] * $ehs[$i]
    }
    -> $dot
}

#> cross product of two 3D vectors
.cross {
    need $ehs: Vector
    if $ehs.dimension != 3 || @dimension != 3
        throw Error(:DimensionError, "Cross product only exists in 3D")
    @a = *self[1] * $ehs[2] - *self[2] * $ehs[1]
    @b = *self[2] * $ehs[0] - *self[0] * $ehs[2]
    @c = *self[0] * $ehs[1] - *self[1] * $ehs[0]
    -> Vector(@a, @b, @c)
}

#> angle between this vector and another of the same dimension, measured in
#| radians
.angleBetween {
    need $ehs: Vector
    # a·b = |a||b|cosθ
    $cosθ = @dot($ehs) / (@magnitude * $ehs.magnitude)
    -> Math.acos($cosθ)
}

#> true if this vector is orthogonal to another of the same dimension
# consider: the dot product is also zero if either is a zero vector. should
# we still -> true in that case?
.orthogonalTo {
    need $ehs: Vector
    -> @dot($ehs) == 0
}

#> true if this vector is parallel to another of the same dimension
.parallelTo {
    need $ehs: Vector
    if $ehs.dimension != @dimension
        throw Error(:DimensionError, "Dimension mismatch $ehs.dimensionHR != @dimensionHR")
    $u1 = @unitVector
    $u2 = $ehs.unitVector
    if $u1 == $u2 || -$u1 == $u2
        -> true
    -> false
}

#> fetches the component at the given index. Allows Vector to conform to
#| IndexedRead such that `$vector[N]` is the N+1th component.
.getValue {
    need $index: Num
    $n = $index + 1
    if @dimension < $n
        throw Error(:DimensionError, "@dimensionHR vector has no component $n")
    -> @items[$index]
}

#> returns a copy of the vector
method copy -> Vector(items: @items.copy!)

.description -> "<" + @items.join(", ") + ">"

#> returns the zero vector in the given dimension
func zeroVector {
    need $dimension: Num
    if $dimension < 1
        throw Error(:DimensionError, "Need dimension >= 1D")
    -> Vector(items: gather for $i in 1..$dimension { take 0 })
}

#> returns the unit vector for the given dimension and axis
func axisUnitVector {
    need $dimension: Num
    need $axis: VectorAxis #< axis number or letter, starting at 1 or "i"
    if $dimension < 1
        throw Error(:DimensionError, "Need dimension >= 1D")
    $items = gather for $i in 1..$dimension {
        if $i == $axis {
            take 1
            next
        }
        take 0
    }
    -> Vector(items: $items)
}

func _axisToNumber {
    need $axis: Num | Char
    if $axis.*isa(Num)
        -> $axis
    $o = $axis.ord
    if $o > 119
        -> $o - 119
    -> $o - 104
}

#> An interface which accepts an axis letter starting at "i" or number starting
#| at 1. For instance, a 3D vector is represented by axes i, j, and k, which
#| correspond to the numbers 1, 2, and 3, respectively.
type VectorAxis {
    #> converts letters starting at 'i' to axis numbers
    transform _axisToNumber($_)
}


boot: update copyright like done on stable branch

\ Copyright (c) 2006-2015 Devin Teske <dteske@FreeBSD.org>
\ Copyright (c) 2016-2017 Deciso B.V.
\ All rights reserved.
\
\ Redistribution and use in source and binary forms, with or without
\ modification, are permitted provided that the following conditions
\ are met:
\ 1. Redistributions of source code must retain the above copyright
\    notice, this list of conditions and the following disclaimer.
\ 2. Redistributions in binary form must reproduce the above copyright
\    notice, this list of conditions and the following disclaimer in the
\    documentation and/or other materials provided with the distribution.
\
\ THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
\ ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
\ IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
\ ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
\ FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
\ DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
\ OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
\ HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
\ LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
\ OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
\ SUCH DAMAGE.
\
\ $FreeBSD$

48 logoX ! 9 logoY ! \ Initialize logo placement defaults

: logo+ ( x y c-addr/u -- x y' )
	2swap 2dup at-xy 2swap \ position the cursor
	[char] # escc! \ replace # with Esc
	type \ print to the screen
	1+ \ increase y for next time we're called
;

: logo ( x y -- ) \ color hourglass logo (15 rows x 32 columns)

	s" #[37;1m  @@@@@@@@@@@@@@@@@@@@@@@@@@@@" logo+
	s" @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@" logo+
	s" @@@@@                    @@@@@" logo+
	s"     @@@@@            @@@@@    " logo+
	s"  @@@@@@@@@@@       @@@@@@@@@@@" logo+
	s"       #[31;1m\\\\\         /////     " logo+
	s" ))))))))))))       (((((((((((" logo+
	s"       /////         \\\\\     #[m" logo+
	s" #[37;1m @@@@@@@@@@@       @@@@@@@@@@@" logo+
	s"     @@@@@            @@@@@    " logo+
	s" @@@@@                    @@@@@" logo+
	s" @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@" logo+
	s" @@@@@@@@@@@@@@@@@@@@@@@@@@@@  " logo+
	s"  #[m                              " logo+
	s"   17.7  ``Insert Name Here''  #[m" logo+

	2drop
;

\ Copyright (c) 2006-2015 Devin Teske <dteske@FreeBSD.org>
\ Copyright (c) 2016 Deciso B.V.
\ All rights reserved.
\
\ Redistribution and use in source and binary forms, with or without
\ modification, are permitted provided that the following conditions
\ are met:
\ 1. Redistributions of source code must retain the above copyright
\    notice, this list of conditions and the following disclaimer.
\ 2. Redistributions in binary form must reproduce the above copyright
\    notice, this list of conditions and the following disclaimer in the
\    documentation and/or other materials provided with the distribution.
\
\ THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
\ ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
\ IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
\ ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
\ FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
\ DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
\ OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
\ HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
\ LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
\ OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
\ SUCH DAMAGE.
\
\ $FreeBSD$

48 logoX ! 9 logoY ! \ Initialize logo placement defaults

: logo+ ( x y c-addr/u -- x y' )
	2swap 2dup at-xy 2swap \ position the cursor
	[char] # escc! \ replace # with Esc
	type \ print to the screen
	1+ \ increase y for next time we're called
;

: logo ( x y -- ) \ color hourglass logo (15 rows x 32 columns)

	s" #[37;1m  @@@@@@@@@@@@@@@@@@@@@@@@@@@@" logo+
	s" @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@" logo+
	s" @@@@@                    @@@@@" logo+
	s"     @@@@@            @@@@@    " logo+
	s"  @@@@@@@@@@@       @@@@@@@@@@@" logo+
	s"       #[31;1m\\\\\         /////     " logo+
	s" ))))))))))))       (((((((((((" logo+
	s"       /////         \\\\\     #[m" logo+
	s" #[37;1m @@@@@@@@@@@       @@@@@@@@@@@" logo+
	s"     @@@@@            @@@@@    " logo+
	s" @@@@@                    @@@@@" logo+
	s" @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@" logo+
	s" @@@@@@@@@@@@@@@@@@@@@@@@@@@@  " logo+
	s"  #[m                              " logo+
	s"   17.7  ``Insert Name Here''  #[m" logo+

	2drop
;


Move things around a little.

Move things around a little.

(( Clock Code ))

\ The gadget that drives it all.   This is rigged for debugging,
\ terminate it by setting clockterm to non-zero.
: CLOCKSTART ( -- )
  init-idata
  load-defaults
  icroot 0 over inter.rtcsem ! \ Reset the free-running counters.
         0 over inter.rtcdsem ! \ Reset the free-running counters.

  begin
   	dup inter.rtcsem  @offex! ?dup if advancehms then
   	dup inter.rtcdsem @offex! ?dup if advancedhms then
    [asm wfi asm]
    uiupdate if use-uivals else use-clockvals then
  key? until drop
;

\ These two contain loops to ensure that we don't
\ drop any timer ticks on the floor.
: ADVANCEHMS ( n -- ) 0 do hms advancetime loop ;
: ADVANCEDHMS ( n -- ) 0 do dhms advancetime loop ;

: LOAD-DEFAULTS ( -- )
  nvramvalid? if _nvramload then
  interp-init
;

\ ----------------------------------------------------------
\ Define the structures before they get used.
\ ----------------------------------------------------------
struct ODN \ On-Deck Needle
	int odn.s
	int odn.m
	int odn.h
end-struct

struct _HMS
	int hms.subsec
	int hms.s
	int hms.m
	int hms.h
	int hms.maxsubsec \ Max Value for subseconds
	int hms.maxsec   \ Max Value for secs+minutes
	int hms.maxhour   \ Max for hours
	ptr hms.w_s    \ Increment the seconds value
	ptr hms.w_m    \ increment the minutes + hours
end-struct

struct _interp_set
	4 cells field interp.a
	4 cells field interp.b
	4 cells field interp.c
end-struct

\ ----------------------------------------------------------
\ Defaults get saved in the user data page.
\ ----------------------------------------------------------
: NVRAMVALID? ( -- t/f )
\ *G See if there is valid data in the NVRAM.
\ ** It consists of 3 words.  If any of them are
\ ** set to 0xffff:ffff, we go with the defaults.
  _USERDATA
  dup      @ -1 <>
  over 4 + @ -1 <> and
  swap 8 + @ -1 <> and
;

: _NVRAMLOAD ( -- )
\ *G Pull the needle maximums from flash.
  $C 0 do I ud@ needle_max I + !   4 +loop
;

: NVRAM! ( addr -- )
\ *G Save the contents of the needle cal values.
 0 UDPAGE_ERASE
 ( addr )
 $C 0 do dup I + @  I ud!  4 +loop
 drop
;

\ ----------------------------------------------------------
\ Needle Management
\ ----------------------------------------------------------
idata
create ODN_HMS  odn allot \ On-Deck, HMS
create ODN_DHMS odn allot \ On-Deck, DHMS
create ODN_UI   odn allot \ On-Deck, Generated by the UI.
cdata

idata \ Has to match an odn.
create NEEDLE_MAX #850 , #850 , #850 ,
cdata

((
: interp-next drop 1 ;
: interp-reset drop ." Reset" ;

))

: RANGECHECK ( max n -- n or zero ) 2dup <= if 2drop 0 else swap drop then ;
: QUADCLIP  ( n -- n) \ Force the contents to be legal ( 0-999 )
  dup 0 < if drop 0 exit then
  dup #999 > if drop #999 then
;

: INTER-BUMP (  max old interp -- new )
\ *G Get the next value from the interpolator, and
\ ** reset the interpolator if it wraps around to zero.
  >R R@ interp-next  \ Get the max
  + rangecheck
  dup 0= if R> interp-reset else R> drop then
  ;

: NFETCH ( odn off -- max old )
\ *G Combine the current value with the maximum.
  >R R@ \ stash the offset.
  +  @ \ Calculate the offset address, get the current val.
  needle_max R> + @
  swap
;

: ++NEEDLE_S \ Called every time.
    odn_hms 0 odn.s nfetch ( max old )
	interp_hms interp.a inter-bump ( new )
	odn_hms odn.s !
	;

: ++NEEDLE_M ( -- )
\ *G Every time we roll the seconds, bump the minutes and the hour
	odn_hms 0 odn.m nfetch ( max old )
	interp_hms interp.b inter-bump
	odn_hms odn.m !

	odn_hms 0 odn.h nfetch ( max old )
	interp_hms interp.c inter-bump
	odn_hms odn.h !
	;

: ++NEEDLE_DS ; \ Called every time.
: ++NEEDLE_DM ; \ Every time we roll the seconds.

: _USE ( odn-addr -- )
  dup odn.s w@ pwm0!
  dup odn.m w@ pwm1!
      odn.h w@ pwm2!
  ;

: USE-CLOCKVALS odn_hms _use ;
: USE-UIVALS    odn_ui  _use ;

\ ----------------------------------------------------------
\ PWM/Timer Manipulation
\ ----------------------------------------------------------
_timer0 $34 + equ _PWM0
_timer0 $44 + equ _PWM1
_timer0 $54 + equ _PWM2

: PWM0! ( n -- ) _pwm0 ! ;
: PWM1! ( n -- ) _pwm1 ! ;
: PWM2! ( n -- ) _pwm2 ! ;

: PWM0@ ( -- n ) _pwm0 w@ ;
: PWM1@ ( -- n ) _pwm1 w@ ;
: PWM2@ ( -- n ) _pwm2 w@ ;

: SCALE0 #1000 0 do I pwm0! loop ;
: SCALE1 #1000 0 do I pwm0! [asm wfi asm] loop ;

\ ----------------------------------------------------------
\ PWM/Timer Quadrature
\ ----------------------------------------------------------

_timer1 $24 + equ QUAD-IN

\ The Quadrature encoder produces two per detent.
: QUAD@ ( -- n ) \ Fetch and zero
  quad-in @off
  [asm sxth tos, tos asm]
  [asm asr .s tos, tos, # 1 asm] \ Divide by two,
  ;

\ ----------------------------------------------------------
\ Keeping track of the time.
\ ----------------------------------------------------------
idata
create HMS
  0 , 0 , 0 , 0 , \ Running Counters
  #16 , #60 , #24 , \ Limits
  ' ++needle_s , ' ++needle_m , \ Words to invoke
create DHMS
  0 , 0 , 0 , 0 , \ Running Counters
  #10 , #100 , #10 , \ Limits
' ++needle_ds , ' ++needle_dm , \ Words to invoke
cdata

\ Core concept - We update the seconds
\ each time this gets called.
\ If the seconds wrap, we update the minutes
\ and the hours.
: ADVANCETIME ( struct-addr -- )
  dup hms.w_s @ execute \ Invoke the official update word.
  1 over hms.subsec +!  \ Increment
   dup hms.subsec @
   over hms.maxsubsec @ < if drop exit then \ If less then max, we're done
  0 over hms.subsec ! \ Reset the subsecs

  1 over hms.s +!
   dup hms.s @ over hms.maxsec @ < if drop exit then
  0 over hms.s ! \ Reset the seconds

  dup hms.w_m @ execute
  1 over hms.m +!
   dup hms.m @ over hms.maxsec @ < if drop exit then
  0 over hms.m ! \ Reset the minutes

  1 over hms.h +!
   dup hms.h @ over hms.maxhour @ < if drop exit then
  0 over hms.h ! \ Reset the minutes

  drop

;

: ? @ . ;
: .hms ( addr -- )
  dup hms.h ?
  dup hms.m ?
  dup hms.s ?
  hms.subsec ?
  cr
;

\ : foo 0 do hms advancetime loop ;

\ ----------------------------------------------------------
\ The interpolator.
\ Implement bresenhams algorithm.  All of this code already
\ exists in C, but we'll own the data structures.  They contain
\ A total of 4 words - fixed, err, num, & denom
\ ----------------------------------------------------------
udata
create interp_hms  _interp_set allot
create interp_dhms _interp_set allot
cdata

#16  #60 * equ raw_sec
#10 #100 * equ raw_dsec

: INTERP-INIT ( -- )
  (interp_init) interp_hms
  2dup interp.a needle_max     @ raw_sec call3--
  2dup interp.b needle_max 4 + @     #60 call3--
       interp.c needle_max 8 + @     #24 #60 * call3--

  (interp_init) interp_dhms
  2dup interp.a needle_max     @  raw_dsec call3--
  2dup interp.b needle_max 4 + @      #100 call3--
       interp.c needle_max 8 + @       #10 #100 * call3--
;

\ Call the reset fn.
: INTERP-RESET ( interp_addr -- )
  (interp_reset) swap call1--
;

: INTERP-NEXT ( addr -- n )
  (interp_next) swap call1--n
;

\ ----------------------------------------------------------
\ ----------------------------------------------------------
\ User Interface
\ ----------------------------------------------------------
\ ----------------------------------------------------------
variable uistate
variable uicount

_GPIO $64  + equ _BUTTONIO
: BUTTONUP?   ( -- t/f ) _buttonio @ 1 and ;
: BUTTONDOWN? ( -- t/f )  buttonup? 1 xor ;

\ The wrapper for all of the UI stuff.
\ Returns true if the UI is active - ie, use the values generated by the UI.
: UIUPDATE ( -- t/f )
  StateHandlers uistate @ + @ execute
;

: uitest
  uistate @ 4 / . ." ->" uiupdate uistate @ 4 / .
  uicount @ . if ." True" else ." False" then
;

\ -----------------------------------------------
\ State numbers.  These are defined x4 so that
\ we can just use the state number as an index
\ into a table of words.
\ -----------------------------------------------
0 4 * equ _s_init
1 4 * equ _s_b1
2 4 * equ _s_1sec

3 4 * equ _s_shSetH
4 4 * equ _s_pendset_m
5 4 * equ _s_setmin

6 4 * equ _s_pendcal0
7 4 * equ _s_cal0
8 4 * equ _s_pendcalh
9 4 * equ _s_calh
10 4 * equ _s_pendcalm
11 4 * equ _s_calm
12 4 * equ _s_pendcals
13 4 * equ _s_cals

4 equ downcount_1s
16 equ downcount_3s

\ All of the state handlers.
\ Everybody needs to know the state of the button, so that must
\ get passed in, thus they all look like this:
\ state ( t/f -- t/f )
: shInit false
   buttondown? if
   _s_b1 uistate !
   uicount off \ Reset this.
   then
  ;

\ Waiting for a full second to go by.
: shB1 false
  buttonup? if _s_init uistate ! exit then
  \ The Button is down.
  uicount @ downcount_1s >= if
    _s_1sec uistate !
    else
    1 uicount +!
	then
  ;

\ We're in the 1 second state
: sh1sec true
  buttonup? if shseth_entry exit then
  uicount @ downcount_3s >= if
    _s_pendcal0 uistate !
    else
    1 uicount +!
	then
;

\ -------------------------------------------------
\ Setting the time
\ -------------------------------------------------
: shSetH_entry
  _s_shseth uistate !
  adj_i off \ Set the index to zero
  needle_max odn.h @ #24 make-set-list
;

: shSetH true buttondown? if shPendSetM_entry exit then
 adj_i @ quad@ - 24 mod \ Now we have the next index.
 dup 0< if #24 + then   \ Negative wrap-around isn't cool
 dup adj_i ! \ Save it.
 adj_points[]@   \ Get the new value.
 odn_ui odn.h !
 ;

: shPendSetM_entry _s_pendset_m uistate ! ;

: shPendSetM true  buttonup? if _s_setmin uistate ! then ;
: shSetMin true  buttondown? if _s_init uistate ! exit then ;

\ -------------------------------------------------
\ Calibration
\ -------------------------------------------------
: shPendCal0 true  buttonup? if _s_cal0 uistate ! helpODNClear then ;
: shCal0 true buttondown? if _s_pendcalh uistate ! exit then ;

: shPendCalH true buttonup? if _s_calh uistate ! helpODNMid then ;
: shCalH true   buttondown? if _s_pendcalm uistate ! exit then
  odn_ui odn.h helpQuad@ ;

: shPendCalM true buttonup? if _s_calm uistate ! then ;
: shCalM true   buttondown? if _s_pendcals uistate ! exit then
  odn_ui odn.m helpQuad@ ;

: shPendCalS true buttonup? if _s_cals uistate ! then ;
: shCalS true   buttondown? if
  _s_init uistate !
  odn_ui nvram! exit then

  odn_ui odn.s helpQuad@ ;

\ -------------------------------------------------
\ Helpers
\ -------------------------------------------------
: helpODNClear ( -- ) \ Set them all to zero
  odn_ui odn bounds do I off 4 +loop ;
: helpODNMid ( -- ) \ Set them all to 500
  odn_ui odn bounds do #750 I ! 4 +loop ;
: helpQuad@ ( addr -- ) \ update a location with the quadrature value
  dup @ quad@ + quadclip swap ! ;

create StateHandlers
' shInit ,
' shB1 ,
' sh1sec ,

' shSetH ,
' shPendSetM ,  ' shSetMin ,

' shPendCal0 ,  ' shCal0 ,
' shPendCalH ,  ' shCalH ,
' shPendCalM ,  ' shCalM ,
' shPendCalS ,  ' shCalS ,

\ -------------------------------------------------
\ Setting the time.
\ -------------------------------------------------
variable adj_i \ THe working index into the points array.

udata
create adj_points #50 cells allot \ 100 16-bit words.
create interp_set  _interp_set allot
cdata

\ Generate a list of points.
: MAKE-SET-LIST ( max steps )
  2dup
  >R  (interp_init) swap interp_set swap R> call3--

  \ Since zero is first, do the w! first.
  swap drop   0 swap \ Keep a running counter.
  0 do
    dup I adj_points[]! \ Save the existing value
    interp_set interp-next +
  loop
  drop
  ;

: ADJ_POINTS[]! ( data index -- ) 2* adj_points + w! ;
: ADJ_POINTS[]@ ( index -- n )    2* adj_points + w@ ;

(( Clock Code ))

\ The gadget that drives it all.   This is rigged for debugging,
\ terminate it by setting clockterm to non-zero.
: CLOCKSTART ( -- )
  init-idata
  load-defaults
  icroot 0 over inter.rtcsem ! \ Reset the free-running counters.
         0 over inter.rtcdsem ! \ Reset the free-running counters.

  begin
   	dup inter.rtcsem  @offex! ?dup if advancehms then
   	dup inter.rtcdsem @offex! ?dup if advancedhms then
    [asm wfi asm]
    uiupdate if use-uivals else use-clockvals then
  key? until drop
;

\ These two contain loops to ensure that we don't
\ drop any timer ticks on the floor.
: ADVANCEHMS ( n -- ) 0 do hms advancetime loop ;
: ADVANCEDHMS ( n -- ) 0 do dhms advancetime loop ;

: LOAD-DEFAULTS ( -- )
  nvramvalid? if _nvramload then
  interp-init
;

\ ----------------------------------------------------------
\ Define the structures before they get used.
\ ----------------------------------------------------------
struct ODN \ On-Deck Needle
	int odn.s
	int odn.m
	int odn.h
end-struct

struct _HMS
	int hms.subsec
	int hms.s
	int hms.m
	int hms.h
	int hms.maxsubsec \ Max Value for subseconds
	int hms.maxsec   \ Max Value for secs+minutes
	int hms.maxhour   \ Max for hours
	ptr hms.w_s    \ Increment the seconds value
	ptr hms.w_m    \ increment the minutes + hours
end-struct

struct _interp_set
	4 cells field interp.a
	4 cells field interp.b
	4 cells field interp.c
end-struct

\ ----------------------------------------------------------
\ Defaults get saved in the user data page.
\ ----------------------------------------------------------
: NVRAMVALID? ( -- t/f )
\ *G See if there is valid data in the NVRAM.
\ ** It consists of 3 words.  If any of them are
\ ** set to 0xffff:ffff, we go with the defaults.
  _USERDATA
  dup      @ -1 <>
  over 4 + @ -1 <> and
  swap 8 + @ -1 <> and
;

: _NVRAMLOAD ( -- )
\ *G Pull the needle maximums from flash.
  $C 0 do I ud@ needle_max I + !   4 +loop
;

: NVRAM! ( addr -- )
\ *G Save the contents of the needle cal values.
 0 UDPAGE_ERASE
 ( addr )
 $C 0 do dup I + @  I ud!  4 +loop
 drop
;

\ ----------------------------------------------------------
\ Needle Management
\ ----------------------------------------------------------
idata
create ODN_HMS  odn allot \ On-Deck, HMS
create ODN_DHMS odn allot \ On-Deck, DHMS
create ODN_UI   odn allot \ On-Deck, Generated by the UI.
cdata

idata \ Has to match an odn.
create NEEDLE_MAX #850 , #850 , #850 ,
cdata

((
: interp-next drop 1 ;
: interp-reset drop ." Reset" ;

))

: RANGECHECK ( max n -- n or zero ) 2dup <= if 2drop 0 else swap drop then ;
: QUADCLIP  ( n -- n) \ Force the contents to be legal ( 0-999 )
  dup 0 < if drop 0 exit then
  dup #999 > if drop #999 then
;

: INTER-BUMP (  max old interp -- new )
\ *G Get the next value from the interpolator, and
\ ** reset the interpolator if it wraps around to zero.
  >R R@ interp-next  \ Get the max
  + rangecheck
  dup 0= if R> interp-reset else R> drop then
  ;

: NFETCH ( odn off -- max old )
\ *G Combine the current value with the maximum.
  >R R@ \ stash the offset.
  +  @ \ Calculate the offset address, get the current val.
  needle_max R> + @
  swap
;

: ++NEEDLE_S \ Called every time.
    odn_hms 0 odn.s nfetch ( max old )
	interp_hms interp.a inter-bump ( new )
	odn_hms odn.s !
	;

: ++NEEDLE_M ( -- )
\ *G Every time we roll the seconds, bump the minutes and the hour
	odn_hms 0 odn.m nfetch ( max old )
	interp_hms interp.b inter-bump
	odn_hms odn.m !

	odn_hms 0 odn.h nfetch ( max old )
	interp_hms interp.c inter-bump
	odn_hms odn.h !
	;

: ++NEEDLE_DS ; \ Called every time.
: ++NEEDLE_DM ; \ Every time we roll the seconds.

: _USE ( odn-addr -- )
  dup odn.s w@ pwm0!
  dup odn.m w@ pwm1!
      odn.h w@ pwm2!
  ;

: USE-CLOCKVALS odn_hms _use ;
: USE-UIVALS    odn_ui  _use ;

\ ----------------------------------------------------------
\ PWM/Timer Manipulation
\ ----------------------------------------------------------
_timer0 $34 + equ _PWM0
_timer0 $44 + equ _PWM1
_timer0 $54 + equ _PWM2

: PWM0! ( n -- ) _pwm0 ! ;
: PWM1! ( n -- ) _pwm1 ! ;
: PWM2! ( n -- ) _pwm2 ! ;

: PWM0@ ( -- n ) _pwm0 w@ ;
: PWM1@ ( -- n ) _pwm1 w@ ;
: PWM2@ ( -- n ) _pwm2 w@ ;

: SCALE0 #1000 0 do I pwm0! loop ;
: SCALE1 #1000 0 do I pwm0! [asm wfi asm] loop ;

\ ----------------------------------------------------------
\ PWM/Timer Quadrature
\ ----------------------------------------------------------

_timer1 $24 + equ QUAD-IN

\ The Quadrature encoder produces two per detent.
: QUAD@ ( -- n ) \ Fetch and zero
  quad-in @off
  [asm sxth tos, tos asm]
  [asm asr .s tos, tos, # 1 asm] \ Divide by two,
  ;

\ ----------------------------------------------------------
\ Keeping track of the time.
\ ----------------------------------------------------------
idata
create HMS
  0 , 0 , 0 , 0 , \ Running Counters
  #16 , #60 , #24 , \ Limits
  ' ++needle_s , ' ++needle_m , \ Words to invoke
create DHMS
  0 , 0 , 0 , 0 , \ Running Counters
  #10 , #100 , #10 , \ Limits
' ++needle_ds , ' ++needle_dm , \ Words to invoke
cdata

\ Core concept - We update the seconds
\ each time this gets called.
\ If the seconds wrap, we update the minutes
\ and the hours.
: ADVANCETIME ( struct-addr -- )
  dup hms.w_s @ execute \ Invoke the official update word.
  1 over hms.subsec +!  \ Increment
   dup hms.subsec @
   over hms.maxsubsec @ < if drop exit then \ If less then max, we're done
  0 over hms.subsec ! \ Reset the subsecs

  1 over hms.s +!
   dup hms.s @ over hms.maxsec @ < if drop exit then
  0 over hms.s ! \ Reset the seconds

  dup hms.w_m @ execute
  1 over hms.m +!
   dup hms.m @ over hms.maxsec @ < if drop exit then
  0 over hms.m ! \ Reset the minutes

  1 over hms.h +!
   dup hms.h @ over hms.maxhour @ < if drop exit then
  0 over hms.h ! \ Reset the minutes

  drop

;

: ? @ . ;
: .hms ( addr -- )
  dup hms.h ?
  dup hms.m ?
  dup hms.s ?
  hms.subsec ?
  cr
;

\ : foo 0 do hms advancetime loop ;

\ ----------------------------------------------------------
\ The interpolator.
\ Implement bresenhams algorithm.  All of this code already
\ exists in C, but we'll own the data structures.  They contain
\ A total of 4 words - fixed, err, num, & denom
\ ----------------------------------------------------------
udata
create interp_hms  _interp_set allot
create interp_dhms _interp_set allot
cdata

#16  #60 * equ raw_sec
#10 #100 * equ raw_dsec

: INTERP-INIT ( -- )
  (interp_init) interp_hms
  2dup interp.a needle_max     @ raw_sec call3--
  2dup interp.b needle_max 4 + @     #60 call3--
       interp.c needle_max 8 + @     #24 #60 * call3--

  (interp_init) interp_dhms
  2dup interp.a needle_max     @  raw_dsec call3--
  2dup interp.b needle_max 4 + @      #100 call3--
       interp.c needle_max 8 + @       #10 #100 * call3--
;

\ Call the reset fn.
: INTERP-RESET ( interp_addr -- )
  (interp_reset) swap call1--
;

: INTERP-NEXT ( addr -- n )
  (interp_next) swap call1--n
;

\ ----------------------------------------------------------
\ ----------------------------------------------------------
\ User Interface
\ ----------------------------------------------------------
\ ----------------------------------------------------------
variable uistate
variable uicount

_GPIO $64  + equ _BUTTONIO
: BUTTONUP?   ( -- t/f ) _buttonio @ 1 and ;
: BUTTONDOWN? ( -- t/f )  buttonup? 1 xor ;

\ The wrapper for all of the UI stuff.
\ Returns true if the UI is active - ie, use the values generated by the UI.
: UIUPDATE ( -- t/f )
  StateHandlers uistate @ + @ execute
;

: uitest
  uistate @ 4 / . ." ->" uiupdate uistate @ 4 / .
  uicount @ . if ." True" else ." False" then
;

\ -----------------------------------------------
\ State numbers.  These are defined x4 so that
\ we can just use the state number as an index
\ into a table of words.
\ -----------------------------------------------
0 4 * equ _s_init
1 4 * equ _s_b1
2 4 * equ _s_1sec

3 4 * equ _s_shSetH
4 4 * equ _s_pendset_m
5 4 * equ _s_setmin

6 4 * equ _s_pendcal0
7 4 * equ _s_cal0
8 4 * equ _s_pendcalh
9 4 * equ _s_calh
10 4 * equ _s_pendcalm
11 4 * equ _s_calm
12 4 * equ _s_pendcals
13 4 * equ _s_cals

4 equ downcount_1s
16 equ downcount_3s

\ All of the state handlers.
\ Everybody needs to know the state of the button, so that must
\ get passed in, thus they all look like this:
\ state ( t/f -- t/f )
: shInit false
   buttondown? if
   _s_b1 uistate !
   uicount off \ Reset this.
   then
  ;

\ Waiting for a full second to go by.
: shB1 false
  buttonup? if _s_init uistate ! exit then
  \ The Button is down.
  uicount @ downcount_1s >= if
    _s_1sec uistate !
    else
    1 uicount +!
	then
  ;

\ We're in the 1 second state
: sh1sec true
  buttonup? if shseth_entry _s_shseth uistate !  exit then
  uicount @ downcount_3s >= if
    _s_pendcal0 uistate !
    else
    1 uicount +!
	then
;

\ -------------------------------------------------
\ Setting the time
\ -------------------------------------------------
: shSetH_entry
  adj_i off \ Set the index to zero
  needle_max odn.h @ #24 make-set-list
;

: shSetH true buttondown? if _s_pendset_m uistate ! exit then
 adj_i @ quad@ - 24 mod \ Now we have the next index.
 dup 0< if #24 + then   \ Negative wrap-around isn't cool
 dup adj_i ! \ Save it.
 adj_points[]@   \ Get the new value.
 odn_ui odn.h !
 ;

: shPendSetM true  buttonup? if _s_setmin uistate ! then ;
: shSetMin true  buttondown? if _s_init uistate ! exit then ;

\ -------------------------------------------------
\ Calibration
\ -------------------------------------------------
: shPendCal0 true  buttonup? if _s_cal0 uistate ! helpODNClear then ;
: shCal0 true buttondown? if _s_pendcalh uistate ! exit then ;

: shPendCalH true buttonup? if _s_calh uistate ! helpODNMid then ;
: shCalH true   buttondown? if _s_pendcalm uistate ! exit then
  odn_ui odn.h helpQuad@ ;

: shPendCalM true buttonup? if _s_calm uistate ! then ;
: shCalM true   buttondown? if _s_pendcals uistate ! exit then
  odn_ui odn.m helpQuad@ ;

: shPendCalS true buttonup? if _s_cals uistate ! then ;
: shCalS true   buttondown? if
  _s_init uistate !
  odn_ui nvram! exit then

  odn_ui odn.s helpQuad@ ;

\ -------------------------------------------------
\ Helpers
\ -------------------------------------------------
: helpODNClear ( -- ) \ Set them all to zero
  odn_ui odn bounds do I off 4 +loop ;
: helpODNMid ( -- ) \ Set them all to 500
  odn_ui odn bounds do #750 I ! 4 +loop ;
: helpQuad@ ( addr -- ) \ update a location with the quadrature value
  dup @ quad@ + quadclip swap ! ;

create StateHandlers
' shInit ,
' shB1 ,
' sh1sec ,

' shSetH ,
' shPendSetM ,  ' shSetMin ,

' shPendCal0 ,  ' shCal0 ,
' shPendCalH ,  ' shCalH ,
' shPendCalM ,  ' shCalM ,
' shPendCalS ,  ' shCalS ,

\ -------------------------------------------------
\ Setting the time.
\ -------------------------------------------------
variable adj_i \ THe working index into the points array.

udata
create adj_points #50 cells allot \ 100 16-bit words.
create interp_set  _interp_set allot
cdata

\ Generate a list of points.
: MAKE-SET-LIST ( max steps )
  2dup
  >R  (interp_init) swap interp_set swap R> call3--

  \ Since zero is first, do the w! first.
  swap drop   0 swap \ Keep a running counter.
  0 do
    dup I adj_points[]! \ Save the existing value
    interp_set interp-next +
  loop
  drop
  ;

: ADJ_POINTS[]! ( data index -- ) 2* adj_points + w! ;
: ADJ_POINTS[]@ ( index -- n )    2* adj_points + w@ ;


refactoring

refactoring

\ KataDiversion in Forth

\ -- utils

\ empty the stack
: EMPTY
    DEPTH 0 <> IF BEGIN
                    DROP DEPTH 0 =
                  UNTIL
               THEN ;

\ power
: ** ( n1 n2 -- n1_pow_n2 ) 1 SWAP ?DUP IF 0 DO OVER * LOOP THEN NIP ;

\ test if the top is a negative number
: ?NEG ( n -- bool ) DUP 0= IF -1 ELSE DUP ABS <> THEN ;

\ log2 (integer)
\ TODO remove this word, and add a new word
\ which compute the value of ( n LOG2 2 SWAP ** )
: LOG2 ( n -- log2_n ) DUP 1 < IF 1 ABORT" Log2 need a positive value."
                               ELSE DUP 1 = IF 0
                                            ELSE
                                                1 >R
                                                BEGIN ( n |R: i=1)
                                                    DUP DUP 2 I ** - 2 *
                                                    ( n n 2*[n-2**i])
                                                    R> 1 + >R ( … |R: i+1)
                                                    > ( n n>2*[n-2**i] )
                                                UNTIL
                                                R> 1 -
                                            THEN
                               THEN NIP ;

\ -- kata

\ test if the given N has two adjacent 1 bits
\ e.g. : 11 -> 1011 -> -1
\         9 -> 1001 ->  0
: ?TWO-ADJACENT-1-BITS ( n -- bool )
    \ the word uses the following algorithm :
    \ (stack|return stack)
    \ ( A N | X )  A: 0, X: N LOG2
    \ loop: if N-X > 0 then A++ else A=0 ; X /= 2
    \       return -1 if A=2
    \       if X=1 end loop and return 0
    0 SWAP DUP DUP 0 <> IF
                            LOG2 2 SWAP ** >R
                            BEGIN
                                DUP I - 0 >= IF
                                                SWAP DUP 1 = IF 1+ SWAP
                                                             ELSE 1+ SWAP I -
                                                            THEN
                                             ELSE NIP 0 SWAP
                                             THEN
                                OVER
                                2 =
                                I 1 = OR
                                R> 2 / >R
                            UNTIL
                            R> 2DROP
                            2 =
                       THEN ;

\ return the maximum number which can be made with N (given number) bits
: ?MAX-NB ( n -- m ) DUP ?NEG IF DROP 0 ( 0 )
                              ELSE
                                  DUP IF DUP 2 SWAP ** NIP ( 2**n )
                                  THEN
                              THEN ;

\ return the number of numbers which can be made with N (given number) bits
\ or less, and which have not two adjacent 1 bits.
\ see http://www.codekata.com/2007/01/code_kata_fifte.html
: ?HOW-MANY-NB-NOT-TWO-ADJACENT-1-BITS ( n -- m ) ( TODO ) ;

\ KataDiversion in Forth

\ -- utils

\ empty the stack
: EMPTY
    DEPTH 0 <> IF BEGIN
                    DROP DEPTH 0 =
                  UNTIL
               THEN ;

\ power
: ** ( n1 n2 -- n1_pow_n2 ) 1 SWAP ?DUP IF 0 DO OVER * LOOP THEN NIP ;

\ test if the top is a negative number
: ?NEG ( n -- bool ) DUP 0= IF -1 ELSE DUP ABS <> THEN ;

\ log2 (integer)
: LOG2 ( n -- log2_n ) DUP 1 < IF 1 ABORT" Log2 need a positive value."
                               ELSE DUP 1 = IF 0
                                            ELSE
                                                1 >R
                                                BEGIN ( n |R: i=1)
                                                    DUP DUP 2 I ** - 2 *
                                                    ( n n 2*[n-2**i])
                                                    R> 1 + >R ( … |R: i+1)
                                                    > ( n n>2*[n-2**i] )
                                                UNTIL
                                                R> 1 -
                                            THEN
                               THEN NIP ;

\ decimal to binary
\ e.g. : ( 11 -- 1 0 1 1 )
: DEC2BIN ( n -- n1 n2 n3 … ) DUP DUP 0 <> IF
                                        LOG2 2 SWAP ** >R ( n |R: X=2 ** n.log2 )
                                        BEGIN
                                            DUP I - 0 >= IF 1 SWAP I - ( 1 n-X )
                                                         ELSE 0 SWAP   ( 0 n )
                                                         THEN
                                            I 1 =
                                            R> 2 / >R ( … | X/2 )
                                        UNTIL
                                        R> 2DROP
                                   THEN
                                ;

\ -- kata

\ test if the given N has two adjacent 1 bits
\ e.g. : 11 -> 1011 -> -1
\         9 -> 1001 ->  0
: ?TWO-ADJACENT-1-BITS ( n -- bool ) 0 SWAP DUP DUP 0 <> IF ( _ n n )
                                        LOG2 2 SWAP ** >R ( _ n |R: X=2 ** n.log2 )
                                        BEGIN
                                            DUP I - ( _ n n-X )  0 >= IF
                                                                        SWAP DUP 1 = IF ( n _ ) 1+ SWAP ( 2 n )
                                                                                ELSE ( n _ ) 1+ SWAP ( _ n ) I -
                                                                                THEN
                                                                      ELSE NIP 0 SWAP ( 0 n )
                                                                      THEN
                                            ( _ n )
                                            OVER ( _ n _ )
                                            2 =
                                            I 1 = OR
                                            R> 2 / >R ( … | X/2 )
                                        UNTIL
                                        R> 2DROP
                                        2 =
                                       THEN ;

\ return the maximum number which can be made with N (given number) bits
: ?MAX-NB ( n -- m ) DUP ?NEG IF DROP 0 ( 0 )
                              ELSE
                                  DUP IF DUP 2 SWAP ** NIP ( 2**n )
                                  THEN
                              THEN ;

\ return the number of numbers which can be made with N (given number) bits
\ or less, and which have not two adjacent 1 bits.
\ see http://www.codekata.com/2007/01/code_kata_fifte.html
: ?HOW-MANY-NB-NOT-TWO-ADJACENT-1-BITS ( n -- m ) ( TODO ) ;