#pragma once

#include <cassert>
#include <iostream>
#include <algorithm>
#include <numeric>

#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/serialization/vector.hpp>

#include <common/error.hpp>

#include <tensor/index.hpp>
#include <tensor/tensor.hpp>

namespace Construction {
	namespace Tensor {

		class IsNoPermutationException : public Exception {
		public:
			IsNoPermutationException(const std::string& error) : Exception(error) { }
			IsNoPermutationException() : Exception("The given combination is no permutation") {}
		};

		class BinaryPermutation {
		public:
			BinaryPermutation() = default;

			BinaryPermutation(unsigned a, unsigned b) : a(a), b(b) { }
			BinaryPermutation(const BinaryPermutation& other)
				: a(other.a), b(other.b) { }
		public:
			bool operator==(const BinaryPermutation& other) const {
				return a == other.a && b == other.b;
			}

			bool operator!=(const BinaryPermutation& other) const {
				return a != other.a || b != other.b;
			}
		public:
			Indices operator()(const Indices& indices) const {
				assert(a > 0 && b > 0 && indices.Size() >= a && indices.Size() >= b);

				Indices result;

				for (unsigned i=0; i < indices.Size(); ++i) {
					if (i == a-1) result.Insert(indices[b-1]);
					else if (i == b-1) result.Insert(indices[a-1]);
					else result.Insert(indices[i]);
				}

				return result;
			}

			std::vector<int> operator()(std::vector<int> list) const {
				assert(a > 0 && b > 0 && list.size() >= a && list.size() >= b);

				std::vector<int> result;

				for (unsigned i=0; i < list.size(); ++i) {
					if (i == a-1) result.push_back(list[b-1]);
					else if (i == b-1) result.push_back(list[a-1]);
					else result.push_back(list[i]);
				}

				return result;
			}

			template<typename... Args>
			std::vector<int> operator()(Args... args) const {
				return (*this)({ args... });
			}

			/*Tensor operator()(const Tensor& tensor) const {
				return Tensor
			}*/
		public:
			friend std::ostream& operator<<(std::ostream& os, const BinaryPermutation& permutation) {
				os << "(" << permutation.a << " <-> " << permutation.b << ")";
				return os;
			}
		private:
			friend class boost::serialization::access;

			template<class Archive>
			void serialize(Archive& ar, const unsigned int version) {
				ar & a;
				ar & b;
			}
		private:
			unsigned a;
			unsigned b;
		};

		/**
			\class Permutation

			Allows arbitrary permutation of an index combination
		 */
		class Permutation {
		public:
			Permutation() = default;

			Permutation(unsigned a, unsigned b) {
				this->permute.emplace_back(BinaryPermutation(a,b));
			}

			Permutation(const BinaryPermutation& permute) {
				this->permute.push_back(permute);
			}

			Permutation(std::vector<BinaryPermutation> permute)
				: permute(permute) { }

			// Copy constructor
			Permutation(const Permutation& other)
				: permute(other.permute) { }
		public:
			/*Permutation& operator=(std::vector<BinaryPermutation> permute) {
				this->permute = permute;
				return *this;
			}*/

			Permutation& operator=(const Permutation& other) {
				permute = other.permute;
				return *this;
			}

			Permutation& operator=(Permutation&& other) {
				permute = std::move(other.permute);
				return *this;
			}
		public:
			void Insert(const BinaryPermutation& p) {
				permute.push_back(p);
			}

			void Insert(unsigned a, unsigned b) {
				permute.emplace_back(BinaryPermutation(a,b));
			}
 		public:
			Indices operator()(const Indices& indices) const {
				Indices result = indices;
				for (auto& p : permute) {
					result = p(result);
				}
				return result;
			}

			std::vector<int> operator()(const std::vector<int>& indices) const {
				std::vector<int> result = indices;
				for (auto& p : permute) {
					result = p(result);
				}
				return result;
			}

			template<typename... Args>
			std::vector<unsigned> operator()(Args... args) const {
				return (*this)({args...});
			}
		public:
			bool IsEven() const {
				return permute.size() % 2 == 0;
			}

			bool IsOdd() const {
				return permute.size() % 2 != 0;
			}

			int Sign() const {
				return IsOdd() ? -1 : 1;
			}
		public:
			/**
				\brief A cyclic permutation of the indices

				{abcd} => {bcda}
			 */
			static Indices Cyclic(const Indices& indices) {
				Permutation p;
				for (int i=1; i<indices.Size(); i++) {
					p.Insert(i, i+1);
				}
				return p(indices);
			}

			static std::vector<int> Cyclic(const std::vector<int>& indices) {
				Permutation p;
				for (int i=1; i<indices.size(); i++) {
					p.Insert(i, i+1);
				}
				return p(indices);
			}

			template<typename... Args>
			static std::vector<int> Cyclic(Args... args) {
				return Cyclic({ args... });
			}

			/**
				Returns a permutation to get from an index combination to another

			 */
			static Permutation From(const Indices& indices, const Indices& to) {
				if (!indices.IsPermutationOf(to)) {
					throw IsNoPermutationException();
				}

				// Clone the vector
				auto vec = indices;

				int pos = 0;
				Permutation result;

				for (int i=0; i<indices.Size(); ++i) {
					auto current = vec[pos];
					auto id = std::find(to.begin(), to.end(), current) - to.begin();

					if (id == pos) {
						pos++;
						continue;
					}

					result.Insert(pos + 1, id + 1);

					// Do the switch
					std::iter_swap(vec.begin() + pos, vec.begin() + id);
				}

				return result;
			}

			static Permutation From(const std::vector<int>& indices, const std::vector<int>& to) {
				if (!Indices::IsPermutationOf(indices, to)) {
					throw IsNoPermutationException();
				}

				// Clone the vector
				auto vec = indices;

				int pos = 0;
				Permutation result;

				for (int i=0; i<indices.size(); ++i) {
					auto current = vec[pos];
					auto id = std::find(to.begin(), to.end(), current) - to.begin();

					if (id == pos) {
						pos++;
						continue;
					}

					result.Insert(pos + 1, id + 1);

					// Do the switch
					std::iter_swap(vec.begin() + pos, vec.begin() + id);
				}

				return result;
			}

			static Permutation From(const std::vector<unsigned>& indices, const std::vector<unsigned>& to) {
				if (!Indices::IsPermutationOf(indices, to)) {
					throw IsNoPermutationException();
				}

				// Clone the vector
				auto vec = indices;

				int pos = 0;
				Permutation result;

				for (int i=0; i<indices.size(); ++i) {
					auto current = vec[pos];
					auto id = std::find(to.begin(), to.end(), current) - to.begin();

					if (id == pos) {
						pos++;
						continue;
					}

					result.Insert(pos + 1, id + 1);

					// Do the switch
					std::iter_swap(vec.begin() + pos, vec.begin() + id);
				}

				return result;
			}

            static Permutation From(Indices indices, std::map<Index, Index> transformation) {
                Permutation result;

                for (unsigned i=0; i<indices.Size(); ++i) {
                    auto it = transformation.find(indices[i]);
                    if (it == transformation.end()) throw IsNoPermutationException();

                    unsigned pos=0;
                    bool found=false;
                    for (unsigned j=0; j<indices.Size(); ++j) {
                        if (it->second == indices[j]) { pos = j; found=true; break; }
                    }

                    if (!found) throw IsNoPermutationException();

                    result.Insert({ i+1, pos+1 });
                }

                return result;
            }

            inline static Indices Shuffle(Indices indices, std::map<Index, Index> transformation) {
                return From(indices, transformation)(indices);
            }
		public:
			friend std::ostream& operator<<(std::ostream& os, const Permutation& permutation) {
				os << "[";
				for (int i=0; i<permutation.permute.size(); i++) {
					os << permutation.permute[i];
					if (i !=permutation.permute.size()-1) os << ", ";
				}
				os << "]";
				return os;
			}
		private:
			friend class boost::serialization::access;

			template<class Archive>
			void serialize(Archive& ar, const unsigned int version) {
				ar & permute;
			}
		private:
			std::vector<BinaryPermutation> permute;
		};

	}
}


//============================================================================
// Name         : dnatemplatecalcfuncs.hpp
// Author       : Roger Fraser
// Contributors :
// Version      : 1.00
// Copyright    : Copyright 2017 Geoscience Australia
//
//                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.
//
// Description  : Advanced and common calculation functions using standard
//				  data types
//============================================================================

#ifndef DNATEMPLATECALCFUNCS_H_
#define DNATEMPLATECALCFUNCS_H_

#if defined(_MSC_VER)
	#if defined(LIST_INCLUDES_ON_BUILD)
		#pragma message("  " __FILE__)
	#endif
#endif

#include <algorithm>
#include <numeric>
#include <functional>
#include <sstream>
#include <string>
#include <vector>
#include <stdlib.h>
#include <math.h>
#include <iostream>

#include <boost/shared_ptr.hpp>
#include <boost/algorithm/string.hpp>

#include <include/config/dnatypes.hpp>
#include <include/config/dnaconsts.hpp>

using namespace std;
using namespace boost;

template <class U>
U sumOfConsecutiveIntegers(const U& max, const U& min = 1)
{
	//U q(static_cast<U>(average<double, U>(max, min) * max));
	U sum(0);
	for (U u=min; u<=max; u++)
		sum += u;
	return sum;
}

template <typename T, typename U>
T removeNegativeZero(const T& t, const U& precision)
{
	if (t < 0.0 || t == T(-0.0))
	{
		T v(floor((t * pow(10.0, precision)) + 0.5));
		if (fabs(v) > 0.)
			// t is non-zero at precision
			return t;
		// t is a negative zero
		return 0.;
	}
	return t;
}

template <typename T>
bool are_floats_equal(const T& lhs, const T& rhs, const T epsilon=PRECISION_1E6)
{
	return (fabs(lhs - rhs) < epsilon);
}

template <typename T, typename U, typename iterator>
T average(const iterator begin, const iterator end, U& sum)
{
	U n(static_cast<UINT32>(std::distance(begin, end)));
	sum = accumulate(begin, end, 0);
	return static_cast<T>(sum) / n;
}

template <class T, class U>
T average(const U& a, const U& b)
{
	T t(a + b);
	return t / 2.;
}

template <class T>
T average(const T& a, const T& b)
{
	T t(a + b);
	return (t / (T)(2.));
}

template <class T, typename U>
T minVal(const T& lhs, const U& rhs)
{
	if (lhs < rhs)
		return lhs;
	else
		return rhs;
}

template <class T, typename U>
T maxVal(const T& lhs, const U& rhs)
{
	if (lhs > rhs)
		return lhs;
	else
		return rhs;
}

// use this for longitude values only, where 180 degrees
// marks the boundary between east (positive) and west (negative)
template <class T>
T Rad180Mod(const T &dValue)
{
	if (dValue < -PI)
		 return dValue + (PI + PI);
	else if (dValue > PI)
		 return dValue - (PI + PI);
	return dValue;
}

template <class T>
T degrees_to_radians_(T& degrees)
{
	return (degrees * DEG_TO_RAD);
}

template <class T>
T Radians(T& degrees)
{
	return degrees_to_radians_(degrees);
}

template <class T>
T Radians(const T& degrees)
{
	return degrees_to_radians_(degrees);
}

template <class T>
void Radians(T* degrees)
{
	*degrees = degrees_to_radians_(*degrees);
}

template <class T>
T radians_to_degrees_(T& radians)
{
	return (radians * RAD_TO_DEG);
}

template <class T>
T Degrees(T& radians)
{
	return radians_to_degrees_(radians);
}

template <class T>
T Degrees(const T& radians)
{
	return radians_to_degrees_(radians);
}

template <class T>
void Degrees(T* radians)
{
	*radians = radians_to_degrees_(*radians);
}

// use DegreesL(const T& radians) for longitude values only, where 180 degrees
// marks the boundary between east (positive) and west (negative)
template <class T>
T DegreesL(const T& radians)
{
	return (Rad180Mod(radians) * RAD_TO_DEG);
}

