/********************************************************************************
 * Copyright 2017 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * 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.
 ********************************************************************************/

#ifndef RW_GEOMETRY_ANALYTIC_QUADRATICS_QUADRATICSURFACE_HPP_
#define RW_GEOMETRY_ANALYTIC_QUADRATICS_QUADRATICSURFACE_HPP_

/**
 * @file QuadraticSurface.hpp
 *
 * \copydoc rw::geometry::QuadraticSurface
 */
#if !defined(SWIG)
#include "QuadraticCurve.hpp"

#include <rw/geometry/PlainTriMesh.hpp>
#include <rw/geometry/analytic/ImplicitSurface.hpp>
#include <rw/math/Transform3D.hpp>
#include <rw/math/Vector3D.hpp>

#include <Eigen/Core>
#include <vector>
#endif
namespace rw { namespace geometry {
    class TriMesh;

    //! @addtogroup geometry
#if !defined(SWIG)
    //! @{
#endif
    /**
     * @brief A quadratic surface.
     *
     * The general quadratic surface is described as an implicit surface of the form:
     *
     * \f$ x^T A x + 2 a^T x + u = 0\f$
     *
     * where
     *
     * A is a symmetric matrix, \f$ A \in \mathbb{R}^{3\times3} \f$ , and \f$ a \in \mathbb{R}^3, u
     * \in \mathbb{R}\f$
     */

    #if !defined(SWIGJAVA)
    class QuadraticSurface : public ImplicitSurface
    #else
    class QuadraticSurface
    #endif
    {
      public:
        //! @brief Smart pointer type for QuadraticSurface
        typedef rw::core::Ptr< QuadraticSurface > Ptr;

        //! @brief Smart pointer type for const QuadraticSurface
        typedef rw::core::Ptr< const QuadraticSurface > CPtr;

        /**
         * @brief A trimming region is defined using an ImplicitSurface.
         *
         * A point is only considered part of this surface, if all trimming conditions evaluate to a
         * negative value.
         */
        typedef rw::geometry::ImplicitSurface::CPtr TrimmingRegion;

        /**
         * @brief Construct new quadratic surface of the implicit form \f$ x^T A x + 2 a^T x + u =
         * 0\f$ when A is diagonal.
         *
         * Some functions, such as #getTriMesh and #extremums, work on a diagonalized surface.
         * When this constructor is used, some effort is saved as the surface is already known to be
         * diagonalized.
         *
         * For a diagonalized surface, all scaled, cloned and translated surfaces will also be
         * diagonalized surfaces.
         *
         * @param A [in] the diagonal of the A matrix.
         * @param a [in] the vector \f$ a \in \mathbb{R}^3\f$ .
         * @param u [in] the scalar offset \f$ u \in \mathbb{R} \f$ .
         * @param conditions [in] (optional) list of trimming conditions.
         */
        QuadraticSurface (
            const Eigen::Diagonal< Eigen::Matrix3d >& A, const Eigen::Vector3d& a, double u,
            const std::vector< TrimmingRegion >& conditions = std::vector< TrimmingRegion > ());

        //! @copydoc QuadraticSurface(const Eigen::Diagonal<Eigen::Matrix3d>&, const
        //! Eigen::Vector3d&, double, const std::vector<TrimmingRegion>&)
        QuadraticSurface (
            const Eigen::DiagonalMatrix< double, 3, 3 >& A, const Eigen::Vector3d& a, double u,
            const std::vector< TrimmingRegion >& conditions = std::vector< TrimmingRegion > ());

        /**
         * @brief Construct new quadratic surface of the implicit form \f$ x^T A x + 2 a^T x + u =
         * 0\f$ when A is non-diagonal.
         * @param A [in] a view of the upper part of the symmetric matrix \f$ A \in
         * \mathbb{R}^{3\times3} \f$ . Use the Eigen function A.selfadjointView<Eigen::Upper>() to
         * extract the upper part.
         * @param a [in] the vector \f$ a \in \mathbb{R}^3\f$ .
         * @param u [in] the scalar offset \f$ u \in \mathbb{R} \f$ .
         * @param conditions [in] (optional) list of trimming conditions.
         */
        QuadraticSurface (
            const Eigen::SelfAdjointView< const Eigen::Matrix3d, Eigen::Upper >& A,
            const Eigen::Vector3d& a, double u,
            const std::vector< TrimmingRegion >& conditions = std::vector< TrimmingRegion > ());

        //! @copydoc QuadraticSurface(const Eigen::SelfAdjointView<const Eigen::Matrix3d,
        //! Eigen::Upper>&, const Eigen::Vector3d&, double, const std::vector<TrimmingRegion>&)
        QuadraticSurface (
            const Eigen::SelfAdjointView< Eigen::Matrix3d, Eigen::Upper >& A,
            const Eigen::Vector3d& a, double u,
            const std::vector< TrimmingRegion >& conditions = std::vector< TrimmingRegion > ());

        /**
         * @brief Construct new quadratic surface of the implicit form \f$ x^T A x + 2 a^T x + u =
         * 0\f$ when A is non-diagonal.
         * @param A [in] a view of the lower part of the symmetric matrix \f$ A \in
         * \mathbb{R}^{3\times3} \f$ . Use the Eigen function A.selfadjointView<Eigen::Lower>() to
         * extract the lower part.
         * @param a [in] the vector \f$ a \in \mathbb{R}^3\f$ .
         * @param u [in] the scalar offset \f$ u \in \mathbb{R} \f$ .
         * @param conditions [in] (optional) list of trimming conditions.
         */
        QuadraticSurface (
            const Eigen::SelfAdjointView< const Eigen::Matrix3d, Eigen::Lower >& A,
            const Eigen::Vector3d& a, double u,
            const std::vector< TrimmingRegion >& conditions = std::vector< TrimmingRegion > ());

        //! @copydoc QuadraticSurface(const Eigen::SelfAdjointView<const Eigen::Matrix3d,
        //! Eigen::Lower>&, const Eigen::Vector3d&, double, const std::vector<TrimmingRegion>&)
        QuadraticSurface (
            const Eigen::SelfAdjointView< Eigen::Matrix3d, Eigen::Lower >& A,
            const Eigen::Vector3d& a, double u,
            const std::vector< TrimmingRegion >& conditions = std::vector< TrimmingRegion > ());

        //! @brief Destructor.
        virtual ~QuadraticSurface ();

        // From ImplicitSurface
        //! @copydoc ImplicitSurface::transform(const rw::math::Transform3D<>&) const
        QuadraticSurface::Ptr transform (const rw::math::Transform3D<>& T) const;

        //! @copydoc ImplicitSurface::transform(const rw::math::Vector3D<double>&) const
        QuadraticSurface::Ptr transform (const rw::math::Vector3D<double>& P) const;

        //! @copydoc ImplicitSurface::scale
        QuadraticSurface::Ptr scale (double factor) const;

        //! @copydoc ImplicitSurface::clone
        QuadraticSurface::Ptr clone () const;

        //! @copydoc ImplicitSurface::extremums
        virtual std::pair< double, double > extremums (const rw::math::Vector3D<double>& direction) const;

        //! @copydoc ImplicitSurface::getTriMesh
        virtual rw::core::Ptr< TriMesh >
        getTriMesh (const std::vector< rw::math::Vector3D<double> >& border =
                        std::vector< rw::math::Vector3D<double> > ()) const;

        //! @copydoc ImplicitSurface::setDiscretizationResolution
        virtual void setDiscretizationResolution (double resolution)
        {
            _stepsPerRevolution = resolution;
        }

        //! @copydoc ImplicitSurface::equals
        virtual bool equals (const Surface& surface, double threshold) const;

#if !defined(SWIG)
        //! @copydoc ImplicitSurface::operator()(const rw::math::Vector3D<double>&) const
        virtual double operator() (const rw::math::Vector3D<double>& x) const;
#else
        CALLOPERATOR(double,const rw::math::Vector3D<double>& );
#endif

        //! @copydoc ImplicitSurface::insideTrimmingRegion
        virtual bool insideTrimmingRegion (const rw::math::Vector3D<double>& P) const;

        //! @copydoc ImplicitSurface::normal
        virtual rw::math::Vector3D<double> normal (const rw::math::Vector3D<double>& x) const;

        //! @copydoc ImplicitSurface::gradient
        virtual rw::math::Vector3D<double> gradient (const rw::math::Vector3D<double>& x) const;

        //! @copydoc ImplicitSurface::reuseTrimmingRegions
        virtual void reuseTrimmingRegions (rw::geometry::ImplicitSurface::Ptr surface) const;

        //! @brief Get the 3 x 3 symmetric matrix for the second order term in the implicit
        //! formulation.
        const Eigen::Matrix3d& A () const { return _A; }

        //! @brief Get the 3d vector for the first order term in the implicit formulation.
        const Eigen::Vector3d& a () const { return _a; }

        //! @brief Get the scalar for the zero order term in the implicit formulation.
        double u () const { return _u; }

        /**
         * @brief Get the determinant of the \f$ \mathbf{A} \f$ matrix.
         * @return the determinant.
         */
        double determinantA () const { return _determinantA; }

        /**
         * @brief Normalize the implicit expression such that the largest coefficient becomes one.
         *
         * For a quadratic surface, a scaling of \f$ \mathbf{A}, \mathbf{a} \f$ and u with a common
         * factor, will give the exact same surface. This means that the numerical values can get
         * arbitrarily big or small. This functions scales the expression such that the largest
         * element becomes 1.
         *
         * @return a mathematically identical surface, where the coefficients of the defining
         * equation is normalized.
         */
        QuadraticSurface::Ptr normalize () const;

        /**
         * @brief Get the trimming conditions for the surface.
         * @return ImplicitSurface vector specifying the boundary of the surface. If surface is
         * unbounded, the length of the vector is zero.
         */
        const std::vector< TrimmingRegion >& getTrimmingConditions () const { return _conditions; }

        /**
         * @brief Set the trimming conditions of this surface.
         * @param conditions [in] a vector of conditions.
         */
        void setTrimmingConditions (const std::vector< TrimmingRegion >& conditions)
        {
            _conditions = conditions;
        }

        /**
         * @brief Add a trimming condition to this surface.
         * @param condition [in] the condition to add.
         */
        void addTrimmingCondition (const TrimmingRegion& condition)
        {
            _conditions.push_back (condition);
        }

        /**
         * @brief Get a diagonalization of the surface.
         * @return the diagonalized surface, and the rotation transforming this surface into the
         * diagonalized surface.
         */
        std::pair< QuadraticSurface, rw::math::Rotation3D<> > diagonalize () const;

        /**
         * @brief Check if this surface is diagonalized.
         * @return true if A is digaonalized, false otherwise.
         */
        bool diagonalized () const { return _diagonal; }

/** @name Normal forms of Quadratic Surfaces
 * Functions for creation of standard Quadratic Surfaces.
 */
#if !defined(SWIG)
///@{
#endif

#if !defined(SWIGJAVA)
        /**
         * @brief Create an ellipsoid with radii \b a, \b b, and \b c respectively.
         *
         * \image html geometry/quadrics_ellipsoid.gif "Normal form of Quadratic Surface:
         * Ellipsoid."
         *
         * @param a [in] radius in the \f$ x_1 \f$ direction.
         * @param b [in] radius in the \f$ x_2 \f$ direction.
         * @param c [in] radius in the \f$ x_3 \f$ direction.
         * @return a QuadraticSurface representation of an ellipsoid.
         */

         #endif
        static QuadraticSurface::Ptr makeEllipsoid (double a, double b, double c);

#if !defined(SWIGJAVA)
        /**
         * @brief Create a spheroid (special case of the ellipsoid).
         *
         * \image html geometry/quadrics_spheroid.gif "Normal form of Quadratic Surface: Spheroid
         * (special case of ellipsoid)."
         *
         * @param a [in] radius in the \f$ x_1 \f$ and \f$ x_2 \f$ directions.
         * @param b [in] radius in the \f$ x_3 \f$ direction.
         * @return a QuadraticSurface representation of a spheroid.
         */

         #endif
        static QuadraticSurface::Ptr makeSpheroid (double a, double b);

#if !defined(SWIGJAVA)
        /**
         * @brief Create a sphere (special case of the ellipsoid and spheroid).
         *
         * \image html geometry/quadrics_sphere.gif "Normal form of Quadratic Surface: Sphere
         * (special case of ellipsoid and spheroid)."
         *
         * @param radius [in] radius of the sphere.
         * @return a QuadraticSurface representation of a sphere.
         */

         #endif
        static QuadraticSurface::Ptr makeSphere (double radius);

#if !defined(SWIGJAVA)
        /**
         * @brief Create an elliptic paraboloid.
         *
         * \image html geometry/quadrics_elliptic_paraboloid.gif "Normal form of Quadratic Surface:
         * Elliptic Paraboloid."
         *
         * @param a [in] radius of the ellipse in the \f$ x_1\f$ direction when \f$ x_3=1\f$ .
         * @param b [in] radius of the ellipse in the \f$ x_2\f$ direction when \f$ x_3=1\f$ .
         * @return a QuadraticSurface representation of an elliptic paraboloid.
         */

         #endif
        static QuadraticSurface::Ptr makeEllipticParaboloid (double a, double b);

#if !defined(SWIGJAVA)
        /**
         * @brief Create a circular paraboloid (special case of elliptic paraboloid).
         *
         * \image html geometry/quadrics_circular_paraboloid.gif "Normal form of Quadratic Surface:
         * Circular Paraboloid (special case of elliptic paraboloid)."
         *
         * @param a [in] radius of the circle when \f$ x_3=1\f$ .
         * @return a QuadraticSurface representation of a circular paraboloid.
         */

         #endif
        static QuadraticSurface::Ptr makeCircularParaboloid (double a);

#if !defined(SWIGJAVA)
        /**
         * @brief Create a hyperbolic paraboloid.
         *
         * \image html geometry/quadrics_hyperbolic_paraboloid.gif "Normal form of Quadratic
         * Surface: Hyperbolic Paraboloid."
         *
         * @param a [in] width in the \f$ x_1\f$ direction when \f$ x_3=1\f$ .
         * @param b [in] width in the \f$ x_2\f$ direction when \f$ x_3=-1\f$ .
         * @return a QuadraticSurface representation of a hyperbolic paraboloid.
         */

         #endif
        static QuadraticSurface::Ptr makeHyperbolicParaboloid (double a, double b);

#if !defined(SWIGJAVA)
        /**
         * @brief Create an elliptic hyperboloid of one sheet.
         *
         * \image html geometry/quadrics_elliptic_hyperboloid_onesheet.gif "Normal form of Quadratic
         * Surface: Elliptic Hyperboloid of One Sheet."
         *
         * @param a [in] radius of the ellipse in the \f$ x_1\f$ direction when \f$ x_3=0\f$ .
         * @param b [in] radius of the ellipse in the \f$ x_2\f$ direction when \f$ x_3=0\f$ .
         * @param c [in] radius is scaled with the factor \f$ \frac{1}{c}\sqrt{x_3^2+c^2}\f$ .
         * @return a QuadraticSurface representation of an elliptic hyperboloid of one sheet.
         */

         #endif
        static QuadraticSurface::Ptr makeEllipticHyperboloidOneSheet (double a, double b, double c);

#if !defined(SWIGJAVA)
        /**
         * @brief Create a circular hyperboloid of one sheet (special case of the elliptic
         * hyperboloid of one sheet).
         *
         * \image html geometry/quadrics_circular_hyperboloid_onesheet.gif "Normal form of Quadratic
         * Surface: Circular Hyperboloid of One Sheet (special case of elliptic hyperboloid of one
         * sheet)."
         *
         * @param a [in] radius of the circle in the \f$ x_1\f$ and \f$ x_2\f$ directions when
         * \f$ x_3=0\f$ .
         * @param b [in] radius is scaled along \f$ x_3\f$ , to \f$ \frac{a}{c}\sqrt{x_3^2+c^2}\f$ .
         * @return a QuadraticSurface representation of a circular hyperboloid of one sheet.
         */

         #endif
        static QuadraticSurface::Ptr makeCircularHyperboloidOneSheet (double a, double b);

#if !defined(SWIGJAVA)
        /**
         * @brief Create an elliptic hyperboloid of two sheets.
         *
         * \image html geometry/quadrics_elliptic_hyperboloid_twosheets.gif "Normal form of
         * Quadratic Surface: Elliptic Hyperboloid of Two Sheets."
         *
         * @param a [in] radius of the ellipse in the \f$ x_1\f$ direction when
         * \f$ x_3=\pm\sqrt{2}c\f$ .
         * @param b [in] radius of the ellipse in the \f$ x_2\f$ direction when
         * \f$ x_3=\pm\sqrt{2}c\f$ .
         * @param c [in] distance from origo to each of the the two sheets.
         * @return a QuadraticSurface representation of an elliptic hyperboloid of two sheets.
         */

         #endif
        static QuadraticSurface::Ptr makeEllipticHyperboloidTwoSheets (double a, double b,
                                                                       double c);

#if !defined(SWIGJAVA)
        /**
         * @brief Create a circular hyperboloid of two sheets (special case of the elliptic
         * hyperboloid of two sheets).
         *
         * \image html geometry/quadrics_circular_hyperboloid_twosheets.gif "Normal form of
         * Quadratic Surface: Circular Hyperboloid of Two Sheets (special case of elliptic
         * hyperboloid of two sheets)."
         *
         * @param a [in] radius of the circle in the \f$ x_1\f$ and \f$ x_2\f$ directions when
         * \f$ x_3=\pm\sqrt{2}b\f$ .
         * @param b [in] distance from origo to each of the the two sheets.
         * @return a QuadraticSurface representation of a circular hyperboloid of two sheets.
         */

         #endif
        static QuadraticSurface::Ptr makeCircularHyperboloidTwoSheets (double a, double b);

#if !defined(SWIGJAVA)
        /**
         * @brief Create an elliptic cone.
         *
         * The cone is a singular (or degenerate) surface as it has a point where the gradient
         * vanishes. In the origo the gradient will always be zero, and no normal can be determined.
         *
         * \image html geometry/quadrics_elliptic_cone.gif "Normal form of Quadratic Surface:
         * Elliptic Cone."
         *
         * @param a [in] radius of the ellipse in the \f$ x_1\f$ direction when \f$ x_3=c\f$ .
         * @param b [in] radius of the ellipse in the \f$ x_2\f$ direction when \f$ x_3=c\f$ .
         * @param c [in] rate of change for the radius.
         * @return a QuadraticSurface representation of an elliptic cone.
         */

         #endif
        static QuadraticSurface::Ptr makeEllipticCone (double a, double b, double c);

#if !defined(SWIGJAVA)
        /**
         * @brief Create a circular cone (special case of the elliptic cone).
         *
         * \image html geometry/quadrics_circular_cone.gif "Normal form of Quadratic Surface:
         * Circular Cone (special case of elliptic cone)."
         *
         * @param a [in] radius of the circle in the \f$ x_1\f$ and \f$ x_2\f$ directions when
         * \f$ x_3=c\f$ .
         * @param b [in] rate of change for the radius.
         * @return a QuadraticSurface representation of a circular cone.
         */

         #endif
        static QuadraticSurface::Ptr makeCircularCone (double a, double b);

#if !defined(SWIGJAVA)
        /**
         * @brief Create an elliptic cylinder.
         *
         * \image html geometry/quadrics_elliptic_cylinder.gif "Normal form of Quadratic Surface:
         * Elliptic Cylinder."
         *
         * @param a [in] radius in the \f$ x_1\f$ direction.
         * @param b [in] radius in the \f$ x_2\f$ direction.
         * @return a QuadraticSurface representation of an elliptic cylinder.
         */

         #endif
        static QuadraticSurface::Ptr makeEllipticCylinder (double a, double b);

#if !defined(SWIGJAVA)
        /**
         * @brief Create a circular cylinder (special case of the elliptic cylinder).
         *
         * \image html geometry/quadrics_circular_cylinder.gif "Normal form of Quadratic Surface:
         * Circular Cylinder (special case of elliptic cylinder)."
         *
         * @param radius [in] radius in the \f$ x_1\f$ and \f$ x_2\f$ directions.
         * @param outward [in] (optional) set to false to create inner surface of cylinder, with
         * normals pointing inwards.
         * @return a QuadraticSurface representation of a circular cylinder.
         */

         #endif
        static QuadraticSurface::Ptr makeCircularCylinder (double radius, bool outward = true);

#if !defined(SWIGJAVA)
        /**
         * @brief Create a hyperbolic cylinder.
         *
         * \image html geometry/quadrics_hyperbolic_cylinder.gif "Normal form of Quadratic Surface:
         * Hyperbolic Cylinder."
         *
         * @param a [in] width in the \f$ x_1\f$ direction at \f$ x_2=0\f$ .
         * @param b [in] controls the rate of change in the \f$ x_1\f$ direction.
         * @return a QuadraticSurface representation of a hyperbolic cylinder.
         */

         #endif
        static QuadraticSurface::Ptr makeHyperbolicCylinder (double a, double b);

#if !defined(SWIGJAVA)
        /**
         * @brief Create a parabolic cylinder.
         *
         * \image html geometry/quadrics_parabolic_cylinder.gif "Normal form of Quadratic Surface:
         * Parabolic Cylinder."
         *
         * @param a [in] controls the focal length of the parabola.
         * @return a QuadraticSurface representation of a parabolic cylinder.
         */

         #endif
        static QuadraticSurface::Ptr makeParabolicCylinder (double a);
#if !defined(SWIG)
///@}
#endif

        /**
         * @brief Represent a plane as a QuadraticSurface.
         *
         * A plane is a particularly simple type of quadratic surface, where
         * \f$ \mathbf{A}=\mathbf{0}\f$ .
         *
         * Even though a plane is not strictly a quadratic surface, is is often convenient to be
         * able to treat it like a quadratic surface.
         *
         * @param n [in] the normal of the plane.
         * @param d [in] the distance from the plane to the origo.
         * @return a QuadraticSurface representing a plane.
         */
        static QuadraticSurface::Ptr makePlane (const rw::math::Vector3D<double>& n, double d);

      private:
        QuadraticSurface (const Eigen::Matrix3d& A, bool diagonal, double determinantA,
                          const Eigen::Vector3d& a, double u,
                          const std::vector< TrimmingRegion >& conditions,
                          double stepsPerRevolution);
        #if !defined(SWIGJAVA)
        // From ImplicitSurface
        inline rw::geometry::ImplicitSurface::Ptr
        doTransformImplicitSurface (const rw::math::Transform3D<>& T) const
        {
            return transform (T);
        }
        inline rw::geometry::ImplicitSurface::Ptr doTransformImplicitSurface (const rw::math::Vector3D<double>& P) const
        {
            return transform (P);
        }
        inline rw::geometry::ImplicitSurface::Ptr doScaleImplicitSurface (double factor) const
        {
            return scale (factor);
        }
        inline rw::geometry::ImplicitSurface::Ptr doCloneImplicitSurface () const { return clone (); }
        #endif
        rw::core::Ptr< TriMesh > getTriMeshDiagonal (
            const std::vector< rw::math::Vector3D<double> >& border,
            const rw::math::Rotation3D<>& R = rw::math::Rotation3D<>::identity ()) const;
        std::pair< double, double > extremumsDiagonal (const rw::math::Vector3D<double>& dir) const;
        std::vector< rw::geometry::QuadraticCurve > findSilhouette (std::size_t u, std::size_t v, std::size_t e,
                                                      double eSplit) const;
        typedef enum Place { FRONT, BACK, BOTH } Place;
        void makeSurface (const std::vector< rw::math::Vector3D<double> > fullPolygon, std::size_t u,
                          std::size_t v, std::size_t e, double eSplit, Place place,
                          const rw::math::Rotation3D<>& R,
                          rw::geometry::PlainTriMeshN1D::Ptr mesh) const;

        const Eigen::Matrix3d _A;
        const Eigen::Vector3d _a;
        const double _u;
        const double _determinantA;
        const bool _diagonal;
        std::vector< TrimmingRegion > _conditions;

        double _stepsPerRevolution;
    };
#if !defined(SWIG)
//! @}
#endif
}}    // namespace rw::geometry

