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

#ifndef BOOST_MATH_BESSEL_YN_HPP
#define BOOST_MATH_BESSEL_YN_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/detail/bessel_y0.hpp>
#include <boost/math/special_functions/detail/bessel_y1.hpp>
#include <boost/math/special_functions/detail/bessel_jy_series.hpp>
#include <boost/math/policies/error_handling.hpp>

// Bessel function of the second kind of integer order
// Y_n(z) is the dominant solution, forward recurrence always OK (though unstable)

namespace boost { namespace math { namespace detail{

template <typename T, typename Policy>
T bessel_yn(int n, T x, const Policy& pol)
{
    BOOST_MATH_STD_USING
    T value, factor, current, prev;

    using namespace boost::math::tools;

    static const char* function = "boost::math::bessel_yn<%1%>(%1%,%1%)";

    if ((x == 0) && (n == 0))
    {
       return -policies::raise_overflow_error<T>(function, 0, pol);
    }
    if (x <= 0)
    {
       return policies::raise_domain_error<T>(function,
            "Got x = %1%, but x must be > 0, complex result not supported.", x, pol);
    }

    //
    // Reflection comes first:
    //
    if (n < 0)
    {
        factor = (n & 0x1) ? -1 : 1;  // Y_{-n}(z) = (-1)^n Y_n(z)
        n = -n;
    }
    else
    {
        factor = 1;
    }

    if(x < policies::get_epsilon<T, Policy>())
    {
       T scale = 1;
       value = bessel_yn_small_z(n, x, &scale, pol);
       if(tools::max_value<T>() * fabs(scale) < fabs(value))
          return boost::math::sign(scale) * boost::math::sign(value) * policies::raise_overflow_error<T>(function, 0, pol);
       value /= scale;
    }
    else if (n == 0)
    {
        value = bessel_y0(x, pol);
    }
    else if (n == 1)
    {
        value = factor * bessel_y1(x, pol);
    }
    else
    {
       prev = bessel_y0(x, pol);
       current = bessel_y1(x, pol);
       int k = 1;
       BOOST_ASSERT(k < n);
       do
       {
           T fact = 2 * k / x;
           if((tools::max_value<T>() - fabs(prev)) / fact < fabs(current))
           {
              prev /= current;
              factor /= current;
              current = 1;
           }
           value = fact * current - prev;
           prev = current;
           current = value;
           ++k;
       }
       while(k < n);
       if(fabs(tools::max_value<T>() * factor) < fabs(value))
          return sign(value) * sign(value) * policies::raise_overflow_error<T>(function, 0, pol);
       value /= factor;
    }
    return value;
}

}}} // namespaces

#endif // BOOST_MATH_BESSEL_YN_HPP


#include <iostream>
#include <vector>
#include <map>
#include <string>
#include <sstream>

#pragma warning( push )
# pragma warning (disable:4800)
#include <Eigen/Dense>
#include <Eigen/StdVector> //NodeArray, EdgeArray
#pragma warning( pop )

namespace cmg {
	typedef double Precision;
	typedef Eigen::Matrix<Precision, Eigen::Dynamic, Eigen::Dynamic> MatT;
	typedef Eigen::Matrix<Precision, Eigen::Dynamic, 1> VecT;
	typedef Eigen::Matrix<Precision, 2, 2> Mat2T;
	typedef Eigen::Matrix<Precision, 2, 1> Vec2T;
	typedef Eigen::Matrix<Precision, 3, 3> Mat3T;
	typedef Eigen::Matrix<Precision, 3, 1> Vec3T;
	typedef Eigen::Matrix<Precision, 4, 4> Mat4T;
	typedef Eigen::Matrix<Precision, 4, 1> Vec4T;
	typedef Eigen::Matrix<Precision, 6, 6> Mat6T;
	typedef Eigen::Matrix<Precision, 6, 1> Vec6T;
	typedef Eigen::Matrix<Precision, 2, 4> Mat2x4T;
	typedef Eigen::Matrix<Precision, 4, 2> Mat4x2T;
	typedef Eigen::Matrix<Precision, 3, 4> Mat3x4T;
	typedef Eigen::Matrix<Precision, 4, 3> Mat4x3T;

	inline static Precision eps() {
		return 10*std::numeric_limits<Precision>::epsilon();
	}

	typedef std::vector<int> Veci;
	inline static void range(const int start, const int end, const int step, Veci& ret)
	{
		ret.clear();
		ret.reserve((end-start+1)/step);
		for(int v=start; v<=end; v+=step) ret.push_back(v);
		ret.resize(ret.size());
	}

	inline static Precision perimeter(const Mat2x4T& u)
	{
		Precision ret=0;
		for(int i=0; i<4; ++i) {
			ret += (u.col(i) - u.col((i+1)%4)).norm();
		}
		return ret;
	}

	struct Calibration {
		Vec4T k;		//[fx, fy, cx, cy]
		Vec2T d;		//[k1, k2]
		Mat4T Ck;		//Cov[k]
		Mat2T Cd;		//Cov[d]
	public:
		EIGEN_MAKE_ALIGNED_OPERATOR_NEW

		inline Mat3T K() const
		{
			Mat3T ret;
			ret.setIdentity();
			ret(0,0)=k(0);
			ret(1,1)=k(1);
			ret(0,2)=k(2);
			ret(1,2)=k(3);
			return ret;
		}

		inline void print() const
		{
			std::cout<<"k="<<k.transpose()<<std::endl;
			std::cout<<"d="<<d.transpose()<<std::endl;
			//std::cout<<"Ck=\n"<<Ck<<std::endl; //TODO: Ck, Cd not used yet
			//std::cout<<"Cd=\n"<<Cd<<std::endl;
		}

		template<typename T>
		void project(const T X[3], T U[2]) const
		{
			T fx(k(0)), fy(k(1)), cx(k(2)), cy(k(3));
			T k1(d(0)), k2(d(1));

			T xn( X[0]/X[2] ), yn( X[1]/X[2] );
			T r2 = xn*xn + yn*yn;
			T factor=T(1)+(k2*r2+k1)*r2;
			T xnp=xn*factor;
			T ynp=yn*factor;
			U[0] = fx*xnp+cx;
			U[1] = fy*ynp+cy;
		}

		Vec2T project(const Vec3T& X) const
		{
			Vec2T ret;
			project(X.data(), ret.data());
			return ret;
		}
	};

	//6D pose
	struct Pose {
		Vec6T p;		//[ra*rx,ra*ry,ra*rz,tx,ty,tz]
		Mat6T Cp;		//Cov[p]
	public:
		EIGEN_MAKE_ALIGNED_OPERATOR_NEW

		static int nParams() { return 6; }

		inline Vec3T transform(const Vec3T& X) const
		{
			return (toT() * X.homogeneous()).hnormalized();
		}

		//return rotation matrix R
		inline Mat3T R() const
		{
			Precision ang = p.head<3>().norm();
			Vec3T axis(1,0,0);
			if(ang>eps())
				axis = p.head<3>().normalized();
			else
				ang = 0;
			Eigen::AngleAxis<Precision> aa(ang, axis);
			return aa.toRotationMatrix();
		}

		//return translation vector t
		inline Vec3T t() const
		{
			return p.tail<3>();
		}

		//return T=[R,t;0,1]
		inline Mat4T toT() const
		{
			Mat4T ret;
			ret.setIdentity();
			ret.topLeftCorner<3,3>() = R();
			ret.topRightCorner<3,1>() = t();
			return ret;
		}

		inline Mat4T invT() const
		{
			Mat3T rot = R().transpose();
			Mat4T T;
			T.setIdentity();
			T.topLeftCorner<3,3>() = rot;
			T.topRightCorner<3,1>() = -rot * t();
			return T;
		}

		inline Vec6T invp() const
		{
			return T2p(invT());
		}

		inline void fromR(const Mat3T& R)
		{
			Eigen::AngleAxis<Precision> aa;
			aa.fromRotationMatrix(R);
			p.head<3>() = aa.axis() * aa.angle();
		}

		inline void fromt(const Vec3T& t)
		{
			p.tail<3>() = t;
		}

		inline void fromT(const Mat4T& T)
		{
			fromR(T.topLeftCorner<3,3>());
			fromt(T.topRightCorner<3,1>());
		}

		inline static Vec6T T2p(const Mat4T& T)
		{
			return Pose(T).p;
		}

		inline static Mat4T p2T(const Vec6T& p)
		{
			return Pose(p).toT();
		}

		Pose() { p.setZero(); Cp.setZero(); }
		Pose(const Vec6T& p_) : p(p_) {
			Cp.setZero();
		}
		Pose(const Mat4T& T) {
			fromT(T);
			Cp.setZero();
		}
		Pose(const Mat4T& T, const Mat6T& Covp) : Cp(Covp)
		{
			fromT(T);
		}
	};
	typedef std::vector< Vec6T, Eigen::aligned_allocator<Vec6T> > Vec6TArray;

	struct Node : public Pose {
		EIGEN_MAKE_ALIGNED_OPERATOR_NEW

		std::string name;	//name of the marker
		Veci vmeids;		//vmEdges' ids linked to this marker (not used now)
	};
	typedef std::vector< Node, Eigen::aligned_allocator<Node> > NodeArray;
	typedef int NID;
	const NID INVALID_NID = -1;

	struct Edge {
		EIGEN_MAKE_ALIGNED_OPERATOR_NEW

		int vid;	//id of view
		int mid;	//id of marker
		Mat2x4T u;	//observations, 2x4, TODO: allow different observations
	};
	typedef std::vector< Edge, Eigen::aligned_allocator<Edge> > EdgeArray;
	typedef int EID;
	const EID INVALID_EID = -1;

	struct Observation {
		EIGEN_MAKE_ALIGNED_OPERATOR_NEW

