/*
 * ModelPropertyTest.cpp
 *
 *  Created on: 14-05-2009
 *      Author: jimali
 */

#include <rw/core/Log.hpp>
#include <rw/graspplanning/GraspTable.hpp>
#include <rw/math/MetricUtil.hpp>
#include <rw/math/Random.hpp>
#include <rw/math/Vector3D.hpp>

#include <boost/numeric/conversion/cast.hpp>

using namespace rw::core;
using namespace rw::graspplanning;
using namespace rw::math;

int main(int argc, char** argv)
{
	const double EPSILON = 0.000001;
	for(int i=0;i<1000000;i++){
		//generate random vector
		Vector3D<> v(Random::ran(0,1),Random::ran(0,1),Random::ran(0,1));
		if(MetricUtil::norm2(v)<0.001)
			continue;

		Vector3D<> normal = normalize(v);
		// create perpendicular vector
        Vector3D<> tdir;
        if( fabs(normal(0))<EPSILON && fabs(normal(1))<EPSILON ){
            tdir = normalize(  Vector3D<>(0,-normal(2),normal(1)) );
        } else {
            tdir = normalize(  Vector3D<>(-normal(1),normal(0),0) );
        }
        //std::cout << "Angle: " << (angle(tdir,normal)*Rad2Deg) << std::endl;
        double ang = 90-angle(tdir,normal)*Rad2Deg;
        if(fabs(ang)>2)
        	std::cout << "Angle: " << (angle(tdir,normal)*Rad2Deg) << std::endl;
	}
	exit(0);

	if( argc < 3 ){
		std::cout << "------ Usage: " << std::endl;
	    std::cout << "- Arg 1 name of grasp table" << std::endl;
	    std::cout << "- Arg 2 size of group\n" << std::endl;
	    return 0;
	}
	std::string filename(argv[1]);
	int groupSize = 1;
	if(argc>2)
	    groupSize = std::atoi(argv[2]);

	Ptr<GraspTable> gtable = GraspTable::load(filename);
	RW_ASSERT(gtable!=NULL);
	Log::infoLog() << "Table size: " << gtable->size() << std::endl;
	int nrOfGroups = boost::numeric_cast<int>(gtable->size())/groupSize;
	int globalStat = 0;
	for(int i=0;i<nrOfGroups; i++){
		GraspTable::GraspData &ngrasp = gtable->getData()[i*groupSize];
		int localStat = 0;
		for(int j=0;j<groupSize-1;j++){
			GraspTable::GraspData &grasp = gtable->getData()[i*groupSize+1+j];
			// test if any of the angles are too far away
			EAA<> eaa1 = grasp.hp.getEAA();
			EAA<> eaa2 = ngrasp.hp.getEAA();
			Vector3D<> v1(eaa1(0),eaa1(1),eaa1(2));
			Vector3D<> v2(eaa2(0),eaa2(1),eaa2(2));
			if( MetricUtil::dist2(v1,v2)>0.5 ){
				grasp = ngrasp;
				localStat++;
				continue;
			}
		}
		globalStat += localStat;
		std::cout << "Stat: " << localStat << "/" << groupSize << std::endl;
	}
	std::cout << "Stat: " << globalStat << "/" << groupSize*nrOfGroups << std::endl;
	gtable->save("grasptable_out.txt");
	return 0;
}


/*
 * utilities_trapz_test.cpp Test fixtures for the trapz function
 *
 * Author:                   Tom Clark (thclark @ github)
 *
 * Copyright (c) 2016-9 Octue Ltd. All Rights Reserved.
 *
 */

#include "gtest/gtest.h"
#include <Eigen/Dense>

#include "utilities/cumsum.h"

using namespace utilities;

class CumsumTest : public ::testing::Test {};

TEST_F(CumsumTest, test_cumsum) {

    // Test that zeros are returned when trying to colwise integrate an array with 1 row
    Eigen::VectorXd in(4);
    in << 1, 2, 6, 9;
    Eigen::VectorXd out_correct(4);
    out_correct << 1, 3, 9, 18;

    Eigen::VectorXd out;
    cumsum(out, in);
    EXPECT_EQ(out.matrix(), out_correct.matrix());

}


// solving A * X = B
// using driver function gesv()

//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <cstddef>
#include <iostream>
#include <boost/numeric/bindings/blas.hpp>
#include <boost/numeric/bindings/atlas/clapack.hpp>
#include <boost/numeric/bindings/traits/tnt.hpp>
#ifndef F_FORTRAN
#  include <tnt/tnt_array2d_utils.h>
#else
#  include <tnt/tnt_fortran_array2d_utils.h>
#endif

namespace blas = boost::numeric::bindings::blas;

using std::size_t;
using std::cout;
using std::endl;

#ifndef F_FORTRAN
typedef TNT::Array2D<double> m_t;
#else
typedef TNT::Fortran_Array2D<double> m_t;
#endif

int main() {

  cout << endl;
  size_t n = 3, nrhs = 1;

  m_t a (n, n);   // system matrix
#ifndef F_FORTRAN
  a[0][0] = 1.; a[0][1] = 1.; a[0][2] = 1.;
  a[1][0] = 2.; a[1][1] = 3.; a[1][2] = 1.;
  a[2][0] = 1.; a[2][1] = -1.; a[2][2] = -1.;
#else
  a(1,1) = 1.; a(1,2) = 1.; a(1,3) = 1.;
  a(2,1) = 2.; a(2,2) = 3.; a(2,3) = 1.;
  a(3,1) = 1.; a(3,2) = -1.; a(3,3) = -1.;
#endif

// see leading comments for `gesv()' in clapack.hpp
#ifndef F_FORTRAN
  m_t b (nrhs, n);  // right-hand side matrix
  b[0][0] = 4.; b[0][1] = 9.; b[0][2] = -2.;
#else
  m_t b (n, nrhs);
  b(1,1) = 4.; b(2,1) = 9.; b(3,1) = -2.;
#endif

  cout << "A: " << a << endl;
  cout << "B: " << b << endl;

  blas::lu_solve (a, b);
  cout << "X: " << b << endl;

  cout << endl;
}


#include <gtest/gtest.h>
#include <iostream>
#include <sstream>
#include <string>
#include <stdexcept>
#include <Eigen/Dense>

// testing following API
#include "estimation/IEstimator.h"
#include "estimation/ExtendedKalmanFilter.h"
#include "estimation/Input.h"
#include "estimation/InputValue.h"
#include "estimation/Output.h"
#include "estimation/OutputValue.h"

using namespace std;
using namespace estimation;

namespace ExtendedKalmanFilterTest
{
  void f(VectorXd& x, const VectorXd& u)
  {
    x[0] = x[0] + u[0];
  }
  void df(MatrixXd& A, const VectorXd& x, const VectorXd& u)
  {
    A(0,0) = 1;
  }
  void h(VectorXd& z, const VectorXd& x)
  {
    z[0] = x[0];
  }
  void dh(MatrixXd& H, const VectorXd& x)
  {
    H(0,0) = 1;
  }

  void f2(VectorXd& x, const VectorXd& u)
  {
    VectorXd x_apriori(x.size());

    x_apriori[0] = x[0] + 0.1*x[1];
    x_apriori[1] = x[1];

    x = x_apriori;
  }
  void df2(MatrixXd& A, const VectorXd& x, const VectorXd& u)
  {
    A(0,0) = 1;
    A(0,1) = 0.1;
    A(1,0) = 0;
    A(1,1) = 1;
  }
  void h2(VectorXd& z, const VectorXd& x)
  {
    z[0] = x[1];
  }
  void dh2(MatrixXd& H, const VectorXd& x)
  {
    H(0,0) = 0;
    H(0,1) = 1;
  }

  // -----------------------------------------
  // tests
  // -----------------------------------------
  TEST(ExtendedKalmanFilterTest, initialization)
  {
    ExtendedKalmanFilter ekf;

    // check setting required params
    EXPECT_THROW(ekf.validate(), IEstimator::estimator_error);	// "STM missing"
    ekf.setStateTransitionModel(f);
    EXPECT_THROW(ekf.validate(), IEstimator::estimator_error);	// "Jacobian of STM missing"
    ekf.setJacobianOfStateTransitionModel(df);
    EXPECT_THROW(ekf.validate(), IEstimator::estimator_error);	// "OM missing"
    ekf.setObservationModel(h);
    EXPECT_THROW(ekf.validate(), IEstimator::estimator_error);	// "Jacobian of OM missing"
    ekf.setJacobianOfObservationModel(dh);
    EXPECT_THROW(ekf.validate(), IEstimator::estimator_error);	// "PNC missing"
    MatrixXd Q(1,1); Q << 0.1;
    ekf.setProcessNoiseCovariance(Q);
    EXPECT_THROW(ekf.validate(), IEstimator::estimator_error);	// "MNC missing"
    MatrixXd R(1,1); R << 10;
    ekf.setMeasurementNoiseCovariance(R);
    EXPECT_NO_THROW(ekf.validate());			// all required params given

    // check setting optional params
    // optional params doesn't set new sizes
    MatrixXd P0_f(2,2); P0_f << 1, 0, 0, 1;
    ekf.setInitialErrorCovariance(P0_f);
    EXPECT_NO_THROW(ekf.validate());
    EXPECT_EQ(ekf.getState().size(), 1);

    VectorXd x_f(2); x_f << 0,1;
    ekf.setInitialState(x_f);
    EXPECT_NO_THROW(ekf.validate());
    EXPECT_NE(ekf.getState().size(), 2);

    // required param Q sets new size of x and out
    MatrixXd Q2(2,2); Q2 << 0.1, 0, 0, 0.1;
    ekf.setProcessNoiseCovariance(Q2);
    EXPECT_NO_THROW(ekf.validate());
    EXPECT_EQ(ekf.getState().size(), 2);
    EXPECT_EQ(ekf.getLastEstimate().size(), 2);

    // check initialization of output
    Output out = ekf.getLastEstimate();
    OutputValue defaultOutVal;
    EXPECT_GT(out.size(), 0);
    EXPECT_DOUBLE_EQ(out.getValue(), defaultOutVal.getValue());

    // check setting the control input
    InputValue ctrl(0);
    Input in_ctrl(ctrl);
    EXPECT_THROW(ekf.setControlInput(in_ctrl), length_error);
    ekf.setControlInputSize(1);
    EXPECT_NO_THROW(ekf.setControlInput(in_ctrl));
  }

  TEST(ExtendedKalmanFilterTest, validation)
  {
    ExtendedKalmanFilter ekf;

    // check validation itself (e.g. invalid sizes of vectors/matrices,
    // empty callbacks, etc.)
    EXPECT_THROW(ekf.validate(), IEstimator::estimator_error);	// "STM missing"
    ekf.setStateTransitionModel(0);
    EXPECT_THROW(ekf.validate(), IEstimator::estimator_error);	// "STM missing"
    ekf.setStateTransitionModel(f);
    ekf.setJacobianOfStateTransitionModel(0);
    EXPECT_THROW(ekf.validate(), IEstimator::estimator_error);	// "STMJ missing"
    ekf.setJacobianOfStateTransitionModel(df);
    ekf.setObservationModel(0);
    EXPECT_THROW(ekf.validate(), IEstimator::estimator_error);	// "OM missing"
    ekf.setObservationModel(h);
    ekf.setJacobianOfObservationModel(0);
    EXPECT_THROW(ekf.validate(), IEstimator::estimator_error);	// "OMJ missing"
    ekf.setJacobianOfObservationModel(dh);

    EXPECT_THROW(ekf.validate(), IEstimator::estimator_error);	// "PNC missing"
    MatrixXd Q_f(2,1); Q_f << 0.1, 1;
    ekf.setProcessNoiseCovariance(Q_f);
    EXPECT_THROW(ekf.validate(), IEstimator::estimator_error);	// "Q not square"
    MatrixXd Q1(1,1); Q1 << 0.1;
    ekf.setProcessNoiseCovariance(Q1);
    EXPECT_THROW(ekf.validate(), IEstimator::estimator_error);	// "MNC missing"
    MatrixXd R_f(2,1); R_f << 10, 1;
    ekf.setMeasurementNoiseCovariance(R_f);
    EXPECT_THROW(ekf.validate(), IEstimator::estimator_error);	// "R not square"
    MatrixXd R(1,1); R << 10;
    ekf.setMeasurementNoiseCovariance(R);

    EXPECT_NO_THROW(ekf.validate());				// all required params given

    // optional params
    VectorXd x(2); x << 0, 1;
    ekf.setInitialState(x);
    EXPECT_NO_THROW(ekf.validate());
    EXPECT_NE(ekf.getState().size(), 2);

    MatrixXd Q(2,2); Q << 0.1, 0, 0, 0.1;
    ekf.setProcessNoiseCovariance(Q);
    EXPECT_NO_THROW(ekf.validate());
    ekf.setInitialState(x);
    EXPECT_NO_THROW(ekf.validate());
    EXPECT_EQ(ekf.getState().size(), 2);			// sizes now ok

    // size of R cannot be checked, must match number of formulas in h
    // -> check during compile time
  }

  TEST(ExtendedKalmanFilterTest, validationEffect)
  {
    ExtendedKalmanFilter ekf;

    ekf.setStateTransitionModel(f);
    ekf.setJacobianOfStateTransitionModel(df);
    ekf.setObservationModel(h);
    ekf.setJacobianOfObservationModel(dh);
    MatrixXd Q(1,1); Q << 0.1;
    ekf.setProcessNoiseCovariance(Q);
    MatrixXd R(1,1); R << 10;
    ekf.setMeasurementNoiseCovariance(R);
    ekf.setControlInputSize(1);		// initializes u, so it can be used in f

    // validate has an effect?
    InputValue measurement(1);
    Input in(measurement);
    Output out;

    EXPECT_THROW(out = ekf.estimate(in), IEstimator::estimator_error);	// "not yet validated"

    EXPECT_NO_THROW(ekf.validate());
    EXPECT_NO_THROW(out = ekf.estimate(in));

    // changing a parameter -> EKF must be re-validated
    ekf.setProcessNoiseCovariance(Q);
    EXPECT_THROW(out = ekf.estimate(in), IEstimator::estimator_error);	// "not yet validated"
    EXPECT_NO_THROW(ekf.validate());

    // EKF should now be released and return an estimate (should not be
    // the default)
    EXPECT_NO_THROW(out = ekf.estimate(in));
    EXPECT_GT(out.size(), 0);
    OutputValue defaultOutVal;
    EXPECT_NE(out.getValue(), defaultOutVal.getValue());
  }

