//
// Created by david on 2019-08-07.
//

#include <complex.h>
#undef I
#include <complex>
#ifndef lapack_complex_float
    #define lapack_complex_float std::complex<float>
#endif
#ifndef lapack_complex_double
    #define lapack_complex_double std::complex<double>
#endif

// complex must be included before lapacke!
#if __has_include(<mkl_lapacke.h>)
    #include <mkl_lapacke.h>
#elif __has_include(<openblas/lapacke.h>)
    #include <openblas/lapacke.h>
#else
    #include <lapacke.h>
#endif

#include <Eigen/Core>
#include <general/class_tic_toc.h>
#include <math/svd.h>

namespace svd {
    template<typename Scalar>
    void print_matrix_lapacke(const Scalar *mat_ptr, long rows, long cols, long dec = 8) {
        auto A = Eigen::Map<const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>>(mat_ptr, rows, cols);
        svd::log->warn("Print matrix of dimensions {}x{}\n", rows, cols);
        for(long r = 0; r < A.rows(); r++) {
            if constexpr(std::is_same_v<Scalar, std::complex<double>>)
                for(long c = 0; c < A.cols(); c++) fmt::print("({1:.{0}f},{2:+.{0}f}) ", dec, std::real(A(r, c)), std::imag(A(r, c)));
            else
                for(long c = 0; c < A.cols(); c++) fmt::print("{1:.{0}f} ", dec, A(r, c));
            fmt::print("\n");
        }
    }
}

template<typename Scalar>
std::tuple<svd::solver::MatrixType<Scalar>, svd::solver::VectorType<Scalar>, svd::solver::MatrixType<Scalar>, long>
    svd::solver::do_svd_lapacke(const Scalar *mat_ptr, long rows, long cols, std::optional<long> rank_max) {
    // Setup useful sizes
    int rowsA = static_cast<int>(rows);
    int colsA = static_cast<int>(cols);
    int sizeS = std::min(rowsA, colsA);
    if(not rank_max.has_value()) rank_max = std::min(rows, cols);

    // Setup the SVD solver
    bool use_jacobi = static_cast<size_t>(sizeS) < switchsize;
    if(use_jacobi and rows < cols) {
        // The jacobi routine needs a tall matrix
        t_adj->tic();
        svd::log->trace("Transposing {}x{} into tall matrix {}x{}", rows, cols, cols, rows);
        MatrixType<Scalar> A = Eigen::Map<const MatrixType<Scalar>>(mat_ptr, rows, cols);
        A.adjointInPlace(); // Adjoint directly on a map seems to give a bug?
        // Sanity checks
        if(A.rows() <= 0) throw std::runtime_error("SVD error: rows() == 0");
        if(A.cols() <= 0) throw std::runtime_error("SVD error: cols() == 0");

        t_adj->toc();
        auto [U, S, VT, rank] = do_svd_lapacke(A.data(), A.rows(), A.cols(), std::max(A.rows(), A.cols()));
        long max_size         = std::min(S.size(), rank_max.value());
        rank                  = (S.head(max_size).real().array() >= threshold).count();
        if(U.rows() != A.rows()) throw std::logic_error(fmt::format("U.rows():{} != A.rows():{}", U.rows(), A.rows()));
        if(VT.cols() != A.cols()) throw std::logic_error(fmt::format("VT.cols():{} != A.cols():{}", VT.cols(), A.cols()));
        return std::make_tuple(VT.adjoint().leftCols(rank), S.head(rank), U.adjoint().topRows(rank), rank);
    }

    // Sanity checks
    if(rows <= 0) throw std::runtime_error("SVD error: rows() == 0");
    if(cols <= 0) throw std::runtime_error("SVD error: cols() == 0");

    MatrixType<Scalar> A = Eigen::Map<const MatrixType<Scalar>>(mat_ptr, rows, cols);
    if(not A.allFinite()) {
        print_matrix_lapacke(mat_ptr, rows, cols);
        throw std::runtime_error("SVD error: matrix has inf's or nan's");
    }
    if(A.isZero(1e-12)) {
        print_matrix_lapacke(mat_ptr, rows, cols, 16);
        throw std::runtime_error("SVD error: matrix is all zeros");
    }

    svd::log->trace("Starting SVD with lapacke");

    int info   = 0;
    int rowsU  = rowsA;
    int colsU  = std::min(rowsA, colsA);
    int rowsVT = std::min(rowsA, colsA);
    int colsVT = colsA;
    int rowsV  = colsA;
    int colsV  = std::min(rowsA, colsA);
    int lda    = rowsA;
    int ldu    = rowsU;
    int ldvt   = rowsVT;
    int ldv    = rowsV;

    MatrixType<Scalar> U;
    VectorType<double> S;
    MatrixType<Scalar> VT;

    if constexpr(std::is_same<Scalar, double>::value) {
        if(use_jacobi) {
            svd::log->debug("Running Lapacke Jacobi SVD with threshold {:.4e} | switchsize {} | size {}", threshold, switchsize, sizeS);
            // http://www.netlib.org/lapack/explore-html/d1/d7e/group__double_g_esing_ga8767bfcf983f8dc6ef2842029ab25599.html#ga8767bfcf983f8dc6ef2842029ab25599
            // For this routine we need rows > cols
            t_wrk->tic();
            int                 lwork = std::max(6, rowsA + colsA);
            std::vector<Scalar> work(static_cast<size_t>(lwork));
            //            work.setConstant(0);
            //            work[0] = 1;
            S.resize(sizeS);
            MatrixType<Scalar> V(rowsV, colsV); // Local matrix gets transposed after computation
            t_wrk->toc();

            svd::log->trace("Running dgejsv");
            t_jac->tic();
            info = LAPACKE_dgesvj_work(LAPACK_COL_MAJOR, 'G', 'U', 'V', rowsA, colsA, A.data(), lda, S.data(), ldv, V.data(), ldv, work.data(), lwork);
            t_jac->toc();
            if(info < 0) throw std::runtime_error(fmt::format("Lapacke SVD error: parameter {} is invalid", -info));
            long max_size = std::min(S.size(), rank_max.value());
            long rank     = (S.head(max_size).array() >= threshold).count();
            U             = A.leftCols(rank);
            VT            = V.adjoint().topRows(rank);
        } else if(use_bdc) {
            svd::log->debug("Running Lapacke BDC SVD with threshold {:.4e} | switchsize {} | size {}", threshold, switchsize, sizeS);
            t_wrk->tic();
            int                 liwork = std::max(1, 8 * std::min(rowsA, colsA));
            std::vector<Scalar> work(1);
            std::vector<int>    iwork(static_cast<size_t>(liwork));

            U.resize(rowsU, colsU);
            S.resize(sizeS);
            VT.resize(rowsVT, colsVT);

            svd::log->trace("Querying dgesvd");
            info = LAPACKE_dgesdd_work(LAPACK_COL_MAJOR, 'S', rowsA, colsA, A.data(), lda, S.data(), U.data(), ldu, VT.data(), ldvt, work.data(), -1,
                                       iwork.data());
            if(info < 0) throw std::runtime_error(fmt::format("Lapacke SVD error: parameter {} is invalid", -info));

            int lwork = static_cast<int>(work[0]);
            work.resize(static_cast<size_t>(lwork));
            t_wrk->toc();

            svd::log->trace("Running dgesvd");
            t_svd->tic();
            info = LAPACKE_dgesdd_work(LAPACK_COL_MAJOR, 'S', rowsA, colsA, A.data(), lda, S.data(), U.data(), ldu, VT.data(), ldvt, work.data(), lwork,
                                       iwork.data());
            t_svd->toc();
            if(info < 0) throw std::runtime_error(fmt::format("Lapacke SVD error: parameter {} is invalid", -info));
        } else {
            svd::log->debug("Running Lapacke SVD with threshold {:.4e} | switchsize {} | size {}", threshold, switchsize, sizeS);
            std::vector<Scalar> work(1);

            U.resize(rowsU, colsU);
            S.resize(sizeS);
            VT.resize(rowsVT, colsVT);

            svd::log->trace("Querying dgesvd");
            info = LAPACKE_dgesvd_work(LAPACK_COL_MAJOR, 'S', 'S', rowsA, colsA, A.data(), lda, S.data(), U.data(), ldu, VT.data(), ldvt, work.data(), -1);
            if(info < 0) throw std::runtime_error(fmt::format("Lapacke SVD error: parameter {} is invalid", -info));

            int lwork = static_cast<int>(work[0]);
            work.resize(static_cast<size_t>(lwork));

            svd::log->trace("Running dgesvd");
            t_svd->tic();
            info = LAPACKE_dgesvd_work(LAPACK_COL_MAJOR, 'S', 'S', rowsA, colsA, A.data(), lda, S.data(), U.data(), ldu, VT.data(), ldvt, work.data(), lwork);
            t_svd->toc();
            if(info < 0) throw std::runtime_error(fmt::format("Lapacke SVD error: parameter {} is invalid", -info));
        }
    }
    if constexpr(std::is_same<Scalar, std::complex<double>>::value) {
        if(use_jacobi) {
            svd::log->debug("Running Lapacke Jacobi SVD with threshold {:.4e} | switchsize {} | size {}", threshold, switchsize, sizeS);
            t_wrk->tic();
            std::vector<Scalar> cwork(1);
            std::vector<double> rwork(1);
            std::vector<int> iwork(1);

            S.resize(sizeS);
            U.resize(rowsU, colsU); // Local matrix gets transposed after computation
            MatrixType<Scalar> V(rowsV, colsV); // Local matrix gets transposed after computation

            auto Ap = reinterpret_cast<lapack_complex_double *>(A.data());
            auto Vp = reinterpret_cast<lapack_complex_double *>(V.data());
            auto pcwork = reinterpret_cast<lapack_complex_double *>(cwork.data());
            rwork = {1.0};
            svd::log->trace("Querying zgesvj");
            info = LAPACKE_zgesvj_work(LAPACK_COL_MAJOR, 'G', 'U', 'V', rowsA, colsA, Ap, lda, S.data(), ldv, Vp, ldv, pcwork, -1, rwork.data(), -1);

            if(info < 0) throw std::runtime_error(fmt::format("Lapacke SVD error: parameter {} is invalid", -info));

            int lcwork  = static_cast<int>(std::real(cwork[0]));
            int lrwork  = static_cast<int>(rwork[0]);
            cwork.resize(static_cast<size_t>(lcwork));
            rwork.resize(static_cast<size_t>(lrwork));
            pcwork = reinterpret_cast<lapack_complex_double *>(cwork.data());
            t_wrk->toc();

            svd::log->trace("Running zgesvj | cwork {} | rwork {}", lcwork, lrwork);
            t_jac->tic();
            info = LAPACKE_zgesvj_work(LAPACK_COL_MAJOR, 'G', 'U', 'V', rowsA, colsA, Ap, lda, S.data(), ldv, Vp, ldv, pcwork, lcwork, rwork.data(), lrwork);
            t_jac->toc();
            if(info < 0) throw std::runtime_error(fmt::format("Lapacke SVD error: parameter {} is invalid", -info));
            long max_size = std::min(S.size(), rank_max.value());
            long rank     = (S.head(max_size).array() >= threshold).count();
            U             = A.leftCols(rank);
            VT            = V.adjoint().topRows(rank);
            svd::log->trace("info {} | rwork {} {} {} {} | rank {}", info, rwork[0], rwork[1], rwork[2], rwork[3], rank );

        } else if(use_bdc) {
            svd::log->debug("Running Lapacke BDC SVD with threshold {:.4e} | switchsize {} | size {}", threshold, switchsize, sizeS);
            t_wrk->tic();
            int                 mx     = std::max(rowsA, colsA);
            int                 mn     = std::min(rowsA, colsA);
            int                 lrwork = std::max(1, mn * std::max(5 * mn + 7, 2 * mx + 2 * mn + 1));
            int                 liwork = std::max(1, 8 * std::min(rowsA, colsA));
            std::vector<int>    iwork(static_cast<size_t>(liwork));
            std::vector<double> rwork(static_cast<size_t>(lrwork));
            std::vector<Scalar> work(1);

            U.resize(rowsU, colsU);
            S.resize(sizeS);
            VT.resize(rowsVT, colsVT);

            auto Ap  = reinterpret_cast<lapack_complex_double *>(A.data());
            auto Up  = reinterpret_cast<lapack_complex_double *>(U.data());
            auto VTp = reinterpret_cast<lapack_complex_double *>(VT.data());
            auto Wp  = reinterpret_cast<lapack_complex_double *>(work.data());

            svd::log->trace("Querying zgesdd");
            info = LAPACKE_zgesdd_work(LAPACK_COL_MAJOR, 'S', rowsA, colsA, Ap, lda, S.data(), Up, ldu, VTp, ldvt, Wp, -1, rwork.data(), iwork.data());
            if(info < 0) throw std::runtime_error(fmt::format("Lapacke SVD error: parameter {} is invalid", -info));

            int lwork = static_cast<int>(std::real(work[0]));
            work.resize(static_cast<size_t>(lwork));
            Wp = reinterpret_cast<lapack_complex_double *>(work.data()); // Update the pointer if reallocated
            t_wrk->toc();
            svd::log->trace("Running zgesdd");
            t_svd->tic();
            info = LAPACKE_zgesdd_work(LAPACK_COL_MAJOR, 'S', rowsA, colsA, Ap, lda, S.data(), Up, ldu, VTp, ldvt, Wp, lwork, rwork.data(), iwork.data());
            t_svd->toc();
            if(info < 0) throw std::runtime_error(fmt::format("Lapacke SVD error: parameter {} is invalid", -info));
        } else {
            svd::log->debug("Running Lapacke SVD with threshold {:.4e} | switchsize {} | size {}", threshold, switchsize, sizeS);
            t_wrk->tic();
            int                 lrwork = 5 * std::min(rowsA, colsA);
            std::vector<Scalar> work(1);
            std::vector<double> rwork(static_cast<size_t>(lrwork));

            U.resize(rowsU, colsU);
            S.resize(sizeS);
            VT.resize(rowsVT, colsVT);

            auto Ap  = reinterpret_cast<lapack_complex_double *>(A.data());
            auto Up  = reinterpret_cast<lapack_complex_double *>(U.data());
            auto VTp = reinterpret_cast<lapack_complex_double *>(VT.data());
            auto Wp  = reinterpret_cast<lapack_complex_double *>(work.data());

            svd::log->trace("Querying zgesvd");
            info = LAPACKE_zgesvd_work(LAPACK_COL_MAJOR, 'S', 'S', rowsA, colsA, Ap, lda, S.data(), Up, ldu, VTp, ldvt, Wp, -1, rwork.data());
            if(info < 0) throw std::runtime_error(fmt::format("Lapacke SVD error: parameter {} is invalid", -info));

            int lwork = static_cast<int>(std::real(work[0]));
            work.resize(static_cast<size_t>(lwork));
            Wp = reinterpret_cast<lapack_complex_double *>(work.data()); // Update the pointer if reallocated
            t_wrk->toc();

            svd::log->trace("Running zgesvd");
            t_svd->tic();
            info = LAPACKE_zgesvd_work(LAPACK_COL_MAJOR, 'S', 'S', rowsA, colsA, Ap, lda, S.data(), Up, ldu, VTp, ldvt, Wp, lwork, rwork.data());
            t_svd->toc();
            if(info < 0) throw std::runtime_error(fmt::format("Lapacke SVD error: parameter {} is invalid", -info));
        }
    }
    svd::log->trace("Truncating singular values");
    if(count) count.value()++;
    long max_size = std::min(S.size(), rank_max.value());
    long rank     = (S.head(max_size).array() >= threshold).count();
    if(rank == S.size()) {
        truncation_error = 0;
    } else {
        truncation_error = S.tail(S.size() - rank).norm();
    }

    if(rank <= 0 or not U.leftCols(rank).allFinite() or not S.head(rank).allFinite() or not VT.topRows(rank).allFinite()) {
        if(not A.allFinite()) {
            print_matrix_lapacke(A.data(), A.rows(), A.cols());
            svd::log->critical("SVD error: matrix has inf's or nan's");
        }
        if(A.isZero(1e-12)) {
            print_matrix_lapacke(A.data(), A.rows(), A.cols(), 16);
            svd::log->critical("SVD error: matrix is all zeros");
        }

        throw std::runtime_error(fmt::format("Lapacke SVD error \n"
                                             "  svd_threshold    = {:.4e}\n"
                                             "  Truncation Error = {:.4e}\n"
                                             "  Rank             = {}\n"
                                             "  Dims             = ({}, {})\n"
                                             "  A all finite     : {}\n"
                                             "  U all finite     : {}\n"
                                             "  S all finite     : {}\n"
                                             "  V all finite     : {}\n",
                                             "  Lapacke info     : {}\n",
                                             threshold, truncation_error, rank, rows, cols, A.allFinite(), U.leftCols(rank).allFinite(),
                                             S.head(rank).allFinite(), VT.topRows(rank).allFinite(), info));
    }
    svd::log->trace("SVD with lapacke finished successfully. info = {}", info);
    return std::make_tuple(U.leftCols(rank), S.head(rank), VT.topRows(rank), rank);
}

