/********************************************************
 *  ██████╗  ██████╗████████╗██╗
 * ██╔════╝ ██╔════╝╚══██╔══╝██║
 * ██║  ███╗██║        ██║   ██║
 * ██║   ██║██║        ██║   ██║
 * ╚██████╔╝╚██████╗   ██║   ███████╗
 *  ╚═════╝  ╚═════╝   ╚═╝   ╚══════╝
 * Geophysical Computational Tools & Library (GCTL)
 *
 * Copyright (c) 2022  Yi Zhang (yizhang-geo@zju.edu.cn)
 *
 * GCTL is distributed under a dual licensing scheme. You can redistribute 
 * it and/or modify it under the terms of the GNU Lesser General Public 
 * License as published by the Free Software Foundation, either version 2 
 * of the License, or (at your option) any later version. You should have 
 * received a copy of the GNU Lesser General Public License along with this 
 * program. If not, see <http://www.gnu.org/licenses/>.
 * 
 * If the terms and conditions of the LGPL v.2. would prevent you from using 
 * the GCTL, please consider the option to obtain a commercial license for a 
 * fee. These licenses are offered by the GCTL's original author. As a rule, 
 * licenses are provided "as-is", unlimited in time for a one time fee. Please 
 * send corresponding requests to: yizhang-geo@zju.edu.cn. Please do not forget 
 * to include some description of your company and the realm of its activities. 
 * Also add information on how to contact you by electronic and paper mail.
 ******************************************************/

#include "lcg.h"
#include "typeinfo"

/**
 * Default parameter for conjugate gradient methods
 */
static const gctl::lcg_para lcg_defparam = {0, 1e-8, 0, 1e-6, 1.0, 0.95, 0.9, 10};

gctl::lcg_solver::lcg_solver()
{
    lcg_param_ = lcg_defparam;
    lcg_inter_ = 1;
    lcg_msg_ = LCG_ITERATION;
}

gctl::lcg_solver::~lcg_solver(){}

void gctl::lcg_solver::LCG_Mx(const array<double> &x, array<double> &mx)
{
	throw std::runtime_error("The preconditioning function 'void gctl::lcg_solver::LCG_Mx(const array<double> &x, array<double> &mx)' must be defined defore using.");
	return;
}

int gctl::lcg_solver::LCG_Progress(const array<double> &m, const double converge, 
    const lcg_para &param, size_t t, std::ostream &ss)
{
    std::string ss_str = typeid(ss).name();
    if (ss_str.find("ofstream") != std::string::npos)
    {
        if (lcg_msg_ > LCG_ERROR && converge <= param.epsilon) // lcg_msg_ == LCG_SOLUTION
        {
            ss << "Iterations: " << std::setw(6) << t << ", Convergence: " << converge;
        }
    }
    else if (lcg_msg_ > LCG_ERROR)
    {
        if (converge <= param.epsilon) // lcg_msg_ == LCG_SOLUTION
        {
            ss << GCTL_CLEARLINE << "\rIterations: " << std::setw(6) << t << ", Convergence: " << converge << std::endl;
            return 0;
        }

        if (lcg_msg_ == LCG_ITERATION && lcg_inter_ > 0 && t%lcg_inter_ == 0)
        {
            ss << GCTL_CLEARLINE << "\rIterations: " << std::setw(6) << t << ", Convergence: " << converge;
        }
    }
    return 0;
}

void gctl::lcg_solver::set_lcg_message(lcg_message_type msg)
{
    lcg_msg_ = msg;
    return;
}

void gctl::lcg_solver::set_lcg_report_interval(size_t inter)
{
    lcg_inter_ = inter;
    return;
}

void gctl::lcg_solver::set_lcg_para(const lcg_para &in_param)
{
    lcg_param_ = in_param;
    return;
}

gctl::lcg_para gctl::lcg_solver::default_lcg_para()
{
    lcg_para dp = lcg_defparam;
    return dp;
}

#ifdef GCTL_OPTIMIZATION_TOML

void gctl::lcg_solver::set_lcg_para(std::string filename)
{
    toml::value toml_data;
    toml_data = toml::parse(filename);

    set_lcg_para(toml_data);
    return;
}

