#include "linbcg.hh"
//#define DEBUG

LinBCG::LinBCG()
{
 ijat_p=NULL;
}


void LinBCG::Preconditioner(int periods, int y_kmin, int y_kmax, int Size, std::map<std::pair<std::pair<int, int> ,int>, int> IM_i, int* index_vara, int* index_equa, int y_size, double *y, bool print_it, int type, Mat_O_DP &a, Vec_O_INT &indx)
{
  std::map<std::pair<std::pair<int, int>, int> , int>::iterator it4_i;
  std::map<std::pair< int, int >, double>::iterator it4;
  int i, j, k, eq=0, eqp, var, lag, pos_u;
  std::map<std::pair< int, int >, double> IM;
  LinBCG U_linbcg, L_linbcg;
  Vec_INT ijL, ijU;
  Vec_DP sL, sU;
  double d;

  IM.clear();
  it4_i=IM_i.begin();
  int u_size=IM_i.size();
  //mexPrintf("Preconditioner\n");
  double u_pos_u;
  while(it4_i!=IM_i.end())
    {
      eq= it4_i->first.first.first;
      var=it4_i->first.first.second;
      lag=it4_i->first.second;
      pos_u=it4_i->second;
      if(pos_u>=Size)  // to eliminate the residual contain in the first (Size) elements of IM
        {
          //mexPrintf("IM_i[%d, (%d, %d)]=%d val=%f ",eq, var, lag, pos_u, u[pos_u]);
          IM[make_pair(eq,var-lag*Size)]+=u[pos_u/*+(periods-1)*u_size*/];
          //mexPrintf(" IM[%d, %d]=%f\n",eq,var-lag*Size, IM[make_pair(eq,var-lag*Size)]);
        }
      it4_i++;
    }
  int Size_SparseMatrixRow=IM.size();
  //mexPrintf("Size_SparseMatrixRow+1=%d\n",Size_SparseMatrixRow+1);
  int ija_d[Size_SparseMatrixRow+1];
  DP sa_d[Size_SparseMatrixRow+1];
  k=Size;
  ija_d[0]=k+1;
  j=0;
  eqp=-99;
  it4=IM.begin();
  while(it4!=IM.end())
    {
      eq=it4->first.first;
      var=it4->first.second;
      u_pos_u=it4->second;
      //mexPrintf("IM[eq=%d, var=%d]=%f \n",eq, var, u_pos_u);
      if(eq!=eqp)
        ija_d[eq+j*Size]=k+1;
      if(eq==var)
        sa_d[var+j*Size]+=u_pos_u;//u[pos_u+j*u_size];
      else
        {
          k++;
          sa_d[k]+=u_pos_u;//u[pos_u+j*u_size];
          ija_d[k]=var+j*Size;
        }
      eqp=eq;
      it4++;
    }
  eq=Size;//Size*periods;
  ija_d[eq]=k+1;
  sa_d[eq]=0;
  ija_p.Format(ija_d,Size_SparseMatrixRow+1);
  sa_p.Format(sa_d,Size_SparseMatrixRow+1);
  /*sprsprt();
  sprs_sprt();*/
  //mexPrintf("ok0\n");
  a.Format(Size,Size);
  sprsout(a);
  indx.Format(Size);
  //mexPrintf("ok1\n");
  /*Mat_DP aa(0.0,Size*periods,Size*periods);
  for(k=0;k<periods;k++)
    for(i=0;i<Size;i++)
      for(j=0;j<Size;j++)
        aa[i+k*Size][j+k*Size]=a[i][j];
  a=aa;*/
  /*mexPrintf("a before=\n");
  a.Print();*/
  sprsludcmp(a,indx, d);
  /*mexPrintf("a after=\n");
  a.Print();*/
  //mexPrintf("end of preconditioner\n");
  /*sprs_LU(sL, ijL, sU, ijU);

  U_linbcg.sprsin(sU, ijU);

  mexPrintf("U\n");
  U_linbcg.sprs_sprt();
  U_linbcg.sprsprt();

  mexPrintf("ijL[0]=%d\n",ijL[0]);
  L_linbcg.sprsin(sL, ijL);
  mexPrintf("L\n");
  L_linbcg.sprs_sprt();
  L_linbcg.sprsprt();*/
}


