/*
   File:        gaussian.c
   Author:      Andrew W. Moore
   Created:     Sun Apr  5 11:49:13 EDT 1998
   Description: Representing a set of clustering gaussian

   Copyright 1998, Carnegie Mellon University
*/

#include "gaussian.h"
#include "bayesreg.h"

bool gaussian_is_diag(gaussian *gs)
{
  return gs->diag;
}

bool gaussian_is_ill_defined(gaussian *gs)
{
  bool result = dyv_is_ill_defined(gs->mean);

  if ( !result )
    result = (gaussian_is_diag(gs)) ?
             dyv_is_ill_defined(gs->cov_diag) : dym_is_ill_defined(gs->cov);
  return result;
}

void check_gaussian_ok(gaussian *gs)
{
  if ( gs->diag )
  {
    if ( gs->cov != NULL ) my_error("Gauss cov should be NULL for diag");
    if ( gs->cov_inv != NULL ) my_error("Gss cov_inv should be NULL for diag");
    if ( gs->cov_diag == NULL ) my_error("Gauss cov_diag should be "
					 "non-null for diag");
  }
  else
  {
    if ( gs->cov == NULL ) my_error("Gauss cov should be nonNULL for nondiag");
    if ( gs->cov_diag != NULL ) my_error("Gauss cov_diag should be "
					 "null for non-diag");
  }
}

gaussian *mk_empty_gaussian(bool diag,int dim)
{
  gaussian *gs = AM_MALLOC(gaussian);

  gs -> diag = diag;
  gs -> mean = mk_zero_dyv(dim);
  gs -> cov_diag = NULL;
  gs -> cov = NULL;
  gs -> cov_inv = NULL;

  if ( diag )
    gs -> cov_diag = mk_zero_dyv(dim);
  else
  {
    gs -> cov = mk_zero_dym(dim,dim);
    gs -> cov_inv = NULL;
  }

  gs -> cov_det_defined = FALSE;
  gs -> cov_det = -77.77;

  check_gaussian_ok(gs);
  return gs;
}

void free_gaussian(gaussian *gs)
{
  check_gaussian_ok(gs);
  free_dyv(gs->mean);
  if ( gs->cov != NULL ) free_dym(gs->cov);
  if ( gs->cov_diag != NULL ) free_dyv(gs->cov_diag);
  if ( gs->cov_inv != NULL ) free_dym(gs->cov_inv);
  AM_FREE(gs,gaussian);
}

/* Updates the gaussian's mean and covariance. Arguments are COPIED in */
void update_full_gaussian(gaussian *gs,dyv *mean,dym *cov)
{
  if ( gaussian_is_diag(gs) )
    my_error("update_full_gaussian: you gave me a diagonal gs!");

  check_gaussian_ok(gs);
  copy_dyv(mean,gs->mean);
  copy_dym(cov,gs->cov);
  if ( gs->cov_inv != NULL )
  {
    free_dym(gs->cov_inv);
    gs -> cov_inv = NULL;
  }
  gs -> cov_det_defined = FALSE;
  gs -> cov_det = -777.77;
  check_gaussian_ok(gs);
}

/* Updates the gaussian's mean and covariance. Arguments are COPIED in */
void update_diag_gaussian(gaussian *gs,dyv *mean,dyv *cov)
{
  if ( !gaussian_is_diag(gs) )
    my_error("update_diag_gaussian: you gave me a gs with full covariance!");

  check_gaussian_ok(gs);
  copy_dyv(mean,gs->mean);
  copy_dyv(cov,gs->cov_diag);
  gs -> cov_det_defined = FALSE;
  gs -> cov_det = -777.77;
  check_gaussian_ok(gs);
}

gaussian *mk_full_gaussian(dyv *mean,dym *cov)
{
  int num_dims = dyv_size(mean);
  gaussian *gs = mk_empty_gaussian(FALSE,num_dims);
  update_full_gaussian(gs,mean,cov);
  return gs;
}

