/* - | Copyright / About | ----------------------------------------------------
   Copyright (c) 2026 Randy Jordan

Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
of the Software, and to permit persons to whom the Software is furnished to do
so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

 * --------------------------------------------------------------------------*/
#include "../include/platform/mem/ci2_pack.h"
#include <ctype.h>
#include <stdarg.h>
#include <string.h>

uint64_t
pack754(long double f, unsigned bits, unsigned expbits)
{
  long double fnorm;
  int shift;
  long long sign, exp, significand;
  unsigned significandbits = bits - expbits - 1; // -1 for sign bit

  if (f == 0.0)
    return 0; // get this special case out of the way

  // check sign and begin normalization
  if (f < 0) {
    sign = 1;
    fnorm = -f;
  } else {
    sign = 0;
    fnorm = f;
  }

  // get the normalized form of f and track the exponent
  shift = 0;
  while (fnorm >= 2.0) {
    fnorm /= 2.0;
    shift++;
  }
  while (fnorm < 1.0) {
    fnorm *= 2.0;
    shift--;
  }
  fnorm = fnorm - 1.0;

  // calculate the binary form (non-float) of the significand data
  significand = fnorm * ((1LL << significandbits) + 0.5f);

  // get the biased exponent
  exp = shift + ((1 << (expbits - 1)) - 1); // shift + bias

  // return the final answer
  return (sign << (bits - 1)) | (exp << (bits - expbits - 1)) | significand;
}

long double
unpack754(uint64_t i, unsigned bits, unsigned expbits)
{
  long double result;
  long long shift;
  unsigned bias;
  unsigned significandbits = bits - expbits - 1; // -1 for sign bit

  if (i == 0)
    return 0.0;

  // pull the significand
  result = (i & ((1LL << significandbits) - 1)); // mask
  result /= (1LL << significandbits);            // convert back to float
  result += 1.0f;                                // add the one back on

  // deal with the exponent
  bias = (1 << (expbits - 1)) - 1;
  shift = ((i >> significandbits) & ((1LL << expbits) - 1)) - bias;
  while (shift > 0) {
    result *= 2.0;
    shift--;
  }
  while (shift < 0) {
    result /= 2.0;
    shift++;
  }

  // sign it
  result *= (i >> (bits - 1)) & 1 ? -1.0 : 1.0;

  return result;
}

static inline void
packi8(uint8_t* buf, int8_t v)
{
  *buf = (uint8_t)v;
}

static inline void
packu8(uint8_t* buf, uint8_t v)
{
  *buf = v;
}

static inline void
packi16(uint8_t* buf, int16_t v)
{
  uint16_t u = (uint16_t)v;
  buf[0] = (uint8_t)(u >> 8);
  buf[1] = (uint8_t)(u);
}

static inline void
packu16(uint8_t* buf, uint16_t v)
{
  buf[0] = (uint8_t)(v >> 8);
  buf[1] = (uint8_t)(v);
}

static inline void
packi32(uint8_t* buf, int32_t v)
{
  uint32_t u = (uint32_t)v;
  buf[0] = (uint8_t)(u >> 24);
  buf[1] = (uint8_t)(u >> 16);
  buf[2] = (uint8_t)(u >> 8);
  buf[3] = (uint8_t)(u);
}

static inline void
packu32(uint8_t* buf, uint32_t v)
{
  buf[0] = (uint8_t)(v >> 24);
  buf[1] = (uint8_t)(v >> 16);
  buf[2] = (uint8_t)(v >> 8);
  buf[3] = (uint8_t)(v);
}

static inline void
packi64(uint8_t* buf, int64_t v)
{
  uint64_t u = (uint64_t)v;
  buf[0] = (uint8_t)(u >> 56);
  buf[1] = (uint8_t)(u >> 48);
  buf[2] = (uint8_t)(u >> 40);
  buf[3] = (uint8_t)(u >> 32);
  buf[4] = (uint8_t)(u >> 24);
  buf[5] = (uint8_t)(u >> 16);
  buf[6] = (uint8_t)(u >> 8);
  buf[7] = (uint8_t)(u);
}

static inline void
packu64(uint8_t* buf, uint64_t v)
{
  buf[0] = (uint8_t)(v >> 56);
  buf[1] = (uint8_t)(v >> 48);
  buf[2] = (uint8_t)(v >> 40);
  buf[3] = (uint8_t)(v >> 32);
  buf[4] = (uint8_t)(v >> 24);
  buf[5] = (uint8_t)(v >> 16);
  buf[6] = (uint8_t)(v >> 8);
  buf[7] = (uint8_t)(v);
}

static inline int8_t
unpacki8(const uint8_t* buf)
{
  return (int8_t)buf[0];
}

static inline uint8_t
unpacku8(const uint8_t* buf)
{
  return buf[0];
}

