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


//=======================================================================
// 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


#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 "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;
}


#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include <stdlib.h>
#include <math.h>
#include <inttypes.h>
#include <string.h>
#include <adept_source.h>
#include <adept.h>
#include <adept_arrays.h>
using adept::adouble;
using adept::aVector;

template<typename Return, typename... T>
Return __enzyme_autodiff(T...);
extern "C" {
  extern int enzyme_dup;
  extern int enzyme_const;
  extern int enzyme_dupnoneed;
}

float tdiff(struct timeval *start, struct timeval *end) {
  return (end->tv_sec-start->tv_sec) + 1e-6*(end->tv_usec-start->tv_usec);
}

#define BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
#define BOOST_NO_EXCEPTIONS
#include <iostream>
#include <boost/array.hpp>

#include <boost/numeric/odeint.hpp>

#include <boost/throw_exception.hpp>
void boost::throw_exception(std::exception const & e){
    //do nothing
}

using namespace std;
using namespace boost::numeric::odeint;

#define N 32
#define xmin 0.
#define xmax 1.
#define ymin 0.
#define ymax 1.

#include <assert.h>
#define RANGE(min, max, i, N) ((max-min)/(N-1)*i + min)
#define GETnb(x, i, j) (x)[N*i+j]
#define GET(x, i, j) GETnb(x, i, j)
//#define GET(x, i, j) ({ assert(i >=0); assert( j>=0); assert(j<N); assert(j<N); GETnb(x, i, j); })

template <typename T>
T brusselator_f(T x, T y, T t) {
  bool eq1 = ((x-0.3)*(x-0.3) + (y-0.6)*(y-0.6)) <= 0.1*0.1;
  bool eq2 = t >= 1.1;
  if (eq1 && eq2) {
    return T(5);
  } else {
    return T(0);
  }
}

void init_brusselator(double* __restrict u, double* __restrict v) {
  for(int i=0; i<N; i++) {
    for(int j=0; j<N; j++) {

      double x = RANGE(xmin, xmax, i, N);
      double y = RANGE(ymin, ymax, j, N);

      GETnb(u, i, j) = 22*(y*(1-y))*sqrt(y*(1-y));
      GETnb(v, i, j) = 27*(x*(1-x))*sqrt(x*(1-x));
    }
  }
}

__attribute__((noinline))
void brusselator_2d_loop(double* __restrict du, double* __restrict dv, const double* __restrict u, const double* __restrict v, const double* __restrict p, double t) {
  double A = p[0];
  double B = p[1];
  double alpha = p[2];
  double dx = (double)1/(N-1);

  alpha = alpha/(dx*dx);

  for(int i=0; i<N; i++) {
    for(int j=0; j<N; j++) {

      double x = RANGE(xmin, xmax, i, N);
      double y = RANGE(ymin, ymax, j, N);

      unsigned ip1 = (i == N-1) ? i : (i+1);
      unsigned im1 = (i == 0) ? i : (i-1);

      unsigned jp1 = (j == N-1) ? j : (j+1);
      unsigned jm1 = (j == 0) ? j : (j-1);

      double u2v = GET(u, i, j) * GET(u, i, j) * GET(v, i, j);

      GETnb(du, i, j) = alpha*( GET(u, im1, j) + GET(u, ip1, j) + GET(u, i, jp1) + GET(u, i, jm1) - 4 * GET(u, i, j))
                      + B + u2v - (A + 1)*GET(u, i, j) + brusselator_f(x, y, t);

      GETnb(dv, i, j) = alpha*( GET(v, im1, j) + GET(v, ip1, j) + GET(v, i, jp1) + GET(v, i, jm1) - 4 * GET(v, i, j))
                      + A * GET(u, i, j) - u2v;
    }
  }
}

typedef boost::array< double , 2 * N * N > state_type;

void lorenz( const state_type &x , state_type &dxdt , double t )
{
    // Extract the parameters
  double p[3] = { /*A*/ 3.4, /*B*/ 1, /*alpha*/10. };
  brusselator_2d_loop(dxdt.c_array(), dxdt.c_array() + N * N, x.data(), x.data() + N * N, p, t);
}

// init_brusselator(x.c_array(), x.c_array() + N*N)

double foobar(const double* p, const state_type x, const state_type adjoint, double t) {
    double dp[3] = { 0. };

    state_type dx = { 0. };

    state_type dadjoint_inp = adjoint;

    state_type dxdu;

    __enzyme_autodiff<void>(brusselator_2d_loop,
//                            enzyme_dup, dxdu.c_array(), dadjoint_inp.c_array(),
//                            enzyme_dup, dxdu.c_array() + N * N, dadjoint_inp.c_array() + N * N,
                            enzyme_dupnoneed, nullptr, dadjoint_inp.data(),
                            enzyme_dupnoneed, nullptr, dadjoint_inp.data() + N * N,
                            enzyme_dup, x.data(), dx.data(),
                            enzyme_dup, x.data() + N * N, dx.data() + N * N,
                            enzyme_dup, p, dp,
                            enzyme_const, t);

    return dx[0];
}

#undef GETnb
#define GETnb(x, i, j) (x)(N*i+j)

void abrusselator_2d_loop(aVector& du, aVector& dv, aVector& u, aVector& v, aVector& p, double t) {
  adouble A = p(0);
  adouble B = p(1);
  adouble alpha = p(2);
  adouble dx = (double)1/(N-1);

  alpha = alpha/(dx*dx);

  for(int i=0; i<N; i++) {
    for(int j=0; j<N; j++) {

      adouble x = RANGE(xmin, xmax, i, N);
      adouble y = RANGE(ymin, ymax, j, N);

      unsigned ip1 = (i == N-1) ? i : (i+1);
      unsigned im1 = (i == 0) ? i : (i-1);

      unsigned jp1 = (j == N-1) ? j : (j+1);
      unsigned jm1 = (j == 0) ? j : (j-1);

      adouble u2v = GET(u, i, j) * GET(u, i, j) * GET(v, i, j);

      GETnb(du, i, j) = alpha*( GET(u, im1, j) + GET(u, ip1, j) + GET(u, i, jp1) + GET(u, i, jm1) - 4 * GET(u, i, j))
                      + B + u2v - (A + 1)*GET(u, i, j) + brusselator_f<adouble>(x, y, t);

      GETnb(dv, i, j) = alpha*( GET(v, im1, j) + GET(v, ip1, j) + GET(v, i, jp1) + GET(v, i, jm1) - 4 * GET(v, i, j))
                      + A * GET(u, i, j) - u2v;
    }
  }
}

double afoobar(const double* p_in, const state_type x, const state_type adjoint, double t) {
    adept::Stack stack;

    aVector p(3);
    for(unsigned i=0; i<3; i++) p(i) = p_in[i];
    aVector ax(N*N);
    aVector ay(N*N);
    for(unsigned i=0; i<N*N; i++) {
      ax(i) = x[i];
      ay(i) = x[i+N*N];
    }

    aVector dxdu(N*N);
    aVector dydu(N*N);

    stack.new_recording();

    abrusselator_2d_loop(dxdu, dydu, ax, ay, p, t);

    for(unsigned i=0; i<N*N; i++) {
      dxdu(i).set_gradient(adjoint[i]);
      dydu(i).set_gradient(adjoint[i+N*N]);
    }
    stack.compute_adjoint();

    return ax(0).get_gradient();
}

