//
// Created by ziqwang on 04.04.20.
//

#include "InterlockingSolver_Clp.h"
#include "tbb/tbb.h"
#include <Eigen/SparseQR>

#include "ClpInterior.hpp"
#include "ClpSimplex.hpp"
#include "ClpCholeskyDense.hpp"

template<typename Scalar>
bool InterlockingSolver_Clp<Scalar>::isTranslationalInterlocking(InterlockingSolver_Clp::pInterlockingData &data) {
    vector<EigenTriple> tris;
    Eigen::Vector2i size;

    InterlockingSolver<Scalar>::computeTranslationalInterlockingMatrix(tris, size);

    if (!checkSpecialCase(data, tris, false, size)) {
        return false;
    }

    int num_var = size[1];
    InterlockingSolver<Scalar>::appendAuxiliaryVariables(tris, size);
    InterlockingSolver<Scalar>::appendMergeConstraints(tris, size, false);

    return solve(data, tris, false, size[0], size[1], num_var);
}

template<typename Scalar>
bool InterlockingSolver_Clp<Scalar>::isRotationalInterlocking(InterlockingSolver_Clp::pInterlockingData &data) {
    vector<EigenTriple> tris;
    Eigen::Vector2i size;

    InterlockingSolver<Scalar>::computeRotationalInterlockingMatrix(tris, size);

//    std::cout << "special case" << std::endl;
    if (!checkSpecialCase(data, tris, true, size)) {
        return false;
    }

    // Ignore this for lpopt - Enlarging the matrix
    int num_var = size[1];
    InterlockingSolver<Scalar>::appendAuxiliaryVariables(tris, size);
    InterlockingSolver<Scalar>::appendMergeConstraints(tris, size, true);

//    std::cout << "solve" << std::endl;
    return solve(data, tris, true, size[0], size[1], num_var);
}

template<typename Scalar>
bool InterlockingSolver_Clp<Scalar>::checkSpecialCase(pInterlockingData &data,
                                                      vector<EigenTriple> copy_tris,
                                                      bool rotationalInterlockingCheck,
                                                      Eigen::Vector2i copy_size) {

    InterlockingSolver<Scalar>::appendMergeConstraints(copy_tris, copy_size, rotationalInterlockingCheck);

    if (copy_size[0] < copy_size[1]) return true;

    EigenSpMat A(copy_size[0], copy_size[1]);
    A.setFromTriplets(copy_tris.begin(), copy_tris.end());

    Eigen::SparseQR<Eigen::SparseMatrix<double>,
            Eigen::COLAMDOrdering<int> > solver;
    solver.compute(A.transpose());

    if (A.cols() - solver.rank() == 0) {
        return true;
    } else {
        Eigen::VectorXd solution = Eigen::MatrixXd(solver.matrixQ()).rightCols(A.cols() - solver.rank()).col(0);
        std::cout << "||A * x||: " << (A * solution).norm() << ", ||x||: " << solution.norm() << std::endl;
        unpackSolution(data, rotationalInterlockingCheck, solution.data(), copy_size[1]);
        return false;
    }
    return true;
}

template<typename Scalar>
bool InterlockingSolver_Clp<Scalar>::solve(InterlockingSolver_Clp::pInterlockingData &data, vector<EigenTriple> &tris,
                                           bool rotationalInterlockingCheck,
                                           int num_row,
                                           int num_col,
                                           int num_var) {

    //Problem definition
    //tris is equal to a sparse matrix A, which size is [num_row x num_col]
    //our variables are [x, t], a row vector.
    //x: (size: num_var) is the instant translational and rotational velocity.
    //t: (size: num_col - num_var) is the auxiliary variable.
    //the optimization is formulated as:
    //              min \sum_{i = 0}^{num_row} -t_i
    //  s.t.            A[x, t] >= 0
    //                  1 >= t >= 0
    //                    x \in R
    // Ideally if the structure is interlocking, the objective value should be zero.
    // In practice, due to numerical error, we have to allow a small tolerance for the objective value.

    EigenSpMat spatMat(num_row, num_col);
    spatMat.setFromTriplets(tris.begin(), tris.end());

    CoinPackedMatrix matrix(true, num_row, num_col, spatMat.nonZeros(), spatMat.valuePtr(), spatMat.innerIndexPtr(),
                            spatMat.outerIndexPtr(), spatMat.innerNonZeroPtr());

    // boundaries for rows and colums values

    double *objective = new double[num_col];
    double *rowLower = new double[num_row];
    double *rowUpper = new double[num_row];
    double *colLower = new double[num_col];
    double *colUpper = new double[num_col];

    //objects
    for (size_t id = 0; id < num_col; id++) {
        if (id < num_var)
            objective[id] = 0;
        else
            objective[id] = -1;
    }

    //bound
    for (size_t id = 0; id < num_row; id++)
        rowLower[id] = 0;

    for (size_t id = 0; id < num_row; id++)
        rowUpper[id] = 0;

    for (size_t id = 0; id < num_col; id++) {
        if (id < num_var)
            colLower[id] = -COIN_DBL_MAX;
        else
            colLower[id] = 0;
    }

    for (size_t id = 0; id < num_col; id++) {
        if (id < num_var)
            colUpper[id] = COIN_DBL_MAX;
        else
            colUpper[id] = 1;
    }

    if (type == SIMPLEX) {
        return solveSimplex(data, rotationalInterlockingCheck, num_row, num_col, num_var, matrix, colLower, colUpper, objective, rowLower,
                            rowUpper);
    } else {
        return solveBarrier(data, rotationalInterlockingCheck, num_row, num_col, num_var, matrix, colLower, colUpper, objective, rowLower,
                            rowUpper);
    }
}

template<typename Scalar>
bool InterlockingSolver_Clp<Scalar>::solveSimplex(pInterlockingData &data,
                                                  bool rotationalInterlockingCheck,
                                                  int num_row,
                                                  int num_col,
                                                  int num_var,
                                                  const CoinPackedMatrix &matrix,
                                                  const double *colLower,
                                                  const double *colUpper,
                                                  const double *objective,
                                                  const double *rowLower,
                                                  const double *rowUpper) {

    ClpSimplex model(true);
    CoinMessageHandler handler;
    handler.setLogLevel(0); //set loglevel to zero will silence the solver
    model.passInMessageHandler(&handler);
    model.newLanguage(CoinMessages::us_en);

    // load problem
    model.loadProblem(matrix, colLower, colUpper, objective, rowLower, rowUpper);

    // set tolerance
    // a experiment discovery: if the structure is interlocking,
    // the maximum "t" (the auxiliary variables) is around tolerance * 10
    // the average of the "t" is around tolerance * 5.
    // it is very useful to use these number to check whether structure is interlocking or not.
    model.setPrimalTolerance(1e-9);
    // Solve
    model.primal();

    // Solution
    const double target_obj_value = model.rawObjectiveValue();
    double *solution = model.primalColumnSolution();
    double *row_solution = model.primalRowSolution();

    double max_sol = 0;
    for (int id = num_var; id < num_col; id++) {
        max_sol = std::max(solution[id], max_sol);
    }

    unpackSolution(data, rotationalInterlockingCheck, solution, num_var);

    double min_row_sol = MAX_FLOAT;
    for (int id = 0; id < num_row; id++) {
        min_row_sol = std::min(row_solution[id], min_row_sol);
    }

    std::cout << "min_row:\t" << min_row_sol; //should be around zero
    std::cout << ",\tmax_t:\t" << std::abs(max_sol); //interlocking if max_t is around zero
    std::cout << ",\taverage_t:\t" << std::abs(target_obj_value) / num_row << std::endl;//interlocking if average_t is around zero
    if (max_sol < 5e-6) {
        return true;
    } else {
        return false;
    }
}

template<typename Scalar>
bool InterlockingSolver_Clp<Scalar>::solveBarrier(pInterlockingData &data,
                                                  bool rotationalInterlockingCheck,
                                                  int num_row,
                                                  int num_col,
                                                  int num_var,
                                                  const CoinPackedMatrix &matrix,
                                                  const double *colLower,
                                                  const double *colUpper,
                                                  const double *objective,
                                                  const double *rowLower,
                                                  const double *rowUpper) {

    ClpInterior int_model;
    ClpCholeskyDense *cholesky = new ClpCholeskyDense();

    CoinMessageHandler handler;
    handler.setLogLevel(0); //set loglevel to zero will silence the solver
    int_model.passInMessageHandler(&handler);
    int_model.newLanguage(CoinMessages::us_en);

    int_model.loadProblem(matrix, colLower, colUpper, objective, rowLower, rowUpper);
    int_model.setCholesky(cholesky);

    int_model.setPrimalTolerance(1e-8);

    int_model.primalDual();

    // Solution
    const double target_obj_value = int_model.rawObjectiveValue();
    double *solution = int_model.primalColumnSolution();
    double max_sol = 0;
    for (int id = num_var; id < num_col; id++) {
        max_sol = std::max(solution[id], max_sol);
    }

    unpackSolution(data, rotationalInterlockingCheck, solution, num_var);

    //verify solution

    double *row_solution = int_model.primalRowSolution();

    double min_row_sol = MAX_FLOAT;
    for (int id = 0; id < num_row; id++) {
        min_row_sol = std::min(row_solution[id], min_row_sol);
    }

    std::cout << "min_row:\t" << min_row_sol; //should be around zero
    std::cout << ",\tmax_t:\t" << std::abs(max_sol); //interlocking if max_t is around zero
    std::cout << ",\taverage_t:\t" << std::abs(target_obj_value) / num_row << std::endl;//interlocking if average_t is around zero
    if (max_sol < 5e-6) {
        return true;
    } else {
        return false;
    }
}

template<typename Scalar>
void InterlockingSolver_Clp<Scalar>::unpackSolution(InterlockingSolver_Clp::pInterlockingData &data,
                                                    bool rotationalInterlockingCheck,
                                                    const double *solution,
                                                    int num_var) {
    data = make_shared<typename InterlockingSolver<Scalar>::InterlockingData>();
    for (pContactGraphNode node: graph->nodes) {
        Vector3 trans(0, 0, 0);
        Vector3 rotate(0, 0, 0);
        Vector3 center = node->centroid;
        if (node->dynamicID != -1) {
            if (rotationalInterlockingCheck) {
                trans = Vector3(solution[node->dynamicID * 6],
                                solution[node->dynamicID * 6 + 1],
                                solution[node->dynamicID * 6 + 2]);
                rotate = -Vector3(solution[node->dynamicID * 6 + 3],
                                  solution[node->dynamicID * 6 + 4],
                                  solution[node->dynamicID * 6 + 5]);
            } else {
                trans = Vector3(solution[node->dynamicID * 3],
                                solution[node->dynamicID * 3 + 1],
                                solution[node->dynamicID * 3 + 2]);
            }
        }

        data->traslation.push_back(trans);
        data->rotation.push_back(rotate);
        data->center.push_back(center);

//        std::cout << node->staticID << ":" << trans.transpose() << ", " << rotate.transpose() << std::endl;
    }
}

void TemporaryFunction_InterlockingSolver_Clp ()
{
    InterlockingSolver_Clp<double> solver(nullptr, nullptr);
}


/* p_integrand.cpp */
#include <math.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <boost/math/special_functions/gamma.hpp>
#include <algorithm>