  TEST(ExtendedKalmanFilterTest, functionality)
  {
    ExtendedKalmanFilter ekf;

    ekf.setStateTransitionModel(f);
    ekf.setJacobianOfStateTransitionModel(df);
    ekf.setObservationModel(h);
    ekf.setJacobianOfObservationModel(dh);
    MatrixXd Q(1,1); Q << 0.1;
    ekf.setProcessNoiseCovariance(Q);
    MatrixXd R(1,1); R << 10;
    ekf.setMeasurementNoiseCovariance(R);

    ekf.validate();

    // check if the calculation of an estimate is correct
    // first measurement z1:
    InputValue measurement(1);
    Input in(measurement);
    Output out;

    // u contained in a state transition formula, so an error will be
    // thrown when u is not initialized with the correct size
    //out = ekf.estimate(in);		// fail assertion produced by Eigen (out-of-range error)
    ekf.setControlInputSize(1);
    ekf.validate();
    EXPECT_NO_THROW(out = ekf.estimate(in));

    EXPECT_EQ(out.size(), 1);
    EXPECT_NEAR(out[0].getValue(), 0.0099, 0.0001);
    EXPECT_NEAR(out[0].getVariance(), 0.0990, 0.0001);

    // next measurement z2:
    in[0].setValue(5);

    // setting a control input = 0 does not change the result, it can
    // be calculated as if there is no control input
    InputValue ctrl(0);
    Input in_ctrl(ctrl);
    ekf.setControlInput(in_ctrl);	// (no validation needed, cannot be validated)

    EXPECT_NO_THROW(out = ekf.estimate(in));

    EXPECT_NEAR(out[0].getValue(), 0.1073, 0.0001);
    EXPECT_NEAR(out[0].getVariance(), 0.1951, 0.0001);

    // missing measurement
    InputValue missingValue;
    Input inMissing(missingValue);
    EXPECT_NO_THROW(out = ekf.estimate(inMissing));

    EXPECT_NEAR(out[0].getValue(), 0.1073, 0.0001);
    EXPECT_NEAR(out[0].getVariance(), 0.2866, 0.0001);

    // another example -----------------------------------
    ExtendedKalmanFilter ekf2;
    ekf2.setStateTransitionModel(f2);
    ekf2.setJacobianOfStateTransitionModel(df2);
    ekf2.setObservationModel(h2);
    ekf2.setJacobianOfObservationModel(dh2);
    MatrixXd Q2(2,2); Q2 << 0.1, 0, 0, 0.1;
    ekf2.setProcessNoiseCovariance(Q2);
    MatrixXd R2(1,1); R2 << 10;
    ekf2.setMeasurementNoiseCovariance(R2);

    ekf2.validate();

    in[0].setValue(1);
    EXPECT_NO_THROW(out = ekf2.estimate(in));
    EXPECT_EQ(out.size(), 2);

    EXPECT_NEAR(out[0].getValue(), 0.0, 0.0001);
    EXPECT_NEAR(out[0].getVariance(), 0.1, 0.0001);
    EXPECT_NEAR(out[1].getValue(), 0.0099, 0.0001);
    EXPECT_NEAR(out[1].getVariance(), 0.0990, 0.0001);

    // missing measurement
    EXPECT_NO_THROW(out = ekf2.estimate(inMissing));

    EXPECT_NEAR(out[0].getValue(), 0.00099, 0.0001);
    EXPECT_NEAR(out[1].getValue(), 0.0099, 0.0001);
  }

}


#pragma once

#include <ctime>
#include <cstdlib>

#include <vpp/vpp.hh>
#include "vpp/algorithms/line_tracker_4_sfm/miscellanous/operations.hh"

#include <Eigen/Core>
#include <Eigen/SVD>
#include <unsupported/Eigen/Polynomials>

using namespace vpp;
using namespace std;
using namespace Eigen;
using namespace iod;
using namespace cv;

namespace vpp
{

//from http://www.mip.informatik.uni-kiel.de/tiki-index.php?page=Lilian+Zhang

struct structure_from_motion_ctx {
    structure_from_motion_ctx(box2d domain) : keypoints(domain) {}

    // Keypoint container.
    // ctx.keypoint[i] to access the ith keypoint.
    keypoint_container<keypoint<int>, int> keypoints;

    std::vector<vfloat3> start_points;
    std::vector<vfloat3> end_points;
    std::vector<vfloat3> directions;
};

structure_from_motion_ctx structure_from_motion_init(box2d domain);

void initialize_rotation_matrix();
void initialize_translation_matrix();
void pose_estimation_from_line_correspondence(MatrixXf start_points, MatrixXf end_points,
                                              MatrixXf directions, MatrixXf points,
                                              MatrixXf &rot_cw, VectorXf &pos_cw);
inline
void cal_campose(MatrixXf XXc, MatrixXf XXw, int n, MatrixXf &R2, VectorXf &t2);
inline
void r_and_t(MatrixXf &rot_cw, VectorXf &pos_cw,MatrixXf start_points, MatrixXf end_points,
             MatrixXf P1w,MatrixXf P2w,MatrixXf initRot_cw,VectorXf initPos_cw,
             int maxIterNum,float TerminateTh,int nargin);

inline
vfloat3 x_cross(vfloat3 a,vfloat3 b);

void triangulation();
void bundle_adjustement();

}

#include "structure_from_motion.hpp"


/*!
 * \file QBezierLine3D.hpp
 * \author Jun Yoshida
 * \copyright (c) 2019 Jun Yoshida.
 * The project is released under the MIT License.
 * \date Descember 6, 2019: created
 */

#pragma once

#include <Eigen/Dense>

#include "../math/Bezier.hpp"
#include "../PathScheme.hpp"
#include "PathFigure3D.hpp"

class QBezierLine3D : public PathFigure3D, public Bezier<Eigen::Vector3d,2>
{
public:
    using BezierT = Bezier<Eigen::Vector3d,2>;

private:
    //! Three control points
    Eigen::Vector3d m_init, m_ctrl, m_term;

public:
    QBezierLine3D(std::array<double,3> const &init, std::array<double,3> const &ctrl, std::array<double,3> const &term)
        : BezierT(
            Eigen::Vector3d(init[0], init[1], init[2]),
            Eigen::Vector3d(ctrl[0], ctrl[1], ctrl[2]),
            Eigen::Vector3d(term[0], term[1], term[2])
            )
    {}

    virtual ~QBezierLine3D() = default;

    //! Methods inherited from PathFigure
    void draw(SchemeType &scheme) const override {
        scheme.moveTo(BezierT::get<0>());
        scheme.qbezierTo(BezierT::get<1>(), BezierT::get<2>());
        scheme.stroke();
    }
};


#include "precompiled.h"

#include "NetworkSenderApp.h"

#define WIN32_LEAN_AND_MEAN
#include <windows.h>

#include <boost/format.hpp>

Vector3 getDerive(const Vector3 &_p, const Vector3 &_q, const Real &_dt, const Real &_T)
{
    return (_p - _q) * (_T/(3*_dt));

}

