// Copyright (c) 2019, Torsten Sattler
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//
//     * Neither the name of the copyright holder nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// author: Torsten Sattler, torsten.sattler.de@googlemail.com

#include <iostream>

#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <Eigen/StdVector>

#include "calibrated_absolute_pose_estimator.h"

namespace ransac_lib {

namespace calibrated_absolute_pose {

struct NormalizedReprojectionError {
  NormalizedReprojectionError(double x, double y, double X, double Y, double Z,
                              double fx, double fy)
      : point2D_x(x),
        point2D_y(y),
        point3D_X(X),
        point3D_Y(Y),
        point3D_Z(Z),
        f_x(fx),
        f_y(fy) {}

  template <typename T>
  bool operator()(const T* const camera, T* residuals) const {
    // The last three entries are the camera position.
    T p[3];
    p[0] = point3D_X - camera[3];
    p[1] = point3D_Y - camera[4];
    p[2] = point3D_Z - camera[5];

    // The first three entries correspond to the rotation matrix stored in an
    // angle-axis representation.
    T p_rot[3];
    ceres::AngleAxisRotatePoint(camera, p, p_rot);

    T x_proj = static_cast<T>(f_x) * p_rot[0] / p_rot[2];
    T y_proj = static_cast<T>(f_y) * p_rot[1] / p_rot[2];

    residuals[0] = static_cast<T>(point2D_x) - x_proj;
    residuals[1] = static_cast<T>(point2D_y) - y_proj;

    return true;
  }

  // Factory function
  static ceres::CostFunction* CreateCost(const double x, const double y,
                                         const double X, const double Y,
                                         const double Z, const double fx,
                                         const double fy) {
    return (new ceres::AutoDiffCostFunction<NormalizedReprojectionError, 2, 6>(
        new NormalizedReprojectionError(x, y, X, Y, Z, fx, fy)));
  }

  // Assumes that the measurement is centered around the principal point.
  // This camera model does not take any radial distortion into account. If
  // radial distortion is present, one should undistort the measurements first.
  double point2D_x;
  double point2D_y;
  // The 3D point position is fixed as we are only interested in refining the
  // camera parameters.
  double point3D_X;
  double point3D_Y;
  double point3D_Z;
  double f_x;
  double f_y;
};

CalibratedAbsolutePoseEstimator::CalibratedAbsolutePoseEstimator(
    const double f_x, const double f_y, const double squared_inlier_threshold,
    const Points2D& points2D, const ViewingRays& rays, const Points3D& points3D)
    : focal_x_(f_x),
      focal_y_(f_y),
      squared_inlier_threshold_(squared_inlier_threshold),
      points2D_(points2D),
      points3D_(points3D),
      adapter_(rays, points3D) {
  num_data_ = static_cast<int>(points2D_.size());
}

int CalibratedAbsolutePoseEstimator::MinimalSolver(
    const std::vector<int>& sample, CameraPoses* poses) const {
  poses->clear();
  CameraPoses p3p_poses = opengv::absolute_pose::p3p_kneip(adapter_, sample);
  if (p3p_poses.empty()) return 0;
  for (const CameraPose& pose : p3p_poses) {
    CameraPose P = pose;
    // OpenGV returns the transformation from the camera to the world coordinate
    // system. We store the rotation from the world to the local coordinate
    // system instead.
    P.topLeftCorner<3, 3>() = pose.topLeftCorner<3, 3>().transpose();
    const double kError = EvaluateModelOnPoint(P, sample[3]);
    if (kError < squared_inlier_threshold_) {
      //      // Refine using all four points.
      //      LeastSquares(sample, &P);
      poses->push_back(P);
      // At most one pose should be correct.
      break;
    }
  }

  return static_cast<int>(poses->size());
}

// Returns 0 if no model could be estimated and 1 otherwise.
// Implemented by a simple linear least squares solver.
int CalibratedAbsolutePoseEstimator::NonMinimalSolver(
    const std::vector<int>& sample, CameraPose* pose) const {
  // Alternative: Run minimal solver and polish.
  CameraPoses poses;
  if (MinimalSolver(sample, &poses) == 1) {
    *pose = poses[0];
    LeastSquares(sample, pose);
    return 1;
  } else {
    return 0;
  }

//   CameraPose P = opengv::absolute_pose::epnp(adapter_, sample);
//   // OpenGV returns the transformation from the camera to the world coordinate
//   // system. We store the rotation from the world to the local coordinate
//   // system instead.
//   *pose = P;
//   pose->topLeftCorner<3, 3>() = P.topLeftCorner<3, 3>().transpose();
//   return 1;
}

// Evaluates the pose on the i-th data point.
double CalibratedAbsolutePoseEstimator::EvaluateModelOnPoint(
    const CameraPose& pose, int i) const {
  Eigen::Vector3d p_c =
      pose.topLeftCorner<3, 3>() * (points3D_[i] - pose.col(3));

  // Check whether point projects behind the camera.
  if (p_c[2] < 0.0) return std::numeric_limits<double>::max();

  Eigen::Vector2d p_2d = p_c.head<2>() / p_c[2];
  p_2d[0] *= focal_x_;
  p_2d[1] *= focal_y_;

  return (p_2d - points2D_[i]).squaredNorm();
}

// Reference implementation using Ceres for refinement.
void CalibratedAbsolutePoseEstimator::LeastSquares(
    const std::vector<int>& sample, CameraPose* pose) const {
  Eigen::AngleAxisd aax(pose->topLeftCorner<3, 3>());
  Eigen::Vector3d aax_vec = aax.axis() * aax.angle();
  double camera[6];
  camera[0] = aax_vec[0];
  camera[1] = aax_vec[1];
  camera[2] = aax_vec[2];
  camera[3] = pose->col(3)[0];
  camera[4] = pose->col(3)[1];
  camera[5] = pose->col(3)[2];

  ceres::Problem refinement_problem;
//  ceres::LossFunction* cauchy_loss = new ceres::CauchyLoss(1.0);
  const int kSampleSize = static_cast<int>(sample.size());
  for (int i = 0; i < kSampleSize; ++i) {
    const int kIdx = sample[i];
    const Eigen::Vector2d& p_img = points2D_[kIdx];
    const Eigen::Vector3d& p_3D = points3D_[kIdx];
    ceres::CostFunction* cost_function =
        NormalizedReprojectionError::CreateCost(
            p_img[0], p_img[1], p_3D[0], p_3D[1], p_3D[2], focal_x_, focal_y_);
//     double error_i = std::sqrt(EvaluateModelOnPoint(*pose, sample[i]));
//     error_i = std::max(0.00001, error_i);
    refinement_problem.AddResidualBlock(cost_function, nullptr, camera);
//    refinement_problem.AddResidualBlock(cost_function, cauchy_loss, camera);
//     refinement_problem.AddResidualBlock(cost_function,
//                                         new ceres::ScaledLoss(nullptr,
//                                                               1.0 / error_i,
//                                                               ceres::DO_NOT_TAKE_OWNERSHIP),
//                                         camera);
  }

  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = false;
  //  options.function_tolerance = 0.000001;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &refinement_problem, &summary);

  //  std::cout << summary.BriefReport() << std::endl;
//  delete cauchy_loss;
//  cauchy_loss = nullptr;

  if (summary.IsSolutionUsable()) {
    Eigen::Vector3d axis(camera[0], camera[1], camera[2]);
    double angle = axis.norm();
    axis.normalize();
    aax.axis() = axis;
    aax.angle() = angle;

    pose->topLeftCorner<3, 3>() = aax.toRotationMatrix();
    pose->col(3) = Eigen::Vector3d(camera[3], camera[4], camera[5]);
  }
}

//// Reference implementation using OpenGV's non-linear refinement.
// void CalibratedAbsolutePoseEstimator::LeastSquares(
//    const std::vector<int>& sample, CameraPose* pose) const {
//  // OpenGV returns the transformation from the camera to the world coordinate
//  // system. We store the rotation from the world to the local coordinate
//  // system instead.
//  Eigen::Matrix3d R = pose->topLeftCorner<3, 3>().transpose();
//  Eigen::Vector3d c = pose->col(3);
//  // At the moment, we need to copy data as we need to add the current pose
//  // estimate to the adapter, which would break the requirement that this
//  // function is constant.
//  const int kSampleSize = static_cast<int>(sample.size());
//  opengv::bearingVectors_t bearing_vectors(kSampleSize);
//  opengv::points_t points(kSampleSize);
//  for (int i = 0; i < kSampleSize; ++i) {
//    const int kIdx = sample[i];
//    bearing_vectors[i] = adapter_.getBearingVector(kIdx);
//    points[i] = adapter_.getPoint(kIdx);
//  }
//
//  opengv::absolute_pose::CentralAbsoluteAdapter lsq_adapter(bearing_vectors,
//                                                            points, c, R);
//
//  CameraPose P = opengv::absolute_pose::optimize_nonlinear(lsq_adapter);
//  *pose = P;
//  pose->topLeftCorner<3, 3>() = P.topLeftCorner<3, 3>().transpose();
//}

void CalibratedAbsolutePoseEstimator::PixelsToViewingRays(
    const double focal_x, const double focal_y, const Points2D& points2D,
    ViewingRays* rays) {
  const int kNumData = static_cast<int>(points2D.size());

  // Creates the bearing vectors and points for the OpenGV adapter.
  rays->resize(kNumData);
  for (int i = 0; i < kNumData; ++i) {
    (*rays)[i] = points2D[i].homogeneous();
    (*rays)[i][0] /= focal_x;
    (*rays)[i][1] /= focal_y;
    (*rays)[i].normalize();
  }
}
}  // namespace calibrated_absolute_pose

}  // namespace ransac_lib


#include <frovedis.hpp>
#include <frovedis/matrix/pblas_wrapper.hpp>

#define BOOST_TEST_MODULE FrovedisTest
#include <boost/test/unit_test.hpp>
#include "../../rmse.hpp"

using namespace frovedis;
using namespace std;

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    // creating blockcyclic matrix from file
    auto bm1 = make_blockcyclic_matrix_load<float> ("./sample_4x4");
    auto bm2 = bm1;

    // slicing rows and cols
    auto row1 = make_row_vector<float> (bm1,0);
    auto row2 = make_row_vector<float> (bm1,1);
    auto col1 = make_col_vector<float> (bm1,0);
    auto col2 = make_col_vector<float> (bm1,1);

    // updating row2 and col2 of bm1, using gemv (matrix-vector multiplication)
    gemv<float>(bm2,row1,row2); // row2-of-bm1 = bm2 * row1-of-bm1
    gemv<float>(bm2,col1,col2); // col2-of-bm1 = bm2 * col1-of-bm1
    bm1.save("./out");

    double tol = 0.01;
    auto out = make_rowmajor_matrix_local_load<float> ("./out");
    auto ref = make_rowmajor_matrix_local_load<float> ("./ref_4x4");
    BOOST_CHECK (calc_rms_err<float> (out.val, ref.val) < tol);
    system("rm -f ./out");
}


#include <rubbishrsa/keys.hpp>

#include <rubbishrsa/log.hpp>

#include <boost/property_tree/json_parser.hpp>

#include <boost/multiprecision/miller_rabin.hpp>

namespace rubbishrsa {
  private_key private_key::from_factors(const bigint& p, const bigint& q, bigint e) {
    // We can now start filling in our result
    private_key ret;
    ret.n = p * q;
    // This is automatically done
    ret.e = e;

    auto lambda_n = carmichael_semiprime(p, q);
    ret.d = modinv(ret.e, lambda_n); // $d \equiv e^{-1} \pmod{\lambda(n)}$

    return ret;
  }

