#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)


//==================================================================================================
/*!
  @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


// 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;
}


#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


/*
 * CostFunctions1_2.hpp
 *
 *  Created on: Nov 18, 2015
 *      Author: atabb
 *      Updated on May25, 2018 to use Eigen instead of newmat by atabb
 *      Also resolved ambiguity between Eigen's enum quaterion type and this project's
 */

#ifndef COSTFUNCTIONS_HPP_
#define COSTFUNCTIONS_HPP_

#include "ceres/ceres.h"
#include "ceres/rotation.h"
#include "glog/logging.h"
#include "Calibration2.hpp"
#include <iostream>

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

using namespace Eigen;

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;

using namespace std;

enum PARAM_TYPE {Euler, AxisAngle, Cali_Quaternion };
//enum COST_TYPE {c1, c2, rp1, rp2, dh1, dh2, z, li_dq, li_kp, hirsh, shah};
enum COST_TYPE {c1, c2, rp1, rp2};
enum SEPARABLE_TYPE {rotation_only, translation_only, simultaneous};

template <typename T>
void Convert6ParameterEulerAngleRepresentationIntoMatrix(const T* X, T* XM){
	T RX[9];
	ceres::EulerAnglesToRotationMatrix(X, 3, RX);

	XM[0] = RX[0];
	XM[1] = RX[1];
	XM[2] = RX[2];
	XM[3] = X[3];

	XM[4] = RX[3];
	XM[5] = RX[4];
	XM[6] = RX[5];
	XM[7] = X[4];

	XM[8] = RX[6];
	XM[9] = RX[7];
	XM[10] = RX[8];
	XM[11] = X[5];

	XM[12] = T(0);
	XM[13] = T(0);
	XM[14] = T(0);
	XM[15] = T(1);

}

template <typename T>
void Convert6ParameterAxisAngleRepresentationIntoMatrix(const T* X, T* XM){
	T RX[9];

	ceres::AngleAxisToRotationMatrix(X, RX);

	XM[0] = RX[0];
	XM[1] = RX[1];
	XM[2] = RX[2];
	XM[3] = X[3];

	XM[4] = RX[3];
	XM[5] = RX[4];
	XM[6] = RX[5];
	XM[7] = X[4];

	XM[8] = RX[6];
	XM[9] = RX[7];
	XM[10] = RX[8];
	XM[11] = X[5];

	XM[12] = T(0);
	XM[13] = T(0);
	XM[14] = T(0);
	XM[15] = T(1);

}

template <typename T>
void Convert7ParameterQuaternionRepresentationIntoMatrix(const T* X, T* XM){
	T RX[9];

	ceres::QuaternionToRotation(X, RX);

	XM[0] = RX[0];
	XM[1] = RX[1];
	XM[2] = RX[2];
	XM[3] = X[4];

	XM[4] = RX[3];
	XM[5] = RX[4];
	XM[6] = RX[5];
	XM[7] = X[5];

	XM[8] = RX[6];
	XM[9] = RX[7];
	XM[10] = RX[8];
	XM[11] = X[6];

	XM[12] = T(0);
	XM[13] = T(0);
	XM[14] = T(0);
	XM[15] = T(1);

}

template <typename T>
void MatrixMultiply(const T* X, const T* Y, T* M, int row_col){

	// assuming square matrices
	int xi, yi;
	T r;
	for (int i = 0; i < row_col; i++){
		for (int j = 0; j < row_col; j++){
			// dot product the ith row of X by the jth column of Y

			r= T(0);
			for (int index = 0; index < row_col; index++){
				xi = i*row_col + index;
				yi = index*row_col + j;

				r += X[xi]*Y[yi];
			}
			M[i*row_col + j] = r;
		}
	}
}

template <typename T>
void MatrixMultiply(const T* X, const T* Y, T* M, int row0, int col0, int row1, int col1){

	//	cout << "Args in Matrix Multiply: " << endl;
	//	cout << row0 << ", " << col0 << ", " << row1 << ", " << col1 << endl;
	//
	//	cout << "First matrix " << endl;
	//	PrintMatrix(X, row0, col0);
	//
	//	cout << "Second matrix " << endl;
	//	PrintMatrix(Y, row1, col1);

	// not assuming square matrices
	if (col0 != row1){
		cout << "Wrong args sent to Matrix Multiply: " << endl;
		cout << row0 << ", " << col0 << ", " << row1 << ", " << col1 << endl;
	}

	int xi, yi;
	T r;
	for (int i = 0; i < row0; i++){
		for (int j = 0; j < col1; j++){
			// dot product the ith row of X by the jth column of Y, results in the
			//cout << "i, j " << i << ", " << j << endl;

			r= T(0);
			for (int index = 0; index < col0; index++){
				xi = i*col0 + index; // walk across the row
				yi = index*col1 + j; // walk down the columm

				r += X[xi]*Y[yi];

				//cout << "Mult " << xi << " from first by " << yi << " from second" << endl;
			}
			//cout << "Result stored in " << i*col1 + j << endl;
			//char ch; cin >> ch;
			M[i*col1 + j] = r;
		}
	}
}

template <typename T>
void PrintMatrix(const T* X, int row0, int col0){

	for (int i = 0, index = 0; i < row0; i++){
		for (int j = 0; j < col0; j++, index++){
			cout << X[index] << " ";
		}
		cout << endl;
	}

}

// one camera for now.
struct CF1_2 {
	CF1_2(double* A, double* B, PARAM_TYPE param_type, COST_TYPE cost_type): A(A), B(B), param_type(param_type), cost_type(cost_type){
	}

