/*
 www.ArduCopter.com - www.DIYDrones.com
 Copyright (c) 2010.  All rights reserved.
 An Open Source Arduino based multicopter.
 
 File     : Debug.pde
 Version  : v1.0, Aug 27, 2010
 Author(s): ArduCopter Team
             Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
             Jani Hirvinen, Ken McEwans, Roberto Navoni,          
             Sandro Benigno, Chris Anderson
 
 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.
 
 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 GNU General Public License for more details.
 
 You should have received a copy of the GNU General Public License
 along with this program. If not, see <http://www.gnu.org/licenses/>.

* ************************************************************** *
ChangeLog:


* ************************************************************** *
TODO:


* ************************************************************** */


#if DEBUG_SUBSYSTEM == 1
void debug_subsystem()
{
	Serial.println("Begin Debug: Radio Subsystem ");
	while(1){
		delay(20);
		// Filters radio input - adjust filters in the radio.pde file
		// ----------------------------------------------------------
		read_radio();
		Serial.print("Radio in ch1: ");
		Serial.print(radio_in[CH_ROLL], DEC);
		Serial.print("\tch2: ");
		Serial.print(radio_in[CH_PITCH], DEC);
		Serial.print("\tch3: ");
		Serial.print(radio_in[CH_THROTTLE], DEC);
		Serial.print("\tch4: ");
		Serial.print(radio_in[CH_RUDDER], DEC);
		Serial.print("\tch5: ");
		Serial.print(radio_in[4], DEC);
		Serial.print("\tch6: ");
		Serial.print(radio_in[5], DEC);
		Serial.print("\tch7: ");
		Serial.print(radio_in[6], DEC);
		Serial.print("\tch8: ");
		Serial.println(radio_in[7], DEC);
	}
}
#endif

#if DEBUG_SUBSYSTEM == 2
void debug_subsystem()
{
	Serial.println("Begin Debug: Servo Subsystem ");
	Serial.print("Reverse ROLL - CH1: ");
	Serial.println(reverse_roll, DEC);
	Serial.print("Reverse PITCH - CH2: ");
	Serial.println(reverse_pitch, DEC);
	Serial.print("Reverse THROTTLE - CH3: ");
	Serial.println(REVERSE_THROTTLE, DEC);
	Serial.print("Reverse RUDDER - CH4: ");
	Serial.println(reverse_rudder, DEC);

	// read the radio to set trims
	// ---------------------------
	trim_radio();

	radio_max[0]				 = 	CH1_MAX;
	radio_max[1]				 = 	CH2_MAX;
	radio_max[2]				 = 	CH3_MAX;
	radio_max[3]				 = 	CH4_MAX;
	radio_max[4]				 = 	CH5_MAX;
	radio_max[5]				 = 	CH6_MAX;
	radio_max[6]				 = 	CH7_MAX;
	radio_max[7]				 = 	CH8_MAX;
	radio_min[0]				 = 	CH1_MIN;
	radio_min[1]				 = 	CH2_MIN;
	radio_min[2]				 = 	CH3_MIN;
	radio_min[3]				 = 	CH4_MIN;
	radio_min[4]				 = 	CH5_MIN;
	radio_min[5]				 = 	CH6_MIN;
	radio_min[6]				 = 	CH7_MIN;
	radio_min[7]				 = 	CH8_MIN;

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

		servo_out[CH_ROLL]	= ((radio_in[CH_ROLL]	- radio_trim[CH_ROLL])	* 45.0f	* reverse_roll) / 500;
		servo_out[CH_PITCH] = ((radio_in[CH_PITCH] - radio_trim[CH_PITCH]) * 45.0f	* reverse_roll) / 500;
		servo_out[CH_RUDDER] = ((radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 45.0f	* reverse_rudder) / 500;

		// write out the servo PWM values
		// ------------------------------
		set_servos_4();
		
		
		for(int y = 4; y < 8; y++) { 
			radio_out[y] = constrain(radio_in[y], 	radio_min[y], 	radio_max[y]);	
			APM_RC.OutputCh(y, radio_out[y]); // send to Servos
		}

		/*
		Serial.print("Servo_out ch1: ");
		Serial.print(servo_out[CH_ROLL], DEC);
		Serial.print("\tch2: ");
		Serial.print(servo_out[CH_PITCH], DEC);
		Serial.print("\tch3: ");
		Serial.print(servo_out[CH_THROTTLE], DEC);
		Serial.print("\tch4: ");
		Serial.print(servo_out[CH_RUDDER], DEC);
		*/
		///*
		Serial.print("ch1: ");
		Serial.print(radio_out[CH_ROLL], DEC);
		Serial.print("\tch2: ");
		Serial.print(radio_out[CH_PITCH], DEC);
		Serial.print("\tch3: ");
		Serial.print(radio_out[CH_THROTTLE], DEC);
		Serial.print("\tch4: ");
		Serial.print(radio_out[CH_RUDDER], DEC);
		Serial.print("\tch5: ");
		Serial.print(radio_out[4], DEC);
		Serial.print("\tch6: ");
		Serial.print(radio_out[5], DEC);
		Serial.print("\tch7: ");
		Serial.print(radio_out[6], DEC);
		Serial.print("\tch8: ");
		Serial.println(radio_out[7], DEC);
		
		//*/
	}
}
#endif


