//
// Created by david on 2019-03-18.
//
#include <Eigen/Core>
#include <iostream>
#include <simulation/class_simulation_status.h>
#include <math/class_eigsolver.h>
#include <math/arpack_extra/matrix_product_stl.h>
#include <math/arpack_extra/matrix_product_sparse.h>
#include <spdlog/spdlog.h>
#include <state/tools/finite/opt.h>
#include <state/class_finite_state.h>
#include <LBFGS.h>
#include <simulation/nmspc_settings.h>
#include <general/nmspc_random_numbers.h>
#include <spdlog/fmt/bundled/ranges.h>

using namespace tools::finite::opt;
using namespace tools::finite::opt::internals;

template<typename T> using MatrixType = Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic>;

std::vector<int> tools::finite::opt::internals::generate_size_list(size_t shape){
    int max_nev ;
    if      (shape <= 512)  {max_nev = shape/4;}
    else if (shape > 512  and shape <= 1024) {max_nev = 128;}
    else if (shape > 1024 and shape <= 2048) {max_nev = 64;}
    else if (shape > 2048 and shape <= 4096) {max_nev = 32;}
    else if (shape > 4096 and shape <= 8192) {max_nev = 16;}
    else                                     {max_nev = 8;}

    int min_nev = std::min(std::min(8,(int)shape),max_nev);

    std::vector<int> nev_list = {min_nev};
    int tmp_nev = min_nev;
    while (tmp_nev < max_nev){
        tmp_nev = std::min(4*tmp_nev, max_nev);
        nev_list.push_back(tmp_nev);
    }
    return nev_list;
}

int idx_best_overlap_in_window(const Eigen::VectorXd &overlaps, const Eigen::VectorXd & eigvals, double lbound, double ubound){
    assert(overlaps.size() == eigvals.size() and "idx_best_overlap_in_window: Mismatch in overlaps and eigvals sizes");
    Eigen::VectorXd overlaps_in_window = overlaps;
    for (int i = 0; i < overlaps.size(); i++){
        if (eigvals(i) > ubound) overlaps_in_window(i) = 0.0;
        if (eigvals(i) < lbound) overlaps_in_window(i) = 0.0;
    }
    if (overlaps_in_window.isZero(0.0)){overlaps_in_window = overlaps;}

    int idx;
    [[maybe_unused]] double max_overlap = overlaps_in_window.maxCoeff(&idx);
    return idx;

}

template<typename Scalar>
std::tuple<Eigen::MatrixXcd, Eigen::VectorXd>
find_subspace_full(const MatrixType<Scalar> & H_local, Eigen::Tensor<std::complex<double>,3> &theta, std::vector<reports::eig_tuple> &eig_log){
    tools::log->trace("Finding subspace -- full");
    using namespace eigutils::eigSetting;
    t_eig->tic();

    Eigen::VectorXd   eigvals;
    Eigen::MatrixXcd  eigvecs;
    class_eigsolver solver;

    if constexpr (!std::is_same<Scalar, double>::value)
    {
        solver.eig<Type::CPLX, Form::SYMMETRIC>(H_local, true, false);
        eigvals = Eigen::Map<const Eigen::VectorXd>(solver.solution.get_eigvals<Form::SYMMETRIC>().data(),
                                                    solver.solution.meta.cols);
        eigvecs = Eigen::Map<const Eigen::MatrixXcd>(solver.solution.get_eigvecs<Type::CPLX, Form::SYMMETRIC>().data(),
                                                     solver.solution.meta.rows, solver.solution.meta.cols);
    }
    else
    {
        solver.eig<Type::REAL, Form::SYMMETRIC>(H_local, true, false);
        eigvals = Eigen::Map<const Eigen::VectorXd>(solver.solution.get_eigvals<Form::SYMMETRIC>().data(),
                                                    solver.solution.meta.cols);
        eigvecs = Eigen::Map<const Eigen::MatrixXd>(solver.solution.get_eigvecs<Type::REAL, Form::SYMMETRIC>().data(),
                                                    solver.solution.meta.rows, solver.solution.meta.cols);
    }
    t_eig->toc();
    tools::log->debug("Finished eigensolver -- condition: Full diagonalization");
    Eigen::Map<const Eigen::VectorXcd> theta_vec   (theta.data(),theta.size());
    Eigen::VectorXd overlaps = (theta_vec.adjoint() * eigvecs).cwiseAbs().real();
    int idx;
    double max_overlap       = overlaps.maxCoeff(&idx);
    double min_overlap       = overlaps.minCoeff();
    double sq_sum_overlap    = overlaps.cwiseAbs2().sum();
    double subspace_quality  = 1.0 - sq_sum_overlap;
    int nev = eigvecs.cols();
    eig_log.emplace_back(nev, max_overlap, min_overlap, sq_sum_overlap, std::log10(subspace_quality), t_eig->get_last_time_interval(), 0);

    return std::make_tuple(eigvecs,eigvals);
}

template<typename Scalar>
std::tuple<Eigen::MatrixXcd, Eigen::VectorXd>
find_subspace_part(const MatrixType<Scalar> & H_local, Eigen::Tensor<std::complex<double>,3> &theta, double energy_target, std::vector<reports::eig_tuple> &eig_log){
    using namespace eigutils::eigSetting;
    tools::log->trace("Finding subspace -- partial");

    t_eig->tic();
    // You need to copy the data into StlMatrixProduct, because the PartialPivLU will overwrite the data in H_local otherwise.
    StlMatrixProduct<Scalar> hamiltonian(H_local.data(),H_local.rows(),Form::SYMMETRIC,Side::R, true);
    hamiltonian.set_shift(energy_target);
    hamiltonian.FactorOP();
    double t_lu = hamiltonian.t_factorOp.get_last_time_interval();
    t_eig->toc();

    double max_overlap_threshold      = 1 - 1e-1; //1.0/std::sqrt(2); //Slightly less than 1/sqrt(2), in case that the choice is between cat states.
    double subspace_quality_threshold = settings::precision::MinSubspaceQuality;

    class_eigsolver solver;
    std::string reason = "exhausted";
    Eigen::VectorXd  eigvals;
    Eigen::MatrixXcd eigvecs;
    Eigen::Map<const Eigen::VectorXcd> theta_vec   (theta.data(),theta.size());
    for (auto nev : generate_size_list(theta.size())){
        t_eig->tic();
        solver.eigs_stl(hamiltonian,nev,-1, energy_target,Form::SYMMETRIC,Ritz::LM,Side::R, true,false);
        t_eig->toc();

        eigvals = Eigen::Map<const Eigen::VectorXd > (solver.solution.get_eigvals<Form::SYMMETRIC>().data()      ,solver.solution.meta.cols);
        if constexpr (std::is_same<std::complex<double>, Scalar >::value){
            eigvecs = Eigen::Map<const Eigen::MatrixXcd> (solver.solution.get_eigvecs<Type::CPLX, Form::SYMMETRIC>().data(),solver.solution.meta.rows,solver.solution.meta.cols);
        }else{
            eigvecs = Eigen::Map<const Eigen::MatrixXd> (solver.solution.get_eigvecs<Type::REAL, Form::SYMMETRIC>().data(),solver.solution.meta.rows,solver.solution.meta.cols);
        }

        Eigen::VectorXd overlaps = (theta_vec.adjoint() * eigvecs).cwiseAbs().real();
        double max_overlap       = overlaps.maxCoeff();
        double min_overlap       = overlaps.minCoeff();
        double sq_sum_overlap    = overlaps.cwiseAbs2().sum();
        double subspace_quality  = 1.0 - sq_sum_overlap;
        eig_log.emplace_back(nev, max_overlap, min_overlap, sq_sum_overlap, std::log10(subspace_quality), t_eig->get_last_time_interval(), t_lu);
        t_lu = 0;
        if(max_overlap            > 1.0 + 1e-10)                 throw std::runtime_error("max_overlap larger than one : "  + std::to_string(max_overlap));
        if(sq_sum_overlap         > 1.0 + 1e-10)                 throw std::runtime_error("eps larger than one : "          + std::to_string(sq_sum_overlap));
        if(min_overlap            < 0.0)                         throw std::runtime_error("min_overlap smaller than zero: " + std::to_string(min_overlap));
        if(max_overlap            >= max_overlap_threshold )    {reason = "overlap is good enough"; break;}
        if(subspace_quality       < subspace_quality_threshold) {reason = "subspace quality is good enough"; break;}
    }
    tools::log->debug("Finished partial eigensolver -- condition: {}",reason);
    return std::make_tuple(eigvecs,eigvals);
}

template<typename Scalar>
std::tuple<Eigen::MatrixXcd, Eigen::VectorXd>
find_subspace(const class_finite_state & state){
    tools::log->trace("Finding subspace");

    using namespace eigutils::eigSetting;
    t_ham->tic();

    MatrixType<Scalar> H_local;
    if constexpr(std::is_same<Scalar,double>::value){
        H_local = state.get_multi_hamiltonian_matrix().real();
    }
    if constexpr(std::is_same<Scalar,std::complex<double>>::value){
        H_local = state.get_multi_hamiltonian_matrix();
    }

    if(not H_local.isApprox(H_local.adjoint(), 1e-14)){
        throw std::runtime_error(fmt::format("H_local is not hermitian: {:.16f}", (H_local - H_local.adjoint()).cwiseAbs().sum()));
    }
    double sparcity = (H_local.array().cwiseAbs2() != 0.0).count()/(double)H_local.size();
    tools::log->debug("H_local nonzeros: {:.8f} %", sparcity*100);

    t_ham->toc();
    auto theta = state.get_multitheta();

    Eigen::MatrixXcd eigvecs;
    Eigen::VectorXd  eigvals;
    std::vector<reports::eig_tuple> eig_log;
    double energy_target = tools::finite::measure::multisite::energy(state,theta);

    // If theta is small enough you can afford full diag.
    if   ((size_t)theta.size() <= settings::precision::MaxSizeFullDiag) {
        std::tie(eigvecs, eigvals) = find_subspace_full(H_local, theta, eig_log);
    }else{
        std::tie(eigvecs, eigvals) = find_subspace_part(H_local,theta,energy_target,eig_log);
    }
    reports::print_report(eig_log);

    if constexpr(std::is_same<Scalar,double>::value){
        Textra::subtract_phase(eigvecs);
        tools::log->trace("truncating imag of eigvecs, sum: {}", eigvecs.imag().cwiseAbs().sum() );
        eigvecs = eigvecs.real();
    }

    eigvecs.colwise().normalize();
    return std::make_tuple(eigvecs, eigvals);
}