// use DegreesL(T* dValue) for longitude values only, where 180 degrees
// marks the boundary between east (positive) and west (negative)
template <class T>
void DegreesL(T* dValue)
{
	*dValue = (Rad180Mod(*dValue) * RAD_TO_DEG);
}

// Seconds from Radians
template <class T>
T Seconds(const T& radians)
{
	return radians * RAD_TO_SEC;
}

template <class T>
T Seconds(T* radians)
{
	return *radians * RAD_TO_SEC;
}

template <class T>
T SecondstoRadians(const T& seconds)
{
	return seconds / static_cast<T>(RAD_TO_SEC);
}

template <class T, typename U>
void DegtoDms(const T& dDegrees, U* dDegMinSec)
{
	T d, m, s;

	*dDegMinSec = fabs(dDegrees);		 // retain original value
	d = floor(*dDegMinSec);
	m = floor((((*dDegMinSec) - d) * 60.0));
	s = ((*dDegMinSec) - d - (m/60.0)) * 3600.0;
	if (fabs(s - 60.0) < 0.000000001)
	{
		s = 0.0;
		m += 1.0;
	}
	*dDegMinSec = d + (m/100.0) + (s/10000.0);
	if (dDegrees < 0.0)
		*dDegMinSec *= -1;
}

// DegtoDms helper
template <class T>
T DegtoDms(const T& dDegrees)
{
	T dDegMinSec;
	DegtoDms(dDegrees, &dDegMinSec);
	return dDegMinSec;
}

template <class T, typename U>
void DmstoDeg(const T& dDegMinSec, U* dDegrees)
{
	T dh, dm, ds;

	dh = fabs(dDegMinSec);		 // retain original value
	*dDegrees = floor(dh);
	dm = floor(((dh - (*dDegrees)) * 100.0) + 0.0001);
	ds = (((dh - (*dDegrees)) * 100.0) - dm) * 100.0;
	*dDegrees += ((dm / 60.0) + (ds / 3600.0));
	if (dDegMinSec < 0.0)
		(*dDegrees) *= -1;
}

// DmstoDeg helper
template <class T>
T DmstoDeg(const T& dDegMinSec)
{
	T dDegrees;
	DmstoDeg(dDegMinSec, &dDegrees);
	return dDegrees;
}

template <class T, typename U>
void DmstoRad(const T& dDegMinSec, U* dRadians)
{
	T dh, dm, ds;

	dh = fabs(dDegMinSec);		 // retain original value
	*dRadians = floor(dh);
	dm = floor(((dh - (*dRadians)) * 100.0) + 0.0001);
	ds = (((dh - (*dRadians)) * 100.0) - dm) * 100.0;
	*dRadians += ((dm / 60.0) + (ds / 3600.0));
	if (dDegMinSec < 0.0)
		(*dRadians) *= -1;
	Radians(dRadians);
}

// DmstoRad helper
template <class T>
T DmstoRad(const T& dDegMinSec)
{
	T dRadians;
	DmstoRad(dDegMinSec, &dRadians);
	return dRadians;
}

template <class T>
T RadtoDms(const T& dRadians)
{
	T dDms;
	DegtoDms(Degrees(dRadians), &dDms);
	return dDms;
}

template <class T>
T RadtoDmsL(const T& dRadians)
{
	T dDms;
	DegtoDms(DegreesL(dRadians), &dDms);
	return dDms;
}

template <class T, typename U>
void DmintoDeg(const T& dDegMin, U* dDegrees)
{
	T dh;

	dh = fabs(dDegMin);	// retain original value
	*dDegrees = floor(dh);
	*dDegrees += (dh - *dDegrees) * 100.0 / 60.0;
	if (dDegMin < 0.0)
		*dDegrees *= -1;
}

// DmintoDeg helper
template <class T>
T DmintoDeg(const T& dDegMin)
{
	T dDegrees;
	DmintoDeg(dDegMin, &dDegrees);
	return dDegrees;
}

template <class T, typename U>
void DegtoDmin(const T& dDegrees, U* dDegMin)
{
	T d;

	*dDegMin = fabs(dDegrees);	// retain original value
	d = floor(*dDegMin);
	*dDegMin = d + ((*dDegMin - d) * 0.6);

	if (dDegrees < 0.0)
		*dDegMin *= -1;
}

// DegDmin helper
template <class T>
T DegtoDmin(const T& dDegrees)
{
	T dDegMin;
	DegtoDin(dDegrees, &dDegMin);
	return dDegMin;
}

template <class T>
//            |
//     4th    |   1st
//            |
//	----------------------
//            |
//     3rd    |   2nd
//            |
T atan_2(const T& x, const T& y)
{
	T theta(atan(x / y));			// first quadrant (default)
	if (y < 0)						// second or third quadrant
		return theta + PI;
	else
	{
		if (x > 0)					// first quadrant
			return theta;
		else						// fourth quadrant
			return theta + TWO_PI;
	}
  return 0.;
}

#endif /* DNATEMPLATECALCFUNCS_H_ */


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2012, 2013 Klaus Spanderen
 Copyright (C) 2014 Johannes Göttker-Schnetmann

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 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 license for more details.
*/

/*! \file fdmsquarerootfwdop.cpp
    \brief Fokker-Planck forward operator for an square root process
*/

#include <ql/math/functional.hpp>
#include <ql/methods/finitedifferences/meshers/fdmmesher.hpp>
#include <ql/methods/finitedifferences/operators/fdmlinearoplayout.hpp>
#include <ql/methods/finitedifferences/operators/firstderivativeop.hpp>
#include <ql/methods/finitedifferences/operators/secondderivativeop.hpp>

#include <ql/experimental/finitedifferences/fdmsquarerootfwdop.hpp>
#include <ql/experimental/finitedifferences/modtriplebandlinearop.hpp>

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/distributions/non_central_chi_squared.hpp>

namespace QuantLib {

    FdmSquareRootFwdOp::FdmSquareRootFwdOp(
        const std::shared_ptr<FdmMesher>& mesher,
        Real kappa, Real theta, Real sigma,
        Size direction, TransformationType transform)
    : direction_(direction),
      kappa_(kappa),
      theta_(theta),
      sigma_(sigma),
      transform_(transform),
      mapX_(transform == Plain ?
          new ModTripleBandLinearOp(FirstDerivativeOp(direction_, mesher)
              .mult(kappa*(mesher->locations(direction_)-theta) + sigma*sigma)
              .add(SecondDerivativeOp(direction_, mesher)
                   .mult(0.5*sigma*sigma*mesher->locations(direction_)))
                .add(Array(mesher->layout()->size(), kappa)))

        : transform == Power ? new ModTripleBandLinearOp(
            SecondDerivativeOp(direction_, mesher)
               .mult(0.5*sigma*sigma*mesher->locations(direction_))
               .add(FirstDerivativeOp(direction_, mesher)
                     .mult(kappa*(mesher->locations(direction_)+theta)))
               .add(Array(mesher->layout()->size(),
                          2*kappa*kappa*theta/(sigma*sigma))))

            : new ModTripleBandLinearOp(FirstDerivativeOp(direction_, mesher)
                    .mult(Exp(-mesher->locations(direction))
                        *( -0.5*sigma*sigma - kappa*theta) + kappa)
                    .add(SecondDerivativeOp(direction_, mesher)
                    .mult(0.5*sigma*sigma*Exp(-mesher->locations(direction))))
                    .add(kappa*theta*Exp(-mesher->locations(direction))))
            ),
      v_  (mesher->layout()->dim()[direction_]) {

        const FdmLinearOpIterator endIter = mesher->layout()->end();
        for (FdmLinearOpIterator iter = mesher->layout()->begin();
            iter != endIter; ++iter) {
            const Real v = mesher->location(iter, direction_);
            v_[iter.coordinates()[direction_]] = v;
        }

        // zero flux boundary condition
        setLowerBC(mesher);
        setUpperBC(mesher);
    }

    void FdmSquareRootFwdOp::setLowerBC(
        const std::shared_ptr<FdmMesher>& mesher) {
        const Size n = 1;
        Real alpha, beta, gamma;

        getCoeff(alpha, beta, gamma, n);
        const Real f = lowerBoundaryFactor(transform_);

        const Real b = -(h(n-1)+h(n))/zeta(n);
        const Real c =  h(n-1)/zetap(n);

        const FdmLinearOpIterator endIter = mesher->layout()->end();
        for (FdmLinearOpIterator iter = mesher->layout()->begin();
            iter != endIter; ++iter) {
            if (iter.coordinates()[direction_] == 0) {
                const Size idx = iter.index();
                mapX_->diag()[idx]  = beta  + f*b; //*v(n-1);
                mapX_->upper()[idx] = gamma + f*c; //*v(n-1);
            }
        }
    }

    void FdmSquareRootFwdOp::setUpperBC(
        const std::shared_ptr<FdmMesher>& mesher) {
        const Size n = v_.size();
        Real alpha, beta, gamma;

        getCoeff(alpha, beta, gamma, n);
        const Real f = upperBoundaryFactor(transform_);

        const Real b = (h(n)+h(n-1))/zeta(n);
        const Real c = -h(n)/zetam(n);

        const FdmLinearOpIterator endIter = mesher->layout()->end();
        for (FdmLinearOpIterator iter = mesher->layout()->begin();
            iter != endIter; ++iter) {
            if (iter.coordinates()[direction_] == n-1) {
                const Size idx = iter.index();
                mapX_->diag()[idx] = beta   + f*b; //*v(n+1);
                mapX_->lower()[idx] = alpha + f*c; //*v(n+1);
            }
        }
    }

    Real FdmSquareRootFwdOp::lowerBoundaryFactor(TransformationType transform) const {
        if (transform == Plain) {
            return f0Plain();
        }
        else if (transform == Power) {
            return f0Power();
        }
        else if (transform == Log) {
            return f0Log();
        }
        else
            QL_FAIL("unknown transform");
    }

    Real FdmSquareRootFwdOp::upperBoundaryFactor(TransformationType transform) const {
        if (transform == Plain) {
            return f1Plain();
        }
        else if (transform == Power) {
            return f1Power();
        }
        else if (transform == Log) {
            return f1Log();
        }
        else
            QL_FAIL("unknown transform");
    }

    Real FdmSquareRootFwdOp::f0Plain() const {
        const Size n = 1;
        const Real a = -(2*h(n-1)+h(n))/zetam(n);
        const Real alpha = sigma_*sigma_*v(n)/zetam(n) - mu(n)*h(n)/zetam(n);
        const Real nu = a*v(n-1) + (2*kappa_*(v(n-1)-theta_) + sigma_*sigma_)
                                        /(sigma_*sigma_);

        return alpha/nu*v(n-1);
    }

    Real FdmSquareRootFwdOp::f1Plain() const {
        const Size n = v_.size();
        const Real a =  (2*h(n)+h(n-1))/zetap(n);
        const Real gamma = sigma_*sigma_*v(n)/zetap(n) + mu(n)*h(n-1)/zetap(n);
        const Real nu = a*v(n+1) + (2*kappa_*(v(n+1)-theta_) + sigma_*sigma_)
                        /(sigma_*sigma_);

        return gamma/nu*v(n+1);
    }

    Real FdmSquareRootFwdOp::f0Power() const {
        const Size n = 1;
        const Real mu = kappa_*(v(n)+theta_);
        const Real a = -(2*h(n-1)+h(n))/zetam(n);
        const Real alpha = sigma_*sigma_*v(n)/zetam(n) - mu*h(n)/zetam(n);
        const Real nu  = a*v(n-1) +2*(kappa_*v(n-1)/(sigma_*sigma_));

        return alpha/nu*v(n-1);
    }

