{-# 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.Query where

import           Apecs
import           Control.Monad.IO.Class (liftIO, MonadIO)
import           Foreign.C.Types
import           Foreign.ForeignPtr    (withForeignPtr)
import           Foreign.Marshal.Alloc
import           Foreign.Ptr
import           Foreign.Storable
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>"


-- cpFloat cpShapeNearestPointQuery(cpShape *shape, cpVect p, cpPointQueryInfo *out)
-- cpShape *cpSpacePointQueryNearest(cpSpace *space, cpVect point, cpFloat maxDistance, cpShapeFilter filter, cpPointQueryInfo *out)

pointQuery :: (MonadIO m, Has w m Physics) => WVec -> Double -> CollisionFilter -> SystemT w m (Maybe PointQueryResult)
pointQuery :: WVec
-> Double
-> CollisionFilter
-> SystemT w m (Maybe PointQueryResult)
pointQuery ((Double -> CDouble) -> WVec -> V2 CDouble
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> V2 CDouble
px CDouble
py) (Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> CDouble
maxDistance) (CollisionFilter CollisionGroup
gr (Bitmask CollisionGroup
cs) (Bitmask CollisionGroup
mk)) = do
  Space IOMap BodyRecord
_ IOMap (Record Shape)
_ IOMap (Record Constraint)
_ IOMap (Record CollisionHandler)
_ SpacePtr
spcPtr :: Space Physics <- SystemT w m (Space Physics)
forall w (m :: * -> *) c. Has w m c => SystemT w m (Storage c)
getStore
  IO (Maybe PointQueryResult) -> SystemT w m (Maybe PointQueryResult)
forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO (IO (Maybe PointQueryResult)
 -> SystemT w m (Maybe PointQueryResult))
-> IO (Maybe PointQueryResult)
-> SystemT w m (Maybe PointQueryResult)
forall a b. (a -> b) -> a -> b
$ (Ptr PointQueryResult -> IO (Maybe PointQueryResult))
-> IO (Maybe PointQueryResult)
forall a b. Storable a => (Ptr a -> IO b) -> IO b
alloca ((Ptr PointQueryResult -> IO (Maybe PointQueryResult))
 -> IO (Maybe PointQueryResult))
-> (Ptr PointQueryResult -> IO (Maybe PointQueryResult))
-> IO (Maybe PointQueryResult)
forall a b. (a -> b) -> a -> b
$ \Ptr PointQueryResult
pq -> do
    SpacePtr -> (Ptr FrnSpace -> IO ()) -> IO ()
forall a b. ForeignPtr a -> (Ptr a -> IO b) -> IO b
withForeignPtr SpacePtr
spcPtr ((Ptr FrnSpace -> IO ()) -> IO ())
-> (Ptr FrnSpace -> IO ()) -> IO ()
forall a b. (a -> b) -> a -> b
$ \Ptr FrnSpace
space -> [C.block| void {
      cpSpacePointQueryNearest
        ( $(cpSpace *space)
        , cpv($(double px), $(double py))
        , $(double maxDistance)
        , cpShapeFilterNew($(unsigned int gr), $(unsigned int cs), $(unsigned int mk))
        , $(cpPointQueryInfo *pq));
      }|]
    PointQueryResult
res <- Ptr PointQueryResult -> IO PointQueryResult
forall a. Storable a => Ptr a -> IO a
peek Ptr PointQueryResult
pq
    if Entity -> Int
unEntity (PointQueryResult -> Entity
pqShape PointQueryResult
res) Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== -Int
1
       then Maybe PointQueryResult -> IO (Maybe PointQueryResult)
forall (m :: * -> *) a. Monad m => a -> m a
return Maybe PointQueryResult
forall a. Maybe a
Nothing
       else Maybe PointQueryResult -> IO (Maybe PointQueryResult)
forall (m :: * -> *) a. Monad m => a -> m a
return (PointQueryResult -> Maybe PointQueryResult
forall a. a -> Maybe a
Just PointQueryResult
res)

instance Storable PointQueryResult where
  sizeOf :: PointQueryResult -> Int
sizeOf ~PointQueryResult
_ = Int
48 -- sizeOf (undefined :: Ptr Shape) + sizeOf (undefined :: CDouble) + 2*sizeOf (undefined :: V2 CDouble)
  alignment :: PointQueryResult -> Int
alignment ~PointQueryResult
_ = Int
8
  peek :: Ptr PointQueryResult -> IO PointQueryResult
peek Ptr PointQueryResult
ptr = do
    Ptr Shape
sPtr :: Ptr Shape <- Ptr PointQueryResult -> Int -> IO (Ptr Shape)
forall a b. Storable a => Ptr b -> Int -> IO a
peekByteOff Ptr PointQueryResult
ptr Int
0
    CIntPtr
s <- [C.block| intptr_t {
            cpShape *shape = $(cpShape *sPtr);
            if (shape==NULL) {
              return -1;
            } else {
              return (intptr_t) cpShapeGetUserData(shape);
            } }|]
    V2 CDouble
p :: V2 CDouble <- Ptr PointQueryResult -> Int -> IO (V2 CDouble)
forall a b. Storable a => Ptr b -> Int -> IO a
peekByteOff Ptr PointQueryResult
ptr Int
8
    CDouble
d :: CDouble <- Ptr PointQueryResult -> Int -> IO CDouble
forall a b. Storable a => Ptr b -> Int -> IO a
peekByteOff Ptr PointQueryResult
ptr Int
24
    V2 CDouble
g :: V2 CDouble <- Ptr PointQueryResult -> Int -> IO (V2 CDouble)
forall a b. Storable a => Ptr b -> Int -> IO a
peekByteOff Ptr PointQueryResult
ptr Int
32
    PointQueryResult -> IO PointQueryResult
forall (m :: * -> *) a. Monad m => a -> m a
return (PointQueryResult -> IO PointQueryResult)
-> PointQueryResult -> IO PointQueryResult
forall a b. (a -> b) -> a -> b
$ Entity -> WVec -> Double -> WVec -> PointQueryResult
PointQueryResult (Int -> Entity
Entity (Int -> Entity) -> (CIntPtr -> Int) -> CIntPtr -> Entity
forall b c a. (b -> c) -> (a -> b) -> a -> c
. CIntPtr -> Int
forall a b. (Integral a, Num b) => a -> b
fromIntegral (CIntPtr -> Entity) -> CIntPtr -> Entity
forall a b. (a -> b) -> a -> b
$ CIntPtr
s) (CDouble -> Double
forall a b. (Real a, Fractional b) => a -> b
realToFrac (CDouble -> Double) -> V2 CDouble -> WVec
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> V2 CDouble
p) (CDouble -> Double
forall a b. (Real a, Fractional b) => a -> b
realToFrac CDouble
d) (CDouble -> Double
forall a b. (Real a, Fractional b) => a -> b
realToFrac (CDouble -> Double) -> V2 CDouble -> WVec
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> V2 CDouble
g)
  poke :: Ptr PointQueryResult -> PointQueryResult -> IO ()
poke = Ptr PointQueryResult -> PointQueryResult -> IO ()
forall a. HasCallStack => a
undefined