/*-
 * SPDX-License-Identifier: BSD-2-Clause
 *
 * Copyright (c) 2020 NKI/AVL, Netherlands Cancer Institute
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * THIS 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 THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#pragma once

#include <functional>

#if HAVE_LIBCLIPPER
#include <clipper/core/coords.h>
#endif

#include <boost/math/quaternion.hpp>

namespace mmcif
{

typedef boost::math::quaternion<float>	Quaternion;

const double
	kPI = 3.141592653589793238462643383279502884;

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

//	Point, a location with x, y and z coordinates as floating point.
//	This one is derived from a tuple<float,float,float> so
//	you can do things like:
//
//	float x, y, z;
//	tie(x, y, z) = atom.loc();

template<typename F>
struct PointF
{
	typedef F FType;

	FType mX, mY, mZ;

	PointF()							: mX(0), mY(0), mZ(0) {}
	PointF(FType x, FType y, FType z)	: mX(x), mY(y), mZ(z) {}

	template<typename PF>
	PointF(const PointF<PF>& pt)
		: mX(static_cast<F>(pt.mX))
		, mY(static_cast<F>(pt.mY))
		, mZ(static_cast<F>(pt.mZ)) {}

#if HAVE_LIBCLIPPER
	PointF(const clipper::Coord_orth& pt): mX(pt[0]), mY(pt[1]), mZ(pt[2]) {}

	PointF& operator=(const clipper::Coord_orth& rhs)
	{
		mX = rhs[0];
		mY = rhs[1];
		mZ = rhs[2];
		return *this;
	}
#endif

	template<typename PF>
	PointF& operator=(const PointF<PF>& rhs)
	{
		mX = static_cast<F>(rhs.mX);
		mY = static_cast<F>(rhs.mY);
		mZ = static_cast<F>(rhs.mZ);
		return *this;
	}

	FType& getX()			{ return mX; }
	FType getX() const		{ return mX; }
	void setX(FType x)		{ mX = x; }

	FType& getY()			{ return mY; }
	FType getY() const		{ return mY; }
	void setY(FType y)		{ mY = y; }

	FType& getZ()			{ return mZ; }
	FType getZ() const		{ return mZ; }
	void setZ(FType z)		{ mZ = z; }

	PointF& operator+=(const PointF& rhs)
	{
		mX += rhs.mX;
		mY += rhs.mY;
		mZ += rhs.mZ;

		return *this;
	}

	PointF& operator+=(FType d)
	{
		mX += d;
		mY += d;
		mZ += d;

		return *this;
	}

	PointF& operator-=(const PointF& rhs)
	{
		mX -= rhs.mX;
		mY -= rhs.mY;
		mZ -= rhs.mZ;

		return *this;
	}

	PointF& operator-=(FType d)
	{
		mX -= d;
		mY -= d;
		mZ -= d;

		return *this;
	}

	PointF& operator*=(FType rhs)
	{
		mX *= rhs;
		mY *= rhs;
		mZ *= rhs;
		return *this;
	}

	PointF& operator/=(FType rhs)
	{
		mX /= rhs;
		mY /= rhs;
		mZ /= rhs;
		return *this;
	}

	FType normalize()
	{
		auto length = mX * mX + mY * mY + mZ * mZ;
		if (length > 0)
		{
			length = std::sqrt(length);
			operator/=(length);
		}
		return length;
	}

	void rotate(const boost::math::quaternion<FType>& q)
	{
		boost::math::quaternion<FType> p(0, mX, mY, mZ);

		p = q * p * boost::math::conj(q);

		mX = p.R_component_2();
		mY = p.R_component_3();
		mZ = p.R_component_4();
	}

#if HAVE_LIBCLIPPER
	operator clipper::Coord_orth() const
	{
		return clipper::Coord_orth(mX, mY, mZ);
	}
#endif

	operator std::tuple<const FType&, const FType&, const FType&>() const
	{
		return std::make_tuple(std::ref(mX), std::ref(mY), std::ref(mZ));
	}

	operator std::tuple<FType&,FType&,FType&>()
	{
		return std::make_tuple(std::ref(mX), std::ref(mY), std::ref(mZ));
	}

	bool operator==(const PointF& rhs) const
	{
		return mX == rhs.mX and mY == rhs.mY and mZ == rhs.mZ;
	}

	// consider point as a vector... perhaps I should rename Point?
	FType lengthsq() const
	{
		return mX * mX + mY * mY + mZ * mZ;
	}

	FType length() const
	{
		return sqrt(mX * mX + mY * mY + mZ * mZ);
	}
};

typedef PointF<float> Point;
typedef PointF<double> DPoint;

template<typename F>
inline std::ostream& operator<<(std::ostream& os, const PointF<F>& pt)
{
	os << '(' << pt.mX << ',' << pt.mY << ',' << pt.mZ << ')';
	return os;
}

template<typename F>
inline PointF<F> operator+(const PointF<F>& lhs, const PointF<F>& rhs)
{
	return PointF<F>(lhs.mX + rhs.mX, lhs.mY + rhs.mY, lhs.mZ + rhs.mZ);
}

template<typename F>
inline PointF<F> operator-(const PointF<F>& lhs, const PointF<F>& rhs)
{
	return PointF<F>(lhs.mX - rhs.mX, lhs.mY - rhs.mY, lhs.mZ - rhs.mZ);
}

template<typename F>
inline PointF<F> operator-(const PointF<F>& pt)
{
	return PointF<F>(-pt.mX, -pt.mY, -pt.mZ);
}

template<typename F>
inline PointF<F> operator*(const PointF<F>& pt, F f)
{
	return PointF<F>(pt.mX * f, pt.mY * f, pt.mZ * f);
}

template<typename F>
inline PointF<F> operator*(F f, const PointF<F>& pt)
{
	return PointF<F>(pt.mX * f, pt.mY * f, pt.mZ * f);
}

template<typename F>
inline PointF<F> operator/(const PointF<F>& pt, F f)
{
	return PointF<F>(pt.mX / f, pt.mY / f, pt.mZ / f);
}

// --------------------------------------------------------------------
// several standard 3d operations

template<typename F>
inline double DistanceSquared(const PointF<F>& a, const PointF<F>& b)
{
	return
		(a.mX - b.mX) * (a.mX - b.mX) +
		(a.mY - b.mY) * (a.mY - b.mY) +
		(a.mZ - b.mZ) * (a.mZ - b.mZ);
}

template<typename F>
inline double Distance(const PointF<F>& a, const PointF<F>& b)
{
	return sqrt(
		(a.mX - b.mX) * (a.mX - b.mX) +
		(a.mY - b.mY) * (a.mY - b.mY) +
		(a.mZ - b.mZ) * (a.mZ - b.mZ));
}

template<typename F>
inline F DotProduct(const PointF<F>& a, const PointF<F>& b)
{
	return a.mX * b.mX + a.mY * b.mY + a.mZ * b.mZ;
}

template<typename F>
inline PointF<F> CrossProduct(const PointF<F>& a, const PointF<F>& b)
{
	return PointF<F>(a.mY * b.mZ - b.mY * a.mZ,
				  a.mZ * b.mX - b.mZ * a.mX,
				  a.mX * b.mY - b.mX * a.mY);
}

template<typename F>
double Angle(const PointF<F>& p1, const PointF<F>& p2, const PointF<F>& p3)
{
	PointF<F> v1 = p1 - p2;
	PointF<F> v2 = p3 - p2;

	return std::acos(DotProduct(v1, v2) / (v1.length() * v2.length())) * 180 / kPI;
}

template<typename F>
double DihedralAngle(const PointF<F>& p1, const PointF<F>& p2, const PointF<F>& p3, const PointF<F>& p4)
{
	PointF<F> v12 = p1 - p2;	// vector from p2 to p1
	PointF<F> v43 = p4 - p3;	// vector from p3 to p4

	PointF<F> z = p2 - p3;		// vector from p3 to p2

	PointF<F> p = CrossProduct(z, v12);
	PointF<F> x = CrossProduct(z, v43);
	PointF<F> y = CrossProduct(z, x);

	double u = DotProduct(x, x);
	double v = DotProduct(y, y);

	double result = 360;
	if (u > 0 and v > 0)
	{
		u = DotProduct(p, x) / sqrt(u);
		v = DotProduct(p, y) / sqrt(v);
		if (u != 0 or v != 0)
			result = atan2(v, u) * 180 / kPI;
	}

	return result;
}

template<typename F>
double CosinusAngle(const PointF<F>& p1, const PointF<F>& p2, const PointF<F>& p3, const PointF<F>& p4)
{
	PointF<F> v12 = p1 - p2;
	PointF<F> v34 = p3 - p4;

	double result = 0;

	double x = DotProduct(v12, v12) * DotProduct(v34, v34);
	if (x > 0)
		result = DotProduct(v12, v34) / sqrt(x);

	return result;
}

template<typename F>
auto DistancePointToLine(const PointF<F> &l1, const PointF<F> &l2, const PointF<F> &p)
{
	auto line       = l2 - l1;
    auto p_to_l1    = p - l1;
    auto p_to_l2    = p - l2;
    auto cross      = CrossProduct(p_to_l1, p_to_l2);
    return cross.length() / line.length();
}

// --------------------------------------------------------------------
// For e.g. simulated annealing, returns a new point that is moved in
// a random direction with a distance randomly chosen from a normal
// distribution with a stddev of offset.

template<typename F>
PointF<F> Nudge(PointF<F> p, F offset);

// --------------------------------------------------------------------
// We use quaternions to do rotations in 3d space

Quaternion Normalize(Quaternion q);

std::tuple<double,Point> QuaternionToAngleAxis(Quaternion q);
Point Centroid(std::vector<Point>& Points);
Point CenterPoints(std::vector<Point>& Points);
Quaternion AlignPoints(const std::vector<Point>& a, const std::vector<Point>& b);
double RMSd(const std::vector<Point>& a, const std::vector<Point>& b);

// --------------------------------------------------------------------
// Helper class to generate evenly divided Points on a sphere
// we use a fibonacci sphere to calculate even distribution of the dots

template<int N>
class SphericalDots
{
  public:
	enum { P = 2 * N + 1 };
	typedef typename std::array<Point,P>	array_type;
	typedef typename array_type::const_iterator	iterator;

	static SphericalDots& instance()
	{
		static SphericalDots sInstance;
		return sInstance;
	}

	size_t size() const							{ return mPoints.size(); }
	const Point operator[](uint32_t inIx) const	{ return mPoints[inIx]; }
	iterator begin() const						{ return mPoints.begin(); }
	iterator end() const						{ return mPoints.end(); }

	double weight() const						{ return mWeight; }

	SphericalDots()
	{

		const double
			kGoldenRatio = (1 + std::sqrt(5.0)) / 2;

		mWeight = (4 * kPI) / P;

		auto p = mPoints.begin();

		for (int32_t i = -N; i <= N; ++i)
		{
			double lat = std::asin((2.0 * i) / P);
			double lon = std::fmod(i, kGoldenRatio) * 2 * kPI / kGoldenRatio;

			p->mX = sin(lon) * cos(lat);
			p->mY = cos(lon) * cos(lat);
			p->mZ =            sin(lat);

			++p;
		}
	}

  private:

	array_type				mPoints;
	double					mWeight;
};

typedef SphericalDots<50> SphericalDots_50;

}


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

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

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

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

 /*!

    @ingroup group-ieee
    Function object implementing bitinteger capabilities

    Transforms a floating point value in a pattern of bits
    stored in an integer with different formulas according to
    the floating point @ref bitofsign (the converse of @ref bitfloating)

    @par Semantic:

    @code
     as_integer_t<T> r = bitinteger(x);
    @endcode

    is similar to:

    @code
      as_integer_t<T> r =
         (is_positive(x) ?
           bitwise_cast<as_integer_t<T>>(x) :
           Signmask<as_integer_t<T>>()-bitwise_cast<as_integer_t<T>>(x);
    @endcode

    @par Note:
    This is an utility function related to the computation of floating
    successors or predecessors

    @see next, prev, successor,  predecessor,  nextafter

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

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

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "boost::simd::meta double"

#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/meta/double.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>

////////////////////////////////////////////////////////////////////////////////
// Test value of real_c constants for every base real types
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE( double_ )
{
  using boost::simd::meta::double_;

  double_<0x3FF0000000000000ULL>          one_dc;
  double one_d = 1.;

  NT2_TEST_EQUAL( one_d, double(one_dc)  );
}


#ifndef VPYTHON_PYTHON_SCALAR_ARRAY_HPP
#define VPYTHON_PYTHON_SCALAR_ARRAY_HPP

// This alternative to numpy is not currently being used
// but is retained in CVS for possible future use.
// In particular, information about array objects such
// as curve cannot currently be cached because we don't
// know when a numpy pos array has been changed.

// Copyright (c) 2000, 2001, 2002, 2003 by David Scherer and others.
// See the file license.txt for complete license terms.
// See the file authors.txt for a complete list of contributors.

#include "util/vector.hpp"
#include <deque>

#include <boost/python/numeric.hpp>

namespace cvisual { namespace python {

class vector_array;

class scalar_array
{
 private:
	std::deque<double> data;
	friend class vector_array;

 public:
	typedef std::deque<double>::iterator iterator;
	typedef std::deque<double>::const_iterator const_iterator;

	inline scalar_array( int size = 0, double fill = 0)
		: data( size, fill) {}

	// Construct from a continuous 1-D sequence (tuple or list)
	explicit scalar_array( const boost::python::list& sequence);
	explicit scalar_array( const boost::python::numeric::array& sequence);

	inline scalar_array( const scalar_array& other)
		: data( other.data) {}

	boost::python::handle<PyObject>
	as_array() const;

	inline iterator
	begin() { return data.begin(); }

	inline const_iterator
	begin() const { return data.begin(); }

	inline iterator
	end() { return data.end(); }

	inline const_iterator
	end() const { return data.end(); }

	// Append a single element to the array.
	void append( double s);

	// Prepend a single element the the array.
	void prepend( double s);

	// Remove a single element from the beginning of the array
	void head_clip();
	// Remove i elemnts from the beginning of the array.
	void head_crop( int i);
	// Remove a single element from the end of the array.
	void tail_clip();
	// Remove i elements from the end of the array.
	void tail_crop( int i);

	scalar_array
	operator*( double s) const;

	scalar_array
	operator*( const scalar_array& s) const;

	vector_array
	operator*( const vector_array& v) const;

	vector_array
	operator*( const vector& v) const;

	const scalar_array&
	operator*=( double s);

	const scalar_array&
	operator*=( const scalar_array& s);

	scalar_array
	operator/( double s) const;

	scalar_array
	operator/( const scalar_array& s) const;

	const scalar_array&
	operator/=( double s);

	const scalar_array&
	operator/=( const scalar_array& s);

	scalar_array
	operator+( const scalar_array& s) const;

	scalar_array
	operator+( double s) const;

	const scalar_array&
	operator+=( double s);

	const scalar_array&
	operator+=( const scalar_array& s);

	scalar_array
	operator-( const scalar_array& s) const;

	scalar_array
	operator-( double s) const;

	const scalar_array&
	operator-=( double s);

	const scalar_array&
	operator-=( const scalar_array& s);

	scalar_array
	operator-() const;

	inline double&
	operator[]( int i) { return data[i]; }

	inline const double&
	operator[]( int i) const { return data[i]; }

	// Returns the number of elemnts in the array.
	inline int
	size() const { return data.size(); }

	double
	py_getitem( int index);

	void
	py_setitem( int index, double value);

	double
	sum() const;

};

void wrap_scalar_array();

} } // !namepace cvisual::python

#endif // !VPYTHON_PYTHON_SCALAR_ARRAY_HPP


//#include <dirent.h>
//#include <iostream>
//#include <vector>
//#include <string>
//#include <utility>
//
//#include <Eigen/Core>
//#include <Eigen/LU>
//#include <Eigen/Geometry>
//
//#include <opencv2/opencv.hpp>
//#define pi acos(-1)
//
//const int G_land_num = 74;
//const int G_train_pic_id_num = 3300;
//const int G_nShape = 47;
//const int G_nVerts = 11510;
//const int G_nFaces = 11540;
//const int G_test_num = 77;
//const int G_iden_num = 77;
//const int G_inner_land_num = 59;
//const int G_line_num = 50;
//const int G_jaw_land_num = 20;
//#define normalization
//struct Target_type {
//	Eigen::VectorXf exp;
//	Eigen::RowVector3f tslt;
//	Eigen::Matrix3f rot;
//	Eigen::MatrixX2f dis;
//	Eigen::Vector3f angle;
//};
//
//struct DataPoint
//{
//	cv::Mat image;
//	cv::Rect face_rect;
//	std::vector<cv::Point2d> landmarks;
//	//std::vector<cv::Point2d> init_shape;
//	Target_type shape, init_shape;
//	Eigen::VectorXf user;
//	Eigen::RowVector2f center;
//	Eigen::MatrixX2f land_2d;
//#ifdef posit
//	float f;
//#endif // posit
//#ifdef normalization
//	Eigen::MatrixX3f s;
//#endif
//
//	Eigen::VectorXi land_cor;
//};
//
//
//void load_lv(std::string name, DataPoint &temp) {
//	std::cout << "load coefficients...file:" << name << "\n";
//	FILE *fp;
//	fopen_s(&fp, name.c_str(), "rb");
//
//	temp.user.resize(G_iden_num);
//	for (int j = 0; j < G_iden_num; j++)
//		fread(&temp.user(j), sizeof(float), 1, fp);
//	std::cout << temp.user << "\n";
//	//system("pause");
//	temp.land_2d.resize(G_land_num, 2);
//	for (int i_v = 0; i_v < G_land_num; i_v++) {
//		fread(&temp.land_2d(i_v, 0), sizeof(float), 1, fp);
//		fread(&temp.land_2d(i_v, 1), sizeof(float), 1, fp);
//	}
//
//
//	fread(&temp.center(0), sizeof(float), 1, fp);
//	fread(&temp.center(1), sizeof(float), 1, fp);
//
//	temp.shape.exp.resize(G_nShape);
//	for (int i_shape = 0; i_shape < G_nShape; i_shape++)
//		fread(&temp.shape.exp(i_shape), sizeof(float), 1, fp);
//
//	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
//		fread(&temp.shape.rot(i, j), sizeof(float), 1, fp);
//
//	for (int i = 0; i < 3; i++) fread(&temp.shape.tslt(i), sizeof(float), 1, fp);
//
//	temp.land_cor.resize(G_land_num);
//	for (int i_v = 0; i_v < G_land_num; i_v++) fread(&temp.land_cor(i_v), sizeof(int), 1, fp);
//
//	temp.s.resize(2, 3);
//	for (int i = 0; i < 2; i++) for (int j = 0; j < 3; j++)
//		fread(&temp.s(i, j), sizeof(float), 1, fp);
//
//	temp.shape.dis.resize(G_land_num, 2);
//	for (int i_v = 0; i_v < G_land_num; i_v++) {
//		fread(&temp.shape.dis(i_v, 0), sizeof(float), 1, fp);
//		fread(&temp.shape.dis(i_v, 1), sizeof(float), 1, fp);
//	}
//	std::cout << temp.shape.dis << "\n";
//	//system("pause");
//	fclose(fp);
//	puts("load successful!");
//}
//
//
////assume the be could not be more than 90
//void cal_uler_angle(Eigen::Matrix3f R) {
//	Eigen::Vector3f x, y, z,t;
//	x = R.row(0).transpose();
//	y = R.row(1).transpose();
//	z = R.row(2).transpose();
//	float al, be, ga, gaw;
//	if (fabs(1 - z(2)*z(2)) < 1e-3) {
//		ga=gaw=be = 0;
//		al = acos(x(0));
//		if (y(0) < 0) al = 2 * pi - al;
//	}
//	else {
//
//		be = acos(z(2));
//		al = acos(std::max(std::min(float(1.0),z(1) / sqrt(1 - z(2)*z(2))),float(-1.0)));
//
//		if (z(0) < 0) al = 2 * pi - al;//according to the sin(al)
//
//
//		t(0) = cos(al), t(1) = sin(al), t(2) = 0;
//		t.normalize();
//		x.normalize();
//		//t.normalized();
//		ga = acos(t.dot(x));
//		gaw = acos(std::max(std::min(float(1.0), -y(2) / sqrt(1 - z(2)*z(2))), float(-1.0)));
//
//		printf("%.10f %.10f %.10f\n", -y(2), sqrt(1 - z(2)*z(2)), -y(2) / sqrt(1 - z(2)*z(2)));
//		if (x(2) < 0) ga = 2 * pi - ga, gaw = 2 * pi - gaw;//according to the sin(ga)
//	}
//	std::cout << R << "\n----------------------\n";
//	printf("%.10f %.10f %.10f %.10f %.10f\n",z(2), al/pi*180, be / pi * 180, ga / pi * 180, gaw / pi * 180);
//	system("pause");
//}
//
//Eigen::Matrix3f get_r_from_angle(float angle, int axis) {
//	Eigen::Matrix3f ans;
//	ans.setZero();
//	ans(axis, axis) = 1;
//	int idx_x = 0, idx_y = 1;
//	if (axis == 0)
//		idx_x = 1, idx_y = 2;
//	else
//		if (axis == 2)
//			idx_x = 0, idx_y = 1;
//		else
//			idx_x = 0, idx_y = 2;
//	ans(idx_x, idx_x) = cos(angle), ans(idx_x, idx_y) = -sin(angle), ans(idx_y, idx_x) = sin(angle), ans(idx_y, idx_y) = cos(angle);
//	return ans;
//}
//
//
//Eigen::Matrix3f get_r_from_angle(const Eigen::Vector3f &angle) {
//	Eigen::Matrix3f ans;
//	float Sa = sin(angle(0)), Ca = cos(angle(0)), Sb = sin(angle(1)),
//		Cb = cos(angle(1)), Sc = sin(angle(2)), Cc = cos(angle(2));
//
//	ans(0, 0) = Ca * Cc - Sa * Cb*Sc;
//	ans(0, 1) = -Sa * Cc - Ca * Cb*Sc;
//	ans(0, 2) = Sb * Sc;
//	ans(1, 0) = Ca * Sc + Sa * Cb*Cc;
//	ans(1, 1) = -Sa * Sc + Ca * Cb*Cc;
//	ans(1, 2) = -Sb * Cc;
//	ans(2, 0) = Sa * Sb;
//	ans(2, 1) = Ca * Sb;
//	ans(2, 2) = Cb;
//	return ans;
//}
//
////assume the be could not be more than 90
//Eigen::Vector3f cal_uler_angle_zyx(Eigen::Matrix3f R) {
//	Eigen::Vector3f x, y, z, t;
//	x = R.row(0).transpose();
//	y = R.row(1).transpose();
//	z = R.row(2).transpose();
//	float al, be, ga;
//	if (fabs(1 - x(2)*x(2)) < 1e-3) {
//		be = asin(x(2));
//		al = ga = 0;
//		exit(1);
//	}
//	else {
//
//		be = asin(std::max(std::min(1.0, double(x(2))), -1.0));
//		al = asin(std::max(std::min(1.0, double(-x(1) / sqrt(1 - x(2)*x(2)))), -1.0));
//		ga = asin(std::max(std::min(1.0, double(-y(2) / sqrt(1 - x(2)*x(2)))), -1.0));
//
//	}
//	std::cout << R << "\n----------------------\n";
//	printf("%.10f %.10f %.10f %.10f\n", x(2), al / pi * 180, be / pi * 180, ga / pi * 180);
//	Eigen::Vector3f ans;
//	ans << al, be, ga;
//	return ans;
//	//system("pause");
//}
//
//Eigen::Matrix3f get_r_from_angle_zyx(const Eigen::Vector3f &angle) {
//	Eigen::Matrix3f ans;
//	float Sa = sin(angle(0)), Ca = cos(angle(0)), Sb = sin(angle(1)),
//		Cb = cos(angle(1)), Sc = sin(angle(2)), Cc = cos(angle(2));
//
//	ans(0, 0) = Ca * Cb;
//	ans(0, 1) = -Sa * Cb;
//	ans(0, 2) = Sb;
//	ans(1, 0) = Sa * Cc + Ca * Sb*Sc;
//	ans(1, 1) = Ca * Cc - Sa * Sb*Sc;
//	ans(1, 2) = -Cb * Sc;
//	ans(2, 0) = Sa * Sc - Ca * Sb*Cc;
//	ans(2, 1) = Ca * Sc + Sa * Sb*Cc;
//	ans(2, 2) = Cb * Cc;
//	return ans;
//}
//
//
//
//
//void test_r(DataPoint data) {
//	Eigen::Matrix3f rot;
//	//rot = get_r_from_angle(data.shape.tslt(2), 2)*get_r_from_angle(data.shape.tslt(1), 0)*get_r_from_angle(data.shape.tslt(0), 2);
//	//std::cout << rot << "\n";
//	std::cout << get_r_from_angle_zyx(data.shape.angle) << "\n";
//	system("pause");
//}
//
//int main() {
//
//	DataPoint data;
//	/*data.shape.angle << 1, 20, 0.5;
//	test_r(data);*/
//
//
//	load_lv("data/lv_mp4_1.lv",data);//data/test_debug_lv_005_04_03_051_05
//	//lv_mp4_1.
//	data.shape.angle=cal_uler_angle_zyx(data.shape.rot);
//	test_r(data);
//	return 0;
//}
////g++ -Wall -std=c++11 `pkg-config --cflags opencv` -o deal deal_falut.cpp `pkg-config --libs opencv`


