#include <sys/types.h>
#include <sys/socket.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <sys/time.h>

#include "Syslog.h"
#include "structDefs.h"
#include "TerrainNavClient.h"

void main(int argc, char **argv)
{
  char buf[TRN_MSG_SIZE];
  measT m0, m1;
  m1.time = 27.0;
  m1.dataType = 2;
  m1.x = 10.0;
  m1.numMeas = 4;
  m1.covariance = NULL;
  //m1.covariance = new double[m1.numMeas];
  //m1.ranges = new double[m1.numMeas];
  //m1.ranges[1] = 15.0;
  m1.crossTrack = new double[m1.numMeas];
  m1.crossTrack[1] = 15.0;
  m1.alongTrack = new double[m1.numMeas];
  m1.altitudes = new double[m1.numMeas];
  m1.measStatus = new bool[m1.numMeas];
  printf("%f, %f\n", m1.time, m1.crossTrack[1]);

  int len = m1.serialize((char*)buf, sizeof(buf));
  printf("len of serialized m1 = %d\n", len);

  len = m0.unserialize((char*)buf, sizeof(buf));
  printf("len of unserialized m0 = %d\n", len);
  printf("%f, %f\n", m0.time, m0.crossTrack[1]);

  m1.time = 0;
  m1 = m0;
  printf("%f, %f\n", m1.time, m1.crossTrack[1]);

  poseT m2, m3;
  m2.time = 270.0;
  m2.covariance[30] = 17.0;
  m2.theta = -7.;
  len = m2.serialize((char*)buf, sizeof(buf));

  printf("len of serialized m2 = %d\n", len);

  len = m3.unserialize((char*)buf, sizeof(buf));
  printf("len of unserialized m3 = %d\n", len);
  printf("%f %f %f\n",m3.time, m3.covariance[30], m3.theta);

  poseT *pm2 = &m2;
  pm2->time = 0;
  *pm2 = m3;
  printf("%f %f %f\n",m2.time, m2.covariance[30], m2.theta);

  // Test commsT with embedded poseT
  {
    printf("m3:%f %f %f\n",m3.time, m3.covariance[30], m3.theta);
    struct commsT ct(TRN_MMSE, m3), ct1;
    ct.serialize(buf, TRN_MSG_SIZE);

    poseT pt;
    ct1.pt = pt;
    ct1.unserialize(buf, TRN_MSG_SIZE);
    printf("ct1.pt:%f %f %f\n",ct1.pt.time, ct1.pt.covariance[30], ct1.pt.theta);
  }

  // Test commsT with embedded measT
  struct commsT ct1;
  {
    printf("m0:%f %f\n",m0.time, m0.crossTrack[1]);
    struct commsT ct(TRN_MOTN, 1, m0);
    ct.serialize(buf, TRN_MSG_SIZE);

    measT m1;
    ct1.mt = m1;
    ct1.unserialize(buf, TRN_MSG_SIZE);
    printf("ct1:%f %f\n",ct1.mt.time, ct1.mt.crossTrack[1]);
  }

  ct1.to_s(buf, TRN_MSG_SIZE);
  printf("%s\n", buf);

  commsT ct2(TRN_INIT, 2, "mapname", "config"), ct3;
  ct2.to_s(buf, TRN_MSG_SIZE);
  printf("%s\n", buf);
  ct2.serialize(buf, TRN_MSG_SIZE);
  ct3.unserialize(buf, TRN_MSG_SIZE);
  ct3.to_s(buf, TRN_MSG_SIZE);
  printf("%s\n", buf);
  ct3.clean();
  ct3.to_s(buf, TRN_MSG_SIZE);
  printf("%s\n", buf);


  // Simple socket client connects, sends a serialized poseT, receives and ack
  //
  int _sockfd;
  if ( (_sockfd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP)) < 0 ) {
    printf("TerrinNavClient: can't create socket", errno);
  }

  struct sockaddr_in _server_addr;
  char *_server_ip = "134.89.32.25";
  short _server_port = 27027;

  memset((void*)&_server_addr, 0, sizeof(_server_addr));

  // Set up client info
  //
  _server_addr.sin_family      = AF_INET;
  _server_addr.sin_addr.s_addr = inet_addr(_server_ip);
  _server_addr.sin_port        = htons(_server_port);

  if (connect(_sockfd, (struct sockaddr *)&_server_addr, sizeof(_server_addr))
	      < 0) {
    printf("TerrainNavClient: can't connect to server: %d\n", errno);
    exit(1);
  }

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

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

  memset(buf, '0', sizeof(TRN_MSG_SIZE)); 
  ct1.clean();
  ct1.msg_type = TRN_INIT;
  ct1.mapname = "PortugueseLedge/PortugueseLedge3dPointsDEM.txt";
  ct1.cfgname = "imagingAUV_specs.cfg";
  ct1.parameter = 2;
  printf("%s\n", ct1.to_s(buf, TRN_MSG_SIZE));

  ct1.serialize(buf, TRN_MSG_SIZE);
  ct1.unserialize(buf, TRN_MSG_SIZE);
  for (int i = 0; i < 100; i++)
    printf("%x ", buf[i]);
  printf("\n");

  len = send(_sockfd, (void*)buf, sizeof(buf), 0);
  printf("Sent %d bytes to server\n", len);

  for (len = 0; len < TRN_MSG_SIZE;)
    len += recv(_sockfd, buf+len, TRN_MSG_SIZE-len, 0);

  if (len > 0) {
    ct1.unserialize(buf, TRN_MSG_SIZE);
    printf("%s\n", ct1.to_s(buf, TRN_MSG_SIZE));
  }
  printf("got %d bytes in response\n", len);
  sleep(2);

  printf("########## IMA ##########\n");
  memset(buf, '0', sizeof(TRN_MSG_SIZE)); 
  commsT ima(TRN_SET_IMA, 1);
  printf("%s\n", ima.to_s(buf, TRN_MSG_SIZE));
  ima.serialize(buf);
  len = send(_sockfd, (void*)buf, sizeof(buf), 0);
  printf("Sent %d bytes to server\n", len);

  for (len = 0; len < TRN_MSG_SIZE;)
    len += recv(_sockfd, buf+len, TRN_MSG_SIZE-len, 0);

  if (len > 0) {
    ct1.unserialize(buf, TRN_MSG_SIZE);
    printf("%s\n", ct1.to_s(buf, TRN_MSG_SIZE));
  }
  printf("got %d bytes in response\n", len);
  sleep(2);

  printf("########## VDR ##########\n");
  memset(buf, '0', sizeof(TRN_MSG_SIZE)); 
  commsT vdr(TRN_SET_VDR, 0, 1.5);
  printf("%s\n", vdr.to_s(buf, TRN_MSG_SIZE));
  vdr.serialize(buf);
  len = send(_sockfd, (void*)buf, sizeof(buf), 0);
  printf("Sent %d bytes to server\n", len);

  for (len = 0; len < TRN_MSG_SIZE;)
    len += recv(_sockfd, buf+len, TRN_MSG_SIZE-len, 0);

  if (len > 0) {
    ct1.unserialize(buf, TRN_MSG_SIZE);
    printf("%s\n", ct1.to_s(buf, TRN_MSG_SIZE));
  }
  printf("got %d bytes in response\n", len);
  sleep(2);

  printf("########## OUT ##########\n");
  memset(buf, '0', sizeof(TRN_MSG_SIZE)); 
  commsT out(TRN_OUT_MEAS);
  printf("%s\n", out.to_s(buf, TRN_MSG_SIZE));
  out.serialize(buf);
  len = send(_sockfd, (void*)buf, sizeof(buf), 0);
  printf("Sent %d bytes to server\n", len);

  for (len = 0; len < TRN_MSG_SIZE;)
    len += recv(_sockfd, buf+len, TRN_MSG_SIZE-len, 0);

  if (len > 0) {
    ct1.unserialize(buf, TRN_MSG_SIZE);
    printf("%s\n", ct1.to_s(buf, TRN_MSG_SIZE));
  }
  printf("got %d bytes in response\n", len);
  sleep(2);

  printf("########## LAST ##########\n");
  memset(buf, '0', sizeof(TRN_MSG_SIZE)); 
  commsT last(TRN_LAST_MEAS);
  printf("%s\n", last.to_s(buf, TRN_MSG_SIZE));
  last.serialize(buf);
  len = send(_sockfd, (void*)buf, sizeof(buf), 0);
  printf("Sent %d bytes to server\n", len);

  for (len = 0; len < TRN_MSG_SIZE;)
    len += recv(_sockfd, buf+len, TRN_MSG_SIZE-len, 0);

  if (len > 0) {
    ct1.unserialize(buf, TRN_MSG_SIZE);
    printf("%s\n", ct1.to_s(buf, TRN_MSG_SIZE));
  }
  printf("got %d bytes in response\n", len);
  sleep(2);

  printf("########## CONV ##########\n");
  memset(buf, '0', sizeof(TRN_MSG_SIZE)); 
  commsT conv(TRN_IS_CONV);
  printf("%s\n", conv.to_s(buf, TRN_MSG_SIZE));
  conv.serialize(buf);
  len = send(_sockfd, (void*)buf, sizeof(buf), 0);
  printf("Sent %d bytes to server\n", len);

  for (len = 0; len < TRN_MSG_SIZE;)
    len += recv(_sockfd, buf+len, TRN_MSG_SIZE-len, 0);

  if (len > 0) {
    ct1.unserialize(buf, TRN_MSG_SIZE);
    printf("%s\n", ct1.to_s(buf, TRN_MSG_SIZE));
  }
  printf("got %d bytes in response\n", len);
  sleep(2);

  printf("########## TYPE ##########\n");
  memset(buf, '0', sizeof(TRN_MSG_SIZE)); 
  commsT type(TRN_FILT_TYPE);
  printf("%s\n", type.to_s(buf, TRN_MSG_SIZE));
  type.serialize(buf);
  len = send(_sockfd, (void*)buf, sizeof(buf), 0);
  printf("Sent %d bytes to server\n", len);

  for (len = 0; len < TRN_MSG_SIZE;)
    len += recv(_sockfd, buf+len, TRN_MSG_SIZE-len, 0);

  if (len > 0) {
    ct1.unserialize(buf, TRN_MSG_SIZE);
    printf("%s\n", ct1.to_s(buf, TRN_MSG_SIZE));
  }
  printf("got %d bytes in response\n", len);
  sleep(2);

  printf("########## STATE ##########\n");
  memset(buf, '0', sizeof(TRN_MSG_SIZE)); 
  commsT state(TRN_FILT_STATE);
  printf("%s\n", state.to_s(buf, TRN_MSG_SIZE));
  state.serialize(buf);
  len = send(_sockfd, (void*)buf, sizeof(buf), 0);
  printf("Sent %d bytes to server\n", len);

  for (len = 0; len < TRN_MSG_SIZE;)
    len += recv(_sockfd, buf+len, TRN_MSG_SIZE-len, 0);

  if (len > 0) {
    ct1.unserialize(buf, TRN_MSG_SIZE);
    printf("%s\n", ct1.to_s(buf, TRN_MSG_SIZE));
  }
  printf("got %d bytes in response\n", len);
  sleep(2);

  printf("########## REINIT ##########\n");
  memset(buf, '0', sizeof(TRN_MSG_SIZE)); 
  commsT reinits(TRN_N_REINITS);
  printf("%s\n", reinits.to_s(buf, TRN_MSG_SIZE));
  reinits.serialize(buf);
  len = send(_sockfd, (void*)buf, sizeof(buf), 0);
  printf("Sent %d bytes to server\n", len);

  for (len = 0; len < TRN_MSG_SIZE;)
    len += recv(_sockfd, buf+len, TRN_MSG_SIZE-len, 0);

  if (len > 0) {
    ct1.unserialize(buf, TRN_MSG_SIZE);
    printf("%s\n", ct1.to_s(buf, TRN_MSG_SIZE));
  }
  printf("got %d bytes in response\n", len);
  sleep(2);

  printf("########## MLE ##########\n");
  memset(buf, '0', sizeof(TRN_MSG_SIZE)); 
  struct commsT mle(TRN_MLE, m2);
  printf("%s\n", mle.to_s(buf, TRN_MSG_SIZE));
  mle.serialize(buf);
  len = send(_sockfd, (void*)buf, sizeof(buf), 0);
  printf("Sent %d bytes to server\n", len);
  
  for (len = 0; len < TRN_MSG_SIZE;)
    len += recv(_sockfd, buf+len, TRN_MSG_SIZE-len, 0);

  if (len > 0) {
    ct1.unserialize(buf, TRN_MSG_SIZE);
    printf("%s\n", ct1.to_s(buf, TRN_MSG_SIZE));
  }
  printf("got %d bytes in response\n", len);
  printf("%s\n", ct1.to_s(buf, TRN_MSG_SIZE));
  sleep(2);

  printf("########## MMSE ##########\n");
  memset(buf, '0', sizeof(TRN_MSG_SIZE)); 
  m3.time = 14.0;
  commsT mmse(TRN_MMSE, m3);
  printf("%s\n", mmse.to_s(buf, TRN_MSG_SIZE));
  mmse.serialize(buf);
  len = send(_sockfd, (void*)buf, sizeof(buf), 0);
  printf("Sent %d bytes to server\n", len);
  
  for (len = 0; len < TRN_MSG_SIZE;)
    len += recv(_sockfd, buf+len, TRN_MSG_SIZE-len, 0);

  if (len > 0) {
    ct1.unserialize(buf, TRN_MSG_SIZE);
    printf("%s\n", ct1.to_s(buf, TRN_MSG_SIZE));
  }
  printf("got %d bytes in response\n", len);
  printf("%s\n", ct1.to_s(buf, TRN_MSG_SIZE));
  sleep(2);

  memset(buf, '0', sizeof(TRN_MSG_SIZE)); 
  commsT bye(TRN_BYE);
  printf("%s\n", bye.to_s(buf, TRN_MSG_SIZE));
  bye.serialize(buf);
  len = send(_sockfd, (void*)buf, sizeof(buf), 0);
  printf("Sent %d bytes to server\n", len);

  close(_sockfd);


