#include "hp34comm.h"
#include "mbed.h"
#include "CircularBuffer.h"

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

#ifdef DEBUG2

DigitalOut inrx(D9);
DigitalOut intx(D5);
DigitalOut ack(D6);
DigitalOut staterx(D7);
DigitalOut statetx(D8);

#endif

DigitalOut lled(LED1);
  
HPSerial::HPSerial(PinName rx):
  serial_rx(NC, rx),
  ncmd(0) {
  
  //pc.printf("HPSerial init\n");

  for(uint8_t i=0; i<MAX_ERRS; i++)
    errs[i] = 0;
  reset();
  
  serial_rx.baud(187500);
  serial_rx.attach(this, &HPSerial::rxIrq, Serial::RxIrq);
}

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::reset(uint8_t errorno) {
  timeouter.detach();
  head = 0;
  tx_state = Idle;
  setstatedbg();
  tx_cmd = 0xFF;
  tx_ack = false;
  tx_len = 0xFF;
  memset(buf, 0, BUF_SIZE);
  if (errorno != 0xFF){
    errs[errorno]++;
  }
  
}

void HPSerial::handleAck(uint8_t val) {
  if ((tx_state == Rx) & (tx_cmd == 0xFF) & (tx_ack == false) & (val == 0x66)) {
    // special case: keypad does not acknwledge and takes precedence
    // on the "bus"
    tx_state = Tx;
    setstatedbg();
  } else
    if (tx_cmd == 0xFF) // beginning of a packet, expect 0x99 as ack
      if (val == 0x99)
	{
	  tx_ack = true;
#ifdef DEBUG2
	  ack = 1;
	  wait_us(2);
	  ack = 0;
#endif
	}
      else 
	reset(1);

    else // expect 0x00 as ack
      if (val == 0x00)
	{
	  tx_ack = true;
#ifdef DEBUG2
	  ack = 1;
	  wait_us(2);
	  ack = 0;
	  wait_us(2);
	  ack = 1;
	  wait_us(2);
	  ack = 0;
#endif
	}
      else
	reset(2);
}

void HPSerial::pushCmd(TrState direction, uint8_t cmd, uint8_t size, char *payload) {
  CMD val;
  uint8_t i;
  val.id = ncmd++;
  val.direction = direction;
  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::handleChar(uint8_t val) {
  if (tx_ack == false)
    reset(3);
  else // remaining of the state machine
    if (tx_cmd == 0xFF) {
      // begin of transmission, expect a cmd
      tx_cmd = val;
      tx_ack = false;
      tx_len = 0xFF;
    }
    else if (tx_len == 0xFF) {
      // got a cmd, expect a payload size
      tx_len = val;
      tx_ack = false;
    }
    else if (tx_len == 0x55) {
      // packet was in fact a keystroke, tx_cmd is in fact the key
      // stroke and no payload is expected 
      pushCmd((TrState)Tx, tx_cmd, 0, NULL);
      reset();
    }
    else if (tx_len > 0) {
      // a payload char
      buf[head++] = val;
      tx_len--;
      tx_ack = false;
    }
    else { // end of payload, manage sent content
      uint8_t cur_state = tx_state;
      pushCmd((TrState)tx_state, tx_cmd, head, (char*)buf); 
      
      switch(val) {
      case 0x66: // a new transmisson
	reset();
	tx_state = cur_state;
	setstatedbg();
	break;
      case 0x55:
	reset();
	break;
      default:
	reset(6);
	break;
      }
    }
}



void HPSerial::setstatedbg(void) {
#ifdef DEBUG2
    if (tx_state == Rx)
      staterx = 1;
    else
      staterx = 0;
    if (tx_state == Tx)
      statetx = 1;
    else
      statetx = 0;
#endif
}

void HPSerial::rxIrq(void) {
  uint8_t val;
  if(serial_rx.readable()) { // no reason why we would end here without this condition, but hey
#ifdef DEBUG2
    inrx=1;
#endif
    lled = 1;
    val = serial_rx.getc();
    
    timeouter.attach(this, &HPSerial::timeout, 0.001); // if nothing else happen in the next ms, reset
    
    if (tx_state == Idle)
      if (val == 0x66) {
	// no transmission in progress, expect a start of transmission
	tx_state = Rx;
	tx_ack = false;
	setstatedbg();
      }
      else {
	reset(4);
      }
    else if (tx_ack == false)  // manage the acks
      handleAck(val);
    else 
      handleChar(val);
    lled = 0;
#ifdef DEBUG2
    inrx=0;
#endif
  }
}


void HPSerial::timeout(void) {
  if (tx_state != Idle) {
    reset(7);
  }
}