extern "C"
{
/* Survival function for R^phi*W */
int RW_marginal_C(double *xval, double phi, double gamma, int n_xval, double *result){
    double tmp2 = pow(gamma/2, phi)/boost::math::tgamma(0.5);
    double tmp1, tmp0, a;
    a = 0.5-phi;

    for(int i=0; i<n_xval; i++){
        tmp1 = gamma/(2*pow(xval[i],1/phi));
        tmp0 = tmp2/(a*xval[i]);
        result[i] = boost::math::gamma_p(0.5L,tmp1) + boost::math::tgamma((long double)(a+1),tmp1)*tmp0-pow(tmp1,a)*exp(-tmp1)*tmp0;
    }
    return 1;
}

/* Marginal distribution function for R^phi*W + epsilon */
int pRW_me_interp_C(double *xval, double *xp, double *surv_p, double tau_sqd, double phi, double gamma, int n_xval, int n_grid, double *result){
    bool tau_bool = (tau_sqd > 0.05);
    double tp[n_grid];
    double integrand_p[n_grid];
    double tmp, tmp_res; /* temporary constant */
    double tmp_sum = 0; /* temporary trapesoid sum */
    double sd = sqrt(tau_sqd);
    double sd_const = sqrt(2)*sd;
    double sd_const_pi =sqrt(2*M_PI)*sd;
    int i,j, tmp_int; /* iterative constants */

    for (i = 0; i < n_xval; i++) {
        if(tau_bool & (xval[i]<820)){
            /* Calculate integrand on a grid */
            for(j=0; j<n_grid;j++){
                tmp = xval[i]-xp[j];
                tp[j] = tmp;
                integrand_p[j] = exp(-tmp*tmp/(2*tau_sqd)) * surv_p[j];
            }

            /* Numerical integral using the trapesoid method */
            for(j=0; j<(n_grid-1);j++){
                tmp_sum+= (tp[j+1]-tp[j])*(integrand_p[j] + integrand_p[j+1])/2;
            }
            tmp_res = 0.5*erfc(-xval[i]/sd_const)-tmp_sum/sd_const_pi;
            tmp_sum = 0;

            /* CDF value must be greater than 0 */
            if(tmp_res < 0){
                tmp_res = 0;
            }
            result[i] = tmp_res;
        }
        else{
            tmp_int = RW_marginal_C(&xval[i], phi, gamma, 1, &tmp_res);
            result[i] = 1-tmp_res;
        }
    }

    return 1;
}

/* Transform to uniform scales from RW mixtures */
int RW_me_2_unifs(double *X, double *xp, double *Surv, double tau_sqd, double *phi, double gamma,
                    int n_s, int n_grid, int n_t, double *unifs){
    int tmp_int, X_lookup, Surv_lookup;
    for (int i = 0; i<n_s;i++){
        X_lookup = i*n_t;
        Surv_lookup = i*n_grid;
        tmp_int = pRW_me_interp_C(X+X_lookup, xp, Surv+Surv_lookup, tau_sqd, phi[i], gamma, n_t, n_grid, unifs+X_lookup);
    }

    return 1;
}

/* Get the quantile range for certain probability levels */
int find_xrange_pRW_C(double min_p, double max_p, double min_x, double max_x, double phi, double gamma, double *x_range){
    if (min_x >= max_x){
        printf("Initial value of mix_x must be smaller than max_x.\n");
        exit(EXIT_FAILURE);
    }

    /* First the min */
    double p_min_x;
    int tmp_int;
    tmp_int = 1-RW_marginal_C(&min_x, phi, gamma, 1, &p_min_x);
    while (1-p_min_x > min_p){
        min_x = min_x/2; /* R^phi*W is always positive */
        tmp_int = RW_marginal_C(&min_x, phi, gamma, 1, &p_min_x);
    }

    x_range[0] = min_x;

    /* Now the max */
    double p_max_x;
    tmp_int = RW_marginal_C(&max_x,  phi, gamma, 1, &p_max_x); /* Survival value */
    while (1 - p_max_x < max_p){
        max_x = max_x*2; /* Upper will set to 20 initially */
        tmp_int = RW_marginal_C(&max_x, phi, gamma, 1, &p_max_x);
    }

    x_range[1] = max_x;
    return 1;
}

/* Get the quantile using the bisection method */
double qRW_bisection_C(double p, double phi, double gamma, int n_x){
    double x_range[2];
    int tmp_res = 0;
    tmp_res = find_xrange_pRW_C(p, p, 1.0, 5.0, phi, gamma, x_range);
    double m = (x_range[0]+x_range[1])/2;
    int iter=0;
    double new_F;
    tmp_res = RW_marginal_C(&m, phi, gamma, 1, &new_F);
    double diff = 1-new_F-p;
    while (iter<100 & abs(diff)> 1e-04){
        if (diff>0){
            x_range[1] = m;}
        else{
            x_range[0]=m;}
        m = (x_range[0]+x_range[1])/2;
        tmp_res = RW_marginal_C(&m, phi, gamma, 1, &new_F);
        diff = 1-new_F-p;
        iter += 1;
    }
    return m;
}

/* Get the quantile range for certain probability levels */
int find_xrange_pRW_me_C(double min_p, double max_p, double min_x, double max_x, double *xp, double *surv_p, double tau_sqd, double phi, double gamma, int n_grid, double *x_range){
    if (min_x >= max_x){
        printf("Initial value of mix_x must be smaller than max_x.\n");
        exit(EXIT_FAILURE);
    }

    /* First the min */
    double p_min_x;
    int tmp_int;
    tmp_int = pRW_me_interp_C(&min_x, xp, surv_p, tau_sqd, phi, gamma, 1, n_grid, &p_min_x);
    while (p_min_x > min_p){
        min_x = min_x-40/phi;
        tmp_int = pRW_me_interp_C(&min_x, xp, surv_p, tau_sqd, phi, gamma, 1, n_grid, &p_min_x);
    }

    x_range[0] = min_x;

    /* Now the max */
    double p_max_x;
    tmp_int = pRW_me_interp_C(&max_x, xp, surv_p, tau_sqd, phi, gamma, 1, n_grid, &p_max_x);
    while (p_max_x < max_p){
        max_x = max_x*2; /* Upper will set to 20 initially */
        tmp_int = pRW_me_interp_C(&max_x, xp, surv_p, tau_sqd, phi, gamma, 1, n_grid, &p_max_x);
    }

    x_range[1] = max_x;
    return 1;
}

/* Density function for R^phi*W */
int RW_density_C(double *xval, double phi, double gamma, int n_xval, double *result){
    double tmp2 = pow(gamma/2, phi)/boost::math::tgamma(0.5);
    double tmp1, tmp0, a;
    a = 0.5-phi;

    for(int i=0; i<n_xval; i++){
        tmp1 = gamma/(2*pow(xval[i],1/phi));
        tmp0 = tmp2/(a*pow(xval[i],2));
        result[i] = (boost::math::tgamma((long double)(a+1),tmp1)-pow(tmp1,a)*exp(-tmp1))*tmp0;
    }
    return 1;
}

/* Get the quantile using Newton-Raphson method */
double qRW_newton_C(double p, double phi, double gamma, int n_x){
    double x_range[2];
    int tmp_res = 0;
    tmp_res = find_xrange_pRW_C(p, p, 1.0, 5.0, phi, gamma, x_range);
    double new_x, current_x = x_range[0];
    int iter=0;
    double error=1;
    double Surv_value, f_value;

    while (iter<400 & error> 1e-08){
        tmp_res = RW_marginal_C(&current_x , phi, gamma, 1, &Surv_value);
        tmp_res = RW_density_C(&current_x , phi, gamma, 1, &f_value);
        new_x = current_x - (1-Surv_value-p)/f_value;
        error = abs(new_x-current_x);
        iter += 1;
        current_x = fmax(x_range[0], new_x);
        if(current_x == x_range[0]){current_x = qRW_bisection_C(p, phi, gamma, 100);}
    }

    return current_x;
}

/* Marginal density function for R^phi*W + epsilon */
int dRW_me_interp_C(double *xval, double *xp, double *den_p, double tau_sqd, double phi, double gamma, int n_xval, int n_grid, double *result){
    double thresh_large = 820;
    if(tau_sqd < 1) {
        thresh_large = 50;
    }
    bool tau_bool = (tau_sqd > 0.05);

    double tp[n_grid];
    double integrand_p[n_grid];
    double tmp, tmp_res; /* temporary constant */
    double tmp_sum = 0; /* temporary trapesoid sum */
    double sd = sqrt(tau_sqd);
    double sd_const_pi =sqrt(2*M_PI)*sd;
    int i,j, tmp_int; /* iterative constants */

    for (i = 0; i < n_xval; i++) {
        if(tau_bool & (xval[i]<thresh_large)){
            /* Calculate integrand on a grid */
            for(j=0; j<n_grid;j++){
                tmp = xval[i]-xp[j];
                tp[j] = tmp;
                integrand_p[j] = exp(-tmp*tmp/(2*tau_sqd)) * den_p[j];
            }

            /* Numerical integral using the trapesoid method */
            for(j=0; j<(n_grid-1);j++){
                tmp_sum+= (tp[j+1]-tp[j])*(integrand_p[j] + integrand_p[j+1])/2;
            }
            tmp_res = tmp_sum/sd_const_pi;
            tmp_sum = 0;
            result[i] = tmp_res;
        }else if((tau_bool & (xval[i]>=thresh_large))|(!tau_bool & (xval[i]>0))){
            tmp_int = RW_density_C(&xval[i], phi, gamma, 1, &tmp_res);
            result[i] = tmp_res;
        }else{
            result[i] = 0;
        }
    }

    return 1;
}

int density_interp_grid(double *xp, double *phi, double gamma, int n_phi, int n_grid, double *Den, double *Surv){
    int counter = 0;
    int i,j, tmp_int;
    double tmp_surv, tmp_den;
    double tmp2, tmp1, tmp0, a, tmp_incomp, tmp_phi, tmp_phi_inv, tmp_xp;
    double gamma_half = gamma/2;

    for(i=0; i<n_phi; i++){
        tmp_phi = phi[i];
        a = 0.5-tmp_phi;
        tmp2 = std::pow(gamma_half, tmp_phi)/(a*sqrt(M_PI));
        tmp_phi_inv = 1/tmp_phi;
        for(j=0; j<n_grid; j++){
            tmp_xp = xp[j];
            tmp1 = gamma_half/std::pow(tmp_xp,tmp_phi_inv);
            tmp0 = tmp2/tmp_xp;
            tmp_incomp = (boost::math::tgamma((long double)(a+1),tmp1)-std::pow(tmp1,a)*exp(-tmp1))*tmp0;
            Surv[counter] = boost::math::gamma_p(0.5L,tmp1) + tmp_incomp;
            Den[counter++] = tmp_incomp/tmp_xp;
        }
    }
    return 1;
}

double dgev_C(double y, double loc, double scale, double shape, bool log_out){
    double t = std::pow(1+shape*((y-loc)/scale), -1/shape);
    double result;
    if(log_out){
        result = -log(scale)+(shape+1)*log(t)-t;
    }else{
        result = std::pow(t, shape+1)*exp(-t)/scale;
    }
    return result;
}

double dnorm_C(double y, double mean, double sd, bool log_out){
    double t=(y-mean)/sd;
    double result;
    if(log_out){
        result = -0.5*log(2*M_PI)-log(sd)-0.5*t*t;
    }else{
        result = exp(-0.5*t*t)/(sqrt(2*M_PI)*sd);
    }
    return result;
}

/* Thresh_X and Thresh_X_above are required */
/* xp, den_p and surv_p are required */
/* Calculate column_wise in C order OR one time  */
double marg_transform_data_mixture_me_likelihood_C(double *Y, double *X, double *X_s, bool *cen, bool *cen_above,
                                                double *Loc, double *Scale, double *Shape,
                                                double tau_sqd, double *phi, double gamma,
                                                double *xp, double *Den, int n_s, int n_grid){
    double sd = sqrt(tau_sqd);
    double sd_const = sqrt(2)*sd;
    double ll=0;
    double RW_den;
    int i, tmp_int, Den_lookup;

    for (i=0; i<n_s; i++){
        if(cen[i]){
            ll += log(0.5*erfc(-(X[i]-X_s[i])/sd_const));
        }else if(cen_above[i]){
            ll += log(1-0.5*erfc(-(X[i]-X_s[i])/sd_const));
        }else{
            Den_lookup = i*n_grid;
            tmp_int = dRW_me_interp_C(&X[i], xp, &Den[Den_lookup], tau_sqd, phi[i], gamma, 1, n_grid, &RW_den);
            ll += dnorm_C(X[i], X_s[i], sd, true)+dgev_C(Y[i], Loc[i], Scale[i], Shape[i], true)-log(RW_den);
        }
    }
    return ll;
}

/* Calculate row_wise in F order OR one location */
double marg_transform_data_mixture_me_likelihood_F(double *Y, double *X, double *X_s, bool *cen, bool *cen_above,
                                                double *Loc, double *Scale, double *Shape,
                                                double tau_sqd, double phi, double gamma,
                                                double *xp, double *den_p, int n_t, int n_grid){
    double sd = sqrt(tau_sqd);
    double sd_const = sqrt(2)*sd;
    double ll=0;
    double RW_den;
    int i, tmp_int;

    for (i=0; i<n_t; i++){
        if(cen[i]){
            ll += log(0.5*erfc(-(X[i]-X_s[i])/sd_const));
        }else if(cen_above[i]){
            ll += log(1-0.5*erfc(-(X[i]-X_s[i])/sd_const));
        }else{
            tmp_int = dRW_me_interp_C(&X[i], xp, den_p, tau_sqd, phi, gamma, 1, n_grid, &RW_den);
            ll += dnorm_C(X[i], X_s[i], sd, true)+dgev_C(Y[i], Loc[i], Scale[i], Shape[i], true)-log(RW_den);
        }
    }
    return ll;
}

/* Calculate all locations and all times */
double marg_transform_data_mixture_me_likelihood_global(double *Y, double *X, double *X_s, bool *cen, bool *cen_above,
                                                double *Loc, double *Scale, double *Shape,
                                                double tau_sqd, double *phi, double gamma,
                                                double *xp, double *Den, int n_s, int n_t, int n_grid){

    double ll=0;
    int site, tmp_int, Den_lookup, X_lookup;

    for (site=0; site<n_s; site++){
        X_lookup = site*n_t;
        Den_lookup = site*n_grid;
        ll += marg_transform_data_mixture_me_likelihood_F(&Y[X_lookup], &X[X_lookup], &X_s[X_lookup], &cen[X_lookup], &cen_above[X_lookup], &Loc[X_lookup], &Scale[X_lookup], &Shape[X_lookup], tau_sqd, phi[site], gamma, xp, &Den[Den_lookup], n_t, n_grid);
    }
    return ll;
}

void print_c(double *Y, int n_grid){
    printf("%4.2f %4.2f\n",*Y,*(Y+n_grid-1));
}

double print_Vec(double *Y, int n_grid, int n_s){
    int Den_lookup;
    for(int i=0; i<n_s; i++){
        Den_lookup = i*n_grid;
        print_c(&Y[Den_lookup], n_grid);
    }

    return Y[0];
}

}


//
// Created by lynn on 2020/7/19.
//
//#include <acado/acado_optimal_control.hpp>
#include <acado/acado_toolkit.hpp>
#include "ros/ros.h"
#include <chrono>
#include "iomanip"
#include <acado/acado_gnuplot.hpp>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Odometry.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float32.h>
#include "offb_posctl/controlstate.h"
//#include <Eigen/Eigen>// This step will generate error about eigen ,the reason is not clear by far
#include <Eigen/Geometry>
#include <Eigen/Core>
#include <thread>
#include <math.h>
#include <stdlib.h>
#include <FILTER.h>

using namespace Eigen;
using namespace std;

geometry_msgs::PoseStamped pos_ref;         //无人机参考位置

mavros_msgs::State current_state;           //无人机当前状态(mode arm)
sensor_msgs::Imu   imu_drone;               //读入的无人机的IMU信息 包括姿态角和线加速度

geometry_msgs::PoseStamped  pos_drone;      //读入的无人机当前位置
geometry_msgs::PoseStamped  pos_drone_last; //读入的无人机上一次位置
geometry_msgs::PoseStamped  fused_drone; //融合了vicon数据的无人机位姿，注意是NED坐标下的，需要翻转

geometry_msgs::TwistStamped vel_drone;      //读入的无人机当前速度

geometry_msgs::Vector3 acc_receive;         //读入的无人机线加速度
geometry_msgs::Vector3 angle_receive;       //读入的无人机姿态（欧拉角）
geometry_msgs::Vector3 angle_fromvicon;       //读入的vicon计算的sgement姿态（欧拉角）
geometry_msgs::Vector3 angle_fromvicon_qua;       //读入的vicon计算的sgement姿态（欧拉角）
geometry_msgs::Vector3 angle_fromviconinit;  //读入的vicon计算的sgement initial 姿态（欧拉角）
geometry_msgs::Vector3 fusedangle_receive;

geometry_msgs::Quaternion orientation_target;   //发给无人机的姿态指令

geometry_msgs::Vector3 angle_des;            //线性模型输出的理想值
//geometry_msgs::Vector3 angle_dis;            //DOB控制器估计的扰动值
geometry_msgs::Vector3 angle_target;            //经DOB控制器作用后的实际系统输入值
geometry_msgs::Vector3 vel_target, vel_read, vel_read2;
geometry_msgs::Vector3 currentRPYangle;   //欧拉角
geometry_msgs::Vector3 filteredPlaneVelmsg;   //欧拉角
// debug data
geometry_msgs::Vector3 vel_vicon;
const float MAX_POSITION_MEASURE_ERROR = 1;
Eigen::Quaterniond current_quaternion(0,0,0,0);