#if DEBUG_SUBSYSTEM == 3
void debug_subsystem()
{
	Serial.println("Begin Debug: Analog Sensor Subsystem ");
	
	Serial.print("AirSpeed sensor enabled: ");
	Serial.println(AIRSPEED_SENSOR, DEC);
		
	Serial.print("Enable Battery: ");
	Serial.println(BATTERY_EVENT, DEC);
	zero_airspeed();
	
	Serial.print("Air pressure offset:");
	Serial.println(airpressure_offset, DEC);

	while(1){
		delay(20);
		read_z_sensor();
		read_XY_sensors();
		read_battery();
		
		Serial.print("Analog IN:");
		Serial.print("  0:");
		Serial.print(analog0, DEC);
		Serial.print(", 1: ");
		Serial.print(analog1, DEC);
		Serial.print(", 2: ");
		Serial.print(analog2, DEC);
		Serial.print(", 3: ");
		Serial.print(airpressure_raw, DEC);
		
		Serial.print("\t\tSensors:");
		Serial.print("  airSpeed:");
		Serial.print(airspeed, DEC);
		Serial.print("m \tbattV:");
		Serial.print(battery_voltage, DEC);
		Serial.println("volts ");
	}
}
#endif

#if DEBUG_SUBSYSTEM == 4
void debug_subsystem()
{
	Serial.println("Begin Debug: GPS Subsystem ");
	
	while(1){
		delay(333);

		// Blink GPS LED if we don't have a fix
		// ------------------------------------
		//update_GPS_light();
		
		GPS.Read();
		
		if (GPS.NewData){
			Serial.print("Lat:");
			Serial.print(GPS.Lattitude, DEC);
			Serial.print(" Lon:");
			Serial.print(GPS.Longitude, DEC);
			Serial.print(" Alt:");
			Serial.print(GPS.Altitude / 100, DEC);
			Serial.print("m, gs: ");
			Serial.print(GPS.Ground_Speed / 100, DEC);
			Serial.print(" COG:");
			Serial.print(GPS.Ground_Course / 1000l);
			Serial.print(" SAT:");
			Serial.print(GPS.NumSats, DEC);
			Serial.print(" FIX:");
			Serial.print(GPS.Fix, DEC);
			Serial.print(" TIM:");
			Serial.print(GPS.Time);
			Serial.println();
		}
	}
}
#endif

#if DEBUG_SUBSYSTEM == 5
void debug_subsystem()
{
	Serial.println("Begin Debug: GPS Subsystem, RAW OUTPUT");
	
	while(1){
		if(Serial1.available() > 0)	// Ok, let me see, the buffer is empty?
		{
			
			delay(60);	// wait for it all
			while(Serial1.available() > 0){
				byte incoming = Serial1.read();
				//Serial.print(incoming,DEC);
				Serial.print(incoming, HEX); // will output Hex values
				Serial.print(",");
			}
			Serial.println(" ");
		}

	}
}
#endif

