module Physics.Hpysics.Body
  ( isStatic, isDynamic, mkStaticBody, mkDynamicBody
  , setPosition, setRotation, setLinearVelocity, setAngularVelocity
  , getPosition, getRotation, getLinearVelocity, getAngularVelocity
  , getMass, getInertia, getRestitution, getShape, getPhantomBody
  , World(..), Body
  )
where
import Physics.Hpysics.Types
import Physics.Hpysics.Utils
import Physics.Hpysics.Poly
import Physics.Hpysics.BoundingSphere
-- | For the dynamic body, the position of shape is relative to body's mass center.
-- For the static body, the position is absolute.
data Body = DynamicBody
    { mass            :: !FloatType
    , inertia         :: !Mat33
    , restitution     :: !FloatType
    , shape           :: !Shape
    , position        :: !Vec
    , rotation        :: !Quaternion
    , linearVelocity  :: !Vec
    , angularVelocity :: !Vec
    }
    | StaticBody
    { shape :: !Shape
    -- phantomBody is dynamic body which has the same shape and large mass and
    -- moment of inertia. It's (for now) used for collision response
    , phantomBody :: !Body
    }
    deriving (Read, Show, Eq)
isStatic, isDynamic :: Body -> Bool
isStatic (StaticBody {}) = True
isStatic _ = False
isDynamic = not.isStatic

mkStaticBody :: FloatType -> Shape -> Body
mkStaticBody rest s = StaticBody
    { shape = constructBoundingSphere s
    , phantomBody = DynamicBody
        { mass = 1e09
        , inertia = 1e09 `matScale` unit33
        , restitution = rest
        , position = zeroVector
        , rotation = unitVec4
        , linearVelocity = zeroVector
        , angularVelocity = zeroVector
        , shape = s
        }
    }
mkDynamicBody
    :: FloatType -- ^ mass
    -> FloatType -- ^ restitution
    -> Shape     -- ^ shape
    -> Body
mkDynamicBody m r s = DynamicBody
    { mass = m
    , restitution = r
    , shape = constructBoundingSphere s
    , position = zeroVector
    , rotation = unitVec4
    , linearVelocity = zeroVector
    , angularVelocity = zeroVector
    , inertia = unit33 -- TODO calculate inertia
    }

setPosition :: Vec -> Body -> Body
setPosition v b = b { position = v }
setRotation :: Vec4 -> Body -> Body
setRotation r b = b { rotation = r }
setLinearVelocity :: Vec -> Body -> Body
setLinearVelocity v b = b { linearVelocity = v }
setAngularVelocity :: Vec -> Body -> Body
setAngularVelocity w b = b { angularVelocity = w }

getPosition :: Body -> Vec
getPosition = position
getRotation :: Body -> Vec4
getRotation = rotation
getLinearVelocity :: Body -> Vec
getLinearVelocity = linearVelocity
getAngularVelocity :: Body -> Vec
getAngularVelocity = angularVelocity
getMass :: Body -> FloatType
getMass = mass
getInertia :: Body -> Mat33
getInertia = inertia
getRestitution :: Body -> FloatType
getRestitution = restitution
getPhantomBody :: Body -> Body
getPhantomBody = phantomBody
-- | Converts chape coordinates from local to global
getShape :: Body -> Shape
getShape b | isStatic b = shape b
           | otherwise = transformPoly pos rot $ shape b
    where
    pos = position b
    rot = rotation b

-- get rid of it?
data World = World
    { bodies       :: ![ Body ]
    , config       :: !Config
    } deriving (Read, Show)