/*-----------------------------------------------------------------------------
    Routine to re-estimate the parameters
    n = noise,
    q = channel
    n_var = variance of noise 

    VTS is prone to instability. This routine checks for instability and 
    if it occurs smooths all the components that bomb out to lie
    in the range of the adjacent correct components.
------------------------------------------------------------------------------*/
#include  <stdio.h>
#include  "VTS.h"

#define DIAGONALLOAD	1e-4;	/* Diagonal load for stable estimates */

int compute_params(float *n, float *q, float *n_var,	/* Noise, channel and noise variance */
		   float *A,	/* Terms used in reestimation */
		   float *B,	/* Terms used in reestimation */
		   float *C,	/* Terms used in reestimation */
		   float *D,	/* Terms used in reestimation */
		   float *E,	/* Terms used in reestimation */
		   float *F,	/* Terms used in reestimation */
		   float *G,	/* Terms used in reestimation */
		   float **wta, float **wtb,	/* The a and (1-a) terms */
		   float **xvar,	/* The variances of the x distribution */
		   int nmodes, int ndim	/* Number of gaussians and dimensionality */
    )
{
	int i, beginsmooth, endsmooth, nsmooth;
	int qerror[64], nerror[64],	/* Dimension-wise error flags for n and q */
	 errorq = 0, errorn = 0,	/* Generic error flags for n and q */
	 error = 0,		/* Generic error flag (n or q) */
	 nverror = 0, errornv[64];	/* Noise variance error flags */
	float width, Denom;
	float min_chan = -5, min_noise = -5 /* , min_nvar = 1.0e-15 */ ;
	float max_chan = 40, max_noise = 40, max_nvar = 10;

	/* Initialize instability flags for each dimension to 0 */
	for (i = 0; i < ndim; ++i)
		nerror[i] = qerror[i] = 0;

	// DIAGONAL LOAD
	for (i = 0; i < ndim; i++) {
		B[i] += DIAGONALLOAD;
		E[i] += DIAGONALLOAD;
		G[i] += DIAGONALLOAD;
	}

	for (i = 0; i < ndim; ++i) {
		Denom = B[i] * E[i] - C[i] * C[i];
		if (Denom != 0) {
			if ((n[i] =
			     (D[i] * B[i] - A[i] * C[i]) / Denom) <
			    min_noise) {
				/* 
				 * If the estimate is lesser than the threshold
				 * assume instability. Warn and set flags
				 */
				error = errorn = 1;
				nerror[i] = 1;
#ifdef DEBUG
				WARNING(("%d component of noise below limit\n", i));
#endif
			}
			if (n[i] > max_noise) {
				/* 
				 * If the estimate is greater than the threshold
				 * assume instability. Warn and set flags
				 */
				error = errorn = 1;
				nerror[i] = 1;
#ifdef DEBUG
				WARNING(("%d component of noise above limit\n", i));
#endif
			}
			if ((q[i] =
			     (A[i] * E[i] - D[i] * C[i]) / Denom) <
			    min_chan) {
				/* 
				 * If the estimate is lesser than the threshold
				 * assume instability. Warn and set flags
				 */
				error = errorq = 1;
				nerror[i] = 1;
#ifdef DEBUG
				WARNING(("%d component of channel below limit\n", i));
#endif
			}
			if (q[i] > max_chan) {
				/* 
				 * If the estimate is greater than the threshold
				 * assume instability. Warn and set flags
				 */
				error = errorq = 1;
				nerror[i] = 1;
#ifdef DEBUG
				WARNING(("%d component of channel above limit\n", i));
#endif
			}
		} else {
			error = errorq = errorn = 1;
			n[i] = max_noise + 1;
			q[i] = max_chan + 1;
#ifdef DEBUG
			WARNING(("%d component of noise and channel above limit\n", i));
#endif
		}
		if (G[i] > 0)
			n_var[i] = F[i] / G[i];
		else {
#ifdef DEBUG
			WARNING(("Denom in noise variance estimate = 0, setting to 1!\n"))
#endif
//            n_var[i] = max_nvar+1;
//            error = nverror = errornv[i] = 1;
			    n_var[i] = 1.0;
		}
		/*
		 * If the noise variance estimate is below the threshold
		 * set it at the threshold
		 */
		if (n_var[i] < MIN_NOISE_VAR) {
#ifdef DEBUG
			WARNING(("%d component of noise var below limit\n",
				 i));
#endif
			n_var[i] = MIN_NOISE_VAR;
		}
		if (n_var[i] > max_nvar) {
#ifdef DEBUG
			WARNING(("%d component of noise var above limit\n",
				 i));
#endif
			n_var[i] = max_nvar + 0.0001;
			error = nverror = errornv[i] = 1;
		}
	}

	/*
	 * If any of the components of noise or channel are in the wrong range
	 * the error flag is set.
	 * Effect a heck of a lot of smoothing..
	 */

	if (error) {
		/* If channel error */
		if (errorq) {
#ifdef DEBUG
			WARNING(("Smoothing channel estimate for component "));
#endif
			/*
			 * Find ranges of error and work on them
			 */
			beginsmooth = 0;
			while (beginsmooth < ndim) {
				while ((beginsmooth < ndim)
				       && (q[beginsmooth] > min_chan)
				       && (q[beginsmooth] < max_chan))
					++beginsmooth;
				/*
				 * If w've reached the end of the vector, there is no need
				 * for further smoothing
				 */
				if (beginsmooth == ndim)
					break;
				endsmooth = beginsmooth;
				--beginsmooth;
				while ((endsmooth < ndim) &&
				       ((q[endsmooth] < min_chan)
					|| (q[endsmooth] > max_chan)))
					++endsmooth;

				/* If the errors are not at the beginning frame on */
				if (beginsmooth >= 0) {
					/* If the errors do no go to the end of frame */
					if (endsmooth < ndim) {
						nsmooth =
						    endsmooth -
						    beginsmooth;
						width =
						    (q[endsmooth] -
						     q[beginsmooth]) /
						    nsmooth;
						for (i =
						     beginsmooth + 1,
						     nsmooth = 1;
						     i < endsmooth;
						     ++i, ++nsmooth) {
#ifdef DEBUG
							printf("%d ", i);
#endif
							q[i] =
							    q[beginsmooth]
							    +
							    nsmooth *
							    width;
						}
					} else {
						nsmooth =
						    ndim + 1 - beginsmooth;
						width =
						    (min_chan -
						     q[beginsmooth]) /
						    nsmooth;
						for (i =
						     beginsmooth + 1,
						     nsmooth = 1; i < ndim;
						     ++i, ++nsmooth) {
#ifdef DEBUG
							printf("%d ", i);
#endif
							q[i] =
							    q[beginsmooth]
							    +
							    nsmooth *
							    width;
						}
					}
				} else {
					if (endsmooth < ndim) {
						nsmooth = endsmooth;
						width =
						    (q[endsmooth] -
						     min_chan) / nsmooth;
						for (i = 0, nsmooth = 0;
						     i < endsmooth;
						     ++i, ++nsmooth) {
#ifdef DEBUG
							printf("%d ", i);
#endif
							q[i] =
							    min_chan +
							    nsmooth *
							    width;
						}
					} else {
						WARNING(("..Setting all channel values to 0!!\n"));
						WARNING(("Possible instability!!\n"));
						for (i = 0; i < ndim; ++i)
							q[i] = 0;
					}
				}
				beginsmooth = endsmooth;
			}
#ifdef DEBUG
			printf("\n");
			fflush(stdout);
#endif
		}
		if (errorn) {
#ifdef DEBUG
			WARNING(("Smoothing noise estimate for component "));
#endif
			beginsmooth = 0;
			while (beginsmooth < ndim) {
				while ((beginsmooth < ndim)
				       && (n[beginsmooth] > min_noise)
				       && (n[beginsmooth] < max_noise))
					++beginsmooth;
				/*
				 * If w've reached the end of the vector, there is no need
				 * for further smoothing
				 */
				if (beginsmooth == ndim)
					break;
				endsmooth = beginsmooth;
				--beginsmooth;
				while ((endsmooth < ndim) &&
				       ((n[endsmooth] < min_noise)
					|| (n[endsmooth] > max_noise)))
					++endsmooth;

				if (beginsmooth >= 0) {
					if (endsmooth < ndim) {
						nsmooth =
						    endsmooth -
						    beginsmooth;
						width =
						    (n[endsmooth] -
						     n[beginsmooth]) /
						    nsmooth;
						for (i =
						     beginsmooth + 1,
						     nsmooth = 1;
						     i < endsmooth;
						     ++i, ++nsmooth) {
#ifdef DEBUG
							printf("%d ", i);
#endif
							n[i] =
							    n[beginsmooth]
							    +
							    nsmooth *
							    width;
						}
					} else {
						for (i = beginsmooth + 1;
						     i < ndim; ++i) {
#ifdef DEBUG
							printf("%d ", i);
#endif
							n[i] =
							    n[beginsmooth];
						}
					}
				} else {
					if (endsmooth < ndim) {
						for (i = 0; i < endsmooth;
						     ++i) {
#ifdef DEBUG
							printf("%d ", i);
#endif
							n[i] =
							    n[endsmooth];
						}
					} else {
						WARNING(("..Setting all noise values to 0!!\n"));
						WARNING(("Possible instability!!\n"));
						for (i = 0; i < ndim; ++i)
							n[i] = 0;
					}
				}
				beginsmooth = endsmooth;
			}
#ifdef DEBUG
			printf("\n");
			fflush(stdout);
#endif
		}
		if (nverror) {
#ifdef DEBUG
			WARNING(("Smoothing noise var estimate for component "));
#endif
			beginsmooth = 0;
			while (beginsmooth < ndim) {
				while ((beginsmooth < ndim)
				       && (n_var[beginsmooth] < max_nvar))
					++beginsmooth;
				/*
				 * If w've reached the end of the vector, there is no need
				 * for further smoothing
				 */
				if (beginsmooth == ndim)
					break;
				endsmooth = beginsmooth;
				--beginsmooth;
				while ((endsmooth < ndim)
				       && (n_var[endsmooth] > max_nvar))
					++endsmooth;

				if (beginsmooth >= 0) {
					if (endsmooth < ndim) {
						nsmooth =
						    endsmooth -
						    beginsmooth;
						width =
						    (n_var[endsmooth] -
						     n_var[beginsmooth]) /
						    nsmooth;
						for (i =
						     beginsmooth + 1,
						     nsmooth = 1;
						     i < endsmooth;
						     ++i, ++nsmooth) {
#ifdef DEBUG
							printf("%d ", i);
#endif
							n_var[i] =
							    n_var
							    [beginsmooth] +
							    nsmooth *
							    width;
						}
					} else {
						for (i = beginsmooth + 1;
						     i < ndim; ++i) {
#ifdef DEBUG
							printf("%d ", i);
#endif
							n_var[i] =
							    n_var
							    [beginsmooth];
						}
					}
				} else {
					if (endsmooth < ndim) {
						for (i = 0; i < endsmooth;
						     ++i) {
#ifdef DEBUG
							printf("%d ", i);
#endif
							n_var[i] =
							    n_var
							    [endsmooth];
						}
					} else {
						WARNING(("..Setting all nvar values to 0!!\n"));
						WARNING(("Possible instability!!\n"));
						for (i = 0; i < ndim; ++i)
							n_var[i] = 1e-10;
					}
				}
				beginsmooth = endsmooth;
			}
#ifdef DEBUG
			printf("\n");
			fflush(stdout);
#endif
		}
		return (UNSURE);
	} else
		return (SUCCESS);
}