//! \relates svd::class_SVD
//! \brief force instantiation of do_svd_lapacke for type 'double'
template std::tuple<svd::solver::MatrixType<double>, svd::solver::VectorType<double>, svd::solver::MatrixType<double>, long>
    svd::solver::do_svd_lapacke(const double *, long, long, std::optional<long>);

using cplx = std::complex<double>;
//! \relates svd::class_SVD
//! \brief force instantiation of do_svd_lapacke for type 'std::complex<double>'
template std::tuple<svd::solver::MatrixType<cplx>, svd::solver::VectorType<cplx>, svd::solver::MatrixType<cplx>, long>
    svd::solver::do_svd_lapacke(const cplx *, long, long, std::optional<long>);


#pragma once

#include <ctime>
#include <cassert>
#include <cmath>
#include <utility>
#include <vector>
#include <algorithm>
#include <cstdlib>
#include <memory>
#include <iostream>

#include <Eigen/Dense>

#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl_conversions/pcl_conversions.h>

#include "scancontext/nanoflann.hpp"
#include "scancontext/KDTreeVectorOfVectorsAdaptor.hpp"

#include "scancontext/tictoc.hpp"

using namespace Eigen;
using namespace nanoflann;

using std::cout;
using std::endl;
using std::make_pair;

using std::atan2;
using std::cos;
using std::sin;

using SCPointType = pcl::PointXYZI;  // using xyz only. but a user can exchange the original bin encoding function (i.e., max hegiht) to max intensity (for detail, refer 20 ICRA Intensity Scan Context)
using KeyMat = std::vector<std::vector<float> >;
using InvKeyTree = KDTreeVectorOfVectorsAdaptor<KeyMat, float>;

// namespace SC2
// {

void coreImportTest(void);

// sc param-independent helper functions
float xy2theta(const float &_x, const float &_y);
MatrixXd circshift(MatrixXd &_mat, int _num_shift);
std::vector<float> eig2stdvec(MatrixXd _eigmat);

class SCManager {
public:
  SCManager() = default;  // reserving data space (of std::vector) could be considered. but the descriptor is lightweight so don't care.

  Eigen::MatrixXd makeScancontext(pcl::PointCloud<SCPointType> &_scan_down);
  Eigen::MatrixXd makeRingkeyFromScancontext(Eigen::MatrixXd &_desc);
  Eigen::MatrixXd makeSectorkeyFromScancontext(Eigen::MatrixXd &_desc);

  int fastAlignUsingVkey(MatrixXd &_vkey1, MatrixXd &_vkey2);
  double distDirectSC(MatrixXd &_sc1, MatrixXd &_sc2);                            // "d" (eq 5) in the original paper (IROS 18)
  std::pair<double, int> distanceBtnScanContext(MatrixXd &_sc1, MatrixXd &_sc2);  // "D" (eq 6) in the original paper (IROS 18)

  // User-side API
  void makeAndSaveScancontextAndKeys(pcl::PointCloud<SCPointType> &_scan_down);
  std::pair<int, float> detectLoopClosureID(void);  // int: nearest node index, float: relative yaw

public:
  // hyper parameters ()
  const double LIDAR_HEIGHT = 2.0;  // lidar height : add this for simply directly using lidar scan in the lidar local coord (not robot base coord) / if you use robot-coord-transformed lidar scans, just set this as 0.

  const int PC_NUM_RING = 20;         // 20 in the original paper (IROS 18)
  const int PC_NUM_SECTOR = 60;       // 60 in the original paper (IROS 18)
  const double PC_MAX_RADIUS = 80.0;  // 80 meter max in the original paper (IROS 18)
  const double PC_UNIT_SECTORANGLE = 360.0 / double(PC_NUM_SECTOR);
  const double PC_UNIT_RINGGAP = PC_MAX_RADIUS / double(PC_NUM_RING);

  // tree
  // *Original
  // const int    NUM_EXCLUDE_RECENT = 50; // simply just keyframe gap, but node position distance-based exclusion is ok.
  // const int    NUM_CANDIDATES_FROM_TREE = 10; // 10 is enough. (refer the IROS 18 paper)
  // *scancontext
  const int NUM_EXCLUDE_RECENT = 1;         // simply just keyframe gap, but node position distance-based exclusion is ok.
  const int NUM_CANDIDATES_FROM_TREE = 10;  // 10 is enough. (refer the IROS 18 paper)

  // loop thres
  // *Original
  // const double SEARCH_RATIO = 0.1; // for fast comparison, no Brute-force, but search 10 % is okay. // not was in the original conf paper, but improved ver.
  // const double SC_DIST_THRES = 0.13; // empirically 0.1-0.2 is fine (rare false-alarms) for 20x60 polar context (but for 0.15 <, DCS or ICP fit score check (e.g., in LeGO-LOAM) should be required for robustness)
  // // const double SC_DIST_THRES = 0.5; // 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold / if not, recommend 0.1-0.15
  // *scancontext
  const double SEARCH_RATIO = 0.1;   // for fast comparison, no Brute-force, but search 10 % is okay. // not was in the original conf paper, but improved ver.
  const double SC_DIST_THRES = 0.2;  // empirically 0.1-0.2 is fine (rare false-alarms) for 20x60 polar context (but for 0.15 <, DCS or ICP fit score check (e.g., in LeGO-LOAM) should be required for robustness)
  // const double SC_DIST_THRES = 0.5; // 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold / if not, recommend 0.1-0.15

  // config
  // *Original
  // const int    TREE_MAKING_PERIOD_ = 50; // i.e., remaking tree frequency, to avoid non-mandatory every remaking, to save time cost / if you want to find a very recent revisits use small value of it (it is enough fast ~ 5-50ms wrt N.).
  // int          tree_making_period_conter = 0;
  // *scancontext
  const int TREE_MAKING_PERIOD_ = 2;  // i.e., remaking tree frequency, to avoid non-mandatory every remaking, to save time cost / if you want to find a very recent revisits use small value of it (it is enough fast ~ 5-50ms wrt N.).
  int tree_making_period_conter = 0;

  // data
  std::vector<double> polarcontexts_timestamp_;  // optional.
  std::vector<Eigen::MatrixXd> polarcontexts_;
  std::vector<Eigen::MatrixXd> polarcontext_invkeys_;
  std::vector<Eigen::MatrixXd> polarcontext_vkeys_;

  KeyMat polarcontext_invkeys_mat_;
  KeyMat polarcontext_invkeys_to_search_;
  std::unique_ptr<InvKeyTree> polarcontext_tree_;

};  // SCManager

// } // namespace SC2


#pragma once

#include <polyfem/Common.hpp>
#include <polyfem/ElasticityUtils.hpp>

#include <polyfem/ElementAssemblyValues.hpp>
#include <polyfem/ElementBases.hpp>
#include <polyfem/AutodiffTypes.hpp>
#include <polyfem/Types.hpp>

#include <Eigen/Dense>
#include <array>

//non linear NeoHookean material model
namespace polyfem
{
	class NeoHookeanElasticity
	{
	public:
		NeoHookeanElasticity();

