#include "bitpack.hh"
#include "common.hh"
#include <algorithm>
#include <cmath>

using std::uint16_t;
using std::vector;
using uchar = unsigned char;
using vuint16 = vector<uint16_t>;
using vuchar = vector<uchar>;
using ustring = std::basic_string<unsigned char>;

int max(const int t_n) { return ipow(2, t_n) - 1; }

///////////////////////////////////////////////////////////////////////////////
//                                  packing                                  //
///////////////////////////////////////////////////////////////////////////////

vuchar pack(const vuint16 &t_input) {
  return pack_9(t_input.begin(), t_input.end());
}

vuchar pack_9(const vuint16::const_iterator t_input,
              const vuint16::const_iterator t_end) {
#ifdef Debug
  std::printf("9 bits\n");
#endif
  vuchar res{};
  uchar current_char = 0;
  int round = 0;
  for (auto it = t_input; it != t_end; ++it) {
    switch (round % 8) {
    case 0: {
      res.push_back(static_cast<uchar>(*it >> 1u & 0xffu));
      current_char = static_cast<uchar>(*it << 7u & 0xffu);
      break;
    }
    case 1: {
      res.push_back(
          static_cast<uchar>(current_char | (*it >> 2u & 0xffu)));
      current_char = static_cast<uchar>(*it << 6u & 0xffu);
      break;
    }
    case 2: {
      res.push_back(
          static_cast<uchar>(current_char | (*it >> 3u & 0xffu)));
      current_char = static_cast<uchar>(*it << 5u & 0xffu);
      break;
    }
    case 3: {
      res.push_back(
          static_cast<uchar>(current_char | (*it >> 4u & 0xffu)));
      current_char = static_cast<uchar>(*it << 4u & 0xffu);
      break;
    }
    case 4: {
      res.push_back(
          static_cast<uchar>(current_char | (*it >> 5u & 0xffu)));
      current_char = static_cast<uchar>(*it << 3u & 0xffu);
      break;
    }
    case 5: {
      res.push_back(
          static_cast<uchar>(current_char | (*it >> 6u & 0xffu)));
      current_char = static_cast<uchar>(*it << 2u & 0xffu);
      break;
    }
    case 6: {
      res.push_back(
          static_cast<uchar>(current_char | (*it >> 7u & 0xffu)));
      current_char = static_cast<uchar>(*it << 1u & 0xffu);
      break;
    }
    case 7: {
      res.push_back(
          static_cast<uchar>(current_char | (*it >> 8u & 0xffu)));
      res.push_back(static_cast<uchar>(*it & 0xffu));
      break;
    }
    default:
      exit(2);
    }
    if (*it >= max(9)) {
      if(round % 8 != 0) {
        res.push_back(current_char);
      }
#ifdef Debug
      std::printf("%ld elements written\n%ld chars written so far\n",
                  std::distance(t_input, it), res.size());
#endif
      return pack_10(++it, t_end, res);
    }
    ++round;
  }
  res.push_back(current_char);
#ifdef Debug
  std::printf("%ld elements written\n%ld chars written so far\n",
              std::distance(t_input, t_end), res.size());
#endif
  return res;
}

vuchar pack_10(const vuint16::const_iterator t_input,
               const vuint16::const_iterator t_end, vuchar &t_res) {
#ifdef Debug
  std::printf("10 bits\n");
#endif
  uchar current_char = 0;
  int round = 0;
  for (auto it = t_input; it != t_end; ++it) {
    switch (round % 4) {
    case 0: {
      t_res.push_back(static_cast<uchar>(*it >> 2u & 0xffu));
      current_char = static_cast<uchar>(*it << 4u & 0xffu);
      break;
    }
    case 1: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 4u & 0xffu));
      current_char = static_cast<uchar>(*it << 4u & 0xffu);
      break;
    }
    case 2: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 6u & 0xffu));
      current_char = static_cast<uchar>(*it << 2u & 0xffu);
      break;
    }
    case 3: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 8u & 0xffu));
      t_res.push_back(static_cast<uchar>(*it & 0xffu));
      break;
    }
    default:
      exit(2);
    }
    if (*it >= max(10)) {
      if(round % 4 != 0) {
        t_res.push_back(current_char);
      }
#ifdef Debug
      std::printf("%ld elements written\n%ld chars written so far\n",
                  std::distance(t_input, it), t_res.size());
#endif
      return pack_11(++it, t_end, t_res);
    }
    ++round;
  }
  t_res.push_back(current_char);
