#include <cmath>
#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

VectorXd bicg(MatrixXd &A, VectorXd &b, VectorXd x, int max_it, double tol) {

    double bnrm2 = b.norm();

    if (bnrm2 == 0.0) bnrm2 = 1.0;

    VectorXd r = b - A * x;

    double error = r.norm() / bnrm2;

    VectorXd r_tld = r;

    double rho = 0, rho_1 = 0;

    double alpha = 0, beta = 0;

    VectorXd z(b.size()), z_tld(b.size());

    VectorXd p(b.size()), p_tld(b.size());

    for (int i = 0; i < max_it; i++) {
        z = r;
        z_tld = r_tld;

        rho = z.dot(r_tld);
        if (rho == 0.0) break;

        if (i > 0) {
            beta = rho / rho_1;
            p = z + beta * p;
            p_tld = z_tld + beta * p_tld;
        } else {
            p = z;
            p_tld = z_tld;
        }

        alpha = rho / (p_tld.dot(A * p));

        x = x + alpha * p;
        r = r - alpha * (A * p);

        error = r.norm() / bnrm2;
        if (error <= tol) break;

        r_tld = r_tld - alpha * (A.transpose() * p_tld);

        rho_1 = rho;
    }

    return x;
}

int main() {
    int N = 5;

    MatrixXd A = MatrixXd::Random(N, N);

    VectorXd b(N);

    for (int i = 0; i < N; i++) {
        b(i) = 1.5 * i;
    }


    VectorXd sol = A.inverse() * b;

    cout << "A =" << endl << A << endl;
    cout << "b =" << endl << b << endl;
    cout << "sol =" << endl << sol << endl;

    VectorXd x0(N);

    for (int i = 0; i < N; i++) {
        x0(i) = 0.0;
    }

    int max_it = 10;
    double tol = 1e-12;

    VectorXd sol1 = bicg(A, b, x0, max_it, tol);

    cout << "sol1 =" << endl << sol1 << endl;

    return 0;

}