#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


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

	}
}


/* 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";
   }
}


/**
 * @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 "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;
        });
}

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


#include <cmath>
#include <memory>
#include <fstream>
#include <set>
#include <numeric>
#include <random>
#include <iostream>

#include <CandyPretty/CandyPretty.h>

#include <boost/exception/all.hpp>
#include <boost/variant.hpp>

#if 0

/*
                D(t)V(t) = E~(D(T)V(T)|F(T))
 */

/*
        This is the analytic solution to geometric brownian motion with drift

                dS = a S dt + b S dw

 */

#include <ql/pricingengines/blackcalculator.hpp>

struct Differential{
        virtual ~Differential()=default;
        /*
                dx = f(x,dt,dw)
                x(t + dt ) = x(t) + dx(t)
         */
        virtual double Eval(double x, double dt, double std_norm)const=0;
};

struct ProcessContext;

struct ProcessIntegral{
        ProcessIntegral()=default;
        ProcessIntegral(ProcessContext& ctx, double x, std::shared_ptr<Differential> dx);
        void SmallChange(double dt, double std_norm){
                x_ += dx_->Eval( x_, dt, std_norm );
        }
        double Value()const{ return x_; }
private:
        double x_;
        std::shared_ptr<Differential> dx_;
};

struct ProcessContext{
        void Register(ProcessIntegral* ptr){
                procs_.push_back(ptr);
        }
        void Step(double dt){
                auto std_norm = [&](){ return D_(G_); };
                for(auto ptr : procs_){
                        ptr->SmallChange(dt, std_norm());
                }
        }
private:
        #if 0
        std::default_random_engine G_;
        #else
        std::random_device G_;
        #endif
        std::normal_distribution<double> D_{0.0, 1.0};
        std::vector<ProcessIntegral*> procs_;
};

inline ProcessIntegral::ProcessIntegral(ProcessContext& ctx, double x, std::shared_ptr<Differential> dx)
        :x_(x),
        dx_(dx)
{
        ctx.Register(this);
}

struct ProcessView{
        struct Impl{
                virtual ~Impl()=default;
                virtual double Value()const=0;
        };
        struct SptrImpl : Impl{
                explicit SptrImpl(std::shared_ptr<ProcessIntegral const> q_) :q(q_) {}
                virtual double Value()const{ return q->Value(); }
                std::shared_ptr<ProcessIntegral const> q;
        };

        ProcessView()=default;
        ProcessView(std::shared_ptr<ProcessIntegral> p){
                impl_ = std::make_shared<SptrImpl>(p);
        }
        // assumes objects lifetime exists for as long as it'self
        ProcessView(ProcessIntegral const& p){
                std::shared_ptr<ProcessIntegral const> aux(&p, [](auto*){});
                impl_ = std::make_shared<SptrImpl>(aux);
        }

        ProcessView& operator=(std::shared_ptr<ProcessIntegral> p){
                impl_ = std::make_shared<SptrImpl>(p);
                return *this;
        }
        ProcessView& operator=(ProcessIntegral const& p){
                std::shared_ptr<ProcessIntegral const> aux(&p, [](auto*){});
                impl_ = std::make_shared<SptrImpl>(aux);
                return *this;
        }

        double Value()const{ return impl_->Value(); }

        // for printing to csv etc
        // this is how it is set, p.Name()  = "Discount ProcessIntegral()"
        std::string& Name(){ return name_; }
        std::string const& Name()const{ return name_; }
protected:
        std::shared_ptr<Impl> impl_;
        std::string name_{"ProcessIntegral"};
};

struct IdentityDifferential : Differential{
        virtual double Eval(double x, double dt, double std_norm)const override{
                return dt;
        }
};

struct BankAccountDifferential : Differential{
        explicit BankAccountDifferential(ProcessView interest_rate)
                :interest_rate_(interest_rate)
        {}
        virtual double Eval(double x, double dt, double std_norm)const override{
                return x * interest_rate_.Value() * dt;
        }
private:
        ProcessView interest_rate_;
};

struct GeometricBrownianMotionWithDriftDifferential : Differential{
        GeometricBrownianMotionWithDriftDifferential(double S0, double r, double sigma)
                :S0_(S0),
                r_(r),
                sigma_(sigma)
        {}
        virtual double Eval(double x, double dt, double std_norm)const override{
                double a = r_ * dt + sigma_ * std_norm * std::sqrt(dt);
                double b = a * x;
                return b;
        }
private:
        double S0_;
        double r_;
        double sigma_;
};

struct VasicekDifferential : Differential{
        VasicekDifferential(double alpha, double beta, double sigma)
                :alpha_(alpha),
                beta_(beta),
                sigma_(sigma)
        {}
        virtual double Eval(double x, double dt, double std_norm)const override{
                double a = ( alpha_ - beta_ * x ) * dt;
                double b = sigma_ * std_norm * std::sqrt(dt);
                double c = a + b;
                return c;
        }
private:
        double alpha_;
        double beta_;
        double sigma_;
};
struct CoxIngersollRos : Differential{
        CoxIngersollRos(double alpha, double beta, double sigma)
                :alpha_(alpha),
                beta_(beta),
                sigma_(sigma)
        {}
        virtual double Eval(double x, double dt, double std_norm)const override{
                double a = ( alpha_ - beta_ * x ) * dt;
                double b = sigma_ * std::sqrt(x) *  std_norm * std::sqrt(dt);
                double c = a + b;
                return c;
        }
private:
        double alpha_;
        double beta_;
        double sigma_;
};

struct AnaBlack : ProcessView{
        AnaBlack(ProcessView t){
                struct AnaBlackImpl : Impl{
                        AnaBlackImpl(ProcessView t)
                                :t_(t)
                        {}
                        virtual double Value()const override{
                                double r = 0.02;
                                double vol = 0.1;
                                double s0 = 10.0;
                                double k = 1.5 * s0;

                                double T = t_.Value();
                                auto discount = std::exp( -r * T );
                                auto fwd = s0 / discount;
                                auto std_dev = std::sqrt( vol * vol * T);
                                QuantLib::BlackCalculator bc(QuantLib::Option::Call,
                                                             k,
                                                             fwd,
                                                             std_dev,
                                                             discount);
                                return bc.value();
                        }
                private:
                        ProcessView t_;
                };
                impl_ = std::make_shared<AnaBlackImpl>(t);
        }
};