#include <CGAL/Simple_cartesian.h>
#include <CGAL/Polyhedron_3.h>
#include <CGAL/Polyhedron_items_with_id_3.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/boost/graph/graph_traits_Linear_cell_complex_for_combinatorial_map.h>

#include <boost/graph/graph_concepts.hpp>
#include <CGAL/boost/graph/graph_concepts.h>

typedef CGAL::Simple_cartesian<double> Kernel;
typedef Kernel::Point_3 Point_3;

template <typename Traits>
struct My_mesh_1 : public CGAL::Polyhedron_3<Traits, CGAL::Polyhedron_items_with_id_3> {};

struct My_mesh_2 : public CGAL::Polyhedron_3<Kernel, CGAL::Polyhedron_items_with_id_3> {};

template <typename PT>
struct My_mesh_3 : public CGAL::Surface_mesh<PT> {};

struct My_mesh_5 : public CGAL::Surface_mesh<Kernel::Point_3> {};

// dim could be hard-coded but for the purpose of the example it is left
template <int dim, typename K>
struct My_mesh_4 :
  CGAL::Linear_cell_complex_for_bgl_combinatorial_map_helper
         <2, dim, CGAL::Linear_cell_complex_traits<dim, K> >::type
{};

/// make My_mesh_1 a valid face graph model
#define CGAL_GRAPH_TRAITS_INHERITANCE_TEMPLATE_PARAMS typename Traits
#define CGAL_GRAPH_TRAITS_INHERITANCE_CLASS_NAME My_mesh_1<Traits>
#define CGAL_GRAPH_TRAITS_INHERITANCE_BASE_CLASS_NAME CGAL::Polyhedron_3<Traits, CGAL::Polyhedron_items_with_id_3>
#include <CGAL/boost/graph/graph_traits_inheritance_macros.h>

