// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example illustrating the use the general purpose non-linear
    least squares optimization routines from the dlib C++ Library.

    This example program will demonstrate how these routines can be used for data fitting.
    In particular, we will generate a set of data and then use the least squares
    routines to infer the parameters of the model which generated the data.
*/

#include <dlib/optimization.h>
#include <iostream>
#include <vector>

using namespace std;
using namespace dlib;

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

typedef matrix<double,1,1> input_vector;
typedef matrix<double,2,1> parameter_vector;
typedef std::vector<std::pair<input_vector, double> > data_samples;

const double sqrt2pi = std::sqrt(2*pi);
const double sqrtpi = std::sqrt(pi);
// ----------------------------------------------------------------------------------------

// We will use this function to generate data.  It represents a function of 2 variables
// and 3 parameters.   The least squares procedure will be used to infer the values of
// the 3 parameters based on a set of input/output pairs.
double model (
    const input_vector& input,
    const parameter_vector& params
)
{
    const double a = params(0);
    const double b = params(1);

    const double x = input(0);

    /*
               /               2 \
               |   (a - log(x))  |
    sqrt(2) exp| - ------------- |
               |           2     |
               \        2 b      /
    ------------------------------
            b x sqrt(pi) 2
     */

    const double logx = log(x);
    const double nominator = sqrt_2 * exp(- ((a - logx)*(a - logx))/(2*b*b));
    const double denominator = b * x * sqrtpi * 2;
    double out = nominator/denominator;
    return out;

}

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

// This function is the "residual" for a least squares problem.   It takes an input/output
// pair and compares it to the output of our model and returns the amount of error.  The idea
// is to find the set of parameters which makes the residual small on all the data pairs.
double residual (
    const std::pair<input_vector, double>& data,
    const parameter_vector& params
)
{
    return model(data.first, params) - data.second;
}

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

// This function is the derivative of the residual() function with respect to the parameters.
parameter_vector residual_derivative (
    const std::pair<input_vector, double>& data,
    const parameter_vector& params
)
{
    /*
    wrt a (mean)
             /               2 \
             |   (a - log(x))  |
  sqrt(2) exp| - ------------- | (2 a - 2 log(x))
             |           2     |
             \        2 b      /
- -----------------------------------------------
                   3
                  b  x sqrt(pi) 4

           /               2 \                            /               2 \
           |   (a - log(x))  |             2              |   (a - log(x))  |
sqrt(2) exp| - ------------- | (a - log(x))    sqrt(2) exp| - ------------- |
           |           2     |                            |           2     |
           \        2 b      /                            \        2 b      /
-------------------------------------------- - ------------------------------
                4                                       2
               b  x sqrt(pi) 2                         b  x sqrt(pi) 2
    */
    parameter_vector der;

    const double a = params(0);
    const double b = params(1);

    const double x = data.first(0);
    const double logx = log(x);
    const double a_logxsq = (a - logx)*(a - logx);
    const double sqb = b*b;
    const double exp_fraction = sqrt_2 * exp(- a_logxsq/(2*sqb));

    const double numeratorA = exp_fraction * (2 * a - 2 * logx);
    const double denominatorA = b*sqb * x * sqrtpi * 4;

    const double numeratorB = exp_fraction * a_logxsq;
    const double denominatorB = sqb*sqb * x * sqrtpi * 2;

    der(0) = -numeratorA/denominatorA;
    der(1) = numeratorB/denominatorB - exp_fraction/(sqb * x * sqrtpi * 2);

    return der;
}

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

bool read_inputfile(std::string filename, data_samples& DS, double& maxYPos, std::string& errmsg){
    bool out = false;
    std::ifstream datafile(filename.c_str());
    if (!datafile.good()) {
        errmsg = "Can't open the file" + filename;
    }
    else{
        std::string line;
        double y;
        double max_y = 0;
        double x = 1;
        input_vector input;
        while (getline(datafile, line)){
            if (line.size() > 0){
                std::istringstream inp(line.c_str());
                inp >> y;
                input(0) = x;
                DS.push_back(make_pair(input, y));
                if (y > max_y){
                    max_y = y;
                    maxYPos = x;
                }
                x = x + 1.0;
            }
        }
        out = true;
    }
    return out;
}

int main(int argc, char* argv[])
{
    if (argc == 1){
        std::cout << "No input file!" << std::endl;
        return 0;
    }

    std::string infile = argv[1];
    data_samples DS;
    double maxYpos = 1;
    std::string msg;
    bool tf = read_inputfile(infile, DS ,maxYpos,msg);

    parameter_vector params;
    params(0) = log(maxYpos);
    params(1) = 0.3;
    try
    {
        // randomly pick a set of parameters to use in this example
//        const parameter_vector params = 10*randm(3,1);
//        cout << "params: " << trans(params) << endl;

        // Now let's generate a bunch of input/output pairs according to our model.
//        std::vector<std::pair<input_vector, double> > data_samples;
//        input_vector input;
//        for (int i = 0; i < 1000; ++i)
//        {
//            input = 10*randm(2,1);
//            const double output = model(input, params);
//
//            // save the pair
//            data_samples.push_back(make_pair(input, output));
//        }

        // Before we do anything, let's make sure that our derivative function defined above matches
        // the approximate derivative computed using central differences (via derivative()).
        // If this value is big then it means we probably typed the derivative function incorrectly.
        //cout << "derivative error: " << length(residual_derivative(DS[18], params) -
        //                                       derivative(residual)(DS[18], params) ) << endl;

        // Now let's use the solve_least_squares_lm() routine to figure out what the
        // parameters are based on just the data_samples.
        parameter_vector x;
        x = params;

        //cout << "Use Levenberg-Marquardt" << endl;
        // Use the Levenberg-Marquardt method to determine the parameters which
        // minimize the sum of all squared residuals.
        solve_least_squares_lm(objective_delta_stop_strategy(1e-7),
                               residual,
                               residual_derivative,
                               DS,
                               x);

        // Now x contains the solution.  If everything worked it will be equal to params.
        //cout << "inferred parameters: "<< trans(x) << endl;
        //cout << "solution error:      "<< length(x - params) << endl;
        //cout << endl;
        cout << trans(x) << endl;

        /*

        x = params;
        cout << "Use Levenberg-Marquardt, approximate derivatives" << endl;
        // If we didn't create the residual_derivative function then we could
        // have used this method which numerically approximates the derivatives for you.
        solve_least_squares_lm(objective_delta_stop_strategy(1e-7).be_verbose(),
                               residual,
                               derivative(residual),
                               DS,
                               x);

        // Now x contains the solution.  If everything worked it will be equal to params.
        cout << "inferred parameters: "<< trans(x) << endl;
        cout << "solution error:      "<< length(x - params) << endl;
        cout << endl;

        x = params;
        cout << "Use Levenberg-Marquardt/quasi-newton hybrid" << endl;
        // This version of the solver uses a method which is appropriate for problems
        // where the residuals don't go to zero at the solution.  So in these cases
        // it may provide a better answer.
        solve_least_squares(objective_delta_stop_strategy(1e-7).be_verbose(),
                            residual,
                            residual_derivative,
                            DS,
                            x);

        // Now x contains the solution.  If everything worked it will be equal to params.
        cout << "inferred parameters: "<< trans(x) << endl;
        cout << "solution error:      "<< length(x - params) << endl;
        */

    }
    catch (std::exception& e)
    {
        cout << e.what() << endl;
    }
}


#include "average_case_relative_error.hpp"
#include "average_case_error.hpp"
#include "cudd_helpers.hpp"
#include <boost/multiprecision/cpp_int.hpp>

namespace abo::error_metrics {
    std::pair<boost::multiprecision::cpp_dec_float_100, boost::multiprecision::cpp_dec_float_100>
            average_relative_value(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &g) {

        BDD zero_so_far = mgr.bddOne();

        boost::multiprecision::cpp_dec_float_100 min_average_result = 0;
        boost::multiprecision::cpp_dec_float_100 max_average_result = 0;
        std::vector<BDD> max_one = abo::util::bdd_max_one(mgr, g);
        for (int i = int(g.size())-1;i>=0;i--) {
            std::vector<BDD> partial_result;
            partial_result.reserve(max_one.size());
            BDD modifier = zero_so_far & max_one[i];
            for (const BDD &b : f) {
                partial_result.push_back(b & modifier);
            }
            auto average = average_value(partial_result);
            min_average_result += average  / (std::pow(2.0, i + 1) - 1);
            max_average_result += average  / std::pow(2.0, i);

            zero_so_far &= !max_one[i];
        }

        return {min_average_result, max_average_result};
    }

    std::pair<boost::multiprecision::cpp_dec_float_100, boost::multiprecision::cpp_dec_float_100>
            average_relative_error_bounds(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &f_hat,
                                          const util::NumberRepresentation num_rep) {

        std::vector<BDD> absolute_difference = abo::util::bdd_absolute_difference(mgr, f, f_hat, num_rep);
        return average_relative_value(mgr, absolute_difference, abo::util::abs(mgr, f));
    }

    boost::multiprecision::cpp_dec_float_100 average_relative_error_add(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &f_hat,
                                                                        const NumberRepresentation num_rep) {

        ADD diff = abo::util::absolute_difference_add(mgr, f, f_hat, num_rep);
        ADD respective_diff = diff.Divide(abo::util::bdd_forest_to_add(mgr, abo::util::abs(mgr, f)).Maximum(mgr.addOne()));
        std::vector<std::pair<double, unsigned long>> terminal_values = abo::util::add_terminal_values(respective_diff);

        boost::multiprecision::cpp_dec_float_100 sum = 0;
        boost::multiprecision::uint256_t path_sum = 0;
        for (auto p : terminal_values) {
            boost::multiprecision::cpp_dec_float_100 value(p.first);
            sum += value * p.second;
            path_sum += p.second;
        }
        return boost::multiprecision::cpp_dec_float_100(sum) /
                boost::multiprecision::cpp_dec_float_100(path_sum);
    }

    boost::multiprecision::cpp_dec_float_100 average_relative_error_symbolic_division(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &f_hat,
                                                                                unsigned int num_extra_bits, const NumberRepresentation num_rep) {

        std::vector<BDD> absolute_difference = abo::util::bdd_absolute_difference(mgr, f, f_hat, num_rep);

        std::vector<BDD> no_zero = abo::util::bdd_max_one(mgr, abo::util::abs(mgr, f));
        std::vector<BDD> divided = abo::util::bdd_divide(mgr, absolute_difference, no_zero, num_extra_bits);

        return average_value(divided) / std::pow(2.0, num_extra_bits);
    }

}


/*
 * libcluster -- A collection of hierarchical Bayesian clustering algorithms.
 * Copyright (C) 2013 Daniel M. Steinberg (daniel.m.steinberg@gmail.com)
 *
 * This file is part of libcluster.
 *
 * libcluster is free software: you can redistribute it and/or modify it under
 * the terms of the GNU Lesser General Public License as published by the Free
 * Software Foundation, either version 3 of the License, or (at your option)
 * any later version.
 *
 * libcluster is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
 * for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with libcluster. If not, see <http://www.gnu.org/licenses/>.
 */

#include "probutils.h"
#include <boost/math/special_functions.hpp>

//
// Namespaces
//

using namespace std;
using namespace Eigen;

//
// Local Constants
//

const double EIGCONTHRESH = 1.0e-8f;
const int    MAXITER      = 100;

//
// Public Functions
//

RowVectorXd probutils::mean (const MatrixXd& X)
{
  return X.colwise().sum()/X.rows();
}

RowVectorXd probutils::mean (const vector<MatrixXd>& X)
{
  const int J = X.size(),
            D = X[0].cols();
  int N = 0;
  RowVectorXd mean = RowVectorXd::Zero(D);

  for (int j = 0; j < J; ++j)
  {
    if (X[j].cols() != D)
      throw invalid_argument("X dimensions are inconsistent between groups!");

    mean += X[j].colwise().sum();
    N    += X[j].rows();
  }
  return mean / N;
}

RowVectorXd probutils::stdev (const MatrixXd& X)
{
  RowVectorXd meanX = mean(X);
  return ((X.rowwise() - meanX).array().square().colwise().sum()
          / (X.rows()-1)).sqrt();
}

MatrixXd probutils::cov (const MatrixXd& X)
{
  if (X.rows() <= 1)
    throw invalid_argument("Insufficient no. of observations.");

  MatrixXd X_mu = X.rowwise() - probutils::mean(X); // X - mu
  return (X_mu.transpose()*X_mu)/(X.rows()-1);      // (X-mu)'*(X-mu)/(N-1)
}