	template <typename T>
	bool operator()(const T* const X,
			const T* const Z,
			T* residuals) const {
		// X and Z are 6 parameters each, and our unknowns
		// need to represent these each as matrices

		//The {pitch,roll,yaw} Euler angles are rotations around the {x,y,z} axes, respectively.
		//They are applied in that same order, so the total rotation R is Rz * Ry * Rx.

		T XM[16];
		T ZM[16];

		switch (param_type){
		case Euler: {
			Convert6ParameterEulerAngleRepresentationIntoMatrix(X, XM);
			Convert6ParameterEulerAngleRepresentationIntoMatrix(Z, ZM);
		}	break;
		case AxisAngle: {
			Convert6ParameterAxisAngleRepresentationIntoMatrix(X, XM);
			Convert6ParameterAxisAngleRepresentationIntoMatrix(Z, ZM);
		}	break;
		case Cali_Quaternion: {
			Convert7ParameterQuaternionRepresentationIntoMatrix(X, XM);
			Convert7ParameterQuaternionRepresentationIntoMatrix(Z, ZM);
		}	break;
		}

		T term0[16];
		T term1[16];

		// ||AX-ZB||
		// have to convert to T
		T AT[16];
		T BT[16];
		for (int i = 0; i < 16; i++){
			AT[i] = T(A[i]);
			BT[i] = T(B[i]);
		}

		switch (cost_type){
		case c1: {
			MatrixMultiply(AT, XM, term0, 4);
			MatrixMultiply(ZM, BT, term1, 4);

			for (int i = 0; i < 12; i++){
				residuals[i] = term1[i] - term0[i];
			}
		} break;
		case c2: {
			MatrixMultiply(ZM, BT, term0, 4);
			MatrixMultiply(term0, XM, term1, 4);

			for (int i = 0; i < 12; i++){
				residuals[i] = AT[i] - term1[i];
			}

		}break;
		default: {
			cout << "Cost type " << cost_type << "  not supported in CF1_2 " << endl;
		}

		}

		return true;
	}

	static ceres::CostFunction* Create(double* A,
			double* B, PARAM_TYPE param_type, COST_TYPE cost_type) {
		return (new ceres::AutoDiffCostFunction<CF1_2, 12, 7, 7>(
				new CF1_2(&A[0], &B[0], param_type, cost_type)));
	}

	double* A;
	double* B;
	PARAM_TYPE param_type;
	COST_TYPE cost_type;
};

struct CF1_2_multi {
	CF1_2_multi(double* A, double* B, PARAM_TYPE param_type, COST_TYPE cost_type, int number_cameras, int this_camera, double* weighting):
		A(A), B(B), param_type(param_type), cost_type(cost_type), number_cameras(number_cameras), this_camera(this_camera), weighting(weighting){
	}

	template <typename T>
	//	bool operator()(const T* const X,
	//			const T* const Z,
	//			T* residuals) const {
	bool operator()(T const* const* parameters,
			T* residuals) const {
		// X and Z are 6 parameters each, and our unknowns
		// need to represent these each as matrices

		//The {pitch,roll,yaw} Euler angles are rotations around the {x,y,z} axes, respectively.
		//They are applied in that same order, so the total rotation R is Rz * Ry * Rx.

		T XM[16];
		T ZM[16];

		T X[7];
		T Z[7];

		T w = T(sqrt(*weighting));
		//cout << "Parameters for " << this_camera << endl;

		for (int i = 0; i < 7; i++){
			Z[i] = T(*parameters[(this_camera + 1)*7 + i]);
			X[i] = T(*parameters[0*7 + i]);
			//cout << "X, Z: " << X[i] << ", " << Z[i] << endl;
		}

		switch (param_type){
		case Euler: {
			Convert6ParameterEulerAngleRepresentationIntoMatrix(X, XM);
			Convert6ParameterEulerAngleRepresentationIntoMatrix(Z, ZM);
		}	break;
		case AxisAngle: {
			Convert6ParameterAxisAngleRepresentationIntoMatrix(X, XM);
			Convert6ParameterAxisAngleRepresentationIntoMatrix(Z, ZM);
		}	break;
		case Cali_Quaternion: {
			Convert7ParameterQuaternionRepresentationIntoMatrix(X, XM);
			Convert7ParameterQuaternionRepresentationIntoMatrix(Z, ZM);
		}	break;
		}

		T term0[16];
		T term1[16];

		// ||AX-ZB||
		// have to convert to T
		T AT[16];
		T BT[16];
		for (int i = 0; i < 16; i++){
			AT[i] = T(A[i]);
			BT[i] = T(B[i]);
		}

		switch (cost_type){
		case c1: {
			MatrixMultiply(AT, XM, term0, 4);
			MatrixMultiply(ZM, BT, term1, 4);

			for (int i = 0; i < 12; i++){
				residuals[i] = w*(term1[i] - term0[i]);
			}
		} break;
		case c2: {
			MatrixMultiply(ZM, BT, term0, 4);
			MatrixMultiply(term0, XM, term1, 4);

			for (int i = 0; i < 12; i++){
				residuals[i] = (AT[i] - term1[i]);
			}

		}break;
		default: {
			cout << "Cost type " << cost_type << "  not supported in CF1_2 " << endl;
		}

		}

		return true;
	}

	//	static ceres::CostFunction* Create(double* A,
	//			double* B, PARAM_TYPE param_type, COST_TYPE cost_type) {
	//		return (new ceres::AutoDiffCostFunction<CF1_2, 12, 7, 7>(
	//				new CF1_2(&A[0], &B[0], param_type, cost_type)));
	//	}

	static ceres::CostFunction* Create(double* A,
			double* B, PARAM_TYPE param_type, COST_TYPE cost_type, int number_cameras, int this_camera, double* weighting) {

		ceres::DynamicAutoDiffCostFunction<CF1_2_multi, 7>* cost_function =
				new ceres::DynamicAutoDiffCostFunction<CF1_2_multi, 7>(
						new CF1_2_multi(A, B, param_type, cost_type, number_cameras, this_camera, weighting));

		for (int i = 0; i < 7*(number_cameras + 1); i++){
			// one block for each X and the one Z
			cost_function->AddParameterBlock(1);
		}
		cost_function->SetNumResiduals(12);

		return cost_function;
	}