void gctl::lcg_solver::set_lcg_para(const toml::value &toml_data)
{
    lcg_param_ = lcg_defparam;

    std::string LCG = "lcg";
	if (toml_data.contains(LCG))
	{
		if (toml_data.at(LCG).contains("max_iterations"))   lcg_param_.max_iterations = toml::find<int>(toml_data, LCG, "max_iterations");
        if (toml_data.at(LCG).contains("epsilon"))          lcg_param_.epsilon = toml::find<double>(toml_data, LCG, "epsilon");
        if (toml_data.at(LCG).contains("abs_diff"))         lcg_param_.abs_diff = toml::find<int>(toml_data, LCG, "abs_diff");
		if (toml_data.at(LCG).contains("restart_epsilon"))  lcg_param_.restart_epsilon = toml::find<double>(toml_data, LCG, "restart_epsilon");
		if (toml_data.at(LCG).contains("step"))             lcg_param_.step = toml::find<double>(toml_data, LCG, "step");
		if (toml_data.at(LCG).contains("sigma"))            lcg_param_.sigma = toml::find<double>(toml_data, LCG, "sigma");
        if (toml_data.at(LCG).contains("beta"))             lcg_param_.beta = toml::find<double>(toml_data, LCG, "beta");
        if (toml_data.at(LCG).contains("maxi_m"))           lcg_param_.maxi_m = toml::find<int>(toml_data, LCG, "maxi_m");
	}
    return;
}

#endif // GCTL_OPTIMIZATION_TOML

void gctl::lcg_solver::lcg_error_str(lcg_return_code err_code, std::ostream &ss)
{
    std::string ss_str = typeid(ss).name();

#if defined _WINDOWS || __WIN32__
    if (lcg_msg_ > LCG_ERROR)
    {
        if (ss_str.find("ofstream") != std::string::npos)
        {
            if (err_code >= 0)
                ss << "LCG Success! ";
            else
                ss << "LCG Fail! ";
        }
        else
        {
            if (err_code >= 0)
            {
                SetConsoleTextAttribute(GetStdHandle(STD_ERROR_HANDLE), FOREGROUND_INTENSITY | FOREGROUND_GREEN);
                ss << "LCG Success! ";
            }
            else
            {
                SetConsoleTextAttribute(GetStdHandle(STD_ERROR_HANDLE), FOREGROUND_INTENSITY | FOREGROUND_RED);
                ss << "LCG Fail! ";
            }
        }
    }
#else
    if (lcg_msg_ > LCG_ERROR)
    {
        if (ss_str.find("ofstream") != std::string::npos)
        {
            if (err_code >= 0)
                ss << "LCG Success! ";
            else
                ss << "LCG Fail! ";
        }
        else
        {
            if (err_code >= 0)
                ss << "\033[1m\033[32mLCG Success! ";
            else
                ss << "\033[1m\033[31mLCG Fail! ";
        }
    }
#endif

    std::string err_str;
    switch (err_code)
    {
        case LCG_SUCCESS:
            err_str = "Iteration reached convergence."; break;
        case LCG_STOP:
            err_str = "Iteration is stopped by the progress evaluation function."; break;
        case LCG_ALREADY_OPTIMIZIED:
            err_str = "The variables are already optimized."; break;
        case LCG_UNKNOWN_ERROR:
            err_str = "Unknown error."; break;
        case LCG_INVILAD_VARIABLE_SIZE:
            err_str = "The size of the variables is negative."; break;
        case LCG_INVILAD_MAX_ITERATIONS:
            err_str = "The maximal iteration times can't be negative."; break;
        case LCG_INVILAD_EPSILON:
            err_str = "The epsilon is not in the range (0, 1)."; break;
        case LCG_INVILAD_RESTART_EPSILON:
            err_str = "The restart threshold can't be negative."; break;
        case LCG_REACHED_MAX_ITERATIONS:
            err_str = "The maximal iteration has been reached."; break;
        case LCG_NULL_PRECONDITION_MATRIX:
            err_str = "The precondition matrix can't be null."; break;
        case LCG_NAN_VALUE:
            err_str = "The model values are NaN."; break;
        case LCG_INVALID_POINTER:
            err_str = "Invalid pointer."; break;
        case LCG_INVALID_LAMBDA:
            err_str = "Invalid value for lambda."; break;
        case LCG_INVALID_SIGMA:
            err_str = "Invalid value for sigma."; break;
        case LCG_INVALID_BETA:
            err_str = "Invalid value for beta."; break;
        case LCG_INVALID_MAXIM:
            err_str = "Invalid value for maxi_m."; break;
        case LCG_SIZE_NOT_MATCH:
            err_str = "The sizes of solution and target do not match."; break;
        default:
            err_str = "Unknown error."; break;
    }

    if (lcg_msg_ == LCG_THROW && err_code < 0) throw std::runtime_error(err_str.c_str());
    else if (lcg_msg_ == LCG_ERROR && err_code < 0) ss << err_str;
    else if (lcg_msg_ > LCG_ERROR) ss << err_str;

#if defined _WINDOWS || __WIN32__
    if (lcg_msg_ > LCG_ERROR)
    {
        if (ss_str.find("ofstream") != std::string::npos)
        {
            ss << " ";
        }
        else
        {
            SetConsoleTextAttribute(GetStdHandle(STD_ERROR_HANDLE), 7);
            ss << std::endl;
        }
    }
#else
    if (lcg_msg_ > LCG_ERROR)
    {
        if (ss_str.find("ofstream") != std::string::npos) ss << " ";
        else ss << "\033[0m" << std::endl;
    }
#endif

    return;
}