  private_key private_key::generate(uint_fast16_t bits) {
    // Apparently we should differ in lengths by a few digits
    // this will differ in length by log10(2^8) = ~3 digits
    auto p = generate_prime(bits / 2 + 4);
    auto q = generate_prime(bits / 2 - 3);

    RUBBISHRSA_LOG_INFO(std::cerr << "(p, q) = (" << p.str() << ", " << q.str() << ')' << std::endl);

    // Now we have a good p and q, we can pass it along
    return private_key::from_factors(p, q);
  }

  void public_key::serialise(std::ostream& os) const {
    boost::property_tree::ptree data;
    data.put("e", e);
    data.put("n", n);
    boost::property_tree::write_json(os, data, false);
  }

  void private_key::serialise(std::ostream& os) const {
    boost::property_tree::ptree data;
    data.put("e", e);
    data.put("d", d);
    data.put("n", n);
    boost::property_tree::write_json(os, data, false);
  }

  public_key public_key::deserialise(std::istream& is) {
    boost::property_tree::ptree data;
    boost::property_tree::read_json(is, data);
    public_key ret;

    ret.e = data.get<bigint>("e");
    ret.n = data.get<bigint>("n");

    return ret;
  }

  private_key private_key::deserialise(std::istream& is) {
    boost::property_tree::ptree data;
    boost::property_tree::read_json(is, data);
    private_key ret;

    ret.e = data.get<bigint>("e");
    ret.d = data.get<bigint>("d");
    ret.n = data.get<bigint>("n");

    return ret;
  }
}


#include <boost/numeric/ublas/matrix.hpp>
#include <cmath>
#include <iostream>
#include <vector>

struct Point
{
public:
		Point(int x, int y)
		{
				this->x = x;
				this->y = y;
		}

		int x;
		int y;
};

struct AStarNode
{
public:
		AStarNode() {}
		AStarNode(const AStarNode& p, const Point& pos)
		{
				if (&parent != nullptr)
						parent = std::make_shared<AStarNode>(p);

				if (&position != nullptr)
						position = std::make_shared<Point>(pos);
		}

		bool eq(const AStarNode& x)
		{
				if (&x == nullptr) return false;

				return (position->x == x.position->x &&
						position->y == x.position->y);
		}

		std::shared_ptr<AStarNode> parent{};
		std::shared_ptr<Point> position{};
		int f = 0;
		int g = 0;
		int h = 0;
};

class AStarPathFinder
{
public:
		AStarPathFinder(boost::numeric::ublas::matrix<int> map)
		{
				this->map = std::make_shared<boost::numeric::ublas::matrix<int>>(map);
		}

		std::shared_ptr<std::vector<Point>> FindPath(Point start, Point end);

private:
		std::shared_ptr<boost::numeric::ublas::matrix<int>> map;
};

std::shared_ptr<std::vector<Point>> AStarPathFinder::FindPath(Point start, Point end)
{
		auto path = std::make_shared<std::vector<Point>>();

		AStarNode none{};

		auto start_node = std::make_shared<AStarNode>(none, start);
		auto end_node = std::make_shared<AStarNode>(none, end);
		auto open_list = std::make_shared<std::vector<std::shared_ptr<AStarNode>>>();
		auto closed_list = std::make_shared<std::vector<std::shared_ptr<AStarNode>>>();

		Point pos_array[4] = {
				{ 0, -1 },
				{ 0, 1 },
				{ -1, 0 },
				{ 1, 0 },
		};

		open_list->emplace_back(start_node);

		while (open_list->size() > 0)
		{
				auto current_node = open_list->front();
				int current_index = 0;

				int _index = 0;
				for (const auto& item : *open_list)
				{
						if (item->f < current_node->f)
						{
								current_node = item;
								current_index = _index;
						}
						_index++;
				}

				open_list->erase(open_list->begin() + current_index);
				closed_list->emplace_back(current_node);

				if (current_node->eq(*end_node))
				{
						auto current = current_node;
						while (current != nullptr && current->position != nullptr)
						{
								Point path_point = { current->position->x, current->position->y };
								path->emplace_back(path_point);
								current = current->parent;
						}

						std::reverse(path->begin(), path->end());
						return path;
				}

				auto children = std::make_shared<std::vector<std::shared_ptr<AStarNode>>>();

				for (const auto& new_position : pos_array)
				{
						auto node_position = std::make_shared<Point>(current_node->position->x + new_position.x, current_node->position->y + new_position.y);

						if (node_position->x > (map->size2() - 1) || node_position->x < 0 ||
								node_position->y >(map->size1() - 1) || node_position->y < 0) continue;

						if ((*map)(node_position->y, node_position->x) != 0) continue;

						auto child = std::make_shared<AStarNode>(*current_node, *node_position);
						children->emplace_back(child);
				}

				for (const auto& child : *children)
				{
						auto closed_list_result = std::find_if(closed_list->begin(), closed_list->end(),
								[&](const std::shared_ptr<AStarNode>& c) {
										return c->eq(*child);
								});

						if (closed_list_result != closed_list->end() && *closed_list_result != nullptr)
								continue;

						child->g = current_node->g + 1;
						child->h = (int)pow(child->position->x - end_node->position->x, 2) + (int)pow(child->position->y - end_node->position->y, 2);
						child->f = child->g + child->h;

						auto open_node_result = std::find_if(open_list->begin(), open_list->end(),
								[&](const std::shared_ptr<AStarNode>& o) {
										return child->eq(*o) && child->g > o->g;
								});

						if (open_node_result != open_list->end() && *open_node_result != nullptr)
								continue;

						open_list->emplace_back(child);
				}
		}

		return nullptr;
}

int main()
{
		int map[10][10] = {
				{ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 },
				{ 0, 0, 0, 0, 1, 0, 0, 1, 0, 0 },
				{ 0, 0, 0, 0, 1, 0, 0, 1, 0, 0 },
				{ 0, 1, 1, 1, 1, 1, 1, 1, 0, 0 },
				{ 0, 1, 0, 0, 0, 0, 0, 0, 0, 0 },
				{ 0, 1, 0, 0, 1, 0, 0, 0, 0, 0 },
				{ 0, 1, 0, 0, 1, 0, 0, 0, 0, 0 },
				{ 0, 1, 0, 0, 1, 0, 0, 0, 0, 0 },
				{ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 },
				{ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }
		};

		auto _map = std::make_shared<boost::numeric::ublas::matrix<int>>(10,10);

		// convert map to a boost matrix
		for (int r = 0; r < 10; ++r)
		{
				for (int c = 0; c < 10; ++c)
				{
						(*_map)(r, c) = map[r][c];
				}
		}

		auto astar = std::make_shared<AStarPathFinder>(*_map);
		auto path = astar->FindPath({ 1, 0 }, { 6, 2 });

		std::cout << "path steps: " << path->size() << std::endl;

		for (int r = 0; r < 10; ++r)
		{
				for (int c = 0; c < 10; ++c)
				{
						auto p = std::find_if(path->begin(), path->end(),
								[&](const Point& s) {
										return s.x == c && s.y == r;
								});

						if (p != path->end()) std::cout << "+";
						else if ((*_map)(r, c) == 0) std::cout << ".";
						else if ((*_map)(r, c) == 1) std::cout << "#";
				}

				std::cout << std::endl;
		}

		//for (const auto& step : *path)
		//{
		//		std::cout << "(" << step.x << ", " << step.y << ")" << std::endl;
		//}

		return 0;
}


#include <maya/MVector.h>
#include <maya/MFnTypedAttribute.h>
#include <maya/MFnNumericAttribute.h>
#include <maya/MFnPlugin.h>
#include <maya/MPointArray.h>
#include <maya/MFnMesh.h>
#include <maya/MMatrix.h>
#include <maya/MItGeometry.h>
#include <maya/MDagPath.h>

#include <numeric>
#include <cmath>
#include <vector>
#include <array>

#include "tbb/tick_count.h"
#include "tbb/parallel_for.h"
#include "tbb/task_scheduler_init.h"

#include <Eigen/Dense>
#include <Eigen/StdVector>

#include "green_cage_deformer.h"

const MTypeId GreenCageDeformer::s_node_id(0xBBBB9255);
const MString GreenCageDeformer::s_node_name("green_cage_deformer");
MObject GreenCageDeformer::s_cage_mesh;

namespace {
	const Eigen::Vector3d NULL_VECTOR(0.0, 0.0, 0);
	constexpr auto TOLERANCE = 1e-6;
}

namespace MathConstants {
	constexpr auto PI = 3.14159265358979323846;
	constexpr auto ONE_OVER_FOUR_PI = 0.07957747154594767;
	constexpr auto SQRT8 = 2.828427124746190097603;
};

const double GCTriInt(const Eigen::Vector3d& p, const Eigen::Vector3d& v1, const Eigen::Vector3d& v2)
{
	const auto v2_v1 = v2 - v1;
	const auto p_v1 = p - v1;
	const auto v1_p = v1 - p;
	const auto v2_p = v2 - p;

	const auto alpha = std::acos(std::min(std::max(((v2_v1).dot((p_v1))) / ((v2_v1).norm() * (p_v1).norm()), -1.0), 1.0));
	if (abs(alpha - MathConstants::PI) < TOLERANCE || abs(alpha) < TOLERANCE) {
		return 0.0;
	}

	const auto beta = std::acos(std::min(std::max((((v1_p).dot((v2_p)))) / ((v1_p).norm() * (v2_p).norm()), -1.0), 1.0));
	const auto lambda = p_v1.squaredNorm() * std::sin(alpha) * std::sin(alpha);
	const auto c = p.squaredNorm();

	const auto sqrt_c = sqrt(c);
	const auto sqrt_lambda = sqrt(lambda);

	const std::array<double, 2> theta{ MathConstants::PI - alpha, MathConstants::PI - alpha - beta };
	std::array<double, 2> I;

	for (size_t i = 0; i < 2; ++i)
	{
		const auto S = std::sin(theta[i]);
		const auto C = std::cos(theta[i]);
		const auto sign = S < 0 ? -1.0 : 1.0;

		const auto SS = S*S;
		const auto half_sign = (-sign * 0.5);
		const auto tan_part = (2 * sqrt_c * std::atan2((sqrt_c * C), sqrt(lambda + (SS * c))));
		const auto log_part = log(((2 * sqrt_lambda * SS) / std::pow(1.0 - C, 2)) * (1.0 - ((2 * c * C) / ((c*(1 + C) + lambda + sqrt((lambda * lambda) + (lambda * c * SS)))))));

		I[i] = half_sign * (tan_part + (sqrt_lambda * log_part));
	}

	return -MathConstants::ONE_OVER_FOUR_PI * abs(I[0] - I[1] - sqrt_c * beta);
}

void GCDeformData::setup_cage() {
	MStatus stat;
	MFnMesh fn_mesh(m_cage_object);
	MIntArray m_tri_counts;
	fn_mesh.getTriangles(m_tri_counts, m_tri_verts);

	m_nr_of_tris = 0;
	for (unsigned int i = 0; i < m_tri_counts.length(); ++i)
	{
		m_nr_of_tris += m_tri_counts[i];
	}

	MPointArray cage_points;
	fn_mesh.getPoints(cage_points);
	m_nr_of_verts = fn_mesh.numVertices();
	m_vertices.resize(m_nr_of_verts);

	for (unsigned int i = 0; i < cage_points.length(); ++i) {
		const auto point = cage_points[i];
		m_vertices[i] = Eigen::Vector3d(point.x, point.y, point.z);
	}

	m_scale_factor.resize(m_nr_of_tris);
	m_tri_areas.resize(m_nr_of_tris);
	m_tri_normals.resize(m_nr_of_tris);
	m_tri_points.resize(m_nr_of_tris);
	m_tri_edges.resize(m_nr_of_tris);

	for (unsigned int j = 0; j < m_nr_of_tris; ++j) {
		const auto tri_idx = j * 3;
		const auto v0 = m_vertices[m_tri_verts[tri_idx + 0]];
		const auto v1 = m_vertices[m_tri_verts[tri_idx + 1]];
		const auto v2 = m_vertices[m_tri_verts[tri_idx + 2]];
		m_tri_points[j][0] = v0;
		m_tri_points[j][1] = v1;
		m_tri_points[j][2] = v2;
		// ccw order
		const Eigen::Vector3d edge1{ v1 - v0 };
		const Eigen::Vector3d edge2{ v2 - v1 };
		const Eigen::Vector3d cross = edge1.cross(edge2);
		m_tri_areas[j] = cross.norm()*0.5;
		m_tri_normals[j] = cross.normalized();
		m_tri_edges[j] = std::make_pair(edge1, edge2);
	}

}

