//---------------------------------------------------------------------------//
// Copyright (c) 2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2021 Ilias Khairullin <ilias@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE algebraic_engine_test

#include <string>
#include <tuple>
#include <unordered_map>
#include <sstream>
#include <cstdlib>
#include <ctime>

#include <boost/test/unit_test.hpp>

#include <nil/crypto3/random/algebraic_random_device.hpp>
#include <nil/crypto3/random/algebraic_engine.hpp>

#include <nil/crypto3/algebra/curves/bls12.hpp>
#include <nil/crypto3/algebra/curves/mnt4.hpp>

using namespace nil::crypto3;

template<typename FieldParams>
void print_field_element(std::ostream &os, const typename algebra::fields::detail::element_fp<FieldParams> &e) {
    os << std::hex << e.data;
}

template<typename FieldParams>
void print_field_element(std::ostream &os, const typename algebra::fields::detail::element_fp2<FieldParams> &e) {
    os << "[" << e.data[0].data << ", " << e.data[1].data << "]";
}

template<typename FpCurveGroupElement>
void print_fp_curve_group_element(std::ostream &os, const FpCurveGroupElement &e) {
    os << "( " << e.X.data << " : " << e.Y.data << " : " << e.Z.data << " )";
}

template<typename Fp2CurveGroupElement>
void print_fp2_curve_group_element(std::ostream &os, const Fp2CurveGroupElement &e) {
    os << "(" << e.X.data[0].data << " , " << e.X.data[1].data << ") : (" << e.Y.data[0].data << " , "
       << e.Y.data[1].data << ") : (" << e.Z.data[0].data << " , " << e.Z.data[1].data << ")";
}

template<typename Fp3CurveGroupElement>
void print_fp3_curve_group_element(std::ostream &os, const Fp3CurveGroupElement &e) {
    os << "(" << e.X.data[0].data << " , " << e.X.data[1].data << " , " << e.X.data[2].data << ") : ("
       << e.Y.data[0].data << " , " << e.Y.data[1].data << " , " << e.Y.data[2].data << ") : (" << e.Z.data[0].data
       << " , " << e.Z.data[1].data << " , " << e.Z.data[2].data << ")";
}

namespace boost {
    namespace test_tools {
        namespace tt_detail {
            template<typename FieldParams>
            struct print_log_value<typename algebra::fields::detail::element_fp<FieldParams>> {
                void operator()(std::ostream &os, typename algebra::fields::detail::element_fp<FieldParams> const &e) {
                    print_field_element(os, e);
                }
            };

            template<typename FieldParams>
            struct print_log_value<typename algebra::fields::detail::element_fp2<FieldParams>> {
                void operator()(std::ostream &os, typename algebra::fields::detail::element_fp2<FieldParams> const &e) {
                    print_field_element(os, e);
                }
            };

            template<typename FieldParams>
            struct print_log_value<typename algebra::fields::detail::element_fp6_3over2<FieldParams>> {
                void operator()(std::ostream &os, typename algebra::fields::detail::element_fp6_3over2<FieldParams> const &e) {
                    print_field_element(os, e);
                }
            };

            template<typename FieldParams>
            struct print_log_value<typename algebra::fields::detail::element_fp12_2over3over2<FieldParams>> {
                void operator()(std::ostream &os,
                                typename algebra::fields::detail::element_fp12_2over3over2<FieldParams> const &e) {
                    print_field_element(os, e);
                }
            };

            template<>
            struct print_log_value<typename algebra::curves::mnt4<298>::g1_type<>::value_type> {
                void operator()(std::ostream &os, typename algebra::curves::mnt4<298>::g1_type<>::value_type const &e) {
                    print_fp_curve_group_element(os, e);
                }
            };

            template<>
            struct print_log_value<typename algebra::curves::mnt4<298>::g2_type<>::value_type> {
                void operator()(std::ostream &os, typename algebra::curves::mnt4<298>::g2_type<>::value_type const &e) {
                    print_fp2_curve_group_element(os, e);
                }
            };

            template<>
            struct print_log_value<typename algebra::curves::bls12<381>::g1_type<>::value_type> {
                void operator()(std::ostream &os, typename algebra::curves::bls12<381>::g1_type<>::value_type const &e) {
                    print_fp_curve_group_element(os, e);
                }
            };

            template<>
            struct print_log_value<typename algebra::curves::bls12<381>::g2_type<>::value_type> {
                void operator()(std::ostream &os, typename algebra::curves::bls12<381>::g2_type<>::value_type const &e) {
                    print_fp2_curve_group_element(os, e);
                }
            };

            template<template<typename, typename> class P, typename K, typename V>
            struct print_log_value<P<K, V>> {
                void operator()(std::ostream &, P<K, V> const &) {
                }
            };
        }    // namespace tt_detail
    }        // namespace test_tools
}    // namespace boost

template<typename T>
void test_UniformRandomBitGenerator() {
    using generator_type = random::algebraic_random_device<T>;
    using printer_type = boost::test_tools::tt_detail::print_log_value<typename generator_type::result_type>;
    generator_type g;
    printer_type print;
    std::cout << "min = ";
    print(std::cout, generator_type::min());
    std::cout << std::endl;
    std::cout << "max = ";
    print(std::cout, generator_type::max());
    std::cout << std::endl;
    for (auto i = 0; i < 10; i++) {
        print(std::cout, g());
        std::cout << std::endl;
    }
}

// TODO: add custom Generator
template<typename T>
void test_RandomNumberEngine() {
    std::srand(std::time(nullptr));
    constexpr std::size_t n = 5;
    using generator_type = random::algebraic_engine<T>;
    using printer_type = boost::test_tools::tt_detail::print_log_value<typename generator_type::result_type>;
    generator_type g;
    printer_type print;
    boost::random::mt19937 seed_seq;

    std::cout << "min = ";
    print(std::cout, generator_type::min());
    std::cout << std::endl;

    std::cout << "max = ";
    print(std::cout, generator_type::max());
    std::cout << std::endl;

    std::cout << "operator():" << std::endl;
    for (auto i = 0; i < n; i++) {
        print(std::cout, g());
        std::cout << std::endl;
    }

    std::cout << "seed():" << std::endl;
    g.seed();
    for (auto i = 0; i < n; i++) {
        print(std::cout, g());
        std::cout << std::endl;
    }

    std::cout << "seed(value):" << std::endl;
    g.seed(0);
    for (auto i = 0; i < n; i++) {
        print(std::cout, g());
        std::cout << std::endl;
    }

    std::cout << "seed(Sseq):" << std::endl;
    g.seed(seed_seq);
    for (auto i = 0; i < n; i++) {
        print(std::cout, g());
        std::cout << std::endl;
    }

    std::cout << "discard(z):" << std::endl;
    g.discard(n);
    for (auto i = 0; i < n; i++) {
        print(std::cout, g());
        std::cout << std::endl;
    }

    std::cout << "operator== and operator!=:" << std::endl;
    generator_type g1;
    std::cout << (g == g1) << std::endl;
    std::cout << (g != g1) << std::endl;
    g.seed();
    std::cout << (g == g1) << std::endl;
    std::cout << (g != g1) << std::endl;

    std::cout << "operator<<:" << std::endl;
    std::cout << g << std::endl;

    std::cout << "operator>>:" << std::endl;
    std::stringstream test_stream;
    test_stream << std::rand();
    test_stream >> g;
    for (auto i = 0; i < n; i++) {
        print(std::cout, g());
        std::cout << std::endl;
    }
}

BOOST_AUTO_TEST_SUITE(algebraic_random_device_tests)

BOOST_AUTO_TEST_CASE(mnt4_test) {
    using curve_type = algebra::curves::mnt4<298>;
    using scalar_field_type = typename curve_type::scalar_field_type;

    test_UniformRandomBitGenerator<scalar_field_type>();
    test_UniformRandomBitGenerator<typename curve_type::g1_type<>::field_type>();
    test_UniformRandomBitGenerator<typename curve_type::g2_type<>::field_type>();
    test_UniformRandomBitGenerator<typename curve_type::g1_type<>>();
    test_UniformRandomBitGenerator<typename curve_type::g2_type<>>();
}