    Real FdmSquareRootFwdOp::f1Power() const {
        const Size n = v_.size();
        const Real mu = kappa_*(v(n)+theta_);
        const Real a =  (2*h(n)+h(n-1))/zetap(n);
        const Real gamma = sigma_*sigma_*v(n)/zetap(n) + mu*h(n-1)/zetap(n);
        const Real nu = a*v(n+1) +2*(kappa_*v(n+1)/(sigma_*sigma_));

        return gamma/nu*v(n+1);
    }

    Real FdmSquareRootFwdOp::f0Log() const {
        const Size n = 1;
        const Real mu = ((-kappa_*theta_-sigma_*sigma_/2.0)*exp(-v(1))+kappa_);
        const Real a = -(2*h(n-1)+h(n))/zetam(n);
        const Real alpha = sigma_*sigma_*exp(-v(n))/zetam(n) - mu*h(n)/zetam(n);
        const Real nu = a*exp(-v(n-1)) + 2*kappa_*(1-theta_*exp(-v(n-1)))
                        /(sigma_*sigma_);

        return alpha/nu*exp(-v(n-1));
    }

    Real FdmSquareRootFwdOp::f1Log() const {
        const Size n = v_.size();
        const Real mu = ((-kappa_*theta_-sigma_*sigma_/2.0)*exp(-v(n))+kappa_);
        const Real a =  (2*h(n)+h(n-1))/zetap(n);
        const Real gamma = sigma_*sigma_*exp(-v(n))/zetap(n) + mu*h(n-1)/zetap(n);
        const Real nu = a*exp(-v(n+1)) + 2*kappa_*(1-theta_*exp(-v(n+1)))
                        /(sigma_*sigma_);

        return gamma/nu*exp(-v(n+1));
    }

    Real FdmSquareRootFwdOp::v(Size i) const {
        if (i > 0 && i <= v_.size()) {
            return v_[i-1];
        }
        else if (i == 0) {
            if (transform_ == Log) {
                return 2*v_[0] - v_[1];
//              log(std::max(0.5*exp(v_[0]), exp(v_[0] - 0.01 * (v_[1] - v_[0]))));
            } else {
                return std::max(0.5*v_[0], v_[0] - 0.01 * (v_[1] - v_[0]));
            }
        }
        else if (i == v_.size()+1) {
            return v_.back() + (v_.back() - *(v_.end()-2));
        }
        else {
            QL_FAIL("unknown index");
        }
    }

    Real FdmSquareRootFwdOp::h(Size i) const {
        return v(i+1) - v(i);
    }
    Real FdmSquareRootFwdOp::mu(Size i) const {
        return kappa_*(v(i) - theta_) + sigma_*sigma_;
    }
    Real FdmSquareRootFwdOp::zetam(Size i) const {
        return h(i-1)*(h(i-1)+h(i));
    }
    Real FdmSquareRootFwdOp::zeta(Size i) const {
        return h(i-1)*h(i);
    }
    Real FdmSquareRootFwdOp::zetap(Size i) const {
        return h(i)*(h(i-1)+h(i));
    }

    Size FdmSquareRootFwdOp::size() const {
        return 1;
    }
    void FdmSquareRootFwdOp::setTime(Time, Time) {
    }

    void FdmSquareRootFwdOp::getCoeff(Real& alpha, Real& beta,
                                               Real& gamma, Size n) const {
        if (transform_ == Plain) {
            getCoeffPlain(alpha, beta, gamma, n);
        }
        else if (transform_ == Power) {
            getCoeffPower(alpha, beta, gamma, n);
        }
        else if (transform_ == Log) {
            getCoeffLog(alpha, beta, gamma, n);
        }
    }

    void FdmSquareRootFwdOp::getCoeffPlain(Real& alpha, Real& beta,
                                               Real& gamma, Size n) const {
        alpha =   sigma_*sigma_*v(n)/zetam(n) - mu(n)*h(n)/zetam(n);
        beta  = - sigma_*sigma_*v(n)/zeta(n)
                    + mu(n)*(h(n)-h(n-1))/zeta(n) + kappa_;
        gamma =   sigma_*sigma_*v(n)/zetap(n) + mu(n)*h(n-1)/zetap(n);

    }

    void FdmSquareRootFwdOp::getCoeffLog(Real& alpha, Real& beta,
                                               Real& gamma, Size n) const {
        const Real mu = ((-kappa_*theta_-sigma_*sigma_/2.0)*exp(-v(n))+kappa_);
        alpha =   sigma_*sigma_*exp(-v(n))/zetam(n) - mu*h(n)/zetam(n);
        beta  = - sigma_*sigma_*exp(-v(n))/zeta(n)
                          + mu*(h(n)-h(n-1))/zeta(n) + kappa_*theta_*exp(-v(n));
        gamma =   sigma_*sigma_*exp(-v(n))/zetap(n) + mu*h(n-1)/zetap(n);
    }

    void FdmSquareRootFwdOp::getCoeffPower(Real& alpha, Real& beta,
                                               Real& gamma, Size n) const {
        const Real mu = kappa_*(theta_+v(n));
        alpha = (sigma_*sigma_*v(n) - mu*h(n))/zetam(n);
        beta = (-sigma_*sigma_*v(n) + mu*(h(n)-h(n-1)))/zeta(n)
                                + 2*kappa_*kappa_*theta_/(sigma_*sigma_);
        gamma=  (sigma_*sigma_*v(n) + mu*h(n-1))/zetap(n);
    }

    Disposable<Array> FdmSquareRootFwdOp::apply(const Array& p) const {
        return mapX_->apply(p);
    }

    Disposable<Array> FdmSquareRootFwdOp::apply_mixed(const Array& r) const {
        Array retVal(r.size(), 0.0);
        return retVal;
    }
    Disposable<Array> FdmSquareRootFwdOp::apply_direction(
        Size direction, const Array& r) const {
        if (direction == direction_) {
            return mapX_->apply(r);
        }
        else {
            Array retVal(r.size(), 0.0);
            return retVal;
        }
    }
    Disposable<Array> FdmSquareRootFwdOp::solve_splitting(
        Size direction, const Array& r, Real dt) const {
        if (direction == direction_) {
            return mapX_->solve_splitting(r, dt, 1.0);
        }
        else {
            Array retVal(r);
            return retVal;
        }
    }

    Disposable<Array> FdmSquareRootFwdOp::preconditioner(
        const Array& r, Real dt) const {
        return solve_splitting(direction_, r, dt);
    }

    #if !defined(QL_NO_UBLAS_SUPPORT)
    Disposable<std::vector<SparseMatrix> >
    FdmSquareRootFwdOp::toMatrixDecomp() const {
        std::vector<SparseMatrix> retVal(1, mapX_->toMatrix());
        return retVal;
    }
    #endif
}


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

#include <nt2/elliptic/functions/am.hpp>
#include <boost/dispatch/attributes.hpp>
#include <nt2/elliptic/functions/generic/details/am_kernel.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/pio_2.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/atan.hpp>
#include <nt2/include/functions/scalar/exp.hpp>
#include <nt2/include/functions/scalar/is_equal.hpp>
#include <nt2/include/functions/scalar/is_eqz.hpp>

namespace nt2 { namespace ext
{

   BOOST_DISPATCH_IMPLEMENT  (am_, tag::cpu_,
                              (A0)(A1)(A2),
                              (scalar_<floating_<A0> >)
                              (scalar_<floating_<A1> >)
                              (scalar_<floating_<A2> >)
                             )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE result_type operator()(const A0 & u, const A1 & x,
                                             const A2 & tol) const
    {
      if(is_eqz(x)) return u;
      result_type k = nt2::abs(x);
      if(eq(k, One<A0>())) return Two<A0>()*atan(exp(u))-Pio_2<A0>();
      return details::am_kernel(u, k, tol);
    }
  };

} }
#endif


// Boost.Geometry

// Copyright (c) 2020, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGY_CARTESIAN_SIDE_NON_ROBUST_HPP
#define BOOST_GEOMETRY_STRATEGY_CARTESIAN_SIDE_NON_ROBUST_HPP

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

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

/*!
\brief Adaptive precision predicate to check at which side of a segment a point lies:
    left of segment (>0), right of segment (< 0), on segment (0).
\ingroup strategies
\tparam CalculationType \tparam_calculation
\details This predicate determines at which side of a segment a point lies
*/
template
<
    typename CalculationType = void
>
struct side_non_robust
{
public:
    //! \brief Computes double the signed area of the CCW triangle p1, p2, p

#ifndef DOXYGEN_SHOULD_SKIP_THIS
    template
    <
        typename P1,
        typename P2,
        typename P
    >
    static inline int apply(P1 const& p1, P2 const& p2, P const& p)
    {
        typedef typename select_calculation_type_alt
            <
                CalculationType,
                P1,
                P2,
                P
            >::type coordinate_type;
        typedef typename select_most_precise
            <
                coordinate_type,
                double
            >::type promoted_type;

        auto detleft = (promoted_type(get<0>(p1)) - promoted_type(get<0>(p)))
                * (promoted_type(get<1>(p2)) - promoted_type(get<1>(p)));
        auto detright = (promoted_type(get<1>(p1)) - promoted_type(get<1>(p)))
                * (promoted_type(get<0>(p2)) - promoted_type(get<0>(p)));
        return detleft > detright ? 1 : (detleft < detright ? -1 : 0 );

    }
#endif

};

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGY_CARTESIAN_SIDE_NON_ROBUST_HPP


#ifndef STAN_ANALYZE_MCMC_COMPUTE_EFFECTIVE_SAMPLE_SIZE_HPP
#define STAN_ANALYZE_MCMC_COMPUTE_EFFECTIVE_SAMPLE_SIZE_HPP

#include <stan/math/prim/fun/Eigen.hpp>
#include <stan/analyze/mcmc/autocovariance.hpp>
#include <stan/analyze/mcmc/split_chains.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <algorithm>
#include <cmath>
#include <vector>
#include <limits>