		std::string name;		//marker's name
		Pose init_marker_pose;	//marker pose in view, i.e., Tmc
		Mat2x4T u;				//observed marker corners in view

		bool operator<(const Observation& other) const {
			return perimeter(u) > perimeter(other.u);
		}
	};
	typedef std::vector< Observation,  Eigen::aligned_allocator<Observation> > ObsArray;
	typedef std::vector<ObsArray> VecObsArray;

	typedef std::map<std::string, int> Str2Int;

	class CMGraph {
	public: //member variables
		NodeArray markers;
		NodeArray views;
		EdgeArray edges;

		Pose fixed_marker_pose;
		NID fixed_marker_id;

		Calibration calib;
		Str2Int name2mid; //name -> makrer id

		Precision marker_half_size;
		bool verbose;

		struct Callback {
			virtual void operator()(const CMGraph& G) = 0;
		};
		typedef Callback* CallbackPtr;
		CallbackPtr cb_addObsFromNewView;
		CallbackPtr cb_optimizeNewViewPose;
		CallbackPtr cb_optimizeOneViewPose;
		CallbackPtr cb_optimizePose;

	public: //member functions
		CMGraph() : fixed_marker_id(INVALID_NID), marker_half_size(1), verbose(true),
			cb_addObsFromNewView(0), cb_optimizeNewViewPose(0), cb_optimizeOneViewPose(0), cb_optimizePose(0)
		{}

		//print input information
		inline void print() const
		{
			std::cout<<"-------------------"<<std::endl;
			std::cout<<"calib:"<<std::endl;
			calib.print();
			std::cout<<std::endl;

			std::cout<<"marker_half_size="<<marker_half_size<<std::endl;
			std::cout<<"verbose="<<verbose<<std::endl;
			std::cout<<"-------------------"<<std::endl;
		}

		//print CMGraph state
		void report(std::ofstream& out) const;

		inline int nParams() const
		{
			return Pose::nParams()*static_cast<int>(markers.size()+views.size());
		}

		inline int nResiduals() const
		{
			return nObsResiduals() + nCstResiduals();
		}

		inline int nObsResiduals() const
		{
			return 8*static_cast<int>(edges.size()); //TODO: allow each marker to have more than 4 point observations
		}

		inline int nCstResiduals() const
		{
			return Pose::nParams();
		}

		inline bool empty() const
		{
			return nParams()<=0;
		}

		NID setFixedMarker(const std::string &fixed_marker_name,
			const Precision p_ang=1e-4,
			const Precision p_pos=1e-2);

		//add all observed markers in this view to the graph, and also add view and edges
		//oa will be sorted by descending order of perimeters of the observed markers in the image
		NID addObsFromNewView(
			ObsArray& oa,
			const std::string &view_name,
			const bool addNewMarker=true);

		bool optimizeNewViewPose(
			ObsArray& oa,
			Node& newView,
			const Precision sigma_u,
			const int max_iter,
			const Precision huber_loss_bandwidth=10, // +/- 10 pixels
			const Precision error_rel_tol=1e-2,
			const bool computeCovariance=false);

		bool optimizeOneViewPose(
			const NID vid,
			const Precision sigma_u,
			const int max_iter,
			const Precision huber_loss_bandwidth=10, // +/- 10 pixels
			const Precision error_rel_tol=1e-2,
			const bool computeCovariance=false);

		//bundle adjustment to optimize all markers' and views' poses
		bool optimizePose(
			const Precision sigma_u,
			const int max_iter,
			const Precision huber_loss_bandwidth=10, // +/- 10 pixels
			const Precision error_rel_tol=1e-2,
			const bool computeCovariance=false);

	public: //static functions
		//return 2D coordinates of a marker's 4 corners
		inline static Mat2x4T marker_x(const Precision half_size=1)
		{
			Precision ret_[]={
				-1,-1,
				 1,-1,
				 1, 1,
				-1, 1
			};

			Mat2x4T ret(ret_);
			return ret * half_size;
		}

		//return 3D coordinates of a marker's 4 corners
		inline static Mat3x4T marker_X(const Precision half_size=1)
		{
			Precision ret_[]={
				-1,-1, 0,
				 1,-1, 0,
				 1, 1, 0,
				-1, 1, 0
			};

			Mat3x4T ret(ret_);
			return ret * half_size;
		}

		static void BatchProcess(
			const VecObsArray &frames, //each frame's Observations' order would be sorted
			const Calibration &calib,
			CMGraph& G,
			const std::string &fixed_marker_name="",
			const Precision p_ang=1e-4,
			const Precision p_pos=1e-2,
			const Precision sigma_u=0.2,
			const int max_iter_per_opt=20,
			const bool do_covariance_estimation=true);

	protected:
		NID newMarker(const Vec6T& p, const Mat6T& Cp, const std::string& name);

		NID newView(const Vec6T& p, const Mat6T& Cp, const std::string& name);

		EID newEdge(const NID vid, const NID mid, const MatT& u);
	};
}


#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include <Eigen/LU>

// this file implements the class defined in:
#include "calibration/Calibration.h"

using namespace Eigen;
using namespace calibration;
// constructor
Calibration::Calibration()
{
	RzOK_ = Matrix3f::Identity();
	RxOK_ = Matrix3f::Identity();
	TzOK_ = Matrix4f::Identity();
	TxOK_ = Matrix4f::Identity();
}

// destructor
Calibration::~Calibration()
{
	// empty destructor
}

// set functions
void Calibration::setInput(const std::vector<Matrix4f> Ta, const std::vector<Matrix4f> Tb)
{

	// set poses number from transformations size
	// check the transformations have the same poses number

	int na = Ta.size();
	int nb = Tb.size();

	if ( (na != nb) )
	{
		std::cerr << "rotation matrix dimension not compatible!! " << std::endl;
		return;
	}

	// n_poses_ is the actual number of poses recorded by camera
	n_poses_ = (na%2 == 0)? na : na - 1;

	// std::cout << "Calibrating for " << n_poses_ << " poses..." << std::endl;

	//resizeAllParametres();
	//std::cout << "all matrices resized " << std::endl;

	// push rotations and translations in respective vectors

	Ra_.clear();
	Rb_.clear();
	ta_.clear();
	tb_.clear();

	for (int i = 0 ; i < n_poses_  ;i++)
	{
		Ra_.push_back( Ta[i].block<3,3>(0,0) );

		// std::cout << "Ra_ pose  " << i << std::endl << Ra_[i] << std::endl;

		Rb_.push_back( Tb[i].block<3,3>(0,0) );

		// std::cout << "Rb_ pose  " << i << std::endl << Rb_[i] << std::endl;

		ta_.push_back( Ta[i].block<3,1>(0,3) );

		// std::cout << "ta_ pose  " << i << std::endl << ta_[i] << std::endl;

		tb_.push_back( Tb[i].block<3,1>(0,3) );

		// std::cout << "tb_ pose  " << i << std::endl << tb_[i] << std::endl;

	}

	//std::cout << "rotation part extracted " << std::endl;

}

// resize all parametres

int Calibration::resizeAllParametres()
{
	QqA_.resize(n_poses_);
	WqB_.resize(n_poses_);
	Ci_.resize(n_poses_);
	Ra_.resize(n_poses_);
	Rb_.resize(n_poses_);
	Rx_.resize(n_poses_);
	Rz_.resize(n_poses_);
	Sq_a.resize(n_poses_);
	Sq_b.resize(n_poses_);
	qa_.resize(n_poses_);
	qb_.resize(n_poses_);
	//qx_.resize(n_elem_);
	//qz_.resize(n_elem_);
	ta_.resize(n_poses_);
	tb_.resize(n_poses_);
	tx_.resize(n_poses_/2);
	tz_.resize(n_poses_/2);
	return 0;
}

// dual quaternions evaluation
/*
	by the skewsymmetrics we can obtain the dual quaternions Q and W as
	combination of the skewsymmetric and the relative quaternion so to have
	qAi * qX = Q(qAi) qX, where "*" is vect prod, and also
	qBi * qZ = W(qBi) qZ, that can substitute qA * qX = qZ *qB as
	Q(qAi) qX - W(qBi) qZ = 0

	Q(qAi) = [  qA0         -qA'
				qA      qA0*I + S(qA)]

	W(qBi) = [  qB0         -qB'
				qB      qB0*I - S(qB)]

*/
int Calibration::computeDualQuaternion()
{
	// compute skewsymmetric

	Matrix3f Sqa, Sqb;
	Matrix4f QqA, WqB;
	Quaternionf use;

	for (int i = 0 ; i < n_poses_  ;i++)
	{
		Sqa << 0, -qa_[i].z(), qa_[i].y(),
			   qa_[i].z(), 0, -qa_[i].x(),
			   -qa_[i].y(), qa_[i].x(), 0;

		Sq_a.push_back(Sqa);

		//std::cout << "from quaternion qa_" << std::endl << Quaternionf(qa_[i]) << std::endl << "the skewsymmetric Sq_a is " << std::endl << Matrix4f(Sq_a[i]) << std::endl;

		Sqb <<  0, -qb_[i].z(), qb_[i].y(),
				qb_[i].z(), 0, -qb_[i].x(),
			  	-qb_[i].y(), qb_[i].x(), 0;

		//std::cout << "from quaternion qb_" << std::endl << Quaternionf(qb_[i]) << std::endl << "the skewsymmetric Sq_b is " << std::endl << Matrix4f(Sq_b[i]) << std::endl;

		QqA.block<3,3>(1,1) = Sqa + (qa_[i].w() * Matrix3f::Identity ());
		use = qa_[i].conjugate();
		QqA.block<1,4>(0,0) = Vector4f( use.w(), use.x(), use.y(), use.z() ).transpose();
		use = qa_[i];
		QqA.block<4,1>(0,0) = Vector4f( use.w(), use.x(), use.y(), use.z() );

		// std::cout  << "QqA" << QqA << std::endl;

		QqA_.push_back(QqA);

		WqB.block<3,3>(1,1) = -Sqb + (qb_[i].w() * Matrix3f::Identity ());
		use = qb_[i].conjugate();
		WqB.block<1,4>(0,0) = Vector4f( use.w(), use.x(), use.y(), use.z() ).transpose();
		use = qb_[i];
		WqB.block<4,1>(0,0) = Vector4f( use.w(), use.x(), use.y(), use.z() );

		// std::cout  << "WqB" << WqB << std::endl;

		WqB_.push_back( WqB );
	}

	return 0;
}