void gctl::lcg_solver::LCG_Minimize(array<double> &m, const array<double> &b, lcg_solver_type solver_id, std::ostream &ss)
{
    // 使用lcg求解 注意当我们使用函数指针来调用求解函数时默认参数不可以省略
#ifdef GCTL_OPENMP
    double start = omp_get_wtime();
    if (solver_id == LCG_CG) lcg(m ,b, ss);
    else if (solver_id == LCG_CGS) lcgs(m, b, ss);
    else if (solver_id == LCG_PCG) lpcg(m, b, ss);
    else if (solver_id == LCG_BICGSTAB) lbicgstab(m, b, ss);
    else if (solver_id == LCG_BICGSTAB2) lbicgstab2(m, b, ss);
    else throw std::invalid_argument("Invalid solver type.");
    double end = omp_get_wtime();

    double costime = 1000*(end-start);
#else
    clock_t start = clock();
    if (solver_id == LCG_CG) lcg(m ,b, ss);
    else if (solver_id == LCG_CGS) lcgs(m, b, ss);
    else if (solver_id == LCG_PCG) lpcg(m, b, ss);
    else if (solver_id == LCG_BICGSTAB) lbicgstab(m, b, ss);
    else if (solver_id == LCG_BICGSTAB2) lbicgstab2(m, b, ss);
    else throw std::invalid_argument("Invalid solver type.");
    clock_t end = clock();

    double costime = 1000*(end-start)/(double)CLOCKS_PER_SEC;
#endif

    if (lcg_msg_ > LCG_ERROR)
    {
        switch (solver_id)
        {
            case LCG_CG:
                ss << "Solver: " << std::setw(9) << "CG, Time cost: " << costime << " ms\n"; break;
            case LCG_CGS:
                ss << "Solver: " << std::setw(9) << "CGS, Time cost: " << costime << " ms\n"; break;
            case LCG_PCG:
                ss << "Solver: " << std::setw(9) << "PCG, Time cost: " << costime << " ms\n"; break;
            case LCG_BICGSTAB:
                ss << "Solver: " << std::setw(9) << "BICGSTAB, Times cost: " << costime << " ms\n"; break;
            case LCG_BICGSTAB2:
                ss << "Solver: " << std::setw(9) << "BICGSTAB2, Time cost: " << costime << " ms\n"; break;
            default:
                ss << "Solver: " << std::setw(9) << "Unknown, Time cost: " << costime << " ms\n"; break;
        }	
    }
    return;
}

void gctl::lcg_solver::LCG_MinimizeConstrained(array<double> &m, const array<double> &b, const array<double> &low, const array<double> &hig, lcg_solver_type solver_id, std::ostream &ss)
{
    // 使用lcg求解 注意当我们使用函数指针来调用求解函数时默认参数不可以省略
#ifdef GCTL_OPENMP
    double start = omp_get_wtime();
    if (solver_id == LCG_PG) lpg(m ,b, low, hig, ss);
    else if (solver_id == LCG_SPG) lspg(m, b, low, hig, ss);
    else throw std::invalid_argument("Invalid solver type.");
    double end = omp_get_wtime();

    double costime = 1000*(end-start);
#else
    clock_t start = clock();
    if (solver_id == LCG_PG) lpg(m ,b, low, hig, ss);
    else if (solver_id == LCG_SPG) lspg(m, b, low, hig, ss);
    else throw std::invalid_argument("Invalid solver type.");
    clock_t end = clock();

    double costime = 1000*(end-start)/(double)CLOCKS_PER_SEC;
#endif

    if (lcg_msg_ > LCG_ERROR)
    {
        switch (solver_id)
        {
            case LCG_PG:
                ss << "Solver: " << std::setw(9) << "PG" << ", Time cost: " << costime << " ms\n"; break;
            case LCG_SPG:
                ss << "Solver: " << std::setw(9) << "SPG" << ", Time cost: " << costime << " ms\n"; break;
            default:
                ss << "Solver: " << std::setw(9) << "Unknown" << ", Time cost: " << costime << " ms\n"; break;
        }
    }
    return;
}