	double* A;
	double* B;
	PARAM_TYPE param_type;
	COST_TYPE cost_type;
	int number_cameras;
	int this_camera;
	double* weighting;
};

struct CF1_2_multi_extended {
	CF1_2_multi_extended(double* A, double* B, double* param_copy, PARAM_TYPE param_type, COST_TYPE cost_type, SEPARABLE_TYPE sep_type,  int number_cameras, int this_camera, double* weighting):
		A(A), B(B), param_copy(param_copy), param_type(param_type), cost_type(cost_type), sep_type(sep_type), number_cameras(number_cameras), this_camera(this_camera), weighting(weighting){
	}

	template <typename T>
	//	bool operator()(const T* const X,
	//			const T* const Z,
	//			T* residuals) const {
	bool operator()(T const* const* parameters,
			T* residuals) const {
		// X and Z are 6 parameters each, and our unknowns
		// need to represent these each as matrices

		//The {pitch,roll,yaw} Euler angles are rotations around the {x,y,z} axes, respectively.
		//They are applied in that same order, so the total rotation R is Rz * Ry * Rx.

		T XM[16];
		T ZM[16];

		T X[7];
		T Z[7];

		T w = T(sqrt(*weighting));
		//cout << "Parameters for " << this_camera << endl;

		switch (sep_type){
		case rotation_only: {
			for (int i = 0; i < 7; i++){
				Z[i] = T(*parameters[(this_camera + 1)*7 + i]);
				X[i] = T(*parameters[0*7 + i]);
				//cout << "X, Z: " << X[i] << ", " << Z[i] << endl;
			}
		}; break;
		case translation_only: {
			if (param_type == Cali_Quaternion){
				for (int i = 0; i < 7; i++){
					if (i < 4){
						Z[i] = T(param_copy[(this_camera + 1)*7 + i]);
						X[i] = T(param_copy[0*7 + i]);
					}	else {
						Z[i] = T(*parameters[(this_camera + 1)*7 + i]);
						X[i] = T(*parameters[0*7 + i]);
					}
					//cout << "X, Z: " << X[i] << ", " << Z[i] << endl;
				}

			}	else {
				for (int i = 0; i < 7; i++){
					if (i < 3){
						Z[i] = T(param_copy[(this_camera + 1)*7 + i]);
						X[i] = T(param_copy[0*7 + i]);
					}	else {
						Z[i] = T(*parameters[(this_camera + 1)*7 + i]);
						X[i] = T(*parameters[0*7 + i]);
					}
					//cout << "X, Z: " << X[i] << ", " << Z[i] << endl;
				}
			}

		} break;
		case simultaneous: {
			cout << "We don't let simultaneous currently use this function .... " << endl;
			exit(1);
		} break;

		}
		//		for (int i = 0; i < 7; i++){
		//			Z[i] = T(*parameters[(this_camera + 1)*7 + i]);
		//			X[i] = T(*parameters[0*7 + i]);
		//			//cout << "X, Z: " << X[i] << ", " << Z[i] << endl;
		//		}

		switch (param_type){
		case Euler: {
			Convert6ParameterEulerAngleRepresentationIntoMatrix(X, XM);
			Convert6ParameterEulerAngleRepresentationIntoMatrix(Z, ZM);
		}	break;
		case AxisAngle: {
			Convert6ParameterAxisAngleRepresentationIntoMatrix(X, XM);
			Convert6ParameterAxisAngleRepresentationIntoMatrix(Z, ZM);
		}	break;
		case Cali_Quaternion: {
			Convert7ParameterQuaternionRepresentationIntoMatrix(X, XM);
			Convert7ParameterQuaternionRepresentationIntoMatrix(Z, ZM);
		}	break;
		}

		T term0[16];
		T term1[16];

		// ||AX-ZB||
		// have to convert to T
		T AT[16];
		T BT[16];
		for (int i = 0; i < 16; i++){
			AT[i] = T(A[i]);
			BT[i] = T(B[i]);
		}

		switch (cost_type){
		case c1: {
			MatrixMultiply(AT, XM, term0, 4);
			MatrixMultiply(ZM, BT, term1, 4);

			switch (sep_type){
			case rotation_only: {
				for (int i = 0; i < 12; i++){
					if ((i+ 1) % 4 == 0){
						residuals[i] = T(0);
					}	else {
						residuals[i] = w*(term1[i] - term0[i]);
					}
				}
			} break;
			case translation_only: {
				for (int i = 0; i < 12; i++){
					if ((i+ 1) % 4 == 0){
						residuals[i] = w*(term1[i] - term0[i]);
					}	else {
						residuals[i] = T(0);
					}
				}

			} break;
			default: {
				for (int i = 0; i < 12; i++){
					residuals[i] = w*(term1[i] - term0[i]);
				}
			}
			}

		} break;
		case c2: {
			MatrixMultiply(ZM, BT, term0, 4);
			MatrixMultiply(term0, XM, term1, 4);

			switch (sep_type){
			case rotation_only: {
				for (int i = 0; i < 12; i++){
					if ((i+ 1) % 4 == 0){
						residuals[i] = T(0);
					}	else {
						residuals[i] = w*(AT[i] - term1[i]);
					}
				}
			} break;
			case translation_only: {
				for (int i = 0; i < 12; i++){
					if ((i+ 1) % 4 == 0){
						residuals[i] = w*(AT[i] - term1[i]);
					}	else {
						residuals[i] = T(0);
					}
				}

			} break;
			default: {
				for (int i = 0; i < 12; i++){
					residuals[i] = w*(AT[i] - term1[i]);
				}
			}
			}

		}break;
		default: {
			cout << "Cost type " << cost_type << "  not supported in CF1_2 " << endl;
		}

		}

		return true;
	}