#ifdef Debug
  std::printf("%ld elements written\n%ld chars written so far\n",
              std::distance(t_input, t_end), t_res.size());
#endif
  return t_res;
}

vuchar pack_11(const vuint16::const_iterator t_input,
               const vuint16::const_iterator t_end, vuchar &t_res) {
#ifdef Debug
  std::printf("11 bits\n");
#endif
  uchar current_char = 0;
  int round = 0;
  for (auto it = t_input; it != t_end; ++it) {
    switch (round % 8) {
    case 0: {
      t_res.push_back(static_cast<uchar>(*it >> 3u & 0xffu));
      current_char = static_cast<uchar>(*it << 5u & 0xffu);
      break;
    }
    case 1: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 6u & 0xffu));
      current_char = static_cast<uchar>(*it << 2u & 0xffu);
      break;
    }
    case 2: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 11u & 0xffu));
      t_res.push_back(static_cast<uchar>(*it << 1 & 0xffu));
      current_char = static_cast<uchar>(*it << 7u & 0xffu);
      break;
    }
    case 3: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 12u & 0xffu));
      current_char = static_cast<uchar>(*it << 4u & 0xffu);
      break;
    }
    case 4: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 7u & 0xffu));
      current_char = static_cast<uchar>((*it << 1u) & 0xffu);
      break;
    }
    case 5: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 10u & 0xffu));
      t_res.push_back(static_cast<uchar>(*it >> 2u & 0xffu));
      current_char = static_cast<uchar>(*it << 6u & 0xffu);
      break;
    }
    case 6: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 5u & 0xffu));
      current_char = static_cast<uchar>(*it << 3u & 0xffu);
      break;
    }
    case 7: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 8u & 0xffu));
      current_char = static_cast<uchar>(*it & 0xffu);
      break;
    }
    default:
      exit(2);
    }
    if (*it >= max(11)) {
      if(round % 8 != 0) {
        t_res.push_back(current_char);
      }
#ifdef Debug
      std::printf("%ld elements written\n%ld chars written so far\n",
                  std::distance(t_input, it), t_res.size());
#endif
      return pack_12(++it, t_end, t_res);
    }
    ++round;
  }
  t_res.push_back(current_char);
#ifdef Debug
  std::printf("%ld elements written\n%ld chars written so far\n",
              std::distance(t_input, t_end), t_res.size());
#endif
  return t_res;
}

vuchar pack_12(const vuint16::const_iterator t_input,
               const vuint16::const_iterator t_end, vuchar &t_res) {
#ifdef Debug
  std::printf("12 bits\n");
#endif
  uchar current_char = 0;
  int round = 0;
  for (auto it = t_input; it != t_end; ++it) {
    switch (round % 2) {
    case 0: {
      t_res.push_back(static_cast<uchar>(*it >> 4u & 0xffu));
      current_char = static_cast<uchar>(*it << 4u & 0xffu);
      break;
    }
    case 1: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 8u & 0xffu));
      current_char = static_cast<uchar>(*it & 0xffu);
      break;
    }
    default:
      exit(2);
    }
    if (*it >= max(12)) {
      if(round % 2 != 0) {
        t_res.push_back(current_char);
      }
#ifdef Debug
      std::printf("%ld elements written\n%ld chars written so far\n",
                  std::distance(t_input, it), t_res.size());
#endif
      return pack_13(++it, t_end, t_res);
    }
    ++round;
  }
  t_res.push_back(current_char);
#ifdef Debug
  std::printf("%ld elements written\n%ld chars written so far\n",
              std::distance(t_input, t_end), t_res.size());
#endif
  return t_res;
}

