/*****************************************************************************
 *                                                                          
 * This software module was developed by 
 *
 *  Anthony Vetro  (MITSUBISHI ELECTRIC ITA)
 *  Huifang Sun    (MITSUBISHI ELECTRIC ITA)
 *  Hung-Ju Lee    (Sarnoff Corporation)
 *  Tihao Chiang   (Sarnoff Corporation)
 *  Sung gul Ryoo  (SAMSUNG A.I.T)                                         
 *                                                      
 *
 * in the course of development of the MPEG-4 Video (ISO/IEC 14496-2) standard.
 * This software module is an implementation of a part of one or more MPEG-4 
 * Video (ISO/IEC 14496-2) tools as specified by the MPEG-4 Video (ISO/IEC 
 * 14496-2) standard. 
 *
 * ISO/IEC gives users of the MPEG-4 Video (ISO/IEC 14496-2) standard free 
 * license to this software module or modifications thereof for use in hardware
 * or software products claiming conformance to the MPEG-4 Video (ISO/IEC 
 * 14496-2) standard. 
 *
 * Those intending to use this software module in hardware or software products
 * are advised that its use may infringe existing patents. The original 
 * developer of this software module and his/her company, the subsequent 
 * editors and their companies, and ISO/IEC have no liability for use of this 
 * software module or modifications thereof in an implementation. Copyright is 
 * not released for non MPEG-4 Video (ISO/IEC 14496-2) Standard conforming 
 * products. 
 *
 * ACTS-MoMuSys partners retain full right to use the code for his/her own 
 * purpose, assign or donate the code to a third party and to inhibit third 
 * parties from using the code for non MPEG-4 Video (ISO/IEC 14496-2) Standard
 * conforming products. This copyright notice must be included in all copies or
 * derivative works. 
 *
 * Copyright (c) 1997
 *
 *****************************************************************************/

/****************************************************************
  COPYRIGHT (1997)  MITSUBISHI ELECTRIC ITA
  ALL RIGHTS RESERVED
  MITSUBISHI PROPRIETARY INFORMATION
  PERMISSION IS GRANTED FOR USE WITHIN MPEG4 STANDARDIZATION PROCESS 
****************************************************************/

/**
 **  rc2.c:	 	rate control module
 **  Author: 		A. Vetro
 **
 **/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <unistd.h>
#include <ctype.h>
#include "vm_config.h"
#include "sim.h"
#include "matrix.h"
#include "vector.h"
#include "rc_mvo.h"
#include "rc.h"
#include "rc_matrix.h"
#include "rc2.h"

extern Int   START_RATE_CONTROL_MVO;          
extern Int   MAX_SLIDING_WINDOW_MVO;          
extern Float PAST_PERCENT_MVO;                
extern Float SAFETY_MARGIN_MVO;                
extern Int   SKIP_MARGIN_MVO;                 
extern Int   MAX_QUANT_MVO;                   
extern Int   SKIP_TH;                         
extern Int   LB_QUANT;

extern Double rc_MMV[20];
extern Double rc_MMV_next[20];
extern Double rc_MAD[20];
extern Double rc_MAD_next[20];
extern Int rc_SIZE[20];
extern Int rc_SIZE_next[20];
extern Int PreviousBits[MAX_NUM_VOS][MAX_NUM_VOLS];


RC_MVO rc_mvo;
Int *first_time;
Int *first_time_mv;
Double *Qlast;

/* local variables */
static Float **Qptr, **Rptr, **Tptr;
static Float **tmp_Qptr, **tmp_Rptr, **tmp_Tptr;
static Short **mark;
static Float *lastmad;
static Int *quant_sum;
static Int *quant_count;


/***************************************************************************
  rc_init_global2()
  ***************************************************************************/
