
\ Definitions in high-level Forth that can be compiled by the small
\ nucleus itself. They are included into the bitstream for default.

\ #######   CORE   ############################################

: [']
    '
; immediate 0 foldable

: [char]
    char
; immediate 0 foldable

: (
    [char] ) parse 2drop
; immediate 0 foldable

: u>= ( u1 u2 -- ? ) u< invert ; 2 foldable
: u<= ( u1 u2 -- ? ) u> invert ; 2 foldable
: >=  ( n1 n2 -- ? )  < invert ; 2 foldable
: <=  ( n1 n2 -- ? )  > invert ; 2 foldable

: else
    postpone ahead
    swap
    postpone then
; immediate

: while
    postpone if
    swap
; immediate

: repeat
     postpone again
     postpone then
; immediate

: create ( "<name>" -- ; -- addr )
    :
    here 2 cells + postpone literal
    postpone ;
;

: buffer: ( u "<name>" -- ; -- addr )
   create allot 0 foldable
;

: >body ( addr -- addr' )
    @ -1 1 rshift and \ Remove the literal opcode MSB
;

: m* ( n1 n2 -- d )
    2dup xor >r
    abs swap abs um*
    r> 0< if dnegate then
; 2 foldable

: variable ( x "name" -- ; -- addr )
    create ,
    0 foldable
;

: constant ( x "name" -- ; -- x ) : postpone literal postpone ; 0 foldable ;

: sgn ( u1 n1 -- n2 ) \ n2 is u1 with the sign of n1
    0< if negate then
; 2 foldable

\ Divide d1 by n1, giving the symmetric quotient n3 and the remainder
\ n2.
: sm/rem ( d1 n1 -- n2 n3 )
    2dup xor >r     \ combined sign, for quotient
    over >r         \ sign of dividend, for remainder
    abs >r dabs r>
    um/mod          ( remainder quotient )
    swap r> sgn     \ apply to remainder
    swap r> sgn     \ apply to quotient
; 3 foldable

\ Divide d1 by n1, giving the floored quotient n3 and the remainder n2.
\ Adapted from hForth
: fm/mod ( d1 n1 -- n2 n3 )
    dup >r 2dup xor >r
    >r dabs r@ abs
    um/mod
    r> 0< if
        swap negate swap
    then
    r> 0< if
        negate         \ negative quotient
        over if
            r@ rot - swap 1-
        then
    then
    r> drop
; 3 foldable

: */mod ( n1 n2 n3 -- n4 n5 ) >r m* r> sm/rem ; 3 foldable
: */    ( n1 n2 n3 -- n4 )    */mod nip ; 3 foldable

: spaces ( n -- )
    begin
        dup 0>
    while
        space 1-
    repeat
    drop
;

( Pictured numeric output                    JCB 08:06 07/18/14)
\ Adapted from hForth

\ "The size of the pictured numeric output string buffer shall
\ be at least (2*n) + 2 characters, where n is the number of
\ bits in a cell."

create BUF0
16 cells 2 + 128 max
allot here constant BUF

0 variable hld

: <# ( -- )
    BUF hld !
;

: hold ( c -- )
    hld @ 1- dup hld ! c!
;

: sign ( n -- )
    0< if
        [char] - hold
    then
;

: .digit ( u -- c )
  9 over <
  [char] A [char] 9 1 + -
  and +
  [char] 0 +
;

: # ( ud -- ud* )
    0 base @ um/mod >r base @ um/mod swap
    .digit hold r>
;

: #s ( ud -- 0 0 )
    begin
        #
        2dup d0=
    until
;

: #> ( ud -- addr len )
    2drop hld @ BUF over -
;

: (d.) ( d -- addr len )
    dup >r dabs <# #s r> sign #>
;

: ud. ( ud -- )
    <# #s #> type space
;

: d. ( d -- )
    (d.) type space
;

: . ( n -- )
    s>d d.
;

: u. ( u -- )
    0 d.
;

: rtype ( caddr u1 u2 -- ) \ display character string specified by caddr u1
                           \ in a field u2 characters wide.
  2dup u< if over - spaces else drop then
  type
;

: d.r ( d length -- )
    >r (d.)
    r> rtype
;

: .r ( n length -- )
    >r s>d r> d.r
;

: u.r ( u length -- )
    0 swap d.r
;

( Memory operations                          JCB 18:02 05/31/15)

: move ( addr1 addr2 u -- )
    >r 2dup u< if
        r> cmove>
    else
        r> cmove
    then
;

: /mod ( n1 n2 -- n3 n4 ) >r s>d r> sm/rem ; 2 foldable
: /    ( n1 n2 -- n3 )    /mod nip ; 2 foldable
: mod  ( n1 n2 -- n3 )    /mod drop ; 2 foldable

: ."
    [char] " parse
    state @ if
        postpone sliteral
        postpone type
    else
        type
    then
; immediate 0 foldable

\ #######   CORE EXT   ########################################

: pad ( -- addr )
    here aligned
;

: within ( n1|u1 n2|u2 n3|u3 -- flag ) over - >r - r> u< ; 3 foldable

: s"
    [char] " parse
    state @ if
        postpone sliteral
    then
; immediate

( CASE                                       JCB 09:15 07/18/14)
\ From ANS specification A.3.2.3.2

: case ( -- 0 ) 0 ; immediate  ( init count of ofs )

: of  ( #of -- orig #of+1 / x -- )
    1+    ( count ofs )
    >r    ( move off the stack in case the control-flow )
          ( stack is the data stack. )
    postpone over  postpone = ( copy and test case value)
    postpone if    ( add orig to control flow stack )
    postpone drop  ( discards case value if = )
    r>             ( we can bring count back now )
; immediate

: endof ( orig1 #of -- orig2 #of )
    >r   ( move off the stack in case the control-flow )
         ( stack is the data stack. )
    postpone else
    r>   ( we can bring count back now )
; immediate

: endcase  ( orig1..orign #of -- )
    postpone drop  ( discard case value )
    0 ?do
      postpone then
    loop
; immediate

\ #######   DICTIONARY   ######################################

: cornerstone ( "name" -- )
  create
    forth 2@        \ preserve FORTH and DP after this
    , 2 cells + ,
  does>
    2@ forth 2! \ restore FORTH and DP
;

\ #######   MEMORY   ##########################################

: unused
    $4000 here -
;

\ #######   Flash   ###########################################

\ Save memory image to SPI Flash

: waitspi ( -- )
  begin
    $05 >spi \ Read status register 1
    spi> not 1 and
    idle
  until
;

: spiwe ( -- )
    $06 >spi \ Write enable
    idle
;

: erase ( sector -- )
  dup 29 u> if \ Never overwrite bitstream in sectors 0 to 29.
    idle
    spiwe
    $D8 >spi \ Sector erase
        >spi  \ Sector number
    $00 >spi   \ Address high
    $00 >spi    \ Address low
    idle
    waitspi
  else drop then
;

: save ( sector -- )
  dup 29 u> if \ Never overwrite bitstream in sectors 0 to 29.

    dup erase
    0      \ 8 kb in 256 byte pages
    begin
      spiwe

      $02 >spi \ Page program, 256 Bytes
     over >spi  \ Sector number
     dup
 8 rshift >spi   \ Address high
      $00 >spi    \ Address low

      begin
        dup c@ >spi
        1+
        dup $FF and 0=
      until

      idle
      waitspi

      dup $4000 =
    until
    2drop

  else drop then \ Bitstream protection
;

\ #######   IO   ##############################################

$0104 constant P1IN    \ Input
$0108 constant P1OUT   \ Output
$0110 constant P1DIR   \ Direction
$0120 constant P1IFG   \ Interrupt Flag
$0140 constant P1IES   \ Interrupt Edge Select
$0180 constant P1IE    \ Interrupt Enable

$0204 constant P2IN
$0208 constant P2OUT
$0210 constant P2DIR
$0220 constant P2IFG
$0240 constant P2IES
$0280 constant P2IE

$0404 constant P3IN
$0408 constant P3OUT
$0410 constant P3DIR
$0420 constant P3IFG
$0440 constant P3IES
$0480 constant P3IE

$0804 constant P4IN
$0808 constant P4OUT
$0810 constant P4DIR
$0820 constant P4IFG
$0840 constant P4IES
$0880 constant P4IE

$8004 constant IRQ-CAUSE
$8008 constant IRQ-ENABLE

: cycles ( -- u ) $8000 io@ ;

   25 constant cycles/us  \ For 25 MHz
25000 constant cycles/ms

: delay-cycles ( cycles -- )
  cycles ( cycles start )
  begin
    pause
    2dup ( cycles start cycles start )
    cycles ( cycles start cycles start current )
    swap - ( cycles start cycles elapsed )
    u<=
  until
  2drop
;

: us ( u -- )       cycles/us *  delay-cycles      ;
: ms ( u -- ) 0 ?do cycles/ms    delay-cycles loop ;

: randombit ( -- 0 | 1 ) $2000 io@ 2 rshift 1 and ;
: random ( -- x ) 0  16 0 do 2* randombit or 100 0 do loop loop ;

: ticks ( -- u ) $4000 io@ ;

: nextirq ( cycles -- ) \ Trigger the next interrupt u cycles after the last one.
  $4000 io@  \ Read current tick
  -           \ Subtract the cycles already elapsed
  8 -          \ Correction for the cycles neccessary to do this
  invert        \ Timer counts up to zero to trigger the interrupt
  $4000 io!      \ Prepare timer for the next irq
;

: leds ( x -- ) $2010 io! ;
: buttons ( -- x ) $2020 io@ ;

: esc? ( -- ? ) key? if key 27 = else false then ;

\ -------------------------------------------------------------
\  Double tools
\ -------------------------------------------------------------

: 2or  ( d1 d2 -- d ) >r swap >r or  r> r> or  ; 4 foldable
: 2and ( d1 d2 -- d ) >r swap >r and r> r> and ; 4 foldable
: 2xor ( d1 d2 -- d ) >r swap >r xor r> r> xor ; 4 foldable

: d0<   ( d -- ? ) nip 0< ; 2 foldable

: d= ( x0 x1 y0 y1 -- ? )

  swap ( x0 x1 y1 y0 )
  >r   ( x0 x1 y1 R: y0 )
  =    ( x0 x1=y1 R: y0 )
  swap ( x1=y1 x0 R: y0 )
  r>   ( x1=y1 x0 y0 )
  =    ( x1=y1 x0=y0 )
  and
; 4 foldable

: d<> d= not ; 4 foldable

: d2/  ( x1 x2 -- x1' x2' ) >r 1 rshift r@ 8 cells 1- lshift or r> 2/       ; 2 foldable
: dshr ( x1 x2 -- x1' x2' ) >r 1 rshift r@ 8 cells 1- lshift or r> 1 rshift ; 2 foldable

\ : 2lshift  ( ud u -- ud* ) begin dup while >r d2*  r> 1- repeat drop ; 3 foldable
\ : 2arshift (  d u --  d* ) begin dup while >r d2/  r> 1- repeat drop ; 3 foldable
\ : 2rshift  ( ud u -- ud* ) begin dup while >r dshr r> 1- repeat drop ; 3 foldable

: 2lshift ( low high u -- )
  dup >r ( low high u R: u )
  lshift ( low high* )
  over 8 cells r@ - rshift or
  over r@ 8 cells - lshift or
  swap r> lshift swap
; 3 foldable

: 2rshift ( low high u -- )
  >r swap ( high low R: u )
  r@ rshift
  over 8 cells r@ - lshift or
  over r@ 8 cells - rshift or
  swap
  r> rshift
; 3 foldable

: 2arshift ( low high u -- )
  dup >r 8 cells u< ( low high R: u )
  if
    swap ( high low R: u )
    r@ rshift
    over 8 cells r@ - lshift or
  else
    nip dup r@ 8 cells - arshift
  then
  swap r> arshift
; 3 foldable

: 2nip ( d1 d2 -- d2 )
  >r nip nip r>
; 4 foldable

: 2rot ( d1 d2 d3 -- d2 d3 d1 )
  >r >r ( d1 d2 R: d3 )
  2swap ( d2 d1 R: d3 )
  r> r> ( d2 d1 d3 )
  2swap ( d2 d3 d1 )
; 6 foldable

: d<            \ ( al ah bl bh -- flag )
    rot         \ al bl bh ah
    2dup =
    if
        2drop u<
    else
        > nip nip
    then
; 4 foldable

: d>  ( d1 d2 -- ? ) 2swap d< ; 4 foldable
: d>= ( d1 d2 -- ? ) d< not   ; 4 foldable
: d<= ( d1 d2 -- ? ) d> not   ; 4 foldable

: dmin ( d1 d2 -- d ) 2over 2over d< if 2drop else 2nip then ; 4 foldable
: dmax ( d1 d2 -- d ) 2over 2over d< if 2nip else 2drop then ; 4 foldable

: du<           \ ( al ah bl bh -- flag )
    rot         \ al bl bh ah
    2dup =
    if
        2drop u<
    else
        u> nip nip
    then
; 4 foldable

: du>  ( d1 d2 -- ? ) 2swap du< ; 4 foldable
: du>= ( d1 d2 -- ? ) du< not   ; 4 foldable
: du<= ( d1 d2 -- ? ) du> not   ; 4 foldable

\ -------------------------------------------------------------
\  Fixpoint output
\ -------------------------------------------------------------

: hold< ( c -- ) \ Add a character at the end of the number string
  hld @   dup 1- dup hld !    BUF hld @ -  move
  BUF 1- c!
;

: f# ( u -- u ) base @ um* .digit hold< ;

: f.n ( f n -- ) ( f-Low f-High n -- ) \ Prints a s15.16 number

  >r ( Low High R: n )

  dup 0< if [char] - emit then
  dabs
  ( uLow uHigh )
  0 <# #s   ( uLow 0 0 )
  drop swap ( 0 uLow )

  [char] , hold<
  r> 0 ?do f# loop

  #> type space
;

: f. ( f -- ) 8 cells f.n ;

\ -------------------------------------------------------------
\  Fixpoint calculations
\ -------------------------------------------------------------

: 2variable ( d -- ) create , , 0 foldable ;
\ : 2constant ( d -- ) create , , 0 foldable does> 2@ ;
: 2constant ( d -- ) swap : postpone literal postpone literal postpone ; 0 foldable ;

: s>f ( n -- f ) 0 swap ; 1 foldable  \ Signed integer --> Fixpoint s15.16
\ : f>s ( f -- n ) nip    ; 2 foldable  \ Fixpoint s15.16 --> Signed integer

: f* ( f1 f2 -- f )

        dup >r dabs
  2swap dup >r dabs

            ( d c b a )
  swap >r   ( d c a R: b )
  2dup *    ( d c a ac R: b )
  >r        ( d c a R: b ac )
  >r        ( d c R: b ac a )
  over      ( d c d R: b ac a )
  r> um*    ( d c L H R: b ac )
  r> +      ( d c L H' R: b )
  rot       ( d L H' c R: b )
  r@        ( d L H' c b R: b )
  um* d+    ( d L' H'' R: b )
  rot       ( L' H'' d R: b )
  r>        ( L' H'' d b )
  um* nip 0 ( L' H'' db 0 )
  d+        ( L'' H''' )

  r> r> xor 0< if dnegate then

; 4 foldable

0. 2variable dividend
0. 2variable shift
0. 2variable divisor

: (ud/mod) ( -- )

  16 cells
  begin

    \ Shift the long chain of four cells.

       dividend cell+ @ dup 8 cells 1- rshift >r 2*    dividend cell+ !
    r> dividend       @ dup 8 cells 1- rshift >r 2* or dividend       !
    r>    shift cell+ @ dup 8 cells 1- rshift >r 2* or    shift cell+ !
    r>    shift       @                          2* or    shift       !

    \ Subtract divisor when shifted out value is large enough

    shift 2@ divisor 2@  du>=

    if \ Greater or Equal: Subtract !
      shift 2@ divisor 2@ d- shift 2!
      dividend cell+ @ 1+ dividend cell+ !
    then

    1- dup 0=
  until
  drop
;

: ud/mod ( ud1 ud2 -- ud-rem ud-div )

     divisor 2!
  0. shift 2!
     dividend 2!

  (ud/mod)

  shift 2@
  dividend 2@

; 4 foldable

: f/ ( f1 f2 -- f )

  dup >r dabs  divisor 2!
  dup >r dabs  0 Shift 2! 0 swap dividend 2!

  (ud/mod)

  dividend 2@
  r> r> xor 0< if dnegate then

; 4 foldable

\ #######   DUMP   ############################################

: dump
    ?dup
    if
        1- 4 rshift 1+
        0 do
            cr dup dup .x space space
            16 0 do
                dup c@ .x2 1+
            loop
            space swap
            16 0 do
                dup c@ dup bl 127 within invert if
                    drop [char] .
                then
                emit 1+
            loop
            drop
        loop
    then
    drop
;

\ #######   INSIGHT   #########################################


( Deep insight into stack, dictionary and code )
( Matthias Koch )

: .s ( -- )
  \ Save initial depth
  depth dup >r

  \ Flush stack contents to temporary storage
  begin
    dup
  while
    1-
    swap
    over cells pad + !
  repeat
  drop

  \ Print original depth
  ." [ "
  r@ .x2
  ." ] "

  \ Print all elements in reverse order
  r@
  begin
    dup
  while
    r@ over - cells pad + @ .x
    1-
  repeat
  drop

  \ Restore original stack
  0
  begin
    dup r@ u<
  while
    dup cells pad + @ swap
    1+
  repeat
  rdrop
  drop
;

: insight ( -- )  ( Long listing of everything inside of the dictionary structure )
    base @ hex cr
    forth @
    begin
        dup
    while
         ." Addr: "     dup .x
        ."  Link: "     dup link@ .x
        ."  Flags: "    dup cell+ c@ 128 and if ." I " else ." - " then
                        dup @ 7 and ?dup if 1- u. else ." - " then
        ."  Code: "     dup cell+ count 127 and + aligned .x
        space           dup cell+ count 127 and type
        link@ cr
    repeat
    drop
    base !
;

0 variable disasm-$    ( Current position for disassembling )
0 variable disasm-cont ( Continue up to this position )

: name. ( Address -- )  ( If the address is Code-Start of a dictionary word, it gets named. )

  dup ['] s, 24 + = \ Is this a string literal ?
  if
    ."   --> s" [char] " emit space
    disasm-$ @ count type
    [char] " emit

    disasm-$ @ c@ 1+ aligned disasm-$ +!
    drop exit
  then

  >r
  forth @
  begin
    dup
  while
    dup cell+ count 127 and + aligned ( Dictionary Codestart )
      r@ = if ."   --> " dup cell+ count 127 and type then
    link@
  repeat
  rdrop drop
;

: alu. ( Opcode -- ) ( If this opcode is from an one-opcode definition, it gets named. This way inlined ALUs get a proper description. )

  dup $6127 = if ." >r"    drop exit then
  dup $6B11 = if ." r@"    drop exit then
  dup $6B1D = if ." r>"    drop exit then
  dup $600C = if ." rdrop" drop exit then

  $FF73 and
  >r
  forth @
  begin
    dup
  while
    dup cell+ count 127 and + aligned @ ( Dictionary First-Opcode )
        dup $E080 and $6080 =
        if
          $FF73 and r@ = if rdrop cell+ count 127 and type space exit then
        else
          drop
        then

    link@
  repeat
  drop r> drop
;


: memstamp ( Addr -- ) dup .x ." : " @ .x ."   " ; ( Shows a memory location nicely )

: disasm-step ( -- )
  disasm-$ @ memstamp
  disasm-$ @ @        ( Fetch next opcode )
  1 cells disasm-$ +! ( Increment position )

  dup $8000 and         if ." Imm  " $7FFF and       dup .x 6 spaces                      .x       exit then ( Immediate )
  dup $E000 and $0000 = if ." Jmp  " $1FFF and cells dup                                  .x name. exit then ( Branch )
  dup $E000 and $2000 = if ." JZ   " $1FFF and cells disasm-cont @ over max disasm-cont ! .x       exit then ( 0-Branch )
  dup $E000 and $4000 = if ." Call " $1FFF and cells dup                                  .x name. exit then ( Call )
                           ." Alu"   13 spaces dup alu. $80 and if ." exit" then                             ( ALU )
;

: seec ( -- ) ( Continues to see )
  base @ hex cr
  0 disasm-cont !
  begin
    disasm-$ @ @
    dup  $E080 and $6080 =           ( Loop terminates with ret )
    swap $E000 and 0= or             ( or when an unconditional jump is reached. )
    disasm-$ @ disasm-cont @ u>= and ( Do not stop when there has been a conditional jump further )

    disasm-step cr
  until

  base !
;

: see ( -- ) ( Takes name of definition and shows its contents from beginning to first ret )
  ' disasm-$ !
  seec
;

cornerstone new

\
\  fix_fft.c - Fixed-point in-place Fast Fourier Transform
\
\  All data are fixed-point smallint integers, in which -32768
\  to +32768 represent -1.0 to +1.0 respectively. Integer
\  arithmetic is used for speed, instead of the more natural
\  floating-point.
\
\  For the forward FFT (time -> freq), fixed scaling is
\  performed to prevent arithmetic overflow, and to map a 0dB
\  sine/cosine wave (i.e. amplitude = 32767) to two -6dB freq
\  coefficients. The return value is always 0.
\
\  For the inverse FFT (freq -> time), fixed scaling cannot be
\  done, as two 0dB coefficients would sum to a peak amplitude
\  of 64K, overflowing the 32k range of the fixed-point integers.
\  Thus, the fix_fft() routine performs variable scaling, and
\  returns a value which is the number of bits LEFT by which
\  the output must be shifted to get the actual amplitude
\  (i.e. if fix_fft() returns 3, each value of fr[] and fi[]
\  must be multiplied by 8 (2**3) for proper scaling.
\  Clearly, this cannot be done within fixed-point smallint
\  integers. In practice, if the result is to be used as a
\  filter, the scale_shift can usually be ignored, as the
\  result will be approximately correctly normalized as is.
\
\  Written by:  Tom Roberts  11/8/89
\  Made portable:  Malcolm Slaney 12/15/94 malcolm@interval.com
\  Enhanced:  Dimitrios P. Bouras  14 Jun 2006 dbouras@ieee.org
\  Translated to Forth:  Matthias Koch, 22 Jan 2020 m.cook@gmx.net

 512 constant N_WAVE       \ full length of Sinewave[]
   9 constant LOG2_N_WAVE  \ log2(N_WAVE)

\  Henceforth "smallint" implies 16-bit word. If this is not
\  the case in your architecture, please replace "smallint"
\  with a type definition which *is* a 16-bit word.

\  Since we only use 3/4 of N_WAVE, we define only
\  this many samples, in order to conserve data space.
\ 0 .. N_WAVE - (N_WAVE div 4) - 1]

create sinewave
      0 ,    402 ,    804 ,   1206 ,   1607 ,   2009 ,   2410 ,   2811 ,
   3211 ,   3611 ,   4011 ,   4409 ,   4807 ,   5205 ,   5601 ,   5997 ,
   6392 ,   6786 ,   7179 ,   7571 ,   7961 ,   8351 ,   8739 ,   9126 ,
   9511 ,   9895 ,  10278 ,  10659 ,  11038 ,  11416 ,  11792 ,  12166 ,
  12539 ,  12909 ,  13278 ,  13645 ,  14009 ,  14372 ,  14732 ,  15090 ,
  15446 ,  15799 ,  16150 ,  16499 ,  16845 ,  17189 ,  17530 ,  17868 ,
  18204 ,  18537 ,  18867 ,  19194 ,  19519 ,  19840 ,  20159 ,  20474 ,
  20787 ,  21096 ,  21402 ,  21705 ,  22004 ,  22301 ,  22594 ,  22883 ,
  23169 ,  23452 ,  23731 ,  24006 ,  24278 ,  24546 ,  24811 ,  25072 ,
  25329 ,  25582 ,  25831 ,  26077 ,  26318 ,  26556 ,  26789 ,  27019 ,
  27244 ,  27466 ,  27683 ,  27896 ,  28105 ,  28309 ,  28510 ,  28706 ,
  28897 ,  29085 ,  29268 ,  29446 ,  29621 ,  29790 ,  29955 ,  30116 ,
  30272 ,  30424 ,  30571 ,  30713 ,  30851 ,  30984 ,  31113 ,  31236 ,
  31356 ,  31470 ,  31580 ,  31684 ,  31785 ,  31880 ,  31970 ,  32056 ,
  32137 ,  32213 ,  32284 ,  32350 ,  32412 ,  32468 ,  32520 ,  32567 ,
  32609 ,  32646 ,  32678 ,  32705 ,  32727 ,  32744 ,  32757 ,  32764 ,

  32767 ,  32764 ,  32757 ,  32744 ,  32727 ,  32705 ,  32678 ,  32646 ,
  32609 ,  32567 ,  32520 ,  32468 ,  32412 ,  32350 ,  32284 ,  32213 ,
  32137 ,  32056 ,  31970 ,  31880 ,  31785 ,  31684 ,  31580 ,  31470 ,
  31356 ,  31236 ,  31113 ,  30984 ,  30851 ,  30713 ,  30571 ,  30424 ,
  30272 ,  30116 ,  29955 ,  29790 ,  29621 ,  29446 ,  29268 ,  29085 ,
  28897 ,  28706 ,  28510 ,  28309 ,  28105 ,  27896 ,  27683 ,  27466 ,
  27244 ,  27019 ,  26789 ,  26556 ,  26318 ,  26077 ,  25831 ,  25582 ,
  25329 ,  25072 ,  24811 ,  24546 ,  24278 ,  24006 ,  23731 ,  23452 ,
  23169 ,  22883 ,  22594 ,  22301 ,  22004 ,  21705 ,  21402 ,  21096 ,
  20787 ,  20474 ,  20159 ,  19840 ,  19519 ,  19194 ,  18867 ,  18537 ,
  18204 ,  17868 ,  17530 ,  17189 ,  16845 ,  16499 ,  16150 ,  15799 ,
  15446 ,  15090 ,  14732 ,  14372 ,  14009 ,  13645 ,  13278 ,  12909 ,
  12539 ,  12166 ,  11792 ,  11416 ,  11038 ,  10659 ,  10278 ,   9895 ,
   9511 ,   9126 ,   8739 ,   8351 ,   7961 ,   7571 ,   7179 ,   6786 ,
   6392 ,   5997 ,   5601 ,   5205 ,   4807 ,   4409 ,   4011 ,   3611 ,
   3211 ,   2811 ,   2410 ,   2009 ,   1607 ,   1206 ,    804 ,    402 ,

      0 ,   -402 ,   -804 ,  -1206 ,  -1607 ,  -2009 ,  -2410 ,  -2811 ,
  -3211 ,  -3611 ,  -4011 ,  -4409 ,  -4807 ,  -5205 ,  -5601 ,  -5997 ,
  -6392 ,  -6786 ,  -7179 ,  -7571 ,  -7961 ,  -8351 ,  -8739 ,  -9126 ,
  -9511 ,  -9895 , -10278 , -10659 , -11038 , -11416 , -11792 , -12166 ,
 -12539 , -12909 , -13278 , -13645 , -14009 , -14372 , -14732 , -15090 ,
 -15446 , -15799 , -16150 , -16499 , -16845 , -17189 , -17530 , -17868 ,
 -18204 , -18537 , -18867 , -19194 , -19519 , -19840 , -20159 , -20474 ,
 -20787 , -21096 , -21402 , -21705 , -22004 , -22301 , -22594 , -22883 ,
 -23169 , -23452 , -23731 , -24006 , -24278 , -24546 , -24811 , -25072 ,
 -25329 , -25582 , -25831 , -26077 , -26318 , -26556 , -26789 , -27019 ,
 -27244 , -27466 , -27683 , -27896 , -28105 , -28309 , -28510 , -28706 ,
 -28897 , -29085 , -29268 , -29446 , -29621 , -29790 , -29955 , -30116 ,
 -30272 , -30424 , -30571 , -30713 , -30851 , -30984 , -31113 , -31236 ,
 -31356 , -31470 , -31580 , -31684 , -31785 , -31880 , -31970 , -32056 ,
 -32137 , -32213 , -32284 , -32350 , -32412 , -32468 , -32520 , -32567 ,
 -32609 , -32646 , -32678 , -32705 , -32727 , -32744 , -32757 , -32764 ,

 sinewave constant sinewave

: sinewave@ ( u -- n ) cells sinewave + @ ; 1 foldable

\  FIX_MPY() - fixed-point multiplication & scaling.
\  Substitute inline assembly for hardware-specific
\  optimization suited to a particluar DSP processor.
\  Scaling ensures that result remains 16-bit.

: FIX_MPY ( a b -- c )
  2dup xor >r ( a b R: sign )
  abs swap abs
  um*
  1 lshift swap 15 rshift or
  r> 0< if negate then
; 2 foldable

: 2/floored ( n -- n/2 ) dup 0< if negate 2/ negate else 2/ then ; 1 foldable

\  fix_fft() - perform forward/inverse fast Fourier transform.
\  fr[n],fi[n] are real and imaginary arrays, both INPUT AND
\  RESULT (in-place FFT), with 0 <= n < 2**m; set inverse to
\  0 for forward transform (FFT), or 1 for iFFT.

N_WAVE cells buffer: fft-real
N_WAVE cells buffer: fft-imag

: fft-real! ( x u -- ) cells fft-real + ! ;
: fft-real@ ( u -- x ) cells fft-real + @ ;

: fft-imag! ( x u -- ) cells fft-imag + ! ;
: fft-imag@ ( u -- x ) cells fft-imag + @ ;

0 variable fft-inverse
0 variable fft-n
0 variable fft-mr
0 variable fft-shift
0 variable fft-scale
0 variable fft-l
0 variable fft-k
0 variable fft-istep
0 variable fft-tr
0 variable fft-ti
0 variable fft-wr
0 variable fft-wi
0 variable fft-i

: fix-fft ( power inverse? -- scale )
  fft-inverse ! 1 swap lshift fft-n !

  \ max FFT size = N_WAVE
  fft-n @ N_WAVE u> if -1 exit then

  0 fft-mr !
  0 fft-scale !

  \ decimation in time - re-order data
  fft-n @ 1 do

    fft-n @ fft-l !

    begin
      fft-l @ 1 arshift fft-l !
      fft-mr @ fft-l @ +   fft-n @  >=  not
    until

    fft-mr @   fft-l @ 1- and   fft-l @ +   fft-mr !

    i fft-mr @ >
    if
      i fft-real@  fft-mr @ fft-real@  i fft-real! fft-mr @ fft-real!
      i fft-imag@  fft-mr @ fft-imag@  i fft-imag! fft-mr @ fft-imag!
    then

  loop

  1 fft-l !
  LOG2_N_WAVE 1- fft-k !

  begin
    fft-l @ fft-n @ <
  while
    fft-inverse @
    if \ variable scaling, depending upon data

      false
      fft-n @ 0 do
        i dup fft-real@ abs $3FFF >
         swap fft-imag@ abs $3FFF > or
        or
      loop
      dup fft-shift !
      if 1 fft-scale +! then

    else
      \ fixed scaling, for proper normalization --
      \ there will be log2(n) passes, so this results
      \ in an overall factor of 1/n, distributed to
      \ maximize arithmetic accuracy.

      true fft-shift !
    then

    \ it may not be obvious, but the shift will be
    \ performed on each data point exactly once,
    \ during this pass.

    fft-l @ 1 lshift fft-istep !

    fft-l @ 0 do

      i fft-k @ lshift \ 0 <= j < N_WAVE/2

      fft-shift @
      if
        dup  N_WAVE 2 rshift + sinewave@                                  2/floored fft-wr !
                               sinewave@ fft-inverse @ not if negate then 2/floored fft-wi !
      else
        dup  N_WAVE 2 rshift + sinewave@                                            fft-wr !
                               sinewave@ fft-inverse @ not if negate then           fft-wi !
      then

      i fft-i !

      begin
        fft-i @ fft-n @ <
      while
        fft-i @ fft-l @ + >r

        fft-wr @   r@ fft-real@   FIX_MPY
        fft-wi @   r@ fft-imag@   FIX_MPY  -  fft-tr !
        fft-wr @   r@ fft-imag@   FIX_MPY
        fft-wi @   r@ fft-real@   FIX_MPY  +  fft-ti !

        fft-i @ dup fft-real@ swap fft-imag@

        fft-shift @
        if
          2/floored swap 2/floored swap
        then

        ( qr qi )

        2dup
        fft-ti @ - r@ fft-imag!
        fft-tr @ - r> fft-real!

        fft-ti @ + fft-i @ fft-imag!
        fft-tr @ + fft-i @ fft-real!

        fft-istep @ fft-i +!
      repeat

    loop

    -1 fft-k +!
    fft-istep @ fft-l !

  repeat

  fft-scale @
;

: fftswap ( 2^m -- )
  1- 1 swap lshift fft-n !
  fft-n @ 0 do
    i fft-real@  i fft-n @ + fft-real@  i fft-real!  i fft-n @ + fft-real!
    i fft-imag@  i fft-n @ + fft-imag@  i fft-imag!  i fft-n @ + fft-imag!
  loop
;

\ Calculate Bitlog and Bitexp - close relatives to logarithm and exponent to base 2.

: clz ( x -- u )
  0
  begin
    over 0< not
  while
    1+
    swap 2* 1 or swap
  repeat
  nip
;

: bitlog ( u -- u )

 \ Invented by Tom Lehman at Invivo Research, Inc., ca. 1990
 \ Changed for 16 bit integers

 \ Gives an integer analog of the log function
 \ For large x, B(x) = 16*(log(base 2)(x) - 2)

  dup 16 u<= if 1 lshift
             else
               ( u )
               13 over clz - 4 lshift
               ( u s1 )
               swap
               ( s1 u )
               11 over clz - rshift $F and
               ( s1 s2 ) or
             then
; 1 foldable

: bitexp ( u -- u )

  \ Returns an integer value equivalent to
  \ the exponential. For numbers > 32,
  \ bitexp(x) approx = 2^(x/16 + 2)

  \ Usable range: 0 .. 223.

  223 umin
  dup 32 u<
  if
    1 rshift
  else
    dup $F and $10 or
    swap 4 rshift 2 - lshift
  then
; 1 foldable

\ Requires double, fixpoint, bitlog and fourier.

\ ------------------------------------------------------------------------------
\  Heapsort
\  https://rosettacode.org/wiki/Sorting_algorithms/Heapsort#Forth
\ ------------------------------------------------------------------------------

: r'@ r> r> r@ swap >r swap >r ;

' < variable heapsort-comparison

: precedes ( n1 n2 a -- f) >r cells r@ + @ swap cells r> + @ swap heapsort-comparison @ execute ;
: exchange ( n1 n2 a --  ) >r cells r@ + swap cells r> + over @ over @ swap rot ! swap ! ;

: siftDown                             ( a e s -- a e s)
  swap >r swap >r dup                  ( s r)
  begin                                ( s r)
    dup 2* 1+ dup r'@ <                ( s r c f)
  while                                ( s r c)
    dup 1+ dup r'@ <                   ( s r c c+1 f)
    if                                 ( s r c c+1)
      over over r@ precedes if swap then
    then drop                          ( s r c)
    over over r@ precedes              ( s r c f)
  while                                ( s r c)
    tuck r@ exchange                   ( s r)
  repeat then                          ( s r)
  drop drop r> swap r> swap            ( a e s)
;

: heapsort-core                        ( a n --)
  over >r                              ( a n)
  dup 1- 1- 2/                         ( a c s)
  begin                                ( a c s)
    dup 0< 0=                          ( a c s f)
  while                                ( a c s)
    siftDown 1-                        ( a c s)
  repeat drop                          ( a c)

  1- 0                                 ( a e 0)
  begin                                ( a e 0)
    over 0>                            ( a e 0 f)
  while                                ( a e 0)
    over over r@ exchange              ( a e 0)
    siftDown swap 1- swap              ( a e 0)
  repeat                               ( a e 0)
  drop drop drop r> drop
;


: heapsort-signed ( addr len -- )
  [']  < heapsort-comparison !
  heapsort-core
;

: heapsort-unsigned ( addr len -- )
  ['] u< heapsort-comparison !
  heapsort-core
;

\ ------------------------------------------------------------------------------
\  Switch between unsigned and signed, necessary for 16 Bit ADCs
\ ------------------------------------------------------------------------------

: un<->signed ( -- ) \ 0 .. 65535 <--> -32768 .. 32767
  N_WAVE 0 do
    i dup fft-real@ $8000 xor swap fft-real!
    \ i fft-real@ $8000 xor i fft-real!
  loop
;

: un<->signed-imag ( -- ) \ 0 .. 65535 <--> -32768 .. 32767
  N_WAVE 0 do
    i dup fft-imag@ $8000 xor swap fft-imag!
    \ i fft-imag@ $8000 xor i fft-imag!
  loop
;

\ ------------------------------------------------------------------------------
\  ASCII art tools
\ ------------------------------------------------------------------------------


: f.nr7 ( f -- ) ( f-Low f-High n -- ) \ Prints a s15.16 number

  >r ( Low High R: n )

  dup 0< >r
  dabs
  ( uLow uHigh )
  0 <# #s   ( uLow 0 0 )

  r> if [char] - hold then

  drop swap ( 0 uLow )

  [char] , hold<
  r> 0 ?do f# loop

  #> dup 12 swap - spaces

  type space
;

: dashes 0 ?do [char] - emit loop ;

: airbar ( min max -- )
    2dup =
    if
      drop
      spaces [char] | emit
    else
      over      spaces [char] < emit
      swap - 1- spaces [char] > emit
    then
;

: fillbar ( min max -- )
    2dup =
    if
      drop
      spaces [char] | emit
    else
      over      spaces [char] < emit
      swap - 1- dashes [char] > emit
    then
;

\ ------------------------------------------------------------------------------
\  Signal statistics
\ ------------------------------------------------------------------------------

: stats ( -- avg min max )
  0.
  N_WAVE 0 do
    i fft-real@  0 d+
  loop
  \ N_WAVE um/mod nip
  LOG2_N_WAVE 2rshift drop

  -1
  N_WAVE 0 do
    i fft-real@ umin
  loop

  0
  N_WAVE 0 do
    i fft-real@ umax
  loop
;

: stats-signed ( -- avg min max )
  0.
  N_WAVE 0 do
    i fft-real@  m+
  loop
  \ N_WAVE sm/rem nip
  LOG2_N_WAVE 2arshift drop

  32767
  N_WAVE 0 do
    i fft-real@ min
  loop

  -32768
  N_WAVE 0 do
    i fft-real@ max
  loop
;

: printstats ( -- )
  cr
  stats
  rot  ." Avg: " 6 u.r cr
  swap ." Min: " 6 u.r cr
       ." Max: " 6 u.r cr
;

: printstats-signed ( -- )
  cr
  stats-signed
  rot  ." Avg: " 6 .r cr
  swap ." Min: " 6 .r cr
       ." Max: " 6 .r cr
;


: printbin-signed ( bin count -- )
  dup if swap 6 .r 6 .r cr else 2drop then
;

: signal-binning-signed ( -- )

  cr
  13 dashes cr
  ."    ADC     #" cr
  13 dashes

  fft-real fft-imag N_WAVE cells move
  fft-imag N_WAVE heapsort-signed

  cr
  -32768 0
  N_WAVE 0 do
    over i fft-imag@ =
    if 1+
    else
      printbin-signed
      i fft-imag@ 1
    then
  loop
  printbin-signed

  fft-imag N_WAVE cells 0 fill
;

: printbin ( bin count -- )
  dup if swap 6 u.r 6 u.r cr else 2drop then
;

: signal-binning ( -- )

  cr
  13 dashes cr
  ."    ADC     #" cr
  13 dashes

  fft-real fft-imag N_WAVE cells move
  fft-imag N_WAVE heapsort-unsigned

  cr
  0 0
  N_WAVE 0 do
    over i fft-imag@ =
    if 1+
    else
      printbin
      i fft-imag@ 1
    then
  loop
  printbin

  fft-imag N_WAVE cells 0 fill
;

\ ------------------------------------------------------------------------------
\  Signal plotters
\ ------------------------------------------------------------------------------

: u*/ ( a b c -- d ) >r um* r> um/mod nip ;

80 13 - constant WIDTH

: signalplot-lin-bounds ( umin umax -- )

  cr
  13 spaces ." | " over 6 u.r ."    Lin" WIDTH 22 - spaces dup 6 u.r ."  |"
  cr
  13 spaces
  WIDTH 0 do [char] = emit loop cr

  2dup = if 1+ then \ Prevent divide by zero later!
  over - ( min range )

  N_WAVE 0 do
    i 6 u.r
    over i fft-real@ dup 6 u.r space
    swap - over WIDTH 1- swap u*/
    spaces [char] | emit cr
  loop

  13 spaces
  WIDTH 0 do [char] = emit loop cr

  2drop
;

: signalplot-lin-bounds-signed ( min max -- )

  cr
  13 spaces ." | " over 6 .r ."    Lin" WIDTH 22 - spaces dup 6 .r ."  |"
  cr
  13 spaces
  WIDTH 0 do [char] = emit loop cr

  2dup = if 1+ then \ Prevent divide by zero later!
  over - ( min range )

  N_WAVE 0 do
    i 5 u.r
    over i fft-real@ dup 7 .r space
    swap - over WIDTH 1- swap u*/
    spaces [char] | emit cr
  loop

  13 spaces
  WIDTH 0 do [char] = emit loop cr

  2drop
;

: signalplot-log-bounds ( umin umax -- )

  cr
  13 spaces ." | " over 6 u.r ."    Log" WIDTH 22 - spaces dup 6 u.r ."  |"
  cr
  13 spaces
  WIDTH 0 do [char] = emit loop cr

  bitlog swap bitlog swap

  2dup = if 1+ then \ Prevent divide by zero later!
  over - ( min range )

  N_WAVE 0 do
    i 6 u.r
    over i fft-real@ dup 6 u.r space bitlog
    swap - over WIDTH 1- swap u*/
    spaces [char] | emit cr
  loop

  13 spaces
  WIDTH 0 do [char] = emit loop cr

  2drop
;

100,0 2variable samplerate

: fft-plot ( -- ) \ FFT output is always signed
  cr
  13 spaces ." | " 0 6 u.r ."    Log" WIDTH 22 - spaces $7FFF 6 u.r ."  |"
  cr
  13 spaces
  WIDTH 0 do [char] = emit loop cr

  N_WAVE
\        0 do  \ Symmetric printout
  N_WAVE 2/ do  \ Start with DC component


    i N_WAVE 2/ - s>f N_WAVE s>f f/ samplerate 2@ f* 3 f.nr7

    i fft-real@ abs bitlog  WIDTH 1- $7FFF bitlog u*/
    i fft-imag@ abs bitlog  WIDTH 1- $7FFF bitlog u*/

    2dup umax >r
         umin r> ( min max )

    fillbar
    cr

  loop

  13 spaces
  WIDTH 0 do [char] = emit loop cr
;


: signalplot-lin ( -- )
  0 65535
  signalplot-lin-bounds
;

: signalplot-lin-min-max ( -- )
  stats rot drop
  signalplot-lin-bounds
;


: signalplot-lin-signed ( -- )
  -32768 32767
  signalplot-lin-bounds-signed
;

: signalplot-lin-min-max-signed ( -- )
  stats-signed rot drop
  signalplot-lin-bounds-signed
;


: signalplot-log ( -- )
  0 65535
  signalplot-log-bounds
;

: signalplot-log-max ( -- )
  0 stats rot drop nip
  signalplot-log-bounds
;

\ ------------------------------------------------------------------------------
\   Registers
\ ------------------------------------------------------------------------------

$2030 constant adc-result
$2040 constant adc-channel
$20B0 constant adc-valid
$20C0 constant adc-fifo

\ ------------------------------------------------------------------------------
\   Capture using FIFOs in logic
\ ------------------------------------------------------------------------------

: capture ( -- )

  \ Clear FIFO
  begin
    adc-valid io@
  while
    adc-fifo io@ drop
  repeat

  \ Capture measurements
  fft-real
  begin
    begin adc-valid io@ until adc-fifo io@

    over !
    1 cells +
    dup fft-real N_WAVE cells + =
  until
  drop

  fft-imag N_WAVE cells 0 fill
;

\ ------------------------------------------------------------------------------
\   Analyse the signal
\ ------------------------------------------------------------------------------

: show ( -- )

  signalplot-lin-min-max
  printstats
  signal-binning

  \ un<->signed

  LOG2_N_WAVE false fix-fft drop
  LOG2_N_WAVE fftswap

  250,000 samplerate 2!
  fft-plot
;

\ How to use:
\
\   0 adc-channel io!
\   capture show
: live-bins ( -- )

  \ Clear bins
  64 0 do 0 i fft-imag! loop

  \ Collect all data points into bins
  N_WAVE 0 do
    1  i fft-real@ 6 rshift cells fft-imag +  +!  \ 10 rshift for 16 bit ADC, 6 rshift for 12 bit ADC
  loop

  \ Find maximum value for scaling
  0
  64 0 do i fft-imag@ umax loop
  bitlog ( maxcount )

  \ Print bins as scaled ASCII art
  64 0 do
    i fft-imag@ bitlog ( maxcount count )

    over 8 swap u*/  \ Scale maximum to available chars

    \  01234567890123456789012345678901234
    s"  _.,:;!|@" drop

    + c@ emit
  loop
  drop
;

: live ( -- )
  cr
  begin
    capture
    stats

    rot   ." Avg: " 6 u.r
    swap ."   Min: " 6 u.r
         ."   Max: " 6 u.r

    ."   [" live-bins ." ]" cr

  esc? until
;
