/*! \file orbitaldensityrand.cpp
    \brief OrbitalDensityRandクラスの実装

    Copyright © 2019-2021 @dc1394 All Rights Reserved.
    (but this is originally adapted by サンマヤ for TDXHydrogenScene.cpp from http://sammaya.garyoutensei.com/math_phys/phys_sym/monte_hydrogen.html )
    https://tsujimotter.hatenablog.com/entry/metropolis-hastings-algorithm も参考にさせて頂きました。ありがとうございます。
    This software is released under the BSD 2-Clause License.
*/

#include "orbitaldensityrand.h"
#include "utility/utility.h"
#include <boost/assert.hpp>                                     // for boost::assert
#include <boost/math/constants/constants.hpp>                   // for boost::math::constants::pi
#include <boost/math/special_functions/spherical_harmonic.hpp>  // for boost::math::spherical_harmonic
#include <boost/range/algorithm.hpp>                            // for boost::fill

namespace orbitaldensityrand {
    // #region コンストラクタ

	OrbitalDensityRand::OrbitalDensityRand(std::shared_ptr<getdata::GetData> const & pgd)
        :   Complete([this] { return complete_.load(); }, nullptr),
            Dt([this] { return dt_; }, [this](auto dt) { return dt_ = dt; }),
            Elapsed_time([this] { return count_ * dt_; }, nullptr),
            Pth([this] { return std::cref(pth_); }, nullptr),
		    Redraw(nullptr, [this](auto redraw) { return redraw_ = redraw; }),
            Rmax([this] { return rmax_; }, nullptr),
            Thread_end(nullptr, [this](auto thread_end) {
			    thread_end_.store(thread_end);
			    return thread_end; }),
            Vertex([this] { return std::cref(vertex_); }, nullptr),
		    Vertexsize([this]{ return vertexsize_.load(); }, [this](std::vector<SimpleVertex>::size_type size) {
				vertexsize_.store(size);
				return size; }),
            pgd_(pgd),
            q0_({ 1.0, 1.0, 0.0 }),
            q_(q0_),
		    rmax_(GetRmax(pgd)),
		    vertex_(pgd_->Rho_wf_type == getdata::GetData::Rho_Wf_type::RHO ? RHO_VERTEXSIZE_INIT_VALUE : WF_VERTEXSIZE_INIT_VALUE),
            vertexsize_(pgd_->Rho_wf_type == getdata::GetData::Rho_Wf_type::RHO ? RHO_VERTEXSIZE_INIT_VALUE : WF_VERTEXSIZE_INIT_VALUE)
    {
    }

    // #endregion コンストラクタ

    // #region privateメンバ関数

    void OrbitalDensityRand::operator()(std::int32_t m, Normal_Nelson_type nornel)
    {
        if (redraw_) {
            if (vertex_.size() != vertexsize_) {
                vertex_.resize(vertexsize_);
            }

            pth_.reset(new std::thread([this, m, nornel] { ClearFillSimpleVertex(m, nornel); }), [this](std::thread * pth)
            {
                if (pth->joinable()) {
                    thread_end_.store(true);
                    pth->join();
                }

                utility::Safe_Delete<std::thread> sd;
                sd(pth);
            });
            redraw_ = false;
        }
    }

	void OrbitalDensityRand::ClearFillSimpleVertex(std::int32_t m, Normal_Nelson_type nornel)
	{
		complete_.store(false);

		SimpleVertex sv{};
		sv.Color = { 0.0f, 0.0f, 0.0f, 0.0f };
		sv.Pos = { 0.0f, 0.0f, 0.0f };
		boost::fill(vertex_, sv);

        switch (nornel)
        {
        case Normal_Nelson_type::NORMAL:
            {
                auto const threads = static_cast<std::int32_t>(std::thread::hardware_concurrency());
                auto const num = static_cast<std::int32_t>(vertexsize_.load() / threads);

                auto thvec = std::vector<std::thread>(threads);
                for (auto i = 0; i < threads - 1; i++) {
                    thvec[i] = std::thread([i, m, num, this]() { FillSimpleVertex(m, num * i, num * (i + 1)); });
                }
                thvec[threads - 1] = std::thread([m, num, threads, this]() { FillSimpleVertex(m, num * (threads - 1), static_cast<std::int32_t>(vertexsize_.load())); });

                for (auto && th : thvec) {
                    th.join();
                }
            }
            break;

        case Normal_Nelson_type::NELSON:
            FillSimpleVertex(m);
            break;

        default:
            BOOST_ASSERT(!"nornelが異常!");
            break;
        }

		complete_.store(true);
	}

    void OrbitalDensityRand::FillSimpleVertex(std::int32_t m)
    {
        using namespace boost::math;
        using namespace constants;

        auto const actual_dt = dt_ * ATTOSECTOAU;

        count_ = 0U;
        do {
            if (thread_end_) {
                return;
            }

            auto const r = std::hypot(q_[0], q_[1], q_[2]);

            if (r < pgd_->R_meshmin() || r > pgd_->R_meshmax()) {
                Resetq();
                continue;
            }

            double phi = 0.0;
            if (std::fabs(q_[1]) < THRESHOLD) {
                if (q_[0] < 0.0) {
                    phi = boost::math::constants::pi<double>();
                }
            }
            else if (q_[0] * q_[0] + q_[1] * q_[1] > 0.0) {
                auto const sign = q_[1] > 0 ? 1 : -1;
                phi = sign * std::acos(q_[0] / std::sqrt(q_[0] * q_[0] + q_[1] * q_[1]));
            }
            auto const theta = std::acos(q_[2] / r);

            auto const ylm = Spherical_harmonic(pgd_->L, m, theta, phi);
            auto const dylmdtheta = Numerical_diff(
                theta,
                myfunctional::make_functional([this, m, phi](double th) { return Spherical_harmonic(pgd_->L, m, th, phi); }));

            if (std::fabs(ylm) < THRESHOLD && std::fabs(dylmdtheta) < THRESHOLD) {
                Resetq();
                continue;
            }

            auto const dylmdphi = Numerical_diff(
                phi,
                myfunctional::make_functional([this, m, theta](double ph) { return Spherical_harmonic(pgd_->L, m, theta, ph); }));

            if (std::fabs(ylm) < THRESHOLD && std::fabs(dylmdphi) < THRESHOLD) {
                Resetq();
                continue;
            }

            auto f_x = std::sin(theta) * std::cos(phi) * pgd_->dphidr(r) / (*pgd_)(r);
            f_x += std::cos(theta) * std::cos(phi) / r * dylmdtheta / ylm;
            f_x -= std::sin(phi) / (r * std::sin(theta)) * dylmdphi / ylm;

            auto f_y = std::sin(theta) * std::sin(phi) * pgd_->dphidr(r) / (*pgd_)(r);
            f_y += std::cos(theta) * std::sin(phi) / r * dylmdtheta / ylm;
            f_y += std::cos(phi) / (r * std::sin(theta)) * dylmdphi / ylm;

            auto f_z = std::cos(theta) * pgd_->dphidr(r) / (*pgd_)(r);
            f_z -= std::sin(theta) / r * dylmdtheta / ylm;

            q_[0] += f_x * actual_dt + mr_.normal_distribution_rand() * std::sqrt(actual_dt);
            q_[1] += f_y * actual_dt + mr_.normal_distribution_rand() * std::sqrt(actual_dt);
            q_[2] += f_z * actual_dt + mr_.normal_distribution_rand() * std::sqrt(actual_dt);

            vertex_[count_].Pos.x = static_cast<float>(q_[0]);
            vertex_[count_].Pos.y = static_cast<float>(q_[1]);
            vertex_[count_].Pos.z = static_cast<float>(q_[2]);

            vertex_[count_].Color.x = 0.8f;
            vertex_[count_].Color.y = 0.0f;
            vertex_[count_].Color.z = 0.8f;
            vertex_[count_].Color.w = 1.0f;

            count_++;
        } while (count_ < vertexsize_.load());
    }

	void OrbitalDensityRand::FillSimpleVertex(std::int32_t m, std::int32_t starti, std::int32_t endi)
	{
		if (thread_end_) {
			return;
		}

		auto sign = 1;
		auto x = 0.0;
        auto y = 0.0;
        auto z = 0.0;

        myrandom::MyRandSfmt mr;

        auto nextflag = false;
        auto count_ = 0;

        do {
            if (thread_end_) {
                return;
            }

            switch (pgd_->Rho_wf_type) {
            case getdata::GetData::Rho_Wf_type::RHO:
            {
                auto rho = [this, m, nextflag](auto x, auto y, auto z) mutable
                {
                    auto const r = std::sqrt(x * x + y * y + z * z);
                    if (r < pgd_->R_meshmin()) {
                        nextflag = true;
                        return 0.0;
                    }

                    double phi = 0.0;
                    if (std::fabs(y) < EPS) {
                        if (x < 0.0) {
                            phi = boost::math::constants::pi<double>();
                        }
                    }
                    else if (x * x + y * y > 0.0) {
                        auto const sign = y > 0 ? 1 : -1;
                        phi = sign * std::acos(x / std::sqrt(x * x + y * y));
                    }

                    auto const ylm = Spherical_harmonic(pgd_->L, m, std::acos(z / r), phi);

                    return (*pgd_)(r) * ylm * ylm;
                };

                auto const maxr = pgd_->R2rhomaxr();

                auto const x_star = mr.normal_distribution_rand(x, maxr);
                auto const y_star = mr.normal_distribution_rand(y, maxr);
                auto const z_star = mr.normal_distribution_rand(z, maxr);

                auto const rho_t = rho(x, y, z);    // xの電子密度
                if (nextflag) {
                    x = x_star;
                    y = y_star;
                    z = z_star;
                    nextflag = false;
                    continue;
                }

                auto const rho_star = rho(x_star, y_star, z_star);  // x* の電子密度

                // 採択率を計算  α = p(x*) / p(x_t)
                auto const alpha = (rho_star * rho_star) / (rho_t * rho_t);

                // 採択率により決定
                auto const ar = mr.myrand();    // 0 <= ar <= 1 の一様乱数 ar を生成
                if (ar <= alpha) {              // 採択
                    x = x_star;
                    y = y_star;
                    z = z_star;
                    if (rho_star >= 0.0) {
                        sign = 1;
                    }
                    else {
                        sign = -1;
                    }
                }
            }
            break;

            case getdata::GetData::Rho_Wf_type::WF:
            {
                auto phi = [this, m, nextflag](auto x, auto y, auto z) mutable
                {
                    auto const r = std::sqrt(x * x + y * y + z * z);
                    if (r < pgd_->R_meshmin()) {
                        nextflag = true;
                        return 0.0;
                    }

                    double phi = 0.0;
                    if (std::fabs(y) < EPS) {
                        if (x < 0.0) {
                            phi = boost::math::constants::pi<double>();
                        }
                    }
                    else if (x * x + y * y > 0.0) {
                        auto const sign = y > 0 ? 1 : -1;
                        phi = sign * std::acos(x / std::sqrt(x * x + y * y));
                    }

                    return (*pgd_)(r) * Spherical_harmonic(pgd_->L, m, std::acos(z / r), phi);
                };

                auto const maxr = pgd_->R2rhomaxr();

                auto const x_star = mr.normal_distribution_rand(x, maxr);
                auto const y_star = mr.normal_distribution_rand(y, maxr);
                auto const z_star = mr.normal_distribution_rand(z, maxr);

                auto const phi_t = phi(x, y, z);    // xの波動関数
                if (nextflag) {
                    x = x_star;
                    y = y_star;
                    z = z_star;
                    nextflag = false;
                    continue;
                }

                auto const phi_star = phi(x_star, y_star, z_star);  // x* の波動関数

                // 採択率を計算  α = p(x*) / p(x_t)
                auto const alpha = (phi_star * phi_star) / (phi_t * phi_t);

                //採択率により決定
                auto const ar = mr.myrand();    // 0 <= ar <= 1 の一様乱数 ar を生成
                if (ar <= alpha) {              // 採択
                    x = x_star;
                    y = y_star;
                    z = z_star;
                    if (phi_star >= 0.0) {
                        sign = 1;
                    }
                    else {
                        sign = -1;
                    }
                }
                else {                          // 採択しない
                    continue;
                }
            }
            break;

            default:
                BOOST_ASSERT(!"何かがおかしい!");
                break;
            }

            vertex_[starti + count_].Pos.x = static_cast<float>(x);
            vertex_[starti + count_].Pos.y = static_cast<float>(y);
            vertex_[starti + count_].Pos.z = static_cast<float>(z);

            vertex_[starti + count_].Color.x = sign > 0 ? 0.8f : 0.0f;
            vertex_[starti + count_].Color.y = sign < 0 ? 0.8f : 0.0f;
            vertex_[starti + count_].Color.z = 0.8f;
            vertex_[starti + count_].Color.w = 1.0f;
            count_++;
		} while (count_ < (endi - starti));
	}

    // #endregion privateメンバ関数

