#include <cstdint>
#include <memory>
#include <vector>
#include <string>
#include <numeric>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <omp.h>
#include <stdio.h>
#include <valarray>
#include <boost/range/combine.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

#include <posit/posit>
// Posit Arithmetic FPGA accelerator library
#include <positarith.h>

#include "main.hpp"
#include "defines.hpp"
#include "utils.hpp"
#include "blas.hpp"
#include "vector_utils.hpp"
#include "matrix_utils.hpp"

using namespace std;
using namespace sw::unum;
using boost::multiprecision::cpp_dec_float_100;

posit<NBITS, ES> random_number(float offset, float dev) {
    float num_float;
    posit<NBITS, 2> num_posit_2;
    posit<NBITS, 3> num_posit_3;

    do {
        num_float = offset + (rand() * dev / RAND_MAX);
        num_posit_2 = num_float;
        num_posit_3 = num_float;
    } while (num_posit_2 != num_float || num_posit_3 != num_float);

    return posit<NBITS, ES>(num_float);
}

int main(int argc, char ** argv)
{
    vector<int> lengths = {10, 20, 30, 40, 50, 100, 200, 300, 400, 500, 1000, 2000, 3000, 4000, 5000, 10000, 20000, 30000, 40000, 50000, 100000, 200000, 300000, 400000, 500000};

    std::string s_dot;
    for(int length : lengths) {
        s_dot = test_dot_product(length);

        ofstream outfile("positarith_dot_es" + std::to_string(ES) + "_" + std::to_string(length) + ".txt", ios::out);
        outfile << s_dot << endl;
        outfile.close();

        remove("top.wdb");
    }

    return 0;
}

std::string test_dot_product(int length) {
    double t_fpga, t_sw, t_float;
    double stop, start;
    int i;
    cpp_dec_float_100 da_sw = 0.0, da_hw = 0.0, da_float = 0.0;

    // Test data set
    std::vector<posit<NBITS,ES>> vec1, vec2;
    // vec1.resize(length); vec2.resize(length);
    for(int i = 0; i < length; i++) {
        posit<NBITS,ES> pos1, pos2;
        pos1 = random_number(0.0, 1.0);
        pos2 = random_number(0.0, 1.0);

        vec1.push_back(pos1);
        vec2.push_back(pos2);
    }

    // Posit HW calculation
    posit<NBITS,ES> res_hw = 0;
    // t_fpga = vector_dot(vec1, vec2, res_hw);

    // Posit SW calculation
    posit<NBITS,ES> res_sw = 0.0;
    start = omp_get_wtime();
    // for(i = 0; i < length; i++) {
    //     res_sw += vec1[i] * vec2[i];
    // }
    res_sw = sw::hprblas::dot(length, vec1, 1, vec2, 1);
    stop = omp_get_wtime();
    t_sw = stop - start;

    // Float calculation
    float res_float = 0.0;
    start = omp_get_wtime();
    #pragma omp parallel private(i) num_threads(8)
    {
        #pragma omp for reduction(+:res_float)
        for(i = 0; i < length; i++) {
            res_float += (float)vec1[i] * (float)vec2[i];
        }
    }
    stop = omp_get_wtime();
    t_float = stop - start;

    // Reference calculation
    cpp_dec_float_100 res_dec = 0.0;
    for(int i = 0; i < length; i++) {
        res_dec += (cpp_dec_float_100)((long double)vec1[i]) * (cpp_dec_float_100)((long double)vec2[i]);
    }

    // Calculate decimal accuracy
    da_float = decimal_accuracy(res_dec, (cpp_dec_float_100)res_float);
    da_sw = decimal_accuracy(res_dec, (cpp_dec_float_100)((long double)res_sw));
    da_hw = decimal_accuracy(res_dec, (cpp_dec_float_100)((long double)res_hw));

    return to_string_precision(t_fpga) + "," + to_string_precision(t_sw) + "," + to_string_precision(t_float) + "," + to_string_precision(da_hw) + "," + to_string_precision(da_sw) + "," + to_string_precision(da_float);
}


#ifndef YANNQ_GROUNDSTATE_NGDEXACT_HPP
#define YANNQ_GROUNDSTATE_NGDEXACT_HPP
#include <complex>

#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <tbb/tbb.h>

#include "ED/ConstructSparseMat.hpp"
#include "Utilities/Utility.hpp"

#include "Machines/AmplitudePhase.hpp"

namespace yannq
{

class NGDExact
{
public:
	using Machine = AmplitudePhase;
	using RealScalar = typename Machine::RealScalar;
	using ComplexScalar = typename Machine::ComplexScalar;

	using RealMatrix = typename Machine::RealMatrix;
	using RealVector = typename Machine::RealVector;

	using ComplexMatrix = typename Machine::ComplexMatrix;
	using ComplexVector = typename Machine::ComplexVector;

private:
	const uint32_t n_;
	const AmplitudePhase& qs_;
	tbb::concurrent_vector<uint32_t> basis_;

	Eigen::SparseMatrix<RealScalar> ham_;

	RealMatrix deltasAmp_;
	RealMatrix deltasPhase_;

	RealMatrix deltasAmpPsis_;
	RealMatrix deltasPhasePsis_;

	RealVector olocAmp_;
	RealVector olocPhase_;

	RealVector grad_;

	RealScalar energy_;

	void constructDeltaAmp()
	{
		using Range = tbb::blocked_range<std::size_t>;
		const int N = qs_.getN();
		deltasAmp_.setZero(basis_.size(), qs_.getDimAmp());
		if(basis_.size() >= 32)
		{
			tbb::parallel_for(Range(std::size_t(0u), basis_.size(), 8),
				[&](const Range& r)
			{
				uint32_t start = r.begin();
				uint32_t end = r.end();
				RealMatrix tmp(end-start, qs_.getDimAmp());
				for(uint32_t l = 0; l < end-start; ++l)
				{
					tmp.row(l) =
						qs_.logDerivAmp(qs_.makeAmpData(toSigma(N, basis_[l+start])));
				}
				deltasAmp_.block(start, 0, end-start, qs_.getDimAmp()) = tmp;
			}, tbb::simple_partitioner());
		}
		else
		{
			for(uint32_t k = 0; k < basis_.size(); k++)
			{
				deltasAmp_.row(k) =
					qs_.logDerivAmp(qs_.makeAmpData(toSigma(N, basis_[k])));
			}
		}
	}

	void constructDeltaPhase()
	{
		using Range = tbb::blocked_range<std::size_t>;
		const int N = qs_.getN();
		deltasPhase_.setZero(basis_.size(), qs_.getDimPhase());

		if(basis_.size() >= 32)
		{
			tbb::parallel_for(Range(std::size_t(0u), basis_.size(), 8),
				[&](const Range& r)
			{
				uint32_t start = r.begin();
				uint32_t end = r.end();
				RealMatrix tmp(end-start, qs_.getDimPhase());
				for(uint32_t l = 0; l < end-start; ++l)
				{
					tmp.row(l) =
						qs_.logDerivPhase(qs_.makePhaseData(toSigma(N, basis_[l+start])));
				}
				deltasPhase_.block(start, 0, end-start, qs_.getDimPhase()) = tmp;
			}, tbb::simple_partitioner());
		}
		else
		{
			for(uint32_t k = 0; k < basis_.size(); k++)
			{
				deltasPhase_.row(k) =
					qs_.logDerivPhase(qs_.makePhaseData(toSigma(N, basis_[k])));
			}

		}
	}

public:

	RealScalar getEnergy() const
	{
		return energy_;
	}

	void constructExact()
	{
		ComplexVector st = getPsi(qs_, basis_, true);
		ComplexVector k = ham_*st;

		energy_ = std::real(ComplexScalar(st.adjoint()*k));

		constructDeltaAmp();
		constructDeltaPhase();

		deltasAmpPsis_ = st.cwiseAbs2().asDiagonal()*deltasAmp_;
		olocAmp_ = deltasAmpPsis_.colwise().sum();

		deltasPhasePsis_ = st.cwiseAbs2().asDiagonal()*deltasPhase_;
		olocPhase_ = deltasPhasePsis_.colwise().sum();

		k = st.conjugate().asDiagonal()*k;
		grad_.resize(qs_.getDim());
		grad_.head(qs_.getDimAmp()) =
			2.0*deltasAmp_.transpose()*k.real();
		grad_.head(qs_.getDimAmp()) -= 2.0*energy_*olocAmp_;
		grad_.tail(qs_.getDimPhase())
			= 2.0*deltasPhase_.transpose()*k.imag();
		/*
		grad_.tail(qs_.getDimPhase())
			-= 2.0*energy_*olocPhase_;
			*/
	}

	RealScalar eloc() const
	{
		return energy_;
	}

	const RealVector& olocAmp() const&
	{
		return olocAmp_;
	}

	RealMatrix olocAmp() &&
	{
		return olocAmp_;
	}

	const RealVector& olocPhase() const&
	{
		return olocPhase_;
	}

	RealMatrix olocPhase() &&
	{
		return olocPhase_;
	}

	RealMatrix corrMatAmp() const
	{
		RealMatrix res = deltasAmp_.transpose()*deltasAmpPsis_;
		res -= olocAmp_*olocAmp_.transpose();
		return res;
	}

	RealMatrix corrMatPhase() const
	{
		RealMatrix res = deltasPhase_.transpose()*deltasPhasePsis_;
		res -= olocPhase_*olocPhase_.transpose();
		return res;
	}

	RealVector energyGrad() const
	{
		return grad_;
	}

	template<class Iterable, class ColFunc>
	NGDExact(const AmplitudePhase& qs, Iterable&& basis, ColFunc&& col)
	  : n_{qs.getN()}, qs_(qs)
	{
		tbb::parallel_for_each(basis.begin(), basis.end(),
				[&](uint32_t elt)
		{
			basis_.emplace_back(elt);
		});
		tbb::parallel_sort(basis_.begin(), basis_.end());

		ham_ = edp::constructSubspaceMat(std::forward<ColFunc>(col), basis_);
	}
};
} //namespace yannq
#endif//YANNQ_GROUNDSTATE_NGDEXACT_HPP


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

#include <boost/math/quadrature/naive_monte_carlo.hpp>
#include <iostream>
#include <iomanip>
#include <limits>
#include <cmath>
#include <thread>
#include <future>
#include <string>
#include <chrono>
#include <boost/math/special_functions/pow.hpp>
#include <boost/math/constants/constants.hpp>

using std::vector;
using std::pair;
using boost::math::quadrature::naive_monte_carlo;

void display_progress(double progress,
                      double error_estimate,
                      double current_estimate,
                      std::chrono::duration<double> estimated_time_to_completion)
{
    int barWidth = 70;

    std::cout << "[";
    int pos = barWidth * progress;
    for (int i = 0; i < barWidth; ++i) {
        if (i < pos) std::cout << "=";
        else if (i == pos) std::cout << ">";
        else std::cout << " ";
    }
    std::cout << "] "
              << int(progress * 100.0)
              << "%, E = "
              << std::setprecision(3)
              << error_estimate
              << ", time to completion: "
              << estimated_time_to_completion.count()
              << " seconds, estimate: "
              << std::setprecision(5)
              << current_estimate
              << "     \r";

    std::cout.flush();
}

