#!/usr/bin/env python

PACKAGE = "controller"
from dynamic_reconfigure.parameter_generator_catkin import *
import roslib;
roslib.load_manifest(PACKAGE)
gen = ParameterGenerator()

# Controller list
controller_enum = gen.enum([
    gen.const("Dec",             int_t, 0, ""),
    gen.const("Rate",                int_t, 1, ""),
    gen.const("Attitude",           int_t, 2, ""),
], "An enum for controllers")
xbox_enum = gen.enum([
    gen.const("Gui",             int_t, 0, ""),
    gen.const("Xbox",                int_t, 1, ""),
], "An enum for xbox control")


gen.add("enable_thruster1",bool_t,0,"",True)
gen.add("enable_thruster2",bool_t,0,"",True)
gen.add("enable_thruster3",bool_t,0,"",True)
gen.add("enable_thruster4",bool_t,0,"",True)
gen.add("enable_thruster5",bool_t,0,"",True)
gen.add("enable_thruster6",bool_t,0,"",True)


# Add Controller
controllers_group = gen.add_group("controllers",type="collapse", state=False)
controllers_group.add("controller", int_t, 0, "Choose which controller to use", 0, 0, 2, edit_method=controller_enum)
controllers_group.add("xbox", int_t, 0, "Use Xbox controller or GUI", 1, 0, 1, edit_method=xbox_enum)
controllers_group.add("loop_rate", double_t, 0, "loop rate for the controller",100,1,600)
controllers_group.add("zb_scaling", double_t, 0, "moment arm for the restoring forces in the exact linearising part",0.5,-1,1)
controllers_group.add("lin_active", bool_t,0, "Enable exact linearising part", False)

rate_group=controllers_group.add_group("Rate_controller",type="collapse", state=False)
rate_group.add("kp_p",double_t, 0, "Kp for p",3.5,0,20)
rate_group.add("kp_q",double_t, 0, "Kp for q",3.5,0,20)
rate_group.add("kp_r",double_t, 0, "Kp for r",3.0,0,20)

rate_group.add("ki_p",double_t, 0, "Ki for p",2.0,0,20)
rate_group.add("ki_q",double_t, 0, "Ki for q",2.0,0,20)
rate_group.add("ki_r",double_t, 0, "Ki for r",2.0,0,20)


attitude_group=controllers_group.add_group("Attitude_controller",type="collapse", state=False)
attitude_group.add("kp_roll",double_t, 0, "Kp for roll",2.0,0,20)
attitude_group.add("kp_pitch",double_t, 0, "Kp for pitch",2.7,0,20)
attitude_group.add("kp_yaw",double_t, 0, "Kp for yaw",0.7,0,20)

attitude_group.add("ki_roll",double_t, 0, "Ki for roll",0.1,0,20)
attitude_group.add("ki_pitch",double_t, 0, "Ki for pitch",0.1,0,20)
attitude_group.add("ki_yaw",double_t, 0, "Ki for yaw",0.1,0,20)

attitude_group.add("kd_roll",double_t, 0, "Kd for roll",0.1,0,20)
attitude_group.add("kd_pitch",double_t, 0, "Kd for pitch",0.1,0,20)
attitude_group.add("kd_yaw",double_t, 0, "Kd for yaw",0.1,0,20)


depth_group=controllers_group.add_group("Depth_controller",type="collapse", state=False)
depth_group.add("enable_depth",bool_t,0,"Enable depth controller?",False)
depth_group.add("kp_depth",double_t, 0, "Kp for p",1,0,20)
depth_group.add("ki_depth",double_t, 0, "Kp for q",0.2,0,20)





# Options for reference signals
reference_enum = gen.enum([ gen.const("Step",      int_t, 1, "Smooth Step"),
                        gen.const("Sin",     int_t, 2, "Sin"),
                        gen.const("Constant",     int_t, 3, "Constant")],
                        "Change reference signals")

reference_group = gen.add_group("reference_signals", type="collapse", state=False)
reference_group.add("phi_enable",bool_t,0,"",False)
reference_group.add("theta_enable",bool_t,0,"",False)
reference_group.add("psi_enable",bool_t,0,"",False)
reference_group.add("p_enable",bool_t,0,"",False)
reference_group.add("q_enable",bool_t,0,"",False)
reference_group.add("r_enable",bool_t,0,"",False)
reference_group.add("d_enable",bool_t,0,"",False)

reference_group.add("start_reference_signals",bool_t,0,"",False)

phi_group = reference_group.add_group("phi_reference", type="collapse", state=False)
phi_group.add("phi_reference_signal", int_t , 0, "Choose reference signal", 3, 1, 3, edit_method=reference_enum)
phi_group.add("phi_start_time", double_t, 0 , "Start time of the reference signal", 3, 0, 6000)
phi_group.add("phi_final_time", double_t, 0 , "End time of the reference signal", 15, 0, 6000)
phi_step_group = phi_group.add_group("phi_step")
phi_step_group.add("phi_final_value", double_t, 0 , "Final value of the step", 1, -3.14, 3.14)
phi_step_group.add("phi_speed_scaling", double_t, 0 , "Scaling of the speed of the step", 1.5, 1, 2)
phi_sin_group = phi_group.add_group("phi_sin")
phi_sin_group.add("phi_amplitude", double_t, 0 , "Amplitude", 0.5, -3.14, 3.14)
phi_sin_group.add("phi_frequency", double_t, 0 , "Frequency", 0.5, 0, 20)
phi_group.add("phi_constant", double_t, 0 , "Constant", 0, -3.14, 3.14)

