#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"
//
////////////////////////////////////////////////////////////////////////////////
//
// PURPOSE:  To test the VcsAgent task.
// AUTHOR:   Rich Henthorn
// DATE:     02/07/07
// COMMENTS: 
//
////////////////////////////////////////////////////////////////////////////////
//
//

#define MISSION_CMD1 "init|behavior missionTimer { timeOut=80;} \
behavior depthEnvelope{minDepth=0;maxDepth=1000;} \
behavior setpoint {id = 0; duration = 3; \
heading      = 35.0; speed = 1.0; verticalMode = pitch; pitch = 0.0;}"

#define MISSION_CMD2 "insert|behavior setpoint {id = 200; duration = 5; \
heading      = 35.0; speed = 1.0; verticalMode = pitch; pitch = 0.0;}"

#define MISSION_CMD3 "insert|behavior setpoint {id = 300; duration = 5; \
heading      = 35.0; speed = 1.0; verticalMode = pitch; pitch = 0.0;}"

#define MISSION_CMD4 "insert|behavior setpoint {id = 400; duration = 5; \
heading      = 35.0; speed = 1.0; verticalMode = pitch; pitch = 0.0;}"

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

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

  if (argc != 4)
  {
    printf("usage: vcsAgentTest  IP.dot.nota.tion  port  string\n");
    exit(0);
  }
  
  signal(SIGINT,  signalHandler);
  signal(SIGTERM, signalHandler);
  signal(SIGQUIT, signalHandler);
  atexit(cleanup);

  ////////////////////////////////////////////////////////////////////////
  // Set up socket for talking to VcsAgent
  //
  int sockfd;
  struct sockaddr_in agent_addr;

  bzero((char*)&agent_addr, sizeof(agent_addr));
  agent_addr.sin_family = AF_INET;
  int translate = inet_aton(argv[1], &agent_addr.sin_addr);
  agent_addr.sin_port = htons(atoi(argv[2]));
  
  if (translate == 0)
  {
    printf("vcsAgentTest: Failed to translate %s. Errno=%d\n", argv[1],
                                                                    errno);
    exit(0);
  }
  
  printf("vcsAgentTest: Connecting to port %s\n", argv[2]);
  if ((sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0)
  {
    printf("vcsAgentTest: failed to create socket. Errno=%d\n", errno);
    exit(0);
  }
  
  if (connect(sockfd, (struct sockaddr*)&agent_addr, sizeof(agent_addr)) < 0)
  {
    printf("vcsAgentTest: failed to connect socket. Errno=%d\n", errno);
    exit(0);
  }
  // Done with socket set-up
  ////////////////////////////////////////////////////////////////////////

  // Now send some test messages to the VcsAgent
  //    
  sleep(1);
  int n;
  
  // First, send the string argument
  //
  if ((n = send(sockfd, argv[3], strlen(argv[3])+1, 0)) < 1)
  {
    printf("vcsAgentTest: failed to send %s. Errno=%d\n", argv[3], errno);
    exit(0);
  }
  printf("vcsAgentTest: Sent %s, %d bytes\n", argv[3], n);

  // Read back the ack/nack from VcsAgent
  //
#if 0
  char buf[128];
  n = recv(sockfd, buf, sizeof(buf), 0);
  if (n > 0) printf("vcs Test: received %d bytes, %s\n", n, buf);
#endif
  sleep(1);

  // Now, send a test mission script
  //
  if ((n = send(sockfd, MISSION_CMD1, strlen(MISSION_CMD1)+1, 0)) < 1)
  {
    printf("vcsAgentTest: failed to send mission. Errno=%d\n", errno);
    exit(0);
  }
  printf("vcsAgentTest: Sent mission, %d bytes\n", n);

  // Now, send a ping
  //
  if ((n = send(sockfd, "ping", strlen("ping")+1, 0)) < 1)
  {
    printf("vcsAgentTest: failed to send mission. Errno=%d\n", errno);
    exit(0);
  }
  printf("vcsAgentTest: Sent ping, %d bytes\n", n);

  // Read back the ack/nack from VcsAgent
  //
#if 0
  n = recv(sockfd, buf, sizeof(buf), 0);
  if (n > 0) printf("vcs Test: received %d bytes, %s\n", n, buf);
#endif
  sleep(5);

  // Now, send a ping
  //
  if ((n = send(sockfd, "ping", strlen("ping")+1, 0)) < 1)
  {
    printf("vcsAgentTest: failed to send mission. Errno=%d\n", errno);
    exit(0);
  }
  printf("vcsAgentTest: Sent ping, %d bytes\n", n);

  // Send another
  //
  if ((n = send(sockfd, MISSION_CMD2, strlen(MISSION_CMD2)+1, 0)) < 1)
  {
    printf("vcsAgentTest: failed to send mission. Errno=%d\n", errno);
    exit(0);
  }
  printf("vcsAgentTest: Sent behavior, %d bytes\n", n);
  sleep(3);

  // Send another
  //
  if ((n = send(sockfd, MISSION_CMD3, strlen(MISSION_CMD3)+1, 0)) < 1)
  {
    printf("vcsAgentTest: failed to send mission. Errno=%d\n", errno);
    exit(0);
  }
  printf("vcsAgentTest: Sent behavior, %d bytes\n", n);
  sleep(2);

  if ((n = send(sockfd, "delete|300", strlen("delete|300")+1, 0)) < 1)
  {
    printf("vcsAgentTest: failed to send delete cmd. Errno=%d\n", errno);
    exit(0);
  }
  printf("vcsAgentTest: Sent delete, %d bytes\n", n);

  // Send another
  //
  if ((n = send(sockfd, MISSION_CMD4, strlen(MISSION_CMD4)+1, 0)) < 1)
  {
    printf("vcsAgentTest: failed to send mission. Errno=%d\n", errno);
    exit(0);
  }
  printf("vcsAgentTest: Sent behavior, %d bytes\n", n);
  sleep(5);

  // Read back the ack/nack from VcsAgent
  //
#if 0
  n = recv(sockfd, buf, sizeof(buf), 0);
  if (n > 0) printf("vcs Test: received %d bytes, %s\n", n, buf);
#endif

  sleep(30);  // Agent should kill LC by now
  
  // Finally, request an exit from the session
  //
  if ((n = send(sockfd, "Exit", 5, 0)) < 1)
  {
    printf("vcsAgentTest: failed to send Exit\n");
    exit(0);
  }

  // Read back the ack/nack from VcsAgent
  //
#if 0
  n = recv(sockfd, buf, sizeof(buf), 0);
  if (n > 0) printf("VCS Test: received %d bytes, %s\n", n, buf);
#endif
  
  close (sockfd);
  return 0;
}


void signalHandler(int signalNo)
{
  exit(0);
}

void cleanup()
{
}