int main()
{
    double exact = 1.3932039296856768591842462603255;
    double A = 1.0 / boost::math::pow<3>(boost::math::constants::pi<double>());
    auto g = [&](std::vector<double> const & x)
    {
      return A / (1.0 - cos(x[0])*cos(x[1])*cos(x[2]));
    };
    vector<pair<double, double>> bounds{{0, boost::math::constants::pi<double>() }, {0, boost::math::constants::pi<double>() }, {0, boost::math::constants::pi<double>() }};
    naive_monte_carlo<double, decltype(g)> mc(g, bounds, 0.001);

    auto task = mc.integrate();

    int s = 0;
    std::cout << "Hit ctrl-c to cancel.\n";
    while (task.wait_for(std::chrono::seconds(1)) != std::future_status::ready)
    {
        display_progress(mc.progress(),
                         mc.current_error_estimate(),
                         mc.current_estimate(),
                         mc.estimated_time_to_completion());
        // TODO: The following shows that cancellation works,
        // but it would be nice to show how it works with a ctrl-c signal handler.
        if (s++ > 25){
          mc.cancel();
          std::cout << "\nCancelling because this is too slow!\n";
        }
    }
    double y = task.get();
    display_progress(mc.progress(),
                     mc.current_error_estimate(),
                     mc.current_estimate(),
                     mc.estimated_time_to_completion());
    std::cout << std::setprecision(std::numeric_limits<double>::digits10) << std::fixed;
    std::cout << "\nFinal value: " << y << std::endl;
    std::cout << "Exact      : " << exact << std::endl;
    std::cout << "Final error estimate: " << mc.current_error_estimate() << std::endl;
    std::cout << "Actual error        : " << abs(y - exact) << std::endl;
    std::cout << "Function calls: " << mc.calls() << std::endl;
    std::cout << "Is this good enough? [y/N] ";
    bool goodenough = true;
    std::string line;
    std::getline(std::cin, line);
    if (line[0] != 'y')
    {
         goodenough = false;
    }
    double new_error = -1;
    if (!goodenough)
    {
        std::cout << "What is the new target error? ";
        std::getline(std::cin, line);
        new_error = atof(line.c_str());
        if (new_error >= mc.current_error_estimate())
        {
           std::cout << "That error bound is already satisfied.\n";
           return 0;
        }
    }
    if (new_error > 0)
    {
        mc.update_target_error(new_error);
        auto task = mc.integrate();
        std::cout << "Hit ctrl-c to cancel.\n";
        while (task.wait_for(std::chrono::seconds(1)) != std::future_status::ready)
        {
            display_progress(mc.progress(),
                             mc.current_error_estimate(),
                             mc.current_estimate(),
                             mc.estimated_time_to_completion());
        }
        double y = task.get();
        display_progress(mc.progress(),
                         mc.current_error_estimate(),
                         mc.current_estimate(),
                         mc.estimated_time_to_completion());
        std::cout << std::setprecision(std::numeric_limits<double>::digits10) << std::fixed;
        std::cout << "\nFinal value: " << y << std::endl;
        std::cout << "Exact      : " << exact << std::endl;
        std::cout << "Final error estimate: " << mc.current_error_estimate() << std::endl;
        std::cout << "Actual error        : " << abs(y - exact) << std::endl;
        std::cout << "Function calls: " << mc.calls() << std::endl;
    }
}


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file StaticNoiseModel-inl.hpp
///
/// \author Sean Anderson, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#include <steam/problem/NoiseModel.hpp>

#include <iostream>
#include <stdexcept>

#include <Eigen/Cholesky>