Eigen::Tensor<class_finite_state::Scalar,3>
tools::finite::opt::internals::old_subspace_optimization(const class_finite_state &state,
                                                         const class_simulation_status &sim_status, OptType optType,
                                                         OptMode optMode){
    tools::log->trace("Optimizing in SUBSPACE mode");
    using Scalar = class_finite_state::Scalar;
    using namespace eigutils::eigSetting;

    double chain_length    = state.get_length();
    auto theta             = state.get_multitheta();
    auto theta_old         = Eigen::Map<const Eigen::VectorXcd>  (theta.data(),theta.size());
    Eigen::MatrixXcd eigvecs;
    Eigen::VectorXd  eigvals;
    switch(optType){
        case OptType::CPLX:     std::tie (eigvecs,eigvals)  = find_subspace<Scalar>(state); break;
        case OptType::REAL:     std::tie (eigvecs,eigvals)  = find_subspace<double>(state); break;
    }
    Eigen::VectorXd overlaps = (theta_old.adjoint() * eigvecs).cwiseAbs().real();

    switch (optMode){
        case OptMode::OVERLAP:
        {
            int idx = idx_best_overlap_in_window(overlaps,eigvals,sim_status.energy_lbound,sim_status.energy_ubound);
            return Textra::Matrix_to_Tensor(eigvecs.col(idx), state.active_dimensions());
        }

        case OptMode::VARIANCE:
        {
            double sq_sum_overlap    = overlaps.cwiseAbs2().sum();
            double subspace_quality  = 1.0 - sq_sum_overlap;
            if(subspace_quality > settings::precision::MinSubspaceQuality) {
                tools::log->debug("Subspace quality is poor: {} > {}. Switching to direct mode", subspace_quality, settings::precision::MinSubspaceQuality);
                return ceres_direct_optimization(state, sim_status, optType);
            }
        }

        }

    Eigen::VectorXcd theta_new;
    double overlap_new  = 0;
    double energy_new,variance_new,norm;
    //Should really use theta_start as the projection towards the previous theta, not best overlapping!
    // Note that alpha_i = <theta_old | theta_new_i> is not supposed to be squared! The overlap
    // Between theta_start and theta_old should be
    Eigen::VectorXcd theta_start      = (eigvecs.adjoint()  * theta_old).normalized()  ;

    std::vector<reports::subspc_opt_tuple> opt_log;

    t_opt->tic();
    state.unset_measurements();
    double energy_0   = tools::finite::measure::energy_per_site(state);
    double variance_0 = tools::finite::measure::energy_variance_per_site(state);
    t_opt->toc();
    Eigen::VectorXcd theta_0 = (eigvecs * theta_start.asDiagonal()).rowwise().sum().normalized();

    int iter_0 = 0;
    double overlap_0 = std::abs(theta_old.dot(theta_0));

    opt_log.emplace_back("Initial",theta.size(), energy_0, std::log10(variance_0), overlap_0, iter_0,0, t_opt->get_last_time_interval());

    t_opt->tic();
    using namespace LBFGSpp;
    using namespace tools::finite::opt::internals;
    double fx;
    int niter,counter;
    LBFGSpp::LBFGSSolver<double> solver(params);
    switch (optType){
        case OptType::CPLX:{
            tools::finite::opt::internals::subspace_functor <Scalar>  functor (state,sim_status,eigvecs,eigvals);
            Eigen::VectorXd  theta_start_cast = Eigen::Map<Eigen::VectorXd>(reinterpret_cast<double*> (theta_start.data()), 2*theta_start.size());
            tools::log->trace("Running LBFGS");
            niter        = solver.minimize(functor, theta_start_cast, fx);
            theta_start  = Eigen::Map<Eigen::VectorXcd>(reinterpret_cast<Scalar*> (theta_start_cast.data()), theta_start_cast.size()/2).normalized();
            counter      = functor.get_count();
            norm         = functor.get_norm();
            energy_new   = functor.get_energy() / chain_length;
            variance_new = functor.get_variance()/chain_length;
            theta_new    = (eigvecs * theta_start.asDiagonal()).rowwise().sum().normalized();
            break;
        }
        case OptType::REAL:{
            tools::finite::opt::internals::subspace_functor <double> functor (state,sim_status,eigvecs.real(),eigvals);
            Eigen::VectorXd  theta_start_cast = theta_start.real();
            tools::log->trace("Running LBFGS");
            niter        = solver.minimize(functor, theta_start_cast, fx);
            theta_start  = theta_start_cast.normalized().cast<Scalar>();
            counter      = functor.get_count();
            norm         = functor.get_norm();
            energy_new   = functor.get_energy() / chain_length;
            variance_new = functor.get_variance()/chain_length;
            theta_new    = (eigvecs.real() * theta_start.real().asDiagonal()).rowwise().sum().normalized();
            break;
        }
    }
    t_opt->toc();

    overlap_new = (theta_old.adjoint() * theta_new).cwiseAbs().sum();
    opt_log.emplace_back("LBFGS++",theta.size(), energy_new, std::log10(variance_new), overlap_new, niter,counter, t_opt->get_last_time_interval());
    tools::log->trace("Finished LBFGS");

    reports::print_report(opt_log);
    state.unset_measurements();

    if (variance_new < variance_0){
        tools::log->debug("Returning new theta");
        return  Textra::Matrix_to_Tensor(theta_new, state.active_dimensions());

    }else{
        tools::log->debug("Returning old theta");
        return  theta;
    }

}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_DIVS_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_DIVS_HPP_INCLUDED
#include <boost/simd/arithmetic/functions/divs.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/simd/include/functions/simd/is_eqz.hpp>
#include <boost/simd/include/functions/simd/is_nez.hpp>
#include <boost/simd/include/functions/simd/divides.hpp>
#include <boost/simd/include/functions/simd/is_ltz.hpp>
#include <boost/simd/include/functions/simd/seladd.hpp>
#include <boost/simd/include/functions/simd/if_zero_else.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/bitwise_xor.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/include/functions/simd/complement.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/constants/valmax.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divs_, tag::cpu_, (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                              ((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return a0/a1;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divs_, tag::cpu_, (A0)(X)
                            , ((simd_<uint_<A0>,X>))
                              ((simd_<uint_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::as_logical<A0>::type bA0;
      const bA0 iseqza1 = is_eqz(a1);
      const A0 aa1 =  seladd(iseqza1, a1, One<A0>());
      const A0 r1 = a0/aa1; //a1!= 0
      const A0 r2 = if_zero_else(is_eqz(a0),Valmax<A0>());
      return if_else(iseqza1, r2, r1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divs_, tag::cpu_, (A0)(X)
                            , ((simd_<int_<A0>,X>))
                              ((simd_<int_<A0>,X>))
                            )
  {
    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::as_logical<A0>::type bA0;
      const bA0 iseqza1 = is_eqz(a1);
      const A0 c = ~(genmask(a0-Valmin<A0>())|genmask(a1+One<A0>()));
      const A0 aa1 = seladd(iseqza1, a1, One<A0>());
      const A0 r1 = (a0-c)/aa1; //a1!= 0
      const A0 r2 = ((genmask(is_ltz(a0))^Valmax<A0>())) & genmask(is_nez(a0)); // r2 is Valmin Valmax or 0
      return if_else(iseqza1, r2, r1);
    }
  };
} } }

#endif


#include <iostream>
#include <algorithm>
#include <string>
#include <vector>
#include <cstdio>
#include <cstring>
//#include <conio.h>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string/replace.hpp>

#define LET_AND_OP '&'
#define LET_OR_OP  '|'
#define LET_NOT_OP '!'

using namespace std;
using namespace boost;

string appName;
bool   debug = false;

class LET_TERM
{
	private:
		string			rawTerm;
		vector<char>	variables;

		static bool isBracket(char c)
		{
			if(c == '('  ||  c == ')')
				return true;
			return false;
		}

		static bool is2OperandOp(char c)
		{
			if(c == LET_AND_OP  ||  c == LET_OR_OP)
				return true;
			return false;
		}

		static bool isBoolNum(char c)
		{
			if(c == '0'  ||  c == '1')
				return true;
			return false;
		}

		static bool isLetter(char c)
		{
			if((c >= 'A'  &&  c <= 'Z')  ||  (c >= 'a'  &&  c <= 'z'))
				return true;
			return false;
		}

		static bool isLetterOrBool(char c)
		{
			if(isLetter(c)  ||  isBoolNum(c))
				return true;
			return false;
		}

		static bool isBadChar(char c)
		{
			if(isLetter(c)  ||  isBracket(c)  ||  isBoolNum(c)  ||  is2OperandOp(c)  ||  c == LET_NOT_OP)
				return false;
			return true;
		}

		static void solveMostInnerBracket(string &term)
		{
			unsigned long partBegin = 0;
			unsigned long partEnd   = term.size()-1;
			string        part;

			for(unsigned long i=0; i<term.size(); i++)
			{
				if(term[i] == '(')
					partBegin = i+1;
				else if(term[i] == ')')
				{
					partEnd = i-1;
					break;
				}
			}

			part = term.substr(partBegin, partEnd-partBegin+1);

			//Solve NOT operations
			for(unsigned long i=0; i<part.size()-1; i++)
			{
				if(part[i] == LET_NOT_OP)
				{
					part.erase(i, 1);
					part[i] = (!(part[i]-'0')) + '0';
				}
			}
			if(debug)
				printf("    Step 2.1: %s\n", part.c_str());

			//Solve AND operations
			for(unsigned long i=1; i<part.size()-1; i++)
			{
				if(part[i] == LET_AND_OP)
				{
					char sol = ((part[i-1]-'0') & (part[i+1]-'0')) + '0';
					part[i-1] = sol;
					part.erase(i, 2);
				}
			}
			if(debug)
				printf("    Step 2.2: %s\n", part.c_str());

			//Solve OR operations
			for(unsigned long i=1; i<part.size()-1; i++)
			{
				if(part[i] == LET_OR_OP)
				{
					char sol = ((part[i-1]-'0') | (part[i+1]-'0')) + '0';
					part[i-1] = sol;
					part.erase(i, 2);
				}
			}
			if(debug)
				printf("    Step 2.3: %s\n", part.c_str());

			term.replace(term.begin()+partBegin, term.begin()+partEnd+1, part);
		}

		static void stripBrackets(string &term)
		{
			replace_all(term, "(0)", "0");
			replace_all(term, "(1)", "1");
		}

	public:
		void set(string newRawTerm)
		{
			rawTerm = newRawTerm;
			rawTerm.erase(remove_if(rawTerm.begin(), rawTerm.end(), &isBadChar), rawTerm.end());
			replace_all(rawTerm, "()", "");
			if(rawTerm != newRawTerm)
				printf("%s: info:  stripped invalid characters, new expression: %s\n", appName.c_str(), rawTerm.c_str());
		}

		bool parse()
		{
			int brackets = 0;

			//Two subsequent letters are not allowed
			for(unsigned long i=0; i<rawTerm.size()-1; i++)
			{
				if(isLetterOrBool(rawTerm[i])  &&  isLetterOrBool(rawTerm[i+1]))
				{
					printf("%s: error: parsing error near character %ld (\"%c%c\" is invalid)\n", appName.c_str(), i, rawTerm[i], rawTerm[i+1]);
					return false;
				}
			}

			//Count number of open and closed brackets, detect possible inequality
			for(unsigned long i=0; i<rawTerm.size(); i++)
			{
				if(rawTerm[i] == '(')
					brackets++;
				else if(rawTerm[i] == ')')
					brackets--;
			}
			if(brackets != 0)
			{
				printf("%s: error: unequal number of open and closed brackets\n", appName.c_str());
				return false;
			}

			//Check AND and OR correctness (only "A|B", "A|!", "A|(", ")|(", ")|!" and ")|B" are correct [also & instead of | and 0 or 1 instead of A or B, respectively])
			for(unsigned long i=0; i<rawTerm.size(); i++)
			{
				if(is2OperandOp(rawTerm[i]))
				{
					if(i == 0  ||  i == rawTerm.size()-1)
					{
						printf("%s: error: parsing error near character %ld (\"%c%c\" is invalid)\n", appName.c_str(), i, rawTerm[i-(i==0?0:1)], rawTerm[i+(i==0?1:0)]);
						return false;
					}
					if(!((isLetterOrBool(rawTerm[i-1])  &&  isLetterOrBool(rawTerm[i+1]))  ||  (isLetterOrBool(rawTerm[i-1])  &&  rawTerm[i+1] == LET_NOT_OP)  ||  (isLetterOrBool(rawTerm[i-1])  &&  rawTerm[i+1] == '(')  ||  (rawTerm[i-1] == ')'  &&  rawTerm[i+1] == '(')  ||  (rawTerm[i-1] == ')'  &&  rawTerm[i+1] == LET_NOT_OP)  ||  (rawTerm[i-1] == ')'  &&  isLetterOrBool(rawTerm[i+1]))))
					{
						printf("%s: error: parsing error near character %ld (\"%c%c%c\" is invalid)\n", appName.c_str(), i, rawTerm[i-1], rawTerm[i], rawTerm[i+1]);
						return false;
					}
				}
			}

			//Check NOT correctness (only "!A" and "!(" are correct, but not "A!B")
			for(unsigned long i=0; i<rawTerm.size(); i++)
			{
				if(rawTerm[i] == LET_NOT_OP)
				{
					if(i == rawTerm.size()-1)
					{
						printf("%s: error: parsing error near character %ld (\"%c\" is invalid)\n", appName.c_str(), i, rawTerm[i]);
						return false;
					}
					if(i == 0)
					{
						if(!(isLetterOrBool(rawTerm[i+1])  ||  rawTerm[i+1] == '('))
						{
							printf("%s: error: parsing error near character %ld (\"%c%c\" is invalid)\n", appName.c_str(), i, rawTerm[i], rawTerm[i+1]);
							return false;
						}
					}
					else
					{
						if(!(isLetterOrBool(rawTerm[i+1])  ||  rawTerm[i+1] == '('))
						{
							printf("%s: error: parsing error near character %ld (\"%c%c\" is invalid)\n", appName.c_str(), i, rawTerm[i], rawTerm[i+1]);
							return false;
						}
						if(!(is2OperandOp(rawTerm[i-1])  ||  rawTerm[i-1] == '('))
						{
							printf("%s: error: parsing error near character %ld (\"%c%c\" is invalid)\n", appName.c_str(), i, rawTerm[i-1], rawTerm[i]);
							return false;
						}
					}
				}
			}

			//Get variables
			for(unsigned long i=0; i<rawTerm.size(); i++)
			{
				if(isLetter(rawTerm[i])  &&  find(variables.begin(), variables.end(), rawTerm[i]) == variables.end())
					variables.push_back(rawTerm[i]);
			}
			/*if(variables.size() == 0)
			{
				printf("%s: error: no variables recognized\n", appName.c_str());
				return false;
			}*/

			return true;
		}

		int evaluate(vector<bool> values)
		{
			string filledTerm = rawTerm;

			if(values.size() != variables.size())
			{
				printf("%s: error: unequal number of recognized (%lu) and provided (%lu) variables\n", appName.c_str(), variables.size(), values.size());
				return -1;
			}

			for(unsigned long i=0; i<values.size(); i++)
				replace_all(filledTerm, lexical_cast<string>(variables[i]), lexical_cast<string>(values[i]));

			//Continue: solve most inner bracket (not->and/or)
			while(filledTerm.size() != 1)
			{
				if(debug)
					printf("Step 1: %s\n", filledTerm.c_str());

				solveMostInnerBracket(filledTerm);
				if(debug)
					printf("Step 2: %s\n", filledTerm.c_str());

				stripBrackets(filledTerm);
				if(debug)
				{
					printf("Step 3: %s\n\n", filledTerm.c_str());
					//getch();
				}

			}

			return filledTerm[0]-'0';
		}

