/*
 * 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 <pthread.h>


/* Options */
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



int main(int argc, char *argv[])
{
  program_name = argv[0] ;
  parse_command_line(argc, argv) ;

  create_socket() ;
  pthread_t send ;
  pthread_create(&send, NULL, thread_send, NULL) ;
  while(1)
    { 
      if (options.listening_port > 0)
        receive_position() ;
    }
  (void) close(socksendfd) ;
  (void) close(sockreceivefd) ;
  //(void) close(socksendlvfd) ; //Use for close labview socket ; for next use
  return 0 ;
}



void parse_command_line(int argc, char **argv)
{
  parse_main_options(argc, argv) ;
  check_destination_ip() ;
  parse_calibration_data(argc, argv) ;
  check_configuration() ;

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

void* thread_send(void* data)
{
  while(1)
    {
      make_packet() ;
      send_request() ;
    }
  return NULL;
}

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 ;
        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: %d\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()
{
  char *ip_labview = "192.168.3.2" ;
  int port_labview = 50000 ;
  socksendfd =
    owlclient_create_trx_socket(options.dest_ip, options.dest_port,
                                &server, options.iface) ;

  //socksendlvfd =
  //owlclient_create_trx_socket(ip_labview, port_labview,
  //                            &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
      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()
{
  // Position of the mobile as computed by the infrastructure:
  char timestampXYZ[128] ;
  char *data_cpy ;
  recvfrom(sockreceivefd, &timestampXYZ, 128, 0, NULL, NULL) ;
  //send_labview(timestampXYZ) ; // Use for future
  data_cpy = strdup(timestampXYZ) ;
  string2data(data_cpy) ;
}

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 ;
  

  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(1)
    {
      //Lecture de l'algorythme utilisé
      ptr = strtok(NULL, delims) ;
      if(ptr==NULL) break ;
      sscanf(ptr, "%s", results[count_algo].algo) ;

      //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) ;

      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 send_labview(char* string_data)
{
  //owlclient_send_request(socksendlvfd, &server, string_data, strlen(string_data),
  //1, 0) ;
  //printf("Send to labView : %s", string_data) ;
}

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-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
         ) ;
}