#pragma once
#include <cmath>
#include <algorithm>
#include <limits>
#include <Eigen/Geometry>
#include <boost/optional.hpp>
#include "BaryCoordT.hh"
#include "util.hh"
#include "RangeAdaptor.hh"

namespace kt84 {
    namespace eigen_util {
        template <typename TMatrix>
        inline auto range_elements(TMatrix& m) -> RangeAdaptor<decltype(m.data())> {
            return make_RangeAdaptor(m.data(), m.data() + m.size());
        }
        template <typename TVector, typename TScalar>
        inline void push_back(TVector& v, TScalar s) {
            int n = v.size();
            v.conservativeResize(n + 1);
            v[n] = s;
        }
        template <typename TVector>
        inline void erase_at(TVector& v, int index) {
            int n = v.size();
            auto tmp(v);
            v.resize(n - 1);
            v << tmp.head(index), tmp.tail(n - 1 - index);
        }
        template <typename TVector>
        inline void swap_xy(TVector& v) {
            std::swap(v.x(), v.y());
        }
        template <typename TAlignedBox>
        inline void bbox_add_margin(TAlignedBox& bbox, double ratio, bool is_margin_square = false) {
            if (is_margin_square) {
                typedef typename TAlignedBox::VectorType VectorType;
                VectorType margin = VectorType::Ones();
                margin *= bbox.diagonal().norm() * ratio;
                bbox.max() += margin;
                bbox.min() -= margin;
            } else {
                bbox.extend((1 + ratio) * bbox.max() - ratio * bbox.min());
                bbox.extend((1 + ratio) * bbox.min() - ratio * bbox.max());
            }
        }
        inline Eigen::Vector2d bbox_bilinear(const Eigen::AlignedBox2d& bbox, double tx, double ty) {            // (0, 0) corresponds to bbox.min, (1, 1) corresponds to bbox.max
            double x = (1 - tx) * bbox.min().x() + tx * bbox.max().x();
            double y = (1 - ty) * bbox.min().y() + ty * bbox.max().x();
            return Eigen::Vector2d(x, y);
        }
        inline Eigen::Vector3d bbox_trilinear(const Eigen::AlignedBox3d& bbox, double tx, double ty, double tz) {            // (0, 0, 0) corresponds to bbox.min, (1, 1, 1) corresponds to bbox.max
            double x = (1 - tx) * bbox.min().x() + tx * bbox.max().x();
            double y = (1 - ty) * bbox.min().y() + ty * bbox.max().x();
            double z = (1 - tz) * bbox.min().z() + tz * bbox.max().z();
            return Eigen::Vector3d(x, y, z);
        }
        inline Eigen::Vector3d orientation_color(Eigen::Vector3d d) {
            auto cx = d.x() > 0 ? Eigen::Vector3d(1, 0, 0) : Eigen::Vector3d(0, 1, 1);
            auto cy = d.y() > 0 ? Eigen::Vector3d(0, 1, 0) : Eigen::Vector3d(1, 0, 1);
            auto cz = d.z() > 0 ? Eigen::Vector3d(0, 0, 1) : Eigen::Vector3d(1, 1, 0);
            d /= std::abs<double>(d.sum());
            d = d.cwiseAbs();
            return d.x() * cx + d.y() * cy + d.z() * cz;
        }
        inline Eigen::Vector3d heat_color(double t) {
            // t     | 0    | 0.25 | 0.5   | 0.75   | 1   |
            // color | blue | cyan | green | yellow | red |
            t = util::clamp(t, 0., 1.);
            Eigen::Vector3d colors[5] = {
                Eigen::Vector3d(0, 0, 1),
                Eigen::Vector3d(0, 1, 1),
                Eigen::Vector3d(0, 1, 0),
                Eigen::Vector3d(1, 1, 0),
                Eigen::Vector3d(1, 0, 0)
            };
            int i = t < 0.25 ? 0 : t < 0.5 ? 1 : t < 0.75 ? 2 : 3;
            double s = (t - i * 0.25) * 4;
            return (1 - s) * colors[i] + s * colors[i + 1];
        }
        template <int N>
        inline int closest_axis(const Eigen::Matrix<double, N, 1, 0, N, 1>& v) {
            int result = -1;
            double v_abs_max = 0;
            for (int i = 0; i < N; ++i) {
                double v_abs = std::abs(v[i]);
                if (v_abs_max < v_abs) {
                    v_abs_max = v_abs;
                    result = i;
                }
            }
            return result;
        }
        inline Eigen::Vector2d compute_gradient(const Eigen::Vector2d& x0, const Eigen::Vector2d& x1, const Eigen::Vector2d& x2, double y0, double y1, double y2) {
            /*
                a.x0 + b = y0
                a.x1 + b = y1
                a.x2 + b = y2
                -->
                a.(x1 - x0) = y1 - y0
                a.(x2 - x0) = y2 - y0
                -->
                |(x1 - x0)^T| * a = |y1 - y0|
                |(x2 - x0)^T|       |y2 - y0|
            */
            Eigen::Matrix2d A;
            A << Eigen::RowVector2d(x1 - x0),
                 Eigen::RowVector2d(x2 - x0);
            return A.inverse() * Eigen::Vector2d(y1 - y0, y2 - y0);
        }
        inline Eigen::Vector3d compute_gradient(const Eigen::Vector3d& x0, const Eigen::Vector3d& x1, const Eigen::Vector3d& x2, const Eigen::Vector3d& x3, double y0, double y1, double y2, double y3) {
            /*
                a.x0 + b = y0
                a.x1 + b = y1
                a.x2 + b = y2
                a.x3 + b = y3
                -->
                a.(x1 - x0) = y1 - y0
                a.(x2 - x0) = y2 - y0
                a.(x3 - x0) = y3 - y0
                -->
                |(x1 - x0)^T|       |y1 - y0|
                |(x2 - x0)^T| * a = |y2 - y0|
                |(x3 - x0)^T|       |y3 - y0|
            */
            Eigen::Matrix3d A;
            A << Eigen::RowVector3d(x1 - x0),
                 Eigen::RowVector3d(x2 - x0),
                 Eigen::RowVector3d(x3 - x0);
            return A.inverse() * Eigen::Vector3d(y1 - y0, y2 - y0, y3 - y0);
        }
        inline Eigen::Vector3d compute_gradient(const Eigen::Vector3d& x0, const Eigen::Vector3d& x1, const Eigen::Vector3d& x2, double y0, double y1, double y2) {
            /*
                Compute gradient restricted to the tangent vectors on the triangle x0-x1-x2.
                a.x0 + b = y0
                a.x1 + b = y1
                a.x2 + b = y2
                a.n      = 0                (n: normal)
                -->
                a.(x1 - x0) = y1 - y0
                a.(x2 - x0) = y2 - y0
                a.n         = 0
                -->
                |(x1 - x0)^T|       |y1 - y0|
                |(x2 - x0)^T| * a = |y2 - y0|
                | n       ^T|       |0      |
            */
            Eigen::Matrix3d A;
            A << Eigen::RowVector3d(x1 - x0),
                 Eigen::RowVector3d(x2 - x0),
                 Eigen::RowVector3d((x1 - x0).cross(x2 - x0));
            return A.inverse() * Eigen::Vector3d(y1 - y0, y2 - y0, 0);
        }
        inline Eigen::Vector2d rotate90(const Eigen::Vector2d& xy) { return Eigen::Vector2d(-xy[1], xy[0]); }
        inline double angle(const Eigen::Vector2d& d0, const Eigen::Vector2d& d1) { return std::atan2(rotate90(d0).dot(d1), d0.dot(d1)); }
        inline double angle(const Eigen::Vector3d& d0, const Eigen::Vector3d& d1) { return std::acos(d0.normalized().dot(d1.normalized())); }
        template <class T>
        inline void orthonormalize(const T& unit, T& p) {
            p -= unit.dot(p) * unit;
            p.normalize();
        }
        template <class T>
        inline bool project_to_line(const T& line_v0, const T& line_v1, const T& point, Eigen::Vector2d& t) {
            /*
                x0 := line_v0
                x1 := line_v1
                y := point
                compute t (which sums up to one) such that
                    | t[0] * x0 + t[1] * x1 - y |^2
                is minimized.
                ---------------------
                u := t[1]
                (1 - u) * x0 + u * x1 =~ y
                u =~ (y - x0).dot(x1 - x0) / (x1 - x0).squaredNorm()
            */

            double r = (line_v1 - line_v0).squaredNorm();
            if (r == 0)
                // degenerate
                return false;

            t[1] = (point - line_v0).dot(line_v1 - line_v0) / r;
            t[0] = 1 - t[1];

            return true;
        }

        template <class T>
        inline bool project_to_triangle(const T& triangle_v0, const T& triangle_v1, const T& triangle_v2, const T& point, Eigen::Vector3d& t) {
            /*
                x0 := triangle_v0
                x1 := triangle_v1
                x2 := triangle_v2
                y := point
                compute t (which sums up to one) such that
                    | t[0] * x0 + t[1] * x1 + t[2] * x2 - y |^2
                is minimized.
                ---------------------
                u := t[1]
                v := t[2]
                (1 - u - v) * x0 + u * x1 + v * x2 =~ y
                (x1-x0, x2-x0) * |u| =~ y-x0
                                 |v|
                |u| =~ |(x1-x0).squaredNorm(), (x1-x0).dot(x2-x0)   |^-1 * | (x1-x0).dot(y-x0) |
                |v|    |(x1-x0).dot(x2-x0)   , (x2-x0).squaredNorm()|      | (x2-x0).dot(y-x0) |
            */

            T d01 = triangle_v1 - triangle_v0;
            T d02 = triangle_v2 - triangle_v0;
            T d0p = point       - triangle_v0;
            Eigen::Matrix2d M;
            M <<
                d01.squaredNorm(), d01.dot(d02),
                d01.dot(d02)     , d02.squaredNorm();
            if (M.determinant() == 0)
                // degenerate
                return false;

            Eigen::Vector2d b;
            b <<
                d01.dot(d0p),
                d02.dot(d0p);

            Eigen::Vector2d uv = ((Eigen::Matrix2d)M.inverse()) * b;

            t[0] = 1 - uv[0] - uv[1];
            t[1] = uv[0];
            t[2] = uv[1];

            return true;
        }

        template <class T>
        inline boost::optional<double> distance_to_line(const T& line_v0, const T& line_v1, const T& point, bool do_clamp = false) {
            Eigen::Vector2d t;
            if (!project_to_line(line_v0, line_v1, point, t))
                // degenrate case
                return boost::none;

            if (do_clamp) {
                t[0] = util::clamp(t[0], 0.0, 1.0);
                t[1] = 1 - t[0];
            }

            return (t[0] * line_v0 + t[1] * line_v1 - point).norm();
        }

