//
// Created by Hamza El-Kebir on 4/17/21.
//

#ifndef LODESTAR_STATESPACE_HPP
#define LODESTAR_STATESPACE_HPP

#include "SystemStateless.hpp"
#include <Eigen/Dense>
#include "Lodestar/aux/CompileTimeQualifiers.hpp"

namespace ls {
    namespace systems {
        template<typename TScalar = double, const int TStateDim = Eigen::Dynamic, const int TInputDim = Eigen::Dynamic, const int TOutputDim = Eigen::Dynamic>
        class StateSpace : public SystemStateless {
        public:
            typedef Eigen::Matrix<TScalar, TStateDim, TStateDim> TDStateMatrix;
            typedef Eigen::Matrix<TScalar, TStateDim, TInputDim> TDInputMatrix;
            typedef Eigen::Matrix<TScalar, TOutputDim, TStateDim> TDOutputMatrix;
            typedef Eigen::Matrix<TScalar, TOutputDim, TInputDim> TDFeedforwardMatrix;

            /**
             * @brief Default constructor.
             */
            StateSpace() : A_(new TDStateMatrix),
                           B_(new TDInputMatrix),
                           C_(new TDOutputMatrix),
                           D_(new TDFeedforwardMatrix),
                           dt_(-1), isDiscrete_(false)
            {
                A_->setIdentity();
                B_->setIdentity();
                C_->setIdentity();
                D_->setZero();
            };

            /**
             * @brief Construct a state space system with the given matrices.
             *
             * @note TState space systems are assumed to be in continuous time by
             * default.
             *
             * @param A Pointer to state matrix.
             * @param B Pointer to input matrix.
             * @param C Pointer to output matrix.
             * @param D Pointer to feedforward matrix.
             */
            StateSpace(TDStateMatrix *A, TDInputMatrix *B,
                       TDOutputMatrix *C, TDOutputMatrix *D);

            //            /**
            //             * @brief Construct a state space system with the given matrices.
            //             *
            //             * @note TState space systems are assumed to be in continuous time by
            //             * default.
            //             *
            //             * @param A TState matrix.
            //             * @param B Input matrix.
            //             * @param C Output matrix.
            //             * @param D Feedforward matrix.
            //             */
            //            StateSpace(const TDStateMatrix &A, const TDInputMatrix &B,
            //                       const TDOutputMatrix &C, const TDOutputMatrix &D);

            template<typename DerivedA, typename DerivedB, typename DerivedC, typename DerivedD>
            StateSpace(const Eigen::EigenBase<DerivedA> &A, const Eigen::EigenBase<DerivedB> &B,
                       const Eigen::EigenBase<DerivedC> &C, const Eigen::EigenBase<DerivedD> &D);

            /**
             * @brief Copy constructor.
             *
             * @param other TState space object to copy.
             */
            StateSpace(const StateSpace &other);

            /**
             * @brief Gets the state matrix.
             *
             * @return TState matrix.
             */
            const TDStateMatrix &getA() const;

            /**
             * @brief Sets the state matrix.
             *
             * @param A Pointer to state matrix.
             */
            void setA(TDStateMatrix *A);

            template<typename Derived>
            void setA(Eigen::EigenBase<Derived> *A);

            /**
             * @brief Sets the state matrix.
             *
             * @param A TState matrix.
             */
            void setA(const TDStateMatrix &A);

            template<typename Derived>
            void setA(const Eigen::EigenBase<Derived> &A);

            /**
             * @brief Gets the input matrix.
             *
             * @return Input matrix.
             */
            const TDInputMatrix &getB() const;

            /**
             * @brief Sets the input matrix.
             *
             * @param B Pointer to input matrix.
             */
            void setB(TDInputMatrix *B);

            template<typename Derived>
            void setB(Eigen::EigenBase<Derived> *B);

            /**
             * @brief Sets the input matrix.
             *
             * @param B Input matrix.
             */
            void setB(const TDInputMatrix &B);

            template<typename Derived>
            void setB(const Eigen::EigenBase<Derived> &B);

            /**
             * @brief Gets the output matrix.
             *
             * @return Output matrix.
             */
            const TDOutputMatrix &getC() const;

            /**
             * @brief Sets the output matrix.
             *
             * @param C Pointer to output matrix.
             */
            void setC(TDOutputMatrix *C);

            template<typename Derived>
            void setC(Eigen::EigenBase<Derived> *C);

            /**
             * @brief Sets the output matrix.
             *
             * @param C Output matrix.
             */
            void setC(const TDOutputMatrix &C);

            template<typename Derived>
            void setC(const Eigen::EigenBase<Derived> &C);

            /**
             * @brief Gets the feedforward matrix.
             *
             * @return Feedforward matrix.
             */
            const TDFeedforwardMatrix &getD() const;

            /**
             * @brief Sets the feedforward matrix.
             *
             * @param D Feedforward matrix.
             */
            void setD(TDFeedforwardMatrix *D);

            template<typename Derived>
            void setD(Eigen::EigenBase<Derived> *D);

            /**
             * @brief Sets the feedforward matrix.
             *
             * @param D Pointer to feedforward matrix.
             */
            void setD(const TDFeedforwardMatrix &D);

            template<typename Derived>
            void setD(const Eigen::EigenBase<Derived> &D);

            /**
             * @brief Copies matrices from one state space object to the current
             * instance.
             *
             * @param ss TState space object to copy matrices from.
             */
            void copyMatrices(const StateSpace &other);

            /**
             * @brief Sets the discrete time system parameters.
             *
             * @param dt Sampling period.
             */
            void setDiscreteParams(double dt);

            /**
             * @brief Sets the discrete time system parameters.
             *
             * @param dt Sampling period.
             * @param discrete If true, the system is treated as a discrete time
             * system.
             */
            void setDiscreteParams(double dt, bool discrete);

            /**
             * @brief Returns a bool that tells if the system is discrete.
             *
             * @return True if the system is discrete.
             */
            inline bool isDiscrete() const;

            /**
             * @brief Returns the sampling period.
             *
             * @return Sampling period.
             */
            inline double getSamplingPeriod() const;

            /**
             * @brief Sets the sampling period.
             *
             * @param dt Sampling period.
             */
            void setSamplingPeriod(double dt);

            /**
             * @brief Returns the state dimension.
             *
             * @return TState dimension.
             */
            IF_DYNAMIC_RETURN(TStateDim, TInputDim, TOutputDim, long)
            inline stateDim() const
            {
                return stateDimDynamic();
            }

            long inline stateDimDynamic() const
            {
                return A_->rows();
            }

            IF_STATIC_RETURN(TStateDim, TInputDim, TOutputDim, long)
            inline stateDim() const
            {
                return stateDimStatic();
            }

            long inline stateDimStatic() const
            {
                return TStateDim;
            }

            /**
             * @brief Returns the input dimension.
             *
             * @return Input dimension.
             */
            IF_DYNAMIC_RETURN(TStateDim, TInputDim, TOutputDim, long)
            inline inputDim() const
            {
                return inputDimDynamic();
            }

            long inline inputDimDynamic() const
            {
                return B_->cols();
            }

            IF_STATIC_RETURN(TStateDim, TInputDim, TOutputDim, long)
            inline inputDim() const
            {
                return inputDimStatic();
            }

            long inline inputDimStatic() const
            {
                return TInputDim;
            }

            /**
             * @brief Returns the output dimension.
             *
             * @return Output dimension
             */
            IF_DYNAMIC_RETURN(TStateDim, TInputDim, TOutputDim, long)
            inline outputDim() const
            {
                return outputDimDynamic();
            }

            long inline outputDimDynamic() const
            {
                return C_->rows();
            }

            IF_STATIC_RETURN(TStateDim, TInputDim, TOutputDim, long)
            inline outputDim() const
            {
                return outputDimStatic();
            }

            long inline outputDimStatic() const
            {
                return TOutputDim;
            }

            /**
             * @brief Appends to state space systems.
             *
             * If the discrete time parameters do not match the current system,
             * the input system is altered to match the current system.
             *
             * @param ss TState space system to append.
             */
            template<int TStateDim2, int TOutputDim2>
            void append(const StateSpace<TScalar, TStateDim2, TOutputDim, TOutputDim2> &ss);

            /**
             * @brief Determines if the system is stable based on its
             * eigenvalues.
             *
             * \p tolerance sets the stability margin that is taken into
             * account during computation.
             *
             * @param tolerance Eigenvalue tolerance.
             * @return True if system is stable.
             */
            inline bool isStable(double tolerance = 0) const;

            /**
             * @brief Returns a state space representation that adds integral action to the original system.
             *
             * @details For details on the discrete time case, see [1].
             *
             * @sa D. D. Ruscio, “Discrete LQ optimal control with integral action: A simple controller on incremental
             *  form for MIMO systems,” MIC, vol. 33, no. 2, pp. 35–44, 2012, doi:
             *  10.4173/mic.2012.2.1.
             *
             * @note This particular function gets called when the state space system is dynamic.
             *
             * @tparam T_TScalar Copy of \c TScalar.
             * @tparam T_TStateDim Copy of \c TStateDim.
             * @tparam T_TInputDim Copy of \c TInputDim.
             * @tparam T_TOutputDim Copy of \c TOutputDim.
             *
             * @return State space system augmented with integral action.
             */
            template<typename T_TScalar = TScalar, const int T_TStateDim = TStateDim, const int T_TInputDim = TInputDim, const int T_TOutputDim = TOutputDim>
            ls::systems::StateSpace<T_TScalar, LS_STATIC_UNLESS_DYNAMIC(
                    T_TStateDim + T_TOutputDim), T_TInputDim, T_TOutputDim>
            addIntegralAction(LS_IS_DYNAMIC_DEFAULT(T_TStateDim, T_TInputDim, T_TOutputDim)) const;

            /**
             * @brief Returns a state space representation that adds integral action to the original system.
             *
             * @details For details on the discrete time case, see [1].
             *
             * @sa [1] D. D. Ruscio, “Discrete LQ optimal control with integral action: A simple controller on
             * incremental form for MIMO systems,” MIC, vol. 33, no. 2, pp. 35–44, 2012, doi:
             *  10.4173/mic.2012.2.1.
             *
             * @note This particular function gets called when the state space system is static.
             *
             * @tparam T_TScalar Copy of \c TScalar.
             * @tparam T_TStateDim Copy of \c TStateDim.
             * @tparam T_TInputDim Copy of \c TInputDim.
             * @tparam T_TOutputDim Copy of \c TOutputDim.
             *
             * @return State space system augmented with integral action.
             */
            template<typename T_TScalar = TScalar, const int T_TStateDim = TStateDim, const int T_TInputDim = TInputDim, const int T_TOutputDim = TOutputDim>
            ls::systems::StateSpace<T_TScalar, LS_STATIC_UNLESS_DYNAMIC(
                    T_TStateDim + T_TOutputDim), T_TInputDim, T_TOutputDim>
            addIntegralAction(LS_IS_STATIC_DEFAULT(T_TStateDim, T_TInputDim, T_TOutputDim)) const;

