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

#define PACKET_SIZE 363    // Number of double values in packet

void usage()
{
  printf("Usage: client_test IP.AD.DR.ESS PORT\n");
}

int main(int argc, char **argv)
{
  struct timeval tv;
  char *_server_ip;
  short _server_port;
  struct sockaddr_in _server_addr;
  int _sockfd;

  if (argc != 3) {
    usage();
    exit(0);
  }
  _server_ip = argv[1];
  _server_port = atoi(argv[2]);

  if ( (_sockfd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP)) < 0 ) {
    printf("client_test: can't create socket", errno);
  }

  ///////////////////////////////////////////////////////////////////////
  // Connect to server
  //
  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("client_test: can't connect to server at %s on port %d: %d\n",
	   _server_ip, _server_port, errno);
    exit(1);
  }

  // Wait up to 15 seconds for a response
  //
  tv.tv_sec = 15;
  tv.tv_usec = 0;
  setsockopt(_sockfd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));

  {
    /////////////////////////////////////////////////////////////////////////
    // Send a request for ranges
    //
    // Build a range request packet
    //
    double data_buf[PACKET_SIZE];
    char  *pbuf = (char *)data_buf;
    size_t buf_size = sizeof(data_buf);
    int len, i;

    data_buf[0] = 34.5;   // Latitude
    data_buf[1] = 123.4;  // Longitude
    data_buf[2] = 56.7;   // Depth meters

    // Beam unit vectors
    //
    for (i = 0; i < 120; i++) {
      int idx = i*3 + 3;
      data_buf[idx]   = i;   // X in NED frame
      data_buf[idx+1] = i;   // Y in NED frame
      data_buf[idx+2] = i;   // Z in NED frame
    }


    // Send the request packet
    //
    for (len = 0; len < buf_size;)
      len += send(_sockfd, pbuf+len, buf_size-len, 0);

    printf("Client: Sent %d bytes to server\n", len);

    // Read the reponse
    //
    for (len = 0; len < buf_size;)
      len += recv(_sockfd, pbuf+len, buf_size-len, 0);

    // Take a look
    //
    printf("\nClient: Response from server:\n");
    for (i = 0; i < 120; i++)
      printf("%3.1f ", data_buf[i]);

    printf("\n");
  }

  return 0;
}