#pragma once
#include <Eigen/Core>
#include <numeric>

namespace mtao::linear_algebra {
    // M is matrix, B is initial vector, Q is the basis for the basis
    // N is the dimension of the subspace

    namespace internal {
        template <typename Derived, typename BDerived, typename VDerived, typename TDerived>
            auto lanczos(const Eigen::MatrixBase<Derived>& M, const Eigen::MatrixBase<BDerived>& v1, Eigen::PlainObjectBase<VDerived>& V, Eigen::PlainObjectBase<TDerived>& T) {
                const int N = T.rows();
                using Scalar = typename Derived::Scalar;
                constexpr Scalar eps = std::numeric_limits<Scalar>::epsilon();
                using Vec = Eigen::Matrix<Scalar,Derived::RowsAtCompileTime, 1>;

                V.setZero();
                T.setZero();

                V.col(0) = v1;
                Vec w;

                {// first iteration
                    auto v = V.col(0);
                    w = M * v;
                    const Scalar& a = T(0,0) = w.dot(v);

                    w -= a * v;
                }
                for(int j = 1; j < N; ++j) {
                    const Scalar& b = T(j,j-1) = T(j-1,j) = w.norm();
                    if(b < eps) {
                        return;
                    }
                    auto vjm = V.col(j-1);
                    auto vj = V.col(j) = w/b;
                    w = M * vj;
                    const Scalar& a = T(j,j) = w.dot(vj);
                    w -= a * vj + b * vjm;
                }
            }
    }

    template <typename Derived, typename BDerived>
        auto lanczos(const Eigen::MatrixBase<Derived>& M, const Eigen::MatrixBase<BDerived>& B, int N) {
            assert(M.rows() == M.cols());

            using Scalar = typename Derived::Scalar;
            using Mat = Eigen::Matrix<Scalar,Derived::RowsAtCompileTime,Eigen::Dynamic>;
            using DMat = Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>;

            Mat Q(M.rows(),N);
            DMat H(N,N);

            internal::lanczos(M,B,Q,H);
            return std::make_tuple(Q,H);
        }
    template <int N, typename Derived, typename BDerived>
        auto lanczos(const Eigen::MatrixBase<Derived>& M, const Eigen::MatrixBase<BDerived>& B) {
            assert(M.rows() == M.cols());

            using Scalar = typename Derived::Scalar;
            using Mat = Eigen::Matrix<Scalar,Eigen::Dynamic,N>;
            using HMat = Eigen::Matrix<Scalar,N,N>;

            Mat Q(M.rows(),N);
            HMat H(N,N);
            internal::lanczos(M,B,Q,H);
            return std::make_tuple(Q,H);
        }
    template <typename Derived>
        auto lanczos(const Eigen::MatrixBase<Derived>& M, int N) {
            using Scalar = typename Derived::Scalar;
            auto B = Eigen::Matrix<Scalar,Derived::RowsAtCompileTime, 1>::Random(M.rows()) ;
            return lanczos(M,B,N);
        }
    template <int N, typename Derived>
        auto lanczos(const Eigen::MatrixBase<Derived>& M) {
            using Scalar = typename Derived::Scalar;
            auto B = Eigen::Matrix<Scalar,Derived::RowsAtCompileTime, 1>::Random(M.rows()) ;
            return lanczos<N>(M,B);
        }
}


/*****************************************************************************/
/*  Copyright (c) 2015, Karl Pauwels                                         */
/*  All rights reserved.                                                     */
/*                                                                           */
/*  Redistribution and use in source and binary forms, with or without       */
/*  modification, are permitted provided that the following conditions       */
/*  are met:                                                                 */
/*                                                                           */
/*  1. Redistributions of source code must retain the above copyright        */
/*  notice, this list of conditions and the following disclaimer.            */
/*                                                                           */
/*  2. Redistributions in binary form must reproduce the above copyright     */
/*  notice, this list of conditions and the following disclaimer in the      */
/*  documentation and/or other materials provided with the distribution.     */
/*                                                                           */
/*  3. Neither the name of the copyright holder nor the names of its         */
/*  contributors may be used to endorse or promote products derived from     */
/*  this software without specific prior written permission.                 */
/*                                                                           */
/*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS      */
/*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT        */
/*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR    */
/*  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT     */
/*  HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,   */
/*  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT         */
/*  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,    */
/*  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY    */
/*  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT      */
/*  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE    */
/*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.     */
/*****************************************************************************/

#include <iostream>
#include <iomanip>
#include <normal_equations.h>
#undef Success
#include <Eigen/Dense>

namespace pose {

NormalEquations::NormalEquations() {
  _A.resize(36, 0);
  _B.resize(6, 0);
  _dTdR.resize(6, 0);
}

void NormalEquations::reset() {
  std::fill(_A.begin(), _A.end(), 0);
  std::fill(_B.begin(), _B.end(), 0);
  std::fill(_dTdR.begin(), _dTdR.end(), 0);
}

double NormalEquations::squaredNormDeltaT() const {
  return (_dTdR.at(0) * _dTdR.at(0) + _dTdR.at(1) * _dTdR.at(1) +
          _dTdR.at(1) * _dTdR.at(1));
}

void NormalEquations::compose(const float *CO, const float *CD) {

  _A[0] = CO[0];
  _A[1] = 0.0;
  _A[2] = CO[1];
  _A[3] = CO[2];
  _A[4] = CO[3];
  _A[5] = CO[4];
  _A[6] = 0.0;
  _A[7] = CO[0];
  _A[8] = CO[5];
  _A[9] = CO[6];
  _A[10] = -CO[2];
  _A[11] = CO[7];
  _A[12] = CO[1];
  _A[13] = CO[5];
  _A[14] = CO[8];
  _A[15] = CO[9];
  _A[16] = CO[10];
  _A[17] = 0.0;
  _A[18] = CO[2];
  _A[19] = CO[6];
  _A[20] = CO[9];
  _A[21] = CO[11];
  _A[22] = CO[12];
  _A[23] = CO[13];
  _A[24] = CO[3];
  _A[25] = -CO[2];
  _A[26] = CO[10];
  _A[27] = CO[12];
  _A[28] = CO[14];
  _A[29] = CO[15];
  _A[30] = CO[4];
  _A[31] = CO[7];
  _A[32] = 0.0;
  _A[33] = CO[13];
  _A[34] = CO[15];
  _A[35] = CO[16];

  _A[0] += CD[0];
  _A[1] += CD[1];
  _A[2] += CD[2];
  _A[3] += CD[3];
  _A[4] += CD[4];
  _A[5] += CD[5];
  _A[6] += CD[1];
  _A[7] += CD[6];
  _A[8] += CD[7];
  _A[9] += CD[8];
  _A[10] += CD[9];
  _A[11] += CD[10];
  _A[12] += CD[2];
  _A[13] += CD[7];
  _A[14] += CD[11];
  _A[15] += CD[12];
  _A[16] += CD[13];
  _A[17] += CD[14];
  _A[18] += CD[3];
  _A[19] += CD[8];
  _A[20] += CD[12];
  _A[21] += CD[15];
  _A[22] += CD[16];
  _A[23] += CD[17];
  _A[24] += CD[4];
  _A[25] += CD[9];
  _A[26] += CD[13];
  _A[27] += CD[16];
  _A[28] += CD[18];
  _A[29] += CD[19];
  _A[30] += CD[5];
  _A[31] += CD[10];
  _A[32] += CD[14];
  _A[33] += CD[17];
  _A[34] += CD[19];
  _A[35] += CD[20];

  for (int i = 0; i < 6; i++)
    _B[i] = CO[17 + i] + CD[21 + i];
}

void NormalEquations::solve(float *dTdR) {

  Eigen::Map<Eigen::Matrix<double, 6, 6> > A(_A.data());
  Eigen::Map<Eigen::Matrix<double, 6, 1> > B(_B.data());
  Eigen::Map<Eigen::Matrix<double, 6, 1> > double_dTdR(_dTdR.data());

  double_dTdR = A.ldlt().solve(B);

  Eigen::Map<Eigen::Matrix<float, 6, 1> > float_dTdR(dTdR);
  float_dTdR = double_dTdR.cast<float>();
}

void NormalEquations::preCondition() {
  for (auto &it : _A)
    it *= 1.0e-7;
}

void NormalEquations::show() const {
  for (int row = 0; row < 6; row++) {
    std::cout << std::scientific;
    std::cout.precision(3);
    for (int col = 0; col < 6; col++)
      std::cout << std::setw(10) << _A.at(row * 6 + col) << " ";
    std::cout << std::fixed;
    std::cout.precision(6);
    std::cout << ((row == 2) ? "X " : "  ") << std::setw(9) << _dTdR.at(row);
    std::cout << std::scientific;
    std::cout.precision(3);
    std::cout << ((row == 2) ? " = " : "   ") << std::setw(10) << _B.at(row)
              << std::endl;
  }
}
}


//
// Created by Amir Masoud Abdol on 2019-04-25.
//

#include <algorithm>
#include <cmath>

#include <spdlog/spdlog.h>
#include <fmt/core.h>

#include "sam.h"

#include "MetaAnalysis.h"
#include "Journal.h"
#include "TestStrategy.h"
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/students_t.hpp>
#include <boost/math/distributions/non_central_t.hpp>
#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/binomial.hpp>

#include <mlpack/methods/linear_regression/linear_regression.hpp>

using namespace std;
using namespace sam;

MetaAnalysis::~MetaAnalysis(){

}

std::unique_ptr<MetaAnalysis> MetaAnalysis::build(std::string name) {

  spdlog::debug("Building a Meta Analysis Method");

  if (name == "FixedEffectEstimator") {
    return std::make_unique<FixedEffectEstimator>();
  }else if (name == "RandomEffectEstimator") {
    return std::make_unique<RandomEffectEstimator>();
  }else if (name == "EggersTestEstimator") {
    return std::make_unique<EggersTestEstimator>();
  }else if (name == "TestOfObsOverExptSig") {
    return std::make_unique<TestOfObsOverExptSig>();
  }else if (name == "TrimAndFill") {
    return std::make_unique<TrimAndFill>();
  }else if (name == "RankCorrelation") {
    return std::make_unique<RankCorrelation>();
  }else{
    spdlog::critical("Invalid Meta Analysis Strategy.");
    exit(1);
  }
}

std::unique_ptr<MetaAnalysis> MetaAnalysis::build(const json &config) {
  if (config["name"] == "FixedEffectEstimator") {
    return std::make_unique<FixedEffectEstimator>();
  }else if (config["name"] == "RandomEffectEstimator") {

    auto p = config.get<RandomEffectEstimator::Parameters>();
    return std::make_unique<RandomEffectEstimator>(p);

  }else if (config["name"] == "EggersTestEstimator") {
    auto p = config.get<EggersTestEstimator::Parameters>();
    return std::make_unique<EggersTestEstimator>(p);

  }else if (config["name"] == "TestOfObsOverExptSig") {
    auto p = config.get<TestOfObsOverExptSig::Parameters>();
    return std::make_unique<TestOfObsOverExptSig>(p);

  }else if (config["name"] == "TrimAndFill") {
    auto p = config.get<TrimAndFill::Parameters>();
    return std::make_unique<TrimAndFill>(p);

  }else if (config["name"] == "RankCorrelation") {
    auto p = config.get<RankCorrelation::Parameters>();
    return std::make_unique<RankCorrelation>(p);

  }else{
    spdlog::critical("Invalid Meta Analysis Strategy.");
    exit(1);
  }
}

std::vector<std::string> MetaAnalysis::Columns(std::string name) {
  if (name == "FixedEffectEstimator") {
    return FixedEffectEstimator::ResultType::Columns();
  }else if (name == "RandomEffectEstimator") {
    return RandomEffectEstimator::ResultType::Columns();
  }else if (name == "EggersTestEstimator") {
    return EggersTestEstimator::ResultType::Columns();
  }else if (name == "TestOfObsOverExptSig") {
    return TestOfObsOverExptSig::ResultType::Columns();
  }else if (name == "TrimAndFill") {
    return TrimAndFill::ResultType::Columns();
  }else if (name == "RankCorrelation") {
    return RankCorrelation::ResultType::Columns();
  }else{
    spdlog::critical("Invalid Meta Analysis Strategy.");
    exit(1);
  }
}