void LinBCG::SolveLinear(int periods, int y_kmin, int y_kmax, int Size, std::map<std::pair<std::pair<int, int> ,int>, int> IM_i, int* index_vara, int* index_equa, int y_size, double *y, bool print_it, bool cvg, Mat_DP &a, Vec_IO_INT &indx)
{
  // Solve iteratively the systme A*x=b
  // A is sparse and rewritten using sa_p and ija_p
  mexPrintf("SolveLinear\n");
  mexEvalString("drawnow;");
  std::map<std::pair<std::pair<int, int>, int> , int>::iterator it4_i;
  std::map<std::pair<int, std::pair<int,int> >, int>::iterator it4;
  int i, j, k, eq=0, eqp, var, lag, pos_u;
  std::map<std::pair< int, std::pair<int,int> >, int> IM;
  clock_t t1 = clock();
  IM.clear();
  it4_i=IM_i.begin();
  int u_size=IM_i.size();
  int Size_SparseMatrixRow=0;
  Vec_DP b(Size*periods), x(Size*periods);
  int ITMAX=10*Size*periods;
  double TOL=1e-9;
  int ITOL=1;
  int sub_iter;
  DP err;
  //Vec_DP Err(Size*periods);

  if (iter>0)
    mexPrintf("Sim : %f ms\n",(1000.0*(double(clock())-double(time00)))/double(CLOCKS_PER_SEC));
  if (isnan(res1) || isinf(res1))
    {
      if (slowc_save<1e-8)
        {
          mexPrintf("Dynare cannot improve the simulation\n");
          mexEvalString("drawnow;");
          filename+=" stopped";
          mexEvalString("st=fclose('all');clear all;");
          mexErrMsgTxt(filename.c_str());
        }
      slowc_save/=2;
      mexPrintf("Error: Simulation diverging, trying to correct it using slowc=%f\n",slowc_save);
      for (i=0;i<y_size*(periods+y_kmin);i++)
        y[i]=ya[i]+slowc_save*direction[i];
      iter--;
      return;
    }
  mexPrintf("-----------------------------------\n");
  mexPrintf("      Simulate     iteration° %d     \n",iter+1);
  mexPrintf("      max. error=%.10e       \n",double(max_res));
  mexPrintf("      sqr. error=%.10e       \n",double(res2));
  mexPrintf("      abs. error=%.10e       \n",double(res1));
  mexPrintf("-----------------------------------\n");
  mexPrintf("cvg=%d\n\n",cvg);
  mexEvalString("drawnow;");
  if (cvg)
    return;
  for(j=0;j<periods;j++)
    for(i=0;i<Size;i++)
      {
        //mexPrintf("b[%d]=-u[%d]=%f\n",i+j*Size,i+j*u_size,-u[i+j*u_size]);
        b[i+j*Size]=-u[i+j*u_size];
      }
  for(i=0;i<Size;i++)
    for(j=y_kmin;j<periods+y_kmin;j++)
      {
        //mexPrintf("x[%d]=y[%d]=%f\n",i+(j-y_kmin)*Size, index_vara[i]+j*y_size, y[index_vara[i]+j*y_size]);
        x[i+(j-y_kmin)*Size]=y[index_vara[i]+j*y_size];
      }
  Size_SparseMatrixRow=periods*(u_size-Size);
  while(it4_i!=IM_i.end())
    {
      eq= it4_i->first.first.first;
      var=it4_i->first.first.second;
      lag=it4_i->first.second;
      pos_u=it4_i->second;
      if(pos_u>=Size)  // to eliminate the residual contain in the first (Size) elements of IM
        {
          //mexPrintf("IM[%d, (%d, %d)]=%d\n",eq, var, lag, pos_u);
          IM[make_pair(eq,make_pair(var,lag))]=pos_u;
          for(j=0;j<=min(y_kmax+y_kmin,periods-1);j++)
            {
              //mexPrintf("j=%d\n",j);
              if(j-y_kmin<=0)
                k=j;
              else
                k=periods+j-y_kmin-y_kmax-1;
              if(lag+j<0 || lag+j>min(periods-1,y_kmax+y_kmin))
                {
                  //mexPrintf("eliminate b[eq+(k)*Size=%d]=u[%d]*y[%d]\n",eq+(k)*Size,pos_u+(k)*u_size,var+(k+1)*y_size);
                  b[eq+(k)*Size]-=u[pos_u+k*u_size]*y[index_vara[var-lag*Size]+(k+lag+1)*y_size];
                  Size_SparseMatrixRow--;
                }

            }
        }
      it4_i++;
    }
  //mexPrintf("Size_SparseMatrixRow+1=%d\n",Size_SparseMatrixRow+1);
  int ija_d[Size_SparseMatrixRow+1];
  DP sa_d[Size_SparseMatrixRow+1];
  k=Size*periods;
  ija_d[0]=k+1;
  for(j=0;j<periods;j++)
    {
      eqp=-99;
      it4=IM.begin();
      while(it4!=IM.end())
        {
          eq=it4->first.first;
          var=it4->first.second.first;
          lag=it4->first.second.second;
          pos_u=it4->second;
          /*if(j==1)
            mexPrintf("IM[eq=%d, var=%d, lag=%d]=%d val=%f \n",eq, var, lag, pos_u,u[pos_u+j*u_size]);*/
          if(lag+j>=0 && lag+j<periods)
            {
              if(eq!=eqp)
                {
                  ija_d[eq+j*Size]=k+1;
                  //mexPrintf("ija_d[%d](1)=%d\n",eq+j*Size,k+1);
                }
              if(eq==var)
                {
#ifdef DEBUG
                  sa_d[var+j*Size]=double(pos_u+j*u_size);
#else
                  sa_d[var+j*Size]=u[pos_u+j*u_size];
#endif
                  //mexPrintf("sa_d[%d]=u[%d]=%f\n",var+j*Size,pos_u+j*u_size,u[pos_u+j*u_size]);
                }
              else
                {
                  if(j+lag>=0 && j+lag<periods)
                    {
                      k++;
#ifdef DEBUG
                      sa_d[k]=double(pos_u+j*u_size);
#else
                      sa_d[k]=u[pos_u+j*u_size];
#endif
                      //mexPrintf("sa_d[%d]=u[%d]=%f\n",k,pos_u+j*u_size,u[pos_u+j*u_size]);
                      ija_d[k]=var+j*Size;
                      //mexPrintf("ija_d[%d](3)=%d\n",k,var+j*Size);
                    }
                }
              eqp=eq;
            }
          it4++;
        }
    }
  eq=Size*periods;
  //mexPrintf("eq=%d\n",eq);
  ija_d[eq]=k+1;
  sa_d[eq]=0;
  /*for(j=0;j<Size_SparseMatrixRow+1;j++)
    {
      mexPrintf("%d ",j);
      mexPrintf("ija=%d ",ija_d[j]);
      mexPrintf("sa=%f\n",sa_d[j]);
    }*/
  //ija_p=(Vec_INT*)mxMalloc(sizeof(Vec_INT));
  ija_p.Format(ija_d,Size_SparseMatrixRow+1);
  //ija_p=;
  //sa_p=(Vec_DP*)mxMalloc(sizeof(Vec_DP));
  sa_p.Format(sa_d,Size_SparseMatrixRow+1);
  //sa_p=;
  /*ija_p=new Vec_INT(ija_d,Size_SparseMatrixRow+1);
  sa_p=new Vec_DP(sa_d,Size_SparseMatrixRow+1);*/
  //sprsprt(*sa_p, *ija_p);
  /*atimes(x,Err,0);
  for(i=0;i<Size*periods;i++)
    {
      Err[i]-=b[i];
      //mexPrintf("Err[%d]=%f\n",i,Err[i]);
    }*/
  BiCGStab(b,x,ITOL,TOL,ITMAX,sub_iter,err, a, indx, periods);
  mexPrintf("Estimated error: %f\n",err);
  mexPrintf("Iterations needed: %d\n",sub_iter);
  //mexPrintf("Solution vector:\n");
  for (i=0;i<y_size*(periods+y_kmin);i++)
    ya[i]=y[i];
  slowc_save=slowc;
  //mexPrintf("slowc=%f\n",slowc);
  for(i=0;i<Size;i++)
    for(j=y_kmin;j<periods+y_kmin;j++)
      {
        direction[index_vara[i]+j*y_size]=x[i+(j-y_kmin)*Size]-y[index_vara[i]+j*y_size];
        y[index_vara[i]+j*y_size]+=slowc*direction[index_vara[i]+j*y_size];
        //mexPrintf("x[%d]=y[%d]=%f\n",i+(j-y_kmin)*Size, index_vara[i]+j*y_size, y[index_vara[i]+j*y_size]);
      }
  /*if(ija_p)
    mxFree(ija_p);
  if(sa_p)
    mxFree(sa_p);*/
  time00=clock();
  if (print_it)
    {
      //pctimer_t t2 = pctimer();
      clock_t t2 = clock();
      mexPrintf("(** %f milliseconds **)\n", 1000.0*(double(t2) - double(t1))/double(CLOCKS_PER_SEC));
      mexEvalString("drawnow;");
    }
}




