#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <unix.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/ioctl.h> // IO Control of socket.

#include "Syslog.h"
#include "System.h"
#include "AuvTimeSyncIF.h"

//
////////////////////////////////////////////////////////////////////////////////
//
// PURPOSE:  To start up the AtsServer test task.
// AUTHOR:   Rich Henthorn
// DATE:     06/07/07
// COMMENTS: 
//
////////////////////////////////////////////////////////////////////////////////
//
//

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

AuvTimeSyncIF *atsServer;
struct sockaddr_in  servaddr;
int setupSocket(short port)
{
  int  sockfd = -1;

  // Attempt to setup a UDP socket on the Ats port
  // We will time time packets to the Ats driver
  //
  
  bzero((char*) &servaddr, sizeof(servaddr));
  servaddr.sin_family      = AF_INET;
  inet_aton("127.0.0.1", &servaddr.sin_addr);
  servaddr.sin_port        = htons((short)port);

  if (0 == servaddr.sin_addr.s_addr)
  {
    Syslog::write("atsTest: Could not resolve ip address");
    return -1;
  }

  // Get a socket resource
  //
  if ( (sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
  {
    Syslog::write("atsTest: Failed to initialize socket");
    return sockfd;
  }

  // Socket is working
  //
  Syslog::write("atsTest: Socket created on fd %d", sockfd);
  
  return sockfd;
}

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

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

  int port = atoi(argv[1]);
  int sockfd;
  if ( (sockfd = setupSocket(port)) < 0)
  {
    Syslog::write("atsTest: socket init failed (socket() call = %d)", sockfd);
    exit(0);
  }

  // Start AtsServer/Driver combo
  //
  int servPid = 0;
  if ( (servPid = System::spawn("atsServer", argv[1])) == -1) {
    fprintf(stderr, "System::spawn(atsServer) failed\n");
    Syslog::write("atsTest: System::spawn(atsServer) failed\n");
    exit( -1 );
  }

  // Acquire a AuvTimeSyncIF referencing the server we just started
  //
  atsServer = 0;
  try {
    atsServer = new AuvTimeSyncIF("Ats");
  }
  catch (Exception e) 
  {
    Syslog::write(
        "atsTest -- Caught exception on creation of AuvTimeSyncIF");
    exit( -1 );
  }
  
  long off = 0;
  Syslog::write("\nDrift should increase by a millisecond each 5 messages\n");
  sleep(1);
  Syslog::write("Here we go...");
  sleep(1);
  for (int it = 0; 1; it++)
  {
    char msg[400];
    struct timespec now;
    off += it/5;    // increase the time offset by one every 5 iterations

    // Get the system time and send it to the listening AtsServer
    // at 5 Hz. Force a 1 millisecond drift every second
    // so we can observe it
    //
    System::milliSleep(500);
    clock_gettime(CLOCK_REALTIME, &now);
    double resonTime = (double)now.tv_sec + ((double)(now.tv_nsec+off*1.e+06))/1.e+09;

    int n = sendto(sockfd, &resonTime, sizeof(double), 0,
                   (struct sockaddr*)&servaddr, sizeof(servaddr));
    if (5 == it) it = 0;

    // The time offset should be visible
    //    
    float delta, drift;
    atsServer->getDrift(&delta);
    atsServer->getDrift(&drift);
    printf("atsTest: Test resonTime: %.3f\tDelta and Drift %.3f and %.3f\n",
            resonTime, delta, drift);
  }

  return 0;

}

pid_t serverPID;

void signalHandler(int signalNo)
{
  kill(serverPID, SIGTERM);

  cleanup();
  exit(0);
}

void cleanup()
{
  if (atsServer) delete atsServer;
}