#define lint

#include <Python.h>
#include "Numeric/arrayobject.h"

#include <float.h>

#include "DAmodule.h"

#ifndef TRUE
#define TRUE 1
#define FALSE 0
#endif

#ifndef DEBUG
#define DEBUG 0
#endif

/**
 * DA Methods Table
 */
static PyMethodDef DAMethods[] =
{
  { "mixture_likelihood"    , DA_mixture_likelihood     , METH_VARARGS },
  { "covar_weights_estimate", DA_covar_weights_estimate , METH_VARARGS },
  //  { "covariance_estimate"   , DA_covariance_estimate    , METH_VARARGS },
  //  { "fix_covariance_matrix" , DA_fix_covariance_matrix  , METH_VARARGS },
  //  { "isposdef"              , DA_is_posdef_dmat         , METH_VARARGS },
  { "make_diag_matrix"      , DA_make_diag_matrix       , METH_VARARGS },
  //  { "dcholdc"               , DA_dcholdc                , METH_VARARGS },
  { "vector_prob_gauss"     , DA_vector_prob_gauss      , METH_VARARGS },

  //  { "TestAsDouble1D"        , DA_TestAsDouble1D         , METH_VARARGS },
  //  { "TestAsDouble2D"        , DA_TestAsDouble2D         , METH_VARARGS },
  //  { "TestAsDouble3D"        , DA_TestAsDouble3D         , METH_VARARGS },

  { NULL, NULL }
};

/**
 * Forward declarations
 */
void make_diag_matrix(double **matrix, int num_rows, int num_cols);

/**
 * DA Module Initialization
 */
void initcDA(void)
{
  /**
   * Initialize this module with the given name and methods defined in
   * DAMethods table.
   */
  Py_InitModule( "cDA", DAMethods );


  /**
   * Required to access NumPy C functions.
   */
  import_array();
}



#define UT_OK 0
#define UT_PI  3.14159265358979
#define streq(a, b) (strcmp((a), (b)) == 0)
typedef unsigned char boolean;

/*************************************8
 * NR utils
 */
#define NR_END 1
#define NR_FREE_ARG char*

void NR_error(char error_text[])
/* Numerical Recipes standard error handler */
{
	fprintf(stderr,"Numerical Recipes run-time error...\n");
	fprintf(stderr,"%s\n",error_text);
	fprintf(stderr,"...now exiting to system...\n");
	exit(1);
}


double *NR_dvector(long nl, long nh)
/* allocate a double vector with subscript range v[nl..nh] */
{
	double *v;

	v=(double *)malloc((size_t) ((nh-nl+1+NR_END)*sizeof(double)));
	if (!v) NR_error("allocation failure in dvector()");
	return v-nl+NR_END;
}

double **NR_dmatrix(long nrl, long nrh, long ncl, long nch)
/* allocate a double matrix with subscript range m[nrl..nrh][ncl..nch] */
{
	long i, nrow=nrh-nrl+1,ncol=nch-ncl+1;
	double **m;

	/* allocate pointers to rows */
	m=(double **) malloc((size_t)((nrow+NR_END)*sizeof(double*)));
	if (!m) NR_error("allocation failure 1 in matrix()");
	m += NR_END;
	m -= nrl;

	/* allocate rows and set pointers to them */
	m[nrl]=(double *) malloc((size_t)((nrow*ncol+NR_END)*sizeof(double)));
	if (!m[nrl]) NR_error("allocation failure 2 in matrix()");
	m[nrl] += NR_END;
	m[nrl] -= ncl;

	for(i=nrl+1;i<=nrh;i++) m[i]=m[i-1]+ncol;

	/* return pointer to array of pointers to rows */
	return m;
}

void NR_free_dvector(double *v, long nl, long nh)
/* free a double vector allocated with dvector() */
{
	free((NR_FREE_ARG) (v+nl-NR_END));
}

void NR_free_dmatrix(double **m, long nrl, long nrh, long ncl, long nch)
/* free a double matrix allocated by dmatrix() */
{
	free((NR_FREE_ARG) (m[nrl]+ncl-NR_END));
	free((NR_FREE_ARG) (m+nrl-NR_END));
}

void NR_dcholdc(double **a, int n, double p[])
{
	void NR_error(char error_text[]);
	int i,j,k;
	double sum;

	for (i=1;i<=n;i++) {
		for (j=i;j<=n;j++) {
      sum=a[i][j];
			for (k=i-1;k>=1;k--) {
        sum -= a[i][k]*a[j][k];
      }
			if (i == j) {
				if (sum <= 0.0) {
					NR_error("NR_dcholdc failed");
        }
				a[i][j] = p[i]=sqrt(sum);
			} else a[j][i]=sum/p[i];
		}
	}
}

static PyObject *
DA_dcholdc(PyObject *self, PyObject *args)
{
  // function parameters
  PyObject *po_matrix      = NULL;

  // C versions of parameters
  double **matrix     = NULL;

  // information computed from parameters
  int M = 0;
  int N = 0;

  // internal status information
  int status      = 0;
  int error       = 0;
  int num_rows    = 0;
  int num_cols    = 0;

  // results of wrapped function
  //double **cholesky = NULL;
  double *p         = NULL;

  // python version of results
  PyObject *po_p    = NULL;
  PyObject *po_result_tuple = NULL;

  //// Interpret paraters passed to the python end of the functio
  //
  // Parse the arguments to get the matricies as Python objects
  //
  status = PyArg_ParseTuple( args, "O", &po_matrix);
  if (status == 0)
  {
    error = 1;
    goto do_exit;
  }

  //
  // Convert the python object po_data to a C 2D 
  // M-by-N double matrix (double **)
  //
  matrix = DAArray_AsDouble2D( &po_matrix, &num_rows, &num_cols);
  if (matrix == NULL)
  {
    error = 1;
    goto do_free_matrix;
  }

  M = num_rows;
  N = num_cols;

  ////// Construct C objects to receive results.
  // Construct 1D matrix p
  p = DAArray_NewDouble1D( &po_p, M);

  if (p == NULL)
  {
    error = 2;
    goto do_free_p;
  }

  NR_dcholdc(matrix, N, p);

  ////// Cleanup and termination
  do_free_p:         DAArray_Free( po_p        , p     );
  do_free_matrix:    DAArray_Free( po_matrix , matrix  );

  //
  // Single exit point for function.
  //
  do_exit:
  if (error == 1)
  {
    return NULL;
  }
  else if (error == 2)
  {
    return PyErr_NoMemory();
  }
  else
  {
    po_result_tuple = PyTuple_New(2);
    if ( po_result_tuple != NULL )
    {
      PyTuple_SetItem(po_result_tuple, 0, po_matrix);
      PyTuple_SetItem(po_result_tuple, 1, po_p);
      return po_result_tuple;
    }
    else
    {
       return NULL;
    }       
  }

}

/*******************************************************************************
COPY_DMAT
Copy a matrix of doubles "a" into a matrix "b".
RG,AG
*******************************************************************************/
int copy_dmat(double **a, double **b, int nr, int nc)
{
  int     mem_size;
  double  *p_a, *p_b;

  p_a = &a[1][1];
  p_b = &b[1][1];

  mem_size = nr * nc * sizeof(double);

  memcpy(p_b, p_a, mem_size);

  return (UT_OK);
}

/*******************************************************************************
GRAB_ROW_DMAT
Create the vector obtained from the specified row of a matrix of doubles.
RG
*******************************************************************************/
int grab_row_dmat(double **mat, int index, int nc, double *vec)
{
  int     mem_size;
  double *p_vec;
  double *p_mat;

  mem_size = nc * sizeof(double);

  p_vec = &vec[1];
  p_mat = &mat[index][1];

  memcpy(p_vec, p_mat, mem_size);

  return (UT_OK);
}

/*******************************************************************************
GRAB_COL_DMAT
Create the vector obtained from the specified column of a matrix of doubles.
AG
*******************************************************************************/
int grab_col_dmat(double **mat, int index, int nr, double *vec)
{
  int   j;

  for (j=1; j <= nr; j++)
    vec[j] = mat[j][index];

  return (UT_OK);
}