void LinBCG::asolve(Vec_I_DP &b, Vec_O_DP &x, const int itrnsp, Mat_DP &a, Vec_IO_INT &indx, const int periods)
{
	int i;

	int n=b.size();
	//mexPrintf("n=%d a.nrows()=%d a.ncols()=%d\n",n, a.nrows(), a.ncols());
	//DIAG(A)
	//for(i=0;i<n;i++) x[i]=((sa_p)[i] != 0.0 ? b[i]/(sa_p)[i] : b[i]);
	//Identity
	//for(i=0;i<n;i++) x[i]=b[i];
	//LU
	/*mexPrintf("x=\n");
	x.Print();*/

	/*mexPrintf("b=\n");
	Vec_O_DP(b).Print();*/
	/*mexPrintf("x=\n");
	x.Print();*/
	x=b;
	if(itrnsp)
	  {
	    //lubksb_trp(a, indx, x);
	    lubksb_blck_trp(a, indx, x, periods);
	  }
  else
    {
      //lubksb(a, indx, x);
	    lubksb_blck(a, indx, x, periods);
    }
	/*mexPrintf("solution\n");
  x.Print();

  Vec_DP z(n);

  atimes(x,z,itrnsp);

  //for(i=0;i<n;i++)
    //z[i]=z[i]-b[i];

  mexPrintf("check\n");
  z.Print();*/
}

void LinBCG::atimes(Vec_I_DP &x, Vec_O_DP &r, const int itrnsp)
{
	if (itrnsp) sprstx(sa_p,ija_p,x,r);
	else sprsax(sa_p,ija_p,x,r);
}

DP LinBCG::snrm(Vec_I_DP &sx, const int itol)
{
	int i,isamax;
	DP ans;

	int n=sx.size();
	if (itol <= 3) {
		ans = 0.0;
		for (i=0;i<n;i++) ans += sx[i]*sx[i];
		return sqrt(ans);
	} else {
		isamax=0;
		for (i=0;i<n;i++) {
			if (fabs(sx[i]) > fabs(sx[isamax])) isamax=i;
		}
		return fabs(sx[isamax]);
	}
}

void LinBCG::sprsax(Vec_I_DP &sa, Vec_I_INT &ija, Vec_I_DP &x, Vec_O_DP &b)
{
	int i,k;
  //mexPrintf("sprsax\n");
	int n=x.size();
	/*mexPrintf("n=%d\n",n);
	mexPrintf("ija[0]=%d\n",ija[0]);*/
	if (ija[0] != n+1)
		mexPrintf("Error: \n sprsax: mismatched vector and matrix\n");
  /*mexPrintf("ok0a\n");*/
	for (i=0;i<n;i++) {
		b[i]=sa[i]*x[i];
		for (k=ija[i];k<ija[i+1];k++) {
			b[i] += sa[k]*x[ija[k]];
		}
	}
	/*mexPrintf("ok0b\n");*/
}

void LinBCG::sprstx(Vec_I_DP &sa, Vec_I_INT &ija, Vec_I_DP &x, Vec_O_DP &b)
{
	int i,j,k;
  //mexPrintf("sprstx\n");
	int n=x.size();
	if (ija[0] != (n+1))
		mexPrintf("Error: \n mismatched vector and matrix in sprstx\n");
	for (i=0;i<n;i++) b[i]=sa[i]*x[i];
	for (i=0;i<n;i++) {
		for (k=ija[i];k<ija[i+1];k++) {
			j=ija[k];
			b[j] += sa[k]*x[i];
		}
	}
}


void LinBCG::sprsin(Vec_DP s, Vec_INT ij)
{
  sa_p.Format(s);
  ija_p.Format(ij);
}

void LinBCG::sprsin(DP *s, int *ij, int size)
{
  sa_p.Format(size);
  ija_p.Format(size);
  for(int i=0;i<size;i++)
    {
      sa_p[i]=s[i];
      ija_p[i]=ij[i];
    }
}


void LinBCG::sprsin(double *a, int NbRow, int NbCol, const DP thresh)
{
	int i,j,k;
  int n=NbRow;
  for(i=1, j=0;i<NbRow*NbCol;i++)
    {
      //mexPrintf("%d %% %d = %d a[%d]=%f\n",i,NbRow+1, (i)%(NbRow+1),i,a[i]);
      if(fabs(a[i])>=thresh && ((i)%(NbRow+1))!=0)
        j++;
    }
  //mexPrintf("j=%d\n",j);
	ija_p.Format(j+NbRow+1);
	sa_p.Format(j+NbRow+1);
	//mexPrintf("nz=%d\n",j+NbRow+1);
	for(j=0;j<n;j++)
	  sa_p[j]=a[j+j*n];
	ija_p[0]=n+1;
	k=n;
	for(i=0;i<n;i++)
	  {
		  for(j=0;j<n;j++)
		    {
			    if(fabs(a[j*n+i]) >= thresh && i != j)
			      {
				      sa_p[++k]=a[j*n+i];
				      ija_p[k]=j;
			      }
		    }
		  ija_p[i+1]=k+1;
	  }
}

void LinBCG::sprsin(Mat_DP &a, const DP thresh)
{
	int i,j,k=0;

	int n=a.nrows();
	for(i=0;i<n;i++)
	  for(j=0;j<n;j++)
	    if(fabs(a[i][j])>=thresh && j!=i)
	      k++;
	ija_p.Format(k+n+1);
	sa_p.Format(k+n+1);
	for (j=0;j<n;j++) sa_p[j]=a[j][j];
	ija_p[0]=n+1;
	k=n;
	for (i=0;i<n;i++) {
		for (j=0;j<n;j++) {
			if (fabs(a[i][j]) >= thresh && i != j) {
				//if (++k > nmax) nrerror("sprsin: sa and ija too small");
				k++;
				sa_p[k]=a[i][j];
				ija_p[k]=j;
			}
		}
		ija_p[i+1]=k+1;
	}
}


