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

#ifndef BOOST_MATH_SF_CBRT_HPP
#define BOOST_MATH_SF_CBRT_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/tools/rational.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/mpl/divides.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_convertible.hpp>

namespace boost{ namespace math{

namespace detail
{

struct big_int_type
{
   operator boost::uintmax_t()const;
};

template <class T>
struct largest_cbrt_int_type
{
   typedef typename mpl::if_<
      boost::is_convertible<big_int_type, T>,
      boost::uintmax_t,
      unsigned int
   >::type type;
};

template <class T, class Policy>
T cbrt_imp(T z, const Policy& pol)
{
   BOOST_MATH_STD_USING
   //
   // cbrt approximation for z in the range [0.5,1]
   // It's hard to say what number of terms gives the optimum
   // trade off between precision and performance, this seems
   // to be about the best for double precision.
   //
   // Maximum Deviation Found:                     1.231e-006
   // Expected Error Term:                         -1.231e-006
   // Maximum Relative Change in Control Points:   5.982e-004
   //
   static const T P[] = {
      static_cast<T>(0.37568269008611818),
      static_cast<T>(1.3304968705558024),
      static_cast<T>(-1.4897101632445036),
      static_cast<T>(1.2875573098219835),
      static_cast<T>(-0.6398703759826468),
      static_cast<T>(0.13584489959258635),
   };
   static const T correction[] = {
      static_cast<T>(0.62996052494743658238360530363911),  // 2^-2/3
      static_cast<T>(0.79370052598409973737585281963615),  // 2^-1/3
      static_cast<T>(1),
      static_cast<T>(1.2599210498948731647672106072782),   // 2^1/3
      static_cast<T>(1.5874010519681994747517056392723),   // 2^2/3
   };

   if(!boost::math::isfinite(z))
   {
      return policies::raise_domain_error("boost::math::cbrt<%1%>(%1%)", "Argument to function must be finite but got %1%.", z, pol);
   }

   int i_exp, sign(1);
   if(z < 0)
   {
      z = -z;
      sign = -sign;
   }
   if(z == 0)
      return 0;

   T guess = frexp(z, &i_exp);
   int original_i_exp = i_exp; // save for later
   guess = tools::evaluate_polynomial(P, guess);
   int i_exp3 = i_exp / 3;

   typedef typename largest_cbrt_int_type<T>::type shift_type;

   if(abs(i_exp3) < std::numeric_limits<shift_type>::digits)
   {
      if(i_exp3 > 0)
         guess *= shift_type(1u) << i_exp3;
      else
         guess /= shift_type(1u) << -i_exp3;
   }
   else
   {
      guess = ldexp(guess, i_exp3);
   }
   i_exp %= 3;
   guess *= correction[i_exp + 2];
   //
   // Now inline Halley iteration.
   // We do this here rather than calling tools::halley_iterate since we can
   // simplify the expressions algebraically, and don't need most of the error
   // checking of the boilerplate version as we know in advance that the function
   // is well behaved...
   //
   typedef typename policies::precision<T, Policy>::type prec;
   typedef typename mpl::divides<prec, mpl::int_<3> >::type prec3;
   typedef typename mpl::plus<prec3, mpl::int_<3> >::type new_prec;
   typedef typename policies::normalise<Policy, policies::digits2<new_prec::value> >::type new_policy;
   //
   // Epsilon calculation uses compile time arithmetic when it's available for type T,
   // otherwise uses ldexp to calculate at runtime:
   //
   T eps = (new_prec::value > 3) ? policies::get_epsilon<T, new_policy>() : ldexp(T(1), -2 - tools::digits<T>() / 3);
   T diff;

   if(original_i_exp < std::numeric_limits<T>::max_exponent - 3)
   {
      //
      // Safe from overflow, use the fast method:
      //
      do
      {
         T g3 = guess * guess * guess;
         diff = (g3 + z + z) / (g3 + g3 + z);
         guess *= diff;
      }
      while(fabs(1 - diff) > eps);
   }
   else
   {
      //
      // Either we're ready to overflow, or we can't tell because numeric_limits isn't
      // available for type T:
      //
      do
      {
         T g2 = guess * guess;
         diff = (g2 - z / guess) / (2 * guess + z / g2);
         guess -= diff;
      }
      while((guess * eps) < fabs(diff));
   }

   return sign * guess;
}

} // namespace detail

template <class T, class Policy>
inline typename tools::promote_args<T>::type cbrt(T z, const Policy& pol)
{
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return static_cast<result_type>(detail::cbrt_imp(value_type(z), pol));
}

template <class T>
inline typename tools::promote_args<T>::type cbrt(T z)
{
   return cbrt(z, policies::policy<>());
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_SF_CBRT_HPP


#include "shortest_path.h"

#include <iostream>
#include <memory>
#include <vector>
#include <queue>
#include <set>

#include <Eigen/Dense>

#include <geometry/patch.h>

namespace shortest_path {
	std::map<Eigen::DenseIndex, std::shared_ptr<DjikstraVertexNode>> djikstras_algorithm(std::shared_ptr<Patch> patch, Eigen::DenseIndex source, std::set<Eigen::DenseIndex> exclude) {
		std::priority_queue<std::shared_ptr<DjikstraVertexNode>, std::vector<std::shared_ptr<DjikstraVertexNode>>, DjikstraDist> q;
		std::map<Eigen::DenseIndex, std::shared_ptr<DjikstraVertexNode>> nodes;

		const Eigen::MatrixXd& V = patch->origin_mesh()->vertices();

		if (exclude.count(source) > 0) {
			// Why would you do that??
			return nodes;
		}

		const Eigen::SparseMatrix<int>& adj = patch->origin_mesh()->adjacency_matrix();
		auto source_node = std::make_shared<DjikstraVertexNode>(nullptr, 0.0, source);

		q.push(source_node);
		nodes.insert(std::pair<Eigen::DenseIndex, std::shared_ptr<DjikstraVertexNode>>(source, source_node));

		while (!q.empty()) {
			std::vector<std::shared_ptr<DjikstraVertexNode>> stored;

			auto u = q.top();
			q.pop();

			while (!q.empty()) {
				stored.push_back(q.top());
				q.pop();
			}

			for (Eigen::SparseMatrix<int>::InnerIterator it(adj, static_cast<int>(u->_vid)); it; ++it) {
				Eigen::DenseIndex neighbor = it.row();   // neighbor vid

				if (patch->vids().count(neighbor) == 0) {
					continue;
				}

				double dist = (V.row(neighbor).block<1, 3>(0, 0) - V.row(u->_vid).block<1, 3>(0, 0)).norm() + u->_dist;
				auto node = nodes.find(neighbor);

				if (node == nodes.cend()) {
					auto neighbor_node = std::make_shared<DjikstraVertexNode>(u, dist, neighbor);

					if (exclude.count(neighbor) <= 0) {
						q.push(neighbor_node);
						nodes.insert(std::pair<Eigen::DenseIndex, std::shared_ptr<DjikstraVertexNode>>(neighbor, neighbor_node));
					}
				}
				else {
					if (dist < node->second->_dist) {
						node->second->_dist = dist;
						node->second->_prev = u;
					}
				}
			}

			// Changing values in a priority_queue invalidates the queue, so update it
			std::priority_queue<std::shared_ptr<DjikstraVertexNode>, std::vector<std::shared_ptr<DjikstraVertexNode>>, DjikstraDist> new_q;
			for (unsigned int i = 0; i < stored.size(); ++i) {
				q.push(stored[i]);
			}
		}

		// q is now empty, and nodes contains all _vid vertices and their shortest paths from source
		return nodes;
	}

	// Nothing fancy, just a straight implementation of the most basic Floyd-Warshall algorithm for shortest path distance values
	Eigen::MatrixXd floyd_warshall(const Eigen::SparseMatrix<double> edge_weights) {
		if (edge_weights.rows() != edge_weights.cols()) {
			throw std::logic_error("Edge weights is not symmetric!");
		}

		Eigen::DenseIndex V = edge_weights.rows();

		Eigen::MatrixXd dist = Eigen::MatrixXd::Constant(V, V, std::numeric_limits<double>::infinity());

		for (Eigen::DenseIndex i = 0; i < V; ++i) {
			dist(i, i) = 0;
		}

		for (int k = 0; k < edge_weights.outerSize(); ++k) {
			for (Eigen::SparseMatrix<double>::InnerIterator it(edge_weights, k); it; ++it) {
				dist(it.row(), it.col()) = it.value();
			}
		}

		for (Eigen::DenseIndex k = 0; k < V; ++k) {
			for (Eigen::DenseIndex i = 0; i < V; ++i) {
				for (Eigen::DenseIndex j = 0; j < V; ++j) {
					if (dist(i, j) > dist(i, k) + dist(k, j)) {
						dist(i, j) = dist(i, k) + dist(k, j);
					}
				}
			}
		}

		return dist;
	}

	struct EdgeComparator {
		bool operator()(const std::pair<Eigen::DenseIndex, Eigen::DenseIndex>& a, const std::pair<Eigen::DenseIndex, Eigen::DenseIndex>& b) const {
			// order of the face identifiers of the edge doesn't matter, only that the combination is the same
			bool res = (a.first == b.first && a.second == b.second) || (a.first == b.second && a.second == b.first);

			if (res) {
				// They're equal
				return false;
			}

			// Otherwise, order by vid (arbitrary)
			if (a.first == b.first) {
				return a.second < b.second;
			}

			return a.first < b.first;
		}
	};

	// This will assert if the two faces are not neighbors
	Eigen::VectorXd shared_edge_midpoint(Eigen::DenseIndex fid1, Eigen::DenseIndex fid2, std::shared_ptr<Mesh> mesh) {
		const Eigen::MatrixXd& V = mesh->vertices();
		const Eigen::MatrixXi& F = mesh->faces();

		std::vector<Eigen::DenseIndex> endpoints;
		for (Eigen::DenseIndex j = 0; j < F.cols(); ++j) {
			Eigen::DenseIndex adj_vid = F(fid2, j);

			if ((F.row(fid1).array() == adj_vid).any()) {
				endpoints.push_back(adj_vid);
			}
		}

		assert(endpoints.size() == 2);

		Eigen::VectorXd midpoint = (V.row(endpoints[0]) + V.row(endpoints[1])).transpose() / 2.0;

		return midpoint;
	}

	std::vector<Eigen::DenseIndex> face_to_face(Eigen::DenseIndex source, Eigen::DenseIndex sink, std::shared_ptr<Mesh> mesh) {
		std::vector<Eigen::DenseIndex> path;

		if (mesh == nullptr) {
			return path;
		}

		std::priority_queue<std::shared_ptr<DjikstraFaceNode>, std::vector<std::shared_ptr<DjikstraFaceNode>>, DjikstraDist> q;
		std::map<std::pair<Eigen::DenseIndex, Eigen::DenseIndex>, std::shared_ptr<DjikstraFaceNode>, EdgeComparator> nodes;

		const Eigen::MatrixXd& V = mesh->vertices();
		const Eigen::MatrixXi& F = mesh->faces();
		const Eigen::MatrixXi& tri_adj = mesh->tri_adjacency_matrix();

		{ // Bootstrap source (the source is actually made up of a number of sources equal to the number of edges of the source face
			for (Eigen::DenseIndex i = 0; i < tri_adj.cols(); ++i) {
				Eigen::DenseIndex adj_fid = tri_adj(source, i);

				if (adj_fid < 0 || adj_fid >= F.rows()) {
					continue;
				}

				Eigen::VectorXd midpoint = shared_edge_midpoint(source, adj_fid, mesh);

				// Note: order here is *important* -- (from_fid, to_fid)
				auto edge = std::make_pair(source, adj_fid);

				auto source_node = std::make_shared<DjikstraFaceNode>(nullptr, 0.0, edge, midpoint);

				q.push(source_node);

				nodes.insert(std::make_pair(edge, source_node));
				nodes.insert(std::make_pair(std::make_pair(adj_fid, source), source_node));

				auto check = nodes.find(edge);

				if (check == nodes.end()) {
					throw std::logic_error("This is dead wrong!");
				}
			}
		}

		// Search
		std::shared_ptr<DjikstraFaceNode> sink_node = nullptr;

		while (!q.empty()) {
			std::vector<std::shared_ptr<DjikstraFaceNode>> stored;

			auto u = q.top();
			q.pop();

			while (!q.empty()) {
				stored.push_back(q.top());
				q.pop();
			}

			Eigen::DenseIndex cur_face = u->_edge.second;
			Eigen::DenseIndex prev_face = u->_edge.first;

			if (cur_face == sink) {
				sink_node = u;
				break;
			}

			for (Eigen::DenseIndex i = 0; i < tri_adj.cols(); ++i) {
				Eigen::DenseIndex neighbor = tri_adj(cur_face, i);   // neighbor fid

				if (neighbor == prev_face || neighbor < 0 || neighbor >= F.rows()) {
					continue;
				}

				Eigen::VectorXd midpoint = shared_edge_midpoint(cur_face, neighbor, mesh);

				auto neighbor_edge = std::make_pair(cur_face, neighbor);

				double dist = u->_dist + (u->_midpoint - midpoint).norm();

				auto node = nodes.find(neighbor_edge);

				if (node == nodes.cend()) {
					auto neighbor_node = std::make_shared<DjikstraFaceNode>(u, dist, neighbor_edge, midpoint);

					q.push(neighbor_node);

					nodes.insert(std::make_pair(neighbor_edge, neighbor_node));
				} else {
					if (dist < node->second->_dist) {
						node->second->_dist = dist;

						if (node->second->_prev == nullptr || node->second->_dist <= 0.0) {
							throw std::logic_error("How are we replacing the prev of a root note??");
						}

						node->second->_prev = u;
					}
				}
			}

			// Changing values in a priority_queue invalidates the queue, so update it
			//std::priority_queue<std::shared_ptr<DjikstraFaceNode>, std::vector<std::shared_ptr<DjikstraFaceNode>>, DjikstraDist> new_q;
			for (unsigned int i = 0; i < stored.size(); ++i) {
				q.push(stored[i]);
			}
		}

		// Populate shortest path from source to sink
		if (sink_node == nullptr) {
			return path;
		}

		while (true) {
			path.push_back(sink_node->_edge.second);

			if (sink_node->_prev == nullptr) {
				break;
			}

			sink_node = sink_node->_prev;
		}

		assert(sink_node->_edge.first == source);

		path.push_back(source);

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

		return path;
	}
}


#ifndef TRAJCOMP_SYMMAT_INC
#define TRAJCOMP_SYMMAT_INC

#include<vector>
/*a nice trick to make rvalues to lvalues*/

template <typename T>
T& as_lvalue(T&& x)
{
    return x;
}

/*Symmetric Square Matrix storing only Upper Triangular, diagonal is fixed zero
 *
 * Especially useful for distance matrices. *
 * */

template<class tvalue>
class UTSquareMatrix
{
	public:
	size_t _size;
	std::vector<tvalue> data;

	size_t size(void)
	{

		return _size;
	}

	void resize(size_t m)
	{
		_size = m;
		size_t size = m*(m-1)/2;
		data.resize(size);
	}

	struct tsymmetricgetter{
		tvalue temp; // for making a zero reference-accessible
		std::vector<tvalue> &base;
		size_t i;
		tsymmetricgetter (std::vector<tvalue> &_base, size_t _i): base(_base),i(_i)
		{};
		tvalue &operator[] (size_t j)
		{
			if (i==j)
			{
				temp =0;
			   return  temp;
		    }
		    if (i < j )
		      swap(i,j);

			return as_lvalue (base[((i*(i-1))/2) + (j)]);
		}
	};

	tsymmetricgetter operator[](size_t i)
	{
		return tsymmetricgetter(data, i);
	}

	template<class Archive>
    void serialize(Archive & ar, const unsigned int version)
    {
		ar & _size;
		ar & data;
    }

};

/* Test Main <=> Documentation ;-)
 *
 * // #include<iostream>
#include <fstream>
//include headers that implement a archive in simple text format
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/serialization/vector.hpp>
using namespace std;
int main(void)
{
	UTSquareMatrix<int> m;
	const int s = 6;
	m.resize(s);
	// Fill in matrix

	for (size_t i=0; i < s; i++)
	{
		for (size_t j=0; j < s; j++)
		{
			m[i][j] = i+j;

    	}
	}
	// Dump before save
	for (size_t i=0; i < s; i++)
	{
		for (size_t j=0; j < s; j++)
		{
			cout << m[i][j]<< "\t";
		}
		cout << endl;
	}
	// Serialize
	 std::ofstream ofs("serial.dat");

    // save data to archive
    {
        boost::archive::text_oarchive oa(ofs);
        // write class instance to archive
        oa << m;
    	// archive and stream closed when destructors are called
    }
	for (size_t i=0; i < s; i++)
	{
		for (size_t j=0; j < s; j++)
		{
			m[i][j] = 4;

    	}
	}

for (size_t i=0; i < s; i++)
	{
		for (size_t j=0; j < s; j++)
		{
			cout << m[i][j]<< "\t";
		}
		cout << endl;
	}

    UTSquareMatrix<int> n;

    {
        // create and open an archive for input
        std::ifstream ifs("serial.dat");
        boost::archive::text_iarchive ia(ifs);
        // read class state from archive
        ia >> n;
        // archive and stream closed when destructors are called
    }
    cout << "--" << endl;
	for (size_t i=0; i < s; i++)
	{
		for (size_t j=0; j < s; j++)
		{
			cout << n[i][j]<< "\t";
		}
		cout << endl;
	}

}

*/
#endif


// Copyright Abel Sinkovics (abel@sinkovics.hu) 2013.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <mpllibs/metamonad/apply.hpp>
#include <mpllibs/metamonad/tmp_value.hpp>
#include <mpllibs/metamonad/returns.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/plus.hpp>
#include <boost/mpl/equal_to.hpp>

#include "common.hpp"

using mpllibs::metamonad::tmp_value;

using boost::mpl::plus;

namespace
{
  struct add : tmp_value<add>
  {
    template <class A, class B>
    struct apply : plus<A, B> {};
  };

  struct id : tmp_value<id>
  {
    template <class A>
    struct apply : A {};
  };
}

BOOST_AUTO_TEST_CASE(test_apply)
{
  using mpllibs::metamonad::apply;
  using mpllibs::metamonad::returns;

  using boost::mpl::equal_to;

  BOOST_MPL_ASSERT((equal_to<int13, apply<add, int11, int2> >));

  BOOST_MPL_ASSERT((
    equal_to<int13, apply<returns<add>, returns<int11>, returns<int2> > >
  ));
}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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)
//=======================================================================

// Revision History:
//   17 March 2006: Fixed a bug: when updating the degree a vertex
//                  could be moved to a wrong bucket. (Roman Dementiev)
//

#ifndef BOOST_SMALLEST_LAST_VERTEX_ORDERING_HPP
#define BOOST_SMALLEST_LAST_VERTEX_ORDERING_HPP
/*
   The smallest-last ordering is defined for the loopless graph G with
   vertices a(j), j = 1,2,...,n where a(j) is the j-th column of A and
   with edge (a(i),a(j)) if and only if columns i and j have a
   non-zero in the same row position.  The smallest-last ordering is
   determined recursively by letting list(k), k = n,...,1 be a column
   with least degree in the subgraph spanned by the un-ordered
   columns.
 */
#include <vector>
#include <algorithm>
#include <boost/config.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>
#include <boost/pending/bucket_sorter.hpp>

namespace boost {