struct DiscountProcess : ProcessView{
        DiscountProcess(ProcessView t, double r){
                struct DPImpl : Impl{
                        DPImpl(ProcessView t_, double r_)
                                :t(t_),
                                r(r_)
                        {}
                        virtual double Value()const override{
                                return std::exp( - t.Value() * r );
                        }
                private:
                        ProcessView t;
                        double r;
                };
                impl_ = std::make_shared<DPImpl>(t,r);
        }
};

struct AverageView : ProcessView{
        struct Final : Impl{
                virtual double Value()const override{
                        size_t n = v_.size();
                        double sigma = 0.0;
                        for(size_t idx=0;idx!=n;++idx){
                                sigma +=  v_[idx].Value();
                        }
                        return sigma / n;
                }
        private:
                friend struct AverageView;
                std::vector<ProcessView> v_;
        };
        AverageView(){
                impl_ = std::make_shared<Final>();
        }
        AverageView& Add(ProcessView view){
                auto casted = dynamic_cast<Final*>(impl_.get());
                casted->v_.push_back(view);
                return *this;
        }
        template<class Iter>
        AverageView(Iter first, Iter last){
                impl_ = std::make_shared<Final>();
                for(;first!=last;++first){
                        Add(*first);
                }
        }
};

struct Option : ProcessView{
        Option(ProcessView process, double strike){
                struct OptionImpl : Impl{
                        OptionImpl(ProcessView process, double strike)
                                :process_(process),
                                strike_(strike)
                        {}
                        virtual double Value()const override{
                                return (std::max)(process_.Value() - strike_, 0.0);
                        }
                private:
                        ProcessView process_;
                        double strike_;
                };
                impl_ = std::make_shared<OptionImpl>(process, strike);
        }
};

struct ProcessViewRenderer{
        ProcessViewRenderer(std::ostream& out, std::vector<ProcessView> const& views)
                :out_{std::shared_ptr<std::ostream>(&out, [](auto*){})}, views_(views)
        {
                EmitHeader_();
        }
        ProcessViewRenderer(std::shared_ptr<std::ostream> out, std::vector<ProcessView> const& views)
                :out_{out}, views_(views)
        {
                EmitHeader_();
        }
        void RenderLine(){
                std::vector<std::string> line;
                for(auto const& view : views_){
                        line.push_back(boost::lexical_cast<std::string>(view.Value()));
                }
                lines_.push_back(std::move(line));
        }
        void Emit(){
                CandyPretty::RenderTablePretty(*out_, lines_, opts_);
        }
private:
        void EmitHeader_(){
                std::vector<std::string> line;
                for(auto const& view : views_){
                        line.push_back(boost::lexical_cast<std::string>(view.Name()));
                }
                lines_.push_back(std::move(line));
        }
        CandyPretty::RenderOptions opts_{CandyPretty::RenderOptions::CsvOptions()};
        std::shared_ptr<std::ostream> out_;
        std::vector<ProcessView> views_;
        std::vector<CandyPretty::LineItem> lines_;
};

void example_0(){
        using namespace CandyPretty;

        double r = 0.02;
        double vol = 0.1;
        double T = 20;
        double s0 = 10.0;

        auto gbm = std::make_shared<GeometricBrownianMotionWithDriftDifferential>(s0, r, vol);

        enum{ SampleSize = 4000 };
        ProcessContext ctx;
        auto t = std::make_shared<ProcessIntegral>(ctx, 0, std::make_shared<IdentityDifferential>() );

        std::vector<std::shared_ptr<ProcessIntegral> > gbm_sample(SampleSize);
        for(size_t idx=0;idx!=SampleSize;++idx){
                gbm_sample[idx] = std::make_shared<ProcessIntegral>(ctx, s0, gbm);
        }

        DiscountProcess disc(t, r);

        std::vector<AverageView> avg_vec;
        auto first = &gbm_sample[0];
        avg_vec.emplace_back( first, first + 10);
        avg_vec.back().Name() = "Avg_{10}";
        avg_vec.emplace_back( first, first + 100);
        avg_vec.back().Name() = "Avg_{100}";
        avg_vec.emplace_back( first, first + 1000);
        avg_vec.back().Name() = "Avg_{1000}";
        avg_vec.emplace_back( first, first + 2000);
        avg_vec.back().Name() = "Avg_{2000}";
        avg_vec.emplace_back( first, first + 4000);
        avg_vec.back().Name() = "Avg_{4000}";

        std::vector<ProcessView> views;
        views.push_back(t);
        views.back().Name() = "t";
        views.push_back(disc);
        views.back().Name() = "D(t)";
        for(auto const& _ : avg_vec){
                views.push_back(_);
        }
        enum{ GbmViews = 20 };
        for(size_t idx=0;idx < gbm_sample.size() && idx < GbmViews;++idx){
                views.push_back(gbm_sample[idx]);
                std::stringstream sstr;
                sstr << "W_{" << idx << "}(t)";
                views.back().Name() = sstr.str();
        }

        std::vector<ProcessView>* render_view = &views;

        #if 0

        AnaBlack black( t);

        std::vector<ProcessView> call_view;
        call_view.push_back(t);
        call_view.push_back(disc);
        call_view.push_back(black);

        std::vector<ProcessView> call_options(SampleSize);
        std::vector<std::pair<AverageView, size_t> > call_avg;
        call_avg.emplace_back(AverageView(), 10);
        call_avg.emplace_back(AverageView(), 100);
        call_avg.emplace_back(AverageView(), 1000);
        call_avg.emplace_back(AverageView(), 2000);
        call_avg.emplace_back(AverageView(), 4000);
        for(auto& _ : call_avg){
                call_view.push_back(_.first);
        }
        for(size_t idx=0;idx!=SampleSize;++idx){
                auto& p = gbm_sample[idx];
                call_options[idx] = Option(p, k);
                for(auto& _ : call_avg){
                        if( idx < _.second ){
                                _.first.Add(call_options[idx]);
                        }
                }
        }

        double ir_0 = 0.05;
        auto f = 10.0;
        auto ir_diff = std::make_shared<VasicekDifferential>(1/f, 20/f, 0.1);
        std::vector<ProcessIntegral> bank_acct_sample(SampleSize);
        std::vector<ProcessView> ir_view;
        AverageView bank_acct_avg;
        AverageView ir_avg;

        ir_view.push_back(t);
        ir_view.push_back(ir_avg);
        ir_view.push_back(bank_acct_avg);

        for(size_t idx=0;idx!=SampleSize;++idx){
                auto irp_p = std::make_shared<ProcessIntegral>(ir_0, ir_diff);
                ProcessView irp(irp_p);
                procs.push_back(irp_p.get());
                auto bank_acct_diff = std::make_shared<BankAccountDifferential>(irp);
                auto bank_acct_p = std::make_shared<ProcessIntegral>(1.0, bank_acct_diff);
                ProcessView bank_acct(bank_acct_p);
                procs.push_back(bank_acct_p.get());

                if( idx < GbmViews ){
                        ir_view.push_back(irp);
                        ir_view.push_back( bank_acct);
                }
                ir_avg.Add(irp);
                bank_acct_avg.Add(bank_acct);
        }
        #endif

        size_t N = 1000;
        double dt = T / N;

        std::ofstream of{"RiskNeutralBrownianMotion.csv"};
        if( ! of.is_open() )
                BOOST_THROW_EXCEPTION(std::domain_error("unable to open RiskNeutralBrownianMotion.csv"));
        ProcessViewRenderer renderer{of, *render_view};

        for(size_t idx=0;idx!=N;++idx){
                ctx.Step(dt);
                renderer.RenderLine();
        }
        renderer.Emit();
}

