//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/module/core.hpp>
#include <eve/module/polynomial.hpp>
#include <cmath>
#include <array>
#include <vector>
#include <boost/math/special_functions/chebyshev.hpp>

//==================================================================================================
//== Types tests
//==================================================================================================
EVE_TEST_TYPES( "Check return types of tchebeval on wide"
        , eve::test::simd::ieee_reals

        )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  using i_t = eve::as_integer_t<v_t>;
  TTS_EXPR_IS( eve::tchebeval(T(), T())  , T);
  TTS_EXPR_IS( eve::tchebeval(T(), T(), T())  , T);
  TTS_EXPR_IS( eve::tchebeval(T(), T(), T())  , T);
  TTS_EXPR_IS( eve::tchebeval(T(), v_t(), v_t())  , T);
  TTS_EXPR_IS( eve::tchebeval(T(), v_t(), int())  , T);
  TTS_EXPR_IS( eve::tchebeval(T(), v_t(), i_t())  , T);
  TTS_EXPR_IS( eve::tchebeval(v_t(), v_t(), v_t()), v_t);

};

//==================================================================================================
//== tchebeval tests
//==================================================================================================
EVE_TEST( "Check behavior of tchebeval on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::ramp(0.0))
        )
<typename T>(T const& a0)
{
  using eve::tchebeval;
  using eve::fma;
  using eve::pedantic;
  using eve::numeric;
  using eve::one;
  using v_t = eve::element_type_t<T>;
  //============================================================================
  //== variadic
  //============================================================================
  std::vector<v_t> c1{1};
  std::vector<v_t> c2{1, 2};
  std::vector<v_t> c3{1, 2, 3};
  auto bcl1 = [c1](auto x)->v_t{return boost::math::chebyshev_clenshaw_recurrence(c1.data(), c1.size(), x);};
  auto bcl2 = [c2](auto x)->v_t{return boost::math::chebyshev_clenshaw_recurrence(c2.data(), c2.size(), x);};
  auto bcl3 = [c3](auto x)->v_t{return boost::math::detail::unchecked_chebyshev_clenshaw_recurrence(c3.data(), c3.size(), v_t(-1), v_t(1), v_t(x));};

  TTS_EQUAL(tchebeval(a0), T(0));
  TTS_EQUAL(tchebeval(a0, 1.0), T(0.5));
  TTS_EQUAL(tchebeval(a0, 1.0),    map(bcl1, a0));
  TTS_EQUAL(tchebeval(a0, 1.0, 2.0), map(bcl2, a0));
  TTS_EQUAL(tchebeval(a0, 1.0, 2.0, 3.0), map(bcl3, a0));
  TTS_ULP_EQUAL(tchebeval(0.0, 1.0, 2.0, 3.0), bcl3(0.0), 0.5);

  TTS_EQUAL((tchebeval)(a0), T(0));
  TTS_EQUAL((tchebeval)(a0, c1), T(0.5));
  TTS_EQUAL((tchebeval)(a0, c2),  map(bcl2, a0));
  TTS_EQUAL((tchebeval)(a0, c3),  map(bcl3, a0));
  TTS_ULP_EQUAL((tchebeval)(v_t(0.24), c3),  bcl3(v_t(0.24)), 2.0);
  TTS_ULP_EQUAL((tchebeval)(v_t(0.24), v_t(-1), v_t(1), c3),  bcl3(v_t(0.24)), 0.5);
  TTS_ULP_EQUAL((tchebeval)(v_t(-0.24), v_t(-1), v_t(1), c3),  bcl3(v_t(-0.24)), 0.5);
  TTS_ULP_EQUAL((tchebeval)(v_t(0.70), v_t(-1), v_t(1), c3),  bcl3(v_t(0.70)), 0.5);
  TTS_ULP_EQUAL((tchebeval)(v_t(-0.70), v_t(-1), v_t(1), c3),  bcl3(v_t(-0.70)), 2.0);

};


#include "MyConditionalPrior.h"
#include "DNest4/code/DNest4.h"
#include <cmath>
#include <boost/math/distributions/normal.hpp>

namespace Obscurity
{

MyConditionalPrior::MyConditionalPrior()
{

}

void MyConditionalPrior::from_prior(DNest4::RNG& rng)
{
    sigma = rng.rand();

    mu_mass = exp(log(1E-3) + log(1E6)*rng.rand());
    mu_width = exp(log(1E-3) + log(1E6)*rng.rand());
}

double MyConditionalPrior::perturb_hyperparameters(DNest4::RNG& rng)
{
	double logH = 0.0;

    int which = rng.rand_int(3);

    if(which == 0)
    {
        sigma += rng.randh();
        DNest4::wrap(sigma, 0.0, 1.0);
    }
    else if(which == 1)
    {
        mu_mass = log(mu_mass);
        mu_mass += log(1E6)*rng.randh();
        DNest4::wrap(mu_mass, log(1E-3), log(1E3));
        mu_mass = exp(mu_mass);
    }
    else
    {
        mu_width = log(mu_width);
        mu_width += log(1E6)*rng.randh();
        DNest4::wrap(mu_width, log(1E-3), log(1E3));
        mu_width = exp(mu_width);
    }

	return logH;
}

// vec = {xc, yc, mass, width}

double MyConditionalPrior::log_pdf(const std::vector<double>& vec) const
{
    double logp = 0.0;

    if(vec[2] < 0 || vec[3] < 0.999*mu_width || vec[3] > 1.001*mu_width)
        return -1E300;

    logp += -log(2*M_PI*sigma*sigma)
                -0.5*(vec[0]*vec[0] + vec[1]*vec[1])/(sigma*sigma);
    logp += -log(mu_mass) - vec[2]/mu_mass;

	return logp;
}

#include <iostream>
void MyConditionalPrior::from_uniform(std::vector<double>& vec) const
{
    const boost::math::normal standard_normal(0.0, 1.0);
    vec[0] = sigma*quantile(standard_normal, vec[0]);
    vec[1] = sigma*quantile(standard_normal, vec[1]);
    vec[2] = -mu_mass*log(1.0 - vec[2]);
    vec[3] = mu_width*(0.999 + 0.002*vec[3]);
}

void MyConditionalPrior::to_uniform(std::vector<double>& vec) const
{
    const boost::math::normal standard_normal(0.0, 1.0);
    vec[0] = cdf(standard_normal, vec[0]/sigma);
    vec[1] = cdf(standard_normal, vec[1]/sigma);
    vec[2] = 1.0 - exp(-vec[2]/mu_mass);
    vec[3] = (vec[3]/mu_width - 0.999)/0.002;
}

void MyConditionalPrior::print(std::ostream& out) const
{
	out<<sigma<<' '<<mu_mass<<' '<<mu_width<<' ';
}

} // namespace Obscurity


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

/*
 Copyright (C) 2003 Ferdinando Ametrano
 Copyright (C) 2000, 2001, 2002, 2003 RiskMap srl
 Copyright (C) 2015 Peter Caspers

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

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

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

/*! \file incrementalstatistics.hpp
    \brief statistics tool based on incremental accumulation
           in the meantime, this is just a wrapper to the boost
           accumulator library, kept for backward compatibility
*/

#ifndef quantlib_incremental_statistics_hpp
#define quantlib_incremental_statistics_hpp

#include <ql/utilities/null.hpp>
#include <ql/errors.hpp>

#if defined(__clang__)
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wc++11-extensions"
#endif
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/sum.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/weighted_mean.hpp>
#include <boost/accumulators/statistics/weighted_variance.hpp>
#include <boost/accumulators/statistics/weighted_skewness.hpp>
#include <boost/accumulators/statistics/weighted_kurtosis.hpp>
#include <boost/accumulators/statistics/weighted_moment.hpp>
#if defined(__clang__)
#pragma clang diagnostic pop
#endif

#include <iomanip>

namespace QuantLib {

    //! Statistics tool based on incremental accumulation
    /*! It can accumulate a set of data and return statistics (e.g: mean,
        variance, skewness, kurtosis, error estimation, etc.).
        This class is a wrapper to the boost accumulator library.
    */

    class IncrementalStatistics {
      public:
        typedef Real value_type;
        IncrementalStatistics();
        //! \name Inspectors
        //@{
        //! number of samples collected
        Size samples() const;

        //! sum of data weights
        Real weightSum() const;

        /*! returns the mean, defined as
            \f[ \langle x \rangle = \frac{\sum w_i x_i}{\sum w_i}. \f]
        */
        Real mean() const;

        /*! returns the variance, defined as
            \f[ \frac{N}{N-1} \left\langle \left(
                x-\langle x \rangle \right)^2 \right\rangle. \f]
        */
        Real variance() const;

        /*! returns the standard deviation \f$ \sigma \f$, defined as the
            square root of the variance.
        */
        Real standardDeviation() const;

        /*! returns the error estimate \f$ \epsilon \f$, defined as the
            square root of the ratio of the variance to the number of
            samples.
        */
        Real errorEstimate() const;

        /*! returns the skewness, defined as
            \f[ \frac{N^2}{(N-1)(N-2)} \frac{\left\langle \left(
                x-\langle x \rangle \right)^3 \right\rangle}{\sigma^3}. \f]
            The above evaluates to 0 for a Gaussian distribution.
        */
        Real skewness() const;

        /*! returns the excess kurtosis, defined as
            \f[ \frac{N^2(N+1)}{(N-1)(N-2)(N-3)}
                \frac{\left\langle \left(x-\langle x \rangle \right)^4
                \right\rangle}{\sigma^4} - \frac{3(N-1)^2}{(N-2)(N-3)}. \f]
            The above evaluates to 0 for a Gaussian distribution.
        */
        Real kurtosis() const;

        /*! returns the minimum sample value */
        Real min() const;

        /*! returns the maximum sample value */
        Real max() const;