    // #region フリー関数

	double GetRmax(std::shared_ptr<getdata::GetData> const & pgd)
	{
		auto const n = static_cast<double>(pgd->N);
		return (2.3622 * n + 3.3340) * n + 1.3228;
	}

    double Spherical_harmonic(std::int32_t l, std::int32_t m, double theta, double phi)
    {
        if (!m) {
            return boost::math::spherical_harmonic_r(l, m, theta, phi);
        }
        else if (m > 0) {
            auto const sign = (m & 1 ? -1 : 1);
            return (sign * boost::math::spherical_harmonic_r(l, m, theta, phi) + boost::math::spherical_harmonic_r(l, -m, theta, phi)) / std::sqrt(2.0);
        }
        else {
            m = -m;
            auto const sign = (m & 1 ? -1 : 1);
            return (sign * boost::math::spherical_harmonic_i(l, m, theta, phi) - boost::math::spherical_harmonic_i(l, -m, theta, phi)) / std::sqrt(2.0);
        }
    }

    // #endregion フリー関数
}


#include "mex.h"
#include <igl/matlab/parse_rhs.h>
#include <igl/matlab/prepare_lhs.h>
#include <igl/matlab/validate_arg.h>
#include <igl/matlab/MexStream.h>
#include <igl/matlab/mexErrMsgTxt.h>
#include <igl/copyleft/tetgen/tetrahedralize.h>
#include <Eigen/Core>
#include <Eigen/Sparse>

void mexFunction(
  int          nlhs,
  mxArray      *plhs[],
  int          nrhs,
  const mxArray *prhs[])
{
  //mexPrintf("Compiled at %s on %s\n",__TIME__,__DATE__);
  using namespace igl;
  using namespace igl::matlab;
  using namespace igl::copyleft::tetgen;
  using namespace Eigen;
  igl::matlab::MexStream mout;
  std::streambuf *outbuf = std::cout.rdbuf(&mout);

  Eigen::MatrixXd SV;
  Eigen::MatrixXi SF;
  mexErrMsgTxt(nrhs>=2,"nrhs should be >= 2");
  parse_rhs_double(prhs+0,SV);
  parse_rhs_index(prhs+1,SF);

  Eigen::MatrixXd SH(0,3);
  std::string flags = "-q2";
  bool verbose = false;

  {
    int i = 2;
    while(i<nrhs)
    {
      mexErrMsgTxt(mxIsChar(prhs[i]),"Parameter names should be strings");
      // Cast to char
      const char * name = mxArrayToString(prhs[i]);
      if(strcmp("Flags",name) == 0)
      {
        validate_arg_char(i,nrhs,prhs,name);
        flags = mxArrayToString(prhs[++i]);
      }else
      {
        mexErrMsgTxt(false,"Unknown parameter");
      }
      i++;
    }
  }

  Eigen::MatrixXd SR(0,5);
  size_t nr = -1;
  Eigen::MatrixXd TV,TR;
  Eigen::MatrixXi TT,TF,TN,PT,FT;
  int ret = tetrahedralize(SV,SF,SH,SR,flags,TV,TT,TF,TR,TN,PT,FT,nr);
  //int ret = tetrahedralize(SV,SF,flags,TV,TT,TF);
  mexErrMsgTxt((ret==0) || (ret==2),"Tetgen failed.");

  switch(nlhs)
  {
    case 7:
      prepare_lhs_index(FT,plhs+7);
    case 6:
      prepare_lhs_index(PT,plhs+5);
    case 5:
      prepare_lhs_index(TN,plhs+4);
    case 4:
      prepare_lhs_double(TR,plhs+3);
    case 3:
      prepare_lhs_index(TF,plhs+2);
    case 2:
      prepare_lhs_index(TT,plhs+1);
    case 1:
      prepare_lhs_double(TV,plhs+0);
    default:break;
  }
}


//  Copyright 2011 Vicente J. Botet Escriba
//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

// test ratio:  equivalent ratios convert with BOOST_RATIO_EXTENSIONS

#define BOOST_RATIO_EXTENSIONS
#include <boost/ratio/ratio.hpp>
#include <boost/detail/lightweight_test.hpp>

boost::intmax_t func(boost::ratio<5,6>  s);

boost::intmax_t func(boost::ratio<5,6>  s) {
    return s.num;
}

void test();

void test() {
    boost::ratio<10,12> r;
    BOOST_TEST((
        func(r)==5
    ));
}

int main()
{
    test();
}


#include "Demos/Utils/Config.h"
#include "Demos/Visualization/MiniGL.h"
#include "Demos/Visualization/Selection.h"
#include "GL/glut.h"
#include "Demos/Simulation/TimeManager.h"
#include <Eigen/Dense>
#include "Demos/Simulation/SimulationModel.h"
#include "Demos/Simulation/TimeStepController.h"
#include <iostream>

#define _USE_MATH_DEFINES
#include "math.h"

// Enable memory leak detection
#ifdef _DEBUG
	#define new DEBUG_NEW
#endif

using namespace PBD;
using namespace Eigen;
using namespace std;

void timeStep ();
void buildModel ();
void createBodyModel();
void render ();
void reset();
void cleanup();
void selection(const Eigen::Vector2i &start, const Eigen::Vector2i &end);
void TW_CALL setTimeStep(const void *value, void *clientData);
void TW_CALL getTimeStep(void *value, void *clientData);
void TW_CALL setVelocityUpdateMethod(const void *value, void *clientData);
void TW_CALL getVelocityUpdateMethod(void *value, void *clientData);
void TW_CALL setMaxIterations(const void *value, void *clientData);
void TW_CALL getMaxIterations(void *value, void *clientData);

SimulationModel model;
TimeStepController sim;

const float width = 0.4f;
const float height = 0.4f;
const float depth = 2.0f;
bool doPause = true;
std::vector<unsigned int> selectedBodies;
Eigen::Vector3f oldMousePos;
float jointColor[4] = { 0.0f, 0.4f, 0.2f, 1.0f };
float dynamicBodyColor[4] = { 0.1f, 0.4f, 0.8f, 1 };
float staticBodyColor[4] = { 0.4f, 0.4f, 0.4f, 1.0f };

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

	// OpenGL
	MiniGL::init (argc, argv, 1280, 960, 0, 0, "Rigid body demo");
	MiniGL::initLights ();
	MiniGL::setClientIdleFunc (50, timeStep);
	MiniGL::setKeyFunc(0, 'r', reset);
	MiniGL::setSelectionFunc(selection);

	buildModel ();

	MiniGL::setClientSceneFunc(render);
	MiniGL::setViewport (60.0f, 0.1f, 500.0f, Vector3f (6.0f, -5.5f, 15.0f), Vector3f (6.0f, -3.0f, 0.0f));

	TwAddVarRW(MiniGL::getTweakBar(), "Pause", TW_TYPE_BOOLCPP, &doPause, " label='Pause' group=Simulation key=SPACE ");
	TwAddVarCB(MiniGL::getTweakBar(), "TimeStepSize", TW_TYPE_FLOAT, setTimeStep, getTimeStep, &model, " label='Time step size'  min=0.0 max = 0.1 step=0.001 precision=4 group=Simulation ");
	TwType enumType = TwDefineEnum("VelocityUpdateMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "VelocityUpdateMethod", enumType, setVelocityUpdateMethod, getVelocityUpdateMethod, &sim, " label='Velocity update method' enum='0 {First Order Update}, 1 {Second Order Update}' group=Simulation");
	TwAddVarCB(MiniGL::getTweakBar(), "MaxIter", TW_TYPE_UINT32, setMaxIterations, getMaxIterations, &sim, " label='Max. iterations'  min=1 step=1 group=Simulation ");

	glutMainLoop ();

	cleanup ();

	return 0;
}

void cleanup()
{
	delete TimeManager::getCurrent();
}

void reset()
{
	model.reset();
	sim.reset();
	TimeManager::getCurrent()->setTime(0.0);
}

void mouseMove(int x, int y)
{
	Eigen::Vector3f mousePos;
	MiniGL::unproject(x, y, mousePos);
	const Eigen::Vector3f diff = mousePos - oldMousePos;

	TimeManager *tm = TimeManager::getCurrent();
	const float h = tm->getTimeStepSize();

	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
	for (size_t j = 0; j < selectedBodies.size(); j++)
	{
		rb[selectedBodies[j]]->getVelocity() += 1.0f / h * diff;
	}
	oldMousePos = mousePos;
}

void selection(const Eigen::Vector2i &start, const Eigen::Vector2i &end)
{
 	std::vector<unsigned int> hits;
 	selectedBodies.clear();

	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
	std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > x;
	x.resize(rb.size());
 	for (unsigned int i = 0; i < rb.size(); i++)
 	{
		x[i] = rb[i]->getPosition();
 	}

 	Selection::selectRect(start, end, &x[0], &x[rb.size() - 1], selectedBodies);
 	if (selectedBodies.size() > 0)
 		MiniGL::setMouseMoveFunc(GLUT_MIDDLE_BUTTON, mouseMove);
 	else
 		MiniGL::setMouseMoveFunc(-1, NULL);

 	MiniGL::unproject(end[0], end[1], oldMousePos);
}

void timeStep ()
{
	if (doPause)
		return;

	// Simulation code
	for (unsigned int i = 0; i < 8; i++)
	{
		sim.step(model);

		// set target angle of motors for an animation
		const float currentTargetAngle = (float)M_PI * 0.5f - (float)M_PI * 0.5f * cos(0.25f*TimeManager::getCurrent()->getTime());
		SimulationModel::ConstraintVector &constraints = model.getConstraints();
		TargetAngleMotorHingeJoint &joint1 = (*(TargetAngleMotorHingeJoint*)constraints[8]);
		TargetVelocityMotorHingeJoint &joint2 = (*(TargetVelocityMotorHingeJoint*)constraints[9]);
		joint1.setTargetAngle(currentTargetAngle);
		joint2.setTargetAngularVelocity(3.5f);

		const float currentTargetPos = 1.5f*sin(2.0f*TimeManager::getCurrent()->getTime());
		TargetPositionMotorSliderJoint &joint3 = (*(TargetPositionMotorSliderJoint*)constraints[12]);
		joint3.setTargetPosition(currentTargetPos);

		float currentTargetVel = 0.25f;
		if (((int) (0.25f*TimeManager::getCurrent()->getTime())) % 2 == 1)
			currentTargetVel = -currentTargetVel;
		TargetVelocityMotorSliderJoint &joint4 = (*(TargetVelocityMotorSliderJoint*)constraints[14]);
		joint4.setTargetVelocity(currentTargetVel);
	}
}

void buildModel ()
{
	TimeManager::getCurrent ()->setTimeStepSize (0.005f);

	createBodyModel();
}

void renderBallJoint(BallJoint &bj)
{
	MiniGL::drawSphere(bj.m_jointInfo.col(2), 0.1f, jointColor);
}

void renderBallOnLineJoint(BallOnLineJoint &bj)
{
	MiniGL::drawSphere(bj.m_jointInfo.col(5), 0.1f, jointColor);
	MiniGL::drawCylinder(bj.m_jointInfo.col(5) - bj.m_jointInfo.col(7), bj.m_jointInfo.col(5) + bj.m_jointInfo.col(7), jointColor, 0.05f);
}

void renderHingeJoint(HingeJoint &hj)
{
	MiniGL::drawSphere(hj.m_jointInfo.col(6) - 0.5*hj.m_jointInfo.col(8), 0.1f, jointColor);
	MiniGL::drawSphere(hj.m_jointInfo.col(6) + 0.5*hj.m_jointInfo.col(8), 0.1f, jointColor);
	MiniGL::drawCylinder(hj.m_jointInfo.col(6) - 0.5*hj.m_jointInfo.col(8), hj.m_jointInfo.col(6) + 0.5*hj.m_jointInfo.col(8), jointColor, 0.05f);
}

void renderUniversalJoint(UniversalJoint &uj)
{
	MiniGL::drawSphere(uj.m_jointInfo.col(4) - 0.5*uj.m_jointInfo.col(6), 0.1f, jointColor);
	MiniGL::drawSphere(uj.m_jointInfo.col(4) + 0.5*uj.m_jointInfo.col(6), 0.1f, jointColor);
	MiniGL::drawSphere(uj.m_jointInfo.col(5) - 0.5*uj.m_jointInfo.col(7), 0.1f, jointColor);
	MiniGL::drawSphere(uj.m_jointInfo.col(5) + 0.5*uj.m_jointInfo.col(7), 0.1f, jointColor);
	MiniGL::drawCylinder(uj.m_jointInfo.col(4) - 0.5*uj.m_jointInfo.col(6), uj.m_jointInfo.col(4) + 0.5*uj.m_jointInfo.col(6), jointColor, 0.05f);
	MiniGL::drawCylinder(uj.m_jointInfo.col(5) - 0.5*uj.m_jointInfo.col(7), uj.m_jointInfo.col(5) + 0.5*uj.m_jointInfo.col(7), jointColor, 0.05f);
}