/// make My_mesh_2 a valid face graph model
// no template parameter, CGAL_GRAPH_TRAITS_INHERITANCE_TEMPLATE_PARAMS is then not defined
#define CGAL_GRAPH_TRAITS_INHERITANCE_CLASS_NAME My_mesh_2
#define CGAL_GRAPH_TRAITS_INHERITANCE_BASE_CLASS_NAME CGAL::Polyhedron_3<Kernel, CGAL::Polyhedron_items_with_id_3>
#include <CGAL/boost/graph/graph_traits_inheritance_macros.h>

/// make My_mesh_3 a valid face graph model
#define CGAL_GRAPH_TRAITS_INHERITANCE_TEMPLATE_PARAMS typename PT
#define CGAL_GRAPH_TRAITS_INHERITANCE_CLASS_NAME My_mesh_3<PT>
#define CGAL_GRAPH_TRAITS_INHERITANCE_BASE_CLASS_NAME CGAL::Surface_mesh<PT>
#include <CGAL/boost/graph/graph_traits_inheritance_macros.h>

/// make My_mesh_4 a valid face graph model
#define CGAL_GRAPH_TRAITS_INHERITANCE_TEMPLATE_PARAMS int dim, typename K
#define CGAL_GRAPH_TRAITS_INHERITANCE_CLASS_NAME My_mesh_4<dim, K>
#define CGAL_GRAPH_TRAITS_INHERITANCE_BASE_CLASS_NAME typename CGAL::Linear_cell_complex_for_bgl_combinatorial_map_helper\
         <2, dim, CGAL::Linear_cell_complex_traits<dim, K> >::type