namespace stan {
namespace analyze {
/**
 * Computes the effective sample size (ESS) for the specified
 * parameter across all kept samples.  The value returned is the
 * minimum of ESS and the number_total_draws *
 * log10(number_total_draws).
 *
 * See more details in Stan reference manual section "Effective
 * Sample Size". http://mc-stan.org/users/documentation
 *
 * Current implementation assumes draws are stored in contiguous
 * blocks of memory.  Chains are trimmed from the back to match the
 * length of the shortest chain.  Note that the effective sample size
 * can not be estimated with less than four draws.
 *
 * @param draws stores pointers to arrays of chains
 * @param sizes stores sizes of chains
 * @return effective sample size for the specified parameter
 */
inline double compute_effective_sample_size(std::vector<const double*> draws,
                                            std::vector<size_t> sizes) {
  int num_chains = sizes.size();
  size_t num_draws = sizes[0];
  for (int chain = 1; chain < num_chains; ++chain) {
    num_draws = std::min(num_draws, sizes[chain]);
  }

  if (num_draws < 4) {
    return std::numeric_limits<double>::quiet_NaN();
  }

  // check if chains are constant; all equal to first draw's value
  bool are_all_const = false;
  Eigen::VectorXd init_draw = Eigen::VectorXd::Zero(num_chains);

  for (int chain_idx = 0; chain_idx < num_chains; chain_idx++) {
    Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, 1>> draw(
        draws[chain_idx], sizes[chain_idx]);

    for (int n = 0; n < num_draws; n++) {
      if (!boost::math::isfinite(draw(n))) {
        return std::numeric_limits<double>::quiet_NaN();
      }
    }

    init_draw(chain_idx) = draw(0);

    if (draw.isApproxToConstant(draw(0))) {
      are_all_const |= true;
    }
  }

  if (are_all_const) {
    // If all chains are constant then return NaN
    // if they all equal the same constant value
    if (init_draw.isApproxToConstant(init_draw(0))) {
      return std::numeric_limits<double>::quiet_NaN();
    }
  }

  Eigen::Matrix<Eigen::VectorXd, Eigen::Dynamic, 1> acov(num_chains);
  Eigen::VectorXd chain_mean(num_chains);
  Eigen::VectorXd chain_var(num_chains);
  for (int chain = 0; chain < num_chains; ++chain) {
    Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, 1>> draw(
        draws[chain], sizes[chain]);
    autocovariance<double>(draw, acov(chain));
    chain_mean(chain) = draw.mean();
    chain_var(chain) = acov(chain)(0) * num_draws / (num_draws - 1);
  }

  double mean_var = chain_var.mean();
  double var_plus = mean_var * (num_draws - 1) / num_draws;
  if (num_chains > 1)
    var_plus += math::variance(chain_mean);
  Eigen::VectorXd rho_hat_s(num_draws);
  rho_hat_s.setZero();
  Eigen::VectorXd acov_s(num_chains);
  for (int chain = 0; chain < num_chains; ++chain)
    acov_s(chain) = acov(chain)(1);
  double rho_hat_even = 1.0;
  rho_hat_s(0) = rho_hat_even;
  double rho_hat_odd = 1 - (mean_var - acov_s.mean()) / var_plus;
  rho_hat_s(1) = rho_hat_odd;

  // Convert raw autocovariance estimators into Geyer's initial
  // positive sequence. Loop only until num_draws - 4 to
  // leave the last pair of autocorrelations as a bias term that
  // reduces variance in the case of antithetical chains.
  size_t s = 1;
  while (s < (num_draws - 4) && (rho_hat_even + rho_hat_odd) > 0) {
    for (int chain = 0; chain < num_chains; ++chain)
      acov_s(chain) = acov(chain)(s + 1);
    rho_hat_even = 1 - (mean_var - acov_s.mean()) / var_plus;
    for (int chain = 0; chain < num_chains; ++chain)
      acov_s(chain) = acov(chain)(s + 2);
    rho_hat_odd = 1 - (mean_var - acov_s.mean()) / var_plus;
    if ((rho_hat_even + rho_hat_odd) >= 0) {
      rho_hat_s(s + 1) = rho_hat_even;
      rho_hat_s(s + 2) = rho_hat_odd;
    }
    s += 2;
  }

  int max_s = s;
  // this is used in the improved estimate, which reduces variance
  // in antithetic case -- see tau_hat below
  if (rho_hat_even > 0)
    rho_hat_s(max_s + 1) = rho_hat_even;

  // Convert Geyer's initial positive sequence into an initial
  // monotone sequence
  for (int s = 1; s <= max_s - 3; s += 2) {
    if (rho_hat_s(s + 1) + rho_hat_s(s + 2) > rho_hat_s(s - 1) + rho_hat_s(s)) {
      rho_hat_s(s + 1) = (rho_hat_s(s - 1) + rho_hat_s(s)) / 2;
      rho_hat_s(s + 2) = rho_hat_s(s + 1);
    }
  }

  double num_total_draws = num_chains * num_draws;
  // Geyer's truncated estimator for the asymptotic variance
  // Improved estimate reduces variance in antithetic case
  double tau_hat = -1 + 2 * rho_hat_s.head(max_s).sum() + rho_hat_s(max_s + 1);
  return std::min(num_total_draws / tau_hat,
                  num_total_draws * std::log10(num_total_draws));
}

/**
 * Computes the effective sample size (ESS) for the specified
 * parameter across all kept samples.  The value returned is the
 * minimum of ESS and the number_total_draws *
 * log10(number_total_draws).
 *
 * See more details in Stan reference manual section "Effective
 * Sample Size". http://mc-stan.org/users/documentation
 *
 * Current implementation assumes draws are stored in contiguous
 * blocks of memory.  Chains are trimmed from the back to match the
 * length of the shortest chain.  Note that the effective sample size
 * can not be estimated with less than four draws.  Argument size
 * will be broadcast to same length as draws.
 *
 * @param draws stores pointers to arrays of chains
 * @param size size of chains
 * @return effective sample size for the specified parameter
 */
inline double compute_effective_sample_size(std::vector<const double*> draws,
                                            size_t size) {
  int num_chains = draws.size();
  std::vector<size_t> sizes(num_chains, size);
  return compute_effective_sample_size(draws, sizes);
}

/**
 * Computes the split effective sample size (ESS) for the specified
 * parameter across all kept samples.  The value returned is the
 * minimum of ESS and the number_total_draws *
 * log10(number_total_draws). When the number of total draws N is
 * odd, the (N+1)/2th draw is ignored.
 *
 * See more details in Stan reference manual section "Effective
 * Sample Size". http://mc-stan.org/users/documentation
 *
 * Current implementation assumes draws are stored in contiguous
 * blocks of memory.  Chains are trimmed from the back to match the
 * length of the shortest chain.  Note that the effective sample size
 * can not be estimated with less than four draws.
 *
 * @param draws stores pointers to arrays of chains
 * @param sizes stores sizes of chains
 * @return effective sample size for the specified parameter
 */
inline double compute_split_effective_sample_size(
    std::vector<const double*> draws, std::vector<size_t> sizes) {
  int num_chains = sizes.size();
  size_t num_draws = sizes[0];
  for (int chain = 1; chain < num_chains; ++chain) {
    num_draws = std::min(num_draws, sizes[chain]);
  }

  std::vector<const double*> split_draws = split_chains(draws, sizes);

  double half = num_draws / 2.0;
  std::vector<size_t> half_sizes(2 * num_chains, std::floor(half));

  return compute_effective_sample_size(split_draws, half_sizes);
}

/**
 * Computes the split effective sample size (ESS) for the specified
 * parameter across all kept samples.  The value returned is the
 * minimum of ESS and the number_total_draws *
 * log10(number_total_draws). When the number of total draws N is
 * odd, the (N+1)/2th draw is ignored.
 *
 * See more details in Stan reference manual section "Effective
 * Sample Size". http://mc-stan.org/users/documentation
 *
 * Current implementation assumes draws are stored in contiguous
 * blocks of memory.  Chains are trimmed from the back to match the
 * length of the shortest chain.  Note that the effective sample size
 * can not be estimated with less than four draws.  Argument size
 * will be broadcast to same length as draws.
 *
 * @param draws stores pointers to arrays of chains
 * @param size size of chains
 * @return effective sample size for the specified parameter
 */
inline double compute_split_effective_sample_size(
    std::vector<const double*> draws, size_t size) {
  int num_chains = draws.size();
  std::vector<size_t> sizes(num_chains, size);
  return compute_split_effective_sample_size(draws, sizes);
}

}  // namespace analyze
}  // namespace stan

#endif


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

#pragma once

#include <cmath>

#include <Eigen/Geometry>

#include "kindr/common/common.hpp"
#include "kindr/common/assert_macros_eigen.hpp"
#include "kindr/rotations/RotationBase.hpp"

namespace kindr {

/*! \class RotationMatrix
 *  \brief Implementation of matrix rotation based on Eigen::Matrix<Scalar, 3, 3>
 *
 *  The following four typedefs are provided for convenience:
 *   - \ref RotationMatrixAD "RotationMatrixD" for double primitive type
 *   - \ref RotationMatrixAF "RotationMatrixF" for float primitive type
 *
 *  \tparam PrimType_ the primitive type of the data (double or float)
 *
 *  \ingroup rotations
 */
template<typename PrimType_>
class RotationMatrix : public RotationBase<RotationMatrix<PrimType_>>, private Eigen::Matrix<PrimType_, 3, 3> {
 private:
  /*! \brief The base type.
   */
  typedef Eigen::Matrix<PrimType_, 3, 3> Base;
 public:
  /*! \brief The implementation type.
   *  The implementation type is always an Eigen object.
   */
  typedef Base Implementation;
  /*! \brief The primitive type.
   *  Float/Double
   */
  typedef PrimType_ Scalar;

  /*! \brief Default constructor using identity rotation.
   */
  RotationMatrix()
    : Base(Base::Identity()) {
  }