int Calibration::computeClosedForm()
{
	transformRotations2Quaternions();
	computeDualQuaternion();
	computeC();
	computeFinalQuaternions();
	transformQuaterions2Rotations();
	computeTranslations();
	setOutput();
	computeError();
	std::cout << "Compute closed form done!" << std::endl;
	return 0;
}

int Calibration::computeNonLinOpt()
{
	computeClosedForm();
	computeOneShot();
	setOutput();
	computeError();
	std::cout << "Compute non linear optimization done!" << std::endl;
	return 0;
}

// C evaluation

/*

C matrix is the compute by the sum of the several Ci (orthogonal matrix of rank equal to 4)
obtained as Ci = -Q (qAi)'W(qBi)

C importance is due to its bond with qz and qx, because of qz is an eigenvector of the symmetric
semipositive definite matrix C'C and by that we can obtain qx = C qz/(lambda-n_poses)

*/

int Calibration::computeC()
{
	C_ = Matrix4f::Zero();
	for ( int i=0; i < n_poses_; i++ )
		C_ += -QqA_[i].transpose()* WqB_[i];
	return 0;
}

// final quaternions qxOK and qzOK evaluation
// alpha and lambda evaluation by eigenvalues

/*

alpha is a 4 float vector made of the C'C eigenvalues
lambda is n_poses,alpha combination (lambda = n_poses +/- sqrt (alpha))
so to have the minimum positive of lambda choosing the best alpha of the list

k_ is the best alpha index so to remember it also when compute the relative eigenvector

*/

int Calibration::computeFinalQuaternions()
{
	EigenSolver<Matrix4f> solver (C_.transpose()*C_ );
	alpha_ = solver.eigenvalues().real();
	k_= 0;
	float Lt;

	lambda_ = ((n_poses_ - sqrt(alpha_(k_)))>0) ? (n_poses_ - sqrt(alpha_(k_))) : (n_poses_ + sqrt(alpha_(k_)));

	for (int i=1; i < 4;i++)
	{
		Lt = n_poses_ - sqrt(alpha_[i]);

		if (Lt < lambda_ && Lt >0)
		{
			lambda_ = Lt;
			k_ = i;
		}
		else
		{
			Lt = n_poses_ + sqrt(alpha_[i]);
			if (Lt < lambda_)
			{
				lambda_ = Lt;
				k_ = i;
			}
		}
	}

	Vector4f qzOK, qxOK;

	qzOK = solver.eigenvectors().col(k_).real();
	qxOK = C_*qzOK/(lambda_ - n_poses_);

	qxOK.normalize();

	qzOK_ = Quaternionf( qzOK[0], qzOK[1], qzOK[2], qzOK[3] );
	qxOK_ = Quaternionf( qxOK[0], qxOK[1], qxOK[2], qxOK[3] );

	return 0;
}

// transform all rotations needed to quaternions

int Calibration::transformRotations2Quaternions()
{
	for (int i = 0; i < n_poses_; i++)
	{
		qa_.push_back( Quaternionf (Ra_[i]) );
		qb_.push_back( Quaternionf (Rb_[i]) );
	}
	return 0;
}

// transform all quaternions needed to rotations (qxOK and qzOK)

int Calibration::transformQuaterions2Rotations()
{
	RxOK_ = qxOK_.toRotationMatrix();
	RzOK_ = qzOK_.toRotationMatrix();
}

int Calibration::computeTranslations()
{
	if (n_poses_ == 1)
	{
		std::cerr << "translations not computable by only one rotation!! " << std::endl;
		return 0;
	}

	txOK_ = Vector3f::Zero ();
	tzOK_ = Vector3f::Zero ();

	Eigen::MatrixXf A;
	A.resize(3*n_poses_,6);
	Eigen::VectorXf c;
	c.resize(3*n_poses_);
	Eigen::VectorXf x;
	x.resize(6);

	for (int i = 0; i < n_poses_ ; i++ )
	{
		A.block<3,3>(3*i,0) = Ra_[i];
		A.block<3,3>(3*i,3) = -1*Eigen::Matrix3f::Identity();

		c.block<3,1>(3*i,0) = RzOK_*tb_[i] - ta_[i];
	}

	MatrixXf Ainv;
	Ainv.resize(6,6);
	Ainv = A.transpose()*A;

	x = Ainv.inverse()*A.transpose()*c;

	txOK_ = x.block<3,1>(0,0);
	tzOK_ = x.block<3,1>(3,0);

	/*for (int i = 0; i < n_poses_; i += 2)
	{
		txOK_ += (Matrix3f ( Ra_[i] - Ra_[i+1]).inverse() )*( RzOK_*(tb_[i] - tb_[i+1]) + ta_[i+1] - ta_[i] );
	}

	txOK_ *= 2/n_poses_;

	for (int i = 0; i < n_poses_; i += 2)
	{
		tzOK_ += Ra_[i]*txOK_ + ta_[i] - RzOK_*tb_[i];
	}

	tzOK_ *= 2/n_poses_;*/

	return 0;
}

int Calibration::computeError()
{
	Er_ = 0;

	float EtNum = 0, EtDen = 1;

	for (int i = 0; i < n_poses_; i++)
	{
		Er_ += Matrix3f( Ra_[i]*RxOK_ - RzOK_*Rb_[i] ).squaredNorm();

		EtDen += Vector3f( Ra_[i]*txOK_ - ta_[i] ).squaredNorm();

		EtNum += Vector3f( Ra_[i]*txOK_ + ta_[i] - RzOK_*tb_[i] - tzOK_ ).squaredNorm();

	}

	//Et_ = sqrt(EtNum/EtDen);
	Et_ = EtNum/n_poses_;
	Er_ = Er_/n_poses_;

	std::cout << "Rotation error is: " << Er_ << std::endl;
	std::cout << "Translation error is: " << Et_ << std::endl;

	return 0;
}

int Calibration::convertFromParamVect2ValuesOk(const VectorXf &in, Matrix3f &RzOK, Matrix3f &RxOK, Vector3f &tzOK, Vector3f &txOK)
{
	for (int i = 0; i < 3; i++)
	{
		RxOK.col(i) = in.block<3,1>(3*i,0);
		RzOK.col(i) = in.block<3,1>((3*i)+9,0);
	}

	txOK = in.block<3,1>(18,0);
	tzOK = in.block<3,1>(21,0);

return 0;

}

int Calibration::convertFromValuesOk2ParamVect(const Matrix3f &RzOK, const Matrix3f &RxOK, const Vector3f &tzOK, const Vector3f &txOK, VectorXf &out)

{
	for (int i = 0; i < 3; i++)
	{
		out.block<3,1>(3*i,0) = RxOK.col(i);
		out.block<3,1>((3*i)+9,0) = RzOK.col(i);
	}

	out.block<3,1>(18,0) = txOK;
	out.block<3,1>(21,0) = tzOK;

	//std::cout << "out: " << out << std::endl;

return 0;

}

void Calibration::setOutput()
{
	TzOK_.block<3,3>(0,0) = RzOK_;
	TzOK_.block<3,1>(0,3) = tzOK_;

	TxOK_.block<3,3>(0,0) = RxOK_;
	TxOK_.block<3,1>(0,3) = txOK_;

	return;
}

void Calibration::getOutput(Matrix4f &TzOK, Matrix4f &TxOK)
{
	TzOK = TzOK_;
	TxOK = TxOK_;
	return;
}

void Calibration::getFullOutput(Matrix4f &TzOK, Matrix4f &TxOK, float &Er, float &Et)
{
	getOutput(TzOK, TxOK);
	Er = Er_;
	Et = Et_;
	return;
}

int Calibration::computeOneShot()
{
	FF_.n_poses__ = n_poses_;
	FF_.Ra__ = Ra_;
	FF_.Rb__ = Rb_;
	FF_.ta__ = ta_;
	FF_.tb__ = tb_;

	Eigen::NumericalDiff<my_functor> numDiff(FF_);
	Eigen::LevenbergMarquardt<Eigen::NumericalDiff<my_functor>,float> lm_solver(numDiff);

	Eigen::VectorXf x(24);
	Eigen::VectorXf x_ini(24);

	// from initial guess to x
	convertFromValuesOk2ParamVect(RzOK_, RxOK_, tzOK_, txOK_, x);

	//x.setRandom();
	x_ini = x;
	// std::cout << "initial guess x : " << std::endl << x << std::endl;

	// init solver parameters
	lm_solver.parameters.maxfev = 2000;
	//lm_solver.parameters.xtol = 1.0e-10;
	//lm_solver.parameters.ftol = 1.0e-10;

	Eigen::LevenbergMarquardtSpace::Status status = lm_solver.minimize(x);

	std::cout << "Iterations in optimization before ending: " << lm_solver.iter << std::endl;
	std::cout << "lm_solver.nfev " << lm_solver.nfev << std::endl;
	std::cout << "lm_solver.njev " << lm_solver.njev << std::endl;
	std::cout << "lm_solver.fnorm " << lm_solver.fnorm << std::endl;
	std::cout << "status code: " << status << std::endl;
	// std::cout << "x that minimizes the function: " << std::endl << x << std::endl;

	std::cout << "|x_ini - x_opt|: " << std::endl << x_ini-x << std::endl;

	// from x to solution
	convertFromParamVect2ValuesOk(x, RzOK_, RxOK_, tzOK_, txOK_);

	std::cout << "RxOK_.determinant() : " << RxOK_.determinant() << std::endl;
	std::cout << "RzOK_.determinant() : " << RzOK_.determinant() << std::endl;

	return 0;
}


