#include <sys/types.h>
#include <sys/socket.h>
#include <sys/select.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <string.h>
#include <errno.h>

#include "PracticalSocket.h"  // For Socket, ServerSocket, and SocketException
#include <iostream>           // For cerr and cout
#include <cstdlib>            // For atoi()
#include <string.h>
#include <stdio.h>

using namespace std;
#include "structDefs.h"
#include "TerrainNav.h"
#include "genFilterDefs.h"

#define NANOSEC_PER_SEC (1000000000L)
#define TRN_DEBUG false

static TerrainNav *_tercom;
static struct sockaddr_in _serv_addr;
static int _servfd;   // socket to bind
static int _connfd;   // socket to handle client
static bool _connected;
static struct commsT _ct;
static struct commsT _ack(TRN_ACK);

struct sockaddr_in _server_addr;     // Server Socket object
struct sockaddr_in _client_addr;

static char _sock_buf[TRN_MSG_SIZE];

char dataPath[256] = "/dataFiles/";

char mapPath[256] = "/Maps/";

bool is_connected()
{

  // If we haven't been connected or the client closed the connection,
  // don't bother checking.
  //
  if (!_connected)
    return _connected;

  struct timeval tv;
  tv.tv_sec = 0;
  tv.tv_usec = 10000L;

  // Use select to see if the client closed the connection
  //
  fd_set clientfd;
  FD_ZERO(&clientfd);
  FD_SET(_connfd, &clientfd);
  char temp[5];

  // If the socket is readable but there are no bytes, the client sent FIN
  //
  int nready = select(_connfd+1, &clientfd, 0, 0, &tv);
  if (nready > 0) {
    if (0 == recv(_connfd, temp, 1, MSG_PEEK)) {
      _connected = false;
      printf("Client closed connection!\n");
      ::close(_connfd);
    }
    else {
      _connected = true;   // Connected and there is data to read 
    }
  }
  else {
    _connected = true;     // Connected but no data to read
  }
  return _connected;
}

int get_msg()
{
  bool debug = false;
  int sl = 0;

  // Get a message as long as client is still connected
  //
  while (is_connected()) {
    if ((sl = recv(_connfd, _sock_buf+sl, TRN_MSG_SIZE-sl, 0)) <= 0) {
      printf("get_msg timeout:%d\n", errno);   // or error
      usleep(1000000);
      continue;
    }

    // We started receiving, get the whole message
    //
    while (sl < TRN_MSG_SIZE) {
      printf("get_msg up to %d bytes, get more\n", sl);

      int s = recv(_connfd, _sock_buf+sl, TRN_MSG_SIZE-sl, 0);
      if (s <= 0) {
	printf("get_msg - no more? errno = %d\n", errno);   // or error
	return sl;
      }
      else
	sl += s;
    }

    if (sl > 0 && (debug || TRN_DEBUG)) {
      for (int i = 0; i < 100; i++)
	printf("%x ", _sock_buf[i]);
      printf("\n");
    }
    return sl;
  }
}

int send_msg(commsT& msg)
{
  int sl = 0;
  printf("Send:%s\n", msg.to_s(_sock_buf, sizeof(_sock_buf)));

  // Check to see if client is still connected first
  //
  if (is_connected()) {
    memset(_sock_buf, 0, sizeof(_sock_buf));
    msg.serialize(_sock_buf);

    // Send the whole message
    //
    for (sl = 0; sl < sizeof(_sock_buf);) {
      sl += send(_connfd, _sock_buf, sizeof(_sock_buf), 0);
      printf("server:send_msg - sent %d bytes\n", sl);
    }
  }
  return sl;
}

int init()
{

  // Destruct any existing current TerrainNav
  //
  if (_tercom) {
    delete _tercom;
    _tercom = 0;
  }

  printf("Constructing tercom with map:%s, cfg:%s, and filter:%d\n",
	 _ct.mapname, _ct.cfgname, _ct.parameter);

  // Construct a TerrainNav object using the info from the client
  //
  char mapname[512], cfgname[512];
  char *mapPath = getenv("TRN_MAPFILES");
  char *cfgPath = getenv("TRN_DATAFILES");
  sprintf(mapname, "%s%s", mapPath, _ct.mapname);
  sprintf(cfgname, "%s%s", cfgPath, _ct.cfgname);
  printf("%s\n", mapname);
  printf("%s\n", cfgname);

  _tercom = new TerrainNav(mapname, cfgname, _ct.parameter);

  printf("tercom constructed, now do stuff\n");
  return 0;
}