gaussian *mk_diag_gaussian(dyv *mean,dyv *cov)
{
  int num_dims = dyv_size(mean);
  gaussian *gs = mk_empty_gaussian(TRUE,num_dims);
  update_diag_gaussian(gs,mean,cov);
  return gs;
}

dym *mk_invert_spd_robustly(const dym *x)
{
  dym *inv = mk_invert_spd_cholesky(x);
  if ( inv == NULL )
  {
    int size = dym_rows(x);
    int i;
    dym *x2 = mk_zero_dym(size,size);
    fprintf(stderr, "Warning: Having trouble inverting a supposedly SPD Matrix. Will cheat\n");
    if (Verbosity >= 0) fprintf_dym(stderr, "Evil-Matrix",x,"\n");


    for ( i = 0 ; i < size ; i++ )
      dym_set(x2,i,i,fabs(dym_ref(x,i,i)));
    inv = mk_invert_spd_cholesky(x2);
    if ( inv == NULL )
    {
      double d = real_max(1e-10,dym_max(x2)) / 10.0;
      for ( i = 0 ; i < size ; i++ )
        dym_set(x2,i,i,real_max(d,dym_ref(x2,i,i)));
      inv = mk_invert_spd_cholesky(x2);
      if ( inv == NULL ) my_error("mk_invert_spd_cholesky: I give up!");
    }
    free_dym(x2);
  }
  return inv;
}
    

dym *private_cov_inv(gaussian *gs)
{
  if ( gs -> cov_inv == NULL )
    gs -> cov_inv = mk_invert_spd_robustly(gs->cov);
  return gs->cov_inv;
}

/*
  Note...

    det(cov) = product_j cov[j]

   so

    log(det(cov)) = sum_j log cov[j]
*/
double diag_log_cov_det(dyv *cov)
{
  double sum = 0.0;
  int i;
  for ( i = 0 ; i < dyv_size(cov) ; i++ )
  {
    double cov_i = dyv_ref(cov,i);
    if ( cov_i <= 0.0 )
      my_error("diag_log_cov_det: univariance covariance <= 0.0");
    sum += log(cov_i);
  }
  return sum;
}

double private_cov_det(gaussian *gs)
{
  if ( !gs -> cov_det_defined )
  {
    gs -> cov_det = 
      ( gaussian_is_diag(gs) ) ? 
      exp(diag_log_cov_det(gs->cov_diag)) : dym_determinant(gs->cov);
    gs -> cov_det_defined = TRUE;
  }
  return gs -> cov_det;
}

void copy_gaussian(gaussian *src,gaussian *dest)
{
  if ( gaussian_is_diag(src) )
    update_diag_gaussian(dest,src->mean,src->cov_diag);
  else
    update_full_gaussian(dest,src->mean,src->cov);
}

gaussian *mk_copy_gaussian(gaussian *gs)
{
  gaussian *ngs = mk_empty_gaussian(gaussian_is_diag(gs),
				    gaussian_num_dims(gs));
  copy_gaussian(gs,ngs);
  return(ngs);
}

int gaussian_num_dims(gaussian *gs)
{
  return dyv_size(gs->mean);
}

#define MAX_USEFUL 400.0

int Cached_num_dims = 0;
double Cached_rtth = 1.0;

double root_two_pi_to_num_dims(int num_dims)
{
  if ( num_dims != Cached_num_dims )
  {
    Cached_num_dims = num_dims;
    Cached_rtth = pow(ROOT_TWO_PI,(double)num_dims);
  }
  return Cached_rtth;
}
/* Assume we are evaluating the pdf of a Gaussian at point x.
   Assume mean Mu and covariance Cov.

   Then, provided mahalanobis_sqd = (x - MU)^T Cov^-1 (x - MU)
         and provided               cov_det = Determinant of COV

   Then this function returns that pdf value.
*/
double eval_full_gauss_from_mah_and_covdet(double mahalanobis_sqd,
                                            double cov_det,
                                            int num_dims)
{
  double numerator = (mahalanobis_sqd/2 > MAX_USEFUL) ? 0.0 :
                     exp(-mahalanobis_sqd/2);
  double denominator = root_two_pi_to_num_dims(num_dims) * sqrt(cov_det);
  double result = numerator / real_max(1e-40,denominator);
#ifndef AMFAST
  if ( is_ill_defined(result) ) my_error("gauss_eval result is_ill_defined");
#endif
  return result;
}