        //! number of negative samples collected
        Size downsideSamples() const;

        //! sum of data weights for negative samples
        Real downsideWeightSum() const;

        /*! returns the downside variance, defined as
            \f[ \frac{N}{N-1} \times \frac{ \sum_{i=1}^{N}
                \theta \times x_i^{2}}{ \sum_{i=1}^{N} w_i} \f],
            where \f$ \theta \f$ = 0 if x > 0 and
            \f$ \theta \f$ =1 if x <0
        */
        Real downsideVariance() const;

        /*! returns the downside deviation, defined as the
            square root of the downside variance.
        */
        Real downsideDeviation() const;

        //@}

        //! \name Modifiers
        //@{
        //! adds a datum to the set, possibly with a weight
        /*! \pre weight must be positive or null */
        void add(Real value, Real weight = 1.0);
        //! adds a sequence of data to the set, with default weight
        template <class DataIterator>
        void addSequence(DataIterator begin, DataIterator end) {
            for (;begin!=end;++begin)
                add(*begin);
        }
        //! adds a sequence of data to the set, each with its weight
        /*! \pre weights must be positive or null */
        template <class DataIterator, class WeightIterator>
        void addSequence(DataIterator begin, DataIterator end,
                         WeightIterator wbegin) {
            for (;begin!=end;++begin,++wbegin)
                add(*begin, *wbegin);
        }
        //! resets the data to a null set
        void reset();
        //@}
     private:
       typedef boost::accumulators::accumulator_set<
           Real,
           boost::accumulators::stats<
               boost::accumulators::tag::count, boost::accumulators::tag::min,
               boost::accumulators::tag::max,
               boost::accumulators::tag::weighted_mean,
               boost::accumulators::tag::weighted_variance,
               boost::accumulators::tag::weighted_skewness,
               boost::accumulators::tag::weighted_kurtosis,
               boost::accumulators::tag::sum_of_weights>,
           Real> accumulator_set;
        accumulator_set acc_;
        typedef boost::accumulators::accumulator_set<
            Real, boost::accumulators::stats<
                      boost::accumulators::tag::count,
                      boost::accumulators::tag::weighted_moment<2>,
                      boost::accumulators::tag::sum_of_weights>,
            Real> downside_accumulator_set;
        downside_accumulator_set downsideAcc_;
    };

    // implementation

    inline IncrementalStatistics::IncrementalStatistics() {
        reset();
    }

    inline Size IncrementalStatistics::samples() const {
        return boost::accumulators::extract_result<
            boost::accumulators::tag::count>(acc_);
    }

    inline Real IncrementalStatistics::weightSum() const {
        return boost::accumulators::extract_result<
            boost::accumulators::tag::sum_of_weights>(acc_);
    }

    inline Real IncrementalStatistics::mean() const {
        QL_REQUIRE(weightSum() > 0.0, "sampleWeight_= 0, unsufficient");
        return boost::accumulators::extract_result<
            boost::accumulators::tag::weighted_mean>(acc_);
    }

    inline Real IncrementalStatistics::variance() const {
        QL_REQUIRE(weightSum() > 0.0, "sampleWeight_= 0, unsufficient");
        QL_REQUIRE(samples() > 1, "sample number <= 1, unsufficient");
        Real n = static_cast<Real>(samples());
        return n / (n - 1.0) *
               boost::accumulators::extract_result<
                   boost::accumulators::tag::weighted_variance>(acc_);
    }

    inline Real IncrementalStatistics::standardDeviation() const {
        return std::sqrt(variance());
    }

    inline Real IncrementalStatistics::errorEstimate() const {
        return std::sqrt(variance() / (samples()));
    }

    inline Real IncrementalStatistics::skewness() const {
        QL_REQUIRE(samples() > 2, "sample number <= 2, unsufficient");
        Real n = static_cast<Real>(samples());
        Real r1 = n / (n - 2.0);
        Real r2 = (n - 1.0) / (n - 2.0);
        return std::sqrt(r1 * r2) *
               boost::accumulators::extract_result<
                   boost::accumulators::tag::weighted_skewness>(acc_);
    }

    inline Real IncrementalStatistics::kurtosis() const {
        QL_REQUIRE(samples() > 3,
                   "sample number <= 3, unsufficient");
        boost::accumulators::extract_result<
            boost::accumulators::tag::weighted_kurtosis>(acc_);
        Real n = static_cast<Real>(samples());
        Real r1 = (n - 1.0) / (n - 2.0);
        Real r2 = (n + 1.0) / (n - 3.0);
        Real r3 = (n - 1.0) / (n - 3.0);
        return ((3.0 + boost::accumulators::extract_result<
                           boost::accumulators::tag::weighted_kurtosis>(acc_)) *
                    r2 -
                3.0 * r3) *
               r1;
    }

    inline Real IncrementalStatistics::min() const {
        QL_REQUIRE(samples() > 0, "empty sample set");
        return boost::accumulators::extract_result<
            boost::accumulators::tag::min>(acc_);
    }

    inline Real IncrementalStatistics::max() const {
        QL_REQUIRE(samples() > 0, "empty sample set");
        return boost::accumulators::extract_result<
            boost::accumulators::tag::max>(acc_);
    }

    inline Size IncrementalStatistics::downsideSamples() const {
        return boost::accumulators::extract_result<
            boost::accumulators::tag::count>(downsideAcc_);
    }

    inline Real IncrementalStatistics::downsideWeightSum() const {
        return boost::accumulators::extract_result<
            boost::accumulators::tag::sum_of_weights>(downsideAcc_);
    }

    inline Real IncrementalStatistics::downsideVariance() const {
        QL_REQUIRE(downsideWeightSum() > 0.0, "sampleWeight_= 0, unsufficient");
        QL_REQUIRE(downsideSamples() > 1, "sample number <= 1, unsufficient");
        Real n = static_cast<Real>(downsideSamples());
        Real r1 = n / (n - 1.0);
        return r1 *
               boost::accumulators::extract_result<
                   boost::accumulators::tag::moment<2> >(downsideAcc_);
    }

    inline Real IncrementalStatistics::downsideDeviation() const {
        return std::sqrt(downsideVariance());
    }

    inline void IncrementalStatistics::add(Real value, Real valueWeight) {
        QL_REQUIRE(valueWeight >= 0.0, "negative weight (" << valueWeight
                                                           << ") not allowed");
        acc_(value, boost::accumulators::weight = valueWeight);
        if(value < 0.0)
            downsideAcc_(value, boost::accumulators::weight = valueWeight);
    }

    inline void IncrementalStatistics::reset() {
        acc_ = accumulator_set();
        downsideAcc_ = downside_accumulator_set();
    }

}

#endif


#ifndef PCP_ALGORITHM_COVARIANCE_HPP
#define PCP_ALGORITHM_COVARIANCE_HPP

/**
 * @file
 * @ingroup algorithm
 */

#include "pcp/common/vector3d_queries.hpp"
#include "pcp/traits/point_map.hpp"

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <utility>