#endif /* RW_GEOMETRY_ANALYTIC_QUADRATICS_QUADRATICSURFACE_HPP_ */


/// Copyright (c) 2018-2021, Parker Owan.  All rights reserved.
/// Licensed under BSD-3 Clause, https://opensource.org/licenses/BSD-3-Clause

#include "sia/math/math.h"

#include <glog/logging.h>
#include <Eigen/SVD>

namespace sia {

const Eigen::VectorXd slice(const Eigen::VectorXd& x,
                            const std::vector<std::size_t>& indices) {
  Eigen::VectorXd y = Eigen::VectorXd::Zero(indices.size());
  for (std::size_t i = 0; i < indices.size(); ++i) {
    y(i) = x(indices.at(i));
  }
  return y;
}

const Eigen::MatrixXd slice(const Eigen::MatrixXd& X,
                            const std::vector<std::size_t>& rows,
                            const std::vector<std::size_t>& cols) {
  Eigen::MatrixXd Y = Eigen::MatrixXd::Zero(rows.size(), cols.size());
  for (std::size_t i = 0; i < rows.size(); ++i) {
    for (std::size_t j = 0; j < cols.size(); ++j) {
      Y(i, j) = X(rows.at(i), cols.at(j));
    }
  }
  return Y;
}

bool llt(const Eigen::MatrixXd& A, Eigen::MatrixXd& L) {
  Eigen::LLT<Eigen::MatrixXd> llt(A);
  L = llt.matrixL();
  if (llt.info() != Eigen::ComputationInfo::Success) {
    LOG(WARNING) << "LLT decomposition of matrix A = " << A << " failed with "
                 << llt.info();
    return false;
  }
  return true;
}

bool ldltSqrt(const Eigen::MatrixXd& A, Eigen::MatrixXd& M) {
  Eigen::LDLT<Eigen::MatrixXd> ldlt(A);
  const Eigen::MatrixXd I = Eigen::MatrixXd::Identity(A.rows(), A.cols());
  const Eigen::MatrixXd P = ldlt.transpositionsP() * I;
  const Eigen::MatrixXd L = ldlt.matrixL();
  const Eigen::VectorXd D = ldlt.vectorD();
  M = P.transpose() * L * D.array().sqrt().matrix().asDiagonal();
  if (ldlt.info() != Eigen::ComputationInfo::Success) {
    LOG(WARNING) << "LDLT decomposition of matrix A = " << A << " failed with "
                 << ldlt.info();
    return false;
  }
  return true;
}

bool svd(const Eigen::MatrixXd& A,
         Eigen::MatrixXd& U,
         Eigen::VectorXd& S,
         Eigen::MatrixXd& V,
         double tolerance) {
  bool result = true;

  // Compute the SVD of A
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(
      A, Eigen::ComputeThinU | Eigen::ComputeThinV);
  const Eigen::VectorXd& singular_values = svd.singularValues();

  // Check for singular condition
  for (int i = 0; i < singular_values.size(); ++i) {
    if (singular_values(i) <= tolerance) {
      LOG(WARNING) << "Singular value is less than tolerance";
      result = false;
    }
  }

  // Return SVD matrices
  U = svd.matrixU();
  S = singular_values.array();
  V = svd.matrixV();
  return result;
}

const Eigen::MatrixXd svdInverse(const Eigen::MatrixXd& U,
                                 const Eigen::VectorXd& S,
                                 const Eigen::MatrixXd& V) {
  return V * S.array().inverse().matrix().asDiagonal() * U.transpose();
}

bool svdInverse(const Eigen::MatrixXd& A,
                Eigen::MatrixXd& Ainv,
                double tolerance) {
  Eigen::MatrixXd U, V;
  Eigen::VectorXd S;
  bool result = svd(A, U, S, V, tolerance);

  // Compute the generalized inverse using SVD
  Ainv = svdInverse(U, S, V);
  return result;
}

const Eigen::MatrixXd lltSqrt(const Eigen::MatrixXd& A) {
  Eigen::LLT<Eigen::MatrixXd> llt(A);
  return llt.matrixL();
}

const Eigen::VectorXd rk4(
    std::function<Eigen::VectorXd(const Eigen::VectorXd&,
                                  const Eigen::VectorXd&)> dynamical_system,
    const Eigen::VectorXd& x,
    const Eigen::VectorXd& u,
    double dt) {
  Eigen::VectorXd f1 = dynamical_system(x, u);
  Eigen::VectorXd f2 = dynamical_system(x + dt * f1 / 2, u);
  Eigen::VectorXd f3 = dynamical_system(x + dt * f2 / 2, u);
  Eigen::VectorXd f4 = dynamical_system(x + dt * f3, u);
  return x + dt / 6 * (f1 + 2 * f2 + 2 * f3 + f4);
}

const Eigen::VectorXd dfdx(std::function<double(const Eigen::VectorXd&)> f,
                           const Eigen::VectorXd& x) {
  std::size_t n = x.size();
  Eigen::VectorXd df = Eigen::VectorXd::Zero(n);
  for (std::size_t i = 0; i < n; ++i) {
    Eigen::VectorXd dx = Eigen::VectorXd::Zero(n);
    dx(i) = NUMERICAL_DERIVATIVE_STEP;
    double fp = f(x + dx);
    double fn = f(x - dx);
    df(i) = (fp - fn) / 2 / NUMERICAL_DERIVATIVE_STEP;
  }
  return df;
}

const Eigen::VectorXd dfdx(
    std::function<double(const Eigen::VectorXd&, const Eigen::VectorXd&)> f,
    const Eigen::VectorXd& x,
    const Eigen::VectorXd& u) {
  std::size_t n = x.size();
  Eigen::VectorXd df = Eigen::VectorXd::Zero(n);
  for (std::size_t i = 0; i < n; ++i) {
    Eigen::VectorXd dx = Eigen::VectorXd::Zero(n);
    dx(i) = NUMERICAL_DERIVATIVE_STEP;
    double fp = f(x + dx, u);
    double fn = f(x - dx, u);
    df(i) = (fp - fn) / 2 / NUMERICAL_DERIVATIVE_STEP;
  }
  return df;
}

const Eigen::MatrixXd dfdx(
    std::function<Eigen::VectorXd(const Eigen::VectorXd&,
                                  const Eigen::VectorXd&)> f,
    const Eigen::VectorXd& x,
    const Eigen::VectorXd& u) {
  std::size_t n = x.size();
  Eigen::MatrixXd Df;
  for (std::size_t i = 0; i < n; ++i) {
    Eigen::VectorXd dx = Eigen::VectorXd::Zero(n);
    dx(i) = NUMERICAL_DERIVATIVE_STEP;
    Eigen::VectorXd fp = f(x + dx, u);
    Eigen::VectorXd fn = f(x - dx, u);
    if (i == 0) {
      Df = Eigen::MatrixXd::Zero(fp.size(), n);
    }
    Df.col(i) = (fp - fn) / 2 / NUMERICAL_DERIVATIVE_STEP;
  }
  return Df;
}

const Eigen::VectorXd dfdu(
    std::function<double(const Eigen::VectorXd&, const Eigen::VectorXd&)> f,
    const Eigen::VectorXd& x,
    const Eigen::VectorXd& u) {
  std::size_t n = u.size();
  Eigen::VectorXd df = Eigen::VectorXd::Zero(n);
  for (std::size_t i = 0; i < n; ++i) {
    Eigen::VectorXd du = Eigen::VectorXd::Zero(n);
    du(i) = NUMERICAL_DERIVATIVE_STEP;
    double fp = f(x, u + du);
    double fn = f(x, u - du);
    df(i) = (fp - fn) / 2 / NUMERICAL_DERIVATIVE_STEP;
  }
  return df;
}

const Eigen::MatrixXd dfdu(
    std::function<Eigen::VectorXd(const Eigen::VectorXd&,
                                  const Eigen::VectorXd&)> f,
    const Eigen::VectorXd& x,
    const Eigen::VectorXd& u) {
  std::size_t n = u.size();
  Eigen::MatrixXd Df;
  for (std::size_t i = 0; i < n; ++i) {
    Eigen::VectorXd du = Eigen::VectorXd::Zero(n);
    du(i) = NUMERICAL_DERIVATIVE_STEP;
    Eigen::VectorXd fp = f(x, u + du);
    Eigen::VectorXd fn = f(x, u - du);
    if (i == 0) {
      Df = Eigen::MatrixXd::Zero(fp.size(), n);
    }
    Df.col(i) = (fp - fn) / 2 / NUMERICAL_DERIVATIVE_STEP;
  }
  return Df;
}

const Eigen::MatrixXd d2fdxx(std::function<double(const Eigen::VectorXd&)> f,
                             const Eigen::VectorXd& x) {
  std::size_t n = x.size();
  Eigen::MatrixXd H = Eigen::MatrixXd::Zero(n, n);
  for (std::size_t i = 0; i < n; ++i) {
    Eigen::VectorXd dx = Eigen::VectorXd::Zero(n);
    dx(i) = NUMERICAL_DERIVATIVE_STEP;
    Eigen::VectorXd fp = dfdx(f, x + dx);
    Eigen::VectorXd fn = dfdx(f, x - dx);
    H.col(i) = (fp - fn) / 2 / NUMERICAL_DERIVATIVE_STEP;
  }
  // Ensure that the Hessian is necessarily symmetric
  return (H + H.transpose()) / 2.0;
}

const Eigen::MatrixXd d2fdxx(
    std::function<double(const Eigen::VectorXd&, const Eigen::VectorXd&)> f,
    const Eigen::VectorXd& x,
    const Eigen::VectorXd& u) {
  std::size_t n = x.size();
  Eigen::MatrixXd H = Eigen::MatrixXd::Zero(n, n);
  for (std::size_t i = 0; i < n; ++i) {
    Eigen::VectorXd dx = Eigen::VectorXd::Zero(n);
    dx(i) = NUMERICAL_DERIVATIVE_STEP;
    Eigen::VectorXd fp = dfdx(f, x + dx, u);
    Eigen::VectorXd fn = dfdx(f, x - dx, u);
    H.col(i) = (fp - fn) / 2 / NUMERICAL_DERIVATIVE_STEP;
  }
  // Ensure that the Hessian is necessarily symmetric
  return (H + H.transpose()) / 2.0;
}

const Eigen::MatrixXd d2fduu(
    std::function<double(const Eigen::VectorXd&, const Eigen::VectorXd&)> f,
    const Eigen::VectorXd& x,
    const Eigen::VectorXd& u) {
  std::size_t n = u.size();
  Eigen::MatrixXd H = Eigen::MatrixXd::Zero(n, n);
  for (std::size_t i = 0; i < n; ++i) {
    Eigen::VectorXd du = Eigen::VectorXd::Zero(n);
    du(i) = NUMERICAL_DERIVATIVE_STEP;
    Eigen::VectorXd fp = dfdu(f, x, u + du);
    Eigen::VectorXd fn = dfdu(f, x, u - du);
    H.col(i) = (fp - fn) / 2 / NUMERICAL_DERIVATIVE_STEP;
  }
  // Ensure that the Hessian is necessarily symmetric
  return (H + H.transpose()) / 2.0;
}

const Eigen::MatrixXd d2fdux(
    std::function<double(const Eigen::VectorXd&, const Eigen::VectorXd&)> f,
    const Eigen::VectorXd& x,
    const Eigen::VectorXd& u) {
  std::size_t m = x.size();
  std::size_t n = u.size();
  Eigen::MatrixXd H = Eigen::MatrixXd::Zero(m, n);
  for (std::size_t i = 0; i < n; ++i) {
    Eigen::VectorXd du = Eigen::VectorXd::Zero(n);
    du(i) = NUMERICAL_DERIVATIVE_STEP;
    Eigen::VectorXd fp = dfdx(f, x, u + du);
    Eigen::VectorXd fn = dfdx(f, x, u - du);
    H.col(i) = (fp - fn) / 2 / NUMERICAL_DERIVATIVE_STEP;
  }
  // Ensure that the Hessian is necessarily symmetric
  // return (H + H.transpose()) / 2.0;
  return H;
}

double frobNormSquared(const Eigen::MatrixXd& A) {
  assert(A.rows() == A.cols());
  double p = double(A.rows());
  return (A * A.transpose()).trace() / p;
}

const Eigen::MatrixXd estimateCovariance(const Eigen::MatrixXd& samples) {
  // See: "A Well-Conditioned Estimator for Large-dimensional Covariance
  // Matrices", 2004.
  std::size_t n = samples.cols();
  std::size_t d = samples.rows();
  double p = double(d);
  const Eigen::MatrixXd cov = samples * samples.transpose() / double(n);
  double mu = cov.trace() / p;
  const Eigen::MatrixXd muI = mu * Eigen::MatrixXd::Identity(d, d);
  double d2 = frobNormSquared(cov - muI);
  double b2 = 0;
  for (std::size_t i = 0; i < n; ++i) {
    const Eigen::VectorXd& x = samples.col(i);
    b2 += frobNormSquared(x * x.transpose() - cov);
  }
  b2 /= pow(double(n), 2);
  b2 = std::min(d2, b2);
  double shrinkage = b2 / d2;
  return (1 - shrinkage) * cov + shrinkage * muI;
}

}  // namespace sia


