#include "Arduino.h"
#include "BackgroundIR.h"
#include "Commands.h"
#include "AardErrors.h"

#include <avr/eeprom.h>
#include <avr/wdt.h>

void setup();
void loop();
void do_qversion();
void do_receiver();
void do_qreceiver();
void do_frequency();
void do_qfrequency();
void do_qmicros();
void do_sendraw(int);
void do_waitavail();
void do_waitsend();
void do_queuesize();
void do_reset();
void do_unrecognized_command(byte, int);
int get_char();
unsigned int get_word();
void discard_words(int);
void async_send_raw_ir();
void async_send_glob_err(unsigned int);
void async_send_garbage(unsigned int);
void async_send_reset();
void send_nack(unsigned int);
void send_ack(byte);
void send_word(byte, unsigned int);
void send_long(byte, unsigned long);
void send_uint(unsigned int);

union CommandWord {    // Highly AVR arch specific union!
   unsigned char  b[2];
   unsigned int   i;
};

union CommandLong {    // Highly AVR arch specific union!
   unsigned char  b[4];
   unsigned int   i[2];
   unsigned long  l;
};

void
setup() {
   wdt_disable();
   Serial.begin(115200);
   BackgroundIR::InitializeIR();
   async_send_reset();
}

// Main loop for the AardRemote arduino microcode.

void
loop() {
   byte b0;

   // Read first byte and make sure it is flagged as a command, keep
   // reading till we find a valid command byte.

   for ( ; ; ) {
      b0 = get_char();
      if (MSG_KIND(b0) == MK_COMMAND) {
         break;
      }
      async_send_garbage(b0);
   }

   // Having gotten a valid command byte, extract the length info
   // and read the next byte (the command code).

   int cmdlen = ((int)(MSG_LEN(b0))) - 1;
   byte command_code = get_char();

   // Validate the length of the command for the commands which expect
   // to get fixed length messages so the individual commands don't
   // have to worry about replicating the length checking code.

   switch (command_code) {
   case MKCR_QVERSION:
   case MKCR_QRECEIVER:
   case MKCR_QFREQUENCY:
   case MKCR_QMICROS:
   case MKCR_WAITAVAIL:
   case MKCR_WAITSEND:
   case MKCR_QUEUESIZE:
      if (cmdlen != 0) {
         discard_words(cmdlen);
         send_nack(ERR_TOO_MUCH_DATA);
         return;
      }
      break;
   case MKCR_RECEIVER:
   case MKCR_FREQUENCY:
      if (cmdlen != 1) {
         if (cmdlen == 0) {
            send_nack(ERR_MISSING_DATA);
         } else {
            discard_words(cmdlen);
            send_nack(ERR_TOO_MUCH_DATA);
         }
         return;
      }
      break;
   }

   // Now actually process the commands (if we made it this far)

   switch (command_code) {
   case MKCR_QVERSION:
      do_qversion();
      break;
   case MKCR_RECEIVER:
      do_receiver();
      break;
   case MKCR_QRECEIVER:
      do_qreceiver();
      break;
   case MKCR_FREQUENCY:
      do_frequency();
      break;
   case MKCR_QFREQUENCY:
      do_qfrequency();
      break;
   case MKCR_QMICROS:
      do_qmicros();
      break;
   case MKCR_SENDRAW:
      do_sendraw(cmdlen);
      break;
   case MKCR_WAITAVAIL:
      do_waitavail();
      break;
   case MKCR_WAITSEND:
      do_waitsend();
      break;
   case MKCR_QUEUESIZE:
      do_queuesize();
      break;
   case MKCR_RESET:
      do_reset();
      break;
   default:
      do_unrecognized_command(command_code, cmdlen);
      break;
   }
}

void
do_qversion() {
   send_word(MKCR_QVERSION, AARDREMOTE_CURRENT_VERSION);
}

void
do_receiver() {
   if (get_word() == 0) {
      BackgroundIR::StopReceivingIR();
   } else {
      BackgroundIR::StartReceivingIR();
   }
   send_ack(MKCR_RECEIVER);
}

void
do_qreceiver() {
   send_word(MKCR_QRECEIVER,
             (unsigned int)BackgroundIR::ReceiverNormallyRunning());
}

void
do_frequency() {
   unsigned int freq = get_word();
   bool rval = BackgroundIR::SetIRModulation((int)freq);
   if (rval) {
      send_ack(MKCR_FREQUENCY);
   } else {
      send_nack(ERR_BAD_FREQUENCY);
   }
}

void
do_qfrequency() {
   send_word(MKCR_QFREQUENCY, (unsigned int)BackgroundIR::GetIRModulation());
}

void
do_qmicros() {
   send_long(MKCR_QMICROS, micros());
}

void
do_sendraw(int cmdlen) {
   unsigned int count = 0;
   unsigned int pulse;
   if (cmdlen == -1) {
      while ((pulse = get_word()) != 0) {
         if (! BackgroundIR::PutData(pulse)) {
            discard_words(cmdlen);
            break;
         }
         ++count;
      }
   } else {
      while (cmdlen > 0) {
         pulse = get_word();
         --cmdlen;
         if (! BackgroundIR::PutData(pulse)) {
            discard_words(cmdlen);
            break;
         }
         ++count;
      }
   }
   send_word(MKCR_SENDRAW, count);
}