Void rc_init_global2(Int start, Int end, Int frame_inc, Int rate, Int numVOs)
{
  Int i;

  Qptr = fmatrix(0,numVOs-1,0,((end-start+1)/frame_inc+1));
  Rptr = fmatrix(0,numVOs-1,0,((end-start+1)/frame_inc+1));
  Tptr = fmatrix(0,numVOs-1,0,((end-start+1)/frame_inc+1));
  tmp_Qptr = fmatrix(0,numVOs-1,0,((end-start+1)/frame_inc+1));
  tmp_Rptr = fmatrix(0,numVOs-1,0,((end-start+1)/frame_inc+1));
  tmp_Tptr = fmatrix(0,numVOs-1,0,((end-start+1)/frame_inc+1));
  mark = (Short **)fmatrix(0,numVOs-1,0,((end-start+1)/frame_inc+1));

  /* Initial frame variables */
  rc_mvo.bit_rate = rate;
  rc_mvo.R = rate*(end-start+1)/30;
  rc_mvo.numFrame = (end-start+1)/frame_inc-1;
  rc_mvo.VBV_size = 0.5 * rate;  
  rc_mvo.VBV_fullness = 0.5 * rc_mvo.VBV_size;
  rc_mvo.Bpp = rc_mvo.R / rc_mvo.numFrame;

  /* Initial VO variables */
  rc_mvo.skip = (Int *)ivector2(0, numVOs - 1); 
  rc_mvo.preskip = (Int *)ivector2(0, numVOs - 1);
  rc_mvo.totalskipped = (Int *)ivector2(0, numVOs - 1);
  rc_mvo.codedVO = (Int *)ivector2(0, numVOs - 1);
  rc_mvo.numVOleft = (Int *)ivector2(0, numVOs - 1);
  rc_mvo.X1 = (Double *)dvector2(0, numVOs - 1);
  rc_mvo.X2 = (Double *)dvector2(0, numVOs - 1);
  rc_mvo.X = (Double *)dvector2(0, numVOs - 1);
  rc_mvo.Q = (Double *)dvector2(0, numVOs - 1);
  rc_mvo.S = (LInt *)ivector2(0, numVOs - 1);
  rc_mvo.T = (LInt *)ivector2(0, numVOs - 1);
  rc_mvo.dataNumber = (Int *)ivector2(0, numVOs - 1);
  rc_mvo.headerBitCount = (Int *)ivector2(0, numVOs - 1);
  quant_count = (Int *)ivector2(0, numVOs - 1);
  quant_sum = (Int *)ivector2(0, numVOs - 1);

  lastmad = (Float *)fvector2(0, numVOs - 1);
  Qlast = (Double *)dvector2(0, numVOs - 1);
  first_time = (Int *)ivector2(0, numVOs - 1);
  first_time_mv = (Int *)ivector2(0, numVOs - 1);
  SIZE = (Double *)dvector2(0, numVOs - 1);
  VARIANCE = (Double *)dvector2(0, numVOs - 1);
  MOTION = (Double *)dvector2(0, numVOs - 1);
  LOW_MODE = 0;

  for(i=0; i<numVOs; i++) {
	rc_mvo.X1[i] = rate * frame_inc / 2;
	rc_mvo.numVOleft[i] = (end-start+1)/frame_inc-1;
	rc_mvo.headerBitCount[i] = 1000;
	rc_mvo.dataNumber[i] = 0;
	first_time[i] = 1;
	first_time_mv[i] = 1;
	quant[i] = 15;
  }
}

/***************************************************************************
  rc_default_parameters
  ***************************************************************************/
Void rc_default_parameters(Int numVOs)
{
  if (numVOs == 1) { /* SVO */
    START_RATE_CONTROL_MVO     =     1;
    MAX_SLIDING_WINDOW_MVO     =      20;
    PAST_PERCENT_MVO           =      0.05;
    SAFETY_MARGIN_MVO          =      0.1;
    SKIP_MARGIN_MVO            =      80;
    MAX_QUANT_MVO              =      31;
    SKIP_TH                    =      2;
    LB_QUANT                   =      30;
  }
  else if (numVOs > 1) { /* MVO */
    START_RATE_CONTROL_MVO     =      5;
    MAX_SLIDING_WINDOW_MVO     =      20;
    PAST_PERCENT_MVO           =      0.20;
    SAFETY_MARGIN_MVO          =      0.25; 
    SKIP_MARGIN_MVO            =      80;
    MAX_QUANT_MVO              =      31;
    SKIP_TH                    =      2;
    LB_QUANT                   =      30;
  }
}

/***************************************************************************
  InitialTargetEstimate()
  ***************************************************************************/