		//energy, gradient, and hessian used in newton method
		Eigen::MatrixXd assemble_hessian(const ElementAssemblyValues &vals, const Eigen::MatrixXd &displacement, const QuadratureVector &da) const;
		Eigen::VectorXd assemble_grad(const ElementAssemblyValues &vals, const Eigen::MatrixXd &displacement, const QuadratureVector &da) const;
		double compute_energy(const ElementAssemblyValues &vals, const Eigen::MatrixXd &displacement, const QuadratureVector &da) const;

		//rhs for fabbricated solution, compute with automatic sympy code
		Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1>
		compute_rhs(const AutodiffHessianPt &pt) const;

		inline int size() const { return size_; }
		void set_size(const int size);

		//von mises and stress tensor
		void compute_von_mises_stresses(const int el_id, const ElementBases &bs, const ElementBases &gbs, const Eigen::MatrixXd &local_pts, const Eigen::MatrixXd &displacement, Eigen::MatrixXd &stresses) const;
		void compute_stress_tensor(const int el_id, const ElementBases &bs, const ElementBases &gbs, const Eigen::MatrixXd &local_pts, const Eigen::MatrixXd &displacement, Eigen::MatrixXd &tensor) const;

		//sets material params
		void set_parameters(const json &params);
		void init_multimaterial(const bool is_volume, const Eigen::MatrixXd &Es, const Eigen::MatrixXd &nus);

	private:
		int size_ = 2;

		LameParameters params_;

		//utulity function that computes energy, the template is used for double, DScalar1, and DScalar2 in energy, gradient and hessian
		template <typename T>
		T compute_energy_aux(const ElementAssemblyValues &vals, const Eigen::MatrixXd &displacement, const QuadratureVector &da) const;

		void assign_stress_tensor(const int el_id, const ElementBases &bs, const ElementBases &gbs, const Eigen::MatrixXd &local_pts, const Eigen::MatrixXd &displacement, const int all_size, Eigen::MatrixXd &all, const std::function<Eigen::MatrixXd(const Eigen::MatrixXd &)> &fun) const;
	};
} // namespace polyfem


/**
 * @file    linearization.cpp
 * @brief   Tools to linearize a nonlinear factor graph to linear system Ax = b
 * @author  Jing Dong
 * @date    Oct 15, 2017
 */

#include <minisam/config.h>

#include <minisam/nonlinear/linearization.h>

#include <minisam/core/Factor.h>
#include <minisam/core/FactorGraph.h>
#include <minisam/core/VariableOrdering.h>
#include <minisam/core/Variables.h>
#include <minisam/nonlinear/SparsityPattern.h>

#include <Eigen/Core>
#include <Eigen/SparseCore>

#ifdef MINISAM_WITH_MULTI_THREADS
#include <mutex>
#include <thread>
#endif

using namespace std;

namespace minisam {

/* ************************************************************************** */
void linearzationJacobian(const FactorGraph& graph, const Variables& variables,
                          Eigen::SparseMatrix<double>& A, Eigen::VectorXd& b) {
  linearzationJacobian(graph, variables, A, b,
                       variables.defaultVariableOrdering());
}

/* ************************************************************************** */
void linearzationJacobian(const FactorGraph& graph, const Variables& variables,
                          Eigen::SparseMatrix<double>& A, Eigen::VectorXd& b,
                          const VariableOrdering& ordering) {
  internal::JacobianSparsityPattern pattern =
      internal::constructJacobianSparsity(graph, variables, ordering);
  internal::linearzationJacobian(graph, variables, pattern, A, b);
}

/* ************************************************************************** */
void linearzationFullHessian(const FactorGraph& graph,
                             const Variables& variables,
                             Eigen::SparseMatrix<double>& AtA,
                             Eigen::VectorXd& Atb) {
  linearzationFullHessian(graph, variables, AtA, Atb,
                          variables.defaultVariableOrdering());
}

/* ************************************************************************** */
void linearzationFullHessian(const FactorGraph& graph,
                             const Variables& variables,
                             Eigen::SparseMatrix<double>& AtA,
                             Eigen::VectorXd& Atb,
                             const VariableOrdering& ordering) {
  internal::LowerHessianSparsityPattern pattern =
      internal::constructLowerHessianSparsity(graph, variables, ordering);
  internal::linearzationFullHessian(graph, variables, pattern, AtA, Atb);
}

namespace internal {

/* ************************************************************************** */
void linearzationJacobian(const FactorGraph& factors, const Variables& values,
                          const JacobianSparsityPattern& sparsity,
                          Eigen::SparseMatrix<double>& A, Eigen::VectorXd& b) {
  // init A and b
  A = Eigen::SparseMatrix<double>(sparsity.A_rows, sparsity.A_cols);
  b = Eigen::VectorXd(sparsity.A_rows);

  // pre-allocate A by number of non-zeros each col
  A.reserve(sparsity.nnz_cols);

  // accumulator for row
  int err_row_counter = 0;

  for (size_t f_idx = 0; f_idx < factors.size(); f_idx++) {
    const std::shared_ptr<Factor>& f = factors.factors()[f_idx];

    // get var index of factors
    vector<int> jacobian_col;
    jacobian_col.reserve(f->keys().size());

    for (auto pkey = f->keys().begin(); pkey != f->keys().end(); pkey++) {
      size_t key_idx = sparsity.var_ordering.searchKey(*pkey);
      jacobian_col.push_back(sparsity.var_col[key_idx]);
    }

    // whiten err and jacobians
    pair<vector<Eigen::MatrixXd>, Eigen::VectorXd> wht_Js_err =
        f->weightedJacobiansError(values);

    const vector<Eigen::MatrixXd>& wht_Js = wht_Js_err.first;
    b.segment(err_row_counter, f->dim()) = -wht_Js_err.second;

    // update jacobian matrix
    for (size_t j_idx = 0; j_idx < wht_Js.size(); j_idx++) {
      // Eigen doesn't allow block write operation
      // write element-wise
      // scan by row for better CPU cache hit
      for (int j = 0; j < wht_Js[j_idx].cols(); j++) {
        for (int i = 0; i < wht_Js[j_idx].rows(); i++) {
          A.insert(i + err_row_counter, j + jacobian_col[j_idx]) =
              wht_Js[j_idx](i, j);
        }
      }
    }

    // update row counter
    err_row_counter += (int)f->dim();
  }

  // always output compressed matrix
  A.makeCompressed();
}

namespace {
/* ************************************************************************** */
// data struct for sort key in
Eigen::MatrixXd stackMatrixCol_(const std::vector<Eigen::MatrixXd>& mats) {
  assert(mats.size() > 0);
  int H_stack_cols = 0;
  for (const auto& H : mats) {
    H_stack_cols += static_cast<int>(H.cols());
  }
  const int rows = static_cast<int>(mats[0].rows());
  Eigen::MatrixXd H_stack(rows, H_stack_cols);
  H_stack_cols = 0;
  for (const auto& H : mats) {
    assert(H.rows() == rows);
    H_stack.block(0, H_stack_cols, rows, H.cols()) = H;
    H_stack_cols += H.cols();
  }
  return H_stack;
}

/* ************************************************************************** */
#ifdef MINISAM_WITH_MULTI_THREADS
void linearzationLowerHessianSingleFactor_(
    const std::shared_ptr<Factor>& f, const Variables& values,
    const LowerHessianSparsityPattern& sparsity,
    Eigen::SparseMatrix<double>& AtA, Eigen::VectorXd& Atb, std::mutex& mutex_A,
    std::mutex& mutex_b) {
#else
void linearzationLowerHessianSingleFactor_(
    const std::shared_ptr<Factor>& f, const Variables& values,
    const LowerHessianSparsityPattern& sparsity,
    Eigen::SparseMatrix<double>& AtA, Eigen::VectorXd& Atb) {
#endif

  // whiten err and jacobians
  vector<size_t> var_idx, jacobian_col, jacobian_col_local;
  var_idx.reserve(f->size());
  jacobian_col.reserve(f->size());
  jacobian_col_local.reserve(f->size());
  size_t local_col = 0;
  for (Key vkey : f->keys()) {
    // A col start index
    size_t key_idx = sparsity.var_ordering.searchKeyUnsafe(vkey);
    var_idx.push_back(key_idx);
    jacobian_col.push_back(sparsity.var_col[key_idx]);
    jacobian_col_local.push_back(local_col);
    local_col += sparsity.var_dim[key_idx];
  }

  const pair<vector<Eigen::MatrixXd>, Eigen::VectorXd> wht_Js_err =
      f->weightedJacobiansError(values);

  const vector<Eigen::MatrixXd>& wht_Js = wht_Js_err.first;
  const Eigen::VectorXd& wht_err = wht_Js_err.second;

  Eigen::MatrixXd stackJ = stackMatrixCol_(wht_Js);

  Eigen::MatrixXd stackJtJ(stackJ.cols(), stackJ.cols());

  // adaptive multiply for better speed
  if (stackJ.cols() > 12) {
    // stackJtJ.setZero();
    memset(stackJtJ.data(), 0, stackJ.cols() * stackJ.cols() * sizeof(double));
    stackJtJ.selfadjointView<Eigen::Lower>().rankUpdate(stackJ.transpose());
  } else {
    stackJtJ.noalias() = stackJ.transpose() * stackJ;
  }

  const Eigen::VectorXd stackJtb = stackJ.transpose() * wht_err;

#ifdef MINISAM_WITH_MULTI_THREADS
  mutex_b.lock();
#endif

  for (size_t j_idx = 0; j_idx < wht_Js.size(); j_idx++) {
    Atb.segment(jacobian_col[j_idx], wht_Js[j_idx].cols()) -=
        stackJtb.segment(jacobian_col_local[j_idx], wht_Js[j_idx].cols());
  }

#ifdef MINISAM_WITH_MULTI_THREADS
  mutex_b.unlock();
  mutex_A.lock();
#endif

  for (size_t j_idx = 0; j_idx < wht_Js.size(); j_idx++) {
    // scan by row
    size_t nnz_AtA_vars_accum_var = sparsity.nnz_AtA_vars_accum[var_idx[j_idx]];
    double* value_ptr = AtA.valuePtr() + nnz_AtA_vars_accum_var;

    for (int j = 0; j < wht_Js[j_idx].cols(); j++) {
      for (int i = j; i < wht_Js[j_idx].cols(); i++) {
        *(value_ptr++) += stackJtJ(jacobian_col_local[j_idx] + i,
                                   jacobian_col_local[j_idx] + j);
      }
      value_ptr += (sparsity.nnz_AtA_cols[jacobian_col[j_idx] + j] -
                    wht_Js[j_idx].cols() + j);
    }
  }

#ifdef MINISAM_WITH_MULTI_THREADS
  mutex_A.unlock();
#endif

  // update lower non-diag hessian blocks
  for (size_t j1_idx = 0; j1_idx < wht_Js.size(); j1_idx++) {
    for (size_t j2_idx = 0; j2_idx < wht_Js.size(); j2_idx++) {
      // we know var_idx[j1_idx] != var_idx[j2_idx]
      // assume var_idx[j1_idx] > var_idx[j2_idx]
      // insert to block location (j1_idx, j2_idx)
      if (var_idx[j1_idx] > var_idx[j2_idx]) {
        size_t nnz_AtA_vars_accum_var2 =
            sparsity.nnz_AtA_vars_accum[var_idx[j2_idx]];
        int var2_dim = sparsity.var_dim[var_idx[j2_idx]];

        int inner_insert_var2_var1 =
            sparsity.inner_insert_map[var_idx[j2_idx]].at(var_idx[j1_idx]);

        double* value_ptr = AtA.valuePtr() + nnz_AtA_vars_accum_var2 +
                            var2_dim + inner_insert_var2_var1;

#ifdef MINISAM_WITH_MULTI_THREADS
        mutex_A.lock();
#endif

        if (j1_idx > j2_idx) {
          for (int j = 0; j < wht_Js[j2_idx].cols(); j++) {
            for (int i = 0; i < wht_Js[j1_idx].cols(); i++) {
              *(value_ptr++) += stackJtJ(jacobian_col_local[j1_idx] + i,
                                         jacobian_col_local[j2_idx] + j);
            }
            value_ptr += (sparsity.nnz_AtA_cols[jacobian_col[j2_idx] + j] - 1 -
                          wht_Js[j1_idx].cols());
          }
        } else {
          for (int j = 0; j < wht_Js[j2_idx].cols(); j++) {
            for (int i = 0; i < wht_Js[j1_idx].cols(); i++) {
              *(value_ptr++) += stackJtJ(jacobian_col_local[j2_idx] + j,
                                         jacobian_col_local[j1_idx] + i);
            }
            value_ptr += (sparsity.nnz_AtA_cols[jacobian_col[j2_idx] + j] - 1 -
                          wht_Js[j1_idx].cols());
          }
        }

#ifdef MINISAM_WITH_MULTI_THREADS
        mutex_A.unlock();
#endif
      }
    }
  }
}

/* ************************************************************************** */
#ifdef MINISAM_WITH_MULTI_THREADS
void linearzationLowerHessianCaller_(
    const FactorGraph& factors, const Variables& values,
    const LowerHessianSparsityPattern& sparsity,
    Eigen::SparseMatrix<double>& AtA, Eigen::VectorXd& Atb, std::mutex& mutex_A,
    std::mutex& mutex_b, int thread_id, int total_thread) {
  for (size_t fidx = thread_id; fidx < factors.size(); fidx += total_thread) {
    linearzationLowerHessianSingleFactor_(factors.factors()[fidx], values,
                                          sparsity, AtA, Atb, mutex_A, mutex_b);
  }
}
#endif
}  // namespace

/* ************************************************************************** */
void linearzationLowerHessian(const FactorGraph& factors,
                              const Variables& values,
                              const LowerHessianSparsityPattern& sparsity,
                              Eigen::SparseMatrix<double>& AtA,
                              Eigen::VectorXd& Atb) {
  // init empty AtA and Atb
  AtA = Eigen::SparseMatrix<double>(sparsity.A_cols, sparsity.A_cols);
  Atb = Eigen::VectorXd::Zero(sparsity.A_cols);

  // pre-allocate AtA by number of non-zeros each col
  AtA.reserve(sparsity.nnz_AtA_cols);

  // prepare empty AtA with zeros
  // depends on IEEE 754 floating point format of 0.0
  memset(AtA.valuePtr(), 0, sparsity.total_nnz_AtA_cols * sizeof(double));
  memcpy(AtA.innerIndexPtr(), &sparsity.inner_index[0],
         sparsity.total_nnz_AtA_cols * sizeof(int));
  memcpy(AtA.innerNonZeroPtr(), &sparsity.inner_nnz_index[0],
         sparsity.A_cols * sizeof(int));
  memcpy(AtA.outerIndexPtr(), &sparsity.outer_index[0],
         sparsity.A_cols * sizeof(int));

// incremental fill-in Hessian
#ifdef MINISAM_WITH_MULTI_THREADS

  mutex mutex_A, mutex_b;  // data mutex

  // init threads
  vector<thread> linthreads;
  linthreads.reserve(MINISAM_WITH_MULTI_THREADS_NUM);
  for (int i = 0; i < MINISAM_WITH_MULTI_THREADS_NUM; i++) {
    linthreads.emplace_back(linearzationLowerHessianCaller_, std::ref(factors),
                            std::ref(values), std::ref(sparsity), std::ref(AtA),
                            std::ref(Atb), std::ref(mutex_A), std::ref(mutex_b),
                            i, MINISAM_WITH_MULTI_THREADS_NUM);
  }

  // wait threads to finish
  for (int i = 0; i < MINISAM_WITH_MULTI_THREADS_NUM; i++) {
    linthreads[i].join();
  }

#else
  for (size_t f_idx = 0; f_idx < factors.size(); f_idx++) {
    const std::shared_ptr<Factor>& f = factors.factors()[f_idx];
    linearzationLowerHessianSingleFactor_(f, values, sparsity, AtA, Atb);
  }
#endif

  // always output compressed matrix
  AtA.makeCompressed();
}

/* ************************************************************************** */
void linearzationFullHessian(const FactorGraph& graph,
                             const Variables& variables,
                             const LowerHessianSparsityPattern& sparsity,
                             Eigen::SparseMatrix<double>& AtA,
                             Eigen::VectorXd& Atb) {
  Eigen::SparseMatrix<double> AtA_lower;
  linearzationLowerHessian(graph, variables, sparsity, AtA_lower, Atb);
  AtA = AtA_lower.selfadjointView<Eigen::Lower>();
}

}  // namespace internal
}  // namespace minisam


// Copyright (C) 2011-2013 Rhys Ulerich
// Copyright (C) ??? Martin Bauer
// Copyright (C) 2017 Henri Menke
//
// This code borrows heavily from code written by Rhys Ulerich and
// Martin Bauer.  They licensed it under the Mozilla Public License,
// v. 2.0 and the GNU General Public License (no version info),
// respectively.  I believe that I have made enough contributions and
// altered this code far enough from the originals that I can
// relicense it under the Boost Software License.
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
#pragma once

#include <algorithm>
#include <cmath>
#include <functional>
#include <iterator>
#include <limits>
#include <map>
#include <stdexcept>
#include <string>

#define BOOST_RESULT_OF_USE_DECLTYPE
#define BOOST_SPIRIT_USE_PHOENIX_V3
#include <boost/math/constants/constants.hpp>
#include <boost/spirit/include/phoenix.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/variant.hpp>

namespace matheval {

namespace detail {

namespace qi = boost::spirit::qi;
namespace ascii = boost::spirit::ascii;

/** @brief Sign function
 *
 * Missing function in the STL.  This calculates the mathematical sign
 * function.
 *
 * @f[
 *   \mathop{\mathrm{sgn}}(x) =
 *   \begin{cases}
 *      1 & x > 0 \\
 *      0 & x = 0 \\
 *     -1 & x < 0 \\
 *   \end{cases}
 * @f]
 *
 * @param[in] x number
 * @returns the sign of x
 */
template < typename T >
T sgn(T x) { return (T{0} < x) - (x < T{0}); }

// AST

template < typename real_t > struct unary_op;
template < typename real_t > struct binary_op;

struct nil {};

/** @brief Abstract Syntax Tree
 *
 * Stores the abstract syntax tree (AST) of the parsed mathematical
 * expression.
 */
template < typename real_t >
struct expr_ast
{
    using tree_t = boost::variant<
        nil // can't happen!
        , real_t
        , std::string
        , boost::recursive_wrapper<expr_ast<real_t>>
        , boost::recursive_wrapper<binary_op<real_t>>
        , boost::recursive_wrapper<unary_op<real_t>>
        >;
public:
    /** @brief AST storage
     *
     * The syntax tree can hold various types.  Numbers (`real_t`),
     * variables (`std::string`), the recursive tree itself
     * (`expr_ast`), binary operators (`binary_op`), and unary
     * operators (`unary_op`).
     */
    tree_t tree;

