#include "hp34comm.h"

#include <mbed.h>
#include <CircularBuffer.h>

/***** HP 34970A communication class ***/

#define RXTIMEOUT 50ms
#define STARTUPRETRY 0.5

HPSerial::statemethod HPSerial::state_table[NUM_STATES] = {
  &HPSerial::do_state_initial,      // STATE_IDLE
  &HPSerial::do_state_command,      // STATE_COMMAND
  &HPSerial::do_state_payload_size, // STATE_PAYLOAD_SIZE
  &HPSerial::do_state_payload,      // STATE_PAYLOAD
  &HPSerial::do_state_sending,      // STATE_SENDING
};


HPSerial::HPSerial(PinName tx, PinName rx):
  serial(tx, rx),
  ncmd(0),
  cur_gstate(GSTATE_IDLE)
{
  serial.baud(187500);
  serial.format(8, BufferedSerial::Even, 1);
  cur_state = STATE_IDLE;
  send_thread.start(callback(this, &HPSerial::send_pending_key));
  serial.attach(callback(this, &HPSerial::rx_irq), SerialBase::RxIrq);
}


// SEND related methods
void HPSerial::sendkey(uint8_t keycode)
{
  if (!sendbuf.full())
    sendbuf.push(keycode);
}


bool HPSerial::wait_for(uint8_t value)
{  // wait for an expected character in the serial port
  char c;

  for(uint8_t i=0; i<2; i++)
  {
    while(!serial.readable())
		wait_us(10);
	serial.read(&c, 1);
	if (value == c)
		return true;
  }
  return false;
}


void HPSerial::send_pending_key() {
  uint8_t c;

  while(true)
  {
	  if (!sendbuf.empty())
	  {
		  if (cur_gstate == GSTATE_IDLE)
		  {
			  serial.attach(0, SerialBase::RxIrq);
			  cur_gstate = GSTATE_TX;

			  c = 0x66;
			  serial.write(&c, 1);
			  if (!wait_for(0x99)) {}
			  // break; // XXX what to do?

			  sendbuf.pop(c);
			  serial.write(&c, 1);
			  if (!wait_for(0x00)) {}

			  c = 0x55;
			  serial.write(&c, 1);
			  cur_gstate = GSTATE_IDLE;
			  serial.attach(callback(this, &HPSerial::rx_irq), SerialBase::RxIrq);
		  }
	  }
	  //else // prevent from flooding the main unit
	  ThisThread::sleep_for(5ms);
  }
}


void HPSerial::send_startup_seq(uint8_t keycode) {
  uint8_t c;

  while (cur_gstate != GSTATE_IDLE) {
	  ThisThread::sleep_for(1ms);
  }

  serial.attach(0, SerialBase::RxIrq);
  cur_gstate = GSTATE_TX;

  // Send the init seq 0x33 0x02 0xFF <keycode> 0x55
  c = 0x33;
  serial.write(&c, 1);
  if (!wait_for(0xCC)) {}

  c = 0x02;
  serial.write(&c, 1);
  if (!wait_for(0x00)) {}

  c = 0xFF;
  serial.write(&c, 1);
  if (!wait_for(0x00)) {}

  c = keycode;
  serial.write(&c, 1);
  if (!wait_for(0x00)) {}

  c = 0x55;
  serial.write(&c, 1);
  cur_gstate = GSTATE_IDLE;
  serial.attach(callback(this, &HPSerial::rx_irq), SerialBase::RxIrq);

}


void HPSerial::send_startup_seq() {
  uint8_t c;

  while (cur_gstate != GSTATE_IDLE) {
	  ThisThread::sleep_for(1ms);
  }

  serial.attach(0, SerialBase::RxIrq);
  cur_gstate = GSTATE_TX;

  // Send the init seq 0x33 0x02 0x00 0x55
  c = 0x33;
  serial.write(&c, 1);
  if (!wait_for(0xCC)) {}

  c = 0x02;
  serial.write(&c, 1);
  if (!wait_for(0x00)) {}

  c = 0x00;
  serial.write(&c, 1);
  if (!wait_for(0x00)) {}

  c = 0x55;
  serial.write(&c, 1);
  cur_gstate = GSTATE_IDLE;
  serial.attach(callback(this, &HPSerial::rx_irq), SerialBase::RxIrq);
}


// RECV related methods

bool HPSerial::cmd_available(void)
{
  return !cmdbuf.empty();
}


bool HPSerial::pop(CMD& cmd)
{
  return cmdbuf.pop(cmd);
}


bool HPSerial::cmd_buf_full(void)
{
  return cmdbuf.full();
}