namespace pcp {
namespace algorithm {

/**
 * @ingroup algorithm
 * @brief Computes the covariance matrix of a point set
 * @tparam ForwardIter Iterator type of input point set
 * @tparam PointMap Type satisfying PointMap concept
 * @param begin Start iterator to input point set
 * @param end End iterator to input point set
 * @param point_map The point map property map
 * @return A pair of the mean and covariance matrix [mu, Cov]
 */
template <class ForwardIter, class PointMap>
std::pair<
    Eigen::Matrix<
        typename std::invoke_result_t<
            PointMap,
            typename std::iterator_traits<ForwardIter>::value_type>::coordinate_type,
        3,
        1>,
    Eigen::Matrix<
        typename std::invoke_result_t<
            PointMap,
            typename std::iterator_traits<ForwardIter>::value_type>::coordinate_type,
        3,
        3>>
covariance(ForwardIter begin, ForwardIter end, PointMap const& point_map)
{
    using element_type = typename std::iterator_traits<ForwardIter>::value_type;

    static_assert(
        traits::is_point_map_v<PointMap, element_type>,
        "point_map must satisfy PointMap concept");

    using point_type  = std::invoke_result_t<PointMap, element_type>;
    using scalar_type = typename point_type::coordinate_type;
    using vector_type = Eigen::Matrix<scalar_type, 3, 1>;
    using matrix_type = Eigen::Matrix<
        typename std::invoke_result_t<
            PointMap,
            typename std::iterator_traits<ForwardIter>::value_type>::coordinate_type,
        3,
        3>;

    /**
     * element 0 : x*x
     * element 1 : y*y
     * element 2 : z*z
     * element 3 : x*y
     * element 4 : x*z
     * element 5 : y*z
     */
    std::array<scalar_type, 6u> cov{0.};

    point_type const mu = common::center_of_geometry(begin, end, point_map);
    std::for_each(begin, end, [&](element_type const& e) {
        auto const& p = point_map(e);
        auto const pp = p - mu;

        auto const xx = pp.x() * pp.x();
        auto const yy = pp.y() * pp.y();
        auto const zz = pp.z() * pp.z();
        auto const xy = pp.x() * pp.y();
        auto const xz = pp.x() * pp.z();
        auto const yz = pp.y() * pp.z();

        cov[0] += xx;
        cov[1] += yy;
        cov[2] += zz;
        cov[3] += xy;
        cov[4] += xz;
        cov[5] += yz;
    });

    matrix_type Cov;
    Cov(0, 0) = cov[0];
    Cov(1, 1) = cov[1];
    Cov(2, 2) = cov[2];

    Cov(0, 1) = cov[3];
    Cov(0, 2) = cov[4];
    Cov(1, 2) = cov[5];
    Cov(1, 0) = Cov(0, 1);
    Cov(2, 0) = Cov(0, 2);
    Cov(2, 1) = Cov(1, 2);

    return {vector_type{mu.x(), mu.y(), mu.z()}, Cov};
}

/**
 * @ingroup algorithm
 * @brief Sorts eigenvalues and eigenvectors in increasing order
 * @tparam ScalarType Coefficient type
 * @param lambda Vector of eigen values
 * @param v Matrix of eigen vectors
 * @return a pair = (sorted eigen values, sorted eigen vectors)
 */
template <class ScalarType>
std::pair<Eigen::Matrix<ScalarType, 3, 1>, Eigen::Matrix<ScalarType, 3, 3>> eigen_sorted(
    Eigen::Matrix<ScalarType, 3, 1> const& lambda,
    Eigen::Matrix<ScalarType, 3, 3> const& v)
{
    using vector_type = Eigen::Matrix<ScalarType, 3, 1>;
    using matrix_type = Eigen::Matrix<ScalarType, 3, 3>;

    std::array<int, 3u> indices{0, 1, 2};
    std::sort(indices.begin(), indices.end(), [&](int const i, int const j) {
        return lambda(i) < lambda(j);
    });

    vector_type const eigen_values{lambda(indices[0]), lambda(indices[1]), lambda(indices[2])};
    matrix_type eigen_vectors(3, 3);
    eigen_vectors.col(0) = v.col(indices[0]);
    eigen_vectors.col(1) = v.col(indices[1]);
    eigen_vectors.col(2) = v.col(indices[2]);

    return {eigen_values, eigen_vectors};
}

/**
 * @ingroup algorithm
 * @brief Sorts eigenvalues and eigenvectors in increasing order
 * @tparam ScalarType Coefficient type
 * @param Cov The covariance matrix
 * @return a pair = (sorted eigen values, sorted eigen vectors)
 */
template <class ScalarType>
std::pair<Eigen::Matrix<ScalarType, 3, 1>, Eigen::Matrix<ScalarType, 3, 3>>
eigen_sorted(Eigen::Matrix<ScalarType, 3, 3> const& Cov)
{
    using vector_type = Eigen::Matrix<ScalarType, 3, 1>;
    using matrix_type = Eigen::Matrix<ScalarType, 3, 3>;

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<ScalarType, 3, 3>> A(Cov);
    vector_type const& lambda = A.eigenvalues();
    matrix_type const& v      = A.eigenvectors();

    return eigen_sorted(lambda, v);
}

/**
 * @ingroup algorithm
 * @brief Returns the sorted eigen values and eigen vectors of the covariance matrix of a point set
 * @tparam ForwardIter Iterator type of input point set
 * @tparam PointMap Type satisfying PointMap concept
 * @param begin Start iterator to the input point set
 * @param end End iterator to the input point set
 * @param point_map The point map property map
 * @return A pair = (sorted eigen values, sorted eigen vectors)
 */
template <class ForwardIter, class PointMap>
std::pair<
    Eigen::Matrix<
        typename std::invoke_result_t<
            PointMap,
            typename std::iterator_traits<ForwardIter>::value_type>::coordinate_type,
        3,
        1>,
    Eigen::Matrix<
        typename std::invoke_result_t<
            PointMap,
            typename std::iterator_traits<ForwardIter>::value_type>::coordinate_type,
        3,
        3>>
pca(ForwardIter begin, ForwardIter end, PointMap const& point_map)
{
    using matrix_type = Eigen::Matrix<
        typename std::invoke_result_t<
            PointMap,
            typename std::iterator_traits<ForwardIter>::value_type>::coordinate_type,
        3,
        3>;

    using vector_type = Eigen::Matrix<
        typename std::invoke_result_t<
            PointMap,
            typename std::iterator_traits<ForwardIter>::value_type>::coordinate_type,
        3,
        1>;

    auto const [mu, sigma] = covariance(begin, end, point_map);
    matrix_type const& Cov = sigma;

    return eigen_sorted(Cov);
}

} // namespace algorithm
} // namespace pcp

#endif // PCP_ALGORITHM_COVARIANCE_HPP


/*
 * @Author: Han
 * @Date: 2018-05-05 20:49:29
 * 相机信息
 */
#include <base/camera.h>

#include <Eigen/Geometry>

namespace h2o {
FrameCamera::FrameCamera() {
    cid_ = INVALID_INDEX;
    iid_ = INVALID_INDEX;
    size_ = Vector2i(-1, -1);
    K = Matrix3d::Zero();
    R = Matrix3d::Zero();
    C = Vector3d::Zero();
    P = Matrix34d::Zero();
}
FrameCamera::FrameCamera(uint32_t cid, uint32_t iid, int width, int height, double delta, double f, double x0,
                         double y0, const Vector3d &r, const Vector3d &X) {
    cid_ = cid;
    iid_ = iid;
    size_(0) = width;
    size_(1) = height;

    K(0, 0) = f / delta;
    K(1, 1) = f / delta;
    double u0 = (width - 1) / 2.0 + x0 / delta;
    double v0 = (height - 1) / 2.0 - y0 / delta;

    K(0, 1) = 0.0;
    K(0, 2) = u0;

    K(1, 0) = 0.0;
    K(1, 2) = v0;

    K(2, 0) = 0.0;
    K(2, 1) = 0.0;
    K(2, 2) = 1.0;

    double angle = r.norm();
    Vector3d axis = r.normalized();
    Matrix3d Rpg = Eigen::AngleAxisd(angle, axis).toRotationMatrix();
    Matrix3d R_x = Eigen::AngleAxisd(M_PI, Vector3d::UnitX()).toRotationMatrix();
    R = Rpg * R_x;
    R.transposeInPlace();

    C = X;
    Vector3d t = -R * C;
    P << R, t;
    P = K * P;
}
FrameCamera::~FrameCamera() {}
} // namespace h2o


#include <boost/math/interpolators/cubic_b_spline.hpp>


#include <stdlib.h>
#include <math.h>
#include <stdint.h>

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmisleading-indentation"
#include <Eigen/Core>
#pragma GCC diagnostic pop

