{-# LANGUAGE ScopedTypeVariables #-}
{-# OPTIONS_GHC -fno-warn-orphans #-}

{- |
WARNING: orphan instances concerning Aeson classes and Linear.Quaternion datatypes have been introduced.
The warnings have been disabled.
-}
module Quaternion (
    qID,
    qz,
    qToV3,
    qToV2,
    qToAng,
    rotateToZ,
    vToQuat,
    comp,
    prePos,
    module Linear.Quaternion,
    qNoRoll,
) where

import Geometry.Vector
import Data.Aeson
import Geometry.Data
import Geometry.Vector3D
import Linear.Quaternion
import qualified Linear.Quaternion as Q
import Control.Lens
import Linear

instance ToJSON a => ToJSON (Q.Quaternion a) where
    toEncoding = genericToEncoding defaultOptions

instance FromJSON a => FromJSON (Q.Quaternion a)

-- apply a rotation as if the z axis moves to the new point.
-- i think this may instead do as if the new point moves to be on z axis
rotateToZ :: Point3 -> Point3 -> Point3
rotateToZ z1
    | cprod == V3 0 0 0 = id
    | otherwise = Q.rotate $ Q.axisAngle cprod (angleVV3 z1 (V3 0 0 1))
  where
    cprod = crossProd z1 (V3 0 0 1)

vToQuat :: Point3 -> Point3 -> Q.Quaternion Float
vToQuat a b
    | cprod == V3 0 0 0 = Q.axisAngle (V3 0 0 1) 0
    | otherwise = Q.axisAngle cprod (angleVV3 a b)
  where
    cprod = crossProd a b

qNoRoll :: Point3 -> Q.Quaternion Float
--qNoRoll x = qz (argV $ x ^. _xy) * Q.axisAngle (V3 0 (-1) 0) (argV (V2 (x ^. _z) (norm (x ^. _xy))))
qNoRoll x =  qNoRoll2 x * qNoRoll1 x
--qNoRoll x = Q.axisAngle (V3 0 1 0) (argV (V2 (x ^. _z) (norm (x ^. _xy)))) * qz (argV $ x ^. _xy)
--
qNoRoll1 :: Point3 -> Q.Quaternion Float
qNoRoll1 x =  Q.axisAngle (V3 0 (-1) 0) . argV $ V2 (norm (x ^. _xy)) (x ^. _z)

qNoRoll2 :: Point3 -> Q.Quaternion Float
qNoRoll2 x =  qz (argV $ x ^. _xy)

qToV3 :: Q.Quaternion Float -> Point3
qToV3 q = Q.rotate q (V3 1 0 0)

qToV2 :: Q.Quaternion Float -> Point2
qToV2 = (\(V3 x y _) -> V2 x y) . qToV3

qToAng :: Quaternion Float -> Float
qToAng = argV . qToV2

qID :: Q.Quaternion Float
qID = Q.axisAngle (V3 1 0 0) 0

comp :: Point3Q -> Point3Q -> Point3Q
comp (p,q) (p1,q1) = (p + Q.rotate q p1, q * q1)

prePos :: Point3Q -> Point3 -> Point3
prePos (p,q) p1 = p + Q.rotate q p1

qz :: Float -> Q.Quaternion Float
qz = Q.axisAngle (V3 0 0 1)

--deriving instance (Flat a => Flat (Quaternion a))