#ifndef BOOST_GEOMETRY_PROJECTIONS_MOD_STER_HPP
#define BOOST_GEOMETRY_PROJECTIONS_MOD_STER_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 4.9.1

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#include <boost/geometry/util/math.hpp>
#include <boost/math/special_functions/hypot.hpp>

#include <boost/geometry/extensions/gis/projections/impl/base_static.hpp>
#include <boost/geometry/extensions/gis/projections/impl/base_dynamic.hpp>
#include <boost/geometry/extensions/gis/projections/impl/projects.hpp>
#include <boost/geometry/extensions/gis/projections/impl/factory_entry.hpp>
#include <boost/geometry/extensions/gis/projections/impl/aasincos.hpp>
#include <boost/geometry/extensions/gis/projections/impl/pj_zpoly1.hpp>

namespace boost { namespace geometry { namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace mod_ster
    {

            static const double EPSLN = 1e-10;

            struct par_mod_ster
            {
                COMPLEX    *zcoeff;
                double    cchio, schio;
                int        n;
            };

            /* based upon Snyder and Linck, USGS-NMD */

            // template class, using CRTP to implement forward/inverse
            template <typename Geographic, typename Cartesian, typename Parameters>
            struct base_mod_ster_ellipsoid : public base_t_fi<base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>
            {

                 typedef double geographic_type;
                 typedef double cartesian_type;

                par_mod_ster m_proj_parm;

                inline base_mod_ster_ellipsoid(const Parameters& par)
                    : base_t_fi<base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>(*this, par) {}

                // FORWARD(e_forward)  ellipsoid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    double sinlon, coslon, esphi, chi, schi, cchi, s;
                    COMPLEX p;

                    sinlon = sin(lp_lon);
                    coslon = cos(lp_lon);
                    esphi = this->m_par.e * sin(lp_lat);
                    chi = 2. * atan(tan((geometry::math::half_pi<double>() + lp_lat) * .5) *
                        pow((1. - esphi) / (1. + esphi), this->m_par.e * .5)) - geometry::math::half_pi<double>();
                    schi = sin(chi);
                    cchi = cos(chi);
                    s = 2. / (1. + this->m_proj_parm.schio * schi + this->m_proj_parm.cchio * cchi * coslon);
                    p.r = s * cchi * sinlon;
                    p.i = s * (this->m_proj_parm.cchio * schi - this->m_proj_parm.schio * cchi * coslon);
                    p = pj_zpoly1(p, this->m_proj_parm.zcoeff, this->m_proj_parm.n);
                    xy_x = p.r;
                    xy_y = p.i;
                }

                // INVERSE(e_inverse)  ellipsoid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    int nn;
                    COMPLEX p, fxy, fpxy, dp;
                    double den, rh = 0, z, sinz = 0, cosz = 0, chi, phi = 0, dphi, esphi;

                    p.r = xy_x;
                    p.i = xy_y;
                    for (nn = 20; nn ;--nn) {
                        fxy = pj_zpolyd1(p, this->m_proj_parm.zcoeff, this->m_proj_parm.n, &fpxy);
                        fxy.r -= xy_x;
                        fxy.i -= xy_y;
                        den = fpxy.r * fpxy.r + fpxy.i * fpxy.i;
                        dp.r = -(fxy.r * fpxy.r + fxy.i * fpxy.i) / den;
                        dp.i = -(fxy.i * fpxy.r - fxy.r * fpxy.i) / den;
                        p.r += dp.r;
                        p.i += dp.i;
                        if ((fabs(dp.r) + fabs(dp.i)) <= EPSLN)
                            break;
                    }
                    if (nn) {
                        rh = boost::math::hypot(p.r, p.i);
                        z = 2. * atan(.5 * rh);
                        sinz = sin(z);
                        cosz = cos(z);
                        lp_lon = this->m_par.lam0;
                        if (fabs(rh) <= EPSLN) {
                            lp_lat = this->m_par.phi0;
                            return;
                        }
                        chi = aasin(cosz * this->m_proj_parm.schio + p.i * sinz * this->m_proj_parm.cchio / rh);
                        phi = chi;
                        for (nn = 20; nn ;--nn) {
                            esphi = this->m_par.e * sin(phi);
                            dphi = 2. * atan(tan((geometry::math::half_pi<double>() + chi) * .5) *
                                pow((1. + esphi) / (1. - esphi), this->m_par.e * .5)) - geometry::math::half_pi<double>() - phi;
                            phi += dphi;
                            if (fabs(dphi) <= EPSLN)
                                break;
                        }
                    }
                    if (nn) {
                        lp_lat = phi;
                        lp_lon = atan2(p.r * sinz, rh * this->m_proj_parm.cchio * cosz - p.i *
                            this->m_proj_parm.schio * sinz);
                    } else
                        lp_lon = lp_lat = HUGE_VAL;
                }

                static inline std::string get_name()
                {
                    return "mod_ster_ellipsoid";
                }

            };

            template <typename Parameters>
            void setup(Parameters& par, par_mod_ster& proj_parm)  /* general initialization */
            {
                double esphi, chio;

                if (par.es) {
                    esphi = par.e * sin(par.phi0);
                    chio = 2. * atan(tan((geometry::math::half_pi<double>() + par.phi0) * .5) *
                        pow((1. - esphi) / (1. + esphi), par.e * .5)) - geometry::math::half_pi<double>();
                } else
                    chio = par.phi0;
                proj_parm.schio = sin(chio);
                proj_parm.cchio = cos(chio);
            }

            // Miller Oblated Stereographic
            template <typename Parameters>
            void setup_mil_os(Parameters& par, par_mod_ster& proj_parm)
            {
                static COMPLEX /* Miller Oblated Stereographic */
            AB[] = {
                {0.924500,    0.},
                {0.,            0.},
                {0.019430,    0.}
            };

                proj_parm.n = 2;
                par.lam0 = geometry::math::d2r<double>() * 20.;
                par.phi0 = geometry::math::d2r<double>() * 18.;
                proj_parm.zcoeff = AB;
                par.es = 0.;
                setup(par, proj_parm);
            }

            // Lee Oblated Stereographic
            template <typename Parameters>
            void setup_lee_os(Parameters& par, par_mod_ster& proj_parm)
            {
                static COMPLEX /* Lee Oblated Stereographic */
            AB[] = {
                {0.721316,    0.},
                {0.,            0.},
                    {-0.0088162,     -0.00617325}
            };

                proj_parm.n = 2;
                par.lam0 = geometry::math::d2r<double>() * -165.;
                par.phi0 = geometry::math::d2r<double>() * -10.;
                proj_parm.zcoeff = AB;
                par.es = 0.;
                setup(par, proj_parm);
            }

            // Mod. Stererographics of 48 U.S.
            template <typename Parameters>
            void setup_gs48(Parameters& par, par_mod_ster& proj_parm)
            {
                static COMPLEX /* 48 United States */
            AB[] = {
                {0.98879,    0.},
                {0.,        0.},
                {-0.050909,    0.},
                {0.,        0.},
                    {0.075528,    0.}
            };

                proj_parm.n = 4;
                par.lam0 = geometry::math::d2r<double>() * -96.;
                par.phi0 = geometry::math::d2r<double>() * -39.;
                proj_parm.zcoeff = AB;
                par.es = 0.;
                par.a = 6370997.;
                setup(par, proj_parm);
            }

            // Mod. Stererographics of Alaska
            template <typename Parameters>
            void setup_alsk(Parameters& par, par_mod_ster& proj_parm)
            {
                static COMPLEX
            ABe[] = { /* Alaska ellipsoid */
                {.9945303,    0.},
                {.0052083,    -.0027404},
                {.0072721,    .0048181},
                {-.0151089,    -.1932526},
                {.0642675,    -.1381226},
                {.3582802,    -.2884586}},
            ABs[] = { /* Alaska sphere */
                {.9972523,    0.},
                {.0052513,    -.0041175},
                {.0074606,    .0048125},
                {-.0153783,    -.1968253},
                {.0636871,    -.1408027},
                    {.3660976,    -.2937382}
            };

                proj_parm.n = 5;
                par.lam0 = geometry::math::d2r<double>() * -152.;
                par.phi0 = geometry::math::d2r<double>() * 64.;
                if (par.es) { /* fixed ellipsoid/sphere */
                    proj_parm.zcoeff = ABe;
                    par.a = 6378206.4;
                    par.e = sqrt(par.es = 0.00676866);
                } else {
                    proj_parm.zcoeff = ABs;
                    par.a = 6370997.;
                }
                setup(par, proj_parm);
            }