void gctl::lcg_solver::lcg(array<double> &m, const array<double> &B, std::ostream &ss)
{
    size_t n_size = B.size();
    //check parameters
    if (n_size <= 0) return lcg_error_str(LCG_INVILAD_VARIABLE_SIZE, ss);
    if (n_size != m.size()) return lcg_error_str(LCG_SIZE_NOT_MATCH, ss);
    if (lcg_param_.max_iterations < 0) return lcg_error_str(LCG_INVILAD_MAX_ITERATIONS, ss);
    if (lcg_param_.epsilon <= 0.0 || lcg_param_.epsilon >= 1.0) return lcg_error_str(LCG_INVILAD_EPSILON, ss);

    // locate memory
    gk.resize(n_size);
    dk.resize(n_size);
    Adk.resize(n_size);

    LCG_Ax(m, Adk);

    vecdiff(gk, Adk, B);
    veccpy(dk, gk, -1.0);

    double gk_mod = vecdot(gk, gk);
    double g0_mod = gk_mod;
    if (g0_mod < 1.0) g0_mod = 1.0;

    size_t t = 0;
    double dTAd, ak, betak, gk1_mod, residual;
    while (1)
    {
        if (lcg_param_.abs_diff) residual = sqrt(gk_mod)/n_size;
        else residual = gk_mod/g0_mod;

        if (LCG_Progress(m, residual, lcg_param_, t, ss))
        {
            return lcg_error_str(LCG_STOP, ss);
        }

        if (residual <= lcg_param_.epsilon)
        {
            if (t == 0) return lcg_error_str(LCG_ALREADY_OPTIMIZIED, ss);
            else return lcg_error_str(LCG_CONVERGENCE, ss);
        }

        if (lcg_param_.max_iterations > 0 && t+1 > lcg_param_.max_iterations)
        {
            return lcg_error_str(LCG_REACHED_MAX_ITERATIONS, ss);
        }

        t++;

        LCG_Ax(dk, Adk);

        dTAd = vecdot(dk, Adk);
        ak = gk_mod/dTAd;

        vecapp(m, dk, ak);
        vecapp(gk, Adk, ak);

        if (!vecvalid(m))
        {
            return lcg_error_str(LCG_NAN_VALUE, ss);
        }

        gk1_mod = vecdot(gk, gk);
        betak = gk1_mod/gk_mod;
        gk_mod = gk1_mod;

        vecdiff(dk, dk, gk, betak);
    }

    return lcg_error_str(LCG_UNKNOWN_ERROR, ss);
}

void gctl::lcg_solver::lpcg(array<double> &m, const array<double> &B, std::ostream &ss)
{
    size_t n_size = B.size();
    //check parameters
    if (n_size <= 0) return lcg_error_str(LCG_INVILAD_VARIABLE_SIZE, ss);
    if (n_size != m.size()) return lcg_error_str(LCG_SIZE_NOT_MATCH, ss);
    if (lcg_param_.max_iterations < 0) return lcg_error_str(LCG_INVILAD_MAX_ITERATIONS, ss);
    if (lcg_param_.epsilon <= 0.0 || lcg_param_.epsilon >= 1.0) return lcg_error_str(LCG_INVILAD_EPSILON, ss);

    // locate memory
    rk.resize(n_size); zk.resize(n_size);
    dk.resize(n_size); Adk.resize(n_size);

    LCG_Ax(m, Adk);

    vecdiff(rk, B, Adk);

    LCG_Mx(rk, zk);

    veccpy(dk, zk);

    double rk_mod = vecdot(rk, rk);
    double r0_mod = rk_mod;
    if (r0_mod < 1.0) r0_mod = 1.0;

    double zTr = vecdot(zk, rk);

    size_t t = 0;
    double dTAd, ak, betak, zTr1, residual;
    while (1)
    {
        if (lcg_param_.abs_diff) residual = sqrt(rk_mod)/n_size;
        else residual = rk_mod/r0_mod;

        if (LCG_Progress(m, residual, lcg_param_, t, ss))
        {
            return lcg_error_str(LCG_STOP, ss);
        }

        if (residual <= lcg_param_.epsilon)
        {
            if (t == 0) return lcg_error_str(LCG_ALREADY_OPTIMIZIED, ss);
            return lcg_error_str(LCG_CONVERGENCE, ss);
        }

        if (lcg_param_.max_iterations > 0 && t+1 > lcg_param_.max_iterations)
        {
            return lcg_error_str(LCG_REACHED_MAX_ITERATIONS, ss);
        }

        t++;

        LCG_Ax(dk, Adk);

        dTAd = vecdot(dk, Adk);
        ak = zTr/dTAd;

        vecapp(m, dk, ak);
        vecapp(rk, Adk, -1.0*ak);

        LCG_Mx(rk, zk);

        rk_mod = vecdot(rk, rk);

        if (!vecvalid(m))
        {
            return lcg_error_str(LCG_NAN_VALUE, ss);
        }

        zTr1 = vecdot(zk, rk);
        betak = zTr1/zTr;
        zTr = zTr1;

        vecadd(dk, zk, dk, 1.0, betak);
    }

    return lcg_error_str(LCG_UNKNOWN_ERROR, ss);
}