bool pose_initialized = false;
bool vel_initialized = false;
int posvel_updatesynflag= false;// the variable to make sure the pos and vel used in ocp is synchronized, In reality, we can put pos and vel in one topic to avoid this problem
USING_NAMESPACE_ACADO
VariablesGrid state, parameter, control;// used for output result
returnValue resultofsolve;
int timeconsumption=0;
float t_end=2;
int pointnumber=20;// the number is almost always 20. It less, the accuracy won't be enough, if more, the time consumpiton will be too large.
int controlfreq=50;
int discretizedpointpersecond=(int)pointnumber/t_end;
float px_ini = -3.0;
float pz_ini = 0.0;
float vx_ini = -0.1;
float vz_ini = 0.0;
float theta_ini = 0.0;
float rate_ini = 0.0;
float vxPlane_ini=0.0,vyPlane_ini=0.0,vzPlane_ini=0.0;
float aircoeffx=1.5,aircoeffz=0.35;
float carPlaneDistance=3.0;
offb_posctl::controlstate controlstate_msg;
bool currentupdateflag= false;
FILTER filterDroneVelx(150);
sensor_msgs::Imu drone_imu;
void do_process()
{
    chrono::time_point<chrono::steady_clock> begin_time = chrono::steady_clock::now();
    DifferentialState        px,pz,vx,vz,vxPlane,theta,rate;     // px=xdrone-xcar, pz=zdrone-zcar vx=dot(px),vz=dot(pz)
    Control                  u1,u2          ;     // u1 is thrust, u2 is theta
    DifferentialEquation     f( 0.0, t_end );     // the differential equation
    f << dot(px) == vx;                         // an implementation
    f << dot(pz) == vz;             // of the model equations
    f << dot(vx) == u1*sin(theta)-aircoeffx*vxPlane;                 // for the drone.
    f << dot(vz) == u1*cos(theta)-9.8-aircoeffz*vxPlane;
    f << dot(vxPlane) == u1*sin(theta)-aircoeffx*vxPlane;
    f << dot(theta)==rate;
    f << dot(rate)==u2;
    OCP ocp_(0.0,t_end,pointnumber);
    ocp_.minimizeLagrangeTerm(1*(pz+px*theta)*(pz+px*theta)+3*(px+carPlaneDistance)*(px+carPlaneDistance)+0.5*(u1-9.8)*(u1-9.8)+0.5*u2*u2); // the 1.5 is for the target high(in this gazebo the height of car is 0)
//        ocp_.minimizeLagrangeTerm(1*(pz+px*u2)*(pz+px*u2)+3*(px+carPlaneDistance)*(px+carPlaneDistance)+0.5*(u1-9.8)*(u1-9.8)+0.5*u2*u2); // the 1.5 is for the target high(in this gazebo the height of car is 0)
//    ocp_.minimizeLagrangeTerm((pz-0.61-(0.1*t-px)*u2)*(pz-0.61-(0.1*t-px)*u2)+(0.1*t-px-3)*(0.1*t-px-3)+0.5*u1*u1+0.5*u2*u2);

    ocp_.subjectTo( f                   );     // minimize T s.t. the model,
    ocp_.subjectTo( AT_START, px == px_ini );     // the initial values for s,
    ocp_.subjectTo( AT_START, pz == pz_ini);     // v,
    ocp_.subjectTo( AT_START, vx == vx_ini);     // and m,
    ocp_.subjectTo( AT_START, vz == vz_ini);     // and m,
    ocp_.subjectTo( AT_START, vxPlane == vxPlane_ini);     // and m,
    ocp_.subjectTo( AT_START, theta == theta_ini);     // and m,
    ocp_.subjectTo( AT_START, rate == rate_ini);     // and m,

    ocp_.subjectTo( 0 <= u1 <=  9.8*2   );     // the crol input u,
    ocp_.subjectTo(  -0.628<= theta <= 0.628  );     // and the time horizon T.
    ocp_.subjectTo(  -1<= vxPlane <= 10  );     // and the time horizon T.
    ocp_.subjectTo(  -3<= rate <= 3  );
    ocp_.subjectTo(  -20<= u2 <= 20  );

    OptimizationAlgorithm algorithm(ocp_);     // the optimization algorithm
    algorithm.set(MAX_NUM_ITERATIONS,50);// default value is 1000
    algorithm.set(MAX_NUM_QP_ITERATIONS,100);//default value is 10000
    algorithm.set(KKT_TOLERANCE,1e-3);// default value is 1e-6
    algorithm.set(PRINTLEVEL,NONE);// do not print solution process.
    try {
        resultofsolve=algorithm.solve();                        // solves the problem.
        algorithm.getControls(control);
        algorithm.getDifferentialStates(state);
//        cout << "controls:  time   |  controls" <<endl;
//        control.print();
    }catch (exception e)
    {
        resultofsolve=-1;
        std::cout<<"ocp throw exception"<<std::endl;
    }
    px.clearStaticCounters();
    pz.clearStaticCounters();
    vx.clearStaticCounters();
    vz.clearStaticCounters();
    vxPlane.clearStaticCounters();
    theta.clearStaticCounters();
    rate.clearStaticCounters();
    u1.clearStaticCounters();
    u2.clearStaticCounters();
    chrono::time_point<chrono::steady_clock> end_time = chrono::steady_clock::now();
    timeconsumption=chrono::duration_cast<chrono::milliseconds>(end_time - begin_time).count();
    cout <<"time acado consume~~...........~~~~~~~~mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm: "<<timeconsumption<< endl;
//    controlcounter=floor(timeconsumption/1000.0*controlfreq)
}

/**match the current state with the newest states solved by OCP, and the time of the nearest state will be the first used point
// in the newest solved input. the return value is the matched index
*/
double tempStateX=0.0,tempStateZ=0.0,tempStateVX=0.0,tempStateVZ=0.0;
int StateMatch()
{
    int k1=1,k2=1,k3=1,k4=1,lefnodeindex=0,rightnodeindex=0,controlcounter=0,mincounter=0;
    float minStateDistance=0.0,currentStateDistance=0.0,stateX=0.0,stateZ=0.0,stateVX=0.0,stateVZ=0.0;
    for(;rightnodeindex<controlstate_msg.stateXarray.size();)
    {
        lefnodeindex=floor(controlcounter*discretizedpointpersecond/controlfreq);
        rightnodeindex=lefnodeindex+1;
        if(rightnodeindex<controlstate_msg.stateXarray.size())
        {
            //interpolition
            stateX= (rightnodeindex-(double)controlcounter*discretizedpointpersecond/controlfreq)*controlstate_msg.stateXarray[lefnodeindex]
                                +((double)controlcounter*discretizedpointpersecond/controlfreq-lefnodeindex)*controlstate_msg.stateXarray[rightnodeindex];

            stateZ =(rightnodeindex-(double)controlcounter*discretizedpointpersecond/controlfreq)*controlstate_msg.stateZarray[lefnodeindex]
                               +((double)controlcounter*discretizedpointpersecond/controlfreq-lefnodeindex)*controlstate_msg.stateZarray[rightnodeindex];

            stateVX= (rightnodeindex-(double)controlcounter*discretizedpointpersecond/controlfreq)*controlstate_msg.stateVXarray[lefnodeindex]
                    +((double)controlcounter*discretizedpointpersecond/controlfreq-lefnodeindex)*controlstate_msg.stateVXarray[rightnodeindex];

            stateVZ =(rightnodeindex-(double)controlcounter*discretizedpointpersecond/controlfreq)*controlstate_msg.stateVZarray[lefnodeindex]
                    +((double)controlcounter*discretizedpointpersecond/controlfreq-lefnodeindex)*controlstate_msg.stateVZarray[rightnodeindex];
            currentStateDistance=k1*pow(stateX-px_ini,2)+k2*pow(stateZ-pz_ini,2)+k3*pow(stateVX-vx_ini,2)+k4*pow(stateVZ-vx_ini,2);
            if(minStateDistance==0)
            {
                minStateDistance=currentStateDistance;
                controlcounter=0;
            } else{
                if(minStateDistance>currentStateDistance)
                {
                    minStateDistance=currentStateDistance;
                    mincounter=controlcounter;
                    tempStateX=stateX;
                    tempStateZ=stateZ;
                    tempStateVX=stateVX;
                    tempStateVZ=stateVZ;
                }
            }
        }
        controlcounter++;
    }
//    std::cout<<"matchedcoutner:---"<<mincounter<<" X:"<<tempStateX<<" Z:"<<tempStateZ<<" VX:"<<tempStateVX<<" VZ:"<<tempStateVZ<<endl;
    return mincounter;
}

void state_cb(const mavros_msgs::State::ConstPtr &msg){
    current_state = *msg;
}

bool hasGotImu = false;
void droneImu_cb(const sensor_msgs::Imu::ConstPtr &msg)
{
    drone_imu=*msg;
    rate_ini=drone_imu.angular_velocity.y;
}

void dronerpy_cb(const geometry_msgs::Vector3::ConstPtr &msg)
{
    currentRPYangle =*msg;
    theta_ini=currentRPYangle.y;
//    std::cout <<"currentRPYangle-----------theta_ini:"<<theta_ini<<endl;
}

void pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg){ //由ConstPtr可以看到msg是指针的引用，因此*msg相当于对被引用的指针对象取值
    if (pose_initialized== false)
    {
        pos_drone_last=*msg;
        pose_initialized=true;
        return;
    }
    if(fabs((*msg).pose.position.x - pos_drone_last.pose.position.x) < MAX_POSITION_MEASURE_ERROR &&
       fabs((*msg).pose.position.y - pos_drone_last.pose.position.y) < MAX_POSITION_MEASURE_ERROR &&
       fabs((*msg).pose.position.z - pos_drone_last.pose.position.z) < MAX_POSITION_MEASURE_ERROR)
    {
        pos_drone_last = pos_drone;
        pos_drone = *msg;
        current_quaternion=Eigen::Quaterniond(pos_drone.pose.orientation.w,pos_drone.pose.orientation.x,pos_drone.pose.orientation.y,pos_drone.pose.orientation.z);
       if(pose_initialized&&vel_initialized)
       {
           if (fabs(pos_drone.header.stamp.nsec-vel_drone.header.stamp.nsec)<30e6)
           {
               posvel_updatesynflag=true;
           }
       }
    } else
    {
        pos_drone_last=*msg;
    }
}

void plane_vel_cb(const geometry_msgs::TwistStamped::ConstPtr &msg){
    vel_drone = *msg;
//    vxPlane_ini=filterDroneVelx.filter(vel_drone.twist.linear.x);
    vxPlane_ini=(vel_drone.twist.linear.x);
    filteredPlaneVelmsg.x=vxPlane_ini;
    vyPlane_ini=vel_drone.twist.linear.y;
    vzPlane_ini=vel_drone.twist.linear.z;
}

void relative_postwist_cb(const nav_msgs::Odometry::ConstPtr &msg)
{
    px_ini=(*msg).pose.pose.position.x;
    pz_ini=(*msg).pose.pose.position.z;
    vx_ini=(*msg).twist.twist.linear.x;
    vz_ini=(*msg).twist.twist.linear.z;
    currentupdateflag=true;
//    std::cout <<"ocpsolveriniconditons-----------px_ini:  " << px_ini <<"   vx_ini:  " << vx_ini<<"  pz_ini:  " << pz_ini <<"   vz_ini:  " << vz_ini<<
//    "   theta_ini:  " << theta_ini<< "   rate_ini:  " << rate_ini<<std::endl;

}
int main( int argc, char ** argv)
{
    ros::init(argc, argv, "acado_lag_control");
    ros::NodeHandle nh;
    ros::Rate rate(100);// it is the rate to check and process callback functions. the high rate is to avoid the latency to accept the pos and vel
//    //Gazebo 仿真数据
    ros::Subscriber drone_imu_sub=nh.subscribe<sensor_msgs::Imu>("/mavros/imu/data",10,droneImu_cb);
    ros::Subscriber plane_rpy_sub = nh.subscribe<geometry_msgs::Vector3>("drone/current_rpy",10,dronerpy_cb);
    ros::Subscriber car_position_sub = nh.subscribe<nav_msgs::Odometry>("current_relative_postwist",10,relative_postwist_cb); //车的pos+twist
    ros::Subscriber plane_velocity_sub = nh.subscribe<geometry_msgs::TwistStamped>("mavros/local_position/velocity_local", 10, plane_vel_cb); //twist

    // 【发布】飞机姿态/拉力信息 坐标系:NED系
    ros::Publisher controlstate_pub=nh.advertise<offb_posctl::controlstate>("ocp/control_state",10);
    ros::Publisher planeVel_pub=nh.advertise<geometry_msgs::Vector3>("filteredPlaneVel",10);
//  -------------------------------------
    while (ros::ok())
    {
        ros::spinOnce();// to examine the queues of the callback functions once
        if(currentupdateflag)
        {
            do_process();
            if(resultofsolve==SUCCESSFUL_RETURN)
            {
                controlstate_msg.discrepointpersecond=discretizedpointpersecond;
                controlstate_msg.inicounter=0;
                controlstate_msg.arraylength=control.getNumPoints();
                controlstate_msg.thrustarray.clear();
                controlstate_msg.thetaarray.clear();
                controlstate_msg.stateXarray.clear();
                controlstate_msg.stateZarray.clear();
                controlstate_msg.stateVXarray.clear();
                controlstate_msg.stateVZarray.clear();
                for(int i=0;i<control.getNumPoints();i++)
                {
//                    cout<<"control.getNumPoints()-----!!!!:"<<control.getNumPoints()<<"  control.getMatrix(i)(0,0):"<<control.getMatrix(i)(0,0)<<endl;
                    controlstate_msg.thrustarray.push_back((float)control.getMatrix(i)(0,0));
//                    controlstate_msg.thetaarray.push_back((float)control.getMatrix(i)(1,0));
                    controlstate_msg.stateXarray.push_back((float)state.getMatrix(i)(0,0));
                    controlstate_msg.stateZarray.push_back((float)state.getMatrix(i)(1,0));
                    controlstate_msg.stateVXarray.push_back((float)state.getMatrix(i)(2,0));
                    controlstate_msg.stateVZarray.push_back((float)state.getMatrix(i)(3,0));
                    controlstate_msg.thetaarray.push_back((float)state.getMatrix(i)(5,0));

                }
//                ros::spinOnce();// update to newest state to match the state.
//                controlstate_msg.inicounter=StateMatch();//match the state
                controlstate_pub.publish(controlstate_msg);
//                cout<<"controlstate_msg.stateXarray[0]-----------fffffff:"<<controlstate_msg.stateXarray[0]<<"  state.getMatrix(i)(0,0):"<<state.getMatrix(0)(0,0)<<endl;
            }
            currentupdateflag= false;
            planeVel_pub.publish(filteredPlaneVelmsg);
        }
        rate.sleep();
    }

    return 0;
}


