/* *
   File:        pivec.c
   Author:      Anthony DiLello
   Created:     Mon June 2 2:40:27 EDT 1997
   Description: Implementation of pivec datatype. Warning: this datatype will only 
                 work properly on machines with 32 bit integers. 

   Copyright (C) 1997, CMU
   
   Updated by AWM to do fast loads and saves with block writes. And to
   allow max_value to be zero.

   And to allow pvectors : packed arrays of floats.
*/

#include "pivec.h"

/* Private prototypes */
static int pivec_max_value_to_bits(int max_value);
static void awm_put_sixbits(int sixbits,FILE *s,int *chars_on_line);
static int awm_get_sixbits(FILE *s);
static void awm_fwrite_32bit_word(int word,FILE *s,int *chars_on_line);
static int awm_fwrite_32bit_words(void *array,int num_words,FILE *s);
static unsigned int awm_fread_32bit_word(FILE *s,bool *r_end_of_file);
static int awm_fread_32bit_words(void *array,int num_words,FILE *s);

/* in fast mode I found this code to return the value 6 when max_value was 31.
   It should be 5 because 31+1 requires only 5 bits.  Somehow the ceil of the
   log used was bumping what looked like 5.000000000 up to a six.  This is
   both incorrect and wasn't consistent between debug and fast modes, which
   meant that an fds file created with one couldn't be read by the other.
   The eps was added to prevent this.  Note that although this is a bad hack
   to be using on such a basic function it should be safe because the ratio
   of those logs is only close to an integral number when it is exactly that
   number (and can have eps safely subtracted) or when max_value exceeds 
   (at least) 10^6
   JGS  10-08-02.  
*/
static int pivec_max_value_to_bits(int max_value)
{
  double eps = 1.0e-10;
  
  /*if (max_value > 0)
    printf("pivec_max_value_to_bits: %d => %d %.40f\n",max_value,my_irint(ceil(log(max_value+1.0)/log(2.0)-eps)),log(max_value+1.0)/log(2.0)-eps);
  */
  return (max_value == 0) ? 0:my_irint(ceil(log(max_value+1.0)/log(2.0)-eps));
}

pivec *mk_pivec(int size,int max_value) /* Makes an array of size "size"
                                           in which we promise that all entries
                                           stored in the array will be between
                                           0 <= array_element <= max_value */
                                        /* The initial values are undefined */
{
  pivec *pi;
  int bits, vec_size, nums_per_word;
  int thirty_two = 32;

  if(max_value<0) my_error("mk_pivec error: max_value should be >= 0.\n");

  if(size < 0) my_error("mk_pivec error: size should be >= 0.\n");

  if ( max_value == 0 )
  {
    bits = 0;
    vec_size = 0;
    nums_per_word = -1;
  }
  else
  {
    bits = pivec_max_value_to_bits(max_value);
    nums_per_word = 32/bits;
    vec_size = (int)ceil((double)size/((double)nums_per_word));
  }

  if(sizeof(int)*8 != thirty_two) 
    my_error("mk_pivec error: The machine word size should be 32 bits.\n");

  pi = AM_MALLOC(pivec);
  (*pi).vec = AM_MALLOC_ARRAY(int, vec_size);

  (*pi).size = size;
  (*pi).max_value = max_value;
  (*pi).bits = bits;
  (*pi).vec_size = vec_size;
  (*pi).nums_per_word = nums_per_word;

  return pi;
}

/* If the new_max_value is an increase, the stored values remain unchanged.
   If it's a decrease, then an previous value > max_value is replaced
   by max_value. */
void pivec_set_max_value(pivec *pi,int new_max_value)
{
  int old_max_value = pivec_max_value(pi);
  int old_bits = pivec_max_value_to_bits(old_max_value);
  int new_bits = pivec_max_value_to_bits(new_max_value);
  if ( old_bits == new_bits )
    pi -> max_value = new_max_value;
  else
  {
    pivec *pi2 = mk_pivec(pivec_size(pi),new_max_value);
    int i;
    for ( i = 0 ; i < pivec_size(pi) ; i++ )
      pivec_set(pi2,i,int_min(new_max_value,pivec_ref(pi,i)));
    /* Now a hacky bit... */
    AM_FREE_ARRAY(pi->vec,int,pi->vec_size);
    pi -> size = pi2 -> size;
    pi -> max_value = pi2 -> max_value;
    pi -> vec = pi2 -> vec;
    pi -> bits = pi2 -> bits;
    pi -> vec_size = pi2 -> vec_size;
    pi -> nums_per_word = pi2 -> nums_per_word;
    AM_FREE(pi2,pivec);
  }
}