void gctl::lcg_solver::lcgs(array<double> &m, const array<double> &B, std::ostream &ss)
{
    size_t n_size = B.size();
    //check parameters
    if (n_size <= 0) return lcg_error_str(LCG_INVILAD_VARIABLE_SIZE, ss);
    if (n_size != m.size()) return lcg_error_str(LCG_SIZE_NOT_MATCH, ss);
    if (lcg_param_.max_iterations < 0) return lcg_error_str(LCG_INVILAD_MAX_ITERATIONS, ss);
    if (lcg_param_.epsilon <= 0.0 || lcg_param_.epsilon >= 1.0) return lcg_error_str(LCG_INVILAD_EPSILON, ss);

    rk.resize(n_size);
    r0_T.resize(n_size);
    pk.resize(n_size);
    vk.resize(n_size);
    Apx.resize(n_size);
    uk.resize(n_size);
    qk.resize(n_size);
    wk.resize(n_size);

    LCG_Ax(m, Apx);

    // 假设p0和q0为零向量 则在第一次迭代是pk和uk都等于rk
    // 所以我们能直接从计算Apx开始迭代
    vecdiff(rk, B, Apx);
    veccpy(r0_T, rk);
    veccpy(uk, rk);
    veccpy(pk, rk);

    double rkr0_T = vecdot(rk, r0_T);
    double rk_mod = vecdot(rk, rk);
    double r0_mod = rk_mod;
    if (r0_mod < 1.0) r0_mod = 1.0;

    size_t t = 0;
    double ak, rk_abs, rkr0_T1, Apr_T, betak, residual;
    while (1)
    {
        if (lcg_param_.abs_diff) residual = sqrt(rk_mod)/n_size;
        else residual = rk_mod/r0_mod;

        if (LCG_Progress(m, residual, lcg_param_, t, ss))
        {
            return lcg_error_str(LCG_STOP, ss);
        }

        if (residual <= lcg_param_.epsilon)
        {
            if (t == 0) return lcg_error_str(LCG_ALREADY_OPTIMIZIED, ss);
            return lcg_error_str(LCG_CONVERGENCE, ss);
        }

        if (lcg_param_.max_iterations > 0 && t+1 > lcg_param_.max_iterations)
        {
            return lcg_error_str(LCG_REACHED_MAX_ITERATIONS, ss);
        }
        
        t++;

        LCG_Ax(pk, Apx);

        Apr_T = vecdot(Apx, r0_T);
        ak = rkr0_T/Apr_T;

        vecdiff(qk, uk, Apx, 1.0, ak);
        vecadd(wk, uk, qk);

        LCG_Ax(wk, Apx);

        vecapp(m, wk, ak);
        vecapp(rk, Apx, -1.0*ak);

        rk_mod = vecdot(rk, rk);

        if (!vecvalid(m))
        {
            return lcg_error_str(LCG_NAN_VALUE, ss);
        }

        rkr0_T1 = vecdot(rk, r0_T);
        betak = rkr0_T1/rkr0_T;
        rkr0_T = rkr0_T1;
        
        vecadd(uk, rk, qk, 1.0, betak);
        vecadd(vk, qk, pk, 1.0, betak);
        vecadd(pk, uk, vk, 1.0, betak);
    }

    return lcg_error_str(LCG_UNKNOWN_ERROR, ss);
}