void example_1(){
        using namespace CandyPretty;

        double r = 0.02;
        double vol = 0.1;
        double T = 40;
        double s0 = 10.0;
        double k = 1.5 * s0;

        auto gbm = std::make_shared<GeometricBrownianMotionWithDriftDifferential>(s0, r, vol);

        enum{ SampleSize = 4000 };
        ProcessContext ctx;
        auto t = std::make_shared<ProcessIntegral>(ctx, 0, std::make_shared<IdentityDifferential>() );

        std::vector<std::shared_ptr<ProcessIntegral> > gbm_sample(SampleSize);
        std::vector<ProcessView> call_options(SampleSize);
        for(size_t idx=0;idx!=SampleSize;++idx){
                gbm_sample[idx] = std::make_shared<ProcessIntegral>(ctx, s0, gbm);
                call_options[idx] = Option(gbm_sample[idx], k);
        }

        DiscountProcess disc(t, r);

        AnaBlack black( t);

        std::vector<AverageView> avg_vec;
        auto first = &call_options[0];
        avg_vec.emplace_back( first, first + 10);
        avg_vec.back().Name() = "Avg_{10}";
        avg_vec.emplace_back( first, first + 100);
        avg_vec.back().Name() = "Avg_{100}";
        avg_vec.emplace_back( first, first + 1000);
        avg_vec.back().Name() = "Avg_{1000}";
        avg_vec.emplace_back( first, first + 2000);
        avg_vec.back().Name() = "Avg_{2000}";
        avg_vec.emplace_back( first, first + 4000);
        avg_vec.back().Name() = "Avg_{4000}";

        std::vector<ProcessView> views;
        views.push_back(t);
        views.back().Name() = "t";
        views.push_back(disc);
        views.back().Name() = "D(t)";
        views.push_back(black);
        views.back().Name() = "BS(.)";
        for(auto const& _ : avg_vec){
                views.push_back(_);
        }
        enum{ GbmViews = 20 };
        for(size_t idx=0;idx < call_options.size() && idx < GbmViews;++idx){
                views.push_back(call_options[idx]);
                std::stringstream sstr;
                sstr << "Call_{" << idx << "}(t)";
                views.back().Name() = sstr.str();
        }

        std::vector<ProcessView>* render_view = &views;

        #if 0

        std::vector<ProcessView> call_view;
        call_view.push_back(t);
        call_view.push_back(disc);
        call_view.push_back(black);

        std::vector<ProcessView> call_options(SampleSize);
        std::vector<std::pair<AverageView, size_t> > call_avg;
        call_avg.emplace_back(AverageView(), 10);
        call_avg.emplace_back(AverageView(), 100);
        call_avg.emplace_back(AverageView(), 1000);
        call_avg.emplace_back(AverageView(), 2000);
        call_avg.emplace_back(AverageView(), 4000);
        for(auto& _ : call_avg){
                call_view.push_back(_.first);
        }
        for(size_t idx=0;idx!=SampleSize;++idx){
                auto& p = gbm_sample[idx];
                call_options[idx] = Option(p, k);
                for(auto& _ : call_avg){
                        if( idx < _.second ){
                                _.first.Add(call_options[idx]);
                        }
                }
        }

        double ir_0 = 0.05;
        auto f = 10.0;
        auto ir_diff = std::make_shared<VasicekDifferential>(1/f, 20/f, 0.1);
        std::vector<ProcessIntegral> bank_acct_sample(SampleSize);
        std::vector<ProcessView> ir_view;
        AverageView bank_acct_avg;
        AverageView ir_avg;

        ir_view.push_back(t);
        ir_view.push_back(ir_avg);
        ir_view.push_back(bank_acct_avg);

        for(size_t idx=0;idx!=SampleSize;++idx){
                auto irp_p = std::make_shared<ProcessIntegral>(ir_0, ir_diff);
                ProcessView irp(irp_p);
                procs.push_back(irp_p.get());
                auto bank_acct_diff = std::make_shared<BankAccountDifferential>(irp);
                auto bank_acct_p = std::make_shared<ProcessIntegral>(1.0, bank_acct_diff);
                ProcessView bank_acct(bank_acct_p);
                procs.push_back(bank_acct_p.get());

                if( idx < GbmViews ){
                        ir_view.push_back(irp);
                        ir_view.push_back( bank_acct);
                }
                ir_avg.Add(irp);
                bank_acct_avg.Add(bank_acct);
        }
        #endif

        size_t N = 1000;
        double dt = T / N;

        std::ofstream of{"CallOption.csv"};
        if( ! of.is_open() )
                BOOST_THROW_EXCEPTION(std::domain_error("unable to open CallOption.csv"));
        ProcessViewRenderer renderer{of, *render_view};

        for(size_t idx=0;idx!=N;++idx){
                ctx.Step(dt);
                renderer.RenderLine();
        }
        renderer.Emit();
}