	//	static ceres::CostFunction* Create(double* A,
	//			double* B, PARAM_TYPE param_type, COST_TYPE cost_type) {
	//		return (new ceres::AutoDiffCostFunction<CF1_2, 12, 7, 7>(
	//				new CF1_2(&A[0], &B[0], param_type, cost_type)));
	//	}

	static ceres::CostFunction* Create(double* A,
			double* B, double* param_copy, PARAM_TYPE param_type, COST_TYPE cost_type, SEPARABLE_TYPE sep_type, int number_cameras, int this_camera, double* weighting) {

		ceres::DynamicAutoDiffCostFunction<CF1_2_multi_extended, 7>* cost_function =
				new ceres::DynamicAutoDiffCostFunction<CF1_2_multi_extended, 7>(
						new CF1_2_multi_extended(A, B, param_copy, param_type, cost_type, sep_type, number_cameras, this_camera, weighting));

		for (int i = 0; i < 7*(number_cameras + 1); i++){
			// one block for each X and the one Z
			cost_function->AddParameterBlock(1);
		}

		cost_function->SetNumResiduals(12);

		return cost_function;
	}

	double* A;
	double* B;
	double* param_copy;
	PARAM_TYPE param_type;
	COST_TYPE cost_type;
	SEPARABLE_TYPE sep_type;
	int number_cameras;
	int this_camera;
	double* weighting;
};

struct RP1_2_multi {
	RP1_2_multi(double* camera_parameters, double* B, double* twoDpoints, double* threeDpoints, PARAM_TYPE param_type, COST_TYPE cost_type, int number_points,
			int number_cameras, int this_camera, double* weighting):
				camera_parameters(camera_parameters), B(B), twoDpoints(twoDpoints), threeDpoints(threeDpoints),
				param_type(param_type), cost_type(cost_type), number_points(number_points),
				number_cameras(number_cameras), this_camera(this_camera), weighting(weighting){
	}

	template <typename T>
	//	bool operator()(const T* const X,
	//			const T* const Z,
	//			T* residuals) const {
	bool operator()(T const* const* parameters,
			T* residuals) const {
		// X and Z are 7 parameters each, and our unknowns
		// need to represent these each as matrices
		// parameters: X0 (7) cam_parameters(12) X1(7) cam_parameters(12) Z(7)

		//The {pitch,roll,yaw} Euler angles are rotations around the {x,y,z} axes, respectively.
		//They are applied in that same order, so the total rotation R is Rz * Ry * Rx.
		//char ch;

		T XM[16];
		T ZM[16];

		T X[7];
		T Z[7];

		T w = T(sqrt(*weighting));

		for (int i = 0; i < 7; i++){
			X[i] = T(*parameters[0*7 + i]);
			Z[i] = T(*parameters[7 + (this_camera)*19 + i]);

		}

		switch (param_type){
		case Euler: {
			Convert6ParameterEulerAngleRepresentationIntoMatrix(X, XM);
			Convert6ParameterEulerAngleRepresentationIntoMatrix(Z, ZM);
		}	break;
		case AxisAngle: {
			Convert6ParameterAxisAngleRepresentationIntoMatrix(X, XM);
			Convert6ParameterAxisAngleRepresentationIntoMatrix(Z, ZM);
		}	break;
		case Cali_Quaternion: {
			Convert7ParameterQuaternionRepresentationIntoMatrix(X, XM);
			Convert7ParameterQuaternionRepresentationIntoMatrix(Z, ZM);
		}	break;
		}

		T term0[16];

		// ||AX-ZB||
		// have to convert to T
		T A_hat_T[16];
		T BT[16];
	//	T M[16];
		T K[9];
		T k1, k2, p1, p2, k3, k4, k5, k6;
		T r_sqr;

		T Xp[4];
		T xp[3];
		T xpp[3];

		for (int i = 0; i < 16; i++){

			BT[i] = T(B[i]);
		}

		for (int i = 0; i < 9; i++){
			K[i] = T(0);
		}

		K[8] = T(1);

		// set up A_hat
		MatrixMultiply(ZM, BT, term0, 4);
		MatrixMultiply(term0, XM, A_hat_T, 4);

		// copy over parameters
		switch (cost_type){
		case rp1: {
			// don't use the parameter version for camera cali -- this amtrix is sent all set up for use ....
			K[0] = T(camera_parameters[0]);
			K[2] = T(camera_parameters[1]);
			K[4] = T(camera_parameters[2]);
			K[5] = T(camera_parameters[3]);
			k1 = T(camera_parameters[4]);
			k2 = T(camera_parameters[5]);
			p1 = T(camera_parameters[6]);
			p2 = T(camera_parameters[7]);
			k3 = T(camera_parameters[8]);
			k4 = T(camera_parameters[9]);
			k5 = T(camera_parameters[10]);
			k6 = T(camera_parameters[11]);
		} break;
		case rp2:{
			K[0] = *parameters[7 + (this_camera)*19 + 7];
			K[2] = *parameters[7 + (this_camera)*19 + 8];
			K[4] = *parameters[7 + (this_camera)*19 + 9];
			K[5] = *parameters[7 + (this_camera)*19 + 10];
			k1 = *parameters[7 + (this_camera)*19 + 11];
			k2 = *parameters[7 + (this_camera)*19 + 12];
			p1 = *parameters[7 + (this_camera)*19 + 13];
			p2 = *parameters[7 + (this_camera)*19 + 14];
			k3 = *parameters[7 + (this_camera)*19 + 15];
			k4 = *parameters[7 + (this_camera)*19 + 16];
			k5 = *parameters[7 + (this_camera)*19 + 17];
			k6 = *parameters[7 + (this_camera)*19 + 18];
		} break;
		default: {

		}

		}

		for (int j = 0; j < 3; j++){
			Xp[j] = T(threeDpoints[j]);
		}
		Xp[3] = T(1);

		MatrixMultiply(A_hat_T, Xp, xp, 3, 4, 4, 1);

		xp[0] = xp[0]/xp[2];
		xp[1] = xp[1]/xp[2];

		r_sqr= xp[0]*xp[0] + xp[1]*xp[1];

		T numer = (T(1) + k1*r_sqr + k2*r_sqr*r_sqr + k3*r_sqr*r_sqr*r_sqr);
		T denom = (T(1) + k4*r_sqr + k5*r_sqr*r_sqr + k6*r_sqr*r_sqr*r_sqr);

		xpp[0] = xp[0]*numer/denom + T(2)*p1*xp[0]*xp[1] + p2*(r_sqr + T(2)*xp[0]*xp[0]);
		xpp[1] = xp[1]*numer/denom + T(2)*p2*xp[0]*xp[1] + p1*(r_sqr + T(2)*xp[1]*xp[1]);

		T predicted_x = (xpp[0]*K[0] + K[2]);
		T predicted_y = (xpp[1]*K[4] + K[5]);

		residuals[0] = w*(predicted_x - T(twoDpoints[0]));
		residuals[1] = w*(predicted_y - T(twoDpoints[1]));

		return true;
	}

