//
// Created by Duzhong Chen on 2020/4/3.
//

#include <string>
#include <boost/lexical_cast.hpp>
#include "parser/NodesMaker.h"
#include "ConstantFolding.h"
#include "utils/string/UString.h"

namespace jetpack {

    inline Literal* MakeIntLiteral(AstContext& ctx, std::int32_t tmp) {
        auto lit = ctx.Alloc<Literal>();
        lit->ty = Literal::Ty::Double;
        lit->str_ = std::to_string(tmp);
        lit->raw = lit->str_;
        return lit;
    }

// tricks from https://stackoverflow.com/questions/11544073/how-do-i-deal-with-the-max-macro-in-windows-h-colliding-with-max-in-std
#define DUMMY

    inline bool IsValieResult(std::int64_t tmp) {
        return tmp >= std::numeric_limits<std::int32_t>::min DUMMY () && tmp <= std::numeric_limits<std::int32_t>::max DUMMY ();
    }

    Expression* ContantFolding::TryBinaryExpression(AstContext& ctx, BinaryExpression* binary) {
        if (binary->left->type == SyntaxNodeType::Literal
            && binary->right->type == SyntaxNodeType::Literal) {

            auto left_lit = dynamic_cast<Literal*>(binary->left);
            auto right_lit = dynamic_cast<Literal*>(binary->right);

            if (binary->operator_ == "+" && left_lit->ty == Literal::Ty::String && right_lit->ty == Literal::Ty::String) {
                std::string result = left_lit->str_ + right_lit->str_;
                return MakeStringLiteral(ctx, result);
            } else if (left_lit->ty == Literal::Ty::Double && right_lit->ty == Literal::Ty::Double) {
                int32_t left_int, right_int;
                try {
                    left_int = boost::lexical_cast<int32_t>(left_lit->str_);
                    right_int = boost::lexical_cast<int32_t>(right_lit->str_);
                } catch (const boost::bad_lexical_cast& ex) {
                    return binary;
                }

                int64_t tmp_result = 0;
                if (binary->operator_ == "+") {
                    tmp_result = left_int + right_int;
                } else if (binary->operator_ == "-") {
                    tmp_result = left_int - right_int;
                } else {
                    return binary;
                }

                if (!IsValieResult(tmp_result)) {
                    return binary;
                }
                return MakeIntLiteral(ctx, static_cast<std::int32_t>(tmp_result));
            }
        }
        return binary;
    }

}


#ifndef __BASE_SAMPLES_RIGID_BODY_STATE_HH
#define __BASE_SAMPLES_RIGID_BODY_STATE_HH

#include <base/Pose.hpp>
#include <base/Time.hpp>
#include <base/Float.hpp>

#include <Eigen/Core>
#include <Eigen/LU>

namespace base { namespace samples {
    /** Representation of the state of a rigid body
     *
     * This is among other things used to express frame transformations by
     * Rock's transformer
     *
     * Given a source and target frame, this structure expresses the _frame
     * change_ between these two frames. In effect, it represents the state of
     * the source frame expressed in the target frame.
     *
     * Per [Rock's conventions](http://rock.opendfki.de/wiki/WikiStart/Standards), you
     * should use a X-forward, right handed coordinate system when assigning
     * frames to bodies (i.e.  X=forward, Y=left, Z=up). In addition,
     * world-fixed frames should be aligned to North (North-West-Up, aka NWU)
     *
     * For instance, if sourceFrame is "body" and targetFrame is "world", then
     * the RigidBodyState object is the state of body in the world frame
     * (usually, the world frame has an arbitrary origin and a North-West-Up
     * orientation).
     */
    struct RigidBodyState
    {

        RigidBodyState(bool doInvalidation=true);

        base::Time time;

	/** Name of the source reference frame */
	std::string sourceFrame;

	/** Name of the target reference frame */
	std::string targetFrame;

        /** Position in m of sourceFrame's origin expressed in targetFrame
         */
        Position   position;
	/** Covariance matrix of the position
	 */
        base::Matrix3d cov_position;

        /** Orientation of targetFrame expressed in sourceFrame */
        Orientation orientation;
        /** Covariance matrix of the orientation as an axis/angle manifold in
         * body coordinates
	 */
        base::Matrix3d cov_orientation;

        /** Velocity in m/s of sourceFrame expressed in targetFrame */
        base::Vector3d velocity;
	/** Covariance of the velocity
	 */
        base::Matrix3d cov_velocity;

        /** Angular Velocity as an axis-angle representation in body fixed frame
         *
         * The direction of the vector is the axis, its length the speed */
        base::Vector3d angular_velocity;
        /** Covariance of the angular velocity
	 */
        base::Matrix3d cov_angular_velocity;

	void setTransform(const Eigen::Affine3d& transform);

	 Eigen::Affine3d getTransform() const;

	void setPose(const base::Pose& pose);

	base::Pose getPose() const;

        double getYaw() const;

        double getPitch() const;

        double getRoll() const;

	template <int _Options>
	operator Eigen::Transform<double, 3, Eigen::Affine, _Options>() const
	{
	    Eigen::Transform<double, 3, Eigen::Affine, _Options> ret;
	    ret.setIdentity();
	    ret.rotate(this->orientation);
	    ret.translation() = this->position;
	    return ret;
	}

        static RigidBodyState unknown();

        static RigidBodyState invalid();

        /** For backward compatibility only. Use invalidate() */
        void initSane();

        /** Initializes the rigid body state with NaN for the
         * position, velocity, orientation and angular velocity.
         */
	void invalidate();

	/**
         * Initializes the rigid body state unknown with Zero for the
         * position, velocity and angular velocity, Identity for the orientation
         * and infinity for all covariances.
         */
	void initUnknown();

        /** Helper method that checks if a value is valid (not NaN anywhere). */
        static bool isValidValue(base::Vector3d const& vec);

        /** Helper method that checks if an orientation is valid (not NaN anywhere)
         *  and that the orientation is an unit quaternion. */
        static bool isValidValue(base::Orientation const& ori);

        /** Helper method that checks if the value whose covariance is
         * represented by the given matrix is a known value (not Inf in the diagonal).
         */
        static bool isKnownValue(base::Matrix3d const& cov);

        /** Helper method that checks if the covariance represented by the given
         * matrix is valid
         */
        static bool isValidCovariance(base::Matrix3d const& cov);

        /** Checks if a given dimension of a vector is valid. */
        static bool isValidValue(base::Vector3d const& vec, int dim);

        /** Checks if a given dimension of a covariance matrix is valid. */
        static bool isValidCovariance(base::Matrix3d const& cov, int dim);

        /** Checks if the dimension of a vector given by a covariance is known. */
        static bool isKnownValue(base::Matrix3d const& cov, int dim);

        static base::Vector3d invalidValue();

        static base::Orientation invalidOrientation();

        static base::Matrix3d setValueUnknown();

        static base::Matrix3d invalidCovariance();

        bool hasValidPosition() const;
        bool hasValidPosition(int idx) const;
        bool hasValidPositionCovariance() const;
        void invalidatePosition();
        void invalidatePositionCovariance();

        bool hasValidOrientation() const;
        bool hasValidOrientationCovariance() const;
        void invalidateOrientation();
        void invalidateOrientationCovariance();

        bool hasValidVelocity() const;
        bool hasValidVelocity(int idx) const;
        bool hasValidVelocityCovariance() const;
        void invalidateVelocity();
        void invalidateVelocityCovariance();

        bool hasValidAngularVelocity() const;
        bool hasValidAngularVelocity(int idx) const;
        bool hasValidAngularVelocityCovariance() const;
        void invalidateAngularVelocity();
        void invalidateAngularVelocityCovariance();

        void invalidateValues(bool invPos, bool invOri, bool invVel = true,
                              bool invAngVel = true);

        void invalidateCovariances(bool invPos = true, bool invOri = true,
                                   bool invVel = true, bool invAngVel = true);
    };
}}

#endif


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

#ifndef SAMPLER_HH
#define SAMPLER_HH

#include "random.hh"
#include <functional>
#include <boost/mpl/if.hpp>

namespace graph_tool
{
using namespace std;
using namespace boost;

// Discrete sampling via vose's alias method.

// See http://www.keithschwarz.com/darts-dice-coins/ for a very clear
// explanation.

template <class Value, class KeepReference = mpl::true_>
class Sampler
{
public:
    Sampler(const vector<Value>& items,
            const vector<double>& probs)
        : _items(items), _probs(probs), _alias(items.size()),
          _S(0)
    {
        for (size_t i = 0; i < _probs.size(); ++i)
            _S += _probs[i];

        vector<size_t> small;
        vector<size_t> large;

        for (size_t i = 0; i < _probs.size(); ++i)
        {
            _probs[i] *= _probs.size() / _S;
            if (_probs[i] < 1)
                small.push_back(i);
            else
                large.push_back(i);
        }

        while (!(small.empty() || large.empty()))
        {
            size_t l = small.back();
            size_t g = large.back();
            small.pop_back();
            large.pop_back();

            _alias[l] = g;
            _probs[g] = (_probs[l] + _probs[g]) - 1;
            if (_probs[g] < 1)
                small.push_back(g);
            else
                large.push_back(g);
        }

        // fix numerical instability
        for (size_t i = 0; i < large.size(); ++i)
            _probs[large[i]] = 1;
        for (size_t i = 0; i < small.size(); ++i)
            _probs[small[i]] = 1;

        _sample = uniform_int_distribution<size_t>(0, _probs.size() - 1);
    }

    Sampler() {}

    template <class RNG>
    const Value& sample(RNG& rng)
    {
        size_t i = _sample(rng);
        bernoulli_distribution coin(_probs[i]);
        if (coin(rng))
            return _items[i];
        else
            return _items[_alias[i]];
    }

    size_t size() const { return _items.size(); }
    bool empty() const { return _S == 0; }
    bool has_n(size_t n) const { return (n == 0 || !empty()); }
    double prob_sum() const { return _S; }

    const Value& operator[](size_t i) const
    {
        return _items[i];
    }

    const auto& items() const
    {
        return _items;
    }

    auto begin() const
    {
        return _items.begin();
    }

    auto end() const
    {
        return _items.end();
    }

private:

    typedef typename mpl::if_<KeepReference,
                              const vector<Value>&,
                              vector<Value> >::type items_t;
    items_t _items;
    vector<double> _probs;
    vector<size_t> _alias;
    uniform_int_distribution<size_t> _sample;
    double _S;
    size_t _size;
};

// uniform sampling from containers

template <class Iter, class RNG>
auto&& uniform_sample(Iter begin, const Iter& end, RNG& rng)
{
    auto N = end - begin;
    std::uniform_int_distribution<size_t> i_rand(0, N - 1);
    std::advance(begin, i_rand(rng));
    return *begin;
}

template <class Container, class RNG>
auto&& uniform_sample(Container& v, RNG& rng)
{
    return uniform_sample(v.begin(), v.end(), rng);
}

} // namespace graph_tool

#endif // SAMPLER_HH


#ifndef STAN_MATH_PRIM_SCAL_FUN_FDIM_HPP
#define STAN_MATH_PRIM_SCAL_FUN_FDIM_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/scal/fun/is_nan.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/is_any_nan.hpp>
#include <boost/math/tools/promotion.hpp>

namespace stan {
namespace math {

/**
 * Return the positive difference of the specified values (C++11).
 *
 * The function is defined by
 *
 * fdim(x, y) = (x > y) ? (x - y) : 0.
 *
 * @param x First value.
 * @param y Second value.
 * @return max(x- y, 0)
 */
template <typename T1, typename T2>
inline return_type_t<T1, T2> fdim(T1 x, T2 y) {
  typedef return_type_t<T1, T2> return_t;
  using std::numeric_limits;
  if (is_any_nan(x, y))
    return NOT_A_NUMBER;
  return (x <= y) ? 0 : x - y;
}

}  // namespace math
}  // namespace stan
#endif


// practice6.0

#include <iostream>