void LinBCG::sprsout(Mat_DP &a)
{
  int i,j,k,l,ke;
  double maxi=0;
  int N=ija_p[0]-1;
  //int total=ija_p[N];
  //mexPrintf("sprsout\n");
  for(i=0;i<N;i++)
    {
      l=ija_p[i];
      k=ija_p[l];
      ke=ija_p[i+1];
      for(j=0;j<N;j++)
        {
          //mexPrintf("i=%d, j=%d\n",i,j);
          if(i==j)
            //mexPrintf(fmt,sa_p[i]);
            a[i][i]=sa_p[i];
          else if(j==k)
            {
              //mexPrintf(fmt,sa_p[l]);
              a[i][j]=sa_p[l];
              if(l+1<ke)
                {
                  l++;
                  k=ija_p[l];
                }
            }
          else
            //mexPrintf(fmt,0.0);
            a[i][j]=0;
        }
      //mexPrintf("\n");
    }
  //mexPrintf("\n");
  //mexPrintf("end of sprsout\n");
}



void LinBCG::sprsprt()
{
  int i,j,k,l,ke;
  double maxi=0;
  int N=ija_p[0]-1;
  int total=ija_p[N];
  char fmt[12];
  memset(fmt,'\0',12);
  string pf;
  for(i=0;i<total;i++)
    if(fabs(sa_p[i])>maxi)
      maxi=fabs(sa_p[i]);
  int size=int(log10(maxi))+1;
  if(size<7)
    {
      itoa(size+8,fmt,10);
      pf= fmt;
      pf.insert(0,"% ");
      pf.append(".6f ");
      pf.copy(fmt,8);
    }
  else
    strcpy(fmt,"%e ");
  for(i=0;i<N;i++)
    {
      l=ija_p[i];
      k=ija_p[l];
      ke=ija_p[i+1];
      for(j=0;j<N;j++)
        {
          if(i==j)
            mexPrintf(fmt,sa_p[i]);
          else if(j==k)
            {
              mexPrintf(fmt,sa_p[l]);
              if(l+1<ke)
                {
                  l++;
                  k=ija_p[l];
                }
            }
          else
            mexPrintf(fmt,0.0);
        }
      mexPrintf("\n");
    }
  mexPrintf("\n");
}


void LinBCG::sprs_sprt()
{
  int i,j,k,l,ke;
  double maxi=0;
  int N=ija_p[0]-1;
  mexPrintf("N=%d\n",N);
  int total=ija_p[N];
  mexPrintf("total=%d\n",total);
  char fmt[12], fmts[12], fmtd[12];
  memset(fmt,'\0',12);
  memset(fmts,'\0',12);
  memset(fmtd,'\0',12);
  string pf, pf1, pf2;
  for(i=0;i<total;i++)
    if(fabs(sa_p[i])>maxi)
      maxi=fabs(sa_p[i]);
  int size=int(log10(maxi))+1;
  if(size<7)
    {
      itoa(size+8,fmt,10);
      pf= fmt;
      pf.insert(0,"% ");
      pf1=pf2=pf;
      pf.append(".6f ");
      pf1.append("s ");
      pf2.append("d ");
      pf.copy(fmt,8);
      pf1.copy(fmts,8);
      pf2.copy(fmtd,8);
    }
  else
    strcpy(fmt,"%e ");
  mexPrintf(fmts,"i");
  mexPrintf(fmts,"ija");
  mexPrintf(fmts,"sa");
  if(ijat_p.size())
    {
      mexPrintf(fmts,"ijat");
      mexPrintf(fmts,"sat");
    }
  mexPrintf("\n");
  for(i=0;i<total;i++)
    {
      mexPrintf(fmtd,i);
      mexPrintf(fmtd,ija_p[i]);
      mexPrintf(fmt,sa_p[i]);
      if(ijat_p.size())
        {
          mexPrintf(fmtd,ijat_p[i]);
          mexPrintf(fmt,sat_p[i]);
        }
      mexPrintf("\n");
    }
  mexPrintf("\n");
}

void LinBCG::BiCG(Vec_I_DP &b, Vec_IO_DP &x, const int itol, const DP tol,
	const int itmax, int &iter, DP &err, Mat_DP &a, Vec_IO_INT &indx, const int periods)
{
	DP ak,akden,bk,bkden=1.0,bknum,bnrm=0,dxnrm,xnrm,zm1nrm,znrm=0;
	const DP EPS=1.0e-14;
	int j;
	int n=b.size();
	//mexPrintf("n=%d\n",n);
	Vec_DP p(n),pp(n),r(n),rr(n),z(n),zz(n);
	iter=0;
	atimes(x,r,0);
	for (j=0;j<n;j++)
	  {
      r[j]=b[j]-r[j];
	    rr[j]=r[j];
	  }
	//atimes(r,rr,0);
	if (itol == 1)
	  {
		  bnrm=snrm(b,itol);
		  asolve(r,z,0, a, indx, periods);
	  }
	else if (itol == 2)
	  {
		  asolve(b,z,0, a, indx, periods);
		  bnrm=snrm(z,itol);
		  asolve(r,z,0, a, indx, periods);
	  }
	else if (itol == 3 || itol == 4)
	  {
		  asolve(b,z,0, a, indx, periods);
		  bnrm=snrm(z,itol);
		  asolve(r,z,0, a, indx, periods);
		  znrm=snrm(z,itol);
	  }
  else
    mexPrintf("Error: \n illegal itol in BiCG\n");
	//cout << fixed << setprecision(6);
	while (iter < itmax)
	  {
		  ++iter;
		  asolve(rr,zz,1, a, indx, periods);
		  for (bknum=0.0,j=0;j<n;j++)
		    bknum += z[j]*rr[j];
		  if (iter == 1)
		    {
			    for (j=0;j<n;j++)
			      {
				      p[j]=z[j];
				      pp[j]=zz[j];
			      }
		    }
		  else
		    {
			    bk=bknum/bkden;
			    for (j=0;j<n;j++)
			      {
				      p[j]=bk*p[j]+z[j];
				      pp[j]=bk*pp[j]+zz[j];
			      }
		    }
		  bkden=bknum;
		  atimes(p,z,0);
		  for(akden=0.0,j=0;j<n;j++)
		    akden += z[j]*pp[j];
		  ak=bknum/akden;
		  atimes(pp,zz,1);
		  for (j=0;j<n;j++)
		    {
			    x[j] += ak*p[j];
			    r[j] -= ak*z[j];
			    rr[j] -= ak*zz[j];
		    }
		  asolve(r,z,0, a, indx, periods);
		  if (itol == 1)
			  err=snrm(r,itol)/bnrm;
		  else if (itol == 2)
			  err=snrm(z,itol)/bnrm;
		  else if (itol == 3 || itol == 4)
		    {
			    zm1nrm=znrm;
			    znrm=snrm(z,itol);
			    if (fabs(zm1nrm-znrm) > EPS*znrm)
			      {
				      dxnrm=fabs(ak)*snrm(p,itol);
				      err=znrm/fabs(zm1nrm-znrm)*dxnrm;
			      }
			    else
			      {
				      err=znrm/bnrm;
				      continue;
			      }
			    xnrm=snrm(x,itol);
			    if (err <= 0.5*xnrm)
			      err /= xnrm;
			    else
			      {
				      err=znrm/bnrm;
				      continue;
			      }
		    }
		  //cout << "iter=" << setw(4) << iter+1 << setw(12) << err << endl;
		  mexPrintf("iter=%d err=%f ak=%f akden=%f\n",iter,err,ak,akden);
		  if (err <= tol)
		    break;
	  }
}

