module Physics.Hpysics.Simulation where

import Physics.Hpysics.Types
import Physics.Hpysics.Collision
import Physics.Hpysics.Utils
import Physics.Hpysics.Body
import Physics.Hpysics.Poly
import Physics.Hpysics.ODE
import Physics.Hpysics.BoundingSphere
import Data.Maybe

integrate :: Config -> FloatType -> Body -> Body
integrate conf ts body
    | isStatic body = body
    | otherwise =
    setPosition (getPosition body `add` ts `scale` getLinearVelocity body `add` ts^2/2 `scale` g) $
    setLinearVelocity (getLinearVelocity body `add` ts `scale` g) $
    setRotation (qnormalize $ eulerIntegrator (rotationDE (getAngularVelocity body)) ts 0 (getRotation body)) $
    body
    where g = gravity conf

detectBodyCollision2phase :: Body -> Body -> Maybe Contact
detectBodyCollision2phase b1 b2 =
    if canIntersect (getShape b1) (getShape b2)
        then detectBodyCollision b1 b2
        else Nothing

makeLittleStep :: FloatType -> World -> World
makeLittleStep ts world = let
    bs = bodies world
    contactingPairs = {-# SCC "contactingPairs" #-} concat
        [ let {b1=bs!!i; b2=bs!!j; mbContact = detectBodyCollision2phase b1 b2} in [(i,(j,mbContact)),(j,(i,fmap dualContact mbContact))] | i <- [ 0 .. length bs - 1 ] , j <- [ (i+1) .. length bs - 1 ] ]
    contactsWithBody = [ (bs!!i, [ (bs!!j, mbC) | (i',(j,mbC)) <- contactingPairs, i' == i ]) | i <- [ 0 .. length bs - 1 ] ]

    modifyBody (b, contacts) = if isStatic b then b else
        foldl applyCollisionImpulse b contacts
    modifiedBodies = map modifyBody contactsWithBody
    in world { bodies = [ integrate (config world) ts b | b <- modifiedBodies ] }

makeStep :: FloatType -> Int -> World -> World
makeStep ts steps world = iterate (makeLittleStep (ts/fromIntegral steps)) world !! steps

-- derivative of orientation
-- Mirtich p. 235
rotationDerivative :: Vec -> Quaternion -> Quaternion
rotationDerivative omega (Vec4 s x y z) =
    1/2 `scale4`
    ((Mat43
        (-x) (-y) (-z)
          s  (-z)   y
          z    s  (-x)
        (-y)   x    s)
    `mat43xVec` omega)
-- autonomous differential equation, doesn't depend on time
rotationDE :: Vec -> (FloatType -> Vec4 -> Vec4)
rotationDE = const . rotationDerivative