// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

// Functions called from the setup menu
static int8_t	setup_radio				(uint8_t argc, const Menu::arg *argv);
static int8_t	setup_motors			(uint8_t argc, const Menu::arg *argv);
static int8_t	setup_accel				(uint8_t argc, const Menu::arg *argv);
static int8_t	setup_frame				(uint8_t argc, const Menu::arg *argv);
static int8_t	setup_factory			(uint8_t argc, const Menu::arg *argv);
static int8_t	setup_erase				(uint8_t argc, const Menu::arg *argv);
static int8_t	setup_flightmodes		(uint8_t argc, const Menu::arg *argv);
static int8_t	setup_batt_monitor		(uint8_t argc, const Menu::arg *argv);
static int8_t	setup_sonar				(uint8_t argc, const Menu::arg *argv);
static int8_t	setup_compass			(uint8_t argc, const Menu::arg *argv);
static int8_t	setup_mag_offset		(uint8_t argc, const Menu::arg *argv);
static int8_t	setup_declination		(uint8_t argc, const Menu::arg *argv);
static int8_t	setup_esc				(uint8_t argc, const Menu::arg *argv);
static int8_t	setup_show				(uint8_t argc, const Menu::arg *argv);

// Command/function table for the setup menu
const struct Menu::command setup_menu_commands[] PROGMEM = {
	// command			function called
	// =======        	===============
	{"erase", 			setup_erase},
	{"reset", 			setup_factory},
	{"radio",			setup_radio},
	{"frame",			setup_frame},
	{"motors",			setup_motors},
	{"esc",				setup_esc},
	{"level",			setup_accel},
	{"modes",			setup_flightmodes},
	{"battery",			setup_batt_monitor},
	{"sonar",			setup_sonar},
	{"compass",			setup_compass},
	{"offsets",			setup_mag_offset},
	{"declination",		setup_declination},
	{"show",			setup_show}
};

// Create the setup menu object.
MENU(setup_menu, "setup", setup_menu_commands);

// Called from the top-level menu to run the setup menu.
int8_t
setup_mode(uint8_t argc, const Menu::arg *argv)
{
	// Give the user some guidance
	Serial.printf_P(PSTR("Setup Mode\n\n\n"));
						 //"\n"
						 //"IMPORTANT: if you have not previously set this system up, use the\n"
						 //"'reset' command to initialize the EEPROM to sensible default values\n"
						 //"and then the 'radio' command to configure for your radio.\n"
						 //"\n"));

	if(g.rc_1.radio_min >= 1300){
		delay(1000);
		Serial.printf_P(PSTR("\n!Warning, your radio is not configured!"));
		delay(1000);
		Serial.printf_P(PSTR("\n Type 'radio' to configure now.\n\n"));
	}

	// Run the setup menu.  When the menu exits, we will return to the main menu.
	setup_menu.run();
}

// Print the current configuration.
// Called by the setup menu 'show' command.
static int8_t
setup_show(uint8_t argc, const Menu::arg *argv)
{
	// clear the area
	print_blanks(8);

	report_version();
	report_radio();
	report_frame();
	report_batt_monitor();
	report_sonar();
	report_gains();
	report_xtrack();
	report_throttle();
	report_flight_modes();
	report_imu();
	report_compass();

	AP_Var_menu_show(argc, argv);
	return(0);
}

// Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults).
// Called by the setup menu 'factoryreset' command.
static int8_t
setup_factory(uint8_t argc, const Menu::arg *argv)
{
	int c;

	Serial.printf_P(PSTR("\n'Y' + Enter to factory reset, any other key to abort:\n"));

	do {
		c = Serial.read();
	} while (-1 == c);

	if (('y' != c) && ('Y' != c))
		return(-1);

	AP_Var::erase_all();
	Serial.printf_P(PSTR("\nFACTORY RESET complete - reboot APM"));

	delay(1000);
	//default_log_bitmask();
	//default_gains();

	for (;;) {
	}
	// note, cannot actually return here
	return(0);
}

