/* boost random/student_t_distribution.hpp header file
 *
 * Copyright Steven Watanabe 2011
 * 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 */

#ifndef BOOST_RANDOM_STUDENT_T_DISTRIBUTION_HPP
#define BOOST_RANDOM_STUDENT_T_DISTRIBUTION_HPP

#include <boost/config/no_tr1/cmath.hpp>
#include <iosfwd>
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/random/detail/operators.hpp>
#include <boost/random/chi_squared_distribution.hpp>
#include <boost/random/normal_distribution.hpp>

namespace boost {
namespace random {

/**
 * The Student t distribution is a real valued distribution with one
 * parameter n, the number of degrees of freedom.
 *
 * It has \f$\displaystyle p(x) =
 *   \frac{1}{\sqrt{n\pi}}
 *   \frac{\Gamma((n+1)/2)}{\Gamma(n/2)}
 *   \left(1+\frac{x^2}{n}\right)^{-(n+1)/2}
 * \f$.
 */
template<class RealType = double>
class student_t_distribution {
public:
    typedef RealType result_type;
    typedef RealType input_type;

    class param_type {
    public:
        typedef student_t_distribution distribution_type;

        /**
         * Constructs a @c param_type with "n" degrees of freedom.
         *
         * Requires: n > 0
         */
        explicit param_type(RealType n_arg = RealType(1.0))
          : _n(n_arg)
        {}

        /** Returns the number of degrees of freedom of the distribution. */
        RealType n() const { return _n; }

        /** Writes a @c param_type to a @c std::ostream. */
        BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, param_type, parm)
        { os << parm._n; return os; }

        /** Reads a @c param_type from a @c std::istream. */
        BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, param_type, parm)
        { is >> parm._n; return is; }

        /** Returns true if the two sets of parameters are the same. */
        BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(param_type, lhs, rhs)
        { return lhs._n == rhs._n; }

        /** Returns true if the two sets of parameters are the different. */
        BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(param_type)

    private:
        RealType _n;
    };

    /**
     * Constructs an @c student_t_distribution with "n" degrees of freedom.
     *
     * Requires: n > 0
     */
    explicit student_t_distribution(RealType n_arg = RealType(1.0))
      : _normal(), _chi_squared(n_arg)
    {}
    /** Constructs an @c student_t_distribution from its parameters. */
    explicit student_t_distribution(const param_type& parm)
      : _normal(), _chi_squared(parm.n())
    {}

    /**
     * Returns a random variate distributed according to the
     * Student t distribution.
     */
    template<class URNG>
    RealType operator()(URNG& urng)
    {
        using std::sqrt;
        return _normal(urng) / sqrt(_chi_squared(urng) / n());
    }

    /**
     * Returns a random variate distributed accordint to the Student
     * t distribution with parameters specified by @c param.
     */
    template<class URNG>
    RealType operator()(URNG& urng, const param_type& parm) const
    {
        return student_t_distribution(parm)(urng);
    }

    /** Returns the number of degrees of freedom. */
    RealType n() const { return _chi_squared.n(); }

    /** Returns the smallest value that the distribution can produce. */
    RealType min BOOST_PREVENT_MACRO_SUBSTITUTION () const
    { return -std::numeric_limits<RealType>::infinity(); }
    /** Returns the largest value that the distribution can produce. */
    RealType max BOOST_PREVENT_MACRO_SUBSTITUTION () const
    { return std::numeric_limits<RealType>::infinity(); }

    /** Returns the parameters of the distribution. */
    param_type param() const { return param_type(n()); }
    /** Sets the parameters of the distribution. */
    void param(const param_type& parm)
    {
        typedef chi_squared_distribution<RealType> chi_squared_type;
        typename chi_squared_type::param_type chi_squared_param(parm.n());
        _chi_squared.param(chi_squared_param);
    }

    /**
     * Effects: Subsequent uses of the distribution do not depend
     * on values produced by any engine prior to invoking reset.
     */
    void reset()
    {
        _normal.reset();
        _chi_squared.reset();
    }

    /** Writes a @c student_t_distribution to a @c std::ostream. */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, student_t_distribution, td)
    {
        os << td.param();
        return os;
    }

    /** Reads a @c student_t_distribution from a @c std::istream. */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, student_t_distribution, td)
    {
        param_type parm;
        if(is >> parm) {
            td.param(parm);
        }
        return is;
    }

    /**
     * Returns true if the two instances of @c student_t_distribution will
     * return identical sequences of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(student_t_distribution, lhs, rhs)
    { return lhs._normal == rhs._normal && lhs._chi_squared == rhs._chi_squared; }

    /**
     * Returns true if the two instances of @c student_t_distribution will
     * return different sequences of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(student_t_distribution)

private:
    normal_distribution<RealType> _normal;
    chi_squared_distribution<RealType> _chi_squared;
};

} // namespace random
} // namespace boost

#endif // BOOST_RANDOM_STUDENT_T_DISTRIBUTION_HPP


/* Copyright Institute of Sound and Vibration Research - All rights reserved */

#include "time_frequency_transform.hpp"

#include "libvisr/signal_flow_context.hpp"

#include <libefl/vector_functions.hpp>

#include <libpml/time_frequency_parameter.hpp>
#include <libpml/time_frequency_parameter_config.hpp>
#include <libpml/vector_parameter.hpp>

#include <librbbl/fft_wrapper_base.hpp>
#include <librbbl/fft_wrapper_factory.hpp>

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

#include <algorithm>
#include <ciso646>
#include <cmath>

namespace visr
{
namespace rcl
{

namespace // unnamed namespace
{

/**
 * Create a slightly asymmetric Hann window as the default window shape.
 * @note In contrast to the standard definition, this version satisfies the COLA (constant overlap-add) property.
 */
template< typename T >
pml::VectorParameter<T> unityHannWindow( std::size_t length )
{
  pml::VectorParameter<T> res( length, cVectorAlignmentSamples );
  for( std::size_t idx( 0 ); idx < length; ++idx )
  {
    res[idx] = static_cast<T>(0.5 - 0.5*std::cos( (2.0*boost::math::constants::pi<T>()*static_cast<T>(idx)) / static_cast<T>(length) ));
  }
  return res;
}

} // namespace unnamed

TimeFrequencyTransform::TimeFrequencyTransform( SignalFlowContext const & context,
                                                char const * name,
                                                CompositeComponent * parent,
                                                std::size_t numberOfChannels,
                                                std::size_t dftLength,
                                                std::size_t windowLength,
                                                std::size_t hopSize,
                                                char const * fftImplementation /*= "default"*/ )
 : TimeFrequencyTransform( context, name, parent,
                           numberOfChannels, dftLength,
                           unityHannWindow<SampleType>( windowLength ),
                           hopSize, fftImplementation )
{
}

TimeFrequencyTransform::TimeFrequencyTransform( SignalFlowContext const & context,
                        char const * name,
                        CompositeComponent * parent,
                        std::size_t numberOfChannels,
                        std::size_t dftLength,
                        efl::BasicVector<SampleType> const & window,
                        std::size_t hopSize, char const * fftImplementation /*= "default"*/ )
 : AtomicComponent( context, name, parent )
 , mAlignment( cVectorAlignmentSamples )
 , mNumberOfChannels( numberOfChannels )
 , mDftlength( dftLength )
 , mWindowLength( window.size() )
 , mDftSamplesPerPeriod( context.period() / hopSize )
 , mHopSize( hopSize )
 , mInputBuffer( numberOfChannels, window.size(), mAlignment )
 , mFftWrapper( rbbl::FftWrapperFactory<SampleType>::create( fftImplementation, dftLength, mAlignment ) )
 , mWindow( window.size(), mAlignment )
 , mCalcBuffer( dftLength, mAlignment )
 , mInput( "in", *this, numberOfChannels )
 , mOutput( "out", *this, pml::TimeFrequencyParameterConfig( dftLength, hopSize, numberOfChannels, mDftSamplesPerPeriod ) )
{
  if( period() % hopSize != 0 )
  {
    throw std::invalid_argument( "TimeFrequencyTransform: Invalid hop size (no integer number of hops per audio processing period)." );
  }
  efl::vectorZero( mCalcBuffer.data(), mCalcBuffer.size(), mCalcBuffer.alignmentElements() );

  // Scale the window to account for the FFT scaling and the DFT length
  SampleType const scaleFactor = static_cast<SampleType>(1.0)
    / (mFftWrapper->forwardScalingFactor() * mFftWrapper->inverseScalingFactor() * mDftlength);
  std::transform( window.data(), window.data()+window.size(), mWindow.data(),
                  [scaleFactor](SampleType val){ return scaleFactor * val;} );
}

TimeFrequencyTransform::~TimeFrequencyTransform() = default;

void TimeFrequencyTransform::process()
{
  pml::TimeFrequencyParameter<SampleType> & outMtx = mOutput.data();
  mInputBuffer.write( mInput.data(), mInput.channelStrideSamples(),
                      mNumberOfChannels, period(), cVectorAlignmentSamples );
  for( std::size_t hopIndex( 0 ); hopIndex < mDftSamplesPerPeriod; ++hopIndex )
  {
    std::size_t const blockStartIndex = mWindowLength + (mDftSamplesPerPeriod - hopIndex - 1) * mHopSize;
    for( std::size_t channelIndex( 0 ); channelIndex < mNumberOfChannels; ++channelIndex )
    {
      efl::ErrorCode res = efl::vectorMultiply( mInputBuffer.getReadPointer( channelIndex, blockStartIndex ),
                                                mWindow.data(), mCalcBuffer.data(), mWindowLength, mAlignment );
      if( res != efl::noError )
      {
        throw std::runtime_error( "TimeFrequencyTransform: Error during input windowing." );
      }
      std::complex<SampleType> * dftPtr = outMtx.dftSlice( channelIndex, hopIndex );
      res = mFftWrapper->forwardTransform( mCalcBuffer.data(), dftPtr );
      if( res != efl::noError )
      {
        throw std::runtime_error( "TimeFrequencyTransform: Error during FFT operation." );
      }
    }
  }
}

} // namespace rcl
} // namespace visr


#include <NTL/ZZ.h>
#include <NTL/BasicThreadPool.h>
#include <NTL/lzz_pXFactoring.h>
#include "FHE.h"
#include "timing.h"
#include "EncryptedArray.h"
#include "FHE_operation.h"

#include <cassert>
#include <cstdio>
#include <iostream>
using namespace std;

int main()
{
	long m = 0;          // 确定系数
	long p = 2147483647; // 模量(素数)，定义超过p/2的数为负数，负数的真值x=D[E[x]]-p
	long r = 1;
	long L = 16;
	long c = 3;
	long w = 64;
	long d = 0;
	long k = 128;
	long s = 0;

	m = FindM(k, L, c, p, d, s, 0);

	FHEcontext context(m, p, r);
	buildModChain(context, L, c);
	ZZX G = context.alMod.getFactorsOverZZ()[0];

	// 生成公钥
	FHESecKey secretKey(context);
	const FHEPubKey& publicKey = secretKey;
	secretKey.GenSecKey(w);

	// 初始化密文
	Ctxt Ea(publicKey);
	Ctxt Eb(publicKey);

	// Test
	long op1[5] = {2, 4, 0, 25, 15};
	long op2[5] = {-1, 1, 4, 0, 2};
	long *res;

	Vec<ZZ> h1 = arr2validVec(op1, 5);
	Vec<ZZ> h2 = arr2validVec(op2, 5);

	publicKey.Encrypt(Ea, to_ZZX(h1));
	publicKey.Encrypt(Eb, to_ZZX(h2));

	ZZX ptSum;
	secretKey.Decrypt(ptSum, FHE_Add(Ea, Eb));
	res = FHE_ptDec(ptSum, p, 5);
	cout << "ptSum : " << endl;
	for (int i=0; i<5; i++)
		cout << res[i] << " ";
	cout << endl;

	ZZX ptMul;
	secretKey.Decrypt(ptMul, FHE_Mul(Ea, Eb, p, publicKey, secretKey, 5));
	res = FHE_ptDec(ptMul, p, 5);
	cout << "ptMul : " << endl;
	for (int i=0; i<5; i++)
		cout << res[i] << " ";
	cout << endl;

	ZZX ptSub;
	secretKey.Decrypt(ptSub, FHE_Sub(Ea, Eb, p, publicKey, secretKey, 5));
	res = FHE_ptDec(ptSub, p, 5);
	cout << "ptSub : " << endl;
	for (int i=0; i<5; i++)
		cout << res[i] << " ";
	cout << endl;

	ZZX ptDiv;
	secretKey.Decrypt(ptDiv, FHE_Div(Ea, Eb, p, publicKey, secretKey, 5));
	res = FHE_ptDec(ptDiv, p, 5);
	cout << "ptDiv : " << endl;
	for (int i=0; i<5; i++)
		cout << res[i] << " ";
	cout << endl;

	return 0;
}


#pragma once

#include <cmath>

#include <Eigen/SparseLU>

#include "OpenABF/ABF.hpp"
#include "OpenABF/Exceptions.hpp"
#include "OpenABF/HalfEdgeMesh.hpp"
#include "OpenABF/Math.hpp"

