// The function ProbImhof(x, A, B, m, S) calculates the cumulative
// distribution function, evaluated at x, of the ratio of quadratic
// forms, (z'Az)/(z'Bz), in a normal random vector z with mean vector
// m and variance matrix S.
// If B=0, then the distribution of z'Az is computed.
//
// ProbImhof(x, A, B, m, S)                                          
//     x	in: scalar, x-value at which distribution is evaluated;                               
//     A	in: nxn matrix (is transformed into symmetric (A+A')/2);
//     B	in: nxn matrix (is transformed into symmetric (B+B')/2) or 0;
//     m	in: nx1 mean vector;
//     S	in: nxn positive definite variance matrix (pd is not checked).
//
// Return value
//     Returns the probability
//					P [ (z'Az) <= x ]					if	B == 0,
//					P [ (z'Az)/(z'Bz) <= x ]			if	B <> 0,
//     with z ~ N[m,S].
//
// QuanImhof(p, A, B, m, S)                                          
//     p	in: scalar, probability at which quantile is evaluated;                               
// returns x;

#include <oxstd.h>
#include <oxfloat.h>
#include <quadpack.h>

static decl s_l, s_d, s_c;
const decl QUANT_MAXIT = 200;
const decl QUANT_EPS   = 1e-8;

static imhof_mgf(const u)
{
	decl eps = 0.5*(sumc(atan(s_l*u)+(s_d.^2).*s_l*u./(1+((s_l*u).^2)))-s_c*u);
	decl gam = prodc(((1+((s_l*u).^2)).^0.25).*exp(0.5*((s_d.*s_l*u).^2)./(1+(s_l*u).^2)));
	return (sin(eps)/(u*gam));
}

ProbImhof(const x, const A, const B, const m, const S)
{
	decl Q, V, result, abserr;
	decl P = choleski(S);
	if (B == 0)
	{
		Q = P'((A+A')/2)*P;
		eigensym(Q,&s_l,&V);
		s_c = x;
	}
	else
	{
		Q = A - B*x;
		Q = (P'((Q+Q')/2)*P);
		eigensym(Q,&s_l,&V);
		s_c = 0;
	}
	s_d = V'solvelu(P, 0, 0, unit(rows(P))) *m;
	s_l = s_l';
	s_d = selectifr(s_d,s_l);
	s_l = selectifr(s_l,s_l);
	QAGI(imhof_mgf,0,1,&result,&abserr);
	return (0.5 - result/M_PI);
}

QuanImhof(const p, const A, const B, const m, const S)
{
	// not very efficient, but it works
	decl i, pa, pb, xa, xb, w, diff, pn, xn;

	if (p <= 0)
		return 0;
	// find an initial bracket
	pb = xb = 0.0;
	do 
	{	pa = pb; xa = xb;
		xb = (xb + 1) * 2;
		pb = ProbImhof(xb, A, B, m, S);
	}  while (pb < p);

	// now narrow bracket
	for (i = 0; ; ++i)
	{
		diff = pb - pa;
		w = diff > 0.01 ? 0.5 : (pb - p) / diff;
		xn = xa * w + xb * (1 - w);
		pn = ProbImhof(xn, A, B, m, S);
		if (pn < p)
			xa = xn, pa = pn;
		else
			xb = xn, pb = pn;

		if (pb - p < QUANT_EPS)
			return xb;
		else if (p - pa < QUANT_EPS)
			return xa;
		if (i >= QUANT_MAXIT)
			return .NaN;
	}
}
