import Physics.Hpysics.Types
import Physics.Hpysics.Utils
import Physics.Hpysics.Collision
import Physics.Hpysics.Objects
import Test.HUnit
import Control.Monad (unless)
import Data.Maybe

-- floating-point assertion

eps :: Double
eps = 1E-4

assertFloatEqual :: String -> FloatType -> FloatType -> Assertion
assertFloatEqual preface expected actual =
  unless (abs (actual - expected) < eps) (assertFailure msg)
 where msg = (if null preface then "" else preface ++ "\n") ++
             "expected: " ++ show expected ++ "\n but got: " ++ show actual
assertVecEqual :: String -> Vec -> Vec -> Assertion
assertVecEqual preface expected actual =
  unless (norm (actual `sub` expected) < eps) (assertFailure msg)
 where msg = (if null preface then "" else preface ++ "\n") ++
             "expected: " ++ show expected ++ "\n but got: " ++ show actual

-- testing collision detection
-- two spheres
twoSpheresCollisionTest1 = let
    s1 = Sphere (Vec 0 0 0) 1
    s2 = Sphere (Vec 1 1 0) 0.4
    in TestCase $ assertEqual "should not collide" (detectCollision s1 s2) Nothing
twoSpheresCollisionTest2 = let
    s1 = Sphere (Vec 0 0 0) 1
    s2 = Sphere (Vec 1 1 0) 0.5
    collision = detectCollision s1 s2
    in TestCase $ do
        assertBool "should collide" $ isJust collision
        let Just (contact, normal) = collision
        assertVecEqual "testing normal"
            (Vec (-1/sqrt 2) (-1/sqrt 2) 0)
            normal
sphereCollisionTests = TestList [twoSpheresCollisionTest1,twoSpheresCollisionTest2]

-- two bodies with equal masses
collisionResponseTest1 = let
    r = 0.1
    m = 1
    v = 2
    b1 = DynamicBody
               { shape = (Sphere zeroVector r)
               , restitution = 0
               , mass = m
               , position = zeroVector
               , rotation = zeroVec4
               , linearVelocity = zeroVector
               , angularVelocity = zeroVector
               }
    b2 = DynamicBody
               { shape = (Sphere zeroVector r)
               , restitution = 0
               , mass = m
               , position = Vec (2*r) 0 0
               , rotation = zeroVec4
               , linearVelocity = Vec (-v) 0 0
               , angularVelocity = zeroVector
               }
    maybeContact = detectCollision (getShape b1) (getShape b2)
    collisionVelocity1 = collisionVelocity b1 (b2, maybeContact)
    collisionVelocity2 = collisionVelocity b2 (b1, dualContact `fmap` maybeContact)
    in TestCase $ do
        assertBool "should collide" $ isJust maybeContact
        assertVecEqual "comparing contact points" (Vec r 0 0) (fst.fromJust$maybeContact)
        assertVecEqual "comparing normals" (Vec (-1) 0 0) (snd.fromJust$maybeContact)
        assertVecEqual "lin.vel. of 1 body" (Vec (-v) 0 0) collisionVelocity1
        assertVecEqual "lin.vel. of 2 body" (Vec v 0 0) collisionVelocity2

-- massive body and light body
collisionResponseTest2 = let
    r = 0.1
    v = 2
    b1 = DynamicBody
               { shape = (Sphere zeroVector r)
               , restitution = 0
               , mass = 1E-4
               , position = zeroVector
               , rotation = zeroVec4
               , linearVelocity = zeroVector
               , angularVelocity = zeroVector
               }
    b2 = DynamicBody
               { shape = (Sphere zeroVector r)
               , restitution = 0
               , mass = 1E4
               , position = Vec (2*r) 0 0
               , rotation = zeroVec4
               , linearVelocity = Vec (-v) 0 0
               , angularVelocity = zeroVector
               }
    maybeContact = detectCollision (getShape b1) (getShape b2)
    collisionVelocity1 = collisionVelocity b1 (b2, maybeContact)
    collisionVelocity2 = collisionVelocity b2 (b1, dualContact `fmap` maybeContact)
    in TestCase $ do
        assertBool "should collide" $ isJust maybeContact
        assertVecEqual "comparing contact points" (Vec r 0 0) (fst.fromJust$maybeContact)
        assertVecEqual "comparing normals" (Vec (-1) 0 0) (snd.fromJust$maybeContact)
        assertVecEqual "lin.vel. of 1 body" (Vec (-2*v) 0 0) collisionVelocity1
        assertVecEqual "lin.vel. of 2 body" (Vec 0 0 0) collisionVelocity2

collisionResponseTests = TestList [collisionResponseTest1, collisionResponseTest2]

collisionTests = TestList [sphereCollisionTests, collisionResponseTests]
allTests = TestList [collisionTests]
main = runTestTT allTests