#!/usr/bin/env python

PACKAGE = 'bitbots_dynamic_kick'
import roslib

roslib.load_manifest(PACKAGE)

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

group_engine = gen.add_group("Engine", type="tab")
group_engine_distances = group_engine.add_group("Distances and Positions")
group_engine.add("engine_rate", int_t, 1,
                 "How often the engine updates motor goals [Hz]",
                 100, min=1, max=500)
group_engine_distances.add("foot_rise", double_t, 2,
                           "Rise of the foot [m]",
                           0.08, min=0, max=2)
group_engine_distances.add("foot_distance", double_t, 2,
                           "How far apart the feet should be from each other. This is only relevant during the initial phase "
                           "when the foot is not yet moving towards the ball [m]",
                           0.18, min=0, max=3)
group_engine_distances.add("kick_windup_distance", double_t, 2,
                           "How far away from the ball to stop and perform the actual (fast) kick movement [m]",
                           0.2, min=0, max=2)

group_engine_timings = group_engine.add_group("Timings")
group_engine_timings.add("move_trunk_time", double_t, 3,
                         "How long it should take to move the trunk above the support foot before raising the flying foot [s]",
                         0.8, min=0)
group_engine_timings.add("raise_foot_time", double_t, 3,
                         "How long it should take to raise the flying foot from the ground [s]",
                         0.1, min=0)
group_engine_timings.add("move_to_ball_time", double_t, 3,
                         "How long it should take to move the raised foot to the windup point [s]",
                         1, min=0)
group_engine_timings.add("kick_time", double_t, 3,  # TODO replace this with dynamic calculation
                         "This will be removed in the future [s]",
                         0.1, min=0)
group_engine_timings.add("move_back_time", double_t, 3,
                         "How long it should take to move the flying foot back after kicking [s]",
                         0.2, min=0)
group_engine_timings.add("lower_foot_time", double_t, 3,
                         "How long it should take to lower the foot [s]",
                         0.8, min=0)
group_engine_timings.add("move_trunk_back_time", double_t, 3,
                         "How long it should take to move the flying foot back after kicking [s]",
                         0.3, min=0)

group_engine_decisions = group_engine.add_group("Decisions")
group_engine_decisions.add("choose_foot_corridor_width", double_t, 3,
                           "How wide the corridor should be in which advanced foot-choosing calculations take place."
                           "If kick goal is placed outside this corridor, the foot on that side will automatically be assigned [m]"
                           "the flying foot.",
                           0.4, min=0)

group_stabilizing = gen.add_group("Stabilizer", type="tab")
group_stabilizing.add("stabilizing", bool_t, 4,
                      "Whether to use automatic stabilizing or not",
                      True)

group_stabilizing_bioik = group_stabilizing.add_group("BioIk weights")
group_stabilizing_bioik.add("minimal_displacement", bool_t, 4,
                            "Try to stabilize with as little movement as possible",
                            True)
group_stabilizing_bioik.add("trunk_height", double_t, 4,
                            "Height of the trunk [m]",
                            0.4, min=0, max=0.6)
group_stabilizing_bioik.add("stabilizing_weight", double_t, 4,
                            "How important bio_ik should regard keeping the robot in a stable position [0:1]",
                            1.0, min=0, max=1)
group_stabilizing_bioik.add("flying_weight", double_t, 4,
                            "How important bio_ik should regard the flying foots position [0:1]",
                            0.85, min=0, max=1)
group_stabilizing_bioik.add("trunk_orientation_weight", double_t, 4,
                            "How important bio_ik should regard keeping the trunk orientation [0:1]",
                            0.24, min=0, max=1)
group_stabilizing_bioik.add("trunk_height_weight", double_t, 4,
                            "How important bio_ik should regard keeping the trunk height [0:1]",
                            0.3, min=0, max=1)

group_stabilizing_cop = group_stabilizing.add_group("CenterOfPressure regulation")
group_stabilizing_cop.add("use_center_of_pressure", bool_t, 4,
                          "Use the center of pressure for stabilizing",
                          True)
group_stabilizing_cop.add("stabilizing_point_x", double_t, 4,
                          "Where to stabilize over in x direction. Measured from the support foots *_sole frame [m]",
                          0.0, min=-0.5, max=0.5)
group_stabilizing_cop.add("stabilizing_point_y", double_t, 4,
                          "Where to stabilize over in y direction. Measured from the support foots *_sole frame. "
                          "Be aware that - always represents the direction towards base_footprint [m]",
                          -0.021, min=-0.5, max=0.5)
group_stabilizing_cop.add("stabilizing_p_x", double_t, 4,
                          "How strong the stabilizing point should be influenced by the center of pressure in x direction. "
                          "This is the value of a P-Controller for the COP based stabilizing",
                          0.2, min=0, max=3)
group_stabilizing_cop.add("stabilizing_p_y", double_t, 4,
                          "How strong the stabilizing point should be influenced by the center of pressure in y direction. "
                          "This is the value of a P-Controller for the COP based stabilizing",
                          0.5, min=0, max=3)
group_stabilizing_cop.add("stabilizing_i_x", double_t, 4,
                          "How strong the stabilizing point should be influenced by the integral of the center of pressure error in x direction. "
                          "This is the value of a I-Controller for the COP based stabilizing",
                          0.0, min=0.001, max=0.1)
group_stabilizing_cop.add("stabilizing_i_y", double_t, 4,
                          "How strong the stabilizing point should be influenced by the integral of the center of pressure error in y direction. "
                          "This is the value of a I-Controller for the COP based stabilizing",
                          0.0015, min=0.0001, max=0.1)
group_stabilizing_cop.add("stabilizing_d_x", double_t, 4,
                          "How strong the stabilizing point should be influenced by the difference of the center of pressure error in x direction. "
                          "This is the value of a D-Controller for the COP based stabilizing",
                          0.0, min=-20, max=20)
group_stabilizing_cop.add("stabilizing_d_y", double_t, 4,
                          "How strong the stabilizing point should be influenced by the difference of the center of pressure error in y direction. "
                          "This is the value of a D-Controller for the COP based stabilizing",
                          1.5, min=-20, max=20)

group_visualization = gen.add_group("Visualization", type="tab")
group_visualization.add("force_enable", bool_t, 9,
                        "force publishing of visualization topics event though the rosparam /debug_active is not true",
                        False)
group_visualization.add("spline_smoothnes", int_t, 8,
                        "how many points to extract from splines for visualiazation",
                        50, min=10)

exit(gen.generate(PACKAGE, 'dynamic_kick', 'DynamicKick'))