void renderSliderJoint(SliderJoint &joint)
{
	MiniGL::drawSphere(joint.m_jointInfo.col(6), 0.1f, jointColor);
	MiniGL::drawCylinder(joint.m_jointInfo.col(7) - joint.m_jointInfo.col(8), joint.m_jointInfo.col(7) + joint.m_jointInfo.col(8), jointColor, 0.05f);
}

void renderTargetPositionMotorSliderJoint(TargetPositionMotorSliderJoint &joint)
{
	MiniGL::drawSphere(joint.m_jointInfo.col(6), 0.1f, jointColor);
	MiniGL::drawCylinder(joint.m_jointInfo.col(7) - joint.m_jointInfo.col(8), joint.m_jointInfo.col(7) + joint.m_jointInfo.col(8), jointColor, 0.05f);
}

void renderTargetVelocityMotorSliderJoint(TargetVelocityMotorSliderJoint &joint)
{
	MiniGL::drawSphere(joint.m_jointInfo.col(6), 0.1f, jointColor);
	MiniGL::drawCylinder(joint.m_jointInfo.col(7) - joint.m_jointInfo.col(8), joint.m_jointInfo.col(7) + joint.m_jointInfo.col(8), jointColor, 0.05f);
}

void renderTargetAngleMotorHingeJoint(TargetAngleMotorHingeJoint &hj)
{
	MiniGL::drawSphere(hj.m_jointInfo.col(6) - 0.5*hj.m_jointInfo.col(8), 0.1f, jointColor);
	MiniGL::drawSphere(hj.m_jointInfo.col(6) + 0.5*hj.m_jointInfo.col(8), 0.1f, jointColor);
	MiniGL::drawCylinder(hj.m_jointInfo.col(6) - 0.5*hj.m_jointInfo.col(8), hj.m_jointInfo.col(6) + 0.5*hj.m_jointInfo.col(8), jointColor, 0.05f);
}

void renderTargetVelocityMotorHingeJoint(TargetVelocityMotorHingeJoint &hj)
{
	MiniGL::drawSphere(hj.m_jointInfo.col(6) - 0.5*hj.m_jointInfo.col(8), 0.1f, jointColor);
	MiniGL::drawSphere(hj.m_jointInfo.col(6) + 0.5*hj.m_jointInfo.col(8), 0.1f, jointColor);
	MiniGL::drawCylinder(hj.m_jointInfo.col(6) - 0.5*hj.m_jointInfo.col(8), hj.m_jointInfo.col(6) + 0.5*hj.m_jointInfo.col(8), jointColor, 0.05f);
}

void render ()
{
	MiniGL::coordinateSystem();

	// Draw sim model

	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
	SimulationModel::ConstraintVector &constraints = model.getConstraints();

	float selectionColor[4] = { 0.8f, 0.0f, 0.0f, 1 };

	for (size_t i = 0; i < rb.size(); i++)
	{
		bool selected = false;
		for (unsigned int j = 0; j < selectedBodies.size(); j++)
		{
			if (selectedBodies[j] == i)
				selected = true;
		}

		if (rb[i]->getMass() == 0.0f)
			MiniGL::drawCube(rb[i]->getPosition(), rb[i]->getRotationMatrix().transpose(), 0.5f, 0.5f, 0.5f, staticBodyColor);
		else
		{
			if (!selected)
				MiniGL::drawCube(rb[i]->getPosition(), rb[i]->getRotationMatrix().transpose(), width, height, depth, dynamicBodyColor);
			else
				MiniGL::drawCube(rb[i]->getPosition(), rb[i]->getRotationMatrix().transpose(), width, height, depth, selectionColor);
		}
	}

	for (size_t i = 0; i < constraints.size(); i++)
	{
		if (constraints[i]->getTypeId() == BallJoint::TYPE_ID)
		{
			renderBallJoint(*(BallJoint*)constraints[i]);
		}
		else if (constraints[i]->getTypeId() == BallOnLineJoint::TYPE_ID)
		{
			renderBallOnLineJoint(*(BallOnLineJoint*)constraints[i]);
		}
		else if (constraints[i]->getTypeId() == HingeJoint::TYPE_ID)
		{
			renderHingeJoint(*(HingeJoint*)constraints[i]);
		}
		else if (constraints[i]->getTypeId() == UniversalJoint::TYPE_ID)
		{
			renderUniversalJoint(*(UniversalJoint*)constraints[i]);
		}
		else if (constraints[i]->getTypeId() == SliderJoint::TYPE_ID)
		{
			renderSliderJoint(*(SliderJoint*)constraints[i]);
		}
		else if (constraints[i]->getTypeId() == TargetAngleMotorHingeJoint::TYPE_ID)
		{
			renderTargetAngleMotorHingeJoint(*(TargetAngleMotorHingeJoint*)constraints[i]);
		}
		else if (constraints[i]->getTypeId() == TargetVelocityMotorHingeJoint::TYPE_ID)
		{
			renderTargetVelocityMotorHingeJoint(*(TargetVelocityMotorHingeJoint*)constraints[i]);
		}
		else if (constraints[i]->getTypeId() == TargetPositionMotorSliderJoint::TYPE_ID)
		{
			renderTargetPositionMotorSliderJoint(*(TargetPositionMotorSliderJoint*)constraints[i]);
		}
		else if (constraints[i]->getTypeId() == TargetVelocityMotorSliderJoint::TYPE_ID)
		{
			renderTargetVelocityMotorSliderJoint(*(TargetVelocityMotorSliderJoint*)constraints[i]);
		}
	}

	float textColor[4] = { 0.0f, .2f, .4f, 1 };
	MiniGL::drawStrokeText(-0.5f, 1.5f, 1.0f, 0.002f, "ball joint", 11, textColor);
	MiniGL::drawStrokeText(3.0f, 1.5f, 1.0f, 0.002f, "ball-on-line joint", 19, textColor);
	MiniGL::drawStrokeText(7.3f, 1.5f, 1.0f, 0.002f, "hinge joint", 12, textColor);
	MiniGL::drawStrokeText(11.2f, 1.5f, 1.0f, 0.002f, "universal joint", 15, textColor);

	MiniGL::drawStrokeText(-1.0f, -4.0f, 1.0f, 0.002f, "motor hinge joint", 17, textColor);
	MiniGL::drawStrokeText(3.4f, -4.0f, 1.0f, 0.002f, "slider joint", 12, textColor);
	MiniGL::drawStrokeText(6.6f, -4.0f, 1.0f, 0.002f, "target position motor", 21, textColor);
	MiniGL::drawStrokeText(10.6f, -4.0f, 1.0f, 0.002f, "target velocity motor", 21, textColor);

	MiniGL::drawTime( TimeManager::getCurrent ()->getTime ());
}

// Compute diagonal inertia tensor
Eigen::Vector3f computeInertiaTensorBox(const float mass, const float width, const float height, const float depth)
{
	const float Ix = (mass / 12.0f) * (height*height + depth*depth);
	const float Iy = (mass / 12.0f) * (width*width + depth*depth);
	const float Iz = (mass / 12.0f) * (width*width + height*height);
	return Eigen::Vector3f(Ix, Iy, Iz);
}

/** Create the rigid body model
*/
void createBodyModel()
{
	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();

	// static body
	rb.resize(24);
	float startX = 0.0f;
	float startY = 1.0f;
	for (unsigned int i = 0; i < 8; i++)
	{
		rb[3*i] = new RigidBody();
		rb[3*i]->initBody(0.0f,
			Eigen::Vector3f(startX, startY, 1.0f),
			computeInertiaTensorBox(1.0f, 0.5f, 0.5f, 0.5f),
			Eigen::Quaternionf(1.0f, 0.0f, 0.0f, 0.0f));

		// dynamic body
		rb[3*i+1] = new RigidBody();
		rb[3*i+1]->initBody(1.0f,
			Eigen::Vector3f(startX, startY-0.25f, 2.0f),
			computeInertiaTensorBox(1.0f, width, height, depth),
			Eigen::Quaternionf(1.0f, 0.0f, 0.0f, 0.0f));

		// dynamic body
		rb[3 * i + 2] = new RigidBody();
		rb[3 * i + 2]->initBody(1.0f,
			Eigen::Vector3f(startX, startY - 0.25f, 4.0f),
			computeInertiaTensorBox(1.0f, width, height, depth),
			Eigen::Quaternionf(1.0f, 0.0f, 0.0f, 0.0f));

		startX += 4.0f;

		if (i == 3)
		{
			startY -= 5.5f;
			startX = 0.0f;
		}
	}

	float jointY = 0.75f;
	model.addBallJoint(0, 1, Eigen::Vector3f(0.25f, jointY, 1.0f));
	model.addBallJoint(1, 2, Eigen::Vector3f(0.25f, jointY, 3.0f));

	model.addBallOnLineJoint(3, 4, Eigen::Vector3f(4.25f, jointY, 1.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f));
	model.addBallJoint(4, 5, Eigen::Vector3f(4.25f, jointY, 3.0f));

	model.addHingeJoint(6, 7, Eigen::Vector3f(8.0f, jointY, 1.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f));
	model.addBallJoint(7, 8, Eigen::Vector3f(8.25f, jointY, 3.0f));

	model.addUniversalJoint(9, 10, Eigen::Vector3f(12.0f, jointY, 1.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 1.0f, 0.0f));
	model.addBallJoint(10, 11, Eigen::Vector3f(12.25f, jointY, 3.0f));

	jointY -= 5.5f;
	model.addTargetAngleMotorHingeJoint(12, 13, Eigen::Vector3f(0.0f, jointY, 1.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f));
	model.addTargetVelocityMotorHingeJoint(13, 14, Eigen::Vector3f(0.0f, jointY, 3.0f), Eigen::Vector3f(0.0f, 1.0f, 0.0f));

	model.addSliderJoint(15, 16, Eigen::Vector3f(4.0f, jointY, 1.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f));
	model.addBallJoint(16, 17, Eigen::Vector3f(4.25f, jointY, 3.0f));

	model.addTargetPositionMotorSliderJoint(18, 19, Eigen::Vector3f(8.0f, jointY, 1.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f));
	model.addBallJoint(19, 20, Eigen::Vector3f(8.25f, jointY, 3.0f));

	model.addTargetVelocityMotorSliderJoint(21, 22, Eigen::Vector3f(12.0f, jointY, 1.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f));
	model.addBallJoint(22, 23, Eigen::Vector3f(12.25f, jointY, 3.0f));
}

void TW_CALL setTimeStep(const void *value, void *clientData)
{
	const float val = *(const float *)(value);
	TimeManager::getCurrent()->setTimeStepSize(val);
}

void TW_CALL getTimeStep(void *value, void *clientData)
{
	*(float *)(value) = TimeManager::getCurrent()->getTimeStepSize();
}

void TW_CALL setVelocityUpdateMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	((TimeStepController*)clientData)->setVelocityUpdateMethod((unsigned int)val);
}

void TW_CALL getVelocityUpdateMethod(void *value, void *clientData)
{
	*(short *)(value) = (short)((TimeStepController*)clientData)->getVelocityUpdateMethod();
}

void TW_CALL setMaxIterations(const void *value, void *clientData)
{
	const unsigned int val = *(const unsigned int *)(value);
	((TimeStepController*)clientData)->setMaxIterations(val);
}

void TW_CALL getMaxIterations(void *value, void *clientData)
{
	*(unsigned int *)(value) = ((TimeStepController*)clientData)->getMaxIterations();
}


#include <boost/accumulators/statistics/mean.hpp>


#ifndef EDMONDS_OPTIMUM_BRANCHING_IMPL_HPP
#define EDMONDS_OPTIMUM_BRANCHING_IMPL_HPP

#include <vector>
#include <algorithm>
#include <list>
#include <boost/property_map/property_map.hpp>
#include <boost/foreach.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/concept_check.hpp>
#include <boost/pending/disjoint_sets.hpp>

// namespace detail
//
// The namespace encapsulates classes and/or functions that are
// required for the implementation of edmonds's optimum branching
// algorithm which should not be visible to the user. This way the
// global namespace remains unpolluted.
namespace detail {
    using namespace boost;

    // class OptimumBranching
    //
    // I encapsulate all the details of edmonds's algorithm inside a
    // class. This makes the code easier to read (and easier to write)
    // since the number of template declarations is reduced
    // considerably. Besides, all the utility functions used to
    // implement one algorithm conceptually do belong together.
    //
    // Note that any concept checks are performed in the function that
    // uses this class, so there is no need to repeat them here.
    template <bool TOptimumIsMaximum,
              bool TAttemptToSpan,
              bool TGraphIsDense,
              class TEdgeListGraph,
              class TVertexIndexMap,
              class TWeightMap,
              class TInputIterator,
              class TOutputIterator>
    class OptimumBranching {
    public:
        typedef TEdgeListGraph                                        Graph;
        typedef typename graph_traits<Graph>::edge_descriptor         Edge;
        typedef typename graph_traits<Graph>::vertex_descriptor       Vertex;
        typedef typename graph_traits<Graph>::edge_iterator           EdgeIter;
        typedef typename property_traits<TWeightMap>::value_type      weight_t;
        typedef typename property_traits<TVertexIndexMap>::value_type vertex_idx_t;