	static ceres::CostFunction* Create(double* camera_parameters, double* B, double* twoDpoints, double* threeDpoints,
			PARAM_TYPE param_type, COST_TYPE cost_type, int number_points, int number_cameras, int this_camera, double* weighting) {

		ceres::DynamicAutoDiffCostFunction<RP1_2_multi, 10>* cost_function =
				new ceres::DynamicAutoDiffCostFunction<RP1_2_multi, 10>(
						new RP1_2_multi(camera_parameters, B, twoDpoints, threeDpoints, param_type, cost_type, number_points, number_cameras, this_camera, weighting));

		for (int i = 0; i <19*(number_cameras) + 7; i++){
			cost_function->AddParameterBlock(1);
		}
		cost_function->SetNumResiduals(2);

		return cost_function;
	}

	double* camera_parameters;
	double* B;
	double* twoDpoints;
	double* threeDpoints;

	PARAM_TYPE param_type;
	COST_TYPE cost_type;
	int number_points;
	int number_cameras;
	int this_camera;
	double* weighting;
};

struct RP1_2_multi_extended {
	RP1_2_multi_extended(double* camera_parameters, double* B, double* param_copy, double* twoDpoints, double* threeDpoints,
			PARAM_TYPE param_type, COST_TYPE cost_type, SEPARABLE_TYPE sep_type, int number_points,
			int number_cameras, int this_camera, double* weighting):
				camera_parameters(camera_parameters), B(B), param_copy(param_copy), twoDpoints(twoDpoints), threeDpoints(threeDpoints),
				param_type(param_type), cost_type(cost_type), sep_type(sep_type), number_points(number_points),
				number_cameras(number_cameras), this_camera(this_camera), weighting(weighting){
	}

	template <typename T>
	//	bool operator()(const T* const X,
	//			const T* const Z,
	//			T* residuals) const {
	bool operator()(T const* const* parameters,
			T* residuals) const {
		// X and Z are 7 parameters each, and our unknowns
		// need to represent these each as matrices
		// parameters: X0 (7) cam_parameters(12) X1(7) cam_parameters(12) Z(7)

		//The {pitch,roll,yaw} Euler angles are rotations around the {x,y,z} axes, respectively.
		//They are applied in that same order, so the total rotation R is Rz * Ry * Rx.
		//char ch;
		//cout << "Line 767 " << endl; //cin >> ch;
		T XM[16];
		T ZM[16];

		T X[7];
		T Z[7];

		T w = T(sqrt(*weighting));

		int break_point = 3;

		if (param_type == Cali_Quaternion){
			break_point = 4;
		}
		switch (sep_type){
		case rotation_only: {
			for (int i = 0; i < 7; i++){
				if (i < break_point){
					X[i] = T(*parameters[0*7 + i]);
					Z[i] = T(*parameters[7 + (this_camera)*19 + i]);
				}	else {
					Z[i] = T(param_copy[(this_camera + 1)*7 + i]);
					X[i] = T(param_copy[0*7 + i]);
				}
			}
		}; break;
		case translation_only: {
			for (int i = 0; i < 7; i++){
				if (i < break_point){
					Z[i] = T(param_copy[(this_camera + 1)*7 + i]);
					X[i] = T(param_copy[0*7 + i]);

				}	else {
					X[i] = T(*parameters[0*7 + i]);
					Z[i] = T(*parameters[7 + (this_camera)*19 + i]);
				}
			}
		} break;
		case simultaneous: {
			for (int i = 0; i < 7; i++){
				X[i] = T(*parameters[0*7 + i]);
				Z[i] = T(*parameters[7 + (this_camera)*19 + i]);
			}
		} break;
		}

		switch (param_type){
		case Euler: {
			Convert6ParameterEulerAngleRepresentationIntoMatrix(X, XM);
			Convert6ParameterEulerAngleRepresentationIntoMatrix(Z, ZM);
		}	break;
		case AxisAngle: {
			Convert6ParameterAxisAngleRepresentationIntoMatrix(X, XM);
			Convert6ParameterAxisAngleRepresentationIntoMatrix(Z, ZM);
		}	break;
		case Cali_Quaternion: {
			Convert7ParameterQuaternionRepresentationIntoMatrix(X, XM);
			Convert7ParameterQuaternionRepresentationIntoMatrix(Z, ZM);
		}	break;
		}

		T term0[16];

		// ||AX-ZB||
		// have to convert to T
		T A_hat_T[16];
		T BT[16];
		//T M[16];
		T K[9];
		T k1, k2, p1, p2, k3, k4, k5, k6;
		T r_sqr;

		T Xp[4];
		T xp[3];
		T xpp[3];

		for (int i = 0; i < 16; i++){

			BT[i] = T(B[i]);
		}

		for (int i = 0; i < 9; i++){
			K[i] = T(0);
		}

		K[8] = T(1);

		// set up A_hat
		MatrixMultiply(ZM, BT, term0, 4);
		MatrixMultiply(term0, XM, A_hat_T, 4);

		// copy over parameters
		switch (cost_type){
		case rp1: {
			// don't use the parameter version for camera cali -- this amtrix is sent all set up for use ....
			K[0] = T(camera_parameters[0]);
			K[2] = T(camera_parameters[1]);
			K[4] = T(camera_parameters[2]);
			K[5] = T(camera_parameters[3]);
			k1 = T(camera_parameters[4]);
			k2 = T(camera_parameters[5]);
			p1 = T(camera_parameters[6]);
			p2 = T(camera_parameters[7]);
			k3 = T(camera_parameters[8]);
			k4 = T(camera_parameters[9]);
			k5 = T(camera_parameters[10]);
			k6 = T(camera_parameters[11]);
		} break;
		case rp2:{
			K[0] = *parameters[7 + (this_camera)*19 + 7];
			K[2] = *parameters[7 + (this_camera)*19 + 8];
			K[4] = *parameters[7 + (this_camera)*19 + 9];
			K[5] = *parameters[7 + (this_camera)*19 + 10];
			k1 = *parameters[7 + (this_camera)*19 + 11];
			k2 = *parameters[7 + (this_camera)*19 + 12];
			p1 = *parameters[7 + (this_camera)*19 + 13];
			p2 = *parameters[7 + (this_camera)*19 + 14];
			k3 = *parameters[7 + (this_camera)*19 + 15];
			k4 = *parameters[7 + (this_camera)*19 + 16];
			k5 = *parameters[7 + (this_camera)*19 + 17];
			k6 = *parameters[7 + (this_camera)*19 + 18];
		} break;
		default: {

		}

		}

		for (int j = 0; j < 3; j++){
			Xp[j] = T(threeDpoints[j]);
		}
		Xp[3] = T(1);

		MatrixMultiply(A_hat_T, Xp, xp, 3, 4, 4, 1);

		xp[0] = xp[0]/xp[2];
		xp[1] = xp[1]/xp[2];

		r_sqr= xp[0]*xp[0] + xp[1]*xp[1];

		T numer = (T(1) + k1*r_sqr + k2*r_sqr*r_sqr + k3*r_sqr*r_sqr*r_sqr);
		T denom = (T(1) + k4*r_sqr + k5*r_sqr*r_sqr + k6*r_sqr*r_sqr*r_sqr);

		xpp[0] = xp[0]*numer/denom + T(2)*p1*xp[0]*xp[1] + p2*(r_sqr + T(2)*xp[0]*xp[0]);
		xpp[1] = xp[1]*numer/denom + T(2)*p2*xp[0]*xp[1] + p1*(r_sqr + T(2)*xp[1]*xp[1]);

		T predicted_x = (xpp[0]*K[0] + K[2]);
		T predicted_y = (xpp[1]*K[4] + K[5]);

		residuals[0] = w*(predicted_x - T(twoDpoints[0]));
		residuals[1] = w*(predicted_y - T(twoDpoints[1]));

		//cin >> ch;
		return true;
	}