  template <class VertexListGraph, class Order, class Degree, class Marker>
  void
  smallest_last_vertex_ordering(const VertexListGraph& G, Order order,
                                Degree degree, Marker marker) {
    typedef typename boost::graph_traits<VertexListGraph> GraphTraits;
    typedef typename GraphTraits::vertex_descriptor Vertex;
    //typedef typename GraphTraits::size_type size_type;
    typedef std::size_t size_type;

    const size_type num = num_vertices(G);

    typedef typename boost::property_map<VertexListGraph, vertex_index_t>::type ID;
    typedef bucket_sorter<size_type, Vertex, Degree, ID> BucketSorter;

    BucketSorter degree_bucket_sorter(num, num, degree,
                                      get(vertex_index,G));

    smallest_last_vertex_ordering(G, order, degree, marker, degree_bucket_sorter);
  }

  template <class VertexListGraph, class Order, class Degree,
            class Marker, class BucketSorter>
  void
  smallest_last_vertex_ordering(const VertexListGraph& G, Order order,
                                Degree degree, Marker marker,
                                BucketSorter& degree_buckets) {
    typedef typename boost::graph_traits<VertexListGraph> GraphTraits;
    typedef typename GraphTraits::vertex_descriptor Vertex;
    //typedef typename GraphTraits::size_type size_type;
    typedef std::size_t size_type;

    const size_type num = num_vertices(G);

    typename GraphTraits::vertex_iterator v, vend;
    for (boost::tie(v, vend) = vertices(G); v != vend; ++v) {
      put(marker, *v, num);
      put(degree, *v, out_degree(*v, G));
      degree_buckets.push(*v);
    }

    size_type minimum_degree = 0;
    size_type current_order = num - 1;

    while ( 1 ) {
      typedef typename BucketSorter::stack MDStack;
      MDStack minimum_degree_stack = degree_buckets[minimum_degree];
      while (minimum_degree_stack.empty())
        minimum_degree_stack = degree_buckets[++minimum_degree];

      Vertex node = minimum_degree_stack.top();
      put(order, current_order, node);

      if ( current_order == 0 ) //find all vertices
        break;

      minimum_degree_stack.pop();
      put(marker, node, 0); //node has been ordered.

      typename GraphTraits::adjacency_iterator v, vend;
      for (boost::tie(v,vend) = adjacent_vertices(node, G); v != vend; ++v)

        if ( get(marker,*v) > current_order ) { //*v is unordered vertex
          put(marker, *v, current_order);  //mark the columns adjacent to node

          //delete *v from the bucket sorter
          degree_buckets.remove(*v);

          //It is possible minimum degree goes down
          //Here we keep tracking it.
          put(degree, *v, get(degree, *v) - 1);
          BOOST_USING_STD_MIN();
          minimum_degree = min BOOST_PREVENT_MACRO_SUBSTITUTION(minimum_degree, get(degree, *v));

          //reinsert *v in the bucket sorter with the new degree
          degree_buckets.push(*v);
        }

      current_order--;
    }

    //at this point, order[i] = v_i;
  }