Void InitialTargetEstimate(Float mad, Int vo_id, Int numVOs)
{
  Int a, b;

  a = rc_mvo.VBV_fullness;
  b = rc_mvo.VBV_size-rc_mvo.VBV_fullness;

  rc_mvo.T[vo_id] = rc_mvo.R / (rc_mvo.numVOleft[vo_id]*numVOs);
  if ( rc_mvo.S[vo_id] != 0)
       rc_mvo.T[vo_id] = rc_mvo.T[vo_id] * (1-PAST_PERCENT_MVO) 
			  + rc_mvo.S[vo_id] * PAST_PERCENT_MVO;

  rc_mvo.T[vo_id] = Max(rc_mvo.bit_rate/(30*numVOs),
			   rc_mvo.T[vo_id]);
#ifdef _RC_DEBUG_
  printf("  --> initial_T(%d) : %d\n", vo_id, rc_mvo.T[vo_id]);
#endif /*_RC_DEBUG_*/

}

/***************************************************************************
  JointBuffer()
  ***************************************************************************/
Int JointBuffer(Int numVOs)
{
  Int T=0;
  Int a, b;
  Int top_margin, bot_margin;
  Int top_level, bot_level;
  Int i;

  /* sum individual targets */
  for(i=0; i<numVOs; i++)
	T += rc_mvo.T[i];

#ifdef _RC_DEBUG_
  printf("\n  Target before buffer control : %d\n", T);
#endif /*_RC_DEBUG_*/

  a = rc_mvo.VBV_fullness;
  b = rc_mvo.VBV_size-rc_mvo.VBV_fullness;

  /* adjust jointly according to buffer fullness */
  T = (a+2.0*b)*T/(2.0*a+b);
 
  /* additional buffer control */
  top_margin = ceil ((1.0 - SAFETY_MARGIN_MVO) * rc_mvo.VBV_size);
  bot_margin = SAFETY_MARGIN_MVO * rc_mvo.VBV_size;
  top_level  = rc_mvo.VBV_fullness + T;
  bot_level  = top_level - rc_mvo.Bpp;

  if ( top_level > top_margin) {
     Int decrease = (Int) ceil (top_level - top_margin);
     T -= decrease;
  } 
  else if ( bot_level < bot_margin) {
     Int increase = (Int) (bot_margin - bot_level);
     T += increase;
  }
#ifdef _RC_DEBUG_
  printf("  Target after buffer control  : %d\n\n", T);
#endif /*_RC_DEBUG_*/

  return(T);

}

/***************************************************************************
  TargetDistribution()
  ***************************************************************************/
Void TargetDistribution(Int T, Int numVOs)
{
  Int s,i;
  Int   Total;
  Float PREV[20];
  
  s = T;
  for(i=0; i<numVOs; i++)
	{
		s -= rc_mvo.headerBitCount[i];
#ifdef _RC_DEBUG_
		printf("s = %d, headerBitCount[i] = %d\n", s, rc_mvo.headerBitCount[i]);
#endif /*_RC_DEBUG_*/
	}

  if(s<0) {

    /* 	force negative values on all targets &	*/
    /* 	skip frames for next coding instant 	*/

    while(s<0) {
#ifdef _RC_DEBUG_
	printf("Target made negative - not enough bits for header!\n");
#endif /*_RC_DEBUG_*/
    	for(i=0; i<numVOs; i++) {
	   rc_mvo.T[i] = -10; 
	   rc_mvo.preskip[i]++;
	}
	s += rc_mvo.Bpp;
    }
  }
  else { 

    /* distribute proportional to */ 
    /* motion, size, and variance */
		Total = 0;
		for(i=0; i<numVOs; i++) 
		{
			Total += PreviousBits[i][0];
		}
		for(i=0; i<numVOs; i++) 
		{
			PREV[i] = (Float)PreviousBits[i][0]/(Float)Total;
		}

    for(i=0; i<numVOs; i++) {
	if(LOW_MODE)
	{
		rc_mvo.T[i] = (Int)((BETA_V_LOW*VARIANCE[i]+
				       BETA_M_LOW*MOTION[i]+
				       BETA_S_LOW*SIZE[i])*T);
#ifdef _RC_DEBUG_
		printf("BETA_V_LOW = %f, BETA_M_LOW = %f, BETA_S_LOW = %f\n", 
		       BETA_V_LOW, BETA_M_LOW, BETA_S_LOW);
#endif /*_RC_DEBUG_*/
	}
	else 
	{
		rc_mvo.T[i] = (Int)((BETA_V_HIGH*VARIANCE[i]+
				       BETA_M_HIGH*MOTION[i]+
				       BETA_S_HIGH*SIZE[i])*T);
	}

#ifdef _RC_DEBUG_
	printf("  --> T(%d) = %d, T = %d\n",i,rc_mvo.T[i], T);
	printf(" MOTION = %5.2f, VAR = %5.2f, SIZE = %5.2f\n", 
	       MOTION[i], VARIANCE[i], SIZE[i]);
#endif /*_RC_DEBUG_*/
    }
  }
#ifdef _RC_DEBUG_
  printf("\n");
#endif /*_RC_DEBUG_*/
}