#include <itp/core>
#include <fstream>
#include <Eigen/Dense>

double safe_acos(double value)
{
	if (value <= -1.0)
	{
		return itp::pi;
	}
	else if (value >= 1.0)
	{
		return 0;
	}
	else
	{
		return std::acos(value);
	}
}

double calcVolume(double RMOF, double R, double L)
{
	double ret = 0;
	if (R <= RMOF - L)
	{
		ret = 4 * itp::pi * R * R * R / 3;
	}
	else if (R > RMOF + L)
	{
		double hmax = std::sqrt(R * R - (RMOF - L) * (RMOF - L));
		double hmin = std::sqrt(R * R - (RMOF + L) * (RMOF + L));
		double dh = 1e-5;
		double r, alpha, belta, cosAlpha, cosBelta;
		for (double h = hmin; h <= hmax; h += dh)
		{
			r = std::sqrt(R * R - h * h);
			cosAlpha = (r * r + L * L - RMOF * RMOF) / (2.0 * r * L);
			cosBelta = (L * L + RMOF * RMOF - r * r) / (2.0 * L * RMOF);
			alpha = safe_acos(cosAlpha);
			belta = safe_acos(cosBelta);
			ret += dh * (alpha * r * r + belta * RMOF * RMOF - L * RMOF * std::sin(belta));
		}
		ret += itp::pi / 3 * (3 * R - (R - hmax)) * (R - hmax) * (R - hmax);
		ret += hmin * itp::pi * RMOF * RMOF;
		ret *= 2;
	}
	else
	{
		double hmax = std::sqrt(R * R - (RMOF - L) * (RMOF - L));
		double dh = 1e-5;
		double r, alpha, belta, cosAlpha, cosBelta;
		for (double h = 0; h <= hmax; h += dh)
		{
			r = std::sqrt(R * R - h * h);
			cosAlpha = (r * r + L * L - RMOF * RMOF) / (2.0 * r * L);
			cosBelta = (L * L + RMOF * RMOF - r * r) / (2.0 * L * RMOF);
			alpha = safe_acos(cosAlpha);

			belta = safe_acos(cosBelta);

			ret += dh * (alpha * r * r + belta * RMOF * RMOF - L * RMOF * std::sin(belta));
		}
		ret += itp::pi / 3 * (3 * R - (R - hmax)) * (R - hmax) * (R - hmax);
		ret *= 2;
	}
	return ret;
}