    /** @brief Default constructor
     *
     * Initializes the tree to a nil value to indicate inconsistent
     * state.
     */
    expr_ast() : tree(nil{}) {}

    /** @brief Copy constructor
     *
     * Deep copies the syntax tree.
     */
    template <typename Expr>
    expr_ast(Expr const &other) : tree(other) {}

    /** @brief Add a tree */
    expr_ast& operator+=(expr_ast const &rhs);
    /** @brief subtract a tree */
    expr_ast& operator-=(expr_ast const &rhs);
    /** @brief Multiply by a tree */
    expr_ast& operator*=(expr_ast const &rhs);
    /** @brief Divide by a tree */
    expr_ast& operator/=(expr_ast const &rhs);
};

/** @brief Store a unary operator and its argument tree */
template < typename real_t >
struct unary_op
{
    /** @brief Signature of a unary operator: op(x) */
    using op_t = std::function<real_t(real_t)>;

    /** @brief Save the operator and the argument tree */
    unary_op(op_t op, expr_ast<real_t> const &rhs)
        : op(op), rhs(rhs)
    {}

    /** @brief Stored operator */
    op_t op;
    /** @brief Stored argument tree */
    expr_ast<real_t> rhs;
};

/** @brief Store a binary operator and its argument trees */
template < typename real_t >
struct binary_op
{
    /** @brief Signature of a binary operator: op(x,y) */
    using op_t = std::function<real_t(real_t,real_t)>;

    /** @brief Save the operator and the argument trees */
    binary_op(op_t op, expr_ast<real_t> const &lhs, expr_ast<real_t> const &rhs)
        : op(op), lhs(lhs), rhs(rhs)
    {}

    /** @brief Stored operator */
    op_t op;
    /** @brief Stored argument tree of first argument */
    expr_ast<real_t> lhs;
    /** @brief Stored argument tree of second argument */
    expr_ast<real_t> rhs;
};

template < typename real_t >
expr_ast<real_t>& expr_ast<real_t>::operator+=(expr_ast<real_t> const &rhs)
{
    tree = binary_op<real_t>(std::plus<real_t>{}, tree, rhs);
    return *this;
}
template < typename real_t >
expr_ast<real_t>& expr_ast<real_t>::operator-=(expr_ast<real_t> const &rhs)
{
    tree = binary_op<real_t>(std::minus<real_t>{}, tree, rhs);
    return *this;
}
template < typename real_t >
expr_ast<real_t>& expr_ast<real_t>::operator*=(expr_ast<real_t> const &rhs)
{
    tree = binary_op<real_t>(std::multiplies<real_t>{}, tree, rhs);
    return *this;
}
template < typename real_t >
expr_ast<real_t>& expr_ast<real_t>::operator/=(expr_ast<real_t> const &rhs)
{
    tree = binary_op<real_t>(std::divides<real_t>{}, tree, rhs);
    return *this;
}

/** @brief Evaluate the Abstract Syntax Tree
 *
 * This visits all the variants of the AST and applies the stored
 * operators.
 */
template < typename real_t >
class eval_ast
{
public:
    /** @brief Necessary typedef for `boost::apply_visitor` */
    using result_type = real_t;

    /** @brief Type of the symbol table */
    using symbol_table_t = std::map<std::string, result_type>;

    /** @brief Constructor
     *
     * Saves the symbol table to apply variables.
     */
    eval_ast(symbol_table_t const &sym) : st(sym) {}

    /** @brief Empty nodes in the tree evaluate to 0 */
    result_type operator()(nil) const { return 0; }

    /** @brief Numbers evaluate to themselves */
    result_type operator()(result_type n)  const { return n; }

    /** @brief Variables evaluate to their value in the symbol table */
    result_type operator()(std::string const &c) const
    {
        auto it = st.find(c);
        if(it == st.end())
            throw std::invalid_argument("Unknown variable " + c);
        return it->second;
    }

    /** @brief Recursively evaluate the AST */
    result_type operator()(expr_ast<real_t> const& ast) const
    {
        return boost::apply_visitor(*this, ast.tree);
    }

    /** @brief Evaluate a binary operator and optionally recurse its operands */
    result_type operator()(binary_op<real_t> const& tree) const
    {
        return tree.op(
            boost::apply_visitor(*this, tree.lhs.tree),
            boost::apply_visitor(*this, tree.rhs.tree)
            );
    }

    /** @brief Evaluate a unary operator and optionally recurse its operand */
    result_type operator()(unary_op<real_t> const& tree) const
    {
        return tree.op(
            boost::apply_visitor(*this, tree.rhs.tree)
            );
    }

private:
    symbol_table_t st;
};

// Expressions

/** @brief Unary expression functor */
template < typename real_t >
struct unary_expr_ {
    /** @brief Make boost::phoenix::function happy */
    template < typename T > struct result { using type = T; };

    /** @brief Create a new AST containing the unary function */
    expr_ast<real_t> operator()(typename unary_op<real_t>::op_t op,
                                expr_ast<real_t> const &rhs) const {
        return expr_ast<real_t>(unary_op<real_t>(op, rhs));
    }
};

/** @brief Binary expression functor */
template < typename real_t >
struct binary_expr_ {
    /** @brief Make boost::phoenix::function happy */
    template < typename T > struct result { using type = T; };