NetworkSenderApp::NetworkSenderApp()
	:mAnimState(NULL)
	,mAnimState2(NULL)
	,mUdpSocket(0)
	,mIpAddress("")
	,mConnected(1)
    ,mTimeSinceLastUpdate(0)
	,mCurrentSpeed(Vector3::ZERO)
	,mIsMoving(false)
    ,mHasMoved(false)
    ,mSamplingInterval(1.0)
    ,mTimeSinceLastSpeedSample(0.0)
    ,mLastTimeDelta(0.0)
{
    mTitle = "Sender";
}
//------------------------------------------------------------------------------
NetworkSenderApp::~NetworkSenderApp(void)
{
}
//------------------------------------------------------------------------------
bool NetworkSenderApp::frameStarted(const FrameEvent& evt)
{
	if (mAnimState)
		mAnimState->addTime(evt.timeSinceLastFrame);

	if (mAnimState2)
		mAnimState2->addTime(evt.timeSinceLastFrame);

    mTimeSinceLastUpdate += evt.timeSinceLastFrame;
    mTimeSinceLastSpeedSample += evt.timeSinceLastFrame;

    Vector3 currentPos = mBallNode->getPosition();

    if (! mIsMoving)
    {
        if(currentPos != mLastBallPosition)
        {
            mHasMoved = true;
            mIsMoving = true;
            //mCurrentSpeed = getDerive(currentPos, mLastBallPosition, evt.timeSinceLastFrame);
            mCurrentSpeed =  0.33 * (currentPos - mLastBallPosition);
            mLastTimeDelta = evt.timeSinceLastFrame;
            mLastBallPosition = currentPos;
        }
    }
    else
    {
        if(mTimeSinceLastUpdate > mSamplingInterval)
        {
             mCurrentSpeed = getDerive(currentPos, mLastBallPosition
                                      ,evt.timeSinceLastFrame
                                      ,mSamplingInterval);
            mLastBallPosition = currentPos;
            _sendPosition();
            mTimeSinceLastUpdate = 0;
        }
        //else if (mTimeSinceLastSpeedSample > mSamplingInterval/10)
        //{
        //    mCurrentSpeed = getDerive(currentPos, mLastBallPosition, mSamplingInterval/10);
        //    mCurrentSpeed *= (mSamplingInterval) / 3;
        //    mLastTimeDelta = evt.timeSinceLastFrame;
        //    mLastBallPosition = currentPos;
        //    mTimeSinceLastSpeedSample = 0.0;
        //
        //}
    }

    mLastBallPosition = currentPos;

   	return true;
}
//------------------------------------------------------------------------------
bool NetworkSenderApp::frameEnded_(const FrameEvent& evt)
{
    //if(mLastBallPosition == mBallNode->getPosition())
    //{
    //    if(mIsMoving)
    //    {
    //        mIsMoving = false;
    //        mCurrentSpeed = Vector3::ZERO;
    //    }
    //}
    //else
    //{
    //    if(!mIsMoving)
    //    {
    //
    //    }
    //}

	//mTimeSinceLastUpdate += evt.timeSinceLastFrame;
	//_sendPosition();

	return true;
}
//------------------------------------------------------------------------------
void NetworkSenderApp::createScene()
{

	const RenderSystemCapabilities* caps = Root::getSingleton().getRenderSystem()->getCapabilities();
	if (!caps->hasCapability(RSC_VERTEX_PROGRAM) || !(caps->hasCapability(RSC_FRAGMENT_PROGRAM)))
	{
		OGRE_EXCEPT(Exception::ERR_NOT_IMPLEMENTED, "Your card does not support vertex and fragment programs, so cannot "
			"run this demo. Sorry!",
			"createScene");
	}

	Viewport *vp = mWindow->getViewport(0);
	vp->setBackgroundColour(ColourValue(0.7, 0.7, 0.7));

	mRoot->addFrameListener(this);

	mSceneMgr->setNormaliseNormalsOnScale(true);

	_createAxes(5);
	_createGrid(5);

		mCamera->setPosition(Vector3(100, 100, 100));
	mCamera->lookAt(Vector3::ZERO);

	_createLight();

	mBallNode = mSceneMgr->getRootSceneNode()->createChildSceneNode("Ball Node");
	Entity *ent = mSceneMgr->createEntity("my ball", "sphere.mesh");
	//ent->setMaterialName("Objects/Ball");

	ent->getSubEntity(0)->setMaterialName("Examples/CelShading");

	ent->getSubEntity(0)->setCustomParameter(0, Vector4(10.0f, 0.0f, 0.0f, 0.0f));
	ent->getSubEntity(0)->setCustomParameter(1, Vector4(0.0f, 0.5f, 0.0f, 1.0f));
	ent->getSubEntity(0)->setCustomParameter(2, Vector4(0.3f, 0.5f, 0.3f, 1.0f));

	float w = ent->getBoundingBox().getSize().x;
	float ws = 10.0 / w;

	mBallNode->attachObject(ent);
	mBallNode->setScale(ws, ws, ws);

	float totalTime = 8;
	float halfTime  = totalTime/2;

	Animation *anim = mSceneMgr->createAnimation("bouncing ball", totalTime);
	anim->setDefaultInterpolationMode(Animation::IM_SPLINE);
	NodeAnimationTrack *track = anim->createNodeTrack(0, mBallNode);
	TransformKeyFrame *key = track->createNodeKeyFrame(0);

	// start
	key->setScale(Vector3(ws, ws, ws));
	key->setTranslate(Vector3(0, 0, 0));

	// first half
	key = track->createNodeKeyFrame(halfTime * 1./4);
	key->setScale(Vector3(ws, ws, ws));
	key->setTranslate(Vector3(50, 0, -25));

	key = track->createNodeKeyFrame(halfTime * 2./4);
	key->setScale(Vector3(ws, ws, ws));
	key->setTranslate(Vector3(100, 0, 0));

	key = track->createNodeKeyFrame(halfTime * 3./4);
	key->setScale(Vector3(ws, ws, ws));
	key->setTranslate(Vector3(50, 0, 25));

	key = track->createNodeKeyFrame(halfTime);
	key->setScale(Vector3(ws, ws, ws));
	key->setTranslate(Vector3(0, 0, 0));

	// second half
	key = track->createNodeKeyFrame(halfTime + halfTime * 1./4);
	key->setScale(Vector3(ws, ws, ws));
	key->setTranslate(Vector3(-50, 0, -25));

	key = track->createNodeKeyFrame(halfTime + halfTime * 2./4);
	key->setScale(Vector3(ws, ws, ws));
	key->setTranslate(Vector3(-100, 0, 0));

	key = track->createNodeKeyFrame(halfTime + halfTime * 3./4);
	key->setScale(Vector3(ws, ws, ws));
	key->setTranslate(Vector3(-50, 0, 25));

	key = track->createNodeKeyFrame(totalTime);
	key->setScale(Vector3(ws, ws, ws));
	key->setTranslate(Vector3(0, 0, 0));

	mAnimState = mSceneMgr->createAnimationState("bouncing ball");
	mAnimState->setEnabled(1);

    _readConfigurationFromFile();
	_initNetwork();

	// start of track
	mLastBallPosition = Vector3(0, 0, 0);
}
//------------------------------------------------------------------------------
void NetworkSenderApp::_createAxes(int _nUnits)
{
	mGridNode = mSceneMgr->getRootSceneNode()->createChildSceneNode("WorldGrid Node");
	ManualObject *line = mSceneMgr->createManualObject("X Axis");
	int scale = 100;

	line->begin("WorldGrid/XAxis", RenderOperation::OT_LINE_LIST);
	line->position(-_nUnits, 0.0, 0.0);     line->normal(Vector3::UNIT_Y);      line->colour(0.1, 0.0, 0.0);
	line->position( _nUnits, 0.0, 0.0);     line->normal(Vector3::UNIT_Y);      line->colour(1.0, 0.0, 0.0);
	line->end();
	mGridNode->attachObject(line);

	line = mSceneMgr->createManualObject("Y Axis");
	line->begin("WorldGrid/YAxis", RenderOperation::OT_LINE_LIST);
	line->position(0.0, -_nUnits, 0.0);     line->normal(Vector3::UNIT_Y);      line->colour(0.0, 0.1, 0.0);
	line->position(0.0,  _nUnits, 0.0);     line->normal(Vector3::UNIT_Y);      line->colour(0.0, 1.0, 0.0);
	line->end();
	mGridNode->attachObject(line);

	line = mSceneMgr->createManualObject("Z Axis");
	line->begin("WorldGrid/ZAxis", RenderOperation::OT_LINE_LIST);
	line->position( 0.0, 0.0, -_nUnits);     line->normal(Vector3::UNIT_Y);      line->colour(0.0, 0.0, 0.1);
	line->position( 0.0, 0.0,  _nUnits);     line->normal(Vector3::UNIT_Y);      line->colour(0.0, 0.0, 1.0);
	line->end();
	mGridNode->attachObject(line);

	mGridNode->scale(scale, scale, scale);

}
//------------------------------------------------------------------------------
void NetworkSenderApp::_createGrid(int _nUnits)
{
	float step = 0.1;
	int nUnits = _nUnits;

	ManualObject *grid = mSceneMgr->createManualObject("Grid Lines");
	grid->begin("WorldGrid/Lines", RenderOperation::OT_LINE_LIST);

	grid->colour(0.5, 0.5, 0.5);

	for(int i=1 ; i <= int(nUnits * (1.0/step)) ; i++)
	{
		float offset=i*step;

		// horizontal (parallel to X axis)

		grid->position( -nUnits, 0.0, offset);   grid->normal(Vector3::UNIT_Y);
		grid->position(  nUnits, 0.0, offset);   grid->normal(Vector3::UNIT_Y);

		grid->position( -nUnits, 0.0, -offset);   grid->normal(Vector3::UNIT_Y);
		grid->position(  nUnits, 0.0, -offset);   grid->normal(Vector3::UNIT_Y);

		// vertical (parallel to Z axis)
		grid->position( offset, 0.0, -nUnits);   grid->normal(Vector3::UNIT_Y);
		grid->position( offset, 0.0,  nUnits);   grid->normal(Vector3::UNIT_Y);

		grid->position( -offset, 0.0, -nUnits);   grid->normal(Vector3::UNIT_Y);
		grid->position( -offset, 0.0,  nUnits);   grid->normal(Vector3::UNIT_Y);

	}

	grid->end();

	mGridNode->attachObject(grid);

}
//------------------------------------------------------------------------------
void NetworkSenderApp::_createLight()
{
	Light *light = mSceneMgr->createLight("Main Light");
	light->setType(Light::LT_POINT);
	light->setPosition(Vector3(0, 100, 0));
	light->setSpecularColour(ColourValue::White);
	light->setDiffuseColour(ColourValue::White);

	mLightNode = mSceneMgr->getRootSceneNode()->createChildSceneNode("light node");
	mLightNode->attachObject(light);

	Animation *anim = mSceneMgr->createAnimation("light track", 4);
	anim->setDefaultInterpolationMode(Animation::IM_SPLINE);

	NodeAnimationTrack *track = anim->createNodeTrack(0, mLightNode);
	TransformKeyFrame *key = track->createNodeKeyFrame(0);
	key->setTranslate(Vector3(100, 100, 0));
	key = track->createNodeKeyFrame(1);
	key->setTranslate(Vector3(100, 100, 100));
	key = track->createNodeKeyFrame(2);
	key->setTranslate(Vector3(-100, 100, 0));
	key = track->createNodeKeyFrame(3);
	key->setTranslate(Vector3(-100, 100, -100));
	key = track->createNodeKeyFrame(4);
	key->setTranslate(Vector3(100, 100, 0));

	mAnimState2 = mSceneMgr->createAnimationState("light track");
	mAnimState2->setEnabled(1);

}
//------------------------------------------------------------------------------
void NetworkSenderApp::_readConfigurationFromFile()
{
    ConfigFile cf;
    cf.load("sender.cfg");

    mIpAddress = cf.getSetting("Peer Address", "Network");
    mUdpPort   =  cf.getSetting("UDP Port", "Network");

    mSamplingInterval = StringConverter::parseReal(cf.getSetting("Sampling Interval", "Network"));

}
//------------------------------------------------------------------------------
void NetworkSenderApp::_initNetwork()
{
	mNetworkLog = LogManager::getSingleton().createLog("network.log");

	mUdpResolver = new udp::resolver(mIOService);
	mUdpQuery = new udp::resolver::query(udp::v4(), mIpAddress, mUdpPort);

    mUdpReceiverEndpoint = *(mUdpResolver->resolve(*mUdpQuery));

	mUdpSocket = new udp::socket(mIOService);
	mUdpSocket->open(udp::v4());

    mConnected = true;

}
//------------------------------------------------------------------------------
void NetworkSenderApp::_sendPosition()
{
	if(mConnected)
	{
        Vector3 pos = mBallNode->getPosition();
        Vector3 speed = mCurrentSpeed;

		boost::format fmt("[new pdu] position (%.2f  %.2f  %.2f) speed (%.3f  %.3f  %.3f) dt %.6f");
		fmt % pos.x % pos.y % pos.z % speed.x % speed.y % speed.z % mLastTimeDelta;
		mNetworkLog->logMessage(fmt.str());

        _sendPdu(pos, speed);
	}
}
//------------------------------------------------------------------------------
void NetworkSenderApp::_sendPdu(const Vector3 &_pos, const Vector3 &_speed)
{
    char arr[6*sizeof(Real)];

    memcpy(arr,                 _pos.ptr(),   3*sizeof(Real));
    memcpy(arr+3*sizeof(Real),  _speed.ptr(), 3*sizeof(Real));

    mUdpSocket->send_to(boost::asio::buffer(arr, 6*sizeof(Real))
                        ,mUdpReceiverEndpoint
                        ,0
                        ,mSocketError);

    if (mSocketError)
    {
        boost::format fmt("socket error : %d");
        fmt % mSocketError;
        mNetworkLog->logMessage(fmt.str());
    }
}
//------------------------------------------------------------------------------
void NetworkSenderApp::_sendFloat(float _val)
{
	char arr[4];
	memcpy(arr, &_val, sizeof(_val));

	//int n = boost::asio::write(*mSocket
	//						  ,boost::asio::buffer(arr, sizeof(_val))
	//						  ,boost::asio::transfer_all()
	//						  ,mSocketError);

	mUdpSocket->send_to(boost::asio::buffer(arr, sizeof(_val))
					   ,mUdpReceiverEndpoint
					   ,0
					   ,mSocketError);

	if (mSocketError)
	{
		boost::format fmt("socket error : %d");
		fmt % mSocketError;
		mNetworkLog->logMessage(fmt.str());
	}
}
//------------------------------------------------------------------------------
void NetworkSenderApp::_sleep(int _ms)
{
	// sleep 20ms
	boost::xtime sleeptime;
	boost::xtime_get(&sleeptime, boost::TIME_UTC);
	sleeptime.nsec += 1000000 * _ms;
	boost::thread::sleep(sleeptime);
}
//------------------------------------------------------------------------------


#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <iostream>
#include <boost/math/special_functions/bessel.hpp>

namespace py = pybind11;

// N is the number of zeros we are looking for
// L is an array of orders l
py::array_t<double> bessel_zeros(int N, py::array_t<uint64_t> L) {

  py::buffer_info info = L.request();
  if (info.ndim != 1)
      throw std::runtime_error("Number of dimensions must be one");
  // Number of entries in the L array
  int Nl = info.shape[0];
  // Accessing the array values
  uint64_t *Lptr = static_cast<uint64_t *>(info.ptr);

  // Allocate the qln table and copy over the zeros
  size_t size = Nl*N;
  double *qln = new double[size];

  #pragma omp parallel for schedule(dynamic)
  for(int l=0; l < Nl; l++) {
      std::vector<long double> roots;
      boost::math::cyl_bessel_j_zero((double) (Lptr[l]+0.5), 1, N, std::back_inserter(roots));
      for(int p=0; p <N; p++) {
          qln[N*l + p] = roots[p];
      }
  }
  // Create a Python object that will free the allocated
  // memory when destroyed:
  py::capsule free_when_done(qln, [](void *f) {
      double *qln = reinterpret_cast<double *>(f);
      delete[] qln;
  });

  return py::array_t<double>(
      {Nl, N}, // shape
      {N*8, 8}, // C-style contiguous strides for double
      qln, // the data pointer
      free_when_done); // numpy array references this parent
}

PYBIND11_MODULE(bessel_tools, m) {
  m.doc() = "Module for Bessel stuff";
  m.def("bessel_zeros", &bessel_zeros, "compute Bessel zeros");
}


/**
 * @file   Vector.cpp
 * @author ALIKAWA Hidehisa <alleyhide@gmail.com>
 * @date   2018/07/07
 *
 * @brief  class Vector
 *
 * Released under the MIT lisence
 */
#include <iostream>
#include <boost/numeric/ublas/io.hpp>

#include "gweyl.hpp"

namespace gweyl{

struct VectorRootSpace::Impl {
    RootSpace space_;
    NumberVector simpleCoefficients_;///< coefficients for simple roots coordinate
    NumberVector fundamentalCoefficients_;///< coefficients for fundamental weights coordinate
};

VectorRootSpace::VectorRootSpace(Type X, NumberVector& v, Coordinate c)
    : pImpl(std::make_unique<Impl>())
{
    RootSpace V(X, v.size());

    pImpl->space_ = V;

    if (c == Coordinate::simple){
        pImpl->simpleCoefficients_ = v;
        matrix A = pImpl->space_.CartanMatrix();
        pImpl->fundamentalCoefficients_ = prod(A, v);
    }else {
        matrix P = pImpl->space_.InverseCartanMatrix();
        pImpl->simpleCoefficients_ = prod(P, v);
        pImpl->fundamentalCoefficients_ = v;
    }
}

VectorRootSpace::VectorRootSpace(): pImpl(std::make_unique<Impl>())
{
}

VectorRootSpace::~VectorRootSpace(){
}

VectorRootSpace::VectorRootSpace(const VectorRootSpace& rhs): pImpl(std::make_unique<Impl>())
{
    RootSpace V(rhs.type(), rhs.rank());
    pImpl->space_ = V;
    pImpl->simpleCoefficients_ = rhs.simpleCoefficients();
    pImpl->fundamentalCoefficients_ = rhs.fundamentalCoefficients();
}

VectorRootSpace& VectorRootSpace::operator=(const VectorRootSpace& rhs){

    // this function does not check the equality of root space
    // because *this is may defined invalid

    RootSpace V(rhs.type(), rhs.rank());
    pImpl->space_ = V;
    pImpl->simpleCoefficients_ = rhs.simpleCoefficients();
    pImpl->fundamentalCoefficients_ = rhs.fundamentalCoefficients();

    return *this;
}

void VectorRootSpace::printf(){
    std::cout << "simple " << pImpl->simpleCoefficients_ << std::endl;
    std::cout << "fundamental " << pImpl->fundamentalCoefficients_ << std::endl;
}

NumberVector VectorRootSpace::simpleCoefficients() const{
    return pImpl->simpleCoefficients_;
}

NumberVector VectorRootSpace::simpleCoefficients(){
    return pImpl->simpleCoefficients_;
}

NumberVector VectorRootSpace::fundamentalCoefficients() const{
    return pImpl->fundamentalCoefficients_;
}

NumberVector VectorRootSpace::fundamentalCoefficients(){
    return pImpl->fundamentalCoefficients_;
}

Type VectorRootSpace::type(){
    return pImpl->space_.type();
}

Type VectorRootSpace::type() const{
    return pImpl->space_.type();
}

unsigned VectorRootSpace::rank(){
    return pImpl->space_.rank();
}

unsigned VectorRootSpace::rank() const{
    return pImpl->space_.rank();
}

bool VectorRootSpace::isInSameSpace(const VectorRootSpace& rhs){
    if (this->type() != rhs.type()){
        return false;
    }

    if (rank() != rhs.rank()){
        return false;
    }
    return true;
}

bool VectorRootSpace::operator==(const VectorRootSpace& rhs){

    if (!isInSameSpace(rhs)){
        return false;
    }

    if (!equal(pImpl->fundamentalCoefficients_, rhs.fundamentalCoefficients())){
        return false;
    }

    return true;
}

bool VectorRootSpace::operator!=(const VectorRootSpace& rhs){
    return !(*this == rhs);
}

VectorRootSpace& VectorRootSpace::operator+=(const VectorRootSpace& rhs){

    if (!isInSameSpace(rhs)){
        std::string msg{"+= of VectorRootSpace error "};
        msg += "LHS ";
        msg += std::to_string(static_cast<int>(type()));
        msg += " ";
        msg += std::to_string(rank());
        msg += ", RHS ";
        msg += std::to_string(static_cast<int>(rhs.type()));
        msg += " ";
        msg += std::to_string(rhs.rank());
        std::runtime_error e(msg);
        throw e;
    }

    pImpl->simpleCoefficients_ += rhs.simpleCoefficients();
    pImpl->fundamentalCoefficients_ += rhs.fundamentalCoefficients();

    return *this;
}

VectorRootSpace& VectorRootSpace::operator-=(const VectorRootSpace& rhs){

    if (!isInSameSpace(rhs)){
        std::string msg{"-= of VectorRootSpace error "};
        msg += "LHS ";
        msg += std::to_string(static_cast<int>(type()));
        msg += " ";
        msg += std::to_string(rank());
        msg += ", RHS ";
        msg += std::to_string(static_cast<int>(rhs.type()));
        msg += " ";
        msg += std::to_string(rhs.rank());
        std::runtime_error e(msg);
        throw e;
    }

    pImpl->simpleCoefficients_ -= rhs.simpleCoefficients();
    pImpl->fundamentalCoefficients_ -= rhs.fundamentalCoefficients();

    return *this;
}

VectorRootSpace& VectorRootSpace::operator*=(rational r){
    pImpl->simpleCoefficients_ *= r;
    pImpl->fundamentalCoefficients_ *= r;

    return *this;
}

bool VectorRootSpace::dominant(){

    NumberVector nv = fundamentalCoefficients();
    for (rational x : nv){
        if (x < 0){
            return false;
        }
    }

    return true;
}

bool VectorRootSpace::integral(){

    NumberVector nv = fundamentalCoefficients();
    for (rational x : nv){
        if (x.denominator() != 1){
            return false;
        }
    }

    return true;
}

bool VectorRootSpace::isDominantIntegral(){
    return ((dominant()) && (integral()));
}

VectorRootSpace operator+(const VectorRootSpace& v1, const VectorRootSpace& v2){
    VectorRootSpace w(v1);
    w += v2;
    return w;
}

VectorRootSpace operator-(const VectorRootSpace& v1, const VectorRootSpace& v2){
    VectorRootSpace w(v1);
    w -= v2;
    return w;
}

VectorRootSpace operator*(const VectorRootSpace& v1, const rational r){
    VectorRootSpace w(v1);
    w *= r;
    return w;
}

VectorRootSpace operator*(const rational r, const VectorRootSpace& v1){
    VectorRootSpace w(v1);
    w *= r;
    return w;
}

}


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/all_of.hpp>
#include <boost/hana/assert.hpp>
#include <boost/hana/cartesian_product.hpp>
#include <boost/hana/contains.hpp>
#include <boost/hana/core/to.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/functional/demux.hpp>
#include <boost/hana/fuse.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/minus.hpp>
#include <boost/hana/mod.hpp>
#include <boost/hana/not.hpp>
#include <boost/hana/not_equal.hpp>
#include <boost/hana/or.hpp>
#include <boost/hana/plus.hpp>
#include <boost/hana/set.hpp>
#include <boost/hana/transform.hpp>
#include <boost/hana/tuple.hpp>
namespace hana = boost::hana;
using namespace hana::literals;