namespace OpenABF
{

/**
 * @brief Compute parameterized interior angles using ABF++
 *
 * Iteratively computes a new set of interior angles which minimize the total
 * angular error of the parameterized mesh. This follows the ABF++ formulation,
 * which solves a 5x smaller system of equations than standard ABF at the
 * expense of more iterations.
 *
 * This class **does not** compute a parameterized mesh. Rather, it calculates
 * the optimal interior angles for such a mesh. To convert this information
 * into a full parameterization, pass the processed HalfEdgeMesh to
 * AngleBasedLSCM.
 *
 * Implements "ABF++: Fast and Robust Angle Based Flattening" by Sheffer
 * _et al._ (2005) \cite sheffer2005abf++.
 *
 * @tparam T Floating-point type
 * @tparam MeshType HalfEdgeMesh type which implements the ABF traits
 * @tparam Solver A solver implementing the
 * [Eigen Sparse solver
 * concept](https://eigen.tuxfamily.org/dox-devel/group__TopicSparseSystems.html)
 * and templated on Eigen::SparseMatrix<T>
 */
template <
    typename T,
    class MeshType = detail::ABF::Mesh<T>,
    class Solver =
        Eigen::SparseLU<Eigen::SparseMatrix<T>, Eigen::COLAMDOrdering<int>>,
    std::enable_if_t<std::is_floating_point<T>::value, bool> = true>
class ABFPlusPlus
{
public:
    /** @brief Mesh type alias */
    using Mesh = MeshType;

    /** @brief Set the maximum number of iterations */
    void setMaxIterations(std::size_t it) { maxIters_ = it; }

    /**
     * @brief Get the mesh gradient
     *
     * **Note:** Result is only valid after running compute().
     */
    auto gradient() const -> T { return grad_; }

    /**
     * @brief Get the number of iterations of the last computation
     *
     * **Note:** Result is only valid after running compute().
     */
    auto iterations() const -> std::size_t { return iters_; }

    /** @copydoc ABFPlusPlus::Compute */
    void compute(typename Mesh::Pointer& mesh)
    {
        Compute(mesh, iters_, grad_, maxIters_);
    }

    /**
     * @brief Compute parameterized interior angles
     *
     * @throws SolverException If matrix cannot be decomposed or if solver fails
     * to find a solution.
     * @throws MeshException If mesh gradient cannot be calculated.
     */
    static void Compute(
        typename Mesh::Pointer& mesh,
        std::size_t& iters,
        T& gradient,
        std::size_t maxIters = 10)
    {
        using namespace detail::ABF;

        // Initialize angles and weights
        InitializeAnglesAndWeights<T>(mesh);

        // while ||∇F(x)|| > ε
        gradient = Gradient<T>(mesh);
        if (std::isnan(gradient) or std::isinf(gradient)) {
            throw MeshException("Mesh gradient cannot be computed");
        }
        auto gradDelta = INF<T>;
        iters = 0;
        while (gradient > 0.001 and gradDelta > 0.001 and iters < maxIters) {
            if (std::isnan(gradient) or std::isinf(gradient)) {
                throw MeshException("Mesh gradient cannot be computed");
            }
            // Typedefs
            using Triplet = Eigen::Triplet<T>;
            using SparseMatrix = Eigen::SparseMatrix<T>;
            using DenseVector = Eigen::Matrix<T, Eigen::Dynamic, 1>;

            // Helpful parameters
            auto vIntCnt = mesh->num_vertices_interior();
            auto edgeCnt = mesh->num_edges();
            auto faceCnt = mesh->num_faces();

            // b1 = -alpha gradient
            std::vector<Triplet> triplets;
            std::size_t idx{0};
            for (const auto& e : mesh->edges()) {
                triplets.emplace_back(idx, 0, -AlphaGrad<T>(e));
                ++idx;
            }
            SparseMatrix b1(edgeCnt, 1);
            b1.reserve(triplets.size());
            b1.setFromTriplets(triplets.begin(), triplets.end());

            // b2 = -lambda gradient
            triplets.clear();
            idx = 0;
            // lambda tri
            for (const auto& f : mesh->faces()) {
                triplets.emplace_back(idx, 0, -TriGrad<T>(f));
                idx++;
            }
            // lambda plan and lambda len
            for (const auto& v : mesh->vertices_interior()) {
                triplets.emplace_back(idx, 0, -PlanGrad<T>(v));
                triplets.emplace_back(vIntCnt + idx, 0, -LenGrad<T>(v));
                idx++;
            }
            SparseMatrix b2(faceCnt + 2 * vIntCnt, 1);
            b2.reserve(triplets.size());
            b2.setFromTriplets(triplets.begin(), triplets.end());

            // vertex idx -> interior vertex idx permutation
            std::map<std::size_t, std::size_t> vIdx2vIntIdx;
            std::size_t newIdx{0};
            for (const auto& v : mesh->vertices_interior()) {
                vIdx2vIntIdx[v->idx] = newIdx++;
            }

            // Compute J1 + J2
            triplets.clear();
            idx = 0;
            // Jacobian of the CTri constraints
            for (; idx < faceCnt; idx++) {
                triplets.emplace_back(idx, 3 * idx, 1);
                triplets.emplace_back(idx, 3 * idx + 1, 1);
                triplets.emplace_back(idx, 3 * idx + 2, 1);
            }
            for (const auto& v : mesh->vertices_interior()) {
                for (const auto& e0 : v->wheel()) {
                    // Jacobian of the CPlan constraint
                    triplets.emplace_back(idx, e0->idx, 1);

                    // Jacobian of the CLen constraint
                    auto e1 = e0->next;
                    auto e2 = e1->next;
                    auto d1 = LenGrad<T>(v, e1);
                    auto d2 = LenGrad<T>(v, e2);
                    triplets.emplace_back(vIntCnt + idx, e1->idx, d1);
                    triplets.emplace_back(vIntCnt + idx, e2->idx, d2);
                }
                ++idx;
            }
            SparseMatrix J(faceCnt + 2 * vIntCnt, 3 * faceCnt);
            J.reserve(triplets.size());
            J.setFromTriplets(triplets.begin(), triplets.end());

            // Lambda = diag(2/w)
            // v.weight == 1/w, so LambdaInv is diag(2*weight)
            // We only need Lambda Inverse, so this is 1 / 2*weight
            triplets.clear();
            idx = 0;
            for (const auto& e : mesh->edges()) {
                triplets.emplace_back(idx, idx, T(1) / (2 * e->weight));
                ++idx;
            }
            SparseMatrix LambdaInv(edgeCnt, edgeCnt);
            LambdaInv.reserve(edgeCnt);
            LambdaInv.setFromTriplets(triplets.begin(), triplets.end());

            // solve Eq. 16
            auto bstar = J * LambdaInv * b1 - b2;
            auto JLiJt = J * LambdaInv * J.transpose();

            SparseMatrix LambdaStarInv = JLiJt.block(0, 0, faceCnt, faceCnt);
            for (int k = 0; k < LambdaStarInv.outerSize(); ++k) {
                for (typename SparseMatrix::InnerIterator it(LambdaStarInv, k);
                     it; ++it) {
                    it.valueRef() = 1.F / it.value();
                }
            }
            auto Jstar = JLiJt.block(faceCnt,0,2*vIntCnt,faceCnt);
            auto JstarT = JLiJt.block(0,faceCnt,faceCnt, 2*vIntCnt);
            auto Jstar2 = JLiJt.block(faceCnt,faceCnt,2*vIntCnt, 2*vIntCnt);
            auto bstar1 = bstar.block(0, 0, faceCnt, 1);
            auto bstar2 = bstar.block(faceCnt, 0, 2*vIntCnt, 1);

            // (J* Lam*^-1 J*^t - J**) delta_lambda_2 = J* Lam*^-1 b*_1 - b*_2
            SparseMatrix A = Jstar * LambdaStarInv * JstarT - Jstar2;
            SparseMatrix b = Jstar * LambdaStarInv * bstar1 - bstar2;
            A.makeCompressed();
            Solver solver;
            solver.compute(A);
            if (solver.info() != Eigen::ComputationInfo::Success) {
                throw SolverException(solver.lastErrorMessage());
            }
            auto deltaLambda2 = solver.solve(b);
            if (solver.info() != Eigen::ComputationInfo::Success) {
                throw SolverException(solver.lastErrorMessage());
            }

            // Compute Eq. 17 -> delta_lambda_1
            auto deltaLambda1 =
                LambdaStarInv * (bstar1 - JstarT * deltaLambda2);

            // Construct deltaLambda
            DenseVector deltaLambda(
                deltaLambda1.rows() + deltaLambda2.rows(), 1);
            deltaLambda << DenseVector(deltaLambda1), DenseVector(deltaLambda2);

            // Compute Eq. 10 -> delta_alpha
            DenseVector deltaAlpha =
                LambdaInv * (b1 - J.transpose() * deltaLambda);

            // lambda += delta_lambda
            for (auto& f : mesh->faces()) {
                f->lambda_tri += deltaLambda(f->idx, 0);
            }
            for (auto& v : mesh->vertices_interior()) {
                auto intIdx = vIdx2vIntIdx.at(v->idx);
                v->lambda_plan += deltaLambda(faceCnt + intIdx, 0);
                v->lambda_len += deltaLambda(faceCnt + vIntCnt + intIdx, 0);
            }

            // alpha += delta_alpha
            // Update sin and cos
            idx = 0;
            for (auto& e : mesh->edges()) {
                e->alpha += deltaAlpha(idx++, 0);
                e->alpha = std::min(std::max(e->alpha, T(0)), PI<T>);
                e->alpha_sin = std::sin(e->alpha);
                e->alpha_cos = std::cos(e->alpha);
            }

            // Recalculate gradient for next iteration
            auto newGrad = Gradient<T>(mesh);
            gradDelta = std::abs(newGrad - gradient);
            gradient = newGrad;
            iters++;
        }
    }

    /** @brief Compute parameterized interior angles */
    static void Compute(typename Mesh::Pointer& mesh)
    {
        std::size_t iters{0};
        T grad{0};
        Compute(mesh, iters, grad);
    }

private:
    /** Gradient */
    T grad_{0};
    /** Number of executed iterations */
    std::size_t iters_{0};
    /** Max iterations */
    std::size_t maxIters_{10};
};

}  // namespace OpenABF


#include <bits/stdc++.h>
#include <boost/optional.hpp>
using namespace std;

/**
 * http://tsutaj.hatenablog.com/entry/2017/03/29/204841
 */
template<typename T>
class SegmentTree {
 private:
  vector<T> nodes;
  unsigned long long size;
  function<T(T, T)> fn;

 public:
  explicit SegmentTree(
      unsigned long long size,
      function<T(T, T)> fn = [](T a, T b) { return a + b; },
      T initial = 0) {
    // size 以上の最小の2べき
    long long sz = 1;
    while (sz <= size) sz <<= 1;
    this->size = sz;

    // root が 1
    // i 番目の要素が size + i にある
    this->nodes = vector<T>(this->size * 2, initial);
    this->fn = fn;
  }

  /**
   * [from_i, to_i) に fn を適用した結果
   * @param from_i
   * @param to_i
   * @param k
   */
  boost::optional<T> get(
      unsigned long long from_i,
      unsigned long long to_i,
      unsigned long long k = 1,
      long long l = 0,
      long long r = -1) {
    if (r < 0) r = size;

    // 被覆してる
    if (from_i <= l && r <= to_i) return nodes[k];
    // 関係ない
    if (r <= from_i || to_i <= l) return boost::none;

    // 子からもらう
    boost::optional<T> left = get(from_i, to_i, k * 2, l, (l + r) / 2);
    boost::optional<T> right = get(from_i, to_i, k * 2 + 1, (l + r) / 2, r);

    if (left && right) return fn(*left, *right);
    if (left) return *left;
    if (right) return *right;
    return boost::none;
  }

  /**
   * i を value で更新する
   * @param i
   * @param value
   */
  void set(unsigned long long i, T value) {
    long long k = size + i;

    nodes[k] = value;
    k /= 2;
    while (k > 0) {
      nodes[k] = fn(nodes[k * 2], nodes[k * 2 + 1]);
      k /= 2;
    }
  }

  /**
   * もとの i 番目と value に fn を適用したものを i 番目に設定する
   * @param i
   * @param value
   */
  void add(unsigned long long i, T value) {
    long long k = size + i;
    set(i, fn(nodes[k], value));
  }
};


///////////////////////////////////////////////////////////////////////////////
// statistics::model::functional::log_likelihood_accumulator.hpp             //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_MODEL_FUNCTIONAL_LOG_LIKELIHOOD_ACCUMULATOR_HPP_ER_2009
#define BOOST_STATISTICS_MODEL_FUNCTIONAL_LOG_LIKELIHOOD_ACCUMULATOR_HPP_ER_2009
#include <boost/concept_check.hpp>
#include <boost/type_traits.hpp>
#include <boost/call_traits.hpp>
#include <boost/operators.hpp>
#include <boost/statistics/model/wrap/aggregate/model_parameter.hpp>
#include <boost/statistics/model/wrap/aggregate/model_data.hpp>
#include <boost/joint_dist/unscope/log_unnormalized_pdf.hpp>
#include <boost/scalar_dist/unscope/log_unnormalized_pdf.hpp>
#include <boost/statistics/model/concept/log_likelihood.hpp>

namespace boost{