BOOST_AUTO_TEST_CASE(bls12_381_test) {
    using curve_type = algebra::curves::bls12<381>;
    using scalar_field_type = typename curve_type::scalar_field_type;

    test_UniformRandomBitGenerator<scalar_field_type>();
    test_UniformRandomBitGenerator<typename curve_type::g1_type<>::field_type>();
    test_UniformRandomBitGenerator<typename curve_type::g2_type<>::field_type>();
    test_UniformRandomBitGenerator<typename curve_type::g1_type<>>();
    test_UniformRandomBitGenerator<typename curve_type::g2_type<>>();
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(algebraic_engine_tests)

BOOST_AUTO_TEST_CASE(mnt4_test) {
    using curve_type = algebra::curves::mnt4<298>;
    using scalar_field_type = typename curve_type::scalar_field_type;

    test_RandomNumberEngine<scalar_field_type>();
    test_RandomNumberEngine<typename curve_type::g1_type<>::field_type>();
    test_RandomNumberEngine<typename curve_type::g2_type<>::field_type>();
    test_RandomNumberEngine<typename curve_type::g1_type<>>();
    test_RandomNumberEngine<typename curve_type::g2_type<>>();
}

BOOST_AUTO_TEST_CASE(bls12_381_test) {
    using curve_type = algebra::curves::bls12<381>;
    using scalar_field_type = typename curve_type::scalar_field_type;

    test_RandomNumberEngine<scalar_field_type>();
    test_RandomNumberEngine<typename curve_type::g1_type<>::field_type>();
    test_RandomNumberEngine<typename curve_type::g2_type<>::field_type>();
    test_RandomNumberEngine<typename curve_type::g1_type<>>();
    test_RandomNumberEngine<typename curve_type::g2_type<>>();
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <polyfem/Problem.hpp>
#include <polyfem/ProblemWithSolution.hpp>

#include <vector>
#include <Eigen/Dense>

namespace polyfem
{
	class DrivenCavity: public Problem
	{
	public:
		DrivenCavity(const std::string &name);

		void rhs(const std::string &formulation, const Eigen::MatrixXd &pts,const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return true; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts,const double t, Eigen::MatrixXd &val) const override;

		bool has_exact_sol() const override { return false; }
		bool is_scalar() const override { return false; }
		// bool is_mixed() const override { return true; }
	};

	class Flow: public Problem
	{
	public:
		Flow(const std::string &name);

		void rhs(const std::string &formulation, const Eigen::MatrixXd &pts,const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return true; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts,const double t, Eigen::MatrixXd &val) const override;

		bool has_exact_sol() const override { return false; }
		bool is_scalar() const override { return false; }
		// bool is_mixed() const override { return true; }

		void set_parameters(const json &params) override;
	private:
		int inflow_;
		int outflow_;

		double inflow_amout_;
		double outflow_amout_;
	};

	class TimeDependentFlow: public Flow
	{
	public:
		TimeDependentFlow(const std::string &name);

		void initial_solution(const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;

		bool is_time_dependent() const override { return true; }
	};
}


#ifndef ASLAM_BACKEND_SCALAR_EXPRESSION_NODE_HPP
#define ASLAM_BACKEND_SCALAR_EXPRESSION_NODE_HPP

#include <aslam/backend/JacobianContainer.hpp>
#include <boost/shared_ptr.hpp>
#include <Eigen/Core>
#include <aslam/backend/VectorExpressionNode.hpp>

namespace aslam {
  namespace backend {
    class ExpressionNodeVisitor;

    /**
     * \class ScalarExpressionNode
     * \brief The superclass of all classes representing scalar points.
     */
    class ScalarExpressionNode
    {
    public:
      typedef double value_t;

      ScalarExpressionNode();
      virtual ~ScalarExpressionNode();

      /// \brief Evaluate the scalar matrix.
      inline double toScalar() const;
      double evaluate() const { return toScalar(); }

      /// \brief Evaluate the Jacobians
      inline void evaluateJacobians(JacobianContainer & outJacobians) const;

      /// \brief Evaluate the Jacobians and apply the chain rule.
      template <typename DERIVED>
      EIGEN_ALWAYS_INLINE void evaluateJacobians(JacobianContainer & outJacobians, const Eigen::MatrixBase<DERIVED> & applyChainRule) const {
        evaluateJacobians(outJacobians.apply(applyChainRule));
      }

      /// \brief Evaluate the Jacobians and apply the chain rule.
      void evaluateJacobians(JacobianContainer & outJacobians, const Differential<Eigen::Matrix<double, 1, 1>, double> & chainRuleDifferentail) const {
        evaluateJacobians(applyDifferentialToJacobianContainer(outJacobians, chainRuleDifferentail, 1));
      }

      void getDesignVariables(DesignVariable::set_t & designVariables) const;

      virtual void accept(ExpressionNodeVisitor& visitor);  //TODO make pure and complete nodes
    protected:
      // These functions must be implemented by child classes.
      virtual double evaluateImplementation() const = 0;
      virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const = 0;
      virtual void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const = 0;
    };

      class ScalarExpressionNodeMultiply : public ScalarExpressionNode
      {
      public:
          ScalarExpressionNodeMultiply(boost::shared_ptr<ScalarExpressionNode> lhs,
                                       boost::shared_ptr<ScalarExpressionNode> rhs);
          ~ScalarExpressionNodeMultiply() override;

          void accept(ExpressionNodeVisitor& visitor) override;
      protected:
          // These functions must be implemented by child classes.
          inline double evaluateImplementation() const override;
          inline void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
          void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

          boost::shared_ptr<ScalarExpressionNode> _lhs;
          boost::shared_ptr<ScalarExpressionNode> _rhs;
    };

      class ScalarExpressionNodeDivide : public ScalarExpressionNode
      {
      public:
          ScalarExpressionNodeDivide(boost::shared_ptr<ScalarExpressionNode> lhs,
                                       boost::shared_ptr<ScalarExpressionNode> rhs);
          ~ScalarExpressionNodeDivide() override;
          void accept(ExpressionNodeVisitor& visitor) override;
      protected:
          // These functions must be implemented by child classes.
          inline double evaluateImplementation() const override;
          inline void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
          void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

          boost::shared_ptr<ScalarExpressionNode> _lhs;
          boost::shared_ptr<ScalarExpressionNode> _rhs;

    };

    class ScalarExpressionNodeNegated : public ScalarExpressionNode
      {
      public:
          ScalarExpressionNodeNegated(boost::shared_ptr<ScalarExpressionNode> rhs);
          ~ScalarExpressionNodeNegated() override;
          void accept(ExpressionNodeVisitor& visitor) override;
       protected:
          // These functions must be implemented by child classes.
          inline double evaluateImplementation() const override;
          inline void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
          void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

          boost::shared_ptr<ScalarExpressionNode> _rhs;
    };

      class ScalarExpressionNodeAdd : public ScalarExpressionNode
      {
      public:
          ScalarExpressionNodeAdd(boost::shared_ptr<ScalarExpressionNode> lhs,
                                  boost::shared_ptr<ScalarExpressionNode> rhs,
                                  double multiplyRhs = 1.0);
          ~ScalarExpressionNodeAdd() override;
          void accept(ExpressionNodeVisitor& visitor) override;
       protected:
          // These functions must be implemented by child classes.
          inline double evaluateImplementation() const override;
          inline void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
          void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

          boost::shared_ptr<ScalarExpressionNode> _lhs;
          boost::shared_ptr<ScalarExpressionNode> _rhs;
          double _multiplyRhs;
    };

      class ScalarExpressionNodeConstant  : public ScalarExpressionNode
      {
      public:
          ScalarExpressionNodeConstant(double s);
          ~ScalarExpressionNodeConstant() override;
          void accept(ExpressionNodeVisitor& visitor) override;
      protected:
          // These functions must be implemented by child classes.
          double evaluateImplementation() const override{return _s;}
          void evaluateJacobiansImplementation(JacobianContainer & /* outJacobians */) const override{}
          void getDesignVariablesImplementation(DesignVariable::set_t & /* designVariables */) const override{}

          double _s;
      };

      class ScalarExpressionNodeSqrt : public ScalarExpressionNode
      {
       public:
          ScalarExpressionNodeSqrt(boost::shared_ptr<ScalarExpressionNode> lhs);
          ~ScalarExpressionNodeSqrt() override;

       protected:
          // These functions must be implemented by child classes.
          inline double evaluateImplementation() const override;
          inline void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
          void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

          boost::shared_ptr<ScalarExpressionNode> _lhs;
      };

      class ScalarExpressionNodeLog : public ScalarExpressionNode
      {
       public:
          ScalarExpressionNodeLog(boost::shared_ptr<ScalarExpressionNode> lhs);
          ~ScalarExpressionNodeLog() override;

       protected:
          // These functions must be implemented by child classes.
          inline double evaluateImplementation() const override;
          inline void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
          void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

          boost::shared_ptr<ScalarExpressionNode> _lhs;
      };

      class ScalarExpressionNodeExp : public ScalarExpressionNode
      {
       public:
          ScalarExpressionNodeExp(boost::shared_ptr<ScalarExpressionNode> lhs);
          ~ScalarExpressionNodeExp() override;

       protected:
          // These functions must be implemented by child classes.
          inline double evaluateImplementation() const override;
          inline void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
          void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

          boost::shared_ptr<ScalarExpressionNode> _lhs;
      };

      class ScalarExpressionNodeAtan : public ScalarExpressionNode
      {
       public:
          ScalarExpressionNodeAtan(boost::shared_ptr<ScalarExpressionNode> lhs);
          ~ScalarExpressionNodeAtan() override;

       protected:
          // These functions must be implemented by child classes.
          inline double evaluateImplementation() const override;
          inline void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
          void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

          boost::shared_ptr<ScalarExpressionNode> _lhs;
      };

      class ScalarExpressionNodeTanh : public ScalarExpressionNode
      {
       public:
          ScalarExpressionNodeTanh(boost::shared_ptr<ScalarExpressionNode> lhs);
          ~ScalarExpressionNodeTanh() override;

       protected:
          // These functions must be implemented by child classes.
          inline double evaluateImplementation() const override;
          inline void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
          void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

          boost::shared_ptr<ScalarExpressionNode> _lhs;
      };

      class ScalarExpressionNodeAtan2 : public ScalarExpressionNode
      {
       public:
          ScalarExpressionNodeAtan2(boost::shared_ptr<ScalarExpressionNode> lhs, boost::shared_ptr<ScalarExpressionNode> rhs);
          ~ScalarExpressionNodeAtan2() override;

       protected:
          // These functions must be implemented by child classes.
          inline double evaluateImplementation() const override;
          inline void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
          void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

          boost::shared_ptr<ScalarExpressionNode> _lhs;
          boost::shared_ptr<ScalarExpressionNode> _rhs;
      };

      class ScalarExpressionNodeAcos : public ScalarExpressionNode
      {
       public:
          ScalarExpressionNodeAcos(boost::shared_ptr<ScalarExpressionNode> lhs);
          ~ScalarExpressionNodeAcos() override;

       protected:
          // These functions must be implemented by child classes.
          inline double evaluateImplementation() const override;
          inline void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
          void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

          boost::shared_ptr<ScalarExpressionNode> _lhs;
      };

      class ScalarExpressionNodeAcosSquared : public ScalarExpressionNode
      {
       public:
          ScalarExpressionNodeAcosSquared(boost::shared_ptr<ScalarExpressionNode> lhs);
          ~ScalarExpressionNodeAcosSquared() override;

       protected:
          // These functions must be implemented by child classes.
          inline double evaluateImplementation() const override;
          inline void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
          void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

          boost::shared_ptr<ScalarExpressionNode> _lhs;
      };

      class ScalarExpressionNodeInverseSigmoid : public ScalarExpressionNode
      {
       public:
          ScalarExpressionNodeInverseSigmoid(boost::shared_ptr<ScalarExpressionNode> lhs, const double height, const double scale, const double shift);
          ScalarExpressionNodeInverseSigmoid(boost::shared_ptr<ScalarExpressionNode> lhs);
          ~ScalarExpressionNodeInverseSigmoid() override;

       protected:
          // These functions must be implemented by child classes.
          inline double evaluateImplementation() const override;
          inline void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
          void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

          boost::shared_ptr<ScalarExpressionNode> _lhs;
          double _height;
          double _scale;
          double _shift;
      };

      class ScalarExpressionNodePower : public ScalarExpressionNode
      {
       public:
          EIGEN_MAKE_ALIGNED_OPERATOR_NEW

          ScalarExpressionNodePower(boost::shared_ptr<ScalarExpressionNode> lhs, const int k);
          ~ScalarExpressionNodePower() override;

       protected:
          // These functions must be implemented by child classes.
          inline double evaluateImplementation() const override;
          inline void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
          void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

          boost::shared_ptr<ScalarExpressionNode> _lhs;
          int _power;
      };

      class ScalarExpressionPiecewiseExpression : public ScalarExpressionNode
      {
       public:
          ScalarExpressionPiecewiseExpression(boost::shared_ptr<ScalarExpressionNode> e1, boost::shared_ptr<ScalarExpressionNode> e2, std::function<bool()> useFirst);
          ~ScalarExpressionPiecewiseExpression() override;

       protected:
          // These functions must be implemented by child classes.
          inline double evaluateImplementation() const override;
          inline void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
          void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

          boost::shared_ptr<ScalarExpressionNode> _e1;
          boost::shared_ptr<ScalarExpressionNode> _e2;
          std::function<bool()> _useFirst;
      };

      template <int VectorSize, int ComponentIndex = 0>
      class ScalarExpressionNodeFromVectorExpression : public ScalarExpressionNode
      {
      public:
          ScalarExpressionNodeFromVectorExpression(boost::shared_ptr<VectorExpressionNode<VectorSize> > lhs) : _lhs(lhs){
            static_assert (ComponentIndex < VectorSize, "component index must be smaller than the vectors size");
          }
          ~ScalarExpressionNodeFromVectorExpression() override{}

       protected:
          // These functions must be implemented by child classes.
          double evaluateImplementation() const override;
          void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
          void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

          boost::shared_ptr<VectorExpressionNode<VectorSize> > _lhs;
      };

    template <int VectorDim, int ComponentIndex>
    double ScalarExpressionNodeFromVectorExpression<VectorDim, ComponentIndex>::evaluateImplementation() const
    {
      if(!_lhs)
        return 0;
      return _lhs->evaluate()(ComponentIndex);
    }

    template <int VectorDim, int ComponentIndex>
    void ScalarExpressionNodeFromVectorExpression<VectorDim, ComponentIndex>::evaluateJacobiansImplementation(JacobianContainer & outJacobians) const
    {
      if(_lhs)
        _lhs->evaluateJacobians(outJacobians, Eigen::Matrix<double, 1, VectorDim>::Unit(ComponentIndex));
    }

    template <int VectorDim, int ComponentIndex>
    void ScalarExpressionNodeFromVectorExpression<VectorDim, ComponentIndex>::getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const
    {
      if(_lhs)
        _lhs->getDesignVariables(designVariables);
    }

  } // namespace backend
} // namespace aslam

#include "implementation/ScalarExpressionNode.hpp"

#endif /* ASLAM_BACKEND_EUCLIDEAN_EXPRESSION_NODE_HPP */


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [compare_not_equal]
#include <boost/simd/pack.hpp>
#include <boost/simd/reduction.hpp>
#include <iostream>

namespace bs = boost::simd;
using pack_it = bs::pack<std::int32_t, 4>;

int main() {
  pack_it pi = {1, 2, 5, 2};
  pack_it qi = {0, 1, 2, 8};

  std::cout << "---- simd" << '\n'
            << "<- pi =                       " << pi << '\n'
            << "<- qi =                       " << qi << '\n'
            << "-> bs::compare_not_equal(pi, pi) = "
            << bs::compare_not_equal(pi, pi) << '\n'
            << "-> bs::compare_not_equal(pi, qi) = "
            << bs::compare_not_equal(pi, qi) << '\n';
  return 0;
}
//! [compare_not_equal]


#include <boost/python/numpy.hpp>
#include <boost/python/module.hpp>
#include <boost/python/def.hpp>
#include <fstream>
#include <iomanip>
#include <string>
#include <numeric>
#include <algorithm>
#include <unistd.h>
#include <cmath>
#include <chrono>
#include <random>
#include <iostream>
#include <assert.h>
#include <vector>

namespace p = boost::python;
namespace np = boost::python::numpy;

int n_nodes;
int n_edges;
int **edges;
float *edge_weights;
float **X;

void gen_rand_network(int n, int m)
{
    int row;
    n_nodes = n;
    n_edges = m;
    edges = (int **)malloc(n_edges*sizeof(int *));
    for(int i = 0; i < n_edges; i++)
        edges[i] = (int *)malloc(2*sizeof(int));

    edge_weights = (float *)malloc(n_edges*sizeof(float));
    assert(edges != NULL);
    row = 0;
    while(row < n_edges)
    {
        edges[row][0] = rand()%n_nodes;
        edges[row][1] = rand()%n_nodes;
        edge_weights[row] = 1.0;
        row++;
    }
}

void init_embedding(int d)
{
    X = (float **)malloc(n_nodes*sizeof(float *));
    for(int i = 0; i < n_nodes; i++)
        X[i] = (float *)malloc(d*sizeof(float));
    unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
    std::default_random_engine generator (seed);
    std::normal_distribution<float> distribution (0.0,1.0);
    for (int i = 0; i < n_nodes; i++)
        for(int j = 0; j < d; j++)
            X[i][j] = 0.01*distribution(generator);
}

void load_network(std::string f_name, bool is_weighted)
{
    std::ifstream f;
    int v_i, v_j, row;
    float w;

    f.open(f_name);
    f >> n_nodes;
    f >> n_edges;
    edges = (int **)malloc(n_edges*sizeof(int *));
    for(int i = 0; i < n_edges; i++)
        edges[i] = (int *)malloc(2*sizeof(int));

    edge_weights = (float *)malloc(n_edges*sizeof(float));
    assert(edges != NULL);

    row = 0;
    if(is_weighted)
    {
        while(f >> v_i  >> v_j >>  w)
        {
            edges[row][0] = v_i;
            edges[row][1] = v_j;
            edge_weights[row] = w;
            row++;
        }
    }
    else
    {
        while(f >> v_i  >> v_j)
        {
            edges[row][0] = v_i;
            edges[row][1] = v_j;
            edge_weights[row] = 1.0;
            row++;
        }
    }
    f.close();
}

void _print_f_value(int d)
{
    float f1 = 0.0, f2 = 0.0;
    float i_j_dot, w_ij;
    int v_i, v_j;
    for(int edge_id = 0; edge_id < n_edges; edge_id++)
    {
        v_i = edges[edge_id][0];
        v_j = edges[edge_id][1];
        w_ij = edge_weights[edge_id];
        i_j_dot = 0.0;
        for(int d_id = 0; d_id < d; d_id++)
            i_j_dot += X[v_i][d_id]*X[v_j][d_id];
        f1 += (w_ij - i_j_dot)*(w_ij - i_j_dot);
    }
    for(int node_id = 0; node_id < n_nodes; node_id++)
        for(int d_id = 0; d_id < d; d_id++)
            f2 += X[node_id][d_id]*X[node_id][d_id];
    std::cout << "\t\tObjective: " << f1+f2 << ", f1: " << f1 << ", f2:" << f2 << std::endl;
}

void saveEmbToTxt(std::string of_name, int d)
{
    std::ofstream f;
    f.open(of_name);
    f << n_nodes << " " << d << std::endl;
    for(int node_id = 0; node_id < n_nodes; node_id++)
    {
        f << node_id;
        for(int d_id = 0; d_id < d; d_id++)
            f << " " << X[node_id][d_id];
        f << std::endl;
    }
    f.close();

}
void learn_embedding(std::string if_name, std::string of_name, bool verbose, bool is_weighted, int d, float eta, float regu, int max_iter)
{
    clock_t t;
    srand(time(NULL));
    t = clock();
    load_network(if_name, is_weighted);
    // gen_rand_network(1e3, 3e4);
    init_embedding(d);
    int v_i, v_j;
    float i_j_dot, w_ij;
    for(int iter_id = 0; iter_id < max_iter; iter_id++)
    {
        if(verbose)
        {
            if(iter_id%100 == 0)
            {
                std::cout << "\tIter id: " << iter_id << std::endl;
                _print_f_value(d);
            }
        }
        for(int edge_id = 0; edge_id < n_edges; edge_id++)
        {
            v_i = edges[edge_id][0];
            v_j = edges[edge_id][1];
            w_ij = edge_weights[edge_id];
            if(v_j <= v_i)
                continue;
            i_j_dot = 0;
            for(int d_id = 0; d_id < d; d_id++)
                i_j_dot += X[v_i][d_id]*X[v_j][d_id];
            for(int d_id = 0; d_id < d; d_id++)
                X[v_i][d_id] -= eta*(regu*X[v_i][d_id] - (w_ij - i_j_dot)*X[v_j][d_id]);
        }
    }
    t = clock() - t;
    t = clock();
    saveEmbToTxt(of_name, d);
}

BOOST_PYTHON_MODULE(graphFac_ext)
{
    // Py_Initialize();
    // Py_Initialize();
    // np::initialize();
    // srand(time(NULL));
    p::def("learn_embedding", learn_embedding);
}


//
// Copyright (c) 2016 - 2017 Mesh Consultants Inc.
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//

#include "Mesh_DecimateMesh.h"

#include <assert.h>

#include "ConversionUtilities.h"
#include "TriMesh.h"

#include <Eigen/Core>
#include <Eigen/Dense>

#pragma warning(push, 0)
#include <igl/decimate.h>
#include <igl/collapse_edge.h>
#include <igl/edge_flaps.h>
#include <igl/remove_unreferenced.h>
#pragma warning(pop)

using namespace Urho3D;

String Mesh_DecimateMesh::iconTexture = "Textures/Icons/Mesh_DecimateMesh.png";

Mesh_DecimateMesh::Mesh_DecimateMesh(Context* context) : IoComponentBase(context, 2, 1)
{
	SetName("DecimateMesh");
	SetFullName("Decimate Mesh");
	SetDescription("Perform decimation on triangle mesh");
	SetGroup(IoComponentGroup::MESH);
	SetSubgroup("Operators");

	inputSlots_[0]->SetName("Mesh");
	inputSlots_[0]->SetVariableName("M");
	inputSlots_[0]->SetDescription("Mesh before decimation");
	inputSlots_[0]->SetVariantType(VariantType::VAR_VARIANTMAP);
	inputSlots_[0]->SetDataAccess(DataAccess::ITEM);

	inputSlots_[1]->SetName("Target Face Count");
	inputSlots_[1]->SetVariableName("C");
	inputSlots_[1]->SetDescription("Target number of faces");
	inputSlots_[1]->SetVariantType(VariantType::VAR_INT);
	inputSlots_[1]->SetDataAccess(DataAccess::ITEM);

	outputSlots_[0]->SetName("Mesh");
	outputSlots_[0]->SetVariableName("M");
	outputSlots_[0]->SetDescription("Mesh after decimation");
	outputSlots_[0]->SetVariantType(VariantType::VAR_VARIANTMAP);
	outputSlots_[0]->SetDataAccess(DataAccess::ITEM);
}

void Mesh_DecimateMesh::SolveInstance(
	const Vector<Variant>& inSolveInstance,
	Vector<Variant>& outSolveInstance
)
{
	assert(inSolveInstance.Size() == inputSlots_.Size());
	assert(outSolveInstance.Size() == outputSlots_.Size());

	///////////////////
	// VERIFY & EXTRACT

	// Verify input slot 0
	Variant inMesh = inSolveInstance[0];
	if (!TriMesh_Verify(inMesh)) {
		URHO3D_LOGWARNING("M must be a valid mesh.");
		outSolveInstance[0] = Variant();
		return;
	}
	// Verify input slot 1
	int faceTarget = inSolveInstance[1].GetInt();
	if (faceTarget <= 0) {
		faceTarget = 1;
	}

	///////////////////
	// COMPONENT'S WORK

	Eigen::MatrixXf V;
	Eigen::MatrixXi F;
	bool loadSuccess = IglMeshToMatrices(inMesh, V, F);

	Eigen::MatrixXd Ud;
	Eigen::MatrixXi G;
	Eigen::VectorXi J;
	bool decimateSuccess = false;
	decimateSuccess = igl::decimate(
		IglFloatToDouble(V),
		F,
		(size_t)faceTarget,
		Ud,
		G,
		J
	);
	if (!decimateSuccess) {
		URHO3D_LOGWARNING("Decimate operation failed.");
		outSolveInstance[0] = Variant();
		return;
	}
	Eigen::MatrixXf U = IglDoubleToFloat(Ud);
	Variant outMesh = TriMesh_Make(U, G);

	/////////////////
	// ASSIGN OUTPUTS

	outSolveInstance[0] = outMesh;
}


#include "src/utils/rng.h"

#include <gtest/gtest.h>

#include <Eigen/Dense>

#include "src/hierarchies/nnig_hierarchy.h"
#include "src/utils/distributions.h"

TEST(rng, test1) {
  int rounds = 1000;
  std::vector<double> p = {0.8, 0.1, 0.1};

  int dim = p.size();
  Eigen::Map<Eigen::VectorXd> probas(p.data(), dim);
  std::vector<int> counts(dim);
  for (int i = 0; i < rounds; i++) {
    auto& rng = bayesmix::Rng::Instance().get();
    int c = bayesmix::categorical_rng(probas, rng);
    counts[c]++;
  }
  std::cout << "[          ] ----> ";
  for (int i = 0; i < dim; i++) {
    std::cout << counts[i] << " ";
  }
  std::cout << std::endl;
}

TEST(rng, test2) {
  int dim = 50;
  std::vector<double> p(dim, 1.0);
  p[0] = 10.0;
  Eigen::Map<Eigen::VectorXd> probas(p.data(), dim);
  probas = probas / probas.sum();

  auto& rng = bayesmix::Rng::Instance().get();
  int c1 = bayesmix::categorical_rng(probas, rng);

  auto& rng2 = bayesmix::Rng::Instance().get();
  int c2 = bayesmix::categorical_rng(probas, rng2);

  std::cout << "[          ] ----> ";
  std::cout << c1 << " " << c2 << " ";

  if (true) {
    auto& rng = bayesmix::Rng::Instance().get();
    int c3 = bayesmix::categorical_rng(probas, rng);
    std::cout << c3 << " ";
  }
  if (true) {
    auto& rng = bayesmix::Rng::Instance().get();
    int c4 = bayesmix::categorical_rng(probas, rng);
    std::cout << c4 << " " << std::endl;
  }
}

TEST(rng, test3) {
  NNIGHierarchy hierarchy;
  bayesmix::NNIGPrior hier_prior;
  hier_prior.mutable_fixed_values()->set_mean(0.0);
  hier_prior.mutable_fixed_values()->set_var_scaling(0.1);
  hier_prior.mutable_fixed_values()->set_shape(2.0);
  hier_prior.mutable_fixed_values()->set_scale(2.0);
  hierarchy.get_mutable_prior()->CopyFrom(hier_prior);
  hierarchy.initialize();

  hierarchy.sample_prior();
  double m1 = hierarchy.get_state().mean;
  double s1 = hierarchy.get_state().var;

  hierarchy.sample_prior();
  double m2 = hierarchy.get_state().mean;
  double s2 = hierarchy.get_state().var;

  ASSERT_NE(m1, m2);
  ASSERT_NE(s1, s2);

  NNIGHierarchy hierarchy2 = hierarchy;
  hierarchy2.sample_prior();

  double m3 = hierarchy2.get_state().mean;
  double s3 = hierarchy2.get_state().var;

  ASSERT_NE(m1, m3);
  ASSERT_NE(s1, s3);

  ASSERT_NE(m3, m2);
  ASSERT_NE(s3, s2);
}

TEST(eigen, random) {
  // doesn't work with auto!
  Eigen::MatrixXd aa = Eigen::MatrixXd::Random(2, 3);
  ASSERT_EQ(aa, aa);
}


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

#include <nt2/include/constants/digits.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>

#ifdef BOOST_MSVC
  #pragma warning(disable: 4309) // truncation of constant value
#endif

////////////////////////////////////////////////////////////////////////////////
// Test value of digit constant for every base types
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE_TPL( digit_value, BOOST_SIMD_SIMD_TYPES )
{
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef boost::simd::native<T,ext_t>             dst_t;

  for(std::size_t i=0; i< nt2::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (nt2::Mten<dst_t>())[i], static_cast<T>(  -10 ) );

  for(std::size_t i=0; i< nt2::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (nt2::Mnine<dst_t>())[i], static_cast<T>( -9 ) );

  for(std::size_t i=0; i< nt2::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (nt2::Meight<dst_t>())[i], static_cast<T>( -8 ) );

  for(std::size_t i=0; i< nt2::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (nt2::Mseven<dst_t>())[i], static_cast<T>( -7 ) );

  for(std::size_t i=0; i< nt2::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (nt2::Msix<dst_t>())[i], static_cast<T>( -6 ) );

  for(std::size_t i=0; i< nt2::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (nt2::Mfive<dst_t>())[i], static_cast<T>( -5 ) );

  for(std::size_t i=0; i< nt2::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (nt2::Mfour<dst_t>())[i], static_cast<T>( -4 ) );

  for(std::size_t i=0; i< nt2::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (nt2::Mthree<dst_t>())[i], static_cast<T>( -3 ) );

  for(std::size_t i=0; i< nt2::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (nt2::Mtwo<dst_t>())[i], static_cast<T>( -2 ) );

  for(std::size_t i=0; i< nt2::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (nt2::Mone<dst_t>())[i], static_cast<T>( -1 ) );

  for(std::size_t i=0; i< nt2::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (nt2::Zero<dst_t>())[i], static_cast<T>(  0 ) );

  for(std::size_t i=0; i< nt2::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (nt2::Ten<dst_t>())[i], static_cast<T>(  10 ) );

  for(std::size_t i=0; i< nt2::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (nt2::Nine<dst_t>())[i], static_cast<T>( 9 ) );

  for(std::size_t i=0; i< nt2::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (nt2::Eight<dst_t>())[i], static_cast<T>( 8 ) );

  for(std::size_t i=0; i< nt2::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (nt2::Seven<dst_t>())[i], static_cast<T>( 7 ) );

  for(std::size_t i=0; i< nt2::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (nt2::Six<dst_t>())[i], static_cast<T>( 6 ) );

  for(std::size_t i=0; i< nt2::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (nt2::Five<dst_t>())[i], static_cast<T>( 5 ) );

  for(std::size_t i=0; i< nt2::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (nt2::Four<dst_t>())[i], static_cast<T>( 4 ) );

  for(std::size_t i=0; i< nt2::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (nt2::Three<dst_t>())[i], static_cast<T>( 3 ) );

  for(std::size_t i=0; i< nt2::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (nt2::Two<dst_t>())[i], static_cast<T>( 2 ) );

  for(std::size_t i=0; i< nt2::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (nt2::One<dst_t>())[i], static_cast<T>( 1 ) );
}

////////////////////////////////////////////////////////////////////////////////
// Test integral_constant for every base types
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE_TPL( integral_constant, BOOST_SIMD_SIMD_INTEGRAL_TYPES )
{
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef boost::simd::native<T,ext_t>             dst_t;

  for(std::size_t i=0; i< nt2::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (nt2::integral_constant<dst_t,42>())[i], static_cast<T>(42) );
}


/***********************************************************************************************************************
*  OpenStudio(R), Copyright (c) 2008-2021, Alliance for Sustainable Energy, LLC, and other contributors. All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
*  following conditions are met:
*
*  (1) Redistributions of source code must retain the above copyright notice, this list of conditions and the following
*  disclaimer.
*
*  (2) Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
*  disclaimer in the documentation and/or other materials provided with the distribution.
*
*  (3) Neither the name of the copyright holder nor the names of any contributors may be used to endorse or promote products
*  derived from this software without specific prior written permission from the respective party.
*
*  (4) Other than as required in clauses (1) and (2), distributions in any form of modifications or other derivative works
*  may not use the "OpenStudio" trademark, "OS", "os", or any other confusingly similar designation without specific prior
*  written permission from Alliance for Sustainable Energy, LLC.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDER(S) AND ANY CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
*  INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER(S), ANY CONTRIBUTORS, THE UNITED STATES GOVERNMENT, OR THE UNITED
*  STATES DEPARTMENT OF ENERGY, NOR ANY OF THEIR EMPLOYEES, BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
*  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
*  USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
*  STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
*  ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
***********************************************************************************************************************/

#ifndef UTILITIES_GEOMETRY_GEOMETRY_HPP
#define UTILITIES_GEOMETRY_GEOMETRY_HPP

#include "../UtilitiesAPI.hpp"

#include <vector>
#include <boost/optional.hpp>

namespace openstudio {

class Point3d;
class PointLatLon;
class Vector3d;

/// convert degrees to radians
UTILITIES_API double degToRad(double degrees);

/// convert radians to degrees
UTILITIES_API double radToDeg(double radians);

// Forces an angle in degrees to be in the [0,360[ range
UTILITIES_API constexpr double normalizeAngle0to360(double angleDegrees) {
  while (angleDegrees < 0.0) {
    angleDegrees += 360.0;
  }

  // This is twice as fast as using std::fmod(angleDegrees, 360.0)
  while (angleDegrees >= 360.0) {
    angleDegrees -= 360.0;
  }
  return angleDegrees;
}

/// compute area from surface as Point3dVector
UTILITIES_API boost::optional<double> getArea(const std::vector<Point3d>& points);

/// compute Newall vector from surface as Point3dVector, direction is same as outward normal
/// magnitude is twice the area
UTILITIES_API boost::optional<Vector3d> getNewallVector(const std::vector<Point3d>& points);

/// compute outward normal from surface as Point3dVector
UTILITIES_API boost::optional<Vector3d> getOutwardNormal(const std::vector<Point3d>& points);

/// compute centroid from surface as Point3dVector
UTILITIES_API boost::optional<Point3d> getCentroid(const std::vector<Point3d>& points);

/// reorder points to upper-left-corner convention
UTILITIES_API std::vector<Point3d> reorderULC(const std::vector<Point3d>& points);

/// removes collinear points
UTILITIES_API std::vector<Point3d> removeCollinear(const std::vector<Point3d>& points, double tol = 0.001);

/// removes collinear points, tolerance is for length of cross product after normalizing each line segment
UTILITIES_API std::vector<Point3d> removeCollinearLegacy(const std::vector<Point3d>& points, double tol = 0.001);

/// return distance between two points
UTILITIES_API double getDistance(const Point3d& point1, const Point3d& point2);

/// return distance squared between two points
/// faster than getDistance; can be used to compare distances
UTILITIES_API double getDistanceSquared(const Point3d& point1, const Point3d& point2);

/// return distance between a point and a line segment
/// returns 0 if lineSegment does not have length 2
UTILITIES_API double getDistancePointToLineSegment(const Point3d& point, const std::vector<Point3d>& lineSegment);

/// return distance between a point and a triangle
/// returns 0 if triangle does not have length 3
UTILITIES_API double getDistancePointToTriangle(const Point3d& point, const std::vector<Point3d>& triangle);

/// return angle (in radians) between two vectors
UTILITIES_API double getAngle(const Vector3d& vector1, const Vector3d& vector2);

/// check if two vectors of points are equal (within tolerance) irregardless of initial ordering.
UTILITIES_API bool circularEqual(const std::vector<Point3d>& points1, const std::vector<Point3d>& points2, double tol = 0.001);

/// if point3d is within tol of any existing points then returns existing point
/// otherwise adds point3d to allPoints and returns point3d
UTILITIES_API Point3d getCombinedPoint(const Point3d& point3d, std::vector<Point3d>& allPoints, double tol = 0.001);

/// compute triangulation of vertices, holes are removed in the triangulation
/// requires that vertices and holes are in clockwise order on the z = 0 plane (i.e. in face coordinates but reversed)
UTILITIES_API std::vector<std::vector<Point3d>> computeTriangulation(const std::vector<Point3d>& vertices,
                                                                     const std::vector<std::vector<Point3d>>& holes, double tol = 0.001);

/// move all vertices towards point by distance, pass negative distance to move away from point
/// no guarantee that resulting polygon will be valid
UTILITIES_API std::vector<Point3d> moveVerticesTowardsPoint(const std::vector<Point3d>& vertices, const Point3d& point, double distance);

/// reverse order of vertices
UTILITIES_API std::vector<Point3d> reverse(const std::vector<Point3d>& vertices);

/// Sets view and daylighting window, overhang and light shelf vertices by reference.  Returns true if successful, false otherwise.
UTILITIES_API bool applyViewAndDaylightingGlassRatios(double viewGlassToWallRatio, double daylightingGlassToWallRatio,
                                                      double desiredViewGlassSillHeight, double desiredDaylightingGlassHeaderHeight,
                                                      double exteriorShadingProjectionFactor, double interiorShelfProjectionFactor,
                                                      const std::vector<Point3d>& surfaceVertices, std::vector<Point3d>& viewVertices,
                                                      std::vector<Point3d>& daylightingVertices, std::vector<Point3d>& exteriorShadingVertices,
                                                      std::vector<Point3d>& interiorShelfVertices);

}  // namespace openstudio

#endif  //UTILITIES_GEOMETRY_GEOMETRY_HPP


#ifndef AIKIDO_CONSTRAINT_NEWTONSMETHODPROJECTABLE_HPP_
#define AIKIDO_CONSTRAINT_NEWTONSMETHODPROJECTABLE_HPP_

#include <Eigen/Dense>

#include "aikido/constraint/Differentiable.hpp"
#include "aikido/constraint/Projectable.hpp"

namespace aikido {
namespace constraint {

/// Uses Newton's method to project state.
class NewtonsMethodProjectable : public Projectable
{
public:
  /// Constructor.
  /// \param _differentiable Differentiable constraint to be projected.
  /// \param _tolerance Tolerances for checking whether the constraints
  ///        are been satisfied. e.g. For equality,
  ///        |_differentiable->getValue(state)| <= tolerance
  ///        The size of tolerances should match _differentiable's constraint
  ///        dimension.
  /// \param _maxIteration Max iteration for Newton's method.
  /// \param _minStepSize Minimum step size to be taken in Newton's method.
  NewtonsMethodProjectable(
      DifferentiablePtr _differentiable,
      std::vector<double> _tolerance,
      int _maxIteration = 1000,
      double _minStepSize = 1e-5);

  // Documentation inherited.
  bool project(
      const statespace::StateSpace::State* _s,
      statespace::StateSpace::State* _out) const override;

  // Documentation inherited.
  statespace::ConstStateSpacePtr getStateSpace() const override;

private:
  DifferentiablePtr mDifferentiable;
  std::vector<double> mTolerance;
  int mMaxIteration;
  double mMinStepSize;
  statespace::ConstStateSpacePtr mStateSpace;

  bool contains(const statespace::StateSpace::State* _s) const;
};

} // namespace constraint
} // namespace aikido

#endif


// Copyright 2004-2006 The Trustees of Indiana University.

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

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_PLOD_GENERATOR_HPP
#define BOOST_GRAPH_PLOD_GENERATOR_HPP

#include <iterator>
#include <utility>
#include <boost/random/uniform_int.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/graph/graph_traits.hpp>
#include <vector>
#include <map>
#include <boost/config/no_tr1/cmath.hpp>
#include <boost/mpl/if.hpp>

namespace boost {
  template<typename RandomGenerator>
  class out_directed_plod_iterator
  {
  public:
    typedef std::forward_iterator_tag            iterator_category;
    typedef std::pair<std::size_t, std::size_t>  value_type;
    typedef const value_type&                    reference;
    typedef const value_type*                    pointer;
    typedef std::ptrdiff_t                       difference_type;

    out_directed_plod_iterator() : gen(0), at_end(true) { }

    out_directed_plod_iterator(RandomGenerator& gen, std::size_t n,
                               double alpha, double beta,
                               bool allow_self_loops)
      : gen(&gen), n(n), alpha(alpha), beta(beta),
        allow_self_loops(allow_self_loops), at_end(false), degree(0),
        current(0, 0)
    {
      using std::pow;

      uniform_int<std::size_t> x(0, n-1);
      std::size_t xv = x(gen);
      degree = (xv == 0? 0 : std::size_t(beta * pow(xv, -alpha)));
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    out_directed_plod_iterator& operator++()
    {
      using std::pow;

      uniform_int<std::size_t> x(0, n-1);

      // Continue stepping through source nodes until the
      // (out)degree is > 0
      while (degree == 0) {
        // Step to the next source node. If we've gone past the
        // number of nodes we're responsible for, we're done.
        if (++current.first >= n) {
          at_end = true;
          return *this;
        }

        std::size_t xv = x(*gen);
        degree = (xv == 0? 0 : std::size_t(beta * pow(xv, -alpha)));
      }

      do {
        current.second = x(*gen);
      } while (current.first == current.second && !allow_self_loops);
      --degree;

      return *this;
    }

    out_directed_plod_iterator operator++(int)
    {
      out_directed_plod_iterator temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const out_directed_plod_iterator& other) const
    {
      return at_end == other.at_end;
    }

    bool operator!=(const out_directed_plod_iterator& other) const
    {
      return !(*this == other);
    }

  private:
    RandomGenerator* gen;
    std::size_t n;
    double alpha;
    double beta;
    bool allow_self_loops;
    bool at_end;
    std::size_t degree;
    value_type current;
  };

  template<typename RandomGenerator>
  class undirected_plod_iterator
  {
    typedef std::vector<std::pair<std::size_t, std::size_t> > out_degrees_t;

  public:
    typedef std::input_iterator_tag              iterator_category;
    typedef std::pair<std::size_t, std::size_t>  value_type;
    typedef const value_type&                    reference;
    typedef const value_type*                    pointer;
    typedef std::ptrdiff_t                       difference_type;

    undirected_plod_iterator()
      : gen(0), out_degrees(), degrees_left(0), allow_self_loops(false) { }

    undirected_plod_iterator(RandomGenerator& gen, std::size_t n,
                             double alpha, double beta,
                             bool allow_self_loops = false)
      : gen(&gen), n(n), out_degrees(new out_degrees_t),
        degrees_left(0), allow_self_loops(allow_self_loops)
    {
      using std::pow;

      uniform_int<std::size_t> x(0, n-1);
      for (std::size_t i = 0; i != n; ++i) {
        std::size_t xv = x(gen);
        std::size_t degree = (xv == 0? 0 : std::size_t(beta * pow(xv, -alpha)));
        if (degree == 0) degree = 1;
        else if (degree >= n) degree = n-1;
        out_degrees->push_back(std::make_pair(i, degree));
        degrees_left += degree;
      }

      next();
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    undirected_plod_iterator& operator++()
    {
      next();
      return *this;
    }

    undirected_plod_iterator operator++(int)
    {
      undirected_plod_iterator temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const undirected_plod_iterator& other) const
    {
      return degrees_left == other.degrees_left;
    }

    bool operator!=(const undirected_plod_iterator& other) const
    { return !(*this == other); }

  private:
    void next()
    {
      std::size_t source, target;
      while (true) {
        /* We may get to the point where we can't actually find any
           new edges, so we just add some random edge and set the
           degrees left = 0 to signal termination. */
        if (out_degrees->size() < 2) {
          uniform_int<std::size_t> x(0, n-1);
          current.first  = x(*gen);
          do {
            current.second = x(*gen);
          } while (current.first == current.second && !allow_self_loops);
          degrees_left = 0;
          out_degrees->clear();
          return;
        }

        uniform_int<std::size_t> x(0, out_degrees->size()-1);

        // Select source vertex
        source = x(*gen);
        if ((*out_degrees)[source].second == 0) {
          (*out_degrees)[source] = out_degrees->back();
          out_degrees->pop_back();
          continue;
        }

        // Select target vertex
        target = x(*gen);
        if ((*out_degrees)[target].second == 0) {
          (*out_degrees)[target] = out_degrees->back();
          out_degrees->pop_back();
          continue;
        } else if (source != target
                   || (allow_self_loops && (*out_degrees)[source].second > 2)) {
          break;
        }
      }

      // Update degree counts
      --(*out_degrees)[source].second;
      --degrees_left;
      --(*out_degrees)[target].second;
      --degrees_left;
      current.first  = (*out_degrees)[source].first;
      current.second = (*out_degrees)[target].first;
    }

    RandomGenerator* gen;
    std::size_t n;
    shared_ptr<out_degrees_t> out_degrees;
    std::size_t degrees_left;
    bool allow_self_loops;
    value_type current;
  };

  template<typename RandomGenerator, typename Graph>
  class plod_iterator
    : public mpl::if_<is_convertible<
                        typename graph_traits<Graph>::directed_category,
                        directed_tag>,
                      out_directed_plod_iterator<RandomGenerator>,
                      undirected_plod_iterator<RandomGenerator> >::type
  {
    typedef typename mpl::if_<
                       is_convertible<
                         typename graph_traits<Graph>::directed_category,
                         directed_tag>,
                        out_directed_plod_iterator<RandomGenerator>,
                        undirected_plod_iterator<RandomGenerator> >::type
      inherited;

  public:
    plod_iterator() : inherited() { }

    plod_iterator(RandomGenerator& gen, std::size_t n,
                  double alpha, double beta, bool allow_self_loops = false)
      : inherited(gen, n, alpha, beta, allow_self_loops) { }
  };

} // end namespace boost

#endif // BOOST_GRAPH_PLOD_GENERATOR_HPP


#include "common.hpp"

#include <cstdio>
#include <cstdlib>
#include <string>
#include <cassert>
#include <random>
#include <vector>
#include <utility>
#include <map>
#include <cmath>
#include <algorithm>

#define rep(i,n) for(int (i)=0;(i)<(n);(i)++)

// eigen
#include <Eigen/Core>

using namespace Eigen;
using namespace std;

constexpr int BUF_SIZE = 1000;

using p_t = pair<int, int>;
using r_t = vector<map<p_t, double> >;
using mat_t = MatrixXd;
using vec_t = VectorXd;
using vec_mat_t = vector<mat_t>;

std::mt19937_64 engine;//(seed_gen());

vector<int> seq(int n){
  vector<int> v(n);
  for(int i = 0; i < n; ++i){v[i] = i;}
  return v;
}

vector<int> random_index(int n){
  vector<int> res = seq(n);
  shuffle(res.begin(), res.end(), engine);
  return res;
}

void seeding(unsigned seed = 0){
  //std::random_device seed_gen;
  //engine = std::mt19937_64(seed_gen());
  engine = std::mt19937_64(seed);
}

string to_str(int x){
  char buf[BUF_SIZE];
  sprintf(buf, "%d", x);
  return buf;
}

mat_t load_mat(const string& infile){
  vector<vector<double> > mat;
  ifstream ifs(infile);
  string line;
  while(getline(ifs, line)){
    vector<double> row;
    for(const auto& field : split(line)){
      double x;
      sscanf(field.c_str(), "%lf", &x);
      row.push_back(x);
    }
    mat.push_back(row);
  }
  int m = mat.size();
  assert(m > 0);
  int n = mat[0].size();
  mat_t res = mat_t::Zero(m, n);
  for(int i = 0; i < m; ++i){
    assert(n == mat[i].size());
    for(int j = 0; j < n; ++j){
      res(i, j) = mat[i][j];
    }
  }
  return res;
}

mat_t ReadModelParameter(const string& prefix, int K, int ItrNum, const string& name){
  char buf[BUF_SIZE];
  sprintf(buf, "%s_Itr%d_%s.csv", prefix.c_str(), ItrNum, name.c_str());
  //read_file(buf);

  //mat_t tmp = load_mat(buf);
  //fprintf(stderr, "[%ld, %ld]\n", tmp.rows(), tmp.cols());

  return load_mat(buf);
}
/*
  ########################### Read model parameters #############################
  # [output1]: A (N x K matrix)
  # [output2]: B (M x K matrix)
  # [output3]: C (M x K matrix)
  # [output4]: D (T x K matrix)
*/
tuple<mat_t, mat_t, mat_t, mat_t> ReadModelParameters(const string& prefix, int K, int ItrNum){
  mat_t A = ReadModelParameter(prefix, K, ItrNum, "A");
  mat_t B = ReadModelParameter(prefix, K, ItrNum, "B");
  mat_t C = ReadModelParameter(prefix, K, ItrNum, "C");
  mat_t D = ReadModelParameter(prefix, K, ItrNum, "D");
  return forward_as_tuple(A, B, C, D);
}

/*
######################### Plausible deniability test ##########################
# [input1]: A (N x K matrix)
# [input2]: B (M x K matrix)
# [input3]: C (M x K matrix)
# [input4]: D (T x K matrix)
# [input5]: N -- Number of users
# [input6]: M -- Number of POIs
# [input7]: T -- Number of time periods
# [output1]: pass_test (N-dim vector)
# [output2]: loglikeli_train (N*TraceNum-dim vector)
# [output3]: loglikeli_verify (N*TraceNum x VUserNum matrix)
 */

using pass_test_t = vector<int>;

void PDTest_out1(const string& _outfile,
		 int N,
		 int TraceNum,
		 const pass_test_t& pass_test){
  //  string outfile = _outfile + "_";
  string outfile = _outfile;
  puts(outfile.c_str());
  FILE* fp = fopen(outfile.c_str(), "w");
  rep(user_index, N){
    rep(trace_no, TraceNum){
      int user_trace_no = user_index * TraceNum + trace_no;
//      fprintf(fp, "%lf\n", static_cast<double>(pass_test[user_trace_no]));
      fprintf(fp, "%d,%d\n", pass_test[user_trace_no + N * TraceNum], pass_test[user_trace_no]);
    }
  }
  int pass_test_sum = 0;
  for(int i = 0; i < N * TraceNum; i++){
    pass_test_sum += pass_test[i];
  }
  fprintf(fp, "-,-,%d,%d,%lf\n", pass_test_sum, N * TraceNum, float(pass_test_sum) / float(N * TraceNum));
  fclose(fp);
}
void PDTest_out2(const string& _outfile,
		 int N,
		 int TraceNum,
		 int VUserNum,
		 const mat_t& loglikeli_train,
		 const mat_t& loglikeli_verify){
  //  string outfile = _outfile + "_";
  string outfile = _outfile;
  puts(outfile.c_str());
  FILE* fp = fopen(outfile.c_str(), "w");
  rep(user_index, N){
    rep(trace_no, TraceNum){
      int user_trace_no = user_index * TraceNum + trace_no;
      fprintf(fp, "%lf", loglikeli_train(user_trace_no, 0));
      rep(n, VUserNum){
	fprintf(fp, ",%lf", loglikeli_verify(user_trace_no,n));
      }
      fprintf(fp, "\n");
    }
  }
  fclose(fp);
}

pass_test_t perform_pd_test(int N,
			    int TraceNum,
			    int VUserNum,
			    const vector<int>& verify_user,
			    const mat_t& loglikeli_train,
			    const mat_t& loglikeli_verify,
			    double ReqEps,
			    int Reqk){
  /*
  pass_test_t pass_test(N * TraceNum, 0);
  // # For each training user
  rep(user_index, N){
    // # For each trace
    rep(trace_no, TraceNum){
      int user_trace_no = user_index * TraceNum + trace_no;
      // # Initialization
      int k = 0;
      // # For each verifying user
      rep(n, VUserNum){
	// # Continue if the training user is the same with the verifying user
	if(user_index == verify_user[n]){continue;}
	// # Increase k if the inequality holds with ReqEps
	if((loglikeli_train(user_trace_no, 0) >= loglikeli_verify(user_trace_no,n) - ReqEps) &&
	   (loglikeli_train(user_trace_no, 0) <= loglikeli_verify(user_trace_no,n) + ReqEps)){
	  ++k;
	}
	// # Break if k reaches Reqk
	if(k == Reqk){
	  pass_test[user_trace_no] = 1;
	  break;
	}
      }
    }
  }
  */
  pass_test_t pass_test(N * TraceNum * 2, 0);
  // # For each training user
  int i_tra, i_ver;
  rep(user_index, N){
    // # For each trace
    rep(trace_no, TraceNum){
      int user_trace_no = user_index * TraceNum + trace_no;
      pass_test[user_trace_no + N * TraceNum] += 1;
      // # Find i s.t. [-(i+1)ReqEps < loglikeli_train[user_trace_no] <= -i ReqEps] --> i_tra
      i_tra = int(loglikeli_train(user_trace_no, 0) / ReqEps);
      // # For each verifying user
      rep(n, VUserNum){
	// # Continue if the training user is the same with the verifying user
	if(user_index == verify_user[n]){continue;}
	// # Find i s.t. [-(i+1)ReqEps < loglikeli_verify[user_trace_no,n] <= -i ReqEps] --> i_ver
	i_ver = int(loglikeli_verify(user_trace_no,n) / ReqEps);
	// # Increase k if the inequality holds with ReqEps (i_tra == i_ver)
	if(i_tra == i_ver){
	  pass_test[user_trace_no + N * TraceNum] += 1;
	}
	// # Pass test (and break (optional)) if k reaches Reqk
	if(pass_test[user_trace_no + N * TraceNum] == Reqk){
	  pass_test[user_trace_no] = 1;
//	  break;
	}
      }
    }
  }
  return pass_test;
}

int str2i(const string& s){
  int x;
  sscanf(s.c_str(), "%d", &x);
  return x;
}

double str2f(const string& s){
  double x;
  sscanf(s.c_str(), "%lf", &x);
  return x;
}

using trans_t = vector<vector<pair<int, int> > >;
using first_visit_t = vector<int>;

tuple<trans_t, first_visit_t, mat_t> read_syn_traces(const string& infile,
					     int TraceNum,
					     int T,
					     int TimInsNum,
					     int N){
  // init
  trans_t trans(N * TraceNum);
  first_visit_t first_visit(N * TraceNum, 0);
  mat_t loglikeli_train = mat_t::Zero(N * TraceNum, 1);
  int poi_index_prev = 0;
  //printf("%s\n", infile.c_str());
  ifstream ifs(infile);
  string line;
  getline(ifs, line); // header
  while(getline(ifs, line)){
    auto lst = split(line);
    int user_index = str2i(lst[0]);
    int trace_no = str2i(lst[1]);
    int time_slot = str2i(lst[2]);
    int time_ins = str2i(lst[3]);
    int poi_index = str2i(lst[4]);
    double ll = str2f(lst[6]);

    // # (user,trace)-pair no. --> user_trace_no
    int user_trace_no = user_index * TraceNum + trace_no;
    if(time_slot == 0 && time_ins == 0){
      // # Visited POI at the first time --> first_visit[user_trace_no]
      first_visit[user_trace_no] = poi_index;
    }else{
      // # Transition from the previous POI to the current POI --> trans[user_trace_no]
      trans[user_trace_no].push_back(make_pair(poi_index_prev,poi_index));
    }
    // # Log-likelihood for the training trace --> loglikeli_train[user_trace_no]
    if(time_slot == T-1 && time_ins == TimInsNum-1){
      loglikeli_train(user_trace_no, 0) = ll;
    }
    poi_index_prev = poi_index;
  }
  return forward_as_tuple(trans, first_visit, loglikeli_train);
}

mat_t calc_ll(int VUserNum,
	      const vector<int>& verify_user,
	      int T, int M,
	      double VisThr,
	      double VisitDelta,
	      const mat_t& A, // N * K
	      const mat_t& B, // M * K
	      const mat_t& C, // M * K
	      const mat_t& D, // T * K
	      double TransDelta,
	      int N,
	      int TraceNum,
	      int TimInsNum,
	      const first_visit_t& first_visit,
	      const trans_t& trans){
  mat_t loglikeli_verify = mat_t::Zero(N*TraceNum,VUserNum);
  rep(n, VUserNum){
    int vn = verify_user[n];
    printf("%d, %d\n", n, vn);
    // # Initialization
    mat_t time_poi_dist = mat_t::Zero(T, M);
    mat_t time_poi_dist_sum = mat_t::Zero(T, 1);
    mat_t prop_mat = mat_t::Zero(M, M);
    mat_t same_trans = mat_t::Zero(T, M); //np.full((T,M), -1.0);
    for(int i = 0; i < T; ++i)for(int j = 0; j < M; ++j){same_trans(i, j) = -1.0;}
    mat_t trans_vec = mat_t::Zero(M, 1);

    // ################### Calculate the POI distributions ###################
    puts("POI distribution");
    rep(t, T){
      mat_t ad = A.row(vn).array() * D.row(t).array();
      rep(i, M){
	// # Elements in a sampled visit tensor --> time_poi_dist
	time_poi_dist(t,i) = (ad.array() * B.row(i).array()).sum();
	// # Assign VisitDelta for an element whose value is less than VisThr
	if(time_poi_dist(t,i) < VisThr){
	  time_poi_dist(t,i) = VisitDelta;
	}
      }
    }
    // # Normalize time_poi_dist
    rep(t, T){
      time_poi_dist_sum(t) = time_poi_dist.row(t).sum();
      if(time_poi_dist_sum(t) > 0){
	time_poi_dist.row(t) /= time_poi_dist_sum(t);
      }else{
	printf("Error: All probabilities are 0 for user %d and time %d\n", n, t);
	exit(1);
      }
    }
    // #################### Calculate the proposal matrix ####################
    puts("Proposal matrix");
    rep(i, M){
      mat_t ab = A.row(vn).array() * B.row(i).array();
      // # Elements in a sampled transition tensor (assign TransDelta for a small transition count) --> prop_mat
      rep(j, M){
	prop_mat(i,j) = max((ab.array() * C.row(j).array()).sum(), TransDelta);
      }
      // # Normalize prop_mat
      double row_sum = prop_mat.row(i).sum();
      prop_mat.row(i) /= row_sum;
    }
    // #################### Calculate the log-likelihood #####################
    puts("Calculating the log-likelihood");
    // # For each training user
    rep(user_index, N){
      // # Continue if the training user is the same with the verifying user
      if(user_index == vn){continue;}
      // # For each trace
      rep(trace_no, TraceNum){
	int user_trace_no = user_index * TraceNum + trace_no;
	// # For each time slot
	rep(t, T){
	  // # For each time instant
	  rep(ins, TimInsNum){
	    if(t == 0 && ins == 0){
	      // # Add the log-likelihood for the first POI
	      int poi_index = first_visit[user_trace_no];
	      loglikeli_verify(user_trace_no, n) = log(time_poi_dist(0,poi_index));
	    }else{
	      // # Add the log-likelihood for the subsequent POIs
	      int tim = t * TimInsNum + ins - 1;
	      int poi_index_pre = trans[user_trace_no][tim].first;
	      int poi_index = trans[user_trace_no][tim].second;
	      double trans_prob;
	      // # If the current POI is different from the previous POI
	      if(poi_index_pre != poi_index){
		// # Calculate the transition probability --> trans_prob
		double alpha = (time_poi_dist(t, poi_index) * prop_mat(poi_index, poi_index_pre))
		  / (time_poi_dist(t, poi_index_pre) * prop_mat(poi_index_pre, poi_index));
		trans_prob = prop_mat(poi_index_pre,poi_index) * min(1.0, alpha);
	      // # If the self-transition probability for the POI at time slot t has been computed
	      }else if(same_trans(t,poi_index_pre) != -1.0){
		// # Use the self-transition probability --> trans_prob
		trans_prob = same_trans(t,poi_index_pre);
	      // # If the self-transition probability for the POI at time slot t has NOT been computed
	      }else{
		// # Compute the self-transition probability for the POI --> same_trans[t,poi_index_pre]
		trans_vec(poi_index_pre, 0) = 0;
		rep(j, M){
		  if(poi_index_pre != j){
		    double alpha = (time_poi_dist(t, j) * prop_mat(j, poi_index_pre))
		      / (time_poi_dist(t, poi_index_pre) * prop_mat(poi_index_pre,j));
		    trans_vec(j, 0) = prop_mat(poi_index_pre,j) * min(1.0, alpha);
		  }
		}
		double row_sum = trans_vec.sum();
		same_trans(t,poi_index_pre) = 1 - row_sum;
		// # Use the self-transition probability --> trans_prob
		trans_prob = same_trans(t,poi_index_pre);
	      }
	      // # Add the log of the transition probability
	      loglikeli_verify(user_trace_no,n) += log(trans_prob);
	      //
	    }
	    //
	  }
	}
      }
    }
    //
  }
  return loglikeli_verify;
}

void PDTest(mat_t& A,
	    mat_t& B,
	    mat_t& C,
	    mat_t& D,
	    int N,
	    int M,
	    int T,
	    int VUserNum,
	    const string& PDTestResFile,
	    int K,
	    int ItrNum,
	    int TraceNum,
	    double ReqEps,
	    int Reqk,
	    const string& SynTraceFile,
	    int TimInsNum,
	    double VisThr, double VisitDelta, double TransDelta){
  // # Initialization

  // # Randomly assign verifying users --> verify_user
  auto verify_user = random_index(VUserNum);

  // # Read synthesized traces --> trans, first_visit, loglikeli_train
  trans_t trans;
  first_visit_t first_visit;
  mat_t loglikeli_train;
  tie(trans, first_visit, loglikeli_train) =
    read_syn_traces(SynTraceFile + "_Itr" + to_str(ItrNum) + ".csv",
		    TraceNum, T, TimInsNum, N);

  // # Calculate the log-likelihood for each verifying user --> loglikeli_verify
  mat_t loglikeli_verify = calc_ll(VUserNum, verify_user, T, M, VisThr, VisitDelta, A, B, C, D, TransDelta, N, TraceNum, TimInsNum, first_visit, trans);

  // # Perform the PD test --> pass_test
  pass_test_t pass_test = perform_pd_test(N,TraceNum, VUserNum,
				    verify_user, loglikeli_train, loglikeli_verify,
				    ReqEps, Reqk);

  // # Output the PD test results (pass_test)
  PDTest_out1(PDTestResFile + "_Itr" + to_str(ItrNum) + ".csv",
	      N, TraceNum, pass_test);
  /*
  // # Output the PD test results (loglikeli_train, loglikeli_verify)
  PDTest_out2(PDTestResFile + "_Itr" + to_str(ItrNum) + "_loglikeli.csv",
	      N, TraceNum, VUserNum,
	      loglikeli_train, loglikeli_verify);
  */
}

int main(int argc, char* argv[]){
  if(argc < 3){
    fprintf(stderr, "Usage: %s [Dataset] [City] ([ReqEps (default:1.0)] [Reqk (default:10)] [VUserNum (default:-1)] [TraceNum (default:10)] [Alp (default:200)] [MaxNumTrans (default:100)] [MaxNumVisit (default:100)] [ItrNum (default:100)])\n", argv[0]);
    exit(1);
  }
  // # Dataset (PF/FS)
  string DataSet = argv[1];
  // # City
  string City = argv[2];

  // # Required epsilon in plausible deniability
  double ReqEps = 1.0;
  if(argc >= 4){
    sscanf(argv[3], "%lf", &ReqEps);
  }

  // # Required k in plausible deniability
  int Reqk = 10;
  if(argc >= 5){
    sscanf(argv[4], "%d", &Reqk);
  }

  // # Number of users for verifying PD (-1: all)
  int VUserNum = -1;
  if(argc >= 6){
    sscanf(argv[5], "%d", &VUserNum);
  }

  // # Number of traces per user
  int TraceNum = 10;
  if(argc >= 7){
    sscanf(argv[6], "%d", &TraceNum);
  }

  // # Hyper-hyper parameter alpha
  double Alp = 200;
  string AlpStr = "200";
  if(argc >= 8){
    sscanf(argv[7], "%lf", &Alp);
    AlpStr = argv[7];
  }

  // # Maximum number of transitions per user (-1: infinity)
  int MaxNumTrans = 100;
  if(argc >= 9){
    sscanf(argv[8], "%d", &MaxNumTrans);
  }

  // # Maximum number of POI visits per user (-1: infinity)
  int MaxNumVisit = 100;
  if(argc >= 10){
    sscanf(argv[9], "%d", &MaxNumVisit);
  }

  // # Number of iterations in Gibbs sampling
  int ItrNum = 100;
  if(argc >= 11){
    sscanf(argv[10], "%d", &ItrNum);
  }

  // # Data directory
  string DataDir = "../data/" + DataSet + "/";

  // # Training user index file (input)
  string TUserIndexFile = DataDir + "tuserindex_%s.csv";
  // # POI index file (input)
  string POIIndexFile = DataDir + "POIindex_%s.csv";

  // # Model parameter directory
//  string ModelParameterDir = DataDir + "PPMTF_" + City + "_alp" + AlpStr + "_mnt" + to_str(MaxNumTrans) + "_mnv" + to_str(MaxNumVisit) + "_py/";
  string ModelParameterDir = DataDir + "PPMTF_" + City + "_alp" + AlpStr + "_mnt" + to_str(MaxNumTrans) + "_mnv" + to_str(MaxNumVisit) + "/";
  // # Prefix of the model parameter file (input)
  string ModelParameterFile = ModelParameterDir + "modelparameter";
  // # Prefix of the synthesized trace file (input)
  string SynTraceFile = ModelParameterDir + "syntraces";

  // # Prefix of the PD test result file (output)
  string PDTestResFile = ModelParameterDir + "pdtest_res";

  // # Name of the model parameter A
  //string ParamA = "A";

  // # Number of time slots
  int T = -1;
  if(DataSet.find("PF") == 0){T = 30;}
  if(DataSet.find("FS") == 0){T = 12;}
  if(T == -1){
    fprintf(stderr, "Wrong Dataset\n");
    exit(1);
  }

  // # Number of time instants per time slot
  int TimInsNum = -1;
  if(DataSet.find("PF") == 0){TimInsNum = 1;}
  if(DataSet.find("FS") == 0){TimInsNum = 2;}
  assert(TimInsNum >= 0);

  // # Number of columns in model parameters (A, B, C)
  int K = 16;
  // # Threshold for a visit count
  double VisThr = 0;
  // # Minimum value of a visit count
  double VisitDelta = 0.00000001;
  // # Minimum value of a transition count
  double TransDelta = 0.00000001;

  // #################################### Main #####################################
  // # Fix a seed
  seeding(1);

  // # Replace XX with City
  TUserIndexFile = string_replace(TUserIndexFile, City);
  POIIndexFile = string_replace(POIIndexFile, City);

  // # Number of training users --> N
  int N = line_num(TUserIndexFile) - 1;
  // # Number of POIs --> M
  int M = line_num(POIIndexFile) - 1;

  // # Number of users for verifying PD (-1: all)
  if(VUserNum == -1){VUserNum = N-1;}

  // # Read model parameters
  mat_t A, B, C, D;
  tie(A, B, C, D) = ReadModelParameters(ModelParameterFile, K, ItrNum);

  // # Plausible deniability test
  PDTest(A, B, C, D, N, M, T,
	 VUserNum, PDTestResFile,
	 K, ItrNum, TraceNum,
	 ReqEps, Reqk,
	 SynTraceFile,
	 TimInsNum,
	 VisThr, VisitDelta, TransDelta);
  //////////////////pass_test, loglikeli_train, loglikeli_verify = PDTest(A, B, C, D, N, M, T)

//  printf("%s\n", DataSet.c_str());
//  printf("%s\n", City.c_str());
  return 0;
}


//
// Created by david on 2018-10-29.
//

#include "matrix_recast.h"
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <iterator>

/*! \brief Prints the content of a vector nicely */
//template<typename Scalar_>
//std::ostream &operator<<(std::ostream &out, const std::vector<Scalar_> &v) {
//    if (!v.empty()) {
////        out << "[ ";
//        std::copy(v.begin(), v.end(), std::ostream_iterator<Scalar_>(out, "  "));
//        out << '\n';
//    }
//    return out;
//}

template<typename Scalar>
matrix_recast<Scalar>::matrix_recast(const Scalar *matrix_ptr_, int L_):matrix_ptr(matrix_ptr_), L(L_){
    recheck_all();
}

template<typename Scalar>
void matrix_recast<Scalar>::prune(double threshold){
    matrix_pruned.resize(L*L);
    Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> matmap   (matrix_ptr,L,L);
    Eigen::Map<Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> matpruned(matrix_pruned.data(),L,L);
    matpruned = (matmap.array().cwiseAbs() < threshold).select(Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>::Zero(L,L), matmap);
    recheck_all();
    pruned = true;
}

template<typename Scalar>
void matrix_recast<Scalar>:: recheck_all(){
    check_if_sparse();
    check_if_hermitian();
    check_if_real();
}

template<typename Scalar>
void matrix_recast<Scalar>::check_if_real() {
    Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> matrix (matrix_ptr,L,L);
    if constexpr (std::is_same<Scalar, double>::value){isReal = true;}
    else {isReal = matrix.imag().isZero(1e-14);}
}

template<typename Scalar>
void matrix_recast<Scalar>::check_if_hermitian() {
    Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> matrix (matrix_ptr,L,L);
    isHermitian = matrix.isApprox(matrix.adjoint(), 1e-14);
}

template<typename Scalar>
void matrix_recast<Scalar>::check_if_sparse() {
    Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> matrix (matrix_ptr,L,L);
    sparcity = (matrix.array().cwiseAbs() > 1e-14 )
                       .select(Eigen::MatrixXd::Ones(L,L),0).sum() / static_cast<double>(matrix.size());
    isSparse =  sparcity < 0.1;
}

template<typename Scalar>
DenseMatrixProduct<double> matrix_recast<Scalar>::get_as_real_dense() {
    if constexpr(std::is_same<Scalar, double>::value){
        if (pruned){return DenseMatrixProduct<double>(matrix_pruned.data(),L,true);}
        else       {return DenseMatrixProduct<double>(matrix_ptr,L,true);}
    }else{
//        assert(is_real and "ERROR: The given matrix has a nonzero imaginary part. Can't convert to real.");
        if (not isReal){
            double sum = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_ptr,L,L).imag().cwiseAbs().sum();
            std::cerr << "WARNING: The given matrix has a nonzero imaginary part, yet converting to real. Imag sum: " << sum << std::endl;
        }        Eigen::MatrixXd matrix_recast;
        if (pruned){matrix_recast = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_pruned.data(),L,L).real();}
        else       {matrix_recast = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_ptr,L,L).real();}
        return DenseMatrixProduct<double>(matrix_recast.data(),L,true);

    }
}

template<typename Scalar>
DenseMatrixProduct<std::complex<double>> matrix_recast<Scalar>::get_as_cplx_dense() {
    if constexpr(std::is_same<Scalar, double>::value){
        Eigen::MatrixXcd matrix_recast(L,L);
        matrix_recast.setZero();
        if (pruned){matrix_recast.real() = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_pruned.data(),L,L);}
        else       {matrix_recast.real() = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_ptr,L,L);}
        return DenseMatrixProduct<std::complex<double>>(matrix_recast.data(),L,true);
    }else{
        return DenseMatrixProduct<std::complex<double>>(matrix_ptr,L,true);
    }
}