int main()
{
	double upPos = 32.927;
	double lowPos = 27.073;

	double RMOF = 0.655;
	double L = 0.4;
	double dL = 0.001;

	int Lbin = RMOF / dL;
	double LMOF = upPos - lowPos;
	double Rmax = LMOF / 2;
	double dR = 0.002; // nm
	int Rbin = Rmax / dR;

	Eigen::ArrayXXd totVolume(Lbin, Rbin);
	totVolume.fill(0);
	fmt::print("{}\n", calcVolume(RMOF, 0.656, 0.001));

}


/*
 * CubicInterpolation.hpp
 *
 *  Created on: Jan 21, 2020
 *      Author: Edo Jelavic
 *      Institute: ETH Zurich, Robotic Systems Lab
 */

#pragma once

#include <Eigen/Core>
#include <vector>
#include <map>
#include "grid_map_core/TypeDefs.hpp"

/*
 * For difference between bicubic convolution interpolation (piecewise cubic)
 * and bicubic interpolation see:
 *
 * https://en.wikipedia.org/wiki/Bicubic_interpolation
 *
 * R. Keys (1981). "Cubic convolution interpolation for digital image processing".
 * IEEE Transactions on Acoustics, Speech, and Signal Processing. 29 (6): 1153–1160.
 *
*   https://web.archive.org/web/20051024202307/
*   http://www.geovista.psu.edu/sites/geocomp99/Gc99/082/gc_082.htm
 */

namespace grid_map {

class GridMap;

/*
 * Data structure (matrix) that contains data
 * necessary for interpolation. These are either 16
 * function values in the case of bicubic convolution interpolation
 * or function values and their derivatives for the case
 * of standard bicubic inteprolation.
 */
using FunctionValueMatrix = Eigen::Matrix4d;

/*!
 * Takes the id requested, performs checks and returns
 * an id that it is within the specified bounds.
 * @param[in] idReq - input index .
 * @param[in] nElem - number of elements in the container
 * @return index that is within [0, nElem-1].
 */
unsigned int bindIndexToRange(unsigned int idReq, unsigned int nElem);

/*!
 * Extract the value of the specific layer at the
 * row and column requested. If row and column are out
 * of bounds they will be bound to range.
 * @param[in] layerMat - matrix of the layer from where
 *                       the data is extracted
 * @param[in] rowReq - row requested
 * @param[in] colReq - column requested
 * @return - value of the layer at rowReq and colReq
 */
double getLayerValue(const Matrix &layerMat, int rowReq, int colReq);

namespace bicubic_conv {

/*!
 * Matrix for cubic interpolation via convolution. Taken from:
 * https://en.wikipedia.org/wiki/Bicubic_interpolation
 */
static const Eigen::Matrix4d cubicInterpolationConvolutionMatrix {
    (Eigen::Matrix4d() << 0.0,  2.0,  0.0,  0.0,
                         -1.0,  0.0,  1.0,  0.0,
                          2.0, -5.0,  4.0, -1.0,
                         -1.0,  3.0, -3.0,  1.0).finished() };

/*
 * Index of the middle knot for bicubic inteprolation. This is
 * the function value with subscripts (0,0), i.e. f00 in
 * https://en.wikipedia.org/wiki/Bicubic_interpolation
 * In the grid map it corresponds to the grid map point closest to the
 * queried point (in terms of Euclidean distance). Queried point has
 * coordinates (x,y) for at which the interpolation is requested.
 * @param[in]  gridMap - grid map with the data
 * @param[in]  queriedPosition - position for which the interpolated data is requested
 * @param[out] index - indices of the middle knot for the interpolation
 * @return - true if success
 */
bool getIndicesOfMiddleKnot(const GridMap &gridMap, const Position &queriedPosition, Index *index);

/*
 * Coordinates used for interpolation need to be shifted and scaled,
 * since the original algorithm operates around the origin and with unit
 * resolution
 * @param[in]  gridMap - grid map with the data
 * @param[in]  queriedPosition - position for which the interpolation is requested
 * @param[out] position - normalized coordinates of the point for which the interpolation is requested
 * @return - true if success
 */
bool getNormalizedCoordinates(const GridMap &gridMap, const Position &queriedPosition,
                              Position *position);

/*
 * Queries the grid map for function values at the coordiantes which are neccesary for
 * performing the interpolation. The right function values are then assembled
 * in a matrix.
 * @param[in]  gridMap - grid map with the data
 * @param[in]  layer - name of the layer that we are interpolating
 * @param[in]  queriedPosition - position for which the interpolation is requested
 * @param[out] data - 4x4 matrix with 16 function values used for interpolation, see
 *           R. Keys (1981). "Cubic convolution interpolation for digital image processing".
 *           IEEE Transactions on Acoustics, Speech, and Signal Processing. 29 (6): 1153–1160.
 *           for the details.
 * @return - true if success
 */
bool assembleFunctionValueMatrix(const GridMap &gridMap, const std::string &layer,
                                 const Position &queriedPosition, FunctionValueMatrix *data);

/*
 * Performs convolution in 1D. the function requires 4 function values
 * to compute the convolution. The result is interpolated data in 1D.
 * @param[in]  t - normalized coordinate (x or y)
 * @param[in]  functionValues - vector of 4 function values neccessary to perform
 *                            interpolation in 1 dimension.
 * @return - interpolated value at normalized coordinate t
 */
double convolve1D(double t, const Eigen::Vector4d &functionValues);

/*
 * Performs convolution in 1D. the function requires 4 function values
 * to compute the convolution. The result is interpolated data in 1D.
 * @param[in]  gridMap - grid map with discrete function values
 * @param[in]  layer - name of the layer for which we want to perform interpolation
 * @param[in]  queriedPosition - position for which the interpolation is requested
 * @param[out] interpolatedValue - interpolated value at queried point
 * @return - true if success
 */
bool evaluateBicubicConvolutionInterpolation(const GridMap &gridMap, const std::string &layer,
                                             const Position &queriedPosition,
                                             double *interpolatedValue);

} /* namespace bicubic_conv */

namespace bicubic {

/*
 * Enum for the derivatives direction
 * to perform interpolation one needs
 * derivatives w.r.t. to x and y dimension.
 */
enum class Dim2D: int {
    X,
    Y
};

/*!
 * Matrix for cubic interpolation. Taken from:
 * https://en.wikipedia.org/wiki/Bicubic_interpolation
 */
static const Eigen::Matrix4d bicubicInterpolationMatrix {
    (Eigen::Matrix4d() << 1.0,  0.0,  0.0,  0.0,
                          0.0,  0.0,  1.0,  0.0,
                         -3.0,  3.0, -2.0, -1.0,
                          2.0, -2.0,  1.0,  1.0).finished() };

/*
 * Data matrix that can hold function values
 * these can be either function values at requested
 * positions or their derivatives.
 */
struct DataMatrix
{
  double topLeft_ = 0.0;
  double topRight_ = 0.0;
  double bottomLeft_ = 0.0;
  double bottomRight_ = 0.0;
};

/*
 * Interpolation is performed on a unit square.
 * Hence we need to compute 4 corners of that unit square,
 * and find their indices in the grid map. IndicesMatrix
 * is a container that stores those indices. Each index
 * contains two numbers (row number, column number) in the
 * grid map.
 */
struct IndicesMatrix
{
  Index topLeft_ { 0, 0 };
  Index topRight_ { 0, 0 };
  Index bottomLeft_ { 0, 0 };
  Index bottomRight_ { 0, 0 };
};

/*
 * Makes sure that all indices in side the
 * data structure IndicesMatrix are within the
 * range of the grid map.
 * @param[in] gridMap - input grid map with discrete function values
 * @param[in/out] indices - indices that are bound to range, i.e.
 *                          rows and columns are with ranges
 */
void bindIndicesToRange(const GridMap &gridMap, IndicesMatrix *indices);

/*
 * Performs bicubic interpolation at requested position.
 * @param[in]  gridMap - grid map with discrete function values
 * @param[in]  layer - name of the layer for which we want to perform interpolation
 * @param[in]  queriedPosition - position for which the interpolation is requested
 * @param[out] interpolatedValue - interpolated value at queried point
 * @return - true if success
 */
bool evaluateBicubicInterpolation(const GridMap &gridMap, const std::string &layer,
                                  const Position &queriedPosition, double *interpolatedValue);

/*
 * Deduces which points in the grid map close a unit square around the
 * queried point and returns their indices (row and column number)
 * @param[in]  gridMap - grid map with discrete function values
 * @param[in]  queriedPosition - position for which the interpolation is requested
 * @param[out] indicesMatrix - data structure with indices forming a unit square
 *                            around the queried point
 * @return - true if success
 */
bool getUnitSquareCornerIndices(const GridMap &gridMap, const Position &queriedPosition,
                                IndicesMatrix *indicesMatrix);

/*
 * Get index (row and column number) of a point in grid map, which
 * is closest to the queried position.
 * @param[in]  gridMap - grid map with discrete function values
 * @param[in]  queriedPosition - position for which the interpolation is requested
 * @param[out] index - indices of the closest point in grid_map
 * @return - true if success
 */
bool getClosestPointIndices(const GridMap &gridMap, const Position &queriedPosition, Index *index);

/*
 * Retrieve function values from the grid map at requested indices.
 * @param[in]  layerData - layer of a grid map with function values
 * @param[in]  indices - indices (row and column numbers) for which function values are requested
 * @param[out] data - requested function values
 * @return - true if success
 */
bool getFunctionValues(const Matrix &layerData, const IndicesMatrix &indices, DataMatrix *data);

/*
 * Retrieve function derivative values from the grid map at requested indices. Function
 * derivatives are approximated using central difference.
 * @param[in]  layerData - layer of a grid map with function values
 * @param[in]  indices - indices (row and column numbers) for which function derivative
 *                       values are requested
 * @param[in]  dim - dimension along which we want to evaluate partial derivatives (X or Y)
 * @param[in]  resolution - resolution of the grid map
 * @param[out] derivatives - values of derivatives at requested indices
 * @return - true if success
 */
bool getFirstOrderDerivatives(const Matrix &layerData, const IndicesMatrix &indices, Dim2D dim,
                              double resolution, DataMatrix *derivatives);

/*
 * Retrieve second order function derivative values from the grid map at requested indices.
 * Function derivatives are approximated using central difference. We compute partial derivative
 * w.r.t to one coordinate and then the other. Note that the order of differentiation
 * does not matter.
 * @param[in]  layerData - layer of a grid map with function values
 * @param[in]  indices - indices (row and column numbers) for which function derivative
 *                       values are requested
 * @param[in]  resolution - resolution of the grid map
 * @param[out] derivatives - values of second order mixed derivatives at requested indices
 * @return - true if success
 */
bool getMixedSecondOrderDerivatives(const Matrix &layerData, const IndicesMatrix &indices,
                                    double resolution, DataMatrix *derivatives);

/*
 * First order derivative for a specific point determined by index.
 * Approximated by central difference.
 * See https://www.mathematik.uni-dortmund.de/~kuzmin/cfdintro/lecture4.pdf
 * for details
 * @param[in]  layerData - layer of a grid map with function values
 * @param[in]  index - index (row and column number) for which function derivative
 *                       value is requested
 * @param[in]  dim - dimension along which we want to evaluate partial derivative (X or Y)
 * @param[in]  resolution - resolution of the grid map
 * @return - value of the derivative at requested index
 */
double firstOrderDerivativeAt(const Matrix &layerData, const Index &index, Dim2D dim,
                              double resolution);

/*
 * Second order mixed derivative for a specific point determined by index.
 * See https://www.mathematik.uni-dortmund.de/~kuzmin/cfdintro/lecture4.pdf
 * for details
 * @param[in]  layerData - layer of a grid map with function values
 * @param[in]  index - index (row and column number) for which function derivative
 *                       value is requested
 * @param[in]  resolution - resolution of the grid map
 * @return - value of the second order mixed derivative at requested index
 */
double mixedSecondOrderDerivativeAt(const Matrix &layerData, const Index &index, double resolution);

/*
 * Evaluate polynomial at requested coordinates. the function will compute the polynomial
 * coefficients and then evaluate it. See
 * https://en.wikipedia.org/wiki/Bicubic_interpolation
 * for details.
 * @param[in]  functionValues - function values and derivatives required to
 *                              compute polynomial coefficients
 * @param[in]  tx - normalized x coordinate for which the interpolation should be computed
 * @param[in]  ty - normalized y coordinate for which the interpolation should be computed
 * @return - interpolated value at requested normalized coordinates.
 */
double evaluatePolynomial(const FunctionValueMatrix &functionValues, double tx, double ty);

/*
 * Assemble function value matrix from small submatrices containing function values
 * or derivative values at the corners of the unit square.
 * See https://en.wikipedia.org/wiki/Bicubic_interpolation for details.
 *
 * @param[in]  f - Function values at the corners of the unit square
 * @param[in]  dfx - Partial derivative w.r.t to x at the corners of the unit square
 * @param[in]  dfy - Partial derivative w.r.t to y at the corners of the unit square
 * @param[in]  ddfxy - Second order partial derivative w.r.t to x and y at the corners of the unit square
 * @param[out]  functionValues - function values and derivatives required to
 *                              compute polynomial coefficients
 */
void assembleFunctionValueMatrix(const DataMatrix &f, const DataMatrix &dfx, const DataMatrix &dfy,
                                 const DataMatrix &ddfxy, FunctionValueMatrix *functionValues);

/*
 * Coordinates used for interpolation need to be shifter and scaled,
 * since the original algorithm operates on a unit square around the origin.
 * @param[in]  gridMap - grid map with the data
 * @param[in]  originIndex - index of a bottom left corner if the unit square in the grid map
 *                            this corner is the origin for the normalized coordinates.
 * @param[in]  queriedPosition - position for which the interpolation is requested
 * @param[out] position - normalized coordinates of the point for which the interpolation is requested
 * @return - true if success
 */
bool computeNormalizedCoordinates(const GridMap &gridMap, const Index &originIndex,
                                  const Position &queriedPosition, Position *normalizedCoordinates);

} /* namespace bicubic */

} /* namespace grid_map*/


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-trigonometric
    This function object returns the quadrant aware `atan2` function.

    @par Header <boost/simd/function/atan2.hpp>

    @par Notes

    - For any real arguments @c x and @c y not both equal to zero, <tt>atan2(y, x)</tt>
    (be aware of the parameter order) is the angle in radians between the positive
    x-axis of a plane and the point  given by the coordinates  <tt>(x, y)</tt>.

    - It is also the angle in \f$[-\pi,\pi[\f$ for which
    \f$x/\sqrt{x^2+y^2}\f$ and \f$y/\sqrt{x^2+y^2}\f$
    are respectively the sine and the cosine.

    - Following IEEE norms,  we should have:
     -  If y is \f$\pm0\f$ and x is negative or -0,\f$\pm\pi\f$ is returned
     -  If y is \f$\pm0\f$ and x is positive or +0, \f$\pm0\f$ is returned
     -  If y is \f$\pm\infty\f$ and x is finite, \f$\pm\pi/2\f$ is returned
     -  If y is \f$\pm\infty\f$ and x is \f$-\infty\f$,\f$\pm3\pi/4\f$ is returned
     -  If y is \f$\pm\infty\f$ and x is \f$+\infty\f$, \f$\pm\pi/4\f$ is returned
     -  If x is \f$\pm0\f$ and y is negative, \f$-\pi/2\f$ is returned
     -  If x is \f$\pm0\f$ and y is positive, \f$+\pi/2\f$  is returned
     -  If x is \f$-\infty\f$ and y is finite and positive, \f$+\pi\f$ is returned
     -  If x is \f$-\infty\f$ and y is finite and negative, \f$-\pi\f$ is returned
     -  If x is \f$+\infty\f$ and y is finite and positive, +0 is returned
     -  If x is \f$+\infty\f$ and y is finite and negative, -0 is returned
     -  If either x is Nan or y is Nan, Nan is returned

     The pedantic_ decorator ensures all these conditions, but the regular version
     (no decorator) will return a NaN if x and y are both either null or infinite,
     result which in fact is not more absurd than the IEEE choices.
     It will be conforming in all other cases.

    @par Decorators

    - std_  provides access to std::atan2

    - pedantic_ ensures the respect of all IEEE limits

    @see atan, atand, atanpi

    @par Example:

      @snippet atan2.cpp atan2

    @par Possible output:

      @snippet atan2.txt atan2

  **/
  IEEEValue atan2(IEEEValue const& y, IEEEValue const& x);
} }
#endif

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