/* Assume we are evaluating the pdf of a Gaussian at point x.
   Assume gs has mean Mu and covariance Cov.

   Then, provided mahalanobis_sqd = (x - MU)^T Cov^-1 (x - MU)

   Then this function returns that pdf value.
*/
double eval_full_gauss_from_mah(gaussian *gs,double mahalanobis_sqd)
{
  double cov_det = private_cov_det(gs);
  int num_dims = gaussian_num_dims(gs);
#ifndef AMFAST
  if ( is_ill_defined(cov_det) ) my_error("gauss_eval cov_det is_ill_defined");
#endif

  return eval_full_gauss_from_mah_and_covdet(mahalanobis_sqd,cov_det,num_dims);
}

/* Returns x^T a x */
double eval_mahalanobis_sqd(const dyv *x, const dym *a)
{
  return dym_xt_a_x_value(x,a);
}

/* Evaluates the pdf of the gaussian at point x. If
 r_mahal_dsqd is non-NULL, returns the mahalanbois distance of x from
 the gaussian mean in *r_mahal_dsqd */
double gauss_eval(gaussian *gs,dyv *x,double *r_mahal_dsqd)
{
  dyv *dx = mk_dyv_subtract(x,gs->mean);
  dym *cov_inv = private_cov_inv(gs);
  double ms = eval_mahalanobis_sqd(dx,cov_inv);
  double result = eval_full_gauss_from_mah(gs,ms);
#ifndef AMFAST
  if ( dyv_is_ill_defined(dx) ) my_error("gauss_eval: dx is_ill_defined");
  if ( dym_is_ill_defined(cov_inv) ) my_error("gauss_eval: cov_inv is_ill_defined");
  if ( is_ill_defined(ms) ) my_error("gauss_eval ms is_ill_defined");
  if ( is_ill_defined(result) ) my_error("gauss_eval result is_ill_defined");
#endif
  if ( r_mahal_dsqd != NULL ) *r_mahal_dsqd = ms;
  free_dyv(dx);
  return result;
}


/* What is the maximum value that the gaussian can take?
   I.E. argmax_x P(x|wj,gaussian)

   and this obviously is maximized by putting x at the center of
   the gaussian. */
double gauss_eval_maximum(gaussian *gs)
{
  double mahal_dsqd;
  return gauss_eval(gs,gs->mean,&mahal_dsqd);
}


/* Find x constrained to lie inside or on the surface of hr
   such that x^T a x is maximized. Returns the resulting
   value: x^T a x.

   argmax_x may be NULL in which case it's ignored. Else it
   gets filled up with the x that causes this maximum. */
double constrained_mahalanobis_sqd_max(hrect *hr,dym *a,dyv *argmax_x)
{
  int size = dym_rows(a);
  dyv *b = mk_zero_dyv(size);
  double c = 0.0;
  ivec *fixed_atts = mk_ivec(0);
  dyv *fixed_vals = mk_dyv(0);
  dyv *lo = hr->lo;
  dyv *hi = hr->hi;
  double value;
  dyv *opt_x = mk_constrained_quadopt(a,b,c,fixed_atts,fixed_vals,
                                         lo,hi,&value);

  if ( argmax_x != NULL ) copy_dyv(opt_x,argmax_x);

  free_dyv(b);
  free_ivec(fixed_atts);
  free_dyv(fixed_vals);
  free_dyv(opt_x);

  return 2 * value; /* multiplied by 2 because mk_constrained_quadopt
                       assumes value = 0.5 x^T A x + x^T b + c */
}

/* Find x constrained to lie inside or on the surface of hr
   such that x^T a x is minimized. Returns the resulting
   value: x^T a x

   argmin_x may be NULL in which case it's ignored. Else it
   gets filled up with the x that causes this minimum. */