pivec *mk_zero_pivec(int size,int max_value) /* As above except that the
                                              initial values are all zero. */
{
  int i;
  pivec *pi;

  pi = mk_pivec(size, max_value);

  if ( pi->bits > 0 )
  {
    for(i = 0; i<pi->vec_size; i++)
      (*pi).vec[i] = 0;
  }

  return pi;
}

int pivec_ref(pivec *pi,int index) /* returns the value in the index'th entry
                                   of the pivec. my_error's if index out
                                   of range. */
{
  int value;

  if(pi == NULL)
    my_error("pivec_ref error: Null pivec pointer passed in.\n");

  if(index >= (*pi).size||index<0)
    my_error("pivec_ref error: index out of bounds\n");

  if ( pi->bits == 0 )
    value = 0;
  else
  {
    int i = index/(*pi).nums_per_word;   
    unsigned int buffer = (*pi).vec[i];
    int shift_left = (*pi).bits*(index%(*pi).nums_per_word);
    int shift_right = 32-(*pi).bits;
    value = (buffer << shift_left) >> shift_right;
  }
  return value;
}

/* sets the index'th element to be value. my_error's if
   index out of range or value > max_value */

void pivec_set(pivec *pi,int index,int value) 
{
  if(pi == NULL)
    my_error("pivec_set error: Null pivec pointer passed in.\n");

  if(index>=(*pi).size)
    my_error("pivec_set error: array index out of bounds.\n");

  if(value>(*pi).max_value || value < 0)
    my_errorf("pivec_set error: value should be 0 <= value <= %d\n"
	      "but in fact value = %d so is out of bounds.\n",
	      pi->max_value,value);

  if ( pi->bits > 0 )
  {
    unsigned int bitmask=0;
    int i = index/(*pi).nums_per_word;
    unsigned int buffer = (*pi).vec[i];
    int bits = (*pi).bits;
    int position = index%(*pi).nums_per_word;
    int s1 = position*bits;
    int s2 = 32 - s1 - bits;
    int j;
    for(j=0; j<s1; j++)
    {
      bitmask *= 2;
      bitmask++;
    }
  
    bitmask = bitmask<<bits;

    for(j=0; j<s2; j++)
    {
      bitmask *= 2;
      bitmask++;
    }

    buffer = buffer&bitmask;
    value = value<<s2;
    buffer += value;

    (*pi).vec[i] = buffer;
  }
}

int pivec_size(pivec *pi) /* returns number of array entries */
{
  if(pi == NULL)
    my_error("pivec_size error: Null pivec pointer passed in.\n");

  return (*pi).size;
}

int pivec_sum(pivec *pi)
{
  int sum = 0;
  int i;
  for (i=0;i<pivec_size(pi);i++) sum += pivec_ref(pi,i);
  return sum;
}

int pivec_max_value(pivec *pi) /* returns the max_value for the pivec */
{
  if(pi == NULL)
    my_error("pivec_max_value error: Null pivec pointer passed in.\n");

  return (*pi).max_value;
}

void free_pivec(pivec *pi)
{
  if(pi == NULL)
    return;

  AM_FREE_ARRAY((*pi).vec, int, (*pi).vec_size);
  AM_FREE(pi, pivec);
} 

pivec *mk_copy_pivec(pivec *pi)
{
  pivec *pi2;
  int i, size;;

  if(pi == NULL)
    my_error("mk_copy_pivec error: Null pivec pointer passed in.\n");

  pi2 = AM_MALLOC(pivec);
  (*pi2).vec = AM_MALLOC_ARRAY(int, (*pi).vec_size);

  (*pi2).size = (*pi).size;
  (*pi2).max_value = (*pi).max_value;
  (*pi2).bits = (*pi).bits;
  (*pi2).vec_size = (*pi).vec_size;
  (*pi2).nums_per_word = (*pi).nums_per_word;

  size = (*pi).size;
  for(i=0; i<(*pi).vec_size; i++)
    (*pi2).vec[i] = (*pi).vec[i];

  return pi2;
}