  /*! \brief Constructor using nine scalars.
   *  In debug mode, an assertion is thrown if the matrix is not a rotation matrix.
   *  \param r11     entry in row 1, col 1
   *  \param r12     entry in row 1, col 2
   *  \param r13     entry in row 1, col 3
   *  \param r21     entry in row 2, col 1
   *  \param r22     entry in row 2, col 2
   *  \param r23     entry in row 2, col 3
   *  \param r31     entry in row 3, col 1
   *  \param r32     entry in row 3, col 2
   *  \param r33     entry in row 3, col 3
   */
  RotationMatrix(Scalar r11, Scalar r12, Scalar r13,
                 Scalar r21, Scalar r22, Scalar r23,
                 Scalar r31, Scalar r32, Scalar r33) {

    *this << r11,r12,r13,r21,r22,r23,r31,r32,r33;

    KINDR_ASSERT_MATRIX_NEAR_DBG(std::runtime_error, this->toImplementation() * this->toImplementation().transpose(), Base::Identity(), static_cast<Scalar>(1e-4), "Input matrix is not orthogonal.");
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->determinant(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input matrix determinant is not 1.");
  }

  /*! \brief Constructor using Eigen::Matrix.
   *  In debug mode, an assertion is thrown if the rotation vector has not unit length.
   *  \param other   Eigen::Matrix<PrimType_,3,3>
   */
  explicit RotationMatrix(const Base& other)
  // : Base(other)
  {

    this->toImplementation() = other;

    KINDR_ASSERT_MATRIX_NEAR_DBG(std::runtime_error, other * other.transpose(), Base::Identity(), static_cast<Scalar>(1e-4), "Input matrix is not orthogonal.");
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, other.determinant(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input matrix determinant is not 1.");
  }

  /*! \brief Constructor using another rotation.
   *  \param other   other rotation
   */
  template<typename OtherDerived_>
  inline explicit RotationMatrix(const RotationBase<OtherDerived_>& other)
  // : Base(internal::ConversionTraits<RotationMatrix, OtherDerived_>::convert(other.derived()).toImplementation())
  {
    this->toImplementation() = internal::ConversionTraits<RotationMatrix, OtherDerived_>::convert(other.derived()).toImplementation();
  }

  /*! \brief Assignment operator using another rotation.
   *  \param other   other rotation
   *  \returns referece
   */
  template<typename OtherDerived_>
  RotationMatrix& operator =(const RotationBase<OtherDerived_>& other) {
    this->toImplementation() = internal::ConversionTraits<RotationMatrix, OtherDerived_>::convert(other.derived()).toImplementation();
    return *this;
  }

  /*! \brief Parenthesis operator to convert from another rotation.
   *  \param other   other rotation
   *  \returns reference
   */
  template<typename OtherDerived_>
  RotationMatrix& operator ()(const RotationBase<OtherDerived_>& other) {
    this->toImplementation() = internal::ConversionTraits<RotationMatrix, OtherDerived_>::convert(other.derived()).toImplementation();
    return *this;
  }

  /*! \brief Returns the inverse of the rotation.
   *  \returns the inverse of the rotation
   */
  RotationMatrix inverted() const {
    RotationMatrix matrix;
    matrix.toImplementation() = this->toImplementation().transpose();
    return matrix;
  }

  /*! \brief Inverts the rotation.
   *  \returns reference
   */
  RotationMatrix& invert() {
    *this = this->inverted();
    return *this;
  }

  /*! \brief Returns the transpose of the rotation matrix.
   *  \returns the inverse of the rotation
   */
  RotationMatrix transposed() const {
    RotationMatrix matrix;
    matrix.toImplementation() = this->toImplementation().transpose();
    return matrix;
  }

  /*! \brief Transposes the rotation matrix.
   *  \returns reference
   */
  RotationMatrix& transpose() {
    *this = this->transposed();
    return *this;
  }

  /*! \brief Returns the determinant of the rotation matrix.
   *  \returns determinant of the rotation matrix
   */
  Scalar determinant() const {
  return toImplementation().determinant();
  }

  /*! \brief Cast to the implementation type.
   *  \returns the implementation for direct manipulation (recommended only for advanced users)
   */
  inline Implementation& toImplementation() {
    return static_cast<Implementation&>(*this);
  }

  /*! \brief Cast to the implementation type.
   *  \returns the implementation for direct manipulation (recommended only for advanced users)
   */
  inline const Implementation& toImplementation() const {
    return static_cast<const Implementation&>(*this);
  }

  /*! \brief Reading access to the rotation matrix.
   *  \returns rotation matrix (matrix) with reading access
   */
  inline Implementation matrix() const {
      return this->toImplementation();
  }

  /*! \brief  Writing access to the rotation matrix.
   */
  inline void setMatrix(const Implementation & input) {
      this->toImplementation() = input;
  }

  /*! \brief  Writing access to the rotation matrix.
   */
  inline void setMatrix(Scalar r11, Scalar r12, Scalar r13,
                        Scalar r21, Scalar r22, Scalar r23,
                        Scalar r31, Scalar r32, Scalar r33) {

     *this << r11,r12,r13,r21,r22,r23,r31,r32,r33;

  }

  /*! \brief Sets the rotation to identity.
   *  \returns reference
   */
  RotationMatrix& setIdentity() {
    this->Implementation::setIdentity();
    return *this;
  }

  /*! \brief Returns a unique matrix rotation.
   *  A rotation matrix is always unique.
   *  This function is used to compare different rotations.
   *  \returns copy of the matrix rotation which is unique
   */
  RotationMatrix getUnique() const {
    return *this;
  }

  /*! \brief Modifies the matrix rotation such that it becomes unique.
   *  A rotation matrix is always unique.
   *  \returns reference
   */
  RotationMatrix& setUnique() {
    return *this;
  }

  /*! \brief Concenation operator.
   *  This is explicitly specified, because Eigen::Matrix provides also an operator*.
   *  \returns the concenation of two rotations
   */
  using RotationBase<RotationMatrix<PrimType_>>::operator*; // otherwise ambiguous RotationBase and Eigen

  /*! \brief Equivalence operator.
   *  This is explicitly specified, because Eigen::Matrix provides also an operator==.
   *  \returns true if two rotations are equal.
   */
  using RotationBase<RotationMatrix<PrimType_>>::operator==; // otherwise ambiguous RotationBase and Eigen

  /*! \brief Inequivalence operator.
   *  This is explicitly specified, because Eigen::Matrix provides also an operator!=.
   *  \returns true if two rotations are not equal.
   */
  using RotationBase<RotationMatrix<PrimType_>>::operator!=; // otherwise ambiguous RotationBase and Eigen

  /*! \brief Used for printing the object with std::cout.
   *  \returns std::stream object
   */
  friend std::ostream& operator << (std::ostream& out, const RotationMatrix& rotationMatrix) {
    out << rotationMatrix.toImplementation();
    return out;
  }
};

//! \brief Active matrix rotation with double primitive type
typedef RotationMatrix<double>  RotationMatrixD;
//! \brief Active matrix rotation with float primitive type
typedef RotationMatrix<float>  RotationMatrixF;
//! \brief Passive matrix rotation with double primitive type
typedef RotationMatrix<double> RotationMatrixPD;
//! \brief Passive matrix rotation with float primitive type
typedef RotationMatrix<float> RotationMatrixPF;

namespace internal {

template<typename PrimType_>
class get_scalar<RotationMatrix<PrimType_>> {
 public:
  typedef PrimType_ Scalar;
};

template<typename PrimType_>
class get_matrix3X<RotationMatrix<PrimType_>>{
 public:
  typedef int  IndexType;

  template <IndexType Cols>
  using Matrix3X = Eigen::Matrix<PrimType_, 3, Cols>;
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Conversion Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<RotationMatrix<DestPrimType_>, AngleAxis<SourcePrimType_>> {
 public:
  inline static RotationMatrix<DestPrimType_> convert(const AngleAxis<SourcePrimType_>& aa) {
    RotationMatrix<DestPrimType_> matrix;
    matrix.toImplementation() = (aa.toImplementation().template cast<DestPrimType_>()).toRotationMatrix();
    return matrix;
  }
};

template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<RotationMatrix<DestPrimType_>, RotationVector<SourcePrimType_>> {
 public:
  inline static RotationMatrix<DestPrimType_> convert(const RotationVector<SourcePrimType_>& rotationVector) {
    return RotationMatrix<DestPrimType_>(RotationQuaternion<DestPrimType_>(rotationVector));
  }
};

template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<RotationMatrix<DestPrimType_>, RotationQuaternion<SourcePrimType_>> {
 public:
  inline static RotationMatrix<DestPrimType_> convert(const RotationQuaternion<SourcePrimType_>& q) {
    RotationMatrix<DestPrimType_> matrix;
    matrix.toImplementation() = (q.toImplementation().template cast<DestPrimType_>()).toRotationMatrix();
    return matrix;
  }
};

template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<RotationMatrix<DestPrimType_>, RotationMatrix<SourcePrimType_>> {
 public:
  inline static RotationMatrix<DestPrimType_> convert(const RotationMatrix<SourcePrimType_>& R) {
    RotationMatrix<DestPrimType_> matrix;
    matrix.toImplementation() = R.toImplementation().template cast<DestPrimType_>();
    return matrix;
  }
};

template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<RotationMatrix<DestPrimType_>, EulerAnglesXyz<SourcePrimType_>> {
 public:
  inline static RotationMatrix<DestPrimType_> convert(const EulerAnglesXyz<SourcePrimType_>& xyz) {
      RotationMatrix<DestPrimType_> matrix;
      matrix.toImplementation() = RotationQuaternion<DestPrimType_>(xyz).toImplementation().toRotationMatrix();
      return matrix;
  }
};

template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<RotationMatrix<DestPrimType_>, EulerAnglesZyx<SourcePrimType_>> {
 public:
  inline static RotationMatrix<DestPrimType_> convert(const EulerAnglesZyx<SourcePrimType_>& zyx) {
    RotationMatrix<DestPrimType_> matrix;
    matrix.toImplementation() = RotationQuaternion<DestPrimType_>(zyx).toImplementation().toRotationMatrix();
    return matrix;
  }
};

/*! \brief Multiplication of two rotation matrices
 */
template<typename PrimType_>
class MultiplicationTraits<RotationBase<RotationMatrix<PrimType_>>, RotationBase<RotationMatrix<PrimType_>>> {
 public:
  inline static RotationMatrix<PrimType_> mult(const RotationMatrix<PrimType_>& lhs, const RotationMatrix<PrimType_>& rhs) {
      RotationMatrix<PrimType_> result;
      result.toImplementation() = lhs.toImplementation() * rhs.toImplementation();
      return result;
  }
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Rotation Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Comparison Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Box Operations - required?
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Fixing Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename PrimType_>
class FixingTraits<RotationMatrix<PrimType_>> {
 public:
  inline static void fix(RotationMatrix<PrimType_>& R) {
    const PrimType_ factor = 1/pow(R.determinant(), 1.0/3.0);
    R.setMatrix(factor*R.matrix()(0,0),
                factor*R.matrix()(0,1),
                factor*R.matrix()(0,2),
                factor*R.matrix()(1,0),
                factor*R.matrix()(1,1),
                factor*R.matrix()(1,2),
                factor*R.matrix()(2,0),
                factor*R.matrix()(2,1),
                factor*R.matrix()(2,2));
  }
};

} // namespace internal
} // namespace kindr


#include "multiplicative_relations.h"
#include <hamming/jain_commitment/jain_commitment.h>
#include <NTL/mat_GF2.h>
#include <NTL/vec_GF2.h>
#include <hamming/knowledge_of_valid_opening/knowledge_of_valid_opening.h>
#include <utils/utils.h>

void hamming_metric::multiplicative_relations::initialize_commitments_and_responses(
        commitments_t *commitments,
        responses_t *responses) {

    responses->t_i_0.SetLength(I);
    responses->t_i_1.SetLength(I);
    responses->t_i_2.SetLength(I);
    responses->t_i_j_0.SetLength(I);
    responses->t_i_j_1.SetLength(I);
    responses->t_i_j_2.SetLength(I);

    commitments->c_i_0.SetLength(I);
    commitments->c_i_1.SetLength(I);
    commitments->c_i_2.SetLength(I);
    commitments->c_i_j_0.SetLength(I);
    commitments->c_i_j_1.SetLength(I);
    commitments->c_i_j_2.SetLength(I);

    commitments->r_i_0.SetLength(I);
    commitments->r_i_1.SetLength(I);
    commitments->r_i_2.SetLength(I);
    commitments->r_i_j_0.SetLength(I);
    commitments->r_i_j_1.SetLength(I);
    commitments->r_i_j_2.SetLength(I);

    for (int i = 0; i < I; i++) {
        responses->t_i_j_0[i].SetLength(J);
        responses->t_i_j_1[i].SetLength(J);
        responses->t_i_j_2[i].SetLength(J);

        commitments->c_i_j_0[i].SetLength(J);
        commitments->c_i_j_1[i].SetLength(J);
        commitments->c_i_j_2[i].SetLength(J);

        commitments->r_i_j_0[i].SetLength(J);
        commitments->r_i_j_1[i].SetLength(J);
        commitments->r_i_j_2[i].SetLength(J);
    }
}

void hamming_metric::multiplicative_relations::generate_private_key(
        private_key_t *private_key) {

    //Generate m
    {
        private_key->m.kill();
        private_key->m.SetLength(I);

        utils::generate_random_binary_vector(
                private_key->m[0],
                JAIN_V);

        utils::generate_random_binary_vector(
                private_key->m[1],
                JAIN_V);

        private_key->m[2].SetLength(JAIN_V);
        for (int i = 0; i < JAIN_V; i++) {
            private_key->m[2][i] = private_key->m[0][i] * private_key->m[1][i];
        }
    }

    //Generate e
    {
        private_key->e.kill();
        private_key->e.SetLength(I);

        for (int i = 0; i < I; i++) {
            utils::generate_vector_of_weight_w(
                    private_key->e[i],
                    JAIN_K,
                    W);
        }
    }
}

void hamming_metric::multiplicative_relations::generate_public_key(
        public_key_t *public_key,
        const private_key_t *private_key) {

    public_key->commitments_i.kill();
    public_key->commitments_i.SetLength(I);

    utils::generate_random_binary_matrix(
            public_key->A,
            JAIN_K,
            JAIN_L + JAIN_V);

    for (int i = 0; i < I; i++) {
        NTL::vec_GF2 r;
        utils::generate_random_binary_vector(
                r,
                JAIN_L);

        hamming_metric::commitment::generate_commitment(
                public_key->commitments_i[i],
                public_key->A,
                r,
                private_key->m[i],
                private_key->e[i]);
    }
}

void hamming_metric::multiplicative_relations::generate_random_values(
        random_values_t *random_values,
        const multiplicative_relation_matrices_t *matrices) {

    //Generate u_i_j
    {
        random_values->u_i_j.kill();
        random_values->u_i_j.SetLength(I);
        for (int i = 0; i < I; i++) {
            random_values->u_i_j[i].SetLength(J);
            for (int j = 0; j < J; j++) {
                random_values->u_i_j[i][j] = NTL::random_vec_GF2(JAIN_L);
            }
        }
    }

    //Generate f_i_j
    {
        random_values->f_i_j.kill();
        random_values->f_i_j.SetLength(I);
        for (int i = 0; i < I; i++) {
            random_values->f_i_j[i].SetLength(J);
            for (int j = 0; j < J; j++) {
                random_values->f_i_j[i][j] = NTL::random_vec_GF2(JAIN_K);
            }
        }
    }

    //Generate v_i_j
    {
        random_values->v_i_j.kill();
        random_values->v_i_j.SetLength(I);
        for (int i = 0; i < I; i++) {
            random_values->v_i_j[i].SetLength(J);
            for (int j = 0; j < J; j++) {
                random_values->v_i_j[i][j] = NTL::random_vec_GF2(JAIN_V);
            }
        }
    }

    {
        random_values->u_i.kill();
        random_values->u_i.SetLength(I);
        for (int i = 0; i < I; i++) {
            random_values->u_i[i] = NTL::random_vec_GF2(JAIN_L);
        }
    }

    {
        random_values->f_i.kill();
        random_values->f_i.SetLength(I);
        for (int i = 0; i < I; i++) {
            random_values->f_i[i] = NTL::random_vec_GF2(JAIN_K);
        }
    }

    //Generate v_i
    {
        random_values->v_i.kill();
        random_values->v_i.SetLength(I);
        for (int i = 0; i < I; i++) {
            random_values->v_i[i].SetLength(JAIN_V);
            for (int j = 0; j < J; j++) {
                random_values->v_i[i] += (matrices->R[j] * random_values->v_i_j[i][j]);
            }
        }
    }
}

void hamming_metric::multiplicative_relations::generate_revealed_values(
        revealed_values_t *revealed_values,
        const private_key_t *private_key,
        const public_key_t *public_key) {

    revealed_values->commitments_i_j.kill();
    revealed_values->commitments_i_j.SetLength(I);

    revealed_values->e_i_j.kill();
    revealed_values->e_i_j.SetLength(I);

    revealed_values->P_i.SetLength(I);
    revealed_values->P_i_j.SetLength(I);

    NTL::Vec<NTL::vec_GF2> m_prime_i;
    {
        m_prime_i.SetLength(I);
        utils::sample_messages(m_prime_i[0], m_prime_i[1], m_prime_i[2], JAIN_V);
        utils::generate_relation_matrix(
                revealed_values->matrices._R,
                revealed_values->matrices.R,
                m_prime_i,
                private_key->m,
                JAIN_V);
    }

    {
        generate_m_prime_i_j_from_m_prime_i(
                revealed_values->m_prime_i_j,
                m_prime_i);
    }

    NTL::vec_GF2 r;
    for (int i = 0; i < I; i++) {
        revealed_values->commitments_i_j[i].SetLength(J);
        revealed_values->e_i_j[i].SetLength(J);
        revealed_values->P_i_j[i].SetLength(J);

        utils::create_permutation_matrix(
                revealed_values->P_i[i],
                JAIN_K);

        for (int j = 0; j < J; j++) {
            utils::create_permutation_matrix(
                    revealed_values->P_i_j[i][j],
                    JAIN_K);

            utils::generate_random_binary_vector(
                    r,
                    JAIN_L);

            utils::generate_vector_of_weight_w(
                    revealed_values->e_i_j[i][j],
                    JAIN_K,
                    W);

            hamming_metric::commitment::generate_commitment(
                    revealed_values->commitments_i_j[i][j],
                    public_key->A,
                    r,
                    revealed_values->m_prime_i_j[i][j],
                    revealed_values->e_i_j[i][j]);
        }
    }
}

void hamming_metric::multiplicative_relations::generate_m_prime_i_j_from_m_prime_i(
        NTL::Vec<NTL::Vec<NTL::vec_GF2>> &m_prime_i_j,
        const NTL::Vec<NTL::vec_GF2> &m_prime_i) {

    m_prime_i_j.kill();
    m_prime_i_j.SetLength(I);
    int start_index;

    for (int i = 0; i < I; i++) {
        m_prime_i_j[i].SetLength(J);
        start_index = 0;
        for (int j = 0; j < J; j++) {
            NTL::vec_GF2 tmp;
            tmp.SetLength(JAIN_V);
            for (int z = 0; z < JAIN_V; z++) {
                tmp[z] = m_prime_i[i][start_index + z];
            }
            start_index += JAIN_V;
            m_prime_i_j[i][j].append(tmp);
        }
    }

}

void hamming_metric::multiplicative_relations::generate_commitments_and_responses(
        responses_t *responses,
        commitments_t *commitments,
        const random_values_t *random_values,
        const revealed_values_t *revealed_values,
        const public_key_t *public_key,
        const private_key_t *private_key) {

    for (int i = 0; i < I; i++) {

        auto u_v = random_values->u_i[i];
        u_v.append(random_values->v_i[i]);
        knowledge_of_valid_opening::generate_commitment_and_response_0(
                commitments->c_i_0[i],
                commitments->r_i_0[i],
                responses->t_i_0[i],
                u_v,
                public_key->A,
                random_values->f_i[i]);

        knowledge_of_valid_opening::generate_commitment_and_response_1(
                commitments->c_i_1[i],
                commitments->r_i_1[i],
                responses->t_i_1[i],
                public_key->A,
                revealed_values->P_i[i],
                random_values->f_i[i]);

        knowledge_of_valid_opening::generate_commitment_and_response_2(
                commitments->c_i_2[i],
                commitments->r_i_2[i],
                responses->t_i_2[i],
                public_key->A,
                revealed_values->P_i[i],
                random_values->f_i[i],
                private_key->e[i]);

        for (int j = 0; j < J; j++) {

            u_v = random_values->u_i_j[i][j];
            u_v.append(random_values->v_i_j[i][j]);
            knowledge_of_valid_opening::generate_commitment_and_response_0(
                    commitments->c_i_j_0[i][j],
                    commitments->r_i_j_0[i][j],
                    responses->t_i_j_0[i][j],
                    u_v,
                    public_key->A,
                    random_values->f_i_j[i][j]);

            knowledge_of_valid_opening::generate_commitment_and_response_1(
                    commitments->c_i_j_1[i][j],
                    commitments->r_i_j_1[i][j],
                    responses->t_i_j_1[i][j],
                    public_key->A,
                    revealed_values->P_i_j[i][j],
                    random_values->f_i_j[i][j]);

            knowledge_of_valid_opening::generate_commitment_and_response_2(
                    commitments->c_i_j_2[i][j],
                    commitments->r_i_j_2[i][j],
                    responses->t_i_j_2[i][j],
                    public_key->A,
                    revealed_values->P_i_j[i][j],
                    random_values->f_i_j[i][j],
                    revealed_values->e_i_j[i][j]);
        }
    }
}

int hamming_metric::multiplicative_relations::verify_0(
        const NTL::Vec<NTL::vec_GF2> &c_i_0,
        const NTL::Vec<NTL::vec_GF2> &r_i_0,
        const NTL::Vec<NTL::vec_GF2> &t_i_0,
        const NTL::Vec<NTL::vec_GF2> &c_i_1,
        const NTL::Vec<NTL::vec_GF2> &r_i_1,
        const NTL::Vec<NTL::vec_GF2> &t_i_1,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &c_i_j_0,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &r_i_j_0,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &t_i_j_0,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &c_i_j_1,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &r_i_j_1,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &t_i_j_1,
        const NTL::Vec<NTL::mat_GF2> &P_i,
        const NTL::Vec<NTL::Vec<NTL::mat_GF2>> &P_i_j,
        const NTL::Vec<NTL::mat_GF2> &R,
        const public_key_t *public_key) {

    for (int i = 0; i < I; i++) {
        auto _t0 = utils::encode_binary_vector(t_i_0[i], JAIN_V);
        auto _t1 = utils::encode_binary_vector(t_i_1[i], JAIN_V);

        if (hamming_metric::commitment::verify(
                c_i_0[i],
                public_key->A,
                r_i_0[i],
                _t0) != 0) {
            std::cout << "Multiplicative Relations. Verification failed on ch = 0 and c0" << std::endl;
            return 1;
        }

        if (hamming_metric::commitment::verify(
                c_i_1[i],
                public_key->A,
                r_i_1[i],
                _t1) != 0) {
            std::cout << "Multiplicative Relations. Verification failed on ch = 0 and c1" << std::endl;
            return 1;
        }
        for (int j = 0; j < J; j++) {
            _t0 = utils::encode_binary_vector(t_i_j_0[i][j], JAIN_V);
            _t1 = utils::encode_binary_vector(t_i_j_1[i][j], JAIN_V);

            if (hamming_metric::commitment::verify(
                    c_i_j_0[i][j],
                    public_key->A,
                    r_i_j_0[i][j],
                    _t0) != 0) {
                std::cout << "Multiplicative Relations. Verification failed on ch = 0 and c0" << std::endl;
                return 1;
            }

            if (hamming_metric::commitment::verify(
                    c_i_j_1[i][j],
                    public_key->A,
                    r_i_j_1[i][j],
                    _t1) != 0) {
                std::cout << "Multiplicative Relations. Verification failed on ch = 0 and c1" << std::endl;
                return 1;
            }
        }
    }

    NTL::Vec<NTL::vec_GF2> results_i;
    NTL::Vec<NTL::Vec<NTL::vec_GF2>> results_i_j;
    results_i_j.SetLength(I);
    for (int i = 0; i < I; i++) {
        NTL::vec_GF2 result;

        if (utils::solve_equation(
                result,
                public_key->A,
                t_i_0[i] + (NTL::inv(P_i[i]) * t_i_1[i])) != 0) {
            std::cout << "No Solutions" << std::endl;
            return 1;
        }
        results_i.append(result);

        for (int j = 0; j < J; j++) {
            if (utils::solve_equation(
                    result,
                    public_key->A,
                    t_i_j_0[i][j] + (NTL::inv(P_i_j[i][j]) * t_i_j_1[i][j])) != 0) {
                std::cout << "No Solutions" << std::endl;
                return 1;
            }
            results_i_j[i].append(result);
        }
    }

    NTL::Vec<NTL::vec_GF2> b_i;
    b_i.SetLength(I);
    for (int i = 0; i < I; i++) {
        b_i[i].SetLength(JAIN_V);
        for (int j = 0; j < JAIN_V; j++) {
            b_i[i][j] = results_i[i][j + JAIN_L];
        }
    }

    NTL::Vec<NTL::Vec<NTL::vec_GF2>> b_i_j;
    b_i_j.SetLength(I);
    for (int i = 0; i < I; i++) {
        b_i_j[i].SetLength(J);
        for (int j = 0; j < J; j++) {
            b_i_j[i][j].SetLength(JAIN_V);
            for (int z = 0; z < JAIN_V; z++) {
                b_i_j[i][j][z] = results_i_j[i][j][z + JAIN_L];
            }
        }
    }

    NTL::Vec<NTL::vec_GF2> r_b;
    r_b.SetLength(I);
    for (int i = 0; i < I; i++) {
        for (int j = 0; j < J; j++) {
            r_b[i].SetLength(JAIN_V);
            r_b[i] += (R[j] * b_i_j[i][j]);
        }
        if (b_i[i] != r_b[i]) {
            std::cout << "Error: verify_0" << std::endl;
            return 1;
        }
    }

    return 0;
}

int hamming_metric::multiplicative_relations::verify_1(
        const NTL::Vec<NTL::vec_GF2> &c_i_0,
        const NTL::Vec<NTL::vec_GF2> &r_i_0,
        const NTL::Vec<NTL::vec_GF2> &t_i_0,
        const NTL::Vec<NTL::vec_GF2> &c_i_2,
        const NTL::Vec<NTL::vec_GF2> &r_i_2,
        const NTL::Vec<NTL::vec_GF2> &t_i_2,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &c_i_j_0,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &r_i_j_0,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &t_i_j_0,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &c_i_j_2,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &r_i_j_2,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &t_i_j_2,
        const NTL::Vec<NTL::mat_GF2> &P_i,
        const NTL::Vec<NTL::Vec<NTL::mat_GF2>> &P_i_j,
        const NTL::Vec<NTL::mat_GF2> &R,
        const NTL::mat_GF2 &_R,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &commitments_i_j,
        const public_key_t *public_key) {

    for (int i = 0; i < I; i++) {
        auto _t0 = utils::encode_binary_vector(t_i_0[i], JAIN_V);
        auto _t2 = utils::encode_binary_vector(t_i_2[i], JAIN_V);

        if (hamming_metric::commitment::verify(
                c_i_0[i],
                public_key->A,
                r_i_0[i],
                _t0) != 0) {
            std::cout << "Multiplicative Relations. Verification failed on ch = 1 and c0" << std::endl;
            return 1;
        }

        if (hamming_metric::commitment::verify(
                c_i_2[i],
                public_key->A,
                r_i_2[i],
                _t2) != 0) {
            std::cout << "Multiplicative Relations. Verification failed on ch = 1 and c2" << std::endl;
            return 1;
        }
        for (int j = 0; j < J; j++) {
            _t0 = utils::encode_binary_vector(t_i_j_0[i][j], JAIN_V);
            _t2 = utils::encode_binary_vector(t_i_j_2[i][j], JAIN_V);

            if (hamming_metric::commitment::verify(
                    c_i_j_0[i][j],
                    public_key->A,
                    r_i_j_0[i][j],
                    _t0) != 0) {
                std::cout << "Multiplicative Relations. Verification failed on ch = 1 and c0" << std::endl;
                return 1;
            }

            if (hamming_metric::commitment::verify(
                    c_i_j_2[i][j],
                    public_key->A,
                    r_i_j_2[i][j],
                    _t2) != 0) {
                std::cout << "Multiplicative Relations. Verification failed on ch = 1 and c2" << std::endl;
                return 1;
            }
        }
    }

    // Check rank of matrix
    auto m = _R;
    if (NTL::gauss(m) != m.NumRows()) {
        std::cout << "Not full rank matrix" << std::endl;
        return 1;
    }

    // Ensure that each row has weight one
    for (int i = 0; i < m.NumRows(); i++) {
        if (NTL::weight(m[i]) != 1) {
            std::cout << "Invalid weight" << std::endl;
            return 1;
        }
    }

    NTL::Vec<NTL::vec_GF2> results_i;
    NTL::Vec<NTL::Vec<NTL::vec_GF2>> results_i_j;
    results_i_j.SetLength(I);
    for (int i = 0; i < I; i++) {
        NTL::vec_GF2 result;

        if (utils::solve_equation(
                result,
                public_key->A,
                public_key->commitments_i[i] + t_i_0[i] + (NTL::inv(P_i[i]) * t_i_2[i])) != 0) {
            std::cout << "No Solutions 1" << std::endl;
            return 1;
        }
        results_i.append(result);

        for (int j = 0; j < J; j++) {
            if (utils::solve_equation(
                    result,
                    public_key->A,
                    commitments_i_j[i][j] + t_i_j_0[i][j] +
                    (NTL::inv(P_i_j[i][j]) * t_i_j_2[i][j])) != 0) {
                std::cout << "No Solutions 2" << std::endl;
                return 1;
            }
            results_i_j[i].append(result);
        }
    }

    NTL::Vec<NTL::vec_GF2> b_i;
    b_i.SetLength(I);
    for (int i = 0; i < I; i++) {
        b_i[i].SetLength(JAIN_V);
        for (int j = 0; j < JAIN_V; j++) {
            b_i[i][j] = results_i[i][j + JAIN_L];
        }
    }

    NTL::Vec<NTL::Vec<NTL::vec_GF2>> b_i_j;
    b_i_j.SetLength(I);
    for (int i = 0; i < I; i++) {
        b_i_j[i].SetLength(J);
        for (int j = 0; j < J; j++) {
            b_i_j[i][j].SetLength(JAIN_V);
            for (int z = 0; z < JAIN_V; z++) {
                b_i_j[i][j][z] = results_i_j[i][j][z + JAIN_L];
            }
        }
    }

    NTL::Vec<NTL::vec_GF2> r_b;
    r_b.SetLength(I);
    for (int i = 0; i < I; i++) {
        for (int j = 0; j < J; j++) {
            r_b[i].SetLength(JAIN_V);
            r_b[i] += (R[j] * b_i_j[i][j]);
        }
        if (b_i[i] != r_b[i]) {
            std::cout << "Error: verify_0" << std::endl;
            return 1;
        }
    }

    return 0;
}

int hamming_metric::multiplicative_relations::verify_2(
        const NTL::Vec<NTL::vec_GF2> &c_i_1,
        const NTL::Vec<NTL::vec_GF2> &r_i_1,
        const NTL::Vec<NTL::vec_GF2> &t_i_1,
        const NTL::Vec<NTL::vec_GF2> &c_i_2,
        const NTL::Vec<NTL::vec_GF2> &r_i_2,
        const NTL::Vec<NTL::vec_GF2> &t_i_2,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &c_i_j_1,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &r_i_j_1,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &t_i_j_1,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &c_i_j_2,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &r_i_j_2,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &t_i_j_2,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &e_i_j,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &m_prime_i_j,
        const public_key_t *public_key) {

    for (int i = 0; i < I; i++) {
        auto _t1 = utils::encode_binary_vector(t_i_1[i], JAIN_V);
        auto _t2 = utils::encode_binary_vector(t_i_2[i], JAIN_V);

        if (hamming_metric::commitment::verify(
                c_i_1[i],
                public_key->A,
                r_i_1[i],
                _t1) != 0) {
            std::cout << "Linear Relations. Verification failed on ch = 2 and c1" << std::endl;
            return 1;
        }

        if (hamming_metric::commitment::verify(
                c_i_2[i],
                public_key->A,
                r_i_2[i],
                _t2) != 0) {
            std::cout << "Linear Relations. Verification failed on ch = 2 and c2" << std::endl;
            return 1;
        }
        for (int j = 0; j < J; j++) {
            _t1 = utils::encode_binary_vector(t_i_j_1[i][j], JAIN_V);
            _t2 = utils::encode_binary_vector(t_i_j_2[i][j], JAIN_V);

            if (hamming_metric::commitment::verify(
                    c_i_j_1[i][j],
                    public_key->A,
                    r_i_j_1[i][j],
                    _t1) != 0) {
                std::cout << "Linear Relations. Verification failed on ch = 2 and c1" << std::endl;
                return 1;
            }

            if (hamming_metric::commitment::verify(
                    c_i_j_2[i][j],
                    public_key->A,
                    r_i_j_2[i][j],
                    _t2) != 0) {
                std::cout << "Linear Relations. Verification failed on ch = 2 and c2" << std::endl;
                return 1;
            }

            if (NTL::weight(e_i_j[i][j]) != W) {
                std::cout << "Error: Wrong weight" << std::endl;
                return 1;
            }

        }
    }

    for (int j = 0; j < J; j++) {
        for (int z = 0; z < m_prime_i_j[0][j].length(); z++) {
            if (m_prime_i_j[0][j][z] * m_prime_i_j[1][j][z] !=
                m_prime_i_j[2][j][z]) {
                std::cout << "Error: Wrong relations" << std::endl;
                return 1;
            }
        }

    }

    return 0;
}


/*
 * Copyright 2009-2015 The VOTCA Development Team (http://www.votca.org)
 *
 * 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 <votca/tools/linalg.h>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include "mkl.h"
#include "mkl_lapacke.h"

namespace votca { namespace tools {

using namespace std;

/* Wrapper for DSYEV
 *  - make copy of input matrix
 */

bool linalg_eigenvalues( ub::matrix<double> &A, ub::vector<double> &E, ub::matrix<double> &V)
{
    // cout << " \n I'm really using MKL! " << endl;

    int n = A.size1();
    int lda = n ;
    // make sure that containers for eigenvalues and eigenvectors are of correct size
    E.resize(n);
    V.resize(n, n);
    // Query and allocate the optimal workspace
    double wkopt;
    double* work;
    int info;
    int lwork;
    lwork = -1;

    // MKL is different to GSL because it overwrites the input matrix
    V = A; // make a copy (might actually be unnecessary in most cases!)

    // make a pointer to the ublas matrix so that LAPACK understands it
    double * pV = const_cast<double*>(&V.data().begin()[0]);
    double * pE = const_cast<double*>(&E.data()[0]);

    // call LAPACK via C interface
    info = LAPACKE_dsyev( LAPACK_ROW_MAJOR, 'V', 'U', n, pV , lda, pE );

    if( info > 0 ) {
        return false;
    } else {
        return true;
    }
};

bool linalg_eigenvalues_symmetric( ub::symmetric_matrix<double> &A, ub::vector<double> &E, ub::matrix<double> &V)
{
    // cout << " \n I'm really using MKL! " << endl;

    int n = A.size1();
    int lda = n ;
    // make sure that containers for eigenvalues and eigenvectors are of correct size
    E.resize(n);
    V.resize(n, n);
    // Query and allocate the optimal workspace
    double wkopt;
    double* work;
    int info;
    int lwork;
    lwork = -1;

    // MKL does not handle conversion of a symmetric_matrix
    V = A;

    // make a pointer to the ublas matrix so that LAPACK understands it
    double * pV = const_cast<double*>(&V.data().begin()[0]);
    double * pE = const_cast<double*>(&E.data()[0]);

    // call LAPACK via C interface
    info = LAPACKE_dsyev( LAPACK_ROW_MAJOR, 'V', 'U', n, pV , lda, pE );

    if( info > 0 ) {
        return false;
    } else {
        return true;
    }
};

bool linalg_eigenvalues(  ub::vector<double> &E, ub::matrix<double> &V)
{
    // cout << " \n I'm really using MKL! " << endl;

    int n = V.size1();
    int lda = n ;
    // make sure that containers for eigenvalues and eigenvectors are of correct size
    E.resize(n);
    // V.resize(n, n);
    // Query and allocate the optimal workspace
    double wkopt;
    double* work;
    int info;
    int lwork;
    lwork = -1;

    // MKL is different to GSL because it overwrites the input matrix
    // V = A; // make a copy (might actually be unnecessary in most cases!)

    // make a pointer to the ublas matrix so that LAPACK understands it
    double * pV = const_cast<double*>(&V.data().begin()[0]);
    double * pE = const_cast<double*>(&E.data()[0]);

    // call LAPACK via C interface
    info = LAPACKE_dsyev( LAPACK_ROW_MAJOR, 'V', 'U', n, pV , lda, pE );

    if( info > 0 ) {
        return false;
    } else {
        return true;
    }
};

bool linalg_eigenvalues(  ub::vector<float> &E, ub::matrix<float> &V)
{
    // cout << " \n I'm really using MKL! " << endl;

    int n = V.size1();
    int lda = n ;
    // make sure that containers for eigenvalues and eigenvectors are of correct size
    E.resize(n);
    // V.resize(n, n);
    // Query and allocate the optimal workspace
    float wkopt;
    float* work;
    int info;
    int lwork;
    lwork = -1;

    // MKL is different to GSL because it overwrites the input matrix
    // V = A; // make a copy (might actually be unnecessary in most cases!)

    // make a pointer to the ublas matrix so that LAPACK understands it
    float * pV = const_cast<float*>(&V.data().begin()[0]);
    float * pE = const_cast<float*>(&E.data()[0]);

    // call LAPACK via C interface
    info = LAPACKE_ssyev( LAPACK_ROW_MAJOR, 'V', 'U', n, pV , lda, pE );

    if( info > 0 ) {
        return false;
    } else {
        return true;
    }
};

/*
 * use expert routine to calculate only a subrange of eigenvalues
 */
bool linalg_eigenvalues( ub::matrix<double> &A, ub::vector<double> &E, ub::matrix<double> &V , int nmax)
{
    /*
     * INPUT:  matrix A (N,N)
     * OUTPUT: matrix V (N,NMAX)
     *         vector E (NMAX)
     */
    double wkopt;
    double* work;
    double abstol, vl, vu;

    MKL_INT lda;
    MKL_INT info;
    MKL_INT lwork;
    MKL_INT il, iu, m, ldz ;

    int n = A.size1();
    MKL_INT ifail[n];
    lda = n;
    ldz = nmax;

    // make sure that containers for eigenvalues and eigenvectors are of correct size
    E.resize(nmax);
    V.resize(n,nmax);

    lwork = -1;
    il = 1;
    iu = nmax;
    abstol = 0.0; // use default
    vl = 0.0;
    vu = 0.0;
    // make a pointer to the ublas matrix so that LAPACK understands it
    double * pA = const_cast<double*>(&A.data().begin()[0]);
    double * pV = const_cast<double*>(&V.data().begin()[0]);
    double * pE = const_cast<double*>(&E.data()[0]);

    // call LAPACK via C interface
    info = LAPACKE_dsyevx( LAPACK_ROW_MAJOR, 'V', 'I', 'U', n, pA , lda, vl, vu, il, iu, abstol, &m, pE, pV, nmax,  ifail );

    if( info > 0 ) {
        return false;
    } else {
        return true;
    }
};

/*
 * use expert routine to calculate only a subrange of eigenvalues
 */
bool linalg_eigenvalues( ub::matrix<float> &A, ub::vector<float> &E, ub::matrix<float> &V , int nmax)
{
    /*
     * INPUT:  matrix A (N,N)
     * OUTPUT: matrix V (N,NMAX)
     *         vector E (NMAX)
     */
    float wkopt;
    float* work;
    float abstol, vl, vu;

    MKL_INT lda;
    MKL_INT info;
    MKL_INT lwork;
    MKL_INT il, iu, m, ldz ;

    int n = A.size1();
    MKL_INT ifail[n];
    lda = n;
    ldz = nmax;

    // make sure that containers for eigenvalues and eigenvectors are of correct size
    E.resize(nmax);
    V.resize(n,nmax);

    lwork = -1;
    il = 1;
    iu = nmax;
    abstol = 0.0; // use default
    vl = 0.0;
    vu = 0.0;
    // make a pointer to the ublas matrix so that LAPACK understands it
    float * pA = const_cast<float*>(&A.data().begin()[0]);
    float * pV = const_cast<float*>(&V.data().begin()[0]);
    float * pE = const_cast<float*>(&E.data()[0]);

    // call LAPACK via C interface
    info = LAPACKE_ssyevx( LAPACK_ROW_MAJOR, 'V', 'I', 'U', n, pA , lda, vl, vu, il, iu, abstol, &m, pE, pV, nmax,  ifail );

    if( info > 0 ) {
        return false;
    } else {
        return true;
    }
};

/* calculate the eigenvalues and vectors of the generalized eigenvalue problem */
bool linalg_eigenvalues_general( ub::matrix<double> &A,ub::matrix<double> &B, ub::vector<double> &E, ub::matrix<double> &V)
{
    // cout << " \n I'm really using MKL! " << endl;
    //check to see if matrices have same size
    int n = A.size1();
    int lda = n ;
    int ldb =B.size1();
    ub::matrix<double> _B(ldb,ldb);
    _B=B;
    if (lda!=ldb){
        cout << "Matrices A and B have not the same size"<< endl;
        exit(1);
    }
    // make sure that containers for eigenvalues and eigenvectors are of correct size
    E.resize(n);
    V.resize(n, n);
    // Query and allocate the optimal workspace
    double wkopt;
    double* work;
    int info;
    int lwork;
    lwork = -1;

    // MKL is different to GSL because it overwrites the input matrix
    V = A; // make a copy (might actually be unnecessary in most cases!)

    // make a pointer to the ublas matrix so that LAPACK understands it
    double * pB = const_cast<double*>(&_B.data().begin()[0]);
    double * pV = const_cast<double*>(&V.data().begin()[0]);
    double * pE = const_cast<double*>(&E.data()[0]);

    // call LAPACK via C interface
    info = LAPACKE_dsygv( LAPACK_ROW_MAJOR,1,'V', 'U', n, pV , lda,pB,ldb, pE );

    if( info > 0 ) {
        return false;
    } else {
        return true;
    }
};

}}


#pragma once

#include <cmath>
#include <boost/optional.hpp>
#include <boost/functional/hash.hpp>
#include <QtCore/QPointF>
#include <protobuf/Point.pb.h>
#include <sstream>
#include <string>

namespace Geometry2d {
/**
Simple class to represent a point in 2d space. Uses floating point coordinates
*/
class Point {
public:
    const double& x() const { return _x; }
    const double& y() const { return _y; }
    double& x() { return _x; }
    double& y() { return _y; }