    // fwd declare
    template<typename T,typename M,typename X,typename Y,typename P>
    T log_likelihood(
        statistics::model::model_data_<M,X,Y>,
        const P&
    );

namespace statistics{
namespace model{

// Functor that accumulates the data contribution to the likelihood of a model
// and its parameter.
//
// Used by algorithm::log_likelihood
template<
    typename T, // result_type
    typename M, // Model
    typename P  // parameter
>
class log_likelihood_accumulator : boost::addable<
    log_likelihood_accumulator<T,M,P>
>{
public:
    typedef T                        result_type;
    typedef model_parameter_<M,P>    model_parameter_type;

    // Construction
    log_likelihood_accumulator();
    log_likelihood_accumulator(model_parameter_type);
    log_likelihood_accumulator(const log_likelihood_accumulator& );
    log_likelihood_accumulator& operator=(const log_likelihood_accumulator& );

    // Operator
    log_likelihood_accumulator& operator+=(
        const log_likelihood_accumulator& that
    );

    // Update
    template<typename X,typename Y>
    result_type operator()(const X&,const Y& y);

    // Access
    const model_parameter_type& model_parameter()const;
    result_type value()const;

private:
    model_parameter_type mp_;
    result_type cum_sum_;
    static result_type zero_;
};

    // Implementation //

template<typename T,typename M,typename P>
typename log_likelihood_accumulator<T,M,P>::result_type
log_likelihood_accumulator<T,M,P>::zero_ = static_cast<result_type>(0);

// Construction
template<typename T,typename M,typename P>
log_likelihood_accumulator<T,M,P>::log_likelihood_accumulator()
:mp_(),cum_sum_(zero_){}

template<typename T,typename M,typename P>
log_likelihood_accumulator<T,M,P>::log_likelihood_accumulator(
    model_parameter_type mp
):mp_(mp),cum_sum_(zero_){}

template<typename T,typename M,typename P>
log_likelihood_accumulator<T,M,P>::log_likelihood_accumulator(
    const log_likelihood_accumulator& that
):mp_(that.mp_),cum_sum_(that.cum_sum_){}

template<typename T,typename M,typename P>
log_likelihood_accumulator<T,M,P>&
log_likelihood_accumulator<T,M,P>::operator=(
    const log_likelihood_accumulator& that
){
    if(&that!=this){
        mp_ = that.mp_;
        cum_sum_ = that.cum_sum_;
    }
    return (*this);
}

// Operator

template<typename T,typename M,typename P>
log_likelihood_accumulator<T,M,P>&
log_likelihood_accumulator<T,M,P>::operator+=(
    const log_likelihood_accumulator& that
){
    (this->cum_sum_)+= that.value();
}

// Update
template<typename T,typename M,typename P>
template<typename X,typename Y>
typename log_likelihood_accumulator<T,M,P>::result_type
log_likelihood_accumulator<T,M,P>::operator()(const X& x,const Y& y){

    // TODO see compile error by uncommenting, e.g. survival_model
    // BOOST_CONCEPT_ASSERT((
    //  HasLogLikelihood<T,M,X,Y,P>
    // ));

    result_type l = log_likelihood<T>(
        make_model_data(
            model_parameter().model(),
            x,
            y
        ),
        model_parameter().parameter()
    );
    cum_sum_ += l;
    return cum_sum_;
}

// Access

template<typename T,typename M,typename P>
const typename log_likelihood_accumulator<T,M,P>::model_parameter_type&
log_likelihood_accumulator<T,M,P>::model_parameter()const{ return mp_; }

template<typename T,typename M,typename P>
typename log_likelihood_accumulator<T,M,P>::result_type
log_likelihood_accumulator<T,M,P>::value()const{ return cum_sum_; }

}// model
}// statistics
}// boost

#endif


/*
 * Copyright (c) 2013-2014 Masahide Kashiwagi (kashi@waseda.jp)
 */

/*
 * test program for rounded math emulation
 *  compare add, sub, mul, div and sqrt under
 *  - changing rounding mode
 *  - emulating rounded math using twosum and twoproduct
 */

#include <fenv.h>
#include <boost/random.hpp>
#include <ctime>

#ifndef NT
#define NT 10000000
#endif

struct hwround {
	public:

	static void roundnear() {
		fesetround(FE_TONEAREST);
	}

	static void rounddown() {
		fesetround(FE_DOWNWARD);
	}

	static void roundup() {
		fesetround(FE_UPWARD);
	}

	static void roundchop() {
		fesetround(FE_TOWARDZERO);
	}

	static double add_up(const double& x, const double& y) {
		volatile double r, x1 = x, y1 = y;
		roundup();
		r = x1 + y1;
		roundnear();
		return r;
	}

	static double add_down(const double& x, const double& y) {
		volatile double r, x1 = x, y1 = y;
		rounddown();
		r = x1 + y1;
		roundnear();
		return r;
	}

	static double sub_up(const double& x, const double& y) {
		volatile double r, x1 = x, y1 = y;
		roundup();
		r = x1 - y1;
		roundnear();
		return r;
	}

	static double sub_down(const double& x, const double& y) {
		volatile double r, x1 = x, y1 = y;
		rounddown();
		r = x1 - y1;
		roundnear();
		return r;
	}

	static double mul_up(const double& x, const double& y) {
		volatile double r, x1 = x, y1 = y;
		roundup();
		r = x1 * y1;
		roundnear();
		return r;
	}

	static double mul_down(const double& x, const double& y) {
		volatile double r, x1 = x, y1 = y;
		rounddown();
		r = x1 * y1;
		roundnear();
		return r;
	}

	static double div_up(const double& x, const double& y) {
		volatile double r, x1 = x, y1 = y;
		roundup();
		r = x1 / y1;
		roundnear();
		return r;
	}

	static double div_down(const double& x, const double& y) {
		volatile double r, x1 = x, y1 = y;
		rounddown();
		r = x1 / y1;
		roundnear();
		return r;
	}

	static double sqrt_up(const double& x) {
		volatile double r, x1 = x;
		roundup();
		r = sqrt(x1);
		roundnear();
		return r;
	}

	static double sqrt_down(const double& x) {
		volatile double r, x1 = x;
		rounddown();
		r = sqrt(x1);
		roundnear();
		return r;
	}
};

struct nohwround {

	static void fasttwosum(const double& a, const double& b, double& x, double& y) {
		double tmp;
		x = a + b;
		tmp = x - a;
		y = b - tmp;
	}

	#if 0
	static void twosum(const double& a, const double& b, double& x, double& y) {
		double tmp;
		x = a + b;
		tmp = x - a;
		if (std::fabs(tmp) == std::numeric_limits<double>::infinity()) {
			tmp = x * 0.5 - a * 0.5;
			y = ((a * 0.5 - (x * 0.5 - tmp)) + (b * 0.5 - tmp)) * 2.;
		} else {
			y = (a - (x - tmp)) + (b - tmp);
		}
	}
	#endif

	static void twosum(const double& a, const double& b, double& x, double& y) {
		double tmp;

		x = a + b;
		if (std::fabs(a) > std::fabs(b)) {
			tmp = x - a;
			y = b - tmp;
		} else {
			tmp = x - b;
			y = a - tmp;
		}
	}

	static void split(const double& a, double& x, double& y) {
		static const double sigma = ldexp(1., 27) + 1.;
		double tmp;

		tmp = a * sigma;
		x = tmp - (tmp - a);
		y = a - x;
	}

	static void twoproduct(const double& a, const double& b, double& x, double& y) {
		static const double th = ldexp(1., 996);
		static const double c1 = ldexp(1., -28);
		static const double c2 = ldexp(1., 28);
		static const double th2 = ldexp(1., 1023);

		double na, nb, a1, a2, b1, b2;

		x = a * b;
		#if 0
		if (std::fabs(x) == std::numeric_limits<double>::infinity()) {
			y = 0.;
			return;
		}
		#endif
		if (std::fabs(a) > th) {
			na = a * c1;
			nb = b * c2;
		} else if (std::fabs(b) > th) {
			na = a * c2;
			nb = b * c1;
		} else {
			na = a;
			nb = b;
		}
		split(na, a1, a2);
		split(nb, b1, b2);
		if (std::fabs(x) > th2) {
			y = a2 * b2 - ((((x * 0.5) - (a1 * 0.5)  * b1) * 2. - a2 * b1) - a1 * b2);
		} else {
			y = a2 * b2 - (((x - a1 * b1) - a2 * b1) - a1 * b2);
		}
	}

	// succ and pred by Rump

	static double succ(const double& x) {
		static const double th1 = ldexp(1., -969);
		static const double th2 = ldexp(1., -1021);
		static const double c1 = ldexp(1., -53) + ldexp(1., -105);
		static const double c2 = ldexp(1., -1074);
		static const double c3 = ldexp(1., 53);
		static const double c4 = ldexp(1., -53);

		double a, c, e;

		a = std::fabs(x);
		if (a >= th1) return x + a * c1;
		if (a < th2) return x + c2;
		c = c3 * x;
		e = c1 * std::fabs(c);
		return (c + e) * c4;
	}

	static double pred(const double& x) {
		static const double th1 = ldexp(1., -969);
		static const double th2 = ldexp(1., -1021);
		static const double c1 = ldexp(1., -53) + ldexp(1., -105);
		static const double c2 = ldexp(1., -1074);
		static const double c3 = ldexp(1., 53);
		static const double c4 = ldexp(1., -53);

		double a, c, e;

		a = std::fabs(x);
		if (a >= th1) return x - a * c1;
		if (a < th2) return x - c2;
		c = c3 * x;
		e = c1 * std::fabs(c);
		return (c - e) * c4;
	}

	static double add_up(const double& x, const double& y) {
		double r, r2;

		twosum(x, y, r, r2);
		if (r == std::numeric_limits<double>::infinity()) {
			return r;
		} else if (r == -std::numeric_limits<double>::infinity()) {
			if (x == -std::numeric_limits<double>::infinity() || y == -std::numeric_limits<double>::infinity()) {
				return r;
			} else {
				return -(std::numeric_limits<double>::max)();
			}
		}

		if (r2 > 0.) {
			return succ(r);
		}

		return r;
	}

	static double add_down(const double& x, const double& y) {
		double r, r2;

		twosum(x, y, r, r2);
		if (r == std::numeric_limits<double>::infinity()) {
			if (x == std::numeric_limits<double>::infinity() || y == std::numeric_limits<double>::infinity()) {
				return r;
			} else {
				return (std::numeric_limits<double>::max)();
			}
		} else if (r == -std::numeric_limits<double>::infinity()) {
			return r;
		}

		if (r2 < 0.) {
			return pred(r);
		}

		return r;
	}

	static double sub_up(const double& x, const double& y) {
		double r, r2;

		twosum(x, -y, r, r2);
		if (r == std::numeric_limits<double>::infinity()) {
			return r;
		} else if (r == -std::numeric_limits<double>::infinity()) {
			if (x == -std::numeric_limits<double>::infinity() || y == std::numeric_limits<double>::infinity()) {
				return r;
			} else {
				return -(std::numeric_limits<double>::max)();
			}
		}

		if (r2 > 0.) {
			return succ(r);
		}

		return r;
	}

	static double sub_down(const double& x, const double& y) {
		double r, r2;

		twosum(x, -y, r, r2);
		if (r == std::numeric_limits<double>::infinity()) {
			if (x == std::numeric_limits<double>::infinity() || y == -std::numeric_limits<double>::infinity()) {
				return r;
			} else {
				return (std::numeric_limits<double>::max)();
			}
		} else if (r == -std::numeric_limits<double>::infinity()) {
			return r;
		}

		if (r2 < 0.) {
			return pred(r);
		}

		return r;
	}

	static double mul_up(const double& x, const double& y) {
		double r, r2;
		double x1, y1;
		double s, s2, t;
		static const double th = ldexp(1., -969); // -1074 + 106 - 1
		static const double c = ldexp(1., 537); // 1074 / 2

		// if (x == 0. || y == 0.) return x * y;

		twoproduct(x, y, r, r2);
		if (r == std::numeric_limits<double>::infinity()) {
			return r;
		} else if (r == -std::numeric_limits<double>::infinity()) {
			if (std::fabs(x) == std::numeric_limits<double>::infinity() || std::fabs(y) == std::numeric_limits<double>::infinity()) {
				return r;
			} else {
				return -(std::numeric_limits<double>::max)();
			}
		}

		if (fabs(r) >= th) {
			if (r2 > 0.) return succ(r);
			return r;
		} else {
			twoproduct(x * c, y * c, s, s2);
			t = (r * c) * c;
			if ( t < s || (t == s && s2 > 0.)) {
				return succ(r);
			}
			return r;
		}
	}

	static double mul_down(const double& x, const double& y) {
		double r, r2;
		double x1, y1;
		double s, s2, t;
		static const double th = ldexp(1., -969); // -1074 + 106 - 1
		static const double c = ldexp(1., 537); // 1074 / 2

		// if (x == 0. || y == 0.) return x * y;

		twoproduct(x, y, r, r2);
		if (r == std::numeric_limits<double>::infinity()) {
			if (std::fabs(x) == std::numeric_limits<double>::infinity() || std::fabs(y) == std::numeric_limits<double>::infinity()) {
				return r;
			} else {
				return (std::numeric_limits<double>::max)();
			}
		} else if (r == -std::numeric_limits<double>::infinity()) {
			return r;
		}

		if (fabs(r) >= th) {
			if (r2 < 0.) return pred(r);
			return r;
		} else {
			twoproduct(x * c, y * c, s, s2);
			t = (r * c) * c;
			if ( t > s || (t == s && s2 < 0.)) {
				return pred(r);
			}
			return r;
		}
	}

