//
// Programmer:    Craig Stuart Sapp <craig@ccrma.stanford.edu>
// Creation Date: Sun Jan 19 20:25:24 PST 1997
// Last Modified: Sun Apr 12 22:18:04 PDT 1998
// Last Modified: Mon Jan 25 22:55:11 PST 2010 (added alternate fp quant)
// Filename:      ...sig/maint/code/utilities/quantization.cpp
// Web Address:   http://sig.sapp.org/src/sig/quantization.cpp
// Syntax:        C++
//

#include "quantization.h"
#include <math.h>

#ifndef OLDCPP
   #include <iostream>
   using namespace std;
#else
   #include <iostream.h>
#endif



//////////////////////////////
//
// printCode -- prints a quantized number code in base-2 form.
//	The sign is the first digit.
//

void printCode(LinearCode x, int style) {

   if (style == 0) {
      if (x.sign) {
         cout << '1';
      } else {
         cout << '0';
      }
   
      int i;
      for (i = x.bits-2; i>=0; i--) {
         if (x.code & (1 << i)) {
            cout << '1';
         } else {
            cout << '0';
         }
      }
   } else {
      // print the code as an integer
      int value = 0;
      if (x.sign) {
         value = 1;
         value <<= x.bits-1;
      }
      value |= x.code;
      cout << value;
   }
       
}



//////////////////////////////
//
// printCodeQuartet -- prints a quantized number code in base-2 form.
//	The sign is the first digit.  Place a space after every 4 bits.
//

void printCodeQuartet(LinearCode x, int style) {
   if (style == 0) {
      if (x.sign) {
         cout << '1';
      } else {
         cout << '0';
      }
   
      int i;
      for (i = x.bits-2; i>=0; i--) {
         if ((i+1) % 4 == 0) {
            cout << '_';
         }
         if (x.code & (1 << i)) {
            cout << '1';
         } else {
            cout << '0';
         }
      }
   } else {
      // print the code as an integer
      int value = 0;
      if (x.sign) {
         value = 1;
         value <<= x.bits-1;
      }
      value |= x.code;
      cout << value;
   }
}



//////////////////////////////
//
// printFloatCode -- prints a quantized number code in base-2 form
//	with the scale shown at the beginning in parenthesis.
//	the sign bit is the first digit of the mantissa.
//

void printFloatCode(FloatCode x, int style) {
   if (style == 0) {
      int i;
      cout << '(';
      for (i = x.Rs - 1; i >=0; i--) {
         if (x.scale & (1 << i))
            cout << '1';
         else
            cout << '0';
      }
      cout << ')';
   
      if (x.sign) {
         cout << '1';
      } else {
         cout << '0';
      }
     
      for (i = x.Rm - 2; i>=0; i--) {
         if (x.mantissa & (1 << i)) { 
            cout << '1';
         } else { 
            cout << '0';
         }
      }
   } else {
      cout << "(" << x.scale << ")";
      cout << x.sign << " ";
      // print the code as an integer
      int value = 0;
      if (x.sign) {
         value = 1;
         value <<= x.Rm-1;
      }
      value |= x.mantissa;
      cout << value;
   }
}



//////////////////////////////
//
// printFloatCodeQuartet -- prints a quantized number code in base-2 form
//	with the scale shown at the beginning in parenthesis.
//	the sign bit is the first digit of the mantissa.
//

void printFloatCodeQuartet(FloatCode x, int style) {
   if (style == 0) {
      int i;
      cout << '(';
      for (i = x.Rs - 1; i >=0; i--) {
         if (x.scale & (1 << i))
            cout << '1';
         else
            cout << '0';
      }
      cout << ')';
   
      if (x.sign) {
         cout << '1';
      } else {
         cout << '0';
      }
     
      for (i = x.Rm - 2; i>=0; i--) {
         if ((i+1) % 4 == 0) {
            cout << '_';
         }
         if (x.mantissa & (1 << i)) {
            cout << '1';
         } else {
            cout << '0';
         }
      }
   } else {
      cout << "(" << x.scale << ")";
      // print the code as an integer
      int value = 0;
      if (x.sign) {
         value = 1;
         value <<= x.Rm-1;
      }
      value |= x.mantissa;
      cout << value;
   }
}