double constrained_mahalanobis_sqd_min(hrect *hr,dym *a,dyv *argmin_x)
{
  dym *minus_a = mk_dym_scalar_mult(a,-1.0);
  double minus_result = constrained_mahalanobis_sqd_max(hr,minus_a,argmin_x);
  double result = 0.0 - minus_result;
  free_dym(minus_a);
  return result;
}

/* Sets *aloj = argmin_x P(x|wj,gaussian)
        *ahij = argmax_x P(x|wj,gaussian)
   where x is allowed to vary anywhere inside or on the surface of
   the given hyper-rectangle.

   You may pass in *aloj or *ahij as NULL if you don't want both the
   upper and lower limits. That'll save some computation.

   If x_minlik is non-NULL, and aloj is non-NULL, then the point
   inside hr that minimizes the likelihood (i.e. the point furthest in
   mahalanobis distance from mean) will be stored in x_minlik.
   Ditto for x_maxlik.
*/
void gauss_eval_limits(hrect *hr,gaussian *gs,
		       double *alo,double *ahi,
		       dyv *x_minlik,dyv *x_maxlik)
{
  hrect *hr_trans = mk_hrect_subtract_dyv(hr,gs->mean);
  /* The max(min) value of (x-mean)^T cov_inv (x-mean) where x
     is constrained to lie in hr is the same as
     The max(min) value of x^T cov_inv x where x
     is constrained to lie in hr_trans */
  dym *cov_inv = private_cov_inv(gs);
  double cov_det = private_cov_det(gs);
  dyv *argmin_x = NULL;
  dyv *argmax_x = NULL;
  int num_dims = dyv_size(gs->mean);

  if ( ahi != NULL ) argmin_x = mk_dyv(hrect_size(hr));
  if ( alo != NULL ) argmax_x = mk_dyv(hrect_size(hr));

  if ( alo != NULL )
  {
    double mahalanobis_sqd_max = 
      constrained_mahalanobis_sqd_max(hr_trans,cov_inv,argmax_x);
    *alo = eval_full_gauss_from_mah_and_covdet(mahalanobis_sqd_max,cov_det,num_dims);
  }
  
  if ( ahi != NULL )
  {
    double mahalanobis_sqd_min = 
      constrained_mahalanobis_sqd_min(hr_trans,cov_inv,argmin_x);
    *ahi = eval_full_gauss_from_mah_and_covdet(mahalanobis_sqd_min,cov_det,num_dims);
  }

  if ( argmax_x != NULL && x_minlik != NULL )
    dyv_plus(argmax_x,gs->mean,x_minlik);
  if ( argmin_x != NULL && x_maxlik != NULL )
    dyv_plus(argmin_x,gs->mean,x_maxlik);

  if ( argmin_x != NULL ) free_dyv(argmin_x);
  if ( argmax_x != NULL ) free_dyv(argmax_x);
  free_hrect(hr_trans);
}


/* Returns a pointer to the mean vector of the gaussian. Do not
   ever free or update this vector. */
dyv *gaussian_mean(gaussian *gs)
{
  return gs->mean;
}

/* Makes a copy of the covariance matrix of the gaussian. You CANNOT and
   SHOULD NOT TRY to access the internal covariance pointer directly
   since we do not promise that it exists. */
dym *mk_covariance_of_full_gaussian(gaussian *gs)
{
  if ( gs->diag )
    my_error("mk_covariance_of_full_gaussian: gs not full");
  return mk_copy_dym(gs->cov);
}

/* Makes a copy of the covariance matrix of the gaussian. You CANNOT and
   SHOULD NOT TRY to access the internal covariance pointer directly
   since we do not promise that it exists. */
dyv *mk_covariance_of_diag_gaussian(gaussian *gs)
{
  if ( !gs->diag )
    my_error("mk_covariance_of_diag_gaussian: gs not diag");
  return mk_copy_dyv(gs->cov_diag);
}

dyv *mk_sample_from_gaussian(gaussian *gs)
{
  return mk_sample_from_normal(gs->mean,gs->cov);
}