MatrixXd probutils::cov (const vector<MatrixXd>& X)
{
  const int J = X.size(),
            D = X[0].cols();
  int N = 0;
  const RowVectorXd mean = probutils::mean(X);
  MatrixXd cov = MatrixXd::Zero(D, D),
           X_mu;

  for (int j = 0; j < J; ++j)
  {
    if (X[j].rows() <= 1)
      throw invalid_argument("Insufficient no. of observations.");
    X_mu = X[j].rowwise() - mean;
    N   += X[j].rows();
    cov.noalias() += (X_mu.transpose() * X_mu); // (X_j-mu)'*(X_j-mu)
  }

  return cov / (N-1);
}

VectorXd probutils::mahaldist (
    const MatrixXd& X,
    const RowVectorXd& mu,
    const MatrixXd& A
    )
{
  // Check for same number of dimensions, D
  if((X.cols() != mu.cols()) || (X.cols() != A.cols()))
    throw invalid_argument("Arguments do not have the same dimensionality");

  // Check if A is square
  if (A.rows() != A.cols())
    throw invalid_argument("Matrix A must be square!");

  // Decompose A
  LDLT<MatrixXd> Aldl(A);

  // Check if A is PD
  if ((Aldl.vectorD().array() <= 0).any() == true)
    throw invalid_argument("Matrix A is not positive definite");

  // Do the Mahalanobis distance for each sample (N times)
  MatrixXd X_mu = (X.rowwise() - mu).transpose();
  return ((X_mu.array() * (Aldl.solve(X_mu)).array())
          .colwise().sum()).transpose();
}

VectorXd probutils::logsumexp (const MatrixXd& X)
{
  const VectorXd mx = X.rowwise().maxCoeff(); // Get max of each row

  // Perform the sum(exp(x - mx)) part
  ArrayXd se = ((X.colwise() - mx).array().exp()).rowwise().sum();

  // return total log(sum(exp(x))) - hoping for return value optimisation
  return (se.log()).matrix() + mx;
}

double probutils::eigpower (const MatrixXd& A, VectorXd& eigvec)
{
  // Check if A is square
  if (A.rows() != A.cols())
    throw invalid_argument("Matrix A must be square!");

  // Check if A is a scalar
  if (A.rows() == 1)
  {
    eigvec.setOnes(1);
    return A(0,0);
  }

  // Initialise working vectors
  VectorXd v = VectorXd::LinSpaced(A.rows(), -1, 1);
  VectorXd oeigvec(A.rows());

  // Initialise eigenvalue and eigenvectors etc
  double eigval = v.norm();
  double vdist = numeric_limits<double>::infinity();
  eigvec = v/eigval;

  // Loop until eigenvector converges or we reach max iterations
  for (int i=0; (vdist>EIGCONTHRESH) && (i<MAXITER); ++i)
  {
    oeigvec = eigvec;
    v.noalias() = A * oeigvec;
    eigval = v.norm();
    eigvec = v/eigval;
    vdist = (eigvec - oeigvec).norm();
  }

  return eigval;
}

double probutils::logdet (const MatrixXd& A)
{
  // Check if A is square
  if (A.rows() != A.cols())
    throw invalid_argument("Matrix A must be square!");

  VectorXd d = A.ldlt().vectorD();  // Get the diagonal from a Cholesky decomp.

  // Check if A is PD
  if ((d.array() <= 0).any() == true)
    throw domain_error("Matrix A is not positive definite.");

  return (d.array().log()).sum();   // ln(det(A)) = sum(log(d))
}

MatrixXd probutils::mxdigamma (const MatrixXd& X)
{
  const int I = X.rows(),
            J = X.cols();
  MatrixXd result(I, J);

  for (int i = 0; i < I; ++i)
    for (int j = 0; j < J; ++j)
      result(i,j) = boost::math::digamma(X(i, j));

  return result;
}

MatrixXd probutils::mxlgamma (const MatrixXd& X)
{
  const int I = X.rows(),
            J = X.cols();
  MatrixXd result(I, J);

  for (int i = 0; i < I; ++i)
    for (int j = 0; j < J; ++j)
      result(i, j) = boost::math::lgamma(X(i, j));

  return result;
}


#ifndef PDC_gauss_hh_included
#define PDC_gauss_hh_included

#include <dlib/optimization.h>
#include <iostream>
#include <vector>

#include "../classes/structs.hh"

typedef dlib::matrix<double,2,1> input_vector;
typedef dlib::matrix<double,3,1> parameter_vector;

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

// We will use this function to generate data.  It represents a function of 2 variables
// and 3 parameters.   The least squares procedure will be used to infer the values of
// the 3 parameters based on a set of input/output pairs.
double model (
    const input_vector& input,
    const parameter_vector& params
)
{
    const double p0 = params(0);
    const double p1 = params(1);
    const double p2 = params(2);

    const double i0 = input(0);
    const double i1 = input(1);

    const double temp = p0*i0 + p1*i1 + p2;

    return temp*temp;
}

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

// This function is the "residual" for a least squares problem.   It takes an input/output
// pair and compares it to the output of our model and returns the amount of error.  The idea
// is to find the set of parameters which makes the residual small on all the data pairs.
double residual (
    const std::pair<input_vector, double>& data,
    const parameter_vector& params
)
{
    return model(data.first, params) - data.second;
}

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

// This function is the derivative of the residual() function with respect to the parameters.
parameter_vector residual_derivative (
    const std::pair<input_vector, double>& data,
    const parameter_vector& params
)
{
    parameter_vector der;

    const double p0 = params(0);
    const double p1 = params(1);
    const double p2 = params(2);

    const double i0 = data.first(0);
    const double i1 = data.first(1);

    const double temp = p0*i0 + p1*i1 + p2;

    der(0) = i0*2*temp;
    der(1) = i1*2*temp;
    der(2) = 2*temp;

    return der;
}

PDC::gaussParameter fitGauss(std::vector<double>& xVals, std::vector<double>& yValsData, PDC::gaussParameter initialParams) {
    try {
        // randomly pick a set of parameters to use in this example
        const parameter_vector params = 10*dlib::randm(3,1);
        std::cout << "params: " << trans(params) << std::endl;

        // Now let's generate a bunch of input/output pairs according to our model.
        std::vector<std::pair<input_vector, double> > data_samples;
        input_vector input;
        for (int i = 0; i < 1000; ++i)
        {
            input = 10*dlib::randm(2,1);
            const double output = model(input, params);

            // save the pair
            data_samples.push_back(std::make_pair(input, output));
        }

        // Before we do anything, let's make sure that our derivative function defined above matches
        // the approximate derivative computed using central differences (via derivative()).
        // If this value is big then it means we probably typed the derivative function incorrectly.
        std::cout << "derivative error: " << length(residual_derivative(data_samples[0], params) -
                                               derivative(residual)(data_samples[0], params) ) << std::endl;

        // Now let's use the solve_least_squares_lm() routine to figure out what the
        // parameters are based on just the data_samples.
        parameter_vector x;
        x = 1;

        std::cout << "Use Levenberg-Marquardt" << std::endl;
        // Use the Levenberg-Marquardt method to determine the parameters which
        // minimize the sum of all squared residuals.
        dlib::solve_least_squares_lm(dlib::objective_delta_stop_strategy(1e-7).be_verbose(),
                               residual,
                               residual_derivative,
                               data_samples,
                               x);

        // Now x contains the solution.  If everything worked it will be equal to params.
        std::cout << "inferred parameters: "<< trans(x) << std::endl;
        std::cout << "solution error:      "<< length(x - params) << std::endl;
        std::cout << std::endl;

        x = 1;
        std::cout << "Use Levenberg-Marquardt, approximate derivatives" << std::endl;
        // If we didn't create the residual_derivative function then we could
        // have used this method which numerically approximates the derivatives for you.
        dlib::solve_least_squares_lm(dlib::objective_delta_stop_strategy(1e-7).be_verbose(),
                               residual,
                               derivative(residual),
                               data_samples,
                               x);

        // Now x contains the solution.  If everything worked it will be equal to params.
        std::cout << "inferred parameters: "<< trans(x) << std::endl;
        std::cout << "solution error:      "<< length(x - params) << std::endl;
        std::cout << std::endl;

        x = 1;
        std::cout << "Use Levenberg-Marquardt/quasi-newton hybrid" << std::endl;
        // This version of the solver uses a method which is appropriate for problems
        // where the residuals don't go to zero at the solution.  So in these cases
        // it may provide a better answer.
        dlib::solve_least_squares(dlib::objective_delta_stop_strategy(1e-7).be_verbose(),
                            residual,
                            residual_derivative,
                            data_samples,
                            x);

        // Now x contains the solution.  If everything worked it will be equal to params.
        std::cout << "inferred parameters: "<< trans(x) << std::endl;
        std::cout << "solution error:      "<< length(x - params) << std::endl;

    }
    catch (std::exception& e) {
        std::cout << "Error while fitting: " << e.what() << std::endl;
    }
}

#endif /* PDC_gauss_hh_included */


#include <iostream>
#include <cmath>
#include <string>
#include <random>
#include <chrono>
#include <fstream>
#include <tuple>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <unsupported/Eigen/SparseExtra>
#include "block_iterative_solvers.hpp"
#include "linear_algebra_addon.hpp"
using namespace std;
using namespace Eigen;
using namespace std::chrono;

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

    double tol= 1e-8;
    int itermax= 1000;

    std::srand((unsigned int) time(0));
    int L=atoi(argv[1]); // dimension of matrix A

    MatrixXcd A;

    switch (argc)
    {
    case 2:
        {
        // load A from a file in matrix market format .mtx
        cout << "please enter filename for A (.mtx): " ;
        string matfname;
        cin >> matfname;
        SparseMatrix<complex<double>> mat;
        //Matrix collections is available in   http://www.cise.ufl.edu/research/sparse/matrices/
        if(!loadMarket(mat,matfname)){
            cerr << "can't open file" << matfname << endl;
            exit(EXIT_FAILURE);
        }
        cout << "A was loaded from " <<  matfname << endl;
        A= MatrixXcd(mat);

        break;
        }
    case 3:
        {
        // Set A as a random matrix
        int n=atoi(argv[2]); // number of RHS vector
        A= MatrixXcd::Random(n,n);
        A= A.transpose()*A; // complex-symmetric
        cout << "A is a " << n << " x " << n << " square random matrix" << endl;
        break;
        }
    default:
        {

        break;
        }
    }

    MatrixXcd B= MatrixXcd::Random(A.rows(),L);

    {
        time_point<steady_clock> start= steady_clock::now();
        cout << "solving AX=B using bl_cocg_rq ..." << endl;
            MatrixXcd X= bl_cocg_rq(A, B, tol, itermax); // only applicable to complex-symmetric matrix A
        cout << " bl_cocg_rq relative error: " << (A*X-B).norm()/B.norm() << endl << endl;
        auto end= steady_clock::now();
        cout << "bl_cocg_rq computation time= " << duration_cast<seconds>((end - start)).count()  << " sec" << endl << endl<< endl;
    }

    {
        time_point<steady_clock> start= steady_clock::now();
        cout << "solving AX=B using bl_bicg_rq ..." << endl;
            MatrixXcd X= bl_bicg_rq(A, B, tol, itermax); // applicable to general matrix A but needs A.adjoint()
        cout << " bl_bicg_rq relative error: " << (A*X-B).norm()/B.norm() << endl << endl;
        auto end= steady_clock::now();
        cout << "bl_bicg_rq computation time= " << duration_cast<seconds>((end - start)).count() << " sec" << endl << endl<< endl;
    }

    {
        time_point<steady_clock> start= steady_clock::now();
        cout << "solving AX=B using bl_bicr_rq ..." << endl;
            MatrixXcd X= bl_bicr_rq(A, B, tol, itermax); // applicable to general matrix A but needs A.adjoint()
        cout << " bl_bicr_rq relative error: " << (A*X-B).norm()/B.norm() << endl << endl;
        auto end= steady_clock::now();
        cout << "bl_bicr_rq computation time= " << duration_cast<seconds>((end - start)).count() << " sec" << endl << endl<< endl;
    }

    {
        time_point<steady_clock> start= steady_clock::now();
        cout << "solving AX=B using bl_bicgstab_rq ..." << endl;
            MatrixXcd X= bl_bicgstab_rq(A, B, tol, itermax); // only applicable to complex-symmetric matrix A
        cout << " bl_bicgstab_rq relative error: " << (A*X-B).norm()/B.norm() << endl << endl;
        auto end= steady_clock::now();
        cout << "bl_bicgstab_rq computation time= " << duration_cast<seconds>((end - start)).count() << " sec" << endl << endl<< endl;
    }

    {
        time_point<steady_clock> start= steady_clock::now();
        cout << "solving AX=B using bl_bicgstab ..." << endl;
            MatrixXcd X= bl_bicgstab(A, B, tol, itermax); // only applicable to complex-symmetric matrix A
        cout << " bl_bicgstab relative error: " << (A*X-B).norm()/B.norm() << endl << endl;
        auto end= steady_clock::now();
        cout << "bl_bicgstab computation time= " << duration_cast<seconds>((end - start)).count() << " sec" << endl << endl<< endl;
    }

    {
        time_point<steady_clock> start= steady_clock::now();
        cout << "solving AX=B using bl_bicggr ..." << endl;
            MatrixXcd X= bl_bicggr(A, B, tol, itermax); // only applicable to complex-symmetric matrix A
        cout << " bl_bicggr relative error: " << (A*X-B).norm()/B.norm() << endl << endl;
        auto end= steady_clock::now();
        cout << "bl_bicggr computation time= " << duration_cast<seconds>((end - start)).count() << " sec" << endl << endl<< endl;
    }

}