            // Mod. Stererographics of 50 U.S.
            template <typename Parameters>
            void setup_gs50(Parameters& par, par_mod_ster& proj_parm)
            {
                static COMPLEX
            ABe[] = { /* GS50 ellipsoid */
                {.9827497,    0.},
                {.0210669,    .0053804},
                {-.1031415,    -.0571664},
                {-.0323337,    -.0322847},
                {.0502303,    .1211983},
                {.0251805,    .0895678},
                {-.0012315,    -.1416121},
                {.0072202,    -.1317091},
                {-.0194029,    .0759677},
                    {-.0210072,    .0834037}
            },
            ABs[] = { /* GS50 sphere */
                {.9842990,    0.},
                {.0211642,    .0037608},
                {-.1036018,    -.0575102},
                {-.0329095,    -.0320119},
                {.0499471,    .1223335},
                {.0260460,    .0899805},
                {.0007388,    -.1435792},
                {.0075848,    -.1334108},
                {-.0216473,    .0776645},
                    {-.0225161,    .0853673}
            };

                proj_parm.n = 9;
                par.lam0 = geometry::math::d2r<double>() * -120.;
                par.phi0 = geometry::math::d2r<double>() * 45.;
                if (par.es) { /* fixed ellipsoid/sphere */
                    proj_parm.zcoeff = ABe;
                    par.a = 6378206.4;
                    par.e = sqrt(par.es = 0.00676866);
                } else {
                    proj_parm.zcoeff = ABs;
                    par.a = 6370997.;
                }
                setup(par, proj_parm);
            }

        }} // namespace detail::mod_ster
    #endif // doxygen

    /*!
        \brief Miller Oblated Stereographic projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal (mod)
        \par Example
        \image html ex_mil_os.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct mil_os_ellipsoid : public detail::mod_ster::base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>
    {
        inline mil_os_ellipsoid(const Parameters& par) : detail::mod_ster::base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>(par)
        {
            detail::mod_ster::setup_mil_os(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Lee Oblated Stereographic projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal (mod)
        \par Example
        \image html ex_lee_os.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct lee_os_ellipsoid : public detail::mod_ster::base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>
    {
        inline lee_os_ellipsoid(const Parameters& par) : detail::mod_ster::base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>(par)
        {
            detail::mod_ster::setup_lee_os(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Mod. Stererographics of 48 U.S. projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal (mod)
        \par Example
        \image html ex_gs48.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct gs48_ellipsoid : public detail::mod_ster::base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>
    {
        inline gs48_ellipsoid(const Parameters& par) : detail::mod_ster::base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>(par)
        {
            detail::mod_ster::setup_gs48(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Mod. Stererographics of Alaska projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal (mod)
        \par Example
        \image html ex_alsk.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct alsk_ellipsoid : public detail::mod_ster::base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>
    {
        inline alsk_ellipsoid(const Parameters& par) : detail::mod_ster::base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>(par)
        {
            detail::mod_ster::setup_alsk(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Mod. Stererographics of 50 U.S. projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal (mod)
        \par Example
        \image html ex_gs50.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct gs50_ellipsoid : public detail::mod_ster::base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>
    {
        inline gs50_ellipsoid(const Parameters& par) : detail::mod_ster::base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>(par)
        {
            detail::mod_ster::setup_gs50(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Factory entry(s)
        template <typename Geographic, typename Cartesian, typename Parameters>
        class mil_os_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<mil_os_ellipsoid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        class lee_os_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<lee_os_ellipsoid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        class gs48_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<gs48_ellipsoid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        class alsk_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<alsk_ellipsoid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        class gs50_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<gs50_ellipsoid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        inline void mod_ster_init(detail::base_factory<Geographic, Cartesian, Parameters>& factory)
        {
            factory.add_to_factory("mil_os", new mil_os_entry<Geographic, Cartesian, Parameters>);
            factory.add_to_factory("lee_os", new lee_os_entry<Geographic, Cartesian, Parameters>);
            factory.add_to_factory("gs48", new gs48_entry<Geographic, Cartesian, Parameters>);
            factory.add_to_factory("alsk", new alsk_entry<Geographic, Cartesian, Parameters>);
            factory.add_to_factory("gs50", new gs50_entry<Geographic, Cartesian, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_MOD_STER_HPP


#ifndef NETKET_IMAGINARY_TIME_PROPAGATION_HPP
#define NETKET_IMAGINARY_TIME_PROPAGATION_HPP

#include <Eigen/Dense>

#include "Dynamics/TimeStepper/time_stepper.hpp"
#include "Hamiltonian/MatrixWrapper/matrix_wrapper.hpp"
#include "Observable/observable.hpp"
#include "Stats/stats.hpp"

#include "json_output_writer.hpp"

namespace netket {

class ImaginaryTimePropagation {
 public:
  using State = Eigen::VectorXcd;
  using Matrix = AbstractMatrixWrapper<Hamiltonian>;
  using Stepper = ode::AbstractTimeStepper<State>;

  using MatrixObs = AbstractMatrixWrapper<Observable>;
  using ObsEntry = std::pair<std::string, std::unique_ptr<MatrixObs>>;
  using ObservableVector = std::vector<ObsEntry>;

  static ImaginaryTimePropagation FromJson(
      const Hamiltonian& hamiltonian,
      const std::vector<Observable>& observables, const json& pars) {
    auto matrix = ConstructMatrixWrapper(pars, hamiltonian);
    auto stepper =
        ode::ConstructTimeStepper<State>(pars, matrix->GetDimension());
    auto range = ode::TimeRange::FromJson(pars);

    ObservableVector wrapped_observables;
    for (const auto& obs : observables) {
      wrapped_observables.emplace_back(obs.Name(),
                                       ConstructMatrixWrapper(pars, obs));
    }

    auto output = JsonOutputWriter::FromJson(pars);

    return ImaginaryTimePropagation(std::move(matrix), std::move(stepper),
                                    std::move(wrapped_observables),
                                    std::move(output), range);
  }

  ImaginaryTimePropagation(std::unique_ptr<Matrix> matrix,
                           std::unique_ptr<Stepper> stepper,
                           ObservableVector observables,
                           JsonOutputWriter output,
                           const ode::TimeRange& time_range)
      : matrix_(std::move(matrix)),
        stepper_(std::move(stepper)),
        observables_(std::move(observables)),
        output_(std::move(output)),
        range_(time_range) {
    ode_system_ = [this](const State& x, State& dxdt, double /*t*/) {
      dxdt.noalias() = -matrix_->Apply(x);
    };
  }

  void Run(State& state) {
    assert(state.size() == GetDimension());

    int step = 0;
    double t = range_.tmin;
    while (t < range_.tmax) {
      double next_dt =
          (t + range_.dt <= range_.tmax) ? range_.dt : range_.tmax - t;

      stepper_->Propagate(ode_system_, state, t, next_dt);

      // renormalize the state to prevent unbounded growth of the norm
      state.normalize();

      ComputeObservables(state);
      auto obs_data = json(obsmanager_);
      output_.WriteLog(step, obs_data, t);
      output_.WriteState(step, state);

      step++;
      t = range_.tmin + step * range_.dt;
    };
  }

  void ComputeObservables(const State& state) {
    const auto mean_variance = matrix_->MeanVariance(state);
    obsmanager_.Reset("Energy");
    obsmanager_.Push("Energy", mean_variance[0].real());
    obsmanager_.Reset("EnergyVariance");
    obsmanager_.Push("EnergyVariance", mean_variance[1].real());

    for (const auto& entry : observables_) {
      const auto& name = entry.first;
      const auto& obs = entry.second;
      obsmanager_.Reset(name);

      const auto value = obs->Mean(state).real();
      obsmanager_.Push(name, value);
    }
  }

  int GetDimension() const { return matrix_->GetDimension(); }

 private:
  std::unique_ptr<Matrix> matrix_;
  std::unique_ptr<Stepper> stepper_;
  ode::OdeSystemFunction<State> ode_system_;

  ObservableVector observables_;
  ObsManager obsmanager_;

  JsonOutputWriter output_;

  ode::TimeRange range_;
};

}  // namespace netket

#endif  // NETKET_IMAGINARY_TIME_PROPAGATION_HPP


#include "kd_tree.h"
#include <Eigen/Eigenvalues>

#ifdef __SRRG_PARALLEL_KDTREE__
#include <omp.h>
#endif

namespace srrg2_core {

  template <class T, size_t D>
  KDTree_<T, D>::TreeNode::TreeNode(KDTree_<T, D>* tree_, int node_num) :
    _tree(tree_),
    _node_num(node_num),
    _node_type(KDTreeNodeType::Leaf) {
    _mean.setZero();
    _normal.setZero();
    _left_child  = 0;
    _right_child = 0;
    _min_index   = -1;
    _max_index   = -1;
    _num_points  = 0;
  }

  template <class T, size_t D>
  KDTree_<T, D>::TreeNode::TreeNode(KDTree_<T, D>* tree_,
                       int node_num,
                       const VectorTD& mean_,
                       const VectorTD& normal_,
                       TreeNode* left_child,
                       TreeNode* right_child) :
    _tree(tree_),
    _node_num(node_num),
    _node_type(KDTreeNodeType::Middle) {
    _min_index = _max_index = -1;
    assert(normal_.rows() == D);
    assert(mean_.rows() == D);
    _normal      = normal_;
    _mean        = mean_;
    _num_points  = 0;
    _left_child  = left_child;
    _right_child = right_child;
    if (_left_child) {
      _num_points += _left_child->_num_points;
    }
    if (_right_child) {
      _num_points += _right_child->_num_points;
    }
  }

  //! dtor
  template <class T, size_t D>
  KDTree_<T, D>::TreeNode::~TreeNode() {
    if (_left_child) {
      delete _left_child;
      _left_child = 0;
    }
    if (_right_child) {
      delete _right_child;
      _right_child = 0;
    }
  }

  template <class T, size_t D>
  void
  KDTree_<T, D>::TreeNode::
  findNeighbors(VectorTDVector& answers,
                std::vector<int>& indices,
                const VectorTD& query,
                const T maximum_distance,
                KDTreeSearchType search_type) const {
    assert(maximum_distance >= 0);
    const T maximum_squared_distance=pow(maximum_distance,2);
    switch (_node_type) {
    case KDTreeNodeType::Leaf: {
      // ds return all points in this leaf that satisfy the distance
      assert(_max_index >= _min_index);
      const size_t number_of_points = _max_index - _min_index;
      size_t s=answers.size();
      answers.reserve(s+number_of_points);
      indices.reserve(s+number_of_points);
      int k=0;
      for (size_t i = _min_index; i < _max_index; ++i) {
        assert(i < _tree->_points.size());
        const T squared_distance = (_tree->_points[i] - query).squaredNorm();
        if (squared_distance < maximum_squared_distance) {
          answers.emplace_back(_tree->_points[i]);
          indices.emplace_back(_tree->_indices[i]);
          ++k;
        }
      }
      //std::cerr << k << "/" << number_of_points << "/" << indices.size() << " " ;
      return;
    }

    case KDTreeNodeType::Middle: {
      T plane_distance = planeDistance(query);
      //std:: cerr << " - {" << plane_distance << "} ";
      if (search_type==Complete) {
        //std::cerr <<"2(";
        //std::cerr << "answers_size (before): " << answers.size() ;
        //std::cerr << "l(";
        _left_child->findNeighbors(answers, indices, query, maximum_distance, search_type);
        //std::cerr << "answers_size (left): " << answers.size() << std::endl;
        //std::cerr << ")r(";
        _right_child->findNeighbors(answers, indices, query, maximum_distance, search_type);
        //std::cerr << "answers_size (right): " << answers.size() << std::endl;
        //std::cerr <<"))";
      } else if (plane_distance < 0) {
        //std::cerr << "l(";
        _left_child->findNeighbors(answers, indices, query, maximum_distance, search_type);
        //std::cerr << ")";
      } else {
        //std::cerr << "r(";
        _right_child->findNeighbors(answers, indices, query, maximum_distance, search_type);
        //std::cerr << ")";
      }
      return;
    }
    default:
      std::cerr << "KDTree::findNeighbors|ERROR, sanity check (" << _tree
                << ") : " << _tree->sanityCheck() << std::endl;
      throw std::runtime_error("KDTree::findNeighbors|ERROR, unknown node type");
    }
  }

  template <class T, size_t D>
  KDTree_<T, D>::KDTree_(const VectorTDVector& points_, T max_leaf_range, size_t min_leaf_points) {
      good_queries = 0;
      _num_nodes   = 0;
      _points      = points_;
      _aux_points  = points_;
      _indices.resize(points_.size());
      _aux_indices.resize(points_.size());
      _min_leaf_points = min_leaf_points;
      for (size_t i = 0; i < _indices.size(); ++i) {
        _indices[i] = i;
      }
      _root = _buildTree(0, points_.size(), max_leaf_range, 0);
    }

    //! dtor
  template <class T, size_t D>
  KDTree_<T, D>::~KDTree_() {
      if (_root) {
        delete _root;
      }
      _root = 0;
    }

  template <class T, size_t D>
  bool KDTree_<T, D>::sanityCheck() const {
    std::vector<int> checked_indices(_points.size());
    std::fill(checked_indices.begin(), checked_indices.end(), -1);
    const size_t k = sanityCheck(checked_indices, 0, _root);
    if (k != _points.size()) {
      throw std::runtime_error("KDTree::sanityCheck(void)|ERROR, illegal size reported");
    }
    std::sort(checked_indices.begin(), checked_indices.end(), std::less<int>());
    for (size_t i = 0; i < checked_indices.size(); i++) {
      if (i != static_cast<size_t>(checked_indices[i])) {
        throw std::runtime_error("KDTree::sanityCheck(void)|ERROR, missing indices");
        }
    }
    return true;
  }

  template <class T, size_t D>
  int KDTree_<T, D>::sanityCheck(std::vector<int>& checked_indices, int k, const TreeNode* node) const {
    if (!node) {
      return k;
    }
    switch (node->_node_type) {
    case KDTreeNodeType::Leaf:
      for (size_t i = node->_min_index; i < node->_max_index; i++) {
        int idx = _indices[i];
        if (checked_indices[k] != -1) {
          throw std::runtime_error("KDTree::sanityCheck|ERROR, writing on an occupied index");
        }
        checked_indices[k] = idx;
        k++;
      }
      return k;
    case KDTreeNodeType::Middle:
      k = sanityCheck(checked_indices, k, node->_left_child);
      k = sanityCheck(checked_indices, k, node->_right_child);
      return k;
    default:
      throw std::runtime_error("KDTree::sanityCheck|ERROR, illegal type index");
    }
  }

  template <class T, size_t D>
  void KDTree_<T, D>::findNeighbors(VectorTDVector& answers,
                     std::vector<int>& indices,
                     const KDTree_<T, D>::VectorTD& query,
                     const T max_distance,
                     KDTreeSearchType search_type) const {
    if (!_root) {
      throw std::runtime_error("KDTree::findNeighbors|ERROR, no root node");
    }
    answers.clear();
    indices.clear();
    good_queries++;
    _root->findNeighbors(answers, indices, query, max_distance, search_type);
    //std::cerr << "indices.size: " << indices.size() << std::endl;
  }

