#include "WCPRess/LassoModel.h"

#include <Eigen/Dense>
#include <Eigen/Sparse>
using namespace Eigen;

#include <iostream>
using namespace std;

/* Minimize the following problem:
 * 1/(2) * ||Y - beta * X||_2^2 + N * lambda * ||beta||_1
 */

WCP::LassoModel::LassoModel(double lambda, int max_iter, double TOL, bool non_negtive)
  : ElasticNetModel(lambda, 1., max_iter, TOL, non_negtive)
  , flag_initial_values(false)
{
    name = "Lasso";
}

WCP::LassoModel::~LassoModel() {}

void WCP::LassoModel::Set_init_values(std::vector<double> values)
{
    flag_initial_values = true;
    init_betas = values;
}

void WCP::LassoModel::Fit()
{
    // initialize solution to zero
    Eigen::VectorXd beta = VectorXd::Zero(_X.cols());

    if (flag_initial_values) {
        for (size_t i = 0; i != beta.size(); i++) {
            beta(i) = init_betas.at(i);
        }
    }

    // initialize active_beta to true
    int nbeta = beta.size();
    _active_beta = vector<bool>(nbeta, true);

    Eigen::VectorXd y = Gety();
    Eigen::MatrixXd X = GetX();

    // cooridate decsent
    int N = y.size();
    VectorXd norm(nbeta);
    for (int j = 0; j < nbeta; j++) {
        norm(j) = X.col(j).squaredNorm();
        if (norm(j) < 1e-6) {
            cerr << "warning: the " << j << "th variable is not used, please consider removing it." << endl;
            norm(j) = 1;
        }
    }
    double tol2 = TOL * TOL * nbeta;

    // calculate the inner product
    Eigen::VectorXd ydX(nbeta);
    Eigen::SparseMatrix<double> XdX(nbeta, nbeta);
    for (int i = 0; i != nbeta; i++) {
        ydX(i) = y.dot(X.col(i));
        // beta(i) = ydX(i) / norm(i); // first time result saved here ...
        for (int j = 0; j != nbeta; j++) {
            double value = X.col(i).dot(X.col(j));
            if (value != 0) XdX.insert(i, j) = value;
            // std::cout << i << " " << j << " " << value << " " << XdX.coeffRef(i,j) << std::endl;
        }
        // std::cout << ydX(i) << " " << norm(i) << std::endl;
    }
    //  std::cout << "Xin " << nbeta << std::endl;

    // start interation ...
    int double_check = 0;
    for (int i = 0; i < max_iter; i++) {
        VectorXd betalast = beta;

        // loop through sparse matrix ...
        for (int j = 0; j != nbeta; j++) {
            if (!_active_beta[j]) {
                continue;
            }
            beta(j) = ydX(j);
            for (SparseMatrix<double>::InnerIterator it(XdX, j); it; ++it) {
                // if (it.row()!=j && beta(it.row())!=0)
                if (it.row() != j) beta(j) -= it.value() * beta(it.row());
            }
            beta(j) = _soft_thresholding(beta(j) / norm(j), lambda * lambda_weight(j));

            if (fabs(beta(j)) < 1e-6) {
                _active_beta[j] = false;
            }
            // VectorXd X_j = X.col(j);
            // VectorXd beta_tmp = betalast;
            // beta_tmp(j) = 0;
            // VectorXd r_j = (y - X * beta_tmp);
            // double delta_j = X_j.dot(r_j);
            // std::cout << i << " " << j << " " << beta(j) << " " << betalast(j) << std::endl;
        }
        double_check++;
        // cout << endl;
        VectorXd diff = beta - betalast;

        // std::cout << i << " " << diff.squaredNorm() << " " << tol2 << std::endl;

        if (diff.squaredNorm() < tol2) {
            if (double_check != 1) {
                double_check = 0;
                for (int k = 0; k < nbeta; k++) {
                    _active_beta[k] = true;
                }
            }
            else {
                // cout << "found minimum at iteration: " << i << " " << flag_initial_values << endl;
                break;
            }
        }
    }

    // save results in the model
    Setbeta(beta);
}

double WCP::LassoModel::chi2_l1() { return 2 * lambda * Getbeta().lpNorm<1>() * Gety().size(); }
