#include "LibAard.h"
#include "AardRemoteMessage.h"
#include "Commands.h"
#include "LogAard.h"
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdlib.h>
#include <sys/select.h>
#include <string.h>
#include <sys/time.h>
#include <sys/ioctl.h>
#include <errno.h>
#include <stdio.h>

LibAard::LibAard()
 : fd(-1),
   has_error(true),
   orig_ts_valid(false),
   incoming(NULL),
   have_leftover_byte(false),
   logger(NULL) {
}

LibAard::~LibAard() {
   CloseAardRemote();
}

int
LibAard::OpenAardRemote(const char * devname) {
   CloseAardRemote();
   fd = open(devname, O_RDWR | O_NOCTTY | O_NONBLOCK);
   if (fd >= 0) {
      if (tcgetattr(fd, &orig_ts) == 0) {
         orig_ts_valid = true;
         struct termios ts = orig_ts;
         ts.c_iflag = IGNBRK;
         ts.c_oflag = 0;
         ts.c_lflag = 0;
         ts.c_cflag = (CS8 | CREAD | CLOCAL);
         ts.c_cc[VMIN]  = 1;
         ts.c_cc[VTIME] = 0;
         cfsetispeed(&ts, B115200);
         cfsetospeed(&ts, B115200);
         tcsetattr(fd, TCSANOW, &ts);
      } else {
         orig_ts_valid = false;
      }
   }
   has_error = (fd < 0);
   return fd;
}

void
LibAard::CloseAardRemote() {
   if (fd != -1) {
      if (orig_ts_valid) {
         tcsetattr(fd, TCSANOW, &orig_ts);
      }
      close(fd);
   }
   fd = -1;
   orig_ts_valid = false;
   have_leftover_byte = false;
   while (! outbox.empty()) {
      delete outbox.front();
      outbox.pop();
   }
   has_error = true;
}

void
LibAard::WaitForStartup() {
   if (HasError()) {
      return;
   }
   unsigned char badbyte = 47;
   unsigned char wantbyte;
   write_and_log((void *)&badbyte, 1);
   int wait_time = 0;
   int state = 0;
   while (state != 99) {
      AardRemoteMessage * msg = WaitMessage(wait_time);
      if (HasError()) {
         return;
      }
      wait_time = 500;
      if (msg == NULL) {
         break;
      }
      switch (state) {
      case 0:
         if ((msg->MessageKind() == MK_ASYNC) && 
             (msg->FunctionCode() == MKAS_RESET)) {

            // We got a reset message meaning the device was just
            // restarted, but the bad command we wrote may or may
            // not have gotten through, so write another one now that
            // we know it is really talking.

            wantbyte = 48;
            write_and_log((void *)&wantbyte, 1);
            state = 1;
         } else if ((msg->MessageKind() == MK_ASYNC) && 
                    (msg->FunctionCode() == MKAS_BILGE)) {
            if ((msg->MessageLength() == 2) &&
                (msg->MessageBody()[1] == 47)) {
               // We saw the initial badbyte we wrote. We are probably
               // in sync, but write another one just to make sure
               wantbyte = 49;
               write_and_log((void *)&wantbyte, 1);
               state = 1;
            }
         }
         if (state == 0) {

            // We didn't see either of the responses we thought we
            // might get. No idea what is happening here, just write
            // the 47 out again and loop back to top with state still 0

            write_and_log((void *)&badbyte, 1);
         }
         break;
      case 1:

         // We wrote a new wantbyte value in previous state, now make sure
         // we see this new value in an error response.

         if ((msg->MessageKind() == MK_ASYNC) && 
             (msg->FunctionCode() == MKAS_BILGE) &&
             (msg->MessageLength() == 2)) {
            if (msg->MessageBody()[1] == wantbyte) {

               // Finally hit the wantbyte message, we are almost certainly
               // synced up now. Try asking for the microcode version
               // and make sure we get a valid response to that.

               SendCommand(MKCR_QVERSION);
               state = 2;
            }
         } else {
            // No idea what is happening. Write out the initial 47 again
            // and try back at state 0
            write_and_log((void *)&badbyte, 1);
            state = 0;
         }
         break;
      case 2:
         if ((msg->MessageKind() == MK_REPLY) &&
             (msg->FunctionCode() == MKCR_QVERSION)) {
            state = 99;
         } else {
            // Again I'm confused!!! Start over one more time...
            write_and_log((void *)&badbyte, 1);
            state = 0;
         }
      }
      delete msg;
      msg = NULL;
   }
}