	static double div_up(const double& x, const double& y) {
		double r, r2;
		double xn, yn, d;
		static const double th1 = ldexp(1., -969); // -1074 + 106 - 1
		static const double th2 = ldexp(1., 918); // 1023 - 105
		static const double c1 = ldexp(1., 105); // -969 - (-1074)
		static const double c2 = ldexp(1., -1074);

		if (x == 0. || y == 0. || std::fabs(x) == std::numeric_limits<double>::infinity() || std::fabs(y) == std::numeric_limits<double>::infinity() || x != x  || y != y) {
			return x / y;
		}

		if (y < 0.) {
			xn = -x;
			yn = -y;
		} else {
			xn = x;
			yn = y;
		}

		if (fabs(xn) < th1) {
			if (fabs(yn) < th2) {
				xn *= c1;
				yn *= c1;
			} else {
				if (xn < 0.) return 0.;
				else return c2;
			}
		}

		d = xn / yn;

		if (d == std::numeric_limits<double>::infinity()) {
			return d;
		} else if (d == -std::numeric_limits<double>::infinity()) {
			return -(std::numeric_limits<double>::max)();
		}

		twoproduct(d, yn, r, r2);
		if ( r < xn || ((r == xn) && r2 < 0.)) {
			return succ(d);
		}
		return d;
	}

	static double div_down(const double& x, const double& y) {
		double r, r2;
		double xn, yn, d;
		static const double th1 = ldexp(1., -969); // -1074 + 106 - 1
		static const double th2 = ldexp(1., 918); // 1023 - 105
		static const double c1 = ldexp(1., 105); // -969 - (-1074)
		static const double c2 = ldexp(1., -1074);

		if (x == 0. || y == 0. || std::fabs(x) == std::numeric_limits<double>::infinity() || std::fabs(y) == std::numeric_limits<double>::infinity() || x != x  || y != y) {
			return x / y;
		}

		if (y < 0.) {
			xn = -x;
			yn = -y;
		} else {
			xn = x;
			yn = y;
		}

		if (fabs(xn) < th1) {
			if (fabs(yn) < th2) {
				xn *= c1;
				yn *= c1;
			} else {
				if (xn < 0.) return -c2;
				else return 0.;
			}
		}

		d = xn / yn;

		if (d == std::numeric_limits<double>::infinity()) {
			return (std::numeric_limits<double>::max)();
		} else if (d == -std::numeric_limits<double>::infinity()) {
			return d;
		}

		twoproduct(d, yn, r, r2);
		if ( r > xn || ((r == xn) && r2 > 0.)) {
			return pred(d);
		}
		return d;
	}

	static double sqrt_up(const double& x) {
		double r, r2, d;
		static const double th1 = ldexp(1., -969); // -1074 + 106 - 1
		static const double c1 = ldexp(1., 106); // -969 - (-1074) + 1
		static const double c2 = ldexp(1., 53); // sqrt(c1)

		d = sqrt(x);

		if (x < th1) {
			double d2, x2;
			x2 = x * c1;
			d2 = d * c2;
			twoproduct(d2, d2, r, r2);
			if ( r < x2 || (r == x2 && r2 < 0.)) {
				return succ(d);
			}
			return d;
		}

		twoproduct(d, d, r, r2);
		if ( r < x || (r == x && r2 < 0.)) {
			return succ(d);
		}
		return d;
	}

	static double sqrt_down(const double& x) {
		double r, r2, d;
		static const double th1 = ldexp(1., -969); // -1074 + 106 - 1
		static const double c1 = ldexp(1., 106); // -969 - (-1074) + 1
		static const double c2 = ldexp(1., 53); // sqrt(c1)

		d = sqrt(x);

		if (x < th1) {
			double d2, x2;
			x2 = x * c1;
			d2 = d * c2;
			twoproduct(d2, d2, r, r2);
			if ( r > x2 || (r == x2 && r2 > 0.)) {
				return pred(d);
			}
			return d;
		}

		twoproduct(d, d, r, r2);
		if ( r > x || (r == x && r2 > 0.)) {
			return pred(d);
		}
		return d;
	}

};

bool samedouble(double x, double y)
{
	// return *((unsigned long long *)(&x)) == *((unsigned long long *)(&y));
	if (x != x && y != y) return true;
	return x == y;
}

void check(double x, double y)
{
	volatile double r1, r2;

	r1 = hwround::add_up(x, y);
	r2 = nohwround::add_up(x, y);
	if (!samedouble(r1, r2)) {
		std::cout << "add_up error\n"; std::cout << x << "\n";
		std::cout << y << "\n";
		std::cout << r1 << "\n";
		std::cout << r2 << "\n";
	}

	r1 = hwround::add_down(x, y);
	r2 = nohwround::add_down(x, y);
	if (!samedouble(r1, r2)) {
		std::cout << "add_down error\n";
		std::cout << x << "\n";
		std::cout << y << "\n";
		std::cout << r1 << "\n";
		std::cout << r2 << "\n";
	}

	r1 = hwround::sub_up(x, y);
	r2 = nohwround::sub_up(x, y);
	if (!samedouble(r1, r2)) {
		std::cout << "sub_up error\n";
		std::cout << x << "\n";
		std::cout << y << "\n";
		std::cout << r1 << "\n";
		std::cout << r2 << "\n";
	}

	r1 = hwround::sub_down(x, y);
	r2 = nohwround::sub_down(x, y);
	if (!samedouble(r1, r2)) {
		std::cout << "sub_down error\n";
		std::cout << x << "\n";
		std::cout << y << "\n";
		std::cout << r1 << "\n";
		std::cout << r2 << "\n";
	}

	r1 = hwround::mul_up(x, y);
	r2 = nohwround::mul_up(x, y);
	if (!samedouble(r1, r2)) {
		std::cout << "mul_up error\n";
		std::cout << x << "\n";
		std::cout << y << "\n";
		std::cout << r1 << "\n";
		std::cout << r2 << "\n";
	}

	r1 = hwround::mul_down(x, y);
	r2 = nohwround::mul_down(x, y);
	if (!samedouble(r1, r2)) {
		std::cout << "mul_down error\n";
		std::cout << x << "\n";
		std::cout << y << "\n";
		std::cout << r1 << "\n";
		std::cout << r2 << "\n";
	}

	r1 = hwround::div_up(x, y);
	r2 = nohwround::div_up(x, y);
	if (!samedouble(r1, r2)) {
		std::cout << "div_up error\n";
		std::cout << x << "\n";
		std::cout << y << "\n";
		std::cout << r1 << "\n";
		std::cout << r2 << "\n";
	}

	r1 = hwround::div_down(x, y);
	r2 = nohwround::div_down(x, y);
	if (!samedouble(r1, r2)) {
		std::cout << "div_down error\n";
		std::cout << x << "\n";
		std::cout << y << "\n";
		std::cout << r1 << "\n";
		std::cout << r2 << "\n";
	}

	r1 = hwround::sqrt_up(x);
	r2 = nohwround::sqrt_up(x);
	if (!samedouble(r1, r2)) {
		std::cout << "sqrt_up error\n";
		std::cout << x << "\n";
		std::cout << r1 << "\n";
		std::cout << r2 << "\n";
	}

	r1 = hwround::sqrt_down(x);
	r2 = nohwround::sqrt_down(x);
	if (!samedouble(r1, r2)) {
		std::cout << "sqrt_down error\n";
		std::cout << x << "\n";
		std::cout << r1 << "\n";
		std::cout << r2 << "\n";
	}
}

int main() {
	double x, y;
	int i, j;
	unsigned long long t;

	double specials[11] = {
		0.,
		-0.,
		std::numeric_limits<double>::infinity(),
		-std::numeric_limits<double>::infinity(),
		(std::numeric_limits<double>::max)(),
		-(std::numeric_limits<double>::max)(),
		(std::numeric_limits<double>::min)(),
		-(std::numeric_limits<double>::min)(),
		std::numeric_limits<double>::denorm_min(),
		-std::numeric_limits<double>::denorm_min()
	};
	specials[10] = specials[2] + specials[3]; // making NaN

	boost::variate_generator<boost::mt19937, boost::uniform_int<unsigned long long> > rand(boost::mt19937(time(0)), boost::uniform_int<unsigned long long>(0, -1));

	std::cout.precision(17);

	// cause overflow of intermediate variable in twoproduct
	// x = 6.929001713869936e+236;
	// y = 2.5944475251952003e+71;
	// check(x, y);

	// cause overflow of intermediate variable in twosum
	// x = 3.5630624444874539e+307;
	// y = -1.7976931348623157e+308;
	// check(x, y);

	// check general-general case

	for (i=0; i<NT; i++) {
		t = rand();
		x = *((double*)(&t));
		t = rand();
		y = *((double*)(&t));
		check(x, y);
	}

	// check general-special case

	for (i=0; i<NT; i++) {
		t = rand();
		x = *((double*)(&t));
		for (j=0; j<11; j++) {
			y = specials[j];
			check(x, y);
			check(y, x);
		}
	}

	// check special-special case

	for (i=0; i<11; i++) {
		x = specials[i];
		for (j=0; j<11; j++) {
			y = specials[j];
			check(x, y);
		}
	}
}


#include "edlib/edlib.hpp"

#include <Eigen/Dense>

#include <fstream>
#include <ios>
#include <iostream>
#include <random>

int main()
{
    using namespace edlib;
    using UINT = uint32_t;

    constexpr int N = 20;

    std::cout << "#N: " << N << std::endl;

    std::vector<double> hs;
    for(int i = 0; i <= 20; i++)
    {
        hs.emplace_back(i * 0.1);
    }
    for(auto h : hs)
    {
        std::array<Eigen::VectorXd, 2> ev;
        {
            Basis1DZ2<UINT> basis(N, 0, 1, false);
            TITFIsing<UINT> ham(basis, 1.0, h);
            const int dim = basis.getDim();

            NodeMV mv(dim, 0, dim, ham);
            auto solver = ArpackSolver(mv, dim);

            if(solver.solve(2) != ErrorType::NormalExit)
            {
                return 1;
            }
            ev[0] = solver.eigenvalues();
        }
        {
            Basis1DZ2<UINT> basis(N, 0, -1, false);
            TITFIsing<UINT> ham(basis, 1.0, h);
            const int dim = basis.getDim();

            NodeMV mv(dim, 0, dim, ham);
            auto solver = ArpackSolver(mv, dim);

            if(solver.solve(2) != ErrorType::NormalExit)
            {
                return 1;
            }
            ev[1] = solver.eigenvalues();
        }
        printf("%f\t%.10f\t%.10f\t%.10f\t%.10f\n", h, ev[0](0), ev[0](1), ev[1](0), ev[1](1));
    }

    return 0;
}


//  (C) Copyright Nick Thompson 2020.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <random>
#include <benchmark/benchmark.h>
#include <boost/math/special_functions/rsqrt.hpp>
#include <boost/multiprecision/float128.hpp>
#include <boost/multiprecision/mpfr.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

using boost::multiprecision::number;
using boost::multiprecision::mpfr_float_backend;
using boost::multiprecision::float128;
using boost::multiprecision::cpp_bin_float_50;
using boost::multiprecision::cpp_bin_float_100;
using boost::math::rsqrt;

template<class Real>
void Rsqrt(benchmark::State& state)
{
    std::random_device rd;
    std::mt19937_64 mt(rd());
    std::uniform_real_distribution<long double> unif(1,100);

    Real x = static_cast<Real>(unif(mt));
    for (auto _ : state)
    {
        benchmark::DoNotOptimize(rsqrt(x));
        x += std::numeric_limits<Real>::epsilon();
    }
}

BENCHMARK_TEMPLATE(Rsqrt, float);
BENCHMARK_TEMPLATE(Rsqrt, double);
BENCHMARK_TEMPLATE(Rsqrt, long double);
BENCHMARK_TEMPLATE(Rsqrt, float128);
BENCHMARK_TEMPLATE(Rsqrt, number<mpfr_float_backend<100>>);
BENCHMARK_TEMPLATE(Rsqrt, number<mpfr_float_backend<200>>);
BENCHMARK_TEMPLATE(Rsqrt, number<mpfr_float_backend<300>>);
BENCHMARK_TEMPLATE(Rsqrt, number<mpfr_float_backend<400>>);
BENCHMARK_TEMPLATE(Rsqrt, number<mpfr_float_backend<1000>>);
BENCHMARK_TEMPLATE(Rsqrt, cpp_bin_float_50);
BENCHMARK_TEMPLATE(Rsqrt, cpp_bin_float_100);

BENCHMARK_MAIN();


//
//  cal_M.hpp
//  hybrid_fem_bie
//
//  Created by Max on 2/10/18.
//
//

#ifndef cal_M_hpp
#define cal_M_hpp

#include <stdio.h>
#include <Eigen/Eigen>
#include <Eigen/KroneckerProduct>

using namespace Eigen;
void cal_M(MatrixXd coord, double density, MatrixXd &M_el);

#endif /* cal_M_hpp */


///////////////////////////////////////////////////////////////
//  Copyright 2013 Christopher Kormanyos. 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_
//

// Test case for ticket:
// #8065: Multiprecision rounding issue

#ifdef _MSC_VER
#  define _SCL_SECURE_NO_WARNINGS
#endif

#include <boost/detail/lightweight_test.hpp>
#include "test.hpp"
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/math/special_functions/round.hpp>

template<int N>
static bool round_test_imp()
{
  typedef boost::multiprecision::cpp_dec_float<N> mp_backend_type;
  typedef boost::multiprecision::number<mp_backend_type, boost::multiprecision::et_off> mp_type;

  const mp_type original_digits(1.0F);

  const mp_type scale = pow(mp_type(10), N);

  mp_type these_digits = original_digits * scale;

  these_digits  = boost::math::round(these_digits);
  these_digits /= scale;

  const std::string result = these_digits.str();

  return (result == original_digits.str());
}

