#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#ifdef _QNX
#include <unix.h>
#else   //linix
#include <string.h>
#include <errno.h>
#endif
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/ioctl.h> // IO Control of socket.
#include "StatePacket.h"
//
////////////////////////////////////////////////////////////////////////////////
//
// PURPOSE:  To start up the SonardyneDriver driver task.
// AUTHOR:   Rich Henthorn
// DATE:     02/07/07
// COMMENTS: 
//
////////////////////////////////////////////////////////////////////////////////
//
//

void signalHandler(int signalNo);
void cleanup();

int main( int argc, char **argv )
{

  signal(SIGINT,  signalHandler);
  signal(SIGTERM, signalHandler);
  signal(SIGQUIT, signalHandler);
  atexit(cleanup);

  struct sockaddr_in  serv_addr;
  struct sockaddr cli_addr;
  int    sockfd, clilen;

  // Get a socket resource
  //
  if ( (sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
  {
    printf("statePublisherTest: socket init failed (socket() call = %d)", sockfd);
    exit(0);
  }

  // Attempt to setup a UDP client socket on the AMC port
  // We will send state packets to the AMC server
  //
  printf("statePublisherTest: Socket params - Port:%d\n", 8002);
  
  bzero((char*) &serv_addr, sizeof(serv_addr));
  serv_addr.sin_family      = AF_INET;
  serv_addr.sin_addr.s_addr = htonl(INADDR_ANY);
  serv_addr.sin_port        = htons(8002);

  int err;
  if (err = (bind(sockfd, (struct sockaddr*)&serv_addr, sizeof(serv_addr))) < 0)
  {
    printf("statePublisherTest: socket init failed (errno = %d)", errno);
    exit(0);
  }

  printf("statePublisherTest: going to listen for messages\n");
  fflush(stdout);

  StatePacket sp;
  while (1)
  {
    char msg[400];
    int n = recv(sockfd, &sp, sp.packetSize(), 0);//, &cli_addr, &clilen);
    if (n < 0)
      printf("statePublisherTest: recvfromerror\n");
    else if (n == sp.packetSize())
    {
      printf("\n\n");
      printf("statePublisherTest: Packet version:%2.2f\tTest data:%4.2f\n",
              sp._header.version, sp._data.test);

      printf("Navigation update time:  %15.2f\n", 
	     (double)sp._data.position.updateTime.tv_sec +
	     ((double)sp._data.position.updateTime.tv_nsec)/1.e9);

      printf("  Attitude in radians and r/s:\n");
      printf("     roll:  %5.2f\n", sp._data.attitude.roll);
      printf("    pitch:  %5.2f\n", sp._data.attitude.pitch);
      printf("      yaw:  %5.2f\n", sp._data.attitude.yaw);
      printf("omega_B_x:  %5.2f\n", sp._data.attitude.omega_B_x);
      printf("omega_B_y:  %5.2f\n", sp._data.attitude.omega_B_y);
      printf("omega_B_z:  %5.2f\n", sp._data.attitude.omega_B_z);
      printf("  Position:\n");
      printf("     latitude:  %9.4f\n", sp._data.position.latitude);
      printf("    longitude:  %9.4f\n", sp._data.position.longitude);

      printf("  Battery:\n");
      printf("      Voltage:  %.2f\n", sp._data.batteryData.voltage);
      printf("      Current:  %.2f\n", sp._data.batteryData.current);
      printf("        State:  %d\n",   sp._data.batteryData.state);
      printf("        Fault:  %d\n",   sp._data.batteryData.fault);

      printf("  Hydroscat:\n");
      printf("      snorm1:   %d\n", sp._data.hydroscat.snorm1);
      printf("      snorm2:   %d\n", sp._data.hydroscat.snorm2);
      printf("      snorm3:   %d\n", sp._data.hydroscat.snorm3);

    }
    else 
      printf("statePublisherTest: Error recv got %d bytes, and expected %d\n",
	     n, sp.packetSize());

    fflush(stdout);
  }

  return 0;

}


void signalHandler(int signalNo)
{

  //  cleanup();
  exit(0);
}

void cleanup()
{
}