// A function that can have an arbitrary compile-time set of values as a domain
// and co-domain. This is most likely purely of theoretical interest, but it
// allows creating functions with very complex domains and co-domains that are
// computed at compile-time.

struct Function { };

template <typename Domain, typename Codomain, typename F>
struct function_type {
    using hana_tag = Function;

    Domain domain_;
    Codomain codomain_;
    F f_;

    template <typename X>
    constexpr auto operator()(X x) const {
        BOOST_HANA_CONSTANT_ASSERT(boost::hana::contains(domain_, x));
        return f_(x);
    }
};

template <typename ...F, typename ...G>
constexpr auto operator==(function_type<F...> f, function_type<G...> g)
{ return hana::equal(f, g); }

template <typename ...F, typename ...G>
constexpr auto operator!=(function_type<F...> f, function_type<G...> g)
{ return hana::not_equal(f, g); }

auto function = [](auto domain, auto codomain) {
    return [=](auto definition) {
        return function_type<decltype(domain), decltype(codomain), decltype(definition)>{
            domain, codomain, definition
        };
    };
};

template <typename Function>
constexpr auto domain(Function f)
{ return f.domain_; }

template <typename Function>
constexpr auto codomain(Function f)
{ return f.codomain_; }

template <typename Function>
constexpr auto range(Function f) {
    // We must convert to hana::tuple first because hana::set is not a Functor
    return hana::to_set(hana::transform(hana::to_tuple(domain(f)), f));
}

template <typename P, typename Q>
constexpr auto implies(P p, Q q) {
    return hana::or_(hana::not_(p), q);
}

template <typename F>
constexpr auto is_injective(F f) {
    auto dom = hana::to_tuple(domain(f));
    auto pairs = hana::cartesian_product(hana::make_tuple(dom, dom));
    return hana::all_of(pairs, hana::fuse([&](auto x, auto y) {
        return implies(hana::not_equal(x, y), hana::not_equal(f(x), f(y)));
    }));
}

template <typename F>
constexpr auto is_onto(F f) {
    return codomain(f) == range(f);
}

namespace boost { namespace hana {
    template <>
    struct equal_impl<Function, Function> {
        template <typename F, typename G>
        static constexpr auto apply(F f, G g) {
            return domain(f) == domain(g) &&
                   hana::all_of(domain(f), hana::demux(hana::equal)(f, g));
        }
    };
}} // end namespace boost::hana

int main() {
    auto f = function(hana::make_set(1_c, 2_c, 3_c), hana::make_set(1_c, 2_c, 3_c, 4_c, 5_c, 6_c))(
        [](auto x) { return x + 1_c; }
    );

    auto g = function(hana::make_set(1_c, 2_c, 3_c), hana::make_set(2_c, 3_c, 4_c))(
        [](auto x) { return x + 1_c; }
    );

    auto h = function(hana::make_set(1_c, 2_c, 3_c), hana::make_set(0_c, 1_c, 2_c))(
        [](auto x) { return x - 1_c; }
    );

    BOOST_HANA_CONSTANT_CHECK(f == g);
    BOOST_HANA_CONSTANT_CHECK(f != h);
    BOOST_HANA_CONSTANT_CHECK(f(1_c) == 2_c);

    BOOST_HANA_CONSTANT_CHECK(range(f) == hana::make_set(4_c, 3_c, 2_c));
    BOOST_HANA_CONSTANT_CHECK(range(g) == hana::make_set(2_c, 3_c, 4_c));
    BOOST_HANA_CONSTANT_CHECK(range(h) == hana::make_set(0_c, 1_c, 2_c));

    BOOST_HANA_CONSTANT_CHECK(hana::not_(is_onto(f)));
    BOOST_HANA_CONSTANT_CHECK(is_onto(g));
    BOOST_HANA_CONSTANT_CHECK(is_onto(h));

    auto even = function(hana::make_set(1_c, 2_c, 3_c), hana::make_set(hana::true_c, hana::false_c))(
        [](auto x) { return x % 2_c == 0_c; }
    );

    BOOST_HANA_CONSTANT_CHECK(is_injective(f));
    BOOST_HANA_CONSTANT_CHECK(is_injective(g));
    BOOST_HANA_CONSTANT_CHECK(is_injective(h));
    BOOST_HANA_CONSTANT_CHECK(hana::not_(is_injective(even)));
}


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/math/vector/object.hpp>
#include <fcppt/math/vector/static.hpp>
#include <fcppt/math/vector/unit.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	math_vector_unit
)
{
FCPPT_PP_POP_WARNING

	typedef fcppt::math::vector::static_<
		int,
		3
	> int3_vector;

	int3_vector const vec(
		fcppt::math::vector::unit<
			int3_vector
		>(
			2
		)
	);

	BOOST_CHECK_EQUAL(
		vec[0],
		0
	);

	BOOST_CHECK_EQUAL(
		vec[1],
		0
	);

	BOOST_CHECK_EQUAL(
		vec[2],
		1
	);
}


#include <Eigen/Dense>
using Eigen::MatrixXf;

#define SGA_USE_EIGEN
#include <sga.hpp>
#include "../common/common.hpp"

#include "../common/json.hpp"
using json = nlohmann::json;

#define STB_IMAGE_IMPLEMENTATION
#include "../common/stb_image.h"

#include <iostream>
#include <fstream>
#include <cassert>

inline bool file_exists(std::string path) {
  return std::ifstream(path).good();
}

void usage(char** argv){
  std::cout << "USAGE: " << argv[0] << " CONFIG_FILE" << std::endl;
}

Eigen::Matrix3f find_homography(std::vector<Eigen::Vector2f> points1, std::vector<Eigen::Vector2f> points2){
  assert(points1.size() == points2.size());
  Eigen::MatrixXf A = Eigen::MatrixXf::Zero(points1.size()*2,9);
  for(unsigned int i = 0; i < points1.size(); i++){
    float x1 = points1[i](0), y1 = points1[i](1);
    float x2 = points2[i](0), y2 = points2[i](1);
    Eigen::Matrix<float, 2, 9> Q;
    Q <<
      x1, y1, 1,  0,  0, 0, -x1*x2, -y1*x2, -x2,
      0 , 0 , 0, x1, y1, 1, -x1*y2, -y1*y2, -y2;
    A.block(i*2,0,2,9) = Q;
  }

  Eigen::JacobiSVD<Eigen::MatrixXf> svd(A, Eigen::ComputeThinU | Eigen::ComputeFullV);
  unsigned int P = svd.matrixV().cols();
  Eigen::Map<const Eigen::Matrix3f> view(svd.matrixV().block(0,P-1,9,1).data(), 3, 3);
  Eigen::Matrix3f H = view.transpose() / view(2,2);

  return H;
}

