///////////////////////////////////////////////////////////////////////////////
// monomials_horner::example::multi_factorial.cpp                            //
//                                                                           //
//  (C) Copyright 2009 Erwann Rogard                                         //
//  Use, modification and distribution are subject to the                    //
//  Boost Software License, Version 1.0. (See accompanying file              //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#include <algorithm>
#include <iterator>
#include <boost/range.hpp>
#include <boost/monomials_horner/multi_indexes_derived.hpp>
#include <libs/monomials_horner/example/multi_factorial.h>

void example_multi_factorial(std::ostream& out){
    out << "-> example_multi_factorial" << std::endl;

    using namespace boost;
    using namespace monomials_horner;

    const unsigned d = 3;
    typedef multi_indexes_derived<d,multi_factorial>        mf_type;
    typedef multi_indexes_derived<d,multi_degree>           md_type;
    typedef multi_indexes_derived<d,multi_power2divfact>    mp_type;
    typedef mf_type::iter_range_type            mf_iter_range_type;
    typedef md_type::iter_range_type            md_iter_range_type;
    typedef mp_type::iter_range_type            mp_iter_range_type;

    unsigned int degree1 = 5;

    mf_iter_range_type mf_ir = mf_type::get(degree1);

    out << " -> multi_factorial<" << d << ">";
    out << "::degree(" << degree1 << ")" << std::endl;
    copy(
        boost::begin(mf_ir),
        boost::end(mf_ir),
        std::ostream_iterator<double>(out," ")
    ); out << std::endl;

    md_iter_range_type md_ir = md_type::get(degree1);

    out << " -> multi_degree<" << d << ">";
    out << "::degree(" << degree1 << ")" << std::endl;
    copy(
        boost::begin(md_ir),
        boost::end(md_ir),
        std::ostream_iterator<double>(out," ")
    ); out << std::endl;
    mp_iter_range_type mp_ir = mp_type::get(degree1);

    out << " -> multi_p2df<" << d << ">";
    out << "::degree(" << degree1 << ")" << std::endl;
    copy(
        boost::begin(mp_ir),
        boost::end(mp_ir),
        std::ostream_iterator<double>(out," ")
    ); out << std::endl;

    out << "<-" << std::endl;

};


/*
 *  The MIT License (MIT)
 *
 *  Copyright (c) 2015 MRL-SPL RoboCup Team
 *
 *  Permission is hereby granted, free of charge, to any person obtaining a copy
 *  of this software and associated documentation files (the "Software"), to deal
 *  in the Software without restriction, including without limitation the rights
 *  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 *  copies of the Software, and to permit persons to whom the Software is
 *  furnished to do so, subject to the following conditions:
 *
 *  The above copyright notice and this permission notice shall be included in all
 *  copies or substantial portions of the Software.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 *  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 *  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 *  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 *  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 *  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 *  SOFTWARE.
 *
 */

/*
 * File:   Chain.hpp
 * Author: Mohammad Ali Sharpasand
 *
 * Created on March 17, 2016
 */

#pragma once
#include "DHFrame.hpp"
#include "AxisAngle.hpp"

#include <vector>
#include <armadillo>

namespace BipedLibrary
{
    using namespace arma;

    class Chain : public std::vector<DHFrame>
    {
    public:
        Chain(vec3 const posi_body_body = vec3(),
              mat33 const ori_body = mat33());

        vec3 posi_body_body(int frame) const;
        mat33 const& ori_body() const;
        vec3 posi_end_last_last() const;
        mat33 ori_end_last() const;

        Chain& setPosi_body_body(vec3 const& posi_body_body);
        Chain& setOri_body(mat33 const& ori_body);
        Chain& setPosi_end_last_last(vec3 const& posi_end_last_last);
        Chain& setOri_end_last(mat33 const& ori_end_last);

        vec3 position_end_body_body() const;
        vec3 position_base_base(int frame = -1) const;
        vec3 position_body_body(int frame = -1) const;
        vec3 position_pre_pre(int i) const;
        vec3 position_com (int frame) const;
        mat33 orientation_end_body() const;
        mat33 orientation_body(int frame = -1) const;
        mat33 orientation_base(int frame = -1) const;
        mat33 orientation_pre(int i) const;
        mat66 jacobi_base(int frame, vec3 target = {0, 0, 0}) const;
        mat66 jacobi_body(int frame, vec3 target = {0, 0, 0}) const;

        bool solveForJointAngles(vec3 const& posi_end_body, mat33 ori_end_body);

    private:
        vec3 posi_body_body_;
        mat33 ori_body_;
        vec3 posi_end_last_last_;
        mat33 ori_end_last_;

    };

}


//=======================================================================
// Copyright (c) 2014 Piotr Smulewicz
//
// 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)
//=======================================================================
/**
 * @file k_cut_example.cpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2014-08-30
 */

    //! [K Cut Example]
#include "paal/greedy/k_cut/k_cut.hpp"

#include <boost/graph/adjacency_list.hpp>

#include <iostream>

int main() {
    // sample data
    std::vector<std::pair<int,int>> edges_p {{1,2},{1,5},{2,3},{2,5},{2,6},
        {3,4},{3,7},{4,7},{4,0},{5,6},{6,7},{7,0}};
    std::vector<int> costs{2,3,3,2,2,4,2,2,2,3,1,3};
    boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
                    boost::no_property,
                    boost::property<boost::edge_index_t, std::size_t>
                    > graph(8);
    for (std::size_t i = 0; i < edges_p.size(); ++i) {
        add_edge(edges_p[i].first, edges_p[i].second, i, graph);
    }
    const int parts = 3;

    auto edge_id = get(boost::edge_index, graph);
    auto weight = make_iterator_property_map(costs.begin(), edge_id);

    // solve
    int cost_cut;
    std::vector<std::pair<int,int>> vertices_parts;
    cost_cut = paal::greedy::k_cut(graph, parts, back_inserter(vertices_parts),
            boost::weight_map(weight));

    // alternative form
    // cost_cut = paal::greedy::k_cut(graph, parts, back_inserter(vertices_parts));
    // this works if the graph has and internal edge weight property map

    // print result
    std::cout << "cost cut:" << cost_cut << std::endl;
    std::vector<int> vertices_to_parts;
    for (auto i: vertices_parts) {
        std::cout << i.first << "(" << i.second << "), ";
    }
    std::cout << std::endl;
}
    //! [K Cut Example]


#ifndef WAV2IMAGE_FFT_H
#define WAV2IMAGE_FFT_H

#include <cstdlib>
#include <cstdint>
#include <string>
#include <exception>
#include <vector>
#include <memory>
#include <boost/container/flat_map.hpp>

using window_list_t = boost::container::flat_map< unsigned int, std::shared_ptr< float > >;

struct fft_failed : public std::runtime_error {
  fft_failed( const std::string &what ) : std::runtime_error( what ) {}
  fft_failed( const char *what ) : std::runtime_error( what ) {}
};

struct fft_initialization_failed : public fft_failed {
  fft_initialization_failed( const std::string &what ) : fft_failed( what ) {}
  fft_initialization_failed( const char *what ) : fft_failed( what ) {}
};
struct fft_allocation_failed : public fft_failed {
  fft_allocation_failed( const std::string &what ) : fft_failed( what ) {}
  fft_allocation_failed( const char *what ) : fft_failed( what ) {}
};
struct fft_execution_failed : public fft_failed {
  fft_execution_failed( const std::string &what ) : fft_failed( what ) {}
  fft_execution_failed( const char *what ) : fft_failed( what ) {}
};
struct fft_data_transfar_failed : public fft_failed {
  fft_data_transfar_failed( const std::string &what ) : fft_failed( what ) {}
  fft_data_transfar_failed( const char *what ) : fft_failed( what ) {}
};

void init_fft();
window_list_t generate_window();
//std::shared_ptr< float > fft( const window_list_t &window, const std::vector< int16_t > &data, size_t resolution, size_t interval, size_t width );
std::pair< std::vector< float >, std::shared_ptr< float > > fftref( const window_list_t &window, const std::vector< int16_t > &data, size_t resolution, float a, float b, size_t width );
std::pair< float, std::vector< float > > fftcomp( const float*, size_t, const window_list_t &window, const std::vector< int16_t > &data, size_t resolution, float a, float b, size_t width );

#endif


#include "declarations.h"
#include <Eigen/Eigen>

#ifdef _MSC_VER
inline bool isnan(double x) {
    return x != x;
}
#endif

void topologicallySubdivide(Cell *qe, unsigned level)
{
	CellVertexIterator cvi(qe);
	Vertex *v;
	while ((v=cvi.next())!=0)
	{
		VertexEdgeIterator vei(v);
		Edge *e;
		while ((e=vei.next())!=0) //calculate midpoints
			if (isnan(e->midpoint[0]))
			{
				e->midpoint=(e->Org()->pos + e->Dest()->pos)*0.5;
				e->Sym()->midpoint=e->midpoint;
			}
			else
				; //midpoint has already been calculated by iterating over a different vertex
	}

	//now do actual topological face splitting
	CellFaceIterator cfi(qe);
	Face *f;
	//unsigned i=0;
	while ((f=cfi.next())!=0)
	{
		Vertex *a, *b, *c;
		Edge *e=f->getEdge();
		Edge *orig=e;

		//get a
		if (isnan(e->midpoint[0]))//this is a new edge
		{
			if (e->Org()->level==level)
				a=e->Org();
			else
			{
				a=e->Dest();
				e=e->Lnext();
			}
		}
		else
		{
			orig=split(e);
			a=e->Org();
			a->level=level;
			a->pos=e->midpoint;
			a->valence=6;
			e->midpoint=vec3(std::numeric_limits<float>::quiet_NaN(),0,0);//deinitialize
			e->Sym()->midpoint=e->midpoint;
		}
		e=e->Lnext();
		//get b
		if (isnan(e->midpoint[0]))//this is a new edge
		{
			b=e->Dest();
			e=e->Lnext();
		}
		else
		{
			split(e);
			b=e->Org();
			b->level=level;
			b->pos=e->midpoint;
			b->valence=6;
			e->midpoint=vec3(std::numeric_limits<float>::quiet_NaN(),0,0);//deinitialize
			e->Sym()->midpoint=e->midpoint;
		}
		e=e->Lnext();
		//get c
		if (isnan(e->midpoint[0]))//this is a new edge
			c=e->Dest();
		else
		{
			split(e);
			c=e->Org();
			c->level=level;
			c->pos=e->midpoint;
			c->valence=6;
			e->midpoint=vec3(std::numeric_limits<float>::quiet_NaN(),0,0);//deinitialize
			e->Sym()->midpoint=e->midpoint;
		}

		//now split face
		qe->makeFaceEdge(f, c, a);
		qe->makeFaceEdge(f, a, b);
		qe->makeFaceEdge(f, b, c);
		//objWriteCell(qe, ("D:\\Temp\\subdivF"+QString::number(++i).rightJustified(2,'0')+".obj").toStdString().c_str());
	}
	//objWriteCell(qe, "D:\\butterfly.obj");
}

Edge* stepAlong(Edge *e, unsigned steps) //in regular mesh
{
	for (unsigned i=1; i<steps; i++)
	{
		e=e->Sym();
		e=e->Onext()->Onext()->Onext();
	}
	return e;
}

void calcSubdivPosExtra3(Vertex *v, Edge *left, unsigned maxLevel)
{
	Edge *e=left;
	vec3 pos=e->Org()->subdivPos*0.75; //+9/12
	pos+=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->subdivPos*0.4166666666666666666; //+5/12
	e=e->Onext();
	pos-=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->subdivPos*0.0833333333333333333; //-1/12
	e=e->Onext();
	pos-=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->subdivPos*0.0833333333333333333; //-1/12
	v->subdivPos=pos;
}

void calcSubdivPosExtra4(Vertex *v, Edge *left, unsigned maxLevel)
{
	Edge *e=left;
	vec3 pos=e->Org()->subdivPos*0.75; //+6/8
	pos+=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->subdivPos*0.375; //+3/8
	e=e->Onext()->Onext();
	pos-=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->subdivPos*0.125; //-1/8
	v->subdivPos=pos;
}

void calcSubdivPosExtraRest(Vertex *v, Edge *left, unsigned maxLevel)
{
	Edge *e=left;
	vec3 pos=e->Org()->subdivPos*0.75;
	double tc;
	unsigned valence=left->Org()->valence;
	for (unsigned i=0; i<valence; i++)
	{
		tc=1.0/valence*(0.25+cos(2*i*M_PI/valence)+0.5*cos(4*i*M_PI/valence));
		pos+=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->subdivPos*tc;
		e=e->Onext();
	}
	v->subdivPos=pos;
}

void calcSubdivPosExtra(Vertex *v, Edge *left, unsigned maxLevel)
//left vertex is extraordinary
{
	switch (left->Org()->valence)
	{
	case 3:
		calcSubdivPosExtra3(v, left, maxLevel);
		break;
	case 4:
		calcSubdivPosExtra4(v, left, maxLevel);
		break;
	default:
		calcSubdivPosExtraRest(v, left, maxLevel);
		break;
	}
}