// Perform radio setup.
// Called by the setup menu 'radio' command.
static int8_t
setup_radio(uint8_t argc, const Menu::arg *argv)
{
	Serial.println("\n\nRadio Setup:");
	uint8_t i;

	for(i = 0; i < 100;i++){
		delay(20);
		read_radio();
	}

	if(g.rc_1.radio_in < 500){
		while(1){
			//Serial.printf_P(PSTR("\nNo radio; Check connectors."));
			delay(1000);
			// stop here
		}
	}

	g.rc_1.radio_min = g.rc_1.radio_in;
	g.rc_2.radio_min = g.rc_2.radio_in;
	g.rc_3.radio_min = g.rc_3.radio_in;
	g.rc_4.radio_min = g.rc_4.radio_in;
	g.rc_5.radio_min = g.rc_5.radio_in;
	g.rc_6.radio_min = g.rc_6.radio_in;
	g.rc_7.radio_min = g.rc_7.radio_in;
	g.rc_8.radio_min = g.rc_8.radio_in;

	g.rc_1.radio_max = g.rc_1.radio_in;
	g.rc_2.radio_max = g.rc_2.radio_in;
	g.rc_3.radio_max = g.rc_3.radio_in;
	g.rc_4.radio_max = g.rc_4.radio_in;
	g.rc_5.radio_max = g.rc_5.radio_in;
	g.rc_6.radio_max = g.rc_6.radio_in;
	g.rc_7.radio_max = g.rc_7.radio_in;
	g.rc_8.radio_max = g.rc_8.radio_in;

	g.rc_1.radio_trim = g.rc_1.radio_in;
	g.rc_2.radio_trim = g.rc_2.radio_in;
	g.rc_4.radio_trim = g.rc_4.radio_in;
	// 3 is not trimed
	g.rc_5.radio_trim = 1500;
	g.rc_6.radio_trim = 1500;
	g.rc_7.radio_trim = 1500;
	g.rc_8.radio_trim = 1500;


	Serial.printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: "));
	while(1){

		delay(20);
		// Filters radio input - adjust filters in the radio.pde file
		// ----------------------------------------------------------
		read_radio();

		g.rc_1.update_min_max();
		g.rc_2.update_min_max();
		g.rc_3.update_min_max();
		g.rc_4.update_min_max();
		g.rc_5.update_min_max();
		g.rc_6.update_min_max();
		g.rc_7.update_min_max();
		g.rc_8.update_min_max();

		if(Serial.available() > 0){
			delay(20);
			Serial.flush();

			g.rc_1.save_eeprom();
			g.rc_2.save_eeprom();
			g.rc_3.save_eeprom();
			g.rc_4.save_eeprom();
			g.rc_5.save_eeprom();
			g.rc_6.save_eeprom();
			g.rc_7.save_eeprom();
			g.rc_8.save_eeprom();

			print_done();
			break;
		}
	}
	report_radio();
	return(0);
}

static int8_t
setup_esc(uint8_t argc, const Menu::arg *argv)
{
	Serial.printf_P(PSTR("\nUnplug, then plug-in battery; Calibrate ESCs.\n Press Enter to cancel.\n"));


	g.esc_calibrate.set_and_save(1);

	while(1){
		delay(20);

		if(Serial.available() > 0){
			g.esc_calibrate.set_and_save(0);
			return(0);
		}
	}
}

void
init_esc()
{
	g.esc_calibrate.set_and_save(0);
	while(1){
		read_radio();
		delay(100);
		update_esc_light();
		APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
		APM_RC.OutputCh(CH_2, g.rc_3.radio_in);
		APM_RC.OutputCh(CH_3, g.rc_3.radio_in);
		APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
		APM_RC.OutputCh(CH_7, g.rc_3.radio_in);
		APM_RC.OutputCh(CH_8, g.rc_3.radio_in);

		#if FRAME_CONFIG ==	OCTA_FRAME
		APM_RC.OutputCh(CH_10,   g.rc_3.radio_in);
		APM_RC.OutputCh(CH_11,   g.rc_3.radio_in);
		#endif

	}
}