int main(int argc, char** argv){
  if(argc < 2) {
    usage(argv);
    return 1;
  }
  std::string config_path = argv[1];
  size_t q = config_path.find_last_of("/\\");
  std::string config_dir = config_path.substr(0,q);
  std::string output_path = "output.png";

  // Enable sga
  sga::init();
  renderdoc_tryenable();
  renderdoc_capture_start();

  // Open and parse config file
  std::ifstream config_file(config_path);
  if(!config_file.good()){
    std::cout << "Failed to open file \"" << config_path << "\"" << std::endl;
    return 1;
  }
  std::vector<std::string> image_paths;
  std::vector<std::pair<std::vector<Eigen::Vector2f>, std::vector<Eigen::Vector2f>>> points;
  unsigned int center = 0;
  unsigned int N;
  bool render_lines = false;
  try{
    json config;
    config_file >> config;
    auto images = config["images"];
    if(!images.is_array()){
      std::cout << "Json config must contain an array of strings \"images\" representing a list of input images." << std::endl;
      return 1;
    }
    for(auto& q : images){
      image_paths.push_back(q);
    }
    N = image_paths.size();
    if(config.count("center") > 0){
      center = config["center"];
    }
    if(config.count("lines") > 0){
      render_lines = config["lines"] != 0;
    }
    auto pointlist = config["points"];
    if(!pointlist.is_array() || pointlist.size() != N-1){
      std::cout << "Json config must contain an array of point coordinates \"points\" of the lentgh equal to the number of images minus one (" << N-1 << ")." << std::endl;
      return 1;
    }
    for(auto& pointspair : pointlist){
      std::vector<Eigen::Vector2f> pl;
      std::vector<Eigen::Vector2f> pr;
      if(pointspair.size() != 2){
        std::cout << "Each entry in \"points\" array must have two elements (a list of points in image N and a list of corresponding points on image N+1)" << std::endl;
        return 1;
      }
      for(auto& point : pointspair[0]){
        Eigen::Vector2f p;
        if(point.size() != 2){
          std::cout << "Each point must be an array of 2 numbers." << std::endl;
          return 1;
        }
        p(0) = point[0]; p(1) = point[1];
        pl.push_back(p);
      }
      for(auto& point : pointspair[1]){
        Eigen::Vector2f p;
        if(point.size() != 2){
          std::cout << "Each point must be an array of 2 numbers." << std::endl;
          return 1;
        }
        p(0) = point[0]; p(1) = point[1];
        pr.push_back(p);
      }
      points.push_back(std::make_pair(pl,pr));
    }
  }catch (json::exception& e){
    std::cout << "Error reading json config: " << e.what() << std::endl;
  }

  // Print out configuration
  /*
  for(unsigned int i = 0; i < N; i++){
    std::cout << image_paths[i] << std::endl;
    if(i < N-1){
      for(auto p : points[i].first){
        std::cout << "[" << p(0) << ", " << p(1) << "] ";
      }
      std::cout << std::endl;
      for(auto p : points[i].second){
        std::cout << "[" << p(0) << ", " << p(1) << "] ";
      }
      std::cout << std::endl;
    }
  }
  */

  // Compute homographies
  std::cout << "Preparing Hs" << std::endl;
  std::vector<Eigen::Matrix3f> Hs;
  Eigen::Matrix3f H0 = Eigen::Matrix3f::Identity(3,3);
  Hs.push_back(H0);

  for(unsigned int i = center+1; i < N; i++){
    std::cout << "Computing H between " << i-1 << " and " << i << std::endl;
    auto Hdiff = find_homography(points[i-1].first, points[i-1].second);
    auto Habs = Hdiff * Hs.back();
    Hs.push_back(Habs);
  }
  for(int i = center-1; i > -1; i--){
    std::cout << "Computing H between " << i+1 << " and " << i << std::endl;
    auto Hdiff = find_homography(points[i].second, points[i].first);
    auto Habs = Hdiff * Hs.front();
    Hs.insert(Hs.begin(), Habs);
  }
  assert(Hs.size() == N);

  // Compute inverses
  std::vector<Eigen::Matrix3f> His;
  for(const auto& H : Hs)
    His.push_back(H.inverse());

  // Load images
  std::cout << "Loading images" << std::endl;
  std::vector<sga::Image> images;
  for(std::string image_path : image_paths){
    image_path = config_dir + "/" + image_path;

    int w,h,n;
    stbi_set_flip_vertically_on_load(1);
    unsigned char* data = stbi_load(image_path.c_str(), &w, &h, &n, 4);
    if(!data){
      std::cout << "Opening image '" << image_path << "' failed: " << stbi_failure_reason() << std::endl;
      return 1;
    }
    sga::Image image(w, h, 4, sga::ImageFormat::NInt8, sga::ImageFilterMode::Anisotropic);
    image.putData(std::vector<uint8_t>(data, data + w*h*4));
    free(data);

    images.push_back(image);
  }

  std::cout << "Preparing render resources" << std::endl;

  // Prepare image bounding boxes
  Eigen::ArrayXXf imgBBs(4*N,3);
  for(unsigned int i = 0; i < N; i++){
    auto& p = images[i];
    auto& Hi = His[i];
    Eigen::Vector3f v0(0,0,1);
    Eigen::Vector3f v1(0,p.getHeight(),1);
    Eigen::Vector3f v2(p.getWidth(),0,1);
    Eigen::Vector3f v3(p.getWidth(),p.getHeight(),1);
    imgBBs.block(4*i + 0, 0, 1, 3) = (Hi*v0).transpose();
    imgBBs.block(4*i + 1, 0, 1, 3) = (Hi*v1).transpose();
    imgBBs.block(4*i + 2, 0, 1, 3) = (Hi*v2).transpose();
    imgBBs.block(4*i + 3, 0, 1, 3) = (Hi*v3).transpose();
  }
  imgBBs.colwise() /= imgBBs.col(2);
  Eigen::Vector3f BBmax, BBmin;
  BBmax = imgBBs.colwise().maxCoeff();
  BBmin = imgBBs.colwise().minCoeff();
  Eigen::Vector3f BBsize = BBmax - BBmin;
  Eigen::Vector3f offset = BBmin;
  std::cout << "offset " << offset << std::endl;

  // Create target image
  sga::Image result(BBsize(0), BBsize(1));

  // Prepare a VBO
  struct VertData{
    Eigen::Vector2f pos;
  };
  std::vector<VertData> vertices = {
    {{0,0}},{{0,1}},{{1,0}},
    {{1,1}},{{1,0}},{{0,1}}
  };
  sga::VBO vbo({sga::DataType::Float2}, vertices.size());
  vbo.write(vertices);

  // Prepare shaders
  auto vertShader = sga::VertexShader::createFromSource(R"(
    mat3 align = mat3(vec3(2, 0, 0),vec3(0, 2, 0),vec3(-1, -1, 1));
    void main(){
      vec2 pos_texturespace = in_position * textureSize(image,0);
      vec3 pos_transformed_homog = H * vec3(pos_texturespace,1);
      float perspective_factor = 1/pos_transformed_homog.z;
      vec2 pos_transformed = pos_transformed_homog.xy/pos_transformed_homog.z;
      vec2 pos3 = 2*(pos_transformed - offset)/sgaResolution.xy - 1;
      gl_Position = vec4(pos3,0,1);
      imageUVW = vec3(in_position.x, 1 - in_position.y, 1) * perspective_factor;
    }
  )");
  vertShader.addInput(sga::DataType::Float2, "in_position");
  vertShader.addOutput(sga::DataType::Float3, "imageUVW");
  vertShader.addSampler("image");
  vertShader.addUniform(sga::DataType::Mat3, "H");
  vertShader.addUniform(sga::DataType::Float2, "offset");
  auto fragShader = sga::FragmentShader::createFromSource(R"(
    void main(){
      out_color = textureProj(image, imageUVW);
    }
  )");
  fragShader.addOutput(sga::DataType::Float4, "out_color");
  fragShader.addInput(sga::DataType::Float3, "imageUVW");
  fragShader.addSampler("image");
  auto program = sga::Program::createAndCompile(vertShader,fragShader);

  // Prepare pipeline
  sga::Pipeline pipeline;
  pipeline.setProgram(program);
  pipeline.setTarget({result});
  pipeline.setFaceCull(sga::FaceCullMode::None);
  pipeline.setBlendModeColor(sga::BlendFactor::One, sga::BlendFactor::OneMinusSrcAlpha);
  pipeline.setBlendModeAlpha(sga::BlendFactor::One, sga::BlendFactor::OneMinusSrcAlpha);
  pipeline.clear();

  std::cout << "Rendering" << std::endl;

  // Draw!
  pipeline.uniform["offset"] = Eigen::Vector2f{offset(0), offset(1)};
  for(unsigned int i = 0; i < N; i++){
    pipeline.sampler["image"] = images[i];
    pipeline.uniform["H"] = His[i];
    std::cout << His[i] << std::endl;
    pipeline.draw(vbo);
  }

  // Prepare another pipeline, just for rendering image edges
  if(render_lines){
    auto linesFragShader = sga::FragmentShader::createFromSource(R"(
      void main(){out_color = vec4(1,0,0,0.2);}
    )");
    linesFragShader.addOutput(sga::DataType::Float4, "out_color");
    linesFragShader.addInput(sga::DataType::Float3, "imageUVW");
    auto lines_program = sga::Program::createAndCompile(vertShader,linesFragShader);
    sga::Pipeline lines_pipeline;
    lines_pipeline.setPolygonMode(sga::PolygonMode::LineStrip);
    lines_pipeline.setLineWidth(2.0);
    lines_pipeline.setTarget(result);
    lines_pipeline.setProgram(lines_program);
    lines_pipeline.setBlendModeColor(sga::BlendFactor::One, sga::BlendFactor::OneMinusSrcAlpha);
    lines_pipeline.setBlendModeAlpha(sga::BlendFactor::One, sga::BlendFactor::OneMinusSrcAlpha);

    // VBO for lines
    std::vector<VertData> lines_vertices = {
      {{0,0}},{{0,1}},{{1,1}},{{1,0}},{{0,0}}
    };
    sga::VBO lines_vbo({sga::DataType::Float2}, lines_vertices.size());
    lines_vbo.write(lines_vertices);

    lines_pipeline.uniform["offset"] = Eigen::Vector2f{offset(0),offset(1)};
    for(unsigned int i = 0; i < N; i++){
      lines_pipeline.sampler["image"] = images[i];
      lines_pipeline.uniform["H"] = His[i];
      lines_pipeline.draw(lines_vbo);
    }
  }

  // Save result
  std::cout << "Saving result to " << output_path << std::endl;
  result.savePNG(output_path);

  renderdoc_capture_end();

  sga::terminate();
}


#include "pypearray.h"

#include <Eigen/Geometry>
#include <pybind11/operators.h>

using namespace PR;
namespace PRPY {

PR_NO_SANITIZE_ADDRESS
void setup_math(py::module& m)
{
	typedef Eigen::Quaternion<float, 0> Quat;

	py::class_<Quat>(m, "Quaternionf")
		.def_static("Identity", &Quat::Identity)

		.def(py::init<>())
		.def(py::init<float, float, float, float>())
		.def_static("UnitRandom", &Quat::UnitRandom)

		.def(py::self * py::self)
		.def(py::self *= py::self)

		.def("angularDistance", &Quat::angularDistance<Quat>)
		.def("conjugate", &Quat::conjugate)
		.def("dot", &Quat::dot<Quat>)
		.def("inverse", &Quat::inverse)
		.def("norm", &Quat::norm)
		.def("squaredNorm", &Quat::squaredNorm)
		.def("normalize", &Quat::normalize)
		.def("normalized", &Quat::normalized)
		.def("toRotationMatrix", &Quat::toRotationMatrix)
		.def("slerp", &Quat::slerp<Quat>)

		.def_property("w", [](const Quat& o) { return o.w(); }, [](Quat& o, float v) { o.w() = v; })
		.def_property("x", [](const Quat& o) { return o.x(); }, [](Quat& o, float v) { o.x() = v; })
		.def_property("y", [](const Quat& o) { return o.y(); }, [](Quat& o, float v) { o.y() = v; })
		.def_property("z", [](const Quat& o) { return o.z(); }, [](Quat& o, float v) { o.z() = v; })

		.def("__getitem__", [](const Quat& o, size_t i) {
			switch (i) {
			case 0:
				return o.w();
			case 1:
				return o.x();
			case 2:
				return o.y();
			case 3:
				return o.z();
			default:
				throw std::runtime_error("Quaternion access out of bound!");
				return 0.0f;
			}
		})
		.def("__setitem__", [](Quat& o, size_t i, float v) {
			switch (i) {
			case 0:
				o.w() = v;
				break;
			case 1:
				o.x() = v;
				break;
			case 2:
				o.y() = v;
				break;
			case 3:
				o.z() = v;
				break;
			default:
				throw std::runtime_error("Quaternion access out of bound!");
			}
		});
	//.def_static("FromTwoVectors", &Eigen::Quaternionf::FromTwoVectors<Eigen::Matrix<float,3,1>, Eigen::Matrix<float,3,1>>)
}
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

//[register_multi_linestring
//` Show the use of the macro BOOST_GEOMETRY_REGISTER_MULTI_LINESTRING

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/multi/geometries/register/multi_linestring.hpp>

typedef boost::geometry::model::linestring
    <
        boost::tuple<float, float>
    > linestring_type;

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_MULTI_LINESTRING(std::deque<linestring_type>)

int main()
{
    // Normal usage of std::
    std::deque<linestring_type> lines(2);
    boost::geometry::read_wkt("LINESTRING(0 0,1 1)", lines[0]);
    boost::geometry::read_wkt("LINESTRING(2 2,3 3)", lines[1]);

    // Usage of Boost.Geometry
    std::cout << "LENGTH: "  << boost::geometry::length(lines) << std::endl;

    return 0;
}

//]

//[register_multi_linestring_output
/*`
Output:
[pre
LENGTH: 2.82843
]
*/
//]


#ifndef STAN_MATH_PRIM_SCAL_PROB_BERNOULLI_LOGIT_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_BERNOULLI_LOGIT_RNG_HPP

#include <boost/random/bernoulli_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_bounded.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/inv_logit.hpp>
#include <stan/math/prim/scal/fun/log1m.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>

namespace stan {
  namespace math {

    /**
     * A Bernoulli random number generator which takes as its argument the
     * often more convenient logit-parametrization.
     *
     * @tparam RNG Random number generator type.
     * @param t logit-transformed probability parameter.
     * @param rng pseudorandom number generator.
     * @return Bernoulli(logit^{-1}(t)) generated random number, either 0 or 1.
     */
    template <class RNG>
    inline int
    bernoulli_logit_rng(double t, RNG& rng) {
      using boost::variate_generator;
      using boost::bernoulli_distribution;
      using stan::math::inv_logit;

      check_finite("bernoulli_logit_rng",
                   "Logit transformed probability parameter", t);

      variate_generator<RNG&, bernoulli_distribution<> >
        bernoulli_rng(rng, bernoulli_distribution<>(inv_logit(t)));
      return bernoulli_rng();
    }

  }
}
#endif


#pragma once
#include <Eigen/Eigen>

class XXZ
{
private:
    uint32_t n_;
    double J_;
    double Delta_;

public:
    XXZ(uint32_t n, double J, double Delta) : n_(n), J_(J), Delta_(Delta) { }

    template<class State> typename State::T operator()(const State& smp) const
    {
        typename State::T s = 0.0;
        // Nearest-neighbor
        for(uint32_t i = 0; i < n_; i++)
        {
            int zz = smp.sigmaAt(i) * smp.sigmaAt((i + 1) % n_);
            s += J_ * Delta_ * zz; // zz
            s += J_ * (1 - zz) * smp.ratio(i, (i + 1) % n_); // xx+yy
        }
        return s;
    }