#include <iostream>
#include <ctime>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Dense>

#define MATRIX_SIZE 50

int main(int argc, char** argv) {
    Eigen::Matrix<float,2,3> matrix_23;
    Eigen::Vector3d v_3d;//Matrix<double,3,1>
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();//Matrix<double,3,3>
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;
    Eigen::MatrixXd matric_x;

    matrix_23<<1,2,3,4,5,6;
    cout<<matrix_23<<endl;

    for (int i = 0; i < 1 ; i++)
        for (int j = 0; j < 2 ; j++)
            cout<<matrix_23(i,j)<<endl;

    v_3d<<3,2,1;
    Eigen::Matrix<double,2,1> result=matrix_23.cast<double>()*v_3d; //cast 转换类型
    cout<<result<<endl;

    matrix_33 = Eigen::Matrix3d::Random();
    cout <<matrix_33<<endl<<endl;

    cout<<matrix_33.transpose()<<endl;
    cout<<matrix_33.sum() <<endl;
    cout<<matrix_33.trace()<<endl;
    cout<<10*matrix_33<<endl;
    cout<<matrix_33.inverse()<<endl;
    cout<<matrix_33.determinant()<<endl;

    //特征值
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver (matrix_33);
    cout<< "Eigen values = "<<eigen_solver.eigenvalues() <<endl;
    cout<< "Eigen vectors = "<<eigen_solver.eigenvectors() <<endl;

    Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE>matrix_NN;
    matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
    Eigen::Matrix<double, MATRIX_SIZE, 1> v_Nd;
    v_Nd =Eigen::MatrixXd::Random(MATRIX_SIZE,1);

    //计时
    clock_t time_stt=clock();
    Eigen::Matrix<double,MATRIX_SIZE,1> x=matrix_NN.inverse()*v_Nd;
    cout<<"time use in normal inverse is "<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;

    time_stt=clock();
    x =matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout<<"time use in Qr composition is "<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;

    return 0;
}


/********************************************************************
 * Implements multivariate regression analysis in an attempt to learn
 * the weight/hypothesis coefficients for a system of eqations.
 * Created: Jun 17 2020
 * Author: Ethan Patterson
 *******************************************************************/

#include <iostream>
#include <fstream>
#include <Eigen/Dense> // Linear Algebra library. Don't forget to add path to Makefile.

#include "HousingData.h" // For reading data files.

#ifndef ITR
#define ITR 2000
#endif

#ifndef ALPHA
#define ALPHA 0.01
#endif

// Made for convinance of managing
// the max and min values of the data.
struct Tuple{
    float x;
    float y;
};

void loadData(Eigen::MatrixXf &X, Eigen::VectorXf &Y, std::ifstream &fp){
    int r = 0;
    HousingData data;
    // Read data line by line and sotre it into X matrix.
    while (fp >> data){
        // Again this could be put into an array.
        // See HousingData.h for why it is currently structered this way.
        //X(r,0) = 1; // Dummy value.
        X(r,0) = std::stof(data.crim, NULL);
        X(r,1) = std::stof(data.zn, NULL);
        X(r,2) = std::stof(data.indus, NULL);
        X(r,3) = std::stof(data.chas, NULL);
        X(r,4) = std::stof(data.nox, NULL);
        X(r,5) = std::stof(data.rm, NULL);
        X(r,6) = std::stof(data.age, NULL);
        X(r,7) = std::stof(data.dis, NULL);
        X(r,8) = std::stof(data.rad, NULL);
        X(r,9) = std::stof(data.tax, NULL);
        X(r,10) = std::stof(data.ptratio, NULL);
        X(r,11) = std::stof(data.B, NULL);
        X(r,12) = std::stof(data.lstat, NULL);

        Y(r) = std::stof(data.medv, NULL); // Values of interest.
        r++;
    }

    fp.clear();
    fp.seekg(0, fp.beg);// Reset to head of file.
}
/**
 * @param fp is a file pointer.
 * Function gets the number of rows in a file.
 */
int getFileRowCount(std::ifstream &fp){
    int num_of_rows = 0;
    std::string rows;
    while (std::getline( fp, rows ))
        num_of_rows++;

    fp.clear();
    fp.seekg(0, fp.beg); // Reset to head of file.
    return num_of_rows;
}

/**
 * @param X Matrix of Quantitative Variables.
 * @param W Learned vector of weight coefficients
 * @param Y Vector of Response Variables.
 * Measures the Sum Squared Error of the learned weights.
 */
float leastSquares(Eigen::MatrixXf &X, Eigen::VectorXf &W, Eigen::VectorXf &Y){
    Eigen::VectorXf e( Y.size() );
    e = (Y - X*W);
    for (int i = 0; i < e.size(); i++)
        e[i] = e[i]*e[i]/Y.size();

    return (float)e.sum();
}
/**
 * @param X Matrix of Quantitative Variables.
 * @param W Learned vector of weight coefficients, hypothosis.
 * @param Y Vector of Response Variables.
 * @param alpha The learning rate to apply.
 * @param iter The number of iterations to complete.
 * Use batch gradient descent to minimize the cost/error of the system.
 * Returns the cost history of the system over iterations.
 */
float* BGD(Eigen::MatrixXf &X, Eigen::VectorXf &W, Eigen::VectorXf &Y, float alpha, int itr, const Tuple &maxmin){
    Eigen::VectorXf Hypo ( Y.size() );
    Eigen::VectorXf Loss ( Y.size() );
    Eigen::VectorXf Gradient ( Y.size() );
    float* costHistory = new float[itr];

    float cost = 0.0;
    for (int i = 0; i < itr; i++){
        Hypo = X*W;
        Loss = Hypo - Y;
        Gradient = (X.transpose() * Loss) / Y.size();
        W = W - alpha * Gradient;
        cost = leastSquares(X, W, Y);
        costHistory[i] = cost*(maxmin.x - maxmin.y) + maxmin.y;
    }
    return costHistory;
}
/**
 * @param X Matrix of Quantitative Variables.
 * @param Y Vector of Response Variables.
 * Nomralize the data between 0 and 1.
 * Returns a tuple of the max and min value.
 * Max is the x component and min is the y.
 */
Tuple normalize(Eigen::MatrixXf &X, Eigen::VectorXf &Y){
    float max_x, max_y, max;
    float min_x, min_y, min;
    Tuple maxmin;

    Eigen::MatrixXf::Index maxRow,maxCol;
    Eigen::MatrixXf::Index minRow,minCol;

    max_x = X.maxCoeff(&maxRow, &maxCol);
    max_y = Y.maxCoeff(&maxRow, &maxCol);

    min_x = X.minCoeff(&minRow, &minCol);
    min_y = Y.minCoeff(&minRow, &minCol);

    if (max_x < max_y)
        max = max_y;
    else
        max = max_x;

    if (min_x < min_y)
        min = min_x;
    else
        min = min_y;

    for (int i = 0; i < X.rows(); i++){
        for (int j = 0; j < X.cols(); j++){
            X(i,j) = (X(i,j) - min) / (max - min);
        }
    }

    for (int i = 0; i < Y.rows(); i++)
        Y(i) = (Y(i) - min) / (max - min);

    maxmin.x = max;
    maxmin.y = min;

    return maxmin;
}
/**
 * @param cost Dynamic array of cost history.
 * @param W Learned vector of weight coefficients, hypothesis.
 * @param Y Vector of Response Variables.
 * Saves data produced such as the cost history, weights/hypotheses, and normalization min max.
 */
void saveData(float *cost, const Tuple &maxmin, Eigen::VectorXf &W){
    std::ofstream weights("weights.csv");
    std::ofstream norm("normalize.csv");
    std::ofstream costHistory("cost_history.csv");

    if (weights.is_open()){
        for (int i = 0; i < W.size()-1; i++)
            weights << W(i) << ",";
        weights << W(W.size()-1);
    }
    weights.close();

    if (norm.is_open()){
        norm << "max" << "," << maxmin.x << "\n";
        norm << "min" << "," << maxmin.y << "\n";
    }
    norm.close();

    if(costHistory.is_open()){
        for (int i = 0; i < ITR; i++)
            if ( i % 2 ) // No need to see cost at every step.
                costHistory << i << "," << cost[i] << "\n";
    }
    costHistory.close();
}

int main()
{
    // Files in training and testing data.
    std::ifstream fp_train("./data/data/housing_train.csv");
    std::ifstream fp_test("./data/data/housing_test.csv");

    // Build maxtix for traning and testing data.
    Eigen::MatrixXf X(getFileRowCount(fp_train), COL_SIZE);
    Eigen::MatrixXf X_test(getFileRowCount(fp_test), COL_SIZE);

    // Build Vectors for target variables and weight/hypotheses.
    Eigen::VectorXf Y( X.rows() );
    Eigen::VectorXf Y_test( X_test.rows() );
    Eigen::VectorXf W ( X.cols() );// Size is the number of features/dimensions.

    Tuple maxmin; // Used to keep track of max and min value during normalization.
    float *costHistory; // Pointer for array to cost history.

    // Load data into matrices and vectors.
    loadData(X, Y, fp_train);
    loadData(X_test, Y_test, fp_test);
    fp_train.close(); // Don't need this anymore, data is now in the matrix.
    fp_test.close();

    // Set all initial weight/hypothesis values to 0.
    W = Eigen::VectorXf::Ones( X.cols() ) * 0;

    normalize(X, Y); // Normalize data.
    maxmin = normalize(X_test, Y_test); // Keep max and min values for future data sets.
    std::cout << "Max: " << maxmin.x << " Min: " << maxmin.y << std::endl;

    // Run batch gradient descent.
    costHistory = BGD(X, W, Y, ALPHA, ITR, maxmin);
    std::cout << "SSE for training set: " << leastSquares(X, W, Y) *
     (maxmin.x - maxmin.y) + maxmin.y << std::endl;
    std::cout << "SSE for testing set: " << leastSquares(X_test, W, Y_test) *
     (maxmin.x - maxmin.y) + maxmin.y << std::endl;

    saveData(costHistory, maxmin, W);
    delete costHistory;

    return 0;
}


#include "case.hpp"

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

using namespace Eigen;

Case1::Case1(const Ref<const MatrixXd> &matM, const Ref<const MatrixXd> &matD,
             const Ref<const MatrixXd> &matK)

    : ndim_(matM.cols()), matM_(matM), matD_(matD), matK_(matK),
      matC_(ndim_ * 2, ndim_ * 2), matG_(ndim_ * 2, ndim_ * 2),
      alphas(ndim_ * 2), betas(ndim_ * 2) {
  MatrixXd mzero = MatrixXd::Zero(ndim_, ndim_);
  MatrixXd mone = MatrixXd::Identity(ndim_, ndim_);
  matC_ << -matD_, -matK_, mone, mzero;
  matG_ << matM_, mzero, mzero, mone;

  GeneralizedEigenSolver<MatrixXd> ges(matC_, matG_, false);
  alphas = ges.alphas();
  betas = ges.betas();
}


#include "matrix.h"
#include "mex.h"

#include <iostream>
#include <math.h>
#include <chrono>
#include <random>
#include <vector>
#include <Eigen\Dense>
#include <Eigen\SVD>

#include "estimator.h"
#include "prosac.h"
#include "mlesac.h"
#include "ransac.h"

using namespace std;
using namespace theia;
using namespace Eigen;

// Structure for a 2-D point
struct Point
{
	double x;
	double y;
	Point() {}
	Point(double _x, double _y) : x(_x), y(_y) {}
	Point(const Point& p)
	{
		x = p.x;
		y = p.y;
	}
};

// A pair of points that correspond to each other. These two points are two keypoints in to separate images that have been matched to each other
struct Correspondence
{
	Point p1;
	Point p2;
	Correspondence() {}
	Correspondence(double x1, double y1, double x2, double y2)
	{
		p1.x = x1;
		p1.y = y1;
		p2.x = x2;
		p2.y = y2;
	}
	Correspondence(Point& _p1, Point& _p2) : p1(_p1), p2(_p2) {}
	Correspondence(const Correspondence& other) : p1(other.p1), p2(other.p2) { }
};

// Homography matrix is a 3-by-3 matrix which is the model we are trying to estimate
struct Homography
{
	double Matrix[9];
	Homography() {}
	Homography(const Homography& other)
	{
		for (size_t i = 0; i < 9; i++)
		{
			Matrix[i] = other.Matrix[i];
		}
	}
};

