/* Generalization Error with Resampling method.
 */
//gcc -Wall -g  -lm  -lgsl -lgslcblas rsm4ge.c   -o rsm4ge
//make rsm4ge
//Usage:rsm4ge data/11train1e3.dat 10 1 data/11test101x101.dat
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <dirent.h>
//#define nNmax 100
#define square(x) ((x)*(x))
#define buffsize 1024
#define linesize 10000
#define nRSmax 2
#define NORS2 -1
#define DIRECT -1
#define LEAVEMANYOUT 0
#define MULTIFOLD 1
#define BOOTSTRAP 2
char *methodname1[4]={"Direct","LeaveManyOut","MultiFold","BootStrap"};
//int *NN;
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix_double.h>
#include "my_misc.c"

void usage(int argc,char **argv){
  //  fprintf(stderr,"Usage: %s <fn_given> <fn_valid> N:<N>:<nD>:<nN> m:<m>:<d>:<nm> [<options>]\n",argv[0]);
  fprintf(stderr,"Usage: %s <fn_given> <fn_valid> <N:<N1>:<NN>:<Nd> | N:<N1>-<N2>:<NStep>> <m:<m1>:<nm>:<md>|m:<m1>-<m2>:<md>> [<options>]\n",argv[0]);
  fprintf(stderr,
	  "fn_given              : file name of given data\n"
	  "<fn_valid>            : the file name of validation data if exist (default /dev/null).\n"
	  "N:<N1>[:<NN>[:<ND>]]  : number of cells N=N1+ND*i for i=0,1,..,NN-1 .\n"
	  "N:<N1>-<N2>[:<ND>]]   : number of cells N=N1+ND*i for i=0,1,... until N<=N2.\n"
	  "m:<m1>:<md>:<nm>      : number of ensenble m=m1+nd*i for i=0,1,2,...,nm-1.\n"
	  "m:<m1>-<m2>:<md>      : number of ensenble m=m1+nd*i for i=0,1,2,... until m<=nd.\n"
	  "k:<k>                 : dimension of the input vector (defalut:2).\n"
	  "rs:<rsprog>           : executable program for resampling method(defalut:rs4irem).\n"
	  "<other options>       : see ensrs.c\n"
	  );
}
static char pr[buffsize];
int main(int argc,char **argv)
{
  char cmd[30000];
  char rdir[buffsize];
  char fngiven[1024];
  char fnvalid[1024];
  char fnplt[1033];
  char fnloss[1033];
  FILE *fp,*fploss;
  char *rdir0="./result-ensrs";
  //  char *rdir1="./result-ensrs/rsresults";
  int i,j,jj,ij;
  int K=2;        //dimension
  int *rsmo;
  char ensrsprog[256];
  int _N,_DN=1,_nN,*NN,_N1,_N2;
  int Ngs1=0,Ngs2=0;
  int _m,_dm,_nm,*mm=NULL,_m1,_m2;
  //  int _e,_de,_ne,*ee,_e1,_e2;
  double *LD,*Ltst,*Lvar,*Lvarval,*Lvar0;
  double *Lib,Libmin5,*Lemp,*H0,*L0,*L0v,*Ltr;
  int ij_LDmin=0;
  int ij_Ltstmin=0;
  int ij_Lvarmin=0;
  int ij_Lvarvalmin=0;
  DIR *pdir;
  //  int _N1,_N2,_m1,_m2;
  int intmode,bayesmode;
  double bayesopt1=0,bayesopt2=0,bayesopt3=0,bayesopt4=0;
  int m_LD=0;
  //  char bagalpha[10]={0,};
  int BAGGING=0,rs_method=-1,rs_seed=10,rs_ens=1;
  //  double rs_alpha=0,kc=2;//kc=2:coverage factor for 95% extended uncertainty
  double rs_alpha=0;//kc=2:coverage factor for 95% extended uncertainty
  double rs_beta=0;
  double *rs_Alpha=NULL;
  char *rsa;
  //  sprintf(ensrsprog,"rs4irem");
  int t_boost=-1;
  //  double rethresh_boost=12.0;
  int dsp=1;
  int DISP=1;
#define NoBoost 0
#define EmBoost 1
#define GbBoost 2
  int Boost=NoBoost;
  sprintf(ensrsprog,"ensrs");

  if(argc<5){
    usage(argc,argv);
    return(-1);
  }

  sprintf(fngiven,"%s",argv[1]);
  sprintf(fnvalid,"%s",argv[2]);

  rsmo=(int *)malloc(sizeof(int)*argc);
  for(i=3;i<argc;i++) rsmo[i]=1;

  for(i=3;i<argc;i++){
    if(strncmp(argv[i],"ib:",3)==0){
      sscanf(&(argv[i][3]),"%d:%d:%lf:%lf:%lf:%lf",&intmode,&bayesmode,&bayesopt1,&bayesopt2,&bayesopt3,&bayesopt4);
      rsmo[i]=1;
      m_LD=bayesopt2;
    }
  }
  for(i=3;i<argc;i++){
    if(strncmp(argv[i],"N:",2)==0){
      char *p;
      int ptn=0;
      for(p=&argv[i][2];;){
	if(*p==0) break;
	if(ptn==0){//unknown pattern
	  if(*p==':') {ptn=1;break;}
	  if(*p=='-') {ptn=2;break;}
	}
	p++;
      }
      if(ptn==1) {sscanf(&argv[i][2],"%d:%d:%d",&_N1,&_DN,&_nN);_N2=_N1+(_nN-1)*_DN;}
      else if(ptn==2) sscanf(&argv[i][2],"%d-%d:%d",&_N1,&_N2,&_DN);
      else _N1=_N2=_DN=1;
      if(_DN<=0) _DN=1;
      for(_nN=0,_N=_N1;_N<=_N2;_N+=_DN) _nN++;
      if(_nN<1) _nN=1;
      NN=(int *)malloc(sizeof(int)*(_nN+1));
      for(jj=0;jj<_nN;jj++) NN[jj]=_N1+_DN*jj;
      NN[_nN]=0;
      rsmo[i]=0;
    }
    else if(strncmp(argv[i],"NN:",3)==0){
      char *p;
      _nN=0;
      for(p=&argv[i][3];;){
	if(*p==0) break;
	if(*p==':') _nN++;
	p++;
      }
      _nN++;
      NN=(int *)malloc(sizeof(int)*(_nN+1));
      for(p=&argv[i][3],_nN=0;;){
	sscanf(p,"%d",&NN[_nN++]);
	for(;;) if(*p==':'|| *p=='-' || *p==0) break; else p++;
	if(*p==0) break;
	if(*p=='-'){
	  int _n,j;
	  sscanf(++p,"%d",&_n);
	  for(j=NN[_nN-1]+1;j<=_n;j++) NN[_nN++]=j;
	  for(;;) if(*p==':'|| *p=='-' || *p==0) break; else p++;
	  if(*p==0) break;
	}
	p++;
      }
      NN[_nN]=0;
      rsmo[i]=0;
    }
    else if(strncmp(argv[i],"Ngs:",4)==0){
      sscanf(&argv[i][4],"%d-%d",&Ngs1,&Ngs2);
      _nN=Ngs2-Ngs1;
      NN=(int *)malloc(sizeof(int)*(_nN+1));
      NN[0]=Ngs1;
      NN[1]=Ngs2;
      rsmo[i]=0;
    }
    else if(strncmp(argv[i],"m:",2)==0){
      char *p;
      int ptn=0;
      for(p=&argv[i][2];;){
	if(*p==0) break;
	if(ptn==0){//unknown pattern
	  if(*p==':') {ptn=1;break;}
	  if(*p=='-') {ptn=2;break;}
	}
	p++;
      }
      if(ptn==1) {sscanf(&argv[i][2],"%d:%d:%d",&_m1,&_dm,&_nm);_m2=_m1+(_nm-1)*_dm;}
      else if(ptn==2) sscanf(&argv[i][2],"%d-%d:%d",&_m1,&_m2,&_dm);
      else _m1=_m2=_nm=1;
      //      sscanf(&argv[i][2],"%d:%d:%d",&_m1,&_m2,&_dm);
      //if(_m1<3) _m1=3;
      if(_dm<1) _dm=1;
      if(_m1<m_LD*2+1) _m1=m_LD*2+1;
      for(_nm=0,_m=_m1;_m<=_m2;_m++) _nm++;
      mm=(int *)malloc(sizeof(int)*(_nm+1));
      for(jj=0;jj<_nm;jj++) mm[jj]=_m1+_dm*jj;
      mm[_nm]=0;
      rsmo[i]=0;
    }
    else if(strncmp(argv[i],"M:",2)==0){
      char *p;
      int ptn=0;
      for(p=&argv[i][2];;){
	if(*p==0) break;
	if(ptn==0){//unknown pattern
	  if(*p==':') {ptn=1;break;}
	  if(*p=='-') {ptn=2;break;}
	}
	p++;
      }
      if(ptn==1) {sscanf(&argv[i][2],"%d:%d:%d",&_m1,&_dm,&_nm);_m2=_m1+(_nm-1)*_dm;}
      else if(ptn==2) sscanf(&argv[i][2],"%d-%d:%d",&_m1,&_m2,&_dm);
      else _m1=_m2=_nm=1;
      if(_m1<1) _m1=1;
      if(_dm<1) _dm=1;
      for(_nm=0,_m=_m1;_m<=_m2;_m+=_dm) _nm++;
      mm=(int *)malloc(sizeof(int)*(_nm+1));
      for(jj=0;jj<_nm;jj++) mm[jj]=_m1+_dm*jj;
      mm[_nm]=0;
      rsmo[i]=0;
    }
    else if(strncmp(argv[i],"k:",2)==0) sscanf(&argv[i][2],"%d",&K);
    else if(strncmp(argv[i],"rs:",3)==0) {sprintf(ensrsprog,"%s",&argv[i][3]);rsmo[i]=0;}
    else if(strncmp(argv[i],"bga:",4)==0){
      //      sprintf(bagalpha,"%s",&argv[i][4]);rsmo[i]=0;
      sscanf(&argv[i][4],"%lf",&rs_alpha);
      rs_method=2;
      rsmo[i]=0;
      rs_seed=10;
      rs_ens=1;
    }//bootstrap aggregation
    else if(strncmp(argv[i],"rsa:",4)==0){
      //      sscanf(&argv[i][4],"%d:%lf:%d",&rs_method,&rs_alpha,&rs_seed);
      int ncol=0;
      char *p=&argv[i][4];
      for(;*p!=0;p++) if(*p==':') ncol++;
      rsa=&argv[i][4];
      if(ncol==2) {sscanf(rsa,"%d:%lf:%d",&rs_method,&rs_alpha,&rs_seed);rs_ens=1;}
      else if(ncol==3) sscanf(rsa,"%d:%lf:%d:%d",&rs_method,&rs_alpha,&rs_seed,&rs_ens);
      else if(ncol==4) sscanf(rsa,"%d:%lf:%d:%d:%lf",&rs_method,&rs_alpha,&rs_seed,&rs_ens,&rs_beta);
      BAGGING=1;//bagging with validfile
      rsmo[i]=0;
    }//resampling aggregation
    else if(strncmp(argv[i],"dsp:",4)==0){
      sscanf(&argv[i][4],"%d",&dsp);
      rsmo[i]=0;
    }
    else if(strncmp(argv[i],"DISP:",5)==0){
      sscanf(&argv[i][5],"%d",&DISP);
      //      rsmo[i]=0;
    }
    else if(strncmp(argv[i],"bst:",4)==0){
      char *p;
      p=&argv[i][4];
      for(j=0;;p++) {
	if(*p==0) break;
	if(*p==':') if(++j>=1) break;
      }
      //      if(j>=1) {sscanf(&argv[i][4],"%d:%lf",&t_boost,&rethresh_boost);}
      if(j>=1) {sscanf(&argv[i][4],"%d:%d",&t_boost,&Boost);}
      else {sscanf(&argv[i][4],"%d",&t_boost);}
      //      sscanf(&argv[i][4],"%d",&t_boost);
      rsmo[i]=0;
    }
  }

  if(rs_method>=0 && strncmp(fnvalid,"/dev/null",9)==0) BAGGING=2;//bagging without validfile
  if(mm==NULL){mm=(int *)malloc(sizeof(int)*(2)); mm[0]=1; mm[1]=0; _nm=1;}//??
  LD   =(double *)malloc(sizeof(double)*_nN*_nm);
  Ltst   =(double *)malloc(sizeof(double)*_nN*_nm);
  Lvar   =(double *)malloc(sizeof(double)*_nN*_nm);
  Lvar0  =(double *)malloc(sizeof(double)*_nN*_nm);
  Lvarval=(double *)malloc(sizeof(double)*_nN*_nm);
  Lib =(double *)malloc(sizeof(double)*_nN*_nm);
  Lemp=(double *)malloc(sizeof(double)*_nN*_nm);
  H0     =(double *)malloc(sizeof(double)*_nN*_nm);//Entropy
  int *s_Ltstmin=(int*)malloc(sizeof(int)*_nN*_nm);
  int smax_Ltstmin=_nN*_nm;
  //  if(smax_Ltstmin>50) smax_Ltstmin=50;
  if(smax_Ltstmin>5) smax_Ltstmin=5;
  L0     =(double *)malloc(sizeof(double)*_nN*_nm);//OriginalLoss
  L0v    =(double *)malloc(sizeof(double)*_nN*_nm);//OriginalLoss - Lob
  Ltr    =(double *)malloc(sizeof(double)*_nN*_nm);//OriginalLoss - Ltr
  int *s_L0vmin=(int*)malloc(sizeof(int)*_nN*_nm);
  int smax_L0vmin=1;
  sprintf(rdir,"%s",rdir0);
  if((pdir=opendir(rdir))==NULL) {sprintf(cmd,"mkdir %s",rdir);system(cmd);}  else closedir(pdir);
  sprintf(rdir,"%s/tmp",rdir0);
  if((pdir=opendir(rdir))==NULL) {sprintf(cmd,"mkdir %s",rdir);system(cmd);} else closedir(pdir);
  { //dir to store the result
    sprintf(rdir,"%s",rdir0);
    if((pdir=opendir(rdir))==NULL) {sprintf(cmd,"mkdir %s",rdir);system(cmd);}  else closedir(pdir);
    sprintf(&rdir[strlen(rdir)],"/%s",fnbody(fngiven,pr));//
    sprintf(&rdir[strlen(rdir)],"+%s",fnbody(fnvalid,pr));//
    for(i=2;i<argc;i++){
      sprintf(&rdir[strlen(rdir)],"%s",argv[i]);
    }
    for(i=strlen(rdir0)+1;rdir[i]!=0;i++) if(rdir[i]=='/') rdir[i]='@';
    rdir[240]=0;
    if((pdir=opendir(rdir))==NULL) {sprintf(cmd,"mkdir %s",rdir);system(cmd);}
    else closedir(pdir);
  }
  sprintf(fnloss,"%s/loss.dat",rdir);
  sprintf(fnplt ,"%s/loss.plt",rdir);
  int nGivenData;
  ///////////For Golden Section Method
  const double r = 2. / (3. + sqrt(5.));//0.381966
  double c, d, fc, fd, t,*pfcd;
  double a, b, fa, fb;
  char *fn_predall="predict+all.dat";
  int n_predall=0;
  //  if((fploss=fopen(fnloss,"r"))==NULL){
  if(1==1 || (fploss=fopen(fnloss,"r"))==NULL){
    for(i=0;i<_nN;i++){
      for(j=0;j<_nm;j++){
	ij=i*_nm+j;
	if(rs_method<0){//for specified valid-file
	  sprintf(cmd,"%s %s -1:%s %d-%d:%d ",ensrsprog,fngiven,fnvalid,NN[i]-m_LD*_DN,NN[i]+(mm[j]-1+m_LD)*_DN,_DN);
	}
	else{//for bagging or unspecified valid-file
	  if(rs_beta>1){
	    if(rs_Alpha==NULL){
	      rs_Alpha=(double *)malloc(sizeof(double)*(_nN+1));
	      FILE *fp=fopen(fngiven,"r");
	      char line[10240];
	      for(nGivenData=0;;nGivenData++){
		fgets(line,10240,fp);
		if(feof(fp)) break;
	      }
	      fclose(fp);
	    }
	    rs_Alpha[i]=-log(NN[i]*(K+1)*rs_beta/nGivenData);
	    if(rs_Alpha[i]>2.3) rs_Alpha[i]=2.3;
	    rs_alpha=rs_Alpha[i];
	  }

	  if(t_boost==-1 || Boost==NoBoost){//default for resampling = Bagging-Only
	    //	    sprintf(cmd,"%s %s %d:%d:%.2f:%d %d-%d:%d bg:%s ",ensrsprog,fngiven,rs_method,rs_ens,rs_Alpha[i],rs_seed,NN[i],NN[i]+(mm[j]-1)*_DN,1,fnvalid);//for ensrs060609.c
	    sprintf(cmd,"%s %s %d:%d:%g:%d %d-%d:%d bg:%s ",ensrsprog,fngiven,rs_method,rs_ens,rs_alpha,rs_seed,NN[i],NN[i]+(mm[j]-1)*_DN,1,fnvalid);//for ensrs060609.c
	    //sprintf(cmd,"%s %s %d:%d:%.2f:%d %d-%d:%d bg:%s ",ensrsprog,fngiven,rs_method,rs_ens,rs_alpha,rs_seed,NN[i]-m_LD*_DN,NN[i]+(mm[j]-1+m_LD)*_DN,1,fnvalid);
	    //sprintf(cmd,"%s %s %d:%d:%.2f:%d %d-%d:%d bg:%s ",ensrsprog,fngiven,rs_method,mm[j],rs_alpha,rs_seed,NN[i],NN[i],1,fnvalid);//for ensrs060608.c
	  }
	  else if(Boost==GbBoost){//Gradient-based boosting
	    int t;
	    for(t=1;t<=t_boost;t++){
	      //	      sprintf(cmd,"%s %s %d:%d:%.2f:%d %d-%d:%d bg:%s bst:%d:%d ",ensrsprog,fngiven,rs_method,rs_ens,rs_Alpha[i],rs_seed,NN[i],NN[i]+(mm[j]-1)*_DN,1,fnvalid,t,Boost);//for ensrs060907.c
	      sprintf(cmd,"%s %s %d:%d:%g:%d %d-%d:%d bg:%s bst:%d:%d ",ensrsprog,fngiven,rs_method,rs_ens,rs_alpha,rs_seed,NN[i],NN[i]+(mm[j]-1)*_DN,1,fnvalid,t,Boost);//for ensrs060907.c
	      if(t==t_boost) break;
	      for(jj=3;jj<argc;jj++) if(rsmo[jj]) sprintf(&cmd[strlen(cmd)],"%s ",argv[jj]);
	      sprintf(&cmd[strlen(cmd)],"> /dev/null");
	      fprintf(stderr,"! Executing '%s'.\n",cmd);
	      system(cmd);
	    }
	  }
	  else if(Boost==EmBoost){
	    //	    sprintf(cmd,"%s %s %d:%d:%.2f:%d %d-%d:%d bg:%s bst:%d:%d ",ensrsprog,fngiven,rs_method,rs_ens,rs_Alpha[i],rs_seed,NN[i],NN[i]+(mm[j]-1)*_DN,1,fnvalid,t_boost,Boost);//for ensrs060907.c
	    sprintf(cmd,"%s %s %d:%d:%g:%d %d-%d:%d bg:%s bst:%d:%d ",ensrsprog,fngiven,rs_method,rs_ens,rs_alpha,rs_seed,NN[i],NN[i]+(mm[j]-1)*_DN,1,fnvalid,t_boost,Boost);//for ensrs060907.c
	  }
	}//elsee{//for bagging or unspecified valid-file
	//	sprintf(cmd,"%s %s -1:%s %d-%d:%d ",ensrsprog,fngiven,fnvalid,NN[i]-mm[j],NN[i]+mm[j],_d);
	//	sprintf(cmd,"%s %s -1:%s %d-%d:%d ",ensrsprog,fngiven,fnvalid,NN[i],NN[i]+mm[j]+2*m_LD,_dm);
	//	sprintf(cmd,"%s %s -1:%s %d-%d:%d ",ensrsprog,fngiven,fnvalid,NN[i],NN[i]+mm[j]*_DN,_DN);
	//	sprintf(cmd,"%s %s -1:%s %d-%d:%d ",ensrsprog,fngiven,fnvalid,NN[i],NN[i]+mm[j],_dm);
	//	sprintf(cmd,"%s %s -1:%s %d:%d:%d ",ensrsprog,fngiven,fnvalid,NN[i],mm[j],_dm);
	//	sprintf(cmd,"%s %s -1:%s %d-%d ",ensrsprog,fngiven,fnvalid,NN[i],NN[i]+mm[j]-1);
	for(jj=3;jj<argc;jj++) if(rsmo[jj]) sprintf(&cmd[strlen(cmd)],"%s ",argv[jj]);
	sprintf(&cmd[strlen(cmd)],"> /dev/null");
	fprintf(stderr,"Executing '%s'.\n",cmd);
	if(system(cmd)==-1){
	  fprintf(stderr,"Error when executing '%s'.\n",cmd);
	  return(-1);
	}
#ifdef PREDALL
	{
	  if(n_predall==0){
	    sprintf(cmd,"cat predict+.dat|awk '{print $2,$3,$1;}'> %s",fn_predall);
	    fprintf(stderr,"#executing %s\n",cmd);
	    system(cmd);
	  }
	  else{
	    sprintf(cmd,"cp %s p1.dat;cat predict+.dat |awk '{print $1}'| paste p1.dat - > %s",fn_predall,fn_predall);
	    fprintf(stderr,"#executing %s\n",cmd);
	    system(cmd);
	  }
	  n_predall++;
	}
#endif//#ifdef PREDALL
	double *func=Ltst;
	//double *func=LD;//check LD/N for estimating bias
	{
	  int _nfiles,_num;
	  fp=fopen("./loss+.dat","r");
	  fscanf(fp,"%lf %lf %lf %lf %lf %d %d %lf %lf %lf %lf %lf",&LD[ij],&Ltst[ij],&Lvar[ij],&Lvarval[ij],&Lvar0[ij],&_nfiles,&_num,&Lib[ij],&Lemp[ij],&H0[ij],&L0[ij],&Ltr[ij]);
	  L0v[ij]=L0[ij]-Lvar[ij];
	  //double tmp=Ltst[ij]; Ltst[ij]=LD[ij];LD[ij]=tmp;//check LD/N for estimating bias
	  //double tmp=Ltst[ij]; Ltst[ij]=LD[ij]/(NN[i]);LD[ij]=tmp;//check LD/N for estimating bias
	  //	  double tmp=Ltst[ij]; Ltst[ij]=LD[ij]/(NN[i]*NN[i]);LD[ij]=tmp;//check LD/N for estimating bias
	  //	  LD[ij]/=(NN[i]*NN[i]);//checkLD/N//check LD/N for estimating bias
	  //	  LD[ij]/=NN[i];//checkLD/N//check LD/N for estimating bias
	  //	  double tmp=Ltst[ij]; Ltst[ij]=LD[ij]/NN[i];LD[ij]=tmp;//check LD/N for estimating bias
	  //	  double tmp=Ltst[ij]; Ltst[ij]=-LD[ij]/NN[i];LD[ij]=tmp;//check LD/N for estimating bias
	  fclose(fp);
	}
	if(Ngs1!=0){//golden section method searches Ltst
	  if(i==0){a=NN[i];fa=func[ij];}	  //	  if(i==0){a=NN[i];fa=Ltst[ij];}
	  else if(i==1){
	    b=NN[i];
	    fb=func[ij]; // fb=Ltst[ij];
	    t = r * (b - a);
	    c=a+t;
	    d=b-t;
	    NN[i+1]=(int)(c+.5);
	    NN[i+2]=(int)(d+.5);
	  }
	  else if(i==2){
	    fc=func[ij];//	    fc=Ltst[ij];
	    pfcd=&fd;
	    if((fa>=fc && fc>=fb) || (fa<=fc && fc<=fb)){
	      fprintf(stderr,"#(i%d)Ltst(%d)%g,Ltst(%d)%g,Ltst(%d)%g\n",i,(int)(a+.5),fa,(int)(c+.5),fc,(int)(b+.5),fb);
	      fprintf(stderr,"# Minimum of L may not be in the range Ngs:%d:%d !!!\n",Ngs1,Ngs2);
	      //	      exit(-1);
	    }
	  }
	  else if(i>=3){
	    *pfcd=func[ij];//	    *pfcd=Ltst[ij];
	    fprintf(stderr,"#(i%d)Ltst(%d)%g,Ltst(%d)%g,Ltst(%d)%g,Ltst(%d)%g \n",i,(int)(a+.5),fa,(int)(c+.5),fc,(int)(d+.5),fd,(int)(b+.5),fb);
	    if(i>=10 && (fc>func[1] &&(fd>func[1]))){      //	    if(i>=10 && (fc> Ltst[1]) &&(fd>Ltst[1])){
	      fprintf(stderr,"#(i%d)Ltst(%d)%g,Ltst(%d)%g,Ltst(%d)%g\n",i,(int)(c+.5),fc,(int)(b+.5),fb,NN[1],Ltst[1]);
	      fprintf(stderr,"# Minimum of L may not be in the range Ngs:%d:%d !!!\n",Ngs1,Ngs2);
	      //	      exit(-1);
	    }
	    if(fc>fd){
	      a = c;  c = d;  fc = fd;  d = b - r * (b - a);
	      if (d - c < 0.5) {
		_nN=i+1;
		break;
	      }
	      NN[i+1]=(int)(d+.5);
	      pfcd=&fd;
	      //	      fd = f(d,par);
	    } else {
	      b = d;  d = c;  fd = fc;  c = a + r * (b - a);
	      if (d - c < 0.5) {
		_nN=i+1;
		break;
	      }
	      NN[i+1]=(int)(c+.5);
	      pfcd=&fc;
	      //	      fc = f(c,par);
	    }
	  }
	}
	//	{//060517
	//	  double _msemean,_msestdp,_msestdm;
	//	  fp=fopen("./tmp/rsresult.dat","r");
	//	  fscanf(fp,"%lf %lf %lf %lf",&_msemean,&_msestdp,&_msestdm,&Lib[ij]);
	//	  fclose(fp);
	//	}
      }
    }
    fploss=fopen(fnloss,"w");
    for(i=0;i<_nN;i++){
      for(j=0;j<_nm;j++){
	ij=i*_nm+j;
	fprintf(fploss,"%d %d %13.7e %13.7e %13.7e %13.7e %13.7e %13.7e %.7e %.7e %.7e#N m LD Ltst Lvar Lvarval Lvar0\n",
		NN[i],mm[j],LD[ij],Ltst[ij],Lvar[ij],Lvarval[ij],Lvar0[ij],Lib[ij],Lemp[ij],H0[ij],L0[ij]);
      }
      fprintf(fploss,"\n");
    }
    fclose(fploss);
    if(Ngs1!=0){
      sprintf(cmd,"sort -g %s >tmploss.dat; mv tmploss.dat %s",fnloss,fnloss);
      system(cmd);
    }
    fploss=fopen(fnloss,"r");
  }//closing  if((fploss=fopen(fnloss,"r"))==NULL){
#define FINISH
#ifdef FINISH
#ifdef PREDALL
  if((DISP&0x02)==0x02){
    sprintf(fnplt ,"dispypall.plt");
    fp=fopen(fnplt,"w");
    fprintf(fp,"set style data lines\n");
    fprintf(fp,"plot \"%s\" using 1:2 w lp t \"y\"",fn_predall);
    int m;
    for(m=0;m<n_predall;m++) fprintf(fp,", \"\" using 1:%d t \"y^N%d\"",3+m,NN[m/_nm]);
    fprintf(fp,"\npause -1 \"hit return to quit.\"\n");
    fclose(fp);
    sprintf(cmd,"xterm -geometry 50x5+0-0 -e gnuplot -geometry 320x180+0+0 %s",fnplt);
    fprintf(stderr,"Execute '%s'.\n",cmd);
    sprintf(&cmd[strlen(cmd)],"&");
    system(cmd);
  }
#endif//#ifdef PREDALL
  return(0);
}
#else

  {
    char buff[buffsize];
    //    int _NN,_mm;
    int _mm;
    ij_LDmin=ij_Ltstmin=ij_Lvarmin=ij_Lvarvalmin=0;
    for(i=0;i<_nN;i++){
      for(j=0;j<_nm;j++){
	ij=i*_nm+j;
	for(;;){
	  fgets(buff,buffsize,fploss);
	  if(feof(fploss)) break;
	  if(strlen(buff)>1) break;
	}
	sscanf(buff,"%d %d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
	       &NN[i],&_mm,&LD[ij],&Ltst[ij],&Lvar[ij],&Lvarval[ij],&Lvar0[ij],&Lib[ij],&Lemp[ij],&H0[ij],&L0[ij],&Ltr[ij]);
	L0v[ij]=L0[ij]-Lvar[ij];
	//	       &_NN,&_mm,&LD[ij],&Ltst[ij],&Lvar[ij],&Lvarval[ij],&Lvar0[ij],&Lib[ij],&Lemp[ij],&H0[ij]);
	//	Lvar[ij]*=(mm[j]/(mm[j]-1+1e-3));
	//	Lvar0[ij]*=(mm[j]/(mm[j]-1+1e-3));
	//		 &NN[i],&mm[j],&LD[ij],&Ltst[ij],&Lvar[ij],&Lvarval[ij]);
	//	if(_mm!=mm[j]){
	//	  fprintf(stderr,"m=%d,%dhen\n",mm[j],_mm);
	//	}
	//	if(_NN!=NN[i]){
	//	  fprintf(stderr,"N=%d,%dhen\n",NN[i],_NN);
	//	}
	{
	  //	  if(LD[ij]<1e-20){  fprintf(stderr,"hen\n");	  }
	  if(LD[ij]<LD[ij_LDmin]) ij_LDmin=ij;
	  if(Ltst[ij]<Ltst[ij_Ltstmin]) ij_Ltstmin=ij;
	  if(Lvar[ij]<Lvar[ij_Lvarmin]) ij_Lvarmin=ij;
	  if(Lvarval[ij]<Lvarval[ij_Lvarmin]) ij_Lvarvalmin=ij;
	  //	    if(LD[ij]>1e-20    && LD[ij]<LD[ij_LDmin]) ij_LDmin=ij;
	  //	    if(Ltst[ij]>1e-20    && Ltst[ij]<Ltst[ij_Ltstmin]) ij_Ltstmin=ij;
	  //	    if(Lvar[ij]>1e-20    && Lvar[ij]<Lvar[ij_Lvarmin]) ij_Lvarmin=ij;
	  //	    if(Lvarval[ij]>1e-20 && Lvarval[ij]<Lvarval[ij_Lvarmin]) ij_Lvarvalmin=ij;
	}
      }
    }
    fclose(fploss);
  }
  double Libr=2.0;
  Libmin5=1e30;
  for(i=0;i<_nN*_nm;i++) {
    s_Ltstmin[i]=i;
    //    if(Libmin5>Lib[i]) Libmin5=Lib[i];
  }
  {
    //    Libmin5*=2.;
    for(i=0;i<smax_Ltstmin;i++){
      for(j=i+1;j<_nN*_nm;j++){
	if(Ltst[s_Ltstmin[j]]<Ltst[s_Ltstmin[i]]){
	  int itmp=s_Ltstmin[i];
	  s_Ltstmin[i]=s_Ltstmin[j];
	  s_Ltstmin[j]=itmp;
	}
      }
    }
  }
  {
    //    Libmin5*=2.;
    for(i=0;i<smax_L0vmin;i++){
      for(j=i+1;j<_nN*_nm;j++){
	if(L0v[s_L0vmin[j]]<L0v[s_L0vmin[i]]){
	  int itmp=s_L0vmin[i];
	  s_L0vmin[i]=s_L0vmin[j];
	  s_L0vmin[j]=itmp;
	}
      }
    }
  }

  {
    //for local min
    int *ss,itmp,n=_nN*_nm,ns=10,is;
    int n0,m0,n1,n2,i0,j0,ii,jj;
    int localmin;
    ss=(int *)malloc(sizeof(int)*n);
    //Local min of LD
    {
      fprintf(stderr,"##Increasing Lib (Check Overfitting)\n");
      ns=3;
      for(i=0;i<n;i++) ss[i]=i;
      is=0;
      for(i=0;i<n;i++){
	for(j=i+1;j<n;j++){
	  if(Lib[ss[i]]>Lib[ss[j]]){//Increasing
	    //	  if(LD[ss[i]]<LD[ss[j]]){//Decreasing
	    itmp=ss[i];
	    ss[i]=ss[j];
	    ss[j]=itmp;
	  }
	}
	//	if(Lib[ss[i]]>Libmin5) continue;
	//	if(Lvar[ss[i]]<1e-20) continue;
	//	i0=ss[i]/_nm;j0=ss[i]%_nm;
	localmin=1;
	//	for(ii=i0-1;ii<=i0+1;ii++){
	//	  for(jj=j0-1;jj<=j0+1;jj++){
	//	    if(ii<0) continue;
	//	    if(ii>_nN-1) continue;
	//	    if(jj<0) continue;
	//	    if(jj>_nm-1) continue;
	//	    if(LD[ii*_nm+jj]<LD[ss[i]]) {localmin=0;break;}
	//	  }
	//	}
	if(localmin==1){
	  is++;
	  if(is<=ns){
	    //	    n0=NN[ss[i]/_nm];m0=mm[ss[i]%_nm];n1=n0;n2=n0+m0-1;//n1=n0+1;n2=n0+m0-2;
	    //	    fprintf(stderr,"%3d:%3d(N:%3d-%3d)LD=%.3e Lvar=%.3e Lvar0=%.3e Ltst=%.3e Lib%d=%.3e",n0,m0,n1,n2,LD[ss[i]],Lvar[ss[i]],Lvar0[ss[i]],Ltst[ss[i]],i,Lib[ss[i]]);
	    //	    //	    fprintf(stderr,"**N:%d-%d\n",(int)(n0+_DN*m_LD),(int)(n2=n0+m0-1-_DN*m_LD));
	    //	    //	    fprintf(stderr,"**N:%d-%d\n",(int)(n0+_DN*m_LD),(int)(n2=n0+m0-1-_DN*m_LD));
	    //	    fprintf(stderr,"**N:%d-%d\n",(int)(n0+_DN*m_LD),(int)(n0+m0-1-_DN*m_LD));

	    n0=NN[ss[i]/_nm];m0=mm[ss[i]%_nm];n1=n0;n2=n0+(m0-1)*_DN;
	    fprintf(stderr,"LD=%.3e Lvar=%.3e Lvar0=%.3e Lib%d=%.3e Ltst=%.3e N:%d-:%d M:%d\n",LD[ss[i]],Lvar[ss[i]],Lvar0[ss[i]],i,Lib[ss[i]],Ltst[ss[i]],n1,_DN,m0);

	    //	    fprintf(stderr,"%3d:%3d(N:%3d-%3d)LD=%.3e Lvar=%.3e Lvar0=%.3e Ltst=%.3e Lib%d=%.3e\n",n0,m0,n1,n2,LD[ss[i]],Lvar[ss[i]],Lvar0[ss[i]],Ltst[ss[i]],i,Lib[ss[i]]);
	    //	    fprintf(stderr,"**N:%d-%d\n",(int)(n0+_DN*m_LD),(int)(n2=n0+m0-1-_DN*m_LD));
	    //	    fprintf(stderr,"**N:%d-%d\n",(int)(n0+_DN*m_LD),(int)(n2=n0+m0-1-_DN*m_LD));
	    //	    fprintf(stderr,"**N:%d-%d\n",(int)(n0+_DN*m_LD),(int)(n0+m0-1-_DN*m_LD));
	  }
	}
	//	if(is>=ns) break;
      }
    }
    if(!BAGGING){
      Libmin5=Lib[ss[0]];
      fprintf(stderr,"#Below, ignore Lib>(Libmin=%.3e)*%.2f=",Libmin5,Libr);
      Libmin5*=Libr;fprintf(stderr,"%e\n",Libmin5);
      {
	fprintf(stderr,"##Increasing LD with local min of LD\n");
	//      fprintf(stderr,"##Decreasing LD with local min of LD\n");
	ns=3;
	for(i=0;i<n;i++) ss[i]=i;
	is=0;
	for(i=0;i<n;i++){
	  for(j=i+1;j<n;j++){
	    if(LD[ss[i]]>LD[ss[j]]){//Increasing
	      //	  if(LD[ss[i]]<LD[ss[j]]){//Decreasing
	      itmp=ss[i];
	      ss[i]=ss[j];
	      ss[j]=itmp;
	    }
	  }
	  if(Lib[ss[i]]>Libmin5) continue;
	  if(Lvar[ss[i]]<1e-20) continue;
	  i0=ss[i]/_nm;j0=ss[i]%_nm;
	  localmin=1;
	  for(ii=i0-1;ii<=i0+1;ii++){
	    for(jj=j0-1;jj<=j0+1;jj++){
	      if(ii<0) continue;
	      if(ii>_nN-1) continue;
	      if(jj<0) continue;
	      if(jj>_nm-1) continue;
	      //	    if(ii<NN[0]) continue;
	      //	    if(ii>NN[_nN-1]) continue;
	      //	    if(jj<mm[0]) continue;
	      //	    if(jj>mm[_nm-1]) continue;
	      //
	      if(LD[ii*_nm+jj]<LD[ss[i]]) {localmin=0;break;}
	    }
	  }
	  if(localmin==1){
	    is++;
	    if(is<=ns){
	      //	    n0=NN[ss[i]/_nm];m0=mm[ss[i]%_nm];n1=n0;n2=n0+m0-1;//n1=n0+1;n2=n0+m0-2;
	      n0=NN[ss[i]/_nm];m0=mm[ss[i]%_nm];n1=n0;n2=n0+(m0-1)*_DN;
	      fprintf(stderr,"LDmin%3d=%.3e Lvar=%.3e Lvar0=%.3e Lib=%.3e Ltst=%.3e N:%d-:%d M:%d\n",i,LD[ss[i]],Lvar[ss[i]],Lvar0[ss[i]],Lib[ss[i]],Ltst[ss[i]],n1,_DN,m0);
	      //	    fprintf(stderr,"%3d:%3d(N:%3d-%3d)LDmin%3d=%.3e Lvar=%.3e Lvar0=%.3e Ltst=%.3e Lib=%.3e\n",n0,m0,n1,n2,i,LD[ss[i]],Lvar[ss[i]],Lvar0[ss[i]],Ltst[ss[i]],Lib[ss[i]]);
	      //	  fprintf(stderr,"%3d:%3d(N:%3d-%3d) LDmin%3d=%e Lvar=%e Ltst=%e\n",n0,m0,n1,n2,i,LD[ss[i]],Lvar[ss[i]],Ltst[ss[i]]);
	      //	  fprintf(stderr,"%3d:%3d(N:%3d-%3d) LDmin%3d=%e Lvar=%e Ltst=%e LD*Lvar=%e\n",n0,m0,n1,n2,i,LD[ss[i]],Lvar[ss[i]],Ltst[ss[i]],LD[ss[i]]*Lvar[ss[i]]);
	      //	    fprintf(stderr,"**N:%d-%d\n",(int)(n0+_DN*m_LD),(int)(n2=n0+m0-1-_DN*m_LD));
	      //	    fprintf(stderr,"**N:%d-%d\n",(int)(n0+_dm*m_LD),(int)(n2=n0+m0-1-_dm*m_LD));
	    }
	  }
	  //	if(is>=ns) break;
	}
	fprintf(stderr,"#TotalLocalmins of LD=%d(<%d)\n",is,n);
      }
      {
	double *_L  =(double *)malloc(sizeof(double)*_nN*_nm);
	fprintf(stderr,"##Increasing _L=(Lvar/Lvar0) with local min of LD\n");
	ns=3;
	for(i=0;i<n;i++){
	  //	_L[i]=Lemp[i]-Lvar0[i];
	  //_L[i]=Lib[i]-Lvar0[i];
	  //	_L[i]=Lib[i]+Lvar0[i];//bad
	  //	_L[i]=Lvar0[i];//bad
	  //	_L[i]=Lvar[i];//good
	  _L[i]=Lvar[i]/Lvar0[i];//NG for 2e3
	  //	_L[i]=Lvar[i]-Lvar0[i];//Good
	}

	for(i=0;i<n;i++) ss[i]=i;
	is=0;
	for(i=0;i<n;i++){
	  for(j=i+1;j<n;j++){
	    //	  if(_L[ss[i]]<_L[ss[j]]){
	    if(_L[ss[i]]>_L[ss[j]]){
	      itmp=ss[i];
	      ss[i]=ss[j];
	      ss[j]=itmp;
	    }
	  }
	  if(Lib[ss[i]]>Libmin5) continue;
	  if(Lvar[ss[i]]<1e-20) continue;
	  i0=ss[i]/_nm;j0=ss[i]%_nm;
	  localmin=1;
	  for(ii=i0-1;ii<=i0+1;ii++){
	    for(jj=j0-1;jj<=j0+1;jj++){
	      if(ii<0) continue;
	      if(ii>_nN-1) continue;
	      if(jj<0) continue;
	      if(jj>_nm-1) continue;
	      if(LD[ii*_nm+jj]<LD[ss[i]]) {localmin=0;break;}
	    }
	  }
	  //	localmin=1;
	  if(localmin==1){
	    is++;
	    if(is<=ns){
	      //	    n0=NN[ss[i]/_nm];m0=mm[ss[i]%_nm];n1=n0;n2=n0+m0-1;//n1=n0+1;n2=n0+m0-2;
	      n0=NN[ss[i]/_nm];m0=mm[ss[i]%_nm];n1=n0;n2=n0+(m0-1)*_DN;
	      fprintf(stderr,"_L%3d=%.3e Lvar=%.3e Lvar0=%.3e Lib=%.3e Ltst=%.3e N:%d-:%d M:%d\n",i,_L[ss[i]],Lvar[ss[i]],Lvar0[ss[i]],Lib[ss[i]],Ltst[ss[i]],n1,_DN,m0);
	      //	    fprintf(stderr,"%3d:%3d(N:%3d-%3d)_L%3d=%.3e Lvar=%.3e Lvar0=%.3e Ltst=%.3e Lib=%.3e",n0,m0,n1,n2,i,_L[ss[i]],Lvar[ss[i]],Lvar0[ss[i]],Ltst[ss[i]],Lib[ss[i]]);
	      //	    fprintf(stderr,"**N:%d-%d\n",(int)(n0+_DN*m_LD),(int)(n2=n0+m0-1-_DN*m_LD));
	      //	    fprintf(stderr,"**N:%d-%d\n",(int)(n0+_dm*m_LD),(int)(n2=n0+m0-1-_dm*m_LD));
	    }
	  }
	}
	free(_L);
      }
      {
	double *_LD2  =(double *)malloc(sizeof(double)*_nN*_nm);
	//      fprintf(stderr,"##Increasing LD with local min of LD\n");
	fprintf(stderr,"##Decreasing LD2 with local min of LD\n");
	ns=3;
	for(i=0;i<n;i++){
	  i0=i/_nm;j0=i%_nm;
	  ii=i0-1;if(ii<0) ii=0;
	  //	_LD2[i]=Lib[ii*_nm+j0]-Lib[i];//060529
	  //_LD2[i]=-Lib[ii*_nm+j0];//060529
	  //_LD2[i]=LD[i]-LD[ii*_nm+j0];//060529
	  //	_LD2[i]=LD[ii*_nm+j0]-LD[i];ii=i0+1;if(ii>=_nN) ii=_nN-1;_LD2[i] = (LD[ii*_nm+j0]-LD[i])/_LD2[i];//060529
	  _LD2[i]=LD[ii*_nm+j0]-LD[i];_LD2[i] = 1./_LD2[i];//060529
	}

	for(i=0;i<n;i++) ss[i]=i;
	is=0;
	for(i=0;i<n;i++){
	  for(j=i+1;j<n;j++){
	    if(_LD2[ss[i]]<_LD2[ss[j]]){
	      //	  if(_LD2[ss[i]]>_LD2[ss[j]]){
	      itmp=ss[i];
	      ss[i]=ss[j];
	      ss[j]=itmp;
	    }
	  }
	  if(Lib[ss[i]]>Libmin5) continue;
	  if(Lvar[ss[i]]<1e-20) continue;
	  i0=ss[i]/_nm;j0=ss[i]%_nm;
	  localmin=1;
	  for(ii=i0-1;ii<=i0+1;ii++){
	    for(jj=j0-1;jj<=j0+1;jj++){
	      if(ii<0) continue;
	      if(ii>_nN-1) continue;
	      if(jj<0) continue;
	      if(jj>_nm-1) continue;
	      //	    if(ii<NN[0]) continue;
	      //	    if(ii>NN[_nN-1]) continue;
	      //	    if(jj<mm[0]) continue;
	      //	    if(jj>mm[_nm-1]) continue;
	      //
	      if(LD[ii*_nm+jj]<LD[ss[i]]) {localmin=0;break;}
	    }
	  }
	  if(localmin==1){
	    is++;
	    if(is<=ns){
	      n0=NN[ss[i]/_nm];m0=mm[ss[i]%_nm];n1=n0;n2=n0+(m0-1)*_DN;
	      fprintf(stderr,"LD2max%3d=%.3e Lvar=%.3e Lvar0=%.3e Lib=%.3e Ltst=%.3e N:%d-:%d M:%d\n",i,_LD2[ss[i]],Lvar[ss[i]],Lvar0[ss[i]],Lib[ss[i]],Ltst[ss[i]],n1,_DN,m0);
	      //	    n0=NN[ss[i]/_nm];m0=mm[ss[i]%_nm];n1=n0;n2=n0+m0-1;//n1=n0+1;n2=n0+m0-2;
	      //	  fprintf(stderr,"%3d:%3d(N:%3d-%3d) LD2:%3d=%e Lvar=%e Ltst=%e\n",n0,m0,n1,n2,i,LD[ss[i]],Lvar[ss[i]],Ltst[ss[i]]);
	      //	  fprintf(stderr,"%3d:%3d(N:%3d-%3d) LD2:%3d=%e Lvar=%e Ltst=%e LD*Lvar=%e\n",n0,m0,n1,n2,i,LD[ss[i]],Lvar[ss[i]],Ltst[ss[i]],LD[ss[i]]*Lvar[ss[i]]);
	      //	    fprintf(stderr,"**N:%d-%d\n",(int)(n0+_DN*m_LD),(int)(n2=n0+m0-1-_DN*m_LD));
	      //	    fprintf(stderr,"**N:%d-%d\n",(int)(n0+_dm*m_LD),(int)(n2=n0+m0-1-_dm*m_LD));
	    }
	    if(is>=ns) break;
	  }
	}
	//      fprintf(stderr,"#TotalLocalmins of LD=%d(<%d)\n",is,n);
	free(_LD2);
      }
      //local min of Lvar
      {
	fprintf(stderr,"##Increasing Lvar with local min of Lvar\n");
	ns=3;
	for(i=0;i<n;i++) ss[i]=i;
	is=0;
	for(i=0;i<n;i++){
	  for(j=i+1;j<n;j++){
	    if(Lvar[ss[i]]>Lvar[ss[j]]){
	      //	  if(Lvar[ss[i]]<Lvar[ss[j]]){
	      itmp=ss[i];
	      ss[i]=ss[j];
	      ss[j]=itmp;
	    }
	  }
	  if(Lib[ss[i]]>Libmin5) continue;
	  if(Lvar[ss[i]]<1e-20) continue;
	  i0=ss[i]/_nm;j0=ss[i]%_nm;
	  localmin=1;
	  for(ii=i0-1;ii<=i0+1;ii++){
	    for(jj=j0-1;jj<=j0+1;jj++){
	      //	  fprintf(stderr,"ii,jj=%d:%d\n",ii,jj);
	      if(ii<0) continue;
	      if(ii>_nN-1) continue;
	      if(jj<0) continue;
	      if(jj>_nm-1) continue;
	      if(Lvar[ii*_nm+jj]<Lvar[ss[i]]) {localmin=0;break;}
	    }
	  }
	  if(localmin==1){
	    is++;
	    n0=NN[ss[i]/_nm];m0=mm[ss[i]%_nm];n1=n0;n2=n0+(m0-1)*_DN;
	    fprintf(stderr,"LD=%.3e Lvarmin%3d=%.3e Lvar0=%.3e Lib=%.3e Ltst=%.3e N:%d-:%d M:%d\n",LD[ss[i]],i,Lvar[ss[i]],Lvar0[ss[i]],Lib[ss[i]],Ltst[ss[i]],n1,_DN,m0);
	    //	  fprintf(stderr,"%3d:%3d(N:%3d-%3d) LD=%.3e Lvarmin%3d=%.3e Lvar0=%.3e Ltst=%.3e Lib=%.3e",n0,m0,n1,n2,LD[ss[i]],i,Lvar[ss[i]],Lvar0[ss[i]],Ltst[ss[i]],Lib[ss[i]]);
	    //	  n0=NN[ss[i]/_nm];m0=mm[ss[i]%_nm];n1=n0;n2=n0+m0-1;//n1=n0+1;n2=n0+m0-2;
	    //	  fprintf(stderr,"%3d:%3d(N:%3d-%3d) LD=%e Lvarmin%3d=%e Ltst=%e\n",n0,m0,n1,n2,LD[ss[i]],i,Lvar[ss[i]],Ltst[ss[i]]);
	    //	  fprintf(stderr,"**N:%d-%d\n",(int)(n0+_DN*m_LD),(int)(n2=n0+m0-1-_DN*m_LD));
	    //	  fprintf(stderr,"**N:%d-%d\n",(int)(n0+_dm*m_LD),(int)(n2=n0+m0-1-_dm*m_LD));
	    if(is>=ns) break;
	  }
	}
      }
      {
	//      fprintf(stderr,"##global Lvar max with local min\n");
	fprintf(stderr,"##Decreasing Lvar with local min of LD\n");
	ns=3;
	for(i=0;i<n;i++) ss[i]=i;
	is=0;
	for(i=0;i<n;i++){
	  for(j=i+1;j<n;j++){
	    //	  if(Lvar[ss[i]]>Lvar[ss[j]]){
	    if(Lvar[ss[i]]<Lvar[ss[j]]){
	      itmp=ss[i];
	      ss[i]=ss[j];
	      ss[j]=itmp;
	    }
	  }
	  i0=ss[i]/_nm;j0=ss[i]%_nm;
	  localmin=1;
	  if(Lib[ss[i]]>Libmin5) continue;
	  for(ii=i0-1;ii<=i0+1;ii++){
	    for(jj=j0-1;jj<=j0+1;jj++){
	      //	  fprintf(stderr,"ii,jj=%d:%d\n",ii,jj);
	      if(ii<0) continue;
	      if(ii>_nN-1) continue;
	      if(jj<0) continue;
	      if(jj>_nm-1) continue;
	      if(Lvar[ii*_nm+jj]<Lvar[ss[i]]) {localmin=0;break;}
	    }
	  }
	  if(localmin==1){
	    is++;
	    n0=NN[ss[i]/_nm];m0=mm[ss[i]%_nm];n1=n0;n2=n0+(m0-1)*_DN;
	    fprintf(stderr,"LD=%.3e Lvarmax%3d=%.3e Lvar0=%.3e Lib=%.3e Ltst=%.3e N:%d-:%d M:%d\n",LD[ss[i]],i,Lvar[ss[i]],Lvar0[ss[i]],Lib[ss[i]],Ltst[ss[i]],n1,_DN,m0);
	    //	  fprintf(stderr,"%3d:%3d(N:%3d-%3d) LD=%.3e Lvarmax%3d=%.3e Lvar0=%.3e Ltst=%.3e Lib=%.3e",n0,m0,n1,n2,LD[ss[i]],i,Lvar[ss[i]],Lvar0[ss[i]],Ltst[ss[i]],Lib[ss[i]]);
	    //	  fprintf(stderr,"**N:%d-%d\n",(int)(n0+_DN*m_LD),(int)(n2=n0+m0-1-_DN*m_LD));
	    //	  n0=NN[ss[i]/_nm];m0=mm[ss[i]%_nm];n1=n0;n2=n0+m0-1;//n1=n0+1;n2=n0+m0-2;
	    //	  fprintf(stderr,"%3d:%3d(N:%3d-%3d) LD=%e Lvarmin%3d=%e Ltst=%e\n",n0,m0,n1,n2,LD[ss[i]],i,Lvar[ss[i]],Ltst[ss[i]]);
	    //	  fprintf(stderr,"**N:%d-%d\n",(int)(n0+_DN*m_LD),(int)(n2=n0+m0-1-_DN*m_LD));
	    if(is>=ns) break;
	  }
	}
      }
      //global Lvar min and local LDmin
      {
	fprintf(stderr,"##Increasing Lvar with local min of LD (9neighbour of (l1,ld) **Use this or the next)** \n");
	//      fprintf(stderr,"##global min of Lvar with local min of LD\n");
	ns=3;
	for(i=0;i<n;i++) ss[i]=i;
	is=0;
	for(i=0;i<n;i++){
	  for(j=i+1;j<n;j++){
	    if(Lvar[ss[i]]>Lvar[ss[j]]){
	      //if(Lvar[ss[i]]<Lvar[ss[j]]){//060517
	      itmp=ss[i];
	      ss[i]=ss[j];
	      ss[j]=itmp;
	    }
	  }
	  if(Lib[ss[i]]>Libmin5) continue;
	  if(Lvar[ss[i]]<1e-20) continue;
	  i0=ss[i]/_nm;j0=ss[i]%_nm;
	  localmin=1;
	  for(ii=i0-1;ii<=i0+1;ii++){
	    for(jj=j0-1;jj<=j0+1;jj++){
	      //	  fprintf(stderr,"ii,jj=%d:%d\n",ii,jj);
	      if(ii<0) continue;
	      if(ii>_nN-1) continue;
	      if(jj<0) continue;
	      if(jj>_nm-1) continue;
	      if(LD[ii*_nm+jj]<LD[ss[i]]) {localmin=0;break;}
	    }
	  }

	  if(localmin==1){
	    is++;
	    n0=NN[ss[i]/_nm];m0=mm[ss[i]%_nm];n1=n0;n2=n0+(m0-1)*_DN;
	    fprintf(stderr,"LD=%.3e Lvarmin%2d=%.3e Lvar0=%.3e Lib=%.3e Ltst=%.3e N:%d-:%d M:%d\n",LD[ss[i]],i,Lvar[ss[i]],Lvar0[ss[i]],Lib[ss[i]],Ltst[ss[i]],n1,_DN,m0);
	    //	  n0=NN[ss[i]/_nm];m0=mm[ss[i]%_nm];n1=n0;n2=n0+m0-1;//n1=n0+1;n2=n0+m0-2;
	    //	  fprintf(stderr,"#%2d:%2d(N:%2d-%2d) LD=%.3e Lvarmin%2d=%.3e Lvar0=%.3e Ltst=%.3e Lib=%.3e",n0,m0,n1,n2,LD[ss[i]],i,Lvar[ss[i]],Lvar0[ss[i]],Ltst[ss[i]],Lib[ss[i]]);
	    //	  fprintf(stderr,"**N:%d-%d\n",(int)(n0+_dm*m_LD),(int)(n2=n0+m0-1-_dm*m_LD));
	    //	  fprintf(stderr,"%3d:%3d(N:%3d-%3d) LD=%e Lvarmin%3d=%e Ltst=%e\n",n0,m0,n1,n2,LD[ss[i]],i,Lvar[ss[i]],Ltst[ss[i]]);
	    //	  fprintf(stderr,"**N:%d-%d\n",(int)(n0+_DN*m_LD),(int)(n2=n0+m0-1-_DN*m_LD));
	    if(is>=ns) break;
	  }
	}
      }

      {
	fprintf(stderr,"##Increasing Lvar with local min of LD (9neighbour of (l1,l2)**Use this or the above)**) \n");
	ns=3;
	for(i=0;i<n;i++) ss[i]=i;
	is=0;
	for(i=0;i<n;i++){
	  for(j=i+1;j<n;j++){
	    if(Lvar[ss[i]]>Lvar[ss[j]]){
	      //	  if(Lvar[ss[i]]<Lvar[ss[j]]){//060517
	      itmp=ss[i];
	      ss[i]=ss[j];
	      ss[j]=itmp;
	    }
	  }
	  if(Lib[ss[i]]>Libmin5) continue;
	  if(Lvar[ss[i]]<1e-20) continue;
	  i0=ss[i]/_nm;j0=ss[i]%_nm;
	  localmin=1;
	  {
	    int iii;
	    for(iii=0;iii<9;iii++){
	      if(iii==0){ii=i0-1;jj=j0  ;}//l1-1,l2-1
	      if(iii==1){ii=i0-1;jj=j0+1;}//l1-1,l2
	      if(iii==2){ii=i0-1;jj=j0+2;}//l1-1,l2+1
	      if(iii==3){ii=i0  ;jj=j0-1;}//l1-1,l2-1
	      if(iii==4){ii=i0  ;jj=j0  ;}//l1-1,l2
	      if(iii==5){ii=i0  ;jj=j0+1;}//l1-1,l2+1
	      if(iii==6){ii=i0+1;jj=j0-2;}//l1-1,l2-1
	      if(iii==7){ii=i0+1;jj=j0-1;}//l1-1,l2
	      if(iii==8){ii=i0+1;jj=j0  ;}//l1-1,l2+1
	      if(ii<0) continue;
	      if(ii>_nN-1) continue;
	      if(jj<0) continue;
	      if(jj>_nm-1) continue;
	      if(LD[ii*_nm+jj]<LD[ss[i]]) {localmin=0;break;}
	    }
	  }

	  if(localmin==1){
	    is++;
	    n0=NN[ss[i]/_nm];m0=mm[ss[i]%_nm];n1=n0;n2=n0+(m0-1)*_DN;
	    fprintf(stderr,"LD=%.3e Lvarmin%3d=%.3e Lvar0=%.3e Lib=%.3e Ltst=%.3e N:%d D:%d m:%d\n",LD[ss[i]],i,Lvar[ss[i]],Lvar0[ss[i]],Lib[ss[i]],Ltst[ss[i]],n1,_DN,m0);
	    //	  n0=NN[ss[i]/_nm];m0=mm[ss[i]%_nm];n1=n0;n2=n0+m0-1;//n1=n0+1;n2=n0+m0-2;
	    //	  //	  fprintf(stderr,"%3d:%3d(N:%3d-%3d) LD=%e Lvarmin%3d=%e Ltst=%e\n",n0,m0,n1,n2,LD[ss[i]],i,Lvar[ss[i]],Ltst[ss[i]]);
	    //	  fprintf(stderr,"%3d:%3d(N:%3d-%3d) LD=%.3e Lvarmin%3d=%.3e Lvar0=%.3e Ltst=%.3e Lib=%.3e",n0,m0,n1,n2,LD[ss[i]],i,Lvar[ss[i]],Lvar0[ss[i]],Ltst[ss[i]],Lib[ss[i]]);
	    //	  //	  fprintf(stderr,"**N:%d-%d\n",n0+_DN,n2=n0+m0-2);
	    //	  //	  fprintf(stderr,"**N:%d-%d\n",(int)(n0+_DN*m_LD),(int)(n2=n0+m0-1-_DN*m_LD));
	    //	  fprintf(stderr,"**N:%d-%d\n",(int)(n0+_dm*m_LD),(int)(n2=n0+m0-1-_dm*m_LD));
	    if(is>=ns) break;
	  }
	}
      }

      {
	double *_Lvar  =(double *)malloc(sizeof(double)*_nN*_nm);

	fprintf(stderr,"##Increasing |Lvar-Lvar0|\n");
	ns=3;
	for(i=0;i<n;i++){
	  ss[i]=i;
	  _Lvar[i]=Lvar[i]-Lvar0[i];//060529
	  //	_Lvar[i]=fabs(Lvar[i]/Lvar0[i]-1.2);//060529
	  //	_Lvar[i]=fabs((Lvar[i]-Lvar0[i])/Lib[i]);//060518??
	  //	_Lvar[i]=Lvar[i]/Lvar0[i];
	  //	_Lvar[i]=fabs(Lvar[i]-Lvar0[i]);//good one
	  //	_Lvar[i]=fabs((Lvar[i]-Lvar0[i])/Lvar0[i]);//060518??
	  //	_Lvar[i]=fabs((Lvar[i]-Lvar0[i])/sqrt(Lib[i]));//060518??
	  //	_Lvar[i]=fabs(Lvar[i]-Lvar0[i])/Lvar[i];
	  //	_Lvar[i]=fabs(Lvar[i]-Lvar0[i])/Lvar0[i];
	}
	is=0;
	for(i=0;i<n;i++){
	  for(j=i+1;j<n;j++){
	    if(_Lvar[ss[i]]>_Lvar[ss[j]]){
	      itmp=ss[i];
	      ss[i]=ss[j];
	      ss[j]=itmp;
	    }
	  }
	  if(Lib[ss[i]]>Libmin5) continue;
	  if(Lvar[ss[i]]<1e-20) continue;
	  i0=ss[i]/_nm;j0=ss[i]%_nm;
	  localmin=1;
	  //	for(ii=i0-1;ii<=i0+1;ii++){
	  //	  for(jj=j0-1;jj<=j0+1;jj++){
	  //	    //	  fprintf(stderr,"ii,jj=%d:%d\n",ii,jj);
	  //	    if(ii<0) continue;
	  //	    if(ii>_nN-1) continue;
	  //	    if(jj<0) continue;
	  //	    if(jj>_nm-1) continue;
	  //	    if(LD[ii*_nm+jj]<LD[ss[i]]) {localmin=0;break;}
	  //	  }
	  //	}
	  if(localmin==1){
	    is++;
	    n0=NN[ss[i]/_nm];m0=mm[ss[i]%_nm];n1=n0;n2=n0+(m0-1)*_DN;
	    fprintf(stderr,"LD=%.3e Lvar=%.3e Lvar0min%3d=%.3e Lib=%.3e Ltst=%.3e N:%d-:%d M:%d\n",LD[ss[i]],Lvar[ss[i]],i,Lvar0[ss[i]],Lib[ss[i]],Ltst[ss[i]],n1,_DN,m0);
	    //	  n0=NN[ss[i]/_nm];m0=mm[ss[i]%_nm];n1=n0;n2=n0+m0-1;//n1=n0+1;n2=n0+m0-2;
	    //	  fprintf(stderr,"%3d:%3d(N:%3d-%3d) LD=%.3e Lvar=%.3e Lvar0min%3d=%.3e Ltst=%.3e Lib=%.3e",n0,m0,n1,n2,LD[ss[i]],Lvar[ss[i]],i,Lvar0[ss[i]],Ltst[ss[i]],Lib[ss[i]]);
	    //	  //	  fprintf(stderr,"**N:%d-%d\n",(int)(n0+_DN*m_LD),(int)(n2=n0+m0-1-_DN*m_LD));
	    //	  fprintf(stderr,"**N:%d-%d\n",(int)(n0+_dm*m_LD),(int)(n2=n0+m0-1-_dm*m_LD));
	    if(is>=ns) break;
	  }
	}
      }
      {
	double *_Lvar  =(double *)malloc(sizeof(double)*_nN*_nm);

	fprintf(stderr,"##Increasing |Lvar-Lvar0| with local min of LD(9neighbour of (l1,ld)\n");
	ns=3;
	for(i=0;i<n;i++){
	  ss[i]=i;
	  _Lvar[i]=Lvar[i]-Lvar0[i];//060529
	  //	_Lvar[i]=fabs(Lvar[i]/Lvar0[i]-1.2);//060529??
	  //	_Lvar[i]=fabs((Lvar[i]-Lvar0[i])/Lib[i]);//060518??
	  //	_Lvar[i]=Lvar[i]/Lvar0[i];
	  //	_Lvar[i]=fabs(Lvar[i]-Lvar0[i]);//good one
	  //	_Lvar[i]=fabs((Lvar[i]-Lvar0[i])/Lvar0[i]);//060518??
	  //	_Lvar[i]=fabs((Lvar[i]-Lvar0[i])/sqrt(Lib[i]));//060518??
	  //	_Lvar[i]=fabs(Lvar[i]-Lvar0[i])/Lvar[i];
	  //	_Lvar[i]=fabs(Lvar[i]-Lvar0[i])/Lvar0[i];
	}
	is=0;
	for(i=0;i<n;i++){
	  for(j=i+1;j<n;j++){
	    if(_Lvar[ss[i]]>_Lvar[ss[j]]){
	      itmp=ss[i];
	      ss[i]=ss[j];
	      ss[j]=itmp;
	    }
	  }
	  if(Lib[ss[i]]>Libmin5) continue;
	  if(Lvar[ss[i]]<1e-20) continue;
	  if(Lvar[ss[i]]<Lvar0[ss[i]]) continue;
	  i0=ss[i]/_nm;j0=ss[i]%_nm;
	  localmin=1;
	  for(ii=i0-1;ii<=i0+1;ii++){
	    for(jj=j0-1;jj<=j0+1;jj++){
	      //	  fprintf(stderr,"ii,jj=%d:%d\n",ii,jj);
	      if(ii<0) continue;
	      if(ii>_nN-1) continue;
	      if(jj<0) continue;
	      if(jj>_nm-1) continue;
	      if(LD[ii*_nm+jj]<LD[ss[i]]) {localmin=0;break;}
	    }
	  }
	  if(localmin==1){
	    is++;
	    n0=NN[ss[i]/_nm];m0=mm[ss[i]%_nm];n1=n0;n2=n0+(m0-1)*_DN;
	    fprintf(stderr,"LD=%.3e Lvar=%.3e Lvar0min%3d=%.3e Lib=%.3e Ltst=%.3e N:%d-:%d M:%d\n",LD[ss[i]],Lvar[ss[i]],i,Lvar0[ss[i]],Lib[ss[i]],Ltst[ss[i]],n1,_DN,m0);
	    //	  n0=NN[ss[i]/_nm];m0=mm[ss[i]%_nm];n1=n0;n2=n0+m0-1;//n1=n0+1;n2=n0+m0-2;
	    //	  fprintf(stderr,"%3d:%3d(N:%3d-%3d) LD=%.3e Lvar=%.3e Lvar0min%3d=%.3e Ltst=%.3e Lib=%.3e",n0,m0,n1,n2,LD[ss[i]],Lvar[ss[i]],i,Lvar0[ss[i]],Ltst[ss[i]],Lib[ss[i]]);
	    //	  //	  fprintf(stderr,"**N:%d-%d\n",(int)(n0+_DN*m_LD),(int)(n2=n0+m0-1-_DN*m_LD));
	    //	  fprintf(stderr,"**N:%d-%d\n",(int)(n0+_DN*m_LD),(int)(n2=n0+m0-1-_DN*m_LD));
	    if(is>=ns) break;
	  }
	}
      }
    }
    static char pr1[buffsize];
    fp=fopen(fnplt,"w+");
    for(i=0;i<smax_Ltstmin;i++){
      n0=NN[s_Ltstmin[i]/_nm];m0=mm[s_Ltstmin[i]%_nm];n1=n0;n2=n0+(m0-1)*_DN;//n1=n0+1;n2=n0+m0-2;
      //      fprintf(stderr,"t:%s v:%s LD%.2eLvar%.2eLvar0%.2eLib%.3eLtst%d=%.3eOvf%.3eLb'%.3eN:%d-:%dM:%drsa:%s\n",
      //	      fnbody(fngiven,pr),fnbody(fnvalid,pr1),LD[s_Ltstmin[i]],Lvar[s_Ltstmin[i]],Lvar0[s_Ltstmin[i]],Lib[s_Ltstmin[i]],i,Ltst[s_Ltstmin[i]],
      //	      //	      Ltst[s_Ltstmin[i]]-Lib[s_Ltstmin[i]],//overfit
      //	      Ltst[s_Ltstmin[i]]+Lvar0[s_Ltstmin[i]]-Lib[s_Ltstmin[i]],//overfit
      //	      (Lib[s_Ltstmin[i]]-Lvar0[s_Ltstmin[i]])/Ltst[s_Ltstmin[i]],//Lbias-Ovrfit
      //	      n1,_DN,m0,rsa);
      if(BAGGING==2){
	//	fprintf(stderr,"t:%s v:%s Ltst%d=%.3e(LvarPop%.3e)LbiasPop%.3e,LbiasPopB%.3eLib%.3eN:%d-:%dM:%drsa:%sbst:%d:%d\n",
	fprintf(stderr,"t:%s v:%s Lob%d=%.3e(uc%.3e)Lbias%.3e,LbiasPopB%.3eLib%.3eLemp%.3eH%.4fN:%d-:%dM:%drsa:%sbst:%d:%d\n",
		fnbody(fngiven,pr),fnbody(fnvalid,pr1),
		i,Ltst[s_Ltstmin[i]],
		LD[s_Ltstmin[i]],
		Ltst[s_Ltstmin[i]]-LD[s_Ltstmin[i]],//Ltst-Lvar=Lbias+Lnoise
		Ltst[s_Ltstmin[i]]+(1.-rs_alpha)*LD[s_Ltstmin[i]],//kc=2:coverage factor for 95% extended uncertainty
		//		Ltst[s_Ltstmin[i]]-(rs_alpha-1.)*LD[s_Ltstmin[i]],//kc=2:coverage factor for 95% extended uncertainty
		//Ltst[s_Ltstmin[i]]-5.*LD[s_Ltstmin[i]]-4.*sqrt((LD[s_Ltstmin[i]])*(Ltst[s_Ltstmin[i]]-LD[s_Ltstmin[i]])),//kc=2:coverage factor for 95% extended uncertainty
		//Ltst[s_Ltstmin[i]]+3.*LD[s_Ltstmin[i]]+4.*sqrt((LD[s_Ltstmin[i]])*(Ltst[s_Ltstmin[i]]-LD[s_Ltstmin[i]])),
		//		Ltst[s_Ltstmin[i]]-5.*LD[s_Ltstmin[i]],//kc=2:coverage factor for 95% extended uncertainty
		//		Ltst[s_Ltstmin[i]]+3.*LD[s_Ltstmin[i]],
		Lib[s_Ltstmin[i]],
		Lemp[s_Ltstmin[i]],
		H0[s_Ltstmin[i]],
		n1,_DN,m0,rsa,t_boost,Boost);
      }
      else{
	fprintf(stderr,"t:%s v:%s LD%.2eLvar%.2eLvar0%.2eLib%.3eH%.4fLtst%d=%.3eN:%d-:%dM:%drsa:%sbst:%d:%d\n",
		fnbody(fngiven,pr),fnbody(fnvalid,pr1),LD[s_Ltstmin[i]],Lvar[s_Ltstmin[i]],Lvar0[s_Ltstmin[i]],Lib[s_Ltstmin[i]],
		//      fprintf(stderr,"t:%s v:%s LD%.2eLvar%.2eLvar0%.2eLemp%.3eH%.4fLtst%d=%.3eN:%d-:%dM:%drsa:%sbst:%d:%d\n",
		//	      fnbody(fngiven,pr),fnbody(fnvalid,pr1),LD[s_Ltstmin[i]],Lvar[s_Ltstmin[i]],Lvar0[s_Ltstmin[i]],Lemp[s_Ltstmin[i]],
		//fprintf(stderr,"t:%s v:%s LD%.2eLvar%.2eLvar0%.2eLib%.3eH%.4fLtst%d=%.3eN:%d-:%dM:%drsa:%sbst:%d:%d\n",
		//fnbody(fngiven,pr),fnbody(fnvalid,pr1),LD[s_Ltstmin[i]],Lvar[s_Ltstmin[i]],Lvar0[s_Ltstmin[i]],Lib[s_Ltstmin[i]],
		H0[s_Ltstmin[i]],
		i,Ltst[s_Ltstmin[i]],
		n1,_DN,m0,rsa,t_boost,Boost);

	//	fprintf(stderr,"t:%s v:%s LD%.2eLvar%.2eLvar0%.2eLib%.3eLtst%d=%.3eLtr+Lvl%.3eN:%d-:%dM:%drsa:%sbst:%d\n",
	//		fnbody(fngiven,pr),fnbody(fnvalid,pr1),LD[s_Ltstmin[i]],Lvar[s_Ltstmin[i]],Lvar0[s_Ltstmin[i]],Lib[s_Ltstmin[i]],i,Ltst[s_Ltstmin[i]],
	//		Ltst[s_Ltstmin[i]]+Lib[s_Ltstmin[i]],n1,_DN,m0,rsa,t_boost);
      }
      ////      fprintf(stderr,"t:%s v:%s LD%.2eLvar%.2eLvar0%.2eLib%.3eLtst%d=%.3eLtr/Lvl%.3fN:%d-:%dM:%drsa:%s\n",
      ////	      fnbody(fngiven,pr),fnbody(fnvalid,pr1),LD[s_Ltstmin[i]],Lvar[s_Ltstmin[i]],Lvar0[s_Ltstmin[i]],Lib[s_Ltstmin[i]],i,Ltst[s_Ltstmin[i]],Lib[s_Ltstmin[i]]/Ltst[s_Ltstmin[i]],n1,_DN,m0,rsa);
      //      n0=NN[s_Ltstmin[i]/_nm];m0=mm[s_Ltstmin[i]%_nm];n1=n0;n2=n0+m0-1;//n1=n0+1;n2=n0+m0-2;
      //      fprintf(stderr,"t:%s v:%s %d:%d(N:%d-%d) LD   =%.3e Ltst%d=%.3e Lvar=%.3e Lvar0=%.3e Lib=%.3e",
      //	      fnbody(fngiven,pr),fnbody(fnvalid,pr1),n0,m0,n1,n2,LD[s_Ltstmin[i]],i,Ltst[s_Ltstmin[i]],Lvar[s_Ltstmin[i]],Lvar0[s_Ltstmin[i]],Lib[s_Ltstmin[i]]);
      //      //    fprintf(stderr,"**N:%d-%d\n",(int)(n0+_DN*m_LD),(int)(n2=n0+m0-1-_DN*m_LD));
      //      fprintf(stderr,"**N:%d-%d\n",(int)(n0+_dm*m_LD),(int)(n2=n0+m0-1-_dm*m_LD));
    }
    for(i=0;i<smax_L0vmin;i++){
      n0=NN[s_Ltstmin[i]/_nm];m0=mm[s_Ltstmin[i]%_nm];n1=n0;n2=n0+(m0-1)*_DN;//n1=n0+1;n2=n0+m0-2;
      //    if(BAGGING==2){
      fprintf(stderr,"t:%s v:%s L0v%d=%.3e=%.3e-%.3eLtst(Lob)%.3eLib%.3eLtr%.3eN:%d-:%dM:%drsa:%sbst:%d:%d\n",
	      fnbody(fngiven,pr),fnbody(fnvalid,pr1),
	      i,
	      L0v[s_L0vmin[i]],
	      L0[s_L0vmin[i]],
	      Lvar[s_L0vmin[i]],
	      Ltst[s_L0vmin[i]],
	      Lib[s_L0vmin[i]],
	      Ltr[s_L0vmin[i]],
	      n1,_DN,m0,rsa,t_boost,Boost);
      //    }
    }

    n0=NN[ij_LDmin/_nm];m0=mm[ij_LDmin%_nm];n1=n0;n2=n0+(m0-1)*_DN;//n1=n0+1;n2=n0+m0-2;
    fprintf(stderr,"t:%s v:%s LDmin=%.3e Lvar=%.3e Lvar0=%.3e Lib=%.3e Ltst=%.3e N:%d-:%d M:%d\n",
	    fnbody(fngiven,pr),fnbody(fnvalid,pr1),LD[ij_LDmin],Lvar[ij_LDmin],Lvar0[ij_LDmin],Lib[ij_LDmin],Ltst[ij_LDmin],n1,_DN,m0);
    //    n0=NN[ij_LDmin/_nm];m0=mm[ij_LDmin%_nm];n1=n0;n2=n0+m0-1;//n1=n0+1;n2=n0+m0-2;
    //    fprintf(stderr,"t:%s v:%s %d:%d(N:%d-%d) LDmin=%.3e Ltst=%.3e Lvar=%.3e Lvar0=%.3e Lib=%.3e",
    //	    fnbody(fngiven,pr),fnbody(fnvalid,pr1),n0,m0,n1,n2,LD[ij_LDmin],Ltst[ij_LDmin],Lvar[ij_LDmin],Lvar0[ij_LDmin],Lib[ij_LDmin]);
    //    fprintf(stderr,"**N:%d-%d\n",(int)(n0+_dm*m_LD),(int)(n2=n0+m0-1-_dm*m_LD));
    //    fprintf(stderr,"test=%d %d %d\n", 1/10,8/10,11/10);
    //    fprintf(stderr,"t:%s v:%s ij%d(i%d,j%d) N:%d:%d LDmin=%e Ltst=%e\n",
    //	    fnbody(fngiven,pr),fnbody(fnvalid,pr1),NN[ij_LDmin/_nm],mm[ij_LDmin%_nm],LD[ij_LDmin],Ltst[ij_LDmin]);

    fprintf(fp,"set style data linespoints\n");
    fprintf(fp,"set logscale z\n");
    fprintf(fp,"set title \"%s %s N:%d-:%d M:%d LDmin=%e Ltst=%e rsa:%s\"\n",
	    fnbody(fngiven,pr),fnbody(fnvalid,pr1),n1,_DN,m0,LD[ij_LDmin],Ltst[ij_LDmin],rsa);
    fprintf(fp,"set xlabel \"l1\";set ylabel \"ld\";set zlabel \"LD\"\n");
    if(BAGGING==2){
      fprintf(fp,"splot \"%s.dat\" using 1:2:4, \"%s.dat\" using 1:2:($4-5*$3), \"%s.dat\" using 1:2:($4+3*$3)\n",fnbody(fnloss,pr),fnbody(fnloss,pr),fnbody(fnloss,pr));
    }
    else{
      //    fprintf(fp,"splot \"%s.dat\" using 1:2:3\n",fnbody(fnloss,pr));
      fprintf(fp,"splot \"%s.dat\" using 1:2:4\n",fnbody(fnloss,pr));
    }
    fprintf(fp,"pause -1 \"hit return for save!\"\n");
    fprintf(fp,"set terminal tgif\n");
    fprintf(fp,"set output \"./lhat.obj\"\n");
    fprintf(fp,"replot\n");
    //
    n0=NN[ij_Ltstmin/_nm];m0=mm[ij_Ltstmin%_nm];n1=n0;n2=n0+(m0-1)*_DN;//n1=n0+1;n2=n0+m0-2;
    fprintf(fp,"set terminal x11\n");
    fprintf(fp,"set title \"%s %s Ltst=%e Ltstmin=%e Lib=%e N:%d-:%d M:%d\"\n",
	    fnbody(fngiven,pr),fnbody(fnvalid,pr1),LD[ij_Ltstmin],Ltst[ij_Ltstmin],Lib[ij_Ltstmin],n1,_DN,m0);
    //    fprintf(fp,"splot \"%s.dat\" using 1:2:4\n",fnbody(fnloss,pr));
    fprintf(fp,"set xlabel \"l1\";set ylabel \"ld\";set zlabel \"4Ltst,8Lib\"\n");
    fprintf(fp,"splot \"%s.dat\" using 1:2:4, \"%s.dat\" using 1:2:8\n",fnbody(fnloss,pr),fnbody(fnloss,pr));
    fprintf(fp,"pause -1 \"hit return for save !\"\n");
    fprintf(fp,"set terminal tgif\n");
    fprintf(fp,"set output \"./lhat.obj\"\n");
    fprintf(fp,"replot\n");
    //
    n0=NN[ij_Lvarmin/_nm];m0=mm[ij_Lvarmin%_nm];n1=n0;n2=n0+(m0-1)*_DN;//n1=n0+1;n2=n0+m0-2;
    fprintf(fp,"set terminal x11\n");
    fprintf(stderr,"t:%s v:%s Lvarmin=%e Lvarval=%e N:%d-:%d M:%d\n",
	    fnbody(fngiven,pr),fnbody(fnvalid,pr1),Lvar[ij_Lvarmin],Lvarval[ij_Lvarmin],n1,_DN,m0);
    fprintf(fp,"set title \"Lvar %s %s Lvarmin=%e Lvarval=%e N:%d-:%d M:%d\"\n",
	    fnbody(fngiven,pr),fnbody(fnvalid,pr1),Lvar[ij_Lvarmin],Lvarval[ij_Lvarmin],n1,_DN,m0);
    fprintf(fp,"set xlabel \"l1\";set ylabel \"ld\";set zlabel \"5Lvar,7Lvar0\"\n");
    //    fprintf(fp,"splot \"%s.dat\" using 1:2:5\n",fnbody(fnloss,pr));
    fprintf(fp,"splot \"%s.dat\" using 1:2:5, \"%s.dat\" using 1:2:7\n",fnbody(fnloss,pr),fnbody(fnloss,pr));
    fprintf(fp,"pause -1 \"hit return for save !\"\n");
    fprintf(fp,"set terminal tgif\n");
    fprintf(fp,"set output \"./lhat.obj\"\n");
    fprintf(fp,"replot\n");
    //
    n0=NN[ij_Lvarvalmin/_nm];m0=mm[ij_Lvarvalmin%_nm];n1=n0;n2=n0+(m0-1)*_DN;//n1=n0+1;n2=n0+m0-2;
    fprintf(fp,"set terminal x11\n");
    fprintf(stderr,"t:%s v:%s Lvar=%e Lvarvalmin=%e N:%d-:%d M:%d\n",
	    fnbody(fngiven,pr),fnbody(fnvalid,pr1),Lvar[ij_Lvarvalmin],Lvarval[ij_Lvarvalmin],n1,_DN,m0);
    fprintf(fp,"set title \"%s %s Lvar=%e Lvarvalmin=%e N:%d-:%d M:%d\"\n",
	    fnbody(fngiven,pr),fnbody(fnvalid,pr1),Lvar[ij_Lvarvalmin],Lvarval[ij_Lvarvalmin],n1,_DN,m0);
    fprintf(fp,"set xlabel \"l1\";set ylabel \"ld\";set zlabel \"6Lvarval\"\n");
    fprintf(fp,"splot \"%s.dat\" using 1:2:6\n",fnbody(fnloss,pr));//Lvarval
    fprintf(fp,"pause -1 \"hit return for save !\"\n");
    fprintf(fp,"set terminal tgif\n");
    fprintf(fp,"set output \"./lhat.obj\"\n");
    fprintf(fp,"replot\n");
    //
    fprintf(fp,"set terminal x11\n");
    fprintf(stderr,"t:%s v:%s Lvar=%e Lvarvalmin=%e N:%d-:%d M:%d\n",
	    fnbody(fngiven,pr),fnbody(fnvalid,pr1),Lvar[ij_Lvarvalmin],Lvarval[ij_Lvarvalmin],n1,_DN,m0);
    fprintf(fp,"set title \"Lvar-Lvar0 %s %s Lvar=%e Lvarvalmin=%e N:%d-:%d M:%d\"\n",
	    fnbody(fngiven,pr),fnbody(fnvalid,pr1),Lvar[ij_Lvarvalmin],Lvarval[ij_Lvarvalmin],n1,_DN,m0);
    //    fprintf(fp,"splot \"%s.dat\" using 1:2:7\n",fnbody(fnloss,pr));//Lvar-Lvar0
    fprintf(fp,"set xlabel \"l1\";set ylabel \"ld\";set zlabel \"5Lvar-7Lvar0\"\n");
    fprintf(fp,"splot [][][0.00001:1] \"%s.dat\" using 1:2:($5-$7)\n",fnbody(fnloss,pr));//Lvar-Lvar0
    //    fprintf(fp,"splot \"%s.dat\" using 1:2:($5/$7)\n",fnbody(fnloss,pr));//Lvar-Lvar0
    //    fprintf(fp,"splot [2:][2:][0.0001:1] \"%s.dat\" using 1:2:5, \"%s.dat\" using 1:2:7 \n",fnbody(fnloss,pr),fnbody(fnloss,pr));//Lvar-Lvar0
    fprintf(fp,"pause -1 \"hit return for save !\"\n");
    fprintf(fp,"set terminal tgif\n");
    fprintf(fp,"set output \"./lhat.obj\"\n");
    fprintf(fp,"replot\n");
    //
    fprintf(fp,"quit\n");
    fclose(fp);
    if(dsp==1 && DISP==1){
      sprintf(cmd,"cd %s;xterm -geometry 40x5-0+0 -e gnuplot %s.plt",rdir,fnbody(fnplt,pr));
      fprintf(stderr,"#Executing '%s'.\n",cmd);
      sprintf(&cmd[strlen(cmd)],"&");
      system(cmd);
    }
  }
  return(0);
}
#endif


