#!/usr/bin/env python

PACKAGE='talon_controllers'

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()
#       Name                               Type      Reconfiguration-level  Description  											Default  Min  Max
gen.add("pid_config",                      int_t,    0,                     "Choose between different PID config sets",   	0,       0,   3)
gen.add("p0",                              double_t, 0,                     "Proportional constant 0",          					0,       0,   500)
gen.add("i0",                              double_t, 0,                     "Integral constant 0",          						0,       0,   500)
gen.add("d0",                              double_t, 0,                     "Derivative constant 0",          						0,       0,   500)
gen.add("f0",                              double_t, 0,                     "Feed forward constant 0",          					0,       0,   100)
gen.add("izone0",                          double_t, 0,                     "Limits integral error to certain range 0",          	0,       0,  100000)
gen.add("allowable_closed_loop_error0",    double_t, 0,                     "Max difference between setpoint and current value 0",  0,       0,   10000)
gen.add("max_integral_accumulator0",       double_t, 0,                     "Max integral accumulator 0",          											0,       0,   100000)
gen.add("closed_loop_peak_output0",        double_t, 0,                     "Peak output for PID slot 0",          											1,       0,   1)
gen.add("closed_loop_period0",             int_t,    0,                     "PID loop period for slot 0",          										20,       0,   1000)
                                           
gen.add("p1",                              double_t, 0,                     "Proportional constant 1",          					0,       0,   500)
gen.add("i1",                              double_t, 0,                     "Integral constant 1",          						0,       0,   500)
gen.add("d1",                              double_t, 0,                     "Derivative constant 1",          						0,       0,   500)
gen.add("f1",                              double_t, 0,                     "Feed forward constant 1",          					0,       0,   100)
gen.add("izone1",                          double_t, 0,                     "Limits integral error to certain range 1",         	0,       0,  100000)
gen.add("allowable_closed_loop_error1",    double_t, 0,                     "Max difference between setpoint and current value 1",  0,       0,   10000)
gen.add("max_integral_accumulator1",       double_t, 0,                     "Max integral accumulator 1",          											0,       0,   100000)
gen.add("closed_loop_peak_output1",        double_t, 0,                     "Peak output for PID slot 1",          											1,       0,   1)
gen.add("closed_loop_period1",             int_t,    0,                     "PID loop period for slot 1",          										20,       0,   1000)

gen.add("p2",                              double_t, 0,                     "Proportional constant 2",          					0,       0,   500)
gen.add("i2",                              double_t, 0,                     "Integral constant 2",          						0,       0,   500)
gen.add("d2",                              double_t, 0,                     "Derivative constant 2",          						0,       0,   500)
gen.add("f2",                              double_t, 0,                     "Feed forward constant 2",          					0,       0,   100)
gen.add("izone2",                          double_t, 0,                     "Limits integral error to certain range 2",         	0,       0,  100000)
gen.add("allowable_closed_loop_error2",    double_t, 0,                     "Max difference between setpoint and current value 2",  0,       0,   10000)
gen.add("max_integral_accumulator2",       double_t, 0,                     "Max integral accumulator 2",          											0,       0,   100000)
gen.add("closed_loop_peak_output2",        double_t, 0,                     "Peak output for PID slot 2",          											1,       0,   1)
gen.add("closed_loop_period2",             int_t,    0,                     "PID loop period for slot 2",          										20,       0,   1000)

gen.add("p3",                              double_t, 0,                     "Proportional constant 3",          					0,       0,   500)
gen.add("i3",                              double_t, 0,                     "Integral constant 3",          						0,       0,   500)
gen.add("d3",                              double_t, 0,                     "Derivative constant 3",          						0,       0,   500)
gen.add("f3",                              double_t, 0,                     "Feed forward constant 3",          					0,       0,   100)
gen.add("izone3",                          double_t, 0,                     "Limits integral error to certain range 3",         	0,       0,  100000)
gen.add("allowable_closed_loop_error3",    double_t, 0,                     "Max difference between setpoint and current value 3",  0,       0,   10000)
gen.add("max_integral_accumulator3",       double_t, 0,                     "Max integral accumulator 3",          											0,       0,   100000)
gen.add("closed_loop_peak_output3",        double_t, 0,                     "Peak output for PID slot 3",          											1,       0,   1)
gen.add("closed_loop_period3",             int_t,    0,                     "PID loop period for slot 3",          										20,       0,   1000)
# demand_type = gen.enum([ gen.const("DemandType_Neutral",	                    int_t, 0,  ""),
# 								gen.const("DemandType_AuxPID", 				    int_t, 1,  ""),
# 								gen.const("DemandType_ArbitraryFeedForward",	int_t, 2,  ""),
# 						], "")
# gen.add("demand1_type",				       int_t,    0,                     "Demand1 Type Setting",		0,		  0,   2, edit_method=demand_type)
# gen.add("demand1_value",                   double_t, 0,                     "Demand1 Value Setting",    1,       -1,   1)
gen.add("aux_pid_polarity",                bool_t,   0,                     "Invert output of aux PID loop",   						False)
                                           