        template <class T>
        inline boost::optional<double> distance_to_triangle(const T& triangle_v0, const T& triangle_v1, const T& triangle_v2, const T& point, bool do_clamp = false) {
            Eigen::Vector3d t;
            if (!project_to_triangle(triangle_v0, triangle_v1, triangle_v2, point, t))
                // degenrate case
                return boost::none;

            if (do_clamp && t[0] < 0 || t[1] < 0 || t[2] < 0) {
                double d0 = *distance_to_line(triangle_v0, triangle_v1, point, true);
                double d1 = *distance_to_line(triangle_v1, triangle_v2, point, true);
                double d2 = *distance_to_line(triangle_v2, triangle_v0, point, true);
                return util::min(d0, d1, d2);
            }

            return (t[0] * triangle_v0 + t[1] * triangle_v1 + t[2] * triangle_v2 - point).norm();
        }
        inline double triangle_area(const Eigen::Vector2d& v0, const Eigen::Vector2d& v1, const Eigen::Vector2d& v2) {
            Eigen::Vector2d d1 = v1 - v0;
            Eigen::Vector2d d2 = v2 - v0;
            return d1.x() * d2.y() - d1.y() * d2.x();
        }
        inline double triangle_area(const Eigen::Vector3d& v0, const Eigen::Vector3d& v1, const Eigen::Vector3d& v2) {
            Eigen::Vector3d d1 = v1 - v0;
            Eigen::Vector3d d2 = v2 - v0;
            return d1.cross(d2).norm() / 2;
        }
        template <class T>
        inline bool intersection(const T& line0_p0, const T& line0_p1, const T& line1_p0, const T& line1_p1, double& line0_coordinate, double& line1_coordinate) {
            /*
                notation:
                    v0 := line0_p0
                    v1 := line0_p1
                    w0 := line1_p0
                    w1 := line1_p1
                    s  := line0_coordinate
                    t  := line1_coordinate
                seek for (s, t) which minimizes:
                    |(1 - s) * v0 + s * v1 - (1 - t) * w0 - t * w1|^2
                least square sense:
                    | v1-v0, -w1+w0 | * |s| =~ -v0+w0
                                        |t|
            */
            T d0 = line0_p1 - line0_p0;
            T d1 = line1_p1 - line1_p0;
            T e  = line1_p0 - line0_p0;
            double d0d0 = d0.squaredNorm();
            double d0d1 = d0.dot(d1);
            double d1d1 = d1.squaredNorm();
            Eigen::Matrix2d A;
            A <<
                d0d0, -d0d1,
                -d0d1, d1d1;

            if (A.determinant() == 0)
                // two lines are parallel
                return false;

            Eigen::Vector2d b(d0.dot(e), -d1.dot(e));
            Eigen::Vector2d st = ((Eigen::Matrix2d)A.inverse()) * b;

            line0_coordinate = st[0];
            line1_coordinate = st[1];
            return true;
        }
        template <typename T>
        inline std::vector<T> eigen_vectorx_to_std_vector(const Eigen::Matrix<T, -1, 1>& eigen_vectorx) {
            int n = eigen_vectorx.rows();
            std::vector<T> result(n);
            for (int i = 0; i < n; ++i)
                result[i] = eigen_vectorx[i];
            return result;
        }
        template <typename T>
        inline Eigen::Matrix<T, -1, 1> std_vector_to_eigen_vectorx(const std::vector<T>& std_vector) {
            int n = std_vector.size();
            Eigen::Matrix<T, -1, 1> result = Eigen::Matrix<T, -1, 1>::Zero(n);
            for (int i = 0; i < n; ++i)
                result[i] = std_vector[i];
            return result;
        }
        template <typename TVector>
        inline int max_axis(const TVector& v) {
            int index_max = -1;
            typename TVector::Scalar value_max = -std::numeric_limits<typename TVector::Scalar>::max();
            for (int i = 0; i < v.size(); ++i) {
                if (value_max < v[i]) {
                    value_max = v[i];
                    index_max = i;
                }
            }
            return index_max;
        }
        template <typename TVector>
        inline int min_axis(const TVector& v) {
            int index_min = -1;
            typename TVector::Scalar value_min = std::numeric_limits<typename TVector::Scalar>::max();
            for (int i = 0; i < v.size(); ++i) {
                if (value_min < v[i]) {
                    value_min = v[i];
                    index_min = i;
                }
            }
            return index_min;
        }
        template <typename TVector>
        inline void rotate(TVector& v) {
            TVector tmp(v);
            int n = v.size();
            for (int i = 0; i < n; ++i)
                v[i] = tmp[(i + 1) % n];
        }
        template <typename TVector>
        inline void reverse(TVector& v) {
            TVector tmp(v);
            int n = v.size();
            for (int i = 0; i < n; ++i)
                v[i] = tmp[n - 1 - i];
        }
    }
}


///
/// \file vandermonde_matrix.hpp
///
#ifndef MXPFIT_VANDERMONDE_MATRIX_HPP
#define MXPFIT_VANDERMONDE_MATRIX_HPP

#include <cassert>

#include <Eigen/Core>

#include <mxpfit/matrix_free_gemv.hpp>

namespace mxpfit
{

///
/// ### VandermondeMatrix
///
/// Expression of a rectangular column Vandermonde matrix.
///
/// \tparam T  Scalar type of matrix elements
///
/// This class represents a \f$ m \times n \f$ column Vandermonde matrix of
/// the form
///
/// \f[
///   \bm{V}(\bm{t}) = \left[ \begin{array}{cccc}
///     1         & 1         & \dots  & 1         \\
///     t_{1}^{}  & t_{2}^{}  & \dots  & t_{n}^{}  \\
///     \vdots    & \vdots    & \ddots & \vdots    \\
///     t_{1}^{m} & t_{2}^{m} & \dots  & t_{n}^{m} \\
///   \end{array} \right].
/// \f]
///
/// This class represents a Vandermonde matrix expression from the given number
/// of rows, \f$ m, \f$ and a vector expression for the coefficients of the
/// second row, \f$(t_1,t_2,\dots,t_n).\f$ If the given vector expression is
/// l-value, this class wraps the existing vector expression, otherwise storage
/// for coefficients are allocated.
///
/// This class also provides the interface for matrix-vector multiplication
/// compatible to `MatrixFreeGEMV`. For this purpose, the class also allocate
/// internal a vector of size \f$n\f$ as working space.
///

template <typename T>
class VandermondeMatrix
{
public:
    using Scalar          = T;
    using RealScalar      = typename Eigen::NumTraits<T>::Real;
    using StorageIndex    = Eigen::Index;
    using Index           = Eigen::Index;
    using CoeffsVector    = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    using CoeffsVectorRef = Eigen::Ref<const CoeffsVector>;
    using PlainObject = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;

private:
    Index m_rows;
    mutable CoeffsVector m_workspace;
    CoeffsVectorRef m_coeffs;

public:
    /// Default constructor: create an empty matrix
    VandermondeMatrix()
        : m_rows(),
          m_workspace(),
          m_coeffs(m_workspace) // need to initialize Eigen::Ref object
    {
    }

    ///
    /// Create a Vandermonde matrix from number of rows and coefficients of the
    /// first row.
    ///
    template <typename InputType>
    VandermondeMatrix(Index nrows, const InputType& coeffs)
        : m_rows(nrows), m_workspace(coeffs.size()), m_coeffs(coeffs)
    {
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(InputType);
    }

    /// Copy constructor
    VandermondeMatrix(const VandermondeMatrix& other)
        : m_rows(other.m_rows),
          m_workspace(other.m_workspace),
          m_coeffs(other.m_coeffs)
    {
    }

    /// Default destructor
    ~VandermondeMatrix()
    {
    }

    /// Delete assignment operator as a consequence that Eigen::Ref is
    /// non-assignable
    VandermondeMatrix& operator=(const VandermondeMatrix& other) = delete;

    /// \return the number of rows
    Index rows() const
    {
        return m_rows;
    }

    /// \return the number of columns
    Index cols() const
    {
        return m_coeffs.size();
    }

    /// Set elements of Vandermonde matrix
    template <typename Derived>
    void setMatrix(Index nrows, const Eigen::EigenBase<Derived>& coeffs)
    {
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
        m_rows = nrows;
        if (m_workspace.size() != coeffs.size())
        {
            m_workspace.resize(coeffs.size());
        }
        m_coeffs.~CoeffsVectorRef();
        ::new (&m_coeffs) CoeffsVectorRef(coeffs.derived());
    }

    void setMatrix(Index nrows, const CoeffsVectorRef& coeffs)
    {
        m_rows = nrows;
        if (&(coeffs.derived()) != &m_coeffs)
        {
            if (m_workspace.size() != coeffs.size())
            {
                m_workspace.resize(coeffs.size());
            }
            m_coeffs.~CoeffsVectorRef();
            ::new (&m_coeffs) CoeffsVectorRef(coeffs);
        }
    }

    ///
    /// \return A const reference to the coefficients of the second row
    ///
    const CoeffsVectorRef& coeffs() const
    {
        return m_coeffs;
    }

    ///
    /// \return The same Vandermonde matrix in dense form.
    ///
    PlainObject toDenseMatrix() const
    {
        PlainObject ret(rows(), cols());
        for (Index j = 0; j < cols(); ++j)
        {
            auto x = m_coeffs(j);
            auto v = Scalar(1);
            ret(0, j) = v;
            for (Index i = 1; i < rows(); ++i)
            {
                v *= x;
                ret(i, j) = v;
            }
        }
        return ret;
    }

    /// Compute matrix-vector product of the form `dst += alpha * A * rhs`
    template <typename Dest, typename RHS>
    void apply(Dest& dst, const Eigen::MatrixBase<RHS>& rhs, Scalar alpha) const
    {
        apply_impl(dst, rhs, alpha, coeffs());
    }

    /// Compute matrix-vector product of the form
    /// `dst += alpha * A.conjugate() * rhs`
    template <typename Dest, typename RHS>
    void applyConjugate(Dest& dst, const Eigen::MatrixBase<RHS>& rhs,
                        Scalar alpha) const
    {
        apply_impl(dst, rhs, alpha, coeffs().conjugate());
    }

    /// Compute matrix-vector product of the form
    /// `dst += alpha * A.transpose() * rhs`
    template <typename Dest, typename RHS>
    void applyTranspose(Dest& dst, const Eigen::MatrixBase<RHS>& rhs,
                        Scalar alpha) const
    {
        apply_transpose_impl(dst, rhs, alpha, coeffs());
    }

    /// Compute matrix-vector product of the form
    /// `dst += alpha * A.adjoint() * rhs`
    template <typename Dest, typename RHS>
    void applyAdjoint(Dest& dst, const Eigen::MatrixBase<RHS>& rhs,
                      Scalar alpha) const
    {
        apply_transpose_impl(dst, rhs, alpha, coeffs().conjugate());
    }

private:
    template <typename Dest, typename RHS, typename CoeffsV>
    void apply_impl(Dest& dst, const Eigen::MatrixBase<RHS>& rhs, Scalar alpha,
                    const Eigen::MatrixBase<CoeffsV>& coeffs) const
    {
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(Dest);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(RHS);

        assert(dst.size() == rows());
        assert(rhs.size() == cols());

        if (alpha == Scalar(/*zero*/))
        {
            return;
        }

        m_workspace = rhs.derived();

        dst(0) += alpha * m_workspace.sum();
        for (Index i = 1; i < rows(); ++i)
        {
            m_workspace.array() *= coeffs.array();
            dst(i) += alpha * m_workspace.sum();
        }
    }

    template <typename Dest, typename RHS, typename CoeffsV>
    void apply_transpose_impl(Dest& dst, const Eigen::MatrixBase<RHS>& rhs,
                              Scalar alpha,
                              const Eigen::MatrixBase<CoeffsV>& coeffs) const
    {
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(Dest);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(RHS);

        assert(dst.size() == cols());
        assert(rhs.size() == rows());

        for (Index i = 0; i < cols(); ++i)
        {
            auto x = coeffs(i);
            // Evaluate polynomial using Honer's method
            auto s = Scalar();
            for (Index j = 0; j < rows(); ++j)
            {
                s = s * x + rhs(rows() - j - 1);
            }

            dst(i) += alpha * s;
        }
    }
};

} // namespace mxpfit

#endif /* MXPFIT_VANDERMONDE_MATRIX_HPP */


#pragma once
#include <iostream>

#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <unsupported/Eigen/NonLinearOptimization>
//#include <unsupported/Eigen/LevenbergMarquardt>

// LM minimize for the model y = a w + b x + c y + d z
//typedef std::vector<Eigen::Vector4d,Eigen::aligned_allocator<Eigen::Vector4d> > Point4DVector;
typedef std::vector<Eigen::Vector4d > Point4DVector;
typedef std::vector<Eigen::Vector3d > Point3DVector;
typedef std::vector<Eigen::Matrix3d > Matrix3DVector;
typedef std::vector<Eigen::Matrix4d > Matrix4DVector;
typedef std::vector<Eigen::Affine3d > Affine3DVector;
typedef std::vector<Eigen::Affine3f > Affine3fVector;