static int8_t
setup_motors(uint8_t argc, const Menu::arg *argv)
{
	while(1){
		output_motor_test();
		if(Serial.available() > 0){
			g.esc_calibrate.set_and_save(0);
			return(0);
		}
	}
}

static int8_t
setup_accel(uint8_t argc, const Menu::arg *argv)
{
	imu.init_accel();
	print_accel_offsets();
	report_imu();
	return(0);
}

static int8_t
setup_frame(uint8_t argc, const Menu::arg *argv)
{
	if (!strcmp_P(argv[1].str, PSTR("x"))) {
		g.frame_orientation.set_and_save(X_FRAME);
	} else if (!strcmp_P(argv[1].str, PSTR("p"))) {
		g.frame_orientation.set_and_save(PLUS_FRAME);
	}else{
		Serial.printf_P(PSTR("\nOptions:[x,p]\n"));
		report_frame();
		return 0;
	}

	report_frame();
	return 0;
}

static int8_t
setup_flightmodes(uint8_t argc, const Menu::arg *argv)
{
	byte switchPosition, _oldSwitchPosition, mode;

	Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes."));
	print_hit_enter();

	while(1){
		delay(20);
		read_radio();
		switchPosition = readSwitch();


		// look for control switch change
		if (_oldSwitchPosition != switchPosition){

			mode = g.flight_modes[switchPosition];
			mode = constrain(mode, 0, NUM_MODES-1);

			// update the user
			print_switch(switchPosition, mode);

			// Remember switch position
			_oldSwitchPosition = switchPosition;
		}

		// look for stick input
		if (radio_input_switch() ==  true){
			mode++;
			if(mode >= NUM_MODES)
				mode = 0;

			// save new mode
			g.flight_modes[switchPosition] = mode;

			// print new mode
			print_switch(switchPosition, mode);
		}

		// escape hatch
		if(Serial.available() > 0){
			g.flight_modes.save();
			print_done();
			report_flight_modes();
			return (0);
		}
	}
}

static int8_t
setup_declination(uint8_t argc, const Menu::arg *argv)
{
	compass.set_declination(radians(argv[1].f));
	report_compass();
}


static int8_t
setup_erase(uint8_t argc, const Menu::arg *argv)
{
	zero_eeprom();
	return 0;
}

static int8_t
setup_compass(uint8_t argc, const Menu::arg *argv)
{
	if (!strcmp_P(argv[1].str, PSTR("on"))) {
		g.compass_enabled.set_and_save(true);
		init_compass();

	} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
		g.compass_enabled.set_and_save(false);

	}else{
		Serial.printf_P(PSTR("\nOptions:[on,off]\n"));
		report_compass();
		return 0;
	}

	g.compass_enabled.save();
	report_compass();
	return 0;
}

static int8_t
setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
{
	if(argv[1].i >= 0 && argv[1].i <= 4){
		g.battery_monitoring.set_and_save(argv[1].i);

	} else {
		Serial.printf_P(PSTR("\nOptions: 0-4"));
	}

	report_batt_monitor();
	return 0;
}

static int8_t
setup_sonar(uint8_t argc, const Menu::arg *argv)
{
	if (!strcmp_P(argv[1].str, PSTR("on"))) {
		g.sonar_enabled.set_and_save(true);

	} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
		g.sonar_enabled.set_and_save(false);

	}else{
		Serial.printf_P(PSTR("\nOptions:[on, off]\n"));
		report_sonar();
		return 0;
	}

	report_sonar();
	return 0;
}

