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


#include "StatePacketSender.h"

int setupSocket(char *, long, int *, struct sockaddr_in &);

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

  StatePacketSender *packetSender = new StatePacketSender();

  char *clientIPName = "127.0.0.1";
  if (argc == 2) {
    clientIPName = argv[1];
  }

  long clientPort = 8002;
  int socketFd;
  struct sockaddr_in servAddr;

  if (setupSocket(clientIPName, clientPort, &socketFd, servAddr)) {
    return -1;
  }

  StatePacket packet;

  int sec = 0;
  float depth = 0.;
  while (1) {

    // Set some fake values
    packet.attitude.updateTimeSec = sec++;
    packet.attitude.updateTimeNanosec = 0;
    packet.attitude.roll = 111.00;
    packet.attitude.pitch = 222.00;
    packet.attitude.yaw = 333.00;
    packet.position.updateTimeSec = sec++;
    packet.position.updateTimeNanosec = 0;
    packet.position.z = depth;
    depth += 1.;
    // Send the packet
    packetSender->send(&packet, socketFd);
    sleep(5);
  }

  return 0;
}

int setupSocket(char *clientIPName, long clientPort, int *socketFd, 
		struct sockaddr_in &servaddr) {
  
  
  // Attempt to setup a UDP client socket on the AMC port
  // We will send state packets to the AMC server
  //
  bzero((char*) &servaddr, sizeof(servaddr));
  servaddr.sin_family      = AF_INET;
  inet_aton(clientIPName, &servaddr.sin_addr);
  servaddr.sin_port        = htons((short)clientPort);

  if (0 == servaddr.sin_addr.s_addr)
  {
    fprintf(stderr, "ERROR looking up host address \"%s\"; use 'dot' notation to specify address\n",
	    clientIPName);
    return -1;
  }

  // Get a socket resource
  //
  if ((*socketFd = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
  {
    fprintf(stderr,
	    "StatePublisher: socket init failed (socket() call = %d)", 
	    *socket);

    return -1;
  }

  return 0;
}