typedef std::vector<Eigen::Vector3d > Vector3dVector;
typedef std::vector<Eigen::Quaterniond > QuaterniondVector;

typedef Eigen::Matrix<double,7,1> Vector7d ;
typedef std::vector<Vector7d > Vector7dVector;

// Generic functor
template<typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
struct Functor
{
typedef _Scalar Scalar;
enum {
    InputsAtCompileTime = NX,
    ValuesAtCompileTime = NY
};
typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;

int m_inputs, m_values;

Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

int inputs() const { return m_inputs; }
int values() const { return m_values; }

};

void NormalizeQ(Eigen::Quaterniond &Q);
void Affine3d_From_Qt( Eigen::Affine3d& T_a2b, Eigen::Quaterniond Q_a2b,Eigen::Vector3d t_a2b );
void Affine3d_To_Vector( const Eigen::Affine3d& T,Eigen::Matrix<double,16,1>& V );
void Vector_To_Affine3d(  const Eigen::VectorXd& V ,Eigen::Affine3d& T);

struct Q_bc_Functor : Functor<double>
{
  int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
  {
      // 0  = w * p[0] + x * p[1] + y * p[2] + z * p[3]
    for(unsigned int i = 0; i < this->Mct_sub_Mb.size(); ++i)
      {
      fvec(i) = this->Mct_sub_Mb[i](0) * x(0) + this->Mct_sub_Mb[i](1) * x(1) +
              this->Mct_sub_Mb[i](2) * x(2) + this->Mct_sub_Mb[i](3) * x(3) +
              (pow( x(0),2 ) + pow( x(1),2 ) + pow( x(2),2 ) + pow( x(3),2 ) - 1)*1;
      }
    return 0;
  }

  Point4DVector Mct_sub_Mb;

  int inputs() const { return 2; } // There are two parameters of the model
  int values() const { return this->Mct_sub_Mb.size(); } // The number of observations
};

struct Q_bc_FunctorNumericalDiff : Eigen::NumericalDiff<Q_bc_Functor> {};

struct T_bc_Functor : Functor<double>
{
    Eigen::VectorXd t_temp_N;
    Eigen::MatrixXd R_temp_N;

    int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
    {
        fvec = t_temp_N - R_temp_N * x;
        return 0;
    }
    int inputs() const { return 2; } // There are two parameters of the model
    int values() const { return t_temp_N.rows(); } // The number of observations
};
struct T_bc_FunctorNumericalDiff : Eigen::NumericalDiff<T_bc_Functor> {};

Point4DVector GeneratePoints(const Eigen::MatrixXd M);

struct T_rw_Functor : Functor<double>
{
    QuaterniondVector Q_r2w_V;
    Vector3dVector t_r2w_V;
    int operator()(const Eigen::VectorXd &Q_t_r2w, Eigen::VectorXd &fvec) const
    {
        for(int i=0;i<Q_r2w_V.size();i++)
        {
            fvec(i*7+0) = Q_r2w_V.at(i).x() - Q_t_r2w(0);
            fvec(i*7+1) = Q_r2w_V.at(i).y() - Q_t_r2w(1);
            fvec(i*7+2) = Q_r2w_V.at(i).z() - Q_t_r2w(2);
            fvec(i*7+3) = Q_r2w_V.at(i).w() - Q_t_r2w(3);

            for(int j=0;j<3;j++)
            {
                fvec(i*7+4+j) = t_r2w_V.at(i)(j) - Q_t_r2w(j+4);
            }
        }
    }
    int inputs() const { return 2; } // There are two parameters of the model
    int values() const { return Q_r2w_V.size()*7; } // The number of observations
};
struct T_rw_FunctorNumericalDiff : Eigen::NumericalDiff<T_rw_Functor> {};

struct InCal_Functor : Functor<double>
{
    Affine3DVector T_w2b_V,T_r2c_V;
    int operator()(const Eigen::VectorXd &T_In, Eigen::VectorXd &fvec) const
    {
//        Eigen::Quaterniond Q_r2w( T_In(3),T_In(0),T_In(1),T_In(2) ) ,Q_b2c( T_In(3+7),T_In(0+7),T_In(1+7),T_In(2+7) );
//        Eigen::Vector3d t_r2w( T_In(4),T_In(5),T_In(6) ) ,t_b2c( T_In(4+7),T_In(5+7),T_In(6+7) );

        Eigen::Affine3d T_r2w,T_c2b;
//        Affine3d_From_Qt(T_r2w,Q_r2w,t_r2w);
//        Affine3d_From_Qt(T_b2c,Q_b2c,t_b2c);
        Eigen::VectorXd V_r2w,V_c2b;
        V_r2w = T_In.block<16,1>(0,0);
        V_c2b = T_In.block<16,1>(16,0);

        Vector_To_Affine3d(V_r2w,T_r2w);
        Vector_To_Affine3d(V_c2b,T_c2b);

        for(int i=0;i<T_w2b_V.size();i++)
        {
            Eigen::Affine3d T_r2b_1 = T_c2b * T_r2c_V.at(i);
            Eigen::Affine3d T_r2b_2 = T_w2b_V.at(i) * T_r2w;

            Eigen::Matrix4d T_err = T_r2b_1.matrix() - T_r2b_2.matrix();

//            Eigen::Affine3d T_r2r =  T_r2b_1.inverse() * T_r2b_2;
//            Eigen::Matrix4d T_err = T_r2r.matrix() - Eigen::Affine3d::Identity().matrix();

            for(int j=0;j<4;j++)
            {
                for(int k=0;k<4;k++)
                {
                    fvec(i*16+j*4+k) = T_err(j,k);
                }
            }
        }
    }
    int inputs() const { return 2; } // There are two parameters of the model
    int values() const { return T_w2b_V.size()*16; } // The number of observations
};
struct Incal_FunctorNumericalDiff : Eigen::NumericalDiff<InCal_Functor> {};

int Cal_Q_bc(Eigen::MatrixXf M,Eigen::VectorXf& xf);
int Cal_T_bc(Eigen::VectorXf t_temp_N,Eigen::MatrixXf R_temp_N,Eigen::VectorXf& t_b2c);
int InstallCalib_LM( Eigen::Affine3f &T_b2c,  Eigen::Affine3f &T_r2w, Affine3fVector& T_r2c_V,Affine3fVector& T_w2b_V);
int Cal_T_rw(Eigen::Affine3f& T_r2w,Affine3fVector& T_r2w_V_f);


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/FluidEigenMappings.hpp"
#include "../util/NNFuncs.hpp"
#include "../util/NNLayer.hpp"
#include "../../data/FluidDataSet.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/FluidTensor.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <random>

namespace fluid {
namespace algorithm {

class MLP
{
  using ArrayXd = Eigen::ArrayXd;
  using ArrayXXd = Eigen::ArrayXXd;

public:
  explicit MLP() = default;
  ~MLP() = default;

  void init(index inputSize, index outputSize,
            FluidTensor<index, 1> hiddenSizes, index hiddenAct, index outputAct)
  {
    mLayers.clear();
    std::vector<index> sizes = {inputSize};
    std::vector<index> activations = {};
    for (auto&& s : hiddenSizes)
    {
      sizes.push_back(s);
      activations.push_back(hiddenAct);
    }
    sizes.push_back(outputSize);
    activations.push_back(outputAct);
    for (index i = 0; i < asSigned(sizes.size() - 1); i++)
    {
      mLayers.push_back(NNLayer(sizes[asUnsigned(i)], sizes[asUnsigned(i + 1)],
                                activations[asUnsigned(i)]));
    }
    for (auto&& l : mLayers) l.init();
    mInitialized = true;
    mTrained = false;
  }

  void getParameters(index layer, RealMatrixView W, RealVectorView b,
                     index& layerType) const
  {
    using namespace _impl;
    W = asFluid(mLayers[asUnsigned(layer)].getWeights());
    b = asFluid(mLayers[asUnsigned(layer)].getBiases());
    layerType = mLayers[asUnsigned(layer)].getActType();
  }

  void setParameters(index layer, RealMatrixView W, RealVectorView b,
                     index layerType)
  {
    using namespace Eigen;
    using namespace std;
    using namespace _impl;
    MatrixXd weights = asEigen<Matrix>(W);
    VectorXd biases = asEigen<Matrix>(b);
    mLayers[asUnsigned(layer)].init(weights, biases, layerType);
  }

  void clear()
  {
    for (auto&& l : mLayers) l.init();
    mInitialized = false;
    mTrained = false;
  }

  double loss(ArrayXXd pred, ArrayXXd out)
  {
    assert(pred.rows() == out.rows());
    return (pred - out).square().sum() / out.rows();
  }

  void process(RealMatrixView in, RealMatrixView out, index startLayer,
               index endLayer)
  {
    using namespace _impl;
    using namespace Eigen;
    ArrayXXd input = asEigen<Eigen::Array>(in);
    ArrayXXd output = ArrayXXd::Zero(out.rows(), out.cols());
    forward(input, output, startLayer, endLayer);
    out = asFluid(output);
  }

  void processFrame(RealVectorView in, RealVectorView out, index startLayer,
                    index endLayer)
  {
    using namespace _impl;
    using namespace Eigen;
    ArrayXd  tmpIn = asEigen<Eigen::Array>(in);
    ArrayXXd input(1, tmpIn.size());
    input.row(0) = tmpIn;
    ArrayXXd output = ArrayXXd::Zero(1, out.size());
    forward(input, output, startLayer, endLayer);
    ArrayXd tmpOut = output.row(0);
    out = asFluid(tmpOut);
  }

  void forward(Eigen::Ref<ArrayXXd> in, Eigen::Ref<ArrayXXd> out)
  {
    forward(in, out, 0, asSigned(mLayers.size()));
  }

  void forward(Eigen::Ref<ArrayXXd> in, Eigen::Ref<ArrayXXd> out,
               index startLayer, index endLayer)
  {
    if (startLayer >= asSigned(mLayers.size()) ||
        endLayer > asSigned(mLayers.size()))
      return;
    if (startLayer < 0 || endLayer <= 0) return;
    ArrayXXd input = in;
    ArrayXXd output;
    for (index i = startLayer; i < endLayer; i++)
    {
      auto&& l = mLayers[asUnsigned(i)];
      output = ArrayXXd::Zero(input.rows(), l.outputSize());
      l.forward(input, output);
      input = output;
    }
    out = output;
  }

  void backward(Eigen::Ref<ArrayXXd> out)
  {
    index    nRows = out.rows();
    ArrayXXd chain =
        ArrayXXd::Zero(nRows, mLayers[mLayers.size() - 1].inputSize());
    mLayers[mLayers.size() - 1].backward(out, chain);
    for (index i = asSigned(mLayers.size() - 2); i >= 0; i--)
    {
      ArrayXXd tmp = ArrayXXd::Zero(nRows, mLayers[asUnsigned(i)].inputSize());
      mLayers[asUnsigned(i)].backward(chain, tmp);
      chain = tmp;
    }
  }

  void update(double learningRate, double momentum)
  {
    for (auto&& l : mLayers) l.update(learningRate, momentum);
  }

  index size() const { return asSigned(mLayers.size()); }
  bool  trained() const { return mTrained; }
  void  setTrained(bool val) { mTrained = val; }
  index initialized() const { return mInitialized; }

  // 0 = size of the input, 1 = output size of first hidden
  index outputSize(index layer) const
  {
    if (layer == 0) return mLayers[0].inputSize();
    if (layer < 0 || layer > asSigned(mLayers.size())) return 0;
    return mLayers[asUnsigned(layer - 1)].outputSize();
  }

  index inputSize(index layer) const
  {
    return (layer >= asSigned(mLayers.size()) || layer < 0)
               ? 0
               : mLayers[asUnsigned(layer)].inputSize();
  }

