module Physics.Bullet.Raw.Types where

import Foreign.Marshal.Alloc
import Foreign.Marshal.Array
import Foreign.ForeignPtr
import Foreign.Storable
import Foreign.Ptr

type Scalar     = Float
data Vector3    = Vector3 !Scalar !Scalar !Scalar
    deriving Show
data Vector4    = Vector4 !Scalar !Scalar !Scalar !Scalar
    deriving Show
data Matrix3x3  = Matrix3x3 !Vector3 !Vector3 !Vector3
    deriving Show
data Quaternion = Quaternion !Scalar !Scalar !Scalar !Scalar
    deriving Show
data Transform  = Transform !Matrix3x3 !Vector3
    deriving Show

-- Vector3
allocaVector3 :: (Storable a) => (Ptr a -> IO b) -> IO b
allocaVector3 = allocaBytes 12

withVector3 :: Vector3 -> (Ptr a -> IO  b) -> IO  b
withVector3 v f = allocaVector3 $ \p -> do
    pokeVector3 v p
    f $ castPtr p

pokeVector3 (Vector3 x y z) p = do
    pokeElemOff p 0 x 
    pokeElemOff p 1 y
    pokeElemOff p 2 z

peekVector3 :: Ptr a -> IO Vector3
peekVector3 p' = do
    let p = castPtr p'
    x <- peekElemOff p 0
    y <- peekElemOff p 1
    z <- peekElemOff p 2
    return $ Vector3 x y z

-- Vector4
allocaVector4 :: (Storable a) => (Ptr a -> IO b) -> IO b
allocaVector4 = allocaBytes 16

withVector4 :: Vector4 -> (Ptr a -> IO  b) -> IO  b
withVector4 v f = allocaVector4 $ \p -> do
    pokeVector4 v p
    f $ castPtr p

pokeVector4 (Vector4 x y z w) p = do
    pokeElemOff p 0 x 
    pokeElemOff p 1 y
    pokeElemOff p 2 z
    pokeElemOff p 3 w

peekVector4 :: Ptr a -> IO Vector4
peekVector4 p' = do
    let p = castPtr p'
    x <- peekElemOff p 0
    y <- peekElemOff p 1
    z <- peekElemOff p 2
    w <- peekElemOff p 3
    return $ Vector4 x y z w

-- Quaternion
allocaQuaternion :: (Storable a) => (Ptr a -> IO b) -> IO b
allocaQuaternion = allocaBytes 16

withQuaternion :: Quaternion -> (Ptr a -> IO  b) -> IO  b
withQuaternion v f = allocaQuaternion $ \p -> do
    pokeQuaternion v p
    f $ castPtr p

pokeQuaternion (Quaternion x y z w) p = do
    pokeElemOff p 0 x 
    pokeElemOff p 1 y
    pokeElemOff p 2 z
    pokeElemOff p 3 w

peekQuaternion :: Ptr a -> IO Quaternion
peekQuaternion p' = do
    let p = castPtr p'
    x <- peekElemOff p 0
    y <- peekElemOff p 1
    z <- peekElemOff p 2
    w <- peekElemOff p 3
    return $ Quaternion x y z w

-- Matrix3x3
allocaMatrix3x3 :: (Storable a) => (Ptr a -> IO b) -> IO b
allocaMatrix3x3 = allocaBytes 36

withMatrix3x3 :: Matrix3x3 -> (Ptr a -> IO  b) -> IO  b
withMatrix3x3 v f = allocaMatrix3x3 $ \p -> do
    pokeMatrix3x3 v p
    f $ castPtr p

pokeMatrix3x3 (Matrix3x3 a b c) p = do
    pokeVector3 a p
    pokeVector3 b $ plusPtr p 12
    pokeVector3 c $ plusPtr p 24

peekMatrix3x3 :: Ptr a -> IO Matrix3x3
peekMatrix3x3 p = do
    a <- peekVector3 p
    b <- peekVector3 $ plusPtr p 12
    c <- peekVector3 $ plusPtr p 24
    return $ Matrix3x3 a b c

-- Transform
allocaTransform :: (Storable a) => (Ptr a -> IO b) -> IO b
allocaTransform = allocaBytes 48

withTransform :: Transform -> (Ptr a -> IO  b) -> IO  b
withTransform v f = allocaTransform $ \p -> do
    pokeTransform v p
    f $ castPtr p

pokeTransform (Transform m v) p = do
    pokeMatrix3x3 m p
    pokeVector3 v $ plusPtr p 36

peekTransform :: Ptr a -> IO Transform
peekTransform p = do
    m <- peekMatrix3x3 p
    v <- peekVector3 $ plusPtr p 36
    return $ Transform m v