/*******************************************************************************
SUBTRACT_DVEC
Subtract one vector of doubles element-wise from another.  The second
vector is subtracted from the first vector.
RG
*******************************************************************************/
int subtract_dvec(double *v1, double *v2, double *v3, int n)
{
  double  *p1;
  double  *p2;
  double  *p3;
  double  *p3_end;
 
  /* assign pointer to last element of output vector */
  p3_end = &v3[n];

  for (p1 = &v1[1], p2 = &v2[1], p3 = &v3[1]; p3 <= p3_end; p1++, p2++, p3++)
    *p3 = *p1 - *p2;

  return (UT_OK);
}

/*******************************************************************************
DOT_PRODUCT_DVEC
Compute the dot product of two vectors of doubles.
RG
*******************************************************************************/
double dot_product_dvec(double *x, double *y, int n)
{
  double *p_x;
  double *p_y;
  double *p_end_x;
  double  result = 0.0;

  /* assign pointer to last element of x */
  p_end_x = &x[n];

  /* compute dot product */
  for (p_x = &x[1], p_y = &y[1]; p_x <= p_end_x; p_x++, p_y++)
    result += (*p_x) * (*p_y);

  return (result);
}

/*******************************************************************************
RESTRICT_ILLCOND_DMATRIX
A hack procedure to restrict an ill-conditioned matrix such that it stays 
out of the realm of ill-conditioned matrices (hopefully), by ensuring that
the diagonal elements are above some small specified value.

mat is the input matrix.
dim is the size of the square matrix.
min_diag is the minimum value for a diagonal element of the matrix.
AG
*******************************************************************************/
int restrict_illcond_dmatrix(double **mat, int dim, double min_diag)
{
  int     i;

  /* NOTE: this should ideally compute the condition number of the matrix,
     and only do this if the condition number is less than some value */

  /* check each element of the diagonal */
  for (i = 1; i <= dim; i++) {
    if (mat[i][i] <= min_diag)
      mat[i][i] = min_diag;
  }

  return (UT_OK);
}


/*******************************************************************************
SCALAR_DIV_DMAT
Scales a matrix of doubles by a specified dividend.

nr and nc are the dimensions of the matrix (num. rows and num. cols).
m is the input matrix.
constant is the dividend to scale the matrix by.
RG
*******************************************************************************/
int scalar_div_dmat(double **m, int nr, int nc, double constant)
{
  double  *p;
  double  *p_end;
  double  inv_constant;

  /* assign pointer to last element */
  p_end = &m[nr][nc];

  /* calculate the inverse of the constant to save on divides */
  if (constant == 0.0)
    inv_constant = DBL_MAX;
  else
    inv_constant = 1.0 / constant;

  /* change each element of the matrix */
  for (p = &m[1][1]; p <= p_end; p++)
    *p *= inv_constant;

  return (UT_OK);
}

/*******************************************************************************
SCALAR_MULT_DVEC
Scales a vector of doubles by a specified multiplicand.

n is the length of the vector.
v is the input vector.
constant is the multiplicand to scale the vector by.
RG
*******************************************************************************/
int scalar_mult_dvec(double *v, int n, double constant)
{
  double  *p;
  double  *p_end;

  /* assign pointer to last element */
  p_end = &v[n];

  /* change each element of the vector */
  for (p = &v[1]; p <= p_end; p++)
    *p *= constant;

  return (UT_OK);
}

/*******************************************************************************
SCALAR_DIV_DVEC
Scales a vector of doubles by a specified dividend.

n is the length of the vector.
v is the input vector.
constant is the dividend to scale the vector by.
RG
*******************************************************************************/
int scalar_div_dvec(double *v, int n, double constant)
{
  double  *p;
  double  *p_end;
  double  inv_constant;

  /* assign pointer to last element */
  p_end = &v[n];

  /* calculate the inverse of the constant to save on divides */
  if (constant == 0.0)
    inv_constant = DBL_MAX;
  else
    inv_constant = 1.0 / constant;

  /* change each element of the vector */
  for (p = &v[1]; p <= p_end; p++)
    *p *= inv_constant;

  return (UT_OK);
}

/*******************************************************************************
SET_DMAT
Sets each of the values of a matrix of doubles to a specified number.

nr and nc are the dimensions of the matrix (num. rows and num. cols).
m is the input matrix.
constant is the amount to set all the values to.
AG
*******************************************************************************/
int set_dmat(double **m, int nr, int nc, double constant)
{
  int     i,j;

  /* change each element of the vector */
  for (i = 1; i <= nr; i++)
    for (j = 1; j <= nc; j++)
      m[i][j] = constant;

  return (UT_OK);
}



/*******************************************************************************
SET_DVEC
Sets each of the values of a vector of doubles to a specified number.

nc is the length of the vector (num. cols).
v is the input vector.
constant is the amount to set all the values to.
AG
*******************************************************************************/
int set_dvec(double *v, int n, double constant)
{
  int     i;

  /* change each element of the vector */
  for (i = 1; i <= n; i++) {
    v[i] = constant;
  }

  return (UT_OK);
}

/*******************************************************************************
SUM_DVEC
Sum the values in a vector of doubles.
RG
*******************************************************************************/
double sum_dvec(double *v, int n)
{
  double  *p;
  double  *p_end;
  double   sum = 0.0;

  /* assign pointer to last element */
  p_end = &v[n];

  /* accumulate square of the vector */
  for (p = &v[1]; p <= p_end; p++)
    sum += *p;

  return (sum);
}

/*******************************************************************************
VECTOR_PROB_GAUSS
Compute the conditional probability of the datum assuming it was drawn from a 
multivariate normal distribution with the specified mean and covariance matrix,
using the normal probability density function.  Returns the vector of proba-
bilities, one for each datum.
RG
*******************************************************************************/
double* vector_prob_gauss(double **data, double *mean, double **covar, int dim, 
                         int numdata, double *probs, double min_diag,
                         char *rows_or_cols)

{
    int    i, j, k;
    double mahalanobis;
    double **covar_inv, **covar_copy;
    double *datum, *diff, *tempvec;
    double denom;
    double sum;
    boolean row_flag;

    /* make temporary storage */
    datum =      NR_dvector(1,dim);
    diff =       NR_dvector(1,dim);
    tempvec =    NR_dvector(1,dim);
    covar_copy = NR_dmatrix(1,dim,1,dim);

    /* set flag to avoid string comparisons */
    if (streq(rows_or_cols, "rows"))
      row_flag = TRUE;
    else
      row_flag = FALSE;

    /* try to ensure that the covariance matrix is not ill-conditioned,
       before we try to invert it */
    restrict_illcond_dmatrix(covar, dim, min_diag);

    /* copy the covariance matrix */
    copy_dmat(covar, covar_copy, dim, dim);

    /* compute the cholesky decomposition L*L^T of the covariance matrix */
    NR_dcholdc(covar_copy, dim, tempvec);

    /* compute the inverse cholesky decomposition L^-1
     * reference: Numerical Recipies, p. 98
     */
    for (i = 1; i <= dim; i++) {
      covar_copy[i][i] = 1.0 / tempvec[i];
      for (j = i+1; j <= dim; j++) {
        sum = 0.0;
        for (k = i; k < j; k++) {
          sum -= covar_copy[j][k] * covar_copy[k][i];
        }
        covar_copy[j][i] = sum / tempvec[j];
      }
    }
    covar_inv = covar_copy;

    /* compute the sqare root of the determinant of the covariance matrix */
    denom = 1.0;
    for (i = 1; i <= dim; i++)
      denom *= tempvec[i];

    /* compute the constant factor */
    denom = pow((double)(2.0*UT_PI), (double)(dim/2.0)) * denom;

    /* compute probability for each datum in the dataset */
    for (k = 1; k <= numdata; k++)
    {
      /* get the k'th datum from the dataset */
      if (row_flag == TRUE)
        grab_row_dmat(data, k, dim, datum);
      else
        grab_col_dmat(data, k, dim, datum);

      /* compute Mahalanobis distance */
      subtract_dvec(datum, mean, diff, dim);
      for (i = 1; i <= dim; i++) {
        tempvec[i] = 0.0;
        for (j = 1; j <= i; j++)
          tempvec[i] += diff[j] * covar_inv[i][j];
      }
      mahalanobis = dot_product_dvec(tempvec, tempvec, dim);

      /* compute probability */
      probs[k] = (double) exp(-0.5 * mahalanobis) / (double) denom;

      /* protect against returning unexpected zeros */
      if (probs[k] == 0.0)
        probs[k] = DBL_MIN;

      /* protect against returning infinity */
      if (probs[k] > DBL_MAX)
        probs[k] = DBL_MAX;
    }

    /* clean up */
    NR_free_dvector(datum,1,dim);
    NR_free_dmatrix(covar_inv,1,dim,1,dim);
    NR_free_dvector(tempvec,1,dim);
    NR_free_dvector(diff,1,dim);

    return (probs);
}