#ifndef STACK_HH
#define STACK_HH

// Stack classes
//
// These are the low-level classes that implement feed-forward and
// recurrent neural networks. All the Eigen-dependant code in this
// library should live in this file.
//
// To keep the Eigen code out of the high-level interface, the STL ->
// Eigen ``preprocessor'' classes are also defined here.
//
// The ordering of classes is as follows:
//  - Feed-forward Stack class
//  - Feed-forward Layer classes
//  - RecurrentStack class
//  - Recurrent layers
//  - Activation functions
//  - Various utility functions

#include "Exceptions.hh"
#include "NNLayerConfig.hh"

#include <Eigen/Dense>

#include <vector>
#include <functional>

namespace lwt {

  using Eigen::VectorXd;
  using Eigen::MatrixXd;

  class ILayer;
  class IRecurrentLayer;

  // ______________________________________________________________________
  // Feed forward Stack class

  class Stack
  {
  public:
    // constructor for dummy net
    Stack();
    // constructor for real net
    Stack(size_t n_inputs, const std::vector<LayerConfig>& layers,
          size_t skip_layers = 0);
    ~Stack();

    // make non-copyable for now
    Stack(Stack&) = delete;
    Stack& operator=(Stack&) = delete;

    VectorXd compute(VectorXd) const;
    size_t n_outputs() const;

  private:
    // returns the size of the next layer
    size_t add_layers(size_t n_inputs, const LayerConfig&);
    size_t add_dense_layers(size_t n_inputs, const LayerConfig&);
    size_t add_normalization_layers(size_t n_inputs, const LayerConfig&);
    size_t add_highway_layers(size_t n_inputs, const LayerConfig&);
    size_t add_maxout_layers(size_t n_inputs, const LayerConfig&);
    std::vector<ILayer*> m_layers;
    size_t m_n_outputs;
  };

  // _______________________________________________________________________
  // Feed-forward layers

  class ILayer
  {
  public:
    virtual ~ILayer() {}
    virtual VectorXd compute(const VectorXd&) const = 0;
  };

  class DummyLayer: public ILayer
  {
  public:
    virtual VectorXd compute(const VectorXd&) const;
  };

  class UnaryActivationLayer: public ILayer
  {
  public:
    UnaryActivationLayer(Activation);
    virtual VectorXd compute(const VectorXd&) const;
  private:
    std::function<double(double)> m_func;
  };

  class SoftmaxLayer: public ILayer
  {
  public:
    virtual VectorXd compute(const VectorXd&) const;
  };

  class BiasLayer: public ILayer
  {
  public:
    BiasLayer(const VectorXd& bias);
    BiasLayer(const std::vector<double>& bias);
    virtual VectorXd compute(const VectorXd&) const;
  private:
    VectorXd m_bias;
  };

  class MatrixLayer: public ILayer
  {
  public:
    MatrixLayer(const MatrixXd& matrix);
    virtual VectorXd compute(const VectorXd&) const;
  private:
    MatrixXd m_matrix;
  };

  class MaxoutLayer: public ILayer
  {
  public:
    typedef std::pair<MatrixXd, VectorXd> InitUnit;
    MaxoutLayer(const std::vector<InitUnit>& maxout_tensor);
    virtual VectorXd compute(const VectorXd&) const;
  private:
    std::vector<MatrixXd> m_matrices;
    MatrixXd m_bias;
  };

  /// Normalization layer ///
  /// https://arxiv.org/abs/1502.03167 ///
  class NormalizationLayer : public ILayer
  {

  public:
    NormalizationLayer(const VectorXd& W,const VectorXd& b);
    virtual VectorXd compute(const VectorXd&) const;

  private:
    VectorXd _W;
    VectorXd _b;

  };

  //http://arxiv.org/pdf/1505.00387v2.pdf
  class HighwayLayer: public ILayer
  {
  public:
    HighwayLayer(const MatrixXd& W,
                 const VectorXd& b,
                 const MatrixXd& W_carry,
                 const VectorXd& b_carry,
                 Activation activation);
    virtual VectorXd compute(const VectorXd&) const;
  private:
    MatrixXd m_w_t;
    VectorXd m_b_t;
    MatrixXd m_w_c;
    VectorXd m_b_c;
    std::function<double(double)> m_act;
  };

  // ______________________________________________________________________
  // Recurrent Stack

  class RecurrentStack
  {
  public:
    RecurrentStack(size_t n_inputs, const std::vector<LayerConfig>& layers);
    ~RecurrentStack();
    RecurrentStack(RecurrentStack&) = delete;
    RecurrentStack& operator=(RecurrentStack&) = delete;
    VectorXd reduce(MatrixXd inputs) const;
    size_t n_outputs() const;
  private:
    std::vector<IRecurrentLayer*> m_layers;
    size_t add_lstm_layers(size_t n_inputs, const LayerConfig&);
    size_t add_gru_layers(size_t n_inputs, const LayerConfig&);
    size_t add_embedding_layers(size_t n_inputs, const LayerConfig&);
    Stack* m_stack;
  };

  // __________________________________________________________________
  // Recurrent layers

  class IRecurrentLayer
  {
  public:
    virtual ~IRecurrentLayer() {}
    virtual MatrixXd scan( const MatrixXd&) const = 0;
  };

  class EmbeddingLayer : public IRecurrentLayer
  {
  public:
    EmbeddingLayer(int var_row_index, MatrixXd W);
    virtual ~EmbeddingLayer() {};
    virtual MatrixXd scan( const MatrixXd&) const;

  private:
    int m_var_row_index;
    MatrixXd m_W;
  };

  /// long short term memory ///
  struct LSTMState;
  class LSTMLayer : public IRecurrentLayer
  {
  public:
    LSTMLayer(Activation activation, Activation inner_activation,
              MatrixXd W_i, MatrixXd U_i, VectorXd b_i,
              MatrixXd W_f, MatrixXd U_f, VectorXd b_f,
              MatrixXd W_o, MatrixXd U_o, VectorXd b_o,
              MatrixXd W_c, MatrixXd U_c, VectorXd b_c);

    virtual ~LSTMLayer() {};
    virtual MatrixXd scan( const MatrixXd&) const;
    void step( const VectorXd& input, LSTMState& ) const;

  private:
    std::function<double(double)> m_activation_fun;
    std::function<double(double)> m_inner_activation_fun;

    MatrixXd m_W_i;
    MatrixXd m_U_i;
    VectorXd m_b_i;

    MatrixXd m_W_f;
    MatrixXd m_U_f;
    VectorXd m_b_f;

    MatrixXd m_W_o;
    MatrixXd m_U_o;
    VectorXd m_b_o;

    MatrixXd m_W_c;
    MatrixXd m_U_c;
    VectorXd m_b_c;

    int m_n_outputs;
  };

  /// gated recurrent unit ///
  struct GRUState;
  class GRULayer : public IRecurrentLayer
  {
  public:
    GRULayer(Activation activation, Activation inner_activation,
             MatrixXd W_z, MatrixXd U_z, VectorXd b_z,
             MatrixXd W_r, MatrixXd U_r, VectorXd b_r,
             MatrixXd W_h, MatrixXd U_h, VectorXd b_h);

    virtual ~GRULayer() {};
    virtual MatrixXd scan( const MatrixXd&) const;
    void step( const VectorXd& input, GRUState& ) const;

  private:
    std::function<double(double)> m_activation_fun;
    std::function<double(double)> m_inner_activation_fun;

    MatrixXd m_W_z;
    MatrixXd m_U_z;
    VectorXd m_b_z;

    MatrixXd m_W_r;
    MatrixXd m_U_r;
    VectorXd m_b_r;

    MatrixXd m_W_h;
    MatrixXd m_U_h;
    VectorXd m_b_h;

    int m_n_outputs;
  };

  // ______________________________________________________________________
  // Activation functions

  // note that others are supported but are too simple to
  // require a special function
  double nn_sigmoid( double x );
  double nn_hard_sigmoid( double x );
  double nn_tanh( double x );
  double nn_relu( double x );
  std::function<double(double)> get_activation(lwt::Activation);

  // WARNING: you own this pointer! Only call when assigning to member data!
  ILayer* get_raw_activation_layer(Activation);

  // ______________________________________________________________________
  // utility functions

  // functions to build up basic units from vectors
  MatrixXd build_matrix(const std::vector<double>& weights, size_t n_inputs);
  VectorXd build_vector(const std::vector<double>& bias);

  // consistency checks
  void throw_if_not_maxout(const LayerConfig& layer);
  void throw_if_not_dense(const LayerConfig& layer);
  void throw_if_not_normalization(const LayerConfig& layer);

  // LSTM component for convenience in some layers
  struct DenseComponents
  {
    Eigen::MatrixXd W;
    Eigen::MatrixXd U;
    Eigen::VectorXd b;
  };
  DenseComponents get_component(const lwt::LayerConfig& layer, size_t n_in);

}

#endif // STACK_HH


//
//  Copyright Toon Knapen, Karl Meerbergen
//
// 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 "../../blas/test/random.hpp"

#include <boost/numeric/bindings/lapack/steqr.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <algorithm>
#include <limits>
#include <iostream>

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;