void calcSubdivPosRegular(Vertex *v, Edge *left, unsigned maxLevel)
{
	Edge *e=left;

	vec3 pos=e->Org()->subdivPos*0.5; //+1/2
	e=e->Onext();
	pos+=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->subdivPos*0.125; //+1/8
	e=e->Onext();
	pos-=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->subdivPos*0.0625; //-1/16
	e=e->Onext()->Onext();
	pos-=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->subdivPos*0.0625; //-1/16

	e=stepAlong(left, exp2i(maxLevel - v->level + 1))->Sym(); //right
	pos+=e->Org()->subdivPos*0.5; //+1/2
	e=e->Onext();
	pos+=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->subdivPos*0.125; //+1/8
	e=e->Onext();
	pos-=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->subdivPos*0.0625; //-1/16
	e=e->Onext()->Onext();
	pos-=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->subdivPos*0.0625; //-1/16
	v->subdivPos=pos;
}

void calcSubdivPos(Vertex *v, Edge *left, unsigned maxLevel)
//vertex in question and edge with origin in left neighbor (pointing towards this vertex)
{
	Edge *right=stepAlong(left, exp2i(maxLevel - v->level + 1))->Sym();
	Vertex *l=left->Org(), *r=right->Org();
	if (l->valence==6 && r->valence==6)
		calcSubdivPosRegular(v, left, maxLevel);
	else
		if (l->valence!=6 && r->valence==6)
			calcSubdivPosExtra(v, left, maxLevel);
		else if (l->valence==6 && r->valence!=6)
			calcSubdivPosExtra(v, right, maxLevel);
		else //both extraordinary
		{
			calcSubdivPosExtra(v, left, maxLevel);
			vec3 pos=v->subdivPos;
			calcSubdivPosExtra(v, right, maxLevel);
			v->subdivPos=(v->subdivPos + pos)*0.5;
		}
}

void calculateSubdivisionPositions(Cell *qe, unsigned maxLevel, unsigned subdivFromLevel)
//smooth interpolating subdivision using modified butterfly scheme
{
	for (unsigned l=subdivFromLevel+1; l<=maxLevel; l++)
	{
		//calculate subdiv positions based on previous level
		CellVertexIterator cvi(qe);
		Vertex *v;
		while ((v = cvi.next())!=0)
			if (v->level==l)
			{
				Edge *e=v->getEdge();
				Edge *e1=stepAlong(e, exp2i(maxLevel-l));
				Vertex *n=e1->Dest();
				while (n->level>=l)
				{
					e=e->Onext();
					e1=stepAlong(e, exp2i(maxLevel-l));
					n=e1->Dest();
				}
				calcSubdivPos(v, e1->Sym(), maxLevel);
			}
	}
}

void copyPos2SubdivPos(Cell *qe)
{
	CellVertexIterator cvi0(qe);
	Vertex *v0;
	while ((v0 = cvi0.next())!=0)
		v0->subdivPos=v0->pos;
}

void copySubdivPos2Pos(Cell *qe)
{
	CellVertexIterator cvi0(qe);
	Vertex *v0;
	while ((v0 = cvi0.next())!=0)
		v0->pos=v0->subdivPos;
}

void swapSubdivPos(Cell *qe)
{
	CellVertexIterator cvi0(qe);
	Vertex *v0;
	while ((v0 = cvi0.next())!=0)
		std::swap(v0->pos,v0->subdivPos);
}

void normalizeHeuristic(Cell *qe, int maxLevel, int howManyFinestLevels)
{
	Vertex *v;
	//objWriteCell(qe, "D:\\Temp\\hierarchyP.obj");

	for (int l=maxLevel-1; l>maxLevel-howManyFinestLevels; l--)
	{
		double weight=0.5/pow(3.0,int(maxLevel-l));
		CellVertexIterator cvi(qe);
		while ((v = cvi.next())!=0)
			if (v->level==l)
			{
				Edge *e=v->getEdge();
				vec3 update(0,0,0);
				do
				{
					Vertex *n=stepAlong(e, exp2i(maxLevel-l-1))->Dest();
                    update+=(n->pos - n->subdivPos);
					e=e->Onext();
				} while (e!=v->getEdge());
                v->pos=v->subdivPos+(v->pos - v->subdivPos)*weight+(update/v->valence)*(1-weight);
			}
	}

    //all coarse levels in one go
    double weight=0.5/3;
	CellVertexIterator cvi(qe);
	while ((v = cvi.next())!=0)
		if (v->level<=maxLevel-howManyFinestLevels)
		{
			Edge *e=v->getEdge();
			vec3 update(0,0,0);
			do
			{
				Vertex *n=e->Dest();
                update+=(n->pos - n->subdivPos);
				e=e->Onext();
			} while (e!=v->getEdge());
            v->pos=v->subdivPos+(v->pos - v->subdivPos)*weight+(update/v->valence)*(1-weight);
		}

	//objWriteCell(qe, "D:\\Temp\\hierarchyN.obj");
	copyPos2SubdivPos(qe);
	calculateSubdivisionPositions(qe, maxLevel, maxLevel-howManyFinestLevels);
	copySubdivPos2Pos(qe);
	//objWriteCell(qe, "D:\\Temp\\hierarchyS.obj");
}

void calcWeightExtra3(Vertex *v, Edge *left, unsigned maxLevel)
{
	Edge *e=left;
	v->weights=e->Org()->weights*0.75; //+9/12
	v->weights+=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->weights*0.4166666666666666666; //+5/12
	e=e->Onext();
	v->weights-=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->weights*0.0833333333333333333; //-1/12
	e=e->Onext();
	v->weights-=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->weights*0.0833333333333333333; //-1/12
}

void calcWeightExtra4(Vertex *v, Edge *left, unsigned maxLevel)
{
	Edge *e=left;
	v->weights=e->Org()->weights*0.75; //+6/8
	v->weights+=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->weights*0.375; //+3/8
	e=e->Onext()->Onext();
	v->weights-=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->weights*0.125; //-1/8
}

void calcWeightExtraRest(Vertex *v, Edge *left, unsigned maxLevel)
{
	Edge *e=left;
	v->weights=e->Org()->weights*0.75;
	double tc;
	unsigned valence=left->Org()->valence;
	for (unsigned i=0; i<valence; i++)
	{
		tc=1.0/valence*(0.25+cos(2*i*M_PI/valence)+0.5*cos(4*i*M_PI/valence));
		v->weights+=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->weights*tc;
		e=e->Onext();
	}
}

void calcWeightExtra(Vertex *v, Edge *left, unsigned maxLevel)
//left vertex is extraordinary
{
	switch (left->Org()->valence)
	{
	case 3:
		calcWeightExtra3(v, left, maxLevel);
		break;
	case 4:
		calcWeightExtra4(v, left, maxLevel);
		break;
	default:
		calcWeightExtraRest(v, left, maxLevel);
		break;
	}
}

void calcWeightRegular(Vertex *v, Edge *left, unsigned maxLevel)
{
	Edge *e=left;

	v->weights=e->Org()->weights*0.5; //+1/2
	e=e->Onext();
	v->weights+=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->weights*0.125; //+1/8
	e=e->Onext();
	v->weights-=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->weights*0.0625; //-1/16
	e=e->Onext()->Onext();
	v->weights-=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->weights*0.0625; //-1/16

	e=stepAlong(left, exp2i(maxLevel - v->level + 1))->Sym(); //right
	v->weights+=e->Org()->weights*0.5; //+1/2
	e=e->Onext();
	v->weights+=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->weights*0.125; //+1/8
	e=e->Onext();
	v->weights-=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->weights*0.0625; //-1/16
	e=e->Onext()->Onext();
	v->weights-=stepAlong(e, exp2i(maxLevel - v->level + 1))->Dest()->weights*0.0625; //-1/16
}

void calcWeight(Vertex *v, Edge *left, unsigned maxLevel)
//vertex in question and edge with origin in left neighbor (pointing towards this vertex)
{
	Edge *right=stepAlong(left, exp2i(maxLevel - v->level + 1))->Sym();
	Vertex *l=left->Org(), *r=right->Org();
	if (l->valence==6 && r->valence==6)
		calcWeightRegular(v, left, maxLevel);
	else
		if (l->valence!=6 && r->valence==6)
			calcWeightExtra(v, left, maxLevel);
		else if (l->valence==6 && r->valence!=6)
			calcWeightExtra(v, right, maxLevel);
		else //both extraordinary
		{
			calcWeightExtra(v, left, maxLevel);
			Eigen::VectorXd temp=v->weights;
			calcWeightExtra(v, right, maxLevel);
			v->weights=(v->weights + temp)*0.5;
		}
}

Eigen::MatrixXd createWeightMatrix(Cell *qe, unsigned maxLevel, unsigned subdivFromLevel, unsigned &freeCount)
{
	Vertex *v;
	freeCount=0;
	{
		CellVertexIterator cvi0(qe);
		while ((v = cvi0.next())!=0)
			if (v->level<=subdivFromLevel)
				freeCount++;
	} //we now have count of free vertices

	CellVertexIterator cvi0(qe);
	while ((v = cvi0.next())!=0)
		if (v->level<=subdivFromLevel)
		{
			v->weights.resize(freeCount);
			v->weights.setZero();
			v->weights[v->realIndex()]=1;
		}

	for (unsigned l=subdivFromLevel+1; l<=maxLevel; l++)
	{
		CellVertexIterator cvi0(qe);
		while ((v = cvi0.next())!=0)
			if (v->level==l)
			{
				v->weights.resize(freeCount);
				v->weights.setZero();
				Edge *e=v->getEdge();
				Edge *e1=stepAlong(e, exp2i(maxLevel-l));
				Vertex *n=e1->Dest();
				while (n->level>=l)
				{
					e=e->Onext();
					e1=stepAlong(e, exp2i(maxLevel-l));
					n=e1->Dest();
				}
				calcWeight(v, e1->Sym(), maxLevel);
			}
	}

	Eigen::MatrixXd W(freeCount, qe->countVertices()); //freeCount rows, totalCount columns
	CellVertexIterator cvi(qe);
	while ((v = cvi.next())!=0)
	{
		W.col(v->realIndex())=v->weights.transpose(); //vectors are columns, not rows (mathematical standard)
	}
	return W;
}


#include "formulation/scalar/drift_diffusion.hpp"

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria.h>

#include "quadrature/calculators/tests/angular_flux_integrator_mock.hpp"
#include "calculator/drift_diffusion/tests/drift_diffusion_vector_calculator_mock.hpp"
#include "data/cross_sections/material_cross_sections.hpp"
#include "domain/finite_element/tests/finite_element_mock.hpp"
#include "data/material/tests/material_mock.hpp"
#include "test_helpers/gmock_wrapper.h"
#include "test_helpers/test_assertions.hpp"
#include "test_helpers/test_helper_functions.h"

namespace  {

using namespace bart;
using test_helpers::AreEqual;

using ::testing::NiceMock, ::testing::Return, ::testing::DoDefault, ::testing::_;
using ::testing::Ref, ::testing::AtLeast;

template <typename DimensionWrapper>
class DriftDiffusionFormulationTest : public ::testing::Test {
 public:
  static constexpr int dim = DimensionWrapper::value;
  using CellPtr = typename domain::CellPtr<dim>;
  using CrossSections = data::cross_sections::MaterialCrossSections;
  using AngularFluxIntegrator = NiceMock<quadrature::calculators::AngularFluxIntegratorMock>;
  using DriftDiffusionCalculator = NiceMock<typename calculator::drift_diffusion::DriftDiffusionVectorCalculatorMock<dim>>;
  using DriftDiffusionFormulation = formulation::scalar::DriftDiffusion<dim>;
  using FiniteElement = NiceMock<typename domain::finite_element::FiniteElementMock<dim>>;
  using Material = NiceMock<data::material::MaterialMock>;

  DriftDiffusionFormulationTest() : dof_handler_(triangulation_), finite_element_(1) {};

  // test object
  std::unique_ptr<DriftDiffusionFormulation> test_formulation_{ nullptr };

  std::shared_ptr<AngularFluxIntegrator> angular_flux_integrator_mock_ptr_{ nullptr };
  std::shared_ptr<DriftDiffusionCalculator> drift_diffusion_calculator_mock_ptr_{ nullptr };
  std::shared_ptr<FiniteElement> finite_element_mock_ptr_{ nullptr };
  std::shared_ptr<CrossSections> cross_sections_ptr_{ nullptr };

  // minimum number of dealii objects to get a valid cell pointer
  CellPtr cell_ptr_;
  dealii::Triangulation<dim> triangulation_;
  dealii::DoFHandler<dim> dof_handler_;
  dealii::FE_Q<dim> finite_element_;

  // Test paraameters
  const int dofs_per_cell_{ 2 };
  const int cell_quadrature_points_{ 2 };
  const int face_quadrature_points_{ 2 };
  const int material_id_{ 1 };
  const int energy_group_{ 0 };
  dealii::FullMatrix<double> expected_result_;
  std::unordered_map<int, std::vector<double>> diffusion_coef_{{material_id_, {0.5, 1.0}}};

