void FFOgkBasis(
const MatrixXd& xi,
const int& calcM,
const int& intercept,
VectorXi& warn,
const int& h,
VectorXi& dIn,
int w3
){
double (*pFo[])(Ref<VectorXd>,int)={&qn,&scaleTau2};
double (*qFo[])(Ref<VectorXd>,int)={&Fmedian,&scaleTau2};
const int p=xi.cols(),n=xi.rows(),h0=(n+1)/2;
double b1=0.0,b2=0.0;
const double tol=1e-8;
int i,j;
MatrixXd x=xi;
RowVectorXd lamba(p);
MatrixXd x2=x;
if(intercept){
for(i=0;i<p;i++) lamba(i)=qCalc(x2.col(i),1,qFo[calcM]);
x.rowwise()-=lamba;
}
for(i=0;i<p;i++) lamba(i)=pCalc(x2.col(i),0,pFo[calcM]);
for(i=0;i<p;i++) warn(i)=(lamba(i)<tol)?1:0;
i=warn.sum();
if(i>0) return;
for(i=0;i<p;i++) x.col(i).array()/=lamba(i);
VectorXd dvec1=VectorXd::Ones(p);
MatrixXd U=dvec1.asDiagonal();
VectorXd sYi(n);
VectorXd sYj(n);
VectorXd dY(n);
for(i=0;i<p;++i){
sYi=x.col(i);
for(j=0;j<i;++j){
sYj=x.col(j);
dY=sYi+sYj;
b1=pCalc(dY,0,pFo[calcM]);
b1*=b1;
dY=sYi-sYj;
b2=pCalc(dY,0,pFo[calcM]);
b2*=b2;
U(i,j)=0.25*(b1-b2);
U(j,i)=U(i,j);
}
}
JacobiSVD<MatrixXd> svd(U,ComputeThinV);
x2=x*svd.matrixV();
for(i=0;i<p;i++) lamba(i)=pCalc(x2.col(i),0,pFo[calcM]);
for(i=0;i<p;i++) warn(i)=(lamba(i)<tol)?1:0;
i=warn.sum();
if(i>0) return;
for(i=0;i<p;i++) x2.col(i).array()/=lamba(i);
dY=x2.array().abs2().rowwise().sum();
dIn.setLinSpaced(n,0,n-1);
std::nth_element(dIn.data(),dIn.data()+h,dIn.data()+dIn.size(),IdLess(dY.data()));
cov_CStep(dIn,x,h,h,w3);
return;
}
void cov_CStep(
VectorXi& dIn,
MatrixXd& x,
const int h,
const int h0,
int w3
){
const int n=x.rows(),p=x.cols();
double w1,w0;
int w2=1,i;
MatrixXd xSub(h,p);
for(i=0;i<h0;i++) xSub.row(i)=x.row(dIn(i));
RowVectorXd xSub_mean(p);
xSub_mean=xSub.topRows(h0).colwise().mean();
xSub.topRows(h0).rowwise()-=xSub_mean;
x.rowwise()-=xSub_mean;
MatrixXd Sig(p,p);
//Sig=xSub.topRows(h0).adjoint()*xSub.topRows(h0);
Sig.setZero().selfadjointView<Lower>().rankUpdate(xSub.topRows(h0).transpose());
Sig.array()/=(double)(h0-1);
LDLT<MatrixXd> chol=Sig.ldlt();
MatrixXd b=MatrixXd::Identity(p,p);
chol.solveInPlace(b);
w1=chol.vectorD().array().minCoeff();
VectorXd dP(n);
if(w1>1e-6){
w1=std::numeric_limits<double>::max();
dP=((x*b).cwiseProduct(x)).rowwise().sum();
} else {
w2=0;
w3=0;
w1=chol.vectorD().array().log().sum()*2.00;
}
while(w2){
dIn.setLinSpaced(n,0,n-1);
std::nth_element(dIn.data(),dIn.data()+h,dIn.data()+dIn.size(),IdLess(dP.data()));
for(i=0;i<h;i++) xSub.row(i)=x.row(dIn(i));
xSub_mean=xSub.colwise().mean();
xSub.rowwise()-=xSub_mean;
x.rowwise()-=xSub_mean;
//Sig=xSub.adjoint()*xSub;
Sig.setZero().selfadjointView<Lower>().rankUpdate(xSub.transpose());
Sig.array()/=(double)(h-1);
chol=Sig.ldlt();
b=MatrixXd::Identity(p,p);
chol.solveInPlace(b);
if(chol.vectorD().array().minCoeff()>1e-6){
w0=w1;
w1=chol.vectorD().array().log().sum()*2.00;
dP=((x*b).cwiseProduct(x)).rowwise().sum();
(w0-w1<1e-3)?(w2=0):(w2=1);
} else {
w2=0;
w3=0;
}
}
}
