#! /usr/bin/env python

PACKAGE = 'bitbots_throw'
import roslib

roslib.load_manifest(PACKAGE)

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()
group_node = gen.add_group("throw_node_and_engine_values", type="tab")
group_robot_data = gen.add_group("robot_and_world_data", type="tab")
debug_data = gen.add_group("debug_data", type="tab")

# Throw Node and Engine Values
#       Name       Type      Level    Description     Default Min   Max
group_node.add("engine_frequency", double_t, 1, "Frequency of the engine update rate [in hz, > 0]", 400.0, min=0.1, max=1000.0)
group_node.add("odom_pub_factor", int_t, 1, "Publish odom every [int] update of the engine", 1, min=1, max=1000)
group_node.add("bio_ik_time", double_t, 1, "Timeout time for bioIK [s]", 0.01, min=0, max=0.05)
group_node.add("arm_stall_torque_usage", double_t, 1, "The recommended fraction of the stall torque the robot arm motor should use", 0.2, min=0.01, max=1.0)
group_node.add("use_default_start_value", bool_t, 1, "Option to always use the default value instead of getting the current pose values of the robot", False)

# Robot Data
#       Name       Type      Level    Description     Default Min   Max
group_robot_data.add("gravity", double_t, 1, "Gravity value, used for calculations", 9.81, min=0.01, max=50.0)
group_robot_data.add("pi", double_t, 1, "Pi value, used for calculations", 3.14159265, min=0.01, max=50.0)
group_robot_data.add("head_height", double_t, 1, "The height of the robots head, measure from the shoulders", 0.2, min=0.1, max=2.0)
group_robot_data.add("trunk_height", double_t, 1, "The height of the robots head, measure from the shoulders", 0.2585, min=0.1, max=2.0)
group_robot_data.add("leg_length", double_t, 1, "The height of the robots head, measure from the shoulders", 0.42, min=0.1, max=2.0)
group_robot_data.add("leg_distance", double_t, 1, "The default distance between one leg and the center of the robot on the y axis", 0.07, min=0.01, max=2.0)
group_robot_data.add("arm_length", double_t, 1, "The length of the arms of the robot", 0.343, min=0.1, max=2.0)
group_robot_data.add("arm_distance", double_t, 1, "The default distance between one arm and the center of the robot on the y axis", 0.118, min=0.01, max=2.0)
group_robot_data.add("arm_max_stall_torque", double_t, 1, "The maximal stall torque the robot arm motor has (in Nm, >= 0)", 7.3, min=0.1, max=100.0)
group_robot_data.add("ball_radius", double_t, 1, "The radius of the ball that should be thrown", 0.07, min=0.01, max=2.0)
group_robot_data.add("ball_weight", double_t, 1, "The weight of the ball that should be thrown", 0.08, min=0.001, max=2.0)
group_robot_data.add("squat_safety_distance", double_t, 1, "The radius of the ball that should be thrown", 0.18, min=0.01, max=2.0)
group_robot_data.add("move_arms_away_from_ball", double_t, 1, "The distance the arms move away from the ball to release it", 0.1, min=0.01, max=2.0)

# Debug Values
#       Name       Type      Level    Description     Default Min   Max
debug_data.add("debug_active", bool_t, 1, "activate debug", False)
debug_data.add("debug_data_path", str_t, 1, "path for the file for storage of the debug data will be save", "~/throw_data/")
debug_data.add("debug_data_file_name", str_t, 1, "name for the file for storage of the debug data", "throw_data.csv")
debug_data.add("print_debug_to_console", bool_t, 1, "prints the debug additionally to the ros console", False)
debug_data.add("visualize_movement", bool_t, 1, "activate the visualization of the limbs points as lines", False)
debug_data.add("visualization_smoothness", int_t, 1, "Frequency of the point update rate for the visualization [in hz, > 0]", 40, min=0, max=100)
debug_data.add("visualize_left_arm_arrows", bool_t, 1, "activate the visualization of the left arm points as arrows", False)
debug_data.add("visualize_right_arm_arrows", bool_t, 1, "activate the visualization of the right arm points as arrows", False)
debug_data.add("visualize_left_foot_arrows", bool_t, 1, "activate the visualization of the left foot points as arrows", False)
debug_data.add("visualize_right_foot_arrows", bool_t, 1, "activate the visualization of the right foot points as arrows", False)
debug_data.add("visualize_arrows_use_gradient", bool_t, 1, "Use Gradient colors for the visualization of the arrows", False)
debug_data.add("visualize_only_points_arrows", bool_t, 1, "Only visualize the points as arrows. Ovverides visualize_arrows_smoothness.", True)
debug_data.add("visualize_arrows_smoothness", int_t, 1, "Frequency of the point update rate for the visualization [in hz, > 0]", 40, min=0, max=100)


exit(gen.generate(PACKAGE, "bitbots_throw", "throw_engine_params"))
