#include <SoftwareSerial.h>

#include "RASCII.h"

SoftwareSerial* RS485Serial;

RASCII::RASCII(bool softserial, uint8_t rxPin, uint8_t txPin, uint8_t txCtrl) {
  m_bSoftwareSerial = softserial;
  m_iSoftSerialRX   = rxPin;
  m_iSoftSerialTX   = txPin;
  m_iSoftSerialTxControl = txCtrl;
  //Initialize Queue:
  memset(Queue, 0, sizeof(Queue) );
}


void RASCII::begin(long int baudrate) {
  if( m_bSoftwareSerial ) {
    RS485Serial = new SoftwareSerial(m_iSoftSerialRX, m_iSoftSerialTX);
    RS485Serial->begin(baudrate);
  }
  else {
    Serial.begin(baudrate);
    while( !Serial);
  }
}

void RASCII::transmit(bool txrx) {
  digitalWrite(m_iSoftSerialTxControl, txrx);
}

bool RASCII::read(byte* cmd) {
  while (Serial.available()) {
    char c = (char)Serial.read();

    if( c == '@' ) {
      char buffer[32];
      char hexa[2];
      if( m_bSoftwareSerial )
        RS485Serial->readBytes(hexa, 2);
      else
        Serial.readBytes(hexa, 2);
      byte len = HexA2Byte(hexa);
      if( m_bSoftwareSerial )
        RS485Serial->readBytes(buffer, len);
      else
        Serial.readBytes(buffer, len);
      buffer[len] = '\0';
      for( byte i = 0; i < len; i++ ) {
        cmd[i] = HexA2Byte(buffer+i*2);
      }
      return true;
    }
  }
  return false;
}

void RASCII::write(const char* remark) {
  byte len = strLen(remark);
  if( len > 0 ) {
    char hexa[3];
    if( m_bSoftwareSerial )
      RS485Serial->write(INF);
    else
      Serial.write(INF);
    if( m_bSoftwareSerial )
      RS485Serial->write(Byte2HexA(len, hexa), 2);
    else
      Serial.write(Byte2HexA(len, hexa), 2);
    if( m_bSoftwareSerial )
      RS485Serial->write(remark, len);
    else {
      Serial.write(remark, len);
      Serial.write("\n", 1);
    }
  }
}

void RASCII::write(byte evt, byte* data, byte len, byte type) {
  char hexa[3];
  // Type @,#,$,&
  if( m_bSoftwareSerial )
    RS485Serial->write(type);
  else
    Serial.write(type);

  // Length
  if( m_bSoftwareSerial )
    RS485Serial->write(Byte2HexA((len+1)*2, hexa), 2);
  else
    Serial.write(Byte2HexA((len+1)*2, hexa), 2);

  // Command, Event
  if( m_bSoftwareSerial )
    RS485Serial->write(Byte2HexA(evt, hexa), 2);
  else
    Serial.write(Byte2HexA(evt, hexa), 2);

  // Data
  for( byte i = 0; i < len; i++ ) {
    if( m_bSoftwareSerial )
      RS485Serial->write(Byte2HexA(data[i], hexa), 2);
    else
      Serial.write(Byte2HexA(data[i], hexa), 2);
  }
  if( !m_bSoftwareSerial )
    Serial.write("\n", 1);
}

byte RASCII::strLen(const char* s) {
  byte len = 0;
  while( s != NULL && s[len] != 0 ) {
    len++;
  }
  return len;
}

byte RASCII::HexA2Byte( const char* s ) {
  byte h, l, val;
  if( s[0] >= 'A' ) h = s[0] - 55;
  else h = s[0] -48;
  if( s[1] >= 'A' ) l = s[1] - 55;
  else l = s[1] -48;
  val = l + (h << 4);
  return val;
}

char* RASCII::Byte2HexA(byte b, char* c) {
  static char cHex[] = {'0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F'};
  c[0] = cHex[(b&0xF0)>>4];
  c[1] = cHex[ b&0x0F    ];
  c[2] = '\0';
  return c;
}

void RASCII::queuePut(const char* remark) {
  byte len = strLen(remark);
  queuePut(0, (byte*)remark, len, INF);
}

void RASCII::queuePut(byte evt, byte* data, byte len, byte type) {
  for( byte i = 0; i < QUEUESIZE; i++ ) {
    if( Queue[i].type == 0 ) {
      Queue[i].type = type;
      Queue[i].evt  = evt;
      Queue[i].len  = len;
      memset(Queue[i].data, 0, sizeof(Queue[i].data));
      memcpy(Queue[i].data, data, len);
      break;
    }
  }
}

void RASCII::queueWrite() {
  for( byte i = 0; i < QUEUESIZE; i++ ) {
    if( Queue[i].type != 0 ) {
      if( Queue[i].type == INF )
        write((const char*)Queue[i].data);
      else
        write(Queue[i].evt, Queue[i].data, Queue[i].len, Queue[i].type);
      Queue[i].type = 0;
      for( byte n = i; n < QUEUESIZE; n++ ) {
        if( n + 1 >= QUEUESIZE )
          break;
        if( Queue[n+1].type != 0 ) {
          Queue[n].type = Queue[n+1].type;
          Queue[n].evt  = Queue[n+1].evt;
          Queue[n].len  = Queue[n+1].len;
          memcpy(Queue[n].data, Queue[n+1].data, Queue[n+1].len);
          Queue[n+1].type = 0;
        }
      }
      break;
    }
  }
}