  auto SetUp() -> void override;
  [[nodiscard]] auto GetShapeGradient(const int i, const int q) const -> dealii::Tensor<1, dim>;
};

template <typename DimensionWrapper>
auto DriftDiffusionFormulationTest<DimensionWrapper>::GetShapeGradient(const int i, const int q) const
-> dealii::Tensor<1, dim> {
  dealii::Tensor<1, dim> gradient_tensor;
  const double val{ (i + q + 0.5) };
  switch (dim) {
    case 3:
      gradient_tensor[2] = 3 * val; [[fallthrough]];
    case 2:
      gradient_tensor[1] = 2 * val; [[fallthrough]];
    case 1:
      gradient_tensor[0] = val;
      break;
  }
  return gradient_tensor;
}

template <typename DimensionWrapper>
auto DriftDiffusionFormulationTest<DimensionWrapper>::SetUp() -> void {
  angular_flux_integrator_mock_ptr_ = std::make_shared<AngularFluxIntegrator>();
  drift_diffusion_calculator_mock_ptr_ = std::make_shared<DriftDiffusionCalculator>();
  finite_element_mock_ptr_ = std::make_shared<FiniteElement>();
  Material mock_material;

  ON_CALL(*finite_element_mock_ptr_, dofs_per_cell()).WillByDefault(Return(dofs_per_cell_));
  ON_CALL(*finite_element_mock_ptr_, n_cell_quad_pts()).WillByDefault(Return(cell_quadrature_points_));
  ON_CALL(*finite_element_mock_ptr_, n_face_quad_pts()).WillByDefault(Return(face_quadrature_points_));

  for (int q = 0; q < cell_quadrature_points_; ++q) {
    ON_CALL(*finite_element_mock_ptr_, Jacobian(q)).WillByDefault(Return(3 * (q + 1)));
    ON_CALL(*finite_element_mock_ptr_, FaceJacobian(q)).WillByDefault(Return(3 * (q + 1)));
    for (int i = 0; i < dofs_per_cell_; ++i) {
      auto shape_gradient{ GetShapeGradient(i, q) };
      ON_CALL(*finite_element_mock_ptr_, ShapeValue(i,q)).WillByDefault(Return(1 + i + q));
      ON_CALL(*finite_element_mock_ptr_, FaceShapeValue(i,q)).WillByDefault(Return(1 + i + q));
      ON_CALL(*finite_element_mock_ptr_, ShapeGradient(i,q)).WillByDefault(Return(shape_gradient));
    }
  }

  dealii::Tensor<1, dim> drift_diffusion;
  switch (dim) {
    case 3:
      drift_diffusion[2] = 1000; [[fallthrough]];
    case 2:
      drift_diffusion[1] = 100; [[fallthrough]];
    case 1:
      drift_diffusion[0] = 10;
      break;
  }

  ON_CALL(*drift_diffusion_calculator_mock_ptr_, DriftDiffusionVector(_, _, _, _)).WillByDefault(Return(drift_diffusion));

  ON_CALL(mock_material, GetDiffusionCoef()).WillByDefault(Return(diffusion_coef_));
  cross_sections_ptr_ = std::make_shared<CrossSections>(mock_material);

  dealii::GridGenerator::hyper_cube(triangulation_, 0, 1);
  dof_handler_.distribute_dofs(finite_element_);
  for (auto cell = dof_handler_.begin_active(); cell != dof_handler_.end(); ++cell) {
    if (cell->is_locally_owned()) {
      cell_ptr_ = cell;
      cell_ptr_->set_material_id(material_id_);
      break;
    }
  }

  std::array<double, 16> expected_result_q_0_values{0.5, 1.5,
                                                    1.0, 3.0};
  std::array<double, 16> expected_result_q_1_values{3.0, 5.0,
                                                    4.5, 7.5};
  auto expected_result_q_0_ = dealii::FullMatrix<double>(2, 2, expected_result_q_0_values.begin());
  expected_result_q_0_ *= 3;
  auto expected_result_q_1_ = dealii::FullMatrix<double>(2, 2, expected_result_q_1_values.begin());
  expected_result_q_1_ *= 6;
  expected_result_ = expected_result_q_0_;
  expected_result_.add(1, expected_result_q_1_);
  if (dim == 1) {
    expected_result_ *= 10;
  } else if (dim == 2) {
    expected_result_ *= 210;
  } else {
    expected_result_ *= 3210;
  }

  test_formulation_ = std::move(std::make_unique<DriftDiffusionFormulation>(finite_element_mock_ptr_,
                                                                            cross_sections_ptr_,
                                                                            drift_diffusion_calculator_mock_ptr_,
                                                                            angular_flux_integrator_mock_ptr_));
}

TYPED_TEST_SUITE(DriftDiffusionFormulationTest, bart::testing::AllDimensions);

TYPED_TEST(DriftDiffusionFormulationTest, ConstructorAndDependencyGetters) {
  EXPECT_CALL(*this->finite_element_mock_ptr_, dofs_per_cell()).WillOnce(DoDefault());
  EXPECT_CALL(*this->finite_element_mock_ptr_, n_cell_quad_pts()).WillOnce(DoDefault());
  EXPECT_CALL(*this->finite_element_mock_ptr_, n_face_quad_pts()).WillOnce(DoDefault());
  formulation::scalar::DriftDiffusion<this->dim> drift_diffusion(
      this->finite_element_mock_ptr_,
      this->cross_sections_ptr_,
      this->drift_diffusion_calculator_mock_ptr_,
      this->angular_flux_integrator_mock_ptr_);
  EXPECT_NE(drift_diffusion.finite_element_ptr(), nullptr);
  EXPECT_NE(drift_diffusion.cross_sections_ptr(), nullptr);
  EXPECT_NE(drift_diffusion.drift_diffusion_calculator_ptr(), nullptr);
  EXPECT_NE(drift_diffusion.angular_flux_integrator_ptr(), nullptr);
}

TYPED_TEST(DriftDiffusionFormulationTest, ConstructorBadDependencies) {
  const int n_dependencies{ 4 };
  for (int i = 0; i < n_dependencies; ++i) {
    EXPECT_ANY_THROW({
      [[maybe_unused]] formulation::scalar::DriftDiffusion<this->dim> drift_diffusion(
          i == 0 ? this->finite_element_mock_ptr_ : nullptr,
          i == 1 ? this->cross_sections_ptr_ : nullptr,
          i == 2 ? this->drift_diffusion_calculator_mock_ptr_ : nullptr,
          i == 3 ? this->angular_flux_integrator_mock_ptr_ : nullptr);
    });
  }
}

TYPED_TEST(DriftDiffusionFormulationTest, FillDriftDiffusion) {
  constexpr int dim = this->dim;
  const int n_dofs(this->dof_handler_.n_dofs());
  auto& finite_element_mock = *this->finite_element_mock_ptr_;
  auto& drift_diffusion_calculator_mock = *this->drift_diffusion_calculator_mock_ptr_;
  std::array<dealii::Vector<double>, dim> current_vectors_at_dofs;
  const dealii::Vector<double> scalar_flux_at_dofs(this->dof_handler_.n_dofs());
  std::vector<double> integrated_angular_flux_at_q{ test_helpers::RandomVector(this->cell_quadrature_points_, 1, 100) };
  std::vector<double> scalar_flux_at_q{ test_helpers::RandomVector(this->cell_quadrature_points_, 1, 100) };
  const double diffusion_coeff{ this->diffusion_coef_.at(this->material_id_).at(this->energy_group_) };

  std::array<std::vector<double>, dim> current_at_q;
  for (auto& current_component : current_at_q)
    current_component = test_helpers::RandomVector(this->cell_quadrature_points_, 1, 100);
  for (int i = 0; i < dim; ++i) {
    current_vectors_at_dofs.at(i) = dealii::Vector<double>(this->dof_handler_.n_dofs());
    auto random_current{ test_helpers::RandomVector(n_dofs, -100, 100) };
    for (int j = 0; j < n_dofs; ++j) {
      current_vectors_at_dofs.at(i)[j] = random_current.at(j);
    }
    EXPECT_CALL(finite_element_mock, ValueAtQuadrature(Ref(current_vectors_at_dofs.at(i))))
        .WillOnce(Return(current_at_q.at(i)));
  }

  EXPECT_CALL(finite_element_mock, ValueAtQuadrature(scalar_flux_at_dofs))
      .WillOnce(Return(scalar_flux_at_q));
  EXPECT_CALL(finite_element_mock, SetCell(this->cell_ptr_));
  for (int q = 0; q < this->cell_quadrature_points_; ++q) {
    EXPECT_CALL(finite_element_mock, Jacobian(q)).Times(AtLeast(1)).WillRepeatedly(DoDefault());
    for (int i = 0; i < this->dofs_per_cell_; ++i) {
      EXPECT_CALL(finite_element_mock, ShapeGradient(i, q)).Times(AtLeast(1)).WillRepeatedly(DoDefault());
      EXPECT_CALL(finite_element_mock, ShapeValue(i, q)).Times(AtLeast(1)).WillRepeatedly(DoDefault());
      dealii::Tensor<1, dim> current;
      for (int j = 0; j < dim; ++j)
        current[j] = current_at_q.at(j).at(q);

      EXPECT_CALL(drift_diffusion_calculator_mock, DriftDiffusionVector(scalar_flux_at_q.at(q),
                                                                        current,
                                                                        this->GetShapeGradient(i, q),
                                                                        diffusion_coeff))
          .Times(AtLeast(1))
          .WillRepeatedly(DoDefault());
    }
  }
  dealii::FullMatrix<double> cell_matrix(this->dofs_per_cell_, this->dofs_per_cell_);
  cell_matrix = 0;
  this->test_formulation_->FillCellDriftDiffusionTerm(cell_matrix,
                                                      this->cell_ptr_,
                                                      system::EnergyGroup(this->energy_group_),
                                                      scalar_flux_at_dofs,
                                                      current_vectors_at_dofs);
  EXPECT_TRUE(AreEqual(this->expected_result_, cell_matrix));
}

TYPED_TEST(DriftDiffusionFormulationTest, FillCellDriftDiffusionBadMatrixSize) {
  std::vector<int> bad_sizes{this->dofs_per_cell_ - 1, this->dofs_per_cell_ + 1};
  const std::array<dealii::Vector<double>, this->dim> current_vectors_at_dofs;
  const dealii::Vector<double> scalar_flux_at_dofs(this->dof_handler_.n_dofs());

  for (const auto bad_size : bad_sizes) {
    dealii::FullMatrix<double> matrix_bad_rows(bad_size, this->dofs_per_cell_);
    dealii::FullMatrix<double> matrix_bad_cols(this->dofs_per_cell_, bad_size);
    for (auto& matrix : std::array<dealii::FullMatrix<double>, 2>{matrix_bad_rows, matrix_bad_cols}) {
      EXPECT_ANY_THROW({
        this->test_formulation_->FillCellDriftDiffusionTerm(matrix,
                                                            this->cell_ptr_,
                                                            system::EnergyGroup(this->energy_group_),
                                                            scalar_flux_at_dofs,
                                                            current_vectors_at_dofs);
      });
    }
  }

}

TYPED_TEST(DriftDiffusionFormulationTest, FillCellBoundaryTerm) {
  constexpr int dim = this->dim;
  using VectorMap = typename formulation::scalar::DriftDiffusion<this->dim>::VectorMap;
  const int n_dofs(this->dof_handler_.n_dofs());
  const domain::FaceIndex face_index{test_helpers::RandomInt(0, 4)};
  auto& finite_element_mock = *this->finite_element_mock_ptr_;

  // Normal values
  dealii::Tensor<1, dim> normal_tensor;
  dealii::Vector<double> normal_vector(dim);

  for (int dir = 0; dir < dim; ++dir) {
    const double random_value{ test_helpers::RandomDouble(-100, 100) };
    normal_tensor[dir] = random_value;
    normal_vector[dir] = random_value;
  }

  // Boundary factor vectors
  std::vector<double> directional_current{ test_helpers::RandomVector(n_dofs, 1, 100) };
  std::vector<double> directional_flux{ test_helpers::RandomVector(n_dofs, 1, 100) };
  dealii::Vector<double> boundary_factor_at_global_dofs(n_dofs);
  const std::vector<double> boundary_factor_at_quadrature{ 10, 20 };

  const int zero_value_position{ test_helpers::RandomInt(0, n_dofs) };
  directional_flux.at(zero_value_position) = 0;

  for (int i = 0; i < n_dofs; ++i)
    boundary_factor_at_global_dofs[i] = directional_current.at(i)/directional_flux.at(i);
  boundary_factor_at_global_dofs[zero_value_position] = 0;

  // Expected results
  const std::array<double, 4> expected_results_values{ 510, 780, 780, 1200 };
  const dealii::FullMatrix<double> expected_results(2, 2, expected_results_values.begin());
  const formulation::BoundaryType reflective_boundary{ formulation::BoundaryType::kVacuum };
  const VectorMap group_angular_flux_map;

  EXPECT_CALL(finite_element_mock, SetFace(this->cell_ptr_, face_index));
  EXPECT_CALL(finite_element_mock, ValueAtFaceQuadrature(::testing::ContainerEq(boundary_factor_at_global_dofs)))
      .WillOnce(Return(boundary_factor_at_quadrature));
  EXPECT_CALL(finite_element_mock, FaceNormal()).WillOnce(Return(normal_tensor));
  EXPECT_CALL(*this->angular_flux_integrator_mock_ptr_, DirectionalCurrent(group_angular_flux_map, normal_vector))
      .WillOnce(Return(directional_current));
  EXPECT_CALL(*this->angular_flux_integrator_mock_ptr_, DirectionalFlux(group_angular_flux_map, normal_vector))
      .WillOnce(Return(directional_flux));

  for (int q = 0; q < this->cell_quadrature_points_; ++q) {
    EXPECT_CALL(finite_element_mock, FaceJacobian(q)).Times(AtLeast(1)).WillRepeatedly(DoDefault());
    for (int i = 0; i < this->dofs_per_cell_; ++i) {
      EXPECT_CALL(finite_element_mock, FaceShapeValue(i, q)).Times(AtLeast(2)).WillRepeatedly(DoDefault());
    }
  }
  dealii::FullMatrix<double> cell_matrix(this->dofs_per_cell_, this->dofs_per_cell_);
  cell_matrix = 0;
  this->test_formulation_->FillCellBoundaryTerm(cell_matrix,
                                                this->cell_ptr_,
                                                face_index,
                                                reflective_boundary,
                                                group_angular_flux_map);
  EXPECT_TRUE(AreEqual(expected_results, cell_matrix));
}

TYPED_TEST(DriftDiffusionFormulationTest, FillCellBoundaryTermReflective) {
  using VectorMap = typename formulation::scalar::DriftDiffusion<this->dim>::VectorMap;
  const int n_dofs(this->dof_handler_.n_dofs());
  const domain::FaceIndex face_index{test_helpers::RandomInt(0, 4)};
  dealii::Vector<double> boundary_factor_at_global_dofs(n_dofs);
  const std::array<double, 4> expected_results_values{ 0, 0, 0, 0 };
  const dealii::FullMatrix<double> expected_results(2, 2, expected_results_values.begin());
  const formulation::BoundaryType reflective_boundary{ formulation::BoundaryType::kReflective };

  dealii::FullMatrix<double> cell_matrix(this->dofs_per_cell_, this->dofs_per_cell_);
  cell_matrix = 0;
  this->test_formulation_->FillCellBoundaryTerm(cell_matrix,
                                                this->cell_ptr_,
                                                face_index,
                                                reflective_boundary,
                                                VectorMap());
  EXPECT_TRUE(AreEqual(expected_results, cell_matrix));
}

TYPED_TEST(DriftDiffusionFormulationTest, FillCellBoundaryTermBadMatrixSize) {
  using VectorMap = typename formulation::scalar::DriftDiffusion<this->dim>::VectorMap;
  std::vector<int> bad_sizes{this->dofs_per_cell_ - 1, this->dofs_per_cell_ + 1};
  dealii::Vector<double> boundary_factor_at_global_dofs(this->dof_handler_.n_dofs());
  const domain::FaceIndex face_index{test_helpers::RandomInt(0, 4)};
  const formulation::BoundaryType reflective_boundary{ formulation::BoundaryType::kVacuum };
  for (const auto bad_size : bad_sizes) {
    dealii::FullMatrix<double> matrix_bad_rows(bad_size, this->dofs_per_cell_);
    dealii::FullMatrix<double> matrix_bad_cols(this->dofs_per_cell_, bad_size);
    for (auto& matrix : std::array<dealii::FullMatrix<double>, 2>{matrix_bad_rows, matrix_bad_cols}) {
      EXPECT_ANY_THROW({
                         this->test_formulation_->FillCellBoundaryTerm(matrix,
                                                                       this->cell_ptr_,
                                                                       face_index,
                                                                       reflective_boundary,
                                                                       VectorMap());
                       });
    }
  }
}

} // namespace