void LinBCG::BiCGStab(Vec_I_DP &b, Vec_IO_DP &x, const int itol, const DP tol,
	const int itmax, int &iter, DP &err, Mat_DP &a, Vec_IO_INT &indx, const int periods)
{
	DP akden,bkden=1.0,bknum,bnrm=0,dxnrm,xnrm,zm1nrm,znrm=0, alpha, omega, beta, omega_num, omega_den, snorm;
	const DP EPS=1.0e-14;
	int j;
	int n=b.size();
	//mexPrintf("n=%d\n",n);
	Vec_DP p(n),v(n),r(n),rr(n),z(n),s(n), t(n), phat(n), shat(n);
	iter=0;
	// Compute the residual
	atimes(x,r,0);
	for (j=0;j<n;j++)
	  {
      r[j]=b[j]-r[j];
	    rr[j]=r[j];
	  }
	//atimes(r,rr,0);
	if (itol == 1)
	  {
		  bnrm=snrm(b,itol);
		  //mexPrintf("asolve(r,z,..)\n");
		  asolve(r,z,0, a, indx, periods);
	  }
	else if (itol == 2)
	  {
		  asolve(b,z,0, a, indx, periods);
		  bnrm=snrm(z,itol);
		  asolve(r,z,0, a, indx, periods);
	  }
	else if (itol == 3 || itol == 4)
	  {
		  asolve(b,z,0, a, indx, periods);
		  bnrm=snrm(z,itol);
		  asolve(r,z,0, a, indx, periods);
		  znrm=snrm(z,itol);
	  }
  else
    mexPrintf("Error: \n illegal itol in BiCGStab\n");
	//cout << fixed << setprecision(6);
	while (iter < itmax)
	  {
		  ++iter;
		  //asolve(rr,zz,1, a, indx);
		  for (bknum=0.0,j=0;j<n;j++)
		    bknum += r[j]*rr[j];
		  if (iter == 1)
		    {
			    for (j=0;j<n;j++)
			      {
			        p[j]=r[j];
			      }
		    }
		  else
		    {
			    beta=bknum/bkden*(alpha/omega);
			    for (j=0;j<n;j++)
			      {
			        p[j]=r[j]+beta*(p[j]-omega*v[j]);
			      }
		    }
      //mexPrintf("asolve(p,phat,..)\n");
      asolve(p,phat,1, a, indx, periods);

      atimes(phat,v,0);
      for(akden=0.0,j=0;j<n;j++)
		    akden += rr[j]*v[j];
      alpha=bknum/akden;
      for(snorm=0.0, j=0;j<n;j++)
        {
          s[j]=r[j]-alpha*v[j];
          snorm+=s[j]*s[j];
        }
      if(sqrt(snorm)<tol)
        {
          for(j=0;j<n;j++)
            x[j]+=alpha*phat[j]+omega*shat[j];
          break;
        }
      //mexPrintf("asolve(s,shat,..)\n");
      asolve(s,shat,1, a, indx, periods);
      atimes(shat,t,0);
      for(omega_num=0.0,omega_den=0.0,j=0;j<n;j++)
        {
          omega_num+=t[j]*s[j];
          omega_den+=t[j]*t[j];
        }
      omega=omega_num/omega_den;
      for(j=0;j<n;j++)
        {
          x[j]+=alpha*phat[j]+omega*shat[j];
          r[j]=s[j]-omega*t[j];
        }
		  bkden=bknum;


		  if (itol == 1)
			  err=snrm(r,itol)/bnrm;
		  else if (itol == 2)
			  err=snrm(z,itol)/bnrm;
		  else if (itol == 3 || itol == 4)
		    {
			    zm1nrm=znrm;
			    znrm=snrm(z,itol);
			    if (fabs(zm1nrm-znrm) > EPS*znrm)
			      {
				      dxnrm=fabs(alpha)*snrm(p,itol);
				      err=znrm/fabs(zm1nrm-znrm)*dxnrm;
			      }
			    else
			      {
				      err=znrm/bnrm;
				      continue;
			      }
			    xnrm=snrm(x,itol);
			    if (err <= 0.5*xnrm)
			      err /= xnrm;
			    else
			      {
				      err=znrm/bnrm;
				      continue;
			      }
		    }
		  //cout << "iter=" << setw(4) << iter+1 << setw(12) << err << endl;
		  mexPrintf("iter=%d err=%f alpha=%f akden=%f\n",iter,err,alpha,akden);
		  mexEvalString("drawnow;");
		  if (err <= tol)
		    break;
	  }
}


void LinBCG::sprs_col_index()
{
  int i, j, k, begin, end;
  int nze=sa_p.size();
  ijat_p.Format(nze);
  sat_p.Format(nze);
  begin=ija_p[0];
  int n=begin-1;
  //mexPrintf("n=%d\n",n);
  map<std::pair<int, int>, double> list;
  map<std::pair<int, int>, double>::iterator list_it;
  for(j=0;j<n;j++)
    sat_p[j]=sa_p[j];
  sat_p[n]=sa_p[n];
  ijat_p[n]=ija_p[n];
  for(i=0;i<n;i++)
    {
      end=ija_p[i+1];
      for(k=begin;k<end;k++)
        {
          //mexPrintf("make_pair(ija_p[k]=%d, i=%d)=%f\n",ija_p[k],i,sa_p[k]);
          list[make_pair(ija_p[k],i)]=sa_p[k];
        }
      begin=end;
    }
  int col=-1;
  int cur_pos=n+1;
  list_it=list.begin();
  while(list_it!=list.end())
    {
      //mexPrintf("list_it->first.first=%d, list_it->first.second=%d, list_it->second=%f\n",list_it->first.first, list_it->first.second, list_it->second);
      while(col!=list_it->first.first)
        {
          //while(col!=list_it->first.first
          col++;
          ijat_p[col]=cur_pos;
        }
      ijat_p[cur_pos]=list_it->first.second;
      sat_p[cur_pos++]=list_it->second;
      list_it++;
    }
}