double gaussian_volume(gaussian *gs)
{
  return private_cov_det(gs);
}

/* Test if the covariance is kind of bad in the sense that it may have only
   been some random bits at the bottom of our double representation that
   allows us to invert it.  We test this by considering the smallest
   eigenvalue (which is 0 in a singular matrix) as a ratio to the largest.
*/
bool gaussian_has_not_so_good_covariance(gaussian *gs)
{
  bool result = FALSE;
  dyv *evals = mk_sorted_eigens_of_spd_dym(gs->cov);
  if (!evals) result = TRUE;
  else
  {
    if ((dyv_ref(evals,dyv_size(evals)-1)/dyv_ref(evals,0) < 1.0e-5) ||
	(dyv_ref(evals,dyv_size(evals)-1) == 0.0))
      result = TRUE;
    free_dyv(evals);
  }
  return result;
}

/* Test if the covariance is very bad in the sense that we are unable to
   invert it.
*/
bool gaussian_has_bad_covariance(gaussian *gs)
{
  dym *inv = mk_invert_spd_cholesky(gs->cov);
  bool result = inv == NULL;
  if ( inv != NULL ) free_dym(inv);
  return result;
}

/* Returns a dyv of size 2 corresponding to the location on the screen
   that the user clicked. Waits for this click. Prints the message to stdout
   before waiting. By default uses the ag coordinate system with 0,0 in
   bottom left and 512,512 in top right. But if ag_set_frame has been called
   uses those coordinates. ag_on(..) must have been called. See draw/amgr.h
   for more details. */
/* Returns the button pressed in *button. 1 is left, 2 middle, 3 right */
dyv *mk_dyv_from_mouse(mapshape *ms,char *message,int *button)
{
  double x,y;
  printf("%s\n",message);
  *button = ms_get_xy(ms,&x,&y);
  return mk_dyv_2(x,y);
}

void gaussian_wait_for_click(void)
{
  double x,y;
  printf("Click in the ag_window");
  (void)ag_get_xy(&x,&y);
}
 
/* Makes a 2-d gaussian with mean at c, and who's 2-std-dev contour
   (which is an ellipse) has its major axis extreme at a, and who's
   minor axis extreme is at b assuming that the user has inputted b so that
   (b-c) is perpendicular to (a-c). 

   If b doesn't satisfy the above, does something sensible anyway (it
   puts the length of the minor axis to be the perpendicular distance from
   b to the line thru a and c).
*/
gaussian *mk_gaussian_from_cab(mapshape *ms,dyv *c,dyv *a,dyv *b)
{
  dyv *d = mk_dyv_subtract(a,c);
  dyv *e = mk_dyv_subtract(b,c);
  dyv *p = mk_copy_dyv(d);
  double lambda = - dyv_scalar_product(e,d) / dyv_scalar_product(d,d);
  double theta;
  dym *w;
  dym *rtheta;
  dym *rthetatranspose;
  dym *cov_inv;
  dym *cov;
  double num_sdevs = 1.96;
  gaussian *gs;
  void draw_gaussian_ellipse_basic(gaussian *gs,double num_sdevs);

  dyv_scalar_mult(p,lambda,p);
  dyv_plus(p,b,p);
  /* p is now the point that will be on the extreme of the minor axis.
     p-c is perpendicular to a-c and is of the desired length. */

  ag_set_pen_color(AG_GREEN);
  ms_dyv(ms,p,DOT_MARKTYPE);

  w = mk_dym_22(1.0/real_max(1e-10,dyv_dsqd(c,a)),   0.0,
                0.0,         1.0/real_max(1e-10,dyv_dsqd(c,p)));

  theta = atan2(dyv_ref(a,1)-dyv_ref(c,1),dyv_ref(a,0)-dyv_ref(c,0));

  printf("theta = %g\n",theta);

  rtheta = mk_dym_22( cos(theta),-sin(theta),
		     sin(theta),cos(theta));

  rthetatranspose = mk_dym_transpose(rtheta);

  cov_inv = mk_dym_mult(w,rthetatranspose);
  dym_mult(rtheta,cov_inv,cov_inv);
  dym_scalar_mult(cov_inv,real_square(num_sdevs)/2,cov_inv);

  cov = mk_invert_dym(cov_inv);

  gs = mk_full_gaussian(c,cov);

  free_dyv(d);
  free_dyv(e);
  free_dyv(p);
  free_dym(w);
  free_dym(rtheta);
  free_dym(rthetatranspose);
  free_dym(cov_inv);
  free_dym(cov);

  return gs;
}