// This is the estimator class for estimating a homography matrix between two images. A model estimation method and error calculation method are implemented
class HomographyEstimator : public Estimator < Correspondence, Homography >
{
public:
	HomographyEstimator() {}
	~HomographyEstimator() {}

	int SampleSize() const { return 4; }
	bool EstimateModel(const vector<Correspondence>& data, vector<Homography>* models) const
	{
		Homography model;

		//calculate homography -- see gc_lecture_notes pp. 17-19
		MatrixXd Q = MatrixXd::Zero(2 * data.size(), 9);
		for (size_t i = 0; i < data.size(); i++)
		{
			Q(i, 0) = data[i].p2.x;
			Q(i, 1) = data[i].p2.y;
			Q(i, 2) = 1.0;
			Q(i, 6) = -1 * data[i].p2.x * data[i].p1.x;
			Q(i, 7) = -1 * data[i].p2.y * data[i].p1.x;
			Q(i, 8) = -1 * data[i].p1.x;

			Q(i + data.size(), 3) = data[i].p2.x;
			Q(i + data.size(), 4) = data[i].p2.y;
			Q(i + data.size(), 5) = 1.0;
			Q(i + data.size(), 6) = -1.0 * data[i].p2.x * data[i].p1.y;
			Q(i + data.size(), 7) = -1.0 * data[i].p2.y * data[i].p1.y;
			Q(i + data.size(), 8) = -1.0 * data[i].p1.y;
		}

		MatrixXd qtq = Q.transpose() * Q;
		JacobiSVD<MatrixXd> svd(qtq, ComputeThinV);
		svd.computeV();
		MatrixXd minEigenVector = svd.matrixV().col(8);
		for (size_t i = 0; i < 9; i++)
		{
			model.Matrix[i] = minEigenVector(i);
		}
		models->push_back(model);
		return true;
	}

	double Error(const Correspondence& correspondence, const Homography& model) const
	{
		// calculate reporojection error
		double projectionPointZ = model.Matrix[6] * correspondence.p1.x + model.Matrix[7] * correspondence.p1.y + model.Matrix[8];
		double projectionPointX = (model.Matrix[0] * correspondence.p1.x + model.Matrix[1] * correspondence.p1.y + model.Matrix[2]) / projectionPointZ;
		double projectionPointY = (model.Matrix[3] * correspondence.p1.x + model.Matrix[4] * correspondence.p1.y + model.Matrix[5]) / projectionPointZ;

		return (double) (fabs(projectionPointX - correspondence.p2.x) + fabs(projectionPointY - correspondence.p2.y));
	}
};

// This is the entry point from MATLAB to this routine
// plhs and prhs represent a pointer to the left hand side (output) and a pointer to the right hand side (input) respectively
// matlab function signature: H = homography_estimator(points1, points2, 'mode')
// H -> 9-by-9 homography matrix as output
// points1 -> n-by-2 matrix, coordinates of the keypoints of the first image
// points2 -> n-by-2 matrix, coordinates of the keypoints of the second image
// 'mode' -> {'ransac', 'prosac', 'mlesac'}
void mexFunction(int nlhs, mxArray *plhs [], int nrhs, const mxArray*prhs [])
{
	if (nlhs != 1)
	{
		mexPrintf("Only one output expected\n");
		return;
	}
	if (nrhs != 3)
	{
		mexPrintf("Only three inputs expected\n");
		return;
	}

	const mwSize *dims1, *dims2;
	char* modeStr;
	double *points1, *points2;

	dims1 = mxGetDimensions(prhs[0]);
	dims2 = mxGetDimensions(prhs[1]);

	int dim1y = (int) dims1[0]; int dim1x = (int) dims1[1];
	int dim2y = (int) dims2[0]; int dim2x = (int) dims2[1];

	if (dim1x != dim2x || dim1y != dim2y)
	{
		mexPrintf("Error! two input points arrays must be of the same size\n");
		return;
	}

	if (dim1x != 2 || dim2x != 2)
	{
		mexPrintf("Error! Points must be 2-D!\n");
		return;
	}

	modeStr = mxArrayToString(prhs[2]);

	if (!modeStr || mxGetDimensions(prhs[2])[0] != 1)
	{
		mexPrintf("Error! Last argument must be an string!\n");
		return;
	}

	mexPrintf("string:\n%s\n", modeStr);
	vector<Correspondence> correspondences(dim1y);
	points1 = mxGetPr(prhs[0]);
	points2 = mxGetPr(prhs[1]);
	for (size_t j = 0; j < dim1y; j++)
	{
		/*Correspondence corr;
		corr.p1.x = points1[j];
		corr.p2.x = points2[j];
		corr.p1.y = points1[dim1y + j];
		corr.p2.y = points2[dim1y + j];*/
		correspondences[j] = Correspondence(points1[j], points1[dim1y + j], points2[j], points2[dim1y + j]);
	}
	Homography homographyModel;
	RansacParameters params;
	params.error_thresh = 0.5;
	HomographyEstimator homographyEstimator;
	SampleConsensusEstimator<HomographyEstimator> *sacEstimator;
	// RANSAC mode
	if (!strcmp("ransac", modeStr))
	{
		sacEstimator = new Ransac<HomographyEstimator>(params, homographyEstimator);
	}
	// MLESAC mode
	else if (!strcmp("mlesac", modeStr))
	{
		sacEstimator = new Mlesac<HomographyEstimator>(params, homographyEstimator);
	}
	// PROSAC mode
	else if (!strcmp("prosac", modeStr))
	{
		sacEstimator = new Prosac<HomographyEstimator>(params, homographyEstimator);
	}
	// Unknown mode
	else
	{
		mexPrintf("Error! unknown estimation mode selected!\n");
		return;
	}

	sacEstimator->Initialize();
	RansacSummary summary;
	sacEstimator->Estimate(correspondences, &homographyModel, &summary);

	auto outArray = mxCreateDoubleMatrix(3, 3, mxREAL);
	double *outp = mxGetPr(outArray);
	// Copy the estimated homography matrix to the output matrix, note the difference between matrix's column-wise arrays vs. C's row-wise arrays convention
	auto lastElement = homographyModel.Matrix[8];
	for (size_t i = 0; i < 3; i++)
	{
		for (size_t j = 0; j < 3; j++)
		{
			outp[i * 3 + j] = homographyModel.Matrix[j * 3 + i] / lastElement;
		}
	}

	plhs[0] = outArray;
}


#include <plll.hpp>
#include <Eigen/Dense>
#include <random>
#include <iostream>
#include <vector>
#include <map>
#include <numeric>
#include <fstream>
#include <algorithm>
#include <chrono>
#include <cstdlib>
#include <ctime>
#include <cmath>
#include <regex>

using namespace std;
//namespace plt = matplotlibcpp;
template <class T>
using Matrix = vector<vector<T>>;
using sec_type = std::chrono::microseconds;
using _type = float;
constexpr double _c = 0.9;

struct matrix_with_det
{
    int det;
    Matrix<int> mat;
};

template <typename T>
void getCofactor(Matrix<T> &A, Matrix<T> &temp, int p, int q, int n)
{
    int i = 0, j = 0;

    // Looping for each element of the matrix
    for (int row = 0; row < n; row++)
    {
        for (int col = 0; col < n; col++)
        {
            //  Copying into temporary matrix only those element
            //  which are not in given row and column
            if (row != p && col != q)
            {
                temp[i][j++] = A[row][col];

                // Row is filled, so increase row index and
                // reset col index
                if (j == n - 1)
                {
                    j = 0;
                    i++;
                }
            }
        }
    }
}
/* Recursive function for finding determinant of matrix.
   n is current dimension of A[][]. */
template <typename T>
T determinant(Matrix<T> &A, int n)
{

    //  Base case : if matrix contains single element
    if (n == 1)
        return A[0][0];
    T D = 0;                            // Initialize result
    Matrix<T> temp(n, vector<T>(n, 0)); // To store cofactors

    float sign = 1; // To store sign multiplier

    // Iterate for each element of first row
    for (int f = 0; f < n; f++)
    {
        // Getting Cofactor of A[0][f]
        getCofactor(A, temp, 0, f, n);
        D += sign * A[0][f] * determinant(temp, n - 1);

        // terms are to be added with alternate sign
        sign = -sign;
    }

    return D;
}

// Function to get adjoint of A[N][N] in adj[N][N].
void adjoint(Matrix<_type> &A, Matrix<_type> &adj)
{
    int n = A.size();
    if (n == 1)
    {
        adj[0][0] = 1;
        return;
    }

    // temp is used to store cofactors of A[][]
    _type sign = 1;
    Matrix<_type> temp(n, vector<_type>(n, 0));

    for (int i = 0; i < n; i++)
    {
        for (int j = 0; j < n; j++)
        {
            // Get cofactor of A[i][j]
            getCofactor(A, temp, i, j, n);

            // sign of adj[j][i] positive if sum of row
            // and column indexes is even.
            sign = ((i + j) % 2 == 0) ? 1 : -1;

            // Interchanging rows and columns to get the
            // transpose of the cofactor matrix
            adj[j][i] = (sign) * (determinant(temp, n - 1));
        }
    }
}

// Function to calculate and store inverse, returns false if
// matrix is singular
bool inverse(Matrix<_type> &A, Matrix<_type> &inverse)
{
    // Find determinant of A[][]
    int det = determinant(A, A.size());
    //_type det = 6;
    if (det == 0)
    {
        cout << "Singular matrix, can't find its inverse";
        return false;
    }

    // Find adjoint
    Matrix<_type> adj;
    for (int i = 0; i < A.size(); i++)
    {
        adj.push_back(vector<_type>());
        for (int j = 0; j < A.size(); j++)
            adj[i].push_back(0);
    }
    adjoint(A, adj);

    // Find Inverse using formula "inverse(A) = adj(A)/det(A)"
    for (int i = 0; i < A.size(); i++)
        for (int j = 0; j < A.size(); j++)
            inverse[i][j] = adj[i][j] / float(det);

    return true;
}

template <typename T, typename T2>
double dotProduct(const vector<T> &v1, const vector<T2> &v2)
{
    double ret = 0;
    for (int i = 0; i < v1.size(); ++i)
        ret += (v1.at(i) * v2.at(i));
    return ret;
}

template <typename T>
vector<T> operator*(const double &c, const vector<T> &v)
{
    vector<T> ret = v;
    for (auto &d : ret)
        d *= c;
    return ret;
}

vector<_type> operator*(Matrix<_type> &M, const vector<_type> &v)
{
    int n = v.size();
    vector<_type> res(n, 0);
    for (size_t i = 0; i < n; i++)
    {
        for (size_t j = 0; j < n; j++)
        {
            res[i] += M[j][i] * v[j];
        }
    }
    return res;
}
template <typename T, typename T2>
vector<T2> subtract(const vector<T> &v1, const vector<T2> &v2)
{
    vector<T2> ret(v1.size());
    for (int i = 0; i < v1.size(); ++i)
        ret[i] = (v1[i] - v2[i]);
    return ret;
}
template <typename T, typename T2>
vector<T> operator-(const vector<T> &v1, const vector<T2> &v2)
{
    vector<T> ret(v1.size());
    for (int i = 0; i < v1.size(); ++i)
        ret[i] = v1[i] - v2[i];
    return ret;
}
template <typename T>
vector<T> inverse(const vector<T> &v1)
{
    vector<T> ret(v1.size());
    for (int i = 0; i < v1.size(); ++i)
        ret[i] = -v1[i];
    return ret;
}
template <typename T>
bool operator==(const vector<T> &v1, const vector<T> &v2)
{
    if (v1.size() == v2.size())
    {
        if (v1.size())
        {
            if (v1[0] == v2[0])
            {
                if (v1[0] == 0)
                {
                    int sign = 0;
                    for (int i = 1; i < v1.size(); i++)
                    {
                        if (v1[i] == 0 && v2[i] == 0)
                            continue;
                        if (sign == 0)
                        {
                            if (v1[i] == v2[i])
                                sign = 1;
                            else if (v1[i] == -v2[i])
                                sign = -1;
                            else
                                return false;
                        }
                        else if (v1[i] != sign * v2[i])
                        {
                            return false;
                        }
                    }
                }
                else
                {
                    for (int i = 1; i < v1.size(); i++)
                    {
                        if (v1[i] != v2[i])
                        {
                            return false;
                        }
                    }
                }
                return true;
            }
            if (v1[0] == -v2[0])
            {
                for (int i = 1; i < v1.size(); i++)
                {
                    if (v1[i] != -v2[i])
                    {
                        return false;
                    }
                }
                return true;
            }
            return false;
        }
        else
            return true;
    }
    else
        return false;
}
template <typename T>
vector<T> operator+(const vector<T> &v1, const vector<T> &v2)
{
    vector<T> ret(v1.size());
    for (int i = 0; i < v1.size(); ++i)
        ret[i] = (v1.at(i) + v2.at(i));
    return ret;
}
vector<_type> roundV(vector<_type> a)
{
    for (size_t i = 0; i < a.size(); i++)
    {
        a[i] = round(a[i]);
    }
    return a;
}