		vector<char> *getVariables()
		{
			return &variables;
		}
};

int main(int argc, char **argv)
{
	LET_TERM		term;
	string			rawTerm;
	vector<bool>	values;
	vector<bool>	results;

	//Name of executable is first provided argument
	appName = argv[0];

	//If no other arguments are provided, output usage information
	if(argc == 1)
	{
		printf("Usage: %s [term]\n", appName.c_str());
		exit(0);
	}

	//Create raw term by reading second argument
	rawTerm = argv[1];

	//If third argument is "TRUE", print debug information
	if(argc >= 3  &&  (string)argv[2] == "TRUE")
		debug = true;

	//Create and parse term
	term.set(rawTerm);
	if(!term.parse())
	{
		printf("%s: error: parsing failed\n", appName.c_str());
		exit(1);
	}

	//Print table header
	printf("\n");
	for(unsigned long i=0; i<term.getVariables()->size(); i++)
		printf(" %c |", term.getVariables()->at(i));
	printf("\n");
	for(unsigned long i=0; i<=term.getVariables()->size(); i++)
	{
		printf("---");
		if(i != term.getVariables()->size())
			printf("+");
	}
	printf("\n");

	//Evaluate and print all true-false-combinations of all variables
	for(unsigned long i=0; i<(unsigned long)1<<term.getVariables()->size(); i++)
	{
		values.clear();
		for(unsigned long j=0; j<term.getVariables()->size(); j++)
			values.push_back((bool)((i&(1<<j)) >> j));
		reverse(values.begin(), values.end());

		results.push_back(term.evaluate(values));
		if(results.back() == -1)
		{
			printf("%s: error: evaluation failed\n", appName.c_str());
			exit(1);
		}

		for(unsigned long j=0; j<term.getVariables()->size(); j++)
			printf(" %d |", (int)values[j]);
		printf(" %d\n", (int)results.back());
	}

	printf("\n% 3.2f%% true\n", (float)count(results.begin(), results.end(), true)/(float)results.size()*100.0f);

	return 0;
}


#include "../../include/waveO2/observer.hpp"
#include "../../include/waveO2/utilities.hpp"
#include "../../include/mymfem/utilities.hpp"

#include <fstream>
#include <iostream>
#include <Eigen/Core>

#include <filesystem>
namespace fs = std::filesystem;

//! Constructor
WaveO2Observer
:: WaveO2Observer (const nlohmann::json& config, int lx)
    : BaseObserver (config, lx)
{
    std::string problem_type = config["problem_type"];

    m_bool_error = false;
    if (config.contains("eval_error")) {
        m_bool_error = config["eval_error"];
    }

    std::string base_out_dir = "../output";
    if (config.contains("base_out_dir")) {
        base_out_dir = config["base_out_dir"];
    }

    std::string sub_out_dir = "wave_"+problem_type;
    if (config.contains("sub_out_dir")) {
        sub_out_dir = config["sub_out_dir"];
    }

    m_output_dir = base_out_dir+"/"+sub_out_dir+"/lx"
            +std::to_string(lx)+"/";
    m_solName_suffix = "_lx"+std::to_string(lx);

    if (m_bool_dumpOut) {
        fs::create_directories(m_output_dir);
    }
}

//! Sets the exact solution coefficients
//! and auxiliary variables needed
void WaveO2Observer
:: set (std::shared_ptr<WaveO2TestCases>& testCase,
        std::shared_ptr<WaveO2XtFEM>& discr)
{
    m_testCase = testCase;
    m_discr = discr;

    m_xndim = discr->get_xMesh()->Dimension();
    m_tVspace = m_discr->get_tFespace();
    m_xVspace = m_discr->get_xFespaces();

    if (m_bool_error)
    {
        m_uECoeff = std::make_unique
                <WaveO2ExactSolutionCoeff>(m_testCase);
        m_dudtECoeff = std::make_unique
                <WaveO2ExactGradtSolutionCoeff>(m_testCase);
        m_dudxECoeff = std::make_unique
                <WaveO2ExactGradxSolutionCoeff>(m_testCase);

        m_uE = std::make_unique<GridFunction>(m_xVspace);
        m_dudtE = std::make_unique<GridFunction>(m_xVspace);
    }
}

//! Computes H1 error for solution
std::tuple <double, double, double, double> WaveO2Observer
:: eval_xH1Error (std::shared_ptr<GridFunction>& u,
                  double t) const
{
    double erruL2=0, uEL2=0;
    double erruH1=0, uEH1=0;

    ConstantCoefficient zero(0);
    ConstantCoefficient one(1.0);
    VectorFunctionCoefficient zeroVec(m_xndim, zeroFn);

    // L2 error in solution
    m_uECoeff->SetTime(t);
    m_uE->ProjectCoefficient(*m_uECoeff);
    uEL2 = m_uE->ComputeL2Error(zero);
    erruL2 = u->ComputeL2Error(*m_uECoeff);

    // H1 error in solution
    m_dudxECoeff->SetTime(t);
    uEH1 = m_uE->ComputeH1Error(&zero, &zeroVec, &one,
                                1.0, 1);
    erruH1 = u->ComputeH1Error(m_uECoeff.get(),
                               m_dudxECoeff.get(),
                               &one, 1.0, 1);

    return {std::move(erruL2), std::move(uEL2),
                std::move(erruH1), std::move(uEH1)};
}

// End of file


//  generate_n test of min_rand  ---------------------------------------------//

//  (C) Copyright Beman Dawes 1999. Permission to copy, use, modify, sell and
//  distribute this software is granted provided this copyright notice appears
//  in all copies. This software is provided "as is" without express or implied
//  warranty, and with no claim as to its suitability for any purpose.

//  Revision History
//   9 Jan 00 Correct header name (Andreas Scherer)
//  27 May 99 Initial version

#include <boost/min_rand.hpp>
#include <algorithm>
#include <iterator>
#include <vector>
#include <iostream>
using boost::min_rand;
using namespace std;

int main() {
  vector<long> v;
  generate_n( inserter(v, v.begin()), 10, min_rand() );
  assert( v.size() == 10 );
  copy( v.begin(), v.end(), ostream_iterator<long>(cout, "\n") );
  return 0;
  } // main


#include <iostream>
#include <boost/accumulators/statistics/weighted_sum.hpp>
#include <boost/accumulators/statistics/sum.hpp>
#include <boost/accumulators/accumulators.hpp>

using namespace boost::accumulators;

int main()
{
	//accumulator_set<int, features<tag::weight_sum> , int> acc;
	accumulator_set<int, features<tag::sum> , int> acc;

	acc(1, weight = 2);acc(2, weight = 4);
	acc(3, weight = 4);acc(5, weight = 6);

	//std::cout<< weight_sum(acc) << std::endl;
	std::cout<< sum(acc) << std::endl;
}


#pragma once

#include <geogram/mesh/mesh.h>
#include <Eigen/Dense>
#include <vector>

namespace polyfem
{

// Compute a graph (V,E) where V are the edges of the input quad mesh, and E
// connects edges from opposite sides of the input quads.
//
// @param[in]  Q               { #Q x 4 input quads }
// @param[out] edge_index      { Map (f, lv) -> edge index for edge (lv, lv+1) }
// @param[out] adj             { Adjacency graph }
// @param[out] pairs_of_edges  { List of mesh edges, corresponding to the
//                             vertices of the output graph }
// @param[out] pairs_of_quads  { List of adjacent quads }
// @param[out] quad_index      { Map (f, lv) -> index of the quad across edge
//                             (lv, lv+1) }
//
void edge_adjacency_graph(const Eigen::MatrixXi &Q, Eigen::MatrixXi &edge_index,
	std::vector<std::vector<int>> &adj,
	std::vector<std::pair<int, int>> *pairs_of_edges = nullptr,
	std::vector<std::pair<int, int>> *pairs_of_quads = nullptr,
	Eigen::MatrixXi *quad_index = nullptr);

typedef std::function<void(const Eigen::MatrixXd &, Eigen::MatrixXd &, int)> EvalParametersFunc;
typedef std::function<std::tuple<int, int, bool>(int, int)> GetAdjacentLocalEdge;

// Instantiate a periodic 2D pattern (triangle-mesh) on a given quad mesh
//
// @param[in]  IV        { #IV x 3 input quad mesh vertices }
// @param[in]  IF        { #IF x 4 input quad mesh facets }
// @param[in]  PV        { #PV x (2|3) input pattern vertices in [0,1]^2 }
// @param[in]  PF        { #PF x (3|4) input pattern facets }
// @param[out] OV        { #OV x 3 output mesh vertices }
// @param[out] OF        { #OF x 3 output mesh facets }
// @param[in]  SF        { #OV x 1 matrix of input source quad index, filled if pointer is non-zero }
// @param[in]  evalFunc  { Evaluate the uv param of the pattern into a 2d or 3d
//                       position }
//
// @return     { Return true in case of success. }
//
bool instantiate_pattern(
	const Eigen::MatrixXd &IV, const Eigen::MatrixXi &IF,
	const Eigen::MatrixXd &PV, const Eigen::MatrixXi &PF,
	Eigen::MatrixXd &OV, Eigen::MatrixXi &OF,
	Eigen::VectorXi *SF = nullptr,
	EvalParametersFunc evalFunc = nullptr,
	GetAdjacentLocalEdge getAdjLocalEdge = nullptr);

//
// Refine a quad-mesh by splitting each quad into 4 quads.
//
// @param[in]  IV    { #IV x 3 input quad mesh vertices }
// @param[in]  IF    { #IF x 4 input quad mesh facets }
// @param[out] OV    { #OV x 3 output mesh vertices }
// @param[out] OF    { #OF x 4 output mesh facets }
//
void refine_quad_mesh(const Eigen::MatrixXd &IV, const Eigen::MatrixXi &IF,
	Eigen::MatrixXd &OV, Eigen::MatrixXi &OF);

namespace Polygons {

	typedef std::function<void(Eigen::MatrixXd &IV, Eigen::MatrixXd &OV, std::vector<std::vector<int>> &OF)> SplitFunction;

	///
	/// Split a polygon using polar refinement. The input polygon must be
	/// star-shaped. A one-ring of quads are create on the outer ring of the
	/// polygon, while at the center a new polygonal facet is created around the
	/// barycenter of the kernel polygon. If the interpolation parameter t is
	/// equal to 0, the central polygon is collapsed into a single vertex, and
	/// facets on the ring become triangles.
	///
	/// @param[in]  IV    { #IV x (2|3) of vertex positions around the polygon }
	/// @param[out] OV    { #OF v (2|3) output vertex positions }
	/// @param[out] OF    { list of output polygonal face indices }
	/// @param[in]  t     { Interpolation parameter to place the new vertices on
	///                   the edge from the barycenter to the outer polygon
	///                   vertices (0 being at the center, 1 being at the
	///                   boundary).t should be >= 0.0 and < 1.0 }
	///
	void polar_split(const Eigen::MatrixXd &IV, Eigen::MatrixXd &OV, std::vector<std::vector<int>> &OF, double t = 0.5);

	/// Helper function
	inline SplitFunction polar_split_func(double t) {
		return [t](const Eigen::MatrixXd &IV, Eigen::MatrixXd &OV, std::vector<std::vector<int>> &OF) { polar_split(IV, OV, OF, t); };
	}

	///
	/// Split a polygon using polar refinement. A single vertex is inserted at the
	/// barycenter of the polygon kernel (the polygon needs to be star-shaped).
	/// Contrary to the `polar_split` function, this function creates quads instead
	/// of triangles. The edges of the original polygon are also assumed to be split
	/// already, so the input polygon must have an even number of vertices. Radial
	/// edges will be inserted every 2 vertices, starting from vertex #1.
	///
	/// @param[in]  IV    { #IV x (2|3) of vertex positions around the polygon  }
	/// @param      OV    { #OF v (2|3) output vertex positions }
	/// @param      OF    { list of output polygonal face indices }
	///
	void catmul_clark_split(const Eigen::MatrixXd &IV, Eigen::MatrixXd &OV, std::vector<std::vector<int>> &OF);

	inline SplitFunction catmul_clark_split_func() { return SplitFunction(catmul_clark_split); }

	///
	/// @brief      Don't split polygons
	///
	void no_split(const Eigen::MatrixXd &IV, Eigen::MatrixXd &OV, std::vector<std::vector<int>> &OF);