void example_2(){
        using namespace CandyPretty;

        double r = 0.02;
        double vol = 0.1;
        double T = 40;
        double s0 = 10.0;

        enum{ SampleSize = 4000 };

        ProcessContext ctx;

        auto t = std::make_shared<ProcessIntegral>(ctx, 0, std::make_shared<IdentityDifferential>() );

        std::vector<ProcessView> interest_rate_samples(SampleSize);
        std::vector<ProcessView> bank_account_samples(SampleSize);

        double ir_0 = 0.05;
        auto f = 10.0;
        auto ir_diff = std::make_shared<VasicekDifferential>(1/f, 20/f, 0.1);

        for(size_t idx=0;idx!=SampleSize;++idx){
                interest_rate_samples[idx] = std::make_shared<ProcessIntegral>(ctx, ir_0, ir_diff);
                auto bank_acct_diff = std::make_shared<BankAccountDifferential>(interest_rate_samples[idx]);
                bank_account_samples[idx]  = std::make_shared<ProcessIntegral>(ctx, 1.0, bank_acct_diff);
        }

        std::vector<AverageView> avg_vec;
        auto first = &bank_account_samples[0];
        avg_vec.emplace_back( first, first + 10);
        avg_vec.back().Name() = "Avg_{10}";
        avg_vec.emplace_back( first, first + 100);
        avg_vec.back().Name() = "Avg_{100}";
        avg_vec.emplace_back( first, first + 1000);
        avg_vec.back().Name() = "Avg_{1000}";
        avg_vec.emplace_back( first, first + 2000);
        avg_vec.back().Name() = "Avg_{2000}";
        avg_vec.emplace_back( first, first + 4000);
        avg_vec.back().Name() = "Avg_{4000}";

        std::vector<ProcessView> views;
        views.push_back(t);
        views.back().Name() = "t";
        for(auto const& _ : avg_vec){
                views.push_back(_);
        }
        enum{ GbmViews = 20 };
        for(size_t idx=0;idx < SampleSize && idx < GbmViews;++idx){
                std::stringstream sstr;
                views.push_back(interest_rate_samples[idx]);
                sstr << "R_{" << idx << "}(t)";
                views.back().Name() = sstr.str();
                views.push_back(bank_account_samples[idx]);
                sstr.str("");
                sstr << "B_{" << idx << "}(t)";
                views.back().Name() = sstr.str();
        }

        std::vector<ProcessView>* render_view = &views;

        size_t N = 1000;
        double dt = T / N;

        std::ofstream of{"BankAccount.csv"};
        if( ! of.is_open() )
                BOOST_THROW_EXCEPTION(std::domain_error("unable to open BankAccount.csv"));
        ProcessViewRenderer renderer{of, *render_view};

        for(size_t idx=0;idx!=N;++idx){
                ctx.Step(dt);
                renderer.RenderLine();
        }
        renderer.Emit();
}

#endif

struct Omega{};
struct Nul{};
struct Not;
struct Union;
struct Intersection;
struct Interval;

using BorelSet = boost::variant<
        Omega,
        Nul,
        boost::recursive_wrapper<Not>,
        boost::recursive_wrapper<Union>,
        boost::recursive_wrapper<Intersection>,
        boost::recursive_wrapper<Interval>
>;

struct Not{
        BorelSet child;
};
struct Union{
        template<class... Args>
        Union(Args&&... args):children{args...}{}

        void Add(Union const& that){
                for(auto const& _ : that.children)
                        children.push_back(_);
        }

        std::vector<BorelSet> children;
};
struct Intersection{
        template<class... Args>
        Intersection(Args&&... args):children{args...}{}

        void Add(Intersection const& that){
                for(auto const& _ : that.children)
                        children.push_back(_);
        }

        std::vector<BorelSet> children;
};
struct IntervalEndPoint{
        friend std::ostream& operator<<(std::ostream& ostr, IntervalEndPoint const& self){
                ostr << "IntervalEndPoint{is_open = " << self.is_open;
                ostr << ", point = " << self.point << "}";
                return ostr;
        }
        bool operator<(IntervalEndPoint const& that)const{
                if( point != that.point )
                        return point < that.point;
                return is_open < that.is_open;
        }
        bool operator==(IntervalEndPoint const& that)const{
                return point == that.point && is_open == that.is_open;
        }
        bool operator!=(IntervalEndPoint const& that)const{
                return ! operator==(that);
        }
        bool is_open;
        double point;

        IntervalEndPoint Switch()const{ return IntervalEndPoint{ ! is_open, point }; }
};
IntervalEndPoint Open(double x){
        return IntervalEndPoint{true, x};
}
IntervalEndPoint Closed(double x){
        return IntervalEndPoint{false, x};
}

struct IntervalUnion{
        template<class... Args>
        IntervalUnion(Args&&... args):children{args...}{}

        void Add(IntervalUnion const& that){
                for(auto const& _ : that.children)
                        children.push_back(_);
        }

        std::vector<Interval> children;

        operator Union()const{
                return AsUnion();
        }
        Union AsUnion()const{
                Union tmp;
                for(auto const& _ : children )
                        tmp.children.push_back(_);
                return tmp;
        }

        #if 0
        bool operator==(IntervalUnion const& that)const{
                return children == that.children;
        }
        bool operator!=(IntervalUnion const& that)const{
                return ! operator==(that);
        }
        #endif
        bool operator<(IntervalUnion const& that)const{
                if( children.size() != that.children.size() ){
                        return  children.size() < that.children.size();
                }
                return children < that.children;
        }
};

struct Interval{
        IntervalEndPoint left;
        IntervalEndPoint right;

        static Interval Closed(double x, double y){
                return Interval{ IntervalEndPoint{ false, x},
                                 IntervalEndPoint{ false, y} };
        }
        static Interval Open(double x, double y){
                return Interval{ IntervalEndPoint{ false, x},
                                 IntervalEndPoint{ false, y} };
        }

        bool operator<(Interval const& that)const{
                if( left != that.left )
                        return left < that.left;
                return right < that.right;
        }