vuchar pack_13(const vuint16::const_iterator t_input,
               const vuint16::const_iterator t_end, vuchar &t_res) {
#ifdef Debug
  std::printf("13 bits\n");
#endif
  uchar current_char = 0;
  int round = 0;
  for (auto it = t_input; it != t_end; ++it) {
    switch (round % 8) {
    case 0: {
      t_res.push_back(static_cast<uchar>(*it >> 5u & 0xffu));
      current_char = static_cast<uchar>(*it << 3u & 0xffu);
      break;
    }
    case 1: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 10u & 0xffu));
      t_res.push_back(static_cast<uchar>(*it >> 2u & 0xffu));
      current_char = static_cast<uchar>(*it << 6u & 0xffu);
      break;
    }
    case 2: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 7u & 0xffu));
      current_char = static_cast<uchar>(*it << 1u & 0xffu);
      break;
    }
    case 3: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 12u & 0xffu));
      t_res.push_back(static_cast<uchar>(*it >> 4u & 0xffu));
      current_char = static_cast<uchar>(*it << 4u & 0xffu);
      break;
    }
    case 4: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 9u & 0xffu));
      t_res.push_back(static_cast<uchar>(*it >> 1u & 0xffu));
      current_char = static_cast<uchar>(*it << 7u & 0xffu);
      break;
    }
    case 5: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 6u & 0xffu));
      current_char = static_cast<uchar>(*it << 2u & 0xffu);
      break;
    }
    case 6: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 11u & 0xffu));
      t_res.push_back(static_cast<uchar>(*it >> 3u & 0xffu));
      current_char = static_cast<uchar>(*it << 5u & 0xffu);
      break;
    }
    case 7: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 8u & 0xffu));
      current_char = static_cast<uchar>(*it & 0xffu);
      break;
    }
    default:
      exit(2);
    }
    if (*it >= max(13)) {
      if(round % 8 != 0) {
        t_res.push_back(current_char);
      }
#ifdef Debug
      std::printf("%ld elements written\n%ld chars written so far\n",
                  std::distance(t_input, it), t_res.size());
#endif
      return pack_14(++it, t_end, t_res);
    }
    ++round;
  }
  t_res.push_back(current_char);
#ifdef Debug
  std::printf("%ld elements written\n%ld chars written so far\n",
              std::distance(t_input, t_end), t_res.size());
#endif
  return t_res;
}

vuchar pack_14(const vuint16::const_iterator t_input,
               const vuint16::const_iterator t_end, vuchar &t_res) {
#ifdef Debug
  std::printf("14 bits\n");
#endif
  uchar current_char = 0;
  int round = 0;
  for (auto it = t_input; it != t_end; ++it) {
    switch (round % 4) {
    case 0: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 6u & 0xffu));
      current_char = static_cast<uchar>(*it << 2u & 0xffu);
      break;
    }
    case 1: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 12u & 0xffu));
      t_res.push_back(static_cast<uchar>(*it >> 4u & 0xffu));
      current_char = static_cast<uchar>(*it << 4u & 0xffu);
      break;
    }
    case 2: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 10u & 0xffu));
      t_res.push_back(static_cast<uchar>(*it >> 2u & 0xffu));
      current_char = static_cast<uchar>(*it << 6u & 0xffu);
      break;
    }
    case 3: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 8u & 0xffu));
      current_char = static_cast<uchar>(*it & 0xffu);
      break;
    }
    default:
      exit(2);
    }
    if (*it >= max(14)) {
      if(round % 4 != 0) {
        t_res.push_back(current_char);
      }
#ifdef Debug
      std::printf("%ld elements written\n%ld chars written so far\n",
                  std::distance(t_input, it), t_res.size());
#endif
      return pack_15(++it, t_end, t_res);
    }
    ++round;
  }
  t_res.push_back(current_char);
#ifdef Debug
  std::printf("%ld elements written\n%ld chars written so far\n",
              std::distance(t_input, t_end), t_res.size());
#endif
  return t_res;
}

