#include <sys/socket.h>
#include <sys/select.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <termios.h>
#include <sys/stat.h>
#include <sys/uio.h>
#include <fcntl.h>
#include <errno.h>
#include <unix.h>

#include "EZ17.h"

struct sockaddr_in mod_addr;
struct timeval     mod_tv;

int udp_open(char *ip, int port)
{
   // Set up socket for comms to em (serial 1 on the Modtronix board)
   // Modtronix will receive and send data from/to us
   //
   int mod_fd;
   if ( (mod_fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0 ) {
      printf("can't create datagram socket\n", errno);
      return mod_fd;
   }

   mod_addr.sin_family      = AF_INET;
   mod_addr.sin_addr.s_addr = inet_addr(ip);
   mod_addr.sin_port        = htons(port);

   // Connect client socket
   if ( connect(mod_fd, (struct sockaddr *)&mod_addr, sizeof(mod_addr)) < 0) {
      printf("can't connect dgram socket: %d\n", errno);
      perror(NULL);
      return -1;
   }
   else {
      // For socket option SO_RCVTIMEO
      //
      mod_tv.tv_sec = 1L;
      mod_tv.tv_usec = 500000L;

      if ( setsockopt(mod_fd, SOL_SOCKET, SO_RCVTIMEO,
               (const void **)&mod_tv, sizeof(struct timeval)) < 0) {
        printf("setsockopt() SO_RCVTIMEOUT failed: %d\n", errno);
        perror(NULL);
        return -1;
      }
   }

   printf("ready to send/receive datagrams from %s on port %d...\n",
      ip, port);
   return mod_fd;
}

send_msg(int fd, char *msg)
{
   char buf[40]; buf[0] = '\0';
   int cl = strlen(buf) + 1;
   if (strcmp("\r\n", msg))
   {
     sprintf(buf, "%s\r", msg);
     cl = strlen(buf);
   }
   else
   {
     sprintf(buf, "%s", msg);
     cl = strlen(buf) + 1;
   }

   printf("| send_msg - sending %d bytes: '%s'\n", cl, buf);

   //int sl = send(fd, buf, cl, 0);
   int sl = write(fd, buf, cl);
   if (sl < 0 || sl != cl)
   {
      printf("| send_msg - issue sending %s: %d", msg, sl);
      perror(NULL);
   }
}

read_msg(int fd, char *msg)
{
   int sl = 0, nbytes = 0;
   //while ((sl = recv(fd, msg+nbytes, 32-nbytes, 0)) > 0)
   while ((sl = read(fd, msg+nbytes, 32-nbytes)) > 0)
      nbytes += sl;

}

home(int fd)
{
  send_msg(fd, "/1V80m90ZR");
  sleep(12);
  send_msg(fd, "/1V80m90z660R");
}


f56(int fd)
{
  send_msg(fd, "/1V80m90A205R");
  sleep(8);
}

// Return 1 if not moving. Otherwise 0 and sets value pointed to by
// old_pos to the current position.
// 
int is_move_complete(EZ17 *ez, int *old_pos)
{
   int result = 0;

   int pos = ez->read_position();
   if (pos != *old_pos)
   {
      *old_pos = pos;
      printf("ezTalk::wait_for_move() - still moving at position %d\n", pos);
   }
   else
   {
      printf("ezTalk::in_move_complete() - Done!\n");
      result = 1;
   }

   return result;
}

int main(int argc, char **argv)
{
   char *ip = "134.89.32.55";
   int  port = 54125;
   if (argc == 3)
   {
      ip = argv[1];
      port = atoi(argv[2]);
   }

   int fd = udp_open(ip, port);

   if (fd < 0)
      return 1;

   EZ17 *ez = new EZ17(fd);
   int motor_pos;

   // Ask for a string to send to the ez stepper; send it; report the response
   //
   while (1)
   {
     printf("|\n| ezTalk v2: Enter your EZ Stepper command string > " );

     char response[3200];
     char *c = gets(response);

     // Got a response?
     //
     //if (strlen(response) == 0) continue;
     if (!strcmp(response, "crlf"))
     {
       send_msg(fd, "\r\n");
       read_msg(fd, response);
       printf("response to CRLF: %s\n", response);
     }

     else if (!strcmp(response, "in_move"))
     {
       if (is_move_complete(ez, &motor_pos))
         printf("|\n| Move is complete at %d\n", motor_pos);
       else
         printf("|\n| Move in progress at position %d\n", motor_pos);
     }
     else if (!strcmp(response, "home"))
     {
//       home(fd);
//       send_msg(fd, "/1?0");
//       read_msg(fd, response);
       ez->home();
//       ez->send_msg("/1V80m90ZR");
//       ez->read_msg();
       motor_pos = 0;
       while (!is_move_complete(ez, &motor_pos)) sleep(1);
       ez->read_position();
       ez->set_position(660);
       ez->read_msg();
       ez->read_position();
     }
     else if (!strcmp(response, "stop") || !strcmp(response, "halt"))
     {
       ez->halt();
     }
     else if (!strcmp(response, "exit") || !strcmp(response, "quit"))
     {
       break;
     }
     else if (!strncmp(response, "S", 1))
     {
       int steps = atoi(response+1);
       if (steps > 0 && steps < 661)
       {
         ez->goto_steps(steps);
         while (!is_move_complete(ez, &motor_pos)) sleep(1);
       }
     }
     else if (!strcmp(response, "f5.6"))
     {
       ez->goto_pos(EZ17::F56);
       while (!is_move_complete(ez, &motor_pos)) sleep(1);
       ez->send_msg("/1?0");
       ez->read_msg();
       ez->read_position();
//       f56(fd);
//       send_msg(fd, "/1?0");
//       read_msg(fd, response);
     }
     else
     {
       printf("Sending %s\n", response);
       if (strlen(response) > 0) ez->send_msg(response);
       ez->read_msg();
       //printf("%s\n", response);
//       send_msg(fd, response);
//       response[0] = '\0';
//       read_msg(fd, response);
//       printf("| The response is '%s'\n", response);
     }
   }

   return 0;
}