// variable names based on pseudocode
void GCDeformData::calc_green_coords(MItGeometry& iter, const MMatrix& mat)
{
	MPointArray target_vertices;
	iter.allPositions(target_vertices);
	m_nr_of_targetverts = iter.exactCount();
	m_psi.resize(m_nr_of_tris*m_nr_of_targetverts, 0.0);
	m_phi.resize(m_nr_of_verts*m_nr_of_targetverts, 0.0);

	tbb::parallel_for(tbb::blocked_range<unsigned int>(0, m_nr_of_targetverts), [&](const tbb::blocked_range<unsigned int>& r) {
		for (unsigned int idx = r.begin(); idx < r.end(); ++idx) {
			const auto maya_vec = target_vertices[idx] * mat;
			const Eigen::Vector3d pvec(maya_vec.x, maya_vec.y, maya_vec.z);
			Eigen::Vector3d s;
			Eigen::Vector3d I;
			std::array<double, 3> II;
			std::array<Eigen::Vector3d, 3> N;
			for (unsigned int i = 0; i < m_nr_of_tris; ++i) {
				const auto nrm = m_tri_normals[i];
				std::array<Eigen::Vector3d, 3> vj;
				for (size_t l = 0; l < 3; ++l) {
					vj[l] = m_tri_points[i][l] - pvec;
				}
				const auto p = nrm * (vj[0].dot(nrm));
				for (size_t k = 0; k < 3; ++k) {
					const auto v0 = vj[k];
					const auto v1 = vj[(k + 1) % 3];
					const auto lg = ((v0 - p).cross((v1 - p))).dot(nrm);
					s[k] = lg < 0 ? -1.0 : 1.0;
					// eta always zero(?)
					I[k] = GCTriInt(p, v0, v1);
					II[k] = GCTriInt(NULL_VECTOR, v1, v0);
					N[k] = (v1.cross(v0)).normalized();
				}

				const auto I_ = -abs(s.dot(I));

				m_psi[(idx*m_nr_of_tris) + i] = -I_;

				Eigen::Vector3d w = I_ * nrm;
				for (int k = 0; k < 3; k++)
					w += (II[k] * N[k]);

				if (w.norm() > DBL_EPSILON) {
					for (unsigned int l = 0; l < 3; l++)
					{
						const auto l1 = (l + 1) % 3;
						const auto a1 = N[l1];
						const auto a2 = w;
						const auto a3 = vj[l];
						const auto val = ((a1.dot(a2)) / (a1.dot(a3)));
						m_phi[m_nr_of_verts*idx + m_tri_verts[(i * 3) + l]] += val;
					}
				}
			}
		}
	});
}

const MPoint GCDeformData::calc_pos(unsigned int idx) const
{
	Eigen::Vector3d pnt(0.0, 0.0, 0.0);
	const auto current_tri = idx*m_nr_of_tris;
	const auto current_vert = idx*m_nr_of_verts;

	for (unsigned int i = 0; i < m_nr_of_verts; ++i) {
		pnt += m_phi[current_vert + i] * m_vertices[i];
	}

	for (unsigned int i = 0; i < m_nr_of_tris; ++i) {

		pnt += m_psi[current_tri + i] * m_scale_factor[i] * m_tri_normals[i];
	}

	return MPoint(pnt.x(), pnt.y(), pnt.z());
}

void GCDeformData::calc_scale_factor()
{

	for (unsigned int i = 0; i < m_nr_of_tris; ++i) {
		const auto rest_edges = m_original_edges[i];
		const auto current_edges = m_tri_edges[i];

		const auto u0 = rest_edges.first;
		const auto v0 = rest_edges.second;

		const auto u1 = current_edges.first;
		const auto v1 = current_edges.second;

		m_scale_factor[i] = sqrt((u1.squaredNorm()) * (v0.squaredNorm()) - 2.0 * (u1.dot(v1)) * (u0.dot(v0)) + (v1.squaredNorm()) * (u0.squaredNorm())) / (MathConstants::SQRT8 * m_original_areas[i]);

	}
}

void GCDeformData::save_original_cage_data()
{
	m_original_areas.reserve(m_nr_of_tris);
	m_original_areas = m_tri_areas;

	m_original_edges.reserve(m_nr_of_tris);
	m_original_edges = m_tri_edges;
}

MStatus GreenCageDeformer::deform(MDataBlock& block, MItGeometry& iter, const MMatrix& mat, unsigned int multiIndex)
{
	MStatus stat;

	// not yet supported
	if (multiIndex > 0)
		return stat;

	if (!m_cage_connected)
		return stat;

	const auto envelope_val = block.inputValue(envelope).asDouble();
	if (envelope_val <= 0.0)
		return stat;

	const auto h_cage = block.inputValue(s_cage_mesh);

	// since we are getting the worldMesh lets not fiddle around with local to world transformations
	const auto cage_object = h_cage.asMeshTransformed();

	p_cage_data->m_cage_object = cage_object;
	p_cage_data->setup_cage();

	if (!m_green_calculated) {
		MArrayDataHandle h_input_array = block.outputArrayValue(input, &stat);
		// atm multiIndex will be only 0
		stat = h_input_array.jumpToElement(multiIndex);
		p_cage_data->calc_green_coords(iter, mat);
		p_cage_data->save_original_cage_data();
		m_green_calculated = true;
		// TODO: write out green coords w/ original cage to be able to reload scene without calculating(also correctly) everytime we open it
	}

	p_cage_data->calc_scale_factor();
	MPointArray new_arr;
	new_arr.setLength(p_cage_data->m_nr_of_targetverts);

	const auto inverse_mat = mat.inverse();
	tbb::parallel_for(tbb::blocked_range<unsigned int>(0, p_cage_data->m_nr_of_targetverts), [&](const tbb::blocked_range<unsigned int>& r) {
		for (unsigned int idx = r.begin(); idx < r.end(); ++idx) {
			new_arr[idx] = p_cage_data->calc_pos(idx) * inverse_mat;
		}
	});

	iter.setAllPositions(new_arr);
	return stat;
}

void GreenCageDeformer::postConstructor()
{
	m_cage_connected = false;
	m_green_calculated = false;
}

MStatus GreenCageDeformer::connectionMade(const MPlug & plug, const MPlug & otherPlug, bool asSrc)
{
	if (!asSrc && plug == s_cage_mesh) {
		p_cage_data.reset(new GCDeformData());
		m_cage_connected = true;
	}

	return MPxDeformerNode::connectionMade(plug, otherPlug, asSrc);
}

MStatus GreenCageDeformer::connectionBroken(const MPlug & plug, const MPlug & otherPlug, bool asSrc)
{
	if (!asSrc && plug == s_cage_mesh) {
		p_cage_data.reset();
		m_cage_connected = false;
		m_green_calculated = false;
	}

	return MPxDeformerNode::connectionBroken(plug, otherPlug, asSrc);
}

void* GreenCageDeformer::creator()
{
	return new GreenCageDeformer();
}

MStatus GreenCageDeformer::initialize()
{
	MStatus stat;
	MFnTypedAttribute tAttr;

	s_cage_mesh = tAttr.create("cageMesh", "cm", MFnData::kMesh, &stat);
	CHECK_MSTATUS_AND_RETURN_IT(stat);
	stat = addAttribute(s_cage_mesh);
	CHECK_MSTATUS_AND_RETURN_IT(stat);
	stat = attributeAffects(s_cage_mesh, outputGeom);

	return stat;
}

MStatus initializePlugin(MObject obj)
{
	MStatus stat;

	MFnPlugin plugin(obj, "Balazs Pataki", "1.0", "Any");
	stat = plugin.registerNode(GreenCageDeformer::s_node_name, GreenCageDeformer::s_node_id, &GreenCageDeformer::creator, &GreenCageDeformer::initialize, MPxNode::kDeformerNode);
	CHECK_MSTATUS_AND_RETURN_IT(stat);

	return stat;
}

MStatus uninitializePlugin(MObject obj)
{
	MStatus stat;

	MFnPlugin plugin(obj);
	stat = plugin.deregisterNode(GreenCageDeformer::s_node_id);
	CHECK_MSTATUS_AND_RETURN_IT(stat);

	return stat;
}


#include "conditional_algorithm.h"

#include <Eigen/Dense>
#include <stan/math/prim/fun.hpp>

#include "algorithm_state.pb.h"
#include "base_algorithm.h"
#include "src/collectors/base_collector.h"

Eigen::VectorXd ConditionalAlgorithm::lpdf_from_state(
    const Eigen::MatrixXd &grid, const Eigen::RowVectorXd &hier_covariate,
    const Eigen::RowVectorXd &mix_covariate) {
  // Read mixing state
  unsigned int n_data = curr_state.cluster_allocs_size();
  unsigned int n_clust = curr_state.cluster_states_size();
  mixing->set_state_from_proto(curr_state.mixing_state());
  // Initialize estimate containers
  Eigen::MatrixXd lpdf_local(grid.rows(), n_clust);
  Eigen::VectorXd lpdf_final(grid.rows());
  auto temp_hier = unique_values[0]->clone();
  // Loop over grid points
  for (size_t i = 0; i < grid.rows(); i++) {
    // Get mixing weights for the i-th grid point
    Eigen::VectorXd logweights =
        mixing->get_weights(true, false, mix_covariate);
    // Loop over clusters
    for (size_t j = 0; j < n_clust; j++) {
      temp_hier->set_state_from_proto(curr_state.cluster_states(j));
      // Get local, single-point estimate
      lpdf_local(i, j) =
          logweights(j) + temp_hier->like_lpdf(grid.row(i), hier_covariate);
    }
    // Final estimate for i-th grid point
    lpdf_final(i) = stan::math::log_sum_exp(lpdf_local.row(i));
  }
  return lpdf_final;
}


#include <iostream>
#include <cassert>
#include <vector>
#include <map>
#include <deque>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/iterator/function_output_iterator.hpp>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>

const int debug_level = 0;

#define DEBUG(min_level, x)      \
  if (debug_level >= min_level)  \
  {                              \
    std::cerr << x << std::endl; \
  }

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, boost::no_property, boost::property<boost::edge_weight_t, long>> Graph;

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Delaunay_triangulation_2<K> Triangulation;