  template <class T, size_t D>
  T KDTree_<T, D>::findNeighbor(VectorTD& answer,
                                int& index,
                                const VectorTD& query,
                                const T max_distance,
                                const KDTreeSearchType search_type) const {
    if (!_root) {
      throw std::runtime_error("KDTree::findNeighbors|ERROR, no root node");
    }
    index=-1;

    // approx search, we take the quick path unrolling tail recursion
    T max_distance2 = pow(max_distance, 2);
    if (search_type==Approximate) {
      TreeNode* current=_root;
      while(! current->_node_type==Leaf) {
        assert(current->_node_type==Middle && "node is not middle");
        T plane_distance = current->planeDistance(query);
        if(plane_distance<0) {
          current=current->_left_child;
        } else {
          current=current->_right_child;
        }
      }
      assert(current->_node_type==Leaf && "node is leaf");
      assert(current->_max_index >= current->_min_index);
      T best_distance=std::numeric_limits<T>::max();
      for (size_t i = current->_min_index; i < current->_max_index; ++i) {
        const T squared_distance = (_points[i] - query).squaredNorm();
        if (squared_distance < best_distance && squared_distance < max_distance2) {
          best_distance=squared_distance;
          answer=_points[i];
          index=_indices[i];
        }
      }
      return best_distance;
    }

    //exact search, we need a buffer
    VectorTDVector answers;
    std::vector<int> indices;
    _root->findNeighbors(answers, indices, query, max_distance, search_type);
    T best_distance=std::numeric_limits<T>::max();
    for (size_t i=0; i<answers.size(); ++i) {
      T current_distance = (query-answers[i]).squaredNorm();
      if (current_distance<max_distance) {
        answer=answers[i];
        index=indices[i];
      }
    }
    return best_distance;
  }

  template <class T, size_t D>
  T KDTree_<T, D>::_splitPoints(KDTree_<T, D>::VectorTD& mean,
                               KDTree_<T, D>::VectorTD& normal,
                               size_t& num_left_points,
                               const size_t min_index,
                               const size_t max_index) {
    // if points empty, nothing to do
    if (min_index == max_index) {
      return 0;
    }

    const size_t num_points            = max_index - min_index;
    const T inverse_num_points         = 1.0 / num_points;
    VectorTD sum                       = VectorTD::Zero();
    Eigen::Matrix<T, D, D> squared_sum = Eigen::Matrix<T, D, D>::Zero();
    Eigen::Matrix<T, D, D> covariance  = Eigen::Matrix<T, D, D>::Zero();
    for (size_t i = min_index; i < max_index; ++i) {
      sum += _points[i];
      squared_sum += _points[i] * _points[i].transpose();
    }
    mean       = sum * inverse_num_points;
    covariance = squared_sum * inverse_num_points - mean * mean.transpose();

    // eigenvalue decomposition
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, D, D>> solver;
    solver.compute(covariance, Eigen::ComputeEigenvectors);
    normal = solver.eigenvectors().col(D - 1).normalized();

    // the following var will contain the range of points along the normal
    // vector
    T max_distance_from_plane = 0;

    // run through the points and split them in the left or the right set
    size_t left_index  = min_index;
    size_t right_index = max_index;

    size_t num_left  = 0;
    size_t num_right = 0;
    for (size_t i = min_index; i < max_index; ++i) {
      T distance_from_plane = normal.dot(_points[i] - mean);
      if (fabs(distance_from_plane) > max_distance_from_plane) {
        max_distance_from_plane = fabs(distance_from_plane);
      }

      bool side = distance_from_plane < 0;
      if (side) {
        _aux_points[left_index]  = _points[i];
        _aux_indices[left_index] = _indices[i];
        left_index++;
        num_left++;
      } else {
        right_index--;
        _aux_points[right_index]  = _points[i];
        _aux_indices[right_index] = _indices[i];
        num_right++;
      }
    }
    assert(max_index - min_index == num_right + num_left);
    for (size_t i = min_index; i < max_index; ++i) {
      _points[i]  = _aux_points[i];
      _indices[i] = _aux_indices[i];
    }

    num_left_points = num_left;
    return max_distance_from_plane;
  }

  template <class T, size_t D>
    typename KDTree_<T, D>::TreeNode*
    KDTree_<T, D>::_buildTree(const size_t min_index, const size_t max_index, const T max_leaf_range, int level) {
      const size_t num_points = max_index - min_index;
      if (!num_points) {
        return 0;
      }

      VectorTD mean;
      VectorTD normal;
      size_t num_left_points = 0;

      const T range = _splitPoints(mean, normal, num_left_points, min_index, max_index);
      assert(range >= 0);
      TreeNode* node = 0;
      if (range < max_leaf_range || num_points < _min_leaf_points) {
        node = new TreeNode(this, _num_nodes);
        node->_min_index=min_index;
        node->_max_index=max_index;
        node->_num_points = num_points;
        //std::cerr << "leaf: " << num_points << std::endl;
      } else {
        TreeNode *left_tree, *right_tree;

#ifdef __SRRG_PARALLEL_KDTREE__
        int num_threads = omp_get_max_threads();
        int split_level = -1;
        if (level > 0)
          split_level = floor(log(num_threads) / log(2));

        if (split_level == level) {
#pragma omp parallel sections
          {
#pragma omp section
            {
              left_tree =
                _buildTree(min_index, min_index + num_left_points, max_leaf_range, level + 1);
            }
#pragma omp section
            {
              right_tree =
                _buildTree(min_index + num_left_points, max_index, max_leaf_range, level + 1);
            }
          }
        } else {
#endif //__SRRG_PARALLEL_KDTREE__

          left_tree = _buildTree(min_index, min_index + num_left_points, max_leaf_range, level + 1);
          right_tree =
            _buildTree(min_index + num_left_points, max_index, max_leaf_range, level + 1);
#ifdef __SRRG_PARALLEL_KDTREE__
        }
#endif
        node = new TreeNode(this, _num_nodes, mean, normal, left_tree, right_tree);
      }
      _num_nodes++;
      return node;
    }

  template <class T, size_t D>
  void KDTree_<T, D>::_printKDTree(TreeNode* node) {
    std::cerr << "(";
    switch (node->_node_type) {
    case Leaf:
      std::cerr << node->_max_index - node->_min_index;
      // for (size_t i = node->_min_index; i < node->_max_index; ++i) {
      //   std::cerr << _points[i].transpose() << std::endl;
      // }
      break;
    case Middle:
      _printKDTree(node->_left_child);
      _printKDTree(node->_right_child);
      break;
    }
    std::cerr << ")";

  }

} // namespace srrg2_core


//
//  main.cpp
//  ArticleDynamicFiniteDomain
//
//  Created by Gabriel Aubut-Lussier on 2015-07-30.
//

#include <boost/icl/interval_map.hpp>
#include <boost/icl/split_interval_map.hpp>
#include <boost/icl/concept/interval.hpp>
#include <iostream>
#include <set>

class DynamicDomain
{
public:
	DynamicDomain(int intValue, std::set<std::unique_ptr<DynamicDomain>>* collection);

	DynamicDomain* getPredecessor();
	DynamicDomain* getSuccessor();

	int intValue;

private:
	std::set<std::unique_ptr<DynamicDomain>>* collection;
};

namespace std
{
	template <>
	struct less<DynamicDomain*>
	{
		bool operator()( const DynamicDomain* lhs, const DynamicDomain* rhs ) const
		{
			return lhs->intValue < rhs->intValue;
		}
	};
}

namespace boost { namespace icl {
	template<>
	bool is_empty<boost::icl::closed_interval<DynamicDomain*>>(const boost::icl::closed_interval<DynamicDomain*>& object)
	{
		return object.lower() == nullptr || object.upper() == nullptr || boost::icl::domain_less<boost::icl::closed_interval<DynamicDomain*>>(object.upper(), object.lower());
	}
}}

namespace boost { namespace icl { namespace detail {
	template <>
	struct successor<DynamicDomain*, true>
	{
		inline static DynamicDomain* apply(DynamicDomain* value)
		{
			return value->getSuccessor();
		}
	};

	template <>
	struct successor<DynamicDomain*, false>
	{
		inline static DynamicDomain* apply(DynamicDomain* value)
		{
			return value->getPredecessor();
		}
	};

	template <>
	struct predecessor<DynamicDomain*, true>
	{
		inline static DynamicDomain* apply(DynamicDomain* value)
		{
			return value->getPredecessor();
		}
	};

	template <>
	struct predecessor<DynamicDomain*, false>
	{
		inline static DynamicDomain* apply(DynamicDomain* value)
		{
			return value->getSuccessor();
		}
	};
}}}

template <typename Domain, typename Codomain, typename Traits = boost::icl::partial_absorber, template<class>class Compare = std::less, template<class>class Combine = boost::icl::inplace_plus, template<class>class Section = boost::icl::inter_section>
using closed_interval_map = boost::icl::interval_map<Domain, Codomain, Traits, Compare, Combine, Section, boost::icl::closed_interval<Domain>>;

typedef closed_interval_map<DynamicDomain*, int> MapType;

int main(int argc, const char * argv[])
{
	std::set<std::unique_ptr<DynamicDomain>> domainSet;
	domainSet.insert(std::unique_ptr<DynamicDomain>(new DynamicDomain(7, &domainSet)));
	domainSet.insert(std::unique_ptr<DynamicDomain>(new DynamicDomain(1, &domainSet)));
	domainSet.insert(std::unique_ptr<DynamicDomain>(new DynamicDomain(2, &domainSet)));
	domainSet.insert(std::unique_ptr<DynamicDomain>(new DynamicDomain(6, &domainSet)));
	domainSet.insert(std::unique_ptr<DynamicDomain>(new DynamicDomain(3, &domainSet)));
	domainSet.insert(std::unique_ptr<DynamicDomain>(new DynamicDomain(4, &domainSet)));
	domainSet.insert(std::unique_ptr<DynamicDomain>(new DynamicDomain(9001, &domainSet)));
	domainSet.insert(std::unique_ptr<DynamicDomain>(new DynamicDomain(5, &domainSet)));

	MapType intervalMap;
	std::set<std::unique_ptr<DynamicDomain>>::iterator it = domainSet.begin();
	intervalMap.add(std::make_pair(MapType::interval_type::closed_interval(it->get(), std::next(it, 1)->get()), 1));
	intervalMap.add(std::make_pair(MapType::interval_type::closed_interval(std::next(it, 2)->get(), std::next(it, 3)->get()), 1));
	intervalMap.add(std::make_pair(MapType::interval_type::closed_interval(std::next(it, 1)->get(), std::next(it, 3)->get()), 1));
	for (auto element : intervalMap) {
		std::cout << "Domain: [" << element.first.lower()->intValue << ", " << element.first.upper()->intValue << "] = " << element.second << std::endl;
	}
}

DynamicDomain::DynamicDomain(int intValue, std::set<std::unique_ptr<DynamicDomain>>* collection)
: intValue(intValue), collection(collection)
{}

DynamicDomain*
DynamicDomain::getPredecessor()
{
	DynamicDomain* result;
	std::set<std::unique_ptr<DynamicDomain>>::iterator it = std::lower_bound(collection->begin(), collection->end(), this, [](const std::unique_ptr<DynamicDomain>& lhs, DynamicDomain* rhs) {
		return std::less<DynamicDomain*>()(lhs.get(), rhs);
	});
	if (it == collection->begin()) {
		result = nullptr;
	} else {
		result = std::prev(it)->get();
	}
	return result;
}

DynamicDomain*
DynamicDomain::getSuccessor()
{
	DynamicDomain* result;
	std::set<std::unique_ptr<DynamicDomain>>::iterator it = std::next(std::lower_bound(collection->begin(), collection->end(), this, [](const std::unique_ptr<DynamicDomain>& lhs, DynamicDomain* rhs) {
		return std::less<DynamicDomain*>()(lhs.get(), rhs);
	}));
	if (it == collection->end()) {
		result = nullptr;
	} else {
		result = it->get();
	}
	return result;
}


// Parallel NanoJPEG -- Copyright (c) 2021 Ngyu-Phee Yen <yyuan@gmx.com>
// NanoJPEG -- KeyJ's Tiny Baseline JPEG Decoder
// version 1.3.5 (2016-11-14)
// Copyright (c) 2009-2016 Martin J. Fiedler <martin.fiedler@gmx.net>
// published under the terms of the MIT license
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to
// deal in the Software without restriction, including without limitation the
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
// sell copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