/*******************************************************************************
MIXTURE_LIKELIHOOD
Given mixture model parameters, a dataset, and allocated space for storing
all the intermediate probabilities, compute the log-likelihood of the model 
given the data.

K is the number of components in the mixture.
means is the  matrix of mean vectors, where each row is a mean.
covars is the array of covariance matrices.
weights is the vector of class weights.
data is the dataset.
probs is the matrix to contain all intermediate probabilities.
prob_data is the vector to contain the posterior probability of each datum
given the model.
sum_probs is the vector to contain the posterior probability of each class
given the data.
numrows is the number of data.
numcols is the number of attributes.
rows_or_cols is the string specifying whether data are stored as rows or as
columns.
min_diag is a small number for perturbing ill-conditioned covariance matrices.
AG
*******************************************************************************/
double mixture_likelihood(int K, double **means, double ***covars, double *weights,
                         double **data, double **probs, double *prob_data, 
                         double *sum_probs, int numrows, int numcols, 
                         char *rows_or_cols, double min_diag)

{
  int      i,k;
  double    log_likelihood;
  double   *mean_k, **covar_k, *probs_k;

  /* initialize */
  set_dvec(prob_data, numrows, 0.0);
  set_dvec(sum_probs, K, 0.0);
  log_likelihood = 0.0;

  for (k=1; k<=K; k++)
  {
    /* get the parameters corresponding to this class; also get the place
       where the probabilities will be stored */
    mean_k  = means[k];
    covar_k = covars[k];
    probs_k = probs[k];
    
    /* note that the following 3 operations all cycle over the data; they
       should be consolidated for efficiency */

    /* A. first compute the LIKELIHOOD of the data given the parameters for
       this class; store it in the array probs[k] */
    vector_prob_gauss(data, mean_k, covar_k, numcols, numrows, probs_k,
                      min_diag, rows_or_cols);
    
    /* B. multiply by the class prior, or weight, to get the JOINT
       probability of the data and the parameters; store it in probs[k] */
    scalar_mult_dvec(probs_k, numrows, weights[k]);

    /* contribute to the probability of each datum across all models */
    for (i=1; i<=numrows; i++)
      prob_data[i] += probs_k[i];
    /*
    add_dvec(numrows, probs_k, prob_data, prob_data);
    */
  }

  /* C. normalize by the probability of the data across all models to get 
     the POSTERIOR probability of each datum; store it in probs[k] */
  for (k=1; k<=K; k++)
  {
    probs_k = probs[k];
    for (i=1; i<=numrows; i++)
      if (prob_data[i] != 0)
        probs_k[i] /= prob_data[i];
  }
  /*
    div_dvec_elt(numrows, probs_k, prob_data, probs_k);
    */

  /* compute the SUM OF THE POSTERIOR probabilities for each class, which
     is used in computing the parameters later */
  for (k=1; k<=K; k++)
    sum_probs[k] = sum_dvec(probs[k], numrows);

  /* compute the log-likelihood of all the data given the mixture model */
  for (i=1; i<=numrows; i++) {
    log_likelihood += (double) log((double) (prob_data[i] + DBL_MIN));
  }

  return(log_likelihood);
}

/*******************************************************************************
POSDEF_SYM_ALLOC_DMAT
Checks whether a symmetric matrix of doubles is positive definite or not, by 
using a Cholesky decomposition.
RG
*******************************************************************************/
int posdef_sym_alloc_dmat(double **mat, int dim, int *posdef)
{
  double *p;
  double **mat_copy;
  int     i, j, k;
  double  sum;

  /* allocate some temporary storage */
  p = NR_dvector(1, dim);
  mat_copy = NR_dmatrix(1, dim, 1, dim);

  /* copy the matrix so that it isn't destroyed */
  copy_dmat(mat, mat_copy, dim, dim);

  /* check whether it is postive definite by performing the decomposition */
  *posdef = TRUE;
  for (i = 1; i <= dim; i++) { 
    for (j = i; j <= dim; j++) { 
      for (sum = mat_copy[i][j], k = i - 1; k >= 1; k--) 
	sum -= mat_copy[i][k] * mat_copy[j][k]; 
      if (i == j) { 
	if (sum <= 0.0) {
	  *posdef = FALSE;
	  break;
	}
	p[i] = sqrt(sum); 
      } else 
       mat_copy[j][i] = sum / p[i];
    }
    if (*posdef == FALSE)
      break;
  }

  /* clean up allocated memory */
  NR_free_dvector(p, 1, dim);
  NR_free_dmatrix(mat_copy, 1, dim, 1, dim);

  return(UT_OK);
}

/**
 * likelihood = DA.mixture_likelihood(data, means, covars, weights, min_diag)
 *
 * Returns the log-likelihood of data given an N dimensional mixture of
 * k-Gaussian models.
 *
 * Where:
 *   - data is a M-by-N matrix containing N-dimensional datapoints (one per
 *     row) to use when computing the log-likelihood.
 *
 *   - means is a k-by-N matrix containing the means (one per row) of the
 *     k-Gaussian models.
 *
 *   - covars is a 3D matrix composed of k N-by-N covariance matrices.
 *
 *   - weights is a 1-by-k vector of model weights.
 *
 *   - min_diag is a small number for perturbing ill-conditioned covariance
 *     matrices.  Optional.  If not specified, defaults to 1.0e-8.
 */
