#ifndef FSTCLASSIFIERMULTINOMNAIVEBAYES_H
#define FSTCLASSIFIERMULTINOMNAIVEBAYES_H

/*!======================================================================
   Feature Selection Toolbox 3 source code
   ---------------------------------------

   \file    classifier_multinom_naivebayes.hpp
   \brief   Implements Naive-like Bayes classifier based on multinomial model
   \author  Petr Somol (somol@utia.cas.cz) with collaborators, see Contacts at http://fst.utia.cz
   \date    March 2011
   \version 3.1.0.beta
   \note    FST3 was developed using gcc 4.3 and requires
   \note    \li Boost library (http://www.boost.org/, tested with versions 1.33.1 and 1.44),
   \note    \li (\e optionally) LibSVM (http://www.csie.ntu.edu.tw/~cjlin/libsvm/,
                tested with version 3.00)
   \note    Note that LibSVM is required for SVM related tools only,
            as demonstrated in demo12t.cpp, demo23.cpp, demo25t.cpp, demo32t.cpp, etc.

*/ /*
=========================================================================
Copyright:
  * FST3 software (with exception of any externally linked libraries)
    is copyrighted by Institute of Information Theory and Automation (UTIA),
    Academy of Sciences of the Czech Republic.
  * FST3 source codes as presented here do not contain code of third parties.
    FST3 may need linkage to external libraries to exploit its functionality
    in full. For details on obtaining and possible usage restrictions
    of external libraries follow their original sources (referenced from
    FST3 documentation wherever applicable).
  * FST3 software is available free of charge for non-commercial use.
    Please address all inquires concerning possible commercial use
    of FST3, or if in doubt, to FST3 maintainer (see http://fst.utia.cz)
  * Derivative works based on FST3 are permitted as long as they remain
    non-commercial only.
  * Re-distribution of FST3 software is not allowed without explicit
    consent of the copyright holder.
Disclaimer of Warranty:
  * FST3 software is presented "as is", without warranty of any kind,
    either expressed or implied, including, but not limited to, the implied
    warranties of merchantability and fitness for a particular purpose.
    The entire risk as to the quality and performance of the program
    is with you. Should the program prove defective, you assume the cost
    of all necessary servicing, repair or correction.
Limitation of Liability:
  * The copyright holder will in no event be liable to you for damages,
    including any general, special, incidental or consequential damages
    arising out of the use or inability to use the code (including but not
    limited to loss of data or data being rendered inaccurate or losses
    sustained by you or third parties or a failure of the program to operate
    with any other programs).
========================================================================== */

#include <boost/smart_ptr.hpp>
#include <iostream>
#include <sstream>
#include <cmath>
#include <cstring> // memcpy
#include "error.hpp"
#include "global.hpp"
#include "classifier.hpp"
#include "model_multinom.hpp"

/*============== Template parameter type naming conventions ==============
--------- Numeric types: -------------------------------------------------
DATATYPE - data sample values - usually real numbers (but may be integers
          in text processing etc.)
REALTYPE - must be real numbers - for representing intermediate results of
          calculations like mean, covariance etc.
IDXTYPE - index values for enumeration of data samples - (nonnegative) integers,
          extent depends on numbers of samples in data
DIMTYPE - index values for enumeration of features (dimensions), or classes (not
          class sizes) - (nonnegative) integers, usually lower extent than IDXTYPE,
          but be aware of expressions like _classes*_features*_features !
          in linearized representations of feature matrices for all classes
BINTYPE - feature selection marker type - represents ca. <10 different feature
          states (selected, deselected, sel./desel. temporarily 1st nested loop, 2nd...)
RETURNTYPE - criterion value: real value, but may be extended in future to support
          multiple values
--------- Class types: ---------------------------------------------------
SUBSET       - class of class type Subset
CLASSIFIER   - class implementing interface defined in abstract class Classifier
EVALUATOR    - class implementing interface defined in abstract class Sequential_Step
DISTANCE     - class implementing interface defined in abstract class Distance
DATAACCESSOR - class implementing interface defined in abstract class Data_Accessor
INTERVALCONTAINER - class of class type TIntervaller
CONTAINER    - STL container of class type TInterval
========================================================================== */

namespace FST {

/*! \brief Implements Naive-like Bayes classifier based on multinomial model */
template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
class Classifier_Multinomial_NaiveBayes : public Classifier<RETURNTYPE,DIMTYPE,SUBSET,DATAACCESSOR> { // abstract class
	// \note In this case the classifier should be better called Naive-like Bayes than Naive Bayes
public:
	typedef boost::shared_ptr<DATAACCESSOR> PDataAccessor;
	typedef boost::shared_ptr<SUBSET> const PSubset;
	typedef typename DATAACCESSOR::PPattern PPattern;
	Classifier_Multinomial_NaiveBayes();
	virtual ~Classifier_Multinomial_NaiveBayes() {notify("Classifier_Multinomial_NaiveBayes destructor.");}

	/*! pre-learning mode:
	    if true, calls once _model->learn() for full set size and later in train() uses only narrow()ing to access submatrixes
	             (makes sense only as long as training data do not change, i.e., within one split)
	    if false (default), learns new model in each train() call
	*/
	void enable_prelearn_mode(const PDataAccessor da);
	void disable_prelearn_mode() {_prelearn_mode=false;}
	bool get_prelearn_mode() const {return _prelearn_mode;}

	// NOTE: must! be called before train() or test() calls whenever the training set changes (i.e., with changing da Splits)
	void initialize(const PDataAccessor da); // must be called to pre-compute mean[] and cov[]

	virtual bool classify(DIMTYPE &cls, const PPattern &pattern);  // classifies pattern, returns the respective class index
	virtual bool train(const PDataAccessor da, const PSubset sub); // learns from designated training part of data
	virtual bool test(RETURNTYPE &result, const PDataAccessor da); // estimates accuracy using designated test data

	DIMTYPE get_n() const {assert(_model); return _model->get_n();}
	DIMTYPE get_d() const {assert(_model); return _model->get_d();}

	Classifier_Multinomial_NaiveBayes* clone() const;
	Classifier_Multinomial_NaiveBayes* sharing_clone() const {throw fst_error("Classifier_Multinomial_NaiveBayes::sharing_clone() not supported, use Classifier_Multinomial_NaiveBayes::clone() instead.");}
	Classifier_Multinomial_NaiveBayes* stateless_clone() const {throw fst_error("Classifier_Multinomial_NaiveBayes::stateless_clone() not supported, use Classifier_Multinomial_NaiveBayes::clone() instead.");}

	virtual std::ostream& print(std::ostream& os) const {os << "Classifier_Multinomial_NaiveBayes()"; return os;}
private:
	Classifier_Multinomial_NaiveBayes(const Classifier_Multinomial_NaiveBayes& cmnb); // copy-constructor
protected:
	boost::scoped_ptr<Model_Multinomial<DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> > _model;