#include <CGAL/boost/graph/graph_traits_inheritance_macros.h>

/// make My_mesh_5 a valid face graph model
// no template parameter, CGAL_GRAPH_TRAITS_INHERITANCE_TEMPLATE_PARAMS is then not defined
#define CGAL_GRAPH_TRAITS_INHERITANCE_CLASS_NAME My_mesh_5
#define CGAL_GRAPH_TRAITS_INHERITANCE_BASE_CLASS_NAME CGAL::Surface_mesh<Kernel::Point_3>
#include <CGAL/boost/graph/graph_traits_inheritance_macros.h>

template <class Graph>
void concept_check()
{
  typedef boost::graph_traits< Graph > Traits;
  typedef typename Traits::edge_descriptor edge_descriptor;
  typedef typename Traits::halfedge_descriptor halfedge_descriptor;
  typedef typename Traits::vertex_descriptor vertex_descriptor;
  typedef typename Traits::face_descriptor face_descriptor;

  boost::function_requires< boost::GraphConcept<Graph> >();
  boost::function_requires< boost::VertexListGraphConcept<Graph> >();
  boost::function_requires< boost::EdgeListGraphConcept<Graph> >();
  boost::function_requires< boost::IncidenceGraphConcept<Graph> >();
  boost::function_requires< boost::AdjacencyMatrixConcept<Graph> >();
  boost::function_requires< boost::BidirectionalGraphConcept<Graph> >();
  boost::function_requires< CGAL::HalfedgeGraphConcept<Graph> >();
  boost::function_requires< CGAL::HalfedgeListGraphConcept<Graph> >();
  boost::function_requires< CGAL::FaceGraphConcept<Graph> >();
  boost::function_requires< CGAL::FaceListGraphConcept<Graph> >();
  boost::function_requires< CGAL::MutableHalfedgeGraphConcept<Graph> >();
  boost::function_requires< CGAL::MutableFaceGraphConcept<Graph> >();

  boost::function_requires< boost::concepts::ReadablePropertyGraph<
    Graph, halfedge_descriptor, CGAL::halfedge_index_t> >();
  boost::function_requires< boost::concepts::ReadablePropertyGraph<
    Graph, edge_descriptor, boost::edge_index_t> >();
  boost::function_requires< boost::concepts::ReadablePropertyGraph<
    Graph, edge_descriptor, boost::edge_weight_t> >();
  boost::function_requires< boost::concepts::PropertyGraph<
    Graph, vertex_descriptor, CGAL::vertex_point_t> >();
  boost::function_requires< boost::concepts::ReadablePropertyGraph<
    Graph, vertex_descriptor, boost::vertex_index_t> >();
  boost::function_requires< boost::concepts::ReadablePropertyGraph<
    Graph, face_descriptor, CGAL::face_index_t> >();

  // null
  boost::graph_traits<Graph>::null_vertex();
  boost::graph_traits<Graph>::null_halfedge();
  boost::graph_traits<Graph>::null_face();
}

int main()
{
  concept_check<My_mesh_1<Kernel>>();
  concept_check<My_mesh_2>();
  concept_check<My_mesh_3<Point_3> >();
  concept_check<My_mesh_4<3,Kernel>>();
  concept_check<My_mesh_5>();
  return 0;
}


#include <boost/geometry/algorithms/intersection.hpp>


#include "TrackerEdController.h"
#include "Utils/VideoUtils.h"
#include "Task/Video/ReadVideo.h"

#include <imgui.h>
#include <GL/glew.h>

#include <opencv2/core.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <boost/log/trivial.hpp>
#include <boost/filesystem.hpp>

#include <opencv2/tracking.hpp>

#include <libmv/simple_pipeline/pipeline.h>
#include <libmv/tracking/brute_region_tracker.h>

TrackerEdController::TrackerEdController() : view(model) {

    this->view.SetAddClipCallback(std::bind(&TrackerEdController::addClipCallback, this, std::placeholders::_1));
    this->view.SetDetectOrbCallback(std::bind(&TrackerEdController::detectORBCallback, this));
    this->view.SetTrackerMarkersCallback(std::bind(&TrackerEdController::trackMarkersCallback, this));
	this->view.SetAddTrackMarkerCallback(std::bind(&TrackerEdController::addTrackMarkerCallback, this, std::placeholders::_1));

}

TrackerEdController::~TrackerEdController() {

}

void TrackerEdController::Init() {
    this->view.Init();
}

void TrackerEdController::DeInit() {
    this->view.DeInit();
}

void TrackerEdController::Render() {
    this->view.Render();
}

void TrackerEdController::addClipCallback(std::string filePath) {
	this->model.CurrVideo.ImportVideoFromPath(filePath);

	this->model.sequence.mFrameMax = this->model.CurrVideo.GetFrameCount();

	this->model.sequence.myItems.push_back(
		TrackSequence::TrackSequenceItem{
			TrackSequenceItemType::Clip,
			0,
			this->model.sequence.mFrameMax,
			false,
			"Clip",
			TrackMarker()
		}
	);

}

void TrackerEdController::detectORBCallback() {

}

template<typename QuadT, typename ArrayT>
void QuadToArrays(const QuadT& quad, ArrayT* x, ArrayT* y) {
	for (int i = 0; i < 4; ++i) {
		x[i] = quad.coordinates(i, 0);
		y[i] = quad.coordinates(i, 1);
	}
}

void MarkerToArrays(const mv::Marker& marker, double* x, double* y) {
	mv::Quad2Df offset_quad = marker.patch;
	mv::Vec2f origin = marker.search_region.Rounded().min;
	offset_quad.coordinates.rowwise() -= origin.transpose();
	QuadToArrays(offset_quad, x, y);
	x[4] = marker.center.x() - origin(0);
	y[4] = marker.center.y() - origin(1);
}

