import           Language.OpenSCAD             as LS
import           Linear.Quaternion
import           Linear.V3
import           Numeric.Units.Dimensional.Prelude
                                                ( (*~)
                                                , degree
                                                , gram
                                                , meter
                                                , radian
                                                , second
                                                )
import qualified Numeric.Units.Dimensional.Prelude
                                               as D
import           Package
import           Parser                         ( extractParts )
import           SCAD
import           Shared
import           System.IO.Unsafe
import           Walker

data LegPos = RL | RR | FR | FL deriving Eq;

kokeTipPos :: Pose
kokeTipPos =
    Pose (V3 (110 *~ mm) (0 *~ mm) (0 *~ mm)) (toQuat zAxis (0 *~ degree))

femurOffset :: ConfType -> Pose
femurOffset conf_type =
    Pose
            (V3 (if conf_type == Expanded then 100 *~ mm else 65 *~ mm)
                (0 *~ mm)
                (0 *~ mm)
            )
        $ toQuat yAxis (180 *~ degree)
tibiaOffset :: Pose
tibiaOffset =
    Pose (V3 (95 *~ mm) (0 *~ mm) (30 *~ mm)) $ toQuat yAxis (270 *~ 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 60
    x     = case pos of
        FR -> x_val *~ mm
        FL -> x_val *~ mm
        RR -> (-x_val) *~ mm
        RL -> (-x_val) *~ mm
    y_val = if conf_type == Expanded then 85 else 52
    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 -> 0
        FL -> 0
        RR -> 180
        RL -> 180

baseAxis :: LegPos -> Axis
baseAxis pos = case pos of
    FR -> V3 (0 *~ mm) (0 *~ mm) ((-1000) *~ mm)
    FL -> zAxis
    RR -> zAxis
    RL -> V3 (0 *~ mm) (0 *~ mm) ((-1000) *~ mm)

actuatorName :: LegPos -> UrdfName -> String
actuatorName lp jname = "servo/" ++ (show id)
  where
    id :: Int
    id = case lp of
        RL -> case jname of
            "alfa" -> 11
            "beta" -> 12
            "gama" -> 13
        RR -> case jname of
            "alfa" -> 21
            "beta" -> 22
            "gama" -> 23
        FL -> case jname of
            "alfa" -> 1
            "beta" -> 2
            "gama" -> 3
        FR -> case jname of
            "alfa" -> 21
            "beta" -> 22
            "gama" -> 23


leg :: LegPos -> ConfType -> SimpleLeg
leg leg_pos conf_type = Leg
    { leg_name   = case leg_pos of
                       RL -> "rl"
                       RR -> "rr"
                       FL -> "fl"
                       FR -> "fr"
    , root_joint = coxa
    }
  where
    coxa :: SimpleJoint
    coxa = SimpleJoint $ Joint
        { joint_name = "alfa"
        , offset     = legPose leg_pos conf_type
        , jtype      = RevoluteJoint
                           { axis          = baseAxis leg_pos
                           , min_angle     = 0 *~ radian
                           , default_angle = case conf_type of
                                                 Folded -> 0 *~ degree
                                                 _      -> 45 *~ degree
                           , max_angle     = 90 *~ degree
                           , vel_lim       = 0.5 *~ (radian D./ second)
                           , effor_lim     = 100
                           , g_steps       = 16
                           , actuator_name = actuatorName leg_pos "alfa"
                           }
        , sub_link   = Link
            { link_name = "coxa"
            , weight    = 320 *~ gram
            , assembly  = Just $ Assembly
                { amodel           = 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"]
                    ]
                }
            }
        , sub_joint  = Just femur
        }

    femur :: SimpleJoint
    femur = SimpleJoint $ Joint
        { joint_name = "beta"
        , offset     = femurOffset conf_type
        , jtype      = RevoluteJoint
                           { axis = V3 (0 *~ meter) ((-1) *~ meter) (0 *~ meter)
                           , min_angle     = 0 *~ radian
                           , default_angle = case conf_type of
                                                 Folded -> 6 *~ degree
                                                 _      -> 180 *~ degree
                           , max_angle     = 240 *~ degree
                           , vel_lim       = 0.5 *~ (radian D./ second)
                           , effor_lim     = 100
                           , g_steps       = 16
                           , actuator_name = actuatorName leg_pos "beta"
                           }
        , sub_link   = Link
            { link_name = "femur"
            , weight    = 50 *~ gram
            , assembly  = Just $ Assembly
                              { amodel           = 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"]
                                                   ]
                              }
            }
        , sub_joint  = Just tibia
        }

    tibia :: SimpleJoint
    tibia = SimpleJoint $ Joint
        { joint_name = "gama"
        , offset     = tibiaOffset
        , jtype      = RevoluteJoint
                           { axis          = yAxis
                           , min_angle     = 0 *~ degree
                           , default_angle = case conf_type of
                                                 Folded -> 276 *~ degree
                                                 _      -> 180 *~ degree
                           , max_angle     = 240 *~ degree
                           , vel_lim       = 0.5 *~ (radian D./ second)
                           , effor_lim     = 100
                           , g_steps       = 16
                           , actuator_name = actuatorName leg_pos "gama"
                           }
        , sub_link   = Link
            { link_name = "tibia"
            , weight    = 170 *~ gram
            , assembly  = Just $ Assembly
                { amodel           = SCADModel
                    "tibia"
                    [ toArg "tipOffset" (pos kokeTipPos)
                    , toArg
                        "expand"
                        (if conf_type == Expanded then 10 *~ mm else 0 *~ mm)
                    ]
                    ["tibia.scad"]
                , collision_models = [ SCADModel
                                           "tibia_collision"
                                           [toArg "tipOffset" (pos kokeTipPos)]
                                           ["tibia.scad"]
                                     ]
                }
            }
        , sub_joint  = Just tip
        }

    tip :: SimpleJoint
    tip = SimpleJoint $ Joint
        { joint_name = "tipj"
        , offset     = kokeTipPos
        , jtype      = FixedJoint
        , sub_link   = Link { link_name = "tipl"
                            , weight    = 0 *~ gram
                            , assembly  = Nothing
                            }
        , sub_joint  = Nothing
        }

koke :: ConfType -> SimpleWalker
koke conf_type = Walker
    { walker_name = "koke"
    , legs        = legs
    , body        = Body
        { body_name   = "base_link"
        , body_weight = 1000 *~ gram
        , body_ass    = Assembly
            { amodel           = 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"]
                ]
            }
        }
    }
  where
    leg_positions = (\x -> pos $ legPose x Default) <$> [RR, RL, FR, FL]
    legs          = (`leg` conf_type) <$> [RR, RL, FR, FL]

main :: IO ()
main = exportPackage koke

