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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/arch/common/detail/generic/tanh_kernel.hpp>
#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/constant/mtwo.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/exp.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/nbtrue.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/tanh.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD_IF( tanh_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        //////////////////////////////////////////////////////////////////////////////
        // if x = abs(a0) is less than 5/8 tanh is computed using a polynomial(float)
        // (respectively rational(double)) approx from cephes.
        // else
        // tanh(a0) is  sign(a0)*(1 - 2/(exp(2*x)+1))
        //////////////////////////////////////////////////////////////////////////////
        A0 x = bs::abs(a0);
        auto test0= is_less(x, Ratio<A0, 5, 8>());
        A0 bts = bitofsign(a0);
        std::size_t nb = nbtrue(test0);
        A0 z = One<A0>();
        if(nb > 0)
        {
          A0 x2 = sqr(x);
          z = detail::tanh_kernel<A0>::tanh(x, x2);
          if(nb >= A0::static_size) return  bitwise_xor(z, bts);
        }
        A0 r = fma(Mtwo<A0>(), rec(inc(exp(x+x))), One<A0>());
        return bitwise_xor(if_else(test0, z, r), bts);
      }
   };

} } }

#endif


#ifndef FAST_GICP_FAST_VGICP_IMPL_HPP
#define FAST_GICP_FAST_VGICP_IMPL_HPP

#include <atomic>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/search/kdtree.h>
#include <pcl/registration/registration.h>

#include <fast_gicp/so3/so3.hpp>
#include <fast_gicp/gicp/fast_vgicp.hpp>

namespace fast_gicp {

template <typename PointSource, typename PointTarget>
FastVGICP<PointSource, PointTarget>::FastVGICP() : FastGICP<PointSource, PointTarget>() {
  this->reg_name_ = "FastVGICP";

  voxel_resolution_ = 1.0;
  search_method_ = NeighborSearchMethod::DIRECT1;
  voxel_mode_ = VoxelAccumulationMode::ADDITIVE;
}

template <typename PointSource, typename PointTarget>
FastVGICP<PointSource, PointTarget>::~FastVGICP() {}

template <typename PointSource, typename PointTarget>
void FastVGICP<PointSource, PointTarget>::setResolution(double resolution) {
  voxel_resolution_ = resolution;
}

template <typename PointSource, typename PointTarget>
void FastVGICP<PointSource, PointTarget>::setNeighborSearchMethod(NeighborSearchMethod method) {
  search_method_ = method;
}

template <typename PointSource, typename PointTarget>
void FastVGICP<PointSource, PointTarget>::setVoxelAccumulationMode(VoxelAccumulationMode mode) {
  voxel_mode_ = mode;
}

template <typename PointSource, typename PointTarget>
void FastVGICP<PointSource, PointTarget>::swapSourceAndTarget() {
  input_.swap(target_);
  source_kdtree_.swap(target_kdtree_);
  source_covs_.swap(target_covs_);
  voxelmap_.reset();
  voxel_correspondences_.clear();
  voxel_mahalanobis_.clear();
}

template <typename PointSource, typename PointTarget>
void FastVGICP<PointSource, PointTarget>::setInputTarget(const PointCloudTargetConstPtr& cloud) {
  // if (target_ == cloud) {
  //   return;
  // }

  FastGICP<PointSource, PointTarget>::setInputTarget(cloud);
  voxelmap_.reset();
}

template <typename PointSource, typename PointTarget>
void FastVGICP<PointSource, PointTarget>::computeTransformation(PointCloudSource& output, const Matrix4& guess) {
  voxelmap_.reset();

  FastGICP<PointSource, PointTarget>::computeTransformation(output, guess);
}

template <typename PointSource, typename PointTarget>
void FastVGICP<PointSource, PointTarget>::update_correspondences(const Eigen::Isometry3d& trans) {
  voxel_correspondences_.clear();
  auto offsets = neighbor_offsets(search_method_);

  std::vector<std::vector<std::pair<int, GaussianVoxel::Ptr>>> corrs(num_threads_);
  for (auto& c : corrs) {
    c.reserve((input_->size() * offsets.size()) / num_threads_);
  }

#pragma omp parallel for num_threads(num_threads_) schedule(guided, 8)
  for (int i = 0; i < input_->size(); i++) {
    const Eigen::Vector4d mean_A = input_->at(i).getVector4fMap().template cast<double>();
    Eigen::Vector4d transed_mean_A = trans * mean_A;
    Eigen::Vector3i coord = voxelmap_->voxel_coord(transed_mean_A);

    for (const auto& offset : offsets) {
      auto voxel = voxelmap_->lookup_voxel(coord + offset);
      if (voxel != nullptr) {
        corrs[omp_get_thread_num()].push_back(std::make_pair(i, voxel));
      }
    }
  }

  voxel_correspondences_.reserve(input_->size() * offsets.size());
  for (const auto& c : corrs) {
    voxel_correspondences_.insert(voxel_correspondences_.end(), c.begin(), c.end());
  }

  // precompute combined covariances
  voxel_mahalanobis_.resize(voxel_correspondences_.size());

#pragma omp parallel for num_threads(num_threads_) schedule(guided, 8)
  for (int i = 0; i < voxel_correspondences_.size(); i++) {
    const auto& corr = voxel_correspondences_[i];
    const auto& cov_A = source_covs_[corr.first];
    const auto& cov_B = corr.second->cov;

    Eigen::Matrix4d RCR = cov_B + trans.matrix() * cov_A * trans.matrix().transpose();
    RCR(3, 3) = 1.0;

    voxel_mahalanobis_[i] = RCR.inverse();
    voxel_mahalanobis_[i](3, 3) = 0.0;
  }
}

template <typename PointSource, typename PointTarget>
double FastVGICP<PointSource, PointTarget>::linearize(const Eigen::Isometry3d& trans, Eigen::Matrix<double, 6, 6>* H, Eigen::Matrix<double, 6, 1>* b) {
  if (voxelmap_ == nullptr) {
    voxelmap_.reset(new GaussianVoxelMap<PointTarget>(voxel_resolution_, voxel_mode_));
    voxelmap_->create_voxelmap(*target_, target_covs_);
  }

  update_correspondences(trans);

  double sum_errors = 0.0;
  std::vector<Eigen::Matrix<double, 6, 6>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 6>>> Hs(num_threads_);
  std::vector<Eigen::Matrix<double, 6, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 1>>> bs(num_threads_);
  for (int i = 0; i < num_threads_; i++) {
    Hs[i].setZero();
    bs[i].setZero();
  }

#pragma omp parallel for num_threads(num_threads_) reduction(+ : sum_errors) schedule(guided, 8)
  for (int i = 0; i < voxel_correspondences_.size(); i++) {
    const auto& corr = voxel_correspondences_[i];
    auto target_voxel = corr.second;

    const Eigen::Vector4d mean_A = input_->at(corr.first).getVector4fMap().template cast<double>();
    const auto& cov_A = source_covs_[corr.first];

    const Eigen::Vector4d mean_B = corr.second->mean;
    const auto& cov_B = corr.second->cov;

    const Eigen::Vector4d transed_mean_A = trans * mean_A;
    const Eigen::Vector4d error = mean_B - transed_mean_A;

    double w = std::sqrt(target_voxel->num_points);
    sum_errors += w * error.transpose() * voxel_mahalanobis_[i] * error;

    if (H == nullptr || b == nullptr) {
      continue;
    }

    Eigen::Matrix<double, 4, 6> dtdx0 = Eigen::Matrix<double, 4, 6>::Zero();
    dtdx0.block<3, 3>(0, 0) = skewd(transed_mean_A.head<3>());
    dtdx0.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity();

    Eigen::Matrix<double, 4, 6> jlossexp = dtdx0;

    Eigen::Matrix<double, 6, 6> Hi = w * jlossexp.transpose() * voxel_mahalanobis_[i] * jlossexp;
    Eigen::Matrix<double, 6, 1> bi = w * jlossexp.transpose() * voxel_mahalanobis_[i] * error;

    int thread_num = omp_get_thread_num();
    Hs[thread_num] += Hi;
    bs[thread_num] += bi;
  }

  if (H && b) {
    H->setZero();
    b->setZero();
    for (int i = 0; i < num_threads_; i++) {
      (*H) += Hs[i];
      (*b) += bs[i];
    }
  }

  return sum_errors;
}

template <typename PointSource, typename PointTarget>
double FastVGICP<PointSource, PointTarget>::compute_error(const Eigen::Isometry3d& trans) {
  double sum_errors = 0.0;
#pragma omp parallel for num_threads(num_threads_) reduction(+ : sum_errors)
  for (int i = 0; i < voxel_correspondences_.size(); i++) {
    const auto& corr = voxel_correspondences_[i];
    auto target_voxel = corr.second;

    const Eigen::Vector4d mean_A = input_->at(corr.first).getVector4fMap().template cast<double>();
    const auto& cov_A = source_covs_[corr.first];

    const Eigen::Vector4d mean_B = corr.second->mean;
    const auto& cov_B = corr.second->cov;

    const Eigen::Vector4d transed_mean_A = trans * mean_A;
    const Eigen::Vector4d error = mean_B - transed_mean_A;

    double w = std::sqrt(target_voxel->num_points);
    sum_errors += w * error.transpose() * voxel_mahalanobis_[i] * error;
  }

  return sum_errors;
}

}  // namespace fast_gicp

#endif


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

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

#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/inv2pi.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/pio_2.hpp>
#include <boost/simd/constant/pix2_1.hpp>
#include <boost/simd/constant/pix2_2.hpp>
#include <boost/simd/constant/pix2_3.hpp>
#include <boost/simd/constant/threeeps.hpp>
#include <boost/simd/constant/twopi.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/rem_pio2.hpp>
#include <boost/simd/function/rem_pio2_medium.hpp>
#include <boost/simd/function/round2even.hpp>
#include <boost/simd/function/tofloat.hpp>
#include <boost/simd/arch/common/detail/tags.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD (rem_2pi_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_ < bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      using i_t = bd::as_integer_t<A0>;
      A0 xr;
      i_t n = rem_pio2(a0, xr);
      xr = xr+tofloat(n)*Pio_2<A0>();
      return if_else((xr > Pi<A0>()), xr-Twopi<A0>(), xr);
    }

  };

  BOOST_DISPATCH_OVERLOAD (rem_2pi_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::generic_ <bd::floating_<A0> >
                          , bd::target_ <bd::unspecified_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A1 const&) const BOOST_NOEXCEPT
    {
      typedef typename A1::type selector;
      return rem2pi<selector, void>::rem(a0);
    }

    template < class T, class dummy = void> struct rem2pi
    {
      static BOOST_FORCEINLINE A0 rem( A0 const&, A0 &, A0&) BOOST_NOEXCEPT
      {
        BOOST_ASSERT_MSG(false, "wrong target for rem_2pi");
      }
    };
    template < class dummy> struct rem2pi < tag::big_tag, dummy>
    {
      static BOOST_FORCEINLINE A0 rem( A0 const& x) BOOST_NOEXCEPT
      {
        return rem_2pi(x);
      }
    };
    template < class dummy> struct rem2pi < tag::very_small_tag, dummy > // |a0| <2*pi
    {
      static BOOST_FORCEINLINE A0 rem( A0 const& x) BOOST_NOEXCEPT
      {
        return if_else(gt(x, Pi<A0>()), x-Twopi<A0>(),
                       if_else(lt(x, -Pi<A0>()), x+Twopi<A0>(), x));
      }
    };
    template < class dummy> struct rem2pi < tag::small_tag, dummy >// |a0| <= 20*pi
    {
      static BOOST_FORCEINLINE A0 rem( A0 const& x) BOOST_NOEXCEPT
      {
        A0 xi =  round2even(x*Inv2pi<A0>());
        A0 xr = x-xi*Pix2_1<A0>();
        xr -= xi*Pix2_2<A0>();
        xr -= xi*Pix2_3<A0>();
        return xr;
      }
    };

    template < class dummy> struct rem2pi < tag::medium_tag, dummy >
    {
      static BOOST_FORCEINLINE A0 rem( A0 const& x) BOOST_NOEXCEPT
      {
        using i_t = bd::as_integer_t<A0>;
        A0 xr;
        i_t n = rem_pio2_medium(x, xr);
        xr += tofloat(n)*Pio_2<A0>();
        xr = if_else(gt(xr, Pi<A0>()), xr-Twopi<A0>(), xr);
        return xr;
      }
    };
  };
} } }