void FixedEffectEstimator::estimate(Journal *journal) {
  spdlog::debug("Computing Fixed Effect Estimate...");

  journal->storeMetaAnalysisResult(FixedEffect(journal->yi, journal->vi));
}

void RandomEffectEstimator::estimate(Journal *journal) {

  spdlog::debug("Computing Random Effect Estimate...");

  float tau2 {0};

  if (params.estimator.find("DL") != std::string::npos){
    tau2 = RandomEffectEstimator::DL(journal->yi, journal->vi, journal->wi);
  }else if (params.estimator.find("PM") != std::string::npos){
    spdlog::critical("Not implemented yet!");
    exit(1);
//    tau2 = RandomEffectEstimator::PM(journal->yi, journal->vi, tau2);
  }

  journal->storeMetaAnalysisResult(RandomEffect(journal->yi, journal->vi, tau2));
}

void EggersTestEstimator::estimate(Journal *journal) {

  spdlog::debug("Computing Eggers Estimate...");

  journal->storeMetaAnalysisResult(EggersTest(journal->yi, journal->vi, params.alpha));
}

RandomEffectEstimator::ResultType
RandomEffectEstimator::RandomEffect(const arma::Row<float> &yi, const arma::Row<float> &vi, float tau2) {

  using boost::math::normal;
  using boost::math::chi_squared;

  normal norm(0, 1);

  // Weight per study
  arma::Row<float> wi = 1. / (vi + tau2);
  // Meta-analytic estimate
  auto est = arma::accu(yi % wi) / arma::accu(wi);
  // Standard error of meta-analytic estimate
  auto se = sqrt(1. / arma::accu(wi));
  // Lower bound CI meta-analytical estimate
  auto ci_lb = est - quantile(norm, 0.975) * se;
  // Upper bound CI meta-analytical estimate
  auto ci_ub = est + quantile(norm, 0.975) * se;
  // Z-value for test of no effect
  auto zval = est/se;
  // Compute one-sided p-value
  auto pval_one = cdf(complement(norm, zval));
  // Compute two-tailed p-value
  auto pval = pval_one > 0.5 ? (1. - pval_one) * 2 : pval_one * 2;

  arma::Row<float> wi_fe = 1. / vi;
  auto est_fe = arma::accu(wi_fe % yi)/ arma::accu(wi_fe);

  // Q-statistic
  auto q_stat = arma::accu(wi_fe % arma::pow(yi - est_fe, 2));

  chi_squared chisq(yi.n_elem - 1);
  // p-value of Q-statistic
  auto q_pval = cdf(complement(chisq, q_stat));

  return ResultType{est, static_cast<float>(se), static_cast<float>(ci_lb), static_cast<float>(ci_ub), static_cast<float>(zval), static_cast<float>(pval), q_stat, static_cast<float>(q_pval), tau2};
}

// General method-of-moments estimate (Eq. 6 in DerSimonian and Kacker, 2007)
float RandomEffectEstimator::DL(const arma::Row<float> &yi, const arma::Row<float> &vi, const arma::Row<float> &wi) {

  spdlog::trace("→ Estimating the tau2 using DL ...");

  auto q = arma::accu(wi % arma::pow(yi - (arma::accu(wi % yi)/arma::accu(wi)), 2));
  // spdlog::trace("Q: {}", q);

  auto tau2 = (q - (yi.n_elem - 1)) / (arma::accu(wi) - (arma::accu(arma::pow(wi, 2))/arma::accu(wi)));
  // spdlog::trace("Tau2: {}", tau2);

  tau2 = tau2 < 0 ? 0 : tau2;

  return tau2;
}

// Function for estimating tau2 with Paule-Mandel estimator
float RandomEffectEstimator::PM(const arma::Row<float> &yi, const arma::Row<float> &vi, const float tau2) {
  // Degrees of freedom of Q-statistic (df is also expected value because chi square distributed)
  auto df = yi.n_elem - 1;
  // Weights in meta-analysis
  arma::Row<float> wi = 1. / (vi + tau2);
  // Meta-analytic effect size
  auto theta = arma::accu(yi % wi)/arma::accu(wi);
  // Q-statistic
  auto Q = arma::accu(wi % arma::pow(yi - theta, 2));

  // Stop iterating if computed Q-statistic equals degrees of freedom

  return (Q - df);
}

EggersTestEstimator::ResultType
EggersTestEstimator::EggersTest(const arma::Row<float> &yi, const arma::Row<float> &vi, float alpha) {

  using namespace mlpack;
  using namespace mlpack::regression;

  using boost::math::students_t;

  arma::Row<double> Yi = arma::conv_to<arma::Row<double>>::from(yi);
  arma::Row<double> Vi = arma::conv_to<arma::Row<double>>::from(vi);

  auto n = Yi.n_elem;
  auto p = 2;
  float df = n - p;

  arma::Row<double> Wi = 1./Vi;
  arma::Row<double> wts = arma::sqrt(Wi);
  arma::Row<double> si = arma::sqrt(Vi);

  arma::Row<double> predictions(n);

  arma::Mat<double> X;
  X.ones(2, n);
  X.row(1) = si;

  LinearRegression lg(X, Yi, Wi);
  lg.Train(X, Yi, Wi, false);
  lg.Predict(X, predictions);

  arma::Row<double> errors = Yi - predictions;

  auto slope = lg.Parameters().at(1);

  arma::Mat<double> W = arma::diagmat(Wi);

  double res_var_2 = sqrt(arma::accu(Wi % arma::pow(errors, 2)) / (n - 2));
  arma::Mat<double> S_2 = arma::diagmat(arma::pow(res_var_2 / sqrt(Wi), 2));

  arma::Mat<double> Z = X.t();
  arma::Mat<double> var_betas = arma::sqrt(arma::inv(Z.t() * W * Z) * (Z.t() * W * S_2 * W.t() * Z) * arma::inv(Z.t() * W * Z));

  double slope_se = var_betas.diag().at(1);

  double slope_stat = slope / slope_se;

  auto res = TTest::compute_pvalue(slope_stat, n - 2, 0.1, TestStrategy::TestAlternative::TwoSided);

  return ResultType{static_cast<float>(slope), static_cast<float>(slope_se), static_cast<float>(slope_stat), res.first, res.second, df};
}

sam::TestOfObsOverExptSig::ResultType
TestOfObsOverExptSig::TES(const arma::Row<float> &sigs, const arma::Row<float> &ni, float beta, float alpha) {

  using boost::math::students_t;
  using boost::math::non_central_t;
  using boost::math::chi_squared;

  float k = sigs.n_elem;

  float O = arma::accu(sigs);

  arma::Row<float> tcvs(k);
  tcvs.imbue([&, i = 0]() mutable {
    students_t tdist(ni[i] - 1); i++;
    return quantile(tdist, 0.95);
  });

  // non-central t-statistics
  arma::Row<float> powers(k);
  powers.imbue([&, i = 0]() mutable {
    non_central_t nct(ni[i] - 1, beta * sqrt(ni[i]));
    return cdf(complement(nct, tcvs[i++]));
  });

  float E = arma::accu(powers);

  /// @note If E is absolute zero, I'm adding some noise that I don't have to deal with the explosion
  if (E < 0.0000001)
    E = 1e-10;

  /// A is most likely different from what R spit out, due to brutal rounding that's happening in R.
  float A {100000};
  float pval {0.0};
  if (k != E) {
    A = pow(O - E, 2.) / E + pow(O - E, 2.) / (k - E);

    if (!isnan(A) and !isinf(A)) {
      chi_squared chisq(1);
      pval = cdf(complement(chisq, A));
    }
  }

  return TestOfObsOverExptSig::ResultType{E, A, pval, pval < alpha};
}

void TestOfObsOverExptSig::estimate(Journal *journal) {

  spdlog::debug("Computing Test Of Obs Over Expt Significance...");

  float beta = FixedEffectEstimator::FixedEffect(journal->yi, journal->vi).est;

  arma::Row<float> sigs(journal->yi.n_elem);
  sigs.imbue([&, i = 0]() mutable {
    return journal->publications_list[i++].dv_.sig_;
  });

  arma::Row<float> ni(journal->yi.n_elem);
  ni.imbue([&, i = 0]() mutable {
    return journal->publications_list[i++].dv_.nobs_;
  });

  journal->storeMetaAnalysisResult(TestOfObsOverExptSig::TES(sigs, ni, beta, 0.05));
}

void TrimAndFill::estimate(Journal *journal) {

  spdlog::debug("Computing Trim And Fill...");

  arma::Row<float> ni(journal->yi.n_elem);
  ni.imbue([&, i = 0]() mutable {
    return journal->publications_list[i++].dv_.nobs_;
  });

  journal->storeMetaAnalysisResult(TrimAndFill::TF(journal->yi, journal->vi, ni, params));
}

