#pragma once
#ifndef OPTIMIZE_HPP
#define OPTIMIZE_HPP

// c++ libraries
#include <iosfwd>
// eigen libraries
#include <Eigen/Dense>
//serialization
#include "src/mem/serialize.hpp"

namespace Opt{

#ifndef OPT_PRINT_FUNC
#define OPT_PRINT_FUNC 0
#endif

#ifndef OPT_PRINT_DATA
#define OPT_PRINT_DATA 0
#endif

//***************************************************
// optimization method
//***************************************************

class Algo{
public:
	enum Type{
		UNKNOWN=0,
		SGD=1,
		SDM=2,
		NAG=3,
		ADAGRAD=4,
		ADADELTA=5,
		RMSPROP=6,
		ADAM=7,
		NADAM=8,
		AMSGRAD=9,
		BFGS=10,
		RPROP=11,
		CG=12,
	};
	//constructor
	Algo():t_(Type::UNKNOWN){}
	Algo(Type t):t_(t){}
	//operators
	operator Type()const{return t_;}
	//member functions
	static Algo read(const char* str);
	static const char* name(const Algo& algo);
private:
	Type t_;
	//prevent automatic conversion for other built-in types
	//template<typename T> operator T() const;
};
std::ostream& operator<<(std::ostream& out, const Algo& algo);

//***************************************************
// stopping criterion
//***************************************************

class Stop{
public:
	enum Type{
		UNKNOWN=0,
		FABS=1,
		FREL=2,
		XABS=3,
		XREL=4
	};
	//constructor
	Stop():t_(Type::UNKNOWN){}
	Stop(Type t):t_(t){}
	//operators
	operator Type()const{return t_;}
	//member functions
	static Stop read(const char* str);
	static const char* name(const Stop& stop);
private:
	Type t_;
	//prevent automatic conversion for other built-in types
	//template<typename T> operator T() const;
};
std::ostream& operator<<(std::ostream& out, const Stop& stop);

//***************************************************
// decay method
//***************************************************

class Decay{
public:
	enum Type{
		UNKNOWN=0,
		CONST=1,
		EXP=2,
		SQRT=3,
		INV=4,
		POW=5,
		STEP=6
	};
	//constructor
	Decay():t_(Type::UNKNOWN){}
	Decay(Type t):t_(t){}
	//operators
	operator Type()const{return t_;}
	//member functions
	static Decay read(const char* str);
	static const char* name(const Decay& decay);
private:
	Type t_;
	//prevent automatic conversion for other built-in types
	//template<typename T> operator T() const;
};
std::ostream& operator<<(std::ostream& out, const Decay& decay);

//***************************************************
// loss function
//***************************************************

struct Loss{
public:
	enum Type{
		UNKNOWN=0,
		MSE=1,
		MAE=2,
		HUBER=3
	};
	//constructor
	Loss():t_(Type::UNKNOWN){}
	Loss(Type t):t_(t){}
	//operators
	operator Type()const{return t_;}
	//member functions
	static Loss read(const char* str);
	static const char* name(const Loss& loss);
	//error
	static double error(Loss loss, const Eigen::VectorXd& value, const Eigen::VectorXd& target);
	static double error(Loss loss, const Eigen::VectorXd& value, const Eigen::VectorXd& target, Eigen::VectorXd& grad);
private:
	Type t_;
	//prevent automatic conversion for other built-in types
	//template<typename T> operator T() const;
};
std::ostream& operator<<(std::ostream& out, const Loss& loss);

//***************************************************
// Data
//***************************************************

class Data{
private:
	//count
		int nPrint_;//print data every n steps
		int nWrite_;//write data every n steps
		int step_;//current step
		int count_;//current count
	//stopping
		int max_;//max steps
		double tol_;//stop tolerance
	//status
		double val_,valOld_;//current, old value
		double dv_,dp_;//change in value, p
	//algorithm
		Algo algo_;//optimization algorithm
		Stop stop_;//the type of value determining the end condition
	//parameters
		int dim_;//dimension of problem
		Eigen::VectorXd p_,pOld_;//current, old parameters
		Eigen::VectorXd g_,gOld_;//current, old gradients
public:
	//==== constructors/destructors ====
	Data(){defaults();}
	Data(int dim){defaults();init(dim);}
	~Data(){}

	//==== operators ====
	friend std::ostream& operator<<(std::ostream& out, const Data& data);

	//==== access ====
	//status
		double& val(){return val_;}
		const double& val()const{return val_;}
		double& valOld(){return valOld_;}
		const double& valOld()const{return valOld_;}
		double& dv(){return dv_;}
		const double& dv()const{return dv_;}
		double& dp(){return dp_;}
		const double& dp()const{return dp_;}
	//count
		int& nPrint(){return nPrint_;}
		const int& nPrint()const{return nPrint_;}
		int& nWrite(){return nWrite_;}
		const int& nWrite()const{return nWrite_;}
		int& step(){return step_;}
		const int& step()const{return step_;}
		int& count(){return count_;}
		const int& count()const{return count_;}
	//stopping
		double& tol(){return tol_;}
		const double& tol()const{return tol_;}
		int& max(){return max_;}
		const int& max()const{return max_;}
	//parameters
		int& dim(){return dim_;}
		const int& dim()const{return dim_;}
		Eigen::VectorXd& p(){return p_;}
		const Eigen::VectorXd& p()const{return p_;}
		Eigen::VectorXd& pOld(){return pOld_;}
		const Eigen::VectorXd& pOld()const{return pOld_;}
		Eigen::VectorXd& g(){return g_;}
		const Eigen::VectorXd& g()const{return g_;}
		Eigen::VectorXd& gOld(){return gOld_;}
		const Eigen::VectorXd& gOld()const{return gOld_;}
	//algorithm
		Algo& algo(){return algo_;}
		const Algo& algo()const{return algo_;}
		Stop& stop(){return stop_;}
		const Stop& stop()const{return stop_;}

	//==== member functions ====
	void defaults();
	void clear(){defaults();}
	void init(int dim);
};

//***************************************************
// Model
//***************************************************

class Model{
protected:
	int dim_;//dimension of the problem
	int period_;//period of decay
	Algo algo_;//optimization algorithm
	Decay decay_;//decay schedule
	double alpha_;//step decay constant
	double gamma_;//gradient step size
	double lambda_;//regularization parameter
	double power_;//step decay power
	double mix_;//mixing parameter
public:
	//==== constructors/destructors ====
	Model(){defaults();}
	virtual ~Model(){}

	//==== operators ====
	friend std::ostream& operator<<(std::ostream& out, const Model& model);

	//==== access ====
	int& dim(){return dim_;}
	const int& dim()const{return dim_;}
	int& period(){return period_;}
	const int& period()const{return period_;}
	Algo& algo(){return algo_;}
	const Algo& algo()const{return algo_;}
	Decay& decay(){return decay_;}
	const Decay& decay()const{return decay_;}
	double& alpha(){return alpha_;}
	const double& alpha()const{return alpha_;}
	double& gamma(){return gamma_;}
	const double& gamma()const{return gamma_;}
	double& lambda(){return lambda_;}
	const double& lambda()const{return lambda_;}
	double& power(){return power_;}
	const double& power()const{return power_;}
	double& mix(){return mix_;}
	const double& mix()const{return mix_;}

	//==== member functions ====
	void defaults();
	void clear();
	void update_step(int step);

	//==== virtual functions ====
	virtual void step(Data& d)=0;
	virtual void init(int dim);

