import Data.Vect.Double
import Data.Vect.Double.Util.Dim3
import Data.Vect.Double.Util.Quaternion
import Data.Vect.Double.Interpolate

--------------------------------------------------------------------------------

delta = 1

ivs = concat
  [ [ (sinCos' a , sinCos' (a+delta) ) | i<-[0..64], let a = (fromIntegral i+0.5) * 2*pi/64 ] 
  , [ (sinCos' a , sinCos' (a+delta) ) | i<-[0..64], let a = (fromIntegral i-0.0) * 2*pi/64 ] 
  , [ (sinCos' (a+delta) , sinCos' a ) | i<-[0..64], let a = (fromIntegral i+0.5) * 2*pi/64 ] 
  , [ (sinCos' (a+delta) , sinCos' a ) | i<-[0..64], let a = (fromIntegral i-0.0) * 2*pi/64 ] 
  ]
  
ts = [0,0.1..1.0] :: [Double]

xs = map fromNormal [ interpolate t p q | (p,q)<-ivs, t<-ts ]  
ys = map fromNormal [ slerp       t p q | (p,q)<-ivs, t<-ts ]

maxerr = maximum $ zipWith distance xs ys
  
--------------------------------------------------------------------------------

axis = Vec3 1 2 3

as0 = [pi-0.200,pi-0.137..]
bs0 = [-0.420,pi+0.293..]

vs0a = map (extendWith 1 . fromNormal . fst) ivs :: [Vec3]
vs0b = map (extendWith 1 . fromNormal . snd) ivs :: [Vec3]

vs1a = map mkNormal $ zipWith (\a v -> rotate3 a axis v) as0 vs0a
vs1b = map mkNormal $ zipWith (\a v -> rotate3 a axis v) bs0 vs0b

ivs3 = zip vs1a vs1b :: [(Normal3,Normal3)]

us = map fromNormal [ interpolate t p q | (p,q)<-ivs3, t<-ts ]  
vs = map fromNormal [ slerp       t p q | (p,q)<-ivs3, t<-ts ]

maxerr3 = maximum $ zipWith distance us vs

--------------------------------------------------------------------------------

{-
axis  = Vec3 1 2 3
angl = pi/5

axis2  = Vec3 5 (-4) 1.77
angl2 = -pi/7

v = Vec3 (-9) 4 (-7)

x1 = rotU axis angl *. v   
x2 = v .* rotMatrix3 axis angl

m1r = fromOrtho $ rightOrthoU $ rotU axis angl
m1l = fromOrtho $ leftOrthoU $ rotU axis angl
m2 = rotMatrix3 axis angl

v' = x1
y1 = v' .* m1r
y2 = m1l *. v'
y3 = v'  .* m2 

z1 =  rotU axis2 angl2 *. rotU axis angl *. v
z2 = (rotU axis2 angl2 .*. rotU axis angl) *. v
z3 = v .* rotMatrix3 axis angl .* rotMatrix3 axis2 angl2
z4 = v .* (rotMatrix3 axis angl .*.  rotMatrix3 axis2 angl2)
-}

--------------------------------------------------------------------------------