static PyObject *
DA_mixture_likelihood(PyObject *self, PyObject *args)
{
  PyObject *po_data      = NULL;
  PyObject *po_means     = NULL;
  PyObject *po_covars    = NULL;
  PyObject *po_weights   = NULL;
  PyObject *po_probs     = NULL;
  PyObject *po_prob_data = NULL;
  PyObject *po_sum_probs = NULL;

  char *format  = NULL;
  char *message = NULL;

  double **data     = NULL;
  double **means    = NULL;
  double ***covars  = NULL;
  double *weights   = NULL;
  double **probs    = NULL;
  double *prob_data = NULL;
  double *sum_probs = NULL;
  double **temp2D   = NULL;
  double min_diag   = -1.0;
  double likelihood =  0.0;

  int i        = 0;
  int k        = 0;
  int M        = 0;
  int N        = 0;
  int num_rows = 0;
  int num_cols = 0;
  int num_mats = 0;
  int error    = 0;
  int size     = 0;
  int status   = 0;


  //
  // Parse args to get matrices as Python objects and min_diag as a C
  // double.
  //
  status = PyArg_ParseTuple( args,
                             "OOOO|d",
                             &po_data,
                             &po_means,
                             &po_covars,
                             &po_weights,
                             &min_diag );

  if (status == 0)
  {
    error = 1;
    goto do_exit;
  }
  

  //
  // Convert the Python object po_data to a C 2D double matrix (double **).
  //
  data = DAArray_AsDouble2D( &po_data, &num_rows, &num_cols );

  if (data == NULL)
  {
    error = 1;
    goto do_exit;
  }

  M = num_rows;
  N = num_cols;


  //
  // Convert the Python object po_means to a C 2D double matrix
  // (double **).
  //
  means = DAArray_AsDouble2D( &po_means, &num_rows, &num_cols );

  if (means == NULL)
  {
    error = 1;
    goto do_free_data;
  }
  else if (num_cols != N)
  {
    error = 1;
    PyErr_SetString( PyExc_ValueError,
                     "Data and means must have the same number of dimensions "
                     "(matrix columns)." );
    goto do_free_means;
  }

  k = num_rows;


  //
  // Convert the Python object po_covars to a C 3D double matrix
  // (double ***).
  //
  covars = DAArray_AsDouble3D( &po_covars, &num_mats, &num_rows, &num_cols );

  if (covars == NULL)
  {
    error = 1;
    goto do_free_means;
  }
  else if (num_rows != num_cols)
  {
    error = 1;
    PyErr_SetString( PyExc_ValueError,
                     "Each covariance matrix must be square." );
    goto do_free_covars;
  }
  else if (k != num_mats)
  {
    error = 1;
    PyErr_SetString( PyExc_ValueError,
                     "The number of covariance matrices does not match "
                     "the number of models (classes)." );
    goto do_free_covars;
  }


  //
  // Test each covariance matrix for symmetry.
  //
  for (i = 1; i <= num_mats; i++)
  {
    temp2D = covars[i];
    status = DAArray_Is2DSymmetric( temp2D, num_rows );

    if (status == 0)
    {
      error = 1;

      format  = "Covariance matrix %d is not symmetric.\n";
      size    = strlen(format) + 2;
      message = (char *) PyMem_Malloc(size);

      if (message == NULL)
      {
        error = 2;
      }
      else
      {
        snprintf(message, size, format, i);
        PyErr_SetString( PyExc_ValueError, message );
      }

      break;
    }
  }


  //
  // Test each covariance matrix for postive definiteness.
  //
  for (i = 1; i <= num_mats; i++)
  {
    temp2D = covars[i];
    posdef_sym_alloc_dmat( temp2D, num_rows, &status );

    if (status == FALSE)
    {
      error = 1;

      format  = "Covariance matrix %d is not positive definite.\n";
      size    = strlen(format) + 2;
      message = (char *) PyMem_Malloc(size);

      if (message == NULL)
      {
        error = 2;
      }
      else
      {
        snprintf(message, size, format, i);
        PyErr_SetString( PyExc_ValueError, message );
      }

      break;
    }
  }

  if (error == 1)
  {
    goto do_free_means;
  }


  //
  // If set, convert the Python object po_weights to a C 1D double matrix
  // (double *).
  //
  weights = DAArray_AsDouble1D( &po_weights, &num_rows );

  if (weights == NULL)
  {
    error = 1;
    goto do_free_covars;
  }
  else if (k != num_rows)
  {
    error = 1;
    PyErr_SetString( PyExc_ValueError,
                     "The number of class weights does not match the "
                     "the number of models (classes)." );
    goto do_free_weights;
  }

  //
  // If not set, assign min_diag a default value.
  //
  if (min_diag == -1.0)
  {
    min_diag = 1.0e-6;
  }


  //
  // Allocate intermediate storage for mixture_likelihood, probs.
  //
  probs = DAArray_NewDouble2D( &po_probs, k, M );

  if (probs == NULL)
  {
    error = 2;
    goto do_free_weights;
  }

  //
  // Allocate intermediate storage for mixture_likelihood, prob_data.
  //
  prob_data = DAArray_NewDouble1D( &po_prob_data, M );

  if (prob_data == NULL)
  {
    error = 2;
    goto do_free_probs;
  }

  //
  // Allocate intermediate storage for mixture_likelihood, sum_probs.
  //
  sum_probs = DAArray_NewDouble1D( &po_sum_probs, k );

  if (sum_probs == NULL)
  {
    error = 2;
    goto do_free_prob_data;
  }

  //
  // Actual call to da_cluster::mixture_likelihood().
  //

  likelihood = mixture_likelihood( k,
                                   means,
                                   covars,
                                   weights,
                                   data,
                                   probs,
                                   prob_data,
                                   sum_probs,
                                   M,
                                   N,
                                   "rows",
                                   min_diag );

  //
  // The labels and gotos are necessary to properly "unwind" the "stack"
  // of mallocs, given that there are multiple points of possible failure
  // above.
  //

  DAArray_Free( po_sum_probs, sum_probs );

  do_free_prob_data: DAArray_Free( po_prob_data, prob_data );
  do_free_probs:     DAArray_Free( po_probs    , probs     );
  do_free_weights:   DAArray_Free( po_weights  , weights   ); 
  do_free_covars:    DAArray_Free( po_covars   , covars    );
  do_free_means:     DAArray_Free( po_means    , means     );
  do_free_data:      DAArray_Free( po_data     , data      );

  //
  // Single exit point for function.
  //
  do_exit:
  if (error == 1)
  {
    return NULL;
  }
  else if (error == 2)
  {
    return PyErr_NoMemory();
  }
  else
  {
    return PyFloat_FromDouble(likelihood);
  }
}


/**
 * covariance = covariance_estimate(data, means, clust_labels)
 *
 * Compute an estimate of the covariance matrix and cluster weights from 
 * cluster means and labels of a k means run.
 *
 * Where:
 *   - data is a M-by-N matrix containing N-dimensional datapoints (one per
 *     row) to use when computing the estimates.
 *
 *   - means is a k-by-N mattrix containing the means (one per row) of the
 *     k-Gaussian models.
 *
 *   - clust_labels is
 *
 *   - covars is a 3D matrix composed of k, N-by-N covariance matricies.
 *
 * C Function being used:
 * int est_covar_matrices(
 *    double ***covars,     [out]
 *    double **data,        [in]
 *    double **means,       [in]
 *    int    *clust_label,  [in]
 *    int    num_data,      [in]
 *    int    num_dims,      [in]
 *    int    K              [in]
 * )
 *
 */

static PyObject *
DA_covariance_estimate(PyObject *self, PyObject *args)
{
  // function parameters
  PyObject *po_data        = NULL;
  PyObject *po_means       = NULL;
  PyObject *po_clust_label = NULL;

  // C versions of parameters
  double **data            = NULL;
  double **means           = NULL;
  int    *clust_label      = NULL;

  // information computed from parameters
  int k = 0;
  int M = 0;
  int N = 0;

  // internal status information
  int status      = 0;
  int error       = 0;
  int num_rows    = 0;
  int num_cols    = 0;
  int i           = 0;

  // results of wrapped function
  double ***covars         = NULL;

  // python version of results
  PyObject *po_covars       = NULL;

  //// Interpret paraters passed to the python end of the functio
  //
  // Parse the arguments to get the matricies as Python objects
  //
  status = PyArg_ParseTuple( args, "OOO", &po_data, &po_means, &po_clust_label);
  if (status == 0)
  {
    error = 1;
    goto do_exit;
  }

  //
  // Convert the python object po_data to a C 2D 
  // M-by-N double matrix (double **)
  //
  data = DAArray_AsDouble2D( &po_data, &num_rows, &num_cols);
  if (data == NULL)
  {
    error = 1;
    goto do_exit;
  }

  M = num_rows;
  N = num_cols;

  //
  // Convert the python object po_means to a C 2D 
  // k-by-N double matrix (double **)
  //
  means = DAArray_AsDouble2D( &po_means, &num_rows, &num_cols );
  if (means == NULL)
  {
    error = 1;
    goto do_free_data;
  }
  else if (num_cols != N)
  {
    error = 1;
    PyErr_SetString( PyExc_ValueError,
		     "Data and means must have the same number of dimensions "
		     "(matrix columns)." );
    goto do_free_means;
  }

  k = num_rows;

  // 
  // Convert po_clust_label to an M double vector (double *)
  //
  clust_label = DAArray_AsInt1D( &po_clust_label , &num_rows );

  if (clust_label == NULL)
  {
    error = 1;
    goto do_free_means;
  }
  else if (M != num_rows)
  {
    error = 1;
    PyErr_SetString (PyExc_ValueError,
		     "The number of class labels does not match the "
		     "number of data vectors." );
    goto do_free_clust_label;
  }

  // The classes in clust_labels are 0 based (from python) 
  // and need to be 1 for the da libraries to work correctly
  for( i = 1; i <= num_rows; i++)
  {
    clust_label[i]++;
  }

  ////// Construct C objects to receive results.
  // Construct covariance 3D matrix
  covars = DAArray_NewDouble3D( &po_covars, k, N, N);

  if (covars == NULL)
  {
    error = 2;
    goto do_free_clust_label;
  }

  /////// Compute covariance matrix
  // 1. Estimate the full covariance matrix.
  status = est_covar_matrices(covars,
			      data,
			      means,
			      clust_label,
			      M,
			      N,
			      k);
  
  ////// Cleanup and termination
 do_free_covars:         DAArray_Free( po_covars      , covars      );
 do_free_clust_label:    DAArray_Free( po_clust_label , clust_label );
 do_free_means:          DAArray_Free( po_means       , means       );
 do_free_data:           DAArray_Free( po_data        , data        );

  //
  // Single exit point for function.
  //
  do_exit:
  if (error == 1)
  {
    return NULL;
  }
  else if (error == 2)
  {
    return PyErr_NoMemory();
  }
  else
  {
    return po_covars;
  }
}

