// INCLUDES
#include "autopilot_interface_test.h"

using namespace std;
// Time

uint64_t
get_time_usec()
{
	static struct timeval _time_stamp;
	gettimeofday(&_time_stamp, NULL);
	return _time_stamp.tv_sec*1000000 + _time_stamp.tv_usec;
}

APInterface::
APInterface(Serial_Port *serial_port_)
{

	serial_port = serial_port_;

}

APInterface::
~APInterface()
{}

void APInterface::
start_read_write_interface()
{
	printf("Started Interface\n");
	int count = 0;
	while(true)
	{
		//printf("Count is %i\n", count);
		bool success;               // receive success flag
		bool received_all = false;  // receive only one message
		Time_Stamps this_timestamps;

		mavlink_message_t message_recv;
		success = serial_port->read_message(message_recv);

		switch(message_recv.msgid)
		{
			case MAVLINK_MSG_ID_HEARTBEAT:
			{	
				//printf("Count is %i\n", count);				
				//printf("Heartbeat message\n");
				mavlink_msg_heartbeat_decode(&message_recv, &(current_messages.heartbeat));

				//printf("System Type: %u\n", current_messages.heartbeat.type);
				//printf("Autopilot class: %u\n", current_messages.heartbeat.autopilot);
				//printf("Base mode: %u\n", current_messages.heartbeat.base_mode);
				//printf("Custom mode: %u\n", current_messages.heartbeat.custom_mode);
				//printf("System status %u\n", current_messages.heartbeat.system_status);
				//printf("\n");
				break;		
			}
			case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
			{
				printf("Count is %i\n", count);	
				printf("Autopilot Version\n");
				mavlink_msg_autopilot_version_decode(&message_recv, &(current_messages.apversion));
				//printf("Autopilotversion capabilities: %"PRIu64"\n", current_messages.apversion.capabilities);
				break;
				
			}
			case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
			{	printf("Global position message\n");
				printf("Count is %i\n", count);	
				mavlink_msg_position_target_global_int_decode(&message_recv, &(current_messages.position_target_global_int));
				printf("lat: %i\n", current_messages.position_target_global_int.lat_int);
				printf("lon: %i\n", current_messages.position_target_global_int.lon_int);
				printf("alt: %f\n", current_messages.position_target_global_int.alt);
				printf("vx: %f\n", current_messages.position_target_global_int.vx);
				printf("vy: %f\n", current_messages.position_target_global_int.vy);
				printf("vz: %f\n", current_messages.position_target_global_int.vz);
				printf("\n");
				break;
			}
			case MAVLINK_MSG_ID_ATTITUDE_TARGET:
			{	printf("Attitude target message\n");
				
				break;
			}
			case MAVLINK_MSG_ID_SYS_STATUS:
			{	//printf("System status message\n");
				break;
			}

		}
	
		if (count > 10000){

		float q[4] = {1.0, 0, 0, 0};

		mavlink_set_attitude_target_t packet;
		mavlink_message_t message_send;

		packet.time_boot_ms = (uint64_t)(get_time_usec()/1000);
		packet.target_system = message_recv.sysid;
		packet.target_component = message_recv.compid;
		packet.type_mask = 0b11111100;
		packet.q[0] = 1.0;
		packet.q[1] = 0.0;
		packet.q[2] = 0.0;
		packet.q[3] = 0.0;

		packet.body_roll_rate = 0.01;
		packet.body_pitch_rate = 0.01;
		packet.body_yaw_rate = 0.01;
		packet.thrust = 0.5;

		mavlink_msg_set_attitude_target_encode(message_recv.sysid, message_recv.compid, &message_send, &packet);

		/*
		mavlink_set_position_target_global_int_t com;
		com.time_boot_ms = (uint32_t)(get_time_usec()/1000);
		com.target_system = message_recv.sysid;
		com.target_component = message_recv.compid;
		com.coordinate_frame = 5;
		com.type_mask = 0b1111111111000000;
		com.lat_int = 30000000;
		com.lon_int = 200000;
		com.alt = 900;
		com.vx = 23.0f;
		com.vy = 0.1f;
		com.vz = 5.0f;


		// Encode
		
		mavlink_msg_set_position_target_global_int_encode(message_recv.sysid, message_recv.compid, &message_send, &com);
		*/
		int len = serial_port->write_message(message_send);
		//printf("Sending data\n");


		
		}
		usleep(1000);
		count++;
	}
}