int set_ima()
{

  printf("Setting IMA to %d\n", _ct.parameter);
  if (_tercom) {
  }
  else
    printf("No TRN object! Have you initialized yet?\n");


  return 1;

}

int set_vdr()
{

  printf("Setting VDR to %f\n", _ct.vdr);
  if (_tercom) {
  }
  else
    printf("No TRN object! Have you initialized yet?\n");


  return 1;

}

int set_mw()
{

  printf("Setting weighting to %d\n", _ct.parameter);
  if (_tercom) {
  }
  else
    printf("No TRN object! Have you initialized yet?\n");


  return 1;

}

int set_fr()
{

  printf("Setting filter reinits to %d\n", _ct.parameter);
  if (_tercom) {
  }
  else
    printf("No TRN object! Have you initialized yet?\n");


  return 1;

}

int filter_grd()
{

  printf("Setting filter gradiant to %d\n", _ct.parameter);
  if (_tercom) {
  }
  else
    printf("No TRN object! Have you initialized yet?\n");


  return 1;

}

int measure_update()
{

  printf("Received measure update with time %f\n", _ct.mt.time);
  if (_tercom) {
  }
  else
    printf("No TRN object! Have you initialized yet?\n");


  return 1;

}

int motion_update()
{

  printf("Received motion update with time %f\n", _ct.pt.time);
  if (_tercom) {
  }
  else
    printf("No TRN object! Have you initialized yet?\n");


  return 1;

}

int send_mle()
{

  printf("Client requests MLE\n");
  if (_tercom) {
  }
  else
    printf("No TRN object! Have you initialized yet?\n");


  return 1;

}

int send_mmse()
{

  printf("Client requests MMSE\n");
  if (_tercom) {
  }
  else
    printf("No TRN object! Have you initialized yet?\n");


  return 1;

}

int main( int argc, char **argv )
{
  _tercom = 0;
  int len = 0;

  printf("Get socket fd\n");
  _servfd = socket(AF_INET, SOCK_STREAM, 0);

  if (_servfd < 0) {
    exit(1);
  }

  memset(&_server_addr, '0', sizeof(_server_addr));
  _server_addr.sin_family = AF_INET;
  _server_addr.sin_addr.s_addr = htonl(INADDR_ANY);
  _server_addr.sin_port = htons(27027); 

  printf("Bind\n");
  len = bind(_servfd, (struct sockaddr*)&_server_addr, sizeof(_server_addr)); 

  if (len < 0) {
    exit(1);
  }

  /////////////////////////////////////////////////////////////////////
  // Server loop: Accept connection, service client until client is
  // done, repeat.
  //
  while (true) {
    time_t ticks; 

    printf("Listen and accept\n");
    listen(_servfd, 10); 

    _connfd = accept(_servfd, (struct sockaddr*)NULL, NULL); 
    _connected = true;

    struct timeval tv;
    tv.tv_sec = 15;
    tv.tv_usec = 0;

    setsockopt(_connfd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));

    ///////////////////////////////////////////////////////////////////////
    // Message loop: Receive and respond to messages from the client until
    // the client breaks the connection (closes the link or says goodbye).
    //
    while (_connected) {
      memset(_sock_buf, '0', sizeof(TRN_MSG_SIZE)); 

      // Get a msg from the client
      //
      int len;
      if ((len = get_msg()) < TRN_MSG_SIZE)
	continue;

      // Determine message type and respond
      //
      len = _ct.unserialize(_sock_buf, TRN_MSG_SIZE);
      printf("%s\n", _ct.to_s(_sock_buf, TRN_MSG_SIZE));

      switch (_ct.msg_type) {
      case TRN_BYE:
	printf("Client exiting connection\n");
	//close(_connfd);
	//_connected = false;
	break;

      case TRN_INIT:
	init();
	break;

      case TRN_SET_IMA:
	set_ima();
	break;

      case TRN_SET_VDR:
	set_vdr();
	break;

      case TRN_MEAS:
	measure_update();
	break;

      case TRN_MOTN:
	motion_update();
	break;

      case TRN_MLE:
	send_mle();
	break;

      case TRN_MMSE:
	send_mle();
	break;

      case TRN_SET_MW:
	set_mw();
	break;

      case TRN_SET_FR:
	set_fr();
	break;

      case TRN_FILT_GRD:
	filter_grd();
	break;

      case TRN_ACK:
      case TRN_NACK:
      default:
	printf("No handler for that message:%c\n", _ct.msg_type);
      }

      send_msg(_ack);
    }
  }
  return 0;
}