        // homogenous operations are here
        IntervalUnion Not()const{
                IntervalUnion result;

                /*
                        A) this  \subset world => this
                        B) world \subset this => that

                 */

                if( left.point > right.point )
                        return result;

                if( 0.0 == left.point && left.is_open ){
                        result.children.push_back(Interval::Closed(0.0, 0.0));
                } else if( 0.0 < left.point ){
                        result.children.push_back(Interval{ IntervalEndPoint{false, 0.0},
                                                   left.Switch() } );
                }

                if( 1.0 == right.point && right.is_open ){
                        result.children.push_back(Interval::Closed(1.0, 1.0));
                } else if( right.point < 1.0 ){
                        result.children.push_back(Interval{ right.Switch(),
                                                   IntervalEndPoint{false, 1.0} } );
                }

                return result;
        }

        bool IsSubsetOf(Interval const& that)const{
                if( that.left.point > left.point )
                        return false;
                if( that.left.point ==left.point ){
                        if( that.left.is_open != left.is_open && that.left.is_open )
                                return false;
                }
                if( that.right.point < right.point )
                        return false;
                if( that.right.point ==right.point ){
                        if( that.right.is_open != right.is_open && that.right.is_open )
                                return false;
                }
                return true;
        }

};

struct SortIntervalUnions{
        bool operator()(Union const& a, Union const& b)const{
                if( a.children.size() != b.children.size() )
                        return a.children.size() < b.children.size();
                for(size_t idx=0;idx!=a.children.size();++idx){
                        auto a_interval = boost::get<Interval>(&a.children[idx]);
                        auto b_interval = boost::get<Interval>(&b.children[idx]);
                        BOOST_ASSERT( a_interval );
                        BOOST_ASSERT( b_interval );
                        if( a_interval->left != b_interval->left )
                                return  a_interval->left < b_interval->left;
                        if( a_interval->right != b_interval->right )
                                return  a_interval->right < b_interval->right;
                }
                return false;
        }
};

std::string ToString(BorelSet const& b){
        struct ToStringImpl{
                void operator()(Omega const&){
                        ostr_ << "Omega";
                }
                void operator()(Nul const&){
                        ostr_ << "Nul";
                }
                void operator()(Not const& obj){
                        ostr_ << "Not{";
                        boost::apply_visitor(*this, obj.child);
                        ostr_ << "}";
                }
                void operator()(Union const& obj){
                        ostr_ << "Union{";
                        for(size_t idx=0;idx!=obj.children.size();++idx){
                                if( idx != 0 )
                                        ostr_ << ", ";
                                boost::apply_visitor(*this, obj.children[idx]);
                        }
                        ostr_ << "}";
                }
                void operator()(Intersection const& obj){
                        ostr_ << "Intersection{";
                        for(size_t idx=0;idx!=obj.children.size();++idx){
                                if( idx != 0 )
                                        ostr_ << ", ";
                                boost::apply_visitor(*this, obj.children[idx]);
                        }
                        ostr_ << "}";
                }
                void operator()(Interval const& i){
                        ostr_ << ( i.left.is_open ? "(" : "[" );
                        ostr_ << i.left.point;
                        ostr_ << ",";
                        ostr_ << i.right.point;
                        ostr_ << ( i.right.is_open ? ")" : "]" );
                }
                std::stringstream ostr_;
        };
        ToStringImpl impl;
        boost::apply_visitor(impl, b);
        return impl.ostr_.str();
}

void Display(BorelSet const& b){
        std::cout << ToString(b) << "\n";
}

IntervalUnion ToIntervals(BorelSet const& b){
        struct ToIntervalsImpl : boost::static_visitor<IntervalUnion>{
                IntervalUnion operator()(Omega const&)const{
                        return IntervalUnion{Interval::Closed(0,1)};
                }
                IntervalUnion operator()(Nul const&)const{
                        return IntervalUnion{};
                }
                IntervalUnion operator()(Not const& obj)const{
                        Union result;
                        for(auto const& i : boost::apply_visitor(*this, obj.child).children ){
                                result.Add( i.Not().AsUnion() );
                        }
                        return operator()(result);
                }
                IntervalUnion operator()(Union const& obj)const{
                        IntervalUnion mapped;
                        for(auto const& _ : obj.children ){
                                for( auto const& inner : boost::apply_visitor(*this,_).children ){
                                        mapped.children.push_back(inner);
                                }
                        }
                        std::vector<Interval const*> subs;
                        for(auto const& _ : mapped.children ){
                                subs.push_back(&_);
                        }
                        boost::sort( subs, [](auto const& l, auto const& r){
                                if( l->left.point != r->left.point )
                                        return l->left.point < r->left.point;
                                // prefer closed
                                return l->left.is_open < r->left.is_open;
                        });

                        IntervalUnion result;
                        size_t iter = 0;
                        for(;iter!=subs.size();++iter){
                                if( subs[iter] == 0 )
                                        continue;

                                // for debugging
                                std::vector<Interval const*> dbg_path;
                                dbg_path.push_back(subs[iter]);

                                IntervalEndPoint left  = subs[iter]->left;
                                IntervalEndPoint right = subs[iter]->right;
                                // what is the largest path we can construct
                                subs[iter] = 0;
                                for(size_t j=iter+1;j!=subs.size();){
                                        if( subs[j] == 0 ){
                                                ++j;
                                                continue;
                                        }
                                        auto head = subs[j];

                                        // do these overlap?
                                        if( head->left.point < right.point ||
                                           (head->left.point ==right.point && ! (head->left.is_open && right.is_open) ) ){

                                                // now can we extend right?
                                                if( right.point < head->right.point ||
                                                   (right.point ==head->right.point && right.is_open && !head->right.is_open ) ){
                                                        right = head->right;
                                                        dbg_path.push_back(head);
                                                        // restart nice and slow
                                                        j = iter+1;
                                                        continue;
                                                }
                                        }
                                        ++j;
                                }
                                Interval i{left, right};

                                for(size_t j=iter+1;j!=subs.size();++j){
                                        if( subs[j] == 0 )
                                                continue;
                                        if( subs[j]->IsSubsetOf(i) ){
                                                subs[j] = 0;
                                        }
                                }

                                result.children.push_back(i);

                        }

                        return result;
                }
                IntervalUnion operator()(Intersection const& obj)const{
                        if( obj.children.empty() )
                                return IntervalUnion{};
                        IntervalUnion mapped;
                        for(auto const& _ : obj.children ){
                                for(auto const& inner : boost::apply_visitor(*this,_).children ){
                                        mapped.children.push_back(inner);
                                }
                        }

                        auto first = &mapped.children.at(0);

                        IntervalEndPoint const* upper_left  = &first->left;
                        IntervalEndPoint const* lower_right = &first->right;

                        for(auto const& i : mapped.children ){
                                auto ptr = &i;

                                if( upper_left->point < ptr->left.point )
                                        upper_left = &ptr->left;
                                else if(  upper_left->point == ptr->left.point &&
                                          ! upper_left->is_open &&
                                          ptr->left.is_open )
                                        upper_left = &ptr->left;

                                if( lower_right->point > ptr->right.point )
                                        lower_right = &ptr->right;
                                else if(  lower_right->point == ptr->right.point &&
                                          ! lower_right->is_open &&
                                          ptr->right.is_open )
                                        lower_right = &ptr->right;

                        }

                        std::cout << "*upper_left = " << *upper_left << "\n";
                        std::cout << "*lower_right = " << *lower_right << "\n";

                        if( upper_left->point <  lower_right->point ||
                            ( upper_left->point == lower_right->point
                              && ! upper_left->is_open && ! lower_right->is_open ) ){
                                return IntervalUnion{ Interval{ *upper_left, *lower_right} };
                        }

                        return IntervalUnion{};

                }
                IntervalUnion operator()(Interval const& i)const{
                        return IntervalUnion{i};
                }
        };

        auto tmp = boost::apply_visitor(ToIntervalsImpl(), b);
        boost::sort(tmp.children);
        return tmp;
}