ivec *mk_ivec_from_pivec(pivec *pi)
{
  int i, size =(*pi).size;
  ivec *iv = mk_ivec(size);

  if(pi == NULL)
    my_error("mk_ivec_from_pivec error: Null pivec pointer passed in.\n");

  for(i = 0; i<size; i++)
    ivec_set(iv, i, pivec_ref(pi, i));

  return iv;
}

pivec *mk_pivec_from_ivec(ivec *iv) /* Uses ivec_max(i) to deduce the max_value
                                      that should be used. Error if ivec contains
                                       any negative numbers */
{
  int size, i;
  pivec *pi;

  if(iv == NULL)
    my_error("mk_pivec_from_ivec error: Null ivec pointer passed in.\n");

  size = ivec_size(iv);

  if ( size == 0 )
    pi = mk_pivec(0,0);
  else
  {
    int max = ivec_max(iv);
    int min = ivec_min(iv);

    if(min<0)
      my_error("mk_pivec_from_ivec error: ivec contains a negative entry.\n");

    pi = mk_pivec(size, max);
    for(i=0; i<size; i++)
      pivec_set(pi, i, ivec_ref(iv, i));
  }

  return pi;
}

void fprintf_pivec(FILE *s,char *m1,pivec *pi,char *m2)
{
  ivec *iv = mk_ivec_from_pivec(pi);

  fprintf_ivec(s, m1, iv, m2);
  free_ivec(iv);
}

/*#define BASE_CHAR ((char)(0x21))*/
char BASE_CHAR = 0x21;

void set_base_encoding_to_xml()
{
  BASE_CHAR = 0x3F;
}

void set_base_encoding_to_normal()
{
  BASE_CHAR = 0x21;
}

static void awm_put_sixbits(int sixbits,FILE *s,int *chars_on_line)
{
#ifndef AMFAST
  if ( sixbits < 0 || sixbits > 0x3F )
    my_error("awm_put_sixbits");
#endif
  if ( *chars_on_line > 75 )
  {
    fprintf(s,"\n");
    *chars_on_line = 0;
  }
  putc((char)(BASE_CHAR+sixbits),s);
  *chars_on_line += 1;
}

static int awm_get_sixbits(FILE *s)
{
  int c1 = clever_fgetc(s);
  int sixbits;

  while ( c1 == ' ' || c1 == '\n' || c1 == 0x0D )
  {
    c1 = clever_fgetc(s);
  }

  if ( c1 == EOF )
    sixbits = EOF;
  else
  {
    sixbits = (int) (c1 - BASE_CHAR);
#ifndef AMFAST
    if ( sixbits < 0 || sixbits > 0x3F )
      my_error("awm_get_sixbits");
#endif
  }

  return sixbits;
}

static void awm_fwrite_32bit_word(int word,FILE *s,int *chars_on_line)
{
  int i;
  for ( i = 0 ; i < 32 ; i += 6 )
    awm_put_sixbits((word >> i) & 0x3F,s,chars_on_line);
}

static int awm_fwrite_32bit_words(void *array,int num_words,FILE *s)
{
  unsigned int *arr = (unsigned int *)array;
  int i;
  int chars_on_line = 0;

  for ( i = 0 ; i < num_words ; i++ )
    awm_fwrite_32bit_word(arr[i],s,&chars_on_line);
  return(num_words);
}

static unsigned int awm_fread_32bit_word(FILE *s,bool *r_end_of_file)
{
  int i;
  int result = 0;
  *r_end_of_file = FALSE;
  for ( i = 0 ; i < 32 && !(*r_end_of_file) ; i += 6 )
  {
    int c = awm_get_sixbits(s);
    if ( c == EOF )
      *r_end_of_file = TRUE;
    else
      result = result | (c << i);
  }

  if ( *r_end_of_file )
    result = 0;

  return result;
}
  
int awm_fread_32bit_words(void *array,int num_words,FILE *s)
{
  unsigned int *arr = (unsigned int *)array;
  int i;
  int result = -1;
  for ( i = 0 ; result < 0 && i < num_words ; i++ )
  {
    bool end_of_file;
    unsigned int word = awm_fread_32bit_word(s,&end_of_file);
    if ( end_of_file )
    {
      printf("unexpected eof\n");
      wait_for_key();
      result = i;
    }
    else
      arr[i] = word;
  }
  if ( result < 0 )
    result = num_words;
  return(result);
}