template <typename T>
int do_value_type() {
   const int n = 10 ;

   typedef typename boost::numeric::bindings::traits::type_traits<T>::real_type real_type ;
   typedef std::complex< real_type >                                            complex_type ;

   typedef ublas::matrix<T, ublas::column_major> matrix_type ;
   typedef ublas::vector<T>                      vector_type ;
   real_type safety_factor (1.5);

   // Set matrix
   matrix_type z( n, n );
   vector_type d( n ), e ( n - 1 ) ;

   std::fill( d.begin(), d.end(), 2.0 ) ;
   std::fill( e.begin(), e.end(), -1.0 ) ;

   // Compute eigendecomposition.
   lapack::steqr( 'I', d, e, z ) ;

   for ( int i=0; i<d.size(); ++i) {
     T sum( 0.0 ) ;
     for (int j=0; j<d.size(); ++j) {
       sum += z(i,j)*z(i,j) * d(j) ;
     }
     if (std::abs( sum - 2.0 ) > safety_factor*10 * std::numeric_limits<T>::epsilon() ) return 1 ;

     if (i>0) {
       sum = 0.0 ;
       for (int j=0; j<d.size(); ++j) {
         sum += z(i-1,j)*z(i,j) * d(j) ;
       }
       if (std::abs( sum + 1.0 ) > safety_factor*10 * std::numeric_limits<T>::epsilon() ) return 1 ;
     }
   }

   return 0 ;
} // do_value_type()

int main() {
   // Run tests for different value_types
   std::cout << "float\n" ;
   if (do_value_type<float>()) return 255;
   std::cout << "double\n" ;
   if (do_value_type<double>()) return 255;

   std::cout << "Regression test succeeded\n" ;
   return 0;
}


#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

int main() {
  MatrixXf m(2, 2);
  MatrixXf n(2, 2);
  MatrixXf result(2, 2);

  m << 1, 2,
      3, 4;
  n << 5, 6,
      7, 8;

  result = (m.array() + 4).matrix() * m;
  cout << "-- Combination 1: --" << endl << result << endl << endl;
  result = (m.array() * n.array()).matrix() * m;
  cout << "-- Combination 2: --" << endl << result << endl << endl;
}


#ifndef WZP_EIGEN_MATRIX_HPP_
#define WZP_EIGEN_MATRIX_HPP_

/**
 * This is just a wapper for eigen matrix(DYNAMIC)
 */
#include <cassert>

#include <Eigen/Dense>

namespace wzp
{

template<typename Dtype>
class EMatrix {
private:
    // the alias for inner use of raw eigen matrix
    template<typename F>
    using RawMatrix = Eigen::Matrix<F, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

    // define the index type
    typedef Eigen::Index Index;

public:
    /**
     * Constructors
     */
    // default contructor
    EMatrix() : m_mat() {}

    // construct by size
    EMatrix(Index m) : m_mat(m, m) { InitByValue(0); }

    // construct by two size
    EMatrix(Index m, Index n, Dtype init_val = 0) : m_mat(m, n) {InitByValue(init_val); }

    // construct by raw point
    EMatrix(Index m, Index n, Dtype* raw_data_ptr) {
        m_mat = std::move(Eigen::Map<RawMatrix<Dtype>>(raw_data_ptr, m, n));
    }

    // construct by datas
    EMatrix(RawMatrix<Dtype>&& raw_mat) : m_mat(std::move(raw_mat)) {}
    EMatrix(const RawMatrix<Dtype>& raw_mat) : m_mat(raw_mat) {}

    // move construct
    EMatrix(EMatrix<Dtype>&& other) : m_mat(std::move(other.m_mat)) { }

    // move operator
    EMatrix<Dtype>& operator= (EMatrix<Dtype>&& other) {
        assert(this != &other);
        m_mat = std::move(other.m_mat);
        return *this;
    }

    // copy constructor
    EMatrix(const EMatrix<Dtype>&) = default;
    EMatrix<Dtype>& operator= (const EMatrix<Dtype>&) = default;

    // get the raw matrix
    inline RawMatrix<Dtype>& get_raw_mat() { return m_mat; }
    inline const RawMatrix<Dtype>& get_raw_mat() const { return m_mat; }

    /**
     * reshape functions
     */
    inline void reshape(Index m, Index n) {
        if(m == m_mat.rows() && n == m_mat.cols()) return;
        m_mat.resize(m, n);
    }

    /**
     * transpose and adjoint
     */
    // transpose
    EMatrix<Dtype> transpose() const {
        auto tmp = m_mat.transpose();
        return EMatrix<Dtype>(std::move(tmp));
    }

    // adjoint
    EMatrix<Dtype> adjoint() const {
        auto tmp = m_mat.adjoint();
        return EMatrix<Dtype>(std::move(tmp));
    }

    /**
     * operations by contant value
     */
    // scalar product
    EMatrix<Dtype> operator* (Dtype val) const {
        auto tmp = m_mat * val;
        return EMatrix<Dtype>(std::move(tmp));
    }

    EMatrix<Dtype>& operator*= (Dtype val) {
        m_mat *= val;
        return *this;
    }

    // scalar +
    EMatrix<Dtype> operator+ (Dtype val) const {
        auto tmp = m_mat + val;
        return EMatrix<Dtype>(std::move(tmp));
    }

    EMatrix<Dtype>& operator+= (Dtype val) {
        m_mat += val;
        return *this;
    }

    // scalar -
    EMatrix<Dtype> operator- (Dtype val) const {
        auto tmp = m_mat - val;
        return EMatrix<Dtype>(std::move(tmp));
    }

    EMatrix<Dtype>& operator-= (Dtype val) {
        m_mat -= val;
        return *this;
    }

    // matrix dot product
    EMatrix<Dtype> operator* (const EMatrix<Dtype>& other) const {
        auto tmp = m_mat * other.get_raw_mat();
        return EMatrix<Dtype>(std::move(tmp));
    }

    // matrix add function
    EMatrix<Dtype> operator+ (const EMatrix<Dtype>& other) const {
        if(other.rows() == 1) {
            assert(other.cols() == cols());
            EMatrix<Dtype> tmp_mat(*this);
            for(Index i = 0; i < rows(); ++i) {
                for(Index j = 0; j < cols(); ++j) {
                    tmp_mat.at(i, j) += other(0, j);
                }
            }
            return tmp_mat;
        } else if(other.cols() == 1) {
            assert(other.rows() == rows());
            EMatrix<Dtype> tmp_mat(*this);
            for(Index j = 0; j < cols(); ++j) {
                for(Index i = 0; i < rows(); ++i) {
                    tmp_mat.at(i, j) += other(i, 0);
                }
            }
            return tmp_mat;
        } else {
            auto tmp = m_mat + other.get_raw_mat();
            return EMatrix<Dtype>(std::move(tmp));
        }
    }

    EMatrix<Dtype>& operator+= (const EMatrix<Dtype>& other) {
        if(other.rows() == 1) {
            assert(other.cols() == cols());
            for(Index i = 0; i < rows(); ++i) {
                for(Index j = 0; j < cols(); ++j) {
                    at(i, j) += other(0, j);
                }
            }
        } else if(other.cols() == 1) {
            assert(other.rows() == rows());
            EMatrix<Dtype> tmp_mat(*this);
            for(Index j = 0; j < cols(); ++j) {
                for(Index i = 0; i < rows(); ++i) {
                    at(i, j) += other(i, 0);
                }
            }
        } else {
            m_mat += other.get_raw_mat();
        }
        return *this;
    }

    /**
     * Block
     */
    EMatrix<Dtype> block(Index i, Index j, Index h, Index w) const {
        auto tmp = m_mat.block(i, j, h, w);
        return EMatrix<Dtype>(std::move(tmp));
    }

    EMatrix<Dtype> row(Index i) const {
        auto tmp = m_mat.row(i);
        return EMatrix<Dtype>(std::move(tmp));
    }

    EMatrix<Dtype> col(Index j) const {
        auto tmp = m_mat.col(j);
        return EMatrix<Dtype>(std::move(tmp));
    }

    void mutable_row(Index i, const EMatrix<Dtype>& row_vec) {
        m_mat.row(i) = row_vec.get_raw_mat();
    }

    void mutable_col(Index j, const EMatrix<Dtype>& col_vec) {
        m_mat.col(j) = col_vec.get_raw_mat();
    }

    /**
     * Getters
     */
    inline bool empty() const { return m_mat.rows() == 0; }
    inline Index rows() const { return m_mat.rows(); }
    inline Index cols() const { return m_mat.cols(); }
    inline Dtype at(Index i, Index j) const { return m_mat(i, j); }
    inline Dtype& at(Index i, Index j) { return m_mat(i, j); }
    inline Dtype operator() (Index i, Index j) const { return at(i, j); }
    inline Dtype& operator() (Index i, Index j) { return at(i, j); }
    inline Dtype at(Index i) const { return m_mat(i); }
    inline Dtype& at(Index i) { return m_mat(i); }
    inline Dtype operator() (Index i) const { return at(i); }
    inline Dtype& operator() (Index i) { return at(i); }

    /**
     * some simple function
     */
    inline Dtype sum() const { return m_mat.sum(); }
    inline Dtype prod() const { return m_mat.prod(); }
    inline Dtype min() const { return m_mat.minCoeff(); }
    inline Dtype max() const { return m_mat.maxCoeff(); }
    inline Dtype trace() const { return m_mat.trace(); }

    /**
     * the print functions
     */
    inline friend std::ostream& operator<<(std::ostream &os, const EMatrix<Dtype>& e) {
        os << e.m_mat;
        return os;
    }

private:
    //the contranier of matrix datas
    RawMatrix<Dtype> m_mat;

private:

    void InitByValue(Dtype val) {
        Index length = m_mat.rows() * m_mat.cols();
        for(Index i = 0; i < length; ++i) {
            m_mat(i) = val;
        }
    }

public:
    // the function to apply element wise operation
    template<typename Fun, typename... Args>
    void element_apply(Index i, Index j, Index p, Index q, Fun&& fun, Args&&... args) {
        assert(i >= 0 && j >= 0 && i + p < rows() && j + q < cols());
        for(Index ii = i; ii < i + p; ++ii) {
            for(Index jj = j; jj < j + q; ++jj) {
                m_mat(ii, jj) = fun(std::forward<Args>(args)...);
            }
        }
    }

};

} //wzp