//! Tapenade
extern "C" {
  /*        Generated by TAPENADE     (INRIA, Ecuador team)
    Tapenade 3.15 (master) -  8 Jan 2020 10:48
*/
#include <adBuffer.h>

/*
  Differentiation of get in reverse (adjoint) mode (with options i4 dr8 r4):
   gradient     of useful results: *x get
   with respect to varying inputs: *x
   Plus diff mem management of: x:in
*/
void get_b(const double *x, double *xb, unsigned int i, unsigned int j, double getb)
{
    double get;
    xb[N*i + j] = xb[N*i + j] + getb;
}

double get_nodiff(const double *x, unsigned int i, unsigned int j) {
    return x[N*i + j];
}

double brusselator_f_nodiff(double x, double y, double t) {
    if ((x-0.3)*(x-0.3) + (y-0.6)*(y-0.6) <= 0.1*0.1 && t >= 1.1)
        return 5.0;
    else
        return 0.0;
}

#if 1
void brusselator_2d_loop_b(double *du, double *dub, double *dv, double *dvb,
        const double *u, double *ub, const double *v, double *vb, const double *p, double *pb,
        double t) {
    double A = p[0];
    double Ab = 0.0;
    double B = p[1];
    double Bb = 0.0;
    double alpha = p[2];
    double alphab = 0.0;
    double dx = (double)1/(N-1);
    alpha = alpha/(dx*dx);
    for (int i = 0; i < N; ++i)
        for (int j = 0; j < N; ++j) {
            double x = (xmax-xmin)/(N-1)*i + xmin;
            double y = (ymax-ymin)/(N-1)*j + ymin;
            unsigned int ip1 = (i == N - 1 ? i : i + 1);
            unsigned int im1 = (i == 0 ? i : i - 1);
            unsigned int jp1 = (j == N - 1 ? j : j + 1);
            unsigned int jm1 = (j == 0 ? j : j - 1);
            double u2v = u[N*i+j]*u[N*i+j]*v[N*i+j];
            double result1;
            pushInteger4(jm1);
            pushInteger4(jp1);
            pushInteger4(im1);
            pushInteger4(ip1);
        }
    *ub = 0.0;
    *vb = 0.0;
    alphab = 0.0;
    Ab = 0.0;
    Bb = 0.0;
    for (int i = N-1; i > -1; --i)
        for (int j = N-1; j > -1; --j) {
            double x;
            double y;
            unsigned int ip1;
            unsigned int im1;
            unsigned int jp1;
            unsigned int jm1;
            double u2v;
            double u2vb = 0.0;
            double result1;
            double temp;
            double tempb;
            popInteger4((int*)&ip1);
            popInteger4((int*)&im1);
            popInteger4((int*)&jp1);
            popInteger4((int*)&jm1);
            temp = u[N*i + j];
            alphab = alphab + (v[N*im1+j]+v[N*ip1+j]+v[N*i+jp1]+v[N*i+jm1]
                -4*v[N*i+j])*dvb[N*i+j] + (u[N*im1+j]+u[N*ip1+j]+u[N*i+
                jp1]+u[N*i+jm1]-4*u[N*i+j])*dub[N*i+j];
            tempb = alpha*dvb[N*i+j];
            Ab = Ab + u[N*i+j]*dvb[N*i+j] - u[N*i+j]*dub[N*i+j];
            ub[N*i + j] = ub[N*i + j] + A*dvb[N*i+j] - (A+1)*dub[N*i+j];
            u2vb = dub[N*i + j] - dvb[N*i + j];
            dvb[N*i + j] = 0.0;
            vb[N*im1 + j] = vb[N*im1 + j] + tempb;
            vb[N*ip1 + j] = vb[N*ip1 + j] + tempb;
            vb[N*i + jp1] = vb[N*i + jp1] + tempb;
            vb[N*i + jm1] = vb[N*i + jm1] + tempb;
            vb[N*i + j] = vb[N*i + j] + temp*temp*u2vb - 4*tempb;
            tempb = alpha*dub[N*i+j];
            Bb = Bb + dub[N*i + j];
            dub[N*i + j] = 0.0;
            ub[N*im1 + j] = ub[N*im1 + j] + tempb;
            ub[N*ip1 + j] = ub[N*ip1 + j] + tempb;
            ub[N*i + jp1] = ub[N*i + jp1] + tempb;
            ub[N*i + jm1] = ub[N*i + jm1] + tempb;
            ub[N*i + j] = ub[N*i + j] + 2*temp*v[N*i+j]*u2vb - 4*tempb;
        }
    alphab = alphab/(dx*dx);
    pb[2] = pb[2] + alphab;
    pb[1] = pb[1] + Bb;
    pb[0] = pb[0] + Ab;

}
#else
/*
  Differentiation of brusselator_2d_loop in reverse (adjoint) mode (with options i4 dr8 r4):
   gradient     of useful results: *du *dv
   with respect to varying inputs: *p *u *du *v *dv
   RW status of diff variables: *p:out *u:out *du:in-out *v:out
                *dv:in-out
   Plus diff mem management of: p:in u:in du:in v:in dv:in
*/
void brusselator_2d_loop_b(double *du, double *dub, double *dv, double *dvb,
        const double *u, double *ub, const double *v, double *vb, const double *p, double *pb,
        double t) {
    double A = p[0];
    double Ab = 0.0;
    double B = p[1];
    double Bb = 0.0;
    double alpha = p[2];
    double alphab = 0.0;
    double dx = (double)1/(N-1);
    alpha = alpha/(dx*dx);
    for (int i = 0; i < N; ++i)
        for (int j = 0; j < N; ++j) {
            double x = (xmax-xmin)/(N-1)*i + xmin;
            double y = (ymax-ymin)/(N-1)*j + ymin;
            unsigned int ip1 = (i == N - 1 ? i : i + 1);
            unsigned int im1 = (i == 0 ? i : i - 1);
            unsigned int jp1 = (j == N - 1 ? j : j + 1);
            unsigned int jm1 = (j == 0 ? j : j - 1);
            double u2v;
            double result1;
            double result2;
            double result3;
            double result4;
            double result5;
            double result6;
            double result7;
            result1 = get_nodiff(u, i, j);
            result2 = get_nodiff(u, i, j);
            result3 = get_nodiff(v, i, j);
            pushReal8(result1);
            result1 = get_nodiff(u, im1, j);
            pushReal8(result2);
            result2 = get_nodiff(u, ip1, j);
            pushReal8(result3);
            result3 = get_nodiff(u, i, jp1);
            result4 = get_nodiff(u, i, jm1);
            result5 = get_nodiff(u, i, j);
            result6 = get_nodiff(u, i, j);
            pushReal8(result1);
            result1 = get_nodiff(v, im1, j);
            pushReal8(result2);
            result2 = get_nodiff(v, ip1, j);
            pushReal8(result3);
            result3 = get_nodiff(v, i, jp1);
            pushReal8(result4);
            result4 = get_nodiff(v, i, jm1);
            pushReal8(result5);
            result5 = get_nodiff(v, i, j);
            pushReal8(result6);
            result6 = get_nodiff(u, i, j);
            pushInteger4(jm1);
            pushInteger4(jp1);
            pushReal8(result6);
            pushReal8(result5);
            pushReal8(result4);
            pushReal8(result3);
            pushReal8(result2);
            pushReal8(result1);
            pushInteger4(im1);
            pushInteger4(ip1);
        }
    *ub = 0.0;
    *vb = 0.0;
    alphab = 0.0;
    Ab = 0.0;
    Bb = 0.0;
    for (int i = N-1; i > -1; --i)
        for (int j = N-1; j > -1; --j) {
            double x;
            double y;
            unsigned int ip1;
            unsigned int im1;
            unsigned int jp1;
            unsigned int jm1;
            double u2v;
            double u2vb;
            double result1;
            double result1b;
            double result2;
            double result2b;
            double result3;
            double result3b;
            double result4;
            double result4b;
            double result5;
            double result5b;
            double result6;
            double result6b;
            double result7;
            double tempb;
            popInteger4((int*)&ip1);
            popInteger4((int*)&im1);
            popReal8(&result1);
            popReal8(&result2);
            popReal8(&result3);
            popReal8(&result4);
            popReal8(&result5);
            popReal8(&result6);
            popInteger4((int*)&jp1);
            popInteger4((int*)&jm1);
            alphab = alphab + (result1+result2+result3+result4-4*result5)*dvb[
                N*i+j];
            tempb = alpha*dvb[N*i+j];
            Ab = Ab + result6*dvb[N*i+j];
            result6b = A*dvb[N*i+j];
            u2vb = dub[N*i + j] - dvb[N*i + j];
            dvb[N*i + j] = 0.0;
            result1b = tempb;
            result2b = tempb;
            result3b = tempb;
            result4b = tempb;
            result5b = -(4*tempb);
            popReal8(&result6);
            get_b(u, ub, i, j, result6b);
            popReal8(&result5);
            get_b(v, vb, i, j, result5b);
            popReal8(&result4);
            get_b(v, vb, i, jm1, result4b);
            popReal8(&result3);
            get_b(v, vb, i, jp1, result3b);
            popReal8(&result2);
            get_b(v, vb, ip1, j, result2b);
            popReal8(&result1);
            get_b(v, vb, im1, j, result1b);
            alphab = alphab + (result1+result2+result3+result4-4*result5)*dub[
                N*i+j];
            tempb = alpha*dub[N*i+j];
            Bb = Bb + dub[N*i + j];
            Ab = Ab - result6*dub[N*i+j];
            result6b = -((A+1)*dub[N*i+j]);
            dub[N*i + j] = 0.0;
            result1b = tempb;
            result2b = tempb;
            result3b = tempb;
            result4b = tempb;
            result5b = -(4*tempb);
            get_b(u, ub, i, j, result6b);
            get_b(u, ub, i, j, result5b);
            get_b(u, ub, i, jm1, result4b);
            popReal8(&result3);
            get_b(u, ub, i, jp1, result3b);
            popReal8(&result2);
            get_b(u, ub, ip1, j, result2b);
            popReal8(&result1);
            get_b(u, ub, im1, j, result1b);
            result1b = result2*result3*u2vb;
            result2b = result1*result3*u2vb;
            result3b = result1*result2*u2vb;
            get_b(v, vb, i, j, result3b);
            get_b(u, ub, i, j, result2b);
            get_b(u, ub, i, j, result1b);
        }
    alphab = alphab/(dx*dx);
    pb[2] = pb[2] + alphab;
    pb[1] = pb[1] + Bb;
    pb[0] = pb[0] + Ab;
}
#endif
}