template<int N>
struct round_test
{
  static bool test()
  {
    return (round_test_imp<N>() && round_test<N - 1>::test());
  }
};

template<>
struct round_test<0>
{
  static bool test()
  {
    return round_test_imp<0>();
  }
};

int main()
{
   //
   // Test cpp_dec_float rounding with boost::math::round() at various precisions:
   //
   const bool round_test_result = round_test<40>::test();

   BOOST_CHECK_EQUAL(round_test_result, true);

   return boost::report_errors();
}


/*
    Boost Competency Test - GSoC 2020
    digu_J - Digvijay Janartha
    NIT Hamirpur - INDIA
*/

#include <algorithm>
#include <iostream>
#include <iterator>
#include <utility>
#include <vector>
#include <random>
#include <chrono>

#include <boost/geometry/geometry.hpp>

#include "../includes/convex_hull_gift_wrapping.hpp"

namespace bg = boost::geometry;
using bg::dsv;

typedef bg::model::point<double, 2, bg::cs::cartesian> point_t;
typedef bg::model::multi_point<point_t> mpoint_t;
typedef bg::model::polygon<point_t> polygon_t;

std::mt19937_64 rang(std::chrono::high_resolution_clock::now().time_since_epoch().count());

int my_rand(int l, int r)
{
    if (l > r)
    {
        std::swap(l, r);
    }
	std::uniform_int_distribution <int> uid(l, r);
	return uid(rang);
}

bool test(mpoint_t &hull1, mpoint_t &hull2)
{
    // Checks if the convex hull generated by gift wrapping is the same as the convex hull generated by boost geometry.
    polygon_t p1, p2;
    for (int i = 0; i < boost::size(hull1); ++i)
    {
        bg::append(p1, hull1[i]);
    }
    for (int i = int(boost::size(hull2)) - 1; i >= 0; --i)
    {
        bg::append(p2, hull2[i]);
    }
    if (bg::covered_by(p1, p2) and bg::covered_by(p2, p1)) // both geometries must cover themselves to be same
    {
        return true;
    }
    std::cout << dsv(p1) << "\n";
    std::cout << dsv(p2) << "\n";
    return false;
}

int main()
{
    std::cout << std::fixed << std::setprecision(0);
    #ifdef HOME
        freopen("input.txt", "r", stdin);
        freopen("output.txt", "w", stdout);
    #endif

    for (int tt = 1; tt <= 500; ++tt) // 500 random test cases
    {
        mpoint_t mpt1, hull1, hull2;

        int points = 30;
        for (int i = 0; i < points; ++i)
        {
            int x = my_rand(0, 100);
            int y = my_rand(0, 100);
            bg::append(mpt1, point_t(x, y));
        }

        algo1::GiftWrapping(mpt1, hull1);
        bg::convex_hull(mpt1, hull2);
        assert(test(hull1, hull2) == true);
    }

    return 0;
}


#pragma once

#include "Rational.hpp"

#include <Eigen/Core>

namespace eccd
{

typedef Eigen::Matrix<Rational, 3, 1> Vector3r;
typedef Eigen::Matrix<double, 3, 1> Vector3d;

template <typename V1, typename V2>
Vector3r cross(const V1 &v1, const V2 &v2)
{
    Vector3r res;
    res[0] = v1[1] * v2[2] - v1[2] * v2[1];
    res[1] = v1[2] * v2[0] - v1[0] * v2[2];
    res[2] = v1[0] * v2[1] - v1[1] * v2[0];

    return res;
}

template<typename V>
void print(const V &v)
{
    std::cout << v[0] << " " << v[1] << " " << v[2] << std::endl;
}

void write(const Vector3d &v, std::ostream &out);
Vector3d read(std::istream &in);

int orient3d(const Vector3r &a, const Vector3r &b, const Vector3r &c, const Vector3r &d);

int origin_ray_triangle_inter(const Vector3d &dir, const Vector3r &t1, const Vector3r &t2, const Vector3r &t3);

bool segment_segment_inter(const Vector3r &s0, const Vector3r &e0, const Vector3r &s1, const Vector3r &e1, Vector3r &res, int axis);

int segment_triangle_inter(const Vector3d &e0, const Vector3d &e1, const Vector3d &t1, const Vector3d &t2, const Vector3d &t3);

} // namespace eccd


//
//  rsaUtil.hpp
//  kcalg
//
//  Created by knightc on 2019/8/8.
//  Copyright © 2019 knightc. All rights reserved.
//

#ifndef rsaUtil_hpp
#define rsaUtil_hpp

#include <NTL/ZZ.h>

using namespace NTL;
using namespace std;

void oula(const ZZ p , const ZZ q , ZZ &n );

#endif /* rsaUtil_hpp */


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    Constant radian in Degree multiplier, \f$\frac{180}\pi\f$.

    @par Header <boost/simd/constant/deginrad.hpp>

    @par Semantic:

    For type T:

    @code
    T r = Deginrad<T>();
    @endcode

    is similar to:

    @code
    T r = Ratio<T, 180>()/Pi<T>() ;
    @endcode

    @see  inrad, indeg, Radindeg, Radindegr, Ratio
    @return a value of type T

**/
  template<typename T> T Deginrad();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      Constant radian in Degree multiplier, \f$\frac{180}\pi\f$.

      Generate the  constant deginrad.

      @return The Deginrad constant for the proper type
    **/
    Value Deginrad();
  }
} }
#endif

#include <boost/simd/constant/scalar/deginrad.hpp>
#include <boost/simd/constant/simd/deginrad.hpp>

#endif


#include <iostream>
#include <unistd.h>
#include <vector>
#include "OrthonormalHermite.h"
#include <Eigen/Dense>

void printDetailsOfObject(unsigned int longSleep, unsigned int shortSleep, APPRSDK::OrthonormalHermite<double>& H1, bool showPartialDerivatives=false)
{
    std::cout<<"Base points"<<std::endl;
    usleep(shortSleep);
    std::cout<<H1.GetDomain()<<std::endl;
    std::cout<<"END OF BASE POINTS"<<std::endl<<std::endl;
    usleep(longSleep);

    std::cout<<"Retrieving calculated first degree Hermite function"<<std::endl;
    usleep(shortSleep);
    std::cout<<H1.GetFunctionSystem().col(0)<<std::endl;
    std::cout<<"END OF HERMITE FUNCTION OUTPUT"<<std::endl<<std::endl;
    usleep(longSleep);

    std::cout<<"Retrieving calculated third degree Hermite function"<<std::endl;
    usleep(shortSleep);
    std::cout<<H1.GetFunctionSystem().col(4)<<std::endl;
    std::cout<<"END OF HERMITE FUNCTION OUTPUT"<<std::endl<<std::endl;
    usleep(longSleep);

    std::cout<<"Retrieving calculated derivative first degree"<<std::endl;
    usleep(shortSleep);
    std::cout<<H1.GetDFunctionSystem().col(0)<<std::endl;
    std::cout<<"END OF DERIVATIVE OUTPUT"<<std::endl;
    usleep(longSleep);

    std::cout<<"Retrieving calculated derivative third degree"<<std::endl;
    usleep(shortSleep);
    std::cout<<H1.GetDFunctionSystem().col(4)<<std::endl;
    std::cout<<"END OF DERIVATIVE OUTPUT"<<std::endl;
    usleep(longSleep);

    if (showPartialDerivatives)
    {
        std::cout<<"Retrieving partial derivatives third and fourth columns"<<std::endl;
        usleep(shortSleep);
        std::cout<<H1.GetPartialDerivativesFunctionSystem().col(4)<<std::endl;
        std::cout<<"END OF THIRD COLUMN"<<std::endl;
        std::cout<<H1.GetPartialDerivativesFunctionSystem().col(5)<<std::endl;
        std::cout<<"END OF PARTIAL DERIVATIVE OUTPUT"<<std::endl;
        usleep(longSleep);

        std::cout<<"Retrieving index values"<<std::endl;
        usleep(shortSleep);
        std::cout<<H1.GetIndex()<<std::endl;
        std::cout<<"END OF INDEX OUTPUT"<<std::endl;
    }
}

int main()
{
    const unsigned int longSleep = 3000;
    const unsigned int shortSleep = 1000;
    Eigen::Matrix<double, 1, 2> params;

    std::cout<<"Function system test begun..."<<std::endl;
    APPRSDK::OrthonormalHermite<double> H1(100, 10);
    std::cout<<"OrthonormalHermite object succesfully created"<<std::endl;

    printDetailsOfObject(longSleep, shortSleep, H1);

    std::cout<<"Applying parameters: lambda = 0.5, t = 50"<<std::endl;
    params(0,0) = 0.5;
    params(0,1) = 50;
    H1.ApplyNonLinearParameters(params);
    std::cout<<"Paramtere application succesful. Printing results...";
    usleep(shortSleep);

    printDetailsOfObject(longSleep, shortSleep, H1, true);

    return 0;
}


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

/*
 Copyright (C) 2015 Andres Hernandez

 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 isotropicrandomwalk.hpp
    \brief Isotropic random walk
*/

#ifndef quantlib_isotropic_random_walk_hpp
#define quantlib_isotropic_random_walk_hpp

#include <ql/mathconstants.hpp>
#include <ql/math/randomnumbers/mt19937uniformrng.hpp>
#include <ql/math/array.hpp>
#include <boost/random/variate_generator.hpp>

namespace QuantLib {

    //! Isotropic random walk
    /*! A variate is used to draw from a random element of a
        probability distribution. The draw corresponds to the
        radius of a d-dimensional sphere. The position on the
        surface of the d-dimensional sphere is randomly chosen
        with all points on the surface having the same probability,
        i.e. all directions are isotropic and the step is randomly
        drawn from the given variate.
    */
    template <class Distribution, class Engine>
    class IsotropicRandomWalk {
      public:
        typedef boost::variate_generator<Engine, Distribution> VariateGenerator;
        IsotropicRandomWalk(const Engine& eng, Distribution dist, Size dim,
                            const Array& weights = Array(),
                            unsigned long seed = 0) :
            variate_(eng, dist), rng_(seed),
            weights_(weights), dim_(dim) {
            if (weights_.empty())
                weights_ = Array(dim, 1.0);
            else
                QL_REQUIRE(dim_ == weights_.size(), "Invalid weights");
        }
        template <class InputIterator>
        inline void nextReal(InputIterator first) const {
            Real radius = variate_();
            Array::const_iterator weight = weights_.begin();
            if (dim_ > 1) {
                //Isotropic random direction
                Real phi = M_PI*rng_.nextReal();
                for (Size i = 0; i < dim_ - 2; i++) {
                    *first++ = radius*cos(phi)*(*weight++);
                    radius *= sin(phi);
                    phi = M_PI*rng_.nextReal();
                }
                *first++ = radius*cos(2.0*phi)*(*weight++);
                *first = radius*sin(2.0*phi)*(*weight);
            }
            else {
                if (rng_.nextReal() < 0.5)
                    *first = -radius*(*weight);
                else
                    *first = radius*(*weight);
            }
        }
        inline void setDimension(Size dim) {
            dim_ = dim;
            weights_ = Array(dim, 1.0);
        }
        inline void setDimension(Size dim, const Array& weights) {
            QL_REQUIRE(dim == weights.size(), "Invalid weights");
            dim_ = dim;
            weights_ = weights;
        }
        /*!
        The isotropic random walk will not adjust its draw to be within the lower and upper bounds,
        but if the limits are provided, they are used to rescale the sphere so as to make it to an
        ellipsoid, with different radius in different dimensions.
        */
        inline void setDimension(Size dim,
            const Array& lowerBound, const Array& upperBound) {
            QL_REQUIRE(dim == lowerBound.size(),
                "Incompatible dimension and lower bound");
            QL_REQUIRE(dim == upperBound.size(),
                "Incompatible dimension and upper bound");
            //Find largest bound
            Array bounds = upperBound - lowerBound;
            Real maxBound = bounds[0];
            for (Size j = 1; j < dim; j++) {
                if (bounds[j] > maxBound) maxBound = bounds[j];
            }
            //weights by dimension is the size of the bound
            //divided by the largest bound
            maxBound = 1.0 / maxBound;
            bounds *= maxBound;
            setDimension(dim, bounds);
        }
      protected:
        mutable VariateGenerator variate_;
        MersenneTwisterUniformRng rng_;
        Array weights_;
        Size dim_;
    };
}
#endif


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

#include <cstddef>
#include <boost/mpl/size_t.hpp>
#include <boost/mpl/integral_c.hpp>

namespace boost { namespace simd { namespace meta
{
  //////////////////////////////////////////////////////////////////////////////
  // Integral meta-function computing the power of 2 less or equal to any
  // integral constant.
  // Documentation: next_power_of_2_c.rst
  //////////////////////////////////////////////////////////////////////////////
  template<std::size_t N>
  struct prev_power_of_2_c
  {
    BOOST_STATIC_CONSTANT(std::size_t, x0    = N               );
    BOOST_STATIC_CONSTANT(std::size_t, x1    = x0 | (x0 >>  1) );
    BOOST_STATIC_CONSTANT(std::size_t, x2    = x1 | (x1 >>  2) );
    BOOST_STATIC_CONSTANT(std::size_t, x3    = x2 | (x2 >>  4) );
    BOOST_STATIC_CONSTANT(std::size_t, x4    = x3 | (x3 >>  8) );
    BOOST_STATIC_CONSTANT(std::size_t, x5    = x4 | (x4 >> 16) );
    BOOST_STATIC_CONSTANT(std::size_t, value = (x5 >> 1) + 1   );
    typedef boost::mpl::size_t<value> type;
  };
  template<std::size_t N>
  std::size_t const prev_power_of_2_c<N>::value;