static int8_t
setup_mag_offset(uint8_t argc, const Menu::arg *argv)
{
	print_hit_enter();
	init_compass();

	float _min[3], _max[3];
	Vector3f _offsets;
	Vector3f compass_mag;

	while(1){
		static float min[3], _max[3], offset[3];
		if (millis() - fast_loopTimer > 100) {
			delta_ms_fast_loop 	= millis() - fast_loopTimer;
			fast_loopTimer		= millis();
			G_Dt 				= (float)delta_ms_fast_loop / 1000.f;


			compass.read();
			compass.calculate(0, 0);	// roll = 0, pitch = 0 for this example
			compass_mag = compass.get_offsets();

			// capture min
			_min[0] = min(_min[0], compass_mag.x);
			_min[1] = min(_min[1], compass_mag.y);
			_min[2] = min(_min[2], compass_mag.z);

			// capture max
			_max[0] = max(_max[0], compass_mag.x);
			_max[1] = max(_max[1], compass_mag.y);
			_max[2] = max(_max[2], compass_mag.z);

			// calculate offsets
			_offsets.x = -(_max[0] + _min[0]) / 2;
			_offsets.y = -(_max[1] + _min[1]) / 2;
			_offsets.z = -(_max[2] + _min[2]) / 2;

			// display all to user
			Serial.printf_P(PSTR("Heading: %u, \t (%4.4f, %4.4f, %4.4f) (%4.4f, %4.4f, %4.4f)\n"),
					(uint16_t)wrap_360(ToDeg(compass.heading)),
					compass_mag.x,
					compass_mag.y,
					compass_mag.z,
					_offsets.x,
					_offsets.y,
					_offsets.z);

			if(Serial.available() > 0){
				compass.set_offsets(_offsets);
				//compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z);
				report_compass();
				break;
			}
		}
	}
}


/***************************************************************************/
// CLI defaults
/***************************************************************************/

void default_log_bitmask()
{
	// convenience macro for testing LOG_* and setting LOGBIT_*
	#define LOGBIT(_s)	(LOG_##_s ? MASK_LOG_##_s : 0)

	g.log_bitmask =
		LOGBIT(ATTITUDE_FAST)	|
		LOGBIT(ATTITUDE_MED)	|
		LOGBIT(GPS)				|
		LOGBIT(PM)				|
		LOGBIT(CTUN)			|
		LOGBIT(NTUN)			|
		LOGBIT(MODE)			|
		LOGBIT(RAW)				|
		LOGBIT(CMD)				|
		LOGBIT(CURRENT);
	#undef LOGBIT

	g.log_bitmask.save();
}

/***************************************************************************/
// CLI reports
/***************************************************************************/

void report_batt_monitor()
{
	Serial.printf_P(PSTR("\nBatt Mointor\n"));
	print_divider();
	if(g.battery_monitoring == 0)	print_enabled(false);
	if(g.battery_monitoring == 1)	Serial.printf_P(PSTR("3 cells"));
	if(g.battery_monitoring == 2)	Serial.printf_P(PSTR("4 cells"));
	if(g.battery_monitoring == 3)	Serial.printf_P(PSTR("batt volts"));
	if(g.battery_monitoring == 4)	Serial.printf_P(PSTR("volts and cur"));
	print_blanks(2);
}

void report_wp(byte index = 255)
{
	if(index == 255){
		for(byte i = 0; i <= g.waypoint_total; i++){
			struct Location temp = get_command_with_index(i);
			print_wp(&temp, i);
		}
	}else{
		struct Location temp = get_command_with_index(index);
		print_wp(&temp, index);
	}
}