template<typename Scalar>
SparseMatrixProduct<double> matrix_recast<Scalar>::get_as_real_sparse() {
    if constexpr(std::is_same<Scalar, double>::value){
        if(pruned){return SparseMatrixProduct<double>(matrix_pruned.data(),L,true);}
        else      {return SparseMatrixProduct<double>(matrix_ptr,L,true);}
    }else{
        if (not isReal){
            double sum = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_ptr,L,L).imag().cwiseAbs().sum();
            std::cerr << "WARNING: The given matrix has a nonzero imaginary part, yet converting to real. Imag sum: " << sum << std::endl;
        }
        Eigen::MatrixXd matrix_recast;
        if(pruned){matrix_recast = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_pruned.data(),L,L).real();}
        else      {matrix_recast = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_ptr,L,L).real();}
        return SparseMatrixProduct<double>(matrix_recast.data(),L,true);
    }
}

template<typename Scalar>
SparseMatrixProduct<std::complex<double>> matrix_recast<Scalar>::get_as_cplx_sparse() {
    if constexpr(std::is_same<Scalar, double>::value){
        Eigen::MatrixXcd matrix_recast(L,L);
        matrix_recast.setZero();
        if(pruned){matrix_recast.real() = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_pruned.data(),L,L);}
        else      {matrix_recast.real() = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_ptr,L,L);}
        return SparseMatrixProduct<std::complex<double>>(matrix_recast.data(),L,true);
    }else{
        if(pruned){return SparseMatrixProduct<std::complex<double>>(matrix_pruned.data(),L,true);}
        else      {return SparseMatrixProduct<std::complex<double>>(matrix_ptr,L,true);}
    }
}

