#!/usr/bin/env python
PACKAGE = "dq_robotics"
import math
math.pi
from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("start_controller",    bool_t,    0, "start control", False)
gen.add("regulation_control",    bool_t,    0, "regulation_control", False)
gen.add("traj_control",    bool_t,    0, "traj_control", False)
gen.add("reset_robot",    bool_t,    0, "reset_robot", False)
gen.add("redundancy_resol_active",    bool_t,    0, "redundancy_resol_active", False)
gen.add("accCntrl_K_JntLmt",    double_t,    0, "proportional gain for K_JntLmt", 1,  0, 200)
gen.add("accCntrl_K_pP",    double_t,    0, "proportional gain for position", 250,  0, 600)
gen.add("accCntrl_K_pO",    double_t,    0, "proportional gain for orientation", 250,  0, 600)
gen.add("accCntrl_K_dP",    double_t,    0, "derivative gain for position", 150,  0, 400)
gen.add("accCntrl_K_dO",    double_t,    0, "derivative gain for orientation", 150,  0, 400)
gen.add("accCntrl_total_time",    double_t,    0, "simulation time", 10,  0, 200)
gen.add("accCntrl_frequency",    double_t,    0, "desired frequecy of controller", 1000,  0, 2000)
gen.add("accCntrl_theta_init",    double_t,    0, "theta_init for rotation task", 0,  0, math.pi)
gen.add("accCntrl_theta_final",    double_t,    0, "theta_final for rotation task", math.pi/2,  0, math.pi)
gen.add("accCntrl_traj_pitch",    double_t,    0, "pitch for rotation task", 0,  -1, 1)
gen.add("reg_x",    double_t,    0, "change in x for regulation", 0,  -1, 1)
gen.add("reg_y",    double_t,    0, "change in y for regulation", 0,  -1, 1)
gen.add("reg_z",    double_t,    0, "change in z for regulation", 0,  -1, 1)
gen.add("reg_theta_x",    double_t,    0, "change in theta_x for regulation", 0,  -1.57, 1.57)
gen.add("reg_theta_y",    double_t,    0, "change in theta_y for regulation", 0,  -1.57, 1.57)
gen.add("reg_theta_z",    double_t,    0, "change in theta_z for regulation", 0,  -1.57, 1.57)
# gen.add("double_param", double_t, 0, "A double parameter",    .5, 0,   1)
# gen.add("str_param",    str_t,    0, "A string parameter",  "Hello World")
# gen.add("bool_param",   bool_t,   0, "A Boolean parameter",  True)

size_enum = gen.enum([ gen.const("dq_controller",      int_t, 0, "dq_controller based on screw"),
                       gen.const("kdl_controller_quat",     int_t, 1, "kdl_controller based on quaternion"),
                       gen.const("kdl_controller_quatVec",      int_t, 2, "kdl_controller based on vector part of quaternion")],
                     "Controller Selection")

gen.add("controller_type", int_t, 0, "Controller Selection", 0, 0, 2, edit_method=size_enum)

exit(gen.generate(PACKAGE, "dq_robotics", "kdl_controller"))