#! /usr/bin/env python3

PACKAGE = 'bitbots_quintic_walk'
import roslib

roslib.load_manifest(PACKAGE)

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()
group_engine_main = gen.add_group("engine_main", type="tab")
group_engine_aditional = gen.add_group("additional", type="tab")
group_engine_exper = gen.add_group("experimental", type="tab")
group_kick = gen.add_group("kick", type="tab")


# todo evaluate to remove following
# foot_z_pause
# foot_put_down_z_offset
# foot_put_down_phase
# foot_apex_phase
# foot_put_down_roll_offset

#       Name       Type      Level    Description     Default Min   Max

# basic parameters
group_engine_main.add("freq", double_t, 1,
                      "Full walk cycle frequency (in Hz, > 0)", min=0.1, max=5)
group_engine_main.add("double_support_ratio", double_t, 1,
                      "Length of double support phase in half cycle(ratio, [0:1])", min=0, max=1)
group_engine_main.add("foot_distance", double_t, 1,
                      "Lateral distance between the feet center (in m, >= 0)", min=0, max=1)
group_engine_main.add("foot_rise", double_t, 1,
                      "Maximum flying foot height (in m, >= 0)", min=0, max=2)
group_engine_main.add("trunk_swing", double_t, 1,
                      "Trunk lateral oscillation amplitude ratio (ratio, >= 0)", min=0, max=2)
group_engine_main.add("trunk_height", double_t, 1,
                      "Height of the trunk from ground (in m, > 0)", min=0, max=1)
group_engine_main.add("trunk_pitch", double_t, 1,
                      "Trunk pitch orientation (in rad)", min=-1, max=1)
group_engine_main.add("trunk_pitch_p_coef_forward", double_t, 1,
                      "Trunk pitch orientation proportional to forward/backward step (in rad/m)", min=0, max=20)
group_engine_main.add("trunk_phase", double_t, 1,
                      "Phase offset of trunk oscillation (half cycle phase, [-1:1])", min=-1, max=1)

group_engine_aditional.add("foot_z_pause", double_t, 1,
                           "Pause of Z movement on highest point (single support cycle ratio, [0,1])", min=0, max=1)
group_engine_aditional.add("foot_put_down_z_offset", double_t, 1,
                           "Let the foot's downward trajectory end above the ground this is helpful if the support leg bends, (in m, >= 0))",
                           min=0, max=0.1)
group_engine_aditional.add("foot_put_down_phase", double_t, 1,
                           "Phase time for moving the foot from Z offset to ground (phase between apex and single support end [0:1])",
                           min=0, max=1)
group_engine_aditional.add("foot_apex_phase", double_t, 1,
                           "Phase of flying foot apex(single support cycle phase, [0:1])", min=0, max=1)
group_engine_aditional.add("foot_overshoot_ratio", double_t, 1,
                           "Foot X/Y overshoot in ratio of step length(ratio, >= 0)", min=0, max=1)
group_engine_aditional.add("foot_overshoot_phase", double_t, 1,
                           "Foot X/Y overshoot phase (single support cycle phase, [foot_apex_phase:1]", min=0, max=1)
group_engine_aditional.add("trunk_x_offset", double_t, 1,
                           "Trunk forward offset (in m)", min=-0.2, max=0.2)
group_engine_aditional.add("trunk_y_offset", double_t, 1,
                           "Trunk lateral offset (in m)", min=-0.2, max=0.2)
group_engine_aditional.add("trunk_pause", double_t, 1,
                           "Trunk swing pause length in phase at apex (half cycle ratio, [0:1])", min=0, max=1)
group_engine_aditional.add("trunk_x_offset_p_coef_forward", double_t, 1,
                           "Trunk forward offset proportional to forward step (in 1)", min=0, max=1)
group_engine_aditional.add("trunk_x_offset_p_coef_turn", double_t, 1,
                           "Trunk forward offset proportional to rotation step (in m/rad)", min=0, max=1)
group_engine_aditional.add("trunk_pitch_p_coef_turn", double_t, 1,
                           "Trunk pitch orientation proportional to rotation step (in 1)", min=-20, max=20)

group_kick.add("kick_length", double_t, 1,
               "Length of kick movement [m]", min=0, max=1)
group_kick.add("kick_vel", double_t, 1,
               "vel kick [m/s]", min=0, max=100)
group_kick.add("kick_phase", double_t, 1,
               "Time of kick apex [ratio of single support phase]", min=0, max=1)

group_engine_exper.add("foot_put_down_roll_offset", double_t, 1,
                "Roll offset on flying foot at put down [rad]", min=-1, max=1)
group_engine_exper.add("first_step_swing_factor", double_t, 1,
                "Give extra swing to first step for better start", min=0, max=10)
group_engine_exper.add("first_step_trunk_phase", double_t, 1,
                       "Trunk phase for the fist step", min=-1, max=1)
group_engine_exper.add("trunk_z_movement", double_t, 1,
                       "Amount of movement in z direction for trunk (around trunk_height) [m]", min=0.0, max=0.1)


exit(gen.generate(PACKAGE, "bitbots_quintic_walk", "bitbots_quintic_walk_engine_params"))