void print_wp(struct Location *cmd, byte index)
{
	Serial.printf_P(PSTR("command #: %d id:%d op:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
		(int)index,
		(int)cmd->id,
		(int)cmd->options,
		(int)cmd->p1,
		cmd->alt,
		cmd->lat,
		cmd->lng);
}

void report_gps()
{
	Serial.printf_P(PSTR("\nGPS\n"));
	print_divider();
	print_enabled(GPS_enabled);
	print_blanks(2);
}

void report_sonar()
{
	g.sonar_enabled.load();
	Serial.printf_P(PSTR("Sonar\n"));
	print_divider();
	print_enabled(g.sonar_enabled.get());
	print_blanks(2);
}

void report_version()
{
	Serial.printf_P(PSTR("FW Version %d\n"),(int)g.format_version.get());
	print_divider();
	print_blanks(2);
}

void report_frame()
{
	Serial.printf_P(PSTR("Frame\n"));
	print_divider();

#if FRAME_CONFIG == QUAD_FRAME
	Serial.printf_P(PSTR("Quad frame\n"));
#elif FRAME_CONFIG == TRI_FRAME
	Serial.printf_P(PSTR("TRI frame\n"));
#elif FRAME_CONFIG == HEXA_FRAME
	Serial.printf_P(PSTR("Hexa frame\n"));
#elif FRAME_CONFIG == Y6_FRAME
	Serial.printf_P(PSTR("Y6 frame\n"));
#elif FRAME_CONFIG == OCTA_FRAME
	Serial.printf_P(PSTR("Octa frame\n"));
#endif

	if(g.frame_orientation == X_FRAME)
		Serial.printf_P(PSTR("X mode\n"));
	else if(g.frame_orientation == PLUS_FRAME)
		Serial.printf_P(PSTR("+ mode\n"));

	print_blanks(2);
}

void report_radio()
{
	Serial.printf_P(PSTR("Radio\n"));
	print_divider();
	// radio
	print_radio_values();
	print_blanks(2);
}

void report_gains()
{
	Serial.printf_P(PSTR("Gains\n"));
	print_divider();

	// Acro
	Serial.printf_P(PSTR("Acro:\nroll:\n"));
	print_PID(&g.pid_acro_rate_roll);
	Serial.printf_P(PSTR("pitch:\n"));
	print_PID(&g.pid_acro_rate_pitch);
	Serial.printf_P(PSTR("yaw:\n"));
	print_PID(&g.pid_acro_rate_yaw);

	// Stabilize
	Serial.printf_P(PSTR("\nStabilize:\nroll:\n"));
	print_PID(&g.pid_stabilize_roll);
	Serial.printf_P(PSTR("pitch:\n"));
	print_PID(&g.pid_stabilize_pitch);
	Serial.printf_P(PSTR("yaw:\n"));
	print_PID(&g.pid_yaw);

	//Serial.printf_P(PSTR("Stab D: %4.3f\n"), (float)g.stabilize_dampener);
	//Serial.printf_P(PSTR("Yaw D: %4.3f\n\n"), (float)g.hold_yaw_dampener);

	// Nav
	Serial.printf_P(PSTR("Nav:\nlat:\n"));
	print_PID(&g.pid_nav_lat);
	Serial.printf_P(PSTR("long:\n"));
	print_PID(&g.pid_nav_lon);
	Serial.printf_P(PSTR("baro throttle:\n"));
	print_PID(&g.pid_baro_throttle);
	Serial.printf_P(PSTR("sonar throttle:\n"));
	print_PID(&g.pid_sonar_throttle);
	print_blanks(2);
}

void report_xtrack()
{
	Serial.printf_P(PSTR("XTrack\n"));
	print_divider();
	// radio
	Serial.printf_P(PSTR("XTRACK: %4.2f\n"
						 "XTRACK angle: %d\n"
						 "PITCH_MAX: %ld"),
						 (float)g.crosstrack_gain,
						 (int)g.crosstrack_entry_angle,
						 (long)g.pitch_max);
	print_blanks(2);
}

void report_throttle()
{
	Serial.printf_P(PSTR("Throttle\n"));
	print_divider();

	Serial.printf_P(PSTR("min: %d\n"
						 "max: %d\n"
						 "cruise: %d\n"
						 "failsafe_enabled: %d\n"
						 "failsafe_value: %d"),
						 (int)g.throttle_min,
						 (int)g.throttle_max,
						 (int)g.throttle_cruise,
						 (int)g.throttle_fs_enabled,
						 (int)g.throttle_fs_value);
	print_blanks(2);
}

void report_imu()
{
	Serial.printf_P(PSTR("IMU\n"));
	print_divider();

	print_gyro_offsets();
	print_accel_offsets();
	print_blanks(2);
}

void report_compass()
{
	Serial.printf_P(PSTR("Compass\n"));
	print_divider();

	print_enabled(g.compass_enabled);

	// mag declination
	Serial.printf_P(PSTR("Mag Dec: %4.4f\n"),
							degrees(compass.get_declination()));

	Vector3f offsets = compass.get_offsets();

	// mag offsets
	Serial.printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f"),
							offsets.x,
							offsets.y,
							offsets.z);
	print_blanks(2);
}

