#include <stdio.h>
#include <math.h>
#include <stdbool.h>
#include <stdlib.h>

/*---------------------------------------*/
/*     .. BLAS and LAPACK ..             */
/*---------------------------------------*/
/*     subroutine daxpy
 *     subroutine dcopy
 *     subroutine dgemv
 *     subroutine dgetrf
 *     subroutine dgetrs
 *     subroutine dlacpy
 *     subroutine dlaset
 *     subroutine dsymv
 *     function ddot
 *     function dnrm2
 */
void daxpy_(int *n, double *da, double *dx, int *incx, double *dy, int *incy); /* y = a*x + y */
void dcopy_(int *n, double *dx, int *incx, double *dy, int *incy); /* dcopy == Copy a vector, x, to a vector, y */
void dgemv_(char *trans,int *m,int *n,double *alpha,double *a,int *lda,double *x,int *incx,double *beta,double *y,int *incy);
void dgetrf_(int *m, int *n, double *a, int *lda, int *ipiv, int *info);
void dgetrs_(char *trans, int *n, int *nrhs, double *a, int *lda, int *ipiv, double *b, int *ldb, int *info);
void dlacpy_(char *uplo,int *m,int *n,double *a, int *lda, double *b,int *ldb);
void dlaset_(char *uplo,int *m,int *n,double *alpha,double *beta,double *a,int *lda);
void dsymv_(char *uplo, int *n, double *alpha, double *a, int *lda, double *x, int *incx, double *beta, double *y, int *incy);
double ddot_(int *n, double *dx, int *incx, double *dy, int *incy); /* ddot == Dot product of two vectors */
double dnrm2_(int *n, double *x, int *incx); /* dnrm2 == Euclidean norm of a vector */

/*     .. Intrinsic ..
 *
 *      C   intrinsic:  fabs, sqrt
 *      C99 intrinsic:  fmax, copysign
 *
 */