	//==== static functions ====
	static std::ostream& print(std::ostream& out, const Model* model);
};

//steepest-desccent
class SGD final: public Model{
public:
	//constructors/destructors
	SGD(){defaults();}
	SGD(int dim){init(dim);}
	~SGD(){}
	//member functions
	void step(Data& d);
	void defaults();
	void init(int dim);
	//operators
	friend std::ostream& operator<<(std::ostream& out, const SGD& sgd);
};

//steepest-descent + momentum
class SDM final: public Model{
private:
	double eta_;//mixing term
	Eigen::VectorXd dx_;//change in parameters
public:
	//constructors/destructors
	SDM(){defaults();}
	SDM(int dim){init(dim);}
	~SDM(){}
	//access
	double& eta(){return eta_;}
	const double& eta()const{return eta_;}
	Eigen::VectorXd& dx(){return dx_;}
	const Eigen::VectorXd& dx()const{return dx_;}
	//member functions
	void step(Data& d);
	void defaults();
	void init(int dim);
	//operators
	friend std::ostream& operator<<(std::ostream& out, const SDM& sdm);
};

//nesterov accelerated gradient
class NAG final: public Model{
private:
	double eta_;//mixing term
	Eigen::VectorXd dx_;
public:
	//constructors/destructors
	NAG(){defaults();}
	NAG(int dim){init(dim);}
	~NAG(){}
	//access
	double& eta(){return eta_;}
	const double& eta()const{return eta_;}
	Eigen::VectorXd& dx(){return dx_;}
	const Eigen::VectorXd& dx()const{return dx_;}
	//member functions
	void step(Data& d);
	void defaults();
	void init(int dim);
	//operators
	friend std::ostream& operator<<(std::ostream& out, const NAG& nag);
};

//adagrad
class ADAGRAD final: public Model{
private:
	static const double eps_;//small term to prevent divergence
	Eigen::VectorXd mgrad2_;//avg of square of gradient
public:
	//constructors/destructors
	ADAGRAD(){defaults();}
	ADAGRAD(int dim){init(dim);}
	~ADAGRAD(){}
	//access
	Eigen::VectorXd& mgrad2(){return mgrad2_;}
	const Eigen::VectorXd& mgrad2()const{return mgrad2_;}
	//member functions
	void step(Data& d);
	void defaults();
	void init(int dim);
	//operators
	friend std::ostream& operator<<(std::ostream& out, const ADAGRAD& adagrad);
};

//adadelta
class ADADELTA final: public Model{
private:
	static const double eps_;//small term to prevent divergence
	double eta_;//mixing fraction
	Eigen::VectorXd mgrad2_;//avg of square of gradient
	Eigen::VectorXd mdx2_;//avg of square of dx
	Eigen::VectorXd dx_;//change in x
public:
	//constructors/destructors
	ADADELTA(){defaults();}
	ADADELTA(int dim){init(dim);}
	~ADADELTA(){}
	//access
	double& eta(){return eta_;}
	const double& eta()const{return eta_;}
	Eigen::VectorXd& mgrad2(){return mgrad2_;}
	const Eigen::VectorXd& mgrad2()const{return mgrad2_;}
	Eigen::VectorXd& mdx2(){return mdx2_;}
	const Eigen::VectorXd& mdx2()const{return mdx2_;}
	Eigen::VectorXd& dx(){return dx_;}
	const Eigen::VectorXd& dx()const{return dx_;}
	//member functions
	void step(Data& d);
	void defaults();
	void init(int dim);
	//operators
	friend std::ostream& operator<<(std::ostream& out, const ADADELTA& adadelta);
};

//rmsprop
class RMSPROP final: public Model{
private:
	static const double eps_;//small term to prevent divergence
	Eigen::VectorXd mgrad2_;//avg of square of gradient
public:
	//constructors/destructors
	RMSPROP(){defaults();}
	RMSPROP(int dim){init(dim);}
	~RMSPROP(){}
	//access
	Eigen::VectorXd& mgrad2(){return mgrad2_;}
	const Eigen::VectorXd& mgrad2()const{return mgrad2_;}
	//member functions
	void step(Data& d);
	void defaults();
	void init(int dim);
	//operators
	friend std::ostream& operator<<(std::ostream& out, const RMSPROP& rmsprop);
};

//adam
class ADAM final: public Model{
private:
	static const double eps_;//small term to prevent divergence
	static const double beta1_;
	static const double beta2_;
	double beta1i_;//power w.r.t i
	double beta2i_;//power w.r.t i
	Eigen::VectorXd mgrad_;//avg of gradient
	Eigen::VectorXd mgrad2_;//avg of square of gradient
public:
	//constructors/destructors
	ADAM(){defaults();}
	ADAM(int dim){init(dim);}
	~ADAM(){}
	//access
	double& beta1i(){return beta1i_;}
	const double& beta1i()const{return beta1i_;}
	double& beta2i(){return beta2i_;}
	const double& beta2i()const{return beta2i_;}
	Eigen::VectorXd& mgrad(){return mgrad_;}
	const Eigen::VectorXd& mgrad()const{return mgrad_;}
	Eigen::VectorXd& mgrad2(){return mgrad2_;}
	const Eigen::VectorXd& mgrad2()const{return mgrad2_;}
	//member functions
	void step(Data& d);
	void defaults();
	void init(int dim);
	//operators
	friend std::ostream& operator<<(std::ostream& out, const ADAM& adam);
};

//nadam
class NADAM final: public Model{
private:
	static const double eps_;//small term to prevent divergence
	static const double beta1_;
	static const double beta2_;
	double beta1i_;//power w.r.t i
	double beta2i_;//power w.r.t i
	Eigen::VectorXd mgrad_;//avg of gradient
	Eigen::VectorXd mgrad2_;//avg of square of gradient
public:
	//constructors/destructors
	NADAM(){defaults();}
	NADAM(int dim){init(dim);}
	~NADAM(){}
	//access
	double& beta1i(){return beta1i_;}
	const double& beta1i()const{return beta1i_;}
	double& beta2i(){return beta2i_;}
	const double& beta2i()const{return beta2i_;}
	Eigen::VectorXd& mgrad(){return mgrad_;}
	const Eigen::VectorXd& mgrad()const{return mgrad_;}
	Eigen::VectorXd& mgrad2(){return mgrad2_;}
	const Eigen::VectorXd& mgrad2()const{return mgrad2_;}
	//member functions
	void step(Data& d);
	void defaults();
	void init(int dim);
	//operators
	friend std::ostream& operator<<(std::ostream& out, const NADAM& nadam);
};

//amsgrad
class AMSGRAD final: public Model{
private:
	static const double eps_;//small term to prevent divergence
	static const double beta1_;
	static const double beta2_;
	double beta1i_;//power w.r.t i
	double beta2i_;//power w.r.t i
	Eigen::VectorXd mgrad_;//avg of gradient
	Eigen::VectorXd mgrad2_;//avg of square of gradient
	Eigen::VectorXd mgrad2m_;
public:
	//constructors/destructors
	AMSGRAD(){defaults();}
	AMSGRAD(int dim){init(dim);}
	~AMSGRAD(){}
	//access
	double& beta1i(){return beta1i_;}
	const double& beta1i()const{return beta1i_;}
	double& beta2i(){return beta2i_;}
	const double& beta2i()const{return beta2i_;}
	Eigen::VectorXd& mgrad(){return mgrad_;}
	const Eigen::VectorXd& mgrad()const{return mgrad_;}
	Eigen::VectorXd& mgrad2(){return mgrad2_;}
	const Eigen::VectorXd& mgrad2()const{return mgrad2_;}
	Eigen::VectorXd& mgrad2m(){return mgrad2m_;}
	const Eigen::VectorXd& mgrad2m()const{return mgrad2m_;}
	//member functions
	void step(Data& d);
	void defaults();
	void init(int dim);
	//operators
	friend std::ostream& operator<<(std::ostream& out, const AMSGRAD& amsgrad);
};

//bfgs
class BFGS final: public Model{
private:
	Eigen::MatrixXd B_,BOld_;
	Eigen::VectorXd s_,y_;
public:
	//constructors/destructors
	BFGS(){defaults();}
	BFGS(int dim){init(dim);}
	~BFGS(){}
	//member functions
	void step(Data& d);
	void defaults();
	void init(int dim);
	//operators
	friend std::ostream& operator<<(std::ostream& out, const BFGS& bfgs);
};

//rprop
class RPROP final: public Model{
private:
	static const double etaP;
	static const double etaM;
	static const double deltaMax;
	static const double deltaMin;
	Eigen::VectorXd delta_;
	Eigen::VectorXd dx_;
public:
	//constructors/destructors
	RPROP(){defaults();}
	RPROP(int dim){init(dim);}
	~RPROP(){}
	//access
	Eigen::VectorXd& delta(){return delta_;}
	const Eigen::VectorXd& delta()const{return delta_;}
	Eigen::VectorXd& dx(){return dx_;}
	const Eigen::VectorXd& dx()const{return dx_;}
	//member functions
	void step(Data& d);
	void defaults();
	void init(int dim);
	//operators
	friend std::ostream& operator<<(std::ostream& out, const RPROP& rprop);
};
/*
	Christian Igel and Michael Hüsken.
		Improving the Rprop Learning Algorithm.
		Second International Symposium on Neural Computation
		(NC 2000), pp. 115-121, ICSC Academic Press, 2000
	Christian Igel and Michael Hüsken.
		Empirical Evaluation of the Improved Rprop Learning Algorithm.
		Neurocomputing 50:105-123, 2003
*/

//cg
class CG final: public Model{
private:
	static const double eps_;//small term to prevent divergence
	Eigen::VectorXd cgd_;//cg direction
public:
	//constructors/destructors
	CG(){defaults();}
	CG(int dim){init(dim);}
	~CG(){}
	//access
	Eigen::VectorXd& cgd(){return cgd_;}
	const Eigen::VectorXd& cgd()const{return cgd_;}
	//member functions
	void step(Data& d);
	void defaults();
	void init(int dim);
	//operators
	friend std::ostream& operator<<(std::ostream& out, const CG& cg);
};

//read from file

Model& read(Model& model, const char* file);
Data& read(Data& data, const char* file);
SGD& read(SGD& sdg, const char* file);
SDM& read(SDM& sdm, const char* file);
NAG& read(NAG& nag, const char* file);
ADAGRAD& read(ADAGRAD& adagrad, const char* file);
ADADELTA& read(ADADELTA& adadelta, const char* file);
RMSPROP& read(RMSPROP& rmsprop, const char* file);
ADAM& read(ADAM& adam, const char* file);
NADAM& read(NADAM& nadam, const char* file);
BFGS& read(BFGS& bfgs, const char* file);
RPROP& read(RPROP& rprop, const char* file);

//read from file pointer

Model& read(Model& model, FILE* reader);
Data& read(Data& data, FILE* reader);
SGD& read(SGD& sdg, FILE* reader);
SDM& read(SDM& sdm, FILE* reader);
NAG& read(NAG& nag, FILE* reader);
ADAGRAD& read(ADAGRAD& adagrad, FILE* reader);
ADADELTA& read(ADADELTA& adadelta, FILE* reader);
RMSPROP& read(RMSPROP& rmsprop, FILE* reader);
ADAM& read(ADAM& adam, FILE* reader);
NADAM& read(NADAM& nadam, FILE* reader);
BFGS& read(BFGS& bfgs, FILE* reader);
RPROP& read(RPROP& rprop, FILE* reader);

//opterators - comparison

bool operator==(const SGD& obj1, const SGD& obj2);
bool operator==(const SDM& obj1, const SDM& obj2);
bool operator==(const NAG& obj1, const NAG& obj2);
bool operator==(const ADAGRAD& obj1, const ADAGRAD& obj2);
bool operator==(const ADADELTA& obj1, const ADADELTA& obj2);
bool operator==(const RMSPROP& obj1, const RMSPROP& obj2);
bool operator==(const ADAM& obj1, const ADAM& obj2);
bool operator==(const NADAM& obj1, const NADAM& obj2);
bool operator==(const BFGS& obj1, const BFGS& obj2);
bool operator==(const RPROP& obj1, const RPROP& obj2);

}

namespace serialize{

	//**********************************************
	// byte measures
	//**********************************************

	template <> int nbytes(const Opt::Data& obj);
	template <> int nbytes(const Opt::Model& obj);
	template <> int nbytes(const Opt::SGD& obj);
	template <> int nbytes(const Opt::SDM& obj);
	template <> int nbytes(const Opt::NAG& obj);
	template <> int nbytes(const Opt::ADAGRAD& obj);
	template <> int nbytes(const Opt::ADADELTA& obj);
	template <> int nbytes(const Opt::RMSPROP& obj);
	template <> int nbytes(const Opt::ADAM& obj);
	template <> int nbytes(const Opt::NADAM& obj);
	template <> int nbytes(const Opt::AMSGRAD& obj);
	template <> int nbytes(const Opt::BFGS& obj);
	template <> int nbytes(const Opt::RPROP& obj);
	template <> int nbytes(const Opt::CG& obj);

	//**********************************************
	// packing
	//**********************************************

	template <> int pack(const Opt::Data& obj, char* arr);
	template <> int pack(const Opt::Model& obj, char* arr);
	template <> int pack(const Opt::SGD& obj, char* arr);
	template <> int pack(const Opt::SDM& obj, char* arr);
	template <> int pack(const Opt::NAG& obj, char* arr);
	template <> int pack(const Opt::ADAGRAD& obj, char* arr);
	template <> int pack(const Opt::ADADELTA& obj, char* arr);
	template <> int pack(const Opt::RMSPROP& obj, char* arr);
	template <> int pack(const Opt::ADAM& obj, char* arr);
	template <> int pack(const Opt::NADAM& obj, char* arr);
	template <> int pack(const Opt::AMSGRAD& obj, char* arr);
	template <> int pack(const Opt::BFGS& obj, char* arr);
	template <> int pack(const Opt::RPROP& obj, char* arr);
	template <> int pack(const Opt::CG& obj, char* arr);

	//**********************************************
	// unpacking
	//**********************************************

	template <> int unpack(Opt::Data& obj, const char* arr);
	template <> int unpack(Opt::Model& obj, const char* arr);
	template <> int unpack(Opt::SGD& obj, const char* arr);
	template <> int unpack(Opt::SDM& obj, const char* arr);
	template <> int unpack(Opt::NAG& obj, const char* arr);
	template <> int unpack(Opt::ADAGRAD& obj, const char* arr);
	template <> int unpack(Opt::ADADELTA& obj, const char* arr);
	template <> int unpack(Opt::RMSPROP& obj, const char* arr);
	template <> int unpack(Opt::ADAM& obj, const char* arr);
	template <> int unpack(Opt::NADAM& obj, const char* arr);
	template <> int unpack(Opt::AMSGRAD& obj, const char* arr);
	template <> int unpack(Opt::BFGS& obj, const char* arr);
	template <> int unpack(Opt::RPROP& obj, const char* arr);
	template <> int unpack(Opt::CG& obj, const char* arr);

}