  template <class VertexListGraph, class Order>
  void
  smallest_last_vertex_ordering(const VertexListGraph& G, Order order) {
    typedef typename graph_traits<VertexListGraph>::vertex_descriptor vertex_descriptor;
    typedef typename graph_traits<VertexListGraph>::degree_size_type degree_size_type;
    smallest_last_vertex_ordering(G, order,
                                  make_shared_array_property_map(num_vertices(G), degree_size_type(0), get(vertex_index, G)),
                                  make_shared_array_property_map(num_vertices(G), (std::size_t)(0), get(vertex_index, G)));
  }

  template <class VertexListGraph>
  std::vector<typename graph_traits<VertexListGraph>::vertex_descriptor>
  smallest_last_vertex_ordering(const VertexListGraph& G) {
    std::vector<typename graph_traits<VertexListGraph>::vertex_descriptor> o(num_vertices(G));
    smallest_last_vertex_ordering(G, make_iterator_property_map(o.begin(), typed_identity_property_map<std::size_t>()));
    return o;
  }
}

#endif


/* A simple program that demonstrates a simple homomorphic program on 4 input values.
 * */

#include <iostream>
#include <NTL/ZZ.h>
#include <NTL/BasicThreadPool.h>
#include "FHE.h"
#include "timing.h"
#include "EncryptedArray.h"
#include <NTL/lzz_pXFactoring.h>

#include <cassert>
#include <cstdio>

namespace std {} using namespace std;

int main(int argc, char *argv[])
{
  ArgMapping amap;

  // Technical parameter. Use if you want to explicitly
  // set generator elements for the plaintext arrays.
  // These are generators in the group theoretic sense of
  // generating a particular group.
  Vec<long> generators;
  amap.arg("generators", generators, "use specified vector of generators", NULL);
  amap.note("e.g., generators='[562 1871 751]'");

  // Technical parameter. The order of each generator
  // specified in orders. Recall the order e of a group
  // element g is the number e such g**e == 1
  Vec<long> orders;
  amap.arg("orders", orders, "use specified vector of orders", NULL);
  amap.note("e.g., orders='[4 2 -4]', negative means 'bad'");

  // Random seed used in scheme.
  long seed=0;
  amap.arg("seed", seed, "PRG seed");

  // The number of rounds of encrypted computation. If
  // num_rounds > 1, then we need to "bootstrap" between
  // rounds which adds a heavy computational overhead.
  long num_rounds = 1;
  amap.arg("num_rounds", num_rounds, "number of rounds");

  // If plaintext_base_prime=2, then plaintext entries are
  // bits.
  long plaintext_base_prime = 2;
  amap.arg("plaintext_base_prime", plaintext_base_prime,
      "plaintext base prime");

  // Technical parameter. In case finite_field_degree=1,
  // then plaintext entries are just bits.
  long finite_field_degree=1;
  amap.arg("finite_field_degree", finite_field_degree,
      "finite_field_degree");

  long d=1;
  amap.arg("d", d, "degree of the field extension");

  // Key-switching is an operation which swaps out the key
  // under which a particular ciphertext is encoded.
  // Key-switching is used in a variety of places in
  // homomorphic encryption, notably during
  // "relinearization" which happens after homomorphic
  // multiplication of ciphertexts. The key-switching
  // matrix is a 2xn matrix, where n is some number <
  // num_levels (see description of num_levels below).
  long num_key_columns = 2;
  amap.arg("num_key_columns", num_key_columns, "number of columns in the key-switching matrices");

  // The number of "bits" of security the scheme provides
  // (see https://en.wikipedia.org/wiki/Security_level).
  // Basic idea is that for a security level of 80, the
  // attacker needs to perform ~2^80 operations to break
  // the scheme.
  long security_parameter=80;
  amap.arg("security_parameter", security_parameter,
      "security parameter");

  // The number of levels in the modulus chain. See
  // detailed comment below.
  long num_levels = 0;
  amap.arg("num_levels", num_levels, "# of levels in the modulus chain",  "heuristic");

  long num_slots=0;
  amap.arg("num_slots", num_slots, "minimum number of slots");

  // See comment about cyclotomic polynomials below. If
  // chosen_cyclotomic_degree is set, this value is passed
  // to helper findM that checks if it is secure.
  long chosen_cyclotomic_degree=0;
  amap.arg("cyclotomic_degree", chosen_cyclotomic_degree, "use specified value for cyclotomic polynomial.", NULL);

  amap.parse(argc, argv);

  SetSeed(ZZ(seed));

  // num_levels is the number of "levels" to the FHE
  // scheme. The number of levels governs how many compute
  // operations can be performed on encrypted data before
  // the encryption needs to be refreshed (this refreshing
  // process is called "bootstrapping").  See comment
  // below about the modulus chain.
  if (num_levels==0) {
    // determine num_levels based on num_rounds,r
    num_levels = 3*num_rounds+3;
    if (plaintext_base_prime>2 || finite_field_degree>1) { // add some more primes for each round
      long addPerRound = 2*ceil(log((double)plaintext_base_prime)*finite_field_degree*3)/(log(2.0)*FHE_p2Size) +1;
      num_levels += num_rounds * addPerRound;
    }
  }

  // Hamming weight of secret key
  long sec_key_weight = 64;

  // The FHE scheme uses a technical parameter called a
  // cyclotomic polynomial.  These polynomials are indexed
  // by whole numbers m. The helper findM helps select a
  // value of m that meets our security requirements.
  long cyclotomic_degree = FindM(security_parameter,
      num_levels, num_key_columns, plaintext_base_prime,
      d, num_slots, chosen_cyclotomic_degree, false);

  // Converting generators and orders into vector<long> types.
  vector<long> generators1, orders1;
  convert(generators1, generators);
  convert(orders1, orders);

  // FHEcontext is a convenient book-keeping class that
  // stores a variety of parameters tied to the fully
  // homomorphic encryption scheme.
  FHEcontext context(cyclotomic_degree,
      plaintext_base_prime, finite_field_degree,
      generators1, orders1);
  // FHE schemes use a sequence of parameters called the
  // modulus chain. These "moduli" are ordered in size,
  // q_0 < q_1 < --- < q_L. At the start of encryption,
  // the largest modulus q_L is used. For technical
  // reasons, as encryption proceeds, have to swap down to
  // smaller and smaller moduli. When q_0 is reached, the
  // FHE scheme can no longer compute on the encrypted
  // data. At this point, a "bootstrapping" step is needed
  // (not used in this file) to refresh.
  buildModChain(context, num_levels, num_key_columns);

  // irred_poly is a technical parameter used to define
  // the plaintexts. Formally, an irreducible polynomial.
  ZZX irred_poly;
  irred_poly = makeIrredPoly(plaintext_base_prime, d);

  // Print some information about the security level of
  // the current scheme.
  std::cout << "security=" << context.securityLevel()<<endl;

  // Stores the secret key. Almost like the FHEPubKey
  // object,
  FHESecKey secretKey(context);

  // The public key contains the encryption of the
  // constant 0 (that is, Enc(0)) along with key-switching
  // matrices and some bookkeeping information.
  const FHEPubKey& publicKey = secretKey;
  // A secret key with specified Hamming weight. The
  // hamming weight is the number of nonzero entries in
  // the secret key.
  secretKey.GenSecKey(sec_key_weight);
  // compute key-switching matrices that we need
  addSome1DMatrices(secretKey);

  // A convenience class that allows for operations on an array of plaintexts.
  // The size of this array is set automatically by the choice of parameters
  // listed above.
  EncryptedArray ea(context, irred_poly);
  long nslots = ea.size();
  std::cout << "nslot = " << nslots << endl;

  // A PlaintextArray must be paired with an EncryptedArray.
  NewPlaintextArray p0(ea);
  NewPlaintextArray p1(ea);
  NewPlaintextArray p2(ea);
  NewPlaintextArray p3(ea);

  // Populate our plaintext arrays with random values.
  random(ea, p0);
  random(ea, p1);
  random(ea, p2);
  random(ea, p3);

  // Construct our ciphertext objects
  Ctxt c0(publicKey), c1(publicKey), c2(publicKey), c3(publicKey);

  // Encrypt our plaintexts into the ciphertext
  ea.encrypt(c0, publicKey, p0);
  ea.encrypt(c1, publicKey, p1);
  ea.encrypt(c2, publicKey, p2);
  ea.encrypt(c3, publicKey, p3);

  // random number in [-nslots/2..nslots/2]
  long shamt = RandomBnd(2*(nslots/2) + 1) - (nslots/2);
  // random number in [-(nslots-1)..nslots-1]
  long rotamt = RandomBnd(2*nslots - 1) - (nslots - 1);

  // two random constants
  NewPlaintextArray const1(ea);
  NewPlaintextArray const2(ea);
  random(ea, const1);
  random(ea, const2);

  ZZX const1_poly, const2_poly;
  ea.encode(const1_poly, const1);
  ea.encode(const2_poly, const2);

  // Perform computation upon encrypted ciphertexts
  c1.multiplyBy(c0);
  c0.addConstant(const1_poly);
  c2.multByConstant(const2_poly);
  Ctxt tmp(c1);
  ea.shift(tmp, shamt);
  c2 += tmp;
  ea.rotate(c2, rotamt);
  c1.negate();
  c3.multiplyBy(c2);
  c0 -= c3;

  // Perform computations upon plaintext data. Will check
  // that decryption of the encrypted data equals the
  // output of the plaintext computation.
  mul(ea, p1, p0);     // c1.multiplyBy(c0)
  add(ea, p0, const1); // c0 += random constant
  mul(ea, p2, const2); // c2 *= random constant
  NewPlaintextArray tmp_p(p1); // tmp = c1
  shift(ea, tmp_p, shamt); // ea.shift(tmp, random amount in [-nSlots/2,nSlots/2])
  add(ea, p2, tmp_p);  // c2 += tmp
  rotate(ea, p2, rotamt); // ea.rotate(c2, random amount in [1-nSlots, nSlots-1])
  ::negate(ea, p1); // c1.negate()
  mul(ea, p3, p2); // c3.multiplyBy(c2)
  sub(ea, p0, p3); // c0 -= c3

  c0.cleanUp();
  c1.cleanUp();
  c2.cleanUp();
  c3.cleanUp();

  // Create new plaintexts we use to store decryption of
  // homomorphic outputs.
  NewPlaintextArray pp0(ea);
  NewPlaintextArray pp1(ea);
  NewPlaintextArray pp2(ea);
  NewPlaintextArray pp3(ea);

  // Decrypt the ciphertexts
  ea.decrypt(c0, secretKey, pp0);
  ea.decrypt(c1, secretKey, pp1);
  ea.decrypt(c2, secretKey, pp2);
  ea.decrypt(c3, secretKey, pp3);

  // Check that the decrypted ciphertexts have the right
  // values.
  if (equals(ea, pp0, p0) && equals(ea, pp1, p1)
      && equals(ea, pp2, p2) && equals(ea, pp3, p3))
       std::cout << "Homomorphic Computation performed correctly.\n";
  else std::cout << "ERROR\n";

}


// SPDX-License-Identifier: MIT
// Copyright (c) 2019-2021 Thomas Vanderbruggen <th.vanderbruggen@gmail.com>

#ifndef SCICPP_LINALG_SOLVE
#define SCICPP_LINALG_SOLVE

#include "scicpp/linalg/utils.hpp"

#include <Eigen/Dense>
#include <type_traits>

namespace scicpp::linalg {

template <class EigenMatrix, class StdContainer>
auto lstsq(const EigenMatrix &A, const StdContainer &b) {
    static_assert(std::is_same_v<typename EigenMatrix::value_type,
                                 typename StdContainer::value_type>);

    return to_std_container(
        A.fullPivHouseholderQr().solve(to_eigen_matrix(b)).eval());
}

} // namespace scicpp::linalg

#endif // SCICPP_LINALG_SOLVE


// Copyright (c) 2022 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <gsl/gsl_spline.h>

#include <Eigen/Core>
#include <functional>
#include <memory>

#include "pyinterp/detail/gsl/accelerator.hpp"

namespace pyinterp::detail::gsl {

/// Interpolate a 1-D function
class Interpolate1D {
 public:
  /// Interpolate a 1-D function
  ///
  /// @param size Size of workspace
  /// @param type fitting model
  /// @param acc Accelerator
  Interpolate1D(const size_t size, const gsl_interp_type* type, Accelerator acc)
      : workspace_(
            std::unique_ptr<gsl_spline, std::function<void(gsl_spline*)>>(
                gsl_spline_alloc(type, size),
                [](gsl_spline* ptr) { gsl_spline_free(ptr); })),
        acc_(std::move(acc)) {}