//////////////////////////////
//
// QuantizeMidtread
//

LinearCode QuantizeMidtread(int bits, double input) {
   double absoluteInput;
   LinearCode output;
   output.bits = bits;

   // set the sign bit:
   if (input < 0.0) {
      output.sign = 1;
      absoluteInput = -input; 
   } else {
      output.sign = 0;
      absoluteInput = input;
   }
   
   if (input >= 1.0 || input <= -1.0) {   // checking for overflow:
      output.code = (1 << (bits-1)) - 1;
   } else {
      output.code = (long)((((1 << bits) - 1)*absoluteInput + 1)/2.0);
   }

   return output;   
}



//////////////////////////////
//
// DequantizeMidtread
//

double DequantizeMidtread(LinearCode x) {
   double sign = (x.sign == 0) ? 1.0 : -1.0;
   return sign * 2.0 * x.code / ((1 << x.bits) - 1.0);
}



//////////////////////////////
//
// QuantizeMidrise
//

LinearCode QuantizeMidrise(int bits, double input) {
   double absoluteInput;
   LinearCode output;
   output.bits = bits;

   // set the sign bit:
   if (input < 0.0) {
      output.sign = 1;
      absoluteInput = -input;
   } else {
      output.sign = 0;
      absoluteInput = input;
   }

   if (input >= 1.0 || input <= -1.0) {   // checking for overflow:
      output.code = (1 << (bits-1)) - 1;
   } else {
      output.code = (long)((1 << (bits-1)) * absoluteInput);
  }

   return output;
}



//////////////////////////////
//
// DequantizeMidrise
//

double DequantizeMidrise(LinearCode x) {
   double sign = (x.sign == 0) ? 1.0 : -1.0;
   return sign * (x.code + 0.5) / (1 << (x.bits - 1));
}



//////////////////////////////
//
// FindLeadingZeros -- counts the number of most significant
//	zeros in a code.
//

int FindLeadingZeros(LinearCode x) {
   int digit = x.bits - 1;
   int counter = 0;

   while (!(x.code & (1 << (digit-1))) && digit > 0) {
      counter++;
      digit--;
   }

   return counter;
}



//////////////////////////////
//
// QuantizeFloatMidtread -- (bug placed in the code, so don't use...)
//

FloatCode QuantizeFloatMidtread(int Rs, int Rm, double input) {
   return QuantizeFloatMidtread2(Rs, Rm, input);
   // Rs = number of scale (exponent) bits
   // Rm = number of mantissa bits

   FloatCode output;
   output.Rs = Rs;
   output.Rm = Rm;
 
// Step I: Quantize the input as an R bit code where R = 2**Rs-1+Rm.

   LinearCode temp;
   temp = QuantizeMidtread((1 << Rs) - 1 + Rm, input);
   output.sign = temp.sign;

// Step II: Count the number of leading zero in |code|.  If the number
//      of leading zeros is less than 2**Rs-1 then set the scale equal
//      to 2**Rs-1 minus the number of leading zeros; otherwise set
//      the scale equal to zero.

   int leadingZeros = FindLeadingZeros(temp);

   if (leadingZeros < (1 << Rs) - 1) {
      output.scale = (1 << Rs) - 1 - leadingZeros;
   } else {
      output.scale = 0;
   }

// Step III: If scale equals zero then set the first mantissa bit
//      equal to sign and set the remaining Rm-1 bits equal to the bits
//      following the 2**Rs-1 leading zeros in |code|; otherwise set the first
//      mantissa bit equal to s and set the remaining Rm-1 bits equal to the
//      bits following the leading zeros OMITTING THE LEADING ONE.

   if (output.scale == 0) {
      output.mantissa = temp.code;
   } else {
      output.mantissa = temp.code >> (temp.bits - 1 - leadingZeros - Rm);
      // remove leading 1 (note sign is stored elsewhere):
      output.mantissa &= !(1 << (Rm-1))-1;
      // output.mantissa ^= (1 << (Rm-1));
   }

   return output;
}