    /**
    sets the point to x,y
    @param x the x coordinate
    @param y the y coordinate
    */
    Point(double x = 0, double y = 0) : _x(x), _y(y) {}

    /**
     * Implicit constructor for creating a Point from a Packet::Point
     */
    Point(const Packet::Point& other) : Point(other.x(), other.y()) {}

    /**
     * Implicit constructor for creating a Point from a QPointF
     */
    Point(const QPointF& other) : Point(other.x(), other.y()) {}

    /**
     * Implicit constructor for creating a Point from a QPoint
     */
    Point(const QPoint& other) : Point(other.x(), other.y()) {}

    /**
     * Implicit constructor for creating a Point from a double*
     */
    Point(const double* other) : Point(other[0], other[1]) {}

    /**
     * to draw stuff and interface with QT
     */
    QPointF toQPointF() const { return QPointF(x(), y()); }

    operator Packet::Point() const {
        Packet::Point out;
        out.set_x(x());
        out.set_y(y());
        return out;
    }
    /**
     * does vector addition
     * adds the + operator, shorthand
     */
    Point operator+(Point other) const {
        return Point(x() + other.x(), y() + other.y());
    }

    /**
     * see operator+
     * does vector division, note the operator
     */
    Point operator/(Point other) const {
        return Point(x() / other.x(), y() / other.y());
    }