#if DEBUG_SUBSYSTEM == 6
void debug_subsystem()
{
	Serial.println("Begin Debug: IMU Subsystem ");
	startup_IMU_ground();
	
	while(1){
		delay(20);
		read_AHRS();
		
		// We are using the IMU
		// ---------------------
		Serial.print("  roll:");
		Serial.print(roll_sensor / 100, DEC);	
		Serial.print("  pitch:");
		Serial.print(pitch_sensor / 100, DEC);
		Serial.print("  yaw:");
		Serial.println(yaw_sensor / 100, DEC);
		
	}
}
#endif

#if DEBUG_SUBSYSTEM == 7
void debug_subsystem()
{
	Serial.println("Begin Debug: Control Switch Test");
	
	while(1){
		delay(20);
		byte switchPosition = readSwitch();
		if (oldSwitchPosition != switchPosition){
			
			switch(switchPosition)
			{
				case 1: // First position
				Serial.println("Position 1");

				break;
		
				case 2: // middle position
				Serial.println("Position 2");
				break;
		
				case 3: // last position
				Serial.println("Position 3");
				break;

				case 4: // last position
				Serial.println("Position 4");
				break;

				case 5: // last position
				Serial.println("Position 5 - Software Manual");
				break;

				case 6: // last position
				Serial.println("Position 6 - Hardware MUX, Manual");
				break;

			}
	
			oldSwitchPosition = switchPosition;
		}
	}
}
#endif

#if DEBUG_SUBSYSTEM == 8
void debug_subsystem()
{
	Serial.println("Begin Debug: Control Switch Test");
	
	while(1){
		delay(20);
		update_servo_switches();
		if (mix_mode == 0) {
			Serial.print("Mix:standard ");
			Serial.print("\t reverse_roll: ");
			Serial.print(reverse_roll, DEC);
			Serial.print("\t reverse_pitch: ");
			Serial.print(reverse_pitch, DEC);
			Serial.print("\t reverse_rudder: ");
			Serial.println(reverse_rudder, DEC);
		} else {
			Serial.print("Mix:elevons ");
			Serial.print("\t reverse_elevons: ");
			Serial.print(reverse_elevons, DEC);
			Serial.print("\t reverse_ch1_elevon: ");
			Serial.print(reverse_ch1_elevon, DEC);
			Serial.print("\t reverse_ch2_elevon: ");
			Serial.println(reverse_ch2_elevon, DEC);
		}
	}
}
#endif


#if DEBUG_SUBSYSTEM == 9
void debug_subsystem()
{
	Serial.println("Begin Debug: Relay");
	pinMode(RELAY_PIN, OUTPUT);
	
	while(1){
		delay(3000);
	
		Serial.println("Relay Position A");
		PORTL |= B00000100;
		delay(3000);
	
		Serial.println("Relay Position B");
		PORTL ^= B00000100;		
	}
}
#endif

#if DEBUG_SUBSYSTEM == 10
void debug_subsystem()
{
	Serial.println("Begin Debug: Magnatometer");
	
	while(1){
		delay(3000);
	}
}
#endif

#if DEBUG_SUBSYSTEM == 11
void debug_subsystem()
{
	ground_alt = 0;
	Serial.println("Begin Debug: Absolute Airpressure");	
	while(1){
		delay(20);
		read_airpressure();
		Serial.print("Air Pressure Altitude: ");
		Serial.print(press_alt / 100, DEC);
		Serial.println("meters");
	}
}
#endif