//////////////////////////////
//
// QuantizeFloatMidtread2 -- Alternate method of floating point quantization.
//

FloatCode QuantizeFloatMidtread2(int Rs, int Rm, double input) {
   // Rs = number of scale (exponent) bits
   // Rm = number of mantissa bits

   int twoRsminus1 = (1 << Rs) - 1;
   FloatCode output;
   output.Rs = Rs;
   output.Rm = Rm;
 
// Step I: Quantize the input as an R bit code where R = 2**Rs-1+Rm.

   LinearCode temp;
   temp = QuantizeMidtread(twoRsminus1 + Rm, input);
   output.sign = temp.sign;

// Step II: Count the number of leading zero in |code|.  If the number of
//      leading zeros is less than 2**Rs-1 then set the scale equal to the
//      number of leading zeros; otherwise, set the scale equal to
//      2**Rs-1.

   int leadingZeros = FindLeadingZeros(temp);

   if (leadingZeros < twoRsminus1) {
      output.scale = leadingZeros;
   } else {
      output.scale = twoRsminus1;
   }

// Step III: If scale equals 2**Rs-1, then set the first mantissa bit equal
//      to the sign bit and set the remaining Rm-1 bits equal to the bits
//      following the 2**Rs-1 leading zeros in |code|; otherwise, set the 
//      first mantissa bit equal to the sign bit and set the remaining Rm-1
//      bits equal to the bits following the leading zeros, omitting the 
//      leading one.

   if (output.scale == twoRsminus1) {
      output.mantissa = temp.code;
   } else {

      // cout << "[before:";
      // printCodeQuartet(temp);
      // cout << "]";

      // remove the leading 1 from the temp.code (destructive of temp):
      temp.code ^= (1 << (temp.bits - 2 - leadingZeros));

      // cout << "[after:";
      // printCodeQuartet(temp);
      // cout << "]";
      
      // store rest of temp.code after leading 1 bit in mantissa:
      output.mantissa = temp.code >> (temp.bits - 1 - leadingZeros - Rm);
   }

   // remove any possible junk above the storage for the mantissa
   // (sign stored elsewhere).
   output.mantissa &= (1 << (output.Rm-1)) - 1;

   return output;
}



//////////////////////////////
//
// QuantizeBlockFloatMidtread2 -- Block floating point quantization, with
//    a local scale (this function is not really supposed to be used, just
//    for testing purposes).
//

FloatCode QuantizeBlockFloatMidtread2(int Rs, int Rm, double input) {
   // Rs = number of scale (exponent) bits
   // Rm = number of mantissa bits

   int twoRsminus1 = (1 << Rs) - 1;
   FloatCode output;
   output.Rs = Rs;
   output.Rm = Rm;
 
// Step I: Quantize the input as an R bit code where R = 2**Rs-1+Rm.

   LinearCode temp;
   temp = QuantizeMidtread(twoRsminus1 + Rm, input);
   output.sign = temp.sign;

// Step II: Count the number of leading zero in |code|.  If the number of
//      leading zeros is less than 2**Rs-1 then set the scale equal to the
//      number of leading zeros; otherwise, set the scale equal to
//      2**Rs-1.

   int leadingZeros = FindLeadingZeros(temp);

   if (leadingZeros < twoRsminus1) {
      output.scale = leadingZeros;
   } else {
      output.scale = twoRsminus1;
   }

// Step III: If scale equals 2**Rs-1, then set the first mantissa bit equal
//      to the sign bit and set the remaining Rm-1 bits equal to the bits
//      following the 2**Rs-1 leading zeros in |code|; otherwise, set the 
//      first mantissa bit equal to the sign bit and set the remaining Rm-1
//      bits equal to the bits following the leading zeros, omitting the 
//      leading one.

   if (output.scale == twoRsminus1) {
      output.mantissa = temp.code;
   } else {

      // don't remove the leading 1 from temp.code
      // store rest of temp.code including leading 1 bit in mantissa:
      output.mantissa = temp.code >> (temp.bits - 0 - leadingZeros - Rm);
   }

   // remove any possible junk above the storage for the mantissa
   // (sign stored elsewhere).
   output.mantissa &= (1 << (output.Rm-1)) - 1;

   return output;
}


