{-# LANGUAGE DataKinds #-}

module Shared where

import           Linear.Quaternion
import           Linear.V3
import           Numeric.NumType.DK.Integers              ( TypeInt
                                                              ( Neg1
                                                              , Neg2
                                                              , Neg3
                                                              , Pos1
                                                              , Pos2
                                                              , Pos3
                                                              , Pos4
                                                              , Zero
                                                              )
                                                          )
import           Numeric.Units.Dimensional                ( Dimension(Dim) )
import           Numeric.Units.Dimensional.Prelude        ( (*~)
                                                          , (/~)
                                                          )
import qualified Numeric.Units.Dimensional.Prelude
                                               as U

type Effort = Double

type Velocity = U.Velocity Double
type AngularVelocity = U.AngularVelocity Double
type Angle = U.Angle Double
type Length = U.Length Double
type Distance = U.Length Double
type Radius = U.Length Double
type Mass = U.Mass Double
type Friction = U.Force Double
type Inertia = U.MomentOfInertia Double

type DRevoluteFriction = 'Dim 'Pos1 'Pos1 'Neg1 'Zero 'Zero 'Zero 'Zero
type RevoluteFriction = U.Quantity DRevoluteFriction Double

type DDamping = 'Dim 'Zero 'Pos1 'Neg1 'Zero 'Zero 'Zero 'Zero
type Damping = U.Quantity DDamping Double

type DRevoluteDamping = 'Dim 'Pos2 'Pos1 'Neg2 'Zero 'Zero 'Zero 'Zero
type RevoluteDamping = U.Quantity DRevoluteDamping Double

type Multiplier = Double
type Offset = Double
type ColorChannel = Double
type Scale = V3 Double
type UrdfName = String
type Filename = String

type Position = V3 Length

type Axis = Position

mm :: U.Unit U.NonMetric U.DLength Double
mm = U.milli U.metre

zAxis :: Axis
zAxis = V3 (0 *~ mm) (0 *~ mm) (1000 *~ mm)

yAxis :: Axis
yAxis = V3 (0 *~ mm) (1000 *~ mm) (0 *~ mm)

xAxis :: Axis
xAxis = V3 (1000 *~ mm) (0 *~ mm) (0 *~ mm)

toQuat :: Axis -> Angle -> Quaternion Double
toQuat (V3 x y z) angle = axisAngle caxis cangle
  where
    caxis  = V3 (x /~ mm) (y /~ mm) (z /~ mm)
    cangle = angle /~ U.radian

noOffset :: Pose
noOffset =
    Pose (V3 (0 *~ mm) (0 *~ mm) (0 *~ mm)) $ toQuat zAxis (0 *~ U.radian)

data Pose = Pose {
        pos :: Position,
        ori :: Quaternion Double
    } deriving Show


quatToRPY :: Quaternion Double -> V3 Double
quatToRPY (Quaternion w (V3 x y z)) = V3 roll pitch yaw
  where
    clamp :: Double -> Double -> Double -> Double
    clamp min val max | min > val = min
                      | max < val = max
                      | otherwise = val
    t0    = 2 * (w * x + y * z)
    t1    = 1 - 2 * (x * x + y * y)
    roll  = atan2 t0 t1
    t2    = clamp (-1) (2 * (w * y - z * x)) 1
    pitch = asin t2
    t3    = 2 * (w * z + x * y)
    t4    = 1 - 2 * (y * y + z * z)
    yaw   = atan2 t3 t4