#endif /*WZP_EIGEN_MATRIX_HPP_*/


/**
 * ECI to ECEF conversion matrices
 *
 * Copyright 2013 Bruce Ide
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 */

#include <Eigen/Core>
#include "gmst.hpp"

#ifndef _HPP_CONVERSION_MATRICES
#define _HPP_CONVERSION_MATRICES

namespace fr {

  namespace coordinates {

    // 6x6 population is the same for both classes, so may as well
    // only write it once.
    class ec_conversion_matrix_interface {
    public:
      ec_conversion_matrix_interface()
      {
      }

      virtual Eigen::Matrix3d get() = 0;
      virtual Eigen::Matrix3d get_dot() = 0;

      virtual Eigen::Matrix<double,6,6> get_xyz_vel()
      {
	Eigen::Matrix3d mat = get();
	Eigen::Matrix3d mat_dot = get_dot();
	Eigen::Matrix<double,6,6> retval;

	for (register int i = 0; i < 3; ++i) {
	  for (register int j = 0; j < 3; ++j) {
	    retval(i,j) = mat(i,j);
	    retval(i,j+3) = 0;
	    retval(i+3,j) = mat_dot(i,j);
	    retval(i+3,j+3) = mat(i,j);
	  }
	}
	return retval;
      }
    };

    /**
     * This class requires a time at which the coordinate was observed.
     */

    class eci_to_ecef : public ec_conversion_matrix_interface {
      double at_time;
      double gha_rad;
      double st,ct;
      double we;

    public:
      eci_to_ecef(const double &at_time) : at_time(at_time)
      {
	fr::time::gmst time_gmst(at_time);
	gha_rad = time_gmst.get_gmst() * 2.0 * fr::constants::pi / fr::constants::secs_per_ut1_day;
	st = sin(gha_rad);
	ct = cos(gha_rad);
	we = fr::constants::ut1_sideral_day_ratio * 2.0 * fr::constants::pi / fr::constants::secs_per_ut1_day;
      }

      ~eci_to_ecef()
      {
      }

      Eigen::Matrix3d get()
      {
	Eigen::Matrix3d retval;
	retval << ct,st,0.0,
	  -1.0 * st,ct,0.0,
	  0.0,0.0,1.0;
	return retval;
      }

      Eigen::Matrix3d get_dot()
      {
	Eigen::Matrix3d retval;
	retval << (-1.0 * we) * st, we * ct, 0.0,
	  (-1.0 * we) * ct, -we * st, 0.0,
	  0.0,0.0,0.0;
	return retval;
      }

    };

    // We can just transpose the eci to ecef matrix to get the ecef to
    // eci matrix

    class ecef_to_eci : public ec_conversion_matrix_interface {
      eci_to_ecef worker;
    public:
      ecef_to_eci(const double &at_time) : worker(at_time)
      {
      }

      ~ecef_to_eci()
      {
      }

      Eigen::Matrix3d get()
      {
	Eigen::Matrix3d interim = worker.get();
	Eigen::Matrix3d retval = interim.transpose();
	return retval;
      }

      Eigen::Matrix3d get_dot()
      {
	Eigen::Matrix3d retval = worker.get_dot().transpose();
	return retval;
      }

    };

  }

}

#endif


#include <cmath>
#include <memory>
#include <utility>

#include <Eigen/Core>
#include <glog/logging.h>
#include <gtest/gtest.h>

#include <geometric-vision/linear-triangulation.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/quaternion-math.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>

using namespace geometric_vision;  // NOLINT

class GeometricVisionTwoViewTriangulationTest
    : public ::testing::TestWithParam<std::pair<double, double> > {};  // NOLINT

TEST_P(GeometricVisionTwoViewTriangulationTest, ParallelOpticalAxes) {
  LinearTriangulation triangulator;

  pose::Transformation camera0(
      pose::Position3D(0, 0, 0), pose::Quaternion(1, 0, 0, 0));
  pose::Transformation camera1(
      pose::Position3D(1, 0, 0), pose::Quaternion(1, 0, 0, 0));

  std::pair<double, double> measurements = GetParam();
  Eigen::Vector2d measurement0(measurements.first, 0);
  Eigen::Vector2d measurement1(measurements.second, 0);

  Eigen::Vector3d triangulated_point;
  EXPECT_TRUE(
      triangulator.triangulateFromNormalizedTwoViews(
          measurement0, camera0, measurement1, camera1, &triangulated_point));

  // Calculation of expected result.
  const double b = camera0.getPosition()(0) - camera1.getPosition()(0);
  const double f = 1;
  const double Z = (b * f) / (measurement0(0) - measurement1(0));
  const double X = measurement0(0) * Z / f;
  const double Y = measurement0(1) * Z / f;
  Eigen::Vector3d expected_result(X, Y, Z);

  EXPECT_NEAR_EIGEN(expected_result, triangulated_point, 1e-15);
}

INSTANTIATE_TEST_CASE_P(
    GeometricVision, GeometricVisionTwoViewTriangulationTest,
    ::testing::Values(
        std::pair<double, double>(0.1, -0.1),
        std::pair<double, double>(0.2, -0.2),
        std::pair<double, double>(0.1, -0.2),
        std::pair<double, double>(0.05, -0.4),
        std::pair<double, double>(0.3, 0.0)));

struct TriangulationParams {
  TriangulationParams(
      double camera1_rotation_y, double camera1_x, double camera1_y,
      double camera1_z, double G_p_fi_x, double G_p_fi_y, double G_p_fi_z)
      : camera1_rotation_Y_(camera1_rotation_y),
        camera1_position_(camera1_x, camera1_y, camera1_z),
        G_p_fi_(G_p_fi_x, G_p_fi_y, G_p_fi_z) {}

  double camera1_rotation_Y_;
  Eigen::Vector3d camera1_position_;
  Eigen::Vector3d G_p_fi_;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

class GeometricVisionRotatedTwoViewParamTest
    : public ::testing::TestWithParam<TriangulationParams> {
 protected:
  Eigen::Vector2d reprojectPoint(
      const pose::Transformation& transform, const Eigen::Vector3d& G_p_fi) {
    const Eigen::Quaterniond& G_q_C =
        transform.getRotation().toImplementation();
    Eigen::Matrix3d G_R_C = G_q_C.toRotationMatrix();

    // TODO(dymczykm) Investigate why the transformation need to be
    // inverted here
    //    const Eigen::Vector3d C_p_fi = C_R_G
    //        * (G_p_fi - transform.getPosition().toImplementation());
    const Eigen::Vector3d C_p_fi = G_R_C * G_p_fi + transform.getPosition();

    return C_p_fi.hnormalized().head<2>();
  }

  pose::Quaternion getQuaternionFromYRotation(double rotation) {
    // Invert quaternion to get passive rotation.
    return pose::Quaternion(cos(rotation / 2), 0.0, -sin(rotation / 2), 0.0);
  }
};

INSTANTIATE_TEST_CASE_P(
    GeometricVision, GeometricVisionRotatedTwoViewParamTest,
    ::testing::Values(
        TriangulationParams(0, 3, 0, 0, 1.5, 0, 5),
        TriangulationParams(0, 1, 0.05, 0, 0.5, 0.1, 5),
        TriangulationParams(0, 1.2, 0, 0.1, 0.64, 0, 3.2),
        TriangulationParams(-0.4, 1, 0.1, -0.3, 0.64, 0.05, 5.1),
        TriangulationParams(-M_PI / 2, 1, 0.0, 1.0, 0, 0, 1)));

TEST_P(
    GeometricVisionRotatedTwoViewParamTest, TwoViewTriangulationOneCamRotated) {
  LinearTriangulation triangulator;
  TriangulationParams params = GetParam();

  pose::Transformation camera0(
      pose::Position3D(0, 0, 0), pose::Quaternion(1, 0, 0, 0));
  pose::Transformation camera1(
      pose::Position3D(params.camera1_position_),
      getQuaternionFromYRotation(params.camera1_rotation_Y_));

  Eigen::Vector2d measurement0 = this->reprojectPoint(camera0, params.G_p_fi_);
  Eigen::Vector2d measurement1 = this->reprojectPoint(camera1, params.G_p_fi_);

  Eigen::Vector3d triangulated_point, triangulated_point_homog;
  EXPECT_TRUE(
      triangulator.triangulateFromNormalizedTwoViews(
          measurement0, camera0, measurement1, camera1, &triangulated_point));
  EXPECT_TRUE(
      triangulator.triangulateFromNormalizedTwoViewsHomogeneous(
          measurement0, camera0, measurement1, camera1,
          &triangulated_point_homog));
  EXPECT_NEAR_EIGEN(params.G_p_fi_, triangulated_point, 1e-12);
  EXPECT_NEAR_EIGEN(triangulated_point, triangulated_point_homog, 1e-12);
}

TEST(GeometricVision, TwoViewTriangulationParallelRays) {
  LinearTriangulation triangulator;
  pose::Transformation camera0(
      pose::Position3D(0, 0, 0), pose::Quaternion(1, 0, 0, 0));
  pose::Transformation camera1(
      pose::Position3D(1, 0, 0), pose::Quaternion(1, 0, 0, 0));

  Eigen::Vector2d measurement0(0, 0);
  Eigen::Vector2d measurement1(0, 0);

  Eigen::Vector3d triangulated_point;
  EXPECT_FALSE(
      triangulator.triangulateFromNormalizedTwoViews(
          measurement0, camera0, measurement1, camera1, &triangulated_point));
}

MAPLAB_UNITTEST_ENTRYPOINT


#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Dense>
#include <math.h>

#include <unsupported/Eigen/MatrixFunctions>
#include <Eigen/Core>

using namespace std;
using namespace Eigen;
//using Eigen::MatrixXd;

MatrixXd readMatrix(const char *filename)
{
    std::ifstream indata;

    indata.open(filename);

    std::string line;
    getline(indata, line);
    std::stringstream lineStream(line);
    std::string cell;
    int count = 0, raw, coll;
    while (std::getline(lineStream, cell, ','))
    {
        if(count == 0) raw = stoi(cell);
        else coll = stoi( cell );
        count++;
    }
    MatrixXd res(raw,coll);
    cout<<"Matrix has been created"<<endl;
    raw = 0; coll = 0;
    while (getline(indata, line))
    {

        std::stringstream lineStream(line);
        std::string cell;

        while (std::getline(lineStream, cell, ','))
        {

            //cout<<"Raw: "<<raw<<" Coll: "<<coll<<endl;
            res(raw,coll) = stof(cell);
            coll++;
        }
        coll = 0;
        raw++;
    }
    indata.close();
    return res;
};

//TODO:
//Saving type of matrix
void writeMatrix(const char *filename, MatrixXd mat)
{
    ofstream outdata(filename,ios_base::out);
    int raws = mat.rows();
    int cols = mat.cols();
    outdata<<raws<<","<<cols<<endl;
    for(int i = 0; i< raws; i++)
    {
        for(int j = 0; j < cols-1; j++)
        {
            outdata<<mat(i,j)<<",";
        }
        outdata<<mat(i,cols-1)<<endl;
    }
    outdata.close();

}

/* void test_read_write_matrix_into_csv()
{
  MatrixXd m = MatrixXd::Random(3,3);
  writeMatrix("test_w.csv", m);
  MatrixXd n = readMatrix("test_w.csv");
  if(m == n) cout<<"True"<<endl;
  cout<<m<<endl;
  cout<<"===="<<endl;
  cout<<n<<endl;
} */

double Exp(double x) // the functor we want to apply
{
    return std::exp(x);
}

int main()
{
    MatrixXd X = readMatrix("X.csv");
    MatrixXd y = readMatrix("y.csv");
    MatrixXd z, z1, p, p1, u, w ;
    float b0 = log(y.mean() / (1 - y.mean()) ) ;
    cout<< b0<<endl;
    VectorXd b = ArrayXd::Zero(20);
    VectorXd b_old = ArrayXd::Zero(20);
    b(0) = b0;
    cout<<"Transposing"<<endl;
    b = b.transpose();
    for(int i = 0; i < 20; i++)
    {
        z = X * b;
        z = -z;
        z = z.unaryExpr([](double d) {return std::exp(d);});
        z1 = z.unaryExpr([](double d) {return d + 1.0;});
        p = z1.unaryExpr([](double d) {return 1.0 / d;});
        p1 = p.unaryExpr([](double d) {return 1 - d;});
        w = p * p1;
        w = w.unaryExpr([](double d) {return 1.0 / d;});
        u = z + (y -  p) * w;
        b_old = b;

    }
    // cout<<m;
    return 0;

}


#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include <manifold/SO3.h>
#include <manifold/gradientDescentSE3.h>
#include <manifold/newtonSE3.h>
#include <random>

class Gmm2pc : public GDSE3<double> {
 public:
  Gmm2pc(const Eigen::Vector3d& muA, const
      Eigen::Matrix3d& covA, const Eigen::MatrixXd& xB)
    : piA_(1.), muA_(muA), covA_(covA), xB_(xB)
  {
    std::cout << "-A-"
      << muA.transpose() << std::endl
      << covA << std::endl;
    std::cout << "-B-"
      << xB_.cols() << std::endl;
  };