// no so efficient at the moment: calculate scale of maxx outside of function.

FloatCode QuantizeBlockFloatMidtread2Max(int Rs, int Rm, double input, 
      double maxx) {
   // Rs = number of scale (exponent) bits
   // Rm = number of mantissa bits
   
   maxx = fabs(maxx);
   FloatCode maxcode = QuantizeBlockFloatMidtread2(Rs, Rm, maxx);
   int maxxscale = maxcode.scale;

   int twoRsminus1 = (1 << Rs) - 1;
   FloatCode output;
   output.Rs = Rs;
   output.Rm = Rm;
 
// Step I: Quantize the input as an R bit code where R = 2**Rs-1+Rm.

   LinearCode temp;
   temp = QuantizeMidtread(twoRsminus1 + Rm, input);
   output.sign = temp.sign;

// Step II: Count the number of leading zero in |code|.  If the number of
//      leading zeros is less than 2**Rs-1 then set the scale equal to the
//      number of leading zeros; otherwise, set the scale equal to
//      2**Rs-1.

   // int leadingZeros = FindLeadingZeros(temp);

   // if (leadingZeros < twoRsminus1) {
   //    output.scale = leadingZeros;
   // } else {
   //    output.scale = twoRsminus1;
   // }
   output.scale = maxxscale;

// Step III: If scale equals 2**Rs-1, then set the first mantissa bit equal
//      to the sign bit and set the remaining Rm-1 bits equal to the bits
//      following the 2**Rs-1 leading zeros in |code|; otherwise, set the 
//      first mantissa bit equal to the sign bit and set the remaining Rm-1
//      bits equal to the bits following the leading zeros, omitting the 
//      leading one.

   if (output.scale == twoRsminus1) {
      output.mantissa = temp.code;
   } else {

      // don't remove the leading 1 from temp.code
      // store rest of temp.code including leading 1 bit in mantissa:
      output.mantissa = temp.code >> (temp.bits - 0 - maxxscale - Rm);
   }

   // remove any possible junk above the storage for the mantissa
   // (sign stored elsewhere).
   output.mantissa &= (1 << (output.Rm-1)) - 1;

   return output;
}



//////////////////////////////
//
// DequantizeFloatMidtread -- used to dequantize values quantized with
//    QuantizeFloatMidtread.  Bug placed in code, so don't use...
//

double DequantizeFloatMidtread(FloatCode x) {
   return DequantizeFloatMidtread2(x);

// Step I: Create an R bit code where R = 2**Rs-1+Rm from the mantissa
//      and scale factor where s is the first mantissa bit and |code|...
   int twoRsminus1 = (1 << x.Rs) - 1;
   LinearCode temp;
   temp.bits = twoRsminus1 + x.Rm;
   temp.sign = x.sign;

// A: has 2**Rs-1-scale leading zeros
// B: followed by the remaining Rm-1 mantissa bits if scale is zero, 
//     otherwise followed by a one and then the remaining mantissa bits.
// C: followed by a one and as many trailing zeros as will fit if scale is
//     greater than one.

   int leadingZeros = twoRsminus1 - x.scale;

   if (x.scale == 0) {
      temp.code = x.mantissa;
   } else {
      // first add the leading and trailing 1's:
      temp.code = (1 << (temp.bits - 2 - leadingZeros)) +
                  (1 << (temp.bits - 2 - leadingZeros - x.Rm));
      // now fill in the blank between the two 1's:
      temp.code |= x.mantissa << (temp.bits - 1 - leadingZeros - x.Rm);
   }

// Step II: Dequantize the R bit code into the number.

   return DequantizeMidtread(temp);   
}