//
//template<typename Scalar>
//void matrix_recast<Scalar>::convert_to_real_dense(){
//    // READ THIS TO LEARN MORE http://atantet.github.io/ATSuite_cpp/atspectrum_8hpp_source.html
//
//    matrix_real_dense.clear();
//    matrix_cplx_dense.clear();
////    matrix_real_sparse.clear();
////    matrix_cplx_sparse.clear();
//
//
////    matrix_real_dense.L    = L;
////    matrix_real_dense.N    = L*L;
//    if constexpr(std::is_same<Scalar, double>::value){
//        matrix_real_dense = DenseMatrixProduct<double>(matrix_ptr,L);
//    }else{
//        Eigen::MatrixXd matrix_recast = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_ptr,L,L).real();
//        matrix_real_dense = DenseMatrixProduct<double>(matrix_recast.data(),L);
//    }
//
//}
//
//
//template<typename Scalar>
//void matrix_recast<Scalar>::convert_to_cplx_dense(){
//    // READ THIS TO LEARN MORE http://atantet.github.io/ATSuite_cpp/atspectrum_8hpp_source.html
//
//    matrix_real_dense.clear();
//    matrix_cplx_dense.clear();
////    matrix_real_sparse.clear();
////    matrix_cplx_sparse.clear();
//
//
////    matrix_cplx_dense.L    = L;
////    matrix_cplx_dense.N    = L*L;
//    if constexpr(std::is_same<Scalar, double>::value){
//        Eigen::MatrixXcd matrix_recast = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>> (matrix_ptr,L,L);
//        matrix_cplx_dense =  DenseMatrixProduct<std::complex<double>>(matrix_recast.data(),L);
//    }else{
//        matrix_cplx_dense = DenseMatrixProduct<std::complex<double>>(matrix_ptr,L);
//    }
//}