	inline SplitFunction no_split_func() { return SplitFunction(no_split); }
}

///
/// Refine a polygonal mesh. Quads and triangles are split into quads. If
/// `refine_polygons` is set to `true`, then polygonal facets are also split
/// into a layer of padding quads, and a new polygon is created around the
/// barycenter
///
/// @param[in]  M_in        { Surface mesh to subdivide }
/// @param[out] M_out       { Refined mesh }
/// @param[in]  split_func  { Functional used to split the new polygon interiors
///                         (boundary has already been split) }
///
void refine_polygonal_mesh(const GEO::Mesh &M_in, GEO::Mesh &M_out, Polygons::SplitFunction split_func);

///
/// Refine a triangle mesh. Each input triangle is split into 4 new triangles
///
/// @param[in]  M_in   { Input surface mesh }
/// @param[out] M_out  { Output surface mesh }
///
void refine_triangle_mesh(const GEO::Mesh &M_in, GEO::Mesh &M_out);

// Kept for compatibility
[[deprecated]] inline void refine_polygonal_mesh(const GEO::Mesh &M_in, GEO::Mesh &M_out, bool refine_polygons = false, double t = 0.5) {
	if (refine_polygons == false) {
		refine_polygonal_mesh(M_in, M_out, Polygons::no_split_func());
	} else {
		refine_polygonal_mesh(M_in, M_out, Polygons::polar_split_func(t));
	}
}

} // namespace polyfem


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_CLOSEST_POINTS_CROSS_TRACK_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_CLOSEST_POINTS_CROSS_TRACK_HPP

#include <algorithm>

#include <boost/tuple/tuple.hpp>
#include <boost/algorithm/minmax.hpp>
#include <boost/config.hpp>
#include <boost/concept_check.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_promotion.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/concepts/distance_concept.hpp>
#include <boost/geometry/strategies/spherical/distance_cross_track.hpp>
#include <boost/geometry/strategies/spherical/distance_haversine.hpp>
#include <boost/geometry/strategies/spherical/point_in_point.hpp>
#include <boost/geometry/strategies/geographic/azimuth.hpp>
#include <boost/geometry/strategies/geographic/distance.hpp>
#include <boost/geometry/strategies/geographic/distance_cross_track.hpp>
#include <boost/geometry/strategies/geographic/parameters.hpp>
#include <boost/geometry/strategies/geographic/intersection.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>
#include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>

#include <boost/geometry/formulas/mean_radius.hpp>
#include <boost/geometry/formulas/result_direct.hpp>
#include <boost/geometry/formulas/spherical.hpp>
#include <boost/geometry/formulas/vincenty_direct.hpp>

#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
#include <boost/geometry/io/dsv/write.hpp>
#endif

#ifndef BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS
#define BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS 100
#endif

#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
#include <iostream>
#endif

namespace boost { namespace geometry
{

namespace strategy { namespace closest_points
{

template
<
    typename FormulaPolicy = geometry::strategy::andoyer,
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
class geographic_cross_track
    : public distance::detail::geographic_cross_track
        <
            FormulaPolicy,
            Spheroid,
            CalculationType,
            false,
            true
        >
{
    using base_t = distance::detail::geographic_cross_track
        <
            FormulaPolicy,
            Spheroid,
            CalculationType,
            false,
            true
        >;

    template <typename Point, typename PointOfSegment>
    struct calculation_type
        : promote_floating_point
          <
              typename select_calculation_type
                  <
                      Point,
                      PointOfSegment,
                      CalculationType
                  >::type
          >
    {};

public :
    explicit geographic_cross_track(Spheroid const& spheroid = Spheroid())
        : base_t(spheroid)
        {}

        template <typename Point, typename PointOfSegment>
        auto apply(Point const& p,
                   PointOfSegment const& sp1,
                   PointOfSegment const& sp2) const
        {
            auto result = base_t::apply(get_as_radian<0>(sp1), get_as_radian<1>(sp1),
                                        get_as_radian<0>(sp2), get_as_radian<1>(sp2),
                                        get_as_radian<0>(p), get_as_radian<1>(p),
                                        base_t::m_spheroid);

            model::point
                <
                    typename calculation_type<Point, PointOfSegment>::type,
                    dimension<PointOfSegment>::value,
                    typename coordinate_system<PointOfSegment>::type
                > cp;

            geometry::set_from_radian<0>(cp, result.lon);
            geometry::set_from_radian<1>(cp, result.lat);

            return cp;
        }
};

}} // namespace strategy::closest_points

}} // namespace boost::geometry
#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_CLOSEST_POINTS_CROSS_TRACK_HPP


#include <iostream>
#include <vector>
#include <random>
#include <cmath>
#include <cstdlib>
#include <fstream>
#include <string>
#include <assert.h>
#include <algorithm>
#include <unordered_map>
#include <utility>

#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>

std::mt19937 g1(1); //Random number generator, mersenne twister
std::uniform_real_distribution<double> RNG(0.0,1.0);

#include "Parameters.h"
#include "Files.h"
#include "DeficitType.h"
#include "Node.h"
#include "DataFile.h"
#include "OutputDataFile.h"
#include "SaveData.h"
#include "2Dvector.h"
#include "RandomNetwork.h"
#include "tree.h"
#include "GillespieTree.h"
#include "UpdateNode.h"
#include "MortalityNodes.h"
#include "ScaleFreeNetworkTree.h"
#include "MortalityRules.h"
#include "OutputNetwork.h"
//#include "AssortativeMixing.h"
#include "SmallWorld.h"
#include "SetUpNetwork.h"

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

    using namespace Parameters; //Using the parameters from cmd line
    SetParameters(argc, argv); //Set initial parameters from command line
    SetSimulate();
    std::cout << "Parameters Loaded" << std::endl;

    //Network
    const int OriginalN = N; //some network types remove unconnected nodes, lowering N. Original N will not change
    std::vector<Node> Network;
    std::vector<int> MortalityNodes(nd);

    //Hold all death ages found
    std::vector<double> DeathAges; DeathAges.reserve(Parameters::Number);

    //Holds the time of change, id of the node, and state of the node d
    std::vector<DeficitVal> DeficitsValues;
    if (SimulateVar::MortalityOnly_bool == false)
	DeficitsValues.reserve(N * 1000);

    //Data file to save intermediate Data
    OutputDataFile File(OriginalN);
    if (SimulateVar::MortalityOnly_bool == false) File.Open();

    //set up network, if single topology set up network before the simulation
    if(Topology == "Single") {
		g1.seed(Parameters::SingleSeed);
		SetUpNetwork(Network, MortalityNodes, OriginalN);
		std::cout << "Single Network Created" << std::endl;
    }

    //loop through runs to average data
    for(int run = 1; run <= Number; run++) {

	if(run == 1) g1.seed(RealSeed); // Set current seed

       	//Set up network for every seed if not single topology
	if(Topology != "Single")
	    SetUpNetwork(Network, MortalityNodes, OriginalN);

	EventTree tree(N, 1.0);
	double TotalRate = N; //sum of rates
	double Time = 0; //current Time (unscaled age)

	//evolve in time until Mortality occurs
	while(true) {

	    int Index = FindRate(tree, TotalRate); //Find rate to be performed
	    UpdateNode(Network[Index], Time, TotalRate, DeficitsValues, run); //perform rate and increase time
	    UpdateLocalFrailty(Network, Network[Index]); //update the local frailty after the node is modified
	    CalculateRates(Network, Network[Index], tree, Index, TotalRate); //calculate new rates for the node and its neighbours

	    //evaluate mortality
	    if(Mortality(Network, MortalityNodes) == 1) break;

	}

	//record death age data
	DeathAges.push_back(Time);

	//Reset Rates and fraility if it is single topology
	if(Topology == "Single")
	    for(auto &x: Network) x.Reset();

	//Save interediate Data every 1000 individuals
	if(SimulateVar::MortalityOnly_bool == false && run%1000 == 0 ) {
	    File.SaveData(DeficitsValues); //possibly change the 1000 (depends on memory restrictions)

	}

	if (run%1000 == 0)
	    std::cout << "Run " << run << std::endl;

    }

    std::cout << "Outputing final data" << std::endl;

    //Save and output final bit of raw Data
    if (SimulateVar::MortalityOnly_bool == false) {
		File.SaveData(DeficitsValues);
		File.FlushDataFile(DeficitsValues);
    }

    OutputDeathAges(DeathAges, OriginalN);
    std::cout << "Finished" << std::endl;

}


//
//  multivector_AWT.hpp
//
//  R. Andreev, 2014-2015
//

#ifndef round1_multivector_AWT_hpp
#define round1_multivector_AWT_hpp

#include <boost/numeric/ublas/vector_of_vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector.hpp>

//#include <algorithm>
#include <numeric>
#include <vector>

#include "stopwatch.hpp"
#include "multivector.hpp"

namespace mypara
{
	namespace ublas = boost::numeric::ublas;

	struct AWT
	{
	public:
		typedef ublas::compressed_matrix<double> matrix;
		typedef ublas::vector<double> vector;
	public:
		class Filter
		{
		public:
			typedef ublas::compressed_matrix<double, ublas::column_major> sparse_matrix;
		public:
			const bool wave;
			const unsigned int n, N;
			const sparse_matrix C, F;
			const sparse_matrix Ct, Ft;
			mypara::Jplan jplan_C, jplan_Ct;
			mypara::Jplan jplan_F, jplan_Ft;
		public:
			Filter(unsigned int n, unsigned int N, const sparse_matrix& C, const sparse_matrix& F)
			:
			wave(n!=0), n(n), N(N),
			C(C), F(F),
			Ct(ublas::trans(C)), Ft(ublas::trans(F)),
			jplan_C(), jplan_Ct(),
			jplan_F(), jplan_Ft()
			{
				assert(C.size1() == n);
				assert(C.size2() == N);
				assert(F.size1() == N-n);
				assert(F.size2() == N);
			}
		};
	private:
		// Should be hidden
		typedef std::vector<Filter> FilterBank;
		FilterBank filterbank;
	public:
		// These should be hidden
		vector gamma;
		unsigned int n;
		unsigned int nu;
		double eta;
	public:
		AWT() : n(0), gamma(0), nu(0), eta(0), filterbank() { }
	public:
		AWT(const matrix& A, const matrix& M, unsigned int nu = 1, double eta = 1.9)
		:
		gamma(0), n((unsigned int)(A.size1())), nu(nu), eta(eta), filterbank()
		// Pre: A and M are square matrices of equal size
		{
			stopwatch::Time time(stopwatch::watches, "mypara::AWT::AWT");

			assert((A.size1() == n) && (A.size2() == n));
			assert((M.size1() == n) && (M.size2() == n));
			assert(eta > 0);

			// Will hold the square roots of generalized "eigenvalues"
			gamma = vector(n);
			gamma.clear();

			typedef ublas::compressed_matrix<double, ublas::row_major> S_type;
			typedef ublas::compressed_matrix<double, ublas::column_major> T_type;

			// Note:
			// prod(S_type, T_type) is more efficient than prod(T_type, S_type)

			typedef ublas::compressed_matrix<double> acmc_type;
			acmc_type mc = M, ac = A;
			unsigned int m = n;
			// Invariant: m is size1 and size2 of mc and ac

			// Iterate over levels
			while (m >= 3)
			{
				const unsigned int m0 = m;
				S_type M = mc;
				S_type A = ac;

				// Step 1. Identify fine hats

				typedef ublas::compressed_vector<double> compressed_vector;
				typedef std::vector<compressed_vector> P_type;
				P_type P;
				{
					//stopwatch::Time time(stopwatch::watches, "spacetime::AWT::AWT().P");

					for (unsigned int i = 0; i != m0; ++i) {
						P.push_back(compressed_vector(m0));
						(P.back())[i] = 1;
					}
				}

				std::vector<unsigned int> I(m0);
				{
					for (unsigned int i = 0; i != m0; ++i) I[i] = i;
				}

				double e0 = 0;
				while (m >= 3)
				{
					unsigned int j; // index of most energetic hat
					double e1; // and its energy
					{
						//stopwatch::Time time(stopwatch::watches, "spacetime::AWT::AWT().e1");
						std::vector<double> E1(m);
						{
							ublas::range ALL(0, m0);
							ublas::matrix_vector_range<acmc_type> dac(ac, ALL, ALL);
							ublas::matrix_vector_range<acmc_type> dmc(mc, ALL, ALL);
							for (unsigned i = 0; i != m; ++i)
								E1[i] = dac(I[i]) / dmc(I[i]);
							E1.front() = E1.back() = 0;
						}
						auto i = std::max_element(E1.begin(), E1.end());
						e1 = *i;
						j = (unsigned int)(std::distance(E1.begin(), i));
						assert(E1[j] == e1);
					}
					assert(e1 > 0);

					if (e1 <= e0 / eta) break;
					if (e0 == 0) e0 = e1;

					double p = -ac(I[j-1], I[j]) / ac(I[j], I[j]);
					double q = -ac(I[j+1], I[j]) / ac(I[j], I[j]);

					// Manipulate ac
					{
						//stopwatch::Time time(stopwatch::watches, "spacetime::AWT::AWT().prod1");

						row(ac, I[j-1]) += p * row(ac, I[j]);
						row(ac, I[j+1]) += q * row(ac, I[j]);

						I.erase(I.begin() + j);

						// New effective size of mc and ac
						m--;
					}

					// Pre-Manipulate T by means of P
					{
						//stopwatch::Time time(stopwatch::watches, "spacetime::AWT::AWT().mani");
						P[j-1] += p * P[j];
						P[j+1] += q * P[j];
						P.push_back(P[j]);
						P.erase(P.begin() + j);
					}
				} // Finished identifying level

				S_type SC(m, m0), SF(m0-m, m0);
				{
					//stopwatch::Time time(stopwatch::watches, "spacetime::AWT::AWT().perm");

					for (unsigned int i = 0; i != m; ++i) row(SC, i) = P[i];
					for (unsigned int i = m; i != m0; ++i) row(SF, i-m) = P[i];
				}

				T_type MSC;
				{
					//stopwatch::Time time(stopwatch::watches, "spacetime::AWT::AWT().mcac");
					T_type tSC(ublas::trans(SC));
					MSC = prod(M, tSC);
					mc = prod(SC, MSC);
					ac = prod(SC, T_type(prod(A, tSC)));
				}

				// Step 2. Orthogonalize level
				{
					T_type DSC(SC);
					{
						//stopwatch::Time time(stopwatch::watches, "spacetime::AWT::AWT().mass");

						for (unsigned int i = 0; i != m; ++i)
						{
							ublas::matrix_row<acmc_type> r(mc, i);
							row(DSC, i) *= (1. / std::accumulate(r.begin(), r.end(), 0.));
						}
					}

					ublas::range IF(m, m0), ALL(0, m0);
					for (unsigned int k = 0; k != nu; ++k)
					{
						//stopwatch::Time time(stopwatch::watches, "spacetime::AWT::AWT().proj2");
						assert(SF.size2() == MSC.size1());
						assert(MSC.size2() == DSC.size1());
						assert(SF.size2() == DSC.size2());
						SF -= prod(S_type(prod(SF, MSC)), DSC);
					}
				}

				// Normalize wavelets
				{
					//stopwatch::Time time(stopwatch::watches, "spacetime::AWT::AWT().norm");
					for (unsigned int i = 0; i != m0-m; ++i)
					{
						auto c = row(SF, i);
						c *= (1. / sqrt(ublas::inner_prod(c, prod(M, ublas::trans(c)))));
						double d = ublas::inner_prod(c, prod(A, ublas::trans(c)));
						gamma[i + m] = sqrt(std::max(0., d));
					}
				}

				// Update filterbank
				{
					Filter filter(m, m0, SC, SF);
					filterbank.push_back(filter);
				}
			} // end while(m >= 3)

			{
				// Normalize ground hat functions
				S_type SF = ublas::identity_matrix<>(m);;
				{
					for (unsigned int i = 0; i != m; ++i)
					{
						auto c = row(SF, i);
						c *= (1. / sqrt(mc(i, i)));
						double d = ublas::inner_prod(c, prod(ac, ublas::trans(c)));
						gamma[i] = sqrt(std::max(0., d));
					}
				}

				// Update filterbank
				{
					Filter filter(0, m, matrix(0, m), SF);
					filterbank.push_back(filter);
				}
			}
		}
	public:
		AWT& operator=(const AWT&) = default;
	public:
		multivector uT(const multivector& u) { return this->uT(u, filterbank.begin()); }
		multivector uTt(const multivector& u) { return this->uTt(u, filterbank.begin()); }
	private:
		multivector uT(const multivector& u, const FilterBank::iterator i);
		multivector uTt(const multivector& u, const FilterBank::iterator i);
	};