struct BorelFamily : std::vector<BorelSet>{
        using impl_type = std::vector<BorelSet>;
        template<class... Args>
        BorelFamily(Args&&... args):impl_type{args...}{}
        friend std::ostream& operator<<(std::ostream& ostr, BorelFamily const& self){
                ostr << "{";
                for(size_t idx=0;idx!=self.size();++idx){
                        ostr << ( idx == 0 ? "" : ", " ) << ToString(self[idx]);
                }
                return ostr << "}";
        }
        void Display(std::ostream& out)const{
                for(size_t idx=0;idx!=size();++idx){
                        out << "    " << std::setw(2) << idx << " : " << ToString(at(idx)) << "\n";
                }
        }
};

#if 1
BorelFamily GenerateSigmaAlgebra(BorelFamily const& family){
        BorelFamily head = family;
        std::vector<BorelSet> to_add;

        std::set<IntervalUnion> interval_set;
        for(auto const& _ : family ){
                interval_set.insert( ToIntervals(_) );
        }

        auto test = [&](BorelSet const& b){
                auto iu = ToIntervals(b);
                if( ! interval_set.count( iu ) ){
                        std::cout << "====== found new ======\n";
                        std::cout << "    b  = " << ToString(b) << "\n";
                        std::cout << "    iu = " << ToString(iu.AsUnion()) << "\n";

                        to_add.push_back(b);
                        interval_set.insert( iu );
                        return 1;
                }
                return 0;
        };

        for(;;){
                int changes = 0;
                for(auto const& _ : head ){
                        auto complement = Not{_};
                        changes += test(complement);
                }
                for(size_t i=0;i+1<head.size();++i){
                        for(size_t j=i+1;j<head.size();++j){
                                auto u = Union{ head[i], head[j] };
                                changes += test(u);
                        }
                }
                if( changes == 0 )
                        break;
                for(auto const& _ : to_add )
                        head.push_back(_);
                //break;
        }

        BorelFamily result;
        for(auto const& _ : interval_set ){
                result.push_back( _.AsUnion() );
        }
        return result;

        return head;

}
#endif

int main(){

        BorelSet b = Intersection{ Interval{ Closed(0.0 ), Closed(0.25) },
                            Interval{ Open(0.25), Open(0.50) },
                            Interval{ Closed(0.1), Closed(0.6) } };
        Display(b);

        Display(ToIntervals(b).AsUnion());

        BorelFamily f0{ Omega(), Nul() };
        std::cout << "f0 = " << f0 << "\n";
        BorelFamily f1{ Omega(), Nul(),
                        Interval{ Closed(0.0), Open(0.5) },
                        Interval{ Closed(0.5), Closed(1.0) } };
        std::cout << "f1 = " << f1 << "\n";
        BorelFamily f2{ Omega(), Nul(),
                        Interval{ Closed(0.0), Open(0.25) },
                        Interval{ Closed(0.25), Open(0.50) },
                        Interval{ Closed(0.50), Open(0.75) },
                        Interval{ Closed(0.75), Closed(1.0) } };
        std::cout << "f2 = " << f2 << "\n";

        std::cout << "GenerateSigmaAlgebra(f2):\n";
        GenerateSigmaAlgebra(f2).Display(std::cout);

        //example_0();
        //example_1();
        //example_2();

}


#include "thin_lq.hpp"

#include <armadillo>
#include <stdexcept>

template <class Real>
void ThinLq(const arma::Mat<Real> &A, arma::Mat<Real> &L, arma::Mat<Real> &Q) {
    // A = Q * R  =>  A^T = R^T * Q^T

    arma::Mat<Real> L_temp;
    arma::Mat<Real> Q_temp;
    bool status = arma::qr_econ(Q_temp, L_temp, A.t());

    if (!status) {
        throw std::runtime_error("RQ decomposition failed");
    }

    Q = Q_temp.t();
    L = L_temp.t();
}

template void ThinLq<float>(const arma::Mat<float> &A, arma::Mat<float> &L,
                            arma::Mat<float> &Q);
template void ThinLq<double>(const arma::Mat<double> &A, arma::Mat<double> &L,
                             arma::Mat<double> &Q);


/**
 * @file generateData.cpp
 * @brief Create 3D simulation data for ICRA 2012 paper.
 * @author Michael Kaess
 * @author David Rosen
 * @version $Id: generateSpheresICRA2012.cpp 6377 2012-03-30 20:06:44Z kaess $
 */

#include <stdio.h>
#include <string>
#include <fstream>
#include <sstream>

#include <isam/Pose3d.h>

using namespace std;
using namespace isam;
using namespace Eigen;

//Number of samples to generate
const int num_samples = 1000;