//////////////////////////////////////////////////////////////////////////////////
// survival::models::exponential::scalar::function::log_unnormalized_pdf.hpp    //
//                                                                              //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                   //
//  Software License, Version 1.0. (See accompanying file                       //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)            //
//////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_MODELS_EXPONENTIAL_SCALAR_FUNCTION_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_MODELS_EXPONENTIAL_SCALAR_FUNCTION_INCLUDE_HPP_ER_2009

#include <boost/statistics/detail/distribution/survival/models/exponential/scalar/function/log_unnormalized_pdf.hpp>

#endif


#ifndef ANGULAR_GAUSS_HPP
#define ANGULAR_GAUSS_HPP

#include <array>

#include <boost/numeric/ublas/matrix.hpp>

#include "types/types.hpp"

namespace ublas = boost::numeric::ublas;

// Angular Gaussian distribution
class AngularGauss {
    Vector mean;
    double mean_norm;

    ublas::matrix<double> lambda;
    double det_lambda;

    // InvMatrix calculates inverse matrix for input_mat and stores it in
    // inverse_mat
    void InvMatrix(const ublas::matrix<double>& input_mat, ublas::matrix<double>& inverse_mat);
    // Det calculates m's determinant
    double Det(const ublas::matrix<double>& m) const;
    // InnerProduct calculates inner product of x and y using lambda
    double InnerProduct(const Vector& x, const Vector& y) const;

public:
    // AngularGauss creates new Angular Gaussian distribution with mean using
    // mean_vec and covariance using cov_mat
    AngularGauss(const Vector& mean_vec, const ublas::matrix<double>& cov_mat);

    // Mean returns mean vector for Angular Gaussian distribution
    const Vector Mean(void) const;

    // Calc calculates the Angular Gaussian PDF at u
    double Calc(const CoordsOfPoint& u) const;
};

#endif // ANGULAR_GAUSS_HPP


//  ================================================================
//  Created by Fei Shan on 03/05/19.
//  ================================================================

#pragma once

//libraries
#include <Eigen/Eigen>

namespace eig = Eigen;

namespace math{

    eig::Matrix3f transformation_vector_to_matrix2d(const eig::Vector3f &twist);
    eig::Matrix4f transformation_vector_to_matrix3d(const eig::Matrix<float, 6, 1>& twist);

}


#include "pd/edge_length_constraint.h"

#include <Eigen/SparseCore>
#include <array>

namespace pd {

void edge_length_constraint_t::project_wi_SiT_AiT_Bi_pi(q_type const& q, Eigen::VectorXd& b) const
{
    using index_type         = decltype(indices().front());
    index_type const vi      = indices().at(0);
    index_type const vj      = indices().at(1);
    Eigen::Vector3d const p1 = q.block(std::size_t{3u} * vi, 0, 3, 1);
    Eigen::Vector3d const p2 = q.block(std::size_t{3u} * vj, 0, 3, 1);
    auto const N             = q.rows() / 3;

    Eigen::Vector3d const spring = p2 - p1;
    auto const length            = spring.norm();
    Eigen::Vector3d const n      = spring / length;
    auto const delta             = scalar_type{0.5} * (length - d_);

    // find the position p1 which results in ||p2 - p1|| = rest length
    Eigen::Vector3d const pi1 = p1 + delta * n;
    Eigen::Vector3d const pi2 = p2 - delta * n;

    constexpr scalar_type half{0.5};
    constexpr std::size_t three{3};
    // the product wi * (Ai*Si)^T * (Ai*Si) only yields non-zero
    // entries at coordinates [3vi, 3vi+3[ and [3vj, 3vj+3[.
    // The matrices Ai,Bi are differential coordinate matrices
    // which result in mean subtraction in every dimension.
    // Thus, we subtract the mean in every dimension directly
    // instead of performing the matrix multiplication.
    b(three * vi + 0) += wi() * half * (pi1.x() - pi2.x());
    b(three * vi + 1) += wi() * half * (pi1.y() - pi2.y());
    b(three * vi + 2) += wi() * half * (pi1.z() - pi2.z());

    b(three * vj + 0) += wi() * half * (pi2.x() - pi1.x());
    b(three * vj + 1) += wi() * half * (pi2.y() - pi1.y());
    b(three * vj + 2) += wi() * half * (pi2.z() - pi1.z());
}

std::vector<Eigen::Triplet<edge_length_constraint_t::scalar_type>>
edge_length_constraint_t::get_wi_SiT_AiT_Ai_Si(positions_type const& p, masses_type const& M) const
{
    int const vi = static_cast<int>(indices().at(0));
    int const vj = static_cast<int>(indices().at(1));
    auto const N = p.rows();

    // We precompute the product (Ai*Si)^T * (Ai*Si) and find that
    // there are only nonzero elements in the blocks
    // [3vi:3vi+3, 3vi:3vi+3], [3vj:3vj+3, 3vi:3vi+3],
    // [3vj:3vj+3, 3vi:3vi+3], [3vj:3vj+3, 3vj:3vj+3]
    // Those blocks contain the differential coordinates of
    // the mean subtraction differential coordinates Ai.
    // We then multiply by wi as in wi * (Ai*Si)^T * (Ai*Si)
    constexpr scalar_type half{0.5};
    constexpr int three{3};
    std::array<Eigen::Triplet<scalar_type>, 12u> triplets;
    triplets[0] = {three * vi + 0, three * vi + 0, wi() * half};
    triplets[2] = {three * vi + 1, three * vi + 1, wi() * half};
    triplets[4] = {three * vi + 2, three * vi + 2, wi() * half};

    triplets[1] = {three * vj + 0, three * vi + 0, -wi() * half};
    triplets[3] = {three * vj + 1, three * vi + 1, -wi() * half};
    triplets[5] = {three * vj + 2, three * vi + 2, -wi() * half};

    triplets[6]  = {three * vi + 0, three * vj + 0, -wi() * half};
    triplets[8]  = {three * vi + 1, three * vj + 1, -wi() * half};
    triplets[10] = {three * vi + 2, three * vj + 2, -wi() * half};

    triplets[7]  = {three * vj + 0, three * vj + 0, wi() * half};
    triplets[9]  = {three * vj + 1, three * vj + 1, wi() * half};
    triplets[11] = {three * vj + 2, three * vj + 2, wi() * half};

    return std::vector<Eigen::Triplet<scalar_type>>{triplets.begin(), triplets.end()};
}

} // namespace pd


#include <iostream>
#include <fstream>
#include <Eigen/Eigen>
//include the bie header files
#include "material.hh"
#include "precomputed_kernel.hh"
#include "bimat_interface.hh"
#include "infinite_boundary.hh"
//include the fem header files
#include "mesh_Generated.hpp"
#include "bcdof.hpp"
#include "cal_ke.hpp"
#include "cal_M.hpp"
#include "cal_M_global_vec.hpp"
#include "cal_fe_global_const_ke.hpp"
#include "mapglobal.hpp"
#include "Slip_Weakening.hpp"
#include "cal_slip_sliprate.hpp"
#include "time_advance.hpp"
#include "BIE_correct.hpp"

using namespace Eigen;
using namespace std;

int main() {
    // Domain Size
    double x_min = -5e3;
    double x_max = 5e3;
    double y_min = -1.0e3;
    double y_max = 1.0e3;
    int dim = 2.0;
    double dx = 100;
    double dy = 100;
    int nx = (x_max-x_min)/dx;
    int ny = (y_max-y_min)/dy;
    MatrixXd Node = MatrixXd::Zero((nx+1)*(ny+1),2);
    MatrixXd Element = MatrixXd::Zero(nx*ny,4);
    VectorXd fault_surf_nodes = VectorXd::Zero((nx+1),1);
    VectorXd fault_surf_nodes_new = VectorXd::Zero((nx+1),1);
    VectorXd BIE_top_surf_nodes = VectorXd::Zero((nx+1),1);
    VectorXd BIE_bot_surf_nodes = VectorXd::Zero((nx+1),1);
    // Mesh
    mesh_Generated(x_min,x_max,y_min,y_max,dx,dy,nx,ny,Node, Element, fault_surf_nodes, fault_surf_nodes_new, BIE_top_surf_nodes, BIE_bot_surf_nodes);
    int n_nodes = Node.rows();
    int n_el = Element.rows();
    int Ndofn = 2;
    int Nnel = Element.cols();
    nx = fault_surf_nodes.size()-1;
    // Material
    double density = 2670.0;
    double v_s =3.464e3;
    double v_p = 6.0e3;
    double G= pow(v_s,2)*density;
    double Lambda = pow(v_p,2)*density-2.0*G;
    double E  = G*(3.0*Lambda+2.0*G)/(Lambda+G);
    double nu = Lambda/(2.0*(Lambda+G));
    // Time
    double alpha = 0.4;
    double dt = alpha*dx/v_p;
    // Reyleigh Damping
    double beta =0.1;
    double q = beta*dt;
    double time_run = 6.0;
    int numt = time_run/dt;
    //numt = 3;
    VectorXd time = dt*VectorXd::LinSpaced(numt,1,numt);
    // Slip weakening friction parameters
    double Dc = 0.2;
    double mu_d= 0.5;
    double mu_s = 0.6;
    // Intialization
    // disp velocity current and next time step (new)
    VectorXd u_n = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd v_n = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd u_new = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd v_new = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd a_n = VectorXd::Zero(n_nodes*Ndofn,1);
    // slip and slip-rate
    VectorXd delt_u_n = VectorXd::Zero(Ndofn*(nx+1),1);
    VectorXd delt_v_n = VectorXd::Zero(Ndofn*(nx+1),1);
    // Stress on the fault T_0 = intial stress , T = sticking force, T_c= stress critical goes into F_global
    VectorXd T_0 = VectorXd::Zero(Ndofn*(nx+1),1);
    VectorXd T = VectorXd::Zero(Ndofn*(nx+1),1);
    VectorXd T_c = VectorXd::Zero(Ndofn*(nx+1),1);
    VectorXd tau_s = VectorXd::Zero(nx+1,1);
    VectorXd F_ext_global = VectorXd::Zero(n_nodes*Ndofn,1);
    // Setting intial stress on the fault
    for (int i=0; i<T_0.size()/2; i++)
    {
        T_0(2*i+1) = -50.0e6;
    }
    VectorXd x = VectorXd::LinSpaced(nx+1,x_min,x_max);
    for (int i=0 ; i<nx+1; i++)
    {
        if ((x(i)<=(x_max+x_min)/2+0.8e3)&&(x(i)>=(x_max+x_min)/2-0.8e3))
        {
            T_0(2*i) = 31.0e6;
        }
        else
        {
            T_0(2*i) = 27.5e6;

        }
    }
    // Get the index degree of freedome for each element
    VectorXd index_el = VectorXd::Zero(Ndofn*Nnel,1);
    MatrixXd index_store = MatrixXd::Zero(Nnel*Ndofn,n_el);
    for (int i=0;i<n_el;i++)
    {
        bcdof(Element.row(i),dim,index_el);
        index_store.col(i) = index_el;
    }
    VectorXd top_surf_index = VectorXd::Zero(Ndofn*(nx+1),1);
    VectorXd bot_surf_index = VectorXd::Zero(Ndofn*(nx+1),1);
    VectorXd BIE_top_surf_index = VectorXd::Zero(Ndofn*(nx+1),1);
    VectorXd BIE_bot_surf_index = VectorXd::Zero(Ndofn*(nx+1),1);
    bcdof(fault_surf_nodes,dim,top_surf_index);
    bcdof(fault_surf_nodes_new,dim,bot_surf_index);
    bcdof(BIE_top_surf_nodes,dim,BIE_top_surf_index);
    bcdof(BIE_bot_surf_nodes,dim,BIE_bot_surf_index);
    // Calculating the Global Mass Vector (lumped mass)
    // Element mass
    double M=density*dx*dy*1.0;
    VectorXd M_el_vec = M/4*VectorXd::Ones(Nnel*Ndofn,1);
    VectorXd M_global_vec=VectorXd::Zero(n_nodes*Ndofn,1);
    cal_M_global_vec(Node, Element, density, index_store, Ndofn, M_global_vec);
//    for (int i=0 ; i<n_el;i++)
//    {
//        index_el = index_store.col(i);
//        mapglobal(index_el,M_global_vec,M_el_vec);
//    }
    // Element matrix
    MatrixXd ke = MatrixXd::Zero(8,8);
    MatrixXd coord = MatrixXd::Zero(4,2);
    VectorXd Element_0= Element.row(0);
    coord.row(0) = Node.row(Element_0(0));
    coord.row(1) = Node.row(Element_0(1));
    coord.row(2) = Node.row(Element_0(2));
    coord.row(3) = Node.row(Element_0(3));
    cal_ke (coord,E,nu,ke);
    // BIE part initiation
    // Setting up the material property for the BIE code
    Material BIE_top_mat = Material(E,nu,density);
    Material BIE_bot_mat = Material(E,nu,density);
    double length = x_max-x_min;
    // infinte bc BIE call infinite_boundary.cc
    PrecomputedKernel h11("kernels/nu_.25_h11.dat");
    PrecomputedKernel h12("kernels/nu_.25_k12.dat");
    PrecomputedKernel h22("kernels/nu_.25_h22.dat");
    InfiniteBoundary BIE_inf_top(length,nx+1,1.0,&BIE_top_mat,&h11,&h12,&h22);
    InfiniteBoundary BIE_inf_bot(length,nx+1,-1.0,&BIE_bot_mat,&h11,&h12,&h22);
    // BIE setting time step
    BIE_inf_top.setTimeStep(dt);
    BIE_inf_bot.setTimeStep(dt);
    // BIE initialization
    BIE_inf_top.init();
    BIE_inf_bot.init();

    //BIE_initiation(E, nu, density, x_max, x_min, nx, dt);
    printf("ready to start\n");
    // Output
    ofstream slip("slip.bin",ios::binary);
    slip.close();
    ofstream slip_rate("slip_rate.bin",ios::binary);
    slip_rate.close();
    ofstream shear("shear.bin",ios::binary);
    shear.close();
    std::ofstream fe("results/fe.txt");

    // Main time loop
    for (int j=0;j<numt;j++)
    {
        // Compute the global internal force
        VectorXd fe_global= VectorXd::Zero(n_nodes*Ndofn,1);
        cal_fe_global_const_ke(n_nodes, n_el, index_store, q, u_n, v_n, Ndofn, ke, fe_global);
        // Friction subroutine
        VectorXd F_fault = VectorXd::Zero(Ndofn*(nx+1),1);
        Slip_Weakening(M_global_vec, top_surf_index, bot_surf_index, fe_global, dt, dx, dy, nx, delt_v_n, delt_u_n, T_0, tau_s, mu_s, mu_d, Dc, Ndofn, M, F_fault, T_c);
        // Calculate the global force vector
        // Adding contribution of the fault force to the global force vector F_total
        VectorXd F_total = F_ext_global-fe_global;
        mapglobal(top_surf_index,F_total,-F_fault);
        mapglobal(bot_surf_index,F_total,F_fault);
        // Central Difference Time integration
        time_advance(u_n, v_n, F_total, M_global_vec, dt);
        // Get the slip and slip rate
        cal_slip_slip_rate(u_n, v_n, top_surf_index, bot_surf_index, Ndofn, nx, delt_u_n, delt_v_n);
        // Correct the BIE surf nodes solutions from FEM with the BIE solution
        BIE_correct(BIE_top_surf_index, BIE_bot_surf_index, fe_global, Ndofn, nx, dx, BIE_inf_top, BIE_inf_bot, u_n, v_n);
        //ofstream file;
        slip.open("slip.bin",ios::binary | ios::app);
        slip.write((char*)(delt_u_n.data()),delt_u_n.size()*sizeof(double));
        slip.close();
        slip_rate.open("slip_rate.bin",ios::binary | ios::app);
        slip_rate.write((char*)(delt_v_n.data()),delt_v_n.size()*sizeof(double));
        slip_rate.close();
        shear.open("shear.bin",ios::binary | ios::app);
        shear.write((char*)(T_c.data()),T_c.size()*sizeof(double));
        shear.close();
        printf("Simulation time = %f\n",time(j));
    }
    return 0;
}