#endif


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * 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.
 ********************************************************************************/

#ifndef RW_MATH_PERSPECTIVETRANSFORM2D_HPP
#define RW_MATH_PERSPECTIVETRANSFORM2D_HPP

#if !defined(SWIG)
#include "Vector2D.hpp"
#include "Vector3D.hpp"

#include <rw/common/Serializable.hpp>
#include <rw/core/macros.hpp>

#include <Eigen/Core>
#endif

namespace rw { namespace math {

    /**
     * @brief The PerspectiveTransform2D is a perspective transform in 2D.
     *
     * The homographic transform can be used to map one arbitrary 2D
     * quadrilateral into another.
     */
    template< class T = double > class PerspectiveTransform2D
    {
      private:
        //! Eigen 3x3 matrix used as internal data structure.
        typedef Eigen::Matrix< T, 3, 3 > EigenMatrix3x3;

      public:
        /**
         * @brief constructor
         */
        PerspectiveTransform2D ()    //: _matrix(3,3)
        {
            _matrix (0, 0) = 1;
            _matrix (0, 1) = 0;
            _matrix (0, 2) = 0;
            _matrix (1, 0) = 0;
            _matrix (1, 1) = 1;
            _matrix (1, 2) = 0;
            _matrix (2, 0) = 0;
            _matrix (2, 1) = 0;
            _matrix (2, 2) = 1;
        }

        /**
         * @brief constructor
         */
        PerspectiveTransform2D (T r11, T r12, T r13, T r21, T r22, T r23, T r31, T r32,
                                T r33)    //: _matrix(3,3)
        {
            _matrix (0, 0) = r11;
            _matrix (0, 1) = r12;
            _matrix (0, 2) = r13;
            _matrix (1, 0) = r21;
            _matrix (1, 1) = r22;
            _matrix (1, 2) = r23;
            _matrix (2, 0) = r31;
            _matrix (2, 1) = r32;
            _matrix (2, 2) = r33;
        }

        /**
         * @brief constructor
         * @param r
         * @return
         */
        template< class R > explicit PerspectiveTransform2D (const Eigen::Matrix< R, 3, 3 >& r)
        //: _matrix(r)
        {
            for (size_t i = 0; i < 3; i++)
                for (size_t j = 0; j < 3; j++)
                    _matrix (i, j) = r (i, j);
        }

        /**
         * @brief constructor
         * @param r
         * @return
         */
        template< class R > explicit PerspectiveTransform2D (const Eigen::MatrixBase< R >& r)
        //: _matrix(r)
        {
            RW_ASSERT (r.rows () == 3);
            RW_ASSERT (r.cols () == 3);
            for (size_t i = 0; i < 3; i++)
                for (size_t j = 0; j < 3; j++)
                    _matrix (i, j) = r.row (i) (j);
        }

        /**
         * @brief calculates a PerspectiveTransform2D that maps points from point
         * set pts1 to point set pts2
         * @param pts1 [in] point set one
         * @param pts2 [in] point set two
         */
        static PerspectiveTransform2D< T >
        calcTransform (std::vector< rw::math::Vector2D< T > > pts1,
                       std::vector< rw::math::Vector2D< T > > pts2);

        /**
         * @brief Returns the inverse of the PerspectiveTransform
         */
        PerspectiveTransform2D< T > inverse () const
        {
            return PerspectiveTransform2D< T > (_matrix.transpose ());
        }
#if !defined(SWIG)
        /**
         * @brief Returns matrix element reference
         * @param row [in] row, row must be @f$ < 3 @f$
         * @param col [in] col, col must be @f$ < 3 @f$
         * @return reference to matrix element
         */
        T& operator() (std::size_t row, std::size_t col)
        {
            assert (row < 3);
            assert (col < 3);
            return _matrix (row, col);
        }

        /**
         * @brief Returns const matrix element reference
         * @param row [in] row, row must be @f$ < 3 @f$
         * @param col [in] col, col must be @f$ < 3 @f$
         * @return const reference to matrix element
         */
        const T& operator() (std::size_t row, std::size_t col) const
        {
            assert (row < 3);
            assert (col < 3);
            return _matrix (row, col);
        }
#else
        MATRIXOPERATOR (T);
#endif

        /**
         * @brief transform a point using this perspective transform
         */
        rw::math::Vector2D< T > operator* (const rw::math::Vector2D< T >& v) const
        {
            const T x = v (0);
            const T y = v (1);

            const T g      = (*this) (2, 0);
            const T h      = (*this) (2, 1);
            const T one    = static_cast< T > (1);
            const T lenInv = one / (g * x + h * y + one);

            const T a = (*this) (0, 0);
            const T b = (*this) (0, 1);
            const T c = (*this) (0, 2);

            const T d = (*this) (1, 0);
            const T e = (*this) (1, 1);
            const T f = (*this) (1, 2);

            return rw::math::Vector2D< T > ((a * x + b * y + c) * lenInv,
                                            (d * x + e * y + f) * lenInv);
        }

        /**
         * @brief transform a 2d point into a 3d point with this
         * perspective transform
         * @param hT
         * @param v
         * @return
         */
        rw::math::Vector3D< T > calc3dVec (const PerspectiveTransform2D< T >& hT,
                                           const rw::math::Vector2D< T >& v)
        {
            const T x = v (0);
            const T y = v (1);

            const T g   = hT (2, 0);
            const T h   = hT (2, 1);
            const T one = static_cast< T > (1);
            // const T lenInv = one / (g * x + h * y + one);
            const T len = (g * x + h * y + one);

            const T a = hT (0, 0);
            const T b = hT (0, 1);
            const T c = hT (0, 2);

            const T d = hT (1, 0);
            const T e = hT (1, 1);
            const T f = hT (1, 2);

            return rw::math::Vector3D< T > ((a * x + b * y + c), (d * x + e * y + f), len);
        }

        /**
         * @brief Returns reference to the 3x3 matrix @f$ \mathbf{M}\in SO(3)
         * @f$ that represents this rotation
         *
         * @return @f$ \mathbf{M}\in SO(3) @f$
         */
        const Eigen::Matrix< T, 3, 3 >& e () const { return _matrix; }

        /**
         * @brief Returns reference to the 3x3 matrix @f$ \mathbf{M}\in SO(3)
         * @f$ that represents this rotation
         *
         * @return @f$ \mathbf{M}\in SO(3) @f$
         */
        Eigen::Matrix< T, 3, 3 >& e () { return _matrix; }

      private:
        EigenMatrix3x3 _matrix;
    };

    /**
     * @brief Take the inverse of a PerspectiveTransform2D.
     * @param aRb [in] a PerspectiveTransform2D.
     * @return the inverse of \b aRb .
     * @relates rw::math::PerspectiveTransform2D
     */
    template< class T > PerspectiveTransform2D< T > inverse (const PerspectiveTransform2D< T >& aRb)
    {
        return aRb.inverse ();
        // return PerspectiveTransform2D<T>(trans(aRb.m()));
    }
#if !defined(SWIG)
    extern template class rw::math::PerspectiveTransform2D< double >;
    extern template class rw::math::PerspectiveTransform2D< float >;
#else
    SWIG_DECLARE_TEMPLATE (PerspectiveTransform2Dd, rw::math::PerspectiveTransform2D< double >);
    SWIG_DECLARE_TEMPLATE (PerspectiveTransform2Df, rw::math::PerspectiveTransform2D< float >);
#endif

    using PerspectiveTransform2Dd = PerspectiveTransform2D< double >;
    using PerspectiveTransform2Df = PerspectiveTransform2D< float >;

}}    // namespace rw::math