//Covariances
const double sigmas[6] = {0.1, 0.1, 0.1, 0.04, 0.04, 0.04};

string directory_name = "sphere_data/";
string base_filename = "sample_";
string file_extension = ".txt";

char* write_string = new char[500];

const bool ADD_NOISE = true;
const bool LOOP_CLOSING = true;
// vehicle on surface of sphere, instead of upright (pitch=roll=0)
const bool PERPENDICULAR = true;

// sample from a normal distribution
#include <boost/random/linear_congruential.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
static boost::minstd_rand generator(27u);
double sample_normal(double sigma = 1.0)
{
  typedef boost::normal_distribution<double> Normal;
  Normal dist(0.0, sigma);
  boost::variate_generator<boost::minstd_rand&, Normal> norm(generator, dist);
  return(norm());
}

void write_constraint(ofstream& outfile, int id0, int id1, const Pose3d& delta_) {
  Pose3d delta = delta_;
  if (ADD_NOISE) {
    // corrupt measurement with Gaussian noise
    VectorXd v = delta.vector();
    for (int i=0; i<6; i++) {
      v(i) += sample_normal(sigmas[i]);
    }
    delta.set(v);
  }

  // X Y Z roll pitch yaw
  sprintf(write_string, "EDGE3 %i %i %g %g %g %g %g %g",
          id0, id1, delta.x(), delta.y(), delta.z(), delta.roll(), delta.pitch(), delta.yaw());
  outfile << write_string;
  for (int i=0; i<6; i++) {
    for (int j=i; j<6; j++) {
      double sqrtinf = 0.;
      // only diagonal entries populated
      if (i==j) {
        // roll,pitch,yaw order
        if (i<3) {
          sqrtinf = 1./sigmas[i];
        } else {
          sqrtinf = 1./sigmas[8-i];
        }
      }
      sprintf(write_string, " %g", sqrtinf);
      outfile << write_string;
    }
  }
  outfile << "\n";
}

void add_constraint(ofstream& outfile, Pose3d* poses, int id0, int id1) {
  Pose3d delta = poses[id1].ominus(poses[id0]);
  write_constraint(outfile, id0, id1, delta);
}

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

  // Make sure that we have a 'test_data' directory.
  int ret = system(("mkdir " + directory_name).c_str());
  require(ret!=-1, "Failed to created directory.");

  for (int s = 1; s <= num_samples; s++) {
    // Form the filename that will be used to hold the output of this sample.

    stringstream current_filename;

    current_filename << directory_name;
    current_filename << base_filename;
    current_filename << s;
    current_filename << file_extension;

    // Use this filename to open up a new file for writing.
    ofstream outfile( (current_filename.str()).c_str());

    // generate poses on the surface of a sphere
    const int steps = 50; // number of poses for one turn around the sphere
    const int slices = 50; // number of rounds around the sphere
    const double radius0 = 2.; // starting radius
    const double radius = 50.; // sphere radius

    int n = steps*slices;
    Pose3d poses[n];
    int pose_id = 0;
    // current vertical angle for elevation/slice (starting angle and increment)
    double alpha = atan(radius0/radius);
    double d_alpha = (M_PI-2*alpha) / (double)(n);
    // angle in horizontal plane (starting angle and increment)
    double phi = 0.;
    double d_phi = 2*M_PI / (double)steps;
    for (int i=0; i<steps; i++) {
      for (int j=0; j<slices; j++) {
        // calculate position
        double r = radius * sin(alpha);
        double h = sqrt(radius*radius - r*r);
        // bottom half of sphere?
        if (i<(slices/2)) h = -h;
        // calculate pose
        double y = r * sin(phi);
        double x = r * cos(phi);
        double z = -(radius + h);
        double yaw = phi + M_PI/2.0;
        double roll = 0.;
        if (PERPENDICULAR) {
          roll = alpha;
        }
        // generate measurements
        poses[pose_id] = Pose3d(x,y,z, yaw,0.,roll);
        if (pose_id>0) {

          add_constraint(outfile, poses, pose_id-1, pose_id);
          if (LOOP_CLOSING) {
            if (pose_id >= steps) {
              add_constraint(outfile, poses, pose_id-steps, pose_id);
            }
          }
        }
        // update for next step
        alpha += d_alpha;
        phi += d_phi;
        pose_id++;
      }
    }
  }
}


