int CheckIfInsideShrinkingGrain(double P0[]){
int counter = 0;
int i, j;
double xinters;
double P1[2], P2[2];
for (j = 0; j < 2; j++) {
if (P0[j] < 0.0) P0[j] += LENGTH[j];
else if (P0[j] > LENGTH[j]) P0[j] -= LENGTH[j];
}
for (j = 0; j < 2; j++) P1[j] = VXPOINT[0][j];
for (i = 1; i <= NVERTEX; i++) {
for (j = 0; j < 2; j++) P2[j] = VXPOINT[(i%NVERTEX)][j];
if (P0[1] > whichmin(P1[1], P2[1])) {
if (P0[1] <= whichmax(P1[1], P2[1])) {
if (P0[0] <= whichmax(P1[0], P2[0])) {
if (P1[1] != P2[1]) {
xinters = (P0[1]-P1[1])*(P2[0] - P1[0])/(P2[1]-P1[1]) + P1[0];
if (P1[0] == P2[0] || P0[0] <= xinters) {
counter++;
}
}
}
}
}
for (j = 0; j < 2; j++) P1[j] = P2[j];
}
if (counter % 2 == 0)
return 0; //Outside
else
return 1; //Inside
}
char *viterbi(double *pars,double **eprob,double **tprob,int nSites){
// fprintf(stderr,"[%s] %f %f %f\n",__FUNCTION__,pars[0],pars[1],pars[2]);
double logres[3]={log(pars[0]),log(pars[1]),log(pars[2])};
// fprintf(stderr,"init logres: %f %f %f\n",logres[0],logres[1],logres[2]);
double **res = new double*[3];
char **ptr = new char*[3];
for(int i=0;i<3;i++){
ptr[i] = new char[nSites];
res[i] = new double[nSites];
res[i][0] = logres[i]+eprob[i][0];
}
for(int i=1;i<nSites;i++){
res[0][i] = eprob[0][i]+max(log(tprob[T00][i])+res[0][i-1],log(tprob[T10][i])+res[1][i-1],log(tprob[T20][i])+res[2][i-1]);
res[1][i] = eprob[1][i]+max(log(tprob[T01][i])+res[0][i-1],log(tprob[T11][i])+res[1][i-1],log(tprob[T21][i])+res[2][i-1]);
res[2][i] = eprob[2][i]+max(log(tprob[T02][i])+res[0][i-1],log(tprob[T12][i])+res[1][i-1],log(tprob[T22][i])+res[2][i-1]);
ptr[0][i] = whichmax(log(tprob[T00][i])+res[0][i-1],log(tprob[T10][i])+res[1][i-1],log(tprob[T20][i])+res[2][i-1]);
ptr[1][i] = whichmax(log(tprob[T01][i])+res[0][i-1],log(tprob[T11][i])+res[1][i-1],log(tprob[T21][i])+res[2][i-1]);
ptr[2][i] = whichmax(log(tprob[T02][i])+res[0][i-1],log(tprob[T12][i])+res[1][i-1],log(tprob[T22][i])+res[2][i-1]);
}
double loglike = max(res[0][nSites-1],res[1][nSites-1],res[2][nSites-1]);
fprintf(stderr,"[%s] \tloglike=%f\n",__FUNCTION__,loglike);
char *vit = new char[nSites];
vit[nSites-1] = whichmax(ptr[0][nSites-1],ptr[1][nSites-1],ptr[2][nSites-1]);
for(int i=(nSites-1);i>0;i--)
if(vit[i]==2)
vit[i-1] = ptr[2][i];
else if(vit[i]==1)
vit[i-1] = ptr[1][i];
else
vit[i-1] = ptr[0][i];
return vit;
}
SEXP getLabels(SEXP mod, SEXP dataset) {
// compute responsibilities w.r.t classic EM style
PROTECT(mod=coerceVector(mod, VECSXP));
PROTECT(dataset=coerceVector(dataset, REALSXP));
int k = length(coerceVector(getListElement(mod, "w"), REALSXP));
int n = INTEGER(getAttrib(dataset, R_DimSymbol))[0];
int d = INTEGER(getAttrib(dataset, R_DimSymbol))[1];
// build a nxk matrix containing temp results
gsl_matrix *response = gsl_matrix_calloc(n, k);
gsl_vector_view view;
SEXP current;
for(int i=0; i<k; i++) {
current = mvndensity(coerceVector(VECTOR_ELT(coerceVector(getListElement(mod, "mean"), VECSXP), i), REALSXP),
coerceVector(VECTOR_ELT(coerceVector(getListElement(mod, "cov"), VECSXP), i), REALSXP), dataset);
view = gsl_matrix_column(response, i);
SXPtoVector(&(view.vector), current);
gsl_vector_scale(&(view.vector), REAL(coerceVector(getListElement(mod, "w"), REALSXP))[i]);
}
gsl_vector *gslres = gsl_vector_alloc(n);
for(int i=0; i<n; i++) {
view = gsl_matrix_row(response, i);
gsl_vector_set(gslres, i, (double)whichmax(&(view.vector))+1.0);
}
SEXP res;
PROTECT(res=allocVector(INTSXP, n));
intVectorToSXP(&res, gslres);
gsl_matrix_free(response);
gsl_vector_free(gslres);
UNPROTECT(3);
return(res);
}
void caspar(double *Xpass, double *Y, double *Xtestpass, double *Ytest, int *npass, int *ppass, int *ntestpass, double *plocs, double *palpha, double *ph, int *psequ, double *psse, int *ptrace){
double **X, **XT, **Xtest, **gram, **TG, **TGI, **BT, *locs, *kern;
double h = *ph, alpha = *palpha, dstep = 1.0;
int n = *npass, p = *ppass, nt = *ntestpass;
int i,j, *numneigh, ncount, **neigh;
int *pcode; int verbose = *ptrace;
pcode = (int *)calloc(1,sizeof(int));
if(alpha == 0.0) alpha = .0001;
X = (double **)calloc(n, sizeof(double *));
for(i=0;i<n;i++) X[i] = (double *)calloc(p,sizeof(double));
Xtest = (double **)calloc(nt, sizeof(double *));
for(i=0;i<nt;i++) Xtest[i] = (double *)calloc(p,sizeof(double));
XT = (double **)calloc(p, sizeof(double *));
for(i=0;i<p;i++) XT[i] = (double *)calloc(n,sizeof(double));
gram = (double **)calloc(p, sizeof(double *));
for(i=0;i<p;i++) gram[i] = (double *)calloc(p,sizeof(double));
TG = (double **)calloc(p, sizeof(double *));
for(i=0;i<p;i++) TG[i] = (double *)calloc(p,sizeof(double));
TGI = (double **)calloc(p, sizeof(double *));
for(i=0;i<p;i++) TGI[i] = (double *)calloc(p,sizeof(double));
BT = (double **)calloc(p, sizeof(double *));
for(i=0;i<p;i++) BT[i] = (double *)calloc(n,sizeof(double));
neigh = (int **)calloc(p, sizeof(int *));
for(i=0;i<p;i++) neigh[i] = (int *)calloc(p,sizeof(int));
locs = (double *)calloc(p*p, sizeof(double));
kern = (double *)calloc(p, sizeof(double));
for(i=0;i<p;i++) kern[i] = alpha;
for(i=0;i<(p*p);i++) locs[i] = plocs[i];
numneigh = (int *)calloc(p, sizeof(int));
for(i=0;i<n;i++)
for(j=0;j<p;j++)
X[i][j] = Xpass[j*n + i];
for(i=0;i<nt;i++)
for(j=0;j<p;j++)
Xtest[i][j] = Xtestpass[j*nt + i];
for(i=0;i<n;i++)
for(j=0;j<p;j++)
XT[j][i] = X[i][j];
MatrixTMultiplyB(X,X,p,n,p,gram);
int step = 0;
int takestep;
double **tempdes;
double **tempdestest;
tempdes = (double **)calloc(n, sizeof(double *));
for(i=0;i<n;i++) tempdes[i] = (double *)calloc(p, sizeof(double));
tempdestest = (double **)calloc(nt, sizeof(double *));
for(i=0;i<nt;i++) tempdestest[i] = (double *)calloc(p, sizeof(double));
int *sequ;
sequ = (int *)calloc(p, sizeof(int));
for(i=0;i<p;i++) sequ[i] = -1;
double *corz;
corz = (double *)calloc(p, sizeof(double));
double *cres;
cres = (double *)calloc(n, sizeof(double));
double *crestest;
crestest = (double *)calloc(nt, sizeof(double));
double *beta;
beta = (double *)calloc(p, sizeof(double));
double deter;
for(i=0;i<n;i++) cres[i] = Y[i];
for(i=0;i<nt;i++) crestest[i] = Ytest[i];
int cutalg = p;
if(p > n) cutalg = n;
while(step < cutalg){
j=0;
for(i=0;i<p;i++){
if(inintvec(step, i, sequ) == 0){
corz[i] = kern[i] * fabs(corel(n, cres, XT[i]));
}
else{
corz[i] = 0.0;
}
}
takestep = whichmax(p,corz);
if(step == 0) takestep = 0;
if(verbose == 1){
Rprintf(" [%d : %d | %d] ", step, n, p);
Rprintf("STEP: %d",takestep+1);
}
sequ[step] = takestep;
psequ[step] = takestep;
if(step > 0){
for(i=0;i<p;i++)
if(kern[i] != alpha && step >0)
kern[i] = (kern[i] - alpha)*(dstep-1.0)/(dstep) + alpha;
//for(i=0;i<p;i++) kern[i] += (1.0-alpha)*depan(fabs(locs[i] - locs[sequ[step]])/h)/dstep;
for(i=0;i<p;i++)
kern[i] += (1.0-alpha)*depan(locs[sequ[step]*p + i]/h)/dstep;
/*
for(i=0;i<p;i++)
Rprintf("(%g)",kern[i]);
*/
}
for(i=0;i<n;i++)
tempdes[i][step] = X[i][sequ[step]];
for(i=0;i<nt;i++)
tempdestest[i][step] = Xtest[i][sequ[step]];
for(i=0;i<(step+1);i++)
for(j=0;j<(step+1);j++)
TG[i][j] = gram[sequ[i]][sequ[j]];
MatInvF(TG,step+1,TGI,pcode);
if(verbose == 1) Rprintf(" [I: %d]\n",pcode[0]);
if(step + 1 == 1) TGI[0][0] = 1.0 / TG[0][0];
MatrixTMultiplyA(TGI,tempdes,step+1,step+1,n,BT);
for(i=0;i<(step+1);i++){
beta[i] = inner(BT[i],Y,n);
}
for(i=0;i<n;i++)
cres[i] = Y[i] - inner(tempdes[i],beta,step+1);
for(i=0;i<nt;i++)
crestest[i] = Ytest[i] - inner(tempdestest[i],beta,step+1);
for(i=0;i<nt;i++)
psse[step] += crestest[i]*crestest[i];
psse[step] /= nt;
step ++;
dstep = dstep + 1.0;
}
for(i=0;i<n;i++) free(X[i]);
free(X);
for(i=0;i<p;i++) free(XT[i]);
free(XT);
for(i=0;i<nt;i++) free(Xtest[i]);
free(Xtest);
for(i=0;i<p;i++) free(gram[i]);
free(gram);
for(i=0;i<p;i++) free(TG[i]);
free(TG);
for(i=0;i<p;i++) free(TGI[i]);
free(TGI);
for(i=0;i<p;i++) free(BT[i]);
free(BT);
for(i=0;i<p;i++) free(neigh[i]);
free(neigh);
for(i=0;i<n;i++) free(tempdes[i]);
free(tempdes);
for(i=0;i<nt;i++) free(tempdestest[i]);
free(tempdestest);
free(locs);
free(kern);
free(numneigh);
free(sequ);
free(corz);
free(cres);
free(crestest);
free(beta);
}