void gctl::lcg_solver::lbicgstab(array<double> &m, const array<double> &B, std::ostream &ss)
{
    size_t n_size = B.size();
	//check parameters
	if (n_size <= 0) return lcg_error_str(LCG_INVILAD_VARIABLE_SIZE, ss);
    if (n_size != m.size()) return lcg_error_str(LCG_SIZE_NOT_MATCH, ss);
	if (lcg_param_.max_iterations < 0) return lcg_error_str(LCG_INVILAD_MAX_ITERATIONS, ss);
	if (lcg_param_.epsilon <= 0.0 || lcg_param_.epsilon >= 1.0) return lcg_error_str(LCG_INVILAD_EPSILON, ss);

	rk.resize(n_size); r0_T.resize(n_size);
	pk.resize(n_size); Adk.resize(n_size);
	sk.resize(n_size); Apx.resize(n_size);
    yk.resize(n_size);

	LCG_Ax(m, Adk);

    vecdiff(rk, B, Adk);
    veccpy(pk, rk);
    veccpy(r0_T, rk);

	double rkr0_T = vecdot(rk, r0_T);
	double rk_mod = vecdot(rk, rk);
	double r0_mod = rk_mod;
	if (r0_mod < 1.0) r0_mod = 1.0;

    size_t t = 0;
	double ak, wk, rkr0_T1, Apr_T, betak, Ass, AsAs, residual;
	while(1)
	{
		if (lcg_param_.abs_diff) residual = sqrt(rk_mod)/n_size;
		else residual = rk_mod/r0_mod;

		if (LCG_Progress(m, residual, lcg_param_, t, ss))
        {
            return lcg_error_str(LCG_STOP, ss);
        }

		if (residual <= lcg_param_.epsilon)
		{
            if (t == 0) return lcg_error_str(LCG_ALREADY_OPTIMIZIED, ss);
			else return lcg_error_str(LCG_CONVERGENCE, ss);
		}

		if (lcg_param_.max_iterations > 0 && t+1 > lcg_param_.max_iterations)
		{
			return lcg_error_str(LCG_REACHED_MAX_ITERATIONS, ss);
		}
		
		t++;

		LCG_Ax(pk, Apx);

        Apr_T = vecdot(Apx, r0_T);
		ak = rkr0_T/Apr_T;

        vecdiff(sk, rk, Apx, 1.0, ak);

		LCG_Ax(sk, Adk);

        Ass = vecdot(Adk, sk);
        AsAs = vecdot(Adk, Adk);
		wk = Ass/AsAs;

        vecapp(m, pk, ak);
        vecapp(m, sk, wk);

		if (!vecvalid(m))
        {
            return lcg_error_str(LCG_NAN_VALUE, ss);
        }

        vecdiff(rk, sk, Adk, 1.0, wk);

        rk_mod = vecdot(rk, rk);
		rkr0_T1 = vecdot(rk, r0_T);
		betak = (ak/wk)*rkr0_T1/rkr0_T;
		rkr0_T = rkr0_T1;

        vecdiff(yk, pk, Apx, 1.0, wk);
        vecadd(pk, rk, yk, 1.0, betak);
	}

	return lcg_error_str(LCG_UNKNOWN_ERROR, ss);
}

void gctl::lcg_solver::lbicgstab2(array<double> &m, const array<double> &B, std::ostream &ss)
{
    size_t n_size = B.size();
	//check parameters
	if (n_size <= 0) return lcg_error_str(LCG_INVILAD_VARIABLE_SIZE, ss);
	if (lcg_param_.max_iterations < 0) return lcg_error_str(LCG_INVILAD_MAX_ITERATIONS, ss);
	if (lcg_param_.epsilon <= 0.0 || lcg_param_.epsilon >= 1.0) return lcg_error_str(LCG_INVILAD_EPSILON, ss);
	if (lcg_param_.restart_epsilon <= 0.0) return lcg_error_str(LCG_INVILAD_RESTART_EPSILON, ss);

	rk.resize(n_size); r0_T.resize(n_size);
	pk.resize(n_size); Adk.resize(n_size);
	sk.resize(n_size); Apx.resize(n_size);
    yk.resize(n_size);

	LCG_Ax(m, Adk);

    vecdiff(rk, B, Adk);
    veccpy(pk, rk);
    veccpy(r0_T, rk);

    double rkr0_T = vecdot(rk, r0_T);
    double rk_mod = vecdot(rk, rk);
	double r0_mod = rk_mod;
	if (r0_mod < 1.0) r0_mod = 1.0;

    size_t t = 0;
	double ak, wk, rk_abs, rkr0_T1, Apr_T, betak;
	double Ass, AsAs, rr1_abs, residual;
	while(1)
	{
		if (lcg_param_.abs_diff) residual = sqrt(rk_mod)/n_size;
		else residual = rk_mod/r0_mod;

		if (LCG_Progress(m, residual, lcg_param_, t, ss))
        {
            return lcg_error_str(LCG_STOP, ss);
        }

		if (residual <= lcg_param_.epsilon)
		{
            if (t == 0) return lcg_error_str(LCG_ALREADY_OPTIMIZIED, ss);
			else return lcg_error_str(LCG_CONVERGENCE, ss);
		}

		if (lcg_param_.max_iterations > 0 && t+1 > lcg_param_.max_iterations)
		{
			return lcg_error_str(LCG_REACHED_MAX_ITERATIONS, ss);
		}
		
		t++;

		LCG_Ax(pk, Apx);

		Apr_T = vecdot(Apx, r0_T);
		ak = rkr0_T/Apr_T;

        vecdiff(sk, rk, Apx, 1.0, ak);

		if (lcg_param_.abs_diff)
		{
			residual = vecdot(sk, sk);
			residual = sqrt(residual)/n_size;

            if (LCG_Progress(m, residual, lcg_param_, t, ss))
            {
                if (t == 0) return lcg_error_str(LCG_ALREADY_OPTIMIZIED, ss);
			    else return lcg_error_str(LCG_CONVERGENCE, ss);
            }

			if (residual <= lcg_param_.epsilon)
			{
                vecapp(m, pk, ak);

                if (!vecvalid(m))
                {
                    return lcg_error_str(LCG_NAN_VALUE, ss);
                }

				return lcg_error_str(LCG_CONVERGENCE, ss);
			}

			if (lcg_param_.max_iterations > 0 && t+1 > lcg_param_.max_iterations)
			{
				return lcg_error_str(LCG_REACHED_MAX_ITERATIONS, ss);
			}
			
			t++;
		}

		LCG_Ax(sk, Adk);

		Ass = vecdot(Adk, sk);
        AsAs = vecdot(Adk, Adk);
		wk = Ass/AsAs;

        vecapp(m, pk, ak);
        vecapp(m, sk, wk);

		if (!vecvalid(m))
        {
            return lcg_error_str(LCG_NAN_VALUE, ss);
        }

        vecdiff(rk, sk, Adk, 1.0, wk);

        rk_mod = vecdot(rk, rk);
		rkr0_T1 = vecdot(rk, r0_T);

		rr1_abs = fabs(rkr0_T1);
		if (rr1_abs < lcg_param_.restart_epsilon)
		{
            veccpy(r0_T, rk);
            veccpy(pk, rk);
			rkr0_T1 = vecdot(rk, r0_T);
			betak = (ak/wk)*rkr0_T1/rkr0_T;
			rkr0_T = rkr0_T1;
		}
		else
		{
			betak = (ak/wk)*rkr0_T1/rkr0_T;
			rkr0_T = rkr0_T1;

            vecdiff(yk, pk, Apx, 1.0, wk);
            vecadd(pk, rk, yk, 1.0, betak);
		}
	}

	return lcg_error_str(LCG_UNKNOWN_ERROR, ss);
}