namespace rw { namespace common {
    class OutputArchive;
    class InputArchive;
    namespace serialization {
        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::PerspectiveTransform2D
         */
        template<>
        void write (const rw::math::PerspectiveTransform2D< double >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::PerspectiveTransform2D
         */
        template<>
        void write (const rw::math::PerspectiveTransform2D< float >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::PerspectiveTransform2D
         */
        template<>
        void read (rw::math::PerspectiveTransform2D< double >& sobject,
                   rw::common::InputArchive& iarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::PerspectiveTransform2D
         */
        template<>
        void read (rw::math::PerspectiveTransform2D< float >& sobject,
                   rw::common::InputArchive& iarchive, const std::string& id);
    }    // namespace serialization
}}       // namespace rw::common

#endif    // end include guard


#include "Math.h"
#include "../utils/GapsAssert.h"

#include <boost/math/distributions/exponential.hpp>
#include <boost/math/distributions/gamma.hpp>
#include <boost/math/distributions/normal.hpp>

#define GAPS_SQ(x) ((x) * (x))

#define Q_GAMMA_THRESHOLD 0.000001f
#define Q_GAMMA_MIN_VALUE 0.f

float gaps::min(float a, float b)
{
    return a < b ? a : b;
}

unsigned gaps::min(unsigned a, unsigned b)
{
    return a < b ? a : b;
}

uint64_t gaps::min(uint64_t a, uint64_t b)
{
    return a < b ? a : b;
}

float gaps::max(float a, float b)
{
    return a < b ? b : a;
}

unsigned gaps::max(unsigned a, unsigned b)
{
    return a < b ? b : a;
}

uint64_t gaps::max(uint64_t a, uint64_t b)
{
    return a < b ? b : a;
}

float gaps::d_gamma(float d, float shape, float scale)
{
    boost::math::gamma_distribution<> gam(shape, scale);
    return pdf(gam, d);
}

float gaps::p_gamma(float p, float shape, float scale)
{
    boost::math::gamma_distribution<> gam(shape, scale);
    return cdf(gam, p);
}

float gaps::q_gamma(float q, float shape, float scale)
{
    if (q < Q_GAMMA_THRESHOLD)
    {
        return Q_GAMMA_MIN_VALUE;
    }
    boost::math::gamma_distribution<> gam(shape, scale);
    return quantile(gam, q);
}

float gaps::d_norm(float d, float mean, float sd)
{
    boost::math::normal_distribution<> norm(mean, sd);
    return pdf(norm, d);
}

float gaps::q_norm(float q, float mean, float sd)
{
    boost::math::normal_distribution<> norm(mean, sd);
    return quantile(norm, q);
}

float gaps::p_norm(float p, float mean, float sd)
{
    boost::math::normal_distribution<> norm(mean, sd);
    return cdf(norm, p);
}

double gaps::lgamma(double x)
{
    return boost::math::lgamma(x); // NOLINT
}


//
// Created by Yohsuke Murase on 2020/06/04.
//

#include <iostream>
#include <vector>
#include <array>
#include <map>
#include <random>
#include <cassert>
#include <fstream>
#include <Eigen/Dense>
#include "StrategyN2M3.hpp"

// memory-1 species in strategy space discretized with `D`
// memory-1 strategy is characterized by tuple (p_{cc}, p_{cd}, p_{dc}, p_{dd}),
// where each of which denotes cooperation probability conditioned by the last Alice's action and the other players' defectors.
// Each of which can take discrete values [0,1/D,2/D,...,D/D].
// ID of a species is given by an integer with base-(D+1).
// ID can take values [0, (D+1)^4-1]
// ID=0 : AllD, ID=ID_MAX-1 : AllC
class Cprobs {
 public:
  Cprobs(size_t id, size_t DIS) {
    double dinv = 1.0 / DIS;
    const size_t B = DIS+1;
    cc = (id % B) * dinv;
    cd = ((id/B) % B) * dinv; // A: c, B: d
    dc = ((id/B/B) % B) * dinv; // A: d, B: c
    dd = ((id/B/B/B) % B) * dinv;
  }
  double cc, cd, dc, dd;
};

class Mem1Species {
 public:
  static size_t N_M1_Species(size_t DIS) { return (DIS+1)*(DIS+1)*(DIS+1)*(DIS+1); }
  Mem1Species(size_t _id, size_t _DIS) : id(_id), DIS(_DIS), prob(_id, _DIS) {
    assert(id <= N_M1_Species(DIS));
  }
  size_t id;
  size_t DIS;
  Cprobs prob;

  std::string ToString() const {
    const size_t B = DIS+1;
    size_t cc = (id % B);
    size_t cd = ((id/B) % B);
    size_t dc = ((id/B/B) % B);
    size_t dd = ((id/B/B/B) % B);
    std::ostringstream oss;
    oss << cc << '-' << cd << '-' << dc << '-' << dd << '/' << DIS;
    if (IsDefensible()) { oss << "_D"; }
    if (IsEfficient()) { oss << "_E"; }
    return oss.str();
  }

  std::vector<double> StationaryState(const Mem1Species& Bstr, double error = 0.0) const {
    Eigen::Matrix<double,4,4> A;

    // state 0: cc, state 1: cd (lower bit is A's history), ... 3: dd
    // calculate transition probability from j to i

    for (size_t j = 0; j < 4; j++) {
      Action last_a = (j & 1ul) ? D : C;
      Action last_b = ((j>>1ul) & 1ul) ? D : C;

      auto cooperation_prob = [](Action a, Action b, const Mem1Species& str) -> double {
        if (a == C) {
          if (b == C) { return str.prob.cc; }
          else { return str.prob.cd; }
        }
        else {
          if (b == C) { return str.prob.dc; }
          else { return str.prob.dd; }
        }
      };
      double c_A = cooperation_prob(last_a, last_b, *this);
      double c_B = cooperation_prob(last_b, last_a, Bstr);

      c_A = (1.0 - error) * c_A + error * (1.0 - c_A);
      c_B = (1.0 - error) * c_B + error * (1.0 - c_B);
      A(0,j) = c_A * c_B;
      A(1,j) = (1.0-c_A) * c_B;
      A(2,j) = c_A * (1.0-c_B);
      A(3,j) = (1.0-c_A) * (1.0-c_B);
    }

    for(int i=0; i<4; i++) {
      A(i,i) -= 1.0;
    }
    for(int i=0; i<4; i++) {
      A(4-1,i) += 1.0;  // normalization condition
    }
    Eigen::VectorXd b(4);
    for(int i=0; i<4; i++) { b(i) = 0.0;}
    b(4-1) = 1.0;
    Eigen::VectorXd x = A.colPivHouseholderQr().solve(b);
    std::vector<double> ans(4, 0.0);
    for(int i=0; i<ans.size(); i++) {
      ans[i] = x(i);
    }

    return ans;
  }

  double CooperationProb(StateN2M3 s) const {
    if (s.a_1 == C) {
      if (s.b_1 == C) { return prob.cc; }
      else { return prob.cd; }
    }
    else {
      if (s.b_1 == C) { return prob.dc; }
      else { return prob.dd; }
    }
  }

  // returns false for mixed strategy
  bool IsDefensible() const {
    double tolerance = 1.0e-8;
    if (prob.cc > tolerance && prob.cc < 1.0 - tolerance) { return false; }
    if (prob.cd > tolerance && prob.cd < 1.0 - tolerance) { return false; }
    if (prob.dc > tolerance && prob.dc < 1.0 - tolerance) { return false; }
    if (prob.dd > tolerance && prob.dd < 1.0 - tolerance) { return false; }

    // N = 2^{2}
    const size_t N = 4;
    typedef std::array<std::array<int, N>, N> d_matrix_t;
    d_matrix_t d;

    // construct adjacency matrix
    const int INF = N; // N is large enough since the path length is between -N/4 to N/4.
    for (size_t i = 0; i < N; i++) {
      for (size_t j = 0; j < N; j++) {
        d[i][j] = INF;
      }
    }

    for (size_t i = 0; i < N; i++) {
      Action a0 = (i & 1) ? D : C; // current state
      Action b0 = (i & 2) ? D : C;
      Action act_a = C;
      if (a0 == C) {
        if (b0 == C) { act_a = (prob.cc > 0.5 ? C : D); }
        else { act_a = (prob.cd > 0.5 ? C : D); }
      }
      else {
        if (b0 == C) { act_a = (prob.dc > 0.5 ? C : D); }
        else { act_a = (prob.dd > 0.5 ? C : D); }
      }

      // Get possible next states
      std::array<Action, 2> act_bs = {C, D};
      for (auto act_b: act_bs) {
        size_t j = 0;
        if (act_a == D) { j += 1; }
        if (act_b == D) { j += 2; }
        if (j == 0 || j == 3) d[i][j] = 0;
        else if (j == 1) d[i][j] = 1;
        else if (j == 2) d[i][j] = -1;
      }
      if (d[i][i] < 0) { return false; }
    }

    for (size_t k = 0; k < N; k++) {
      for (size_t i = 0; i < N; i++) {
        for (size_t j = 0; j < N; j++) {
          d[i][j] = std::min(d[i][j], d[i][k] + d[k][j]);
        }
        if (d[i][i] < 0) { return false; }
      }
    }
    return true;
  }

  bool IsEfficient() const {
    auto ss = StationaryState(*this, 0.0001);
    return (ss[0] > 0.99);
  }
};

class Species { // either Mem1Species or StrategyN3M5
 public:
  Species(size_t ID, size_t DIS) : m1(Mem1Species(0, DIS)), m5(0ull){
    const size_t N_M1 = Mem1Species::N_M1_Species(DIS);
    if (ID < N_M1) {
      is_m1 = true;
      m1 = Mem1Species(ID, DIS);
      m5 = StrategyN2M3(0ull);
      name = m1.ToString();
    }
    else if (ID == N_M1) {
      is_m1 = false;
      m1 = Mem1Species(0, DIS);
      m5 = StrategyN2M3::CAPRI2();
      name = "CAPRI2";
    }
    else if (ID == N_M1 + 1) {
      is_m1 = false;
      m1 = Mem1Species(0, DIS);
      m5 = StrategyN2M3::CAPRI();
      name = "CAPRI";
    }
    else if (ID == N_M1 + 2) {
      is_m1 = false;
      m1 = Mem1Species(0, DIS);
      m5 = StrategyN2M3::TFT_ATFT();
      name = "TFT_ATFT";
    }
    else if (ID == N_M1 + 3) {
      is_m1 = false;
      m1 = Mem1Species(0, DIS);
      m5 = StrategyN2M3::AON(2);
      name = "AON2";
    }
    else if (ID == N_M1 + 4) {
      is_m1 = false;
      m1 = Mem1Species(0, DIS);
      m5 = StrategyN2M3::AON(3);
      name = "AON3";
    }
    else {
      throw std::runtime_error("must not happen");
    }
  };
  bool is_m1;
  Mem1Species m1;
  StrategyN2M3 m5;
  std::string name;
  std::string ToString() const { return name; }
  std::vector<double> StationaryState(const Species &sb, double error) const {
    if (is_m1 && sb.is_m1) {
      return m1.StationaryState(sb.m1, error);
    }
    std::cerr << "calculating stationary state: " << name << ' ' << sb.name << std::endl;

    typedef Eigen::Triplet<double> T;
    std::vector<T> tripletVec;

    for (size_t j = 0; j < 64; j++) {
      // calculate transition probability from j to i
      const StateN2M3 sj(j);
      double c_a = _CooperationProb(sj);
      double c_b = sb._CooperationProb(sj.SwapAB());
      // cooperation probability taking noise into account
      c_a = (1.0 - error) * c_a + error * (1.0 - c_a);
      c_b = (1.0 - error) * c_b + error * (1.0 - c_b);

      for (size_t t = 0; t < 4; t++) {
        Action act_a = (t & 1ul) ? D : C;
        Action act_b = (t & 2ul) ? D : C;
        size_t i = sj.NextState(act_a, act_b).ID();
        double p_a = (act_a == C) ? c_a : (1.0-c_a);
        double p_b = (act_b == C) ? c_b : (1.0-c_b);
        tripletVec.emplace_back(i, j, p_a * p_b);
      }
    }

    const size_t S = 64;
    Eigen::SparseMatrix<double> A(S, S);
    A.setFromTriplets(tripletVec.cbegin(), tripletVec.cend());

    // subtract unit matrix & normalization condition
    std::vector<T> iVec;
    for (int i = 0; i < S-1; i++) { iVec.emplace_back(i, i, -1.0); }
    for (int i = 0; i < S-1; i++) { iVec.emplace_back(S-1, i, 1.0); }
    Eigen::SparseMatrix<double> I(S, S);
    I.setFromTriplets(iVec.cbegin(), iVec.cend());
    A = A + I;
    // std::cerr << "  transition matrix has been created" << std::endl;

    Eigen::VectorXd b = Eigen::VectorXd::Zero(S);
    b(S-1) = 1.0;

    Eigen::BiCGSTAB<Eigen::SparseMatrix<double>, Eigen::IncompleteLUT<double> > solver;
    solver.compute(A);
    Eigen::VectorXd x = solver.solve(b);

    // std::cerr << "#iterations:     " << solver.iterations() << std::endl;
    // std::cerr << "estimated error: " << solver.error() << std::endl;

    std::vector<double> ans(S, 0.0);
    for (int i = 0; i < S; i++) { ans[i] = x[i]; }
    return ans;
  }
 private:
  double _CooperationProb(const StateN2M3 &s) const {
    if (is_m1) { return m1.CooperationProb(s); }
    else { return m5.ActionAt(s) == C ? 1.0 : 0.0; }
  }

 public:
  static std::vector<Species> Memory1Species(size_t discrete_level) {
    std::vector<Species> ans;
    size_t n = Mem1Species::N_M1_Species(discrete_level);
    for (size_t i = 0; i < n; i++) {
      ans.emplace_back(i, discrete_level);
    }
    return std::move(ans);
  }
  static std::vector<Species> ReactiveMem1Species(size_t discrete_level) {
    std::vector<Species> ans;
    size_t B = discrete_level + 1;
    size_t n = Mem1Species::N_M1_Species(discrete_level);
    size_t B2 = B * B;
    for (size_t i = 0; i < n/B2; i++) {
      size_t id = i * B2 + i;
      ans.emplace_back(id, discrete_level);
    }
    return std::move(ans);
  }
};

class Ecosystem {
 public:
  Ecosystem(const std::vector<Species> &species_pool, double error) :pool(species_pool), N_SPECIES(species_pool.size()), e(error) {
    CalculateSSCache();
  };
  size_t N_SPECIES;
  std::vector<Species> pool;
  const double e;
  typedef std::vector<double> ss_cache_t;
  std::vector<std::vector<ss_cache_t> > ss_cache;
  // ss_cache[i][i] stores the stationary state when PG game is played by (i,i)
  // ss_cache[i][j] stores the stationary state when PG game is played by (i,j)

