/* This code is originally from the program wgrib by wesley ebisuzaki and 
   available at http://wesley.wwb.noaa.gov/

   Copyright (C) 2000 James P. Edwards (jedwards@inmet.gov.br).
   All rights reserved. There is no warranty. You are allowed
   to redistribute this software / documentation under certain 
   conditions. For details, see the file COPYING in the PDL
   distribution. If this file is separated from the PDL distribution,
   the copyright notice should be included in the file.
*/

#include <stdio.h>
#include <stddef.h>
#include <limits.h>
/*
#include "grib.h"
#include "pds4.h"
#include "bms.h"
#include "bds.h"
*/
#define UNDEFINED         9.999e20
/* 1996				wesley ebisuzaki
 *
 * Unpack BDS section
 *
 * input: *bits, pointer to packed integer data
 *        *bitmap, pointer to bitmap (undefined data), NULL if none
 *        n_bits, number of bits per packed integer
 *        n, number of data points (includes undefined data)
 *        ref, scale: flt[] = ref + scale*packed_int
 * output: *flt, pointer to output array
 *        undefined values filled with UNDEFINED
 *
 * note: code assumes an integer > 32 bits
 *
 * 7/98 v1.2.1 fix bug for bitmaps and nbit >= 25 found by Larry Brasfield
 */

static unsigned int mask[] = {0,1,3,7,15,31,63,127,255};
static unsigned int map_masks[8] = {128, 64, 32, 16, 8, 4, 2, 1};

void BDS_unpack(float *flt, unsigned char *bits, unsigned char *bitmap,
					 int n_bits, int n, double ref, double scale) {

  int i, mask_idx, t_bits, c_bits, j_bits;
  unsigned int j, map_mask, tbits, jmask, bbits;
  long int jj;

  tbits = bbits = 0;

  /* assume integer has 32+ bits */
  if (n_bits <= 25) {
	 jmask = (1 << n_bits) - 1;
	 t_bits = 0;
	 
	 if (bitmap) {
		for (i = 0; i < n; i++) {
		  /* check bitmap */
		  mask_idx = i & 7;
		  if (mask_idx == 0) bbits = *bitmap++;
		  if ((bbits & map_masks[mask_idx]) == 0) {
		    *flt++ = UNDEFINED;
		    continue;
		  }
		  
		  while (t_bits < n_bits) {
			 tbits = (tbits * 256) + *bits++;
			 t_bits += 8;
		  }
		  t_bits -= n_bits;
		  j = (tbits >> t_bits) & jmask;
		  *flt++ = ref + scale*j;
		}
	 }
	 else {
		for (i = 0; i < n; i++) {
		  while (t_bits < n_bits) {
			 tbits = (tbits * 256) + *bits++;
			 t_bits += 8;
		  }
		  t_bits -= n_bits;
		  flt[i] = (tbits >> t_bits) & jmask;
		}
		/* at least this vectorizes :) */
		for (i = 0; i < n; i++) {
		  flt[i] = ref + scale*flt[i];
		}
	 }
  }
  else {
	 /* older unoptimized code, not often used */
	 c_bits = 8;
	 map_mask = 128;
	 while (n-- > 0) {
		if (bitmap) {
		  j = (*bitmap & map_mask);
		  if ((map_mask >>= 1) == 0) {
		    map_mask = 128;
		    bitmap++;
		  }
		  if (j == 0) {
		    *flt++ = UNDEFINED;
		    continue;
		  }
		}
		
		jj = 0;
		j_bits = n_bits;
		while (c_bits <= j_bits) {
		  if (c_bits == 8) {
		    jj = (jj << 8) + *bits++;
		    j_bits -= 8;
		  }
		  else {
		    jj = (jj << c_bits) + (*bits & mask[c_bits]);
		    bits++;
		    j_bits -= c_bits;
		    c_bits = 8;
		  }
		}
		if (j_bits) {
		  c_bits -= j_bits;
		  jj = (jj << j_bits) + ((*bits >> c_bits) & mask[j_bits]);
		}
		*flt++ = ref + scale*jj;
	 }
  }
  return;
}
