/*
 * This file is part of the rtap localisation project.
 */

#include "../libowlps-client/owlps-client.h"
#include "owlps-drone.h"

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <inttypes.h>
#include <string.h>

#include <net/if.h>

#include <time.h>

#include <sys/ioctl.h>
#include <sys/types.h>
#include <fcntl.h>

#include <assert.h>

#include <signal.h>

#define SEPARATOR ','
#define EOL '$'
#define TYPE "GPS"

/* Options */

gps datagps;

struct timespec timestamp ;
struct {
  char dest_ip[INET_ADDRSTRLEN] ; // Destination IP of the packets
  uint_fast16_t dest_port ;
  char iface[IFNAMSIZ + 1] ; // Source network interface
  int_fast32_t delay ; // Time between two packet transmissions
  uint_fast16_t nb_pkt ; // Number of packets to send
  uint_fast16_t listening_port ;
  // Calibration data:
  owl_direction direction ;
  float x ;
  float y ;
  float z ;
} options = {
  "",
  LOC_REQUEST_DEFAULT_PORT,
  "",
  -1,
  0,
  0,
  0, 0, 0, 0
} ;

char *program_name = NULL ;

// TRUE if the packet is a calibration request, FALSE if it is a simple
// positioning request:
owl_bool is_calibration_request = FALSE ;

int socksendfd ; // Sending socket descriptor (send positioning packets)
int sockreceivefd ; // Receiving socket descriptor (Receive position)
int socksendlvfd ; // Sending labview socket descriptor (Send position to labview)
struct sockaddr_in server ; // Server info
uint8_t *packet = NULL ; // Packet to send
uint_fast16_t packet_size ; // Packet size

// Shared variables for OwlSIG (Geo Information System)
//double share_x, share_y, share_z;

struct point share_gps;
struct point share_gps_retro;
struct point share_gps_cons;
struct point share_gps_origin;
struct point_relatif share_relatif;
struct point_relatif share_relatif_retro;
struct point_relatif share_relatif_cons;

int state_flying=0, lock=0, i=-1, start=0, run=1;

/* Socket drone */

int sockcmdfd;
struct sockaddr_in servcmd;
char sendline[100];
char count[100];

//double share_lat_cons=-999.0, share_lon_cons=-999.00, share_alt_cons=-999.0;
//double share_x_cons=-999.0, share_y_cons=-999.00, share_z_cons=-999.0, share_t_cons=-999.0;
double share_t_cons=-999.00;

int main(int argc, char *argv[])
{
  float angle, distance, speed_pitch, distance_total;
  struct sigaction action;

  program_name = argv[0] ;
  parse_command_line(argc, argv) ;


  action.sa_flags = 0;
  sigemptyset(&action.sa_mask);
  action.sa_handler = drone_handler_sigint;
  sigaction(SIGINT, &action, NULL);

  // Reset value share variables
  memset(&share_gps,0,sizeof(share_gps));
  memset(&share_gps_cons,0,sizeof(share_gps_cons));
  memset(&share_gps_retro,0,sizeof(share_gps_retro));
  memset(&share_gps_origin,0,sizeof(share_gps_origin));
  memset(&share_relatif,0,sizeof(share_relatif));
  memset(&share_relatif_cons,0,sizeof(share_relatif_cons));
  memset(&share_relatif_retro,0,sizeof(share_relatif_retro));

  // Thread communication TCP OwlSIG
  pthread_t control ;
  pthread_create(&control, NULL, thread_control, NULL) ;

  if(!strcmp(TYPE,"WIFI"))
    {
      pthread_t send ;
      pthread_t receive ;
      create_socket() ;
      pthread_create(&send, NULL, thread_send, NULL) ;
      if (options.listening_port > 0)
        pthread_create(&receive, NULL, receive_position, NULL) ;
    }
  else if(!strcmp(TYPE,"GPS"))
    {
      pthread_t gps ;
      pthread_create(&gps, NULL, thread_gps, NULL) ;
    }
  else
    {
      printf("TYPE is not correct, change value of TYPE\n");
      return -1;
    }

  drone_socket_create() ;
  pthread_t drone_watchdog ;
  pthread_create(&drone_watchdog, NULL, drone_socket_watchdog, NULL) ;
  while(start==0) usleep(200000);
  speed_pitch=-1;

  if(read_position(TYPE)<0)
    drone_err(1);

  if(cmd_drone_take_off()<0)
    drone_err(2);

  if(cmd_drone_init_direction()<0)
    drone_err(3);

  calcul_trajectory(&angle, &distance);
  distance_total = distance;
  while(!check_destination(share_gps, share_gps_cons))
    {
      if(read_position(TYPE)<0)
        drone_err(1);

      calcul_trajectory(&angle, &distance);

      if(speed_pitch < -1) speed_pitch = -1;
      else if(speed_pitch > 1) speed_pitch = 1;

      if(speed_pitch<-(distance/distance_total))
        speed_pitch=speed_pitch;
      else
        speed_pitch=-(distance/distance_total);

      cmd_drone_move(speed_pitch, angle);
      sleep(1);
    }

  (void) close(socksendfd) ;
  (void) close(sockreceivefd) ;

  return 0 ;
}