  /// Returns the name of the interpolation type used
  [[nodiscard]] inline auto name() const noexcept -> std::string {
    return gsl_spline_name(workspace_.get());
  }

  /// Return the minimum number of points required by the interpolation
  [[nodiscard]] inline auto min_size() const noexcept -> size_t {
    return gsl_spline_min_size(workspace_.get());
  }

  /// Return the interpolated value of y for a given point x
  [[nodiscard]] inline auto interpolate(const Eigen::VectorXd& xa,
                                        const Eigen::VectorXd& ya,
                                        const double x) -> double {
    init(xa, ya);
    return gsl_spline_eval(workspace_.get(), x, acc_);
  }

  /// Return the derivative d of an interpolated function for a given point x
  [[nodiscard]] inline auto derivative(const Eigen::VectorXd& xa,
                                       const Eigen::VectorXd& ya,
                                       const double x) -> double {
    init(xa, ya);
    return gsl_spline_eval_deriv(workspace_.get(), x, acc_);
  }

  /// Return the second derivative d of an interpolated function for a given
  /// point x
  [[nodiscard]] inline auto second_derivative(const Eigen::VectorXd& xa,
                                              const Eigen::VectorXd& ya,
                                              const double x) -> double {
    init(xa, ya);
    return gsl_spline_eval_deriv2(workspace_.get(), x, acc_);
  }

  /// Return the numerical integral result of an interpolated function over the
  /// range [a, b],
  [[nodiscard]] inline auto integral(const Eigen::VectorXd& xa,
                                     const Eigen::VectorXd& ya, const double a,
                                     const double b) -> double {
    init(xa, ya);
    return gsl_spline_eval_integ(workspace_.get(), a, b, acc_);
  }

 private:
  std::unique_ptr<gsl_spline, std::function<void(gsl_spline*)>> workspace_;
  Accelerator acc_;

  /// Initializes the interpolation object
  inline auto init(const Eigen::VectorXd& xa,
                   const Eigen::VectorXd& ya) noexcept -> void {
    acc_.reset();
    gsl_spline_init(workspace_.get(), xa.data(), ya.data(), xa.size());
  }
};

}  // namespace pyinterp::detail::gsl


#include <cmath>

#include <gtest/gtest.h>
#include <Eigen/Dense>

#include <BingoCpp/implicit_regression.h>

#include "test_fixtures.h"

using namespace bingo;

namespace {

class ImplicitRegressionFixture {
 public:
  ImplicitTrainingData *training_data_;
  testutils::SumEquation sum_equation_;

 protected:
  ImplicitTrainingData* init_sample_training_data() {
    const int num_points = 50;
    const int num_data_per_feature = 10;
    const int num_feature = 50 / num_data_per_feature;
    const int index_block_mod = 3;

    Eigen::ArrayXXd x = Eigen::ArrayXd::LinSpaced(num_points, 0, 0.98);
    x = x.reshaped(num_feature, num_data_per_feature);
    x.transposeInPlace();

    Eigen::ArrayXXd dx_dt = Eigen::ArrayXXd::Constant(x.rows(), x.cols(), 1);
    dx_dt.block(
        0, index_block_mod, dx_dt.rows(), dx_dt.cols() - index_block_mod)
        = Eigen::ArrayXXd::Constant(x.rows(), 2, -1);
    dx_dt.col(dx_dt.cols()/2) = Eigen::ArrayXd::Constant(dx_dt.rows(), 0);
    return new ImplicitTrainingData(x, dx_dt);
  }
};

class ImplicitRegressionTestNormalize : public ImplicitRegressionFixture,
                                        public testing::TestWithParam<bool> {
 public:
  virtual void SetUp() {
    training_data_ = init_sample_training_data();
    sum_equation_ = testutils::init_sum_equation();
  }

  virtual void TearDown() {
    delete training_data_;
  }
};

TEST_P(ImplicitRegressionTestNormalize, EvaluateIndividualFitness) {
  bool normalize_dot_product = GetParam();
  auto regressor
      = new ImplicitRegression(training_data_, -1, normalize_dot_product);
  double fitness = regressor->EvaluateIndividualFitness(sum_equation_);
  ASSERT_TRUE(0.14563031020 - fitness < 1e-10);
  delete regressor;
}
INSTANTIATE_TEST_CASE_P(,ImplicitRegressionTestNormalize, testing::Bool());

class ImplicitRegressionTestNonNormalized :
    public ImplicitRegressionFixture,
    public testing::TestWithParam<std::tuple<int, bool>> {
 public:
  virtual void SetUp() {
    training_data_ = init_sample_training_data();
    sum_equation_ = testutils::init_sum_equation();
  }

  virtual void TearDown() {
    delete training_data_;
  }
};

TEST_P(ImplicitRegressionTestNonNormalized, EvaluateIndividualFitness) {
  auto const &param = GetParam();
  auto required_params = std::get<0>(param);
  auto infinite_fitness = std::get<1>(param);
  auto regressor = new ImplicitRegression(training_data_,
                                          required_params,
                                          infinite_fitness);
  double fitness = regressor->EvaluateIndividualFitness(sum_equation_);
  ASSERT_TRUE(!std::isfinite(fitness) == infinite_fitness);
  delete regressor;
}
INSTANTIATE_TEST_CASE_P(instance_one, ImplicitRegressionTestNonNormalized,
  ::testing::Values(std::make_tuple(4, false), std::make_tuple(5, true))
);

class ImplicitRegressionTest : public ImplicitRegressionFixture,
                               public testing::Test {
 public:
  virtual void SetUp() {
    training_data_ = init_sample_training_data();
    sum_equation_ = testutils::init_sum_equation();
  }

  virtual void TearDown() {
    delete training_data_;
  }
};

TEST_F(ImplicitRegressionTest, GetSubsetOfData) {
  auto data_input = Eigen::ArrayXd::LinSpaced(5, 0, 4);
  auto training_data = new ImplicitTrainingData(data_input, data_input);
  auto subset_training_data = training_data->GetItem(std::vector<int>{0, 2, 3});
  Eigen::ArrayXXd expected_subset(3, 1);
  expected_subset << 0, 2, 3;
  ASSERT_TRUE(subset_training_data->x.isApprox(expected_subset));
  ASSERT_TRUE(subset_training_data->dx_dt.isApprox(expected_subset));
  delete training_data;
  delete subset_training_data;
}

TEST_F(ImplicitRegressionTest, CorrectTrainingDataSize) {
  for (int size : std::vector<int> {2, 5, 50}) {
    Eigen::ArrayXXd data_input = Eigen::ArrayXd::LinSpaced(size, 0, 10);
    auto training_data = new ImplicitTrainingData(data_input, data_input);
    ASSERT_EQ(training_data->Size(), size);
    delete training_data;
  }
}

TEST(ImplicitRegressionPartials, PartialCalculationInTrainingData) {
  auto data_input = Eigen::ArrayXd::LinSpaced(20, 0., 19.);
  Eigen::ArrayXXd data_array(data_input.rows(), 3);
  data_array << data_input * 0, data_input * 1, data_input * 2;
  auto training_data = new ImplicitTrainingData(data_array);
  Eigen::ArrayXXd expected_derivatives(13, 3);
  expected_derivatives << Eigen::ArrayXd::Ones(13) * 0,
                          Eigen::ArrayXd::Ones(13) * 1,
                          Eigen::ArrayXd::Ones(13) * 2;
  ASSERT_TRUE(training_data->dx_dt.isApprox(expected_derivatives));
  delete training_data;
}

TEST(ImplicitRegressionPartials, PartialCalculationInTrainingDataNaN) {
  auto data_input = Eigen::ArrayXd::LinSpaced(20, 0., 19.) * 2;
  Eigen::ArrayXXd data_array(data_input.rows() * 2 + 1, 1);
  data_array << data_input,
                std::numeric_limits<double>::quiet_NaN(),
                data_input;
  auto training_data = new ImplicitTrainingData(data_array);
  Eigen::ArrayXd expected_derivative = Eigen::ArrayXd::Constant(26, 2.0);
  ASSERT_TRUE(training_data->dx_dt.isApprox(expected_derivative));
  delete training_data;
}
} // namespace (anonymous)


/**************************************************************************
** This file is a part of our work (Siggraph'16 paper, binary, code and dataset):
**
** Roto++: Accelerating Professional Rotoscoping using Shape Manifolds
** Wenbin Li, Fabio Viola, Jonathan Starck, Gabriel J. Brostow and Neill D.F. Campbell
**
** w.li AT cs.ucl.ac.uk
** http://visual.cs.ucl.ac.uk/pubs/rotopp
**
** Copyright (c) 2016, Wenbin Li
** All rights reserved.
**
** Redistribution and use in source and binary forms, with or without
** modification, are permitted provided that the following conditions are met:
**
** -- Redistributions of source code and data must retain the above
**    copyright notice, this list of conditions and the following disclaimer.
** -- Redistributions in binary form must reproduce the above copyright
**    notice, this list of conditions and the following disclaimer in the
**    documentation and/or other materials provided with the distribution.
**
** THIS WORK AND THE RELATED SOFTWARE, SOURCE CODE AND DATA IS PROVIDED BY
** THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
** WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
** MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN
** NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
** INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
** BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
** USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
** NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
** EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
***************************************************************************/

#ifndef PLANARTRACKERSOLVER_HPP
#define PLANARTRACKERSOLVER_HPP

#include "ceres/ceres.h"
#include "glog/logging.h"
#include "ceres/solver.h"
#include "gflags/gflags.h"
#include <glog/logging.h>

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

#include <array>
#include <random>
#include <cmath>
#include <fstream>

#include "include/rotoSolver/gplvm.hpp"
#include "include/rotoSolver/fileUtils.hpp"
#include "include/rotoSolver/baseDefs.hpp"
#include "include/rotoSolver/eigenUtils.hpp"
#include "include/rotoSolver/transformations.hpp"

DECLARE_double(s);
DECLARE_double(a);
DECLARE_double(r);
DECLARE_double(trans_weight);
DECLARE_double(rot_weight);
DECLARE_double(scale_weight);
DECLARE_bool(skip_initial_optimisation);

struct PlanarTrackSolve
{
    typedef Eigen::MatrixXd Matrix;
    typedef Eigen::VectorXd Vector;

    typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> RowMatrix;
    typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> ColMatrix;

private:

    shared_ptr<GPLVM> _gplvm;

    // Very important that it's RowMajor here..
    RowMatrix _U;

    const int _N, _Q;
    const int _startIdx, _endIdx;

    PlanarTrackingData _keyFrames;

    std::vector<int> _keyFramesIndex;

    std::vector<PlanarTrackingData> _targetTracks;

    Vector _normalisationWeight;

    shared_ptr<RigidMotionEstimator> _motionEstimator;

    RowMatrix _translations;
    Vector _rotations;
    Vector _scales;

    ceres::LossFunctionWrapper* _residualLossFunctionPtr;

public:

    PlanarTrackSolve(shared_ptr<GPLVM> gplvm,
                     const Matrix& U,
                     const std::vector<PlanarTrackingData>& targetTracks,
                     const int startIdx, const int endIdx,
                     const PlanarTrackingData& keyFrames,
                     const std::vector<int>& keyFramesIndex,
                     shared_ptr<RigidMotionEstimator> motionEstimator = shared_ptr<RigidMotionEstimator>());

    void SpecialTest();

    bool RunSolve();

    Matrix GetOutput(Vector* gplvmVariance = NULL) const;
    Matrix GetOutputLinearInterp(Vector* gplvmVariance = NULL) const;

    void SaveToFile(std::string textFilename, std::string confidenceOutputFile = "") const;
    void SaveToFileLinearInterp(std::string textFilename, std::string confidenceOutputFile = "") const;

    bool UsingRotationAndTranslation() const
    {
        return (_motionEstimator.get() != 0);
    }

    const RowMatrix& GetU() const
    {
        return _U;
    }

private:

    Vector CalcConfidence(const Vector& gplvmVariance) const;

    void CropTargetTracks(int width = 1920, int height = 1080);

    void InitRotationAndTranslation();

    template <typename Mat>
    double* getFrameParameterPointer(Mat& M, const int frameIdx)
    {
        nassert (frameIdx >= 0);
        nassert (frameIdx < _N);
        double* ptr = M.data();
        ptr += (frameIdx * M.cols());
        return ptr;
    }

};

#endif // PLANARTRACKERSOLVER_HPP


//
//  rntn.h
//  NeuralNetwork
//
//  Created by Nikhil Joshi on 12/14/14.
//  Copyright (c) 2014 Nikhil Joshi. All rights reserved.
//

#ifndef __NeuralNetwork__rntn__
#define __NeuralNetwork__rntn__

#include <unordered_map>
#include <string>
#include <ctime>

#include <Eigen/Dense>

#include "utility-rntn.hpp"
#include "functionalitysuite.hpp"

namespace NNetwork {

    typedef std::unordered_map<std::string, unsigned long> Vocabulary;

    class RNTN {
    public:

        // constructor
        RNTN(const size_t dimWordRep,
             const size_t numClasses,
             const size_t lowerClassIndex,
             const bool useTensor = true)
        : _dimWordRep(dimWordRep),
        _numClasses(numClasses),
        _lowerClassIndex(lowerClassIndex),
        _useTensor(useTensor) {
            init();
        }

        // Set vocabulary
        void setVocabulary(Vocabulary& vocab)                     { _vocabMap = vocab; }
        // Get vocabulary
        Vocabulary getVocabulary(void) const                      { return _vocabMap; }
        // Set non-linearity
        void setNonlinearlity(const FunctionalitySuite& nonlinearity);

        // Train the network
        double train(const char* trainingFile, const double regularizer = 0,
                     const bool verbos = true);
        // Predict sentiment
        size_t predict(PTree& p);
        size_t predict(PTree_uptr& p);

        // A unit_test method
        friend bool isValidRntnImplementation(RNTN& testNet, const char* testFile,
                                              const size_t numExamples, const bool resetNet);

    private:

        size_t _dimWordRep, _lowerClassIndex, _numClasses;
        Eigen::MatrixXd _sentimentMatrix, _wordRepMatrix, _weightMatrix;
        std::vector<Eigen::MatrixXd> _interactionTensor;
        Vocabulary _vocabMap;
        FunctionalitySuite* _nonlinearity;
        bool _useTensor;

        // No copying of RNTN network
        RNTN& operator=(const RNTN& o) {
            return *this;
        }
        // No assignments
        RNTN(const RNTN& o)
        : _vocabMap(o._vocabMap) {

        }

        // Initialize network to random state
        void init(void);
        // Is the given node a leaf
        bool isLeaf(const PTree& p) const;
        bool isLeaf(const PTree_uptr& p) const;
        // Is a given node a root
        bool isRoot(const PTree& p) const;
        bool isRoot(const PTree_uptr& p) const;
        // Get current representation of a word at the (leaf) node
        Eigen::VectorXd getRepresentation(const PTree& p);
        Eigen::VectorXd getRepresentation(const PTree_uptr& p);
        // Evaluate the sentiment for the given phrase at a given node.
        Eigen::VectorXd evaluateSentiment(PTree& p);
        Eigen::VectorXd evaluateSentiment(PTree_uptr& p);
        // Evaluate representation for the tree rooted at given node
        Eigen::VectorXd evaluateRepresentation(PTree& p);
        Eigen::VectorXd evaluateRepresentation(PTree_uptr& p);
        // Loss function (softmax)
        double loss(const Eigen::Ref<const Eigen::VectorXd>& output,
                    const Eigen::Ref<const Eigen::VectorXd>& label, const double regularizer = 0);
        double loss(PTree& p, const double regularizer = 0);
        double loss(PTree_uptr& p, const double regularizer = 0);
        double loss(std::vector<PTree_uptr>& trees, const double regularizer = 0);
        // Corrections to Ws, W, V
        void correctionsToParamMatrices(PTree& p,
                                        Eigen::MatrixXd& correctionWsent,
                                        Eigen::MatrixXd& correctionW,
                                        std::vector<Eigen::MatrixXd>& correctionV,
                                        Eigen::VectorXd carryDown);
        void correctionsToParamMatrices(PTree_uptr& p,
                                        Eigen::MatrixXd& correctionWsent,
                                        Eigen::MatrixXd& correctionW,
                                        std::vector<Eigen::MatrixXd>& correctionV,
                                        Eigen::VectorXd carryDown);
        void correctionsToParamMatrices(std::vector<PTree_uptr>& trees,
                                        Eigen::MatrixXd& correctionWsent,
                                        Eigen::MatrixXd& correctionW,
                                        std::vector<Eigen::MatrixXd>& correctionV,
                                        Eigen::VectorXd carryDown);

    };

    // Implementation test method
    bool isValidRntnImplementation(RNTN& testNet, const char* testFile,
                                   const size_t numExamples, const bool resetNet = true);

}

#endif /* defined(__NeuralNetwork__rntn__) */


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [saturated_abs]
#include <boost/simd/arithmetic.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/constant/minf.hpp>
#include <iostream>

namespace bs =  boost::simd;
using pack_ft =  bs::pack <float, 4>;
using pack_it =  bs::pack <std::int16_t,4>;

int main()
{
  pack_ft pf = {-1.0f, 2.0f, -3.0f, bs::Minf<float>()       };
  pack_it pi = {-1,    2,    -3,    bs::Minf<std::int16_t>()};
  std::cout << " pf =  " << pf  << " -> bs::abs(pf) =                  " << bs::abs(pf)                 << std::endl;
  std::cout << " pi =  " << pi  << " -> bs::abs(pi) =                  " << bs::abs(pi)                 << std::endl;
  std::cout << " pi =  " << pi  << " -> bs::saturated_(bs::abs(pi)) =  " << bs::saturated_(bs::abs)(pi) << std::endl;
  return 0;
}
//! [saturated_abs]


#pragma once

#ifndef _VECTOR_H_
#define _VECTOR_H_

// Includes
#include <Eigen/Dense>
#include <viennacl/vector.hpp>
#include "AbstractLinearAlgebraObject.hpp"

namespace LightBulb
{
	/**
	 * \brief Describes a one dimensional linear algebra data structure, also called vector.
	 * \tparam DataType The data type which should be stored in the data structure.
	 */
	template<typename DataType = float>
	class Vector : public AbstractLinearAlgebraObject<Eigen::Matrix<DataType, -1, 1>, viennacl::vector<DataType>>
	{
	protected:
		void copyToEigen() const override
		{
			if (this->eigenValue.size() != this->viennaclValue.size())
				this->eigenValue.resize(this->viennaclValue.size());

			viennacl::copy(this->viennaclValue, this->eigenValue);
		}
		void copyToViennaCl() const override
		{
			if (this->eigenValue.size() != this->viennaclValue.size())
				this->viennaclValue.resize(this->eigenValue.size());

			if (this->eigenValue.size() != 0)
				viennacl::copy(this->eigenValue, this->viennaclValue);
		}
	public:
		/**
		* \brief Creates a new matrix with a specific size.
		* \param rows The amount of rows.
		*/
		Vector(int rows = 0)
		{
			if (rows > 0) {
				this->eigenValue = Eigen::Matrix<DataType, -1, 1>(rows);
				this->eigenValueIsDirty = true;
			}
		}
		Vector(const Vector& other)
			: AbstractLinearAlgebraObject<Eigen::Matrix<DataType, -1, 1>, viennacl::vector<DataType>>()
		{
			if (!((other.eigenValueIsDirty && other.eigenValue.size() == 0) || (other.viennaclValueIsDirty && other.viennaclValue.empty()) || (other.eigenValue.size() == 0 && other.viennaclValue.empty())))
				this->copyAllFrom(other);
		}
		Vector(const Eigen::Matrix<DataType, -1, 1>& eigenVector)
		{
			this->eigenValue = eigenVector;
			this->eigenValueIsDirty = true;
		}
	};
}

#include "LightBulb/IO/VectorIO.hpp"

#endif


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

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

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

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