bool testcase()
{
  int n;
  std::cin >> n;
  if (n == 0)
  {
    return false;
  }
  assert(n >= 2 && n <= 6e4);

  DEBUG(1, "\ntest case; n = " << n);

  std::vector<K::Point_2> infected_points;
  for (int i = 0; i < n; i++)
  {
    int x, y;
    std::cin >> x >> y;
    assert(abs(x) < (1 << 24) && abs(y) < (1 << 24));
    infected_points.emplace_back(x, y);
  }

  int m;
  std::cin >> m;
  assert(m >= 1 && m <= 4e4);

  std::vector<std::pair<K::Point_2, long>> healthy_points;
  for (int i = 0; i < m; i++)
  {
    int x, y;
    long d;
    std::cin >> x >> y >> d;
    assert(abs(x) < (1 << 24) && abs(y) < (1 << 24));
    assert(d >= 0 && d < (long(1) << 49));
    healthy_points.push_back(std::make_pair(K::Point_2(x, y), d));
  }

  Triangulation triangulation;
  triangulation.insert(infected_points.begin(), infected_points.end());

  const int indexed_face_count = triangulation.number_of_faces() + 1;
  Graph mst_input_graph(indexed_face_count);
  auto mst_input_weights = boost::get(boost::edge_weight, mst_input_graph);
  std::map<Triangulation::Face_handle, int> face_indices_by_handle;
  int next_free_face_index = 0;
  const int infinite_face_index = next_free_face_index++;
  auto index_from_face_handle = [&face_indices_by_handle, &next_free_face_index, &triangulation, indexed_face_count, infinite_face_index](Triangulation::Face_handle fh) {
    if (triangulation.is_infinite(fh))
    {
      DEBUG(4, "infinite face");
      return infinite_face_index;
    }

    int face_index;
    if (face_indices_by_handle.count(fh) == 1)
    {
      DEBUG(4, "existing face");
      face_index = face_indices_by_handle.at(fh);
    }
    else
    {
      DEBUG(4, "new face");
      assert(face_indices_by_handle.count(fh) == 0);
      face_index = next_free_face_index++;
      face_indices_by_handle.insert(std::make_pair(fh, face_index));
    }
    DEBUG(4, "face_index " << face_index << " limit " << indexed_face_count);
    assert(face_index >= 0 && face_index < indexed_face_count);
    return face_index;
  };
  for (auto it = triangulation.finite_edges_begin(); it != triangulation.finite_edges_end(); it++)
  {
    long weight = -CGAL::squared_distance(it->first->vertex((it->second + 1) % 3)->point(), it->first->vertex((it->second + 2) % 3)->point());
    int vertex_a = index_from_face_handle(it->first);
    int vertex_b = index_from_face_handle(it->first->neighbor(it->second));
    if (vertex_a > vertex_b)
    {
      std::swap(vertex_a, vertex_b);
    }
    assert(vertex_a < vertex_b);
    DEBUG(3, "vertex_a " << vertex_a << " vertex_b " << vertex_b << " weight " << weight);
    auto edge_result = boost::edge(vertex_a, vertex_b, mst_input_graph);
    if (edge_result.second)
    {
      long &saved_weight = mst_input_weights[edge_result.first];
      saved_weight = std::min(saved_weight, weight);
    }
    else
    {
      boost::add_edge(vertex_a, vertex_b, weight, mst_input_graph);
    }
  }

  Graph mst(indexed_face_count);
  auto add_mst_edge = [&mst, &mst_input_graph](Graph::edge_descriptor edge) {
    long weight = boost::get(boost::edge_weight_t(), mst_input_graph, edge);
    DEBUG(3, "add_mst_edge " << edge.m_source << " " << edge.m_target << " " << weight);
    boost::add_edge(edge.m_source, edge.m_target, weight, mst);
  };
  boost::kruskal_minimum_spanning_tree(mst_input_graph, boost::make_function_output_iterator(std::ref(add_mst_edge)));

  std::deque<std::pair<int, long>> bfs_queue{std::make_pair(infinite_face_index, std::numeric_limits<long>::max())};
  std::vector<long> largest_escape_by_face(indexed_face_count, 0);
  while (!bfs_queue.empty())
  {
    int prev_vertex = bfs_queue.front().first;
    long prev_largest_escape = bfs_queue.front().second;
    bfs_queue.pop_front();

    assert(largest_escape_by_face.at(prev_vertex) == 0);
    largest_escape_by_face.at(prev_vertex) = prev_largest_escape;
    DEBUG(3, "prev_vertex " << prev_vertex << " prev_largest_escape " << prev_largest_escape);

    for (auto its = boost::out_edges(prev_vertex, mst); its.first != its.second; its.first++)
    {
      int next_vertex = its.first->m_target;
      assert(next_vertex != prev_vertex);
      if (largest_escape_by_face.at(next_vertex) > 0)
      {
        continue;
      }
      DEBUG(3, "next_vertex " << next_vertex);

      long limit_of_this_edge = -boost::get(boost::edge_weight_t(), mst, *its.first);
      long next_largest_escape = std::min(prev_largest_escape, limit_of_this_edge);
      DEBUG(3, "limit_of_this_edge " << limit_of_this_edge << " next_largest_escape " << next_largest_escape)
      assert(next_largest_escape > 0);

      bfs_queue.push_back(std::make_pair(next_vertex, next_largest_escape));
    }
  }
  int min_largest_escape_by_triangle = *std::min_element(largest_escape_by_face.begin(), largest_escape_by_face.end());
  DEBUG(2, "min_largest_escape_by_triangle " << min_largest_escape_by_triangle);
  assert(min_largest_escape_by_triangle > 0);

  std::vector<bool> can_escape_by_index(m);
  for (int i = 0; i < m; i++)
  {
    auto &healthy_point = healthy_points.at(i);
    Triangulation::Face_handle face = triangulation.locate(healthy_point.first);
    int face_index = index_from_face_handle(face);
    DEBUG(2, "face_index " << face_index);
    long largest_escape = largest_escape_by_face.at(face_index);
    DEBUG(2, "largest_escape (initial) " << largest_escape);

    auto nearest = triangulation.nearest_vertex(healthy_point.first);
    DEBUG(3, "limiting using point " << nearest->point());
    largest_escape = std::min(largest_escape, 4 * long(CGAL::squared_distance(healthy_point.first, nearest->point())));

    DEBUG(2, "largest_escape (adjusted) " << largest_escape);
    can_escape_by_index.at(i) = 4 * healthy_point.second <= largest_escape;
  }

  for (bool v : can_escape_by_index)
  {
    std::cout << (v ? "y" : "n");
  }
  std::cout << "\n";

  return true;
}

int main()
{
  std::ios_base::sync_with_stdio(false);

  while (testcase())
  {
  }

  return 0;
}


#define _USE_MATH_DEFINES

#include "mesh.hpp"
#include "plane2d.hpp"
#include "line_interval.hpp"
#include "found_path.hpp"
#include <iostream>
#include <map>
#include <string>
#include <vector>
#include <limits>
#include <functional>
#include <future>
#include <cmath>
#include <thread>

#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

Mesh::Mesh(const int numVertices, const int numFaces, const int numCells){
    vertices.reserve(numVertices);
    faces.reserve(numFaces);
    tetrahedrons.reserve(numCells);
}

void Mesh::setVertices(const vector<array<double, 3>> & _vertices){

    for(unsigned long int i=0; i < _vertices.size(); ++i){
	Vertex3d pt(_vertices[i]);
	vertices.push_back(pt);
    }

}

void Mesh::addTetrahedron(const int id, const array<int, 4> vertexIds,
	const vector<unsigned long int> neighborIds, const double weight, const int label){

    vector<reference_wrapper<Vertex3d>> refs = {vertices[vertexIds[0]], vertices[vertexIds[1]], vertices[vertexIds[2]], vertices[vertexIds[3]]};

    Tetrahedron tet(id, refs, weight, label);

    unsigned long int current_size = tetrahedrons.size();

    for(unsigned long int i=0; i<neighborIds.size(); ++i){
	if(neighborIds[i]<current_size){
	    Tetrahedron& neighbor = tetrahedrons[neighborIds[i]];

	    neighbor.addNeighbor(tet.Id());
	    tet.addNeighbor(neighbor.Id());
	}
    }
    tetrahedrons.push_back(tet);
}

void Mesh::addFace(const array<int, 3> vertexIds, const int tetId){
    vector<reference_wrapper<Vertex3d>> refs = {vertices[vertexIds[0]], vertices[vertexIds[1]], vertices[vertexIds[2]]};

    Face face(refs, tetId);
    faces.push_back(face);
}

bool Mesh::setTarget(array<double, 3> _target){
    bool targetSet=false;
    for(unsigned long int i=0; i < tetrahedrons.size(); ++i){
	if (tetrahedrons[i].contains(_target)){
	    Vector3d t(_target[0], _target[1], _target[2]);
	    target = t;
	    targetTetId=i;
	    targetSet=true;
	    break;
	}
    }
    return targetSet;
}

vector<Shape3d> Mesh::sliceIndv(array<double, 2> rotation){
    vector<int> planeLastChecked;
    planeLastChecked.reserve(tetrahedrons.size());
    for(unsigned long int i=0; i<tetrahedrons.size(); i++){
	planeLastChecked.push_back(-1);
    }

    Plane3d plane(0, rotation[0], rotation[1], target);
    return slice(plane, planeLastChecked);
}

vector<FoundPath> Mesh::findPaths(vector<Plane3d> planes, double distBound){
    vector<int> planeLastChecked;
    planeLastChecked.reserve(tetrahedrons.size());
    for(unsigned long int i=0; i<tetrahedrons.size(); i++){
	planeLastChecked.push_back(-1);
    }

    double minUpperBound=numeric_limits<double>::max();

    //vector<vector<LineInterval>> allCandidateIntervals;
    vector<vector<FoundPath>> allFoundPaths;

    for(int i=0; i<planes.size(); ++i){
	vector<Shape3d> planeSlice = slice(planes[i], planeLastChecked);
	Plane2d plane2d(planeSlice, planes[i]);
	vector<FoundPath> planePaths=plane2d.FindPaths(distBound);
	//cout << "plane paths size:" << endl;
	//cout << planePaths.size() << endl;
	allFoundPaths.push_back(planePaths);
	if(plane2d.MinUpperBound()<minUpperBound){
	    minUpperBound=plane2d.MinUpperBound();
	}
    }
    vector<FoundPath> foundPaths;
    //cout << "min upper bound:" << endl;
    //cout << minUpperBound << endl;
    //cout << "all found paths size:" << endl;
    //cout << allFoundPaths.size() << endl;

    for(int i=0; i<planes.size(); ++i){
	vector<FoundPath> planePaths=allFoundPaths[i];

	for(int j=0; j<planePaths.size(); ++j){
	    FoundPath foundPath=planePaths[j];
	    if(foundPath.LowerBound()<minUpperBound){
		array<Vector3d, 2> endPoints=foundPath.Points();
		Vector3d pt0=planes[i].Get3dPoint(endPoints[0]);
		Vector3d pt1=planes[i].Get3dPoint(endPoints[1]);

		FoundPath fp(planes[i].Id(), pt0, pt1, foundPath.LowerBound(), foundPath.UpperBound());
		foundPaths.push_back(fp);
	    }
	}
    }
    //cout << "return paths size:" << endl;
    //cout << foundPaths.size() << endl;
    return foundPaths;
}

vector<Shape3d> Mesh::slice(Plane3d plane, vector<int> &planeLastChecked){
    vector<Shape3d> slice = computeSliceComponent(plane, planeLastChecked, targetTetId);

    for(unsigned long int i=0; i<faces.size(); i++){
	if(faces[i].intersectsPlane(plane) && planeLastChecked[faces[i].TetId()]!=plane.Id()){
	    vector<Shape3d> sliceComponent = computeSliceComponent(plane, planeLastChecked, faces[i].TetId());
	    slice.insert(slice.end(), sliceComponent.begin(), sliceComponent.end());
	}
    }
    return slice;
}

vector<Shape3d> Mesh::computeSliceComponent(Plane3d plane, vector<int> &planeLastChecked, unsigned long int initTet){

    vector<unsigned long int> tetStack;
    tetStack.push_back(initTet);

    vector<Shape3d> allPoints;

    while(tetStack.size() > 0){
	Tetrahedron& tet = tetrahedrons[tetStack.back()];
	tetStack.pop_back();

	if(planeLastChecked[tet.Id()]!=plane.Id()){

	    vector<array<double, 3>> intersectionPoints = tet.intersectsPlane(plane);

	    if(intersectionPoints.size()>2){
		Shape3d shape(tet.Id(), intersectionPoints, tet.Weight(), tet.Label());
		allPoints.push_back(shape);
		vector<unsigned long int> neighbors = tet.Neighbors();

		for(int i=0; i<neighbors.size();++i){
		    tetStack.push_back(neighbors[i]);
		}
	    }

	    planeLastChecked[tet.Id()]=plane.Id();
	}
    }

    return allPoints;
}