/**
 * GeoDa TM, Copyright (C) 2011-2015 by Luc Anselin - all rights reserved
 *
 * This file is part of GeoDa.
 *
 * GeoDa is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * GeoDa is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <list>
#include <map>
#include <set>
#include <boost/foreach.hpp>
#include <boost/random.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <wx/stopwatch.h>
#include "../SpatialIndAlgs.h"
#include "../GenGeomAlgs.h"
#include "../PointSetAlgs.h"
#include "../logger.h"
#include "CorrelogramAlgs.h"

void CorrelogramAlgs::GetSampMeanAndVar(const std::vector<double>& Z_,
                                        const std::vector<bool>& Z_undef,
										double& mean, double& var)
{
    // get valid Z using Z_undef
    std::vector<double> Z;
    for (size_t i=0; i<Z_.size(); i++) {
        if (Z_undef[i] == false) {
            Z.push_back(Z_[i]);
        }
    }
    // get mean & var
	size_t Z_sz = Z.size();
	double N = (double) Z_sz;
	double sum = 0.0;
    for (size_t i=0; i<Z_sz; ++i) {
        sum += Z[i];
    }
	double smpl_mn = sum/N;
	double ssd = 0.0;
	double diff = 0;
	for (size_t i=0; i<Z_sz; ++i) {
		diff = Z[i] - smpl_mn;
		ssd += diff*diff;
	}
	double smpl_var = ssd/N;

	mean = smpl_mn;
	var = smpl_var;
}

bool CorrelogramAlgs::MakeCorrRandSamp(const std::vector<wxRealPoint>& pts,
									   const std::vector<double>& Z,
									   const std::vector<bool>& Z_undef,
									   bool is_arc,
									   double dist_cutoff,
									   int num_bins, int iters,
									   std::vector<CorreloBin>& out)
{
	using namespace std;
	using namespace GenGeomAlgs;
	LOG_MSG("Entering CorrelogramAlgs::MakeCorrRandSamp");
	wxStopWatch sw;

	// Mersenne Twister random number generator, randomly seeded
	// with current time in seconds since Jan 1 1970.
	static boost::mt19937 rng(std::time(0));
	static boost::random::uniform_int_distribution<> X(0, pts.size()-1);

	double nbins_d = (double) num_bins;
	if (dist_cutoff <= 0) {
		wxRealPoint a,b;
		dist_cutoff = PointSetAlgs::EstDiameter(pts, is_arc, a, b);
		if (is_arc) dist_cutoff = DegToRad(dist_cutoff);
	}

	bool calc_prods = (Z.size() == pts.size());
	double mean = 0;
	double var = 0;
	if (calc_prods) {
		GetSampMeanAndVar(Z, Z_undef, mean, var);
		if (var <= 0) {
			return false;
		}
	}
	if (num_bins <= 0)
        num_bins = 1;
	out.clear();
	out.resize(num_bins);
	double binw = dist_cutoff/nbins_d; // bin width
	out[0].dist_min = 0;
	out[0].dist_max = binw;
	for (int i=1; i<num_bins; ++i) {
		out[i].dist_min = binw*((double) i);
		out[i].dist_max = binw*((double) (i+1));
		out[i].corr_avg_valid = calc_prods;
	}

	int ta_cnt = 0; // throw away count
    int t = 0;
    int t_max = 0;
    int t_max_const = iters + 9999999;
    while (t < iters ) {
		size_t i=X(rng);
		size_t j=X(rng);

        // potential hangs here
        if (Z_undef.size() > 0 && (Z_undef[i] || Z_undef[j])) {
            if (t_max < t_max_const) {
                continue;
            } else {
                // ERROR: too many undefined values, computing hangs and exits.
                return false;
            }
            t_max += 1;
        }

        t += 1;

		const wxRealPoint& p1(pts[i]);
		const wxRealPoint& p2(pts[j]);
		double d;

        // NOTE: performance boost here: cache the computed distance
        // the cache could have a limited size e.g. 10000
		if (is_arc) {
			d = ComputeArcDistRad(p1.x, p1.y, p2.x, p2.y);
		} else {
			d = ComputeEucDist(p1.x, p1.y, p2.x, p2.y);
		}
		int b = (int) (d/binw);
		if (b >= num_bins || b<0) {
			ta_cnt++;
			continue;
		}
		out[b].num_pairs++;
		if (calc_prods && var != 0) {
			double prod = (Z[i]-mean)*(Z[j]-mean)/var;
			out[b].corr_avg += prod;
		}
	}
	for (size_t b=0; b<num_bins; ++b) {
		if (out[b].num_pairs >0 && calc_prods) {
			out[b].corr_avg /= ((double) out[b].num_pairs);
		}
	}

	LOG_MSG("Exiting CorrelogramAlgs::MakeCorrRandSamp");
	return true;
}

bool CorrelogramAlgs::MakeCorrAllPairs(const std::vector<wxRealPoint>& pts,
									   const std::vector<double>& Z,
                                       const std::vector<bool>& Z_undef,
									   bool is_arc, int num_bins,
									   std::vector<CorreloBin>& out)

{
	using namespace std;
	using namespace GenGeomAlgs;
	LOG_MSG("Entering CorrelogramAlgs::MakeCorrAllPairs");
	wxStopWatch sw;

	size_t nobs = pts.size();

	bool calc_prods = (Z.size() == nobs);
	double mean = 0;
	double var = 0;

	if (calc_prods) {
		GetSampMeanAndVar(Z, Z_undef, mean, var);
		if (var <= 0) {
			return false;
		}
	}

	double min_d, max_d;
	min_d = (is_arc ?
             ComputeArcDistRad(pts[0].x, pts[0].y, pts[1].x, pts[1].y) :
			 ComputeEucDist(pts[0].x, pts[0].y, pts[1].x, pts[1].y));
	max_d = min_d;

	size_t pairs = ((nobs-1)*nobs)/2;
	vector<double> Zdist(pairs);
	vector<double> Zprod(calc_prods ? pairs : 0);
	vector<double> Zprod_undef(calc_prods ? pairs : 0);
	size_t pc = 0;
	for (size_t i=0; i<nobs; ++i) {
		for (size_t j=i+1; j<nobs; ++j) {

            if (Z_undef.size() > 0 && (Z_undef[i] || Z_undef[j])) {
                Zprod_undef[pc] = true;
                Zdist[pc] = 0;
                Zprod[pc] = 0;
                continue;
            }

            Zprod_undef[pc] = false;
			double d = (is_arc ?
                        ComputeArcDistRad(pts[i].x, pts[i].y,pts[j].x, pts[j].y) :
						ComputeEucDist(pts[i].x, pts[i].y, pts[j].x, pts[j].y));
			if (d < min_d) {
				min_d = d;
			} else if (d > max_d) {
				max_d = d;
			}
			Zdist[pc] = d;
            if (calc_prods && var != 0) {
                Zprod[pc] = (Z[i]-mean)*(Z[j]-mean)/var;
            }
			++pc;
		}
	}

    if (num_bins <= 0) {
        num_bins = 1;
    }
	out.clear();
	out.resize(num_bins);
	double nbins_d = (double) num_bins;
	double binw = max_d/nbins_d; // bin width
	out[0].dist_min = 0;
	out[0].dist_max = binw;
	for (int i=1; i<num_bins; ++i) {
		out[i].dist_min = binw*((double) i);
		out[i].dist_max = binw*((double) (i+1));
		out[i].corr_avg_valid = calc_prods;
	}

	size_t ta_cnt = 0;
	for (size_t i=0, sz=Zdist.size(); i<sz; ++i) {
        if (wxIsNaN(Zdist[i]) || Zprod_undef[i]) {
            // todo something wrong here, Zdist value is NaN
            continue;
        }

		int b = (int) (Zdist[i]/binw);
		if (b >= num_bins) {
			b=num_bins-1;
			ta_cnt++;
		}
		out[b].num_pairs++;
        if (calc_prods) {
            out[b].corr_avg += Zprod[i];
        }
	}
	LOG(ta_cnt); // should be at most 1

	for (size_t b=0; b<num_bins; ++b) {
		if (out[b].num_pairs != 0 && calc_prods) {
            out[b].corr_avg /= ((double) out[b].num_pairs);
        }
	}

	/*
		stringstream ss;
		ss << "MakeCorrMakeCorrAllPairs with " << pairs
		   << " pairs finished in " << sw.Time() << " ms.";
		LOG_MSG(ss.str());
	*/
	LOG_MSG("Exiting CorrelogramAlgs::MakeCorrAllPairs");
	return true;
}

bool CorrelogramAlgs::MakeCorrThresh(const rtree_pt_2d_t& rtree,
									 const std::vector<double>& Z,
                                     const std::vector<bool>& Z_undef,
									 double thresh, int num_bins,
									 std::vector<CorreloBin>& out)
{
	using namespace std;
	using namespace GenGeomAlgs;
	using namespace SpatialIndAlgs;
	LOG_MSG("Entering CorrelogramAlgs::MakeCorrThresh (plane)");
	wxStopWatch sw;

	if (thresh <= 0) return false;
	bool calc_prods = (Z.size() == rtree.size());
	double nbins_d = (double) num_bins;

	if (num_bins <= 0) num_bins = 1;
	out.clear();
	out.resize(num_bins);
	double binw = thresh/nbins_d; // bin width
	out[0].dist_min = 0;
	out[0].dist_max = binw;
	for (int i=1; i<num_bins; ++i) {
		out[i].dist_min = binw*((double) i);
		out[i].dist_max = binw*((double) (i+1));
		out[i].corr_avg_valid = calc_prods;
	}

	double mean= 0;
	double var=0;
	if (calc_prods) {
		GetSampMeanAndVar(Z, Z_undef, mean, var);
		if (var <= 0) {
			//LOG_MSG("Error: non-positive variance calculated");
			return false;
		}
	}

	int ta_cnt = 0; // throw away count
	int pairs_cnt = 0; // pairs count
	for (rtree_pt_2d_t::const_query_iterator it =
			 rtree.qbegin(bgi::intersects(rtree.bounds()));
		 it != rtree.qend() ; ++it)
	{
		const pt_2d_val& v = *it;
		const size_t i = v.second;
		double x = v.first.get<0>();
		double y = v.first.get<1>();
		box_2d b(pt_2d(x-thresh, y-thresh), pt_2d(x+thresh, y+thresh));
		vector<pt_2d_val> q;
		rtree.query(bgi::intersects(b), std::back_inserter(q));
		BOOST_FOREACH(const pt_2d_val& w, q) {
			const size_t j = w.second;
			if (i >= j) continue;
            if (Z_undef.size() > 0 && (Z_undef[i] || Z_undef[j]))
                continue;

			double d = bg::distance(v.first, w.first);
			int b = (int) (d/binw);
			if (b >= num_bins || b<0) {
				++ta_cnt;
				continue;
			}

			++pairs_cnt;
			out[b].num_pairs++;
			if (calc_prods && var !=0) {
                double prod = (Z[i]-mean)*(Z[j]-mean)/var;
                out[b].corr_avg += prod;
			}
		}
	}

	for (size_t b=0; b<num_bins; ++b) {
		if (out[b].num_pairs != 0 && calc_prods) {
            out[b].corr_avg /= ((double) out[b].num_pairs);
		}
	}

	/*
		stringstream ss;
		ss << "MakeCorrThresh with threshold " << thresh
		   << " finished in " << sw.Time() << " ms.";
		LOG_MSG(ss.str());
	*/
	LOG_MSG("Exiting CorrelogramAlgs::MakeCorrThresh (plane)");
	return true;
}