theta_group = reference_group.add_group("theta_reference", type="collapse", state=False)
theta_group.add("theta_reference_signal", int_t , 0, "Choose reference signal", 3, 1, 3, edit_method=reference_enum)
theta_group.add("theta_start_time", double_t, 0 , "Start time of the reference signal", 3, 0, 6000)
theta_group.add("theta_final_time", double_t, 0 , "End time of the reference signal", 15, 0, 6000)
theta_step_group = theta_group.add_group("theta_step")
theta_step_group.add("theta_final_value", double_t, 0 , "Final value of the step", 1, -3.14, 3.14)
theta_step_group.add("theta_speed_scaling", double_t, 0 , "Scaling of the speed of the step", 1.5, 1, 2)
theta_sin_group = theta_group.add_group("theta_sin")
theta_sin_group.add("theta_amplitude", double_t, 0 , "Amplitude", 0.5, -3.14, 3.14)
theta_sin_group.add("theta_frequency", double_t, 0 , "Frequency", 0.5, 0, 20)
theta_group.add("theta_constant", double_t, 0 , "Constant", 0, -3.14, 3.14)

psi_group = reference_group.add_group("psi_reference", type="collapse", state=False)
psi_group.add("psi_reference_signal", int_t , 0, "Choose reference signal", 3, 1, 3, edit_method=reference_enum)
psi_group.add("psi_start_time", double_t, 0 , "Start time of the reference signal", 3, 0, 6000)
psi_group.add("psi_final_time", double_t, 0 , "End time of the reference signal", 15, 0, 6000)
psi_step_group = psi_group.add_group("psi_step")
psi_step_group.add("psi_final_value", double_t, 0 , "Final value of the step", 1, -3.14, 3.14)
psi_step_group.add("psi_speed_scaling", double_t, 0 , "Scaling of the speed of the step", 1.5, 1, 2)
psi_sin_group = psi_group.add_group("psi_sin")
psi_sin_group.add("psi_amplitude", double_t, 0 , "Amplitude", 0.5, -3.14, 3.14)
psi_sin_group.add("psi_frequency", double_t, 0 , "Frequency", 0.5, 0, 20)
psi_group.add("psi_constant", double_t, 0 , "Constant", 0, -3.14, 3.14)

p_group = reference_group.add_group("p_reference", type="collapse", state=False)
p_group.add("p_reference_signal", int_t , 0, "Choose reference signal", 3, 1, 3, edit_method=reference_enum)
p_group.add("p_start_time", double_t, 0 , "Start time of the reference signal", 3, 0, 6000)
p_group.add("p_final_time", double_t, 0 , "End time of the reference signal", 15, 0, 6000)
p_step_group = p_group.add_group("p_step")
p_step_group.add("p_final_value", double_t, 0 , "Final value of the step", 1, -3.14, 3.14)
p_step_group.add("p_speed_scaling", double_t, 0 , "Scaling of the speed of the step", 1.5, 1, 2)
p_sin_group = p_group.add_group("p_sin")
p_sin_group.add("p_amplitude", double_t, 0 , "Amplitude", 0.5, -3.14, 3.14)
p_sin_group.add("p_frequency", double_t, 0 , "Frequency", 0.5, 0, 20)
p_group.add("p_constant", double_t, 0 , "Constant", 0, -3.14, 3.14)

q_group = reference_group.add_group("q_reference", type="collapse", state=False)
q_group.add("q_reference_signal", int_t , 0, "Choose reference signal", 3, 1, 3, edit_method=reference_enum)
q_group.add("q_start_time", double_t, 0 , "Start time of the reference signal", 3, 0, 6000)
q_group.add("q_final_time", double_t, 0 , "End time of the reference signal", 15, 0, 6000)
q_step_group = q_group.add_group("q_step")
q_step_group.add("q_final_value", double_t, 0 , "Final value of the step", 1, -3.14, 3.14)
q_step_group.add("q_speed_scaling", double_t, 0 , "Scaling of the speed of the step", 1.5, 1, 2)
q_sin_group = q_group.add_group("q_sin")
q_sin_group.add("q_amplitude", double_t, 0 , "Amplitude", 0.5, -3.14, 3.14)
q_sin_group.add("q_frequency", double_t, 0 , "Frequency", 0.5, 0, 20)
q_group.add("q_constant", double_t, 0 , "Constant", 0, -3.14, 3.14)

