#ifdef __ARMCC_VERSION
#define _SYS_STAT_H
#endif

#include <VP_Com/vp_com_socket.h>
#include <VP_Com/vp_com_error.h>

#include <VP_Os/vp_os_malloc.h>
#include <VP_Os/vp_os_print.h>
#include <VP_Os/vp_os_signal.h>

#include <fcntl.h>
#include <errno.h>

#if defined(__linux__) || defined (__elinux__) || defined(TARGET_OS_ANDROID)
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <unistd.h>
#include <arpa/inet.h>

#define CYGPKG_NET 1
#endif

#if defined(USE_MINGW32) || defined(TARGET_OS_IPHONE) || defined(TARGET_IPHONE_SIMULATOR)

#define MSG_NOSIGNAL 0

#define CYGPKG_NET 1
#endif // USE_MINGW32 || TARGET_OS_IPHONE || TARGET_IPHONE_SIMULATOR

#ifdef CYGPKG_NET

#if !defined(USE_MINGW32) && !defined(__linux__)&& !defined(__elinux__)
extern int inet_addr(const char *);
#endif



C_RESULT vp_com_open_socket(vp_com_socket_t* sck, Read* read, Write* write)
{
  C_RESULT res = VP_COM_OK;

  int s = -1, type;
  struct sockaddr_in name = { 0 };

  switch( sck->protocol )
  {
    case VP_COM_TCP:
      s = socket( AF_INET, SOCK_STREAM, 0 );
      res = ( s < 0 ) ? VP_COM_ERROR : VP_COM_OK;
      type = sck->type;
      break;

    case VP_COM_UDP:
      s = socket( AF_INET, SOCK_DGRAM, 0 );
      sck->scn  = inet_addr(sck->serverHost); // Cache destination in int format
      res = ( s < 0 ) ? VP_COM_ERROR : VP_COM_OK;
      type = VP_COM_SERVER; // Make sure we will bind the socket in the connection-less protocol
      break;

    default:
      type = VP_COM_CLIENT;
      res = VP_COM_PARAMERROR;
      break;
  }

  if( FAILED(res) )
  {
    PRINT("\nSocket opening failed\n");
  }

  VP_COM_CHECK( res );

  name.sin_family = AF_INET;
  name.sin_port   = htons( sck->port );
  switch( type )
  {
    case VP_COM_CLIENT:
      name.sin_addr.s_addr  = inet_addr(sck->serverHost);
      if ( connect( s, (struct sockaddr*)&name, sizeof( name ) ) == -1 )
        res = VP_COM_ERROR;
      break;

    case VP_COM_SERVER:
      name.sin_addr.s_addr  = INADDR_ANY;

      if ( bind( s, (struct sockaddr*)&name, sizeof(struct sockaddr)) < 0 )
        res = VP_COM_ERROR;

      if ( sck->is_multicast == 1 )
      {

        if ( sck->multicast_base_addr == 0 )
        {
          PRINT("Error : multicast base address is not defined\n");
          res = VP_COM_ERROR;
          break;
        }

#ifdef USE_ANDROID
		__u32 multicast_address, drone_address;
#else
        in_addr_t multicast_address, drone_address;
#endif        
        vp_com_wifi_config_t *wifi_cfg = (vp_com_wifi_config_t*) wifi_config();

        // compute remote address according to local address
    	drone_address = inet_addr(wifi_cfg->server);
        multicast_address = htonl( sck->multicast_base_addr | (ntohl(drone_address) & 0xFF) );

        struct ip_mreq mreq;
        mreq.imr_interface.s_addr = inet_addr(wifi_cfg->localHost);
        mreq.imr_multiaddr.s_addr = multicast_address;

        if ( setsockopt( s, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq) ) < 0) {
          PRINT("!!! Error enabling multicast !!!\n");
        }
      }

      break;

    default:
      res = VP_COM_PARAMERROR;
      break;
  }

  if(res == VP_COM_OK)
  {
    sck->priv = (void*) s;

    switch( sck->protocol )
    {
      case VP_COM_TCP:
        if(read)  *read   = (Read) vp_com_read_socket;
        if(write) *write  = (Write) vp_com_write_socket;
        break;

      case VP_COM_UDP:
        if(read)  *read   = (Read) vp_com_read_udp_socket;
        if(write) *write  = (Write) vp_com_write_udp_socket;
        break;

      default:
        if(read)  *read   = NULL;
        if(write) *write  = NULL;
        break;
    }
  }
  else
  {
    close( s );
  }

  if (sck->block != VP_COM_DEFAULT &&
      sck->block != VP_COM_WAITALL &&
      sck->block != VP_COM_DONTWAIT)
  {
    sck->block = VP_COM_DEFAULT;
  }

  return res;
}