/** thresh is assumed to be in radians.  Output is also in radians. */
bool CorrelogramAlgs::MakeCorrThresh(const rtree_pt_3d_t& rtree,
                                     const std::vector<double>& Z,
                                     const std::vector<bool>& Z_undef,
                                     double thresh, int num_bins,
                                     std::vector<CorreloBin>& out)
{
	using namespace std;
	using namespace GenGeomAlgs;
	using namespace SpatialIndAlgs;
	LOG_MSG("Entering CorrelogramAlgs::MakeCorrThresh (sphere)");
	wxStopWatch sw;

	if (thresh <= 0) return false;
	bool calc_prods = (Z.size() == rtree.size());
	double nbins_d = (double) num_bins;
	if (num_bins <= 0) num_bins = 1;
	out.clear();
	out.resize(num_bins);
	double binw = thresh/nbins_d; // bin width
	out[0].dist_min = 0;
	out[0].dist_max = binw;
	for (int i=1; i<num_bins; ++i) {
		out[i].dist_min = binw*((double) i);
		out[i].dist_max = binw*((double) (i+1));
		out[i].corr_avg_valid = calc_prods;
	}

	double mean= 0;
	double var=0;
	if (calc_prods) {
		GetSampMeanAndVar(Z, Z_undef, mean, var);
		if (var <= 0) {
			//LOG_MSG("Error: non-positive variance calculated");
			return false;
		}
	}

	// thresh is in radians.  Need to convert to unit sphere secant distance
	double sec_thresh = RadToUnitDist(thresh);

	int ta_cnt = 0; // throw away count
	int pairs_cnt = 0; // pairs count
	for (rtree_pt_3d_t::const_query_iterator it =
			 rtree.qbegin(bgi::intersects(rtree.bounds()));
			 it != rtree.qend() ; ++it)
	{
		const pt_3d_val& v = *it;
		const size_t i = v.second;
		double x = v.first.get<0>();
		double y = v.first.get<1>();
		double z = v.first.get<2>();
		box_3d b(pt_3d(x-sec_thresh, y-sec_thresh, z-sec_thresh),
						 pt_3d(x+sec_thresh, y+sec_thresh, z+sec_thresh));
		vector<pt_3d_val> q;
		rtree.query(bgi::intersects(b), std::back_inserter(q));
		BOOST_FOREACH(const pt_3d_val& w, q) {
			const size_t j = w.second;
			if (i >= j) continue;
            if (Z_undef.size() > 0 && (Z_undef[i] || Z_undef[j]))
                continue;
			double d = bg::distance(v.first, w.first);
			double rad_d = UnitDistToRad(d);

			int b = (int) (rad_d/binw);
			if (b >= num_bins || b<0) {
				++ta_cnt;
				continue;
			}
			++pairs_cnt;
			out[b].num_pairs++;
			if (calc_prods && var != 0) {
				double prod = (Z[i]-mean)*(Z[j]-mean)/var;
				out[b].corr_avg += prod;
			}
		}
	}

	for (size_t b=0; b<num_bins; ++b) {
		if (out[b].num_pairs != 0 && calc_prods) {
			out[b].corr_avg /= ((double) out[b].num_pairs);
		}
	}

	/*
		stringstream ss;
		ss << "MakeCorrThresh with threshold " << thresh
		<< " finished in " << sw.Time() << " ms.";
		LOG_MSG(ss.str());
	*/
	LOG_MSG("Exiting CorrelogramAlgs::MakeCorrThresh (sphere)");
	return true;
}


/****************************************************************************
 *  Copyright (C) 2019 RoboMaster.
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program. If not, see <http://www.gnu.org/licenses/>.
 ***************************************************************************/

#include <tf/tf.h>
#include <tf/transform_listener.h>

#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>

#include <Eigen/Eigen>
#include <chrono>

#include "utility.h"

namespace robomaster{

class PIDPlanner{
 public:
  PIDPlanner(ros::NodeHandle& given_nh):nh(given_nh),plan_(false){

    nh.param<double>("max_x_speed", max_x_speed_, 1.0);
    nh.param<double>("max_y_speed", max_y_speed_, 1.0);
    nh.param<double>("max_yaw_speed", max_yaw_speed_, 2.0);
    nh.param<double>("p_x_coeff", p_x_coeff_, 1);
    nh.param<double>("p_y_coeff", p_y_coeff_, 1);
    nh.param<double>("p_yaw_coeff", p_yaw_coeff_, 1);
    nh.param<int>("plan_frequency", plan_freq_, 30);
    nh.param<double>("goal_dist_tolerance", goal_dist_tolerance_, 0.1);
    nh.param<double>("goal_angle_tolerance", goal_angle_tolerance_, 0.05);

    tf_listener_ = std::make_shared<tf::TransformListener>();

    cmd_vel_pub_ = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);

    goal_sub_ = nh.subscribe("/move_base_simple/goal", 5, &PIDPlanner::GoalCallback,this);

    plan_timer_ = nh.createTimer(ros::Duration(1.0/plan_freq_),&PIDPlanner::Plan,this);

  }
  ~PIDPlanner()= default;
  void GoalCallback(const geometry_msgs::PoseStamped::ConstPtr & msg){
   goal_=*msg;
   plan_=true;
  }
 private:

  void Plan(const ros::TimerEvent& event){

    if (plan_){

      geometry_msgs::PoseStamped goal_pose;
      goal_.header.stamp=ros::Time::now();
      //1. Get goal pose in the base_link frame
      if (!TransformPose(tf_listener_,"base_link", goal_, goal_pose)){
        return;
      }
      //2. Get yaw angle difference
      double diff_yaw = GetYawFromOrientation(goal_pose.pose.orientation);
      geometry_msgs::Twist cmd_vel;

      //3. Check if current robot position is near the goal position within the distance tolerance
      //   - If so, Check if robot orientation is near the goal orientation:
      //            - If so, planning succeed ,
      //            - If not, use p controller to control robot turn to goal orientation
      //   - If not, use p controller to control robot move to goal position without turning
      if (hypot(goal_pose.pose.position.x,goal_pose.pose.position.y)<= goal_dist_tolerance_){

        if(std::abs(diff_yaw) < goal_angle_tolerance_){
          plan_ = false;
          geometry_msgs::Twist cmd_vel;
          cmd_vel.linear.x = 0;
          cmd_vel.linear.y = 0;
          cmd_vel.angular.z = 0;
          cmd_vel_pub_.publish(cmd_vel);
          ROS_INFO("Planning Success!");
          return;
        } else{
          if( diff_yaw > 0 ){
          cmd_vel.angular.z = std::min(p_yaw_coeff_*diff_yaw, max_yaw_speed_);
          }else{
          cmd_vel.angular.z = std::max(p_yaw_coeff_*diff_yaw, -max_yaw_speed_);
          }
          cmd_vel.linear.x = 0;
          cmd_vel.linear.y = 0;
        }

      } else{
        if( goal_pose.pose.position.x > 0 ){
          cmd_vel.linear.x = std::min(p_x_coeff_*goal_pose.pose.position.x, max_x_speed_);
        }else{
          cmd_vel.linear.x  = std::max(p_x_coeff_*goal_pose.pose.position.x, -max_x_speed_);
        }

        if( goal_pose.pose.position.y > 0 ){
          cmd_vel.linear.y = std::min(p_y_coeff_*goal_pose.pose.position.y, max_y_speed_);
        }else{
          cmd_vel.linear.y  = std::max(p_y_coeff_*goal_pose.pose.position.y, -max_y_speed_);
        }

         cmd_vel.angular.z = 0;
      }

      double x = cmd_vel.linear.x;
      cmd_vel.linear.x = -cmd_vel.linear.y;
      cmd_vel.linear.y = x;
      cmd_vel.angular.z*=-1.0;
      cmd_vel_pub_.publish(cmd_vel);

    }

  }

 private:

  ros::NodeHandle nh;
  std::shared_ptr<tf::TransformListener> tf_listener_;

  geometry_msgs::PoseStamped goal_;
  ros::Timer plan_timer_;

  ros::Subscriber goal_sub_;
  ros::Publisher cmd_vel_pub_;

  bool plan_;

  double max_x_speed_, max_y_speed_, max_yaw_speed_;
  double p_x_coeff_,p_y_coeff_,p_yaw_coeff_;
  double goal_dist_tolerance_,goal_angle_tolerance_;

  int plan_freq_;

};
}

using namespace robomaster;
int main(int argc, char **argv)
{
  ros::init(argc, argv, "pid_planner");
  ros::NodeHandle nh("~");
  PIDPlanner pid_planner(nh);
  ros::spin();
  return 0;
}


/**
 * @file    QuadOctreeBase.hpp
 *
 * @date    2019-08-24
 *
 * Copyright (c) organization
 *
 */

#pragma once

#include <Eigen/Eigen>
#include <algorithm>
#include <array>
#include <functional>
#include <limits>
#include <numeric>
#include <queue>
#include <string>
#include <utility>
#include <vector>

#ifdef WITH_DEBUG
#define ENABLE_DEBUG 1
#endif

namespace algo
{
/**
 *  @brief Base Class for both Quadtree and Octree
 *
 */
template <typename DATA_TYPE, size_t POINT_DIMENSION,
          class PointContainer = Eigen::Matrix<DATA_TYPE, POINT_DIMENSION, 1>>
class QuadOctreeBase
{
 public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    using PointType = PointContainer;

    using VecPointType = std::vector<PointType, Eigen::aligned_allocator<PointType>>;

    //! array type to store point coordinates
    using PointCoordinates = std::array<DATA_TYPE, POINT_DIMENSION>;

    //! constant number of children stored in each node
    static constexpr size_t NUM_CHILD = std::pow(2, POINT_DIMENSION);

    /**
     *  @brief constructor to initialize member variables and also contruct the tree
     *
     *  @param points points to be stored in the trees
     *  @param maxLevel max level of tree nodes
     *  @param maxNumPoints max number of points to be stored in each node
     *  @param offsetMinPoint offset min point to control the boundary of the tree space
     *  @param offsetMaxPoint offset max point to control the boundary of the tree space
     */
    QuadOctreeBase(const VecPointType& points, uint8_t maxLevel, uint32_t maxNumPoints, const PointType& offsetMinPoint,
                   const PointType& offsetMaxPoint, const std::string& nodeName = "QuadOctreeBaseNode");

    /**
     *  @brief destructor to delete the root node
     *
     */
    ~QuadOctreeBase();

    /**
     *  @brief constant getter of the points member variable
     *
     */
    const VecPointType& points() const;

    /**
     *  @brief inorder traversal of the tree from root node
     *  @return coordinates about points stored in each node and center point of the node itself
     */
    virtual std::stringstream traversal() const;

    /**
     *  @brief check if a point is in the tree's boundary space
     *
     *  @param query the query point
     *  @return true if the point lies in the boudary space; false the otherwise
     */
    virtual bool insideSpace(const PointType& query) const;

    /**
     *  @brief find one neighbor point of a point (knn with k=1)
     *
     *  @param query the query point
     *  @param minDistance min distance from the query point to the neighbor point
     *  @return index of the neighbor point
     */
    virtual uint32_t findNeighbor(const PointType& query, DATA_TYPE minDistance = -1) const;

    /**
     *  @brief find k neighbors of a point
     *
     *  @param query the query point
     *  @param k number of neighbors to find
     *  @param minDistance min distance from the query point to the neighbor point
     *  @return vector of indices of the neighbors
     */
    virtual std::vector<uint32_t> knn(const PointType& query, uint32_t k, DATA_TYPE minDistance = -1) const;

    std::array<std::vector<DATA_TYPE>, POINT_DIMENSION> extractDataEachAxis(const VecPointType& vecPointType) const;

    virtual void drawOctree(const std::function<void(const PointCoordinates&, const DATA_TYPE)>& func) const
    {
        this->drawOctree(this->_root, func);
    }

 protected:
    /**
     *  @brief struct of each node stored in the tree
     *
     */
    struct QuadOctreeBaseNode {
        /**
         *  @brief constructor with default value
         *
         */
        QuadOctreeBaseNode();

        /**
         *  @brief destructor to delete pointers to child nodes
         *
         */
        ~QuadOctreeBaseNode();

        //! flag to check if the current node is leaf or not
        bool isLeaf;

        //! coordinate values of the center point of the node space
        PointCoordinates coordinates;

        //! "square" radius of the node space
        DATA_TYPE radius;

        //! start index of the points stored in this node
        uint32_t startIdx;

        //! end index of the points stored in this node
        uint32_t endIdx;

        //! number of points stored in this node
        uint32_t numPoints;

        //! level of this node
        uint8_t level;