gen.add("invert_output",                   bool_t,   0,                     "Invert output of talon",          						False)
gen.add("sensor_phase",                    bool_t,   0,                     "Inverts value of sensor (encoder)",          			False)
                                           
gen.add("encoder_ticks_per_rotation",      int_t,    0,                     "Dependent on encoder type",        					4096,      0,   10000)
feedback_type_enum = gen.enum([ gen.const("Feedback_Uninitialized",	    int_t, 0, ""),
								gen.const("QuadEncoder", 				int_t, 1, ""),
								gen.const("IntegratedSensor",			int_t, 2, ""),
								gen.const("Analog",						int_t, 3, ""),
								gen.const("Tachometer",					int_t, 4, ""),
								gen.const("PulseWidthEncodedPosition",	int_t, 5, ""),
								gen.const("SensorSum",					int_t, 6, ""),
								gen.const("SensorDifference",			int_t, 7, ""),
								gen.const("RemoteSensor0",				int_t, 8, ""),
								gen.const("RemoteSensor1",				int_t, 9, ""),
								gen.const("None",						int_t, 10, ""),
								gen.const("SoftwareEmulatedSensor",		int_t, 11, ""),
								gen.const("CTRE_Magencoder_Absolute",	int_t, 4, ""),
								gen.const("CTRE_Magencoder_Relative",	int_t, 1, ""),
							], "")
gen.add("feedback_type",				   int_t,    0,                     "Select feedback sensor type"                   ,		0,		  0,   14, edit_method=feedback_type_enum)
gen.add("feedback_coefficient",            double_t, 0,                     "Selected feedback sensor feedback coefficient",        1,  -100000,   100000)

remote_feedback_type_enum = gen.enum([ gen.const("RemoteFeedbackDevice_SensorSum",              int_t, 0, ""),
									   gen.const("RemoteFeedbackDevice_SensorDifference",       int_t, 1, ""),
									   gen.const("RemoteFeedbackDevice_RemoteSensor0",          int_t, 2, ""),
									   gen.const("RemoteFeedbackDevice_RemoteSensor1",          int_t, 3, ""),
									   gen.const("RemoteFeedbackDevice_None",                   int_t, 4, ""),
									   gen.const("RemoteFeedbackDevice_SoftwareEmulatedSensor", int_t, 5, ""),
							], "")
gen.add("remote_feedback_type",			   int_t,    0,                     "Select remote feedback sensor type",		0,		  0,   5, edit_method=remote_feedback_type_enum)

remote_sensor_source_enum = gen.enum([ gen.const("RemoteSensorSource_Off",                     int_t,  0, ""),
									   gen.const("RemoteSensorSource_TalonSRX_SelectedSensor", int_t,  1, ""),
									   gen.const("RemoteSensorSource_Pigeon_Yaw",              int_t,  2, ""),
									   gen.const("RemoteSensorSource_Pigeon_Pitch",            int_t,  3, ""),
									   gen.const("RemoteSensorSource_Pigeon_Roll",             int_t,  4, ""),
									   gen.const("RemoteSensorSource_CANifier_Quadrature",     int_t,  5, ""),
									   gen.const("RemoteSensorSource_CANifier_PWMInput0",      int_t,  6, ""),
									   gen.const("RemoteSensorSource_CANifier_PWMInput1",      int_t,  7, ""),
									   gen.const("RemoteSensorSource_CANifier_PWMInput2",      int_t,  8, ""),
									   gen.const("RemoteSensorSource_CANifier_PWMInput3",      int_t,  9, ""),
									   gen.const("RemoteSensorSource_GadgeteerPigeon_Yaw",     int_t, 10, ""),
									   gen.const("RemoteSensorSource_GadgeteerPigeon_Pitch",   int_t, 11, ""),
									   gen.const("RemoteSensorSource_GadgeteerPigeon_Roll",    int_t, 12, ""),
									   gen.const("RemoteSensorSource_CANCoder",                int_t, 13, ""),
							],"")
