module Geom(Vec2(..), Vec3(..), Vec4(..), Mat4(..), Addable(..), Multipliable(..), mat4Transit, mat4rot3d,
  HasLength, normalize) where

import Foreign.Storable(Storable(..))
import Foreign.Ptr (Ptr, castPtr, plusPtr)

data Vec2 = Vec2 !Float !Float
  deriving (Eq, Show)

data Vec3 = Vec3 !Float !Float !Float
  deriving (Eq, Show)

data Vec4 = Vec4 !Float !Float !Float !Float
  deriving (Eq, Show)

infixl 6 ^+^
class Addable a where
  (^+^) :: a -> a -> a

instance Addable Vec2 where
  (Vec2 ax ay) ^+^ (Vec2 bx by) = Vec2 (ax + bx) (ay + by)

instance Addable Vec3 where
  (Vec3 ax ay az) ^+^ (Vec3 bx by bz) =
    Vec3 (ax + bx) (ay + by) (az + bz)

instance Addable Vec4 where
  (Vec4 a1 a2 a3 a4) ^+^ (Vec4 b1 b2 b3 b4) =
    Vec4 (a1 + b1) (a2 + b2) (a3 + b3) (a4 + b4)


infixl 7 ^*^
class Multipliable a b c where
    (^*^) :: a -> b -> c

instance Multipliable Vec2 Float Vec2 where
    (Vec2 ax ay) ^*^ b = Vec2 (ax * b) (ay * b)

instance Multipliable Vec3 Float Vec3 where
    (Vec3 ax ay az) ^*^ b = Vec3 (ax * b) (ay * b) (az * b)

instance Multipliable Vec4 Float Vec4 where
    (Vec4 ax ay az aw) ^*^ b = Vec4 (ax * b) (ay * b) (az * b) (aw * b)

data Mat4 = Mat4 !Vec4 !Vec4 !Vec4 !Vec4

instance Multipliable Mat4 Vec4 Vec4 where
  (Mat4 vx vy vz vw) ^*^ (Vec4 ax ay az aw) = (vx ^*^ ax ^+^ vy ^*^ ay ^+^ vz ^*^ az ^+^ vw ^*^ aw)

instance Multipliable Mat4 Mat4 Mat4 where
  m ^*^ (Mat4 bx by bz bw) = Mat4 (m ^*^ bx) (m ^*^ by) (m ^*^ bz) (m ^*^ bw)

mat4Transit :: Vec3 -> Mat4
mat4Transit (Vec3 x y z) = Mat4 (Vec4 1 0 0 0) (Vec4 0 1 0 0) (Vec4 0 0 1 0) (Vec4 x y z 1)

mat4rot3d :: Vec3 -> Float -> Mat4
mat4rot3d (Vec3 rx ry rz) a = let cosa = cos(a) in let sina = sin(a) in
  Mat4 
    (Vec4 (rx * rx * (1 - cosa) + cosa)       (rx * ry * (1 - cosa) + sina * rz) (rx * rz * (1 - cosa) - sina * ry) 0)
    (Vec4 (rx * ry * (1 - cosa) - sina * rz) (ry * ry * (1 - cosa) + cosa)       (ry * rz * (1 - cosa) + sina * rx) 0)
    (Vec4 (rx * rz * (1 - cosa) + sina * ry) (ry * rz * (1 - cosa) - sina * rx) (rz * rz * (1 - cosa) + cosa) 0)
    
    (Vec4 0 0 0 1)
    

instance Storable Vec2 where
  sizeOf _ = 2 * sizeOf (undefined :: Float)
  alignment _ = 4
  peek ptr = do
    let fptr = castPtr ptr :: Ptr Float
    x <- peek fptr
    y <- peek (fptr `plusPtr` sizeOf (undefined :: Float))
    return $ Vec2 x y
  poke ptr (Vec2 x y) = do
    let fptr = castPtr ptr :: Ptr Float
    poke fptr x
    poke (fptr `plusPtr` sizeOf (undefined :: Float)) y

instance Storable Vec3 where
  sizeOf _ = 3 * sizeOf (undefined :: Float)
  alignment _ = 4
  peek ptr = do
    let fptr = castPtr ptr :: Ptr Float
    let floatSize = sizeOf (undefined :: Float)
    x <- peek fptr
    y <- peek (fptr `plusPtr` floatSize)
    z <- peek (fptr `plusPtr` (2 * floatSize))
    return $ Vec3 x y z
  poke ptr (Vec3 x y z) = do
    let fptr = castPtr ptr :: Ptr Float
    let floatSize = sizeOf (undefined :: Float)
    poke fptr x
    poke (fptr `plusPtr` floatSize) y
    poke (fptr `plusPtr` (2 * floatSize)) z

instance Storable Vec4 where
  sizeOf _ = 4 * sizeOf (undefined :: Float)
  alignment _ = 4
  peek ptr = do
    let fptr = castPtr ptr :: Ptr Float
    let floatSize = sizeOf (undefined :: Float)
    x <- peek fptr
    y <- peek (fptr `plusPtr` floatSize)
    z <- peek (fptr `plusPtr` (2 * floatSize))
    w <- peek (fptr `plusPtr` (3 * floatSize))
    return $ Vec4 x y z w
  poke ptr (Vec4 x y z w) = do
    let fptr = castPtr ptr :: Ptr Float
    let floatSize = sizeOf (undefined :: Float)
    poke fptr x
    poke (fptr `plusPtr` floatSize) y
    poke (fptr `plusPtr` (2 * floatSize)) z
    poke (fptr `plusPtr` (3 * floatSize)) w

instance Storable Mat4 where
  sizeOf _ = 4 * sizeOf (undefined :: Vec4)
  alignment _ = 4
  peek ptr = do
    let vec4Size = sizeOf (undefined :: Vec4)
    v1 <- peek (castPtr ptr)
    v2 <- peek (ptr `plusPtr` vec4Size)
    v3 <- peek (ptr `plusPtr` (2 * vec4Size))
    v4 <- peek (ptr `plusPtr` (3 * vec4Size))
    return $ Mat4 v1 v2 v3 v4
  poke ptr (Mat4 v1 v2 v3 v4) = do
    let vec4Size = sizeOf (undefined :: Vec4)
    poke (castPtr ptr) v1
    poke (ptr `plusPtr` vec4Size) v2
    poke (ptr `plusPtr` (2 * vec4Size)) v3
    poke (ptr `plusPtr` (3 * vec4Size)) v4

class HasLength a where
  vlength :: a -> Float

normalize :: (Multipliable a Float a, HasLength a) => a -> a
normalize v = v ^*^ (1 / (vlength v))

instance HasLength Vec2 where
  vlength (Vec2 a b) = sqrt (a * a + b * b)