        //! pointers to child nodes
        QuadOctreeBaseNode* child[NUM_CHILD];
    };

    //! struct to represent the bounding box with coordinates of the center points and "square" radius
    struct BoundingBox {
        //! coordinates of the center point
        PointCoordinates coordinates;

        //! "square" radius
        DATA_TYPE radius;
    };

    /**
     *  @brief create a new tree node
     *
     *  @param coordinates coordinates of the center point of the node
     *  @param radius "square" radius
     *  @param startIdx start index of the points stored in this node
     *  @param endIdx end index of the points stored in this node
     *  @param numPoints number of points stored in this node
     *  @param level level of this node
     *  @return pointer to the new generated node
     */
    QuadOctreeBaseNode* createQuadOctreeBaseNode(const PointCoordinates& coordinates, DATA_TYPE radius,
                                                 uint32_t startIdx, uint32_t endIdx, uint32_t numPoints, uint8_t level);

    /**
     *  @brief block copy constructor
     *
     */
    QuadOctreeBase(const QuadOctreeBase&);

    /**
     *  @brief check if a circle (quadtree case) or a ball (octree case) S(query, r) overlaps with the node space
     *
     *  @param query the query point
     *  @param radius the radius of the circle (or ball)
     *  @param quadOctreeBaseNode pointer to constant node
     *  @return true if overlapped; false the otherwise
     */
    bool overlaps(const PointType& query, DATA_TYPE radius, const QuadOctreeBaseNode* quadOctreeBaseNode) const;

    /**
     *  @brief check if a circle (quadtree case) or a ball (octree case) S(query, r) is inside the node space
     *
     *  @param query the query point
     *  @param radius the radius of the circle (or ball)
     *  @param quadOctreeBaseNode pointer to constant node
     *  @return true if inside; false the otherwise
     */
    bool inside(const PointType& query, DATA_TYPE radius, const QuadOctreeBaseNode* quadOctreeBaseNode) const;

    /**
     *  @brief check if a point lies inside the node space
     *
     *  @param query the query point
     *  @param radius the radius of the circle (or ball)
     *  @param quadOctreeBaseNode pointer to constant node
     *  @return true if inside; false the otherwise
     */
    bool inside(const PointType& query, const QuadOctreeBaseNode* quadOctreeBaseNode) const;

    /**
     *  @brief estimate the boundary that cover all the input points
     *
     *  @param points input vector of points
     *  @param offsetMinPoint offset min point to control the size of the boundary
     *  @param offsetMaxPoint offset max point to control the size of the boundary
     *  @return bounding box of the boundary space
     */
    const BoundingBox estimateBounds(const VecPointType& points, const PointType& offsetMinPoint,
                                     const PointType& offsetMaxPoint) const;

    /**
     *  @brief find one neighbor point of a point (knn with k=1) starting from the target node
     *
     *  @param query the query point
     *  @param minDistance min distance from the query point to the neighbor point
     *  @param quadOctreeBaseNode pointer to constant node
     *  @return index of the neighbor point
     */
    uint32_t findNeighbor(const PointType& query, DATA_TYPE minDistance,
                          const QuadOctreeBaseNode* quadOctreeBaseNode) const;

    /**
     *  @brief find k neighbors of a point starting from the target node
     *
     *  @param query the query point
     *  @param k number of neighbors to find
     *  @param minDistance min distance from the query point to the neighbor point
     *  @param quadOctreeBaseNode pointer to constant node
     *  @return vector of indices of the neighbors
     */
    std::vector<uint32_t> knn(const PointType& query, uint32_t k, DATA_TYPE minDistance,
                              const QuadOctreeBaseNode* quadOctreeBaseNode) const;

    /**
     *  @brief inorder traversal of the tree starting from the target node
     *  @param quadOctreeBaseNode pointer to constant node
     *  @return coordinates about points stored in each node and center point of the node itself
     */
    std::stringstream traversal(const QuadOctreeBaseNode* quadOctreeBaseNode) const;

    void drawOctree(const QuadOctreeBaseNode* quadOctreeBaseNode,
                    const std::function<void(const PointCoordinates&, const DATA_TYPE)>& func) const;

 protected:
    //! vector of points held inside the tree
    VecPointType _points;

    //! pointer to root node
    QuadOctreeBaseNode* _root;

    //! vector to store the successive index of the point that follows the index of the current point.
    //! Supposed we have the vector of n points (p0,p1,...,p_(n-1)) with the index (i0=0;i1=1;...;i(n-1)=n-1).
    //! Then _successors[i_(j)] = i_(j+1) for j = [0,(n-1)].
    //! When a new permutation of n points is needed; we can keep the vector of points intact; and modify
    //! this successors vector
    std::vector<uint32_t> _successors;

    //! max level of a node
    uint8_t _maxLevel;

    //! max number of points stored in a node
    uint32_t _maxNumPoints;