    /** @brief Create a new AST containing the binary function */
    expr_ast<real_t> operator()(typename binary_op<real_t>::op_t op,
                                expr_ast<real_t> const &lhs,
                                expr_ast<real_t> const &rhs) const {
        return expr_ast<real_t>(binary_op<real_t>(op, lhs, rhs));
    }
};

// Grammar

/** @brief Expression Grammar */
template < typename real_t, typename Iterator >
struct grammar
    : qi::grammar<
            Iterator, expr_ast<real_t>(), ascii::space_type
        >
{
private:
    qi::rule<Iterator, expr_ast<real_t>(), ascii::space_type> expression;
    qi::rule<Iterator, expr_ast<real_t>(), ascii::space_type> term;
    qi::rule<Iterator, expr_ast<real_t>(), ascii::space_type> factor;
    qi::rule<Iterator, expr_ast<real_t>(), ascii::space_type> primary;
    qi::rule<Iterator, std::string()> variable;
public:
    /** @brief symbol table for constants like "pi" */
    struct constant_
        : boost::spirit::qi::symbols<
                typename std::iterator_traits<Iterator>::value_type,
                real_t
            >
    {
        constant_()
        {
            this->add
                ("e"      , boost::math::constants::e<real_t>()   )
                ("epsilon", std::numeric_limits<real_t>::epsilon())
                ("pi"     , boost::math::constants::pi<real_t>()  )
            ;
        }
    } constant;

    /** @brief symbol table for unary functions like "abs" */
    struct ufunc_
        : boost::spirit::qi::symbols<
                typename std::iterator_traits<Iterator>::value_type,
                typename unary_op<real_t>::op_t
            >
    {
        ufunc_()
        {
            this->add
                ("abs"   , static_cast<real_t(*)(real_t)>(&std::abs   ))
                ("acos"  , static_cast<real_t(*)(real_t)>(&std::acos  ))
                ("acosh" , static_cast<real_t(*)(real_t)>(&std::acosh ))
                ("asin"  , static_cast<real_t(*)(real_t)>(&std::asin  ))
                ("asinh" , static_cast<real_t(*)(real_t)>(&std::asinh ))
                ("atan"  , static_cast<real_t(*)(real_t)>(&std::atan  ))
                ("atanh" , static_cast<real_t(*)(real_t)>(&std::atanh ))
                ("cbrt"  , static_cast<real_t(*)(real_t)>(&std::cbrt  ))
                ("ceil"  , static_cast<real_t(*)(real_t)>(&std::ceil  ))
                ("cos"   , static_cast<real_t(*)(real_t)>(&std::cos   ))
                ("cosh"  , static_cast<real_t(*)(real_t)>(&std::cosh  ))
                ("erf"   , static_cast<real_t(*)(real_t)>(&std::erf   ))
                ("erfc"  , static_cast<real_t(*)(real_t)>(&std::erfc  ))
                ("exp"   , static_cast<real_t(*)(real_t)>(&std::exp   ))
                ("exp2"  , static_cast<real_t(*)(real_t)>(&std::exp2  ))
                ("floor" , static_cast<real_t(*)(real_t)>(&std::floor ))
                ("log"   , static_cast<real_t(*)(real_t)>(&std::log   ))
                ("log2"  , static_cast<real_t(*)(real_t)>(&std::log2  ))
                ("log10" , static_cast<real_t(*)(real_t)>(&std::log10 ))
                ("round" , static_cast<real_t(*)(real_t)>(&std::round ))
                ("sgn"   , static_cast<real_t(*)(real_t)>(&sgn        ))
                ("sin"   , static_cast<real_t(*)(real_t)>(&std::sin   ))
                ("sinh"  , static_cast<real_t(*)(real_t)>(&std::sinh  ))
                ("sqrt"  , static_cast<real_t(*)(real_t)>(&std::sqrt  ))
                ("tan"   , static_cast<real_t(*)(real_t)>(&std::tan   ))
                ("tanh"  , static_cast<real_t(*)(real_t)>(&std::tanh  ))
                ("tgamma", static_cast<real_t(*)(real_t)>(&std::tgamma))
            ;
        }
    } ufunc;

    /** @brief symbol table for binary functions like "pow" */
    struct bfunc_
        : boost::spirit::qi::symbols<
                typename std::iterator_traits<Iterator>::value_type,
                typename binary_op<real_t>::op_t
            >
    {
        bfunc_()
        {
            this->add
                ("atan2", static_cast<real_t(*)(real_t,real_t)>(&std::atan2))
                ("max"  , static_cast<real_t(*)(real_t,real_t)>(&std::fmax ))
                ("min"  , static_cast<real_t(*)(real_t,real_t)>(&std::fmin ))
                ("pow"  , static_cast<real_t(*)(real_t,real_t)>(&std::pow  ))
            ;
        }
    } bfunc;

    /** @brief Constructor builds the grammar */
    grammar() : grammar::base_type(expression)
    {
        using boost::spirit::qi::real_parser;
        using boost::spirit::qi::real_policies;
        real_parser<real_t,real_policies<real_t>> real;

        using boost::spirit::lexeme;
        using boost::spirit::qi::_1;
        using boost::spirit::qi::_2;
        using boost::spirit::qi::_3;
        using boost::spirit::qi::_val;
        using boost::spirit::qi::alpha;
        using boost::spirit::qi::alnum;
        using boost::spirit::qi::raw;

        boost::phoenix::function<unary_expr_<real_t>> unary_expr;
        boost::phoenix::function<binary_expr_<real_t>> binary_expr;

        auto fmod = static_cast<real_t(*)(real_t,real_t)>(&std::fmod);
        auto pow = static_cast<real_t(*)(real_t,real_t)>(&std::pow);

        expression =
            term                   [_val =  _1]
            >> *(  ('+' >> term    [_val += _1])
                |  ('-' >> term    [_val -= _1])
                )
            ;

        term =
            factor                 [_val =  _1]
            >> *(  ('*' >> factor  [_val *= _1])
                |  ('/' >> factor  [_val /= _1])
                |  ('%' >> factor  [_val = binary_expr(fmod, _val, _1)])
                )
            ;

        factor =
            primary                [_val =  _1]
            >> *(  ("**" >> factor [_val = binary_expr(pow, _val, _1)])
                )
            ;

        variable =
            raw[lexeme[alpha >> *(alnum | '_')]];

        primary =
            real                   [_val =  _1]
            |   '(' >> expression  [_val =  _1] >> ')'
            |   ('-' >> primary    [_val = unary_expr(std::negate<real_t>{}, _1)])
            |   ('+' >> primary    [_val =  _1])
            |   (ufunc >> '(' >> expression >> ')')
                                   [_val = unary_expr(_1, _2)]
            |   (bfunc >> '(' >> expression >> ',' >> expression >> ')')
                                   [_val = binary_expr(_1, _2, _3)]
            |   constant           [_val =  _1]
            |   variable           [_val =  _1]
            ;
    }
};

} // namespace detail

/** @brief Class interface
 *
 * This class hides the grammar, AST, and AST traversal behind some
 * member functions.
 *
 * @tparam real_t datatype of the result
 */
template < typename real_t >
class Parser
{
    detail::expr_ast<real_t> ast;
public:
    /** @brief Parse an expression
     *
     * This function builds the grammar and parses the iterator into
     * an AST.
     *
     * @param[in] first iterator to the start of the input sequence
     * @param[in] last  iterator to the end of the input sequence
     */
    template < typename Iterator >
    void parse(Iterator first, Iterator last)
    {
        static detail::grammar<real_t,Iterator> const g;

        ast = detail::expr_ast<real_t>{}; // Drop old AST

        bool r = boost::spirit::qi::phrase_parse(
            first, last, g,
            boost::spirit::ascii::space, ast);

        if (!r || first != last)
        {
            std::string rest(first, last);
            throw std::runtime_error("Parsing failed at " + rest);
        }
    }

    /** @overload parse(Iterator first, Iterator last) */
    void parse(std::string const &str)
    {
        parse(str.begin(), str.end());
    }

    /** @brief Evaluate the AST with a given symbol table
     *
     * @param[in] st the symbol table for variables
     */
    real_t evaluate(typename detail::eval_ast<real_t>::symbol_table_t const &st)
    {
        detail::eval_ast<real_t> solver(st);
        return solver(ast);
    }
};

/** @brief Convenience function
 *
 * This function builds the grammar, parses the iterator to an AST,
 * evaluates it, and returns the result.
 *
 * @param[in] first iterator to the start of the input sequence
 * @param[in] last  iterator to the end of the input sequence
 * @param[in] st    the symbol table for variables
 */
template < typename real_t, typename Iterator >
real_t parse(Iterator first, Iterator last,
             typename detail::eval_ast<real_t>::symbol_table_t const &st)
{
    Parser<real_t> parser;
    parser.parse(first, last);
    return parser.evaluate(st);
}

/** @overload parse(Iterator first, Iterator last, typename detail::eval_ast<real_t>::symbol_table_t const &st) */
template < typename real_t >
real_t parse(std::string const &str,
             typename detail::eval_ast<real_t>::symbol_table_t const &st)
{
    return parse<real_t>(str.begin(), str.end(), st);
}

} // namespace expression


//=======================================================================
// Copyright (c) 2013 Piotr Smulewicz
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file multiway_cut_example.cpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2013-11-13
 */