void TrackerEdController::trackMarkersCallback() {

	// libmv uses kalman filter
	// https://www.myzhar.com/blog/tutorials/tutorial-opencv-ball-tracker-using-kalman-filter/
    // https://www.youtube.com/watch?v=5SgWfSP6jaM
	// https://github.com/son-oh-yeah/Moving-Target-Tracking-with-OpenCV

	VideoFrameAccessor accessor;
	accessor.SetVideoSource(this->model.CurrVideo);

	mv::AutoTrack* autotrack = new mv::AutoTrack(&accessor);

		//autotrack.options.

	libmv::vector<mv::Marker> markerCollection;
	for (TrackSequence::TrackSequenceItem sequenceItem : this->model.sequence.myItems) {
		if (sequenceItem.SequenceItemType == TrackSequenceItemType::Track) {
			markerCollection.push_back(sequenceItem.marker.GetLibmvMarker());
		}
	}
	int markerCollectionSize = markerCollection.size();
	autotrack->SetMarkers(&markerCollection);

	mv::TrackRegionOptions trackRegionOptions;
	trackRegionOptions.mode = mv::TrackRegionOptions::TRANSLATION;
	trackRegionOptions.minimum_correlation = 0.75;
	trackRegionOptions.sigma = 0.9;
	trackRegionOptions.max_iterations = 50;
	trackRegionOptions.use_brute_initialization = true;
	trackRegionOptions.use_normalized_intensities = false;
	trackRegionOptions.use_esm = true;
	trackRegionOptions.attempt_refine_before_brute = false;
	trackRegionOptions.num_extra_points = 1;
	trackRegionOptions.regularization_coefficient = 0.0;
	trackRegionOptions.minimum_corner_shift_tolerance_pixels = 0.005;

	// Test
	{
		TrackMarker ctMarker = this->model.sequence.myItems[1].marker;

		mv::Marker refMarker     = ctMarker.mvMarker;
		mv::Marker markerToTrack = ctMarker.mvMarker;

		for (int frameIdx = 0; frameIdx < this->model.CurrVideo.GetFrameCount(); frameIdx++) {

			mv::FloatImage image1(100, 100, 1);
			accessor.GetImage(
				0,
				0, // Frame 0
				mv::FrameAccessor::MONO,
				0,
				&refMarker.search_region.Rounded(),
				nullptr,
				&image1);

			mv::FloatImage image2(100, 100, 1);
			accessor.GetImage(
				0,
				frameIdx + 1, // Frame 1
				mv::FrameAccessor::MONO,
				0,
				&markerToTrack.search_region.Rounded(),
				nullptr,
				&image2);

			double x1[5];
			double y1[5];
			MarkerToArrays(refMarker, x1, y1);

			double x2[5]; // New pos of the patch
			double y2[5];
			MarkerToArrays(markerToTrack, x2, y2);

			mv::Vec2f original_center = markerToTrack.center;

			mv::TrackRegionResult res;
			//libmv::TrackRegion(image1, image2, x1, y1, trackRegionOptions, x2, y2, &res);

			libmv::BruteRegionTracker regionTracker;
			regionTracker.minimum_correlation = 0.75;
			regionTracker.half_window_size = 20;
			bool result = regionTracker.Track(image1, image2, x1[4], y1[4], x2, y2);

			if (!result) {
				this->model.sequence.myItems[1].marker.EndFrame = frameIdx;
				this->model.sequence.myItems[1].mFrameEnd = frameIdx;
				break;
			}

			// Update and add markerToTrack to the list
			// Copy results over the tracked marker.
			//mv::Vec2f tracked_origin = markerToTrack.search_region.Rounded().min;
			//for (int i = 0; i < 4; ++i) {
			//	markerToTrack.patch.coordinates(i, 0) = x2[i] + tracked_origin[0];
			//	markerToTrack.patch.coordinates(i, 1) = y2[i] + tracked_origin[1];
			//}

			//markerToTrack.center(0) = x2[4] + tracked_origin[0];
			//markerToTrack.center(1) = y2[4] + tracked_origin[1];

			//mv::Vec2f delta = markerToTrack.center - original_center;

			//markerToTrack.search_region.Offset(delta);

			markerToTrack.center(0) = markerToTrack.center(0) + x2[0];
			markerToTrack.center(1) = markerToTrack.center(1) + y2[0];

			double deltaX = x2[0] - x1[4];
			double deltaY = y2[0] - y1[4];

			markerToTrack.source = mv::Marker::TRACKED;
			markerToTrack.status = mv::Marker::UNKNOWN;
			markerToTrack.reference_clip = refMarker.clip;
			markerToTrack.reference_frame = refMarker.frame;

			this->model.sequence.myItems[1].marker.TrackedMarker.push_back(markerToTrack);

			//refMarker = markerToTrack;

			if (!res.is_usable()) {
				//this->model.sequence.myItems[1].marker.EndFrame = frameIdx;
				//this->model.sequence.myItems[1].mFrameEnd = frameIdx;
				//break;
			}

		}

	}

	return;

	// For each marker

	for (TrackSequence::TrackSequenceItem &sequenceItem : this->model.sequence.myItems) {

		if (sequenceItem.SequenceItemType != TrackSequenceItemType::Track) {
			continue;
		}
		int markerIdx = 0;
		for (int frameIdx = 0; frameIdx < this->model.CurrVideo.GetFrameCount(); frameIdx++) {

			// Check if frame has marker by index
			mv::Marker currentMarker;
			bool hasMarker = autotrack->GetMarker(
				0,
				frameIdx,
				markerIdx,
				&currentMarker);

			// current frame doesnt have markers which are placed by the user so check next frame
			if (!hasMarker) {
				continue;
			}

			// -----
			// TODO: Check if marker is in bounds
			// -----

			mv::Marker trackedMarker      = currentMarker; // trackedMarker a new marker which is created on the next frame
			trackedMarker.frame           = frameIdx + 1; // Next frame to track
			trackedMarker.reference_frame = currentMarker.reference_frame;

			mv::TrackRegionResult result;

			bool status = autotrack->TrackMarker(&trackedMarker, &result, &trackRegionOptions);

			if (status) {
				autotrack->AddMarker(trackedMarker);
				sequenceItem.marker.TrackedMarker.push_back(trackedMarker);
				sequenceItem.mFrameEnd = frameIdx;
				sequenceItem.marker.EndFrame = frameIdx;
			}
			else {
				sequenceItem.mFrameEnd = frameIdx;
				sequenceItem.marker.EndFrame = frameIdx;

				break;
			}

			// According to the implementation of AddMarker it add a new marker instance to the vector in autotrack
			// it assigns it the vector  -> markers_[i] = marker; -> it updates the existing marker

		}

		markerIdx++;
	}

	delete autotrack;

}

void TrackerEdController::addTrackMarkerCallback(TrackMarker marker) {
	this->model.sequence.myItems.push_back(
		TrackSequence::TrackSequenceItem {
			TrackSequenceItemType::Track,
			marker.BeginFrame,
			marker.EndFrame,
			false,
			"Track " + std::to_string(this->model.sequence.myItems.size()),
			std::move(marker)
		}
	);

}


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

#ifndef LinearSearchKNNPropertyLimitLocalReuse_HPP
#define LinearSearchKNNPropertyLimitLocalReuse_HPP

// STL
#include <algorithm> // for lower_bound()
#include <limits> // for infinity()
#include <set>

// Boost
#include <boost/utility.hpp> // for enable_if()
#include <boost/type_traits.hpp> // for is_same()

// Custom
#include "Utilities/Utilities.hpp"

// Submodules
#include <Mask/Mask.h>
#include <Utilities/Debug/Debug.h>

/**
  * This function template is similar to std::min_element but can be used when the comparison
  * involves computing a derived quantity (a.k.a. distance). This algorithm will search for the
  * the elements in the range [first,last) with the "smallest" distances (of course, both the
  * distance metric and comparison can be overriden to perform something other than the canonical
  * Euclidean distance and less-than comparison, which would yield the element with minimum distance).
  * This function will fill the output container with a number of nearest-neighbors.
  * \tparam DistanceValueType The value-type for the distance measures.
  * \tparam DistanceFunctionType The functor type to compute the distance measure.
  */
template <typename TPatchDescriptorPropertyMap, typename TPatchDistanceFunction,
          typename TImageToWrite>
class LinearSearchKNNPropertyLimitLocalReuse : public Debug
{
  typedef float DistanceValueType;

  /** The map of vertex descriptors. */
  TPatchDescriptorPropertyMap PatchDescriptorPropertyMap;

  /** How many top patches to return. */
  unsigned int K;

  /** The function to use to compare patches. */
  TPatchDistanceFunction PatchDistanceFunction;

  /** An image indicating where each pixel came from. */
  typedef itk::Image<itk::Index<2>, 2> SourcePixelMapImageType;
  SourcePixelMapImageType* SourcePixelMapImage;

  /** The current inpainting mask. */
  Mask* MaskImage;

  /** Store the full region so that it can be reference without an image or mask. */
  itk::ImageRegion<2> FullRegion;

  unsigned int Iteration = 0;

  TImageToWrite* ImageToWrite;

  float LocalRegionSizeMultiplier;

  float MaxAllowedUsedPixelsRatio;

public:
  LinearSearchKNNPropertyLimitLocalReuse(TPatchDescriptorPropertyMap patchDescriptorPropertyMap, Mask* mask, const unsigned int k = 1000,
                                         float localRegionSizeMultiplier = 3.0f, float maxAllowedUsedPixelsRatio = 0.5f,
                                         TPatchDistanceFunction patchDistanceFunction = TPatchDistanceFunction(),
                                         SourcePixelMapImageType* sourcePixelMapImage = nullptr, TImageToWrite* imageToWrite = nullptr) :
    PatchDescriptorPropertyMap(patchDescriptorPropertyMap), K(k), PatchDistanceFunction(patchDistanceFunction),
    SourcePixelMapImage(sourcePixelMapImage), MaskImage(mask), ImageToWrite(imageToWrite),
    LocalRegionSizeMultiplier(localRegionSizeMultiplier), MaxAllowedUsedPixelsRatio(maxAllowedUsedPixelsRatio)
  {
    this->FullRegion = this->MaskImage->GetLargestPossibleRegion();
  }

  /** Set the number of nearest neighbors to return. */
  void SetK(const unsigned int k)
  {
    this->K = k;
  }

  /** Get the number of nearest neighbors to return. */
  unsigned int GetK() const
  {
    return this->K;
  }

  typedef std::set<itk::Index<2>, itk::Index<2>::LexicographicCompare> UsedIndexSetType;