double tfoobar(const double* p, const state_type x, const state_type adjoint, double t) {
    double dp[3] = { 0. };

    state_type dx = { 0. };

    state_type dadjoint_inp = adjoint;

    state_type dxdu;

    brusselator_2d_loop_b(nullptr, dadjoint_inp.data(),
                          nullptr, dadjoint_inp.data() + N * N,
                          x.data(), dx.data(),
                          x.data() + N * N, dx.data() + N * N,
                          p, dp,
                          t);

    return dx[0];
}

//! Main
int main(int argc, char** argv) {
  const double p[3] = { /*A*/ 3.4, /*B*/ 1, /*alpha*/10. };

  state_type x;
  init_brusselator(x.data(), x.data() + N * N);

  state_type adjoint;
  init_brusselator(adjoint.data(), adjoint.data() + N * N);

  double t = 2.1;

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res;
  for(int i=0; i<10000; i++)
  res = afoobar(p, x, adjoint, t);

  gettimeofday(&end, NULL);
  printf("Adept combined %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res;
  for(int i=0; i<10000; i++)
  res = tfoobar(p, x, adjoint, t);

  gettimeofday(&end, NULL);
  printf("Tapenade combined %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res;
  for(int i=0; i<10000; i++)
  res = foobar(p, x, adjoint, t);

  gettimeofday(&end, NULL);
  printf("Enzyme combined %0.6f res=%f\n", tdiff(&start, &end), res);
  }
  //printf("res=%f\n", foobar(1000));
}

#if 0

typedef boost::array< double , 6 > state_type;

void lorenz( const state_type &x , state_type &dxdt , double t )
{
    // Extract the parameters
    double k1 = x[3];
    double k2 = x[4];
    double k3 = x[5];

    dxdt[0] = -k1 * x[0] + k3 * x[1] * x[2];
    dxdt[1] = k1 * x[0] - k2 * x[1] * x[1] - k3 * x[1] * x[2];
    dxdt[2] = k2 * x[1] * x[1];

    // Don't change the parameters p
    dxdt[3] = 0;
    dxdt[4] = 0;
    dxdt[5] = 0;
}

double foobar(double* p, uint64_t iters) {
    state_type x = { 1.0, 0, 0, p[0], p[1], p[2] }; // initial conditions
    double t = 1e5;
    typedef controlled_runge_kutta< runge_kutta_dopri5< state_type , typename state_type::value_type , state_type , double > > stepper_type;
    //typedef euler< state_type , typename state_type::value_type , state_type , double > stepper_type;
    integrate_const( stepper_type(), lorenz , x , 0.0 , t, t/iters );

    return x[0];
}

typedef boost::array< adouble , 6 > astate_type;

void alorenz( const astate_type &x , astate_type &dxdt , adouble t )
{
    // Extract the parameters
    adouble k1 = x[3];
    adouble k2 = x[4];
    adouble k3 = x[5];

    dxdt[0] = -k1 * x[0] + k3 * x[1] * x[2];
    dxdt[1] = k1 * x[0] - k2 * x[1] * x[1] - k3 * x[1] * x[2];
    dxdt[2] = k2 * x[1] * x[1];

    // Don't change the parameters p
    dxdt[3] = 0;
    dxdt[4] = 0;
    dxdt[5] = 0;
}

adouble afoobar(adouble* p, uint64_t iters) {
    astate_type x = { 1.0, 0, 0, p[0], p[1], p[2] }; // initial conditions
    double t = 1e5;
    typedef controlled_runge_kutta< runge_kutta_dopri5< astate_type , typename astate_type::value_type , astate_type , adouble > > stepper_type;
    //typedef euler< astate_type , typename astate_type::value_type , astate_type , adouble > stepper_type;
    integrate_const( stepper_type(), alorenz , x , 0.0 , t, t/iters );

    return x[0];
}

static
double afoobar_and_gradient(double* p_in, double* dp_out, uint64_t iters) {
    adept::Stack stack;
    adouble x[3] = { p_in[0], p_in[1], p_in[2] };
    stack.new_recording();
    adouble y = afoobar(x, iters);
    y.set_gradient(1.0);
    stack.compute_adjoint();
    for(int i=0; i<3; i++)
      dp_out[i] = x[i].get_gradient();
    return y.value();
}

static void adept_sincos(uint64_t iters) {
  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double p[3] = { 0.04,3e7,1e4 };
  double res = foobar(p, iters);

  gettimeofday(&end, NULL);
  printf("Adept real %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  adept::Stack stack;
  adouble p[3] = { 0.04,3e7,1e4 };
 // stack.new_recording();
  adouble resa = afoobar(p, iters);
  double res = resa.value();

  gettimeofday(&end, NULL);
  printf("Adept forward %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double p[3] = { 0.04,3e7,1e4 };
  double dp[3] = { 0 };
  afoobar_and_gradient(p, dp, iters);

  gettimeofday(&end, NULL);
  printf("Adept combined %0.6f res'=%f\n", tdiff(&start, &end), dp[0]);
  }
}

static void enzyme_sincos(double inp, uint64_t iters) {

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double p[3] = { 0.04,3e7,1e4 };
  double res = foobar(p, iters);

  gettimeofday(&end, NULL);
  printf("Enzyme real %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double p[3] = { 0.04,3e7,1e4 };
  double res = foobar(p, iters);

  gettimeofday(&end, NULL);
  printf("Enzyme forward %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double p[3] = { 0.04,3e7,1e4 };
  double dp[3] = { 0 };
  __enzyme_autodiff<void>(foobar, p, dp, iters);

  gettimeofday(&end, NULL);
  printf("Enzyme combined %0.6f res'=%f\n", tdiff(&start, &end), dp[0]);
  }
}

int main(int argc, char** argv) {

  int max_iters = atoi(argv[1]) ;
  double inp = 2.1;

  //for(int iters=max_iters/20; iters<=max_iters; iters+=max_iters/20) {
  auto iters = max_iters;
    printf("iters=%d\n", iters);
    adept_sincos(inp, iters);
    enzyme_sincos(inp, iters);
  //}
}
#endif


#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Cholesky>
#include <adept_arrays.h>
#include <chrono>

using namespace adept;
using namespace std;
using namespace std::chrono;
int main(int argc, char **argv)
{
   string output_filename = argv[1];
   cout << output_filename << endl;

   int num_params = 4010;
   int num_vars = 1;

   Stack stack;
   aVector  k(num_params);

   Eigen::VectorXd args(num_params * num_vars);
   Eigen::VectorXd ders(num_params * num_vars);

   std::ifstream file("./tests/utils/params.txt");
   int i = 0;
   for (std::string line; std::getline(file, line);)
   {
       args(i) = stod(line.c_str());
       i++;
   }
   file.close();

   for (int index = 0; index < num_params; index++) {
			k(index) = args[index * num_vars + 0];
    }

   ofstream outfile;
   outfile.open(output_filename);

   auto start = high_resolution_clock::now();
   stack.new_recording();                 // Clear any existing differential statements
   aReal J = sum(((k*k+3*k)-k/4)/k+k*k*k*k+k*k*(22/7*k)+k*k*k*k*k*k*k*k*k);
   J.set_gradient(1.0);                   // Seed the dependent variable
   stack.reverse();                       // Reverse-mode differentiation

   auto stop = high_resolution_clock::now();
   auto duration = duration_cast<microseconds>(stop - start);
   outfile << (double)duration.count() / 1000000.0 << " ";
   for (int index = 0; index < num_params; index++) {
			ders[index * num_vars + 0] = k.get_gradient()[index];
    }
   for (int i = 0; i < num_params * num_vars; i++)
   {
        ostringstream ss;
        ss.precision(3);
       ss << fixed << ders[i];
       outfile << ss.str() << " ";
   }

   outfile.close();

   return 0;
}


#include "postprocess.h"

#include <fstream>
#include <map>

#include <Eigen/Dense>

#include "simple_svg.h"

using namespace svg;

int EvaluateEdgeType(Arrangement_2::Halfedge_const_handle he, const Mesh& mesh, const Arrangement_2& overlay, double angle_thres) {
	int fid = he->face()->data() - 1;
	auto& params = mesh.GetPlanes();
	if (he->twin()->face() == overlay.unbounded_face()) {
		return 2;
	}
	else {
		int fid1 = he->twin()->face()->data() - 1;
		if (fid1 == -1)
			return 2;
		if (fid1 != fid) {
			K z = params[fid].ComputeDepth(he->source()->point());
			K n_z = params[fid1].ComputeDepth(he->source()->point());
			if (std::abs((z-n_z).convert_to<double>()) > 1e-6) {
				return 2;
			} else {
				K z = params[fid].ComputeDepth(he->target()->point());
				K n_z = params[fid1].ComputeDepth(he->target()->point());
				if (std::abs((z-n_z).convert_to<double>()) > 1e-6)
					return 2;

				auto& p1 = mesh.GetPlanes()[fid];
				auto& p2 = mesh.GetPlanes()[fid1];
				K dot = p1.n1_ * p2.n1_ + p1.n2_ * p2.n2_ + p1.n3_ * p2.n3_;
				double t1 = dot.convert_to<double>();
				double t2 = cos(angle_thres * 3.141592654 / 180.0);

				if (std::abs(t1) < t2) {
					return 1;
				}
			}
		}
	}

	return 0;
}

void PostProcess::CollectFaceAndVertices(const Mesh& mesh, const Arrangement_2& overlay, double angle_thres) {
	auto& points = points_;
	auto& facets_id = facets_id_;
	auto& facets = facets_;
	auto& params = mesh.GetPlanes();
	auto& depths = depths_;
	std::map<std::pair<Arrangement_2::Vertex_const_handle,int>, int> vertexID;

	for (auto fit = overlay.faces_begin(); fit != overlay.faces_end(); ++fit) {
		int fid = fit->data() - 1;
		if (fit == overlay.unbounded_face() || fid < 0)
			continue;

		EdgeList e;
		auto e_handle = fit->outer_ccb();
		Arrangement_2::Ccb_halfedge_const_circulator curr = e_handle;
		do {
			Arrangement_2::Halfedge_const_handle he = curr;
			auto key = std::make_pair(he->source(), fid);
			auto it = vertexID.find(key);
			if (it == vertexID.end()) {
				vertexID[key] = points.size();
				e.outer_indices.push_back(points.size());
				K z = params[fid].ComputeDepth(he->source()->point());
				points.push_back(key);
				depths.push_back(z);
			} else {
				e.outer_indices.push_back(it->second);
			}
			e.outer_type.push_back(EvaluateEdgeType(he, mesh, overlay, angle_thres));
		} while (++curr != e_handle);

		for (auto hi = fit->holes_begin(); hi != fit->holes_end(); ++hi) {
			auto circ = *hi;
			Arrangement_2::Ccb_halfedge_const_circulator curr = circ;
			e.inner_indices.push_back(std::vector<int>());
			e.inner_type.push_back(std::vector<int>());
			do {
				Arrangement_2::Halfedge_const_handle he = curr;
				auto key = std::make_pair(he->source(), fid);
				auto it = vertexID.find(key);
				if (it == vertexID.end()) {
					vertexID[key] = points.size();
					e.inner_indices.back().push_back(points.size());
					K z = params[fid].ComputeDepth(he->source()->point());
					points.push_back(key);
					depths.push_back(z);

				} else {
					e.inner_indices.back().push_back(it->second);
				}
				e.inner_type.back().push_back(EvaluateEdgeType(he, mesh, overlay, angle_thres));
			} while (++curr != circ);
		}
		facets.push_back(e);
		facets_id.push_back(fid);
	}
}

void PostProcess::RemoveRedundantVertices()
{
	auto& facets = facets_;
	auto& points = points_;
	auto& depths = depths_;
	auto& degrees = degrees_;

	ComputeDegree();

	for (auto& f : facets) {
		RemoveRedundantVerticesFromLoop(f.outer_indices, f.outer_type);
		for (int i = 0; i < f.inner_indices.size(); ++i) {
			RemoveRedundantVerticesFromLoop(f.inner_indices[i], f.inner_type[i]);
		}
	}

	ComputeDegree();
	std::vector<int> compressed_vertexID(degrees.size());
	compressed_vertexID[0] = 0;
	for (int i = 1; i < compressed_vertexID.size(); ++i) {
		compressed_vertexID[i] = compressed_vertexID[i - 1] + (degrees[i - 1] > 0 ? 1 : 0);
	}

	int top = 0;
	for (int i = 0; i < points.size(); ++i) {
		if (degrees[i]) {
			points[top] = points[i];
			depths[top] = depths[i];
			top += 1;
		}
	}

	points.resize(top);
	depths.resize(top);
	for (auto& i : facets) {
		for (auto& e : i.outer_indices) {
			e = compressed_vertexID[e];
		}
		for (auto& es : i.inner_indices) {
			for (auto& e : es)
				e = compressed_vertexID[e];
		}
	}
}

void PostProcess::MergeDuplex(const Mesh& mesh)
{
	auto & vertices = mesh.GetVertices();
	auto & faces = mesh.GetFaces();

	auto& facets = facets_;
	auto& points = points_;
	auto& depths = depths_;
	// merge duplex
	std::map<std::pair<int,std::pair<int,int> >, int> vID;
	std::vector<int> compressed_vertexID;

	int top = 0;
	compressed_vertexID.resize(points.size());
	std::vector<VertexSignature> points_buf;
	std::vector<K> depths_buf;
	for (int i = 0; i < points.size(); ++i) {
		double x = points[i].first->point().x().convert_to<double>();
		double y = points[i].first->point().y().convert_to<double>();
		double z = depths[i].convert_to<double>();

		auto key = std::make_pair(int(x * 1e5), std::make_pair(int(y * 1e5), int(z * 1e5)));
		auto it = vID.find(key);
		if (it == vID.end()) {
			compressed_vertexID[i] = top;
			points_buf.push_back(points[i]);
			depths_buf.push_back(depths[i]);
			vID[key] = top++;
		} else {
			compressed_vertexID[i] = it->second;
		}
	}
	points = points_buf;
	depths = depths_buf;

	auto shrink = [&](std::vector<int>& v) {
		std::vector<int> m(v.size(), 1);
		for (int i = 0; i < v.size(); ++i) {
			int curr = v[i];
			int next = v[(i + 1) % v.size()];
			if (curr == next) {
				m[i] = 0;
			}
		}
		int top = 0;
		for (int i = 0; i < v.size(); ++i) {
			if (m[i] == 1)
				v[top++] = v[i];
		}
		v.resize(top);
	};

	for (auto& i : facets) {
		for (auto& e : i.outer_indices) {
			e = compressed_vertexID[e];
		}
		shrink(i.outer_indices);
		for (auto& es : i.inner_indices) {
			for (auto& e : es) {
				e = compressed_vertexID[e];
			}
			shrink(es);
		}
	}
}

void PostProcess::CollectEdges(const Mesh& mesh) {
	auto & vertices = mesh.GetVertices();
	auto & faces = mesh.GetFaces();
	auto & face_normals = mesh.GetFaceNormals();
	auto & facets_id = facets_id_;
	auto & edge2face = edge2face_;

	auto& facets = facets_;
	auto& points = points_;

	for (int i = 0; i < facets.size(); ++i) {
		for (int j = 0; j < facets[i].outer_indices.size(); ++j) {
			int v0 = facets[i].outer_indices[j];
			int v1 = facets[i].outer_indices[(j + 1) % facets[i].outer_indices.size()];
			auto key = (v0 < v1) ? std::make_pair(v0, v1) : std::make_pair(v1, v0);
			edge2face[key].insert(facets_id[i]);
		}
		for (auto& es : facets[i].inner_indices) {
			for (int j = 0; j < es.size(); ++j) {
				int v0 = es[j];
				int v1 = es[(j + 1) % es.size()];
				auto key = (v0 < v1) ? std::make_pair(v0, v1) : std::make_pair(v1, v0);
				edge2face[key].insert(facets_id[i]);
			}
		}
	}
}

void PostProcess::SaveToFile(const Mesh& mesh, const char* filename) {
	auto & vertices = mesh.GetVertices();
	auto & faces = mesh.GetFaces();
	auto & facets = facets_;
	auto & face_normals = mesh.GetFaceNormals();
	auto & facets_id = facets_id_;
	auto & edge2face = edge2face_;
	auto & points = points_;
	auto & depths = depths_;

	std::ofstream os;

	os.open(filename);

	for (int i = 0; i < points.size(); ++i) {
		double x = points[i].first->point().x().convert_to<double>();
		double y = points[i].first->point().y().convert_to<double>();
		double z = depths[i].convert_to<double>();

		Eigen::Vector3d v(x * z, y * z, z);
		//Eigen::Vector3d v(x, y, z);
		os << "v " << v[0] << " " << v[1] << " " << v[2] << "\n";
	}

	for (auto& i : facets) {
		if (i.outer_indices.size() < 3)
			continue;
		os << "f";
		for (auto& e : i.outer_indices) {
			os << " " << e + 1;
		}
		os << "\n";
		for (auto& es : i.inner_indices) {
			if (es.size() < 3)
				continue;
			os << "###holes### f";
			for (auto& e : es) {
				os << " " << e + 1;
			}
			os << "\n";
		}
	}

	for (int i = 0; i < points.size(); ++i) {
		double x = points[i].first->point().x().convert_to<double>();
		double y = points[i].first->point().y().convert_to<double>();
		double z = depths[i].convert_to<double>();

		Eigen::Vector3d v(x * z, y * z, z);
		//Eigen::Vector3d v(x, y, z);
		os << "v " << v[0] << " " << v[1] << " " << v[2] << "\n";
	}
	os << "### occlude boundaries\n";
	for (auto& i : facets) {
		if (i.outer_indices.size() < 3)
			continue;
		for (int j = 0; j < i.outer_indices.size(); ++j) {
			if (i.outer_type[j] == 2) {
				int next_j = (j + 1) % i.outer_indices.size();
				os << "l " << i.outer_indices[j] + 1
				   << " " << i.outer_indices[next_j] + 1 << "\n";
			}
		}
	}

	for (int i = 0; i < points.size(); ++i) {
		double x = points[i].first->point().x().convert_to<double>();
		double y = points[i].first->point().y().convert_to<double>();
		double z = depths[i].convert_to<double>();

		Eigen::Vector3d v(x * z, y * z, z);
		//Eigen::Vector3d v(x, y, z);
		os << "v " << v[0] << " " << v[1] << " " << v[2] << "\n";
	}
	os << "### sharp edges\n";
	for (auto& i : facets) {
		if (i.outer_indices.size() < 3)
			continue;
		for (int j = 0; j < i.outer_indices.size(); ++j) {
			if (i.outer_type[j] == 1) {
				int next_j = (j + 1) % i.outer_indices.size();
				os << "l " << i.outer_indices[j] + 1
				   << " " << i.outer_indices[next_j] + 1 << "\n";
			}
		}
	}
	os.close();
}

void PostProcess::SaveToSVG(const Mesh& mesh, const char* filename) {
    auto & facets = facets_;
    auto & points = points_;
    auto & face_normals = mesh.GetFaceNormals();
    auto & edge2face = edge2face_;

   	Dimensions dimensions(1000, 1000);
    Document doc(filename, Layout(dimensions, Layout::BottomLeft));

    // Red image border.
    Polygon border(Stroke(1, Color::Red));
    border << Point(0, 0) << Point(dimensions.width, 0)
        << Point(dimensions.width, dimensions.height) << Point(0, dimensions.height);
    doc << border;
    // Render polygon

	for (auto& i : facets) {
		if (i.outer_indices.size() < 3)
			continue;

	    Polygon poly(Color(128,128,128), Stroke(.0, Color(150, 160, 200)));

	    for (auto& e : i.outer_indices) {
	    	auto& p = points[e].first->point();
	    	int x = (p.x().convert_to<double>()+0.5) * 1000;
	    	int y = (p.y().convert_to<double>()+0.5) * 1000;
	    	poly << Point(x, 1000-y);
	    }
	    doc << poly;
	}

	for (int seq = 1; seq <= 2; ++seq) {
		for (auto& i : facets) {
			if (i.outer_indices.size() < 3)
				continue;
			for (int j = 0; j < i.outer_indices.size(); ++j) {
				if (i.outer_type[j] > 0 && i.outer_type[j] == seq) {
			    	Polyline poly(Stroke(2, Color::Blue));
			    	if (i.outer_type[j] == 2)
			    		poly = Polyline(Stroke(2, Color::Red));
				    {
					    auto& p1 = points[i.outer_indices[j]].first->point();
				    	int x = (p1.x().convert_to<double>()+0.5) * 1000;
				    	int y = (p1.y().convert_to<double>()+0.5) * 1000;
				    	poly << Point(x, 1000-y);
				    }
				    {
				    	int next_j = (j + 1) % i.outer_indices.size();
					    auto& p1 = points[i.outer_indices[next_j]].first->point();
				    	int x = (p1.x().convert_to<double>()+0.5) * 1000;
				    	int y = (p1.y().convert_to<double>()+0.5) * 1000;
				    	poly << Point(x, 1000-y);
				    }
				    doc << poly;
			    }
			}
		}
	}

    doc.save();
}

void PostProcess::ComputeDegree()
{
	degrees_.resize(points_.size());

	for (auto& d :degrees_)
		d = 0;
	for (auto& f : facets_) {
		for (auto& e : f.outer_indices) {
			degrees_[e] += 2;
		}
		for (auto& es : f.inner_indices) {
			for (auto& e : es) {
				degrees_[e] += 2;
			}
		}
	}
}

void PostProcess::RemoveRedundantVerticesFromLoop(std::vector<int>& indices, std::vector<int>& types) {
	auto& points = points_;
	auto& degrees = degrees_;

	std::vector<int> mask(indices.size(), 0);
	for (int i = 0; i < indices.size(); ++i) {
		if (degrees[indices[i]] > 2) {
			mask[i] = 1;
			continue;
		}
		if (types[i] != types[(i + indices.size() - 1) % indices.size()]) {
			mask[i] = 1;
			continue;
		}
		int prev_id = indices[(i + indices.size() - 1) % indices.size()];
		int next_id = indices[(i + 1) % indices.size()];

		auto& p1 = points[prev_id];
		auto& p2 = points[indices[i]];
		auto& p3 = points[next_id];

		auto diff1 = p2.first->point() - p1.first->point();
		auto diff2 = p3.first->point() - p2.first->point();

		K a = diff1.x() * diff2.y() - diff1.y() * diff2.x();
		if (a != K(0)) {
			mask[i] = 1;
		}
	}
	int top = 0;
	for (int i = 0; i < indices.size(); ++i) {
		if (mask[i] == 1) {
			indices[top] = indices[i];
			types[top] = types[i];
			top += 1;
		}
	}
	indices.resize(top);
	types.resize(top);

}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2014, 2015, 2018.
// Modifications copyright (c) 2014-2018, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is 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_GEOMETRY_UTIL_MATH_HPP
#define BOOST_GEOMETRY_UTIL_MATH_HPP

#include <cmath>
#include <limits>

#include <boost/core/ignore_unused.hpp>

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
//#include <boost/math/special_functions/round.hpp>
#include <boost/numeric/conversion/cast.hpp>
#include <boost/type_traits/is_fundamental.hpp>
#include <boost/type_traits/is_integral.hpp>

#include <boost/geometry/core/cs.hpp>

#include <boost/geometry/util/select_most_precise.hpp>

namespace boost { namespace geometry
{

namespace math
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename T>
inline T const& greatest(T const& v1, T const& v2)
{
    return (std::max)(v1, v2);
}

template <typename T>
inline T const& greatest(T const& v1, T const& v2, T const& v3)
{
    return (std::max)(greatest(v1, v2), v3);
}

template <typename T>
inline T const& greatest(T const& v1, T const& v2, T const& v3, T const& v4)
{
    return (std::max)(greatest(v1, v2, v3), v4);
}

template <typename T>
inline T const& greatest(T const& v1, T const& v2, T const& v3, T const& v4, T const& v5)
{
    return (std::max)(greatest(v1, v2, v3, v4), v5);
}

template <typename T>
inline T bounded(T const& v, T const& lower, T const& upper)
{
    return (std::min)((std::max)(v, lower), upper);
}

template <typename T>
inline T bounded(T const& v, T const& lower)
{
    return (std::max)(v, lower);
}

template <typename T,
          bool IsFloatingPoint = boost::is_floating_point<T>::value>
struct abs
{
    static inline T apply(T const& value)
    {
        T const zero = T();
        return value < zero ? -value : value;
    }
};

template <typename T>
struct abs<T, true>
{
    static inline T apply(T const& value)
    {
        using ::fabs;
        using std::fabs; // for long double

        return fabs(value);
    }
};

struct equals_default_policy
{
    template <typename T>
    static inline T apply(T const& a, T const& b)
    {
        // See http://www.parashift.com/c++-faq-lite/newbie.html#faq-29.17
        return greatest(abs<T>::apply(a), abs<T>::apply(b), T(1));
    }
};

template <typename T,
          bool IsFloatingPoint = boost::is_floating_point<T>::value>
struct equals_factor_policy
{
    equals_factor_policy()
        : factor(1) {}
    explicit equals_factor_policy(T const& v)
        : factor(greatest(abs<T>::apply(v), T(1)))
    {}
    equals_factor_policy(T const& v0, T const& v1, T const& v2, T const& v3)
        : factor(greatest(abs<T>::apply(v0), abs<T>::apply(v1),
                          abs<T>::apply(v2), abs<T>::apply(v3),
                          T(1)))
    {}

    T const& apply(T const&, T const&) const
    {
        return factor;
    }

    T factor;
};

template <typename T>
struct equals_factor_policy<T, false>
{
    equals_factor_policy() {}
    explicit equals_factor_policy(T const&) {}
    equals_factor_policy(T const& , T const& , T const& , T const& ) {}

    static inline T apply(T const&, T const&)
    {
        return T(1);
    }
};

template <typename Type,
          bool IsFloatingPoint = boost::is_floating_point<Type>::value>
struct equals
{
    template <typename Policy>
    static inline bool apply(Type const& a, Type const& b, Policy const&)
    {
        return a == b;
    }
};

template <typename Type>
struct equals<Type, true>
{
    template <typename Policy>
    static inline bool apply(Type const& a, Type const& b, Policy const& policy)
    {
        boost::ignore_unused(policy);

        if (a == b)
        {
            return true;
        }

        if (boost::math::isfinite(a) && boost::math::isfinite(b))
        {
            // If a is INF and b is e.g. 0, the expression below returns true
            // but the values are obviously not equal, hence the condition
            return abs<Type>::apply(a - b)
                <= std::numeric_limits<Type>::epsilon() * policy.apply(a, b);
        }
        else
        {
            return a == b;
        }
    }
};

template <typename T1, typename T2, typename Policy>
inline bool equals_by_policy(T1 const& a, T2 const& b, Policy const& policy)
{
    return detail::equals
        <
            typename select_most_precise<T1, T2>::type
        >::apply(a, b, policy);
}

template <typename Type,
          bool IsFloatingPoint = boost::is_floating_point<Type>::value>
struct smaller
{
    static inline bool apply(Type const& a, Type const& b)
    {
        return a < b;
    }
};

template <typename Type>
struct smaller<Type, true>
{
    static inline bool apply(Type const& a, Type const& b)
    {
        if (!(a < b)) // a >= b
        {
            return false;
        }

        return ! equals<Type, true>::apply(b, a, equals_default_policy());
    }
};

template <typename Type,
          bool IsFloatingPoint = boost::is_floating_point<Type>::value>
struct smaller_or_equals
{
    static inline bool apply(Type const& a, Type const& b)
    {
        return a <= b;
    }
};

template <typename Type>
struct smaller_or_equals<Type, true>
{
    static inline bool apply(Type const& a, Type const& b)
    {
        if (a <= b)
        {
            return true;
        }

        return equals<Type, true>::apply(a, b, equals_default_policy());
    }
};

template <typename Type,
          bool IsFloatingPoint = boost::is_floating_point<Type>::value>
struct equals_with_epsilon
    : public equals<Type, IsFloatingPoint>
{};

template
<
    typename T,
    bool IsFundemantal = boost::is_fundamental<T>::value /* false */
>
struct square_root
{
    typedef T return_type;

    static inline T apply(T const& value)
    {
        // for non-fundamental number types assume that sqrt is
        // defined either:
        // 1) at T's scope, or
        // 2) at global scope, or
        // 3) in namespace std
        using ::sqrt;
        using std::sqrt;

        return sqrt(value);
    }
};

template <typename FundamentalFP>
struct square_root_for_fundamental_fp
{
    typedef FundamentalFP return_type;

    static inline FundamentalFP apply(FundamentalFP const& value)
    {
#ifdef BOOST_GEOMETRY_SQRT_CHECK_FINITENESS
        // This is a workaround for some 32-bit platforms.
        // For some of those platforms it has been reported that
        // std::sqrt(nan) and/or std::sqrt(-nan) returns a finite value.
        // For those platforms we need to define the macro
        // BOOST_GEOMETRY_SQRT_CHECK_FINITENESS so that the argument
        // to std::sqrt is checked appropriately before passed to std::sqrt
        if (boost::math::isfinite(value))
        {
            return std::sqrt(value);
        }
        else if (boost::math::isinf(value) && value < 0)
        {
            return -std::numeric_limits<FundamentalFP>::quiet_NaN();
        }
        return value;
#else
        // for fundamental floating point numbers use std::sqrt
        return std::sqrt(value);
#endif // BOOST_GEOMETRY_SQRT_CHECK_FINITENESS
    }
};

template <>
struct square_root<float, true>
    : square_root_for_fundamental_fp<float>
{
};

template <>
struct square_root<double, true>
    : square_root_for_fundamental_fp<double>
{
};

template <>
struct square_root<long double, true>
    : square_root_for_fundamental_fp<long double>
{
};

template <typename T>
struct square_root<T, true>
{
    typedef double return_type;

    static inline double apply(T const& value)
    {
        // for all other fundamental number types use also std::sqrt
        //
        // Note: in C++98 the only other possibility is double;
        //       in C++11 there are also overloads for integral types;
        //       this specialization works for those as well.
        return square_root_for_fundamental_fp
            <
                double
            >::apply(boost::numeric_cast<double>(value));
    }
};

template
<
    typename T,
    bool IsFundemantal = boost::is_fundamental<T>::value /* false */
>
struct modulo
{
    typedef T return_type;

    static inline T apply(T const& value1, T const& value2)
    {
        // for non-fundamental number types assume that a free
        // function mod() is defined either:
        // 1) at T's scope, or
        // 2) at global scope
        return mod(value1, value2);
    }
};

template
<
    typename Fundamental,
    bool IsIntegral = boost::is_integral<Fundamental>::value
>
struct modulo_for_fundamental
{
    typedef Fundamental return_type;

    static inline Fundamental apply(Fundamental const& value1,
                                    Fundamental const& value2)
    {
        return value1 % value2;
    }
};

// specialization for floating-point numbers
template <typename Fundamental>
struct modulo_for_fundamental<Fundamental, false>
{
    typedef Fundamental return_type;

    static inline Fundamental apply(Fundamental const& value1,
                                    Fundamental const& value2)
    {
        return std::fmod(value1, value2);
    }
};

// specialization for fundamental number type
template <typename Fundamental>
struct modulo<Fundamental, true>
    : modulo_for_fundamental<Fundamental>
{};

/*!
\brief Short constructs to enable partial specialization for PI, 2*PI
       and PI/2, currently not possible in Math.
*/
template <typename T>
struct define_pi
{
    static inline T apply()
    {
        // Default calls Boost.Math
        return boost::math::constants::pi<T>();
    }
};

template <typename T>
struct define_two_pi
{
    static inline T apply()
    {
        // Default calls Boost.Math
        return boost::math::constants::two_pi<T>();
    }
};

template <typename T>
struct define_half_pi
{
    static inline T apply()
    {
        // Default calls Boost.Math
        return boost::math::constants::half_pi<T>();
    }
};

template <typename T>
struct relaxed_epsilon
{
    static inline T apply(const T& factor)
    {
        return factor * std::numeric_limits<T>::epsilon();
    }
};

// This must be consistent with math::equals.
// By default math::equals() scales the error by epsilon using the greater of
// compared values but here is only one value, though it should work the same way.
// (a-a) <= max(a, a) * EPS       -> 0 <= a*EPS
// (a+da-a) <= max(a+da, a) * EPS -> da <= (a+da)*EPS
template <typename T, bool IsFloat = boost::is_floating_point<T>::value>
struct scaled_epsilon
{
    static inline T apply(T const& val)
    {
        return (std::max)(abs<T>::apply(val), T(1))
                    * std::numeric_limits<T>::epsilon();
    }
};

template <typename T>
struct scaled_epsilon<T, false>
{
    static inline T apply(T const&)
    {
        return T(0);
    }
};

// ItoF ItoI FtoF
template <typename Result, typename Source,
          bool ResultIsInteger = std::numeric_limits<Result>::is_integer,
          bool SourceIsInteger = std::numeric_limits<Source>::is_integer>
struct rounding_cast
{
    static inline Result apply(Source const& v)
    {
        return boost::numeric_cast<Result>(v);
    }
};

// TtoT
template <typename Source, bool ResultIsInteger, bool SourceIsInteger>
struct rounding_cast<Source, Source, ResultIsInteger, SourceIsInteger>
{
    static inline Source apply(Source const& v)
    {
        return v;
    }
};

// FtoI
template <typename Result, typename Source>
struct rounding_cast<Result, Source, true, false>
{
    static inline Result apply(Source const& v)
    {
        return boost::numeric_cast<Result>(v < Source(0) ?
                                            v - Source(0.5) :
                                            v + Source(0.5));
    }
};

} // namespace detail
#endif

template <typename T>
inline T pi() { return detail::define_pi<T>::apply(); }

template <typename T>
inline T two_pi() { return detail::define_two_pi<T>::apply(); }

template <typename T>
inline T half_pi() { return detail::define_half_pi<T>::apply(); }

template <typename T>
inline T relaxed_epsilon(T const& factor)
{
    return detail::relaxed_epsilon<T>::apply(factor);
}

template <typename T>
inline T scaled_epsilon(T const& value)
{
    return detail::scaled_epsilon<T>::apply(value);
}

// Maybe replace this by boost equals or so

/*!
    \brief returns true if both arguments are equal.
    \ingroup utility
    \param a first argument
    \param b second argument
    \return true if a == b
    \note If both a and b are of an integral type, comparison is done by ==.
    If one of the types is floating point, comparison is done by abs and
    comparing with epsilon. If one of the types is non-fundamental, it might
    be a high-precision number and comparison is done using the == operator
    of that class.
*/

template <typename T1, typename T2>
inline bool equals(T1 const& a, T2 const& b)
{
    return detail::equals
        <
            typename select_most_precise<T1, T2>::type
        >::apply(a, b, detail::equals_default_policy());
}

template <typename T1, typename T2>
inline bool equals_with_epsilon(T1 const& a, T2 const& b)
{
    return detail::equals_with_epsilon
        <
            typename select_most_precise<T1, T2>::type
        >::apply(a, b, detail::equals_default_policy());
}

template <typename T1, typename T2>
inline bool smaller(T1 const& a, T2 const& b)
{
    return detail::smaller
        <
            typename select_most_precise<T1, T2>::type
        >::apply(a, b);
}

template <typename T1, typename T2>
inline bool larger(T1 const& a, T2 const& b)
{
    return detail::smaller
        <
            typename select_most_precise<T1, T2>::type
        >::apply(b, a);
}

template <typename T1, typename T2>
inline bool smaller_or_equals(T1 const& a, T2 const& b)
{
    return detail::smaller_or_equals
        <
            typename select_most_precise<T1, T2>::type
        >::apply(a, b);
}

template <typename T1, typename T2>
inline bool larger_or_equals(T1 const& a, T2 const& b)
{
    return detail::smaller_or_equals
        <
            typename select_most_precise<T1, T2>::type
        >::apply(b, a);
}

template <typename T>
inline T d2r()
{
    static T const conversion_coefficient = geometry::math::pi<T>() / T(180.0);
    return conversion_coefficient;
}

template <typename T>
inline T r2d()
{
    static T const conversion_coefficient = T(180.0) / geometry::math::pi<T>();
    return conversion_coefficient;
}

#ifndef DOXYGEN_NO_DETAIL
namespace detail {

template <typename DegreeOrRadian>
struct as_radian
{
    template <typename T>
    static inline T apply(T const& value)
    {
        return value;
    }
};

template <>
struct as_radian<degree>
{
    template <typename T>
    static inline T apply(T const& value)
    {
        return value * d2r<T>();
    }
};

template <typename DegreeOrRadian>
struct from_radian
{
    template <typename T>
    static inline T apply(T const& value)
    {
        return value;
    }
};

template <>
struct from_radian<degree>
{
    template <typename T>
    static inline T apply(T const& value)
    {
        return value * r2d<T>();
    }
};

} // namespace detail
#endif

template <typename DegreeOrRadian, typename T>
inline T as_radian(T const& value)
{
    return detail::as_radian<DegreeOrRadian>::apply(value);
}

template <typename DegreeOrRadian, typename T>
inline T from_radian(T const& value)
{
    return detail::from_radian<DegreeOrRadian>::apply(value);
}

/*!
    \brief Calculates the haversine of an angle
    \ingroup utility
    \note See http://en.wikipedia.org/wiki/Haversine_formula
    haversin(alpha) = sin2(alpha/2)
*/
template <typename T>
inline T hav(T const& theta)
{
    T const half = T(0.5);
    T const sn = sin(half * theta);
    return sn * sn;
}

/*!
\brief Short utility to return the square
\ingroup utility
\param value Value to calculate the square from
\return The squared value
*/
template <typename T>
inline T sqr(T const& value)
{
    return value * value;
}

/*!
\brief Short utility to return the square root
\ingroup utility
\param value Value to calculate the square root from
\return The square root value
*/
template <typename T>
inline typename detail::square_root<T>::return_type
sqrt(T const& value)
{
    return detail::square_root
        <
            T, boost::is_fundamental<T>::value
        >::apply(value);
}

/*!
\brief Short utility to return the modulo of two values
\ingroup utility
\param value1 First value
\param value2 Second value
\return The result of the modulo operation on the (ordered) pair
(value1, value2)
*/
template <typename T>
inline typename detail::modulo<T>::return_type
mod(T const& value1, T const& value2)
{
    return detail::modulo
        <
            T, boost::is_fundamental<T>::value
        >::apply(value1, value2);
}

/*!
\brief Short utility to workaround gcc/clang problem that abs is converting to integer
       and that older versions of MSVC does not support abs of long long...
\ingroup utility
*/
template<typename T>
inline T abs(T const& value)
{
    return detail::abs<T>::apply(value);
}

/*!
\brief Short utility to calculate the sign of a number: -1 (negative), 0 (zero), 1 (positive)
\ingroup utility
*/
template <typename T>
inline int sign(T const& value)
{
    T const zero = T();
    return value > zero ? 1 : value < zero ? -1 : 0;
}

/*!
\brief Short utility to cast a value possibly rounding it to the nearest
       integral value.
\ingroup utility
\note If the source T is NOT an integral type and Result is an integral type
      the value is rounded towards the closest integral value. Otherwise it's
      casted without rounding.
*/
template <typename Result, typename T>
inline Result rounding_cast(T const& v)
{
    return detail::rounding_cast<Result, T>::apply(v);
}

/*!
\brief Short utility to calculate the power
\ingroup utility
*/
template <typename T1, typename T2>
inline T1 pow(T1 const& a, T2 const& b)
{
    using std::pow;
    return pow(a, b);
}

} // namespace math

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_UTIL_MATH_HPP


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2016 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2014, 2016.
// Modifications copyright (c) 2014-2016 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is 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_GEOMETRY_STRATEGIES_GEOGRAPHIC_ANDOYER_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_ANDOYER_HPP

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/srs.hpp>

#include <boost/geometry/algorithms/detail/andoyer_inverse.hpp>
#include <boost/geometry/algorithms/detail/flattening.hpp>

#include <boost/geometry/strategies/distance.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

/*!
\brief Point-point distance approximation taking flattening into account
\ingroup distance
\tparam Spheroid The reference spheroid model
\tparam CalculationType \tparam_calculation
\author After Andoyer, 19xx, republished 1950, republished by Meeus, 1999
\note Although not so well-known, the approximation is very good: in all cases the results
are about the same as Vincenty. In my (Barend's) testcases the results didn't differ more than 6 m
\see http://nacc.upc.es/tierra/node16.html
\see http://sci.tech-archive.net/Archive/sci.geo.satellite-nav/2004-12/2724.html
\see http://home.att.net/~srschmitt/great_circle_route.html (implementation)
\see http://www.codeguru.com/Cpp/Cpp/algorithms/article.php/c5115 (implementation)
\see http://futureboy.homeip.net/frinksamp/navigation.frink (implementation)
\see http://www.voidware.com/earthdist.htm (implementation)
\see http://www.dtic.mil/docs/citations/AD0627893
\see http://www.dtic.mil/docs/citations/AD703541
*/
template
<
    typename Spheroid,
    typename CalculationType = void
>
class andoyer
{
public :
    template <typename Point1, typename Point2>
    struct calculation_type
        : promote_floating_point
          <
              typename select_calculation_type
                  <
                      Point1,
                      Point2,
                      CalculationType
                  >::type
          >
    {};

    typedef Spheroid model_type;

    inline andoyer()
        : m_spheroid()
    {}

    explicit inline andoyer(Spheroid const& spheroid)
        : m_spheroid(spheroid)
    {}

    template <typename Point1, typename Point2>
    inline typename calculation_type<Point1, Point2>::type
    apply(Point1 const& point1, Point2 const& point2) const
    {
        return geometry::detail::andoyer_inverse
            <
                typename calculation_type<Point1, Point2>::type,
                true, false
            >::apply(get_as_radian<0>(point1), get_as_radian<1>(point1),
                     get_as_radian<0>(point2), get_as_radian<1>(point2),
                     m_spheroid).distance;
    }

    inline Spheroid const& model() const
    {
        return m_spheroid;
    }

private :
    Spheroid m_spheroid;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename Spheroid, typename CalculationType>
struct tag<andoyer<Spheroid, CalculationType> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename Spheroid, typename CalculationType, typename P1, typename P2>
struct return_type<andoyer<Spheroid, CalculationType>, P1, P2>
    : andoyer<Spheroid, CalculationType>::template calculation_type<P1, P2>
{};

template <typename Spheroid, typename CalculationType>
struct comparable_type<andoyer<Spheroid, CalculationType> >
{
    typedef andoyer<Spheroid, CalculationType> type;
};

template <typename Spheroid, typename CalculationType>
struct get_comparable<andoyer<Spheroid, CalculationType> >
{
    static inline andoyer<Spheroid, CalculationType> apply(andoyer<Spheroid, CalculationType> const& input)
    {
        return input;
    }
};

template <typename Spheroid, typename CalculationType, typename P1, typename P2>
struct result_from_distance<andoyer<Spheroid, CalculationType>, P1, P2>
{
    template <typename T>
    static inline typename return_type<andoyer<Spheroid, CalculationType>, P1, P2>::type
        apply(andoyer<Spheroid, CalculationType> const& , T const& value)
    {
        return value;
    }
};

template <typename Point1, typename Point2>
struct default_strategy<point_tag, point_tag, Point1, Point2, geographic_tag, geographic_tag>
{
    typedef strategy::distance::andoyer
                <
                    srs::spheroid
                        <
                            typename select_coordinate_type<Point1, Point2>::type
                        >
                > type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_ANDOYER_HPP