void LinBCG::sprs_swap_line_copy(map<std::pair<int, int>, double> &list, int &pos_2_blck, int begin, int end)
{
  int i, j, init_pos;
  for(i=begin;i<end;i++)
    {
      init_pos=pos_2_blck;
      list[make_pair(i,init_pos)]=sa_p[i];
      for(j=ija_p[i];j<ija_p[i+1];j++)
        {
          //mexPrintf("list[%d,%d]=%f\n",pos_2_blck,ija_p[j],sa_p[j]);
          list[make_pair(pos_2_blck++,ija_p[j])]=sa_p[j];
        }
    }
}


void LinBCG::sprs_swap_line_exchange(map<std::pair<int, int>, double> &list, int &pos_2_blck, int LS, int LD)
{
  int j;
  bool OK=false, OK1=true;
  int init_pos=pos_2_blck;
  for(j=ija_p[LS];j<ija_p[LS+1];j++)
    {
      if(ija_p[j]==LD)
        {
          OK=true;
          list[make_pair(LD,init_pos)]=sa_p[j];
          //mexPrintf("found list[%d,%d]=%f\n",LD,init_pos,sa_p[j]);
        }
      else
        {
          if(ija_p[j]>=LS && OK1)
            {
              OK1=false;
              if(sa_p[LS]!=0.0)
                {
                  //mexPrintf("list[%d,%d]=%f\n",pos_2_blck,LS,sa_p[LS]);
                  list[make_pair(pos_2_blck++,LS)]=sa_p[LS];
                }
            }
          //mexPrintf("list[%d,%d]=%f\n",pos_2_blck,ija_p[j],sa_p[j]);
          list[make_pair(pos_2_blck++,ija_p[j])]=sa_p[j];
        }
    }
  if(OK1)
    {
      if(sa_p[LS]!=0.0)
        {
          //mexPrintf("list[%d,%d]=%f\n",pos_2_blck,LS,sa_p[LS]);
          list[make_pair(pos_2_blck++,LS)]=sa_p[LS];
        }
    }
  if(!OK)
    {
      list[make_pair(LD,init_pos)]=0.0;
      //mexPrintf("not found list[%d,%d]=%f\n",LD,init_pos,0.0);
    }
}

void LinBCG::sprs_swap_line(int L0, int L1)
{
  int i,j,k, pos_2_blck, init_pos;
  map<std::pair<int, int>, double> list;
  map<std::pair<int, int>, double>::iterator list_it;
  int n=ija_p[0]-1;
  if(L0>L1)
    {
      i=L0;
      L0=L1;
      L1=i;
    }
  else if(L0==L1)
    return;
  else if(L0>=n || L1>=n)
    {
      mexPrintf("out of range in sprs_swap_line (L0=%d and L1=%d)\n",L0, L1);
      mexEvalString("drawnow;");
      mexEvalString("st=fclose('all');clear all;");
      filename+=" stopped";
      mexErrMsgTxt(filename.c_str());
    }
  int nze=sa_p.size();
  bool OK, OK1;
  int nze0=0, nze1=0;
  pos_2_blck=n+1;

  sprs_swap_line_copy(list, pos_2_blck, 0, L0);

  //mexPrintf("L1=>L0\n");

  sprs_swap_line_exchange(list, pos_2_blck, L1, L0);

  sprs_swap_line_copy(list, pos_2_blck, L0+1, L1);

  //mexPrintf("L0=>L1\n");

  sprs_swap_line_exchange(list, pos_2_blck, L0, L1);

  sprs_swap_line_copy(list, pos_2_blck, L1+1, n);

  sa_p.Format(list.size()+1);
  ija_p.Format(list.size()+1);
  //mexPrintf("list.size()+1=%d\n",list.size()+1);
  list_it=list.begin();
  while(list_it!=list.end())
    {
      //mexPrintf("sa_p[list_it->first.first=%d]=list_it->second=%f\n",list_it->first.first,list_it->second);
      sa_p[list_it->first.first]=list_it->second;
      //mexPrintf("ija_p[list_it->first.first=%d]=list_it->first.second=%d\n",list_it->first.first,list_it->first.second);
      ija_p[list_it->first.first]=list_it->first.second;
      list_it++;
    }
  ija_p[n]=list.size()+1;
  sa_p[n]=0;
  sprs_col_index();
}



