#!/usr/bin/env python2
# -*- coding: utf-8 -*-
PACKAGE = "controller_evasion"

from dynamic_reconfigure.parameter_generator  import *

gen = ParameterGenerator()

gen.add("v_max", double_t, 0, "v_max",    100, 0.001,   300)
gen.add("sigmoid_v_min", double_t, 0, "sigmoid_v_min",    .8, 0.001, 1)
gen.add("sigmoid_v_max", double_t, 0, "sigmoid_v_max",    1, 0, 1)
gen.add("v_mpc", double_t, 0, "v_mpc",    1, 0.001, 4)

gen.add("t_end", double_t, 0, "t_end",    .4, 0.001,   3)
gen.add("steps", int_t, 0, "steps",    5, 1,   40)
# gen.add("solutions", int_t, 0, "solutions",    5, 1,   40)

# gen.add("wechsel_zeit", double_t, 0, "wechsel_zeit",    0.7, 0,   4)
# gen.add("wechsel_threshold", double_t, 0, "wechsel_threshold",    0.2, 0,   10)
# gen.add("wechsel_gain", double_t, 0, "wechsel_gain",    0.1, 0,   1)

# gen.add("spur_links", double_t, 0, "spur_links",    0.4, 0,   0.6)
# gen.add("stop_line_distance_offset", double_t, 0, "stop_line_distance_offset",    -0.4, -1,   1)
# gen.add("min_wait_for_hindernis", double_t, 0, "min_wait_for_hindernis",    1, 0,   10)
# gen.add("distance_to_stop_history_len", int_t, 0, "distance_to_stop_history_len",    5, 1,   10)
# gen.add("distance_to_stop_threshold", double_t, 0, "distance_to_stop_threshold",    0.4, 0,   1)

# gen.add("mpc_u_servo_start_bound", double_t, 0, "mpc_u_servo_start_bound",    0.2, 0.001,   0.5)
# gen.add("parable_support_distance", double_t, 0, "parable_support_distance",    0.2, 0.001,   0.5)

gen.add("cost_error_slice", int_t, 0, "cost_error_slice",    5, 1, 20)
gen.add("cost_error_y", double_t, 0, "cost_error_y",    100, 0, 1000)
gen.add("cost_error_x", double_t, 0, "cost_error_x",    100, 0, 1000)
gen.add("cost_error_theta", double_t, 0, "cost_error_theta",    40, 0, 1000)
gen.add("cost_error_servo_regularisation", double_t, 0, "cost_error_servo_regularisation",    40, 0, 1000)
gen.add("cost_error_servo_regularisation_velocity", double_t, 0, "cost_error_servo_regularisation_velocity",    75, 0, 400)
gen.add("cost_error_distance_end_points", double_t, 0, "cost_error_distance_end_points",    40, 0, 1000)

gen.add("reference_trajectory_offset_x", double_t, 0, "reference_trajectory_offset_x",    0, -1, 1)
gen.add("reference_trajectory_offset_y", double_t, 0, "reference_trajectory_offset_y",    0, -1, 1)

gen.add("pf_num_iterations", int_t, 0, "pf_num_iterations",    2, 0, 20)
gen.add("pf_num_particles", int_t, 0, "pf_num_particles",    7, 1, 20)

gen.add("preventive_dt_gain", double_t, 0, "preventive_dt_gain",    0.1, 0, 1)
gen.add("preventive_back_threshold", double_t, 0, "preventive_back_threshold",    0.15, 0, 2)
gen.add("preventive_history_size", int_t, 0, "preventive_history_size",    100, 1, 1000)
gen.add("preventive_back_trajectory_skip", int_t, 0, "preventive_back_trajectory_skip",    5, 1, 20)

gen.add("backwards_dead_time", double_t, 0, "backwards_dead_time",    1, 0, 5)
gen.add("backwards_speed", double_t, 0, "backwards_speed",    75, 50, 100)
gen.add("backwards_max_duration", double_t, 0, "backwards_max_duration",    2, 0, 5)
gen.add("backwards_generate_linear_trajectory", bool_t, 0, "backwards_generate_linear_trajectory",  False)
gen.add("backwards_duration_since_last_trajectory_factor", double_t, 0, "backwards_duration_since_last_trajectory_factor",    1, 0, 20)


gen.add("camera_confidence_gain", double_t, 0, "camera_confidence_gain",    0.2, 0, 1)
gen.add("camera_confidence_threshold", double_t, 0, "camera_confidence_threshold",    1, 0, 10)


gen.add("angle_out_filter_gain", double_t, 0,"angle_out_filter_gain",  0.2, 0, 1)
gen.add("angle_out_offset", double_t, 0,"angle_out_offset",  0, -1, 1)
gen.add("only_one_kurvature_deadzone", double_t, 0,"only_one_kurvature_deadzone",  0, -1, 5)
gen.add("only_one_kurvature_dt_moving_average_window_size", int_t, 0,"only_one_kurvature_dt_moving_average_window_size",  3, 1, 10)
gen.add("resample_interval", double_t, 0,"resample_interval",  0.1, 0, 1)
# gen.add("mpc_use_best_of_many", bool_t, 0, "mpc_use_best_of_many",  False)

gen.add("config_filename", str_t, 0, "config_filename",  "config.json")
gen.add("save", bool_t, 0, "save",  False)
gen.add("save_confirm", bool_t, 0, "save_confirm",  False)
gen.add("load", bool_t, 0, "load",  False)
gen.add("load_confirm", bool_t, 0, "load_confirm",  False)


# gen.add("start", bool_t, 0, "start",  False)
# gen.add("hindernis", bool_t, 0, "hindernis",  False)


# size_enum = gen.enum([ gen.const("Small",      int_t, 0, "A small constant"),
#                        gen.const("Medium",     int_t, 1, "A medium constant"),
#                        gen.const("Large",      int_t, 2, "A large constant"),
#                        gen.const("ExtraLarge", int_t, 3, "An extra large constant")],
#                      "An enum to set size")

# gen.add("size", int_t, 0, "A size parameter which is edited via an enum", 1, 0, 3, edit_method=size_enum)

exit(gen.generate(PACKAGE, "controller_evasion", "Controller"))