#if 0
  memset(buf, '0', sizeof(TRN_MSG_SIZE)); 
  commsT meas(TRN_MEAS, 0, m0);
  printf("%s\n", meas.to_s(buf, TRN_MSG_SIZE));
  meas.serialize(buf);
  len = send(_sockfd, (void*)buf, sizeof(buf), 0);
  printf("Sent %d bytes to server\n", len);

  for (len = 0; len < TRN_MSG_SIZE;)
    len += recv(_sockfd, buf+len, TRN_MSG_SIZE-len, 0);

  if (len > 0) {
    ct1.unserialize(buf, TRN_MSG_SIZE);
    printf("%s\n", ct1.to_s(buf, TRN_MSG_SIZE));
  }
  printf("got %d bytes in response\n", len);
  sleep(2);

  memset(buf, '0', sizeof(TRN_MSG_SIZE)); 
  m2.time = 13.0;
  commsT motn(TRN_MOTN, m2);
  printf("%s\n", motn.to_s(buf, TRN_MSG_SIZE));
  motn.serialize(buf);
  len = send(_sockfd, (void*)buf, sizeof(buf), 0);
  printf("Sent %d bytes to server\n", len);

  for (len = 0; len < TRN_MSG_SIZE;)
    len += recv(_sockfd, buf+len, TRN_MSG_SIZE-len, 0);

  if (len > 0) {
    ct1.unserialize(buf, TRN_MSG_SIZE);
    printf("%s\n", ct1.to_s(buf, TRN_MSG_SIZE));
  }
  printf("got %d bytes in response\n", len);
  sleep(2);

