#include "grib_int.h"

// clang-format off

/* pack 8-bit bytes from 64-bit words to a packed buffer */
/* same as : for (int i = 0; i < bc; ++i) cp[i] = (unsigned char) up[i]; */

long packInt64(uint64_t *up, unsigned char *cp, long bc, long tc)
{
#if defined (CRAY)
  (void) _pack(up, cp, bc, tc);
#else
  unsigned char *cp0;
  uint64_t upi, *up0, *ip0, *ip1, *ip2, *ip3, *ip4, *ip5, *ip6, *ip7;
  long ipack = sizeof(int64_t);
  
  // Bytes until first word boundary in destination buffer

  long head = ( (long) cp ) & (ipack-1);
  if ( head != 0 ) head = ipack - head;

  long inner = bc - head;

  // Trailing bytes which do not make a full word

  long trail = inner & (ipack-1);

  // Number of bytes/words to be processed in fast loop

  inner -= trail;
  inner /= ipack;

  ip0 = up + head;
  ip1 = ip0 + 1;
  ip2 = ip0 + 2;
  ip3 = ip0 + 3;
  ip4 = ip0 + 4;
  ip5 = ip0 + 5;
  ip6 = ip0 + 6;
  ip7 = ip0 + 7;

  up0 = (uint64_t *)(void *)(cp + head);

  /* Here we should process any bytes until the first word boundary 
   * of our destination buffer 
   * That code is missing so far  because our output buffer is 
   * word aligned by FORTRAN 
   */

  long j = 0;

  if ( IS_BIGENDIAN() )
    {
#if defined (CRAY)
#pragma _CRI ivdep
#endif
#if defined (SX)
#pragma vdir nodep
#endif
#ifdef __uxpch__
#pragma loop novrec
#endif
      for (long i = 0; i < inner; ++i)
	{
	  upi =             (   ip0[j]          << 56 ) 
	                 |  ( ( ip1[j] & 0xFF ) << 48 )
	                 |  ( ( ip2[j] & 0xFF ) << 40 )
	                 |  ( ( ip3[j] & 0xFF ) << 32 )
	                 |  ( ( ip4[j] & 0xFF ) << 24 ) ;
	  up0[i] = upi   |  ( ( ip5[j] & 0xFF ) << 16 )
	                 |  ( ( ip6[j] & 0xFF ) <<  8 )
	                 |    ( ip7[j] & 0xFF ) ;
	  j += ipack;
	}
    }
  else
    {
      for (long i = 0; i < inner; ++i)
	{
	  upi =             (   ip7[j]          << 56 ) 
	                 |  ( ( ip6[j] & 0xFF ) << 48 )
                         |  ( ( ip5[j] & 0xFF ) << 40 )
                         |  ( ( ip4[j] & 0xFF ) << 32 )
                         |  ( ( ip3[j] & 0xFF ) << 24 ) ;
	  up0[i] = upi   |  ( ( ip2[j] & 0xFF ) << 16 )
                         |  ( ( ip1[j] & 0xFF ) <<  8 )
                         |    ( ip0[j] & 0xFF ) ;
	  j += ipack;
	}
    }

  cp0 = (unsigned char *) ( up0 + inner );
  if ( trail > 0 )
    {
      up0[inner] = 0;
      for (long i = 0; i < trail ; ++i)
	{
	  *cp0 = (unsigned char) ip0[ipack*inner+i];
	  cp0++;
	}
    }

  if ( tc != -1 )
    {
      bc++;
      *cp0 = (unsigned char) tc;
    }
#endif
  return (bc);
}

/* unpack 8-bit bytes from a packed buffer with 64-bit words */
/* same as : for (int i = 0; i < bc; ++i) up[i] = (int64_t) cp[i]; */