struct Aligned {
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

#ifndef M_PI
#define M_PI            3.14159265358979323846
#define M_SQRT2         1.41421356237309504880
#define M_SQRT1_2       0.707106781186547524401
#endif

#ifdef __SSE2__
#include <emmintrin.h>
#endif

#define TEMPLATE template<typename T>
#define INNER static inline
#ifdef _MSC_VER
#define PURE
#define CONST
#define RESTRICT
#else
#define PURE __attribute__((pure))
#define CONST __attribute__((const))
#define RESTRICT __restrict__
#endif

typedef unsigned long ulong; // __attribute((aligned(16)));

#ifndef BLOCK_SIZE
#define BLOCK_SIZE 256
#endif

#include "vectors.hpp"

#ifdef FORCE_SINGLE
#define v2df v2sf
#endif

INNER void
disable_denormals()
{
	#if __SSE2__
	_mm_setcsr(_mm_getcsr() | 0x8040);
	#endif
}

#define LIMIT(v,l,u) ((v)<(l)?(l):((v)>(u)?(u):(v)))
#define DB2LIN(x) ((x) > -90 ? pow(10, (x) * 0.05) : 0)

/* frequency to rads/sec (angular frequency) */
#define ANGULAR(fc, fs)     (2 * M_PI / (fs) * (fc))
#define ANGULAR_LIM(fc, fs) (2 * M_PI / (fs) * LIMIT((fc), 1, (fs)/2))

/* via http://www.rgba.org/articles/sfrand/sfrand.htm */
INNER float
whitenoise(unsigned int &mirand)
{
	union either {
		float f;
		unsigned int i;
	} white;
	mirand *= 16807;
	white.i = (mirand & 0x007FFFFF) | 0x40000000;
	return white.f - 3;
}

typedef enum {
	FILT_PEAKING,
	FILT_LOWSHELF,
	FILT_HIGHSHELF,
	FILT_LOWPASS,
	FILT_HIGHPASS,
	FILT_ALLPASS,
	FILT_BANDPASS,
	FILT_BANDPASS_2,
	FILT_NOTCH,
	FILT_GAIN
} filter_t;


//
//  Copyright (c) 2018-2019, Cem Bassoy, cem.bassoy@gmail.com
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  Fraunhofer IOSB, Ettlingen, Germany
//

#include <boost/numeric/ublas/tensor.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <iostream>

int main()
{
	using namespace boost::numeric::ublas;

	using format_t  = column_major;
	using value_t   = float; // std::complex<double>;
	using tensor_t = tensor<value_t,format_t>;
	using matrix_t = matrix<value_t,format_t>;
	using vector_t = vector<value_t>;

	// Tensor-Vector-Multiplications - Including Transposition
	{

		auto n = shape{3,4,2};
		auto A = tensor_t(n,2);
		auto q = 0u; // contraction mode

		// C1(j,k) = T2(j,k) + A(i,j,k)*T1(i);
		q = 1u;
		tensor_t C1 = matrix_t(n[1],n[2],2) + prod(A,vector_t(n[q-1],1),q);

		// C2(i,k) = A(i,j,k)*T1(j) + 4;
		q = 2u;
		tensor_t C2 = prod(A,vector_t(n[q-1],1),q) + 4;

		// C3() = A(i,j,k)*T1(i)*T2(j)*T2(k);
		tensor_t C3 = prod(prod(prod(A,vector_t(n[0],1),1),vector_t(n[1],1),1),vector_t(n[2],1),1);

		// C4(i,j) = A(k,i,j)*T1(k) + 4;
		q = 1u;
		tensor_t C4 = prod(trans(A,{2,3,1}),vector_t(n[2],1),q) + 4;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C1(j,k) = T2(j,k) + A(i,j,k)*T1(i);" << std::endl << std::endl;
		std::cout << "C1=" << C1 << ";" << std::endl << std::endl;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C2(i,k) = A(i,j,k)*T1(j) + 4;" << std::endl << std::endl;
		std::cout << "C2=" << C2 << ";" << std::endl << std::endl;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C3() = A(i,j,k)*T1(i)*T2(j)*T2(k);" << std::endl << std::endl;
		std::cout << "C3()=" << C3(0) << ";" << std::endl << std::endl;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C4(i,j) = A(k,i,j)*T1(k) + 4;" << std::endl << std::endl;
		std::cout << "C4=" << C4 << ";" << std::endl << std::endl;

	}

	// Tensor-Matrix-Multiplications - Including Transposition
	{

		auto n = shape{3,4,2};
		auto A = tensor_t(n,2);
		auto m = 5u;
		auto q = 0u; // contraction mode

		// C1(l,j,k) = T2(l,j,k) + A(i,j,k)*T1(l,i);
		q = 1u;
		tensor_t C1 = tensor_t(shape{m,n[1],n[2]},2) + prod(A,matrix_t(m,n[q-1],1),q);

		// C2(i,l,k) = A(i,j,k)*T1(l,j) + 4;
		q = 2u;
		tensor_t C2 = prod(A,matrix_t(m,n[q-1],1),q) + 4;

		// C3(i,l1,l2) = A(i,j,k)*T1(l1,j)*T2(l2,k);
		q = 3u;
		tensor_t C3 = prod(prod(A,matrix_t(m+1,n[q-2],1),q-1),matrix_t(m+2,n[q-1],1),q);

		// C4(i,l1,l2) = A(i,j,k)*T2(l2,k)*T1(l1,j);
		tensor_t C4 = prod(prod(A,matrix_t(m+2,n[q-1],1),q),matrix_t(m+1,n[q-2],1),q-1);

		// C5(i,k,l) = A(i,k,j)*T1(l,j) + 4;
		q = 3u;
		tensor_t C5 = prod(trans(A,{1,3,2}),matrix_t(m,n[1],1),q) + 4;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C1(l,j,k) = T2(l,j,k) + A(i,j,k)*T1(l,i);" << std::endl << std::endl;
		std::cout << "C1=" << C1 << ";" << std::endl << std::endl;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C2(i,l,k) = A(i,j,k)*T1(l,j) + 4;" << std::endl << std::endl;
		std::cout << "C2=" << C2 << ";" << std::endl << std::endl;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C3(i,l1,l2) = A(i,j,k)*T1(l1,j)*T2(l2,k);" << std::endl << std::endl;
		std::cout << "C3=" << C3 << ";" << std::endl << std::endl;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C4(i,l1,l2) = A(i,j,k)*T2(l2,k)*T1(l1,j);" << std::endl << std::endl;
		std::cout << "C4=" << C4 << ";" << std::endl << std::endl;
		std::cout << "% C3 and C4 should have the same values, true? " << std::boolalpha << (C3 == C4) << "!" << std::endl;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C5(i,k,l) = A(i,k,j)*T1(l,j) + 4;" << std::endl << std::endl;
		std::cout << "C5=" << C5 << ";" << std::endl << std::endl;
	}

	// Tensor-Tensor-Multiplications Including Transposition
	{

		using perm_t = std::vector<std::size_t>;

		auto na = shape{3,4,5};
		auto nb = shape{4,6,3,2};
		auto A = tensor_t(na,2);
		auto B = tensor_t(nb,3);

		// C1(j,l) = T(j,l) + A(i,j,k)*A(i,j,l) + 5;
		tensor_t C1 = tensor_t(shape{na[2],na[2]},2) + prod(A,A,perm_t{1,2}) + 5;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C1(k,l) = T(k,l) + A(i,j,k)*A(i,j,l) + 5;" << std::endl << std::endl;
		std::cout << "C1=" << C1 << ";" << std::endl << std::endl;

		// C2(k,l,m) = T(k,l,m) + A(i,j,k)*B(j,l,i,m) + 5;
		tensor_t C2 = tensor_t(shape{na[2],nb[1],nb[3]},2) + prod(A,B,perm_t{1,2},perm_t{3,1}) + 5;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "%  C2(k,l,m) = T(k,l,m) + A(i,j,k)*B(j,l,i,m) + 5;" << std::endl << std::endl;
		std::cout << "C2=" << C2 << ";" << std::endl << std::endl;

		// C3(k,l,m) = T(k,l,m) + A(i,j,k)*trans(B(j,l,i,m),{2,3,1,4})+ 5;
		tensor_t C3 = tensor_t(shape{na[2],nb[1],nb[3]},2) + prod(A,trans(B,{2,3,1,4}),perm_t{1,2}) + 5;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "%  C3(k,l,m) = T(k,l,m) + A(i,j,k)*trans(B(j,l,i,m),{2,3,1,4})+ 5;" << std::endl << std::endl;
		std::cout << "C3=" << C3 << ";" << std::endl << std::endl;

	}
}


#include <NTL/lzz_pXFactoring.h>
#include <NTL/lzz_pEXFactoring.h>

NTL_CLIENT

int main()
{
   zz_p::init(17);

   zz_pX P;
   BuildIrred(P, 10);

   zz_pE::init(P);

   zz_pEX f, g, h;

   random(f, 20);
   SetCoeff(f, 20);

   random(h, 20);

   g = MinPolyMod(h, f);

   if (deg(g) < 0) Error("bad zz_pEXTest (1)");
   if (CompMod(g, h, f) != 0)
      Error("bad zz_pEXTest (2)");

   vec_pair_zz_pEX_long v;

   long i;
   for (i = 0; i < 5; i++) {
      long n = RandomBnd(20)+1;
      cerr << n << " ";

      random(f, n);
      SetCoeff(f, n);

      v = CanZass(f);

      g = mul(v);
      if (f != g) cerr << "oops1\n";

      long i;
      for (i = 0; i < v.length(); i++)
         if (!DetIrredTest(v[i].a))
            Error("bad zz_pEXTest (3)");

   }

   cerr << "\n";

   cerr << "zz_pEXTest OK\n";
}


/* Copyright (C) 2012-2017 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
// g++ -o cgauss -I/usr/local/include cgauss.cpp -L/usr/local/lib -lntl -lm

// So this confirms that E(s^k)/H^k = k!

#include <NTL/ZZ.h>

#include <complex>

NTL_CLIENT

//evaluate f at e^{2 pi i/m}, returning a complex number

complex<double> evalPoly(double *f, long i, long m)
{
  complex<double> t(0.0, 2*M_PI*i/((double) m));
  complex<double> x = exp(t);

  complex<double> res = 0.0;
  for (long j = m-1; j >= 0; j--)
    res = res*x + f[j];

  return res;
}

int main()
{
   long m = 1001;
   long h = 100;

   long maxpow = 7;
   long iter = 100000;

   double *s = new double[m];
   complex<double> *pow = new complex<double> [m];
   double *sum = new double[maxpow+1];

   for (long k = 1; k <= maxpow; k++) sum[k] = 0;

   complex<double> t(0.0, 2*M_PI*1/((double) m));
   complex<double> x = exp(t);
   complex<double> xi = 1.0;
   for (long i = 0; i < m; i++) {
      pow[i] = xi;
      xi *= x;
   }

   for (long u = 0; u < iter; u++) {
      complex<double> v = 0.0;
      for (long i = 0; i < m; i++) {
         if (RandomBnd(m) < h) { // true w/ prob. h/m
            if (RandomBnd(2) == 0)
               v += pow[i];
            else
               v -= pow[i];
         }
      }

      double nv = norm(v);
      double nvk = nv;

      for (long k = 1; k <= maxpow; k++) {
         sum[k] += nvk;
         nvk *= nv;
      }
   }

   for (long k = 1; k <= maxpow; k++) {
      double ave = sum[k]/iter;
      cout << ave/exp(k*log(h)) << "\n";
   }
}


// (C) Copyright Andrew Sutton 2007
//
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0 (See accompanying file
// LICENSE_1_0.txt or http://www.boost.org/LICENSE_1_0.txt)

//[scaled_closeness_centrality_example
#include <iostream>
#include <iomanip>

#include <boost/graph/undirected_graph.hpp>
#include <boost/graph/exterior_property.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>
#include <boost/graph/closeness_centrality.hpp>
#include "helper.hpp"

using namespace std;
using namespace boost;

// This template struct provides a generic version of a "scaling"
// closeness measure. Specifically, this implementation divides
// the number of vertices in the graph by the sum of geodesic
// distances of each vertex. This measure allows customization
// of the distance type, result type, and even the underlying
// divide operation.
template <typename Graph,
          typename Distance,
          typename Result,
          typename Divide = divides<Result> >
struct scaled_closeness_measure
{
    typedef Distance distance_type;
    typedef Result result_type;

    Result operator ()(Distance d, const Graph& g)
    {
        if(d == numeric_values<Distance>::infinity()) {
            return numeric_values<Result>::zero();
        }
        else {
            return div(Result(num_vertices(g)), Result(d));
        }
    }
    Divide div;
};

// The Actor type stores the name of each vertex in the graph.
struct Actor
{
    std::string name;
};

// Declare the graph type and its vertex and edge types.
typedef undirected_graph<Actor> Graph;
typedef graph_traits<Graph>::vertex_descriptor Vertex;
typedef graph_traits<Graph>::edge_descriptor Edge;

// The name map provides an abstract accessor for the names of
// each vertex. This is used during graph creation.
typedef property_map<Graph, string Actor::*>::type NameMap;

// Declare a matrix type and its corresponding property map that
// will contain the distances between each pair of vertices.
typedef exterior_vertex_property<Graph, int> DistanceProperty;
typedef DistanceProperty::matrix_type DistanceMatrix;
typedef DistanceProperty::matrix_map_type DistanceMatrixMap;

// Declare the weight map so that each edge returns the same value.
typedef constant_property_map<Edge, int> WeightMap;

// Declare a container and its corresponding property map that
// will contain the resulting closeness centralities of each
// vertex in the graph.
typedef boost::exterior_vertex_property<Graph, float> ClosenessProperty;
typedef ClosenessProperty::container_type ClosenessContainer;
typedef ClosenessProperty::map_type ClosenessMap;

int
main(int argc, char *argv[])
{
    // Create the graph and a property map that provides access
    // to the actor names.
    Graph g;
    NameMap nm(get(&Actor::name, g));

    // Read the graph from standard input.
    read_graph(g, nm, cin);

    // Compute the distances between all pairs of vertices using
    // the Floyd-Warshall algorithm. Note that the weight map is
    // created so that every edge has a weight of 1.
    DistanceMatrix distances(num_vertices(g));
    DistanceMatrixMap dm(distances, g);
    WeightMap wm(1);
    floyd_warshall_all_pairs_shortest_paths(g, dm, weight_map(wm));

    // Create the scaled closeness measure.
    scaled_closeness_measure<Graph, int, float> m;

    // Compute the degree centrality for graph
    ClosenessContainer cents(num_vertices(g));
    ClosenessMap cm(cents, g);
    all_closeness_centralities(g, dm, cm, m);

    // Print the scaled closeness centrality of each vertex.
    graph_traits<Graph>::vertex_iterator i, end;
    for(boost::tie(i, end) = vertices(g); i != end; ++i) {
        cout << setw(12) << setiosflags(ios::left)
             << g[*i].name << get(cm, *i) << endl;
    }

    return 0;
}
//]


#include <boost/test/test_case_template.hpp>
#include <boost/test/unit_test.hpp>

#include <vmath/functions.hpp>
#include "type_lists.hpp"

BOOST_AUTO_TEST_SUITE(functions)

BOOST_AUTO_TEST_CASE_TEMPLATE(pi, T, float_types) {
	BOOST_CHECK_CLOSE(static_cast<T>(vmath::PI), static_cast<T>(3.14159265358979323846), 1e-4f);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(equals, T, float_types) {
	BOOST_CHECK(vmath::equals(static_cast<T>(1.2), static_cast<T>(1.2)));
	BOOST_CHECK(!vmath::equals(static_cast<T>(1.2), static_cast<T>(1.3)));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(equals_specify_ulp, T, float_types) {
	BOOST_CHECK(vmath::equals(static_cast<T>(1.2), static_cast<T>(1.2), 3));
	BOOST_CHECK(!vmath::equals(static_cast<T>(1.2), static_cast<T>(1.3), 3));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(degrees_to_radians, T, float_types) {
	BOOST_CHECK_SMALL(vmath::radians(static_cast<T>(0.0)), static_cast<T>(1e-7));
	BOOST_CHECK_CLOSE(vmath::radians(static_cast<T>(180.0)), static_cast<T>(vmath::PI), 1e-4f);
	BOOST_CHECK_CLOSE(vmath::radians(static_cast<T>(23.5)), static_cast<T>(0.410152374), 1e-4f);
	BOOST_CHECK_CLOSE(vmath::radians(static_cast<T>(985.0)), static_cast<T>(17.1914921), 1e-4f);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(radians_to_degrees, T, float_types) {
	BOOST_CHECK_SMALL(vmath::degrees(static_cast<T>(0.0)), static_cast<T>(1e-7));
	BOOST_CHECK_CLOSE(vmath::degrees(static_cast<T>(vmath::PI)), static_cast<T>(180.0), 1e-4f);
	BOOST_CHECK_CLOSE(vmath::degrees(static_cast<T>(0.345)), static_cast<T>(19.76704393), 1e-4f);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(clamp, T, float_types) {
	BOOST_CHECK_CLOSE(vmath::clamp(static_cast<T>(1.2), static_cast<T>(1.5), static_cast<T>(2.0)), static_cast<T>(1.5), 1e-4f);
	BOOST_CHECK_CLOSE(vmath::clamp(static_cast<T>(1.5), static_cast<T>(1.5), static_cast<T>(2.0)), static_cast<T>(1.5), 1e-4f);
	BOOST_CHECK_CLOSE(vmath::clamp(static_cast<T>(1.75), static_cast<T>(1.5), static_cast<T>(2.0)), static_cast<T>(1.75), 1e-4f);
	BOOST_CHECK_CLOSE(vmath::clamp(static_cast<T>(2.0), static_cast<T>(1.5), static_cast<T>(2.0)), static_cast<T>(2.0), 1e-4f);
	BOOST_CHECK_CLOSE(vmath::clamp(static_cast<T>(2.2), static_cast<T>(1.5), static_cast<T>(2.0)), static_cast<T>(2.0), 1e-4f);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(lerp, T, float_types) {
	BOOST_CHECK_CLOSE(vmath::lerp(static_cast<T>(1.0), static_cast<T>(2.0), static_cast<T>(0.0)), static_cast<T>(1.0), 1e-4f);
	BOOST_CHECK_CLOSE(vmath::lerp(static_cast<T>(1.0), static_cast<T>(2.0), static_cast<T>(1.0)), static_cast<T>(2.0), 1e-4f);
	BOOST_CHECK_CLOSE(vmath::lerp(static_cast<T>(1.0), static_cast<T>(2.0), static_cast<T>(0.5)), static_cast<T>(1.5), 1e-4f);
	BOOST_CHECK_CLOSE(vmath::lerp(static_cast<T>(1.2), static_cast<T>(3.4), static_cast<T>(0.34)), static_cast<T>(1.948), 1e-4f);
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * @file shortest_path.cc
 *
 * Implement the BGL shortest path algorithm wrappers.
 */

/*
 * David Gleich
 * 19 April 2006
 */

/*
 * 18 April 2007
 * Added src/dst vertex pairs to all the calls to allow partial searches.
 * Corrected small documentation bugs.
 *
 * 9 July 2007
 * Switched to simple_csr_matrix graph type
 */

#include "include/matlab_bgl.h"

#include <yasmic/simple_csr_matrix_as_graph.hpp>
#include <yasmic/iterator_utility.hpp>

#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <yasmic/boost_mod/bellman_ford_shortest_paths.hpp>
#include <boost/graph/dag_shortest_paths.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <yasmic/boost_mod/floyd_warshall_shortest.hpp>

#include "visitor_macros.hpp"
#include "stop_visitors.hpp"
#include "libmbgl_util.hpp"

struct stop_dijkstra {}; // stop dijkstra exception

int dijkstra_sp(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight, /* connectivity params */
    mbglIndex src, mbglIndex dst, /* problem data */
    double* d, mbglIndex *pred, double dinf)
{
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_weighted_graph;
    crs_weighted_graph g(nverts, nverts, ia[nverts], ia, ja, weight);

    if (dst == nverts) {
        dijkstra_shortest_paths(g, src, distance_inf(dinf).predecessor_map(pred).distance_map(d));
    } else {
        try {
            dijkstra_shortest_paths(g, src,
                distance_inf(dinf).predecessor_map(pred).distance_map(d).
                visitor(make_dijkstra_visitor(
                            stop_search_on_vertex_target(dst, stop_dijkstra(), on_discover_vertex()))));
        } catch (stop_dijkstra) {}
    }

    return (0);
}

template <class Graph>
struct c_dijkstra_visitor
{
    dijkstra_visitor_funcs_t *vis;