        // struct EdgeNode
        //
        // One unique EdgeNode object is created for each edge of the
        // input graph. Any containers then store pointers to these
        // objects. The edges of the graph F (which is described in
        // the document describing the implementation) are stored in
        // each EdgeNode object via the parent and children members.
        // For efficiency reasons a boolean member 'removed_from_F' is
        // also present in each EdgeNode object. If this member is
        // set, then the edge (which is also a vertex in F) was
        // removed during the expansion phase of the algorithm.
        struct EdgeNode {
            Edge                        edge;
            vertex_idx_t                source;
            vertex_idx_t                target;
            weight_t                    weight;
            EdgeNode                   *parent;
            std::vector<EdgeNode *>     children;
            bool                        removed_from_F;

            EdgeNode(const Edge &e,
                     const vertex_idx_t &s,
                     const vertex_idx_t &t,
                     const weight_t w)
                : edge(e), source(s), target(t), weight(w), parent(0),
                  removed_from_F(false)
            {
                ;
            }

            // operator<
            //
            // This is only used temporarily when sorting the
            // EdgeNodes by their sources. Once a radix-sort algorithm
            // has been implemented, this is no longer needed.
            bool        operator<(const EdgeNode &en) const
            {
                return source < en.source;
            }
        };

        // The data members of the OptimumBranching class. These
        // include both the input and the variables needed internally for the implemenationa.
        const TEdgeListGraph   &g;
        const TVertexIndexMap  &index;
        const TWeightMap       &weight;
        TInputIterator          roots_begin;
        TInputIterator          roots_end;
        TOutputIterator         out;

        // The constructor
        OptimumBranching(const TEdgeListGraph &g,
                         const TVertexIndexMap &index,
                         const TWeightMap &weight,
                         TInputIterator roots_begin,
                         TInputIterator roots_end,
                         TOutputIterator out)
            : g(g), index(index), weight(weight),
              roots_begin(roots_begin), roots_end(roots_end), out(out)
        {
            ;
        }

        // remove_from_F()
        //
        // It removes the EdgeNode en and all its ancestors from the
        // graph F (by resetting the parent and children members and
        // setting the flag removed_from_F). Any newly created roots
        // of F are inserted into F_roots. Note that the root of F
        // that is removed as a consequence is not actually removed
        // from F_roots. It is simply marked as removed via the
        // 'removed_from_F' flag.
        void remove_from_F(EdgeNode *en, std::vector<EdgeNode *> &F_roots)
        {
            // Note that en is inserted into F_roots as well. But
            // since it is marked as removed_from_F it will not cause
            // any trouble. This is more efficient than making sure
            // that only the siblings of en are inserted into F_roots.
            for ( ; en != 0; en = en->parent)
            {
                en->removed_from_F = true;
                BOOST_FOREACH (EdgeNode *child, en->children)
                {
                    F_roots.push_back(child);
                    child->parent = 0;
                }

                // free the memory used in en->children.
                std::vector<EdgeNode *>().swap(en->children);
            }
        }

        // sort_edges()
        //
        // sorts a vector of EdgeNode pointers with EdgeNode.source as
        // key using the radix-sort algorithm. Also, if there are
        // several EdgeNode pointers with the same source, the
        // function only keeps the one with optimum weight.
        void sort_edges(std::vector<EdgeNode *> &edge_vec)
        {
            const int byte_len = 8;
            const int num_buckets = 1u << byte_len;
            const unsigned digits = (sizeof (vertex_idx_t)) * std::numeric_limits<unsigned char>::digits;
            const unsigned mask = (1u << byte_len) - 1;

            std::vector< std::list<EdgeNode *> > buckets(num_buckets);

            for (unsigned i = 0; byte_len * i <= digits; ++i)
            {
                BOOST_FOREACH (EdgeNode *en, edge_vec)
                {
                    buckets[(en->source >> byte_len * i) & mask].push_back(en);
                }

                edge_vec.clear();
                BOOST_FOREACH (std::list<EdgeNode *> &bucket, buckets)
                {
                    BOOST_FOREACH (EdgeNode *en, bucket)
                    {
                        if (!edge_vec.empty() && edge_vec.back()->source == en->source)
                        {
                            bool en_is_better = TOptimumIsMaximum ?
                                en->weight > edge_vec.back()->weight :
                                en->weight < edge_vec.back()->weight;
                            if (en_is_better)
                            {
                                edge_vec.back() = en;
                            }
                        }
                        else
                        {
                            edge_vec.push_back(en);
                        }
                    }
                }

                buckets.clear();
                buckets.resize(num_buckets);
            }
        }

        // operator()
        //
        // This is the main function implementing Tarjan's
        // implementation of Edmonds's algorithm.
        void operator()()
        {
            std::vector<EdgeNode> all_edges;
            vertex_idx_t max_vertex_idx;

            // Create EdgeNodes for all the edges and find the maximum vertex
            // index. Note that we skip self-loops.
            max_vertex_idx = 0;
            BOOST_FOREACH (const Edge &e, edges(g))
            {
                if (source(e, g) == target(e, g))
                    continue;

                all_edges.push_back(EdgeNode (e, source(e, g), target(e, g), get(weight, e)));
                max_vertex_idx = std::max(max_vertex_idx, index[target(e, g)]);
            }

            // insert into in_edges[v] all edges entering v.

            //!! TODO !! If sparse graphs, I have to change the
            //representation of in_edges to a special kind of priority
            //queue that are able to be merged in log n time.

            std::vector< std::vector<EdgeNode *> > in_edges(max_vertex_idx + 1);
            std::vector<weight_t> edge_weight_change(max_vertex_idx + 1);
            BOOST_FOREACH (EdgeNode &en, all_edges)
            {
                in_edges[en.target].push_back(&en);
            }
            BOOST_FOREACH (std::vector<EdgeNode *> &edges, in_edges)
            {
                sort_edges(edges);
            }

            // Save the specified roots in a random access fashion.
            std::vector<bool> is_specified_root(max_vertex_idx + 1);
            std::vector<vertex_idx_t> final_roots;
            for ( ; roots_begin != roots_end; ++roots_begin)
            {
                is_specified_root[index[*roots_begin]] = true;
                final_roots.push_back(index[*roots_begin]);
            }

            // Initialize S, W, roots, cycles, lambda, enter, F, and min
            std::vector< std::vector<EdgeNode *> > cycle(max_vertex_idx + 1);
            std::vector<EdgeNode *> lambda(max_vertex_idx + 1);
            std::vector<vertex_idx_t> roots;
            disjoint_sets_with_storage<> S(2*(max_vertex_idx +1));
            disjoint_sets_with_storage<> W(2*(max_vertex_idx +1));
            std::vector<vertex_idx_t> min(max_vertex_idx + 1);
            std::vector<EdgeNode *> enter(max_vertex_idx + 1);
            std::vector<EdgeNode *> F;
            for (vertex_idx_t v = 0; v <= max_vertex_idx; ++v)
            {
                S.make_set(v);
                W.make_set(v);
                min[v] = v;
                if (!is_specified_root[v])
                    roots.push_back(v);
            }

            // Keep adding critical edges and contracting cycles while
            // doing a whole bunch of book-keeping.
            while (!roots.empty())
            {
                // Find an S-component with an entering edge
                vertex_idx_t cur_root = roots.back(); roots.pop_back();
                if (in_edges[cur_root].empty())
                {
                    final_roots.push_back(min[cur_root]);
                    continue;
                }

                // Find an optimum-weight edge entering cur_root

                //!! TODO !! We have to do this differently for sparse graphs.
                EdgeNode *critical_edge = in_edges[cur_root].front();
                BOOST_FOREACH (EdgeNode *en, in_edges[cur_root])
                {
                    bool en_is_better = TOptimumIsMaximum ?
                        en->weight > critical_edge->weight :
                        en->weight < critical_edge->weight;
                    if (en_is_better)
                    {
                        critical_edge = en;
                    }
                }

                // Do not add critical_edge if it worsens the total
                // weight and we are not attempting to span.
                if (!TAttemptToSpan)
                {
                    bool improves = TOptimumIsMaximum ?
                        critical_edge->weight > weight_t(0) :
                        critical_edge->weight < weight_t(0);
                    if (!improves)
                    {
                        final_roots.push_back(min[cur_root]);
                        continue;
                    }
                }

                // Insert critical_edge into "F" and let any edges in
                // cycle[cur_root] be its children.
                F.push_back(critical_edge);
                BOOST_FOREACH (EdgeNode *en, cycle[cur_root])
                {
                    en->parent = critical_edge;
                    critical_edge->children.push_back(en);
                }

                // If critical_edge is a leaf in "F", then add a
                // pointer to it.
                if (cycle[cur_root].empty())
                {
                    lambda[cur_root] = critical_edge;
                }

                // If adding critical_edge didn't create a cycle
                if (W.find_set(critical_edge->source) !=
                    W.find_set(critical_edge->target))
                {
                    enter[cur_root] = critical_edge;
                    W.union_set(critical_edge->source, critical_edge->target);
                }
                else // If adding critical_edge did create a cycle
                {
                    // Find the edges of the cycle, the
                    // representatives of the strong components in the
                    // cycle, and the least costly edge of the cycle.
                    std::vector<EdgeNode *> cycle_edges;
                    std::vector<vertex_idx_t> cycle_repr;
                    EdgeNode *least_costly_edge = critical_edge;
                    enter[cur_root] = 0;

                    cycle_edges.push_back(critical_edge);
                    cycle_repr.push_back(S.find_set(critical_edge->target));
                    for (vertex_idx_t v = S.find_set(critical_edge->source);
                         enter[v] != 0; v = S.find_set(enter[v]->source))
                    {
                        cycle_edges.push_back(enter[v]);
                        cycle_repr.push_back(v);
                        bool is_less_costly = TOptimumIsMaximum ?
                            enter[v]->weight < least_costly_edge->weight :
                            enter[v]->weight > least_costly_edge->weight;
                        if (is_less_costly)
                        {
                            least_costly_edge = enter[v];
                        }
                    }
                    // change the weight of the edges entering
                    // vertices of the cycle.
                    //!! TODO !! Change this for sparse graphs
                    BOOST_FOREACH (EdgeNode *en, cycle_edges)
                    {
                        edge_weight_change[S.find_set(en->target)] =
                            least_costly_edge->weight - en->weight;
                    }

                    // Save the vertex that would be root if the newly
                    // created strong component would be a root.
                    vertex_idx_t cycle_root =
                        min[S.find_set(least_costly_edge->target)];

                    // Union all components of the cycle into one component.
                    vertex_idx_t new_repr = cycle_repr.front();
                    BOOST_FOREACH (vertex_idx_t v, cycle_repr)
                    {
                        S.link(v, new_repr);
                        new_repr = S.find_set(new_repr);
                    }
                    min[new_repr] = cycle_root;
                    roots.push_back(new_repr);
                    cycle[new_repr].swap(cycle_edges);

                    //!! TODO !! Needs to be changed for sparse graphs.
                    BOOST_FOREACH (vertex_idx_t v, cycle_repr)
                    {
                        BOOST_FOREACH (EdgeNode *en, in_edges[v])
                        {
                            en->weight += edge_weight_change[v];
                        }
                    }

                    // Merge all in_edges of the cycle into one list.
                    //!! TODO !! needs to be changed for sparse graphs.
                    std::vector<EdgeNode *> new_in_edges;
                    for (unsigned i = 1; i < cycle_repr.size(); ++i)
                    {
                        typedef typename std::vector<EdgeNode *>::iterator Iter;
                        Iter i1 = in_edges[cycle_repr[i]].begin();
                        Iter e1 = in_edges[cycle_repr[i]].end();
                        Iter i2 = in_edges[cycle_repr[i-1]].begin();
                        Iter e2 = in_edges[cycle_repr[i-1]].end();

                        ///*
                        while (i1 != e1 || i2 != e2)
                        {
                            while (i1 != e1 && S.find_set((*i1)->source) == new_repr)
                            {
                                ++i1;
                            }
                            while (i2 != e2 && S.find_set((*i2)->source) == new_repr)
                            {
                                ++i2;
                            }

                            if (i1 == e1 && i2 == e2)
                                break;

                            if (i1 == e1)
                            {
                                new_in_edges.push_back(*i2);
                                ++i2;
                            }
                            else if (i2 == e2)
                            {
                                new_in_edges.push_back(*i1);
                                ++i1;
                            }
                            else if (((*i1)->source) < ((*i2)->source))
                            {
                                new_in_edges.push_back(*i1);
                                ++i1;
                            }
                            else if ((*i1)->source > (*i2)->source)
                            {
                                new_in_edges.push_back(*i2);
                                ++i2;
                            }
                            else // if the sources are equal
                            {
                                bool i1_is_better = TOptimumIsMaximum ?
                                    (*i1)->weight > (*i2)->weight :
                                    (*i1)->weight < (*i2)->weight;
                                if (i1_is_better)
                                {
                                    new_in_edges.push_back(*i1);
                                }
                                else
                                {
                                    new_in_edges.push_back(*i2);
                                }
                                ++i1;
                                ++i2;
                            }
                        }
                        in_edges[cycle_repr[i]].swap(new_in_edges);
                        new_in_edges.clear();
                    }
                    in_edges[new_repr].swap(in_edges[cycle_repr.back()]);
                    edge_weight_change[new_repr] = weight_t(0);
                    //*/
                }
            } // while (!roots.empty())

            // Extract the optimum branching

            // Find all roots of F.
            std::vector<EdgeNode *> F_roots;
            BOOST_FOREACH (EdgeNode *en, F)
            {
                if (en->parent == 0)
                {
                    F_roots.push_back(en);
                }
            }

            // Remove edges entering the root nodes.
            BOOST_FOREACH (vertex_idx_t v, final_roots)
            {
                if (lambda[v] != 0)
                {
                    remove_from_F(lambda[v], F_roots);
                }
            }

            while (!F_roots.empty())
            {
                EdgeNode *en = F_roots.back(); F_roots.pop_back();
                if (en->removed_from_F)
                    continue;

                *out = en->edge;
                ++out;
                remove_from_F(lambda[en->target], F_roots);
            }
        }

    };
}

