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           Schpin.SCAD
import           Schpin.Shared
import           System.IO.Unsafe
import           Schpin.Walker

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

kokeTipPos :: Pose
kokeTipPos = Pose (V3 (110 *~ mm) (0 *~ mm) (0 *~ mm)) neutralQuat

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)

actuatorId :: LegPos -> UrdfName -> Int
actuatorId lp jname = case lp of
  RL -> case jname of
    "alfa" -> 11
    "beta" -> 12
    "gamma" -> 13
  RR -> case jname of
    "alfa" -> 21
    "beta" -> 22
    "gamma" -> 23
  FL -> case jname of
    "alfa" -> 1
    "beta" -> 2
    "gamma" -> 3
  FR -> case jname of
    "alfa" -> 31
    "beta" -> 32
    "gamma" -> 33

actuatorName :: LegPos -> UrdfName -> String
actuatorName lp jname = "servo/" ++ (show $ actuatorId lp jname)

lewanActuator :: LegPos -> String -> Actuator
lewanActuator leg_pos key = Actuator { ac_name = actuatorName leg_pos key
                                     , ac_type = Lewan { lw_goto_p = Just 10.0
                                                       , lw_goto_i = Just 0.0
                                                       , lw_goto_d = Just 0.0
                                                       , lw_vel_p = Just 0.6
                                                       , lw_vel_i = Just 0.005
                                                       , lw_vel_d = Just 0.6
                                                  }
                                     , ac_id = actuatorId leg_pos key
                                     , ac_from = 0
                                     , ac_to = 1000
                                     }

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
  , neutral_dir = pos $ legPose leg_pos conf_type
  }
 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 = lewanActuator leg_pos "alfa" 
                     }
    , sub_link   = Link { link_name = "coxa"
                        , weight    = 320 *~ gram
                        , assembly  = Just coxa_assembly
                        }
    , sub_joint  = Just femur
    }

  coxa_assembly :: SimpleAssembly
  coxa_assembly = 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"]
                         ]
    }

  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 = lewanActuator leg_pos "beta"
                     }
    , sub_link   = Link { link_name = "femur"
                        , weight    = 50 *~ gram
                        , assembly  = Just femur_assembly
                        }
    , sub_joint  = Just tibia
    }

  femur_assembly :: SimpleAssembly
  femur_assembly = 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"]
                         ]
    }


  tibia :: SimpleJoint
  tibia = SimpleJoint $ Joint
    { joint_name = "gamma"
    , 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 = lewanActuator leg_pos "gamma"
                     }
    , sub_link   = Link { link_name = "tibia"
                        , weight    = 170 *~ gram
                        , assembly  = Just tibia_assembly
                        }
    , sub_joint  = Just tip
    }

  tibia_assembly :: SimpleAssembly
  tibia_assembly = 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"]
                         ]
    }

  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 = do
  exportPackage koke
  exportExtras koke