vector<FoundPath> Mesh::shortestPaths(const int epsilon, const int numThreads, double distBound){

    future<vector<FoundPath>> futures[numThreads];

    vector<Plane3d> planes;
    //cout << numThreads << endl;

    for(int i=0; i<epsilon; i++){
	for(int j=0; j<epsilon; j++){
	    Plane3d plane((i*epsilon)+j, i*M_PI/epsilon, j*M_PI/epsilon, target);
	    planes.push_back(plane);
	}
    }

    int subarraySize = ((epsilon * epsilon) + numThreads - 1) / numThreads;

    for(int i=0; i<numThreads; i++){
	int start=i*subarraySize;
	int end=start+subarraySize;
	if(end>(epsilon*epsilon)){
	    end=epsilon*epsilon;
	}

	vector<Plane3d> planeVector;

	for(int idx=start; idx<end; idx++){
	    planeVector.push_back(planes[idx]);
	}

	futures[i]=async(&Mesh::findPaths, this, planeVector, distBound);
    }

    vector<FoundPath> allFoundPaths;
    double minUpperBound=numeric_limits<double>::max();
    for(unsigned long int i=0; i<numThreads; ++i){
	//cout << i << endl;
	vector<FoundPath> foundPaths=futures[i].get();
	for(unsigned long int j=0; j<foundPaths.size(); ++j){
	    if(foundPaths[j].UpperBound()<minUpperBound){
		minUpperBound=foundPaths[j].UpperBound();
	    }
	    allFoundPaths.push_back(foundPaths[j]);
	}
	//vector<FoundPath> foundPaths=futures[i].get();
	//allFoundPaths.push_back(foundPaths);
    }

    vector<FoundPath> ret;
    for(unsigned long int i=0; i<allFoundPaths.size(); ++i){
	if(allFoundPaths[i].LowerBound()<minUpperBound){
	    ret.push_back(allFoundPaths[i]);
	}
    }

    //cout << "ret size:" << endl;
    //cout << ret.size() << endl;
    return ret;
}

unsigned long int Mesh::getTargetTetId(){
    return targetTetId;
}


/**********************************************************************************************************************
This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
Authors:  Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
Licensed under Apache2 license (see LICENSE file in main directory)
**********************************************************************************************************************/

#include <memory>
#include <array>

#include <iostream>

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

#include <ct/core/core.h>
#include <ct/rbd/rbd.h>

#include "ct/models/HyA/HyA.h"

using namespace ct;
using namespace ct::rbd;

#define Debug

TEST(HyaLinearizerTest, NumDiffComparison)
{
    typedef FixBaseFDSystem<HyA::Dynamics> HyASystem;

    const size_t STATE_DIM = HyASystem::STATE_DIM;
    const size_t CONTROL_DIM = HyASystem::CONTROL_DIM;

    std::shared_ptr<HyASystem> hyaSystem(new HyASystem);
    std::shared_ptr<HyASystem> hyaSystem2(new HyASystem);

    RbdLinearizer<HyASystem> rbdLinearizer(hyaSystem, true);
    core::SystemLinearizer<STATE_DIM, CONTROL_DIM> systemLinearizer(hyaSystem2, true);

    core::StateVector<STATE_DIM> x;
    x.setZero();
    core::ControlVector<CONTROL_DIM> u;
    u.setZero();

    auto A_rbd = rbdLinearizer.getDerivativeState(x, u, 1.0);
    auto B_rbd = rbdLinearizer.getDerivativeControl(x, u, 1.0);

    auto A_system = systemLinearizer.getDerivativeState(x, u, 1.0);
    auto B_system = systemLinearizer.getDerivativeControl(x, u, 1.0);

    ASSERT_LT((A_rbd - A_system).array().abs().maxCoeff(), 1e-5);
    ASSERT_LT((B_rbd - B_system).array().abs().maxCoeff(), 1e-4);

    size_t nTests = 1000;
    for (size_t i = 0; i < nTests; i++)
    {
        x.setRandom();
        u.setRandom();

        auto A_rbd = rbdLinearizer.getDerivativeState(x, u, 0.0);
        auto B_rbd = rbdLinearizer.getDerivativeControl(x, u, 0.0);

        auto A_system = systemLinearizer.getDerivativeState(x, u, 0.0);
        auto B_system = systemLinearizer.getDerivativeControl(x, u, 0.0);

        ASSERT_LT((A_rbd - A_system).array().abs().maxCoeff(), 1e-5);

        ASSERT_LT((B_rbd - B_system).array().abs().maxCoeff(), 1e-4);
    }
}

TEST(CodegenLinearizerTest, NumDiffComparison)
{
    typedef FixBaseFDSystem<HyA::Dynamics> HyASystem;

    const size_t STATE_DIM = HyASystem::STATE_DIM;
    const size_t CONTROL_DIM = HyASystem::CONTROL_DIM;

    std::shared_ptr<HyASystem> hyaSystem(new HyASystem);

    RbdLinearizer<HyASystem> rbdLinearizer(hyaSystem, true);

    ct::models::HyA::HyALinearizedForward hyaLinear;

    core::StateVector<STATE_DIM> x;
    core::ControlVector<CONTROL_DIM> u;

    size_t nTests = 1000;
    for (size_t i = 0; i < nTests; i++)
    {
        x.setRandom();
        u.setRandom();

        auto A_rbd = rbdLinearizer.getDerivativeState(x, u, 0.0);
        auto A_gen = hyaLinear.getDerivativeState(x, u, 0.0);

        auto B_rbd = rbdLinearizer.getDerivativeControl(x, u, 0.0);
        auto B_gen = hyaLinear.getDerivativeControl(x, u, 0.0);

        ASSERT_LT((A_rbd - A_gen).array().abs().maxCoeff(), 1e-5);
        ASSERT_LT((B_rbd - B_gen).array().abs().maxCoeff(), 1e-4);
    }
}

TEST(IntegratorTest, IntegratorTestHya)
{
    typedef FixBaseFDSystem<HyA::Dynamics> HyASystem;

    const size_t STATE_DIM = HyASystem::STATE_DIM;

    std::shared_ptr<HyASystem> hyaSystem(new HyASystem);

    core::Integrator<STATE_DIM> integratorEulerOdeint(hyaSystem, core::EULER);
    core::Integrator<STATE_DIM> integratorRk4Odeint(hyaSystem, core::RK4);

    core::Integrator<STATE_DIM> integratorEulerCT(hyaSystem, core::EULERCT);
    core::Integrator<STATE_DIM> integratorRK4CT(hyaSystem, core::RK4CT);

    double dt = 0.001;
    double startTime = 0.0;
    size_t numSteps = 10;

    size_t nTests = 10000;
    std::vector<core::StateVector<STATE_DIM>, Eigen::aligned_allocator<core::StateVector<STATE_DIM>>> xEulerOdeint(
        nTests),
        xEulerCt(nTests), xRk4Odeint(nTests), xRk4CT(nTests);

    for (size_t i = 0; i < nTests; ++i)
    {
        xEulerOdeint[i].setRandom();
        xEulerCt[i] = xEulerOdeint[i];
        xRk4Odeint[i].setRandom();
        xRk4CT[i] = xRk4Odeint[i];
    }

    auto start = std::chrono::high_resolution_clock::now();
    for (size_t i = 0; i < nTests; i++)
    {
        integratorEulerOdeint.integrate_n_steps(xEulerOdeint[i], startTime, numSteps, dt);
    }

    auto end = std::chrono::high_resolution_clock::now();
    auto diff = end - start;
    double msTotal = std::chrono::duration<double, std::micro>(diff).count() / 1000.0;
    std::cout << "integratorEulerOdeint: " << msTotal << " ms. Average: " << msTotal / double(nTests) << " ms"
              << std::endl;

    start = std::chrono::high_resolution_clock::now();
    for (size_t i = 0; i < nTests; i++)
    {
        integratorEulerCT.integrate_n_steps(xEulerCt[i], startTime, numSteps, dt);
    }
    end = std::chrono::high_resolution_clock::now();
    diff = end - start;
    msTotal = std::chrono::duration<double, std::micro>(diff).count() / 1000.0;
    std::cout << "integratorEulerCT: " << msTotal << " ms. Average: " << msTotal / double(nTests) << " ms" << std::endl;

    start = std::chrono::high_resolution_clock::now();
    for (size_t i = 0; i < nTests; i++)
    {
        integratorRk4Odeint.integrate_n_steps(xRk4Odeint[i], startTime, numSteps, dt);
    }
    end = std::chrono::high_resolution_clock::now();
    diff = end - start;
    msTotal = std::chrono::duration<double, std::micro>(diff).count() / 1000.0;
    std::cout << "integratorRk4Odeint: " << msTotal << " ms. Average: " << msTotal / double(nTests) << " ms"
              << std::endl;

    start = std::chrono::high_resolution_clock::now();
    for (size_t i = 0; i < nTests; i++)
    {
        integratorRK4CT.integrate_n_steps(xRk4CT[i], startTime, numSteps, dt);
    }
    end = std::chrono::high_resolution_clock::now();
    diff = end - start;
    msTotal = std::chrono::duration<double, std::micro>(diff).count() / 1000.0;
    std::cout << "integratorRK4CT: " << msTotal << " ms. Average: " << msTotal / double(nTests) << " ms" << std::endl;

    for (size_t i = 0; i < nTests; ++i)
    {
        ASSERT_LT((xRk4CT[i] - xRk4Odeint[i]).array().abs().maxCoeff(), 1e-12);
        ASSERT_LT((xEulerCt[i] - xEulerOdeint[i]).array().abs().maxCoeff(), 1e-12);
    }
}