    VISITOR_VERTEX_FUNC(initialize_vertex, stop_dijkstra)
    VISITOR_VERTEX_FUNC(examine_vertex, stop_dijkstra)
    VISITOR_VERTEX_FUNC(discover_vertex, stop_dijkstra)
    VISITOR_VERTEX_FUNC(finish_vertex, stop_dijkstra)

    VISITOR_EDGE_FUNC(examine_edge, stop_dijkstra)
    VISITOR_EDGE_FUNC(edge_relaxed, stop_dijkstra)
    VISITOR_EDGE_FUNC(edge_not_relaxed, stop_dijkstra)

};

int dijkstra_sp_visitor(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight, /* connectivity params */
    mbglIndex src, /* problem data */
    double* d, mbglIndex *pred,
    double dinf, dijkstra_visitor_funcs_t vis)
{
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_weighted_graph;
    crs_weighted_graph g(nverts, nverts, ia[nverts], ia, ja, weight);

    c_dijkstra_visitor<crs_weighted_graph> visitor_impl;
    visitor_impl.vis = &vis;

    try
    {
        dijkstra_shortest_paths(g, src, distance_inf(dinf).predecessor_map(pred).distance_map(d).visitor(visitor_impl));
    }
    catch (stop_dijkstra)
    {
    }

    return (0);
}

struct stop_bellman_ford {}; // stop bellman_ford exception

int bellman_ford_sp(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight, /* connectivity params */
    mbglIndex src, mbglIndex dst, /* problem data */
    double* d, mbglIndex *pred, double dinf)
{
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_weighted_graph;
    crs_weighted_graph g(nverts, nverts, ia[nverts], ia, ja, weight);

    if (dst == nverts) {
        bellman_ford_shortest_paths(g,
            root_vertex(src).distance_inf(dinf).predecessor_map(pred).distance_map(d));
    } else {
        try {
            bellman_ford_shortest_paths(g,
                root_vertex(src).distance_inf(dinf).predecessor_map(pred).distance_map(d).
                visitor(make_bellman_visitor(
                            stop_search_on_vertex_target(dst, stop_bellman_ford(), on_discover_vertex()))));
        } catch (stop_bellman_ford) {}
    }

    return (0);
}

template <class Graph>
struct c_bellman_ford_visitor
{
    bellman_ford_visitor_funcs_t *vis;