	static ceres::CostFunction* Create(double* camera_parameters, double* B, double* param_copy, double* twoDpoints, double* threeDpoints,
			PARAM_TYPE param_type, COST_TYPE cost_type, SEPARABLE_TYPE sep_type, int number_points, int number_cameras, int this_camera, double* weighting) {

		ceres::DynamicAutoDiffCostFunction<RP1_2_multi_extended, 10>* cost_function =
				new ceres::DynamicAutoDiffCostFunction<RP1_2_multi_extended, 10>(
						new RP1_2_multi_extended(camera_parameters, B, param_copy, twoDpoints, threeDpoints, param_type, cost_type, sep_type, number_points, number_cameras, this_camera, weighting));

		for (int i = 0; i <19*(number_cameras) + 7; i++){
			cost_function->AddParameterBlock(1);
		}
		cost_function->SetNumResiduals(2);

		return cost_function;
	}

	double* camera_parameters;
	double* B;
	double* param_copy;
	double* twoDpoints;
	double* threeDpoints;

	PARAM_TYPE param_type;
	COST_TYPE cost_type;
	SEPARABLE_TYPE sep_type;
	int number_points;
	int number_cameras;
	int this_camera;
	double* weighting;
};

// this is all per camera, we don't need to send out all items at once ....
struct ReconstructX {
	ReconstructX(double* camera_parameters, double* transformation_matrix, double* twoDpoints, int number_points, bool individual, int this_point):
		camera_parameters(camera_parameters), transformation_matrix(transformation_matrix), twoDpoints(twoDpoints),
		number_points(number_points), individual(individual), this_point(this_point){
	}