int main(int argc, char **argv)
{
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


/*
 * G_mod_p.cpp
 *
 *  Created on: 20.09.2010
 *      Author: stephaniebayer
 */

#include "G_mod_p.h"
#include <NTL/ZZ.h>
NTL_CLIENT

#include <time.h>
G_mod_p::G_mod_p() {
	// TODO Auto-generated constructor stub

}

//Sets the generator to gen and the mod to p, checks if the generator has the right modular value p
G_mod_p::G_mod_p(Mod_p gen,  long p){

	generator = gen;
	mod = to_ZZ(p);
	if (gen.get_mod() != p)
		cout  << "The modular value of the generator and p are not equal" << endl;
}

//Sets the generator to gen and the mod to p, checks if the generator has the right modular value p
G_mod_p::G_mod_p(Mod_p gen,  ZZ p){

	generator = gen;
	mod = p;

	if (gen.get_mod() != p)
		cout  << "The modular value of the generator and p are not equal" << endl;

}

//Sets the generator to the value gen and the mod to p
G_mod_p::G_mod_p(ZZ val,  long p){

	generator = Mod_p(val, p);
	mod = to_ZZ(p);

}

//Sets the generator to the value gen and the mod to p
G_mod_p::G_mod_p(ZZ val, ZZ p){

	generator = Mod_p(val, p);
	mod = p;

}

//Sets the generator to the value gen and the mod to p
G_mod_p::G_mod_p(long val,  long p){

	generator = Mod_p(val, p);
	mod = to_ZZ(p);

}

//Sets the generator to the value gen and the mod to p
G_mod_p::G_mod_p(long val, ZZ p){

	generator = Mod_p(val, p);
	mod = p;

}

//Creates a group given the generator, mod is set to modular value if gen
G_mod_p::G_mod_p(Mod_p gen){

	generator = gen;
	mod = gen.get_mod();
}

//Creates a group ZZ/p, the function sets the generator to the smallest possible one
G_mod_p::G_mod_p(ZZ p){

	ZZ i;
	mod = p;
	for (i = to_ZZ(1); i < p; ++i)
	{
		if (is_generator(i))
		{
			generator = Mod_p(i,p);
			break;
		}
	}
}

//Creates a group ZZ/p, the function sets the generator to the smallest possible one
G_mod_p::G_mod_p(long p){

	long i;
	mod = p;
	for (i = 1; i < p; ++i)
	{
		if (is_generator(i))
		{
			generator = Mod_p(i,p);
			break;
		}
	}
}

//Destructor
G_mod_p::~G_mod_p() {
	// TODO Auto-generated destructor stub
}

//Returns the generator
Mod_p G_mod_p::get_gen()const{

	return generator;
}

//Returns the modular value
ZZ G_mod_p::get_mod()const{

	return mod;
}

//Checks if an element is a generator of the group
bool G_mod_p::is_generator(const Mod_p& el){
	ZZ pow;
	bool b;
	b=false;
	pow = PowerMod(el.get_val(),(mod-1)/2,mod);
	if(pow == (mod-1))
	{
		if(el.get_val()!=(mod-1))
		{b=true;
		}
	}
	return b;
}

//Checks if an element with value x is a generator of the group
bool G_mod_p::is_generator(const ZZ& x){
	ZZ pow;
	bool b;
	b=false;
	pow = PowerMod(x,(mod-1)/2,mod);

	if(pow == (mod-1))
	{
		if(x!=(mod-1))
		{b=true;
		}
	}

	return b;
}

//Checks if an element with value x is a generator of the group
bool G_mod_p::is_generator(const long& x){
	ZZ pow;
	bool b;
	pow = PowerMod(to_ZZ(x),(mod-1)/2, mod);
	if(pow == (mod-1))
	{
		if(x!=(mod-1))
		{b=true;
		}
	}

	return b;
}

//Returns the identity of the group
Mod_p G_mod_p::identity(){

	return Mod_p(1, mod);
}

//return a random element of the group
Mod_p G_mod_p::random_el(){

	ZZ ran;
	SetSeed(to_ZZ(time(0)));
	ran = RandomBnd(mod);

	return Mod_p(ran,mod);

}

//Creates an element with the value v modular mod
Mod_p G_mod_p::element(ZZ v){

	return Mod_p(v,mod);

}

//Creates an element with the value v modular mod
Mod_p G_mod_p::element(long v){

	return Mod_p(v,mod);

}

//Returns a n-th root of unity of the group
Mod_p G_mod_p::rootofunity(long n){

	ZZ i;
	ZZ pow;
	if ((mod-1) % n == 0)
	{	for(i =  to_ZZ(2); i<mod;++i)
		{
			if(GCD(i,mod)==to_ZZ(1))
			{

				if (n&1)
				{
					pow = PowerMod(i,n, mod);
					if(pow==1)
						return Mod_p(i,mod);
				}
				else
				{
					pow = PowerMod(i,n/2,mod);
					if(pow == (mod-1))
					{
						return Mod_p(i,mod);
					}
				}
			}
		}
	}
	else
	{
		cout << "There is no" << n <<"-th root of unity"<< endl;
		return Mod_p(1,mod);
	}
	return Mod_p(1,mod);
}

//Returns a n-th root of unity of the group
Mod_p G_mod_p::rootofunity(ZZ n){

	ZZ i;
	ZZ pow;
	if ((mod-1) % n == 0)
	{	for(i =  to_ZZ(2); i<mod;++i)
		{
			if(GCD(i,mod)==to_ZZ(1))
			{

				if (IsOdd(n))
				{
					pow = PowerMod(i,n, mod);
					if(pow==1)
						return Mod_p(i,mod);
				}
				else
				{
					pow = PowerMod(i,n/2,mod);
					if(pow == (mod-1))
					{
						return Mod_p(i,mod);
					}
				}
			}
		}
	}
	else
	{
		cout << "There is no" << n <<"-th root of unity"<< endl;
		return Mod_p(1,mod);
	}
	return Mod_p(1,mod);
}

//Returns the inverse of an element with value x
Mod_p G_mod_p::inverse(ZZ x){

	ZZ temp;
	temp = InvMod(x,mod);
	return Mod_p(temp,mod);
}

//Returns the inverse of an element with value x
Mod_p G_mod_p::inverse(long x){

	ZZ temp;
	temp = InvMod(to_ZZ(x),mod);
	return Mod_p(temp,mod);
}

//Assignment operator of the group
void G_mod_p::operator =(const G_mod_p& H){

	generator = H.get_gen();
	mod = H.get_mod();
}


// Copyright (c) 2016
// Author: Chrono Law
#include <std.hpp>
using namespace std;

//#include <boost/type_traits.hpp>

#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/next_prior.hpp>

using namespace boost;
using namespace boost::mpl;

///////////////////////////////////////

void case1()
{
    typedef int_<2> i2;
    typedef integral_c<short, 2> s2;

    assert(i2::value == 2);
    assert(i2::value == s2::value);

    assert((is_same<i2::type, i2>::value));
    assert((is_same<s2::value_type, short>::value));

    assert(i2::next::value == 3);
    assert(prior<s2>::type::value == 1);

    i2 two1;
    s2 two2;

    int i = two1 + two2;
    assert(i == int_<4>());

}

///////////////////////////////////////

#include <boost/mpl/bool.hpp>

void case2()
{
    assert(true_::value == true);
    assert(false_::value == false);

    assert((is_same<true_::type, bool_<true> >::value));
    assert((is_same<false_::value_type, bool>::value));

    //next<true_>::type;

}

///////////////////////////////////////

#include <boost/mpl/arithmetic.hpp>
#include <boost/mpl/logical.hpp>
#include <boost/mpl/comparison.hpp>

void case3()
{
    typedef int_<2> i2;
    typedef int_<5> i5;
    typedef int_<7> i7;

    assert((boost::mpl::plus<i2, i5, i7>::type::value == 14));
    assert((boost::mpl::equal_to<boost::mpl::minus<i7, i5>::type, i2>::type::value));

    assert((boost::mpl::less<i2, i7>::type::value));
    assert((is_same<boost::mpl::greater<i5, i2>::type, true_>::value));

    assert((not_<and_<true_, false_>::type>::type::value));
    assert((or_<true_, false_>::type()));

}

///////////////////////////////////////

#include <boost/mpl/char.hpp>
#include <boost/mpl/long.hpp>

void case4()
{
    typedef boost::mpl::plus<int_<1>, char_<2>, long_<3>>::type
            result;

    assert(!(is_same<result, long_<6>>::type::value));

    assert((is_same<result, integral_c<long, 6>>::type::value));

}

///////////////////////////////////////

int main()
{
    std::cout << "hello integral" << std::endl;

    case1();
    case2();
    case3();
}


// fifteen puzzle

#include <iostream>
#include <queue>
#include <vector>
#include <algorithm>

#ifndef CLOSED_USE_VECTOR
#include <unordered_set>
#endif

// #define DUMP
// #define CLOSED_USE_VECTOR   // 2800 ms VS 46 ms !!!!!

template <class State>
class BreadthFirst
{
public:
    BreadthFirst(const State& start, const State& _end) : end(_end)
    {
        open.push(start);
    }
    bool Solve()
    {
        do
        {
            auto current = open.front();
            open.pop();
            if ( NextStep(current) )
            {
                solution = current;
                return true;
            }
        }
        while(!open.empty());

        return false;
    }
    State Solution() const { return solution; }
    void PrintStatistics() const
    {
        std::cout << "Visited states: " << closed.size() << std::endl;
    }
private:
    bool NextStep(const State& current)
    {
#ifdef DUMP
        std::cout << "\n********* new iteration *************\n" << std::endl;
        std::cout << "current:\n";
        current.Print();
#endif

        if (current == end)
            return true; // found
#ifdef CLOSED_USE_VECTOR
        closed.push_back(current);
#else
        closed.insert(current);
#endif
        auto nextStates = current.Next();

#ifdef DUMP
        std::cout << "\nnext:\n";
#endif

        for (auto s: nextStates)
        {
#ifdef CLOSED_USE_VECTOR
            if (std::find(closed.begin(), closed.end(), s) == closed.end()) // not visited yet
#else
            if ( closed.find(s) == closed.end() ) // not visited yet
#endif
            {
#ifdef DUMP
                s.Print();
#endif
                open.push(s);
            }
        }
        return false;
    }

    const State end;
    State solution;
    std::queue<State> open;
#ifdef CLOSED_USE_VECTOR
    std::vector<State> closed;
#else
    std::unordered_set<State> closed;
#endif
};

//

#include <array>
#include <cassert>
#include <iomanip>
#include <boost/functional/hash.hpp>

class FState
{
public:
    FState() : configuration( {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 0} )
    {
    }
    FState(std::array<int, 16>&& s) : configuration(std::move(s)) {}
    FState(const FState& s) : configuration(s.configuration), moves(s.moves) {}
    FState(FState&& s) : configuration(std::move(s.configuration)), moves(std::move(s.moves)) {}
    FState& operator = (const FState& s)
    {
        if (&s != this)
        {
            configuration = s.configuration;
            moves = s.moves;
        }
        return *this;
    }
    std::vector<FState> Next() const
    {
        auto emptyIt = std::find(configuration.begin(), configuration.end(), 0);
        auto emptyPosition = emptyIt - configuration.begin();
        using V = std::vector<FState>;
        switch (emptyPosition)
        {
            case 0: return V{ Move(0, 1), Move(0, 4) }; break;
            case 1: return V{ Move(1, 0), Move(1, 5), Move(1, 2) }; break;
            case 2: return V{ Move(2, 1), Move(2, 6), Move(2, 3) }; break;
            case 3: return V{ Move(3, 2), Move(3, 7) }; break;
            case 4: return V{ Move(4, 0), Move(4, 5), Move(4, 8) }; break;
            case 5: return V{ Move(5, 1), Move(5, 4), Move(5, 6), Move(5, 9) }; break;
            case 6: return V{ Move(6, 2), Move(6, 5), Move(6, 7), Move(6, 10) }; break;
            case 7: return V{ Move(7, 3), Move(7, 6), Move(7, 11) }; break;
            case 8: return V{ Move(8, 4), Move(8, 9), Move(8, 12) }; break;
            case 9: return V{ Move(9, 5), Move(9, 8), Move(9, 10), Move(9, 13) }; break;
            case 10: return V{ Move(10, 6), Move(10, 9), Move(10, 11), Move(10, 14) }; break;
            case 11: return V{ Move(11, 7), Move(11, 10), Move(11, 15) }; break;
            case 12: return V{ Move(12, 8), Move(12, 13)}; break;
            case 13: return V{ Move(13, 12), Move(13, 9), Move(13, 14) }; break;
            case 14: return V{ Move(14, 13), Move(14, 10), Move(14, 15) }; break;
            case 15: return V{ Move(15, 11), Move(15, 14)}; break;
            default: assert(false);
        }
        return {};
    }
    bool operator == (const FState& other) const
    {
        return configuration == other.configuration;
    }
    void Print() const
    {
        std::cout << std::endl;
        PrintItem(0);
        PrintItem(1);
        PrintItem(2);
        PrintItem(3);
        std::cout << std::endl;
        PrintItem(4);
        PrintItem(5);
        PrintItem(6);
        PrintItem(7);
        std::cout << std::endl;
        PrintItem(8);
        PrintItem(9);
        PrintItem(10);
        PrintItem(11);
        std::cout << std::endl;
        PrintItem(12);
        PrintItem(13);
        PrintItem(14);
        PrintItem(15);
        std::cout << std::endl;
    }
    void PrintMoves() const
    {
        for (auto move: moves)
            std::cout << move << ' ';
        std::cout << std::endl;
    }
    std::size_t Hash() const
    {
        return boost::hash_range(std::begin(configuration), std::end(configuration));
    }
private:
    void PrintItem(std::size_t pos) const
    {
        std::cout << std::setw(3);
        std::cout << configuration[pos];
    }
    FState Move(std::size_t pivot, std::size_t item) const
    {
        auto cfg = *this;
        std::swap(cfg.configuration[pivot], cfg.configuration[item]);
        cfg.moves.push_back(item);
        return cfg;
    }

    std::array<int, 16> configuration;
    std::vector<int> moves;
};

// custom specialization of std::hash can be injected in namespace std
namespace std
{
    template<> struct hash<FState>
    {
        typedef FState argument_type;
        typedef std::size_t result_type;
        result_type operator()(argument_type const& s) const noexcept
        {
            return s.Hash();
        }
    };
}

#include <boost/chrono/chrono.hpp>
#include <boost/chrono/process_cpu_clocks.hpp>

using namespace boost::chrono;

int main()
{
    //const FState start( std::array<int, 16>({1,12,6,4,9,7,11,10,15,3,2,13,5,8,14,0}) );
    const FState start( std::array<int, 16>({2,3,7,4,1,0,11,8,5,6,10,12,9,13,14,15}) );
    const FState goal;
    BreadthFirst<FState> search(start, goal);

    auto t0 = process_user_cpu_clock::now(); // boost

    bool found = search.Solve();

    auto t1 = process_user_cpu_clock::now(); // boost
    using ms = boost::chrono::milliseconds;
    ms d = boost::chrono::duration_cast<ms>(t1-t0);
    std::cout << d.count() << " ms" << std::endl;

    if (found)
    {
        std::cout << "Solution found:" << std::endl;
        search.Solution().PrintMoves();
    }
    else
    {
        std::cout << "Solution not found!" << std::endl;
    }
    search.PrintStatistics();

    return 0;
}


/// \file
/// \brief Unit tests for dfe::poly

#include <array>
#include <valarray>
#include <vector>

#include <boost/test/unit_test.hpp>

#include "dfe/dfe_poly.hpp"

// make std::pair printable to allow direct comparision w/ boost::test

namespace std {
template<typename T>
inline ostream&
operator<<(ostream& os, const pair<T, T>& x)
{
  os << "{" << x.first << ", " << x.second << "}";
  return os;
};
} // namespace std

// test different input container types

#define COEFFS \
  { \
    1.0, 2.0, 0.25, 0.025 \
  }
constexpr double X0 = 0.5;
constexpr double Y0 = 2.065625;
constexpr double D0 = 2.2687500000000003;
constexpr std::pair<double, double> YD0 = {Y0, D0};

BOOST_AUTO_TEST_CASE(poly_initializerlist)
{
  BOOST_TEST(dfe::polynomial_val(X0, COEFFS) == Y0);
  BOOST_TEST(dfe::polynomial_der(X0, COEFFS) == D0);
  BOOST_TEST(dfe::polynomial_valder(X0, COEFFS) == YD0);
}

BOOST_AUTO_TEST_CASE(poly_array)
{
  double coeffs[] = COEFFS;
  BOOST_TEST(dfe::polynomial_val(X0, coeffs) == Y0);
  BOOST_TEST(dfe::polynomial_der(X0, coeffs) == D0);
  BOOST_TEST(dfe::polynomial_valder(X0, coeffs) == YD0);
}

BOOST_AUTO_TEST_CASE(poly_stdarray)
{
  std::array<double, 4> coeffs = COEFFS;
  BOOST_TEST(dfe::polynomial_val(X0, coeffs) == Y0);
  BOOST_TEST(dfe::polynomial_der(X0, coeffs) == D0);
  BOOST_TEST(dfe::polynomial_valder(X0, coeffs) == YD0);
}

BOOST_AUTO_TEST_CASE(poly_stdvector)
{
  std::vector<double> coeffs = COEFFS;
  BOOST_TEST(dfe::polynomial_val(X0, coeffs) == Y0);
  BOOST_TEST(dfe::polynomial_der(X0, coeffs) == D0);
  BOOST_TEST(dfe::polynomial_valder(X0, coeffs) == YD0);
}

// use std::valarray to calculate polynomial for multiple x values at once

BOOST_AUTO_TEST_CASE(poly_valarray)
{
  std::valarray<float> x(1024);
  for (std::size_t i = 0; i < x.size(); ++i) {
    x[i] = -1.0 + (2.0 / x.size()) * i;
  }
  auto linear = dfe::polynomial_val(x, {0.0, 1.0});
  auto quadratic = dfe::polynomial_val(x, {0.5, 0.0, 1.0});

  BOOST_TEST(x.size() == linear.size());
  BOOST_TEST(x.size() == quadratic.size());
  BOOST_TEST((x - linear).sum() == 0.0);
  BOOST_TEST((0.5 + x * x - quadratic).sum() == 0.0);
}

// special case of empty coefficients

BOOST_AUTO_TEST_CASE(poly_empty)
{
  BOOST_TEST(dfe::polynomial_val(-1.0, std::array<double, 0>{}) == 0.0);
  BOOST_TEST(dfe::polynomial_val(+0.0, std::array<double, 0>{}) == 0.0);
  BOOST_TEST(dfe::polynomial_val(+1.0, std::array<double, 0>{}) == 0.0);
}

// test different fixed polynomial orders

BOOST_AUTO_TEST_CASE(poly_const)
{
  BOOST_TEST(dfe::polynomial_val(-1.0, {42.0}) == 42.0);
  BOOST_TEST(dfe::polynomial_val(+0.0, {42.0}) == 42.0);
  BOOST_TEST(dfe::polynomial_val(+1.0, {42.0}) == 42.0);
}

BOOST_AUTO_TEST_CASE(poly_linear)
{
  BOOST_TEST(dfe::polynomial_val(-0.5, {42.0, 1.0}) == 41.5);
  BOOST_TEST(dfe::polynomial_val(+0.0, {42.0, 1.0}) == 42.0);
  BOOST_TEST(dfe::polynomial_val(+0.5, {42.0, 1.0}) == 42.5);
}

BOOST_AUTO_TEST_CASE(poly_quadratic)
{
  BOOST_TEST(dfe::polynomial_val(-0.5, {42.0, 1.0, 0.5}) == 41.625);
  BOOST_TEST(dfe::polynomial_val(+0.0, {42.0, 1.0, 0.5}) == 42.0);
  BOOST_TEST(dfe::polynomial_val(+0.5, {42.0, 1.0, 0.5}) == 42.625);
}

BOOST_AUTO_TEST_CASE(poly_cubic)
{
  BOOST_TEST(dfe::polynomial_val(-0.5, {42.0, 1.0, 0.5, -1.0}) == 41.75);
  BOOST_TEST(dfe::polynomial_val(+0.0, {42.0, 1.0, 0.5, -1.0}) == 42.0);
  BOOST_TEST(dfe::polynomial_val(+0.5, {42.0, 1.0, 0.5, -1.0}) == 42.5);
}


#include <TriPQ/CGAL_Spherical_Polyhedron_Traits.h>
#include <TriPQ/SelectNearestEdge3.h>
#include <TriPQ/StartFromMostLocatedEdge.h>
#include <TriPQ/StartFromFixedEdge.h>
#include <TriPQ/TriPQ.h>

#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <CGAL/Polyhedron_items_with_id_3.h>
#include <CGAL/Polyhedron_incremental_builder_3.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Polyhedron_3.h>
#include <CGAL/Simple_cartesian.h>
#include <boost/iterator/transform_iterator.hpp>
#include <CGAL/IO/Polyhedron_iostream.h>

#include <array>
#include <cmath>
#include <chrono>
#include <random>
#include <vector>

typedef CGAL::Simple_cartesian<double> Kernel;
typedef CGAL::Polyhedron_items_with_id_3 Polyhedron_items;
typedef CGAL::Polyhedron_3<Kernel, Polyhedron_items> Polyhedron;
typedef CGAL::Triangulation_vertex_base_with_info_2<std::size_t, Kernel> Vb;
typedef CGAL::Triangulation_face_base_2<Kernel> Fb;
typedef CGAL::Triangulation_data_structure_2<Vb, Fb> Tds;
typedef CGAL::Delaunay_triangulation_2<Kernel, Tds> Delaunay;
typedef Kernel::Point_3 Point_3;
typedef Kernel::Point_2 Point_2;

Point_3 sphericalToCart(Point_2 const &s) {
  using std::sin;
  using std::cos;
  return Point_3(sin(s[0]) * cos(s[1]), sin(s[0]) * sin(s[1]), -cos(s[0]));
}

inline Point_2 PhiN(Point_3 const &x) {
  return Point_2(x[0] / (1.0 - x[2]), x[1] / (1.0 - x[2]));
}

std::vector<std::array<std::size_t, 3>>
triangulate(std::vector<Point_3> &points) {
  std::size_t vertexIndex = 0;
  auto const f = [&vertexIndex](auto const &p) {
    return std::make_pair(PhiN(p), vertexIndex++);
  };

  typedef boost::transform_iterator<
      decltype(f), std::vector<Point_3>::const_iterator> Iterator;

  // Delaunay dt(Iterator(points.cbegin(), f), Iterator(points.cend(), f));
  Delaunay dt;

  // ensure that no three points inserted to dt are collinear
  std::vector<std::pair<Point_2, std::size_t>> pt(Iterator(points.cbegin(), f),
                                                  Iterator(points.cend(), f));
  auto p0 = pt[0], p1 = pt[1];
  pt.erase(pt.begin());
  pt.erase(pt.begin());
  auto iter = pt.begin();
  while (!pt.empty()) {
    auto p2 = *iter;
    if (CGAL::collinear(p0.first, p1.first, p2.first)) {
      ++iter;
      continue;
    }
    dt.push_back(iter->first)->info() = iter->second;
    p0 = p1;
    p1 = p2;
    iter = pt.erase(iter);
    if (iter == pt.end()) iter = pt.begin();
  }

  std::size_t const N = points.size();
  std::vector<std::array<std::size_t, 3>> facets;
  facets.reserve(dt.number_of_faces());
  for (auto t = dt.all_faces_begin(); t != dt.all_faces_end(); ++t) {
    auto const v0 = t->vertex(0);
    auto const v1 = t->vertex(1);
    auto const v2 = t->vertex(2);
    auto const i0 = dt.is_infinite(v0) ? N : v0->info();
    auto const i1 = dt.is_infinite(v1) ? N : v1->info();
    auto const i2 = dt.is_infinite(v2) ? N : v2->info();
    facets.push_back({i0, i1, i2});
  }
  // The infinite point is the south pole
  points.emplace_back(0, 0, 1);

  return facets;
}

std::vector<Point_3> genPoints(std::size_t const N,
                               Point_2 offset = Point_2(0, 0)) {
  std::vector<Point_3> v;
  assert(N >= 2 && "N must be at leat 2");
  v.reserve(N * (N - 2) + 2);
  for (unsigned int i = 0; i <= N; ++i) {
    double const theta = offset[0] + M_PI * double(i) / double(N);
    for (unsigned int j = 0; j < N; ++j) {
      double const phi = offset[1] + 2.0 * M_PI * double(j) / double(N);
      Point_3 const pCart(sphericalToCart(Point_2(theta, phi)));
      v.push_back(pCart);
      if (i == 0 || i == N) break;
    }
  }
  return v;
}

Polyhedron constructPolyhedron() {
  typedef Polyhedron::HalfedgeDS HDS;
  auto v = genPoints(500);
  // remove north pole
  v.pop_back();
  auto const f = triangulate(v);

  struct Builder : public CGAL::Modifier_base<HDS> {
    std::vector<Point_3> const &v_;
    std::vector<std::array<std::size_t, 3>> const &f_;
    Builder(std::vector<Point_3> const &vv,
            std::vector<std::array<std::size_t, 3>> const &ff)
        : v_(vv), f_(ff) {}
    void operator()(HDS &hds) {
      CGAL::Polyhedron_incremental_builder_3<HDS> b(hds, true);
      std::size_t vIdx = 0;
      b.begin_surface(v_.size(), f_.size());
      for (auto const &p : v_) { b.add_vertex(p)->id() = vIdx++; }
      for (auto const &t : f_) {
        b.begin_facet();
        b.add_vertex_to_facet(t[2]);
        b.add_vertex_to_facet(t[1]);
        b.add_vertex_to_facet(t[0]);
        b.end_facet();
      }
      b.end_surface();
    }
  } builder(v, f);
  Polyhedron p;
  p.delegate(builder);
  return p;
}

static std::size_t comparisonCount = 0;

template <class P>
struct CountingTraits
    : public TriPQ::CGALSphericalPolyhedronTraitsBase<P, CountingTraits<P>> {
  typedef TriPQ::CGALSphericalPolyhedronTraits<P> Base;
  typedef CountingTraits<P> Self;
  struct IsRightOf {
    template <class Point>
    inline bool operator()(typename Self::Edge e, Point const &p) const {
      ++comparisonCount;
      return typename Base::IsRightOf()(e, p);
    }
  };
};

namespace std {

template <>
struct hash<typename CountingTraits<Polyhedron>::Edge>
    : public hash<void const *> {
  size_t operator()(typename CountingTraits<Polyhedron>::Edge e) const {
    return hash<void const *>()(static_cast<void const *>(&*e));
  }
};

} // namespace std

template <class Edge, class Point>
bool assertPointInTriangle(Edge e, Point const &x) {
  typedef TriPQ::CGALSphericalPolyhedronTraits<Polyhedron> T;

  if (typename T::IsRightOf()(e, x)) return false;
  if (typename T::IsRightOf()(e->next(), x)) return false;
  if (typename T::IsRightOf()(e->next()->next(), x)) return false;
  return true;
}

template <class Query, class Points>
void runQuery(Query const &q, Points const &p) {
  comparisonCount = 0;
  using Clock = std::chrono::high_resolution_clock;
  using TimePoint = std::chrono::time_point<Clock>;
  std::cout << "\tQuerying " << p.size() << " points..." << std::flush;
  TimePoint const start = Clock::now();

  auto const edges = q(p);

  using ms = std::chrono::milliseconds;
  TimePoint const end = Clock::now();
  auto const timeDiff = end - start;
  std::cout << "Done." << std::endl;
  std::cout << "\tTook " << std::chrono::duration_cast<ms>(timeDiff).count()
            << "ms" << std::endl;
  std::cout << "\tOn average " << double(comparisonCount) / double(p.size())
            << " comparisons" << std::endl;

  for (unsigned int i = 0; i < p.size(); ++i) {
    if (!assertPointInTriangle(edges[i], p[i])) {
      std::cerr << "Point not in triangle" << std::endl;
      std::cerr << "Point " << p[i] << std::endl;
      std::cerr << "Edge: (" << edges[i]->opposite()->vertex()->id() << ", "
                << edges[i]->vertex()->id() << ")" << std::endl;
      std::cerr << "Triangle " << edges[i]->vertex()->id() << ", "
                << edges[i]->next()->vertex()->id() << ", "
                << edges[i]->next()->next()->vertex()->id() << std::endl;
      exit(EXIT_FAILURE);
    }
  }
}

int main(int, char **) {
  std::random_device rd;
  std::mt19937 gen(rd());
  std::uniform_real_distribution<double> theta(0, M_PI);
  std::uniform_real_distribution<double> phi(0, 2.0 * M_PI);

  typedef TriPQ::PointQuery<CountingTraits<Polyhedron>,
                            TriPQ::StartFromFixedEdge,
                            TriPQ::RandomEdgeSelect> QueryFixedRandom;
  typedef TriPQ::PointQuery<CountingTraits<Polyhedron>,
                            TriPQ::StartFromFixedEdge,
                            TriPQ::SelectNearestEdge3> QueryFixedNearest;
  typedef TriPQ::PointQuery<CountingTraits<Polyhedron>,
                            TriPQ::StartFromLastEdge,
                            TriPQ::RandomEdgeSelect> QueryLastRandom;
  typedef TriPQ::PointQuery<CountingTraits<Polyhedron>,
                            TriPQ::StartFromLastEdge,
                            TriPQ::SelectNearestEdge3> QueryLastNearest;
  typedef TriPQ::PointQuery<CountingTraits<Polyhedron>,
                            TriPQ::StartFromMostLocatedEdge,
                            TriPQ::RandomEdgeSelect> QueryMRRandom;
  typedef TriPQ::PointQuery<CountingTraits<Polyhedron>,
                            TriPQ::StartFromMostLocatedEdge,
                            TriPQ::SelectNearestEdge3> QueryMRNearest;
  typedef TriPQ::PointQuery<CountingTraits<Polyhedron>,
                            TriPQ::StartFromMostLocatedEdgeUnordered,
                            TriPQ::RandomEdgeSelect> QueryMRURandom;
  typedef TriPQ::PointQuery<CountingTraits<Polyhedron>,
                            TriPQ::StartFromMostLocatedEdgeUnordered,
                            TriPQ::SelectNearestEdge3> QueryMRUNearest;

  std::size_t const N = 40000;
  // std::size_t const N = 4;

  // Generate polyhedron
  std::cout << "Constructing triangulation... " << std::flush;
  auto const p = constructPolyhedron();
  std::cout << "done." << std::endl;

  std::vector<Point_3> randomPoints(N - 1);
  // generate query points
  for (unsigned int i = 0; i < N - 1; ++i) {
    randomPoints[i] = sphericalToCart(Point_2(theta(gen), phi(gen)));
  }

  auto const sequencialPoints =
      genPoints(std::sqrt(N), Point_2(theta(gen), phi(gen)));

  auto const e0 = p.halfedges_begin();

  std::cout << "Triangulation dimensions:" << std::endl;
  std::cout << "\t" << p.size_of_vertices() << " vertices" << std::endl;
  std::cout << "\t" << p.size_of_facets() << " triangles" << std::endl;
  std::cout << "\t" << p.size_of_halfedges() / 2 << " edges" << std::endl;

  std::cout << "Fixed starting edge, random edge select" << std::endl;
  std::cout << "Random points" << std::endl;
  runQuery(QueryFixedRandom(e0), randomPoints);
  std::cout << "Sequencial points" << std::endl;
  runQuery(QueryFixedRandom(e0), sequencialPoints);

  std::cout << "Fixed starting edge, nearest edge" << std::endl;
  std::cout << "Random points" << std::endl;
  runQuery(QueryFixedNearest(e0), randomPoints);
  std::cout << "Sequencial points" << std::endl;
  runQuery(QueryFixedNearest(e0), sequencialPoints);

  std::cout << "Last starting edge, random edge" << std::endl;
  std::cout << "Random points" << std::endl;
  runQuery(QueryLastRandom(e0), randomPoints);
  std::cout << "Sequencial points" << std::endl;
  runQuery(QueryLastRandom(e0), sequencialPoints);

  std::cout << "Last starting edge, nearest edge" << std::endl;
  std::cout << "Random points" << std::endl;
  runQuery(QueryLastNearest(e0), randomPoints);
  std::cout << "Sequencial points" << std::endl;
  runQuery(QueryLastNearest(e0), sequencialPoints);

  std::cout << "Most located starting edge, random edge" << std::endl;
  std::cout << "Random points" << std::endl;
  runQuery(QueryMRRandom(e0), randomPoints);
  std::cout << "Sequencial points" << std::endl;
  runQuery(QueryMRRandom(e0), sequencialPoints);

  std::cout << "Most located starting edge, nearest edge" << std::endl;
  std::cout << "Random points" << std::endl;
  runQuery(QueryMRNearest(e0), randomPoints);
  std::cout << "Sequencial points" << std::endl;
  runQuery(QueryMRNearest(e0), sequencialPoints);

  std::cout << "Most located starting edge (unordered map), random edge"
            << std::endl;
  std::cout << "Random points" << std::endl;
  runQuery(QueryMRUNearest(e0), randomPoints);
  std::cout << "Sequencial points" << std::endl;
  runQuery(QueryMRURandom(e0), sequencialPoints);

  std::cout << "Most located starting edge (unordered map), nearest edge"
            << std::endl;
  std::cout << "Random points" << std::endl;
  runQuery(QueryMRUNearest(e0), randomPoints);
  std::cout << "Sequencial points" << std::endl;
  runQuery(QueryMRUNearest(e0), sequencialPoints);

  return EXIT_SUCCESS;
}


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>

TEST(MathFunctions, digamma) {
  EXPECT_FLOAT_EQ(boost::math::digamma(0.5), stan::math::digamma(0.5));
  EXPECT_FLOAT_EQ(boost::math::digamma(-1.5), stan::math::digamma(-1.5));
}

TEST(MathFunctions, digamma_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::digamma(nan));

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::digamma(-1));
}