/***************************************************************************
  AdjustDistribParams()
  ***************************************************************************/
Void AdjustDistribParams(Int numVOs)
{
 Int i;
 Int size_tot=0; 
 Double mot_tot=0.0; 
 Double mad_tot=0.0;

 /* obtain totals */
 for(i=0; i<numVOs; i++) {
	mot_tot += rc_MMV[i];
	size_tot += rc_SIZE[i];
	mad_tot += rc_MAD[i]*rc_MAD[i];
 }

 /* distribute accordingly */
 if(mot_tot == 0) {
   for(i=0; i<numVOs; i++)
	MOTION[i] = 1/numVOs;
 }
 else {
   for(i=0; i<numVOs; i++)
	MOTION[i] = rc_MMV[i]/mot_tot;
 }

 if(size_tot == 0) {
   SIZE[0] = 1;
 }
 else {
    for(i=0; i<numVOs; i++)
	SIZE[i] = (Float)rc_SIZE[i]/(Float)size_tot;
 }

 if(mad_tot == 0) {
   /* very unlikely to occur */
   printf("unexpected mad_tot = 0, exiting program.\n");
   exit(3);
 }
 else {
    for(i=0; i<numVOs; i++)
	VARIANCE[i] = (rc_MAD[i]*rc_MAD[i])/mad_tot;
  }

 /* update zero for next frame */
 rc_MMV[0] = rc_MMV_next[0];
 rc_SIZE[0] = rc_SIZE_next[0];

}

/***************************************************************************
  AdjustRateParams()
  ***************************************************************************/
Void AdjustRateParams(Int bitCount, Int header, Int vo_id)
{
 
  rc_mvo.VBV_fullness += bitCount;

  rc_mvo.R -= bitCount;
  rc_mvo.S[vo_id] = bitCount;
  rc_mvo.headerBitCount[vo_id] = header;
  rc_mvo.numVOleft[vo_id] --;
  if(vo_id==0) /* assumes each VO coded at same rate */ 
      rc_mvo.numFrame--;

#ifdef _RC_DEBUG_
  printf("  Num bits left for coding : %d\n",rc_mvo.R);
  printf("  Num bits used coding VO%d : %d\n",vo_id,rc_mvo.S[vo_id]);
#endif /*_RC_DEBUG_*/
}

/***************************************************************************
  setTargetModelVO()
  ***************************************************************************/
Void setTargetModelVO(Float q, Float r, Float h, Float mad, Int vo_id)
{
  Int    i, n;
  Int    tmp_n;

  rc_mvo.dataNumber[vo_id]++;
  Qptr[vo_id][rc_mvo.dataNumber[vo_id]-1] = (Float) q;
  Rptr[vo_id][rc_mvo.dataNumber[vo_id]-1] = (Float) (r-h) / mad;

  n = Min(rc_mvo.dataNumber[vo_id], MAX_SLIDING_WINDOW_MVO);
  n = (lastmad[vo_id]/mad > 1.0) ? 
	(Int)((mad/lastmad[vo_id])*n+1) : 
	(Int)((lastmad[vo_id] / mad) * n + 1);
  EstimatorVO(Qptr, Rptr, n, vo_id);
  for (i=rc_mvo.dataNumber[vo_id]; i>rc_mvo.dataNumber[vo_id]-n; i--) 
      Tptr[vo_id][i-1] = calModelBitrate(rc_mvo.X1[vo_id], 
					  rc_mvo.X2[vo_id], 
					  Qptr[vo_id][i-1], 
					  mad);
  (Void)removeOutlierVO(Tptr, Qptr, Rptr, n, mad,
                        tmp_Tptr, tmp_Qptr, tmp_Rptr, 
			&tmp_n, vo_id);
  EstimatorVO(tmp_Qptr, tmp_Rptr, tmp_n, vo_id);
  lastmad[vo_id] = mad;

}