namespace steam {

template<int MEAS_DIM>
DynamicNoiseModel<MEAS_DIM>::DynamicNoiseModel(std::shared_ptr<NoiseEvaluator<MEAS_DIM>> eval) :
eval_(eval) {
  this->setByCovariance(eval_->evaluateCovariance());
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Get a reference to the square root information matrix
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
const Eigen::Matrix<double,MEAS_DIM,MEAS_DIM>& DynamicNoiseModel<MEAS_DIM>::getSqrtInformation() const {
  this->setByCovariance(eval_->evaluateCovariance());
  return this->sqrtInformation_;
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Get the norm of the whitened error vector, sqrt(rawError^T * info * rawError)
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
double DynamicNoiseModel<MEAS_DIM>::getWhitenedErrorNorm(
    const Eigen::Matrix<double,MEAS_DIM,1>& rawError) const {
  this->setByCovariance(eval_->evaluateCovariance());
  return (this->sqrtInformation_*rawError).norm();
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Get the whitened error vector, sqrtInformation*rawError
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
Eigen::Matrix<double,MEAS_DIM,1> DynamicNoiseModel<MEAS_DIM>::whitenError(
    const Eigen::Matrix<double,MEAS_DIM,1>& rawError) const {
  this->setByCovariance(eval_->evaluateCovariance());
  return this->sqrtInformation_*rawError;
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Default constructor
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
StaticNoiseModel<MEAS_DIM>::StaticNoiseModel() {
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief General constructor
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
BaseNoiseModel<MEAS_DIM>::BaseNoiseModel(const Eigen::Matrix<double,MEAS_DIM,MEAS_DIM>& matrix,
                                 MatrixType type) {

  // Depending on the type of 'matrix', we set the internal storage
  switch(type) {
    case COVARIANCE :
      setByCovariance(matrix);
      break;
    case INFORMATION :
      setByInformation(matrix);
      break;
    case SQRT_INFORMATION :
      setBySqrtInformation(matrix);
      break;
  }
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Set by covariance matrix
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
void BaseNoiseModel<MEAS_DIM>::setByCovariance(
    const Eigen::Matrix<double,MEAS_DIM,MEAS_DIM>& matrix) const {

  // Information is the inverse of covariance
  this->setByInformation(matrix.inverse());
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Set by information matrix
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
void BaseNoiseModel<MEAS_DIM>::setByInformation(
    const Eigen::Matrix<double,MEAS_DIM,MEAS_DIM>& matrix) const {

  // Check that the matrix is positive definite
  this->assertPositiveDefiniteMatrix(matrix);

  // Perform an LLT decomposition
  Eigen::LLT<Eigen::Matrix<double,MEAS_DIM,MEAS_DIM> > lltOfInformation(matrix);

  // Store upper triangular matrix (the square root information matrix)
  this->setBySqrtInformation(lltOfInformation.matrixL().transpose());
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Set by square root of information matrix
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
void BaseNoiseModel<MEAS_DIM>::setBySqrtInformation(
    const Eigen::Matrix<double,MEAS_DIM,MEAS_DIM>& matrix)  const {

  // Set internal storage matrix
  sqrtInformation_ = matrix; // todo: check this is upper triangular
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Assert that the matrix is positive definite
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
void BaseNoiseModel<MEAS_DIM>::assertPositiveDefiniteMatrix(
    const Eigen::Matrix<double,MEAS_DIM,MEAS_DIM>& matrix)  const{

  // Initialize an eigen value solver
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,MEAS_DIM,MEAS_DIM> >
      eigsolver(matrix, Eigen::EigenvaluesOnly);

  // Check the minimum eigen value
  if (eigsolver.eigenvalues().minCoeff() <= 0) {
    std::stringstream ss; ss << "Covariance \n" << matrix << "\n must be positive definite. "
                             << "Min. eigenvalue : " << eigsolver.eigenvalues().minCoeff();
    throw std::invalid_argument(ss.str());
  }
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief General constructor
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
StaticNoiseModel<MEAS_DIM>::StaticNoiseModel(const Eigen::Matrix<double,MEAS_DIM,MEAS_DIM>& matrix,
                                             MatrixType type)
: BaseNoiseModel<MEAS_DIM>::BaseNoiseModel(matrix,type) {

}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Get a reference to the square root information matrix
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
const Eigen::Matrix<double,MEAS_DIM,MEAS_DIM>& StaticNoiseModel<MEAS_DIM>::getSqrtInformation() const {
  return this->sqrtInformation_;
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Get the norm of the whitened error vector, sqrt(rawError^T * info * rawError)
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
double StaticNoiseModel<MEAS_DIM>::getWhitenedErrorNorm(
    const Eigen::Matrix<double,MEAS_DIM,1>& rawError) const {
  return (this->sqrtInformation_*rawError).norm();
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Get the whitened error vector, sqrtInformation*rawError
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
Eigen::Matrix<double,MEAS_DIM,1> StaticNoiseModel<MEAS_DIM>::whitenError(
    const Eigen::Matrix<double,MEAS_DIM,1>& rawError) const {
  return this->sqrtInformation_*rawError;
}

} // steam


#include <boost/algorithm/hex.hpp>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include <nil/crypto3/zk/components/blueprint.hpp>
#include <nil/crypto3/zk/components/blueprint_variable.hpp>

#include <nil/crypto3/zk/snark/schemes/ppzksnark/r1cs_gg_ppzksnark.hpp>
#include <nil/crypto3/zk/snark/schemes/ppzksnark/r1cs_gg_ppzksnark/marshalling.hpp>

#include <nil/crypto3/zk/snark/algorithms/generate.hpp>
#include <nil/crypto3/zk/snark/algorithms/verify.hpp>
#include <nil/crypto3/zk/snark/algorithms/prove.hpp>

#include <nil/crypto3/algebra/curves/bls12.hpp>
#include <nil/crypto3/algebra/fields/bls12/base_field.hpp>
#include <nil/crypto3/algebra/fields/bls12/scalar_field.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/bls12.hpp>
#include <nil/crypto3/algebra/curves/params/multiexp/bls12.hpp>
#include <nil/crypto3/algebra/curves/params/wnaf/bls12.hpp>
#include "detail/components.hpp"

typedef algebra::curves::bls12<381> curve_type;
typedef curve_type::scalar_field_type field_type;
typedef field_type::value_type value_type;
typedef zk::snark::r1cs_gg_ppzksnark<curve_type> scheme_type;

using namespace nil::crypto3::zk::components;
using namespace nil::crypto3::zk::snark;

boost::filesystem::path PRIMARY_KEY_PATH = "prov_key",
    VERIFICATION_KEY_PATH = "ver_key",
    PROOF_PATH = "proof";

std::vector<std::uint8_t> readfile(boost::filesystem::path path) {
    boost::filesystem::ifstream stream(path, std::ios::in | std::ios::binary);
    auto eos = std::istreambuf_iterator<char>();
    auto buffer = std::vector<uint8_t>(std::istreambuf_iterator<char>(stream), eos);
    return buffer;
}

bool generate_keys() {
    blueprint<field_type> bp;
    contest::bank_component<field_type> contest_component(bp);
    contest_component.generate_r1cs_constraints();

    const r1cs_constraint_system<field_type> constraint_system = bp.get_constraint_system();

    scheme_type::keypair_type keypair = generate<scheme_type>(constraint_system);

    std::vector<std::uint8_t> proving_key_byteblob =
        nil::marshalling::verifier_input_serializer_tvm<scheme_type>::process(keypair.first);
    std::vector<std::uint8_t> verification_key_byteblob =
        nil::marshalling::verifier_input_serializer_tvm<scheme_type>::process(keypair.second);

    boost::filesystem::ofstream pk_out(PRIMARY_KEY_PATH);
    for (const auto &v : proving_key_byteblob) {
        pk_out << v;
    }
    pk_out.close();
    std::cout << "prooving key saved to " << PRIMARY_KEY_PATH << std::endl;

    boost::filesystem::ofstream vk_out(VERIFICATION_KEY_PATH );
    for (const auto &v : verification_key_byteblob) {
        vk_out << v;
    }
    vk_out.close();
    std::cout << "verification key saved to " << VERIFICATION_KEY_PATH << std::endl;

    return true;
}

bool generate_proof(uint min_salary, uint min_age, uint salary, uint age) {
    std::vector<std::uint8_t> proving_key_byteblob = readfile(PRIMARY_KEY_PATH);
    nil::marshalling::status_type provingProcessingStatus = nil::marshalling::status_type::success;
    typename scheme_type::proving_key_type pk = nil::marshalling::verifier_input_deserializer_tvm<scheme_type>::proving_key_process(
        proving_key_byteblob.cbegin(),
        proving_key_byteblob.cend(),
        provingProcessingStatus);

    blueprint<field_type> bp;
    contest::bank_component<field_type> contest_component(bp);
    contest_component.generate_r1cs_constraints();
    contest_component.generate_r1cs_witness(min_salary, min_age, salary, age);

    std::cout << "Circuit satisfied: " << bp.is_satisfied() << std::endl;
    if (!bp.is_satisfied()) {
        return false;
    }

    const scheme_type::proof_type proof = prove<scheme_type>(pk, bp.primary_input(), bp.auxiliary_input());

    std::vector<std::uint8_t> proof_byteblob =
        nil::marshalling::verifier_input_serializer_tvm<scheme_type>::process(proof);

    std::cout << "proof is saved to " << PROOF_PATH << std::endl;
    boost::filesystem::ofstream proof_out(PROOF_PATH);
    for (const auto &v : proof_byteblob) {
        proof_out << v;
    }
    proof_out.close();

    return true;
}

bool verify_proof(uint min_salary, uint min_age) {
    std::vector<std::uint8_t> proof_byteblob = readfile(PROOF_PATH);
    nil::marshalling::status_type proofProcessingStatus = nil::marshalling::status_type::success;
    typename scheme_type::proof_type proof = nil::marshalling::verifier_input_deserializer_tvm<scheme_type>::proof_process(
        proof_byteblob.cbegin(),
        proof_byteblob.cend(),
        proofProcessingStatus);

    std::vector<std::uint8_t> verification_key_byteblob = readfile(VERIFICATION_KEY_PATH);
    nil::marshalling::status_type verificationProcessingStatus = nil::marshalling::status_type::success;
    typename scheme_type::verification_key_type vk = nil::marshalling::verifier_input_deserializer_tvm<scheme_type>::verification_key_process(
        verification_key_byteblob.cbegin(),
        verification_key_byteblob.cend(),
        verificationProcessingStatus );

    r1cs_primary_input<field_type> input = contest::get_public_input<field_type>(min_salary, min_age);
    using basic_proof_system = r1cs_gg_ppzksnark<curve_type>;
    const bool verified = verify<basic_proof_system>(vk, input, proof);
    std::cout << "proof verified " << verified << std::endl;

    return verified;
}

int main(int argc, char *argv[]) {
    uint salary, age;
    uint min_salary, min_age;

    boost::program_options::options_description options(
        "R1CS Generic Group PreProcessing Zero-Knowledge Succinct Non-interactive ARgument of Knowledge "
        "(https://eprint.iacr.org/2016/260.pdf) CLI Proof Generator");
    options.add_options()
    ("help,h", "Display help message")
    ("keygen", "Generate keys")
    ("proof", "Generate proof")
    ("verify", "Verify proof")
    ("min-age,x", boost::program_options::value<uint>(&min_age)->default_value(18))
    ("min-salary,y", boost::program_options::value<uint>(&min_salary)->default_value(1000))
    ("age,a", boost::program_options::value<uint>(&age)->default_value(0))
    ("salary,s", boost::program_options::value<uint>(&salary)->default_value(0));

    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(options).run(), vm);
    boost::program_options::notify(vm);

    if (vm.count("help") || argc < 2) {
        std::cout << options << std::endl;
        return 0;
    } else if (vm.count("keygen")) {
        generate_keys();
    } else if (vm.count("proof")) {
        generate_proof(min_salary, min_age, salary, age);
    } else if (vm.count("verify")) {
        verify_proof(min_salary, min_age);
    }
    return 0;
}


//---------------------------------------------------------------------------//
//!
//! \file   DataGen_AdjointBremsstrahlungCrossSectionEvaluator.cpp
//! \author Luke Kersting
//! \brief  Adjoint bremsstrahlung cross section evaluator definition
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <limits>

// Boost Includes
#include <boost/function.hpp>
#include <boost/bind.hpp>

// FRENSIE Includes
#include "DataGen_AdjointBremsstrahlungCrossSectionEvaluator.hpp"
#include "Utility_GaussKronrodIntegrator.hpp"
#include "Utility_PhysicalConstants.hpp"
#include "Utility_ContractException.hpp"
#include "MonteCarlo_TwoDDistributionHelpers.hpp"

namespace DataGen{

// Constructor
AdjointBremsstrahlungCrossSectionEvaluator::AdjointBremsstrahlungCrossSectionEvaluator(
    Teuchos::RCP<MonteCarlo::ElectroatomicReaction>& bremsstrahlung_reaction,
    const BremsstrahlungDistribution& energy_loss_distribution )
  : d_bremsstrahlung_reaction( bremsstrahlung_reaction ),
    d_energy_loss_distribution( energy_loss_distribution )
{
  // Make sure the data is valid
  testPrecondition( !d_bremsstrahlung_reaction.is_null() );
  testPrecondition( d_energy_loss_distribution.size() > 0 );
}

// Evaluate the differential adjoint bremsstrahlung cross section (dc/dx)
double AdjointBremsstrahlungCrossSectionEvaluator::evaluateDifferentialCrossSection(
	  const double incoming_energy,
          const double outgoing_energy ) const
{
  // Make sure the energies are valid
  testPrecondition( incoming_energy > 0.0 );
  testPrecondition( outgoing_energy > 0.0 );

  // Evaluate the forward cross section at the incoming energy
  double forward_cs = d_bremsstrahlung_reaction->getCrossSection( incoming_energy );

  // Evaluate the energy loss distribution at a given incoming and outgoing energy
  double forward_pdf = MonteCarlo::evaluateTwoDDistributionCorrelatedPDF(
                                     incoming_energy,
                                     outgoing_energy,
                                     d_energy_loss_distribution );

  return forward_cs*forward_pdf;
}

// Return the cross section value at a given energy
double AdjointBremsstrahlungCrossSectionEvaluator::evaluateCrossSection(
                               const double energy,
			       const double precision ) const
{
  // Make sure the energies are valid
  testPrecondition( energy > 0.0 );

  double cross_section = 0.0;

  // Create boost rapper function for the adjoint Bremsstahlung differential cross section
  boost::function<double (double x)> diff_adjoint_brem_wrapper =
    boost::bind<double>( &AdjointBremsstrahlungCrossSectionEvaluator::evaluateDifferentialCrossSection,
                         boost::cref( *this ),
                         _1,
                         energy );

    double abs_error;

    Utility::GaussKronrodIntegrator integrator( precision );

    integrator.integrateAdaptively<15>(
					diff_adjoint_brem_wrapper,
					d_energy_loss_distribution.front().first,
					d_energy_loss_distribution.back().first,
					cross_section,
					abs_error );
}

} // end DataGen namespace

//---------------------------------------------------------------------------//
// end DataGen_AdjointBremsstrahlungCrossSectionEvaluator.cpp
//---------------------------------------------------------------------------//


#define EIGEN_DONT_PARALLELIZE
#ifndef _MSC_VER
#include "update_ops_cpp.hpp"
extern "C" {
#include "utility.h"
#include "update_ops.h"
}
#else
#include "update_ops_cpp.hpp"
#include "update_ops.h"
#include "utility.h"
#endif
#include <Eigen/Core>

void double_qubit_dense_matrix_gate(UINT target_qubit_index1, UINT target_qubit_index2, const CTYPE matrix[16], CTYPE *state, ITYPE dim) {
	double_qubit_dense_matrix_gate_c(target_qubit_index1, target_qubit_index2, matrix, state, dim);
}

void double_qubit_dense_matrix_gate(UINT target_qubit_index1, UINT target_qubit_index2, const Eigen::Matrix4cd& eigen_matrix, CTYPE *state, ITYPE dim) {
	double_qubit_dense_matrix_gate_eigen(target_qubit_index1, target_qubit_index2, eigen_matrix, state, dim);
}

void double_qubit_dense_matrix_gate_eigen(UINT target_qubit_index1, UINT target_qubit_index2, const Eigen::Matrix4cd& eigen_matrix, CTYPE *state, ITYPE dim) {
	// target mask

	const UINT min_qubit_index = get_min_ui(target_qubit_index1, target_qubit_index2);
	const UINT max_qubit_index = get_max_ui(target_qubit_index1, target_qubit_index2);
	const ITYPE min_qubit_mask = 1ULL << min_qubit_index;
	const ITYPE max_qubit_mask = 1ULL << (max_qubit_index - 1);
	const ITYPE low_mask = min_qubit_mask - 1;
	const ITYPE mid_mask = (max_qubit_mask - 1) ^ low_mask;
	const ITYPE high_mask = ~(max_qubit_mask - 1);

	const ITYPE target_mask1 = 1ULL << target_qubit_index1;
	const ITYPE target_mask2 = 1ULL << target_qubit_index2;
	std::complex<double>* eigen_state = reinterpret_cast<std::complex<double>*>(state);

	// loop variables
	const ITYPE loop_dim = dim / 4;
	ITYPE state_index;

	for (state_index = 0; state_index < loop_dim; ++state_index) {
		// create index
		ITYPE basis_0 = (state_index&low_mask)
			+ ((state_index&mid_mask) << 1)
			+ ((state_index&high_mask) << 2);

		// gather index
		ITYPE basis_1 = basis_0 + target_mask1;
		ITYPE basis_2 = basis_0 + target_mask2;
		ITYPE basis_3 = basis_1 + target_mask2;

		// fetch values
		Eigen::Vector4cd vec(state[basis_0], state[basis_1], state[basis_2], state[basis_3]);
		vec = eigen_matrix * vec;
		eigen_state[basis_0] = vec[0];
		eigen_state[basis_1] = vec[1];
		eigen_state[basis_2] = vec[2];
		eigen_state[basis_3] = vec[3];
	}
}


/* Copyright 2020, 2021 Evandro Chagas Ribeiro da Rosa <evandro.crr@posgrad.ufsc.br>
 * Copyright 2020, 2021 Rafael de Santiago <r.santiago@ufsc.br>
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "../include/ket_bitwise.hpp"
#include <boost/container/map.hpp>
#include <random>
#include <algorithm>

using namespace ket;
using namespace std::complex_literals;

Bitwise::Bitwise() {
    qbits[Index()] = 1;
}

void Bitwise::x(size_t idx, const ctrl_list& ctrl) {
    map qbits_tmp{};
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec) {
            auto j = i.first;
            j.flip(idx);
            qbits_tmp[j] = i.second;
        } else {
            qbits_tmp[i.first] = i.second;
        }
    }
    qbits.swap(qbits_tmp);
}

void Bitwise::y(size_t idx, const ctrl_list& ctrl) {
    map qbits_tmp{};
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec) {
            auto j = i.first;
            j.flip(idx);
            if (i.first.is_one(idx)) {
                qbits_tmp[j] = i.second*-1i;
            } else {
                qbits_tmp[j] = i.second*1i;
            }
        } else {
            qbits_tmp[i.first] = i.second;
        }
    }
    qbits.swap(qbits_tmp);
}

void Bitwise::z(size_t idx, const ctrl_list& ctrl) {
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec and i.first.is_one(idx)) {
            qbits[i.first] *= -1;
        }
    }
}

void Bitwise::h(size_t idx, const ctrl_list& ctrl) {
    map qbits_tmp{};
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec) {
            if (i.first.is_one(idx)) {
                qbits_tmp[i.first] -= i.second/std::sqrt(2);
            } else {
                qbits_tmp[i.first] += i.second/std::sqrt(2);
            }
            if (std::abs(qbits_tmp[i.first]) < 1e-10) {
                qbits_tmp.erase(i.first);
            }
            auto j = i.first;
            j.flip(idx);
            qbits_tmp[j] += i.second/std::sqrt(2);
            if (std::abs(qbits_tmp[j]) < 1e-10) {
                qbits_tmp.erase(j);
            }
        } else {
            qbits_tmp[i.first] = i.second;
        }

    }
    qbits.swap(qbits_tmp);
}

void Bitwise::s(size_t idx, const ctrl_list& ctrl) {
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec and i.first.is_one(idx)) {
            qbits[i.first] *= 1i;
        }
    }
}

void Bitwise::sd(size_t idx, const ctrl_list& ctrl) {
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec and i.first.is_one(idx)) {
            qbits[i.first] *= -1i;
        }
    }
}

void Bitwise::t(size_t idx, const ctrl_list& ctrl) {
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec and i.first.is_one(idx)) {
            qbits[i.first] *= std::exp(1i*M_PI/4.0);
        }
    }
}

void Bitwise::td(size_t idx, const ctrl_list& ctrl) {
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec and i.first.is_one(idx)) {
            qbits[i.first] *= std::exp(-1i*M_PI/4.0);
        }
    }
}

void Bitwise::cnot(size_t ctrl, size_t target,  const ctrl_list& ctrl2) {
    map qbits_tmp{};

    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl2) exec &= i.first.is_one(j);
        if (exec and i.first.is_one(ctrl)) {
            auto j = i.first;
            j.flip(target);
            qbits_tmp[j] = i.second;
        } else {
            qbits_tmp[i.first] = i.second;
        }
    }

    qbits.swap(qbits_tmp);
}

void Bitwise::p(double lambda, size_t idx, const ctrl_list& ctrl) {
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec and i.first.is_one(idx)) {
            qbits[i.first] *= std::exp(1i*lambda);
        }
    }
}

void Bitwise::u2(double phi, double lambda, size_t idx, const ctrl_list& ctrl) {
    map qbits_tmp{};
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec) {
            auto j = i.first;
            j.flip(idx);
            if (i.first.is_one(idx)) {
                qbits_tmp[i.first] += i.second*std::exp(1i*(lambda+phi))/std::sqrt(2);
                if (std::abs(qbits_tmp[i.first]) < 1e-10)
                    qbits_tmp.erase(i.first);

                qbits_tmp[j] -= i.second*std::exp(1i*lambda)/std::sqrt(2);
                if (std::abs(qbits_tmp[j]) < 1e-10)
                    qbits_tmp.erase(j);
            } else {
                qbits_tmp[i.first] += i.second/sqrt(2);
                if (std::abs(qbits_tmp[i.first]) < 1e-10)
                    qbits_tmp.erase(i.first);

                qbits_tmp[j] += i.second*std::exp(1i*phi)/std::sqrt(2);
                if (std::abs(qbits_tmp[j]) < 1e-10)
                    qbits_tmp.erase(j);
            }

        } else {
            qbits_tmp[i.first] = i.second;
        }
    }

    qbits.swap(qbits_tmp);
}

void Bitwise::u3(double theta, double phi, double lambda, size_t idx, const ctrl_list& ctrl) {
    map qbits_tmp{};
    std::complex<double> amp;
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec) {
            auto j = i.first;
            j.flip(idx);
            if (i.first.is_one(idx)) {
                amp = std::exp(1i*(lambda+phi))*std::cos(theta/2);
                qbits_tmp[i.first] += i.second*amp;
                if (std::abs(qbits_tmp[i.first]) < 1e-10)
                    qbits_tmp.erase(i.first);

                amp = std::exp(1i*lambda)*std::sin(theta/2);
                qbits_tmp[j] -= i.second*amp;
                if (std::abs(qbits_tmp[j]) < 1e-10)
                    qbits_tmp.erase(j);
            } else {
                amp = std::cos(theta/2);
                qbits_tmp[i.first] += i.second*amp;
                if (std::abs(qbits_tmp[i.first]) < 1e-10)
                    qbits_tmp.erase(i.first);

                amp = std::exp(1i*phi)*std::sin(theta/2);
                qbits_tmp[j] += i.second*amp;
                if (std::abs(qbits_tmp[j]) < 1e-10)
                    qbits_tmp.erase(j);
            }
        } else {
            qbits_tmp[i.first] = i.second;
        }
    }

    qbits.swap(qbits_tmp);
}

void Bitwise::rx(double theta, size_t idx, const ctrl_list& ctrl) {
    u3(theta, -M_PI_2, M_PI_2, idx, ctrl);
}

void Bitwise::ry(double theta, size_t idx, const ctrl_list& ctrl) {
    u3(theta, 0, 0, idx, ctrl);
}

void Bitwise::rz(double lambda, size_t idx, const ctrl_list& ctrl) {
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec) {
            if (i.first.is_one(idx))
                qbits[i.first] *= std::exp(1i*lambda/2.0);
            else
                qbits[i.first] *= std::exp(-1i*lambda/2.0);
        }
    }
}

int Bitwise::measure(size_t idx) {
    double p = 0;

    for (auto &i : qbits) {
        if (i.first.is_zero(idx)) {
            p += std::pow(std::abs(i.second), 2);
        }
    }

    auto result = p != 0 and
                  (double(std::rand()) / double(RAND_MAX) <= p)?
                  0 : 1;

    p = result == 0? std::sqrt(p) : std::sqrt(1.0-p);

    map qbits_tmp{};

    for (auto &i : qbits)
        if (i.first.is_zero(idx) xor result)
            qbits_tmp[i.first] = i.second/p;

    qbits.swap(qbits_tmp);
    return result;
}

void Bitwise::measure_zero(size_t idx) {
    double p = 0;

    for (auto &i : qbits)
        if (i.first.is_zero(idx))
            p += std::pow(std::abs(i.second), 2);

    auto result = p != 0 and
                  (double(std::rand()) / double(RAND_MAX) <= p)?
                  0 : 1;

    p = result == 0? std::sqrt(p) : std::sqrt(1.0-p);

    map qbits_tmp{};

    for (auto &i : qbits) {
        if (i.first.is_zero(idx) xor result) {
            if (result == 0) {
                qbits_tmp[i.first] = i.second/p;
            } else {
                auto j = i.first;
                j.flip(idx);
                qbits_tmp[j] = i.second/p;
            }
        }
    }

    qbits.swap(qbits_tmp);
}

std::ostream& ket::operator<<(std::ostream &os, const Bitwise& q) {
    boost::container::map<Index, complex> sorted;
    sorted.insert(q.qbits.begin(), q.qbits.end());
    for (auto &i : sorted) {
        os << i.first << ' ' << i.second << std::endl;
    }
    return os;
}

Bitwise::Bitwise(const Bitwise& a, const Bitwise& b) {
    for (const auto &i: a.qbits) for (const auto &j: b.qbits)
        qbits[i.first|j.first] = i.second*j.second;
}

void Bitwise::swap(size_t a, size_t b) {
    map qbits_tmp{};
    for (auto &i : qbits) {
        if (i.first.is_one(a) != i.first.is_one(b)) {
            auto j = i.first;
            j.flip(a);
            j.flip(b);
            qbits_tmp[j] = i.second;
        } else {
            qbits_tmp[i.first] = i.second;
        }
    }
    qbits.swap(qbits_tmp);
}

map& Bitwise::get_map() {
    return qbits;
}

dump_t Bitwise::dump(size_t size) const {
    dump_t state;

    for (auto &i : qbits) {
        std::vector<uint64_t> tmp_state;
        for (auto j = 0ul; j < size/64; j++)
            tmp_state.push_back(i.first[j]);
        tmp_state.push_back(i.first[size/64] & ((1ul << size%64) -1));
        state[tmp_state].push_back(i.second);
    }
    for (auto &i : state) {
        std::sort(i.second.begin(), i.second.end(), [](std::complex<double> a, std::complex<double> b) {
            if (a.real() == b.real()) return a.imag() < b.imag();
            else return a.real() < b.real();
        });
    }

    return state;
}


/******************************************************************************

  This source file is part of the Avogadro project.

  Copyright 2012-2014 Kitware, Inc.

  This source code is released under the New BSD License, (the "License").

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.

******************************************************************************/

#include "camera.h"

#include <Eigen/LU>

#include <cmath>

namespace Avogadro {
namespace Rendering {

Camera::Camera() : m_width(0), m_height(0),
  m_projectionType(Perspective), m_orthographicScale(1.0)
{
  m_projection.setIdentity();
  m_modelView.setIdentity();
}

Camera::~Camera()
{
}

void Camera::translate(const Vector3f &translate_)
{
  m_modelView.translate(translate_);
}

void Camera::preTranslate(const Vector3f &translate_)
{
  m_modelView.pretranslate(translate_);
}

void Camera::rotate(float angle, const Vector3f &axis)
{
  m_modelView.rotate(Eigen::AngleAxisf(angle, axis));
}

void Camera::preRotate(float angle, const Vector3f &axis)
{
  m_modelView.prerotate(Eigen::AngleAxisf(angle, axis));
}

void Camera::scale(float s)
{
  if (m_projectionType == Perspective)
    m_modelView.scale(s);
  else
    m_orthographicScale *= s;
}

void Camera::lookAt(const Vector3f &eye, const Vector3f &center,
                    const Vector3f &up)
{
  Vector3f f = (center - eye).normalized();
  Vector3f u = up.normalized();
  Vector3f s = f.cross(u).normalized();
  u = s.cross(f);

  m_modelView.setIdentity();
  m_modelView(0, 0) = s.x();
  m_modelView(0, 1) = s.y();
  m_modelView(0, 2) = s.z();
  m_modelView(1, 0) = u.x();
  m_modelView(1, 1) = u.y();
  m_modelView(1, 2) = u.z();
  m_modelView(2, 0) =-f.x();
  m_modelView(2, 1) =-f.y();
  m_modelView(2, 2) =-f.z();
  m_modelView(0, 3) =-s.dot(eye);
  m_modelView(1, 3) =-u.dot(eye);
  m_modelView(2, 3) = f.dot(eye);
}

float Camera::distance(const Vector3f &point) const
{
  return (m_modelView * point).norm();
}

Vector3f Camera::project(const Vector3f &point) const
{
  Eigen::Matrix4f mvp = m_projection.matrix() * m_modelView.matrix();
  Vector4f tPoint(point.x(), point.y(), point.z(), 1.0f);
  tPoint = mvp * tPoint;
  Vector3f result(static_cast<float>(m_width)
                  * (tPoint.x() / tPoint.w() + 1.0f) / 2.0f,
                  static_cast<float>(m_height)
                  * (tPoint.y() / tPoint.w() + 1.0f) / 2.0f,
                  (tPoint.z() / tPoint.w() + 1.0f) / 2.0f);
  return result;
}

Vector3f Camera::unProject(const Vector3f &point) const
{
  Eigen::Matrix4f mvp = m_projection.matrix() * m_modelView.matrix();
  Vector4f result(2.0f * point.x() / static_cast<float>(m_width) - 1.0f,
                  2.0f * (static_cast<float>(m_height) - point.y()) /
                  static_cast<float>(m_height) - 1.0f,
                  2.0f * point.z() - 1.0f,
                  1.0f);
  result = mvp.matrix().inverse() * result;
  return Vector3f(result.x() / result.w(), result.y() / result.w(),
                  result.z() / result.w());
}

Vector3f Camera::unProject(const Vector2f &point,
                           const Vector3f &reference) const
{
  return unProject(Vector3f(point.x(), point.y(), project(reference).z()));
}

void Camera::calculatePerspective(float fieldOfView, float aspectRatio,
                                  float zNear, float zFar)
{
  m_projection.setIdentity();
  float f = 1.0f / std::tan(fieldOfView * float(M_PI) / 360.0f);
  m_projection(0, 0) = f / aspectRatio;
  m_projection(1, 1) = f;
  m_projection(2, 2) = (zNear + zFar) / (zNear - zFar);
  m_projection(2, 3) = (2.0f * zFar * zNear) / (zNear - zFar);
  m_projection(3, 2) = -1;
  m_projection(3, 3) = 0;
}

void Camera::calculatePerspective(float fieldOfView, float zNear, float zFar)
{
  calculatePerspective(fieldOfView, static_cast<float>(m_width) /
                       static_cast<float>(m_height), zNear, zFar);
}

void Camera::calculateOrthographic(float left, float right,
                                   float bottom, float top,
                                   float zNear, float zFar)
{
  left *= m_orthographicScale;
  right *= m_orthographicScale;
  bottom *= m_orthographicScale;
  top *= m_orthographicScale;
  m_projection.setIdentity();
  m_projection(0, 0) = 2.0f / (right - left);
  m_projection(0, 3) = -(right + left) / (right - left);
  m_projection(1, 1) = 2.0f / (top - bottom);
  m_projection(1, 3) = -(top + bottom) / (top - bottom);
  m_projection(2, 2) = -2.0f / (zFar - zNear);
  m_projection(2, 3) = -(zFar + zNear) / (zFar - zNear);
  m_projection(3, 3) = 1;
}

void Camera::setViewport(int w, int h)
{
  m_width = w;
  m_height = h;
}

void Camera::setProjection(const Eigen::Affine3f &transform)
{
  m_projection = transform;
}

void Camera::setModelView(const Eigen::Affine3f &transform)
{
  m_modelView = transform;
}

} // End Rendering namespace
} // End Avogadro namespace


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2015 Johannes Goettker-Schnetmann
 Copyright (C) 2015 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file hestonblackvolsurface.hpp
    \brief Black volatility surface back by Heston model
*/

#include <ql/math/functional.hpp>
#include <ql/math/solvers1d/brent.hpp>
#include <ql/time/calendars/nullcalendar.hpp>
#include <ql/pricingengines/blackformula.hpp>
#include <ql/termstructures/volatility/equityfx/hestonblackvolsurface.hpp>

#include <boost/bind.hpp>
#include <boost/make_shared.hpp>

#include <limits>

namespace QuantLib {

    namespace {
        Real blackValue(Option::Type optionType, Real strike,
                        Real forward, Real maturity,
                        Volatility vol, Real discount, Real npv) {

            return blackFormula(optionType, strike, forward,
                                std::max(0.0, vol)*std::sqrt(maturity),
                                discount)-npv;
        }
    }

    HestonBlackVolSurface::HestonBlackVolSurface(
        const Handle<HestonModel>& hestonModel)
    : BlackVolTermStructure(
          hestonModel->process()->riskFreeRate()->referenceDate(),
          NullCalendar(),
          Following,
          hestonModel->process()->riskFreeRate()->dayCounter()),
      hestonModel_(hestonModel),
      integration_(AnalyticHestonEngine::Integration::gaussLaguerre(164)) {
        registerWith(hestonModel_);
    }

    DayCounter HestonBlackVolSurface::dayCounter() const {
        return hestonModel_->process()->riskFreeRate()->dayCounter();
    }
    Date HestonBlackVolSurface::maxDate() const {
        return Date::maxDate();
    }
    Real HestonBlackVolSurface::minStrike() const {
        return 0.0;
    }
    Real HestonBlackVolSurface::maxStrike() const {
        return std::numeric_limits<Real>::max();
    }

    Real HestonBlackVolSurface::blackVarianceImpl(Time t, Real strike) const {
        return square<Real>()(blackVolImpl(t, strike))*t;
    }

    Volatility HestonBlackVolSurface::blackVolImpl(Time t, Real strike) const {
        const boost::shared_ptr<HestonProcess> process = hestonModel_->process();

        const DiscountFactor df = process->riskFreeRate()->discount(t, true);
        const DiscountFactor div = process->dividendYield()->discount(t, true);
        const Real spotPrice = process->s0()->value();

        const Real fwd = spotPrice
            * process->dividendYield()->discount(t, true)
            / process->riskFreeRate()->discount(t, true);

        const PlainVanillaPayoff payoff(
            fwd > strike ? Option::Put : Option::Call, strike);

        const Real kappa = hestonModel_->kappa();
        const Real theta = hestonModel_->theta();
        const Real rho   = hestonModel_->rho();
        const Real sigma = hestonModel_->sigma();
        const Real v0    = hestonModel_->v0();

        const AnalyticHestonEngine::ComplexLogFormula cpxLogFormula
            = AnalyticHestonEngine::Gatheral;

        const AnalyticHestonEngine* const hestonEnginePtr = 0;

        Real npv;
        Size evaluations;

        AnalyticHestonEngine::doCalculation(
            df, div, spotPrice, strike, t,
            kappa, theta, sigma, v0, rho,
            payoff, integration_, cpxLogFormula,
            hestonEnginePtr, npv, evaluations);

        if (npv <= 0.0) return std::sqrt(theta);

        Brent solver;
        solver.setMaxEvaluations(10000);
        const Volatility guess = std::sqrt(theta);
        const Real accuracy = std::numeric_limits<Real>::epsilon();

        const boost::function<Real(Real)> f = boost::bind(
            &blackValue, payoff.optionType(), strike, fwd, t, _1, df, npv);

        return solver.solve(f, accuracy, guess, 0.01);
    }
}


/** * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 * @file ClothoidSpline-LMSolver.cc
 * @author Matteo Ragni (info@ragni.me)
 *
 * @copyright Copyright (c) 2022 Matteo Ragni
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifdef G2LIB_LMSOLVE_CLOTHOID_SPLINE

#include "Clothoids/ClothoidSpline-Interpolation.hxx"

#include <vector>
#include <algorithm>
#include <stdexcept>
#include <Eigen/Eigen>
#include <unsupported/Eigen/LevenbergMarquardt>

namespace G2lib {
  namespace Interpolation {

    using G2lib::ClothoidSplineG2;
    using G2lib::int_type;
    using G2lib::real_type;
    typedef Eigen::SparseFunctor<real_type, int_type> SparseFunctor;

    /* Levemberg Marquardt Solver */
    class LMSolver : public Solver {
      struct ClothoidSplineProblem : SparseFunctor {
        LMSolver &             m_solver;
        std::vector<int_type>  m_jacobian_rows;
        std::vector<int_type>  m_jacobian_cols;
        std::vector<real_type> m_jacobian_result;

        ClothoidSplineProblem(LMSolver & solver);
        int operator()(const SparseFunctor::InputType & theta, SparseFunctor::ValueType & constraints_value) const;
        int df(const SparseFunctor::InputType & theta, SparseFunctor::JacobianType & jacobian_value);
      };

     public:
      LMSolver(const ClothoidSplineG2 & spline) : Solver(spline) {};
      virtual Result solve() override;
    };

    LMSolver::ClothoidSplineProblem::ClothoidSplineProblem(LMSolver & solver)
        : SparseFunctor(solver.theta_size(), solver.constraints_size()), m_solver(solver),
          m_jacobian_rows(std::vector<int_type>(solver.jacobian_pattern_size(), 0)),
          m_jacobian_cols(std::vector<int_type>(solver.jacobian_pattern_size(), 0)),
          m_jacobian_result(std::vector<real_type>(solver.jacobian_pattern_size(), 0.0)) {
      m_solver.spline().jacobian_pattern(&m_jacobian_rows.front(), &m_jacobian_cols.front());
    }

    int LMSolver::ClothoidSplineProblem::operator()(
        const SparseFunctor::InputType & theta, SparseFunctor::ValueType & constraints_value) const {
      if (m_solver.spline().constraints(theta.data(), constraints_value.data()))
        return 0;
      return 1;
    }

    int LMSolver::ClothoidSplineProblem::df(
        const SparseFunctor::InputType & theta, SparseFunctor::JacobianType & jacobian_value) {
      if (!(m_solver.spline().jacobian(theta.data(), &m_jacobian_result.front())))
        return 1;
      for (int i = 0; i < m_solver.jacobian_pattern_size(); i++) {
        jacobian_value.coeffRef(m_jacobian_rows[i], m_jacobian_cols[i]) = m_jacobian_result[i];
      }
      jacobian_value.makeCompressed();
      return 0;
    }

    Result LMSolver::solve() {
      LMSolver::ClothoidSplineProblem problem(*this);
      Eigen::LevenbergMarquardt<LMSolver::ClothoidSplineProblem> lm(problem);
      lm.setFtol(1e-20);

      Eigen::VectorXd theta_opts = Eigen::VectorXd::Map(theta_solution().data(), theta_solution().size());
      lm.minimize(theta_opts);
      Eigen::ComputationInfo info = lm.info();
      std::copy(theta_opts.data(), theta_opts.data() + theta_opts.size(), theta_solution().begin());

      switch (info) {
        case (Eigen::Success):
          return Result(ResultType::Success, lm.fnorm(), static_cast<int_type>(lm.iterations()));
          break;
        case (Eigen::NumericalIssue):
          return Result(ResultType::NumericalIssue, lm.fnorm(), static_cast<int_type>(lm.iterations()));
          break;
        case (Eigen::NoConvergence):
          return Result(ResultType::NoConvergence, lm.fnorm(), static_cast<int_type>(lm.iterations()));
          break;
        case (Eigen::InvalidInput):
          return Result(ResultType::InvalidInput, lm.fnorm(), static_cast<int_type>(lm.iterations()));
          break;
      }
      return Result(ResultType::InternalError);
    }

    Result Interpolator::buildP1(real_type theta_0, real_type theta_1, ClothoidList & result) {
      m_spline.setP1(theta_0, theta_1);
      build_clothoid_spline();
      LMSolver solver(m_spline);
      solver.guess();
      auto status = solver.solve();
      build_clothoid_list(solver.theta_solution(), result);
      return status;
    }

    Result Interpolator::buildP2(ClothoidList & result) {
      m_spline.setP2();
      build_clothoid_spline();
      LMSolver solver(m_spline);
      solver.guess();
      auto status = solver.solve();
      build_clothoid_list(solver.theta_solution(), result);
      return status;
    }

  } /* namespace Interpolation */
} /* namespace G2lib */

#endif


#pragma comment(linker, "/STACK:100000000")
#pragma comment(linker, "/HEAP:1000000000")

#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include "Lookup.h"
#include <random>
#include <string>
#include <algorithm>
#include <thread>
#include <iterator>
using std::string;
using Eigen::MatrixXd;
using Eigen::VectorXd;
#include <chrono>

float Q_rsqrt(float number)
{
	long i;
	float x2, y;
	const float threehalfs = 1.5F;

	x2 = number * 0.5F;
	y = number;
	i = *(long *)&y;                       // evil floating point bit level hacking
	i = 0x5f3759df - (i >> 1);               // what the fuck?
	y = *(float *)&i;
	y = y * (threehalfs - (x2 * y * y));   // 1st iteration
										   //	y  = y * ( threehalfs - ( x2 * y * y ) );   // 2nd iteration, this can be removed

	return y;
}

void call_from_thread() {
	std::cout << "Hello, World" << std::endl;

}
float get_random(float min, float max)
{
	static std::mt19937_64 rng(std::chrono::system_clock::now().time_since_epoch().count());
	std::uniform_real_distribution<> dis(min, max);
	return dis(rng);
}

string GetInput(string message) {
	printf(message.c_str());
	string response;
	std::getline(std::cin, response);

	return response;
}

void Write(string filename, string value) {
	std::ofstream file;
	file.open(filename);
	file << value.c_str();
	file.close();
}

string Read(string filename) {
	{
		string line;
		std::ifstream input(filename);
		if (input.is_open())
		{
			std::stringstream strStream;
			strStream << input.rdbuf();//read the file
			return strStream.str();//str holds the content of the file
		}
		else
		{
			return "";
		}
	}
}

void MakeLower(string& value) {
	std::transform(value.begin(), value.end(), value.begin(), ::tolower);
}

int main()
{
	auto inputFile = GetInput("Please enter full filename to train chain from : \n");
	auto outputFile = GetInput("Please enter full filename set output to : \n");

	auto defaults = split(Read("default.ini"), v<string>("\n"));
	bool usedDefaults = false;

	if (inputFile == "") {
		inputFile = defaults[0];
		bool usedDefaults = true;
	}
	if (outputFile == "") {
		outputFile = defaults[1];
		bool usedDefaults = true;
	}

	if (!usedDefaults)
		Write("default.ini", inputFile + "\n" + outputFile);

	string trainingText = "";

	{
		string line;
		std::ifstream input(inputFile);
		if (input.is_open())
		{
			std::stringstream strStream;
			strStream << input.rdbuf();//read the file
			trainingText = strStream.str();//str holds the content of the file
		}
		else {
			return 0;
		}
	}
	auto words = split(trainingText, v<string>(" ", "	 ", ".", ",", "(", ")", ";", "\"", "-", "!", ":", "?"));

	MatrixXd m(2, 2);
	m(0, 0) = 3;
	m(1, 0) = 2.;
	m(0, 1) = -1;
	m(1, 1) = m(1, 0) + m(0, 1);
	std::cout << m << std::endl;
	Lookup lookup;
	auto result = lookup.Convert(words);
	auto resultSize = (int)result.size();
	auto lookupSize = (int)lookup.size();

	MatrixXd data = MatrixXd::Zero(lookupSize, lookupSize);

	for (int index = 0; index < resultSize; index++) {
		for (int subIndex = std::max(0, index - 5); subIndex < std::min(resultSize, index + 30); subIndex++) {
			double weight = 5;
			if (subIndex < index)
				weight = 0.2;
			else if (subIndex == index)
				continue;

			//auto change = weight / pow(index - subIndex, 2);
			data(result[subIndex], result[index]) += weight / pow(abs(index - subIndex), 1.2);
		//	data(result[subIndex], result[index]) = std::max(0.0, data(result[subIndex], result[index]));
		}
	}

	for (int index = 0; index < data.cols(); index++) {
		data.col(index) /= data.col(index).sum();
	}

	VectorXd input = VectorXd::Zero(lookupSize);
	input[0] = 1;

	vector<int> textOutput(500);

	string finalResult = "";
	for (int i = 0; i < 500; i++) {
		input = data * input;
		//auto max = input[0];
		//max = input[0];
		//auto maxIndex = 0;

		//for (int index = 1; index < lookupSize; index++) {
		//	maxIndex = input[index] > max ? index : maxIndex;

		//	if (maxIndex == index) {
		//		max = input[index];
		//	}
		//}

		float potSize = (float)input.sum();
		auto random = (double)get_random(0, potSize);

		int index = 0;
		for (double cursor = 0; cursor + input[index] < random; cursor += input[index]) {
			index++;
		}

		input[index] = 1;

		textOutput[i] = index;
		finalResult = join(lookup.Extract<string>(textOutput, ""), " ");
		std::cout << finalResult + "\n";
	}

	Write(outputFile, finalResult);

	int i = 0;
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      110527    L. van der Ham    File created.
 *      110602    L. van der Ham    Made LibrationPoints class and added comments.
 *      110707    L. van der Ham    Make code compatible with Tudat revision 114.
 *      110629    L. van der Ham    Modifications according to comments first code check.
 *      110710    K. Kumar          Removed duplicated code; modified libration point
 *                                  compute-functions; changed filename and class; L1 and L2
 *                                  function coefficient discrepancies spotted.
 *      110712    L. van der Ham    Changed L1, L2 and L3 function coefficients.
 *      110927    L. van der Ham    Reverted to full equations of motion for determination location
 *                                  of colinear libration points.
 *      111027    K. Kumar          Moved 1-line functions to header file.
 *      120307    K. Kumar          Moved file.
 *      120326    D. Dirkx          Changed raw pointers to shared pointers.
 *      120813    P. Musegaas       Changed code to new root finding structure.
 *
 *    References
 *      van der Ham, L. Interplanetary trajectory design using dynamical systems theory,
 *          MSc thesis, Delft University of Technology, Delft, The Netherlands, 2012.
 *      Mireles James, J.D. Celestial Mechanics Notes Set 4: The Circular Restricted Three Body
 *          Problem, 2006, http://www.math.utexas.edu/users/jjames/hw4Notes.pdf,
 *          last accessed: 18th May, 2012.
 *
 *    Notes
 *      WARNING: There seems to be a bug in the computation of the L3 location!
 *
 */

#include <cmath>
#include <iostream>
#include <stdexcept>

#include <boost/bind.hpp>
#include <boost/exception/all.hpp>

#include "Tudat/Astrodynamics/Gravitation/librationPoint.h"
#include "Tudat/Mathematics/BasicMathematics/functionProxy.h"

namespace tudat
{
namespace gravitation
{
namespace circular_restricted_three_body_problem
{

using namespace root_finders;
using namespace basic_mathematics;

//! Compute location of Lagrange libration point.
void LibrationPoint::computeLocationOfLibrationPoint(
        LagrangeLibrationPoints lagrangeLibrationPoint )
{
    using std::pow;
    using std::sqrt;

    // Set functions for Newton-Raphson based on collinear libration point passed as input
    // parameter, or computed locations directly of equilateral libration points.
    switch( lagrangeLibrationPoint )
    {
    case l1:
    {
        // Create an object containing the function of which we whish to obtain the root from.
        UnivariateProxyPointer rootFunction = boost::make_shared< UnivariateProxy >(
                    boost::bind( &LibrationPoint::computeL1LocationFunction, this, _1 ) );

        // Add the first derivative of the root function.
        rootFunction->addBinding( -1, boost::bind( &LibrationPoint::
                computeL1FirstDerivativeLocationFunction, this, _1 ) );

        // Set position vector of L1 in Cartesian elements based on result of Newton-Raphson
        // root-finding algorithm.
        positionOfLibrationPoint_ << rootFinder->execute( rootFunction, 1.0 ), 0.0, 0.0;
    }
        break;

    case l2:
    {
        // Create an object containing the function of which we whish to obtain the root from.
        UnivariateProxyPointer rootFunction = boost::make_shared< UnivariateProxy >(
                    boost::bind( &LibrationPoint::computeL2LocationFunction, this, _1 ) );

        // Add the first derivative of the root function.
        rootFunction->addBinding( -1, boost::bind( &LibrationPoint::
                computeL2FirstDerivativeLocationFunction, this, _1 ) );

        // Set position vector of L1 in Cartesian elements based on result of Newton-Raphson
        // root-finding algorithm.
        positionOfLibrationPoint_ << rootFinder->execute( rootFunction, 1.0 ), 0.0, 0.0;
    }
        break;

    case l3:
    {
        // Create an object containing the function of which we whish to obtain the root from.
        UnivariateProxyPointer rootFunction = boost::make_shared< UnivariateProxy >(
                    boost::bind( &LibrationPoint::computeL3LocationFunction, this, _1 ) );

        // Add the first derivative of the root function.
        rootFunction->addBinding( -1, boost::bind( &LibrationPoint::
                computeL3FirstDerivativeLocationFunction, this, _1 ) );

        // Set position vector of L1 in Cartesian elements based on result of Newton-Raphson
        // root-finding algorithm.
        positionOfLibrationPoint_ << rootFinder->execute( rootFunction, -1.0 ), 0.0, 0.0;
    }
        break;

    case l4:

        // Set position vector of L4 in Cartesian elements.
        positionOfLibrationPoint_.x( ) = 0.5 - massParameter;
        positionOfLibrationPoint_.y( ) = 0.5 * sqrt( 3.0 );
        positionOfLibrationPoint_.z( ) = 0.0;

        break;

    case l5:

        // Set position vector of L5 in Cartesian elements.
        positionOfLibrationPoint_.x( ) = 0.5 - massParameter;
        positionOfLibrationPoint_.y( ) = -0.5 * sqrt( 3.0 );
        positionOfLibrationPoint_.z( ) = 0.0;

        break;

    default:

        boost::throw_exception(
                    boost::enable_error_info(
                        std::runtime_error(
                            "The Lagrange libration point requested does not exist." ) ) );
    };
}

} // namespace circular_restricted_three_body_problem
} // namespace gravitation
} // namespace tudat


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__COMPAT__ODEINT_HPP_
#define SMOOTH__COMPAT__ODEINT_HPP_

/**
 * @file
 * @brief boost::odeint compatability header.
 */

#include <boost/numeric/odeint/algebra/operations_dispatcher.hpp>

#include "smooth/concepts.hpp"

namespace smooth
{

/**
 * @brief \p boost::odeint Stepper operations for Manifold types.
 *
 * \p boost::odeint Butcher tableaus are evaluated by weighted
 * calculations of the form y = Σ_{i=1}^n alpha_i x_i which are generically
 * implemented.
 *
 * However, for the special case of butcher tableaus it holds that alpha_1 = 1,
 * and furthermore x_1 is always of the state type while x_2 ... x_n are
 * of the derivative type. The scale sum can therefore be generalized to the Lie
 * group case as
 *
 *   y = x_1 * exp(Σ_{i=2}^n alpha_i x_i)
 *
 * The methods below inject those calculations into boost:odeint to enable
 * numerical integration on Lie groups. For succintness we implement a single
 * method using variadic templates.
 */
struct BoostOdeintOps
{
  /**
   * @brief Variadic scale_sum implementation.
   */
  template<typename ... Fac>
  struct scale_sum
  {
    //! Storage for scale sum weights.
    const std::tuple<Fac ...> m_alpha;

    //! Constructor for scale sum.
    scale_sum(Fac ... alpha)
    : m_alpha(alpha ...)
    {
      if (std::get<0>(m_alpha) != std::tuple_element_t<0, std::tuple<Fac...>>(1)) {
        throw std::runtime_error("BoostOdeintOps only valid for alpha1 = 1");
      }
    }

    //! Helper for scaled addition operation.
    template<typename ... Ts, std::size_t ... Is>
    auto helper(std::index_sequence<Is...>, const Ts & ... as)
    {
      // plus 1 since alpha1 = 1 is not included in Ts...
      return ((std::get<Is + 1>(m_alpha) * as) + ...);
    }

    //! Scaled addition operation.
    template<Manifold T1, Manifold T2, typename ... Ts>
    requires std::is_same_v<T1, T2>&&
    std::conjunction_v<std::is_same<typename T1::Tangent, Ts>...>
    void operator()(T1 & y, const T2 & x, const Ts & ... as)
    {
      y = x + helper(std::make_index_sequence<sizeof...(Ts)>(), as...);
    }

    //! Required typedef.
    using result_type = void;
  };

  // \cond
  template<typename Fac1, typename Fac2 = Fac1>
  using scale_sum2 = scale_sum<Fac1, Fac2>;

  template<typename Fac1, typename Fac2 = Fac1, typename Fac3 = Fac2>
  using scale_sum3 = scale_sum<Fac1, Fac2, Fac3>;

  template<typename Fac1, typename Fac2 = Fac1, typename Fac3 = Fac2, typename Fac4 = Fac3>
  using scale_sum4 = scale_sum<Fac1, Fac2, Fac3, Fac4>;

  template<typename Fac1, typename Fac2 = Fac1, typename Fac3 = Fac2, typename Fac4 = Fac3, typename Fac5 = Fac4>
  using scale_sum5 = scale_sum<Fac1, Fac2, Fac3, Fac4, Fac5>;

  template<typename Fac1, typename Fac2 = Fac1, typename Fac3 = Fac2, typename Fac4 = Fac3, typename Fac5 = Fac4, typename Fac6 = Fac5>
  using scale_sum6 = scale_sum<Fac1, Fac2, Fac3, Fac4, Fac5, Fac6>;

  template<typename Fac1, typename Fac2 = Fac1, typename Fac3 = Fac2, typename Fac4 = Fac3, typename Fac5 = Fac4, typename Fac6 = Fac5, typename Fac7 = Fac6>
  using scale_sum7 = scale_sum<Fac1, Fac2, Fac3, Fac4, Fac5, Fac6, Fac7>;

  template<typename Fac1, typename Fac2 = Fac1, typename Fac3 = Fac2, typename Fac4 = Fac3, typename Fac5 = Fac4, typename Fac6 = Fac5, typename Fac7 = Fac6, typename Fac8 = Fac7>
  using scale_sum8 = scale_sum<Fac1, Fac2, Fac3, Fac4, Fac5, Fac6, Fac7, Fac8>;

  template<typename Fac1, typename Fac2 = Fac1, typename Fac3 = Fac2, typename Fac4 = Fac3, typename Fac5 = Fac4, typename Fac6 = Fac5, typename Fac7 = Fac6, typename Fac8 = Fac7, typename Fac9 = Fac8>
  using scale_sum9 = scale_sum<Fac1, Fac2, Fac3, Fac4, Fac5, Fac6, Fac7, Fac8, Fac9>;

  template<typename Fac1, typename Fac2 = Fac1, typename Fac3 = Fac2, typename Fac4 = Fac3, typename Fac5 = Fac4, typename Fac6 = Fac5, typename Fac7 = Fac6, typename Fac8 = Fac7, typename Fac9 = Fac8, typename Fac10 = Fac9>
  using scale_sum10 = scale_sum<Fac1, Fac2, Fac3, Fac4, Fac5, Fac6, Fac7, Fac8, Fac9, Fac10>;

  template<typename Fac1, typename Fac2 = Fac1, typename Fac3 = Fac2, typename Fac4 = Fac3, typename Fac5 = Fac4, typename Fac6 = Fac5, typename Fac7 = Fac6, typename Fac8 = Fac7, typename Fac9 = Fac8, typename Fac10 = Fac9, typename Fac11 = Fac10>
  using scale_sum11 = scale_sum<Fac1, Fac2, Fac3, Fac4, Fac5, Fac6, Fac7, Fac8, Fac9, Fac10, Fac11>;

  template<typename Fac1, typename Fac2 = Fac1, typename Fac3 = Fac2, typename Fac4 = Fac3, typename Fac5 = Fac4, typename Fac6 = Fac5, typename Fac7 = Fac6, typename Fac8 = Fac7, typename Fac9 = Fac8, typename Fac10 = Fac9, typename Fac11 = Fac10, typename Fac12 = Fac11>
  using scale_sum12 = scale_sum<Fac1, Fac2, Fac3, Fac4, Fac5, Fac6, Fac7, Fac8, Fac9, Fac10, Fac11, Fac12>;

  template<typename Fac1, typename Fac2 = Fac1, typename Fac3 = Fac2, typename Fac4 = Fac3, typename Fac5 = Fac4, typename Fac6 = Fac5, typename Fac7 = Fac6, typename Fac8 = Fac7, typename Fac9 = Fac8, typename Fac10 = Fac9, typename Fac11 = Fac10, typename Fac12 = Fac11, typename Fac13 = Fac12>
  using scale_sum13 = scale_sum<Fac1, Fac2, Fac3, Fac4, Fac5, Fac6, Fac7, Fac8, Fac9, Fac10, Fac11, Fac12, Fac13>;

  template<typename Fac1, typename Fac2 = Fac1, typename Fac3 = Fac2, typename Fac4 = Fac3, typename Fac5 = Fac4, typename Fac6 = Fac5, typename Fac7 = Fac6, typename Fac8 = Fac7, typename Fac9 = Fac8, typename Fac10 = Fac9, typename Fac11 = Fac10, typename Fac12 = Fac11, typename Fac13 = Fac12, typename Fac14 = Fac13>
  using scale_sum14 = scale_sum<Fac1, Fac2, Fac3, Fac4, Fac5, Fac6, Fac7, Fac8, Fac9, Fac10, Fac11, Fac12, Fac13, Fac14>;
  // \endcond
};

}  // namespace smooth

/**
 * @brief SFINAE dispatcher for Manifold types.
 */
// \cond
template<smooth::LieGroup G>
struct boost::numeric::odeint::operations_dispatcher_sfinae<G, void>
{
  using operations_type = ::smooth::BoostOdeintOps;
};
// \endcond

#endif  // SMOOTH__COMPAT__ODEINT_HPP_


#ifndef ALEPH_MATH_BOOTSTRAP_HH__
#define ALEPH_MATH_BOOTSTRAP_HH__

#include <aleph/math/KahanSummation.hh>

#include <boost/math/distributions/students_t.hpp>

#include <algorithm>
#include <iterator>
#include <random>
#include <vector>

#include <cmath>

namespace aleph
{

namespace math
{

/**
  @class Bootstrap
  @brief Generic bootstrap functor

  This functor provides a generic interface for performing bootstrap
  operations on *arbitrary* data, using an *arbitrary* statistic for
  testing. Several convenience functions for estimating *confidence*
  values are provided.
*/

class Bootstrap
{
public:

  /**
    Given a range of data of some type, calculates a set of bootstrap replicates
    for a desired statistic. This function will not perform any type conversions
    in order to preserve all original types. The type of the output data depends
    on the return value type of the functor.

    @param[in]  numSamples samples Number of bootstrap samples
    @param[in]  begin      Input iterator to begin of data range
    @param[in]  end        Input iterator to end of data range
    @param[in]  functor    Functor for calculating a statistic on the replicate
    @param[out] result     Output iterator for storing the results
  */

  template <class InputIterator, class OutputIterator, class Functor>
  void makeReplicates( unsigned numSamples,
                       InputIterator begin, InputIterator end,
                       Functor functor,
                       OutputIterator result )
  {
    using SampleValueType  = typename std::iterator_traits<InputIterator>::value_type;
    using FunctorValueType = decltype( functor( begin, end ) );

    std::vector<SampleValueType> samples( begin, end );

    // We cannot continue anyway, so let's just be nice and stop. This
    // does *not* constitute an error condition, though, because users
    // might just be weird when calling this function with empty data.
    if( samples.empty() )
      return;

    std::random_device rd;
    std::mt19937 rng( rd() );

    std::uniform_int_distribution<std::size_t> distribution( 0, samples.size() - 1 );

    std::vector<FunctorValueType> replicates;
    replicates.reserve( numSamples );

    for( unsigned sampleIndex = 0; sampleIndex < numSamples; sampleIndex++ )
    {
      std::vector<SampleValueType> sample;
      sample.reserve( samples.size() );

      for( std::size_t i = 0; i < samples.size(); i++ )
        sample.push_back( samples.at( distribution( rng ) ) );

      replicates.push_back( functor( sample.begin(),
                                     sample.end() ) );
    }

    std::copy( replicates.begin(), replicates.end(), result );
  }

  /**
    Calculates a bootstrap estimate of the standard error of a test
    statistics on a data set.

    @param numSamples Number of bootstrap samples
    @param begin      Input iterator to begin of data range
    @param end        Input iterator to end of data range
    @param functor    Functor to describe the test statistics that is to be
                      calculated on the data range. The functor has to take
                      the `value_type` of the range as a parameter, because
                      the function will show the individual samples to it.\n

                      A good example of a functor is the following *mean*
                      functor:\n

    \code{.cpp}
    auto meanCalculation = [] ( auto begin, auto end )
    {
      using T  = typename std::iterator_traits<decltype(begin)>::value_type;
      auto sum = std::accumulate( begin, end, T() );

      return static_cast<double>( sum / static_cast<double>( std::distance(begin, end) ) );
    };
    \endcode
                     Note that the functor requires `C++14` because the use
                     of `auto` in a lambda expression.

    @returns Bootstrapped estimated of the standard error
  */

  template <class InputIterator, class Functor>
  double standardError( unsigned numSamples,
                        InputIterator begin, InputIterator end,
                        Functor functor )
  {
    using FunctorValueType = decltype( functor(begin, end) );

    std::vector<FunctorValueType> estimates;
    estimates.reserve( numSamples );

    this->makeReplicates( numSamples,
                          begin, end,
                          functor,
                          std::back_inserter( estimates ) );

    using namespace aleph::math;

    double mean  = static_cast<double>( accumulate_kahan( estimates.begin(), estimates.end(), FunctorValueType() ) );
    mean        /= numSamples;

    std::vector<double> sampleSquaredDeviations;
    sampleSquaredDeviations.reserve( numSamples );

    for( auto&& estimate : estimates )
    {
      auto delta  = mean - estimate;
      delta      *= delta;

      sampleSquaredDeviations.push_back( delta );
    }

    double sumSquaredDeviations
      = accumulate_kahan_sorted(
          sampleSquaredDeviations.begin(),
          sampleSquaredDeviations.end(),
          0.0 );

    sumSquaredDeviations /= (numSamples - 1);
    return std::sqrt( static_cast<double>( sumSquaredDeviations ) );
  }

  template <class InputIterator, class Functor>
  auto basicConfidenceInterval( unsigned numSamples,
                                double alpha,
                                InputIterator begin, InputIterator end,
                                Functor functor ) -> std::pair< decltype( functor(begin, end) ), decltype( functor(begin, end) ) >
  {
    auto theta             = functor( begin, end );
    using FunctorValueType = decltype( theta );

    std::vector<FunctorValueType> estimates;
    estimates.reserve( numSamples );

    this->makeReplicates( numSamples,
                          begin, end,
                          functor,
                          std::back_inserter( estimates ) );

    std::sort( estimates.begin(), estimates.end() );

    auto upperPercentile = alpha / 2;
    auto upperEstimate   = estimates.at( Bootstrap::index( numSamples, upperPercentile ) );
    auto lowerPercentile = 1 - upperPercentile;
    auto lowerEstimate   = estimates.at( Bootstrap::index( numSamples, lowerPercentile ) );

    return std::make_pair( 2*theta - lowerEstimate, 2*theta - upperEstimate );
  }

  template <class InputIterator, class Functor>
  auto percentileConfidenceInterval( unsigned numSamples,
                                     double alpha,
                                     InputIterator begin, InputIterator end,
                                     Functor functor ) -> std::pair< decltype( functor(begin, end) ), decltype( functor(begin, end) ) >
  {
    using FunctorValueType = decltype( functor( begin, end ) );

    std::vector<FunctorValueType> estimates;
    estimates.reserve( numSamples );

    this->makeReplicates( numSamples,
                          begin, end,
                          functor,
                          std::back_inserter( estimates ) );

    std::sort( estimates.begin(), estimates.end() );

    auto lowerPercentile = alpha / 2;
    auto lowerEstimate   = estimates.at( Bootstrap::index( numSamples, lowerPercentile ) );
    auto upperPercentile = 1 - lowerPercentile;
    auto upperEstimate   = estimates.at( Bootstrap::index( numSamples, upperPercentile ) );

    return std::make_pair( lowerEstimate, upperEstimate );
  }

  template <class InputIterator, class Functor>
  auto studentConfidenceInterval( unsigned numSamples,
                                  double alpha,
                                  InputIterator begin, InputIterator end,
                                  Functor functor ) -> std::pair< decltype( functor(begin, end) ), decltype( functor(begin, end) ) >
  {
    auto theta = functor( begin, end );

    boost::math::students_t distribution( static_cast<double>( std::distance( begin, end ) - 1 ) );

    auto tl = boost::math::quantile( distribution, 1 - alpha );
    auto tu = boost::math::quantile( distribution,     alpha );
    auto se = this->standardError( numSamples,
                                   begin, end,
                                   functor );

    return std::make_pair( theta - tl * se, theta - tu * se );
  }

  /** Calculates index at a certain percentile of the data */
  static unsigned index( unsigned int samples, double alpha )
  {
    // This accounts for rounding and works regardless of whether
    // the product samples * alpha is an integer or not. Note the
    // offset of -1. It is required because, say, the 100th value
    // is at index 99 of the vector.
    return static_cast<unsigned>( std::ceil( samples * alpha ) ) - 1;
  }
};

} // namespace math

} // namespace aleph

#endif


///////////////////////////////////////////////////////////////////////////////
// kernel::functional::nw_visitor.hpp                                        //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_KERNEL_ESTIMATION_NW_VISITOR_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_KERNEL_ESTIMATION_NW_VISITOR_HPP_ER_2009
#include <boost/type_traits/is_reference.hpp>
#include <boost/mpl/not.hpp>
#include <boost/call_traits.hpp>
#include <boost/statistics/detail/kernel/estimation/detail/mean_accumulator.hpp>
#include <boost/statistics/detail/kernel/estimation/rp_visitor.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace kernel{

// This visitor, f, updates a Nadaraya-Watson estimate of E[Y|X=x0] each
// time f(x,y) is called.
//
// K,X,A : See rp_visitor
template<
    typename K,
    typename X,
    typename A = typename
        statistics::detail::kernel::detail::mean_accumulator<typename K::result_type>::type
>
class nw_visitor{
    public:
    typedef rp_visitor<K,X,A> rp_visitor_type;
    typedef typename rp_visitor_type::result_type result_type;
    typedef K kernel_type;
    typedef A accumulator_type;

    //Construct
    nw_visitor();
    nw_visitor(typename call_traits<X>::param_type);
    nw_visitor(
        K k, // passing radius should call implicit conversion
        typename call_traits<X>::param_type x
    );
    nw_visitor(
        K k,
        typename call_traits<X>::param_type,
        const accumulator_type&
    );
    nw_visitor(const nw_visitor&);
    nw_visitor& operator=(const nw_visitor&);

    // Update
    protected:
    template<typename X1,typename Y1> // Training data point
    result_type operator()(const X1& x1,const Y1& y1);

    public:
    // Access
    result_type unnormalized_estimate()const;
    result_type normalizing_constant()const;
    result_type estimate()const;

    const A& accumulator()const;
    const rp_visitor_type& rp_visitor()const;

    private:
    rp_visitor_type rp_visitor_;
    A a_;
};

//Construction
template<typename K,typename X,typename A>
nw_visitor<K,X,A>::nw_visitor():rp_visitor_(),a_(){}

template<typename K,typename X,typename A>
nw_visitor<K,X,A>::nw_visitor(K k,typename call_traits<X>::param_type x)
:rp_visitor_(k,x),a_(){}

template<typename K,typename X,typename A>
nw_visitor<K,X,A>::nw_visitor(
    K k,typename call_traits<X>::param_type x,const A& a
):rp_visitor_(k,x,a),a_(a){}

template<typename K,typename X,typename A>
nw_visitor<K,X,A>::nw_visitor(const nw_visitor& that)
:rp_visitor_(that.rp_visitor_),a_(that.a_){}

template<typename K,typename X,typename A>
typename nw_visitor<K,X,A>::nw_visitor&
nw_visitor<K,X,A>::operator=(const nw_visitor& that){
    if(&that!=this){
        rp_visitor_ = that.rp_visitor_;
        a_ = that.a_;
    }
    return *this;
}

// Update
template<typename K,typename X,typename A>
template<typename X1,typename Y1>
typename nw_visitor<K,X,A>::result_type
nw_visitor<K,X,A>::operator()(const X1& x1,const Y1& y){
    result_type w = (this->rp_visitor_(x1));
    result_type wy = w * y;
    this->a_(wy);
    return wy;
}

// Access
template<typename K,typename X,typename A>
typename nw_visitor<K,X,A>::result_type
nw_visitor<K,X,A>::unnormalized_estimate()const{
    return accumulators::mean(
        this->accumulator()
    );
}

template<typename K,typename X,typename A>
typename nw_visitor<K,X,A>::result_type
nw_visitor<K,X,A>::normalizing_constant()const{
    return (this->rp_visitor_).estimate();
}

template<typename K,typename X,typename A>
typename nw_visitor<K,X,A>::result_type
nw_visitor<K,X,A>::estimate()const{
    return (this->unnormalized_estimate()/this->normalizing_constant());
}

template<typename K,typename X,typename A>
const A& nw_visitor<K,X,A>::accumulator()const{ return  this->a_; }

template<typename K,typename X,typename A>
const rp_visitor<K,X,A>&
nw_visitor<K,X,A>::rp_visitor()const{
    return (this->rp_visitor_);
}

}// kernel
}// detail
}// statistics
}// boost

#endif


#include "coordsys.h"
#include "utils.h"
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/LU>

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

namespace batoid {

    CoordSys::CoordSys() :
        m_origin(Vector3d::Zero()), m_rot(Matrix3d::Identity()) {}

    CoordSys::CoordSys(const Vector3d origin, const Matrix3d rot) :
        m_origin(origin), m_rot(rot) {}

    CoordSys::CoordSys(const Vector3d origin) :
        m_origin(origin), m_rot(Matrix3d::Identity()) {}

    CoordSys::CoordSys(const Matrix3d rot) :
        m_origin(Vector3d::Zero()), m_rot(rot) {}

    CoordSys CoordSys::shiftGlobal(const Vector3d& dr) const {
        return CoordSys(m_origin+dr, m_rot);
    }

    CoordSys CoordSys::shiftLocal(const Vector3d& dr) const {
        // Note m_rot*dr instead of m_rot.transpose()*dr, b/c we are doing a passive rotation
        // instead of an active one.
        return shiftGlobal(m_rot*dr);
    }

    CoordSys CoordSys::rotateGlobal(const Matrix3d& rot) const {
        return CoordSys(rot*m_origin, rot*m_rot);
    }

    CoordSys CoordSys::rotateGlobal(const Matrix3d& rot, const Vector3d& rotCenter, const CoordSys& coordSys) const {
        CoordTransform toGlobal(coordSys, CoordSys());
        Vector3d globalRotCenter = toGlobal.applyForward(rotCenter);
        return CoordSys(
            rot*(m_origin-globalRotCenter)+globalRotCenter,
            rot*m_rot
        );
    }

    CoordSys CoordSys::rotateLocal(const Matrix3d& rot) const {
        // first rotate rot into global coords, then apply that
        // m_rot rot m_rot^-1 m_rot = m_rot rot
        return CoordSys(m_origin, m_rot*rot);
    }

    CoordSys CoordSys::rotateLocal(const Matrix3d& rot, const Vector3d& rotCenter, const CoordSys& coordSys) const {
        CoordTransform toGlobal(coordSys, CoordSys());
        Vector3d globalRotCenter = toGlobal.applyForward(rotCenter);
        return CoordSys(
            m_rot*rot*(m_rot.transpose())*(m_origin-globalRotCenter)+globalRotCenter,
            m_rot*rot
        );
    }

    Vector3d CoordSys::getXHat() const {
        return Vector3d(m_rot.data()[0], m_rot.data()[3], m_rot.data()[6]);
    }

    Vector3d CoordSys::getYHat() const {
        return Vector3d(m_rot.data()[1], m_rot.data()[4], m_rot.data()[7]);
    }

    Vector3d CoordSys::getZHat() const {
        return Vector3d(m_rot.data()[2], m_rot.data()[5], m_rot.data()[8]);
    }

    std::ostream& operator<<(std::ostream& os, const CoordSys& cs) {
        return os << cs.repr();
    }

    bool operator==(const CoordSys& cs1, const CoordSys& cs2) {
        return cs1.m_origin == cs2.m_origin && cs1.m_rot == cs2.m_rot;
    }

    bool operator!=(const CoordSys& cs1, const CoordSys& cs2) {
        return !(cs1 == cs2);
    }

    // x is global coordinate
    // y is destination, with corresponding R and dr
    // z is source, with corresponding S and ds
    //
    // y = Rinv(x-dr)
    // z = Sinv(x-ds)
    // implies
    // x = S z + ds
    //
    // y = Rinv(S z + ds - dr)
    //   = Rinv S z + Rinv ds - Rinv dr
    //   = Rinv S z + Rinv S Sinv ds - Rinv S Sinv dr
    //   = Rinv S (z + Sinv ds - Sinv dr)
    //   = (Sinv R)^-1 (z - (Sinv dr - Sinv ds))
    //   = (Sinv R)^-1 (z - Sinv (dr - ds))

    CoordTransform::CoordTransform(const CoordSys& source, const CoordSys& destination) :
        _dr(source.m_rot.transpose()*(destination.m_origin - source.m_origin)),
        _rot(source.m_rot.transpose()*destination.m_rot),
        _source(source), _destination(destination) {}

    CoordTransform::CoordTransform(const Vector3d& dr, const Matrix3d& rot) :
        _dr(dr), _rot(rot) {}

    // We actively shift and rotate the coordinate system axes,
    // This looks like y = R x + dr
    // For a passive transformation of a fixed vector from one coord sys to another
    // though, we want the opposite transformation: y = R^-1 (x - dr)
    Vector3d CoordTransform::applyForward(const Vector3d& r) const {
        return _rot.transpose()*(r-_dr);
    }

    Vector3d CoordTransform::applyReverse(const Vector3d& r) const {
        return _rot*r+_dr;
    }

    Ray CoordTransform::applyForward(const Ray& r) const {
        if (r.failed) return r;
        return Ray(_rot.transpose()*(r.r-_dr), _rot.transpose()*r.v,
                r.t, r.wavelength, r.flux, r.vignetted);
    }

    Ray CoordTransform::applyReverse(const Ray& r) const {
        if (r.failed) return r;
        return Ray(_rot*r.r + _dr, _rot*r.v,
            r.t, r.wavelength, r.flux, r.vignetted);
    }

    void CoordTransform::applyForwardInPlace(Ray& r) const {
        if (r.failed) return;
        r.r = _rot.transpose()*(r.r-_dr);
        r.v = _rot.transpose()*r.v;
    }

    void CoordTransform::applyReverseInPlace(Ray& r) const {
        if (r.failed) return;
        r.r = _rot*r.r+_dr;
        r.v = _rot*r.v;
    }

    RayVector CoordTransform::applyForward(const RayVector& rv) const {
        std::vector<Ray> result(rv.size());
        parallelTransform(rv.cbegin(), rv.cend(), result.begin(),
            [this](const Ray& r) { return applyForward(r); }
        );
        return RayVector(std::move(result), rv.getWavelength());
    }

    RayVector CoordTransform::applyReverse(const RayVector& rv) const {
        std::vector<Ray> result(rv.size());
        parallelTransform(rv.cbegin(), rv.cend(), result.begin(),
            [this](const Ray& r) { return applyReverse(r); }
        );
        return RayVector(std::move(result), rv.getWavelength());
    }

    void CoordTransform::applyForwardInPlace(RayVector& rv) const {
        parallel_for_each(rv.begin(), rv.end(),
            [this](Ray& r) { applyForwardInPlace(r); }
        );
    }

    void CoordTransform::applyReverseInPlace(RayVector& rv) const {
        parallel_for_each(rv.begin(), rv.end(),
            [this](Ray& r) { applyReverseInPlace(r); }
        );
    }

    bool operator==(const CoordTransform& ct1, const CoordTransform& ct2) {
        return ct1.getRot() == ct2.getRot() &&
               ct1.getDr() == ct2.getDr();
    }

    bool operator!=(const CoordTransform& ct1, const CoordTransform& ct2) {
        return !(ct1 == ct2);
    }

    std::ostream& operator<<(std::ostream &os, const CoordTransform& ct) {
        return os << ct.repr();
    }

}