{-# LANGUAGE ImportQualifiedPost #-}

module Support.Reference.Quaternion
  ( multiply
  , axisAngle
  , intrinsic
  , extrinsic
  , rotate
  ) where

import Linear (Quaternion(..), V3(..))
import Linear.Epsilon (Epsilon)

import Support.Reference.Vec3 qualified as Vec3

multiply :: Num a => Quaternion a -> Quaternion a -> Quaternion a
multiply (Quaternion as v1) (Quaternion bs v2) = Quaternion s v
  where
    s = as * bs - Vec3.dot v1 v2
    v = Vec3.cross v1 v2 + fmap (* as) v2 + fmap (* bs) v1

axisAngle :: (Floating a, Epsilon a) => V3 a -> a -> Quaternion a
axisAngle axis rads =
  Quaternion (cos half) (fmap (* sin half) (Vec3.normalize axis))
  where
    half = rads / 2

intrinsic :: Floating a => a -> a -> a -> Quaternion a
intrinsic roll pitch yaw =
  Quaternion
    (cx * cy * cz + sx * sy * sz)
    (V3
      (sx * cy * cz + cx * sy * sz)
      (cx * sy * cz - sx * cy * sz)
      (cx * cy * sz - sx * sy * cz))
  where
    cx = cos (pitch * 0.5)
    sx = sin (pitch * 0.5)
    cy = cos (yaw * 0.5)
    sy = sin (yaw * 0.5)
    cz = cos (roll * 0.5)
    sz = sin (roll * 0.5)

extrinsic :: (Floating a, Epsilon a) => a -> a -> a -> Quaternion a
extrinsic heading elevation tilt =
  axisAngle (V3 0 0 1) tilt `multiply`
  axisAngle (V3 1 0 0) elevation `multiply`
  axisAngle (V3 0 1 0) heading

rotate :: Num a => Quaternion a -> V3 a -> V3 a
rotate (Quaternion w (V3 x y z)) (V3 vx vy vz) =
  V3
    (vx + w * tx + y * tz - z * ty)
    (vy + w * ty + z * tx - x * tz)
    (vz + w * tz + x * ty - y * tx)
  where
    x2 = x + x
    y2 = y + y
    z2 = z + z
    tx = y2 * vz - z2 * vy
    ty = z2 * vx - x2 * vz
    tz = x2 * vy - y2 * vx