void LinBCG::sprsludcmp(Mat_DP &a, Vec_O_INT &indx, DP &d)
{
	const DP TINY=1.0e-20;
	int i,imax,j,k, ii, ik, begin_ija, end_ija, begin_ijat, end_ijat, dum1, dum2;
	DP big,dum,sum,temp;
	Vec_DP saa_p(sa_p);
	Vec_INT ijaa_p(ija_p);
  int nze=sa_p.size();
	int n=ija_p[0]-1;
	Vec_DP vv(n);
	d=1.0;

  n=a.nrows();
	//a=(double**)mxMalloc(n*n*sizeof(double));
  //mexPrintf("sprsludcmp\n");

	for (i=0;i<n;i++) {
		big=0.0;
		for (j=0;j<n;j++)
		  {
		    //mexPrintf("%f ",a[i][j]);
			  if ((temp=fabs(a[i][j])) > big) big=temp;
		  }
    //mexPrintf("\n");
		if(big == 0.0)
      {
        mexPrintf("Singular Matrix in routine sprsludcmp\n");
        mexEvalString("st=fclose('all');clear all;");
        mexErrMsgTxt("End of simulate");
      }
		vv[i]=1.0/big;
	}
  /*mexPrintf("First Step\n");
  mexPrintf("a.ncols()=%d\n",a.ncols());
  mexPrintf("a.nrows()=%d\n",a.nrows());*/
	/*for(i=0;i<n;i++)
	  {
	    big=fabs(sa_p[i]);
	    for(j=ija_p[i];j<ija_p[i+1];j++)
	      if(temp=fabs(sa_p[j])>big)
	        big=temp;
      if(big==0.0)
        {
          mexPrintf("Singular Matrix in routine sprsludcmp\n");
          mexErrMsgTxt("End of simulate");
        }
      vv[i]=1.0/big;
	  }*/
  /*sprs_col_index();  // create a col-index compact storage
  for (j=0;j<n;j++)
    {
      //imax=j;
		  for (i=ijat_p[j];i<ijat_p[j+1] && ijat_p[i]<j;i++)
		    {
			    sum=sat_p[i];
          begin_ija=ija_p[i];
          end_ija=ija_p[i+1];
          begin_ijat=ijat_p[i];
          end_ijat=ijat_p[i+1];
          while(begin_ija<end_ija && begin_ijat<end_ijat && begin_ijat<begin_ija)
            {
              if(ija_p[begin_ija]<ijat_p[begin_ijat])
                begin_ija++;
              else if(ija_p[begin_ija]>ijat_p[begin_ijat])
                begin_ijat++;
              else
                sum -= sa_p[begin_ija++]*sat_p[begin_ijat++];
            }
          sat_p[i]=sum;
		    }
      big=0.0;
      for (;i<ijat_p[j+1] ;i++)
		    {
          sum=sat_p[i];
          begin_ija=ija_p[i];
          end_ija=ija_p[i+1];
          begin_ijat=ijat_p[i];
          end_ijat=ijat_p[i+1];
          while(begin_ija<end_ija && ija_p[begin_ija]<j && begin_ijat<end_ijat && ijat_p[begin_ijat]<j)
            {
              if(ija_p[begin_ija]<ijat_p[begin_ijat])
                begin_ija++;
              else if(ija_p[begin_ija]>ijat_p[begin_ijat])
                begin_ijat++;
              else
                sum -= sa_p[begin_ija++]*sat_p[begin_ijat++];
            }
          sat_p[i]=sum;
          if((dum=vv[ija_p[i]]*fabs(sum)) >= big)
            {
				      big=dum;
				      mexPrintf("imax=i=%d\n",i);
				      imax=ija_p[i];
			      }
		    }
     if (j != imax)
       {
         sprs_swap_line(j,imax);
			   //for (k=0;k<n;k++)
			   //  {
			   //    dum=a[imax][k];
				 //    a[imax][k]=a[j][k];
				 //    a[j][k]=dum;
			   //  }
			   d = -d;
			   vv[imax]=vv[j];
       }
     indx[j]=imax;
     if (sa_p[j] == 0.0) sa_p[j]=TINY;
		   if (j != n-1)
  		   {
	  		   dum=1.0/(sat_p[j]);
		  	   for (i=ija_p[j];i<ija_p[j+1];i++)
		  	     if(ija_p[j]>j)
		  	       sa_p[i] *= dum;
		     }
    }
}*/
	for (j=0;j<n;j++)
	  {
		  for (i=0;i<j;i++)
		    {
			    sum=a[i][j];
			    for (k=0;k<i;k++)
			      sum -= a[i][k]*a[k][j];
			    a[i][j]=sum;
		    }
		  big=0.0;
		  //mexPrintf("ok0\n");
		  for (i=j;i<n;i++)
		    {
			    sum=a[i][j];
			    for (k=0;k<j;k++)
			      sum -= a[i][k]*a[k][j];
			    a[i][j]=sum;
			    if ((dum=vv[i]*fabs(sum)) >= big)
			      {
				      big=dum;
				      imax=i;
			      }
		    }
      //mexPrintf("ok1\n");
		  if (j != imax)
		    {
			    for (k=0;k<n;k++)
			      {
				      dum=a[imax][k];
				      a[imax][k]=a[j][k];
				      a[j][k]=dum;
			      }
			    d = -d;
			    vv[imax]=vv[j];
		    }
		  indx[j]=imax;
		  //mexPrintf("ok2\n");
		  if (a[j][j] == 0.0)
		    a[j][j]=TINY;
		  if (j != n-1)
		    {
			    dum=1.0/(a[j][j]);
			    for (i=j+1;i<n;i++)
			      a[i][j] *= dum;
		    }
      //mexPrintf("ok3\n");
	  }
  mexPrintf("end of sprsludcmp\n");
  //sprsin(a,1e-10);
  //mxFree(a);
}



void LinBCG::lubksb_blck(Mat_I_DP &a, Vec_I_INT &indx, Vec_IO_DP &b, const int periods)
{
	int i,ii,ip,j, k;
	DP sum;
	int n=a.nrows();
	for(k=0;k<periods;k++)
	  {
	    ii=0;
	    for (i=0;i<n;i++)
	      {
    		  ip=indx[i];
	    	  sum=b[ip+k*n];
		      b[ip+k*n]=b[i+k*n];
		      /*if (ii != 0)
			      for (j=ii-1;j<i;j++)
			        sum -= a[i][j]*b[j+k*n];
		      else if (sum != 0.0)
			      ii=i+1;*/
		      for (j=0;j<i;j++)
		        sum -= a[i][j]*b[j+k*n];
		      b[i+k*n]=sum;
	      }
	  }
  for(k=periods-1;k>=0;k--)
    {
	    for (i=n-1;i>=0;i--)
	      {
		      sum=b[i+k*n];
		      for (j=i+1;j<n;j++)
		        sum -= a[i][j]*b[j+k*n];
          if(fabs(a[i][i])<tol)
            {
              mexPrintf("Error: Singularity in lubksb_blck a[%d][%d](max=%d)=%g\n",i,i,n,a[i][i]);
              mexEvalString("st=fclose('all');clear all;");
              mexErrMsgTxt("Exit from Dynare");
            }
		      b[i+k*n]=sum/a[i][i];
	      }
	  }
}

void LinBCG::lubksb_blck_trp(Mat_I_DP &a, Vec_I_INT &indx, Vec_IO_DP &b, const int periods)
{
	int i,ii,ip,j, k;
	DP sum;
	int n=a.nrows();
	for(k=0;k<periods;k++)
	  {
	    ii=0;
	    for (i=0;i<n;i++)
	      {
    		  ip=indx[i];
	    	  sum=b[ip+k*n];
		      b[ip+k*n]=b[i+k*n];
		      /*if (ii != 0)
			      for (j=ii-1;j<i;j++)
			        sum -= a[j][i]*b[j+k*n];
		      else if (sum != 0.0)
			      ii=i+1;*/
  	      for (j=0;j<i;j++)
			      sum -= a[j][i]*b[j+k*n];
          if(fabs(a[i][i])<tol)
            {
              mexPrintf("Error: Singularity in lubksb_blck_trp a[%d][%d](max=%d)=%g\n",i,i,n,a[i][i]);
              mexEvalString("st=fclose('all');clear all;");
              mexErrMsgTxt("Exit from Dynare");
            }
		      b[i+k*n]=sum/a[i][i];
	      }
	  }
  for(k=periods-1;k>=0;k--)
    {
	    for (i=n-1;i>=0;i--)
	      {
		      sum=b[i+k*n];
		      for (j=i+1;j<n;j++)
		        sum -= a[j][i]*b[j+k*n];
		      b[i+k*n]=sum;
	      }
	  }
}