  virtual void ComputeJacobian(const SE3d& theta, Eigen::Matrix<double,6,1>* J, double* f) {
    SE3d T = theta;
    Eigen::Matrix3d R = T.matrix().topLeftCorner(3,3);
    Eigen::Vector3d t = T.matrix().topRightCorner(3,1);
    uint32_t N = xB_.cols();
    if (J) J->fill(0.);
    if (f) *f = 0.;
    for (uint32_t i=0; i<N; ++i) {
      double logCA = -0.5*log(2.*M_PI)*3-0.5*log(covA_.determinant());
      double logD = log(piA_) + logCA;
      Eigen::Vector3d a = R*xB_.col(i)+t-muA_;
      double z = -0.5*a.dot(covA_.ldlt().solve(a));
      if (J) {
        J->topRows(3) -= covA_.ldlt().solve(a);
        for (uint32_t j=0; j<3; ++j) {
          (*J)(3+j) -= a.dot(covA_.ldlt().solve(SO3d::G(j)*R*xB_.col(i)));
        }
      }
      if (f)
        *f += logD + z;
    }
    if (J) *J *= -1./N;
    if (f) *f *= -1./N;
  };
 protected:
  double piA_;
  Eigen::Vector3d muA_;
  Eigen::Matrix3d covA_;
  Eigen::MatrixXd xB_;
};

class Gmm2pcNewton : public NewtonSE3<double> {
 public:
  Gmm2pcNewton(const Eigen::Vector3d& muA, const
      Eigen::Matrix3d& covA, const Eigen::MatrixXd& xB)
    : piA_(1.), muA_(muA), covA_(covA), xB_(xB)
  {
    std::cout << "-A-"
      << muA.transpose() << std::endl
      << covA << std::endl;
    std::cout << "-B-"
      << xB_.cols() << std::endl;
  };

  virtual void ComputeJacobianAndHessian(const SE3d& theta,
      Eigen::Matrix<double,6,6>*H, Eigen::Matrix<double,6,1>* J,
      double* f) {
    SE3d T = theta;
    Eigen::Matrix3d R = T.matrix().topLeftCorner(3,3);
    Eigen::Vector3d t = T.matrix().topRightCorner(3,1);
    uint32_t N = xB_.cols();
    if (H) H->fill(0.);
    if (J) J->fill(0.);
    if (f) *f = 0.;
    for (uint32_t i=0; i<N; ++i) {
      Eigen::Vector3d Rx = R*xB_.col(i);
      double logCA = -0.5*log(2.*M_PI)*3-0.5*log(covA_.determinant());
      double logD = log(piA_) + logCA;
      Eigen::Vector3d a = Rx+t-muA_;
      double z = -0.5*a.dot(covA_.ldlt().solve(a));
      if (H) {
        H->topLeftCorner(3,3) -= covA_.inverse();
        for (uint32_t j=0; j<3; ++j) {
          Eigen::Vector3d Htw_j = covA_.ldlt().solve(SO3d::G(j)*Rx);
          H->block<3,1>(0,j+3) -= Htw_j;
          H->block<1,3>(j+3,0) -= Htw_j.transpose();
        }
        for (uint32_t k=0; k<3; ++k) {
          for (uint32_t j=0; j<3; ++j) {
            (*H)(3+j,3+k) -= 0.5*(a.dot(covA_.ldlt().solve((
                    SO3d::G(k)*SO3d::G(j)+SO3d::G(j)*SO3d::G(k))*Rx)))
              - Rx.dot(SO3d::G(j)*covA_.ldlt().solve(SO3d::G(k)*Rx));
          }
        }
//        std::cout << *H << std::endl << std::endl;
      }
      if (J) {
        J->topRows(3) -= covA_.ldlt().solve(a);
        for (uint32_t j=0; j<3; ++j) {
          (*J)(3+j) -= a.dot(covA_.ldlt().solve(SO3d::G(j)*R*xB_.col(i)));
        }
      }
      if (f)
        *f += logD + z;
    }
    if (H) *H *= -1./N;
    if (J) *J *= -1./N;
    if (f) *f *= -1./N;
  };
 protected:
  double piA_;
  Eigen::Vector3d muA_;
  Eigen::Matrix3d covA_;
  Eigen::MatrixXd xB_;
};

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

  double theta = 15.*M_PI/180.;
  Eigen::Matrix3d R;
  R << 1, 0, 0,
         0, cos(theta), sin(theta),
         0, -sin(theta), cos(theta);
  Eigen::Vector3d t = Eigen::Vector3d::Ones();

  Eigen::Matrix3d covA =   Eigen::Vector3d(1.,.1,3.).asDiagonal();
  Eigen::Vector3d muA = Eigen::Vector3d::Zero();

  std::random_device rd;
  std::mt19937 gen(rd());
  std::normal_distribution<> d1(muA(0),sqrt(covA(0,0)));
  std::normal_distribution<> d2(muA(1),sqrt(covA(1,1)));
  std::normal_distribution<> d3(muA(2),sqrt(covA(2,2)));

  Eigen::Matrix<double,3,Eigen::Dynamic> xB(3,1000);
  for (uint32_t i=0; i<xB.cols(); ++i) {
    xB(0,i) = d1(gen);
    xB(1,i) = d2(gen);
    xB(2,i) = d3(gen);
    xB.col(i) = R*xB.col(i) + t;
  }
  SE3d T;

  Gmm2pc gd(muA, covA, xB);
  gd.Compute(T, 1e-6, 200);
//  gd.Compute(T, 0, 200);
  T = gd.GetMinimum();
  Eigen::Vector3d tEst = T.matrix().topRightCorner(3,1);
  Eigen::Matrix3d REst = T.matrix().topLeftCorner(3,3);
  std::cout << " - T -" << std::endl;
  std::cout << T << std::endl;
  std::cout << " - R -" << std::endl;
  std::cout << R << std::endl;
  std::cout << " - Rest -" << std::endl;
  std::cout << REst.transpose() << std::endl;
  std::cout << " - t -" << std::endl;
  std::cout << t.transpose() << std::endl;
  std::cout << " - tEst -" << std::endl;
  std::cout << (-REst.transpose()*tEst).transpose() << std::endl;
  std::cout << " - dR = "
    << SO3d::Log_(R.transpose()*REst.transpose()).norm()*180./M_PI << std::endl;
  std::cout << " - dt = "
    << (t-(-REst.transpose()*tEst)).norm() << std::endl;