// Include statements.
#include <algorithm>
#include <atomic>
#include <array>
#include <cassert>
#include <cstddef>
#include <cstdint>
#include <exception>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>
#include <memory>
#include <utility>
#include <thread>

#include <boost/program_options/options_description.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/variables_map.hpp>
#include <boost/tokenizer.hpp>
#include <boost/token_functions.hpp>

#include "thread_pool/thread_pool.hpp"

using namespace std;
using namespace thread_pool;
using namespace boost::program_options;

struct block_context
{
    const uint8_t *pos;
    int size;
    int buf;
    int bufbits;
    array<int, 3> dcpred;
    array<int, 64> block;
};

struct vlc_code
{
    uint8_t bits;
    uint8_t code;
};

struct component
{
    int cid;
    int ssx;
    int ssy;
    int width;
    int height;
    int stride;
    int qtsel;
    int actabsel;
    int dctabsel;
    int dcpred;
    uint8_t *pixels;
};

struct context
{
    std::vector<uint8_t> bitstream;
    int width;
    int height;
    int mbwidth;
    int mbheight;
    int mbsizex;
    int mbsizey;
    int ncomp;
    array<component, 3> comp;
    int qtused;
    int qtavail;
    int rstinterval;
    array<array<uint8_t, 64>, 4> qtab;
    array<array<vlc_code, 64 * 1024>, 5> vlctab;
};

static const char njZZ[64] = { 0, 1, 8, 16, 9, 2, 3, 10, 17, 24, 32, 25, 18,
11, 4, 5, 12, 19, 26, 33, 40, 48, 41, 34, 27, 20, 13, 6, 7, 14, 21, 28, 35,
42, 49, 56, 57, 50, 43, 36, 29, 22, 15, 23, 30, 37, 44, 51, 58, 59, 52, 45,
38, 31, 39, 46, 53, 60, 61, 54, 47, 55, 62, 63 };

inline static uint8_t clip(const int x)
{
    return x < 0 ? 0 : (x > 0xFF ? 0xFF : static_cast<uint8_t>(x));
}

#define W1 2841
#define W2 2676
#define W3 2408
#define W5 1609
#define W6 1108
#define W7 565

inline static void row_idct(int *blk)
{
    int x0, x1, x2, x3, x4, x5, x6, x7, x8;

    if (!((x1 = blk[4] << 11) | (x2 = blk[6]) | (x3 = blk[2]) |
          (x4 = blk[1]) | (x5 = blk[7]) | (x6 = blk[5]) | (x7 = blk[3])))
    {
        blk[0] = blk[1] = blk[2] = blk[3] = blk[4] =
        blk[5] = blk[6] = blk[7] = blk[0] << 3;
        return;
    }

    x0 = (blk[0] << 11) + 128;
    x8 = W7 * (x4 + x5);
    x4 = x8 + (W1 - W7) * x4;
    x5 = x8 - (W1 + W7) * x5;
    x8 = W3 * (x6 + x7);
    x6 = x8 - (W3 - W5) * x6;
    x7 = x8 - (W3 + W5) * x7;
    x8 = x0 + x1;
    x0 -= x1;
    x1 = W6 * (x3 + x2);
    x2 = x1 - (W2 + W6) * x2;
    x3 = x1 + (W2 - W6) * x3;
    x1 = x4 + x6;
    x4 -= x6;
    x6 = x5 + x7;
    x5 -= x7;
    x7 = x8 + x3;
    x8 -= x3;
    x3 = x0 + x2;
    x0 -= x2;
    x2 = (181 * (x4 + x5) + 128) >> 8;
    x4 = (181 * (x4 - x5) + 128) >> 8;
    blk[0] = (x7 + x1) >> 8;
    blk[1] = (x3 + x2) >> 8;
    blk[2] = (x0 + x4) >> 8;
    blk[3] = (x8 + x6) >> 8;
    blk[4] = (x8 - x6) >> 8;
    blk[5] = (x0 - x4) >> 8;
    blk[6] = (x3 - x2) >> 8;
    blk[7] = (x7 - x1) >> 8;
}

inline static void col_idct(const int *blk,
                            uint8_t *out, int stride)
{
    int x0, x1, x2, x3, x4, x5, x6, x7, x8;

    if (!((x1 = blk[8*4] << 8) | (x2 = blk[8*6]) | (x3 = blk[8*2]) |
          (x4 = blk[8*1]) | (x5 = blk[8*7]) | (x6 = blk[8*5]) | (x7 = blk[8*3])))
    {
        x1 = clip(((blk[0] + 32) >> 6) + 128);
        for (x0 = 8; x0; --x0)
        {
            *out = static_cast<uint8_t>(x1);
            out += stride;
        }
        return;
    }

    x0 = (blk[0] << 8) + 8192;
    x8 = W7 * (x4 + x5) + 4;
    x4 = (x8 + (W1 - W7) * x4) >> 3;
    x5 = (x8 - (W1 + W7) * x5) >> 3;
    x8 = W3 * (x6 + x7) + 4;
    x6 = (x8 - (W3 - W5) * x6) >> 3;
    x7 = (x8 - (W3 + W5) * x7) >> 3;
    x8 = x0 + x1;
    x0 -= x1;
    x1 = W6 * (x3 + x2) + 4;
    x2 = (x1 - (W2 + W6) * x2) >> 3;
    x3 = (x1 + (W2 - W6) * x3) >> 3;
    x1 = x4 + x6;
    x4 -= x6;
    x6 = x5 + x7;
    x5 -= x7;
    x7 = x8 + x3;
    x8 -= x3;
    x3 = x0 + x2;
    x0 -= x2;
    x2 = (181 * (x4 + x5) + 128) >> 8;
    x4 = (181 * (x4 - x5) + 128) >> 8;
    *out = clip(((x7 + x1) >> 14) + 128);  out += stride;
    *out = clip(((x3 + x2) >> 14) + 128);  out += stride;
    *out = clip(((x0 + x4) >> 14) + 128);  out += stride;
    *out = clip(((x8 + x6) >> 14) + 128);  out += stride;
    *out = clip(((x8 - x6) >> 14) + 128);  out += stride;
    *out = clip(((x0 - x4) >> 14) + 128);  out += stride;
    *out = clip(((x3 - x2) >> 14) + 128);  out += stride;
    *out = clip(((x7 - x1) >> 14) + 128);
}

inline static std::pair<uint8_t, int> seek_rst(const uint8_t *bitstream,
                                               int size)
{
    uint8_t rst = 0xFF;
    const uint8_t *pos = bitstream;
    while (0 < size)
    {
        if (0xFF == pos[0] && 0xD0 == (pos[1] & 0xF8))
        {
            rst = pos[1] & 0x07;
            break;
        }
        pos++; size--;
    }
    if (0xFF == rst)
    {
        if (0xFF == pos[-2] && 0xD9 == pos[-1])
        {
            return std::make_pair(rst, pos - bitstream - 2);
        }
        else
        {
            throw length_error("data too short to decode RST");
        }
    }

    return std::make_pair(rst, pos - bitstream);
}

inline static uint16_t decode_16b(const uint8_t *bitstream)
{
    return (bitstream[0] << 8) | bitstream[1];
}

inline static uint16_t decode_len(const uint8_t *&bitstream, const int size)
{
    if (2 > size)
    {
        throw length_error("data too short to decode length");
    }

    uint16_t length = decode_16b(bitstream);

    if (length > size)
    {
        throw length_error("data too short to decode content");
    }

    bitstream += 2;

    return length - 2;
}

inline static int show_bits(block_context *bc, int bits)
{
    uint8_t newbyte;
    if (!bits) return 0;

    while (bc->bufbits < bits)
    {
        if (bc->size <= 0)
        {
            bc->buf = (bc->buf << 8) | 0xFF;
            bc->bufbits += 8;
            continue;
        }
        newbyte = *bc->pos++;
        bc->size--;
        bc->bufbits += 8;
        bc->buf = (bc->buf << 8) | newbyte;
        if (newbyte == 0xFF)
        {
            if (bc->size)
            {
                uint8_t marker = *bc->pos++;
                bc->size--;
                switch (marker)
                {
                    case 0x00:
                    case 0xFF:
                        break;
                    case 0xD9: bc->size = 0; break;
                    default:
                        if ((marker & 0xF8) != 0xD0)
                        {
                            throw runtime_error("invalid semantics in decoding VLC");
                        }
                        else
                        {
                            bc->buf = (bc->buf << 8) | marker;
                            bc->bufbits += 8;
                        }
                }
            }
            else
            {
                throw runtime_error("invalid semantics in decoding VLC");
            }
        }
    }
    return (bc->buf >> (bc->bufbits - bits)) & ((1 << bits) - 1);
}

inline static void skip_bits(block_context *bc, int bits)
{
    if (bc->bufbits < bits) show_bits(bc, bits);
    bc->bufbits -= bits;
}

inline static int get_bits(block_context *bc, int bits)
{
    int res = show_bits(bc, bits);
    skip_bits(bc, bits);

    return res;
}

static int get_vlc(block_context *bc, const vlc_code *vlc, uint8_t *code)
{
    int value = show_bits(bc, 16);
    int bits = vlc[value].bits;
    if (!bits)
    {
        throw runtime_error("invalid semantics in decoding VLC");
        return 0;
    }
    skip_bits(bc, bits);
    value = vlc[value].code;
    if (code) *code = static_cast<uint8_t>(value);
    bits = value & 15;
    if (!bits) return 0;

    value = get_bits(bc, bits);
    if (value < (1 << (bits - 1))) value += ((-1) << bits) + 1;
    return value;
}

inline static void decode_blk(const context &ctx, const int i,
                              block_context *bc, uint8_t *out)
{
    uint8_t code = 0;
    int value, coef = 0;
    const component &c = ctx.comp[i];

    bc->block.fill(0);
    bc->dcpred[i] += get_vlc(bc, &ctx.vlctab[c.dctabsel][0], nullptr);
    bc->block[0] = bc->dcpred[i] * ctx.qtab[c.qtsel][0];

    do
    {
        value = get_vlc(bc, &ctx.vlctab[c.actabsel][0], &code);
        if (!code) break;  // EOB
        if (!(code & 0x0F) && (code != 0xF0))
        {
            throw runtime_error("invalid semantics in decoding BLK");
        }
        coef += (code >> 4) + 1;
        if (coef > 63)
        {
            throw runtime_error("invalid semantics in decoding BLK");
        }
        bc->block[njZZ[coef]] = value * ctx.qtab[c.qtsel][coef];
    } while (coef < 63);

    for (coef = 0; coef < 64; coef += 8) row_idct(&bc->block[coef]);
    for (coef = 0; coef < 8; ++coef)
    {
        col_idct(&bc->block[coef], &out[coef], c.stride);
    }
}

void decode_blks(const uint8_t *bitstream, int size, const context &ctx,
                 const int start_block, const int num_blocks)
{
    int mby = start_block / ctx.mbwidth;
    int mbx = start_block - mby * ctx.mbwidth;

    std::unique_ptr<block_context> bc = std::make_unique<block_context>();

    std::memset(bc.get(), 0, sizeof(block_context));

    bc->pos = bitstream;
    bc->size = size;

    for (int b = 0; b < num_blocks; ++b)
    {
        for (int i = 0; i < ctx.ncomp; ++i)
        {
            const component &c = ctx.comp[i];
            for (int sby = 0; sby < c.ssy; ++sby)
            {
                for (int sbx = 0; sbx < c.ssx; ++sbx)
                {
                    decode_blk(ctx, i, bc.get(),
                               &c.pixels[((mby * c.ssy + sby) * c.stride + mbx * c.ssx + sbx) << 3]);
                }
            }
        }
        if (++mbx >= ctx.mbwidth)
        {
            mbx = 0;
            if (++mby >= ctx.mbheight) break;
        }
    }
}