void
LibAard::SendCommand(unsigned char cmdcode) {
   unsigned char cmd[2];
   cmd[0] = MK_COMMAND | 1;
   cmd[1] = cmdcode;
   write_and_log((void *)&cmd[0], sizeof(cmd));
}

void
LibAard::SendCommand(unsigned char cmdcode, unsigned short cmdata) {
   unsigned short cmd[2];
   unsigned char * c = (unsigned char *)&cmd[0];
   c[0] = MK_COMMAND | 2;
   c[1] = cmdcode;
   cmd[1] = cmdata;
   write_and_log((void *)&cmd[0], sizeof(cmd));
}

void
LibAard::SendCommand(
   unsigned char cmdcode,
   const unsigned short * cmdata,
   int len) {

   unsigned int totlen = len + 1;
   unsigned char hdrlen = MSG_LEN(totlen);
   if (hdrlen != totlen) {

      // This message is too long to pass length in MSG_LEN field,
      // so send it as a variable length message terminated with 0x0000.

      ++totlen;
      hdrlen = 0;
   }
   unsigned short * fullmsg = new unsigned short [totlen];
   unsigned char * c = (unsigned char *)fullmsg;
   c[0] = MK_COMMAND | hdrlen;
   c[1] = cmdcode;
   memcpy((void *)&fullmsg[1], (void *)cmdata, len * sizeof(unsigned short));
   if (hdrlen == 0) {
      fullmsg[totlen-1] = 0x0000;
   }
   write_and_log((void *)fullmsg, totlen * 2);
   delete [] fullmsg;
}

void
LibAard::SendCommand(
   unsigned char cmdcode,
   const std::vector<unsigned short> & dat) {
   SendCommand(cmdcode, &dat.front(), dat.size());
}

int
LibAard::ReadMoreData() {
   // If I have an "incoming" AardRemoteMessage then I've seen some, but
   // not all the data expected to show up, and I need to read more
   // before I can append the last of the data to incoming and call it
   // complete.
   //
   // The messages should come in multiples of 2 byte chunks, but I may
   // also have an odd leftover byte from the previous reads so I need to
   // make sure I don't lose it.

   unsigned char inbuf[1024];
   unsigned char * bufp = &inbuf[0];
   unsigned int bufsize = sizeof(inbuf);
   if (have_leftover_byte) {
      *bufp++ = leftover_byte;
      --bufsize;
      have_leftover_byte = false;
   }
   while (bufsize > 0) {

      // If select() claims there is data available, but we read zero
      // bytes, that means we are actually at EOF and the device
      // was closed as far as the kernel is concerned, so call that
      // an error.

      fd_set rs;
      struct timeval tv;
      FD_ZERO(&rs);
      FD_SET(fd, &rs);
      tv.tv_sec = 0;
      tv.tv_usec = 0;
      int selstat = select(fd + 1, &rs, NULL, NULL, &tv);
      ssize_t inbytes = read_and_log((void *)bufp, bufsize);
      if (inbytes > 0) {
         bufsize -= inbytes;
         bufp += inbytes;
      } else if (inbytes < 0) {
         if (errno == EAGAIN) {
            break;
         }
         if (errno != EINTR) {
            has_error = true;
            break;
         }
      } else if ((inbytes == 0) && (selstat > 0)) {
         has_error = true;
         break;
      } else {
         break;
      }
   }

   // Now I have as much data as is available (or will fit, whichever comes
   // first) in the buffer, so figure out what to do with it.

   bufsize = bufp - &inbuf[0];
   bufp = &inbuf[0];
   while (bufsize >= 2) {
      if (incoming == NULL) {

         // I have no message under construction. The data in the buffer
         // should therefore be the header at the start of a new message.
         // (If it isn't, I have no idea what to do, so I won't check :-).

         incoming = new AardRemoteMessage(*(unsigned short *)bufp);
         bufp += 2;
         bufsize -= 2;
         IncomingToOutbox();
      } else {

         // Extract the next word and push on incoming (or terminate
         // incoming if it is the zero at the end of a variable length
         // message).

         unsigned short nextword = *(unsigned short *)bufp;
         bufp += 2;
         bufsize -= 2;
         if ((incoming->MessageLength() == 0) && (nextword == 0)) {
            outbox.push(incoming);
            incoming = NULL;
         } else {
            incoming->Append(nextword);
            IncomingToOutbox();
         }
      }
   }
   if (bufsize == 1) {
      leftover_byte = *bufp;
      have_leftover_byte = true;
   }
   return outbox.size();
}