  void CalculateSSCache() {
    ss_cache.resize(N_SPECIES);
    for (size_t i = 0; i < N_SPECIES; i++) {
      ss_cache[i].resize(N_SPECIES);
    }

#pragma omp parallel for schedule(dynamic,1)
    for (size_t I=0; I < N_SPECIES * N_SPECIES; I++) {
      size_t i = I / N_SPECIES;
      size_t j = I % N_SPECIES;
      ss_cache[i][j] = pool[i].StationaryState(pool[j], e);
    }
  }

  // payoff of species i and j when the game is played by (i,j)
  std::array<double,2> PayoffVersus(size_t i, size_t j, double benefit, double cost) const {
    return Payoffs(ss_cache[i][j], benefit, cost);
  }

  std::array<double,2> Payoffs(const ss_cache_t &ss, double benefit, double cost) const {
    std::array<double, 2> ans = {0.0, 0.0};
    if (ss.size() == 4) {
      for (size_t i = 0; i < 4; i++) {
        double pa = 0.0, pb = 0.0;
        if ((i & 1ul) == 0) {
          pa -= cost;
          pb += benefit;
        }
        if ((i & 2ul) == 0) {
          pb -= cost;
          pa += benefit;
        }
        ans[0] += ss[i] * pa;
        ans[1] += ss[i] * pb;
      }
    }
    else {
      assert(ss.size() == 64);
      for (size_t i = 0; i < 64; i++) {
        StateN2M3 s(i);
        double pa = 0.0, pb = 0.0;
        if (s.a_1 == C) {
          pa -= cost;
          pb += benefit;
        }
        if (s.b_1 == C) {
          pb -= cost;
          pa += benefit;
        }
        ans[0] += ss[i] * pa;
        ans[1] += ss[i] * pb;
      }
    }
    return ans;
  }

  // calculate the equilibrium distribution exactly by linear algebra
  std::vector<double> CalculateEquilibrium(double benefit, double cost, uint64_t N, double sigma) const {
    Eigen::MatrixXd A(N_SPECIES, N_SPECIES);
    #pragma omp parallel for
    for (size_t ii = 0; ii < N_SPECIES * N_SPECIES; ii++) {
      size_t i = ii / N_SPECIES;
      size_t j = ii % N_SPECIES;
      if (i == j) { A(i, j) = 0.0; continue; }
      double p = FixationProb(benefit, cost, N, sigma, i, j);
      // std::cerr << "Fixation prob of mutant (mutant,resident): " << p << " (" << pool[i].ToString() << ", " << pool[j].ToString() << ")" << std::endl;
      A(i, j) = p * (1.0 / N_SPECIES);
    }

    for (size_t j = 0; j < N_SPECIES; j++) {
      double p_sum = 0.0;
      for (size_t i = 0; i < N_SPECIES; i++) {
        p_sum += A(i, j);
      }
      assert(p_sum <= 1.0);
      A(j, j) = 1.0 - p_sum; // probability that the state doesn't change
    }

    size_t n_row = A.rows();

    // subtract Ax = x => (A-I)x = 0
    for (size_t i = 0; i < A.rows(); i++) {
      A(i, i) -= 1.0;
    }
    // normalization condition
    for (size_t i = 0; i < A.rows(); i++) {
      A(A.rows()-1, i) += 1.0;
    }

    Eigen::VectorXd b(A.rows());
    for(int i=0; i<A.rows()-1; i++) { b(i) = 0.0;}
    b(A.rows()-1) = 1.0;
    Eigen::VectorXd x = A.householderQr().solve(b);
    std::vector<double> ans(A.rows());
    double prob_total = 0.0;
    for(int i=0; i<ans.size(); i++) {
      ans[i] = x(i);
      prob_total += x(i);
      assert(x(i) > -0.000001);
    }
    assert(std::abs(prob_total - 1.0) < 0.00001);
    return ans;
  }

  double FixationProb(double benefit, double cost, uint64_t N, double sigma, size_t mutant_idx, size_t resident_idx) const {
    // \frac{1}{\rho} = \sum_{i=0}^{N-1} \exp\left( \sigma \sum_{j=1}^{i} \left[(N-j-1)s_{yy} + js_{yx} - (N-j)s_{xy} - (j-1)s_{xx} \right] \right) \\
    //                = \sum_{i=0}^{N-1} \exp\left( \frac{\sigma i}{2} \left[(-i+2N-3)s_{yy} + (i+1)s_{yx} - (-i+2N-1)s_{xy} - (i-1)s_{xx} \right] \right)

    double s_xx = PayoffVersus(mutant_idx, mutant_idx, benefit, cost)[0];
    double s_yy = PayoffVersus(resident_idx, resident_idx, benefit, cost)[0];
    auto xy = PayoffVersus(mutant_idx, resident_idx, benefit, cost);
    double s_xy = xy[0];
    double s_yx = xy[1];

    double num_games = (N-1);
    s_xx /= num_games;
    s_yy /= num_games;
    s_xy /= num_games;
    s_yx /= num_games;
    double rho_inv = 0.0;
    for (int i=0; i < N; i++) {
      double x = sigma * i * 0.5 * (
          (2*N-3-i) * s_yy
          + (i+1) * s_yx
          - (2*N-1-i) * s_xy
          - (i-1) * s_xx
      );
      rho_inv += std::exp(x);
    }
    return 1.0 / rho_inv;
  }