void gctl::lcg_solver::lpg(array<double> &m, const array<double> &B, 
    const array<double> &low, const array<double> &hig, std::ostream &ss)
{
    size_t n_size = B.size();
    // check parameters
    if (n_size <= 0) return lcg_error_str(LCG_INVILAD_VARIABLE_SIZE, ss);
    if (n_size != m.size()) return lcg_error_str(LCG_SIZE_NOT_MATCH, ss);
    if (lcg_param_.max_iterations < 0) return lcg_error_str(LCG_INVILAD_MAX_ITERATIONS, ss);
    if (lcg_param_.epsilon <= 0.0) return lcg_error_str(LCG_INVILAD_EPSILON, ss);
    if (lcg_param_.step <= 0.0 || lcg_param_.epsilon >= 1.0) return lcg_error_str(LCG_INVALID_LAMBDA, ss);

    // locate memory
    gk.resize(n_size);
    Adk.resize(n_size);
    m_new.resize(n_size);
    gk_new.resize(n_size);
    sk.resize(n_size);
    yk.resize(n_size);
    double alpha_k = lcg_param_.step;

    vectop(m, hig);
    vecbtm(m, low);

    LCG_Ax(m, Adk);

    vecdiff(gk, Adk, B);

    double gk_mod = vecdot(gk, gk);
    double g0_mod = gk_mod;
    if (g0_mod < 1.0) g0_mod = 1.0;

    size_t t = 0;
    double sk_mod, syk_mod, residual;
    while(1)
    {
        if (lcg_param_.abs_diff) residual = sqrt(gk_mod)/n_size;
        else residual = gk_mod/g0_mod;

        if (LCG_Progress(m, residual, lcg_param_, t, ss))
        {
            return lcg_error_str(LCG_STOP, ss);
        }

        if (residual <= lcg_param_.epsilon)
        {
            if (t == 0) return lcg_error_str(LCG_ALREADY_OPTIMIZIED, ss);
            else return lcg_error_str(LCG_CONVERGENCE, ss);
        }

        if (lcg_param_.max_iterations > 0 && t+1 > lcg_param_.max_iterations)
        {
            return lcg_error_str(LCG_REACHED_MAX_ITERATIONS, ss);
        }
        
        t++;
        
        vecdiff(m_new, m, gk, 1.0, alpha_k);
        vectop(m_new, hig);
        vecbtm(m_new, low);

        LCG_Ax(m_new, Adk);

        vecdiff(gk_new, Adk, B);
        vecdiff(sk, m_new, m);
        vecdiff(yk, gk_new, gk);

        sk_mod = vecdot(sk, sk);
        syk_mod = vecdot(sk, yk);

        alpha_k = sk_mod/syk_mod;

        veccpy(m, m_new);
        veccpy(gk, gk_new);

        gk_mod = vecdot(gk, gk);
    }

    return lcg_error_str(LCG_UNKNOWN_ERROR, ss);
}