void decode_dri(const uint8_t *bitstream, int size, context &ctx)
{
    uint32_t length = decode_len(bitstream, size);
    if (2 > length)
    {
        throw length_error("data too short to decode DRI");
    }
    ctx.rstinterval = decode_16b(bitstream);

    //cout << "DRI done\n";
}

void decode_dqt(const uint8_t *bitstream, int size, context &ctx,
                atomic<int *> &qtavail)
{
    uint32_t length = decode_len(bitstream, size);

    while (length >= 65)
    {
        int i = bitstream[0];
        if (i & 0xFC)
        {
            throw runtime_error("invalid semantics in decoding DQT");
        }
        *qtavail |= 1 << i;
        std::copy_n(&bitstream[i + 1], 64, ctx.qtab[i].begin());
        bitstream += 65; length -= 65;
    }
    if (length)
    {
        throw length_error("extra data after decoding DQT");
    }

    //cout << "DQT done\n";
}

void decode_sos(const uint8_t *bitstream, int size, context &ctx)
{
    uint32_t length = decode_len(bitstream, size);

    if (4 + ctx.ncomp * 2 > length)
    {
        throw length_error("data too short to decode SOS");
    }
    if (bitstream[0] != ctx.ncomp)
    {
        throw runtime_error("invalid semantics in decoding SOS");
    }
    bitstream += 1; length -= 1;
    for (int i = 0; i < ctx.ncomp; ++i)
    {
        component &c = ctx.comp[i];
        if (c.cid != bitstream[0] || (bitstream[1] & 0xEE))
        {
            throw runtime_error("invalid semantics in decoding SOS");
        }
        c.dctabsel = bitstream[1] >> 4;
        c.actabsel = (bitstream[1] & 1) | 2;
        bitstream += 2; length -= 2;
    }
    if (bitstream[0] || bitstream[1] != 63 || bitstream[2])
    {
        throw runtime_error("invalid semantics in decoding SOS");
    }

    //cout << "SOS done\n";
}

void decode_dht(const uint8_t *bitstream, int size, context &ctx)
{
    uint32_t length = decode_len(bitstream, size);

    std::array<uint8_t, 16> counts;

    while (length >= 17)
    {
        int i = bitstream[0];
        if (i & 0xEC)
        {
            throw runtime_error("invalid semantics in decoding DHT");
        }
        if (i & 0x02)
        {
            throw runtime_error("unsupported semantics in decoding DHT");
        }
        i = (i | (i >> 3)) & 3;  // combined DC/AC + tableid value

        std::copy_n(&bitstream[1], 16, counts.begin());

        bitstream += 17; length -= 17;

        vlc_code *vlc = ctx.vlctab[i].data();
        int spread = 65536;
        int remain = spread;
        for (int codelen = 1; codelen <= 16; ++codelen)
        {
            spread >>= 1;
            int currcnt = counts[codelen - 1];
            if (!currcnt) continue;

            if (length < currcnt)
            {
                throw length_error("data too short to decode DHT");
            }
            remain -= currcnt << (16 - codelen);
            if (remain < 0)
            {
                throw length_error("data too short to decode DHT");
            }
            for (i = 0; i < currcnt; ++i)
            {
                uint8_t code = bitstream[i];
                for (int j = spread; j; --j, ++vlc)
                {
                    *vlc = {static_cast<uint8_t>(codelen), code};
                }
            }
            bitstream += currcnt; length -= currcnt;
        }
        while (remain--)
        {
            *vlc = {0, 0};
            ++vlc;
        }
    }
    if (length)
    {
        throw length_error("extra data after decoding DHT");
    }
    //cout << "DHT done\n";
}

void decode_sof(const uint8_t *bitstream, int size, context &ctx)
{
    uint32_t length = decode_len(bitstream, size);

    if (length < 9)
    {
        throw length_error("data too short to decode SOF");
    }
    if (bitstream[0] != 8)
    {
        throw runtime_error("unsupported semantics in decoding SOF");
    }

    ctx.height = decode_16b(&bitstream[1]);
    ctx.width = decode_16b(&bitstream[3]);
    if (!ctx.width || !ctx.height)
    {
        throw runtime_error("invalid semantics in decoding SOF");
    }
    ctx.ncomp = bitstream[5];
    bitstream += 6; length -= 6;

    switch (ctx.ncomp)
    {
        case 1:
        case 3:
            break;
        default:
            throw runtime_error("unsupported semantics in decoding SOF");
            break;
    }

    if (length < ctx.ncomp * 3)
    {
        throw length_error("data too short to decode SOF");
    }

    int qtused = 0;
    int ssxmax = 0;
    int ssymax = 0;
    component *c = nullptr;
    for (int i = 0; i < ctx.ncomp; ++i)
    {
        c = &ctx.comp[i];

        c->cid = bitstream[0];
        if (!(c->ssx = bitstream[1] >> 4))
        {
            throw runtime_error("invalid semantics in decoding SOF");
        }
        if (c->ssx & (c->ssx - 1))
        {
            throw runtime_error("unsupported semantics in decoding SOF");
        }
        if (!(c->ssy = bitstream[1] & 15))
        {
            throw runtime_error("invalid semantics in decoding SOF");
        }
        if (c->ssy & (c->ssy - 1))
        {
            throw runtime_error("unsupported semantics in decoding SOF");
        }
        if ((c->qtsel = bitstream[2]) & 0xFC)
        {
            throw runtime_error("invalid semantics in decoding SOF");
        }
        bitstream += 3; length -= 3;
        ctx.qtused |= 1 << c->qtsel;
        if (c->ssx > ssxmax) ssxmax = c->ssx;
        if (c->ssy > ssymax) ssymax = c->ssy;
    }

    if (ctx.ncomp == 1)
    {
        c = &ctx.comp[0];
        c->ssx = c->ssy = ssxmax = ssymax = 1;
    }
    ctx.mbsizex = ssxmax << 3;
    ctx.mbsizey = ssymax << 3;
    ctx.mbwidth = (ctx.width + ctx.mbsizex - 1) / ctx.mbsizex;
    ctx.mbheight = (ctx.height + ctx.mbsizey - 1) / ctx.mbsizey;
    for (int i = 0; i < ctx.ncomp; ++i)
    {
        c = &ctx.comp[i];
        c->width = (ctx.width * c->ssx + ssxmax - 1) / ssxmax;
        c->height = (ctx.height * c->ssy + ssymax - 1) / ssymax;
        c->stride = ctx.mbwidth * c->ssx << 3;
        if (((c->width < 3) && (c->ssx != ssxmax)) ||
            ((c->height < 3) && (c->ssy != ssymax)))
        {
            throw runtime_error("invalid semantics in decoding SOF");
        }
    }

    //cout << "SOF done\n";
}