unsigned int HPSerial::nerrors(uint8_t errorno)
{
  return errs[errorno];
}


void HPSerial::pushCmd(uint8_t cmd, uint8_t size, char *payload) {
  CMD val;
  uint8_t i;
  val.id = ncmd++;
  val.cmd = cmd;
  val.size = size;
  for(i=0; i<size; i++)
    val.value[i] = payload[i];
  val.value[i] = 0x00;
  cmdbuf.push(val);
}


void HPSerial::send_ack(uint8_t c) {
  serial.write(&c, 1);
  set_timer(RXTIMEOUT); // if nothing else happen in the next RXTIMEOUT ms, reset
}


HPSerial::state_t HPSerial::do_state_initial(uint8_t c)
{
  // we are idle, incoming char is a handcheck
  // knwon handcheck values are 0x66 and 0x33
  set_timer(RXTIMEOUT); // reset the watchdog
  switch (c) {
  case 0x33:  // XXX? when are we expecting a 0x33 here?
    send_ack(0xCC);
    return HPSerial::STATE_PAYLOAD_SIZE;
    break;
  case 0x55: // EoT
    return HPSerial::STATE_IDLE;
    break;
  case 0x66:
    send_ack(0x99);
    return HPSerial::STATE_COMMAND;
    break;
  case 0xFF:
    return HPSerial::STATE_IDLE;
  default: // unknown value
    send_ack(0xFF);
    return HPSerial::STATE_IDLE;
  }
}


HPSerial::state_t HPSerial::do_state_command(uint8_t c)
{
  if (c == 0x55) { // EoT
    return STATE_IDLE;
  }

  tr_data.cmd = c;
  tr_data.size = 0;
  tr_data.pos = 0;
  send_ack(0x00);

  if (c == 0x86) { // shutdown
    pushCmd(tr_data.cmd, tr_data.size, tr_data.payload);
    return HPSerial::STATE_IDLE;
  }
  return STATE_PAYLOAD_SIZE;
}


HPSerial::state_t HPSerial::do_state_payload_size(uint8_t c)
{
  tr_data.size = c;
  tr_data.pos = 0;
  send_ack(0x00);
  return STATE_PAYLOAD;
}


HPSerial::state_t HPSerial::do_state_payload(uint8_t c)
{
  tr_data.payload[tr_data.pos++] = c;
  send_ack(0x00);
  if (tr_data.pos >= tr_data.size) {
    pushCmd(tr_data.cmd, tr_data.size, tr_data.payload);
    return STATE_IDLE;
  }
  return STATE_PAYLOAD;
}


HPSerial::state_t HPSerial::do_state_sending(uint8_t c)
{
  // check the ack value returned by the main unit

  if ((tr_data.pos == 1) && (tr_data.payload[0] == 0x66))
  {
	  if (c != 0x99)
	  {
          // did not received the expected ack
		  if (c == 0x66)
		  {
			  // we received a start of transmission while trying to emit something,
			  // ignore it, the correct ack should be sent but the main unit just behind...
			  set_timer(RXTIMEOUT);
			  return cur_state;
		  }
		  else
		  {
			  // not sure how this may happen, in doubt, try again
			  tr_data.pos--;
		  }
	  }
  }
  /*
  else if (c != 0x00)
  { // resend current char
	  tr_data.pos--;
  }
  */

  // TODO: check ACK values (c is the received ack)
  if (tr_data.pos >= tr_data.size)
  {
	  c = 0x55;
	  serial.write(&c, 1); // EoT
	  cur_gstate = GSTATE_IDLE;
	  set_timer(); // We are IDLE, detach the timeouter
	  return STATE_IDLE;
  }
  else
  {
	  serial.write(&tr_data.payload[tr_data.pos++], 1);
	  set_timer(RXTIMEOUT);
	  return STATE_SENDING;
  }
}


HPSerial::state_t HPSerial::run_state(HPSerial::state_t cur_state,
									  uint8_t c)
{
  return (this->*(HPSerial::state_table[cur_state]))(c);
};


void HPSerial::rx_irq(void) {
  uint8_t val;
  if(serial.readable())
  { // no reason why we would end here without
	// this condition, but hey
	if (cur_gstate == GSTATE_IDLE)
	  // occurs when the CPU starts a new transmission
	  // at this point, cur_state should be STATE_IDLE also (TODO add a check?)
	  cur_gstate = GSTATE_RX;
	serial.read(&val, 1);
    cur_state = run_state(cur_state, val);
  }
}


void HPSerial::timeout(void) {
  set_timer(); // detach the timeouter
  cur_gstate = GSTATE_IDLE;
  cur_state = STATE_IDLE;
}
