{-# OPTIONS_GHC -Wall #-}

module Main where

import SpatialMath
import qualified Quat

import Vis

ts :: Double
ts = 0.01

data State a = State (a,a) (Quat a)

simFun :: Float -> State Double -> State Double
simFun _ (State (x,v) q0) = State (x + v*ts, v + 5*ts*(-1 - x)) (Quat.qmult' q0 dq)
  where
    dq = Quat 1 (x*ts) (v*ts) (x*v*ts)

drawFun :: State Double -> VisObject Double
drawFun (State (x,_) quat) = VisObjects $ [axes,box,ellipsoid,sphere] ++ (map text [-5..5]) ++ [boxText, plane] 
  where
    axes = VisAxes (0.5, 15) (Xyz 0 0 0) (Quat 1 0 0 0)
    sphere = VisSphere 0.15 (Xyz 0 x (-1)) Wireframe (makeColor 0.2 0.3 0.8 1)
    ellipsoid = VisEllipsoid (0.2, 0.3, 0.4) (Xyz x 0 (-1)) quat Solid (makeColor 1 0.3 0.5 1)
    box = VisBox (0.2, 0.2, 0.2) (Xyz 0 0 x) quat Wireframe (makeColor 0 1 1 1)
    plane = VisPlane (Xyz 0 0 1) 0 (makeColor 1 1 1 1) (makeColor 0.4 0.6 0.65 0.4)
    text k = Vis2dText "OLOLOLOLOLO" (100,500 - k*100*x) TimesRoman24 (makeColor 0 (0.5 + x'/2) (0.5 - x'/2) 1)
      where
        x' = realToFrac $ (x + 1)/0.4*k/5
    boxText = Vis3dText "trololololo" (Xyz 0 0 (x-0.2)) TimesRoman24 (makeColor 1 0 0 1)

main :: IO ()
main = do
  let state0 = State (-1.4,0) (Quat 1 0 0 0)
  simulate ts state0 drawFun simFun