///////////////////////////////////////////////////////////////////////////////
int main(int argc, char *argv[])
{
    options_description desc
    (
        "\nThread-pool based NanoJPEG.\n"
        "\nAllowed arguments"
    );

    // Define command line arguments using either formats:
    //
    //     (“long-name,short-name”, “Description of argument”)
    //     for flag values or
    //
    //     (“long-name,short-name”, <data-type>,
    //     “Description of argument”) arguments with values
    //
    // Remember that arguments with values may be multi-values
    // and must be vectors
    desc.add_options()
    ("help", "Print help message.")
    ("threads,t", value<int>(), "Number of threads in pool, default: 0")
    ("dump-width,w", value<int>(),
     "dumped width default: min(width, 1920)")
    ("dump-height,h", value<int>(),
     "dumped height default: min(height, 1080)")
    ("ow", value<int>(),
     "dumped width offset default: 0")
    ("oh", value<int>(),
     "dumped height offset default: 0")
    ("output-file,o", value<string>(),
     "Path to the output image, default: out.yuv")
    ("input-file,i", value<string>(), "Path to the input image.");

    // Map positional parameters to their tag valued types
    // (e.g. –input-file parameters)
    positional_options_description p;
    p.add("input-file", -1);

    // Parse the command line catching and displaying any
    // parser errors
    variables_map vm;
    try
    {
        store(command_line_parser(argc, argv).options(desc)
                                             .positional(p).run(), vm);
        notify(vm);
    }
    catch (exception &e)
    {
        cout << '\n' << e.what() << '\n';
        cout << desc << '\n';
    }

    // Display help text when requested
    if (vm.count("help"))
    {
        cout << "–help specified\n";
        cout << desc << '\n';
        return 0;
    }

    int threads = 0;
    if (vm.count("threads"))
    {
        threads = vm["threads"].as<int>();
        cout << "–threads specified with value = " << threads << '\n';
    }

    string out_filename("out.yuv");
    if (vm.count("output-file"))
    {
        out_filename = vm["output-file"].as<string>();
        cout << "–output-file specified with value = " << out_filename << '\n';
    }

    string in_filename;
    if (vm.count("input-file"))
    {
        in_filename = vm["input-file"].as<string>();
        cout << "–input-file specified with value = " << in_filename << '\n';
    }

    ThreadPool thread_pool{threads ? threads : thread::hardware_concurrency()};

    if (in_filename.empty())
    {
        throw invalid_argument("input file path not given");
    }

    context jpg_ctx;
    memset(&jpg_ctx, 0, sizeof(context));
    for (auto &qt : jpg_ctx.qtab) qt.fill(0);
    for (auto &vt : jpg_ctx.vlctab) vt.fill({0, 0});

    std::ifstream ifs(in_filename, std::ios::binary | std::ios::ate);
    if (false == ifs.is_open())
    {
        throw system_error(error_code(), "cannot open input file");
    }
    const auto fsize = ifs.tellg() & 0x7FFFFFF;

    jpg_ctx.bitstream.resize(fsize);
    ifs.seekg(std::ios::beg);
    ifs.read(reinterpret_cast<char *>(jpg_ctx.bitstream.data()), fsize);
    const uint8_t *bitstream = jpg_ctx.bitstream.data();

    uint32_t length = fsize;

    if (2 > length)
    {
        throw length_error("data too short to decode JPEG");
    }
    if ((bitstream[0] ^ 0xFF) | (bitstream[1] ^ 0xD8))
    {
        throw runtime_error("invalid semantics in decoding JPEG");
    }
    bitstream += 2; length -= 2;

    uint32_t bitmask = 0;
    atomic<int *> qtavail{&jpg_ctx.qtavail};
    vector<future<void>> futures;

    bool no_rst = false;
    while (length && 15 != bitmask)
    {
        if (2 > length || (bitstream[0] != 0xFF))
        {
            throw length_error("data too short to decode SOF");
        }
        bitstream += 2; length -= 2;
        int marker_len = decode_16b(bitstream);
        switch (bitstream[-1])
        {
            case 0xC0:
                cout << "SOF buffer = "
                     << static_cast<const void *>(bitstream)
                     << ", length = " << marker_len << '\n';
                futures.emplace_back(
                    thread_pool.Submit(decode_sof, bitstream, marker_len,
                                       std::ref(jpg_ctx)));
                bitmask |= 1;
                break;
            case 0xC4:
                cout << "DHT buffer = "
                     << static_cast<const void *>(bitstream)
                     << ", length = " << marker_len << '\n';
                futures.emplace_back(
                    thread_pool.Submit(decode_dht, bitstream, marker_len,
                                       std::ref(jpg_ctx)));
                bitmask |= 2;
                break;
            case 0xDA:
                cout << "No DRI/RST in file\n";
                no_rst = true;
                bitstream -= 2; length += 2;
                break;
            case 0xDB:
                cout << "DQT buffer = "
                     << static_cast<const void *>(bitstream)
                     << ", length = " << marker_len << '\n';
                futures.emplace_back(
                    thread_pool.Submit(decode_dqt, bitstream, marker_len,
                                       std::ref(jpg_ctx), std::ref(qtavail)));
                bitmask |= 4;
                break;
            case 0xDD:
                cout << "DRI buffer = "
                     << static_cast<const void *>(bitstream)
                     << ", length = " << marker_len << '\n';
                futures.emplace_back(
                    thread_pool.Submit(decode_dri, bitstream, marker_len,
                                       std::ref(jpg_ctx)));
                bitmask |= 8;
                break;
            case 0xFE:
                break;
            default:
                if ((bitstream[-1] & 0xF0) != 0xE0)
                {
                    throw runtime_error("unsupported semantics in decoding JPEG");
                }
                break;
        }

        if (no_rst) break;

        bitstream += marker_len; length -= marker_len;
    }

    for (const auto &f : futures)
    {
        f.wait();
    }

    array<unique_ptr<uint8_t []>, 3> out_buffer;
    for (int i = 0; i < jpg_ctx.ncomp; ++i)
    {
        component &c = jpg_ctx.comp[i];
        out_buffer[i] = make_unique<uint8_t[]>(c.stride * jpg_ctx.mbheight *
                                               c.ssy << 3);
        c.pixels = out_buffer[i].get();
    }

#if 1
    cout << "RST interval = " << jpg_ctx.rstinterval << '\n';
    cout << "QTable availability = " << jpg_ctx.qtavail << '\n';
    cout << "width = " << jpg_ctx.width << ", "
         << "height = " << jpg_ctx.height << '\n';
    cout << "mbwidth = " << jpg_ctx.mbwidth << ", "
         << "mbheight = " << jpg_ctx.mbheight << '\n';

    for (int i = 0; i < jpg_ctx.ncomp; ++i)
    {
        const component &c = jpg_ctx.comp[i];
        cout << "Component " << i << ":\n";
        cout << "\tssx = " << c.ssx << ", ssy = " << c.ssy << '\n';
        cout << "\tw = " << c.width << ", h = " << c.height << ", "
             << "stride = " << c.stride << '\n';
        cout << "\tqtsel = " << c.qtsel << ", "
             << "acsel = " << c.actabsel << ", "
             << "dcsel = " << c.dctabsel << '\n';
        cout << "\tbuffer = " << static_cast<void *>(c.pixels) << '\n';
    }
#endif

    futures.resize(0);
    bitmask = 0;
    while (length && 1 != bitmask)
    {
        if (2 > length || (bitstream[0] != 0xFF))
        {
            throw length_error("data too short to decode SOS");
        }
        bitstream += 2; length -= 2;
        const int marker_len = decode_16b(bitstream);
        switch (bitstream[-1])
        {
            case 0xDA:
                cout << "SOS buffer = "
                     << static_cast<const void *>(bitstream)
                     << ", length = " << marker_len << '\n';
                futures.emplace_back(
                    thread_pool.Submit(decode_sos, bitstream, marker_len,
                                       std::ref(jpg_ctx)));
                bitmask = 1;
                break;
            case 0xC0:
            case 0xC4:
            case 0xDB:
            case 0xDD:
                throw runtime_error("unsupported semantics in decoding JPEG");
                break;
            case 0xFE:
                break;
            default:
                if ((bitstream[-1] & 0xF0) != 0xE0)
                {
                    throw runtime_error("unsupported semantics in decoding JPEG");
                }
                break;
        }
        bitstream += marker_len; length -= marker_len;
    }

    for (const auto &f : futures)
    {
        f.wait();
    }

#if 0
    for (auto &t : jpg_ctx.vlctab)
    {
        for (auto &v : t)
        {
            cout << "bits = " << static_cast<uint32_t>(v.bits)
                 << ", code = " << static_cast<uint32_t>(v.code) << '\n';
        }
    }

    for (auto &qt : jpg_ctx.qtab)
    {
        cout << '\n';
        for (auto &q : qt) cout << static_cast<uint32_t>(q) << ", ";
        cout << '\n';
    }
#endif

    if (no_rst)
    {
        int num_blocks = jpg_ctx.mbheight * jpg_ctx.mbwidth;
        decode_blks(bitstream, length, jpg_ctx, 0, num_blocks);
    }
    else
    {
        int mbxy = 0;
        int curr_rst = 7;

        futures.resize(0);
        while (length)
        {
            auto blocks = seek_rst(bitstream, length);
            int num_blocks = ((8 + blocks.first - curr_rst) & 7) *
                             jpg_ctx.rstinterval;

            if (0xFF == blocks.first)
            {
                num_blocks = jpg_ctx.mbwidth * jpg_ctx.mbheight - mbxy;
            }

#if 0
            format_to(cout, "BLOCKS buffer = {1}, length = {2}\n",
                      bitstream, blocks.second) << flush;
            format_to(cout, "BLOCKS start_mb = {1}, length_mb = {2}\n",
                      mbxy, num_blocks) << flush;
#endif

            futures.emplace_back(
                thread_pool.Submit(decode_blks, bitstream, blocks.second,
                                   std::cref(jpg_ctx), mbxy, num_blocks));
            curr_rst = blocks.first;
            mbxy += num_blocks;

            bitstream += blocks.second + 2;
            length -= blocks.second + 2;
            if (0xFF == blocks.first)
            {
                cout << "EOI reached\n";
            }
        }

        for (const auto &f : futures)
        {
            f.wait();
        }
    }

    std::ofstream ofs(out_filename, std::ios::binary);
    if (false == ofs.is_open())
    {
        throw system_error(error_code(), "cannot open output file");
    }

    int width = min(jpg_ctx.width, 1920);
    int height = min(jpg_ctx.height, 1080);
    if (vm.count("dump-width"))
    {
        width = vm["dump-width"].as<int>();
    }
    if (vm.count("dump-height"))
    {
        height = vm["dump-height"].as<int>();
    }

    int offset_w = 0;
    int offset_h = 0;
    if (vm.count("ow"))
    {
        offset_w = vm["ow"].as<int>() & ~1;
    }
    if (vm.count("oh"))
    {
        offset_h = vm["oh"].as<int>() & ~1;
    }

    if (offset_w + width > jpg_ctx.width)
    {
        throw invalid_argument("dumped region exceeds width");
    }
    if (offset_h + height > jpg_ctx.height)
    {
        throw invalid_argument("dumped region exceeds height");
    }

    for (int i = 0; i < jpg_ctx.ncomp; ++i)
    {
        component &c = jpg_ctx.comp[i];
        const int scale_w = jpg_ctx.comp[0].ssx / c.ssx;
        const int scale_h = jpg_ctx.comp[0].ssy / c.ssy;
        for (int j = 0; j < (height + scale_h - 1) / scale_h; ++j)
        {
            const int start_offset = (offset_h / scale_h + j) * c.stride +
                                     offset_w / scale_w;
            ofs.write(reinterpret_cast<const char *>(&c.pixels[start_offset]),
                      (width + scale_w - 1) / scale_w);
        }
    }
    ofs.close();
}


#ifndef CORE_ALGEBRA_ENCVEC_HPP
#define CORE_ALGEBRA_ENCVEC_HPP
#include "core/coda.hpp"
#include <NTL/vec_ZZ.h>
#include <iosfwd>
#include <vector>
typedef NTL::vec_ZZ Vector;
double norm(const Vector &vec);
namespace core {
class EncVec {
public:
    EncVec(core::pk_ptr pk);

    EncVec(const EncVec &oth);

    EncVec& operator=(const EncVec &oth);

	EncVec() {}

    ~EncVec() {}

    EncVec& pack(const Vector &vec);

	bool unpack(Vector &result,
				core::sk_ptr sk,
				bool negate = false) const;

    EncVec& add(const EncVec &c);

    EncVec& add(const Vector &c);

	EncVec& sub(const EncVec &c);

	EncVec& sub(const Vector &c);

    EncVec& mul(const Vector &c);

    EncVec& mul(const NTL::ZZ &c);

    EncVec& mul(const long &c);

    // equals to lowLevelMul + reLinearize
    EncVec& mul(const EncVec &oth);

    EncVec& lowLevelMul(const EncVec &oth);

	EncVec& negate();

    EncVec& reLinearize();

    EncVec replicate(long i) const;

    EncVec replicate(long i, long width) const;

    std::vector<EncVec> replicateAll() const;

    std::vector<EncVec> replicateAll(long width) const;

    long length() const;

    bool dump(std::ostream &out) const;

    bool restore(std::istream &in);

private:
	class Imp;
    friend class Imp;
	std::shared_ptr<Imp> imp_;
};
} // namespace core
#endif // CORE_ALGEBRA_ENCVEC_HPP


#include <cctype>
#include <type_traits>
#include <iostream>
#include <boost/timer/timer.hpp>
#include <boost/program_options.hpp>
#include <sys/time.h>
#include <cctype>
#include <random>
#include <iomanip>
#include <type_traits>
#include <boost/timer/timer.hpp>
#include <boost/tr1/random.hpp>
#include <boost/format.hpp>
#include <boost/program_options.hpp>

#include "kgraph.h"
#include "kgraph-data.h"

using namespace boost::timer;
using namespace std;
using namespace boost;
using namespace kgraph;
namespace po = boost::program_options;

#ifndef KGRAPH_VALUE_TYPE
#define KGRAPH_VALUE_TYPE float
#endif

typedef KGRAPH_VALUE_TYPE value_type;