 /*!

    @ingroup group-trigonometric
    Function object implementing acsc capabilities

    inverse cosecant in radian.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = acsc(x);
    @endcode

    is similar to:

    @code
    T r = asin(rec(x));
    @endcode

    @see acscd

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

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

#endif


#include <iostream>
#include <armadillo>

using namespace std;
using namespace arma;

int main()
{
  ///////////////////////////ANN LAYER TEST (USER INPUT)////////////////////////
  arma::mat input;
  input << 1  << 19  << arma::endr
        << 2  << 20  << arma::endr
        << 3  << 21  << arma::endr
        << 4  << 22  << arma::endr
        << 5  << 23  << arma::endr
        << 6  << 24  << arma::endr
        << 7  << 25  << arma::endr
        << 8  << 26  << arma::endr
        << 9  << 27  << arma::endr
        << 10 << 28  << arma::endr
        << 11 << 29  << arma::endr
        << 12 << 30  << arma::endr
        << 13 << 31  << arma::endr
        << 14 << 32  << arma::endr
        << 15 << 33  << arma::endr
        << 16 << 34  << arma::endr
        << 17 << 35  << arma::endr
        << 18 << 36  << arma::endr;

  size_t size = 3; // number of channels
  const double eps = 1e-5;
  const double momentum = 0.1;
  //////////////////////////////////////////////////////////////////////////////

  ///////////////////////////INSTANCE NORM FORWARD//////////////////////////////
  const size_t shapeA = input.n_rows;
  const size_t shapeB = input.n_cols;
  const size_t shapeC = size;
  arma::mat runningTemp = arma::zeros(shapeC, 1);
  size *= input.n_cols;
  input = arma::vectorise(input);

    /////////////////////////BATCH NORM RESET + FORWARD/////////////////////////
    arma::mat weights, runningMean, runningVariance;
    weights.set_size(size + size, 1);
    runningMean.zeros(size, 1);
    runningVariance.ones(size, 1);
    arma::mat gamma, beta;
    gamma = arma::mat(weights.memptr(), size, 1, false, false);
    beta = arma::mat(weights.memptr() + gamma.n_elem, size, 1, false, false);
    gamma.fill(1.0);
    beta.fill(0.0);
    const size_t batchSize = input.n_cols;
    const size_t inputSize = input.n_rows / size;
    arma::mat output;
    output.set_size(arma::size(input));
    arma::cube inputTemp(const_cast<arma::mat&>(input).memptr(), inputSize, size, batchSize, false, false);
    arma::cube outputTemp(const_cast<arma::mat&>(output).memptr(), inputSize, size, input.n_cols, false, false);
    outputTemp = inputTemp;
    arma::mat mean = arma::mean(arma::mean(inputTemp, 2), 0);
    arma::mat variance = arma::mean(arma::mean(arma::pow(inputTemp.each_slice() - arma::repmat(mean,inputSize, 1), 2), 2), 0);
    outputTemp.each_slice() -= arma::repmat(mean, inputSize, 1);
    arma::cube inputMean;
    inputMean.set_size(arma::size(inputTemp));
    inputMean = outputTemp;
    outputTemp.each_slice() /= arma::sqrt(arma::repmat(variance, inputSize, 1) + eps);
    arma::cube normalized;
    normalized.set_size(arma::size(inputTemp));
    normalized = outputTemp;
    outputTemp.each_slice() %= arma::repmat(gamma.t(),inputSize, 1);
    outputTemp.each_slice() += arma::repmat(beta.t(), inputSize, 1);
    double nElements = 1.0 / (input.n_elem - size + eps);
    runningMean = (1 - momentum) * runningMean + momentum * mean.t();
    runningVariance = (1 - momentum) * runningVariance + input.n_elem * nElements * momentum * variance.t();
    //////////////////////////////////////////////////////////////////////////////

  input.reshape(shapeA, shapeB);
  output.reshape(shapeA, shapeB);
  runningMean.reshape(shapeC, shapeB);
  runningVariance.reshape(shapeC, shapeB);
  runningTemp = arma::mean(runningMean, 1);
  runningMean.set_size(shapeC, 1);
  runningMean = runningTemp;
  runningTemp = arma::mean(runningVariance, 1);
  runningVariance.set_size(shapeC, 1);
  runningVariance = runningTemp;
  mean.reshape(shapeC, shapeB);
  //////////////////////////////////////////////////////////////////////////////

  ///////////////////////////ANN LAYER TEST (USER INPUT)////////////////////////
  arma::mat gy = output;
  //////////////////////////////////////////////////////////////////////////////

  ///////////////////////////INSTANCE NORM BACKWARD/////////////////////////////
  gy = arma::vectorise(gy);
  input = arma::vectorise(input);

    ///////////////////////////BATCH NORM BACKWARD////////////////////////////////
    arma::mat g;
    const arma::mat stdInv = 1.0 / arma::sqrt(variance + eps);
    g.set_size(arma::size(input));
    arma::cube gyTemp(const_cast<arma::mat&>(gy).memptr(), input.n_rows / size, size, input.n_cols, false, false);
    arma::cube gTemp(const_cast<arma::mat&>(g).memptr(), input.n_rows / size, size, input.n_cols, false, false);
    arma::cube norm = gyTemp.each_slice() % arma::repmat(gamma.t(), input.n_rows / size, 1);
    arma::mat temp = arma::sum(norm % inputMean, 2);
    arma::mat vars = temp % arma::repmat(arma::pow(stdInv, 3), input.n_rows / size, 1) * -0.5;
    gTemp = (norm.each_slice() % arma::repmat(stdInv, input.n_rows / size, 1) + (inputMean.each_slice() % vars * 2)) / input.n_cols;
    arma::mat normTemp = arma::sum(norm.each_slice() %arma::repmat(-stdInv, input.n_rows / size, 1) , 2) / input.n_cols;
    gTemp.each_slice() += normTemp;
    //////////////////////////////////////////////////////////////////////////////

  input.reshape(shapeA, shapeB);
  output.reshape(shapeA, shapeB);
  g.reshape(shapeA, shapeB);
  gy.reshape(shapeA, shapeB);
  //////////////////////////////////////////////////////////////////////////////

  cout << "-----------------------------------" << endl;
  mean.print("Input mean: ");
  cout << "-----------------------------------" << endl;
  variance.print("Input variance: ");
  cout << "-----------------------------------" << endl;
  output.print("Output: ");
  cout << "-----------------------------------" << endl;
  runningMean.print("Running Mean: ");
  cout << "-----------------------------------" << endl;
  runningVariance.print("Running Variance: ");
  cout << "-----------------------------------" << endl;
  g.print("g: ");
  cout << "-----------------------------------" << endl;
  gy.print("gy: ");
  cout << "-----------------------------------" << endl;
  cout << "Sum of values in g matrix : " << arma::accu(g) << endl;
  cout << "-----------------------------------" << endl;

  return 0;
}


#include <gtest/gtest.h>

#include <Eigen/Eigenvalues>
#include <cppsim/circuit.hpp>
#include <cppsim/gate_factory.hpp>
#include <cppsim/gate_named_pauli.hpp>
#include <cppsim/observable.hpp>
#include <cppsim/pauli_operator.hpp>
#include <cppsim/state.hpp>
#include <cppsim/type.hpp>
#include <cppsim/utility.hpp>
#include <csim/constant.hpp>
#include <fstream>

#include "../util/util.hpp"

TEST(ObservableTest, CheckExpectationValue) {
    const UINT n = 4;
    const UINT dim = 1ULL << n;
    const double eps = 1e-14;
    double coef;
    CPPCTYPE res;
    CPPCTYPE test_res;
    Random random;

    Eigen::MatrixXcd X(2, 2);
    X << 0, 1, 1, 0;

    QuantumState state(n);
    state.set_computational_basis(0);
    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    test_state(0) = 1.;

    coef = random.uniform();
    Observable observable(n);
    observable.add_operator(coef, "X 0");
    Eigen::MatrixXcd test_observable = Eigen::MatrixXcd::Zero(dim, dim);
    test_observable += coef * get_expanded_eigen_matrix_with_identity(0, X, n);

    res = observable.get_expectation_value(&state);
    test_res = (test_state.adjoint() * test_observable * test_state);
    ASSERT_NEAR(test_res.real(), res.real(), eps);
    ASSERT_NEAR(res.imag(), 0, eps);
    ASSERT_NEAR(test_res.imag(), 0, eps);

    state.set_Haar_random_state();
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state.data_cpp()[i];
    res = observable.get_expectation_value(&state);
    test_res = (test_state.adjoint() * test_observable * test_state);
    ASSERT_NEAR(test_res.real(), res.real(), eps);
    ASSERT_NEAR(res.imag(), 0, eps);
    ASSERT_NEAR(test_res.imag(), 0, eps);

    for (UINT repeat = 0; repeat < 10; ++repeat) {
        Observable rand_observable(n);
        Eigen::MatrixXcd test_rand_observable =
            Eigen::MatrixXcd::Zero(dim, dim);

        UINT term_count = random.int32() % 10 + 1;
        for (UINT term = 0; term < term_count; ++term) {
            std::vector<UINT> paulis(n, 0);
            Eigen::MatrixXcd test_rand_observable_term =
                Eigen::MatrixXcd::Identity(dim, dim);
            coef = random.uniform();
            for (UINT i = 0; i < paulis.size(); ++i) {
                paulis[i] = random.int32() % 4;

                test_rand_observable_term *=
                    get_expanded_eigen_matrix_with_identity(
                        i, get_eigen_matrix_single_Pauli(paulis[i]), n);
            }
            test_rand_observable += coef * test_rand_observable_term;

            std::string str = "";
            for (UINT ind = 0; ind < paulis.size(); ind++) {
                UINT val = paulis[ind];
                if (val != 0) {
                    if (val == 1)
                        str += " X";
                    else if (val == 2)
                        str += " Y";
                    else if (val == 3)
                        str += " Z";
                    str += " " + std::to_string(ind);
                }
            }
            rand_observable.add_operator(coef, str.c_str());
        }

        state.set_Haar_random_state();
        for (ITYPE i = 0; i < dim; ++i) test_state[i] = state.data_cpp()[i];

        res = rand_observable.get_expectation_value(&state);
        test_res = test_state.adjoint() * test_rand_observable * test_state;
        ASSERT_NEAR(test_res.real(), res.real(), eps);
        ASSERT_NEAR(res.imag(), 0, eps);
        ASSERT_NEAR(test_res.imag(), 0, eps);
    }
}

TEST(ObservableTest, CheckParsedObservableFromOpenFermionText) {
    auto func = [](const std::string str,
                    const QuantumStateBase* state) -> CPPCTYPE {
        CPPCTYPE energy = 0;

        std::vector<std::string> lines = split(str, "\n");

        for (std::string line : lines) {
            // std::cout << state->get_norm() << std::endl;

            std::vector<std::string> elems;
            elems = split(line, "()j[]+");

            chfmt(elems[3]);

            CPPCTYPE coef(std::stod(elems[0]), std::stod(elems[1]));
            // std::cout << elems[3].c_str() << std::endl;

            PauliOperator mpt(elems[3].c_str(), coef.real());

            // std::cout << mpt.get_coef() << " ";
            // std::cout << elems[3].c_str() << std::endl;
            energy += mpt.get_expectation_value(state);
            // mpt.get_expectation_value(state);
        }
        return energy;
    };

    const double eps = 1e-14;
    const std::string text =
        "(-0.8126100000000005+0j) [] +\n"
        "(0.04532175+0j) [X0 Z1 X2] +\n"
        "(0.04532175+0j) [X0 Z1 X2 Z3] +\n"
        "(0.04532175+0j) [Y0 Z1 Y2] +\n"
        "(0.04532175+0j) [Y0 Z1 Y2 Z3] +\n"
        "(0.17120100000000002+0j) [Z0] +\n"
        "(0.17120100000000002+0j) [Z0 Z1] +\n"
        "(0.165868+0j) [Z0 Z1 Z2] +\n"
        "(0.165868+0j) [Z0 Z1 Z2 Z3] +\n"
        "(0.12054625+0j) [Z0 Z2] +\n"
        "(0.12054625+0j) [Z0 Z2 Z3] +\n"
        "(0.16862325+0j) [Z1] +\n"
        "(-0.22279649999999998+0j) [Z1 Z2 Z3] +\n"
        "(0.17434925+0j) [Z1 Z3] +\n"
        "(-0.22279649999999998+0j) [Z2]";

    CPPCTYPE res, test_res;

    Observable* observable;
    observable = observable::create_observable_from_openfermion_text(text);
    ASSERT_NE(observable, (Observable*)NULL);
    UINT qubit_count = observable->get_qubit_count();

    QuantumState state(qubit_count);
    state.set_computational_basis(0);

    res = observable->get_expectation_value(&state);
    test_res = func(text, &state);

    ASSERT_NEAR(test_res.real(), res.real(), eps);
    ASSERT_NEAR(test_res.imag(), res.imag(), eps);

    state.set_Haar_random_state();

    res = observable->get_expectation_value(&state);
    test_res = func(text, &state);

    ASSERT_NEAR(test_res.real(), res.real(), eps);
    ASSERT_NEAR(test_res.imag(), 0, eps);
    ASSERT_NEAR(res.imag(), 0, eps);
}

/*

TEST(ObservableTest, CheckParsedObservableFromOpenFermionFile) {
    auto func = [](const std::string path,
        const QuantumStateBase* state) -> CPPCTYPE {
        std::ifstream ifs;
        ifs.open(path);
        if (!ifs) {
            std::cerr << "ERROR: Cannot open file" << std::endl;
            return -1.;
        }

        CPPCTYPE energy = 0;

        std::string str;
        while (getline(ifs, str)) {
            // std::cout << state->get_norm() << std::endl;

            std::vector<std::string> elems;
            elems = split(str, "()j[]+");

            chfmt(elems[3]);

            CPPCTYPE coef(std::stod(elems[0]), std::stod(elems[1]));
            // std::cout << elems[3].c_str() << std::endl;

            PauliOperator mpt(elems[3].c_str(), coef.real());

            // std::cout << mpt.get_coef() << " ";
            // std::cout << elems[3].c_str() << std::endl;
            energy += mpt.get_expectation_value(state);
            // mpt.get_expectation_value(state);
        }
        if (!ifs.eof()) {
            std::cerr << "ERROR: Invalid format" << std::endl;
            return -1.;
        }
        ifs.close();
        return energy;
    };

    const double eps = 1e-14;
    const char* filename = "../test/cppsim/H2.txt";

    CPPCTYPE res, test_res;

    Observable* observable;
    observable = observable::create_observable_from_openfermion_file(filename);
    ASSERT_NE(observable, (Observable*)NULL);
    UINT qubit_count = observable->get_qubit_count();

    QuantumState state(qubit_count);
    state.set_computational_basis(0);

    res = observable->get_expectation_value(&state);
    test_res = func(filename, &state);

    ASSERT_EQ(test_res, res);

    state.set_Haar_random_state();

    res = observable->get_expectation_value(&state);
    test_res = func(filename, &state);

    ASSERT_NEAR(test_res.real(), res.real(), eps);
    ASSERT_NEAR(test_res.imag(), 0, eps);
    ASSERT_NEAR(res.imag(), 0, eps);
}

TEST(ObservableTest, CheckSplitObservable) {
    auto func = [](const std::string path,
                    const QuantumStateBase* state) -> CPPCTYPE {
        std::ifstream ifs;
        CPPCTYPE coef;
        ifs.open(path);
        if (!ifs) {
            std::cerr << "ERROR: Cannot open file" << std::endl;
            return -1.;
        }

        CPPCTYPE energy = 0;

        std::string str;
        while (getline(ifs, str)) {
            // std::cout << state->get_norm() << std::endl;

            std::vector<std::string> elems;
            elems = split(str, "()j[]+");

            chfmt(elems[3]);

            CPPCTYPE coef(std::stod(elems[0]), std::stod(elems[1]));
            // std::cout << elems[3].c_str() << std::endl;

            PauliOperator mpt(elems[3].c_str(), coef.real());

            // std::cout << mpt.get_coef() << " ";
            // std::cout << elems[3].c_str() << std::endl;
            energy += mpt.get_expectation_value(state);
            // mpt.get_expectation_value(state);
        }
        if (!ifs.eof()) {
            std::cerr << "ERROR: Invalid format" << std::endl;
            return -1.;
        }
        ifs.close();
        return energy;
    };

    const double eps = 1e-14;
    const char* filename = "../test/cppsim/H2.txt";

    CPPCTYPE diag_res, test_res, non_diag_res;

    std::pair<Observable*, Observable*> observables;
    observables = observable::create_split_observable(filename);
    ASSERT_NE(observables.first, (Observable*)NULL);
    ASSERT_NE(observables.second, (Observable*)NULL);

    UINT qubit_count = observables.first->get_qubit_count();
    QuantumState state(qubit_count);
    state.set_computational_basis(0);

    diag_res = observables.first->get_expectation_value(&state);
    non_diag_res = observables.second->get_expectation_value(&state);
    test_res = func(filename, &state);

    ASSERT_NEAR(test_res.real(), (diag_res + non_diag_res).real(), eps);
    ASSERT_NEAR(test_res.imag(), 0, eps);
    ASSERT_NEAR(diag_res.imag(), 0, eps);
    ASSERT_NEAR(non_diag_res.imag(), 0, eps);

    state.set_Haar_random_state();

    diag_res = observables.first->get_expectation_value(&state);
    non_diag_res = observables.second->get_expectation_value(&state);
    test_res = func(filename, &state);

    ASSERT_NEAR(test_res.real(), (diag_res + non_diag_res).real(), eps);
    ASSERT_NEAR(test_res.imag(), 0, eps);
    ASSERT_NEAR(diag_res.imag(), 0, eps);
    ASSERT_NEAR(non_diag_res.imag(), 0, eps);
}

*/

// Kind of eigenvalue calculation method.
// Only used to specify method in `test_eigenvalue()`.
enum class CalculationMethod {
    PowerMethod,
    ArnoldiMethod,
    LanczosMethod,
};

// Test calculating eigenvalue.
// Actual test code calls this function with prepared observable.
// Return an error message if failed, an empty string if passed.
std::string test_eigenvalue(Observable& observable, const UINT iter_count,
    const double eps, const CalculationMethod method) {
    auto observable_matrix = convert_observable_to_matrix(observable);
    const auto eigenvalues = observable_matrix.eigenvalues();
    CPPCTYPE test_ground_state_eigenvalue = eigenvalues[0];
    for (UINT i = 0; i < eigenvalues.size(); i++) {
        if (eigenvalues[i].real() < test_ground_state_eigenvalue.real()) {
            test_ground_state_eigenvalue = eigenvalues[i];
        }
    }

    const auto qubit_count = observable.get_qubit_count();
    QuantumState state(qubit_count);
    state.set_Haar_random_state();
    CPPCTYPE ground_state_eigenvalue;
    if (method == CalculationMethod::PowerMethod) {
        ground_state_eigenvalue =
            observable.solve_ground_state_eigenvalue_by_power_method(
                &state, iter_count);
    } else if (method == CalculationMethod::ArnoldiMethod) {
        ground_state_eigenvalue =
            observable.solve_ground_state_eigenvalue_by_arnoldi_method(
                &state, iter_count);
    } else if (method == CalculationMethod::LanczosMethod) {
        ground_state_eigenvalue =
            observable.solve_ground_state_eigenvalue_by_lanczos_method(
                &state, iter_count);
    }
    std::string err_message;
    err_message = _CHECK_NEAR(ground_state_eigenvalue.real(),
        test_ground_state_eigenvalue.real(), eps);
    if (err_message != "") return err_message;

    QuantumState multiplied_state(qubit_count);
    QuantumState work_state(qubit_count);
    // multiplied_state = A|q>
    observable.apply_to_state(&work_state, state, &multiplied_state);
    // state = λ|q>
    state.multiply_coef(ground_state_eigenvalue);
    multiplied_state.normalize(multiplied_state.get_squared_norm());
    state.normalize(state.get_squared_norm());

    for (UINT i = 0; i < state.dim; i++) {
        err_message = _CHECK_NEAR(multiplied_state.data_cpp()[i].real(),
            state.data_cpp()[i].real(), eps);
        if (err_message != "") return err_message;
        err_message = _CHECK_NEAR(multiplied_state.data_cpp()[i].imag(),
            state.data_cpp()[i].imag(), eps);
        if (err_message != "") return err_message;
    }
    return "";
}

TEST(ObservableTest, MinimumEigenvalueByPowerMethod) {
    constexpr double eps = 1e-2;
    constexpr UINT qubit_count = 4;
    constexpr UINT test_count = 10;
    UINT pass_count = 0;
    Random random;

    for (UINT i = 0; i < test_count; i++) {
        const UINT operator_count =
            random.int32() % 10 + 2;  // 2 <= operator_count <= 11
        auto observable = Observable(qubit_count);
        observable.add_random_operator(operator_count);
        std::string err_message = test_eigenvalue(
            observable, 500, eps, CalculationMethod::PowerMethod);
        if (err_message == "")
            pass_count++;
        else
            std::cerr << err_message;
    }
    ASSERT_GE(pass_count, test_count - 1);
}

TEST(ObservableTest, MinimumEigenvalueByArnoldiMethod) {
    constexpr double eps = 1e-6;
    constexpr UINT test_count = 10;
    UINT pass_count = 0;
    Random random;

    for (UINT i = 0; i < test_count; i++) {
        // 3 <= qubit_count <= 5
        const auto qubit_count = random.int32() % 4 + 3;
        // 2 <= operator_count <= 11
        const auto operator_count = random.int32() % 10 + 2;
        auto observable = Observable(qubit_count);
        observable.add_random_operator(operator_count);
        std::string err_message = test_eigenvalue(
            observable, 60, eps, CalculationMethod::ArnoldiMethod);
        if (err_message == "")
            pass_count++;
        else
            std::cerr << err_message;
    }
    ASSERT_GE(pass_count, test_count - 1);
}

void add_identity(Observable* observable, Random random) {
    std::vector<UINT> qil;
    std::vector<UINT> qpl;
    for (UINT i = 0; i < observable->get_qubit_count(); i++) {
        qil.push_back(i);
        qpl.push_back(0);
    }
    auto op = PauliOperator(qil, qpl, random.uniform());
    observable->add_operator(&op);
}

// Test observable with identity pauli operator because calculation was unstable
// in this situation.
TEST(ObservableTest, MinimumEigenvalueByArnoldiMethodWithIdentity) {
    constexpr double eps = 1e-6;
    constexpr UINT test_count = 10;
    UINT pass_count = 0;
    Random random;

    for (UINT i = 0; i < test_count; i++) {
        // 3 <= qubit_count <= 5
        const auto qubit_count = random.int32() % 4 + 3;
        // 2 <= operator_count <= 11
        const auto operator_count = random.int32() % 10 + 2;
        auto observable = Observable(qubit_count);
        observable.add_random_operator(operator_count);
        add_identity(&observable, random);
        std::string err_message = test_eigenvalue(
            observable, 70, eps, CalculationMethod::ArnoldiMethod);
        if (err_message == "")
            pass_count++;
        else
            std::cerr << err_message;
    }
    ASSERT_GE(pass_count, test_count - 1);
}

TEST(ObservableTest, MinimumEigenvalueByLanczosMethod) {
    constexpr double eps = 1e-6;
    constexpr UINT test_count = 10;
    UINT pass_count = 0;
    Random random;

    for (UINT i = 0; i < test_count; i++) {
        // 3 <= qubit_count <= 6
        const auto qubit_count = random.int32() % 4 + 3;
        // 2 <= operator_count <= 11
        const auto operator_count = random.int32() % 10 + 2;
        auto observable = Observable(qubit_count);
        observable.add_random_operator(operator_count);
        std::string err_message = test_eigenvalue(
            observable, 70, eps, CalculationMethod::LanczosMethod);
        if (err_message == "")
            pass_count++;
        else
            std::cerr << err_message;
    }
    ASSERT_GE(pass_count, test_count - 1);
}

TEST(ObservableTest, GetDaggerTest) {
    constexpr double eps = 1e-2;
    constexpr UINT qubit_count = 4;

    auto observable = Observable(qubit_count);
    observable.add_operator(1.0, "X 0");
    auto dagger_observable = observable.get_dagger();
    std::string s = dagger_observable->to_string();
    ASSERT_TRUE(s == "(1,-0) X 0" || s == "(1,0) X 0");
}

TEST(ObservableTest, ObservableAndStateHaveDifferentQubitCountTest) {
    auto func = [](const std::string str,
                    const QuantumStateBase* state) -> CPPCTYPE {
        CPPCTYPE energy = 0;

        std::vector<std::string> lines = split(str, "\n");

        for (std::string line : lines) {
            std::vector<std::string> elems;
            elems = split(line, "()j[]+");
            chfmt(elems[3]);
            CPPCTYPE coef(std::stod(elems[0]), std::stod(elems[1]));
            PauliOperator mpt(elems[3].c_str(), coef.real());
            energy += mpt.get_expectation_value(state);
        }
        return energy;
    };

    const double eps = 1e-14;
    const std::string text =
        "(-0.8126100000000005+0j) [] +\n"
        "(0.04532175+0j) [X0 Z1 X2] +\n"
        "(0.04532175+0j) [X0 Z1 X2 Z3] +\n"
        "(0.04532175+0j) [Y0 Z1 Y2] +\n"
        "(0.04532175+0j) [Y0 Z1 Y2 Z3] +\n"
        "(0.17120100000000002+0j) [Z0] +\n"
        "(0.17120100000000002+0j) [Z0 Z1] +\n"
        "(0.165868+0j) [Z0 Z1 Z2] +\n"
        "(0.165868+0j) [Z0 Z1 Z2 Z3] +\n"
        "(0.12054625+0j) [Z0 Z2] +\n"
        "(0.12054625+0j) [Z0 Z2 Z3] +\n"
        "(0.16862325+0j) [Z1] +\n"
        "(-0.22279649999999998+0j) [Z1 Z2 Z3] +\n"
        "(0.17434925+0j) [Z1 Z3] +\n"
        "(-0.22279649999999998+0j) [Z2]";

    CPPCTYPE res, test_res;

    Observable* observable;
    observable = observable::create_observable_from_openfermion_text(text);
    ASSERT_NE(observable, (Observable*)NULL);
    UINT qubit_count = observable->get_qubit_count();

    QuantumState state(qubit_count + 2);  // +2 is point. diff test
    state.set_computational_basis(0);

    res = observable->get_expectation_value(&state);
    test_res = func(text, &state);

    ASSERT_NEAR(res.real(), test_res.real(), eps);
    ASSERT_NEAR(res.imag(), test_res.imag(), eps);

    state.set_Haar_random_state();

    res = observable->get_expectation_value(&state);
    test_res = func(text, &state);

    ASSERT_NEAR(res.real(), test_res.real(), eps);
    ASSERT_NEAR(0, test_res.imag(), eps);
    ASSERT_NEAR(0, res.imag(), eps);
}

TEST(ObservableTest, ApplyIdentityToState) {
    const double eps = 1e-14;

    double coef = .5;
    int n_qubits = 3;
    Observable obs(n_qubits);
    obs.add_operator(coef, "I");
    QuantumState state(n_qubits);
    QuantumState dst_state(n_qubits);
    obs.apply_to_state(&state, &dst_state);
    state.add_state_with_coef(-1 / coef, &dst_state);
    ASSERT_NEAR(0., state.get_squared_norm(), eps);
}


//==================================================================================================
/*!
  @file
    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-euler
   This fFunction object computes the complementary error function
   \f$\displaystyle \frac{2}{\sqrt\pi}\int_{x}^{\infty} e^{-t^2}\mbox{d}t\f$

    @par Header <boost/simd/function/erfc.hpp>

    @par Decorators

   - std_ calls @c std::erfc

    @see erf, erfcx

    @par Example:

      @snippet erfc.cpp erfc

    @par Possible output:

      @snippet erfc.txt erfc
  **/
  IEEEValue erfc(IEEEValue const& x);
} }
#endif

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