/*
 * Copyright (c) 2010-2018 The University of Tennessee and The University
 *                         of Tennessee Research Foundation.  All rights
 *                         reserved.
 * Copyright (c) 2013      Inria. All rights reserved.
 * $COPYRIGHT
 *
 * @precisions normal z -> s d c
 *
 */
#include "dplasma.h"
#include <math.h>
#include <lapacke.h>
#include "parsec/data_dist/matrix/two_dim_rectangle_cyclic.h"

/**
 *******************************************************************************
 *
 * @ingroup dplasma_complex64_check
 *
 * check_zpotrf - Check the correctness of the Cholesky factorization computed
 * Cholesky functions with the following criteria:
 *
 *    \f[ ||L'L-A||_oo/(||A||_oo.N.eps) < 60. \f]
 *
 *  or
 *
 *    \f[ ||UU'-A||_oo/(||A||_oo.N.eps) < 60. \f]
 *
 *  where A is the original matrix, and L, or U, the result of the Cholesky
 *  factorization.
 *
 *******************************************************************************
 *
 * @param[in,out] parsec
 *          The parsec context of the application that will run the operation.
 *
 * @param[in] loud
 *          The level of verbosity required.
 *
 * @param[in] uplo
 *          = PlasmaUpper: Upper triangle of A and A0 are referenced;
 *          = PlasmaLower: Lower triangle of A and A0 are referenced.
 *
 * @param[in] A
 *          Descriptor of the distributed matrix A result of the Cholesky
 *          factorization. Holds L or U. If uplo == PlasmaUpper, the only the
 *          upper part is referenced, otherwise if uplo == PlasmaLower, the
 *          lower part is referenced.
 *
 * @param[in] A0
 *          Descriptor of the original distributed matrix A before
 *          factorization. If uplo == PlasmaUpper, the only the upper part is
 *          referenced, otherwise if uplo == PlasmaLower, the lower part is
 *          referenced.
 *
 *******************************************************************************
 *
 * @return
 *          \retval 1, if the result is incorrect
 *          \retval 0, if the result is correct
 *
 ******************************************************************************/
