{-# LANGUAGE TypeFamilies #-}
{-# LANGUAGE UndecidableInstances #-}
{-# LANGUAGE FlexibleContexts #-}
--import Control.Monad
import           Vis hiding (Plane)
import qualified Vis
import Xyz hiding (normalize)
import qualified Quat

import qualified Data.Vec as Vec
import           Data.Vec ((:.)(..),Vec3,vec,diagonal,packMat)
import qualified Data.IntMap as IntMap
import Data.Foldable (toList)

import Data.Vec.Quaternion
import Physics.Space
import Physics.Step
import Physics.Mesh
import Physics.Mesh.FaceVertex
import Physics.Object
import Physics.Constraint

--import Debug.Trace

-- TODO: gyroscope
-- TODO: newton's cradle

unitCube = execMeshBuilder $ do
    [a,b,c,d,e,f,g,h] <- mapM newVertex [
        (-0.5):.(-0.5):.(-0.5) :: Vec3 Double,
        (-0.5):.(-0.5):.( 0.5),
        (-0.5):.( 0.5):.(-0.5),
        (-0.5):.( 0.5):.( 0.5),
        ( 0.5):.(-0.5):.(-0.5),
        ( 0.5):.(-0.5):.( 0.5),
        ( 0.5):.( 0.5):.(-0.5),
        ( 0.5):.( 0.5):.( 0.5)]
    quad a b c d
    quad g h e f
    quad e f a b
    quad c d g h
    quad a c e g
    quad f h b d
    where quad a b c d = newFace a b c >> newFace b d c

smallPlane :: FaceVertexMesh
smallPlane = execMeshBuilder $ do
    let l = 10.0
    [a,b,c,d] <- mapM newVertex [
        (-l):.(-l):.0 :: Vec3 Double,
        (-l):.( l):.0,
        ( l):.(-l):.0,
        ( l):.( l):.0]
    newFace a c b
    newFace c d b
    
gyroscopeDemo = defaultEmptySpace {
    sContents = IntMap.fromList $ zip [0..]
        [ Object {
            oShape = scaleShape (1 :. 1 :. 0.1) (TriMesh unitCube),
            oBody = defaultBody {
                bMass = 2,
                bPosition = 0.1 :. 0 :. 1,
                -- TODO: get this calculated automatically
                bAngularMass = packMat (diagonal (1 :. 1 :. 2)),
                bAngularVelocity = 0.2 :. 0 :. 4
                }
            }
        ],
    sConstraints = [ FixedPoint 0 (0 :. 0 :. (-1)) (0 :. 0 :. 0) ],
    sGravity = 0:.0:.(-9.8):.()
    }

pendulumDemo = defaultEmptySpace {
    sContents = IntMap.fromList $ zip [0..]
        [ Object {
            oShape = scaleShape (0.5 :. 0.5 :. 2) (TriMesh unitCube),
            oBody = defaultBody {
                bMass = 1,
                bPosition = 0:.0:.1,
                -- TODO: get this calculated automatically
                bAngularMass = packMat (diagonal (0.5 :. 0.5 :. 2))
            } }
        , Object {
            oShape = scaleShape (0.5 :. 0.5 :. 2) (TriMesh unitCube),
            oBody = defaultBody {
                bMass = 1,
                bPosition = 0:.0:.(-1),
                -- TODO: get this calculated automatically
                bAngularMass = packMat (diagonal (0.5 :. 0.5 :. 2)),
                bVelocity = 3:.0:.0
            } }
        , Object {
            oShape = scaleShape (0.5 :. 0.5 :. 2) (TriMesh unitCube),
            oBody = defaultBody {
                bMass = 1,
                bPosition = 0:.0:.(-3),
                -- TODO: get this calculated automatically
                bAngularMass = packMat (diagonal (0.5 :. 0.5 :. 2)),
                bVelocity = 3:.0:.0
            } }
        ],
    sConstraints =
        [ FixedPoint 0 (0:.0:.1) (0:.0:.2)
        , BallSocket 0 (0:.0:.(-1)) 1 (0:.0:.1)
        , BallSocket 1 (0:.0:.(-1)) 2 (0:.0:.1)
        ],
    sGravity  = 0:.0:.(-9.8)
    }

main = do
    let initial = pendulumDemo
    simulate Nothing "rigbo3 demo" (1/60.0) initial drawWorld (\_ -> step (1/60.0))

drawWorld w = Scale (1,-1,-1) (toVisObject w)

class Visualizable a where
    toVisObject :: a -> VisObject Double

instance Visualizable Space where
    -- TODO: show constraints
    toVisObject s = VisObjects $ map toVisObject (toList (sContents s)) ++ [Axes (1.0, 15)]

instance Visualizable Object where
    toVisObject (Object b s) =
        Trans (toXyz (bPosition b)) (RotQuat (toQuat (bAngularPosition b)) (toVisObject s))

instance Visualizable Shape where
    toVisObject (TriMesh m) = toVisObject m

instance Visualizable Body where
    toVisObject b = VisObjects [] -- TODO

instance Visualizable FaceVertexMesh where
    toVisObject m = VisObjects $ concatMap (\(a',b',c') ->
            let (a,b,c) = (toXyz a',toXyz b',toXyz c')
                center = toXyz $ (a'+b'+c')/vec 3
                normal = toXyz $ tnormal a' b' c'
                area = tarea a' b' c'
            in [ Triangle a b c blue
               , Line [a,b] gray
               , Line [b,c] gray
               , Line [c,a] gray
               , Trans center (Arrow (0.1*sqrt area, 15) normal gray)
               ]) (toTriangleList m)
            where gray = greyN 0.8

uncurry3 f ~(a,b,c) = f a b c

toXyz (x:.y:.z:.()) = Xyz x y z
toQuat q = let (w,x:.y:.z:.()) = (Vec.head q, Vec.tail q) in Quat.Quat w x y z