#endif  
  
  TerrainNavClient *nav =
    new TerrainNavClient("134.89.32.25", 27027,
			 "PortugueseLedge/PortugueseLedge3dPointsDEM.txt",
			 "imagingAUV_specs.cfg", 2);
			 

  poseT pt;
  printf("########## Client MLE ##########\n");
  nav->estimatePose(&pt, 1);
  printf("poseT time = %f\n", pt.time);
  sleep(2);

  printf("########## Client MMSE ##########\n");
  nav->estimatePose(&pt, 2);
  printf("poseT time = %f\n", pt.time);
  sleep(2);

  printf("########## Client OUT ##########\n");
  bool yes = nav->outstandingMeas();
  printf("outstanding meas = %s\n", (yes? "true":"false"));
  sleep(2);

  measT mt;
  printf("########## Client MEAS ##########\n");
  nav->measUpdate(&mt, 1);
  printf("measT time = %f\n", mt.time);
  sleep(2);

  printf("########## Client MOTN ##########\n");
  nav->motionUpdate(&pt);
  printf("measT time = %f\n", mt.time);
  sleep(2);

  printf("########## Client LAST ##########\n");
  yes = nav->lastMeasSuccessful();
  printf("last meas success = %s\n", (yes? "true":"false"));
  sleep(2);

  printf("########## Client IMA ##########\n");
  nav->setInterpMeasAttitude(true);
  sleep(2);

  printf("########## Client CONV ##########\n");
  yes = nav->isConverged();
  printf("converged = %s\n", (yes? "true":"false"));
  sleep(2);

  printf("########## Client VDR ##########\n");
  nav->setVehicleDriftRate(7.27);
  sleep(2);

  printf("########## Client LOW ##########\n");
  nav->useLowGradeFilter();
  sleep(2);

  printf("########## Client HIGH ##########\n");
  nav->useHighGradeFilter();
  sleep(2);

  printf("########## Client TYPE ##########\n");
  printf("Filter type is %d\n", nav->getFilterType());
  sleep(2);

  printf("########## Client SET_FR ##########\n");
  nav->setFilterReinit(true);
  sleep(2);

  printf("########## Client SET_MW ##########\n");
  nav->setModifiedWeighting(true);
  sleep(2);

  printf("########## Client SET_MIM ##########\n");
  nav->setMapInterpMethod(3);
  sleep(2);

  printf("########## Client STATE ##########\n");
  printf("Filter state = %d\n", nav->getFilterState());
  sleep(2);

  printf("########## Client REINITS ##########\n");
  printf("Num reinits = %d\n", nav->getNumReinits());
  sleep(2);

  exit(0);

}