/*******************************************************************************
K_MEANS_MIXTURE_ESTIMATE
Compute covariance matrices and mixture weights based on the means and cluster
memberships resulting from a k-means run.
AG
*******************************************************************************/
int k_means_mixture_estimate(double **data, double **means, double ***covars, 
                             double *weights, int *clust_label, 
                             int num_data, int num_dims, int K)

{
  int i, j, j2, k;
  double **covar_k, *mean_k, *data_i;

  /* set up parameters for summing */
  set_dvec(weights, K, 0.0);
  for (k = 1; k <= K; k++)
    set_dmat(covars[k], num_dims, num_dims, 0.0);

  for (i = 1; i <= num_data; i++)
  {
    k = clust_label[i];
        
    /* get the parameters corresponding to the class of this datum */
    covar_k = covars[k];
    mean_k  = means[k];

    /* estimate mixture weights given members of k-means clusters */
    /* add the contribution of this datum to its class */
    weights[k]++;
        
    /* estimate initial covariance matrices given members of k-means 
       clusters */
    /* add the square of the deviation vectors to the right position in the
       covariance */
    data_i = data[i];
    for (j = 1; j <= num_dims; j++)
    {
      for (j2 = j; j2 <= num_dims; j2++)
        covar_k[j][j2] += (data_i[j] - mean_k[j]) * (data_i[j2] - mean_k[j2]);
    }
  }

  /* normalize the covariance matrices by the number of data */
  for (k = 1; k <= K; k++)
    scalar_div_dmat(covars[k], num_dims, num_dims, weights[k] - 1.0);

  /* normalize weights by number of data */
  scalar_div_dvec(weights, K, num_data);

  /* fill in the lower triangle of each covariance matrix based on upper 
     triangle */
  for (k = 1; k <= K; k++)
  {
    covar_k = covars[k];
    for (j=1; j<=num_dims; j++)
      for (j2 = 1; j2 < j; j2++)
        covar_k[j][j2] = covar_k[j2][j];
  }

  return (UT_OK);
}

/**
 * (covars, weights) = 
 * covariance_and_weights_estimate(data, means, clust_labels)
 *
 * Compute an estimate of the covariance matrix and cluster weights from 
 * cluster means and labels of a k means run.
 *
 * Where:
 *   - data is a M-by-N matrix containing N-dimensional datapoints (one per
 *     row) to use when computing the estimates.
 *
 *   - means is a k-by-N mattrix containing the means (one per row) of the
 *     k-Gaussian models.
 *
 *   - clust_labels is
 *
 *   - covars is a 3D matrix composed of k, N-by-N covariance matricies.
 *
 *   - weights is a 1-by-k vector of model weights.
 *
 * C Function prototype
 * int k_means_mixture_estimate(
 *    double **data,        [in]
 *    double **means,       [in]
 *    double ***covars,     [out]
 *    double *weights,      [out]
 *    int    *clust_label,  [in]
 *    int    num_data,      [in]
 *    int    num_dims,      [in]
 *    int    K              [in]
 * )
 *
 *
 */
static PyObject *
DA_covar_weights_estimate(PyObject *self, PyObject *args)
{
  // function parameters
  PyObject *po_data        = NULL;
  PyObject *po_means       = NULL;
  PyObject *po_clust_label = NULL;

  // C versions of parameters
  double **data            = NULL;
  double **means           = NULL;
  int    *clust_label      = NULL;

  // information computed from parameters
  int k = 0;
  int M = 0;
  int N = 0;

  // internal status information
  int status      = 0;
  int error       = 0;
  int num_rows    = 0;
  int num_cols    = 0;
  int i           = 0;

  // results of wrapped function
  double ***covars         = NULL;
  double *weights          = NULL;

  // python version of results
  PyObject *po_covars       = NULL;
  PyObject *po_weights      = NULL;
  PyObject *po_result_tuple = NULL;

  //// Interpret paraters passed to the python end of the functio
  //
  // Parse the arguments to get the matricies as Python objects
  //
  status = PyArg_ParseTuple( args, "OOO", &po_data, &po_means, &po_clust_label);
  if (status == 0)
  {
    error = 1;
    goto do_exit;
  }

  //
  // Convert the python object po_data to a C 2D 
  // M-by-N double matrix (double **)
  //
  data = DAArray_AsDouble2D( &po_data, &num_rows, &num_cols);
  if (data == NULL)
  {
    error = 1;
    goto do_exit;
  }

  M = num_rows;
  N = num_cols;

  //
  // Convert the python object po_means to a C 2D 
  // k-by-N double matrix (double **)
  //
  means = DAArray_AsDouble2D( &po_means, &num_rows, &num_cols );
  if (means == NULL)
  {
    error = 1;
    goto do_free_data;
  }
  else if (num_cols != N)
  {
    error = 1;
    PyErr_SetString( PyExc_ValueError,
		     "Data and means must have the same number of dimensions "
		     "(matrix columns)." );
    // we've initalized but with an invalid condition
    goto do_free_means;
  }

  k = num_rows;

  // 
  // Convert po_clust_label to an M double vector (double *)
  //
  clust_label = DAArray_AsInt1D( &po_clust_label , &num_rows );

  if (clust_label == NULL)
  {
    error = 1;
    goto do_free_means;
  }
  else if (M != num_rows)
  {
    error = 1;
    PyErr_SetString (PyExc_ValueError,
		     "The number of class labels does not match the "
		     "number of data vectors." );
    // we've initalized but with an invalid condition
    goto do_free_clust_label; 
  }

  // The classes in clust_labels are 0 based (from python) 
  // and need to be 1 for the da libraries to work correctly
  for( i = 1; i <= num_rows; i++)
  {
    clust_label[i]++;
  }

  ////// Construct C objects to receive results.
  // Construct covariance 3D matrix
  covars = DAArray_NewDouble3D( &po_covars, k, N, N);

  if (covars == NULL)
  {
    error = 2;
    goto do_free_clust_label;
  }

  // Construct weights vector
  weights = DAArray_NewDouble1D( &po_weights, k);
  
  if (weights == NULL)
  {
    error = 2;
    goto do_free_covars;
  }

  /////// Compute covariance matrix
  // 1. Estimate the full covariance matrix.
  status = k_means_mixture_estimate(data,
				    means,
				    covars,
				    weights, 
				    clust_label,
				    M,
				    N,
				    k);

 ////// Cleanup and termination
 do_free_weights:        DAArray_Free( po_weights     , weights     );
 do_free_covars:         DAArray_Free( po_covars      , covars      );
 do_free_clust_label:    DAArray_Free( po_clust_label , clust_label );
 do_free_means:          DAArray_Free( po_means       , means       );
 do_free_data:           DAArray_Free( po_data        , data        );

  //
  // Single exit point for function.
  //
  do_exit:
  if (error == 1)
  {
    return NULL;
  }
  else if (error == 2)
  {
    return PyErr_NoMemory();
  }
  else
  {
    po_result_tuple = PyTuple_New(2);
    if ( po_result_tuple != NULL )
    {
      PyTuple_SetItem(po_result_tuple, 0, po_covars);
      PyTuple_SetItem(po_result_tuple, 1, po_weights);
      return po_result_tuple;
    }
  }
  return NULL;
}