vuchar pack_15(const vuint16::const_iterator t_input,
               const vuint16::const_iterator t_end, vuchar &t_res) {
#ifdef Debug
  std::printf("15 bits\n");
#endif
  uchar current_char = 0;
  int round = 0;
  for (auto it = t_input; it != t_end; ++it) {
    switch (round % 8) {
    case 0: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 7u & 0xffu));
      current_char = static_cast<uchar>(*it << 1u & 0xffu);
      break;
    }
    case 1: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 14u & 0xffu));
      t_res.push_back(static_cast<uchar>(*it >> 6u & 0xffu));
      current_char = static_cast<uchar>(*it << 2u & 0xffu);
      break;
    }
    case 2: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 13u & 0xffu));
      t_res.push_back(static_cast<uchar>(*it >> 5u & 0xffu));
      current_char = static_cast<uchar>(*it << 3u & 0xffu);
      break;
    }
    case 3: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 12u & 0xffu));
      t_res.push_back(static_cast<uchar>(*it >> 4u & 0xffu));
      current_char = static_cast<uchar>(*it << 4u & 0xffu);
      break;
    }
    case 4: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 11u & 0xffu));
      t_res.push_back(static_cast<uchar>(*it >> 3u & 0xffu));
      current_char = static_cast<uchar>(*it << 5u & 0xffu);
      break;
    }
    case 5: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 10u & 0xffu));
      t_res.push_back(static_cast<uchar>(*it >> 2u & 0xffu));
      current_char = static_cast<uchar>(*it << 6u & 0xffu);
      break;
    }
    case 6: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 9u & 0xffu));
      t_res.push_back(static_cast<uchar>(*it >> 1u & 0xffu));
      current_char = static_cast<uchar>(*it << 7u & 0xffu);
      break;
    }
    case 7: {
      t_res.push_back(current_char | static_cast<uchar>(*it >> 8u & 0xffu));
      current_char = static_cast<uchar>(*it & 0xffu);
      break;
    }
    default:
      exit(2);
    }
    if (*it >= max(15)) {
      if(round % 8 != 0) {
        t_res.push_back(current_char);
      }
#ifdef Debug
      std::printf("%ld elements written\n%ld chars written so far\n",
                  std::distance(t_input, it), t_res.size());
#endif
      return pack_16(++it, t_end, t_res);
    }
    ++round;
  }
  t_res.push_back(current_char);
#ifdef Debug
  std::printf("%ld elements written\n%ld chars written so far\n",
              std::distance(t_input, t_end), t_res.size());
#endif
  return t_res;
}

vuchar pack_16(const vuint16::const_iterator t_input,
               const vuint16::const_iterator t_end, vuchar &t_res) {
  std::for_each(t_input, t_end, [&](const auto value) {
    t_res.push_back(static_cast<uchar>(value >> 8 & 0xFFu));
    t_res.push_back(static_cast<uchar>(value & 0xFFu));
                                });
#ifdef Debug
  std::printf("%ld elements written\n%ld chars written so far\n",
              std::distance(t_input, t_end), t_res.size());
#endif
  return t_res;
}

///////////////////////////////////////////////////////////////////////////////
//                                 unpacking                                 //
///////////////////////////////////////////////////////////////////////////////

constexpr uint16_t mask_n(int t_nb_bits) {
  if (t_nb_bits == 0) {
    return 0;
  }
  uint16_t mask = mask_n(t_nb_bits - 1);
  mask = static_cast<uint16_t>(mask << 1);
  mask |= 0x1;
  return mask;
}

constexpr uint16_t masks[17] = {
    mask_n(0),  mask_n(1),  mask_n(2),  mask_n(3),  mask_n(4),  mask_n(5),
    mask_n(6),  mask_n(7),  mask_n(8),  mask_n(9),  mask_n(10), mask_n(11),
    mask_n(12), mask_n(13), mask_n(14), mask_n(15), mask_n(16)};

vuint16 unpack(ustring &&t_input) {
  return unpack_9(t_input.begin(), t_input.end());
}

vuint16 unpack_9(const ustring::const_iterator t_input,
                 const ustring::const_iterator t_end) {
#ifdef Debug
  std::printf("9 bits\n");
#endif
  const auto max_val = max(9);
  int round = 0;
  uint16_t current_char = 0;
  auto it = t_input;
  vuint16 res{};
  while (current_char < max_val && it < t_end - 1) {
    switch (round % 8) {
    case 0: {
      current_char = static_cast<uint16_t>(*it) << 1u;
      current_char |= static_cast<uint16_t>(*++it) >> 7u;
      break;
    }
    case 1: {
      current_char = static_cast<uint16_t>(*it) << 2u;
      current_char |= static_cast<uint16_t>(*++it) >> 6u;
      break;
    }
    case 2: {
      current_char = static_cast<uint16_t>(*it) << 3u;
      current_char |= static_cast<uint16_t>(*++it) >> 5u;
      break;
    }
    case 3: {
      current_char = static_cast<uint16_t>(*it) << 4u;
      current_char |= static_cast<uint16_t>(*++it) >> 4u;
      break;
    }
    case 4: {
      current_char = static_cast<uint16_t>(*it) << 5u;
      current_char |= static_cast<uint16_t>(*++it) >> 3u;
      break;
    }
    case 5: {
      current_char = static_cast<uint16_t>(*it) << 6u;
      current_char |= static_cast<uint16_t>(*++it) >> 2u;
      break;
    }
    case 6: {
      current_char = static_cast<uint16_t>(*it) << 7u;
      current_char |= static_cast<uint16_t>(*++it) >> 1u;
      break;
    }
    case 7: {
      current_char = static_cast<uint16_t>(*it) << 8u;
      current_char |= static_cast<uint16_t>(*++it);
      ++it;
      break;
    }
    default:
      exit(2);
    }
    current_char &= masks[9];
    res.push_back(current_char);
    ++round;
  }
  if (current_char >= max_val) {
#ifdef Debug
    std::printf("%ld elements written\n%ld chars written so far\n",
                std::distance(t_input, it), res.size());
#endif
    return unpack_10(it, t_end, res);
  }
  return res;
}