int check_zpotrf( parsec_context_t *parsec, int loud,
                  PLASMA_enum uplo,
                  parsec_tiled_matrix_dc_t *A,
                  parsec_tiled_matrix_dc_t *A0 )
{
    two_dim_block_cyclic_t *twodA = (two_dim_block_cyclic_t *)A0;
    two_dim_block_cyclic_t LLt;
    int info_factorization;
    double Rnorm = 0.0;
    double Anorm = 0.0;
    double result = 0.0;
    int M = A->m;
    int N = A->n;
    double eps = LAPACKE_dlamch_work('e');
    PLASMA_enum side;

    two_dim_block_cyclic_init(&LLt, matrix_ComplexDouble, matrix_Tile,
                              A->super.nodes, twodA->grid.rank,
                              A->mb, A->nb, M, N, 0, 0,
                              M, N, twodA->grid.strows, twodA->grid.stcols, twodA->grid.rows);

    LLt.mat = parsec_data_allocate((size_t)LLt.super.nb_local_tiles *
                                  (size_t)LLt.super.bsiz *
                                  (size_t)parsec_datadist_getsizeoftype(LLt.super.mtype));

    dplasma_zlaset( parsec, PlasmaUpperLower, 0., 0.,(parsec_tiled_matrix_dc_t *)&LLt );
    dplasma_zlacpy( parsec, uplo, A, (parsec_tiled_matrix_dc_t *)&LLt );

    /* Compute LL' or U'U  */
    side = (uplo == PlasmaUpper ) ? PlasmaLeft : PlasmaRight;
    dplasma_ztrmm( parsec, side, uplo, PlasmaConjTrans, PlasmaNonUnit, 1.0,
                   A, (parsec_tiled_matrix_dc_t*)&LLt);

    /* compute LL' - A or U'U - A */
    dplasma_ztradd( parsec, uplo, PlasmaNoTrans,
                    -1.0, A0, 1., (parsec_tiled_matrix_dc_t*)&LLt);

    Anorm = dplasma_zlanhe(parsec, PlasmaInfNorm, uplo, A0);
    Rnorm = dplasma_zlanhe(parsec, PlasmaInfNorm, uplo,
                           (parsec_tiled_matrix_dc_t*)&LLt);

    result = Rnorm / ( Anorm * N * eps ) ;

    if ( loud > 2 ) {
        printf("============\n");
        printf("Checking the Cholesky factorization \n");

        if ( loud > 3 )
            printf( "-- ||A||_oo = %e, ||L'L-A||_oo = %e\n", Anorm, Rnorm );

        printf("-- ||L'L-A||_oo/(||A||_oo.N.eps) = %e \n", result);
    }

    if ( isnan(Rnorm)  || isinf(Rnorm)  ||
         isnan(result) || isinf(result) ||
         (result > 60.0) )
    {
        if( loud ) printf("-- Factorization is suspicious ! \n");
        info_factorization = 1;
    }
    else
    {
        if( loud ) printf("-- Factorization is CORRECT ! \n");
        info_factorization = 0;
    }

    parsec_data_free(LLt.mat); LLt.mat = NULL;
    parsec_tiled_matrix_dc_destroy( (parsec_tiled_matrix_dc_t*)&LLt);

    return info_factorization;
}

