#include <ardrone_tool/Navdata/ardrone_navdata_client.h>

#include <Navdata/navdata.h>
#include <Control/control.h>
#include <stdio.h>
#include <stdlib.h>

#define CTRL_STATES_STRING
#include "control_states.h"

/* Initialization local variables before event loop  */
inline C_RESULT demo_navdata_client_init( void* data )
{
  return C_OK;
}

/* Receving navdata during the event loop */
inline C_RESULT demo_navdata_client_process( const navdata_unpacked_t* const navdata )
{
	const navdata_demo_t*nd = &navdata->navdata_demo;

	printf("===================== Navdata for flight demonstrations =====================\n");
	printf("Control state : %i                  / Battery level : %i mV\n", nd->ctrl_state, nd->vbat_flying_percentage);
	printf("Altitude      : %i\n",nd->altitude);
	printf("Orientation   : [Theta] %4.3f  [Phi] %4.3f  [Psi] %4.3f\n", nd->theta, nd->phi, nd->psi);
	printf("Speed         : [vX] %4.3f  [vY] %4.3f  [vZ] %4.3f\n",nd->vx,nd->vy,nd->vz);
	printf("============================= Controller states =============================\n");
	printf("Start : %i                          / Emergency : %i\n", controller_state.start, controller_state.emergency);
	printf("[Gaz] %4.3f\n", controller_state.gaz);
	printf("[Theta] %4.3f  [Phi] %4.3f  [Yaw] %4.3f  [magnet-Psi] %4.3f\n", controller_state.theta, controller_state.phi, controller_state.yaw, controller_state.magneto_psi);
	printf("Mode : [hover] %i  [yaw] %i  [absolute] %i  [psi-accuracy] %4.3f\n", controller_state.hover_mode, controller_state.yaw_mode, controller_state.absolute_mode, controller_state.magneto_psi_accuracy);

  return C_OK;
}

/* Relinquish the local resources after the event loop exit */
inline C_RESULT demo_navdata_client_release( void )
{
  return C_OK;
}

/* Registering to navdata client */
BEGIN_NAVDATA_HANDLER_TABLE
  NAVDATA_HANDLER_TABLE_ENTRY(demo_navdata_client_init, demo_navdata_client_process, demo_navdata_client_release, NULL)
END_NAVDATA_HANDLER_TABLE