template <bool TOptimumIsMaximum,
          bool TAttemptToSpan,
          bool TGraphIsDense,
          class TEdgeListGraph,
          class TVertexIndexMap,
          class TWeightMap,
          class TInputIterator,
          class TOutputIterator>
void
edmonds_optimum_branching(TEdgeListGraph &g,
                          TVertexIndexMap index,
                          TWeightMap weight,
                          TInputIterator roots_begin,
                          TInputIterator roots_end,
                          TOutputIterator out)
{
    using namespace boost;

    typedef typename graph_traits<TEdgeListGraph>::edge_descriptor    Edge;
    typedef typename graph_traits<TEdgeListGraph>::vertex_descriptor  Vertex;
    typedef typename graph_traits<TEdgeListGraph>::edge_iterator      EdgeIter;
    typedef typename property_traits<TWeightMap>::value_type          weight_t;

    function_requires< EdgeListGraphConcept<TEdgeListGraph> >();
    function_requires< ReadablePropertyMapConcept<TWeightMap, Edge> >();
    function_requires< ReadablePropertyMapConcept<TVertexIndexMap, Vertex> >();
    function_requires< InputIteratorConcept<TInputIterator> >();
    function_requires< OutputIteratorConcept<TOutputIterator, Edge> >();
    //!! Add the following requirements:
    //
    // property_traits<TVertexIndexMap>::value_type is a built-in
    // integral type, or perhaps require that it can be used to index
    // into arrays.
    //
    // property_traits<TWeightMap>::value_type is a numeric type that
    // handles the operations +, -, and <.
    //
    // TInputIterator's value type is Vertex
    // TOutputIterator's value type is Edge

    ::detail::OptimumBranching<TOptimumIsMaximum, TAttemptToSpan,
        TGraphIsDense, TEdgeListGraph, TVertexIndexMap, TWeightMap,
        TInputIterator, TOutputIterator>
          optimum_branching(g, index, weight, roots_begin, roots_end, out);
    optimum_branching();
}

#endif // not EDMONDS_OPTIMUM_BRANCHING_IMPL_HPP


#include <bits/stdc++.h>
#include <boost/program_options.hpp>
#include <boost/random.hpp>
#include "oracles/util/util.h"
#include "oracles/randomized_response.h"
#include "oracles/rappor.h"
#include "oracles/pi_rappor.h"
#include "oracles/subset_selection.h"
#include "oracles/hadamard_response.h"
#include "oracles/vanilla_hadamard_response.h"
#include "oracles/recursive_hadamard_response.h"
#include "oracles/projective_geometry.h"
#include "oracles/hybrid_projective_geometry.h"

namespace po = boost::program_options;

using namespace std;

int n, trials, K;
int var_rep; // in run_variance_experiments, how many times to run the experiment per value of eps
double var_eps_min, var_eps_max;
int num_var_epsilons;
int kmin, kmax, numk;
int hpgq;
bool debug;
bool rtime, rmse, rvar;
bool run_ss, run_rappor, run_pirappor, run_rr, run_hr, run_vhr, run_rhr, run_pg, run_hpg;
double epsilon;
string base_filename, output_dir;
shared_ptr<istream> input;
ofstream ovar, otime, omse, omax;
vector<int> hist, data;

// if option o1 is set, then at least one of o2 or o3 flags must be true
// also, if either o2 or o3 is true, then o1 must be set
void option_dependency2(const po::variables_map &vm, const char* o1, const char* o2,
			const char* o3, bool o2_triggered, bool o3_triggered) {
  if (o2_triggered && !vm.count(o1))
    throw logic_error(string("Option ' ") + o2 + "' requires option '" + o1 + "'.");
  if (o3_triggered && !vm.count(o1))
    throw logic_error(string("Option ' ") + o3 + "' requires option '" + o1 + "'.");
  if (vm.count(o1) && !(o2_triggered || o3_triggered))
    throw logic_error(string("Option ' ") + o1 + "' requires either option '" + o2 + "' or '" + o3 + "'.");
}

// if option o1 is set, then boolean flag o2 must be true; if o2 is true, o1 must be set
void option_dependency(const po::variables_map &vm, const char* o1, const char* o2, bool o2_triggered) {
  if (o2_triggered && !vm.count(o1))
    throw logic_error(string("Option ' ") + o2 + "' requires option '" + o1 + "'.");
  if (vm.count(o1) && !o2_triggered)
    throw logic_error(string("Option ' ") + o1 + "' requires option '" + o2 + "'.");
}

void init_oracles(PrivateFrequencyOracle **RR, PrivateFrequencyOracle **AsymmRAPPOR,
		  PrivateFrequencyOracle **PIAsymmRAPPOR, PrivateFrequencyOracle **SS,
		  PrivateFrequencyOracle **HR, PrivateFrequencyOracle **VHR,
		  PrivateFrequencyOracle **RHR, PrivateFrequencyOracle **PG,
		  PrivateFrequencyOracle **HPG, vector<PrivateFrequencyOracle*> &algos,
		  vector<string> &algo_names, double eps, int K_, int seed=boost::random::mt19937::default_seed) {
  if (debug) cerr << "initializing oracles" << endl;

  if (run_rr) *RR = new RandomizedResponse(K_, eps, debug, seed);
  if (run_rappor) *AsymmRAPPOR = new AsymmetricRAPPOR(K_, eps, debug, seed);
  if (run_pirappor) *PIAsymmRAPPOR = new PIAsymmetricRAPPOR(K_, eps, debug, seed);
  if (run_ss) *SS = new SubsetSelection(K_, eps, debug, seed);
  if (run_hr) *HR = new HadamardResponse(K_, eps, debug, seed);
  if (run_vhr) *VHR = new VanillaHadamardResponse(K_, eps, debug, seed);
  if (run_rhr) *RHR = new RecursiveHadamardResponse(K_, eps, INT_MAX, debug, seed); // b=K will certainly be overwritten
  int q = int(ceil(exp(eps) + 1));
  if (q > 2) {
    if (q % 2 == 0)
      q += 1;
    while (!Util::is_prime(q))
      q += 2;
  }
  if (run_pg) *PG = new ProjectiveGeometryResponse(K_, eps, q, debug, seed);
  if (run_hpg) *HPG = new HybridProjectiveGeometryResponse(K_, eps, max(2, (int)ceil((exp(eps)+1)/hpgq)), hpgq, debug, seed);

  if (run_rr)
    algos.push_back(*RR), algo_names.push_back("RR");
  if (run_rappor)
    algos.push_back(*AsymmRAPPOR), algo_names.push_back("RAPPOR");
  if (run_pirappor)
    algos.push_back(*PIAsymmRAPPOR), algo_names.push_back("PI-RAPPOR");
  if (run_ss)
    algos.push_back(*SS), algo_names.push_back("SS");
  if (run_hr)
    algos.push_back(*HR), algo_names.push_back("HR");
  if (run_vhr)
    algos.push_back(*VHR), algo_names.push_back("VHR");
  if (run_rhr)
    algos.push_back(*RHR), algo_names.push_back("RHR");
  if (run_pg)
    algos.push_back(*PG), algo_names.push_back("PG");
  if (run_hpg)
    algos.push_back(*HPG), algo_names.push_back(string("HPG") + to_string(hpgq));

  if (debug) cerr << "done initializing oracles" << endl;
}

void destroy_oracles(PrivateFrequencyOracle *RR, PrivateFrequencyOracle *AsymmRAPPOR,
		     PrivateFrequencyOracle *PIAsymmRAPPOR, PrivateFrequencyOracle *SS,
		     PrivateFrequencyOracle *HR, PrivateFrequencyOracle *VHR,
		     PrivateFrequencyOracle *RHR, PrivateFrequencyOracle *PG,
		     PrivateFrequencyOracle *HPG) {
  if (run_rr) delete RR;
  if (run_rappor) delete AsymmRAPPOR;
  if (run_pirappor) delete PIAsymmRAPPOR;
  if (run_ss) delete SS;
  if (run_hr) delete HR;
  if (run_vhr) delete VHR;
  if (run_rhr) delete RHR;
  if (run_pg) delete PG;
  if (run_hpg) delete HPG;
}

void run_timing_experiments() {
  if (debug) cerr << "==================\nRUNNING TIMING EXPERIMENTS\n==================" << endl;
  PrivateFrequencyOracle *RR, *AsymmRAPPOR, *PIAsymmRAPPOR, *SS, *HR, *VHR, *RHR, *PG, *HPG;
  vector<PrivateFrequencyOracle*> algos;
  vector<string> algo_names;
  int num_algos = run_ss + run_rappor + run_pirappor + run_rr
    + run_hr + run_vhr + run_rhr + run_pg + run_hpg;
  vector< vector<double> > results(num_algos);
  // the actual user data shouldn't affect runtime for any of our oracle implementations
  // so just let each user hold the item "0"
  vector<int> timing_data(n, 0);
  for (int t = 0; t < numk; ++t) {
    int K_ = kmin + (numk == 1 ? 0 : static_cast<int64_t>(t)*(kmax - kmin) / (numk - 1));
    algos.clear();
    algo_names.clear();
    init_oracles(&RR, &AsymmRAPPOR, &PIAsymmRAPPOR, &SS, &HR, &VHR, &RHR, &PG, &HPG, algos, algo_names, epsilon, K_);
    for (int i = 0; i < num_algos; ++i) {
      if (debug) cerr << "K=" << K_ << ", " << algo_names[i] << endl;
      vector<Message> msgs = vector<Message>(n);
      for (int j = 0; j < timing_data.size(); ++j)
	msgs[j] = algos[i]->local_randomizer(timing_data[j]);
      auto start = chrono::high_resolution_clock::now();
      vector<double> ret = algos[i]->estimate_all_freqs(msgs);
      auto stop = chrono::high_resolution_clock::now();
      if (debug) {
	cerr << "printing first 10 estimates" << endl;
	for (int j = 0; j < 10; ++j)
	  cerr << ret[j] << " ";
	cerr << endl;
      }
      auto duration = chrono::duration_cast<chrono::milliseconds>(stop - start);
      results[i].push_back(duration.count());
    }
    destroy_oracles(RR, AsymmRAPPOR, PIAsymmRAPPOR, SS, HR, VHR, RHR, PG, HPG);
  }
  for (int i = 0; i < algos.size(); ++i)
    for (int t = 0; t < numk; ++t)
      otime << algo_names[i] << "," << kmin + (numk == 1 ? 0 : static_cast<int64_t>(t)*(kmax - kmin) / (numk - 1)) << "," << results[i][t] << endl;
}