/**
 *******************************************************************************
 *
 * @ingroup dplasma_complex64_check
 *
 * check_zaxmb - Returns the result of the following test
 *
 *    \f[ (|| A x - b ||_oo / ((||A||_oo * ||x||_oo + ||b||_oo) * N * eps) ) < 60. \f]
 *
 *  where A is the original matrix, b the original right hand side, and x the
 *  solution computed through any factorization.
 *
 *******************************************************************************
 *
 * @param[in,out] parsec
 *          The parsec context of the application that will run the operation.
 *
 * @param[in] loud
 *          The level of verbosity required.
 *
 * @param[in] uplo
 *          = PlasmaUpper: Upper triangle of A is referenced;
 *          = PlasmaLower: Lower triangle of A is referenced.
 *
 * @param[in] A
 *          Descriptor of the distributed matrix A result of the Cholesky
 *          factorization. Holds L or U. If uplo == PlasmaUpper, the only the
 *          upper part is referenced, otherwise if uplo == PlasmaLower, the
 *          lower part is referenced.
 *
 * @param[in,out] b
 *          Descriptor of the original distributed right hand side b.
 *          On exit, b is overwritten by (b - A * x).
 *
 * @param[in] x
 *          Descriptor of the solution to the problem, x.
 *
 *******************************************************************************
 *
 * @return
 *          \retval 1, if the result is incorrect
 *          \retval 0, if the result is correct
 *
 ******************************************************************************/