	//

	multivector
	AWT::uT(const multivector& u, const AWT::FilterBank::iterator i)
	// Note (RA, 20160509):
	// This routine is called uT because
	// morally it computes "T * column vector",
	// (but here we have row vectors by
	//  the convention of putting temporal
	//  degrees of freedom along the j-dimension).
	// This is why we have here Ft and Ct (not F and C).
	{
		stopwatch::Time time(stopwatch::watches, "mypara::AWT::uT");

		if ((i->n) == 0)
		{
            multivector v = u;
//            mypara::prod(u, ublas::trans(i->F), i->jplan_Ft, v);
            mypara::prod(u, i->Ft, v);
			return v;
		}

		multivector uc(u.getcm()), uf(u.getcm());
        u.split(uc, uf, i->n);

		// mypara::prod(u, ublas::trans(i->F), i->jplan_Ft(mypara::Jplan::Strategy::jreverse), uf);
        mypara::prod(u, i->Ft, uf);

		// mypara::prod(u, ublas::trans(i->C), i->jplan_Ct, uc);
        mypara::prod(u, i->Ct, uc);

        if (uc.shrink_communicator()) {
			uc = AWT::uT(uc, i + 1);
        }

		multivector v = u; v.clear();
        v.merge(uc, uf);

		return v;
	}

	multivector
	AWT::uTt(const multivector& u, const AWT::FilterBank::iterator i)
	{
		stopwatch::Time time(stopwatch::watches, "mypara::AWT::uTt");

		if ((i->n) == 0)
		{
			multivector v = u;
			// mypara::prod(u, i->F, i->jplan_F, v);
            mypara::prod(u, i->F, v);
			return v;
		}

		multivector uc(u.getcm()), uf(u.getcm());
		{
			stopwatch::Time time(stopwatch::watches, "mypara::AWT::uTt.split");

			u.split(uc, uf, i->n);
		}

		multivector v2 = u; v2.clear();
		{
			stopwatch::Time time(stopwatch::watches, "mypara::AWT::uTt.prod1");

			// mypara::prod(uf, i->F, i->jplan_F, v2);
			mypara::prod(uf, i->F, v2);
		}

		mpi::communicator ucomm = uc.getcm();
		if (uc.shrink_communicator()) {
			stopwatch::Time time(stopwatch::watches, "mypara::AWT::uTt.rec");

            uc = uTt(uc, i + 1);
        }
        uc.embrace_communicator(ucomm);

		multivector v1 = u; v1.clear();
		{
			stopwatch::Time time(stopwatch::watches, "mypara::AWT::uTt.prod2");

			// mypara::prod(uc, i->C, i->jplan_C, v1);
			mypara::prod(uc, i->C, v1);
		}

		v1 += v2;

		return v1;
	}
}

#endif


// Boost.Geometry

// Copyright (c) 2020, Oracle and/or its affiliates.

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

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_RELATE_CARTESIAN_HPP
#define BOOST_GEOMETRY_STRATEGIES_RELATE_CARTESIAN_HPP

// TEMP - move to strategy
#include <boost/geometry/strategies/agnostic/point_in_box_by_side.hpp>
#include <boost/geometry/strategies/cartesian/intersection.hpp>
#include <boost/geometry/strategies/cartesian/box_in_box.hpp>
#include <boost/geometry/strategies/cartesian/point_in_point.hpp>
#include <boost/geometry/strategies/cartesian/point_in_poly_crossings_multiply.hpp>
#include <boost/geometry/strategies/cartesian/point_in_poly_franklin.hpp>
#include <boost/geometry/strategies/cartesian/point_in_poly_winding.hpp>
#include <boost/geometry/strategies/cartesian/disjoint_box_box.hpp>

#include <boost/geometry/strategies/envelope/cartesian.hpp>
#include <boost/geometry/strategies/relate/services.hpp>
#include <boost/geometry/strategies/detail.hpp>

#include <boost/geometry/strategy/cartesian/area.hpp>
#include <boost/geometry/strategy/cartesian/side_robust.hpp>
#include <boost/geometry/strategy/cartesian/side_by_triangle.hpp>
#include <boost/geometry/strategy/cartesian/area_box.hpp>

#include <boost/geometry/util/type_traits.hpp>

namespace boost { namespace geometry
{

namespace strategies { namespace relate
{

template <typename CalculationType = void>
class cartesian
    : public strategies::envelope::cartesian<CalculationType>
{
public:
    //area

    template <typename Geometry>
    static auto area(Geometry const&,
                     std::enable_if_t<! util::is_box<Geometry>::value> * = nullptr)
    {
        return strategy::area::cartesian<CalculationType>();
    }

    template <typename Geometry>
    static auto area(Geometry const&,
                     std::enable_if_t<util::is_box<Geometry>::value> * = nullptr)
    {
        return strategy::area::cartesian_box<CalculationType>();
    }

    // covered_by

    template <typename Geometry1, typename Geometry2>
    static auto covered_by(Geometry1 const&, Geometry2 const&,
                           std::enable_if_t
                                <
                                    util::is_pointlike<Geometry1>::value
                                 && util::is_box<Geometry2>::value
                                > * = nullptr)
    {
        return strategy::covered_by::cartesian_point_box();
    }

    template <typename Geometry1, typename Geometry2>
    static auto covered_by(Geometry1 const&, Geometry2 const&,
                           std::enable_if_t
                            <
                                util::is_box<Geometry1>::value
                             && util::is_box<Geometry2>::value
                            > * = nullptr)
    {
        return strategy::covered_by::cartesian_box_box();
    }

    // disjoint

    template <typename Geometry1, typename Geometry2>
    static auto disjoint(Geometry1 const&, Geometry2 const&,
                         std::enable_if_t
                            <
                                util::is_box<Geometry1>::value
                             && util::is_box<Geometry2>::value
                            > * = nullptr)
    {
        return strategy::disjoint::cartesian_box_box();
    }

    template <typename Geometry1, typename Geometry2>
    static auto disjoint(Geometry1 const&, Geometry2 const&,
                         std::enable_if_t
                            <
                                util::is_segment<Geometry1>::value
                             && util::is_box<Geometry2>::value
                            > * = nullptr)
    {
        // NOTE: Inconsistent name.
        return strategy::disjoint::segment_box();
    }

    // relate

    template <typename Geometry1, typename Geometry2>
    static auto relate(Geometry1 const&, Geometry2 const&,
                       std::enable_if_t
                            <
                                util::is_pointlike<Geometry1>::value
                             && util::is_pointlike<Geometry2>::value
                            > * = nullptr)
    {
        return strategy::within::cartesian_point_point();
    }

    template <typename Geometry1, typename Geometry2>
    static auto relate(Geometry1 const&, Geometry2 const&,
                       std::enable_if_t
                            <
                                util::is_pointlike<Geometry1>::value
                             && ( util::is_linear<Geometry2>::value
                               || util::is_polygonal<Geometry2>::value )
                            > * = nullptr)
    {
        return strategy::within::cartesian_winding<void, void, CalculationType>();
    }

    // The problem is that this strategy is often used with non-geometry ranges.
    // So dispatching only by geometry categories is impossible.
    // In the past it was taking two segments, now it takes 3-point sub-ranges.
    // So dispatching by segments is impossible.
    // It could be dispatched by (linear || polygonal || non-geometry point range).
    // For now implement as 0-parameter, special case relate.

    //template <typename Geometry1, typename Geometry2>
    static auto relate(/*Geometry1 const&, Geometry2 const&,
                       std::enable_if_t
                            <
                                ( util::is_linear<Geometry1>::value
                               || util::is_polygonal<Geometry1>::value )
                             && ( util::is_linear<Geometry2>::value
                               || util::is_polygonal<Geometry2>::value )
                            > * = nullptr*/)
    {
        return strategy::intersection::cartesian_segments<CalculationType>();
    }

    // side

    static auto side()
    {
        return strategy::side::side_robust<CalculationType>();
    }

    // within

    template <typename Geometry1, typename Geometry2>
    static auto within(Geometry1 const&, Geometry2 const&,
                       std::enable_if_t
                            <
                                util::is_pointlike<Geometry1>::value
                                && util::is_box<Geometry2>::value
                            > * = nullptr)
    {
        return strategy::within::cartesian_point_box();
    }

    template <typename Geometry1, typename Geometry2>
    static auto within(Geometry1 const&, Geometry2 const&,
                       std::enable_if_t
                            <
                                util::is_box<Geometry1>::value
                             && util::is_box<Geometry2>::value
                            > * = nullptr)
    {
        return strategy::within::cartesian_box_box();
    }
};

namespace services
{

template <typename Geometry1, typename Geometry2>
struct default_strategy<Geometry1, Geometry2, cartesian_tag, cartesian_tag>
{
    using type = strategies::relate::cartesian<>;
};

template <>
struct strategy_converter<strategy::within::cartesian_point_point>
{
    static auto get(strategy::within::cartesian_point_point const& )
    {
        return strategies::relate::cartesian<>();
    }
};

template <>
struct strategy_converter<strategy::within::cartesian_point_box>
{
    static auto get(strategy::within::cartesian_point_box const&)
    {
        return strategies::relate::cartesian<>();
    }
};

template <>
struct strategy_converter<strategy::covered_by::cartesian_point_box>
{
    static auto get(strategy::covered_by::cartesian_point_box const&)
    {
        return strategies::relate::cartesian<>();
    }
};

template <>
struct strategy_converter<strategy::covered_by::cartesian_box_box>
{
    static auto get(strategy::covered_by::cartesian_box_box const&)
    {
        return strategies::relate::cartesian<>();
    }
};

template <>
struct strategy_converter<strategy::disjoint::cartesian_box_box>
{
    static auto get(strategy::disjoint::cartesian_box_box const&)
    {
        return strategies::relate::cartesian<>();
    }
};

template <>
struct strategy_converter<strategy::disjoint::segment_box>
{
    static auto get(strategy::disjoint::segment_box const&)
    {
        return strategies::relate::cartesian<>();
    }
};

template <>
struct strategy_converter<strategy::within::cartesian_box_box>
{
    static auto get(strategy::within::cartesian_box_box const&)
    {
        return strategies::relate::cartesian<>();
    }
};

template <typename P1, typename P2, typename CalculationType>
struct strategy_converter<strategy::within::cartesian_winding<P1, P2, CalculationType>>
{
    static auto get(strategy::within::cartesian_winding<P1, P2, CalculationType> const& )
    {
        return strategies::relate::cartesian<CalculationType>();
    }
};

template <typename CalculationType>
struct strategy_converter<strategy::intersection::cartesian_segments<CalculationType>>
{
    static auto get(strategy::intersection::cartesian_segments<CalculationType> const& )
    {
        return strategies::relate::cartesian<CalculationType>();
    }
};

template <typename CalculationType>
struct strategy_converter<strategy::within::cartesian_point_box_by_side<CalculationType>>
{
    struct altered_strategy
        : strategies::relate::cartesian<CalculationType>
    {
        template <typename Geometry1, typename Geometry2>
        static auto covered_by(Geometry1 const&, Geometry2 const&,
                               std::enable_if_t
                                    <
                                        util::is_pointlike<Geometry1>::value
                                     && util::is_box<Geometry2>::value
                                    > * = nullptr)
        {
            return strategy::covered_by::cartesian_point_box_by_side<CalculationType>();
        }

        template <typename Geometry1, typename Geometry2>
        static auto within(Geometry1 const&, Geometry2 const&,
                           std::enable_if_t
                                <
                                    util::is_pointlike<Geometry1>::value
                                    && util::is_box<Geometry2>::value
                                > * = nullptr)
        {
            return strategy::within::cartesian_point_box_by_side<CalculationType>();
        }
    };