  index dims() const
  {
    return mLayers.size() == 0 ? 0 : mLayers[0].inputSize();
  }

  std::vector<NNLayer> mLayers;
  bool                 mInitialized{false};
  bool                 mTrained{false};
};
} // namespace algorithm
} // namespace fluid


// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ASTRONOMY_COORDINATE_ARITHMETIC_HPP
#define BOOST_ASTRONOMY_COORDINATE_ARITHMETIC_HPP

#include <type_traits>

#include <boost/units/quantity.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/arithmetic/cross_product.hpp>
#include <boost/geometry/arithmetic/dot_product.hpp>
#include <boost/geometry/algorithms/transform.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/units/conversion.hpp>

#include <boost/astronomy/coordinate/base_representation.hpp>
#include <boost/astronomy/coordinate/cartesian_representation.hpp>

namespace boost { namespace astronomy { namespace coordinate {

namespace bg = boost::geometry;
namespace bu = boost::units;

//!Returns the cross product of representation1 and representation2
template
<
    template<typename ...> class Representation1,
    template<typename ...> class Representation2,
    typename ...Args1,
    typename ...Args2
>
auto cross
(
    Representation1<Args1...> const& representation1,
    Representation2<Args2...> const& representation2
)
{
    /*!both the coordinates/vector are first converted into
    cartesian coordinate system then cross product of both cartesian
    vectors is converted into requested type and returned*/

    /*checking types if it is not subclass of
    base_representaion then compile time erorr is generated*/
    //BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
    //    <
    //        boost::astronomy::coordinate::base_representation,
    //        Representation1<Args1...>
    //    >::value),
    //    "First argument type is expected to be a representation class");
    //BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
    //    <
    //        boost::astronomy::coordinate::base_representation,
    //        Representation2<Args2...>
    //    >::value),
    //    "Second argument type is expected to be a representation class");

    /*converting both coordinates/vector into cartesian system*/

    typedef Representation1<Args1...> representation1_type;
    typedef Representation2<Args2...> representation2_type;

    bg::model::point
    <
        typename std::conditional
        <
            sizeof(typename representation2_type::type) >=
                sizeof(typename representation1_type::type),
            typename representation2_type::type,
            typename representation1_type::type
        >::type,
        3,
        bg::cs::cartesian
    > tempPoint1, tempPoint2, result;

    bg::transform(representation1.get_point(), tempPoint1);
    bg::transform(representation2.get_point(), tempPoint2);

    bg::set<0>(result, (bg::get<1>(tempPoint1)*bg::get<2>(tempPoint2)) -
        ((bg::get<2>(tempPoint1)*
        bu::conversion_factor(typename representation1_type::quantity3::unit_type(),
        typename representation1_type::quantity2::unit_type()))*
        (bg::get<1>(tempPoint2)*
        bu::conversion_factor(typename representation2_type::quantity2::unit_type(),
        typename representation2_type::quantity3::unit_type()))));

    bg::set<1>(result, (bg::get<2>(tempPoint1)*bg::get<0>(tempPoint2)) -
        ((bg::get<0>(tempPoint1)*
        bu::conversion_factor(typename representation1_type::quantity1::unit_type(),
        typename representation1_type::quantity3::unit_type()))*
        (bg::get<2>(tempPoint2)*
        bu::conversion_factor(typename representation2_type::quantity3::unit_type(),
        typename representation2_type::quantity1::unit_type()))));

    bg::set<2>(result, (bg::get<0>(tempPoint1)*bg::get<1>(tempPoint2)) -
        ((bg::get<1>(tempPoint1)*
        bu::conversion_factor(typename representation1_type::quantity2::unit_type(),
        typename representation1_type::quantity1::unit_type()))*
        (bg::get<0>(tempPoint2)*
        bu::conversion_factor(typename representation2_type::quantity1::unit_type(),
        typename representation2_type::quantity2::unit_type()))));

    return Representation1
        <
            typename representation1_type::type,
            bu::quantity<typename bu::multiply_typeof_helper
            <
                typename representation1_type::quantity2::unit_type,
                typename representation2_type::quantity3::unit_type>::type
            >,
            bu::quantity<typename bu::multiply_typeof_helper
            <
                typename representation1_type::quantity3::unit_type,
                typename representation2_type::quantity1::unit_type>::type
            >,
            bu::quantity<typename bu::multiply_typeof_helper
            <
                typename representation1_type::quantity1::unit_type,
                typename representation2_type::quantity2::unit_type>::type
            >
        >(result);
}

//! Returns dot product of representation1 and representation2
template<typename Representation1, typename Representation2>
auto dot(Representation1 const& representation1, Representation2 const& representation2)
{
    /*!both the coordinates/vector are first converted into
    cartesian coordinate system then dot product of both cartesian
    product is converted into requested type and returned*/

    /*converting both coordinates/vector into cartesian system*/
    bg::model::point
    <
        typename std::conditional
        <
            sizeof(typename Representation2::type) >=
                sizeof(typename Representation1::type),
            typename Representation2::type,
            typename Representation1::type
        >::type,
        3,
        bg::cs::cartesian
    > tempPoint1, tempPoint2;

    auto cartesian1 = make_cartesian_representation(representation1);
    auto cartesian2 = make_cartesian_representation(representation2);

    typedef decltype(cartesian1) cartesian1_type;
    typedef decltype(cartesian2) cartesian2_type;

    bg::set<0>(tempPoint1, cartesian1.get_x().value());
    bg::set<1>(tempPoint1,
        static_cast<typename cartesian1_type::quantity1>(cartesian1.get_y()).value());
    bg::set<2>(tempPoint1,
        static_cast<typename cartesian1_type::quantity1>(cartesian1.get_z()).value());

    bg::set<0>(tempPoint2, cartesian2.get_x().value());
    bg::set<1>(tempPoint2,
        static_cast<typename cartesian2_type::quantity1>(cartesian2.get_y()).value());
    bg::set<2>(tempPoint2,
        static_cast<typename cartesian2_type::quantity1>(cartesian2.get_z()).value());

    return bg::dot_product(tempPoint1, tempPoint2) *
        typename cartesian1_type::quantity1::unit_type() *
        typename cartesian2_type::quantity1::unit_type();
}

//! Returns magnitude of the cartesian vector
template
<
    typename CoordinateType,
    typename XQuantity,
    typename YQuantity,
    typename ZQuantity
>
auto magnitude
(
    cartesian_representation
    <
        CoordinateType,
        XQuantity,
        YQuantity,
        ZQuantity
    > const& vector
)
{
    CoordinateType result = 0;
    bg::model::point
    <
        CoordinateType,
        3,
        bg::cs::cartesian
    > tempPoint;

    bg::set<0>(tempPoint, vector.get_x().value());
    bg::set<1>(tempPoint, static_cast<XQuantity>(vector.get_y()).value());
    bg::set<2>(tempPoint, static_cast<XQuantity>(vector.get_z()).value());

    result += std::pow(bg::get<0>(tempPoint), 2) +
        std::pow(bg::get<1>(tempPoint), 2) +
        std::pow(bg::get<2>(tempPoint), 2);

    return std::sqrt(result) * typename XQuantity::unit_type();
}

//! Returns magnitude of the vector other than cartesian
template <typename Coordinate>
auto magnitude(Coordinate const& vector)
{
    return bg::get<2>(vector.get_point()) * typename Coordinate::quantity3::unit_type();
}

//! Returns the unit vector of vector given
template <typename ...Args>
cartesian_representation<Args...>
unit_vector(cartesian_representation<Args...> const& vector)
{
    bg::model::point
    <
        typename cartesian_representation<Args...>::type,
        3,
        bg::cs::cartesian
    > tempPoint;
    auto mag = magnitude(vector); //magnitude of vector

    //performing calculations to find unit vector
    bg::set<0>(tempPoint, vector.get_x().value() / mag.value());
    bg::set<1>(tempPoint,
        vector.get_y().value() /
        static_cast<typename cartesian_representation<Args...>::quantity2>(mag).value());
    bg::set<2>(tempPoint,
        vector.get_z().value() /
        static_cast<typename cartesian_representation<Args...>::quantity3>(mag).value());

    return cartesian_representation<Args...>(tempPoint);
}

//! Returns unit vector of given vector other than Cartesian
template <typename Coordinate>
auto unit_vector(Coordinate const& vector)
{
    Coordinate tempVector;

    tempVector.set_lat(vector.get_lat());
    tempVector.set_lon(vector.get_lon());
    tempVector.set_dist(1.0 * typename Coordinate::quantity3::unit_type());

    return tempVector;
}

//! Returns sum of representation1 and representation2
template<typename Representation1, typename Representation2>
Representation1 sum
(
    Representation1 const& representation1,
    Representation2 const& representation2
)
{
    /*!both the coordinates/vector are first converted into
    cartesian coordinate system then sum of both cartesian
    vectors is converted into the type of first argument and returned*/

    /*checking types if it is not subclass of
    base_representaion then compile time erorr is generated*/
    //BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
    //    <
    //        boost::astronomy::coordinate::base_representation,
    //        Representation1
    //    >::value),
    //    "First argument type is expected to be a representation class");
    //BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
    //    <
    //        boost::astronomy::coordinate::base_representation,
    //        Representation2
    //    >::value),
    //    "Second argument type is expected to be a representation class");

    /*converting both coordinates/vector into cartesian system*/
    bg::model::point
    <
        typename std::conditional
        <
            sizeof(typename Representation2::type) >=
                sizeof(typename Representation1::type),
            typename Representation2::type,
            typename Representation1::type
        >::type,
        3,
        bg::cs::cartesian
    > result;

    auto cartesian1 = make_cartesian_representation(representation1);
    auto cartesian2 = make_cartesian_representation(representation2);

    typedef decltype(cartesian1) cartesian1_type;

    //performing calculation to find the sum
    bg::set<0>(result, (cartesian1.get_x().value() +
        static_cast<typename cartesian1_type::quantity1>(cartesian2.get_x()).value()));
    bg::set<1>(result, (cartesian1.get_y().value() +
        static_cast<typename cartesian1_type::quantity2>(cartesian2.get_y()).value()));
    bg::set<2>(result, (cartesian1.get_z().value() +
        static_cast<typename cartesian1_type::quantity3>(cartesian2.get_z()).value()));

    return Representation1(result);
}

//! Returns mean of representation1 and representation2
template<typename Representation1, typename Representation2>
Representation1 mean
(
    Representation1 const& representation1,
    Representation2 const& representation2
)
{

    /*!both the coordinates/vector are first converted into
    cartesian coordinate system then mean of both cartesian
    vectors is converted into the type of first argument and returned*/

    /*checking return type if it is not subclass of
    base_representaion then compile time erorr is generated*/
    //BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
    //    <boost::astronomy::coordinate::base_representation, ReturnType>::value),
    //    "return type is expected to be a representation class");

    /*converting both coordinates/vector into cartesian system*/
    bg::model::point
    <
        typename std::conditional
        <
            sizeof(typename Representation2::type) >=
                sizeof(typename Representation1::type),
            typename Representation2::type,
            typename Representation1::type
        >::type,
        3,
        bg::cs::cartesian
    > result;

    auto cartesian1 = make_cartesian_representation(representation1);
    auto cartesian2 = make_cartesian_representation(representation2);

    typedef decltype(cartesian1) cartesian1_type;

    //performing calculation to find the mean
    bg::set<0>(result, (cartesian1.get_x().value() +
        static_cast<typename cartesian1_type::quantity1>(cartesian2.get_x()).value())/2);
    bg::set<1>(result, (cartesian1.get_y().value() +
        static_cast<typename cartesian1_type::quantity2>(cartesian2.get_y()).value())/2);
    bg::set<2>(result, (cartesian1.get_z().value() +
        static_cast<typename cartesian1_type::quantity3>(cartesian2.get_z()).value())/2);

    return Representation1(result);
}

}}} // namespace boost::astronomy::coordinate
#endif // !BOOST_ASTRONOMY_COORDINATE_ARITHMETIC_HPP