    VISITOR_VERTEX_FUNC(initialize_vertex, stop_bellman_ford)

    VISITOR_EDGE_FUNC(examine_edge, stop_bellman_ford)
    VISITOR_EDGE_FUNC(edge_relaxed, stop_bellman_ford)
    VISITOR_EDGE_FUNC(edge_not_relaxed, stop_bellman_ford)
    VISITOR_EDGE_FUNC(edge_minimized, stop_bellman_ford)
    VISITOR_EDGE_FUNC(edge_not_minimized, stop_bellman_ford)
};

int bellman_ford_sp_visitor(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight, /* connectivity params */
    mbglIndex src,  /* problem data */
    double* d, mbglIndex *pred, double dinf,
    bellman_ford_visitor_funcs_t vis)
{
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_weighted_graph;
    crs_weighted_graph g(nverts, nverts, ia[nverts], ia, ja, weight);

    c_bellman_ford_visitor<crs_weighted_graph> visitor_impl;
    visitor_impl.vis = &vis;

    try
    {
        bellman_ford_shortest_paths(g,
            root_vertex(src).distance_inf(dinf).predecessor_map(pred).distance_map(d).visitor(visitor_impl));
    }
    catch (stop_bellman_ford)
    {
    }

    return (0);
}

struct stop_dag {}; // stop dag exception

int dag_sp(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight, /* connectivity params */
    mbglIndex src, mbglIndex dst, /* problem data */
    double* d, mbglIndex *pred, double dinf)
{
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_weighted_graph;
    crs_weighted_graph g(nverts, nverts, ia[nverts], ia, ja, weight);

    if (dst == nverts) {
        dag_shortest_paths(g, src,
            distance_inf(dinf).predecessor_map(pred).distance_map(d));
    } else {
        try {
            dag_shortest_paths(g, src,
                distance_inf(dinf).predecessor_map(pred).distance_map(d).
                visitor(make_dijkstra_visitor(
                    stop_search_on_vertex_target(dst, stop_dag(), on_discover_vertex()))));
        } catch (stop_dag) {}
    }

    return (0);
}

int johnson_all_sp(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight, /* connectivity params */
    double* D, double dinf)
{
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_weighted_graph;
    crs_weighted_graph g(nverts, nverts, ia[nverts], ia, ja, weight);

    row_matrix<double> Dmat(D,nverts,nverts);

    bool rval = johnson_all_pairs_shortest_paths(g, Dmat,
		distance_inf(dinf).distance_combine(std::plus<double>()));

	if (rval == true)
	{
		return (0);
	}

	// else, there was an error
	return (-1);
}

int floyd_warshall_all_sp(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight, /* connectivity params */
    double* D, double dinf,
    mbglIndex* pred)
{
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_weighted_graph;
    crs_weighted_graph g(nverts, nverts, ia[nverts], ia, ja, weight);

    row_matrix<double> Dmat(D,nverts,nverts);
    bool rval = false;
    if (!pred) {
        rval = floyd_warshall_all_pairs_shortest_paths(g,
            Dmat, distance_inf(dinf).distance_combine(std::plus<double>()));
    } else {
        row_matrix<mbglIndex> Pmat(pred,nverts,nverts);
        //rval = floyd_warshall_all_pairs_shortest_paths(g,
        //    Dmat, distance_inf(dinf).distance_combine(std::plus<double>()).predecessor_map(Pmat));
        // making this call is ridiculous, but otherwise, it won't pick up the right type!
        rval = floyd_warshall_all_pairs_shortest_paths(g, Dmat, Pmat, get(edge_weight,g),
            std::less<double>(),std::plus<double>(),dinf,0.0);
    }

	if (rval == true)
	{
		return (0);
	}

	// else, there was an error
	return (-1);
}


/**
 * @file quasiinterpolation.cc
 * @brief NPDE exam TEMPLATE CODE FILE
 * @author Oliver Rietmann
 * @date 15.07.2020
 * @copyright Developed at SAM, ETH Zurich
 */

#include "quasiinterpolation.h"

#include <lf/base/base.h>  // nonstd::span
#include <lf/geometry/geometry.h>
#include <lf/mesh/utils/utils.h>

#include <Eigen/Core>
#include <memory>
#include <utility>

namespace QuasiInterpolation {

// Auxiliary function: computing the length of an edge
double edgeLength(const lf::mesh::Entity &edge) {
  Eigen::Matrix2d corners = lf::geometry::Corners(*(edge.Geometry()));
  return (corners.col(1) - corners.col(0)).norm();
}

// Auxiliary function: computing the length of the longest edge
double maxLength(const nonstd::span<const lf::mesh::Entity *const> &edges) {
  double length = 0.0;
  for (const lf::mesh::Entity *edge : edges) {
    length = std::max(length, edgeLength(*edge));
  }
  return length;
}

/* SAM_LISTING_BEGIN_1 */
lf::mesh::utils::CodimMeshDataSet<
    std::pair<const lf::mesh::Entity *, unsigned int>>
findKp(std::shared_ptr<const lf::mesh::Mesh> mesh_p) {
  // Variable for returning result
  lf::mesh::utils::CodimMeshDataSet<
      std::pair<const lf::mesh::Entity *, unsigned int>>
      KpMeshDataSet(mesh_p, 2);
  // Auxiliary array storing size of largest triangle adjacent to a node
  lf::mesh::utils::CodimMeshDataSet<double> sizeMeshDataSet(mesh_p, 2);
  // loop over all cells
  for (const lf::mesh::Entity *triangle : mesh_p->Entities(0)) {
    LF_ASSERT_MSG(triangle->RefEl() == lf::base::RefEl::kTria(),
                  "Only implemented for triangles");
    // Fetch coordinates of vertices
    const Eigen::MatrixXd corners{lf::geometry::Corners(*triangle->Geometry())};
    // Determine size of triangle
    const double newSize = std::max({(corners.col(1) - corners.col(0)).norm(),
                                     (corners.col(2) - corners.col(1)).norm(),
                                     (corners.col(0) - corners.col(2)).norm()});
    // Obtain array of pointers to vertex objects of current triangle
    nonstd::span<const lf::mesh::Entity *const> vertices{
        triangle->SubEntities(2)};
    // Loop over vertices and update size of largest adjacent triangle.
    for (unsigned int i = 0; i < 3; ++i) {
      // Note that 'size' is a reference!
      double &size = sizeMeshDataSet(*vertices[i]);
      // Current triangle is larger than those recorded earlier
      if (newSize > size) {
        // Update entry of auxiliary array
        size = newSize;
        // Store pointer to current triangle and local vertex index
        KpMeshDataSet(*vertices[i]) = std::make_pair(triangle, i);
      }
    }
  }  // end of loop over triangles
  return KpMeshDataSet;
}
/* SAM_LISTING_END_1 */

}  // namespace QuasiInterpolation


#include <boost/multiprecision/cpp_int.hpp>
#include <string>
#include <iostream>

using namespace std;
using namespace boost::multiprecision;

using big_int = cpp_int;
// using rational = cpp_rational;

int main()
{
    big_int i;
    cout << "Enter an arbitrary large integer number: " << endl;
    cin >> i;

    cout << "You entered:\n" << i << endl;
    cout << "i * 2 = " << i * 2 << endl;
    cout << "i / i = " << i / i << endl;
}


#include "dpmeans.hpp"
#include <iostream>
#include <limits>
#include <random>
#include <cmath>
#include <ctime>
#include <unordered_set>
#include <fstream>
#include <vector>

#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

mt19937 rng;

dpmeans::dpmeans(const Ref<const MatrixXf>& X)
{
	K = 1;
	K_init = 4;
	n = X.rows();
	d = X.cols();
	z = VectorXi::Zero(n);
	mu = MatrixXf::Zero(K, d);

	nk = VectorXf::Zero(K);
	pik = VectorXf::Ones(K);

    //init mu
	mu.row(0) = X.colwise().sum() / (float) n;

	cout << "mu = " << endl << mu << endl;
    //init lambda
	lambda = kpp_init(X, K_init);

	max_iter = 100;
	vector<double> obj(max_iter, 0);
	vector<double> em_time(max_iter, 0);
}

float dpmeans::kpp_init(const Ref<const MatrixXf>& X, int k)
{
	// k++ init
	// lambda is max distance to k++ means
	float lambda = 0.0;

	int n = X.rows();
	int d = X.cols();
	MatrixXf mu = MatrixXf::Zero(k, d);

	VectorXf dist = VectorXf::Ones(n);
	VectorXf pdist = VectorXf::Zero(n);
	dist = dist * numeric_limits<float>::max();

	uniform_int_distribution<int> dis0n(0,n-1);
	uniform_real_distribution<float> dis01(0, 1);
	int idx = dis0n(rng);

	mu.row(0) = X.row(idx);
	MatrixXf D = MatrixXf::Zero(n, d);

	for (int i = 1; i < k; ++i)
	{
		D = X - mu.row(i - 1).replicate(n, 1);
		dist = dist.cwiseMin(D.cwiseProduct(D).rowwise().sum());
		//cout << "X = " << endl << X << endl;
		//cout << "mu = " << endl << mu << endl;
		//cout << "dist = " << endl << dist << endl;

		//sample discrete
		pdist = dist / dist.sum();

		for (int j = 1; j < pdist.size(); ++j)
		{
			pdist[j] = pdist[j] + pdist[j - 1];  //cumsum
		}

		int pidx = 0;
		float z01 = dis01(rng);
		for (int item = 0; item < pdist.size(); ++item)
		{
			if (z01 < pdist[item])
			{
				pidx = item;
				break;
			}
		}
		mu.row(i) = X.row(pidx);
		lambda = dist.maxCoeff();
	}

	return lambda;
}

VectorXi dpmeans::dpmeans_fit(const Ref<const MatrixXf>& X)
{

	int n = X.rows();
	int d = X.cols();
	int K = this->K;
	int max_iter = this->max_iter;

	double obj_tol = 1e-3;
	cout << "running dp-means..." << endl;

	//cout << "X = " << endl << X << endl;
	for (int iter = 0; iter < max_iter; ++iter)
	{
		clock_t tic = clock();
		MatrixXf dist = MatrixXf::Zero(n, K);

		//assignment step
		MatrixXf Xm = MatrixXf::Zero(n, d);
		for (int k = 0; k < K; ++k)
		{
			Xm = X - mu.row(k).replicate(n, 1);
			dist.col(k) = Xm.cwiseProduct(Xm).rowwise().sum();
		}
		//cout << "mu = " << endl << mu << endl;
		//cout << "dist = " << endl << dist << endl;

		//update labels
		VectorXf dmin = VectorXf::Zero(n);
		for (int ridx = 0; ridx < n; ++ridx)
		{
			MatrixXf::Index minIndex;
			dmin(ridx) = dist.row(ridx).minCoeff(&minIndex);
			z(ridx) = minIndex;
		}
		//cout << "dmin = " << endl << dmin << endl;
		//cout << "z = " << endl << z << endl;
		//cout << "lambda = " << endl << lambda << endl;

		VectorXi dmin_idx = VectorXi::Zero(n);
		for (int ridx = 0; ridx < n; ++ridx)
		{
			if (dmin(ridx) > lambda)
				dmin_idx(ridx) = 1;
		}
		int num_new = dmin_idx.sum();
		//cout << "num_new = " << endl << num_new << endl;

		if (num_new > 0)
		{
			//create a new cluster for points
			//with dmin > lambda
			K = K + 1;
			//cout << "K = " << endl << K << endl;

			VectorXf new_mean = VectorXf::Zero(d);
			for (int ridx = 0; ridx < n; ++ridx)
			{
				// if assigned to new cluster
				if (dmin_idx(ridx) > 0)
				{
					z(ridx) = K-1;  // cluster labels: [0,...,K-1]
					new_mean = new_mean + X.row(ridx).transpose();
				}
			}
			//cout << "z = " << endl << z << endl;
			mu.conservativeResize(mu.rows() + 1, NoChange);
			//cout << "new mean: " << endl << new_mean << endl;
			//cout << "num_new: " << endl << num_new << endl;
			mu.row(K-1) = new_mean / (float) num_new;  // add new mean

			MatrixXf Xm = MatrixXf::Zero(n, d);
			Xm = X - mu.row(K-1).replicate(n, 1);
			dist.conservativeResize(NoChange, dist.cols() + 1);
			dist.col(K-1) = Xm.cwiseProduct(Xm).rowwise().sum(); //add dist to new mean
			//cout << "dist = " << endl << dist << endl;

		}

		//update step
		nk = VectorXf::Zero(K);
		mu = MatrixXf::Zero(K, d);
		//for (int ridx = 0; ridx < z.size(); ++ridx) { nk(z(ridx))++; } //histogram
		for (int k = 0; k < K; ++k)
		{
			nk(k) = (float) (z.array() == k).count();

			for (int ridx = 0; ridx < z.size(); ++ridx)
			{
				if (z(ridx) == k)
				{
					mu.row(k) = mu.row(k) + X.row(ridx);
				}

			}
			mu.row(k) = mu.row(k) / nk(k);
		}
		pik = nk / nk.sum();
		cout << "mu = " << endl << mu << endl;
		cout << "nk = " << endl << nk << endl;
		cout << "pik = " << endl << pik << endl;

		//compute objective
		int kidx = 0;
		double suml2sq = 0.0;
		for (int ridx = 0; ridx < n; ++ridx)
		{
			kidx = z(ridx);
			suml2sq = suml2sq + dist(ridx, kidx);
		}
		suml2sq += lambda * K;
		obj.push_back(suml2sq);
		cout << "obj = " << endl << suml2sq << endl;

		//check convergence
		if (iter > 0 && abs(obj.at(iter) - obj.at(iter - 1)) < obj_tol *obj.at(iter))
		{
			cout << "converged in " << iter << " iterations." << endl;
			break;
		}

		clock_t toc = clock();
		double elapsed_sec = double(toc - tic) / (double) CLOCKS_PER_SEC;
		cout << "elapsed sec = " << endl << elapsed_sec << endl;
		em_time.push_back(elapsed_sec);
	}

	this->K = K;

	return z;
}

float dpmeans::compute_nmi(const Ref<const VectorXi>& z1, const Ref<const VectorXi>& z2)
{
	// compute normalized mutual information
	float nmi = 0.0;

	int n = z1.size();

	//compute number of unique
	//labels in z1 and z2
	unordered_set<int> s1;
	unordered_set<int> s2;

	int item;
	item = 0;
	for (int idx = 0; idx < n; ++idx)
	{
		item = z1(idx);
		s1.insert(item);
	}

	item = 0;
	for (int idx = 0; idx < n; ++idx)
	{
		item = z2(idx);
		s2.insert(item);
	}

	int k1 = s1.size();
	int k2 = s2.size();

	VectorXf nk1 = VectorXf::Zero(k1);
	VectorXf nk2 = VectorXf::Zero(k2);
	for (int idx = 0; idx < k1; ++idx)
	{
		nk1(idx) = (float) (z1.array() == idx).count();
	}
	for (int idx = 0; idx < k2; ++idx)
	{
		nk2(idx) = (float) (z2.array() == idx).count();
	}

	VectorXf pk1 = nk1 / (float)nk1.sum();
	VectorXf pk2 = nk2 / (float)nk2.sum();

	MatrixXi nk12 = MatrixXi::Zero(k1, k2);
	for (int idx1 = 0; idx1 < k1; ++idx1)
	{
		for (int idx2 = 0; idx2 < k2; ++idx2)
		{
			VectorXi b1 = VectorXi::Zero(n);
			b1 = (z1.array() == idx1).select(VectorXi::Ones(n), VectorXi::Zero(n));

			VectorXi b2 = VectorXi::Zero(n);
			b2 = (z2.array() == idx2).select(VectorXi::Ones(n), VectorXi::Zero(n));

			VectorXi b1b2 = b1.cwiseProduct(b2);

			nk12(idx1, idx2) = b1b2.sum();
		}
	}
	MatrixXf pk12 = nk12.cast<float>() / n;
	cout << "nk12 = " << endl << nk12 << endl;
	cout << "pk12 = " << endl << pk12 << endl;

	VectorXf logpk1 = (pk1 + numeric_limits<float>::epsilon()*VectorXf::Ones(k1)).array().log();
	VectorXf logpk2 = (pk2 + numeric_limits<float>::epsilon()*VectorXf::Ones(k2)).array().log();
	MatrixXf logpk12 = (pk12 + numeric_limits<float>::epsilon()*MatrixXf::Ones(k1, k2)).array().log();

	float Hx = -pk1.dot(logpk1);
	float Hy = -pk2.dot(logpk2);
	float Hxy = -pk12.cwiseProduct(logpk12).sum();

	float MI = Hx + Hy - Hxy;
	nmi = MI / (float) (0.5*(Hx + Hy));

	return nmi;
}

void dpmeans::display_params()
{
	cout << "K = " << this->K << endl;
	cout << "K_init = " << this->K_init << endl;
	//cout << "Labels: " << endl << this->z << endl;
	cout << "Means: " << endl << this->mu << endl;
	cout << "Counts: " << endl << this->nk << endl;
	cout << "Proportions: " << endl << this->pik << endl;
	cout << "Lambda: " << this->lambda << endl;
}

void load_iris(Ref<MatrixXf> X, Ref<VectorXi> y)
{

	//read-in labels
	int cnt = 0;
	string line;
	ifstream fin2("./data/iris_labels.txt");
	if (fin2.is_open())
	{
		while (getline(fin2, line))
		{
			y(cnt) = stoi(line);
			cnt++;
		}
		fin2.close();
	}
	else cout << "Unable to open fin2\n";
	//cout << "y = " << endl << y << endl;
	//cout << "y.size = " << endl << y.size() << endl;

	//read-in data
	cnt = 0;
	int nrows = 0;
	int ncols = 0;
	ifstream fin3("./data/iris_data.txt");
	if (fin3.is_open())
	{
		fin3 >> nrows >> ncols;
		for (int row = 0; row < nrows; row++)
			for (int col = 0; col < ncols; col++)
			{
				float num = 0.0;
				fin3 >> num;
				X(row, col) = num;
			}
		fin3.close();
	}
	else cout << "Unable to open fin3\n";
	//cout << "X = " << endl << X << endl;

}

int main(int argc, char* argv[])
{

	//generate data
	//int rows = 150, cols = 4;
	//MatrixXf X = MatrixXf::Random(rows, cols);

	//load iris
	int nrows = 0;
	int ncols = 0;
	string line;
	ifstream fin1("./data/iris_data.txt");
	if (fin1.is_open())
	{
		fin1 >> nrows >> ncols;
		fin1.close();
	}
	else cout << "Unable to open data file\n";
	cout << "nrows = " << nrows << endl;
	cout << "ncols = " << ncols << endl;

	VectorXi y = VectorXi::Zero(nrows);
	MatrixXf X = MatrixXf::Zero(nrows, ncols);

	load_iris(X,y);
	//cout << "y = " << endl << y << endl;
	//cout << "X = " << endl << X << endl;

	dpmeans dp(X);
	dp.display_params();

	VectorXi z;
	z = dp.dpmeans_fit(X);
	//cout << "z = " << endl << z << endl;

	float nmi;
	nmi = dp.compute_nmi(z, y);
	cout << "nmi = " << endl << nmi << endl;

	return 0;
}


/*******************************************************************************
 * examples/tutorial/k-means_step6.cpp
 *
 * Part of Project Thrill - http://project-thrill.org
 *
 * Copyright (C) 2016 Timo Bingmann <tb@panthema.net>
 *
 * All rights reserved. Published under the BSD-2 license in the LICENSE file.
 ******************************************************************************/

//! \example examples/tutorial/k-means_step6.cpp
//!
//! This example is part of the k-means tutorial. See \ref kmeans_tutorial_step6

#include <thrill/api/all_gather.hpp>
#include <thrill/api/cache.hpp>
#include <thrill/api/generate.hpp>
#include <thrill/api/print.hpp>
#include <thrill/api/read_lines.hpp>
#include <thrill/api/reduce_by_key.hpp>
#include <thrill/api/sample.hpp>
#include <thrill/api/write_lines.hpp>

// Boost Spirit Qi is a header-only library
#include <boost/spirit/include/qi.hpp>

#include <ostream>
#include <random>
#include <sstream>
#include <string>
#include <vector>

//! A 2-dimensional point with double precision
struct Point {
    //! point coordinates
    double x, y;