  std::vector<std::string> SpeciesNames() const {
    std::vector<std::string> ans;
    for(auto s: pool) {
      ans.emplace_back( s.ToString() );
    }
    return std::move(ans);
  }
  double CooperationLevelSpecies(size_t i) const {
    const ss_cache_t &ss = ss_cache[i][i];
    if (ss.size() == 4) {
      double level = 0.0;
      for (size_t s = 0; s < 4; s++) {
        size_t num_c = 2 - std::bitset<2>(s).count();
        level += ss[s] * (num_c / 2.0);
      };
      return level;
    }
    else {
      double level = 0.0;
      for (size_t s = 0; s < 64; s++) {
        StateN2M3 state(s);
        size_t num_c = 2ul;
        if (state.a_1 == D) num_c -= 1;
        if (state.b_1 == D) num_c -= 1;
        level += ss[s] * (num_c / 2.0);
      }
      return level;
    }
  }
  double CooperationLevel(const std::vector<double> &eq_rate) const {
    assert(eq_rate.size() == N_SPECIES);
    double ans = 0.0;
    for (size_t i = 0; i < N_SPECIES; i++) {
      double c_lev = CooperationLevelSpecies(i);
      ans += eq_rate[i] * c_lev;
    }
    return ans;
  }
};

int main(int argc, char *argv[]) {
  Eigen::initParallel();
  if( argc != 11 ) {
    std::cerr << "Error : invalid argument" << std::endl;
    std::cerr << "  Usage : " << argv[0] << " <Nmax> <sigma> <error rate> <discrete_level> <0:reactive/1:full memory1> <1:capri2> <1:capri> <1:aon2> <1:aon3> <1:tft_atft>" << std::endl;
    return 1;
  }

  if (false)  // debugging
  {
    Species wsls(0b1001, 1);
    Species cccd(0b0111, 1);
    Species capri2(0b10000, 1);
    std::cerr << wsls.ToString() << std::endl;
    std::cerr << capri2.ToString() << std::endl;
    auto x = capri2.StationaryState(wsls, 0.0001);
    std::cerr << x[0] << std::endl;
    // std::vector<Species> pool = Species::Memory1Species(1);
    std::vector<Species> pool;
    pool.push_back(wsls);
    pool.push_back(capri2);
    pool.push_back(cccd);
    Ecosystem eco(pool, 0.0001);
    double p01 = eco.FixationProb(4.0, 1.0, 64, 1.0, 0, 1);
    double p10 = eco.FixationProb(4.0, 1.0, 64, 1.0, 1, 0);
    double p02 = eco.FixationProb(4.0, 1.0, 64, 1.0, 0, 2);
    double p20 = eco.FixationProb(4.0, 1.0, 64, 1.0, 2, 0);
    double p12 = eco.FixationProb(4.0, 1.0, 64, 1.0, 1, 2);
    double p21 = eco.FixationProb(4.0, 1.0, 64, 1.0, 2, 1);
    auto eq = eco.CalculateEquilibrium(4.0, 1.0, 64, 1.0);
    std::cerr << cccd.ToString() << std::endl;
    std::cerr << ' ' << p02 << std::endl;
  }

  double cost = 1.0;
  uint64_t Nmax = std::strtoull(argv[1], nullptr,0);
  double sigma = std::strtod(argv[2], nullptr);
  double e = std::strtod(argv[3], nullptr);
  uint64_t discrete_level = std::strtoull(argv[4], nullptr,0);

  unsigned long full_or_reactive = std::strtoul(argv[5], nullptr, 0);
  unsigned long add_capri2 = std::strtoul(argv[6], nullptr, 0);
  unsigned long add_capri = std::strtoul(argv[7], nullptr, 0);
  unsigned long add_aon2 = std::strtoul(argv[8], nullptr, 0);
  unsigned long add_aon3 = std::strtoul(argv[9], nullptr, 0);
  unsigned long add_tftatft = std::strtoul(argv[10], nullptr, 0);

  std::vector<Species> pool = (full_or_reactive == 1 ? Species::Memory1Species(discrete_level) : Species::ReactiveMem1Species(discrete_level));
  size_t N_M1 = Mem1Species::N_M1_Species(discrete_level);
  if (add_capri2) {
    pool.emplace_back(N_M1+0, discrete_level);
  }
  if (add_capri) {
    pool.emplace_back(N_M1+1, discrete_level);
  }
  if (add_aon2) {
    pool.emplace_back(N_M1+3, discrete_level);
  }
  if (add_aon3) {
    pool.emplace_back(N_M1+4, discrete_level);
  }
  if (add_tftatft) {
    pool.emplace_back(N_M1+2, discrete_level);
  }
  Ecosystem eco(pool, e);

  {
    std::ofstream namout("species.txt");
    for(auto name: eco.SpeciesNames()) {
      namout << name << std::endl;
    }
    namout << std::endl;
  }

  auto SweepOverBeta = [&eco,cost,sigma](size_t N)->std::vector<std::pair<double,double>> {
    char fname1[100];
    sprintf(fname1, "abundance_%zu.dat", N);
    std::ofstream eqout(fname1);
    std::vector<std::pair<double,double>> c_levels;
    for (int i = 5; i <= 300; i+=5) {
      double benefit = 1.0 + i / 100.0;
      auto eq = eco.CalculateEquilibrium(benefit, cost, N, sigma);
      eqout << benefit << ' ';
      for (double x: eq) { eqout << x << ' '; }
      eqout << std::endl;
      double c_lev = eco.CooperationLevel(eq);
      c_levels.push_back(std::make_pair(benefit, c_lev));
    }
    return c_levels;
  };

  std::vector<std::vector<std::pair<double,double>>> ans;
  for (int N = 2; N <= Nmax; N++) {
    auto a = SweepOverBeta(N);
    ans.push_back(a);
  }

  std::ofstream fout("cooperation_level.dat");
  for (size_t i = 0; i < ans[0].size(); i++) {
    fout << ans[0][i].first;
    for (size_t j = 0; j < ans.size(); j++) {
      fout << ' ' << ans[j][i].second;
    }
    fout << "\n";
  }
  fout.close();

  return 0;
}


//  Copyright John Maddock 2008.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/math/bindings/rr.hpp>
#include <boost/test/included/test_exec_monitor.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/tools/test.hpp>
#include <fstream>

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

using namespace boost::math::tools;
using namespace std;

struct asinh_data_generator
{
   boost::math::ntl::RR operator()(boost::math::ntl::RR z)
   {
      std::cout << z << " ";
      boost::math::ntl::RR result = log(z + sqrt(z * z + 1));
      std::cout << result << std::endl;
      return result;
   }
};

struct acosh_data_generator
{
   boost::math::ntl::RR operator()(boost::math::ntl::RR z)
   {
      std::cout << z << " ";
      boost::math::ntl::RR result = log(z + sqrt(z * z - 1));
      std::cout << result << std::endl;
      return result;
   }
};

struct atanh_data_generator
{
   boost::math::ntl::RR operator()(boost::math::ntl::RR z)
   {
      std::cout << z << " ";
      boost::math::ntl::RR result = log((z + 1) / (1 - z)) / 2;
      std::cout << result << std::endl;
      return result;
   }
};

int test_main(int argc, char*argv [])
{
   boost::math::ntl::RR::SetPrecision(500);
   boost::math::ntl::RR::SetOutputPrecision(40);

   parameter_info<boost::math::ntl::RR> arg1;
   test_data<boost::math::ntl::RR> data;
   std::ofstream ofs;

   bool cont;
   std::string line;

   std::cout << "Welcome.\n"
      "This program will generate spot tests for the inverse hyperbolic sin function:\n";

   do{
      if(0 == get_user_parameter_info(arg1, "z"))
         return 1;
      data.insert(asinh_data_generator(), arg1);

      std::cout << "Any more data [y/n]?";
      std::getline(std::cin, line);
      boost::algorithm::trim(line);
      cont = (line == "y");
   }while(cont);

   std::cout << "Enter name of test data file [default=asinh_data.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "asinh_data.ipp";
   ofs.open(line.c_str());
   write_code(ofs, data, "asinh_data");
   data.clear();

   std::cout << "Welcome.\n"
      "This program will generate spot tests for the inverse hyperbolic cos function:\n";

   do{
      if(0 == get_user_parameter_info(arg1, "z"))
         return 1;
      data.insert(acosh_data_generator(), arg1);

      std::cout << "Any more data [y/n]?";
      std::getline(std::cin, line);
      boost::algorithm::trim(line);
      cont = (line == "y");
   }while(cont);

   std::cout << "Enter name of test data file [default=acosh_data.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "acosh_data.ipp";
   ofs.close();
   ofs.open(line.c_str());
   write_code(ofs, data, "acosh_data");
   data.clear();

   std::cout << "Welcome.\n"
      "This program will generate spot tests for the inverse hyperbolic tan function:\n";

   do{
      if(0 == get_user_parameter_info(arg1, "z"))
         return 1;
      data.insert(atanh_data_generator(), arg1);

      std::cout << "Any more data [y/n]?";
      std::getline(std::cin, line);
      boost::algorithm::trim(line);
      cont = (line == "y");
   }while(cont);

   std::cout << "Enter name of test data file [default=atanh_data.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "atanh_data.ipp";
   ofs.close();
   ofs.open(line.c_str());
   write_code(ofs, data, "atanh_data");

   return 0;
}


#include "Align.h"

#include <iostream>

#include <Eigen/SVD>
#include <cmath>

int flag = 0;
using namespace Eigen;
using namespace std;

Eigen::Matrix3d exp(Eigen::Matrix3d x) {
	//return x.exp();
	double theta = sqrt(x(0, 1)*x(0, 1) + x(0, 2)*x(0, 2) + x(1, 2)*x(1, 2));
	if (abs(theta) == 0) return Eigen::Matrix3d::Identity();
	x /= theta;
	return Eigen::Matrix3d::Identity() +
		x * sin(theta) +
		x * x * (1 - cos(theta));
}

Eigen::Matrix3d log(Eigen::Matrix3d x) {
	//return x.log();
	double theta = (x.trace() - 1) / 2;
	theta = acos(max(-1.0, min(1.0, theta)));
	if (abs(theta) == 0) return Eigen::Matrix3d::Zero();
	return (theta / (2 * sin(theta))) * (x - x.transpose());
}

double polarDec(const Eigen::Matrix3d &a, Eigen::Matrix3d &r, Eigen::Matrix3d &s) {
	JacobiSVD<Eigen::MatrixXd> svd(a, ComputeThinU | ComputeThinV);
	r = svd.matrixU() * svd.matrixV().transpose();
	s = svd.matrixV() * svd.singularValues().asDiagonal() * svd.matrixV().transpose();

	if (r.determinant() < 0) {
		Vector3d sv = svd.singularValues();
		int minsv = 0;
		r = svd.matrixU();
		if (sv(1) < sv(minsv)) minsv = 1;
		if (sv(2) < sv(minsv)) minsv = 2;
		if (sv(minsv) < -Eps) {
			cerr << "polar dec Error, min singular values <= 0 :" << endl;
			cerr << a << endl;
		}
		//cout << "Min SV " << sv(minsv) << " " << minsv << endl;
		r.col(minsv) *= -1;
		sv(minsv) *= -1;
		//cout << "R :\n" << r << endl;
		r = r * svd.matrixV().transpose();
		s = svd.matrixV() * sv.asDiagonal() * svd.matrixV().transpose();
		return sv.sum();
	}
	return svd.singularValues().sum();
}

AffineAlign::AffineAlign(std::vector<Eigen::Vector3d> &v) : p(v), AtA(Matrix3d::Zero()) {
	//if (flag) fout << p.size() << endl;
	for (int i = 0; i < p.size(); i++) {
		//assert(p[i] == p[i]);
		AtA += p[i] * p[i].transpose();
		//if (flag) fout << p[i] << endl << endl;
	}
	if (flag) {
		//fout << "AtA" << endl;
		//fout << AtA << endl;
		//fout << AtA.determinant() << endl;
	}
	AtA = AtA.inverse().eval();
}

Matrix3d AffineAlign::calc(const std::vector<Vector3d> &v) {
	if (v.size() != p.size()) {
		cout << "!!Error v.size() != p.size()" << endl;
		//fout << "!!Error v.size() != p.size()" << endl;
	}
	Vector3d vx(Vector3d::Zero()), vy(Vector3d::Zero()), vz(Vector3d::Zero());
	for (int i = 0; i < p.size(); i++) {
		vx += p[i] * v[i](0);
		vy += p[i] * v[i](1);
		vz += p[i] * v[i](2);
	}
	//assert(vx == vx);
	vx = AtA * vx;
	vy = AtA * vy;
	vz = AtA * vz;
	Matrix3d res;
	res << vx, vy, vz;
	return res.transpose();
}

double AffineAlign::residualwithoutnormal(Eigen::Matrix3d m, std::vector<Eigen::Vector3d> v) {
	double rs = 0;
	for (int i = 0; i < v.size() - 1; i++)
		rs += (v[i] - m*p[i]).squaredNorm();
	return rs;
}

OpenMesh::Vec3d EtoO(const Eigen::Vector3d &v) {
	return OpenMesh::Vec3d(v(0), v(1), v(2));
}

Eigen::Vector3d OtoE(const OpenMesh::Vec3d &v) {
	return Eigen::Vector3d(v[0], v[1], v[2]);
}

void Rot::ToLogR()
{
	double the = circlek * 2 * M_PI + theta;
	//the = theta;
	logr = Eigen::Matrix3d::Zero();
	logr(0, 1) = -axis(2);
	logr(0, 2) = axis(1);
	logr(1, 2) = -axis(0);
	Eigen::Matrix3d logr1 = logr.transpose();
	logr = the*(logr - logr1);
}

double Rot::ToAngle()
{
	double the = circlek * 2 * M_PI + theta;
	return the;
}

Rot logrot(Eigen::Matrix3d jrotation)
{
	Rot jrot;
	double theta = (jrotation.trace() - 1) / 2;
	theta = acos(max(-1.0, min(1.0, theta)));
	if (abs(theta) <= 1e-6)
	{
		jrot.circlek = 0;
		jrot.theta = 0;
		jrot.axis = Eigen::Vector3d::Zero();
	}
	else
	{
		jrot.theta = theta;
		Eigen::Matrix3d tmp = (1 / (2 * sin(theta))) * (jrotation - jrotation.transpose());
		Eigen::Vector3d jaxis;
		jaxis(0) = tmp(2, 1);
		jaxis(1) = tmp(0, 2);
		jaxis(2) = tmp(1, 0);
		jrot.axis = jaxis / jaxis.norm();
	}
	jrot.ToLogR();
	return jrot;
}

void logrot(Rot & jrot ,Eigen::Matrix3d & jrotation)
{
	double theta = (jrotation.trace() - 1) / 2;
	theta = acos(max(-1.0, min(1.0, theta)));
	if (abs(theta) <= 1e-6)
	{
		jrot.circlek = 0;
		jrot.theta = 0;
		jrot.axis = Eigen::Vector3d::Zero();
	}
	else
	{
		jrot.theta = theta;
		Eigen::Matrix3d tmp = (1 / (2 * sin(theta))) * (jrotation - jrotation.transpose());
		Eigen::Vector3d jaxis;
		jaxis(0) = tmp(2, 1);
		jaxis(1) = tmp(0, 2);
		jaxis(2) = tmp(1, 0);
		jrot.axis = jaxis / jaxis.norm();
	}
	//jrot.ToLogR();
}

Rot logrot(Eigen::Matrix3d jrotation, Rot irot)
{

	Rot jrot;
	double theta = (jrotation.trace() - 1) / 2;
	theta = acos(max(-1.0, min(1.0, theta)));
	if (abs(theta) <=1e-6)
	//if (abs(sin(theta))<0.0001||abs(theta) <= 0.15)// cylinder ר��
	{
		jrot = irot;
		jrot.theta = 0;
	}
	else
	{
		Eigen::Vector3d jaxis;
		Eigen::Matrix3d tmp = Eigen::Matrix3d::Zero();
		if (abs(theta - M_PI) <= 1e-6)
		{
			jaxis = irot.axis;
			//jrot.theta = M_PI;
			tmp(0, 1) = -jaxis(2);
			tmp(0, 2) = jaxis(1);
			tmp(1, 2) = -jaxis(0);
			Eigen::Matrix3d tmp2 = tmp.transpose();
			tmp = (tmp - tmp2);
		}
		else
		{
			tmp = (1 / (2 * sin(theta))) * (jrotation - jrotation.transpose());

			jaxis(0) = tmp(2, 1);
			jaxis(1) = tmp(0, 2);
			jaxis(2) = tmp(1, 0);
		}

		//debug
		Eigen::Matrix3d logr = Eigen::Matrix3d::Zero();
		logr(0, 1) = -jaxis(2);
		logr(0, 2) = jaxis(1);
		logr(1, 2) = -jaxis(0);
		Eigen::Matrix3d tmp1 = logr.transpose();
		logr = (logr - tmp1);
		double _norm1 = (tmp - logr).squaredNorm();
		if (_norm1 >= 0.0001)
		{
			//cout<<"error"<<endl;
		}
		tmp = theta*tmp;
		double _norm2 = (tmp - log(jrotation)).squaredNorm();
		if (_norm2 > 0.00001)
		{
			log(jrotation);
		}
		double _norm = jaxis.norm();
		if (_norm < 1)
		{
			//cout<<_norm<<endl;
		}
		double _sign = jaxis.dot(irot.axis);
		if (_sign < 0)
		{
			jaxis = -jaxis;
			theta = 2 * PI - theta;
		}
		jrot.axis = jaxis / jaxis.norm();//
		//jrot.axis = jaxis;
		jrot.theta = theta;
		jrot.circlek = irot.circlek;
	}

	if (abs(theta - irot.theta) > M_PI)
	{
		if (irot.theta < M_PI)
		{
			//[0,2*PI)
			jrot.circlek = jrot.circlek - 1;
		}
		else
		{
			if (jrot.circlek >= 0)
			{
				// cout<<"test"<<endl;
			}
			jrot.circlek = jrot.circlek + 1;
		}
	}
	jrot.ToLogR();
	Eigen::Matrix3d tmp = log(jrotation);
	double _norm1 = (jrot.logr - tmp).squaredNorm();
	if (_norm1 >= 0.0001)
	{
		//cout<<"error"<<endl;
	}

	return jrot;
}


// Portfolio VaR through MC simulation
// from test_ptf_mc_var.cpp

#include <string>
#include <vector>

#include <Eigen/Dense>

#include "CVaR.h"

#include "compute_returns_eigen.h"
#include "compute_var.h"
#include "path.h"
#include "mc_engine.h"
#include "rng.h"
#include "pca.h"
#include "portfolio.h"
#include "ptf_var.h"

#include <boost/math/special_functions/erf.hpp>
using boost::math::erfc_inv;

namespace RiskJS {

using namespace Eigen;
using namespace std;

double CVaRHistorical(priceData rawPrices, weightData weights, double alphatest) {
    // Remove lines with missing values
    size_t n(rawPrices.size() - 1);
    size_t m(rawPrices[0].size() - 1);

    Mat _prices;
    _prices.resize(m,Vec(n));

    for(size_t i = 1;i < n+1;++i){
        for(size_t j = 1;j < rawPrices[i].size();++j){
            string tmp = rawPrices[i][j];
            if(tmp.empty()){
                _prices[j-1][i-1] = 99999.;
            }
            else{
                _prices[j-1][i-1] = stod(tmp);
            }
        }
    }

    vector<string> indexNames(rawPrices[0].size() - 1);

    for(size_t i = 1;i < rawPrices[0].size();++i){
        indexNames[i-1] = rawPrices[0][i];
    }

		// Remove missing values to compute trailling returns
    Mat prices;
    prices.resize(m,Vec(0));

    bool goodrow = true;
    for(size_t j = 0;j < _prices[0].size();++j){
        for(size_t i = 0;i < _prices.size();++i){
            if((_prices[i][j] == 99999) || (_prices[i][j] == 0))
                goodrow = false;
        }
        if (goodrow)
            for(size_t i = 0;i < _prices.size();++i){
                prices[i].push_back(_prices[i][j]);
            }
            goodrow = true;
        }
    unsigned int windowsize = prices[0].size()-2;
    std::shared_ptr<ComputeReturn> cr(new ComputeReturn(prices,1,windowsize,true));

    // create portfolio
  	Ptf _ptf;
  	for(size_t i = 0;i < m;++i){
        shared_ptr<Instrument> instrument(new DeltaOne());
        auto p = std::make_pair(i,instrument);
		    _ptf.push_back(p);
    }
	  shared_ptr<Portfolio> ptf(new Portfolio(_ptf, weights, cr, false, 1.e+07));

    HistoricalVaR model;

    // CVaRhistorical
    model.setAlpha(1.0-alphatest);
    double CVaRHistorical = model(0,ptf->getReturns());

    return CVaRHistorical;
}

}