#endif


/*
 * OrientationBox.hpp
 *
 *  Created on: 01.11.2018
 *      Author: tomlucas
 */

#ifndef ESTIMATORS_STATEBOXES_ORIENTATIONBOX_HPP_
#define ESTIMATORS_STATEBOXES_ORIENTATIONBOX_HPP_

#include "StateBox.hpp"
#include <eigen3/Eigen/Core>
#include <Eigen_Utils.hpp>
#include <stdio.h>

namespace zavi
::estimator::state_boxes {
	/**
	 * Orientation modelled as euler rodriguez rotation
	 */
	class OrientationBox : public StateBox<9,3,9> {
		template<typename T>
		using ROT_MATRIX= Eigen::Matrix<T,3,3>;

	public:
		OrientationBox(std::shared_ptr<plugin::SensorPlugin> sensor):StateBox<9,3,9>(sensor) {
		}
		virtual ~OrientationBox() {};

		template<typename T>
		inline static OUTER_T<T> boxPlus(const OUTER_T<T> &state,const INNER_T<T> &delta) {
			assert_inputs(state,delta);
			return Eigen::Map<OUTER_T<T> >(zavi::eigen_util::boxPlusOrientation<T>(Eigen::Map<const ROT_MATRIX<T> >(state.data()),delta).data());
		}
		template<typename T>
		inline static INNER_T<T> boxPlusInnerSpace(const INNER_T<T> &delta1,const INNER_T<T> &delta2) {
			assert_inputs(delta1,delta2);
			INNER_T<T> sum=delta1+delta2;
			sum=zavi::eigen_util::wrapRotDeltaVector(sum);
			return sum;
		}

		template<typename T>
		inline static INNER_T<T> boxMinus(const OUTER_T<T> &a,const OUTER_T<T> &b) {
			assert_inputs(a,b);
			return zavi::eigen_util::boxMinusOrientation<T,T>(Eigen::Map<const ROT_MATRIX<T> >(a.data()),Eigen::Map<const ROT_MATRIX<T> >(b.data()));
		}

		template<typename T>
		inline static OUTER_T<T> stateTransition(const OUTER_T<T> & state,double time_diff ) {
			assert_inputs(state,time_diff);
			return state;
		}
		inline INNER_T<double> getSTD(double time_diff) {
			INNER_T<double> noise=INNER_T<double>::Ones()*30*time_diff;
			return noise;
		}

		static OUTER_T<double> static_normalise(const OUTER_T<double> &state) {
					return Eigen::Map<const OUTER_T<double>>(eigen_util::normaliseRotation(Eigen::Map<const ROT_MATRIX<double> >(state.data()) ).data());
				}
		virtual OUTER_T<double> normalise(const OUTER_T<double> &state) const {
			return static_normalise(state);
		}
	};
}
//zavi::estimator::state_boxes

#endif /* ESTIMATORS_STATEBOXES_ORIENTATIONBOX_HPP_ */


#include <boost/test/unit_test.hpp>

#include "../math/math.h"

USING_NAMESPACE(sway)

BOOST_AUTO_TEST_SUITE(TSizeTestSuite)

/*!
 * Убеждаемся, что конструктор по умолчанию приводит все компоненты к нулю.
 */
BOOST_AUTO_TEST_CASE(TSizeTestCase_DefaultConstructor) {
	const math::TSize<s32> size;

	BOOST_CHECK_EQUAL(size.getW(), 0);
	BOOST_CHECK_EQUAL(size.getH(), 0);
}

/*!
 * Убеждаемся, что конструктор устанавливает все значения компонентов в те,
 * которые были заданы.
 */
BOOST_AUTO_TEST_CASE(TSizeTestCase_ComponentConstructor) {
	const s32 w = 1, h = 2;
	const math::TSize<s32> size(w, h);

	BOOST_CHECK_EQUAL(size.getW(), w);
	BOOST_CHECK_EQUAL(size.getH(), h);
}

BOOST_AUTO_TEST_CASE(TSizeTestCase_ScalarConstructor) {
	const s32 w = 1, h = 2;
	const math::TSize<s32> size(math::TSize<s32>(w, h));

	BOOST_CHECK_EQUAL(size.getW(), w);
	BOOST_CHECK_EQUAL(size.getH(), h);
}

/*!
 * Тест для оператора равенства.
 */
BOOST_AUTO_TEST_CASE(TSizeTestCase_EqualityOperator1) {
	const math::TSize<s32> size1(0, 0), size2(0, 0);

	BOOST_CHECK_EQUAL(size1 == size2, true);
}

/*!
 * Тест для оператора равенства.
 */
BOOST_AUTO_TEST_CASE(TSizeTestCase_EqualityOperator2) {
	const math::TSize<s32> size1(1, 0), size2(0, 0);

	BOOST_CHECK_EQUAL(size1 == size2, false);
}

/*!
 * Тест для оператора равенства.
 */
BOOST_AUTO_TEST_CASE(TSizeTestCase_EqualityOperator3) {
	const math::TSize<s32> size1(0, 0), size2(0, 2);

	BOOST_CHECK_EQUAL(size1 == size2, false);
}

BOOST_AUTO_TEST_CASE(TSizeTestCase_EqualityOperator4) {
	const math::TSize<s32> size(1, 2);

	BOOST_CHECK_EQUAL(size, math::TSize<s32>(1, 2));
}

/*!
 * Тест для оператора неравенства.
 */
BOOST_AUTO_TEST_CASE(TSizeTestCase_NonEqualityOperator1) {
	const math::TSize<s32> size1(1, 2), size2(1, 2);

	BOOST_CHECK_EQUAL(size1 != size2, false);
}

/*!
 * Тест для оператора неравенства.
 */
BOOST_AUTO_TEST_CASE(TSizeTestCase_NonEqualityOperator2) {
	const math::TSize<s32> size1(1, 2), size2(0, 2);

	BOOST_CHECK_EQUAL(size1 != size2, true);
}

/*!
 * Тест для оператора неравенства.
 */
