```{-# LANGUAGE NoImplicitPrelude #-}
{-# LANGUAGE MultiParamTypeClasses #-}
{-# LANGUAGE FlexibleInstances #-}
{- |
Maintainer  :  numericprelude@henning-thielemann.de
Stability   :  provisional
Portability :  portable (?)

Quaternions
-}

module Number.Quaternion
(
-- * Cartesian form
T(real,imag),
fromReal,
(+::),

-- * Conversions
toRotationMatrix,
fromRotationMatrix,
fromRotationMatrixDenorm,
toComplexMatrix,
fromComplexMatrix,

-- * Operations
scalarProduct,
crossProduct,
conjugate,
scale,
norm,
normSqr,
normalize,
similarity,
slerp,
)  where

import qualified Algebra.NormedSpace.Euclidean as NormedEuc
import qualified Algebra.VectorSpace  as VectorSpace
import qualified Algebra.Module       as Module
import qualified Algebra.Vector       as Vector
import qualified Algebra.Transcendental as Trans
import qualified Algebra.Algebraic    as Algebraic
import qualified Algebra.Field        as Field
import qualified Algebra.Ring         as Ring
import qualified Algebra.ZeroTestable as ZeroTestable

import Algebra.ZeroTestable(isZero)
import Algebra.Module((*>))
-- import Algebra.Algebraic((^/))

import qualified Number.Complex as Complex

import Number.Complex ((+:))

-- import qualified Data.Typeable as Ty
import Data.Array (Array, (!))
import qualified Data.Array as Array

import qualified Prelude as P
import PreludeBase
import NumericPrelude hiding (signum)
import Text.Show.HT (showsInfixPrec, )

{- TODO:
conversion to and from complex matrix
-}

infix  6  +::, `Cons`

{- |
Quaternions could be defined based on Complex numbers.
However quaternions are often considered as real part and three imaginary parts.
-}
data T a
= Cons {real :: !a           -- ^ real part
,imag :: !(a, a, a)   -- ^ imaginary parts
}
deriving (Eq)

fromReal :: Additive.C a => a -> T a
fromReal x = Cons x zero

plusPrec :: Int
plusPrec = 6

instance (Show a) => Show (T a) where
showsPrec prec (x `Cons` y) = showsInfixPrec "+::" plusPrec prec x y

-- | Construct a quaternion from real and imaginary part.
(+::) :: a -> (a,a,a) -> T a
(+::) = Cons

-- | The conjugate of a quaternion.
{-# SPECIALISE conjugate :: T Double -> T Double #-}
conjugate	 :: (Additive.C a) => T a -> T a
conjugate (Cons r i) =  Cons r (negate i)

-- | Scale a quaternion by a real number.
{-# SPECIALISE scale :: Double -> T Double -> T Double #-}
scale		 :: (Ring.C a) => a -> T a -> T a
scale r (Cons xr xi) =  Cons (r * xr) (scaleImag r xi)

-- | like Module.*> but without additional class dependency
scaleImag	 :: (Ring.C a) => a -> (a,a,a) -> (a,a,a)
scaleImag r (xi,xj,xk) =  (r * xi, r * xj, r * xk)

-- | the same as NormedEuc.normSqr but with a simpler type class constraint
normSqr		 :: (Ring.C a) => T a -> a
normSqr (Cons xr xi) = xr*xr + scalarProduct xi xi

norm		 :: (Algebraic.C a) => T a -> a
norm x = sqrt (normSqr x)

-- | scale a quaternion into a unit quaternion
normalize	 :: (Algebraic.C a) => T a -> T a
normalize x = scale (recip (norm x)) x

scalarProduct	 :: (Ring.C a) => (a,a,a) -> (a,a,a) -> a
scalarProduct (xi,xj,xk) (yi,yj,yk) =
xi*yi + xj*yj + xk*yk

crossProduct	 :: (Ring.C a) => (a,a,a) -> (a,a,a) -> (a,a,a)
crossProduct (xi,xj,xk) (yi,yj,yk) =
(xj*yk - xk*yj, xk*yi - xi*yk, xi*yj - xj*yi)

{- | similarity mapping as needed for rotating 3D vectors

It holds
@similarity (cos(a\/2) +:: scaleImag (sin(a\/2)) v) (0 +:: x) == (0 +:: y)@
where @y@ results from rotating @x@ around the axis @v@ by the angle @a@.
-}
similarity	 :: (Field.C a) => T a -> T a -> T a
similarity c x = c*x/c

{-
rotate	 :: (Field.C a) =>
(a,a,a)  {- ^ rotation axis, must be normalized -}
-> T a
-> T a
rotate c x = c*x/c
-}

{- |
Let @c@ be a unit quaternion, then it holds
@similarity c (0+::x) == toRotationMatrix c * x@
-}
toRotationMatrix :: (Ring.C a) => T a -> Array (Int,Int) a
toRotationMatrix (Cons r (i,j,k)) =
let r2 = r^2
i2 = i^2;   j2 = j^2;   k2 = k^2
ri = 2*r*i; rj = 2*r*j; rk = 2*r*k
jk = 2*j*k; ki = 2*k*i; ij = 2*i*j
in  Array.listArray ((0,0),(2,2)) \$ concat \$
[r2+i2-j2-k2, ij-rk,       ki+rj      ] :
[ij+rk,       r2-i2+j2-k2, jk-ri      ] :
[ki-rj,       jk+ri,       r2-i2-j2+k2] :
[]

fromRotationMatrix :: (Algebraic.C a) => Array (Int,Int) a -> T a
fromRotationMatrix =
normalize . fromRotationMatrixDenorm

checkBounds :: (Int,Int) -> Array (Int,Int) a -> Array (Int,Int) a
checkBounds (c,r) arr =
let bnds@((c0,r0), (c1,r1)) = Array.bounds arr
in  if c1-c0==c && r1-r0==r
then Array.listArray ((0,0), (c1-c0, r1-r0))
(Array.elems arr)
else error ("Quaternion.checkBounds: invalid matrix size "
++ show bnds)

{- |
The rotation matrix must be normalized.
(I.e. no rotation with scaling)
The computed quaternion is not normalized.
-}
fromRotationMatrixDenorm :: (Ring.C a) => Array (Int,Int) a -> T a
fromRotationMatrixDenorm mat' =
let mat = checkBounds (2,2) mat'
trace = sum (map (\i -> mat ! (i,i)) [0..2])
dif (i,j) = mat!(i,j) - mat!(j,i)
in  Cons (trace+1) (dif (2,1), dif (0,2), dif (1,0))

{- |
Map a quaternion to complex valued 2x2 matrix,
such that quaternion addition and multiplication
is mapped to matrix addition and multiplication.
The determinant of the matrix equals the squared quaternion norm ('normSqr').
Since complex numbers can be turned into real (orthogonal) matrices,
a quaternion could also be converted into a real matrix.
-}
T a -> Array (Int,Int) (Complex.T a)
toComplexMatrix (Cons r (i,j,k)) =
Array.listArray ((0,0), (1,1))
[r+:i, (-j)+:(-k), j+:(-k), r+:(-i)]

{- |
Revert 'toComplexMatrix'.
-}
fromComplexMatrix :: (Field.C a) =>
Array (Int,Int) (Complex.T a) -> T a
fromComplexMatrix mat =
let xs = Array.elems (checkBounds (1,1) mat)
[ar,br,cr,dr] = map Complex.real xs
[ai,bi,ci,di] = map Complex.imag xs
in  scale (1/2) (Cons (ar+dr) (ai-di, cr-br, -ci-bi))

{- |
Spherical Linear Interpolation

Can be generalized to any transcendent Hilbert space.
In fact, we should also include the real part in the interpolation.
-}
slerp :: (Trans.C a) =>
a   {- ^ For @0@ return vector @v@,
for @1@ return vector @w@ -}
-> (a,a,a)  {- ^ vector @v@, must be normalized -}
-> (a,a,a)  {- ^ vector @w@, must be normalized -}
-> (a,a,a)
slerp c v w =
let scal  = scalarProduct v w /
sqrt (scalarProduct v v * scalarProduct w w)
angle = Trans.acos scal
in  scaleImag (recip (Algebraic.sqrt (1-scal^2)))
(scaleImag (Trans.sin ((1-c)*angle)) v +
scaleImag (Trans.sin (   c *angle)) w)

instance (NormedEuc.Sqr a b) => NormedEuc.Sqr a (T b) where
normSqr (Cons r i) = NormedEuc.normSqr r + NormedEuc.normSqr i

instance (Algebraic.C a, NormedEuc.Sqr a b) => NormedEuc.C a (T b) where
norm = NormedEuc.defltNorm

instance (ZeroTestable.C a) => ZeroTestable.C (T a)  where
isZero (Cons r i)  = isZero r && isZero i

{-# SPECIALISE instance Additive.C (T Float) #-}
{-# SPECIALISE instance Additive.C (T Double) #-}
zero			=  Cons zero zero
(Cons xr xi) + (Cons yr yi)	=  Cons (xr+yr) (xi+yi)
(Cons xr xi) - (Cons yr yi)	=  Cons (xr-yr) (xi-yi)
negate (Cons x y)		=  Cons (negate x) (negate y)

instance (Ring.C a) => Ring.C (T a)  where
{-# SPECIALISE instance Ring.C (T Float) #-}
{-# SPECIALISE instance Ring.C (T Double) #-}
one				=  Cons one zero
fromInteger			=  fromReal . fromInteger
(Cons xr xi) * (Cons yr yi)	=
Cons (xr*yr - scalarProduct xi yi)
(scaleImag xr yi + scaleImag yr xi +
crossProduct xi yi)

instance (Field.C a) => Field.C (T a)  where
{-# SPECIALISE instance Field.C (T Float) #-}
{-# SPECIALISE instance Field.C (T Double) #-}
recip x = scale (recip (normSqr x)) (conjugate x)
(Cons xr xi) / y@(Cons yr yi) =
scale (recip (normSqr y))
(Cons (xr*yr + scalarProduct xi yi)
(scaleImag yr xi - scaleImag xr yi - crossProduct xi yi))

instance Vector.C T where
zero  = zero
(<+>) = (+)
(*>)  = scale

-- | The '(*>)' method can't replace 'scale'
--   because it requires the Algebra.Module constraint
instance (Module.C a b) => Module.C a (T b) where
s *> (Cons r i)  = Cons (s *> r) (s *> i)

instance (VectorSpace.C a b) => VectorSpace.C a (T b)

```