#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

int module2()
{
    BZ_USING_NAMESPACE(blitz::tensor)

    Array<int,1> A(4);
    A = pow2(i);
    return 0;
}


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

#ifndef CRYPTO3_MATH_EVALUATE_HPP
#define CRYPTO3_MATH_EVALUATE_HPP

#include <algorithm>
#include <vector>

#include <boost/math/tools/polynomial.hpp>

namespace nil {
    namespace crypto3 {
        namespace math {
            /*!
             * @brief
             * Naive evaluation of a *single* polynomial, used for testing purposes.
             *
             * The inputs are:
             * - an integer m
             * - a vector coeff representing monomial P of size m
             * - a field element element t
             * The output is the polynomial P(x) evaluated at x = t.
             */
            template<typename FieldValueType, typename ContiguousIterator>
            inline FieldValueType evaluate_polynomial(ContiguousIterator first, ContiguousIterator last,
                                                      const FieldValueType &t, std::size_t m) {
                BOOST_ASSERT(std::distance(first, last) == m);

                return boost::math::tools::evaluate_polynomial(&*first, t, m);
            }

            template<typename FieldValueType, typename ContiguousContainer>
            inline FieldValueType evaluate_polynomial(const ContiguousContainer &coeff, const FieldValueType &t,
                                                      std::size_t m) {
                return evaluate_polynomial(coeff.begin(), coeff.end(), t, m);
            }