  //   template <typename ForwardIteratorType, typename OutputContainerType>
  //   void operator()(ForwardIteratorType first, ForwardIteratorType last,
  //                   typename ForwardIteratorType::value_type queryNode, OutputContainerType& output)
  /**
    * \tparam ForwardIterator The forward-iterator type.
    * \tparam OutputContainer The container type which can contain the list of nearest-neighbors (STL like container, with iterators, insert, size, and pop_back).
    * \param first Start of the range in which to search.
    * \param last One element past the last element in the range in which to search (usually container.end() ).
    * \param output The container that will have the sorted list of elements with the smallest distance.
    * \param compare A callable object that returns true if the first element is the preferred one (less-than) of the two.
    * \param max_neighbors The maximum number of elements of smallest distance to output in the sorted list.
    */
  template <typename TForwardIterator, typename TOutputIterator>
  inline TOutputIterator
  operator()(TForwardIterator first, TForwardIterator last,
             typename TForwardIterator::value_type queryNode,
             TOutputIterator outputFirst)
  {
    if(this->DebugScreenOutputs)
    {
      std::cout << "LinearSearchKNNPropertyLimitLocalReuse searching " << last - first << " patches." << std::endl;
    }

    // Nothing to do if the input range is empty
    if(first == last)
    {
      return outputFirst;
    }

    // This doesn't work correctly - the image is written but it cannot be opened in a viewer like gwenview,
    // and in Paraview it does not seem to contain columns of patches and numbers? Perhaps because of the extreme
    // size of the image?
//    if(this->DebugImages && this->Iteration == 140)
//    {
//      if(this->ImageToWrite == nullptr)
//      {
//        throw std::runtime_error("LinearSearchBestLidarTextureGradient cannot WriteTopPatches without having an ImageToWrite!");
//      }
//      PatchHelpers::WriteTopPatches(this->ImageToWrite, this->PatchDescriptorPropertyMap, first, last,
//                                    "KNNPatches", this->Iteration);
//    }

    if(this->DebugImages)
    {
      ITKHelpers::WriteIndexImage(this->SourcePixelMapImage, Helpers::GetSequentialFileName("SourcePixelMap", this->Iteration, "mha", 3));
    }

    // Use a priority queue to keep the items sorted
    typedef std::pair<DistanceValueType, TForwardIterator> PairType;
    typedef compare_pair_first<DistanceValueType, TForwardIterator, std::greater<DistanceValueType> > CompareType;
    typedef std::priority_queue< PairType, std::vector<PairType>,
        CompareType> PriorityQueueType;

    typedef typename TPatchDescriptorPropertyMap::value_type DescriptorType;
    DescriptorType queryDescriptor = get(this->PatchDescriptorPropertyMap, queryNode);

    typedef typename TForwardIterator::value_type NodeType;

    // Create a region from the node
    itk::Index<2> queryIndex = Helpers::ConvertFrom<itk::Index<2>, NodeType>(queryNode);

    itk::ImageRegion<2> queryRegion = queryDescriptor.GetRegion();

    // Look for pixels that have already been used

    // Create a bigger region
    itk::ImageRegion<2> largerTargetRegion =
        ITKHelpers::GetRegionInRadiusAroundPixel(queryIndex,
                                                 queryRegion.GetSize()[0]/2.0f * this->LocalRegionSizeMultiplier);
    largerTargetRegion.Crop(this->FullRegion);
    std::cout << "The region to search for repeated pixels is " << largerTargetRegion << std::endl;

    itk::ImageRegionConstIterator<SourcePixelMapImageType> sourcePixelMapIterator(this->SourcePixelMapImage,
                                                                                largerTargetRegion);

    UsedIndexSetType usedIndices;
    while(!sourcePixelMapIterator.IsAtEnd())
    {
      usedIndices.insert(sourcePixelMapIterator.Get());

      ++sourcePixelMapIterator;
    }

    unsigned int numberOfHolePixels = this->MaskImage->CountHolePixels(queryRegion);
    unsigned int maxAllowedUsedPixels = this->MaxAllowedUsedPixelsRatio * numberOfHolePixels;

    PriorityQueueType outputQueue;
    while(outputQueue.size() == 0)
    {
      outputQueue = FindUsablePatches<PriorityQueueType, TForwardIterator, DescriptorType>(first, last, usedIndices, maxAllowedUsedPixels, queryDescriptor);
      maxAllowedUsedPixels += 10;
    }

//    std::cout << "There are " << outputQueue.size() << " items in the queue." << std::endl;

    // Keep only the best K matches
    if(outputQueue.size() < this->K)
    {
      std::cerr << "Warning: LinearSearchKNNPropertyLimitLocalReuse only has " << outputQueue.size()
                << " nodes. (" << this->K << " were requested.)" << std::endl;
    }
    Helpers::KeepTopN(outputQueue, this->K);

//    std::cout << "There are " << outputQueue.size() << " items in the queue." << std::endl;

    if(outputQueue.size() < this->K)
    {
      std::stringstream ss;
      ss << "LinearSearchKNNPropertyLimitLocalReuse: Requested " << this->K << " items but only found " << outputQueue.size();
      throw std::runtime_error(ss.str());
    }

    std::cout << "Best patch score is: " << outputQueue.top().first << std::endl;

    // Copy the best matches from the queue into the output
    TOutputIterator currentOutputIterator = outputFirst;
    while( !outputQueue.empty() )
    {
      *currentOutputIterator = *(outputQueue.top().second);
      outputQueue.pop();
      ++currentOutputIterator;
    }

    this->Iteration++;

    return currentOutputIterator;
  } // end operator()

  template <typename T1, typename T2, typename Compare>
  struct compare_pair_first : std::binary_function< std::pair<T1, T2>, std::pair<T1, T2>, bool>
  {
    Compare comp;
    compare_pair_first(const Compare& aComp = Compare()) : comp(aComp) { }
    bool operator()(const std::pair<T1, T2>& x, const std::pair<T1, T2>& y) const
    {
      return comp(x.first, y.first);
    }
  };

  /** This is the case where the output iterators contain nodes.
    * 'result' does not have to be passed by reference because it is an iterator.
    * The return type if this function is enabled is OutputIteratorType.
    */
  template <typename PairPriorityQueueType,
  typename OutputIteratorType>
  inline
  typename boost::enable_if<
  boost::is_same<
  typename std::iterator_traits<typename PairPriorityQueueType::value_type::second_type >::value_type,
  typename std::iterator_traits< OutputIteratorType >::value_type
  >,
  OutputIteratorType >::type copy_neighbors_from_queue(PairPriorityQueueType& Q, OutputIteratorType result)
  {
    OutputIteratorType first = result;
    while( !Q.empty() )
    {
      *result = *(Q.top().second);
      Q.pop();
      ++result;
    };
    std::reverse(first, result);
    return result;
  }

  template <typename TPriorityQueue, typename TForwardIterator, typename TDescriptor>
  TPriorityQueue FindUsablePatches(TForwardIterator first, TForwardIterator last,
                                   UsedIndexSetType usedIndices, unsigned int maxAllowedUsedPixels, TDescriptor& queryDescriptor)
  {
    TPriorityQueue outputQueue;

    typedef typename TForwardIterator::value_type NodeType;

    // The queue stores the items in descending score order.
    #pragma omp parallel for
//    for(ForwardIteratorType current = first; current != last; ++current) // OpenMP 3 doesn't allow != in the loop ending condition
    for(TForwardIterator currentIterator = first; currentIterator < last; ++currentIterator)
    {
      NodeType currentNode = *currentIterator;

      itk::ImageRegion<2> queryRegion = queryDescriptor.GetRegion();

      typename TPatchDescriptorPropertyMap::value_type currentDescriptor = get(this->PatchDescriptorPropertyMap, currentNode);

      if(currentDescriptor.GetStatus() != PixelDescriptor::SOURCE_NODE)
      {
        throw std::runtime_error("LinearSearchKNNPropertyLimitLocalReuse: Node is not a source node!");
      }

      itk::ImageRegion<2> potentialSourceRegion = currentDescriptor.GetRegion();

      potentialSourceRegion.Crop(this->FullRegion);

      if(!this->MaskImage->IsValid(potentialSourceRegion))
      {
        throw std::runtime_error("LinearSearchKNNPropertyLimitLocalReuse: potentialSourceRegion is not fully valid!");
      }

      // Count the number of pixels that were already copied from this patch
      unsigned int usedPixelCounter = 0;

      itk::ImageRegionConstIteratorWithIndex<SourcePixelMapImageType>
          sourceRegionIterator(this->SourcePixelMapImage, potentialSourceRegion);

      // The image that this is iterating over is irrelevant, we just need the indices.
      itk::ImageRegionConstIteratorWithIndex<SourcePixelMapImageType>
          queryRegionIterator(this->SourcePixelMapImage, queryRegion);

      while(!sourceRegionIterator.IsAtEnd())
      {
        UsedIndexSetType::iterator usedIndexSetIterator;
        if(this->MaskImage->IsHole(queryRegionIterator.GetIndex()))
        {
          // We want to use the index value in the SourcePixelMapImage,
          // because the value might not equal the current index in the case where new patches are allowed.
          usedIndexSetIterator = usedIndices.find(sourceRegionIterator.Get());

          if(usedIndexSetIterator != usedIndices.end()) // found
          {
            usedPixelCounter++;
          }
        }

        ++sourceRegionIterator;
        ++queryRegionIterator;
      }

      typedef std::pair<DistanceValueType, TForwardIterator> PairType;
      if(usedPixelCounter <= maxAllowedUsedPixels)
      {
        DistanceValueType d = this->PatchDistanceFunction(currentDescriptor, queryDescriptor); // (source, target) (the query node is the target node)

        #pragma omp critical // There are weird crashes without this guard
        outputQueue.push(PairType(d, currentIterator));
      }
      else
      {
//        std::cout << "Prevented use because " << usedPixelCounter
//                  << " pixels were already used (out of " << numberOfHolePixels
//                  << " hole pixels)." << std::endl;
      }
    } // end loop over all patches

    return outputQueue;
  }

};