TrimAndFill::ResultType TrimAndFill::TF(arma::Row<float> yi, arma::Row<float> vi, arma::Row<float> ni, const Parameters &params) {

  int k = yi.n_elem;
  arma::Row<float> wi = 1. / vi;

  std::string side = params.side;

  /// Determining the side
  float beta = FixedEffectEstimator::FixedEffect(yi, vi).est;

  if (params.side.find("auto") != std::string::npos) {
    if (beta < 0) {
      side = "right";
    } else {
      side = "left";
    }
  }

  /// flip data if examining right side
  if (side.find("right") != std::string::npos){
    yi = -1. * yi;
  }

  /// sort data by increasing yi
  arma::uvec ix = arma::sort_index(yi);
  arma::Row<float> yi_s = yi.elem(ix).as_row();
  arma::Row<float> vi_s = vi.elem(ix).as_row();
  arma::Row<float> wi_s = wi.elem(ix).as_row();
  arma::Row<float> ni_s = wi.elem(ix).as_row();

  int iter{0};
  int maxiter{100};

  float k0_sav{-1};
  float k0{0}; // estimated number of missing studies;
  float se_k0{0};
  float Sr{0};
  float varSr{0};
  float k0_pval{0};

  arma::Row<float> yi_c;
  arma::Row<float> yi_c_r;
  arma::Row<float> yi_c_r_s;

  while (abs(k0 - k0_sav) > 0) {

    k0_sav = k0; // save current value of k0;

    iter++;

    if (iter > maxiter)
      break;

    //  truncated data
    arma::uvec elems = arma::regspace<arma::uvec>(0, 1, k - k0 - 1);
    arma::Row<float> yi_t = yi_s.elem(elems).as_row();
    arma::Row<float> vi_t = vi_s.elem(elems).as_row();
    arma::Row<float> wi_t = wi_s.elem(elems).as_row();
    arma::Row<float> ni_t = wi_s.elem(elems).as_row();

    //  intercept estimate based on truncated data
    beta = FixedEffectEstimator::FixedEffect(yi_t, vi_t).est;

    yi_c     = yi_s - beta;                             ///  centered values;
    yi_c_r   = rankdata(abs(yi_c), "average").as_row(); /// @todo ties_method="first"); //  ranked absolute centered values;
    yi_c_r_s = arma::sign(yi_c) % yi_c_r;               ///  signed ranked centered values;

    //  estimate the number of missing studies with the R0 estimator

    if (params.estimator.find("R0") != std::string::npos) {
      arma::uvec inx = arma::find(yi_c_r_s < 0);
      k0 = (k - arma::max(-1. * yi_c_r_s.elem(inx))) - 1;
      se_k0 = sqrt(2 * std::max(static_cast<float>(0.), k0) + 2);
    }

    ///  estimate the number of missing studies with the L0 estimator
    if (params.estimator.find("L0") != std::string::npos) {
      arma::uvec inx = arma::find(yi_c_r_s > 0);
      Sr = arma::accu(yi_c_r_s.elem(inx));
      k0 = (4.*Sr - k*(k+1.)) / (2.*k - 1.);
      varSr = 1./24 * (k*(k+1.)*(2.*k+1.) + 10.*pow(k0,3) + 27.*pow(k0,2) + 17.*k0 - 18.*k*pow(k0,2) - 18.*k*k0 + 6.*pow(k,2)*k0);
      se_k0 = 4.*sqrt(varSr) / (2*k - 1);
    }

    ///  estimate the number of missing studies with the Q0 estimator
    if (params.estimator.find("Q0") != std::string::npos) {
      arma::uvec inx = arma::find(yi_c_r_s > 0);
      Sr = arma::accu(yi_c_r_s.elem(inx));
      k0 = k - 1./2 - sqrt(2*pow(k,2) - 4.*Sr + 1./4);
      varSr = 1./24 * (k*(k+1.)*(2*k+1.) + 10.*pow(k0,3) + 27.*pow(k0,2) + 17.*k0 - 18.*k*pow(k0,2) - 18.*k*k0 + 6.*pow(k,2)*k0);
      se_k0 = 2. * sqrt(varSr) / sqrt(pow(k-0.5,2) - k0*(2.*k - k0 - 1.));
    }

    ///  round k0 and make sure that k0 is non-negative
    k0 = std::max(static_cast<float>(0.), std::round(k0));
    se_k0 = std::max(static_cast<float>(0.), se_k0);

  }

  /// ------------------ Filling and estimating ----------------

  auto res = FixedEffectEstimator::FixedEffect(yi, vi);
  float imputed_est = res.est;
  float imputed_pval = res.pval;

  /// if estimated number of missing studies is > 0
  if (k0 > 0) {

    /// flip data back if side is right
    if (side.find("right") != std::string::npos) {
      yi_c = -1 * (yi_c - beta);
      yi = -1 * yi;
    } else {
      yi_c = yi_c - beta;
    }

    /// create filled-in data set
    arma::Row<float> yi_f = yi_c;
    arma::Row<float> yi_fill = yi;
    yi_fill.insert_cols(yi_f.n_elem, -1. * yi_c.elem(arma::regspace<arma::uvec>(k - k0, 1, k - 1)).as_row());

    /// apply limits if specified
    /// @todo: to be implemented
    //    if (!missing(ilim)) {
    //      ilim = sort(ilim)
    //      if (length(ilim) != 2L)
    //        stop(mstyle$stop("Argument 'ilim' must be of length 2_"))
    //        yi_fill[yi_fill < ilim[1]] = ilim[1]
    //        yi_fill[yi_fill > ilim[2]] = ilim[2]
    //        }

    arma::Row<float> vi_fill = vi;
    vi_fill.insert_cols(vi.n_elem, vi.elem(arma::regspace<arma::uvec>(k - k0, 1, k - 1)).as_row());
    arma::Row<float> wi_fill = wi;
    wi_fill.insert_cols(wi.n_elem, wi.elem(arma::regspace<arma::uvec>(k - k0, 1, k - 1)).as_row());
    arma::Row<float> ni_fill = ni;
    ni_fill.insert_cols(ni.n_elem, ni.elem(arma::regspace<arma::uvec>(k - k0, 1, k - 1)).as_row());

    /// fit model with imputed data
    auto res = FixedEffectEstimator::FixedEffect(yi_fill, vi_fill);
    imputed_est = res.est;
    imputed_pval = res.pval;

  }

  /// @todo need to be integrated!
  std::optional<float> p_k0;

  /// Adjustment for p_k0
  if (params.estimator.find("R0") != std::string::npos) {
    arma::Row<float> m {arma::regspace<arma::Row<float>>(-1, 1, (k0-1))};
    arma::Row<float> bin_coefs(m.n_elem);
    /// @todo This imbue can be improved
    bin_coefs.imbue([&, i = 0]() mutable {
      auto x = boost::math::binomial_coefficient<float>(0+m.at(i)+1, m.at(i)+1);
      i++;
      return x;
    });
    arma::Row<float> tmp(m.n_elem);
    tmp.imbue([&, i = 0]() mutable {
      return pow(0.5, static_cast<int>(0 + m.at(i++) + 2));
    });
    p_k0 = 1 - arma::accu(bin_coefs % tmp);
  } //else
    // p_k0 = NA

  /// @todo Still need to report the p_k0
  return ResultType{.k0 = k0, .se_k0 = se_k0, .k_all = k + k0, .side = side, .imputed_est = imputed_est, .imputed_pval = imputed_pval};

}

namespace sam {

/*-------------------------------------------------------------------------
 * This function calculates the Kendall correlation tau_b.
 *
 * from: https://afni.nimh.nih.gov/pub/dist/src/ktaub.c
 */
float kendallcor(const arma::Row<float> &x, const arma::Row<float> &y) {

  spdlog::debug(" → Computing Kendall Correlation...");

  int len = x.n_elem;

  int m1 = 0, m2 = 0, s = 0, nPair , i,j ;
  float cor ;

  for(i = 0; i < len; i++) {
    for(j = i + 1; j < len; j++) {
      if(y[i] > y[j]) {
        if (x[i] > x[j]) {
          s++;
        } else if(x[i] < x[j]) {
          s--;
        } else {
          m1++;
        }
      } else if(y[i] < y[j]) {
        if (x[i] > x[j]) {
          s--;
        } else if(x[i] < x[j]) {
          s++;
        } else {
          m1++;
        }
      } else {
        m2++;

        if(x[i] == x[j]) {
          m1++;
        }
      }
    }
  }

  nPair = len * (len - 1) / 2;

  if( m1 < nPair && m2 < nPair )
    cor = s / ( sqrtf((float)(nPair-m1)) * sqrtf((float)(nPair-m2)) );
  else
    cor = 0.0f;

  return cor;
}

float ckendall(int k, int n, arma::Mat<float> &w) {
  int i, u;
  float s;

  u =  (n * (n - 1) / 2);
  if ((k < 0) || (k > u))
    return(0);

  if (w.at(n, k) < 0) {
    if (n == 1)
      w.at(n, k) = (k == 0);
    else {
      s = 0;
      for (i = 0; i < n; i++)
        s += ckendall(k - i, n - 1, w);
      w.at(n, k) = s;
    }
  }
  return(w.at(n, k));
}

float pkendall(int len, int n) {

  spdlog::debug(" → Computing Kendall Probability...");

  int i, j;
  float p, q;

  p = 0;
  q = len;

  size_t u =  (n * (n - 1) / 2);
  arma::Mat<float> w(n, u); w.fill(-1);

    if (q < 0)
      p = 0;
    else if (q > (n * (n - 1) / 2))
      p = 1;
    else {
      p = 0;
      for (j = 0; j <= q; j++)
        p += ckendall(j, n, w);
      p = p / boost::math::tgamma(n + 1);
    }

  spdlog::trace(" → → p = {:f}\n", p);
  return p;
}

std::pair<float, float> kendall_cor_test(const arma::Row<float> &x, const arma::Row<float> &y, const TestStrategy::TestAlternative alternative) {

  spdlog::debug(" → Running Kendall Correlation Test...");

  auto n = x.n_elem;
  auto r = kendallcor(x, y);

  auto q = round((r + 1.) * n * (n - 1.) / 4.);

  arma::Row<float> x_uqniues = arma::unique(x);
  size_t x_n_uqniues = x_uqniues.n_elem;
  arma::Row<float> y_uqniues = arma::unique(y);
  size_t y_n_uqniues = y_uqniues.n_elem;

  bool ties = (min(x_n_uqniues, y_n_uqniues) < n);

  float p{0};
  float statistic;

  if (!ties) {

    statistic = q;
    spdlog::trace(" → → Statistic: {}", q);

    switch (alternative) {
      case TestStrategy::TestAlternative::TwoSided: {
        if(q > n * (n - 1) / 4){
          p = 1 - pkendall(q - 1, n);
        }else{
          p = pkendall(q, n);
        }
        p = std::min(2. * p, 1.);
      } break;
      case TestStrategy::TestAlternative::Greater: {
        p = 1. - pkendall(q - 1, n);
      } break;
      case TestStrategy::TestAlternative::Less: {
        p = pkendall(q, n);
      } break;
    }

  }else{
    /// @note I'm not 100% sure if this is a good replacement for `table` but it seems to
    /// be working!
    spdlog::trace("Found ties...");
    spdlog::warn("Cannot compute exact p-value with ties!");

    /// xties <- table(x[duplicated(x)]) + 1;
    arma::urowvec xties;
    if (x_n_uqniues > 0) {
      xties = arma::hist(x, arma::sort(arma::unique(x))) - 1;
      xties = arma::nonzeros(xties).as_row() + 1;
    }else
      xties = arma::urowvec({0});

    /// yties <- table(y[duplicated(y)]) + 1;
    arma::urowvec yties;
    if (y_n_uqniues) {
      yties = arma::hist(y, arma::sort(arma::unique(y))) - 1;
      yties = arma::nonzeros(yties).as_row() + 1;
    }else
      yties = arma::urowvec({0});

    float T0 = n * (n - 1)/2;

    float T1 = arma::accu(xties % (xties - 1))/2;

    float T2 = arma::accu(yties % (yties - 1))/2;

    float S = r * sqrt((T0 - T1) * (T0 - T2));

    float v0 = n * (n - 1) * (2 * n + 5);

    float vt = arma::accu(xties % (xties - 1) % (2 * xties + 5));

    float vu = arma::accu(yties % (yties - 1) % (2 * yties + 5));

    float v1 = arma::accu((xties % (xties - 1))) * arma::accu(yties % (yties - 1));

    float v2 = arma::accu((xties % (xties - 1)) % (xties - 2)) * arma::accu(yties % (yties - 1) % (yties - 2));

    float var_S = (v0 - vt - vu) / 18. + v1 / (2. * n * (n - 1.)) + v2 / (9. * n * (n - 1.) * (n - 2.));

    statistic = S / sqrt(var_S);

    using boost::math::normal;
    normal norm;

    /// @todo check if these are what I want
    switch (alternative) {
      case TestStrategy::TestAlternative::TwoSided: {
        p = 2 * min(cdf(norm, statistic), cdf(complement(norm, statistic)));
      } break;
      case TestStrategy::TestAlternative::Greater: {
        p = cdf(complement(norm, statistic));
      } break;
      case TestStrategy::TestAlternative::Less: {
        p = cdf(norm, statistic);
      } break;
    }

  }

  return std::make_pair(r, p);
}

}

RankCorrelation::ResultType RankCorrelation::RankCor(arma::Row<float> yi, arma::Row<float> vi, const Parameters &params) {

  auto res  = FixedEffectEstimator::FixedEffect(yi, vi);
  auto beta = res.est;
  auto vb = pow(res.se, 2);

  arma::Row<float> vi_star = vi - vb;
  arma::Row<float> yi_star = (yi - beta) / arma::sqrt(vi_star);

//  vi_star.replace(arma::datum::nan, 0.);
//  yi_star.replace(arma::datum::nan, 0.);
  auto ken_res = kendall_cor_test(yi_star, vi, params.alternative);

  auto tau  = ken_res.first;
  auto pval = ken_res.second;
  spdlog::trace("Kendal Correlation Test: tau: {}, p: {}", tau, pval);

  return {.est = tau, .pval = pval, .sig = pval < params.alpha};

}

void RankCorrelation::estimate(sam::Journal *journal) {
  journal->storeMetaAnalysisResult(RankCorrelation::RankCor(journal->yi, journal->vi, params));
}


// Copyright (C) 2014 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/estimators/estimate_essential_matrix.h"

#include <Eigen/Core>
#include <vector>

#include "theia/alignment/alignment.h"
#include "theia/solvers/estimator.h"
#include "theia/matching/feature_correspondence.h"
#include "theia/sfm/pose/five_point_relative_pose.h"
#include "theia/sfm/pose/util.h"
#include "theia/util/util.h"