/***************************************************************************
  EstimatorVO()
  ***************************************************************************/
Void EstimatorVO(Float **Qptr, Float **Rptr, Int n, Int vo_id)
{
  Float **X, **y;
  Float **b = NULL;
  Int reduce;
  Int i;

  /* generate the X and Y matrices */
  X = (Float **)matrix(1, n, 1, 2);
  y = (Float **)matrix(1, n, 1, 1);
  b = (Float **)matrix(1, 2, 1, 1);
  genXYmatrixVO(X, y, n, Qptr, Rptr, vo_id);

  /* Default Model Parameters */
  rc_mvo.X1[vo_id] = 0;
  rc_mvo.X2[vo_id] = 0;
  for (i = 1; i <= n; i++)
      rc_mvo.X1[vo_id] += y[i][1] / n;

  rc_mvo.X[vo_id] = rc_mvo.X1[vo_id];
  /* Check if the rank is sufficient */
  reduce = checkXAngleVO(Qptr, n, vo_id);

  /* Start model estimation when data points are more than START_RATE_CONTROL */
  if (n >= START_RATE_CONTROL_MVO && reduce == 0) {
     buildLRModel(X, y, &b, n, 2);
     if ( b [1][1] != 0) {
        rc_mvo.X1[vo_id] = b [1][1];
        rc_mvo.X2[vo_id] = b [2][1];
     }
     free_matrix(b, 1, 2, 1, 1);
  }
  free_matrix(X, 1, n, 1, 2);  
  free_matrix(y, 1, n, 1, 1);  
}

/***************************************************************************
  calModelBitrate()
  ***************************************************************************/
Float calModelBitrate(Float X1, Float X2, Float Q, Float mad)
{
   Float T;

   T = X1 / Q +  X2 / (Q * Q);
   T *= mad;
   return T;
}

/***************************************************************************
  genXYmatrixVO()
  **************************************************************************/
Void genXYmatrixVO(Float **X, Float **y, Int n, Float **Qptr, Float **Rptr, Int vo_id)
{
   Int i,k;

    for (k = 1, i = rc_mvo.dataNumber[vo_id]; i > rc_mvo.dataNumber[vo_id]-n; i--, k++) {
        X[k][1] = 1;
        X[k][2] = 1 / Qptr[vo_id][i-1];
    }

    for (k = 1, i = rc_mvo.dataNumber[vo_id]; i > rc_mvo.dataNumber[vo_id]-n; i--, k++) {
        y[k][1] = Rptr[vo_id][i-1] * Qptr[vo_id][i-1];
    }

}

/***************************************************************************
  checkXAngleVO()
  ***************************************************************************/
Int checkXAngleVO(Float **Qptr, Int n, Int vo_id)
{
  Int    i,
         reduce;
  Float  val;

  val = Qptr[vo_id][rc_mvo.dataNumber[vo_id]-1];
  reduce = 1;

  for (i = rc_mvo.dataNumber[vo_id]; i > rc_mvo.dataNumber[vo_id]-n; i--) {
      if (Qptr[vo_id][i-1] != val) {
         reduce = 0;
         break;
      }
  }
  return (reduce);
}

/***************************************************************************
    removeOutlierVO()
  ***************************************************************************/
Double removeOutlierVO(Float **old_T, Float **old_Q, Float **old_R, Int old_n, 
                Float mad, Float **new_T, Float **new_Q, Float **new_R, 
                Int *new_n, Int vo_id)
{ 
   Int    i,j,
          tmp_n;
   Double tmp,
          sqr_sum_R,
          std_R,
          mean_R;
   Int    dnum; 

   dnum = rc_mvo.dataNumber[vo_id];
   sqr_sum_R = 0.0;
   mean_R = 0.0;

   for (i=dnum; i>dnum-old_n; i--) {
       tmp = fabs(old_T[vo_id][i-1] - old_R[vo_id][i-1] * mad);
       sqr_sum_R += tmp*tmp;
       if (tmp != 0) 
          mean_R += old_T[vo_id][i-1]/tmp;
   }
   std_R = sqrt(sqr_sum_R/old_n);
   mean_R = mean_R/old_n;

   for (i=dnum; i>dnum-old_n; i--) {
       mark[vo_id][i-1] = 1;
       if (fabs(old_T[vo_id][i-1]-mad*old_R[vo_id][i-1]) > std_R) 
          mark[vo_id][i-1] = 0;
   }
 
   /* set current data point to be considered */
   mark[vo_id][dnum-1] = 1;    
   tmp_n = 0; 

   for (j=dnum, i=dnum; i>dnum-old_n; i--)  {
       if (mark[vo_id][i-1] == 1) {
          new_R[vo_id][j-1] = old_R[vo_id][i-1];
          new_Q[vo_id][j-1] = old_Q[vo_id][i-1];
          new_T[vo_id][j-1] = old_T[vo_id][i-1];
          j--; tmp_n++;
       }
   }

   sqr_sum_R = 0;
   mean_R =0;
   for (i=dnum; i>dnum-tmp_n; i--) {
       tmp = fabs(new_T[vo_id][i-1] - new_R[vo_id][i-1]*mad);
       sqr_sum_R += tmp*tmp;
       if (tmp !=0) mean_R += new_T[vo_id][i-1]/tmp;
   }
   std_R = sqrt(sqr_sum_R/tmp_n);
   mean_R = mean_R/tmp_n;

   *new_n = tmp_n;
   return mean_R;

}