/**
 * boolean = DA_fix_covariance_matrix(Covariance_matrix, num_rows, num_cols)
 *
 * Check to make sure that a matrix is positive definite,
 * if it is not, convert it to a diagonal covariance matrix.
 *
 */
static PyObject *
DA_fix_covariance_matrix(PyObject *self, PyObject *args)
{
  // function parameters
  PyObject *po_covar       = NULL;

  // C versions of parameters
  double **covar           = NULL;
  
  // internal status information
  int status      = 0;
  int error       = 0;
  int num_rows    = 0;
  int num_cols    = 0;
  
  //// Interpret paraters passed to the python end of the functio
  //
  // Parse the arguments to get the matricies as Python objects
  //
  status = PyArg_ParseTuple( args, "O", &po_covar);
  if (status == 0)
  {
    error = 1;
    goto do_exit;
  }
  
  //
  // Convert the python object po_data to a C 2D 
  // M-by-N double matrix (double **)
  //
  covar = DAArray_AsDouble2D( &po_covar, &num_rows, &num_cols);
  
  if (covar == NULL)
  {
    error = 1;
    goto do_exit;
  }
  
  
  status = fix_covariance_matrix(covar, num_rows, num_cols);
  
 do_free_covar:      DAArray_Free( po_covar        , covar        );
 do_exit:
  if (error == 1)
  {
    return NULL;
  }
  else if (error == 2)
  {
    return PyErr_NoMemory();
  }
  else
  {
    return PyInt_FromLong(status);
  }
}

static PyObject *
DA_is_posdef_dmat(PyObject *self, PyObject *args)
{
  int      dim1;
  int      dim2;
  int      status;
  int      is_posdef = FALSE;
  double   **matrix;
  PyObject *po_matrix;

  status = PyArg_ParseTuple( args, "O", &po_matrix );

  if (status != 0)
  {
    matrix = DAArray_AsDouble2D( &po_matrix, &dim1, &dim2 );
    
    posdef_sym_alloc_dmat( matrix, dim1, &is_posdef );
  }

  return  PyInt_FromLong(is_posdef);
}

/**
 * Given a matrix delete everything not on the diagonal
 */
static PyObject *
DA_make_diag_matrix(PyObject *self, PyObject *args)
{
  int      dim1;
  int      dim2;
  int      status;
  double   **matrix;
  PyObject *po_matrix;

  status = PyArg_ParseTuple( args, "O", &po_matrix );

  // Check shape of matrix?
  if (status != 0)
  {
    matrix = DAArray_AsDouble2D( &po_matrix, &dim1, &dim2 );
    
    make_diag_matrix(matrix, dim1, dim2);
  }

  Py_INCREF(Py_None);
  return Py_None;
}


/**
Compute the conditional probability of the datum assuming it was drawn from a 
multivariate normal distribution with the specified mean and covariance matrix,
using the normal probability density function.  Returns the vector of proba-
bilities, one for each datum.
 */
static PyObject *
DA_vector_prob_gauss(PyObject *self, PyObject *args)
{
  // Parameters
  PyObject *po_data      = NULL;
  PyObject *po_means     = NULL;
  PyObject *po_covars    = NULL;

  double **data     = NULL;
  double *means    = NULL;
  double **covars  = NULL;

  double min_diag   = -1.0;

  // Return value
  PyObject *po_probs = NULL;
  double *probs      = NULL;

  int M = 0;
  int N = 0;

  // Error messages
  char *format  = NULL;
  char *message = NULL;

  // scratch space
  int i        = 0;
  int num_rows = 0;
  int num_cols = 0;
  int error    = 0;
  int size     = 0;
  int status   = 0;

  //
  // Parse args to get matrices as Python objects and min_diag as a C
  // double.
  //
  status = PyArg_ParseTuple( args,
                             "OOO|d",
                             &po_data,
                             &po_means,
                             &po_covars,
                             &min_diag );

  if (status == 0)
  {
    error = 1;
    goto do_exit;
  }
  

  //
  // Convert the Python object po_data to a C 2D double matrix (double **).
  //
  data = DAArray_AsDouble2D( &po_data, &num_rows, &num_cols );

  if (data == NULL)
  {
    error = 1;
    goto do_exit;
  }

  M = num_rows;
  N = num_cols;


  //
  // Convert the Python object po_means to a C 2D double matrix
  // (double **).
  //
  means = DAArray_AsDouble1D( &po_means, &num_rows );

  if (means == NULL)
  {
    error = 1;
    goto do_free_data;
  }

  //
  // Convert the Python object po_covars to a C 3D double matrix
  // (double ***).
  //
  covars = DAArray_AsDouble2D( &po_covars, &num_rows, &num_cols );

  if (covars == NULL)
  {
    error = 1;
    goto do_free_means;
  }
  else if (num_rows != num_cols)
  {
    error = 1;
    PyErr_SetString( PyExc_ValueError,
                     "Each covariance matrix must be square." );
    goto do_free_covars;
  }

  //
  // Test each covariance matrix for symmetry.
  //
  status = DAArray_Is2DSymmetric( covars, num_rows );
  
  if (status == 0)
  {
     error = 1;
     
     format  = "Covariance matrix %d is not symmetric.\n";
     size    = strlen(format) + 2;
     message = (char *) PyMem_Malloc(size);
     
     if (message == NULL)
     {
        error = 2;
     }
     else
     {
        snprintf(message, size, format, i);
        PyErr_SetString( PyExc_ValueError, message );
     }
  }


  //
  // Test each covariance matrix for postive definiteness.
  //
  posdef_sym_alloc_dmat( covars, num_rows, &status );

  if (status == FALSE)
  {
     error = 1;
     
     format  = "Covariance matrix %d is not positive definite.\n";
     size    = strlen(format) + 2;
     message = (char *) PyMem_Malloc(size);
     
     if (message == NULL)
     {
        error = 2;
     }
     else
     {
        snprintf(message, size, format, i);
        PyErr_SetString( PyExc_ValueError, message );
     }
  }

  if (error == 1)
  {
    goto do_free_means;
  }

  //
  // If not set, assign min_diag a default value.
  //
  if (min_diag == -1.0)
  {
    min_diag = 1.0e-6;
  }

  //
  // Allocate intermediate storage for mixture_likelihood, probs.
  //
  probs = DAArray_NewDouble1D( &po_probs, M );

  if (probs == NULL)
  {
    error = 2;
    goto do_free_probs;
  }

  //
  // Actual call to da_cluster::mixture_likelihood().
  //
  vector_prob_gauss(data, means, covars, N, M, probs, min_diag, "rows");

  //
  // The labels and gotos are necessary to properly "unwind" the "stack"
  // of mallocs, given that there are multiple points of possible failure
  // above.
  //

  do_free_probs:     DAArray_Free( po_probs    , probs     );
  do_free_covars:    DAArray_Free( po_covars   , covars    );
  do_free_means:     DAArray_Free( po_means    , means     );
  do_free_data:      DAArray_Free( po_data     , data      );

  //
  // Single exit point for function.
  //
  do_exit:
  if (error == 1)
  {
    return NULL;
  }
  else if (error == 2)
  {
    return PyErr_NoMemory();
  }
  else
  {
     return po_probs;
  }
}