r_group = reference_group.add_group("r_reference", type="collapse", state=False)
r_group.add("r_reference_signal", int_t , 0, "Choose reference signal", 3, 1, 3, edit_method=reference_enum)
r_group.add("r_start_time", double_t, 0 , "Start time of the reference signal", 3, 0, 6000)
r_group.add("r_final_time", double_t, 0 , "End time of the reference signal", 15, 0, 6000)
r_step_group = r_group.add_group("r_step")
r_step_group.add("r_final_value", double_t, 0 , "Final value of the step", 1, -3.14, 3.14)
r_step_group.add("r_speed_scaling", double_t, 0 , "Scaling of the speed of the step", 1.5, 1, 2)
r_sin_group = r_group.add_group("r_sin")
r_sin_group.add("r_amplitude", double_t, 0 , "Amplitude", 0.5, -3.14, 3.14)
r_sin_group.add("r_frequency", double_t, 0 , "Frequency", 0.5, 0, 20)
r_group.add("r_constant", double_t, 0 , "Constant", 0, -3.14, 3.14)

d_group = reference_group.add_group("d_reference", type="collapse", state=False)
d_group.add("d_reference_signal", int_t , 0, "Choose reference signal", 3, 1, 3, edit_method=reference_enum)
d_group.add("d_start_time", double_t, 0 , "Start time of the reference signal", 3, 0, 6000)
d_group.add("d_final_time", double_t, 0 , "End time of the reference signal", 15, 0, 6000)
d_step_group = d_group.add_group("d_step")
d_step_group.add("d_final_value", double_t, 0 , "Final value of the step", 1, -3.14, 3.14)
d_step_group.add("d_speed_scaling", double_t, 0 , "Scaling of the speed of the step", 1.5, 1, 2)
d_sin_group = d_group.add_group("d_sin")
d_sin_group.add("d_amplitude", double_t, 0 , "Amplitude", 0.5, -3.14, 3.14)
d_sin_group.add("d_frequency", double_t, 0 , "Frequency", 0.5, 0, 20)
d_group.add("d_constant", double_t, 0 , "Constant", 0, -3.14, 3.14)


# Options for telegraph generator
onoff_enum = gen.enum([ gen.const("ON",      int_t, 0, "Turn on test"),
                        gen.const("OFF",     int_t, 1, "Turn off test")],
                        "An enum to set on/off for tests")

lock_enum = gen.enum([ gen.const("Un_Lock",      int_t, 0, "Un-Lock thruster 1 and 2"),
                        gen.const("Lock",     int_t, 1, "Lock thruster 1 and 2")],
                        "An enum to lock thruster 1 and 2 to same telegraph source")

telegraph_group = gen.add_group("telegraph_signal", type="collapse", state=False)
telegraph_group.add("test", int_t, 0, "Set the test signal on/off", 1, 0, 1, edit_method=onoff_enum)
telegraph_group.add("lock_t1_t2",int_t , 0, "Lock thruster 1 and 2",0, 0, 1, edit_method=lock_enum)
thruster1_group = telegraph_group.add_group("Thruster1", type="collapse", state=False)
thruster1_group.add("switch_factor1", int_t, 0 , "Chance to switch", 50, 0, 100)
thruster1_group.add("max_scale1", double_t, 0 , "Max scale", 1, 0, 1)
thruster1_group.add("min_scale1", double_t, 0 , "Min scale", 1, 0, 1)

thruster2_group = telegraph_group.add_group("Thruster2", type="collapse", state=False)
thruster2_group.add("switch_factor2", int_t, 0 , "Chance to switch", 50, 0, 100)
thruster2_group.add("max_scale2", double_t, 0 , "Max scale", 1, 0, 1)
thruster2_group.add("min_scale2", double_t, 0 , "Min scale", 1, 0, 1)

thruster3_group = telegraph_group.add_group("Thruster3", type="collapse", state=False)
thruster3_group.add("switch_factor3", int_t, 0 , "Chance to switch", 50, 0, 100)
thruster3_group.add("max_scale3", double_t, 0 , "Max scale", 1, 0, 1)
thruster3_group.add("min_scale3", double_t, 0 , "Min scale", 1, 0, 1)

thruster4_group = telegraph_group.add_group("Thruster4", type="collapse", state=False)
thruster4_group.add("switch_factor4", int_t, 0 , "Chance to switch", 50, 0, 100)
thruster4_group.add("max_scale4", double_t, 0 , "Max scale", 1, 0, 1)
thruster4_group.add("min_scale4", double_t, 0 , "Min scale", 1, 0, 1)

thruster5_group = telegraph_group.add_group("Thruster5", type="collapse", state=False)
thruster5_group.add("switch_factor5", int_t, 0 , "Chance to switch", 50, 0, 100)
thruster5_group.add("max_scale5", double_t, 0 , "Max scale", 1, 0, 1)
thruster5_group.add("min_scale5", double_t, 0 , "Min scale", 1, 0, 1)

thruster6_group = telegraph_group.add_group("Thruster6", type="collapse", state=False)
thruster6_group.add("switch_factor6", int_t, 0 , "Chance to switch", 50, 0, 100)
thruster6_group.add("max_scale6", double_t, 0 , "Max scale", 1, 0, 1)
thruster6_group.add("min_scale6", double_t, 0 , "Min scale", 1, 0, 1)

# generate the config headers
exit(gen.generate("controller", "controller", "controller"))