    std::map<uint32_t, double> operator()(uint32_t col) const;
};


#include <Eigen/Core>
#include <catch2/catch.hpp>

#include <finitediff.hpp>

#include <barrier/barrier.hpp>
#include <utils/eigen_ext.hpp>

using namespace ipc;
using namespace ipc::rigid;

TEST_CASE("Test barriers and their derivatives", "[opt][barrier]")
{
    double s = GENERATE(range(-5, 2));
    s = pow(10, s);

    BarrierType barrier_type =
        GENERATE(BarrierType::IPC, BarrierType::POLY_LOG, BarrierType::SPLINE);

    double x = GENERATE_COPY(take(10, random(s / 2, 0.9 * s))); // ∈ [0, s]
    Vector1d x_vec;
    x_vec << x;

    Eigen::VectorXd fgrad(1);
    fd::finite_gradient(
        x_vec,
        [&](const Eigen::VectorXd& x) {
            return barrier(x[0], s, barrier_type);
        },
        fgrad);

    Eigen::VectorXd grad(1);
    grad << barrier_gradient(x, s, barrier_type);

    CAPTURE(barrier_type, s, x, fgrad(0), grad(0));
    CHECK(fd::compare_gradient(fgrad, grad));

    fd::finite_gradient(
        x_vec,
        [&](const Eigen::VectorXd& x) {
            return barrier_gradient(x[0], s, barrier_type);
        },
        fgrad);

    grad << barrier_hessian(x, s, barrier_type);

    CAPTURE(barrier_type, s, x, fgrad(0), grad(0));
    CHECK(fd::compare_gradient(fgrad, grad));
}


#pragma once

#include <boost/random/random_number_generator.hpp>
#include <boost/random/uniform_int_distribution.hpp>

namespace calotypes
{

template<class Engine>
void FisherYatesShuffle( unsigned int numItems, std::vector<unsigned int>& inds, Engine& engine )
{
	inds.resize( numItems );
	for( unsigned int i = 0; i < numItems; i++ )
	{
		inds[i] = i;
	}

	unsigned int temp;
	for( unsigned int i = numItems-1; i > 0; i-- )
	{
		boost::random::uniform_int_distribution<> dist( 0, i ); // i inclusive
		unsigned int j = (unsigned int) dist( engine );
		temp = inds[j];
		inds[j] = inds[i];
		inds[i] = temp;
	}
}

} // end namespace calotypes


#include "backend.h"
#include "sofunction.h"
#include <boost/multiprecision/cpp_dec_float.hpp>
#include "solvers/regulafalsi.h"
#include "solvers/secant.h"
#include "solvers/bisection.h"
#include <iomanip>
#include <fenv.h>

Backend::Backend() {

}

Backend::~Backend() {
    delete function;
}

std::string Backend::intervalToString(interval x, int decimals) {
    std::stringstream str;
    str << std::scientific;
    str << std::setprecision(decimals);

    if (singleton(x)) {
        str << "[" << median(x) << "]";
    } else {
        int old_rounding = fegetround();

        fesetround(FE_DOWNWARD);
        str << "[" << x.lower() << ", ";
        fesetround(FE_UPWARD);
        str << x.upper() << "]";

        fesetround(old_rounding);
    }

    return str.str();
}

long double Backend::stringToFloat(const std::string &value) {
    if (value.find("bla") != std::string::npos) {
        throw "Nie tym razem, panie profesorze ;)";
    }

    try {
        return std::stold(value);
    } catch (std::invalid_argument &error) {
        throw "Nie udało się zinterpretować wpisanych danych jako liczbę!";
    }
}

interval Backend::stringToInterval(const std::string &value, char separator) {
    size_t split_pos = value.find(separator);
    std::string left_str, right_str;
    boost::multiprecision::cpp_dec_float_50 left_mp, right_mp;
    long double left, right;

    if (split_pos == std::string::npos) {
        left_str = right_str = value;
    } else {
        // przedział
        left_str = value.substr(0, split_pos);
        right_str = value.substr(split_pos + 1);
    }

    try {
        left_mp.assign(left_str);
        right_mp.assign(right_str);
    } catch (std::runtime_error &error) {
        throw "Nie udało się zinterpretować wpisanych danych jako liczbę!";
    }

    int old_rounding = fegetround();
    fesetround(FE_DOWNWARD);
    left = left_mp.convert_to<long double>();
    fesetround(FE_UPWARD);
    right = right_mp.convert_to<long double>();
    fesetround(old_rounding);

    return interval(left, right);
}

struct SingleFloatSummary Backend::floatSummary(long double solution, std::string more) {
    struct SingleFloatSummary out;

    std::stringstream str;
    str << std::scientific;
    str << std::setprecision(decimals);

    str << solution;
    out.x = str.str();
    str.str(std::string());

    long double y = function->evaluate(solution);
    str << y;
    out.y = str.str();
    str.str(std::string());

    out.more = more;

    return out;
}

struct SingleIntervalSummary Backend::intervalSummary(interval solution, std::string more) {
    struct SingleIntervalSummary out;

    std::stringstream str;
    str << std::scientific;
    str << std::setprecision(decimals);

    out.x = intervalToString(solution, decimals);

    str << median(solution);
    out.median = str.str();
    str.str(std::string());

    int old_rounding = fegetround();
    fesetround(FE_UPWARD);
    str << upper(solution) - lower(solution);
    out.width = str.str();
    str.str(std::string());

    fesetround(old_rounding);

    interval y = function->evaluate(solution);
    out.y = intervalToString(y, decimals);

    out.more = more;

    return out;
}

void Backend::loadFunction(char filename[]) {
    Function *new_function;

    new_function = new SOFunction(filename);
    if (function != nullptr) {
        delete function;
    }

    function = new_function;
}

struct FloatSummary Backend::solveFloatingPoint(const std::string &a_str, const std::string &b_str) {
    long double a, b, x;
    a = stringToFloat(a_str);
    b = stringToFloat(b_str);

   struct FloatSummary out;
   bool secant_only = false;
   try {
       check_interval(a, b, function, true);
   } catch (int err) {
       if (err == NO_REAL_ROOTS) {
           secant_only = true;
       }
   }

    try {
        x = Secant(a, b, function);
        out.secant = floatSummary(x);

        if (!secant_only) {
            x = RegulaFalsi(a, b, function);
            out.regulafalsi = floatSummary(x);

            bool reached;
            x = Bisection(a, b, function, bisectionTolerance, bisectionIterations, reached);
            out.bisection = floatSummary(x, std::string("reached = ")+(reached?"true":"false"));
        }
    } catch(int err) {
        if (err == WRONG_INTERVAL) {
            throw "Lewy koniec przedziału musi być mniejszy od prawego końca!";
        } else if (err == NO_REAL_ROOTS) {
            throw "Brak rozwiązań rzeczywistych w tym przedziale. Upewnij się, że f(a) * f(b) < 0";
        }
    }

    return out;
}

struct IntervalSummary Backend::solveInterval(const std::string &a_str, const std::string &b_str) {
    interval a, b, x;

    try {
        a = stringToInterval(a_str);
        b = stringToInterval(b_str);
    } catch (std::runtime_error err) {
        throw "Nie można zbudować takiego przedziału!";
    }

    struct IntervalSummary out;
    bool secant_only = false;
    try {
        check_interval(a, b, function, true);
    } catch (int err) {
        if (err == NO_REAL_ROOTS) {
            secant_only = true;
        }
    }

    try {
        x = Secant(a, b, function);
        out.secant = intervalSummary(x);

        if (!secant_only) {
            bool reached;
            x = Bisection(a, b, function, bisectionTolerance, bisectionIterations, reached);
            out.bisection = intervalSummary(x, std::string("reached = ")+(reached?"true":"false"));

            x = RegulaFalsi(a, b, function);
            out.regulafalsi = intervalSummary(x);
        }
    } catch(int err) {
        if (err == WRONG_INTERVAL) {
            throw "Lewy koniec przedziału musi być mniejszy od prawego końca!";
        } else if (err == NO_REAL_ROOTS) {
            throw "Możliwy brak rozwiązań rzeczywistych w tym przedziale. Upewnij się, że f(a) * f(b) < 0";
        }
    }

    return out;
}


#include <gtest/gtest.h>

#include <Eigen/Core>
#include <algorithm>
#include <csim/init_ops.hpp>
#include <csim/memory_ops.hpp>
#include <csim/stat_ops.hpp>
#include <csim/update_ops.hpp>
#include <csim/update_ops_cpp.hpp>
#include <string>

#include "../util/util.hpp"

void test_double_dense_matrix_gate(
    std::function<void(UINT, UINT, const CTYPE*, CTYPE*, ITYPE)> func) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;

    std::vector<UINT> index_list;
    for (UINT i = 0; i < n; ++i) index_list.push_back(i);

    Eigen::Matrix<std::complex<double>, 2, 2, Eigen::RowMajor> U, U2;
    Eigen::Matrix<std::complex<double>, 4, 4, Eigen::RowMajor> Umerge;

    UINT targets[2];

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state(state, dim);
    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

    Eigen::MatrixXcd whole_I = Eigen::MatrixXcd::Identity(dim, dim);

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        // two qubit dense matrix gate
        U = get_eigen_matrix_random_single_qubit_unitary();
        U2 = get_eigen_matrix_random_single_qubit_unitary();

        std::random_shuffle(index_list.begin(), index_list.end());

        targets[0] = index_list[0];
        targets[1] = index_list[1];
        Umerge = kronecker_product(U2, U);
        // the below two lines are equivalent to the above two line
        // UINT targets_rev[2] = { targets[1], targets[0] };
        // Umerge = kronecker_product(U, U2);
        test_state =
            get_expanded_eigen_matrix_with_identity(targets[1], U2, n) *
            get_expanded_eigen_matrix_with_identity(targets[0], U, n) *
            test_state;
        func(targets[0], targets[1], (CTYPE*)Umerge.data(), state, dim);
        state_equal(state, test_state, dim, "two-qubit separable dense gate");
    }

    std::vector<std::pair<UINT, UINT>> test_pairs;
    for (UINT i = 0; i < n; ++i) {
        for (UINT j = 0; j < n; ++j) {
            if (i == j) continue;
            test_pairs.push_back(std::make_pair(i, j));
        }
    }
    for (auto pair : test_pairs) {
        // two qubit dense matrix gate
        U = get_eigen_matrix_random_single_qubit_unitary();
        U2 = get_eigen_matrix_random_single_qubit_unitary();

        std::random_shuffle(index_list.begin(), index_list.end());
        targets[0] = pair.first;
        targets[1] = pair.second;
        Umerge = kronecker_product(U2, U);
        // the below two lines are equivalent to the above two line
        // UINT targets_rev[2] = { targets[1], targets[0] };
        // Umerge = kronecker_product(U, U2);
        test_state =
            get_expanded_eigen_matrix_with_identity(targets[1], U2, n) *
            get_expanded_eigen_matrix_with_identity(targets[0], U, n) *
            test_state;
        func(targets[0], targets[1], (CTYPE*)Umerge.data(), state, dim);
        state_equal(state, test_state, dim, "two-qubit separable dense gate");
    }

    release_quantum_state(state);
}

TEST(UpdateTest, TwoQubitDenseMatrixTest) {
    test_double_dense_matrix_gate(double_qubit_dense_matrix_gate_c);
    test_double_dense_matrix_gate(double_qubit_dense_matrix_gate_nosimd);
#ifdef _USE_SIMD
    test_double_dense_matrix_gate(double_qubit_dense_matrix_gate_simd);
#endif
}


#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/segmentation/extract_clusters.h>
#include <Eigen/Core>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
#include <pcl/common/time.h>
#include <pcl/common/angles.h>
#include <pcl/registration/transformation_estimation_svd.h>

using namespace std;
typedef pcl::PointXYZ PointType;
typedef struct myPointType
{
    double x;  //mm world coordinate x
    double y;  //mm world coordinate y
    double z;  //mm world coordinate z
	int num;   //point num
};

// Get N bits of the string from back to front.
char* Substrend(char*str,int n)
{
	char *substr=(char*)malloc(n+1);
	int length=strlen(str);
	if (n>=length)
	{
		strcpy(substr,str);
		return substr;
	}
	int k=0;
	for (int i=length-n;i<length;i++)
	{
		substr[k]=str[i];
		k++;
	}
	substr[k]='\0';
	return substr;
}