#include <boost/math/tools/roots.hpp>
#include <boost/math/distributions/binomial.hpp>

#include <sys/types.h>
#include <sys/stat.h>
#include <time.h>
#include <sstream>
#include <unordered_map>
#include <iostream>

using namespace std;
extern "C" double tau_c(double edit_error, int kmer_size, double MAX_ERROR,double MAX_EDIT_ERROR)
{
	// cout << MAX_EDIT_ERROR << "\t" << (MAX_ERROR - MAX_EDIT_ERROR) << endl;
	const double ERROR_RATIO = (MAX_ERROR - MAX_EDIT_ERROR) / MAX_EDIT_ERROR;

	double gap_error = std::min(1.0, ERROR_RATIO * edit_error);
	double a = (1 - gap_error) / (1 + gap_error);
	double b = 1 / (2 * std::exp(kmer_size * edit_error) - 1);
	return a * b;
}

extern "C" double solve_inverse_jaccard_c(int j, int kmer_size, double MAX_ERROR,double MAX_EDIT_ERROR)
{
	if (j == 0)
	{
		return 1;
	}
	if (j == 1)
	{
		return 0;
	}
	return boost::math::tools::newton_raphson_iterate([j, kmer_size,MAX_ERROR,MAX_EDIT_ERROR](double d){
		const double ERROR_RATIO = (MAX_ERROR - MAX_EDIT_ERROR) / MAX_EDIT_ERROR;
		double E = exp(d * kmer_size);
		return make_tuple(
			((1 - d * ERROR_RATIO) / (1 + d * ERROR_RATIO)) * (1.0 / (2 * E - 1)) - j,
			2 * (- kmer_size * E + ERROR_RATIO - 2 * ERROR_RATIO * E + E * kmer_size * pow(d * ERROR_RATIO, 2)) /
				pow((2 * E - 1) * (1 + d * ERROR_RATIO), 2)
		);
	}, 0.10, 0.0, 1.0, numeric_limits<double>::digits);
}

extern "C" double relaxed_jaccard_estimate_c(int s, int kmer_size, double  MAX_EDIT_ERROR, double MAX_ERROR, double result)
{

	using namespace boost::math;
	const double CI = 0.75;
	const double Q2 = (1.0 - CI) / 2; // one side interval probability
	// cout << "tau" << endl;
	result = ceil(s * tau_c(MAX_EDIT_ERROR, kmer_size,MAX_ERROR,MAX_EDIT_ERROR));
	// cout << result << endl;
	for (; result >= 0; result--) {

		double d = solve_inverse_jaccard_c(result / s, kmer_size,MAX_ERROR,MAX_EDIT_ERROR); // returns edit error

		double x = quantile(complement(binomial(s, tau_c(d, kmer_size,MAX_ERROR,MAX_EDIT_ERROR)), Q2)); // inverse binomial
		double low_d = solve_inverse_jaccard_c(x / s, kmer_size,MAX_ERROR,MAX_EDIT_ERROR);
		if (100 * (1 - low_d) < MAX_EDIT_ERROR) {
			result++;
			break;
		}
	}

	result = max(result, 0.0);
	return result;
}

extern "C" int fun1(int a){
	return a*4;
}


/*
gammaGenerator.cpp - This file is part of the Bayesembler (v1.1.1)

The MIT License (MIT)

Copyright (c) 2014 Lasse Maretty and Jonas Andreas Sibbesen

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include <math.h>
#include <gammaGenerator.h>
#include <boost/math/constants/constants.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/math/distributions/gamma.hpp>
#include <boost/math/distributions/lognormal.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/lognormal_distribution.hpp>

/*
 Gamma-sampler class
 */

SymmetricGammaGenerator::SymmetricGammaGenerator(HyperPrior * hyper_prior_in, int slice_iterations_in, double slice_window_size_in, int slice_max_windows_in, boost::random::mt19937* mt_rng_pt_in) {

    hyper_prior = hyper_prior_in;
    slice_iterations = slice_iterations_in;
    slice_window_size = slice_window_size_in;
    slice_max_windows = slice_max_windows_in;
	mt_rng_pt = mt_rng_pt_in;
}

// Init gamma with sample from the hyperprior
double SymmetricGammaGenerator::initGamma() {

	return hyper_prior->init();
}

double SymmetricGammaGenerator::generateGamma(ExpressionValueContainer expression) {

	vector<double> expression_s_plus;

	for (int i = 0; i < expression.getPlusSize(); i++) {

		int idx = expression.getPlus(i);
		double value = expression.getValue(idx);
		expression_s_plus.push_back(value);
    }

    // Init gamma and uniform sampler
    uniform_01_sampler_t sample_uniform_01(mt_rng_pt);

    double gamma_current = sample_uniform_01();

	if (gamma_current < double_underflow) {

		gamma_current = double_underflow;
	}

	double gamma = gamma_current;

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

        double y = calculateLogDensity(expression_s_plus, gamma_current) + hyper_prior->calculateLogDensity(gamma_current) + log(1-sample_uniform_01());

        // Find slice by "step-out"
        double left = gamma_current - sample_uniform_01() * slice_window_size;
        double right = left + slice_window_size;

        int j = floor(slice_max_windows*sample_uniform_01());
        int k = slice_max_windows-1-j;

		// Truncate at zero
        if (left < double_underflow) {

            left = double_underflow;
            j = 0;
        }

        while (j > 0 && y < (calculateLogDensity(expression_s_plus, left) + hyper_prior->calculateLogDensity(left))) {

			left = left - slice_window_size;
            j--;

            if (left < double_underflow) {

                left = double_underflow;
                break;
            }
		}

        // Expand window to the right
        while (k > 0 && y < (calculateLogDensity(expression_s_plus, right) + hyper_prior->calculateLogDensity(right))) {

			right = right + slice_window_size;
            k--;
        }

		// Sample from the window until in slice
		gamma = sample_uniform_01()*(right-left) + left;

		while ( y >= (calculateLogDensity(expression_s_plus, gamma) + hyper_prior->calculateLogDensity(gamma))) {

			if (gamma < gamma_current) {

				left = gamma;
				gamma = sample_uniform_01()*(right-left) + left;

			} else {

				right = gamma;
				gamma = sample_uniform_01()*(right-left) + left;
			}
		}

		gamma_current = gamma;
	}

    return gamma;

}

double SymmetricGammaGenerator::calculateLogDensity(vector<double>& expression, double gamma) {

    int size = expression.size();

    // Init prob with normalisation constant
    double prob = boost::math::lgamma(size*gamma) - size*boost::math::lgamma(gamma);

    for (int i=0; i < size; i++) {
        prob += (gamma-1)*log(expression[i]);
    }

    return prob;
}

FixedGammaGenerator::FixedGammaGenerator(double gamma_in) {
    gamma = gamma_in;
}

double FixedGammaGenerator::initGamma() {

	return gamma;
}

double FixedGammaGenerator::generateGamma(ExpressionValueContainer expression) {

    return gamma;
}

GammaHyperPrior::GammaHyperPrior(double shape_in, double scale_in, mt_rng_pt_t mt_rng_pt_in) {

    shape = shape_in;
    scale = scale_in;
	mt_rng_pt = mt_rng_pt_in;
};

double GammaHyperPrior::init() {

	boost::random::gamma_distribution<> gamma_dist(shape, scale);
	boost::random::variate_generator<boost::random::mt19937*, boost::random::gamma_distribution<> > sample_gamma(mt_rng_pt, gamma_dist);

	double gamma = sample_gamma();

	return gamma;
}

double GammaHyperPrior::calculateLogDensity(double gamma) {

	double prob = (shape-1)*log(gamma) - (gamma/scale);

    return prob;

};

LogNormalHyperPrior::LogNormalHyperPrior(double location_in, double scale_in, mt_rng_pt_t mt_rng_pt_in) {

    location = location_in;
    scale = scale_in;
	mt_rng_pt = mt_rng_pt_in;
}

double LogNormalHyperPrior::init() {

	boost::random::lognormal_distribution<> lognormal_dist(location, scale);
	boost::random::variate_generator<boost::random::mt19937*, boost::random::lognormal_distribution<> > sample_gamma(mt_rng_pt, lognormal_dist);

	double gamma = sample_gamma();

	return gamma;
}

double LogNormalHyperPrior::calculateLogDensity(double gamma) {

	double prob = - log(gamma) - pow(log(gamma)-location,2)/(2*pow(scale,2));

    return prob;
}


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

/*
 Copyright (C) 2015 Jose Aparicio

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

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

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#ifndef quantlib_cva_swap_engine_hpp
#define quantlib_cva_swap_engine_hpp

#include <ql/handle.hpp>
#include <ql/instruments/vanillaswap.hpp>
#include <ql/termstructures/defaulttermstructure.hpp>

namespace QuantLib {

    class YieldTermStructure;
    class Quote;

  /*! Bilateral (CVA and DVA) default adjusted vanilla swap pricing
    engine. Collateral is not considered. No wrong way risk is
    considered (rates and counterparty default are uncorrelated).
    Based on:
    Sorensen,  E.H.  and  Bollier,  T.F.,  Pricing  swap  default
    risk. Financial Analysts Journal, 1994, 50, 23–33
    Also see sect. II-5 in: Risk Neutral Pricing of Counterparty Risk
    D. Brigo, M. Masetti, 2004
    or in sections 3 and 4 of "A Formula for Interest Rate Swaps
      Valuation under Counterparty Risk in presence of Netting Agreements"
    D. Brigo and M. Masetti; May 4, 2005

    to do: Compute fair rate through iteration instead of the
    current approximation .
    to do: write Issuer based constructors (event type)
    to do: Check consistency between option engine discount and the one given
   */
  class CounterpartyAdjSwapEngine : public VanillaSwap::engine {
    public:
      //! \name Constructors
      //@{
      //!
      /*! Creates the engine from an arbitrary swaption engine.
        If the investor default model is not given a default
        free one is assumed.
        @param discountCurve Used in pricing.
        @param swaptionEngine Determines the volatility and thus the
        exposure model.
        @param ctptyDTS Counterparty default curve.
        @param ctptyRecoveryRate Counterparty recovey rate.
        @param invstDTS Investor (swap holder) default curve.
        @param invstRecoveryRate Investor recovery rate.
       */
      CounterpartyAdjSwapEngine(
          const Handle<YieldTermStructure>& discountCurve,
          const Handle<PricingEngine>& swaptionEngine,
          const Handle<DefaultProbabilityTermStructure>& ctptyDTS,
          Real ctptyRecoveryRate,
          const Handle<DefaultProbabilityTermStructure>& invstDTS =
              Handle<DefaultProbabilityTermStructure>(),
          Real invstRecoveryRate = 0.999);
      /*! Creates an engine with a black volatility model for the
        exposure.
        If the investor default model is not given a default
        free one is assumed.
        @param discountCurve Used in pricing.
        @param blackVol Black volatility used in the exposure model.
        @param ctptyDTS Counterparty default curve.
        @param ctptyRecoveryRate Counterparty recovey rate.
        @param invstDTS Investor (swap holder) default curve.
        @param invstRecoveryRate Investor recovery rate.
       */
      CounterpartyAdjSwapEngine(
          const Handle<YieldTermStructure>& discountCurve,
          const Volatility blackVol,
          const Handle<DefaultProbabilityTermStructure>& ctptyDTS,
          Real ctptyRecoveryRate,
          const Handle<DefaultProbabilityTermStructure>& invstDTS =
              Handle<DefaultProbabilityTermStructure>(),
          Real invstRecoveryRate = 0.999);
      /*! Creates an engine with a black volatility model for the
        exposure. The volatility is given as a quote.
        If the investor default model is not given a default
        free one is assumed.
        @param discountCurve Used in pricing.
        @param blackVol Black volatility used in the exposure model.
        @param ctptyDTS Counterparty default curve.
        @param ctptyRecoveryRate Counterparty recovey rate.
        @param invstDTS Investor (swap holder) default curve.
        @param invstRecoveryRate Investor recovery rate.
      */
      CounterpartyAdjSwapEngine(
          const Handle<YieldTermStructure>& discountCurve,
          const Handle<Quote>& blackVol,
          const Handle<DefaultProbabilityTermStructure>& ctptyDTS,
          Real ctptyRecoveryRate,
          const Handle<DefaultProbabilityTermStructure>& invstDTS =
              Handle<DefaultProbabilityTermStructure>(),
          Real invstRecoveryRate = 0.999);
      //@}
      void calculate() const;
    private:
      Handle<PricingEngine> baseSwapEngine_;
      Handle<PricingEngine> swaptionletEngine_;
      Handle<YieldTermStructure> discountCurve_;
      Handle<DefaultProbabilityTermStructure> defaultTS_;
      Real ctptyRecoveryRate_;
      Handle<DefaultProbabilityTermStructure> invstDTS_;
      Real invstRecoveryRate_;
  };

}

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