#endif


#include <iostream>
#include <array>
#include <boost/histogram.hpp>
#include <boost/format.hpp>
#include <boost/progress.hpp>

constexpr long int addresses = (2 << 21);

std::array<int, addresses> visits = {0};

constexpr inline long int LCG(long int previous) {
    constexpr long int a = 1025;
    constexpr long int c = 3;
    constexpr long int m = addresses;

    return (a * previous + c) % m;
}

int main() {
    long int x = 1;

    boost::progress_display show_progress(2 * addresses);
    for (int i = 0; i < addresses; i++) {
        x = LCG(x);
        visits.at(x)++;
        ++show_progress;
//        std::cout << std::hex << x << std::endl;
    }

    // Histogram
    auto h = boost::histogram::make_histogram(
            boost::histogram::axis::variable<>({0, 1, 2, 3, 4, 9999999})
    );

    for (auto&& addressVisits : visits) {
        h(addressVisits);
        ++show_progress;
    }

    // iterate over bins
    for (auto&& bin : indexed(h)) {
        std::cout << boost::format("bin %i [ %.1f, %.1f ): %i\n")
                     % bin.index() % bin.bin().lower() % bin.bin().upper() % *bin;
    }
}


#include "mwtrans.hpp"

#include <Eigen/Dense>
#include <algorithm>
#include <boost/math/quadrature/gauss_kronrod.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <cmath>
#include <fmt/format.h>
#include <iostream>
#include <limits>

