{-# LANGUAGE BangPatterns #-}
module Geometry.Vector3D
    where
import Geometry.Vector
import Geometry.Data

import Data.List

infixl 6 +.+.+, -.-.-
infixl 7 *.*.*

{- | 3D coordinate-wise addition.  -}
(+.+.+) :: Point3 -> Point3 -> Point3
{-# INLINE (+.+.+) #-}
V3 x1 y1 z1 +.+.+ V3 x2 y2 z2 =
  let
    !x = x1 + x2
    !y = y1 + y2
    !z = z1 + z2
  in V3 x y z
{- | 3D coordinate-wise subtraction.  -}
(-.-.-) :: Point3 -> Point3 -> Point3
{-# INLINE (-.-.-) #-}
V3 x1 y1 z1 -.-.- V3 x2 y2 z2 =
  let
    !x = x1 - x2
    !y = y1 - y2
    !z = z1 - z2
  in V3 x y z
{- | 3D scalar multiplication.  -}
(*.*.*) :: Float -> Point3 -> Point3
{-# INLINE (*.*.*) #-}
a *.*.* (V3 x2 y2 z2) =
  let
    !x = a * x2
    !y = a * y2
    !z = a * z2
  in V3 x y z

crossProd :: Point3 -> Point3 -> Point3
{-# INLINE crossProd #-}
crossProd (V3 x y z) (V3 a b c) = V3
    ( y * c - z * b)
    ( z * a - x * c)
    ( x * b - y * a)

rotate3 :: Float -> Point3 -> Point3
{-# INLINE rotate3 #-}
rotate3 a (V3 x y z) = V3 x' y' z
  where 
    (V2 x' y') = rotateV a (V2 x y)

rotate3z :: Float -> Point3 -> Point3
{-# INLINE rotate3z #-}
rotate3z a (V3 x y z) = V3 x' y' z
  where 
    (V2 x' y') = rotateV a (V2 x y)

rotate3x :: Float -> Point3 -> Point3
{-# INLINE rotate3x #-}
rotate3x a (V3 x y z) = V3 x y' z'
  where 
    (V2 y' z') = rotateV a (V2 y z)

rotate3y :: Float -> Point3 -> Point3
{-# INLINE rotate3y #-}
rotate3y a (V3 x y z) = V3 x' y z'
  where 
    (V2 x' z') = rotateV a (V2 x z)

vNormaly :: Point3 -> Point3
{-# INLINE vNormaly #-}
vNormaly (V3 x y z) = V3 x' y z'
  where
    V2 x' z' = vNormal $ V2 x z

magV3 :: Point3 -> Float
{-# INLINE magV3 #-}
magV3 (V3 x y z) = sqrt $ x^i + y^i + z^i
  where
    i = 2 :: Int

normalizeV3 :: Point3 -> Point3
{-# INLINE normalizeV3 #-}
normalizeV3 (V3 0 0 0) = V3 0 0 0
normalizeV3 p = (1 / magV3 p) *.*.* p

addZ :: Float -> Point2 -> Point3
{-# INLINE addZ #-}
addZ z (V2 x y) = V3 x y z

stripZ :: Point3 -> Point2
{-# INLINE stripZ #-}
stripZ (V3 x y _) = V2 x y

dist3 :: Point3 -> Point3 -> Float
{-# INLINE dist3 #-}
dist3 !p1 !p2 = magV3 (p2 -.-.- p1)

orderAround3
    :: Point3 -- ^ Vector to order around
    -> [Point3]
    -> [Point3]
orderAround3 v ps = sortOn (argV . prj) ps
  where
    xdir = crossProd v (head ps)
    ydir = crossProd v xdir
    prj p = V2 (dotV3 xdir p) (dotV3 ydir p)

vCen3 :: [Point3] -> Point3
vCen3 ps = (1 / fromIntegral (length ps)) *.*.* foldr (+.+.+) (V3 0 0 0) ps 

dotV3 :: Point3 -> Point3 -> Float
dotV3 (V3 x y z) (V3 a b c) = x*a + y*b + z*c

angleVV3 :: Point3 -> Point3 -> Float
angleVV3 a b 
    | a == b = 0
    | otherwise = acos $ dotV3 a b / (magV3 a * magV3 b)

projV3 :: Point3 -> Point3 -> Point3
projV3 = undefined

onXY :: (Point2 -> Point2) -> Point3 -> Point3
onXY f (V3 x y z) = V3 x' y' z
  where
    V2 x' y' = f (V2 x y)