/*
 Copyright (C) 2015 Jose Aparicio

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

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

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/cashflows/fixedratecoupon.hpp>
#include <ql/cashflows/floatingratecoupon.hpp>
#include <ql/indexes/iborindex.hpp>
#include <ql/instruments/makevanillaswap.hpp>
#include <ql/exercise.hpp>
#include <ql/pricingengines/swap/discountingswapengine.hpp>
#include <ql/termstructures/credit/flathazardrate.hpp>
#include <ql/pricingengines/swaption/blackswaptionengine.hpp>
#include <boost/make_shared.hpp>

namespace QuantLib {

  inline CounterpartyAdjSwapEngine::CounterpartyAdjSwapEngine(
      const Handle<YieldTermStructure>& discountCurve,
      const Handle<PricingEngine>& swaptionEngine,
      const Handle<DefaultProbabilityTermStructure>& ctptyDTS,
      Real ctptyRecoveryRate,
      const Handle<DefaultProbabilityTermStructure>& invstDTS,
      Real invstRecoveryRate)
  : baseSwapEngine_(Handle<PricingEngine>(
      boost::make_shared<DiscountingSwapEngine>(discountCurve))),
    swaptionletEngine_(swaptionEngine),
    discountCurve_(discountCurve),
    defaultTS_(ctptyDTS),
    ctptyRecoveryRate_(ctptyRecoveryRate),
    invstDTS_(invstDTS.empty() ? Handle<DefaultProbabilityTermStructure>(
        boost::make_shared<FlatHazardRate>(0, ctptyDTS->calendar(), 1.e-12,
        ctptyDTS->dayCounter()) ) : invstDTS ),
    invstRecoveryRate_(invstRecoveryRate)
  {
      registerWith(discountCurve);
      registerWith(ctptyDTS);
      registerWith(invstDTS_);
      registerWith(swaptionEngine);
  }

    inline CounterpartyAdjSwapEngine::CounterpartyAdjSwapEngine(
        const Handle<YieldTermStructure>& discountCurve,
        const Volatility blackVol,
        const Handle<DefaultProbabilityTermStructure>& ctptyDTS,
        Real ctptyRecoveryRate,
        const Handle<DefaultProbabilityTermStructure>& invstDTS,
        Real invstRecoveryRate)
  : baseSwapEngine_(Handle<PricingEngine>(
      boost::make_shared<DiscountingSwapEngine>(discountCurve))),
    swaptionletEngine_(Handle<PricingEngine>(
      boost::make_shared<BlackSwaptionEngine>(discountCurve,
        blackVol))),
    discountCurve_(discountCurve),
    defaultTS_(ctptyDTS),
    ctptyRecoveryRate_(ctptyRecoveryRate),
    invstDTS_(invstDTS.empty() ? Handle<DefaultProbabilityTermStructure>(
        boost::make_shared<FlatHazardRate>(0, ctptyDTS->calendar(), 1.e-12,
        ctptyDTS->dayCounter()) ) : invstDTS ),
    invstRecoveryRate_(invstRecoveryRate)
  {
      registerWith(discountCurve);
      registerWith(ctptyDTS);
      registerWith(invstDTS_);
  }

  inline CounterpartyAdjSwapEngine::CounterpartyAdjSwapEngine(
        const Handle<YieldTermStructure>& discountCurve,
        const Handle<Quote>& blackVol,
        const Handle<DefaultProbabilityTermStructure>& ctptyDTS,
        Real ctptyRecoveryRate,
        const Handle<DefaultProbabilityTermStructure>& invstDTS,
        Real invstRecoveryRate)
  : baseSwapEngine_(Handle<PricingEngine>(
      boost::make_shared<DiscountingSwapEngine>(discountCurve))),
    swaptionletEngine_(Handle<PricingEngine>(
      boost::make_shared<BlackSwaptionEngine>(discountCurve,
        blackVol))),
    discountCurve_(discountCurve),
    defaultTS_(ctptyDTS),
    ctptyRecoveryRate_(ctptyRecoveryRate),
    invstDTS_(invstDTS.empty() ? Handle<DefaultProbabilityTermStructure>(
        boost::make_shared<FlatHazardRate>(0, ctptyDTS->calendar(), 1.e-12,
        ctptyDTS->dayCounter()) ) : invstDTS ),
    invstRecoveryRate_(invstRecoveryRate)
  {
      registerWith(discountCurve);
      registerWith(ctptyDTS);
      registerWith(invstDTS_);
      registerWith(blackVol);
  }

  inline void CounterpartyAdjSwapEngine::calculate() const {
      /* both DTS, YTS ref dates and pricing date consistency
         checks? settlement... */
    QL_REQUIRE(!discountCurve_.empty(),
                 "no discount term structure set");
    QL_REQUIRE(!defaultTS_.empty(),
                 "no ctpty default term structure set");
    QL_REQUIRE(!swaptionletEngine_.empty(),
                 "no swap option engine set");

    Date priceDate = defaultTS_->referenceDate();

    Real cumOptVal = 0.,
        cumPutVal = 0.;
    // Vanilla swap so 0 leg is floater

    std::vector<Date>::const_iterator nextFD =
      arguments_.fixedPayDates.begin();
    Date swapletStart = priceDate;
    while (*nextFD < priceDate) ++nextFD;

    // Compute fair spread for strike value:
    // copy args into the non risky engine
    Swap::arguments * noCVAArgs = dynamic_cast<Swap::arguments*>(
      baseSwapEngine_->getArguments());
    QL_REQUIRE(noCVAArgs != 0, "wrong argument type");

    noCVAArgs->legs = this->arguments_.legs;
    noCVAArgs->payer = this->arguments_.payer;

    baseSwapEngine_->calculate();

    boost::shared_ptr<FixedRateCoupon> coupon = boost::dynamic_pointer_cast<FixedRateCoupon>(arguments_.legs[0][0]);
    QL_REQUIRE(coupon,"dynamic cast of fixed leg coupon failed.");
    Rate baseSwapRate = coupon->rate();

    const Swap::results * vSResults =
        dynamic_cast<const Swap::results *>(baseSwapEngine_->getResults());
    QL_REQUIRE(vSResults != 0, "wrong result type");

    Rate baseSwapFairRate = -baseSwapRate * vSResults->legNPV[1] /
        vSResults->legNPV[0];
    Real baseSwapNPV = vSResults->value;

    VanillaSwap::Type reversedType = arguments_.type == VanillaSwap::Payer ?
        VanillaSwap::Receiver : VanillaSwap::Payer;

    // Swaplet options summatory:
    while(nextFD != arguments_.fixedPayDates.end()) {
      // iFD coupon not fixed, create swaptionlet:
      boost::shared_ptr<FloatingRateCoupon> floatCoupon = boost::dynamic_pointer_cast<FloatingRateCoupon>(arguments_.legs[1][0]);
      QL_REQUIRE(floatCoupon,"dynamic cast of floating leg coupon failed.");
      boost::shared_ptr<IborIndex> swapIndex = boost::dynamic_pointer_cast<IborIndex>(floatCoupon->index());
      QL_REQUIRE(swapIndex,"dynamic cast of floating leg index failed.");

      // Alternatively one could cap this period to, say, 1M
      // Period swapPeriod = boost::dynamic_pointer_cast<FloatingRateCoupon>(
      //   arguments_.legs[1][0])->index()->tenor();

      Period baseSwapsTenor(arguments_.fixedPayDates.back().serialNumber()
	    - swapletStart.serialNumber(), Days);
      boost::shared_ptr<VanillaSwap> swaplet = MakeVanillaSwap(
        baseSwapsTenor,
        swapIndex,
        baseSwapFairRate // strike
        )
	    .withType(arguments_.type)
	    .withNominal(arguments_.nominal)
          ////////	    .withSettlementDays(2)
        .withEffectiveDate(swapletStart)
        .withTerminationDate(arguments_.fixedPayDates.back());
      boost::shared_ptr<VanillaSwap> revSwaplet = MakeVanillaSwap(
        baseSwapsTenor,
        swapIndex,
        baseSwapFairRate // strike
        )
	    .withType(reversedType)
	    .withNominal(arguments_.nominal)
          /////////	    .withSettlementDays(2)
        .withEffectiveDate(swapletStart)
        .withTerminationDate(arguments_.fixedPayDates.back());

      Swaption swaptionlet(swaplet,
        boost::make_shared<EuropeanExercise>(swapletStart));
      Swaption putSwaplet(revSwaplet,
        boost::make_shared<EuropeanExercise>(swapletStart));
      swaptionlet.setPricingEngine(swaptionletEngine_.currentLink());
      putSwaplet.setPricingEngine(swaptionletEngine_.currentLink());

      // atm underlying swap means that the value of put = value
      // call so this double pricing is not needed
      cumOptVal += swaptionlet.NPV() * defaultTS_->defaultProbability(
          swapletStart, *nextFD);
      cumPutVal += putSwaplet.NPV()  * invstDTS_->defaultProbability(
	      swapletStart, *nextFD);

      swapletStart = *nextFD;
      ++nextFD;
    }

    results_.value = baseSwapNPV - (1.-ctptyRecoveryRate_) * cumOptVal
        + (1.-invstRecoveryRate_) * cumPutVal;

    results_.fairRate =  -baseSwapRate * (vSResults->legNPV[1]
        - (1.-ctptyRecoveryRate_) * cumOptVal +
          (1.-invstRecoveryRate_) * cumPutVal )
      / vSResults->legNPV[0];

  }

}

#endif


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2016 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifdef Cygwin
// availability of std::to_string
#define _GLIBCXX_USE_C99 1
#endif

#include <iostream>

// #include <boost/timer/timer.hpp>

#include "dune/grid/config.h"
#include "dune/grid/uggrid.hh"
#include <dune/istl/operators.hh>

#include "utilities/enums.hh"
#include "fem/assemble.hh"
#include "fem/spaces.hh"
#include "io/vtk.hh"
#include "io/matlab.hh"
#include "linalg/direct.hh"
#include "linalg/apcg.hh"
#include "linalg/threadedMatrix.hh"
#include "linalg/triplet.hh"
#include "mg/multigrid.hh"
#include "mg/additiveMultigrid.hh"
#include "utilities/gridGeneration.hh"
#include "utilities/kaskopt.hh"
#include "utilities/memory.hh"

using namespace Kaskade;
#include "elastomechanics.hh"