int main(int argc, char **argv)
{
	// create point cloud
	pcl::PointCloud<PointType>::Ptr cloud(new pcl::PointCloud<PointType>());

	// load data
	char* fileType;
	if (argc>1)
	{
		fileType = Substrend(argv[1],3);
	}
	if (!strcmp(fileType,"pcd"))
	{
    	// load pcd file
		pcl::io::loadPCDFile(argv[1], *cloud);
	}
	else if(!strcmp(fileType,"txt"))
	{
		// load txt data file
		int number_Txt;
		myPointType txtPoint;
		vector<myPointType> points;
		FILE *fp_txt;
		fp_txt = fopen(argv[1], "r");
		if (fp_txt)
		{
		    while (fscanf(fp_txt, "%lf %lf %lf", &txtPoint.x, &txtPoint.y, &txtPoint.z) != EOF)
		    {
		        points.push_back(txtPoint);
		    }
		}
		else
		    std::cout << "txt数据加载失败！" << endl;
		number_Txt = points.size();

		cloud->width = number_Txt;
		cloud->height = 1;
		cloud->is_dense = false;
		cloud->points.resize(cloud->width * cloud->height);

		for (size_t i = 0; i < cloud->points.size(); ++i)
		{
		    cloud->points[i].x = points[i].x;
		    cloud->points[i].y = points[i].y;
		    cloud->points[i].z = 0;
		}
	}
	else
	{
		std::cout << "please input data file name"<<endl;
		return 0;
	}

	// start calculating time
    pcl::StopWatch time;

    Eigen::Vector4f pcaCentroid;
    pcl::compute3DCentroid(*cloud, pcaCentroid);
    Eigen::Matrix3f covariance;
    pcl::computeCovarianceMatrixNormalized(*cloud, pcaCentroid, covariance);
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
    Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();
    Eigen::Vector3f eigenValuesPCA = eigen_solver.eigenvalues();
    eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1)); //校正主方向间垂直
    eigenVectorsPCA.col(0) = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2));
    eigenVectorsPCA.col(1) = eigenVectorsPCA.col(2).cross(eigenVectorsPCA.col(0));

    std::cout << "特征值va(3x1):\n" << eigenValuesPCA << std::endl;
    std::cout << "特征向量ve(3x3):\n" << eigenVectorsPCA << std::endl;
    std::cout << "质心点(4x1):\n" << pcaCentroid << std::endl;
    /*
    // 另一种计算点云协方差矩阵特征值和特征向量的方式:通过pcl中的pca接口，如下，这种情况得到的特征向量相似特征向量
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPCAprojection (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PCA<pcl::PointXYZ> pca;
    pca.setInputCloud(cloudSegmented);
    pca.project(*cloudSegmented, *cloudPCAprojection);
    std::cerr << std::endl << "EigenVectors: " << pca.getEigenVectors() << std::endl;//计算特征向量
    std::cerr << std::endl << "EigenValues: " << pca.getEigenValues() << std::endl;//计算特征值
    */
    Eigen::Matrix4f tm = Eigen::Matrix4f::Identity();
    Eigen::Matrix4f tm_inv = Eigen::Matrix4f::Identity();
    tm.block<3, 3>(0, 0) = eigenVectorsPCA.transpose();   //R.
    tm.block<3, 1>(0, 3) = -1.0f * (eigenVectorsPCA.transpose()) *(pcaCentroid.head<3>());//  -R*t
    tm_inv = tm.inverse();

    std::cout << "变换矩阵tm(4x4):\n" << tm << std::endl;
    std::cout << "逆变矩阵tm'(4x4):\n" << tm_inv << std::endl;

    pcl::PointCloud<PointType>::Ptr transformedCloud(new pcl::PointCloud<PointType>);
    pcl::transformPointCloud(*cloud, *transformedCloud, tm);

    PointType min_p1, max_p1;
    Eigen::Vector3f c1, c;
    pcl::getMinMax3D(*transformedCloud, min_p1, max_p1);
    c1 = 0.5f*(min_p1.getVector3fMap() + max_p1.getVector3fMap());

    std::cout << "型心c1(3x1):\n" << c1 << std::endl;

    Eigen::Affine3f tm_inv_aff(tm_inv);
    pcl::transformPoint(c1, c, tm_inv_aff);

    Eigen::Vector3f whd, whd1;
    whd1 = max_p1.getVector3fMap() - min_p1.getVector3fMap();
    whd = whd1;
    float sc1 = (whd1(0) + whd1(1) + whd1(2)) / 3;  //点云平均尺度，用于设置主方向箭头大小

    std::cout << "width1=" << whd1(0) << endl;
    std::cout << "heght1=" << whd1(1) << endl;
    std::cout << "depth1=" << whd1(2) << endl;
    std::cout << "scale1=" << sc1 << endl;

    const Eigen::Quaternionf bboxQ1(Eigen::Quaternionf::Identity());
    const Eigen::Vector3f    bboxT1(c1);
    const Eigen::Quaternionf bboxQ(tm_inv.block<3, 3>(0, 0));
    const Eigen::Vector3f    bboxT(c);

    //变换到原点的点云主方向
    PointType op;
    op.x = 0.0;
    op.y = 0.0;
    op.z = 0.0;
    Eigen::Vector3f px, py, pz;
    Eigen::Affine3f tm_aff(tm);
    pcl::transformVector(eigenVectorsPCA.col(0), px, tm_aff);
    pcl::transformVector(eigenVectorsPCA.col(1), py, tm_aff);
    pcl::transformVector(eigenVectorsPCA.col(2), pz, tm_aff);
    PointType pcaX;
    pcaX.x = sc1 * px(0);
    pcaX.y = sc1 * px(1);
    pcaX.z = sc1 * px(2);
    PointType pcaY;
    pcaY.x = sc1 * py(0);
    pcaY.y = sc1 * py(1);
    pcaY.z = sc1 * py(2);
    PointType pcaZ;
    pcaZ.x = sc1 * pz(0);
    pcaZ.y = sc1 * pz(1);
    pcaZ.z = sc1 * pz(2);

    //初始点云的主方向
    PointType cp;
    cp.x = pcaCentroid(0);
    cp.y = pcaCentroid(1);
    cp.z = pcaCentroid(2);
    PointType pcX;
    pcX.x = sc1 * eigenVectorsPCA(0, 0) + cp.x;
    pcX.y = sc1 * eigenVectorsPCA(1, 0) + cp.y;
    pcX.z = sc1 * eigenVectorsPCA(2, 0) + cp.z;
    PointType pcY;
    pcY.x = sc1 * eigenVectorsPCA(0, 1) + cp.x;
    pcY.y = sc1 * eigenVectorsPCA(1, 1) + cp.y;
    pcY.z = sc1 * eigenVectorsPCA(2, 1) + cp.z;
    PointType pcZ;
    pcZ.x = sc1 * eigenVectorsPCA(0, 2) + cp.x;
    pcZ.y = sc1 * eigenVectorsPCA(1, 2) + cp.y;
    pcZ.z = sc1 * eigenVectorsPCA(2, 2) + cp.z;

	//Rectangular vertex
	pcl::PointCloud<PointType>::Ptr transVertexCloud(new pcl::PointCloud<PointType>);//存放变换后点云包围盒的6个顶点
	pcl::PointCloud<PointType>::Ptr VertexCloud(new pcl::PointCloud<PointType>);//存放原来点云中包围盒的6个顶点
	transVertexCloud->width = 6;
	transVertexCloud->height = 1;
	transVertexCloud->is_dense = false;
	transVertexCloud->points.resize(transVertexCloud->width * transVertexCloud->height);
	transVertexCloud->points[0].x = max_p1.x;
	transVertexCloud->points[0].y = max_p1.y;
	transVertexCloud->points[0].z = max_p1.z;
	transVertexCloud->points[1].x = max_p1.x;
	transVertexCloud->points[1].y = max_p1.y;
	transVertexCloud->points[1].z = min_p1.z;
	transVertexCloud->points[2].x = max_p1.x;
	transVertexCloud->points[2].y = min_p1.y;
	transVertexCloud->points[2].z = min_p1.z;
	transVertexCloud->points[3].x = min_p1.x;
	transVertexCloud->points[3].y = max_p1.y;
	transVertexCloud->points[3].z = max_p1.z;
	transVertexCloud->points[4].x = min_p1.x;
	transVertexCloud->points[4].y = min_p1.y;
	transVertexCloud->points[4].z = max_p1.z;
	transVertexCloud->points[5].x = min_p1.x;
	transVertexCloud->points[5].y = min_p1.y;
	transVertexCloud->points[5].z = min_p1.z;
	pcl::transformPointCloud(*transVertexCloud, *VertexCloud, tm_inv);

	// 逆变换回来的角度
	cout << whd1(0) << " "<< whd1(1) << " " << whd1(2) << endl;
	auto euler = bboxQ1.toRotationMatrix().eulerAngles(0, 1, 2);
	std::cout << "Euler from quaternion in roll, pitch, yaw"<< std::endl << euler/3.14*180 << std::endl<<std::endl;

	//Output time consumption
	std::cout << "运行时间" << time.getTime() << "ms" << std::endl;

    //visualization
    pcl::visualization::PCLVisualizer viewer;
    pcl::visualization::PointCloudColorHandlerCustom<PointType> tc_handler(transformedCloud, 0, 255, 0); //设置点云颜色
	//Visual transformed point cloud
    viewer.addPointCloud(transformedCloud, tc_handler, "transformCloud");
    viewer.addCube(bboxT1, bboxQ1, whd1(0), whd1(1), whd1(2), "bbox1");
    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "bbox1");
    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, "bbox1");

    viewer.addArrow(pcaX, op, 1.0, 0.0, 0.0, false, "arrow_X");
    viewer.addArrow(pcaY, op, 0.0, 1.0, 0.0, false, "arrow_Y");
    viewer.addArrow(pcaZ, op, 0.0, 0.0, 1.0, false, "arrow_Z");

    pcl::visualization::PointCloudColorHandlerCustom<PointType> color_handler(cloud, 255, 0, 0);
    viewer.addPointCloud(cloud, color_handler, "cloud");
    viewer.addCube(bboxT, bboxQ, whd(0), whd(1), whd(2), "bbox");
    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "bbox");
    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "bbox");

    viewer.addArrow(pcX, cp, 1.0, 0.0, 0.0, false, "arrow_x");
    viewer.addArrow(pcY, cp, 0.0, 1.0, 0.0, false, "arrow_y");
    viewer.addArrow(pcZ, cp, 0.0, 0.0, 1.0, false, "arrow_z");

    viewer.addCoordinateSystem(0.5f*sc1);
    viewer.setBackgroundColor(0.0, 0.0, 0.0);

	viewer.addPointCloud(VertexCloud, "temp_cloud");
	viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "temp_cloud");
    while (!viewer.wasStopped())
    {
          viewer.spinOnce();
    }

    return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_ELLIPTIC_FUNCTIONS_SCALAR_ELLINT_2_HPP_INCLUDED