int main(int argc, char *argv[]){
    vector<string> params;
    string input_data_path;
    string index_NNList_path;
    string index_DPG_path;
    unsigned L;

    po::options_description desc_visible("General options");
    desc_visible.add_options()
    ("help,h", "produce help message.")
    ("version,v", "print version number.")
    ("data", po::value(&input_data_path), "input data path")
    ("index", po::value(&index_NNList_path), "NNList index path")
    ("output", po::value(&index_DPG_path), "DPG index path")
    (",L", po::value(&L)->default_value(default_K), "")
    ;

    po::options_description desc("Allowed options");
    desc.add(desc_visible);

    po::positional_options_description p;
    p.add("data", 1);
    p.add("index", 1);
    p.add("output", 1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
    po::notify(vm);

    if (vm.count("help") || vm.count("data") == 0 || vm.count("index") == 0 || vm.count("output") == 0) {
        cout << "DPG_diverse <data> <K-NN graph index> <diversified index> " << endl;
        cout << desc_visible << endl;
        return 0;
    }

    cerr << L << " : L value" << endl;

    Matrix<value_type> data;
    data.load_lshkit(input_data_path.c_str());
    MatrixOracle<value_type, metric::l2sqr> oracle(data);

    KGraph *kgraph = KGraph::create();
    if (index_NNList_path.size() >0 )
    {
        kgraph->load(index_NNList_path.c_str());
    }

    boost::timer::auto_cpu_timer timer;

    // start the diversification ?? ( seperately )
    // add the diversification part here
    // cerr << "Start the diversification process... (angular dissimilarity )" << endl << endl;
    // kgraph->remove_near_edges(oracle, L); // here not knn's k, L is the length of NN list ( note that the true NN list length might be smaller than L)

    cerr << "Start the diversification process... ( counting cuts )" << endl << endl;
    kgraph->diversify_by_cut(oracle, L); // here not knn's k, L is the length of NN list ( note that the true NN list length might be smaller than L)

    // reverse the edges here
    cerr << "Add reverse edges ..." << endl << endl;
    kgraph->add_backward_edges();

    float time = timer.elapsed().wall / 1e9;

    if ( index_DPG_path.size() > 0 )
    kgraph->save( index_DPG_path.c_str());

    cout << time << " # diversification time" << endl;

    delete kgraph;

}


#include <iostream>
#include <armadillo>
#include <mpi.h>
#include <omp.h>
#include <mkl.h>

#include "class_data.hpp"
#include "class_approximation.hpp"
#include "class_partition.hpp"
#include "evaluate_covariance.hpp"
#include "constants.hpp"
#include <string.h>

/////////////////// Below are private member functions /////////////////

void Approximation::prior_allocate_memory()
{
    //Allocate memory for RChol
    RChol = new mat [partition->nRegionsInTotal];

    //Allocate memory for wTilde (first dimension: partition.nKnots, second dimesnion: partition.nLvs-1)
    wTilde = new mat [partition->nRegionsInTotal];

    //Allocate memory for ATilde (first dimension: partition.nKnots, second dimesnion: partition.nKnots, third dimension or the number of slices: (partition.nLvs-1)*partition.nLvs/2)
    ATilde = new cube [partition->nRegionsInTotal];

    //Assign paramter values if predicting
    if(CALCULATION_MODE == "prediction")
    {
        posteriorPredictionMean = new vec [partition->nRegionsAtEachLevel[NUM_LEVELS_M-1]];
        posteriorPredictionVariance = new vec [partition->nRegionsAtEachLevel[NUM_LEVELS_M-1]];
        BTilde = new double** [partition->nRegionsAtEachLevel[NUM_LEVELS_M-1]];
    }
}

void Approximation::get_all_ancestors(unsigned long* ancestorArray, unsigned long currentRegion, const unsigned long &nAncestors)
{
    for(unsigned long iAncestor = 0; iAncestor < nAncestors; iAncestor++)
    {
        currentRegion = (currentRegion-1)/NUM_PARTITIONS_J;
        ancestorArray[nAncestors-iAncestor-1] = currentRegion;
    }
}

//Update: Changed the column-row for KcW and covariance matrix. Need to modify prediction routines accordingly
void Approximation::get_conditional_covariance_matrix(const int &ancestorLevel, double **ancestorNow, double **ancestorBefore, double* nowBefore, const int &numKnotsNow, const int &numKnotsBefore, const int &numKnotsAncestor)
{
    for(int iLevel = 0; iLevel < ancestorLevel; iLevel++)
        cblas_dgemm(CblasColMajor,CblasTrans,CblasNoTrans,numKnotsBefore,numKnotsNow,numKnotsAncestor,-1.0,ancestorBefore[iLevel],numKnotsAncestor,ancestorNow[iLevel],numKnotsAncestor,1.0,nowBefore,numKnotsBefore);
}

void Approximation::get_wTilde_and_ATilde_in_prior(const int &nKnots, const int &currentLevel, const unsigned long &iRegion, vec *Sicy, mat **SicB)
{
    cube* ATildeCurrent;

    if(SAVE_TO_DISK_FLAG)
    {
        ATildeCurrent = new cube;
        (*ATildeCurrent).set_size(nKnots,nKnots,(NUM_LEVELS_M-1)*NUM_LEVELS_M/2);
    }

    unsigned long count = 0;
    for(int jLevel = 0; jLevel < currentLevel; jLevel++)
    {
        mat tmp = (*SicB[jLevel]).t();
        wTilde[iRegion].unsafe_col(jLevel) = tmp*(*Sicy);
        for(int kLevelAfterjLevel = jLevel; kLevelAfterjLevel < currentLevel; kLevelAfterjLevel++)
        {
            if(SAVE_TO_DISK_FLAG)
                (*ATildeCurrent).slice(count++) = tmp*(*SicB[kLevelAfterjLevel]);
            else
                ATilde[iRegion].slice(count++) = tmp*(*SicB[kLevelAfterjLevel]);
        }
    }

    if(SAVE_TO_DISK_FLAG)
    {
        std::string fileName=TMP_DIRECTORY+"/"+std::to_string(iRegion)+".bin";
        ATildeCurrent->save(fileName,arma_binary);
        delete ATildeCurrent;
    }
}

void Approximation::loop_regions_before_finest_level_in_prior(double ***KCholTimesw)
{
    int maxOpenMPThreads=omp_get_max_threads();

    unsigned long **ancestorArray = new unsigned long* [maxOpenMPThreads];
    for(int i = 0; i < maxOpenMPThreads; i++) ancestorArray[i] = new unsigned long [NUM_LEVELS_M-2];

    //Allocate memory for the current region of KCholTimesw
    for(int currentLevel = 0; currentLevel < NUM_LEVELS_M-1; currentLevel++)
    {
        for(unsigned long iRegion = REGION_START[currentLevel]; iRegion < REGION_END[currentLevel] + 1; iRegion++)
        {
            KCholTimesw[iRegion] = new double* [currentLevel];
            for(int jLevel = 0; jLevel < currentLevel; jLevel++)
                KCholTimesw[iRegion][jLevel]= new double [partition->nKnots*partition->nKnots];

            RChol[iRegion].set_size(partition->nKnots,partition->nKnots);
        }
    }

    //Auxilliary variables for matrix computations
    char L='L', N='N';
    int nKnots = partition->nKnots;

    for(int currentLevel = 0; currentLevel < NUM_LEVELS_M-1; currentLevel++)
    {
        #pragma omp parallel for num_threads(maxOpenMPThreads) schedule(dynamic,1)
        for(unsigned long iRegion = REGION_START[currentLevel]; iRegion < REGION_END[currentLevel] + 1; iRegion++)
        {
            int rankOpenMP = omp_get_thread_num();
            int status;
            //Find the indices of all the ancestors

            get_all_ancestors(ancestorArray[rankOpenMP], iRegion, currentLevel);

            //Loop for all ancestors to get the conditional cross-covariance matrix
            for(int jLevel = 0; jLevel < currentLevel; jLevel++)
            {
                evaluate_cross_covariance(KCholTimesw[iRegion][jLevel], partition->knotsX[iRegion], partition->knotsY[iRegion], partition->knotsX[ancestorArray[rankOpenMP][jLevel]], partition->knotsY[ancestorArray[rankOpenMP][jLevel]], partition->nKnots, partition->nKnots, sill, range, nugget);

                get_conditional_covariance_matrix(jLevel, KCholTimesw[iRegion], KCholTimesw[ancestorArray[rankOpenMP][jLevel]],KCholTimesw[iRegion][jLevel],nKnots,nKnots,nKnots);

                dtrtrs_(&L,&N,&N,&nKnots,&nKnots,(RChol[ancestorArray[rankOpenMP][jLevel]]).memptr(),&nKnots,KCholTimesw[iRegion][jLevel],&nKnots,&status);
            }

            //Get the conditional variance-covariance matrix of this region
            evaluate_variance_covariance(RChol[iRegion].memptr(), partition->knotsX[iRegion], partition->knotsY[iRegion], partition->nKnots, sill, range, nugget);

            get_conditional_covariance_matrix(currentLevel, KCholTimesw[iRegion], KCholTimesw[iRegion], RChol[iRegion].memptr(),nKnots,nKnots,nKnots);

            //dpotrf_(&L,&nKnots,(RChol[iRegion]).memptr(),&nKnots,&status);
            RChol[iRegion] = chol(RChol[iRegion],"lower");

        }

        timeval timeNow;
        gettimeofday(&timeNow, NULL);

        if(WORKER == 0) cout<<"Prior: Level "<<currentLevel+1<<" is complete. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n";
    }

    for(int i = 0; i < maxOpenMPThreads; i++) delete[] ancestorArray[i];
    delete[] ancestorArray; ancestorArray = NULL;
}

template<typename T> void Approximation::receive(T &object, double *tempMemory, int tag, unsigned long iRegion)
{
    std::set<unsigned long>::iterator workerToReceive = WORKERS_FOR_EACH_REGION[iRegion].begin();
    for(workerToReceive++; workerToReceive != WORKERS_FOR_EACH_REGION[iRegion].end(); workerToReceive++)
    {
        MPI_Recv(tempMemory,object.size(),MPI_DOUBLE,*workerToReceive,tag,WORLD,MPI_STATUS_IGNORE);
        for(unsigned long i = 0; i < object.size(); i++) object[i] += tempMemory[i];
    }
}

template<typename T> void Approximation::send(const T &object, int tag, unsigned long iRegion)
{
    MPI_Send(object.memptr(),object.size(),MPI_DOUBLE,*WORKERS_FOR_EACH_REGION[iRegion].begin(),tag,WORLD);
}

void Approximation::aggregate_A(mat &A, double *tempMemory, const unsigned long &indexA, const unsigned long &indexStart, const unsigned long &indexEnd, unsigned long iRegion, bool &supervisor, bool &synchronizeFlag)
{
    bool firstChild = true;

    for(unsigned long jChild = indexStart; jChild < indexEnd + 1; jChild++)
    {
        if( !WORKING_REGION_FLAG[jChild] )
        {
            synchronizeFlag = true;
            continue;
        }
        if(firstChild)
        {
            A = ATilde[jChild].slice(indexA);
            firstChild = false;
        }
        else
            A += ATilde[jChild].slice(indexA);
    }

    if(synchronizeFlag)
    {
        supervisor = WORKER == *WORKERS_FOR_EACH_REGION[iRegion].begin();
        //Check whether this worker is the master worker
        if(supervisor)
        {
            //Supervisor, to receive
            receive<mat>(A, tempMemory, MPI_TAG_MAT, iRegion);
        }
        else
        {
            //Not supervisor, to send
            send<mat>(A, MPI_TAG_MAT, iRegion);
        }
    }
}

void Approximation::aggregate_w_and_A(vec &w, mat &A, double *tempMemory, const int &jLevel, const unsigned long &indexA, const unsigned long &indexStart, const unsigned long &indexEnd, unsigned long iRegion, bool &supervisor, bool &synchronizeFlag)
{
    bool firstChild = true;
    for(unsigned long jChild = indexStart; jChild < indexEnd+1; jChild++)
    {
        if( !WORKING_REGION_FLAG[jChild] )
        {
            synchronizeFlag = true;
            continue;
        }
        if(firstChild)
        {
            w = wTilde[jChild].unsafe_col(jLevel);
            A = ATilde[jChild].slice(indexA);
            firstChild = false;
        }
        else
        {
            w += wTilde[jChild].unsafe_col(jLevel);
            A += ATilde[jChild].slice(indexA);
        }
    }

    if(synchronizeFlag)
    {
        //Check whether this worker is the supervisor
        supervisor = WORKER == *WORKERS_FOR_EACH_REGION[iRegion].begin();

        if(supervisor)
        {
            //Supervisor, to receive
            receive<vec>(w, tempMemory, MPI_TAG_VEC, iRegion);
            receive<mat>(A, tempMemory,  MPI_TAG_MAT, iRegion);
        }
        else
        {
            //Not supervisor, to send
            send<vec>(w, MPI_TAG_VEC, iRegion);
            send<mat>(A, MPI_TAG_MAT, iRegion);
        }
    }
}

void Approximation::get_wTilde_and_ATilde_in_posterior(vec &w, mat &A, double *tempMemory, const unsigned long &nKnots, cube &KCholTimesCurrentA, const vec &KCholTimesCurrentw, const unsigned long &iRegion, const int &currentLevel, bool &supervisor, bool &synchronizeFlag)
{
    if(currentLevel == 0) return;

    wTilde[iRegion].set_size(nKnots,currentLevel);
    ATilde[iRegion].set_size(nKnots,nKnots,(currentLevel+1)*currentLevel/2);

    unsigned long indexAChild = (currentLevel+1)*(currentLevel+2)/2-1;
    unsigned long indexACurrent = currentLevel*(currentLevel+1)/2;
    for(long jLevel = currentLevel-1; jLevel > -1; jLevel--)
    {
        aggregate_w_and_A(w, A, tempMemory, jLevel, --indexAChild, iRegion*NUM_PARTITIONS_J+1, iRegion*NUM_PARTITIONS_J+NUM_PARTITIONS_J, iRegion, supervisor, synchronizeFlag);

        if(supervisor)
        {
            KCholTimesCurrentA.slice(jLevel) = solve(trimatl(RChol[iRegion]),A.t());

            wTilde[iRegion].unsafe_col(jLevel) = w-KCholTimesCurrentA.slice(jLevel).t()*KCholTimesCurrentw;
        }

        for(long kLevelAfterjLevel = currentLevel-1; kLevelAfterjLevel > jLevel-1; kLevelAfterjLevel--)
        {
            aggregate_A(A, tempMemory, --indexAChild, iRegion*NUM_PARTITIONS_J+1, iRegion*NUM_PARTITIONS_J+NUM_PARTITIONS_J, iRegion, supervisor, synchronizeFlag);

            if(supervisor)
                ATilde[iRegion].slice(--indexACurrent) = A-KCholTimesCurrentA.slice(jLevel).t()*KCholTimesCurrentA.slice(kLevelAfterjLevel);
        }
    }
}

void Approximation::synchronizeIndicesRegionsForEachWorker()
{
    //Get the working region indices of this worker at the current level
    INDICES_REGIONS_AT_ONE_LEVEL_BELOW = INDICES_REGIONS_AT_CURRENT_LEVEL;
    INDICES_REGIONS_AT_CURRENT_LEVEL.clear();
    for(std::set<unsigned long>::iterator child = INDICES_REGIONS_AT_ONE_LEVEL_BELOW.begin(); child != INDICES_REGIONS_AT_ONE_LEVEL_BELOW.end(); child++)
    {
        unsigned long ancestor = (*child-1)/NUM_PARTITIONS_J;
        INDICES_REGIONS_AT_CURRENT_LEVEL.insert(ancestor);
        WORKING_REGION_FLAG[ancestor] = true;
    }

    if(INDICES_REGIONS_AT_CURRENT_LEVEL.size() > (unsigned long)INT_MAX)
    {
        cout<<"Program exits with an error: too many levels. It is required that the number of regions at the third finest level is smaller than the maximum value of type \"int\" "<<INT_MAX<<endl;
        exit(EXIT_FAILURE);
    }

    int *numWorkingRegionsEachWorker = new int [MPI_SIZE];
    int numWorkingRegionsThisWorker = INDICES_REGIONS_AT_CURRENT_LEVEL.size();

    unsigned long *indicesWorkingRegionsThisWorker = new unsigned long [numWorkingRegionsThisWorker];
    std::set<unsigned long>::iterator iter = INDICES_REGIONS_AT_CURRENT_LEVEL.begin();
    for(int i = 0; i < numWorkingRegionsThisWorker; i++)
    {
        indicesWorkingRegionsThisWorker[i] = *iter;
        iter++;
    }

    //Synchronize the number of working regions of each worker
    MPI_Allgather(&numWorkingRegionsThisWorker,1,MPI_INT,numWorkingRegionsEachWorker,1,MPI_INT,WORLD);

    //Get the max number of working regions among all workers
    int maxNumWorkingRegionsEachWorker = 0;
    for(int i = 0; i < MPI_SIZE; i++)
        if (maxNumWorkingRegionsEachWorker < numWorkingRegionsEachWorker[i]) maxNumWorkingRegionsEachWorker = numWorkingRegionsEachWorker[i];

    //Synchronize the working region indices of each worker
    unsigned long *indicesWorkingRegionsEachWorker = new unsigned long [(unsigned long)MPI_SIZE*maxNumWorkingRegionsEachWorker];
    int *offset = new int [MPI_SIZE];
    offset[0] = 0;
    for(int i = 1; i < MPI_SIZE; i++)
        offset[i] = offset [i-1] + maxNumWorkingRegionsEachWorker;

    MPI_Allgatherv(indicesWorkingRegionsThisWorker,numWorkingRegionsThisWorker,MPI_UNSIGNED_LONG,indicesWorkingRegionsEachWorker,numWorkingRegionsEachWorker,offset,MPI_UNSIGNED_LONG,WORLD);

    //Update WORKERS_FOR_EACH_REGION
    for(int iWorker = 0; iWorker < MPI_SIZE; iWorker++)
        for(int jIndex = offset[iWorker]; jIndex < offset[iWorker]+numWorkingRegionsEachWorker[iWorker]; jIndex++)
            WORKERS_FOR_EACH_REGION[indicesWorkingRegionsEachWorker[jIndex]].insert(iWorker);

    delete[] offset; offset = NULL;
    delete[] indicesWorkingRegionsEachWorker; indicesWorkingRegionsEachWorker = NULL;
    delete[] indicesWorkingRegionsThisWorker; indicesWorkingRegionsThisWorker = NULL;
    delete[] numWorkingRegionsEachWorker; numWorkingRegionsEachWorker = NULL;
}

void Approximation::free_wTilde_and_ATilde(const unsigned long &indexStartFreeingATilde, const unsigned long &indexEndFreeingATilde)
{
    //#pragma omp parallel for
    for(unsigned long i = indexStartFreeingATilde; i < indexEndFreeingATilde + 1; i++)
    {
        if( !WORKING_REGION_FLAG[i] ) continue;
        ATilde[i].reset();
        wTilde[i].reset();
    }
}

void Approximation::load_ATilde_from_disk(const unsigned long &indexChildrenStart, const unsigned long &indexChildrenEnd, bool secondLastLevel, unsigned long* nKnotsAtFinestLevel, unsigned long offset, unsigned long size1, unsigned long size2)
{
    for(unsigned long i = indexChildrenStart; i < indexChildrenEnd + 1; i++)
    {
        if( !WORKING_REGION_FLAG[i] ) continue;
        if( secondLastLevel && nKnotsAtFinestLevel[i-offset]==0)
        {
            ATilde[i].set_size(size1,size1,size2);
            ATilde[i].zeros();
            continue;
        }
        std::string fileName=TMP_DIRECTORY+"/"+std::to_string(i)+".bin";
        ATilde[i].load(fileName,arma_binary);
    }
}

/////////////////// Above are private member functions /////////////////

/////////////////// Below are public member functions /////////////////

Approximation::Approximation(const double &sill, const double &range, const double &nugget)
{
    (*this).sill=sill;
    (*this).range=range;
    (*this).nugget=nugget;
    loglikelihood = 0;
}

void Approximation::create_prior()
{
    if(WORKER == 0)
    {
        gettimeofday(&timeNow, NULL);
        cout<<"===========================> Processor 1: calculating prior quantities starts. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n";
    }

    prior_allocate_memory();

    //Allocate memory for temporary variables storing K^{1/2}*W
    double ***KCholTimesw = new double** [partition->nRegionsInTotal];

    //Loop each region before the finest level
    loop_regions_before_finest_level_in_prior(KCholTimesw);

    //Loop each region in the finest level
    loop_regions_at_finest_level_in_prior(KCholTimesw);

    timeval timeNow;
    gettimeofday(&timeNow, NULL);

    if(WORKER == 0) cout<<"Prior: Level "<<NUM_LEVELS_M<<" is complete. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n";

    for(int currentLevel = 0; currentLevel < NUM_LEVELS_M-1; currentLevel++)
    {
        for(unsigned long iRegion = REGION_START[currentLevel]; iRegion < REGION_END[currentLevel] + 1; iRegion++)
        {
            for(int jLevel = 0; jLevel < currentLevel; jLevel++)
                delete[] KCholTimesw[iRegion][jLevel];
            delete[] KCholTimesw[iRegion];
        }
    }

    if(CALCULATION_MODE=="prediction")
    {
        for(unsigned long iRegion = REGION_START[NUM_LEVELS_M-1]; iRegion < REGION_END[NUM_LEVELS_M-1] + 1; iRegion++)
        {
            if(partition->nKnotsAtFinestLevel[iRegion-partition->nRegionsInTotal+partition->nRegionsAtEachLevel[NUM_LEVELS_M-1]] == 0) continue;

            for(int jLevel = 0; jLevel < NUM_LEVELS_M - 1; jLevel++)
                delete[] KCholTimesw[iRegion][jLevel];
            delete[] KCholTimesw[iRegion];
        }
    }

    delete [] KCholTimesw; KCholTimesw=NULL;

    if(WORKER == 0)
    {
        gettimeofday(&timeNow, NULL);
        cout<<"===========================> Processor 1: calculating prior quantities is complete. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n\n";
    }
}

void Approximation::likelihood()
{
	create_prior();
	posterior_inference();
}

void Approximation::dump_prediction_result()
{
    if(WORKER == 0)
    {
        gettimeofday(&timeNow, NULL);
        cout<<"===========================> Processor 1: dumping prediction results starts. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n";
    }

    std::ofstream file;
    char fileNameThisWorker[255];
    if(MPI_SIZE>1)
        sprintf(fileNameThisWorker,"%s%d",PREDICTION_RESULTS_FILE_NAME.c_str(),WORKER);
    else
        sprintf(fileNameThisWorker,"%s",PREDICTION_RESULTS_FILE_NAME.c_str());

    file.open(fileNameThisWorker, ios::binary);

    unsigned long offset = partition->nRegionsAtEachLevel[NUM_LEVELS_M-1]-partition->nRegionsInTotal;
    if(file.is_open())
    {
        //Write the number of prediction locations
        unsigned long n = 0;
        for(unsigned long indexRegionAtThisLevel = REGION_START[NUM_LEVELS_M-1]+offset; indexRegionAtThisLevel < REGION_END[NUM_LEVELS_M-1]+1+offset; indexRegionAtThisLevel++)
            n += partition->nPredictionsAtFinestLevel[indexRegionAtThisLevel];
        file.write((char*)&n,sizeof(unsigned long));

        //Write the prediction longtitude
        for(unsigned long indexRegionAtThisLevel = REGION_START[NUM_LEVELS_M-1]+offset; indexRegionAtThisLevel < REGION_END[NUM_LEVELS_M-1]+1+offset; indexRegionAtThisLevel++)
        {
            if(partition->nPredictionsAtFinestLevel[indexRegionAtThisLevel]==0) continue;
            file.write((char*)partition->predictionX[indexRegionAtThisLevel],sizeof(double)*partition->nPredictionsAtFinestLevel[indexRegionAtThisLevel]);
        }

        //Write the prediction lattitude
        for(unsigned long indexRegionAtThisLevel = REGION_START[NUM_LEVELS_M-1]+offset; indexRegionAtThisLevel < REGION_END[NUM_LEVELS_M-1]+1+offset; indexRegionAtThisLevel++)
        {
            if(partition->nPredictionsAtFinestLevel[indexRegionAtThisLevel]==0) continue;
            file.write((char*)partition->predictionY[indexRegionAtThisLevel],sizeof(double)*partition->nPredictionsAtFinestLevel[indexRegionAtThisLevel]);
        }

        //Write the prediction mean
        for(unsigned long indexRegionAtThisLevel = REGION_START[NUM_LEVELS_M-1]+offset; indexRegionAtThisLevel < REGION_END[NUM_LEVELS_M-1]+1+offset; indexRegionAtThisLevel++)
            for(unsigned long jPrediction = 0; jPrediction < partition->nPredictionsAtFinestLevel[indexRegionAtThisLevel]; jPrediction++)
            {
                vec designVec(3);
                designVec[1] = partition->predictionX[indexRegionAtThisLevel][jPrediction];
                designVec[2] = partition->predictionY[indexRegionAtThisLevel][jPrediction];
                designVec[0] = 1.0;
                double mean = posteriorPredictionMean[indexRegionAtThisLevel][jPrediction]+dot(designVec,data->coefficients);
                file.write((char*)&mean,sizeof(double));
            }

        //Write the prediction variance
        for(unsigned long indexRegionAtThisLevel = REGION_START[NUM_LEVELS_M-1]+offset; indexRegionAtThisLevel < REGION_END[NUM_LEVELS_M-1]+1+offset; indexRegionAtThisLevel++)
            for(unsigned long jPrediction = 0; jPrediction < partition->nPredictionsAtFinestLevel[indexRegionAtThisLevel]; jPrediction++)
                file.write((char*)&posteriorPredictionVariance[indexRegionAtThisLevel][jPrediction],sizeof(double));

        //Close the data file
        file.close();
    }else
    {
        cout<<"Program exits with an error: fail to open the output file "<<PREDICTION_RESULTS_FILE_NAME<<endl;
        exit(EXIT_FAILURE);
    }

    if(WORKER == 0)
    {
        gettimeofday(&timeNow, NULL);
        cout<<"===========================> Processor 1: dumping prediction results is complete. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n\n";
    }
}

Approximation::~Approximation()
{
    delete[] RChol;

    delete[] wTilde;
    delete[] ATilde;

    delete[] KCholTimesCurrentw;
    delete[] KCholTimesCurrentA;

    if(CALCULATION_MODE == "prediction")
    {
        delete[] BTilde;

        delete[] posteriorPredictionMean;
        delete[] posteriorPredictionVariance;
    }
    //cout<<"Approximation is deleted\n";
}