    //! the name of node struct (quadrant for quadtree, octant for octree)
    std::string _nodeName;
};

template <typename DATA_TYPE, size_t POINT_DIMENSION, class PointContainer>
QuadOctreeBase<DATA_TYPE, POINT_DIMENSION, PointContainer>::QuadOctreeBase(const VecPointType& points, uint8_t maxLevel,
                                                                           uint32_t maxNumPoints,
                                                                           const PointType& offsetMinPoint,
                                                                           const PointType& offsetMaxPoint,
                                                                           const std::string& nodeName)
    : _points(points), _maxLevel(maxLevel), _maxNumPoints(maxNumPoints), _nodeName(nodeName)
{
    const uint32_t N = this->_points.size();

    this->_successors.resize(N);
    std::iota(this->_successors.begin(), this->_successors.end(), 1);

    const BoundingBox bbox = this->estimateBounds(this->_points, offsetMinPoint, offsetMaxPoint);

    this->_root = this->createQuadOctreeBaseNode(bbox.coordinates, bbox.radius, 0, N - 1, N, 0);
}

template <typename DATA_TYPE, size_t POINT_DIMENSION, class PointContainer>
QuadOctreeBase<DATA_TYPE, POINT_DIMENSION, PointContainer>::~QuadOctreeBase()
{
    free(_root);
}

template <typename DATA_TYPE, size_t POINT_DIMENSION, class PointContainer>
const typename QuadOctreeBase<DATA_TYPE, POINT_DIMENSION, PointContainer>::VecPointType&
QuadOctreeBase<DATA_TYPE, POINT_DIMENSION, PointContainer>::points() const
{
    return this->_points;
}

template <typename DATA_TYPE, size_t POINT_DIMENSION, class PointContainer>
bool QuadOctreeBase<DATA_TYPE, POINT_DIMENSION, PointContainer>::insideSpace(const PointType& query) const
{
    return this->inside(query, this->_root);
}

template <typename DATA_TYPE, size_t POINT_DIMENSION, class PointContainer>
typename QuadOctreeBase<DATA_TYPE, POINT_DIMENSION, PointContainer>::QuadOctreeBaseNode*
QuadOctreeBase<DATA_TYPE, POINT_DIMENSION, PointContainer>::createQuadOctreeBaseNode(
    const PointCoordinates& coordinates, DATA_TYPE radius, uint32_t startIdx, uint32_t endIdx, uint32_t numPoints,
    uint8_t level)
{
    QuadOctreeBaseNode* quadOctreeBaseNode = new QuadOctreeBaseNode;
    quadOctreeBaseNode->isLeaf = true;

    quadOctreeBaseNode->coordinates = coordinates;
    quadOctreeBaseNode->radius = radius;
    quadOctreeBaseNode->startIdx = startIdx;
    quadOctreeBaseNode->endIdx = endIdx;
    quadOctreeBaseNode->numPoints = numPoints;
    quadOctreeBaseNode->level = level;

    static const DATA_TYPE factor[] = {-0.5f, 0.5f};

    if (numPoints > this->_maxNumPoints && level < this->_maxLevel) {
        quadOctreeBaseNode->isLeaf = false;
        std::vector<uint32_t> childStarts(NUM_CHILD, 0);
        std::vector<uint32_t> childEnds(NUM_CHILD, 0);
        std::vector<uint32_t> childSizes(NUM_CHILD, 0);

        uint32_t idx = startIdx;

        for (uint32_t i = 0; i < numPoints; ++i) {
            const PointType& curPoint = this->_points[idx];
            uint32_t mortonCode = 0;

            for (int i = 0; i < POINT_DIMENSION; ++i) {
                if (curPoint[i] > coordinates[i]) {
                    mortonCode |= (1 << i);
                }
            }

            if (childSizes[mortonCode] == 0) {
                childStarts[mortonCode] = idx;
            } else {
                this->_successors[childEnds[mortonCode]] = idx;
            }

            childSizes[mortonCode] += 1;

            childEnds[mortonCode] = idx;
            idx = this->_successors[idx];
        }

        const DATA_TYPE childRadius = 0.5f * radius;
        bool firstTime = true;
        uint32_t lastChildIdx = 0;

        for (uint32_t i = 0; i < NUM_CHILD; ++i) {
            if (childSizes[i] == 0) {
                continue;
            }

            PointCoordinates childCoordinates;
            for (int j = 0; j < POINT_DIMENSION; ++j) {
                childCoordinates[j] = coordinates[j] + factor[(i & (1 << j)) > 0] * radius;
            }

            quadOctreeBaseNode->child[i] = this->createQuadOctreeBaseNode(childCoordinates, childRadius, childStarts[i],
                                                                          childEnds[i], childSizes[i], level + 1);

            if (firstTime) {
                quadOctreeBaseNode->startIdx = quadOctreeBaseNode->child[i]->startIdx;
            } else {
                this->_successors[quadOctreeBaseNode->child[lastChildIdx]->endIdx] =
                    quadOctreeBaseNode->child[i]->startIdx;
            }

            lastChildIdx = i;
            quadOctreeBaseNode->endIdx = quadOctreeBaseNode->child[i]->endIdx;
            firstTime = false;
        }
    }

    return quadOctreeBaseNode;
}

template <typename DATA_TYPE, size_t POINT_DIMENSION, class PointContainer>
bool QuadOctreeBase<DATA_TYPE, POINT_DIMENSION, PointContainer>::overlaps(
    const PointType& query, DATA_TYPE radius, const QuadOctreeBaseNode* quadOctreeBaseNode) const
{
    DATA_TYPE maxDist = radius + quadOctreeBaseNode->radius;

    std::array<DATA_TYPE, POINT_DIMENSION> delta;
    for (size_t i = 0; i < POINT_DIMENSION; ++i) {
        delta[i] = query[i] - quadOctreeBaseNode->coordinates[i];
        delta[i] = std::abs(delta[i]);

        if (delta[i] > maxDist) {
            return false;
        }
    }

    for (size_t i = 0; i < POINT_DIMENSION; ++i) {
        if (delta[i] < quadOctreeBaseNode->radius) {
            return true;
        }
    }

    DATA_TYPE sqrDistToEdge = 0.0;
    for (size_t i = 0; i < POINT_DIMENSION; ++i) {
        delta[i] = std::max(delta[i] - quadOctreeBaseNode->radius, static_cast<DATA_TYPE>(0.0f));
        sqrDistToEdge += std::pow(delta[i], 2);
    }

    return (std::sqrt(sqrDistToEdge) < radius);
}

template <typename DATA_TYPE, size_t POINT_DIMENSION, class PointContainer>
bool QuadOctreeBase<DATA_TYPE, POINT_DIMENSION, PointContainer>::inside(
    const PointType& query, DATA_TYPE radius, const QuadOctreeBaseNode* quadOctreeBaseNode) const
{
    std::array<DATA_TYPE, POINT_DIMENSION> delta;
    for (size_t i = 0; i < POINT_DIMENSION; ++i) {
        DATA_TYPE iDelta = query[i] - quadOctreeBaseNode->coordinates[i];
        iDelta = std::abs(iDelta) + radius;

        if (iDelta > quadOctreeBaseNode->radius) {
            return false;
        }
    }

    return true;
}

template <typename DATA_TYPE, size_t POINT_DIMENSION, class PointContainer>
bool QuadOctreeBase<DATA_TYPE, POINT_DIMENSION, PointContainer>::inside(
    const PointType& query, const QuadOctreeBaseNode* quadOctreeBaseNode) const
{
    for (size_t i = 0; i < POINT_DIMENSION; ++i) {
        DATA_TYPE iDelta = query[i] - quadOctreeBaseNode->coordinates[i];
        iDelta = std::abs(iDelta);

        if (iDelta > quadOctreeBaseNode->radius) {
            return false;
        }
    }

    return true;
}

template <typename DATA_TYPE, size_t POINT_DIMENSION, class PointContainer>
const typename QuadOctreeBase<DATA_TYPE, POINT_DIMENSION, PointContainer>::BoundingBox
QuadOctreeBase<DATA_TYPE, POINT_DIMENSION, PointContainer>::estimateBounds(const VecPointType& points,
                                                                           const PointType& offsetMinPoint,
                                                                           const PointType& offsetMaxPoint) const
{
    BoundingBox result;
    PointType boundMinPoint = offsetMinPoint;
    PointType boundMaxPoint = offsetMaxPoint;

    for (const PointType& curP : points) {
        for (size_t i = 0; i < POINT_DIMENSION; ++i) {
            if (boundMinPoint[i] > curP[i]) {
                boundMinPoint[i] = curP[i];
            }

            if (boundMaxPoint[i] < curP[i])
                boundMaxPoint[i] = curP[i];
        }
    }

    PointType deltaPoint = boundMaxPoint - boundMinPoint;
    PointType center = boundMinPoint + deltaPoint * 0.5;

    assert(center.size() == POINT_DIMENSION);
    std::copy(center.data(), center.data() + POINT_DIMENSION, result.coordinates.begin());

    DATA_TYPE maxDimension = std::numeric_limits<DATA_TYPE>::min();
    for (int i = 0; i < POINT_DIMENSION; ++i) {
        maxDimension = std::max(maxDimension, deltaPoint[i]);
    }

    result.radius = 0.5 * maxDimension;

    return result;
}

template <typename DATA_TYPE, size_t POINT_DIMENSION, class PointContainer>
std::stringstream QuadOctreeBase<DATA_TYPE, POINT_DIMENSION, PointContainer>::traversal() const
{
    return traversal(this->_root);
}

template <typename DATA_TYPE, size_t POINT_DIMENSION, class PointContainer>
std::stringstream QuadOctreeBase<DATA_TYPE, POINT_DIMENSION, PointContainer>::traversal(
    const QuadOctreeBaseNode* quadOctreeBaseNode) const
{
    std::stringstream ss;

    if (quadOctreeBaseNode->isLeaf) {
        uint32_t idx = quadOctreeBaseNode->startIdx;
        ss << "----------------------------\n";
        ss << this->_nodeName << ": "
           << "\nlevel: " << int(quadOctreeBaseNode->level) << "\nradius: " << quadOctreeBaseNode->radius << "\n";

        for (size_t i = 0; i < POINT_DIMENSION; ++i) {
            ss << i << " axis: " << quadOctreeBaseNode->coordinates[i] << "\n";
        }

        for (uint32_t i = 0; i < quadOctreeBaseNode->numPoints; ++i) {
            ss << "Point: \n";
            ss << this->_points[idx];
            ss << "\n";
            idx = this->_successors[idx];
        }
        ss << "----------------------------\n";
        return ss;
    }

    for (size_t i = 0; i < NUM_CHILD; ++i) {
        if (quadOctreeBaseNode->child[i] != 0) {
            ss << traversal(quadOctreeBaseNode->child[i]).str();
        }
    }

    return ss;
}

template <typename DATA_TYPE, size_t POINT_DIMENSION, class PointContainer>
void QuadOctreeBase<DATA_TYPE, POINT_DIMENSION, PointContainer>::drawOctree(
    const QuadOctreeBaseNode* quadOctreeBaseNode,
    const std::function<void(const PointCoordinates&, const DATA_TYPE)>& func) const
{
    if (quadOctreeBaseNode->isLeaf) {
        func(quadOctreeBaseNode->coordinates, quadOctreeBaseNode->radius);
        return;
    }

    for (size_t i = 0; i < NUM_CHILD; ++i) {
        if (quadOctreeBaseNode->child[i] != 0) {
            this->drawOctree(quadOctreeBaseNode->child[i], func);
        }
    }
}

template <typename DATA_TYPE, size_t POINT_DIMENSION, class PointContainer>
uint32_t QuadOctreeBase<DATA_TYPE, POINT_DIMENSION, PointContainer>::findNeighbor(const PointType& query,
                                                                                DATA_TYPE minDistance) const
{
    return this->findNeighbor(query, minDistance, this->_root);
}

template <typename DATA_TYPE, size_t POINT_DIMENSION, class PointContainer>
uint32_t QuadOctreeBase<DATA_TYPE, POINT_DIMENSION, PointContainer>::findNeighbor(
    const PointType& query, DATA_TYPE minDistance, const QuadOctreeBaseNode* quadOctreeBaseNode) const
{
    uint32_t resultIdx = -1;

    DATA_TYPE maxDistance = std::numeric_limits<DATA_TYPE>::max();
    std::vector<const QuadOctreeBaseNode*, Eigen::aligned_allocator<const QuadOctreeBaseNode*>> quadOctreeBaseNodePtrs;

    quadOctreeBaseNodePtrs.emplace_back(quadOctreeBaseNode);

    while (!quadOctreeBaseNodePtrs.empty()) {
        const QuadOctreeBaseNode* curQuadOctreeBaseNode = quadOctreeBaseNodePtrs.back();
        quadOctreeBaseNodePtrs.pop_back();

        if (!this->overlaps(query, maxDistance, curQuadOctreeBaseNode)) {
            continue;
        }

        if (curQuadOctreeBaseNode->isLeaf) {
            uint32_t idx = curQuadOctreeBaseNode->startIdx;

            for (uint32_t i = 0; i < curQuadOctreeBaseNode->numPoints; ++i) {
                const PointType& curPoint = this->_points[idx];
                const DATA_TYPE delta = (query - curPoint).norm();

                if (delta > minDistance && delta < maxDistance) {
                    resultIdx = idx;
                    maxDistance = delta;
                }

                idx = this->_successors[idx];
            }
        }

        for (size_t i = 0; i < NUM_CHILD; ++i) {
            if (curQuadOctreeBaseNode->child[i] != 0) {
                quadOctreeBaseNodePtrs.emplace_back(curQuadOctreeBaseNode->child[i]);
            }
        }
    }

    return resultIdx;
}

template <typename DATA_TYPE, size_t POINT_DIMENSION, class PointContainer>
std::vector<uint32_t> QuadOctreeBase<DATA_TYPE, POINT_DIMENSION, PointContainer>::knn(const PointType& query,
                                                                                uint32_t k,
                                                                                DATA_TYPE minDistance) const
{
    return this->knn(query, k, minDistance, this->_root);
}

template <typename DATA_TYPE, size_t POINT_DIMENSION, class PointContainer>
std::vector<uint32_t> QuadOctreeBase<DATA_TYPE, POINT_DIMENSION, PointContainer>::knn(
    const PointType& query, uint32_t k, DATA_TYPE minDistance, const QuadOctreeBaseNode* quadOctreeBaseNode) const
{
    std::vector<uint32_t> resultIndices;

    if (k >= this->_points.size()) {
        resultIndices.resize(this->_points.size());
        std::iota(resultIndices.begin(), resultIndices.end(), 0);
        std::sort(resultIndices.begin(), resultIndices.end(), [&query, this](const uint32_t idx1, const uint32_t idx2) {
            return (this->_points[idx1] - query).norm() < (this->_points[idx2] - query).norm();
        });

        return resultIndices;
    }

    DATA_TYPE maxDistance = std::numeric_limits<DATA_TYPE>::max();
    std::vector<const QuadOctreeBaseNode*, Eigen::aligned_allocator<const QuadOctreeBaseNode*>> quadOctreeBaseNodePtrs;

    //! DistanceIndex -> (distance from point P_i to query point, index of P_i in this->_points)
    using DistanceIndex = std::pair<DATA_TYPE, uint32_t>;

    std::priority_queue<DistanceIndex, std::vector<DistanceIndex>, std::less<DistanceIndex>> maxHeap;

    quadOctreeBaseNodePtrs.emplace_back(quadOctreeBaseNode);
    while (!quadOctreeBaseNodePtrs.empty()) {
        const QuadOctreeBaseNode* curQuadOctreeBaseNode = quadOctreeBaseNodePtrs.back();
        quadOctreeBaseNodePtrs.pop_back();

        if (!this->overlaps(query, maxDistance, curQuadOctreeBaseNode)) {
            continue;
        }

        if (curQuadOctreeBaseNode->isLeaf) {
            uint32_t idx = curQuadOctreeBaseNode->startIdx;

            for (uint32_t i = 0; i < curQuadOctreeBaseNode->numPoints; ++i) {
                const PointType& curPoint = this->_points[idx];
                const DATA_TYPE delta = (query - curPoint).norm();

                if (delta > minDistance) {
                    maxHeap.emplace(std::make_pair(delta, idx));
                }

                idx = this->_successors[idx];
            }
        }

        while (maxHeap.size() > k) {
            maxHeap.pop();
        }

        if (maxHeap.size() == k) {
            maxDistance = maxHeap.top().first;
        }

        for (size_t i = 0; i < NUM_CHILD; ++i) {
            if (curQuadOctreeBaseNode->child[i] != 0) {
                quadOctreeBaseNodePtrs.emplace_back(curQuadOctreeBaseNode->child[i]);
            }
        }
    }

    resultIndices.reserve(maxHeap.size());
    while (!maxHeap.empty()) {
        resultIndices.emplace_back(maxHeap.top().second);
        maxHeap.pop();
    }

    std::reverse(resultIndices.begin(), resultIndices.end());

    return resultIndices;
}

template <typename DATA_TYPE, size_t POINT_DIMENSION, class Container>
std::array<std::vector<DATA_TYPE>, POINT_DIMENSION>
QuadOctreeBase<DATA_TYPE, POINT_DIMENSION, Container>::extractDataEachAxis(const VecPointType& vecPointType) const
{
    std::array<std::vector<DATA_TYPE>, POINT_DIMENSION> result;

    for (size_t i = 0; i < POINT_DIMENSION; ++i) {
        std::vector<DATA_TYPE> curV;
        curV.reserve(vecPointType.size());
        std::transform(vecPointType.begin(), vecPointType.end(), std::back_inserter(curV),
                       [&i](const PointType& p) { return p[i]; });
        result[i] = curV;
    }

    return result;
}

template <typename DATA_TYPE, size_t POINT_DIMENSION, class PointContainer>
QuadOctreeBase<DATA_TYPE, POINT_DIMENSION, PointContainer>::QuadOctreeBaseNode::QuadOctreeBaseNode()
    : isLeaf(true), radius(0.0f), startIdx(0), endIdx(0), numPoints(0)
{
    this->coordinates.fill(0.0f);

    memset(&child, 0, NUM_CHILD * sizeof(QuadOctreeBaseNode*));
}

template <typename DATA_TYPE, size_t POINT_DIMENSION, class PointContainer>
QuadOctreeBase<DATA_TYPE, POINT_DIMENSION, PointContainer>::QuadOctreeBaseNode::~QuadOctreeBaseNode()
{
    for (uint32_t i = 0; i < NUM_CHILD; ++i) {
        if (child[i]) {
            delete child[i];
        }
    }
}

}  // namespace algo


/**
 * @file crypto.hpp
 * @author Bensuperpc (bensuperpc@gmail.com)
 * @brief
 * @version 1.0.0
 * @date 2021-04-01
 *
 * MIT License
 *
 */

/*
** Source: https://stackoverflow.com/questions/178265/what-is-the-most-hard-to-understand-piece-of-c-code-you-know https://cs.uwaterloo.ca/~m32rober/rsqrt.pdf
https://github.com/bavlayan/Encrypt-Decrypt-with-OpenSSL---RSA
*/

#ifndef CRYPTO_HPP_
#define CRYPTO_HPP_

#define BUFFSIZE 16384

#include <cstring>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <string>

// For MD5 and SHA
#include <boost/crc.hpp> //CRC32
#include <openssl/md5.h>
#include <openssl/sha.h>
#include <string_view>

//#include "openssl_rsa.h"

#define BUFFSIZE 16384

namespace my
{
namespace crypto
{
/**
 * @brief Get the md5hash object
 *
 * @param str
 * @return std::string
 */
std::string get_md5hash(const std::string &str);
// void get_md5hash_from_string(const unsigned char *, size_t &);
/**
 * @brief Get the md5hash from string object
 *
 * @param str
 * @return std::string
 */
std::string get_md5hash_from_string(const std::string &str);

/**
 * @brief Get the sha1hash object
 *
 * @param str
 * @return std::string
 */
std::string get_sha1hash(const std::string &str);
// void get_sha1hash_from_string(const unsigned char *, size_t &);

/**
 * @brief Get the sha1hash from string object
 *
 * @param str
 * @return std::string
 */
std::string get_sha1hash_from_string(const std::string &str);

/**
 * @brief Get the sha224hash object
 *
 * @param str
 * @return std::string
 */
std::string get_sha224hash(const std::string &str);
// void get_sha224hash_from_string(const unsigned char *, size_t &);

/**
 * @brief Get the sha224hash from string object
 *
 * @param str
 * @return std::string
 */
std::string get_sha224hash_from_string(const std::string &str);

/**
 * @brief Get the sha256hash object
 *
 * @param str
 * @return std::string
 */
std::string get_sha256hash(const std::string &str);
// void get_sha256hash_from_string(const unsigned char *, size_t &);

/**
 * @brief Get the sha256hash from string object
 *
 * @param str
 * @return std::string
 */
std::string get_sha256hash_from_string(const std::string &str);

/**
 * @brief Get the sha384hash object
 *
 * @param str
 * @return std::string
 */
std::string get_sha384hash(const std::string &str);
// void get_sha384hash_from_string(const unsigned char *, size_t &);

/**
 * @brief Get the sha384hash from string object
 *
 * @param str
 * @return std::string
 */
std::string get_sha384hash_from_string(const std::string &str);

/**
 * @brief Get the sha512hash object
 *
 * @param str
 * @return std::string
 */
std::string get_sha512hash(const std::string &str);
// void get_sha512hash_from_string(const unsigned char *, size_t &);
/**
 * @brief Get the sha512hash from string object
 *
 * @param str
 * @return std::string
 */
std::string get_sha512hash_from_string(const std::string &str);
} // namespace crypto
} // namespace my
// https://www.quora.com/How-can-I-get-the-MD5-or-SHA-hash-of-a-file-in-C

#endif


#include "main.h"

#include <Eigen/CXX11/Tensor>

using Eigen::Tensor;
using Eigen::RowMajor;