C_RESULT vp_com_close_socket(vp_com_socket_t* socket)
{
  if(socket == NULL)
    return VP_COM_PARAMERROR;

  // shutdown( (int) socket->priv, SHUT_RDWR );
  close( (int) socket->priv );

  socket->priv = NULL;

  return VP_COM_OK;
}

C_RESULT vp_com_wait_socket(vp_com_socket_t* server, vp_com_socket_t* client, int32_t queue_length)
{
  C_RESULT res = VP_COM_OK;

  if(server == NULL)
    return VP_COM_PARAMERROR;

  int s = (int) server->priv;
  int c = 0;
  socklen_t l = sizeof(struct sockaddr_in);
  struct sockaddr_in raddr = { 0 }; // remote address

  server->queue_length = queue_length;

  listen(s, queue_length);
  c = accept( s, (struct sockaddr*)&raddr, &l );
  if( c < 0 )
    res = VP_COM_ERROR;

  if(SUCCEED( res ))
  {
    vp_os_memcpy( client, server, sizeof(vp_com_socket_t) );
    client->priv = (void*) c;
  }

  return res;
}

#endif

C_RESULT vp_com_sockopt_ip(vp_com_t* vp_com, vp_com_socket_t* socket, VP_COM_SOCKET_OPTIONS options)
{
  int32_t one  = 1;
  int32_t zero = 0;

  C_RESULT res = VP_COM_ERROR;
#ifdef CYGPKG_NET
  int s = (int) socket->priv;

  if( options & VP_COM_NON_BLOCKING )
  {
#ifndef USE_MINGW32
    int32_t arg = 1;

    PRINT("Setting socket %d to non blocking\n", s);
    res = ioctl( s, FIONBIO, &arg ) < 0 ? C_FAIL : C_OK;
#endif

    if( FAILED(res) )
      PRINT("error setting non blocking\n");
  }

  if( options & VP_COM_NO_DELAY )
  {
    PRINT("Disabling the Nagle (TCP No Delay) algorithm for socket %d\n", s);

    res = setsockopt( s, IPPROTO_TCP, TCP_NODELAY, (char *)&one, sizeof(one) ) < 0 ? C_FAIL : C_OK;

    if( FAILED(res) )
      PRINT("error disabling the Nagle algorithm\n");
  }

  if ( options & VP_COM_MULTICAST_ON )
  {
    res = setsockopt( s, IPPROTO_IP, IP_MULTICAST_LOOP, &zero, sizeof(zero) );
    if ( FAILED(res) )
      PRINT("Error : cannot set IP_MULTICAST_LOOP to 0\n");

    struct in_addr interface_addr;
    interface_addr.s_addr = INADDR_ANY;
    res = setsockopt( s, IPPROTO_IP, IP_MULTICAST_IF, &interface_addr, sizeof(interface_addr) );
    if ( FAILED(res) ) {
      PRINT("Error : cannot enable multicast.\n");
    }
  }
#endif
  return res;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////

#ifdef CYGPKG_NET

C_RESULT vp_com_read_udp_socket(vp_com_socket_t* sck, int8_t* buffer, int32_t* size)
{
  C_RESULT res;
  int s = (int) sck->priv;
  struct sockaddr_in from;

  socklen_t from_len = sizeof(from);

  int flags = MSG_NOSIGNAL;
  if (VP_COM_DONTWAIT == sck->block)
    flags |= MSG_DONTWAIT;
  else if (VP_COM_WAITALL == sck->block)
    flags |= MSG_WAITALL;
    

  if(s >= 0)
  {
    res = VP_COM_OK;
    *size = recvfrom(s, (char*)buffer, *size, flags, (struct sockaddr*)&from, &from_len );

    if(*size < 0)
    {

#ifdef USE_MINGW32
      switch( WSAGetLastError() )
      {
        case WSAEOPNOTSUPP:
          PRINT("MSG_NOSIGNAL is not supported on this platform\n");
          res = VP_COM_ERROR;
          break;

        case WSAEINTR:
          *size = 0;
          break;

        case WSAENETDOWN:
        case WSAETIMEDOUT:
        case WSAECONNRESET:
          PRINT("Connection with peer is not enabled\n");
          res = VP_COM_ERROR;
          break;
      }
#else
      switch( errno )
      {
        case EAGAIN:
        case EINTR:
          *size = 0;
          break;

        case EOPNOTSUPP:
          PRINT("MSG_NOSIGNAL is not supported on this platform\n");
          res = VP_COM_ERROR;
          break;

        case EPIPE:
        case ENOTCONN:
        case ECONNRESET:
          PRINT("Connection with peer is not enabled\n");
          res = VP_COM_ERROR;
          break;

        default:
          PRINT("recvfrom fails with error: %s\n", strerror(errno));
          res = VP_COM_ERROR;
          break;
      }
#endif // USE_MINGW32
    }
    else
    {
      sck->scn   = from.sin_addr.s_addr;
      sck->port  = ntohs(from.sin_port);
    }
  }
  else
  {
    res = VP_COM_ERROR;
  }

  return res;
}

C_RESULT vp_com_make_udp_target( vp_com_socket_t* sck )
{
  C_RESULT res = C_FAIL;

  if( sck->protocol == VP_COM_UDP )
  {
    sck->scn  = inet_addr(sck->serverHost); // We use scn field to store ip in order to avoid a call to inet_addr each time we call write
    res = C_OK;
  }

  return res;
}

C_RESULT vp_com_write_udp_socket(vp_com_socket_t* sck, const int8_t* buffer, int32_t* size)
{
  C_RESULT res;
  int s = (int) sck->priv;
  struct sockaddr_in to;

  int flags = 0;
  if (VP_COM_DONTWAIT == sck->block)
    flags |= MSG_DONTWAIT;
  else if (VP_COM_WAITALL == sck->block)
    flags |= MSG_WAITALL;

  if(s >= 0)
  {
    res = VP_COM_OK;

    vp_os_memset( (char*)&to, 0, sizeof(to) );
    to.sin_family       = AF_INET;
    to.sin_addr.s_addr  = sck->scn;
    if ( sck -> remotePort )
        to.sin_port     = htons(sck->remotePort);
    else
        to.sin_port     = htons(sck->port);

    *size = sendto( s, (char*)buffer, *size, flags, (struct sockaddr*)&to, sizeof(to) );

    if(*size < 0)
    {
#ifdef USE_MINGW32

      switch( WSAGetLastError() )
      {
        case WSAEOPNOTSUPP:
          PRINT("MSG_NOSIGNAL is not supported on this platform\n");
          res = VP_COM_ERROR;
          break;

        case WSAEINTR:
          *size = 0;
          break;

        case WSAENETDOWN:
        case WSAETIMEDOUT:
        case WSAECONNRESET:
          PRINT("Connection with peer is not enabled\n");
          res = VP_COM_ERROR;
          break;
      }

#else
      if( errno == EAGAIN )
      {
        *size = 0;
      }
      else
      {
        switch( errno )
        {
          case EOPNOTSUPP:
            PRINT("MSG_NOSIGNAL is not supported on this platform\n");
            break;

          case EPIPE:
          case ENOTCONN:
          case ECONNRESET:
            PRINT("Connection with peer is not enabled\n");
            break;

          default:
            PRINT("sendto fails with error: %d - %s\n", errno, strerror(errno));
            break;
        }

        res = VP_COM_ERROR;
      }
#endif // USE_MINGW32
    }
  }
  else
  {
    res = VP_COM_ERROR;
  }

  return res;
}

C_RESULT vp_com_read_socket(vp_com_socket_t* socket, int8_t* buffer, int32_t* size)
{
  C_RESULT res;
  int s = (int) socket->priv;

  int flags = 0;
  if (VP_COM_DONTWAIT == socket->block)
    flags |= MSG_DONTWAIT;
  else if (VP_COM_WAITALL == socket->block)
    flags |= MSG_WAITALL;

  if(s >= 0)
  {
    res = VP_COM_OK;
    *size = recv(s, buffer, *size, flags);
    if(*size < 0)
    {
      if( errno == EAGAIN ||
          errno == EWOULDBLOCK )
      {
        *size = 0;
      }
      else
      {
        res = VP_COM_ERROR;
      }
    }
  }
  else
  {
    res = VP_COM_ERROR;
  }

  return res;
}

C_RESULT vp_com_write_socket(vp_com_socket_t* socket, const int8_t* buffer, int32_t* size)
{
  C_RESULT res;
  int s = (int) socket->priv;

  int flags = 0;
  if (VP_COM_DONTWAIT == socket->block)
    flags |= MSG_DONTWAIT;
  else if (VP_COM_WAITALL == socket->block)
    flags |= MSG_WAITALL;

  if(s >= 0)
  {
    res = VP_COM_OK;
    *size = send(s, buffer, *size, flags);
    if(*size < 0)
    {
      if( errno == EAGAIN )
      {
        *size = 0;
      }
      else
      {
        res = VP_COM_ERROR;
      }
    }
  }
  else
  {
    res = VP_COM_ERROR;
  }

  return res;
}

#endif

/////////////////////////////////////////////////////////////////////////////////////////////////////////////

static vp_os_mutex_t  server_initialisation_mutex;
static vp_os_cond_t   server_initialisation_wait;
static bool_t         server_init_not_finished = FALSE;

C_RESULT vp_com_init_server(void)
{
  server_init_not_finished = TRUE;
  vp_os_mutex_init(&server_initialisation_mutex);
  vp_os_cond_init(&server_initialisation_wait, &server_initialisation_mutex);

  return C_OK;
}

C_RESULT vp_com_wait_for_server_up(void)
{
  if( server_init_not_finished )
  {
    vp_os_mutex_lock(&server_initialisation_mutex);
    vp_os_cond_wait(&server_initialisation_wait);
    vp_os_mutex_unlock(&server_initialisation_mutex);
  }

  return C_OK;
}

C_RESULT vp_com_timed_wait_for_server_up(uint32_t ms)
{
  C_RESULT res = C_OK;

  if( server_init_not_finished )
  {
    vp_os_mutex_lock(&server_initialisation_mutex);
    res = vp_os_cond_timed_wait(&server_initialisation_wait, ms);
    vp_os_mutex_unlock(&server_initialisation_mutex);
  }

  return res;
}

extern int32_t vp_com_fill_read_fs(vp_com_socket_t* sockets, int32_t num_sockets, int32_t max, fd_set* read_fs );
extern void vp_com_close_client_sockets(vp_com_socket_t* client_sockets, int32_t num_client_sockets);
extern C_RESULT vp_com_client_open_socket(vp_com_socket_t* server_socket, vp_com_socket_t* client_socket);
extern void vp_com_client_receive( vp_com_socket_t *client_socket );

DEFINE_THREAD_ROUTINE_STACK( vp_com_server, thread_params, VP_COM_THREAD_SERVER_STACK_SIZE )
{
#ifdef CYGPKG_NET
  vp_com_socket_t client_sockets[VP_COM_THREAD_NUM_MAX_CLIENTS];
  struct timeval tv, *ptv;

  // This thread setup connection then loop & wait for a socket event
  vp_com_server_thread_param_t* params = (vp_com_server_thread_param_t*) thread_params;

  int32_t i, rc, ncs, s, max = 0, num_server_sockets = params->num_servers, num_client_sockets = 0;
  vp_com_socket_t* server_sockets = params->servers;
  fd_set read_fs;

  vp_os_memset( client_sockets, 0, sizeof( client_sockets ));

  if(FAILED(vp_com_init(params->com)))
  {
    DEBUG_PRINT_SDK("[VP_COM_SERVER] Failed to init com\n");
    vp_com_shutdown(params->com);
  }
  else if(FAILED(vp_com_local_config(params->com, params->config)))
  {
    DEBUG_PRINT_SDK("[VP_COM_SERVER] Failed to configure com\n");
    vp_com_shutdown(params->com);
  }
  else if(FAILED(vp_com_connect(params->com, params->connection, 1)))
  {
    DEBUG_PRINT_SDK("[VP_COM_SERVER] Failed to connect\n");
    vp_com_shutdown(params->com);
  }
  else
  {
    vp_os_mutex_lock(&server_initialisation_mutex);
    vp_os_cond_signal(&server_initialisation_wait);
    vp_os_mutex_unlock(&server_initialisation_mutex);

    server_init_not_finished = FALSE;

    for( i = 0; i < num_server_sockets; i++ )
    {
      if(FAILED( vp_com_open_socket(&server_sockets[i], NULL, NULL) ))
      {
        DEBUG_PRINT_SDK("[VP_COM_SERVER] Unable to open server socket\n");
        server_sockets[i].is_disable = TRUE;
      }
      else
      {
        listen((int32_t)server_sockets[i].priv, server_sockets[i].queue_length);
      }
    }

    params->run = TRUE;

    while( params->run == TRUE )
    {
      if( params->timer_enable == FALSE || ( params->wait_sec == 0 && params->wait_usec == 0 ) )
      {
        ptv = NULL;
      }
      else
      {
        tv.tv_sec   = params->wait_sec;
        tv.tv_usec  = params->wait_usec;
        ptv         = &tv;
      }

      FD_ZERO(&read_fs);
      max = vp_com_fill_read_fs( &server_sockets[0], num_server_sockets, 0, &read_fs );
      max = vp_com_fill_read_fs( &client_sockets[0], num_client_sockets, max, &read_fs );

      rc = select( max + 1, &read_fs, NULL, NULL, ptv );
      if( rc == -1 && ( errno == EINTR || errno == EAGAIN ) )
        continue;

      if( rc == 0 )
      {
        DEBUG_PRINT_SDK("[VP_COM_SERVER] select timeout\n");

        vp_com_close_client_sockets(&client_sockets[0], num_client_sockets);
        num_client_sockets = 0;

        params->timer_enable  = FALSE;
        vp_os_memset( client_sockets, 0, sizeof( client_sockets ));
      }

      for( i = 0; i < num_server_sockets && rc != 0; i++ )
      {
        s = (int32_t) server_sockets[i].priv;

        if( ( !server_sockets[i].is_disable ) && FD_ISSET( s, &read_fs) )
        {
          rc --;

          // Recycle previously released sockets
          for( ncs = 0; ncs < num_client_sockets && client_sockets[ncs].priv != NULL; ncs++ );

          if( ncs < VP_COM_THREAD_NUM_MAX_CLIENTS)
          {
            if( SUCCEED(vp_com_client_open_socket(&server_sockets[i], &client_sockets[ncs])) && ( ncs == num_client_sockets ) )
              num_client_sockets ++;
          }
        }
      }

      for( i = 0; i < num_client_sockets && rc != 0; i++ )
      {
        s = (int32_t) client_sockets[i].priv;
        if( ( !client_sockets[i].is_disable ) && FD_ISSET( s, &read_fs) )
        {
          rc--;

          vp_com_client_receive( &client_sockets[i] );
        }
      }
    }

    for( i = 0; i < num_server_sockets; i++ )
    {
      vp_com_close_socket(&server_sockets[i]);
    }
  }

  vp_com_disconnect(params->com);
  vp_com_shutdown(params->com);
#endif

  THREAD_RETURN( 0 );
}