    static auto get(strategy::covered_by::cartesian_point_box_by_side<CalculationType> const&)
    {
        return altered_strategy();
    }

    static auto get(strategy::within::cartesian_point_box_by_side<CalculationType> const&)
    {
        return altered_strategy();
    }
};

template <typename CalculationType>
struct strategy_converter<strategy::covered_by::cartesian_point_box_by_side<CalculationType>>
    : strategy_converter<strategy::within::cartesian_point_box_by_side<CalculationType>>
{};

template <typename P1, typename P2, typename CalculationType>
struct strategy_converter<strategy::within::franklin<P1, P2, CalculationType>>
{
    struct altered_strategy
        : strategies::relate::cartesian<CalculationType>
    {
        template <typename Geometry1, typename Geometry2>
        static auto relate(Geometry1 const&, Geometry2 const&,
                           std::enable_if_t
                                <
                                    util::is_pointlike<Geometry1>::value
                                 && ( util::is_linear<Geometry2>::value
                                   || util::is_polygonal<Geometry2>::value )
                                > * = nullptr)
        {
            return strategy::within::franklin<void, void, CalculationType>();
        }
    };

    static auto get(strategy::within::franklin<P1, P2, CalculationType> const&)
    {
        return altered_strategy();
    }
};

template <typename P1, typename P2, typename CalculationType>
struct strategy_converter<strategy::within::crossings_multiply<P1, P2, CalculationType>>
{
    struct altered_strategy
        : strategies::relate::cartesian<CalculationType>
    {
        template <typename Geometry1, typename Geometry2>
        static auto relate(Geometry1 const&, Geometry2 const&,
                           std::enable_if_t
                                <
                                    util::is_pointlike<Geometry1>::value
                                 && ( util::is_linear<Geometry2>::value
                                   || util::is_polygonal<Geometry2>::value )
                                > * = nullptr)
        {
            return strategy::within::crossings_multiply<void, void, CalculationType>();
        }
    };