int check_zaxmb( parsec_context_t *parsec, int loud,
                 PLASMA_enum uplo,
                 parsec_tiled_matrix_dc_t *A,
                 parsec_tiled_matrix_dc_t *b,
                 parsec_tiled_matrix_dc_t *x )
{
    int info_solution;
    double Rnorm = 0.0;
    double Anorm = 0.0;
    double Bnorm = 0.0;
    double Xnorm, result;
    int N = b->m;
    double eps = LAPACKE_dlamch_work('e');

    Anorm = dplasma_zlanhe(parsec, PlasmaInfNorm, uplo, A);
    Bnorm = dplasma_zlange(parsec, PlasmaInfNorm, b);
    Xnorm = dplasma_zlange(parsec, PlasmaInfNorm, x);

    /* Compute b - A*x */
    dplasma_zhemm( parsec, PlasmaLeft, uplo, -1.0, A, x, 1.0, b);

    Rnorm = dplasma_zlange(parsec, PlasmaInfNorm, b);

    result = Rnorm / ( ( Anorm * Xnorm + Bnorm ) * N * eps ) ;

    if ( loud > 2 ) {
        printf("============\n");
        printf("Checking the Residual of the solution \n");
        if ( loud > 3 )
            printf( "-- ||A||_oo = %e, ||X||_oo = %e, ||B||_oo= %e, ||A X - B||_oo = %e\n",
                    Anorm, Xnorm, Bnorm, Rnorm );

        printf("-- ||Ax-B||_oo/((||A||_oo||x||_oo+||B||_oo).N.eps) = %e \n", result);
    }

    if (  isnan(Xnorm) || isinf(Xnorm) || isnan(result) || isinf(result) || (result > 60.0) ) {
        if( loud ) printf("-- Solution is suspicious ! \n");
        info_solution = 1;
    }
    else{
        if( loud ) printf("-- Solution is CORRECT ! \n");
        info_solution = 0;
    }

    return info_solution;
}