#endif


#include <ostream>
#include <vector>
#include <tuple>
#include <unordered_set>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/test/unit_test.hpp>

#include "algos.h"
#include "random_graph.h"

#define REPEAT(n) for (unsigned i__ = 0; i__ < (n); i__++)

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> Graph;
typedef VertexDesc<Graph> Vertex;
typedef VertexSizeT<Graph> VertexSz;

// Helpers for easy loggin of non-trivial data types
namespace std {
	std::ostream &operator<<(std::ostream &stream, const VertexOrder<Graph> &order) {
		for (const auto v : order)
			stream << v << ' ';
		return stream;
	}

	std::ostream &operator<<(std::ostream &stream, const EdgeSet<Graph> &edges) {
		for (const auto [a, b] : edges)
			stream << '(' << a << ", " << b << ") ";
		return stream;
	}

	std::ostream &operator<<(std::ostream &stream, const Graph &g) {
		boost::write_graphviz(stream, g);
		return stream;
	}
}

BOOST_AUTO_TEST_SUITE(LexM)

/**
 * Helper function: check whether set `a` is strictly contained in set `b`, i.e.
 * `a` has fewer elements than `b` and all elements of `a` are in `b`.
 */
template <class Set>
static bool is_strict_subset(const Set &a, const Set &b) {
	if (a.size() >= b.size())
		return false;

	for (const auto &el : a) {
		if (b.find(el) == b.end())
			return false;
	}

	return true;
}

/**
 * Ensure that the elimination order computed by lex_m() is minimal, that is: no
 * other order exists for the same graph such that its fill-in is strictly
 * contained in the fill-in obtained from the order computed by lex_m(). This is
 * a simple brute force test.
 */
BOOST_AUTO_TEST_CASE(order_is_minimal) {
	REPEAT(50) {
		Graph g = gen_random_connected_graph<Graph>(7, 0.6);

		const auto min_order = lex_m(g);
		const auto min_fill  = fill_in(g, min_order);
		auto cur_order = std::make_from_tuple<std::vector<Vertex>>(boost::vertices(g));

		do {
			if (cur_order == min_order)
				continue;

			auto cur_fill = fill_in(g, cur_order);
			auto ok = !is_strict_subset(cur_fill, min_fill);

			BOOST_CHECK_MESSAGE(ok, "elimination order is non-minimal");

			if (!ok) {
				BOOST_TEST_MESSAGE(g);
				BOOST_TEST_MESSAGE("lex_m() order: " << min_order << "with fill-in: " << min_fill);
				BOOST_TEST_MESSAGE("better order : " << cur_order << "with fill-in: " << cur_fill);
				return;
			}
		} while (boost::range::next_permutation(cur_order)); // 7! = 5040
	}
}

/**
 * Ensure that the elimination order computed by lex_m() on a chordal graph is
 * perfect (i.e. it has empty fill-in). This is true because minimal implies
 * perfect for an elimination order of a chordal graph.
 */
BOOST_AUTO_TEST_CASE(order_is_perfect_for_chordal_graphs) {
	REPEAT(10) {
		Graph g = gen_random_chordal_graph<Graph>(200, 10000);
		auto o = lex_m(g);
		auto f = fill_in(g, o);

		BOOST_CHECK_EQUAL(f.size(), 0);
		BOOST_CHECK(is_perfect_elimination_order(g, o));
	}
}

/**
 * Ensure that the elimination order computed by lex_m() on a complete graph is
 * perfect (i.e. it has empty fill-in). This is true because complete implies
 * chordal.
 */
BOOST_AUTO_TEST_CASE(order_is_perfect_for_complete_graphs) {
	REPEAT(10) {
		// edge_prob = 1 => complete graph
		Graph g = gen_random_connected_graph<Graph>(200, 1);

		auto o = lex_m(g);
		auto f = fill_in(g, o);

		BOOST_CHECK_EQUAL(f.size(), 0);
		BOOST_CHECK(is_perfect_elimination_order(g, o));
	}
}

BOOST_AUTO_TEST_SUITE_END()


#include <Eigen/Dense>
#include <iostream>
#include <rosette/types.hpp>

namespace rosette {
namespace energy {

namespace Etype {  // just score for enum... can't use enum class
enum Etype { fa_atr = 0, fa_rep, fa_sol, Ntypes };
}

template <typename F>
F dummy_func() {
  return 12345.0;
}

/// takes these Ref<Eigen::Matrix<...>> things... they map transparently wtih
/// python. if they are Ref<const Eigen::Matrix...> allegedly no copy
template <typename F>
MatX<F> basic_energy_safe(CRefMatX4<F> atoms, CRefVecXi4 atype,
                          CRefVecXi4 exatype, CRefMatX4i4 connect) {
  MatX<F> scores(atoms.rows(), (int)Etype::Ntypes);
  for (int iatom = 0; iatom < atoms.rows(); ++iatom) {
    // std::cout << iatom << " " << atoms.row(iatom) << ' ' << atype[iatom] << "
    // "
    // << exatype[iatom] << " " << connect.row(iatom) << std::endl;
    scores(iatom, Etype::fa_atr) = 0.0;
    scores(iatom, Etype::fa_rep) = 0.0;
    scores(iatom, Etype::fa_sol) = 0.0;
  }
  return scores;
}
}
}


#include "ContingencyTable.hh"
#include "ContingencyTables.hh"
#include "PiecewiseLinearFunction.hh"
#include "ProgressDisplay.hh"
#include "SignificantShapelets.hh"
#include "SlidingWindow.hh"
#include "Utilities.hh"

#include "distances/Minkowski.hh"

#include <algorithm>
#include <iostream>
#include <limits>
#include <vector>

#include <cassert>
#include <cmath>

#include <boost/log/trivial.hpp>
#include <boost/math/special_functions/factorials.hpp>

SignificantShapelets::SignificantShapelets( unsigned size, unsigned windowStride )
  : _minWindowSize( size )
  , _maxWindowSize( size )
  , _windowStride( windowStride )
{
}

SignificantShapelets::SignificantShapelets( unsigned minSize, unsigned maxSize, unsigned windowStride )
  : _minWindowSize( minSize )
  , _maxWindowSize( maxSize )
  , _windowStride( windowStride )
{
}