void run_variance_experiments() {
  if (debug) cerr << "==================\nRUNNING PRIVACY/UTILITY TRADEOFF EXPERIMENTS\n==================" << endl;
  PrivateFrequencyOracle *RR, *AsymmRAPPOR, *PIAsymmRAPPOR, *SS, *HR, *VHR, *RHR, *PG, *HPG;
  vector<PrivateFrequencyOracle*> algos;
  vector<string> algo_names;
  init_oracles(&RR, &AsymmRAPPOR, &PIAsymmRAPPOR, &SS, &HR, &VHR, &RHR, &PG, &HPG, algos, algo_names, 5.0, 22000); // ugly hack to get algo_names set
  destroy_oracles(RR, AsymmRAPPOR, PIAsymmRAPPOR, SS, HR, VHR, RHR, PG, HPG);
  int num_algos = run_ss + run_rappor + run_pirappor + run_rr
    + run_hr + run_vhr + run_rhr + run_pg + run_hpg;
  vector< vector<double> > results(num_algos);
  double increment = (var_eps_max - var_eps_min) / (num_var_epsilons - 1);
  for (int t = 0; t < num_var_epsilons; ++t) {
    double eps = var_eps_min + t*increment;
    vector<double> mse_averages(num_algos, 0.0);
    if (debug) cerr << "epsilon=" << eps << endl;
    for (int rr = 0; rr < var_rep; ++rr) {
      algos = vector<PrivateFrequencyOracle*>();
      algo_names = vector<string>();
      init_oracles(&RR, &AsymmRAPPOR, &PIAsymmRAPPOR, &SS, &HR, &VHR, &RHR, &PG, &HPG, algos, algo_names, eps, K, boost::random::mt19937::default_seed + rr);
      for (int i = 0; i < num_algos; ++i) {
	if (debug) cerr << algo_names[i] << ":" << endl;
	vector<Message> msgs = vector<Message>(n);
	for (int j = 0; j < data.size(); ++j)
	  msgs[j] = algos[i]->local_randomizer(data[j]);
	vector<double> ret = algos[i]->estimate_all_freqs(msgs);
	if (debug) {
	  cerr << "printing first 10 estimates (actuals)" << endl;
	  for (int j = 0; j < 10; ++j)
	    cerr << ret[j] << "(" << hist[j] << ") ";
	  cerr << endl;
	}
	for (int j = 0; j < K; ++j)
	  mse_averages[i] += (ret[j]-hist[j])*(ret[j]-hist[j]);
	if (debug) cerr << "done computing mse averages" << endl;
      }
      destroy_oracles(RR, AsymmRAPPOR, PIAsymmRAPPOR, SS, HR, VHR, RHR, PG, HPG);
    }
    for (int i = 0; i < num_algos; ++i)
      results[i].push_back(mse_averages[i] / (var_rep * K));
  }
  for (int i = 0; i < algos.size(); ++i)
    for (int t = 0; t < num_var_epsilons; ++t)
      ovar << algo_names[i] << "," << var_eps_min + t*increment << "," << results[i][t] << endl;
}

void run_mse_experiments() {
  if (debug) cerr << "==================\nRUNNING MSE EXPERIMENTS\n==================" << endl;
  PrivateFrequencyOracle *RR, *AsymmRAPPOR, *PIAsymmRAPPOR, *SS, *HR, *VHR, *RHR, *PG, *HPG;
  vector<PrivateFrequencyOracle*> algos;
  vector<string> algo_names;
  init_oracles(&RR, &AsymmRAPPOR, &PIAsymmRAPPOR, &SS, &HR, &VHR, &RHR, &PG, &HPG, algos, algo_names, epsilon, K);

  vector< vector<double> > results(algos.size()), max_results(algos.size());

  for (int rr = 0; rr < trials; ++rr)
    for (int i = 0; i < algos.size(); ++i) {
      if (debug) cerr << "trial# " << rr << ", " << algo_names[i] << endl;
      vector<Message> msgs = vector<Message>(n);
      for (int j = 0; j < data.size(); ++j)
	msgs[j] = algos[i]->local_randomizer(data[j]);
      if (debug) cerr << "estimating all frequencies" << endl;
      vector<double> ret = algos[i]->estimate_all_freqs(msgs);
      if (debug) {
	cerr << "printing first 10 estimates (actuals)" << endl;
	for (int j = 0; j < 10; ++j)
	  cerr << ret[j] << "(" << hist[j] << ") ";
	cerr << endl;
      }
      double bias = 0;
      for (int j = 0; j < K; ++j)
	bias += (ret[j] - hist[j]);
      if (debug) cerr << "avg bias in universe is " << bias/K << endl;
      double mse = 0, maxdev = 0;
      for (int j = 0; j < K; ++j) {
	mse += (ret[j]-hist[j])*(ret[j]-hist[j]);
	maxdev = max(maxdev, abs(ret[j]-hist[j]));
      }
      results[i].push_back(mse / K);
      max_results[i].push_back(maxdev);
    }

  for (int i = 0; i < algos.size(); ++i) {
    sort(results[i].begin(), results[i].end());
    sort(max_results[i].begin(), max_results[i].end());
    assert(results[i].size() == max_results[i].size());
    for (int j = 0; j < results[i].size(); ++j) {
      omse << algo_names[i] << "," << results[i][j] << endl;
      omax << algo_names[i] << "," << max_results[i][j] << endl;
    }
  }

  destroy_oracles(RR, AsymmRAPPOR, PIAsymmRAPPOR, SS, HR, VHR, RHR, PG, HPG);
}

void set_base_filename(string s) {
  if (!s.compare("-"))
    base_filename = "pfotest";
  int i = s.find_last_of('/');
  if (i != string::npos)
    s = s.substr(i + 1);
  i = s.find_last_of('.');
  if (i == string::npos)
    base_filename = string(s);
  else
    base_filename = s.substr(0, i);
}

void read_histogram_data() {
  hist.resize(K);
  data.resize(n);
  fill(hist.begin(), hist.end(), 0);
  int x, cnt = 0;
  while (*input >> x) {
    assert(cnt < n);
    assert(x>=0 && x<K);
    data[cnt++] = x;
    hist[x]++;
  }
  assert(cnt == n);
}

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

    po::options_description desc("Allowed options");
    desc.add_options()
      ("help,h",                                                                              "print usage message")
      ("num-users,n",                   po::value<int>(&n)->required(),                       "number of users")
      ("universe-size,u",               po::value<int>(&K),                                   "universe size")
      ("num-trials,t",                  po::value<int>(&trials),                              "number of random trials to run in mse experiments")
      ("epsilon,e",                     po::value<double>(&epsilon),                          "privacy parameter epsilon for mse/timing experiments")
      ("input-file,i",                  po::value<string>()->default_value("-"),              "file path specifying users' items")
      ("output-dir,o",                  po::value<string>()->default_value("pfotmp/"),        "dir in which to write experiment output files")
      ("debug",                         po::bool_switch(&debug)->default_value(false),        "debug mode prints more to cerr")
      ("include-ss,ss",                 po::bool_switch(&run_ss)->default_value(false),       "include SubsetSelection in experiments")
      ("include-rappor,rappor",         po::bool_switch(&run_rappor)->default_value(false),   "include RAPPOR in experiments")
      ("include-pirappor,pirappor",     po::bool_switch(&run_pirappor)->default_value(false), "include PI-RAPPOR in experiments")
      ("include-rr,rr",                 po::bool_switch(&run_rr)->default_value(false),       "include RandomizedResponse in experiments")
      ("include-hr,hr",                 po::bool_switch(&run_hr)->default_value(false),       "include HadamardResponse in experiments")
      ("include-vhr,vhr",               po::bool_switch(&run_vhr)->default_value(false),      "include VanillaHadamardResponse in experiments")
      ("include-rhr,rhr",               po::bool_switch(&run_rhr)->default_value(false),      "include RecursiveHadamardResponse in experiments")
      ("hpgq",                          po::value<int>(&hpgq),                                "value of q to use with HybridProjectiveGeometry")
      ("include-pg,pg",                 po::bool_switch(&run_pg)->default_value(false),       "include ProjectiveGeometry in experiments")
      ("include-hpg,hpg",               po::bool_switch(&run_hpg)->default_value(false),      "include HybridProjectiveGeometry in experiments")
      ("run-variance-experiments,rvar", po::bool_switch(&rvar)->default_value(false),         "run experiments to test variance")
      ("run-timing-experiments,rtime",  po::bool_switch(&rtime)->default_value(false),        "run experiments to test runtimes")
      ("run-mse-experiments,rmse",      po::bool_switch(&rmse)->default_value(false),         "run experiments to test mse")
      ("variance-repetitions,vr",       po::value<int>(&var_rep),                             "number of mse's to average per eps when running variance experiments")
      ("var-epsilon-min,vemn",          po::value<double>(&var_eps_min),                      "smallest eps to use in variance experiment")
      ("var-epsilon-max,vemx",          po::value<double>(&var_eps_max),                      "largest eps to use in variance experiment")
      ("num-var-epsilons,nve",          po::value<int>(&num_var_epsilons),                    "number of epsilons to use in variance experiment")
      ("universe-min,umin",             po::value<int>(&kmin),                                "smallest universe size to use in timing experiments")
      ("universe-max,umax",             po::value<int>(&kmax),                                "largest universe size to use in timing experiments")
      ("num-universes,nu",              po::value<int>(&numk),                                "number of universe sizes to try between umin and umax")
      ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);

    if (vm.count("help")) {
      cout << desc << endl;
      return 0;
    }

    po::notify(vm);

    option_dependency2(vm, "epsilon", "run-mse-experiments", "run-timing-experiments", rmse, rtime);
    option_dependency2(vm, "universe-size", "run-mse-experiments", "run-variance-experiments", rmse, rvar);
    option_dependency(vm, "hpgq", "include-hpg", run_hpg);
    option_dependency(vm, "variance-repetitions", "run-variance-experiments", rvar);
    option_dependency(vm, "var-epsilon-min", "run-variance-experiments", rvar);
    option_dependency(vm, "var-epsilon-max", "run-variance-experiments", rvar);
    option_dependency(vm, "num-var-epsilons", "run-variance-experiments", rvar);
    option_dependency(vm, "universe-min", "run-timing-experiments", rtime);
    option_dependency(vm, "universe-max", "run-timing-experiments", rtime);
    option_dependency(vm, "num-universes", "run-timing-experiments", rtime);
    option_dependency(vm, "num-trials", "run-mse-experiments", rmse);

    if (rvar)
      assert((num_var_epsilons > 1) && (var_eps_min != var_eps_max));
    if (rtime)
      assert(kmin <= kmax && numk >= 1);

    // set input and output files
    if (!vm["input-file"].as<string>().compare("-"))
      input.reset(&cin, [](...){});
    else
      input.reset(new ifstream(vm["input-file"].as<string>()));
    set_base_filename(vm["input-file"].as<string>());

    output_dir = vm["output-dir"].as<string>();
    if (output_dir[output_dir.size() - 1] != '/')
      output_dir += string("/");
    if (rvar) ovar = ofstream(output_dir + base_filename + string(".var"));
    if (rtime) otime = ofstream(output_dir + base_filename + string(".time"));
    if (rmse) {
      omse = ofstream(output_dir + base_filename + string(".mse"));
      omax = ofstream(output_dir + base_filename + string(".max"));
    }

    // make sure at least one oracle is being tested
    if (!(run_ss || run_rappor || run_pirappor || run_rr || run_hr || run_vhr || run_rhr || run_pg || run_hpg))
      throw logic_error("At least one algorithm must be included in experiment.");

    // make sure at least one type of experiment is being run
    if (!(rvar || rtime || rmse))
      throw logic_error("At least one type of experiment must be run.");

    // read in user data for the experiments if not a timing experiment
    if (rmse || rvar)
      read_histogram_data();

    if (rvar) run_variance_experiments();
    if (rtime) run_timing_experiments();
    if (rmse) run_mse_experiments();

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

  return 0;
}


/**
 *****************************************************************************
 * @author     This file is part of libsnark, developed by SCIPR Lab
 *             and contributors (see AUTHORS).
 * @copyright  MIT license (see LICENSE file)
 *****************************************************************************/
#include <algorithm>
#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#ifndef MINDEPS
#include <boost/program_options.hpp>
#endif

#include "common/default_types/tinyram_ppzksnark_pp.hpp"
#include "common/profiling.hpp"
#include "reductions/ram_to_r1cs/ram_to_r1cs.hpp"
#include "relations/ram_computations/rams/tinyram/tinyram_params.hpp"
#include "zk_proof_systems/ppzksnark/ram_ppzksnark/ram_ppzksnark.hpp"

#ifndef MINDEPS
namespace po = boost::program_options;

bool process_arithm_command_line(const int argc, const char** argv,
                                 std::string &assembly_fn,
                                 std::string &processed_assembly_fn,
                                 std::string &architecture_params_fn,
                                 std::string &computation_bounds_fn,
                                 std::string &primary_input_fn,
                                 std::string &auxiliary_input_fn)
{
    try
    {
        po::options_description desc("Usage");
        desc.add_options()
            ("help", "print this help message")
            ("assembly", po::value<std::string>(&assembly_fn)->required())
            ("processed_assembly", po::value<std::string>(&processed_assembly_fn)->required())
            ("architecture_params", po::value<std::string>(&architecture_params_fn)->required())
            ("computation_bounds", po::value<std::string>(&computation_bounds_fn)->required())
            ("primary_input", po::value<std::string>(&primary_input_fn)->required())
            ("auxiliary_input", po::value<std::string>(&auxiliary_input_fn)->required());

        po::variables_map vm;
        po::store(po::parse_command_line(argc, argv, desc), vm);

        if (vm.count("help"))
        {
            std::cout << desc << "\n";
            return false;
        }

        po::notify(vm);
    }
    catch(std::exception& e)
    {
        std::cerr << "Error: " << e.what() << "\n";
        return false;
    }

    return true;
}
#endif