vector<_type> proj(const vector<_type> &v1, const vector<_type> &v2)
{
    vector<_type> proj = (dotProduct(v1, v2) / dotProduct(v1, v1)) * v1;
    return proj;
}

vector<_type> proj(const vector<_type> &v1, const vector<int> &v2)
{
    vector<_type> proj = (dotProduct(v1, v2) / dotProduct(v1, v1)) * v1;
    return proj;
}

Matrix<_type> gSchmidt(const Matrix<int> &vspace)
{
    Matrix<_type> uspace(vspace.size(), vector<_type>(vspace[0].size()));
    for (int i = 0; i < vspace[0].size(); i++)
    {
        uspace[0][i] = vspace[0][i];
    }
    for (int i = 1; i < vspace.size(); ++i)
    {
        for (int j = 0; j < i; ++j)
        {
            uspace.at(i) = subtract(vspace.at(i), proj(uspace.at(j), vspace.at(i)));
        }
    }
    return uspace;
}
Matrix<_type> gSchmidt(const Matrix<_type> &vspace, int limit)
{
    Matrix<_type> uspace;
    for (int i = 0; i <= limit; ++i)
    {
        uspace.push_back(vspace.at(i));
        for (int j = 0; j < i; ++j)
        {
            uspace.at(i) = uspace.at(i) - proj(uspace.at(j), vspace.at(i));
        }
    }
    return uspace;
}
template <typename T>
void printVS(const Matrix<T> &vspace)
{
    for (auto &v : vspace)
    {
        cout << '|';
        for (auto &d : v)
            cout << d << ' ';
        cout << "|\n";
    }
}
template <typename T>
void printVS(const Matrix<T> &vspace, ostream &file)
{
    file << '[';
    for (auto &v : vspace)
    {
        file << '[';
        for (auto &d : v)
            file << d << ' ';
        file << "]\n";
    }
    file << "]\n";
}
template <typename T>
void printVec(const vector<T> &v)
{
    cout << endl;
    cout << "$$$$$$$$$$$$$$$$$$$$$$$$" << endl;
    for (auto &d : v)
        cout << d << ' ';
    cout << "$$$$$$$$$$$$$$$$$$$$$$$$" << endl;
}
template <typename T>
void printVec(const vector<T> &v, ostream &file)
{
    file << endl;
    file << "$$$$$$$$$$$$$$$$$$$$$$$$" << endl;
    for (auto &d : v)
        file << d << ' ';
    file << endl
         << "$$$$$$$$$$$$$$$$$$$$$$$$" << endl;
}
template <typename T>
float mu(int k1, int k2, Matrix<T> *m, Matrix<_type> *gsm)
{
    T init = 0;
    vector<T> *a = &m->at(k1);
    vector<_type> *b = &gsm->at(k2);
    return std::inner_product(a->begin(), a->end(), b->begin(), init) /
           std::inner_product(b->begin(), b->end(), b->begin(), init);
}
template <typename T, typename T2>
float mu(vector<T> &a, vector<T2> &b)
{
    float init = 0.0;
    double d = std::inner_product(a.begin(), a.end(), b.begin(), init);
    double dd = std::inner_product(b.begin(), b.end(), b.begin(), init);
    return d / dd;
}
template <typename T>
double squaredLengthOfVector(vector<T> *v)
{
    return std::inner_product(v->begin(), v->end(), v->begin(), 0.0);
}
template <typename T>
double squaredLengthOfVector(vector<T> &v)
{
    return std::inner_product(v.begin(), v.end(), v.begin(), 0.0);
}

int KabatianskyLevenshteinC(Matrix<int> &M)
{
    float angle = 1;
    for (int i = 0; i < M.size(); i++)
    {
        auto v1 = M[i];
        for (int j = i + 1; j < M.size(); j++)
        {
            auto v2 = M[j];
            float t = dotProduct(v1, v2) / (sqrt(squaredLengthOfVector(&v1)) *
                                            sqrt(squaredLengthOfVector(&v2)));
            angle = fmin(angle, t);
        }
    }
    return fmax((int)(-1.0 / 2.0 * log(1 - cos(angle)) - 0.099), 1);
}

vector<int> lll_reduce(vector<_type> &gsreducing, vector<int> &target,
                       vector<int> &reducing)
{
    vector<int> t = target - round(mu(target, gsreducing)) * reducing;
    return t;
}

void LLL(Matrix<int> &B, ostream &o)
{
    bool cont;
    Matrix<_type> gramSmidt;
    int iter = 0;
    do
    {
        iter++;
        gramSmidt = gSchmidt(B);
        cont = false;
        for (int i = 1; i < B.size(); i++)
        {
            for (int j = 0; j < i; j++)
            {
                B.at(i) = lll_reduce(gramSmidt.at(j), B.at(i), B.at(j));
            }
        }
        for (int i = 1; i < B.size() && !cont; i++)
        {
            float a1 = squaredLengthOfVector(&gramSmidt.at(i - 1));
            float a2 = squaredLengthOfVector(&gramSmidt.at(i));
            float mu1 = mu(i, i - 1, &B, &gramSmidt);
            if (a2 < (_c - mu1 * mu1) * a1)
            {
                B.at(i - 1).swap(B.at(i));
                cont = true;
            }
        }
    } while (cont);
    o << "lll iter : " << iter << "|\n";
}

void LLL(Matrix<int> &B)
{
    bool cont;
    Matrix<_type> gramSmidt;
    int iter = 0;
    do
    {
        iter++;
        gramSmidt = gSchmidt(B);
        cont = false;
        for (int i = 1; i < B.size(); i++)
        {
            for (int j = 0; j < i; j++)
            {
                B.at(i) = lll_reduce(gramSmidt.at(j), B.at(i), B.at(j));
            }
            float a1 = squaredLengthOfVector(&gramSmidt.at(i - 1));
            float a2 = squaredLengthOfVector(&gramSmidt.at(i));
            float mu1 = mu(i, i - 1, &B, &gramSmidt);
            if (a2 < (_c - mu1 * mu1) * a1)
            {
                B.at(i - 1).swap(B.at(i));
                i--;
                cont = true;
            }
        }
    } while (cont);
}
// opera 918
void LLL2(Matrix<int> &B)
{
    bool cont;
    Matrix<_type> gramSmidt;
    int iter = 0;
    do
    {
        iter++;
        gramSmidt = gSchmidt(B);
        cont = false;
        for (int i = 1; i < B.size(); i++)
        {
            for (int j = i - 1; j > 0; j--)
            {
                B.at(i) = lll_reduce(gramSmidt.at(j), B.at(i), B.at(j));
            }
            float a1 = squaredLengthOfVector(&gramSmidt.at(i - 1));
            float a2 = squaredLengthOfVector(&gramSmidt.at(i));
            float mu1 = mu(i, i - 1, &B, &gramSmidt);
            if (a2 < (_c - mu1 * mu1) * a1)
            {
                B.at(i - 1).swap(B.at(i));
                i--;
                cont = true;
            }
        }
    } while (cont);
}
void preparePlot(Matrix<_type> res, Matrix<_type> og)
{
    map<_type, _type> m;
    vector<_type> x, y, z;
    int maxSize = 20;
    if (res.size() == 2)
    {
        vector<_type> bounds = 2 * og[0] + 2 * og[1];
        int boundX = bounds[0];
        int boundY = bounds[1];
        for (int i = -maxSize; i < maxSize; i++)
        {
            for (int j = -maxSize; j < maxSize; j++)
            {
                vector<_type> t = i * res.at(0) + j * res.at(1);
                if (-boundX < t[0] < boundX && -boundY < t[1] < boundY)
                {
                    x.push_back(t.at(0));
                    y.push_back(t.at(1));
                }
            }
        }
        //plt::plot(x, y, ".");
    }
    if (res.size() == 3)
    {
        vector<_type> bounds = 2 * og[0] + 2 * og[1];
        int boundX = bounds[0];
        int boundY = bounds[1];
        int boundZ = bounds[2];
        for (int i = -maxSize; i < maxSize; i++)
        {
            for (int j = -maxSize; j < maxSize; j++)
                for (int k = -maxSize; k < maxSize; j++)
                {
                    vector<_type> t = i * res.at(0) + j * res.at(1) + k * res.at(2);
                    if (-boundX < t[0] < boundX && -boundY < t[1] < boundY)
                    {
                        x.push_back(t.at(0));
                        y.push_back(t.at(1));
                        z.push_back(t.at(2));
                    }
                }
        }
    }
    /*plt::plot({ 0 }, { 0 }, "bo");
    plt::plot({ res.at(0).at(0) }, { res.at(0).at(1) }, "r+");
    plt::plot({ res.at(1).at(0) }, { res.at(1).at(1) }, "r+");
    plt::plot({ og.at(0).at(0) }, { og.at(0).at(1) }, "g*");
    plt::plot({ og.at(1).at(0) }, { og.at(1).at(1) }, "g*");*/
}
//https://tel.archives-ouvertes.fr/tel-01245066v2/document
vector<int> kleinSample(Matrix<int> &B, Matrix<_type> &GSO, float deviation, vector<int> &c)
{
    vector<int> v(c.size(), 0);
    std::random_device mch;
    std::default_random_engine generator(mch());

    double d, z;
    for (int i = B.size() - 1; i >= 0; i--)
    {
        double length = squaredLengthOfVector(GSO.at(i));
        d = dotProduct(c, GSO.at(i)) / length;
        double sigma = deviation / sqrt(length);
        std::normal_distribution<double> distribution(d, 1);
        z = round(distribution(generator));
        //std::cout << z << " " ;
        c = c - z * B[i];
        v = v + z * B[i];
    }
    //std::cout << std::endl;
    return v;
}

// todo discover
vector<_type> sample_gaussian(Matrix<_type> &B)
{
    vector<_type> a;
    a = (5 - rand() % 10) * B.back();
    for (auto b : B)
        a = a + (5 - rand() % 10) * b;
    return a;
}

bool reduce(vector<int> &p, Matrix<int> &L, vector<double> &lengths)
{
    double s = squaredLengthOfVector(p);
    for (size_t i = 0; i < L.size(); i++)
    {
        if (s >= lengths[i])
        {
            auto diff = p - L[i];
            double s2 = squaredLengthOfVector(diff);
            if (s2 < s)
            {
                p = diff;
                return true;
            }
        }
    }
    return false;
}

vector<int> gauss_reduce(vector<int> &v_new, Matrix<int> &L, Matrix<int> &S, vector<double> &lengths)
{
    bool cont = true;
    while (cont)
    {
        auto same = std::find_if(L.begin(), L.end(), [&v_new](vector<int> &v) { return v_new == v; });
        if (same != L.end())
        {
            return vector<int>(v_new.size());
        }
        // 2 раза считается разница, можно сохранить и сравнить с минимумом
        cont = reduce(v_new, L, lengths);
    }
    auto it = L.begin();
    auto it2 = lengths.begin();
    int i = 0;
    vector<int> a(v_new.size());
    while (it != L.end())
    {
        double v_new_length = squaredLengthOfVector(&v_new);
        vector<int> v = *it;
        if (lengths[i] > v_new_length)
        {
            a = v - v_new;
            if (squaredLengthOfVector(a) <= lengths[i])
            {
                S.push_back(a);
                it = L.erase(it);
                it2 = lengths.erase(it2);
            }
            else
            {
                ++it;
                ++it2, ++i;
            }
        }
        else
        {
            ++it;
            ++it2, ++i;
        }
    }
    return v_new;
}
// https://cseweb.ucsd.edu/~daniele/papers/Sieve.pdf
// c= 1000
vector<int> gaussSieve(Matrix<int> &B, float bound, ostream &o)
{
    Matrix<int> L, S;
    vector<int> v_new;
    Matrix<_type> gramSmidt = gSchmidt(B);
    vector<double> squarelengthsVector;
    int K = 0, iter = 0;
    //int c = KabatianskyLevenshteinC(B);
    map<int, int> _map;

    size_t max_number_of_collistions = 200;
    do
    {
        _map.insert(make_pair(iter, K));

        iter++;
        if (S.size() > 0)
        {
            v_new = S.back();
            S.pop_back();
        }
        else
        {
            vector<int> a(B[0].size(), 0);
            v_new = kleinSample(B, gramSmidt, 1, a);
        }
        v_new = gauss_reduce(v_new, L, S, squarelengthsVector);
        if (!v_new.size() ||
            std::count(v_new.begin(), v_new.end(), 0) == v_new.size())
        {
            K++;
        }
        else if (sqrt(squaredLengthOfVector(v_new)) < bound)
        {
            o << "early exit gauss iter : " << iter << "|\n";
            return v_new;
        }
        else
        {
            L.push_back(v_new);
            squarelengthsVector.push_back(squaredLengthOfVector(v_new));
        }
        max_number_of_collistions = std::max(max_number_of_collistions, L.size());
    } while (10 * K < max_number_of_collistions + 2000 || L.size() == 0);
    vector<int> shortestVector = L[0];
    int _a = 0, count = 0;
    for (int i = 1; i < L.size(); i++)
    {
        if (sqrt(squaredLengthOfVector(L[i])) <= bound)
        {
            count++;
        }
        if (squaredLengthOfVector(shortestVector) > squaredLengthOfVector(L[i]))
        {
            shortestVector = L[i];
            _a = i;
        }
    }
    cout << "count " << count << endl;
    cout << "gauss at : " << _map.at(_a) << endl;
    o << "gauss iter : " << iter << "|\n";
    return shortestVector;
}