    /**
     * @returns (x*x,y*y)
     */
    Point operator*(Point other) const {
        return Point(x() * other.x(), y() * other.y());
    }

    /**
     * see operator+
     * does vector subtraction, note the operator
     * without parameter, it is the negative
     */
    Point operator-(Point other) const {
        return Point(x() - other.x(), y() - other.y());
    }

    /**
     * multiplies the point by a -1 vector
     */
    Point operator-() const { return Point(-x(), -y()); }

    /**
     * see operator+
     * this modifies the value instead of returning a new value
     */
    Point& operator+=(Point other) {
        x() += other.x();
        y() += other.y();

        return *this;
    }

    /**
     * see operator-
     * this modifies the value instead of returning a new value
     */
    Point& operator-=(Point other) {
        x() -= other.x();
        y() -= other.y();

        return *this;
    }

    /**
     * see operator*
     * this modifies the value instead of returning a new value
     */
    Point& operator*=(double s) {
        x() *= s;
        y() *= s;

        return *this;
    }

    /**
     * see operator/
     * this modifies the value instead of returning a new value
     */
    Point& operator/=(double s) {
        x() /= s;
        y() /= s;

        return *this;
    }

    /**
     * adds the / operator for vectors
     *  scalar division
     */
    Point operator/(double s) const { return Point(x() / s, y() / s); }
    /**
     * adds the * operator for vectors
     * scalar multiplication
     */
    Point operator*(double s) const { return Point(x() * s, y() * s); }

