USING: accessors kernel math math.functions math.order ranges 
sequences math.vectors sets sorting assocs assocs.extras arrays 
grouping generalizations
   juere-engine.utils
   juere-engine.level-editor
   juere-engine.stage ;
IN: juere-engine.physics

TUPLE: tangible < angled
     { height initial: 0 } { impulses initial: { } } 
    { airtime initial: 0 } {  net-vel initial: { 0 0 0 } } 
   { step-height initial: 0.25 } ;

TUPLE: physical < stage
   gravity ;
M: physical substage 
   drop { 0.01 } ;

: (dummy) ( -- tangible )
   tangible new 
      { 0.5 0.5 } >>position 2 >>height 
      0 >>angle "dummy" >>id ;

: tangible-xyz ( tangible -- xyz )
   [ position>> first2 ] [ height>> ] bi swap 3array ;

TUPLE: impulse 
   magni direction asymtope decay since ;
SINGLETON: upwards

: <impulse> ( magni dir asym decay tangible -- impulse )
   ticks>> impulse boa ;

: <impulse>* ( tangible magni dir asym decay -- tangible )
   5 npick <impulse> '[ _ suffix ] change-impulses ;

: seed-geometric ( target rate -- initial )
   dupd * - ;

:: geometric-sum ( a r n -- sum )
   n [0..b] [ r swap ^ a * ] map sum ;

: decayed-magni ( impulse tangible -- mag/f )
   [ [ magni>> ] [ decay>> ] [ since>> ] tri ] [ ticks>> ] bi*
   [ dupd [ seed-geometric ] keep ] [ swap - ] 2bi*
   1 + geometric-sum - >float [ abs 0.0075 > ] 1guard ;

CONSTANT: asym-decay 3

: asym-multiplier ( asym velocity -- multiplier )
   swap / asym-decay ^ -1 * 1 + 0 1 clamp ;

: magni-in-direction ( angle net-vel -- relative-magni )
   over upwards? [ nip second ] [ 
      [ deg>rad ] [ [ first ] [ last ] bi ] bi*
      rect> >polar swapd - cos *
   ] if ;

: applied-magni ( impulse entity -- mag/f )
      [ decayed-magni ] [ drop asymtope>> ] 
      [ [ direction>> ] [ net-vel>> ] bi* ]
   2tri magni-in-direction asym-multiplier * ;

: angle>xyz ( mag angle -- xyz )
   [ upwards? ]
      [ drop '{ 0 _ 0 } ] [ deg>rad polar> >rect '{ _ 0 _ } ]
   1if ;

: cull-negligible ( tangible -- tangible )
   dup '[ _ '[ _ decayed-magni ] filter ] change-impulses ;

: apply-impulse ( tangible impulse -- entity )
   dupd [ swap applied-magni ] [ nip direction>> ] 2bi  
   angle>xyz [ v+ ] curry change-net-vel ;

: apply-impulses ( tangible -- tangible )
   [ impulses>> ] keep [ apply-impulse ] reduce ;

: reset-airtime ( entity -- entity )
   dup ticks>> >>airtime ;

