#include <config.h>
#include <distributions/DMNorm.h>
#include <matrix/lapack.h>
#include <matrix/matrix.h>
#include <sarray/SArray.h>

#include <cmath>
#include <vector>
#include <stdexcept>

#include <Rmath.h>

using std::vector;
using std::logic_error;

DMNorm::DMNorm()
  : Distribution("dmnorm", 2, false, false) 
{}

DMNorm::~DMNorm()
{}

double DMNorm::logLikelihood(SArray const &x,
			     vector<SArray const *> const &parameters) const
{
  double const *y = x.value();
  int m = parameters[0]->length();
  double const * mu = parameters[0]->value();
  double const * T = parameters[1]->value();

  double loglik = logdet(T, m)/2;
  double * delta = new double[m];
  for (int i = 0; i < m; ++i) {
    delta[i] = y[i] - mu[i];
    loglik -= (delta[i] * T[i + i * m] * delta[i])/2;
    for (int j = 0; j < i; ++j) {
      loglik -= (delta[i] * T[i + j * m] * delta[j]);
    }
  }
  delete [] delta;

  return loglik;
}

void DMNorm::randomSample(SArray &x,
			  vector<SArray const *> const &parameters) const
{
  double const * mu = parameters[0]->value();
  double const * T = parameters[1]->value();
  int nrow = parameters[0]->length();

  double *y = new double[nrow];
  randomsample(y, mu, T, nrow);
  x.setValue(y, nrow);
  delete [] y;
}

void DMNorm::randomsample(double *x, double const *mu, double const *T,
			  int nrow)
{
  int N = nrow*nrow;
  double * Tcopy = new double[N];
  for (int i = 0; i < N; ++i) {
    Tcopy[i] = T[i];
  }
  double * w = new double[nrow];

  int info = 0;
  double worktest;
  int lwork = -1;
  // Workspace query
  F77_DSYEV ("V", "L", &nrow, Tcopy, &nrow, w, &worktest, &lwork, &info);
  // Now get eigenvalues/vectors with optimal work space
  lwork = static_cast<int>(worktest + DBL_EPSILON);
  double * work = new double[lwork];
  F77_DSYEV ("V", "L", &nrow, Tcopy, &nrow, w, work, &lwork, &info);
  delete [] work;

  /* Generate independent random normal variates, scaled by
     the eigen values. We reuse the array w. */
  for (int i = 0; i < nrow; ++i) {
    w[i] = rnorm(0, 1/sqrt(w[i]));
  }

  /* Now transform them to dependant variates 
    (On exit from DSYEV, Tcopy contains the eigenvectors)
  */
  for (int i = 0; i < nrow; ++i) {
    x[i] = mu[i];
    for (int j = 0; j < nrow; ++j) {
      x[i] += Tcopy[i + j * nrow] * w[j];
    }
  }
  delete [] w;
  delete [] Tcopy;
}

bool DMNorm::checkParameterDim(vector<SArray const *> const &parameters) const
{
  Index const &dim0 = parameters[0]->dim(true);
  Index const &dim1 = parameters[1]->dim(true);

  if (dim0.size() != 1)
    return false;
  if (dim1.size() != 2)
    return false;
  if (dim1[0] != dim1[1] || dim0[0] != dim1[0]) {
    return false;
  }
  return true;
}

Index const &DMNorm::dim(vector<SArray const *> const &parameters) const
{
  return parameters[0]->dim(true);
}

bool DMNorm::checkParameterValue(vector<SArray const *> const &parameters) const
{
  long n = parameters[0]->length();

  double const *T = parameters[1]->value();
  // Check symmetry
  for (int i = 0; i < n - 1; i++) {
    for (int j = i + 1; j < n; j++) {
      if (fabs(T[i + j*n] - T[j + i*n]) > DBL_EPSILON)
	return false;
    }
  }
  // Don't bother checking positive definiteness

  return true;
}

unsigned long DMNorm::df(std::vector<SArray const *> const &parameters) const
{
    return parameters[0]->length();
}

double 
DMNorm::lowerSupport(unsigned long i,
		     std::vector<SArray const *> const &parameters) const
{
  unsigned long m = parameters[0]->length();
  if (i >= m)
    throw logic_error("Invalid index in DMNorm::lowerSupport");
  
  return -DBL_MAX;
}

double 
DMNorm::upperSupport(unsigned long i,
		     std::vector<SArray const *> const &parameters) const
{
  unsigned long m = parameters[0]->length();
  if (i >= m)
    throw logic_error("Invalid index in DMNorm::upperSupport");

  return DBL_MAX;
}