module Properties where
import Physics.Hpysics
import Physics.Hpysics.Collision
import Physics.Hpysics.VClip
import Test.QuickCheck
import Control.Monad

instance Arbitrary Vec where
    arbitrary = liftM3 Vec arbitrary arbitrary arbitrary 
    coarbitrary = undefined
instance Arbitrary Vec4 where
    arbitrary = liftM4 Vec4 arbitrary arbitrary arbitrary arbitrary
    coarbitrary = undefined
instance Arbitrary Mat33 where
    arbitrary = do
        a11 <- arbitrary
        a12 <- arbitrary
        a13 <- arbitrary
        a21 <- arbitrary
        a22 <- arbitrary
        a23 <- arbitrary
        a31 <- arbitrary
        a32 <- arbitrary
        a33 <- arbitrary
        return $ Mat33 a11 a12 a13 a21 a22 a23 a31 a32 a33
    coarbitrary = undefined
{-
instance Arbitrary Body where
    arbitrary = cubeBodies
    coarbitrary = undefined
-}
cubes :: Gen Poly
cubes = liftM cube positive
rects :: Gen Poly
rects = liftM3 rect positive positive positive
{-
cubeBodies = do
    shape <- cubes
    mass <- positive
    let inertia = unit33
    position <- arbitrary
    rotation <- unitvec4
    linearVelocity <- arbitrary
    angularVelocity <- arbitrary
    return $ DynamicBody mass inertia 0 shape position rotation linearVelocity angularVelocity
-}
nonzeroVec :: Gen Vec
nonzeroVec = do
    vec <- arbitrary
    if norm vec == 0 then return $ Vec 1 0 0 else return vec
unitvec = fmap normalize nonzeroVec
unitvec4 = fmap qnormalize nonzeroVec4
nonzeroVec4 :: Gen Vec4
nonzeroVec4 = do
    vec <- arbitrary
    if qnorm vec == 0 then return $ Vec4 1 0 0 0 else return vec
positive :: Gen FloatType
positive = sized $ \n -> if n == 0 then choose (0.01,10e6) else choose (0.01,fromIntegral n)
planes :: Gen Plane
planes = do
    normal <- unitvec
    shift <- arbitrary
    return (normal, shift)
pointOnPlane :: Plane -> Gen Vec
pointOnPlane plane = liftM (projectToPlane plane) arbitrary
{-
squares :: Gen Feature
squares = do
    plane <- planes
    center <- pointOnPlane plane
    vertex <- pointOnPlane plane
    return $ Square plane center vertex
-}

prop_CountVertices = forAll cubes $ \c->length (vertices c) == 8
prop_CountEdges = forAll cubes $ \c->length (edges c) == 12
prop_CountFaces = forAll cubes $ \c->length (faces c) == 6
prop_CountVertexNeighbours = forAll cubes $ \c ->
    all (\v->length (neighbours v)==3) (vertices c)
prop_CountEdgeNeighbours = forAll cubes $ \c ->
    all (\v->length (neighbours v)==4) (edges c)
prop_CountFaceNeighbours = forAll cubes $ \c ->
    all (\v->length (neighbours v)==4) (faces c)
-- prop_BoundedBy1 = forAll squares $ \s@(Square plane center vertex) -> all (s`boundedBy`) (edgesOfSquare plane center vertex)
prop_EdgesEqualLength = forAll cubes $ \c-> let {(e:es) = map edgeLength $ edges c; edgeLength = uncurry distance . computeEdge} in all (e`equal`) es
prop_UnitRotate axis point = rotatePointAroundAxis axis 0 point == point
prop_UnitRotate2 axis point = distance (rotatePointAroundAxis axis (2*pi) point) point < 1e-5
prop_RotateInvariant p1 p2 q = abs (distance (rotatePoint q p1) (rotatePoint q p2) - distance p1 p2) < 1e-6
prop_FacesOrientation = forAll rects $ \r -> and [ distToPlaneSigned v (computeFacePlane f) <= 0 | v <- vertices_v r, f <- faces r ]
prop_NeighboursOfVertex = forAll cubes $ \c->
    all (\v@(Vertex vi,_)-> all (\(Edge a b,_)->a==vi) (neighbours v)) (vertices c)
prop_VClipTerminates = forAll (two rects) $ \(c1, c2) ->
    let (status, f1, f2) = vClip c1 c2
    in collect status $ collect (ty f1, ty f2) $ status /= Continue
    where ty f = case f of
            (Vertex _,_) -> "Vertex"
            (Edge _ _,_) -> "Edge"
            (Face _  ,_) -> "Face"
prop_VClipSymmetric = forAll (two cubes) $ \(c1,c2) ->
    let (status, _, _) = vClip c1 c2
        (status', _, _) = vClip c2 c1
    in status==status'


vector' :: Int -> Gen a -> Gen [a]
vector' n g = sequence [ g | i <- [1..n] ]

linearSystem :: Gen ([[FloatType]], [FloatType])
linearSystem = sized $ \n -> if n < 10 then resize 10 result else result
    where result = liftM2 (,) (vector' 12 (vector' 12 arbitrary)) (vector' 12 arbitrary)
prop_GaussSolve = forAll linearSystem $ \(system, vars) ->
    let
        coeffs = map (dot vars) system
        solution = gaussSolve system coeffs
    in (<1e-8) $ sum $ map abs $ zipWith (-) solution vars

{-
prop_CollisionSymmetric = forAll (sized $ \n -> (if n<5 then resize 5 else id) $ do 
    b1 <- cubeBodies
    b2 <- cubeBodies
    cp <- arbitrary
    n <- unitvec
    return (b1,b2,(cp,n))) $ \(b1,b2,contact)->
    let
    solution1 = uncurry gaussSolve (collisionLinearSystem b1 b2 contact)
    solution2 = uncurry gaussSolve (collisionLinearSystem b2 b1 (dualContact contact))
    in solution1 !! 12 `equal` - solution2 !! 12
    && solution1 !! 13 `equal` - solution2 !! 13
    && solution1 !! 14 `equal` - solution2 !! 14
-}
prop_makeOrthoBasis = forAll nonzeroVec $ \k-> let
    (i,j,k') = makeOrthoBasis k
    in abs (k'<.>i) < floatEpsilon
    && abs (j <.>i) < floatEpsilon
    && abs (k'<.>j) < floatEpsilon
prop_CrossAsMatrix a b = (crossAsMatrix a `mat33xVec` b) == (a `cross` b)