        protected:
            TDStateMatrix *A_; //! TState matrix.
            TDInputMatrix *B_; //! Input matrix.
            TDOutputMatrix *C_; //! Output matrix.
            TDFeedforwardMatrix *D_; //! Feedforward matrix.
            double dt_; //! Sampling period.
            bool isDiscrete_; //! Discrete flag.
        };
    }
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::StateSpace(
        const StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> &other):
        A_(new TDStateMatrix(other.getA())),
        B_(new TDInputMatrix(other.getB())),
        C_(new TDOutputMatrix(other.getC())),
        D_(new TDFeedforwardMatrix(other.getD())),
        dt_(other.getSamplingPeriod()),
        isDiscrete_(other.isDiscrete())
{}

// TODO: Replace constructors by Eigen::EigenBase<*>
template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::StateSpace(
        TDStateMatrix *A, TDInputMatrix *B,
        TDOutputMatrix *C, TDOutputMatrix *D):
        A_(new TDStateMatrix(*A)),
        B_(new TDInputMatrix(*B)),
        C_(new TDOutputMatrix(*C)),
        D_(new TDFeedforwardMatrix(*D))
{
    //    *A_ = *A;
    //    *B_ = *B;
    //    *C_ = *C;
    //    *D_ = *D;
    dt_ = -1;
    isDiscrete_ = false;
}

//template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
//ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::StateSpace(
//        const TDStateMatrix &A, const TDInputMatrix &B,
//        const TDOutputMatrix &C, const TDOutputMatrix &D) : A_(new TDStateMatrix(A)), B_(new TDInputMatrix(B)),
//                                                            C_(new TDOutputMatrix(C)), D_(new TDFeedforwardMatrix(D))
//{
////    *A_ = A;
////    *B_ = B;
////    *C_ = C;
////    *D_ = D;
//    dt_ = -1;
//    isDiscrete_ = false;
//}

template<typename TScalar, const int TStateDim, const int TInputDim, const int TOutputDim>
template<typename DerivedA, typename DerivedB, typename DerivedC, typename DerivedD>
ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::StateSpace(const Eigen::EigenBase<DerivedA> &A,
                                                                               const Eigen::EigenBase<DerivedB> &B,
                                                                               const Eigen::EigenBase<DerivedC> &C,
                                                                               const Eigen::EigenBase<DerivedD> &D):
        A_(new TDStateMatrix(A)),
        B_(new TDInputMatrix(B)),
        C_(new TDOutputMatrix(C)),
        D_(new TDFeedforwardMatrix(D))
{
    *A_ = A;
    *B_ = B;
    *C_ = C;
    *D_ = D;
    dt_ = -1;
    isDiscrete_ = false;
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
const typename ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::TDStateMatrix &
ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::getA() const
{
    return *A_;
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::setA(TDStateMatrix *A)
{
    *A_ = *A;
}

template<typename TScalar, const int TStateDim, const int TInputDim, const int TOutputDim>
template<typename Derived>
void ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::setA(Eigen::EigenBase<Derived> *A)
{
    *A_ = *A;
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::setA(
        const TDStateMatrix &A)
{
    *A_ = A;
}

template<typename TScalar, const int TStateDim, const int TInputDim, const int TOutputDim>
template<typename Derived>
void ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::setA(const Eigen::EigenBase<Derived> &A)
{
    *A_ = A;
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
const typename ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::TDInputMatrix &
ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::getB() const
{
    return *B_;
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::setB(TDInputMatrix *B)
{
    *B_ = *B;
}

template<typename TScalar, const int TStateDim, const int TInputDim, const int TOutputDim>
template<typename Derived>
void ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::setB(Eigen::EigenBase<Derived> *B)
{
    *B_ = *B;
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::setB(
        const TDInputMatrix &B)
{
    *B_ = B;
}

template<typename TScalar, const int TStateDim, const int TInputDim, const int TOutputDim>
template<typename Derived>
void ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::setB(const Eigen::EigenBase<Derived> &B)
{
    *B_ = B;
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
const typename ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::TDOutputMatrix &
ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::getC() const
{
    return *C_;
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void
ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::setC(TDOutputMatrix *C)
{
    *C_ = *C;
}

template<typename TScalar, const int TStateDim, const int TInputDim, const int TOutputDim>
template<typename Derived>
void ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::setC(Eigen::EigenBase<Derived> *C)
{
    *C_ = *C;
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::setC(
        const TDOutputMatrix &C)
{
    *C_ = C;
}

template<typename TScalar, const int TStateDim, const int TInputDim, const int TOutputDim>
template<typename Derived>
void ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::setC(const Eigen::EigenBase<Derived> &C)
{
    *C_ = C;
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
const typename ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::TDFeedforwardMatrix &
ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::getD() const
{
    return *D_;
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::setD(
        TDFeedforwardMatrix *D)
{
    *D_ = *D;
}

template<typename TScalar, const int TStateDim, const int TInputDim, const int TOutputDim>
template<typename Derived>
void ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::setD(Eigen::EigenBase<Derived> *D)
{
    *D_ = *D;
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::setD(
        const TDFeedforwardMatrix &D)
{
    *D_ = D;
}

template<typename TScalar, const int TStateDim, const int TInputDim, const int TOutputDim>
template<typename Derived>
void ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::setD(const Eigen::EigenBase<Derived> &D)
{
    *D_ = D;
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
bool ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::isDiscrete() const
{
    return isDiscrete_;
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
double ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::getSamplingPeriod() const
{
    return dt_;
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::setSamplingPeriod(double dt)
{
    dt_ = dt;
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::setDiscreteParams(double dt)
{
    dt_ = dt;
    isDiscrete_ = true;
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::setDiscreteParams(double dt, bool discrete)
{
    dt_ = dt;
    isDiscrete_ = discrete;
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
template<int TStateDim2, int TOutputDim2>
void ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::append(
        const ls::systems::StateSpace<TScalar, TStateDim2, TOutputDim, TOutputDim2> &ss)
{
    // TODO: Implement state space append function.
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
bool ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::isStable(const double tolerance) const
{
    auto eig = A_->eigenvalues();

    if (isDiscrete()) {
        double tol = (tolerance < 0 ? -tolerance * tolerance : tolerance * tolerance);

        for (int i = 0; i < eig.size(); i++) {
            if (eig(i).real() * eig(i).real() + eig(i).imag() * eig(i).imag() > 1 + tol)
                return false;
        }
    } else {
        for (int i = 0; i < eig.size(); i++) {
            if (eig(i).real() > -tolerance)
                return false;
        }
    }

    return true;
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::copyMatrices(
        const ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> &other)
{
    setA(other.getA());
    setB(other.getB());
    setC(other.getC());
    setD(other.getD());

}

template<typename TScalar, const int TStateDim, const int TInputDim, const int TOutputDim>
template<typename T_TScalar, const int T_TStateDim, const int T_TInputDim, const int T_TOutputDim>
ls::systems::StateSpace<T_TScalar, LS_STATIC_UNLESS_DYNAMIC(T_TStateDim + T_TOutputDim), T_TInputDim, T_TOutputDim>
ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::addIntegralAction(
        LS_IS_DYNAMIC(T_TStateDim, T_TInputDim, T_TOutputDim)) const
{
    Eigen::Matrix<TScalar, Eigen::Dynamic, Eigen::Dynamic> A(stateDim() + outputDim(), stateDim() + outputDim());
    A.setZero();
    A.topLeftCorner(stateDim(), stateDim()) = getA();
    A.bottomLeftCorner(outputDim(), stateDim()) = getC();
    A.bottomRightCorner(outputDim(), outputDim()).setIdentity();

    Eigen::Matrix<TScalar, Eigen::Dynamic, Eigen::Dynamic> B(stateDim() + outputDim(), inputDim());
    B.setZero();
    B.topRows(stateDim()) = getB();

    Eigen::Matrix<TScalar, Eigen::Dynamic, Eigen::Dynamic> C(outputDim(), stateDim() + outputDim());
    C.setZero();
    C.leftCols(stateDim()) = getC();
    C.rightCols(outputDim()).setIdentity();

    Eigen::Matrix<TScalar, Eigen::Dynamic, Eigen::Dynamic> D(outputDim(), inputDim());
    D = getD();

    ls::systems::StateSpace<T_TScalar, LS_STATIC_UNLESS_DYNAMIC(
            T_TStateDim + T_TOutputDim), T_TInputDim, T_TOutputDim> ssi{A, B, C, D};

    return ssi;
}

template<typename TScalar, const int TStateDim, const int TInputDim, const int TOutputDim>
template<typename T_TScalar, const int T_TStateDim, const int T_TInputDim, const int T_TOutputDim>
ls::systems::StateSpace<T_TScalar, LS_STATIC_UNLESS_DYNAMIC(T_TStateDim + T_TOutputDim), T_TInputDim, T_TOutputDim>
ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::addIntegralAction(
        LS_IS_STATIC(T_TStateDim, T_TInputDim, T_TOutputDim)) const
{
    Eigen::Matrix<TScalar, TStateDim + TOutputDim, TStateDim + TOutputDim> A{};
    A.setZero();
    A.template topLeftCorner<TStateDim, TStateDim>() = getA();
    A.template bottomLeftCorner<TOutputDim, TStateDim>() = getC();
    A.template bottomRightCorner<TOutputDim, TOutputDim>().setIdentity();

    Eigen::Matrix<TScalar, TStateDim + TOutputDim, TInputDim> B{};
    B.setZero();
    B.template topRows<TStateDim>() = getB();

    Eigen::Matrix<TScalar, TOutputDim, TStateDim + TOutputDim> C{};
    C.setZero();
    C.template leftCols<TStateDim>() = getC();
    C.template rightCols<TOutputDim>().setIdentity();

    Eigen::Matrix<TScalar, TOutputDim, TInputDim> D{};
    D = getD();

    ls::systems::StateSpace<T_TScalar, LS_STATIC_UNLESS_DYNAMIC(
            T_TStateDim + T_TOutputDim), T_TInputDim, T_TOutputDim> ssi{A, B, C, D};

    return ssi;
}

#endif //LODESTAR_STATESPACE_HPP


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

#include <Eigen/Dense>

#include "timer.h"

//! \brief Structure holding a triplet in format (row, col, value)
//! For convenience, we provide a void constructor and a init constructor. All members are public
//! Used in the TripletMatrix class to provide a nice wrapper for a triplet
//! \tparam scalar represents the type of the triplet (e.g. double)
template <class scalar>
struct Triplet {
    Triplet(void)
    : i(0), j(0), v(0) { }

    Triplet(std::size_t i_, std::size_t j_, scalar v_)
        : i(i_), j(j_), v(v_) { }

    std::size_t i, j; //< row and col
    scalar v; //< value @ (row,col)
};

//! \brief Defines a matrix stored in triplet format (using the Triplet<scalar> class.
//! Triplets may be duplicated and in *any* order. If there is a multiple triplet for (row,col) pair, we assume
//! that the values are intended to be added together
//! Also stores dimension
//! \tparam scalar represents the scalar type of the matrix (and of the triplet) (e.g. double)
template <class scalar>
struct TripletMatrix {
    std::size_t rows, cols; //< Sizes: nrows and ncols
    std::vector< Triplet<scalar> > triplets;

    //! \brief Converts *this to an eigen (dense) function, used for visualization
    //! Loops over all triplets and add value to Zero matrix (read-only)
    //! WARNING: just for debug, may fill-in a lot of nonzeros if n,m >> 1
    //! \return Eigen::Matrix of Dynamic size and scalar type
    Eigen::Matrix<scalar, -1, -1> densify() const {
        Eigen::Matrix<scalar, -1, -1> M = Eigen::Matrix<scalar, -1, -1>::Zero(rows, cols);
        for(auto it = triplets.begin(); it != triplets.end(); ++it) {
            M(it->i, it->j) += it->v;
        }
        return M;
    }
};

// const TripletMatrix;

//! \brief Structure holding a pair column index-value to be used in CRS format
//! Provides handy constructor and comparison operators.
//! \tparam scalar represents the scalar type of the value stored (e.g. double)
template <class scalar>
struct ColValPair {
    ColValPair(std::size_t col_, scalar v_)
        : col(col_), v(v_) { }

    //! \brief Comparison operator for std::sort and std::lower_bound
    //! Basic sorting operator < for use with std:: functions (i.e. for ordering according to first component (col)
    //! We keep the column values sorted, either by sorting after insertion of by sorted insertion
    //! \return true if this->col < other.col
    bool operator<(const ColValPair & other) const {
        return this->col < other.col;
    }

    std::size_t col; //< col index
    scalar v; //< scalar value at col
};

//! \brief Defines a matrix stored in CRS format (using the ColValPair<scalar> struct.
//! The row_pt contains the data, indexed by row and column position
//! Also stores dimension
//! \tparam scalar represents the scalar type of the matrix (and of the ColValPair) (e.g. double)
template <class scalar>
struct CRSMatrix {
    std::size_t rows, cols; //< Size of the matrix rows, cols
    std::vector< std::vector< ColValPair<scalar> > > row_pt; //< Vector containing, for each row, al vector of (col, value) pairs (CRS format)

    //! \brief Converts *this to an eigen (dense) function, used for visualization
    //! Loops over all rows and add value at col (in ColValPair) to Zero matrix (read-only)
    //! WARNING: just for debug, may fill-in a lot of nonzeros if n,m >> 1
    //! \return Eigen::Matrix of Dynamic size and scalar type
    Eigen::Matrix<scalar, -1, -1> densify() const {
        Eigen::Matrix<scalar, -1, -1> M = Eigen::Matrix<scalar, -1, -1>::Zero(rows, cols);
        std::size_t i = 0;
        for(auto it = row_pt.begin(); it != row_pt.end(); ++it) {
            for(auto it2 = it->begin(); it2 != it->end(); ++it2) {
                M(i, it2->col) = it2->v;
            }
            ++i;
        }
        return M;
    }
};

//! \brief Converts a matrix given as triplet matrix to a matrix in CRS format
//! No assumption is made on the triplets, may be unsorted and/or duplicated
//! In case of duplicated triplets, values are added toghether
//! The output CRS matrix may be empty (in that case T = C) or may be already filled with values (in which case C += T)
//! This version inserts the ColValParis already sorted in the list
//! Complexity: loops over all triplets (lets say k) and look up over all columns of the row (say n_i), performing a
//! linear search and an insertion (complexity O(n_i))
//! Assuming n_i is bounded by n small, complexity is k*n, otherwise k^2
template <class scalar>
void tripletToCRS(const TripletMatrix<scalar> & T, CRSMatrix<scalar> & C) {
    // Copy sizes and reserve memory for rows
    C.rows = T.rows;
    C.cols = T.cols;
    C.row_pt.resize(C.rows);

    // Loop over all triplets
    for(auto triplet_it = T.triplets.begin(); triplet_it != T.triplets.end(); ++triplet_it) {
        // Store row (containing the ColValPairs for this row) inside the row_pt vector
        auto & row = C.row_pt.at(triplet_it->i);
        // Create a ColVal pair that must be inserted somewhere
        ColValPair<scalar> col_val(triplet_it->j, triplet_it->v);
        // Find the place (as iterator) where to insert col_val, i.e. the first place where col_val < C.row_pt.at(i)
        // WARNING: Costly call (loops over all of row (worts case scenario))
        auto lb_it = std::lower_bound(row.begin(), row.end(), col_val);
        // If lower bound has already a col (and is not end()) with some value, just add the value to the column, othervise insert it
        if(lb_it != row.end() && lb_it->col == triplet_it->j) {
            lb_it->v += triplet_it->v;
        } else {
            // WARNING: Costly call (loops over all of row (worts case scenario))
            row.insert(lb_it, col_val);
        }
    }
}

//! \brief Converts a matrix given as triplet matrix to a matrix in CRS format
//! No assumption is made on the triplets, may be unsorted and/or duplicated
//! In case of duplicated triplets, values are added toghether
//! The output CRS matrix may be empty (in that case T = C) or may be already filled with values (in which case C += T)
//! This version inserts all the triplets (push_back) then sorts eatch row and cumsum al the duplicated col values
//! *** Complexity: loops over all triplets (lets say k) and do a cheap (amortized) o(1) push back (complexity k*1). Then sorts an array
//! of rows (ith quicksort complexity k * log(k))
template <class scalar>
void tripletToCRS_sortafter(const TripletMatrix<scalar> & T, CRSMatrix<scalar> & C) {
    // Copy dimensions and reserve known space
    C.rows = T.rows;
    C.cols = T.cols;
    C.row_pt.resize(C.rows);

    // Loops over all triplets and push them at the ritgh place (cheap)
    for(auto triplets_it = T.triplets.begin(); triplets_it != T.triplets.end(); ++triplets_it) {
        ColValPair<scalar> cp(triplets_it->j, triplets_it->v);
        C.row_pt.at(triplets_it->i).push_back(cp);
    }
    // Loops over all rows , sort them (according to col) and sum duplicated values
    for(auto row_it = C.row_pt.begin(); row_it != C.row_pt.end(); ++row_it) {
        // WARNING: costly call in this case
        std::sort(row_it->begin(), row_it->end());
        // Sum duplicated cols
        // NOTE: iterating backwards should be faster
        for(auto col_it = row_it->begin(); col_it != row_it->end(); ++col_it) {
            // Check that next col is not at the end and that has same col value
            if((col_it + 1) != row_it->end() && (col_it + 1)->col == col_it->col) {
                // Last col with same value (starting from next col)
                auto last_col_it = col_it + 1;
                while(last_col_it != row_it->end() && last_col_it->col == col_it->col) {
                    col_it->v += last_col_it->v;
                    ++last_col_it;
                }
                // Remove range from next col to last col with same value
                // WARNING: std::vector keeps the data as c-array, so mildly costly call (from col_it to end() (at most M)
                row_it->erase(col_it+1, last_col_it);
            }
        }
    }
}

//! \brief overload of operator << for output of Triplet Matrix (debug).
//! WARNING: uses densify() so there may be a lot of fill-in
//! \param o standard output stream
//! \param S matrix in Triplet matrix format
//! \return a ostream o, s.t. you can write o << A << B;
std::ostream & operator<<(std::ostream & o, const TripletMatrix<double> & S) {
    return o << S.densify();
}

//! \brief overload of operator << for output of CRS Matrix (debug).
//! WARNING: uses densify() so there may be a lot of fill-in
//! \param o standard output stream
//! \param S matrix in CRS matrix format
//! \return a ostream o, s.t. you can write o << A << B;
std::ostream & operator<<(std::ostream & o, const CRSMatrix<double> & S) {
    return o << S.densify();
}

int main() {
    //// Correctness test
    std::size_t nrows = 7, ncols = 5, ntriplets = 9;

    TripletMatrix<double> T;
    CRSMatrix<double> C,D;

    T.rows = nrows;
    T.cols = ncols;
    T.triplets.reserve(ntriplets); // Always reserve space if you can
    for(auto i = 0u; i < ntriplets; ++i) {
        // Test unordered triplets, random and maybe repeated triplets
        T.triplets.push_back(Triplet<double>(rand() % nrows, rand() % ncols, rand() % 1000));
    }

    std::cout << "***Test conversion with random matrices***" << std::endl;
    tripletToCRS(T, C);
    std::cout << "--> Frobenius norm of T - C: " << (T.densify()-C.densify()).norm() << std::endl;
    std::cout << "T = " << std::endl << T << std::endl;
    std::cout << "C = " << std::endl << C << std::endl;

    tripletToCRS_sortafter(T, D);
    std::cout << "--> Frobenius norm of T - D: " << (T.densify()-D.densify()).norm() << std::endl;
    std::cout << "D = " << D << std::endl;

    // Big benefit of how we defined our functions: can add new triplets to the old ones:
    std::cout << "***Test addition with random matrices***" << std::endl;
    TripletMatrix<double> T2;
    T2.rows = nrows;
    T2.cols = ncols;
    T2.triplets.reserve(ntriplets); // Always reserve space if you can
    for(auto i = 0u; i < ntriplets; ++i) {
        // Test unordered triplets, random and maybe repeated triplets
        T2.triplets.push_back(Triplet<double>(rand() % nrows, rand() % ncols, rand() % 1000));
    }
    tripletToCRS(T2, C);
    tripletToCRS_sortafter(T2, D);
    std::cout << "T = " << std::endl << T << std::endl;
    std::cout << "T2 = " << std::endl << T2 << std::endl;
    std::cout << "C = " << std::endl << C << std::endl;
    std::cout << "D = " << std::endl << D << std::endl;

    //// Runtime test
    bool noruntime = false; // Skip runtime measurments
    bool noaddition = true; // Do not test addition of new triplets

    if(noruntime) {
        return 0;
    }
    std::cout << "***Runtime test***" << std::endl;

    // Play around with this parameters (also introducing lambda functions)
    auto frows = [](std::size_t M) { return 2*M; };
    auto fcols = [](std::size_t M) { return M; };
    auto ftriplets = [](std::size_t M) { return M*5; };

    // Do some timings
    timer<> sortafter_timer, insertsort_timer;
    for(std::size_t M = 2u; M < 1024u; M *= 2u) {
        sortafter_timer.reset();
        insertsort_timer.reset();
        std::cout << "Runtime for " << M << "x" << M/2 << " matrix (with nnz(A) <= " << M << "):" << std::endl;
        for(int r = 0; r < 4; ++r) {

            TripletMatrix<double> A;
            CRSMatrix<double> B, E;

            A.rows = frows(M); // nrows
            A.cols = fcols(M); //ncols
            A.triplets.reserve(ftriplets(M));
            for(auto i = 0u; i < ftriplets(M); ++i) {
                A.triplets.push_back(Triplet<double>(rand() % A.rows, rand() % A.cols, rand() % 1000));
            }

            insertsort_timer.start();
            tripletToCRS(A, B);
            if(!noaddition) tripletToCRS(A, B);
            insertsort_timer.stop();

            sortafter_timer.start();
            tripletToCRS_sortafter(A, E);
            if(!noaddition) tripletToCRS_sortafter(A, E);
            sortafter_timer.stop();
        }
        std::cout << "Insertsort took:  " << insertsort_timer.min().count() / 1000. << " ms." << std::endl;
        std::cout << "Sortafter took:   " << sortafter_timer.min().count() / 1000. << " ms." << std::endl;
    }
}

Nummerical Methods for CSE/PS3/solutions_ps3/tripletToCRS_solution.cpp

Nummerical Methods for CSE/PS3/tripletToCRS_solution.cpp


/*=============================================================================
  Copyright (c) 2010-2016 Bolero MURAKAMI
  https://github.com/bolero-MURAKAMI/Sprig

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

#include <sprig/config/config.hpp>

#ifdef SPRIG_USING_PRAGMA_ONCE
#	pragma once
#endif	// #ifdef SPRIG_USING_PRAGMA_ONCE

#include <cstddef>
#include <cmath>
#include <boost/array.hpp>
#include <boost/call_traits.hpp>
#include <boost/numeric/ublas/matrix.hpp>

namespace sprig {
	namespace ublas {
		//
		// make_scaling_matrix_2d
		//
		template<typename T>
		SPRIG_INLINE boost::numeric::ublas::matrix<T> make_scaling_matrix_2d(T const sx, T const sy) {
			boost::numeric::ublas::matrix<T> mat = boost::numeric::ublas::identity_matrix<T>(3);
			mat(0, 0) = sx;
			mat(1, 1) = sy;
			return mat;
		}
		//
		// make_rotation_matrix_2d
		//
		template<typename T>
		SPRIG_INLINE boost::numeric::ublas::matrix<T> make_rotation_matrix_2d(T const a) {
			boost::numeric::ublas::matrix<T> mat = boost::numeric::ublas::identity_matrix<T>(3);
			mat(0, 0) = std::cos(a);
			mat(0, 1) = std::sin(a);
			mat(1, 0) = -mat(0, 1);
			mat(1, 1) = mat(0, 0);
			return mat;
		}
		//
		// make_translation_matrix_2d
		//
		template<typename T>
		SPRIG_INLINE boost::numeric::ublas::matrix<T> make_translation_matrix_2d(T const tx, T const ty) {
			boost::numeric::ublas::matrix<T> mat = boost::numeric::ublas::identity_matrix<T>(3);
			mat(2, 0) = tx;
			mat(2, 1) = ty;
			return mat;
		}
		//
		// matrix_prod
		//
		template<typename T>
		SPRIG_INLINE boost::numeric::ublas::matrix<T> matrix_prod(
			boost::numeric::ublas::matrix<T> const& m1,
			boost::numeric::ublas::matrix<T> const& m2
			)
		{
			return boost::numeric::ublas::prod(m1, m2);
		}
		template<typename T>
		SPRIG_INLINE boost::numeric::ublas::matrix<T> matrix_prod(
			boost::numeric::ublas::matrix<T> const& m1,
			boost::numeric::ublas::matrix<T> const& m2,
			boost::numeric::ublas::matrix<T> const& m3
			)
		{
			return boost::numeric::ublas::prod(matrix_prod(m1, m2), m3);
		}
		template<typename T>
		SPRIG_INLINE boost::numeric::ublas::matrix<T> matrix_prod(
			boost::numeric::ublas::matrix<T> const& m1,
			boost::numeric::ublas::matrix<T> const& m2,
			boost::numeric::ublas::matrix<T> const& m3,
			boost::numeric::ublas::matrix<T> const& m4
			)
		{
			return boost::numeric::ublas::prod(matrix_prod(m1, m2, m3), m4);
		}
		template<typename T>
		SPRIG_INLINE boost::numeric::ublas::matrix<T> matrix_prod(
			boost::numeric::ublas::matrix<T> const& m1,
			boost::numeric::ublas::matrix<T> const& m2,
			boost::numeric::ublas::matrix<T> const& m3,
			boost::numeric::ublas::matrix<T> const& m4,
			boost::numeric::ublas::matrix<T> const& m5
			)
		{
			return boost::numeric::ublas::prod(matrix_prod(m1, m2, m3, m4), m5);
		}
		//
		// scaling_2d
		//
		template<typename T>
		class scaling_2d {
		public:
			typedef T value_type;
			typedef boost::numeric::ublas::matrix<value_type> matrix_type;
		private:
			boost::array<value_type, 2> elems_;
		public:
			scaling_2d() {
				elems_[0] = value_type();
				elems_[1] = value_type();
			}
			scaling_2d(
				typename boost::call_traits<value_type>::param_type v1,
				typename boost::call_traits<value_type>::param_type v2
				)
			{
				elems_[0] = v1;
				elems_[1] = v2;
			}
			matrix_type matrix() const{
				return make_scaling_matrix_2d(elems_[0], elems_[1]);
			}
			template<std::size_t N>
			value_type const& get() const {
				return elems_[N];
			}
			template<std::size_t N>
			value_type& get() {
				return elems_[N];
			}
		};
		//
		// rotation_2d
		//
		template<typename T>
		class rotation_2d {
		public:
			typedef T value_type;
			typedef boost::numeric::ublas::matrix<value_type> matrix_type;
		private:
			boost::array<value_type, 1> elems_;
		public:
			rotation_2d() {
				elems_[0] = value_type();
			}
			rotation_2d(
				typename boost::call_traits<value_type>::param_type v1
				)
			{
				elems_[0] = v1;
			}
			matrix_type matrix() const{
				return make_rotation_matrix_2d(elems_[0]);
			}
			template<std::size_t N>
			value_type const& get() const {
				return elems_[N];
			}
			template<std::size_t N>
			value_type& get() {
				return elems_[N];
			}
		};
		//
		// translation_2d
		//
		template<typename T>
		class translation_2d {
		public:
			typedef T value_type;
			typedef boost::numeric::ublas::matrix<value_type> matrix_type;
		private:
			boost::array<value_type, 2> elems_;
		public:
			translation_2d() {
				elems_[0] = value_type();
				elems_[1] = value_type();
			}
			translation_2d(
				typename boost::call_traits<value_type>::param_type v1,
				typename boost::call_traits<value_type>::param_type v2
				)
			{
				elems_[0] = v1;
				elems_[1] = v2;
			}
			matrix_type matrix() const{
				return make_translation_matrix_2d(elems_[0], elems_[1]);
			}
			template<std::size_t N>
			value_type const& get() const {
				return elems_[N];
			}
			template<std::size_t N>
			value_type& get() {
				return elems_[N];
			}
		};
	}	// namespace ublas
}	// namespace sprig

#endif	// #ifndef SPRIG_UBLAS_MATRIX_HPP


#include <utility>
#include <vector>
#include <stack>
#include <algorithm>

#include <boost/geometry/geometry.hpp>

using namespace boost::geometry;

template
<
    typename Point
>
struct triangle {
    Point p1,p2,p3;
    std::vector<std::size_t> adjacent_indices;
};

template
<
    typename Area,
    typename Point
>
Area triangle_double_area(const Point& p1, const Point& p2, const Point& p3) {
    return std::abs(boost::numeric_cast<Area>(get<0>(p1))*boost::numeric_cast<Area>(get<1>(p2))-boost::numeric_cast<Area>(get<0>(p1))*boost::numeric_cast<Area>(get<1>(p3))+boost::numeric_cast<Area>(get<0>(p2))*boost::numeric_cast<Area>(get<1>(p3))-boost::numeric_cast<Area>(get<0>(p2))*boost::numeric_cast<Area>(get<1>(p1))+boost::numeric_cast<Area>(get<0>(p3))*boost::numeric_cast<Area>(get<1>(p1))-boost::numeric_cast<Area>(get<0>(p3))*boost::numeric_cast<Area>(get<1>(p2)));
}

template
<
    typename Point
>
double circumcircle_diameter(const Point& p1, const Point& p2, const Point& p3)
{
    return distance(p1,p2)*distance(p1,p3)*distance(p2,p3)/triangle_double_area<double,Point>(p1,p2,p3);
}

template
<
    typename Point
>
Point circumcircle_center(const Point& p1, const Point& p2, const Point& p3)
{
    auto ax = get<0>(p1);
    auto ay = get<1>(p1);
    auto bx = get<0>(p2);
    auto by = get<1>(p2);
    auto cx = get<0>(p3);
    auto cy = get<1>(p3);
    auto d = 2*(ax*(by-cy)+bx*(cy-ay)+cx*(ay-by));
    auto x = ((ax*ax+ay*ay)*(by-cy)+(bx*bx+by*by)*(cy-ay)+(cx*cx+cy*cy)*(ay-by))/d;
    auto y = ((ax*ax+ay*ay)*(cx-bx)+(bx*bx+by*by)*(ax-cx)+(cx*cx+cy*cy)*(bx-ax))/d;
    return make<Point>(x,y);
}

template
<
    typename Segment
>
double angle(const Segment& s)
{
    return std::atan2(get<1,1>(s)-get<0,1>(s),get<1,0>(s)-get<0,0>(s));
}

template
<
    typename Point,
    typename Segment
>
bool is_left_of(const Point& p, const Segment& s)
{
    return (get<0>(p)-get<0,0>(s))*(get<1,1>(s)-get<0,1>(s))-(get<1>(p)-get<0,1>(s))*(get<1,0>(s)-get<0,0>(s))<0;
}

template
<
    typename Point
>
bool adjacent_at_edge(const triangle<Point>& t1, const triangle<Point>& t2, const Point& p1, const Point& p2)
{
    //Very naive check, not necessary with proper triangulation data structure
    return (equals(p1,t1.p1) || equals(p1,t1.p2) || equals(p1,t1.p3))
        && (equals(p2,t1.p1) || equals(p2,t1.p2) || equals(p2,t1.p3))
        && (equals(p1,t2.p1) || equals(p1,t2.p2) || equals(p1,t2.p3))
        && (equals(p2,t2.p1) || equals(p2,t2.p2) || equals(p2,t2.p3));
}

template
<
    typename Point
>
std::vector<std::pair<std::size_t,std::size_t>> flip_if_not_locally_delaunay(std::vector<triangle<Point>>& triangles, std::size_t ti1, std::size_t ti2)
{
    triangle<Point>& t1 = triangles[ti1];
    if(std::find(t1.adjacent_indices.begin(),t1.adjacent_indices.end(),ti2)==t1.adjacent_indices.end())
        return {};
    triangle<Point>& t2 = triangles[ti2];
    Point *p1, *p2, *p3, *p4;
    if(!equals(t1.p1,t2.p1) && !equals(t1.p1,t2.p2) && !equals(t1.p1, t2.p3)) {
        p4 = &t1.p1;
        p1 = &t1.p2;
        p3 = &t1.p3;
    }
    else if(!equals(t1.p2,t2.p1) && !equals(t1.p2,t2.p2) && !equals(t1.p2, t2.p3)) {
        p4 = &t1.p2;
        p1 = &t1.p3;
        p3 = &t1.p1;
    }
    else {
        p4 = &t1.p3;
        p1 = &t1.p1;
        p3 = &t1.p2;
    }
    if(!equals(t2.p1,*p1) && !equals(t2.p1,*p3))
        p2 = &t2.p1;
    else if(!equals(t2.p2,*p1) && !equals(t2.p2,*p3))
        p2 = &t2.p2;
    else
        p2 = &t2.p3;
    bool loc_delaunay = distance(circumcircle_center(*p1,*p2,*p3),*p4)>circumcircle_diameter(*p1,*p2,*p3)/2;
    if(loc_delaunay)
        return {};
    std::size_t t1a1=-1, t2a1=-1;
    for(const auto& i : t1.adjacent_indices) {
        if(i!=ti2)
            if(adjacent_at_edge(t1, triangles[i], *p3, *p4))
                t1a1 = i;
    }
    for(const auto& i : t2.adjacent_indices) {
        if(i!=ti1)
            if(adjacent_at_edge(t2, triangles[i], *p1, *p2))
                t2a1 = i;
    }
    Point temp;
    assign(temp,*p3);
    assign(*p3,*p2);
    if(equals(t2.p1,temp))
        assign(t2.p2,*p4);
    else if(equals(t2.p2,temp))
        assign(t2.p3,*p4);
    else
        assign(t2.p1,*p4);
    if(t1a1!=-1 && t2a1!=-1) {
        std::replace(triangles[t1a1].adjacent_indices.begin(),triangles[t1a1].adjacent_indices.end(),ti1,ti2);
        std::replace(t1.adjacent_indices.begin(),t1.adjacent_indices.end(),t1a1,t2a1);
        std::replace(triangles[t2a1].adjacent_indices.begin(),triangles[t2a1].adjacent_indices.end(),ti2,ti1);
        std::replace(t2.adjacent_indices.begin(),t2.adjacent_indices.end(),t2a1,t1a1);
    }
    else if(t1a1!=-1) {
        std::replace(triangles[t1a1].adjacent_indices.begin(),triangles[t1a1].adjacent_indices.end(),ti1,ti2);
        t1.adjacent_indices.erase(std::find(t1.adjacent_indices.begin(),t1.adjacent_indices.end(),t1a1));
        t2.adjacent_indices.push_back(t1a1);
    }
    else if(t2a1!=-1) {
        std::replace(triangles[t2a1].adjacent_indices.begin(),triangles[t2a1].adjacent_indices.end(),ti2,ti1);
        t2.adjacent_indices.erase(std::find(t2.adjacent_indices.begin(),t2.adjacent_indices.end(),t2a1));
        t1.adjacent_indices.push_back(t2a1);
    }
    std::vector<std::pair<std::size_t,std::size_t>> outer_edges;
    for(const auto& t : {ti1,ti2}) {
        for(const auto& ta : triangles[t].adjacent_indices)
            if(ta != ti1 && ta != ti2)
                outer_edges.push_back(std::pair<std::size_t,std::size_t>(t,ta));
    }
    return outer_edges;
}

template
<
    typename MultiPoint
>
std::pair<std::vector<triangle<typename boost::range_value<MultiPoint>::type>>,std::vector<model::segment<typename boost::range_value<MultiPoint>::type>>> shull(const MultiPoint& input, bool skip_delaunay = false, double eps=0.0001)
{
    typedef typename boost::range_value<MultiPoint>::type Point;
    typedef triangle<Point> Triangle;
    std::vector<Point> points;
    points.reserve(boost::size(input));
    for(const auto& in : input)
        points.push_back(in);
    //Step 2
    std::sort(points.begin()+1, points.end(), [&points](const Point& p1, const Point& p2) {
       auto d1 = comparable_distance(points[0],p1);
       auto d2 = comparable_distance(points[0],p2);
       if(d1<d2)
           return true;
       else if(d2<d1)
           return false;
       if(get<0>(p1)<get<0>(p2) || (get<0>(p1)==get<0>(p2) && get<1>(p1)<get<1>(p2) ))
           return true;
       return false;});
    for(auto it=points.end()-1;it!=points.begin();--it) //remove almost-duplicates
        if(distance(*it,*(it-1))<=eps)
            it = points.erase(it);
    //Step 4
    double min_circumcircle_diameter = circumcircle_diameter(points[0],points[1],points[2]);
    std::size_t mcd_index = 2;
    for(std::size_t i=3;i<points.size();++i) {
        auto d = circumcircle_diameter(points[0],points[1],points[i]);
        if(d<min_circumcircle_diameter) {
            mcd_index=i;
            min_circumcircle_diameter=d;
        }
        if(distance(points[0],points[i])>min_circumcircle_diameter)
            break;
    }
    std::swap(points[2],points[mcd_index]);
    Point C = circumcircle_center(points[0], points[1], points[2]);

    //Step 5
    typedef typename model::segment<Point> Segment;
    typedef typename std::pair<Segment,std::size_t> indexed_segment;
    if(!is_left_of(points[2], Segment{points[0],points[1]}))
        std::swap(points[1],points[2]);
    std::sort(points.begin()+2, points.end(), [&C](const Point& p1, const Point& p2) {
       return comparable_distance(C,p1) < comparable_distance(C,p2);
    });

    std::vector<indexed_segment> outer_hull = {indexed_segment(Segment{points[0],points[1]},0),indexed_segment(Segment{points[1],points[2]},0),indexed_segment(Segment{points[2],points[0]},0)};
    std::vector<Triangle> triangles = {Triangle{points[0],points[1],points[2],{}}};

    auto outer_hull_pred = [](const indexed_segment& s1, const indexed_segment& s2) { return angle(s1.first)<angle(s2.first); };
    //Step 6
    std::sort(outer_hull.begin(), outer_hull.end(),outer_hull_pred);

    //Step 7
    for(std::size_t i = 3; i<points.size(); ++i) {
        const Point& p = points[i];
        std::size_t end_tri = triangles.size();
        //The following could (probably) be done faster with binary searches (for large hulls)
        auto begin = std::find_if(outer_hull.begin(), outer_hull.end(), [&p](const indexed_segment& s) {
            return !is_left_of(p,s.first);
        });
        auto end = std::find_if(begin, outer_hull.end(), [&p](const indexed_segment& s) {
            return is_left_of(p,s.first);
        });
        for(auto it = begin; it != end; ++it) {
            triangles.push_back(Triangle{it->first.first, p, it->first.second,{it->second}});
            triangles[it->second].adjacent_indices.push_back(triangles.size()-1);
            if(it!=begin) {
                triangles[triangles.size()-1].adjacent_indices.push_back(triangles.size()-2);
                triangles[triangles.size()-2].adjacent_indices.push_back(triangles.size()-1);
            }
        }
        indexed_segment new_outer1(Segment(begin->first.first,p),end_tri), new_outer2(Segment(p,(end-1)->first.second),triangles.size()-1);
        if(begin==outer_hull.begin()) {
            auto begin2 = std::find_if(end, outer_hull.end(), [&p](const indexed_segment& s) {
                return !is_left_of(p,s.first);
            });

            if(begin2 != outer_hull.end()) {
                std::size_t end_tri2 = triangles.size();
                for(auto it = begin2; it != outer_hull.end(); ++it) {
                    triangles.push_back(Triangle{it->first.first, p, it->first.second,{it->second}});
                    triangles[it->second].adjacent_indices.push_back(triangles.size()-1);
                    if(it!=begin2) {
                        triangles[triangles.size()-1].adjacent_indices.push_back(triangles.size()-2);
                        triangles[triangles.size()-2].adjacent_indices.push_back(triangles.size()-1);
                    }
                }
                triangles.back().adjacent_indices.push_back(end_tri);
                triangles[end_tri].adjacent_indices.push_back(triangles.size()-1);
                new_outer1 = indexed_segment(Segment(begin2->first.first,p), end_tri2);
                outer_hull.erase(begin2, outer_hull.end());
            }
        }
        outer_hull.erase(begin, end);
        for(const auto& s : {new_outer1, new_outer2})
            outer_hull.insert(std::upper_bound(outer_hull.begin(), outer_hull.end(), s, outer_hull_pred),s);
    }

    //Step 9
    if(!skip_delaunay) {
        std::stack<std::pair<std::size_t,std::size_t>> L;
        for(std::size_t i=0; i < triangles.size(); ++i)
            for(const auto& a : triangles[i].adjacent_indices)
                if(i<a)
                    L.emplace(i,a);
        while(!L.empty()) {
            auto edge = L.top();
            L.pop();
            auto new_edges = flip_if_not_locally_delaunay<Point>(triangles, edge.first, edge.second);
            for(const auto& ne : new_edges)
                L.push(ne);
        }
    }
    std::vector<Segment> no_index_hull;
    no_index_hull.reserve(outer_hull.size());
    for(const auto& si : outer_hull)
        no_index_hull.push_back(si.first);
    return std::make_pair(triangles,no_index_hull);
}


/*
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).
*/

// Modified Jonker-Volgenant Algorithm
// DF Crouse. On implementing 2D rectangular assignment algorithms.
// IEEE Transactions on Aerospace and Electronic Systems, 52(4):1679-1696, August 2016,

#pragma once

#include "AlgorithmUtils.hpp"
#include "FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include <Eigen/Core>
#include <cassert>
#include <cmath>

namespace fluid {
namespace algorithm {

class Assign2D
{
public:
  using ArrayXd = Eigen::ArrayXd;
  using ArrayXXd = Eigen::ArrayXXd;
  using ArrayXi = Eigen::ArrayXi;
  static const index UNASSIGNED = -1;
  bool process(Eigen::Ref<const Eigen::ArrayXXd> costMatrix,
               Eigen::Ref<Eigen::ArrayXi>        result)
  {
    using namespace std;
    using namespace Eigen;
    double minCost = costMatrix.minCoeff();
    if(minCost > 0) minCost = 0;
    mCost = costMatrix - minCost;
    if(mCost.cols() > mCost.rows()){
      mCost.transposeInPlace();
      mTransposed = true;
    }
    else mTransposed = false;

    mRow2Col = ArrayXi::Constant(mCost.rows(), UNASSIGNED);
    mCol2Row = ArrayXi::Constant(mCost.cols(), UNASSIGNED);
    mV = ArrayXd::Zero(mCost.rows());
    mU = ArrayXd::Zero(mCost.cols());
    for (index c = 0; c < mCost.cols(); c++){
      index sink = shortestPath(c);
      if(sink == UNASSIGNED) return false;
      index j = sink;
      index h;
      while(true){
        index i = mPred(j);
        mRow2Col(j) = i;
        h = mCol2Row(i);
        mCol2Row(i) = j;
        j = h;
        if(i == c) break;
      }
    }
    result = mTransposed?mCol2Row:mRow2Col;
    return true;
  }

  index shortestPath(index c){
    using namespace std;
    mPred = ArrayXi::Zero(mCost.rows());
    ArrayXi scannedCols = ArrayXi::Zero(mCost.cols());
    ArrayXi scannedRows = ArrayXi::Zero(mCost.rows());
    vector<int> row2Scan(mCost.rows());
    iota(row2Scan.begin(), row2Scan.end(), 0);
    index nRows2Scan = mCost.rows();
    index sink = UNASSIGNED;
    double delta = 0;
    index currentCol = c;
    index currentRow, currentClosestRow, closestRow;
    double minVal, reducedCost;
    ArrayXd shortestPathCost = ArrayXd::Constant(mCost.rows(), infinity);
    while(sink == UNASSIGNED){
      scannedCols(currentCol) = 1;
      minVal = infinity;
      for(index r = 0; r < nRows2Scan; r++){
        currentRow = row2Scan[r];
        reducedCost = delta
                    + mCost(currentRow, currentCol)
                    - mU(currentCol) - mV(currentRow);
        if(reducedCost < shortestPathCost(currentRow)){
          mPred(currentRow) = currentCol;
          shortestPathCost(currentRow) = reducedCost;
        }
        if(shortestPathCost(currentRow) < minVal){
          minVal = shortestPathCost(currentRow);
          currentClosestRow = r;
        }
      }
      if(isinf(minVal)) return UNASSIGNED;
      closestRow = row2Scan[currentClosestRow];
      scannedRows(closestRow) = 1;
      nRows2Scan--;
      row2Scan.erase(row2Scan.begin() + currentClosestRow);
      delta = shortestPathCost(closestRow);
      if(mRow2Col(closestRow) == UNASSIGNED) sink = closestRow;
      else currentCol = mRow2Col(closestRow);
    }
    mU(c) = mU(c) + delta;
    for(index i = 0; i < mCost.cols(); i++){
      if(i !=c && scannedCols(i) != 0){
        mU(i) = mU(i) + delta - shortestPathCost(mCol2Row(i));
      }
    }

    for(index i = 0; i < mCost.rows(); i++){
      if(scannedRows(i) != 0){
        mV(i) = mV(i) - delta + shortestPathCost(i);
      }
    }
    return sink;
}
private:
  ArrayXXd mCost;
  ArrayXi  mRow2Col;
  ArrayXi  mCol2Row;
  ArrayXd  mU;
  ArrayXd  mV;
  ArrayXi mPred;
  bool mTransposed{false};
};
} // namespace algorithm
} // namespace fluid


#include <mex.h>
#include <math.h>
#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include <string>

using namespace Eigen;
using namespace std;

double amips_param;
double c1_param;
double c2_param;
double d1_param;

Vector2d energy_derivative(int type, Vector2d S)
{

    Vector2d es;
    double J;
    double l1;
    double l2;

    switch(type)
    {
        case 0: //arap
            es[0] = 2 * (S[0] - 1);
            es[1] = 2 * (S[1] - 1);
            break;

        case 1: // mips
            es[0] = 1.0 / S[1] - S[1] / (S[0] * S[0]);
            es[1] = 1.0 / S[0] - S[0] / (S[1] * S[1]);
            //es[0] *= 100;
            //es[1] *= 100;
            break;

        case 2: // iso
            es[0] = 2 * S[0] - 2.0 / (S[0] * S[0] * S[0]);
            es[1] = 2 * S[1] - 2.0 / (S[1] * S[1] * S[1]);
            break;

        case 3: // amips
            es[0] = amips_param * exp(amips_param * (S[0] / S[1] + S[1] / S[0])) * (1.0 / S[1] - S[1] / (S[0] * S[0]));
            es[1] = amips_param * exp(amips_param * (S[0] / S[1] + S[1] / S[0])) * (1.0 / S[0] - S[0] / (S[1] * S[1]));
            break;

        case 4: // conf
            es[0] = 2 * S[0] / (S[1] * S[1]);
            es[1] = -2 * S[0] * S[0] / (S[1] * S[1] * S[1]);
            break;

        case 5: // gmr
						//            J = S[0] * S[1];
						//            l1 = S[0] * S[0] + S[1] * S[1];
						//            l2 = S[0] * S[0] * S[1] * S[1];
						//
						//            es[0] = c1_param * (-2.0 / 3.0 * pow(J, -5.0 / 3.0) * S[1] * l1 + pow(J, -2.0 / 3.0) * 2 * S[0])
						//                    + c2_param * (-4.0 / 3.0 * pow(J, -7.0 / 3.0) * S[1] * l2 + pow(J, -4.0 / 3.0) * 2 * S[1] * S[1] * S[0])
						//                    + d1_param * 2 * (J - 1) * S[1];
						//
						//            es[1] = c1_param * (-2.0 / 3.0 * pow(J, -5.0 / 3.0) * S[0] * l1 + pow(J, -2.0 / 3.0) * 2 * S[1])
						//                    + c2_param * (-4.0 / 3.0 * pow(J, -7.0 / 3.0) * S[0] * l2 + pow(J, -4.0 / 3.0) * 2 * S[0] * S[0] * S[1])
						//                    + d1_param * 2 * (J - 1) * S[0];

						es[0] = c1_param * (1.0 / S[1] - S[1] / (S[0] * S[0])) - d1_param *2.*S[1]*(1.0 - S[0]*S[1]);
						es[1] = c1_param * (1.0 / S[0] - S[0] / (S[1] * S[1])) - d1_param *2.*S[0]*(1.0 - S[0]*S[1]);

						break;

        case 6: // olg

            if(S[0] > 1.0 / S[1])
            {
                es[0] = 1;
                es[1] = 0;
            }else{
                es[0] = 0;
                es[1] = -1.0 / (S[1] * S[1]);
            }

            break;
    }

    return es;

}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{

    mxArray *output_mex, *J_value_mex, *JT_value_mex, *wu_mex, *bu_mex;
    const int *dims;
    double *tri_num, *X_g_inv, *tri_areas, *obj_tri, *q_target, *type, *amips_s, *F_dot, *ver_num, *c1_g, *c2_g, *d1_g, *J_index, *JT_index, *JTJ_info, *x2u;
    double *output, *J_value, *JT_value, *wu, *bu;

    tri_num = mxGetPr(prhs[0]);
    X_g_inv = mxGetPr(prhs[1]);
    tri_areas = mxGetPr(prhs[2]);
    obj_tri = mxGetPr(prhs[3]);
    q_target = mxGetPr(prhs[4]);
    type = mxGetPr(prhs[5]);
    amips_s = mxGetPr(prhs[6]);
    F_dot = mxGetPr(prhs[7]);
    ver_num = mxGetPr(prhs[8]);
    c1_g = mxGetPr(prhs[9]);
    c2_g = mxGetPr(prhs[10]);
    d1_g = mxGetPr(prhs[11]);
    J_index = mxGetPr(prhs[12]);
    JT_index = mxGetPr(prhs[13]);
    JTJ_info = mxGetPr(prhs[14]);
    x2u = mxGetPr(prhs[15]);

    int tri_n = tri_num[0];
    int ver_n = ver_num[0];
    int energy_type = type[0];
    amips_param = amips_s[0];
    c1_param = c1_g[0];
    c2_param = c2_g[0];
    d1_param = d1_g[0];
    int J_rn = JTJ_info[0];
    int J_cn = JTJ_info[1];
    int JT_rn = JTJ_info[2];
    int JT_cn = JTJ_info[3];

    output_mex = plhs[0] = mxCreateDoubleMatrix(2 * ver_n, 1, mxREAL);
    J_value_mex = plhs[1] = mxCreateDoubleMatrix(J_rn * J_cn, 1, mxREAL);
    JT_value_mex = plhs[2] = mxCreateDoubleMatrix(JT_rn * JT_cn, 1, mxREAL);
    wu_mex = plhs[3] = mxCreateDoubleMatrix(tri_n, 1, mxREAL);
    bu_mex = plhs[4] = mxCreateDoubleMatrix(tri_n, 1, mxREAL);

    output = mxGetPr(output_mex);
    J_value = mxGetPr(J_value_mex);
    JT_value = mxGetPr(JT_value_mex);
    wu = mxGetPr(wu_mex);
    bu = mxGetPr(bu_mex);

    memset(J_value, 0, J_rn * J_cn * sizeof(double));
    memset(JT_value, 0, JT_rn * JT_cn * sizeof(double));
    memset(output, 0, 2 * ver_n * sizeof(double));

    vector<int> offset;

    offset.resize(2 * ver_n, 0);

    int tri[3];
    double tri_area;

    Matrix2d B, X_f, A, F_d;
    Vector2d S, u0, u1, v0, v1;
    Matrix2d U, V;

    Vector2d es;
    Vector2d cs;

    double es1, es2;
    double d1, d2;
    double dphi;

    for(int i = 0; i < tri_n; i++)
    {
        tri[0] = obj_tri[i] - 1;
        tri[1] = obj_tri[i + tri_n] - 1;
        tri[2] = obj_tri[i + 2 * tri_n] - 1;
        tri_area = tri_areas[i];

        B(0, 0) = X_g_inv[i];
        B(0, 1) = X_g_inv[i + tri_n * 2];
        B(1, 0) = X_g_inv[i + tri_n];
        B(1, 1) = X_g_inv[i + tri_n * 3];

        X_f(0, 0) = q_target[2 * tri[1]] - q_target[2 * tri[0]];
        X_f(1, 0) = q_target[2 * tri[1] + 1] - q_target[2 * tri[0] + 1];
        X_f(0, 1) = q_target[2 * tri[2]] - q_target[2 * tri[0]];
        X_f(1, 1) = q_target[2 * tri[2] + 1] - q_target[2 * tri[0] + 1];

        A = X_f * B;

        JacobiSVD<Matrix2d> svd(A, ComputeFullU | ComputeFullV);

        S = svd.singularValues();
        U = svd.matrixU();
        V = svd.matrixV();

        if(A.determinant() <= 0)
        {
            //mexPrintf("inverted\n");
        }

        bu[i] = S[0] * S[1];

        es = energy_derivative(energy_type, S);

        cs = S;

        es1 = es[0];
        es2 = es[1];

        ////////////////////////////////////////////

        wu[i] = 0;

        u0 = U.col(0);
        u1 = U.col(1);
        v0 = V.col(0);
        v1 = V.col(1);

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

            F_d(0, 0) = F_dot[i + j * tri_n];
            F_d(0, 1) = F_dot[i + j * tri_n + 6 * 2 * tri_n];
            F_d(1, 0) = F_dot[i + j * tri_n + 6 * tri_n];
            F_d(1, 1) = F_dot[i + j * tri_n + 6 * 3 * tri_n];

            d1 = u0.transpose() * F_d * v0;
            d2 = u1.transpose() * F_d * v1;

            dphi = d1 * es1 + d2 * es2;

            int cache_1 = 2 * tri[j / 2] + (j % 2);
            int cache_2 = x2u[cache_1];

            output[cache_1] += tri_area * dphi;

            if(cache_2 > 0)
            {
                int idx = cache_2 - 1;

                double value = d1 * cs[1] + d2 * cs[0];

                J_value[idx * J_cn + offset[cache_1]] = value;

                offset[cache_1]++;

                JT_value[i * JT_cn + j] = value;

                wu[i] += value * value;
            }
        }
    }

    return;

}


#include "Kinematics.h"
#include <Eigen/SVD>

using namespace MotionControl;

Matrix<double,3,3> Kinematics::Rodrigues(Matrix<double,3,1> a, double q)
{
	return AngleAxisd(q,a).toRotationMatrix();
}

Matrix<double,3,1> Kinematics::rot2omega(Matrix<double,3,3> R)
{
	double alpha = (R(0,0)+R(1,1)+R(2,2)-1)/2;
	double th;
	Matrix<double,3,1> vector_R(Matrix<double,3,1>::Zero());

	if(fabs(alpha-1) < eps)
		return Matrix<double,3,1>::Zero();

	th = std::cos(alpha);
	vector_R << R(2,1)-R(1,2), R(0,2)-R(2,0), R(1,0)-R(0,1);
	return 0.5*th/std::sin(th)*vector_R;
}

Eigen::Matrix<double,3,3> Kinematics::computeMatrixFromAngles(double roll, double pitch, double yaw)
{
	Eigen::Matrix<double,3,3> R;

	R(0,0) = std::cos(pitch) * std::cos(yaw) - std::sin(roll) * std::sin(pitch) * std::sin(yaw);
	R(0,1) = -std::cos(roll) * std::sin(yaw);
	R(0,2) = std::sin(roll) * std::cos(yaw) + std::sin(roll) * std::cos(pitch) * std::sin(yaw);
	R(1,0) = std::cos(pitch) * std::sin(yaw) + std::sin(roll) * std::sin(pitch) * std::cos(yaw);
	R(1,1) = std::cos(roll) * std::cos(yaw);
	R(1,2) = std::sin(pitch) * std::sin(yaw) - std::sin(roll) * std::cos(pitch) * std::cos(yaw);
	R(2,0) = -std::cos(roll) * std::sin(pitch);
	R(2,1) = std::sin(roll);
	R(2,2) = std::cos(roll) * std::cos(pitch);

	return R;
}

void Kinematics::computeAnglesFromMatrix(Eigen::Matrix<double,3,3> R, double &roll, double &pitch, double &yaw)
{
	float threshold = 0.001;
	if(abs(R(2,1) - 1.0) < threshold){ // R(2,1) = std::sin(x) = 1の時
		roll 	= M_PI / 2;
		pitch = 0;
		yaw 	= std::atan2(R(1,0), R(0,0));
	}else if(abs(R(2,1) + 1.0) < threshold){ // R(2,1) = std::sin(x) = -1の時
		roll	= - M_PI / 2;
		pitch = 0;
		yaw		= std::atan2(R(1,0), R(0,0));
	}else{
		roll	= std::sin(R(2,1));
		pitch	= std::atan2(-R(2,0), R(2,2));
		yaw		= std::atan2(-R(0,1), R(1,1));
	}
}

vector<int> Kinematics::FindRoute(int to)
{
	vector<int> idx;
	int link_num = to;

	while(link_num != 0)
	{
		idx.push_back(link_num);
		link_num = ulink[link_num].parent;
	}
	reverse(idx.begin(), idx.end());
	return idx;
}

Matrix<double,6,1> Kinematics::calcVWerr(Link Cref, Link Cnow)
{
	Matrix<double,3,1> perr = Cref.p - Cnow.p;
	Matrix<double,3,3> Rerr = Cref.R - Cnow.R;
	Matrix<double,3,1> werr = Cnow.R * rot2omega(Rerr);
	Matrix<double,6,1> err;

	err << perr,werr;
	return err;
}

void Kinematics::calcForwardKinematics(int rootlink)
{
	if(rootlink == -1) return;
	if(rootlink != 0)
	{
		int parent = ulink[rootlink].parent;
		ulink[rootlink].p = ulink[parent].R * ulink[rootlink].b + ulink[parent].p;
		ulink[rootlink].R = ulink[parent].R * Rodrigues(ulink[rootlink].a, ulink[rootlink].q);
	}
	calcForwardKinematics(ulink[rootlink].sister);
	calcForwardKinematics(ulink[rootlink].child);
}

// 逆運動学
bool Kinematics::calcInverseKinematics(int to, Link target)
{
	Matrix<double,6,6> J;
	Matrix<double,6,1> dq, err;

	ColPivHouseholderQR<MatrixXd> QR; //QR分解?
	const double lambda = 0.5;
	const int iteration = 100;

	calcForwardKinematics(CC);
	vector<int> idx = FindRoute(to);

	for(int n=0;n<iteration;n++){
		J = calcJacobian(ulink, idx);
		err = calcVWerr(target, ulink[to]);
		if(err.norm() < eps) return true;
		dq = lambda * (J.inverse() * err);
		for(size_t nn=0;nn<idx.size();nn++){
			int j = idx[nn];
			ulink[j].q += dq(nn);
		}
		calcForwardKinematics(CC);
	}
	return false;
}

void Kinematics::MoveJoints( vector<int> idx, MatrixXd dq )
{
	for(int n=0; n<idx.size(); n++)
	{
		int j = idx[n];
		ulink[j].q += dq(n);
	}
}

bool Kinematics::calcLMInverseKinematics(int to, Link target)
{
	MatrixXd J, Jh, dq;
	vector<int> idx = FindRoute(to);
	double wn_pos = 1/0.3;
	double wn_ang = 1/(2*pi);
	double lambda, Ek, Ek2;
	Matrix< double, 6,1 > we, err, gerr;
	we << wn_pos, wn_pos, wn_pos, wn_ang, wn_ang, wn_ang;
	Matrix< double, 6,6 >We = we.array().matrix().asDiagonal();
	MatrixXd Wn = MatrixXd::Identity(idx.size(),idx.size());

	calcForwardKinematics(CC);
	err = calcVWerr( target, ulink[to]);
	Ek = err.transpose() * We * err;

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

		J = calcJacobian( ulink, idx );
		lambda = Ek + 0.002;
		Jh   = J.transpose() * We * J + Wn * lambda; //Hk + wn
		gerr = J.transpose() * We * err; // gk

		dq = Jh.inverse() * gerr; // new qk
		MoveJoints(idx, dq);

		Ek2 = gerr.transpose() * We * gerr;

		if(Ek2 < 1E-12)
		{
			return true;
		}
		else if( Ek2 < Ek ) Ek = Ek2;
		else
		{
			MoveJoints(idx, -dq);
			return true;
		}
	}
	return false;
}


#include <Eigen/Dense>
#include <iostream>
#include <set>
#include <time.h>
#include <fstream>
#include <vector>
#include <math.h>

using namespace std;
using namespace Eigen;

// N * E = B * T
#define N 128*150	// number of training data samples
#define D 784	// number of features
#define T 150	// total number of iterations per epoch
#define B_size 128	// size of mini-batch
#define ALPHA 128	// 2^7
#define epoch 1
#define SCALING_FACTOR 8192

typedef Matrix<unsigned long long int, Dynamic, Dynamic> MATRIX;
typedef Matrix<double, Dynamic, Dynamic> MATRIXd;
typedef Matrix<unsigned long long int, Dynamic, 1, ColMajor> ColVectorXi64;
typedef Matrix<double, Dynamic, 1, ColMajor> ColVectorXd;
typedef Matrix<double, 1, Dynamic, RowMajor> RowVectorXd;
typedef Matrix<unsigned long long int, 1, Dynamic, RowMajor> RowVectorXi64;

// Simulates dditive shares created by the clients and distributed to the two servers.
void genDataShares(MATRIX& X0, MATRIX& X1, MATRIX& Y0, MATRIX& Y1);

// Initialize W0 and W1 to be random
void initializeW(MATRIX& W0, MATRIX& W1);

// Simulates generation of U, V, Z multiplication triplets
void genUVZmultTriplets(MATRIX& U0, MATRIX& U1, MATRIX& V0, MATRIX& V1, MATRIX& Z0, MATRIX& Z1);

// Simulates generation of V', Z' multiplication triplets
void genVZprimeMultTriplets(MATRIX& UB0, MATRIX& UB1, MATRIX& Vp0, MATRIX& Vp1, MATRIX& Zp0, MATRIX& Zp1);

// Returns set of random indices corresponding to training data for current minibatch(iteration)
set<int> createMiniBatch(set<int>& usedIndices);

// Reconstructs M, by adding MO and M1
void reconstruct(MATRIX M0, MATRIX M1, MATRIX& M);

void maskMatrix(MATRIX Data, MATRIX Mask, MATRIX& Final);

void computeDifference(MATRIX& Dif, MATRIX Y_, MATRIX Y);

int reverse_int(int i);

void read_MNIST_data(bool train, vector<vector<unsigned long long int>>& vec, int& number_of_images, int& number_of_features);

void read_MNIST_labels(bool train, vector<unsigned long long int>& vec);

void read_MNIST_data_test(bool train, vector<vector<double>>& vec, int& number_of_images, int& number_of_features);

void read_MNIST_labels_test(bool train, vector<double>& vec);

void secretShareData(MATRIX& X, MATRIX& Y, MATRIX& X0, MATRIX& X1, MATRIX& Y0, MATRIX& Y1);

int main() {

	cout << unsigned long int(-28145) % 4294967296 << endl;

	// READ TRAINING DATA
	vector<vector<unsigned long long int>> training_data;
	int param_n= N; int param_d= D;
	read_MNIST_data(true, training_data, param_n, param_d);
	MATRIX X(param_n, param_d);
	for(int i = 0; i < training_data.size(); i++){
        X.row(i) << Map<RowVectorXi64>(training_data[i].data(), training_data[i].size());
    }
	X *= SCALING_FACTOR;
	X /= 255;
	//cout << X << endl;
	//cout << X.rows() << " " << X.cols() << endl;
	/*for (int i = 0 ; i < 784 ; ++i) {
		cout << training_data.at(0).at(i) << " ";
	}
	cout << endl<<  X.row(0) << endl;*/

	// READ TRAINING LABEL
	vector<unsigned long long int> training_labels;
	read_MNIST_labels(true, training_labels);
	MATRIX Y(N, 1);
	//for (int i = 0; i < training_labels.size(); ++i) {
	//	Y(i) = training_labels.at(i);
	//}
	Y << Map<ColVectorXi64>(training_labels.data(), training_labels.size());
	Y *= SCALING_FACTOR;
	Y /= 10;

	cout << sizeof(unsigned long long int) << endl;
	cout << sizeof(uint64_t) << endl;

	// READ TESTING DATA
	vector<vector<double> > testing_data;
	int n_;
	//param_n= N; param_d= D;
	read_MNIST_data_test(false, testing_data, n_, param_d);
	MATRIXd Xt(n_, param_d);
	for(int i = 0; i < testing_data.size(); i++){
        Xt.row(i) << Map<RowVectorXd>(testing_data[i].data(), testing_data[i].size());
    }
	//Xt *= SCALING_FACTOR;
	Xt /= 255;

	// READING TESTING LABEL
	vector<double> testing_labels;
	read_MNIST_labels_test(false, testing_labels);
	MATRIXd Yt(n_, 1);
	//for (int i = 0; i < training_labels.size(); ++i) {
	//	Y(i) = training_labels.at(i);
	//}
	Yt << Map<ColVectorXd>(testing_labels.data(), testing_labels.size());
	//Yt *= SCALING_FACTOR;
	//Yt /= 10;

	/*for (long int n : training_labels) {
		cout << n << endl;
	}
	cout << training_labels.size() << endl;*/
	//cout << Yt << endl;
	//cout << Y.rows() << " " << Y.cols() << endl;

	//print out numbers
	/*int c = 0;
	for (int i = 0; i < 784; ++i) {
		if (c % 28 == 0) {
			cout << endl;
		}
		cout << X(2, i) << " ";
		++c;
	}
	cout << training_labels.at(2) << endl;*/
	//cout << Xt.row(0) << endl;

	/*START OF PROTOCOL*/

	// shares of training data
	clock_t start = clock();
	MATRIX X0(N, D);	MATRIX X1(N, D);
	MATRIX Y0(N, 1);	MATRIX Y1(N, 1);
	//genDataShares(X0, X1, Y0, Y1);
	secretShareData(X, Y, X0, X1, Y0, Y1);

	// tests:
	/*cout << "X0 MATRIX:\n" << X0 << endl;
	cout << "X1 MATRIX:\n" << X1 << endl;
	cout << "<X>:\n" << X0 + X1 << endl;*/
	/*cout << "Y0 MATRIX:\n" << Y0 << endl;
	cout << "Y1 MATRIX:\n" << Y1 << endl;
	cout << "<Y>:\n" << Y0 + Y1 << endl;*/

	// coefficient vertor w
	MATRIX W0(D, 1);	MATRIX W1(D, 1);
	initializeW(W0, W1);

	// tests:
	/*cout << "W0:\n" << W0 << endl;
	cout << "W1:\n" << W1 << endl;*/

	// generate shared U, V, Z multiplication triplets
	MATRIX U0(N, D);	MATRIX U1(N, D);
	MATRIX V0(D, T);	MATRIX V1(D, T);
	MATRIX Z0(N, T);	MATRIX Z1(N, T);
	genUVZmultTriplets(U0, U1, V0, V1, Z0, Z1);

	// tests:
	/*cout << "MATRIX U0: \n" << U0 << endl;
	cout << "MATRIX U1: \n" << U1 << endl;
	cout << "MATRIX V0: \n" << V0 << endl;
	cout << "MATRIX V1: \n" << V1 << endl;
	cout << "MATRIX Z0: \n " << Z0 << endl;
	cout << "MATRIX Z1: \n " << Z1 << endl;*/

	// * STEP 1: each party runs Ei = Xi - Ui masking
	MATRIX E0(N, D);	MATRIX E1(N, D);	MATRIX E(N, D);
	maskMatrix(X0, U0, E0);
	maskMatrix(X1, U1, E1);
	reconstruct(E0, E1, E);		// |?| how to code how each party does this independently

	// tests:
	/*cout << "MATRIX E0:\n" << E0 << endl;
	cout << "MATRIX E1:\n" << E1 << endl;
	cout << "MATRIX E: \n" << E << endl;*/

	// * STEP 2: start of each iteration
	set<int> usedIndices;
	for (int j = 0; j < T; ++j) {

		//cout << "Iteration " << j+1 << "..." << endl;

		// * STEP 3: select minibatch
		set<int> minibatch = createMiniBatch(usedIndices); // select minibatch indices

		// tests:
		/*cout << "minibatch:" << endl;
		for (int n : minibatch) {
			cout << n << " ";
		}
		cout << endl;*/

		// submatrices size B
		MATRIX EB(B_size, D);
		MATRIX UB0(B_size, D);	MATRIX UB1(B_size, D);
		MATRIX XB0(B_size, D);	MATRIX XB1(B_size, D);
		MATRIX YB0(B_size, 1);	MATRIX YB1(B_size, 1);
		MATRIX ZB0(B_size, T);	MATRIX ZB1(B_size, T);

		int r = 0;	int k = 0;
		for (int n : minibatch) {
			k = n - 1;
			EB.row(r) = E.row(k);
			UB0.row(r) = U0.row(k);
			UB1.row(r) = U1.row(k);
			XB0.row(r) = X0.row(k);
			XB1.row(r) = X1.row(k);
			YB0.row(r) = Y0.row(k);
			YB1.row(r) = Y1.row(k);
			ZB0.row(r) = Z0.row(k);
			ZB1.row(r) = Z1.row(k);
			r++;
		}

		// tests:
		/*cout << "MATRIX EB:\n" << EB << endl;
		cout << "MATRIX UB0:\n" << UB0 << endl;
		cout << "MATRIX UB1:\n" << UB1 << endl;
		cout << "MATRIX XB0:\n" << XB0 << endl;
		cout << "MATRIX XB1:\n" << XB1 << endl;
		cout << "MATRIX YB0:\n" << YB0 << endl;
		cout << "MATRIX YB1:\n" << YB1 << endl;
		cout << "MATRIX ZB0:\n" << ZB0 << endl;
		cout << "MATRIX ZB1:\n" << ZB1 << endl;*/

		// generate V' and Zp multiplication triplets based on minibatch indices
		MATRIX Vp0(B_size, T);	MATRIX Vp1(B_size, T);
		MATRIX Zp0(D, T);		MATRIX Zp1(D, T);
		genVZprimeMultTriplets(UB0, UB1, Vp0, Vp1, Zp0, Zp1);

		// tests:
	/*	cout << "matrix Vp0: \n" << Vp0 << endl;
		cout << "matrix Vp1: \n" << Vp1 << endl;
		cout << "matrix Zp0: \n" << Zp0 << endl;
		cout << "matrix Zp1: \n" << Zp1 << endl;*/

		//// * STEP 4: each party computes Fji = wi - V[j]i	jth iteration
		MATRIX F0(D, 1);	MATRIX F1(D, 1);	MATRIX F(D, 1);
		MATRIX V0j = V0.col(j);
		MATRIX V1j = V1.col(j);
		maskMatrix(W0, V0j, F0);
		maskMatrix(W1, V1j, F1);
		reconstruct(F0, F1, F);

		// tests:
		/*cout << "MATRIX V0j: \n" << V0j << endl;
		cout << "MATRIX F0:\n" << F0 << endl;
		cout << "MATRIX V1j: \n" << V1j << endl;
		cout << "MATRIX F1:\n" << F1 << endl;
		cout << "MATRIX F:\n" << F << endl;*/

		// * STEP 5: calculate predicted output
		MATRIX Ypred_0 = -0 * (EB * F) + (XB0 * F) + (EB * W0) + ZB0.col(j);	// replace with j
		MATRIX Ypred_1 = -1 * (EB * F) + (XB1 * F) + (EB * W1) + ZB1.col(j);	// replace with j

		// tests:
		/*cout << "EB\n" << EB << endl;
		cout << "XB0\n" << XB0<< endl;
		cout << "XB1\n" << XB1<< endl;
		cout << "ZB0.col(0)\n" << ZB0.col(0) << endl;
		cout << "ZB1.col(0)\n" << ZB1.col(0) << endl;
		cout << "Ypred_0:\n" << Ypred_0 << endl;
		cout << "Ypred_1:\n" << Ypred_1 << endl;*/

		// * STEP 6:
		MATRIX D0; MATRIX D1;
		computeDifference(D0, Ypred_0, YB0);
		computeDifference(D1, Ypred_1, YB1);

		// tests:
		/*cout << "Ypred_0:\n" << Ypred_0 << endl;
		cout << "YB0 \n" << YB0 << endl;
		cout << "D0:\n" <<D0 << endl;
		cout << "Ypred_1:\n" << Ypred_1 << endl;
		cout << "YB1 \n" << YB1 << endl;
		cout << "D1:\n" <<D1 << endl;*/

		// * STEP 7: mask D0, and D1
		MATRIX Fp0;	MATRIX Fp1; MATRIX Fp;
		MATRIX Vp0j = Vp0.col(j);
		MATRIX Vp1j = Vp1.col(j);
		maskMatrix(D0, Vp0j, Fp0);
		maskMatrix(D1, Vp1j, Fp1);
		reconstruct(Fp0, Fp1, Fp);

		// tests:
		/*cout << "MATRIX Fp0: \n" << Fp0 << endl;
		cout << "MATRIX Fp1: \n" << Fp1 << endl;
		cout << "MATRIX Fp: \n" << Fp << endl;*/

		// * STEP 8: compute shares of delta
		MATRIX delta0 = -0 * (EB.transpose() * Fp) + (XB0.transpose() * Fp) + (EB.transpose() * D0) + Zp0.col(j);
		MATRIX delta1 = -1 * (EB.transpose() * Fp) + (XB1.transpose() * Fp) + (EB.transpose() * D1) + Zp1.col(j);

		// tests:
		/*cout << "Before truncation:" << endl;
		cout << "delta0:\n" << delta0 << endl;
		cout << "delta1:\n" << delta1 << endl;*/

		// *STEP 9: truncation
		int l = 64;	int x = 32;	int d = 13;

		for (int i = 0; i < D; ++i) {
			delta0(i) = delta0(i) >> d;
			delta1(i) = delta1(i) >> d;
		}

		// tests:
		/*cout << "After truncation:" << endl;
		cout << "delta0:\n" << delta0 << endl;
		cout << "delta1:\n" << delta1 << endl;*/

		// *STEP 10:
		/*cout << "W0:\n" << W0 << endl;
		cout << "W1:\n" << W1 << endl;*/
		//cout << (B_size * ALPHA) << endl;

		W0 = W0 - (delta0 / (B_size * ALPHA));
		W1 = W1 - (delta1 / (B_size * ALPHA));

		/*cout << "W0:\n" << W0 << endl;
		cout << "W1:\n" << W1 << endl;*/

	}

	// *STEP 11: Reconstruct W
	MATRIX W(D,1);
    reconstruct(W0, W1, W);

	// tests:
	//cout << "FINAL W:\n" << W << endl;

	clock_t end = clock();
	double elapsed = (double(end) - double(start)) / CLOCKS_PER_SEC;
	//cout << "Time measured : " << elapsed << "seconds.\n";

	ColVectorXd Wd;
	W /= SCALING_FACTOR;
	Wd = W.cast <double>();

	//cout << Yt << endl;

	/*cout << "Xt size: " << Xt.rows() << " " << Xt.cols() << endl;
	cout << "Wd size: " << Wd.rows() << " " << Wd.cols() << endl;*/
	// Xt will always be 10000 x 784, make sure Wd is also 784

	ColVectorXd prediction = Xt * Wd;

	prediction /= SCALING_FACTOR;
	prediction *= 10;
    n_ = Yt.rows();	// 10000
	double temp1;
	long int temp2, temp3;
    prediction = round(prediction.array());

	cout << "SIZE: " << n_ << endl;

	cout << prediction << endl;
	int count = 0; int count_zero = 0;
    for (int i = 0; i < n_ ; i++){
		temp3 = (long long int)prediction(i);

		//temp1 = temp3/(double)pow(2,20+8);
		temp2 = (long long int)Yt(i);
		//if (temp2 == 0) {
		//	count_zero++;
		//}

		//if(temp3>0.5 && temp2 == 1){
		//	count++;
		//}
		//else if (temp3 < 0.5 && temp2 == 0) {
		//	count++;
		//}

		if(prediction(i) == Yt(i))
            count++;

		//cout << "pred: " << prediction(i) << "	test label: " << Yt(i) << endl;
  //
    }

	cout << "num zeros: " << count_zero << endl;

	cout << "num correct" << count << endl;
	double accuracy = count/((double) n_);
	cout << "Accuracy on testing the trained model is " << accuracy * 100 << endl;

} // end of main()

void secretShareData(MATRIX& X, MATRIX& Y, MATRIX& X0, MATRIX& X1, MATRIX& Y0, MATRIX& Y1) {
	unsigned long long int n, n0;

	for (int i = 0; i < N; ++i) {
		for (int j = 0; j < D; j++) {
			n = X(i, j);
			n0 = rand();
			X0(i, j) = n0;
			X1(i, j) = (n - n0);
		}
	}

	for (int i = 0; i < N; ++i) {
		n = Y(i);
		n0 = rand();
		Y0(i) = n0;
		Y1(i) = (n - n0)  ;	// add mod 2l ?
	}
}

void genDataShares(MATRIX& X0, MATRIX& X1, MATRIX& Y0, MATRIX& Y1) {
	unsigned long long int n, n0, n1;
	for (int i = 0; i < N; ++i) {
		for (int j = 0; j < D; j++) {
			n = rand();
			n0 = rand();
			n1 = (n - n0); 		// add mod 2l ?
			X0(i, j) = n0;
			X1(i, j) = n1;
		}
	}

	for (int i = 0; i < N; ++i) {
		n = rand();
		n0 = rand();
		n1 = (n - n0) ;		// add mod 2l ?
		Y0(i) = n0;
		Y1(i) = n1;
	}
}

void initializeW(MATRIX& W0, MATRIX& W1) {
	for (int i = 0; i < D; ++i) {
		W0(i) = rand();
		W1(i) = rand();
	}
}

void genUVZmultTriplets(MATRIX& U0, MATRIX& U1, MATRIX& V0, MATRIX& V1, MATRIX& Z0, MATRIX& Z1) {

	unsigned long long int n, n0;
	for (int i = 0; i < N; i++) {
		for (int j = 0; j < D; j++) {
			n = rand();
			n0 = rand();
			U0(i, j) = n0;
			U1(i, j) = (n - n0);
		}
	}

	for (int i = 0; i < D; i++) {
		for (int j = 0; j < T; j++) {
			n = rand();
			n0 = rand();
			V0(i, j) = n0;
			V1(i, j) = (n - n0);
		}
	}

	Z0 = U0 * V0;
	Z1 = U1 * V1;
}

void genVZprimeMultTriplets(MATRIX& UB0, MATRIX& UB1, MATRIX& Vp0, MATRIX& Vp1, MATRIX& Zp0, MATRIX& Zp1) {
	MATRIX UBT0 = UB0.transpose();
	MATRIX UBT1 = UB1.transpose();

	unsigned long long int n, n0;
	for (int i = 0; i < B_size; i++) {
		for (int j = 0; j < T; j++) {
			n = rand();
			n0 = rand();
			Vp0(i, j) = n0;
			Vp1(i, j) = n - n0;
		}
	}

	Zp0 = UBT0 * Vp0;
	Zp1 = UBT1 * Vp1;
	/*cout << "MATRIX UBT0:\n" << UBT0 << endl;
	cout << "MATRIX UBT1:\n" << UBT1 << endl;*/
}

void reconstruct(MATRIX M0, MATRIX M1, MATRIX& M) {	M = M0 + M1;}

void maskMatrix(MATRIX Data, MATRIX Mask, MATRIX& Final) {	Final = Data - Mask;}

set<int> createMiniBatch(set<int>& usedIndices) {
	set<int> minibatch;
	int num = 0;
	//srand(time(NULL));
	while (minibatch.size() != B_size) {
		num = (rand() % (N - 1 + 1)) + 1;
		if (usedIndices.count(num) == 0) {
			usedIndices.insert(num);
			minibatch.insert(num);
		}
	}
	return minibatch;
}

void computeDifference(MATRIX& Dif, MATRIX Y_, MATRIX Y) {    Dif = Y_ - Y;}

int reverse_int(int i){
    unsigned char ch1, ch2, ch3, ch4;
    ch1 = i & 255;
    ch2 = (i >> 8) & 255;
    ch3 = (i >> 16) & 255;
    ch4 = (i >> 24) & 255;
    return ((int) ch1 << 24) + ((int) ch2 << 16) + ((int) ch3 << 8) + ch4;
}

void read_MNIST_data(bool train, vector<vector<unsigned long long int>> &vec, int& number_of_images, int& number_of_features){
    std::ifstream file;
	if (train == true)
		file.open("/Users/ariad/Documents/train-images-idx3-ubyte", std::ios::binary);
	else
		file.open("/Users/ariad/Documents/t10k-images-idx3-ubyte", std::ios::binary);

    if(!file){
        std::cout<<"Unable to open file";
        return;
    }
    if (file.is_open()){
        int magic_number = 0;
        int n_rows = 0;
        int n_cols = 0;
        file.read((char*) &magic_number, sizeof(magic_number));
        magic_number = reverse_int(magic_number);
        file.read((char*) &number_of_images, sizeof(number_of_images));
        number_of_images = reverse_int(number_of_images);
        if(train == true)
            number_of_images = N;
        file.read((char*) &n_rows, sizeof(n_rows));
        n_rows = reverse_int(n_rows);
        file.read((char*) &n_cols, sizeof(n_cols));
        n_cols = reverse_int(n_cols);
        number_of_features = n_rows * n_cols;
        std::cout << "Number of Images: " << number_of_images << std::endl;
        std::cout << "Number of Features: " << number_of_features << std::endl;
        for(int i = 0; i < number_of_images; ++i){
            std::vector<unsigned long long int> tp;
            for(int r = 0; r < n_rows; ++r)
                for(int c = 0; c < n_cols; ++c){
                    unsigned char temp = 0;
                    file.read((char*) &temp, sizeof(temp));
                    tp.push_back(((unsigned long long int) temp));
                }
            vec.push_back(tp);
        }
    }
}

void read_MNIST_labels(bool train, vector<unsigned long long int> &vec){
    std::ifstream file;
	if (train == true) {
		file.open("/Users/ariad/Documents/train-labels-idx1-ubyte", std::ios::binary);
	}
	else {
		file.open("/Users/ariad/Documents/t10k-labels-idx1-ubyte", std::ios::binary);
	}
    if(!file){
        std::cout << "Unable to open file";
        return;
    }
    if (file.is_open()){
        int magic_number = 0;
        int number_of_images = 0;
        file.read((char*) &magic_number, sizeof(magic_number));
        magic_number = reverse_int(magic_number);
        file.read((char*) &number_of_images, sizeof(number_of_images));
        number_of_images = reverse_int(number_of_images);
		cout << "number of images inside MNIST LAbels: " << number_of_images << endl;

        if(train == true)
            number_of_images = N;
        for(int i = 0; i < number_of_images; ++i){
            unsigned char temp = 0;
            file.read((char*) &temp, sizeof(temp));
			//vec.push_back((unsigned long int) temp);

            if((unsigned long long int) temp == 0)
                vec.push_back((unsigned long long int) 0);
            else
                vec.push_back((unsigned long long int) 1);
        }
    }
}

void read_MNIST_data_test(bool train, vector<vector<double>> &vec, int& number_of_images, int& number_of_features){
    std::ifstream file;
	if (train == true)
		file.open("/Users/ariad/Documents/train-images-idx3-ubyte", std::ios::binary);
	else
		file.open("/Users/ariad/Documents/t10k-images-idx3-ubyte", std::ios::binary);

    if(!file){
        std::cout<<"Unable to open file";
        return;
    }
    if (file.is_open()){
        int magic_number = 0;
        int n_rows = 0;
        int n_cols = 0;
        file.read((char*) &magic_number, sizeof(magic_number));
        magic_number = reverse_int(magic_number);
        file.read((char*) &number_of_images, sizeof(number_of_images));
        number_of_images = reverse_int(number_of_images);
        if(train == true)
            number_of_images = N;
        file.read((char*) &n_rows, sizeof(n_rows));
        n_rows = reverse_int(n_rows);
        file.read((char*) &n_cols, sizeof(n_cols));
        n_cols = reverse_int(n_cols);
        number_of_features = n_rows * n_cols;
        std::cout << "Number of Images: " << number_of_images << std::endl;
        std::cout << "Number of Features: " << number_of_features << std::endl;
        for(int i = 0; i < number_of_images; ++i){
            std::vector<double> tp;
            for(int r = 0; r < n_rows; ++r)
                for(int c = 0; c < n_cols; ++c){
                    unsigned char temp = 0;
                    file.read((char*) &temp, sizeof(temp));
                    tp.push_back(((double) temp));
                }
            vec.push_back(tp);
        }
    }
}

void read_MNIST_labels_test(bool train, vector<double> &vec){
    std::ifstream file;
	if (train == true) {
		file.open("/Users/ariad/Documents/train-labels-idx1-ubyte", std::ios::binary);
	}
	else {
		file.open("/Users/ariad/Documents/t10k-labels-idx1-ubyte", std::ios::binary);
	}
    if(!file){
        std::cout << "Unable to open file";
        return;
    }
    if (file.is_open()){
        int magic_number = 0;
        int number_of_images = 0;
        file.read((char*) &magic_number, sizeof(magic_number));
        magic_number = reverse_int(magic_number);
        file.read((char*) &number_of_images, sizeof(number_of_images));
        number_of_images = reverse_int(number_of_images);
		cout << "number of images inside MNIST LAbels: " << number_of_images << endl;

        if(train == true)
            number_of_images = N;
        for(int i = 0; i < number_of_images; ++i){
            unsigned char temp = 0;
            file.read((char*) &temp, sizeof(temp));
			//vec.push_back((double) temp);

            if((double) temp == 0)
                vec.push_back((double) 0);
            else
                vec.push_back((double) 1);
        }
    }
}

/*
	Modulus
	Epochs, testing accuracy
*/


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

/*
 Copyright (C) 2018 Klaus Spanderen

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

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

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

/*! \file cevrndcalculator.cpp */

#include <ql/errors.hpp>
#include <ql/math/functional.hpp>
#include <ql/math/solvers1d/brent.hpp>
#include <ql/math/distributions/normaldistribution.hpp>

#include <ql/methods/finitedifferences/utilities/cevrndcalculator.hpp>

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

namespace QuantLib {

    CEVRNDCalculator::CEVRNDCalculator(Real f0, Real alpha, Real beta)
    : f0_(f0),
      alpha_(alpha),
      beta_(beta),
      delta_((1.0-2.0*beta)/(1.0-beta)),
      x0_(X(f0)) {
        QL_REQUIRE(beta != 1.0, "beta can not be one");
    }

    Real CEVRNDCalculator::massAtZero(Time t) const {
        if (delta_ < 2.0)
            return 1.0-boost::math::gamma_p(-0.5*delta_+1.0,x0_/(2.0*t));
        else
            return 0.0;
    }

    Real CEVRNDCalculator::X(Real f) const {
        return std::pow(f, 2.0*(1.0-beta_))/square<Real>()(alpha_*(1.0-beta_));
    }

    Real CEVRNDCalculator::invX(Real x) const {
        return std::pow(x*square<Real>()(alpha_*(1.0-beta_)),
                        1.0/(2.0*(1.0-beta_)));
    }

    Real CEVRNDCalculator::pdf(Real f, Time t) const {
        const Real y = X(f);

        if (delta_ < 2.0) {
            return boost::math::pdf(
                boost::math::non_central_chi_squared_distribution<Real>(
                    4.0-delta_, y/t), x0_/t)/t * 2.0*(1.0-beta_)*y/f;
        }
        else {
            return boost::math::pdf(
                boost::math::non_central_chi_squared_distribution<Real>(
                    delta_, x0_/t), y/t)/t * 2.0*(beta_-1.0)*y/f;
        }
    }

    Real CEVRNDCalculator::cdf(Real f, Time t) const {
        const Real y = X(f);

        if (delta_ < 2.0)
            return 1.0 - boost::math::cdf(
                boost::math::non_central_chi_squared_distribution<Real>(
                    2.0-delta_, y/t), x0_/t);
        else
            return 1.0 - boost::math::cdf(
                boost::math::non_central_chi_squared_distribution<Real>(
                    delta_, x0_/t), y/t);
    }

    Real CEVRNDCalculator::sankaranApprox(Real c, Time t, Real x) const {
        const Real a = x0_/t;
        const Real b = 2.0 - delta_;

        c = std::max(c, -0.45*b);

        const Real h = 1 - 2*(b+c)*(b+3*c)/(3*square<Real>()(b+2*c));
        const Real p = (b+2*c)/square<Real>()(b+c);
        const Real m = (h-1)*(1-3*h);

        const Real u = (std::pow(a/(b+c), h) - (1 + h*p*(h-1-0.5*(2-h)*m*p)))/
                (h*std::sqrt(2*p)*(1+0.5*m*p));

        return u - x;
    }

    Real CEVRNDCalculator::invcdf(Real q, Time t) const {
        if (delta_ < 2.0) {
            if (f0_ < QL_EPSILON || q < massAtZero(t))
                return 0.0;

            const Real x = InverseCumulativeNormal()(1-q);

            auto cdfApprox = [&](Real _c){ return sankaranApprox(_c, t, x); };

            const Real y0 = X(f0_)/t;

            try {
                Brent brent;
                brent.setMaxEvaluations(20);
                const Real guess =
                    invX(brent.solve(cdfApprox, 1e-8, y0, 0.02*y0) * t);

                return InvCDFHelper(this, guess, 1e-8, 100).inverseCDF(q, t);
            }
            catch (...) {
                return InvCDFHelper(this, f0_, 1e-8, 100).inverseCDF(q, t);
            }
        }
        else {
            const Real x = t * boost::math::quantile(
                boost::math::non_central_chi_squared_distribution<Real>(
                    delta_, x0_/t), 1-q);
            return invX(x);
        }
    }
}


#include <iostream>
#include <fstream>
#include "spaND.h"
#include <Eigen/IterativeLinearSolvers>
#include <unsupported/Eigen/IterativeSolvers>
#include "mmio.hpp"

using namespace Eigen;
using namespace std;
using namespace spaND;

int main(int argc, char* argv[]) {
    assert(argc == 5);
    string folder(argv[1]);
    int nlevels = atoi(argv[2]);
    double tol = atof(argv[3]);
    int skip = atoi(argv[4]);
    cout << "Inputs folder " << folder << " nlevels = " << nlevels << " tol = " << tol << " skip = " << skip << endl;
    string file  = folder + "/jac4.mm";
    string xfile = folder + "/xCoords.mm";
    string yfile = folder + "/yCoords.mm";
    cout << "Reading from " << file << " " << xfile << " " << yfile << endl;
    SpMat A = mmio::sp_mmread<double,int>(file);
    int N = A.rows();
    cout << "A has " << N << " rows" << endl;
    VectorXd xcoords = mmio::dense_mmread<double>(xfile);
    VectorXd ycoords = mmio::dense_mmread<double>(yfile);
    // Create X coordinate matrix
    MatrixXd X(2, N);
    assert(xcoords.size() == N/2);
    assert(ycoords.size() == N/2);
    for(int i = 0; i < N; i++) {
        X(0,i) = xcoords(i/2);
        X(1,i) = ycoords(i/2);
    }
    Tree t = Tree(nlevels);
    t.set_tol(tol);
    t.set_skip(skip);
    t.set_use_geo(true);
    t.set_Xcoo(&X);
    // Run algo
    timer start = wctime();
    t.partition(A);
    // Rest
    timer part = wctime();
    t.assemble(A);
    timer ass = wctime();
    try {
        t.factorize();
    } catch (exception& ex) {
        cout << ex.what();
        exit(1);
    }
    timer end = wctime();
    t.print_log();
    cout << "Timings:" << endl << "  Partition: " << elapsed(start,part) << " s." << endl << "  Assembly: " << elapsed(part,ass) << " s." << endl << "  Factorization: " << elapsed(ass, end) << " s." << endl;
    // Use CG
    VectorXd x = VectorXd::Zero(N);
    VectorXd b = VectorXd::Random(N);
    timer cg0 = wctime();
    int iter = cg(A, b, x, t, 500, 1e-12, true);
    timer cg1 = wctime();
    cout << "CG: #iterations: " << iter << ", residual |Ax-b|/|b|: " << (A*x-b).norm() / b.norm() << endl;
    cout << "  CG: " << elapsed(cg0, cg1) << " s." << endl;
    return 0;
}