void save_pivec_fast(FILE *s,pivec *pv)
{
  int check;
  int four = 4;

  if (pv)
  {
    fprintf(s,"pivec fast size %d max_value %d\n",
	    pivec_size(pv),pivec_max_value(pv));
    fprintf(s,"X");

    if ( sizeof(int) != four ) my_error("ints must be 4 bytes");

    check = awm_fwrite_32bit_words((void *)(pv->vec),pv->vec_size,s);
    if ( check != pv->vec_size ) my_error("fwrite screwup");

    fprintf(s,"\nend pivec fast\n");
  }
  else fprintf(s,"pivec fast NULL\n");
}

pivec *mk_load_pivec_fast(FILE *s,int *line,char **r_errmess)
{
  string_array *sa = mk_next_interesting_line(s,line);
  pivec *pi = NULL;
  *r_errmess = NULL;
  if ( sa != NULL )
  {
    if ((string_array_size(sa) == 3) &&
	eq_string("pivec",string_array_ref(sa,0)) &&
	eq_string("fast",string_array_ref(sa,1)) &&
	eq_string("NULL",string_array_ref(sa,2)))
    {
      pi = NULL;
    }
    else if (string_array_size(sa) == 6 &&
	     eq_string("pivec",string_array_ref(sa,0)) &&
	     eq_string("fast",string_array_ref(sa,1)) &&
	     eq_string("size",string_array_ref(sa,2)) &&
	     is_a_number(string_array_ref(sa,3)) &&
	     eq_string("max_value",string_array_ref(sa,4)) &&
	     is_a_number(string_array_ref(sa,5)))
    { /* This is the code path where things actually happen */
      int size = atoi(string_array_ref(sa,3));
      int max_value = atoi(string_array_ref(sa,5));
      int num_read;
      int c = ' ';
      pi = mk_pivec(size,max_value);

      while ( c != 'X' && c != EOF )
        c = clever_fgetc(s);

      num_read = awm_fread_32bit_words((void *)(pi->vec),pi->vec_size,s);

      if ( num_read != pi->vec_size )
      {
        printf("feof(s) = %d\n",feof(s));
        printf("ferror(s) = %d\n",ferror(s));
        *r_errmess = mk_copy_string("Unexpectedly little data reading pivec");
      }
      else
      {
        free_string_array(sa);
        sa = mk_next_interesting_line(s,line);
        if ( sa == NULL ||
             string_array_size(sa) != 3 ||
             !eq_string("end",string_array_ref(sa,0)) ||
             !eq_string("pivec",string_array_ref(sa,1)) ||
             !eq_string("fast",string_array_ref(sa,2)) )
          *r_errmess = mk_copy_string("Expected pivec to be terminated by "
                           "end pivec fast");
      }
    }
    else
    {
      *r_errmess = mk_copy_string("Fast Pivec syntax error. "
                     "Expected: pivec fast size <number> max_value <number>"
                     "          pivec fast NULL");
    }
  }

  if ( sa != NULL ) free_string_array(sa);
  if ( *r_errmess != NULL && pi != NULL )
  {
    free_pivec(pi);
    pi = NULL;
  }

  return(pi);
}
  
/*********** PACKED VECTORS OF REALS ****/

pvector *mk_pvector(int size)
{
  pvector *pv = AM_MALLOC(pvector);
  pv->size = size;
  pv->array = AM_MALLOC_ARRAY(float,size);
  return(pv);
}

int pvector_size(pvector *pv)
{
  return(pv->size);
}

double pvector_ref(pvector *pv,int index)
{
  if ( index < 0 || index >= pv->size )
    my_error("pvector_ref");
  return((double)(pv->array[index]));
}

void pvector_set(pvector *pv,int index,double value)
{
  if ( index < 0 || index >= pv->size ) {
    my_errorf("pvector_set: index %d out of bounds [0,%d)", index, pv->size);
  }
  pv->array[index] = (float) value;
}

void zero_pvector(pvector *pv)
{
  int i;
  for ( i = 0 ; i < pv->size ; i++ )
    pvector_set(pv,i,0.0);
}