/***************************************************************************
  QuantCalculate();
  ***************************************************************************/
Void QuantCalculate(Double * Q, LInt *T, Int headerBitCount,
			Double mad, Int vo_id, Int numVOs)
{

    Double a, b, c, d, x, q, t;
    Int lb_flag=0;

    t = (Double)*T - rc_mvo.headerBitCount[vo_id];

    t /= mad;

    if(t<0)
	lb_flag = 1;

    a = rc_mvo.X2[vo_id];
    b = rc_mvo.X1[vo_id];
    c = Max (rc_mvo.Bpp/(3*mad*numVOs), t);

    c = -c;
    d = b*b-4*a*c;
    if (d < 0) {
       a = 0;
       b = rc_mvo.X[vo_id];
    }

    if (a == 0 || d < 0) {
      x = - c / b;
      q = 1 / x;
      t = rc_mvo.X[vo_id] * x ;
    } 
    else {
      x = (sqrt(d)-b)/(2*a);
      q = 1 / x;
      t = b * x + a * x * x;
    }

    *Q = (Int) floor(q+0.5);
    if(lb_flag || LOW_MODE)
	*Q =  Max(LB_QUANT,*Q);
    else
	*Q =  Max(1,*Q);
    *Q =  Min(MAX_QUANT_MVO,*Q); 

	if (Qlast[vo_id] != 0.0) {
    	  if (*Q > Qlast[vo_id]) 
       		*Q = Min(Qlast[vo_id]+ceil(Qlast[vo_id]/4.0),*Q);
    	  else 
       		*Q = Max(Qlast[vo_id]-ceil(Qlast[vo_id]/4.0),*Q);
  	}

    t = b / (*Q) + a / (*Q * *Q);
    *T = (Int) floor(t*mad+0.5) + rc_mvo.headerBitCount[vo_id];

}

/***************************************************************************
  PutVopQuantizerVO();
  ***************************************************************************/
Void PutVopQuantizerVO(Int Q, Int vo_id)
{
  quant[vo_id] = Q;
}

/***************************************************************************
  GetVopQuantizerVO();
  ***************************************************************************/
Int GetVopQuantizerVO(Int vo_id)
{
  return(quant[vo_id]);
}

/***************************************************************************
  QuantAccumulate();
  ***************************************************************************/
Void QuantAccumulate(Int Q, Int vo_id)
{

 quant_sum[vo_id] += Q;
 quant_count[vo_id]++; 
#ifdef _RC_DEBUG_
  printf("avg_Q = %5.2f\n", quant_sum[vo_id]/(Float)quant_count[vo_id]);
#endif /*_RC_DEBUG_*/
}
/***************************************************************************
  PrintAverageQuant();
  ***************************************************************************/
Void PrintAverageQuant(Int vo_id, FILE *fp)
{

#ifdef _RC_DEBUG_
  fprintf(fp,"  Average Quant: %5.2f\n", quant_sum[vo_id]/(Float)quant_count[vo_id]);
#endif /*_RC_DEBUG_*/

}


/***************************************************************************
  matrix();
  ***************************************************************************/