vector<_type> randomInBall(const int dimm, float r)
{
    vector<_type> randVector(dimm);
    for (int i = 0; i < dimm; i++)
    {
        randVector[i] = (rand() % (int)(r + 1));
    }
    return (r * (rand() / double(RAND_MAX)) / sqrt(squaredLengthOfVector(randVector))) * randVector;
}
//https://crypto.stackexchange.com/questions/29661/how-to-find-the-value-of-a-vector-modulo-a-basis-in-lattice-based-cryptography/29701#29701
vector<_type> mod(vector<_type> vec, Matrix<_type> m)
{
    Matrix<_type> r(m.size(), vector<_type>(m.size(), 0));
    inverse(m, r);
    return m * roundV(r * vec);
}

void ListReduce(vector<int> &p, Matrix<int> &L, vector<double> &lengths)
{
    bool cont = true;
    while (cont)
    {
        auto same = std::find(L.begin(), L.end(), p);
        if (same != L.end())
        {
            p = vector<int>(p.size());
            return;
        }
        if (!reduce(p, L, lengths))
            return;
        /*auto f = std::find_if(L.begin(), L.end(), [&p](vector<int>& v) {
            return squaredLengthOfVector(&p) >= squaredLengthOfVector(&v) && squaredLengthOfVector(&(p - v)) < squaredLengthOfVector(&p);
            });
        if (f == L.end()) return;
        p = p - *f;*/
    }
}
//https://math.stackexchange.com/questions/396382/what-does-this-dollar-sign-over-arrow-in-function-mapping-mean
//https://eprint.iacr.org/2014/714.pdf
vector<int> ListSieve(Matrix<int> &B, float bound, ostream &o)
{
    Matrix<int> L;
    Matrix<_type> gramSmidt = gSchmidt(B);
    vector<int> a(B[0].size(), 0);
    vector<double> lengths;
    map<int, int> _map;
    size_t max_number_of_collistions = 200;
    int iter = 0;
    for (int i = 0, k = 0; 10*i < max_number_of_collistions + 2000 || L.size() == 0; k++)
    {
        iter++;
        vector<int> v = kleinSample(B, gramSmidt, 1, a);
        ListReduce(v, L, lengths);
        bool isZero = true;
        for (auto in : v)
        {
            if (in != 0)
            {
                isZero = false;
                break;
            }
        }
        _map.insert(make_pair(k, i));
        if (isZero)
        {
            i++;
        }
        else if (sqrt(squaredLengthOfVector(v)) < bound)
        {
            o << "early list iter : " << iter << "|\n";
            return v;
        }
        else
        {
            L.push_back(v);
            lengths.push_back(squaredLengthOfVector(v));
        }
        max_number_of_collistions = std::max(max_number_of_collistions, L.size());

    }
    vector<int> shortestVector = L[0];
    int _a = 0, count = 0;
    for (int i = 1; i < L.size(); i++)
    {
        if (sqrt(squaredLengthOfVector(L[i])) <= bound)
        {
            count++;
        }
        if (squaredLengthOfVector(shortestVector) > squaredLengthOfVector(L[i]))
        {
            shortestVector = L[i];
            _a = i;
        }
    }
    cout << "count " << count << endl;
    cout << "list at : " << _map.at(_a) << endl;
    o << "list iter : " << iter << "|\n";
    return shortestVector;
}

vector<int> sumVectorsWithCoeff(Matrix<int> &matrix, vector<int> &c)
{
    vector<int> a(c.size(), 0);
    for (size_t i = 0; i < c.size(); i++)
    {
        a = a + c[i] * matrix[i];
    }
    return a;
}

bool checkifBiggerByEachCoordinate(vector<int> &a, vector<int> &b)
{
    for (size_t i = 0; i < a.size(); i++)
    {
        if (abs(a[i]) > abs(b[i]))
            return false;
    }
    return true;
}

vector<int> LLLenumerate(Matrix<int> &matrix, ofstream &myfile)
{
    Matrix<int> enumeration;
    int n = matrix.size();
    int lim = pow(2, n * (n - 1) / 4.0);
    vector<int> c(n, 0);
    c[0] = 1;
    vector<int> shortest = sumVectorsWithCoeff(matrix, c);
    auto shortestL = squaredLengthOfVector(shortest);
    long long tmpL;
    vector<int> tmp;
    tmp = sumVectorsWithCoeff(matrix, c);
    if (!checkifBiggerByEachCoordinate(shortest, tmp))
    {
        tmpL = squaredLengthOfVector(tmp);
        if (tmpL > 0 && shortestL > tmpL)
        {
            shortestL = tmpL;
            shortest = tmp;
        }
    }
    for (int i = 0; i < n;)
    {
        int &a = c[i];
        if (a == -lim)
        {
            for (int j = 0; j < i; j++)
                c[j] = -lim;
            a++;
            i = 0;
            tmp = sumVectorsWithCoeff(matrix, c);
            if (!checkifBiggerByEachCoordinate(shortest, tmp))
            {
                tmpL = squaredLengthOfVector(tmp);
                if (tmpL > 0 && shortestL > tmpL)
                {
                    shortestL = tmpL;
                    shortest = tmp;
                }
            }
            // for (auto &bb : c)
            //     cout << bb;
            // cout << endl;
            // // enumeration.push_back(sumVectorsWithCoeff(matrix, c));
        }
        else if (a < lim)
        {
            a++;
            if (a == lim)
                i++;
            else
                i = 0;
            tmp = sumVectorsWithCoeff(matrix, c);
            if (!checkifBiggerByEachCoordinate(shortest, tmp))
            {
                tmpL = squaredLengthOfVector(tmp);
                if (tmpL > 0 && shortestL > tmpL)
                {
                    shortestL = tmpL;
                    shortest = tmp;
                }
            }
            // for (auto &bb : c)
            //     cout << bb;
            // cout << endl;
            // // enumeration.push_back(sumVectorsWithCoeff(matrix, c));
        }
        else if (a == lim)
        {
            i++;
        }
    }
    return shortest;
}

//https://web.eecs.umich.edu/~cpeikert/pubs/lattice-survey.pdf
Matrix<int> generateLattice(int dimm, int diff, int iter, bool print, ofstream &file)
{
    Matrix<int> m(dimm, vector<int>(dimm, 0));

    file << "---------------------------------------------\n\n"
         << "dimm: " << dimm << " iter: " << iter << " diff: " << diff << "|\n";
    int summ = 0;
    srand(time(0));
    file << '[';
    for (auto &v : m)
    {
        file << '[';
        for (auto &el : v)
        {
            float f = rand();
            el = f / RAND_MAX * (float)diff * 3;
            summ += el;
            file << el << ' ';
        }
        file << "]\n";
    }
    file << "]\n";
    file << " summ: " << summ << "\n\n";
    return m;
}
//void preprocessVoronoi(Matrix<int>& B)
//{
//    LLL(B);
//}
//
//vector<int> findRelevantHelper(Matrix<int>& B, int p, int n)
//{
//    vector<int> res(B.size(), 0);
//    for (size_t i = 0; i < n; i++)
//    {
//        if (p & 1)
//        {
//            res = res - B[i];
//        }
//        p = p >> 1;
//    }
//    for (auto& b : res)
//        b = b / 2;
//    return res;
//}
//
//vector<int> CVPP2V(vector<int>& t, Matrix<vector<int>>& Voronoi) {
//    int i = 0;
//    vector<int> t0(t.size()), ti(t.size());
//    while (find_if(Voronoi.begin(), Voronoi.end(), [&ti](auto& v) {
//        return v[0] == ti
//    }) == Voronoi.end()) {
//        float tmp = mu(ti, Voronoi[0][0]);
//        int maxi = 0;
//        for (size_t j = 1; j < Voronoi.size(); j++)
//        {
//            if (tmp < mu(ti,Voronoi[j][0])) {
//                maxi = j;
//            }
//        }
//        ti = ti - ti;
//        i++;
//    }
//
//    return t0 - ti;
//}
//vector<int> CVPP(vector<int>& t, Matrix<int>& B, Matrix<vector<int>>& Voronoi) {
//    vector<int> res(t.size(), 0), tp;
//    for (auto& v : B) {
//        res = res + mu(t, v) * v;
//    }
//    // not sure about this
//    int p = 0;
//    while(true) {
//        p++;
//        if (squaredLengthOfVector(t) < (squaredLengthOfVector(pow(2, p) * Voronoi[0][0]))) {
//            break;
//        }
//    }
//
//    for (int i = p - 1; i > 0; i--) {
//        tp = tp - CVPP2V(tp, pow(2, i - 1) * Voronoi);
//    }
//    return res - tp;
//}
//
//vector<int> rankReduceCVP(const vector<int>& t, Matrix<int>& B, Matrix<vector<int>>& Voronoi, int H) {
//    vector<int> res(t.size(), 0), v, closest, tmp;
//    int h = mu(t, B.at(Voronoi.size()));
//    float length, tmplength;
//    int i = h - H + 1;
//    res = CVPP(t - h * B.at(Voronoi.size()), Voronoi.back());
//    length = squaredLengthOfVector(res);
//    i++;
//    // todo is it correct?
//    for (; abs(i - h) < H; i++) {
//        v = CVPP(t - i * B.at(Voronoi.size()), Voronoi.back());
//        tmp = v - t;
//        tmplength = squaredLengthOfVector(tmp);
//        if (tmplength < length) {
//            length = tmplength;
//            res = tmp;
//        }
//    }
//    return res;
//}
//
//Matrix<vector<int>> findRelevant(Matrix<int>& B, Matrix<vector<int>>& Voronoi, int H)
//{
//    Matrix<vector<int>> res;
//    vector<int> t, c;
//    int l = pow(2, B.size());
//    // p - вектор из нелей и 1
//    for (int p = 1; p < l; p++)
//    {
//        t = findRelevantHelper(B, p, B.size());
//        c = rankReduceCVP(t, B, Voronoi, H);
//        t = 2 * (t - c);
//        c = inverse(t);
//        res.push_back({ t, c });
//    }
//    return res;
//}
//void removeNonRelevant(Matrix<vector<int>>& res)
//{
//    for (size_t i = 0; i < res.size(); i++)
//    {
//        for (size_t j = 0; j < res.size(); j++)
//        {
//            if (i != j && squaredLengthOfVector(res[j][0] - 0.5 * res[i][0]) <= squaredLengthOfVector(0.5 * res[i][0])) {
//                // debug whats in it
//                res.erase(res.begin() + i);
//            }
//        }
//
//    }
//
//}
//
//Matrix<vector<int>> computeVcell(Matrix<int>& B, Matrix<vector<int>>& Voronoi, int H)
//{
//    Matrix<vector<int>> res = findRelevant(B, Voronoi, H);
//    removeNonRelevant(res);
//    return res;
//}
//// https://cseweb.ucsd.edu/~pvoulgar/files/voronoi_full.pdf
//void basicVoronoiCell(Matrix<int>& B)
//{
//    preprocessVoronoi(B);
//    Matrix<vector<int>> Voronoi(1);
//    Matrix<int> newB;
//    Voronoi[0] = { B[0], inverse(B[0]) };
//    for (int i = 1; i < B.size(); i++)
//    {
//        for (size_t j = 0; j < i; j++)
//        {
//            newB.push_back(B[j]);
//        }
//        // nedd to remake to vec of vec of vec
//        Voronoi = computeVcell(newB, Voronoi, pow(2, 0.5 * i));
//    }
//}
// void printShortestVector(const plll::linalg::math_matrix<plll::arithmetic::Integer> & A,
//                          unsigned index, const plll::arithmetic::Integer & sqnorm)
// {
//     std::cout << "The currently shortest vector is " << A.row(index)
//               << " with squared norm " << sqnorm << " (Integer)\n";
// }
// void printShortestVector_LI(const plll::linalg::math_matrix<plll::arithmetic::NInt<long> > & A,
//                             unsigned index, const plll::arithmetic::NInt<long> & sqnorm)
// {
//     std::cout << "The currently shortest vector is " << A.row(index)
//               << " with squared norm " << sqnorm << " (NInt<long>)\n";
// }