void parse_command_line(int argc, char **argv)
{
  parse_main_options(argc, argv) ;
  if(!strcmp(TYPE,"WIFI"))
    {
      check_destination_ip() ;
      parse_calibration_data(argc, argv) ;
      check_configuration() ;


#ifdef DEBUG
      print_configuration() ;
#endif // DEBUG
    }
}


void traficGPS(int gps) {
  int offset=0, c=1, i=0;
  char buffer[4096];
  
  do 
    {
      c=read(gps, buffer, 4096);
      printf("lecture intiale %d\n",c);
    } 
  while (c>99);
  do 
    {
      c=read(gps, buffer+offset, 1);
      offset=offset+c;
    } 
  while (buffer[offset-c]!='$');
  //buffer[offset]='\0';
  //printf("phrase x%sx\n",buffer);
  // Debut de phrase GPS sans le dollar
  while (run) {
    i=0;
    offset=0;
    do 
      {
        c=read(gps, buffer+offset, 1);
        offset=offset+c;
      } 
    while (buffer[offset-c]!='$');
    //buffer[offset]='\0';
    //printf("phrase s x%sx\n",buffer);
    if (strncmp(buffer+i,"GPRMC",5)==0)
      {
        char data[15][20];
        int offset_line, length, ct_string, y;
        int z;

        ct_string = offset_line = y = z = 0 ;
        length = strlen(buffer+i) ;
        memset(data,0,200);
        while(y<=length)
          {
            if((buffer)[y+i]==EOL && z==0) z=1;
            else if((buffer)[y+i]==EOL && z==1)
              {
                length=y-1;
                break;
              }
            y++;
          }
        printf("\nTaille finale : %d\n", length);

        y=0;
        while(y<=length)
          {
            if ((buffer)[y+i]==SEPARATOR)
              {

                if((y-offset_line)==0)
                  memcpy(data[ct_string], "\0", 1);

                else
                  {
                    memcpy(data[ct_string], buffer+i+offset_line, y-offset_line);
                    data[ct_string][y-offset_line]=0;
                  }
                ct_string++;
                offset_line=y+1;
              }
            y++;
          }

        if(!(ct_string==0 || offset_line==0))
          memcpy(data[ct_string], buffer+i+offset_line, length-offset_line);
        data[ct_string][length-offset_line]=0;

        datagps.time = atof(data[1]);
        datagps.state = data[2][0];
        datagps.latitude = atof(strndup(data[3],2))+atof(strndup(data[3]+2,strlen(data[3])-2))/60.0 ;
        datagps.orilat = data[4][0];
        datagps.longitude = atof(strndup(data[5],3))+atof(strndup(data[5]+3,strlen(data[5])-3))/60.0 ;
        datagps.orilon = data[6][0];
        datagps.speed = atof(data[7])*1.852;
        datagps.cap = atof(data[8]);
        datagps.date = atoi(data[9]);
        datagps.decmag = atof(data[10]);

        if (datagps.orilat=='S') datagps.latitude=-datagps.latitude;
        if (datagps.orilon=='W') datagps.longitude=-datagps.longitude;
        share_gps_retro.lat=share_gps.lat;
        share_gps_retro.lon=share_gps.lon;
        share_gps.lat=datagps.latitude;
        share_gps.lon=datagps.longitude;
        printf("lat %s lon %s\n",data[3], data[5]);

        printf("Time : %f | State : %c | Latitude : %.10f%c | Longitude %.10f%c | Speed : %f km/h | Cap : %f° | Date : %d | DecMagn : %f maps %.10f %.10f\n",datagps.time, datagps.state, datagps.latitude, datagps.orilat, datagps.longitude, datagps.orilon, datagps.speed, datagps.cap, datagps.date, datagps.decmag, share_gps.lat, share_gps.lon);

      }
  }
}