namespace theia {
namespace {

// An estimator for computing the essential matrix from 5 feature
// correspondences. The feature correspondences should be normalized
// by the focal length with the principal point at (0, 0).
class EssentialMatrixEstimator
    : public Estimator<FeatureCorrespondence, Eigen::Matrix3d> {
 public:
  EssentialMatrixEstimator() {}

  // 5 correspondences are needed to determine an essential matrix.
  double SampleSize() const { return 5; }

  // Estimates candidate essential matrices from correspondences.
  bool EstimateModel(const std::vector<FeatureCorrespondence>& correspondences,
                     std::vector<Eigen::Matrix3d>* essential_matrices) const {
    std::vector<Eigen::Vector2d> image1_points, image2_points;
    image1_points.reserve(correspondences.size());
    image2_points.reserve(correspondences.size());
    for (int i = 0; i < correspondences.size(); i++) {
      image1_points.emplace_back(correspondences[i].feature1);
      image2_points.emplace_back(correspondences[i].feature2);
    }

    return FivePointRelativePose(image1_points,
                                 image2_points,
                                 essential_matrices);
  }

  // The error for a correspondences given a model. This is the squared sampson
  // error.
  double Error(const FeatureCorrespondence& correspondence,
               const Eigen::Matrix3d& essential_matrix) const {
    return SquaredSampsonDistance(essential_matrix,
                                  correspondence.feature1,
                                  correspondence.feature2);
  }

 private:
  DISALLOW_COPY_AND_ASSIGN(EssentialMatrixEstimator);
};

}  // namespace

bool EstimateEssentialMatrix(
    const RansacParameters& ransac_params,
    const RansacType& ransac_type,
    const std::vector<FeatureCorrespondence>& normalized_correspondences,
    Eigen::Matrix3d* essential_matrix,
    RansacSummary* ransac_summary) {
  EssentialMatrixEstimator essential_matrix_estimator;
  std::unique_ptr<SampleConsensusEstimator<EssentialMatrixEstimator> >
      ransac = CreateAndInitializeRansacVariant(ransac_type,
                                                ransac_params,
                                                essential_matrix_estimator);

  // Estimate essential matrix.
  return ransac->Estimate(normalized_correspondences,
                          essential_matrix,
                          ransac_summary);
}

}  // namespace theia


#pragma once

#include <vector>
#include <Eigen/Dense>

class BaseVector {
public:
    BaseVector() {}

    explicit BaseVector(const std::vector<double>& data) {
        _data = data;
    }

    // Read-only access when indexing the data
    const double operator[](int index) const {
        return _data[index];
    }

    size_t size() { return _data.size(); }

    const std::vector<double> data() const { return _data; }

    const std::vector<double>* data_ptr() const { return &_data; }

protected:
    std::vector<double> _data;

    void push_back(double value) {_data.push_back(value);}
};

class Vector2 : public BaseVector {
public:
    using BaseVector::BaseVector;

    Vector2(double x, double y) {
        _data.push_back(x);
        _data.push_back(y);
    }

    double x() const {return _data[0];}

    double y() const {return _data[1];}
};

struct Vector3 : BaseVector {
public:
    Vector3(double x, double y, double z) {
        _data.push_back(x);
        _data.push_back(y);
        _data.push_back(z);
    }

    // Read-only access when indexing the data
    const double operator[](int index) const {
        return _data[index];
    }

    double x() const {return _data[0];}

    double y() const {return _data[1];}

    double z() const {return _data[2];}
};

double magnitude(const Vector2& u);
Vector2 scale(const Vector2& u, double alpha);
Vector2 normalize(const Vector2& u);
Vector2 add(const Vector2& u, const Vector2& v);
Vector2 subtract(const Vector2& u, const Vector2& v);

double magnitude(const Vector3& u);
Vector3 scale(const Vector3& u, double alpha);
Vector3 normalize(const Vector3& u);
Vector3 add(const Vector3& u, const Vector3& v);
Vector3 subtract(const Vector3& u, const Vector3& v);


#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include <stdlib.h>
#include <math.h>
#include <inttypes.h>
#include <string.h>

template<typename Return, typename... T>
Return __enzyme_autodiff(T...);

static float tdiff(struct timeval *start, struct timeval *end) {
  return (end->tv_sec-start->tv_sec) + 1e-6*(end->tv_usec-start->tv_usec);
}

#define BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
#define BOOST_NO_EXCEPTIONS
#include <iostream>
#include <boost/array.hpp>

#include <boost/numeric/odeint.hpp>

#include <boost/throw_exception.hpp>

using namespace std;
using namespace boost::numeric::odeint;

#include <stdio.h>

typedef boost::array< double , 1 > state_type;

void lorenz( const state_type &x , state_type &dxdt , double t )
{
    const double a = 1.2;
    dxdt[0] = -a * x[0];
}

double foobar(double t, uint64_t iters) {
    state_type x = { 1.0 }; // initial conditions

    //typedef controlled_runge_kutta< runge_kutta_dopri5< state_type , typename state_type::value_type , state_type , double > > stepper_type;
    typedef euler< state_type , typename state_type::value_type , state_type , double > stepper_type;
    integrate_const( stepper_type(), lorenz , x , 0.0 , t, t/iters );

    //printf("final result t=%f x(t)=%f, exp(-1.2* t)=%f\n", t, x[0], exp(- 1.2 * t));
    return x[0];
}

void adept_sincos(double inp, uint64_t iters);

static void enzyme_sincos(double inp, uint64_t iters) {

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res = foobar(inp, iters);

  gettimeofday(&end, NULL);
  printf("Enzyme real %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res = foobar(inp, iters);

  gettimeofday(&end, NULL);
  printf("Enzyme forward %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);
  double res2;

  res2 = __enzyme_autodiff<double>(foobar, inp, iters);

  gettimeofday(&end, NULL);
  printf("Enzyme combined %0.6f res'=%f\n", tdiff(&start, &end), res2);
  }
}

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

  int max_iters = atoi(argv[1]) ;
  double inp = 2.1;

  unsigned i=0;
  for(int iters=max_iters/20; iters<=max_iters; iters+=max_iters/20) {
    printf("iters=%d\n", iters);
    adept_sincos(inp, iters);
    enzyme_sincos(inp, iters);
    i++;
    if (i == 10) break;
  }
}


// Boost.Geometry

// Copyright (c) 2016 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_FORMULAS_SJOBERG_INTERSECTION_HPP
#define BOOST_GEOMETRY_FORMULAS_SJOBERG_INTERSECTION_HPP

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/srs.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/algorithms/detail/flattening.hpp>

namespace boost { namespace geometry { namespace formula
{

/*!
\brief The intersection of two geodesics as proposed by Sjoberg.
\author See
    - [Sjoberg02] Lars E. Sjoberg, Intersections on the sphere and ellipsoid, 2002
      http://link.springer.com/article/10.1007/s00190-001-0230-9
    - [Sjoberg07] Lars E. Sjoberg, Geodetic intersection on the ellipsoid, 2007
      http://link.springer.com/article/10.1007/s00190-007-0204-7
*/
template
<
    typename CT,
    template <typename, bool, bool, bool, bool, bool> class Inverse,
    unsigned int Order = 4
>
class sjoberg_intersection
{
    typedef Inverse<CT, false, true, false, false, false> inverse_type;
    typedef typename inverse_type::result_type inverse_result;

public:
    template <typename T1, typename T2, typename Spheroid>
    static inline bool apply(T1 const& lona1, T1 const& lata1,
                             T1 const& lona2, T1 const& lata2,
                             T2 const& lonb1, T2 const& latb1,
                             T2 const& lonb2, T2 const& latb2,
                             CT & lon, CT & lat,
                             Spheroid const& spheroid)
    {
        CT const lon_a1 = lona1;
        CT const lat_a1 = lata1;
        CT const lon_a2 = lona2;
        CT const lat_a2 = lata2;
        CT const lon_b1 = lonb1;
        CT const lat_b1 = latb1;
        CT const lon_b2 = lonb2;
        CT const lat_b2 = latb2;

        CT const alpha1 = inverse_type::apply(lon_a1, lat_a1, lon_a2, lat_a2, spheroid).azimuth;
        CT const alpha2 = inverse_type::apply(lon_b1, lat_b1, lon_b2, lat_b2, spheroid).azimuth;

        return apply(lon_a1, lat_a1, alpha1, lon_b1, lat_b1, alpha2, lon, lat, spheroid);
    }