    double DistanceSquare(const Point& b) const {
        return (x - b.x) * (x - b.x) + (y - b.y) * (y - b.y);
    }
    Point operator + (const Point& b) const {
        return Point { x + b.x, y + b.y };
    }
    Point operator / (double s) const {
        return Point { x / s, y / s };
    }
};

//! make ostream-able for Print()
std::ostream& operator << (std::ostream& os, const Point& p) {
    return os << '(' << p.x << ',' << p.y << ')';
}

//! Assignment of a point to a cluster.
struct ClosestCenter {
    size_t cluster_id;
    Point  point;
    size_t count;
};
//! make ostream-able for Print()
std::ostream& operator << (std::ostream& os, const ClosestCenter& cc) {
    return os << '(' << cc.cluster_id
              << ':' << cc.point << ':' << cc.count << ')';
}

//! our main processing method
void Process(const thrill::DIA<Point>& points, const char* output) {

    // print out the points
    // points.Print("points");

    // pick some initial random cluster centers
    thrill::DIA<Point> centers = points.Sample(/* num_clusters */ 10);

    for (size_t iter = 0; iter < /* iterations */ 10; ++iter)
    {
        // collect centers in a local vector on each worker
        std::vector<Point> local_centers = centers.AllGather();

        auto new_centers =
            points
            // calculate the closest center for each point
            .Map(
                [local_centers](const Point& p) {
                    double min_dist = p.DistanceSquare(local_centers[0]);
                    size_t cluster_id = 0;

                    for (size_t i = 1; i < local_centers.size(); ++i) {
                        double dist = p.DistanceSquare(local_centers[i]);
                        if (dist < min_dist)
                            min_dist = dist, cluster_id = i;
                    }
                    return ClosestCenter { cluster_id, p, /* count */ 1 };
                })
            // new centers as the mean of all points associated with it
            .ReduceByKey(
                // key extractor: the cluster id
                [](const ClosestCenter& cc) { return cc.cluster_id; },
                // reduction: add points and the counter
                [](const ClosestCenter& a, const ClosestCenter& b) {
                    return ClosestCenter {
                        a.cluster_id, a.point + b.point, a.count + b.count
                    };
                })
            .Map([](const ClosestCenter& cc) {
                     return cc.point / cc.count;
                 });

        // new_centers.Print("new_centers");

        // Collapse() is needed to fold lambda chain to DIA<Points>
        centers = new_centers.Collapse();
    }

    if (output) {
        // write output as "x y" lines
        centers
        .Map([](const Point& p) {
                 return std::to_string(p.x) + " " + std::to_string(p.y);
             })
        .WriteLines(output);
    }
    else {
        centers.Print("final centers");
    }
}

thrill::DIA<Point> GeneratePoints(thrill::Context& ctx) {
    std::default_random_engine rng(std::random_device { } ());
    std::uniform_real_distribution<double> dist(0.0, 1000.0);

    // generate 100 random points using uniform distribution
    auto points =
        Generate(
            ctx, /* size */ 100,
            [&](const size_t&) {
                return Point { dist(rng), dist(rng) };
            });
    // Execute() is require due to lazy evaluation
    return points.Cache().Execute();
}

//! [step6 LoadPoints]
thrill::DIA<Point> LoadPoints(thrill::Context& ctx, const char* path) {

    // shorthand namespace
    namespace qi = boost::spirit::qi;

    // load points from text file
    auto points =
        ReadLines(ctx, path)
        .Map(
            [](const std::string& input) {
                // parse "<x> <y>" lines
                Point p;
                std::string::const_iterator begin = input.begin(), end = input.end();

                qi::phrase_parse(
                    begin, end,                 // iterators
                    qi::double_ >> qi::double_, // parser grammar: two doubles
                    qi::ascii::space,           // skip grammar: spaces
                    p.x, p.y);                  // put directly into the Point

                if (begin != end)               // check that fully parsed
                    die("Could not parse point coordinates: " << input);
                return p;
            });
    return points.Cache();
}
//! [step6 LoadPoints]

int main(int argc, char* argv[]) {
    // launch Thrill program: the lambda function will be run on each worker.
    return thrill::Run(
        [&](thrill::Context& ctx) {
            if (argc == 1)
                Process(GeneratePoints(ctx), nullptr);
            else if (argc == 2)
                Process(LoadPoints(ctx, argv[1]), nullptr);
            else if (argc == 3)
                Process(LoadPoints(ctx, argv[1]), argv[2]);
            else
                std::cerr << "Usage: " << argv[0]
                          << " [points] [output]" << std::endl;
        });
}

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


// This is an interactive demo, so feel free to change the code and click the 'Run' button.

// This simple program uses the mlpack::neighbor::NeighborSearch object
// to find the nearest neighbor of each point in a dataset using the L1 metric,
// and then print the index of the neighbor and the distance of it to stdout.

#include <mlpack/core.hpp>
#include <mlpack/methods/ann/layer/layer.hpp>
#include <mlpack/methods/ann/ffn.hpp>
#include <mlpack/methods/neighbor_search/neighbor_search.hpp>
#include <armadillo>
#include <iostream>

using namespace mlpack;
using namespace mlpack::neighbor; // NeighborSearch and NearestNeighborSort
using namespace mlpack::metric; // ManhattanDistance
using namespace mlpack::ann;

int main() {
    std::cout << "MLPack Example " << std::endl;
    // Load the data from data.csv (hard-coded).  Use CLI for simple command-line
    // parameter handling.
    arma::mat data("0.339406815,0.843176636,0.472701471; \
                  0.212587646,0.351174901,0.81056695;  \
                  0.160147626,0.255047893,0.04072469;  \
                  0.564535197,0.943435462,0.597070812");
    data = data.t();
    std::cout << data << std::endl;

    // Use templates to specify that we want a NeighborSearch object which uses
    // the Manhattan distance.
    NeighborSearch<NearestNeighborSort, ManhattanDistance> nn(data);

    // Create the object we will store the nearest neighbors in.
    arma::Mat<size_t> neighbors;
    arma::mat distances; // We need to store the distance too.

    // Compute the neighbors.
    nn.Search(1, neighbors, distances);

    // Write each neighbor and distance using Log.
    for (size_t i = 0; i < neighbors.n_elem; ++i) {
        std::cout << "Nearest neighbor of point " << i << " is point "
                  << neighbors[i] << " and the distance is " << distances[i] << "." << std::endl;
    }

    /// next step
    // Load the training set and testing set.
    arma::mat trainData;
    data::Load("/Users/zachbortoff/Downloads/mlpack/build/thyroid_train.csv", trainData, true);
    arma::mat testData;
    data::Load("/Users/zachbortoff/Downloads/mlpack/build/thyroid_test.csv", testData, true);
    // Split the labels from the training set and testing set respectively.
    arma::mat trainLabels = trainData.row(trainData.n_rows - 1);
    arma::mat testLabels = testData.row(testData.n_rows - 1);
    trainData.shed_row(trainData.n_rows - 1);
    testData.shed_row(testData.n_rows - 1);
    // Initialize the network.
    FFN<> model;
    model.Add<Linear<> >(trainData.n_rows, 8);
    model.Add<SigmoidLayer<> >();
    model.Add<Linear<> >(8, 3);
    model.Add<LogSoftMax<> >();
    // Train the model.
    model.Train(trainData, trainLabels);
    // Use the Predict method to get the predictions.
    arma::mat predictionTemp;
    model.Predict(testData, predictionTemp);
    /*
      Since the predictionsTemp is of dimensions (3 x number_of_data_points)
      with continuous values, we first need to reduce it to a dimension of
      (1 x number_of_data_points) with scalar values, to be able to compare with
      testLabels.
      The first step towards doing this is to create a matrix of zeros with the
      desired dimensions (1 x number_of_data_points).
      In predictionsTemp, the 3 dimensions for each data point correspond to the
      probabilities of belonging to the three possible classes.
    */
    arma::mat prediction = arma::zeros<arma::mat>(1, predictionTemp.n_cols);
    // Find index of max prediction for each data point and store in "prediction"
    for (size_t i = 0; i < predictionTemp.n_cols; ++i)
    {
        // we add 1 to the max index, so that it matches the actual test labels.
        prediction(i) = arma::as_scalar(arma::find(
                arma::max(predictionTemp.col(i)) == predictionTemp.col(i), 1)) + 1;
    }
    /*
      Compute the error between predictions and testLabels,
      now that we have the desired predictions.
    */
    size_t correct = arma::accu(prediction == testLabels);
    double classificationError = 1 - double(correct) / testData.n_cols;
    // Print out the classification error for the testing dataset.
    std::cout << "Classification Error for the Test set: " << classificationError << std::endl;

    return 0;
}