    //! [Multiway Cut Example]
#include "paal/multiway_cut/multiway_cut.hpp"

#include <boost/graph/adjacency_list.hpp>

int main() {
    // sample data
    std::vector<std::pair<int,int>> edges_p{{0,3},{1,3},
                                           {0,4},{2,4},
                                           {1,5},{2,5},
                                           {3,6},{4,6},
                                           {3,7},{5,7},
                                           {4,8},{5,8},
                                           {6,7},{6,8},{7,8}
    };
    const int vertices_num = 9;
    std::vector<int> cost_edges{100,100,100,100,100,100,10,10,10,10,10,10,1,1,1};

    std::vector<int> terminals = { 0, 1, 2 };
    boost::adjacency_list<
        boost::vecS, boost::vecS, boost::undirectedS,
        boost::property<boost::vertex_index_t, int,
                        boost::property<boost::vertex_color_t, int>>,
                    boost::property<boost::edge_weight_t, int>
                    > graph(edges_p.begin(), edges_p.end(), cost_edges.begin(), vertices_num);

    for (std::size_t i = 1; i <= terminals.size(); ++i) {
        put(boost::vertex_color, graph, terminals[i - 1], i);
    }

    //solve
    std::vector<std::pair<int,int>> vertices_parts;
    auto cost_cut = paal::multiway_cut(graph, back_inserter(vertices_parts));

    //print result
    std::cout << "cost cut: " << cost_cut << std::endl;
    std::cout << "vertices (part)" << std::endl;
    for(auto i: vertices_parts) {
        std::cout << "  " << i.first << "      ( " << i.second << " )" << std::endl;
    }
    paal::lp::glp::free_env();
}
    //! [Multiway Cut Example]


//N0680337
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

#include "logs.h"
#include "route.h"
#include "track.h"

using namespace GPS;

BOOST_AUTO_TEST_SUITE( Route_maxLongitude )

const bool isFileName = true;
// More test details in DesignOfUnitTests.pdf
// Used boost check close to allow a tolerance when using floating points
// Test increasing longitude values to identify that max longitude works
BOOST_AUTO_TEST_CASE( MaxLongHorizontalLine )
{
   Route route = Route(LogFiles::GPXRoutesDir + "IncreaseInLongitude-N0680337.gpx", isFileName);
   BOOST_CHECK_CLOSE( route.maxLongitude(), 51.3975, 0.1);
}

// Test if very small changes are detected
BOOST_AUTO_TEST_CASE( VerySmallChange )
{
   Route route = Route(LogFiles::GPXRoutesDir + "VerySmallChange-N0680337.gpx", isFileName);
   BOOST_CHECK_CLOSE( route.maxLongitude(), 10.0000002, 0.0000001 );
}

// Test that the function works with only one longitude value
BOOST_AUTO_TEST_CASE( MaxLongVerticalLine )
{
   Route route = Route(LogFiles::GPXRoutesDir + "ConstantLongitude-N0680337.gpx", isFileName);
   BOOST_CHECK_CLOSE( route.maxLongitude(), -50, 0.1);
}

// Test a constant value with the maximum possible longitude
BOOST_AUTO_TEST_CASE( MaxPossibleConstantLongitude )
{
   Route route = Route(LogFiles::GPXRoutesDir + "MaxPossibleConstantLongitude-N0680337.gpx", isFileName);
   BOOST_CHECK_CLOSE( route.maxLongitude(), 180, 0.1);
}

// Test the function against negative values only (A, G, F gridworld points)

BOOST_AUTO_TEST_CASE( NegativeMaxLongHorizontalLine )
{
   Route route = Route(LogFiles::GPXRoutesDir + "NegativeMaxLong-N0680337.gpx", isFileName);
   BOOST_CHECK_CLOSE( route.maxLongitude(), -51.3975, 0.01);
}

// Test the function works with the Prime Meridian as the max longitude (0 longitude, C, H, M gridworld points)

BOOST_AUTO_TEST_CASE( PrimeMeridianVerticalLine )
{
   Route route = Route(LogFiles::GPXRoutesDir + "PrimeMeridianVerticalLine-N0680337.gpx", isFileName);
   BOOST_CHECK_CLOSE( route.maxLongitude(), 0, 0.1);
}

// Test the function against negative values, 0, and positive values to find the max longitude
BOOST_AUTO_TEST_CASE( NegativeToPositiveLong )
{
   Route route = Route(LogFiles::GPXRoutesDir + "ThroughPrimeMeridian-N0680337.gpx", isFileName);
   BOOST_CHECK_CLOSE( route.maxLongitude(), 27.9505, 0.1);
}

// Test the maximum positive longitude to the maximum negative longitude
BOOST_AUTO_TEST_CASE( MaxPositiveToMaxNegativeLong )
{
   Route route = Route(LogFiles::GPXRoutesDir + "MaxPositiveToMaxNegative-N0680337.gpx", isFileName);
   BOOST_CHECK_CLOSE( route.maxLongitude(), 180, 0.1);
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef EXPONENTIAL_HH
#define EXPONENTIAL_HH

#include <boost/random/exponential_distribution.hpp>

#include "random.hh"

//	A simple wrapper for the boost exponential distribution generator
//	 usage:	Exponential exp_distribution_;
//          double val = exp_distribution_.sample();
class Exponential
{
private:
  boost::random::exponential_distribution<> distribution;

  PRNG & prng;

public:
  Exponential( const double & rate, PRNG & s_prng ) : distribution( rate ), prng( s_prng ) {}

  double sample( void ) { return distribution( prng ); }
};

#endif


// Boost.Geometry

// Copyright (c) 2018 Yaghyavardhan Singh Khangarot, Hyderabad, India.

// Contributed and/or modified by Yaghyavardhan Singh Khangarot, as part of Google Summer of Code 2018 program.

// 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_TEST_FRECHET_DISTANCE_HPP
#define BOOST_GEOMETRY_TEST_FRECHET_DISTANCE_HPP

#include <geometry_test_common.hpp>
#include <boost/geometry/algorithms/discrete_frechet_distance.hpp>
#include <boost/geometry/io/wkt/wkt.hpp>
#include <boost/geometry/strategies/strategies.hpp>
#include <boost/variant/variant.hpp>

template <typename Geometry1,typename Geometry2>
void test_frechet_distance(Geometry1 const& geometry1,Geometry2 const& geometry2,
    typename bg::distance_result
        <
            typename bg::point_type<Geometry1>::type,
            typename bg::point_type<Geometry2>::type
        >::type expected_frechet_distance )
{
    using namespace bg;
    typedef typename distance_result
        <
            typename point_type<Geometry1>::type,
            typename point_type<Geometry2>::type
        >::type result_type;
    result_type h_distance = bg::discrete_frechet_distance(geometry1,geometry2);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::ostringstream out;
    out << typeid(typename bg::coordinate_type<Geometry1>::type).name()
        << std::endl
        << typeid(typename bg::coordinate_type<Geometry2>::type).name()
        << std::endl
        << typeid(h_distance).name()
        << std::endl
        << "frechet_distance : " << bg::discrete_frechet_distance(geometry1,geometry2)
        << std::endl;
    std::cout << out.str();
#endif

    BOOST_CHECK_CLOSE(h_distance, expected_frechet_distance, 0.001);
}

template <typename Geometry1,typename Geometry2>
void test_geometry(std::string const& wkt1,std::string const& wkt2,
    typename bg::distance_result
        <
            typename bg::point_type<Geometry1>::type,
            typename bg::point_type<Geometry2>::type
        >::type expected_frechet_distance)
{
    Geometry1 geometry1;
    bg::read_wkt(wkt1, geometry1);
    Geometry2 geometry2;
    bg::read_wkt(wkt2, geometry2);
    test_frechet_distance(geometry1,geometry2,expected_frechet_distance);
#if defined(BOOST_GEOMETRY_TEST_DEBUG)
    test_frechet_distance(boost::variant<Geometry1>(geometry1),boost::variant<Geometry2>(geometry2), expected_frechet_distance);
#endif
}

template <typename Geometry1,typename Geometry2 ,typename Strategy>
void test_frechet_distance(Geometry1 const& geometry1,Geometry2 const& geometry2,Strategy strategy,
    typename bg::distance_result
        <
            typename bg::point_type<Geometry1>::type,
            typename bg::point_type<Geometry2>::type,
            Strategy
        >::type expected_frechet_distance )
{
    using namespace bg;
    typedef typename distance_result
        <
            typename point_type<Geometry1>::type,
            typename point_type<Geometry2>::type,
            Strategy
        >::type result_type;
    result_type h_distance = bg::discrete_frechet_distance(geometry1,geometry2,strategy);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::ostringstream out;
    out << typeid(typename bg::coordinate_type<Geometry1>::type).name()
        << std::endl
        << typeid(typename bg::coordinate_type<Geometry2>::type).name()
        << std::endl
        << typeid(h_distance).name()
        << std::endl
        << "frechet_distance : " << bg::discrete_frechet_distance(geometry1,geometry2,strategy)
        << std::endl;
    std::cout << out.str();
#endif

    BOOST_CHECK_CLOSE(h_distance, expected_frechet_distance, 0.001);
}

template <typename Geometry1,typename Geometry2,typename Strategy>
void test_geometry(std::string const& wkt1,std::string const& wkt2,Strategy strategy,
    typename bg::distance_result
        <
            typename bg::point_type<Geometry1>::type,
            typename bg::point_type<Geometry2>::type,
            Strategy
        >::type expected_frechet_distance)
{
    Geometry1 geometry1;
    bg::read_wkt(wkt1, geometry1);
    Geometry2 geometry2;
    bg::read_wkt(wkt2, geometry2);
    test_frechet_distance(geometry1,geometry2,strategy,expected_frechet_distance);
#if defined(BOOST_GEOMETRY_TEST_DEBUG)
    test_frechet_distance(boost::variant<Geometry1>(geometry1),boost::variant<Geometry2>(geometry2),strategy, expected_frechet_distance);
#endif
}

template <typename Geometry1,typename Geometry2>
void test_empty_input(Geometry1 const& geometry1,Geometry2 const& geometry2)
{
    try
    {
        bg::discrete_frechet_distance(geometry1,geometry2);
    }
    catch(bg::empty_input_exception const& )
    {
        return;
    }
    BOOST_CHECK_MESSAGE(false, "A empty_input_exception should have been thrown" );
}

#endif


#include "Planet.h"
#include "helper/benchmark_helper.h"
#include <cmath>
#include <vector>
#include <chrono>
#include <string>
#include <memory>
#ifdef GPU_SUPPORT
#include <amp.h>
#include <amp_math.h>
#endif
#include <algorithm>
#include <thread>
#include <boost/math/special_functions/sign.hpp>

using std::vector;
using std::chrono::steady_clock;
using std::chrono::microseconds;
using std::chrono::duration_cast;
using std::string;
using std::to_string;
using std::unique_ptr;
using std::make_unique;
using std::min;
using std::max;
using std::thread;

using namespace GeneticSimulation;

// default constructor
GeneticSimulation::Planet::Planet() : initialized(false), timesteps(0) {}

// precompute temperatures
void GeneticSimulation::Planet::precompute_temperatures(const Config& config, bool benchmark)
{
	// set lookup table to correct size
	temperatures.resize(config.area_height * config.orbital_period);
	timesteps = config.orbital_period;

#ifdef GPU_SUPPORT
	// get whether to use GPU
	const bool use_cpu = !config.precompute_temperatures_gpu;
#else
	constexpr bool use_cpu = true;
#endif

	// precompute or benchmark on selected device
	if (use_cpu) {
		// determine number of threads to use
		auto num_threads = config.precompute_temperatures_cpu_threads == 0 ?
			thread::hardware_concurrency() :
			config.precompute_temperatures_cpu_threads;
		// benchmark or precompute once
		benchmark ? benchmark_temperature_computation_cpu(num_threads, config) :
			precompute_temperatures_cpu(num_threads, config);
	}
#ifdef GPU_SUPPORT
	else {
		// benchmark or precompute once
		benchmark ? benchmark_temperature_computation_gpu(config) :
			precompute_temperatures_gpu(config);
	}
#endif

	// record initialization
	initialized = true;
}

// get temperature from lookup table
float GeneticSimulation::Planet::get_temperature(unsigned int y, unsigned int t) const
{
	// return -1 if temperatures have not been computed, otherwise return precomputed temperature
	return initialized ? temperatures[y * timesteps + (t % timesteps)] : -1.f;
}

// precompute temperatures using the CPU
void GeneticSimulation::Planet::precompute_temperatures_cpu(unsigned int worker_threads, const Config& config)
{
	// calculate number of timesteps per thread
	unsigned int timesteps_per_thread = timesteps / worker_threads + 1;
	// create vector for thread objects
	vector<unique_ptr<thread>> threads;
	// start threads
	for (unsigned int i = 0; i < worker_threads; i++) {
		threads.push_back(make_unique<thread>(
			[&, timesteps_per_thread, i] {
				precompute_temperatures_for_timestep_range_cpu(i * timesteps_per_thread,
					(i + 1) * timesteps_per_thread, config);
			}
		));
	}
	// join threads
	for (auto& t_ptr : threads) {
		t_ptr->join();
	}
}

// precompute temperatures on the CPU for the given timestep range
void GeneticSimulation::Planet::precompute_temperatures_for_timestep_range_cpu(unsigned int start_t, unsigned int end_t, const Config& config)
{
	// cap end_t
	end_t = min(end_t, config.orbital_period);

	// initialize vector for storing intermediate equatorial temperature results
	vector<double> equatorial_black_body_temperatures(end_t - start_t);

	// pi
	const double pi = 3.14159265358979323846;

	// reused loop variables
	double angle, pos_x, pos_y, squared_dist, black_body_temperature;

	// calculate equatorial black body temperatures for each timestep (angle in orbit)
	for (unsigned int t = start_t; t < end_t; t++)
	{
		// calculate orbital angle corresponding to timestep
		angle = (static_cast<double>(t) / static_cast<double>(config.orbital_period)) * 2 * pi;

		// calculate the x and y coordinates of the planet at this angle in the orbital ellipse
		pos_x = (config.orbit_radius_x * cos(angle) * cos(config.orbit_rotation)) -
			(config.orbit_radius_y * sin(angle) * sin(config.orbit_rotation)) +
			config.orbit_center_offset_x;
		pos_y = (config.orbit_radius_x * cos(angle) * sin(config.orbit_rotation)) +
			(config.orbit_radius_y * sin(angle) * cos(config.orbit_rotation)) +
			config.orbit_center_offset_y;
		// calculate squared distance from star (0, 0) based on these coordinates
		squared_dist = pos_x * pos_x + pos_y * pos_y;

		// calculate equivalent black body temperature based on this squared distance
		black_body_temperature = pow((config.star_luminosity * (1 - config.albedo)) /
			(16 * pi * squared_dist * 5.670373e-8), 0.25);

		// calculate an approximated equatorial temperature from the average black body temperature
		equatorial_black_body_temperatures[t - start_t] = black_body_temperature / cos(pi / 6.0);
	}

	// reused loop variables
	double latitude, angle_from_vernal_equinox, effective_axial_tilt, effective_latitude,
		height_to_latitude, effective_tilt_plane_dist, width_at_latitude, plane_dist_radius_ratio,
		extra_logitude, daylight_proportion, radiation_strength, base_temperature, moderated_temperature;

	// calculate final temperatures based on timestep (angle in orbit) and y position (latitude)
	for (unsigned int y = 0; y < config.area_height; y++)
	{
		// calculate latitude corresponding to y coordinate
		latitude = -(((static_cast<double>(y) / static_cast<double>(config.area_height - 1))
			* (90.f - -90.f)) - 90.f);

		for (unsigned int t = start_t; t < end_t; t++)
		{
			// calculate orbital angle corresponding to timestep
			angle = (static_cast<double>(t) / static_cast<double>(config.orbital_period)) * 2 * pi;

			// calculate effective axial tilt
			angle_from_vernal_equinox = angle + config.orbit_rotation;
			effective_axial_tilt = sin(angle_from_vernal_equinox) * config.axial_tilt;

			// calculate effective latitude based on effective axial tilt
			effective_latitude = latitude - effective_axial_tilt;

			// calculate the vertical height to the current latitude
			height_to_latitude = sin((latitude / 360.0) * 2 * pi) * config.radius;
			// calculate distance between axially tilted plane and plane
			// dividing day and night, travelling along latitude
			effective_tilt_plane_dist = tan((effective_axial_tilt / 360.0) * 2 * pi) * height_to_latitude;
			// calculate the width of the planet at the current latitude
			width_at_latitude = max(0., cos((latitude / 360.0) * 2 * pi) * config.radius);
			// calculate a safe ratio of the plane distance to the width at latitude
			plane_dist_radius_ratio = width_at_latitude == 0 ?
				boost::math::sign(effective_tilt_plane_dist) :
				effective_tilt_plane_dist / width_at_latitude;
			// calculate the extra longitude in or out of daylight
			extra_logitude = asin(max(-1.0, (min(1.0, plane_dist_radius_ratio))));
			// calculate the proportion of daylight hours at current latitude and effective tilt
			daylight_proportion = (pi + 2.0 * extra_logitude) / (2 * pi);

			// calculate solar radiation strength at current effective latitude
			radiation_strength = max(0., cos((effective_latitude / 360) * 2 * pi));

			// calculate base temperature
			base_temperature = equatorial_black_body_temperatures[t - start_t]
				* radiation_strength * (daylight_proportion * 2);

			// calculate moderated temperature to account for convection etc.
			moderated_temperature = ((base_temperature
				- (equatorial_black_body_temperatures[t - start_t] * config.temperature_moderation_bias))
				/ config.temperature_moderation_factor) + (equatorial_black_body_temperatures[t - start_t]
					* config.temperature_moderation_bias);

			// calculate final temperature which accounts for greenhouse effect
			temperatures[y * config.orbital_period + t] = moderated_temperature
				* pow((1 + 0.75 * config.atmosphere_optical_thickness), 0.25);
		}
	}
}

#ifdef GPU_SUPPORT
// precompute temperatures on the GPU
void GeneticSimulation::Planet::precompute_temperatures_gpu(const Config& config)
{
	// use concurrency namespace for C++ AMP
	using namespace concurrency;

	// pi
	const float pi = 3.14159f;

	// initialize vector for storing intermediate equatorial temperature results
	vector<float> equatorial_black_body_temperatures(config.orbital_period);
	// initialize array view for this vector
	array_view<float, 1> equatorial_black_body_temperatures_av(config.orbital_period,
		equatorial_black_body_temperatures);
	// do not transfer 0s to GPU
	equatorial_black_body_temperatures_av.discard_data();

	// create array view for temperatures lookup table
	array_view<float, 2> temperatures_av(config.area_height, config.orbital_period, temperatures);
	// do not transfer 0s to GPU
	temperatures_av.discard_data();

	// save config values in local variables so AMP can use them
	const unsigned int area_height = config.area_height;
	const float latitude_range = config.latitude_range;
	const unsigned int orbital_period = config.orbital_period;
	const float orbit_center_offset_x = config.orbit_center_offset_x;
	const float orbit_center_offset_y = config.orbit_center_offset_y;
	const float orbit_radius_x = config.orbit_radius_x;
	const float orbit_radius_y = config.orbit_radius_y;
	const float orbit_rotation = config.orbit_rotation;
	const float star_luminosity = config.star_luminosity;
	const float albedo = config.albedo;
	const float axial_tilt = config.axial_tilt;
	const float radius = config.radius;
	const float atmosphere_optical_thickness = config.atmosphere_optical_thickness;
	const float temperature_moderation_factor = config.temperature_moderation_factor;
	const float temperature_moderation_bias = config.temperature_moderation_bias;

	// kernel to calculate equatorial black body temperatures for each time step
	parallel_for_each(
		equatorial_black_body_temperatures_av.extent,
		[=](index<1> idx) restrict(amp) {
			// calculate orbital angle corresponding to timestep
			float angle = (static_cast<float>(idx[0]) / static_cast<float>(orbital_period)) * 2 * pi;

			// calculate the x and y coordinates of the planet at this angle in the orbital ellipse
			float pos_x = (orbit_radius_x * fast_math::cos(angle) * fast_math::cos(orbit_rotation)) -
				(orbit_radius_y * fast_math::sin(angle) * fast_math::sin(orbit_rotation)) +
				orbit_center_offset_x;
			float pos_y = (orbit_radius_x * fast_math::cos(angle) * fast_math::sin(orbit_rotation)) +
				(orbit_radius_y * fast_math::sin(angle) * fast_math::cos(orbit_rotation)) +
				orbit_center_offset_y;
			// calculate squared distance from star (0, 0) based on these coordinates
			float squared_dist = pos_x * pos_x + pos_y * pos_y;

			// calculate equivalent black body temperature based on this squared distance
			float black_body_temperature = fast_math::pow((star_luminosity * (1 - albedo)) /
				(16 * pi * squared_dist * 5.670373e-8), 0.25);

			// calculate an approximated equatorial temperature from the average black body temperature
			equatorial_black_body_temperatures_av[idx] = black_body_temperature / fast_math::cos(pi / 6.0);
		}
	);

	// kernel to calculate final temperatures for each time step and latitude
	parallel_for_each(
		temperatures_av.extent,
		[=](index<2> idx) restrict(amp)
		{
			// calculate latitude corresponding to y coordinate
			float latitude = -(((static_cast<float>(idx[0]) / static_cast<float>(area_height - 1))
				* (90.f - -90.f)) - 90.f);

			// calculate orbital angle corresponding to timestep
			float angle = (static_cast<float>(idx[1]) / static_cast<float>(orbital_period)) * 2 * pi;

			// calculate effective axial tilt
			float angle_from_vernal_equinox = angle + orbit_rotation;
			float effective_axial_tilt = fast_math::sin(angle_from_vernal_equinox) * axial_tilt;

			// calculate effective latitude based on effective axial tilt
			float effective_latitude = latitude - effective_axial_tilt;

			// calculate the vertical height to the current latitude
			float height_to_latitude = fast_math::sin((latitude / 360.0) * 2 * pi) * radius;
			// calculate distance between axially tilted plane and plane
			// dividing day and night, travelling along latitude
			float effective_tilt_plane_dist = fast_math::tan((effective_axial_tilt / 360.0) * 2 * pi) * height_to_latitude;
			// calculate the width of the planet at the current latitude
			float width_at_latitude = max(0, fast_math::cos((latitude / 360.0) * 2 * pi) * radius);
			// calculate a safe ratio of the plane distance to the width at latitude
			float plane_dist_radius_ratio = width_at_latitude == 0 ?
				(0 < effective_tilt_plane_dist) - (effective_tilt_plane_dist < 0) :
				effective_tilt_plane_dist / width_at_latitude;
			// calculate the extra longitude in or out of daylight
			float extra_logitude = fast_math::asin(max(-1.0, (min(1.0, plane_dist_radius_ratio))));
			// calculate the proportion of daylight hours at current latitude and effective tilt
			float daylight_proportion = (pi + 2.0 * extra_logitude) / (2 * pi);

			// calculate solar radiation strength at current effective latitude
			float radiation_strength = max(0, fast_math::cos((effective_latitude / 360) * 2 * pi));

			// save equatorial black body temperature
			float equatorial_black_body_temperature = equatorial_black_body_temperatures_av[idx[1]];

			// calculate base temperature
			float base_temperature = equatorial_black_body_temperature
				* radiation_strength * (daylight_proportion * 2);

			// calculate moderated temperature to account for convection etc.
			float moderated_temperature = ((base_temperature
				- (equatorial_black_body_temperature * temperature_moderation_bias))
				/ temperature_moderation_factor) + (equatorial_black_body_temperature
					* temperature_moderation_bias);

			// calculate final temperature which accounts for greenhouse effect
			temperatures_av[idx] = moderated_temperature
				* fast_math::pow((1 + 0.75 * atmosphere_optical_thickness), 0.25);
		}
	);

	// transfer temperature data from GPU
	temperatures_av.synchronize();
}
#endif

// benchmark precomputation on the CPU
void GeneticSimulation::Planet::benchmark_temperature_computation_cpu(unsigned int worker_threads, const Config& config)
{
	// time points for benchmarking
	steady_clock::time_point start, end;

	// allocate space to store results
	vector<unsigned long long> times(config.planet_benchmark_samples);

	// precompute temperatures and record results
	for (unsigned int i = 0; i < config.planet_benchmark_samples; i++) {
		start = steady_clock::now();
		precompute_temperatures_cpu(worker_threads, config);
		end = steady_clock::now();
		times[i] = duration_cast<microseconds>(end - start).count();
	}

	// output results to file
	string filename = "planet_benchmark_cpu_" + to_string(worker_threads) + "_threads.csv";
	string header = "time_microseconds_" + to_string(worker_threads) + " _threads";
	write_benchmark_results(times, header, filename, config.results_path);
}

#ifdef GPU_SUPPORT
// benchmark precomputation on the GPU
void GeneticSimulation::Planet::benchmark_temperature_computation_gpu(const Config& config)
{
	// time points for benchmarking
	steady_clock::time_point start, end;

	// allocate space to store results
	vector<unsigned long long> times(config.planet_benchmark_samples);

	// precompute temperatures and record results
	for (unsigned int i = 0; i < config.planet_benchmark_samples; i++) {
		start = steady_clock::now();
		precompute_temperatures_gpu(config);
		end = steady_clock::now();
		times[i] = duration_cast<microseconds>(end - start).count();
	}

	// output results to file
	string filename = "planet_benchmark_gpu.csv";
	string header = "time_microseconds_gpu";
	write_benchmark_results(times, header, filename, config.results_path);
}
#endif


#include <fmt/core.h>
#include <string_view>
#include <chrono>

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

#include <autodiff/forward/real.hpp>
#include <autodiff/forward/real/eigen.hpp>

#include "types.hh"
#include "cvode_wrapper.hh"

using namespace types;

struct System
{
    template<class T>
    inline auto f(Eigen::MatrixBase<T>& y, double t)
    {
        using mt = T::PlainObject;
        mt ydot(y.rows());

        ydot(0) = -0.04*y(0) + 1.0E4*y(1)*y(2);
        ydot(2) = 3.0E7*y(1)*y(1);
        ydot(1) = -ydot(0) - ydot(2);

        return ydot;
    }

    template<class T>
    inline auto J(Eigen::MatrixBase<T>& y, double t)
    {
        auto _f = [this](auto&& ...args){ return f(std::forward<decltype(args)>(args)...); };
        dvector_t dydt(y.size());
        dvector_t yy = y;
        matrix_t J = autodiff::jacobian(_f, autodiff::wrt(yy), autodiff::at(yy,t), dydt);
        return J;
    }
};

int main()
{

    vector_t y0(3);
    y0(0) = 1.0; y0(1) = 0.0; y0(2) = 0.0;

    auto stepper = cvode_wrapper::cvode_stepper<System>(cvode_wrapper::cv_options{});
    stepper.initialize(y0);

    stepper.letsgo();
    return 0;
}


#pragma once
#include <Eigen/Core>
#include <cppad/cppad.hpp>
#include <limits>

namespace Eigen {

template <class Base>
struct NumTraits<CppAD::AD<Base>> {
    // type that corresponds to the real part of an AD<Base> value
    typedef CppAD::AD<Base> Real;
    // type for AD<Base> operations that result in non-integer values
    typedef CppAD::AD<Base> NonInteger;
    //  type to use for numeric literals such as "2" or "0.5".
    typedef CppAD::AD<Base> Literal;
    // type for nested value inside an AD<Base> expression tree
    typedef CppAD::AD<Base> Nested;

    enum {
        // does not support complex Base types
        IsComplex             = 0 ,
        // does not support integer Base types
        IsInteger             = 0 ,
        // only support signed Base types
        IsSigned              = 1 ,
        // must initialize an AD<Base> object
        RequireInitialization = 1 ,
        // computational cost of the corresponding operations
        ReadCost              = 1 ,
        AddCost               = 2 ,
        MulCost               = 2
    };

    // machine epsilon with type of real part of x
    // (use assumption that Base is not complex)
    static CppAD::AD<Base> epsilon(void)
    {
        return CppAD::numeric_limits<CppAD::AD<Base>>::epsilon();
    }

    // relaxed version of machine epsilon for comparison of different
    // operations that should result in the same value
    static CppAD::AD<Base> dummy_precision(void)
    {
        return 100. * CppAD::numeric_limits<CppAD::AD<Base>>::epsilon();
    }

    // minimum normalized positive value
    static CppAD::AD<Base> lowest(void)
    {
        return CppAD::numeric_limits< CppAD::AD<Base> >::min();
    }

    // maximum finite value
    static CppAD::AD<Base> highest(void)
    {
        return CppAD::numeric_limits< CppAD::AD<Base> >::max();
    }

    // number of decimal digits that can be represented without change.
    static int digits10(void)
    {
        return CppAD::numeric_limits< CppAD::AD<Base> >::digits10;
    }
};

}

namespace CppAD {

// functions that return references
template <class Base>
const AD<Base>& conj(const AD<Base>& x) { return x; }
template <class Base>
const AD<Base>& real(const AD<Base>& x) { return x; }

// functions that return values (note abs is defined by cppad.hpp)
template <class Base> AD<Base>
imag(const AD<Base>& x) { return CppAD::AD<Base>(0.); }
template <class Base> AD<Base>
abs2(const AD<Base>& x) { return x * x; }

}

template <typename Type>
Eigen::Matrix<Type, 3, 3> exp3x3AD(const Eigen::Matrix<Type, 3, 1>& ax)
{
    Eigen::Matrix<Type, 3, 3> out;
    out.setZero();
    Type angle = CppAD::sqrt(ax.dot(ax));
    if (CppAD::abs(angle) < std::numeric_limits<double>::epsilon()) {
        out(0, 0) = Type(1);
        out(1, 1) = Type(1);
        out(2, 2) = Type(1);
    } else {
        Type x = ax(0) / angle;
        Type y = ax(1) / angle;
        Type z = ax(2) / angle;
        Type sa = CppAD::sin(angle);
        Type ca = CppAD::cos(angle);
        Type t = 1 - ca;

        out(0, 0) = ca + t * x * x;
        out(1, 0) = t * y * x + sa * z;
        out(2, 0) = t * z * x - sa * y;
        out(0, 1) = t * x * y - sa * z;
        out(1, 1) = ca + t * y * y;
        out(2, 1) = t * z * y + sa * x;
        out(0, 2) = t * x * z + sa * y;
        out(1, 2) = t * y * z - sa * x;
        out(2, 2) = ca + t * z * z;
    }

    return out;
}


#include "odoflow/RigidEstimator.h"
#include "camplex/FiducialCommon.h"
#include "argus_utils/utils/ParamUtils.h"

#include <opencv2/calib3d.hpp>
#include <Eigen/SVD>

namespace argus
{
RigidEstimator::RigidEstimator( ros::NodeHandle& nh, ros::NodeHandle& ph )
{
	_logReprojThreshold.Initialize( ph, -2,
	                                "log_reprojection_threshold",
	                                "RANSAC log10 reprojection inlier threshold" );

	_maxIters.Initialize( ph, 10, "max_iters",
	                      "RANSAC max iterations" );
	_maxIters.AddCheck<GreaterThan>( 0 );
	_maxIters.AddCheck<IntegerValued>( ROUND_CEIL );
}

bool RigidEstimator::EstimateMotion( InterestPoints& key,
                                     InterestPoints& tar,
									 std::vector<unsigned int>& inlierInds,
                                     PoseSE2& transform  )
{
	if( key.empty() || tar.empty() )
	{
		ROS_INFO_STREAM( "RigidEstimator: Received empty points." );
		return false;
	}

	std::vector<char> inliers;
	// We want tar in frame of key, so this is the ordering
	cv::Mat Hxest = cv::findHomography( tar,
	                                    key,
	                                    cv::RANSAC,
	                                    std::pow( 10, _logReprojThreshold ),
	                                    inliers,
	                                    _maxIters );
	if( Hxest.empty() )
	{
		ROS_INFO_STREAM( "RigidEstimator: Failed to find homography." );
		return false;
	}

	InterestPoints keyInliers, tarInliers;
	for( unsigned int i = 0; i < inliers.size(); i++ )
	{
		if( inliers[i] )
		{
			inlierInds.push_back(i);
		}
	}

	Eigen::MatrixXd Ab = MatToEigen<double>( Hxest );

	// Extract the rotation using Procrustes solution
	Eigen::Matrix2d A = Ab.block<2, 2>( 0, 0 );
	Eigen::JacobiSVD<Eigen::Matrix2d> svd( A, Eigen::ComputeFullU | Eigen::ComputeFullV );
	Eigen::Matrix2d R = svd.matrixU() * svd.matrixV().transpose();

	// NOTE We use standard coordinates (x-forward)
	FixedMatrixType<3, 3> H = FixedMatrixType<3, 3>::Identity();
	H.block<2, 2>( 1, 1 ) = R;
	H( 1, 2 ) = -Ab( 0, 2 ); // Image x corresponds to camera -y
	H( 2, 2 ) = -Ab( 1, 2 ); // Image y corresponds to camera -z
	transform = PoseSE2( H );
	return true;
}
}


//
// Copyright (c) 2015-2018 CNRS
//

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/utils/timer.hpp"
#include "pinocchio/parsers/sample-models.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( test_com )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  pinocchio::Data data(model);

  VectorXd q = VectorXd::Ones(model.nq);
  q.middleRows<4> (3).normalize();
  VectorXd v = VectorXd::Ones(model.nv);
  VectorXd a = VectorXd::Ones(model.nv);

  crba(model,data,q);

	/* Test COM against CRBA*/
  Vector3d com = centerOfMass(model,data,q);
  BOOST_CHECK(data.com[0].isApprox(getComFromCrba(model,data), 1e-12));

	/* Test COM against Jcom (both use different way to compute the COM). */
  com = centerOfMass(model,data,q);
  jacobianCenterOfMass(model,data,q);
  BOOST_CHECK(com.isApprox(data.com[0], 1e-12));

	/* Test COM against Jcom (both use different way to compute the COM). */
  centerOfMass(model,data,q,v,a);
  BOOST_CHECK(com.isApprox(data.com[0], 1e-12));

  /* Test vCoM against nle algorithm without gravity field */
  a.setZero();
  model.gravity.setZero();
  centerOfMass(model,data,q,v,a);
  nonLinearEffects(model, data, q, v);

  pinocchio::SE3::Vector3 acom_from_nle (data.nle.head <3> ()/data.mass[0]);
  BOOST_CHECK((data.liMi[1].rotation() * acom_from_nle).isApprox(data.acom[0], 1e-12));

	/* Test Jcom against CRBA  */
  Eigen::MatrixXd Jcom = jacobianCenterOfMass(model,data,q);
  BOOST_CHECK(data.Jcom.isApprox(getJacobianComFromCrba(model,data), 1e-12));

  /* Test CoM velocity againt jacobianCenterOfMass */
  BOOST_CHECK((Jcom * v).isApprox(data.vcom[0], 1e-12));

  centerOfMass(model,data,q,v);
  /* Test CoM velocity againt jacobianCenterOfMass */
  BOOST_CHECK((Jcom * v).isApprox(data.vcom[0], 1e-12));

//  std::cout << "com = [ " << data.com[0].transpose() << " ];" << std::endl;
//  std::cout << "mass = [ " << data.mass[0] << " ];" << std::endl;
//  std::cout << "Jcom = [ " << data.Jcom << " ];" << std::endl;
//  std::cout << "M3 = [ " << data.M.topRows<3>() << " ];" << std::endl;
}

BOOST_AUTO_TEST_CASE ( test_mass )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);

  double mass = computeTotalMass(model);

  BOOST_CHECK(mass == mass); // checking it is not NaN

  double mass_check = 0.0;
  for(size_t i=1; i<(size_t)(model.njoints);++i)
    mass_check += model.inertias[i].mass();

  BOOST_CHECK_CLOSE(mass, mass_check, 1e-12);

  pinocchio::Data data1(model);

  double mass_data = computeTotalMass(model,data1);

  BOOST_CHECK(mass_data == mass_data); // checking it is not NaN
  BOOST_CHECK_CLOSE(mass, mass_data, 1e-12);
  BOOST_CHECK_CLOSE(data1.mass[0], mass_data, 1e-12);

  pinocchio::Data data2(model);
  VectorXd q = VectorXd::Ones(model.nq);
  q.middleRows<4> (3).normalize();
  centerOfMass(model,data2,q);

  BOOST_CHECK_CLOSE(data2.mass[0], mass, 1e-12);
}