using namespace libsnark;

int main(int argc, const char * argv[])
{
    typedef Fr<default_ec_pp> FieldT;
    typedef ram_tinyram<FieldT> default_ram;

    default_ec_pp::init_public_params();

#ifdef MINDEPS
    std::string assembly_fn = "assembly.s";
    std::string processed_assembly_fn = "processed.txt";
    std::string architecture_params_fn = "architecture_params.txt";
    std::string computation_bounds_fn = "computation_bounds.txt";
    std::string primary_input_fn = "primary_input.txt";
    std::string auxiliary_input_fn = "auxiliary_input.txt";
#else
    std::string assembly_fn;
    std::string processed_assembly_fn;
    std::string architecture_params_fn;
    std::string computation_bounds_fn;
    std::string primary_input_fn;
    std::string auxiliary_input_fn;

    if (!process_arithm_command_line(argc, argv, assembly_fn, processed_assembly_fn, architecture_params_fn,
                                     computation_bounds_fn, primary_input_fn, auxiliary_input_fn))
    {
        return 1;
    }
#endif
    start_profiling();

    printf("================================================================================\n");
    printf("TinyRAM example loader\n");
    printf("================================================================================\n\n");

    /* load everything */
    ram_architecture_params<default_ram> ap;
    std::ifstream f_ap(architecture_params_fn);
    f_ap >> ap;

    printf("Will run on %zu register machine (word size = %zu)\n", ap.k, ap.w);

    std::ifstream f_rp(computation_bounds_fn);
    size_t tinyram_input_size_bound, tinyram_program_size_bound, time_bound;
    f_rp >> tinyram_input_size_bound >> tinyram_program_size_bound >> time_bound;

    std::ifstream processed(processed_assembly_fn);
    std::ifstream raw(assembly_fn);
    tinyram_program program = load_preprocessed_program(ap, processed);
    printf("Program:\n%s\n", std::string((std::istreambuf_iterator<char>(raw)),
                                         std::istreambuf_iterator<char>()).c_str());

    std::ifstream f_primary_input(primary_input_fn);
    std::ifstream f_auxiliary_input(auxiliary_input_fn);

    enter_block("Loading primary input");
    tinyram_input_tape primary_input(load_tape(f_primary_input));
    leave_block("Loading primary input");

    enter_block("Loading auxiliary input");
    tinyram_input_tape auxiliary_input = load_tape(f_auxiliary_input);
    leave_block("Loading auxiliary input");

    const size_t boot_trace_size_bound = tinyram_input_size_bound + tinyram_program_size_bound;
    const ram_boot_trace<default_ram> boot_trace = tinyram_boot_trace_from_program_and_input(ap, boot_trace_size_bound, program, primary_input);

    typedef ram_ppzksnark_machine_pp<default_tinyram_ppzksnark_pp> default_ram;

    ram_to_r1cs<default_ram> r(ap, boot_trace_size_bound, time_bound);
    r.instance_map();

    const r1cs_primary_input<FieldT> r1cs_primary_input = ram_to_r1cs<default_ram>::primary_input_map(ap, boot_trace_size_bound, boot_trace);
    const r1cs_auxiliary_input<FieldT> r1cs_auxiliary_input = r.auxiliary_input_map(boot_trace, auxiliary_input);
    const r1cs_constraint_system<FieldT> constraint_system = r.get_constraint_system();

    r.print_execution_trace();
    assert(constraint_system.is_satisfied(r1cs_primary_input, r1cs_auxiliary_input));
}


#include "test.h"
#include "tpofinder/configure.h"
#include "tpofinder/detect.h"

#include <boost/foreach.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <vector>

using namespace cv;
using namespace tpofinder;

class detect : public ::testing::Test {
public:

    virtual void SetUp() {
        models.add(PROJECT_BINARY_DIR + "/data/taco");
        models.add(PROJECT_BINARY_DIR + "/data/blokus");
        detector = Detector(models);
        image = imread(PROJECT_BINARY_DIR + "/data/test/scene-blokus-taco-1.png");
        scene = detector.describe(image);
    }

    Modelbase models;
    Detector detector;
    Mat image;
    Scene scene;

    size_t findIndex(const vector<Detection> detections, const string& modelName) {
        for (size_t i = 0; i < detections.size(); i++) {
            if (detections[i].model.name == modelName) {
                return i;
            }
        }
        return -1;
    }

};

TEST_F(detect, describeSiftCompatible) {
    Feature feature("SIFT", "SIFT", "BruteForce");
    Detector d = Detector(models, feature);
    d.describe(image);
}

TEST_F(detect, describeSurfCompatible) {
    Feature feature("SURF", "SURF", "BruteForce");
    Detector d = Detector(models, feature);
    d.describe(image);
}

TEST_F(detect, sceneHasKeypoints) {
    EXPECT_GE(scene.keypoints.size(), 300);
}

TEST_F(detect, detectTacoInScene) {
    vector<Detection> detections = detector.detect(scene);
    EXPECT_GE(detections.size(), 1);
    EXPECT_GE(findIndex(detections, "taco"), 0);
}

TEST_F(detect, detectTacoInTrainingImage) {
    Scene trainingView = detector.describe(imread(PROJECT_BINARY_DIR + "/data/taco/ref.jpg"));
    vector<Detection> detections = detector.detect(trainingView);
    EXPECT_GE(detections.size(), 1);
    int tacoInd = findIndex(detections, "taco");
    EXPECT_GE(tacoInd, 0);

    Mat h = detections[tacoInd].homography;
    double n = h.at<double>(2, 2);

    EXPECT_NEAR(h.at<double>(0, 0) / n, 1, 0.05);
    EXPECT_NEAR(h.at<double>(1, 1) / n, 1, 0.05);
    EXPECT_NEAR(h.at<double>(0, 1) / n, 0, 0.05);
    EXPECT_NEAR(h.at<double>(1, 0) / n, 0, 0.05);
    // Translation in pixels. Here we must accept some pixel shift.
    EXPECT_NEAR(h.at<double>(0, 2) / n, 0, 5);
    EXPECT_NEAR(h.at<double>(1, 2) / n, 0, 5);
}

/** For the matching process, each model is regarded as a single image with a
 * collection of keypoints and descriptors. The fact that the keypoints and the
 * descriptors originally came from different images is not transparent to the
 * matcher. The imgIdx member of DMatch shall be the index of the matched model.
 * If it is required to find the training image of the model where the keypoint
 * was selected in the first place, then accumulating the number of keypoints in
 * the list of training images will allow to recover this piece of information.
 */
TEST_F(detect, modelMatchesImgIdxRefersToModel) {
    vector<Detection> detections = detector.detect(scene);
    for (size_t i = 0; i < detections.size(); i++) {

        BOOST_FOREACH(DMatch& m, detections[i].matches) {
            EXPECT_EQ(m.imgIdx, i);
        }
    }
}

/** Check whether the trainIdx member of a DMatch is within the bounds; minimum
 * is 0 and maximum is allKeypoints.size() of the model it belongs to. */
TEST_F(detect, modelMatchesTrainIdxWithinBounds) {
    vector<Detection> detections = detector.detect(scene);
    for (size_t i = 0; i < detections.size(); i++) {

        BOOST_FOREACH(DMatch& m, detections[i].matches) {
            EXPECT_GE(m.trainIdx, 0);
            EXPECT_LT(m.trainIdx, detections[i].model.allKeypoints.size());
        }
    }
}

TEST_F(detect, eigenvalueFilterIdentity) {
    Detection d;
    d.homography = Mat::eye(3, 3, CV_64FC1);
    EigenvalueFilter f;
    EXPECT_TRUE(f.accept(d));
}

TEST_F(detect, eigenvalueFilterMultipleOfIdentity) {
    EigenvalueFilter f(1. / 3., 3.);

    Detection d1;
    d1.homography = 4 * Mat::eye(3, 3, CV_64FC1);
    EXPECT_FALSE(f.accept(d1));

    Detection d2;
    d2.homography = 2.9 * Mat::eye(3, 3, CV_64FC1);
    EXPECT_TRUE(f.accept(d2));
}

TEST_F(detect, eigenvalueFilterNonSymmetric) {
    EigenvalueFilter f1(1. / 2., 3.0);
    EigenvalueFilter f2(1. / 3., 3.0);
    EigenvalueFilter f3(1. / 3., 1.5);

    Detection d;
    d.homography = (Mat_<double>(3, 3) <<
            2.0, 0.0, 6.0,
            0.0, 0.4, 2.0,
            0.0, 0.0, 1.0);
    EXPECT_FALSE(f1.accept(d));
    EXPECT_TRUE(f2.accept(d));
    EXPECT_FALSE(f3.accept(d));
}

TEST_F(detect, inliersRatioFilterNoMatches) {
    InliersRatioFilter f;
    Detection d;
    EXPECT_FALSE(f.accept(d));
}


#include <iostream>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

#include <pcl/features/feature.h>
#include <pcl/features/fpfh.h>
#include <pcl/features/3dsc.h>

#include <pcl/keypoints/sift_keypoint.h>
#include <pcl/keypoints/smoothed_surfaces_keypoint.h>

#include <pcl/kdtree/kdtree_flann.h>

#include <pcl/search/kdtree.h>
#include <pcl/search/search.h>
#include <pcl/search/flann_search.h>

#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/obj_io.h>
#include <pcl/io/vtk_io.h>

#include <pcl/registration/transformation_estimation_point_to_plane_lls.h>
#include <pcl/registration/registration.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/ia_ransac.h>

#include <boost/filesystem.hpp>

typedef pcl::PointNormal                    PointType;
typedef pcl::PointCloud<PointType>          CloudType;
typedef pcl::PointCloud<PointType>::Ptr     CloudTypePtr;
typedef pcl::FPFHSignature33                FeatureType;
typedef pcl::PointCloud<FeatureType>        FeatureCloudType;
typedef pcl::PointCloud<FeatureType>::Ptr   FeatureCloudTypePtr;

typedef pcl::PointWithScale                     ScalePointType;
typedef pcl::PointCloud<ScalePointType>         ScaleCloudType;
typedef pcl::PointCloud<ScalePointType>::Ptr    ScaleCloudTypePtr;

using namespace std;

CloudTypePtr loadCloud(const string &filename) {
    using namespace boost::filesystem;
    if (!exists(filename)) {
        PCL_ERROR("File not exists : %s\n", filename.c_str());
        exit(1);
    }
    CloudTypePtr cloud(new CloudType);

    path path(filename);
    if (path.extension() == ".ply") {
        pcl::io::loadPLYFile(filename, *cloud);
    }
    else if (path.extension() == ".pcd") {
        pcl::io::loadPCDFile(filename, *cloud);
    }
    else {
        PCL_ERROR("Unsupported file format : %s\n", filename.c_str());
        exit(1);
    }

    return cloud;
}

void saveCloud(CloudTypePtr cloud, const string &filename) {
    using namespace boost::filesystem;
    path path(filename);
    if (path.extension() == ".ply") {
        pcl::io::savePLYFileBinary(filename, *cloud);
    }
    else if (path.extension() == ".pcd") {
        pcl::io::savePCDFileBinary(filename, *cloud);
    }
    else {
        PCL_ERROR("Unsupported file format : %s\n", filename.c_str());
        exit(1);
    }
}

CloudTypePtr downsampledCloudWithNumberOfPoints(CloudTypePtr cloud, int points)
{
    if (cloud->size() <= points)
        return cloud;

    size_t size = cloud->size();
    CloudTypePtr downsampledCloud(new CloudType);

    boost::random::mt19937 rng;
    boost::random::uniform_int_distribution<> six(0, size - 1);
    for (int i = 0; i < points; ++i)
    {
        downsampledCloud->points.push_back(cloud->points[six(rng)]);
    }
    downsampledCloud->width = downsampledCloud->points.size();
    downsampledCloud->height = 1;
    downsampledCloud->is_dense = true;
    return downsampledCloud;
}

void detect_keypoints(CloudTypePtr cloud, float min_scale, int nr_octaves, int nr_scale_per_octave, float min_contrast, ScaleCloudTypePtr &out)
{
    pcl::SIFTKeypoint<PointType, ScalePointType> sift;
    pcl::search::KdTree<PointType>::Ptr tree(new pcl::search::KdTree<PointType>);
    sift.setSearchMethod(tree);
    sift.setScales(min_scale, nr_octaves, nr_scale_per_octave);
    sift.setMinimumContrast(min_contrast);
    sift.setInputCloud(cloud);
    sift.setSearchSurface(cloud);
    sift.setKSearch(150);
    sift.compute(*out);
}