gen.add("remote_feedback_device_id0", 	   int_t,    0,                     "Select remote feedback device id 0",		0,		  0,  63)
gen.add("remote_feedback_filter0",		   int_t,    0,                     "Select remote feedback sensor 0 type",		0,		  0,  13, edit_method=remote_sensor_source_enum)
gen.add("remote_feedback_device_id1", 	   int_t,    0,                     "Select remote feedback device id 0",		0,		  0,  63)
gen.add("remote_feedback_filter1",		   int_t,    0,                     "Select remote feedback sensor 1 type",		0,		  0,  13, edit_method=remote_sensor_source_enum)
gen.add("sensor_term_sum0",				   int_t,    0,                     "Select feedback sensor type for sum0",		1,		  0,  12, edit_method=feedback_type_enum)
gen.add("sensor_term_sum1",				   int_t,    0,                     "Select feedback sensor type for sum1",		1,		  0,  12, edit_method=feedback_type_enum)
gen.add("sensor_term_diff0",			   int_t,    0,                     "Select feedback sensor type for diff0",	1,		  0,  12, edit_method=feedback_type_enum)
gen.add("sensor_term_diff1",			   int_t,    0,                     "Select feedback sensor type for diff1",	1,		  0,  12, edit_method=feedback_type_enum)

gen.add("closed_loop_ramp",                double_t, 0,                     "Ramp rate to prevent instantaneous changes in output", 0,       0,   100)
gen.add("open_loop_ramp",                  double_t, 0,                     "Ramp rate to present intantaneous changes in output",  0,       0,   100)
gen.add("peak_output_forward",             double_t, 0,                     "Max output voltage",          							0,       0,   1)
gen.add("peak_output_reverse",             double_t, 0,                     "Max output voltage in opposite direction",          	0,       -1,   0)
gen.add("nominal_output_forward",          double_t, 0,                     "Min output voltage",          							0,       0,   1)
gen.add("nominal_output_reverse",          double_t, 0,                     "Min output voltage in opposite direction",          	0,       -1,   0)
gen.add("neutral_deadband",                double_t, 0,                     "Motor output deadband ",          						0.0401,   0,   100)

gen.add("voltage_compensation_saturation", double_t, 0,                     "Ideal voltage value used for voltage compensation", 	12,       0,   100)
gen.add("voltage_measurement_filter",      int_t,    0,                     "The number of values taken into voltage average",      32,       0,   100)
gen.add("voltage_compensation_enable",     bool_t,   0,                     "Enable or disable voltage compensation",          		False)

velocity_measurement_period_enum = gen.enum([ gen.const("Period_1Ms",    int_t, 1,  ""),
											  gen.const("Period_2Ms",	 int_t, 2,  ""),
											  gen.const("Period_5Ms",  	 int_t, 5,  ""),
											  gen.const("Period_10Ms",   int_t, 10,  ""),
											  gen.const("Period_20Ms",   int_t, 20,  ""),
											  gen.const("Period_25Ms",   int_t, 25,  ""),
											  gen.const("Period_50Ms",   int_t, 50,  ""),
											  gen.const("Period_100Ms",  int_t, 100,  ""),
							], "")
gen.add("velocity_measurement_period",     int_t,    0,                     "How often to report velocity data from sensors", 		100,	 	 0,   100, edit_method=velocity_measurement_period_enum)
gen.add("velocity_measurement_window",     int_t,    0,                     "The number of values taken into velocity average",     64,       0,   256)

gen.add("softlimit_forward_threshold",     double_t, 0,                     "Top threshold to disable talons outside certain range",0,       0,   100)
gen.add("softlimit_forward_enable",        bool_t,   0,                     "Enable or disable top soft limit threshold",          	False)
gen.add("softlimit_reverse_threshold",     double_t, 0,                     "Bottom threshold to disable talons outside range",     0,       0,   100)
gen.add("softlimit_reverse_enable",        bool_t,   0,                     "Enable or disable bottom soft limit threshold",        False)
gen.add("softlimits_override_enable",      bool_t,   0,                     "Override previous soft limit settings and disable",    False)

gen.add("current_limit_peak_amps",         int_t,    0,                     "Current amperage limit",          0,       0,   300)
gen.add("current_limit_peak_msec",         int_t,    0,                     "Configure peak current limit to threshold",          	0,       0,   20000)
gen.add("current_limit_continuous_amps",   int_t,    0,                     "Configure peak current duration to allowable",         0,       0,   200)
gen.add("current_limit_enable",            bool_t,   0,                     "Enable or disable current limiting",          			False)

