/*  miscprob.cc

    Christian Beckmann & Mark Woolrich, FMRIB Image Analysis Group

    Copyright (C) 1999-2000 University of Oxford  */

/*  CCOPYRIGHT  */

// Miscellaneous maths functions that rely on libprob

#include "miscprob.h"
#include "stdlib.h"
#include "newmatio.h"
#include <iostream>

using namespace NEWMAT;

namespace MISCMATHS {

ReturnMatrix unifrnd(const int dim1, const int dim2, const float start, const float end)
{
  int tdim = dim2;
  double tmpD=1.0;
  if(tdim<0){tdim=dim1;}
  Matrix res(dim1,tdim);
  for (int mc=1; mc<=res.Ncols(); mc++) {
    for (int mr=1; mr<=res.Nrows(); mr++) {
      tmpD = (rand()+1)/double(RAND_MAX+2.0);
      res(mr,mc)=(tmpD)*(end-start)+start;
      //drand(&tmpD);
      //res(mr,mc)=(tmpD-1)*(end-start)+start;
    }
  }

  res.Release();
  return res;
}

ReturnMatrix normrnd(const int dim1, const int dim2, const float mu, const float sigma)
{
  int tdim = dim2;
  double tmpD=1.0;
  if(tdim<0){tdim=dim1;}
  Matrix res(dim1,tdim);
  for (int mc=1; mc<=res.Ncols(); mc++) {
    for (int mr=1; mr<=res.Nrows(); mr++) {
      tmpD = (rand()+1)/double(RAND_MAX+2.0);
      res(mr,mc)=ndtri(tmpD)*sigma+mu ;
      //drand(&tmpD);
      //res(mr,mc)=ndtri(tmpD-1)*sigma+mu ;
    }
  }

  res.Release();

  return res;
}

ReturnMatrix normpdf(const RowVector& vals, const float mu, const float var)
{
  RowVector res(vals);
  for (int mc=1; mc<=res.Ncols(); mc++){
    res(mc) = std::exp(-0.5*(std::pow(vals(mc)-mu,2)/var))*std::pow(2*M_PI*var,-0.5);
  }

  res.Release();
  return res;
}
float normpdf(const ColumnVector& vals, const ColumnVector& mu, const SymmetricMatrix& sigma)
{
  float res;
  LogAndSign ld=(2*M_PI*sigma).LogDeterminant();

  res =  std::exp(-0.5*(
			((vals-mu).t()*sigma.i()*(vals-mu)).AsScalar()
			+ld.LogValue()
			));
  return res;
}
ReturnMatrix normpdf(const Matrix& vals, const ColumnVector& mu, const SymmetricMatrix& sigma)
{
  RowVector res(vals);
  LogAndSign ld = (2*M_PI*sigma).LogDeterminant();
  Matrix isigma = sigma.i();

  for (int mc=1; mc<=res.Ncols(); mc++){
    res(mc) = std::exp( -0.5*(
			      ((vals.Column(mc)-mu).t()*isigma*(vals.Column(mc)-mu)).AsScalar() 
			      +ld.LogValue()
			      ));
  }

  res.Release();
  return res;
}
ReturnMatrix normcdf(const RowVector& vals, const float mu, const float var)
{
  RowVector res(vals);
  RowVector tmp;
  tmp = (vals-mu)/std::sqrt(var);
  for (int mc=1; mc<=res.Ncols(); mc++){
    res(mc) = ndtr(tmp(mc));
  }

  res.Release();
  return res;
}

ReturnMatrix gammacdf(const RowVector& vals, const float mu, const float var)
{
  RowVector res(vals);
  res=0;
  if((mu>0)&&(var>0)){
    float b = std::pow(mu,2)/var;
    float a = mu/var;  
    for (int mc=1; mc<=res.Ncols(); mc++){
      if(vals(mc)>0)
	res(mc) = gdtr(a,b,vals(mc));
    }
  }
  res.Release();
  return res;
}

ReturnMatrix gammapdf(const RowVector& vals, const float mu, const float var)
{
  RowVector res(vals);
  res=0;
  if((mu>0)&&(var>0.00001)){
    float a = std::pow(mu,2)/var;
    float b = mu/var;
    float c = lgam(a);
    if(std::abs(c) < 150){
      for (int mc=1; mc<=res.Ncols(); mc++){
	if(vals(mc)>0.000001){
	  res(mc) = std::exp(a*std::log(b) + 
			     (a-1) * std::log(vals(mc)) 
			     - b*vals(mc) - c);
	}
      }
    }
  }
  res.Release();
  return res;
}


ReturnMatrix normpdf(const RowVector& vals, const RowVector& mu, const RowVector& var)
{
  Matrix res(mu.Ncols(),vals.Ncols());
  for (int mc=1; mc<=res.Ncols(); mc++){
    for (int mr=1; mr<=res.Nrows(); mr++){
      res(mr,mc) = std::exp(-0.5*(std::pow(vals(mc)-mu(mr),2)/var(mr)))*std::pow(2*M_PI*var(mr),-0.5);
    }
  }

  res.Release();
  return res;
}


ReturnMatrix mvnrnd(const RowVector& mu, const SymmetricMatrix& covar, int nsamp) 
{     
//   Matrix eig_vec; 
//   DiagonalMatrix eig_val;
//   EigenValues(covar,eig_val,eig_vec);

//   Matrix ret = ones(nsamp, 1)*mu + dnormrandm(nsamp,mu.Ncols())*sqrt(eig_val)*eig_vec.t();
  Mvnormrandm mvn(mu, covar);

  return mvn.next(nsamp);
}

// Saad: Wishart and inverseWishart Random Generator
ReturnMatrix wishrnd(const SymmetricMatrix& sigma,const int dof){
  // compute cholesky factor for sigma
  LowerTriangularMatrix L = Cholesky(sigma);

  // for small degrees of freedom, use the definition
  int n = sigma.Nrows();
  Matrix X;
  if(dof <= 81+n ){
    X.ReSize(dof,n);
    X = normrnd(dof,n) * L.t();
  }
  // otherwise, use Smith & Hocking procedure
  else{
    X.ReSize(n,n);
    Matrix A(n,n);
    for(int i=1;i<=n;i++){
      Gamma G((dof-i+1)/2);
      G.Set(rand()/float(RAND_MAX));
      for(int j=1;j<=n;j++){
	if     (i>j) { A(i,j) = 0; }
	else if(i<j) { A(i,j) = normrnd(1,1).AsScalar(); }
	else         { A(i,j) = std::sqrt(2*G.Next()); }
      }
    }
    X = A * L.t();
  }

  SymmetricMatrix res(n);
  res << X.t() * X;

  res.Release();
  return res;
}
ReturnMatrix iwishrnd(const SymmetricMatrix& sigma,const int dof){
  // assumes inv-Wishart(sigma.i(),dof)

  SymmetricMatrix res;
  res = wishrnd(sigma,dof);
  res = res.i();

  res.Release();
  return res;
}

ReturnMatrix perms(const int n){
  if(n<=1){
    Matrix P(1,1);
    P << n;
    P.Release();
    return P;
  }
  Matrix Q = perms(n-1);  // recursive calls
  int m = Q.Nrows();
  Matrix P(n*m,n);
  for(int i=1;i<=m;i++){
    P(i,1)=n;
    for(int j=1;j<=Q.Ncols();j++)
      P(i,j+1)=Q(i,j);
  }
  for(int i=n-1;i>=1;i--){
    int jj=1;
    for(int j=(n-i)*m+1;j<=(n-i+1)*m;j++){
      P(j,1)=i;
      for(int k=1;k<=n-1;k++){
	P(j,k+1)= (Q(jj,k)==i) ? n : Q(jj,k);
      }
      jj++;
    } 
  }
  P.Release();
  return P;
}

}