int main(int argc, char *argv[])
{
  using namespace boost::fusion;

  std::cout << "Start elastomechanics tutorial program" << std::endl;

  int coarseGridSize, maxit, order, refinements, solver, verbose;
  bool additive, direct, vtk;
  double atol;
  std::string material;

  if (getKaskadeOptions(argc,argv,Options
    ("refinements",      refinements,     3,          "number of uniform grid refinements")
    ("coarse",           coarseGridSize,  1,          "number of coarse grid elements along each cube edge")
    ("order",            order,           1,          "finite element ansatz order")
    ("material",         material,        "steel",    "type of material")
    ("direct",           direct,          true,       "if true, use a direct solver")
    ("solver",           solver,          2,          "0=UMFPACK, 1=PARDISO 2=MUMPS 3=SUPERLU 4=UMFPACK32/64 5=UMFPACK64")
    ("additive",         additive,        false,      "use additive multigrid")
    ("verbosity",        verbose,         0,          "amount of reported details")
    ("vtk",              vtk,             false,      "write solution to VTK file")
    ("atol",             atol,            1e-8,       "absolute energy error tolerance for iterative solver")
    ("maxit",            maxit,           100,        "maximum number of iterations")))
    return 1;

  boost::timer::cpu_timer totalTimer;

  std::cout << "refinements of original mesh : " << refinements << std::endl;
  std::cout << "discretization order         : " << order << std::endl;

  constexpr int DIM = 3;
  using Grid = Dune::UGGrid<DIM>;
  using Spaces = boost::fusion::vector<H1Space<Grid> const*>;
  using VariableDescriptions = boost::fusion::vector<VariableDescription<0,DIM,0> >;
  using VarSetDesc = VariableSetDescription<Spaces,VariableDescriptions>;
  using Functional = ElasticityFunctional<VarSetDesc>;
  using Assembler = VariationalFunctionalAssembler<LinearizationAt<Functional> >;
  using CoefficientVectors = VarSetDesc::CoefficientVectorRepresentation<0,1>::type;

  Dune::FieldVector<double,DIM> c0(0.0), dc(1.0);
  GridManager<Grid> gridManager( createCuboid<Grid>(c0,dc,1.0/coarseGridSize,true) );
  gridManager.globalRefine(refinements);
  gridManager.enforceConcurrentReads(true);
  std::cout << "grid creation & refinement time: " << totalTimer.format();

  // construction of finite element space for the scalar solution T.
  H1Space<Grid> h1Space(gridManager,gridManager.grid().leafGridView(),order);

  Spaces spaces(&h1Space);

  std::string varNames[1] = { "u" };

  VarSetDesc varSetDesc(spaces,varNames);

  // Create the variational functional.
  Functional F(ElasticModulus::material(material));

  // construct Galerkin representation
  Assembler assembler(gridManager,spaces);
  VarSetDesc::VariableSet x(varSetDesc);
  VarSetDesc::VariableSet dx(varSetDesc);

  boost::timer::cpu_timer assembTimer;
  assembler.assemble(linearization(F,x));
  std::cout << "computing time for assemble: " << boost::timer::format(assembTimer.elapsed()) << "\n";

  CoefficientVectors rhs(assembler.rhs());
  CoefficientVectors solution(VarSetDesc::CoefficientVectorRepresentation<0,1>::init(spaces));
  boost::timer::cpu_timer solveTimer;
  if (direct)
  {
    DirectType directType = static_cast<DirectType>(solver);
    AssembledGalerkinOperator<Assembler> A(assembler, directType == DirectType::MUMPS || directType == DirectType::PARDISO);
    directInverseOperator(A,directType,MatrixProperties::POSITIVEDEFINITE).applyscaleadd(-1.0,rhs,solution);
    x.data = solution.data;
  }
  else
  {
    using X = Dune::BlockVector<Dune::FieldVector<double,DIM>>;
    DefaultDualPairing<X,X> dp;
    using Matrix = NumaBCRSMatrix<Dune::FieldMatrix<double,DIM,DIM>>;
    using LinOp = Dune::MatrixAdapter<Matrix,X,X>;
    Matrix Amat(assembler.get<0,0>(),true);
    LinOp A(Amat);
    SymmetricLinearOperatorWrapper<X,X> sa(A,dp);
    PCGEnergyErrorTerminationCriterion<double> term(atol,maxit);

    Dune::InverseOperatorResult res;
    X xi(component<0>(rhs).N());

    std::unique_ptr<SymmetricPreconditioner<X,X>> mg;
    if (additive)
    {
      if (order==1)
        mg = moveUnique(makeBPX(Amat,gridManager));
      else
      {
        H1Space<Grid> p1Space(gridManager,gridManager.grid().leafGridView(),1);
        mg = moveUnique(makePBPX(Amat,h1Space,p1Space));
      }
    }
    else
      mg = makeMultigrid(Amat,h1Space);

    Pcg<X,X> pcg(sa,*mg,term,verbose);
    pcg.apply(xi,component<0>(rhs),res);
    xi *= -1;
    component<0>(x) = xi;
  }
  std::cout << "computing time for solve: " << solveTimer.format();

  // output of solution in VTK format for visualization,
  // the data are written as ascii stream into file elasto.vtu,
  // possible is also binary
  if (vtk)
    writeVTKFile(x,"elasto",IoOptions().setOrder(order).setPrecision(7));

  std::cout << "total computing time: " << boost::timer::format(totalTimer.elapsed()) << "\n";
  std::cout << "End elastomechanics tutorial program" << std::endl;

  return 0;
}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example illustrating the use of the graph_labeler and
    structural_graph_labeling_trainer objects.

    Suppose you have a bunch of objects and you need to label each of them as true or
    false.  Suppose further that knowing the labels of some of these objects tells you
    something about the likely label of the others.  This is common in a number of domains.
    For example, in image segmentation problems you need to label each pixel, and knowing
    the labels of neighboring pixels gives you information about the likely label since
    neighboring pixels will often have the same label.

    We can generalize this problem by saying that we have a graph and our task is to label
    each node in the graph as true or false.  Additionally, the edges in the graph connect
    nodes which are likely to share the same label.  In this example program, each node
    will have a feature vector which contains information which helps tell if the node
    should be labeled as true or false.  The edges also contain feature vectors which give
    information indicating how strong the edge's labeling consistency constraint should be.
    This is useful since some nodes will have uninformative feature vectors and the only
    way to tell how they should be labeled is by looking at their neighbor's labels.

    Therefore, this program will show you how to learn two things using machine learning.
    The first is a linear classifier which operates on each node and predicts if it should
    be labeled as true or false.  The second thing is a linear function of the edge
    vectors.  This function outputs a penalty for giving two nodes connected by an edge
    differing labels.  The graph_labeler object puts these two things together and uses
    them to compute a labeling which takes both into account.  In what follows, we will use
    a structural SVM method to find the parameters of these linear functions which minimize
    the number of mistakes made by a graph_labeler.

    Finally, you might also consider reading the book Structured Prediction and Learning in
    Computer Vision by Sebastian Nowozin and Christoph H. Lampert since it contains a good
    introduction to machine learning methods such as the algorithm implemented by the
    structural_graph_labeling_trainer.
*/

#include <dlib/svm_threaded.h>
#include <iostream>

using namespace std;
using namespace dlib;

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

// The first thing we do is define the kind of graph object we will be using.
// Here we are saying there will be 2-D vectors at each node and 1-D vectors at
// each edge.  (You should read the matrix_ex.cpp example program for an introduction
// to the matrix object.)
typedef matrix<double,2,1> node_vector_type;
typedef matrix<double,1,1> edge_vector_type;
typedef graph<node_vector_type, edge_vector_type>::kernel_1a_c graph_type;

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

template <
    typename graph_type,
    typename labels_type
    >
void make_training_examples(
    dlib::array<graph_type>& samples,
    labels_type& labels
)
{
    /*
        This function makes 3 graphs we will use for training.   All of them
        will contain 4 nodes and have the structure shown below:

          (0)-----(1)
           |       |
           |       |
           |       |
          (3)-----(2)

        In this example, each node has a 2-D vector.  The first element of this vector
        is 1 when the node should have a label of false while the second element has
        a value of 1 when the node should have a label of true.  Additionally, the
        edge vectors will contain a value of 1 when the nodes connected by the edge
        should share the same label and a value of 0 otherwise.

        We want to see that the machine learning method is able to figure out how
        these features relate to the labels.  If it is successful it will create a
        graph_labeler which can predict the correct labels for these and other
        similarly constructed graphs.

        Finally, note that these tools require all values in the edge vectors to be >= 0.
        However, the node vectors may contain both positive and negative values.
    */

    samples.clear();
    labels.clear();

    std::vector<bool> label;
    graph_type g;

    // ---------------------------
    g.set_number_of_nodes(4);
    label.resize(g.number_of_nodes());
    // store the vector [0,1] into node 0.  Also label it as true.
    g.node(0).data = 0, 1; label[0] = true;
    // store the vector [0,0] into node 1.
    g.node(1).data = 0, 0; label[1] = true;  // Note that this node's vector doesn't tell us how to label it.
                                             // We need to take the edges into account to get it right.
    // store the vector [1,0] into node 2.
    g.node(2).data = 1, 0; label[2] = false;
    // store the vector [0,0] into node 3.
    g.node(3).data = 0, 0; label[3] = false;

    // Add the 4 edges as shown in the ASCII art above.
    g.add_edge(0,1);
    g.add_edge(1,2);
    g.add_edge(2,3);
    g.add_edge(3,0);

    // set the 1-D vector for the edge between node 0 and 1 to the value of 1.
    edge(g,0,1) = 1;
    // set the 1-D vector for the edge between node 1 and 2 to the value of 0.
    edge(g,1,2) = 0;
    edge(g,2,3) = 1;
    edge(g,3,0) = 0;
    // output the graph and its label.
    samples.push_back(g);
    labels.push_back(label);

    // ---------------------------
    g.set_number_of_nodes(4);
    label.resize(g.number_of_nodes());
    g.node(0).data = 0, 1; label[0] = true;
    g.node(1).data = 0, 1; label[1] = true;
    g.node(2).data = 1, 0; label[2] = false;
    g.node(3).data = 1, 0; label[3] = false;

    g.add_edge(0,1);
    g.add_edge(1,2);
    g.add_edge(2,3);
    g.add_edge(3,0);

    // This time, we have strong edges between all the nodes.  The machine learning
    // tools will have to learn that when the node information conflicts with the
    // edge constraints that the node information should dominate.
    edge(g,0,1) = 1;
    edge(g,1,2) = 1;
    edge(g,2,3) = 1;
    edge(g,3,0) = 1;
    samples.push_back(g);
    labels.push_back(label);
    // ---------------------------

    g.set_number_of_nodes(4);
    label.resize(g.number_of_nodes());
    g.node(0).data = 1, 0; label[0] = false;
    g.node(1).data = 1, 0; label[1] = false;
    g.node(2).data = 1, 0; label[2] = false;
    g.node(3).data = 0, 0; label[3] = false;

    g.add_edge(0,1);
    g.add_edge(1,2);
    g.add_edge(2,3);
    g.add_edge(3,0);

    edge(g,0,1) = 0;
    edge(g,1,2) = 0;
    edge(g,2,3) = 1;
    edge(g,3,0) = 0;
    samples.push_back(g);
    labels.push_back(label);
    // ---------------------------

}

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