#include <boost/type_traits/is_reference.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/is_pointer.hpp>
#include <boost/type_traits/is_float.hpp>
#include <boost/type_traits/alignment_of.hpp>
#include <boost/type_traits/remove_reference.hpp>
#include <boost/type_traits/remove_pointer.hpp>
#include <boost/type_traits/add_pointer.hpp>
#include <boost/type_traits/is_function.hpp>
#include <boost/type_traits/is_const.hpp>
#include <boost/static_assert.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/apply.hpp>

#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/not_equal_to.hpp>
#include <boost/mpl/greater.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/or.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/multiplies.hpp>

#include <boost/mpl/at.hpp>
#include <boost/mpl/next.hpp>
#include <boost/mpl/prior.hpp>
#include <boost/mpl/begin.hpp>
#include <boost/mpl/end.hpp>
#include <boost/mpl/advance.hpp>
#include <boost/mpl/push_front.hpp>
#include <boost/mpl/pop_front.hpp>
#include <boost/mpl/insert.hpp>
#include <boost/mpl/size.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/mpl/max_element.hpp>
#include <boost/mpl/min_element.hpp>
#include <boost/mpl/sizeof.hpp>
#include <boost/mpl/less.hpp>
#include <boost/mpl/copy.hpp>

using namespace boost::mpl::placeholders;
namespace mpl = boost::mpl;
using namespace boost::lambda;

#include <iterator>
#include <utility>
#include <list>
#include <vector>
#include <string>
#include <assert.h>
#include <algorithm>

struct none{};
template<class T=int>
struct wrapAssert : T
{
	typedef typename T type;
	BOOST_STATIC_ASSERT(0);
};

struct v : mpl::vector_c<int,17,10,25,2,11>
{};

//typedef mpl::copy< v, binary_tree_inserter<tree<> > >:: type bst;
struct push_binary_tree
{
	template<class S, class T>
	struct apply
	{
		typedef typename mpl::push_back<S, mpl::integral_c<int,T::value> >::type  type;
	};
};

typedef mpl::copy< v, mpl::inserter<mpl::vector_c<int>, push_binary_tree > >:: type bst;

template<int Pos, int Level>
struct leftChild
{
	static const int value = Pos * 2 + 1;
};

template<int Pos, int Level>
struct rightChild
{
	static const int value = Pos * 2 + 2;
};

template<int Level>
struct downStair
{
	static const int value = Level+1;
};

template<class S, int Pos=0, int Level=0>
struct inorder_view
{
	template<bool traverse, class S, int Pos, int Level> struct trav{};

	static const bool traverse = mpl::less<mpl::int_<Pos>, mpl::size<S>::type>::type::value;
	typedef typename trav<traverse,S,Pos,Level>::type type;

	template<class S, int Pos, int Level>
	struct trav<true, S, Pos, Level>
	{
		typedef typename
			mpl::copy<
					typename inorder_view<S, rightChild<Pos,Level>::value, downStair<Level>::value>::type
				,
				mpl::back_inserter<
					typename mpl::push_back<
					typename inorder_view<S, leftChild<Pos,Level>::value, downStair<Level>::value>::type
					,  typename mpl::at<S, mpl::int_<Pos> >::type
					//mpl::int_<Pos>
					>::type
				>
			>::type
			type;
	};

	template<class S, int Pos, int Level>
	struct trav<false, S, Pos, Level>
	{
		typedef typename mpl::vector_c<int> type;
	};

};

typedef mpl::copy< v, mpl::front_inserter<mpl::vector_c<int> > >:: type testV;

// struct test1 :
// 	mpl::eval_if<
// 		mpl::equal<
// 			mpl::vector_c<int,11,2,10,25,17>
// 			,testV
// 		>::type
// 		, none
// 		, wrapAssert<>
// 	>
// {};

//
// struct test2 :
// 	mpl::eval_if<
// 		mpl::equal<mpl::vector_c<int,1,1,1,1,1>,bst>::type
// 	, none
// 	, wrapAssert<>
// 	>
// {};

struct test4
	: inorder_view< bst >::type
{
};

// BOOST_STATIC_ASSERT(
// mpl::equal< inorder_view<bst>
// , mpl::vector_c<int,2,10,11,17,25>
// >::value ));

void main()
{
	bool b1 = mpl::greater<mpl::int_<1>, typename mpl::size<bst>::type>::type::value;
	inorder_view< bst >::type t2;

	typedef mpl::vector_c<int,1,2,3> v1;
	typedef mpl::copy< mpl::vector_c<int,4,5,6>,
		mpl::back_inserter<
		 v1> >::type v2;

	v1 V1;
	v2 V2;

}


///////////////////////////////////////////////////////////////////////////////
//
// 3DimViewer
// Lightweight 3D DICOM viewer.
//
// Copyright 2008-2015 3Dim Laboratory s.r.o.
//
// 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
//
// 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.
//
///////////////////////////////////////////////////////////////////////////////

#include <geometry/base/functions.h>
#include <geometry/base/CBoundingRectangle.h>
#include <Eigen/Eigenvalues>
#include <array>
#include <unordered_set>
#include <unordered_map>