//////////////////////////////
//
// DequantizeFloatMidtread2 -- used to dequantize values quantized with
//     QuantizeFloatMidtread2.
//

double DequantizeFloatMidtread2(FloatCode x, int debug) {

// Step I: Create an R bit code where R = 2**Rs-1+Rm from the mantissa
//      and scale factor where s (sign) is the first mantissa bit and |code|...
   LinearCode temp;
   int twoRsminus1 = (1 << x.Rs) - 1;
   temp.bits = twoRsminus1 + x.Rm;
   temp.sign = x.sign;
   temp.code = 0;
   int mshift = 0;

// A: has scale leading zeros.
   int leadingZeros = x.scale;

// B: followed by the remaining Rm-1 mantissa bits if scale is 2**Rs-1, 
//     otherwise followed by a one and then the remaining mantissa bits.
   if (x.scale == twoRsminus1) {
      temp.code = x.mantissa;
   } else {
      // insert the mantissa 
      mshift = temp.bits - 1 - leadingZeros - x.Rm;
      temp.code |= x.mantissa << (temp.bits - 1 - leadingZeros - x.Rm);
      // add the leading one (which was originally dropped)
      temp.code |= (1 << (temp.bits - 2 - leadingZeros));
   }
	    
// C: followed by a one and as many trailing zeros as will fit if scale is
//     is less than 2**Rs-1.

   if (x.scale < twoRsminus1) {
      if (mshift >= 0) {
         temp.code |= (1 << (mshift - 1));
      }
   }

// Step II: Dequantize the R bit code into the number.
//

   // clear out any clutter in the higher bits of the code:
   // sign bit is stored elsewhere
   temp.code &= (1 << (temp.bits - 1)) - 1;

   if (debug) {
      cout << " u:";
      printCodeQuartet(temp);
      cout << " ";
   }

   return DequantizeMidtread(temp);   
}



//////////////////////////////
//
// DequantizeBlockFloatMidtread2 -- used to dequantize values quantized with
//     QuantizeBlockFloatMidtread2.
//

double DequantizeBlockFloatMidtread2(FloatCode x, int debug) {

// Step I: Create an R bit code where R = 2**Rs-1+Rm from the mantissa
//      and scale factor where s (sign) is the first mantissa bit and |code|...
   LinearCode temp;
   int twoRsminus1 = (1 << x.Rs) - 1;
   temp.bits = twoRsminus1 + x.Rm;
   temp.sign = x.sign;
   temp.code = 0;
   int mshift = 0;

// A: has scale leading zeros.
   int leadingZeros = x.scale;

// B: followed by the remaining Rm-1 mantissa bits.
    // insert the mantissa 
    mshift = temp.bits - 1 - leadingZeros - x.Rm + 1;
    temp.code |= x.mantissa << mshift;
    // don't add a leading one (which was not originally dropped)

// C: followed by a one and as many trailing zeros as will fit if scale is
//     is less than 2**Rs-1 unless the code contains all zeros.

   if ((x.mantissa != 0) && (x.scale < twoRsminus1)) {
      if (mshift >= 0) {
         temp.code |= (1 << (mshift - 1));
      }
   }

// Step II: Dequantize the R bit code into the number.
//

   // clear out any clutter in the higher bits of the code:
   // sign bit is stored elsewhere
   temp.code &= (1 << (temp.bits - 1)) - 1;

   if (debug) {
      cout << " u:";
      printCodeQuartet(temp);
      cout << " ";
   }

   return DequantizeMidtread(temp);   
}



// md5sum: 10577f8ae983b96c74e704f30c8fea77 quantization.cpp [20050403]