// Copyright (c) 2020 Chris Richardson
// FEniCS Project
// SPDX-License-Identifier:    MIT

#include "crouzeix-raviart.h"
#include "core/element-families.h"
#include "core/polyset.h"
#include "core/quadrature.h"
#include <Eigen/Dense>
#include <numeric>
#include <vector>

using namespace basix;

//-----------------------------------------------------------------------------
FiniteElement basix::create_cr(cell::type celltype, int degree)
{
  if (degree != 1)
    throw std::runtime_error("Degree must be 1 for Crouzeix-Raviart");

  const int tdim = cell::topological_dimension(celltype);
  if (tdim < 2)
    throw std::runtime_error("Tdim must be 2 or 3 for Crouzeix-Raviart");

  const std::vector<std::vector<std::vector<int>>> topology
      = cell::topology(celltype);
  const std::vector<std::vector<int>> facet_topology = topology[tdim - 1];
  const Eigen::ArrayXXd geometry = cell::geometry(celltype);

  const int ndofs = facet_topology.size();
  Eigen::ArrayXXd pts = Eigen::ArrayXXd::Zero(ndofs, tdim);

  // Compute facet midpoints
  int c = 0;
  for (const std::vector<int>& f : facet_topology)
  {
    for (int i : f)
      pts.row(c) += geometry.row(i);
    pts.row(c) /= static_cast<double>(f.size());
    ++c;
  }

  Eigen::MatrixXd dual = polyset::tabulate(celltype, 1, 0, pts)[0];
  int perm_count = tdim == 2 ? 3 : 14;
  std::vector<Eigen::MatrixXd> base_permutations(
      perm_count, Eigen::MatrixXd::Identity(ndofs, ndofs));

  const Eigen::MatrixXd coeffs = compute_expansion_coefficients(
      Eigen::MatrixXd::Identity(ndofs, ndofs), dual);

  // Crouzeix-Raviart has one dof on each entity of tdim-1.
  std::vector<std::vector<int>> entity_dofs(topology.size());
  entity_dofs[0].resize(topology[0].size(), 0);
  entity_dofs[1].resize(topology[1].size(), (tdim == 2) ? 1 : 0);
  entity_dofs[2].resize(topology[2].size(), (tdim == 3) ? 1 : 0);
  if (tdim == 3)
    entity_dofs[3] = {0};

  return FiniteElement(element::family::CR, celltype, 1, {1}, coeffs,
                       entity_dofs, base_permutations, {});
}
//-----------------------------------------------------------------------------


#pragma once
#include <boost/numeric/ublas/matrix.hpp>
#include <random>

// fills matrix with pseudorandomly generated elements
template<class Matrix, class Generator>
void randomize_matrix(Generator&& r, Matrix& m) {
  std::uniform_int_distribution<typename Matrix::value_type> dist;
  for(size_t i = 0; i < m.size1(); i++) {
    for(size_t j = 0; j < m.size2(); j++) {
      m(i, j) = dist(r);
    }
  }
}


#include "mtf/SSM/SL3.h"
#include "mtf/SSM/HomographyEstimator.h"
#include "mtf/Utilities/warpUtils.h"
#include "mtf/Utilities/miscUtils.h"
#include "mtf/Utilities/excpUtils.h"

#include <unsupported/Eigen/MatrixFunctions>
#include "opencv2/calib3d/calib3d.hpp"
#include <boost/random/random_device.hpp>
#include <boost/random/seed_seq.hpp>

#define VALIDATE_SL3_WARP(warp) \
	assert(warp.determinant() == 1.0);

#define SL3_NORMALIZED_BASIS 0
#define SL3_ITERATIVE_SAMPLE_MEAN 1
#define SL3_SAMPLE_MEAN_MAX_ITERS 10
#define SL3_SAMPLE_MEAN_EPS 1e-4
#define SL3_DEBUG_MODE 0

#ifndef SL3_MAX_VALID_VAL
#define SL3_MAX_VALID_VAL 1e10
#endif
#define is_unbounded(eig_mat) (eig_mat.array().cwiseAbs().eval() > SL3_MAX_VALID_VAL).any()

_MTF_BEGIN_NAMESPACE

SL3Params::SL3Params(const SSMParams *ssm_params,
bool _normalized_init, bool _iterative_sample_mean,
int _sample_mean_max_iters, double _sample_mean_eps,
bool _debug_mode) :
SSMParams(ssm_params),
normalized_init(_normalized_init),
iterative_sample_mean(_iterative_sample_mean),
sample_mean_max_iters(_sample_mean_max_iters),
sample_mean_eps(_sample_mean_eps),
debug_mode(_debug_mode){}

//! copy/default constructor
SL3Params::SL3Params(const SL3Params *params) :
SSMParams(params),
normalized_init(SL3_NORMALIZED_BASIS),
iterative_sample_mean(SL3_ITERATIVE_SAMPLE_MEAN),
sample_mean_max_iters(SL3_SAMPLE_MEAN_MAX_ITERS),
sample_mean_eps(SL3_SAMPLE_MEAN_EPS),
debug_mode(SL3_DEBUG_MODE){
	if(params){
		normalized_init = params->normalized_init;
		iterative_sample_mean = params->iterative_sample_mean;
		sample_mean_max_iters = params->sample_mean_max_iters;
		sample_mean_eps = params->sample_mean_eps;
		debug_mode = params->debug_mode;
	}
}

SL3::SL3(
	const ParamType *_params) :
	ProjectiveBase(_params), params(_params){

	printf("\n");
	printf("Using SL3 SSM with:\n");
	printf("resx: %d\n", resx);
	printf("resy: %d\n", resy);
	printf("normalized_init: %d\n", params.normalized_init);
	printf("iterative_sample_mean: %d\n", params.iterative_sample_mean);
	printf("sample_mean_max_iters: %d\n", params.sample_mean_max_iters);
	printf("sample_mean_eps: %f\n", params.sample_mean_eps);
	printf("debug_mode: %d\n", params.debug_mode);

	name = "sl3";
	state_size = 8;
	curr_state.resize(state_size);

	log_fname = log_fname;

	lie_alg_mat = Matrix3d::Zero();
	warp_mat = Matrix3d::Identity();

	lieAlgBasis[0] <<
		1, 0, 0,
		0, -1, 0,
		0, 0, 0;
	lieAlgBasis[1] <<
		0, 0, 0,
		0, -1, 0,
		0, 0, 1;
	lieAlgBasis[2] <<
		0, -1, 0,
		1, 0, 0,
		0, 0, 0;
	lieAlgBasis[3] <<
		0, 1, 0,
		1, 0, 0,
		0, 0, 0;
	lieAlgBasis[4] <<
		0, 0, 1,
		0, 0, 0,
		0, 0, 0;
	lieAlgBasis[5] <<
		0, 0, 0,
		0, 0, 1,
		0, 0, 0;
	lieAlgBasis[6] <<
		0, 0, 0,
		0, 0, 0,
		1, 0, 0;
	lieAlgBasis[7] <<
		0, 0, 0,
		0, 0, 0,
		0, 1, 0;

	utils::getNormUnitSquarePts(norm_pts, norm_corners, resx, resy,
		-static_cast<double>(resx) / 2.0, -static_cast<double>(resy) / 2.0,
		static_cast<double>(resx) / 2.0, static_cast<double>(resy) / 2.0);
	utils::homogenize(norm_corners, norm_corners_hm);
	utils::homogenize(norm_pts, norm_pts_hm);

	init_corners = norm_corners;
	init_corners_hm = norm_corners_hm;
	init_pts = norm_pts;
	init_pts_hm = norm_pts_hm;

	if(params.debug_mode){
#ifdef _WIN32
		FILE *fid;
		errno_t err;
		if((err = fopen_s(&fid, log_fname, "r")) != 0) {
			throw utils::InvalidArgument(cv_format("SL3 :: Log file %s could not be opened successfully : %s\n",
				log_fname, strerror(err)));
		}
#else
		FILE *fid = fopen(log_fname, "w");
		if(!fid){
			throw utils::InvalidArgument(cv_format("SL3 :: Log file %s could not be opened successfully\n", log_fname));
		}
#endif
		fclose(fid);
	}
}

void SL3::setState(const VectorXd &ssm_state){
	validate_ssm_state(ssm_state);
	//utils::printMatrix(ssm_state.transpose(), " SL3::setState :: ssm_state");
	if(!ssm_state.allFinite()){
		utils::printMatrix(ssm_state.transpose(), "ssm_state");
		throw mtf::utils::InvalidTrackerState("SL3::setState::Invalid state provided");
	}
	curr_state = ssm_state;
	getWarpFromState(curr_warp, curr_state);
	curr_pts_hm.noalias() = curr_warp * init_pts_hm;
	curr_corners_hm.noalias() = curr_warp * init_corners_hm;
	utils::dehomogenize(curr_pts_hm, curr_pts);
	utils::dehomogenize(curr_corners_hm, curr_corners);
	if(params.debug_mode){
		utils::printMatrixToFile(curr_warp, "setState::curr_warp", log_fname);
		utils::printMatrixToFile(curr_corners, "setState::curr_corners", log_fname);
	}
}

void SL3::setCorners(const CornersT& corners){
	if(!corners.allFinite() || is_unbounded(corners)){
		utils::printMatrix(corners, "corners");
		throw mtf::utils::InvalidTrackerState("SL3::setCorners::Invalid corners provided");
	}
	curr_corners = corners;
	curr_warp = utils::computeHomographyDLT(norm_corners, curr_corners);
	double warp_det = curr_warp.determinant();
	if(!warp_mat.allFinite() || is_unbounded(warp_mat) || warp_det == 0 || std::isnan(warp_det) || std::isinf(warp_det)){
		printf("SL3::Cannot set SSM to the provided corners as the corresponding warp matrix is invalid: \n");
		utils::printMatrix(corners, "corners");
		utils::printMatrix(curr_warp, "warp");
		utils::printScalar(warp_det, "warp_det");
		throw mtf::utils::InvalidTrackerState("SL3::setCorners :: Cannot set SSM to the provided points as the corresponding warp matrix is invalid");
	}
	utils::homogenize(curr_corners, curr_corners_hm);
	curr_pts_hm = curr_warp * norm_pts_hm;
	utils::dehomogenize(curr_pts_hm, curr_pts);
	curr_warp = curr_warp / cbrt(warp_det);
	if(params.normalized_init){
		getStateFromWarp(curr_state, curr_warp);
	} else{
		init_corners = curr_corners;
		init_corners_hm = curr_corners_hm;
		init_pts = curr_pts;
		init_pts_hm = curr_pts_hm;
		curr_warp = Matrix3d::Identity();
		curr_state.fill(0);
	}
	if(params.debug_mode){
		utils::printMatrixToFile(init_pts, "setCorners::init_pts", log_fname);
		utils::printMatrixToFile(init_corners, "setCorners::init_corners", log_fname);
		utils::printMatrixToFile(curr_warp, "setCorners::curr_warp", log_fname);
	}
}

void SL3::compositionalUpdate(const VectorXd& state_update){
	validate_ssm_state(state_update);

	getWarpFromState(warp_update_mat, state_update);
	curr_warp = curr_warp * warp_update_mat;
	if(!curr_warp.allFinite() || is_unbounded(curr_warp)){
		utils::printMatrix(state_update.transpose(), "state_update");
		utils::printMatrix(curr_warp, "curr_warp");
		utils::printMatrix(warp_update_mat, "warp_update_mat");
		throw mtf::utils::InvalidTrackerState("SL3::compositionalUpdate::Invalid state update provided");

	}
	curr_pts_hm.noalias() = curr_warp * init_pts_hm;
	curr_corners_hm.noalias() = curr_warp * init_corners_hm;

	utils::dehomogenize(curr_pts_hm, curr_pts);
	utils::dehomogenize(curr_corners_hm, curr_corners);

	getStateFromWarp(curr_state, curr_warp);

}

