content: {-# LANGUAGE DataKinds #-}
  module Main where
  
  -- base
  import Control.Monad          (forM_, unless, void)
  import Control.Monad.IO.Class (liftIO)
  
  -- GPipe
  import Graphics.GPipe
  
  -- GPipe-GLFW
  import qualified Graphics.GPipe.Context.GLFW as GLFW
  
  -- switch
  import Device.Nintendo.Switch
  
  import Madgwick
  
  toV3 :: V4 a -> V3 a
  toV3 (V4 x y z _) = V3 x y z
  
  toV4 :: Num a => V3 a -> V4 a
  toV4 (V3 x y z) = V4 x y z 1
  
  rot :: Num a => Quaternion a -> V3 a -> V3 a
  rot (Quaternion s u) v = (V3 a a a) * u + (V3 b b b) * v + (V3 c c c) * cross u v
    where a = 2 * (dot u v)
          b = s*s - dot u u
          c = 2 * s
  
  main :: IO ()
  main = do
    putStrLn "--------------------------------------------------------------------"
    putStrLn "Testing sensors of all connected left Joy-Cons one after another ..."
    putStrLn "Close the window to end the test run."
    putStrLn "--------------------------------------------------------------------"
    withConsole $ \switch -> do
      infos <- getControllerInfos switch
      forM_ infos $ \info ->
        withController info $ \controller -> do
          setInertialMeasurement True controller
          void $ getInput controller -- ack
          runContextT GLFW.defaultHandleConfig $ do
            -- window
            let windowConfig =
                  GLFW.WindowConfig
                    800 800
                    "Left Joy-Con 3D Test (will drift, press Capture to reset)"
                    Nothing [] Nothing
            win <- newWindow (WindowFormatColor RGB8) windowConfig
            
            -- cube vertices
            vertexBuffer <- newBuffer 36
            writeBuffer vertexBuffer 0
              [ (V4 (-0.5) (-0.5) (-0.5) 1, V3 1 0 1)
              , (V4 ( 0.5) ( 0.5) (-0.5) 1, V3 1 0 1)
              , (V4 ( 0.5) (-0.5) (-0.5) 1, V3 1 0 1)
              , (V4 (-0.5) (-0.5) (-0.5) 1, V3 1 0 1)
              , (V4 (-0.5) ( 0.5) (-0.5) 1, V3 1 0 1)
              , (V4 ( 0.5) ( 0.5) (-0.5) 1, V3 1 0 1)
              
              , (V4 (-0.5) (-0.5) (-0.5) 1, V3 0 0 1)
              , (V4 (-0.5) (-0.5) ( 0.5) 1, V3 0 0 1)
              , (V4 (-0.5) ( 0.5) ( 0.5) 1, V3 0 0 1)
              , (V4 (-0.5) (-0.5) (-0.5) 1, V3 0 0 1)
              , (V4 (-0.5) ( 0.5) ( 0.5) 1, V3 0 0 1)
              , (V4 (-0.5) ( 0.5) (-0.5) 1, V3 0 0 1)
              
              , (V4 ( 0.5) (-0.5) (-0.5) 1, V3 0 1 0)
              , (V4 ( 0.5) ( 0.5) ( 0.5) 1, V3 0 1 0)
              , (V4 ( 0.5) (-0.5) ( 0.5) 1, V3 0 1 0)
              , (V4 ( 0.5) (-0.5) (-0.5) 1, V3 0 1 0)
              , (V4 ( 0.5) ( 0.5) (-0.5) 1, V3 0 1 0)
              , (V4 ( 0.5) ( 0.5) ( 0.5) 1, V3 0 1 0)
              
              , (V4 (-0.5) (-0.5) ( 0.5) 1, V3 1 0 0)
              , (V4 ( 0.5) (-0.5) ( 0.5) 1, V3 1 0 0)
              , (V4 ( 0.5) ( 0.5) ( 0.5) 1, V3 1 0 0)
              , (V4 (-0.5) (-0.5) ( 0.5) 1, V3 1 0 0)
              , (V4 ( 0.5) ( 0.5) ( 0.5) 1, V3 1 0 0)
              , (V4 (-0.5) ( 0.5) ( 0.5) 1, V3 1 0 0)
              
              , (V4 (-0.5) (-0.5) (-0.5) 1, V3 0 1 1)
              , (V4 ( 0.5) (-0.5) (-0.5) 1, V3 0 1 1)
              , (V4 ( 0.5) (-0.5) ( 0.5) 1, V3 0 1 1)
              , (V4 (-0.5) (-0.5) (-0.5) 1, V3 0 1 1)
              , (V4 ( 0.5) (-0.5) ( 0.5) 1, V3 0 1 1)
              , (V4 (-0.5) (-0.5) ( 0.5) 1, V3 0 1 1)
              
              , (V4 (-0.5) ( 0.5) (-0.5) 1, V3 1 1 0)
              , (V4 ( 0.5) ( 0.5) ( 0.5) 1, V3 1 1 0)
              , (V4 ( 0.5) ( 0.5) (-0.5) 1, V3 1 1 0)
              , (V4 (-0.5) ( 0.5) (-0.5) 1, V3 1 1 0)
              , (V4 (-0.5) ( 0.5) ( 0.5) 1, V3 1 1 0)
              , (V4 ( 0.5) ( 0.5) ( 0.5) 1, V3 1 1 0)
              ]
            
            uniformBuffer <- newBuffer 1
            
            shader <- compileShader $ do
              quart <- getUniform (const (uniformBuffer,0))
              primitiveStream <- toPrimitiveStream id
              let rotatedStream = fmap (\(v,c) -> (toV4 (rot quart (toV3 v)), c)) primitiveStream
              fragmentStream <- rasterize (const (Front, ViewPort (V2 0 0) (V2 800 800), DepthRange 0 1)) rotatedStream
              drawWindowColor (const (win, ContextColorOption NoBlending (V3 True True True))) fragmentStream
            
            loop controller (Quaternion 1 (V3 0 0 0)) vertexBuffer uniformBuffer shader win
          setInputMode Simple controller
          void $ getInput controller -- ack
  
  loop
    :: Controller 'LeftJoyCon
    -> Quaternion Float
    -> Buffer os (B4 Float, B3 Float)
    -> Buffer os (Uniform (Quaternion (B Float)))
    -> CompiledShader os (PrimitiveArray Triangles (B4 Float, B3 Float))
    -> Window os RGBFloat ()
    -> ContextT GLFW.Handle os IO ()
  loop controller quart vertexBuffer uniformBuffer shader win = do
    input <- liftIO $ getInput controller
    let newQuart@(Quaternion w (V3 xq yq zq)) =
          -- Capture button resets rotation because of sensor drift
          if btnCapture input then
            Quaternion 1 (V3 0 0 0)
          else
            case extras input of
              Inertial (a1,a2,a3) (g1, g2, g3) ->
                let
                  -- convert Joy-Con axes to Madgwick axes
                  flipAxes (x,y,z) = (x,-y,-z)
                  -- disable Z rotation (yaw) to stabilize output with 6 DoF IMU
                  zeroZ (x,y,_) = (x,y,0)
                  -- beta parameter can be tweaked for Madgwick algorithm
                  beta = 0.01
                  -- Joy-Con provides three measurements in 15ms, hence a 5ms rate
                  freq = 1 / 0.005
                  -- Madgwick algorithm for accelerometer and gyroscope sensor fusion
                  newQ1 = madgwick (flipAxes a1) (flipAxes $ zeroZ g1) beta freq quart
                  newQ2 = madgwick (flipAxes a2) (flipAxes $ zeroZ g2) beta freq newQ1
                  newQ3 = madgwick (flipAxes a3) (flipAxes $ zeroZ g3) beta freq newQ2
                in newQ3
              _ -> quart
    -- convert Madgwick axes to OpenGL axes
    let openGLquart = Quaternion w (V3 (-yq) zq xq)
    writeBuffer uniformBuffer 0 [openGLquart]
    render $ do
      clearWindowColor win (V3 0 0 0)
      vertexArray <- newVertexArray vertexBuffer
      shader $ toPrimitiveArray TriangleList vertexArray
    swapWindowBuffers win
  
    closeRequested <- GLFW.windowShouldClose win
    unless (closeRequested == Just True) $
      loop controller newQuart vertexBuffer uniformBuffer shader win