static inline int16_t
unpacki16(const uint8_t* buf)
{
  uint16_t u = ((uint16_t)buf[0] << 8) | ((uint16_t)buf[1]);
  return (int16_t)u;
}

static inline uint16_t
unpacku16(const uint8_t* buf)
{
  return ((uint16_t)buf[0] << 8) | ((uint16_t)buf[1]);
}

static inline int32_t
unpacki32(const uint8_t* buf)
{
  uint32_t u = ((uint32_t)buf[0] << 24) | ((uint32_t)buf[1] << 16) |
               ((uint32_t)buf[2] << 8) | ((uint32_t)buf[3]);
  return (int32_t)u;
}

static inline uint32_t
unpacku32(const uint8_t* buf)
{
  return ((uint32_t)buf[0] << 24) | ((uint32_t)buf[1] << 16) |
         ((uint32_t)buf[2] << 8) | ((uint32_t)buf[3]);
}

static inline int64_t
unpacki64(const uint8_t* buf)
{
  uint64_t u = ((uint64_t)buf[0] << 56) | ((uint64_t)buf[1] << 48) |
               ((uint64_t)buf[2] << 40) | ((uint64_t)buf[3] << 32) |
               ((uint64_t)buf[4] << 24) | ((uint64_t)buf[5] << 16) |
               ((uint64_t)buf[6] << 8) | ((uint64_t)buf[7]);
  return (int64_t)u;
}

static inline uint64_t
unpacku64(const uint8_t* buf)
{
  return ((uint64_t)buf[0] << 56) | ((uint64_t)buf[1] << 48) |
         ((uint64_t)buf[2] << 40) | ((uint64_t)buf[3] << 32) |
         ((uint64_t)buf[4] << 24) | ((uint64_t)buf[5] << 16) |
         ((uint64_t)buf[6] << 8) | ((uint64_t)buf[7]);
}

size_t
ci2_pre_pack(const char* format, ...)
{
  /* Signed Formats */
  int8_t g;
  int16_t h;
  int32_t i;
  int64_t j;

  /* Unsigned Formats */
  uint8_t G;
  uint16_t H;
  uint32_t I;
  uint64_t J;

  /* Float Formats */
  float f;
  double F;
  // uint32_t l; // float32
  // uint64_t L; // double-64

  /* Strings */
  char* s;         // strings
  size_t len = 0;  // track string size
  size_t size = 0; // total pack size

  /* Args */
  va_list ap;
  va_start(ap, format);

  for (; *format != '\0'; format++) {
    switch (*format) {

      case ' ': // Ignore spaces
        break;

      case 'g': // 8-bit Signed
        size += 1;
        g = (int8_t)va_arg(ap, int); // promoted
        (void)g;
        break;

      case 'G': // 8-bit Unsigned
        size += 1;
        G = (uint8_t)va_arg(ap, int); // promoted
        (void)G;
        break;

      case 'h': // 16-bit Signed
        size += 2;
        h = (int16_t)va_arg(ap, int); // promoted
        (void)h;
        break;

      case 'H': // 16-bit Unsigned
        size += 2;
        H = (uint16_t)va_arg(ap, int); // promoted
        (void)H;
        break;

      case 'i': // 32-bit Signed
        size += 4;
        i = va_arg(ap, int32_t);
        (void)i;
        break;

      case 'I': // 32-bit Unsigned
        size += 4;
        I = va_arg(ap, uint32_t);
        (void)I;
        break;

      case 'j': // 64-bit Signed
        size += 8;
        j = va_arg(ap, int64_t);
        (void)j;
        break;

      case 'J': // 64-bit Unigned
        size += 8;
        J = va_arg(ap, uint64_t);
        (void)J;
        break;

      case 'f': // float 32-bit
        size += 4;
        f = (float)va_arg(ap, double); // promoted
        (void)f;
        break;

      case 'F': // double 64-bit
        size += 8;
        F = (double)va_arg(ap, double);
        (void)F;
        break;

      case 's': // string
        s = va_arg(ap, char*);
        len = strlen(s);
        size += len + 8;
        break;
    }
  }

  va_end(ap);

  return size;
}