AardRemoteMessage *
LibAard::PollMessage() {
   AardRemoteMessage * rval = NULL;
   if (ReadMoreData() > 0) {
      rval = outbox.front();
      outbox.pop();
   }
   return rval;
}

AardRemoteMessage *
LibAard::WaitMessage(unsigned int milliseconds) {
   if (! outbox.empty()) {
      return PollMessage();
   }
   struct timeval target_time;
   if (milliseconds > 0) {
      gettimeofday(&target_time, NULL);
      unsigned int secs = milliseconds / 1000u;
      unsigned int msecs = milliseconds - (1000u * secs);
      target_time.tv_sec += secs;
      target_time.tv_usec += (1000u * msecs);
      while (target_time.tv_usec > 1000000u) {
         target_time.tv_usec--;
         target_time.tv_sec++;
      }
   }
   for ( ; ; ) {
      struct timeval tv;
      if (milliseconds > 0) {
         // Compute time to wait so that we'll return from this
         // function "milliseconds" after we were first called.
         struct timeval now;
         gettimeofday(&now, NULL);
         // Compute tv = target_time - now;
         if ((now.tv_sec > target_time.tv_sec) ||
             ((now.tv_sec == target_time.tv_sec) &&
              (now.tv_usec >= target_time.tv_usec))) {
            // Hey! Time has expired. Return NULL;
            return NULL;
         }
         tv.tv_sec = target_time.tv_sec - now.tv_sec;
         if (now.tv_usec > target_time.tv_usec) {
            tv.tv_sec -= 1;
            tv.tv_usec = (target_time.tv_usec + 1000000) -
                         now.tv_usec;
         } else {
            tv.tv_usec = target_time.tv_usec - now.tv_usec;
         }
      } else {
         // We never get out of select on a file descriptor error
         // if we wait forever, so just wait 1/2 second even if the
         // caller wants us to wait forever so we'll try a read every
         // so often and then get an I/O error on the bad descriptor.
         tv.tv_sec = 0;
         tv.tv_usec = 500000;
      }
      fd_set rs;
      FD_ZERO(&rs);
      FD_SET(fd, &rs);
      int selstat = select(fd + 1, &rs, NULL, NULL, &tv);

      // GAAH! I don't get an error from select() when the FD goes bad.
      // I need to make the timeout here never be zero and poll occasionally
      // anyway just to notice when the descriptor goes bad...

      if (selstat >= 0) {
         AardRemoteMessage * msg = PollMessage();
         if (msg != NULL) {
            return msg;
         }
         if (HasError()) {
            return NULL;
         }
      } else if (errno != EINTR) {
         has_error = true;
         return NULL;
      }
   }
}

bool
LibAard::HasError() {
   return has_error;
}

void
LibAard::SetLogger(LogAard * new_logger) {
   logger = new_logger;
}

ssize_t
LibAard::write_and_log(const void * buf, size_t count) {
   ssize_t rval = write(fd, buf, count);
   if (logger) logger->LogWrite(rval, buf, count);
   return rval;
}

ssize_t
LibAard::read_and_log(void * buf, size_t count) {
   ssize_t rval = read(fd, buf, count);
   if (logger) logger->LogRead(rval, buf, count);
   return rval;
}

void
LibAard::IncomingToOutbox() {
   if (incoming != NULL) {
      if (incoming->MessageLength() == incoming->message.size()) {

         // This incoming message is now long enough. We can push it on
         // the end of the outbox and NULL out incoming.

         outbox.push(incoming);
#ifdef DEBUGOUT
         printf("pushed:");
         incoming->DebugDump();
#endif
         incoming = NULL;
      }
   }
}