//#include <iostream>
//#include <ostream>
//#include <fstream>
//#include <armadillo>
//#include "master.hpp"
//
//Rzxy(,)
//F_A: functions for the Agisoft conventions
//double yaw_rotZXY(mat Rzxy,bool giveRadians=false)
//{
//
//    double c = (180/datum::pi);
//
//    if(giveRadians)
//    {
//        c = 1.0f;
//    }
//    F_A
//    return atan2(Rzxy(0,1),Rzxy(1,1)) * c;
//}
//
//double pitch_rotZXY(mat Rzxy,bool giveRadians=false)
//{
//    double c = (180/datum::pi);
//
//    if(giveRadians)
//    {
//        c = 1.0f;
//    }
//    F_A
//    return -asin(Rzxy(2,1)) * c;
//}
//
//double roll_rotZXY(mat Rzxy,bool giveRadians=false)
//{
//    double c = (180/datum::pi);
//
//    if(giveRadians)
//    {
//        c = 1.0f;
//    }
//    F_A
//    return atan2(Rzxy(2,0),Rzxy(2,2)) * c;
//}
//
//mat one_axis_rot(double a,int axis,bool giveInverseT=false,bool isRadians=false)
//{
//
//    these will give you the rotation in a CLOCKWISE way
//
//    mat res = eye(3,3);
//    double c = (datum::pi/180);
//
//    if (isRadians)
//    {
//        c = 1.0f;
//    }
//
//    double ca = cos(a*c);
//    double sa = sin(a*c);
//
//    if(axis == 1)
//    {
//        res(1,1) =  ca;
//        res(1,2) =  sa;
//        res(2,1) = -sa;
//        res(2,2) =  ca;
//    }
//    if(axis == 2)
//    {
//        res(0,0) =  ca;
//        res(0,2) = -sa;
//        res(2,0) =  sa;
//        res(2,2) =  ca;
//    }
//    if (axis == 3)
//    {
//        res(0,0) =  ca;
//        res(0,1) =  sa;
//        res(1,0) = -sa;
//        res(1,1) =  ca;
//    }
//
//    if (giveInverseT)
//    {res = res.t();}
//
//    return res;
//}
//
//mat novatel_DCM(double roll,double pitch,double azimuth,bool isRadians=false,bool giveInverseT=false)
//{
//    matrix that gives the transformation
//    FROM   BODY FRAME
//    TO     LOCAL LEVEL FRAME
//
//    accordingly to the manual, is a combination of the following order:
//    Rz * Rx * Ry  or R3 * R1 * R2 , all of them TRANSPOSED
//
//    NOVATEL definitions:
//    roll   is around the Y axis
//    pitch  is around the X axis
//    yaw    is around the Z axis (and also Azimuth)
//
//    there is the fourth argument that can be used to give the inverse transformation
//
//    mat res = zeros(3,3);
//
//    double c = (datum::pi/180);
//
//    if (isRadians)
//    {
//        c = 1.0f;
//    }
//
//    conversion between azimuth and yaw
//    double yaw = -azimuth;
//
//    double cr = cos(roll * c);
//    double sr = sin(roll * c);
//
//    double cp = cos(pitch * c);
//    double sp = sin(pitch * c);
//
//    double cy = cos(yaw * c);
//    double sy = sin(yaw * c);
//
//    linewise splitted
//    res(0,0)= cy*cr-sy*sp*sr;
//    res(0,1)= -sy*cp;
//    res(0,2)= cy*sr + sy*sp*cr;
//
//    res(1,0)=  sy*cr+cy*sp*sr;
//    res(1,1)=  cy*cp;
//    res(1,2)=  sy*sr-cy*sp*cr;
//
//    res(2,0)=-cp*sr;
//    res(2,1)= sp;
//    res(2,2)= cp*cr;
//
//    if (giveInverseT)
//    {res = res.t();}
//
//    return res;
//}
//
//
//struct outputPoseToPhotoscan
//{
//    struct for individual data
//    string img_name;
//    double lat,lgt,h,yaw,pitch,roll;
//    double lat0,lgt0,h0; //coordinates of the origin
//
//    vec3 v0ECEF;
//
//    mat attMat;
//    outputPoseToPhotoscan(vec3 vecBF,mat bsight,unsigned int ind);
//};
//
//outputPoseToPhotoscan::outputPoseToPhotoscan(vec3 vBF,mat bsight,unsigned int ind)
//{
//    constants for multiplication and transformation
//    double to_deg = 180/datum::pi;
//    double to_rad = datum::pi/180;
//
/// First: position of the camera CP
//    filling the origin
//    lat0 = job.finalObsWcovs.at(ind).observation.lat;
//    lgt0 = job.finalObsWcovs.at(ind).observation.lgt;
//    h0   = job.finalObsWcovs.at(ind).observation.h;
//
//    converting the origin to XYZ
//    GEODESY_ConvertGeodeticCurvilinearToEarthFixedCartesianCoordinates(
//    GEODESY_REFERENCE_ELLIPSE_WGS84,
//    lat0*to_rad,lgt0*to_rad,h0,&v0ECEF(0),&v0ECEF(1),&v0ECEF(2));
//
//    compute the rotation matrix
//    mat R = novatel_DCM(
//    job.finalObsWcovs.at(ind).observation.roll,
//    job.finalObsWcovs.at(ind).observation.pitch,
//    job.finalObsWcovs.at(ind).observation.azimuth);
//
//    transform the vector to the LLF
//    vec3 vLLF = R * vBF;
//    transform to the ECEF system
//    vec3 vecECEF = ENU_to_ECEF(vLLF,v0ECEF,lat0,lgt0,h0);
//
//    transform to lat, long, h
//    GEODESY_ConvertEarthFixedCartesianToGeodeticCurvilinearCoordinates(
//    GEODESY_REFERENCE_ELLIPSE_WGS84,vecECEF(0),vecECEF(1),vecECEF(2),&lat,&lgt,&h);
//
//    converting to degrees:
//    lat *= to_deg;lgt *= to_deg;
//
//    / second: orientation
//
//         R is from IMU BF to IMU CN LLF
//
//        matrix from ECEF to IMU CN LLF
//        mat Rel1 = R_ecef_enu(lat0,lgt0);
//
//        matrix from ECEF to camera CP LLF
//        mat Rel2 = R_ecef_enu(lat,lgt);
//
//        matrix from IMU CN LLF to camera CP LLF
//        mat Rl1l2 = Rel1 * Rel2.t();
//
//        matrix from the camera CP LLF to IMU BF
//        mat Rl2bf1 = R.t() * Rl1l2.t();
//
//        finally, the camera BF to camera CP LLF
//        the bsight needs to be Rbf2bf1, aka from Camera BF to IMU
//        attMat = Rl2bf1.t()*bsight.t();
//
//        now the yaw pitch roll to photoscan
//        yaw     = yaw_rotZXY(attMat);
//        pitch   = pitch_rotZXY(attMat);
//        roll    = roll_rotZXY(attMat);
//    }
//
//struct outputterToPhotoscan
//{
// vector<outputPoseToPhotoscan> individuals;
//
//
//
// outputterToPhotoscan(void);
//};
//
//outputterToPhotoscan::outputterToPhotoscan(void)
//{
// ofstream outL("to_photoscan_left.txt");
// ofstream outR("to_photoscan_right.txt");
//
// string separator = "   ";
//
//    for (unsigned int i=0;i<job.finalObsWcovs.size();i++)
//    {
//        outputPoseToPhotoscan  Left(rCalib.LcamLA,rCalib.Rimu_LC.t(),i);
//        outputPoseToPhotoscan Right(rCalib.RcamLA,rCalib.Rimu_RC.t(),i);
//
//        outL<<Left.img_name<<separator<<Left.lat<<separator<<Left.lgt<<separator<<Left.h;
//        outL<<separator<<Left.yaw<<separator<<Left.pitch<<separator<<Left.roll<<endl;
//
//        outR<<Right.img_name<<separator<<Right.lat<<separator<<Right.lgt<<separator<<Left.h;
//        outR<<separator<<Right.yaw<<separator<<Right.pitch<<separator<<Right.roll<<endl;
//    }
//}
//