vuint16 unpack_10(const ustring::const_iterator t_input,
                  const ustring::const_iterator t_end,
									vuint16& t_res) {
#ifdef Debug
  std::printf("10 bits\n");
#endif
  const auto max_val = max(10);
  int round = 0;
  uint16_t current_char = 0;
  auto it = t_input;
  while (current_char < max_val && it < t_end - 1) {
    switch (round % 4) {
    case 0: {
      current_char = static_cast<uint16_t>(*it) << 2u;
      current_char |= static_cast<uint16_t>(*++it) >> 6u;
      break;
    }
    case 1: {
      current_char = static_cast<uint16_t>(*it) << 4u;
      current_char |= static_cast<uint16_t>(*++it) >> 4u;
      break;
    }
    case 2: {
      current_char = static_cast<uint16_t>(*it) << 6u;
      current_char |= static_cast<uint16_t>(*++it) >> 2u;
      break;
    }
    case 3: {
      current_char = static_cast<uint16_t>(*it) << 8u;
      current_char |= static_cast<uint16_t>(*++it);
      ++it;
      break;
    }
    default:
      exit(2);
    }
    current_char &= masks[10];
    t_res.push_back(current_char);
    ++round;
  }
  if (current_char >= max_val) {
#ifdef Debug
    std::printf("%ld elements written\n%ld chars written so far\n",
                std::distance(t_input, it), t_res.size());
#endif
    return unpack_11(it, t_end, t_res);
  }
  return t_res;
}

vuint16 unpack_11(const ustring::const_iterator t_input,
                  const ustring::const_iterator t_end,
									vuint16& t_res) {
#ifdef Debug
  std::printf("11 bits\n");
#endif
  const auto max_val = max(11);
  int round = 0;
  uint16_t current_char = 0;
  auto it = t_input;
  while (current_char < max_val && it < t_end - 1) {
    switch (round % 8) {
    case 0: {
      current_char = static_cast<uint16_t>(*it) << 3u;
      current_char |= static_cast<uint16_t>(*++it) >> 5u;
      break;
    }
    case 1: {
      current_char = static_cast<uint16_t>(*it) << 6u;
      current_char |= static_cast<uint16_t>(*++it) >> 2u;
      break;
    }
    case 2: {
      current_char = static_cast<uint16_t>(*it) << 9u;
      current_char |= static_cast<uint16_t>(*++it) << 1u;
      current_char |= static_cast<uint16_t>(*++it) >> 7u;
      break;
    }
    case 3: {
      current_char = static_cast<uint16_t>(*it) << 4u;
      current_char |= static_cast<uint16_t>(*++it) >> 4u;
      break;
    }
    case 4: {
      current_char = static_cast<uint16_t>(*it) << 7u;
      current_char |= static_cast<uint16_t>(*++it) >> 1u;
      break;
    }
    case 5: {
      current_char = static_cast<uint16_t>(*it) << 10u;
      current_char |= static_cast<uint16_t>(*++it) << 2u;
      current_char |= static_cast<uint16_t>(*++it) >> 6u;
      break;
    }
    case 6: {
      current_char = static_cast<uint16_t>(*it) << 5u;
      current_char |= static_cast<uint16_t>(*++it) >> 3u;
      break;
    }
    case 7: {
      current_char = static_cast<uint16_t>(*it) << 8u;
      current_char |= static_cast<uint16_t>(*++it);
      ++it;
      break;
    }
    default:
      exit(2);
    }
    current_char &= masks[11];
    t_res.push_back(current_char);
    ++round;
  }
  if (current_char >= max_val) {
#ifdef Debug
    std::printf("%ld elements written\n%ld chars written so far\n",
                std::distance(t_input, it), t_res.size());
#endif
    return unpack_12(it, t_end, t_res);
  }
  return t_res;
}