long unpackInt64(const unsigned char *cp, uint64_t *up, long bc, long tc)
{
  const unsigned char *cp0;
  uint64_t *ip0, *ip1, *ip2, *ip3, *ip4, *ip5, *ip6, *ip7;
  long offset;
  long ipack = sizeof(int64_t);

  UNUSED(tc);

  // Bytes until first word boundary in source buffer

  long head = ( (long) cp ) & (ipack-1);
  if ( head != 0 ) head = ipack - head;
  if ( head > bc ) head = bc;

  long inner = bc - head;

  // Trailing bytes which do not make a full word
 
  long trail = inner & (ipack-1);
 
  // Number of bytes/words to be processed in fast loop

  inner -= trail;
  inner /= ipack;

  ip0 = up + head;
  ip1 = ip0 + 1;
  ip2 = ip0 + 2;
  ip3 = ip0 + 3;
  ip4 = ip0 + 4;
  ip5 = ip0 + 5;
  ip6 = ip0 + 6;
  ip7 = ip0 + 7;

  const uint64_t *up0 = (const uint64_t *)(const void *)(cp + head);

  /* Process any bytes until the first word boundary 
   * of our source buffer 
   */
  for (long i = 0; i < head; ++i) up[i] = (uint64_t) cp[i];

  long j = 0;

  if ( IS_BIGENDIAN() )
    {
#if defined (CRAY)
#pragma _CRI ivdep
#endif
#if defined (SX)
#pragma vdir nodep
#endif
#ifdef __uxpch__
#pragma loop novrec
#endif
      for (long i = 0; i < inner; ++i)
	{
	  ip0[j] = (up0[i] >> 56) & 0xFF;
	  ip1[j] = (up0[i] >> 48) & 0xFF;
	  ip2[j] = (up0[i] >> 40) & 0xFF;
	  ip3[j] = (up0[i] >> 32) & 0xFF;
	  ip4[j] = (up0[i] >> 24) & 0xFF;
	  ip5[j] = (up0[i] >> 16) & 0xFF;
	  ip6[j] = (up0[i] >>  8) & 0xFF;
	  ip7[j] = (up0[i])       & 0xFF;

	  j += ipack;
	}
    }
  else
    {
      for (long i = 0; i < inner; ++i)
	{
	  ip7[j] = (up0[i] >> 56) & 0xFF;
	  ip6[j] = (up0[i] >> 48) & 0xFF;
	  ip5[j] = (up0[i] >> 40) & 0xFF;
	  ip4[j] = (up0[i] >> 32) & 0xFF;
	  ip3[j] = (up0[i] >> 24) & 0xFF;
	  ip2[j] = (up0[i] >> 16) & 0xFF;
	  ip1[j] = (up0[i] >>  8) & 0xFF;
	  ip0[j] = (up0[i])       & 0xFF;

	  j += ipack;
	}
    }

  if ( trail > 0 )
    {
      offset = head + ipack*inner;
      cp0 = cp + offset;
      for (long i = 0; i < trail; ++i) up[i+offset] = (uint64_t) cp0[i];
    }
  /*
  if ( tc != -1 ) {
    bc++;
    *cp0 = (unsigned char) tc;
  }
  */
  return bc;
}

/* pack 8-bit bytes from 32-bit words to a packed buffer */
/* same as : for (int i = 0; i < bc; ++i) cp[i] = (char) up[i]; */