  T = SE3d();
  Gmm2pcNewton newton(muA, covA, xB);
  newton.Compute(T, 1e-6, 300);
//  gd.Compute(T, 0, 200);
  T = newton.GetMinimum();
  tEst = T.matrix().topRightCorner(3,1);
  REst = T.matrix().topLeftCorner(3,3);
  std::cout << " - T -" << std::endl;
  std::cout << T << std::endl;
  std::cout << " - R -" << std::endl;
  std::cout << R << std::endl;
  std::cout << " - Rest -" << std::endl;
  std::cout << REst.transpose() << std::endl;
  std::cout << " - t -" << std::endl;
  std::cout << t.transpose() << std::endl;
  std::cout << " - tEst -" << std::endl;
  std::cout << (-REst.transpose()*tEst).transpose() << std::endl;
  std::cout << " - dR = "
    << SO3d::Log_(R.transpose()*REst.transpose()).norm()*180./M_PI << std::endl;
  std::cout << " - dt = "
    << (t-(-REst.transpose()*tEst)).norm() << std::endl;
}


/*
 * test_STDP.cpp
 *
 *  Created on: 2019-4-18
 *      Author: fasiondog
 */

#ifdef TEST_ALL_IN_ONE
    #include <boost/test/unit_test.hpp>
#else
    #define BOOST_TEST_MODULE test_hikyuu_indicator_suite
    #include <boost/test/unit_test.hpp>
#endif

#include <fstream>
#include <hikyuu/StockManager.h>
#include <hikyuu/indicator/crt/KDATA.h>
#include <hikyuu/indicator/crt/STDP.h>
#include <hikyuu/indicator/crt/PRICELIST.h>

using namespace hku;

/**
 * @defgroup test_indicator_STDP test_indicator_STDP
 * @ingroup test_hikyuu_indicator_suite
 * @{
 */

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_STDP ) {
    /** @arg n > 1 的正常情况 */
    PriceList d;
    for (size_t i = 0; i < 15; ++i) {
        d.push_back(i+1);
    }
    d[5] = 4.0;
    d[7] = 4.0;
    d[11] = 6.0;

    Indicator ind = PRICELIST(d);
    Indicator dev = STDP(ind, 10);
    BOOST_CHECK(dev.name() == "STDP");
    BOOST_CHECK(dev.size() == 15);
    BOOST_CHECK(dev[8] == Null<price_t>());
    BOOST_CHECK(std::fabs(dev[9] - 2.77308) < 0.00001 );
    BOOST_CHECK(std::fabs(dev[10] - 2.98161) < 0.00001 );
    BOOST_CHECK(std::fabs(dev[11] - 2.68514) < 0.00001 );
    BOOST_CHECK(std::fabs(dev[12] - 3.1) < 0.00001 );
    BOOST_CHECK(std::fabs(dev[13] - 3.46554) < 0.00001 );
    BOOST_CHECK(std::fabs(dev[14] - 3.79605) < 0.00001 );

    /** @arg n = 1时 */
    dev = STDP(ind, 1);
    BOOST_CHECK(dev.name() == "STDP");
    BOOST_CHECK(dev.size() == 15);
    for (size_t i = 0; i < dev.size(); ++i) {
        BOOST_CHECK(dev[i] == Null<price_t>());
    }

    /** @arg operator() */
    Indicator expect = STDP(ind, 10);
    dev = STDP(10);
    Indicator result = dev(ind);
    BOOST_CHECK(result.size() == expect.size());
    for (size_t i = 0; i < expect.size(); ++i) {
        BOOST_CHECK(result[i] == expect[i]);
    }
}

//-----------------------------------------------------------------------------
// test export
//-----------------------------------------------------------------------------
#if HKU_SUPPORT_SERIALIZATION

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_STDP_export ) {
    StockManager& sm = StockManager::instance();
    string filename(sm.tmpdir());
    filename += "/STDP.xml";

    Stock stock = sm.getStock("sh000001");
    KData kdata = stock.getKData(KQuery(-20));
    Indicator ma1 = STDP(CLOSE(kdata), 10);
    {
        std::ofstream ofs(filename);
        boost::archive::xml_oarchive oa(ofs);
        oa << BOOST_SERIALIZATION_NVP(ma1);
    }

    Indicator ma2;
    {
        std::ifstream ifs(filename);
        boost::archive::xml_iarchive ia(ifs);
        ia >> BOOST_SERIALIZATION_NVP(ma2);
    }

    BOOST_CHECK(ma2.name() == "STDP");
    BOOST_CHECK(ma1.size() == ma2.size());
    BOOST_CHECK(ma1.discard() == ma2.discard());
    BOOST_CHECK(ma1.getResultNumber() == ma2.getResultNumber());
    for (size_t i = 0; i < ma1.size(); ++i) {
        BOOST_CHECK_CLOSE(ma1[i], ma2[i], 0.00001);
    }
}
#endif /* #if HKU_SUPPORT_SERIALIZATION */

/** @} */


#ifndef HYPSYS1D_RECONSTRUCTION_HPP
#define HYPSYS1D_RECONSTRUCTION_HPP

#include <Eigen/Dense>
#include <cmath>
#include <memory>

#include <ancse/grid.hpp>
#include <ancse/model.hpp>
#include <ancse/limiters.hpp>
#include <ancse/rate_of_change.hpp>
#include <ancse/simulation_time.hpp>

// Reconstructions:
class PWConstantReconstruction
{
    public:
        void set(const Eigen::MatrixXd &u) const
        {
            up.resize(u.rows(),u.cols());
            up = u;
        }

        /// Compute the left and right trace at the interface i + 1/2.
        /** Note: This API is agnostic to the number of cell-averages required
         *        by the method. Therefore, reconstructions with different stencil
         *        sizes can implement this API; and this call can be used in parts
         *        of the code that do not need to know about the details of the
         *        reconstruction.
         */
        std::pair<Eigen::VectorXd, Eigen::VectorXd> operator()(int i) const
        {
            return (*this)(up.col(i), up.col(i+1));
        }

        /// Compute the left and right trace at the interface.
        /** Piecewise constant reconstruction of the left and right trace only
         *  requires the cell-average to the left and right of the interface.
         *
         *  Note: Compared to the other overload this reduces the assumption on
         *        how the cell-averages are stored. This is useful when testing and
         *        generally makes the function useful in more situations.
         */
        inline std::pair<Eigen::VectorXd, Eigen::VectorXd> operator()
            (Eigen::VectorXd ua, Eigen::VectorXd ub) const
        {
            return {std::move(ua), std::move(ub)};
        }

        // To have method in common with PWLinearReconstruction:
        std::pair<Eigen::VectorXd, Eigen::VectorXd> operator()
            (const Eigen::MatrixXd& u, const int i) const
        {
            return (*this)(u.col(i), u.col(i + 1));
        }

    private:
        mutable Eigen::MatrixXd up;
};

// Wrapping the available scalar slope limiters for use with quantity vectors u
template <class SlopeLimiter>
class VectorSlopeLimiter
{
    public:
        explicit VectorSlopeLimiter(const SlopeLimiter& sigma) : sigma(sigma) {}

        Eigen::VectorXd operator() (const Eigen::VectorXd& sL, const Eigen::VectorXd& sR) const
        {
            assert ((sL.size() == sR.size()));

            const int size= sL.size();
            Eigen::VectorXd limited_s(size);

            for (int i= 0; i < size; i++)
                limited_s(i)= sigma(sL(i), sR(i));

            return limited_s;
        }

    private:
        SlopeLimiter sigma;
};

template <class SlopeLimiter>
class PWLinearReconstruction
{
    public:
        explicit PWLinearReconstruction(const SlopeLimiter& slope_limiter)
            : slope_limiter(slope_limiter) {}

        std::pair<Eigen::VectorXd, Eigen::VectorXd> operator()
            (const Eigen::MatrixXd& u, const int i) const
        {
            return (*this)(u.col(i - 1), u.col(i), u.col(i + 1), u.col(i + 2));
        }

        std::pair<Eigen::VectorXd, Eigen::VectorXd> operator()
            (Eigen::VectorXd ua, Eigen::VectorXd ub, Eigen::VectorXd uc, Eigen::VectorXd ud) const
        {
            Eigen::VectorXd sL= ub - ua;
            Eigen::VectorXd sM= uc - ub;
            Eigen::VectorXd sR= ud - uc;

            Eigen::VectorXd uL= ub + 0.5 * slope_limiter(sL, sM);
            Eigen::VectorXd uR= uc - 0.5 * slope_limiter(sM, sR);

            return {uL, uR};
        }

    private:
        VectorSlopeLimiter<SlopeLimiter> slope_limiter;
};

#endif // HYPSYS1D_RATE_OF_CHANGE_HPP


#include <vector>
#include <boost/math/distributions/normal.hpp>
#include "normal_dist.h"

stochastic::NormalDistribution::NormalDistribution(double mean, double std_dev)
  : Distribution(),
    mean_{mean},
    std_dev_{std_dev},
    distribution_{mean, std_dev_}
{}

std::vector<double> stochastic::NormalDistribution::cumulative_dist_func(
    const std::vector<double>& locations) const {
  std::vector<double> evaluations(locations.size());

  for (unsigned int i = 0; i < locations.size(); ++i) {
    evaluations[i] = cdf(distribution_, locations[i]);
  }

  return evaluations;
}

std::vector<double> stochastic::NormalDistribution::inv_cumulative_dist_func(
    const std::vector<double>& probabilities) const {
  std::vector<double> evaluations(probabilities.size());

  for (unsigned int i = 0; i < probabilities.size(); ++i) {
    evaluations[i] = quantile(distribution_, probabilities[i]);
  }

  return evaluations;
}