/**
 *******************************************************************************
 *
 * @ingroup dplasma_complex64_check
 *
 * check_zpoinv - Returns the result of the following test
 *
 *    \f[ (|| I - A * A^(-1) ||_one / (||A||_one * ||A^(-1)||_one * N * eps) ) < 10. \f]
 *
 *  where A is the original matrix, and Ainv the result of a cholesky inversion.
 *
 *******************************************************************************
 *
 * @param[in,out] parsec
 *          The parsec context of the application that will run the operation.
 *
 * @param[in] loud
 *          The level of verbosity required.
 *
 * @param[in] uplo
 *          = PlasmaUpper: Upper triangle of A is referenced;
 *          = PlasmaLower: Lower triangle of A is referenced.
 *
 * @param[in] A
 *          Descriptor of the distributed original matrix A.
 *          A must be two_dim_block_cyclic and fully generated.
 *
 * @param[in] Ainv
 *          Descriptor of the computed distributed A inverse.
 *
 *******************************************************************************
 *
 * @return
 *          \retval 1, if the result is incorrect
 *          \retval 0, if the result is correct
 *
 ******************************************************************************/
int check_zpoinv( parsec_context_t *parsec, int loud,
                  PLASMA_enum uplo,
                  parsec_tiled_matrix_dc_t *A,
                  parsec_tiled_matrix_dc_t *Ainv )
{
    two_dim_block_cyclic_t *twodA = (two_dim_block_cyclic_t *)A;
    two_dim_block_cyclic_t Id;
    int info_solution;
    double Anorm, Ainvnorm, Rnorm;
    double eps, result;

    eps = LAPACKE_dlamch_work('e');

    two_dim_block_cyclic_init(&Id, matrix_ComplexDouble, matrix_Tile,
                               A->super.nodes, twodA->grid.rank,
                               A->mb, A->nb, A->n, A->n, 0, 0,
                               A->n, A->n, twodA->grid.strows, twodA->grid.stcols, twodA->grid.rows);

    Id.mat = parsec_data_allocate((size_t)Id.super.nb_local_tiles *
                                  (size_t)Id.super.bsiz *
                                  (size_t)parsec_datadist_getsizeoftype(Id.super.mtype));

    dplasma_zlaset( parsec, PlasmaUpperLower, 0., 1., (parsec_tiled_matrix_dc_t *)&Id);

    /* Id - A^-1 * A */
    dplasma_zhemm(parsec, PlasmaLeft, uplo,
                  -1., Ainv, A,
                  1., (parsec_tiled_matrix_dc_t *)&Id );

    Anorm    = dplasma_zlanhe( parsec, PlasmaOneNorm, uplo, A );
    Ainvnorm = dplasma_zlanhe( parsec, PlasmaOneNorm, uplo, Ainv );
    Rnorm    = dplasma_zlange( parsec, PlasmaOneNorm, (parsec_tiled_matrix_dc_t*)&Id );

    result = Rnorm / ( (Anorm*Ainvnorm)*A->n*eps );
    if ( loud > 2 ) {
        printf("  ||A||_one = %e, ||A^(-1)||_one = %e, ||I - A * A^(-1)||_one = %e, result = %e\n",
               Anorm, Ainvnorm, Rnorm, result);
    }

    if ( isinf(Ainvnorm) || isnan(result) || isinf(result) || (result > 10.0) ) {
        info_solution = 1;
    }
    else {
        info_solution = 0;
    }

    parsec_data_free(Id.mat);
    parsec_tiled_matrix_dc_destroy((parsec_tiled_matrix_dc_t*)&Id);

    return info_solution;
}


/* multifit_nlinear/dogleg.c
 *
 * Copyright (C) 2016 Patrick Alken
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_multifit_nlinear.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>

/*
 * This module contains an implementation of the Powell dogleg
 * algorithm for nonlinear optimization problems. This implementation
 * closely follows the following works:
 *
 * [1] H. B. Nielsen, K. Madsen, Introduction to Optimization and
 *     Data Fitting, Informatics and Mathematical Modeling,
 *     Technical University of Denmark (DTU), 2010.
 *
 * [2] J. E. Dennis and H. H. W. Mei, Two new unconstrained optimization
 *     algorithms which use function and gradient values, J. Opt. Theory and
 *     Appl., 28(4), 1979.
 */

typedef struct
{
  size_t n;                  /* number of observations */
  size_t p;                  /* number of parameters */
  gsl_vector *dx_gn;         /* Gauss-Newton step, size p */
  gsl_vector *dx_sd;         /* steepest descent step, size p */
  double norm_Dgn;           /* || D dx_gn || */
  double norm_Dsd;           /* || D dx_sd || */
  double norm_Dinvg;         /* || D^{-1} g || */
  double norm_JDinv2g;       /* || J D^{-2} g || */
  gsl_vector *workp;         /* workspace, length p */
  gsl_vector *workn;         /* workspace, length n */

  /* tunable parameters */
  gsl_multifit_nlinear_parameters params;
} dogleg_state_t;

#include "common.c"

static void * dogleg_alloc (const void * params, const size_t n, const size_t p);
static void dogleg_free(void *vstate);
static int dogleg_init(const void *vtrust_state, void *vstate);
static int dogleg_preloop(const void * vtrust_state, void * vstate);
static int dogleg_step(const void * vtrust_state, const double delta,
                       gsl_vector * dx, void * vstate);
static int dogleg_double_step(const void * vtrust_state, const double delta,
                              gsl_vector * dx, void * vstate);
static int dogleg_preduction(const void * vtrust_state, const gsl_vector * dx,
                             double * pred, void * vstate);