gen.add("supply_current_limit",            double_t, 0,                  	"Supply Current limit (TalonFX only)",          		0,       0,   200)
gen.add("supply_current_trigger_threshold_current",  double_t,    0,        "Supply Current trigger threshold current (TalonFX only)",0,       0,   200)
gen.add("supply_current_trigger_threshold_time",  double_t,    0,        	"Supply Current trigger threshold time(TalonFX only)",	0,       0,   20000)
gen.add("supply_current_limit_enable",     bool_t  , 0,                  	"Supply Current enable (TalonFX only)",          		False)
gen.add("stator_current_limit",            double_t, 0,                  	"Supply Current limit (TalonFX only)",          		0,       0,   200)
gen.add("stator_current_trigger_threshold_current",  double_t,    0,        "Supply Current trigger threshold current (TalonFX only)",0,       0,   200)
gen.add("stator_current_trigger_threshold_time",  double_t,    0,        	"Supply Current trigger threshold time(TalonFX only)",	0,       0,   20000)
gen.add("stator_current_limit_enable",     bool_t  , 0,                  	"Supply Current enable (TalonFX only)",          		False)
gen.add("motion_cruise_velocity",          double_t, 0,                     "Max velocity of talon while trying to move to target", 0,       0,   100000)
gen.add("motion_acceleration",             double_t, 0,                     "Max acceleration of talon while trying to move",       0,       0,   100000)
gen.add("motion_s_curve_strength",         int_t,    0,                     "Amount of smoothing of motion magic curve",            0,       0,   8)
gen.add("status_1_general_period",         int_t,    0,                     "Status frame period 1",                    			10,      0,   255)
gen.add("status_2_feedback0_period",       int_t,    0,                     "Status frame period 2",                    			20,      0,   255)
gen.add("status_3_quadrature_period",      int_t,    0,                     "Status frame period 3",                    			160,     0,   255)
gen.add("status_4_aintempvbat_period",     int_t,    0,                     "Status frame period 4",                    			160,     0,   255)
gen.add("status_6_misc_period",            int_t,    0,                     "Status frame period 6",                    			0,       0,   255)
gen.add("status_7_commstatus_period",      int_t,    0,                     "Status frame period 7",                    			0,       0,   255)
gen.add("status_8_pulsewidth_period",      int_t,    0,                     "Status frame period 8",                    			160,     0,   255)
gen.add("status_9_motprofbuffer_period",   int_t,    0,                     "Status frame period 9",                    			50,      0,   255)
gen.add("status_10_motionmagic_period",    int_t,    0,                     "Status frame period 10",                    			160,     0,   255)
gen.add("status_11_uartgadgeteer_period",  int_t,    0,                     "Status frame period 11",                    			250,     0,   255)
gen.add("status_12_feedback1_period",      int_t,    0,                     "Status frame period 12",                    			250,     0,   255)
gen.add("status_13_base_pidf0_period",     int_t,    0,                     "Status frame period 13",                    			160,     0,   255)
gen.add("status_14_turn_pidf1_period",     int_t,    0,                     "Status frame period 14",                    			250,     0,   255)
gen.add("status_15_firmwareapistatus_period",    int_t,    0,               "Status frame period 15",                    			160,     0,   255)
gen.add("status_17_targets1_period",       int_t,    0,                     "Status frame period 17",                    			160,     0,   255)
gen.add("status_brushless_current_period", int_t,    0,                     "Status frame brushless current",            			160,     0,   255)
gen.add("motion_profile_trajectory_period",int_t,    0,                     "Base period added to each trajectory duration",        0,       0,   2000)
gen.add("control_3_general_period",        int_t,    0,                     "Control frame period 3",                    			10,      0,   255)
gen.add("control_4_advanced_period",       int_t,    0,                     "Control frame period 4",                    			0,       0,   255)
gen.add("control_5_feedbackoutputoverride_period",   int_t,    0,           "Control frame period 5",                    			0,       0,   255)
gen.add("control_6_motprofaddtrajpoint_period",      int_t,    0,           "Control frame period 6",                    			0,       0,   255)

neutral_mode_enum = gen.enum([ gen.const("NeutralMode_Uninitialized",int_t, 0,  ""),
								gen.const("EEPROM_Setting",   		 int_t, 1,  ""),
								gen.const("Coast", 					 int_t, 2,  ""),
								gen.const("Brake", 					 int_t, 3,  ""),
							], "")
gen.add("neutral_mode", 		   		   int_t,    0,                     "Mode of operation during neutral output",				2,		  0,   4, edit_method=neutral_mode_enum)