void SL3::getWarpFromState(Matrix3d &warp_mat,
	const VectorXd& ssm_state){
	validate_ssm_state(ssm_state);
	assert(ssm_state.size() == 8);
	if(!ssm_state.allFinite() || is_unbounded(ssm_state)){
		utils::printMatrix(ssm_state.transpose(), "ssm_state");
		throw mtf::utils::InvalidTrackerState("SL3::getWarpFromState::Invalid state provided");
	}
	getLieAlgMatFromState(lie_alg_mat, ssm_state);
	if(!lie_alg_mat.allFinite() || is_unbounded(lie_alg_mat)){
		utils::printMatrix(ssm_state.transpose(), "ssm_state");
		utils::printMatrix(lie_alg_mat, "lie_alg_mat");
		throw mtf::utils::InvalidTrackerState("SL3::getWarpFromState::Invalid sl3 matrix corresponds to the given state");
	}
	warp_mat = lie_alg_mat.exp();
}

void SL3::getStateFromWarp(VectorXd &state_vec,
	const Matrix3d& warp_mat){
	validate_ssm_state(state_vec);
	double warp_det = warp_mat.determinant();
	if(!warp_mat.allFinite() || is_unbounded(warp_mat) || warp_det == 0 || std::isnan(warp_det) || std::isinf(warp_det)){
		utils::printMatrix(warp_mat, "warp_mat");
		utils::printScalar(warp_det, "warp_det");
		throw mtf::utils::InvalidTrackerState("SL3::getStateFromWarp :: Invalid warp matrix provided");
	}
	Matrix3d norm_warp_mat = warp_mat / cbrt(warp_det);
	if(!norm_warp_mat.allFinite() || is_unbounded(norm_warp_mat)){
		utils::printMatrix(warp_mat, "warp_mat");
		utils::printScalar(warp_det, "warp_det");
		utils::printMatrix(norm_warp_mat, "norm_warp_mat");
		throw mtf::utils::InvalidTrackerState("SL3::getStateFromWarp :: Invalid normalized warp matrix found");
	}
	lie_alg_mat = norm_warp_mat.log();
	getStateFromLieAlgMat(state_vec, lie_alg_mat);
}

void SL3::getLieAlgMatFromState(Matrix3d& lie_alg_mat,
	const VectorXd& ssm_state){
	validate_ssm_state(ssm_state);
	assert(ssm_state.size() == state_size);
	lie_alg_mat(0, 0) = ssm_state(0);
	lie_alg_mat(0, 1) = ssm_state(3) - ssm_state(2);
	lie_alg_mat(0, 2) = ssm_state(4);
	lie_alg_mat(1, 0) = ssm_state(3) + ssm_state(2);
	lie_alg_mat(1, 1) = -ssm_state(1) - ssm_state(0);
	lie_alg_mat(1, 2) = ssm_state(5);
	lie_alg_mat(2, 0) = ssm_state(6);
	lie_alg_mat(2, 1) = ssm_state(7);
	lie_alg_mat(2, 2) = ssm_state(1);
}

void SL3::getStateFromLieAlgMat(VectorXd &ssm_state,
	const Matrix3d& lie_alg_mat){
	validate_ssm_state(ssm_state);
	ssm_state(0) = lie_alg_mat(0, 0);
	ssm_state(1) = -lie_alg_mat(1, 1) - ssm_state(0);
	ssm_state(2) = (lie_alg_mat(1, 0) - lie_alg_mat(0, 1)) / 2.0;
	ssm_state(3) = (lie_alg_mat(1, 0) + lie_alg_mat(0, 1)) / 2.0;
	ssm_state(4) = lie_alg_mat(0, 2);
	ssm_state(5) = lie_alg_mat(1, 2);
	ssm_state(6) = lie_alg_mat(2, 0);
	ssm_state(7) = lie_alg_mat(2, 1);
	if(params.debug_mode){
		utils::printMatrixToFile(ssm_state.transpose(), "getStateFromLieAlgMat :: ssm_state", log_fname);
		utils::printMatrixToFile(lie_alg_mat, "getStateFromLieAlgMat :: lie_alg_mat", log_fname);
	}
}

void SL3::initializeSampler(const VectorXd &state_sigma,
	const VectorXd &state_mean){
	if(state_sigma.size() != 8){
		throw utils::InvalidArgument(
			cv::format("SL3::initializeSampler :: SSM sigma has invalid size: %d\n",
			state_sigma.size()));
	}
	covariance_mat = state_sigma.asDiagonal();
	printf("Using SL3 sampler with sigma:\n");
	utils::printMatrix(state_sigma.transpose(), nullptr, "%e");

	state_perturbation.resize(state_size);
	rand_gen.resize(1);
	rand_dist.resize(1);

	boost::random_device r;
	boost::random::seed_seq seed{ r(), r(), r(), r(), r(), r(), r(), r() };
	rand_gen[0] = SampleGenT(seed);
	rand_dist[0] = SampleDistT(0, 1);

	if(params.debug_mode){
		utils::printMatrixToFile(covariance_mat, "covariance_mat", log_fname);
	}
	is_initialized.sampler = true;
}

void SL3::setSampler(const VectorXd &state_sigma,
	const VectorXd &state_mean){
	assert(state_sigma.size() == state_size);
	covariance_mat = state_sigma.asDiagonal();
}

VectorXd SL3::getSamplerSigma(){
	VectorXd sampler_sigma = covariance_mat.diagonal();
	return sampler_sigma;
}

void SL3::generatePerturbation(VectorXd &perturbation){
	assert(perturbation.size() == state_size);
	Vector8d rand_vec;
	for(unsigned int state_id = 0; state_id < 8; state_id++){
		rand_vec(state_id) = rand_dist[0](rand_gen[0]);
	}
	perturbation = covariance_mat*rand_vec;
}

// use Random Walk model to generate perturbed sample
void SL3::compositionalRandomWalk(VectorXd &perturbed_state,
	const VectorXd &base_state){
	generatePerturbation(state_perturbation);
	perturbed_state = base_state + state_perturbation;
}
// use first order Auto Regressive model to generate perturbed sample
//void SL3::compositionalAutoRegression1(VectorXd &perturbed_state, VectorXd &perturbed_ar,
//	const VectorXd &base_state, const VectorXd &base_ar, double a){
//	ProjWarpT warp_perturbation, sl3_perturbation, sl3_base_ar, base_warp;
//	generatePerturbation(state_perturbation);
//	getLieAlgMatFromState(warp_perturbation, state_perturbation);
//	getLieAlgMatFromState(sl3_base_ar, base_ar);
//	getWarpFromState(base_warp, base_state);
//	sl3_perturbation = warp_perturbation + a*sl3_base_ar;
//	ProjWarpT SL3_perturbation = sl3_perturbation.exp();
//	ProjWarpT perturbed_warp = base_warp*SL3_perturbation;
//	ProjWarpT base_warp_inv = base_warp.inverse();
//	if(params.debug_mode){
//		utils::printMatrixToFile(state_perturbation.transpose(), "rand_perturbation", log_fname);
//		utils::printMatrixToFile(warp_perturbation.transpose(), "warp_perturbation", log_fname);
//		utils::printMatrixToFile(sl3_perturbation, "sl3_perturbation", log_fname);
//		utils::printMatrixToFile(base_ar.transpose(), "base_ar", log_fname);
//		utils::printMatrixToFile(sl3_base_ar, "sl3_base_ar", log_fname);
//		utils::printMatrixToFile(SL3_perturbation, "SL3_perturbation", log_fname);
//		utils::printMatrixToFile(base_warp, "base_warp", log_fname);
//		utils::printMatrixToFile(base_warp_inv, "base_warp_inv", log_fname);
//		utils::printMatrixToFile(perturbed_warp, "perturbed_warp", log_fname);
//		utils::printMatrixToFile(perturbed_state.transpose(), "perturbed_state", log_fname);
//		utils::printMatrixToFile(perturbed_ar.transpose(), "perturbed_ar", log_fname);
//	}
//	ProjWarpT SL3_perturbed_ar = a*(base_warp_inv*perturbed_warp);
//	ProjWarpT sl3_perturbed_ar = SL3_perturbed_ar.log();
//	if(params.debug_mode){
//		utils::printMatrixToFile(sl3_perturbed_ar, "sl3_perturbed_ar", log_fname);
//	}
//	getStateFromWarp(perturbed_state, perturbed_warp);
//	getStateFromLieAlgMat(perturbed_ar, sl3_perturbed_ar);
//
//}
void SL3::compositionalAutoRegression1(VectorXd &perturbed_state, VectorXd &perturbed_ar,
	const VectorXd &base_state, const VectorXd &base_ar, double a){
	ProjWarpT sl3_perturbation, lie_alg_base_ar, base_warp;
	generatePerturbation(state_perturbation);
	getLieAlgMatFromState(sl3_perturbation, state_perturbation);
	getLieAlgMatFromState(lie_alg_base_ar, base_ar);
	getWarpFromState(base_warp, base_state);
	ProjWarpT SL3_perturbation = (a*lie_alg_base_ar + sl3_perturbation).exp();
	ProjWarpT perturbed_warp = base_warp*SL3_perturbation;
	ProjWarpT base_warp_inv = base_warp.inverse();
	ProjWarpT lie_alg_perturbed_ar = a*(base_warp_inv*perturbed_warp).log();
	getStateFromWarp(perturbed_state, perturbed_warp);
	getStateFromLieAlgMat(perturbed_ar, lie_alg_perturbed_ar);
	if(params.debug_mode){
		utils::printMatrixToFile(state_perturbation.transpose(), "state_perturbation", log_fname);
		utils::printMatrixToFile(sl3_perturbation, "sl3_perturbation", log_fname);
		utils::printMatrixToFile(base_ar.transpose(), "base_ar", log_fname);
		utils::printMatrixToFile(lie_alg_base_ar, "lie_alg_base_ar", log_fname);
		utils::printMatrixToFile(SL3_perturbation, "SL3_perturbation", log_fname);
		utils::printMatrixToFile(base_warp, "base_warp", log_fname);
		utils::printMatrixToFile(perturbed_warp, "perturbed_warp", log_fname);
		utils::printMatrixToFile(lie_alg_perturbed_ar, "lie_alg_perturbed_ar", log_fname);
	}
}

void SL3::getInitPixGrad(Matrix2Xd &ssm_grad, int pt_id) {
	double x = init_pts(0, pt_id);
	double y = init_pts(1, pt_id);

	ssm_grad <<
		x, -x, -y, y, 1, 0, -x*x, -x*y,
		-x, -2*y, x, x, 0, 1, -y*x, -y*y;
}

void SL3::getCurrPixGrad(Matrix2Xd &ssm_grad, int pt_id) {
	double x = init_pts(0, pt_id);
	double y = init_pts(1, pt_id);

	double curr_x = curr_pts(0, pt_id);
	double curr_y = curr_pts(1, pt_id);

	double xx = x*curr_x, xy = x*curr_y, yy = y*curr_y, yx = y*curr_x;

	Matrix3d curr_warp;
	getWarpFromState(curr_warp, curr_state);
	double a1 = curr_warp(0, 0), a2 = curr_warp(0, 1), a3 = curr_warp(0, 2);
	double a4 = curr_warp(1, 0), a5 = curr_warp(1, 1), a6 = curr_warp(1, 2);
	double a7 = curr_warp(2, 0), a8 = curr_warp(2, 1), a9 = curr_warp(2, 2);

	double p11 = a1*x - a2*y - a7*xx + a8*yx;
	double p12 = a1*y - a7*yx;
	double p13 = a1 - a7*curr_x;
	double p14 = a2*x - a8*xx;
	double p15 = a2*y - a3 - a8*yx + a9*curr_x;
	double p16 = a2 - a8*curr_x;
	double p17 = a3*x - a9*xx;
	double p18 = a3*y - a9*yx;

	double p21 = a4*x - a5*y - a7*xy + a8*yy;
	double p22 = a4*y - a7*yy;
	double p23 = a4 - a7*curr_y;
	double p24 = a5*x - a8*xy;
	double p25 = a5*y - a6 - a8*yy + a9*curr_y;
	double p26 = a5 - a8*curr_y;
	double p27 = a6*x - a9*xy;
	double p28 = a6*y - a9*yy;

	ssm_grad <<
		p11, p12, p13, p14, p15, p16, p17, p18,
		p21, p22, p23, p24, p25, p26, p27, p28;

	double inv_d = 1.0 / curr_pts_hm(2, pt_id);
	ssm_grad *= inv_d;
}

void SL3::cmptInitPixJacobian(MatrixXd &dI_dp,
	const PixGradT &dI_dw){
	validate_ssm_jacobian(dI_dp, dI_dw);

	unsigned int ch_pt_id = 0;
	for(unsigned int pt_id = 0; pt_id < n_pts; ++pt_id){
		double x = init_pts(0, pt_id);
		double y = init_pts(1, pt_id);

		for(unsigned int ch_id = 0; ch_id < n_channels; ++ch_id){
			double Ix = dI_dw(ch_pt_id, 0);
			double Iy = dI_dw(ch_pt_id, 1);

			double Ixx = Ix * x;
			double Iyy = Iy * y;
			double Ixy = Ix * y;
			double Iyx = Iy * x;

			dI_dp(ch_pt_id, 0) = Ixx - Iyx;
			dI_dp(ch_pt_id, 1) = -(2 * Iyy + Ixx);
			dI_dp(ch_pt_id, 2) = Iyx - Ixy;
			dI_dp(ch_pt_id, 3) = Iyx + Ixy;
			dI_dp(ch_pt_id, 4) = Ix;
			dI_dp(ch_pt_id, 5) = Iy;
			dI_dp(ch_pt_id, 6) = -Ixx*x - Iyy*x;
			dI_dp(ch_pt_id, 7) = -Ixx*y - Iyy*y;

			++ch_pt_id;
		}
	}
}