void gctl::lcg_solver::lspg(array<double> &m, const array<double> &B, 
    const array<double> &low, const array<double> &hig, std::ostream &ss)
{
    size_t n_size = B.size();
    // check parameters
    if (n_size <= 0) return lcg_error_str(LCG_INVILAD_VARIABLE_SIZE, ss);
    if (n_size != m.size()) return lcg_error_str(LCG_SIZE_NOT_MATCH, ss);
    if (lcg_param_.max_iterations < 0) return lcg_error_str(LCG_INVILAD_MAX_ITERATIONS, ss);
    if (lcg_param_.epsilon <= 0.0 || lcg_param_.epsilon >= 1.0) return lcg_error_str(LCG_INVILAD_EPSILON, ss);
    if (lcg_param_.step <= 0.0) return lcg_error_str(LCG_INVALID_LAMBDA, ss);
    if (lcg_param_.sigma <= 0.0 || lcg_param_.sigma >= 1.0) return lcg_error_str(LCG_INVALID_SIGMA, ss);
    if (lcg_param_.beta <= 0.0 || lcg_param_.beta >= 1.0) return lcg_error_str(LCG_INVALID_BETA, ss);
    if (lcg_param_.maxi_m <= 0) return lcg_error_str(LCG_INVALID_MAXIM, ss);

    // locate memory
    gk.resize(n_size);
    Adk.resize(n_size);
    m_new.resize(n_size);
    gk_new.resize(n_size);
    sk.resize(n_size);
    yk.resize(n_size);
    dk.resize(n_size);
    qk_m.resize(lcg_param_.maxi_m);
    double lambda_k = lcg_param_.step;
    double qk = 0;

    vectop(m, hig);
    vecbtm(m, low);

    LCG_Ax(m, Adk);

    vecdiff(gk, Adk, B);
    double gk_mod = vecdot(gk, gk);

    double g0_mod = gk_mod;
    if (g0_mod < 1.0) g0_mod = 1.0;

    // calculate qk
    double mAdk, Bm;
    mAdk = vecdot(m, Adk);
    Bm = vecdot(B, m);
    qk = 0.5*mAdk - Bm;
    qk_m[0] = qk;
    
    size_t i;
    for (i = 1; i < lcg_param_.maxi_m; i++)
    {
        qk_m[i] = -1e+30;
    }

    size_t t = 0;
    double alpha_k, maxi_qk, residual;
    double alpha_mod, sk_mod, syk_mod;
    while(1)
    {
        if (lcg_param_.abs_diff) residual = sqrt(gk_mod)/n_size;
        else residual = gk_mod/g0_mod;

        if (LCG_Progress(m, residual, lcg_param_, t, ss))
        {
            return lcg_error_str(LCG_STOP, ss);
        }

        if (residual <= lcg_param_.epsilon)
        {
            if (t == 0) return lcg_error_str(LCG_ALREADY_OPTIMIZIED, ss);
            else return lcg_error_str(LCG_CONVERGENCE, ss);
        }

        if (lcg_param_.max_iterations > 0 && t+1 > lcg_param_.max_iterations)
        {
            return lcg_error_str(LCG_REACHED_MAX_ITERATIONS, ss);
        }

        t++;

        vecdiff(dk, m, gk, 1.0, lambda_k);
        vectop(dk, hig);
        vecbtm(dk, low);
        vecsub(dk, m);

        alpha_k = 1.0;
        vecadd(m_new, m, dk, 1.0, alpha_k);

        LCG_Ax(m_new, Adk);
        
        mAdk = vecdot(m_new, Adk);
        Bm = vecdot(B, m_new);
        qk = 0.5*mAdk - Bm;

        alpha_mod = vecdot(gk, dk);
        alpha_mod *= lcg_param_.sigma*alpha_k;

        maxi_qk = qk_m[0];
        for (i = 1; i < lcg_param_.maxi_m; i++)
        {
            if (qk_m[i] > maxi_qk) maxi_qk = qk_m[i];
        }

        while(qk > maxi_qk + alpha_mod)
        {
            alpha_k = alpha_k*lcg_param_.beta;

            vecadd(m_new, m, dk, 1.0, alpha_k);

            LCG_Ax(m_new, Adk);
            
            mAdk = vecdot(m_new, Adk);
            Bm = vecdot(B, m_new);
            qk = 0.5*mAdk - Bm;

            alpha_mod = vecdot(gk, dk);
            alpha_mod *= lcg_param_.sigma*alpha_k;
        }

        // put new values in qk_m
        qk_m[(t+1)%lcg_param_.maxi_m] = qk;

        vecdiff(gk_new, Adk, B);
        vecdiff(sk, m_new, m);
        vecdiff(yk, gk_new, gk);

        sk_mod = vecdot(sk, sk);
        syk_mod = vecdot(sk, yk);
        lambda_k = sk_mod/syk_mod;

        veccpy(m, m_new);
        veccpy(gk, gk_new);
        
        gk_mod = vecdot(gk, gk);
    }

    return lcg_error_str(LCG_UNKNOWN_ERROR, ss);
}