content: {-# LANGUAGE FlexibleContexts           #-}
  {-# LANGUAGE FlexibleInstances          #-}
  {-# LANGUAGE GeneralizedNewtypeDeriving #-}
  {-# LANGUAGE MultiParamTypeClasses      #-}
  {-# LANGUAGE OverloadedStrings          #-}
  {-# LANGUAGE QuasiQuotes                #-}
  {-# LANGUAGE RecordWildCards            #-}
  {-# LANGUAGE ScopedTypeVariables        #-}
  {-# LANGUAGE Strict                     #-}
  {-# LANGUAGE TemplateHaskell            #-}
  {-# LANGUAGE TypeApplications           #-}
  {-# LANGUAGE TypeFamilies               #-}
  {-# LANGUAGE UndecidableInstances       #-}
  {-# LANGUAGE ViewPatterns               #-}
  
  module Apecs.Physics.Constraint
    (
    ) where
  
  import           Apecs
  import           Apecs.Core
  import           Control.Monad
  import           Control.Monad.IO.Class (liftIO, MonadIO)
  import qualified Data.IntMap         as M
  import qualified Data.IntSet         as S
  import           Data.IORef
  import qualified Data.Vector.Unboxed as U
  import           Foreign.ForeignPtr  (withForeignPtr)
  import           Foreign.Ptr
  import qualified Language.C.Inline   as C
  import           Linear.V2
  
  import           Apecs.Physics.Space ()
  import           Apecs.Physics.Types
  
  C.context phycsCtx
  C.include "<chipmunk.h>"
  
  -- Constraint
  newConstraint :: SpacePtr -> Ptr Body -> Ptr Body -> Int -> ConstraintType -> IO (Ptr Constraint)
  newConstraint spacePtr bodyA bodyB (fromIntegral -> ety)
                (PinJoint (fmap realToFrac -> V2 ax ay) (fmap realToFrac -> V2 bx by)) =
    withForeignPtr spacePtr $ \space -> [C.block| cpConstraint* {
      cpVect anchorA = cpv( $(double ax), $(double ay) );
      cpVect anchorB = cpv( $(double bx), $(double by) );
      cpConstraint* constraint = cpPinJointNew($(cpBody* bodyA), $(cpBody* bodyB),anchorA,anchorB);
      cpConstraintSetUserData(constraint, (void*) $(intptr_t ety));
      return cpSpaceAddConstraint($(cpSpace* space), constraint);
      } |]
  
  newConstraint spacePtr bodyA bodyB (fromIntegral -> ety)
                (SlideJoint (fmap realToFrac -> V2 ax ay) (fmap realToFrac -> V2 bx by) (realToFrac -> min) (realToFrac -> max)) =
    withForeignPtr spacePtr $ \space -> [C.block| cpConstraint* {
      cpVect anchorA = cpv( $(double ax), $(double ay) );
      cpVect anchorB = cpv( $(double bx), $(double by) );
      cpConstraint* constraint = cpSlideJointNew($(cpBody* bodyA), $(cpBody* bodyB),anchorA,anchorB,$(double min),$(double max));
      cpConstraintSetUserData(constraint, (void*) $(intptr_t ety));
      return cpSpaceAddConstraint($(cpSpace* space), constraint);
      } |]
  
  newConstraint spacePtr bodyA bodyB (fromIntegral -> ety)
                (PivotJoint (fmap realToFrac -> V2 x y)) =
    withForeignPtr spacePtr $ \space -> [C.block| cpConstraint* {
      cpVect anchor = cpv( $(double x), $(double y) );
      cpConstraint* constraint = cpPivotJointNew($(cpBody* bodyA), $(cpBody* bodyB), anchor);
      cpConstraintSetUserData(constraint, (void*) $(intptr_t ety));
      return cpSpaceAddConstraint($(cpSpace* space), constraint);
      } |]
  
  newConstraint spacePtr bodyA bodyB (fromIntegral -> ety)
                (PivotJoint2 (fmap realToFrac -> V2 ax ay) (fmap realToFrac -> V2 bx by)) =
    withForeignPtr spacePtr $ \space -> [C.block| cpConstraint* {
      cpVect va = cpv( $(double ax), $(double ay) );
      cpVect vb = cpv( $(double bx), $(double by) );
      cpConstraint* constraint = cpPivotJointNew2($(cpBody* bodyA), $(cpBody* bodyB), va, vb);
      cpConstraintSetUserData(constraint, (void*) $(intptr_t ety));
      return cpSpaceAddConstraint($(cpSpace* space), constraint);
      } |]
  
  newConstraint spacePtr bodyA bodyB (fromIntegral -> ety)
                (GrooveJoint (fmap realToFrac -> V2 ax ay) (fmap realToFrac -> V2 bx by) (fmap realToFrac -> V2 ancx ancy)) =
    withForeignPtr spacePtr $ \space -> [C.block| cpConstraint* {
      cpVect va = cpv( $(double ax), $(double ay) );
      cpVect vb = cpv( $(double bx), $(double by) );
      cpVect anchor = cpv( $(double ancx), $(double ancy) );
      cpConstraint* constraint = cpGrooveJointNew($(cpBody* bodyA), $(cpBody* bodyB), va, vb, anchor);
      cpConstraintSetUserData(constraint, (void*) $(intptr_t ety));
      return cpSpaceAddConstraint($(cpSpace* space), constraint);
      } |]
  
  newConstraint spacePtr bodyA bodyB (fromIntegral -> ety)
                (DampedSpring (fmap realToFrac -> V2 ax ay) (fmap realToFrac -> V2 bx by) (realToFrac -> rl) (realToFrac -> stf) (realToFrac -> damping)) =
    withForeignPtr spacePtr $ \space -> [C.block| cpConstraint* {
      cpVect va = cpv( $(double ax), $(double ay) );
      cpVect vb = cpv( $(double bx), $(double by) );
      cpConstraint* constraint = cpDampedSpringNew($(cpBody* bodyA), $(cpBody* bodyB), va, vb, $(double rl), $(double stf), $(double damping));
      cpConstraintSetUserData(constraint, (void*) $(intptr_t ety));
      return cpSpaceAddConstraint($(cpSpace* space), constraint);
      } |]
  
  newConstraint spacePtr bodyA bodyB (fromIntegral -> ety)
                (DampedRotarySpring (realToFrac -> ra) (realToFrac -> stf) (realToFrac -> damping)) =
    withForeignPtr spacePtr $ \space -> [C.block| cpConstraint* {
      cpConstraint* constraint = cpDampedRotarySpringNew($(cpBody* bodyA), $(cpBody* bodyB), $(double ra), $(double stf), $(double damping));
      cpConstraintSetUserData(constraint, (void*) $(intptr_t ety));
      return cpSpaceAddConstraint($(cpSpace* space), constraint);
      } |]
  
  newConstraint spacePtr bodyA bodyB (fromIntegral -> ety)
                (RotaryLimitJoint  (realToFrac -> min) (realToFrac -> max)) =
    withForeignPtr spacePtr $ \space -> [C.block| cpConstraint* {
      cpConstraint* constraint = cpRotaryLimitJointNew($(cpBody* bodyA), $(cpBody* bodyB), $(double min), $(double max));
      cpConstraintSetUserData(constraint, (void*) $(intptr_t ety));
      return cpSpaceAddConstraint($(cpSpace* space), constraint);
      } |]
  
  newConstraint spacePtr bodyA bodyB (fromIntegral -> ety)
                (RatchetJoint (realToFrac -> phase) (realToFrac -> ratchet)) =
    withForeignPtr spacePtr $ \space -> [C.block| cpConstraint* {
      cpConstraint* constraint = cpRatchetJointNew($(cpBody* bodyA), $(cpBody* bodyB), $(double phase), $(double ratchet));
      cpConstraintSetUserData(constraint, (void*) $(intptr_t ety));
      return cpSpaceAddConstraint($(cpSpace* space), constraint);
      } |]
  
  newConstraint spacePtr bodyA bodyB (fromIntegral -> ety)
                (GearJoint (realToFrac -> phase) (realToFrac -> ratio)) =
    withForeignPtr spacePtr $ \space -> [C.block| cpConstraint* {
      cpConstraint* constraint = cpGearJointNew($(cpBody* bodyA), $(cpBody* bodyB), $(double phase), $(double ratio));
      cpConstraintSetUserData(constraint, (void*) $(intptr_t ety));
      return cpSpaceAddConstraint($(cpSpace* space), constraint);
      } |]
  
  newConstraint spacePtr bodyA bodyB (fromIntegral -> ety)
                (SimpleMotor (realToFrac -> rate)) =
    withForeignPtr spacePtr $ \space -> [C.block| cpConstraint* {
      cpConstraint* constraint = cpSimpleMotorNew($(cpBody* bodyA), $(cpBody* bodyB), $(double rate));
      cpConstraintSetUserData(constraint, (void*) $(intptr_t ety));
      return cpSpaceAddConstraint($(cpSpace* space), constraint);
      } |]
  
  destroyConstraint :: SpacePtr -> Ptr Constraint -> IO ()
  destroyConstraint spacePtr constraintPtr = withForeignPtr spacePtr $ \space -> [C.block| void {
    cpConstraint *constraint = $(cpConstraint* constraintPtr);
    cpSpaceRemoveConstraint($(cpSpace* space), constraint);
    cpConstraintFree(constraint); }|]
  
  instance Component Constraint where
    type Storage Constraint = Space Constraint
  
  instance (MonadIO m, Has w m Physics) => Has w m Constraint where
    getStore = (cast :: Space Physics -> Space Constraint) <$> getStore
  
  instance MonadIO m => ExplSet m (Space Constraint) where
    explSet sp@(Space bMap _ cMap _ spcPtr) cEty cons@(Constraint (Entity bEtyA) (Entity bEtyB) ctype) = liftIO $ do
      explDestroy sp cEty
      mBrA <- M.lookup bEtyA <$> readIORef bMap
      mBrB <- M.lookup bEtyB <$> readIORef bMap
      case (mBrA,mBrB) of
        (Just brA, Just brB) -> do
          cPtr <- newConstraint spcPtr (brPtr brA) (brPtr brB) cEty ctype
  
          modifyIORef' cMap (M.insert cEty (Record cPtr cons))
          modifyIORef' (brConstraints brA) (S.insert cEty)
          modifyIORef' (brConstraints brB) (S.insert cEty)
  
        _ -> return ()
  
  instance MonadIO m => ExplDestroy m (Space Constraint) where
    explDestroy (Space bMap _ cMap _ spc) cEty = liftIO $ do
      rd <- M.lookup cEty <$> readIORef cMap
      forM_ rd $ \(Record cPtr (Constraint (Entity bEtyA) (Entity bEtyB) _)) -> do
        bMap' <- readIORef bMap
        let rmConstraint ref = modifyIORef' (brConstraints ref) (S.delete cEty)
        mapM_ rmConstraint (M.lookup bEtyA bMap')
        mapM_ rmConstraint (M.lookup bEtyB bMap')
        modifyIORef' cMap $ M.delete cEty
        destroyConstraint spc cPtr
  
  instance MonadIO m => ExplMembers m (Space Constraint) where
    explMembers (Space _ _ cMap _ _) = liftIO $ U.fromList . M.keys <$> readIORef cMap
  
  instance MonadIO m => ExplGet m (Space Constraint) where
    explExists (Space _ _ cMap _ _) ety = liftIO $ M.member ety <$> readIORef cMap
    explGet    (Space _ _ cMap _ _) ety = liftIO $ do
      Just (Record _ cons)  <- M.lookup ety <$> readIORef cMap
      return cons
  
  -- MaxForce
  getMaxForce :: Ptr Constraint -> IO Double
  getMaxForce c = realToFrac <$> [C.exp| double { cpConstraintGetMaxForce ($(cpConstraint* c)) } |]
  
  setMaxForce :: Ptr Constraint -> Double -> IO ()
  setMaxForce c (realToFrac -> maxForce) = [C.exp| void { cpConstraintSetMaxForce($(cpConstraint* c), $(double maxForce)); } |]
  
  instance Component MaxForce where
    type Storage MaxForce = Space MaxForce
  
  instance (MonadIO m, Has w m Physics) => Has w m MaxForce where
    getStore = (cast :: Space Physics -> Space MaxForce) <$> getStore
  
  instance MonadIO m => ExplMembers m (Space MaxForce) where
    explMembers s = explMembers (cast s :: Space Constraint)
  
  instance MonadIO m => ExplSet m (Space MaxForce) where
    explSet (Space _ _ cMap _ _) ety (MaxForce vec) = liftIO $ do
      rd <- M.lookup ety <$> readIORef cMap
      case rd of
        Nothing -> return ()
        Just (Record c _) -> setMaxForce c vec
  
  instance MonadIO m => ExplGet m (Space MaxForce) where
    explExists s ety = explExists (cast s :: Space Constraint) ety
    explGet (Space _ _ cMap _ _) ety = liftIO $ do
      Just (Record c _) <- M.lookup ety <$> readIORef cMap
      MaxForce <$> getMaxForce c
  
  -- MaxBias
  getMaxBias :: Ptr Constraint -> IO Double
  getMaxBias c = do
    maxBias <- [C.exp| double { cpConstraintGetMaxBias ($(cpConstraint* c)) } |]
    return (realToFrac maxBias)
  
  setMaxBias :: Ptr Constraint -> Double -> IO ()
  setMaxBias c (realToFrac -> maxBias) = [C.exp| void { cpConstraintSetMaxBias($(cpConstraint* c), $(double maxBias)); } |]
  
  instance Component MaxBias where
    type Storage MaxBias = Space MaxBias
  
  instance (MonadIO m, Has w m Physics) => Has w m MaxBias where
    getStore = (cast :: Space Physics -> Space MaxBias) <$> getStore
  
  instance MonadIO m => ExplMembers m (Space MaxBias) where
    explMembers s = explMembers (cast s :: Space Constraint)
  
  instance MonadIO m => ExplSet m (Space MaxBias) where
    explSet (Space _ _ cMap _ _) ety (MaxBias vec) = liftIO $ do
      rd <- M.lookup ety <$> readIORef cMap
      case rd of
        Nothing -> return ()
        Just (Record c _) -> setMaxBias c vec
  
  instance MonadIO m => ExplGet m (Space MaxBias) where
    explGet (Space _ _ cMap _ _) ety = liftIO $ do
      Just (Record c _) <- M.lookup ety <$> readIORef cMap
      MaxBias <$> getMaxBias c
    explExists s ety = explExists (cast s :: Space Constraint) ety
  
  -- ErrorBias
  getErrorBias :: Ptr Constraint -> IO Double
  getErrorBias c = liftIO $ do
    errorBias <- [C.exp| double { cpConstraintGetErrorBias ($(cpConstraint* c)) } |]
    return (realToFrac errorBias)
  
  setErrorBias :: Ptr Constraint -> Double -> IO ()
  setErrorBias c (realToFrac -> errorBias) = [C.exp| void { cpConstraintSetErrorBias($(cpConstraint* c), $(double errorBias)); } |]
  
  instance Component ErrorBias where
    type Storage ErrorBias = Space ErrorBias
  
  instance (MonadIO m, Has w m Physics) => Has w m ErrorBias where
    getStore = (cast :: Space Physics -> Space ErrorBias) <$> getStore
  
  instance MonadIO m => ExplMembers m (Space ErrorBias) where
    explMembers s = explMembers (cast s :: Space Constraint)
  
  instance MonadIO m => ExplSet m (Space ErrorBias) where
    explSet (Space _ _ cMap _ _) ety (ErrorBias vec) = liftIO $ do
      rd <- M.lookup ety <$> readIORef cMap
      case rd of
        Nothing -> return ()
        Just (Record c _) -> setErrorBias c vec
  
  instance MonadIO m => ExplGet m (Space ErrorBias) where
    explExists s ety = explExists (cast s :: Space Constraint) ety
    explGet (Space _ _ cMap _ _) ety = liftIO $ do
      Just (Record c _) <- M.lookup ety <$> readIORef cMap
      ErrorBias <$> getErrorBias c
  
  -- Impulse
  getImpulse :: Ptr Constraint -> IO Double
  getImpulse c = realToFrac <$> [C.exp| double { cpConstraintGetImpulse ($(cpConstraint* c)) } |]
  
  instance Component Impulse where
    type Storage Impulse = Space Impulse
  
  instance (MonadIO m, Has w m Physics) => Has w m Impulse where
    getStore = (cast :: Space Physics -> Space Impulse) <$> getStore
  
  instance MonadIO m => ExplMembers m (Space Impulse) where
    explMembers s = explMembers (cast s :: Space Constraint)
  
  instance MonadIO m => ExplGet m (Space Impulse) where
    explExists s ety = explExists (cast s :: Space Constraint) ety
    explGet (Space _ _ cMap _ _) ety = liftIO $ do
      Just (Record c _) <- M.lookup ety <$> readIORef cMap
      Impulse <$> getImpulse c
  
  -- CollideBodies
  getCollideBodies :: Ptr Constraint -> IO Bool
  getCollideBodies c = do
    collide <- [C.exp| int { cpConstraintGetCollideBodies ($(cpConstraint* c)) } |]
    return . toEnum . fromIntegral $ collide
  
  setCollideBodies :: Ptr Constraint -> Bool -> IO ()
  setCollideBodies c (fromIntegral . fromEnum -> collide) = [C.exp| void { cpConstraintSetCollideBodies($(cpConstraint* c), $(int collide)); } |]
  
  instance Component CollideBodies where
    type Storage CollideBodies = Space CollideBodies
  
  instance (MonadIO m, Has w m Physics) => Has w m CollideBodies where
    getStore = (cast :: Space Physics -> Space CollideBodies) <$> getStore
  
  instance MonadIO m => ExplMembers m (Space CollideBodies) where
    explMembers s = explMembers (cast s :: Space Constraint)
  
  instance MonadIO m => ExplSet m (Space CollideBodies) where
    explSet (Space _ _ cMap _ _) ety (CollideBodies vec) = liftIO $ do
      rd <- M.lookup ety <$> readIORef cMap
      case rd of
        Nothing -> return ()
        Just (Record c _) -> setCollideBodies c vec
  
  instance MonadIO m => ExplGet m (Space CollideBodies) where
    explExists s ety = explExists (cast s :: Space Constraint) ety
    explGet (Space _ _ cMap _ _) ety = liftIO $ do
      Just (Record c _) <- M.lookup ety <$> readIORef cMap
      CollideBodies <$> getCollideBodies c
  