#endif


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * 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.
 ********************************************************************************/

#ifndef RW_MATH_VECTOR2D_HPP
#define RW_MATH_VECTOR2D_HPP

/**
 * @file Vector2D.hpp
 */

#if !defined(SWIG)
#include <rw/common/Serializable.hpp>

#include <Eigen/Core>
#endif

namespace rw { namespace math {
    /** @addtogroup math */
    /*@{*/

    /**
     * @brief A 2D vector @f$ \mathbf{v}\in \mathbb{R}^2 @f$
     *
     * @f$ \robabx{i}{j}{\mathbf{v}} = \left[
     *  \begin{array}{c}
     *  v_x \\
     *  v_y
     *  \end{array}
     *  \right]
     *  @f$
     *
     *  In addition, Vector2D supports the cross product operator:
     *  v3 = cross(v1, v2)
     *
     *  Usage example:
     *  @code
     *  using namespace rw::math;
     *
     *  Vector2D<> v1(1.0, 2.0);
     *  Vector2D<> v2(6.0, 7.0);
     *  Vector2D<> v3 = cross( v1, v2 );
     *  Vector2D<> v4 = v2 - v1;
     *  @endcode
     */
    template< class T = double > class Vector2D
    {
      public:
        //! Eigen based Vector2D
        typedef Eigen::Matrix< T, 2, 1 > EigenVector2D;

        //! Value type.
        typedef T value_type;

        /**
         * @brief Creates a 2D vector initialized with 0's
         */
        Vector2D ()
        {
            _vec[0] = 0;
            _vec[1] = 0;
        }

        /**
         * @brief Creates a 2D vector
         *
         * @param x [in] @f$ x @f$
         *
         * @param y [in] @f$ y @f$
         */
        Vector2D (T x, T y)
        {
            _vec[0] = x;
            _vec[1] = y;
        }

        /**
         * @brief Creates a 2D vector from Eigen Vector
         * @param r [in] an Eigen Vector
         */
        template< class R > Vector2D (const Eigen::MatrixBase< R >& r)
        {
            EigenVector2D v (r);
            _vec[0] = v (0);
            _vec[1] = v (1);
        }

        /**
         * @brief Copy Constructor
         */
        Vector2D (const Vector2D< T >& copy)
        {
            _vec[0] = copy[0];
            _vec[1] = copy[1];
        }

        /**
           @brief Returns Eigen vector equivalent to *this.
         */
        EigenVector2D e () const
        {
            EigenVector2D v;
            v (0) = _vec[0];
            v (1) = _vec[1];
            return v;
        }

        /**
           @brief The dimension of the vector (i.e. 2).

           This method is provided to help support generic algorithms using
           size() and operator[].
        */
        size_t size () const { return 2; }

        // Various operators.
#if !defined(SWIG)
        /**
         * @brief Returns reference to vector element
         *
         * @param i [in] index in the vector \f$i\in \{0,1\} \f$
         *
         * @return const reference to element
         */
        const T& operator() (size_t i) const { return _vec[i]; }

        /**
         * @brief Returns reference to vector element
         *
         * @param i [in] index in the vector \f$i\in \{0,1\} \f$
         *
         * @return reference to element
         */
        T& operator() (size_t i) { return _vec[i]; }

        /**
         * @brief Returns reference to vector element
         * @param i [in] index in the vector \f$i\in \{0,1,2\} \f$
         * @return const reference to element
         */
        const T& operator[] (size_t i) const { return _vec[i]; }

        /**
         * @brief Returns reference to vector element
         * @param i [in] index in the vector \f$i\in \{0,1,2\} \f$
         * @return reference to element
         */
        T& operator[] (size_t i) { return _vec[i]; }
#else
        ARRAYOPERATOR (T);
#endif

        /**
           @brief Scalar division.
         */
        const Vector2D< T > operator/ (T s) const
        {
            return Vector2D< T > ((*this)[0] / s, (*this)[1] / s);
        }

        /**
           @brief Scalar multiplication.
         */
        const Vector2D< T > operator* (T s) const
        {
            return Vector2D< T > ((*this)[0] * s, (*this)[1] * s);
        }
#if !defined(SWIGPYTHON)
        /**
           @brief Scalar multiplication.
         */
        friend const Vector2D< T > operator* (T s, const Vector2D< T >& v)
        {
            return Vector2D< T > (s * v[0], s * v[1]);
        }
#endif

        /**
           @brief Vector subtraction.
         */
        const Vector2D< T > operator- (const Vector2D< T >& b) const
        {
            return Vector2D< T > ((*this) (0) - b (0), (*this) (1) - b (1));
        }

        /**
           @brief Vector addition.
         */
        const Vector2D< T > operator+ (const Vector2D< T >& b) const
        {
            return Vector2D< T > ((*this) (0) + b (0), (*this) (1) + b (1));
        }

        /**
           @brief Scalar multiplication.
         */
        Vector2D< T >& operator*= (T s)
        {
            _vec[0] *= s;
            _vec[1] *= s;
            return *this;
        }

        /**
           @brief Scalar division.
         */
        Vector2D< T >& operator/= (T s)
        {
            _vec[0] /= s;
            _vec[1] /= s;
            return *this;
        }

        /**
           @brief Vector addition.
         */
        Vector2D< T >& operator+= (const Vector2D< T >& v)
        {
            _vec[0] += v (0);
            _vec[1] += v (1);
            return *this;
        }

        /**
           @brief Vector subtraction.
         */
        Vector2D< T >& operator-= (const Vector2D< T >& v)
        {
            _vec[0] -= v (0);
            _vec[1] -= v (1);
            return *this;
        }

        /**
           @brief Unary minus.
         */
        const Vector2D< T > operator- () const { return Vector2D< T > (-_vec[0], -_vec[1]); }

        /**
         * @brief Compares \b a and \b b for equality.
         * @param b [in]
         * @return True if a equals b, false otherwise.
         */
        bool operator== (const Vector2D< T >& b) const { return _vec[0] == b[0] && _vec[1] == b[1]; }

        /**
         *  @brief Compares \b a and \b b for inequality.
         * @param b [in]
         * @return True if a and b are different, false otherwise.
         */
        bool operator!= (const Vector2D< T >& b) const { return !(*this == b); }

        /**
         * @brief returns the counter clock-wise angle between
         * this vector and the x-axis vector (1,0). The angle
         * returned will be in the interval [-Pi,Pi]
         */
        double angle () { return atan2 (_vec[1], _vec[0]); }
#if !defined(SWIG)
        /**
           @brief Streaming operator.
         */
        friend std::ostream& operator<< (std::ostream& out, const Vector2D< T >& v)
        {
            return out << "Vector2D {" << v[0] << ", " << v[1] << "}";
        }
#else
        TOSTRING (rw::math::Vector2D< T >);
#endif

        /**
         * @brief Returns the Euclidean norm (2-norm) of the vector
         * @return the norm
         */
        T norm2 () const { return sqrt (_vec[0] * _vec[0] + _vec[1] * _vec[1]); }

        /**
         * @brief Returns the Manhatten norm (1-norm) of the vector
         * @return the norm
         */
        T norm1 () const { return fabs (_vec[0]) + fabs (_vec[1]); }

        /**
         * @brief Returns the infinte norm (\f$\inf\f$-norm) of the vector
         * @return the norm
         */
        T normInf () const
        {
            T res      = fabs (_vec[0]);
            const T f1 = fabs (_vec[1]);
            if (f1 > res)
                res = f1;
            return res;
        }

      private:
        T _vec[2];
    };

    /**
     * @brief Calculates the 2D vector cross product @f$ \mathbf{v1} \times \mathbf{v2} @f$
     *
     * @param v1 [in] @f$ \mathbf{v1} @f$
     *
     * @param v2 [in] @f$ \mathbf{v2} @f$
     *
     * @return the cross product @f$ \mathbf{v1} \times \mathbf{v2} @f$
     *
     * The 2D vector cross product is defined as:
     *
     * @f$
     * \mathbf{v1} \times \mathbf{v2} =  v1_x * v2_y - v1_y * v2_x
     * @f$
     */
    template< class T > T cross (const Vector2D< T >& v1, const Vector2D< T >& v2)
    {
        return v1 (0) * v2 (1) - v1 (1) * v2 (0);
    }

    /**
     * @brief Calculates the dot product @f$ \mathbf{v1} . \mathbf{v2} @f$
     * @param v1 [in] @f$ \mathbf{v1} @f$
     * @param v2 [in] @f$ \mathbf{v2} @f$
     *
     * @return the dot product @f$ \mathbf{v1} . \mathbf{v2} @f$
     */
    template< class T > double dot (const Vector2D< T >& v1, const Vector2D< T >& v2)
    {
        return v1 (0) * v2 (0) + v1 (1) * v2 (1);
    }

    /**
     * @brief calculates the counter clock-wise angle from v1 to
     * v2. the value returned will be in the interval [-2Pi,2Pi]
     */
    template< class T > double angle (const Vector2D< T >& v1, const Vector2D< T >& v2)
    {
        return atan2 (v2 (1), v2 (0)) - atan2 (v1 (1), v1 (0));
    }

    /**
     * @brief Returns the normalized vector
     * \f$\mathbf{n}=\frac{\mathbf{v}}{\|\mathbf{v}\|} \f$.
     *
     * If \f$ \| \mathbf{v} \| = 0\f$ then the zero vector is returned.
     *
     * @param v [in] \f$ \mathbf{v} \f$ which should be normalized
     *
     * @return the normalized vector \f$ \mathbf{n} \f$
     */
    template< class T > const Vector2D< T > normalize (const Vector2D< T >& v)
    {
        T length = v.norm2 ();
        if (length != 0)
            return Vector2D< T > (v (0) / length, v (1) / length);
        else
            return Vector2D< T > (0, 0);
    }

    /**
     * @brief Casts Vector2D<T> to Vector2D<Q>
     *
     * @param v [in] Vector2D with type T
     *
     * @return Vector2D with type Q
     */
    template< class Q, class T > const Vector2D< Q > cast (const Vector2D< T >& v)
    {
        return Vector2D< Q > (static_cast< Q > (v (0)), static_cast< Q > (v (1)));
    }

#if !defined(SWIG)
    extern template class rw::math::Vector2D< double >;
    extern template class rw::math::Vector2D< float >;
#else
    SWIG_DECLARE_TEMPLATE (Vector2Dd, rw::math::Vector2D< double >);
    SWIG_DECLARE_TEMPLATE (Vector2Df, rw::math::Vector2D< float >);
#endif
    using Vector2Dd = Vector2D< double >;
    using Vector2Df = Vector2D< float >;

    /**@}*/
}}    // namespace rw::math