static int dogleg_calc_gn(const gsl_multifit_nlinear_trust_state * trust_state, gsl_vector * dx);
static double dogleg_beta(const double t, const double delta,
                          const gsl_vector * diag, dogleg_state_t * state);

static void *
dogleg_alloc (const void * params, const size_t n, const size_t p)
{
  const gsl_multifit_nlinear_parameters *mparams = (const gsl_multifit_nlinear_parameters *) params;
  dogleg_state_t *state;

  state = calloc(1, sizeof(dogleg_state_t));
  if (state == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate dogleg state", GSL_ENOMEM);
    }

  state->dx_gn = gsl_vector_alloc(p);
  if (state->dx_gn == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for dx_gn", GSL_ENOMEM);
    }

  state->dx_sd = gsl_vector_alloc(p);
  if (state->dx_sd == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for dx_sd", GSL_ENOMEM);
    }

  state->workp = gsl_vector_alloc(p);
  if (state->workp == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for workp", GSL_ENOMEM);
    }

  state->workn = gsl_vector_alloc(n);
  if (state->workn == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for workn", GSL_ENOMEM);
    }

  state->n = n;
  state->p = p;
  state->params = *mparams;

  return state;
}

static void
dogleg_free(void *vstate)
{
  dogleg_state_t *state = (dogleg_state_t *) vstate;

  if (state->dx_gn)
    gsl_vector_free(state->dx_gn);

  if (state->dx_sd)
    gsl_vector_free(state->dx_sd);

  if (state->workp)
    gsl_vector_free(state->workp);

  if (state->workn)
    gsl_vector_free(state->workn);

  free(state);
}

/*
dogleg_init()
  Initialize dogleg solver

Inputs: vtrust_state - trust state
        vstate       - workspace

Return: success/error
*/

static int
dogleg_init(const void *vtrust_state, void *vstate)
{
  (void)vtrust_state;
  (void)vstate;

  return GSL_SUCCESS;
}

/*
dogleg_preloop()
  Initialize dogleg method prior to iteration loop.
This involves computing the steepest descent step. The
Gauss-Newton step is computed if needed in the _step()
routines.

Notes: on output,
1) state->dx_sd contains steepest descent step
2) state->norm_Dinvg contains || D^{-1} g ||
3) state->norm_JDinv2g contains || J D^{-2} g ||
*/

static int
dogleg_preloop(const void * vtrust_state, void * vstate)
{
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  dogleg_state_t *state = (dogleg_state_t *) vstate;
  double u;
  double alpha; /* ||g||^2 / ||Jg||^2 */

  /* calculate the steepest descent step */

  /* compute workp = D^{-1} g and its norm */
  gsl_vector_memcpy(state->workp, trust_state->g);
  gsl_vector_div(state->workp, trust_state->diag);
  state->norm_Dinvg = gsl_blas_dnrm2(state->workp);

  /* compute workp = D^{-2} g */
  gsl_vector_div(state->workp, trust_state->diag);

  /* compute: workn = J D^{-2} g */
  gsl_blas_dgemv(CblasNoTrans, 1.0, trust_state->J, state->workp, 0.0, state->workn);
  state->norm_JDinv2g = gsl_blas_dnrm2(state->workn);

  u = state->norm_Dinvg / state->norm_JDinv2g;
  alpha = u * u;

  /* dx_sd = -alpha D^{-2} g */
  gsl_vector_memcpy(state->dx_sd, state->workp);
  gsl_vector_scale(state->dx_sd, -alpha);

  state->norm_Dsd = scaled_enorm(trust_state->diag, state->dx_sd);
  state->norm_Dgn = -1.0; /* computed later if needed */

  return GSL_SUCCESS;
}

/*
dogleg_step()
  Calculate a new step vector
*/

static int
dogleg_step(const void * vtrust_state, const double delta,
            gsl_vector * dx, void * vstate)
{
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  dogleg_state_t *state = (dogleg_state_t *) vstate;

  if (state->norm_Dsd >= delta)
    {
      /* steepest descent step is outside trust region;
       * truncate steepest descent step to trust region boundary */
      gsl_vector_memcpy(dx, state->dx_sd);
      gsl_vector_scale(dx, delta / state->norm_Dsd);
    }
  else
    {
      /* compute Gauss-Newton step if needed */
      if (state->norm_Dgn < 0.0)
        {
          int status = dogleg_calc_gn(trust_state, state->dx_gn);

          if (status)
            return status;

          /* compute || D dx_gn || */
          state->norm_Dgn = scaled_enorm(trust_state->diag, state->dx_gn);
        }

      if (state->norm_Dgn <= delta)
        {
          /* Gauss-Newton step is inside trust region, use it as final step
           * since it is the global minimizer of the quadratic model function */
          gsl_vector_memcpy(dx, state->dx_gn);
        }
      else
        {
          /* Gauss-Newton step is outside trust region, but steepest
           * descent is inside; use dogleg step */

          double beta = dogleg_beta(1.0, delta, trust_state->diag, state);

          /* compute: workp = dx_gn - dx_sd */
          scaled_addition(1.0, state->dx_gn, -1.0, state->dx_sd, state->workp);

          /* dx = dx_sd + beta*(dx_gn - dx_sd) */
          scaled_addition(beta, state->workp, 1.0, state->dx_sd, dx);
        }
    }

  return GSL_SUCCESS;
}

/*
dogleg_double_step()
  Calculate a new step with double dogleg method. Based on
section 3 of [2]
*/

static int
dogleg_double_step(const void * vtrust_state, const double delta,
                   gsl_vector * dx, void * vstate)
{
  const double alpha_fac = 0.8; /* recommended value from Dennis and Mei */
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  dogleg_state_t *state = (dogleg_state_t *) vstate;

  if (state->norm_Dsd >= delta)
    {
      /* steepest descent step is outside trust region;
       * truncate steepest descent step to trust region boundary */
      gsl_vector_memcpy(dx, state->dx_sd);
      gsl_vector_scale(dx, delta / state->norm_Dsd);
    }
  else
    {
      /* compute Gauss-Newton step if needed */
      if (state->norm_Dgn < 0.0)
        {
          int status = dogleg_calc_gn(trust_state, state->dx_gn);

          if (status)
            return status;

          /* compute || D dx_gn || */
          state->norm_Dgn = scaled_enorm(trust_state->diag, state->dx_gn);
        }

      if (state->norm_Dgn <= delta)
        {
          /* Gauss-Newton step is inside trust region, use it as final step
           * since it is the global minimizer of the quadratic model function */
          gsl_vector_memcpy(dx, state->dx_gn);
        }
      else
        {
          double t, u, v, c;

          /* compute: u = ||D^{-1} g||^2 / ||J D^{-2} g||^2 */
          v = state->norm_Dinvg / state->norm_JDinv2g;
          u = v * v;

          /* compute: v = g^T dx_gn */
          gsl_blas_ddot(trust_state->g, state->dx_gn, &v);

          /* compute: c = ||D^{-1} g||^4 / (||J D^{-2} g||^2 * |g^T dx_gn|) */
          c = u * (state->norm_Dinvg / fabs(v)) * state->norm_Dinvg;

          /* compute: t = 1 - alpha_fac*(1-c) */
          t = 1.0 - alpha_fac*(1.0 - c);

          if (t * state->norm_Dgn <= delta)
            {
              /* set dx = (delta / ||D dx_gn||) dx_gn */
              gsl_vector_memcpy(dx, state->dx_gn);
              gsl_vector_scale(dx, delta / state->norm_Dgn);
            }
          else
            {
              /* Cauchy point is inside, Gauss-Newton is outside trust region;
               * use double dogleg step */

              double beta = dogleg_beta(t, delta, trust_state->diag, state);

              /* compute: workp = t*dx_gn - dx_sd */
              scaled_addition(t, state->dx_gn, -1.0, state->dx_sd, state->workp);

              /* dx = dx_sd + beta*(t*dx_gn - dx_sd) */
              scaled_addition(beta, state->workp, 1.0, state->dx_sd, dx);
            }
        }
    }

  return GSL_SUCCESS;
}

static int
dogleg_preduction(const void * vtrust_state, const gsl_vector * dx,
                  double * pred, void * vstate)
{
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  dogleg_state_t *state = (dogleg_state_t *) vstate;

  *pred = quadratic_preduction(trust_state->f, trust_state->J, dx, state->workn);

  return GSL_SUCCESS;
}

/*
dogleg_calc_gn()
  Calculate Gauss-Newton step which satisfies:

J dx_gn = -f

Inputs: trust_state - trust state variables
        dx          - (output) Gauss-Newton step

Return: success/error
*/

static int
dogleg_calc_gn(const gsl_multifit_nlinear_trust_state * trust_state, gsl_vector * dx)
{
  int status;
  const gsl_multifit_nlinear_parameters *params = trust_state->params;

  /* initialize linear least squares solver */
  status = (params->solver->init)(trust_state, trust_state->solver_state);
  if (status)
    return status;

  /* prepare the linear solver to compute Gauss-Newton step */
  status = (params->solver->presolve)(0.0, trust_state, trust_state->solver_state);
  if (status)
    return status;

  /* solve: J dx_gn = -f for Gauss-Newton step */
  status = (params->solver->solve)(trust_state->f,
                                   dx,
                                   trust_state,
                                   trust_state->solver_state);
  if (status)
    return status;

  return GSL_SUCCESS;
}

/*
dogleg_beta()
  This function finds beta in [0,1] such that the step

dx = dx_sd + beta*(t*dx_gn - dx_sd)

has norm

||D dx|| = delta

beta is the positive root of the quadratic:

a beta^2 + b beta + c = 0

with

a = ||D(t*dx_gn - dx_sd)||^2
b = 2 dx_sd^T D^T D (t*dx_gn - dx_sd)
c = ||D dx_sd||^2 - delta^2

Inputs: t     - amount of Gauss-Newton step to use for dogleg
                (= 1 for classical dogleg, <= 1 for double dogleg)
        delta - trust region radius
        diag  - diag(D) scaling matrix
        state - workspace
*/

static double
dogleg_beta(const double t, const double delta,
            const gsl_vector * diag, dogleg_state_t * state)
{
  double beta;
  double a, b, c;

  /* compute: workp = t*dx_gn - dx_sd */
  scaled_addition(t, state->dx_gn, -1.0, state->dx_sd, state->workp);

  /* a = || D (t*dx_gn - dx_sd) ||^2 */
  a = scaled_enorm(diag, state->workp);
  a *= a;

  /* workp = D^T D (t*dx_gn - dx_sd) */
  gsl_vector_mul(state->workp, diag);
  gsl_vector_mul(state->workp, diag);

  /* b = 2 dx_sd^T D^T D (t*dx_gn - dx-sd) */
  gsl_blas_ddot(state->dx_sd, state->workp, &b);
  b *= 2.0;

  /* c = || D dx_sd ||^2 - delta^2 = (||D dx_sd|| + delta) (||D dx_sd|| - delta) */
  c = (state->norm_Dsd + delta) * (state->norm_Dsd - delta);

  if (b > 0.0)
    {
      beta = (-2.0 * c) / (b + sqrt(b*b - 4.0*a*c));
    }
  else
    {
      beta = (-b + sqrt(b*b - 4.0*a*c)) / (2.0 * a);
    }

  return beta;
}

static const gsl_multifit_nlinear_trs dogleg_type =
{
  "dogleg",
  dogleg_alloc,
  dogleg_init,
  dogleg_preloop,
  dogleg_step,
  dogleg_preduction,
  dogleg_free
};

const gsl_multifit_nlinear_trs *gsl_multifit_nlinear_trs_dogleg = &dogleg_type;

static const gsl_multifit_nlinear_trs ddogleg_type =
{
  "double-dogleg",
  dogleg_alloc,
  dogleg_init,
  dogleg_preloop,
  dogleg_double_step,
  dogleg_preduction,
  dogleg_free
};

const gsl_multifit_nlinear_trs *gsl_multifit_nlinear_trs_ddogleg = &ddogleg_type;


#include <gsl/gsl_matrix.h>
#include "cimple_safe_mode.h"

/**
 * Adds a node to the beginning of the list
 */
void push_burn_node(burn_graph_node ** head,
                    abstract_state *state)
{

    burn_graph_node * new_node;
    new_node = malloc(sizeof(burn_graph_node));

    new_node->state = state;
    new_node->next = *head;
    *head = new_node;
};

/**
 * Remove a node from the beginning of the list
 */
void pop_burn_node(burn_graph_node ** head)
{

    burn_graph_node * next_node = NULL;

    next_node = (*head)->next;
    free(*head);
    *head = next_node;

};

/**
 * Delete and free memory of all nodes of a list
 */
void clear_burn_list(burn_graph_node **head)
{
    while (head !=NULL){
        pop_burn_node(head);
    }
};

/**
 * Given a set of abstract states (associated with the transitions going out from a state, where the function was called)
 * it finds the shortest distance to a state with an invariant set.
 */
abstract_state* fastest_burn(abstract_state **transitions,
                             int transitions_count)
{
    //Initialize
    abstract_state* fastest = transitions[0];
    int distance = transitions[0]->distance_invariant_set;

    //Check that there are more than 1 transition to evaluate, else trivial
    if(transitions_count >1){

        //Compare transitions
        for(int i = 1; i<transitions_count; i++){
            if(transitions[i]->distance_invariant_set<distance){
                distance = transitions[i]->distance_invariant_set;
                fastest = transitions[i];
            }
        }

    }

    return fastest;
}

/**
 * Given two polytopes, computes one step backwards from second polytope towards first polytope.
 */
polytope* previous_polytope(polytope *P1,
                            polytope *P2,
                            system_dynamics *s_dyn)
{

    size_t n = s_dyn->A->size2;  // State space dimension;
    size_t m = s_dyn->B->size2;  // Input space dimension;
    polytope *scaled_W_set = polytope_linear_transform(s_dyn->W_set, s_dyn->E); // multiplication: EW

    polytope * robust_P1 = polytope_pontryagin(P1, scaled_W_set);
    polytope * robust_P2 = polytope_pontryagin(P2, scaled_W_set);

    size_t sum_dim = robust_P1->H->size1+robust_P2->H->size1;

    polytope *precedent_polytope = polytope_alloc(sum_dim+s_dyn->U_set->H->size1, n+m);

    // FOR precedent_polytope G
    /*
     *     |P1_G|
     * G = |P2_G|
     *     |  0 |
     */
    gsl_vector_set_zero(precedent_polytope->G);
    gsl_vector_view G_P1 = gsl_vector_subvector(precedent_polytope->G, 0, robust_P1->G->size);
    gsl_vector_memcpy(&G_P1.vector, robust_P1->G);
    gsl_vector_view G_P2 = gsl_vector_subvector(precedent_polytope->G, robust_P1->G->size,robust_P2->G->size);
    gsl_vector_memcpy(&G_P2.vector, robust_P2->G);
//    gsl_vector * P2_HdotK = gsl_vector_alloc(robust_P2->G->size);
//    gsl_blas_dgemv(CblasNoTrans,1.0, robust_P2->H, s_dyn->K, 0.0, P2_HdotK);
//    gsl_vector_sub(&G_P2.vector, P2_HdotK);
//    //Clean up!
//    gsl_vector_free(P2_HdotK);

    // FOR Dist
    /*
     *         |  0   |
     * Dist =  |P2_H.E|
     *         |  0   |
     */
//    gsl_matrix *Dist = gsl_matrix_alloc(sum_dim+s_dyn->U_set->H->size1, p);
//    gsl_matrix_set_zero(Dist);
//    gsl_matrix_view Dist_P2 = gsl_matrix_submatrix(Dist, P1->H->size1, 0, P2->H->size1, Dist->size2);
//    gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, P2->H, s_dyn->E, 0.0, &Dist_P2.matrix);

    // FOR precedent_polytope H
    /*
     *  H = |H1   0 |  OR  H = |H1    0 |
     *      |H2A H2B|          |H2A  H2B|
     *      | 0  HU |          |   HU   |
     */
    gsl_matrix_set_zero(precedent_polytope->H);

    gsl_matrix_view H_P1 = gsl_matrix_submatrix(precedent_polytope->H, 0, 0, robust_P1->H->size1, n);
    gsl_matrix_memcpy(&H_P1.matrix, robust_P1->H);
    gsl_matrix_view H_P2_1 = gsl_matrix_submatrix(precedent_polytope->H, robust_P1->H->size1, 0, robust_P2->H->size1, n);
    gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, robust_P2->H, s_dyn->A, 0.0, &H_P2_1.matrix);
    gsl_matrix_view H_P2_2 = gsl_matrix_submatrix(precedent_polytope->H, robust_P1->H->size1, n, robust_P2->H->size1, m);
    gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, robust_P2->H, s_dyn->B, 0.0, &H_P2_2.matrix);

    if (s_dyn->U_set->H->size2 == m){
        gsl_matrix_view HU = gsl_matrix_submatrix(precedent_polytope->H,sum_dim, n, s_dyn->U_set->H->size1,m);
        gsl_matrix_memcpy(&HU.matrix,s_dyn->U_set->H);
    } else if (s_dyn->U_set->H->size2 == m+n){
        // transforms U_set.H from |constraints_ input constraints_state| to |constraints_state constraints_input|
        /*
         * |m m m m n n n|    |n n n m m m m|
         * |m m m m n n n| => |n n n m m m m|
         * |m m m m n n n|    |n n n m m m m|
         */
        gsl_matrix_view HU = gsl_matrix_submatrix(precedent_polytope->H,sum_dim, 0, s_dyn->U_set->H->size1,m+n);
        gsl_matrix * exchange_matrix = gsl_matrix_alloc(n+m,n+m);
        gsl_matrix_set_zero(exchange_matrix);
        gsl_matrix_view eye_m = gsl_matrix_submatrix(exchange_matrix, 0, n, m, m);
        gsl_matrix_set_identity(&eye_m.matrix);
        gsl_matrix_view eye_n = gsl_matrix_submatrix(exchange_matrix, m, 0, n, n);
        gsl_matrix_set_identity(&eye_n.matrix);
        gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0, s_dyn->U_set->H,exchange_matrix, 0.0, &HU.matrix);
    }

    polytope *return_polytope = polytope_projection(precedent_polytope, n);

    //Clean up!