void* thread_gps(void* NULL_value)
{
  char buffer[1024];

  int gps_fd = open("/dev/ttyPA0",O_RDONLY);

  read(gps_fd, buffer, 80);
  traficGPS(gps_fd);
  pthread_exit(NULL_value);
}

void* thread_send(void* NULL_value)
{
  while(run)
    {
      make_packet() ;
      send_request() ;
      sleep(1) ;
    }
  pthread_exit(NULL_value);
}

// TCP Server OwlSIG
void* thread_control(void* NULL_value)
{
  int sockfd, newsockfd, portno=8080;
  socklen_t clilen;
  char request[2048], response[2048], *c;
  struct sockaddr_in serv_addr, cli_addr;
  int n,i=4;
  char *cmd, *sx, *sy, *sz, *st;

  sockfd = socket(AF_INET, SOCK_STREAM, 0);
  if (sockfd < 0)
    perror("ERROR opening socket TCP");
  bzero((char *) &serv_addr, sizeof(serv_addr));
  serv_addr.sin_family = AF_INET;
  serv_addr.sin_addr.s_addr = INADDR_ANY;
  serv_addr.sin_port = htons(portno);
  if (bind(sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0)
    perror("ERROR on binding");
  listen(sockfd,1);
  clilen = sizeof(cli_addr);
  memset(request,0,1500);
  cmd=malloc(256);
  strcpy(cmd,"");
  while (strcmp(cmd,"end")!=0) {
    newsockfd = accept(sockfd, (struct sockaddr *) &cli_addr, &clilen);
    if (newsockfd < 0)
      perror("ERROR on accept");
    //       while ( strcmp(request,"end")!=0) {
    n = read(newsockfd,request,1500);
    printf("\ntaille %d\n", n);
    fflush(NULL);
    if (n < 0) perror("ERROR reading from socket");
    printf("%s", request);
    // Add command processing
    c=strtok(request,"=");
    printf("val 1 %s\n",c);
    c=strtok(NULL,"=");
    printf("val 2 %s\n",c);
    cmd=strtok(c,";");
    printf("val 3 %s\n",cmd);
    sx=strtok(NULL,";");
    sy=strtok(NULL,";");
    sz=strtok(NULL,";");
    st=strtok(NULL,";");

    strcpy(response,"HTTP/1.1 200 OK\nDate: Mon, 18 Apr 2011 19:51:52 GMT\nServer: Apache/ProXad [Aug  5 2010 16:17:11]\nAccess-Control-Allow-Origin: *\nX-Powered-By: PHP/4.4.3-dev\nConnection: close\nContent-Type: text/html\n\n");
    if (strcmp(cmd, "ReadPosition")==0)
      sprintf(response,"%sPosition;OK;%d.0;5.0;6.1\n",response,i++);
    else if (strcmp(cmd, "ReadGeoPosition")==0)
      sprintf(response,"%sGeoPosition;OK;%f;%f;%f\n",response,share_gps.lat,share_gps.lon,share_gps.alt);
    else if (strcmp(cmd, "SendGeoSetpoint")==0) {
      share_gps_cons.lat=atof(sx);
      share_gps_cons.lon=atof(sy);
      share_gps_cons.alt=atof(sz);
      share_t_cons=atof(st);
      sprintf(response,"%sGeoSetpoint;OK;%.15f;%.15f;%f;%f\n",response,share_gps_cons.lat,share_gps_cons.lon,share_gps_cons.alt,share_t_cons);
    }
    else if (strcmp(cmd, "SendSetpoint")==0) {
      share_relatif_cons.x=atof(sx);
      share_relatif_cons.y=atof(sy);
      share_relatif_cons.z=atof(sz);
      share_t_cons=atof(st);
      sprintf(response,"%sSetpoint;OK;%f;%f;%f;%f\n",response,share_relatif_cons.x,share_relatif_cons.y,share_relatif_cons.z,share_t_cons);
    }
    else if(strcmp(cmd, "SendStart")==0)
      {
        start=1;
        sprintf(response,"%sStarted;OK;",response);
      }
    else if(strcmp(cmd, "SendLanding")==0)
      {
        start=0;
        cmd_drone_landing();
        sprintf(response,"%sLanding;OK;",response);
      }
    else
      strcat(response,"Not understand\n");
    n = write(newsockfd,response,strlen(response));
    if (n < 0) perror("ERROR writing to socket");
    printf ("Write : %d %d\n", n, (int) strlen(response));
    close(newsockfd);

    //       }
  }
  close(sockfd);
  pthread_exit(NULL_value);
}

void parse_main_options(int argc, char **argv)
{
  int opt ;

  while ((opt = getopt(argc, argv, OPTIONS)) != -1)
    {
      switch (opt)
        {
        case 'd' :
          strncpy(options.dest_ip, optarg, INET_ADDRSTRLEN) ;
          break ;
        case 'h' :
          print_usage() ;
          exit(0) ;
        case 'i' :
          strncpy(options.iface, optarg, IFNAMSIZ + 1) ;
          break ;
        case 'l' :
          /* Facultative getopt options does not handle separated
           * values (like -l <port>) */
          if (optarg == 0)
            {
              /* If we are at the end of the string, or the next optind
               * is an option, we have -l without a port number */
              if (argv[optind] == NULL || argv[optind][0] == '-')
                // Take the default value:
                options.listening_port = MOBILE_DEFAULT_PORT ;
              else
                {
                  // Take the optind value:
                  options.listening_port =
                    strtoul(argv[optind], NULL, 0) ;
                  optind++ ;
                }
            }
          else // We got an option like -l<port>, it's OK
            options.listening_port = strtoul(optarg, NULL, 0) ;
          break ;
        case 'n' :
          options.nb_pkt = strtoul(optarg, NULL, 0) ;
          break ;
        case 'p' :
          options.dest_port = strtoul(optarg, NULL, 0) ;
          break ;
        case 't' :
          options.delay = strtol(optarg, NULL, 0) ;
          break ;
        case 'V' :
          print_version() ;
          exit(0) ;
        default :
          print_usage() ;
          exit(ERR_BAD_USAGE) ;
        }
    }
}



void check_destination_ip()
{
  /* Check if we got a destination IP address */
  if (options.dest_ip[0] == '\0')
    {
      fprintf(stderr, "Error! You must specify a destination IP address"
              " (-d).\n") ;
      print_usage() ;
      exit(ERR_BAD_USAGE) ;
    }
}



void parse_calibration_data(int argc, char **argv)
{
  /* Parse remaining arguments (possible calibration data) */
  if (argc - optind != 0)
    {
      if (argc - optind == 4)
        {
          is_calibration_request = TRUE ;
          options.direction = strtoul(argv[optind++], NULL, 0) ;
          options.x = strtod(argv[optind++], NULL) ;
          options.y = strtod(argv[optind++], NULL) ;
          options.z = strtod(argv[optind], NULL) ;
        }
      else // Bad number of arguments
        {
          print_usage() ;
          exit(ERR_BAD_USAGE) ;
        }
    }
}



void check_configuration()
{
  // Delay not specified (or bad delay):
  if (options.delay < 0)
    {
#ifdef DEBUG
      fprintf(stderr,
              "Warning! delay: failing back to default value.\n") ;
#endif // DEBUG
      if (is_calibration_request)
        options.delay = DEFAULT_DELAY_CALIB ;
      else
        options.delay = DEFAULT_DELAY_NORMAL ;
    }

  // Number of packet not specified (or bad number)
  if (options.nb_pkt < 1)
    {
#ifdef DEBUG
      fprintf(stderr,
              "Warning! nb_pkt: failing back to default value.\n") ;
#endif // DEBUG
      if (is_calibration_request)
        options.nb_pkt = DEFAULT_NBPKT_CALIB ;
      else
        options.nb_pkt = DEFAULT_NBPKT_NORMAL ;
    }

  // Calibration request but bad direction
  if (is_calibration_request)
    if (options.direction < OWL_DIRECTION_MIN ||
        options.direction > OWL_DIRECTION_MAX)
      {
        fprintf(stderr, "Error! « %"PRIu8" » is not a valid"
                " direction.\n", options.direction) ;
        exit(ERR_BAD_USAGE) ;
      }

  // Check port numbers
  if (options.dest_port < 1 || options.dest_port > 65535)
    {
#ifdef DEBUG
      fprintf(stderr, "Warning! Bad dest_port:"
              " failing back to default value.\n") ;
      options.dest_port = LOC_REQUEST_DEFAULT_PORT ;
#endif // DEBUG
    }
  if (options.listening_port > 65535)
    {
#ifdef DEBUG
      fprintf(stderr, "Warning! listening_port too high: ignored.\n") ;
      options.listening_port = 0 ;
#endif // DEBUG
    }

  // We want to send a calibration request AND to be located, which is
  // not allowed:
  if (is_calibration_request && options.listening_port > 0)
    {
#ifdef DEBUG
      fprintf(stderr, "Warning! You cannot wait for a server answer when"
              " you calibrate. Option -l ignored…\n") ;
#endif // DEBUG
      options.listening_port = 0 ;
    }
}



#ifdef DEBUG
void print_configuration()
{
  fprintf(stderr, "Options:\n"
          "\tDestination IP: %s\n"
          "\tDestination port: %"PRIuFAST16"\n"
          "\tInterface: %s\n"
          "\tDelay: %"PRIuFAST32"\n"
          "\tNumber of packets: %"PRIuFAST16"\n"
          "\tListening port: %"PRIuFAST16"\n"
          "\tDirection: %"PRIu8"\n"
          "\tX: %f\n"
          "\tY: %f\n"
          "\tZ: %f\n"
          ,
          options.dest_ip,
          options.dest_port,
          options.iface,
          options.delay,
          options.nb_pkt,
          options.listening_port,
          options.direction,
          options.x,
          options.y,
          options.z
          ) ;
}
#endif // DEBUG



void create_socket()
{
  socksendfd =
    owlclient_create_trx_socket(options.dest_ip, options.dest_port,
                                &server, options.iface) ;
  sockreceivefd =
    owl_create_udp_listening_socket(options.listening_port) ;
}



/* Creates the packet to send. */
void make_packet()
{
  uint_fast16_t offset ; // Index used to create the packet
  owl_timestamp request_time ;
  char request_time_str[OWL_TIMESTAMP_STR_LEN] ;

  // Get the current time and copy it as a string before to switch it to
  // network endianess:
  owl_timestamp_now(&request_time) ;
  owl_timestamp_to_string(request_time_str, request_time) ;
  request_time = owl_hton_timestamp(request_time) ;

  if (is_calibration_request) // Calibration packet
    {
      printf("Preparing calibration request packet…\n") ;

      offset = 0 ;
      packet_size =
        sizeof(uint8_t) * 2 + sizeof(owl_timestamp) + sizeof(float) * 3 ;
      packet = malloc(packet_size) ;

      memset(&packet[offset], OWL_REQUEST_CALIBRATION, 1) ; // Packet type
      ++offset ;
      memcpy(&packet[offset], &request_time, sizeof(request_time)) ;
      offset += sizeof(request_time) ;
      packet[offset++] = options.direction ; // Direction
#ifdef DEBUG
      printf("Direction = %d, X = %f, Y = %f, Z = %f\n",
             packet[offset - 1], options.x, options.y, options.z) ;
#endif // DEBUG
      options.x = owl_htonf(options.x) ;
      options.y = owl_htonf(options.y) ;
      options.z = owl_htonf(options.z) ;
      memcpy(&packet[offset], &options.x, sizeof(float)) ;
      offset += sizeof(float) ;
      memcpy(&packet[offset], &options.y, sizeof(float)) ;
      offset += sizeof(float) ;
      memcpy(&packet[offset], &options.z, sizeof(float)) ;
    }

  else // Standard packet
    {
      printf("Preparing request packet…\n") ;
      packet_size = sizeof(uint8_t) + sizeof(owl_timestamp) ;
      packet = malloc(packet_size) ;
      memset(&packet[0], OWL_REQUEST_NORMAL, 1) ; // Packet type
      memcpy(&packet[1], &request_time, sizeof(request_time)) ;
    }

  printf("Packet timestamp: %s\n", request_time_str) ;
}



void send_request()
{
  owlclient_send_request(socksendfd, &server, packet, packet_size,
                         options.nb_pkt, options.delay) ;
}



void* receive_position(void* NULL_value)
{
  // Position of the mobile as computed by the infrastructure:
  char timestampXYZ[128] ;
  char *data_cpy ;
  while(run)
    {
      recvfrom(sockreceivefd, &timestampXYZ, 128, 0, NULL, NULL) ;
      data_cpy = strdup(timestampXYZ) ;
      string2data(data_cpy) ;
      usleep(100000);
    }
  pthread_exit(NULL_value);
}

void string2data(char* string_data)
{
  /*Découpage de la chaine de caractère reçu du serveur de positionnement
    sous forme de addrMAC;TypeRequete;Timestamp1.Timestamp2;Algo;X;Y;Z
    en variables séparées
  */

  char *mac = NULL ;
  char *ptr = NULL ;

  char *delims = ";" ;

  int type_req = 0 ;
  int count_algo= 0 ;
  int count_print = 0 ;
  int onetime = 0 ;
  int error = 0 ;

  while(onetime<1)
    {

      //Lecture Adresse Mac
      ptr = strtok(string_data, delims) ;
      if(ptr==NULL)
        {
          print_error("mac") ;
          break ;
        }
      mac = ptr ;

      //Lecture Type Request
      ptr = strtok(NULL, delims) ;
      if (ptr==NULL)
        {
          print_error("request") ;
          break ;
        }
      type_req = atoi(ptr) ;

      //Lecture TimeStamp1
      ptr = strtok(NULL, ".") ;
      if (ptr==NULL)
        {
          print_error("timestamp") ;
          break ;
        }
      sscanf(ptr, "%ld", &timestamp.tv_sec) ;

      //Lecture TimeStamp2
      ptr = strtok(NULL, ";") ;
      if (ptr==NULL)
        {
          print_error("timestamp") ;
          break ;
        }
      sscanf(ptr, "%ld", &timestamp.tv_nsec) ;
      onetime++;
    }
  result results[10];

  while(run)
    {
      //Lecture de l'algorythme utilisé
      ptr = strtok(NULL, delims) ;
      if(ptr==NULL) break ;
      sscanf(ptr, "%s", results[count_algo].algo) ;
      if(!strcmp(results[count_algo].algo,"Real"))
        error=1;

      //Lecture du point X
      ptr = strtok(NULL, delims) ;
      if(ptr==NULL)
        {
          if(count_algo==0)
            {
              print_error ("trame");
              break ;
            }
          else
            {
              print_error ("algo");
              count_algo-- ;
              break ;
            }
        }
      results[count_algo].x = atof(ptr) ;

      //Lecture du point Y
      ptr = strtok(NULL, delims) ;
      if(ptr==NULL)
        {
          if(count_algo==0)
            {
              print_error ("trame");
              break ;
            }
          else
            {
              print_error ("algo");
              count_algo--;
              break ;
            }
        }
      results[count_algo].y = atof(ptr) ;

      //Lecture du point Z
      ptr = strtok(NULL, delims) ;
      if(ptr==NULL)
        {
          if(count_algo==0)
            {
              print_error ("trame");
              break ;
            }
          else
            {
              perror ("algo");
              count_algo--;
              break ;
            }
        }
      results[count_algo].z = atof(ptr) ;
  if(error==1)
        {
          //Lecture du message Error
          ptr = strtok(NULL, delims) ;
          if(ptr==NULL)
            {
              if(count_algo==0)
                {
                  print_error ("trame");
                  break ;
                }
              else
                {
                  print_error ("algo");
                  count_algo--;
                  break ;
                }
            }
          results[count_algo].check = ptr ;

          //Lecture valeur Error
          ptr = strtok(NULL, delims) ;
          if(ptr==NULL)
            {
              if(count_algo==0)
                {
                  print_error ("trame");
                  break ;
                }
              else
                {
                  perror ("algo");
                  count_algo--;
                  break ;
                }
            }
          results[count_algo].err = atof(ptr) ;


          count_algo++;
        }

      else
        count_algo++;
    }

  for(count_print=0;count_print<count_algo;count_print++)
    {
      printf("\n----------------\n"
             " Adresse Mac : %s\n"
             " Type de requete : %d\n"
             " Timestamp : %ld.%ld\n"
             " Algo : %s\n"
             " X : %f\n "
             " Y : %f\n"
             " Z : %f\n"
             "------------------\n", mac, type_req, timestamp.tv_sec, timestamp.tv_nsec, results[count_print].algo, results[count_print].x, results[count_print].y, results[count_print].z);
    }
}

void print_error(char* merror)
{
  if(!strcmp(merror,"trame")) printf("Impossible à lire la trame : Abandon") ;
  else if (!strcmp(merror,"algo")) printf("Impossible à lire les coordonnées de l'algo : Abandon") ;
  else printf("Erreur inconnu : Abandon") ;
}

void print_usage()
{
  printf("Usage:\n"
         "Localisation request:\n"
         "\t%s -d dest_ip [-p dest_port] [-i iface] [-t delay]"
         " [-n nb_packets] [-l [port]]\n"
         "Calibration request:\n"
         "\t%s -d dest_ip [-p dest_port] [-i iface] [-t delay]"
         " [-n nb_packets] direction x y z\n"
         "\n"
         "Options:\n"
         "\t-h\t\tPrint this help.\n"
         "\t-V\t\tPrint version information.\n"
         "\t-d dest_ip\tDestination IP address of the localisation request.\n"
         "\t-p dest_port\tDestination port of the localisation request"
         " (default: %d).\n"
         "\t-t delay\tTime between each packet transmission (default: %d"
         " µs for a normal request, %d µs for a calibration request).\n"
         "\t-n nb_packets\tNumber of packet transmitted for the request"
         " (default: %d for a normal request, %d for a calibration"
         " request).\n"
         "\t-i iface\tName of the network interface used to transmit the"
         " request (e.g. \"eth2\"). If this option is absent, interface"
         " is selected automatically. You must be root to use this"
         " option.\n"
         "\t-l [port]\tWait for the computed position and display it."
         " Optional argument 'port' allows to specify the listening"
         " port (default: %d).\n"
         ,
         program_name,
         program_name,
         LOC_REQUEST_DEFAULT_PORT,
         DEFAULT_DELAY_NORMAL,
         DEFAULT_DELAY_CALIB,
         DEFAULT_NBPKT_NORMAL,
         DEFAULT_NBPKT_CALIB,
         MOBILE_DEFAULT_PORT
         ) ;
}


void print_version()
{
  printf("This is OwlPS Client for AR.Drone, part of the Open Wireless"
         " Positioning System project.\n"
         "Version: %s.\n",
#ifdef OWLPS_VERSION
         OWLPS_VERSION
#else // OWLPS_VERSION
         "unknown version"
#endif // OWLPS_VERSION:w
         ) ;
}


/* Commands for drone */



void calcul_trajectory(float *distance, float *angle)
{
  int a, b;
  float dist_av, dist_ar, dep ;
  (*distance) = dist_av = dist_ar = dep = (*angle) = 0 ;

  dist_av = sqrt(pow(share_relatif_cons.x-share_relatif.x,2)+pow(share_relatif_cons.y-share_relatif.y,2));
  dist_ar = sqrt(pow(share_relatif_cons.x-share_relatif_retro.x,2)+pow(share_relatif_cons.x-share_relatif_retro.y,2));
  dep = sqrt(pow(share_relatif.x-share_relatif_retro.x,2)+pow(share_relatif.y-share_relatif_retro.y,2));

  *angle = acos((pow(dist_ar,2)+pow(dep,2)-pow(dist_av,2))/(2*dist_ar*dep));
  *angle = (*angle)*180/M_PI;
  *distance = dist_av;

  a=share_relatif_cons.x-share_relatif.x;
  b=share_relatif_cons.y-share_relatif.y;

  /* ********** */
  /*  a b | R   */
  /*  F F | F   */
  /*  F T | T   */
  /*  T F | T   */
  /*  T T | F   */
  /* ********** */

  if((a<0)!=(b<0))
    (*angle)=(*angle);
  else (*angle) = -(*angle);

}

int check_destination(struct point drone, struct point target)
{
  if(oc_distance_between(drone, target)<1.8)
    return 1;
  else return 0;
}

int read_position(char *type)
{
  if(!strcmp(type,"WIFI")) printf("Localisation by WIFI\n");
  else if (!strcmp(type,"GPS"))
    {
      printf("Localisation by GPS\n");
      while((share_gps.lat==0||share_gps.lon==0)&&(share_gps_origin.lat==0||share_gps_origin.lon==0))
        {
          if(share_gps.lat!=0 && share_gps.lon!=0)
            {
              share_gps_origin.lat = share_gps.lat;
              share_gps_origin.lon = share_gps.lon;
            }
          usleep(100000);
        }
      share_relatif = oc_convert(share_gps_origin, share_gps);
      share_relatif_retro = oc_convert(share_gps_origin, share_gps_retro);
      share_relatif_cons = oc_convert(share_gps_origin, share_gps_cons);

      printf("X current : %f Y current : %f X old : %f Y old : %f X target : %f Y target : %f\n",share_relatif.x, share_relatif.y, -share_relatif_retro.x, share_relatif_retro.y, share_relatif_cons.x, share_relatif_cons.y);

    }
  else return -1;
  return 0;
}

int cmd_drone_take_off(void)
{
  printf("Take-off...\n");
  state_flying=1;
  return 0;
}
int cmd_drone_landing(void)
{
  printf("Landing....\n");
  state_flying=0;
  return 0;
}

int cmd_drone_init_direction(void)
{
  int boucle;
  float speed;
  assert(sizeof(float) == 4) ;

  if(state_flying==0)
    return -1;
  //Implemented socket communication
  printf("Initialisation...\n");
  speed=-0.5;
  while(lock==1);
  lock=1;
  for(boucle=0;boucle<3;boucle++)
    {
      int *floatint_speed = (int32_t*) &speed ;
      sprintf(sendline,"AT*PCMD=%d,0,0,%d,0,0\r", i, *floatint_speed);
    }
  lock=0;
  return 1;
}

int cmd_drone_move(float speed_pitch, float angle)
{
  float speed_yaw;
  if(state_flying==0)
    return -1;
  else
    {
      speed_yaw=(1/180)*angle;
      while(lock==1);
      lock=1;
      int *floatint_speed_pitch = (int32_t*) &speed_pitch;
      int *floatint_speed_yaw = (int32_t*) &speed_yaw;
      sprintf(sendline,"AT*PCMD=%d,0,0,%d,0,%d\r",i,*floatint_speed_pitch,*floatint_speed_yaw);
      i++;
      sendto(sockcmdfd,sendline,strlen(sendline),0,
             (struct sockaddr *)&servcmd,sizeof(servcmd));

    }
  lock=0;
  return 0;
}

void drone_err(int err)
{
  char msg[100];

  switch(err)
    {
    case 1 :
      strcpy(msg,"Position is not determined or type of localisation is invalid, canceled\n");
      break;
    case 2 :
      strcpy(msg,"Taking-off failed, canceled\n");
      break;
    case 3 :
      strcpy(msg,"Calibration failed, canceled\n");
      break;
    case 4 :
      strcpy(msg,"Can not move drone, it's not flying, canceled\n");
      break;
    }

  printf("%s",msg);
  //drone_handler_sigint(SIGINT);
}

int drone_socket_create(void)
{
  sockcmdfd=socket(AF_INET,SOCK_DGRAM,0);

  bzero(&servcmd,sizeof(servcmd));
  servcmd.sin_family = AF_INET;
  servcmd.sin_addr.s_addr=inet_addr(IP);
  servcmd.sin_port=htons(PORT);

  return sockcmdfd;
}

int drone_socket_close(void)
{
  return close(sockcmdfd);
}

void* drone_socket_watchdog(void* NULL_value)
{
  while(run)
    {
      usleep(20000);
      memset(sendline, 0, sizeof(sendline));
      if(i==-1)
        {
          strncpy(sendline,"AT*CONFIG=1,\"general:navdata_demo\",\"TRUE\"\rAT*CONFIG=2,\"control:control_yaw\",\"2.5\"\rAT*PMODE=3,2\rAT*MISC=4,2,20,2000,3000\r", LENSTR);
          i=4;
        }
      else
        {
          while(lock==1);
          lock=1;
          snprintf(count, 100, "%d", i);
          strncat(sendline, ATREF, LENSTR);
          strncat(sendline, count, LENSTR);
          if(state_flying==0)
            strncat(sendline, LANDING, LENSTR);
          else if(state_flying==1)
            strncat(sendline, TAKEOFF, LENSTR);
          else
            strncat(sendline, LANDING, LENSTR);
          strncat(sendline, "\r", LENSTR);
        }
      i++;
      if(i>=101)
        {
          strncat(sendline, "AT*COMWDG=", LENSTR);
          snprintf(count, 100, "%d", i);
          strncat(sendline, count, LENSTR);
          strncat(sendline, "\r", LENSTR);
          i=1;
        }
      sendto(sockcmdfd,sendline,strlen(sendline),0,
             (struct sockaddr *)&servcmd,sizeof(servcmd));
      lock=0;
    }
  pthread_exit(NULL_value);
}

void drone_handler_sigint(const int num)
{
  if(num==SIGINT)
    {
      (void)cmd_drone_landing();
      sleep(2);
      run=0;
      (void) close(socksendfd) ;
      (void) close(sockreceivefd) ;
      (void) drone_socket_close ;
      exit(0);
    }
}