#include "stdafx.h"

#include "cpuencode.h"
#include "print_helpers.h"

using namespace std;

#if 1

// The max. codeword length for each byte symbol is 32-bits

extern "C" void cpu_vlc_encode(unsigned int *indata, unsigned int num_elements,
                               unsigned int *outdata, unsigned int *outsize,
                               unsigned int *codewords,
                               unsigned int *codewordlens) {
  unsigned int *bitstreamPt =
      (unsigned int *)outdata; /* Pointer to current byte   */
  *bitstreamPt = 0x00000000U;
  unsigned int startbit = 0;
  unsigned int totalBytes = 0;

  for (unsigned int k = 0; k < num_elements; k++) {
    unsigned int cw32 = 0;
    unsigned int val32 = indata[k];
    unsigned int numbits = 0;
    unsigned int mask32;

    for (unsigned int i = 0; i < 4; i++) {
      unsigned char symbol = (unsigned char)(val32 >> (8 * (3 - i)));
      cw32 = codewords[symbol];
      numbits = codewordlens[symbol];

      while (numbits > 0) {
        int writebits = min(32 - startbit, numbits);
        if (numbits == writebits)
          mask32 = (cw32 & ((1 << numbits) - 1))
                   << (32 - startbit -
                       numbits); // first make sure that the start of the word
                                 // is clean, then shift to the left as many
                                 // places as you need
        else
          mask32 = cw32 >>
                   (numbits - writebits); // shift out the bits that can not fit
        *bitstreamPt = (*bitstreamPt) | mask32;
        numbits = numbits - writebits;
        startbit = (startbit + writebits) % 32;
        if (startbit == 0) {
          bitstreamPt++;
          *bitstreamPt = 0x00000000;
          totalBytes += 4;
        }
      }
    }
  }
  totalBytes += (startbit / 8) +
                ((startbit % 8 == 0) ? 0 : 1); // return aligned to 8-bits
  *outsize = totalBytes;
}

//////////////////////////////////////////////////////////////////////
/// ALTERNATIVE CODER
/// ASSUMPTION: The max. length of 4 combined codewords can be 2x original data,
/// i.e. g 64 bits
///////////////////////////////////////////////////////////////////////

#else

extern "C" void cpu_vlc_encode(unsigned int *indata, unsigned int num_elements,
                               unsigned int *outdata, unsigned int *outsize,
                               unsigned int *codewords,
                               unsigned int *codewordlens) {
  unsigned int *bitstreamPt =
      (unsigned int *)outdata; /* Pointer to current byte   */
  // assume memset is done.
  *bitstreamPt = 0x00000000U;
  unsigned int startbit = 0;
  unsigned int totalBytes = 0;

  for (unsigned int k = 0; k < num_elements; k++) {
    unsigned long long cw64 = 0, mask64 = 0;
    unsigned int val32 = indata[k];
    unsigned int numbits = 0;
    unsigned int mask32, temp32;

    for (unsigned int i = 0; i < 4; i++) {
      unsigned char symbol = (unsigned char)(val32 >> (8 * (3 - i)));
      cw64 = (cw64 << codewordlens[symbol]) | codewords[symbol];
      numbits += codewordlens[symbol];
      // if (numbits>32) printf("WARRNING! Element %d is combined into numbits =
      // %d!!!!!!!\n", k, numbits);
    }

    while (numbits > 0) {
      int writebits = min(32 - startbit, numbits);
      if (numbits == writebits) {
        temp32 = (unsigned int)cw64; //(cw64 & 0xFFFFFFFF);
        mask32 = temp32 << (32 - startbit - numbits);
      } else {
        mask32 = (unsigned int)(cw64 >> (numbits - writebits));
        cw64 = cw64 & ((1 << (numbits - writebits)) - 1);
      }
      *bitstreamPt = (*bitstreamPt) | mask32;
      numbits = numbits - writebits;
      startbit = (startbit + writebits) % 32;
      if (startbit == 0) {
        bitstreamPt++;
        *bitstreamPt = 0x00000000;
        totalBytes += 4;
      }
    }
  }
  totalBytes += (startbit / 8) +
                ((startbit % 8 == 0) ? 0 : 1); // return aligned to 8-bits
  *outsize = totalBytes;
}
#endif