#define WANT_STREAM
#define WANT_MATH

#include <iostream>
#include "newmat.h"    // need matrix output routines
#include "newmatio.h"    // need matrix output routines
#include "newmatnl.h"    // need matrix newton routines

using namespace std;  //introduces namespace std

class BB_LL : public LL_D_FI
{
   ColumnVector D;				// derivatives of loglikelihood
   SymmetricMatrix D2;			// second derivatives
   ColumnVector p;				// the parameters
   SymmetricMatrix S;
   Matrix K;
   double N2;
   int nvar, npar;
	
public:

   BB_LL(const SymmetricMatrix& S_in, const Matrix K_in,
   	const ColumnVector& p_in, const double N2_in, const int nvar_in, const int npar_in)
   	: S(S_in), K(K_in), p(p_in), N2(N2_in), nvar(nvar_in), npar(npar_in) {}  
   	// arguments are covariance matrix, basis matrix, coefficient columnvector,
   	// and N/2, nvar(iables), and npar(ameters)

   void Set(const ColumnVector& p_set) { // set parameter values
      p = p_set;
   }

   bool IsValid();                 // are parameters valid
   Real LogLikelihood();           // return the loglikelihood
   ReturnMatrix Derivatives();     // derivatives of log-likelihood
   ReturnMatrix FI();              // Fisher Information matrix
};

bool BB_LL::IsValid() {
	 return true;
}

Real BB_LL::LogLikelihood() {
	Real LL;          // the log likelihood
   
	D.ReSize(npar);
	D2.ReSize(npar);
   
	IdentityMatrix I(nvar);
	DiagonalMatrix Delta(nvar);
	double gamma;
	SymmetricMatrix KDK(nvar);
	SymmetricMatrix Sigma(nvar);
	SymmetricMatrix SigmaInverse(nvar);
	Matrix SigmaS(nvar,nvar);
	Matrix W(nvar,nvar);
	Matrix Kprime(nvar,nvar);
	ColumnVector Psi1Delta(npar-1);
	ColumnVector Psi1gamma(1);
	ColumnVector Psi1(npar);
	Matrix Psi2Delta(npar-1,npar-1);
	ColumnVector Psi2Deltagamma(npar-1);
	ColumnVector Psi2gamma(1);
	Matrix Psi2top(npar-1,npar);
	RowVector Psi2bottom(npar);
	Matrix Psi2(npar,npar);
	
	Delta = (p.SubMatrix(1,nvar,1,1)).AsDiagonal();
	gamma = p(nvar+1);
	Kprime = K.t();
	KDK << K * Delta * Kprime;
	Sigma = KDK + gamma*I;
	SigmaInverse = Sigma.i();
	SigmaS = SigmaInverse * S;

	LL = -N2 * (log(Sigma.Determinant()) + SigmaS.Trace());

	W = SigmaS * SigmaInverse;
	Psi1Delta << N2 * (SP(I,(Kprime * (SigmaInverse - W) * K))).AsColumn();
	Psi1gamma << N2 * (SigmaInverse - W).Trace();
	Psi1 = Psi1Delta & Psi1gamma;

	D = -Psi1;	// the matrix routines want the derivatives as a column

	if (wg) {    // do only if second derivatives wanted

		Psi2Delta = N2 * SP((Kprime * SigmaInverse * K) ,
					 (Kprime * ((2*W) - SigmaInverse) * K));
		Psi2Deltagamma = N2 * (SP(I,(Kprime * SigmaInverse 
									* (2*W - SigmaInverse) * K))).AsColumn();
		Psi2gamma << N2 * (SigmaInverse * (2*W - SigmaInverse)).Trace();

		Psi2top = Psi2Delta | Psi2Deltagamma;
		Psi2bottom = Psi2Deltagamma.t() | Psi2gamma;
		Psi2 = Psi2top & Psi2bottom;

		D2 << Psi2;
		}
/*
cout << "loglikelihood " << LL << endl;
   	cout << "parameters " << p;
   	cout << "LL " << LL << endl;
	cout << "D " << D;
	if (wg) cout << "D2 " << D2;
*/	
   return LL;
}

ReturnMatrix BB_LL::Derivatives() { 
	return D; 
}

ReturnMatrix BB_LL::FI() {
   if (!wg) cout << endl << "unexpected call of FI" << endl;
   return D2;
}