plll::LatticeReduction::MinCallbackFunction_LI f_LI(ostream &file, vector<int> &r)
{
    return [&r, &file](const plll::linalg::math_matrix<plll::arithmetic::NInt<long>> &A,
                       unsigned index, const plll::arithmetic::NInt<long> &sqnorm) {
        auto i = A.row(index).enumerate();
        int iter = 0;
        while (i.has_current())
        {
            r[iter++] = plll::arithmetic::convert<int>(i.current());
            i.next();
        }
        file << "The currently shortest vector is " << A.row(index)
             << " with squared norm " << sqnorm << " (Integer)\n";
    };
}

plll::LatticeReduction::MinCallbackFunction f(ostream &file, vector<int> &r)
{
    return [&r, &file](const plll::linalg::math_matrix<plll::arithmetic::Integer> &A,
                       unsigned index, const plll::arithmetic::Integer &sqnorm) {
        auto i = A.row(index).enumerate();
        int iter = 0;
        while (i.has_current())
        {
            r[iter++] = plll::arithmetic::convert<int>(i.current());
            i.next();
        }
        file << "The currently shortest vector is " << A.row(index)
             << " with squared norm " << sqnorm << " (Integer)\n";
    };
}
vector<int> shortestVectorByVoronoi(Matrix<int> &B, int dimm, ostream &file)
{
    cout << "mat";
    vector<int> res(dimm);
    cout << "mat";
    plll::linalg::base_matrix<plll::arithmetic::Integer> mat(dimm, dimm);
    cout << "mat";

    plll::linalg::base_matrix<plll::arithmetic::Integer>::Enumerator iterr = mat.enumerate();
    cout << "mat";

    int i = 0, x, y;
    while (iterr.has_current())
    {
        x = i / dimm;
        y = i % dimm;
        iterr.current() = plll::arithmetic::Integer(B[x][y]);
        iterr.next();
        i++;
    }
    cout << mat << endl;
    plll::LatticeReduction lr;
    //cin >> mat;
    plll::linalg::math_matrix<plll::arithmetic::Integer> A(mat);
    lr.setLattice(A);
    lr.setSVPMode(plll::LatticeReduction::SVP_VoronoiCellSVP);
    cout << "mat";

    lr.setMinCallbackFunction(f(file, res), f_LI(file, res));
    lr.svp();
    file << "voronoi res " << lr.getLattice() << endl
         << endl;
    mat = lr.getLattice();
    iterr = mat.enumerate();
    for (int i = 0; i < dimm; i++)
    {
        res[i] = plll::arithmetic::convert<int>(iterr.current());
        iterr.next();
    }
    return res;
}

matrix_with_det readFromFile(ifstream &myfile)
{
    matrix_with_det res;
    string line;
    std::regex e("det.*");
    getline(myfile, line);
    // cout << line << endl;
    // while (!regex_match(line, e))
    // {
        if (myfile.eof())
        {
            res.det = 0;
            return res;
        }
    //     getline(myfile, line);
    //     cout << line << "k" << endl;
    // }
    std::regex e2("\\d+");
    std::smatch sm;

    // std::regex_search(line, sm, e2);
    int det = 3;
    // line = sm.suffix().str();
    // cout << "\ndet " << det;
    // std::regex_search(line, sm, e2);
    int dimm = 3;

    // std::regex_search(line, sm, e2);
    // int det = stoi(sm.str());
    // line = sm.suffix().str();
    // cout << "\ndet " << det;
    // std::regex_search(line, sm, e2);
    // int dimm = stoi(sm.str());

    Matrix<int> matr(dimm, vector<int>(dimm));
    cout << "\ndimm " << dimm;

    for (int i = 0; i < dimm; i++)
    {
        getline(myfile, line);
        int j = 0;
        while (std::regex_search(line, sm, e2))
        {
            matr[i][j++] = stoi(sm.str());
            line = sm.suffix().str();
        }
        cout << endl;
    }
    printVS(matr);
    res.det = det;
    res.mat = matr;
    return res;
}

void printResVector(vector<int> &v, string method, ostream &o)
{
    o << endl
      << method << " size: " << sqrt(squaredLengthOfVector(v)) << endl
      << "[ ";
    for (auto &a : v)
    {
        o << a << " ";
    }
    o << "] " << endl;
}

int main()
{

    ofstream myfile;

    ifstream datafile;
    myfile.open("example.txt");

    datafile.open("det3.txt");
    matrix_with_det md = readFromFile(datafile);
    cout << md.det;

    while (md.det != 0)
    {
        cout << "det" << md.det;

        Matrix<int> matrix = md.mat;
        Matrix<int> matrix2 = matrix;
        Matrix<int> matrix3 = matrix;
        Matrix<int> original(matrix2);
        Matrix<float> gramSmidt = gSchmidt(matrix);

        cout << "starting voronoi" << endl;
        auto t1 = std::chrono::high_resolution_clock::now();
        int dimm = matrix.size();
        cout << "t";
        auto vector_voronoi = shortestVectorByVoronoi(matrix, matrix.size(), myfile);
        auto t2 = std::chrono::high_resolution_clock::now();
        auto duration = std::chrono::duration_cast<sec_type>(t2 - t1).count();
        myfile << "voronoi duration " << duration << endl;
        myfile << "\n\n";

        float bound = 1;
        // bound = 1;
        cout << "starting gauss" << endl;
        t1 = std::chrono::high_resolution_clock::now();
        vector<int> sv = gaussSieve(matrix2, bound, myfile);
        t2 = std::chrono::high_resolution_clock::now();
        duration = std::chrono::duration_cast<sec_type>(t2 - t1).count();
        myfile << "gauss duration: " << duration << endl;
        myfile << "\n\n";
        cout << "f\n\n";

        cout << "starting list" << endl;
        t1 = std::chrono::high_resolution_clock::now();
        vector<int> v3 = ListSieve(matrix3, bound, myfile);
        // auto vector_voronoi = v3;
        t2 = std::chrono::high_resolution_clock::now();
        duration = std::chrono::duration_cast<sec_type>(t2 - t1).count();
        myfile << "list duration: " << duration << endl;

        bound = 1.01 * (sqrt(dimm)) * pow(abs(md.det), 1.0 / dimm);
        myfile << "bound " << bound << endl;
        t1 = std::chrono::high_resolution_clock::now();
        vector<int> gauss_with_bound = gaussSieve(matrix2, bound, myfile);
        t2 = std::chrono::high_resolution_clock::now();
        duration = std::chrono::duration_cast<sec_type>(t2 - t1).count();
        myfile << "gauss with bound duration: " << duration << endl;
        myfile << "\n\n";
        cout << "f\n\n";

        cout << "starting bound list" << endl;
        t1 = std::chrono::high_resolution_clock::now();
        vector<int> list_with_bound = ListSieve(matrix3, bound, myfile);
        // auto vector_voronoi = v3;
        t2 = std::chrono::high_resolution_clock::now();
        duration = std::chrono::duration_cast<sec_type>(t2 - t1).count();
        myfile << "list with bound duration: " << duration;

        if (squaredLengthOfVector(list_with_bound) != squaredLengthOfVector(gauss_with_bound))
        {
            myfile << "found different norms of vectors";
            printResVector(list_with_bound, "list with bound", myfile);
            printResVector(gauss_with_bound, "gauss with bound", myfile);
        }
        else
        {
            myfile << endl
                   << "shortest with bound norm" << (long)squaredLengthOfVector(gauss_with_bound) << endl;
            printVec(gauss_with_bound, myfile);
        }

        if (squaredLengthOfVector(vector_voronoi) != squaredLengthOfVector(sv) || squaredLengthOfVector(sv) != squaredLengthOfVector(vector_voronoi))
        {
            myfile << "found different norms of vectors";

            printResVector(vector_voronoi, "voronoi", myfile);
            printResVector(sv, "gauss", myfile);
            printResVector(v3, "list", myfile);
        }
        else
        {
            myfile << endl
                   << "shortest with norm" << (long)squaredLengthOfVector(sv) << endl;
            printVec(sv, myfile);
        }
        myfile << "---------------------------------------------------------------------\n\n";
        md = readFromFile(datafile);
    }

    // myfile << dimm << " " << diff << " " << iter << endl;

    //         }
    //     }
    // }
    // }
    // catch (exception e)
    // {
    //     cout << e.what();
    // }
    myfile.close();
    return 0;
}


#include <iostream>
#include <vector>
#include <fstream>
using namespace std;
#include <boost/timer.hpp>

// for sophus
#include <sophus/se3.h>
using Sophus::SE3;

// for eigen
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace Eigen;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

using namespace cv;

/**********************************************
* 本程序演示了单目相机在已知轨迹下的稠密深度估计
* 使用极线搜索 + NCC 匹配的方式，与书本的 13.2 节对应
* 请注意本程序并不完美，你完全可以改进它——我其实在故意暴露一些问题。
***********************************************/

// ------------------------------------------------------------------
// parameters
const int boarder = 20; 	// 边缘宽度
const int width = 640;  	// 宽度
const int height = 480;  	// 高度
const double fx = 481.2f;	// 相机内参
const double fy = -480.0f;
const double cx = 319.5f;
const double cy = 239.5f;
const int ncc_window_size = 2;	// NCC 取的窗口半宽度
const int ncc_area = (2*ncc_window_size+1)*(2*ncc_window_size+1); // NCC窗口面积
const double min_cov = 0.1;	// 收敛判定：最小方差
const double max_cov = 10;	// 发散判定：最大方差

// ------------------------------------------------------------------
// 重要的函数
// 从 REMODE 数据集读取数据
bool readDatasetFiles(
    const string& path,
    vector<string>& color_image_files,
    vector<SE3>& poses
);

// 根据新的图像更新深度估计
bool update(
    const Mat& ref,
    const Mat& curr,
    const SE3& T_C_R,
    Mat& depth,
    Mat& depth_cov
);

// 极线搜索
bool epipolarSearch(
    const Mat& ref,
    const Mat& curr,
    const SE3& T_C_R,
    const Vector2d& pt_ref,
    const double& depth_mu,
    const double& depth_cov,
    Vector2d& pt_curr
);

// 更新深度滤波器
bool updateDepthFilter(
    const Vector2d& pt_ref,
    const Vector2d& pt_curr,
    const SE3& T_C_R,
    Mat& depth,
    Mat& depth_cov
);

// 计算 NCC 评分
double NCC( const Mat& ref, const Mat& curr, const Vector2d& pt_ref, const Vector2d& pt_curr );

// 双线性灰度插值
inline double getBilinearInterpolatedValue( const Mat& img, const Vector2d& pt ) {
    uchar* d = & img.data[ int(pt(1,0))*img.step+int(pt(0,0)) ];
    double xx = pt(0,0) - floor(pt(0,0));
    double yy = pt(1,0) - floor(pt(1,0));
    return  (( 1-xx ) * ( 1-yy ) * double(d[0]) +
            xx* ( 1-yy ) * double(d[1]) +
            ( 1-xx ) *yy* double(d[img.step]) +
            xx*yy*double(d[img.step+1]))/255.0;
}

// ------------------------------------------------------------------
// 一些小工具
// 显示估计的深度图
bool plotDepth( const Mat& depth );

// 像素到相机坐标系
inline Vector3d px2cam ( const Vector2d px ) {
    return Vector3d (
        (px(0,0) - cx)/fx,
        (px(1,0) - cy)/fy,
        1
    );
}

// 相机坐标系到像素
inline Vector2d cam2px ( const Vector3d p_cam ) {
    return Vector2d (
        p_cam(0,0)*fx/p_cam(2,0) + cx,
        p_cam(1,0)*fy/p_cam(2,0) + cy
    );
}

// 检测一个点是否在图像边框内
inline bool inside( const Vector2d& pt ) {
    return pt(0,0) >= boarder && pt(1,0)>=boarder
        && pt(0,0)+boarder<width && pt(1,0)+boarder<=height;
}

// 显示极线匹配
void showEpipolarMatch( const Mat& ref, const Mat& curr, const Vector2d& px_ref, const Vector2d& px_curr );

// 显示极线
void showEpipolarLine( const Mat& ref, const Mat& curr, const Vector2d& px_ref, const Vector2d& px_min_curr, const Vector2d& px_max_curr );
// ------------------------------------------------------------------