/* Instructs the user to do three mouse clicks to enter the
   covariance of a 2-d gaussian */
gaussian *mk_gaussian_from_mouse_with_given_center(mapshape *ms,dyv *c)
{
  int button;
  dyv *a;
  dyv *b;
  gaussian *gs;

  ag_set_pen_color(AG_BLUE);
  ms_dyv(ms,c,DOT_MARKTYPE);

  a = mk_dyv_from_mouse(ms,"Enter 2-sdev location of major axis",&button);

  ms_dyv(ms,a,DOT_MARKTYPE);
  ag_set_pen_color(AG_PURPLE);
  ms_dyv_line(ms,c,a);
  
  b = mk_dyv_from_mouse(ms,
			"Enter minor axis length as distance from major axis",
			&button);

  ag_set_pen_color(AG_BLUE);
  ms_dyv(ms,b,DOT_MARKTYPE);

  gs = mk_gaussian_from_cab(ms,c,a,b);

  free_dyv(a);
  free_dyv(b);

  return gs;
}

/* Instructs the user to do three mouse clicks to enter the mean and
   covariance of a 2-d gaussian */
gaussian *mk_gaussian_from_mouse(mapshape *ms)
{
  int button;
  dyv *c = mk_dyv_from_mouse(ms,"Enter center of Gaussian",&button);
  gaussian *gs = mk_gaussian_from_mouse_with_given_center(ms,c);
  free_dyv(c);
  return gs;
}

void fprintf_gaussian(FILE *s,char *m1,gaussian *gs,char *m2)
{
  char buff[500];
  sprintf(buff,"%s -> mean",m1);
  fprintf_dyv(s,buff,gs->mean,m2);
  sprintf(buff,"%s -> cov",m1);
  fprintf_dym(s,buff,gs->cov,m2);
  sprintf(buff,"%s -> cov_inv",m1);
  fprintf_dym(s,buff,gs->cov_inv,m2);
  sprintf(buff,"%s -> cov_det_defined",m1);
  fprintf_bool(s,buff,gs->cov_det_defined,m2);
  sprintf(buff,"%s -> cov_det",m1);
  fprintf_double(s,buff,gs->cov_det,m2);
}

/* Returns the minimum lambda such that for all unit vectors n,
   n^T cov^-1 n <= (num_sigmas/lambda)^2,

   i.e. such that sqrt(q^T cov^-1 q) <= num_sigmas where

           q = lambda n 

*/ 
double cov_radius(dym *cov,double num_sigmas)
{
  dyv *eigens = mk_sorted_eigens_of_spd_dym(cov);
  double lambda_sqd = dyv_ref(eigens,0) * num_sigmas;
  free_dyv(eigens);
  return sqrt(lambda_sqd);
}

/* Returns the minimum lambda such that for all unit vectors n,
   [InvDiag(metric) n]^T cov^-1 [InvDiag(metric) n]] <= (num_sigmas/lambda)^2,

   i.e. such that sqrt(q^T cov^-1 q) <= num_sigmas where

           q = lambda InvDiag(metric) n 

*/ 
double cov_metric_radius(dym *cov,dyv *metric,double num_sigmas)
{
  dym *cov2 = mk_copy_dym(cov);
  int i,j;
  double lambda;

  for ( i = 0 ; i < dym_rows(cov) ; i++ )
    for ( j = 0 ; j < dym_rows(cov) ; j++ )
      dym_set(cov2,i,j,dym_ref(cov,i,j)/(dyv_ref(metric,i)*dyv_ref(metric,j)));
  
  lambda = cov_radius(cov2,num_sigmas);

  free_dym(cov2);

  return lambda;
}