void bicgstabl(int l, int n, double *x, bool nonzero_x, double *b,
               void (*matvec)(int, double *, double *), /* Compute the matrix-vector product: y=A*x */
	       void (*l_precond)(int, double *), /* Apply a left preconditioner if pcnd=true, i.e. solve the system: M*y=x */
               bool pcnd, double *toler, int *mxmatvec, int *info)
{

/* ===purpose=== */

/**********************************************************************************************************************************/
/**************************** bicgstabl: (Preconditioned Real) Bi-Conjugate Gradient Stabilized solver ****************************/
/****************************  Iterative method for solving real non-symmetric linear system: A*x=b *******************************/
/**********************************************************************************************************************************/

/* Developer: Sylvain Nintcheu Fata */
/* Version:   V.1.3                 */
/* Released:  Jun 14, 2012          */


/******************************************************/
/* Parameter list:                                    */
/******************************************************/
/*
 * l         == (input) INTEGER BiCGstab's dimension
 * n         == (input) INTEGER size of the system to solve 
 * x         == (input/output) DOUBLE PRECISION array dimension n, initial guess on input, solution on output
 * nonzero_x == (input) LOGICAL, indicates whether the initial guess is zero vector
 *              If nonzero_x == .false., one matrix-vector multiplication will be saved
 * b         == (input) DOUBLE PRECISION array dimension n, right-hand side (rhs) vector
 * matvec    == (input) EXTERNAL name of matrix-vector subroutine to deliver y:=A*x by CALL matvec(n,x,y)
 * l_precond == (input) EXTERNAL name of subroutine to perform a left preconditioner by CALL l_precond(n,x)
 *              This call delivers solution to M*y=x, M is the left preconditioning matrix
 *              x is rhs on input, and solution y is written in x on output
 * pcnd      == (input) LOGICAL, indicates whether the preconditioner exist
 *              If pcnd == .true., there is a preconditioner otherwise pcnd is .false.
 * toler     == (input) DOUBLE PRECISION tolerance for the relative residual norm stopping criterion: ||r_k|| <= toler*||r_0||
 * mxmatvec  == (input/output) INTEGER.  On input: maximum number of matrix-vector multiplications allowed
 *                                      On output: actual number of matrix-vector multiplications achieved
 * work      == (workspace) DOUBLE PRECISION array dimension (n,3+2*(l+1))
 * rwork     == (workspace) DOUBLE PRECISION array dimension (l+1,3+2*(l+1))
 * iwork     == (workspace) INTEGER array dimension (l+1)
 * info      == (output) INTEGER.  Info = 0 in case of normal computations
 *              info = m < 0 - means paramater number m has an illegal value
 *              info = 1 - means no convergence achieved (stopping criterion is not fulfilled)
 *              info = 2 - means breakdown of the algorithm (try to enlarge parameter l to get rid of this)
 */


/******************************************************/
/* Original Fortran 77 routine name and parameters:   */
/******************************************************/
/*
 *      subroutine bistbl (l, n, x, b, mv, solve, tol, mxmv, work, ldw, rwork, ldrw, iwork, info)
 *
 * subroutine bistbl v1.0 1995 by D.R. Fokkema.
 *
 * Parameter list (by M.Botchev, October, 1997):
 *
 * l      == (input) INTEGER BiCGstab's dimension
 * n      == (input) INTEGER size of the system to solve 
 * x      == (input/output) DOUBLE PRECISION array dimension n
 *           initial guess on input, solution on output
 * b      == (input) DOUBLE PRECISION array dimension n
 *           right-hand side (rhs) vector
 * mv     == (input) EXTERNAL name of matrix vector subroutine
 *           to deliver y:=A*x by CALL matvec(n,x,y)
 * solve  == (input) EXTERNAL name of subroutine to perform
 *           a left preconditioner solve by CALL solve(n,x). This call 
 *           delivers solution to M*x=x, M is the left preconditioning 
 *           matrix, x is rhs on input and solution on output.
 * tol    == (input) DOUBLE PRECISION tolerance for the relative
 *           residual norm stopping criterion: ||r_k|| <= toler*||r_0||
 * mxmv   == (input/output) INTEGER.  On input: maximum number of matrix 
 *           vector multiplications allowed to be done.  On input: 
 *           actual number of matrix vector multiplications done
 * work   == (workspace) DOUBLE PRECISION array dimension (n,3+2*(l+1))
 * ldw    == (input) INTEGER leading dimension of work (??)
 * rwork  == (workspace) DOUBLE PRECISION array dimension (l+1,3+2*(l+1))
 * ldrw   == (input) INTEGER leading dimension of rwork (??)
 * iwork  == (workspace) INTEGER array dimension (l+1)
 * info   == (output) INTEGER.  Info = 0 in case of normal computations
 *           and 
 *           info = m < 0 - means paramater number m has an illegal value
 *           info = 1 - means no convergence achieved (stopping criterion
 *           is not fulfilled)
 *           info = 2 - means breakdown of the algorithm (try to enlarge
 *           parameter l to get rid of this)
 */


/* Local variables: */
char uplo_n,uplo_u,trns_t,trns_n;
bool rcmp, xpdt;
int i, j, k, nmatvec,wsz,inc,ii,lp1,lm1;
int *iwork; /* iwork[l+1] */
double rnrm0, rnrm;
double mxnrmx, mxnrmr;
double kappa0, kappal;
double *work, *rwork; /* work[n][3+2*(l+1)], rwork[l+1][3+2*(l+1)] */
double alpha, beta, omega, rho0, rho1, sigma, m_alp;
double varrho, hatgamma;

/* Aliases for the parts of the work array: */
int r, u, xp, bp, nr, nbp, nu, nxp, zz, y0, yl, y, lp1y0, lp1yl, lp1y;

/* Constants: */
double zero = 0.0, one = 1.0, delta = 1.0E-2, m_one = -1.0;



/*     ===========================     */
/*     .. Executable Statements ..     */
/*     ===========================     */

      *info = 0; lp1 = l+1;
      wsz = 3+2*lp1; 

      if (l<1) *info = -1;
      if (n<1) *info = -2;
      if (*toler<=zero) *info = -7;
      if (*mxmatvec<0) *info = -8;

      r = 1; u = 1+lp1; /* r = 1; u = r+(l+1); */
      xp = u+lp1;
      bp = xp+1;

      zz = lp1; y0 = lp1+lp1; /* zz = l+1; y0 = zz+(l+1); */
      yl = y0+1;
      y = yl+1;

      if (*info != 0) return;

      
/*     --- Initialize first residual */
    trns_t = 't'; trns_n = 'n'; lm1 = l-1; uplo_n = 'n'; uplo_u = 'u';
    nr = n*r; nbp = n*bp; nu = n*u; nxp = n*xp;
    lp1y0 = lp1*y0; lp1yl = lp1*yl; lp1y = lp1*y;
    
    if ( (work = (double *)malloc(n*wsz*sizeof(double))) == NULL ) /* If work is not successfully allocated */
    {printf("bicgstabl: Allocate array \"work\" failed\n"); exit(1);} /* work[n][wsz], wsz = 3+2*(l+1) */
    
    if ( (rwork = (double *)malloc(lp1*wsz*sizeof(double))) == NULL ) /* If rwork is not successfully allocated */
    {printf("bicgstabl: Allocate array \"rwork\" failed\n"); exit(1);} /* rwork[lp1][wsz], lp1=l+1, wsz = 3+2*(l+1) */
    
    if ( (iwork = (int *)malloc(lp1*sizeof(int))) == NULL ) /* If iwork is not successfully allocated */
    {printf("bicgstabl: Allocate array \"iwork\" failed\n"); exit(1);} /* iwork[l+1] */

    if (nonzero_x)
    {
      matvec(n, x, &work[nr]); /* matvec(n, x, work[0:n-1][r]) */
      for (i=0; i<n; i++) work[i+nr] = b[i] - work[i+nr]; /* work[0:n-1][r] = b - work[0:n-1][r] */
      nmatvec = 1;
    }
    else
    {
      for (i=0; i<n; i++) work[i+nr] = b[i]; /* work[0:n-1][r] = b */
      nmatvec = 0;
    }

    if (pcnd) l_precond(n, &work[nr]); /* l_precond(n, work[0:n-1][r]) */


/*     --- Initialize iteration loop */
      inc = 1;
      dcopy_(&n, &work[nr], &inc, work, &inc); /* work[0:n-1][0] = work[0:n-1][r] */
      dcopy_(&n, &work[nr], &inc, &work[nbp], &inc); /* work[0:n-1][bp] = work[0:n-1][r] */
      dcopy_(&n, x, &inc, &work[nxp],&inc); /* work[0:n-1][xp] = x */
      dlaset_(&uplo_n, &n, &inc, &zero, &zero, x, &inc); /* x = 0.0 */
      rnrm0 = dnrm2_(&n, &work[nr], &inc); /* rnrm0 = sqrt(dot_product(work[0:n-1][r],work[0:n-1][r])) */
      rnrm = rnrm0;

      mxnrmx = rnrm0; mxnrmr = rnrm0;
      rcmp = false; xpdt = false;
      alpha = zero; omega = one; sigma = one; rho0 = one;


/*     --- Iterate */
      while ( rnrm>*toler*rnrm0 && nmatvec<*mxmatvec)
      {

/*     ===================== */
/*     --- The BiCG part --- */
/*     ===================== */

	 rho0 = -omega*rho0;
	 for (k=1; k<=l; k++)
	 {
	    rho1 = ddot_(&n, work, &inc, &work[n*(r+k-1)], &inc); /* rho1 = dot_product(work[0:n-1][0],work[0:n-1][r+k-1]) */
	    if (rho0 == zero) { *info = 2; return; }
	    beta = alpha*(rho1/rho0);
	    rho0 = rho1;
	    for (j=0; j<=k-1; j++)
	       for (i=0; i<n; i++)
		  work[i+n*(u+j)] = work[i+n*(r+j)] - beta*work[i+n*(u+j)];
	    matvec(n, &work[n*(u+k-1)], &work[n*(u+k)]); /* matvec(n,work[0:n-1][u+k-1],work[0:n-1][u+k]) */
	    if (pcnd) l_precond(n, &work[n*(u+k)]);
	    nmatvec++;

	    sigma = ddot_(&n, work, &inc, &work[n*(u+k)], &inc); /* sigma = dot_product(work[0:n-1][0],work[0:n-1][u+k]) */
	    if (sigma == zero) { *info = 2; return; }
	    alpha = rho1/sigma;
	    daxpy_(&n, &alpha, &work[nu], &inc, x, &inc); /* x = alpha*work[0:n-1][u] + x */
	    m_alp = -alpha;
	    for (j=0; j<=k-1; j++) 
	       daxpy_(&n,&m_alp,&work[n*(u+j+1)],&inc,&work[n*(r+j)],&inc); /* work[0:n-1][r+j] = -alpha*work[0:n-1][u+j+1] + work[0:n-1][r+j] */
	    matvec(n, &work[n*(r+k-1)], &work[n*(r+k)]); /* matvec(n,work[0:n-1][r+k-1],work[0:n-1][r+k]) */
	    if (pcnd) l_precond(n, &work[n*(r+k)]);
	    nmatvec++;

	    rnrm = dnrm2_(&n, &work[nr], &inc); /* rnrm = sqrt(dot_product(work[0:n-1][r],work[0:n-1][r])) */
	    mxnrmx = fmax(mxnrmx, rnrm); mxnrmr = fmax(mxnrmr, rnrm);
	 }


/*     ================================== */
/*     --- The convex polynomial part --- */
/*     ================================== */

/*        --- Z = R'R */
	 for (i=0; i<=l; i++)
	 {
	    ii = lp1-i; k = l-i;
	    dgemv_(&trns_t,&n,&ii,&one,&work[n*(r+i)],&n,&work[n*(r+i)],&inc,&zero,&rwork[i+lp1*i],&inc); /* The lower triangular part of Z is computed */
	    /* rwork[i:l][i] = matmul(transpose(work[0:n-1][r+i:r+l]),work[0:n-1][r+i]) */
	    
	    dcopy_(&k,&rwork[i+1+lp1*i],&inc,&rwork[i+lp1*(i+1)],&lp1); /* Compute the strictly upper triangular part of Z knowing that Z^T=Z */
	    /* rwork[i][i+1:l+1] = rwork[i+1:l+1][i] */
	 }

	 dlacpy_(&uplo_n,&lp1,&lp1,rwork,&lp1,&rwork[lp1*zz],&lp1); /* rwork[0:l][zz:zz+l] = rwork[0:l][0:l] <==> duplicate */
	 dgetrf_(&lm1,&lm1,&rwork[1+lp1*(zz+1)],&lp1,iwork,info); /* LU factorization */


/*        --- tilde r0 and tilde rl (small vectors) */
	 rwork[lp1y0] = -one; /* rwork[0][y0] = -one */
	 dcopy_(&lm1,&rwork[1],&inc,&rwork[1+lp1y0],&inc); /* rwork[1:l-1][y0] = rwork[1:l-1][0]*/
	 dgetrs_(&trns_n,&lm1,&inc,&rwork[1+lp1*(zz+1)],&lp1,iwork,&rwork[1+lp1y0],&lp1,info);
	 rwork[l+lp1y0] = zero; /* rwork[l][y0] = zero */
	 
	 rwork[lp1yl] = zero; /* rwork[0][yl] = zero */
	 dcopy_(&lm1,&rwork[1+lp1*l],&inc,&rwork[1+lp1yl],&inc); /* rwork[1:l-1][yl] = rwork[1:l-1][l] */
	 dgetrs_(&trns_n,&lm1,&inc,&rwork[1+lp1*(zz+1)],&lp1,iwork,&rwork[1+lp1yl],&lp1,info);
	 rwork[l+lp1yl] = -one; /* rwork[l][yl] = -one */


/*        --- Convex combination */
	 dsymv_(&uplo_u,&lp1,&one,rwork,&lp1,&rwork[lp1y0],&inc,&zero,&rwork[lp1y],&inc); /* rwork[0:l][y] = matmul(rwork[0:l][0:l],rwork[0:l][y0]) */
	 kappa0 = sqrt(ddot_(&lp1,&rwork[lp1y0],&inc,&rwork[lp1y],&inc)); /* kappa0 = sqrt(dot_product(rwork[0:l][y0],rwork[0:l][y])) */

	 dsymv_(&uplo_u,&lp1,&one,rwork,&lp1,&rwork[lp1yl],&inc,&zero,&rwork[lp1y],&inc); /* rwork[0:l][y] = matmul(rwork[0:l][0:l],rwork[0:l][yl]) */
	 kappal = sqrt(ddot_(&lp1,&rwork[lp1yl],&inc,&rwork[lp1y],&inc)); /* kappal = sqrt(dot_product(rwork[0:l][yl],rwork[0:l][y])) */

	 dsymv_(&uplo_u,&lp1,&one,rwork,&lp1,&rwork[lp1y0],&inc,&zero,&rwork[lp1y],&inc); /* rwork[0:l][y] = matmul(rwork[0:l][0:l],rwork[0:l][y0]) */
	 varrho = ddot_(&lp1,&rwork[lp1yl],&inc,&rwork[lp1y],&inc)/(kappa0*kappal);

	 hatgamma = copysign(1.0,varrho)*fmax(fabs(varrho),0.7)*(kappa0/kappal);
	 m_alp = -hatgamma;
	 daxpy_(&lp1,&m_alp,&rwork[lp1yl],&inc,&rwork[lp1y0],&inc); /* rwork[0:l][y0] = -hatgamma*rwork[0:l][yl] + rwork[0:l][y0] */


/*        --- Update */
	 omega = rwork[l+lp1y0]; /* omega = rwork[l][y0] */

	 dgemv_(&trns_n,&n,&l,&m_one,&work[nu+n],&n,&rwork[1+lp1y0],&inc,&one,&work[nu],&inc); 
	 /* work[0:n-1][u] = work[0:n-1][u] - matmul(work[0:n-1][u+1:u+l],rwork[1:l][y0]) */

	 dgemv_(&trns_n,&n,&l,&one,&work[nr],&n,&rwork[1+lp1y0],&inc,&one,x,&inc);
	 /* x = x + matmul(work[0:n-1][r:r+l-1],rwork[1:l][y0]) */
	 
	 dgemv_(&trns_n,&n,&l,&m_one,&work[nr+n],&n,&rwork[1+lp1y0],&inc,&one,&work[nr],&inc);
	 /* work[0:n-1][r] = work[0:n-1][r] - matmul(work[0:n-1][r+1:r+l],rwork[1:l][y0]) */

/* y0 has changed; compute y=Z*y0 once more */
	 dsymv_(&uplo_u,&lp1,&one,rwork,&lp1,&rwork[lp1y0],&inc,&zero,&rwork[lp1y],&inc); /* rwork[0:l][y] = matmul(rwork[0:l][0:l],rwork[0:l][y0]) */
	 rnrm = sqrt(ddot_(&lp1,&rwork[lp1y0],&inc,&rwork[lp1y],&inc)); /* rnrm = sqrt(dot_product(rwork[0:l][y0],rwork[0:l][y])) */


/*     ================================ */
/*     --- The reliable update part --- */
/*     ================================ */

	 mxnrmx = fmax(mxnrmx, rnrm); mxnrmr = fmax(mxnrmr, rnrm);
	 xpdt = (rnrm<delta*rnrm0 && rnrm0<mxnrmx);
	 rcmp = ((rnrm<delta*mxnrmr && rnrm0<mxnrmr) || xpdt);

	 if (rcmp)
	 {
	    matvec(n, x, &work[nr]); /* matvec(n, x, work[0:n-1][r]) */
	    if (pcnd) l_precond(n, &work[nr]);  /* l_precond(n, work[0:n-1][r]) */
            nmatvec++;
	    
	    for (i=0; i<n; i++) work[i+nr] =  work[i+nbp] - work[i+nr]; /* work[0:n-1][r] = work[0:n-1][bp] - work[0:n-1][r] */
	    mxnrmr = rnrm;
	    if (xpdt)
	    {
	       daxpy_(&n, &one, x, &inc, &work[nxp], &inc); /* work[0:n-1][xp] = x + work[0:n-1][xp] */
	       dlaset_(&trns_n, &n, &inc, &zero, &zero, x, &inc); /* x = 0.0 */
	       dcopy_(&n, &work[nr], &inc, &work[nbp], &inc); /* work[0:n-1][bp] = work[0:n-1][r] */
	       mxnrmx = rnrm;
	    }
	 }

      }

      free(rwork); free(iwork);
/*     ========================= */
/*     --- End of iterations --- */
/*     ========================= */

      daxpy_(&n, &one, &work[nxp], &inc, x, &inc); /* x = work[0:n-1][xp] + x */
      if (rnrm>*toler*rnrm0) *info = 1;


/* compute the true residual: */
/* --------------------- One matvec can be saved by commenting out this: */
/*     --- Check stopping criterion                                      */
      matvec(n, x, &work[nr]); /* matvec (n, x, work[0:n-1][r]) */
      for (i=0; i<n; i++) work[i+nr] = b[i] - work[i+nr]; /* work[0:n-1][r] = b - work[0:n-1][r] */

      if (pcnd) l_precond (n, &work[nr]); /* call l_precond (n, work[0:n-1][r]) */
      rnrm = dnrm2_(&n, &work[nr], &inc); /* rnrm = sqrt(dot_product(work[0:n-1][r],work[0:n-1][r])) */
      nmatvec++;
/* --------------------- One matvec can be saved by commenting out this: */
      *toler = rnrm/rnrm0; *mxmatvec = nmatvec;

      free(work);

}