/**
 *
 */
static PyObject *
DA_TestAsDouble1D(PyObject *self, PyObject *args)
{
  int      dim1;
  int      status;
  double   *matrix;
  PyObject *object;


  status = PyArg_ParseTuple( args, "O", &object );

  if (status != 0)
  {
    matrix = DAArray_AsDouble1D( &object, &dim1 );

    if (matrix != NULL)
    {
      DAArray_PrintDouble1D( matrix, dim1, 0 );
    }
  }

  Py_INCREF(Py_None);
  return Py_None;
}


/**
 *
 */
static PyObject *
DA_TestAsDouble2D(PyObject *self, PyObject *args)
{
  int      dim1;
  int      dim2;
  int      status;
  double   **matrix;
  PyObject *object;


  status = PyArg_ParseTuple( args, "O", &object );

  if (status != 0)
  {
    matrix = DAArray_AsDouble2D( &object, &dim1, &dim2 );

    if (matrix != NULL)
    {
      DAArray_PrintDouble2D( matrix, dim1, dim2, 0 );
    }
  }

  Py_INCREF(Py_None);
  return Py_None;
}


/**
 *
 */
static PyObject *
DA_TestAsDouble3D(PyObject *self, PyObject *args)
{
  int      dim1;
  int      dim2;
  int      dim3;
  int      status;
  double   ***matrix;
  PyObject *object;


  status = PyArg_ParseTuple( args, "O", &object );

  if (status != 0)
  {
    matrix = DAArray_AsDouble3D( &object, &dim1, &dim2, &dim3 );

    if (matrix != NULL)
    {
      DAArray_PrintDouble3D( matrix, dim1, dim2, dim3, 0 );
    }
  }

  Py_INCREF(Py_None);
  return Py_None;
}



/**
 *
 */
double *
DAArray_AsDouble1D(PyObject **object, int *dim1)
{
  void          *matrix1D = NULL;
  PyArrayObject *array    = NULL;


  array = (PyArrayObject *)
          PyArray_ContiguousFromObject( *object, PyArray_DOUBLE, 1, 1 );

  if (array != NULL)
  {
    *object  = (PyObject *) array;
    *dim1    = array->dimensions[0];
    matrix1D = array->data - array->strides[0];
  }

  return (double *) matrix1D;
}


/**
 *
 */
double **
DAArray_AsDouble2D(PyObject **object, int *dim1, int *dim2)
{
  int i    = 0;
  int size = 0;

  PYARRAY_DATATYPE *base     = NULL;
  void          **matrix2D = NULL;
  PyArrayObject  *array    = NULL;


  array = (PyArrayObject *)
          PyArray_ContiguousFromObject( *object, PyArray_DOUBLE, 2, 2 );

  if (array != NULL)
  {
    size     = array->dimensions[0] * sizeof(void *);
    matrix2D = (void **) PyMem_Malloc( size );

    if (matrix2D != NULL)
    {
      *object = (PyObject *) array;
      *dim1   = array->dimensions[0];
      *dim2   = array->dimensions[1];

      base = array->data - array->strides[1];

      for (i = 0; i < array->dimensions[0]; i++)
      {
        matrix2D[i] = base + (i * array->strides[0]);
      }

      matrix2D--;
    }
    else
    {
      Py_DECREF( array );
    }
  }

  return (double **) matrix2D;
}


/**
 *
 */
double ***
DAArray_AsDouble3D(PyObject **object, int *dim1, int *dim2, int *dim3)
{
  int i    = 0;
  int size = 0;

  PYARRAY_DATATYPE *base     = NULL;
  void  **base2D   = NULL;
  void  **matrix2D = NULL;
  void ***matrix3D = NULL;

  PyArrayObject *array = NULL;


  array = (PyArrayObject *)
          PyArray_ContiguousFromObject( *object, PyArray_DOUBLE, 3, 3 );

  if (array != NULL)
  {
    size     = array->dimensions[0] * array->dimensions[1] * sizeof(void *);
    matrix2D = (void **) PyMem_Malloc( size );

    if (matrix2D != NULL)
    {
      size     = array->dimensions[0] * sizeof(void **);
      matrix3D = (void ***) PyMem_Malloc( size );

      if (matrix3D != NULL)
      {
        *object = (PyObject *) array;
        *dim1   = array->dimensions[0];
        *dim2   = array->dimensions[1];
        *dim3   = array->dimensions[2];

        base2D = matrix2D - 1;

        for (i = 0; i < array->dimensions[0]; i++)
        {
          matrix3D[i] = base2D + (i * array->dimensions[1]);
        }

        base = array->data - array->strides[2];
        size = array->dimensions[0] * array->dimensions[1];

        for (i = 0; i < size; i++)
        {
          matrix2D[i] = base + (i * array->strides[1]);
        }

        matrix3D--;
      }
      else
      {
        PyMem_Free( matrix2D );
        Py_DECREF( array );
      }
    }
    else
    {
      Py_DECREF( array );
    }
  }

  return (double ***) matrix3D;
}


/**
 *
 */
void
DAArray_Free(PyObject *object, void *ptr)
{
  double  **matrix2D;
  double ***matrix3D;

  PyArrayObject *array = (PyArrayObject *) object;


  if (array->nd == 3)
  {
    matrix3D = (double ***) ptr;
    PyMem_Free( &matrix3D[1][1] );
    PyMem_Free( &matrix3D[1]    );
  }
  else if (array->nd == 2)
  {
    matrix2D = (double **) ptr;
    PyMem_Free( &matrix2D[1] );
  }

  Py_DECREF(array);
}


/**
 *
 */
int
DAArray_Is2DSymmetric(double **matrix, int dim)
{
  int i, j;
  int result = 1;


  for (i = 1; i <= dim; i++)
  {
    for (j = i + 1; j <= dim; j++)
    {
      if (matrix[i][j] != matrix[j][i])
      {
        result = 0;
        break;
      }
    }

    if (result == 0)
    {
      break;
    }
  }

  return result;
}


/**
 *
 */
double *
DAArray_NewDouble1D(PyObject **object, int dim1)
{
  int      dims[1];
  double   *matrix1D = NULL;
  PyObject *result   = NULL;


  dims[0] = dim1;
  result  = PyArray_FromDims( 1, dims, PyArray_DOUBLE );

  if (result != NULL)
  {
    *object  = result;
    matrix1D = DAArray_AsDouble1D( object, &dims[0] );
  }

  return matrix1D;
}


/**
 *
 */
double **
DAArray_NewDouble2D(PyObject **object, int dim1, int dim2)
{
  int      dims[2];
  double   **matrix2D = NULL;
  PyObject  *result   = NULL;


  dims[0] = dim1;
  dims[1] = dim2;
  result  = PyArray_FromDims( 2, dims, PyArray_DOUBLE );

  if (result != NULL)
  {
    *object  = result;
    matrix2D = DAArray_AsDouble2D( object, &dims[0], &dims[1]  );
  }

  return matrix2D;
}

/**
 *
 */
double ***
DAArray_NewDouble3D(PyObject **object, int dim1, int dim2, int dim3)
{
  int      dims[3];
  double   ***matrix3D = NULL;
  PyObject   *result   = NULL;

  dims[0] = dim1;
  dims[1] = dim2;
  dims[2] = dim3;

  result  = PyArray_FromDims( 3, dims, PyArray_DOUBLE );

  if (result != NULL)
  {
    *object  = result;
    matrix3D = DAArray_AsDouble3D( object, &dims[0], &dims[1], &dims[2]  );
  }

  return matrix3D;
}

/**
 *
 */
void
DAArray_PrintDouble1D(double *matrix, int dim1, int indent)
{
  int i;


  for (i = 0; i < indent; i++)
  {
    printf(" ");
  }

  printf("[");

  for (i = 1; i <= dim1; i++)
  {
    printf("\t%g", matrix[i]);
  }

  printf("\t]\n");
}