long packInt32(uint32_t  *up, unsigned char *cp, long bc, long tc)
{
  unsigned char *cp0;
  uint32_t *up0, *ip0, *ip1, *ip2, *ip3;
  long ipack = sizeof(int32_t);
  
  // Bytes until first word boundary in destination buffer

  long head = ( (long) cp ) & (ipack-1);
  if ( head != 0 ) head = ipack - head;

  long inner = bc - head;

  // Trailing bytes which do not make a full word

  long trail = inner & (ipack-1);

  // Number of bytes/words to be processed in fast loop

  inner -= trail;
  inner /= ipack;

  ip0 = up + head;
  ip1 = ip0 + 1;
  ip2 = ip0 + 2;
  ip3 = ip0 + 3;

  up0 = (uint32_t *)(void *)(cp + head);

  /* Here we should process any bytes until the first word boundary 
   * of our destination buffer 
   * That code is missing so far  because our output buffer is 
   * word aligned by FORTRAN 
   */

  long j = 0;

  if ( IS_BIGENDIAN() )
    {
#if defined (CRAY)
#pragma _CRI ivdep
#endif
#if defined (SX)
#pragma vdir nodep
#endif
#ifdef __uxpch__
#pragma loop novrec
#endif
      for (long i = 0; i < inner; ++i)
	{
	  up0[i] =          (   ip0[j]          << 24 ) 
	                 |  ( ( ip1[j] & 0xFF ) << 16 )
	                 |  ( ( ip2[j] & 0xFF ) <<  8 )
	                 |    ( ip3[j] & 0xFF ) ;
	  j += ipack;
	}
    }
  else
    {
      for (long i = 0; i < inner; ++i)
	{
	  up0[i] =          (   ip3[j]          << 24 ) 
	                 |  ( ( ip2[j] & 0xFF ) << 16 )
                         |  ( ( ip1[j] & 0xFF ) <<  8 )
                         |    ( ip0[j] & 0xFF ) ;
	  j += ipack;
	}
    }

  cp0 = (unsigned char *) ( up0 + inner );
  if ( trail > 0 )
    {
      up0[inner] = 0;
      for (long i = 0; i < trail; ++i)
	{
	  *cp0 = (unsigned char) ip0[ipack*inner+i];
	  cp0++;
	}
    }

  if ( tc != -1 )
    {
      bc++;
      *cp0 = (unsigned char) tc;
    }

  return (bc);
}

/* unpack 8-bit bytes from a packed buffer with 32-bit words */
/* same as : for (int i = 0; i < bc; ++i) up[i] = (int32_t) cp[i]; */

long unpackInt32(const unsigned char *cp, uint32_t *up, long bc, long tc)
{
  const unsigned char *cp0;
  uint32_t *ip0, *ip1, *ip2, *ip3;
  long offset;
  long ipack = sizeof(int32_t);

  UNUSED(tc);

  // Bytes until first word boundary in source buffer

  long head = ( (long) cp ) & (ipack-1);
  if ( head != 0 ) head = ipack - head;
  if ( head > bc ) head = bc;

  long inner = bc - head;

  // Trailing bytes which do not make a full word
 
  long trail = inner & (ipack-1);
 
  // Number of bytes/words to be processed in fast loop

  inner -= trail;
  inner /= ipack;

  ip0 = up + head;
  ip1 = ip0 + 1;
  ip2 = ip0 + 2;
  ip3 = ip0 + 3;

  const uint32_t *up0 = (const uint32_t *)(const void *)(cp + head);

  /* Process any bytes until the first word boundary 
   * of our source buffer 
   */
  for (long i = 0; i < head; ++i) up[i] = (uint32_t) cp[i];

  long j = 0;

  if ( IS_BIGENDIAN() )
    {
#if defined (CRAY)
#pragma _CRI ivdep
#endif
#if defined (SX)
#pragma vdir nodep
#endif
#ifdef __uxpch__
#pragma loop novrec
#endif
      for (long i = 0; i < inner; ++i)
	{
	  ip0[j] = (up0[i] >> 24) & 0xFF;
	  ip1[j] = (up0[i] >> 16) & 0xFF;
	  ip2[j] = (up0[i] >>  8) & 0xFF;
	  ip3[j] = (up0[i])       & 0xFF;

	  j += ipack;
	}
    }
  else
    {
      for (long i = 0; i < inner; ++i)
	{
	  ip3[j] = (up0[i] >> 24) & 0xFF;
	  ip2[j] = (up0[i] >> 16) & 0xFF;
	  ip1[j] = (up0[i] >>  8) & 0xFF;
	  ip0[j] = (up0[i])       & 0xFF;

	  j += ipack;
	}
    }

  if ( trail > 0 )
    {
      offset = head + ipack*inner;
      cp0 = cp + offset;
      for (long i = 0; i < trail; ++i) up[i+offset] = (uint32_t) cp0[i];
    }
  /*
  if ( tc != -1 ) {
    bc++;
    *cp0 = (unsigned char) tc;
  }
  */

  return (bc);
}

// clang-format on