void SL3::cmptWarpedPixJacobian(MatrixXd &dI_dp,
	const PixGradT &dI_dw) {
	validate_ssm_jacobian(dI_dp, dI_dw);

	double a00 = curr_warp(0, 0);
	double a01 = curr_warp(0, 1);
	double a10 = curr_warp(1, 0);
	double a11 = curr_warp(1, 1);
	double a20 = curr_warp(2, 0);
	double a21 = curr_warp(2, 1);

	unsigned int ch_pt_id = 0;
	for(unsigned int pt_id = 0; pt_id < n_pts; ++pt_id) {
		double w_x = curr_pts(0, pt_id);
		double w_y = curr_pts(1, pt_id);

		double D = curr_pts_hm(2, pt_id);
		double inv_det = 1.0 / D;

		double dwx_dx = (a00 - a20*w_x);
		double dwx_dy = (a01 - a21*w_x);
		double dwy_dx = (a10 - a20*w_y);
		double dwy_dy = (a11 - a21*w_y);

		double x = init_pts(0, pt_id);
		double y = init_pts(1, pt_id);

		//double Ix = pix_grad(pt_id, 0);
		//double Iy = pix_grad(pt_id, 1);
		for(unsigned int ch_id = 0; ch_id < n_channels; ++ch_id){
			double Ix = (dwx_dx*dI_dw(ch_pt_id, 0) + dwy_dx*dI_dw(ch_pt_id, 1))*inv_det;
			double Iy = (dwx_dy*dI_dw(ch_pt_id, 0) + dwy_dy*dI_dw(ch_pt_id, 1))*inv_det;

			double Ixx = Ix * x;
			double Ixy = Ix * y;
			double Iyy = Iy * y;
			double Iyx = Iy * x;

			dI_dp(ch_pt_id, 0) = Ixx - Iyx;
			dI_dp(ch_pt_id, 1) = -(2 * Iyy + Ixx);
			dI_dp(ch_pt_id, 2) = Iyx - Ixy;
			dI_dp(ch_pt_id, 3) = Iyx + Ixy;
			dI_dp(ch_pt_id, 4) = Ix;
			dI_dp(ch_pt_id, 5) = Iy;
			dI_dp(ch_pt_id, 6) = -Ixx*x - Iyy*x;
			dI_dp(ch_pt_id, 7) = -Ixx*y - Iyy*y;

			++ch_pt_id;
		}
	}
}

void SL3::cmptPixJacobian(MatrixXd &dI_dp,
	const PixGradT &pix_jacobian){
	validate_ssm_jacobian(dI_dp, pix_jacobian);

	for(unsigned int pt_id = 0; pt_id < n_pts; ++pt_id){
		double x = init_pts(0, pt_id);
		double y = init_pts(1, pt_id);
		double Ix = pix_jacobian(pt_id, 0);
		double Iy = pix_jacobian(pt_id, 1);

		double curr_x = curr_pts(0, pt_id);
		double curr_y = curr_pts(1, pt_id);
		double inv_d = 1.0 / curr_pts_hm(2, pt_id);

		double Ixx = Ix * x;
		double Iyy = Iy * y;
		double Ixy = Ix * y;
		double Iyx = Iy * x;

		dI_dp(pt_id, 0) = (Ixx - Iyy) * inv_d;
		dI_dp(pt_id, 1) = Ixy * inv_d;
		dI_dp(pt_id, 2) = Ix * inv_d;
		dI_dp(pt_id, 3) = Iyx * inv_d;
		dI_dp(pt_id, 4) = (Ix*curr_x + Iy*(y + curr_y)) * inv_d;
		dI_dp(pt_id, 5) = Iy * inv_d;
		dI_dp(pt_id, 6) = (-Ixx*curr_x - Iyx*curr_y) * inv_d;
		dI_dp(pt_id, 7) = (-Ixy*curr_x - Iyy*curr_y) * inv_d;
	}
	//dI_dp.array().colwise() /= curr_pts_hm.array().row(2).transpose();
}

void SL3::cmptApproxPixJacobian(MatrixXd &dI_dp,
	const PixGradT &pix_jacobian) {
	validate_ssm_jacobian(dI_dp, pix_jacobian);

	curr_warp /= curr_warp(2, 2);

	double h00_plus_1 = curr_warp(0, 0);
	double h01 = curr_warp(0, 1);
	double h10 = curr_warp(1, 0);
	double h11_plus_1 = curr_warp(1, 1);
	double h20 = curr_warp(2, 0);
	double h21 = curr_warp(2, 1);

	for(unsigned int pt_id = 0; pt_id < n_pts; ++pt_id){

		double Nx = curr_pts_hm(0, pt_id);
		double Ny = curr_pts_hm(1, pt_id);
		double D = curr_pts_hm(2, pt_id);
		double D_sqr_inv = 1.0 / (D*D);

		double a = (h00_plus_1*D - h21*Nx) * D_sqr_inv;
		double b = (h01*D - h21*Nx) * D_sqr_inv;
		double c = (h10*D - h20*Ny) * D_sqr_inv;
		double d = (h11_plus_1*D - h21*Ny) * D_sqr_inv;
		double inv_det = 1.0 / ((a*d - b*c)*D);

		double x = init_pts(0, pt_id);
		double y = init_pts(1, pt_id);

		double curr_x = curr_pts(0, pt_id);
		double curr_y = curr_pts(1, pt_id);

		double Ix = pix_jacobian(pt_id, 0);
		double Iy = pix_jacobian(pt_id, 1);

		double Ixx = Ix * x;
		double Ixy = Ix * y;
		double Iyy = Iy * y;
		double Iyx = Iy * x;

		double factor1 = b*curr_y - d*curr_x;
		double factor2 = c*curr_x - a*curr_y;

		dI_dp(pt_id, 0) = (Ixx*d + Ixy*b - Iyx*c - Iyy*a) * inv_det;
		dI_dp(pt_id, 1) = (Ixy*d - Iyy*c) * inv_det;
		dI_dp(pt_id, 2) = (Ix*d - Iy*c) * inv_det;
		dI_dp(pt_id, 3) = (Iyx*a - Ixx*b) * inv_det;
		dI_dp(pt_id, 4) = (Iyy*a - Ix*factor1 - Ixy*b - Iy*factor2) * inv_det;
		dI_dp(pt_id, 5) = (Iy*a - Ix*b) * inv_det;
		dI_dp(pt_id, 6) = (Ixx*factor1 + Iyx*factor2) * inv_det;
		dI_dp(pt_id, 7) = (Ixy*factor1 + Iyy*factor2) * inv_det;

		//dI_dp(i, 0) = (Ix*(d*x + b*y) - Iy*(c*x + a*y)) * inv_det;
		//dI_dp(i, 1) = (Ix*d*y - Iy*c*y) * inv_det;
		//dI_dp(i, 2) = (Ix*d - Iy*c) * inv_det;
		//dI_dp(i, 3) = (Iy*a*x - Ix*b*x) * inv_det;
		//dI_dp(i, 4) = (Ix*(d*curr_x - b*(y + curr_y)) + Iy*(-c*curr_x + a*(y + curr_y))) * inv_det;
		//dI_dp(i, 5) = (Iy*a - Ix*b) * inv_det;
		//dI_dp(i, 6) = (Ix*(-d*x*curr_x + b*x*curr_y) + Iy*(c*x*curr_x - a*x*curr_y)) * inv_det;
		//dI_dp(i, 7) = (Ix*(-d*y*curr_x + b*y*curr_y) + Iy*(c*y*curr_x - a*y*curr_y)) * inv_det;
	}
}

void SL3::estimateWarpFromCorners(VectorXd &state_update, const Matrix24d &in_corners,
	const Matrix24d &out_corners){
	validate_ssm_state(state_update);
	Matrix3d warp_update_mat = utils::computeHomographyDLT(in_corners, out_corners);
	getStateFromWarp(state_update, warp_update_mat);
}

void SL3::estimateWarpFromPts(VectorXd &state_update, vector<uchar> &mask,
	const vector<cv::Point2f> &in_pts, const vector<cv::Point2f> &out_pts,
	const EstimatorParams &est_params){
	cv::Mat warp_mat_cv = estimateHomography(in_pts, out_pts, mask, est_params);
	utils::copyCVToEigen<double, Matrix3d>(warp_mat, warp_mat_cv);
	getStateFromWarp(state_update, warp_mat);
}

void SL3::estimateMeanOfSamples(VectorXd &sample_mean,
	const std::vector<VectorXd> &samples, int n_samples){
	if(params.iterative_sample_mean){
		vector<ProjWarpT> lie_group_samples;
		lie_group_samples.resize(n_samples);
		// convert state vectors to SL3 matrices
		for(int sample_id = 0; sample_id < n_samples; ++sample_id){
			getWarpFromState(lie_group_samples[sample_id], samples[sample_id]);
		}
		ProjWarpT lie_group_mean = lie_group_samples[0];
		ProjWarpT lie_group_mean_inv = lie_group_mean.inverse();
		for(int iter_id = 0; iter_id < params.sample_mean_max_iters; ++iter_id){
			ProjWarpT lie_algebra_mean = ProjWarpT::Zero();
			for(int sample_id = 0; sample_id < n_samples; ++sample_id){
				lie_algebra_mean += (lie_group_mean_inv*lie_group_samples[sample_id]).log();
			}
			lie_algebra_mean /= n_samples;
			ProjWarpT lie_group_mean_upd = lie_algebra_mean.exp();
			lie_group_mean = lie_group_mean*lie_group_mean_upd;
			lie_group_mean_inv = lie_group_mean.inverse();
			double upd_norm = lie_group_mean_upd.squaredNorm();
			if(upd_norm < params.sample_mean_eps){
				break;
			}
		}
		getStateFromWarp(sample_mean, lie_group_mean);
	} else{
		ProjectiveBase::estimateMeanOfSamples(sample_mean, samples, n_samples);
	}
}

_MTF_END_NAMESPACE


/*
 * Copyright 2009-2015 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include <votca/tools/linalg.h>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include <gsl/gsl_linalg.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_eigen.h>

namespace votca { namespace tools {

using namespace std;

void linalg_cholesky_decompose( ub::matrix<double> &A){
        // Cholesky decomposition using GSL
        const size_t N = A.size1();

        gsl_matrix_view A_view = gsl_matrix_view_array(&A(0,0), N, N);

        // get the Cholesky matrices
        int status = gsl_linalg_cholesky_decomp ( &A_view.matrix );
}

void linalg_cholesky_solve(ub::vector<double> &x, ub::matrix<double> &A, ub::vector<double> &b){
    /* calling program should catch the error error code GSL_EDOM
     * thrown by gsl_linalg_cholesky_decomp and take
     * necessary steps
     */

    gsl_matrix_view m
        = gsl_matrix_view_array (&A(0,0), A.size1(), A.size2());

    gsl_vector_view gb
        = gsl_vector_view_array (&b(0), b.size());

    gsl_vector *gsl_x = gsl_vector_alloc (x.size());

    gsl_set_error_handler_off();
    int status = gsl_linalg_cholesky_decomp(&m.matrix);

    if( status == GSL_EDOM)
        throw std::runtime_error("Matrix not symmetric positive definite");

    gsl_linalg_cholesky_solve(&m.matrix, &gb.vector, gsl_x);

    for (size_t i =0 ; i < x.size(); i++)
        x(i) = gsl_vector_get(gsl_x, i);

    gsl_vector_free (gsl_x);
}

}}


//
// Created by Bryn Elesedy on 12/11/2018.
// Linear regression with a single output dimension

#include <getopt.h>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <Eigen/Dense>

using Eigen::MatrixXd;
using Eigen::VectorXd;

const double DEFAULT_LAMBDA_VALUE = 0;
const std::streamsize DEFAULT_PREC = 2;
const char DELIM = ',';
const std::string INPUT_FILE_EXTENSION = ".csv";
const std::string OUTPUT_PREDICTION_EXTENSION = ".fittedvalues";
const std::string OUTPUT_WEIGHTS_EXTENSION = ".weights";

// ----------------
// Parse command line arguments

struct command_line_args {
    command_line_args() : lambda(DEFAULT_LAMBDA_VALUE), prec(DEFAULT_PREC) {}

    double lambda;
    std::streamsize prec;
    std::string input_filename;

};

void print_help_message() {
    std::cout << "==========================================================\n";
    std::cout << ">>>>>>>>>>>>>>>>>L2 Regularised Regression<<<<<<<<<<<<<<<<\n";
    std::cout << "==========================================================\n";
    std::cout << "Usage: linregress <path/to/input_file> [--lambda] [--prec]\n";
    std::cout << "----------------------------------------------------------\n";
    std::cout << "Parameters:\n";
    std::cout << "input_file: File containing training data, must be csv.\n"
                 "            Row stacked training examples.\n"
                 "            Final column the y values, others form X.\n";
    std::cout << "--lambda (numeric): Ridge regularisation parameter.\n"
                 "                    Must be >=0, 0 gives OLS regression.\n"
                 "                    Default is " << DEFAULT_LAMBDA_VALUE << "\n";
    std::cout << "--prec (int): Significant figures for console output.\n"
                 "              Default is " << DEFAULT_PREC << "\n";
    std::cout << "==========================================================\n";
    return;
}

command_line_args parse_args(int argc, char *argv[]) {
    const char *const short_opts = "l:h";
    const option long_opts[] = {
            {"lambda", required_argument, nullptr, 'l'},
            {"prec",   required_argument,       nullptr, 'p'},
            {"help",   no_argument,       nullptr, 'h'},
    };

    command_line_args output;
    int opt;
    int option_index = 0;
    while ((opt = getopt_long(argc, argv, short_opts, long_opts, &option_index)) != -1) {
        switch (opt) {
            case 'l':
                output.lambda = std::stod(optarg);
                break;
            case 'p':
                output.prec = (std::streamsize) std::stoi(optarg);
                break;
            case 'h':
            case '?':
            default:
                print_help_message();
                exit(EXIT_SUCCESS);
        }
    }

    if (optind < argc) {
        output.input_filename = argv[optind++];
    } else {
        std::cerr << "Failure: must give an input file." << std::endl;
        exit(EXIT_FAILURE);
    }

    if (optind < argc) {
        std::cerr << "Warning: " << argc - optind << " additional arguments given, all ignored!\n" << std::endl;
    }

    return output;
}

void check_args(command_line_args &args) {

    if (args.lambda < 0) {
        std::cerr << "Error: Ridge regularisation parameter lambda must be >= 0" << std::endl;
        exit(EXIT_FAILURE);
    }

    if (args.prec < 0) {
        std::cerr << "Warning: given output precision < 0." << std::endl;
    }

    return;
}

// ----------------

// ----------------
// Parse inputs into Eigen matrices

struct data_size {
    std::size_t n_features;
    std::size_t n_examples;
};

struct input_data {
    MatrixXd X;
    VectorXd y;
};

data_size get_data_size(std::string input_filename, char delim) {
    std::ifstream infile(input_filename);

    if (!infile.is_open()) {
        std::cerr << "Error: Failed to open " << input_filename << std::endl;
        exit(EXIT_FAILURE);
    }

    data_size input_sizes;
    std::string line;
    std::size_t lc = 0;
    while (std::getline(infile, line)) {
        if (line.empty()) {
            continue;

        }

        if (lc == 0) {
            input_sizes.n_features = std::count(line.begin(), line.end(), delim);
        }
        lc++;
    }
    input_sizes.n_examples = lc;

    infile.close();
    return input_sizes;
}