pvector *mk_zero_pvector(int size)
{
  pvector *pv = mk_pvector(size);
  zero_pvector(pv);
  return(pv);
}

void free_pvector(pvector *pv)
{
  AM_FREE_ARRAY(pv->array,float,pv->size);
  AM_FREE(pv,pvector);
}

pvector *mk_copy_pvector(pvector *pv)
{
  pvector *newpv = mk_pvector(pvector_size(pv));
  int i;
  for ( i = 0 ; i < pvector_size(pv) ; i++ )
    pvector_set(newpv,i,pvector_ref(pv,i));
  return(newpv);
}

dyv *mk_dyv_from_pvector(pvector *pv)
{
  dyv *dv = mk_dyv(pvector_size(pv));
  int i;
  for ( i = 0 ; i < dyv_size(dv) ; i++ )
    dyv_set(dv,i,pvector_ref(pv,i));
  return(dv);
}

pvector *mk_pvector_from_dyv(dyv *dv)
{
  pvector *pv = mk_pvector(dyv_size(dv));
  int i;
  for ( i = 0 ; i < dyv_size(dv) ; i++ )
    pvector_set(pv,i,dyv_ref(dv,i));
  return(pv);
}

void fprintf_pvector(FILE *s,char *m1,pvector *pv,char *m2)
{
  dyv *dv = mk_dyv_from_pvector(pv);
  fprintf_dyv(s,m1,dv,m2);
  free_dyv(dv);
}

void save_pvector_fast(FILE *s,pvector *pv)
{
  int four = 4;
  fprintf(s,"pvector fast size %d\n",
          pvector_size(pv));

  if ( sizeof(float) != four ) my_error("floats must be 4 bytes");

  awm_fwrite_32bit_words((void *)(pv->array),pv->size,s);
  fprintf(s,"\nend pvector fast\n");
}

pvector *mk_load_pvector_fast(FILE *s,int *line,char **r_errmess)
{
  string_array *sa = mk_next_interesting_line(s,line);
  pvector *pv = NULL;
  *r_errmess = NULL;
  if ( sa != NULL )
  {
    if ( string_array_size(sa) != 4 ||
         !eq_string("pvector",string_array_ref(sa,0)) ||
         !eq_string("fast",string_array_ref(sa,1)) ||
         !eq_string("size",string_array_ref(sa,2)) ||
         !is_a_number(string_array_ref(sa,3)) )
    {
      *r_errmess = mk_copy_string("Fast Pvector syntax error. "
                     "Expected: pvector fast size <number>");
    }
    else
    {
      int size = atoi(string_array_ref(sa,3));
      int num_read;
      pv = mk_pvector(size);
      num_read = awm_fread_32bit_words((char *)(pv->array),pv->size,s);
      if ( num_read != pv->size )
        *r_errmess =mk_copy_string("Unexpectedly little data reading pvector");
      else
      {
        free_string_array(sa);
        sa = mk_next_interesting_line(s,line);
        if ( sa == NULL ||
             string_array_size(sa) != 3 ||
             !eq_string("end",string_array_ref(sa,0)) ||
             !eq_string("pvector",string_array_ref(sa,1)) ||
             !eq_string("fast",string_array_ref(sa,2)) )
          *r_errmess = mk_copy_string("Expected pvector to be terminated by "
                           "end pvector fast");
      }
    }
  }

  if ( sa != NULL ) free_string_array(sa);
  if ( *r_errmess != NULL && pv != NULL )
  {
    free_pvector(pv);
    pv = NULL;
  }

  return(pv);
}

int pivec_num_bytes(pivec *pi)
{
  return(sizeof(pivec) + pi->vec_size * sizeof(int));
}

/**** The following functions will not change even if the internals
      if pivecs or pvectors change ***/

/* PRE: 0 <= start_index <= end_index <= size
   Returns a new pivec of size end_index - start_index
   consisting of 
     { piv[start_index] , piv[start_index+1] ... piv[end_index-1] }
*/
pivec *mk_pivec_segment(pivec *x,int start_index,int end_index)
{
  int seg_size = end_index - start_index;
  pivec *seg = mk_pivec(seg_size,pivec_max_value(x));
  int i;
  for ( i = 0 ; i < seg_size ; i++ )
    pivec_set(seg,i,pivec_ref(x,start_index+i));
  return seg;
}