            /*!
             * @brief
             * Naive evaluation of a *single* Lagrange polynomial, used for testing purposes.
             *
             * The inputs are:
             * - an integer m
             * - a domain S = (a_{0},...,a_{m-1}) of size m
             * - a field element element t
             * - an index idx in {0,...,m-1}
             * The output is the polynomial L_{idx,S}(z) evaluated at z = t.
             */
            template<typename FieldValueType, typename InputIterator>
            inline FieldValueType evaluate_lagrange_polynomial(InputIterator first, InputIterator last,
                                                               const FieldValueType &t, std::size_t m,
                                                               std::size_t idx) {
                typedef typename std::iterator_traits<InputIterator>::value_type value_type;

                BOOST_STATIC_ASSERT(std::is_same<value_type, FieldValueType>::value);

                if (m != std::distance(first, last)) {
                    throw std::invalid_argument("expected m == domain.size()");
                }
                if (idx >= m) {
                    throw std::invalid_argument("expected idx < m");
                }

                value_type num = value_type::one();
                value_type denom = value_type::one();

                for (std::size_t k = 0; k < m; ++k) {
                    if (k == idx) {
                        continue;
                    }

                    num *= t - *(first + k);
                    denom *= *(first + idx) - *(first + k);
                }

                return num * denom.inversed();
            }

            template<typename FieldValueType, typename Range>
            inline FieldValueType evaluate_lagrange_polynomial(const Range &domain, const FieldValueType &t,
                                                               std::size_t m, std::size_t idx) {
                typedef FieldValueType value_type;
                BOOST_STATIC_ASSERT(std::is_same<value_type, typename std::iterator_traits<decltype(std::begin(
                                                                 std::declval<Range>()))>::value_type>::value);

                return evaluate_lagrange_polynomial(domain.begin(), domain.end(), t, m, idx);
            }
        }    // namespace math
    }        // namespace crypto3
}    // namespace nil

#endif    // ALGEBRA_FFT_NAIVE_EVALUATE_HPP