limit_switch_source_enum = gen.enum([ gen.const("LimitSwitchSource_Uninitialized",int_t, 0,  ""),
								gen.const("FeedbackConnector",		 int_t, 1,  ""),
								gen.const("RemoteTalonSRX",          int_t, 2,  ""),
								gen.const("RemoteCANifier",    	     int_t, 3,  ""),
								gen.const("Deactivated",       	     int_t, 4,  ""),
							], "")
limit_switch_normal_enum = gen.enum([ gen.const("LimitSwitchNormal_Uninitialized",int_t, 0,  ""),
								gen.const("NormallyOpen",     		 int_t, 1,  ""),
								gen.const("NormallyClosed",          int_t, 2,  ""),
								gen.const("Disabled",          	     int_t, 3,  ""),
							], "")

gen.add("limit_switch_local_forward_source", int_t,    0,                     "Which forward limit switch is being used",			0,		  0,   5, edit_method=limit_switch_source_enum)
gen.add("limit_switch_local_forward_normal", int_t,    0,                     "Normally open, closed, or disabled",			0,		  0,   4, edit_method=limit_switch_normal_enum)
gen.add("limit_switch_local_reverse_source", int_t,    0,                     "Which reverse limit switch is being used",			0,		  0,   5, edit_method=limit_switch_source_enum)
gen.add("limit_switch_local_reverse_normal", int_t,    0,                     "Normally open, closed, or disabled",			0,		  0,   4, edit_method=limit_switch_normal_enum)

remote_limit_switch_source_enum = gen.enum([ gen.const("RemoteLimitSwitchSource_Uninitialized",int_t, 0,  ""),
								gen.const("Remote_RemoteTalonSRX",          int_t, 1,  ""),
								gen.const("Remote_RemoteCANifier",    	     int_t, 2,  ""),
								gen.const("Remote_Deactivated",       int_t, 3,  ""),
							], "")
gen.add("limit_switch_remote_forward_source", int_t,    0,                     "Which remote forward limit switch is being used",	0,	  0,   4, edit_method=remote_limit_switch_source_enum)
gen.add("limit_switch_remote_forward_normal", int_t,    0,                     "Normally open, closed, or disabled",				0,	  0,   4, edit_method=limit_switch_normal_enum)
gen.add("limit_switch_remote_forward_id",     int_t,    0,                     "CAN ID of remote Talon or Canifier source",      0, 0, 63)
gen.add("limit_switch_remote_reverse_source", int_t,    0,                     "Which remote reverse limit switch is being used",	0,	  0,   4, edit_method=remote_limit_switch_source_enum)
gen.add("limit_switch_remote_reverse_normal", int_t,    0,                     "Normally open, closed, or disabled",				0,	  0,   4, edit_method=limit_switch_normal_enum)
gen.add("limit_switch_remote_reverse_id",     int_t,    0,                     "CAN ID of remote Talon or Canifier source",      0, 0, 63)

gen.add("conversion_factor",         		 double_t, 0,                      "Arbitrary factor for scaling encoder values - use for transmissions or similar",          1,       0,   100)


motor_commutation_enum = gen.enum([ 
		gen.const("Trapezoidal",int_t, 0,  ""),
							], "")
gen.add("motor_commutation", 				int_t,    0,                       "TalonFX motor commutation mode",	0,	  0,   1, edit_method=motor_commutation_enum)

absolute_sensor_range_enum = gen.enum([ 
		gen.const("Unsigned_0_to_360",int_t, 0,  ""),
		gen.const("Signed_PlusMinus180",int_t, 1,  ""),
							], "")
gen.add("absolute_sensor_range", 				int_t,    0,                 	"TalonFX absolute sensor range",	0,	  0,   2, edit_method=absolute_sensor_range_enum)

sensor_initialization_strategy_enum = gen.enum([ 
		gen.const("BootToZero",int_t, 0,  ""),
		gen.const("BootToAbsolutePosition",int_t, 1,  ""),
							], "")
gen.add("sensor_initialization_strategy", 		int_t,    0,                 	"TalonFX sensor initialization strategy",	0,	  0,   2, edit_method=sensor_initialization_strategy_enum)

gen.add("custom_profile_hz",         		 double_t, 0,                     "hz of profiling loop",          20,       0,   100)
gen.add("custom_profile_run",         		 bool_t, 0,                     "run the profile with below slot",          False)
gen.add("custom_profile_slot",         		 int_t, 0,                     "if profile is run this slot will be used",      0, 0, 3)
exit(gen.generate(PACKAGE, "talon_reconfigure_node", "TalonConfig"))