namespace geometry
{

int isnan(double x)
{
    return x != x;
}

int isnan(const geometry::Vec2 &pt)
{
    return isnan(pt[0]) || isnan(pt[1]);
}

int isnan(const geometry::Vec3 &pt)
{
    return isnan(pt[0]) || isnan(pt[1]) || isnan(pt[2]);
}

geometry::Vec3 toVec3d(const geometry::Vec4 &v)
{
    return geometry::Vec3(v[0], v[1], v[2]);
}

geometry::Vec3 lerp(const geometry::Vec3 &v0, const geometry::Vec3 &v1, double w)
{
    return v0 + (v1 - v0) * geometry::Scalar(w);
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Calculates intersection between two line segments
//!
//!\param   l0a             Line0's first point
//!\param   l0b             Line0's second point
//!\param   l1a             Line1's first point
//!\param   l1b             Line1's second point
//!\param   i               Intersection point
//!\param   includePoints   Flag if lines' points can touch without causing intersection
//!
//!\return  True if lines intersect each other, false otherwise
////////////////////////////////////////////////////////////////////////////////////////////////////
bool calculateIntersection2d(const geometry::Vec2 &l0a, const geometry::Vec2 &l0b, const geometry::Vec2 &l1a, const geometry::Vec2 &l1b, geometry::Vec2 &i, double snapThreshold, double angleThreshold, bool includePoints)
{
    if (!includePoints && ((l0a == l1a) || (l0b == l1b) || (l0a == l1b) || (l1a == l0b)))
    {
        return false;
    }

    if ((l0b == l0a) || (l1b == l1a))
        return false;

#ifdef _DEBUG
    if (isnan(l0a) || isnan(l0b) || isnan(l1a) || isnan(l1b))
    {
        std::cout << "nan!";
        return false;
    }
#endif

    geometry::CBoundingRectangle l0r, l1r;
    l0r.expandBy(l0a);
    l0r.expandBy(l0b);
    l0r.expandBy(l0r.getMin() - geometry::Vec2(snapThreshold, snapThreshold));
    l0r.expandBy(l0r.getMax() + geometry::Vec2(snapThreshold, snapThreshold));
    l1r.expandBy(l1a);
    l1r.expandBy(l1b);
    l1r.expandBy(l1r.getMin() - geometry::Vec2(snapThreshold, snapThreshold));
    l1r.expandBy(l1r.getMax() + geometry::Vec2(snapThreshold, snapThreshold));

    if (!l0r.intersects(l1r))
    {
        return false;
    }

    geometry::Vec2 v0 = l0b - l0a;
    geometry::Vec2 v0n = v0;
    v0n.normalize();
    geometry::Vec2 n0 = geometry::Vec2(-v0n[1], v0n[0]);
    double a0 = n0[0];
    double b0 = n0[1];
    double c0 = -1 * (a0 * l0a[0] + b0 * l0a[1]);

    geometry::Vec2 v1 = l1b - l1a;
    geometry::Vec2 v1n = v1;
    v1n.normalize();
    geometry::Vec2 n1 = geometry::Vec2(-v1n[1], v1n[0]);
    double a1 = n1[0];
    double b1 = n1[1];
    double c1 = -1 * (a1 * l1a[0] + b1 * l1a[1]);

    // check if ray and segment are parallel
    if (1.0 - std::fabs(v0n * v1n) < angleThreshold)
    {
        return false;
    }

    double denom = a1 * b0 - a0 * b1;
    if (0.0 == denom)
    {
        return false;
    }

    // find intersection point
    double iy = (a0 * c1 - a1 * c0) / denom;
    double ix = (b0 * c1 - b1 * c0) / (-denom);

    // calculate "t" parameters
    double t0 = std::fabs(v0n[0]) < snapThreshold ? (iy - l0a[1]) / v0[1] : (ix - l0a[0]) / v0[0];
    double t1 = std::fabs(v1n[0]) < snapThreshold ? (iy - l1a[1]) / v1[1] : (ix - l1a[0]) / v1[0];

    // check if intersection is in valid area
    if ((t0 < -snapThreshold || t0 > 1.0 + snapThreshold) || (t1 < -snapThreshold || t1 > 1.0 + snapThreshold))
    {
        return false;
    }

    i = l0a + v0 * geometry::Scalar(t0);
#ifdef _DEBUG
    assert(!isnan(i[0]));
    assert(!isnan(i[1]));
#endif
    return true;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Calculates intersection between two line segments
//!
//!\param   l0a             Line0's first point
//!\param   l0b             Line0's second point
//!\param   l1a             Line1's first point
//!\param   l1b             Line1's second point
//!\param   i               Intersection point
//!\param   includePoints   Flag if lines' points can touch without causing intersection
//!
//!\return  True if lines intersect each other, false otherwise
////////////////////////////////////////////////////////////////////////////////////////////////////
bool calculateIntersection3d(const geometry::Vec3 &l0a, const geometry::Vec3 &l0b, const geometry::Vec3 &l1a, const geometry::Vec3 &l1b, geometry::Vec3 &i, double snapThreshold, double angleThreshold)
{
#if(1)
    geometry::CPlane plane = geometry::CPlane(l0a, l0b, l1a);
    if (std::fabs(plane.distance(l1b)) > snapThreshold)
    {
        return false;
    }

    geometry::Vec3 u = l0b - l0a;
    geometry::Vec3 v = l1b - l1a;

    geometry::Vec3 un = u;
    geometry::Vec3 vn = v;
    un.normalize();
    vn.normalize();

    // check if ray and segment are parallel
    if (1.0 - std::fabs(un * vn) < angleThreshold)
    {
        return false;
    }

    int i0 = 0;
    i0 = (std::fabs(u[0]) > std::fabs(u[1]) ? 0 : 1);
    i0 = (std::fabs(u[2]) > std::fabs(u[i0]) ? 2 : i0);

    int i1 = 0;
    i1 = (std::max(std::fabs(u[(i0 + 1) % 3]), std::fabs(v[(i0 + 1) % 3])) > std::max(std::fabs(u[(i0 + 2) % 3]), std::fabs(v[(i0 + 2) % 3])) ? (i0 + 1) % 3 : (i0 + 2) % 3);

    double denominator = v[i0] * u[i1] - v[i1] * u[i0];
    if (std::fabs(denominator) < snapThreshold)
    {
        return false;
    }

    double t2 = (u[i0] * (l1a[i1] - l0a[i1]) - l1a[i0] * u[i1] + l0a[i0] * u[i1]) / denominator;
    double t1 = (l1a[i0] + t2 * v[i0] - l0a[i0]) / u[i0];

    double uLength = u.length();
    double vLength = v.length();

    if (t1 * uLength < -snapThreshold || t1 * uLength > uLength + snapThreshold || t2 * vLength < -snapThreshold || t2 * vLength > vLength + snapThreshold)
    {
        return false;
    }

    i = l0a + u * geometry::Scalar(t1);

    return true;

#else
#ifdef _DEBUG
    if (isnan(l0a) || isnan(l0b) || isnan(l1a) || isnan(l1b))
    {
        std::cout << "nan!";
        return false;
    }
#endif

    geometry::Vec3 u = l0b - l0a;
    geometry::Vec3 v = l1b - l1a;
    geometry::Vec3 w = l0a - l1a;

    double a = u * u;
    double b = u * v;
    double c = v * v;
    double d = u * w;
    double e = v * w;
    double D = a * c - b * b;
    double sc = D;
    double sN = D;
    double sD = D;
    double tc = D;
    double tN = D;
    double tD = D;

    if (D < snapThreshold)
    {
        sN = 0.0;
        sD = 1.0;
        tN = e;
        tD = c;
    }
    else
    {
        sN = (b * e - c * d);
        tN = (a * e - b * d);
        if (sN < 0.0)
        {
            sN = 0.0;
            tN = e;
            tD = c;
        }
        else if (sN > sD)
        {
            sN = sD;
            tN = e + b;
            tD = c;
        }
    }

    if (tN < 0.0)
    {
        tN = 0.0;
        if (-d < 0.0)
        {
            sN = 0.0;
        }
        else if (-d > a)
        {
            sN = sD;
        }
        else
        {
            sN = -d;
            sD = a;
        }
    }
    else if (tN > tD)
    {
        tN = tD;
        if ((-d + b) < 0.0)
        {
            sN = 0;
        }
        else if ((-d + b) > a)
        {
            sN = sD;
        }
        else
        {
            sN = (-d + b);
            sD = a;
        }
    }
    sc = (std::fabs(sN) < snapThreshold ? 0.0 : sN / sD);
    tc = (std::fabs(tN) < snapThreshold ? 0.0 : tN / tD);

    geometry::Vec3 usc = u * sc;
    geometry::Vec3 vtc = v * tc;

    geometry::Vec3 dP = w + usc - vtc;

    if (dP.length() > snapThreshold)
    {
        return false;
    }

    geometry::Vec3 i0 = l0a + usc;
    geometry::Vec3 i1 = l0a - w + vtc;
    geometry::Vec3 i2 = l1a + vtc;
    geometry::Vec3o i3 = l0a - w + vtc + dP;

    i = i0;

#ifdef _DEBUG
    assert(!isnan(i[0]));
    assert(!isnan(i[1]));
    assert(!isnan(i[2]));
#endif

    return true;
#endif
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Performs very simple offset of polygon along point normals
//!         (! only use for tiny offsets as it does not solves overlaps, intersections etc.)
//!
//!\param   offset  Offset
//!\param   polygon Polygon
////////////////////////////////////////////////////////////////////////////////////////////////////
void dummyOffset(double offset, std::vector<geometry::Vec2> &polygon)
{
    polygon.resize(polygon.size() - 1);

    std::vector<geometry::Vec2> normals;
    for (int i = 0; i < polygon.size(); ++i)
    {
        geometry::Vec2 prev = polygon[(polygon.size() + i - 1) % polygon.size()];
        geometry::Vec2 curr = polygon[i];
        geometry::Vec2 next = polygon[(i + 1) % polygon.size()];

        geometry::Vec2 vec0 = curr - prev;
        geometry::Vec2 vec1 = next - curr;

        vec0.normalize();
        vec1.normalize();

        vec0 = geometry::Vec2(-vec0[1], vec0[0]);
        vec1 = geometry::Vec2(-vec1[1], vec1[0]);

        geometry::Vec2 normal = vec0 + vec1;
        normal.normalize();

        normals.push_back(normal);
    }

    for (int i = 0; i < polygon.size(); ++i)
    {
        polygon[i] += normals[i] * geometry::Scalar(offset);
    }

    polygon.push_back(polygon.front());
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Checks if point lies within polygon
//!
//!\param   p       Point
//!\param   polygon Polygon
//!
//!\return  True if point lies in polygon, false otherwise
////////////////////////////////////////////////////////////////////////////////////////////////////
bool pointInPolygon(const geometry::Vec2 &p, const std::vector<geometry::Vec2> &polygon)
{
    int i_first = 0;
    int i_last = polygon.size() - 1;
    int i_curr = 0;
    bool result = false;
    {
        do
        {
            geometry::Vec2 curr = polygon[i_curr];
            geometry::Vec2 prev = polygon[(polygon.size() - 1 + i_curr - 1) % (polygon.size() - 1)];

            if ((((curr.y() <= p.y()) && (p.y() < prev.y())) || ((prev.y() <= p.y()) && (p.y() < curr.y()))) &&
                (p.x() < (prev.x() - curr.x()) * (p.y() - curr.y()) / (prev.y() - curr.y()) + curr.x()))
            {
                result = !result;
            }
            i_curr = (i_curr + 1) % polygon.size();
        }
        while (i_curr != i_last);
    }
    return result;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Calculates signed area of polygon
//!
//!\param   polygon Polygon
//!
//!\ return Signed area of polygon
////////////////////////////////////////////////////////////////////////////////////////////////////
double polygonArea(const std::vector<geometry::Vec2> &polygon)
{
    int last = polygon.size() - 1;

    double a = (polygon[last][0] + polygon[0][0]) * (polygon[0][1] - polygon[last][1]);
    for (int i = 1; i < polygon.size(); ++i)
    {
        geometry::Vec2 vertex0 = polygon[i];
        geometry::Vec2 vertex1 = polygon[i - 1];

        a += (vertex1[0] + vertex0[0]) * (vertex0[1] - vertex1[1]);
    }

    return a / 2;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Checks orientation of polygon
//!
//!\param   polygon List of vertices forming polygon or a hole
//!
//!\ return True if polygon is a polygon (CW order), false if it is a hole (CCW order)
////////////////////////////////////////////////////////////////////////////////////////////////////
bool isPolygon(const std::vector<geometry::Vec2> &polygon)
{
    return polygonArea(polygon) < 0.0;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Checks orientation of triangle
//!
//!\param   v0 Vertex #0
//!\param   v1 Vertex #1
//!\param   v2 Vertex #2
//!
//!\ return True if vertices form a polygon (CW order), false if it is a hole (CCW order)
////////////////////////////////////////////////////////////////////////////////////////////////////
bool isPolygon(const geometry::Vec2 &v0, const geometry::Vec2 &v1, const geometry::Vec2 &v2)
{
#if(0)
    std::vector<geometry::Vec2> polygon;
    polygon.push_back(v0);
    polygon.push_back(v1);
    polygon.push_back(v2);
    return isPolygon(polygon);
#else
    double a = (v2.x() + v0.x()) * (v0.y() - v2.y());
    a += (v1.x() + v0.x()) * (v1.y() - v0.y());
    a += (v2.x() + v1.x()) * (v2.y() - v1.y());
    return a < 0.0; //return (a / 2) > 0.0;
#endif
}

bool triOnPlane(const geometry::CPlane &plane, const geometry::Vec3 &pointA, const geometry::Vec3 &pointB, const geometry::Vec3 &pointC, double snapThreshold)
{
    double distanceA = std::fabs(plane.distance(pointA));
    double distanceB = std::fabs(plane.distance(pointB));
    double distanceC = std::fabs(plane.distance(pointC));

    if (distanceA + distanceB + distanceC < snapThreshold)
    {
        return true;
    }

    return false;
}

void getLookAt(const geometry::Matrix &matrix, geometry::Vec3 &eye, geometry::Vec3 &center, geometry::Vec3 &up, double lookDistance)
{
    geometry::Matrix inv = geometry::Matrix::inverse(matrix);

    // note: e and c variables must be used inside this method instead of eye and center
    // because eye and center are references and they may point to the same variable.
    geometry::Vec3 e = inv.operator*(geometry::Vec3(0.0, 0.0, 0.0));
    geometry::Vec3 u = inv.operator*(geometry::Vec3(0.0, 1.0, 0.0));
    geometry::Vec3 f = inv.operator*(geometry::Vec3(0.0, 0.0, -1.0));
    u = u - e;
    f = f - e;
    u.normalize();
    f.normalize();
    geometry::Vec3 c = e + f * geometry::Scalar(lookDistance);

    // assign the results
    eye = e;
    center = c;
    up = u;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Calculates Principal Component Analysis
//!
//!\param   pc              Point cloud
//!\param   eigenVectors    Result eigen vectors
//!\param   eigenValues     Result eigen values
//!
////////////////////////////////////////////////////////////////////////////////////////////////////
void pca(const std::vector<geometry::Vec3> & pc, std::array<geometry::Vec3, 3>& eigenVectors, std::array<double, 3>& eigenValues)
{
    if (pc.empty())
    {
        return;
    }

    Eigen::MatrixXd mat(pc.size(), 3);

    for (unsigned i = 0; i < pc.size(); i++)
    {
        mat.row(i) = Eigen::Vector3d(pc[i].getPtr());
    }
    Eigen::MatrixXd centered = mat.rowwise() - mat.colwise().mean();
    Eigen::MatrixXd cov = centered.adjoint() * centered;

    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(cov);

    // Sort eigenvectors and eigenvalues in descendent order
    for (int i = 0; i < 3; ++i)
    {
        eigenValues[i] = eig.eigenvalues()[2 - i];
        eigenVectors[i][0] = eig.eigenvectors().col(2 - i)[0];
        eigenVectors[i][1] = eig.eigenvectors().col(2 - i)[1];
        eigenVectors[i][2] = eig.eigenvectors().col(2 - i)[2];
    }
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Finds connected geometry of given mesh in defined radius
//!
//!\param   mesh        Input mesh
//!\param   seed        Starting vertex
//!\param   center      center of search radius
//!\param   radius      search radius
//!\param   handles     found vertex handles
//!\param   neighbors   found vertices
//!\param   distances   distances of vertices from center
//!
////////////////////////////////////////////////////////////////////////////////////////////////////
void getConnectedGeometryInRadius(const geometry::CMesh& mesh, const geometry::CMesh::VertexHandle seed, const geometry::Vec3 center, const float radius, std::vector<geometry::CMesh::VertexHandle>& handles, std::vector<geometry::Vec3>& neighbors, std::vector<double>& distances)
{
    handles.clear();
    neighbors.clear();
    distances.clear();
    geometry::CMesh copy(mesh);
    geometry::CMesh::Point s = copy.point(seed);
    geometry::CMesh::Point c(center[0], center[1], center[2]);
    double cdist = (s - c).length();
    if (cdist > radius)
    {
        return;
    }

    OpenMesh::VPropHandleT< bool > visited;
    copy.add_property(visited);

    geometry::CMesh::VertexIter v_it, v_end;
    v_end = copy.vertices_end();

    // iterate over all vertices
    for (v_it = copy.vertices_begin(); v_it != v_end; ++v_it)
    {
        copy.property(visited, v_it) = false;
    }

    handles.push_back(seed);
    neighbors.push_back(geometry::Vec3(s[0], s[1], s[2]));
    distances.push_back(cdist);
    copy.property(visited, seed) = true;
    int i = 0;
    while (i < handles.size())
    {
        geometry::CMesh::VertexHandle current = handles[i];
        for (geometry::CMesh::VertexVertexIter vv_it = copy.vv_iter(current); vv_it.is_valid(); ++vv_it)
        {
            if (copy.property(visited, vv_it))
            {
                continue;
            }

            geometry::CMesh::Point p = copy.point(vv_it.handle());
            double dist = (p - c).length();
            if (dist < radius)
            {
                handles.push_back(vv_it.handle());
                neighbors.push_back(geometry::Vec3(p[0], p[1], p[2]));
                distances.push_back(dist);
            }
            copy.property(visited, vv_it) = true;
        }
        i++;
    }
    copy.remove_property(visited);
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Projects point on line
//!
//!\param   pointOnLine     Point on line
//!\param   lineDirection   Line direction
//!\param   queryPoint      Point of intersection
//!\param   distance        Signed distance from pointOnLine to projected point
//!
//!\return  Projected point
////////////////////////////////////////////////////////////////////////////////////////////////////
geometry::Vec3 closestPointOnLine(const geometry::Vec3& pointOnLine, const geometry::Vec3& lineDirection, const geometry::Vec3& queryPoint, double& distance)
{
    geometry::Vec3 pq = queryPoint - pointOnLine;
    geometry::Vec3 l(lineDirection);
    distance = (pq * l) / (l * l);
    geometry::Vec3 result = pointOnLine + (l * vpl::CScalar<double>(distance));
    return result;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Projects point on plane
//!
//!\param   pointOnPlane    Point on plane
//!\param   planeNormal     Normal vector of plane
//!\param   queryPoint      Point of intersection
//!\param   distance        Signed distance from queryPoint to plane
//!
//!\return  Projected point
////////////////////////////////////////////////////////////////////////////////////////////////////
geometry::Vec3 projectPointOnPlane(const geometry::Vec3& pointOnPlane, const geometry::Vec3& planeNormal, const geometry::Vec3& queryPoint, double& distance)
{
    closestPointOnLine(pointOnPlane, planeNormal, queryPoint, distance);
    geometry::Vec3 result = queryPoint - (planeNormal * vpl::CScalar<double>(distance));
    return result;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Checks if line intesects plane
//! this treats edge case of line coincident with plane as no intersection
//!
//!\param   pointOnPlane        Point on plane
//!\param   planeNormal         Normal vector of plane
//!\param   pointOnLine         Point on line
//!\param   lineDirection       Line direction
//!\param   intersectionPoint   Point of intersection
//!\param   distance            Signed distance from pointOnLine to intersectionPoint
//!
//!\return  True if there is any intersection found
////////////////////////////////////////////////////////////////////////////////////////////////////
bool linePlaneIntersection(const geometry::Vec3& pointOnPlane, const geometry::Vec3& planeNormal, const geometry::Vec3& pointOnLine, const geometry::Vec3& lineDirection, geometry::Vec3& intersectionPoint, double& distance)
{
    double dot = lineDirection * planeNormal;
    double dist2plane = 0.0;
    closestPointOnLine(pointOnPlane, planeNormal, pointOnLine, dist2plane);
    if (std::fabs(dot) > 0.000001)
    {
        // point intersection
        distance = -dist2plane / dot;
        intersectionPoint = pointOnLine + (lineDirection * vpl::CScalar<double>(distance));
        return true;
    }

    // no intersection
    intersectionPoint = geometry::Vec3(0.0, 0.0, 0.0);
    return false;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Checks if point lies inside of triangle
//!
//!\param   point           Point
//!\param   triangle        Vector of three triangle vertices
//!
//!\return  True if the point lies inside of the triangle
////////////////////////////////////////////////////////////////////////////////////////////////////
bool pointInTriangle(const geometry::Vec2 & point, const std::vector<geometry::Vec2>& triangle)
{
    // check triangle size
    if (triangle.size() != 3)
        return false;

    // compute barycentric coordinates of point
    double ABlen = (triangle[1] - triangle[0]).length();
    double AClen = (triangle[2] - triangle[0]).length();
    double PAlen = (triangle[0] - point).length();
    double PBlen = (triangle[1] - point).length();
    double PClen = (triangle[2] - point).length();
    double inv2areaABC = 1.0 / (ABlen * AClen);
    double alpha = PBlen * PClen * inv2areaABC;
    double beta = PClen * PAlen * inv2areaABC;
    double gamma = 1.0 - alpha - beta;

    return alpha >= 0.0 && alpha <= 1.0 && beta >= 0.0 && beta <= 1.0 && gamma >= 0.0 && gamma <= 1.0;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Checks if point lies inside of triangle
//!
//!\param   point           Point
//!\param   triangle        Vector of three triangle vertices
//!\param   triangleNormal  Normal vector of triangle
//!
//!\return  True if the point lies inside of the triangle
////////////////////////////////////////////////////////////////////////////////////////////////////
bool pointInTriangle(const geometry::Vec3& point, const std::vector<geometry::Vec3>& triangle, const geometry::Vec3& triangleNormal)
{
    // check triangle size
    if (triangle.size() != 3)
        return false;

    // check if point lies on triangle plane
    double distance = 0.0;
    closestPointOnLine(triangle[0], triangleNormal, point, distance);
    if (std::fabs(distance) > 0.000001)
        return false;

    // find biggest component of normal
    geometry::Vec3 N = geometry::Vec3(triangleNormal).abs();
    int idx = -1;
    if (N[0] > N[1])
    {
        if (N[0] > N[2])
            idx = 0;
        else
            idx = 2;
    }
    else if (N[1] > N[2])
        idx = 1;
    else
        idx = 2;

    // project point and triangle on one of the main planes (XY, YZ, XZ) that is most parallel with triangle plane
    geometry::Vec2 P2;
    std::vector<geometry::Vec2> T2(3);
    for (int i = 0, c = 0; i < 3; ++i)
    {
        if (i == idx)
            continue;
        P2[c] = point[i];
        T2[0][c] = triangle[0][i];
        T2[1][c] = triangle[1][i];
        T2[2][c] = triangle[2][i];
        c++;
    }
    // test in 2D
    return pointInTriangle(P2, T2);
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Checks if line intersects triangle
//!
//!\param   triangle            Vector of three triangle vertices
//!\param   triangleNormal      Normal vector of triangle
//!\param   pointOnLine         Point on line
//!\param   lineDirection       Line direction
//!\param   intersectionPoint   Point of intersection
//!\param   distance            Signed distance from pointOnLine to intersection
//!
//!\return  True if there is any intersection found
////////////////////////////////////////////////////////////////////////////////////////////////////
bool lineTriangleIntersection(const std::vector<geometry::Vec3>& triangle, const geometry::Vec3& triangleNormal, const geometry::Vec3& pointOnLine, const geometry::Vec3& lineDirection, geometry::Vec3& intersectionPoint, double& distance)
{
    if (triangle.size() != 3)
        return false;

    if (!linePlaneIntersection(triangle[0], triangleNormal, pointOnLine, lineDirection, intersectionPoint, distance))
        return false;

    return pointInTriangle(intersectionPoint, triangle, triangleNormal);
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Checks if point is inside of mesh
//! if the mesh is opened, the algorithm assumes the maximal dimensions in the given direction (upVector) as mesh boundaries
//! octree of mesh needs to be updated
//!
//!\param   mesh        Input mesh
//!\param   point       Point in question
//!\param   upVector    Vector pointing upwards
//!
//!\return  True if the point is inside of mesh
////////////////////////////////////////////////////////////////////////////////////////////////////
bool isPointInsideMesh(geometry::CMesh& mesh, const geometry::Vec3& point, const geometry::Vec3& upVector)
{
    // find dimensions of model in direction of upVector
    double maxDist = -std::numeric_limits<double>::max(), minDist = std::numeric_limits<double>::max();
    geometry::Vec3 maxVec, minVec;
    geometry::CMesh::VertexIter v_it = mesh.vertices_begin(), v_end = mesh.vertices_end();
    for (; v_it != v_end; ++v_it)
    {
        geometry::Vec3 p = geometry::convert3<geometry::Vec3>(mesh.point(v_it.handle()));
        double distance = 0.0;
        geometry::Vec3 q = closestPointOnLine(point, upVector, p, distance);
        if (distance > maxDist)
        {
            maxDist = distance;
            maxVec = q;
        }
        if (distance < minDist)
        {
            minDist = distance;
            minVec = q;
        }
    }

    // point lies outside of model dimensions
    if (maxDist < 0.0 || minDist > 0.0)
        return false;

    // get nodes from octree
    osg::BoundingBox bb;
    bb.expandBy(minVec[0], minVec[1], minVec[2]);
    bb.expandBy(maxVec[0], maxVec[1], maxVec[2]);
    // make sure the bounding box is valid
    bb.expandBy(minVec[0] - 0.1, minVec[1] - 0.1, minVec[2] - 0.1);
    bb.expandBy(maxVec[0] + 0.1, maxVec[1] + 0.1, maxVec[2] + 0.1);
    const std::vector<geometry::CMeshOctreeNode *> &nodes = mesh.getOctree()->getIntersectedNodes(bb);

    // get triangles from octree
    std::vector<geometry::CMesh::FaceHandle> faceHandles;

    // use reverse iterators because the last nodes are the leaf nodes and should include closer triangles
    std::vector<geometry::CMeshOctreeNode *>::const_reverse_iterator itn = nodes.rbegin(), itnEnd = nodes.rend();
    for (; itn != itnEnd; ++itn)
    {
        faceHandles.insert(faceHandles.end(), (*itn)->faces.begin(), (*itn)->faces.end());
    }

    // structure for intersections data
    typedef struct SIntersection {
        geometry::Vec3 point;
        double distance;
        bool faceUp;
        bool invalid;
    } SIntersection;

    std::vector<SIntersection> intersections(faceHandles.size());

    // find all faces that intersects with the line
    SIntersection invalidIntersection = { geometry::Vec3(), 0.0, false, true };
#pragma omp parallel for shared (intersections)
    for (int i = 0; i < static_cast<int>(faceHandles.size()); ++i)
    {
        geometry::CMesh::FaceHandle fh = faceHandles[i];
        std::vector<geometry::Vec3> triangle;
        triangle.reserve(3);
        for (geometry::CMesh::ConstFaceVertexIter fv_it = mesh.cfv_iter(fh); fv_it.is_valid(); ++fv_it)
        {
            geometry::CMesh::Point p = mesh.point(fv_it);
            triangle.push_back(geometry::convert3<geometry::Vec3>(p));
        }
        if (triangle.size() != 3)
        {
            intersections[i] = invalidIntersection;
            continue;
        }

        geometry::Vec3 triangleNormal = geometry::convert3<geometry::Vec3>(mesh.normal(fh));
        triangleNormal.normalize();

        // find intersection with triangle
        geometry::Vec3 intersection;
        double distance = 0.0;
        if (!lineTriangleIntersection(triangle, triangleNormal, point, upVector, intersection, distance))
        {
            intersections[i] = invalidIntersection;
            continue;
        }

        // save intersection data
        bool faceUp = (upVector * triangleNormal) > 0.0;
        intersections[i] = { intersection, distance, faceUp, false };
    }

    // clear invalid intersections
    intersections.erase(std::remove_if(intersections.begin(), intersections.end(), [](SIntersection intersection) {return intersection.invalid; }), intersections.end());

    // no intersection, point is outside
    double size = intersections.size();
    if (size <= 0)
        return false;

    // sort intersections from bottom up
    std::sort(intersections.begin(), intersections.end(), [](SIntersection a, SIntersection b) { return a.distance < b.distance; });

    std::vector<SIntersection> copy;
    copy.reserve(intersections.size()+2);

    // Go through intersections and store only change of state
    ////////////////////////////////////////////////////////////////////////////////
    //  intersections:
    //          min          point                 max
    //        ---|------)--)---0----(-(-(--)--------|---
    //
    //  copy:
    //        ---(---------)---0----(------)------------
    //        out    in       out      in      out
    ////////////////////////////////////////////////////////////////////////////////
    bool inside = false;
    std::vector<SIntersection>::iterator it = intersections.begin(), itEnd = intersections.end();
    for (; it != itEnd; ++it)
    {
        if (inside)
        {
            if (!it->faceUp)
            { // when "inside" ignore other inside region openings
                continue;
            }
            else
            { // closeing of last inside region
                copy.push_back(*it);
                inside = false;
            }
        }
        else
        {
            if (!it->faceUp)
            { // opening of new inside region
                copy.push_back(*it);
                inside = true;
            }
            else
            {
                if (copy.empty())
                {
                    // insert cap to "close" the model
                    copy.push_back({ (point + upVector * vpl::CScalar<double>(minDist)), minDist, false });
                    // insert current intersectin  to close the inside reguion
                    copy.push_back(*it);
                }
                else
                { // overwrite the closing of last inside region
                    copy.back() = *it;
                }
            }
        }

    }
    if (!copy.empty() && !copy.back().faceUp)
    { // insert cap to "close" the model
        copy.push_back({ (point + upVector * vpl::CScalar<double>(maxDist)), maxDist, true });
    }

    // go through intersections vector and check if point is inside, 0.0 is position of point
    std::vector<SIntersection>::iterator it2 = copy.begin(), it2End = copy.end();
    for (; it2 != it2End; ++it2)
    {
        if (it2->faceUp && it2->distance >= 0.0)
            return true; // point inside
        if (!it2->faceUp && it2->distance > 0.0)
            return false; // point outside
    }

    return false;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Creates new mesh from given mesh including only selected vertices and faces betweeen them
//!
//!\param   mesh    Source mesh
//!\param   handles Collection of vertex handles
//!
//!\return  New mesh
////////////////////////////////////////////////////////////////////////////////////////////////////
geometry::CMesh * cutMeshByVerticesSubset(const geometry::CMesh& mesh, std::vector<geometry::CMesh::VertexHandle>& handles)
{
    if (handles.size() < 3)
        return nullptr;

    geometry::CMesh* cutoutMesh = new geometry::CMesh();

    std::unordered_map<geometry::CMesh::VertexHandle, geometry::CMesh::VertexHandle> vhandlesMap;
    for (auto& vh : handles)
    { // copy vertices into new CMesh
        geometry::CMesh::Point p = mesh.point(vh);
        vhandlesMap[vh] = cutoutMesh->add_vertex(p);
    }

    if (cutoutMesh->n_vertices() < 3)
    {
        delete cutoutMesh;
        return nullptr;
    }

//     for (geometry::CMesh::FaceIter f_it = mesh.faces_begin(); f_it != mesh.faces_end(); ++f_it)
//     { // copy faces into new CMesh
//         std::vector<geometry::CMesh::VertexHandle> fvhandles;
//         for (geometry::CMesh::ConstFaceVertexIter fv_it = mesh.cfv_iter(f_it.handle()); fv_it.is_valid(); ++fv_it)
//         {
//             if (vhandlesMap.find(fv_it.handle()) != vhandlesMap.end())
//             {
//                 fvhandles.push_back(vhandlesMap[fv_it.handle()]);
//             }
//         }
//         if (fvhandles.size() == 3)
//         { // all three face vertices are in defined subset
//             cutoutMesh->add_face(fvhandles);
//         }
//     }

    std::unordered_set<geometry::CMesh::FaceHandle> faces_created;
    std::vector<geometry::CMesh::VertexHandle> fvhandles;
    for (auto &vh : handles)
    {
        // For all faces around this vertex
        for (geometry::CMesh::ConstVertexFaceIter vf_it = mesh.cvf_iter(vh); vf_it.is_valid(); ++vf_it)
        {
            const geometry::CMesh::FaceHandle fh = vf_it.handle();
            // If face not created yet
            if (faces_created.find(fh) == faces_created.end())
            {
                // Circulate around face and test if all vertices are in the enabled vertices
                for (geometry::CMesh::ConstFaceVertexIter fv_it = mesh.cfv_iter(fh); fv_it.is_valid(); ++fv_it)
                {
                    std::unordered_map<geometry::CMesh::VertexHandle, geometry::CMesh::VertexHandle>::const_iterator it = vhandlesMap.find(fv_it.handle());
                    if (it == vhandlesMap.end())
                        break;
                    fvhandles.push_back(it->second);
                }
                // all three face vertices are in defined subset
                if (fvhandles.size() == 3)
                    cutoutMesh->add_face(fvhandles);

                // clear handles vector for reuse
                fvhandles.clear();

                // Set face as tested (and possibly created)
                faces_created.insert(fh);
            }
        }
    }

    if (cutoutMesh->n_faces() < 1)
    {
        delete cutoutMesh;
        return nullptr;
    }

    return cutoutMesh;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Checks if polygon is self-intersecting
//!
//!\param   polygon     Polygon to check
//!
//!\return  true if contains self-intersection, false otherwise
////////////////////////////////////////////////////////////////////////////////////////////////////
bool checkSelfIntersections(std::vector<geometry::Vec2> &polygon, double snapThreshold, double angleThreshold)
{
    for (int l0 = 0; l0 < polygon.size() - 1; ++l0)
    {
        geometry::Vec2 l0a = polygon[l0 + 0];
        geometry::Vec2 l0b = polygon[l0 + 1];

        for (int l1 = l0 + 2; l1 < polygon.size() - 1; ++l1)
        {
            geometry::Vec2 l1a = polygon[l1 + 0];
            geometry::Vec2 l1b = polygon[l1 + 1];
            geometry::Vec2 i;

            if (calculateIntersection2d(l0a, l0b, l1a, l1b, i, snapThreshold, angleThreshold, false))
            {
                return true;
            }
        }
    }

    return false;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Calculates Euler angles from given quaternion
//! Quaternion does not need to be normalized
//! Resulting rotation is supposed to be applied in this order: X firts, Y second and Z last
//! Right hand coordinate system
//! Angles are in radians
//!
//!\param   v0 Vertex #0
//!\param   v1 Vertex #1
//!\param   v2 Vertex #2
//!
//!\return  True if vertices form a polygon (CW order), false if it is a hole (CCW order)
////////////////////////////////////////////////////////////////////////////////////////////////////
geometry::Vec3 toEuler(const geometry::Quat &q)
{
    double sqx = q.x() * q.x();
    double sqy = q.y() * q.y();
    double sqz = q.z() * q.z();
    double sqw = q.w() * q.w();

    double unit = sqx + sqy + sqz + sqw;
    double test = q.x() * q.y() + q.z() * q.w();
    double x, y, z;

    if (test > 0.499)
    {
        y = 2 * std::atan2(q.x(), q.w());
        z = vpl::math::getPi<double>() * 0.5;
        x = 0.0;
    }
    else if (test < -0.499)
    {
        y = -2 * std::atan2(q.x(), q.w());
        z = -vpl::math::getPi<double>() * 0.5;
        x = 0.0;
    }
    else
    {
        y = atan2(2 * q.y() * q.w() - 2 * q.x() * q.z(), sqx - sqy - sqz + sqw);
        z = asin(2 * test / unit);
        x = atan2(2 * q.x() * q.w() - 2 * q.y() * q.z(), -sqx + sqy - sqz + sqw);
    }

    return geometry::Vec3(x, y, z);
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Compares two matrices
//!
//!\param   a   Matrix a
//!\param   b   Matrix b
//!
//!\return  True if matrices are the same, false otherwise
////////////////////////////////////////////////////////////////////////////////////////////////////
bool compareMatrices(const geometry::Matrix &a, const geometry::Matrix &b)
{
    for (int y = 0; y < 4; ++y)
    {
        for (int x = 0; x < 4; ++x)
        {
            if (a.at(y, x) != b.at(y, x))
            {
                return false;
            }
        }
    }

    return true;
}

bool isGuideTubeInCollisionWithMesh(geometry::CMesh& mesh, const geometry::Matrix& modelMatrix, const double GTOffset, const double GTHeight, const double GTDistance, const double GTAllowance, const double MCHeight, const double MCDiameter, const double MCLipWidth, const geometry::Vec3& IPosition, const geometry::Vec3& IDirection)
{
    // get implant position and direction in model coordinates
    geometry::Matrix invModelMat = geometry::Matrix::inverse(modelMatrix);
    geometry::Vec3 position(invModelMat * IPosition);
    geometry::Vec3 translation(invModelMat.getTrans<double>());
    invModelMat.setTrans(translation);
    geometry::Vec3 direction((invModelMat * IDirection) - translation);
    direction.normalize();

    // compute center point and up vector
    geometry::Vec3 center = position - direction * geometry::Scalar(GTOffset);
    geometry::Vec3 upVector((invModelMat * geometry::Vec3(0.0, 0.0, 1.0)) - translation);
    upVector.normalize();

    // Compute dimmension of first collision cylinder (inside of guide tube)
    double cyl1radius = (MCDiameter + GTDistance) * 0.5;
    double cyl1height = std::max(0.0, GTHeight - GTAllowance);
    geometry::Vec3 cyl1center = position - direction * geometry::Scalar(GTOffset - (cyl1height * 0.5));

    // Compute dimmension of second collision cylinder (on top of guide tube)
    double cyl2radius = ((MCDiameter + GTDistance) * 0.5) + MCLipWidth;
    double cyl2height = MCHeight;
    geometry::Vec3 cyl2center = position - direction * geometry::Scalar(GTOffset + (cyl2height * 0.5));

    // Compute bounding box of collision cylinders
    geometry::Vec3 bbcenter = center;
    double bbhside = std::sqrt((cyl2radius * cyl2radius) + (cyl2height * cyl2height));
    geometry::Vec3 bbhdist(bbhside, bbhside, bbhside);
    osg::BoundingBox bb(geometry::convert3<osg::Vec3>(bbcenter - bbhdist), geometry::convert3<osg::Vec3>(bbcenter + bbhdist));

    if (nullptr == mesh.getOctree())
    {
        assert(false); // bug?
        return false;
    }

    // get nodes from octree
    const std::vector<geometry::CMeshOctreeNode *> &nodes = mesh.getOctree()->getIntersectedNodes(bb);

    // get triangles from octree
    std::vector<geometry::CMesh::FaceHandle> faceHandles;

    // use reverse iterators because the last nodes are the leaf nodes and should include closer triangles
    std::vector<geometry::CMeshOctreeNode *>::const_reverse_iterator itn = nodes.rbegin(), itnEnd = nodes.rend();
    for (; itn != itnEnd; ++itn)
    {
        faceHandles.insert(faceHandles.end(), (*itn)->faces.begin(), (*itn)->faces.end());
    }

    bool intersectionFound = false;
    // For all faces
    int facesSize = static_cast<int>(faceHandles.size());
#pragma omp parallel for
    for (int i = 0; i < facesSize; ++i)
    {
        // check if intersection was already found
#pragma omp flush (intersectionFound)
        if (!intersectionFound)
        {
            geometry::CMesh::FaceHandle fh = faceHandles[i];
            std::vector<geometry::Vec3> triangle;
            triangle.reserve(3);

            // Circulate around face and save all vertices into triangles vector
            for (geometry::CMesh::ConstFaceVertexIter fv_it = mesh.cfv_iter(fh); fv_it.is_valid(); ++fv_it)
            {
                geometry::CMesh::Point p = mesh.point(*fv_it);
                triangle.push_back(geometry::convert3<geometry::Vec3>(p));
            }

            //check if face intersects any ot the cylinders
            if (geometry::triangleCylinderIntersection(triangle, cyl1center, direction, cyl1height, cyl1radius))
            {
                intersectionFound = true;
#pragma omp flush (intersectionFound)
                continue;
            }

            if (geometry::triangleCylinderIntersection(triangle, cyl2center, direction, cyl2height, cyl2radius))
            {
                intersectionFound = true;
#pragma omp flush (intersectionFound)
                continue;
            }
        }
    }

    // if some intersection was found
    if (intersectionFound)
        return true;

    // triangleCylinderIntersection handles only mesh surface, we approximate the metal cylinder as one point and test if it lies inside of the mesh
    if (geometry::isPointInsideMesh(mesh, center, upVector))
        return true;

    return false;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Generates orthonormal basis
//! The input vector must be normalized to create orthonormal basis
//!
//!\param   u first output base vector
//!\param   v second output base vector
//!\param   w input base vector
//!
////////////////////////////////////////////////////////////////////////////////////////////////////
void generateOrthonormalBasis(geometry::Vec3 &u, geometry::Vec3 &v, const geometry::Vec3 &w)
{
    double invLength;

    if (std::fabs(w[0]) >= std::fabs(w[1]))
    {
        // W.x or W.z is the largest magnitude component, swap them
        invLength = 1.0 / std::sqrt(w[0] * w[0] + w[2] * w[2]);
        u[0] = -w[2] * invLength;
        u[1] = 0.0;
        u[2] = +w[0] * invLength;
        v[0] =  w[1] * u[2];
        v[1] =  w[2] * u[0] - w[0] * u[2];
        v[2] = -w[1] * u[0];
    }
    else
    {
        // W.y or W.z is the largest magnitude component, swap them
        invLength = 1.0 / std::sqrt(w[1] * w[1] + w[2] * w[2]);
        u[0] = 0.0;
        u[1] = +w[2] * invLength;
        u[2] = -w[1] * invLength;
        v[0] =  w[1] * u[2] - w[2] * u[1];
        v[1] = -w[0] * u[2];
        v[2] =  w[0] * u[1];
    }
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Checks i there is any intersection between cylinder and triangle
//!
//!\param   triangle            Vector with three vertices of triangle
//!\param   cylinderCenter      Center of cylinder
//!\param   cylinderDirection   orientation of cylinder
//!\param   cylinderHeight      height of cylinder
//!\param   cylinderRadius      radius of cylinder
//!
//!\return  True if there is any intersection found
////////////////////////////////////////////////////////////////////////////////////////////////////
bool triangleCylinderIntersection(const std::vector<geometry::Vec3> &triangle, const geometry::Vec3 &cylinderCenter, const geometry::Vec3 &cylinderDirection, const double cylinderHeight, const double cylinderRadius)
{
    if (triangle.size() < 3)
        return false;

    // Get an orthonormal basis {U,V,D}
    geometry::Vec3 U, V, D = cylinderDirection;
    D.normalize();
    generateOrthonormalBasis(U, V, D);

    // Compute coordinates of P[i] in system {C;U,V,D}.
    geometry::Vec3 temp[3];
    int i;
    for (i = 0; i < 3; ++i)
    {
        geometry::Vec3 delta = triangle[i] - cylinderCenter;
        temp[i][0] = U * delta;
        temp[i][1] = V * delta;
        temp[i][2] = cylinderDirection * delta;
    }

    // Sort by z-value.
    int j0, j1, j2;
    if (temp[0][2] < temp[1][2])
    {
        if (temp[2][2] < temp[0][2])
        {
            j0 = 2; j1 = 0; j2 = 1;
        }
        else if (temp[2][2] < temp[1][2])
        {
            j0 = 0; j1 = 2; j2 = 1;
        }
        else
        {
            j0 = 0; j1 = 1; j2 = 2;
        }
    }
    else
    {
        if (temp[2][2] < temp[1][2])
        {
            j0 = 2; j1 = 1; j2 = 0;
        }
        else if (temp[2][2] < temp[0][2])
        {
            j0 = 1; j1 = 2; j2 = 0;
        }
        else
        {
            j0 = 1; j1 = 0; j2 = 2;
        }
    }

    // Maintain the xy-components and z-components separately.  The
    // z-components are used for clipping against bottom and top of
    // cylinder.  The xy-components are used for containment tests
    // in disk x*x+y*y <= r*r.
    std::vector<geometry::Vec2> Q(3);
    Q[0] = geometry::Vec2(temp[j0][0], temp[j0][1]);
    Q[1] = geometry::Vec2(temp[j1][0], temp[j1][1]);
    Q[2] = geometry::Vec2(temp[j2][0], temp[j2][1]);

    // From sorting we know that z[0] <= z[1] <= z[2].
    double z[3] =
    {
        temp[j0][2],
        temp[j1][2],
        temp[j2][2]
    };

    // Fast no-intersection.
    const double hhalf = 0.5f*cylinderHeight;
    if (z[2] < -hhalf || z[0] > hhalf)
    {
        // Triangle strictly below or above the cylinder.
        return false;
    }

    // Fast no-clipping.
    if (-hhalf <= z[0] && z[2] <= hhalf)
    {
        return diskOverlapsPolygon(cylinderRadius, Q);
    }

    // Clip against |z| <= h/2.  At this point we know that z2 >= -h/2 and
    // z0 <= h/2 with either z0 < -h/2 or z2 > h/2 (or both).
    std::vector<geometry::Vec2> polygon;
    polygon.reserve(5);
    double t, numer0, numer1, invDenom0, invDenom1;

    if (z[0] < -hhalf)
    {
        if (z[2] > hhalf)
        {
            if (z[1] >= hhalf)
            {
                numer0 = -hhalf - z[0];
                numer1 = +hhalf - z[0];
                invDenom0 = (1.0) / (z[1] - z[0]);
                invDenom1 = (1.0) / (z[2] - z[0]);
                t = numer0 * invDenom1;
                polygon.push_back(Q[0] + ((Q[2] - Q[0]) * vpl::CScalar<double>(t)));
                t = numer0 * invDenom0;
                polygon.push_back(Q[0] + ((Q[1] - Q[0]) * vpl::CScalar<double>(t)));
                t = numer1 * invDenom0;
                polygon.push_back(Q[0] + ((Q[1] - Q[0]) * vpl::CScalar<double>(t)));
                t = numer1 * invDenom1;
                polygon.push_back(Q[0] + ((Q[2] - Q[0]) * vpl::CScalar<double>(t)));
                polygon.shrink_to_fit();
                return diskOverlapsPolygon(cylinderRadius, polygon);
            }
            else if (z[1] <= -hhalf)
            {
                numer0 = -hhalf - z[2];
                numer1 = +hhalf - z[2];
                invDenom0 = (1.0) / (z[1] - z[2]);
                invDenom1 = (1.0) / (z[0] - z[2]);
                t = numer0 * invDenom1;
                polygon.push_back(Q[2] + ((Q[0] - Q[2]) * vpl::CScalar<double>(t)));
                t = numer0 * invDenom0;
                polygon.push_back(Q[2] + ((Q[1] - Q[2]) * vpl::CScalar<double>(t)));
                t = numer1 * invDenom0;
                polygon.push_back(Q[2] + ((Q[1] - Q[2]) * vpl::CScalar<double>(t)));
                t = numer1 * invDenom1;
                polygon.push_back(Q[2] + ((Q[0] - Q[2]) * vpl::CScalar<double>(t)));
                polygon.shrink_to_fit();
                return diskOverlapsPolygon(cylinderRadius, polygon);
            }
            else
            {
                numer0 = -hhalf - z[0];
                numer1 = +hhalf - z[2];
                invDenom0 = (1.0) / (z[1] - z[0]);
                invDenom1 = (1.0) / (z[2] - z[0]);
                t = numer0 * invDenom1;
                polygon.push_back(Q[0] + ((Q[2] - Q[0]) * vpl::CScalar<double>(t)));
                t = numer0 * invDenom0;
                polygon.push_back(Q[0] + ((Q[1] - Q[0]) * vpl::CScalar<double>(t)));
                polygon.push_back(Q[1]);
                invDenom0 = (1.0) / (z[1] - z[2]);
                invDenom1 = (1.0) / (z[0] - z[2]);
                t = numer1 * invDenom0;
                polygon.push_back(Q[2] + ((Q[1] - Q[2]) * vpl::CScalar<double>(t)));
                t = numer1 * invDenom1;
                polygon.push_back(Q[2] + ((Q[0] - Q[2]) * vpl::CScalar<double>(t)));
                polygon.shrink_to_fit();
                return diskOverlapsPolygon(cylinderRadius, polygon);
            }
        }
        else if (z[2] > -hhalf)
        {
            if (z[1] <= -hhalf)
            {
                numer0 = -hhalf - z[2];
                invDenom0 = (1.0) / (z[1] - z[2]);
                invDenom1 = (1.0) / (z[0] - z[2]);
                t = numer0 * invDenom0;
                polygon.push_back(Q[2] + ((Q[1] - Q[2]) * vpl::CScalar<double>(t)));
                t = numer0 * invDenom1;
                polygon.push_back(Q[2] + ((Q[0] - Q[2]) * vpl::CScalar<double>(t)));
                polygon.push_back(Q[2]);
                polygon.shrink_to_fit();
                return diskOverlapsPolygon(cylinderRadius, polygon);
            }
            else
            {
                numer0 = -hhalf - z[0];
                invDenom0 = (1.0) / (z[2] - z[0]);
                invDenom1 = (1.0) / (z[1] - z[0]);
                t = numer0 * invDenom0;
                polygon.push_back(Q[0] + ((Q[2] - Q[0]) * vpl::CScalar<double>(t)));
                t = numer0 * invDenom1;
                polygon.push_back(Q[0] + ((Q[1] - Q[0]) * vpl::CScalar<double>(t)));
                polygon.push_back(Q[1]);
                polygon.push_back(Q[2]);
                polygon.shrink_to_fit();
                return diskOverlapsPolygon(cylinderRadius, polygon);
            }
        }
        else
        {
            if (z[1] < -hhalf)
            {
                return diskOverlapsPoint(cylinderRadius, Q[2]);
            }
            else
            {
                return diskOverlapsSegment(cylinderRadius, Q[2], Q[1]);
            }
        }
    }
    else if (z[0] < hhalf)
    {
        if (z[1] >= hhalf)
        {
            numer0 = hhalf - z[0];
            invDenom0 = (1.0) / (z[2] - z[0]);
            invDenom1 = (1.0) / (z[1] - z[0]);
            t = numer0 * invDenom0;
            polygon.push_back(Q[0] + ((Q[2] - Q[0]) * vpl::CScalar<double>(t)));
            t = numer0 * invDenom1;
            polygon.push_back(Q[0] + ((Q[1] - Q[0]) * vpl::CScalar<double>(t)));
            polygon.push_back(Q[0]);
            polygon.shrink_to_fit();
            return diskOverlapsPolygon(cylinderRadius, polygon);
        }
        else
        {
            numer0 = hhalf - z[2];
            invDenom0 = (1.0) / (z[1] - z[2]);
            invDenom1 = (1.0) / (z[0] - z[2]);
            t = numer0 * invDenom0;
            polygon.push_back(Q[2] + ((Q[1] - Q[2]) * vpl::CScalar<double>(t)));
            t = numer0 * invDenom1;
            polygon.push_back(Q[2] + ((Q[0] - Q[2]) * vpl::CScalar<double>(t)));
            polygon.push_back(Q[0]);
            polygon.push_back(Q[1]);
            polygon.shrink_to_fit();
            return diskOverlapsPolygon(cylinderRadius, polygon);
        }
    }
    else
    {
        if (z[1] > hhalf)
        {
            return diskOverlapsPoint(cylinderRadius, Q[0]);
        }
        else
        {
            return diskOverlapsSegment(cylinderRadius, Q[0], Q[1]);
        }
    }
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Checks if disk overlaps point
//!
//!\param   diskRadius  Radius of a disk centered at 0,0
//!\param   point       Point
//!
//!\return  True if there is any overlaping parts between disc and point
////////////////////////////////////////////////////////////////////////////////////////////////////
bool diskOverlapsPoint(const double diskRadius, const geometry::Vec2 &point)
{
    return point[0] * point[0] + point[1] * point[1] <= diskRadius * diskRadius;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Computes dot product with perpendicular vector: v0 dot perp(v1)
//!
//!\param   v0  Vector #0
//!\param   v1  Vector #1
//!
//!\return  Result
////////////////////////////////////////////////////////////////////////////////////////////////////
double dotPerp(const geometry::Vec2 &v0, const geometry::Vec2 &v1)
{
    return v0[0] * v1[1] - v0[1] * v1[0];
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Checks if disk overlaps line segment
//!
//!\param   diskRadius  Radius of a disk centered at 0,0
//!\param   point0      First endpoint of line segment
//!\param   point1      Second endpoint of line segment
//!
//!\return  True if there is any overlaping parts between disc and line segment
////////////////////////////////////////////////////////////////////////////////////////////////////
bool diskOverlapsSegment(const double diskRadius, const geometry::Vec2 &point0, const geometry::Vec2 &point1)
{
    double rSqr = diskRadius * diskRadius;
    geometry::Vec2 D = point0 - point1;
    double dot = point0 * D;
    if (dot <= 0.0)
    {
        return point0 * point0 <= rSqr;
    }

    double lenSqr = D * D;
    if (dot >= lenSqr)
    {
        return point1 * point1 <= rSqr;
    }

    dot = dotPerp(D, point0);
    return dot * dot <= lenSqr * rSqr;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//!\brief   Checks if disk overlaps polygon
//!
//!\param   diskRadius  radius of a disk centered at 0,0
//!\param   polygon     vertices of a polygon
//!
//!\return  True if there is any overlaping parts between disc and polygon
////////////////////////////////////////////////////////////////////////////////////////////////////
bool diskOverlapsPolygon(const double diskRadius, const std::vector<geometry::Vec2> &polygon)
{
    // Test whether the polygon contains (0,0).
    int positive = 0, negative = 0;
    int i0, i1;
    int numVertices = polygon.size();
    for (i0 = numVertices - 1, i1 = 0; i1 < numVertices; i0 = i1++)
    {
        double dot = dotPerp(polygon[i0], (polygon[i0] - polygon[i1]));
        if (dot > 0.0)
        {
            ++positive;
        }
        else if (dot < 0.0)
        {
            ++negative;
        }
    }
    if (positive == 0 || negative == 0)
    {
        // The polygon contains (0,0), so the disk and polygon overlap.
        return true;
    }

    // Test whether any edge is overlapped by the polygon.
    for (i0 = numVertices - 1, i1 = 0; i1 < numVertices; i0 = i1++)
    {
        if (diskOverlapsSegment(diskRadius, polygon[i0], polygon[i1]))
        {
            return true;
        }
    }

    return false;
}

} // namespace geometry


/*
#include <algorithm>
#include <boost/numeric/interval.hpp>
#include <cassert>
#include <cmath>
#include <functional>
#include <iostream>
#include <list>
#include <map>
#include <memory>
#include <sstream>
#include <string>
#include <tuple>
#include <utility>
#include <variant>
#include <vector>
*/
#include <sstream>

#include "analyze.h"
//#include "draw.h"
#include "functions.h"

//#include "pbPlots.h"
//#include "supportLib.h"

#include "print.h"

int main(int argc, char *argv[])
{
    if (argc == 1) {
        listFunctions();
    } else if (argc != 5) {
        std::cout << argv[0] << " function lsb lo hi" << std::endl;
    } else {
        // analyze(argv[1], name2function(argv[1]), I(atof(argv[3]), atof(argv[4])), atoi(argv[2]), false);
        // compute histogram
        auto        fun = name2function(argv[1]);
        std::string msg = argv[1];
        int         lsb = atoi(argv[2]);
        I           i(atof(argv[3]), atof(argv[4]));

        auto H = histogram(fun, i, lsb);
        gnuplotfile(msg, lsb, i.lower(), i.upper(), H);
    }

    return 0;
}

/*
acos Primitive
asin Primitive
atan Primitive
atan2 Primitive
cos Primitive
sin Primitive
tan Primitive
exp Primitive
log Primitive
log10 Primitive
pow Primitive
sqrt Primitive
abs Primitive
min Primitive
max Primitive
fmod Primitive
remainder Primitive
floor Primitive
ceil Primitive
rint Primitive
*/


#include <iostream>
#include <algorithm>

#include <QtGlobal>
#if QT_VERSION >= 0x050000
#include <QtWidgets>
#else
#include <QtGui>
#endif

#ifndef Q_MOC_RUN
#include <boost/compute/system.hpp>
#include <boost/compute/image/image2d.hpp>
#include <boost/compute/interop/qt.hpp>
#include <boost/compute/utility/dim.hpp>
#include <boost/compute/utility/source.hpp>
#endif // Q_MOC_RUN

namespace compute = boost::compute;

inline void box_filter_image(const compute::image2d &input,
                             compute::image2d &output,
                             compute::uint_ box_height,
                             compute::uint_ box_width,
                             compute::command_queue &queue)
{
    using compute::dim;

    const compute::context &context = queue.get_context();

    // simple box filter kernel source
    const char source[] = BOOST_COMPUTE_STRINGIZE_SOURCE(
        __kernel void box_filter(__read_only image2d_t input,
                                 __write_only image2d_t output,
                                 uint box_height,
                                 uint box_width)
        {
            int x = get_global_id(0);
            int y = get_global_id(1);
            int h = get_image_height(input);
            int w = get_image_width(input);
            int k = box_width;
            int l = box_height;

            if(x < k/2 || y < l/2 || x >= w-(k/2) || y >= h-(l/2)){
                write_imagef(output, (int2)(x, y), (float4)(0, 0, 0, 1));
            }
            else {
                const sampler_t sampler = CLK_ADDRESS_NONE | CLK_FILTER_NEAREST;

                float4 sum = { 0, 0, 0, 0 };
                for(int i = 0; i < k; i++){
                    for(int j = 0; j < l; j++){
                        sum += read_imagef(input, sampler, (int2)(x+i-k, y+j-l));
                    }
                }
                sum /= (float) k * l;
                float4 value = (float4)( sum.x, sum.y, sum.z, 1.f );
                write_imagef(output, (int2)(x, y), value);
            }
        }
    );

    // build box filter program
    compute::program program =
        compute::program::create_with_source(source, context);
    program.build();

    // setup box filter kernel
    compute::kernel kernel(program, "box_filter");
    kernel.set_arg(0, input);
    kernel.set_arg(1, output);
    kernel.set_arg(2, box_height);
    kernel.set_arg(3, box_width);

    // execute the box filter kernel
    queue.enqueue_nd_range_kernel(kernel, dim(0, 0), input.size(), dim(1, 1));
}

// this example shows how to load an image using Qt, apply a simple
// box blur filter, and then display it in a Qt window.
int main(int argc, char *argv[])
{
    QApplication app(argc, argv);

    // check command line
    if(argc < 2){
        std::cout << "usage: qimage_blur [FILENAME]" << std::endl;
        return -1;
    }

    // load image using Qt
    QString fileName = argv[1];
    QImage qimage(fileName);

    size_t height = qimage.height();
    size_t width = qimage.width();
    size_t bytes_per_line = qimage.bytesPerLine();

    qDebug() << "height:" << height
             << "width:" << width
             << "bytes per line:" << bytes_per_line
             << "depth:" << qimage.depth()
             << "format:" << qimage.format();

    // create compute context
    compute::device gpu = compute::system::default_device();
    compute::context context(gpu);
    compute::command_queue queue(context, gpu);
    std::cout << "device: " << gpu.name() << std::endl;

    // get the opencl image format for the qimage
    compute::image_format format =
        compute::qt_qimage_format_to_image_format(qimage.format());

    // create input and output images on the gpu
    compute::image2d input_image(context, width, height, format);
    compute::image2d output_image(context, width, height, format);

    // copy host qimage to gpu image
    compute::qt_copy_qimage_to_image2d(qimage, input_image, queue);

    // apply box filter
    box_filter_image(input_image, output_image, 7, 7, queue);

    // copy gpu blurred image from to host qimage
    compute::qt_copy_image2d_to_qimage(output_image, qimage, queue);

    // show image as a pixmap
    QLabel label;
    label.setPixmap(QPixmap::fromImage(qimage));
    label.show();

    return app.exec();
}


// Copyright 2019-2022 Cambridge Quantum Computing
//
// 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
//
// 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.

#pragma once

#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <memory>

#include "Ops/Op.hpp"
#include "Utils/BiMapHeaders.hpp"
#include "Utils/EigenConfig.hpp"
#include "Utils/Json.hpp"
#include "Utils/MatrixAnalysis.hpp"
#include "Utils/UnitID.hpp"

namespace tket {

class Circuit;

/**
 * Abstract class for an operation from which a circuit can be extracted
 */
class Box : public Op {
 public:
  explicit Box(const OpType &type, const op_signature_t &signature = {})
      : Op(type), signature_(signature), circ_(), id_(idgen()) {
    if (!is_box_type(type)) throw NotValid();
  }

  Box(const Box &other)
      : Op(other.get_type()),
        signature_(other.signature_),
        circ_(other.circ_),
        id_(other.id_) {}

  /** Number of Quantum inputs */
  unsigned n_qubits() const override;

  /** Number of Boolean inputs */
  unsigned n_boolean() const;

  /** Number of Classical inputs */
  unsigned n_classical() const;

  op_signature_t get_signature() const override;

  nlohmann::json serialize() const override;

  static Op_ptr deserialize(const nlohmann::json &j);

  /** Circuit represented by box */
  std::shared_ptr<Circuit> to_circuit() const {
    if (circ_ == nullptr) generate_circuit();
    return circ_;
  };

  /** Unique identifier (preserved on copy) */
  boost::uuids::uuid get_id() const { return id_; }

  template <typename BoxT>
  friend Op_ptr set_box_id(BoxT &b, boost::uuids::uuid newid);

 protected:
  static boost::uuids::uuid idgen() {
    static boost::uuids::random_generator gen = {};

    return gen();
  }
  op_signature_t signature_;
  mutable std::shared_ptr<Circuit> circ_;
  boost::uuids::uuid id_;

  virtual void generate_circuit() const = 0;
};

// json for base Box attributes
nlohmann::json core_box_json(const Box &box);

/**
 * @brief Set explicit ID on a box
 *
 * This is used for deserialization.
 *
 * @param b box
 * @param[in] newid new ID
 *
 * @tparam BoxT concrete box type
 *
 * @return box with desired ID
 */
template <typename BoxT>
Op_ptr set_box_id(BoxT &b, boost::uuids::uuid newid) {
  b.id_ = newid;
  return std::make_shared<BoxT>(b);
}

/**
 * Operation defined as a circuit
 */
class CircBox : public Box {
 public:
  /**
   * Construct from a given circuit
   */
  explicit CircBox(const Circuit &circ);

  /**
   * Construct from the empty circuit
   */
  CircBox();

  /**
   * Copy constructor
   */
  CircBox(const CircBox &other);

  ~CircBox() override {}

  Op_ptr symbol_substitution(
      const SymEngine::map_basic_basic &sub_map) const override;

  SymSet free_symbols() const override;

  /**
   * Equality check between two CircBox instances
   */
  bool is_equal(const Op &op_other) const override {
    const CircBox &other = dynamic_cast<const CircBox &>(op_other);
    return id_ == other.get_id();
  }

  Op_ptr dagger() const override;

  Op_ptr transpose() const override;

  static Op_ptr from_json(const nlohmann::json &j);

  static nlohmann::json to_json(const Op_ptr &op);

 protected:
  void generate_circuit() const override {}  // Already set by constructor
};

/**
 * One-qubit operation defined as a unitary matrix
 */
class Unitary1qBox : public Box {
 public:
  /**
   * Construct from a given 2x2 unitary matrix
   *
   * @param m unitary matrix
   */
  explicit Unitary1qBox(const Eigen::Matrix2cd &m);

  /**
   * Construct from the identity matrix
   */
  Unitary1qBox();

  /**
   * Copy constructor
   */
  Unitary1qBox(const Unitary1qBox &other);

  ~Unitary1qBox() override {}

  Op_ptr symbol_substitution(
      const SymEngine::map_basic_basic &) const override {
    return Op_ptr();
  }

  SymSet free_symbols() const override { return {}; }

  /**
   * Equality check between two Unitary1qBox instances
   */
  bool is_equal(const Op &op_other) const override {
    const Unitary1qBox &other = dynamic_cast<const Unitary1qBox &>(op_other);
    return id_ == other.get_id();
  }

  /** Get the unitary matrix correspnding to this operation */
  Eigen::Matrix2cd get_matrix() const { return m_; }

  Eigen::MatrixXcd get_unitary() const override { return m_; }

  Op_ptr dagger() const override;

  Op_ptr transpose() const override;

  static Op_ptr from_json(const nlohmann::json &j);

  static nlohmann::json to_json(const Op_ptr &op);

 protected:
  void generate_circuit() const override;

 private:
  const Eigen::Matrix2cd m_;
};

/**
 * Two-qubit operation defined as a unitary matrix (ILO-BE)
 */
class Unitary2qBox : public Box {
 public:
  /**
   * Construct from a given 4x4 unitary matrix
   *
   * @param m unitary matrix
   * @param basis basis order convention for matrix
   */
  explicit Unitary2qBox(
      const Eigen::Matrix4cd &m, BasisOrder basis = BasisOrder::ilo);

  /**
   * Construct from the identity matrix
   */
  Unitary2qBox();

  /**
   * Copy constructor
   */
  Unitary2qBox(const Unitary2qBox &other);

  ~Unitary2qBox() override {}

  Op_ptr symbol_substitution(
      const SymEngine::map_basic_basic &) const override {
    return Op_ptr();
  }

  SymSet free_symbols() const override { return {}; }

  /**
   * Equality check between two Unitary2qBox instances
   */
  bool is_equal(const Op &op_other) const override {
    const Unitary2qBox &other = dynamic_cast<const Unitary2qBox &>(op_other);
    return id_ == other.get_id();
  }

  /** Get the unitary matrix correspnding to this operation */
  Eigen::Matrix4cd get_matrix() const { return m_; }

  Eigen::MatrixXcd get_unitary() const override { return m_; }

  Op_ptr dagger() const override;

  Op_ptr transpose() const override;

  static Op_ptr from_json(const nlohmann::json &j);

  static nlohmann::json to_json(const Op_ptr &op);

 protected:
  void generate_circuit() const override;

 private:
  const Eigen::Matrix4cd m_;
};

/**
 * Three-qubit operation defined as a unitary matrix (ILO-BE)
 */
class Unitary3qBox : public Box {
 public:
  /**
   * Construct from a given 8x8 unitary matrix
   *
   * @param m unitary matrix
   * @param basis basis order convention for matrix
   */
  explicit Unitary3qBox(const Matrix8cd &m, BasisOrder basis = BasisOrder::ilo);

  /**
   * Construct from the identity matrix
   */
  Unitary3qBox();

  /**
   * Copy constructor
   */
  Unitary3qBox(const Unitary3qBox &other);

  ~Unitary3qBox() override {}

  Op_ptr symbol_substitution(
      const SymEngine::map_basic_basic &) const override {
    return Op_ptr();
  }

  SymSet free_symbols() const override { return {}; }

  /**
   * Equality check between two Unitary3qBox instances
   */
  bool is_equal(const Op &op_other) const override {
    const Unitary3qBox &other = dynamic_cast<const Unitary3qBox &>(op_other);
    return id_ == other.get_id();
  }

  /** Get the unitary matrix correspnding to this operation */
  Matrix8cd get_matrix() const { return m_; }

  Eigen::MatrixXcd get_unitary() const override { return m_; }

  Op_ptr dagger() const override;

  Op_ptr transpose() const override;

  static Op_ptr from_json(const nlohmann::json &j);

  static nlohmann::json to_json(const Op_ptr &op);

 protected:
  void generate_circuit() const override;

 private:
  const Matrix8cd m_;
};

/**
 * Two-qubit operation defined in terms of a hermitian matrix and a phase.
 *
 * The unitary corresponding to the matrix A and phase t is exp(i*t*A).
 * Matrix A is stored in ILO-BE form.
 */
class ExpBox : public Box {
 public:
  /**
   * Construct from a given 4x4 hermitian matrix and optional phase.
   *
   * @param A hermitian matrix
   * @param t phase to apply
   * @param basis basis order convention for matrix
   */
  ExpBox(
      const Eigen::Matrix4cd &A, double t = 1.,
      BasisOrder basis = BasisOrder::ilo);

  /**
   * Construct from the zero matrix (resulting in the identity)
   */
  ExpBox();

  /**
   * Copy constructor
   */
  ExpBox(const ExpBox &other);

  ~ExpBox() override {}

  Op_ptr symbol_substitution(
      const SymEngine::map_basic_basic &) const override {
    return Op_ptr();
  }

  SymSet free_symbols() const override { return {}; }

  /**
   * Equality check between two ExpBox instances
   */
  bool is_equal(const Op &op_other) const override {
    const ExpBox &other = dynamic_cast<const ExpBox &>(op_other);
    return id_ == other.get_id();
  }

  /** Get the hermitian matrix and phase parameter */
  std::pair<Eigen::Matrix4cd, double> get_matrix_and_phase() const {
    return std::make_pair(A_, t_);
  }

  Op_ptr dagger() const override;

  Op_ptr transpose() const override;

  static Op_ptr from_json(const nlohmann::json &j);

  static nlohmann::json to_json(const Op_ptr &op);

 protected:
  void generate_circuit() const override;

 private:
  const Eigen::Matrix4cd A_;
  double t_;
};

/**
 * Operation defined as the exponential of a tensor of Pauli operators
 */
class PauliExpBox : public Box {
 public:
  /**
   * The operation implements the unitary operator
   * \f$ e^{-\frac12 i \pi t \sigma_0 \otimes \sigma_1 \otimes \cdots} \f$
   * where \f$ \sigma_i \in \{I,X,Y,Z\} \f$ are the Pauli operators.
   */
  PauliExpBox(const std::vector<Pauli> &paulis, const Expr &t);

  /**
   * Construct from the empty vector
   */
  PauliExpBox();

  /**
   * Copy constructor
   */
  PauliExpBox(const PauliExpBox &other);

  ~PauliExpBox() override {}

  SymSet free_symbols() const override;

  /**
   * Equality check between two PauliExpBox instances
   */
  bool is_equal(const Op &op_other) const override {
    const PauliExpBox &other = dynamic_cast<const PauliExpBox &>(op_other);
    return id_ == other.get_id();
  }

  /** Get the Pauli string */
  std::vector<Pauli> get_paulis() const { return paulis_; }

  /** Get the phase parameter */
  Expr get_phase() const { return t_; }

  Op_ptr dagger() const override;

  Op_ptr transpose() const override;

  Op_ptr symbol_substitution(
      const SymEngine::map_basic_basic &sub_map) const override;

  static Op_ptr from_json(const nlohmann::json &j);

  static nlohmann::json to_json(const Op_ptr &op);

 protected:
  void generate_circuit() const override;

 private:
  std::vector<Pauli> paulis_;
  Expr t_;
};

class CompositeGateDef;
typedef std::shared_ptr<CompositeGateDef> composite_def_ptr_t;

// CompositeGateDef
JSON_DECL(composite_def_ptr_t)

class CompositeGateDef : public std::enable_shared_from_this<CompositeGateDef> {
 public:
  CompositeGateDef(
      const std::string &name, const Circuit &def,
      const std::vector<Sym> &args);

  static composite_def_ptr_t define_gate(
      const std::string &name, const Circuit &def,
      const std::vector<Sym> &args);

  Circuit instance(const std::vector<Expr> &params) const;

  std::string get_name() const { return name_; }
  std::vector<Sym> get_args() const { return args_; }
  std::shared_ptr<Circuit> get_def() const { return def_; }
  unsigned n_args() const { return args_.size(); }
  op_signature_t signature() const;

  bool operator==(const CompositeGateDef &other) const;

 private:
  std::string name_;
  std::shared_ptr<Circuit> def_;
  std::vector<Sym> args_;

  CompositeGateDef() {}
};

class CustomGate : public Box {
 public:
  CustomGate(const composite_def_ptr_t &gate, const std::vector<Expr> &params);
  CustomGate(const CustomGate &other);

  SymSet free_symbols() const override;

  composite_def_ptr_t get_gate() const { return gate_; }
  std::vector<Expr> get_params() const override { return params_; }
  std::string get_name(bool latex = false) const override;

  Op_ptr symbol_substitution(
      const SymEngine::map_basic_basic &sub_map) const override;

  /**
   * Equality check between two CustomGate instances
   */
  bool is_equal(const Op &op_other) const override {
    const CustomGate &other = dynamic_cast<const CustomGate &>(op_other);
    return this->id_ == other.id_;
  }

  static Op_ptr from_json(const nlohmann::json &j);

  static nlohmann::json to_json(const Op_ptr &op);

 protected:
  void generate_circuit() const override;
  CustomGate() : Box(OpType::CustomGate), gate_(), params_() {}

 private:
  composite_def_ptr_t gate_;
  const std::vector<Expr> params_;
};

/**
 * Wraps another quantum op, adding control qubits
 */
class QControlBox : public Box {
 public:
  /**
   * Construct from a given op and number of controls
   *
   * @param op op to control
   * @param n_controls number of qubit controls to add
   */
  explicit QControlBox(const Op_ptr &op, unsigned n_controls = 1);

  /**
   * Copy constructor
   */
  QControlBox(const QControlBox &other);

  ~QControlBox() override {}

  Op_ptr symbol_substitution(
      const SymEngine::map_basic_basic &sub_map) const override;

  SymSet free_symbols() const override;

  /**
   * Equality check between two QControlBox instances
   */
  bool is_equal(const Op &op_other) const override {
    const QControlBox &other = dynamic_cast<const QControlBox &>(op_other);
    return id_ == other.get_id();
  }

  std::string get_command_str(const unit_vector_t &args) const override;

  Op_ptr dagger() const override;

  Op_ptr transpose() const override;

  Op_ptr get_op() const { return op_; }
  unsigned get_n_controls() const { return n_controls_; }

  static Op_ptr from_json(const nlohmann::json &j);

  static nlohmann::json to_json(const Op_ptr &op);

 protected:
  void generate_circuit() const override;
  QControlBox()
      : Box(OpType::QControlBox), op_(), n_controls_(0), n_inner_qubits_(0) {}

 private:
  const Op_ptr op_;
  const unsigned n_controls_;
  unsigned n_inner_qubits_;
};

class ProjectorAssertionBox : public Box {
 public:
  /**
   * Construct from a given 2x2, 4x4 or 8x8 projector matrix
   *
   * @param m projector matrix
   * @param basis basis order convention for matrix
   */
  explicit ProjectorAssertionBox(
      const Eigen::MatrixXcd &m, BasisOrder basis = BasisOrder::ilo);

  /**
   * Copy constructor
   */
  ProjectorAssertionBox(const ProjectorAssertionBox &other);

  ~ProjectorAssertionBox() override {}

  Op_ptr symbol_substitution(
      const SymEngine::map_basic_basic &) const override {
    return Op_ptr();
  }

  SymSet free_symbols() const override { return {}; }

  /**
   * Equality check between two ProjectorAssertionBox instances
   */
  bool is_equal(const Op &op_other) const override {
    const ProjectorAssertionBox &other =
        dynamic_cast<const ProjectorAssertionBox &>(op_other);
    return id_ == other.get_id();
  }

  /** Get the unitary matrix correspnding to this operation */
  Eigen::MatrixXcd get_matrix() const { return m_; }
  std::vector<bool> get_expected_readouts() const { return expected_readouts_; }

  Op_ptr dagger() const override;

  Op_ptr transpose() const override;

  op_signature_t get_signature() const override;

  static Op_ptr from_json(const nlohmann::json &j);

  static nlohmann::json to_json(const Op_ptr &op);

 protected:
  void generate_circuit() const override;

 private:
  const Eigen::MatrixXcd m_;
  // expected readouts the debug bits
  // false -> 0
  // true -> 1
  mutable std::vector<bool> expected_readouts_;
};

class StabiliserAssertionBox : public Box {
 public:
  /**
   * Construct from a set of stabiliser Pauli strings
   *
   * @param paulis a set of stabiliser Pauli strings
   */
  explicit StabiliserAssertionBox(const PauliStabiliserList &paulis);

  /**
   * Copy constructor
   */
  StabiliserAssertionBox(const StabiliserAssertionBox &other);

  ~StabiliserAssertionBox() override {}
  Op_ptr symbol_substitution(
      const SymEngine::map_basic_basic &) const override {
    return Op_ptr();
  }

  SymSet free_symbols() const override { return {}; }

  /**
   * Equality check between two StabiliserAssertionBox instances
   */
  bool is_equal(const Op &op_other) const override {
    const StabiliserAssertionBox &other =
        dynamic_cast<const StabiliserAssertionBox &>(op_other);
    return id_ == other.get_id();
  }

  /** Get the pauli stabilisers */
  PauliStabiliserList get_stabilisers() const { return paulis_; }
  std::vector<bool> get_expected_readouts() const { return expected_readouts_; }

  Op_ptr dagger() const override;

  Op_ptr transpose() const override;

  static Op_ptr from_json(const nlohmann::json &j);

  static nlohmann::json to_json(const Op_ptr &op);

  op_signature_t get_signature() const override;

 protected:
  void generate_circuit() const override;

 private:
  const PauliStabiliserList paulis_;
  // expected readouts the debug bits
  // false -> 0
  // true -> 1
  mutable std::vector<bool> expected_readouts_;
};

}  // namespace tket