/**
 *
 */
void
DAArray_PrintDouble2D(double **matrix, int dim1, int dim2, int indent)
{
  int i;


  for (i = 0; i < indent; i++)
  {
    printf(" ");
  }

  printf("[\n");

  for (i = 1; i <= dim1; i++)
  {
    DAArray_PrintDouble1D( matrix[i], dim2, indent + 2 );
  }

  for (i = 0; i < indent; i++)
  {
    printf(" ");
  }

  printf("]\n");
}


/**
 *
 */
void
DAArray_PrintDouble3D(double ***matrix, int dim1, int dim2, int dim3, int indent)
{
  int i;


  for (i = 0; i < indent; i++)
  {
    printf(" ");
  }

  printf("[\n");

  for (i = 1; i <= dim1; i++)
  {
    DAArray_PrintDouble2D( matrix[i], dim2, dim3, indent + 2 );
  }

  for (i = 0; i < indent; i++)
  {
    printf(" ");
  }

  printf("]\n");
}

/******************************
 *    Int Array Functions     *
 ******************************/

/**
 *
 */
int *
DAArray_AsInt1D(PyObject **object, int *dim1)
{
  void          *matrix1D = NULL;
  PyArrayObject *array    = NULL;


  array = (PyArrayObject *)
          PyArray_ContiguousFromObject( *object, PyArray_INT, 1, 1 );

  if (array != NULL)
  {
    *object  = (PyObject *) array;
    *dim1    = array->dimensions[0];
    matrix1D = array->data - array->strides[0];
  }

  return (int *) matrix1D;
}


/**
 *
 */
int **
DAArray_AsInt2D(PyObject **object, int *dim1, int *dim2)
{
  int i    = 0;
  int size = 0;

  PYARRAY_DATATYPE *base     = NULL;
  void          **matrix2D = NULL;
  PyArrayObject  *array    = NULL;


  array = (PyArrayObject *)
          PyArray_ContiguousFromObject( *object, PyArray_INT, 2, 2 );

  if (array != NULL)
  {
    size     = array->dimensions[0] * sizeof(void *);
    matrix2D = (void **) PyMem_Malloc( size );

    if (matrix2D != NULL)
    {
      *object = (PyObject *) array;
      *dim1   = array->dimensions[0];
      *dim2   = array->dimensions[1];

      base = array->data - array->strides[1];

      for (i = 0; i < array->dimensions[0]; i++)
      {
        matrix2D[i] = base + (i * array->strides[0]);
      }

      matrix2D--;
    }
    else
    {
      Py_DECREF( array );
    }
  }

  return (int **) matrix2D;
}


/**
 *
 */
int ***
DAArray_AsInt3D(PyObject **object, int *dim1, int *dim2, int *dim3)
{
  int i    = 0;
  int size = 0;

  PYARRAY_DATATYPE *base     = NULL;
  void  **base2D   = NULL;
  void  **matrix2D = NULL;
  void ***matrix3D = NULL;

  PyArrayObject *array = NULL;


  array = (PyArrayObject *)
          PyArray_ContiguousFromObject( *object, PyArray_INT, 3, 3 );

  if (array != NULL)
  {
    size     = array->dimensions[0] * array->dimensions[1] * sizeof(void *);
    matrix2D = (void **) PyMem_Malloc( size );

    if (matrix2D != NULL)
    {
      size     = array->dimensions[0] * sizeof(void **);
      matrix3D = (void ***) PyMem_Malloc( size );

      if (matrix3D != NULL)
      {
        *object = (PyObject *) array;
        *dim1   = array->dimensions[0];
        *dim2   = array->dimensions[1];
        *dim3   = array->dimensions[2];

        base2D = matrix2D - 1;

        for (i = 0; i < array->dimensions[0]; i++)
        {
          matrix3D[i] = base2D + (i * array->dimensions[1]);
        }

        base = array->data - array->strides[2];
        size = array->dimensions[0] * array->dimensions[1];

        for (i = 0; i < size; i++)
        {
          matrix2D[i] = base + (i * array->strides[1]);
        }

        matrix3D--;
      }
      else
      {
        PyMem_Free( matrix2D );
        Py_DECREF( array );
      }
    }
    else
    {
      Py_DECREF( array );
    }
  }

  return (int ***) matrix3D;
}

/**
 *
 */
void
DAArray_PrintInt1D(int *matrix, int dim1, int indent)
{
  int i;


  for (i = 0; i < indent; i++)
  {
    printf(" ");
  }

  printf("[");

  for (i = 1; i <= dim1; i++)
  {
    printf("\t%d", matrix[i]);
  }

  printf("\t]\n");
}


/**
 *
 */
void
DAArray_PrintInt2D(int **matrix, int dim1, int dim2, int indent)
{
  int i;


  for (i = 0; i < indent; i++)
  {
    printf(" ");
  }

  printf("[\n");

  for (i = 1; i <= dim1; i++)
  {
    DAArray_PrintInt1D( matrix[i], dim2, indent + 2 );
  }

  for (i = 0; i < indent; i++)
  {
    printf(" ");
  }

  printf("]\n");
}


/**
 *
 */
void
DAArray_PrintInt3D(int ***matrix, int dim1, int dim2, int dim3, int indent)
{
  int i;


  for (i = 0; i < indent; i++)
  {
    printf(" ");
  }

  printf("[\n");

  for (i = 1; i <= dim1; i++)
  {
    DAArray_PrintInt2D( matrix[i], dim2, dim3, indent + 2 );
  }

  for (i = 0; i < indent; i++)
  {
    printf(" ");
  }

  printf("]\n");
}

/*********************************************
 *  Covariance matrix manipulation routines  *
 *   (should probably exist somewhere )      *
 *   ( in the da library              )      *
 *********************************************/

int est_covar_matrices(double ***covars,
		       double  **data,
		       double  **means,
		       int      *clust_label,
		       int       num_data,
		       int       num_dims, 
		       int       K)

{
  int i, j, j2, k;
  double **covar_k, *mean_k, *data_i;

  /* set up parameters for summing */

  for (i = 1; i <= num_data; i++)
  {
    k = clust_label[i];
        
    /* get the parameters corresponding to the class of this datum */
    covar_k = covars[k];
    mean_k  = means[k];

    /* estimate initial covariance matrices given members of k-means 
       clusters */
    /* add the square of the deviation vectors to the right position in the
       covariance */
    for (j = 1; j <= num_dims; j++)
    {
      data_i = data[i];
      for (j2 = j; j2 <= num_dims; j2++)
        covar_k[j][j2] += (data_i[j] - mean_k[j]) * (data_i[j2] - mean_k[j2]);
    }
  }

  /* normalize the covariance matrices by the number of data */
  for (k = 1; k <= K; k++)
    scalar_div_dmat(covars[k], num_dims, num_dims, num_data);

  /* fill in the lower triangle of each covariance matrix based on upper 
     triangle */
  for (k = 1; k <= K; k++)
  {
    covar_k = covars[k];
    for (j=1; j<=num_dims; j++)
      for (j2 = 1; j2 < j; j2++)
        covar_k[j][j2] = covar_k[j2][j];
  }

  return (UT_OK);
}

void make_diag_matrix(double **matrix, int num_rows, int num_cols)
{
  int i;
  int j;
  for(i=1; i <= num_rows; i++) // num rows
  {
    for( j = 1; j <= num_cols; j++ ) // num_columns
    {
      if( i != j)
      {
	matrix[i][j] = 0.0;
      }
    }
  }
}

int fix_covariance_matrix(double **covar, int num_rows, int num_cols)
{
  int status;
  int covar_posdef = TRUE;

  posdef_sym_alloc_dmat( covar, num_rows, &status );

  // if not positive definite, convert to a diagonal covariance matrix
  if (status == FALSE)
  {
    covar_posdef = FALSE;
    make_diag_matrix(covar, num_rows, num_cols);
  }
  return covar_posdef;
}