void
do_waitavail() {
   while (! BackgroundIR::PutDataOK()) {
      delayMicroseconds(100);
   }
   send_ack(MKCR_WAITAVAIL);
}

void
do_waitsend() {
   while (BackgroundIR::GetState() == BackgroundIR::BGIR_SENDING) {
      delayMicroseconds(100);
   }
   send_ack(MKCR_WAITSEND);
}

void
do_queuesize() {
   send_word(MKCR_QUEUESIZE, (unsigned int)BackgroundIR::GetBufferSize());
}

void
do_reset() {
   uint8_t ch;
   ch = eeprom_read_byte((uint8_t *)0);
   ch |= 0x01;
   eeprom_write_byte((uint8_t *)0, ch);
   wdt_enable(WDTO_2S);
   send_ack(MKCR_RESET);
   while (1) ;
}

void
do_unrecognized_command(byte command_code, int cmdlen) {
   discard_words(cmdlen);
   send_nack(ERR_UNKNOWN_COMMAND);
}

// Wait for and return next incoming character and do other background
// processing while waiting for characters to show up.
//
int
get_char() {
   while (Serial.available() <= 0) {
      if (BackgroundIR::GetState() == BackgroundIR::BGIR_HAS_DATA) {
         async_send_raw_ir();
      }
      unsigned int errs = BackgroundIR::GetGlobalErrors();
      if (errs != 0) {
         async_send_glob_err(errs);
      }
   }
   return Serial.read();
}

// Wait for and return a two byte word from input stream
//
unsigned int
get_word() {
   CommandWord rval;
   rval.b[0] = get_char();
   rval.b[1] = get_char();
   return rval.i;
}

void
discard_words(int cmdlen) {
   // If cmdlen == -1, read words till we read 0x0000
   // If cmdlen > 0, read cmdlen words

   if (cmdlen == -1) {
      while (get_word() != 0x0000) ;
   } else {
      while (cmdlen-- > 0) {
         get_word();
      }
   }
}

void
async_send_raw_ir() {
   // If there are a few IR codes in the buffer, send them in fixed
   // length message. If there are a lot, send them in variable
   // length message.

   unsigned int msg[5];
   union CommandWord h;
   unsigned int len = 1;
   h.b[0] = MK_ASYNC;
   h.b[1] = MKAS_RAWIR;

   while ((len < 5) &&
          (BackgroundIR::GetState() == BackgroundIR::BGIR_HAS_DATA)) {
      msg[len++] = BackgroundIR::GetData();
   }
   if ((len == 5) && (BackgroundIR::GetState() == BackgroundIR::BGIR_HAS_DATA)) {
      // Send variable length message till we run out of data.
      msg[0] = h.i;
      Serial.write((unsigned char *)&msg[0], len * 2);
      while (BackgroundIR::GetState() == BackgroundIR::BGIR_HAS_DATA) {
         send_uint(BackgroundIR::GetData());
      }
      send_uint(0x0000);
   } else {
      // Send fixed length message with the data that was available
      h.b[0] |= len;
      msg[0] = h.i;
      Serial.write((unsigned char *)&msg[0], len * 2);
   }
}

void
async_send_glob_err(unsigned int errcode) {
   // Compose an MKAS_GLOBERR message with the errcode in 2nd word
   // and shove it out the wire.

   union CommandWord r;
   r.b[0] = MK_ASYNC | 2;
   r.b[1] = MKAS_GLOBERR;
   send_uint(r.i);
   send_uint(errcode);
}

void
async_send_garbage(unsigned int bilge) {
   union CommandWord r;
   r.b[0] = MK_ASYNC | 2;
   r.b[1] = MKAS_BILGE;
   send_uint(r.i);
   send_uint(bilge);
}

void
async_send_reset() {
   union CommandWord r;
   r.b[0] = MK_ASYNC | 1;
   r.b[1] = MKAS_RESET;
   send_uint(r.i);
}

void
send_nack(unsigned int errcode) {
   // Compose a MKR_NACK reply message with the errcode in 2nd word
   // and send it out.

   union CommandWord r;
   r.b[0] = MK_REPLY | 2;
   r.b[1] = MKR_NACK;
   send_uint(r.i);
   send_uint(errcode);
}

void
send_ack(byte cmdcode) {
   // Compose a 1 word ACK reply with the command code and send
   // it out.

   union CommandWord r;
   r.b[0] = MK_REPLY | 1;
   r.b[1] = cmdcode;
   send_uint(r.i);
}

void
send_word(byte cmdcode, unsigned int dat) {
   // Compose a 2 word reply with the command code and the data
   // in 2nd word and send it out.

   union CommandWord r;
   r.b[0] = MK_REPLY | 2;
   r.b[1] = cmdcode;
   send_uint(r.i);
   send_uint(dat);
}

void
send_long(byte cmdcode, unsigned long dat) {
   // Compose a 3 word reply with the command code and the data
   // and send it out.

   union CommandWord r;
   r.b[0] = MK_REPLY | 3;
   r.b[1] = cmdcode;
   send_uint(r.i);

   union CommandLong l;
   l.l = dat;
   send_uint(l.i[0]);
   send_uint(l.i[1]);
}

void
send_uint(unsigned int v) {
   union CommandWord w;
   w.i = v;
   Serial.write(w.b[0]);
   Serial.write(w.b[1]);
}