void LinBCG::lubksb(Mat_I_DP &a, Vec_I_INT &indx, Vec_IO_DP &b)
{
  // Solving A.x=b
	int i,ii,ip,j;
	DP sum;
	int n=a.nrows();
  ii=0;
  // Solving L.y=b
  // L is a lower triangular matrix with element on the main diagonal equal to 1
  // Implementation of a foreward substitution to solve this system
  for (i=0;i<n;i++)
    {
 		  ip=indx[i];
  	  sum=b[ip];
      b[ip]=b[i];
      if (ii != 0)
	      for (j=ii-1;j<i;j++)
	        sum -= a[i][j]*b[j];
	    else if (sum != 0.0)
			  ii=i+1;
		  b[i]=sum;
    }
  // Solving U.x=y
  // U is an upper triangular matrix with element on the main diagonal is different from 1
  // Implementation of a backward substitution to solve this system
  for (i=n-1;i>=0;i--)
    {
      sum=b[i];
	    for (j=i+1;j<n;j++)
	      sum -= a[i][j]*b[j];
      if(fabs(a[i][i])<tol)
        {
          mexPrintf("Error: Singularity in lubksb a[%d][%d](max=%d)=%g\n",i,i,n,a[i][i]);
          mexEvalString("st=fclose('all');clear all;");
          mexErrMsgTxt("Exit from Dynare");
        }
	    b[i]=sum/a[i][i];
	  }
}


void LinBCG::lubksb_trp(Mat_I_DP &a, Vec_I_INT &indx, Vec_IO_DP &b)
{
  // Solving A'.x=b <=> (LU)'.x=b <=> U'L'x=b <=> U'.y=b with x solution of L'x=y
	int i,ii,ip,j;
	DP sum;
	int n=a.nrows();
  ii=0;
  // Solving U'.y=b
  // U' is a lower triangular matrix with element on the main diagonal differnet from 1
  // Implementation of a foreward substitution to solve this system
  for (i=0;i<n;i++)
    {
 		  ip=indx[i];
  	  sum=b[ip];
      b[ip]=b[i];
      if (ii != 0)
	      for (j=ii-1;j<i;j++)
	        sum -= a[j][i]*b[j];
	    else if (sum != 0.0)
			  ii=i+1;
      if(fabs(a[i][i])<tol)
        {
          mexPrintf("Error: Singularity in lubksb_trp a[%d][%d](max=%d)=%g\n",i,i,n,a[i][i]);
          mexEvalString("st=fclose('all');clear all;");
          mexErrMsgTxt("Exit from Dynare");
        }
		  b[i]=sum/a[i][i];
    }
  // Solving L'.x=y
  // L' is an upper triangular matrix with element on the main diagonal equal to 1
  // Implementation of a backward substitution to solve this system
  for (i=n-1;i>=0;i--)
    {
      sum=b[i];
	    for (j=i+1;j<n;j++)
	      sum -= a[j][i]*b[j];
	    b[i]=sum;
	  }
}


void LinBCG::sprs_LU(Vec_O_DP &sL, Vec_O_INT &ijL, Vec_O_DP &sU, Vec_O_INT &ijU)
{
  int j,k, nU, nL;
  int n=ija_p[0]-1;
  int L_count, A_count, U_count;
  L_count=A_count=U_count=n+1;
  int nn=ija_p.size();
  Vec_INT indx(n);
  Vec_INT save_ija_p(ija_p);
  Vec_DP save_sa_p(sa_p);
  DP d;

  mexPrintf("sprs_LU\n");

  Mat_DP a(n,n);

  sprsludcmp(a,indx, d);

  Vec_DP res(n), res_save(n);
  for(int i=0;i<n;i++)
    res_save[i]=res[i]=i;

  a.Print();

  //lubksb(a, indx, res, 0);
  lubksb(a, indx, res);

  mexPrintf("solution\n");
  res.Print();

  Vec_DP z(n);

  atimes(res,z,0);

  mexPrintf("check\n");
  z.Print();

  nU=nL=n;
  for(k=0;k<n;k++)
   for(j=ija_p[k];j<ija_p[k+1];j++)
     if(ija_p[j]<k)
        nU++;
     else
        nL++;
  mexPrintf("nU+1=%d, nL+1=%d sa_p.size()=%d\n",nU+1, nL+1, sa_p.size());
  ijU.Format(nU+1);
  sU.Format(nU+1);

  ijL.Format(nL+1);
  //mexPrintf("ijL[0]=%d\n",ijL[0]);
  sL.Format(nL+1);

  for(k=0;k<n;k++)
   {
     //the diagonal elements
     sU[k]=sa_p[k];
     sL[k]=1.0;

     ijU[k]=U_count;
     ijL[k]=L_count;
     for(j=ija_p[k];j<ija_p[k+1];j++)
       {
         if(ija_p[j]<k)
           {
             mexPrintf("U_count=%d, A_count=%d\n",U_count, A_count);
             ijU[U_count]=ija_p[A_count];
             sU[U_count++]=sa_p[A_count++];
           }
         else
           {
             mexPrintf("L_count=%d, A_count=%d\n",L_count, A_count);
             ijL[L_count]=ija_p[A_count];
             sL[L_count++]=sa_p[A_count++];
           }
       }
   }
  sU[n]=0.0;
  sL[n]=0.0;
  ijU[n]=nU+1;
  ijL[n]=nL+1;
  mexPrintf("ijL[0]=%d\n",ijL[0]);
  mexPrintf("end0 of sprs_LU\n");
  sa_p.Format(save_sa_p);
  ija_p.Format(save_ija_p);
  mexPrintf("end of sprs_LU\n");
}

void LinBCG::Initialize(string filename_arg, double res1_arg, double res2_arg, double max_res_arg, double slowc_arg, double *ya_arg, double *direction_arg, int iter_arg)
{
  filename=filename_arg;
  res1=res1_arg;
  res2=res2_arg;
  max_res=max_res_arg;
  slowc=slowc_arg;
  slowc_save=slowc;
  ya=ya_arg;
  direction=direction_arg;
  iter=iter_arg;
}