input_data read_input_file(std::string input_filename, data_size input_sizes, char delim) {

    std::ifstream infile(input_filename);

    if (!infile.is_open()) {
        std::cerr << "Error: Failed to open " << input_filename << std::endl;
        exit(EXIT_FAILURE);
    }

    MatrixXd X(input_sizes.n_examples, input_sizes.n_features);
    VectorXd y(input_sizes.n_examples);

    std::size_t col = 0;
    std::size_t row = 0;
    std::string line;
    while (std::getline(infile, line)) {
        std::stringstream this_line(line);
        std::string item;

        while (std::getline(this_line, item, delim)) {
            if (item.empty()) {
                continue;
            }

            if (col == input_sizes.n_features) {
                y(row) = std::stod(item);
                col = 0;
                row++;
            } else {
                X(row, col) = std::stod(item);
                col++;
            }
        }

    }
    infile.close();

    input_data output;
    output.X = X;
    output.y = y;
    return output;
}

// ----------------

// ----------------
// Do the Regression

MatrixXd append_ones_column(MatrixXd X) {
    MatrixXd new_x(X.rows(), X.cols() + 1);
    VectorXd c = VectorXd::Constant(X.rows(), 1.);
    new_x << X, c;
    return new_x;
}

class RidgeRegression {
private:
    double lambda;
    bool has_constant;
    bool is_fitted;

    Eigen::HouseholderQR <MatrixXd> qr_decomp;
    VectorXd weights;

public:
    RidgeRegression(double lambda) : lambda(lambda), is_fitted(false) {}

    void fit(MatrixXd X, VectorXd y, bool add_constant);

    VectorXd predict(MatrixXd x_vals);

    inline VectorXd get_weights() { return this->weights; }

    inline bool has_const() { return this->has_constant; }
};

void RidgeRegression::fit(MatrixXd X, VectorXd y, bool add_constant) {
    if (X.rows() != y.rows()) {
        throw std::invalid_argument("X and y must have same number of rows.");
    }

    MatrixXd X_train = (add_constant) ? append_ones_column(X) : X;

    Eigen::MatrixXd eye = Eigen::MatrixXd::Identity(X_train.cols(), X_train.cols());

    this->qr_decomp = (this->lambda * eye + X_train.transpose() * X_train).householderQr();
    this->weights = this->qr_decomp.solve(X_train.transpose() * y);
    this->has_constant = add_constant;
    this->is_fitted = true;
    return;
}

VectorXd RidgeRegression::predict(MatrixXd xvals) {
    if (!this->is_fitted) {
        throw std::domain_error("Need to fit regression before prediction.");
    }

    if (has_constant) {
        double offset = this->weights(this->weights.rows() - 1);
        VectorXd slope = this->weights.head(this->weights.rows() - 1);
        return ((xvals * slope).array() + offset).matrix();
    } else {
        return xvals * this->weights;
    }
}
// ----------------

// ----------------
// Output

double rmse(VectorXd y1, VectorXd y2) {
    return std::sqrt((y1 - y2).array().pow(2).mean());
}

void print_results(RidgeRegression regressor, double rmse_, std::streamsize prec) {

    VectorXd weights = regressor.get_weights();

    std::streamsize curr_prec = std::cout.precision();
    std::cout << "Fitted Model: y = ";
    std::cout << std::setprecision(prec);
    int i = 0;
    while (i < weights.size() - 1) {
        std::cout << weights(i) << " x" << i + 1 << " + ";
        i++;
    }

    std::cout << weights(i) << std::endl;
    std::cout << "RMSE: " << rmse_ << std::endl;
    std::cout << std::setprecision(curr_prec);
    return;
}

std::string get_output_filename(std::string input_filename, std::string old_ext, std::string new_ext) {
    std::string prefix;
    int d = input_filename.size() - old_ext.size();
    if (d > 0 && input_filename.compare(d, old_ext.size(), old_ext) == 0) {
        prefix = input_filename.substr(0, d);
    } else {
        prefix = input_filename;
    }
    return prefix + new_ext;
}

void save_vector(VectorXd fitted_values, std::string filename) {
    std::ofstream outfile(filename);

    if (!outfile.is_open()) {
        std::cerr << "Failed out open output file " << filename << " values not written." << std::endl;
        return;
    }

    for (std::size_t i = 0; i < fitted_values.rows(); i++) {
        outfile << fitted_values(i) << std::endl;
    }

    outfile.close();
    return;
}

// ----------------

int main(int argc, char *argv[]) {

    command_line_args args = parse_args(argc, argv);

    check_args(args);

    data_size dims = get_data_size(args.input_filename, DELIM);
    input_data data = read_input_file(args.input_filename, dims, DELIM);

    std::cout << "=================================================" << std::endl;
    std::cout << "Least Squares Regression. L2 regularisation = " << args.lambda << std::endl;
    std::cout << "-------------------------------------------------" << std::endl;
    std::cout << "Read data from " << args.input_filename << ", found " << dims.n_features << " features and ";
    std::cout << dims.n_examples << " examples" << std::endl;
    std::cout << "-------------------------------------------------" << std::endl;

    RidgeRegression regressor(args.lambda);
    // Previously considered giving user option to not include constant
    // in regression but I changed my mind.
    regressor.fit(data.X, data.y, true);

    VectorXd fitted_values = regressor.predict(data.X);

    print_results(regressor, rmse(data.y, fitted_values), args.prec);
    save_vector(
            fitted_values,
            get_output_filename(args.input_filename, INPUT_FILE_EXTENSION, OUTPUT_PREDICTION_EXTENSION)
            );

    save_vector(
            regressor.get_weights(),
            get_output_filename(args.input_filename, INPUT_FILE_EXTENSION, OUTPUT_WEIGHTS_EXTENSION)
    );
    std::cout << "=================================================" << std::endl;

    return 0;

}


/*
 * Copyright 2015 David A. Boyuka II
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/*
 * dilate-impl.hpp
 *
 * Bitwise integer dilation/undilation. Algorithms courtesy of
 * "Converting to and from Dilated Integers" by Raman and Wise
 * (http://www.cs.indiana.edu/~dswise/Arcee/castingDilated-comb.pdf).
 *
 *  Created on: Sep 16, 2015
 *      Author: David A. Boyuka II
 */
#ifndef SRC_UTIL_IMPL_DILATE_IMPL_HPP_
#define SRC_UTIL_IMPL_DILATE_IMPL_HPP_

//#include <boost/math/special_functions/pow.hpp>
#include <boost/integer/static_log2.hpp>

/*

// WIP - general multiplication-based implementation
template<typename word_t, int d>
struct DilaterHelper {
	static constexpr int w = std::numeric_limits<word_t>::digits;
	static constexpr int s = w / d;

	template<int i>
	class DilaterRound {
		static constexpr int di = std::pow(d, i);
		static constexpr int bitcount = (di < s ? di : s);

		template<int j>
		static constexpr word_t z = z<j-1> | (z<0> >> (j * di * d));

		template<>
		static constexpr word_t z<0> = ((1 << bitcount) - 1) << (d * (s - 1) + 1 - bitcount);

	};
};
*/

template<typename word_t, int d>
word_t Dilater<word_t, d>::dilate(word_t word) { abort(); }

template<typename word_t, int d>
word_t Dilater<word_t, d>::undilate(word_t word) { abort(); }

// Partial specialization for d=2
template<typename word_t>
class Dilater<word_t, 2> {
public:
	static word_t dilate(word_t word);
	static word_t undilate(word_t word);
};

namespace detail {
	template<typename word_t> constexpr int S() { return std::numeric_limits<word_t>::digits/2; } // num dilateable bits
	template<typename word_t> constexpr int ROUNDS() { return boost::static_log2< S<word_t>() >::value; }

	template<typename word_t, int power> constexpr word_t EXP2P1() { return ((word_t)1<<((word_t)1<<power)) + (word_t)1; }

	template<typename word_t, int round>
	struct rounds {
		// DILATION

		// Useful <rounds> go from 1 to ROUNDS inclusive (0 is just a base case, though it is used by UNDILATE_MASK below)
		// MASK<0> is a set mask of the lower half bits of word_t
		// Each successive MASK chops all groups of set bits in half (keeping the lower halves), then
		// duplicates those groups up by twice the width of these now smaller groups
		// Examples: for 8-bit: MASK<0> = 00001111, MASK<1> = 00110011, MASK<2> = 01010101
		static constexpr word_t DILATE_MASK() { return rounds<word_t, round-1>::DILATE_MASK() / EXP2P1<word_t, ROUNDS<word_t>()-round>() * EXP2P1<word_t, ROUNDS<word_t>()-round+1>(); }
		static constexpr word_t dilate_round(word_t word) {
			return
				word = rounds<word_t, round-1>::dilate_round(word),
				(word | (word << (S<word_t>() >> round))) & rounds<word_t, round>::DILATE_MASK();
		}

		// UNDILATION

		// Useful <rounds> go from 1 to ROUNDS inclusive (0 is just a base case)
		// UNDILATE_MASK is just DILATE_MASK in reverse order, shifted by one down
		// Examples: UNDILATE_MASK<1> is DILATE_MASK<ROUNDS-1>, UNDILATE_MASK<ROUNDS> is DILATE_MASK<0>
		static constexpr word_t UNDILATE_MASK() { return rounds<word_t, ROUNDS<word_t>()-round>::DILATE_MASK(); }
		static constexpr word_t undilate_round(word_t word) {
			return
				word = rounds<word_t, round-1>::undilate_round(word),
				(word | (word >> ((word_t)1 << (round-1)))) & rounds<word_t, round>::UNDILATE_MASK();
		}
	};

	template<typename word_t>
	struct rounds<word_t, 0> {
		static constexpr word_t DILATE_MASK() { return ((word_t)1 << S<word_t>()) - 1; }
		static constexpr word_t dilate_round(word_t word) { return word; }
		static constexpr word_t undilate_round(word_t word) { return word; }
	};

	template<typename word_t> constexpr word_t dilate(word_t word) { return rounds<word_t, ROUNDS<word_t>()>::dilate_round(word); }
	template<typename word_t> constexpr word_t undilate(word_t word) { return rounds<word_t, ROUNDS<word_t>()>::undilate_round(word); }
} // namespace detail

template<typename word_t>
word_t Dilater<word_t, 2>::dilate(word_t word) {
	return detail::dilate<word_t>(word);
}

template<typename word_t>
word_t Dilater<word_t, 2>::undilate(word_t word){
	return detail::undilate<word_t>(word);
}

#endif /* SRC_UTIL_IMPL_DILATE_IMPL_HPP_ */


/**
 *  testFrameConversion.cpp
 *
 *  Test the frame conversion between inertial and body frames.
 *
 *  Created by Yinan Li on Nov. 27, 2020.
 *  Hybrid Systems Group, University of Waterloo.
 */

#include <iostream>
#include <cmath>
#include <boost/numeric/odeint.hpp>
#include <cstdlib>
#include <ctime>

#include "src/definitions.h"

/* define dynamics */
struct car_dynamics {
    rocs::Rn u;
    car_dynamics (const rocs::Rn param): u (param) {}

    void operator() (rocs::Rn &x, rocs::Rn &dxdt, double t) const
    {
	dxdt[0] = u[0]*std::cos(x[2]);
	dxdt[1] = u[0]*std::sin(x[2]);
	dxdt[2] = u[1];
    }
};

struct twoagent_ode {
    rocs::Rn u;
    rocs::Rn d;
    twoagent_ode (const rocs::Rn p1, const rocs::Rn p2): u(p1), d(p2){}
    /**
     * ODE model
     * @param x system state: [x,y,theta], n=3
     * @param dxdt vector field
     * @param t time
     */
    void operator() (rocs::Rn &x, rocs::Rn &dxdt, double t) const
    {
	dxdt[0] = -u[0] + d[0]*cos(x[2]) + u[1]*x[1];
	dxdt[1] = d[0]*sin(x[2]) - u[1]*x[0];
	dxdt[2] = d[1] - u[1];
    }
};

void inertia_to_body(rocs::Rn &z, rocs::Rn z0) {
    double x = std::cos(z0[2])*z[0] + std::sin(z0[2])*z[1];
    double y = -std::sin(z0[2])*z[0] + std::cos(z0[2])*z[1];
    z[0] = x;
    z[1] = y;
}

void body_to_inertia(rocs::Rn &z, rocs::Rn z0) {
    double x = std::cos(z0[2])*z[0] - std::sin(z0[2])*z[1];
    double y = std::sin(z0[2])*z[0] + std::cos(z0[2])*z[1];
    z[0] = x;
    z[1] = y;
}

int main() {
    boost::numeric::odeint::runge_kutta_cash_karp54<rocs::Rn> rk45;
    rocs::Rn z0{5.08561,10.1089,1.8}, z1{3.7, 10.8, 0.0};
    rocs::Rn u{0.9, -0.9}, d{0.6, 0.6/4.61};
    rocs::Rn zr(3);
    for(int i = 0; i < 3; ++i) {
	zr[i] = z1[i]-z0[i];
    }
    std::cout << "z1 position to z0 in inertial frame initially: ";
    for(int i = 0; i < 3; ++i) {
	std::cout << zr[i] << ' ';
    }
    std::cout << '\n';

    inertia_to_body(zr, z0);
    std::cout << "z1 position to z0 in z0 body frame initially: ";
    for(int i = 0; i < 3; ++i) {
	std::cout << zr[i] << ' ';
    }
    std::cout << '\n';

    const double h = 0.3;  // sampling time
    const double dt = 0.001; //integration step size for odeint
    /* Integrate z0 and z1 separately */
    boost::numeric::odeint::integrate_const(rk45, car_dynamics(u), z0, 0.0, h, dt);
    boost::numeric::odeint::integrate_const(rk45, car_dynamics(d), z1, 0.0, h, dt);
    std::cout << "z1-z0 = ";
    for(int i = 0; i < 3; ++i) {
	std::cout << z1[i]-z0[i] << ' ';
    }
    std::cout << '\n';
    /* Integrate by the local model */
    boost::numeric::odeint::integrate_const(rk45, twoagent_ode(u,d), zr, 0.0, h, dt);
    std::cout << "z1 position to z0 in z0 body frame after 0.3s: ";
    for(int i = 0; i < 3; ++i) {
	std::cout << zr[i] << ' ';
    }
    std::cout << '\n';

    body_to_inertia(zr, z0);
    std::cout << "z1 position to z0 in inertial frame after 0.3s: ";
    for(int i = 0; i < 3; ++i) {
	std::cout << zr[i] << ' ';
    }
    std::cout << '\n';
}