//template<typename Scalar>
//void matrix_recast<Scalar>::convert_to_real_sparse(){
    // READ THIS TO LEARN MORE http://atantet.github.io/ATSuite_cpp/atspectrum_8hpp_source.html

//    matrix_real_dense.clear();
//    matrix_cplx_dense.clear();
//    matrix_real_sparse.clear();
//    matrix_cplx_sparse.clear();
//
//    assert(isReal and "Matrix is not real!");
//    Eigen::SparseMatrix<double> matrix_recast = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>>(matrix_ptr,L,L).real().sparseView();
//    matrix_recast.makeCompressed();
//    matrix_real_sparse.nnz = matrix_recast.nonZeros();
//    matrix_real_sparse.L   = L;
//    matrix_real_sparse.N   = L*L;
//    matrix_real_sparse.irow = std::vector<int>   (matrix_recast.innerIndexPtr(), matrix_recast.innerIndexPtr() + matrix_recast.nonZeros() );
//    matrix_real_sparse.pcol = std::vector<int>   (matrix_recast.outerIndexPtr(), matrix_recast.outerIndexPtr() + matrix_recast.outerSize() +1);
//    matrix_real_sparse.vals = std::vector<double>(matrix_recast.valuePtr()     , matrix_recast.valuePtr()      + matrix_recast.nonZeros());
//