vuint16 unpack_12(const ustring::const_iterator t_input,
                  const ustring::const_iterator t_end,
									vuint16& t_res) {
#ifdef Debug
  std::printf("12 bits\n");
#endif
  const auto max_val = max(12);
  int round = 0;
  uint16_t current_char = 0;
  auto it = t_input;
  while (current_char < max_val && it < t_end - 1) {
    switch (round % 2) {
    case 0: {
      current_char = static_cast<uint16_t>(*it) << 4u;
      current_char |= static_cast<uint16_t>(*++it) >> 4u;
      break;
    }
    case 1: {
      current_char = static_cast<uint16_t>(*it) << 8u;
      current_char |= static_cast<uint16_t>(*++it);
      ++it;
      break;
    }
    default:
      exit(2);
    }
    current_char &= masks[12];
    t_res.push_back(current_char);
    ++round;
  }
  if (current_char >= max_val) {
#ifdef Debug
    std::printf("%d\n", current_char);
#endif
    return unpack_13(it, t_end, t_res);
  }
  return t_res;
}

vuint16 unpack_13(const ustring::const_iterator t_input,
                  const ustring::const_iterator t_end,
									vuint16& t_res) {
#ifdef Debug
  std::printf("13 bits\n");
#endif
  const auto max_val = max(13);
  int round = 0;
  uint16_t current_char = 0;
  auto it = t_input;
  while (current_char < max_val && it < t_end - 1) {
    switch (round % 8) {
    case 0: {
      current_char = static_cast<uint16_t>(*it) << 5u;
      current_char |= static_cast<uint16_t>(*++it) >> 3u;
      break;
    }
    case 1: {
      current_char = static_cast<uint16_t>(*it) << 10u;
      current_char |= static_cast<uint16_t>(*++it) << 2u;
      current_char |= static_cast<uint16_t>(*++it) >> 6u;
      break;
    }
    case 2: {
      current_char = static_cast<uint16_t>(*it) << 7u;
      current_char |= static_cast<uint16_t>(*++it) >> 1u;
      break;
    }
    case 3: {
      current_char = static_cast<uint16_t>(*it) << 12u;
      current_char |= static_cast<uint16_t>(*++it) << 4u;
      current_char |= static_cast<uint16_t>(*++it) >> 4u;
      break;
    }
    case 4: {
      current_char = static_cast<uint16_t>(*it) << 9u;
      current_char |= static_cast<uint16_t>(*++it) << 1u;
      current_char |= static_cast<uint16_t>(*++it) >> 7u;
      break;
    }
    case 5: {
      current_char = static_cast<uint16_t>(*it) << 6u;
      current_char |= static_cast<uint16_t>(*++it) >> 2u;
      break;
    }
    case 6: {
      current_char = static_cast<uint16_t>(*it) << 11u;
      current_char |= static_cast<uint16_t>(*++it) << 3u;
      current_char |= static_cast<uint16_t>(*++it) >> 5u;
      break;
    }
    case 7: {
      current_char = static_cast<uint16_t>(*it) << 8u;
      current_char |= static_cast<uint16_t>(*++it);
      break;
    }
    default:
      exit(2);
    }
    current_char &= masks[13];
    t_res.push_back(current_char);
    ++round;
  }
  if (current_char >= max_val) {
#ifdef Debug
    std::printf("%ld elements written\n%ld chars written so far\n",
                std::distance(t_input, it), t_res.size());
#endif
    return unpack_14(it, t_end, t_res);
  }
  return t_res;
}