    static auto get(strategy::within::crossings_multiply<P1, P2, CalculationType> const&)
    {
        return altered_strategy();
    }
};

// TEMP used in distance segment/box
template <typename CalculationType>
struct strategy_converter<strategy::side::side_by_triangle<CalculationType>>
{
    static auto get(strategy::side::side_by_triangle<CalculationType> const&)
    {
        return strategies::relate::cartesian<CalculationType>();
    }
};

template <typename CalculationType>
struct strategy_converter<strategy::side::side_robust<CalculationType>>
{
    static auto get(strategy::side::side_robust<CalculationType> const&)
    {
        return strategies::relate::cartesian<CalculationType>();
    }
};

} // namespace services

}} // namespace strategies::relate

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_RELATE_CARTESIAN_HPP


#include <tinyxml.h>
#include <Eigen/Core>
#include <map>
#include <vector>
#include <fstream>
#define LARGE_VALUE 1E6
#define REVOLUTE_JOINT_LIMIT 0.05
#define PROXIMAL_JOINT_LIMIT 1.0
namespace Eigen
{
	typedef Matrix<double,1,1> Vector1d;
};
struct UserConstant
{
	UserConstant(std::string parent,std::string joint_type,double mass,const Eigen::VectorXd& joint_lower_limit,const Eigen::VectorXd& joint_upper_limit,const Eigen::Vector3d& axis = Eigen::Vector3d::Zero(),std::string bvh_map="None",std::string obj_map="None",bool con = false)
		:mParent(parent),mJointType(joint_type),mMass(mass),mJointLowerLimit(joint_lower_limit),mJointUpperLimit(joint_upper_limit),mAxis(axis),mBVHMap(bvh_map),mOBJMap(obj_map),collision_on(con)
	{

	}
	std::string mParent;
	std::string mJointType;
	double mMass;
	std::string mBVHMap;
	std::string mOBJMap;
	Eigen::VectorXd mJointLowerLimit;
	Eigen::VectorXd mJointUpperLimit;
	Eigen::Vector3d mAxis;
	bool collision_on;
};
struct MayaConstant
{
	MayaConstant(std::string name, const Eigen::Vector3d& size,const Eigen::VectorXd& R,const Eigen::Vector3d& bt,const Eigen::Vector3d& jt)
		:mName(name),mSize(size),mBodyR(R),mBodyT(bt),mJointT(jt)
	{
		// mSize *= 0.5;
		// std::cout<<name<<std::endl;
		// std::cout<<mSize.transpose()<<std::endl;
		// std::cout<<mBodyR.transpose()<<std::endl;
		// std::cout<<mBodyT.transpose()<<std::endl;
		// std::cout<<mJointT.transpose()<<std::endl<<std::endl;
	}
	std::string mName;
	Eigen::Vector3d mSize;
	Eigen::VectorXd mBodyR;
	Eigen::Vector3d mBodyT;
	Eigen::Vector3d mJointT;
};
std::string toString(const Eigen::Vector3d& v)
{
	std::string ret ="";
	for(int i=0;i<v.rows();i++){
		ret += std::to_string(v[i]);
		ret += " ";
	}
	return ret;

}
std::string toString(const Eigen::VectorXd& v)
{
	std::string ret ="";
	for(int i=0;i<v.rows();i++){
		ret += std::to_string(v[i]);
		ret += " ";
	}
	return ret;
}
void ReadMayaConstant(std::vector<MayaConstant>& mc,std::string path)
{
	std::ifstream ifs(path);
	if(!(ifs.is_open()))
	{
		std::cout<<"Can't read file "<<path<<std::endl;
		return;
	}
	std::string str;
	std::string index;
	std::stringstream ss;

	double a1,a2,a3,a4,a5,a6,a7,a8,a9;
	std::string name;
	Eigen::Vector3d size;
	Eigen::VectorXd bodyR(9);
	Eigen::Vector3d bodyT;
	Eigen::Vector3d jointT;
	while(!ifs.eof())
	{
		str.clear();
		index.clear();
		ss.clear();

		std::getline(ifs,str);
		ss.str(str);
		ss>>index;
		if(index=="n")
		{
			ss>>name;
		}
		else if(index=="s")
		{
			ss>>a1>>a2>>a3;
			size<<a1,a2,a3;
		}
		else if(index=="R")
		{
			ss>>a1>>a2>>a3>>a4>>a5>>a6>>a7>>a8>>a9;
			bodyR<<a1,a2,a3,a4,a5,a6,a7,a8,a9;
		}
		else if(index=="t")
		{
			ss>>a1>>a2>>a3;
			bodyT<<a1,a2,a3;
		}
		else if(index=="j")
		{
			ss>>a1>>a2>>a3;
			jointT<<a1,a2,a3;
			mc.push_back(MayaConstant(name,size,bodyR,bodyT,jointT));
		}
	}

	ifs.close();
}
#define DETAIL
int main(int argc,char** argv)
{
	std::map<std::string,UserConstant> ucs;
	std::vector<MayaConstant> mcs;
	ReadMayaConstant(mcs,argv[1]);
	ucs.insert(std::make_pair("Pelvis",UserConstant("None","FreeJoint",15.0,Eigen::Vector1d(-LARGE_VALUE),Eigen::Vector1d(LARGE_VALUE),Eigen::Vector3d(0,0,0),"Character1_Hips","Pelvis_mesh.obj")));
	//Upper body
	ucs.insert(std::make_pair("Spine",UserConstant("Pelvis","BallJoint",5.0,Eigen::Vector3d(-0.4,-0.4,-0.4),Eigen::Vector3d(0.4,0.4,0.4),Eigen::Vector3d(0,1,0),"Character1_Spine","Spine_mesh.obj")));
	ucs.insert(std::make_pair("Torso",UserConstant("Spine","BallJoint",10.0,Eigen::Vector1d(-LARGE_VALUE),Eigen::Vector1d(LARGE_VALUE),Eigen::Vector3d(0,0,0),"Character1_Spine1","Torso_mesh.obj")));
	ucs.insert(std::make_pair("Neck",UserConstant("Torso","RevoluteJoint",2.0,Eigen::Vector1d(-0.4),Eigen::Vector1d(0.4),Eigen::Vector3d(1,0,0),"Character1_Neck","Neck_mesh.obj")));
	ucs.insert(std::make_pair("Head",UserConstant("Neck","BallJoint",4.0,Eigen::Vector1d(-LARGE_VALUE),Eigen::Vector1d(LARGE_VALUE),Eigen::Vector3d(1,0,0),"Character1_Head","Skull_mesh.obj")));
	ucs.insert(std::make_pair("ShoulderL",UserConstant("Torso","RevoluteJoint",3.0,Eigen::Vector1d(-LARGE_VALUE),Eigen::Vector1d(LARGE_VALUE),Eigen::Vector3d(0,1,0),"Character1_LeftShoulder","L_Shoulder_mesh.obj")));
	ucs.insert(std::make_pair("ArmL",UserConstant("ShoulderL","BallJoint",2.0,Eigen::Vector1d(-LARGE_VALUE),Eigen::Vector1d(LARGE_VALUE),Eigen::Vector3d(0,0,0),"Character1_LeftArm","L_Humerus_mesh.obj")));
	ucs.insert(std::make_pair("ForeArmL",UserConstant("ArmL","RevoluteJoint",1.0,Eigen::Vector1d(-3.0),Eigen::Vector1d(-0.0),Eigen::Vector3d(0,1,0),"Character1_LeftForeArm","L_Radius_mesh.obj")));
	// ucs.insert(std::make_pair("HandL",UserConstant("ForeArmL","BallJoint",0.7,Eigen::Vector3d(-0.7,-0.7,-0.7),Eigen::Vector3d(0.7,0.7,0.7),Eigen::Vector3d(0,0,0),"Character1_LeftHand","HandL_racket_mesh.obj")));
	ucs.insert(std::make_pair("HandL",UserConstant("ForeArmL","BallJoint",0.7,Eigen::Vector3d(-0.7,-0.7,-0.7),Eigen::Vector3d(0.7,0.7,0.7),Eigen::Vector3d(0,0,0),"Character1_LeftHand","L_Hand_mesh.obj")));
	ucs.insert(std::make_pair("ShoulderR",UserConstant("Torso","RevoluteJoint",3.0,Eigen::Vector1d(-LARGE_VALUE),Eigen::Vector1d(LARGE_VALUE),Eigen::Vector3d(0,1,0),"Character1_RightShoulder","R_Shoulder_mesh.obj")));
	ucs.insert(std::make_pair("ArmR",UserConstant("ShoulderR","BallJoint",2.0,Eigen::Vector1d(-LARGE_VALUE),Eigen::Vector1d(LARGE_VALUE),Eigen::Vector3d(0,0,0),"Character1_RightArm","R_Humerus_mesh.obj")));
	ucs.insert(std::make_pair("ForeArmR",UserConstant("ArmR","RevoluteJoint",1.0,Eigen::Vector1d(-0.5),Eigen::Vector1d(0.0),Eigen::Vector3d(0,1,0),"Character1_RightForeArm","R_Radius_mesh.obj")));
	ucs.insert(std::make_pair("HandR",UserConstant("ForeArmR","BallJoint",0.7,Eigen::Vector3d(-0.7,-0.7,-0.7),Eigen::Vector3d(0.7,0.7,0.7),Eigen::Vector3d(0,0,0),"Character1_RightHand","R_Hand_mesh.obj")));
	//Lower body
	ucs.insert(std::make_pair("FemurL",UserConstant("Pelvis","BallJoint",7.0,Eigen::Vector1d(-LARGE_VALUE),Eigen::Vector1d(LARGE_VALUE),Eigen::Vector3d(0,0,0),"Character1_LeftUpLeg","L_Femur_mesh.obj")));
	ucs.insert(std::make_pair("TibiaL",UserConstant("FemurL","RevoluteJoint",3.0,Eigen::Vector1d(-LARGE_VALUE),Eigen::Vector1d(LARGE_VALUE),Eigen::Vector3d(1,0,0),"Character1_LeftLeg","L_Tibia_mesh.obj")));
	// ucs.insert(std::make_pair("TibiaL",UserConstant("FemurL","RevoluteJoint",3.0,Eigen::Vector1d(-0.3),Eigen::Vector1d(2.0),Eigen::Vector3d(1,0,0),"Character1_LeftLeg","L_Tibia_mesh.obj")));
	ucs.insert(std::make_pair("FemurR",UserConstant("Pelvis","BallJoint",7.0,Eigen::Vector1d(-LARGE_VALUE),Eigen::Vector1d(LARGE_VALUE),Eigen::Vector3d(0,0,0),"Character1_RightUpLeg","R_Femur_mesh.obj")));
	ucs.insert(std::make_pair("TibiaR",UserConstant("FemurR","RevoluteJoint",3.0,Eigen::Vector1d(-LARGE_VALUE),Eigen::Vector1d(LARGE_VALUE),Eigen::Vector3d(1,0,0),"Character1_RightLeg","R_Tibia_mesh.obj")));
	// ucs.insert(std::make_pair("TibiaR",UserConstant("FemurR","RevoluteJoint",3.0,Eigen::Vector1d(-0.3),Eigen::Vector1d(2.0),Eigen::Vector3d(1,0,0),"Character1_RightLeg","R_Tibia_mesh.obj")));

#ifndef DETAIL
	ucs.insert(std::make_pair("TalusL",UserConstant("TibiaL","BallJoint",1.0,Eigen::Vector3d(-0.5,-0.5,-0.2),Eigen::Vector3d(0.5,0.5,0.2),Eigen::Vector3d(0,0,0),"Character1_LeftFoot","L_Talus_merge_mesh.obj",true)));
	ucs.insert(std::make_pair("TalusR",UserConstant("TibiaR","BallJoint",1.0,Eigen::Vector3d(-0.5,-0.5,-0.2),Eigen::Vector3d(0.5,0.5,0.2),Eigen::Vector3d(0,0,0),"Character1_RightFoot","R_Talus_merge_mesh.obj",true)));
#else
	ucs.insert(std::make_pair("TalusL",UserConstant("TibiaL","BallJoint",0.6,Eigen::Vector3d(-0.5,-0.5,-0.2),Eigen::Vector3d(0.5,0.5,0.2),Eigen::Vector3d(0,0,0),"Character1_LeftFoot","TalusL_mesh2.obj",true)));
	ucs.insert(std::make_pair("FootThumbL",UserConstant("TalusL","RevoluteJoint",0.2,Eigen::Vector1d(-1.3),Eigen::Vector1d(1.3),Eigen::Vector3d(1,0,0),"None","FootThumbL_mesh.obj",true)));
	ucs.insert(std::make_pair("FootPinkyL",UserConstant("TalusL","RevoluteJoint",0.2,Eigen::Vector1d(-1.3),Eigen::Vector1d(1.3),Eigen::Vector3d(1,0,0),"None","FootPinkyL_mesh.obj",true)));

	ucs.insert(std::make_pair("TalusR",UserConstant("TibiaR","BallJoint",0.6,Eigen::Vector3d(-0.5,-0.5,-0.2),Eigen::Vector3d(0.5,0.5,0.2),Eigen::Vector3d(0,0,0),"Character1_RightFoot","TalusR_mesh2.obj",true)));
	ucs.insert(std::make_pair("FootThumbR",UserConstant("TalusR","RevoluteJoint",0.2,Eigen::Vector1d(-1.3),Eigen::Vector1d(1.3),Eigen::Vector3d(1,0,0),"None","FootThumbR_mesh.obj",true)));
	ucs.insert(std::make_pair("FootPinkyR",UserConstant("TalusR","RevoluteJoint",0.2,Eigen::Vector1d(-1.3),Eigen::Vector1d(1.3),Eigen::Vector3d(1,0,0),"None","FootPinkyR_mesh.obj",true)));
	//Left
	// ucs.insert(std::make_pair("TalusL",UserConstant("TibiaL","BallJoint",0.3,Eigen::Vector1d(-LARGE_VALUE),Eigen::Vector1d(LARGE_VALUE),Eigen::Vector3d(0,0,0),"Character1_LeftFoot","L_Talus_mesh.obj")));
	// ucs.insert(std::make_pair("NavicularL",UserConstant( "TalusL","WeldJoint",0.1,Eigen::Vector1d(-LARGE_VALUE),Eigen::Vector1d(LARGE_VALUE),Eigen::Vector3d(0,0,0),"None","L_Navicular_mesh.obj")));
	// ucs.insert(std::make_pair("MedialCuneiformL",UserConstant( "NavicularL","WeldJoint",0.1,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","L_Medial_Cuneiform_mesh.obj")));
	// ucs.insert(std::make_pair("Metatarsal1L",UserConstant( "MedialCuneiformL","WeldJoint",0.07,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","L_F_Metatarsal_1_mesh.obj")));
	// ucs.insert(std::make_pair("ProximalPhalanx1L",UserConstant( "Metatarsal1L","WeldJoint",0.05,Eigen::Vector1d(-PROXIMAL_JOINT_LIMIT),Eigen::Vector1d(PROXIMAL_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","L_F_Proximal_Phalanx_1_mesh.obj")));
	// ucs.insert(std::make_pair("DistalPhalanx1L",UserConstant( "ProximalPhalanx1L","WeldJoint",0.05,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","L_F_Distal_Phalanx_1_mesh.obj")));
	// ucs.insert(std::make_pair("IntermediateCuneiformL",UserConstant( "NavicularL","WeldJoint",0.1,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","L_Intermediate_Cuneiform_mesh.obj")));
	// ucs.insert(std::make_pair("Metatarsal2L",UserConstant( "IntermediateCuneiformL","WeldJoint",0.07,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","L_F_Metatarsal_2_mesh.obj")));
	// ucs.insert(std::make_pair("ProximalPhalanx2L",UserConstant( "Metatarsal2L","WeldJoint",0.05,Eigen::Vector1d(-PROXIMAL_JOINT_LIMIT),Eigen::Vector1d(PROXIMAL_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","L_F_Proximal_Phalanx_2_mesh.obj")));
	// ucs.insert(std::make_pair("MiddlePhalanx2L",UserConstant( "ProximalPhalanx2L","WeldJoint",0.05,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","L_F_Middle_Phalanx_2_mesh.obj")));
	// ucs.insert(std::make_pair("DistalPhalanx2L",UserConstant( "MiddlePhalanx2L","WeldJoint",0.04,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","L_F_Distal_Phalanx_2_mesh.obj")));
	// ucs.insert(std::make_pair("LateralCuneiformL",UserConstant( "NavicularL","WeldJoint",0.1,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","L_Lateral_Cuneiform_mesh.obj")));
	// ucs.insert(std::make_pair("Metatarsal3L",UserConstant( "LateralCuneiformL","WeldJoint",0.07,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","L_F_Metatarsal_3_mesh.obj")));
	// ucs.insert(std::make_pair("ProximalPhalanx3L",UserConstant( "Metatarsal3L","WeldJoint",0.05,Eigen::Vector1d(-PROXIMAL_JOINT_LIMIT),Eigen::Vector1d(PROXIMAL_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","L_F_Proximal_Phalanx_3_mesh.obj")));
	// ucs.insert(std::make_pair("MiddlePhalanx3L",UserConstant( "ProximalPhalanx3L","WeldJoint",0.05,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","L_F_Middle_Phalanx_3_mesh.obj")));
	// ucs.insert(std::make_pair("DistalPhalanx3L",UserConstant( "MiddlePhalanx3L","WeldJoint",0.04,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","L_F_Distal_Phalanx_3_mesh.obj")));
	// ucs.insert(std::make_pair("CuboidL",UserConstant( "TalusL","WeldJoint",0.1,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(0,0,1),"None","L_Cuboid_mesh.obj")));
	// ucs.insert(std::make_pair("Metatarsal4L",UserConstant( "CuboidL","WeldJoint",0.07,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","L_F_Metatarsal_4_mesh.obj")));
	// ucs.insert(std::make_pair("ProximalPhalanx4L",UserConstant( "Metatarsal4L","WeldJoint",0.05,Eigen::Vector1d(-PROXIMAL_JOINT_LIMIT),Eigen::Vector1d(PROXIMAL_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","L_F_Proximal_Phalanx_4_mesh.obj")));
	// ucs.insert(std::make_pair("MiddlePhalanx4L",UserConstant( "ProximalPhalanx4L","WeldJoint",0.05,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","L_F_Middle_Phalanx_4_mesh.obj")));
	// ucs.insert(std::make_pair("DistalPhalanx4L",UserConstant( "MiddlePhalanx4L","WeldJoint",0.04,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","L_F_Distal_Phalanx_4_mesh.obj")));
	// ucs.insert(std::make_pair("Metatarsal5L",UserConstant( "CuboidL","WeldJoint",0.07,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","L_F_Metatarsal_5_mesh.obj")));
	// ucs.insert(std::make_pair("ProximalPhalanx5L",UserConstant( "Metatarsal5L","WeldJoint",0.05,Eigen::Vector1d(-PROXIMAL_JOINT_LIMIT),Eigen::Vector1d(PROXIMAL_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","L_F_Proximal_Phalanx_5_mesh.obj")));
	// ucs.insert(std::make_pair("MiddlePhalanx5L",UserConstant( "ProximalPhalanx5L","WeldJoint",0.05,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","L_F_Middle_Phalanx_5_mesh.obj")));
	// ucs.insert(std::make_pair("DistalPhalanx5L",UserConstant( "MiddlePhalanx5L","WeldJoint",0.04,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","L_F_Distal_Phalanx_5_mesh.obj")));
	// ucs.insert(std::make_pair("CalcaneusL",UserConstant( "TalusL","WeldJoint",0.1,Eigen::Vector1d(-0.1),Eigen::Vector1d(0.1),Eigen::Vector3d(1,0,0),"None","L_Calcaneus_mesh.obj")));
	// //Right
	// ucs.insert(std::make_pair("TalusR",UserConstant("TibiaR","BallJoint",0.3,Eigen::Vector1d(-LARGE_VALUE),Eigen::Vector1d(LARGE_VALUE),Eigen::Vector3d(0,0,0),"Character1_RightFoot","R_Talus_mesh.obj")));
	// ucs.insert(std::make_pair("NavicularR",UserConstant( "TalusR","WeldJoint",0.1,Eigen::Vector1d(-LARGE_VALUE),Eigen::Vector1d(LARGE_VALUE),Eigen::Vector3d(0,0,0),"None","R_Navicular_mesh.obj")));
	// ucs.insert(std::make_pair("MedialCuneiformR",UserConstant( "NavicularR","WeldJoint",0.1,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","R_Medial_Cuneiform_mesh.obj")));
	// ucs.insert(std::make_pair("Metatarsal1R",UserConstant( "MedialCuneiformR","WeldJoint",0.07,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","R_F_Metatarsal_1_mesh.obj")));
	// ucs.insert(std::make_pair("ProximalPhalanx1R",UserConstant( "Metatarsal1R","WeldJoint",0.05,Eigen::Vector1d(-PROXIMAL_JOINT_LIMIT),Eigen::Vector1d(PROXIMAL_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","R_F_Proximal_Phalanx_1_mesh.obj")));
	// ucs.insert(std::make_pair("DistalPhalanx1R",UserConstant( "ProximalPhalanx1R","WeldJoint",0.05,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","R_F_Distal_Phalanx_1_mesh.obj")));
	// ucs.insert(std::make_pair("IntermediateCuneiformR",UserConstant( "NavicularR","WeldJoint",0.1,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","R_Intermediate_Cuneiform_mesh.obj")));
	// ucs.insert(std::make_pair("Metatarsal2R",UserConstant( "IntermediateCuneiformR","WeldJoint",0.07,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","R_F_Metatarsal_2_mesh.obj")));
	// ucs.insert(std::make_pair("ProximalPhalanx2R",UserConstant( "Metatarsal2R","WeldJoint",0.05,Eigen::Vector1d(-PROXIMAL_JOINT_LIMIT),Eigen::Vector1d(PROXIMAL_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","R_F_Proximal_Phalanx_2_mesh.obj")));
	// ucs.insert(std::make_pair("MiddlePhalanx2R",UserConstant( "ProximalPhalanx2R","WeldJoint",0.05,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","R_F_Middle_Phalanx_2_mesh.obj")));
	// ucs.insert(std::make_pair("DistalPhalanx2R",UserConstant( "MiddlePhalanx2R","WeldJoint",0.04,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","R_F_Distal_Phalanx_2_mesh.obj")));
	// ucs.insert(std::make_pair("LateralCuneiformR",UserConstant( "NavicularR","WeldJoint",0.1,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","R_Lateral_Cuneiform_mesh.obj")));
	// ucs.insert(std::make_pair("Metatarsal3R",UserConstant( "LateralCuneiformR","WeldJoint",0.07,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","R_F_Metatarsal_3_mesh.obj")));
	// ucs.insert(std::make_pair("ProximalPhalanx3R",UserConstant( "Metatarsal3R","WeldJoint",0.05,Eigen::Vector1d(-PROXIMAL_JOINT_LIMIT),Eigen::Vector1d(PROXIMAL_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","R_F_Proximal_Phalanx_3_mesh.obj")));
	// ucs.insert(std::make_pair("MiddlePhalanx3R",UserConstant( "ProximalPhalanx3R","WeldJoint",0.05,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","R_F_Middle_Phalanx_3_mesh.obj")));
	// ucs.insert(std::make_pair("DistalPhalanx3R",UserConstant( "MiddlePhalanx3R","WeldJoint",0.04,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","R_F_Distal_Phalanx_3_mesh.obj")));
	// ucs.insert(std::make_pair("CuboidR",UserConstant( "TalusR","WeldJoint",0.1,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(0,0,1),"None","R_Cuboid_mesh.obj")));
	// ucs.insert(std::make_pair("Metatarsal4R",UserConstant( "CuboidR","WeldJoint",0.07,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","R_F_Metatarsal_4_mesh.obj")));
	// ucs.insert(std::make_pair("ProximalPhalanx4R",UserConstant( "Metatarsal4R","WeldJoint",0.05,Eigen::Vector1d(-PROXIMAL_JOINT_LIMIT),Eigen::Vector1d(PROXIMAL_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","R_F_Proximal_Phalanx_4_mesh.obj")));
	// ucs.insert(std::make_pair("MiddlePhalanx4R",UserConstant( "ProximalPhalanx4R","WeldJoint",0.05,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","R_F_Middle_Phalanx_4_mesh.obj")));
	// ucs.insert(std::make_pair("DistalPhalanx4R",UserConstant( "MiddlePhalanx4R","WeldJoint",0.04,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","R_F_Distal_Phalanx_4_mesh.obj")));
	// ucs.insert(std::make_pair("Metatarsal5R",UserConstant( "CuboidR","WeldJoint",0.07,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","R_F_Metatarsal_5_mesh.obj")));
	// ucs.insert(std::make_pair("ProximalPhalanx5R",UserConstant( "Metatarsal5R","WeldJoint",0.05,Eigen::Vector1d(-PROXIMAL_JOINT_LIMIT),Eigen::Vector1d(PROXIMAL_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","R_F_Proximal_Phalanx_5_mesh.obj")));
	// ucs.insert(std::make_pair("MiddlePhalanx5R",UserConstant( "ProximalPhalanx5R","WeldJoint",0.05,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","R_F_Middle_Phalanx_5_mesh.obj")));
	// ucs.insert(std::make_pair("DistalPhalanx5R",UserConstant( "MiddlePhalanx5R","WeldJoint",0.04,Eigen::Vector1d(-REVOLUTE_JOINT_LIMIT),Eigen::Vector1d(REVOLUTE_JOINT_LIMIT),Eigen::Vector3d(1,0,0),"None","R_F_Distal_Phalanx_5_mesh.obj")));
	// ucs.insert(std::make_pair("CalcaneusR",UserConstant( "TalusR","WeldJoint",0.1,Eigen::Vector1d(-0.1),Eigen::Vector1d(0.1),Eigen::Vector3d(1,0,0),"None","R_Calcaneus_mesh.obj")));
#endif
	TiXmlDocument doc;
	TiXmlElement* skel_elem = new TiXmlElement("Skeleton");
	skel_elem->SetAttribute("name","Foot");
	doc.LinkEndChild(skel_elem);
	for(int i =0;i<mcs.size();i++)
	{
		TiXmlElement* joint_elem = new TiXmlElement("Joint");
		std::cout<<mcs[i].mName<<std::endl;
		if(ucs.find(mcs[i].mName)==ucs.end())
			continue;
		auto& uc = ucs.at(mcs[i].mName);
		// std::cout<<mcs[i].mName<<std::endl;

		joint_elem->SetAttribute("type",uc.mJointType);
		joint_elem->SetAttribute("name",mcs[i].mName);
		joint_elem->SetAttribute("parent_name",uc.mParent);
		joint_elem->SetAttribute("size",toString(mcs[i].mSize));
		joint_elem->SetAttribute("mass",std::to_string(uc.mMass));
		if(uc.mBVHMap!="None"){
			joint_elem->SetAttribute("bvh",uc.mBVHMap);
		}
		if(uc.mOBJMap!="None"){
			// joint_elem->SetAttribute("obj",uc.mOBJMap);
		}
		if(uc.mAxis.norm()>1E-4)
		{
			joint_elem->SetAttribute("axis",toString(uc.mAxis));
		}
		if(uc.collision_on==true)
		{
			joint_elem->SetAttribute("contact","On");
		}
		TiXmlElement* body_position_elem = new TiXmlElement("BodyPosition");
		body_position_elem->SetAttribute("linear",toString(mcs[i].mBodyR));
		body_position_elem->SetAttribute("translation",toString(mcs[i].mBodyT));
		TiXmlElement* joint_position_elem = new TiXmlElement("JointPosition");
		joint_position_elem->SetAttribute("translation",toString(mcs[i].mJointT));
		if(uc.mJointLowerLimit.norm()<100.0)
		{
			joint_position_elem->SetAttribute("lower",toString(uc.mJointLowerLimit));
			joint_position_elem->SetAttribute("upper",toString(uc.mJointUpperLimit));
		}

		joint_elem->LinkEndChild(body_position_elem);
		joint_elem->LinkEndChild(joint_position_elem);
		skel_elem->LinkEndChild( joint_elem );
	}
	TiXmlPrinter printer;
	printer.SetIndent( "\n" );

	doc.Accept( &printer );
	doc.SaveFile(argv[2]);

	return 0;
}


// Copyright 2016 Victor Smirnov
//
// 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.

#pragma once

#include <memoria/core/types.hpp>
#include <memoria/core/tools/bitmap.hpp>

#include <boost/multiprecision/cpp_int.hpp>

namespace memoria {

template <typename> class ValueCodec;

namespace mp = boost::multiprecision;

template <
        unsigned MinDigits,
        unsigned MaxDigits,
        mp::cpp_integer_type SignType,
        mp::cpp_int_check_type Checked,
        class Allocator,
        mp::expression_template_option ExpressionTemplates
>
class ValueCodec<mp::number<mp::cpp_int_backend<MinDigits, MaxDigits, SignType, Checked, Allocator>, ExpressionTemplates>> {
public:
    using BufferType    = uint8_t;
    using T             = BufferType;
    using V             = mp::number<mp::cpp_int_backend<MinDigits, MaxDigits, SignType, Checked, Allocator>, ExpressionTemplates>;