namespace rw { namespace common {
    class OutputArchive;
    class InputArchive;
    namespace serialization {
        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Vector2D
         */
        template<>
        void write (const rw::math::Vector2D< double >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Vector2D
         */
        template<>
        void write (const rw::math::Vector2D< float >& sobject, rw::common::OutputArchive& oarchive,
                    const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Vector2D
         */
        template<>
        void read (rw::math::Vector2D< double >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Vector2D
         */
        template<>
        void read (rw::math::Vector2D< float >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);
    }    // namespace serialization
}}       // namespace rw::common

namespace boost { namespace serialization {
    /**
     * @brief Boost serialization.
     * @param archive [in] the boost archive to read from or write to.
     * @param vector [in/out] the vector to read/write.
     * @param version [in] class version (currently version 0).
     * @relatedalso rw::math::Vector2D
     */
    template< class Archive, class T >
    void serialize (Archive& archive, rw::math::Vector2D< T >& vector, const unsigned int version)
    {
        archive& vector[0];
        archive& vector[1];
    }
}}    // namespace boost::serialization

#endif    // end include guard


#include <iostream>
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;
int main()
{
  Matrix3d A = Matrix3d::Random();
  A = (A + Matrix3d::Constant(1.2)) * 50;
  cout << "A =" << endl << A << endl;
  Vector3d v(1,2,3);
  cout << "A * v =" << endl << A * v << endl;
  cout << " (A * v)^T =" << endl << (A * v).transpose() << endl;
}


/*
    hg.cpp -- Henyey-Greenstein model evaluation routines

    Copyright (c) 2015 Wenzel Jakob <wenzel@inf.ethz.ch>

    All rights reserved. Use of this source code is governed by a
    BSD-style license that can be found in the LICENSE file.
*/

#include <layer/microfacet.h>
#include <layer/frame.h>
#include <layer/math.h>
#include <layer/fourier.h>
#include <layer/fresnel.h>
#include <Eigen/SVD>
#include <numeric>

NAMESPACE_BEGIN(layer)

static int expcosCoefficientCount(Float B, Float relerr) {
    Float prod = 1, invB = 1.0f / B;
    if (B == 0)
        return 1;

    for (int i=0; ; ++i) {
        prod /= 1 + i * invB;

        if (prod < relerr)
            return i+1;
    }
}

static Float modBesselRatio(Float B, Float k) {
    const Float eps = std::numeric_limits<Float>::epsilon(),
                invTwoB = 2.0f / B;

    Float i = (Float) k,
           D = 1 / (invTwoB * i++),
           Cd = D, C = Cd;

    while (std::abs(Cd) > eps * std::abs(C)) {
        Float coeff = invTwoB * i++;
        D = 1 / (D + coeff);
        Cd *= coeff*D - 1;
        C += Cd;
    }

    return C;
}

void expCosFourierSeries(Float A, Float B, Float relerr, std::vector<Float> &coeffs) {
    /* Determine the required number of coefficients and allocate memory */
    int n = expcosCoefficientCount(B, relerr);
    coeffs.resize(n);

    /* Determine the last ratio and work downwards */
    coeffs[n-1] = modBesselRatio(B, n - 1);
    for (int i=n-2; i>0; --i)
        coeffs[i] = B / (2*i + B*coeffs[i+1]);

    /* Evaluate the exponentially scaled I0 and correct scaling */
    coeffs[0] = math::i0e(B) * std::exp(A+B);

    /* Apply the ratios & factor of two upwards */
    Float prod = 2*coeffs[0];
    for (int i=1; i<n; ++i) {
        prod *= coeffs[i];
        if (std::abs(prod) < coeffs[0] * relerr) {
            coeffs.erase(coeffs.begin() + i, coeffs.end());
            break;
        }
        coeffs[i] = prod;
    }
}

Float smithG1(const Vector3 &v, const Vector3 &m, Float alpha) {
    const Float tanTheta = std::abs(Frame::tanTheta(v));

    /* Can't see the back side from the front and vice versa */
    if (v.dot(m) * Frame::cosTheta(v) <= 0)
        return 0.0f;

    Float a = 1.0f / (alpha * tanTheta);
    if (a < 1.6f) {
        /* Use a fast and accurate (<0.35% rel. error) rational
           approximation to the shadowing-masking function */
        const Float aSqr = a * a;
        return (3.535f * a + 2.181f * aSqr)
             / (1.0f + 2.276f * a + 2.577f * aSqr);
    }

    return 1.f;
}

Float microfacet(Float mu_o, Float mu_i, std::complex<Float> eta_,
                 Float alpha, Float phi_d) {
    Float sinThetaI = math::safe_sqrt(1-mu_i*mu_i),
          sinThetaO = math::safe_sqrt(1-mu_o*mu_o),
          cosPhi = std::cos(phi_d),
          sinPhi = std::sin(phi_d);

    Vector wi(-sinThetaI, 0, -mu_i);
    Vector wo(sinThetaO*cosPhi, sinThetaO*sinPhi, mu_o);
    bool reflect = -mu_i*mu_o > 0;

    if (mu_o == 0 || mu_i == 0)
        return 0.f;

    bool conductor = eta_.imag() != 0.0f;
    if (conductor && !reflect)
        return 0.0f;
    std::complex<Float> eta =
        (-mu_i > 0 || conductor) ? eta_ : std::complex<Float>(1) / eta_;

    Vector H = (wi + wo * (reflect ? 1.0f : eta.real())).normalized();
    H *= math::signum(Frame::cosTheta(H));

    Float cosThetaH2 = Frame::cosTheta2(H),
          exponent = -Frame::tanTheta2(H) / (alpha*alpha),
          D = std::exp(exponent) / (math::Pi * alpha*alpha * cosThetaH2*cosThetaH2),
          F = !conductor ? fresnelDielectric(wi.dot(H), eta_.real())
                         : fresnelConductor(std::abs(wi.dot(H)), eta),
          G = smithG1(wi, H, alpha) * smithG1(wo, H, alpha);

    if (reflect) {
        return F * D * G / (4.0f * std::abs(mu_i*mu_o));
    } else {
        Float sqrtDenom = wi.dot(H) + eta.real() * wo.dot(H);

        return std::abs(((1 - F) * D * G * eta.real() * eta.real() * wi.dot(H)
            * wo.dot(H)) / (mu_i*mu_o * sqrtDenom * sqrtDenom));
    }
}

Float microfacetNoExp(Float mu_o, Float mu_i, std::complex<Float> eta_,
                      Float alpha, Float phi_d) {
    Float sinThetaI = math::safe_sqrt(1-mu_i*mu_i),
          sinThetaO = math::safe_sqrt(1-mu_o*mu_o),
          cosPhi = std::cos(phi_d),
          sinPhi = std::sin(phi_d);

    Vector wi(-sinThetaI, 0, -mu_i);
    Vector wo(sinThetaO*cosPhi, sinThetaO*sinPhi, mu_o);
    bool reflect = -mu_i*mu_o > 0;

    if (mu_o == 0 || mu_i == 0)
        return 0.f;

    bool conductor = eta_.imag() != 0.0f;
    if (conductor && !reflect)
        return 0.0f;
    std::complex<Float> eta =
        (-mu_i > 0 || conductor) ? eta_ : std::complex<Float>(1) / eta_;

    Vector H = (wi + wo * (reflect ? 1.0f : eta.real())).normalized();
    H *= math::signum(Frame::cosTheta(H));

    Float cosThetaH2 = Frame::cosTheta2(H),
          D = (Float) 1 / (math::Pi * alpha*alpha * cosThetaH2*cosThetaH2),
          F = !conductor ? fresnelDielectric(wi.dot(H), eta_.real())
                         : fresnelConductor(std::abs(wi.dot(H)), eta),
          G = smithG1(wi, H, alpha) * smithG1(wo, H, alpha);

    if (reflect) {
        return F * D * G / (4.0f * std::abs(mu_i*mu_o));
    } else {
        Float sqrtDenom = wi.dot(H) + eta.real() * wo.dot(H);

        return std::abs(((1 - F) * D * G * eta.real() * eta.real() * wi.dot(H)
            * wo.dot(H)) / (mu_i*mu_o * sqrtDenom * sqrtDenom));
    }
}

static Float Bmax(size_t n, Float relerr) {
    if (relerr >= 1e-1f)
        return 0.1662f*std::pow((Float) n, (Float) 2.05039);
    else if (relerr >= 1e-2f)
        return 0.0818f*std::pow((Float) n, (Float) 2.04982);
    else if (relerr >= 1e-3f)
        return 0.0538f*std::pow((Float) n, (Float) 2.05001);
    else if (relerr >= 1e-4f)
        return 0.0406f*std::pow((Float) n, (Float) 2.04686);
    else if (relerr >= 1e-5f)
        return 0.0337f*std::pow((Float) n, (Float) 2.03865);
    else if (relerr >= 1e-6f)
        return 0.0299f*std::pow((Float) n, (Float) 2.02628);
    else
        throw std::runtime_error("Bmax(): unknown relative error bound!");
}

void microfacetNoExpFourierSeries(Float mu_o, Float mu_i, std::complex<Float> eta_,
                                  Float alpha, size_t n, Float phiMax,
                                  std::vector<Float> &result) {

    bool reflect = -mu_i * mu_o > 0;

    Float sinMu2 = math::safe_sqrt((1 - mu_i * mu_i) * (1 - mu_o * mu_o)),
          phiCritical = 0.0f;

    bool conductor = (eta_.imag() != 0.0f);
    std::complex<Float> eta =
        (-mu_i > 0 || conductor) ? eta_ : std::complex<Float>(1) / eta_;

    if (reflect) {
        if (!conductor)
            phiCritical = math::safe_acos((2*eta.real()*eta.real()-mu_i*mu_o-1)/sinMu2);
    } else if (!reflect) {
        if (conductor)
            throw std::runtime_error("lowfreqFourierSeries(): encountered refraction case for a conductor");
        Float etaDenser = (eta.real() > 1 ? eta.real() : 1 / eta.real());
        phiCritical = math::safe_acos((1 - etaDenser * mu_i * mu_o) /
                                      (etaDenser * sinMu2));
    }

    if (!conductor && phiCritical > math::Epsilon &&
        phiCritical < math::Pi - math::Epsilon &&
        phiCritical < phiMax - math::Epsilon) {
        /* Uh oh, some high frequency content leaked in the generally low frequency part.
           Increase the number of coefficients so that we can capture it. Fortunately, this
           happens very rarely. */
        n = std::max(n, (size_t) 100);
    }

    VectorX coeffs(n);
    coeffs.setZero();
    std::function<Float(Float)> integrand = std::bind(
        &microfacetNoExp, mu_o, mu_i, eta_, alpha, std::placeholders::_1);

    const int nEvals = 200;
    if (reflect) {
        if (phiCritical > math::Epsilon && phiCritical < phiMax-math::Epsilon) {
            filonIntegrate(integrand, coeffs.data(), n, nEvals, 0, phiCritical);
            filonIntegrate(integrand, coeffs.data(), n, nEvals, phiCritical, phiMax);
        } else {
            filonIntegrate(integrand, coeffs.data(), n, nEvals, 0, phiMax);
        }
    } else {
        filonIntegrate(integrand, coeffs.data(), n, nEvals, 0,
                       std::min(phiCritical, phiMax));
    }

    if (phiMax < math::Pi - math::Epsilon) {
        /* Precompute some sines and cosines */
        VectorX cosPhi(n), sinPhi(n);
        for (size_t i=0; i<n; ++i) {
            sinPhi[i] = std::sin(i*phiMax);
            cosPhi[i] = std::cos(i*phiMax);
        }

        /* The fit only occurs on a subset [0, phiMax], where the Fourier
           Fourier basis functions are not orthogonal anymore! The following
           then does a change of basis to proper Fourier coefficients. */
        MatrixX A(n, n);

        for (MatrixX::Index i=0; i < (MatrixX::Index) n; ++i) {
            for (MatrixX::Index j=0; j <= (MatrixX::Index) i; ++j) {
                if (i != j) {
                    A(i, j) = A(j, i) = (i * cosPhi[j] * sinPhi[i] -
                                         j * cosPhi[i] * sinPhi[j]) /
                                        (i * i - j * j);
                } else if (i != 0) {
                    A(i, i) = (std::sin(2 * i * phiMax) + 2 * i * phiMax) / (4 * i);
                } else {
                    A(i, i) = phiMax;
                }
            }
        }

        auto svd = A.bdcSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
        const MatrixX &U = svd.matrixU();
        const MatrixX &V = svd.matrixV();
        const VectorX &sigma = svd.singularValues();

        if (sigma[0] == 0) {
            result.clear();
            result.push_back(0);
            return;
        }

        VectorX temp = VectorX::Zero(n);
        coeffs[0] *= math::Pi;
        coeffs.tail(n-1) *= 0.5 * math::Pi;
        for (size_t i=0; i<n; ++i) {
            if (sigma[i] < 1e-9f * sigma[0])
                break;
            temp += V.col(i) * U.col(i).dot(coeffs) / sigma[i];
        }
        coeffs = temp;
    }

    result.resize(coeffs.size());
    memcpy(result.data(), coeffs.data(), sizeof(Float) * coeffs.size());
}

void microfacetFourierSeries(Float mu_o, Float mu_i, std::complex<Float> eta_,
                             Float alpha, size_t n, Float relerr,
                             std::vector<Float> &result) {
    bool reflect = -mu_i * mu_o > 0;

    /* Compute the 'A' and 'B' constants, as well as the critical azimuth */
    Float A, B;
    Float sinMu2 = math::safe_sqrt((1 - mu_i * mu_i) * (1 - mu_o * mu_o));

    bool conductor = (eta_.imag() != 0.0f);
    std::complex<Float> eta =
        (-mu_i > 0 || conductor) ? eta_ : std::complex<Float>(1) / eta_;

    if (reflect) {
        Float temp = 1.0f / (alpha * (mu_i - mu_o));
        A = (mu_i * mu_i + mu_o * mu_o - 2) * temp * temp;
        B = 2 * sinMu2 * temp * temp;
    } else {
        if (conductor) {
            /* No refraction in conductors */
            result.clear();
            result.push_back(0.0f);
            return;
        } else {
            Float temp = 1.0f / (alpha * (mu_i - eta.real() * mu_o));
            A = (mu_i * mu_i - 1 + eta.real() * eta.real() * (mu_o * mu_o - 1)) * temp * temp;
            B = 2 * eta.real() * sinMu2 * temp * temp;
        }
    }

    /* Minor optimization: don't even bother computing the Fourier series
       if the contribution to the scattering model is miniscule */
    if (math::i0e(B) * std::exp(A+B) < 1e-10) {
        result.clear();
        result.push_back(0.0f);
        return;
    }

    Float B_max = Bmax(n, relerr);
    if (B > B_max) {
        A = A + B - B_max + std::log(math::i0e(B) / math::i0e(B_max));
        B = B_max;
    }

    std::vector<Float> lowfreq_coeffs, expcos_coeffs;

    /* Compute Fourier coefficients of the exponential term */
    expCosFourierSeries(A, B, relerr, expcos_coeffs);

    /* Compute Fourier coefficients of the low-frequency term
       Only fit in the region where the result is actually
       going to make some sort of difference given the convolution
       with expcos_coeffs */
    Float phiMax = math::safe_acos(1 + std::log(relerr) / B);

    microfacetNoExpFourierSeries(mu_o, mu_i, eta_, alpha,
                                 12, phiMax, lowfreq_coeffs);

    /* Perform discrete circular convolution of the two series */
    result.resize(lowfreq_coeffs.size() + expcos_coeffs.size() - 1);

    convolveFourier(lowfreq_coeffs.data(), lowfreq_coeffs.size(),
        expcos_coeffs.data(), expcos_coeffs.size(), result.data());

    /* Truncate the series if error bounds are satisfied */
    for (size_t i=0; i<result.size(); ++i) {
        assert(std::isfinite(result[i]));
        if (result[i] == 0 || std::abs(result[i]) < result[0] * relerr) {
            result.erase(result.begin() + i, result.end());
            break;
        }
    }
}

NAMESPACE_END(layer)


#include <armadillo>

#include <glm/lm.hpp>
#include <glm/irls.hpp>

using namespace arma;

double loglikelihood(const vec &residuals, double sigma_square, double n)
{
    return -n/2*log(2*datum::pi) - n/2*log( sigma_square ) - 1/(2*sigma_square) * accu( residuals % residuals );
}

vec
lm(const mat &X, const vec &y, const uvec &missing, const glm_model &model, glm_info &output)
{
    vec w = ones<vec>( y.n_elem );
    set_missing_to_zero( missing, w );
    vec beta = weighted_least_squares( X, y, w );

    double n = accu( w );
    double k = X.n_cols;

    vec mu = X * beta;
    vec residuals = y - mu;
    double sigma_square = as_scalar( trans( residuals ) * ( w % residuals ) / ( n - k ) );

    mat cov = trans( X ) * ( diagmat( w ) * X );
    mat cov_inv;
    if( !inv( cov_inv, cov ) )
    {
        output.success = false;
        return beta;
    }

    vec sd = arma::sqrt( sigma_square * diagvec( cov_inv ) );

    output.se_beta = sd;
    output.p_value = 1 - chi_square_cdf( beta % beta / ( sd % sd ), 1 );
    output.mu = mu;
    output.logl = loglikelihood( residuals % w, sigma_square, n );
    output.success = true;
    output.converged = true;

    return beta;
}


#include <fstream>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <fmt/format.h>
#include <string>
#include <iomanip>
#include <boost/variant.hpp>
#include <unordered_map>
#include <cstdint>
#include <vector>
#include <range/v3/algorithm.hpp>
using gint = std::int64_t;
using Coordinate = std::complex<gint>;
enum Command {
  TurnRight,
  TurnLeft,
  TurnUp,
  TurnDown
};
class Car
{
  std::complex<gint> pos_{ 1, 1 };
  std::complex<gint> turn_[4]{
    { 1, 0 },
    { -1, 0 },
    { 0, -1 },
    { 0, 1 }
  };
  char map_[5][5] = {
    '0',
    '0',
    '1',
    '0',
    '0',
    '0',
    '2',
    '3',
    '4',
    '0',
    '5',
    '6',
    '7',
    '8',
    '9',
    '0',
    'A',
    'B',
    'C',
    '0',
    '0',
    '0',
    'D',
    '0',
    '0'
  };
  std::vector<char> values_;
  bool validpos(Coordinate pos) const
  {
    int i = pos.imag();
    int j = pos.real();
    return (0 <= pos.real() && pos.real() < 5 && 0 <= pos.imag() && pos.imag() < 5)
           && (i + j) >= 2 && (i + j <= 6)
           && (j - i) >= -2 && (j - i <= 2);
  }