using namespace Eigen;
using namespace boost::math::quadrature;
using boost::math::cyl_bessel_j_zero;
using std::abs;

MWtransInt::MWtransInt(double lb, double v, double abserr, double referr,
                       int max_depth)
    : max_depth_(max_depth), zeros_(nzero_), lb_(lb), v_(v), abserr_(abserr),
      referr_(referr) {
  int nz = 0;
  int i = 0;
  while (nz < nzero_) {
    double r = cyl_bessel_j_zero(v_, ++i);
    if (r > lb_) {
      zeros_(nz) = r;
      ++nz;
    }
  }
  ub_ = zeros_(0) - lb_;
}

double MWtransInt::MWtransInt::perform(std::function<double(double)> functor) {
  // int_lb^z0 functor
  double error;
  double int1 = gauss_kronrod<double, 15>::integrate(
      functor, lb_, zeros_(0), max_depth_, referr_, &error);

  // int_z0^\infty functor
  ArrayXXd aM = ArrayXXd::Zero(niter_, niter_);
  ArrayXXd aN = ArrayXXd::Zero(niter_, niter_);
  ArrayXd aW = ArrayXd::Zero(niter_);
  ArrayXd x = ArrayXd::Zero(niter_);

  aW(0) = 1.0;
  aW(1) = 1.0;

  int i0 = 0;
  double left = zeros_(i0++);
  double right = zeros_(i0++);

  double fxs = gauss_kronrod<double, 15>::integrate(
      functor, left, right, max_depth_, referr_, &error);
  left = right;
  right = zeros_(i0++);
  x(0) = right;

  int neval = 1;
  int count = 0;
  double diff = std::numeric_limits<double>::max();
  double errorbound = 0.0;

  while (count < niter_ - 3 && diff > errorbound) {
    double phixs = gauss_kronrod<double, 15>::integrate(
        functor, left, right, max_depth_, referr_, &error);
    if (phixs == 0 || abs(phixs) < 1.0e-100) {
      diff = 0.0;
      break;
    }
    left = right;
    right = zeros_(i0++);
    x(count + 1) = right;
    ++neval;

    aM(count, 0) = fxs / phixs;
    aN(count, 0) = 1.0 / phixs;
    fxs += phixs;

    if (count == 1) {
      double denom = 1.0 / (1.0 / x(0) - 1.0 / x(1));
      aM(0, 0) = (aM(0, 0) - aM(1, 0)) * denom;
      aN(0, 0) = (aN(0, 0) - aN(1, 0)) * denom;
      denom = 1.0 / (1.0 / x(0) - 1.0 / x(2));
      aM(0, 1) = (aM(0, 0) - aM(1, 0)) * denom;
      aN(0, 1) = (aN(0, 0) - aN(1, 0)) * denom;
    } else if (count > 1) {
      for (int i = 1; i <= count; ++i) {
        double denom = 1.0 / (1.0 / x(count - i) - 1.0 / x(count + 1));
        aM(count - i, i) =
            (aM(count - i, i - 1) - aM(count + 1 - i, i - 1)) * denom;
        aN(count - i, i) =
            (aN(count - i, i - 1) - aN(count + 1 - i, i - 1)) * denom;
      }
    }
    aW(count) = aM(0, count) / aN(0, count);
    errorbound = std::max(abserr_, referr_ * abs(aW(count)));

    if (count > 1) {
      diff = std::max(abs(aW(count) - aW(count - 1)),
                      abs(aW(count) - aW(count - 2)));
    }
    if (std::isnan(diff)) {
      throw "Warning: Divergent partial sums for mW tranformation.";
    }
    count++;
  }
  if ((count == niter_ - 3) && (diff > errorbound)) {
    throw "Warning: Possible non-convergence of mW transformation";
  }

  double result;
  if (count > 0) {
    result = aW(count - 1);
  } else {
    result = aW(count);
    if (count == 0)
      result = 0;
  }

  return result + int1;
}