    template <typename Spheroid>
    static inline bool apply(CT const& lon1, CT const& lat1, CT const& alpha1,
                             CT const& lon2, CT const& lat2, CT const& alpha2,
                             CT & lon, CT & lat,
                             Spheroid const& spheroid)
    {
        // coordinates in radians

        // TODO - handle special cases like degenerated segments, equator, poles, etc.

        CT const c0 = 0;
        CT const c1 = 1;
        CT const c2 = 2;

        CT const pi = math::pi<CT>();
        CT const pi_half = pi / c2;
        CT const f = detail::flattening<CT>(spheroid);
        CT const one_minus_f = c1 - f;
        CT const e_sqr = f * (c2 - f);

        CT const sin_alpha1 = sin(alpha1);
        CT const sin_alpha2 = sin(alpha2);

        CT const tan_beta1 = one_minus_f * tan(lat1);
        CT const tan_beta2 = one_minus_f * tan(lat2);
        CT const beta1 = atan(tan_beta1);
        CT const beta2 = atan(tan_beta2);
        CT const cos_beta1 = cos(beta1);
        CT const cos_beta2 = cos(beta2);
        CT const sin_beta1 = sin(beta1);
        CT const sin_beta2 = sin(beta2);

        // Clairaut constants (lower-case in the paper)
        int const sign_C1 = math::abs(alpha1) <= pi_half ? 1 : -1;
        int const sign_C2 = math::abs(alpha2) <= pi_half ? 1 : -1;
        // Cj = 1 if on equator
        CT const C1 = sign_C1 * cos_beta1 * sin_alpha1;
        CT const C2 = sign_C2 * cos_beta2 * sin_alpha2;

        CT const sqrt_1_C1_sqr = math::sqrt(c1 - math::sqr(C1));
        CT const sqrt_1_C2_sqr = math::sqrt(c1 - math::sqr(C2));

        // handle special case: segments on the equator
        bool const on_equator1 = math::equals(sqrt_1_C1_sqr, c0);
        bool const on_equator2 = math::equals(sqrt_1_C2_sqr, c0);
        if (on_equator1 && on_equator2)
        {
            return false;
        }
        else if (on_equator1)
        {
            CT const dL2 = d_lambda_e_sqr(sin_beta2, c0, C2, sqrt_1_C2_sqr, e_sqr);
            CT const asin_t2_t02 = asin(C2 * tan_beta2 / sqrt_1_C2_sqr);
            lat = c0;
            lon = lon2 - asin_t2_t02 + dL2;
            return true;
        }
        else if (on_equator2)
        {
            CT const dL1 = d_lambda_e_sqr(sin_beta1, c0, C1, sqrt_1_C1_sqr, e_sqr);
            CT const asin_t1_t01 = asin(C1 * tan_beta1 / sqrt_1_C1_sqr);
            lat = c0;
            lon = lon1 - asin_t1_t01 + dL1;
            return true;
        }

        CT const t01 = sqrt_1_C1_sqr / C1;
        CT const t02 = sqrt_1_C2_sqr / C2;

        CT const asin_t1_t01 = asin(tan_beta1 / t01);
        CT const asin_t2_t02 = asin(tan_beta2 / t02);
        CT const t01_t02 = t01 * t02;
        CT const t01_t02_2 = c2 * t01_t02;
        CT const sqr_t01_sqr_t02 = math::sqr(t01) + math::sqr(t02);

        CT t = tan_beta1;
        int t_id = 0;

        // find the initial t using simplified spherical solution
        // though not entirely since the reduced latitudes and azimuths are spheroidal
        // [Sjoberg07]
        CT const k_base = lon1 - lon2 + asin_t2_t02 - asin_t1_t01;

        {
            CT const K = sin(k_base);
            CT const d1 = sqr_t01_sqr_t02;
            //CT const d2 = t01_t02_2 * math::sqrt(c1 - math::sqr(K));
            CT const d2 = t01_t02_2 * cos(k_base);
            CT const D1 = math::sqrt(d1 - d2);
            CT const D2 = math::sqrt(d1 + d2);
            CT const K_t01_t02 = K * t01_t02;

            CT const T1 = K_t01_t02 / D1;
            CT const T2 = K_t01_t02 / D2;
            CT asin_T1_t01 = 0;
            CT asin_T1_t02 = 0;
            CT asin_T2_t01 = 0;
            CT asin_T2_t02 = 0;

            // test 4 possible results
            CT l1 = 0, l2 = 0, dl = 0;
            bool found = check_t<0>( T1,
                                    lon1,  asin_T1_t01 = asin(T1 / t01), asin_t1_t01,
                                    lon2,  asin_T1_t02 = asin(T1 / t02), asin_t2_t02,
                                    t, l1, l2, dl, t_id)
                      || check_t<1>(-T1,
                                    lon1, -asin_T1_t01                 , asin_t1_t01,
                                    lon2, -asin_T1_t02                 , asin_t2_t02,
                                    t, l1, l2, dl, t_id)
                      || check_t<2>( T2,
                                    lon1,  asin_T2_t01 = asin(T2 / t01), asin_t1_t01,
                                    lon2,  asin_T2_t02 = asin(T2 / t02), asin_t2_t02,
                                    t, l1, l2, dl, t_id)
                      || check_t<3>(-T2,
                                    lon1, -asin_T2_t01                 , asin_t1_t01,
                                    lon2, -asin_T2_t02                 , asin_t2_t02,
                                    t, l1, l2, dl, t_id);

            boost::ignore_unused(found);
        }

        // [Sjoberg07]
        //int const d2_sign = t_id < 2 ? -1 : 1;
        int const t_sign = (t_id % 2) ? -1 : 1;
        // [Sjoberg02]
        CT const C1_sqr = math::sqr(C1);
        CT const C2_sqr = math::sqr(C2);

        CT beta = atan(t);
        CT dL1 = 0, dL2 = 0;
        CT asin_t_t01 = 0;
        CT asin_t_t02 = 0;

        for (int i = 0; i < 10; ++i)
        {
            CT const sin_beta = sin(beta);

            // integrals approximation
            dL1 = d_lambda_e_sqr(sin_beta1, sin_beta, C1, sqrt_1_C1_sqr, e_sqr);
            dL2 = d_lambda_e_sqr(sin_beta2, sin_beta, C2, sqrt_1_C2_sqr, e_sqr);

            // [Sjoberg07]
            /*CT const k = k_base + dL1 - dL2;
            CT const K = sin(k);
            CT const d1 = sqr_t01_sqr_t02;
            //CT const d2 = t01_t02_2 * math::sqrt(c1 - math::sqr(K));
            CT const d2 = t01_t02_2 * cos(k);
            CT const D = math::sqrt(d1 + d2_sign * d2);
            CT const t_new = t_sign * K * t01_t02 / D;
            CT const dt = math::abs(t_new - t);
            t = t_new;
            CT const new_beta = atan(t);
            CT const dbeta = math::abs(new_beta - beta);
            beta = new_beta;*/

            // [Sjoberg02] - it converges faster


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/estimators/estimate_homography.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <limits>
#include <memory>
#include <vector>

#include "theia/matching/feature_correspondence.h"
#include "theia/sfm/create_and_initialize_ransac_variant.h"
#include "theia/sfm/pose/four_point_homography.h"
#include "theia/sfm/pose/util.h"
#include "theia/solvers/estimator.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/util/util.h"

namespace theia {
namespace {

using Eigen::Matrix3d;
using Eigen::Vector3d;

// An estimator for computing a homography from 4 feature correspondences. The
// feature correspondences should be normalized by the focal length with the
// principal point at (0, 0).
class HomographyEstimator
    : public Estimator<FeatureCorrespondence, Eigen::Matrix3d> {
 public:
  HomographyEstimator() {}

  // 4 correspondences are needed to determine a homography.
  double SampleSize() const { return 4; }

  // Estimates candidate relative poses from correspondences.
  bool EstimateModel(const std::vector<FeatureCorrespondence>& correspondences,
                     std::vector<Eigen::Matrix3d>* homography) const {
    std::vector<Eigen::Vector2d> image1_points(4), image2_points(4);
    for (int i = 0; i < 4; i++) {
      image1_points[i] = correspondences[i].feature1;
      image2_points[i] = correspondences[i].feature2;
    }

    Eigen::Matrix3d homography_matrix;
    if (!FourPointHomography(image1_points,
                             image2_points,
                             &homography_matrix)) {
      return false;
    }

    homography->emplace_back(homography_matrix);
    return true;
  }

  // The error for a correspondences given a model. This is the asymmetric
  // distance that measures reprojection error in one image.
  double Error(const FeatureCorrespondence& correspondence,
               const Eigen::Matrix3d& homography) const {
    const Eigen::Vector3d reprojected_point =
        homography * correspondence.feature1.homogeneous();
    return (correspondence.feature2 - reprojected_point.hnormalized())
        .squaredNorm();
  }

 private:
  DISALLOW_COPY_AND_ASSIGN(HomographyEstimator);
};

}  // namespace

bool EstimateHomography(
    const RansacParameters& ransac_params,
    const RansacType& ransac_type,
    const std::vector<FeatureCorrespondence>& correspondences,
    Eigen::Matrix3d* homography,
    RansacSummary* ransac_summary) {
  HomographyEstimator homography_estimator;
  std::unique_ptr<SampleConsensusEstimator<HomographyEstimator> > ransac =
      CreateAndInitializeRansacVariant(ransac_type,
                                       ransac_params,
                                       homography_estimator);
  // Estimate the homography.
  return ransac->Estimate(correspondences, homography, ransac_summary);
}

}  // namespace theia


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_CSCH_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_CSCH_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-hyperbolic
    Function object implementing csch capabilities

    hyperbolic cosecant: \f$1/\sinh(x)\f$.

    @par Semantic:

    For every parameter of floating type @c T

    @code
    T r = csch(x);
    @endcode

    @see rec, sinh

  **/
  Value csch(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/csch.hpp>
#include <boost/simd/function/simd/csch.hpp>

#endif


#include "MatrixStack.h"

#include <cassert>
#include <stdio.h>
#include <vector>

#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;

MatrixStack::MatrixStack() {
  mstack = make_shared<stack<Matrix4f>>();
  mstack->push(Matrix4f::Identity());
}

MatrixStack::~MatrixStack() {}

void MatrixStack::pushMatrix() {
  const Matrix4f &top = mstack->top();
  mstack->push(top);
  assert(mstack->size() < 100);
}

void MatrixStack::popMatrix() {
  assert(!mstack->empty());
  mstack->pop();
  // There should always be one matrix left.
  assert(!mstack->empty());
}

void MatrixStack::loadIdentity() {
  Matrix4f &top = mstack->top();
  top = Matrix4f::Identity();
}

void MatrixStack::translate(const Vector3f &t) {
  Matrix4f &top = mstack->top();
  Matrix4f E = Matrix4f::Identity();
  E(0, 3) = t(0);
  E(1, 3) = t(1);
  E(2, 3) = t(2);
  top *= E;
}

void MatrixStack::translate(float x, float y, float z) {
  translate(Vector3f(x, y, z));
}

void MatrixStack::scale(const Vector3f &s) {
  Matrix4f &top = mstack->top();
  Matrix4f E = Matrix4f::Identity();
  E(0, 0) = s(0);
  E(1, 1) = s(1);
  E(2, 2) = s(2);
  top *= E;
}

void MatrixStack::scale(float x, float y, float z) { scale(Vector3f(x, y, z)); }

void MatrixStack::scale(float s) { scale(Vector3f(s, s, s)); }

void MatrixStack::rotate(float angle, const Vector3f &axis) {
  Matrix4f &top = mstack->top();
  Matrix4f E = Matrix4f::Identity();
  E.block<3, 3>(0, 0) =
      AngleAxisf(angle * M_PI / 180.0f, axis.normalized()).toRotationMatrix();
  top *= E;
}

void MatrixStack::rotate(float angle, float x, float y, float z) {
  rotate(angle, Vector3f(x, y, z));
}

void MatrixStack::multMatrix(const Matrix4f &matrix) {
  Matrix4f &top = mstack->top();
  top *= matrix;
}

void MatrixStack::ortho(float left, float right, float bottom, float top,
                        float zNear, float zFar) {
  assert(left != right);
  assert(bottom != top);
  assert(zFar != zNear);
  // Sets the top of the stack
  Matrix4f &M = mstack->top();
  M = Matrix4f::Zero();
  M(0, 0) = 2.0f / (right - left);
  M(1, 1) = 2.0f / (top - bottom);
  M(2, 2) = -2.0f / (zFar - zNear);
  M(0, 3) = -(right + left) / (right - left);
  M(1, 3) = -(top + bottom) / (top - bottom);
  M(2, 3) = -(zFar + zNear) / (zFar - zNear);
  M(3, 3) = 1.0f;
}

void MatrixStack::ortho2D(float left, float right, float bottom, float top) {
  ortho(left, right, bottom, top, -1.0, 1.0);
}

void MatrixStack::perspective(float fovy, float aspect, float zNear,
                              float zFar) {
  assert(fovy != 0.0f);
  assert(aspect != 0.0f);
  assert(zFar != zNear);
  // Sets the top of the stack
  Matrix4f &M = mstack->top();
  M = Matrix4f::Zero();
  float tanHalfFovy = tan(0.5f * fovy * M_PI / 180.0f);
  M(0, 0) = 1.0f / (aspect * tanHalfFovy);
  M(1, 1) = 1.0f / (tanHalfFovy);
  M(2, 2) = -(zFar + zNear) / (zFar - zNear);
  M(2, 3) = -(2.0f * zFar * zNear) / (zFar - zNear);
  M(3, 2) = -1.0f;
}

void MatrixStack::frustum(float left, float right, float bottom, float top,
                          float nearval, float farval) {
  // http://cgit.freedesktop.org/mesa/mesa/tree/src/mesa/math/m_matrix.c
  float x, y, a, b, c, d;
  x = (2.0f * nearval) / (right - left);
  y = (2.0f * nearval) / (top - bottom);
  a = (right + left) / (right - left);
  b = (top + bottom) / (top - bottom);
  c = -(farval + nearval) / (farval - nearval);
  d = -(2.0f * farval * nearval) / (farval - nearval);

  // Sets the top of the stack
  Matrix4f &M = mstack->top();
  M(0, 0) = x;
  M(0, 1) = 0.0f;
  M(0, 2) = a;
  M(0, 3) = 0.0f;
  M(1, 0) = 0.0f;
  M(1, 1) = y;
  M(1, 2) = b;
  M(1, 3) = 0.0f;
  M(2, 0) = 0.0f;
  M(2, 1) = 0.0f;
  M(2, 2) = c;
  M(2, 3) = d;
  M(3, 0) = 0.0f;
  M(3, 1) = 0.0f;
  M(3, 2) = -1.0f;
  M(3, 3) = 0.0f;
}

void MatrixStack::lookAt(const Vector3f &eye, const Vector3f &center,
                         const Vector3f &up) {
  // http://cgit.freedesktop.org/mesa/mesa/tree/src/glu/mesa/glu.c?h=mesa_3_2_dev
  Vector3f x, y, z;
  z = (eye - center).normalized();
  y = up;
  x = y.cross(z);
  y = z.cross(x);
  x.normalize();
  y.normalize();
  Matrix4f M = Matrix4f::Identity();
  M.block<1, 3>(0, 0) = x;
  M.block<1, 3>(1, 0) = y;
  M.block<1, 3>(2, 0) = z;
  multMatrix(M);
  translate(-eye);
}

void MatrixStack::lookAt(float ex, float ey, float ez, float tx, float ty,
                         float tz, float ux, float uy, float uz) {
  lookAt(Vector3f(ex, ey, ez), Vector3f(tx, ty, tz), Vector3f(ux, uy, uz));
}

const Matrix4f &MatrixStack::topMatrix() const { return mstack->top(); }

void MatrixStack::print(const Matrix4f &mat, const char *name) const {
  if (name) {
    printf("%s = [\n", name);
  }
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      printf("%- 5.2f ", mat(i, j));
    }
    printf("\n");
  }
  if (name) {
    printf("];");
  }
  printf("\n");
}

void MatrixStack::print(const char *name) const { print(mstack->top(), name); }

// void MatrixStack::printStack() const
// {
// 	// Copy everything to a non-const stack
// 	auto tempStack = mstack;
// 	while(!tempStack.empty()) {
// 		Matrix4f &top = tempStack.top();
// 		print(top);
// 		tempStack.pop();
// 	}
// }

// #include <iostream>
// int main(int argc, char **argv)
// {
// 	MatrixStack M;
// 	M.frustum(-1, 1, -2, 2, 0.1, 10.0);
// 	std::cout << M.topMatrix() << std::endl;
// }


//
//  AutoFlipSVD.hpp
//  DOT
//
//  Created by Minchen Li on 6/21/18.
//

#ifndef AutoFlipSVD_hpp
#define AutoFlipSVD_hpp

#include "ImplicitQRSVD.h"

#include <Eigen/Eigen>

#include <iostream>

namespace DOT {

    template<typename MatrixType>
    class AutoFlipSVD : Eigen::JacobiSVD<MatrixType>
    {
    protected:
        bool flipped_U, flipped_V, flipped_sigma;

        typename Eigen::JacobiSVD<MatrixType>::SingularValuesType singularValues_flipped;
        MatrixType matrixU_flipped, matrixV_flipped;

    public:
        AutoFlipSVD(void) {}
        AutoFlipSVD(const MatrixType& mtr, unsigned int computationOptions = 0)
        {
            compute(mtr, computationOptions);
        }

    public:
        template<int dim = MatrixType::RowsAtCompileTime>
        typename std::enable_if<dim == 3, AutoFlipSVD<MatrixType>>::type&
        compute(const MatrixType& mtr, unsigned int computationOptions)
        {
            flipped_U = flipped_V = flipped_sigma = true;
#ifdef USE_IQRSVD
            JIXIE::singularValueDecomposition(mtr,
                                              matrixU_flipped,
                                              singularValues_flipped,
                                              matrixV_flipped);
#else
            if((computationOptions & Eigen::ComputeFullU) ||
               (computationOptions & Eigen::ComputeFullV))
            {
                fastSVD3d(mtr, matrixU_flipped, singularValues_flipped, matrixV_flipped);
            }
            else {
                fastComputeSingularValues3d(mtr, singularValues_flipped);
            }
#endif
            return *this;
        }
        template<int dim = MatrixType::RowsAtCompileTime>
        typename std::enable_if<dim == 2, AutoFlipSVD<MatrixType>>::type&
        compute(const MatrixType& mtr, unsigned int computationOptions)
        {
#ifdef USE_IQRSVD
            flipped_U = flipped_V = flipped_sigma = true;
            JIXIE::singularValueDecomposition(mtr,
                                              matrixU_flipped,
                                              singularValues_flipped,
                                              matrixV_flipped);
#else
            flipped_U = flipped_V = flipped_sigma = false;
            Eigen::JacobiSVD<MatrixType>::compute(mtr, computationOptions);
            flip2d(mtr, computationOptions);
#endif
            return *this;
        }

        void set(const Eigen::Matrix3d& U, const Eigen::Vector3d& Sigma, const Eigen::Matrix3d& V)
        {
            flipped_U = true, flipped_V = true, flipped_sigma = true;
            matrixU_flipped = U;
            singularValues_flipped = Sigma;
            matrixV_flipped = V;
        }

    protected:
        void flip2d(const MatrixType& mtr, unsigned int computationOptions) {
            //!!! this flip algorithm is only valid in 2D
            bool fullUComputed = (computationOptions & Eigen::ComputeFullU);
            bool fullVComputed = (computationOptions & Eigen::ComputeFullV);
            if(fullUComputed && fullVComputed) {
                if(Eigen::JacobiSVD<MatrixType>::m_matrixU.determinant() < 0.0) {
                    matrixU_flipped = Eigen::JacobiSVD<MatrixType>::m_matrixU;
                    matrixU_flipped.col(1) *= -1.0;
                    flipped_U = true;

                    if(!flipped_sigma) {
                        singularValues_flipped = Eigen::JacobiSVD<MatrixType>::m_singularValues;
                    }
                    singularValues_flipped[1] *= -1.0;
                    flipped_sigma = true;
                }
                if(Eigen::JacobiSVD<MatrixType>::m_matrixV.determinant() < 0.0) {
                    matrixV_flipped = Eigen::JacobiSVD<MatrixType>::m_matrixV;
                    matrixV_flipped.col(1) *= -1.0;
                    flipped_V = true;

                    if(!flipped_sigma) {
                        singularValues_flipped = Eigen::JacobiSVD<MatrixType>::m_singularValues;
                    }
                    singularValues_flipped[1] *= -1.0;
                    flipped_sigma = true;
                }
            }
            else if(mtr.determinant() < 0.0) {
                singularValues_flipped = Eigen::JacobiSVD<MatrixType>::m_singularValues;
                singularValues_flipped[1] *= -1.0;
                flipped_sigma = true;
            }

            if(std::isnan(singularValues()[0]) || std::isnan(singularValues()[1])) {
                // degenerated case
                singularValues_flipped.setZero();
                flipped_sigma = true;
                if(fullUComputed && fullVComputed) {
                    matrixU_flipped.setIdentity();
                    matrixV_flipped.setIdentity();
                    flipped_U = flipped_V = true;
                }
            }
        }

        //TODO: merge with IglUtils::computeCofactorMtr
        template<int dim>
        void computeCofactorMtr(const Eigen::Matrix<double, dim, dim>& F,
                                Eigen::Matrix<double, dim, dim>& A)
        {
            switch(dim) {
                case 2:
                    A(0, 0) = F(1, 1);
                    A(0, 1) = -F(1, 0);
                    A(1, 0) = -F(0, 1);
                    A(1, 1) = F(0, 0);
                    break;

                case 3:
                    A(0, 0) = F(1, 1) * F(2, 2) - F(1, 2) * F(2, 1);
                    A(0, 1) = F(1, 2) * F(2, 0) - F(1, 0) * F(2, 2);
                    A(0, 2) = F(1, 0) * F(2, 1) - F(1, 1) * F(2, 0);
                    A(1, 0) = F(0, 2) * F(2, 1) - F(0, 1) * F(2, 2);
                    A(1, 1) = F(0, 0) * F(2, 2) - F(0, 2) * F(2, 0);
                    A(1, 2) = F(0, 1) * F(2, 0) - F(0, 0) * F(2, 1);
                    A(2, 0) = F(0, 1) * F(1, 2) - F(0, 2) * F(1, 1);
                    A(2, 1) = F(0, 2) * F(1, 0) - F(0, 0) * F(1, 2);
                    A(2, 2) = F(0, 0) * F(1, 1) - F(0, 1) * F(1, 0);
                    break;

                default:
                    assert(0 && "dim not 2 or 3");
                    break;
            }
        }
        void fastEigenvalues(const Eigen::Matrix3d& A_Sym,
                             Eigen::Vector3d& lambda)
        // 24 mults, 20 adds, 1 atan2, 1 sincos, 2 sqrts
        {
            using T = double;
            using std::max;
            using std::swap;
            T m = ((T)1 / 3) * (A_Sym(0, 0) + A_Sym(1, 1) + A_Sym(2, 2));
            T a00 = A_Sym(0, 0) - m;
            T a11 = A_Sym(1, 1) - m;
            T a22 = A_Sym(2, 2) - m;
            T a12_sqr = A_Sym(0, 1) * A_Sym(0, 1);
            T a13_sqr = A_Sym(0, 2) * A_Sym(0, 2);
            T a23_sqr = A_Sym(1, 2) * A_Sym(1, 2);
            T p = ((T)1 / 6) * (a00 * a00 + a11 * a11 + a22 * a22 + 2 * (a12_sqr + a13_sqr + a23_sqr));
            T q = (T).5 * (a00 * (a11 * a22 - a23_sqr) - a11 * a13_sqr - a22 * a12_sqr) + A_Sym(0, 1) * A_Sym(0, 2) * A_Sym(1, 2);
            T sqrt_p = sqrt(p);
            T disc = p * p * p - q * q;
            T phi = ((T)1 / 3) * atan2(sqrt(max((T)0, disc)), q);
            T c = cos(phi), s = sin(phi);
            T sqrt_p_cos = sqrt_p * c;
            T root_three_sqrt_p_sin = sqrt((T)3) * sqrt_p * s;
            lambda(0) = m + 2 * sqrt_p_cos;
            lambda(1) = m - sqrt_p_cos - root_three_sqrt_p_sin;
            lambda(2) = m - sqrt_p_cos + root_three_sqrt_p_sin;
            if (lambda(0) < lambda(1))
                swap(lambda(0), lambda(1));
            if (lambda(1) < lambda(2))
                swap(lambda(1), lambda(2));
            if (lambda(0) < lambda(1))
                swap(lambda(0), lambda(1));
        }
        void fastEigenvectors(const Eigen::Matrix3d& A_Sym,
                              const Eigen::Vector3d& lambda,
                              Eigen::Matrix3d& V)
        // 71 mults, 44 adds, 3 divs, 3 sqrts
        {
            // flip if necessary so that first eigenvalue is the most different
            using T = double;
            using std::sqrt;
            using std::swap;
            bool flipped = false;
            Eigen::Vector3d lambda_flip(lambda);
            if (lambda(0) - lambda(1) < lambda(1) - lambda(2)) { // 2a
                swap(lambda_flip(0), lambda_flip(2));
                flipped = true;
            }

            // get first eigenvector
            Eigen::Matrix3d C1;
            computeCofactorMtr<3>(A_Sym - lambda_flip(0) * Eigen::Matrix3d::Identity(), C1);
            Eigen::Matrix3d::Index i;
            T norm2 = C1.colwise().squaredNorm().maxCoeff(&i); // 3a + 12m+6a + 9m+6a+1d+1s = 21m+15a+1d+1s
            Eigen::Vector3d v1;
            if (norm2 != 0) {
                T one_over_sqrt = (T)1 / sqrt(norm2);
                v1 = C1.col(i) * one_over_sqrt;
            }
            else
                v1 << 1, 0, 0;

            // form basis for orthogonal complement to v1, and reduce A to this space
            Eigen::Vector3d v1_orthogonal = v1.unitOrthogonal(); // 6m+2a+1d+1s (tweak: 5m+1a+1d+1s)
            Eigen::Matrix<T, 3, 2> other_v;
            other_v.col(0) = v1_orthogonal;
            other_v.col(1) = v1.cross(v1_orthogonal); // 6m+3a (tweak: 4m+1a)
            Eigen::Matrix2d A_reduced = other_v.transpose() * A_Sym * other_v; // 21m+12a (tweak: 18m+9a)

            // find third eigenvector from A_reduced, and fill in second via cross product
            Eigen::Matrix2d C3;
            computeCofactorMtr<2>(A_reduced - lambda_flip(2) * Eigen::Matrix2d::Identity(), C3);
            Eigen::Matrix2d::Index j;
            norm2 = C3.colwise().squaredNorm().maxCoeff(&j); // 3a + 12m+6a + 9m+6a+1d+1s = 21m+15a+1d+1s
            Eigen::Vector3d v3;
            if (norm2 != 0) {
                T one_over_sqrt = (T)1 / sqrt(norm2);
                v3 = other_v * C3.col(j) * one_over_sqrt;
            }
            else
                v3 = other_v.col(0);

            Eigen::Vector3d v2 = v3.cross(v1); // 6m+3a

            // finish
            if (flipped) {
                V.col(0) = v3;
                V.col(1) = v2;
                V.col(2) = -v1;
            }
            else {
                V.col(0) = v1;
                V.col(1) = v2;
                V.col(2) = v3;
            }
        }
        void fastSolveEigenproblem(const Eigen::Matrix3d& A_Sym,
                                   Eigen::Vector3d& lambda,
                                   Eigen::Matrix3d& V)
        // 71 mults, 44 adds, 3 divs, 3 sqrts
        {
            fastEigenvalues(A_Sym, lambda);
            fastEigenvectors(A_Sym, lambda, V);
        }

        void fastSVD3d(const Eigen::Matrix3d& A,
                       Eigen::Matrix3d& U,
                       Eigen::Vector3d& singular_values,
                       Eigen::Matrix3d& V)
        // 182 mults, 112 adds, 6 divs, 11 sqrts, 1 atan2, 1 sincos
        {
            using T = double;
            // decompose normal equations
            Eigen::Vector3d lambda;
            fastSolveEigenproblem(A.transpose() * A, lambda, V);

            // compute singular values
            if (lambda(2) < 0)
                lambda = (lambda.array() >= (T)0).select(lambda, (T)0);
            singular_values = lambda.array().sqrt();
            if (A.determinant() < 0)
                singular_values(2) = -singular_values(2);

            // compute singular vectors
            U.col(0) = A * V.col(0);
            T norm = U.col(0).norm();
            if (norm != 0) {
                T one_over_norm = (T)1 / norm;
                U.col(0) = U.col(0) * one_over_norm;
            }
            else
                U.col(0) << 1, 0, 0;
            Eigen::Vector3d v1_orthogonal = U.col(0).unitOrthogonal();
            Eigen::Matrix<T, 3, 2> other_v;
            other_v.col(0) = v1_orthogonal;
            other_v.col(1) = U.col(0).cross(v1_orthogonal);
            Eigen::Vector2d w = other_v.transpose() * A * V.col(1);
            norm = w.norm();
            if (norm != 0) {
                T one_over_norm = (T)1 / norm;
                w = w * one_over_norm;
            }
            else
                w << 1, 0;
            U.col(1) = other_v * w;
            U.col(2) = U.col(0).cross(U.col(1));
        }

        void fastComputeSingularValues3d(const Eigen::Matrix3d& A,
                                         Eigen::Vector3d& singular_values)
        {
            using T = double;
            // decompose normal equations
            Eigen::Vector3d lambda;
            fastEigenvalues(A.transpose() * A, lambda);

            // compute singular values
            if (lambda(2) < 0)
                lambda = (lambda.array() >= (T)0).select(lambda, (T)0);
            singular_values = lambda.array().sqrt();
            if (A.determinant() < 0)
                singular_values(2) = -singular_values(2);
        }

    public:
        const typename Eigen::JacobiSVD<MatrixType>::SingularValuesType& singularValues(void) const {
            if(flipped_sigma) {
                return singularValues_flipped;
            }
            else {
                return Eigen::JacobiSVD<MatrixType>::singularValues();
            }
        }
        const MatrixType& matrixU(void) const {
            if(flipped_U) {
                return matrixU_flipped;
            }
            else {
                return Eigen::JacobiSVD<MatrixType>::matrixU();
            }
        }
        const MatrixType& matrixV(void) const {
            if(flipped_V) {
                return matrixV_flipped;
            }
            else {
                return Eigen::JacobiSVD<MatrixType>::matrixV();
            }
        }

        void setIdentity(void) {
            flipped_sigma=true;
            flipped_V=true;
            flipped_U=true;

            matrixU_flipped.setIdentity();
            matrixV_flipped.setIdentity();
            singularValues_flipped.setOnes();
        }
    };

}

#endif /* AutoFlipSVD_hpp */


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

#ifndef RW_MATH_JACOBIAN_HPP
#define RW_MATH_JACOBIAN_HPP

/**
 * @file math/Jacobian.hpp
 */

#if !defined(SWIG)
#include <rw/common/Serializable.hpp>
#include <rw/math/Q.hpp>
#include <rw/math/Rotation3D.hpp>
#include <rw/math/Transform3D.hpp>
#include <rw/math/VelocityScrew6D.hpp>

#include <Eigen/Core>
#endif
namespace rw {
namespace math {

    /** @addtogroup math */
    /*@{*/

    /**
     * @brief A Jacobian class. A jacobian with m rows and n columns.
     *
     * An ordinary robot jacobian defined over the joints 0 to n with
     * configuration \b q is expressed as a @f$ 6\times n @f$ matrix:
     * \f[
     * \robabx{0}{n}{\bf{J}}(\bf{q}) = [
     * \robabx{0}{1}{\bf{J}}(\bf{q}),
     * \robabx{1}{2}{\bf{J}}(\bf{q}),...,
     * \robabx{n-1}{n}{\bf{J}}(\bf{q}) ]
     * \f]
     *
     */
    class Jacobian
    {
      public:
        //! @brief The type of the internal Eigen matrix implementation.
        typedef Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > Base;

        /**
         * @brief Creates an empty @f$ m\times n @f$ (uninitialized) Jacobian matrix
         *
         * @param m [in] number of rows
         *
         * @param n [in] number of columns
         */
        Jacobian (size_t m, size_t n) : _jac (m, n) {}

        /**
         * @brief Default constructor
         */
        Jacobian () {}

        /**
           @brief The number of rows.
         */
        size_t size1 () const { return _jac.rows (); }

        /**
           @brief The number of columns.
         */
        size_t size2 () const { return _jac.cols (); }

        /**
         * @brief Creates an empty @f$ 6\times n @f$ (uninitialized) Jacobian matrix
         *
         * @param n [in] number of columns
         */
        explicit Jacobian (size_t n) : _jac (6, n) {}

        /**
         * @brief Creates a Jacobian from a Eigen::MatrixBase
         *
         * @param r [in] an Eigen Matrix
         */
        template< class R > explicit Jacobian (const Eigen::MatrixBase< R >& r) : _jac (r) {}

        /**
         * @brief Construct zero initialized Jacobian.
         * @param size1 [in] number of rows.
         * @param size2 [in] number of columns.
         * @return zero-initialized jacobian.
         */
        static Jacobian zero (size_t size1, size_t size2)
        {
            return Jacobian (
                Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic >::Zero (size1, size2));
        }

        /**
         * @brief Accessor for the internal Eigen matrix state.
         */
        Base& e () { return _jac; }

        /**
         * @brief Accessor for the internal Eigen matrix state.
         */
        const Base& e () const { return _jac; }

#if !defined(SWIG)
        /**
         * @brief Returns reference to matrix element
         * @param row [in] row
         * @param column [in] column
         * @return reference to the element
         */
        double& operator() (size_t row, size_t column) { return _jac (row, column); }

        /**
         * @brief Returns reference to matrix element
         * @param row [in] row
         * @param column [in] column
         * @return reference to the element
         */
        const double& operator() (size_t row, size_t column) const { return _jac (row, column); }
#else
        MATRIXOPERATOR (double);
#endif
        /**
         * @brief Get an element of the jacobian.
         * @param row [in] the row.
         * @param col [in] the column.
         * @return reference to the element.
         */
        double& elem (size_t row, size_t col) { return _jac (row, col); }

#if !defined(SWIGJAVA)

        /**
         * @brief Creates the velocity transform jacobian
         * @f$ \robabcdx{a}{b}{a}{b}{\bf{J_v}} @f$
         * for transforming both the reference frame and the velocity
         * reference point from one frame \b b to another frame \b a
         *
         * @param aTb [in] @f$ \robabx{a}{b}{\bf{T}} @f$
         *
         * @return @f$ \robabcdx{a}{b}{a}{b}{\bf{J_v}} @f$
         *
         * \f[
         * \robabcdx{a}{b}{a}{b}{\bf{J_v}} =
         * \left[
         *  \begin{array}{cc}
         *    \robabx{a}{b}{\mathbf{R}} & S(\robabx{a}{b}{\mathbf{d}})\robabx{a}{b}{\mathbf{R}} \\
         *    \mathbf{0}^{3x3} & \robabx{a}{b}{\mathbf{R}}
         *  \end{array}
         * \right]
         * \f]
         *
         * Change the frame of reference from \b b to frame \b a and reference point
         * from frame \b a to frame \b b:
         * @f$ \robabx{a}{b}{\bf{J}} =  \robabcdx{a}{b}{a}{b}{\bf{J}_v} \cdot \robabx{b}{a}{\bf{J}}
         * @f$
         */

#endif
        explicit Jacobian (const rw::math::Transform3D<double>& aTb);

#if !defined(SWIGJAVA)
        /**
         * @brief Creates the velocity transform jacobian
         * @f$ \robabcdx{a}{b}{i}{i}{\bf{J_v}} @f$
         * for transforming a velocity screw from one frame of reference \b b to
         * another frame \b a
         *
         * @param aRb [in] @f$ \robabx{a}{b}{\bf{R}} @f$
         *
         * @return @f$ \robabcdx{a}{b}{i}{i}{\bf{J}_v} @f$
         *
         * \f[
         * \robabcdx{a}{b}{i}{i}{\bf{J_v}} =
         * \left[
         *  \begin{array}{cc}
         *    \robabx{a}{b}{\mathbf{R}} & \mathbf{0}^{3x3} \\
         *    \mathbf{0}^{3x3} & \robabx{a}{b}{\mathbf{R}}
         *  \end{array}
         * \right]
         * \f]
         *
         * Change the frame of reference from \b b to frame \b a :
         * @f$ \robabx{a}{c}{\bf{J}} =  \robabcdx{a}{b}{c}{c}{\bf{J}_v} \cdot \robabx{b}{c}{\bf{J}}
         * @f$
         *
         */

#endif
        explicit Jacobian (const rw::math::Rotation3D<>& aRb);

#if !defined(SWIGJAVA)

        /**
         * @brief Creates the velocity transform jacobian
         * @f$ \robabcdx{i}{i}{b}{a}{\bf{J}_v} @f$
         * for transforming the reference point of a velocity screw from one
         * frame \b b to another frame \b a
         *
         * @param aPb [in] @f$ \robabx{a}{b}{\bf{P}} @f$
         *
         * @return @f$ \robabcdx{i}{i}{b}{a}{\bf{J}_v} @f$
         *
         * \f[
         * \robabcdx{i}{i}{b}{a}{\bf{J}_v} =
         * \left[
         *  \begin{array}{cc}
         *    \bf{I}^{3x3} & S(\robabx{a}{b}{\bf{P}}) \\
         *    \bf{0}^{3x3} & \bf{I}^{3x3}
         *  \end{array}
         * \right]
         * \f]
         *
         *  transforming the reference point of a Jacobian from
         * frame \b c to frame \b d :
         * @f$ \robabx{a}{d}{\mathbf{J}} =  \robabcdx{a}{a}{c}{d}{\mathbf{J_v}} \cdot
         * \robabx{a}{c}{\mathbf{J}} @f$
         */

#endif
        explicit Jacobian (const rw::math::Vector3D<>& aPb);

        /**
         * @brief add rotation jacobian to a specific row and column in this jacobian
         * @param part
         * @param row
         * @param col
         */
        void addRotation (const rw::math::Vector3D<>& part, size_t row, size_t col);

        /**
         * @brief add position jacobian to a specific row and column in this jacobian
         * @param part
         * @param row
         * @param col
         */
        void addPosition (const rw::math::Vector3D<>& part, size_t row, size_t col);

      private:
        Base _jac;
    };

    /**
     * @brief Calculates velocity vector
     * @param Jq [in] the jacobian @f$ \mathbf{J}_{\mathbf{q}} @f$
     * @param dq [in] the joint velocity vector @f$ \dot{\mathbf{q}} @f$
     * @return the velocity vector @f$ \mathbf{\nu} @f$
     * @relates Jacobian
     */
    inline const rw::math::VelocityScrew6D<> operator* (const Jacobian& Jq, const rw::math::Q& dq)
    {
        return rw::math::VelocityScrew6D<> (Jq.e () * dq.e ());
    }

    /**
     * @brief Calculates joint velocities
     *
     * @param JqInv [in] the inverse jacobian @f$ \mathbf{J}_{\mathbf{q}}^{-1} @f$
     *
     * @param v [in] the velocity vector @f$ \mathbf{\nu} @f$
     *
     * @return the joint velocity vector @f$ \dot{\mathbf{q}} @f$
     *
     * @relates Jacobian
     */
    inline const rw::math::Q operator* (const Jacobian& JqInv, const rw::math::VelocityScrew6D<>& v)
    {
        return rw::math::Q (JqInv.e () * v.e ());
        // prod(JqInv.m(), v.m()));
    }

    /**
     * @brief Multiplies jacobians @f$ \mathbf{J} = \mathbf{J}_1 *
     * \mathbf{J}_2 @f$
     *
     * @param j1 [in] @f$ \mathbf{J}_1 @f$
     *
     * @param j2 [in] @f$ \mathbf{J}_2 @f$
     *
     * @return @f$ \mathbf{J} @f$
     *
     * @relates Jacobian
     */
    inline const Jacobian operator* (const Jacobian& j1, const Jacobian& j2)
    {
        return Jacobian (j1.e () * j2.e ());
        // return Jacobian(prod(j1.m(), j2.m()));
    }

    /**
       @brief Streaming operator.

       @relates Jacobian
    */
    inline std::ostream& operator<< (std::ostream& out, const Jacobian& v) { return out << v.e (); }

    /**
       @brief Rotates each column of \b v by \b r.

       The Jacobian must be of height 6.

       @relates Jacobian
    */
    const Jacobian operator* (const rw::math::Rotation3D<>& r, const Jacobian& v);

    /*@}*/
}
}    // namespace rw::math

namespace rw { namespace common {
    class OutputArchive;
    class InputArchive;
    namespace serialization {
        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Jacobian
         */
        template<>
        void write (const rw::math::Jacobian& sobject, rw::common::OutputArchive& oarchive,
                    const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Jacobian
         */
        template<>
        void read (rw::math::Jacobian& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);
    }    // namespace serialization
}}       // namespace rw::common

#endif    // end include guard


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file chi_squared.hpp
 * \date August 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <boost/math/distributions.hpp>

#include <fl/util/meta.hpp>
#include <fl/util/scalar_matrix.hpp>

#include "uniform_distribution.hpp"
#include "interface/evaluation.hpp"
#include "interface/standard_gaussian_mapping.hpp"

namespace fl
{

/**
 * \ingroup distributions
 *
 * \brief ChiSquared represents a univariate Chi-squared distribution
 * \f$\chi^2_k\f$, with \f$k \in \mathbb{N}^{*}\f$ degrees-of-freedom
 */
class ChiSquared
    : public Evaluation<ScalarMatrix>,
      public StandardGaussianMapping<ScalarMatrix, 1>
{
private:
    typedef StandardGaussianMapping<ScalarMatrix, 1> StdGaussianMappingBase;

public:
    /**
     * \brief Represents the StandardGaussianMapping standard variate type which
     *        is of the same dimension as the \c TDistribution \c Variate. The
     *        StandardVariate type is used to sample from a standard normal
     *        Gaussian and map it to this \c TDistribution
     */
    typedef ScalarMatrix Variate;

    /**
     * \brief StandardVariate type which is used to sample from and mapped it
     * into the distribution space
     */
    typedef typename StdGaussianMappingBase::StandardVariate StandardVariate;

public:
    /**
     * Creates a dynamic or fixed size t-distribution.
     *
     * \param degrees_of_freedom
     *                  t-distribution degree-of-freedom
     * \param dimension Dimension of the distribution. The default is defined by
     *                  the dimension of the variable type \em Vector. If the
     *                  size of the Vector at compile time is fixed, this will
     *                  be adapted. For dynamic-sized Variable the dimension is
     *                  initialized to 0.
     */
    explicit ChiSquared(Real degrees_of_freedom)
       : StdGaussianMappingBase(1),
        chi2_(degrees_of_freedom)
    { }

    /**
     * \brief Overridable default destructor
     */
    virtual ~ChiSquared() noexcept { }

    /**
     * \brief Returns aa t-distribution sample of the type \c Variate determined
     * by mapping a standard normal sample into the t-distribution sample space
     *
     * \param n    Standard normal sample
     *
     * \throws See Gaussian<Variate>::map_standard_normal
     */
    virtual Variate map_standard_uniform(const StandardVariate& n) const
    {
        return boost::math::quantile(chi2_, n);
    }

    /**
     * \brief Returns a t-distribution sample of the type \c Variate determined
     * by mapping a standard normal sample into the t-distribution sample space
     *
     * \param n    Standard normal sample
     *
     * \throws See Gaussian<Variate>::map_standard_normal
     */
    Variate map_standard_normal(const StandardVariate& n) const override
    {
        return map_standard_uniform(uniform_.map_standard_normal(n));
    }

    /**
     * \brief Returns the log probability of the given sample \c variate
     *
     * \param variate sample which should be evaluated
     *
     * \throws See Gaussian<Variate>::has_full_rank()
     */
    Real log_probability(const Variate& variate) const override
    {
        assert(variate.size() == 1);
        return std::log(probability(variate));
    }

    /**
     * \brief Evaluates the probability for the specified variate.
     *
     * \param variate Sample \f$x\f$ to evaluate
     *
     * \return \f$p(x)\f$
     */
    Real probability(const Variate& variate) const override
    {
        assert(variate.size() == 1);
        return boost::math::pdf(chi2_, variate);
    }

    /**
     * \brief Returns t-distribution degree-of-freedom
     */
    Real degrees_of_freedom() const
    {
        return chi2_.degrees_of_freedom();
    }

    /**
     * \brief Sets t-distribution degree-of-freedom
     */
    void degrees_of_freedom(Real dof)
    {
        chi2_ = boost::math::chi_squared_distribution<Real>(dof);
    }

protected:
    /** \cond internal */
    UniformDistribution uniform_;
    boost::math::chi_squared_distribution<Real> chi2_;
    /** \endcond */
};

}


#pragma once

#include "PolynomialBasisGen.hh"
#include "RBFKernel.hh"
#include "FiniteDifferentiator.hh"
#include <Eigen/Dense>
#include <utility>
#include <vector>

namespace kt84 {

template <int _DimIn, int _DimOut, class _RBFKernel_Core, int _DegreePolynomial>
struct RBF
    : public FiniteDifferentiator<RBF<_DimIn, _DimOut, _RBFKernel_Core, _DegreePolynomial>, _DimIn, _DimOut>
{
    enum {
        DimIn  = _DimIn,
        DimOut = _DimOut,
        DegreePolynomial = _DegreePolynomial,
    };

    typedef Eigen::Matrix<double, DimIn , 1> Point;                 // TODO: treat 1x1 matrix as scalar using Matrix11ToScalar
    typedef Eigen::Matrix<double, DimOut, 1> Value;
    typedef Eigen::Matrix<double, DimOut, DimIn> Gradient;
    typedef PolynomialBasisGenT<DimIn, DegreePolynomial> PolynomialBasisGen;
    typedef RBFKernel_Bivariate<DimIn, _RBFKernel_Core> Kernel;
    typedef std::pair<Point, Value> Constraint;

    std::vector<Constraint> constraints;
    Kernel kernel;
    Eigen::Matrix<double, -1, DimOut> weights;
    Eigen::MatrixXd A_matrix;
    Eigen::ColPivHouseholderQR<Eigen::MatrixXd> A_factorized;

    void clear_constraints() {
        constraints.clear();
    }
    void add_constraint(const Point& point, const Value& value) {
        constraints.push_back(Constraint(point, value));
    }
    void factorize() {
        const int P = PolynomialBasisGen::DimOut;
        const size_t n = constraints.size();
        const int m = n + P;
        A_matrix = Eigen::MatrixXd::Zero(m, m);
        for (size_t i = 0; i < n; ++i) {
            const Point& point_i = constraints[i].first;
            // rbf part
            A_matrix(i, i) = kernel.univariate(0);
            for (size_t j = i + 1; j < n; ++j) {
                const Point& point_j = constraints[j].first;
                A_matrix(i, j) = A_matrix(j, i) = kernel(point_i, point_j);
            }
            // polynomial part
            A_matrix.block<P, 1>(n, i) << PolynomialBasisGen::basis(point_i);
            A_matrix.block<1, P>(i, n) = A_matrix.block<P, 1>(n, i).transpose();
        }
        A_factorized.compute(A_matrix);         // factorize
    }
    void solve() {
        const int P = PolynomialBasisGen::DimOut;
        const size_t n = constraints.size();
        const int m = n + P;
        Eigen::Matrix<double, -1, DimOut> b;
        b.setZero(m, DimOut);
        // constraint part
        for (size_t i = 0; i < n; ++i) {
            const Value& value_i = constraints[i].second;
            b.row(i).transpose() << value_i;
        }
        // polynomial part is just 0
        weights = A_factorized.solve(b);        // solve
    }
    void factorize_and_solve() {
        factorize();
        solve();
    }
    Value operator()(const Point& point) const {
        const int P = PolynomialBasisGen::DimOut;
        Value result = Value::Zero();
        // rbf part
        for (size_t i = 0; i < constraints.size(); ++i) {
            const Point& point_i = constraints[i].first;
            result += kernel(point, point_i) * weights.row(i).transpose();
        }
        // polynomial part
        auto basis = PolynomialBasisGen::basis(point);
        result += (basis.transpose() * weights.bottomRows(P)).transpose();
        return result;
    }
    Gradient gradient(const Point& point) const {
        const int P = PolynomialBasisGen::DimOut;
        Gradient result = Gradient::Zero();
        // rbf part
        for (size_t i = 0; i < constraints.size(); ++i) {
            const Point& point_i = constraints[i].first;
            result += weights.row(i).transpose() * kernel.gradient(point, point_i);
        }
        // polynomial part
        auto b_gradient = PolynomialBasisGen::gradient(point);
        result += weights.bottomRows(P).transpose() * b_gradient;
        return result;
    }
};

}


/**
 * @file expfittedupwind_main.cc
 * @brief NPDE homework ExpFittedUpwind
 * @author Amélie Loher, Philippe Peter
 * @date 07.01.2020
 * @copyright Developed at ETH Zurich
 */

#include <lf/assemble/assemble.h>
#include <lf/fe/fe.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/utils/utils.h>
#include <lf/refinement/refinement.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <cmath>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <memory>

#include "expfittedupwind.h"

int main() {
  // Define Mesh-independent Data:
  //====================
  // Your code goes here
  //====================

  // Output file
  std::ofstream L2output;
  L2output.open("L2error.txt");
  L2output << "No. of dofs, L2 error" << std::endl;

  // generate a mesh hierarchy:
  unsigned int reflevels = 6;
  std::unique_ptr<lf::mesh::MeshFactory> mesh_factory_ptr =
      std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::mesh::utils::TPTriagMeshBuilder builder(std::move(mesh_factory_ptr));
  builder.setBottomLeftCorner(Eigen::Vector2d{0.0, 0.0})
      .setTopRightCorner(Eigen::Vector2d{1.0, 1.0})
      .setNumXCells(2)
      .setNumYCells(2);
  auto top_mesh = builder.Build();

  std::shared_ptr<lf::refinement::MeshHierarchy> multi_mesh_p =
      lf::refinement::GenerateMeshHierarchyByUniformRefinemnt(top_mesh,
                                                              reflevels);
  lf::refinement::MeshHierarchy& multi_mesh{*multi_mesh_p};
  multi_mesh.PrintInfo(std::cout);

  // get number of levels:
  auto L = multi_mesh.NumLevels();

  // perform computations on all levels:
  for (int l = 0; l < L; ++l) {
    // Compute finite element solution and compute L2 error on current level:
    double L2_err = 1.0;

    // get current mesh and fe space
    auto mesh_p = multi_mesh.getMesh(l);
    auto fe_space =
        std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

    const lf::assemble::DofHandler& dofh{fe_space->LocGlobMap()};
    const lf::uscalfe::size_type N_dofs(dofh.NumDofs());

    //====================
    // Your code goes here
    //====================

    L2output << N_dofs << ", " << L2_err << std::endl;
    std::cout << N_dofs << "," << L2_err << std::endl;
  }

  L2output.close();

  // Plot the computed L2 error
  std::system("python3 " CURRENT_SOURCE_DIR "/plot_error.py " CURRENT_BINARY_DIR
              "/L2error.txt " CURRENT_BINARY_DIR "/results.eps");

  return 0;
}