  //////////////////////////////////////////////////////////////////////////////
  // Integral meta-function computing the power of 2 less or equal to any
  // Integral Constant.
  // Documentation: next_power_of_2.rst
  //////////////////////////////////////////////////////////////////////////////
  template<class N>
  struct  prev_power_of_2
        : boost::mpl::integral_c< typename N::value_type
                                , prev_power_of_2_c<N::value>::value
                                > {};

  //////////////////////////////////////////////////////////////////////////////
  // Integral meta-function computing the power of 2 greater or equal to any
  // integral constant.
  // Documentation: next_power_of_2_c.rst
  //////////////////////////////////////////////////////////////////////////////
  template<std::size_t N>
  struct next_power_of_2_c
  {
    BOOST_STATIC_CONSTANT(std::size_t, x0    = N-1             );
    BOOST_STATIC_CONSTANT(std::size_t, x1    = x0 | (x0 >>  1) );
    BOOST_STATIC_CONSTANT(std::size_t, x2    = x1 | (x1 >>  2) );
    BOOST_STATIC_CONSTANT(std::size_t, x3    = x2 | (x2 >>  4) );
    BOOST_STATIC_CONSTANT(std::size_t, x4    = x3 | (x3 >>  8) );
    BOOST_STATIC_CONSTANT(std::size_t, x5    = x4 | (x4 >> 16) );
    BOOST_STATIC_CONSTANT(std::size_t, value = x5 + 1          );
    typedef boost::mpl::size_t<value> type;
  };

  // MSVC warning fix
  template<> struct next_power_of_2_c<0> : boost::mpl::size_t<0> {};

  template<std::size_t N>
  std::size_t const next_power_of_2_c<N>::value;

  //////////////////////////////////////////////////////////////////////////////
  // Integral meta-function computing the power of 2 greater or equal to any
  // Integral Constant.
  // Documentation: next_power_of_2.rst
  //////////////////////////////////////////////////////////////////////////////
  template<class N>
  struct  next_power_of_2
        : boost::mpl::integral_c< typename N::value_type
                                , next_power_of_2_c<N::value>::value
                                > {};
} } }

#endif


#pragma once

#include <Eigen/Dense>
#include <boost/gil.hpp>
#include <boost/gil/extension/io/png.hpp>
#include <boost/iostreams/copy.hpp>
#include <boost/iostreams/filter/zlib.hpp>
#include <boost/iostreams/filtering_stream.hpp>
#include <cassert>
#include <cinttypes>
#include <fstream>
#include <iostream>
#include <limits>  // std::numeric_limits
#include <typeinfo>
#include <vector>

#include "fft.hpp"
#include "progressbar.hpp"

using namespace boost::gil;
using Eigen::MatrixXcd, Eigen::MatrixXd;
using std::vector;

// member typedefs provided through inheriting from std::iterator
template <typename Pixel_t, typename Original_t>
class channel_iterator
    : public std::iterator<
          std::random_access_iterator_tag,  // iterator_category
          Pixel_t                           // value_type
          > {
  const Original_t& original;
  size_t channel;

 public:
  explicit channel_iterator(const Original_t& original, size_t channel)
      : original(original), channel(channel){};
  const Pixel_t& operator[](size_t n) const { return original[n][channel]; };
};

template <typename ImgView>
vector<Eigen::MatrixXcd> dft(const ImgView& src) {
  typedef std::complex<double> complex;
  typedef Eigen::Matrix<complex, -1, -1, Eigen::ColMajor> mattype;

  typedef typename channel_type<ImgView>::type cs_t;

  auto h = src.height();
  auto w = src.width();
  constexpr auto nc = num_channels<ImgView>::value;
  vector<mattype> dfts;
  dfts.reserve(nc);
  for (size_t k = 0; k < nc; k++) dfts.push_back(mattype(h, w));

  progressbar bar((h + w) * nc);

  // first do fourier traffo of rows
  for (int y = 0; y < h; y++) {
    typename ImgView::x_iterator src_it = src.row_begin(y);
    for (size_t c = 0; c < nc; c++) {
      bar.update();
      std::vector<complex> buf;
      buf.resize(w);
      if constexpr (nc == 1) {
        fft(src_it, buf.data(), w);
      } else {
        auto src_channel_it =
            channel_iterator<cs_t, typename ImgView::x_iterator>(src_it, c);
        fft(src_channel_it, buf.data(), w);
      }
      for (int x = 0; x < w; x++) dfts[c](y, x) = buf[x];
    }
  }

  // now of cols
  for (int x = 0; x < w; x++) {
    for (size_t c = 0; c < nc; c++) {
      bar.update();
      std::vector<complex> buf;
      buf.resize(h);
      fft(&(dfts[c](0, x)), buf.data(), h);

      for (int y = 0; y < h; y++) dfts[c](y, x) = buf[y];
    }
  }

  return dfts;
}

template <typename ImgView>
vector<Eigen::MatrixXd> dct(const ImgView& src) {
  typedef Eigen::Matrix<double, -1, -1, Eigen::ColMajor> mattype;

  typedef typename channel_type<ImgView>::type cs_t;

  auto h = src.height();
  auto w = src.width();
  auto nc = num_channels<ImgView>::value;
  vector<mattype> dfts;
  dfts.reserve(nc);
  for (size_t k = 0; k < nc; k++) dfts.push_back(mattype(h, w));

  progressbar bar((h + w) * nc);

  // first do fourier traffo of rows
  std::vector<double> buf;
  buf.resize(w);
  for (int y = 0; y < h; y++) {
    typename ImgView::x_iterator src_it = src.row_begin(y);
    for (size_t c = 0; c < nc; c++) {
      bar.update();
      auto src_channel_it =
          channel_iterator<cs_t, typename ImgView::x_iterator>(src_it, c);
      dct(src_channel_it, buf.data(), w);
      for (int x = 0; x < w; x++) dfts[c](y, x) = buf[x];
    }
  }

  // now of cols
  buf.resize(h);
  for (int x = 0; x < w; x++) {
    for (size_t c = 0; c < nc; c++) {
      bar.update();
      dct(&(dfts[c](0, x)), buf.data(), h);

      for (int y = 0; y < h; y++) dfts[c](y, x) = buf[y];
    }
  }

  return dfts;
}

template <typename ImgView>
vector<Eigen::MatrixXd> idct(const ImgView& src) {
  typedef Eigen::Matrix<double, -1, -1, Eigen::ColMajor> mattype;

  typedef typename channel_type<ImgView>::type cs_t;

  auto h = src.height();
  auto w = src.width();
  auto nc = num_channels<ImgView>::value;
  vector<mattype> dfts;
  dfts.reserve(nc);
  for (size_t k = 0; k < nc; k++) dfts.push_back(mattype(h, w));

  progressbar bar((h + w) * nc);

  // first do fourier traffo of rows
  for (int y = 0; y < h; y++) {
    typename ImgView::x_iterator src_it = src.row_begin(y);
    for (size_t c = 0; c < nc; c++) {
      bar.update();
      auto src_channel_it =
          channel_iterator<cs_t, typename ImgView::x_iterator>(src_it, c);
      std::vector<double> buf;
      buf.resize(w);
      idct(src_channel_it, buf.data(), w);
      for (int x = 0; x < w; x++) dfts[c](y, x) = buf[x];
    }
  }

  // now of cols
  for (int x = 0; x < w; x++) {
    for (size_t c = 0; c < nc; c++) {
      bar.update();
      std::vector<double> buf;
      buf.resize(h);
      idct(&(dfts[c](0, x)), buf.data(), h);

      for (int y = 0; y < h; y++) dfts[c](y, x) = buf[y];
    }
  }

  return dfts;
}

std::tuple<double, double> mag_bounds(MatrixXcd& mat) {
  double max = 0;
  double min = std::numeric_limits<double>::max();
  for (Eigen::Index row = 0; row < mat.rows(); row++)
    for (Eigen::Index col = 0; col < mat.cols(); col++) {
      double val = std::abs(mat(row, col));
      if (val > max) max = val;
      if (val < min) min = val;
    }
  return {min, max};
}

std::tuple<double, double> bounds(MatrixXd& mat) {
  double max = std::numeric_limits<double>::min();
  double min = std::numeric_limits<double>::max();
  for (Eigen::Index row = 0; row < mat.rows(); row++)
    for (Eigen::Index col = 0; col < mat.cols(); col++) {
      double val = mat(row, col);
      if (val > max) max = val;
      if (val < min) min = val;
    }
  return {min, max};
}

std::tuple<double, double> real_bounds(MatrixXcd& mat) {
  double max = std::numeric_limits<double>::min();
  double min = std::numeric_limits<double>::max();
  for (Eigen::Index row = 0; row < mat.rows(); row++)
    for (Eigen::Index col = 0; col < mat.cols(); col++) {
      double val = mat(row, col).real();
      if (val > max) max = val;
      if (val < min) min = val;
    }
  return {min, max};
}

template <typename SrcView, typename DstView>
void dft(const SrcView& src, DstView& dst_mag, DstView& dst_phase,
         bool shifted = true, bool log = true) {
  assert(src.dimensions() == dst_mag.dimensions());
  assert(src.dimensions() == dst_phase.dimensions());

  typedef typename channel_type<DstView>::type cs_t;
  cs_t max_val = std::numeric_limits<cs_t>::max();

  auto mats = dft(src);

  std::vector<std::tuple<double, double>> limits;
  limits.reserve(mats.size());
  for (size_t k = 0; k < mats.size(); k++)
    limits.push_back(mag_bounds(mats[k]));

  auto w = dst_mag.width();
  auto h = dst_mag.height();

  for (int y = 0; y < h; y++) {
    typename DstView::x_iterator dst_mag_it = dst_mag.row_begin(y);
    typename DstView::x_iterator dst_phase_it = dst_phase.row_begin(y);
    for (int x = 0; x < w; x++)
      for (size_t c = 0; c < mats.size(); c++) {
        auto [min, max] = limits[c];
        Eigen::Index xcord, ycord;
        if (shifted) {
          ycord = (y + h / 2) % h;
          xcord = (x + w / 2) % w;
        } else {
          xcord = x;
          ycord = y;
        }
        if (log) {
          dst_mag_it[x][c] =
              (std::log(std::abs(mats[c](ycord, xcord))) - std::log(min)) /
              (std::log(max) - std::log(min)) * max_val;
        } else {
          dst_mag_it[x][c] = ((std::abs(mats[c](ycord, xcord))) - (min)) /
                             ((max) - (min)) * max_val;
        }
        dst_phase_it[x][c] =
            std::arg(mats[c](ycord, xcord)) / 2. / pi * max_val;
      }
  }
}

template <typename SrcView, typename DstView>
void dct(const SrcView& src, DstView& dst) {
  assert(src.dimensions() == dst.dimensions());

  // typedef typename channel_type<DstView>::type cs_t;
  // cs_t max_val = std::numeric_limits<cs_t>::max();

  auto mats = dct(src);

  std::vector<std::tuple<double, double>> limits;
  limits.reserve(mats.size());
  for (size_t k = 0; k < mats.size(); k++) limits.push_back(bounds(mats[k]));

  auto w = dst.width();
  auto h = dst.height();

  for (int y = 0; y < h; y++) {
    typename DstView::x_iterator dst_it = dst.row_begin(y);
    for (int x = 0; x < w; x++)
      for (size_t c = 0; c < mats.size(); c++) {
        // auto [min, max] = limits[c];
        dst_it[x][c] = mats[c](y, x);
      }
  }
}

template <typename SrcView, typename DstView>
void idct(const SrcView& src, DstView& dst) {
  assert(src.dimensions() == dst.dimensions());

  typedef typename channel_type<DstView>::type cs_t;
  cs_t max_val = std::numeric_limits<cs_t>::max();

  auto mats = idct(src);

  std::vector<std::tuple<double, double>> limits;
  limits.reserve(mats.size());
  for (size_t k = 0; k < mats.size(); k++) limits.push_back(bounds(mats[k]));

  auto w = dst.width();
  auto h = dst.height();

  for (int y = 0; y < h; y++) {
    typename DstView::x_iterator dst_it = dst.row_begin(y);
    for (int x = 0; x < w; x++)
      for (size_t c = 0; c < mats.size(); c++) {
        auto [min, max] = limits[c];
        dst_it[x][c] = (mats[c](y, x) - min) / (max - min) * max_val;
        ;
      }
  }
}

size_t closest_smaller_power2(size_t number) {
  size_t log2n = 0;
  while ((number >> ++log2n) > 0) {
  };

  return 1 << (log2n - 1);
}

template <typename DstView>
void to_image(vector<MatrixXcd>& src, DstView& dst) {
  assert(src[0].cols() == dst.width());
  assert(src[0].rows() == dst.height());

  typedef typename channel_type<DstView>::type cs_t;
  cs_t max_val = std::numeric_limits<cs_t>::max();

  std::vector<std::tuple<double, double>> limits;
  limits.reserve(src.size());
  for (size_t k = 0; k < src.size(); k++) limits.push_back(mag_bounds(src[k]));

  auto w = src[0].cols();
  auto h = src[0].rows();

  for (int y = 0; y < h; y++) {
    typename DstView::x_iterator dst_it = dst.row_begin(y);
    for (int x = 0; x < w; x++)
      for (size_t c = 0; c < src.size(); c++) {
        auto [min, max] = limits[c];

        dst_it[x][c] = std::abs(src[c](y, x)) / max * max_val;
      }
  }
}