int main(int argc, char *argv[]) {
    if (argc < 3) {
        printf("Usage: %s <target_cloud> <input_cloud>\n", argv[0]);
        return 0;
    }
    string filename(argv[2]);
    string ext = filename.substr(filename.rfind('.'));
    string outfile = filename.substr(0, filename.rfind('.')) + "-aligned" + ext;

    CloudTypePtr cloud0 = loadCloud(argv[1]);
    CloudTypePtr cloud1 = loadCloud(argv[2]);

    CloudTypePtr transformed(new CloudType);
    CloudTypePtr aligned_cloud(new CloudType);
    ScaleCloudTypePtr out(new ScaleCloudType);

    float p1 = 0.01f;
    int p2 = 3, p3 = 1;
    float p4 = 0.f;
    sscanf(argv[3], "%f", &p1);
    sscanf(argv[4], "%d", &p2);
    sscanf(argv[5], "%d", &p3);
    sscanf(argv[6], "%f", &p4);

    detect_keypoints(cloud0, p1, p2, p3, p4, out);
    if (transformed->size() == 0) {
        PCL_ERROR("No keypoints!");
    }
    else {
        saveCloud(transformed, outfile);
    }
    return 0;

    FeatureCloudType feature_cloud0;
    FeatureCloudType feature_cloud1;

    pcl::search::KdTree<PointType>::Ptr tree(new pcl::search::KdTree<PointType>);
    pcl::FPFHEstimation<PointType, PointType, FeatureType> fpfh;
    fpfh.setSearchMethod(tree);
    fpfh.setKSearch(120);
    CloudTypePtr down0 = downsampledCloudWithNumberOfPoints(cloud0, 20000);
    fpfh.setInputCloud(down0);
    fpfh.setInputNormals(cloud0);
    fpfh.setSearchSurface(cloud0);
    fpfh.compute(feature_cloud0);

    CloudTypePtr down1 = downsampledCloudWithNumberOfPoints(cloud1, 20000);
    fpfh.setInputCloud(down1);
    fpfh.setInputNormals(cloud1);
    fpfh.setSearchSurface(cloud1);
    fpfh.compute(feature_cloud1);

    pcl::SampleConsensusInitialAlignment<PointType, PointType, FeatureType> sac;
    sac.setMinSampleDistance(0.15);
    sac.setMaxCorrespondenceDistance(0.15);
    sac.setMaximumIterations(1000);
//    Provide a pointer to the input point cloud and features
    sac.setInputCloud(down1);
    sac.setSourceFeatures(feature_cloud1.makeShared());
//    Provide a pointer to the target point cloud and features
    sac.setInputTarget(down0);
    sac.setTargetFeatures(feature_cloud0.makeShared());
//    Align input to target to obtain
    sac.align(*aligned_cloud);
    if (sac.hasConverged()) {
        PCL_INFO("\tSAC fitness score is %.6f\n", sac.getFitnessScore());
        PCL_INFO("\tTransform matrix :\n");
        cout << sac.getFinalTransformation() << endl;
        saveCloud(aligned_cloud, outfile);
    }
    else {
        PCL_ERROR("Can't align cloud pair!\n");
    }
    return 0;

    pcl::IterativeClosestPoint<PointType, PointType> icp;
    typedef pcl::registration::TransformationEstimationPointToPlaneLLS<PointType, PointType> P2PlaneEstimation;
    boost::shared_ptr<P2PlaneEstimation> point_to_plane(new P2PlaneEstimation);

    printf("Align %s( %d points ) and %s( %d points )\n", argv[1], cloud0->size(), argv[2], cloud1->size());

    icp.setInputCloud(downsampledCloudWithNumberOfPoints(cloud1, 50000));
    icp.setInputTarget(downsampledCloudWithNumberOfPoints(cloud0, 50000));
    icp.setMaxCorrespondenceDistance(0.25);
    icp.setMaximumIterations(50);
    icp.setTransformationEpsilon(1e-6);
    icp.setTransformationEstimation(point_to_plane);
    icp.align(*transformed);

    if (icp.hasConverged()) {

        PCL_INFO("\tICP fitness score is %.6f\n", icp.getFitnessScore());
        PCL_INFO("\tTransform matrix :\n");
        cout << icp.getFinalTransformation() << endl;
        saveCloud(transformed, outfile);
        /*
        Eigen::Affine3f changed;
        changed = icp.getFinalTransformation();
        if (changed.translation().maxCoeff() > m_params.translationThres || changed.translation().minCoeff() < -m_params.translationThres) {
            PCL_WARN("Pair align translate too much!! Reject\n");
            pair.corresIdx = std::make_pair(-1, -1);
            return;
        }

        Eigen::AngleAxisf angle;
        angle.fromRotationMatrix(changed.linear());
        if (fabsf(angle.angle()) > m_params.rotationThres * M_PI / 180) {
            PCL_WARN("Pair align rotate too much!! Reject\n");
            pair.corresIdx = std::make_pair(-1, -1);
            return;
        }

        pair.relativeTrans = icp.getFinalTransformation();
        */
    }
    else {
        //PCL_WARN("\tCan't align pair <%d, %d>\n", pair.corresIdx.first, pair.corresIdx.second);
        //pair.corresIdx = std::make_pair(-1, -1);
    }

    return 0;
}


/*  Copyright (c) 2013, Bo Li, prclibo@gmail.com
    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 copyright holder nor the
          names of its contributors may be used to endorse or promote products
          derived from this software without specific prior written permission.

    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
    ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
    WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
    DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
*/

/****************************************************************************
 *							IMPORTANT NOTICE								*
 ****************************************************************************
 *																			*
 * Most of this code was completele rewritten by Josef Maier				*
 * (josef.maier.fl@ait.ac.at) as the original code was not working			*
 * correctly. Moreover, additional functionalities (like ARRSAC or			*
 * refinement procedures) were added. Most interfaces also differ			*
 * completely to the original ones.											*
 *																			*
 * For further information please contact Josef Maier, AIT Austrian			*
 * Institute of Technology.													*
 *																			*
 ****************************************************************************/

#ifndef FIVE_POINT_HPP
#define FIVE_POINT_HPP

#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>

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

#include <opencv2/core/eigen.hpp>

#define ARRSAC 3

using namespace cv;

bool findEssentialMat(OutputArray Essential, InputArray points1, InputArray points2, /*double focal = 1.0, Point2d pp = Point2d(0, 0), */
					int method = cv::RANSAC,
					double prob = 0.999, double threshold = 1, OutputArray mask = noArray(), bool lesqu = false,
					void (*refineEssential)(cv::InputArray points1, cv::InputArray points2, cv::InputArray E_init,
											cv::Mat & E_refined, double th, unsigned int iters, bool makeClosestE,
											double *sumSqrErr_init, double *sumSqrErr,
											cv::OutputArray errors, cv::InputOutputArray mask, int model, bool tryOrientedEpipolar, bool normalizeCorrs) = NULL);

void decomposeEssentialMat( const Mat & E, Mat & R1, Mat & R2, Mat & t );

int recoverPose( const Mat & E, InputArray points1, InputArray points2, Mat & R, Mat & t, Mat & Q3D,
					/*double focal = 1.0, Point2d pp = Point2d(0, 0), */
					InputOutputArray mask = noArray(), const double dist = 50.0, cv::InputArray t_only = cv::noArray());

inline bool isZero(double d)
{
    //Decide if determinants, etc. are too close to 0 to bother with
    const double EPSILON = 1e-3;
    return (d<EPSILON) && (d>-EPSILON);
}

#endif


//  (C) Copyright Matt Borland 2021.
//  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)

#ifndef BOOST_MATH_CCMATH_ISNAN
#define BOOST_MATH_CCMATH_ISNAN

#include <cmath>
#include <type_traits>
#include <boost/math/tools/is_constant_evaluated.hpp>

namespace boost::math::ccmath {

template <typename T>
inline constexpr bool isnan(T x)
{
    if(BOOST_MATH_IS_CONSTANT_EVALUATED(x))
    {
        return x != x;
    }
    else
    {
        using std::isnan;

        if constexpr (!std::is_integral_v<T>)
        {
            return isnan(x);
        }
        else
        {
            return isnan(static_cast<double>(x));
        }
    }
}

}

#endif // BOOST_MATH_CCMATH_ISNAN


#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/stl.h>

#include <Eigen/Dense>

#include <kontiki/trajectories/uniform_r3_spline_trajectory.h>
#include "trajectory_helper.h"
#include "spline_helpers.h"

namespace py = pybind11;

namespace TT = kontiki::trajectories;

struct PyR3SplineHelper {
  using PyControlPointType = Eigen::Vector3d;

  static Eigen::Vector3d ConvertCppToPy(const Eigen::Vector3d& cp) {
    return cp;
  }

  static Eigen::Vector3d ConvertPyToCpp(const Eigen::Vector3d& cp) {
    return cp;
  }
};

PYBIND11_MODULE(_uniform_r3_spline_trajectory, m) {
  m.doc() = "Uniform splined trajectory in R3";

  using Class = TT::UniformR3SplineTrajectory;
  using Helper = PyR3SplineHelper;
  auto cls = py::class_<Class, std::shared_ptr<Class>>(m, "UniformR3SplineTrajectory");
  cls.doc() = R"pbdoc( A spline with control points in R3

  Control points are vectors in R3.
  )pbdoc";

  // Common attributes
  declare_spline_common<Class, Helper>(cls);
  declare_trajectory_common<Class>(cls);

} // PYBIND11_MODULE


/*
 * This file is part of the DUDS project. It is subject to the BSD-style
 * license terms in the LICENSE file found in the top-level directory of this
 * distribution and at https://github.com/jjackowski/duds/blob/master/LICENSE.
 * No part of DUDS, including this file, may be copied, modified, propagated,
 * or distributed except according to the terms contained in the LICENSE file.
 *
 * Copyright (C) 2017  Jeff Jackowski
 */
#include <duds/BuildConfig.h>
#ifdef HAVE_INT128
#include <boost/multiprecision/cpp_int.hpp>
#include <sstream>

namespace duds { namespace data {

std::istream &operator >> (std::istream &is, __int128 &b) {
	boost::multiprecision::int128_t mpi;
	is >> mpi;
	b = static_cast<__int128>(mpi);
	return is;
}
std::ostream &operator << (std::ostream &os, __int128 const &b) {
	boost::multiprecision::int128_t mpi(b);
	return os << mpi;
}

} }

#endif


/*
 * EigenDef.hpp
 *
 *  Created on: Oct 27, 2016
 *      Author: wyan
 */

#ifndef EIGENDEF_HPP_
#define EIGENDEF_HPP_

#include <cmath>
#include <vector>

#define EIGEN_DONT_PARALLELIZE // 1 thread for any eigen stuff
#include <Eigen/Cholesky>
#include <Eigen/Dense>
#include <Eigen/Sparse>

/*
 * Do not use auto with Eigen
 *
 * Eigen defaults to column-major storage.
 * Mat(i,j) is not affected by row major or column major storage format
 * */

constexpr double Pi = 3.141592653589793238462643383279;

// unaligned fixed size vectors
using Evec2 = Eigen::Matrix<double, 2, 1, Eigen::DontAlign>;
using Evec3 = Eigen::Matrix<double, 3, 1, Eigen::DontAlign>;
using Evec4 = Eigen::Matrix<double, 4, 1, Eigen::DontAlign>;
using Evec6 = Eigen::Matrix<double, 6, 1, Eigen::DontAlign>;

// unaligned fixed size matrices
using Emat2 = Eigen::Matrix<double, 2, 2, Eigen::DontAlign>;
using Emat3 = Eigen::Matrix<double, 3, 3, Eigen::DontAlign>;
using Emat4 = Eigen::Matrix<double, 4, 4, Eigen::DontAlign>;
using Emat6 = Eigen::Matrix<double, 6, 6, Eigen::DontAlign>;
using EmatPsi =
    Eigen::Matrix<double, 4, 3,
                  Eigen::DontAlign>; // psi mat for quaternion rotation

// unaligned quaternion
using Equatn = Eigen::Quaternion<double, Eigen::DontAlign>;

// aligned fixed size vectors
using EAvec2 = Eigen::Vector2d;
using EAvec3 = Eigen::Vector3d;
using EAvec4 = Eigen::Vector4d;
using EAvec6 = Eigen::Matrix<double, 6, 1>;

// aligned fixed size matrices
using EAmat2 = Eigen::Matrix2d;
using EAmat3 = Eigen::Matrix3d;
using EAmat4 = Eigen::Matrix4d;
using EAmat6 = Eigen::Matrix<double, 6, 6>;
using EAmatPsi = Eigen::Matrix<double, 4, 3, Eigen::DontAlign>;

// aligned quaternion
using EAquatn = Eigen::Quaterniond;

// map from raw pointer to fixed size type
using Emap3 = Eigen::Map<Evec3>;

// dynamic size types
using Evec = Eigen::VectorXd;
using Emat = Eigen::Matrix<double, Eigen::Dynamic,
                           Eigen::Dynamic>; // default to column major
using Espmat = Eigen::SparseMatrix<double>;

// copy swap
template <typename T> void copySwap(T &a, T &b) {
    T temp = b;
    b = a;
    a = b;
}

#endif /* EIGENDEF_HPP_ */