    using ValuePtr      = ValuePtrT1<BufferType>;

    static const int32_t BitsPerOffset  = 16;
    static const int32_t ElementSize    = 8; // In bits;

    ValuePtr describe(const T* buffer, size_t idx)
    {
        return ValuePtr(buffer + idx, length(buffer, idx, -1ull));
    }

    size_t length(const T* buffer, size_t idx, size_t limit) const
    {
        auto head = buffer[idx];
        if (head < 252) {
            return 1;
        }
        else if (head < 254) {
            return buffer[idx + 1] + 2;
        }
        else {
            size_t len = 0;
            for (size_t c = idx + 1; c < idx + 4; c++)
            {
                len |= buffer[c];
                len <<= 8;
            }

            return len + 5;
        }
    }

    size_t length(const V& value) const
    {
        auto size       = value.backend().size();
        auto limbs      = value.backend().limbs();
        bool negative   = value.sign() == -1;

        using LimbType  = decltype(*limbs);

        if (size == 1u)
        {
            if (!negative)
            {
                if (limbs[0] < 126ul)
                {
                    return 1;
                }
                else {
                    return 2 + byte_length(limbs, size);
                }
            }
            else {
                if (limbs[0] < 127ul)
                {
                    return 1;
                }
                else {
                    return 2 + byte_length(limbs, size);
                }
            }
        }
        else if (size <= 256u / sizeof(LimbType))
        {
            return 2 + byte_length(limbs, size);
        }
        else {
            return 5 + byte_length(limbs, size);
        }
    }

    size_t decode(const T* buffer, V& value, size_t idx, size_t limit) const
    {
        return decode(buffer, value, idx);
    }

    size_t decode(const T* buffer, V& value, size_t idx) const
    {
        auto header = buffer[idx];

        if (header < 126u)
        {
            value = (unsigned long long)header;
            return 1;
        }
        else if (header < 252u)
        {
            value = -(header - 125);
            return 1;
        }
        else if (header < 254u)
        {
            idx++;

            size_t len = buffer[idx++];

            deserialize(buffer, value, idx, len);

            value.backend().sign(header == 253u);

            return len + 2;
        }
        else {
            idx++;

            size_t len = 0;

            for (uint32_t c = 0; c < 4; c++)
            {
                len |= ((size_t)buffer[idx++]) << (c << 3);
            }

            deserialize(buffer, value, idx, len);

            value.backend().sign(header == 255u);

            return len + 5;
        }
    }

    size_t encode(T* buffer, const ValuePtr& value, size_t idx) const
    {
        copy(value.addr(), 0, buffer, idx, value.length());
        return value.length();
    }

    size_t encode(T* buffer, const V& value, size_t idx, size_t limit) const
    {
        return encode(buffer, value, idx);
    }

    size_t encode(T* buffer, const V& value, size_t idx) const
    {
        auto size       = value.backend().size();
        auto limbs      = value.backend().limbs();
        bool negative   = value.sign() == -1;
        using LimbType  = decltype(*limbs);

        if (size == 1u)
        {
            if (!negative)
            {
                if (limbs[0] < 126ul)
                {
                    buffer[idx] = limbs[0];
                    return 1;
                }
                else {
                    buffer[idx]     = 252u;

                    size_t len = serialize_limb(buffer, limbs[0], idx + 2);

                    buffer[idx + 1] = len;

                    return 2 + len;
                }
            }
            else {
                if (limbs[0] < 127ul)
                {
                    buffer[idx] = limbs[0] + 125u;
                    return 1;
                }
                else {
                    buffer[idx] = 253u;

                    size_t len = serialize_limb(buffer, limbs[0], idx + 2);

                    buffer[idx + 1] = len;

                    return 2 + len;
                }
            }
        }
        else if (size <= 256u/sizeof(LimbType))
        {
            size_t len = serialize(buffer, value, idx + 2);

            buffer[idx] = negative ? 253u : 252u;
            buffer[idx + 1] = len;

            return 2 + len;
        }
        else {
            size_t len = serialize(buffer, value, idx + 5);

            buffer[idx++] = negative ? 255u : 254u;

            for (size_t c = 0; c < 4; c++)
            {
                buffer[idx++] = len >> (c << 3);
            }

            return 5 + len;
        }
    }

    void move(T* buffer, size_t from, size_t to, size_t size) const
    {
        CopyBuffer(buffer + from, buffer + to, size);
    }

    void copy(const T* src, size_t from, T* tgt, size_t to, size_t size) const
    {
        CopyBuffer(src + from, tgt + to, size);
    }

private:

    static size_t serialize_limb(T* buffer, unsigned long value, size_t idx)
    {
        uint32_t len = bytes(value);

        for (uint32_t c = 0; c < len; c++)
        {
            buffer[idx++] = value >> (c << 3);
        }

        return len;
    }

    static size_t serialize_limb(T* buffer, unsigned long long value, size_t idx)
    {
        uint32_t len = bytes(value);

        for (uint32_t c = 0; c < len; c++)
        {
            buffer[idx++] = value >> (c << 3);
        }

        return len;
    }

    static size_t serialize(T* buffer, const V& value, size_t idx)
    {
        auto end = mp::export_bits(value, buffer + idx, 8, false);
        return (size_t)(end - (buffer + idx));
    }

    static void deserialize(const T* buffer, V& value, size_t idx, size_t len)
    {
        mp::import_bits(value, buffer + idx, buffer + idx + len, 8, false);
    }

    constexpr static uint32_t msb(unsigned digits)
    {
        return 31 - __builtin_clz(digits);
    }

    constexpr static    uint32_t msb(unsigned long digits)
    {
        return 63 - __builtin_clzl(digits);
    }

    constexpr static uint32_t msb(unsigned long long digits)
    {
        return 63 - __builtin_clzll(digits);
    }

    template <typename T>
    constexpr static uint32_t bytes(T digits)
    {
        uint32_t v = msb(digits) + 1;
        return (v >> 3) + ((v & 0x7) != 0);
    }

    template <typename T>
    constexpr static uint32_t byte_length(const T* data, unsigned size)
    {
        return bytes(data[size - 1]) + (size - 1) * sizeof(T);
    }
};

}