#!/usr/bin/env python3

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]",
                 min=1, max=500)
group_engine_distances.add("foot_rise", double_t, 2,
                           "Rise of the foot [m]",
                           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]",
                           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]",
                           min=0, max=2)
group_engine_distances.add("trunk_height", double_t, 4,
                           "Height of the trunk [m]",
                           min=0, max=0.6)
group_engine_distances.add("trunk_roll", double_t, 4,
                           "Roll of the trunk, positive means toward the support foot [rad]",
                           min=-1, max=1)
group_engine_distances.add("trunk_pitch", double_t, 4,
                           "Pitch of the trunk, positive means toward the front [rad]",
                           min=-1, max=1)
group_engine_distances.add("trunk_yaw", double_t, 4,
                           "Yaw of the trunk, positive means turning toward the kicking foot [rad]",
                           min=-1, max=1)

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]",
                         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]",
                         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]",
                         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]",
                         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]",
                         min=0)
group_engine_timings.add("lower_foot_time", double_t, 3,
                         "How long it should take to lower the foot [s]",
                         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]",
                         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.",
                           min=0)

group_stabilizing = gen.add_group("Stabilizer", type="tab")

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")
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]",
                          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]",
                          min=-0.5, max=0.5)

group_visualization = gen.add_group("Visualization", type="tab")
group_visualization.add("spline_smoothness", int_t, 8,
                        "how many points to extract from splines for visualization",
                        min=1, max=200)

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