  void process(Command command) noexcept
  {
    auto newpos = pos_ + turn_[command];
    if (validpos(newpos)) {
      pos_ = newpos;
    }
  }

public:
  void process(std::vector<Command> const &commands) noexcept
  {
    for (auto command : commands) {
      process(command);
    }
    values_.push_back(map_[pos_.imag()][pos_.real()]);
  }
  auto codes() const
  {
    return values_;
  }
};

int main(int argc, char **argv)
{
  if (argc > 1) {
    std::ifstream ifs(argv[1]);
    char c;
    int forward;
    Car car;
    std::string s;
    while (std::getline(ifs, s)) {
      std::vector<Command> coms;
      ranges::transform(s, std::back_inserter(coms), [](char c) {
        Command com[256];
        com['L'] = Command::TurnLeft;
        com['R'] = Command::TurnRight;
        com['U'] = Command::TurnUp;
        com['D'] = Command::TurnDown;
        return com[c];
      });
      car.process(coms);
    }
    for (auto n : car.codes()) {
      fmt::print("{},", n);
    }
    fmt::print("\n");
  }
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_TWOTO31_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_TWOTO31_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate  2 to the power 31 (\f$2^{31}\f$)

    @par Semantic:

    @code
    T r = Twoto31<T>();
    @endcode

    is similar to:

    @code
    T r = pow(2, 31);
    @endcode

    @return The Twoto31 constant for the proper type
  **/
  template<typename T> T Twoto31();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generate the  constant twoto31.

      @return The Twoto31 constant for the proper type
    **/
    const boost::dispatch::functor<tag::twoto31_> twoto31 = {};
  }
} }
#endif

#include <boost/simd/constant/definition/twoto31.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


/*-----------------------------------------------------------------------------+
Copyright (c) 2018-2018: Hagen Pache
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#pragma once

#include <boost/test/unit_test.hpp>

#include <dil/pi/FunctionsMap.hpp>

namespace dil {
namespace pi {

template <typename T>
class TestHelper : public FunctionsMap<T>
{
     public:
     void test_exactly_relation(    const Relations& relation_in,
                                    const T& lhs,
                                    const Interval<T>& rhs)
     {
        for( auto const& [rel, fun] : this->_function_map )
        {
            if (relation_in == rel)
                BOOST_TEST(fun(lhs, rhs) == true);
            else
                BOOST_TEST( fun(lhs, rhs) == false,
                            "expected " << relation_in <<
                            " but also " << rel <<
                            " with (lhs " << lhs << ", rhs " << rhs <<") got true");
        }

     }
};

}
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testSparseMatrix.cpp
 * @author  Mandy Xie
 * @author  Fan Jiang
 * @author  Gerry Chen
 * @author  Frank Dellaert
 * @date    Jan, 2021
 */

#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SparseEigen.h>

#include <boost/assign/list_of.hpp>
using boost::assign::list_of;

#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>

using namespace std;
using namespace gtsam;

/* ************************************************************************* */
TEST(SparseEigen, sparseJacobianEigen) {
  GaussianFactorGraph gfg;
  SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5);
  const Key x123 = 0, x45 = 1;
  gfg.add(x123, (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished(),
          Vector2(4, 8), model);
  gfg.add(x123, (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished(),
          x45,  (Matrix(2, 2) << 11, 12, 14, 15.).finished(),
          Vector2(13, 16), model);

  // Sparse Matrix
  auto sparseResult = sparseJacobianEigen(gfg);
  EXPECT_LONGS_EQUAL(16, sparseResult.nonZeros());
  EXPECT(assert_equal(4, sparseResult.rows()));
  EXPECT(assert_equal(6, sparseResult.cols()));
  EXPECT(assert_equal(gfg.augmentedJacobian(), Matrix(sparseResult)));

  // Call sparseJacobian with optional ordering...
  auto ordering = Ordering(list_of(x45)(x123));

  // Eigen Sparse with optional ordering
  EXPECT(assert_equal(gfg.augmentedJacobian(ordering),
                      Matrix(sparseJacobianEigen(gfg, ordering))));

  // Check matrix dimensions when zero rows / cols
  gfg.add(x123, Matrix23::Zero(), Vector2::Zero(), model);  // zero row
  gfg.add(2, Matrix21::Zero(), Vector2::Zero(), model);     // zero col
  sparseResult = sparseJacobianEigen(gfg);
  EXPECT_LONGS_EQUAL(16, sparseResult.nonZeros());
  EXPECT(assert_equal(8, sparseResult.rows()));
  EXPECT(assert_equal(7, sparseResult.cols()));
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


// BLAS level 3
// hermitian matrices, herk

#include <stddef.h>
#include <iostream>
#include <boost/numeric/bindings/atlas/cblas3.hpp>
#include <boost/numeric/bindings/traits/ublas_hermitian.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace atlas = boost::numeric::bindings::atlas;
namespace traits = boost::numeric::bindings::traits;

using std::cout;
using std::cin;
using std::endl;

typedef double real_t;
typedef std::complex<real_t> cmplx_t;

typedef ublas::matrix<cmplx_t, ublas::column_major> cm_t;
typedef ublas::matrix<cmplx_t, ublas::row_major> rm_t;
typedef ublas::hermitian_adaptor<cm_t, ublas::upper> ucha_t;
typedef ublas::hermitian_adaptor<cm_t, ublas::lower> lcha_t;
typedef ublas::hermitian_adaptor<rm_t, ublas::upper> urha_t;
typedef ublas::hermitian_adaptor<rm_t, ublas::lower> lrha_t;

#define N 3
#define K 4

int main() {

  cm_t ac (N, K);
  rm_t ar (N, K);

  ac(0,0) = ar(0,0) = cmplx_t (1., 1.);
  ac(1,0) = ar(1,0) = cmplx_t (2., 1.);
  ac(2,0) = ar(2,0) = cmplx_t (3., 1.);
#if (K > 1)
  ac(0,1) = ar(0,1) = cmplx_t (1., 1.);
  ac(1,1) = ar(1,1) = cmplx_t (2., 1.);
  ac(2,1) = ar(2,1) = cmplx_t (3., 1.);
#if (K > 2)
  ac(0,2) = ar(0,2) = cmplx_t (1., 1.);
  ac(1,2) = ar(1,2) = cmplx_t (2., 1.);
  ac(2,2) = ar(2,2) = cmplx_t (3., 1.);
#if (K > 3)
  ac(0,3) = ar(0,3) = cmplx_t (1., 1.);
  ac(1,3) = ar(1,3) = cmplx_t (2., 1.);
  ac(2,3) = ar(2,3) = cmplx_t (3., 1.);
#endif
#endif
#endif

  print_m (ac, "ac");
  cout << endl;
  print_m (ar, "ar");
  cout << endl << endl;

  cm_t cmu (N, N);
  cm_t cml (N, N);
  rm_t rmu (N, N);
  rm_t rml (N, N);
  ucha_t ucha (cmu);
  lcha_t lcha (cml);
  urha_t urha (rmu);
  lrha_t lrha (rml);

  atlas::herk (CblasNoTrans, ac, ucha);
  atlas::herk (CblasNoTrans, 1.0, ac, 0.0, lcha);
  atlas::herk (CblasNoTrans, 1.0, ar, 0.0, urha);
  atlas::herk (CblasNoTrans, ar, lrha);

  print_m (ucha, "ucha");
  cout << endl;
  print_m (lcha, "lcha");
  cout << endl;
  print_m (urha, "urha");
  cout << endl;
  print_m (lrha, "lrha");
  cout << endl << endl;

  // part 2

  cm_t act (ublas::herm (ac));
  rm_t art (ublas::herm (ar));
  print_m (act, "act");
  cout << endl;
  print_m (art, "art");
  cout << endl << endl;

  init_m (cmu, const_val<cmplx_t> (cmplx_t (0, 0)));
  init_m (cml, const_val<cmplx_t> (cmplx_t (0, 0)));
  init_m (rmu, const_val<cmplx_t> (cmplx_t (0, 0)));
  init_m (rml, const_val<cmplx_t> (cmplx_t (0, 0)));

  atlas::herk (CblasUpper, CblasConjTrans, 1.0, act, 0.0, cmu);
  atlas::herk (CblasLower, CblasConjTrans, 1.0, act, 0.0, cml);
  atlas::herk (CblasUpper, CblasConjTrans, 1.0, art, 0.0, rmu);
  atlas::herk (CblasLower, CblasConjTrans, 1.0, art, 0.0, rml);

  print_m (cmu, "cmu");
  cout << endl;
  print_m (cml, "cml");
  cout << endl;
  print_m (rmu, "rmu");
  cout << endl;
  print_m (rml, "rml");
  cout << endl;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_DIVROUND2EVEN_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DIVROUND2EVEN_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing divround2even capabilities

    Computes the round2even of the division.

    @par semantic:
    For any given value @c x,  @c y of type @c T:

    @code
    T r = divround2even(x, y);
    @endcode

    The code is similar to:

    @code
    T r = round2even(x/y);
    @endcode

    for integral types, if y is null, it returns @ref Valmax or @ref Valmin
    if x is positive (resp. negative) and 0 if x is null.
    Take also care that dividing @ref Valmin by -1 for signed integral types has
    undefined behaviour.

    @see  divides, rec, divs, divfloor, divceil, divround, divfix

  **/
  const boost::dispatch::functor<tag::divround2even_> divround2even = {};
} }
#endif

#include <boost/simd/function/scalar/divround2even.hpp>
#include <boost/simd/function/simd/divround2even.hpp>

#endif


#pragma once

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

#include "mpfr/import_std_math.hpp"

#include <boost/math/policies/error_handling.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/laguerre.hpp>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/tools/config.hpp>

namespace boost {
namespace math {

// forward declarations
template <class T, class Policy>
typename tools::promote_args<T>::type
laguerren(unsigned n, unsigned m, T x, const Policy& pol);

template <class T1, class T2>
typename laguerre_result<T1, T2>::type
laguerren(unsigned n, T1 m, T2 x);

namespace detail {

template <class T>
inline typename tools::promote_args<T>::type
laguerren(unsigned n, unsigned m, T x, const mpl::false_&)
{
  return boost::math::laguerren(n, m, x, policies::policy<>());
}

}  // namespace detail

// Recurrence for associated polynomials:
template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type
laguerren_next(unsigned n, unsigned l, T1 x, T2 Pl, T3 Plm1)
{
  typedef typename tools::promote_args<T1, T2, T3>::type result_type;

  result_type nn = n;
  result_type mm = l;
  return (Pl * (mm + 2 * nn - x + 1) / ::math::sqrt((nn + 1) * (mm + nn + 1)) -
          Plm1 * nn * (nn + mm) / ::math::sqrt(nn * (nn + 1) * (nn + mm) * (mm + nn + 1)));
}

namespace detail {
// Laguerre Associated Polynomial:
template <class T, class Policy>
T
laguerren_imp(unsigned n, unsigned m, T x, const Policy& pol)
{
  // Special cases:
  if (m == 0) return boost::math::laguerre(n, x, pol);

  // normalization
  T a = 1;
  for (unsigned int i = m + n; i > n; --i) {
    a *= i;
  }

  T p0 = 1 / ::math::sqrt(a);

  if (n == 0) return p0;

  T p1 = (m + 1 - x) / ::math::sqrt(a);

  unsigned c = 1;

  while (c < n) {
    std::swap(p0, p1);
    p1 = laguerren_next(c, m, x, p0, p1);
    ++c;
  }
  return p1;
}
}

template <class T, class Policy>
inline typename tools::promote_args<T>::type
laguerren(unsigned n, unsigned m, T x, const Policy& pol)
{
  typedef typename tools::promote_args<T>::type result_type;
  typedef typename policies::evaluation<result_type, Policy>::type value_type;
  // return policies::checked_narrowing_cast<result_type, Policy>(detail::laguerren_imp(n, m,
  // static_cast<value_type>(x), pol),
  //                                                              "boost::math::laguerren<%1%>(unsigned,
  //                                                              unsigned,
  //                                                              %1%)");

  return detail::laguerren_imp(n, m, static_cast<value_type>(x), pol);
}

template <class T1, class T2>
inline typename laguerre_result<T1, T2>::type
laguerren(unsigned n, T1 m, T2 x)
{
  typedef typename policies::is_policy<T2>::type tag_type;
  return detail::laguerren(n, m, x, tag_type());
}

}  // namespace math
}  // namespace boost


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
#pragma once

#include <Eigen/Dense>

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/distributions/inverse_gamma.hpp>

#include <boost/random/chi_squared_distribution.hpp>
#include <boost/random/normal_distribution.hpp>

#include <dpMM/basemeasure.hpp>
#include <dpMM/distribution.hpp>
#include <dpMM/normalSphere.hpp>
#include <dpMM/iw.hpp>
#include <dpMM/sphere.hpp>
#include <dpMM/karcherMean.hpp>
#include <dpMM/clGMMData.hpp>

using namespace Eigen;
using std::endl;
using std::cout;

template<typename T>
class NiwSphereFull : public BaseMeasure<T>
{
public:

  uint32_t D_;
  IW<T> iw0_; // IW prior on the covariance of the normal in T_\muS^D
  Sphere<T> S_;
  NormalSphere<T> normalS_; // sampled normal on sphere - distribution of data

  NiwSphereFull(const IW<T>& iw, boost::mt19937* pRndGen);
  ~NiwSphereFull();

  virtual baseMeasureType getBaseMeasureType() const {return(NIW_SPHERE_FULL); }

  virtual BaseMeasure<T>* copy();
  virtual NiwSphereFull<T>* copyNative();

  /* for any point on the sphere; maps into T_muS and rotates north first */
  virtual T logLikelihood(const Matrix<T,Dynamic,1>& x) const ;
  virtual T logLikelihood(const Matrix<T,Dynamic,Dynamic>& x, uint32_t i) const
    {return logLikelihood(x.col(i));};
  /* assumes x is already in T_northS */
  virtual T logLikelihoodNorth(const Matrix<T,Dynamic,1>& x) const ;

  void posterior(const Matrix<T,Dynamic,Dynamic>& x, const VectorXu& z,
    uint32_t k);
  void posterior(const shared_ptr<ClGMMData<T> >& cldp, uint32_t k);
  /* assumes the x are already in T_northS correctly */
  void posteriorFromPtsInTpS(const Matrix<T,Dynamic,Dynamic>& x,
    const VectorXu& z, uint32_t k, uint32_t zDivider=1);

  void sampleMergedParams();
  /* samples Cov and then proposes means (covs are always accepted!) */
  void sample();
  /* proposes means and covariances jointly (and rejects them jointly as well)*/
  void sample_2();

  T logPdfUnderPrior() const;
  T logPdfUnderPriorMarginalized() const;
  T logPdfUnderPriorMarginalizedMerged(const shared_ptr<NiwSphereFull<T> >& other) const;
  T qRandomMuProposal() const;

  void print() const;

  virtual NiwSphereFull<T>* merge(const NiwSphereFull<T>& other);
  void fromMerge(const NiwSphereFull<T>& niwA, const NiwSphereFull<T>& niwB);

  const Matrix<T,Dynamic,Dynamic>& scatter() const {return iw0_.scatter();};
  Matrix<T,Dynamic,Dynamic>& scatter() {return iw0_.scatter();};
  const Matrix<T,Dynamic,1>& mean() const {return iw0_.mean();};
  Matrix<T,Dynamic,1>& mean() {return iw0_.mean();};
  T count() const {return iw0_.count();};
  T& count() {return iw0_.count();};

  const Matrix<T,Dynamic,Dynamic>& Sigma() const {return normalS_.Sigma();};

  const Matrix<T,Dynamic,1>& getMean() const {return normalS_.getMean();};
  const Matrix<T,Dynamic,1>& getMeanKarch() const {return meanKarch_;};
  // this is the sample mean
  void setMeanKarch(const Matrix<T,Dynamic,1>& mean) {
    meanKarch_ = mean;
  };
  void setMean(const Matrix<T,Dynamic,1>& mean) {normalS_.setMean(mean);};
  virtual uint32_t getDim() const {return(D_);};
private:

  void computeMergedSS( const NiwSphereFull<T>& niwA,
    const NiwSphereFull<T>& niwB, Matrix<T,Dynamic,Dynamic>& scatterM,
    Matrix<T,Dynamic,1>& meanM,
    Matrix<T,Dynamic,1>& muM, T& countM) const;

  Matrix<T,Dynamic,1> meanKarch_;

};

typedef NiwSphereFull<double> NiwSphereFulld;
typedef NiwSphereFull<float> NiwSphereFullf;


//==============================================================================
//         Copyright 2009 - 2013 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          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
//==============================================================================

#include <boost/fusion/include/at.hpp>
#include <vector>
#include <iostream>

#include <nt2/sdk/bench/benchmark.hpp>
#include <nt2/sdk/bench/protocol/max_duration.hpp>
#include <nt2/sdk/bench/metric/cycles_per_element.hpp>
#include <nt2/sdk/bench/stats/median.hpp>
#include <nt2/sdk/bench/setup/geometric.hpp>
#include <nt2/sdk/bench/setup/combination.hpp>
#include <nt2/sdk/bench/setup/constant.hpp>

using namespace nt2::bench;
using namespace nt2;

int mandelbrot_work(float a, float b, int max_iter)
{
  int iter = 0;
  float x = 0;
  float y = 0;
  float t;
  do {
     t = x*x - y*y + a;
     y = 2*x*y + b;
     x = t;
     iter++;
    } while ((x*x+y*y<4) && (iter<max_iter));

  return iter;
}

template<typename T> struct mandelbrot_scalar
{
  typedef T value_type;
  template<typename Setup>
  mandelbrot_scalar(Setup const& s)
                    :  h_(boost::fusion::at_c<0>(s))
                    ,  w_(boost::fusion::at_c<1>(s))
                    ,  a0_(boost::fusion::at_c<2>(s))
                    ,  a1_(boost::fusion::at_c<3>(s))
                    ,  b0_(boost::fusion::at_c<4>(s))
                    ,  b1_(boost::fusion::at_c<5>(s))
                    ,  max_iter_(boost::fusion::at_c<6>(s))
  {
    size_ = h_ * w_;

    A.resize(size_);
    B.resize(size_);
    C.resize(size_);

    T interval_A=(a1_-a0_)/(h_-1);
    T new_val=a0_;
    for (std::size_t jj=0;jj<w_;jj++){
      for (std::size_t ii=0;ii<h_;ii++){
        A[jj*h_+ii]=new_val;
        new_val+=interval_A;
      }
      new_val=a0_;
    }
    new_val=b0_;
    T interval_B=(b1_-b0_)/(w_-1);
    for (std::size_t jj=0;jj<w_;jj++)
    {
      for (std::size_t ii=0;ii<h_;ii++)
      {
        B[jj*h_+ii]=new_val;
      }
      new_val+=interval_B;
    }
  }

  void operator()()
  {
    std::size_t i, j;

    for(i=0; i<h_; i++)
    {
      for(j=0; j<w_; j++)
      {
        C[j+w_*i] = mandelbrot_work(A[j+w_*i], B[j+w_*i], max_iter_);
      }
    }
  }

  friend std::ostream& operator<<(std::ostream& os, mandelbrot_scalar<T> const& p)
  {
    return os << "(" << p.h_ << " x " << p.w_ << ")";
  }

  std::size_t size() const { return size_; }

private:
  std::size_t h_, w_;
  T a0_, a1_, b0_, b1_;
  std::size_t max_iter_, size_;
  std::vector<value_type> A, B;
  std::vector<int> C;
};

NT2_REGISTER_BENCHMARK_TPL( mandelbrot_scalar, (float) )
{
  std::size_t hmin = args("hmin", 100);
  std::size_t hmax = args("hmax", 1600);
  std::size_t hstep = args("hstep", 2);
  std::size_t wmin = args("wmin", 100);
  std::size_t wmax = args("wmax",1600);
  std::size_t wstep = args("wstep", 2);
  T xmin = args("xmin", -1.5);
  T xmax = args("xmax", 1.5);
  T ymin = args("ymin", -1.5);
  T ymax = args("ymax", 1.5);
  T max_iter = args("max_iter", 256);

  run_during_with< mandelbrot_scalar<float> > ( 1.
                                              , and_( geometric(hmin,hmax,hstep)
                                                    , geometric(wmin,wmax,wstep)
                                                    , constant(xmin)
                                                    , constant(xmax)
                                                    , constant(ymin)
                                                    , constant(ymax)
                                                    , constant(max_iter)
                                                    )
                                              , cycles_per_element<stats::median_>()
                                              );
}


#include <sec/ShortestEdgeCollapse.h>

#include <wmtk/utils/Logger.hpp>
#include <wmtk/utils/ManifoldUtils.hpp>

#include <igl/Timer.h>
#include <igl/is_edge_manifold.h>
#include <igl/is_vertex_manifold.h>
#include <igl/read_triangle_mesh.h>

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

using namespace sec;
using namespace wmtk;

TEST_CASE("separate-manifold-patch", "[test_sec]")
{
    std::vector<Eigen::Vector3d> v = {
        {Eigen::Vector3d(0, 0, 0),
         Eigen::Vector3d(1, 0, 0),
         Eigen::Vector3d(0, 1, 0),
         Eigen::Vector3d(0, 0, 1),
         Eigen::Vector3d(0, -1, 0)}};
    std::vector<std::array<size_t, 3>> tris = {{
        {{0, 1, 2}},
        {{0, 1, 3}},
        {{0, 1, 4}},
    }};

    std::vector<Eigen::Vector3d> out_v;
    std::vector<std::array<size_t, 3>> out_f;
    std::vector<size_t> freeze_v;
    wmtk::separate_to_manifold(v, tris, out_v, out_f, freeze_v);
    REQUIRE(out_v.size() == 9);
    REQUIRE(out_f.size() == 3);

    // after extraction the output should be manifold
    Eigen::VectorXi dummy;
    Eigen::MatrixXd F(out_f.size(), 3);
    for (int i = 0; i < out_f.size(); i++) {
        F.row(i) << out_f[i][0], out_f[i][1], out_f[i][2];
    }
    REQUIRE(igl::is_edge_manifold(F));

    ShortestEdgeCollapse m(out_v);
    m.create_mesh(out_v.size(), out_f, freeze_v, 0);
    m.collapse_shortest(-1);
    Eigen::MatrixXi Fafter = Eigen::MatrixXi::Constant(m.tri_capacity(), 3, -1);
    for (auto& t : m.get_faces()) {
        auto i = t.fid(m);
        auto vs = m.oriented_tri_vertices(t);
        Fafter.row(i) << vs[0].vid(m), vs[1].vid(m), vs[2].vid(m);
    }
    // after operations the mesh is manifold
    REQUIRE(igl::is_edge_manifold(Fafter));
    REQUIRE(igl::is_vertex_manifold(Fafter, dummy));
}

TEST_CASE("manifold-separate-test-37989", "[test_sec]")
{
    std::string filename = WMT_DATA_DIR "/37989_sf.obj";
    wmtk::manifold_internal::Vertices V;
    wmtk::manifold_internal::Facets F;
    igl::read_triangle_mesh(filename, V, F);
    REQUIRE_FALSE(igl::is_edge_manifold(F));
    std::vector<size_t> modified_vertices;
    wmtk::manifold_internal::resolve_nonmanifoldness(V, F, modified_vertices);
    REQUIRE(modified_vertices.size() > 0);
    REQUIRE(igl::is_edge_manifold(F));
    Eigen::VectorXi VI;
    REQUIRE(igl::is_vertex_manifold(F, VI));
}

TEST_CASE("shortest_edge_collapse", "[test_sec]")
{
    // 0___1___2                *
    // \  /\  /                 *
    // 3\/__\/4  ==>    __      *
    //   \  /          \  /     *
    //    \/5           \/5     *
    // 3-4 is shortest

    std::vector<Eigen::Vector3d> v_positions(6);
    v_positions[0] = Eigen::Vector3d(-3, 3, 0);
    v_positions[1] = Eigen::Vector3d(0, 3, 0);
    v_positions[2] = Eigen::Vector3d(3, 3, 0);
    v_positions[3] = Eigen::Vector3d(0, 0, 0);
    v_positions[4] = Eigen::Vector3d(0.5, 0, 0);
    v_positions[5] = Eigen::Vector3d(0, -3, 0);
    ShortestEdgeCollapse m(v_positions);
    std::vector<std::array<size_t, 3>> tris = {{{0, 1, 3}}, {{1, 2, 4}}, {{1, 4, 3}}, {{3, 4, 5}}};
    m.create_mesh_nofreeze(6, tris);
    std::vector<TriMesh::Tuple> edges = m.get_edges();
    // find the shortest edge
    double shortest = std::numeric_limits<double>::max();
    TriMesh::Tuple shortest_edge;
    for (TriMesh::Tuple t : edges) {
        size_t v1 = t.vid(m);
        size_t v2 = m.switch_vertex(t).vid(m);
        if ((v_positions[v1] - v_positions[v2]).squaredNorm() < shortest) {
            shortest = (v_positions[v1] - v_positions[v2]).squaredNorm();
            shortest_edge = t;
        }
    }

    REQUIRE_FALSE(m.check_link_condition(shortest_edge));
    m.collapse_shortest(-1);

    REQUIRE_FALSE(shortest_edge.is_valid(m));

    REQUIRE(m.get_vertices().size() == 3);
    REQUIRE(m.get_faces().size() == 1);
}

TEST_CASE("shortest_edge_collapse_boundary_edge", "[test_sec]")
{
    // 0___1___2    0 __1___2      0 __1
    // \  /\  /      \  |  /         \ |
    // 3\/__\/4  ==>  \ | / ==>        6
    //                 \|/5
    //

    std::vector<Eigen::Vector3d> v_positions(6);
    v_positions[0] = Eigen::Vector3d(-3, 3, 0);
    v_positions[1] = Eigen::Vector3d(0, 3, 0);
    v_positions[2] = Eigen::Vector3d(3, 3, 0);
    v_positions[3] = Eigen::Vector3d(0, 0, 0);
    v_positions[4] = Eigen::Vector3d(0.5, 0, 0);
    ShortestEdgeCollapse m(v_positions);
    std::vector<std::array<size_t, 3>> tris = {{{0, 1, 3}}, {{1, 2, 4}}, {{3, 1, 4}}};
    m.create_mesh_nofreeze(5, tris);
    std::vector<TriMesh::Tuple> edges = m.get_edges();
    // find the shortest edge
    double shortest = std::numeric_limits<double>::max();
    TriMesh::Tuple shortest_edge;
    for (TriMesh::Tuple t : edges) {
        size_t v1 = t.vid(m);
        size_t v2 = m.switch_vertex(t).vid(m);
        if ((v_positions[v1] - v_positions[v2]).squaredNorm() < shortest) {
            shortest = (v_positions[v1] - v_positions[v2]).squaredNorm();
            shortest_edge = t;
        }
    }
    m.collapse_shortest(100);
    // the collapsed edge tuple is not valid anymore
    REQUIRE_FALSE(shortest_edge.is_valid(m));

    m.write_triangle_mesh("collapsed.obj");
    REQUIRE(m.get_vertices().size() == 3);
    REQUIRE(m.get_faces().size() == 1);
}

TEST_CASE("shortest_edge_collapse_closed_mesh", "[test_sec]")
{
    SECTION("test on tet")
    {
        // create a tet and collapse can't happen
        std::vector<Eigen::Vector3d> v_positions(6);
        v_positions[0] = Eigen::Vector3d(-3, 3, 0);
        v_positions[1] = Eigen::Vector3d(0, 3, 0);
        v_positions[2] = Eigen::Vector3d(0, 0, 2);
        v_positions[3] = Eigen::Vector3d(0, 0, 0);

        ShortestEdgeCollapse m(v_positions);
        std::vector<std::array<size_t, 3>> tris = {
            {{0, 1, 3}},
            {{1, 2, 3}},
            {{0, 3, 2}},
            {{0, 1, 2}}};
        m.create_mesh_nofreeze(4, tris);
        m.collapse_shortest(100);
        REQUIRE(m.vert_capacity() == 4);

        REQUIRE(m.tri_capacity() == 4);
    }
    SECTION("test on cube, end with tet")
    {
        // then test on a cube
        // will have a tet in the end
        const std::string root(WMT_DATA_DIR);
        const std::string path = root + "/piece_0.obj";

        Eigen::MatrixXd V;
        Eigen::MatrixXi F;
        bool ok = igl::read_triangle_mesh(path, V, F);

        REQUIRE(ok);

        REQUIRE(V.rows() == 8);
        REQUIRE(F.rows() == 12);

        std::vector<Eigen::Vector3d> v(V.rows());
        std::vector<std::array<size_t, 3>> tri(F.rows());
        for (int i = 0; i < V.rows(); i++) {
            v[i] = V.row(i);
        }
        for (int i = 0; i < F.rows(); i++) {
            for (int j = 0; j < 3; j++) tri[i][j] = (size_t)F(i, j);
        }
        ShortestEdgeCollapse m(v);
        m.create_mesh(V.rows(), tri);
        REQUIRE(m.check_mesh_connectivity_validity());
        REQUIRE(m.collapse_shortest(100));

        std::vector<TriMesh::Tuple> edges = m.get_edges();
        REQUIRE(m.get_vertices().size() == 4);

        REQUIRE(m.get_faces().size() == 4);
    }
}

TEST_CASE("shortest_edge_collapse_octocat", "[test_sec][.slow]")
{
    const std::string root(WMT_DATA_DIR);
    const std::string path = root + "/Octocat.obj";

    Eigen::MatrixXd V;
    Eigen::MatrixXi F;
    bool ok = igl::read_triangle_mesh(path, V, F);

    REQUIRE(ok);

    std::vector<Eigen::Vector3d> v(V.rows());
    std::vector<std::array<size_t, 3>> tri(F.rows());
    for (int i = 0; i < V.rows(); i++) {
        v[i] = V.row(i);
    }
    for (int i = 0; i < F.rows(); i++) {
        for (int j = 0; j < 3; j++) tri[i][j] = (size_t)F(i, j);
    }
    ShortestEdgeCollapse m(v);
    m.create_mesh(V.rows(), tri);
    REQUIRE(m.collapse_shortest(2000));
    REQUIRE(m.check_mesh_connectivity_validity());
}

TEST_CASE("edge_manifold", "[test_sec]")
{
    const std::string root(WMT_DATA_DIR);
    const std::string path = root + "/circle.obj";

    Eigen::MatrixXd V;
    Eigen::MatrixXi F;
    bool ok = igl::read_triangle_mesh(path, V, F);

    REQUIRE(ok);

    std::vector<Eigen::Vector3d> v(V.rows());
    std::vector<std::array<size_t, 3>> tri(F.rows());
    for (int i = 0; i < V.rows(); i++) {
        v[i] = V.row(i);
    }
    for (int i = 0; i < F.rows(); i++) {
        for (int j = 0; j < 3; j++) tri[i][j] = (size_t)F(i, j);
    }
    ShortestEdgeCollapse m(v);
    m.create_mesh(V.rows(), tri);
    REQUIRE(m.check_mesh_connectivity_validity());
    REQUIRE(igl::is_edge_manifold(F) == m.check_edge_manifold());
}

TEST_CASE("shortest_edge_collapse_circle", "[test_sec]")
{
    const std::string root(WMT_DATA_DIR);
    const std::string path = root + "/circle.obj";

    Eigen::MatrixXd V;
    Eigen::MatrixXi F;
    bool ok = igl::read_triangle_mesh(path, V, F);

    REQUIRE(ok);

    std::vector<Eigen::Vector3d> v(V.rows());
    std::vector<std::array<size_t, 3>> tri(F.rows());
    for (int i = 0; i < V.rows(); i++) {
        v[i] = V.row(i);
    }
    for (int i = 0; i < F.rows(); i++) {
        for (int j = 0; j < 3; j++) tri[i][j] = (size_t)F(i, j);
    }
    ShortestEdgeCollapse m(v);
    m.create_mesh(V.rows(), tri);
    REQUIRE(m.check_mesh_connectivity_validity());
    REQUIRE(igl::is_edge_manifold(F));
    REQUIRE(m.check_edge_manifold());
    REQUIRE(m.collapse_shortest(100));
    m.consolidate_mesh();
    m.write_triangle_mesh("collapsed.obj");
}

TEST_CASE("metis_test_bigmesh", "[test_sec][.slow]")
{
    const std::string root(WMT_DATA_DIR);
    const std::string path = root + "/circle.obj";

    Eigen::MatrixXd V;
    Eigen::MatrixXi F;
    bool ok = igl::read_triangle_mesh(path, V, F);
    REQUIRE(ok);

    // change this for max concurrency
    int max_num_threads = 8;

    std::vector<double> timecost;

    for (int thread = 1; thread <= max_num_threads; thread *= 2) {
        std::vector<Eigen::Vector3d> v(V.rows());
        std::vector<std::array<size_t, 3>> tri(F.rows());
        for (int i = 0; i < V.rows(); i++) {
            v[i] = V.row(i);
        }
        for (int i = 0; i < F.rows(); i++) {
            for (int j = 0; j < 3; j++) tri[i][j] = (size_t)F(i, j);
        }

        ShortestEdgeCollapse m(v, thread);
        // m.print_num_attributes();
        m.create_mesh(V.rows(), tri);
        REQUIRE(m.check_mesh_connectivity_validity());
        igl::Timer timer;
        double time;
        timer.start();

        // change this for num of operations
        m.collapse_shortest(20);
        time = timer.getElapsedTimeInMilliSec();
        timecost.push_back(time);
    }
}


#include <iostream>                  // std::cout/endl
#include <stdint.h>                  // uint64_t
#include <boost/coroutine2/all.hpp>  // boost::coroutines2

typedef boost::coroutines2::coroutine<const uint64_t> coro_t;

void fibonacci(coro_t::push_type& yield)
{
    uint64_t a = 0;
    uint64_t b = 1;
    while (true) {
        yield(b);
        auto tmp = a;
        a = b;
        b += tmp;
    }
}

int main()
{
    for (auto i : coro_t::pull_type(
             boost::coroutines2::fixedsize_stack(),
             fibonacci)) {
        if (i >= 10000) {
            break;
        }
        std::cout << i << std::endl;
    }
}


/**
 * @file
 * @copyright 2020 Max Planck Gesellschaft. All rights reserved.
 * @license BSD 3-clause
 */
#pragma once

#include <Eigen/Eigen>
#include <ostream>

namespace trifinger_cameras
{
struct CameraParameters
{
    unsigned int image_width;
    unsigned int image_height;

    Eigen::Matrix3d camera_matrix;
    Eigen::Matrix<double, 1, 5> distortion_coefficients;

    Eigen::Matrix4d tf_world_to_camera;
};

std::ostream& operator<<(std::ostream& os, const CameraParameters& cp);

}  // namespace trifinger_cameras


#pragma once

#include "prx/simulation/plant.hpp"

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

namespace prx
{
	class ackermann_FO : public plant_t
	{
	public:
		ackermann_FO(const std::string& path);
		virtual ~ackermann_FO();

		virtual void propagate(const double simulation_step) override final;

		virtual void update_configuration() override;

	protected:

		virtual void compute_derivative() override final;

		double x;
		double y;
		double theta;

		double x_dot;
		double y_dot;
		double theta_dot;

		double gamma;
		double V;

		// Distance between front and back wheels
		double L = 1.5;

		const double max_delta_deg = 60;
		const double max_delta_rad = max_delta_deg * PRX_PI / 180.0;

		std::vector<double> min_ctrl_bound = {-max_delta_rad, -30};
		std::vector<double> max_ctrl_bound = {max_delta_rad, 30};

		std::vector<double> min_state_bound = {-10, -10, -PRX_PI};
		std::vector<double> max_state_bound = { 10,  10, PRX_PI};
	};
}

PRX_REGISTER_SYSTEM(ackermann_FO, Ackermann_FO)
PRX_REGISTER_VELOCITY_FN(Ackermann_FO, [](system_ptr_t s){return s -> input_control_space -> get_bounds()[1].second;})