#define NT2_TOOLBOX_ELLIPTIC_FUNCTIONS_SCALAR_ELLINT_2_HPP_INCLUDED
#include <nt2/toolbox/elliptic/functions/ellint_2.hpp>
#include <nt2/include/constants/digits.hpp>
#include <nt2/include/constants/real.hpp>
#include <boost/math/special_functions/ellint_2.hpp>
#include <nt2/toolbox/polynomials/functions/scalar/impl/horner.hpp>
#include <nt2/include/functions/scalar/sqrt.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/oneminus.hpp>
#include <nt2/sdk/error/policies.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::ellint_2_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                            )
  {

    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;

    NT2_FUNCTOR_CALL(1)
    {
      return nt2::ellint_2(result_type(a0));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is double
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::ellint_2_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {

    typedef typename meta::strip<A0>::type result_type;

    NT2_FUNCTOR_CALL(1)
    {
      result_type x = nt2::abs(a0);
      if (x>One<A0>())    return Nan<A0>();
      if (x == One<A0>()) return x;
      return boost::math::ellint_2(a0, nt2_policy());
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is float
/////////////////////////////////////////////////////////////////////////////
// NT2_REGISTER_DISPATCH(tag::ellint_2_, tag::cpu_,
//                           (A0),
//                           (single_<A0>)
//                          )

// namespace nt2 { namespace ext
// {
//   template<class Dummy>
//   struct call<tag::ellint_2_(tag::float_),
//               tag::cpu_, Dummy> : callable
//   {
//     template<class Sig> struct result;
//     template<class This,class A0>
//     struct result<This(A0)> :
//       meta::result_of<meta::floating(A0)>{};

//     NT2_FUNCTOR_CALL(1)
//     {
//       A0 x = nt2::abs(a0);
//       if (x>One<A0>()) return Nan<A0>();
//       if (x == One<A0>()) return x;
//       const A0 a = nt2::sqrt(oneminus(x));
//       return horner< NT2_HORNER_COEFF_T(A0, 11,
//                               (0x392102f5,
//                                0x3b246c1b,
//                                0x3c0e578f,
//                                0x3c2fe240,
//                                0x3bfebca9,
//                                0x3bf882cf,
//                                0x3c3d8b3f,
//                                0x3cb2d89a,
//                                0x3d68ac90,
//                                0x3ee2e430,
//                                0x3f800000) ) > (a)
//                 -nt2::log(a)*a*horner< NT2_HORNER_COEFF_T(A0, 10,
//                                     (0x38098de4,
//                                      0x3a84557e,
//                                      0x3bd53114,
//                                      0x3c8a54f6,
//                                      0x3cd67118,
//                                      0x3d0925e1,
//                                      0x3d2ef92b,
//                                      0x3d6fffe9,
//                                      0x3dc00000,
//                                      0x3e800000
//                                      ) ) > (a);
//     }
//   };
// } }

#endif


/**
 * @file   main.cpp
 * @author Simon Pintarelli <simon@thinkpadX1>
 * @date   Wed Oct 21 17:37:10 2015
 *
 * @brief  Example for Polar->Nodal basis transformation and quadrature
 *         in the nodal basis
 *
 *
 */

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <iostream>

#include <Eigen/Dense>
#include <algorithm>

#include "quadrature/qhermite.hpp"

//#include "spectral/hermite_to_nodal.hpp"
#include "aux/eigen2hdf.hpp"
#include "post_processing/mass.hpp"
#include "spectral/basis/spectral_basis_factory_hermite.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"
#include "spectral/polar_to_hermite.hpp"
#include "spectral/polar_to_nodal.hpp"

using namespace std;
using namespace boltzmann;

namespace po = boost::program_options;

// obviously wrong ... But it is not used, see below.
void test1(int K)
{
  typedef Eigen::VectorXd vec_t;

  typedef typename SpectralBasisFactoryKS::basis_type polar_basis_t;
  polar_basis_t polar_basis;
  SpectralBasisFactoryKS::create(polar_basis, K, K, 2, true);
  SpectralBasisFactoryKS::write_basis_descriptor(polar_basis, "spectral_basis.desc");

  Polar2Nodal<polar_basis_t> p2n;
  p2n.init(polar_basis, 0.5);

  Mass mass(polar_basis);

  std::vector<size_t> elems = {0, 1, 2, 3, 4, 5, 6};
  // quadrature
  QHermiteW quad(0.5, K);
  auto& w = quad.wts();
  auto& x = quad.pts();

  // Achtung mit den Knoten und der Skalierung der Gewichte
  QHermiteW quad1(1, K);
  auto& xh = quad1.pts();

  std::string fname = std::string("P2N") + std::to_string(K) + ".h5";
  hid_t file = H5Fcreate(fname.c_str(), H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  for (size_t eidx : elems) {
    Eigen::VectorXd cp(polar_basis.n_dofs());
    cp.setZero();
    cp(eidx) = 1.0;

    const double mass_ref = mass.compute(cp.data());

    Eigen::MatrixXd cn(K, K);
    p2n.to_nodal(cn, cp);
    eigen2hdf::save(file, "cn" + std::to_string(eidx), cn);

    auto id = polar_basis.get_elem(eidx).id();

    double sum = 0;
    for (size_t q1 = 0; q1 < w.size(); ++q1) {
      for (size_t q2 = 0; q2 < w.size(); ++q2) {
        // (*) QUAD EXAMPLE
        sum += cn(q1, q2) * std::sqrt(w[q1] * w[q2]) *
               std::exp(-xh[q1] * xh[q1] / 2 - xh[q2] * xh[q2] / 2);
      }
    }
    cout << id.to_string() << "Sum: " << setprecision(5) << scientific << sum << endl;
    cout << id.to_string() << "Ref: " << setprecision(5) << scientific << mass_ref << "\n\n";
  }

  typedef Eigen::VectorXd vec_t;
  Eigen::Map<const vec_t> xq(quad.points_data(), K);
  Eigen::Map<const vec_t> wq(quad.weights_data(), K);

  eigen2hdf::save(file, "xq", xq);
  eigen2hdf::save(file, "wq", wq);

  auto& N2H = p2n.get_h2n()->get_n2h();
  auto& H2N = p2n.get_h2n()->get_h2n();

  eigen2hdf::save(file, "H2N", H2N);
  eigen2hdf::save(file, "N2H", N2H);

  H5Fclose(file);
}

// Polar-Laguerre coefficients with exponential decay
void test2(int K, const std::function<double(double)>& cfct)
{
  typedef Eigen::VectorXd vec_t;

  typedef typename SpectralBasisFactoryKS::basis_type polar_basis_t;
  polar_basis_t polar_basis;
  SpectralBasisFactoryKS::create(polar_basis, K, K, 2, true);
  SpectralBasisFactoryKS::write_basis_descriptor(polar_basis, "spectral_basis.desc");

  Polar2Nodal<polar_basis_t> p2n;
  p2n.init(polar_basis, 1.0);

  unsigned int N = polar_basis.n_dofs();
  Eigen::VectorXd cp(N);
  for (unsigned int i = 0; i < N; ++i) {
    cp(i) = cfct(float(i) / N);
  }
  Eigen::MatrixXd cn(K, K);
  p2n.to_nodal(cn, cp);
  Eigen::VectorXd cp2(N);
  p2n.to_polar(cp2, cn);

  auto diff = cp2;
  diff.setZero();
  for (unsigned int i = 0; i < N; ++i) {
    diff(i) = std::abs(cp2(i) - cp(i));
  }
  cout << " sum(err): " << diff.sum() << endl;
}

int main(int argc, char* argv[])
{
  if (argc < 2) {
    cout << "usage: " << argv[0] << " K\n";
    exit(1);
  }
  const int K = atoi(argv[1]);

  cout << "Test: P->N->P "
       << "\n";
  cout << "Exponential decaying Polar-Laguerre coefficients: cp[i] = exp(-20*i/N)"
       << "\n";
  test2(K, [](double i) { return std::exp(-20 * i); });

  cout << "Constant coefficients: cp[i] = 1.0"
       << "\n";
  test2(K, [](double i) { return 1.0; });
  return 0;
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Tetrahedral_remeshing/Remeshing_triangulation_3.h>
#include <CGAL/tetrahedral_remeshing.h>

#include <CGAL/property_map.h>

#include <boost/unordered_set.hpp>

#include <iostream>
#include <utility>

#include "tetrahedral_remeshing_generate_input.h"

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;

typedef CGAL::Tetrahedral_remeshing::Remeshing_triangulation_3<K> Remeshing_triangulation;

typedef Remeshing_triangulation::Point         Point;
typedef Remeshing_triangulation::Vertex_handle Vertex_handle;
typedef Remeshing_triangulation::Cell_handle   Cell_handle;
typedef Remeshing_triangulation::Edge          Edge;

class Constrained_edges_property_map
{
public:
  typedef bool                               value_type;
  typedef bool                               reference;
  typedef std::pair<Vertex_handle, Vertex_handle> key_type;
  typedef boost::read_write_property_map_tag category;

private:
  boost::unordered_set<key_type>* m_set_ptr;

public:
  Constrained_edges_property_map()
    : m_set_ptr(NULL)
  {}
  Constrained_edges_property_map(boost::unordered_set<key_type>* set_)
    : m_set_ptr(set_)
  {}

public:
  friend void put(Constrained_edges_property_map& map,
                  const key_type& k,
                  const bool b)
  {
    CGAL_assertion(map.m_set_ptr != NULL);
    CGAL_assertion(k.first < k.second);
    if (b)  map.m_set_ptr->insert(k);
    else    map.m_set_ptr->erase(k);
  }

  friend value_type get(const Constrained_edges_property_map& map,
                              const key_type& k)
  {
    CGAL_assertion(map.m_set_ptr != NULL);
    CGAL_assertion(k.first < k.second);
    return (map.m_set_ptr->count(k) > 0);
  }
};

void set_subdomain(Remeshing_triangulation& tr, const int index)
{
  for (Remeshing_triangulation::Finite_cells_iterator cit = tr.finite_cells_begin();
       cit != tr.finite_cells_end(); ++cit)
  {
    cit->set_subdomain_index(index);
  }
}

int main(int argc, char* argv[])
{
  const double target_edge_length = (argc > 1) ? atof(argv[1]) : 0.02;
  const int nb_iter = (argc > 2) ? atoi(argv[2]) : 1;
  const int nbv = (argc > 3) ? atoi(argv[3]) : 500;

  Remeshing_triangulation t3;
  boost::unordered_set<std::pair<Vertex_handle, Vertex_handle> > constraints;

  CGAL::Tetrahedral_remeshing::generate_input_cube(nbv, t3, constraints);
  make_constraints_from_cube_edges(t3, constraints);
  CGAL_assertion(t3.is_valid());

  CGAL::tetrahedral_isotropic_remeshing(t3, target_edge_length,
    CGAL::parameters::edge_is_constrained_map(
      Constrained_edges_property_map(&constraints))
    .number_of_iterations(nb_iter));

  return EXIT_SUCCESS;
}


// #include <iostream>
// #include <string>
// #include <boost/lexical_cast.hpp>
// #include <fstream>
// #include <iomanip>

// #include "quadrature/qmaxwell.hpp"
// #include "quadrature/qmidpoint.hpp"
// #include "quadrature/tensor_product_quadrature.hpp"
// #include "quadrature/quadrature_handler.hpp"

// #include "spectral/basis/spectral_basis_factory_ks.hpp"
// #include "matrix/assembly/velocity_radial_integrator.hpp"
// #include "matrix/assembly/velocity_var_form.hpp"
// #include "matrix/assembly/weight.hpp"

// #include "spectral/laguerren.hpp"
// #include "spectral/laguerrenw.hpp"

// using namespace std;
// using namespace boltzmann;

// // typedef boltzmann::QuadratureHandler<
// //   boltzmann::TensorProductQuadratureC<boltzmann::QMidpoint, QMaxwell> > quad_type;

// typedef double numeric_t;

// template<typename MAP>
// void print(const MAP& m, std::ofstream& fout, string title)
// {
//   fout << "----- " << title << endl;
//   for (auto it = m.begin(); it != m.end(); ++it) {
//     fout << it->first.first << "\t"
//          << it->first.second
//          << "\t"
//          << setprecision(16) << it->second
//          << endl;
//   }
// }

// template<typename CONT>
// void print_basis(const CONT& cont, std::ofstream& fout)
// {
//   for (int i = 0; i < cont.size(); ++i) {
//     fout  << cont[i].get_id() << endl;
//   }
// }

// // ----------------------------------------------------------------------
// int main(int argc, char *argv[])
// {
//   const double beta = 2;

//   if ( argc < 3) {
//     cerr << "info: " << argv[0] << " K q"
//          << endl
//          << "q: No. quad. points\n";
//     return 1;
//   }
//   int K = atoi(argv[1]);
//   int N = atoi(argv[2]);
//   const int digits = 256;

//   QMaxwell qmaxwell(1, N, digits);

//   std::ofstream fout("quadrule_order" + boost::lexical_cast<string>(N) + "_" +
//   boost::lexical_cast<string>(digits) + ".dat");
//   for (unsigned int i = 0; i < qmaxwell.size(); ++i) {
//     fout << setprecision(30) << qmaxwell.pts(i)
//          << "\t"
//          << setprecision(30) << qmaxwell.wts(i)
//          << endl;
//   }
//   fout.close();

//   typedef boltzmann::SpectralBasisFactoryKS basis_factory_t;
//   typedef typename basis_factory_t::basis_type basis_type;

//   L2Weight weight(beta);

//   basis_type basis;
//   basis_factory_t::create(basis, K, K, beta);

//   typedef typename std::tuple_element<1, typename basis_type::elem_t::container_t>::type rad_t;
//   typedef typename basis_type::DimAcc::template get_vec<rad_t> accessor_t;

//   const auto& radial_basis = accessor_t()(basis);

//   LaguerreN<numeric_t> L(K);
//   std::vector<numeric_t> r2(N);
//   std::transform(qmaxwell.pts().begin(), qmaxwell.pts().end(), r2.begin(), [](double r) {return
//   r*r;} );
//   L.compute(r2);

//   ofstream foutn("errors-normalized.dat");
//   for (auto elem = radial_basis.begin(); elem != radial_basis.end(); ++elem) {
//     unsigned int n = elem->get_degree();
//     unsigned int alpha = elem->get_order();
//     unsigned int k = elem->get_id().k;
//     unsigned int j = elem->get_id().j;
//     const numeric_t* values = L.get(n, alpha);
//     double I = 0;
//     for (unsigned int q = 0; q < qmaxwell.size(); ++q) {
//       const double r2j = std::pow(qmaxwell.pts(q), 4*j+ 2* (k%2) );
//       I += r2j*values[q] * values[q] * qmaxwell.wts(q);
//     }
//     foutn << elem->get_id() << "\t" << setw(30) << setprecision(20) << scientific <<
//     std::abs(I-0.5) << endl;
//   }
//   foutn.close();

//   ofstream foutnw("errors-normalized-weighted.dat");
//   LaguerreNW<numeric_t> LW(K);
//   LW.compute(r2);
//   for (auto elem = radial_basis.begin(); elem != radial_basis.end(); ++elem) {
//     unsigned int n = elem->get_degree();
//     unsigned int alpha = elem->get_order();
//     unsigned int k = elem->get_id().k;
//     unsigned int j = elem->get_id().j;
//     const numeric_t* values = LW.get(n, alpha);
//     double I = 0;
//     for (unsigned int q = 0; q < qmaxwell.size(); ++q) {
//       const double r = qmaxwell.pts(q);
//       const double r2j = std::pow(r, 4*j+ 2* (k%2) );

//       I += r2j*(values[q] * ::math::exp(r*r*0.5))* (values[q] * ::math::exp(r*r*0.5)) *
//       qmaxwell.wts(q);
//     }
//     foutnw << elem->get_id() << "\t" << setw(30) << setprecision(20) << scientific <<
//     std::abs(I-0.5) << endl;
//   }
//   foutnw.close();

//   return 0;
// }


#pragma once

#ifdef NOOB_PLATFORM_LINUX
#undef Success
#endif

#include <Eigen/Geometry>
#include "vec3.hpp"

namespace noob
{
	template <typename T>
	class plane
	{
		public:
			void through(noob::vec3_type<T>& a, const noob::vec3_type<T>& b, const noob::vec3_type<T>& c)
			{
				inner = Eigen::Hyperplane<float, 3>::Through(Eigen::Vector3f(b.v[0], b.v[1], b.v[2]), Eigen::Vector3f(c.v[0], c.v[1], c.v[2]));
			}

			void normalize()
			{
				inner.normalize();
			}

			float signed_distance(const noob::vec3_type<T>& p) const
			{
				return inner.signedDistance(Eigen::Vector3f(p.v[0], p.v[1], p.v[2]));
			}

			noob::vec3_type<T> normal() const
			{
				return vec3_from_eigen_vec3(inner.normal());
			}

			float offset() const
			{
				return inner.offset();
			}

			noob::vec3_type<T> projection(const noob::vec3_type<T>& p) const
			{
				return vec3_from_eigen_vec3(inner.projection(Eigen::Vector3f(p.v[0], p.v[1], p.v[2])));
			}

		protected:
			Eigen::Hyperplane<T, 3> inner;
	};
}


#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/multi/geometries/multi_point.hpp>

#include <iostream>
#include <vector>

#include "mesh.h"
#include "component.h"
#include "vertex.h"

void Mesh::createHull(int c_num)
{

	using boost::geometry::append;
	using boost::geometry::make;
	using boost::geometry::model::d2::point_xy;

	boost::geometry::model::multi_point<point_xy<dReal> > pointset;

	for (int i = 0; i < components.at(c_num)->faces.size(); i++)
	{
		for (std::vector<int>::iterator vertIter = components.at(c_num)->faces.at(i)->vertices.begin(); vertIter != components.at(c_num)->faces.at(i)->vertices.end(); vertIter++)
		{
			append(pointset, make<point_xy<dReal> >(vertexPalette.at(*vertIter)->pos.x, vertexPalette.at(*vertIter)->pos.y));
		}
	}

	boost::geometry::model::multi_point<point_xy<dReal> > hull;
	boost::geometry::convex_hull(pointset, hull);

	for (std::vector<point_xy<dReal> >::size_type i = 0; i < hull.size(); i++)
	{
		components.at(c_num)->hull.push_back(std::make_pair(boost::geometry::get<0>(hull[i]), boost::geometry::get<1>(hull[i])));
	}

}


#include <iostream>
#include <string>
#include <unordered_map>
#include <vector>
#include <memory>
#include <Eigen/Dense>
#include "../include/sample_network.h"
#include "../datasets/include/mnist.h"

using namespace Eigen;

int main()
{
    using std::cout;
    using std::endl;
    using std::vector;
    using std::string;
    using std::unordered_map;
    using namespace MyDL;

    int num_iters = 1000;
    double learning_rate = 0.1;

    int batch_size = 100;
    int input_size = 28 * 28;
    int hidden_size = 50;
    int output_size = 10;

    MatrixXd train_X = MatrixXd::Zero(batch_size, input_size);
    MatrixXd test_X = MatrixXd::Zero(batch_size, input_size);
    MatrixXd train_y = MatrixXd::Zero(batch_size, 10);
    MatrixXd test_y = MatrixXd::Zero(batch_size, 10);

    MnistEigenDataset mnist(batch_size);

    TwoLayerNet net(input_size, hidden_size, output_size);

    vector<MatrixXd> inputs, loss, val_inputs;
    unordered_map<string, MatrixXd> grads;
    double accuracy;

    mnist.next_train(train_X, train_y, true);
    inputs.push_back(train_X);
    inputs.push_back(train_y);

    grads = net.gradient(inputs);

    // cout << "--- parameter b1 ---" << endl;
    // cout << *(net.params["b1"]) << endl;
    // cout << "--- parameter b1 update ---" << endl;
    // *(net.params["b1"]) -= -learning_rate * grads["b1"];
    // MatrixXd dParam = grads["b1"];
    // *(net.params["b1"]) -= -learning_rate * dParam;
    // cout << *(net.params["b1"]) << endl;
    // cout << dParam << endl;
    // *(net.params["b1"]) -= MatrixXd::Ones(1, hidden_size); // こちらは更新される->gradsによる更新がおかしい？
    // cout << *(net.params["b1"]) << endl;

    cout << "--- parameter b1 ---" << endl;
    cout << net.params["b1"] << endl;
    cout << "--- parameter b1 update ---" << endl;
    net.params["b1"] -= -learning_rate * grads["b1"];

    MatrixXd dParam = grads["b1"];
    net.params["b1"] -= -learning_rate * dParam;
    cout << net.params["b1"] << endl;
    cout << dParam << endl;
    net.params["b1"] -= MatrixXd::Ones(1, hidden_size); // こちらは更新される->gradsによる更新がおかしい？
    cout << net.params["b1"] << endl;

    return 0;
}