/* As above except take the elements given in subset */
pivec *mk_pivec_subset(pivec *x,ivec *subset)
{
  int seg_size = ivec_size(subset);
  pivec *seg = mk_pivec(seg_size,pivec_max_value(x));
  int i;
  for ( i = 0 ; i < seg_size ; i++ )
    pivec_set(seg,i,pivec_ref(x,ivec_ref(subset,i)));
  return seg;
}

/* Returns a copy of x, except one in which the size is increased by n,
   and in which the final n elements have value 0 */
pivec *mk_add_elements_to_pivec(pivec *x,int n)
{
  int old_size = pivec_size(x);
  int new_size = old_size + n;
  int maxval = pivec_max_value(x);
  int i;
  pivec *y = mk_pivec(new_size,maxval);
  for ( i = 0 ; i < old_size ; i++ )
    pivec_set(y,i,pivec_ref(x,i));
  for ( i = old_size ; i < new_size ; i++ )
    pivec_set(y,i,0);
  return y;
}

int pivec_num_non_zero(pivec *piv)
{
  int result = 0;
  int i;
  for ( i = 0 ; i < pivec_size(piv) ; i++ )
    if ( pivec_ref(piv,i) != 0 )
      result += 1;
  return result;
}

/* Just like mk_copy_pivec, except survives if input is NULL (in
   which case it makes no copy and just returns NULL) */
pivec *mk_copy_pivec_maybe_null(pivec *x)
{
  return (x == NULL) ? NULL : mk_copy_pivec(x);
}

/* Just like fprintf_pivec except survives when input is NULL, and 
   prints nicely accordingly. */
void fprintf_pivec_maybe_null(FILE *s,char *m1,pivec *x,char *m2)
{
  if ( x == NULL ) 
    fprintf(s,"%s = NULL%s",m1,m2);
  else
    fprintf_pivec(s,m1,x,m2);
}

/* Just like mk_copy_pvector, except survives if input is NULL (in
   which case it makes no copy and just returns NULL) */
pvector *mk_copy_pvector_maybe_null(pvector *x)
{
  return (x == NULL) ? NULL : mk_copy_pvector(x);
}

/* Just like fprintf_pvector except survives when input is NULL, and 
   prints nicely accordingly. */
void fprintf_pvector_maybe_null(FILE *s,char *m1,pvector *x,char *m2)
{
  if ( x == NULL ) 
    fprintf(s,"%s = NULL%s",m1,m2);
  else
    fprintf_pvector(s,m1,x,m2);
}

/* PRE: 0 <= start_index <= end_index <= size
   Returns a new pvector of size end_index - start_index
   consisting of 
     { piv[start_index] , piv[start_index+1] ... piv[end_index-1] }
*/
pvector *mk_pvector_segment(pvector *x,int start_index,int end_index)
{
  int seg_size = end_index - start_index;
  pvector *seg = mk_pvector(seg_size);
  int i;
  for ( i = 0 ; i < seg_size ; i++ )
    pvector_set(seg,i,pvector_ref(x,start_index+i));
  return seg;
}

/* As above except take elements from subset */
pvector *mk_pvector_subset(pvector *x, ivec *subset)
{
  int seg_size = ivec_size(subset);
  pvector *seg = mk_pvector(seg_size);
  int i;
  for ( i = 0 ; i < seg_size ; i++ )
    pvector_set(seg,i,pvector_ref(x,ivec_ref(subset,i)));
  return seg;
}

/* Returns a copy of x, except one in which the size is increased by n,
   and in which the final n elements have value 0 */
pvector *mk_add_elements_to_pvector(pvector *x,int n)
{
  int old_size = pvector_size(x);
  int new_size = old_size + n;
  int i;
  pvector *y = mk_pvector(new_size);
  for ( i = 0 ; i < old_size ; i++ )
    pvector_set(y,i,pvector_ref(x,i));
  for ( i = old_size ; i < new_size ; i++ )
    pvector_set(y,i,0.0);
  return y;
}

void pivec_increment_max_value(pivec *pi)
{
  int new_max_value = pivec_max_value(pi)+1;
  pivec_set_max_value(pi,new_max_value);
}

