#include "ITA2String.h"

#include <string.h>

// ITA2 character table: - position in array corresponds to 5-bit ITA2 code
//                       - characters to the left are in letters shift, characters to the right in figures shift
//                       - characters marked 0x7F do not have ASCII equivalent
static const char ITA2Table[RADIOLIB_ITA2_LENGTH][2] RADIOLIB_NONVOLATILE = {
  {'\0', '\0'}, {'E', '3'}, {'\n', '\n'}, {'A', '-'}, {' ', ' '}, {'S', '\''}, {'I', '8'}, {'U', '7'},
  {'\r', '\r'}, {'D', 0x05}, {'R', '4'}, {'J', '\a'}, {'N', ','}, {'F', '!'}, {'C', ':'}, {'K', '('},
  {'T', '5'}, {'Z', '+'}, {'L', ')'}, {'W', '2'}, {'H', 0x7F}, {'Y', '6'}, {'P', '0'}, {'Q', '1'},
  {'O', '9'}, {'B', '?'}, {'G', '&'}, {0x7F, 0x7F}, {'M', '.'}, {'X', '/'}, {'V', ';'}, {0x7F, 0x7F}
};

ITA2String::ITA2String(char c) {
  asciiLen = 1;
  #if !RADIOLIB_STATIC_ONLY
  strAscii = new char[1];
  #endif
  strAscii[0] = c;
  ita2Len = 0;
}

ITA2String::ITA2String(const char* str) {
  asciiLen = strlen(str);
  #if !RADIOLIB_STATIC_ONLY
  strAscii = new char[asciiLen + 1];
  #endif
  strcpy(strAscii, str);
  ita2Len = 0;
}

ITA2String::ITA2String(const ITA2String& ita2) {
  this->asciiLen = ita2.asciiLen;
  this->ita2Len = ita2.ita2Len;
  #if !RADIOLIB_STATIC_ONLY
  this->strAscii = new char[asciiLen + 1];
  #endif
  strcpy(this->strAscii, ita2.strAscii);
}

ITA2String& ITA2String::operator=(const ITA2String& ita2) {
  if(&ita2 != this) {
    this->asciiLen = ita2.asciiLen;
    this->ita2Len = ita2.ita2Len;
    #if !RADIOLIB_STATIC_ONLY
    this->strAscii = new char[asciiLen + 1];
    #endif
    strcpy(this->strAscii, ita2.strAscii);
  }
  return(*this);
}

ITA2String::~ITA2String() {
  #if !RADIOLIB_STATIC_ONLY
    delete[] strAscii;
  #endif
}

size_t ITA2String::length() {
  // length returned by this method is different than the length of ASCII-encoded strAscii
  // ITA2-encoded string length varies based on how many number and characters the string contains

  if(ita2Len == 0) {
    // ITA2 length wasn't calculated yet, call byteArr() to calculate it
    byteArr();
  }

  return(ita2Len);
}

uint8_t* ITA2String::byteArr() {
  // create temporary array 2x the string length (figures may be 3 bytes)
  #if RADIOLIB_STATIC_ONLY
    uint8_t temp[RADIOLIB_STATIC_ARRAY_SIZE*2 + 1];
  #else
    uint8_t* temp = new uint8_t[asciiLen*2 + 1];
  #endif

  // ensure the minimum possible array size is always initialized
  temp[0] = 0;
  
  size_t arrayLen = 0;
  bool flagFigure = false;
  for(size_t i = 0; i < asciiLen; i++) {
    uint16_t code = getBits(strAscii[i]);
    uint8_t shift = (code >> 5) & 0b11111;
    uint8_t character = code & 0b11111;
    // check if the code is letter or figure
    if(shift == RADIOLIB_ITA2_FIGS) {
      // check if this is the first figure in sequence
      if(!flagFigure) {
        flagFigure = true;
        temp[arrayLen++] = RADIOLIB_ITA2_FIGS;
      }

      // add the character code
      temp[arrayLen++] = character & 0b11111;

      // check the following character (skip for message end)
      if(i < (asciiLen - 1)) {
        uint16_t nextCode = getBits(strAscii[i+1]);
        uint8_t nextShift = (nextCode >> 5) & 0b11111;
        if(nextShift == RADIOLIB_ITA2_LTRS) {
          // next character is a letter, terminate figure shift
          temp[arrayLen++] = RADIOLIB_ITA2_LTRS;
          flagFigure = false;
        }
      } else {
        // reached the end of the message, terminate figure shift
        temp[arrayLen++] = RADIOLIB_ITA2_LTRS;
        flagFigure = false;
      }
    } else {
      temp[arrayLen++] = character & 0b11111;
    }
  }

  // save ITA2 string length
  ita2Len = arrayLen;

  uint8_t* arr = new uint8_t[arrayLen];
  memcpy(arr, temp, arrayLen);
  #if !RADIOLIB_STATIC_ONLY
    delete[] temp;
  #endif

  return(arr);
}

uint16_t ITA2String::getBits(char c) {
  // search ITA2 table
  uint16_t code = 0x0000;
  for(uint8_t i = 0; i < RADIOLIB_ITA2_LENGTH; i++) {
    char* ptr = const_cast<char*>(&ITA2Table[i][0]);
    if(RADIOLIB_NONVOLATILE_READ_BYTE(ptr) == c) {
      // character is in letter shift
      code = (RADIOLIB_ITA2_LTRS << 5) | i;
      break;
    }
    
    ptr = const_cast<char*>(&ITA2Table[i][1]);
    if(RADIOLIB_NONVOLATILE_READ_BYTE(ptr) == c) {
      // character is in figures shift
      code = (RADIOLIB_ITA2_FIGS << 5) | i;
      break;
    }
  }

  return(code);
}