BOOST_AUTO_TEST_CASE ( test_subtree_masses )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);

  pinocchio::Data data1(model);

  computeSubtreeMasses(model,data1);

  pinocchio::Data data2(model);
  VectorXd q = VectorXd::Ones(model.nq);
  q.middleRows<4> (3).normalize();
  centerOfMass(model,data2,q);

  for(size_t i=0; i<(size_t)(model.njoints);++i)
  {
    BOOST_CHECK_CLOSE(data1.mass[i], data2.mass[i], 1e-12);
  }
}

//BOOST_AUTO_TEST_CASE ( test_timings )
//{
//  using namespace Eigen;
//  using namespace pinocchio;
//
//  pinocchio::Model model;
//  pinocchio::buildModels::humanoidRandom(model);
//  pinocchio::Data data(model);
//
//  long flag = BOOST_BINARY(1111);
//  PinocchioTicToc timer(PinocchioTicToc::US);
//  #ifdef NDEBUG
//    #ifdef _INTENSE_TESTING_
//      const size_t NBT = 1000*1000;
//    #else
//      const size_t NBT = 10;
//    #endif
//  #else
//    const size_t NBT = 1;
//    std::cout << "(the time score in debug mode is not relevant)  " ;
//  #endif
//
//  bool verbose = flag & (flag-1) ; // True is two or more binaries of the flag are 1.
//  if(verbose) std::cout <<"--" << std::endl;
//  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
//
//  if( flag >> 0 & 1 )
//  {
//    timer.tic();
//    SMOOTH(NBT)
//    {
//      centerOfMass(model,data,q);
//    }
//    if(verbose) std::cout << "COM =\t";
//    timer.toc(std::cout,NBT);
//  }
//
//  if( flag >> 1 & 1 )
//  {
//    timer.tic();
//    SMOOTH(NBT)
//    {
//      centerOfMass(model,data,q,false);
//    }
//    if(verbose) std::cout << "Without sub-tree =\t";
//    timer.toc(std::cout,NBT);
//  }
//
//  if( flag >> 2 & 1 )
//  {
//    timer.tic();
//    SMOOTH(NBT)
//    {
//      jacobianCenterOfMass(model,data,q);
//    }
//    if(verbose) std::cout << "Jcom =\t";
//    timer.toc(std::cout,NBT);
//  }
//}

BOOST_AUTO_TEST_SUITE_END ()