	DIMTYPE _classes, _features; // size of arrays below
	boost::scoped_array<REALTYPE> _Pcd; // P(class|document) (lazy allocation)

private:
	bool _prelearn_mode;
	boost::scoped_array<DIMTYPE> _index;
	DIMTYPE _subfeatures;
};

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
Classifier_Multinomial_NaiveBayes<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::Classifier_Multinomial_NaiveBayes(const Classifier_Multinomial_NaiveBayes& cmnb) :
	_classes(cmnb._classes),
	_features(cmnb._features),
	_prelearn_mode(cmnb._prelearn_mode),
	_subfeatures(cmnb._subfeatures)
{
	notify("Classifier_Multinomial_NaiveBayes copy-constructor.");
	if(cmnb._model) _model.reset(new Model_Multinomial<DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>(*cmnb._model));
	if(_classes>0)
	{
		_Pcd.reset(new REALTYPE[_classes]); memcpy((void *)_Pcd.get(),(void *)(cmnb._Pcd).get(),sizeof(REALTYPE)*_classes);
	}
	_index.reset(new DIMTYPE[_features]); memcpy((void *)_index.get(),(void *)(cmnb._index).get(),sizeof(DIMTYPE)*_features);
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
Classifier_Multinomial_NaiveBayes<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>* Classifier_Multinomial_NaiveBayes<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::clone() const
{
	Classifier_Multinomial_NaiveBayes<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> *clone=new Classifier_Multinomial_NaiveBayes<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>(*this);
	clone->set_cloned();
	return clone;
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
Classifier_Multinomial_NaiveBayes<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::Classifier_Multinomial_NaiveBayes()
{
	notify("Classifier_Multinomial_NaiveBayes constructor.");
	_model.reset(new Model_Multinomial<DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>);
	_classes=0;
	_features=0;
	_subfeatures=0;
	_prelearn_mode=false;
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
void Classifier_Multinomial_NaiveBayes<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::enable_prelearn_mode(const PDataAccessor da)
{
	notify("Classifier_Multinomial_NaiveBayes::init_prelearn_mode().");
	assert(_model);
	assert(da);
	assert(da->getNoOfFeatures()>0);
	assert(da->getNoOfClasses()>0);
	_model->learn(da);
	_prelearn_mode=true;
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
void Classifier_Multinomial_NaiveBayes<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::initialize(PDataAccessor da)
{
	notify("Classifier_Multinomial_NaiveBayes::initialize().");
	//assert(_model);
	assert(da);
	assert(da->getNoOfFeatures()>0);
	assert(da->getNoOfClasses()>0);

	if(!_Pcd || _classes!=da->getNoOfClasses()) _Pcd.reset(new REALTYPE[da->getNoOfClasses()]);
	_classes=da->getNoOfClasses();

	if(!_index || _features!=da->getNoOfFeatures()) _index.reset(new DIMTYPE[da->getNoOfFeatures()]);
	_features=da->getNoOfFeatures();
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Classifier_Multinomial_NaiveBayes<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::train(const PDataAccessor da, const PSubset sub)
{
	// NOTE: mean[] and cov[] must be pre-computed using initialize()
	// NOTE: explicit call is needed here to ensure wrapper functionality
	//       but a work-aroung can be implemented to optionally disable the call
	//       (would make sense when testing different subsets for the same da split)
	initialize(da);

	notify("Classifier_Multinomial_NaiveBayes::train().");
	assert(_model);
	assert(da);
	if(_prelearn_mode) assert(get_n()>0);
	assert(da->getNoOfFeatures()>0);
	assert(sub);
	assert(sub->get_frozen_mode()==false);
	if(_prelearn_mode) assert(sub->get_n_raw()==get_n());
	//{
	//	ostringstream sos;
	//	sos << "sub->get_n_raw()="<<sub->get_n_raw() << " da->getNoOfFeatures()="<<da->getNoOfFeatures() << std::endl;
	//	syncout::print(std::cout,sos);
	//}
	assert(sub->get_n_raw()==da->getNoOfFeatures());

	if(_prelearn_mode) _model->narrow_to(sub); // assume _n-dimensional model is pre-learned and needs to be narrow()ed only (NOTE: training data must not change since)
	else {_model->learn(da,sub); _model->denarrow();} // re-learn from scratch, training data has changed (i.e., after switch to next data split)

//	if(sub->get_d_raw()==1) // ??? single feature subsets can not be used for multinomial model based classification
//	{
//	}

	// prepare feature subset index buffering
	DIMTYPE f;
	bool b;
	for(b=sub->getFirstFeature(f),_subfeatures=0;b==true;b=sub->getNextFeature(f),_subfeatures++) {assert(_subfeatures<get_n()); _index[_subfeatures]=f;}

	_model->compute_theta(); //thetas to be used in classify() and test()

#ifdef DEBUG
	{ostringstream sos; sos << "index: "; for(f=0;f<_subfeatures;f++) sos << _index[f] << " "; syncout::print(std::cout,sos);}
#endif
	assert(_subfeatures==sub->get_d_raw());
	return true;
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Classifier_Multinomial_NaiveBayes<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::classify(DIMTYPE &cls, const PPattern &pattern)
{
	// NOTE: mean[] and cov[] must be pre-computed using initialize()
	// NOTE: _inverse[] _det[] and _constant[] must be pre-computed
	notify("Classifier_Multinomial_NaiveBayes::test().");
	assert(_model);
	if(_prelearn_mode) assert(get_n()>0);
	assert(_model->get_classes()>1);
	assert(_classes==_model->get_classes());
	assert(_index);
	assert(_subfeatures==_model->get_d());
	if(!_prelearn_mode) assert(_subfeatures==_model->get_n());
	assert(_subfeatures>0);

	DIMTYPE f;
	DIMTYPE c_cand;
	REALTYPE res;
	DIMTYPE wTH;
	REALTYPE *theta=&(_model->get_theta()[0]); // dirty, but this object owns _model thus no memory corruption is possible
	REALTYPE *theta_tmp;

	if(_subfeatures==1) {
		// NOTE: single features are unusable for classification because theta=1 in each class,
		//       thus log(theta)=0 and consequently Pcd[] does not depend on feature frequency
		cls=0; // consider all single features equally unusable
		return false;
	} else {
		// compute P(c|pattern) for each class
		wTH=0;
		for(c_cand=0;c_cand<_classes;c_cand++)
		{
			theta_tmp=&theta[wTH];
			res=0.0;
			for(f=0;f<_subfeatures;f++)
			{
				//{
				//	ostringstream sos; sos << "f:" << _index[f] << " ptmp[]=" << (REALTYPE)ptmp[_index[f]] << ", theta=" << _model->get_theta()[wTH+f] << ", log=" << log(_model->get_theta()[wTH+f]) << std::endl;
				//	syncout::print(std::cout,sos);
				//}
				res+=(REALTYPE)pattern[_index[f]] * log(theta_tmp[f]);
			}
			_Pcd[c_cand]=log(_model->get_Pc(c_cand))+res;
			//{
			//	ostringstream sos; sos << " Pc["<<c_cand<<"]=" << _model->get_Pc(c_cand) << ", log(Pc)=" << log(_model->get_Pc(c_cand)) << ", res="<< res << ", _Pcd[]=" << _Pcd[c_cand] << std::endl << std::endl;
			//	syncout::print(std::cout,sos);
			//}
			wTH+=_subfeatures;
		}
		// find maximum P[c|pattern]
		res=_Pcd[0]; cls=0;
		for(c_cand=1;c_cand<_classes;c_cand++) if(_Pcd[c_cand]>res) {res=_Pcd[c_cand]; cls=c_cand;}
	}
	return true;
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Classifier_Multinomial_NaiveBayes<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::test(RETURNTYPE &result, const PDataAccessor da)
{
	// NOTE: mean[] and cov[] must be pre-computed using initialize()
	// NOTE: _inverse[] _det[] and _constant[] must be pre-computed
	notify("Classifier_Multinomial_NaiveBayes::test().");
	assert(_model);
	if(_prelearn_mode) assert(get_n()>0);
	assert(_model->get_classes()>1);
	assert(_classes==_model->get_classes());
	assert(da);
	if(_prelearn_mode) assert(da->getNoOfFeatures()==get_n());
	assert(da->getNoOfClasses()>0);
	assert(da->getNoOfClasses()==_classes);
	assert(_index);
	assert(_subfeatures==_model->get_d());
	if(!_prelearn_mode) assert(_subfeatures==_model->get_n());
	assert(_subfeatures>0);

	typename DATAACCESSOR::PPattern p;
	IDXTYPE s,i;
	IDXTYPE count, correct;
	DIMTYPE _features=da->getNoOfFeatures();
	DIMTYPE clstmp;

	if(_subfeatures==1) {
		// NOTE: single features are unusable for classification because theta=1 in each class,
		//       thus log(theta)=0 and consequently Pcd[] does not depend on feature frequency
		correct=0; count=1; // consider all single features equally unusable
	} else {
		bool b;
		const DIMTYPE da_test_loop=1; // to avoid mixup of get*Block() loops of different types

		count=0;
		correct=0;
		for(DIMTYPE c_test=0;c_test<_classes;c_test++)
		{
			da->setClass(c_test);
			for(b=da->getFirstBlock(TEST,p,s,da_test_loop);b==true;b=da->getNextBlock(TEST,p,s,da_test_loop)) for(i=0;i<s;i++)
			{
				if(!classify(clstmp,&p[i*_features])) return false;
				if(clstmp==c_test) correct++;
				count++;
			}
		}
	}
	assert(count>0);
	result=(RETURNTYPE)correct/(RETURNTYPE)count;
#ifdef DEBUG
	{
		ostringstream sos; sos << " result=" << result << std::endl;
		syncout::print(std::cout,sos);
	}
#endif
	return true;
}

} // namespace
#endif // FSTCLASSIFIERMULTINOMNAIVEBAYES_H ///:~


// Copyright (C) 2012  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/sparse_vector.h>
#include "tester.h"
#include <dlib/rand.h>
#include <dlib/string.h>
#include <vector>
#include <sstream>
#include <ctime>

namespace
{
    using namespace test;
    using namespace dlib;
    using namespace std;
    dlib::logger dlog("test.sparse_vector");

    void test_sparse_matrix_vector_multiplies()
    {
        dlib::rand rnd;

        const long size = 30;

        for (int iter = 0; iter < 10; ++iter)
        {
            print_spinner();

            std::vector<sample_pair> edges;
            std::vector<ordered_sample_pair> oedges;
            matrix<double> M(size,size);
            M = 0;
            for (long i = 0; i < M.size()/3; ++i)
            {
                const long r = rnd.get_random_32bit_number()%M.nr();
                const long c = rnd.get_random_32bit_number()%M.nc();
                const double d = rnd.get_random_gaussian()*10;
                M(r,c) += d;
                oedges.push_back(ordered_sample_pair(r,c,d));
            }

            matrix<double> SM(size,size);
            SM = 0;
            for (long i = 0; i < SM.size()/3; ++i)
            {
                const long r = rnd.get_random_32bit_number()%SM.nr();
                const long c = rnd.get_random_32bit_number()%SM.nc();
                const double d = rnd.get_random_gaussian()*10;
                SM(r,c) += d;
                if (r != c)
                    SM(c,r) += d;
                edges.push_back(sample_pair(r,c,d));
            }

            const matrix<double> v = randm(size,1);

            matrix<double> result;

            sparse_matrix_vector_multiply(oedges, v, result);
            DLIB_TEST_MSG(length(M*v - result) < 1e-12, length(M*v - result));

            sparse_matrix_vector_multiply(edges, v, result);
            DLIB_TEST_MSG(length(SM*v - result) < 1e-12, length(SM*v - result));

        }
    }

// ----------------------------------------------------------------------------------------

    void test_sparse_matrix_vector_multiply1()
    {
        print_spinner();
        std::map<unsigned long,double> sv;
        sv[2] = 8;
        sv[6] = 2.3;

        matrix<double,10,1> v;
        v = 0;
        v(2) = 8;
        v(6) = 2.3;

        matrix<double,0,1> r1, r2;

        r1 = gaussian_randm(4,10)*v;
        r2 = sparse_matrix_vector_multiply(gaussian_randm(4,std::numeric_limits<long>::max()),sv);

        DLIB_TEST(max(abs(r1-r2)) < 1e-15);
    }

// ----------------------------------------------------------------------------------------

    void test_sparse_matrix_vector_multiply2()
    {
        std::vector<std::pair<unsigned long,double> > sv;
        sv.push_back(make_pair(6, 1.42));
        sv.push_back(make_pair(3, 5));

        matrix<double,9,1> v;
        v = 0;
        v(3) = 5;
        v(6) = 1.42;

        matrix<double,0,1> r1, r2;

        r1 = gaussian_randm(3,9)*v;
        r2 = sparse_matrix_vector_multiply(gaussian_randm(3,std::numeric_limits<long>::max()),sv);

        DLIB_TEST(max(abs(r1-r2)) < 1e-15);
    }

// ----------------------------------------------------------------------------------------

    void test_make_sparse_vector_inplace()
    {
        std::vector<std::pair<unsigned long,double> > vect;
        vect.push_back(make_pair(4,1));
        vect.push_back(make_pair(0,1));
        vect.push_back(make_pair(4,1));
        vect.push_back(make_pair(3,1));
        vect.push_back(make_pair(8,1));
        vect.push_back(make_pair(8,1));
        vect.push_back(make_pair(8,1));
        vect.push_back(make_pair(8,1));

        make_sparse_vector_inplace(vect);

        DLIB_TEST(vect.size() == 4);
        DLIB_TEST(vect[0].first == 0);
        DLIB_TEST(vect[1].first == 3);
        DLIB_TEST(vect[2].first == 4);
        DLIB_TEST(vect[3].first == 8);

        DLIB_TEST(vect[0].second == 1);
        DLIB_TEST(vect[1].second == 1);
        DLIB_TEST(vect[2].second == 2);
        DLIB_TEST(vect[3].second == 4);
    }

// ----------------------------------------------------------------------------------------

    class sparse_vector_tester : public tester
    {
    public:
        sparse_vector_tester (
        ) :
            tester (
                "test_sparse_vector",       // the command line argument name for this test
                "Run tests on the sparse_vector routines.", // the command line argument description
                0                     // the number of command line arguments for this test
            )
        {
        }

        void perform_test (
        )
        {
            test_make_sparse_vector_inplace();

            std::map<unsigned int, double> v;
            v[4] = 8;
            v[2] = -4;
            v[9] = 10;

            DLIB_TEST(max(v) == 10);
            DLIB_TEST(min(v) == -4);

            v.clear();
            v[4] = 8;
            v[9] = 10;
            DLIB_TEST(max(v) == 10);
            DLIB_TEST(min(v) == 0);

            v.clear();
            v[4] = -9;
            v[9] = -4;
            DLIB_TEST(max(v) == 0);
            DLIB_TEST(min(v) == -9);

            {
                matrix<double> a(2,2), b(2,2);
                a = randm(2,2);
                b = randm(2,2);

                DLIB_TEST(equal(a-b, subtract(a,b)));
                DLIB_TEST(equal(a+b, add(a,b)));
                DLIB_TEST(equal(a-(b+b), subtract(a,b+b)));
                DLIB_TEST(equal(a+b+b, add(a,b+b)));
            }

            {
                std::map<unsigned long,double> a, b, c;
                a[1] = 2;
                a[3] = 5;

                b[0] = 3;
                b[1] = 1;

                c = add(a,b);
                DLIB_TEST(c.size() == 3);
                DLIB_TEST(c[0] == 3);
                DLIB_TEST(c[1] == 3);
                DLIB_TEST(c[3] == 5);

                c = subtract(a,b);
                DLIB_TEST(c.size() == 3);
                DLIB_TEST(c[0] == -3);
                DLIB_TEST(c[1] == 1);
                DLIB_TEST(c[3] == 5);

                c = add(b,a);
                DLIB_TEST(c.size() == 3);
                DLIB_TEST(c[0] == 3);
                DLIB_TEST(c[1] == 3);
                DLIB_TEST(c[3] == 5);

                c = subtract(b,a);
                DLIB_TEST(c.size() == 3);
                DLIB_TEST(c[0] == 3);
                DLIB_TEST(c[1] == -1);
                DLIB_TEST(c[3] == -5);

                std::vector<std::pair<unsigned long,double> > aa, bb, cc;

                aa.assign(a.begin(), a.end());
                bb.assign(b.begin(), b.end());

                cc = add(aa,bb);
                DLIB_TEST(cc.size() == 3);
                DLIB_TEST(cc[0].first == 0);
                DLIB_TEST(cc[1].first == 1);
                DLIB_TEST(cc[2].first == 3);
                DLIB_TEST(cc[0].second == 3);
                DLIB_TEST(cc[1].second == 3);
                DLIB_TEST(cc[2].second == 5);

                cc = subtract(aa,bb);
                DLIB_TEST(cc.size() == 3);
                DLIB_TEST(cc[0].first == 0);
                DLIB_TEST(cc[1].first == 1);
                DLIB_TEST(cc[2].first == 3);
                DLIB_TEST(cc[0].second == -3);
                DLIB_TEST(cc[1].second == 1);
                DLIB_TEST(cc[2].second == 5);

                cc = add(bb,aa);
                DLIB_TEST(cc.size() == 3);
                DLIB_TEST(cc[0].first == 0);
                DLIB_TEST(cc[1].first == 1);
                DLIB_TEST(cc[2].first == 3);
                DLIB_TEST(cc[0].second == 3);
                DLIB_TEST(cc[1].second == 3);
                DLIB_TEST(cc[2].second == 5);

                cc = subtract(bb,aa);
                DLIB_TEST(cc.size() == 3);
                DLIB_TEST(cc[0].first == 0);
                DLIB_TEST(cc[1].first == 1);
                DLIB_TEST(cc[2].first == 3);
                DLIB_TEST(cc[0].second == 3);
                DLIB_TEST(cc[1].second == -1);
                DLIB_TEST(cc[2].second == -5);

            }

            test_sparse_matrix_vector_multiplies();
            test_sparse_matrix_vector_multiply1();
            test_sparse_matrix_vector_multiply2();

            {
                matrix<double,0,1> a, b;
                a = gaussian_randm(6,1, 0);
                b = gaussian_randm(6,1, 1);

                std::vector<std::pair<unsigned long,double> > aa, bb;

                assign(aa, a);
                assign(bb, b);

                // dot() does something special when the sparse vectors have entries for
                // each dimension, which is what happens when they are copied from dense
                // vectors.  So the point of the tests in this block is to make sure dot()
                // works right in this case.
                DLIB_TEST(std::abs(dot(a,b) - dot(aa,bb)) < 1e-14);
                a(3) = 0;
                assign(aa, a);
                DLIB_TEST(std::abs(dot(a,b) - dot(aa,bb)) < 1e-14);
            }
        }
    };

    sparse_vector_tester a;

}


#pragma once

#include <xul/util/id_generator.hpp>
#include <xul/lang/object_impl.hpp>
#include <boost/unordered_set.hpp>
#include <xul/util/random.hpp>
#include <limits>

namespace xul {

template <typename ValueT>
class random_id_generator
{
public:
    random_id_generator()
    {
        set_range(std::numeric_limits<ValueT>::min(), std::numeric_limits<ValueT>::max());
    }

    void set_range(ValueT range_min, ValueT range_max)
    {
        m_min = range_min;
        m_max = range_max;
    }

    ValueT generate()
    {
        ValueT val = 0;
        for (int i = 0; i < 100; ++i)
        {
            if (do_generate(val))
                return val;
        }
        return 0;
    }
    void release(ValueT val)
    {
        m_ids.erase(val);
    }

private:
    bool do_generate(ValueT& val)
    {
        ValueT tempval = random_generator<ValueT>::generate();
        if (m_ids.find(tempval) != m_ids.end())
            return false;
        val = tempval;
        return true;
    }

private:
    boost::unordered_set<ValueT> m_ids;
    ValueT m_min;
    ValueT m_max;
};

class sequential_id_generator : public object_impl<id_generator>
{
public:
    sequential_id_generator()
    {
        set_range(0, UINT_MAX);
    }

    void set_range(uint32_t range_min, uint32_t range_max)
    {
        m_min = range_min;
        m_max = range_max;
        m_next = m_min;
    }

    uint32_t generate()
    {
        uint32_t val = m_next;
        ++m_next;
        return val;
    }
    void release(uint32_t val)
    {
    }

private:
    uint32_t m_min;
    uint32_t m_max;
    uint32_t m_next;
};

}


#include <Engine/MeshEdit/ARAP.h>
#include <Engine/MeshEdit/MinSurf.h>
#include <Engine/MeshEdit/Paramaterize.h>
#include <Engine/Primitive/TriMesh.h>
#include <Eigen/Sparse>
#include <Eigen/SparseQR>
#include <set>

using namespace Ubpa;
using namespace Eigen;
using namespace std;

Ubpa::ARAP::ARAP(Ptr<TriMesh> triMesh, int iter_n, int log_verbosity)
	: heMesh(make_shared<HEMesh<V>>())
	, heMesh_orig(make_shared<HEMesh<V>>())
	, iter_n(iter_n), log_verbosity(log_verbosity)
{
	Init(triMesh, false, true);

	// copy it into triMesh_orig
}

void Ubpa::ARAP::Clear()
{
	heMesh->Clear();
	heMesh_orig->Clear();
	triMesh = nullptr;
}

std::array<pointf2, 3> Ubpa::ARAP::genEmbed(pointf3 v0, pointf3 v1, pointf3 v2) {
	vecf3 v10 = v1 - v0;
	vecf3 v20 = v2 - v0;
	vecf3 v21 = v2 - v1;

	float cos_theta = v10.cos_theta(v20);
	return std::array{ pointf2(0, 0), pointf2(v10.norm(), 0)
		, pointf2(v20.norm() * cos_theta, v20.norm() * sqrtf(1 - (cos_theta * cos_theta))) };
}

bool Ubpa::ARAP::Init(Ptr<TriMesh> triMesh, bool noClear, bool initOrig)
{
	if (!noClear)
		Clear();

	if (triMesh == nullptr)
		return true;

	if (triMesh->GetType() == TriMesh::INVALID) {
		printf("ERROR::ARAP::Init:\n"
			"\t""trimesh is invalid\n");
		return false;
	}

	// init half-edge structure
	size_t nV = triMesh->GetPositions().size();
	vector<vector<size_t>> triangles;
	triangles.reserve(triMesh->GetTriangles().size());
	for (auto triangle : triMesh->GetTriangles())
		triangles.push_back({ triangle->idx[0], triangle->idx[1], triangle->idx[2] });
	heMesh->Reserve(nV);
	heMesh->Init(triangles);

	if (initOrig) {
		heMesh_orig->Clear();
		heMesh_orig->Reserve(nV);
		heMesh_orig->Init(triangles);
	}

	if (!heMesh->IsTriMesh() || !heMesh->HaveBoundary()) {
		printf("ERROR::ARAP::Init:\n"
			"\t""trimesh is not a triangle mesh or hasn't a boundaries\n");
		heMesh->Clear();
		if (initOrig) {
			heMesh_orig->Clear();
		}
		return false;
	}

	// triangle mesh's positions ->  half-edge structure's positions
	for (int i = 0; i < nV; i++) {
		auto v = heMesh->Vertices().at(i);
		v->pos = triMesh->GetPositions()[i].cast_to<vecf3>();
		if (initOrig) {
			auto u = heMesh_orig->Vertices().at(i);
			u->pos = triMesh->GetPositions()[i].cast_to<vecf3>();
		}
	}

	this->triMesh = triMesh;
	return true;
}

// calculate energy with HeMesh

//namespace Ubpa {
//	template<typename V, typename E, typename P>
//	const std::vector<E*> TPolygon<V, E, P>::BoundaryEdges() {
//		std::vector<E*> edges;
//		for (auto he : BoundaryHEs())
//			edges.push_back(he->Edge());
//		return edges;
//	}
//
//	template<typename V, typename E, typename P>
//	const std::vector<V*> TPolygon<V, E, P>::BoundaryVertice() {
//		std::vector<V*> vertices;
//		for (auto he : BoundaryHEs())
//			vertices.push_back(he->Origin());
//		return vertices;
//	}
//}

Ubpa::ARAP::V* Ubpa::ARAP::findOpp(P* trig, V* v1, V* v2) {
	decltype(v1) opp_v = nullptr;;
	if (trig != nullptr) {
		for (auto vv : trig->BoundaryVertice()) {
			if (vv != v1 && vv != v2) {
				opp_v = vv;
			}
		}
		assert(opp_v != nullptr);
	}
	return opp_v;
}

std::tuple<Vector2f, Vector2f> Ubpa::ARAP::calcXVecFull(int xi, int xj) {
	auto vi = heMesh_orig->Vertices()[xi];
	auto vj = heMesh_orig->Vertices()[xj];

	auto tri_ij = vi->HalfEdgeTo(vj)->Polygon();
	auto tri_ji = vj->HalfEdgeTo(vi)->Polygon();

	Vector2f x_vec_ij = Vector2f::Zero();
	Vector2f x_vec_ji = Vector2f::Zero();

	// calculate x vec in mapped 2d space
	auto getIdx = [](decltype(tri_ij) trig, decltype(vi) vert) {
		int i = 0;
		for (auto vv : trig->BoundaryVertice()) {
			if (vv == vert) {
				break;
			}
			else {
				i++;
			}
		}
		assert(i < 3);
		return i;
	};

	if (tri_ij != nullptr) {
		auto tri_ij_embed = genEmbed(tri_ij->BoundaryVertice()[0]->pos.cast_to<pointf3>()
			, tri_ij->BoundaryVertice()[1]->pos.cast_to<pointf3>()
			, tri_ij->BoundaryVertice()[2]->pos.cast_to<pointf3>());

		int vi_idx = getIdx(tri_ij, vi);
		int vj_idx = getIdx(tri_ij, vj);
		x_vec_ij = { (tri_ij_embed[vi_idx] - tri_ij_embed[vj_idx])[0]
						 , (tri_ij_embed[vi_idx] - tri_ij_embed[vj_idx])[1] };

		if (log_verbosity > 40) {
			printf("- embed_ij: ");
			print(tri_ij_embed);
			printf("- x_vec_ij: \n");
			cout << x_vec_ij << endl;
		}
	}

	if (tri_ji != nullptr) {
		auto tri_ji_embed = genEmbed(tri_ji->BoundaryVertice()[0]->pos.cast_to<pointf3>()
			, tri_ji->BoundaryVertice()[1]->pos.cast_to<pointf3>()
			, tri_ji->BoundaryVertice()[2]->pos.cast_to<pointf3>());

		int vi_idx = getIdx(tri_ji, vi);
		int vj_idx = getIdx(tri_ji, vj);
		x_vec_ji = { (tri_ji_embed[vi_idx] - tri_ji_embed[vj_idx])[0]
						 , (tri_ji_embed[vi_idx] - tri_ji_embed[vj_idx])[1] };

		if (log_verbosity > 40) {
			printf("- embed_ji: ");
			print(tri_ji_embed);
			printf("- x_vec_ji: \n");
			cout << x_vec_ji << endl;
		}
	}

	return std::make_tuple(x_vec_ij, x_vec_ji);

}

// x_vec, cotij, cotji
std::tuple<Vector2f, double, double> Ubpa::ARAP::calcXVec(int xi, int xj) {
	auto vi = heMesh_orig->Vertices()[xi];
	auto vj = heMesh_orig->Vertices()[xj];

	auto tri_ij = vi->HalfEdgeTo(vj)->Polygon();
	auto tri_ji = vj->HalfEdgeTo(vi)->Polygon();

	if (log_verbosity > 50) {
		cout << "- tri_ij: ";
		print(tri_ij);

		cout << "- tri_ji: ";
		print(tri_ji);
	}

	// mapped NaN (inf) to zero
	auto getCtgf = [](float cosine) {
		return (cosine == 1 || cosine == -1) ? 0 : (cosine / (sqrtf(1 - cosine * cosine)));
	};

	decltype(vj) opp_v_ji = findOpp(tri_ji, vi, vj);
	decltype(vj) opp_v_ij = findOpp(tri_ij, vi, vj);

	double cos_theta_ij = opp_v_ij != nullptr ? (vi->pos - opp_v_ij->pos).cos_theta(vj->pos - opp_v_ij->pos) : 1;
	double cos_theta_ji = opp_v_ji != nullptr ? (vi->pos - opp_v_ji->pos).cos_theta(vj->pos - opp_v_ji->pos) : 1;

	double cot_theta_ij = getCtgf(cos_theta_ij);
	double cot_theta_ji = getCtgf(cos_theta_ji);

	// calculate x vec in mapped 2d space
	auto getIdx = [](decltype(tri_ij) trig, decltype(vi) vert) {
		int i = 0;
		for (auto vv : trig->BoundaryVertice()) {
			if (vv == vert) {
				break;
			} else {
				i++;
			}
		}
		assert(i < 3);
		return i;
	};

	decltype(tri_ij) trig;
	// only one trig necessary
	if (tri_ij == nullptr) {
		trig = tri_ji;
	} else {
		trig = tri_ij;
	}

	if (log_verbosity > 50) {
		printf("- embed_input: ");
		print(trig);
	}

	auto tri_embed = genEmbed(trig->BoundaryVertice()[0]->pos.cast_to<pointf3>()
				, trig->BoundaryVertice()[1]->pos.cast_to<pointf3>()
				, trig->BoundaryVertice()[2]->pos.cast_to<pointf3>());

	if (log_verbosity > 40) {
		printf("- embed: ");
		print(tri_embed);
	}

	int vi_idx = getIdx(trig, vi);
	int vj_idx = getIdx(trig, vj);
	Vector2f x_vec = { (tri_embed[vi_idx] - tri_embed[vj_idx])[0]
					 , (tri_embed[vi_idx] - tri_embed[vj_idx])[1] };

	return std::make_tuple(x_vec, cot_theta_ij, cot_theta_ji);
}

void Ubpa::ARAP::UniformPara() {
	// First, detect and fix boundary
	random_set<V*> boundary_points;
	random_set<V*> inner_points;

	auto boundaries = this->heMesh->Boundaries();
	if (boundaries.size() != 1) {
		cout << "ERROR::Parameterize::DoPara:" << endl
			<< "\t" << "got boundaries = " << boundaries.size()
			<< " (expect 1)" << endl;
		return;
	}

	for (auto v : boundaries[0]) {
		boundary_points.insert(v->Origin());
	}

	for (auto v : heMesh->Vertices()) {
		if (!boundary_points.contains(v)) {
			inner_points.insert(v);
		}
	}

	//const float boost_factor = 100;
	const float boost_factor = 1;
	// Fix our boundary

	int points_total = boundary_points.size();
	float step = 4.0f / points_total;

	float curr = 0;
	for (auto v : boundary_points) {
		vecf3 new_pos;
		if (curr >= 0 && curr < 1) {
			new_pos[0] = curr;
			new_pos[1] = new_pos[2] = 0;
		} else if (curr >= 1 && curr < 2) {
			new_pos[0] = 1;
			new_pos[1] = curr - 1;
			new_pos[2] = 0;
		} else if (curr >= 2 && curr < 3) {
			new_pos[0] = 1 - (curr - 2);
			new_pos[1] = 1;
			new_pos[2] = 0;
		} else { // curr >= 3; remember to cut off as fp precision is an issue
			new_pos[0] = 0;
			new_pos[1] = curr > 4 ? 4 : 4 - curr;
			new_pos[2] = 0;
		}
		v->pos = new_pos * boost_factor;
		curr += step;
	}

	// Build sparse matrix
	size_t n = inner_points.size();
	SparseMatrix<float> coeff_mat(n, n);
	coeff_mat.setZero();
	VectorXf b_vec_x = VectorXf::Zero(n);
	VectorXf b_vec_y = VectorXf::Zero(n);
	VectorXf b_vec_z = VectorXf::Zero(n);

	cout << "coeff mat build start" << endl;

	int current_row = 0;
	for (auto v : inner_points) {
		// vidx CERTAINLY follows order (and it's redundant)
		size_t vidx = inner_points.idx(v);
		auto adj = v->AdjVertices();
		size_t degree = v->Degree();
		for (auto adjv : adj) {
				// check type
			if (boundary_points.contains(adjv)) { // this set is usually smaller
				b_vec_x(current_row) += (1.0f / degree) * adjv->pos[0];
				b_vec_y(current_row) += (1.0f / degree) * adjv->pos[1];
				b_vec_z(current_row) += (1.0f / degree) * adjv->pos[2];
			} else { // inner
				assert(inner_points.contains(adjv));
				size_t adjidx = inner_points.idx(adjv);
				// todo add assert = 0
				coeff_mat.insert(current_row, adjidx) = -1.0f / degree;
			}
		}

		// add itself
		// todo add assert
		coeff_mat.insert(current_row, vidx) = 1;
		current_row++;
	}

	cout << "coeff mat build complete" << endl;

	// Solve
	SparseQR<SparseMatrix<float>, COLAMDOrdering<int>> solver;

	cout << "begin makeCompressed()" << endl;
	coeff_mat.makeCompressed();

	cout << "begin compute()" << endl;
	solver.compute(coeff_mat);
	if (solver.info() != Eigen::Success) {
		cout << "solver: decomposition was not successful." << endl;
		return;
	}

	cout << "begin solve() for x" << endl;
	VectorXf res_x = solver.solve(b_vec_x);

	cout << "begin solve() for y" << endl;
	VectorXf res_y = solver.solve(b_vec_y);

	cout << "begin solve() for z" << endl;
	VectorXf res_z = solver.solve(b_vec_z);

	// Update vertex coordinates
	for (int i = 0; i < n; i++) {
		// find the corresponding point
		auto v = inner_points[i];
		vecf3 new_pos = { res_x(i), res_y(i), res_z(i) }; // works?

		//cout << new_pos << endl;
		v->pos = new_pos;
	}

}

void Ubpa::ARAP::print(Ubpa::ARAP::V* v) {
	cout << "V: (" << v->pos[0] << ", " << v->pos[1] << ", " << v->pos[2] << ")";
}
void Ubpa::ARAP::print(Ubpa::ARAP::P* p) {
	if (p == nullptr) {
		cout << "P: nullptr" << endl;
		return;
	}
	cout << "P: [";
	print(p->BoundaryVertice()[0]);
	cout << ", ";
	print(p->BoundaryVertice()[1]);
	cout << ", ";
	print(p->BoundaryVertice()[2]);
	cout << "]" << endl;
}

void Ubpa::ARAP::print(pointf2& v) {
	cout << "V: (" << v[0] << ", " << v[1] << ")";
}

void Ubpa::ARAP::print(std::array<pointf2, 3>& p) {
	cout << "P: [";
	print(p[0]);
	cout << ", ";
	print(p[1]);
	cout << ", ";
	print(p[2]);
	cout << "]" << endl;
}

double Ubpa::ARAP::calcEnergy(vector<Matrix2f> L_t)
{
	double energy = 0;

	auto polys = heMesh->Polygons();
	for (int poly_id = 0; poly_id < polys.size(); poly_id++) {
		if (log_verbosity > 50) {
			print(polys[poly_id]);
		}

		auto hes = polys[poly_id]->BoundaryHEs();
		for (auto he : hes) {

			double cotij;
			Vector2f x_vec;
			std::tie(x_vec, cotij, std::ignore) = calcXVec(heMesh->Index(he->Origin()), heMesh->Index(he->End()));

			if (log_verbosity > 50) {
				cout << x_vec << endl;
			}

			Vector2f u_vec = { he->Origin()->pos[0] - he->End()->pos[0] , he->Origin()->pos[1] - he->End()->pos[1] };

			double energy_component = cotij * (u_vec - L_t[poly_id] * x_vec).squaredNorm();
			if (log_verbosity > 50) {
				printf("* energy += %lf\n", energy_component);
			}
			energy += energy_component;
		}
	}

	energy /= 2;

	return energy;
}

void Ubpa::ARAP::DoARAP()
{
	cout << "DoARAP() called." << endl;

	// initialize 2d mapping
	UniformPara();
	//for (auto v : heMesh->Vertices()) {
	//	v->pos[2] = 0;
	//}

	for (auto v : heMesh->Vertices()) {
		assert(v->pos[2] == 0);
	}

	// envvars
	int total_polys = heMesh->NumPolygons();
	auto polys = heMesh->Polygons();
	auto polys_orig = heMesh_orig->Polygons();

	// initialize L_t
	vector<Matrix2f> L_t;
	for (int i = 0; i < total_polys; i++) {
		L_t.push_back(Eigen::Matrix2f::Identity());
		//L_t.push_back(Eigen::Matrix2f::Zero());
	}

	printf("Start with energy=%lf\n", calcEnergy(L_t));

	for (int iter_count = 0; iter_count < iter_n; iter_count++) {
		{
			// local phase, update L_t
			for (int poly_id = 0; poly_id < total_polys; poly_id++) {
				// calculate S_t(u) in place of J_t(u)
				Matrix2f S_u = Eigen::Matrix2f::Zero();

				auto verts = polys[poly_id]->BoundaryVertice();
				auto verts_orig = polys_orig[poly_id]->BoundaryVertice();
				// todo assuming counterclockwise
				// actually okay for both

				for (int i = 0; i < 3; i++) {
					// cot = cos / sin
					int dest_angle = (i + 2) % 3;
					double cos_theta;
					auto embed = genEmbed(verts_orig[0]->pos.cast_to<pointf3>()
						, verts_orig[1]->pos.cast_to<pointf3>()
						, verts_orig[2]->pos.cast_to<pointf3>());

					if (dest_angle == 0) {
						cos_theta = (embed[2] - embed[0]).cos_theta(embed[1] - embed[0]);
					}
					else if (dest_angle == 1) {
						cos_theta = (embed[0] - embed[1]).cos_theta(embed[2] - embed[1]);
					}
					else {
						cos_theta = (embed[0] - embed[2]).cos_theta(embed[1] - embed[2]);
					}

					double cot_theta = cos_theta / (sqrt(1 - cos_theta * cos_theta) + 1e-5);  // todo check 1e-5

					Vector2f u_vec = { verts[i]->pos[0] - verts[(i + 1) % 3]->pos[0] , verts[i]->pos[1] - verts[(i + 1) % 3]->pos[1] };
					Vector2f x_vec = { embed[i][0] - embed[(i + 1) % 3][0] , embed[i][1] - embed[(i + 1) % 3][1] };

					S_u += cot_theta * (u_vec * x_vec.transpose());
				}

				JacobiSVD<Matrix2f> svd(S_u, ComputeFullU | ComputeFullV);
				assert(svd.singularValues()(0) >= 0 && svd.singularValues()(1) >= 0);

				// the ARAP case
				Matrix2f res = svd.matrixU() * svd.matrixV().transpose();
				L_t[poly_id] = res;
			}
		}
		printf("[%d] local iteration done, energy=%lf\n", iter_count, calcEnergy(L_t));
		// global iteration
		{
			// anchors containing vertices index
			std::set<int> anchors;
			//anchors.insert(0);
			//anchors.insert(1);

			int total_vertices = heMesh->NumVertices();
			int vertices_unknown = total_vertices - anchors.size();
			//assert(total_polys == vertices_unknown + 2);

			std::unordered_map<int, int> index_map;
			{
				int cur_row = 0;
				for (int i = 0; i < total_vertices; i++) {
					if (anchors.count(i) > 0) {
						continue;
					}
					else {
						index_map[i] = cur_row;
						cur_row++;
					}
				}
				assert(cur_row == vertices_unknown);
			}

			Eigen::SparseMatrix<float> coeff_mat(vertices_unknown, vertices_unknown);
			Eigen::VectorXf b_vec_x = VectorXf::Zero(vertices_unknown);
			Eigen::VectorXf b_vec_y = VectorXf::Zero(vertices_unknown);
			coeff_mat.setZero();

			int current_row = 0;
			for (int i = 0; i < total_vertices; i++) {
				if (anchors.count(i) > 0) {
					continue;
				}

				// prepare coefficient
				// calculate cos_theta_ij
				auto v = heMesh->Vertices()[i];
				//cout << "Processing vertice: " << v << endl;

				for (auto adjv : v->AdjVertices()) {
					int j = heMesh->Index(adjv);

					//auto he_ji = v->HalfEdgeTo(adjv);
					//auto he_ij = adjv->HalfEdgeTo(v);
					auto he_ji = adjv->HalfEdgeTo(v);
					auto he_ij = v->HalfEdgeTo(adjv);

					auto tri_ji = he_ji->Polygon();
					auto tri_ij = he_ij->Polygon();

					double ctg_ij, ctg_ji;

					if (log_verbosity > 50) {
						cout << "* i: ";
						print(v);
						cout << endl;

						cout << "* j: ";
						print(adjv);
						cout << endl;
					}

					std::tie(std::ignore, ctg_ij, ctg_ji) = calcXVec(heMesh->Index(v), heMesh->Index(adjv));

					if (log_verbosity > 50) {
						printf("* ctg_ij=%lf , ctg_ji=%lf\n", ctg_ij, ctg_ji);
					}

					// the u stuff
					if (anchors.count(j) > 0) {
						// put into b
						b_vec_x(current_row) += (ctg_ij + ctg_ji) * adjv->pos[0];
						b_vec_y(current_row) += (ctg_ij + ctg_ji) * adjv->pos[1];
					}
					else {
						coeff_mat.coeffRef(current_row, index_map[j]) += -(ctg_ij + ctg_ji);
					}

					coeff_mat.coeffRef(current_row, index_map[i]) += (ctg_ij + ctg_ji);

					// the x stuff

					Vector2f x_vec_ij, x_vec_ji;
					std::tie(x_vec_ij, x_vec_ji) = calcXVecFull(heMesh->Index(v), heMesh->Index(adjv));

					Vector2f rhs = Vector2f::Zero();

					if (tri_ij != nullptr) {
						rhs += ctg_ij * L_t[heMesh->Index(he_ij->Polygon())] * x_vec_ij;
					}

					if (tri_ji != nullptr) {
						rhs += ctg_ji * L_t[heMesh->Index(he_ji->Polygon())] * x_vec_ji;
					}

					if (log_verbosity > 50) {
						cout << "rhs: " << endl;
						cout << rhs << endl;
					}

					b_vec_x(current_row) += rhs(0);
					b_vec_y(current_row) += rhs(1);
				}
				current_row++;
			}

			// Solve
			SparseQR<SparseMatrix<float>, COLAMDOrdering<int>> solver;

			cout << "begin makeCompressed()" << endl;
			coeff_mat.makeCompressed();

			if (log_verbosity > 50) {
				cout << coeff_mat << endl;
			}

			cout << "begin compute()" << endl;
			solver.compute(coeff_mat);
			if (solver.info() != Eigen::Success) {
				cout << "solver: decomposition was not successful." << endl;
				return;
			}

			if (log_verbosity > 50) {
				cout << "b_vec_x: " << endl;
				cout << b_vec_x << endl;

				cout << "b_vec_y: " << endl;
				cout << b_vec_y << endl;
			}

			VectorXf x_res = solver.solve(b_vec_x);
			cout << "error: " << (coeff_mat * x_res - b_vec_x).squaredNorm() << " (x_res)" << endl;

			VectorXf y_res = solver.solve(b_vec_y);
			cout << "error: " << (coeff_mat * y_res - b_vec_y).squaredNorm() << " (y_res)" << endl;;

			// write back
			for (int i = 0; i < total_vertices; i++) {
				if (anchors.count(i) > 0) {
					continue;
				}
				else {
					heMesh->Vertices()[i]->pos = { x_res(index_map[i]), y_res(index_map[i]), 0 };
				}
			}

		}
		printf("[%d] global iteration done, energy=%lf\n", iter_count, calcEnergy(L_t));
	}
}

bool Ubpa::ARAP::Run()
{
	if (heMesh->IsEmpty() || !triMesh) {
		printf("ERROR::ARAP::Run\n"
			"\t""heMesh->IsEmpty() || !triMesh\n");
		return false;
	}

	DoARAP();

	// half-edge structure -> triangle mesh
	size_t nV = heMesh->NumVertices();
	size_t nF = heMesh->NumPolygons();
	vector<pointf3> positions;
	vector<unsigned> indice;
	positions.reserve(nV);
	indice.reserve(3 * nF);
	for (auto v : heMesh->Vertices())
		positions.push_back(v->pos.cast_to<pointf3>());
	for (auto f : heMesh->Polygons()) { // f is triangle
		for (auto v : f->BoundaryVertice()) // vertices of the triangle
			indice.push_back(static_cast<unsigned>(heMesh->Index(v)));
	}

	triMesh->Init(indice, positions);

	//vector<pointf2> texcoords;
	//for (auto v : heMesh->Vertices())
	//	texcoords.push_back(v->pos.cast_to<pointf2>());

	//triMesh->Update(texcoords);

	return true;
}


#pragma once

#include <string>
#include <vector>
#include <sstream>
#include <initializer_list>
#include <boost/optional.hpp>

#include "Coefficient.hpp"
#include "Condition.hpp"

class Formula
{
	private:
		boost::optional<double>					result;
		std::vector<Coefficient>				summands;

	public:
		Formula(std::initializer_list<Coefficient> = {});
		Formula(int);

		void									derive();
		void									derive(int);
		void									insert(double, double);
		void									insert(Condition&);
		double									getNumericalFactor(unsigned int);
		void									clean();
		std::string								asString();
		double									getResult();
};


#ifndef BOOST_TEST_DYN_LINK
#define BOOST_TEST_DYN_LINK
#endif

#include <sstream>

#include <boost/test/unit_test.hpp>

#include "../simulation/point.hpp"

namespace PointTest
{

BOOST_AUTO_TEST_CASE( position )
{
    Point p(0,0);
    BOOST_CHECK_EQUAL( p.posX(), 0 );
    BOOST_CHECK_EQUAL( p.posY(), 0 );

    Point q(123,456);
    BOOST_CHECK_EQUAL( q.posX(), 123 );
    BOOST_CHECK_EQUAL( q.posY(), 456 );

    BOOST_CHECK( q!=p );

    p.setPosY(q.posY());
    BOOST_CHECK_EQUAL( p.posX(), 0 );
    BOOST_CHECK_EQUAL( p.posY(), 456 );

    p.setPosX(q.posX());
    BOOST_CHECK_EQUAL( p.posX(), 123 );
    BOOST_CHECK_EQUAL( p.posY(), 456 );

    BOOST_CHECK( q==p );
}

BOOST_AUTO_TEST_CASE(test_isInBounds)
{
    Point p(10, 10);
    BOOST_CHECK_EQUAL(p.isInBounds(15, 15), true);
    BOOST_CHECK_EQUAL(p.isInBounds(11, 11), true);
    BOOST_CHECK_EQUAL(p.isInBounds(10, 10), false);
    BOOST_CHECK_EQUAL(p.isInBounds(15, 5), false);
    BOOST_CHECK_EQUAL(p.isInBounds(5, 15), false);
    BOOST_CHECK_EQUAL(p.isInBounds(5, 5), false);

    BOOST_CHECK_EQUAL(p.isInBounds(Point(15, 15)), true);
    BOOST_CHECK_EQUAL(p.isInBounds(Point(5, 15)), false);
    BOOST_CHECK_EQUAL(p.isInBounds(Point(15, 5)), false);
    BOOST_CHECK_EQUAL(p.isInBounds(Point(5, 5)), false);

    BOOST_CHECK_EQUAL(p.isInBounds(Point(5, 5), Point(15, 15)), true);
    BOOST_CHECK_EQUAL(p.isInBounds(Point(13, 5), Point(15, 15)), false);
    BOOST_CHECK_EQUAL(p.isInBounds(Point(5, 13), Point(15, 15)), false);
    BOOST_CHECK_EQUAL(p.isInBounds(Point(5, 5), Point(8, 15)), false);
    BOOST_CHECK_EQUAL(p.isInBounds(Point(5, 5), Point(15, 8)), false);
}

BOOST_AUTO_TEST_CASE(test_operators)
{
    Point p(2, 2), q(5, 5);
    BOOST_CHECK_EQUAL(Point(7, 7), p + q);
    BOOST_CHECK_EQUAL(Point(-3, -3), p - q);
    BOOST_CHECK_EQUAL(20, p * q);
    BOOST_CHECK_EQUAL(Point(6, 6), p * 3);

    p += q;
    BOOST_CHECK_EQUAL(Point(7, 7), p);
    q -= p;
    BOOST_CHECK_EQUAL(Point(-2, -2), q);

    p *= 2;
    BOOST_CHECK_EQUAL(Point(14, 14), p);

    BOOST_CHECK(p != q);
    BOOST_CHECK(!(p == q));
}

BOOST_AUTO_TEST_CASE(test_distance)
{
    Point p(2, 2), q(4, 2);
    BOOST_CHECK_CLOSE(p.getDistance(q), 2, 0.00001);
}

BOOST_AUTO_TEST_CASE(test_isAdjacent)
{
    Point p(5, 5);
    BOOST_CHECK(p.isAdjacent(Point(5, 5)));

    BOOST_CHECK(p.isAdjacent(Point(4, 5)));
    BOOST_CHECK(p.isAdjacent(Point(6, 5)));
    BOOST_CHECK(p.isAdjacent(Point(5, 4)));
    BOOST_CHECK(p.isAdjacent(Point(5, 6)));

    BOOST_CHECK(!p.isAdjacent(Point(4, 4)));
    BOOST_CHECK(!p.isAdjacent(Point(6, 6)));
    BOOST_CHECK(!p.isAdjacent(Point(4, 6)));
    BOOST_CHECK(!p.isAdjacent(Point(6, 4)));
}

BOOST_AUTO_TEST_CASE(test_getGridDistance)
{
    Point p(2, 2), q(4, 4);
    BOOST_CHECK_EQUAL(p.getGridDistance(q), 4);
}

BOOST_AUTO_TEST_CASE(test_toString)
{
    Point p(2, 2);
    BOOST_CHECK_EQUAL(p.toString(), "2,2");
}

BOOST_AUTO_TEST_CASE(test_toStringStream)
{
    Point p(2, 2);
    std::stringstream test_stream;
    test_stream << p;
    BOOST_CHECK_EQUAL(test_stream.str(), "2,2");
}

BOOST_AUTO_TEST_CASE(test_clamped)
{
    Point c1(5, 5), c2(15, 15);
    BOOST_CHECK_EQUAL(Point(10, 10).clamped(c1, c2), Point(10, 10));

    BOOST_CHECK_EQUAL(Point(20, 10).clamped(c1, c2), Point(15, 10));
    BOOST_CHECK_EQUAL(Point(0, 10).clamped(c1, c2), Point(5, 10));
    BOOST_CHECK_EQUAL(Point(10, 20).clamped(c1, c2), Point(10, 15));
    BOOST_CHECK_EQUAL(Point(10, 0).clamped(c1, c2), Point(10, 5));

    BOOST_CHECK_EQUAL(Point(0, 0).clamped(c1, c2), Point(5, 5));
    BOOST_CHECK_EQUAL(Point(20, 20).clamped(c1, c2), Point(15, 15));
    BOOST_CHECK_EQUAL(Point(0, 20).clamped(c1, c2), Point(5, 15));
    BOOST_CHECK_EQUAL(Point(20, 0).clamped(c1, c2), Point(15, 5));
}

} // namespace PointTest


//
// Created by jakub on 21.2.2020.
//

#include <amr_planner/GraphSearchBoost.h>
#include <boost/graph/astar_search.hpp>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/property_map/property_map.hpp>
#include <exception>

using namespace boost;

struct do_nothing_dijkstra_visitor : boost::default_dijkstra_visitor {};

GraphSearchBoost::GraphSearchBoost(const Graph& graph, GraphSearchBoost::SearchMethod searchMethod)
        : GraphSearchInterface(graph),
          searchMethod(searchMethod) {

}

std::vector<Node> GraphSearchBoost::getPath(const Node& nStart, const Node& nEnd) {

//    graph_t boost_graph = graph.getBoostGraph();
//    std::vector<vertex_t> p(boost::num_vertices(boost_graph));
//    std::vector<int> d(boost::num_vertices(boost_graph));
//
//
//    vertex_t startVertex;
//    if (!graph.findVertex(nStart, startVertex)) {
//        throw std::invalid_argument("Start Node is not in graph.");
//    }
//
//
//    boost::dijkstra_shortest_paths(boost_graph, startVertex,
//            boost::predecessor_map(boost::make_iterator_property_map(p.begin(), boost::get(boost::vertex_index, boost_graph)))
//            .distance_map(boost::make_iterator_property_map(d.begin(), boost::get(boost::vertex_index, boost_graph))));
//
//
//
//

//    graph_t boost_graph = graph.getBoostGraph();
//    std::vector<vertex_t> p(num_vertices(boost_graph));
//    std::vector<int> d(num_vertices(boost_graph));
//    vertex_t startVertex;
//    if (!graph.findVertex(nStart, startVertex)) {
//        throw std::invalid_argument("Start Node is not in graph.");
//    }
//
//    totalDist = 0;
//    std::cout << "\nPlease enter the location name to start from: ";
//    std::cin >> tempName1;
//    transform(tempName1.begin(), tempName1.end(), tempName1.begin(), ::toupper);
//    std::cout << "\nPlease enter the location name for the destination: ";
//    std::cin >> tempName2;
//    transform(tempName2.begin(), tempName2.end(), tempName2.begin(), ::toupper);
//
//    start_v = name2v[tempName1];
//    goal_v = name2v[tempName2];
//

//    dijkstra_shortest_paths(boost_graph, startVertex, get(&Node::predecessor, boost_graph), get(&Edge::distance, boost_graph),
//                            get(&Edge::distance, boost_graph), boost::identity_property_map(), std::less<double>(),
//                            std::plus<double>(), std::numeric_limits<double>::infinity(), 0.0,
//                            do_nothing_dijkstra_visitor(), get(&Node::color, boost_graph));
//    std::cout << "\n";
//    std::cout << "Shortest Path From " << tempName1 << " to " << tempName2 << ": \n";
//    cur_v = start_v;
//    while (cur_v != goal_v) {
//        std::cout << "(" << g[cur_v].name << ", ";
//        totalDist += g[cur_v].distance;
//        tempDist = g[cur_v].distance;
//        cur_v = g[cur_v].predecessor;
//        std::cout << g[cur_v].name << ")\n";
//        totalDist -= g[cur_v].distance;
//    };
//    std::cout << "Total Weight: " << totalDist << "\n";
//    name2v = name1v;
//    break;
//

//    graph_t boost_graph = graph.getBoostGraph();
//
//    std::vector<vertex_t> p(num_vertices(boost_graph));
//    std::vector<int> d(num_vertices(boost_graph));
//
//
//    vertex_t startVertex;
//    if (!graph.findVertex(nStart, startVertex)) {
//        throw std::invalid_argument("Start Node is not in graph.");
//    }
//
//    dijkstra_shortest_paths(boost_graph, startVertex, //
////             predecessor_map(&p[0]).distance_map(&d[0])
//                            predecessor_map(boost::make_iterator_property_map(p.begin(), get(boost::vertex_index, boost_graph))).
//                            distance_map(boost::make_iterator_property_map(d.begin(), get(boost::vertex_index, boost_graph)))
//
//                            );
//    graph_traits < graph_t >::vertex_iterator vi, vend;
//    for (boost::tie(vi, vend) = vertices(boost_graph); vi != vend; ++vi) {
//        std::cout << "distance(" << boost_graph[*vi].uuid << ") = " << d[*vi] << ", ";
//        std::cout << "parent(" << boost_graph[*vi].uuid << ") = " << p[*vi] << std::
//        endl;
//    }
//
//

//    astar_search_tree(
//            graph.getBoostGraph(),
//            startVertex,
//
//                      distance_heuristic<graph_t, cost, location*>(locations, goal),
//                      predecessor_map(make_iterator_property_map(p.begin(), get(vertex_index, g))).
//                              distance_map(make_iterator_property_map(d.begin(), get(vertex_index, g))).
//                              visitor(astar_goal_visitor<vertex>(goal)));

}


#include "cloud_map.h"
#include "eigen_gpu.h"
#include "gpu.h"
#include <iomanip>

#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#include <Eigen/Eigen>
#include <Eigen/Sparse>

/** Results: GPU vs CPU

> time ./learn nopgu gg new \
    -d data/gloves/big.cloud \
    -c data/gloves/big.cloud \
    -o data/gloves/big-big.map

  real	62m1.322s
  user	208m1.570s
  sys	0m6.200s

> time ./learn gg new \
    -d data/gloves/big.cloud \
    -c data/gloves/big.cloud \
    -o data/gloves/big-big.map

  real	25m13.199s
  user	132m19.040s
  sys	0m18.100s
  XXX these are with HACK_TO_LIMIT_GPU_POWER_USAGE
  XXX with no such hack, GPU is about 2x faster

*/

#define LOG1(message)

namespace Cloud
{

MapOptimizer::MapOptimizer (const JointPrior & d, const JointPrior & c)
  : dom(d.dom),
    cod(c.dom),
    dom_flow(d),
    cod_flow(c),

    m_dom_flow_sum(dom_flow.joint.sum()),
    m_cod_flow_sum(cod_flow.joint.sum()),

    m_dom_prior(int(dom.size)),
    m_cod_prior(int(cod.size)),
    m_conditional(int(cod.size), int(dom.size)),
    m_joint(int(cod.size), int(dom.size)),

    m_temp_dom(int(dom.size)),
    m_temp_cod(int(cod.size)),

    // m_temp_dQ & cod_flow.joint have the same sparsity pattern
    m_temp_dQ(cod_flow.joint),
    m_temp_dQt(cod_flow.joint.transpose()),

    m_P(NULL),
    m_Pt(NULL),

    debug(false),
    logging(false)
{
  LOG("Building MapOptimizer : " << dom.size << " -> " << cod.size);
  dom.get_prior(m_dom_prior);
  cod.get_prior(m_cod_prior);

  ASSERT_LT(0, m_dom_prior.minCoeff());
  ASSERT_LT(0, m_cod_prior.minCoeff());

  const size_t X = dom.size;
  const size_t Y = cod.size;

  if (X > Y) m_cod_prior.array() *= float(X) / Y;
  if (Y > X) m_dom_prior.array() *= float(Y) / X;

  float tol = 1e-4f;
  ASSERT_LT(sqr(m_dom_prior.sum() - m_cod_prior.sum()), tol);

  PRINT2(density(m_dom_prior), density(m_cod_prior));
  PRINT2(density(dom_flow.joint), density(cod_flow.joint));
  PRINT2(dom_flow.joint.nonZeros(), cod_flow.joint.nonZeros());
}

MapOptimizer::~MapOptimizer ()
{
  if (m_P) delete m_P;
  if (m_Pt) delete m_Pt;
}

void MapOptimizer::update_joint ()
{
  const size_t Y = cod.size;

  const MatrixXf & H = m_conditional;
  MatrixXf & J = m_joint;
  const VectorXf & p = m_dom_prior;

  J = H.cwiseProduct(MatrixXf::Ones(Y,1) * p.transpose());
}

void MapOptimizer::update_conditional ()
{
  MatrixXf & H = m_conditional;
  const MatrixXf & J = m_joint;

  H = J;
  normalize_columns_l1(H);
}

void MapOptimizer::init (const MatrixXf & joint, float tol)
{
  const size_t X = dom.size;
  const size_t Y = cod.size;

  ASSERT_EQ(joint.rows(), int(Y));
  ASSERT_EQ(joint.cols(), int(X));

  m_joint = joint;

  constrain_joint(tol);
  update_conditional();
}

void MapOptimizer::init (const MatrixSf & joint, float tol)
{
  const size_t X = dom.size;
  const size_t Y = cod.size;

  ASSERT_EQ(joint.rows(), int(Y));
  ASSERT_EQ(joint.cols(), int(X));

  m_joint = joint;

  constrain_joint(tol);
  densify_joint(tol);
  constrain_joint(tol);

  update_conditional();
}

void MapOptimizer::init_random (float tol)
{
  const size_t XY = dom.size * cod.size;

  float * restrict J_ = & m_joint.coeffRef(0,0);

  for (size_t xy = 0; xy < XY; ++xy) {

    J_[xy] = expf(random_std());
  }

  constrain_joint(tol);
  update_conditional();
}

void MapOptimizer::add_noise (float tol)
{
  const size_t XY = dom.size * cod.size;

  float * restrict J_ = & m_joint.coeffRef(0,0);

  for (size_t xy = 0; xy < XY; ++xy) {

    J_[xy] *= expf(random_std());
  }

  constrain_joint(tol);
  update_conditional();
}

void MapOptimizer::solve (MatrixXf & dense_out, float tol, size_t max_steps)
{
  ASSERT_EQ(dense_out.rows(), int(cod.size));
  ASSERT_EQ(dense_out.cols(), int(dom.size));

  LOG("Optimizing map for at most " << max_steps << " iterations");

  LOG("-------------"
      "-------------"
      "-------------");
  LOG( std::setw(13) << "relentropy"
    << std::setw(13) << "density(J)"
    << std::setw(13) << "stepsize");

  MatrixXf & J = m_joint;
  MatrixXf & dJ = dense_out; // used as temporary

  double relentropy = INFINITY;
  float stepsize = NAN;

  Timer timer;

  size_t step = 0;
  while (step < max_steps) {
    if (logging) LOG(" step " << step << "/" << max_steps);

    double new_relentropy = compute_relentropy(dJ);

    if (new_relentropy > relentropy) {
      WARN(" terminating due to objective function increase");
      break;
    }

    if (relentropy - new_relentropy < tol) {
      LOG(" map optimization converged to tolerance");
      break;
    }

    relentropy = new_relentropy;

    // should we be adaptively selecting stepsize?

    precondition_direction(dJ);
    constrain_direction(dJ, tol);
    scale_stepsize(dJ);
    stepsize = (dJ.array() / J.array()).square().sum();

    LOG( std::setw(13) << relentropy
      << std::setw(13) << density(J)
      << std::setw(13) << stepsize);

    J += dJ;

    densify_joint(tol);
    constrain_joint(tol);
    update_conditional();

    ++step;
  }

  LOG( std::setw(13) << compute_relentropy()
    << std::setw(13) << density(J)
    << std::setw(13) << "-");
  LOG("-------------"
      "-------------"
      "-------------");

  float time = timer.elapsed();
  LOG(" optimization took " << time << " sec = "
      << (time / step) << " sec/step x " << step << " steps");

  dense_out = m_joint;
}

void MapOptimizer::solve (MatrixSf & sparse_out, float tol, size_t max_steps)
{
  MatrixXf dense(int(cod.size), int(dom.size));

  solve(dense, tol, max_steps);

  size_t max_entries = max_entries_heuristic(dense);
  sparsify_hard_relative_to_row_col_max(dense, sparse_out, tol, max_entries);
}

// We minimize the relentropy between Q and the projection H P' H
//
//   KL(Q || H P H')
//                                           Q(y,y')
//       = sum y,y'. Q(y,y') log ---------------------------------
//                               sum x,x'. H(y,x) P(x,x') H(y',x')
//
//       = sum y,y'. Q(y,y') log dQ(y,y')
//
// using gradient descent with the derivative
//
//   -d/dH KL(Q || H P H') = dQ H P' + dQ' H P
//
// where dQ = Q ./ (H P H')

double MapOptimizer::compute_relentropy () const
{
  if (logging) LOG("  computing relentropy");

  return Gpu::using_cuda()
    ? compute_relentropy_gpu()
    : compute_relentropy_cpu();
}

double MapOptimizer::compute_relentropy (MatrixXf & dH) const
{
  if (logging) LOG("  computing relentropy & descent direction");

  return Gpu::using_cuda()
    ? compute_relentropy_gpu(dH)
    : compute_relentropy_cpu(dH);
}

double MapOptimizer::compute_relentropy_cpu () const
{
  const MatrixXf & H = m_conditional;
  const MatrixSf & P = dom_flow.joint;

  MatrixXf HP = H * P;
  MatrixXf HPH = HP * H.transpose();

  const float sum_P = m_dom_flow_sum;
  const float sum_Q = m_cod_flow_sum;
  const float dQ_scale = sum_P / sum_Q;

  const MatrixSf & Q = cod_flow.joint;

  double relentropy = 0;
  for (int i = 0; i < Q.outerSize(); ++i) {
    for (MatrixSf::InnerIterator iter(Q,i); iter; ++iter) {

      float Q_yy = iter.value();
      float HPH_yy = HPH(iter.row(), iter.col());

      relentropy += Q_yy * log(dQ_scale * Q_yy / HPH_yy);
    }
  }
  relentropy /= sum_Q;
  ASSERT_LE(0, relentropy);

  return relentropy;
}

double MapOptimizer::compute_relentropy_cpu (MatrixXf & dH) const
{
  const MatrixXf & H = m_conditional;
  const MatrixSf & P = dom_flow.joint;

  MatrixXf HP(H.rows(), H.cols());
  MatrixXf HPt(H.rows(), H.cols());

  #pragma omp parallel sections
  {
    #pragma omp section
    HP.noalias() = H * P;

    #pragma omp section
    HPt.noalias() = H * P.transpose();
  }

  MatrixXf HPH = HP * H.transpose();

  const float sum_P = m_dom_flow_sum;
  const float sum_Q = m_cod_flow_sum;
  const float dQ_scale = sum_P / sum_Q;

  const MatrixSf & Q = cod_flow.joint;
  MatrixSf & dQ = m_temp_dQ;

  double relentropy = 0;
  for (int i = 0; i < Q.outerSize(); ++i) {
    for (MatrixSf::InnerIterator iter(Q,i); iter; ++iter) {

      const float Q_yy = iter.value();
      const float HPH_yy = HPH(iter.row(), iter.col());
      const float dQ_yy = dQ_scale * Q_yy / HPH_yy;

      relentropy += Q_yy * log(dQ_yy);

      dQ.coeffRef(iter.row(), iter.col()) = dQ_yy;
    }
  }
  relentropy /= sum_Q;
  ASSERT_LE(0, relentropy);

  MatrixXf dH2(H.rows(), H.cols());

  #pragma omp parallel sections
  {
    #pragma omp section
    dH.noalias() = dQ.transpose() * HP;

    #pragma omp section
    dH2.noalias() = dQ * HPt;
  }

  dH += dH2;

  return relentropy;
}

void MapOptimizer::initialize_P_gpu () const
{
  const MatrixSf & P = dom_flow.joint;
  if (not m_P) {
    m_P = new Gpu::SparseMultiplier(P);
  }

  if (not m_Pt) {
    MatrixSf dPt = P.transpose();
    m_Pt = new Gpu::SparseMultiplier(dPt);
  }
}

double MapOptimizer::compute_relentropy_gpu () const
{
  initialize_P_gpu();

  const MatrixXf & H = m_conditional;

  MatrixXf PHt = H.transpose();
  m_Pt->left_imul(PHt, true);

  MatrixXf HPHt(H.rows(), H.rows());
#ifdef HACK_TO_LIMIT_GPU_POWER_USAGE
  HPHt.noalias() = H * PHt;
#else // HACK_TO_LIMIT_GPU_POWER_USAGE
  Gpu::matrix_multiply(H, false, PHt, false, HPHt);
#endif // HACK_TO_LIMIT_GPU_POWER_USAGE

  const float sum_P = m_dom_flow_sum;
  const float sum_Q = m_cod_flow_sum;
  const float dQ_scale = sum_P / sum_Q;

  const MatrixSf & Q = cod_flow.joint;

  double relentropy = 0;
  for (int i = 0; i < Q.outerSize(); ++i) {
    for (MatrixSf::InnerIterator iter(Q,i); iter; ++iter) {

      const float Q_yy = iter.value();
      const float HPH_yy = HPHt(iter.row(), iter.col());
      const float dQ_yy = dQ_scale * Q_yy / HPH_yy;

      relentropy += Q_yy * log(dQ_yy);
    }
  }
  relentropy /= sum_Q;
  ASSERT_LE(0, relentropy);

  return relentropy;
}

double MapOptimizer::compute_relentropy_gpu (MatrixXf & dH) const
{
  initialize_P_gpu();

  const MatrixXf & H = m_conditional;

  MatrixXf PHt = H.transpose();
  MatrixXf PtHt = PHt;

  //TODO run cusparse in two different cuda streams
  m_P->left_imul(PtHt, true);
  m_Pt->left_imul(PHt, true);

  MatrixXf HPHt(H.rows(), H.rows());
#ifdef HACK_TO_LIMIT_GPU_POWER_USAGE
  HPHt.noalias() = H * PHt;
#else // HACK_TO_LIMIT_GPU_POWER_USAGE
  Gpu::matrix_multiply(H, false, PHt, false, HPHt);
#endif // HACK_TO_LIMIT_GPU_POWER_USAGE

  const float sum_P = m_dom_flow_sum;
  const float sum_Q = m_cod_flow_sum;
  const float dQ_scale = sum_P / sum_Q;

  const MatrixSf & Q = cod_flow.joint;
  MatrixSf & dQ = m_temp_dQ;
  MatrixSf & dQt = m_temp_dQt;

  double relentropy = 0;
  for (int i = 0; i < Q.outerSize(); ++i) {
    for (MatrixSf::InnerIterator iter(Q,i); iter; ++iter) {

      const float Q_yy = iter.value();
      const float HPH_yy = HPHt(iter.row(), iter.col());
      const float dQ_yy = dQ_scale * Q_yy / HPH_yy;

      relentropy += Q_yy * log(dQ_yy);

      dQ.coeffRef(iter.row(), iter.col()) = dQ_yy;
      dQt.coeffRef(iter.col(), iter.row()) = dQ_yy;
    }
  }
  relentropy /= sum_Q;
  ASSERT_LE(0, relentropy);

  MatrixXf HP(H.rows(), H.cols());
  MatrixXf HPt(H.rows(), H.cols());

  #pragma omp sections
  {
    #pragma omp section
    HP = PtHt.transpose();

    #pragma omp section
    HPt = PHt.transpose();
  }

  //TODO run cusparse in two different cuda streams
  Gpu::SparseMultiplier(dQ).left_mul(HP, dH, true);
  Gpu::SparseMultiplier(dQt).left_fma(HPt, dH, true);

  return relentropy;
}

void MapOptimizer::precondition_direction (MatrixXf & dH) const
{
  if (logging) LOG("  preconditioning direction");

  // Transform descent direction from H(y,x) coords to log(H(y,x)) coords,
  // which are equivalent to log(J(y,x)) coords (so hereafter dH = dJ).

  const MatrixXf & H = m_conditional;

  dH.array() *= H.array();
}

void MapOptimizer::constrain_direction (MatrixXf & dJ, float tol) const
{
  // Enforce the simultaneous constraints
  //
  //   /\x. sum y. dJ(y,x) = 0
  //   /\y. sum x. dJ(y,x) = 0
  //
  // We combine the two constraints by iteratively weakly enforcing both:
  // Let Px,Py project to the feasible subspaces for constraints 1,2, resp.
  // Each projection has eigenvalues in {0,1}.
  // We approximate the desired projection Pxy as a linear combination of Px,Py
  //   Pxy' = 1 - alpha ((1-Px) + (1-Py))
  // which has eigenvalues in {1} u [1 - alpha, 1 - 2 alpha].
  // Hence Pxy = lim n->infty Pxy'^n, where convergence rate depends on alpha.
  // The optimal alpha is 2/3, yielding Pxy' eigenvalues in {1} u [-1/3,1/3],
  // and resulting in project_scale = -alpha below.

  if (logging) LOG("  constraining direction");

  const size_t X = dom.size;
  const size_t Y = cod.size;

  const MatrixXf & J = m_joint;
  const VectorXf & sum_y_J = m_dom_prior;
  const VectorXf & sum_x_J = m_cod_prior;
  const float sum_xy_J = m_cod_prior.sum();

  VectorXf sum_y_dJ(J.cols());
  VectorXf sum_x_dJ(J.rows());

  // this is iterative, so we hand-optimize by merging loops

  const float * restrict J_ = J.data();
  const float * restrict sum_y_J_ = sum_y_J.data();
  const float * restrict sum_x_J_ = sum_x_J.data();

  float * restrict dJ_ = dJ.data();
  float * restrict project_y_ = sum_y_dJ.data();
  float * restrict project_x_ = sum_x_dJ.data();
  const float project_scale = -2/3.0;

  Vector<float> accum_x_dJ(Y);
  float * restrict accum_x_dJ_ = accum_x_dJ;

  // accumulate first projection
  accum_x_dJ.zero();

  for (size_t x = 0; x < X; ++x) {

    const float * restrict dJ_x_ = dJ_ + Y * x;

    float accum_y_dJ = 0;

    for (size_t y = 0; y < Y; ++y) {

      float dJ_xy = dJ_x_[y];

      accum_y_dJ += dJ_xy;
      accum_x_dJ_[y] += dJ_xy;
    }

    project_y_[x] = project_scale * accum_y_dJ / sum_y_J_[x];
  }

  for (size_t y = 0; y < Y; ++y) {
    project_x_[y] = project_scale * accum_x_dJ_[y] / sum_x_J_[y];
    accum_x_dJ_[y] = 0;
  }

  // apply previous projection and accumulate next projection
  for (size_t iter = 0; iter < 100; ++iter) {

    float error = 0;

    for (size_t x = 0; x < X; ++x) {

      const float * restrict J_x_ = J_ + Y * x;
      float * restrict dJ_x_ = dJ_ + Y * x;

      float accum_y_dJ = 0;

      for (size_t y = 0; y < Y; ++y) {

        float dJ_xy = dJ_x_[y] += J_x_[y] * (project_x_[y] + project_y_[x]);

        accum_y_dJ += dJ_xy;
        accum_x_dJ_[y] += dJ_xy;
      }

      project_y_[x] = project_scale * accum_y_dJ / sum_y_J_[x];
      imax(error, max(-accum_y_dJ, accum_y_dJ));
    }

    for (size_t y = 0; y < Y; ++y) {

      float accum_x_dJ_y = accum_x_dJ_[y];
      accum_x_dJ_[y] = 0;

      project_x_[y] = project_scale * accum_x_dJ_y / sum_x_J_[y];
      imax(error, max(-accum_x_dJ_y, accum_x_dJ_y));
    }

    if (error < tol) {
      if (logging) {
        LOG("   after " << (1+iter) << " iterations, error < " << error);
      }
      break;
    }
  }

  // apply final projection
  for (size_t x = 0; x < X; ++x) {

    const float * restrict J_x_ = J_ + Y * x;
    float * restrict dJ_x_ = dJ_ + Y * x;

    for (size_t y = 0; y < Y; ++y) {
      dJ_x_[y] += J_x_[y] * (project_x_[y] + project_y_[x]);
    }
  }

  if (debug) {

    sum_y_dJ = dJ.colwise().sum();
    sum_x_dJ = dJ.rowwise().sum();
    float sum_xy_dJ = sum_x_dJ.sum();

    DEBUG("max constraint errors = "
        << sqrt(sum_x_dJ.array().square().maxCoeff())<< ", "
        << sqrt(sum_y_dJ.array().square().maxCoeff())<< ", "
        << sum_xy_dJ);

    sum_y_dJ.array() /= sum_y_J.array();
    sum_x_dJ.array() /= sum_x_J.array();
    sum_xy_dJ /= sum_xy_J;

    DEBUG("max relative constraints errors = "
        << sqrt(sum_x_dJ.array().square().maxCoeff()) << ", "
        << sqrt(sum_y_dJ.array().square().maxCoeff()) << ", "
        << sum_xy_dJ);

    DEBUG("max(|dJ|) = " << dJ.array().abs().maxCoeff()
        << ", rms(dJ) = " << sqrt(dJ.array().square().mean()));
    DEBUG("max(J) / min(J) = " << (J.maxCoeff() / J.minCoeff()));
    DEBUG("max(sum x. J) / min(sum x. J) = "
        << (sum_x_J.maxCoeff() / sum_x_J.minCoeff()));
    DEBUG("max(sum y. J) / min(sum y. J) = "
        << (sum_y_J.maxCoeff() / sum_y_J.minCoeff()));
  }
}

void MapOptimizer::scale_stepsize (MatrixXf & dJ) const
{
  if (logging) LOG("  computing stepsize");

  const size_t XY = dom.size * cod.size;

  const float * restrict J_ = m_joint.data();
  const float * restrict dJ_ = dJ.data();

  float time_to_boundary = INFINITY;

  for (size_t xy = 0; xy < XY; ++xy) {

    float dJ_xy = dJ_[xy];
    if (dJ_xy < 0) {

      imin(time_to_boundary, -J_[xy] / dJ_xy);
    }
  }

  float keepaway = 1 / M_E;
  float scale = time_to_boundary * keepaway;
  if (logging) LOG("   scaling step size by " << scale);

  dJ *= scale;
}

void MapOptimizer::densify_joint (float tol)
{
  // add a completely uniform prior just below the sparsify threshold

  if (logging) LOG("  bounding joint probabilities away from zero");

  MatrixXf & J = m_joint;
  VectorXf dom_thresh(J.cols());
  VectorXf cod_thresh(J.rows());

  dom_thresh = J.colwise().maxCoeff();
  cod_thresh = J.rowwise().maxCoeff();

  const size_t X = dom.size;
  const size_t Y = cod.size;

  const float * restrict dom_thresh_ = dom_thresh.data();
  const float * restrict cod_thresh_ = cod_thresh.data();
  const float * restrict dom_prior_ = m_dom_prior.data();
  const float * restrict cod_prior_ = m_cod_prior.data();

  float scale = INFINITY;
  for (size_t x = 0; x < X; ++x) {
    for (size_t y = 0; y < Y; ++y) {

      float thresh = min(dom_thresh_[x], cod_thresh_[y]);
      float prior = dom_prior_[x] * cod_prior_[y];

      imin(scale, thresh / prior);
    }
  }

  scale *= 0.1f * tol;
  ASSERT_LT(0, scale);

  J += scale * m_cod_prior * m_dom_prior.transpose();
}

void MapOptimizer::constrain_joint (float tol)
{
  constrain_marginals_bp(
      m_joint,
      m_dom_prior,
      m_cod_prior,
      m_temp_dom,
      m_temp_cod,
      tol);
}

} // namespace Cloud


#include <boost/program_options.hpp>
#include <cmath>
#include <algorithm>
#include "flpmc.h"
#include "simrank.h"
using namespace std;
using namespace boost::program_options;

FLPMC::FLPMC(string g_name_, DirectedG &g_, double c_, double epsilon_, double delta_, size_t Q_):
    g_name(g_name_),
    g(&g_),
    c(c_),
    epsilon(epsilon_),
    delta(delta_),
    Q(Q_){
        // init local push
        size_t n = num_vertices(*g);
        cout << format("r_max: %s") % get_rmax() << endl;
        // lp = new Reduced_LocalPush(*g, g_name, c, get_lp_epsilon(), n);
        lp = new Full_LocalPush(*g, g_name, c, get_lp_epsilon(), n);
        if(!lp_file_exists(g_name,c,get_lp_epsilon(),n, true)){ // test wether the local push index exists
            cout << "local push offline index doesn't exists.. " << endl;
            lp->local_push(*g);
            cout << format("building compete, saving to %s " ) % lp->get_file_path_base() << endl;
            lp->save();
            cout << "saved." << endl;
        }else{
            cout << "offline index exists. " << endl;
            lp->load();
        }
    }

double FLPMC::get_rmax(){
    double r = pow(epsilon, 1.0 / 1.5);
    // cout << format("r_max of local push: %s") % r << endl;
    return r;
}

double FLPMC::get_lp_epsilon(){
    double lp_epsilon = get_rmax() / (1-c);
    cout << format("local push error: %s") % lp_epsilon << endl;
    return lp_epsilon;
}

double FLPMC::query_one2one(NodePair np){
    if(np.first == np.second){
        return 1 ;
    }else if(np.first > np.second){ // make sure np.first < np.second
        np = NodePair({np.second, np.first});
    }
    /* FLP result */
    double p_i = lp->query_P(np);
    double r_i = lp->query_R(np);

    // set up the random number generator
    std::random_device rd;  //Will be used to obtain a seed for the random number engine
    std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
    std::uniform_real_distribution<double> distribution(0.0,1.0);

    // set up the geometry distribution
    std::geometric_distribution<int> geo_distribution(1-c);

    /* MC sampling phase */
    int N = get_N();
    // cout << format("number of samples: %s") % N << endl;
    double E_residual = 0;
    for(int i = 0; i< N;i++){
        // cout << "---------" << endl;
        double sum = 0; // the sum of residuals that passed by
        int a = np.first;
        int b = np.second;
        int length_of_random_walk = geo_distribution(gen);
        int step = 0;
        bool is_dead_node = false;
        while(( step < length_of_random_walk + 1) && (a != b)){
            a = sample_in_neighbor(a, *g);
            b = sample_in_neighbor(b, *g);

            step ++;
            if(a == -1 || b == -1){
                is_dead_node = true;
                break;
            }
        }
        if(is_dead_node){
            E_residual += 0;
        }else{
            E_residual += lp->query_R(a,b);
        }
        // cout << "terminal residual: " <<lp->query_R(a,b) << endl;
        // cout << "---------" << endl;
    }

    // cout << format("p_i: %s,r_i:%s, estimate residual: %s ")  % p_i  % r_i %( E_residual / N )<< endl;
    // cout << format("number of samples: %s") % get_N() << endl;
    double result = p_i + r_i + (c / (1-c)) * (E_residual / N);
    return result;
}

double FLPMC::get_N(){
    // compute the number of needed samples
    double tmp = ( pow(c,2.) * pow(get_rmax(),2.) * log(2. / delta) ) / ( 2 * pow(epsilon,2.) * pow((1-c),2.));
    int n = ceil(tmp);
    return n;
}


#include "ddCRP.h"
#include "MultivariateNormal.h"
#include "DontcareLikelihood.h"
#include <eigen3/Eigen/Dense>
#include <boost/program_options.hpp>
#include <fstream>
#include <iomanip>

namespace utils
{

template<typename M, int StorageType = Eigen::RowMajor>
M load_csv (const std::string& csvfile)
{
    std::ifstream indata;
    indata.open(csvfile);
    std::string line;
    std::vector<double> values;
    uint rows = 0;
    while (std::getline(indata, line)) {
        std::stringstream lineStream(line);
        std::string cell;
        while (std::getline(lineStream, cell, ',')) {
            values.push_back(std::stod(cell));
        }
        ++rows;
    }
    return Eigen::Map<const Eigen::Matrix<typename M::Scalar, M::RowsAtCompileTime, M::ColsAtCompileTime, StorageType> >(values.data(), rows, values.size()/rows);
}

}
int main(int argc, char* argv[])
{
    namespace po = boost::program_options;

    po::options_description desc("Allowed options");
    unsigned int seed, num_samples, num_burn_in_samples;
    double S, k, v;
    desc.add_options()
            ("help", "produce help message")
            ("log-decay-file,l", po::value<std::string>(), "csv file containing the log decay function values")
            ("feature-file,f", po::value<std::string>(), "csv file containing the feature vectors of the data points")
            ("prior-cov-file,S", po::value<std::string>(), "csv file with prior cluster covariance matrix")
            ("v", po::value<double>(&v)->default_value(14), "strength of cluster prior covariance")
            ("prior-mean-file,m", po::value<std::string>(), "csv file with cluster prior mean")
            ("k", po::value<double>(&k)->default_value(0.01), "strength of cluster prior mean")
            ("n", po::value<unsigned int>(&num_samples)->default_value(50), "number of samples to draw")
            ("b", po::value<unsigned int>(&num_burn_in_samples)->default_value(50), "number of burn-in samples for MCMC")
            ("seed,s", po::value<unsigned int>(&seed)->default_value(1234567890), "RNG seed")
            ("draw-from-prior,p", po::bool_switch()->default_value(false), "draw from ddCRP prior (ignore features and likelihood model)")
            ("wordy,w", po::bool_switch()->default_value(false), "toggle verbose mode with extra output")
            ;

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

    if (vm.count("help") || (argc == 1))
    {
        std::cout << desc << "\n";
        return 1;
    }

    Eigen::MatrixXd log_decay;
    if (vm.count("log-decay-file"))
    {
        std::string d_file = vm["log-decay-file"].as<std::string>();
        if ( vm["wordy"].as<bool>() )
            std::cout << "Loading log decay file " << d_file << "\n";
        log_decay = utils::load_csv<Eigen::MatrixXd>(d_file);
        if ( log_decay.rows() != log_decay.cols())
        {
            std::cout << "Log decay matrix number of rows (" << log_decay.rows() << ") must match number of columns (" << log_decay.cols() <<")!\n";
            return 1;
        }
    }
    else
    {
        std::cout << "Log decay file must be set!\n";
        return 1;
    }

    Eigen::MatrixXd features;
    if (vm.count("feature-file"))
    {
        std::string f_file = vm["feature-file"].as<std::string>();
        if ( vm["wordy"].as<bool>() )
            std::cout << "Loading feature file " << f_file << "\n";
        features = utils::load_csv<Eigen::MatrixXd>(f_file);
        if ( log_decay.rows() != features.rows())
        {
            std::cout << "Feature matrix number of rows (" << features.rows() << ") must match number of rows in log decay matrix (" << log_decay.rows() <<")!\n";
            return 1;
        }
    }
    else
    {
        std::cout << "Feature file must be set!\n";
        return 1;
    }

    Eigen::MatrixXd S0;
    if (vm.count("prior-cov-file"))
    {
        std::string s_file = vm["prior-cov-file"].as<std::string>();
        if ( vm["wordy"].as<bool>() )
            std::cout << "Loading prior covariance file " << s_file << "\n";
        S0 = utils::load_csv<Eigen::MatrixXd>(s_file);
        if ( S0.rows() != S0.cols() || S0.rows() != features.cols() )
        {
            std::cout << "Prior covariance matrix size is " << S0.rows() << " by " << S0.cols() << " - expected square matrix with dimensions matching feature dimensionality " << features.cols() << "!\n";
            return 1;
        }
    }
    else
    {
        std::cout << "Prior covariance file must be set!\n";
        return 1;
    }

    Eigen::VectorXd m0;
    if (vm.count("prior-mean-file"))
    {
        std::string m_file = vm["prior-mean-file"].as<std::string>();
        if ( vm["wordy"].as<bool>() )
            std::cout << "Loading prior mean file " << m_file << "\n";
        m0 = utils::load_csv<Eigen::VectorXd, Eigen::ColMajor>(m_file);
        if ( m0.rows() != features.cols() )
        {
            std::cout << "Prior mean vector length is " << m0.rows() << " - expected vector with length matching feature dimensionality " << features.cols() << "!\n";
            return 1;
        }
    }
    else
    {
        std::cout << "Prior mean file must be set!\n";
        return 1;
    }

    ddCRP clustering(log_decay, seed);

    std::shared_ptr<LikelihoodFcn> likelihood;
    if ( vm["draw-from-prior"].as<bool>() )
    {
        likelihood = std::shared_ptr<LikelihoodFcn> ( new DontcareLikelihood(features, m0, S0, k, v) );
    }
    else
    {
        likelihood = std::shared_ptr<LikelihoodFcn> ( new MultivariateNormal(features, m0, S0, k, v) );
    }

    clustering.setLikelihood(likelihood);

    if ( vm["wordy"].as<bool>() )
        std::cout << "Starting sampling!\n";

    for ( unsigned int i = 0; i < num_burn_in_samples + num_samples; ++i)
    {
        clustering.iterate();
        // skip the samples until burn-in is complete
        if ( i < num_burn_in_samples )
        {
            if ( vm["wordy"].as<bool>() )
                std::cout << "Burn-in-sample #" << i << "\n";
            continue;
        }

        if ( vm["wordy"].as<bool>() )
            std::cout << "Sample " << i - num_burn_in_samples << "\n";

        std::stringstream st;
        st << "clustering_" << std::setfill('0') << std::setw(4) << i - num_burn_in_samples << ".csv";
        std::ofstream table_file(st.str());
        if (table_file.is_open())
        {
            clustering.print_tables( table_file );
        }
        table_file.close();
    }

    return 0;
}


#include <boost/date_time/gregorian/gregorian.hpp>
#include <iostream>

int main( ) {
   using namespace boost::gregorian ;

   std::cout
      << "Yuletide holidays must be allowed in the following years:\n" ;
   for ( int i = 2008 ; i < 2121 ; i++ ) {
      greg_year gy = i ;
      date d  ( gy, Dec , 25 ) ;
      if ( d.day_of_week( ) == Sunday ) {
	 std::cout << i << std::endl ;
      }
   }
   std::cout << "\n" ;
   return 0 ;
}


#ifndef LATTICE_REGULAR_CUBE_HPP
#define LATTICE_REGULAR_CUBE_HPP

#include <string>
#include <vector>
#include <boost/lexical_cast.hpp>

namespace lattice {

class RegularCube {
public:
  static std::string name() { return "Regular Cube";}
  static int set_num_particles(int Ns){return Ns*Ns*Ns;}
  RegularCube(int LatticeSize) : dim_(LatticeSize) {}

  void create_table(std::vector<std::vector < int > >& table){
  //for bulk
    for (int i = 0; i < dim_; ++i){
      for (int j = 0; j < dim_; ++j){
        for (int k = 0; k < dim_; ++k){
          int l = numberize(i,j,k);
          int temp = 0;
          for(int d = -1; d < 2; d =d+2){
            table[l][temp] = numberize(i+d,j,k);
            temp += 1;
            table[l][temp] = numberize(i,j+d,k);
            temp += 1;
            table[l][temp] = numberize(i,j,k+d);
            temp += 1;
          }
        }
      }
    }
  }

  int numberize(const int i, const int j,const int k){
    int i_t = (i + dim_);
    int j_t = (j + dim_);
    int k_t = (k + dim_);
    i_t %= dim_ ;
    j_t %= dim_ ;
    k_t %= dim_ ;
    return i_t + j_t * dim_ + k_t * dim_ * dim_;
  }

  int latticize(int d, const int l){ //d is direction(0 ~ ) ex. 0 denotes x, 1 denotes y,~
    int k = l / (dim_*dim_) ;
    int j = (l - dim_ * dim_* k)/dim_ ;
    int i = (l - dim_ * dim_ * k - dim_ *j);
    if(d==0) return i;
    else if (d==1) return j;
    else return k;
  }

  int target_number(const int i, const int observe){
    int sight_x = 0;
    int sight_y = 0;
    int sight_z = 0;
    sight_x = latticize(0,observe);
    sight_y = latticize(1,observe);
    sight_z = latticize(2,observe);
    return numberize(sight_x + i, sight_y, sight_z);
  }

 int number_adjacent() {return 6;}

private:
  int dim_;
};

} //end namespace

#endif //LATTICE_REGULAR_CUBE_HPP


/* test_non_central_chi_squared_distribution.cpp
 *
 * Copyright Steven Watanabe 2011
 * Copyright Thijs van den Berg 2014
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * $Id$
 *
 */

#include <boost/random/non_central_chi_squared_distribution.hpp>
#include <limits>

#define BOOST_RANDOM_DISTRIBUTION boost::random::non_central_chi_squared_distribution<>
#define BOOST_RANDOM_ARG1 k
#define BOOST_RANDOM_ARG2 lambda
#define BOOST_RANDOM_ARG1_DEFAULT 1.0
#define BOOST_RANDOM_ARG2_DEFAULT 1.0
#define BOOST_RANDOM_ARG1_VALUE 4.0
#define BOOST_RANDOM_ARG2_VALUE 42.0

#define BOOST_RANDOM_DIST0_MIN 0
#define BOOST_RANDOM_DIST0_MAX (std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST1_MIN 0
#define BOOST_RANDOM_DIST1_MAX (std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST2_MIN 0
#define BOOST_RANDOM_DIST2_MAX (std::numeric_limits<double>::infinity)()

#define BOOST_RANDOM_TEST1_PARAMS
#define BOOST_RANDOM_TEST1_MIN 0.0
#define BOOST_RANDOM_TEST1_MAX 100.0

#define BOOST_RANDOM_TEST2_PARAMS (10000.0)
#define BOOST_RANDOM_TEST2_MIN 100.0

#include "test_distribution.ipp"


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

#include "graph_pathfinder.h"

#include <unordered_set>
#include <unordered_map>

#include <boost/heap/fibonacci_heap.hpp>

#include <log/logger.h>
#include <exc/exception.h>

using namespace ues::pf;

const std::string component_name = "Pathfinder";

namespace
{

/** The state type contains a point index and the estimated cost of such point.*/
template<unsigned short N>
struct state
{
    typename visibility_graph<N>::size_type point_index;
    ues::math::numeric_type accumulated_cost;
    ues::math::numeric_type estimated_cost;
};

/** The state_comparator class contains the required comparator method so the
 * states are sorted in the desired order in the priority queue. */
template<unsigned short N>
struct state_comparator
{
    bool operator() ( const state<N> & one, const state<N> & two ) const noexcept
    {
        return one.estimated_cost > two.estimated_cost;
    }
};

/** The priority_queue stores and keeps a sorted list of state objects. */
template<unsigned short N>
using priority_queue = boost::heap::fibonacci_heap< state<N>, boost::heap::compare< state_comparator<N> > >;
/** The index_set contains all the points whose cost has already been computed. */
template<unsigned short N>
using index_set = std::unordered_set< typename visibility_graph<N>::size_type >;
/** Stores the handles that allow modifying the states of the priority queue. */
template<unsigned short N>
using handle_storage = std::unordered_map< typename visibility_graph<N>::size_type, typename priority_queue<N>::handle_type >;
/** Stores the point from which current point has been reached. */
template<unsigned short N>
using parent_point = std::vector< typename visibility_graph<N>::size_type >;

}

template<unsigned short N>
path<N> graph_pathfinder<N>::find_path ( const ues::geom::point<N> & origin,
                                         const ues::geom::point<N> & target )
{
    ues::log::logger lg;

    if ( lg.min_level() <= ues::log::TRACE_LVL )
    {
        ues::log::event e ( ues::log::TRACE_LVL, component_name, "Starting pathfinding" );
        e.message() << *graph << '\n';
        lg.record ( std::move ( e ) );
    }

    // Get indices to the origin and target points.
    const typename visibility_graph<N>::size_type origin_index = graph->point_to_index ( origin );
    const typename visibility_graph<N>::size_type target_index = graph->point_to_index ( target );

    // Declare necesary data structures.
    priority_queue<N> frontier;
    handle_storage<N> handles;
    index_set<N> explored;
    parent_point<N> parents ( graph->size() );
    parents[ origin_index ] = origin_index;

    // Add initial node to the priority queue.
    typename priority_queue<N>::handle_type handle = frontier.push ( { origin_index, 0, origin.distance_to ( target ) } );
    handles.insert ( { origin_index, handle } );

    while ( !frontier.empty() )
    {
        state<N> node = frontier.top();
        frontier.pop();
        handles.erase ( node.point_index );

        if ( node.point_index == target_index )
        {
            path<N> reverse_result;
            typename visibility_graph<N>::size_type current_point = target_index;
            while ( current_point != parents[ current_point ] )
            {
                reverse_result.push_back ( graph->index_to_point ( current_point ) );
                current_point = parents[ current_point ];
            }
            reverse_result.push_back ( origin );
            path<N> result ( reverse_result.rbegin(), reverse_result.rend() );

            if ( lg.min_level() <= ues::log::DEBUG_LVL )
            {
                ues::log::event e ( ues::log::DEBUG_LVL, component_name, "Found path" );
                e.message() << "Cost of the path: " << node.accumulated_cost << '\n';
                e.message() << result << '\n';
                lg.record ( std::move ( e ) );
            }

            return std::move ( result );
        }

        explored.insert ( node.point_index );

        for ( typename visibility_graph<N>::size_type p : graph->adjacents ( node.point_index ) )
        {
            if ( explored.find ( p ) == explored.end() )
            {
                // Get the cost of the edge from node.point_index to p.
                ues::math::numeric_type last_edge;
                graph->check_visibility ( node.point_index, p, last_edge );

                // Generate a node with the cost of getting to p from node.point_index.
                state<N> new_node;
                new_node.point_index = p;
                new_node.accumulated_cost = node.accumulated_cost + last_edge;
                ues::math::numeric_type heuristic_cost = graph->index_to_point ( p ).distance_to ( target );
                new_node.estimated_cost = new_node.accumulated_cost + heuristic_cost;

                typename handle_storage<N>::const_iterator it = handles.find ( p );
                if ( it == handles.end() )
                {
                    handle = frontier.push ( new_node );
                    handles.insert ( { p, handle } );
                    parents[p] = node.point_index;
                }
                else if ( new_node.estimated_cost < ( *it->second ).estimated_cost )
                {
                    ( *it->second ).estimated_cost = new_node.estimated_cost;
                    ( *it->second ).accumulated_cost = new_node.accumulated_cost;
                    frontier.decrease ( it->second );
                    parents[p] = node.point_index;
                }
            }
        }
    }

    throw ues::exc::exception ( "Unable to find a path between points", UES_CONTEXT );
}

template<unsigned short N>
graph_pathfinder<N>::graph_pathfinder ( std::shared_ptr< visibility_graph<N> > graph )
    : graph ( std::move ( graph ) )
{
    if ( this->graph.get() == nullptr )
        throw ues::exc::exception ( "Provided graph cannot be null", UES_CONTEXT );
}

// Instantiate the templates in this translation unit, just once.
template class ues::pf::graph_pathfinder<2>;
template class ues::pf::graph_pathfinder<3>;


#ifndef GAME_STATE_HPP
#define GAME_STATE_HPP

#include <armadillo>

/*
Install Armadillo:
	If not present already, install LAPACK, Boost and BLAS  with
		sudo apt-get install liblapack-dev
		sudo apt-get install libblas-dev
		sudo apt-get install libboost-dev
	Install Armadillo using
		sudo apt-get install libarmadillo-dev
*/

struct Robot
{
	arma::vec2 pos; // (x, y) robot position in centimeters.
	arma::vec2 dir; // Unit vector indicating robot direction.
	bool missing;   // True if robot is not being seen.

	Robot()
	{
		pos = {0.0, 0.0};
		dir = {1.0, 0.0};
		missing = true;
	}
};

struct Ball
{
	arma::vec2 pos; // Ball position
	bool missing;   // True if ball is not being seen.

	Ball()
	{
		pos = {0.0, 0.0};
		missing = true;
	}
};

struct GameState
{
	Ball ball;					// Ball Position (x, y) in centimeters.
	std::vector<Robot> robots;  // Friendly robots.
	std::vector<Robot> enemies; // Enemy robots.

	GameState() : ball()
	{
		this->robots = std::vector<Robot>(3, Robot());
		this->enemies = std::vector<Robot>(3, Robot());
	}
};

#endif


/*
 Copyright (C) 2021 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file lgmcalibrationinfo.hpp
    \brief info data on how a lgm model was calibrated
    \ingroup models
*/

#pragma once

#include <ql/types.hpp>
#include <ql/utilities/null.hpp>

#include <boost/any.hpp>

#include <map>
#include <vector>

namespace QuantExt {
using namespace QuantLib;

struct SwaptionData {
    Real timeToExpiry;
    Real swapLength;
    Real strike;
    Real atmForward;
    Real annuity;
    Real vega;
    Real stdDev;
};

struct LgmCalibrationData {
    Real modelTime;
    Real modelVol;
    Real marketVol;
    Real modelValue;
    Real marketValue;
    Real modelAlpha;
    Real modelKappa;
    Real modelHwSigma;
};

struct LgmCalibrationInfo {
    bool valid = false;
    Real rmse = Null<Real>();
    std::vector<SwaptionData> swaptionData;
    std::vector<LgmCalibrationData> lgmCalibrationData;
};

std::map<std::string, boost::any> getAdditionalResultsMap(const LgmCalibrationInfo& info);

} // namespace QuantExt