BOOST_AUTO_TEST_CASE(TSizeTestCase_NonEqualityOperator3) {
	const math::TSize<s32> size1(1, 2), size2(1, 0);

	BOOST_CHECK_EQUAL(size1 != size2, true);
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef SHASTA_ENUMERATE_PATHS_HPP
#define SHASTA_ENUMERATE_PATHS_HPP

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/iteration_macros.hpp>

#include "algorithm.hpp"
#include <stack>
#include "tuple.hpp"
#include "vector.hpp"

namespace shasta {

template<class G> void enumerateSelfAvoidingPaths(const G&,
    typename G::vertex_descriptor vA, typename G::vertex_descriptor vB,
    vector<vector<typename G::edge_descriptor> > &paths);

}

// Enumerate self-avoiding paths starting at v0 and ending at v1.
// Self-avoiding means that an edge cannot be used twice.
template<class G> void shasta::enumerateSelfAvoidingPaths(const G &g,
    typename G::vertex_descriptor vA, typename G::vertex_descriptor vB,
    vector<vector<typename G::edge_descriptor> > &paths)
{
    using vertex_descriptor = typename G::vertex_descriptor;
    using edge_descriptor = typename G::edge_descriptor;
    using out_edge_iterator = typename G::out_edge_iterator;
    using Path = vector<typename G::edge_descriptor>;
    using std::stack;

    paths.clear();
    stack<Path> partialPaths;

    // For some reason I was not able to get BGL_FORALL_OUTEDGES_T
    // to work, so using explicit iterators instead.
    out_edge_iterator it, end;

    std::tie(it, end) = boost::out_edges(vA, g);
    for (; it != end; ++it) {
        const edge_descriptor e = *it;
        const vertex_descriptor v1 = boost::target(e, g);
        const Path path(1, e);
        if (v1 == vB) {
            paths.push_back(path);
        } else {
            partialPaths.push(path);
        }
    }

    // Recursively add edges to the partial paths.
    while (not partialPaths.empty()) {
        const Path path = partialPaths.top();
        partialPaths.pop();

        const vertex_descriptor v0 = boost::target(path.back(), g);
        std::tie(it, end) = boost::out_edges(v0, g);
        for (; it != end; ++it) {
            const edge_descriptor e = *it;

            // If e is already on the path, skip it.
            if (find(path.begin(), path.end(), e) != path.end()) {
                continue;
            }

            const vertex_descriptor v1 = target(e, g);
            Path newPath = path;
            newPath.push_back(e);
            if (v1 == vB) {
                paths.push_back(newPath);
            } else {
                partialPaths.push(newPath);
            }
        }
    }
}

#endif


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// 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)
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <vector>
#include <string>
#include <boost/graph/adjacency_list.hpp>
#include <boost/tuple/tuple.hpp>
enum family
{
    Jeanie,
    Debbie,
    Rick,
    John,
    Amanda,
    Margaret,
    Benjamin,
    N
};
int main()
{
    using namespace boost;
    const char* name[] = { "Jeanie", "Debbie", "Rick", "John", "Amanda",
        "Margaret", "Benjamin" };

    adjacency_list<> g(N);
    add_edge(Jeanie, Debbie, g);
    add_edge(Jeanie, Rick, g);
    add_edge(Jeanie, John, g);
    add_edge(Debbie, Amanda, g);
    add_edge(Rick, Margaret, g);
    add_edge(John, Benjamin, g);

    graph_traits< adjacency_list<> >::vertex_iterator i, end;
    graph_traits< adjacency_list<> >::adjacency_iterator ai, a_end;
    property_map< adjacency_list<>, vertex_index_t >::type index_map
        = get(vertex_index, g);

    for (boost::tie(i, end) = vertices(g); i != end; ++i)
    {
        std::cout << name[get(index_map, *i)];
        boost::tie(ai, a_end) = adjacent_vertices(*i, g);
        if (ai == a_end)
            std::cout << " has no children";
        else
            std::cout << " is the parent of ";
        for (; ai != a_end; ++ai)
        {
            std::cout << name[get(index_map, *ai)];
            if (boost::next(ai) != a_end)
                std::cout << ", ";
        }
        std::cout << std::endl;
    }
    return EXIT_SUCCESS;
}


/* This file is part of the Tomographer project, which is distributed under the
 * terms of the MIT license.
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2016 ETH Zurich, Institute for Theoretical Physics, Philippe Faist
 * Copyright (c) 2017 Caltech, Institute for Quantum Information and Matter, Philippe Faist
 *
 * 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 <cmath>

#include <string>
#include <sstream>
#include <random>

#include <boost/math/constants/constants.hpp>

// include before <Eigen/*> !
#include "test_tomographer.h"

#include <tomographer/densedm/dmtypes.h>

// -----------------------------------------------------------------------------
// fixture(s)

struct test_dmtypes_fixture {
  test_dmtypes_fixture()
  {
  }
  ~test_dmtypes_fixture()
  {
  }
  template<typename DMTypes> void test_dmtypes(const Eigen::Index dim)
  {
    // has RealScalar and ComplexScalar types
    typename DMTypes::RealScalar a = 1.0;
    typename DMTypes::ComplexScalar z(3.0, 4.0);

    BOOST_CHECK_CLOSE(a, 1.0, tol_percent);
    BOOST_CHECK_CLOSE(std::abs(z), 5.0, tol_percent);

    DMTypes dmt(dim);

    // has dim() property
    BOOST_CHECK_EQUAL(dmt.dim(), dim);

    BOOST_CHECKPOINT("About to test that a wrong fixed dimension will throw");

    // if has fixed dim, make sure that if we attempt to construct a bad dimension,
    // something explodes
    if (DMTypes::FixedDim != Eigen::Dynamic) {
      //      fprintf(stderr, "entering block of interest...\n");
      EigenAssertTest::setting_scope settingvariable(true); // eigen_assert() should throw an exception.
      //      fprintf(stderr, "initialized setting variable\n");
      BOOST_CHECK_THROW(
     	  DMTypes baddmt(dim+1),
     	  ::Tomographer::Tools::EigenAssertException
     	  );
      //      fprintf(stderr, "exiting block of interest\n");
    }

    // matrix type
    BOOST_CHECKPOINT("Testing MatrixType");

    typename DMTypes::MatrixType rho(dmt.initMatrixType());
    BOOST_CHECK_EQUAL(rho.cols(), dim);
    BOOST_CHECK_EQUAL(rho.rows(), dim);

    // initializes to zero
    BOOST_CHECK_CLOSE(rho.norm(), 0, tol_percent);

    // rho is a superposition of the two first basis states
    rho(0,0) = typename DMTypes::ComplexScalar(.5, 0.0);
    rho(0,1) = typename DMTypes::ComplexScalar(0.0, .5);
    rho(1,0) = typename DMTypes::ComplexScalar(0.0, -.5);
    rho(1,1) = typename DMTypes::ComplexScalar(.5, 0.0);
    BOOST_CHECK_CLOSE(rho(0,1).imag(), .5, tol_percent);
    BOOST_CHECK_CLOSE(rho.trace().real(), 1.0, tol_percent);
    BOOST_CHECK_CLOSE(rho.trace().imag(), 0.0, tol_percent);
    BOOST_CHECK_CLOSE(rho.eigenvalues().real().maxCoeff(), 1.0, tol_percent);

    // vector param type
    BOOST_CHECKPOINT("Testing VectorParamType");

    typename DMTypes::VectorParamType x(dmt.initVectorParamType());
    BOOST_CHECK_EQUAL(x.cols(), 1);
    BOOST_CHECK_EQUAL(x.rows(), dim*dim);
    BOOST_CHECK_CLOSE(x.norm(), 0, tol_percent);

    // vector param Ndof type
    BOOST_CHECKPOINT("Testing VectorParamNdofType");
    typename DMTypes::VectorParamNdofType x2(dmt.initVectorParamNdofType());
    BOOST_CHECK_EQUAL(x2.cols(), 1);
    BOOST_CHECK_EQUAL(x2.rows(), dim*dim-1);
    BOOST_CHECK_CLOSE(x2.norm(), 0, tol_percent);
  }
};

// -----------------------------------------------------------------------------
// test suites

BOOST_FIXTURE_TEST_SUITE(test_densedm_densedmtypes, test_dmtypes_fixture)

BOOST_AUTO_TEST_CASE(qubit_static)
{
  test_dmtypes<Tomographer::DenseDM::DMTypes<2> >(2);
}
BOOST_AUTO_TEST_CASE(qubit_dyn)
{
  test_dmtypes<Tomographer::DenseDM::DMTypes<Eigen::Dynamic> >(2);
}
BOOST_AUTO_TEST_CASE(qudit_static)
{
  test_dmtypes<Tomographer::DenseDM::DMTypes<10> >(10);
}
BOOST_AUTO_TEST_CASE(qudit_dyn)
{
  test_dmtypes<Tomographer::DenseDM::DMTypes<Eigen::Dynamic> >(10);
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <Eigen/Core>

#include "emitter_extra.hpp"

//// define YAML_DEF_EMIT_WITH_EIGEN_FORMATTER to enable Eigen formatter
////   see @ref https://eigen.tuxfamily.org/dox/structEigen_1_1IOFormat.html

// #define YAML_DEF_EMIT_WITH_EIGEN_FORMATTER

namespace YAML
{
namespace detail
{

//// Eigen::DenseCoeffsBase(readonly)

template <typename T>
struct generic_emitter<T, 4,
					   enable_if_t<std::is_base_of<
							   Eigen::DenseCoeffsBase<T, Eigen::ReadOnlyAccessors>, T>::value>>
{
	inline static Emitter& emit(Emitter& emitter, const T& value)
	{
#ifdef YAML_DEF_EMIT_WITH_EIGEN_FORMATTER

		Eigen::IOFormat   format(Eigen::StreamPrecision, 0, ", ", "\n", "[", "]");
		std::stringstream ss;

#ifdef YSL_NAMESPACE // extension

		emitter.SetStreamablePrecision<typename T::Scalar>(ss);

#endif

		return detail::emit_streamable(
				emitter << LocalTag("tensor") << Literal, value.format(format), &ss);

#else

		const auto matrix = value.eval();

		if (matrix.rows() > 1 && matrix.cols() > 1) // simplify vector representation
		{
			emitter << LocalTag("tensor");
		}
		else
		{
			emitter << Flow;
		}

		emitter << BeginSeq;
		for (Eigen::Index i = 0; i < matrix.rows(); ++i)
		{
			emitter << Flow << BeginSeq;
			for (Eigen::Index j = 0; j < matrix.cols(); ++j)
			{
				emitter << detail::as_numeric(matrix(i, j));
			}
			emitter << EndSeq;
		}
		return emitter << EndSeq;

#endif
	}
};

} // namespace detail
} // namespace YAML


#pragma once

#include <boost/optional.hpp>
#include <crab/common/types.hpp>
#include <crab/domains/term/term_expr.hpp>
/*
   Simplifiers for table terms after giving meaning to functors.
*/

namespace crab {
namespace domains {
namespace term {

// Common API to simplifiers
template <class Num, class Ftor> class Simplifier {
protected:
  typedef term_table<Num, Ftor> term_table_t;
  typedef typename term_table_t::term_id_t term_id_t;

  term_table_t &_ttbl;

public:
  Simplifier(term_table_t &term_table) : _ttbl(term_table) {}

  virtual ~Simplifier() {}

  virtual void simplify() = 0;

  // This should not modify the term table
  // FIXME: cannot make const the method without changing
  // constness of other methods.
  virtual boost::optional<term_id_t> simplify_term(term_id_t t) = 0;
};

// Trivial simplifier by giving standard mathematical meaning
// to arithmetic operators assuming that conmutativity,
// associativity etc properties hold as expected.
template <class Num> class NumSimplifier : Simplifier<Num, binary_operation_t> {
  typedef Simplifier<Num, binary_operation_t> simplifier_t;

  typedef term_table<Num, binary_operation_t> term_table_t;
  typedef typename term_table_t::term_id_t term_id_t;
  typedef typename term_table_t::term_t term_t;

  // Simplify term f(left,right)
  boost::optional<term_id_t> simplify_term(binary_operation_t f, term_id_t left,
                                           term_id_t right) {
    // Only consider these two rules:
    //   '/'('*'(x,y),x) = y
    //   '/'('*'(x,y),y) = x
    switch (f) {
    case BINOP_SDIV:
    case BINOP_UDIV: {
      term_t *tleft = this->_ttbl.get_term_ptr(left);
      term_t *tright = this->_ttbl.get_term_ptr(right);

      if ((tleft->kind() == TERM_APP) && term_ftor(tleft) == BINOP_MUL) {
        std::vector<term_id_t> &args(term_args(tleft));
        assert(args.size() == 2);
        term_t *tl = this->_ttbl.get_term_ptr(args[0]);
        term_t *tr = this->_ttbl.get_term_ptr(args[1]);

        if (tl == tright)
          return args[1];
        if (tr == tright)
          return args[0];
      }
    }
    default:
      return boost::optional<term_id_t>();
    }
  }

public:
  NumSimplifier(term_table_t &term_table) : simplifier_t(term_table) {}

  void simplify() {}

  boost::optional<term_id_t> simplify_term(term_id_t t) {
    if (term_t *tt = this->_ttbl.get_term_ptr(t)) {
      if (tt->kind() == TERM_APP) {
        std::vector<term_id_t> &args(term_args(tt));
        assert(args.size() == 2);

        return simplify_term(term_ftor(tt), args[0], args[1]);
      }
    }
    return boost::optional<term_id_t>();
  }
};

} // end namespace term
} // end namespace domains
} // end namespace crab


/******************************************************************************

  This source file is part of the Avogadro project.

  Copyright 2010 Eric C. Brown

  This source code is released under the New BSD License, (the "License").

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.

******************************************************************************/

#include "qtaimcriticalpointlocator.h"
#include "qtaimlsodaintegrator.h"
#include "qtaimmathutilities.h"
#include "qtaimodeintegrator.h"
#include "qtaimwavefunction.h"

#include <Eigen/Core>

#include <QList>

#include <QtConcurrent/QtConcurrentMap>

#include <QDataStream>
#include <QDir>
#include <QFile>
#include <QTemporaryFile>

#include <QVariant>

#include <QFuture>
#include <QFutureWatcher>
#include <QProgressDialog>

using namespace std;
using namespace Eigen;

#define HUGE_REAL_NUMBER 1.e20
#define SMALL_GRADIENT_NORM 1.e-4

namespace Avogadro {
namespace QtPlugins {

QList<QVariant> QTAIMLocateNuclearCriticalPoint(QList<QVariant> input)
{
  const QString fileName = input.at(0).toString();
  const qint64 nucleus = input.at(1).toInt();
  const QVector3D x0y0z0(input.at(2).toReal(), input.at(3).toReal(),
                         input.at(4).toReal());

  QTAIMWavefunction wfn;
  wfn.loadFromBinaryFile(fileName);

  QTAIMWavefunctionEvaluator eval(wfn);

  QVector3D result;

  if (wfn.nuclearCharge(nucleus) < 4) {
    //      QTAIMODEIntegrator
    //      ode(eval,QTAIMODEIntegrator::CMBPMinusThreeGradientInElectronDensity);
    QTAIMLSODAIntegrator ode(
      eval, QTAIMLSODAIntegrator::CMBPMinusThreeGradientInElectronDensity);
    result = ode.integrate(x0y0z0);
  } else {
    result = x0y0z0;
  }

  bool correctSignature;
  Matrix<qreal, 3, 1> xyz;
  xyz << result.x(), result.y(), result.z();

  if (QTAIMMathUtilities::signatureOfASymmetricThreeByThreeMatrix(
        eval.hessianOfElectronDensity(xyz)) == -3) {
    correctSignature = true;
  } else {
    correctSignature = false;
  }

  QList<QVariant> value;

  if (correctSignature) {
    value.append(correctSignature);
    value.append(result.x());
    value.append(result.y());
    value.append(result.z());
  } else {
    value.append(false);
  }

  return value;
}

QList<QVariant> QTAIMLocateBondCriticalPoint(QList<QVariant> input)
{

  QList<QVariant> value;
  value.clear();

  const QString wfnFileName = input.at(0).toString();
  const QString nuclearCriticalPointsFileName = input.at(1).toString();
  const qint64 nucleusA = input.at(2).toInt();
  const qint64 nucleusB = input.at(3).toInt();
  const QVector3D x0y0z0(input.at(4).toReal(), input.at(5).toReal(),
                         input.at(6).toReal());

  QTAIMWavefunction wfn;
  wfn.loadFromBinaryFile(wfnFileName);

  QList<QVector3D> nuclearCriticalPoints;
  QFile nuclearCriticalPointsFile(nuclearCriticalPointsFileName);
  nuclearCriticalPointsFile.open(QIODevice::ReadOnly);
  QDataStream nuclearCriticalPointsFileIn(&nuclearCriticalPointsFile);
  nuclearCriticalPointsFileIn >> nuclearCriticalPoints;
  nuclearCriticalPointsFile.close();

  QList<QPair<QVector3D, qreal>> betaSpheres;
  for (qint64 i = 0; i < nuclearCriticalPoints.length(); ++i) {
    QPair<QVector3D, qreal> thisBetaSphere;
    thisBetaSphere.first = nuclearCriticalPoints.at(i);
    thisBetaSphere.second = 0.1;
    betaSpheres.append(thisBetaSphere);
  }

  QTAIMWavefunctionEvaluator eval(wfn);

  QList<QVector3D> ncpList;

  QVector3D result;
  //    QTAIMODEIntegrator
  //    ode(eval,QTAIMODEIntegrator::CMBPMinusOneGradientInElectronDensity);
  QTAIMLSODAIntegrator ode(
    eval, QTAIMLSODAIntegrator::CMBPMinusOneGradientInElectronDensity);
  result = ode.integrate(x0y0z0);
  Matrix<qreal, 3, 1> xyz;
  xyz << result.x(), result.y(), result.z();

  if (!(QTAIMMathUtilities::signatureOfASymmetricThreeByThreeMatrix(
          eval.hessianOfElectronDensity(xyz)) == -1) ||
      (eval.gradientOfElectronDensity(xyz)).norm() > SMALL_GRADIENT_NORM) {
    value.append(false);
    value.append(result.x());
    value.append(result.y());
    value.append(result.z());
    return value;
  }

  Matrix<qreal, 3, 3> eigenvectorsOfHessian;
  eigenvectorsOfHessian =
    QTAIMMathUtilities::eigenvectorsOfASymmetricThreeByThreeMatrix(
      eval.hessianOfElectronDensity(xyz));
  Matrix<qreal, 3, 1> highestEigenvectorOfHessian;
  highestEigenvectorOfHessian << eigenvectorsOfHessian(0, 2),
    eigenvectorsOfHessian(1, 2), eigenvectorsOfHessian(2, 2);

  const qreal smallStep = 0.01;

  QVector3D forwardStartingPoint(
    result.x() + smallStep * highestEigenvectorOfHessian(0),
    result.y() + smallStep * highestEigenvectorOfHessian(1),
    result.z() + smallStep * highestEigenvectorOfHessian(2));

  QVector3D backwardStartingPoint(
    result.x() - smallStep * highestEigenvectorOfHessian(0),
    result.y() - smallStep * highestEigenvectorOfHessian(1),
    result.z() - smallStep * highestEigenvectorOfHessian(2));

  //    QTAIMODEIntegrator
  //    forwardODE(eval,QTAIMODEIntegrator::SteepestAscentPathInElectronDensity);
  QTAIMLSODAIntegrator forwardODE(
    eval, QTAIMLSODAIntegrator::SteepestAscentPathInElectronDensity);
  forwardODE.setBetaSpheres(betaSpheres);
  QVector3D forwardEndpoint = forwardODE.integrate(forwardStartingPoint);
  QList<QVector3D> forwardPath = forwardODE.path();

  //    QTAIMODEIntegrator
  //    backwardODE(eval,QTAIMODEIntegrator::SteepestAscentPathInElectronDensity);
  QTAIMLSODAIntegrator backwardODE(
    eval, QTAIMLSODAIntegrator::SteepestAscentPathInElectronDensity);
  backwardODE.setBetaSpheres(betaSpheres);
  QVector3D backwardEndpoint = backwardODE.integrate(backwardStartingPoint);
  QList<QVector3D> backwardPath = backwardODE.path();

  qreal smallestDistance = HUGE_REAL_NUMBER;
  qint64 smallestDistanceIndex = 0;

  for (qint64 n = 0; n < wfn.numberOfNuclei(); ++n) {
    Matrix<qreal, 3, 1> a(forwardEndpoint.x(), forwardEndpoint.y(),
                          forwardEndpoint.z());
    Matrix<qreal, 3, 1> b(wfn.xNuclearCoordinate(n), wfn.yNuclearCoordinate(n),
                          wfn.zNuclearCoordinate(n));

    qreal distance = QTAIMMathUtilities::distance(a, b);

    if (distance < smallestDistance) {
      smallestDistance = distance;
      smallestDistanceIndex = n;
    }
  }
  qint64 forwardNucleusIndex = smallestDistanceIndex;

  smallestDistance = HUGE_REAL_NUMBER;
  smallestDistanceIndex = 0;

  for (qint64 n = 0; n < wfn.numberOfNuclei(); ++n) {
    Matrix<qreal, 3, 1> a(backwardEndpoint.x(), backwardEndpoint.y(),
                          backwardEndpoint.z());
    Matrix<qreal, 3, 1> b(wfn.xNuclearCoordinate(n), wfn.yNuclearCoordinate(n),
                          wfn.zNuclearCoordinate(n));

    qreal distance = QTAIMMathUtilities::distance(a, b);

    if (distance < smallestDistance) {
      smallestDistance = distance;
      smallestDistanceIndex = n;
    }
  }
  qint64 backwardNucleusIndex = smallestDistanceIndex;

  bool bondPathConnectsPair;
  if ((forwardNucleusIndex == nucleusA && backwardNucleusIndex == nucleusB) ||
      (forwardNucleusIndex == nucleusB && backwardNucleusIndex == nucleusA)) {
    bondPathConnectsPair = true;
  } else {
    bondPathConnectsPair = false;
  }

  if (bondPathConnectsPair) {
    value.append(true);
    value.append(nucleusA);
    value.append(nucleusB);
    value.append(result.x());
    value.append(result.y());
    value.append(result.z());
    Matrix<qreal, 3, 1> xyz_;
    xyz_ << result.x(), result.y(), result.z();
    value.append(eval.laplacianOfElectronDensity(xyz_));
    value.append(QTAIMMathUtilities::ellipticityOfASymmetricThreeByThreeMatrix(
      eval.hessianOfElectronDensity(xyz_)));
    value.append(1 + forwardPath.length() + 1 + backwardPath.length() + 1);
    value.append(forwardEndpoint.x());
    for (qint64 i = forwardPath.length() - 1; i >= 0; --i) {
      value.append(forwardPath.at(i).x());
    }
    value.append(result.x());
    for (qint64 i = 0; i < backwardPath.length(); ++i) {
      value.append(backwardPath.at(i).x());
    }
    value.append(backwardEndpoint.x());
    value.append(forwardEndpoint.y());
    for (qint64 i = forwardPath.length() - 1; i >= 0; --i) {
      value.append(forwardPath.at(i).y());
    }
    value.append(result.y());
    for (qint64 i = 0; i < backwardPath.length(); ++i) {
      value.append(backwardPath.at(i).y());
    }
    value.append(backwardEndpoint.y());
    value.append(forwardEndpoint.z());
    for (qint64 i = forwardPath.length() - 1; i >= 0; --i) {
      value.append(forwardPath.at(i).z());
    }
    value.append(result.z());
    for (qint64 i = 0; i < backwardPath.length(); ++i) {
      value.append(backwardPath.at(i).z());
    }
    value.append(backwardEndpoint.z());

  } else {
    value.append(false);
    // for debugging
    value.append(result.x());
    value.append(result.y());
    value.append(result.z());
  }

  return value;
}

QList<QVariant> QTAIMLocateElectronDensitySink(QList<QVariant> input)
{
  qint64 counter = 0;
  const QString fileName = input.at(counter).toString();
  counter++;
  //    const qint64 nucleus=input.at(counter).toInt(); counter++
  qreal x0 = input.at(counter).toReal();
  counter++;
  qreal y0 = input.at(counter).toReal();
  counter++;
  qreal z0 = input.at(counter).toReal();
  counter++;

  const QVector3D x0y0z0(x0, y0, z0);

  QTAIMWavefunction wfn;
  wfn.loadFromBinaryFile(fileName);

  QTAIMWavefunctionEvaluator eval(wfn);

  bool correctSignature;
  QVector3D result;

  Matrix<qreal, 3, 1> xyz;
  xyz << x0, y0, z0;
  if (eval.electronDensity(xyz) < 1.e-1) {
    correctSignature = false;
  } else {
    //      QTAIMODEIntegrator
    //      ode(eval,QTAIMODEIntegrator::CMBPMinusThreeGradientInElectronDensityLaplacian);
    QTAIMLSODAIntegrator ode(
      eval,
      QTAIMLSODAIntegrator::CMBPMinusThreeGradientInElectronDensityLaplacian);
    result = ode.integrate(x0y0z0);

    Matrix<qreal, 3, 1> xyz_;
    xyz_ << result.x(), result.y(), result.z();

    if (eval.electronDensity(xyz_) > 1.e-1 &&
        eval.gradientOfElectronDensityLaplacian(xyz_).norm() < 1.e-3) {
      if (QTAIMMathUtilities::signatureOfASymmetricThreeByThreeMatrix(
            eval.hessianOfElectronDensityLaplacian(xyz_)) == -3) {
        correctSignature = true;
      } else {
        correctSignature = false;
      }
    } else {
      correctSignature = false;
    }
  }

  QList<QVariant> value;
  if (correctSignature) {
    value.append(correctSignature);
    value.append(result.x());
    value.append(result.y());
    value.append(result.z());
  } else {
    value.append(false);
  }

  return value;
}

QList<QVariant> QTAIMLocateElectronDensitySource(QList<QVariant> input)
{
  qint64 counter = 0;
  const QString fileName = input.at(counter).toString();
  counter++;
  //    const qint64 nucleus=input.at(counter).toInt(); counter++
  qreal x0 = input.at(counter).toReal();
  counter++;
  qreal y0 = input.at(counter).toReal();
  counter++;
  qreal z0 = input.at(counter).toReal();
  counter++;

  const QVector3D x0y0z0(x0, y0, z0);

  QTAIMWavefunction wfn;
  wfn.loadFromBinaryFile(fileName);

  QTAIMWavefunctionEvaluator eval(wfn);

  bool correctSignature;
  QVector3D result;

  Matrix<qreal, 3, 1> xyz;
  xyz << x0, y0, z0;
  if (eval.electronDensity(xyz) < 1.e-1) {
    correctSignature = false;
  } else {
    //      QTAIMODEIntegrator
    //      ode(eval,QTAIMODEIntegrator::CMBPPlusThreeGradientInElectronDensityLaplacian);
    QTAIMLSODAIntegrator ode(
      eval,
      QTAIMLSODAIntegrator::CMBPPlusThreeGradientInElectronDensityLaplacian);
    result = ode.integrate(x0y0z0);

    Matrix<qreal, 3, 1> xyz_;
    xyz_ << result.x(), result.y(), result.z();

    if (eval.electronDensity(xyz_) > 1.e-1 &&
        eval.gradientOfElectronDensityLaplacian(xyz_).norm() < 1.e-3) {
      if (QTAIMMathUtilities::signatureOfASymmetricThreeByThreeMatrix(
            eval.hessianOfElectronDensityLaplacian(xyz_)) == 3) {
        correctSignature = true;
      } else {
        correctSignature = false;
      }
    } else {
      correctSignature = false;
    }
  }

  QList<QVariant> value;
  if (correctSignature) {
    value.append(correctSignature);
    value.append(result.x());
    value.append(result.y());
    value.append(result.z());
  } else {
    value.append(false);
  }

  return value;
}

QTAIMCriticalPointLocator::QTAIMCriticalPointLocator(QTAIMWavefunction& wfn)
{
  m_wfn = &wfn;

  m_nuclearCriticalPoints.empty();
  m_bondCriticalPoints.empty();
  m_ringCriticalPoints.empty();
  m_cageCriticalPoints.empty();

  m_laplacianAtBondCriticalPoints.empty();
  m_ellipticityAtBondCriticalPoints.empty();

  m_bondPaths.empty();
  m_bondedAtoms.empty();

  m_electronDensitySources.empty();
  m_electronDensitySinks.empty();
}

void QTAIMCriticalPointLocator::locateNuclearCriticalPoints()
{

  QString tempFileName = QTAIMCriticalPointLocator::temporaryFileName();

  QList<QList<QVariant>> inputList;

  const qint64 numberOfNuclei = m_wfn->numberOfNuclei();

  for (qint64 n = 0; n < numberOfNuclei; ++n) {
    QList<QVariant> input;
    input.append(tempFileName);
    input.append(n);
    input.append(m_wfn->xNuclearCoordinate(n));
    input.append(m_wfn->yNuclearCoordinate(n));
    input.append(m_wfn->zNuclearCoordinate(n));

    inputList.append(input);
  }

  m_wfn->saveToBinaryFile(tempFileName);

  QProgressDialog dialog;
  dialog.setWindowTitle("QTAIM");
  dialog.setLabelText(QString("Nuclear Critical Points Search"));

  QFutureWatcher<void> futureWatcher;
  QObject::connect(&futureWatcher, SIGNAL(finished()), &dialog, SLOT(reset()));
  QObject::connect(&dialog, SIGNAL(canceled()), &futureWatcher, SLOT(cancel()));
  QObject::connect(&futureWatcher, SIGNAL(progressRangeChanged(int, int)),
                   &dialog, SLOT(setRange(int, int)));
  QObject::connect(&futureWatcher, SIGNAL(progressValueChanged(int)), &dialog,
                   SLOT(setValue(int)));

  QFuture<QList<QVariant>> future =
    QtConcurrent::mapped(inputList, QTAIMLocateNuclearCriticalPoint);
  futureWatcher.setFuture(future);
  dialog.exec();
  futureWatcher.waitForFinished();

  QList<QList<QVariant>> results;
  if (futureWatcher.future().isCanceled()) {
    results.clear();
  } else {
    results = future.results();
  }

  QFile file;
  file.remove(tempFileName);

  for (qint64 n = 0; n < results.length(); ++n) {

    bool correctSignature = results.at(n).at(0).toBool();

    if (correctSignature) {

      QVector3D result(results.at(n).at(1).toReal(),
                       results.at(n).at(2).toReal(),
                       results.at(n).at(3).toReal());

      m_nuclearCriticalPoints.append(result);
    }
  }
}

void QTAIMCriticalPointLocator::locateBondCriticalPoints()
{

  if (m_nuclearCriticalPoints.length() < 1) {
    return;
  }

  const qint64 numberOfNuclei = m_wfn->numberOfNuclei();

  if (numberOfNuclei < 2) {
    return;
  }

  QString tempFileName = QTAIMCriticalPointLocator::temporaryFileName();

  QString nuclearCriticalPointsFileName =
    QTAIMCriticalPointLocator::temporaryFileName();
  QFile nuclearCriticalPointsFile(nuclearCriticalPointsFileName);
  nuclearCriticalPointsFile.open(QIODevice::WriteOnly);
  QDataStream nuclearCriticalPointsOut(&nuclearCriticalPointsFile);
  nuclearCriticalPointsOut << m_nuclearCriticalPoints;
  nuclearCriticalPointsFile.close();

  QList<QList<QVariant>> inputList;

  for (qint64 M = 0; M < numberOfNuclei - 1; ++M) {
    for (qint64 N = M + 1; N < numberOfNuclei; ++N) {

      const qreal distanceCutoff = 8.0;

      Matrix<qreal, 3, 1> a;
      Matrix<qreal, 3, 1> b;

      a << m_wfn->xNuclearCoordinate(M), m_wfn->yNuclearCoordinate(M),
        m_wfn->zNuclearCoordinate(M);
      b << m_wfn->xNuclearCoordinate(N), m_wfn->yNuclearCoordinate(N),
        m_wfn->zNuclearCoordinate(N);

      if (QTAIMMathUtilities::distance(a, b) < distanceCutoff) {
        QVector3D x0y0z0(
          (m_wfn->xNuclearCoordinate(M) + m_wfn->xNuclearCoordinate(N)) / 2.0,
          (m_wfn->yNuclearCoordinate(M) + m_wfn->yNuclearCoordinate(N)) / 2.0,
          (m_wfn->zNuclearCoordinate(M) + m_wfn->zNuclearCoordinate(N)) / 2.0);

        QList<QVariant> input;
        input.append(tempFileName);
        input.append(nuclearCriticalPointsFileName);
        input.append(M);
        input.append(N);
        input.append(x0y0z0.x());
        input.append(x0y0z0.y());
        input.append(x0y0z0.z());

        inputList.append(input);
      }
    } // end N
  }   // end M

  m_wfn->saveToBinaryFile(tempFileName);

  QProgressDialog dialog;
  dialog.setWindowTitle("QTAIM");
  dialog.setLabelText(QString("Bond Critical Points Search"));

  QFutureWatcher<void> futureWatcher;
  QObject::connect(&futureWatcher, SIGNAL(finished()), &dialog, SLOT(reset()));
  QObject::connect(&dialog, SIGNAL(canceled()), &futureWatcher, SLOT(cancel()));
  QObject::connect(&futureWatcher, SIGNAL(progressRangeChanged(int, int)),
                   &dialog, SLOT(setRange(int, int)));
  QObject::connect(&futureWatcher, SIGNAL(progressValueChanged(int)), &dialog,
                   SLOT(setValue(int)));

  QFuture<QList<QVariant>> future =
    QtConcurrent::mapped(inputList, QTAIMLocateBondCriticalPoint);
  ;
  futureWatcher.setFuture(future);
  dialog.exec();
  futureWatcher.waitForFinished();

  QList<QList<QVariant>> results;
  if (futureWatcher.future().isCanceled()) {
    results.clear();
  } else {
    results = future.results();
  }

  QFile file;
  file.remove(tempFileName);
  file.remove(nuclearCriticalPointsFileName);

  for (qint64 i = 0; i < results.length(); ++i) {
    QList<QVariant> thisCriticalPoint = results.at(i);

    bool success = thisCriticalPoint.at(0).toBool();

    if (success) {
      QPair<qint64, qint64> bondedAtoms_;
      bondedAtoms_.first = thisCriticalPoint.at(1).toInt();
      bondedAtoms_.second = thisCriticalPoint.at(2).toInt();
      m_bondedAtoms.append(bondedAtoms_);

      QVector3D coordinates(thisCriticalPoint.at(3).toReal(),
                            thisCriticalPoint.at(4).toReal(),
                            thisCriticalPoint.at(5).toReal());

      m_bondCriticalPoints.append(coordinates);

      m_laplacianAtBondCriticalPoints.append(thisCriticalPoint.at(6).toReal());
      m_ellipticityAtBondCriticalPoints.append(
        thisCriticalPoint.at(7).toReal());
      qint64 pathLength = thisCriticalPoint.at(8).toInt();

      QList<QVector3D> bondPath;
      for (qint64 j = 0; j < pathLength; ++j) {
        QVector3D pathPoint(
          thisCriticalPoint.at(9 + j).toReal(),
          thisCriticalPoint.at(9 + j + pathLength).toReal(),
          thisCriticalPoint.at(9 + j + 2 * pathLength).toReal());

        bondPath.append(pathPoint);
      }

      m_bondPaths.append(bondPath);
    }
  }
}

void QTAIMCriticalPointLocator::locateElectronDensitySources()
{

  QString tempFileName = QTAIMCriticalPointLocator::temporaryFileName();

  QList<QList<QVariant>> inputList;

  qreal xmin, ymin, zmin;
  qreal xmax, ymax, zmax;
  qreal xstep, ystep, zstep;

  // TODO: if only we were using Eigen data structures...
  QList<qreal> xNuclearCoordinates;
  QList<qreal> yNuclearCoordinates;
  QList<qreal> zNuclearCoordinates;

  for (qint64 i = 0; i < m_wfn->numberOfNuclei(); ++i) {
    xNuclearCoordinates.append(m_wfn->xNuclearCoordinate(i));
    yNuclearCoordinates.append(m_wfn->yNuclearCoordinate(i));
    zNuclearCoordinates.append(m_wfn->zNuclearCoordinate(i));
  }

  xmin = xNuclearCoordinates.first();
  xmax = xNuclearCoordinates.first();
  for (qint64 i = 1; i < m_wfn->numberOfNuclei(); ++i) {
    if (xNuclearCoordinates.at(i) < xmin) {
      xmin = xNuclearCoordinates.at(i);
    }
    if (xNuclearCoordinates.at(i) > xmax) {
      xmax = xNuclearCoordinates.at(i);
    }
  }

  ymin = yNuclearCoordinates.first();
  ymax = yNuclearCoordinates.first();
  for (qint64 i = 1; i < yNuclearCoordinates.count(); ++i) {
    if (yNuclearCoordinates.at(i) < ymin) {
      ymin = yNuclearCoordinates.at(i);
    }
    if (yNuclearCoordinates.at(i) > ymax) {
      ymax = yNuclearCoordinates.at(i);
    }
  }

  zmin = zNuclearCoordinates.first();
  zmax = zNuclearCoordinates.first();
  for (qint64 i = 1; i < zNuclearCoordinates.count(); ++i) {
    if (zNuclearCoordinates.at(i) < zmin) {
      zmin = zNuclearCoordinates.at(i);
    }
    if (zNuclearCoordinates.at(i) > zmax) {
      zmax = zNuclearCoordinates.at(i);
    }
  }

  xmin = -2.0 + xmin;
  ymin = -2.0 + ymin;
  zmin = -2.0 + zmin;

  xmax = 2.0 + xmax;
  ymax = 2.0 + ymax;
  zmax = 2.0 + zmax;

  xstep = ystep = zstep = 0.5;

  for (qreal x = xmin; x < xmax + xstep; x = x + xstep) {
    for (qreal y = ymin; y < ymax + ystep; y = y + ystep) {
      for (qreal z = zmin; z < zmax + zstep; z = z + zstep) {
        QList<QVariant> input;
        input.append(tempFileName);
        //          input.append( n );
        input.append(x);
        input.append(y);
        input.append(z);

        inputList.append(input);
      }
    }
  }

  m_wfn->saveToBinaryFile(tempFileName);

  QProgressDialog dialog;
  dialog.setWindowTitle("QTAIM");
  dialog.setLabelText(QString("Electron Density Sources Search"));

  QFutureWatcher<void> futureWatcher;
  QObject::connect(&futureWatcher, SIGNAL(finished()), &dialog, SLOT(reset()));
  QObject::connect(&dialog, SIGNAL(canceled()), &futureWatcher, SLOT(cancel()));
  QObject::connect(&futureWatcher, SIGNAL(progressRangeChanged(int, int)),
                   &dialog, SLOT(setRange(int, int)));
  QObject::connect(&futureWatcher, SIGNAL(progressValueChanged(int)), &dialog,
                   SLOT(setValue(int)));

  QFuture<QList<QVariant>> future =
    QtConcurrent::mapped(inputList, QTAIMLocateElectronDensitySource);
  futureWatcher.setFuture(future);
  dialog.exec();
  futureWatcher.waitForFinished();

  QList<QList<QVariant>> results;
  if (futureWatcher.future().isCanceled()) {
    results.clear();
  } else {
    results = future.results();
  }

  QFile file;
  file.remove(tempFileName);

  for (qint64 n = 0; n < results.length(); ++n) {

    qint64 counter = 0;
    bool correctSignature = results.at(n).at(counter).toBool();
    counter++;

    if (correctSignature) {
      qreal x = results.at(n).at(counter).toReal();
      counter++;
      qreal y = results.at(n).at(counter).toReal();
      counter++;
      qreal z = results.at(n).at(counter).toReal();
      counter++;

      if ((xmin < x && x < xmax) && (ymin < y && y < ymax) &&
          (zmin < z && z < zmax)) {
        QVector3D result(x, y, z);

        qreal smallestDistance = HUGE_REAL_NUMBER;

        for (qint64 i = 0; i < m_electronDensitySources.length(); ++i) {

          Matrix<qreal, 3, 1> a(x, y, z);
          Matrix<qreal, 3, 1> b(m_electronDensitySources.at(i).x(),
                                m_electronDensitySources.at(i).y(),
                                m_electronDensitySources.at(i).z());

          qreal distance = QTAIMMathUtilities::distance(a, b);

          if (distance < smallestDistance) {
            smallestDistance = distance;
          }
        }

        if (smallestDistance > 1.e-2) {
          m_electronDensitySources.append(result);
        }
      }
    }
  }
  //    qDebug() << "SOURCES" << m_electronDensitySources;
}

void QTAIMCriticalPointLocator::locateElectronDensitySinks()
{

  QString tempFileName = QTAIMCriticalPointLocator::temporaryFileName();

  QList<QList<QVariant>> inputList;

  qreal xmin, ymin, zmin;
  qreal xmax, ymax, zmax;
  qreal xstep, ystep, zstep;

  // TODO: if only we were using Eigen data structures...
  QList<qreal> xNuclearCoordinates;
  QList<qreal> yNuclearCoordinates;
  QList<qreal> zNuclearCoordinates;

  for (qint64 i = 0; i < m_wfn->numberOfNuclei(); ++i) {
    xNuclearCoordinates.append(m_wfn->xNuclearCoordinate(i));
    yNuclearCoordinates.append(m_wfn->yNuclearCoordinate(i));
    zNuclearCoordinates.append(m_wfn->zNuclearCoordinate(i));
  }

  xmin = xNuclearCoordinates.first();
  xmax = xNuclearCoordinates.first();
  for (qint64 i = 1; i < m_wfn->numberOfNuclei(); ++i) {
    if (xNuclearCoordinates.at(i) < xmin) {
      xmin = xNuclearCoordinates.at(i);
    }
    if (xNuclearCoordinates.at(i) > xmax) {
      xmax = xNuclearCoordinates.at(i);
    }
  }

  ymin = yNuclearCoordinates.first();
  ymax = yNuclearCoordinates.first();
  for (qint64 i = 1; i < yNuclearCoordinates.count(); ++i) {
    if (yNuclearCoordinates.at(i) < ymin) {
      ymin = yNuclearCoordinates.at(i);
    }
    if (yNuclearCoordinates.at(i) > ymax) {
      ymax = yNuclearCoordinates.at(i);
    }
  }

  zmin = zNuclearCoordinates.first();
  zmax = zNuclearCoordinates.first();
  for (qint64 i = 1; i < zNuclearCoordinates.count(); ++i) {
    if (zNuclearCoordinates.at(i) < zmin) {
      zmin = zNuclearCoordinates.at(i);
    }
    if (zNuclearCoordinates.at(i) > zmax) {
      zmax = zNuclearCoordinates.at(i);
    }
  }

  xmin = -2.0 + xmin;
  ymin = -2.0 + ymin;
  zmin = -2.0 + zmin;

  xmax = 2.0 + xmax;
  ymax = 2.0 + ymax;
  zmax = 2.0 + zmax;

  xstep = ystep = zstep = 0.5;

  for (qreal x = xmin; x < xmax + xstep; x = x + xstep) {
    for (qreal y = ymin; y < ymax + ystep; y = y + ystep) {
      for (qreal z = zmin; z < zmax + zstep; z = z + zstep) {
        QList<QVariant> input;
        input.append(tempFileName);
        //          input.append( n );
        input.append(x);
        input.append(y);
        input.append(z);

        inputList.append(input);
      }
    }
  }

  m_wfn->saveToBinaryFile(tempFileName);

  QProgressDialog dialog;
  dialog.setWindowTitle("QTAIM");
  dialog.setLabelText(QString("Electron Density Sinks Search"));

  QFutureWatcher<void> futureWatcher;
  QObject::connect(&futureWatcher, SIGNAL(finished()), &dialog, SLOT(reset()));
  QObject::connect(&dialog, SIGNAL(canceled()), &futureWatcher, SLOT(cancel()));
  QObject::connect(&futureWatcher, SIGNAL(progressRangeChanged(int, int)),
                   &dialog, SLOT(setRange(int, int)));
  QObject::connect(&futureWatcher, SIGNAL(progressValueChanged(int)), &dialog,
                   SLOT(setValue(int)));

  QFuture<QList<QVariant>> future =
    QtConcurrent::mapped(inputList, QTAIMLocateElectronDensitySink);
  futureWatcher.setFuture(future);
  dialog.exec();
  futureWatcher.waitForFinished();

  QList<QList<QVariant>> results;
  if (futureWatcher.future().isCanceled()) {
    results.clear();
  } else {
    results = future.results();
  }

  QFile file;
  file.remove(tempFileName);

  for (qint64 n = 0; n < results.length(); ++n) {

    qint64 counter = 0;
    bool correctSignature = results.at(n).at(counter).toBool();
    counter++;

    if (correctSignature) {
      qreal x = results.at(n).at(counter).toReal();
      counter++;
      qreal y = results.at(n).at(counter).toReal();
      counter++;
      qreal z = results.at(n).at(counter).toReal();
      counter++;

      if ((xmin < x && x < xmax) && (ymin < y && y < ymax) &&
          (zmin < z && z < zmax)) {
        QVector3D result(x, y, z);

        qreal smallestDistance = HUGE_REAL_NUMBER;

        for (qint64 i = 0; i < m_electronDensitySinks.length(); ++i) {

          Matrix<qreal, 3, 1> a(x, y, z);
          Matrix<qreal, 3, 1> b(m_electronDensitySinks.at(i).x(),
                                m_electronDensitySinks.at(i).y(),
                                m_electronDensitySinks.at(i).z());

          qreal distance = QTAIMMathUtilities::distance(a, b);

          if (distance < smallestDistance) {
            smallestDistance = distance;
          }
        }

        if (smallestDistance > 1.e-2) {
          m_electronDensitySinks.append(result);
        }
      }
    }
  }
  //    qDebug() << "SINKS" << m_electronDensitySinks;
}

QString QTAIMCriticalPointLocator::temporaryFileName()
{
  QTemporaryFile temporaryFile;
  temporaryFile.open();
  QString tempFileName = temporaryFile.fileName();
  temporaryFile.close();
  temporaryFile.remove();

  // wait for temporary file to be deleted
  QDir dir;
  do {
    // Nothing
  } while (dir.exists(tempFileName));

  return tempFileName;
}

} // namespace QtPlugins
} // namespace Avogadro


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE HSMM
#include <boost/test/unit_test.hpp>
#include <HSMM.hpp>
#include <memory>

#define EPSILON 1e-6

using namespace arma;
using namespace hsmm;
using namespace std;

mat transition = {{0.0, 0.1, 0.4, 0.5},
                  {0.3, 0.0, 0.6, 0.1},
                  {0.2, 0.2, 0.0, 0.6},
                  {0.4, 0.4, 0.2, 0.0}};

mat duration = {{0.0, 0.1, 0.4, 0.5},
                {0.3, 0.0, 0.6, 0.1},
                {0.2, 0.2, 0.0, 0.6},
                {0.4, 0.4, 0.2, 0.0}};

int min_duration = 4;

vec pi = {0.25, 0.25, 0.25, 0.25};

int ndurations = duration.n_cols;

int nstates = duration.n_rows;

// Emission parameters.
vec means = {0, 5, 10, 15};
vec std_devs =  {0.5, 1.0, 0.1, 2.0};

BOOST_AUTO_TEST_CASE( OnlineHSMM_test ) {
    shared_ptr<AbstractEmissionOnlineSetting> ptr_emission(new DummyGaussianEmission(
            means, std_devs));
    OnlineHSMM dhsmm(ptr_emission, transition, pi, duration, min_duration);
    ivec hiddenStates, hiddenDurations;
    int nSampledSegments = 10;
    field<mat> samples = dhsmm.sampleSegments(nSampledSegments, hiddenStates,
            hiddenDurations);
    int nobs = samples.n_elem;
    for(int i = 0; i < nobs; i++)
        dhsmm.addNewObservation(samples(i));
    int nsamples = 20;
    field<mat> next = dhsmm.sampleNextObservations(nsamples);
    assert(next.n_elem == nsamples);
    cout << "Hidden states " << endl << hiddenStates << endl;
    cout << "Hidden duration " << endl << hiddenDurations << endl;
}

BOOST_AUTO_TEST_CASE( fitting_from_fully_labeled_sequences ) {
    shared_ptr<AbstractEmissionOnlineSetting> ptr_emission(new DummyGaussianEmission(
            means, std_devs));
    OnlineHSMM dhsmm(ptr_emission, transition, pi, duration, min_duration);
    ivec hiddenStates, hiddenDurations;
    int nSampledSegments = 10000;
    field<mat> samples = dhsmm.sampleSegments(nSampledSegments, hiddenStates,
            hiddenDurations);
    vector<int> sequence_states;
    for(int i = 0; i < hiddenStates.n_elem; i++)
        for(int j = 0; j < hiddenDurations(i); j++)
            sequence_states.push_back(hiddenStates(i));\
    ivec hs_seq = conv_to<ivec>::from(sequence_states);
    field<ivec> seqs = {hs_seq};
    dhsmm.setPiFromLabels(seqs);
    dhsmm.setTransitionFromLabels(seqs);
    dhsmm.setDurationFromLabels(seqs);

    // TODO: compare with the actual parameters.
    cout << "Pi:" << endl << dhsmm.pi_ << endl;
    cout << "Transition:" << endl << dhsmm.transition_ << endl;
    cout << "Duration:" << endl << dhsmm.duration_ << endl;
}


#include <cmath>
#include <exception>
#include <iostream>
#include <stdexcept>
#include <string>
#include <vector>

#include <boost/lexical_cast.hpp>
#include <boost/program_options.hpp>

#include <frovedis.hpp>
#include <frovedis/ml/tree/tree_model.hpp>
#include <frovedis/matrix/rowmajor_matrix.hpp>

namespace po = boost::program_options;
using namespace frovedis;

// default values for program options
constexpr size_t ARGNAME_LENGTH = 24 - (2 + 1);
#define POSARG_CAPTION "positional arguments"
#define OPTARG_CAPTION "optional arguments"

template <typename T>
void show_classification_error_rate(
  zipped_dvectors<T, T>& zipped, const T num_records
) {
  std::cout << "Error rate: " << zipped.map(
    +[] (const T predict, const T answer) -> T {
      return static_cast<T>(predict != answer);
    }
  ).reduce(add<T>) / num_records * 100 << "%" << std::endl;
}

template <typename T>
void show_regression_errors(
  zipped_dvectors<T, T>& zipped, const T num_records
) {
  const T mae = zipped.map(
    +[] (const T predict, const T answer) -> T {
      return std::abs(predict - answer);
    }
  ).reduce(add<T>) / num_records;

  const T mse = zipped.map(
    +[] (const T predict, const T answer) -> T {
      const T error = predict - answer;
      return error * error;
    }
  ).reduce(add<T>) / num_records;

  std::cout << "MAE:  " << mae << std::endl;
  std::cout << "MSE:  " << mse << std::endl;
  std::cout << "RMSE: " << std::sqrt(mse) << std::endl;
}

template <typename T>
void do_validate(
  const std::vector<T>& predicts, dvector<T>& answers,
  const tree::algorithm algo
) {
  const T num_records = static_cast<T>(predicts.size());
  auto dpredicts = make_dvector_scatter(predicts);
  auto zipped = zip(dpredicts, answers);

  switch (algo) {
  case tree::algorithm::Classification:
    show_classification_error_rate(zipped, num_records);
    return;
  case tree::algorithm::Regression:
    show_regression_errors(zipped, num_records);
    return;
  default:
    throw std::logic_error("invalid tree algorithm");
  }
}

template <typename T>
void do_predict(
  const std::string& input,
  const std::string& saved_model,
  const std::string& output,
  const std::string& corrected,
  const bool binary_mode
) {
  time_spent timer(DEBUG);
  decision_tree_model<T> model;

  if (binary_mode) {
    timer.reset();
    auto dataset = make_rowmajor_matrix_local_loadbinary<T>(input);
    timer.show("load matrix: ");
    model.loadbinary(saved_model);
    timer.show("load model:  ");
    auto results = model.predict(dataset);
    timer.show("predict:     ");
    make_dvector_scatter(results).savebinary(output);
    timer.show("save result: ");

    if (!corrected.empty()) {
      timer.reset();
      auto answers = make_dvector_loadbinary<T>(corrected);
      timer.show("load answer: ");
      do_validate(results, answers, model.get_algo());
      timer.show("validate:    ");
    }
  } else {
    timer.reset();
    auto dataset = make_rowmajor_matrix_local_load<T>(input);
    timer.show("load matrix: ");
    model.load(saved_model);
    timer.show("load model:  ");
    auto results = model.predict(dataset);
    timer.show("predict:     ");
    make_dvector_scatter(results).saveline(output);
    timer.show("save result: ");

    if (!corrected.empty()) {
      timer.reset();
      auto answers = make_dvector_loadline(corrected).map(
        +[] (const std::string& line) -> T {
          return boost::lexical_cast<T>(line);
        }
      );
      timer.show("load answer: ");
      do_validate(results, answers, model.get_algo());
      timer.show("validate:    ");
    }
  }
}

template <typename T>
void do_predict(const po::variables_map& argmap) {
  do_predict<T>(
    argmap["input"].as<std::string>(),
    argmap["model"].as<std::string>(),
    argmap["output"].as<std::string>(),
    argmap["corrected"].as<std::string>(),
    argmap.count("binary")
  );
}

void abort(const int code = 0) {
  std::vector<std::string> dummy;
  send_bcast_rpcreq(rpc_type::finalize_type, 0, 0, "", dummy);
  MPI_Finalize();
  exit(code);
}

po::variables_map parse(int argc, char** argv) {
  // a description of positional arguments
  po::options_description posarg_desc(POSARG_CAPTION);
  posarg_desc.add_options()(
    "input", po::value<std::string>(), "an input matrix"
  )(
    "model", po::value<std::string>(), "an input model"
  )(
    "output", po::value<std::string>(), "an output predict result"
  );
  po::positional_options_description posarg_settings;
  posarg_settings.add("input", 1).add("model", 1).add("output", 1);

  // a description of optional arguments
  po::options_description optarg_desc(OPTARG_CAPTION);
  optarg_desc.add_options()(
    "help,h", "show this help message and exit"
  )(
    "corrected", po::value<std::string>()->default_value(""),
    "corrected labels for validation"
  )(
    "binary", "use binary input/output"
  )(
    "double", "use 64-bit float type (default: 32-bit float)"
  )(
    "verbose", "set log-level to DEBUG"
  )(
    "trace", "set log-level to TRACE"
  );

  // merge descriptions of all arguments
  po::options_description arg_desc;
  arg_desc.add(posarg_desc).add(optarg_desc);

  po::variables_map argmap;
  try {
    auto parsed = po::command_line_parser(argc, argv)
      .options(arg_desc)
      .positional(posarg_settings)
//    .allow_unregistered()
      .run();

    // make sure there are no `optional-style` positional arguments
    for (const auto parsed_opt: parsed.options) {
      if (parsed_opt.position_key < 0) {
        for (const auto opt: posarg_desc.options()) {
          if (parsed_opt.string_key == opt->long_name()) {
            throw po::unknown_option(parsed_opt.string_key);
          }
        }
      }
    }

    po::store(parsed, argmap);
    po::notify(argmap);
  } catch (const po::error_with_option_name& e) {
    std::cerr << e.what() << std::endl;
    abort(1);
  }

  // help message
  if (argmap.count("help")) {
    size_t max_length = ARGNAME_LENGTH;
    for (const auto opt: posarg_desc.options()) {
      const size_t temp = opt->long_name().length();
      if (max_length < temp) { max_length = temp; }
    }
    for (const auto opt: optarg_desc.options()) {
      const size_t namelen = opt->format_name().length();
      const size_t paramlen = opt->format_parameter().length();
      const size_t temp = namelen + paramlen + !!(paramlen > 0);
      if (max_length < temp) { max_length = temp; }
    }

    std::cerr << POSARG_CAPTION << ":" << std::endl;
    for (const auto opt: posarg_desc.options()) {
      const auto name = opt->long_name();
      std::cerr <<
        "  " << name <<
        std::string(max_length - name.length(), ' ') << " " <<
        opt->description() <<
      std::endl;
    }
    std::cerr << optarg_desc;

    abort(0);
  }

  // check required arguments
  for (const auto opt: posarg_desc.options()) {
    const std::string& name = opt->long_name();
    if (!argmap.count(name)) {
      std::cerr << "missing argument '" << name << "'" << std::endl;
      abort(1);
    }
  }

  return argmap;
}

int main(int argc, char** argv) {
  use_frovedis use(argc, argv);

  const auto argmap = parse(argc, argv);
  if (argmap.count("verbose")) { set_loglevel(DEBUG); }
  if (argmap.count("trace")) { set_loglevel(TRACE); }

  try {
    if (argmap.count("double")) {
      do_predict<double>(argmap);
    } else {
      do_predict<float>(argmap);
    }
  } catch (const std::exception& e) {
    std::cerr << e.what() << std::endl;
    abort(1);
  }

  return 0;
}


// Copyright (c) 2021 The Bitcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <minisketch.h>
#include <node/minisketchwrapper.h>
#include <random.h>
#include <test/util/setup_common.h>

#include <boost/test/unit_test.hpp>

#include <utility>

using node::MakeMinisketch32;

BOOST_AUTO_TEST_SUITE(minisketch_tests)

BOOST_AUTO_TEST_CASE(minisketch_test)
{
    for (int i = 0; i < 100; ++i) {
        uint32_t errors = 0 + InsecureRandRange(11);
        uint32_t start_a = 1 + InsecureRandRange(1000000000);
        uint32_t a_not_b = InsecureRandRange(errors + 1);
        uint32_t b_not_a = errors - a_not_b;
        uint32_t both = InsecureRandRange(10000);
        uint32_t end_a = start_a + a_not_b + both;
        uint32_t start_b = start_a + a_not_b;
        uint32_t end_b = start_b + both + b_not_a;

        Minisketch sketch_a = MakeMinisketch32(10);
        for (uint32_t a = start_a; a < end_a; ++a) sketch_a.Add(a);
        Minisketch sketch_b = MakeMinisketch32(10);
        for (uint32_t b = start_b; b < end_b; ++b) sketch_b.Add(b);

        Minisketch sketch_ar = MakeMinisketch32(10);
        Minisketch sketch_br = MakeMinisketch32(10);
        sketch_ar.Deserialize(sketch_a.Serialize());
        sketch_br.Deserialize(sketch_b.Serialize());

        Minisketch sketch_c = std::move(sketch_ar);
        sketch_c.Merge(sketch_br);
        auto dec = sketch_c.Decode(errors);
        BOOST_CHECK(dec.has_value());
        auto sols = std::move(*dec);
        std::sort(sols.begin(), sols.end());
        for (uint32_t i = 0; i < a_not_b; ++i) BOOST_CHECK_EQUAL(sols[i], start_a + i);
        for (uint32_t i = 0; i < b_not_a; ++i) BOOST_CHECK_EQUAL(sols[i + a_not_b], start_b + both + i);
    }
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef MI_MATH_HPP
#define MI_MATH_HPP 1
#include <Eigen/Dense>
namespace mi
{
        typedef Eigen::Vector3d Vector3d;
        typedef Eigen::Vector3f Vector3f;
        typedef Eigen::Matrix< short , 3 , 1> Vector3s;

        typedef Eigen::Vector3d Point3d;
        typedef Eigen::Vector3f Point3f;
        typedef Eigen::Vector3i Point3i;

        typedef Eigen::Vector3f Color3f;
};
#endif// MI_MATH_HPP


#ifndef __METROPOLIS_HPP__
#define __METROPOLIS_HPP__

#include <vector>
#include <array>
#include <random>
#include <functional>
#include <limits>
#include <armadillo>
#include <exception>
#include <initializer_list>
#include "pauth_types.hpp"
#include "molecular.hpp"
#include "potentials.hpp"
#include "acceptance.hpp"
#include "boundary.hpp"
#include "distance.hpp"
#include "trial.hpp"
#include "seed.hpp"

/* TODO: make random number generator more general so that user can specify
 *       which random number generator they'd prefer to use
 */

namespace pauth {

static const double _default_kB = 1.0;

static metric _default_metric = periodic_euclidean;
static bc _default_bc = periodic_bc;
static acc _default_acc = metropolis_acc;

class metropolis {
public:
  static metric DEFAULT_METRIC;
  static bc DEFAULT_BC;
  static acc DEFAULT_ACC;

  /*! \brief Default constructor
   */
  metropolis() {}

  /*! \brief Constructor for a Markov chain Monte Carlo simulation
   *
   * \param     id          Molecular id of simulation molecules
   * \param     N           Number of molecules
   * \param     D           Number of dimensions
   * \param     L           Simulation box edge length
   * \param     tmg         Trial move generator
   * \param     pot         Molecular potential
   * \param     T           Simulation temperature
   * \param     kB          Boltzmann's constant
   * \param     m           Metric for measuring molecule-molecule distances
   * \param     bc          Boundary conditions
   * \param     acceptance  Determines whether a move is accepted for rejected
   * \param     sg          Function for generating a seed
   * \param     init_zeros  If true, initializes all molecules to position zero
   * \return                Markov chain Monte Carlo simulation object
   */
  metropolis(const molecular_id id, const size_t N, const size_t D,
             const double L, trial_move_generator tmg,
             abstract_potential* pot, const double T,
             const double kB = _default_kB,
             metric m = _default_metric, bc boundary = _default_bc,
             acc acceptance = _default_acc,
             seed_gen sg = _default_seed_gen,
             const bool init_zeros = false);

  /*! \brief Constructor for a Markov chain Monte Carlo simulation
   *
   * \param     fname         Filename of initial molecular positions
   * \param     id            Molecular id of simulation molecules
   * \param     N             Number of molecules
   * \param     D             Number of dimensions
   * \param     L             Simulation box edge length
   * \param     tmg           Trial move generator
   * \param     pot           Molecular potential
   * \param     T             Simulation temperature
   * \param     kB            Boltzmann's constant
   * \param     m             Metric for measuring molecule-molecule distances
   * \param     bc            Boundary conditions
   * \param     acceptance    Determines whether a move is accepted for rejected
   * \param     sg            Function for generating a seed
   * \return                  Markov chain Monte Carlo simulation object
   */
  metropolis(const char *fname, const molecular_id id, const size_t N,
             const size_t D, const double L, trial_move_generator tmg,
             abstract_potential* pot, const double T,
             const double kB = _default_kB,
             metric m = _default_metric, bc boundary = _default_bc,
             acc acceptance = _default_acc,
             seed_gen sg = _default_seed_gen);

  /*! \brief Constructor for a Markov chain Monte Carlo simulation
   *
   * \param     fname         Filename of initial molecular positions
   * \param     N             Number of molecules
   * \param     D             Number of dimensions
   * \param     L             Simulation box edge length
   * \param     tmg           Trial move generator
   * \param     pot           Molecular potential
   * \param     T             Simulation temperature
   * \param     kB            Boltzmann's constant
   * \param     m             Metric for measuring molecule-molecule distances
   * \param     bc            Boundary conditions
   * \param     acceptance    Determines whether a move is accepted for rejected
   * \param     sg            Function for generating a seed
   * \return                  Markov chain Monte Carlo simulation object
   */
  metropolis(const char *fname, const size_t N,
             const size_t D, const double L, trial_move_generator tmg,
             abstract_potential* pot, const double T,
             const double kB = _default_kB,
             metric m = _default_metric, bc boundary = _default_bc,
             acc acceptance = _default_acc,
             seed_gen sg = _default_seed_gen);

  /*! \brief Constructor for a Markov chain Monte Carlo simulation
   *
   * \param     id          Molecular id of simulation molecules
   * \param     N           Number of molecules
   * \param     D           Number of dimensions
   * \param     L           Simulation box edge length
   * \param     tmg         Trial move generator
   * \param     pots        Molecular potentials
   * \param     T           Simulation temperature
   * \param     kB          Boltzmann's constant
   * \param     m           Metric for measuring molecule-molecule distances
   * \param     bc          Boundary conditions
   * \param     acceptance  Determines whether a move is accepted for rejected
   * \param     sg          Function for generating a seed
   * \param     init_zeros  If true, initializes all molecules to position zero
   * \return                Markov chain Monte Carlo simulation object
   */
  metropolis(const molecular_id id, const size_t N, const size_t D,
             const double L, trial_move_generator tmg,
             std::initializer_list<abstract_potential*> pots, const double T,
             const double kB = _default_kB,
             metric m = _default_metric, bc boundary = _default_bc,
             acc acceptance = _default_acc,
             seed_gen sg = _default_seed_gen,
             const bool init_zeros = false);

  /*! \brief Constructor for a Markov chain Monte Carlo simulation
   *
   * \param     fname         Filename of initial molecular positions
   * \param     id            Molecular id of simulation molecules
   * \param     N             Number of molecules
   * \param     D             Number of dimensions
   * \param     L             Simulation box edge length
   * \param     tmg           Trial move generator
   * \param     pots        Molecular potentials
   * \param     T             Simulation temperature
   * \param     kB            Boltzmann's constant
   * \param     m             Metric for measuring molecule-molecule distances
   * \param     bc            Boundary conditions
   * \param     acceptance    Determines whether a move is accepted for rejected
   * \param     sg            Function for generating a seed
   * \return                  Markov chain Monte Carlo simulation object
   */
  metropolis(const char *fname, const molecular_id id, const size_t N,
             const size_t D, const double L, trial_move_generator tmg,
             std::initializer_list<abstract_potential*> pots, const double T,
             const double kB = _default_kB,
             metric m = _default_metric, bc boundary = _default_bc,
             acc acceptance = _default_acc,
             seed_gen sg = _default_seed_gen);

  /*! \brief Constructor for a Markov chain Monte Carlo simulation
   *
   * \param     fname         Filename of initial molecular positions
   * \param     N             Number of molecules
   * \param     D             Number of dimensions
   * \param     L             Simulation box edge length
   * \param     tmg           Trial move generator
   * \param     pots        Molecular potentials
   * \param     T             Simulation temperature
   * \param     kB            Boltzmann's constant
   * \param     m             Metric for measuring molecule-molecule distances
   * \param     bc            Boundary conditions
   * \param     acceptance    Determines whether a move is accepted for rejected
   * \param     sg            Function for generating a seed
   * \return                  Markov chain Monte Carlo simulation object
   */
  metropolis(const char *fname, const size_t N,
             const size_t D, const double L, trial_move_generator tmg,
             std::initializer_list<abstract_potential*> pots, const double T,
             const double kB = _default_kB,
             metric m = _default_metric, bc boundary = _default_bc,
             acc acceptance = _default_acc,
             seed_gen sg = _default_seed_gen);

  /*! \brief Copy constructor
   *
   * \param     sim         Metropolis simulation object
   * \param     sg          Function for generating a seed
   * \return                Copy
   */
  metropolis(const metropolis &sim, seed_gen sg = _default_seed_gen)
    : _molecular_ids(sim._molecular_ids), _positions(sim._positions),
      _edge_lengths(sim._edge_lengths), _V(sim._V), _potentials(sim._potentials),
      _T(sim._T), _kB(sim._kB), _beta(sim._beta), _m(sim._m), _bc(sim._bc),
      _eps_dist(0.0, 1.0), _choice_dist(0, sim._molecular_ids.size()-1),
      _tmg(sim._tmg), _acc(sim._acc),
      _parallel_callbacks(sim._parallel_callbacks),
      _sequential_callbacks(sim._sequential_callbacks), _step(sim._step),
      _dx(sim._dx), _choice(sim._choice), _dU(sim._dU), _U(sim._U), _eps(sim._eps),
      _accepted(sim._accepted) { _rng.seed(sg()); }

  /*! \brief Assignment operator
   *
   * \param     rhs     Metropolis simulation object
   * \return            Self
   */
  metropolis operator=(const metropolis &rhs);

  /*! \brief Update the (cached) system energy
   */
  void update_U();

  /*! \brief Get number of molecules
   *
   * \return      Number of molecules
   */
  inline auto N() const { return _molecular_ids.size(); }

  /*! \brief Get number of dimensions
   *
   * \return      Number of dimensions
   */
  inline auto D() const { return _positions.n_rows; }

  /*! \brief Get molecular ids
   *
   * \return      Container of molecular ids
   */
  inline const auto &molecular_ids() const { return _molecular_ids; }

  /*! \brief Get molecular positions
   *
   * \return      Molecular positions
   */
  inline auto &positions() { return _positions; }

  /*! \brief Get molecular positions
   *
   * \return      Molecular positions
   */
  inline const auto &positions() const { return _positions; }

  /*! \brief Get Boltzmann's constant
   *
   * \return      Boltzmann's constant
   */
  inline auto kB() const { return _kB; }

  /*! \brief Get temperature
   *
   * \return      Temperature
   */
  inline auto T() const { return _T; }

  /*! \brief Get beta
   *
   * \return      1 / kT
   */
  inline auto beta() const { return _beta; }

  /*! \brief Get distance between molecules
   *
   * \param   r_i            Position of molecule i
   * \param   r_j            Position of molecule j
   * \param   edge_lengths   Edge lengths of simulation box
   * \return                 Distance
   */
  inline auto m(const arma::vec &r_i, const arma::vec &r_j,
                const arma::vec &edge_lengths) const {
    return std::get<1>(_m)(r_i, r_j, edge_lengths);
  }

  /*! \brief Get relative position
   *
   * \param   r_i            Position of molecule i
   * \param   r_j            Position of molecule j
   * \param   edge_lengths   Edge lengths of simulation box
   * \return                 Relative position
   */
  inline auto rij(const arma::vec &r_i, const arma::vec &r_j,
                  const arma::vec &edge_lengths) const {
    return std::get<0>(_m)(r_i, r_j, edge_lengths);
  }

  /*! \brief Get edge lengths of simulation box
   *
   * \return      Edge lengths
   */
  inline const auto &edge_lengths() const { return _edge_lengths; }

  /*! \brief Get volume of simulation box
   *
   * \return      Volume
   */
  inline auto V() const { return _V; }

  /*! \brief Add a callback function
   *
   * \param   cb    Callback function to add
   */
  inline void add_parallel_callback(const callback &cb) const {
    _parallel_callbacks.push_back(cb);
  }

  /*! \brief Add a callback function
   *
   * \param   cb    Callback function to add
   */
  inline void add_sequential_callback(const callback &cb) const {
    _sequential_callbacks.push_back(cb);
  }

  /*! \brief Add a callback function to default container
   *
   * \param   cb    Callback function to add
   */
  inline void add_callback(const callback &cb) const {
    add_sequential_callback(cb);
  }

  /*! \brief Add a stopping criterion
   *
   * \param   sc    Stopping criterion
   */
  inline void add_stopping_criterion(const stopping_criterion &sc) const {
    _stopping_criteria.push_back(sc);
  }

  /*! \brief Clear all stopping criteria
   */
  inline void clear_stopping_criteria() const {
    _stopping_criteria.clear();
  }

  /*! \brief Get current step
   *
   * \return    Current step
   */
  inline auto step() const { return _step; }

  /*! \brief Molecular potentials accessor
   *
   * \return    Molecular potentials
   */
  inline const auto &potentials() const { return _potentials; }

  /*! \brief Accessor for trial move
   *
   * \return    Change in x for trial move
   */
  inline const auto &dx() const { return _dx; }

  /*! \brief Index of molecule that has been chosen to move
   *
   * \return      Molecule number of ``chosen'' molecule
   */
  inline auto choice() const { return _choice; }

  /*! \brief Accessor for change in energy that results from the move
   *
   * \return      Change in energy as a result of trial move
   */
  inline const auto &dU() const { return _dU; }

  /*! \brief Accessor for the current system energy
   *
   * \return      System energy
   */
  inline const auto &U() const { return _U; }

  /*! \brief Accessor for random number used to determine acceptance/rejection
   *
   * \return      Epsilon such that epsilon < B results in accepting the move
   */
  inline const auto &eps() const { return _eps; }

  /*! \brief Accessor for whether or not the trial move was accepted
   *
   * \return      Answers: ``was the trial move accepted?''
   */
  inline const auto &accepted() const { return _accepted; }

  /*! \brief Run metropolis simulation
   *
   * \param   nsteps    Number of steps to simulate
   */
  long unsigned simulate(const long unsigned nsteps);

  /*! \brief Set molecular positions
   *
   * \param   new_positions   New position matrix
   */
  inline void set_positions(const arma::mat &new_positions) {
    _positions = new_positions;
    update_U();
  }

  /*! \brief Set molecular position for molecule j
   *
   * \param   new_x   New position
   * \param   j       Index of molecular to change
   */
  inline void set_positions(const arma::vec &new_x, const size_t j) {
    _positions.col(j) = new_x;
    update_U();
  }

  /*! \brief Seed the random number generator
   *
   * \param     sd      Seed
   */
  inline void seed(const unsigned sd) {
    _rng.seed(sd);
  }

  /*! \brief Reset number of steps
   *
   * \param   step    Step number to set
   */
  inline void reset_step(const unsigned long step = 0) {
    _step = step;
  }

private:
  std::vector<molecular_id> _molecular_ids;
  arma::mat _positions;
  arma::vec _edge_lengths;
  double _V;
  std::vector<abstract_potential*> _potentials;
  double _T;
  double _kB;
  double _beta;
  metric _m;
  bc _bc;
  std::default_random_engine _rng;
  std::uniform_real_distribution<double> _eps_dist;
  std::uniform_int_distribution<size_t> _choice_dist;
  trial_move_generator _tmg;
  acc _acc;
  mutable std::vector<callback> _parallel_callbacks;
  mutable std::vector<callback> _sequential_callbacks;
  mutable std::vector<stopping_criterion> _stopping_criteria;
  long unsigned _step;

  arma::vec _dx;
  size_t _choice;
  double _dU;
  double _U;
  double _eps;
  bool _accepted;
};

} // namespace pauth

#endif


//=============================================================================
//
//  CLASS NConstraintInterface
//
//=============================================================================

#ifndef COMISO_NCONSTRAINTINTERFACE_HH
#define COMISO_NCONSTRAINTINTERFACE_HH

//== COMPILE-TIME PACKAGE REQUIREMENTS ========================================
#include <CoMISo/Config/config.hh>
#if COMISO_Eigen3_AVAILABLE

//== INCLUDES =================================================================

#include <CoMISo/Config/CoMISoDefines.hh>
#include "SuperSparseMatrixT.hh"

#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#include <Eigen/Sparse>

//== FORWARDDECLARATIONS ======================================================

//== NAMESPACES ===============================================================

namespace COMISO {

//== CLASS DEFINITION =========================================================

/** \class NProblemGmmInterface NProblemGmmInterface.hh <ACG/.../NPRoblemGmmInterface.hh>

    Brief Description.

    A more elaborate description follows.
*/
class COMISODLLEXPORT NConstraintInterface
{
public:

  // define Sparse Datatypes
  typedef Eigen::SparseVector<double> SVectorNC;
  typedef SuperSparseMatrixT<double>  SMatrixNC;

  // different types of constraints
  enum ConstraintType {NC_EQUAL, NC_LESS_EQUAL, NC_GREATER_EQUAL};

  /// Default constructor
  NConstraintInterface(const ConstraintType _type = NC_EQUAL) : type_(_type) {}

  /// Destructor
  virtual ~NConstraintInterface() {}

  virtual int             n_unknowns      (                                      ) = 0;
  virtual double          eval_constraint ( const double* _x                     ) = 0;
  virtual void            eval_gradient   ( const double* _x, SVectorNC& _g      ) = 0;
  virtual void            eval_hessian    ( const double* _x, SMatrixNC& _h      ) = 0;

  virtual ConstraintType  constraint_type (                                      ) { return type_; }

  virtual bool            is_satisfied    ( const double* _x, double _eps = 1e-6 )
  {
    switch( type_)
    {
      case NC_EQUAL        : return (fabs(eval_constraint(_x)) <=  _eps); break;
      case NC_LESS_EQUAL   : return (     eval_constraint(_x)  <=  _eps); break;
      case NC_GREATER_EQUAL: return (     eval_constraint(_x)  >= -_eps); break;
    }
    return false;
  }

  virtual bool   is_linear() { return false;}

  virtual double gradient_update_factor( const double* _x, double _eps = 1e-6 )
  {
    double val = eval_constraint(_x);
    bool   upper_bound_ok = ( val <=  _eps);
    bool   lower_bound_ok = ( val >= -_eps);

    if(upper_bound_ok)
    {
      if(lower_bound_ok || type_ == NC_LESS_EQUAL) return 0.0;
      else                                         return 1.0;
    }
    else
    {
      if(lower_bound_ok && type_ == NC_GREATER_EQUAL) return  0.0;
      else                                            return -1.0;
    }
  }

private:
  // constraint type
  ConstraintType type_;
};

//=============================================================================
} // namespace COMISO
//=============================================================================
#endif // COMISO_Eigen3_AVAILABLE
//=============================================================================
#endif // ACG_NCONSTRAINTINTERFACE_HH defined
//=============================================================================