std::vector<SignificantShapelets::SignificantShapelet> SignificantShapelets::operator()(
  const std::vector<TimeSeries>& timeSeries,
  const std::vector<bool>& labels,
  long double& tarone,
  std::vector<long double>& thresholds )
{
  assert( timeSeries.size() == labels.size() );

  unsigned n  = unsigned( timeSeries.size() );
  unsigned n1 = unsigned( std::count( labels.begin(), labels.end(), true ) );

  BOOST_LOG_TRIVIAL(info) << "n = " << n << ", n1 = " << n1;

  auto min_attainable_p_values = SignificantShapelets::min_attainable_p_values( n, n1, _withPseudocounts );

  // Remove thresholds that are larger than the desired significance
  // threshold. This does not change testability of patterns because
  // as long as Tarone's threshold is larger than alpha, we are only
  // adding patterns that may never be significant.
  while( min_attainable_p_values.back() > _alpha )
    min_attainable_p_values.pop_back();

  SlidingWindow sw( _minWindowSize,
                    _maxWindowSize,
                    _windowStride );

  sw.setRemoveDuplicates( _removeDuplicates );

  if( _removeDuplicates )
    BOOST_LOG_TRIVIAL(info) << "Performing duplicate removal during sliding window extraction";

  std::vector<TimeSeries> candidates;

  for( std::size_t i = 0; i < timeSeries.size(); i++ )
  {
    auto localCandidates = sw( timeSeries[i] );

    // Set the time series the local candidate originates from in order
    // to simplify post-procssing.
    for( auto&& localCandidate : localCandidates )
      localCandidate.setIndex( unsigned(i) );

    if( not _removeDuplicates )
      candidates.insert( candidates.end(), localCandidates.begin(), localCandidates.end() );
    else
    {
      // If duplicates are to be removed, we are only allowed to copy
      // a local candidate for which no *other* time series satisfies
      // the proxmimity criterion.
      std::copy_if(
        localCandidates.begin(), localCandidates.end(),
        std::back_inserter( candidates ),
        [&candidates] ( const TimeSeries& localCandidate )
        {
          return std::none_of( candidates.begin(), candidates.end(),
            [&localCandidate] ( const TimeSeries& t )
            {
              return t.isClose( localCandidate );
            }
          );
        }
      );
    }
  }

  BOOST_LOG_TRIVIAL(info) << "Obtained " << candidates.size() << " candidate shapelets";

  unsigned maxLength = 0;
  for( auto&& ts : timeSeries )
    maxLength = std::max( maxLength, unsigned( ts.length() ) );

  BOOST_LOG_TRIVIAL(info) << "Maximum length of input time series is " << maxLength;

  BOOST_LOG_TRIVIAL(info) << "Naive Bonferroni correction factor is " <<
    _alpha / (  ( static_cast<long double>( candidates.size() ) )
              * ( static_cast<long double>( timeSeries.size() ) + 1 )
  );

  // -------------------------------------------------------------------
  // Main loop for significant shapelet mining
  // -------------------------------------------------------------------
  //
  // Here, we iterate over all remaining candidate shapelets, figure out
  // the best split (with respect to the distance measure), and add them
  // to the list of significant shapelets (at least for the time being).

  std::vector<SignificantShapelet> significantShapelets;

  // Initial threshold for Tarone's method. This will be adjusted inside
  // the main loop.
  auto p_tarone = min_attainable_p_values.back();
  thresholds.push_back( p_tarone );

  ProgressDisplay progress( candidates.size() );
  progress.addField( "FWER" );
  progress.addField( "Tarone" );
  progress.addField( "No. testable patterns" );
  progress.addField( "No. tested patterns" );

  unsigned numHypotheses = 0;

  if( !_quiet )
    progress.draw();

  for( std::size_t i = 0; i < candidates.size(); i++ )
  {
    // Set up contingency tables and update them -----------------------
    //
    // This involves iterating over distances in the order in which they
    // appear and constantly update all contingency tables. Pruning will
    // be performed so that not all tables will have to be examined.
    ContingencyTables tables( n, n1, _withPseudocounts );

    bool skip = false;
    for( std::size_t j = 0; j < timeSeries.size(); j++ )
    {
      TimeSeries::ValueType distance = TimeSeries::ValueType();

      // No special distance functor specified, so we fall back to the
      // original squared Euclidean distance.
      if( !_distance )
        distance = candidates[i].distance( timeSeries[j] );

      // Use the client-provided distance functor
      else
        distance = _distance->operator()( candidates[i], timeSeries[j] );

      if( _disablePruning )
        tables.insert( distance, labels[j] );
      else
        tables.insert( distance, labels[j], p_tarone );

      // Have to increment number of hypotheses here because the
      // `ContingencyTables` class is allowed to prune tables on
      // its own. Hence, the number of candidates afterwards may
      // be lower in certain cases.
      numHypotheses++;

      if( tables.empty() )
      {
        skip = true;
        break;
      }
    }

    if( !_quiet )
      ++progress;

    if( skip )
      continue;

    bool updated = false;

    for( auto&& table : tables )
    {
      auto p_min = table.min_attainable_p();

      // Pattern is testable according to the current threshold set by
      // Tarone's criterion. Or else the user is crazy and wants us to
      // report all shapelets regardless of testability.
      if( p_min <= p_tarone || _reportAllShapelets )
      {
        significantShapelets.push_back(
          {
            candidates[i],
            p_min,
            table
          }
        );

        // At least one shapelet has been added, so we need to update
        // our FWER estimate below.
        updated = true;
      }
    }

    // If nothing has been changed, i.e. no shapelet has been added to
    // the list of the most significant ones, we do not have to change
    // the FWER estimate.
    if( !updated )
      continue;

    // If the user desired to see *all* shapelets, even those that are
    // statistically insignificant, we can continue the iteration.
    if( _reportAllShapelets )
      continue;

    auto estimateFWER
      = p_tarone * static_cast<long double>( significantShapelets.size() );

    // Adjust the testability threshold until the FWER estimate has
    // been sufficiently decreased.
    while( estimateFWER > _alpha )
    {
      min_attainable_p_values.pop_back();
      p_tarone = min_attainable_p_values.back();

      if( !_quiet )
        progress.setField( "Tarone", p_tarone );

      significantShapelets.erase(
        std::remove_if( significantShapelets.begin(), significantShapelets.end(),
          [&p_tarone] ( const SignificantShapelet& ss )
          {
            return ss.p > p_tarone;
          }
        ),
        significantShapelets.end()
      );

      estimateFWER
        = p_tarone * static_cast<long double>( significantShapelets.size() );

      thresholds.push_back( p_tarone );
    }

    if( !_quiet )
    {
      progress.setField( "FWER", estimateFWER );
      progress.setField( "No. testable patterns", significantShapelets.size() );
      progress.setField( "No. tested patterns", numHypotheses );
    }
  }

  // Replace the minimum attainable $p$-value by an actual $p$-value
  // because we have finished the extraction phase.
  std::transform(
    significantShapelets.begin(), significantShapelets.end(),
    significantShapelets.begin(),
      [] ( const SignificantShapelet& ss )
      {
        SignificantShapelet ss_new = {
          ss.shapelet,
          ss.table.p(), // returns the actual $p$-value of the contingency table
          ss.table
        };

        return ss_new;
      }
  );

  // Remove the shapelets that are not significant according to  the
  // current Tarone threshold. Also remove shapelets whose $p$-value
  // is NaN.
  significantShapelets.erase(
    std::remove_if( significantShapelets.begin(), significantShapelets.end(),
      [&p_tarone] ( const SignificantShapelet& ss )
      {
        return std::isnan( ss.p ) || ss.p > p_tarone;
      }
    ),
    significantShapelets.end()
  );

  // Report the lowest threshold according to Tarone. This can be used
  // to decide upon further corrections, such as Bonferroni.
  tarone = p_tarone;

  // Perform contingency table merging if desired by the user. This
  // reduces the number of significant shapelets that are reported,
  // but also makes it easier to sift through the results.
  if( _mergeTables )
  {
    BOOST_LOG_TRIVIAL(info) << "Merging contingency tables";

    // Sort contingency tables in lexicographical order. This makes it
    // easier to remove duplicates afterwards.
    std::stable_sort( significantShapelets.begin(), significantShapelets.end(),
      [] ( const SignificantShapelet& S, const SignificantShapelet& T )
      {
        auto as = S.table.as();
        auto bs = S.table.bs();
        auto cs = S.table.cs();
        auto ds = S.table.ds();

        auto at = T.table.as();
        auto bt = T.table.bs();
        auto ct = T.table.cs();
        auto dt = T.table.ds();

        if( as != at )
          return as < at;
        else
        {
          if( bs != bt )
            return bs < bt;
          else
          {
            if( cs != ct )
              return cs < ct;
            else
            {
              if( ds != dt )
                return ds < dt;
            }
          }
        }

        return false;
      }
    );

    // Only keep the first contingency table of every 'class' of
    // contingency tables.
    significantShapelets.erase(
      std::unique( significantShapelets.begin(), significantShapelets.end(),
        [] ( const SignificantShapelet& S, const SignificantShapelet& T )
        {
          return S.table == T.table;
        }
      ),
      significantShapelets.end()
    );
  }

  // Sort by increasing $p$-value in order to make the output easier to
  // parse for humans.
  std::stable_sort( significantShapelets.begin(), significantShapelets.end(),
    [] ( const SignificantShapelet& S, const SignificantShapelet& T )
    {
      // Sort by $p$-value first...
      if( S.p != T.p )
        return S.p < T.p;

      // ...and by length second, in case the $p$-values are equal
      else
        return S.shapelet.length() < T.shapelet.length();
    }
  );

  {
    std::vector<SignificantShapelet> significantShapelets_;

    std::copy_if( significantShapelets.begin(), significantShapelets.end(),
      std::back_inserter( significantShapelets_ ),
        [&significantShapelets_] ( const SignificantShapelet& ss )
        {
          return std::none_of( significantShapelets_.begin(), significantShapelets_.end(),
            [&ss] ( const SignificantShapelet& tt )
            {
              return ss.shapelet == tt.shapelet;
            }
          );
        }
    );

    significantShapelets.swap( significantShapelets_ );
  }

  BOOST_LOG_TRIVIAL(info)
    << "Detected " << significantShapelets.size()
    << " significant shapelet"
    << ( significantShapelets.size() != 1 ? "s" : "" );

  return significantShapelets;
}

std::vector<long double> SignificantShapelets::min_attainable_p_values( unsigned n, unsigned n1, bool withPseudocounts )
{
  ContingencyTable C( n,
                      n1,
                      0.0,
                      withPseudocounts ); // use a dummy threshold

  std::vector<long double> p_values;
  p_values.reserve( n );

  for( unsigned rs = 0; rs <= n; rs++ )
    p_values.push_back( C.min_attainable_p(rs) );

  std::sort( p_values.begin(), p_values.end() );

  p_values.erase(
    std::unique( p_values.begin(),
                 p_values.end() ),
    p_values.end()
  );

  return p_values;
}

std::ostream& operator<<( std::ostream& o, const SignificantShapelets::SignificantShapelet& ss )
{
  o << "  {\n"
    << "    \"p_val\": "     << std::setprecision( 32 ) << ss.p                 << ",\n"
    << "    \"threshold\": " << std::setprecision( 16 ) << ss.table.threshold() << ",\n"
    << "    \"table\": ["                               << ss.table             << "],\n"
    << "    \"index\": "                                << ss.shapelet.index()  << ",\n"
    << "    \"start\": "                                << ss.shapelet.start()  << ",\n"
    << "    \"shapelet\": [\n"
    << "      ";

  for( auto it = ss.shapelet.begin(); it != ss.shapelet.end(); ++it )
  {
    if( it != ss.shapelet.begin() )
      o << ",";

    o << *it;
  }

  o << "    ]\n"
    << "  }";

  return o;
}