/*

This test assumes you are at the LOWl demo Airport

*/

#include "Waypoints.h"
#include "Navigation.h"
#include "AP_GPS_IMU.h"
#include "AP_RC.h"


AP_GPS_IMU gps;
Navigation	nav((GPS *) &gps);
AP_RC rc;

#define CH_ROLL 0
#define CH_PITCH 1
#define CH_THROTTLE 2
#define CH_RUDDER 3

#define CH3_MIN 1100
#define CH3_MAX 1900

#define REVERSE_RUDDER 1
#define REVERSE_ROLL 1
#define REVERSE_PITCH 1

int8_t did_init_home;
int16_t servo_out[4];
int16_t radio_trim[4] = {1500,1500,1100,1500};
int16_t radio_in[4];

void setup()
{
	Serial.begin(38400);
	Serial.println("Navigation test");
	Waypoints::WP	location_B = {0, 0, 74260, 472586364, 113366012};
	nav.set_next_wp(location_B);
	rc.init(radio_trim);
}

void loop()
{
	delay(20);
	gps.update();
	rc.read_pwm();
	for(int y=0; y<4; y++) { 
		//rc.set_ch_pwm(y, rc.input[y]); // send to Servos
		radio_in[y] = rc.input[y];
	}
	
	servo_out[CH_ROLL]  = ((radio_in[CH_ROLL]  - radio_trim[CH_ROLL]) * 45  * REVERSE_ROLL) / 500;
	servo_out[CH_PITCH] = ((radio_in[CH_PITCH] - radio_trim[CH_PITCH]) * 45 * REVERSE_PITCH) / 500;
	servo_out[CH_RUDDER] = ((radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 45 * REVERSE_RUDDER) / 500;
	servo_out[CH_THROTTLE] = (float)(radio_in[CH_THROTTLE] - CH3_MIN) / (float)(CH3_MAX - CH3_MIN) *100;
	servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], 0, 100);	

	output_Xplane();
	if(gps.new_data /* && gps.fix */ ){
		if(did_init_home == 0){
			did_init_home = 1;
			Waypoints::WP wp = {0, 0, gps.altitude, gps.lattitude, gps.longitude};
			nav.set_home(wp);
			Serial.println("MSG init home");
		}else{
			nav.update_gps();
		}
		//print_loc();
	}
}

void print_loc()
{
	Serial.print("MSG GPS: ");
	Serial.print(nav.location.lat, DEC);					
	Serial.print(", ");
	Serial.print(nav.location.lng, DEC);					
	Serial.print(", ");
	Serial.print(nav.location.alt, DEC);
	Serial.print("\tDistance = ");
	Serial.print(nav.distance, DEC);	
	Serial.print(" Bearing = ");
	Serial.print(nav.bearing/100, DEC);
	Serial.print(" Bearing err = ");
	Serial.print(nav.bearing_error/100, DEC);
	Serial.print(" alt err = ");
	Serial.print(nav.altitude_error/100, DEC);
	Serial.print(" Alt above home = ");
	Serial.println(nav.altitude_above_home/100, DEC);
}

void print_pwm()
{
	Serial.print("ch1 ");
	Serial.print(rc.input[CH_ROLL],DEC);
	Serial.print("\tch2: ");
	Serial.print(rc.input[CH_PITCH],DEC);
	Serial.print("\tch3 :");
	Serial.print(rc.input[CH_THROTTLE],DEC);
	Serial.print("\tch4 :");
	Serial.println(rc.input[CH_RUDDER],DEC);
}




byte buf_len = 0;
byte out_buffer[32];

void output_Xplane(void)
{
	// output real-time sensor data
	Serial.print("AAA"); 		 						// 		Message preamble
	output_int((int)(servo_out[CH_ROLL]*100));			// 	0	bytes 0,1
	output_int((int)(servo_out[CH_PITCH]*100));			// 	1	bytes 2,3
	output_int((int)(servo_out[CH_THROTTLE]));			// 	2	bytes 4,5
	output_int((int)(servo_out[CH_RUDDER]*100));		// 	3	bytes 6,7
	output_int((int)nav.distance);						// 	4	bytes 8,9
	output_int((int)nav.bearing_error);					// 	5	bytes 10,11
	output_int((int)nav.altitude_error);				// 	6	bytes 12,13
	output_int(0);										// 	7	bytes 14,15
	output_byte(1);										// 	8	bytes 16
	output_byte(1);										// 	9	bytes 17
	
	// print out the buffer and checksum
	// ---------------------------------
	print_buffer();
}

void output_int(int value)
{
	//buf_len += 2;
	out_buffer[buf_len++]	= value & 0xff;
	out_buffer[buf_len++]	= (value >> 8) & 0xff;
}
void output_byte(byte value)
{
	out_buffer[buf_len++]	= value;
}

void print_buffer()
{
	byte ck_a = 0;
	byte ck_b = 0;
	for (byte i = 0;i < buf_len; i++){
		Serial.print (out_buffer[i]);
	}
	Serial.print('\r');
	Serial.print('\n');
	buf_len = 0;
}