//    gsl_matrix_free(Dist);
//    gsl_vector_free(D_hat);
    polytope_free(precedent_polytope);

    return return_polytope;
};

/**
 * Computes N-1 polytopes the system has to transition to go from origin to target in N time steps
 */
polytope ** compute_path(polytope *origin,
                         polytope *target,
                         system_dynamics *s_dyn,
                         int N)
{

    //Initialize
    polytope **path = malloc(sizeof(polytope) * N);

    //Instead of just pointing path[N] = target, the memory is copied,
    // thus when the target polytope is freed somewhere the path stays complete
    path[N] = polytope_alloc(target->H->size1,target->H->size2);
    gsl_matrix_memcpy(path[N]->H,target->H);

    path[0] = polytope_alloc(origin->H->size1,origin->H->size2);
    gsl_matrix_memcpy(path[0]->H,origin->H);

    //Compute intermediate steps
    for(int j = N-1; j>0;j--){
        path[j]=previous_polytope(path[0], path[j+1], s_dyn);
    }

    return path;
};

polytope *pre_alpha(polytope *R_i,
                    gsl_matrix *A,
                    gsl_matrix *B,
                    polytope *W_set,
                    polytope *U_set,
                    polytope *scaled_unit_cube)
{
    size_t n = A->size2;
    size_t m = B->size2;
    polytope *W_set_scaled = polytope_minkowski(W_set, scaled_unit_cube);
    polytope *R_i_scaled = polytope_pontryagin(R_i, W_set_scaled);

    polytope *P0 = polytope_alloc(R_i_scaled->H->size1+U_set->H->size1, n+m);
    gsl_matrix_set_zero(P0->H);
    gsl_matrix_view H_P2_1 = gsl_matrix_submatrix(P0->H, 0, 0, R_i_scaled->H->size1, n);
    gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, R_i_scaled->H, A, 0.0, &H_P2_1.matrix);
    gsl_matrix_view H_P2_2 = gsl_matrix_submatrix(P0->H, 0, n, R_i_scaled->H->size1, m);
    gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, R_i_scaled->H, B, 0.0, &H_P2_2.matrix);
    if (U_set->H->size2 == m){
        gsl_matrix_view HU = gsl_matrix_submatrix(P0->H,R_i_scaled->H->size1, n, U_set->H->size1,m);
        gsl_matrix_memcpy(&HU.matrix,U_set->H);
    } else if (U_set->H->size2 == m+n){
        // transforms U_set.H from |constraints_ input constraints_state| to |constraints_state constraints_input|
        /*
         * |m m m m n n n|    |n n n m m m m|
         * |m m m m n n n| => |n n n m m m m|
         * |m m m m n n n|    |n n n m m m m|
         */
        gsl_matrix_view HU = gsl_matrix_submatrix(P0->H,R_i_scaled->H->size1, 0, U_set->H->size1,m+n);
        gsl_matrix * exchange_matrix = gsl_matrix_alloc(n+m,n+m);
        gsl_matrix_set_zero(exchange_matrix);
        gsl_matrix_view eye_m = gsl_matrix_submatrix(exchange_matrix, 0, n, m, m);
        gsl_matrix_set_identity(&eye_m.matrix);
        gsl_matrix_view eye_n = gsl_matrix_submatrix(exchange_matrix, m, 0, n, n);
        gsl_matrix_set_identity(&eye_n.matrix);
        gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0, U_set->H,exchange_matrix, 0.0, &HU.matrix);
    }
    gsl_vector_set_zero(P0->G);
    gsl_vector_view G_P1 = gsl_vector_subvector(P0->G, 0, R_i_scaled->G->size);
    gsl_vector_memcpy(&G_P1.vector, R_i_scaled->G);
    gsl_vector_view G_P2 = gsl_vector_subvector(P0->G, R_i_scaled->G->size,U_set->G->size);
    gsl_vector_memcpy(&G_P2.vector, U_set->G);

    polytope *pre_R_1 = polytope_projection(P0, n);
    polytope_free(P0);
    return pre_R_1;
}
polytope * compute_invariant_set(polytope* X,
                                 gsl_matrix *A,
                                 gsl_matrix *B,
                                 polytope *W_set,
                                 polytope *U_set,
                                 double alpha)
{
    polytope * scaled_unit_cube = polytope_scaled_unit_cube(alpha, (int)X->H->size2);

    polytope *R_i = polytope_alloc(X->H->size1,X->H->size2);
    gsl_matrix_memcpy(R_i->H,X->H);
    gsl_vector_memcpy(R_i->G,X->G);
    polytope *pre_R_i = pre_alpha(R_i,A,B,W_set,U_set,scaled_unit_cube);
    polytope *R_i_plus = polytope_unite_inequalities(pre_R_i, X);
    dd_ErrorType err = dd_NoError;
    dd_WriteMatrix(stdout,dd_CopyGenerators(polytope_to_cdd(R_i_plus,&err)));

    polytope *R_scaled = polytope_minkowski(R_i_plus, scaled_unit_cube);
    while(!(polytope_is_subset(R_i, R_scaled))){
        polytope_free(pre_R_i);
        polytope_free(R_scaled);
        polytope_free(R_i);
        R_i = R_i_plus;
        pre_R_i = pre_alpha(R_i,A,B,W_set,U_set,scaled_unit_cube);
        R_i_plus = polytope_unite_inequalities(pre_R_i, X);
        R_scaled = polytope_minkowski(R_i_plus, scaled_unit_cube);
    }
    polytope_free(pre_R_i);
    polytope_free(R_i_plus);
    polytope_free(R_scaled);
    return R_i;

};

bool has_transition(abstract_state *state,
                    abstract_state *transition)
{
    bool transition_found = false;
    for(int i = 0; i<state->transitions_in_count; i++){
        if(state->transitions_in[i] == transition){
            transition_found = true;
        }
    }

    return transition_found;
};
/**
 * 1) Runs through all abstract states,
 * 2) checks if invariant set is contained
 * 3) sets path for all cells in those states containing an invariant set if possible
 */
burn_graph_node *set_invariant_sets(discrete_dynamics *d_dyn,
                                    system_dynamics *s_dyn)
{

    //Initialize graph
    burn_graph_node * head_node = NULL;
    head_node = malloc(sizeof(burn_graph_node));
    head_node->state = NULL;
    head_node->next = NULL;
    int N = (int)d_dyn->time_horizon;
    double unit_cube_scale = 1;
    //Create graph by
    // 1) running through all abstract_states and
    // 2) check whether it contains an invariant set
    for(int i = 0; i< d_dyn->abstract_states_count; i++){
        bool found_set = false;
        for(int j=0; j<d_dyn->abstract_states_set[i]->cells_count; j++){
            //Try to compute invariant set
            polytope *invariant_set = compute_invariant_set(d_dyn->abstract_states_set[i]->cells[j]->polytope_description,
                                                            s_dyn->A,
                                                            s_dyn->B,
                                                            s_dyn->W_set,
                                                            s_dyn->U_set,
                                                            unit_cube_scale);

            //if invariant set is found
            if(invariant_set != NULL){
                found_set = true;
                //Set invariant_set cell to A cell containing an invariant set
                //(may be more than one invariant set!!)
                d_dyn->abstract_states_set[i]->invariant_set = d_dyn->abstract_states_set[i]->cells[j];
                //Compute path for rest of cell to invariant set (may be just a subset of that polytope)
                d_dyn->abstract_states_set[i]->cells[j]->safe_mode = compute_path(d_dyn->abstract_states_set[i]->cells[j]->polytope_description,
                                                                      invariant_set,
                                                                      s_dyn,
                                                                      N);
            }else{
                //Set NULL because later safe_mode==NULL cells will be picked out
                d_dyn->abstract_states_set[i]->cells[j]->safe_mode = NULL;
            }
        }

        //If abstract state contains at least one invariant set it is appended to the graph
        if(found_set){
            //Check whether state has a transition to itself
            if(has_transition(d_dyn->abstract_states_set[i], d_dyn->abstract_states_set[i])){
                //if yes compute the safe_mode path for the cells that do not contain an invariant set
                for(int j = 0; j < d_dyn->abstract_states_set[i]->cells_count; j++){
                    if(d_dyn->abstract_states_set[i]->cells[j]->safe_mode == NULL){
                        d_dyn->abstract_states_set[i]->cells[j]->safe_mode = compute_path(d_dyn->abstract_states_set[i]->cells[j]->polytope_description,
                                                                              d_dyn->abstract_states_set[i]->invariant_set->polytope_description,
                                                                              s_dyn,
                                                                              N);
                    }
                }
            }
            //Append state to graph
            if (head_node->state == NULL) {
                head_node->state = d_dyn->abstract_states_set[i];
            }else{
                push_burn_node(&head_node,d_dyn->abstract_states_set[i]);
            }
        }
    }
    return head_node;

};

/**
 * Gets list of states containing invariant sets.
 * Burns through transition system, setting all other safe mode paths.
 */
//void burning_method(burn_graph_node *seeds,
//                    discrete_dynamics *d_dyn,
//                    system_dynamics *s_dyn)
//{
//
//    //Initialize needed variables
//    bool graph_burnt = false;
//    int N = (int)d_dyn->time_horizon;
//    burn_graph_node *current_burning = seeds;
//    int burning_round = 0;
//
//
//    while(!graph_burnt){
//        burning_round +=1;
//        burn_graph_node *next_burning = current_burning;
//        while (current_burning != NULL) {
//            if(current_burning->state->distance_invariant_set == INFINITY){
//                for(int i = 0; i<current_burning->state->cells_count;i++){
//                    if(current_burning->state->cells[i]->safe_mode == NULL){
//                        current_burning->state->cells[i]->safe_mode = compute_path(current_burning->state->cells[i]->polytope_description,
//                                                                                current_burning->state->next_state->convex_hull,
//                                                                                s_dyn,
//                                                                                N);
//                    }
//
//                }
//                current_burning->state->distance_invariant_set = burning_round;
//            }
//            current_burning = current_burning->next;
//        }
//        while(next_burning != NULL){
//            for(int i = 0;i<next_burning->state->transitions_in_count;i++){
//                if(next_burning->state->transitions_in[i]->distance_invariant_set == NULL){
//                    push_burn_node(&current_burning,next_burning->state->transitions_in[i]);
//                }
//            }
//            next_burning = next_burning->next;
//        }
//        if(current_burning == NULL){
//            graph_burnt = true;
//        }
//    }
//    while (seeds !=NULL){
//        if(seeds->state->next_state != seeds->state){
//            abstract_state* fastest = fastest_burn(seeds->state->transitions_out,seeds->state->transitions_out_count);
//            for(int i = 0; i< seeds->state->cells_count; i++){
//                if(seeds->state->cells[i]->safe_mode == NULL){
//                    seeds->state->cells[i]->safe_mode = compute_path(seeds->state->cells[i]->polytope_description, fastest->convex_hull,s_dyn,N);
//                }
//            }
//        }
//        seeds = seeds->next;
//    }
//};

/**
 * Set the safe_mode path for every cell in every abstract_state:
 * 1) Compute invariant sets
 * 2) Find path towards invariant sets for other abstract_states
 */
//void compute_safe_mode(discrete_dynamics *d_dyn,
//                       system_dynamics *s_dyn)
//{
//
//    burn_graph_node * invariant_sets = set_invariant_sets(d_dyn, s_dyn);
//    burning_method(invariant_sets, d_dyn, s_dyn);
//    free(invariant_sets);
//};

//gsl_vector *one_step_input(polytope *A, polytope *B)
//{
//    gsl_vector *u;
//    return u;
//};

//abstract_state *find_current_state(gsl_vector *x,
//                                   discrete_dynamics *d_dyn)
//{
//    abstract_state *now;
//    return now;
//};
//
//void *apply_safe_mode(current_state *now,
//                      discrete_dynamics *d_dyn,
//                      system_dynamics *s_dyn,
//                      void *arg)
//{
//
//    //find correct cell
//    cell * current_cell = NULL;
//    int N = (int)d_dyn->time_horizon;
//    int found = 0;
//    int i;
//    for(i = 0; i < d_dyn->abstract_states_set[now->current_abs_state]->cells_count; i++){
//        found = polytope_check_state(d_dyn->abstract_states_set[now->current_abs_state]->cells[i]->polytope_description, now->x);
//        if(found){
//            current_cell = d_dyn->abstract_states_set[now->current_abs_state]->cells[i];
//            break;
//        }
//    }
//
//    //if not found check all other abstract_states_set all other cells
//    if(current_cell ==NULL){
//        for(i = 0; i<d_dyn->abstract_states_count; i++){
//            for(int j = 0; j < d_dyn->abstract_states_set[i]->cells_count; j++){
//                found = polytope_check_state(d_dyn->abstract_states_set[i]->cells[j]->polytope_description, now->x);
//                if(found){
//                    now->current_abs_state = i;
//                    current_cell = d_dyn->abstract_states_set[i]->cells[j];
//                }
//            }
//        }
//    }
//    int safe_mode_state = 0;
//    found = 0;
//    for(i = N; i>0; i--){
//        found = polytope_check_state(current_cell->safe_mode[i], now->x);
//        if(found){
//            safe_mode_state = i;
//            break;
//        }
//    }
//    pthread_mutex_t *mx = arg;
//
//    //while ! in invariant set
//    while(d_dyn->abstract_states_set[now->current_abs_state]->invariant_set == NULL && !needQuit(mx)){
//        //find polytope of safe_mode
//        int check = polytope_check_state(current_cell->safe_mode[safe_mode_state], now->x);
//        if(!check){
//            safe_mode_state = 0;
//            found = 0;
//            for(i = N; i>0; i--){
//                found = polytope_check_state(current_cell->safe_mode[i], now->x);
//                if(found){
//                    safe_mode_state = i;
//                    break;
//                }
//            }
//        }
//
//        //linear combination to next polytope in line
//        gsl_vector *u = one_step_input(current_cell->safe_mode[safe_mode_state],
//                                               current_cell->safe_mode[safe_mode_state+1]);
//        gsl_vector *w = gsl_vector_alloc(s_dyn->E->size2);
//        simulate_disturbance(w, 0, 0.01);
//        apply_control(now->x, u, s_dyn->A, s_dyn->B, s_dyn->E, w, (size_t)safe_mode_state);
//
//    }
//
//    //while not interrupted stay in invariant set
//
//    while( !needQuit(mx) ) {
//        int check = polytope_check_state(d_dyn->abstract_states_set[now->current_abs_state]->invariant_set->polytope_description, now->x);
//        if(!check){
//            safe_mode_state = 0;
//            found = 0;
//            for(i = N; i>0; i--){
//                found = polytope_check_state(current_cell->safe_mode[i], now->x);
//                if(found){
//                    safe_mode_state = i;
//                    break;
//                }
//            }
//        }
//        one_step_input(d_dyn->abstract_states_set[now->current_abs_state]->invariant_set->polytope_description, d_dyn->abstract_states_set[now->current_abs_state]->invariant_set->polytope_description);
//    }
//    return NULL;
//};