: apply-gravity ( tangible stage -- tangible )
   [ physical? ] 
      [ over dup 
         [ gravity>> ] [ ticks>> ] [ airtime>> ]
        tri* - -0.5 * *
         '{ 0 _ 0 } >>net-vel ] 
   [ drop ] 1if ;

GENERIC#: add-impulse 5 ( subject id magni dir asym decay -- )
M: object add-impulse 6 ndrop ;
M: tangible add-impulse
   [ <impulse>* ] 4 ncurry [ over id>> = ] dip when drop ;

DEFER: apply-velocity

M: tangible entity-action ( tangible stage quots -- quots )
   pick cull-negligible pick apply-gravity apply-impulses
      [ id>> ] [ net-vel>> ] bi [ apply-velocity ] 2curry suffix
   call-next-method ;

: t@q ( q q0 dq -- t ) 
   [ - ] dip [ 0 = ] [ 2drop f ] [ / ] 1if ;

: grid-thresholds ( xz-from xz-to -- ts )
   dupd [ [ [ floor ] bi@ [a..b] ] 2map ]
        [ [ swap - ] 2map ]
   2bi swapd 
      [ [ t@q ] 2curry map ] 3map 
   concat sift members sort 
   [ [ 1 < ] [ 0 >= ] bi and ] filter ;

: q@t ( t q0 dq -- q ) swapd * + ;

: xz@ts ( ts xz0 dxdz -- xzs )
   [ [ q@t ] 2curry [ map ] curry ] 
   2map dupd spread( x x -- x x )
   2 nzip ;

: xyz@ts ( ts xyz0 dxdydz -- xyzs )
   [ [ q@t ] 2curry [ map ] curry ] 
   2map dupd dupd spread( x x x -- x x x )
   3 nzip ;

: grid-collisions ( xyz-from xyz-to -- xyz )
   [ [ unclip*-2nd nip ] bi@ grid-thresholds ]
   [ drop ] [ swap v- ] 2tri xyz@ts ;

: all-points ( xyz-from xyz-to -- xyz-pairs )
   [ grid-collisions ] 2keep [ prefix ] [ suffix ] bi*
   2 clump ;

TUPLE:  medial value ;
TUPLE: lateral value ;

: endpoints>pointslope ( point point -- point0 dqs )
   dupd [ swap - ] 2map ;

: do-medial ( h enter exit -- collision )
   [ [ second ] bi@ swap between? ] [
      endpoints>pointslope [ [ second ] bi@ t@q 1array ] 2keep
      xyz@ts first [ second fp-special? ]
         [ drop f ] [ medial boa ] 1if
   ] [ 3drop f ] 3if ;

:: do-tile-collisions ( enter exit tile -- collisions )
   tile height>> :> h
   enter second h < [ enter lateral boa ] [ f ] if :> start
    exit second h < [  exit lateral boa ] [ f ] if :> end
   h enter exit do-medial :> mid
   { start mid end } sift ;

: tile-hitscan ( xyz-from xyz-to tilemap -- tile-collisions )
   [ all-points ] dip 
      '[ first unclip*-2nd nip [ floor >integer ] map _ at ] 
       zip-with [ second ] filter
      [ first2 [ first2 ] dip do-tile-collisions ] 
   map concat ;

: next-position ( xyz-from xyz-to tilemap -- next-y next-xz )
   [ tile-hitscan [ medial? ] partition ] keepd
      '[ ?first [ value>> ] [ _ ] if* ] bi@
   [ second ] [ unclip*-2nd nip ] bi* ;

: jostle ( xyz -- xyzs ) ! *Unused
   1/100 [ '{ _ 0 0 } ] [ '{ 0 0 _ } ] bi
   overd [ [ v+ ] 2bi@ ] 4 nkeep [ v- ] 2bi@ 4array ;

: underground? ( tilemap xyz -- height ? )
   unclip*-2nd swapd [ floor >integer ] map ?of
   [ height>> [ <= ] keep swap ] 
               [ 2drop -0/0. f ] if ;

:: (apply-velocity) ( stage entity vels -- entity )
   entity tangible-xyz :> e-xyz
         e-xyz vels v+ :> e-xyz'
   entity 
      e-xyz' unclip*-2nd nip >>position
      stage tiles>> e-xyz' underground? [
         dup e-xyz unclip*-2nd drop - abs
         entity reset-airtime 
                step-height>> <=
         [ >>height ] [ drop
            e-xyz unclip*-2nd nip >>position
!           * Get next-position to push entities out?
!           e-xyz e-xyz' stage tiles>> next-position
!           nip >>position
         ] if
   ] [ drop e-xyz' unclip*-2nd drop >>height ] 
   if ;

GENERIC#: apply-velocity 2 ( stage id vels -- )
M: object apply-velocity 3drop ;
M: physical apply-velocity
   -rot dupd find-entity [
      rot (apply-velocity) drop
   ] [ 2drop ] if* ;

TUPLE: walker < tangible
   last-step stride-time stride-strength ;

: (dummy-w) ( -- walker )
   walker new 
      { 0.5 0.5 } >>position 2 >>height 
      0 >>angle "dummy" >>id ;

: (walk) ( walker angle -- walker )
   over [ stride-time>> ] [ ticks>> ] [ last-step>> ] tri
   - < [ dupd [ stride-strength>> ] dip
         over 3 * 11/16 <impulse>* 
         dup ticks>> >>last-step ] [ drop ] if ;

: (jump) ( walker -- walker )
   reset-airtime
   1/2 upwards 4 11/16 <impulse>* ;
! *Can't run+jump with this.
!   Ideal behavior: buffer jump 'till end of stride?
   ! reset-airtime [ 4 * ] change-stride-strength
   !   upwards (walk)
   ! [ 4 / ] change-stride-strength ;

GENERIC#: footfall 2 ( subject id angle -- )
M: object footfall 3drop ;
M: walker footfall ! Really ought to write this as Macro.
   [ (walk) ] curry [ over id>> = ] dip when drop ;

GENERIC#: jump 1 ( subject id -- )
M: object jump 2drop ;
M: walker jump
   over id>> = [ (jump) ] when drop ;

M: walker move-entity 3drop ;

: phys-chicken ( -- tangible )
   0 "blank.bmp" <rsrc> 0.020 { 0.5 0.5 }
   0 { } { }
   chicken-up-ani "idle" "idle" 1
   chicken-angles { 1 0 chicken-down-ani } -45
   2.25 { } 0 { 0 0 0 } 0.25
   0 2 0.08
   walker boa ;

: physics-test ( -- physical )
   3 ramped-tiles V{ } V{ }
      2 "liveangel.bmp" <rsrc>
      0.015 { -0.5 -0.5 } entity boa suffix
      phys-chicken suffix
   V{ } V{ }
   { 0 8 8 } 0 0
   0.02 physical boa ;