#include "hp34comm.h"

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

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

#ifdef DEBUG2

DigitalOut inrx(D9);

#endif

DigitalOut lled(LED3);

#define RXTIMEOUT 50ms
#define STARTUPRETRY 0.5

const uint8_t startup_seq[] = {
  0x33,
  0x02,
  0x00,
};

HPSerial::statemethod HPSerial::state_table[NUM_STATES] = {
  &HPSerial::do_state_initial,
  &HPSerial::do_state_command,
  &HPSerial::do_state_payload_size,
  &HPSerial::do_state_payload,
  &HPSerial::do_state_sending,
  &HPSerial::do_state_eot,
};


HPSerial::HPSerial(PinName tx, PinName rx):
  serial(tx, rx),
  ncmd(0),
  cur_gstate(GSTATE_IDLE)
{
  serial.baud(187500);
  cur_state = STATE_IDLE;
  serial.attach(callback(this, &HPSerial::rxIrq), SerialBase::RxIrq);
}

void HPSerial::startup(void) {
  cur_gstate = GSTATE_STARTING;
  set_timer(10ms); // launch the startup in 10ms
}

void HPSerial::_startup(void)
{
  cur_gstate = GSTATE_STARTING;
  tr_data.size = sizeof(startup_seq);
  tr_data.cmd = 0xFF;
  tr_data.pos = 0;
  for(uint8_t i=0; i<tr_data.size; i++)
    tr_data.payload[i] = startup_seq[i];
  cur_state = do_state_sending();
}

void HPSerial::send(const uint8_t *buf, uint8_t size)
{
  //  tx_data->
  //send(startup, sizeof(startup)/sizeof(startup[0]));
}

void HPSerial::sendkey(uint8_t keycode)
{
  tr_data.size = 2;
  tr_data.cmd = 0xFF;
  tr_data.pos = 0;
  tr_data.payload[0] = 0x66;
  tr_data.payload[1] = keycode;
  cur_state = do_state_sending();
}

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 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:
    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 HPSerial::STATE_IDLE;
  }
  return HPSerial::STATE_PAYLOAD;
}

HPSerial::state_t HPSerial::do_state_sending(uint8_t c)
{
  // ghee
  if (c == 0xFF)
  { // resend current char
    tr_data.pos--;
	tr_data.payload[tr_data.pos] += 1;
  }
  // TODO: check ACK values (c is the received ack)
  if (tr_data.pos >= tr_data.size)
  {
    return do_state_eot();
  }
  serial.write(&tr_data.payload[tr_data.pos++], 1);
  set_timer(RXTIMEOUT);
  return HPSerial::STATE_SENDING;
}

HPSerial::state_t HPSerial::do_state_eot(uint8_t c)
{
  serial.write(&c, 1); // EoT
  cur_gstate = GSTATE_IDLE;
  set_timer(); // We are IDLE, detach the timeouter
  return STATE_IDLE;
}

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

void HPSerial::rxIrq(void) {
  uint8_t val;
  if(serial.readable())
  { // no reason why we would end here without
	// this condition, but hey
#ifdef DEBUG2
    inrx=1;
#endif
    //lled = 1;
    //val = serial.getc();
	serial.read(&val, 1);
    cur_state = run_state(cur_state, val);
    //lled = 0;
#ifdef DEBUG2
    inrx=0;
#endif
  }
}


void HPSerial::timeout(void) {
  set_timer(); // detach the timeouter
  if (cur_gstate == GSTATE_STARTING)
    _startup();
  else // reset
    {
    cur_gstate = GSTATE_IDLE;
    cur_state = STATE_IDLE;
    }
}