template <typename DstView>
void to_image_mag(vector<MatrixXcd>& src, DstView& dst) {
  assert(src[0].cols() == dst.width());
  assert(src[0].rows() == dst.height());

  typedef typename channel_type<DstView>::type cs_t;
  cs_t max_val = std::numeric_limits<cs_t>::max();

  std::vector<std::tuple<double, double>> limits;
  limits.reserve(src.size());
  for (size_t k = 0; k < src.size(); k++) limits.push_back(mag_bounds(src[k]));

  auto w = src[0].cols();
  auto h = src[0].rows();

  for (int y = 0; y < h; y++) {
    typename DstView::x_iterator dst_it = dst.row_begin(y);
    for (int x = 0; x < w; x++)
      for (size_t c = 0; c < src.size(); c++) {
        auto [min, max] = limits[c];

        auto ycord = (y + h / 2) % h;
        auto xcord = (x + w / 2) % w;
        dst_it[x][c] = (std::log(std::abs(src[c](ycord, xcord)) + 1e-6) -
                        std::log(min + 1e-6)) /
                       (std::log(max) - std::log(min + 1e-6)) * max_val;
      }
  }
}

void apply_filter(const char* from, const char* to, const char* fs,
                  std::function<void(MatrixXcd&)> filter, bool gray = false) {
  rgb8_image_t img;
  read_image(from, img, png_tag());

  // typedef typename channel_type<rgb8_image_t>::type cs_t;
  // cs_t max_val = std::numeric_limits<cs_t>::max();

  std::vector<MatrixXcd> mats;
  if (gray) {
    mats = dft(color_converted_view<gray8_pixel_t>(view(img)));
  } else
    mats = dft(view(img));

  auto w = img.width();
  auto h = img.height();

  // apply filter
  for (size_t c = 0; c < mats.size(); c++) filter(mats[c]);

  if (gray) {
    gray8_image_t img_control(img.dimensions());
    to_image_mag(mats, view(img_control));
    write_view(fs, view(img_control), png_tag());
  } else {
    rgb8_image_t img_control(img.dimensions());
    to_image_mag(mats, view(img_control));
    write_view(fs, view(img_control), png_tag());
  }

  // transform back
  // first do fourier traffo of rows
  progressbar bar((h + w) * mats.size());
  std::vector<complex<double>> buf;
  buf.resize(w);
  for (int y = 0; y < h; y++) {
    for (size_t c = 0; c < mats.size(); c++) {
      bar.update();
      ifft(mats[c].row(y), buf.data(), w);
      for (int x = 0; x < w; x++) mats[c](y, x) = buf[x];
    }
  }
  // now of columns
  buf.resize(h);
  for (int x = 0; x < w; x++) {
    for (size_t c = 0; c < mats.size(); c++) {
      bar.update();
      ifft(mats[c].col(x), buf.data(), h);
      for (int y = 0; y < h; y++) mats[c](y, x) = buf[y];
    }
  }

  if (gray) {
    gray8_image_t sharpened(img.dimensions());
    to_image(mats, view(sharpened));

    write_view(to, view(sharpened), png_tag());
  } else {
    rgb8_image_t sharpened(img.dimensions());
    to_image(mats, view(sharpened));

    write_view(to, view(sharpened), png_tag());
  }
}

void sharpen(const char* from, const char* to, const char* fs, double radius,
             bool gray = false) {
  apply_filter(
      from, to, fs,
      [&](MatrixXcd& mat) {
        double limit = radius * std::min(mat.cols(), mat.rows()) / 2.;
        limit *= limit;
        for (int x = 0; x < mat.cols(); x++)
          for (int y = 0; y < mat.rows(); y++) {
            double pos_x =
                ((x + mat.cols() / 2) % mat.cols()) - (mat.cols() - 1) / 2.;
            double pos_y =
                ((y + mat.rows() / 2) % mat.rows()) - (mat.rows() - 1) / 2.;
            if (pos_x * pos_x + pos_y * pos_y < limit) {
              mat(y, x) = 0;
            }
          }
      },
      gray);
}

void sharpen_smooth(const char* from, const char* to, const char* fs,
                    double exp_fac, bool gray = false) {
  apply_filter(
      from, to, fs,
      [&](MatrixXcd& mat) {
        for (int x = 0; x < mat.cols(); x++)
          for (int y = 0; y < mat.rows(); y++) {
            double pos_x =
                ((x + mat.cols() / 2) % mat.cols()) - (mat.cols() - 1) / 2.;
            double pos_y =
                ((y + mat.rows() / 2) % mat.rows()) - (mat.rows() - 1) / 2.;

            double r2 = pos_x * pos_x + pos_y * pos_y;
            mat(y, x) *= (1 - std::exp(-r2 * exp_fac));
          }
      },
      gray);
}

void blur_smooth(const char* from, const char* to, const char* fs,
                 double exp_fac, bool gray = false) {
  apply_filter(
      from, to, fs,
      [&](MatrixXcd& mat) {
        for (int x = 0; x < mat.cols(); x++)
          for (int y = 0; y < mat.rows(); y++) {
            double pos_x =
                ((x + mat.cols() / 2) % mat.cols()) - (mat.cols() - 1) / 2.;
            double pos_y =
                ((y + mat.rows() / 2) % mat.rows()) - (mat.rows() - 1) / 2.;

            double r2 = pos_x * pos_x + pos_y * pos_y;
            mat(y, x) *= std::exp(-r2 * exp_fac);
          }
      },
      gray);
}

void blur(const char* from, const char* to, const char* fs, double radius,
          bool gray = false) {
  apply_filter(
      from, to, fs,
      [&](MatrixXcd& mat) {
        double limit = radius * std::min(mat.cols(), mat.rows()) / 2.;
        for (int x = 0; x < mat.cols(); x++)
          for (int y = 0; y < mat.rows(); y++) {
            double pos_x =
                ((x + mat.cols() / 2) % mat.cols()) - (mat.cols() - 1) / 2.;
            double pos_y =
                ((y + mat.rows() / 2) % mat.rows()) - (mat.rows() - 1) / 2.;
            if (pos_x * pos_x + pos_y * pos_y > limit * limit) {
              mat(y, x) = 0;
            }
          }
      },
      gray);
}

void rect_filter(const char* from, const char* to, const char* fs, double width,
                 double height, bool gray = false) {
  apply_filter(
      from, to, fs,
      [&](MatrixXcd& mat) {
        for (int x = 0; x < mat.cols(); x++)
          for (int y = 0; y < mat.rows(); y++) {
            double pos_x =
                ((x + mat.cols() / 2) % mat.cols()) - (mat.cols() - 1) / 2.;
            double pos_y =
                ((y + mat.rows() / 2) % mat.rows()) - (mat.rows() - 1) / 2.;
            if (pos_x > mat.cols() * width / 2 ||
                pos_x < -mat.cols() * width / 2 ||
                pos_y > mat.rows() * height / 2 ||
                pos_y < -mat.rows() * height / 2) {
              mat(y, x) = 0;
            }
          }
      },
      gray);
}

void anti_rect_filter(const char* from, const char* to, const char* fs,
                      double width, double height, bool gray = false) {
  apply_filter(
      from, to, fs,
      [&](MatrixXcd& mat) {
        for (int x = 0; x < mat.cols(); x++)
          for (int y = 0; y < mat.rows(); y++) {
            double pos_x =
                ((x + mat.cols() / 2) % mat.cols()) - (mat.cols() - 1) / 2.;
            double pos_y =
                ((y + mat.rows() / 2) % mat.rows()) - (mat.rows() - 1) / 2.;
            if (!(pos_x > mat.cols() * width / 2 ||
                  pos_x < -mat.cols() * width / 2 ||
                  pos_y > mat.rows() * height / 2 ||
                  pos_y < -mat.rows() * height / 2)) {
              mat(y, x) = 0;
            }
          }
      },
      gray);
}

typedef uint_least16_t pos_t;
typedef uint_least8_t channel_t;
typedef std::tuple<pos_t, pos_t, channel_t> index_t;
void compress_image(const char* from, const char* to,
                    double compression_level) {
  rgb8_image_t img;
  read_image(from, img, png_tag());
  std::cout << "Image read! Dimensions x: " << img.dimensions().x
            << " y: " << img.dimensions().y << std::endl;

  // typedef typename channel_type<rgb8_image_t>::type cs_t;
  // cs_t max_val = std::numeric_limits<cs_t>::max();

  auto mats = dct(view(img));
  // x, y, c
  std::vector<index_t> indices;

  indices.reserve(mats[0].cols() * mats[0].rows() * mats.size());
  for (Eigen::Index x = 0; x < mats[0].cols(); x++)
    for (Eigen::Index y = 0; y < mats[0].rows(); y++)
      for (size_t c = 0; c < mats.size(); c++) indices.push_back({x, y, c});

  std::sort(indices.begin(), indices.end(),
            [&](index_t& a, index_t& b) -> bool {
              auto [xa, ya, ca] = a;
              auto [xb, yb, cb] = b;
              return std::abs(mats[ca](ya, xa)) > std::abs(mats[cb](yb, xb));
            });

  std::fstream file(to, file.binary | file.trunc | file.out);
  if (!file.is_open()) {
    throw std::runtime_error("failed to open file");
  } else {
    boost::iostreams::filtering_ostream os;
    os.push(boost::iostreams::zlib_compressor());
    os.push(file);
    // write dimensions
    uint_least16_t width = mats[0].cols();
    uint_least16_t height = mats[0].rows();
    os.write(reinterpret_cast<char*>(&width), sizeof width);  // binary output
    os.write(reinterpret_cast<char*>(&height),
             sizeof height);  // binary output

    size_t lastk = indices.size() * compression_level;
    progressbar b2(lastk);

    for (size_t k = 0; k < lastk; k++) {
      b2.update();
      auto [x, y, c] = indices[k];
      float val = mats[c](y, x);

      os.write(reinterpret_cast<char*>(&x), sizeof x);      // binary output
      os.write(reinterpret_cast<char*>(&y), sizeof y);      // binary output
      os.write(reinterpret_cast<char*>(&c), sizeof c);      // binary output
      os.write(reinterpret_cast<char*>(&val), sizeof val);  // binary output
    }
  }
}

void decompress_image(const char* from, const char* to) {
  std::fstream file(from, file.binary | file.in);
  if (!file.is_open()) {
    throw std::runtime_error("failed to open file");
  }
  boost::iostreams::filtering_istream is;
  is.push(boost::iostreams::zlib_decompressor());
  is.push(file);
  // read dimensions
  uint_least16_t width, height;
  is.read(reinterpret_cast<char*>(&width), sizeof width);
  is.read(reinterpret_cast<char*>(&height), sizeof height);
  std::vector<MatrixXd> mats;
  for (int c = 0; c < 3; c++) mats.push_back(MatrixXd::Zero(height, width));

  while (!is.eof()) {
    pos_t x, y;
    channel_t c;
    float val;

    is.read(reinterpret_cast<char*>(&x), sizeof x);      // binary input
    is.read(reinterpret_cast<char*>(&y), sizeof y);      // binary input
    is.read(reinterpret_cast<char*>(&c), sizeof c);      // binary input
    is.read(reinterpret_cast<char*>(&val), sizeof val);  // binary input

    mats[c](y, x) = val;
  }

  // first do fourier traffo of rows
  std::vector<double> buf;
  buf.resize(width);
  for (int y = 0; y < height; y++) {
    for (size_t c = 0; c < 3; c++) {
      idct(mats[c].row(y), buf.data(), width);
      for (int x = 0; x < width; x++) mats[c](y, x) = buf[x];
    }
  }

  rgb8_image_t img(width, height);
  // typedef typename channel_type<rgb8_image_t>::type cs_t;
  // cs_t max_val = std::numeric_limits<cs_t>::max();
  auto img_view = view(img);

  // now of cols
  buf.resize(height);
  for (int x = 0; x < width; x++) {
    for (size_t c = 0; c < 3; c++) {
      idct(mats[c].col(x), buf.data(), height);
      for (int y = 0; y < height; y++) img_view(x, y)[c] = buf[y];
    }
  }

  write_view(to, img_view, png_tag());
}

void dft_image(const char* from, const char* to_mag, const char* to_phase,
               bool crop = true, bool log = true, bool shifted = true) {
  rgb8_image_t img;
  read_image(from, img, png_tag());
  std::cout << "Image read! Dimensions x: " << img.dimensions().x
            << " y: " << img.dimensions().y << std::endl;

  rgb8_image_t img_ft_mag, img_ft_phase;
  if (crop) {
    auto subw = closest_smaller_power2(img.width());
    auto subh = closest_smaller_power2(img.height());
    auto sub_img = subimage_view(view(img), 0, 0, subw, subh);
    img_ft_mag = rgb8_image_t(sub_img.dimensions());
    img_ft_phase = rgb8_image_t(sub_img.dimensions());
    dft(sub_img, view(img_ft_mag), view(img_ft_phase), shifted, log);
  } else {
    img_ft_mag = rgb8_image_t(img.dimensions());
    img_ft_phase = rgb8_image_t(img.dimensions());
    dft(view(img), view(img_ft_mag), view(img_ft_phase), shifted, log);
  }

  write_view(to_mag, view(img_ft_mag), png_tag());
  write_view(to_phase, view(img_ft_phase), png_tag());
}