	template <typename T>
	//	bool operator()(const T* const X,
	//			const T* const Z,
	//			T* residuals) const {
	bool operator()(T const* const* parameters,
			T* residuals) const {
		// the parameters are the threed points now

		// have to convert to T
		T A_hat_T[16];

		T K[9];
		T k1, k2, p1, p2, k3, k4, k5, k6;
		T r_sqr;
		T Xp[4];
		T xp[3];
		T xpp[3];

		// read in transformation matric
		for (int i = 0; i < 16; i++){
			A_hat_T[i] = T(transformation_matrix[i]);
		}

		// Read in camera calibration matrix
		for (int i = 0; i < 9; i++){
			K[i] = T(0);
		}
		K[8] = T(1);

		// don't use the parameter version for camera cali -- this amtrix is sent all set up for use ....
		K[0] = T(camera_parameters[0]);
		K[2] = T(camera_parameters[1]);
		K[4] = T(camera_parameters[2]);
		K[5] = T(camera_parameters[3]);
		k1 = T(camera_parameters[4]);
		k2 = T(camera_parameters[5]);
		p1 = T(camera_parameters[6]);
		p2 = T(camera_parameters[7]);
		k3 = T(camera_parameters[8]);
		k4 = T(camera_parameters[9]);
		k5 = T(camera_parameters[10]);
		k6 = T(camera_parameters[11]);

		if (!individual){
			for (int i = 0; i < number_points; i++){
				for (int j = 0; j < 3; j++){
					Xp[j] = T(*parameters[3*i + j]);
				}
				Xp[3] = T(1);

				MatrixMultiply(A_hat_T, Xp, xp, 3, 4, 4, 1);

				xp[0] = xp[0]/xp[2];
				xp[1] = xp[1]/xp[2];

				r_sqr= xp[0]*xp[0] + xp[1]*xp[1];

				T numer = (T(1) + k1*r_sqr + k2*r_sqr*r_sqr + k3*r_sqr*r_sqr*r_sqr);
				T denom = (T(1) + k4*r_sqr + k5*r_sqr*r_sqr + k6*r_sqr*r_sqr*r_sqr);

				xpp[0] = xp[0]*numer/denom + T(2)*p1*xp[0]*xp[1] + p2*(r_sqr + T(2)*xp[0]*xp[0]);
				xpp[1] = xp[1]*numer/denom + T(2)*p2*xp[0]*xp[1] + p1*(r_sqr + T(2)*xp[1]*xp[1]);

				T predicted_x = (xpp[0]*K[0] + K[2]);
				T predicted_y = (xpp[1]*K[4] + K[5]);

				//if (i < 1){
				residuals[2*i] = (predicted_x - T(twoDpoints[2*i + 0]));
				residuals[2*i + 1] = (predicted_y - T(twoDpoints[2*i + 1]));

			}
		}	else {
			int i = this_point;

			for (int j = 0; j < 3; j++){
				Xp[j] = T(*parameters[j]);
			}
			Xp[3] = T(1);

			MatrixMultiply(A_hat_T, Xp, xp, 3, 4, 4, 1);

			xp[0] = xp[0]/xp[2];
			xp[1] = xp[1]/xp[2];

			r_sqr= xp[0]*xp[0] + xp[1]*xp[1];

			T numer = (T(1) + k1*r_sqr + k2*r_sqr*r_sqr + k3*r_sqr*r_sqr*r_sqr);
			T denom = (T(1) + k4*r_sqr + k5*r_sqr*r_sqr + k6*r_sqr*r_sqr*r_sqr);

			xpp[0] = xp[0]*numer/denom + T(2)*p1*xp[0]*xp[1] + p2*(r_sqr + T(2)*xp[0]*xp[0]);
			xpp[1] = xp[1]*numer/denom + T(2)*p2*xp[0]*xp[1] + p1*(r_sqr + T(2)*xp[1]*xp[1]);

			T predicted_x = (xpp[0]*K[0] + K[2]);
			T predicted_y = (xpp[1]*K[4] + K[5]);

			residuals[0] = (predicted_x - T(twoDpoints[2*i + 0]));
			residuals[1] = (predicted_y - T(twoDpoints[2*i + 1]));
		}

		return true;
	}

	static ceres::CostFunction* Create(double* camera_parameters, double* transformation_matrix, double* twoDpoints, int number_points) {

		ceres::DynamicAutoDiffCostFunction<ReconstructX, 3>* cost_function =
				new ceres::DynamicAutoDiffCostFunction<ReconstructX, 3>(
						new ReconstructX(camera_parameters, transformation_matrix, twoDpoints, number_points, false, 0 ));

		for (int i = 0; i <3*number_points; i++){
			cost_function->AddParameterBlock(1);
		}
		cost_function->SetNumResiduals(2*number_points);

		return cost_function;
	}

	static ceres::CostFunction* CreateID(double* camera_parameters, double* transformation_matrix, double* twoDpoints, int number_points, int this_point) {

		ceres::DynamicAutoDiffCostFunction<ReconstructX, 3>* cost_function =
				new ceres::DynamicAutoDiffCostFunction<ReconstructX, 3>(
						new ReconstructX(camera_parameters, transformation_matrix, twoDpoints, number_points, true, this_point));

		for (int i = 0; i <3; i++){
			cost_function->AddParameterBlock(1);
		}
		cost_function->SetNumResiduals(2);

		return cost_function;
	}

	// we need the camera parameters, transformation matrices (one per camera), and twoDpoints for each camera that can see the pattern.
	// for now, solve all together as one big cost function
	// result is the three d points for the pattern.
	double* camera_parameters;
	double* transformation_matrix;
	double* twoDpoints;

	int number_points;
	bool individual;
	int this_point;

};

void CF1_2_one_camera(vector< vector<Matrix4d> >& As, vector<Matrix4d>& Bs, double* x, double* z, std::ofstream& out, PARAM_TYPE param_type, COST_TYPE cost_type);

void CF1_2_multi_camera(vector< vector<MatrixXd> >& As, vector<Matrix4d>& Bs, double* x, std::ofstream& out, PARAM_TYPE param_type, COST_TYPE cost_type);

void CF1_2_multi_camera_separable(vector< vector<MatrixXd> >& As, vector<Matrix4d>& Bs, double* x, std::ofstream& out, PARAM_TYPE param_type, COST_TYPE cost_type, SEPARABLE_TYPE sep_type);

void RP1_2_multi_camera(vector<CaliObjectOpenCV2>& COs, vector<Matrix4d>& Bs, double* camera_params, double* x,
		std::ofstream& out, PARAM_TYPE param_type, COST_TYPE cost_type);

void RP1_2_multi_camera_sparse(vector<CaliObjectOpenCV2>& COs, vector<Matrix4d>& Bs, double* camera_params, double* x,
		std::ofstream& out, PARAM_TYPE param_type, COST_TYPE cost_type);

void RP1_2_multi_camera_sparse_seperable(vector<CaliObjectOpenCV2>& COs, vector<Matrix4d>& Bs, double* camera_params, double* x,
		std::ofstream& out, PARAM_TYPE param_type, COST_TYPE cost_type, SEPARABLE_TYPE sep_type);

