#! /usr/bin/env python

PACKAGE = 'bitbots_quintic_walk'
import roslib

roslib.load_manifest(PACKAGE)

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()
group_limits = gen.add_group("limits", type="tab")
group_node = gen.add_group("node", type="tab")

tab_stability = gen.add_group("stability", type="tab")
group_stability = tab_stability.add_group("stability_main", type="tab")
group_reset = tab_stability.add_group("phase reset", type="tab")

#       Name       Type      Level    Description     Default Min   Max
group_node.add("debug_active", bool_t, 1,
               "actviate debug")
group_node.add("engine_freq", double_t, 1,
               "Max freq of engine update rate [hz]", min=1, max=1000)
group_node.add("odom_pub_factor", int_t, 1,
               "Publish odom every [int] update of walk engine", min=1, max=1000)
group_node.add("bio_ik_time", double_t, 1,
               "Timeout time for bioIK [s]", min=0, max=0.05)

group_reset.add("phase_reset_active", bool_t, 1,
                "Activates phase resetting when foot gets ground contact")
group_reset.add("ground_min_pressure", double_t, 1,
                "Minimal pressure on flying foot to say that it has contact to the ground. Used to invoke phase reset.",
                min=0, max=10)
group_reset.add("phase_reset_phase", double_t, 1,
                "Minimal phase distance to end of step to invoke phase reset", min=0, max=10)

group_stability.add("pause_duration", double_t, 1,
                    "Time that the walking is paused when becoming unstable [s]", min=0, max=10)
group_stability.add("cop_stop_active", bool_t, 1,
                    "Activates stopping when center of pressure position gets to bad.")
group_stability.add("cop_x_threshold", double_t, 1,
                    "Threshold of CoP distance to center in X [m].", min=0, max=1)
group_stability.add("cop_y_threshold", double_t, 1,
                    "Threshold of CoP distance to center in Y [m].", min=0, max=1)
group_stability.add("imu_active", bool_t, 1,
                    "Activates the IMU to stop the walking when the robot gets unstable")
group_stability.add("imu_pitch_threshold", double_t, 1,
                    "Threshold for stopping for the robot pitch [rad]", min=0, max=1)
group_stability.add("imu_roll_threshold", double_t, 1,
                    "Threshold for stopping for the robot roll [rad]", min=0, max=1)
group_stability.add("imu_pitch_vel_threshold", double_t, 1,
                    "Threshold for stopping for the robot pitch angular velocity [rad/s]", min=0, max=10)
group_stability.add("imu_roll_vel_threshold", double_t, 1,
                    "Threshold for stopping for the robot roll angular velocity [rad/s]", min=0, max=10)

group_limits.add("max_step_x", double_t, 1,
                 "Maximal step length in X [m])", min=0, max=1)
group_limits.add("max_step_y", double_t, 1,
                 "Maximal step length in Y [m])", min=0, max=1)
group_limits.add("max_step_xy", double_t, 1,
                 "Maximal step length in X and Y combined [m])", min=0, max=1)
group_limits.add("max_step_z", double_t, 1,
                 "Maximal step turn in yaw [rad])", min=0, max=1.5)


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