//}

//template<typename Scalar>
//void matrix_recast<Scalar>::convert_to_cplx_sparse(){
    // READ THIS TO LEARN MORE http://atantet.github.io/ATSuite_cpp/atspectrum_8hpp_source.html

//    matrix_real_dense.clear();
//    matrix_cplx_dense.clear();
//    matrix_real_sparse.clear();
//    matrix_cplx_sparse.clear();
//    assert(not isReal and "Matrix is not cplx!");
//    Eigen::SparseMatrix<std::complex<double>> matrix_recast = Eigen::Map<const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>>(matrix_ptr,L,L).template cast<std::complex<double>>().sparseView();
//    matrix_recast.makeCompressed();
//    matrix_recast.finalize();
//    std::cout << "Matrix compressed: \n" << matrix_recast << std::endl;
//
//    matrix_cplx_sparse.nnz = matrix_recast.nonZeros();
//    matrix_cplx_sparse.L   = L;
//    matrix_cplx_sparse.N   = L*L;
//    matrix_cplx_sparse.irow = std::vector<int>   (matrix_recast.innerIndexPtr(), matrix_recast.innerIndexPtr() + matrix_recast.nonZeros() );
//    matrix_cplx_sparse.pcol = std::vector<int>   (matrix_recast.outerIndexPtr(), matrix_recast.outerIndexPtr() + matrix_recast.outerSize() +1);
//    matrix_cplx_sparse.vals = std::vector<std::complex<double>>(matrix_recast.valuePtr()       , matrix_recast.valuePtr()       + matrix_recast.nonZeros());
//
//    std::cout << "nonzeros : " << matrix_recast.nonZeros() << std::endl;
//    std::cout << "innerSize: " << matrix_recast.innerSize() << std::endl;
//    std::cout << "outerSize: " << matrix_recast.outerSize() << std::endl;
//    std::cout << "irow \n" << matrix_cplx_sparse.irow << std::endl;
//    std::cout << "pcol \n" << matrix_cplx_sparse.pcol << std::endl;
//    std::cout << "vals \n" << matrix_cplx_sparse.vals << std::endl;
//    auto innernnz = std::vector<int>   (matrix_recast.innerNonZeroPtr(), matrix_recast.innerNonZeroPtr() + matrix_recast.innerSize() );
//    auto innernnz = std::vector<int>   (matrix_recast.innerNonZeroPtr(), matrix_recast.innerIndexPtr() + matrix_recast.innerSize() );
//    std::cout << "innernnz: \n" << innernnz << std::endl;