void CopyFromCalibration(vector<CaliObjectOpenCV2>& COs, double* camera_params);

void CopyToCalibration(vector<CaliObjectOpenCV2>& COs, double* camera_parameters);

void ReconstructXFunction(vector<CaliObjectOpenCV2>& COs, vector<Matrix4d>& Bs, Matrix4d& X, vector<Matrix4d>& Zs, double* threeDpoints, vector<double>& reprojection_errors,
		std::ofstream& out);

double ComputeSummedSquaredDistanceBetweenSets(double* set0, double* set1, int number_points);

void Initialize3DPoints(vector<CaliObjectOpenCV2>& COs, double* threeDpoints, int number_points);

void ReconstructXFunctionIndividuals(vector<CaliObjectOpenCV2>& COs, vector<Matrix4d>& Bs, Matrix4d& X, vector<Matrix4d>& Zs, double* threeDpoints, vector<double>& reprojection_errors,
		std::ofstream& out);

#endif /* COSTFUNCTIONS_HPP_ */


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

/*
 Copyright (C) 2014 Klaus Spanderen

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

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

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

/*! \file analyticpdfhestonengine.cpp
    \brief Analytic engine for arbitrary European payoffs under the Heston model
*/

#include <ql/math/functional.hpp>
#include <ql/math/solvers1d/brent.hpp>
#include <ql/math/integrals/gausslobattointegral.hpp>
#include <ql/experimental/exoticoptions/analyticpdfhestonengine.hpp>

#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif

#include <boost/bind.hpp>

#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic pop
#endif

#include <cmath>

namespace QuantLib {

    namespace {
        struct Hestonp {
            Real v0, kappa, theta, sigma, rho;
        };

        std::complex<Real> gamma(const Hestonp& p, Real p_x) {
            return std::complex<Real>(p.kappa, p.rho*p.sigma*p_x);
        }

        std::complex<Real> omega(const Hestonp& p, Real p_x) {
           const std::complex<Real> g = gamma(p, p_x);
           return std::sqrt(g*g
                  + p.sigma*p.sigma*std::complex<Real>(p_x*p_x, -p_x));
        }

        std::complex<Real> cpx_pv(const Hestonp& p, Real p_x, Real x, Time t){
            const Real sigma2 = p.sigma*p.sigma;
            const std::complex<Real> g = gamma(p, p_x);
            const std::complex<Real> o = omega(p, p_x);

            return
                std::exp(std::complex<Real>(0.0, p_x*x)
                         - p.v0*std::complex<Real>(p_x*p_x, -p_x)
                           /(g+o*std::cosh(0.5*o*t)/std::sinh(0.5*o*t))
                         + p.kappa*g*p.theta*t/sigma2)
                /std::pow(std::cosh(0.5*o*t)+g/o*std::sinh(0.5*o*t),
                          2.0*p.kappa*p.theta/sigma2);
        }

        Real zero_pv(const Hestonp& p, Real p_x, Real x, Time t){
            return std::abs(cpx_pv(p, p_x, x, t))-QL_EPSILON;
        }

        Real pv(const Hestonp& p, Real p_x, Real x, Time t){
            return cpx_pv(p, p_x, x, t).real();
        }
    }

    AnalyticPDFHestonEngine::AnalyticPDFHestonEngine(
        const boost::shared_ptr<HestonModel>& model,
        Real eps, Size nIterations, Real xMax)
    : eps_(eps), xMax_(xMax),
      nIterations_(nIterations),
      model_(model) {
    }

    void AnalyticPDFHestonEngine::calculate() const {
        // this is an European option pricer
        QL_REQUIRE(arguments_.exercise->type() == Exercise::European,
                   "not an European option");

        const boost::shared_ptr<HestonProcess>& process = model_->process();

        const Time t = process->time(arguments_.exercise->lastDate());

        const Real xMax = 10 * std::sqrt(process->theta()*t
        	+ (process->v0() - process->theta())*(1-std::exp(-process->kappa()*t)));

        results_.value = GaussLobattoIntegral(nIterations_, eps_)
            (boost::bind(&AnalyticPDFHestonEngine::weightedPayoff, this,_1, t),
             -xMax, xMax);
    }

    Real AnalyticPDFHestonEngine::Pv(Real s_0, Real s_t, Time t) const {
        const boost::shared_ptr<HestonProcess>& process = model_->process();
        const DiscountFactor d=  process->riskFreeRate()->discount(t)
                               / process->dividendYield()->discount(t);
        const Real x_t = std::log(d * s_t/s_0);

        return Pv(x_t, t);
    }

    Real AnalyticPDFHestonEngine::Pv(Real x_t, Time t) const {
        const Hestonp p = { model_->v0(),
                            model_->kappa(),
                            model_->theta(),
                            model_->sigma(),
                            model_->rho() };
        Real xMax = (xMax_ != Null<Real>()) ? xMax_
            : Brent().solve(boost::bind(&zero_pv, p, _1, x_t, t),
                            0.01, 1.0, 1.0);

        return GaussLobattoIntegral(nIterations_, 0.1*eps_)
               (boost::bind(&pv, p, _1, x_t, t), -xMax, xMax)/M_TWOPI;
    }

    Real AnalyticPDFHestonEngine::weightedPayoff(Real x_t, Time t) const {
        const boost::shared_ptr<HestonProcess>& process = model_->process();

        const Real s_0 = process->s0()->value();
        const DiscountFactor rD = process->riskFreeRate()->discount(t);
        const DiscountFactor dD = process->dividendYield()->discount(t);

        const Real s_t = s_0*std::exp(x_t)*dD/rD;
        const Real payoff = (*arguments_.payoff)(s_t);

        return (payoff != 0.0) ? payoff*Pv(x_t, t)*rD : 0.0;
    }
}