#if DEBUG_SUBSYSTEM == 12
void debug_subsystem()
{
	ground_alt = 0;
	Serial.println("Begin Debug: Display Waypoints");	
	delay(500);

	eeprom_busy_wait();
	uint8_t options 	= eeprom_read_byte((uint8_t *) EE_CONFIG);

	eeprom_busy_wait();
	int32_t hold = eeprom_read_dword((uint32_t *) EE_ALT_HOLD_HOME);

	// save the alitude above home option
	if(options & HOLD_ALT_ABOVE_HOME){
		Serial.print("Hold this altitude over home: ");
		Serial.print(hold / 100, DEC);
		Serial.println(" meters");
	}else{
		Serial.println("Maintain current altitude ");
	}
	
	Serial.print("Number of Waypoints: ");
	Serial.println(wp_total, DEC);

	Serial.print("Hit radius for Waypoints: ");
	Serial.println(wp_radius, DEC);

	Serial.print("Loiter radius around Waypoints: ");
	Serial.println(loiter_radius, DEC);
	Serial.println(" ");
	
	for(byte i = 0; i < wp_total; i++){
		struct Location temp = get_wp_with_index(i);
		print_waypoint(&temp, i);
	}
	
	while(1){
	}

}
#endif



#if DEBUG_SUBSYSTEM == 13
void debug_subsystem()
{
	Serial.println("Begin Debug: Throttle Subsystem ");
	read_radio();
	
	uint16_t low_pwm = radio_in[CH_THROTTLE];
	uint16_t high_pwm = radio_in[CH_THROTTLE];
	
	while(1){
		delay(20);
		// Filters radio input - adjust filters in the radio.pde file
		// ----------------------------------------------------------
		read_radio();
		//update_throttle();
		set_servos_4();
		low_pwm = min(low_pwm, radio_in[CH_THROTTLE]);
		high_pwm = max(high_pwm, radio_in[CH_THROTTLE]);
		
		Serial.print("Radio_in: ");
		Serial.print(radio_in[CH_THROTTLE], DEC);
		Serial.print("\tPWM output: ");
		Serial.print(radio_out[CH_THROTTLE], DEC);
		Serial.print("\tThrottle: ");
		Serial.print(servo_out[CH_THROTTLE], DEC);
		Serial.print("\tPWM Min: ");
		Serial.print(low_pwm, DEC);
		Serial.print("\tPWM Max: ");
		Serial.println(high_pwm, DEC);
	}
}
#endif


#if DEBUG_SUBSYSTEM == 14
void debug_subsystem()
{
	Serial.println("Begin Debug: Radio Min Max ");
	uint16_t low_pwm[8];
	uint16_t high_pwm[8];
	uint8_t i;
	
	for(i = 0; i < 100; i++){
		delay(20);
		read_radio();
	}
	
	for(i = 0; i < 8; i++){
		radio_min[i] = 0;
		radio_max[i] = 2400;
	 	low_pwm[i]	= radio_in[i];
 		high_pwm[i]	= radio_in[i];
	}
	
	while(1){
		delay(100);
		// Filters radio input - adjust filters in the radio.pde file
		// ----------------------------------------------------------
		read_radio();
		for(i = 0; i < 8; i++){
			low_pwm[i] = min(low_pwm[i], radio_in[i]);
			high_pwm[i] = max(high_pwm[i], radio_in[i]);
		}

		for(i = 0; i < 8; i++){
			Serial.print("CH");
			Serial.print(i + 1, DEC);
			Serial.print(": ");
			low_pwm[i] = min(low_pwm[i], radio_in[i]);
			Serial.print(low_pwm[i], DEC);
			Serial.print("|");
			high_pwm[i] = max(high_pwm[i], radio_in[i]);
			Serial.print(high_pwm[i], DEC);
			Serial.print("   ");
		}
		Serial.println(" ");
	}
}
#endif


#if DEBUG_SUBSYSTEM == 15
void debug_subsystem()
{
	Serial.println("Begin Debug: EEPROM Dump ");
	uint16_t temp;
	for(int n = 0; n < 512; n++){
		for(int i = 0; i < 4; i++){
			temp = eeprom_read_word((uint16_t *) mem);
			mem += 2;
			Serial.print(temp, HEX);
			Serial.print("  ");
		}
		Serial.print("  ");
		for(int i = 0; i < 4; i++){
			temp = eeprom_read_word((uint16_t *) mem);
			mem += 2;
			Serial.print(temp, HEX);
			Serial.print("  ");
		}
	}
}
#endif