int main( int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"Usage: dense_mapping path_to_test_dataset"<<endl;
        return -1;
    }

    // 从数据集读取数据
    vector<string> color_image_files;
    vector<SE3> poses_TWC;
    bool ret = readDatasetFiles( argv[1], color_image_files, poses_TWC );
    if ( ret==false )
    {
        cout<<"Reading image files failed!"<<endl;
        return -1;
    }
    cout<<"read total "<<color_image_files.size()<<" files."<<endl;

    // 第一张图
    Mat ref = imread( color_image_files[0], 0 );                // gray-scale image
    SE3 pose_ref_TWC = poses_TWC[0];
    double init_depth   = 3.0;    // 深度初始值
    double init_cov2    = 3.0;    // 方差初始值
    Mat depth( height, width, CV_64F, init_depth );             // 深度图
    Mat depth_cov( height, width, CV_64F, init_cov2 );          // 深度图方差

    for ( int index=1; index<color_image_files.size(); index++ )
    {
        cout<<"*** loop "<<index<<" ***"<<endl;
        Mat curr = imread( color_image_files[index], 0 );
        if (curr.data == nullptr) continue;
        SE3 pose_curr_TWC = poses_TWC[index];
        SE3 pose_T_C_R = pose_curr_TWC.inverse() * pose_ref_TWC; // 坐标转换关系： T_C_W * T_W_R = T_C_R
        update( ref, curr, pose_T_C_R, depth, depth_cov );
        plotDepth( depth );
        imshow("image", curr);
        waitKey(1);
    }

    cout<<"estimation returns, saving depth map ..."<<endl;
    imwrite( "depth.png", depth );
    cout<<"done."<<endl;

    return 0;
}

bool readDatasetFiles(
    const string& path,
    vector< string >& color_image_files,
    std::vector<SE3>& poses
)
{
    ifstream fin( path+"/first_200_frames_traj_over_table_input_sequence.txt");
    if ( !fin ) return false;

    while ( !fin.eof() )
    {
		// 数据格式：图像文件名 tx, ty, tz, qx, qy, qz, qw ，注意是 TWC 而非 TCW
        string image;
        fin>>image;
        double data[7];
        for ( double& d:data ) fin>>d;

        color_image_files.push_back( path+string("/images/")+image );
        poses.push_back(
            SE3( Quaterniond(data[6], data[3], data[4], data[5]),
                 Vector3d(data[0], data[1], data[2]))
        );
        if ( !fin.good() ) break;
    }
    return true;
}

// 对整个深度图进行更新
bool update(const Mat& ref, const Mat& curr, const SE3& T_C_R, Mat& depth, Mat& depth_cov )
{
#pragma omp parallel for
    for ( int x=boarder; x<width-boarder; x++ )
#pragma omp parallel for
        for ( int y=boarder; y<height-boarder; y++ )
        {
			// 遍历每个像素
            if ( depth_cov.ptr<double>(y)[x] < min_cov || depth_cov.ptr<double>(y)[x] > max_cov ) // 深度已收敛或发散
                continue;
            // 在极线上搜索 (x,y) 的匹配
            Vector2d pt_curr;
            bool ret = epipolarSearch (
                ref,
                curr,
                T_C_R,
                Vector2d(x,y),
                depth.ptr<double>(y)[x],
                sqrt(depth_cov.ptr<double>(y)[x]),
                pt_curr
            );

            if ( ret == false ) // 匹配失败
                continue;

			// 取消该注释以显示匹配
            // showEpipolarMatch( ref, curr, Vector2d(x,y), pt_curr );

            // 匹配成功，更新深度图
            updateDepthFilter( Vector2d(x,y), pt_curr, T_C_R, depth, depth_cov );
        }
}

// 极线搜索
// 方法见书 13.2 13.3 两节
bool epipolarSearch(
    const Mat& ref, const Mat& curr,
    const SE3& T_C_R, const Vector2d& pt_ref,
    const double& depth_mu, const double& depth_cov,
    Vector2d& pt_curr )
{
    Vector3d f_ref = px2cam( pt_ref );
    f_ref.normalize();
    Vector3d P_ref = f_ref*depth_mu;	// 参考帧的 P 向量

    Vector2d px_mean_curr = cam2px( T_C_R*P_ref ); // 按深度均值投影的像素
    double d_min = depth_mu-3*depth_cov, d_max = depth_mu+3*depth_cov;
    if ( d_min<0.1 ) d_min = 0.1;
    Vector2d px_min_curr = cam2px( T_C_R*(f_ref*d_min) );	// 按最小深度投影的像素
    Vector2d px_max_curr = cam2px( T_C_R*(f_ref*d_max) );	// 按最大深度投影的像素

    Vector2d epipolar_line = px_max_curr - px_min_curr;	// 极线（线段形式）
    Vector2d epipolar_direction = epipolar_line;		// 极线方向
    epipolar_direction.normalize();
    double half_length = 0.5*epipolar_line.norm();	// 极线线段的半长度
    if ( half_length>100 ) half_length = 100;   // 我们不希望搜索太多东西

	// 取消此句注释以显示极线（线段）
    // showEpipolarLine( ref, curr, pt_ref, px_min_curr, px_max_curr );

    // 在极线上搜索，以深度均值点为中心，左右各取半长度
    double best_ncc = -1.0;
    Vector2d best_px_curr;
    for ( double l=-half_length; l<=half_length; l+=0.7 )  // l+=sqrt(2)
    {
        Vector2d px_curr = px_mean_curr + l*epipolar_direction;  // 待匹配点
        if ( !inside(px_curr) )
            continue;
        // 计算待匹配点与参考帧的 NCC
        double ncc = NCC( ref, curr, pt_ref, px_curr );
        if ( ncc>best_ncc )
        {
            best_ncc = ncc;
            best_px_curr = px_curr;
        }
    }
    if ( best_ncc < 0.85f )      // 只相信 NCC 很高的匹配
        return false;
    pt_curr = best_px_curr;
    return true;
}

double NCC (
    const Mat& ref, const Mat& curr,
    const Vector2d& pt_ref, const Vector2d& pt_curr
)
{
    // 零均值-归一化互相关
    // 先算均值
    double mean_ref = 0, mean_curr = 0;
    vector<double> values_ref, values_curr; // 参考帧和当前帧的均值
    for ( int x=-ncc_window_size; x<=ncc_window_size; x++ )
        for ( int y=-ncc_window_size; y<=ncc_window_size; y++ )
        {
            double value_ref = double(ref.ptr<uchar>( int(y+pt_ref(1,0)) )[ int(x+pt_ref(0,0)) ])/255.0;
            mean_ref += value_ref;

            double value_curr = getBilinearInterpolatedValue( curr, pt_curr+Vector2d(x,y) );
            mean_curr += value_curr;

            values_ref.push_back(value_ref);
            values_curr.push_back(value_curr);
        }

    mean_ref /= ncc_area;
    mean_curr /= ncc_area;

	// 计算 Zero mean NCC
    double numerator = 0, demoniator1 = 0, demoniator2 = 0;
    for ( int i=0; i<values_ref.size(); i++ )
    {
        double n = (values_ref[i]-mean_ref) * (values_curr[i]-mean_curr);
        numerator += n;
        demoniator1 += (values_ref[i]-mean_ref)*(values_ref[i]-mean_ref);
        demoniator2 += (values_curr[i]-mean_curr)*(values_curr[i]-mean_curr);
    }
    return numerator / sqrt( demoniator1*demoniator2+1e-10 );   // 防止分母出现零
}

bool updateDepthFilter(
    const Vector2d& pt_ref,
    const Vector2d& pt_curr,
    const SE3& T_C_R,
    Mat& depth,
    Mat& depth_cov
)
{
    // 我是一只喵
    // 不知道这段还有没有人看
    // 用三角化计算深度
    SE3 T_R_C = T_C_R.inverse();
    Vector3d f_ref = px2cam( pt_ref );
    f_ref.normalize();
    Vector3d f_curr = px2cam( pt_curr );
    f_curr.normalize();

    // 方程
    // d_ref * f_ref = d_cur * ( R_RC * f_cur ) + t_RC
    // => [ f_ref^T f_ref, -f_ref^T f_cur ] [d_ref] = [f_ref^T t]
    //    [ f_cur^T f_ref, -f_cur^T f_cur ] [d_cur] = [f_cur^T t]
    // 二阶方程用克莱默法则求解并解之
    Vector3d t = T_R_C.translation();
    Vector3d f2 = T_R_C.rotation_matrix() * f_curr;
    Vector2d b = Vector2d ( t.dot ( f_ref ), t.dot ( f2 ) );
    double A[4];
    A[0] = f_ref.dot ( f_ref );
    A[2] = f_ref.dot ( f2 );
    A[1] = -A[2];
    A[3] = - f2.dot ( f2 );
    double d = A[0]*A[3]-A[1]*A[2];
    Vector2d lambdavec =
        Vector2d (  A[3] * b ( 0,0 ) - A[1] * b ( 1,0 ),
                    -A[2] * b ( 0,0 ) + A[0] * b ( 1,0 )) /d;
    Vector3d xm = lambdavec ( 0,0 ) * f_ref;
    Vector3d xn = t + lambdavec ( 1,0 ) * f2;
    Vector3d d_esti = ( xm+xn ) / 2.0;  // 三角化算得的深度向量
    double depth_estimation = d_esti.norm();   // 深度值

    // 计算不确定性（以一个像素为误差）
    Vector3d p = f_ref*depth_estimation;
    Vector3d a = p - t;
    double t_norm = t.norm();
    double a_norm = a.norm();
    double alpha = acos( f_ref.dot(t)/t_norm );
    double beta = acos( -a.dot(t)/(a_norm*t_norm));
    double beta_prime = beta + atan(1/fx);
    double gamma = M_PI - alpha - beta_prime;
    double p_prime = t_norm * sin(beta_prime) / sin(gamma);
    double d_cov = p_prime - depth_estimation;
    double d_cov2 = d_cov*d_cov;

    // 高斯融合
    double mu = depth.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ];
    double sigma2 = depth_cov.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ];

    double mu_fuse = (d_cov2*mu+sigma2*depth_estimation) / ( sigma2+d_cov2);
    double sigma_fuse2 = ( sigma2 * d_cov2 ) / ( sigma2 + d_cov2 );

    depth.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ] = mu_fuse;
    depth_cov.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ] = sigma_fuse2;

    return true;
}

// 后面这些太简单我就不注释了（其实是因为懒）
bool plotDepth(const Mat& depth)
{
    imshow( "depth", depth*0.4 );
    waitKey(1);
}

void showEpipolarMatch(const Mat& ref, const Mat& curr, const Vector2d& px_ref, const Vector2d& px_curr)
{
    Mat ref_show, curr_show;
    cv::cvtColor( ref, ref_show, CV_GRAY2BGR );
    cv::cvtColor( curr, curr_show, CV_GRAY2BGR );

    cv::circle( ref_show, cv::Point2f(px_ref(0,0), px_ref(1,0)), 5, cv::Scalar(0,0,250), 2);
    cv::circle( curr_show, cv::Point2f(px_curr(0,0), px_curr(1,0)), 5, cv::Scalar(0,0,250), 2);

    imshow("ref", ref_show );
    imshow("curr", curr_show );
    waitKey(1);
}

void showEpipolarLine(const Mat& ref, const Mat& curr, const Vector2d& px_ref, const Vector2d& px_min_curr, const Vector2d& px_max_curr)
{

    Mat ref_show, curr_show;
    cv::cvtColor( ref, ref_show, CV_GRAY2BGR );
    cv::cvtColor( curr, curr_show, CV_GRAY2BGR );

    cv::circle( ref_show, cv::Point2f(px_ref(0,0), px_ref(1,0)), 5, cv::Scalar(0,255,0), 2);
    cv::circle( curr_show, cv::Point2f(px_min_curr(0,0), px_min_curr(1,0)), 5, cv::Scalar(0,255,0), 2);
    cv::circle( curr_show, cv::Point2f(px_max_curr(0,0), px_max_curr(1,0)), 5, cv::Scalar(0,255,0), 2);
    cv::line( curr_show, Point2f(px_min_curr(0,0), px_min_curr(1,0)), Point2f(px_max_curr(0,0), px_max_curr(1,0)), Scalar(0,255,0), 1);

    imshow("ref", ref_show );
    imshow("curr", curr_show );
    waitKey(1);
}


// C++ includes
#include <iostream>
using namespace std;

// Eigen includes
#include <Eigen/Core>
using namespace Eigen;

// autodiff include
#include <autodiff/reverse.hpp>
#include <autodiff/reverse/eigen.hpp>
using namespace autodiff;

// The scalar function for which the gradient is needed
var f(const VectorXvar& x)
{
    return sqrt(x.cwiseProduct(x).sum()); // sqrt(sum([x(i) * x(i) for i = 1:5]))
}

int main()
{
    VectorXvar x(5);     // the input vector x with 5 variables
    x << 1, 2, 3, 4, 5;  // x = [1, 2, 3, 4, 5]

    var u = f(x);  // the output variable u

    VectorXd g;  // the gradient vector to be computed in method `hessian`
    MatrixXd H = hessian(u, x, g);  // evaluate the Hessian matrix H and the gradient vector g of u

    cout << "u = " << u << endl;    // print the evaluated output variable u
    cout << "g = \n" << g << endl;  // print the evaluated gradient vector of u
    cout << "H = \n" << H << endl;  // print the evaluated Hessian matrix of u
}