    /**
     * compares two points to see if both x and y are the same
     * adds the == operator
     */
    bool operator==(Point other) const {
        return x() == other.x() && y() == other.y();
    }

    /**
     * this is the negation of operator operator !=
     */
    bool operator!=(Point other) const {
        return x() != other.x() || y() != other.y();
    }

    const double& operator[](int i) const {
        if (0 == i) {
            return _x;
        } else if (1 == i) {
            return _y;
        } else {
            throw std::out_of_range("Out of range index for Geometry2d::Point");
        }
    }

    double& operator[](int i) {
        return const_cast<double&>((static_cast<const Point*>(this))->operator[](i));
    }

    /**
     * Hash function for Geometry2d::Point
     */
    static size_t hash(Point pt) {
        size_t seed = 0;
        boost::hash_combine(seed, pt.x());
        boost::hash_combine(seed, pt.y());
        return seed;
    }

    /**
    computes the dot product of this point and another.
    behaves as if the points were 2d vectors
    @param p the second point
    @return the dot product of the two
    */
    double dot(Point p) const { return x() * p.x() + y() * p.y(); }

    /**
    computes the magnitude of the point, as if it were a vector
    @return the magnitude of the point
    */
    double mag() const { return sqrtf(x() * x() + y() * y()); }

    /**
    computes magnitude squared
    this is faster than mag()
    @return the magnitude squared
    */
    double magsq() const { return x() * x() + y() * y(); }

    /**
     * @brief Restricts the point to a given magnitude
     * @param max The magnitude to restrict the vector
     */
    Point& clamp(double max) {
        double ratio = mag() / max;
        if (ratio > 1) {
            x() /= ratio;
            y() /= ratio;
        }
        return *this;
    }

    /**
    rotates the point around another point by specified angle in the CCW
    direction
    @param origin the point to rotate around
    @param angle the angle in radians
    */
    Point& rotate(const Point& origin, double angle) {
        *this -= origin;
        rotate(angle);
        *this += origin;
        return *this;
    }

    /**
    * rotates the point around the origin
    */
    Point& rotate(double angle) {
        double newX = x() * cos(angle) - y() * sin(angle);
        double newY = y() * cos(angle) + x() * sin(angle);
        x() = newX;
        y() = newY;
        return *this;
    }

    /**
     * Like rotate(), but returns a new point instead of changing *this
     */
    Point rotated(double angle) const {
        double newX = x() * cos(angle) - y() * sin(angle);
        double newY = y() * cos(angle) + x() * sin(angle);
        return Point(newX, newY);
    }

    /**
     * Returns a new Point rotated around the origin
     */
    Point rotated(const Point& origin, double angle) const {
        return rotated(*this, origin, angle);
    }

    /**
    * static function to use rotate
    */
    static Point rotated(const Point& pt, const Point& origin, double angle) {
        Point newPt = pt;
        newPt.rotate(origin, angle);
        return newPt;
    }

    /**
    computes the distance from the current point to another
    @param other the point to find the distance to
    @return the distance between the points
    */
    double distTo(const Point& other) const {
        Point delta = other - *this;
        return delta.mag();
    }

    /**
    * Returns a vector with the same direction as this vector but with magnitude
    * given,
    * unless this vector is zero.
    * If the vector is (0,0), Point(0,0) is returned
    */
    Point normalized(double magnitude = 1.0) const {
        double m = mag();
        if (m == 0) {
            return Point(0, 0);
        }

        return Point(magnitude * x() / m, magnitude * y() / m);
    }

    /// Alias for normalized() - matches Eigen's syntax
    Point norm() const { return normalized(); }

    /**
    * Returns true if this point is within the given distance (threshold) of
    * (pt)
    */
    bool nearPoint(const Point& other, double threshold) const {
        return (*this - other).magsq() <= (threshold * threshold);
    }

    /**
    * Returns the angle of this point in radians CCW from +X.
    */
    double angle() const { return atan2(y(), x()); }

    /**
    * Returns a unit vector in the given direction (in radians)
    */
    static Point direction(double theta) {
        return Point(cos(theta), sin(theta));
    }

    /** returns the perpendicular to the point, Clockwise */
    Point perpCW() const { return Point(y(), -x()); }

    /** returns the perpendicular to the point, Counter Clockwise */
    Point perpCCW() const { return Point(-y(), x()); }

    /** saturates the magnitude of a vector */
    static Geometry2d::Point saturate(Geometry2d::Point value, double max) {
        double mag = value.mag();
        if (mag > fabs(max)) {
            return value.normalized() * fabs(max);
        }
        return value;
    }

    double angleTo(const Point& other) const { return (other - *this).angle(); }

    double cross(const Point& other) const {
        return x() * other.y() - y() * other.x();
    }

    /** returns the angle between the two normalized points (radians) */
    double angleBetween(const Point& other) const {
        return acos(normalized().dot(other.normalized()));
    }

    bool nearlyEquals(Point other) const;

    std::string toString() const {
        std::stringstream str;
        str << "Point(" << x() << ", " << y() << ")";
        return str.str();
    }

    friend std::ostream& operator<<(std::ostream& stream, const Point& point) {
        stream << point.toString();
        return stream;
    }

private:
    double _x, _y;
};  // \class Point

// global operations

/**
 * adds the * operator for vectors
 * scalar multiplication
 */
inline Point operator*(const double& s, const Point& pt) {
    return Point(pt.x() * s, pt.y() * s);
}
}