static void test_comparison_sugar() {
  // we already trust comparisons between tensors, we're simply checking that
  // the sugared versions are doing the same thing
  Tensor<int, 3> t(6, 7, 5);

  t.setRandom();
  // make sure we have at least one value == 0
  t(0,0,0) = 0;

  Tensor<bool,0> b;

#define TEST_TENSOR_EQUAL(e1, e2) \
  b = ((e1) == (e2)).all();       \
  VERIFY(b())

#define TEST_OP(op) TEST_TENSOR_EQUAL(t op 0, t op t.constant(0))

  TEST_OP(==);
  TEST_OP(!=);
  TEST_OP(<=);
  TEST_OP(>=);
  TEST_OP(<);
  TEST_OP(>);
#undef TEST_OP
#undef TEST_TENSOR_EQUAL
}

void test_cxx11_tensor_sugar()
{
  CALL_SUBTEST(test_comparison_sugar());
}


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/euler/include/functions/erfc.hpp>

#include <nt2/sdk/functor/meta/call.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/simd/sdk/simd/io.hpp>

#include <nt2/include/functions/splat.hpp>
#include <nt2/include/functions/all.hpp>
#include <nt2/include/functions/is_less.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/mzero.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/ten.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/two.hpp>

NT2_TEST_CASE_TPL ( erfc_real,  NT2_SIMD_REAL_TYPES)
{
  using nt2::erfc;
  using nt2::tag::erfc_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename nt2::meta::call<erfc_(vT)>::type r_t;
  typedef vT wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(erfc(nt2::Inf<vT>()), nt2::Zero<r_t>(), 200);
  NT2_TEST_ULP_EQUAL(erfc(nt2::Nan<vT>()), nt2::Nan<r_t>(), 200);
#endif
  NT2_TEST_ULP_EQUAL(erfc(nt2::Mzero<vT>()), nt2::One<r_t>(), 200);
  NT2_TEST_ULP_EQUAL(erfc(nt2::One<vT>()), nt2::splat<vT>(0.157299207050285), 200);
  NT2_TEST_ULP_EQUAL(erfc(nt2::Half<vT>()), nt2::splat<vT>(0.479500122186953), 200);
  NT2_TEST_ULP_EQUAL(erfc(nt2::Two<vT>()), nt2::splat<vT>(0.004677734981047), 200);
  NT2_TEST_ULP_EQUAL(erfc(nt2::Zero<vT>()), nt2::One<r_t>(), 200);
  NT2_TEST(nt2::all(nt2::is_less(erfc((nt2::Ten<vT>())- nt2::splat<vT>(2.088487583762545e-45)), nt2::Eps<T>())));
  NT2_TEST(nt2::all(nt2::is_less(erfc(nt2::splat<vT>(15))- nt2::splat<vT>(7.212994172451206e-100), nt2::Eps<T>())));
  NT2_TEST_ULP_EQUAL(erfc(nt2::splat<vT>(-6)), nt2::Two<r_t>(), 4);
}


//igl
#include <igl/opengl/glfw/Viewer.h>
#include <igl/readMESH.h>
#include <igl/readOBJ.h>
#include <igl/writeOBJ.h>
#include <igl/per_vertex_normals.h>
#include <igl/per_corner_normals.h>
#include <igl/boundary_facets.h>
#include <igl/lbs_matrix.h>
#include <igl/deform_skeleton.h>
#include <igl/mat_max.h>
#include <igl/mat_min.h>
#include <igl/PI.h>
#include <igl/volume.h>
#include <igl/cat.h>
// #include <igl/dirname.h>

// #include <igl/matlab/matlabinterface.h>

//internal
#include <json.hpp>
#include <read_data_from_json.h>

#include <lumped_mass_matrix.h>
#include <lbs_matrix.h>
#include <create_mask_matrix.h>
#include <line_search.h>

//Bartels
#include <linear_tetmesh_dphi_dX.h>
#include <linear_tetmesh_arap_dq.h>
#include <linear_tetmesh_arap_dq2.h>
#include <linear_tetmesh_arap_q.h>
#include <linear_tetmesh_neohookean_dq.h>
#include <linear_tetmesh_neohookean_dq2.h>
#include <linear_tetmesh_neohookean_q.h>
#include <linear_tetmesh_stvk_dq.h>
#include <linear_tetmesh_stvk_dq2.h>
#include <linear_tetmesh_stvk_q.h>
#include <linear_tetmesh_corotational_dq.h>
#include <linear_tetmesh_corotational_dq2.h>
#include <linear_tetmesh_corotational_q.h>
#include <Eigen/Sparse>
#include <Eigen/Core>

#include <iostream>
#include <filesystem>

std::string folder_path = "../examples";

//#include "include/read_data_from_json.h"
const Eigen::Vector3d red(255./255.,0./255.,0./255.);

int frame = 0;

void emu_to_lame(const double& E, const double& p, double& lambda, double& mu)
{
  lambda = (E*p)/((1.0+p)*(1.0-2.0*p));
  mu = E/(2.0*(1.0+p));
}

bool read_json_data(const std::string& filename,
Eigen::MatrixXd& V, Eigen::MatrixXi& T, Eigen::MatrixXi& F,
Eigen::MatrixXd& C, Eigen::VectorXi& PI, Eigen::MatrixXi& BE,
Eigen::MatrixXd& W, std::vector<Eigen::MatrixXd>& T_list,
double& dt, int& k, double& YM, double& pr, double& scale, std::string& physic_model)
{

#if defined(WIN32) || defined(_WIN32)
	#define PATH_SEPARATOR std::string("\\")
	#else
#define PATH_SEPARATOR std::string("/")
#endif

  using json = nlohmann::json;

	std::ifstream infile(filename);
	if (!infile)
		return false;
	json j;
	infile >> j;

	const std::string dir = igl::dirname(filename) + PATH_SEPARATOR;
  std::cout<<"dir: "<<dir<<std::endl;
  // std::string model_name;
	if(j.count("model"))
		physic_model = j["model"];
	else{
		std::cerr<<"no model specified"<<std::endl;
		return false;
	}
  	if(j.count("mesh_file"))
	{
		std::string mesh_filename = j["mesh_file"];
    std::cout<<"mesh_filename: "<<mesh_filename<<std::endl;
		// Eigen::MatrixXi TF;
		igl::readMESH(dir+mesh_filename, V, T, F);
	}
  	if(j.count("handle_file"))
	{
		std::string handle_filename = j["handle_file"];
		Eigen::MatrixXi E, CE, PE;
		igl::readTGF(dir+handle_filename, C, E, PI, BE, CE, PE);
	}
  	if(j.count("weight_file"))
	{
		std::string weights_filename = j["weight_file"];
		igl::readDMAT(dir+weights_filename, W);
	}

  if(j.count("dt"))
		dt = j["dt"];
	if(j.count("k"))
		k = j["k"];
	if(j.count("YM"))
		YM = j["YM"];
	if(j.count("pr"))
		pr = j["pr"];
	if(j.count("scale"))
		scale = j["scale"];

  Eigen::RowVectorXd Vcenter;
  double Vbound;
  Eigen::VectorXd Vmax, Vmin;
  Eigen::VectorXi I;
  igl::mat_max(V, 1, Vmax, I);
  igl::mat_min(V, 1, Vmin, I);
  Vcenter = (Vmax+Vmin)/2.0;
  //Vcenter = V.colwise().minCoeff();
  V = V.rowwise()-Vcenter;
  Vbound = V.maxCoeff();
  V = V/Vbound;

  C = C.rowwise()-Vcenter;
  C = C/Vbound;

  if(j.count("anim_file"))
	{
		std::string anim_filename = j["anim_file"];

    //assumes the skeleton is either all point handles, or all bone handles. need to fix
    std::cout<<"BE rows: "<<BE.rows()<<std::endl;
    if(BE.rows()>0){
      Eigen::VectorXi P;
      igl::directed_edge_parents(BE, P);
      read_bone_anim(dir+anim_filename,C,BE,P,Vcenter.transpose(),Vbound,T_list);
    }
    else{
      read_pnt_anim(dir+anim_filename, C, Vcenter.transpose(), Vbound, T_list);
    }
	}

}

Eigen::VectorXd vectorize(const Eigen::MatrixXd& M)
{
  Eigen::MatrixXd MT = M.transpose();
  return Eigen::Map<Eigen::VectorXd>(MT.data(),M.rows()*M.cols());
}

Eigen::MatrixXd matrixize(Eigen::VectorXd V)
{
  Eigen::MatrixXd MT = Eigen::Map<Eigen::MatrixXd>(V.data(), 3, V.size()/3);
  return MT.transpose();
}

int main(int argc, char *argv[])
{
  Eigen::MatrixXd V, C, W, VM;
  Eigen::MatrixXi T,F, E, BE;
  Eigen::VectorXi PI;

  std::vector<Eigen::MatrixXd> T_list;
  double dt, YM, pr, scale;
  int k;

  std::string model = argc>1?std::string(argv[1]):"sphere";
  std::string json_path = folder_path+"/"+model+"/"+model+".json";

  Eigen::MatrixXd SV,TC; Eigen::MatrixXi SF,FTC,FN;
  {
    std::string obj_path = folder_path+"/"+model+"/"+model+".obj";
    Eigen::MatrixXd N;
    igl::readOBJ(obj_path,SV,TC,N,SF,FTC,FN);
    std::cout<<"N row: "<<N.rows()<<", col: "<<N.cols()<<std::endl;
  }
  std::string physic_model;
  std::cout<<"json_path: "<<json_path<<std::endl;
  read_json_data(json_path,V,T,F,C,PI,BE,W,T_list,dt,k,YM,pr,scale,physic_model);

  std::cout<<"physic_model: "<<physic_model<<std::endl;

  std::string str = argc>2?std::string(argv[2]):"";
  bool export_objs = false;
  if(str=="export")
    export_objs=true;

  double lambda, mu;
  emu_to_lame(YM,pr,lambda,mu);

  Eigen::MatrixXd params(T.rows(),2);
  params.col(0) = 0.5*lambda*Eigen::VectorXd::Ones(T.rows());
  params.col(1) = mu*Eigen::VectorXd::Ones(T.rows());

  igl::boundary_facets(T, F);
  F = F.rowwise().reverse().eval();

	igl::lbs_matrix(V,W,VM);

  Eigen::VectorXd vol;

  igl::volume(V,T,vol);

  Eigen::MatrixXd dX;
  sim::linear_tetmesh_dphi_dX(dX,V,T);

  Eigen::SparseMatrix<double> M;

  lumped_mass_matrix(V,T,M);
  M = M*1000;

  Eigen::SparseMatrix<double> A;
  lbs_matrix_column(V,W,A);

  Eigen::SparseMatrix<double> phi;
  create_poisson_mask_matrix(V,T,phi);

  Eigen::SparseMatrix<double> Aeq = A.transpose()*M*phi;
  Eigen::VectorXd Beq = Eigen::VectorXd::Zero(A.cols());

  Eigen::MatrixXd T_new = T_list[0];

  Eigen::MatrixXd Vr = VM*T_new;

  std::cout<<"V row: "<<V.rows()<<std::endl;
  std::cout<<"Vr row: "<<Vr.rows()<<std::endl;

  std::cout<<"T_new"<<std::endl;
  std::cout<<T_new<<std::endl;

  std::vector<Eigen::MatrixXd> Vn_list(T_list.size());
  int max_iter = 20;
  for(int ai=0; ai<T_list.size(); ai++){
    std::cout<<"ai: "<<ai<<std::endl;

    Eigen::MatrixXd TM = T_list[ai];
    Vr = VM*TM;
    Vn_list[ai]=Vr;

  }
  if(export_objs)
  {
    for(int ai=0; ai<Vn_list.size(); ai++)
    {
      Eigen::MatrixXd Vn = Vn_list[ai];
      std::string numstr = std::to_string(ai);
      int strlen = numstr.length();

      std::string seqstr;
      for(int k=0; k<(4-strlen); k++){
          seqstr.append("0");
      }
      seqstr.append(numstr);

      std::string save_path = folder_path+"/"+model+"/output/"+model+seqstr+".obj";

      Eigen::MatrixXd SVn = Vn.block(0,0,SV.rows(),3);
      Eigen::MatrixXd SN;
      //igl::per_vertex_normals(SVn, SF,SN);
      igl::per_corner_normals(SVn, SF, 20, SN);
      igl::writeOBJ(save_path,SVn,SF,Eigen::MatrixXd(),Eigen::MatrixXi(),TC,FTC);
    }
  }

	igl::opengl::glfw::Viewer viewer;
	viewer.callback_pre_draw = [&](igl::opengl::glfw::Viewer &) -> bool
	{
		if(viewer.core().is_animating)
		{
      Eigen::MatrixXd Vn = Vn_list[frame];
      viewer.data().set_vertices(Vn);
      viewer.data().compute_normals();
			frame++;
      if(frame == Vn_list.size()){
        frame = 0;
        //viewer.core().is_animating = false;
      }
		}
		return false;
	};
  viewer.callback_key_down = [&](igl::opengl::glfw::Viewer &, unsigned int key, int mod)
  {
    if(key==' '){
      viewer.core().is_animating = !viewer.core().is_animating;
    }
    return false;
  };
  viewer.data().set_mesh(V, F);
  viewer.core().is_animating = true;
  viewer.core().animation_max_fps = 24.;
  viewer.data().show_lines = true;
  viewer.data().show_overlay_depth = false;
  viewer.launch();
}