size_t
ci2_pack(uint8_t* buf, const char* format, ...)
{
  /* Signed Formats */
  int8_t g;
  int16_t h;
  int32_t i;
  int64_t j;

  /* Unsigned Formats */
  uint8_t G;
  uint16_t H;
  uint32_t I;
  uint64_t J;

  /* Float Formats */
  float f;
  double F;
  uint32_t l; // float32
  uint64_t L; // double-64

  /* Strings */
  char* s;         // strings
  size_t len = 0;  // track string size
  size_t size = 0; // total pack size

  /* Args */
  va_list ap;
  va_start(ap, format);

  for (; *format != '\0'; format++) {
    switch (*format) {

      case ' ': // Ignore spaces
        break;

      case 'g': // 8-bit Signed
        size += 1;
        g = (int8_t)va_arg(ap, int); // promoted
        packi8(buf, g);
        buf += 1;
        break;

      case 'G': // 8-bit Unsigned
        size += 1;
        G = (uint8_t)va_arg(ap, int); // promoted
        packu8(buf, G);
        buf += 1;
        break;

      case 'h': // 16-bit Signed
        size += 2;
        h = (int16_t)va_arg(ap, int); // promoted
        packi16(buf, h);
        buf += 2;
        break;

      case 'H': // 16-bit Unsigned
        size += 2;
        H = (uint16_t)va_arg(ap, int); // promoted
        packu16(buf, H);
        buf += 2;
        break;

      case 'i': // 32-bit Signed
        size += 4;
        i = va_arg(ap, int32_t);
        packi32(buf, i);
        buf += 4;
        break;

      case 'I': // 32-bit Unsigned
        size += 4;
        I = va_arg(ap, uint32_t);
        packu32(buf, I);
        buf += 4;
        break;

      case 'j': // 64-bit Signed
        size += 8;
        j = va_arg(ap, int64_t);
        packi64(buf, j);
        buf += 8;
        break;

      case 'J': // 64-bit Unigned
        size += 8;
        J = va_arg(ap, uint64_t);
        packu64(buf, J);
        buf += 8;
        break;

      case 'f': // float 32-bit
        size += 4;
        f = (float)va_arg(ap, double); // promoted
        l = pack754_32(f);             // convert to IEEE 754
        packi32(buf, l);
        buf += 4;
        break;

      case 'F': // double 64-bit
        size += 8;
        F = (double)va_arg(ap, double);
        L = pack754_64(F); // convert to IEEE 754
        packi64(buf, L);
        buf += 8;
        break;

      case 's': // string
        s = va_arg(ap, char*);
        len = strlen(s);
        size += len + 8;
        packu64(buf, (uint64_t)len);
        buf += 8;
        memcpy(buf, s, len);
        buf += len;
        break;
    }
  }

  va_end(ap);

  return size;
}

size_t
ci2_unpack(uint8_t* buf, const char* format, ...)
{
  int8_t* g;
  int16_t* h;
  int32_t* i;
  int64_t* j;

  uint8_t* G;
  uint16_t* H;
  uint32_t* I;
  uint64_t* J;

  float* f;
  double* F;

  char* s;
  uint64_t len = 0;
  size_t count = 0;
  size_t size = 0;
  size_t maxstrlen = 0;

  va_list ap;
  va_start(ap, format);

  for (; *format != '\0'; format++) {
    switch (*format) {
      case 'g':
        g = va_arg(ap, int8_t*);
        *g = unpacki8(buf);
        buf += 1;
        size += 1;
        break;

      case 'G':
        G = va_arg(ap, uint8_t*);
        *G = unpacku8(buf);
        buf += 1;
        size += 1;
        break;

      case 'h':
        h = va_arg(ap, int16_t*);
        *h = unpacki16(buf);
        buf += 2;
        size += 2;
        break;

      case 'H':
        H = va_arg(ap, uint16_t*);
        *H = unpacku16(buf);
        buf += 2;
        size += 2;
        break;

      case 'i':
        i = va_arg(ap, int32_t*);
        *i = unpacki32(buf);
        buf += 4;
        size += 4;
        break;

      case 'I':
        I = va_arg(ap, uint32_t*);
        *I = unpacku32(buf);
        buf += 4;
        size += 4;
        break;

      case 'j':
        j = va_arg(ap, int64_t*);
        *j = unpacki64(buf);
        buf += 8;
        size += 8;
        break;

      case 'J':
        J = va_arg(ap, uint64_t*);
        *J = unpacku64(buf);
        buf += 8;
        size += 8;
        break;

      case 'f':
        f = va_arg(ap, float*);
        *f = unpack754_32(unpacku32(buf));
        buf += 4;
        size += 4;
        break;

      case 'F':
        F = va_arg(ap, double*);
        *F = unpack754_64(unpacku64(buf));
        buf += 8;
        size += 8;
        break;

      case 's':
        s = va_arg(ap, char*);
        len = unpacku64(buf);
        buf += 8;
        size += 8 + (size_t)len;

        if (maxstrlen > 0 && len > maxstrlen)
          count = maxstrlen - 1;
        else
          count = (size_t)len;

        memcpy(s, buf, count);
        s[count] = '\0';
        buf += len;
        break;

      default:
        if (isdigit((unsigned char)*format)) {
          maxstrlen = maxstrlen * 10 + (*format - '0');
        }
        break;
    }

    if (!isdigit((unsigned char)*format))
      maxstrlen = 0;
  }

  va_end(ap);
  return size;
}