

import           Data.SCAD
import           Data.Shared
import           Data.SRDF.Xml                     (writeSRDFXML)
import           Data.URDF.Xml                     (writeXML)
import           Data.Walker
import           Linear.Quaternion
import           Linear.V3
import           Numeric.Units.Dimensional.Prelude (degree, radian, second,
                                                    (*~))
import qualified Numeric.Units.Dimensional.Prelude as D

data LegPos = RL | RR | FR | FL deriving Eq;
data ConfType = Default | Folded | Expanded deriving (Show, Eq);


kokeTipPos = V3 (100 *~ mm) (0 *~ mm) (0 *~ mm)
femurOffset :: ConfType -> Pose
femurOffset conf_type =
  Pose
      ( V3 (if conf_type == Expanded then 100 *~ mm else 65 *~ mm)
           (0 *~ mm)
           (0 *~ mm)
      )
    $ toQuat zAxis (0 *~ degree)
tibiaOffset :: Pose
tibiaOffset =
  Pose (V3 (95 *~ mm) (0 *~ mm) (30 *~ mm)) $ toQuat yAxis (90 *~ degree)

legPose :: LegPos -> ConfType -> Pose
legPose pos conf_type = Pose (V3 x y (0 *~ mm))
  $ toQuat zAxis (angle *~ degree)
 where
  x_val = if conf_type == Expanded then 85 else 45
  x     = case pos of
    FR -> (x_val) *~ mm
    FL -> (x_val) *~ mm
    RR -> (-x_val) *~ mm
    RL -> (-x_val) *~ mm
  y_val = 36
  y     = case pos of
    FR -> (y_val) *~ mm
    FL -> (-y_val) *~ mm
    RR -> (y_val) *~ mm
    RL -> (-y_val) *~ mm

  angle = case pos of
    FR -> 90 - 45
    FL -> -90 + 45
    RR -> 90 + 45
    RL -> -90 - 45


leg :: LegPos -> ConfType -> Leg
leg leg_pos conf_type = Leg
  { leg_name = case leg_pos of
    RL -> "rl"
    RR -> "rr"
    FL -> "fl"
    FR -> "fr"
  , chain    = [ Link
                 { link_name    = "coxa"
                 , offset       = legPose leg_pos conf_type
                 , part         = Part
                   { model            = SCADModel
                     "coxa"
                     [ toArg "femurOffset" $ pos $ femurOffset Default
                     , toArg
                       "expand"
                       (if conf_type == Expanded then 10 *~ mm else 0 *~ mm)
                     ]
                     ["coxa.scad"]
                   , collision_models = [ SCADModel
                                            "coxa_collision"
                                            [ toArg "femurOffset"
                                              $ pos
                                              $ femurOffset Default
                                            ]
                                            ["coxa.scad"]
                                        ]
                   , part_weight      = Nothing
                   , center_of_mass   = Nothing
                   }
                 , axis         = zAxis
                 , joint_config = JointConfig
                   { min_angle     = (-3 * pi / 8) *~ radian
                   , default_angle = if conf_type == Folded
                     then case leg_pos of
                       FR -> (-45) *~ degree
                       FL -> 45 *~ degree
                       RR -> 45 *~ degree
                       RL -> (-45) *~ degree
                     else case leg_pos of
                       FR -> 0 *~ radian
                       FL -> 0 *~ radian
                       RR -> 0 *~ radian
                       RL -> 0 *~ radian
                   , max_angle     = (3 * pi / 8) *~ radian
                   , vel_lim       = 0.5 *~ (radian D./ second)
                   , effor_lim     = 100
                   , g_steps       = 8
                   }
                 }
               , Link
                 { link_name    = "femur"
                 , offset       = femurOffset conf_type
                 , part         = Part
                   { model            = SCADModel
                     "femur"
                     [ toArg "tibiaOffset" $ pos $ tibiaOffset
                     , toArg
                       "expand"
                       (if conf_type == Expanded then 10 *~ mm else 0 *~ mm)
                     ]
                     ["femur.scad"]
                   , collision_models = [ SCADModel
                                          "femur_collision_a"
                                          [toArg "tibiaOffset" $ pos $ tibiaOffset]
                                          ["femur.scad"]
                                        , SCADModel
                                          "femur_collision_b"
                                          [toArg "tibiaOffset" $ pos $ tibiaOffset]
                                          ["femur.scad"]
                                        ]
                   , part_weight      = Nothing
                   , center_of_mass   = Nothing
                   }
                 , axis         = yAxis
                 , joint_config = JointConfig
                   { min_angle     = (-3 * pi / 8) *~ radian
                   , default_angle = if conf_type == Folded
                     then (pi * 0.95) *~ radian
                     else (-pi / 12) *~ radian
                   , max_angle     = (3 * pi / 8) *~ radian
                   , vel_lim       = 0.5 *~ (radian D./ second)
                   , effor_lim     = 100
                   , g_steps       = 8
                   }
                 }
               , Link
                 { link_name    = "tibia"
                 , offset       = tibiaOffset
                 , part         = Part
                   { model            = SCADModel
                     "tibia"
                     [ toArg "tipOffset" kokeTipPos
                     , toArg
                       "expand"
                       (if conf_type == Expanded then 10 *~ mm else 0 *~ mm)
                     ]
                     ["tibia.scad"]
                   , collision_models = [ SCADModel
                                            "tibia_collision"
                                            [toArg "tipOffset" kokeTipPos]
                                            ["tibia.scad"]
                                        ]
                   , part_weight      = Nothing
                   , center_of_mass   = Nothing
                   }
                 , axis         = yAxis
                 , joint_config = JointConfig
                   { min_angle     = (-3 * pi / 8) *~ radian
                   , default_angle = if conf_type == Folded
                     then (-pi * 1.45) *~ radian
                     else (-pi / 12) *~ radian
                   , max_angle     = (3 * pi / 8) *~ radian
                   , vel_lim       = 0.5 *~ (radian D./ second)
                   , effor_lim     = 100
                   , g_steps       = 8
                   }
                 }
               ]
  , tip      = Tip {point = kokeTipPos}
  , pose     = noOffset --  is not considered during offset in schpin_ompl -> remove
  }

koke :: ConfType -> Walker
koke conf_type = Walker walker_name legs $ Body Part
  { model            = SCADModel
    "body"
    [ toArg "positions" leg_positions
    , toArg "expand"    (if conf_type == Expanded then 10 *~ mm else 0 *~ mm)
    ]
    ["body.scad"]
  , collision_models = [ SCADModel "body_collision"
                                   [toArg "positions" leg_positions]
                                   ["body.scad"]
                       ]
  , part_weight      = Nothing
  , center_of_mass   = Nothing
  }
 where
  leg_positions = map (\x -> pos $ legPose x Default) [RR, RL, FR, FL]
  walker_name   = "Tote"
  legs =
    [leg RR conf_type, leg RL conf_type, leg FR conf_type, leg FL conf_type]

main :: IO ()
main = do
  mapM_ (uncurry renderSCAD) $ getSCADFiles (koke Default)
  mapM_ (uncurry renderSCAD) $ getSCADSkeletons "default" (koke Default)
  mapM_ (uncurry renderSCAD) $ getSCADSkeletons "folded" (koke Folded)
  mapM_ (uncurry renderSCAD) $ getSCADSkeletons "expanded" (koke Expanded)
  writeXML     (getURDF (koke Default)) "src/urdf/koke.urdf"
  writeSRDFXML (getSRDF (koke Default)) "src/srdf/koke.srdf"
  renderWalkerStl (koke Default)