/* Returns the minimum lambda such that for all unit vectors n,
   [InvDiag(metric) n]^T cov^-1 [InvDiag(metric) n]] <= (num_sigmas/lambda)^2,

   i.e. such that sqrt(q^T cov^-1 q) <= num_sigmas where

           q = lambda InvDiag(metric) n 

*/ 
double gaussian_metric_radius(gaussian *gs,dyv *metric,double num_sigmas)
{
  return cov_metric_radius(gs->cov,metric,num_sigmas);
}

/* NULL rows means "use all rows" */
gaussian *mk_gaussian_from_dym_rows(bool naive,dym *x,ivec *rows)
{
  dyv *mean = mk_mean_of_dym_rows(x,rows);
  gaussian *gs;

  if ( naive )
  {
    dyv *cov = mk_diag_covariance_of_dym_rows(x,mean,rows);
    gs = mk_diag_gaussian(mean,cov);
    free_dyv(cov);
  }
  else
  {
    dym *cov = mk_covariance_of_dym_rows(x,mean,rows);
    gs = mk_full_gaussian(mean,cov);
    free_dym(cov);
  }

  free_dyv(mean);

  return gs;
}

/* Assume we are evaluating the pdf of a Gaussian at point x.
   Assume mean Mu and covariance Cov.

   Then, provided mahalanobis_sqd = (x - MU)^T Cov^-1 (x - MU)
         and provided               cov_det = Determinant of COV

   Then this function returns that pdf value.
*/
double loglike_full_gauss_from_mah_and_covdet(double mahalanobis_sqd,
					      double cov_det,
					      int num_dims)
{
  double log_numerator = -mahalanobis_sqd/2;
  double log_denominator = num_dims * LOG_TWO_PI / 2.0 + log(cov_det)/2.0;
  double log_like = log_numerator - log_denominator;
#ifndef AMFAST
  if ( is_ill_defined(log_like) ) 
    my_error("gauss_eval log_like is_ill_defined");
  else
  {
    double log_a = log_like;
    double b = eval_full_gauss_from_mah_and_covdet(mahalanobis_sqd,
						   cov_det,num_dims);
    if ( b > 1e-50 )
    {
      double log_b = log(b);
      double norm = sqrt(log_a * log_a + log_b * log_b);
      if ( norm > 1e-10 && fabs(log_a - log_b) / norm > 1e-5 )
	my_error("loglike_full_gauss_from_mah_and_covdet: inconsistent");
    }
  }
#endif
  return log_like;
}

/* Assume we are evaluating the pdf of a Gaussian at point x.
   Assume gs has mean Mu and covariance Cov.

   Then, provided mahalanobis_sqd = (x - MU)^T Cov^-1 (x - MU)

   Then this function returns that pdf value.
*/
double loglike_full_gauss_from_mah(gaussian *gs,double mahalanobis_sqd)
{
  double cov_det = private_cov_det(gs);
  int num_dims = gaussian_num_dims(gs);
#ifndef AMFAST
  if ( is_ill_defined(cov_det) ) my_error("gauss_loglike cov_det is_ill_defined");
#endif

  return loglike_full_gauss_from_mah_and_covdet(mahalanobis_sqd,cov_det,num_dims);
}

/* Evaluates the pdf of the gaussian at point x. If
 r_mahal_dsqd is non-NULL, returns the mahalanbois distance of x from
 the gaussian mean in *r_mahal_dsqd */