void dct_image(const char* from, const char* to, bool crop = false) {
  rgb8_image_t img;
  read_image(from, img, png_tag());
  std::cout << "Image read! Dimensions x: " << img.dimensions().x
            << " y: " << img.dimensions().y << std::endl;

  rgb8_image_t img_dct;
  if (crop) {
    auto subw = closest_smaller_power2(img.width());
    auto subh = closest_smaller_power2(img.height());
    auto sub_img = subimage_view(view(img), 0, 0, subw, subh);
    img_dct = rgb8_image_t(sub_img.dimensions());
    dct(sub_img, view(img_dct));
  } else {
    img_dct = rgb8_image_t(img.dimensions());
    dct(view(img), view(img_dct));
  }

  write_view(to, view(img_dct), png_tag());
}

void idct_image(const char* from, const char* to, bool crop = false) {
  rgb8_image_t img;
  read_image(from, img, png_tag());
  std::cout << "Image read! Dimensions x: " << img.dimensions().x
            << " y: " << img.dimensions().y << std::endl;

  rgb8_image_t img_dct;
  if (crop) {
    auto subw = closest_smaller_power2(img.width());
    auto subh = closest_smaller_power2(img.height());
    auto sub_img = subimage_view(view(img), 0, 0, subw, subh);
    img_dct = rgb8_image_t(sub_img.dimensions());
    idct(sub_img, view(img_dct));
  } else {
    img_dct = rgb8_image_t(img.dimensions());
    idct(view(img), view(img_dct));
  }

  write_view(to, view(img_dct), png_tag());
}

void image_test() {
  using namespace boost::gil;

  std::string filename("images/webb.png");
  rgb8_image_t img;
  read_image(filename, img, png_tag());
  std::cout << "Image read! Dimensions x: " << img.dimensions().x
            << " y: " << img.dimensions().y << std::endl;

  gray8_image_t img_ft_mag(img.dimensions());
  gray8_image_t img_ft_phase(img.dimensions());
  std::cout << "Now performing a gray naive fourier transform" << std::endl;

  // dft(color_converted_view<gray8_pixel_t>(view(img)), view(img_ft_mag),
  // view(img_ft_phase));
  std::cout << "Saving image" << std::endl;
  write_view("build/output/test_mag.png", view(img_ft_mag), png_tag());
  write_view("build/output/test_phase.png", view(img_ft_phase), png_tag());

  std::cout
      << "Now performing a fourier transform of closest power of two subimage"
      << std::endl;
  auto subw = closest_smaller_power2(img.width());
  auto subh = closest_smaller_power2(img.height());
  auto sub_img = subimage_view(color_converted_view<gray8_pixel_t>(view(img)),
                               0, 0, subw, subh);
  gray8_image_t img_sub_ft_mag(sub_img.dimensions());
  gray8_image_t img_sub_ft_phase(sub_img.dimensions());

  dft(sub_img, view(img_sub_ft_mag), view(img_sub_ft_phase));
  std::cout << "Saving image" << std::endl;
  write_view("build/output/sub_test_mag.png", view(img_sub_ft_mag), png_tag());
  write_view("build/output/sub_test_phase.png", view(img_sub_ft_phase),
             png_tag());

  std::cout << "Now performing a colored fourier transform of closest power of "
               "two subimage"
            << std::endl;
  auto sub_img_c = subimage_view(view(img), 0, 0, subw, subh);
  rgb8_image_t img_sub_c_ft_mag(sub_img_c.dimensions());
  rgb8_image_t img_sub_c_ft_phase(sub_img_c.dimensions());

  dft(sub_img_c, view(img_sub_c_ft_mag), view(img_sub_c_ft_phase));
  std::cout << "Saving image" << std::endl;
  write_view("build/output/sub_c_test_mag.png", view(img_sub_c_ft_mag),
             png_tag());
  write_view("build/output/sub_c_test_phase.png", view(img_sub_c_ft_phase),
             png_tag());

  std::cout << "Now performing a colored cosine transform of closest power of "
               "two subimage"
            << std::endl;
  rgb8_image_t img_sub_c_dct(sub_img_c.dimensions());

  dct(sub_img_c, view(img_sub_c_dct));
  std::cout << "Saving image" << std::endl;
  write_view("build/output/sub_c_test_dct.png", view(img_sub_c_dct), png_tag());
}

void sharpen_test() {
  sharpen("images/dune.png", "build/output/dune_sharp.png",
          "build/output/dune_sharp_mask.png", .1);
  blur("images/dune.png", "build/output/dune_blur.png",
       "build/output/dune_blur_mask.png", .5);
}

void compress_test() {
  compress_image("images/dune.png", "build/output/dune_cmp.ldw", .01);
  decompress_image("build/output/dune_cmp.ldw", "build/output/reconst.png");
}


#include <Eigen/Dense>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <sm/assert_macros.hpp>
#include <sm/kinematics/rotations.hpp>
#include <sm/kinematics/three_point_methods.hpp>

namespace sm {
namespace kinematics {

// Original code from the ROS vslam package pe3d.cpp
// uses the SVD procedure for aligning point clouds
//   SEE: Arun, Huang, Blostein: Least-Squares Fitting of Two 3D Point Sets
Eigen::Matrix4d threePointSvd(Eigen::MatrixXd const& p0, Eigen::MatrixXd const& p1) {
    using namespace Eigen;

    SM_ASSERT_EQ_DBG(std::runtime_error, p0.rows(), 3, "p0 must be a 3xK matrix");
    SM_ASSERT_EQ_DBG(std::runtime_error, p1.rows(), 3, "p1 must be a 3xK matrix");
    SM_ASSERT_EQ_DBG(std::runtime_error, p0.cols(), p1.cols(), "p0 and p1 must have the same number of columns");

    Vector3d c0 = p0.rowwise().mean();
    Vector3d c1 = p1.rowwise().mean();

    Matrix3d H(Matrix3d::Zero());
    // subtract out
    // p0a -= c0;
    // p0b -= c0;
    // p0c -= c0;
    // p1a -= c1;
    // p1b -= c1;
    // p1c -= c1;

    // Matrix3d H = p1a*p0a.transpose() + p1b*p0b.transpose() +
    // 	p1c*p0c.transpose();
    for (int i = 0; i < p0.cols(); ++i) {
        H += (p0.col(i) - c0) * (p1.col(i) - c1).transpose();
    }

    // do the SVD thang
    JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);
    Matrix3d V = svd.matrixV();
    Matrix3d R = V * svd.matrixU().transpose();
    double det = R.determinant();

    if (det < 0.0) {
        V.col(2) = V.col(2) * -1.0;
        R = V * svd.matrixU().transpose();
    }
    Vector3d tr = c0 - R.transpose() * c1;  // translation

    // transformation matrix, 3x4
    Matrix4d tfm(Matrix4d::Identity());
    //        tfm.block<3,3>(0,0) = R.transpose();
    //        tfm.col(3) = -R.transpose()*tr;
    tfm.topLeftCorner<3, 3>() = R.transpose();
    tfm.topRightCorner<3, 1>() = tr;

    return tfm;
}

Eigen::Matrix3d qMethod(Eigen::MatrixXd const& p0, Eigen::MatrixXd const& p1, const Eigen::VectorXd& w) {
    SM_ASSERT_EQ_DBG(std::runtime_error, p0.rows(), 3, "p0 must be a 3xK matrix");
    SM_ASSERT_EQ_DBG(std::runtime_error, p1.rows(), 3, "p1 must be a 3xK matrix");
    SM_ASSERT_EQ_DBG(std::runtime_error, p0.cols(), p1.cols(), "p0 and p1 must have the same number of columns");
    SM_ASSERT_EQ_DBG(std::runtime_error, w.size(), p0.cols(), "w must have the same number of columns as p0");

    Eigen::MatrixXd W = p0;
    Eigen::MatrixXd V = p1;

    for (int i = 0; i < p0.cols(); i++) {
        double wi = sqrt(w[i]);
        SM_ASSERT_NEAR_DBG(std::runtime_error, p0.col(i).norm(), 1.0, 1e-4,
                           "Column " << i << " of p0 was not a unit vector");
        SM_ASSERT_NEAR_DBG(std::runtime_error, p1.col(i).norm(), 1.0, 1e-4,
                           "Column " << i << " of p1 was not a unit vector");

        W.col(i) = wi * W.col(i);
        V.col(i) = wi * V.col(i);
    }

    Eigen::MatrixXd B = W * V.transpose();
    Eigen::MatrixXd Q = B + B.transpose();

    Eigen::Vector3d Z(B(1, 2) - B(2, 1), B(2, 0) - B(0, 2), B(0, 1) - B(1, 0));
    double sigma = B(0, 0) + B(1, 1) + B(2, 2);

    Eigen::Matrix4d K;
    K.topLeftCorner<3, 3>() = Q - sigma * Eigen::Matrix3d::Identity();
    K.topRightCorner<3, 1>() = Z;
    K.bottomLeftCorner<1, 3>() = Z.transpose();
    K(3, 3) = sigma;

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> eigensolver(K);

    Eigen::Vector4d eigenvalues = eigensolver.eigenvalues();
    // Find the maximum eigenvalue

    double maxVal = eigenvalues(0);
    int maxValIdx = 0;
    for (int i = 1; i < 4; i++) {
        if (eigenvalues(i) > maxVal) {
            maxVal = eigenvalues(i);
            maxValIdx = i;
        }
    }

    // The corresponding eigenvector is the quaternion q_01
    Eigen::Vector4d q_01 = eigensolver.eigenvectors().col(maxValIdx);
    q_01 /= q_01.norm();

    Eigen::Vector3d qv = q_01.head<3>();
    double qs = q_01(3);

    Eigen::Matrix3d C_01 = (qs * qs - qv.dot(qv)) * Eigen::Matrix3d::Identity() + 2.0 * qv * qv.transpose() -
                           2.0 * qs * sm::kinematics::crossMx(qv);

    return C_01;
}

Eigen::Matrix3d qMethod(Eigen::MatrixXd const& p0, Eigen::MatrixXd const& p1) {
    return qMethod(p0, p1, Eigen::VectorXd::Ones(p1.cols()));
}

}  // namespace kinematics
}  // namespace sm


#pragma once

#include <cmath>
#include <cstdint>

#include <Eigen/Core>

#include "surface_normal/base.hpp"
#include "surface_normal/cuda_compatibility.hpp"
#include "svd3_cuda.hpp"

namespace surface_normal {

__host__ __device__ __forceinline__ uint8_t f2b(float x) {
  return static_cast<uint8_t>(127.5 * (1 - x));
}

template <typename T>
__host__ __device__ __forceinline__ void
depth_to_normals_rgb_inner(const ImageView<const T> &depth, ImageView<uint8_t, 3> &normals,
                           const CameraIntrinsics &intrinsics, int radius, float max_rel_depth_diff,
                           int center_row, int center_col) {
  float center_depth = depth.at(center_row, center_col);
  if (center_depth == 0) {
    return;
  }

  float f_inv = 1.f / intrinsics.f;
  float cx    = intrinsics.cx;
  float cy    = intrinsics.cy;
  Eigen::Vector3f mid{(center_col - cx) * center_depth * f_inv,
                      (center_row - cy) * center_depth * f_inv, center_depth};
  int n                          = 0;
  Eigen::Vector3f centroid       = Eigen::Vector3f::Zero();
  Eigen::Matrix3f outer_prod_sum = Eigen::Matrix3f::Zero();
  for (int i = -radius; i <= radius; i++) {
    for (int j = -radius; j <= radius; j++) {
      int x = center_col + j;
      int y = center_row + i;

      if (x < 0 || x >= depth.width || y < 0 || y >= depth.height) {
        continue;
      }

      float z = depth.at(y, x);
      if (z == 0 || std::abs(z - center_depth) > max_rel_depth_diff * center_depth) {
        continue;
      }

      Eigen::Vector3f p{(x - cx) * z * f_inv, (y - cy) * z * f_inv, z};
      p -= mid; // subtract midpoint for improved numeric stability in outer product
      centroid += p;
      // '* 1' to suppress
      // warning: calling a __host__ function from a __host__ __device__ function is not allowed
      outer_prod_sum += p * p.transpose() * 1;
      n++;
    }
  }

  if (n < 3)
    return;

  centroid /= n;
  Eigen::Matrix3f cov = (outer_prod_sum - n * centroid * centroid.transpose()) / (n - 1);

  Eigen::Matrix3f U, V;
  Eigen::Vector3f S;
  svd(cov(0, 0), cov(0, 1), cov(0, 2), cov(1, 0), cov(1, 1), cov(1, 2), cov(2, 0), cov(2, 1),
      cov(2, 2),                                                                       // cov
      U(0, 0), U(0, 1), U(0, 2), U(1, 0), U(1, 1), U(1, 2), U(2, 0), U(2, 1), U(2, 2), // output U
      S(0), S(1), S(2),                                                                // output S
      V(0, 0), V(0, 1), V(0, 2), V(1, 0), V(1, 1), V(1, 2), V(2, 0), V(2, 1), V(2, 2)  // output V
  );
  Eigen::Vector3f normal = V.col(2).normalized();

  if (mid.dot(normal) < 0) {
    normal *= -1;
  }

  normals.at(center_row, center_col, 0) = f2b(normal(0));
  normals.at(center_row, center_col, 1) = f2b(normal(2));
  normals.at(center_row, center_col, 2) = f2b(normal(1));
}
} // namespace surface_normal