void report_flight_modes()
{
	Serial.printf_P(PSTR("Flight modes\n"));
	print_divider();

	for(int i = 0; i < 6; i++ ){
		print_switch(i, g.flight_modes[i]);
	}
	print_blanks(2);
}

/***************************************************************************/
// CLI utilities
/***************************************************************************/

void
print_PID(PID * pid)
{
	Serial.printf_P(PSTR("P: %4.2f, I:%4.2f, D:%4.2f, IMAX:%ld\n"),
						pid->kP(),
						pid->kI(),
						pid->kD(),
						(long)pid->imax());
}

void
print_radio_values()
{
	Serial.printf_P(PSTR("CH1: %d | %d\n"), (int)g.rc_1.radio_min, (int)g.rc_1.radio_max);
	Serial.printf_P(PSTR("CH2: %d | %d\n"), (int)g.rc_2.radio_min, (int)g.rc_2.radio_max);
	Serial.printf_P(PSTR("CH3: %d | %d\n"), (int)g.rc_3.radio_min, (int)g.rc_3.radio_max);
	Serial.printf_P(PSTR("CH4: %d | %d\n"), (int)g.rc_4.radio_min, (int)g.rc_4.radio_max);
	Serial.printf_P(PSTR("CH5: %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_max);
	Serial.printf_P(PSTR("CH6: %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_max);
	Serial.printf_P(PSTR("CH7: %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_max);
	//Serial.printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max);
}

void
print_switch(byte p, byte m)
{
	Serial.printf_P(PSTR("Pos %d: "),p);
	Serial.println(flight_mode_strings[m]);
}

void
print_done()
{
	Serial.printf_P(PSTR("\nSaved Settings\n\n"));
}

void
print_blanks(int num)
{
	while(num > 0){
		num--;
		Serial.println("");
	}
}

void
print_divider(void)
{
	for (int i = 0; i < 40; i++) {
		Serial.print("-");
	}
	Serial.println("");
}

// read at 50Hz
bool
radio_input_switch(void)
{
	static int8_t bouncer = 0;

	if (int16_t(g.rc_1.radio_in - g.rc_1.radio_trim) > 100) {
	    bouncer = 10;
	}
	if (int16_t(g.rc_1.radio_in - g.rc_1.radio_trim) < -100) {
	    bouncer = -10;
	}
	if (bouncer >0) {
	    bouncer --;
	}
	if (bouncer <0) {
	    bouncer ++;
	}

	if (bouncer == 1 || bouncer == -1) {
	    return bouncer;
	}else{
	    return 0;
	}
}


void zero_eeprom(void)
{
	byte b;
	Serial.printf_P(PSTR("\nErasing EEPROM\n"));
	for (int i = 0; i < EEPROM_MAX_ADDR; i++) {
		eeprom_write_byte((uint8_t *) i, b);
	}
	Serial.printf_P(PSTR("done\n"));
}

void print_enabled(boolean b)
{
	if(b)
		Serial.printf_P(PSTR("en"));
	else
		Serial.printf_P(PSTR("dis"));
	Serial.printf_P(PSTR("abled\n"));
}

void
print_accel_offsets(void)
{
	Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\n"),
						(float)imu.ax(),
						(float)imu.ay(),
						(float)imu.az());
}

void
print_gyro_offsets(void)
{
	Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"),
						(float)imu.gx(),
						(float)imu.gy(),
						(float)imu.gz());
}