double full_gauss_loglike(gaussian *gs,dyv *x,double *r_mahal_dsqd)
{
  dyv *dx = mk_dyv_subtract(x,gs->mean);
  dym *cov_inv = private_cov_inv(gs);
  double ms = eval_mahalanobis_sqd(dx,cov_inv);
  double log_like = loglike_full_gauss_from_mah(gs,ms);
#ifndef AMFAST
  if ( dyv_is_ill_defined(dx) ) my_error("gauss_loglike: dx is_ill_defined");
  if ( dym_is_ill_defined(cov_inv) ) my_error("gauss_loglike: cov_inv is_ill_defined");
  if ( is_ill_defined(ms) ) my_error("gauss_loglike ms is_ill_defined");
  if ( is_ill_defined(log_like) ) my_error("gauss_loglike log_like is_ill_defined");
#endif
  if ( r_mahal_dsqd != NULL ) *r_mahal_dsqd = ms;
  free_dyv(dx);
  return log_like;
}

/* Evaluates the pdf of the gaussian at point x. If
 r_mahal_dsqd is non-NULL, returns the mahalanbois distance of x from
 the gaussian mean in *r_mahal_dsqd */
/* For naive gaussian (in which cov is diagonal)

    p(x|mu,cov) = 1/[(root_two_pi^D) sqrt(det(cov))] *
                  exp(-sum_i [0.5 * (x[i] - mu[i])^2/cov[i]]^2)

   So log p(x|mu,cov) =

      - sum_i [(x[i] - mu[i])/(2 cov[i])]^2
         - D/2 log two_pi 
         - 1/2 log det(cov)
*/
double diag_gauss_loglike(gaussian *gs,dyv *x,double *r_mahal_dsqd)
{
  double loglike = 0.0;
  double cov_det = private_cov_det(gs);
  double mahal_dsqd = 0.0;
  int num_dims = dyv_size(x);
  int i;

  my_assert(dyv_size(gs->mean) == num_dims);

  for ( i = 0 ; i < num_dims ; i++ )
  {
    double dx = dyv_ref(gs->mean,i) - dyv_ref(x,i);
    double covi = dyv_ref(gs->cov_diag,i);

    my_assert(covi > 0.0);

    mahal_dsqd += dx * dx / covi;
  }

  if ( cov_det <= 0.0 )
    my_error("diag_gauss_loglike: cov_det <= 0.0");

  loglike = -(mahal_dsqd + num_dims * LOG_TWO_PI + log(cov_det)) / 2;

  if ( r_mahal_dsqd != NULL ) *r_mahal_dsqd = mahal_dsqd;

  return loglike;
}

/* Evaluates the pdf of the gaussian at point x. If
 r_mahal_dsqd is non-NULL, returns the mahalanbois distance of x from
 the gaussian mean in *r_mahal_dsqd */
double gauss_loglike(gaussian *gs,dyv *x,double *r_mahal_dsqd)
{
  double result = (gaussian_is_diag(gs)) ? 
                    diag_gauss_loglike(gs,x,r_mahal_dsqd) :
                    full_gauss_loglike(gs,x,r_mahal_dsqd);
  return result;
}

/* The following four functions all return a scalar property of
   the Gaussian, specific to one of the dimensions. (Actually, the
   covariance function depends on two of the dimensions) */

double gaussian_dim_to_mean(gaussian *gs,int i)
{
  return dyv_ref(gaussian_mean(gs),i);
}

double gaussian_dim_to_variance(gaussian *gs,int i)
{
  double result = (gaussian_is_diag(gs)) ? dyv_ref(gs->cov_diag,i) :
                                           dym_ref(gs->cov,i,i);
  return result;
}

double gaussian_dim_to_sdev(gaussian *gs,int i)
{
  double var = gaussian_dim_to_variance(gs,i);
  my_assert_always(var >= 0.0);
  return sqrt(var);
}

/* PRE: gaussian must be full. Do not call if
   gaussian_is_diag(gs) is TRUE. */
double gaussian_dims_to_cov(gaussian *gs,int i,int j)
{
  if ( gaussian_is_diag(gs) )
    my_error("gaussian_dims_to_cov: you called with non-diagonal gaussian");
  return dym_ref(gs->cov,i,j);
}

bool gaussian_loglik_is_ill_defined(gaussian *gs) {
  if(gaussian_is_ill_defined(gs))
    return TRUE;
  return private_cov_det(gs) < 0;
}