Float **matrix(Int nrl, Int nrh, Int ncl, Int nch)
{
    Int i, j;
    Float **m;
    
    m = (Float **) malloc((U_Int) (nrh - nrl + 2) * sizeof(Float *));
    if (!m)
        exit(printf("allocation failure 1 in matrix()"));
    m -= nrl;
    m++;

    m[nrl-1]=(Float *) malloc((2 * sizeof(Float)));
    m[nrl-1][0]= (Float) (nrh - nrl + 1);
    m[nrl-1][1]= (Float) (nch - ncl + 1);

    for (i = nrl; i <= nrh; i++) {
        m[i] = (Float *) malloc((U_Int) (nch - ncl + 1) * sizeof(Float));
        if (!m[i])
            exit(printf("allocation failure 2 in matrix()"));
        m[i] -= ncl;
    }

    for (i = nrl; i <= nrh; i++)
        for (j = ncl; j <= nch; j++)
            m[i][j] = 0.0;

   return(m);
}

/***************************************************************************
  free_matrix();
  ***************************************************************************/

Void free_matrix(Float **m, Int nrl, Int nrh, Int ncl, Int nch)
{
    Int i;

    if((m[nrl-1][0] != (Float)(nrh - nrl + 1)) ||
       (m[nrl-1][1] != (Float)(nch - ncl + 1)))
      {
        fprintf(stderr,"original h=%d w=%d release h=%d w=%d\n",
               (Int)m[nrl-1][0],(Int)m[ncl-1][1], 
               nrh - nrl + 1, nch - ncl + 1);
        exit(printf("Inconsistency"));
      }
    m[nrl-1][1]=m[nrl-1][0]=0;
    free((Float *) (m[nrl-1]));

    for (i = nrh; i >= nrl; i--)
        free((Float *) (m[i] + ncl)), m[i]=NULL;
    free((Float *) (m + nrl-1)), m=NULL;
  }


/**************************************************************************** 
    Function
    --------
    buildLRModel();

    Arguments:
    ----------
    Float X;       independent data
    Float y;       dependent data
    Float b;       coefficient
    Int n;         number of data point
    Int order;     order number

    Return Values:
    -------------
    none

  ***************************************************************************/
/****************************************************************************
  buildLRModel();
  ***************************************************************************/
Void buildLRModel(Float **X, Float **y, Float ***b, Int n, Int order)
{
    Float **Xt, **XtX, **inverseXtX, **inverseXtX_Xt;
  
    /* X'*X */
    Xt=transpose_matrix(X, n, order);
    XtX=multiply_matrix(Xt, X, order, n, order);
 
    /* (X'*X)^-1 */
    inverseXtX=inverse(XtX, order);
    free_matrix(XtX, 1, order, 1, order);
 
    /* (X*X')^-1*X' */
    inverseXtX_Xt=multiply_matrix(inverseXtX, Xt, order, order, n);
    free_matrix(Xt, 1, order, 1, n);
    free_matrix(inverseXtX, 1, order, 1, order);
 
    /* (X*X')^-1*X'*y */
    *b=multiply_matrix(inverseXtX_Xt, y, order, n, 1);
    free_matrix(inverseXtX_Xt, 1, order, 1, n);
 
}


Float **transpose_matrix(Float **src, Int nrow, Int ncol)
{
    Int row, col;
    Float **dest;

    dest=matrix(1,ncol,1,nrow);
    for (row = 1; row <= nrow; row++)
        for (col = 1; col <= ncol; col++)
            dest[col][row] = src[row][col];
    return(dest);
}

Float **multiply_matrix(Float **ma, Float **mb, Int m, Int n, Int p)
{
    Int i, j, k;
    Float **mab;

    mab=matrix(1,m,1,p);
    for (i = 1; i <= m; i++)
        for (j = 1; j <= p; j++) {
            mab[i][j] = 0.0;
            for (k = 1; k <= n; k++)
                mab[i][j] += (ma[i][k] * mb[k][j]);
        }
    return(mab);
}

/*****/
Float **inverse(Float **a, Int N)
{
  Float **y;
  Float det;

  y=matrix(1,N,1,N);
  det = a[1][1]*a[2][2] - a[2][1]*a[1][2];
  if (det==0)
       exit(fprintf(stderr, "*****inverse: det=0\n"));
  y[2][2] = a[1][1]/det; 
  y[1][1] = a[2][2]/det;
  y[1][2] = -a[1][2]/det;
  y[2][1] = -a[2][1]/det;
  return (y);

}