//}

template class matrix_recast<double>;
template class matrix_recast<std::complex<double>>;


#include <stdexcept>
#define BOOST_TEST_MODULE MyTest
#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_CASE( test_success_1 )
{
    BOOST_CHECK( 2 * 3 == 6 );
}

BOOST_AUTO_TEST_CASE( test_success_2 )
{
    BOOST_CHECK( 4 * 3 == 12 );
}


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2013 The University of Sydney
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT
// HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
// BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
// OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
// IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <gtest/gtest.h>
#include "../frame_transforms.h"
#include <boost/math/constants/constants.hpp>

using namespace snark::frame_transforms;

TEST(transforms, dh_to_matrix)
{
    dh_transform T_dh;
    T_dh.alpha=1;
    T_dh.r=1;
    T_dh.theta=boost::math::constants::pi<double>()/6;
    T_dh.alpha=-boost::math::constants::pi<double>()/2;
    EXPECT_LT((dh_to_matrix(T_dh)-(Eigen::Matrix4d()<<0.866025,0,-0.5,0.866025,0.5,0,0.866025,0.5,0,-1,0,0,0,0,0,1).finished()).norm(),1e-2);

}

TEST(transforms, dh_to_tr)
{
    dh_transform T_dh;
    T_dh.alpha=1;
    T_dh.r=1;
    T_dh.theta=boost::math::constants::pi<double>()/6;
    T_dh.alpha=-boost::math::constants::pi<double>()/2;
    tr_transform T_tr=dh_to_tr(T_dh);
    EXPECT_LT((homogeneous_transform(T_tr.rotation.toRotationMatrix(),T_tr.translation)-(Eigen::Matrix4d()<<0.866025,0,-0.5,0.866025,0.5,0,0.866025,0.5,0,-1,0,0,0,0,0,1).finished()).norm(),1e-2);

}

int main(int argc, char *argv[])
{
    ::testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


#include <vector>
#include <algorithm>
#include <cstdint>
#include <cmath>

#include <boost/multiprecision/cpp_int.hpp>

namespace opencas
{
	namespace multiprecision
	{
		using Integer = boost::multiprecision::cpp_int;
	}
}