#include <stdio.h>
#include <pthread.h>
#include <math.h>
/* include OpenCoordinate */
#include "oc.c"

#define DEBUG

/* Lenght of string algo */
#define ALGO_STRLEN 15
/* Error codes */
#define ERR_BAD_USAGE 1 // Bad program call (bad number of arguments)

/* Number of packets to send */
#define DEFAULT_NBPKT_CALIB 20 // 20 packets when calibrating
#define DEFAULT_NBPKT_NORMAL 10 // 10 packets when requesting the position

/* Delay between two packet transmissions (in microseconds) */
#define DEFAULT_DELAY_CALIB 50000 // Calibration request
#define DEFAULT_DELAY_NORMAL 25000 // Localisation request

/* Program arguments (getopt string) */
#define OPTIONS "d:hi:l::n:p:t:V"

/* Define M_PI if is not define */
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

//Define send command
#define LENSTR 1000
#define ATREF "AT*REF="
#define LANDING ",290717696"
#define TAKEOFF ",290718208"
//Define socket
//#define IP "172.20.105.4"
#define IP "192.168.1.1"
#define PORT 5556


/* Function headers */
void parse_command_line(int argc, char **argv) ;
void parse_main_options(int argc, char **argv) ;
void check_destination_ip(void) ;
void parse_calibration_data(int argc, char **argv) ;
void check_configuration(void) ;
#ifdef DEBUG
void print_configuration(void) ;
#endif // DEBUG
void create_socket(void) ;
void make_packet(void) ;
void send_request(void) ;
void* receive_position(void*) ;
void print_usage(void) ;
void string2data(char*) ;
void traficGPS(int);
void* thread_gps(void*) ;
void* thread_send(void*) ;
void* thread_control(void*) ;
void print_error(char*) ;
void print_version(void) ;
void drone_handler_sigint(const int);
typedef struct result result;
typedef struct gps gps;
/* Struct */

struct result
{
  char algo[ALGO_STRLEN];
  float x;
  float y;
  float z;
  char *check;
  float err;
};

struct gps
{
  float time;
  char state;
  double latitude;
  char orilat;
  double longitude;
  char orilon;
  float speed;
  float cap;
  int date;
  float decmag;
};



/* Prototype drone */

void drone_err(int);
//Position
int read_position(char*);
//Socket
int drone_socket_create(void);
void* drone_socket_watchdog(void*);
int drone_socket_close(void);
//Commands drone
int cmd_drone_take_off(void); //Add socket
int cmd_drone_landing(void); //Add socket
int cmd_drone_move(float, float);
int cmd_drone_init_direction(void); //Add socket
//Algo
void calcul_trajectory(float*, float*);
float calcul_speed(float);
int check_destination(struct point, struct point);