int main()
{
    try
    {
        // Get the training samples we defined above.
        dlib::array<graph_type> samples;
        std::vector<std::vector<bool> > labels;
        make_training_examples(samples, labels);

        // Create a structural SVM trainer for graph labeling problems.  The vector_type
        // needs to be set to a type capable of holding node or edge vectors.
        typedef matrix<double,0,1> vector_type;
        structural_graph_labeling_trainer<vector_type> trainer;
        // This is the usual SVM C parameter.  Larger values make the trainer try
        // harder to fit the training data but might result in overfitting.  You
        // should set this value to whatever gives the best cross-validation results.
        trainer.set_c(10);

        // Do 3-fold cross-validation and print the results.  In this case it will
        // indicate that all nodes were correctly classified.
        cout << "3-fold cross-validation: " << cross_validate_graph_labeling_trainer(trainer, samples, labels, 3) << endl;

        // Since the trainer is working well.  Let's have it make a graph_labeler
        // based on the training data.
        graph_labeler<vector_type> labeler = trainer.train(samples, labels);

        /*
            Let's try the graph_labeler on a new test graph.  In particular, let's
            use one with 5 nodes as shown below:

            (0 F)-----(1 T)
              |         |
              |         |
              |         |
            (3 T)-----(2 T)------(4 T)

            I have annotated each node with either T or F to indicate the correct
            output (true or false).
        */
        graph_type g;
        g.set_number_of_nodes(5);
        g.node(0).data = 1, 0;  // Node data indicates a false node.
        g.node(1).data = 0, 1;  // Node data indicates a true node.
        g.node(2).data = 0, 0;  // Node data is ambiguous.
        g.node(3).data = 0, 0;  // Node data is ambiguous.
        g.node(4).data = 0.1, 0; // Node data slightly indicates a false node.

        g.add_edge(0,1);
        g.add_edge(1,2);
        g.add_edge(2,3);
        g.add_edge(3,0);
        g.add_edge(2,4);

        // Set the edges up so nodes 1, 2, 3, and 4 are all strongly connected.
        edge(g,0,1) = 0;
        edge(g,1,2) = 1;
        edge(g,2,3) = 1;
        edge(g,3,0) = 0;
        edge(g,2,4) = 1;

        // The output of this shows all the nodes are correctly labeled.
        cout << "Predicted labels: " << endl;
        std::vector<bool> temp = labeler(g);
        for (unsigned long i = 0; i < temp.size(); ++i)
            cout << " " << i << ": " << temp[i] << endl;

        // Breaking the strong labeling consistency link between node 1 and 2 causes
        // nodes 2, 3, and 4 to flip to false.  This is because of their connection
        // to node 4 which has a small preference for false.
        edge(g,1,2) = 0;
        cout << "Predicted labels: " << endl;
        temp = labeler(g);
        for (unsigned long i = 0; i < temp.size(); ++i)
            cout << " " << i << ": " << temp[i] << endl;
    }
    catch (std::exception& e)
    {
        cout << "Error, an exception was thrown!" << endl;
        cout << e.what() << endl;
    }
}


#include "UCCSD.hpp"
#include "GateFunction.hpp"
#include "FermionToSpinTransformation.hpp"
#include "CommutingSetGenerator.hpp"
#include <boost/math/constants/constants.hpp>

using namespace xacc::quantum;

namespace xacc {
namespace vqe {

std::shared_ptr<Function> UCCSD::generate(
		std::shared_ptr<AcceleratorBuffer> buffer,
		std::vector<InstructionParameter> parameters) {

	auto runtimeOptions = RuntimeOptions::instance();

	if (!runtimeOptions->exists("n-electrons")) {
		xacc::error("To use this UCCSD State Prep IRGenerator, you "
				"must specify the number of electrons.");
	}

	if (!runtimeOptions->exists("n-qubits")) {
		xacc::error("To use this UCCSD State Prep IRGenerator, you "
				"must specify the number of qubits.");
	}

	auto nQubits = std::stoi((*runtimeOptions)["n-qubits"]);
	auto nElectrons = std::stoi((*runtimeOptions)["n-electrons"]);

	// Compute the number of parameters
	auto _nOccupied = (int) std::ceil(nElectrons / 2.0);
	auto _nVirtual = nQubits / 2 - _nOccupied;
	auto nSingle = _nOccupied * _nVirtual;
	auto nDouble = std::pow(nSingle, 2);
	auto _nParameters = nSingle + nDouble;

	auto singletIndex = [=](int i, int j) -> int {
		return i * _nOccupied + j;
	};

	auto doubletIndex = [=](int i, int j, int k, int l) -> int {
		return
		(i * _nOccupied * _nVirtual * _nOccupied +
				j * _nVirtual * _nOccupied +
				k * _nOccupied +
				l);
	};

	std::vector<xacc::InstructionParameter> variables;
	std::vector<std::string> params;
	for (int i = 0; i < _nParameters; i++) {
		params.push_back("theta" + std::to_string(i));
		variables.push_back(InstructionParameter("theta" + std::to_string(i)));
	}

	auto kernel = std::make_shared<FermionKernel>("fermiUCCSD");
	xacc::info("Constructing UCCSD Fermion Operator.");
	for (int i = 0; i < _nVirtual; i++) {
		for (int j = 0; j < _nOccupied; j++) {
			for (int l = 0; l < 2; l++) {
				std::vector<std::pair<int, int>> operators { { 2
						* (i + _nOccupied) + l, 1 }, { 2 * j + l, 0 } };
				auto fermiInstruction1 = std::make_shared<FermionInstruction>(
						operators, params[singletIndex(i, j)]);
				kernel->addInstruction(fermiInstruction1);

				std::vector<std::pair<int, int>> operators2 { { 2 * j + l, 1 },
						{ 2 * (i + _nOccupied) + l, 0 } };
				auto fermiInstruction2 = std::make_shared<FermionInstruction>(
						operators2, params[singletIndex(i, j)]);

				auto nP = fermiInstruction2->nParameters();
				InstructionParameter p(-1.0*std::complex<double>(1,0));
				fermiInstruction2->setParameter(nP-2,p);

				kernel->addInstruction(fermiInstruction2);

			}
		}
	}

	for (int i = 0; i < _nVirtual; i++) {
		for (int j = 0; j < _nOccupied; j++) {
			for (int l = 0; l < 2; l++) {
				for (int i2 = 0; i2 < _nVirtual; i2++) {
					for (int j2 = 0; j2 < _nOccupied; j2++) {
						for (int l2 = 0; l2 < 2; l2++) {
							std::vector<std::pair<int, int>> operators1 { { 2
									* (i + _nOccupied) + l, 1 },
									{ 2 * j + l, 0 }, { 2 * (i2 + _nOccupied)
											+ l2, 1 }, { 2 * j2 + l2, 0 } };

							std::vector<std::pair<int, int>> operators2 { { 2
									* j2 + l2, 1 }, { 2 * (i2 + _nOccupied)
									+ l2, 0 }, { 2 * j + l, 1 }, { 2
									* (i + _nOccupied) + l, 0 } };

							auto doubletIdx1 = nSingle
									+ doubletIndex(i, j, i2, j2);
							auto doubletIdx2 = nSingle
									+ doubletIndex(i, j, i2, j2);

							auto fermiInstruction1 = std::make_shared<
									FermionInstruction>(operators1,
									params[doubletIdx1]);

							kernel->addInstruction(fermiInstruction1);

							auto fermiInstruction2 = std::make_shared<
									FermionInstruction>(operators2,
									params[doubletIdx2]);

							auto nP = fermiInstruction2->nParameters();
							InstructionParameter p(-1.0*std::complex<double>(1,0));
							fermiInstruction2->setParameter(nP-2,p);
							kernel->addInstruction(fermiInstruction2);

						}
					}
				}
			}
		}
	}

//	std::cout << "KERNEL: \n" << kernel->toString("") << "\n";
	// Create the FermionIR to pass to our transformation.
	auto fermionir = std::make_shared<FermionIR>();
	fermionir->addKernel(kernel);

	xacc::info("Done constructing UCCSD Fermion Operator.");
	xacc::info("Mapping UCCSD Fermion Operator to Spin. ");

	std::shared_ptr<FermionToSpinTransformation> transform;
	if (xacc::optionExists("fermion-transformation")) {
		auto transformStr = xacc::getOption("fermion-transformation");
		transform = xacc::getService<FermionToSpinTransformation>(
				transformStr);
	} else {
		transform = xacc::getService<FermionToSpinTransformation>(
				"jw");
	}

	auto compositeResult = transform->transform(*kernel.get());
//	auto resultsStr = compositeResult.toString();
//	boost::replace_all(resultsStr, "+", "+\n");

	// Create the Spin Hamiltonian
	auto transformedIR = compositeResult.toXACCIR();
	xacc::info("Done mapping UCCSD Fermion Operator to Spin.");

	std::unordered_map<std::string, Term> terms = compositeResult.getTerms();

	CommutingSetGenerator gen;
	auto commutingSets = gen.getCommutingSet(compositeResult, nQubits);
	auto pi = boost::math::constants::pi<double>();
	auto gateRegistry = xacc::getService<IRProvider>("gate");

	auto uccsdGateFunction = gateRegistry->createFunction("uccsdPrep",{},
			variables);

	// Perform Trotterization...
	for (auto s : commutingSets) {

		for (auto inst : s) {
			Term spinInst = inst;

			// Get the individual pauli terms
			auto termsMap = std::get<2>(spinInst);

			std::vector<std::pair<int,std::string>> terms;
			for (auto& kv : termsMap) {
				if (kv.second != "I" && !kv.second.empty()) {
					terms.push_back({kv.first, kv.second});
				}
			}
			// The largest qubit index is on the last term
			int largestQbitIdx = terms[terms.size() - 1].first;
			auto tempFunction = gateRegistry->createFunction("temp", {}, {});

			for (int i = 0; i < terms.size(); i++) {

				auto qbitIdx = terms[i].first;
				auto gateName = terms[i].second;

				if (i < terms.size() - 1) {
					auto cnot =
							gateRegistry->createInstruction(
									"CNOT",
									std::vector<int> { qbitIdx,
											terms[i + 1].first });
					tempFunction->addInstruction(cnot);
				}

				if (gateName == "X") {
					auto hadamard =
							gateRegistry->createInstruction(
									"H", std::vector<int> { qbitIdx });
					tempFunction->insertInstruction(0, hadamard);
				} else if (gateName == "Y") {
					auto rx =
							gateRegistry->createInstruction(
									"Rx", std::vector<int> { qbitIdx });
					InstructionParameter p(pi / 2.0);
					rx->setParameter(0, p);
					tempFunction->insertInstruction(0, rx);
				}

				// Add the Rotation for the last term
				if (i == terms.size() - 1) {
					// FIXME DONT FORGET DIVIDE BY 2
					std::stringstream ss;
					ss << 2*std::imag(std::get<0>(spinInst)) << " * "
							<< std::get<1>(spinInst);
					auto rz =
							gateRegistry->createInstruction(
									"Rz", std::vector<int> { qbitIdx });

					InstructionParameter p(ss.str());
					rz->setParameter(0, p);
					tempFunction->addInstruction(rz);

				}
			}

			int counter = tempFunction->nInstructions();
			// Add the instruction on the backend of the circuit
			for (int i = terms.size() - 1; i >= 0; i--) {

				auto qbitIdx = terms[i].first;
				auto gateName = terms[i].second;

				if (i < terms.size() - 1) {
					auto cnot =
							gateRegistry->createInstruction(
									"CNOT",
									std::vector<int> { qbitIdx,
											terms[i + 1].first });
					tempFunction->insertInstruction(counter, cnot);
					counter++;
				}

				if (gateName == "X") {
					auto hadamard =
							gateRegistry->createInstruction(
									"H", std::vector<int> { qbitIdx });
					tempFunction->addInstruction(hadamard);
				} else if (gateName == "Y") {
					auto rx =
							gateRegistry->createInstruction(
									"Rx", std::vector<int> { qbitIdx });
					InstructionParameter p(4 * pi - (pi /2.0));
					rx->setParameter(0, p);
					tempFunction->addInstruction(rx);
				}

			}
			// Add to the total UCCSD State Prep function
			for (auto inst : tempFunction->getInstructions()) {
				uccsdGateFunction->addInstruction(inst);
			}
		}
	}

	for (int i = nElectrons-1; i >= 0; i--) {
		auto xGate = gateRegistry->createInstruction(
				"X", std::vector<int>{i});
		uccsdGateFunction->insertInstruction(0,xGate);
	}

	return uccsdGateFunction;
}

}
}