vuint16 unpack_14(const ustring::const_iterator t_input,
                  const ustring::const_iterator t_end,
									vuint16& t_res) {
#ifdef Debug
  std::printf("14 bits\n");
#endif
  const auto max_val = max(14);
  int round = 0;
  uint16_t current_char = 0;
  auto it = t_input;
  while (current_char < max_val && it < t_end - 1) {
    switch (round % 4) {
    case 0: {
      current_char = static_cast<uint16_t>(*it) << 6u;
      current_char |= static_cast<uint16_t>(*++it) >> 2u;
      break;
    }
    case 1: {
      current_char = static_cast<uint16_t>(*it) << 12u;
      current_char |= static_cast<uint16_t>(*++it) << 4u;
      current_char |= static_cast<uint16_t>(*++it) >> 4u;
      break;
    }
    case 2: {
      current_char = static_cast<uint16_t>(*it) << 10u;
      current_char |= static_cast<uint16_t>(*++it) << 2u;
      current_char |= static_cast<uint16_t>(*++it) >> 6u;
      break;
    }
    case 3: {
      current_char = static_cast<uint16_t>(*it) << 8u;
      current_char |= static_cast<uint16_t>(*++it);
      break;
    }
    default:
      exit(2);
    }
    current_char &= masks[14];
    t_res.push_back(current_char);
    ++round;
  }
  if (current_char >= max_val) {
#ifdef Debug
    std::printf("%ld elements written\n%ld chars written so far\n",
                std::distance(t_input, it), t_res.size());
#endif
		return unpack_15(it, t_end, t_end, t_res);
  }
  return t_res;
}

vuint16 unpack_15(const ustring::const_iterator t_begin,
                  const ustring::const_iterator t_end,
									vuint16& t_res) {
#ifdef Debug
  std::printf("15 bits\n");
#endif
  const auto max_val = max(15);
  int round = 0;
  uint16_t current_char = 0;
  auto it = t_begin;
  while (current_char < max_val && it < t_end - 1) {
    switch (round % 8) {
    case 0: {
      current_char = static_cast<uint16_t>(*it) << 7u;
      current_char |= static_cast<uint16_t>(*++it) >> 1u;
      break;
    }
    case 1: {
      current_char = static_cast<uint16_t>(*it) << 14u;
      current_char |= static_cast<uint16_t>(*++it) << 6u;
      current_char |= static_cast<uint16_t>(*++it) >> 2u;
      break;
    }
    case 2: {
      current_char = static_cast<uint16_t>(*it) << 13u;
      current_char |= static_cast<uint16_t>(*++it) << 5u;
      current_char |= static_cast<uint16_t>(*++it) >> 3u;
      break;
    }
    case 3: {
      current_char = static_cast<uint16_t>(*it) << 12u;
      current_char |= static_cast<uint16_t>(*++it) << 4u;
      current_char |= static_cast<uint16_t>(*++it) >> 4u;
      break;
    }
    case 4: {
      current_char = static_cast<uint16_t>(*it) << 11u;
      current_char |= static_cast<uint16_t>(*++it) << 3u;
      current_char |= static_cast<uint16_t>(*++it) >> 5u;
      break;
    }
    case 5: {
      current_char = static_cast<uint16_t>(*it) << 10u;
      current_char |= static_cast<uint16_t>(*++it) << 2u;
      current_char |= static_cast<uint16_t>(*++it) >> 6u;
      break;
    }
    case 6: {
      current_char = static_cast<uint16_t>(*it) << 9u;
      current_char |= static_cast<uint16_t>(*++it) << 1u;
      current_char |= static_cast<uint16_t>(*++it) >> 7u;
      break;
    }
    case 7: {
      current_char = static_cast<uint16_t>(*it) << 8u;
      current_char |= static_cast<uint16_t>(*++it);
      break;
    }
    default:
      exit(2);
    }
    current_char &= masks[15];
    t_res.push_back(current_char);
    ++round;
  }
  if (current_char >= max_val) {
#ifdef Debug
    std::printf("%ld elements written\n%ld chars written so far\n",
                std::distance(t_begin, it), t_res.size());
#endif
		return unpack_16(it, t_end, t_res);
  }
  return t_res;
}

vuint16 unpack_16(const ustring::const_iterator t_begin,
                  const ustring::const_iterator t_end,
                  vuint16& t_res) {
#ifdef Debug
  std::printf("16 bits\n");
#endif
  for (auto it = t_begin; it < t_end; ++it) {
    t_res.push_back(static_cast<uint16_t>(*it << 8u) |
                  static_cast<uint16_t>(*++it & 0xffu));
  }
#ifdef Debug
  std::printf("%ld elements written\n%ld chars written so far\n",
              std::distance(t_begin, t_end), t_res.size());
#endif
  return t_res;
}