// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2012-2014 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)

#ifndef BOOST_GEOMETRY_STRATEGIES_AGNOSTIC_BUFFER_DISTANCE_SYMMETRIC_HPP
#define BOOST_GEOMETRY_STRATEGIES_AGNOSTIC_BUFFER_DISTANCE_SYMMETRIC_HPP

#include <boost/core/ignore_unused.hpp>

#include <boost/geometry/strategies/buffer.hpp>
#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace buffer
{

/*!
\brief Let the buffer algorithm create buffers with same distances
\ingroup strategies
\tparam NumericType \tparam_numeric
\details This strategy can be used as DistanceStrategy for the buffer algorithm.
    It can be applied for all geometries. It uses one distance for left and
    for right.
    If the distance is negative and used with a (multi)polygon or ring, the
    geometry will shrink (deflate) instead of expand (inflate).

\qbk{
[heading Example]
[buffer_distance_symmetric]
[heading Output]
[$img/strategies/buffer_distance_symmetric.png]
[heading See also]
\* [link geometry.reference.algorithms.buffer.buffer_7_with_strategies buffer (with strategies)]
\* [link geometry.reference.strategies.strategy_buffer_distance_asymmetric distance_asymmetric]
}
 */
template<typename NumericType>
class distance_symmetric
{
public :
    //! \brief Constructs the strategy, a distance must be specified
    //! \param distance The distance (or radius) of the buffer
    explicit inline distance_symmetric(NumericType const& distance)
        : m_distance(distance)
    {}

#ifndef DOXYGEN_SHOULD_SKIP_THIS
    //! Returns the distance-value
    template <typename Point>
    inline NumericType apply(Point const& , Point const& ,
                buffer_side_selector )  const
    {
        return negative() ? geometry::math::abs(m_distance) : m_distance;
    }

    //! Used internally, returns -1 for deflate, 1 for inflate
    inline int factor() const
    {
        return negative() ? -1 : 1;
    }

    //! Returns true if distance is negative
    inline bool negative() const
    {
        return m_distance < 0;
    }

    //! Returns the max distance distance up to the buffer will reach
    template <typename JoinStrategy, typename EndStrategy>
    inline NumericType max_distance(JoinStrategy const& join_strategy,
            EndStrategy const& end_strategy) const
    {
        boost::ignore_unused(join_strategy, end_strategy);

        NumericType const dist = geometry::math::abs(m_distance);
        return (std::max)(join_strategy.max_distance(dist),
                          end_strategy.max_distance(dist));
    }

    //! Returns the distance at which the input is simplified before the buffer process
    inline NumericType simplify_distance() const
    {
        return geometry::math::abs(m_distance) / 1000.0;
    }
#endif // DOXYGEN_SHOULD_SKIP_THIS

private :
    NumericType m_distance;
};

}} // namespace strategy::buffer

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_AGNOSTIC_BUFFER_DISTANCE_SYMMETRIC_HPP


/*
   Copyright 2015 Ruben Moreno Montoliu <ruben3d at gmail dot com>

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

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

//
// RenderTask.cpp - Created on 2009.01.15
//

#include <boost/thread.hpp>
#include "RenderTask.h"
#include "Raytracer.h"
#include "RenderProgressCallback.h"
#include "Bucket.h"
#include "Scene.h"
#include "BucketOrder.h"
#include "Camera.h"
#include "CameraMount.h"
#include "Random.h"
#include "FrameBuffer.h"

#define GAMMA(color, gamma) (pow((color),1.0/(gamma)))

boost::mutex mutexCallback;
boost::mutex mutexBuckets;

void RenderTask::operator()(const unsigned int depth, RenderProgressCallback* callback)
{
	double pixelWidth = 1.0/m_frameBuffer->getWidth();
	double pixelHeight = 1.0/m_frameBuffer->getHeight();

	Raytracer* rt = new Raytracer(m_scene);

	mutexBuckets.lock();
		Bucket bucket = m_order->getNextBucket();
	mutexBuckets.unlock();

	while (bucket != Bucket::INVALID)
	{
		if (callback)
		{
			mutexCallback.lock();
				callback->onBucketStart(bucket);
			mutexCallback.unlock();
		}

		for (unsigned int y=bucket.y0; y<=bucket.yd; y++)
		{
		for (unsigned int x=bucket.x0; x<=bucket.xd; x++)
		{
			Color rawColor(0,0,0);

			Camera* camera = m_scene->getCameraMount()->getCamera();
			double normX = XScreenToViewPlane(x);
			double normY = YScreenToViewPlane(y);

			unsigned int haltonSeq = 0/*Random::Instance().generate(256)*/;
			for (unsigned int i=1; i<=m_aaSamples; i++)
			{
				haltonSeq++;
				std::vector<Ray> rays = camera->makeSampleRays(
					normX+Random::Instance().haltonSeq(haltonSeq,2)*pixelWidth-pixelWidth*0.5,
					normY+Random::Instance().haltonSeq(haltonSeq,3)*pixelHeight-pixelHeight*0.5);
				for (unsigned int r=0; r<rays.size(); r++)
				{
					//m_primaryRaysCount++;
					rays[r] = m_scene->getCameraMount()->T(rays[r]);	// Local to World
					double distance;
					rawColor += rt->traceRay(rays[r], depth, distance) * (1.0/(double)rays.size());
				}
			}
			rawColor.r /= (double)m_aaSamples;
			rawColor.g /= (double)m_aaSamples;
			rawColor.b /= (double)m_aaSamples;

			rawColor.r = GAMMA(rawColor.r, 2.2);
			rawColor.g = GAMMA(rawColor.g, 2.2);
			rawColor.b = GAMMA(rawColor.b, 2.2);
			m_frameBuffer->setColorAt(x,y, rawColor);
		}
		}

		if (callback)
		{
			mutexCallback.lock();
				callback->onBucketEnd(bucket);
			mutexCallback.unlock();
		}

		mutexBuckets.lock();
			bucket = m_order->getNextBucket();
		mutexBuckets.unlock();
	}

	delete rt;
}

double RenderTask::XScreenToViewPlane(const double x) const
{
	double width = m_frameBuffer->getWidth();
	return ((x+0.5) - (double)width / 2.0) / (double)width;
}

double RenderTask::YScreenToViewPlane(const double y) const
{
	double height = m_frameBuffer->getHeight();
	return (((double)(height-1) - (y+0.5)) - (double)height / 2.0) /
			(double)height;
}


// Licensed to the Apache Software Foundation (ASF) under one
// or more contributor license agreements.  See the NOTICE file
// distributed with this work for additional information
// regarding copyright ownership.  The ASF licenses this file
// to you under the Apache License, Version 2.0 (the
// "License"); you may not use this file except in compliance
// with the License.  You may obtain a copy of the License at
//
//   http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing,
// software distributed under the License is distributed on an
// "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
// KIND, either express or implied.  See the License for the
// specific language governing permissions and limitations
// under the License.

#include <iostream>
#include <vector>

#include "cutil.hpp"
#include "methods.hpp"
#include "util.hpp"

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <boost/regex.hpp>

namespace po = boost::program_options;
namespace fs = boost::filesystem;

po::variables_map parse_cmdargs(int argc, char const* argv[])
{
    po::variables_map vm;
    po::options_description desc("Allowed options");
    // clang-format off
    desc.add_options()
        ("help,h", "produce help message")
        ("text,t", "text input (default is uint32_t binary)")
        ("input,i",po::value<std::string>()->required(), "the input dir");
    // clang-format on
    try {
        po::store(po::parse_command_line(argc, argv, desc), vm);
        if (vm.count("help")) {
            std::cout << desc << "\n";
            exit(EXIT_SUCCESS);
        }
        po::notify(vm);
    } catch (const po::required_option& e) {
        std::cout << desc;
        std::cerr << "Missing required option: " << e.what() << std::endl;
        exit(EXIT_FAILURE);
    } catch (po::error& e) {
        std::cout << desc;
        std::cerr << "Error parsing cmdargs: " << e.what() << std::endl;
        exit(EXIT_FAILURE);
    }

    return vm;
}

template <uint32_t fidelity> uint32_t compute_bucket(uint32_t x)
{
    const uint32_t radix = 8;
    uint32_t radix_mask = ((1 << radix) - 1);
    size_t offset = 0;
    size_t thres = 1 << (fidelity + radix - 1);
    while (x >= thres) {
        auto digit = x & radix_mask;
        x = x >> radix;
        offset = offset + (1 << (fidelity - 1)) * radix_mask;
    }
    return x + offset;
}

double kl_divergance(std::vector<double>& P, std::vector<double>& Q)
{
    double kl = 0.0;
    for (size_t i = 0; i < P.size(); i++) {
        if (P[i] != 0.0 && Q[i] != 0.0)
            kl += P[i] * log2(Q[i] / P[i]);
    }
    return -kl;
}

template <uint32_t fidelity>
void compute_skew(std::vector<uint32_t>& input, std::string name)
{
    std::vector<uint32_t> bucket_map(1 << 27);
    for (size_t i = 0; i < bucket_map.size(); i++) {
        bucket_map[i] = compute_bucket<fidelity>(i);
    }
    std::vector<uint32_t> bucket_sizes;
    std::vector<uint32_t> bucket_min;
    std::vector<uint32_t> bucket_max;
    uint32_t prev = bucket_map[0];
    bucket_min.push_back(0);
    auto cur_size = 1;
    for (size_t i = 1; i < bucket_map.size(); i++) {
        auto cur = bucket_map[i];
        if (cur != prev) {
            bucket_max.push_back(i - 1);
            bucket_min.push_back(i);
            bucket_sizes.push_back(cur_size);
            cur_size = 0;
        }
        cur_size++;
        prev = cur;
    }
    if (cur_size) {
        bucket_max.push_back(bucket_map.size() - 1);
        bucket_sizes.push_back(cur_size);
    }

    std::vector<uint32_t> bucket_usage(bucket_sizes.size());
    for (size_t i = 0; i < input.size(); i++) {
        auto bucket_id = compute_bucket<fidelity>(input[i]);
        bucket_usage[bucket_id]++;
    }

    std::vector<std::vector<double>> real_dists(bucket_sizes.size());
    for (size_t i = 0; i < input.size(); i++) {
        auto bucket_id = compute_bucket<fidelity>(input[i]);
        if (real_dists[bucket_id].size() == 0)
            real_dists[bucket_id].resize(bucket_sizes[bucket_id]);
        auto bucket_offset = input[i] - bucket_min[bucket_id];
        real_dists[bucket_id][bucket_offset] += 1.0;
    }
    size_t usage_cum_sum = 0;
    for (size_t i = 0; i < bucket_sizes.size(); i++) {
        if (bucket_usage[i] != 0) {
            double bits_uniform = -log2(double(1.0) / double(bucket_sizes[i]));
            double bits_real = 0.0;
            for (size_t j = 0; j < real_dists[i].size(); j++) {
                if (real_dists[i][j] != 0.0) {
                    double bits
                        = -log2(real_dists[i][j] / double(bucket_usage[i]));
                    bits_real += real_dists[i][j] * bits;
                }
            }
            bits_real /= double(bucket_usage[i]);
            usage_cum_sum += bucket_usage[i];
            std::cout << name << ";" << i << ";" << fidelity << ";"
                      << bucket_min[i] << ";" << bucket_max[i] << ";"
                      << bucket_sizes[i] << ";" << bucket_usage[i] << ";"
                      << usage_cum_sum << ";" << input.size() << ";"
                      << (bits_uniform - bits_real) << std::endl;
        }
    }
}

int main(int argc, char const* argv[])
{
    auto cmdargs = parse_cmdargs(argc, argv);
    auto input_dir = cmdargs["input"].as<std::string>();

    boost::regex input_file_filter(".*\\.u32");
    if (cmdargs.count("text")) {
        input_file_filter = boost::regex(".*\\.txt");
    }

    // single file also works!
    boost::filesystem::path p(input_dir);
    if (boost::filesystem::is_regular_file(p)) {
        input_file_filter = boost::regex(p.filename().string());
        input_dir = p.parent_path().string();
    }

    boost::filesystem::directory_iterator
        end_itr; // Default ctor yields past-the-end
    for (boost::filesystem::directory_iterator i(input_dir); i != end_itr;
         ++i) {
        if (!boost::filesystem::is_regular_file(i->status()))
            continue;
        boost::smatch what;
        if (!boost::regex_match(
                i->path().filename().string(), what, input_file_filter))
            continue;

        std::string file_name = i->path().string();
        std::vector<uint32_t> input_u32s;
        if (cmdargs.count("text")) {
            input_u32s = read_file_text(file_name);
        } else {
            input_u32s = read_file_u32(file_name);
        }
        std::string short_name = i->path().stem().string();

        compute_skew<1>(input_u32s, short_name);
        compute_skew<2>(input_u32s, short_name);
        compute_skew<3>(input_u32s, short_name);
        compute_skew<4>(input_u32s, short_name);
        compute_skew<5>(input_u32s, short_name);
        compute_skew<6>(input_u32s, short_name);
    }

    return EXIT_SUCCESS;
}


#include <string>

#define BOOST_TEST_MODULE LatticeConstrTests

#include "DenseLattice.h"
#include "SparseLattice.h"
#include "MIAConfig.h"

#ifdef MIA_USE_HEADER_ONLY_TESTS
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif

constexpr int dim=5;

template<typename T>
void constructor_work(){
    typedef T Lat1;

    Lat1 test;
    BOOST_CHECK_EQUAL(test.height(),0);
    BOOST_CHECK_EQUAL(test.width(),0);
    BOOST_CHECK_EQUAL(test.depth(),0);
    BOOST_CHECK_EQUAL(test.data_end()-test.data_begin(),0);

    Lat1 test2(dim,dim,dim);
    BOOST_CHECK_EQUAL(test2.height(),dim);
    BOOST_CHECK_EQUAL(test2.width(),dim);
    BOOST_CHECK_EQUAL(test2.depth(),dim);

}

template<typename T>
void dense_constructor_work(){
    typedef T Lat1;
    constructor_work<T>();
    Lat1 test2(dim,dim,dim);

    BOOST_CHECK_EQUAL(test2.data_end()-test2.data_begin(),dim*dim*dim);
    for(auto it=test2.data_begin();it<test2.data_end();++it)
        BOOST_CHECK_EQUAL(*it,0);

    test2.randu(-5,5);
    Lat1 test3(test2.data_begin(),test2.height(),test2.width(),test2.depth());
    BOOST_CHECK_EQUAL(test3.height(),dim);
    BOOST_CHECK_EQUAL(test3.width(),dim);
    BOOST_CHECK_EQUAL(test3.depth(),dim);
    for(auto it=test2.data_begin(),it2=test3.data_begin();it<test2.data_end();++it,++it2)
        BOOST_CHECK_EQUAL(*it,*it2);

}

template<typename T>
void sparse_constructor_work(){
    typedef T Lat1;

    constructor_work<T>();

    Lat1 test;
    BOOST_CHECK_EQUAL(test.index_end()-test.index_begin(),0);

    Lat1 test2(dim,dim,dim);
    BOOST_CHECK_EQUAL(test2.data_end()-test2.data_begin(),0);
    BOOST_CHECK_EQUAL(test2.index_end()-test2.index_begin(),0);
    test2.eye();
    Lat1 test3(test2.data(),test2.indices(),test2.height(),test2.width(),test2.depth());

}

BOOST_AUTO_TEST_CASE( DenseLatticeConstrTests )
{

    dense_constructor_work<LibMIA::DenseLattice<float> >();
    dense_constructor_work<LibMIA::DenseLattice<double> >();
    dense_constructor_work<LibMIA::DenseLattice<int32_t> >();
    dense_constructor_work<LibMIA::DenseLattice<int64_t> >();
}

BOOST_AUTO_TEST_CASE( SparseLatticeConstrTests )
{

    sparse_constructor_work<LibMIA::SparseLattice<float> >();
    sparse_constructor_work<LibMIA::SparseLattice<double> >();
    sparse_constructor_work<LibMIA::SparseLattice<int32_t> >();
    sparse_constructor_work<LibMIA::SparseLattice<int64_t> >();
}


#include <boost/math/bindings/e_float.hpp>


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    HessianFactor.cpp
 * @author  Richard Roberts
 * @date    Dec 8, 2010
 */

#include <gtsam/linear/HessianFactor.h>

#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/linearExceptions.h>
#include <gtsam/base/cholesky.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/ThreadsafeException.h>
#include <gtsam/base/timing.h>

#include <boost/format.hpp>
#include <boost/make_shared.hpp>
#include <boost/tuple/tuple.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
#include <boost/assign/list_of.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/adaptor/map.hpp>
#include <boost/range/algorithm/copy.hpp>

#include <sstream>
#include <limits>

using namespace std;
using namespace boost::assign;
namespace br {
using namespace boost::range;
using namespace boost::adaptors;
}

namespace gtsam {

/* ************************************************************************* */
void HessianFactor::Allocate(const Scatter& scatter) {
  gttic(HessianFactor_Allocate);

  // Allocate with dimensions for each variable plus 1 at the end for the information vector
  const size_t n = scatter.size();
  keys_.resize(n);
  FastVector<DenseIndex> dims(n + 1);
  DenseIndex slot = 0;
  for(const SlotEntry& slotentry: scatter) {
    keys_[slot] = slotentry.key;
    dims[slot] = slotentry.dimension;
    ++slot;
  }
  dims.back() = 1;
  info_ = SymmetricBlockMatrix(dims);

}

//====================== MHHessianFactor::mhAllocate() =================
void MHHessianFactor::mhAllocate(const Scatter& scatter) {
  //gttic(HessianFactor_Allocate);

  // Allocate with dimensions for each variable plus 1 at the end for the information vector
  const size_t n = scatter.size();
  keys_.resize(n);

  //FastVector<DenseIndex> dims(n + 1);

  DenseIndex slot = 0;
  for(const SlotEntry& slotentry: scatter) {
    keys_[slot] = slotentry.key;
    ++slot;
  }

}

//====================== END MHHessianFactor::mhAllocate() =================

/* ************************************************************************* */
HessianFactor::HessianFactor(const Scatter& scatter) {
  Allocate(scatter);
}

/* ************************************************************************* */
HessianFactor::HessianFactor() :
    info_(cref_list_of<1>(1)) {
  assert(info_.rows() == 1);
  constantTerm() = 0.0;
}

/* ************************************************************************* */
HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f) :
    GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(G.cols())(1)) {
  if (G.rows() != G.cols() || G.rows() != g.size())
    throw invalid_argument(
        "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
  info_.setDiagonalBlock(0, G);
  linearTerm() = g;
  constantTerm() = f;
}

/* ************************************************************************* */
// error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu)
// where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g
HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) :
    GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(Sigma.cols())(1)) {
  if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size())
    throw invalid_argument(
        "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
  info_.setDiagonalBlock(0, Sigma.inverse()); // G
  linearTerm() = info_.diagonalBlock(0) * mu; // g
  constantTerm() = mu.dot(linearTerm().col(0)); // f
}

/* ************************************************************************* */
HessianFactor::HessianFactor(Key j1, Key j2, const Matrix& G11,
    const Matrix& G12, const Vector& g1, const Matrix& G22, const Vector& g2,
    double f) :
    GaussianFactor(cref_list_of<2>(j1)(j2)), info_(
        cref_list_of<3>(G11.cols())(G22.cols())(1)) {
  info_.setDiagonalBlock(0, G11);
  info_.setOffDiagonalBlock(0, 1, G12);
  info_.setDiagonalBlock(1, G22);
  linearTerm() << g1, g2;
  constantTerm() = f;
}

/* ************************************************************************* */
HessianFactor::HessianFactor(Key j1, Key j2, Key j3, const Matrix& G11,
    const Matrix& G12, const Matrix& G13, const Vector& g1, const Matrix& G22,
    const Matrix& G23, const Vector& g2, const Matrix& G33, const Vector& g3,
    double f) :
    GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), info_(
        cref_list_of<4>(G11.cols())(G22.cols())(G33.cols())(1)) {
  if (G11.rows() != G11.cols() || G11.rows() != G12.rows()
      || G11.rows() != G13.rows() || G11.rows() != g1.size()
      || G22.cols() != G12.cols() || G33.cols() != G13.cols()
      || G22.cols() != g2.size() || G33.cols() != g3.size())
    throw invalid_argument(
        "Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
  info_.setDiagonalBlock(0, G11);
  info_.setOffDiagonalBlock(0, 1, G12);
  info_.setOffDiagonalBlock(0, 2, G13);
  info_.setDiagonalBlock(1, G22);
  info_.setOffDiagonalBlock(1, 2, G23);
  info_.setDiagonalBlock(2, G33);
  linearTerm() << g1, g2, g3;
  constantTerm() = f;
}

/* ************************************************************************* */
namespace {
DenseIndex _getSizeHF(const Vector& m) {
  return m.size();
}
}

/* ************************************************************************* */
HessianFactor::HessianFactor(const std::vector<Key>& js,
    const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
    GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true) {
  // Get the number of variables
  size_t variable_count = js.size();

  // Verify the provided number of entries in the vectors are consistent
  if (gs.size() != variable_count
      || Gs.size() != (variable_count * (variable_count + 1)) / 2)
    throw invalid_argument(
        "Inconsistent number of entries between js, Gs, and gs in HessianFactor constructor.\nThe number of keys provided \
        in js must match the number of linear vector pieces in gs. The number of upper-diagonal blocks in Gs must be n*(n+1)/2");

  // Verify the dimensions of each provided matrix are consistent
  // Note: equations for calculating the indices derived from the "sum of an arithmetic sequence" formula
  for (size_t i = 0; i < variable_count; ++i) {
    DenseIndex block_size = gs[i].size();
    // Check rows
    for (size_t j = 0; j < variable_count - i; ++j) {
      size_t index = i * (2 * variable_count - i + 1) / 2 + j;
      if (Gs[index].rows() != block_size) {
        throw invalid_argument(
            "Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
      }
    }
    // Check cols
    for (size_t j = 0; j <= i; ++j) {
      size_t index = j * (2 * variable_count - j + 1) / 2 + (i - j);
      if (Gs[index].cols() != block_size) {
        throw invalid_argument(
            "Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
      }
    }
  }

  // Fill in the blocks
  size_t index = 0;
  for (size_t i = 0; i < variable_count; ++i) {
    for (size_t j = i; j < variable_count; ++j) {
      if (i == j) {
        info_.setDiagonalBlock(i, Gs[index]);
      } else {
        info_.setOffDiagonalBlock(i, j, Gs[index]);
      }
      index++;
    }
    info_.setOffDiagonalBlock(i, variable_count, gs[i]);
  }
  constantTerm() = f;
}

/* ************************************************************************* */
namespace {
void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info) {
  gttic(HessianFactor_fromJacobian);
  const SharedDiagonal& jfModel = jf.get_model();
  auto A = jf.matrixObject().full();
  if (jfModel) {
    if (jf.get_model()->isConstrained())
      throw invalid_argument(
          "Cannot construct HessianFactor from JacobianFactor with constrained noise model");

    auto D = (jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal();

    info.setFullMatrix(A.transpose() * D * A);
  } else {
    info.setFullMatrix(A.transpose() * A);
  }
}
}

/* ************************************************************************* */
HessianFactor::HessianFactor(const JacobianFactor& jf) :
    GaussianFactor(jf), info_(
        SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject())) {
  _FromJacobianHelper(jf, info_);
}

/* ************************************************************************* */
HessianFactor::HessianFactor(const GaussianFactor& gf) :
    GaussianFactor(gf) {
  // Copy the matrix data depending on what type of factor we're copying from
  if (const JacobianFactor* jf = dynamic_cast<const JacobianFactor*>(&gf)) {
    info_ = SymmetricBlockMatrix::LikeActiveViewOf(jf->matrixObject());
    _FromJacobianHelper(*jf, info_);
  } else if (const HessianFactor* hf = dynamic_cast<const HessianFactor*>(&gf)) {
    info_ = hf->info_;
  } else {
    throw std::invalid_argument(
        "In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor");
  }
}

/* ************************************************************************* */
HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
    boost::optional<const Scatter&> scatter) {
  gttic(HessianFactor_MergeConstructor);

  Allocate(scatter ? *scatter : Scatter(factors));

  // Form A' * A
  gttic(update);
  info_.setZero();
  for(const auto& factor: factors) {
    if (factor) {
      factor->updateHessian(keys_, &info_); //mhsiao: link back to JacobianFactor
    }
  }

  gttoc(update);
}

/* ************************************************************************* */
void HessianFactor::print(const std::string& s,
    const KeyFormatter& formatter) const {
  cout << s << "\n";
  cout << " keys: ";
  for (const_iterator key = begin(); key != end(); ++key)
    cout << formatter(*key) << "(" << getDim(key) << ") ";
  cout << "\n";
  gtsam::print(Matrix(info_.selfadjointView()),
      "Augmented information matrix: ");
}

/* ************************************************************************* */
bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
  const HessianFactor* rhs = dynamic_cast<const HessianFactor*>(&lf);
  if (!rhs || !Factor::equals(lf, tol))
    return false;
  return equal_with_abs_tol(augmentedInformation(), rhs->augmentedInformation(),
      tol);
}

/* ************************************************************************* */
Matrix HessianFactor::augmentedInformation() const {
  return info_.selfadjointView();
}

/* ************************************************************************* */
Eigen::SelfAdjointView<SymmetricBlockMatrix::constBlock, Eigen::Upper>
HessianFactor::informationView() const {
  return info_.selfadjointView(0, size());
}

/* ************************************************************************* */
Matrix HessianFactor::information() const {
  return informationView();
}

/* ************************************************************************* */
VectorValues HessianFactor::hessianDiagonal() const {
  VectorValues d;
  for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
    d.insert(keys_[j], info_.diagonal(j));
  }
  return d;
}

/* ************************************************************************* */
// Raw memory access version should be called in Regular Factors only currently
void HessianFactor::hessianDiagonal(double* d) const {
  throw std::runtime_error(
      "HessianFactor::hessianDiagonal raw memory access is allowed for Regular Factors only");
}

/* ************************************************************************* */
map<Key, Matrix> HessianFactor::hessianBlockDiagonal() const {
  map<Key, Matrix> blocks;
  // Loop over all variables
  for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
    // Get the diagonal block, and insert it
    blocks.emplace(keys_[j], info_.diagonalBlock(j));
  }
  return blocks;
}

/* ************************************************************************* */
Matrix HessianFactor::augmentedJacobian() const {
  return JacobianFactor(*this).augmentedJacobian();
}

/* ************************************************************************* */
std::pair<Matrix, Vector> HessianFactor::jacobian() const {
  return JacobianFactor(*this).jacobian();
}

/* ************************************************************************* */
double HessianFactor::error(const VectorValues& c) const {
  // error 0.5*(f - 2*x'*g + x'*G*x)
  const double f = constantTerm();
  if (empty()) {
    return 0.5 * f;
  }
  double xtg = 0, xGx = 0;
  // extract the relevant subset of the VectorValues
  // NOTE may not be as efficient
  const Vector x = c.vector(keys());
  xtg = x.dot(linearTerm().col(0));
  auto AtA = informationView();
  xGx = x.transpose() * AtA * x;
  return 0.5 * (f - 2.0 * xtg + xGx);
}

/* ************************************************************************* */
void HessianFactor::updateHessian(const FastVector<Key>& infoKeys,
                                  SymmetricBlockMatrix* info) const {
  gttic(updateHessian_HessianFactor);
  assert(info);
  // Apply updates to the upper triangle
  DenseIndex nrVariablesInThisFactor = size(), nrBlocksInInfo = info->nBlocks() - 1;
  vector<DenseIndex> slots(nrVariablesInThisFactor + 1);
  // Loop over this factor's blocks with indices (i,j)
  // For every block (i,j), we determine the block (I,J) in info.
  for (DenseIndex j = 0; j <= nrVariablesInThisFactor; ++j) {
    const bool rhs = (j == nrVariablesInThisFactor);
    const DenseIndex J = rhs ? nrBlocksInInfo : Slot(infoKeys, keys_[j]);
    slots[j] = J;
    for (DenseIndex i = 0; i <= j; ++i) {
      const DenseIndex I = slots[i];  // because i<=j, slots[i] is valid.

      if (i == j) {
        assert(I == J);
        info->updateDiagonalBlock(I, info_.diagonalBlock(i));
      } else {
        assert(i < j);
        assert(I != J);
        info->updateOffDiagonalBlock(I, J, info_.aboveDiagonalBlock(i, j));
      }
    }
  }

}
//============================== MHHF::mhUpdateHessian() ===================
//[MH-A]:
void MHHessianFactor::mhUpdateHessian(const FastVector<Key>& keys, HessList& hessian_list, const int& max_layer_idx) const {

  // max_layer_idx decides the recursive iter...
  const int this_layer_idx = resulting_layer_->getLayerIdx();
  const int layer_diff = max_layer_idx - this_layer_idx;

  std::vector<int> descendant_num_arr(hypoSize());

  size_t dna_idx = 0;
  const HypoList& hypo_list = getHypoList();
  for (HypoListCstIter it = hypo_list.begin(); it != hypo_list.end(); ++it) {
    descendant_num_arr[dna_idx] = (*it)->findDescendantNum(layer_diff);
    ++dna_idx;
  }

  HessListIter hit = hessian_list.begin();
  int dna_count = 0;
  for (HessListCstIter ihit = hessian_list_.begin(); ihit != hessian_list_.end(); ++ihit) { //should NOT ++hit here

    auto& num = descendant_num_arr[dna_count];
    for (int k = 0; k < num; ++k) {
      (*ihit).updateHessian(keys, &((*hit).info()));

      ++hit;
    }
    dna_count++;
  }

}
//============================== END MHHF::mhUpdateHessian() ===================

/* ************************************************************************* */
GaussianFactor::shared_ptr HessianFactor::negate() const {
  shared_ptr result = boost::make_shared<This>(*this);
  // Negate the information matrix of the result
  result->info_.negate();
  return result;
}

/* ************************************************************************* */
void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
    VectorValues& yvalues) const {

  // Create a vector of temporary y values, corresponding to rows i
  vector<Vector> y;
  y.reserve(size());
  for (const_iterator it = begin(); it != end(); it++)
    y.push_back(Vector::Zero(getDim(it)));

  // Accessing the VectorValues one by one is expensive
  // So we will loop over columns to access x only once per column
  // And fill the above temporary y values, to be added into yvalues after
  for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
    // xj is the input vector
    Vector xj = x.at(keys_[j]);
    DenseIndex i = 0;
    for (; i < j; ++i)
      y[i] += info_.aboveDiagonalBlock(i, j) * xj;

    // blocks on the diagonal are only half
    y[i] += info_.diagonalBlock(j) * xj;
    // for below diagonal, we take transpose block from upper triangular part
    for (i = j + 1; i < (DenseIndex) size(); ++i)
      y[i] += info_.aboveDiagonalBlock(j, i).transpose() * xj;
  }

  // copy to yvalues
  for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) {
    bool didNotExist;
    VectorValues::iterator it;
    boost::tie(it, didNotExist) = yvalues.tryInsert(keys_[i], Vector());
    if (didNotExist)
      it->second = alpha * y[i]; // init
    else
      it->second += alpha * y[i]; // add
  }
}

/* ************************************************************************* */
VectorValues HessianFactor::gradientAtZero() const {
  VectorValues g;
  size_t n = size();
  for (size_t j = 0; j < n; ++j)
    g.insert(keys_[j], -info_.aboveDiagonalBlock(j, n));
  return g;
}

/* ************************************************************************* */
// Raw memory access version should be called in Regular Factors only currently
void HessianFactor::gradientAtZero(double* d) const {
  throw std::runtime_error(
      "HessianFactor::gradientAtZero raw memory access is allowed for Regular Factors only");
}

/* ************************************************************************* */
Vector HessianFactor::gradient(Key key, const VectorValues& x) const {
  const Factor::const_iterator it_i = find(key);
  const DenseIndex I = std::distance(begin(), it_i);

  // Sum over G_ij*xj for all xj connecting to xi
  Vector b = Vector::Zero(x.at(key).size());
  for (Factor::const_iterator it_j = begin(); it_j != end(); ++it_j) {
    const DenseIndex J = std::distance(begin(), it_j);

    // Obtain Gij from the Hessian factor
    // Hessian factor only stores an upper triangular matrix, so be careful when i>j
    const Matrix Gij = info_.block(I, J);
    // Accumulate Gij*xj to gradf
    b += Gij * x.at(*it_j);
  }
  // Subtract the linear term gi
  b += -linearTerm(it_i);
  return b;
}

/* ************************************************************************* */
boost::shared_ptr<GaussianConditional> HessianFactor::eliminateCholesky(const Ordering& keys) {
  gttic(HessianFactor_eliminateCholesky);

  GaussianConditional::shared_ptr conditional;

  try {
    // Do dense elimination
    size_t nFrontals = keys.size();
    assert(nFrontals <= size());

    info_.choleskyPartial(nFrontals);

    // TODO(frank): pre-allocate GaussianConditional and write into it
    const VerticalBlockMatrix Ab = info_.split(nFrontals);
    conditional = boost::make_shared<GaussianConditional>(keys_, nFrontals, Ab);

    // Erase the eliminated keys in this factor
    keys_.erase(begin(), begin() + nFrontals);
  } catch (const CholeskyFailed& e) {
    throw IndeterminantLinearSystemException(keys.front());
  }

  // Return result
  return conditional;
}

/* ************************************************************************* */
VectorValues HessianFactor::solve() {
  gttic(HessianFactor_solve);

  // Do Cholesky Factorization
  const size_t n = size();
  assert(info_.nBlocks() == n + 1);
  info_.choleskyPartial(n);
  auto R = info_.triangularView(0, n);
  auto eta = linearTerm();

  // Solve
  const Vector solution = R.solve(eta);

  // Parse into VectorValues
  VectorValues delta;
  std::size_t offset = 0;
  for (DenseIndex j = 0; j < (DenseIndex)n; ++j) {
    const DenseIndex dim = info_.getDim(j);
    delta.insert(keys_[j], solution.segment(offset, dim));
    offset += dim;
  }

  return delta;
}

/* ************************************************************************* */
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> >
EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) {
  gttic(EliminateCholesky);

  // Build joint factor
  HessianFactor::shared_ptr jointFactor;
  try {
    Scatter scatter(factors, keys);
    jointFactor = boost::make_shared<HessianFactor>(factors, scatter);
  } catch (std::invalid_argument&) {
    throw InvalidDenseElimination(
        "EliminateCholesky was called with a request to eliminate variables that are not\n"
        "involved in the provided factors.");
  }

  // Do dense elimination
  auto conditional = jointFactor->eliminateCholesky(keys);

  // Return result
  return make_pair(conditional, jointFactor);
}

/* ************************************************************************* */
std::pair<boost::shared_ptr<GaussianConditional>,
    boost::shared_ptr<GaussianFactor> > EliminatePreferCholesky(
    const GaussianFactorGraph& factors, const Ordering& keys) {
  gttic(EliminatePreferCholesky);

  // If any JacobianFactors have constrained noise models, we have to convert
  // all factors to JacobianFactors.  Otherwise, we can convert all factors
  // to HessianFactors.  This is because QR can handle constrained noise
  // models but Cholesky cannot.
  if (hasConstraints(factors))
    return EliminateQR(factors, keys);
  else
    return EliminateCholesky(factors, keys);
}

//================================== MHEliminateCholesky() ===========================
//[MH-A]: convert JF/MHJF to HF/MHHF here... returns GaussianConditional and MHHessianFactor
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<MHHessianFactor> >
MHEliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) {

  //gttic(EliminateCholesky);

  // Build joint factor
  MHHessianFactor::shared_ptr jointFactor;
  try {

    Scatter scatter(factors, true, keys);

    //[MH-A]: [REFINE] constructor(): combine several MHJacobianFactor into one MHHessianFactor
    jointFactor = boost::make_shared<MHHessianFactor>(factors, scatter);

  } catch (std::invalid_argument&) {
    throw InvalidDenseElimination(
        "MHEliminateCholesky was called with a request to eliminate variables that are not involved in the provided factors.");
  }

  // Do dense elimination
  //[MH-A]: careful "auto"
  auto conditional = jointFactor->mhEliminateCholesky(keys); //MHHessianFactor::mhEliminateCholesky() returns GaussianConditional

  // Return result
  return make_pair(conditional, jointFactor);
  //
}

//================================== END MHEliminateCholesky() ============================

//
//========================== MHHessianFactor::mhEliminateCholesky() =========================
boost::shared_ptr<GaussianConditional> MHHessianFactor::mhEliminateCholesky(const Ordering& keys) {

  //gttic(HessianFactor_eliminateCholesky);

  GaussianConditional::shared_ptr conditional;

  try {

    // Do dense elimination
    size_t nFrontals = keys.size();
    assert(nFrontals <= size());

    std::list<VerticalBlockMatrix> Ab_list;
    for (HessListIter it = hessian_list_.begin(); it != hessian_list_.end(); ++it) {

      (it->info()).choleskyPartial(nFrontals);

      // TODO(frank): pre-allocate GaussianConditional and write into it
      Ab_list.push_back((it->info()).split(nFrontals));

    }
    //[MH-A]: [DONE] call MHJacobianFactor() as base constructor...

    conditional = boost::make_shared<GaussianConditional>(keys_, nFrontals, Ab_list, resulting_layer_);

    // Erase the eliminated keys in this factor
    keys_.erase(begin(), begin() + nFrontals);
    for (HessListIter it = hessian_list_.begin(); it != hessian_list_.end(); ++it) {
      it->keys().erase(it->begin(), it->begin() + nFrontals);
    }

  } catch (const CholeskyFailed& e) {
    throw IndeterminantLinearSystemException(keys.front());
  }

  // Return result
  return conditional;
}
//================================== END mhEliminateCholesky() ============================

//======================= HessianFactor(GaussianFactorGraph, Sactter) ==================
MHHessianFactor::MHHessianFactor(const GaussianFactorGraph& factors, boost::optional<const Scatter&> scatter) {
  //gttic(HessianFactor_MergeConstructor);

  //[MH-A]: try to copy this...
  mhAllocate(scatter ? *scatter : Scatter(factors, true));

  // Find the max_hypo_num
  int max_layer_idx = -1;
  boost::shared_ptr<GaussianFactor> max_fac_ptr;
  for(const auto& fac: factors) {
    if (fac) {
      const int tmp_layer_idx = fac->getHypoLayer()->getLayerIdx();

      if (tmp_layer_idx >= max_layer_idx) {
        max_layer_idx = tmp_layer_idx;
        max_fac_ptr = fac;
      }
    }
  }

  const HypoList& hypo_list = max_fac_ptr->getHypoList();

  hessian_list_.resize(hypo_list.size());

  for (HessListIter it = hessian_list_.begin(); it != hessian_list_.end(); ++it) {

    (*it).Allocate(scatter ? *scatter : Scatter(factors, true));
  }

  for(const auto& fac: factors) {
    //[MH-A]: find corresponding hypos in each factor and update corresponding hessian info (use recursive)

    if (fac) {
      //[MH-C]:
      if (fac->removeAccumulatedPruned()) { //[MH-opt]: work on cached factors only...
        // Some Factors NOT pruned correctly????
        //TODO: Do nothing for now...
      }

      fac->mhUpdateHessian(keys_, hessian_list_, max_layer_idx);
    }
  }

  resulting_layer_ = max_fac_ptr->getHypoLayer();

  //gttoc(update);

}
//======================= END HessianFactor(GaussianFactorGraph, Sactter) ==================

} // gtsam


#include <chrono>

#ifdef WITH_BOOST_ODEINT
#include <boost/numeric/odeint.hpp>
#endif // WITH_BOOST_ODEINT

#include "catch.hpp"
#include <Eigen/Eigen>

#include "netket.hpp"
#include "Dynamics/TimeStepper/explicit_time_steppers.hpp"
#include "Dynamics/TimeStepper/controlled_time_steppers.hpp"

using namespace netket;

TEST_CASE("Observer function is called once at each time step", "[time-evolution]")
{
    using State = Eigen::VectorXd;
    std::vector<double> ts;

    auto noop = [](const State& /*x*/, State& /*dxdt*/, double /*t*/) {};
    auto observer = [&](const State& /*x*/, double t) {
        ts.push_back(t);
    };

    const double t0 = -10.;
    const double tmax = 10.;
    const double dt = 1.;

    State initial(1);
    initial << 1.;

    SECTION("Euler with internal time step = dt")
    {
        using TimeStepper = ode::EulerTimeStepper<State>;
        TimeStepper stepper(dt, initial.size());
        ode::Integrate<TimeStepper, State>(stepper, noop, initial, {t0, tmax, dt}, observer);

        CHECK(ts.size() == 21);
        for(size_t j = 0; j < ts.size(); ++j)
        {
            double current_t = -10. + j;
            CHECK(Approx(ts[j]) == current_t);
        }
    }
    SECTION("Euler with internal time step != dt")
    {
        using TimeStepper = ode::EulerTimeStepper<State>;
        TimeStepper stepper(0.3 * dt, initial.size());
        ode::Integrate<TimeStepper, State>(stepper, noop, initial, {t0, tmax, dt}, observer);

        CHECK(ts.size() == 21);
        for(size_t j = 0; j < ts.size(); ++j)
        {
            double current_t = -10. + j;
            CHECK(Approx(ts[j]) == current_t);
        }
    }
    SECTION("RK4")
    {
        using TimeStepper = ode::RungeKutta4Stepper<State>;
        TimeStepper stepper(0.4 * dt, initial.size());
        ode::Integrate<TimeStepper, State>(stepper, noop, initial, {t0, tmax, dt}, observer);

        CHECK(ts.size() == 21);
        for(size_t j = 0; j < ts.size(); ++j)
        {
            double current_t = -10. + j;
            CHECK(Approx(ts[j]) == current_t);
        }
    }
    SECTION("controlled Heun")
    {
        using TimeStepper = ode::HeunTimeStepper<State>;
        TimeStepper stepper(1e-6, 1e-6, initial.size());
        ode::Integrate<TimeStepper, State>(stepper, noop, initial, {t0, tmax, dt}, observer);

        CHECK(ts.size() == 21);
        for(size_t j = 0; j < ts.size(); ++j)
        {
            double current_t = -10. + j;
            CHECK(Approx(ts[j]) == current_t);
        }
    }
    SECTION("controlled Dopri54")
    {
        using TimeStepper = ode::Dopri54TimeStepper<State>;
        TimeStepper stepper(1e-6, 1e-6, initial.size());
        ode::Integrate<TimeStepper, State>(stepper, noop, initial, {t0, tmax, dt}, observer);

        CHECK(ts.size() == 21);
        for(size_t j = 0; j < ts.size(); ++j)
        {
            double current_t = -10. + j;
            CHECK(Approx(ts[j]) == current_t);
        }
    }
}

TEST_CASE("Integrators can propagate trivial ODE", "[time-evolution]")
{
    using State = Eigen::VectorXd;

    std::vector<double> ts;
    std::vector<State> xs;

    auto eom = [](const State& /*x*/, State& dxdt, double /*t*/) {
        dxdt(0) = 1.0;
    };
    auto observer = [&](const State& x, double t) {
        xs.push_back(x);
        ts.push_back(t);
    };

    const double t0 = -10.;
    const double tmax = 10.;
    const double dt = 1.;

    State initial(1);
    initial << t0;

    // use small absolute margin
    const double m = 1e-14;
    SECTION("Euler with internal time step = dt")
    {
        using TimeStepper = ode::EulerTimeStepper<State>;
        TimeStepper stepper(dt, initial.size());
        ode::Integrate<TimeStepper, State>(stepper, eom, initial, {t0, tmax, dt}, observer);

        for(size_t j = 0; j < ts.size(); ++j)
        {
            CHECK(Approx(xs[j](0)).margin(m) == ts[j]);
        }
    }
    SECTION("Euler with internal time step != dt")
    {
        using TimeStepper = ode::EulerTimeStepper<State>;
        TimeStepper stepper(0.3 * dt, initial.size());
        ode::Integrate<TimeStepper, State>(stepper, eom, initial, {t0, tmax, dt}, observer);

        for(size_t j = 0; j < ts.size(); ++j)
        {
            CHECK(Approx(xs[j](0)).margin(m) == ts[j]);
        }
    }
    SECTION("RK4")
    {
        using TimeStepper = ode::RungeKutta4Stepper<State>;
        TimeStepper stepper(0.4 * dt, initial.size());
        ode::Integrate<TimeStepper, State>(stepper, eom, initial, {t0, tmax, dt}, observer);

        for(size_t j = 0; j < ts.size(); ++j)
        {
            CHECK(Approx(xs[j](0)).margin(m) == ts[j]);
        }
    }
    SECTION("controlled Heun")
    {
        using TimeStepper = ode::HeunTimeStepper<State>;
        TimeStepper stepper(1e-6, 1e-6, initial.size());
        ode::Integrate<TimeStepper, State>(stepper, eom, initial, {t0, tmax, dt}, observer);

        for(size_t j = 0; j < ts.size(); ++j)
        {
            CHECK(Approx(xs[j](0)).margin(m) == ts[j]);
        }
    }
    SECTION("controlled Dopri54")
    {
        using TimeStepper = ode::Dopri54TimeStepper<State>;
        TimeStepper stepper(1e-6, 1e-6, initial.size());
        ode::Integrate<TimeStepper, State>(stepper, eom, initial, {t0, tmax, dt}, observer);

        for(size_t j = 0; j < ts.size(); ++j)
        {
            CHECK(Approx(xs[j](0)).margin(m) == ts[j]);
        }
    }
}

TEST_CASE("Integrators approximately conserve norm when propagating Schroedinger eq", "[time-evolution]")
{
    using State = Eigen::VectorXcd;

    const double sqrt2 = std::sqrt(2.);
    const std::complex<double> im(0, 1);

    const std::complex<double> g(1/sqrt2, 1/sqrt2);

    const double delta = 1.;

    Eigen::MatrixXcd hamiltonian(2, 2);
    hamiltonian << -delta/2, g, std::conj(g), delta/2;

    auto eom = [&](const State& x, State& dxdt, double /*t*/) {
        dxdt.noalias() = -im * hamiltonian * x;
    };

    const double t0 = 0.;
    const double tmax = 20.;
    const double dt = 1.;

    State x(2);
    x << 1., 0;

    SECTION("explicit Euler")
    {
        using TimeStepper = ode::EulerTimeStepper<State>;
        TimeStepper stepper(5e-5 * dt, x.size());
        ode::Integrate<TimeStepper, State>(stepper, eom, x, {t0, tmax, dt});

        const double m = 1e-3; // explicit Euler has a large error
        CHECK(Approx(x.norm()).margin(m) == 1.);
    }
    SECTION("RK4")
    {
        using TimeStepper = ode::RungeKutta4Stepper<State>;
        TimeStepper stepper(5e-4 * dt, x.size());
        ode::Integrate<TimeStepper, State>(stepper, eom, x, {t0, tmax, dt});

        CHECK(Approx(x.norm()) == 1.);
    }
    SECTION("controlled Heun")
    {
        using TimeStepper = ode::HeunTimeStepper<State>;
        TimeStepper stepper(1e-6, 1e-6, x.size());
        ode::Integrate<TimeStepper, State>(stepper, eom, x, {t0, tmax, dt});

        CHECK(Approx(x.norm()) == 1.);
    }
    SECTION("controlled Dopri54")
    {
        using TimeStepper = ode::Dopri54TimeStepper<State>;
        TimeStepper stepper(1e-9, 1e-9, x.size());
        ode::Integrate<TimeStepper, State>(stepper, eom, x, {t0, tmax, dt});

        CHECK(Approx(x.norm()) == 1.);
    }
}

#ifdef WITH_BOOST_ODEINT
namespace odeint = boost::numeric::odeint;

TEST_CASE("Comparison with boost::odeint for Schroedinger eq", "[time-evolution]")
{
    using State = Eigen::VectorXcd;
    using OdeintState = std::vector<std::complex<double>>;

    const double sqrt2 = std::sqrt(2.);
    const std::complex<double> im(0, 1);

    const std::complex<double> g(1/sqrt2, 1/sqrt2);
    const auto gc = std::conj(g);

    const double delta = 1.;

    Eigen::MatrixXcd hamiltonian(3, 3);
    hamiltonian << -delta/2, g, g,
                    gc,      0, 0,
                    gc,      0, delta/2;

    auto eom = [&](const State& x, State& dxdt, double /*t*/) {
        dxdt.noalias() = -im * hamiltonian * x;
    };

    auto odeint_eom = [&](const OdeintState& x, OdeintState& dxdt, double /*t*/) {
        Eigen::Map<const State> x_map(x.data(), x.size());
        Eigen::Map<State> dxdt_map(dxdt.data(), dxdt.size());

        dxdt_map = -im * hamiltonian * x_map;
    };

    const double t0 = 0.;
    const double tmax = 100.;
    const double dt = 10.;

    State x(3);
    x << 1., 0., 0.;

    OdeintState xo{{1., 0.}, {0., 0.}, {0., 0.}};

    SECTION("explicit Euler")
    {
        using TimeStepper = ode::EulerTimeStepper<State>;
        TimeStepper stepper(5e-5 * dt, x.size());
        ode::Integrate<TimeStepper, State>(stepper, eom, x, {t0, tmax, 5e-5 * dt});

        using OdeintStepper = odeint::euler<OdeintState>;
        odeint::integrate_const(OdeintStepper(), odeint_eom, xo, t0, tmax, 5e-5 * dt);

        for(size_t i = 0; i < xo.size(); i++)
        {
            REQUIRE(Approx(x(i).real()) == xo[i].real());
            REQUIRE(Approx(x(i).imag()) == xo[i].imag());
        }
    }
    SECTION("RK4")
    {
        using TimeStepper = ode::RungeKutta4Stepper<State>;
        TimeStepper stepper(1e-4 * dt, x.size());
        ode::Integrate<TimeStepper, State>(stepper, eom, x, {t0, tmax, 1e-4 * dt});

        using OdeintStepper = odeint::runge_kutta4<OdeintState>;
        odeint::integrate_const(OdeintStepper(), odeint_eom, xo, t0, tmax, 1e-4 * dt);

        for(size_t i = 0; i < xo.size(); i++)
        {
            REQUIRE(Approx(x(i).real()) == xo[i].real());
            REQUIRE(Approx(x(i).imag()) == xo[i].imag());
        }
    }
    SECTION("controlled Dopri54")
    {
        using TimeStepper = ode::Dopri54TimeStepper<State>;
        TimeStepper stepper(1e-12, 1e-12, x.size());

        Stopwatch watch;
        ode::Integrate<TimeStepper, State>(stepper, eom, x, {t0, tmax, dt});
        auto elapsed = watch.elapsed();
        std::cout << "Dopri54 ellapsed time: " << elapsed.count() << "us" << std::endl;

        using OdeintStepper = odeint::runge_kutta_dopri5<OdeintState>;
        auto odeint_stepper = odeint::make_controlled<OdeintStepper>(1e-12, 1e-12);

        watch.restart();
        odeint::integrate_const(odeint_stepper, odeint_eom, xo, t0, tmax, dt);
        elapsed = watch.elapsed();
        std::cout << " odeint ellapsed time: " << elapsed.count() << "us" << std::endl;

        double m = 1e-17;
        for(size_t i = 0; i < xo.size(); i++)
        {
            REQUIRE(Approx(x(i).real()).margin(m) == xo[i].real());
            REQUIRE(Approx(x(i).imag()).margin(m) == xo[i].imag());
        }
    }
}
#endif


#include "uint.h"

#include <algorithm>
#include <boost/static_assert.hpp>
#include <assert.h>

const char kDigits[] = "0123456789abcdef";
BOOST_STATIC_ASSERT(sizeof kDigits == 17);

void UnsignedInt::sub(const UnsignedInt& x)
{
  if (lessThan(x))
  {
    assert(0 && "Underflow");
    abort();
  }
  const value_type& rhs = x.limbs_;
  assert (rhs.size() <= limbs_.size());
  size_t len = std::min(limbs_.size(), rhs.size());
  uint64_t carry = 0;

  for (size_t i = 0; i < len; ++i)
  {
    uint64_t sum = limbs_[i] - static_cast<uint64_t>(rhs[i]) - carry;
    limbs_[i] = sum & kMask_;
    carry = sum > kMask_;
  }
  if (carry)
  {
    for (size_t i = len; i < limbs_.size() && carry; ++i)
    {
      uint64_t sum = limbs_[i] - carry;
      limbs_[i] = sum & kMask_;
      carry = sum > kMask_;
    }
  }
  if (carry)
  {
    assert(0 && "Underflow");
    abort();
  }
  while (!limbs_.empty() && limbs_.back() == 0)
    limbs_.pop_back();
}

std::string UnsignedInt::toHex() const
{
  std::string result;
  result.reserve(limbs_.size()*8);

  if (limbs_.empty())
  {
    result.push_back('0');
    return result;
  }

  assert(limbs_.size() > 0);
  for (size_t i = 0; i < limbs_.size()-1; ++i)
  {
    uint32_t x = limbs_[i];
    for (int j = 0; j < 8; ++j)
    {
      int lsd = x % 16;
      x /= 16;
      result.push_back(kDigits[lsd]);
    }
  }

  uint32_t x = limbs_.back();
  do
  {
    int lsd = x % 16;
    x /= 16;
    result.push_back(kDigits[lsd]);
  } while (x != 0);

  std::reverse(result.begin(), result.end());

  return result;
}

std::string UnsignedInt::toDec() const
{
  const uint32_t segment = 1000000000;
  std::string result;
  if (limbs_.empty())
  {
    result.push_back('0');
    return result;
  }

  result.reserve(9*limbs_.size() + 7*limbs_.size()/11 + 1);
  // log10(2**32) = 32*log10(2) = 9.633 digits per word
  UnsignedInt copy = *this;
  while (copy.limbs_.size() > 1)
  {
    uint32_t x = copy.devide(segment);
    for (int i = 0; i < 9; ++i)
    {
      int lsd = x  % 10;
      x /= 10;
      result.push_back(kDigits[lsd]);
    }
  }

  uint32_t x = copy.limbs_[0];
  do
  {
    int lsd = x % 10;
    x /= 10;
    result.push_back(kDigits[lsd]);
  } while (x != 0);

  std::reverse(result.begin(), result.end());

  return result;
}

UnsignedInt::UnsignedInt(const std::string& x, Radix r)
{
  if (r == kDec)
  {
    parseDec(x);
  }
  else if (r == kHex)
  {
    parseHex(x);
  }
  else
  {
    assert(0 && "Radix invalid");
    abort();
  }
}

uint32_t fromHex(char c)
{
  if (c >= '0' && c <= '9')
    return c - '0';
  if (c >= 'a' && c <= 'f')
    return c - 'a' + 10;
  if (c >= 'A' && c <= 'F')
    return c - 'A' + 10;
  return -1;
}

void UnsignedInt::parseHex(const std::string& str)
{
  limbs_.reserve((str.size()+7) / 8);
  for (size_t i = 0; i < str.size(); ++i)
  {
    if (i % 8 == 0)
      limbs_.push_back(0);
    uint32_t digit = fromHex(str[str.size() - i - 1]);
    uint32_t shift = 4 * (i % 8);
    limbs_.back() |= (digit << shift);
  }
  if (limbs_.size() == 1 && limbs_[0] == 0)
  {
    limbs_.clear();
  }
}

uint32_t fromDec(char c)
{
  if (c >= '0' && c <= '9')
    return c - '0';
  return -1;
}

uint32_t parseSegment(const char* str, int len)
{
  uint32_t seg = 0;
  for (int i = 0; i < len; ++i)
  {
    seg *= 10;
    seg += fromDec(str[i]);
  }
  return seg;
}

void UnsignedInt::parseDec(const std::string& str)
{
  const uint32_t kSegment = 1000 * 1000 * 1000;
  const uint32_t kSegmentDigits = 9;
  // log2(10)/32 = 0.10381025296523
  // 8/77 = 0.103896103896104
  limbs_.reserve(1 + str.size() * 8 / 77);

  int first = str.size() % kSegmentDigits;
  if (first)
  {
    uint32_t seg = parseSegment(str.c_str(), first);
    add(seg);
  }
  for (size_t i = first; i < str.size(); i += kSegmentDigits)
  {
    assert(i + kSegmentDigits <= str.size());
    uint32_t seg = parseSegment(str.c_str() + i, kSegmentDigits);
    multiply(kSegment);
    add(seg);
  }
}


#include "Day24-ElectromagneticMoat.h"

#include <AdventOfCodeCommon/DisableLibraryWarningsMacros.h>

#include "BridgeGraph.h"

__BEGIN_LIBRARIES_DISABLE_WARNINGS
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
__END_LIBRARIES_DISABLE_WARNINGS

namespace AdventOfCode
{
namespace Year2017
{
namespace Day24
{

namespace
{

const size_t MAX_NODES = 51;

}

BridgeGraph::NodeIDToNeighbors nodeIDToNeighborsFromComponentLines(const std::vector<std::string>& componentLines)
{
    BridgeGraph::NodeIDToNeighbors nodeIDToNeighbors(MAX_NODES);
    for (const auto& line : componentLines)
    {
        std::vector<std::string> tokens;
        boost::split(tokens, line, boost::is_any_of("/"));
        if (tokens.size() != 2)
        {
            throw std::runtime_error("Expected exactly two tokens.");
        }

        const BridgeGraph::NodeIDType node1 = boost::lexical_cast<BridgeGraph::NodeIDType>(tokens[0]);
        const BridgeGraph::NodeIDType node2 = boost::lexical_cast<BridgeGraph::NodeIDType>(tokens[1]);

        if (node1 >= MAX_NODES || node2 >= MAX_NODES)
        {
            throw std::runtime_error("Node ID is too large.");
        }

        nodeIDToNeighbors[node1].push_back(node2);
        nodeIDToNeighbors[node2].push_back(node1);
    }

    return nodeIDToNeighbors;
}

unsigned strengthOfStrongestBridge(const std::vector<std::string>& componentLines)
{
    BridgeGraph::NodeIDToNeighbors nodeIDToNeighbors = nodeIDToNeighborsFromComponentLines(componentLines);
    BridgeGraph bridgeGraph{nodeIDToNeighbors};
    return bridgeGraph.maxCostWalk();
}

unsigned strengthOfStrongestLongestBridge(const std::vector<std::string>& componentLines)
{
    BridgeGraph::NodeIDToNeighbors nodeIDToNeighbors = nodeIDToNeighborsFromComponentLines(componentLines);
    BridgeGraph bridgeGraph{nodeIDToNeighbors};
    return bridgeGraph.maxCostLongestWalk();
}

}
}
}


#include "tsv.h"

#include <boost/algorithm/string.hpp>
#include <fstream>

using Eigen::VectorXd;
using std::string;
using std::vector;

ClassificationDataset load_tsv(const string& filename) {
    std::ifstream input(filename);
    if ( ! input) {
        std::ostringstream o;
        o << "Could not open file " << filename;
        throw std::runtime_error(o.str());
    }

    vector<VectorXd> x;
    vector<int> y;

    for (string line; std::getline(input, line); ) {
        // skip empty lines
        if (line.size() == 0)
            continue;

        // split line
        vector<string> strings;
        boost::split(strings, line, [](char c) { return c == '\t'; });

        // get point
        VectorXd v(3);
        for (unsigned i = 0; i < 3; ++i)
            v(i) = atof(strings[i].c_str());
        x.push_back(v);

        // get class
        y.push_back(strings[3] == "1");
    }

    return ClassificationDataset { x, y };
}


/* boost random/ranlux.hpp header file
 *
 * Copyright Jens Maurer 2002
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 *
 * Revision history
 *  2001-02-18  created
 */

#ifndef BOOST_RANDOM_RANLUX_HPP
#define BOOST_RANDOM_RANLUX_HPP

#include <boost/config.hpp>
#include <boost/random/subtract_with_carry.hpp>
#include <boost/random/discard_block.hpp>

namespace boost {

namespace random {
  typedef subtract_with_carry<int, (1<<24), 10, 24, 0> ranlux_base;
  typedef subtract_with_carry_01<float, 24, 10, 24> ranlux_base_01;
  typedef subtract_with_carry_01<double, 48, 10, 24> ranlux64_base_01;
}

namespace random {
namespace detail {
/**
 * The ranlux family of generators are described in
 *
 *  @blockquote
 *  "A portable high-quality random number generator for lattice field theory
 *  calculations", M. Luescher, Computer Physics Communications, 79 (1994)
 *  pp 100-110.
 *  @endblockquote
 *
 * The levels are given in
 *
 *  @blockquote
 *  "RANLUX: A Fortran implementation ofthe high-quality
 *  pseudorandom number generator of Luescher", F. James,
 *  Computer Physics Communications 79 (1994) 111-114
 *  @endblockquote
 */
class ranlux_documentation {};
}
}

/** @copydoc boost::random::detail::ranlux_documentation */
typedef random::discard_block<random::ranlux_base, 223, 24> ranlux3;
/** @copydoc boost::random::detail::ranlux_documentation */
typedef random::discard_block<random::ranlux_base, 389, 24> ranlux4;

/** @copydoc boost::random::detail::ranlux_documentation */
typedef random::discard_block<random::ranlux_base_01, 223, 24> ranlux3_01;
/** @copydoc boost::random::detail::ranlux_documentation */
typedef random::discard_block<random::ranlux_base_01, 389, 24> ranlux4_01;

/** @copydoc boost::random::detail::ranlux_documentation */
typedef random::discard_block<random::ranlux64_base_01, 223, 24> ranlux64_3_01;
/** @copydoc boost::random::detail::ranlux_documentation */
typedef random::discard_block<random::ranlux64_base_01, 389, 24> ranlux64_4_01;

#if !defined(BOOST_NO_INT64_T) && !defined(BOOST_NO_INTEGRAL_INT64_T)
namespace random {
  typedef random::subtract_with_carry<int64_t, (int64_t(1)<<48), 10, 24, 0> ranlux64_base;
}
/** @copydoc boost::random::detail::ranlux_documentation */
typedef random::discard_block<random::ranlux64_base, 223, 24> ranlux64_3;
/** @copydoc boost::random::detail::ranlux_documentation */
typedef random::discard_block<random::ranlux64_base, 389, 24> ranlux64_4;
#endif /* !BOOST_NO_INT64_T && !BOOST_NO_INTEGRAL_INT64_T */

} // namespace boost

#endif // BOOST_RANDOM_LINEAR_CONGRUENTIAL_HPP


#pragma once

#include <Eigen/Core>

#include <ipc/collision_constraint.hpp>

#include <opt/collision_constraint.hpp>

#include <autodiff/autodiff_types.hpp>
#include <barrier/barrier.hpp>
#include <ccd/ccd.hpp>
#include <ipc/broad_phase/hash_grid.hpp>
#include <utils/eigen_ext.hpp>

namespace ipc::rigid {

class DistanceBarrierConstraint : public CollisionConstraint {
public:
    DistanceBarrierConstraint(
        const std::string& name = "distance_barrier_constraint");

    nlohmann::json settings() const override;
    void settings(const nlohmann::json& json) override;

    virtual void initialize() override;

    double barrier_activation_distance() const
    {
        return m_barrier_activation_distance;
    }
    void barrier_activation_distance(const double dhat)
    {
        m_barrier_activation_distance = dhat;
    }

    bool has_active_collisions(
        const RigidBodyAssembler& bodies,
        const PosesD& poses_t0,
        const PosesD& poses_t1) const;

    double compute_earliest_toi(
        const RigidBodyAssembler& bodies,
        const PosesD& poses_t0,
        const PosesD& poses_t1) const;

    void compute_constraints(
        const RigidBodyAssembler& bodies,
        const PosesD& poses,
        Eigen::VectorXd& barriers);

    void construct_constraint_set(
        const RigidBodyAssembler& bodies,
        const PosesD& poses,
        Constraints& constraint_set) const;

    template <typename T>
    T distance_barrier(const T& distance, const double dhat) const;

    template <typename T> T distance_barrier(const T& distance) const
    {
        return distance_barrier(distance, m_barrier_activation_distance);
    }

    // TODO: Add distance_barrier_gradient if necessary

    double distance_barrier_hessian(double distance) const
    {
        return barrier_hessian(
            distance, m_barrier_activation_distance, barrier_type);
    }

    double compute_minimum_distance(
        const RigidBodyAssembler& bodies,
        const PosesD& poses) const;

    // Settings
    // ----------
    /// @brief Initial d̂ to use in the barrier function.
    double initial_barrier_activation_distance;

    /// @brief Choice of barrier function (see barrier/barrier.hpp).
    BarrierType barrier_type;

    double minimum_separation_distance;

protected:
    bool has_active_collisions_narrow_phase(
        const RigidBodyAssembler& bodies,
        const PosesD& poses_t0,
        const PosesD& poses_t1,
        const Candidates& candidates) const;

    double compute_earliest_toi_narrow_phase(
        const RigidBodyAssembler& bodies,
        const PosesD& poses_t0,
        const PosesD& poses_t1,
        const Candidates& candidates) const;

    /// @brief Max distance, d̂, at which the barrier forces are activate.
    double m_barrier_activation_distance;
};

} // namespace ipc::rigid

#include "distance_barrier_constraint.tpp"


#include <iostream>
#include <cstdio>

#include <boost/program_options.hpp>

#include <harp.hpp>

#include <specex_message.h>
#include <specex_image_data.h>
#include <specex_fits.h>
#include <specex_stamp.h>

#include <harp_plugin_specex.h>

#include <specex_serialization.h>

using namespace std;
namespace popts = boost::program_options;

class Patch {
  public :
  size_t x_offset;
  size_t y_offset;
  harp::matrix_double h;
  specex::Stamp stamp;
  Patch() : x_offset(0),y_offset(0) {};
};

typedef std::map<size_t,Patch> PatchMap;
typedef std::map<size_t,Patch>::iterator PatchIterator;
typedef std::map<size_t,Patch>::const_iterator PatchConstIterator;

//////////////////////////////////////////
size_t saved_npix;
size_t saved_nparams;
harp::vector_double P;
harp::vector_double Ninv;
harp::matrix_double A;
harp::matrix_double AtNinv;
harp::matrix_double Cinv;
harp::vector_double Dinv;
harp::vector_double Dquartinv;
harp::vector_double Dquart;
harp::matrix_double WDquartinv;
harp::matrix_double WDquart;
harp::matrix_double WDhalfinvWt;
harp::matrix_double WDhalfWt;
harp::matrix_double W;
harp::matrix_double temp;
harp::vector_double S;
harp::vector_double AtNinvP;
harp::vector_double SinvRf;
boost::numeric::ublas::vector < int > support;
//////////////////////////////////////////
#define MINEIGENVAL 1.e-40

void specex_column_norm (const harp::matrix_double & mat,  harp::vector_double & S ) {
  S.clear();
  for ( size_t i = 0; i < mat.size2(); ++i ) {
    for ( size_t j = 0; j < mat.size1(); ++j ) {
      S[ j ] += mat( j, i );
    }
  }
  // Invert
  for ( size_t i = 0; i < S.size(); ++i ) {
    S[i] = 1.0 / S[i];
  }
  return;
}
void specex_column_norm_lower (const harp::matrix_double & mat,  harp::vector_double & S ) {
  S.clear();
  for ( size_t i = 0; i < mat.size2(); ++i ) {
    for ( size_t j = 0; j < mat.size1(); ++j ) {
      if(j<i)
	S[ j ] += mat( i, j );
      else
	S[ j ] += mat( j, i );
    }
  }
  // Invert
  for ( size_t i = 0; i < S.size(); ++i ) {
    S[i] = 1.0 / S[i];
  }
  return;
}
void specex_apply_norm_left(const harp::vector_double& v, harp::matrix_double & mat) {
  for ( size_t i = 0; i < mat.size2(); ++i ) {
    for ( size_t j = 0; j < mat.size1(); ++j ) {
      mat( j, i ) *= v[j];
    }
  }
  return;
}

void specex_apply_norm_right(const harp::vector_double& v, harp::matrix_double & mat) {
  for ( size_t i = 0; i < mat.size2(); ++i ) {
    for ( size_t j = 0; j < mat.size1(); ++j ) {
      mat( i, j ) *= v[j];
    }
  }
  return;
}
void specex_apply_norm(const harp::vector_double& n, harp::vector_double & v) {
  for ( size_t i = 0; i < v.size(); ++i ) {
    v[i] *= n[i];
  }
  return;
}

void extract(const harp::psf* psf, const specex::image_data& image, const specex::image_data& weight, double& rflux, double& invar, size_t spec_index, size_t lambda_index, PatchMap& patches) {

  //#define TIMING
#ifdef TIMING
  double tstart;
  double tstop;
  double tstart1;
  double tstop1;
#endif
  //SPECEX_INFO("Extracting fiber " << spec_index << " lambda " << lambda_index);

  // defining data stamp
  size_t bx,by,nx,ny;
  psf->extent(spec_index,lambda_index,bx,by,nx,ny);

  specex::Stamp stamp(image);
  stamp.begin_i=max(stamp.begin_i,int(bx));
  stamp.begin_j=max(stamp.begin_j,int(by));
  stamp.end_i=min(stamp.end_i,int(bx+nx));
  stamp.end_j=min(stamp.end_j,int(by+ny));
  size_t nspec   = psf->n_spec();
  size_t nlambda = psf->n_lambda();

  // loading patches

  // book keeping
  std::map<size_t,size_t> used_patches;

  int spec_margin = 20; // 20
  int lambda_margin = 1000; // 1000

#ifdef TIMING
  tstart = harp::wtime();
#endif

  size_t param_index = 0;
  //  load neighbouring patches( compute only if not in store)
  for(size_t other_spec = size_t(max(0,int(spec_index)-spec_margin)); other_spec<min(nspec,spec_index+spec_margin+1); other_spec++) {
   for(size_t other_lambda = size_t(max(0,int(lambda_index)-lambda_margin)); other_lambda<min(nlambda,lambda_index+lambda_margin+1); other_lambda++) {

     psf->extent(other_spec,other_lambda,bx,by,nx,ny);

     specex::Stamp other_stamp; other_stamp.SetParent(stamp); // stamp in the coordinate system of the target stamp
     int dx = int(bx)-stamp.begin_i;
     int dy = int(by)-stamp.begin_j;
     other_stamp.begin_i=max(other_stamp.begin_i,dx);
     other_stamp.begin_j=max(other_stamp.begin_j,dy);
     dx = int(bx+nx)-stamp.begin_i;
     dy = int(by+ny)-stamp.begin_j;
     other_stamp.end_i=min(other_stamp.end_i,dx);
     other_stamp.end_j=min(other_stamp.end_j,dy);

     if(other_stamp.n_rows()>0 && other_stamp.n_cols()>0) {

       //SPECEX_INFO(other_spec << "," << other_lambda << " is a neighbour of " << spec_index << "," << lambda_index << " size= " << other_stamp.n_cols() << "x" << other_stamp.n_rows());

       int index = other_lambda+nlambda*other_spec;

       used_patches[index] = param_index; param_index++;

       if( patches.find(index) == patches.end()) {

	 //SPECEX_INFO("Loading patch at lambda=" << other_lambda << " spec=" << other_spec);

	 Patch patch;
	 psf->response(other_spec,other_lambda,patch.x_offset,patch.y_offset,patch.h);
	 patches[index] = patch;
       }

       patches.find(index)->second.stamp = other_stamp; // save stamp for filling A

     }
   }
  }

  // clear cache of unused patches to save memory
  PatchIterator it=patches.begin();
  while(it!=patches.end()) {
    if( used_patches.find(it->first) == used_patches.end()) {
      //SPECEX_INFO("Erasing patch at " << it->first%nlambda << " "<<   it->first/nlambda);
      patches.erase(it);
      it=patches.begin(); // better start from begining
    }else{
      it++;
    }
  }

#ifdef TIMING
  tstop = harp::wtime();
  cout << "load/unload patches : " << tstop-tstart << " s" << endl;
#endif

  // fill A matrix (keep notations of spectroperf.)
  size_t nparams     = used_patches.size();
  size_t stamp_ncols = stamp.n_cols();
  size_t stamp_nrows = stamp.n_rows();
  size_t npix        = stamp_ncols*stamp_nrows;

  if(npix > saved_npix) {
    cout << "resizing npix" << endl;
    P.resize(npix);
    Ninv.resize(npix);
  }
  if(nparams > saved_nparams) {
    cout << "resizing nparams" << endl;
    Cinv.resize(nparams,nparams);
    Dinv.resize(nparams);
    Dquartinv.resize(nparams);
    Dquart.resize(nparams);
    W.resize (nparams,nparams);
    WDquartinv.resize(nparams,nparams);
    WDquart.resize(nparams,nparams);
    WDhalfinvWt.resize(nparams,nparams);
    WDhalfWt.resize(nparams,nparams);
    temp.resize(nparams,nparams);
    S.resize(nparams);
    AtNinvP.resize(nparams);
    SinvRf.resize(nparams);
    support.resize(2*nparams);
  }
  if((npix > saved_npix) || (nparams > saved_nparams)) {
    A.resize(npix,nparams);
    AtNinv.resize(nparams,npix);
  }

  if(npix>saved_npix) saved_npix = npix;
  if(nparams>saved_nparams) saved_nparams = nparams;

  P.clear(); // the image
  Ninv.clear();// the noise diagonal matrix

  int pixindex=0;
  for(int j=stamp.begin_j;j<stamp.end_j;j++) {
    for(int i=stamp.begin_i;i<stamp.end_i;i++,pixindex++) {
      P(pixindex)=image(i,j);
      Ninv(pixindex)=weight(i,j);
    }
  }

  A.clear(); // P=A*f

#ifdef TIMING
  tstart = harp::wtime();
#endif
  for(PatchIterator it=patches.begin(); it!=patches.end();it++) {

    size_t param_index = used_patches.find(it->first)->second;

    // tricky part : putting the patch at the right place in matrix A

    const specex::Stamp & other_stamp = it->second.stamp;

    for(int j=other_stamp.begin_j;j<other_stamp.end_j;j++) {

      for(int i=other_stamp.begin_i;i<other_stamp.end_i;i++) {

	// this is the row-major index of the pixels where i and j are pixel coordinates in the stamp
	int pix_index   = i+j*stamp_ncols;

	// i and j are pixel coordinates in the stamp, so we have to add the coordinate of the bottom left corner in the CCD image
	int ccd_x   = i+stamp.begin_i;
	int ccd_y   = j+stamp.begin_j;

	// the patch coordinates begins at x_offset y_offset
	int patch_x = ccd_x-it->second.x_offset;
	int patch_y = ccd_y-it->second.y_offset;

	// the patch is flipped in harp
	A(pix_index,param_index) = it->second.h(patch_y,patch_x); // h is flipped in harp

      }
    }

  }

#ifdef TIMING
  tstop = harp::wtime();
  cout << "filling A : " << tstop-tstart << " s" << endl;
#endif

  // now brute force algebra

#ifdef TIMING
  tstart = harp::wtime();
#endif

  // computing AtNinv and Cinv
#ifdef TIMING
  tstart1 = harp::wtime();
#endif

  AtNinv.clear(); // At*Ni
  for ( size_t i = 0; i < nparams; ++i ) {
    for ( size_t j = 0; j < npix; ++j ) {
      AtNinv(i,j) = A(j,i)*Ninv(j);
    }
  }

  blas::gemm(1,AtNinv,A,0,Cinv); // Cinv=At*Ni*A
#ifdef TIMING
  tstop1 = harp::wtime();
  cout << "filling Cinv   : " << tstop1-tstart1 << " s " << endl;
#endif

  // eigen decomposition
#ifdef TIMING
  tstart1=harp::wtime();
#endif
  {
    ublas::noalias(temp)=Cinv;
    int nfound;
    boost::numeric::bindings::lapack::syevr ( 'V', 'A', boost::numeric::bindings::lower ( temp ), 0.0, 0.0, 0, 0, 0.0, nfound, Dinv, W, support );
  }
#ifdef TIMING
  tstop1=harp::wtime();
  cout << "eigen decompose   : " << tstop1-tstart1 << " s " << endl;
#endif

  // eigen composition
#ifdef TIMING
  tstart1=harp::wtime();
#endif
  for(size_t i=0;i<Dinv.size();i++) {
    if(Dinv(i)<MINEIGENVAL) {
      //cout << "fixing eigenval=" << Dinv(i) << endl;
      Dinv(i)=MINEIGENVAL;
    }
    Dquartinv(i)=sqrt(sqrt(Dinv(i)));
    Dquart(i)=1/Dquartinv(i);
  }
  noalias(WDquartinv)=W;
  specex_apply_norm_right(Dquartinv,WDquartinv); // WDquartinv
  harp::matrix_double& WDquart=W; // just to remember what it is
  specex_apply_norm_right(Dquart,WDquart); // WDquart

  //blas::gemm(1,WDquart,boost::numeric::bindings::trans(WDquart),0,WDhalfWt);
  //blas::gemm(1,WDquartinv,boost::numeric::bindings::trans(WDquartinv),1,WDhalfinvWt);
  blas::syrk(1,WDquart,0,boost::numeric::bindings::lower(WDhalfWt));
  blas::syrk(1,WDquartinv,0,boost::numeric::bindings::lower(WDhalfinvWt));

#ifdef TIMING
  tstop1=harp::wtime();
  cout << "eigen compose   : " << tstop1-tstart1 << " s " << endl;
#endif

  // computing S, Sij=1/sum_j Qij , where Q=W*Dinv^1/2*Wt
  specex_column_norm_lower ( WDhalfinvWt, S );

  blas::gemv(1,AtNinv,P,0,AtNinvP); // AtNinvP = AtNinv P

  // in fact I care about one line here, so I could go faster :
  blas::symv(1,boost::numeric::bindings::lower(WDhalfWt),AtNinvP,0,SinvRf); // SinvRf = Wt D^(1/2) W At Ninv P

  // collecting results
  int index_of_target_param = used_patches.find(lambda_index+nlambda*spec_index)->second;

  rflux=S(index_of_target_param)*SinvRf(index_of_target_param);
  invar=S(index_of_target_param); invar*=invar; // Ctilde=S^-2 and we want here Ctilde^-1=S^2

#ifdef TIMING
  tstop = harp::wtime();
  cout << "algebra   : " << tstop-tstart << " s " << endl;
#endif

  // we discard all the rest

}

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

  saved_npix=0;
  saved_nparams=0;

  string jsonpar = "";
  string outfilename = "toto.fits";

  // Declare options
  popts::options_description desc ( "Allowed Options" );

  desc.add_options()
  ( "help,h", "display usage information" )
  ( "verbose,v", "supress information printing" )
  ( "debug,d", "write out intermediate data products for debugging" )
  ( "out", popts::value<string>( &outfilename ), "root path of output files (default = \"harp_\")" )
  ( "par", popts::value < string > ( &jsonpar ), "JSON parameter file" )
  ;

  popts::variables_map vm;

  popts::store(popts::command_line_parser( argc, argv ).options(desc).run(), vm);

  popts::notify(vm);

  if ( ( argc < 2 ) || vm.count( "help" ) || ( ! vm.count( "par" ) ) ) {
    cerr << endl;
    cerr << desc << endl;
    return 0;
  }

  specex_set_verbose(vm.count("verbose")>0);

  SPECEX_INFO("Reading " << jsonpar);

  boost::property_tree::ptree par;
  boost::property_tree::json_parser::read_json ( jsonpar, par );

  SPECEX_INFO("Loading PSF");

  boost::property_tree::ptree psf_props = par.get_child ( "psf" );
  harp::psf* psf = harp::specex_psf_create(psf_props);

  SPECEX_INFO("Loading image");

  boost::property_tree::ptree image_props = par.get_child ( "image" );
  string path = image_props.get("path","");
  specex::image_data image,weight;
  specex::read_fits_images(path,image,weight);

  SPECEX_INFO("Starting extraction");

  size_t nspec   = psf->n_spec();
  size_t nlambda = psf->n_lambda();

  specex::image_data rflux(nlambda,nspec);
  specex::image_data invar(nlambda,nspec);

  PatchMap patches;

  for(size_t s=0;s<nspec;s++) {
    if(s%2==0) {
      for(int l=0;l<nlambda;l++) {
	extract(psf,image,weight,rflux(l,s),invar(l,s),s,l,patches);
	SPECEX_INFO("spec " << s << " lambda " << l << " flux " <<  rflux(l,s) << " error " << 1./sqrt(invar(l,s)));
      }
    } else { // go the other way to minimize number of patches to reload
      for(int l=nlambda-1;l>=0;l--) {
	extract(psf,image,weight,rflux(l,s),invar(l,s),s,l,patches);
	SPECEX_INFO("spec " << s << " lambda " << l << " flux " <<  rflux(l,s) << " error " << 1./sqrt(invar(l,s)));
      }
    }
  }

  { // writing output

    harp::vector_double lambda = psf->lambda();
    specex::image_data wave(nlambda,1);
    for(size_t i=0;i<nlambda;i++) wave.data(i)=lambda(i);

    fitsfile * fp;
    harp::fits::create ( fp, outfilename );
    harp::fits::img_append < double > ( fp, rflux.n_rows(), rflux.n_cols() );
    harp::fits::img_write ( fp, rflux.data ,false);
    harp::fits::key_write(fp,"WHAT","RFLUX","");
    harp::fits::img_append < double > ( fp, invar.n_rows(), invar.n_cols() );
    harp::fits::img_write ( fp, invar.data ,false);
    harp::fits::key_write(fp,"EXTNAME","IVAR","");
    harp::fits::img_append < double > ( fp, 1, wave.n_cols() );
    harp::fits::img_write ( fp, wave.data ,false);
    harp::fits::key_write(fp,"EXTNAME","WAVELENGTH","");
    harp::fits::close ( fp );
  }
  return 0;
}


#include "LaplacianSmoother.h"

#include <maya/MFnNumericAttribute.h>
#include <maya/MDataHandle.h>

#include <Eigen/Dense>
#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#include <Eigen/Sparse>
#include <Eigen/SparseLU>

#include "MeshLaplacianData.h"
#include "MayaEigen.h"
#include "TimeCounter.h"

MTypeId LaplacianSmoother::id ( 0x09101 );
MObject LaplacianSmoother::aLambda;
MObject LaplacianSmoother::aTimingBuild;
MObject LaplacianSmoother::aTimingSolve;

//! Macro for an input attribute.
#define MAKE_INPUT(attr) \
    CHECK_MSTATUS(attr.setKeyable(true)); \
    CHECK_MSTATUS(attr.setStorable(true)); \
    CHECK_MSTATUS(attr.setReadable(true)); \
    CHECK_MSTATUS(attr.setWritable(true));

//! Macro for an output attribute.
#define MAKE_OUTPUT(attr) \
    CHECK_MSTATUS(attr.setKeyable(false)); \
    CHECK_MSTATUS(attr.setStorable(true)); \
    CHECK_MSTATUS(attr.setReadable(true)); \
    CHECK_MSTATUS(attr.setWritable(true));

///! Create a new LaplacianSmoother instance.
void* LaplacianSmoother::creator()
{
    LaplacianSmoother* res = new LaplacianSmoother();
    return res;
}

//! Initializes the node attributes.
MStatus LaplacianSmoother::initialize()
{
    MFnNumericAttribute nAttr;

    aLambda = nAttr.create ( "lambda", "lm", MFnNumericData::kFloat );
    MAKE_INPUT ( nAttr );
    nAttr.setMin ( 0.00001 );
    nAttr.setMax ( 100.00 );

    aTimingBuild = nAttr.create ( "timingbuild", "tb", MFnNumericData::kFloat );
    MAKE_OUTPUT ( nAttr );

    aTimingSolve = nAttr.create ( "timingsolve", "ts", MFnNumericData::kFloat );
    MAKE_OUTPUT ( nAttr );

    CHECK_MSTATUS ( addAttribute ( aLambda ) );
    CHECK_MSTATUS ( addAttribute ( aTimingBuild ) );
    CHECK_MSTATUS ( addAttribute ( aTimingSolve ) );

    // deform operator will be affected by aLambda.
    CHECK_MSTATUS ( attributeAffects ( aLambda, outputGeom ) );

    // timing attributes will be affected by aLambda.
    CHECK_MSTATUS ( attributeAffects ( aLambda, aTimingBuild ) );
    CHECK_MSTATUS ( attributeAffects ( aLambda, aTimingSolve ) );

    return MStatus::kSuccess;
}

//! Return if matrix M should be updated or not.
/*!
    Simple check with the size of matrix M and the number of vertices.
*/
bool LaplacianSmoother::needUpdateM ( MObject& mesh )
{
    if ( _M.rows() == 0 || _M.cols() == 0 ) return true;

    MFnMesh meshFn ( mesh );
    if ( meshFn.numVertices() != _M.rows() ) return true;

    return false;
}

//! Compute matrix M as Lt L.
void LaplacianSmoother::computeM ( MObject& mesh )
{
    MeshLaplacianData laplacian ( mesh );
    _L = laplacian.GetLaplacianMatrix ( 1 );
    _M = _L.transpose() * _L;
}

//! Solve Laplacian smoothing.
/*!
    S = (I + \lambda M)^{-1} P

    Solve the linear equation with Eigen::SimplicialCholesky solver.
*/
const Eigen::MatrixXd LaplacianSmoother::solve ( MDataBlock& block, const Eigen::MatrixXd& P )
{
    float& lambda = block.inputValue ( aLambda ).asFloat();

    Eigen::SparseMatrix<double> I = Eigen::MatrixXd::Identity ( _M.rows(), _M.cols() ).sparseView();

    Eigen::SparseMatrix<double> A = I + lambda * _M;

    Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver;
    solver.compute ( A );

    if ( solver.info() != Eigen::Success )
    {
        cout << "solver: fail" << endl;
        return P;
    }

    Eigen::MatrixXd P_new = solver.solve ( P );

    return P_new;
}

//! Get target mesh object.
MObject LaplacianSmoother::getMeshObject ( const MPlug& plug, MDataBlock& block )
{
    // get the input corresponding to this output
    unsigned int index = plug.logicalIndex();
    MObject thisNode = this->thisMObject();
    MPlug inPlug ( thisNode, input );
    inPlug.selectAncestorLogicalIndex ( index, input );
    MDataHandle hInput = block.inputValue ( inPlug );

    // get the input geometry and input groupId
    MDataHandle hGeom = hInput.child ( inputGeom );
    MDataHandle hGroup = hInput.child ( groupId );
    unsigned int groupId = hGroup.asLong();
    MDataHandle hOutput = block.outputValue ( plug );
    hOutput.copy ( hGeom );
    return hOutput.asMesh();
}

//! Compute function to deform.
MStatus LaplacianSmoother::compute ( const MPlug& plug, MDataBlock& block )
{
    MStatus status = MStatus::kUnknownParameter;
    if ( plug.attribute() == outputGeom )
    {
        TimeCounter tcSolve ( "Solve" );

        MObject& mesh = getMeshObject ( plug, block );

        Eigen::MatrixXd P = MayaEigen::getPositionMatrix ( mesh );

        if ( needUpdateM ( mesh ) )
        {
            TimeCounter tcBuild ( "Build" );
            computeM ( mesh );

            block.outputValue ( aTimingBuild ).setFloat ( tcBuild.getElapsedTime() );
        }

        Eigen::MatrixXd P_new = solve ( block, P );

        MayaEigen::setPositions ( mesh, P_new );

        block.outputValue ( aTimingSolve ).setFloat ( tcSolve.getElapsedTime() );

        status = MStatus::kSuccess;
    }
    return status;
}


/*
ID: septicmk
LANG: C++
TASK:
*/

#include "PCA.h"
#include "../Core/Core.h"
#include <boost/serialization/vector.hpp>
#include <boost/serialization/export.hpp>
#include <vector>

BOOST_CLASS_EXPORT(pca::PCA)
namespace pca{
    void print_matrix( char* desc, int m, int n, double* a, int lda ) {
        int i, j;
        printf( "\n %s\n", desc );
        for( i = 0; i < m; i++ ) {
                for( j = 0; j < n; j++ ) printf( " %6.2f", a[i+j*lda] );
                printf( "\n" );
        }
    }
    void Matrix::init (size_t nrow, size_t ncol, std::vector<double> data){
        this->nrow = nrow;
        this->ncol = ncol;
        this->data = data;
    }

    PCA* PCA::operator +(PCA &rhs){
        PCA *pret = new PCA();
        pret->mat.init(mat.nrow,mat.ncol,mat.data);
        return pret;
    }

    void PCA::beginDataScan(std::vector<double> records, size_t feat_dim){
        return;
    }

    void PCA::endDataScan(){
        return;
    }

    PCA* PCA::processRecord(std::vector<double> records, size_t feat_dim){
        PCA* pret = new PCA();
        pret->mat.init(mat.nrow,mat.ncol,mat.data);
        return pret;
    }

    bool PCA::isConverged(PCA* rhs, size_t feat_dim, double eps){
        return true;
    }

    void PCA::finish(std::vector<double> records, size_t feat_dim){
        printf("PCA finish\n");
        int n = feat_dim;
        int m = records.size()/feat_dim;
        int LDA = m;
        int LDU = m;
        int LDVT = n;
        int info, lwork;
        double wkopt;
        double* work;
        int *iwork = (int *)malloc(8*n*sizeof(int));
        double *s = (double *)malloc(n*sizeof(double));
        double *u = (double *)malloc(LDU*n*sizeof(double));
        double *vt = (double *)malloc(LDVT*n*sizeof(double));
        std::vector<double> A;
        A.clear();
        for(int i = 0 ; i < n ; i++){
            for(int j = 0; j < m ; j ++){
                A.push_back(records[j*feat_dim+i]);
            }
        }
        printf("PCA Results\n");
        lwork = -1;
        dgesdd_("Singular vectors", &m, &n, &*A.begin(), &LDA, s, u, &LDU, vt, &LDVT, &wkopt,
                &lwork, iwork, &info);
        lwork = (int)wkopt;
        work = (double *)malloc( lwork*sizeof(double) );
        dgesdd_("Singular vectors", &m, &n, &*A.begin(), &LDA, s, u, &LDU, vt, &LDVT, work,
                &lwork, iwork, &info);
        if( info > 0){
            printf("The algorithm computing SVD failed to cinverge.\n");
            return;
        }

        print_matrix ( "Right sigual vectors(rowwise)", n, n, vt, LDVT);
        free( (void*)work);
    }
}


#pragma once

#include <bitset>
#include <functional>
#include <numeric>
#include <stdexcept>
#include <vector>

#include <dtl/dtl.hpp>
#include <dtl/bits.hpp>
#include <dtl/math.hpp>
#include <dtl/simd.hpp>
#include <dtl/filter/blocked_bloomfilter/block_addressing_logic.hpp>
#include <dtl/filter/blocked_bloomfilter/blocked_bloomfilter_batch_dispatch.hpp>
#include <dtl/filter/blocked_bloomfilter/blocked_bloomfilter_block_logic.hpp>

#include <boost/integer/static_min_max.hpp>

#include "immintrin.h"

namespace dtl {

//===----------------------------------------------------------------------===//
// A high-performance blocked Bloom filter template.
//===----------------------------------------------------------------------===//
template<
    typename Tk,                  // the key type
    template<typename Ty, u32 i> class Hasher,      // the hash function family to use
    typename Tb,                  // the block type
    dtl::block_addressing block_addressing = dtl::block_addressing::POWER_OF_TWO,
    u1 early_out = false          // branch out early, if possible
>
struct blocked_bloomfilter_logic {

  //===----------------------------------------------------------------------===//
  // The static part.
  //===----------------------------------------------------------------------===//
  using key_t = typename std::remove_cv<Tk>::type;
  using word_t = typename std::remove_cv<typename Tb::word_t>::type;
  using block_t = typename std::remove_cv<Tb>::type;
  static_assert(std::is_integral<key_t>::value, "The key type must be an integral type.");
  static_assert(std::is_integral<word_t>::value, "The word type must be an integral type.");

  using size_t = $u64;

  static constexpr u32 word_cnt_per_block = block_t::word_cnt;
  static constexpr u32 word_cnt_per_block_log2 = dtl::ct::log_2<word_cnt_per_block>::value;
  static_assert(dtl::is_power_of_two(word_cnt_per_block), "The number of words per block must be a power of two.");

  static constexpr u32 word_bitlength = sizeof(word_t) * 8;
  static constexpr u32 word_bitlength_log2 = dtl::ct::log_2_u32<word_bitlength>::value;

  static constexpr u32 block_bitlength = word_bitlength * word_cnt_per_block;

  // Inspect the given hash function.
  using hash_value_t = decltype(Hasher<key_t, 0>::hash(42)); // TODO find out why NVCC complains
  static_assert(std::is_integral<hash_value_t>::value, "Hash function must return an integral type.");
  static constexpr u32 hash_value_bitlength = sizeof(hash_value_t) * 8;

  static constexpr u32 k = block_t::k;
  static constexpr u32 sector_cnt = block_t::sector_cnt;

  // The block addressing logic (either MAGIC or POWER_OF_TWO).
  using addr_t = block_addressing_logic<block_addressing>;
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  // Members
  //===----------------------------------------------------------------------===//
  /// Addressing logic instance.
  const addr_t addr;
  //===----------------------------------------------------------------------===//

  /// C'tor.
  /// Note, that the actual length might be (slightly) different to the
  /// desired length. The function get_length() returns the actual length.
  explicit
  blocked_bloomfilter_logic(const std::size_t desired_length)
      : addr((desired_length + (block_bitlength - 1)) / block_bitlength) { }

  /// Copy c'tor
  blocked_bloomfilter_logic(const blocked_bloomfilter_logic&) = default;

  ~blocked_bloomfilter_logic() = default;

  //===----------------------------------------------------------------------===//
  // Insert
  //===----------------------------------------------------------------------===//
//  __forceinline__ __host__
  void
  insert(word_t* __restrict filter_data,
         const key_t key) noexcept {
    const hash_value_t block_addressing_hash_val = Hasher<const key_t, 0>::hash(key);
    const hash_value_t block_idx = addr.get_block_idx(block_addressing_hash_val);
    const hash_value_t bitvector_word_idx = block_idx << word_cnt_per_block_log2;

    auto block_ptr = &filter_data[bitvector_word_idx];

    block_t::insert(block_ptr, key);
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  // Batch Insert
  //===----------------------------------------------------------------------===//
  void
  batch_insert(word_t* __restrict filter_data,
               const key_t* keys, u32 key_cnt) noexcept {
    for (std::size_t i = 0; i < key_cnt; i++) {
      insert(filter_data, keys[i]);
    }
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  // Contains
  //===----------------------------------------------------------------------===//
  __forceinline__ __host__ __device__
  u1
  contains(const word_t* __restrict filter_data,
           const key_t key) const noexcept {
    const hash_value_t block_addressing_hash_val = Hasher<const key_t, 0>::hash(key);
    const hash_value_t block_idx = addr.get_block_idx(block_addressing_hash_val);
    const hash_value_t bitvector_word_idx = block_idx << word_cnt_per_block_log2;

    const auto block_ptr = &filter_data[bitvector_word_idx];

    u1 found = block_t::contains(block_ptr, key);
    return found;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  // Contains (SIMD) - called by batch dispatch
  //===----------------------------------------------------------------------===//
  template<u64 n> // the vector length
  __forceinline__ __host__
  typename vec<word_t, n>::mask
  contains_vec(const word_t* __restrict filter_data,
               const vec<key_t, n>& keys) const noexcept {
    // Typedef the vector types.
    using key_vt = vec<key_t, n>;
    using hash_value_vt = vec<hash_value_t, n>;

    const hash_value_vt block_addressing_hash_vals = Hasher<key_vt, 0>::hash(keys);
    const hash_value_vt block_idxs = addr.get_block_idxs(block_addressing_hash_vals);
    const hash_value_vt bitvector_word_idx = block_idxs << word_cnt_per_block_log2;

    auto found = block_t::contains(keys, filter_data, bitvector_word_idx);
    return found;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  // Batch-wise Contains
  //===----------------------------------------------------------------------===//
  template<u64 vector_len>
  $u64
  batch_contains(const word_t* __restrict filter_data,
                 const key_t* __restrict keys, u32 key_cnt,
                 $u32* __restrict match_positions, u32 match_offset) const {
    return internal::dispatch<blocked_bloomfilter_logic, vector_len>
             ::batch_contains(*this, filter_data,
                              keys, key_cnt,
                              match_positions, match_offset);
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Returns the (actual) length in bits.
  std::size_t
  get_length() const noexcept {
    return u64(addr.get_block_cnt()) * block_bitlength;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Returns the (actual) length in number of words.
  std::size_t
  word_cnt() const noexcept {
    return u64(addr.get_block_cnt()) * word_cnt_per_block;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  // "Low-level" functions.
  //===----------------------------------------------------------------------===//
  /// Determines the block index for the given key.
  __forceinline__ __host__ __device__
  hash_value_t
  get_block_idx(const key_t key) const noexcept {
    const hash_value_t block_addressing_hash_val = Hasher<const key_t, 0>::hash(key);
    const hash_value_t block_idx = addr.get_block_idx(block_addressing_hash_val);
    return block_idx;
  }

  /// Determines the block index for the given key (called by batch_get_block_idx).
  template<u64 n> // the vector length
  __forceinline__ __host__
  vec<hash_value_t, n>
  get_block_idx_vec(const vec<key_t, n>& keys) const noexcept {
    // Typedef the vector types.
    using key_vt = vec<key_t, n>;
    using hash_value_vt = vec<hash_value_t, n>;

    const hash_value_vt block_addressing_hash_vals = Hasher<key_vt, 0>::hash(keys);
    const hash_value_vt block_idxs = addr.get_block_idxs(block_addressing_hash_vals);
    return block_idxs;
  }

  /// Determines the block indexes for a batch of keys.
  template<u64 vector_len>
  void
  batch_get_block_idx(const key_t* __restrict keys, u32 key_cnt,
                      $u32* __restrict block_idxs) const {
    internal::dispatch<blocked_bloomfilter_logic, vector_len>
        ::batch_get_block_idx(*this, keys, key_cnt, block_idxs);
  }

  /// Returns a pointer to the block with the given index.
  __forceinline__ __host__ __device__
  const word_t*
  get_block_ptr(const word_t* __restrict filter_data,
                const hash_value_t block_idx) const noexcept {
    const hash_value_t bitvector_word_idx = block_idx << word_cnt_per_block_log2;
    const word_t* block_ptr = &filter_data[bitvector_word_idx];
    return block_ptr;
  }
  //===----------------------------------------------------------------------===//

};

} // namespace dtl


#include <iostream>

#include <boost/numeric/linear_algebra/operators.hpp>
#include <boost/numeric/linear_algebra/concepts.hpp>

#include <libs/numeric/linear_algebra/test/algebraic_functions.hpp>

// float has a concept map for Field
// and PartiallyInvertibleMonoid w.r.t. mult
// This implies concept maps for all other scalar concepts

using math::identity; using math::add;

int main(int, char* [])
{
    using namespace std;
    using namespace mtl;

    math::add<float>    float_add;
    math::mult<float>   float_mult;

    cout << "equal_results(2.,5.,  3.,4., float_add) "
         << equal_results(2.f,5.f,  3.f,4.f, float_add)  << endl;
    cout << "equal_results(2.,6.,  3.,4., float_add) "
         << equal_results(2.,6.,  3.,4., float_add)  << endl;

    cout << "identity_pair(2.,4., float_mult) "
         << identity_pair(2.f,4.f, float_mult)  << endl;
    cout << "identity_pair(0.5,2., float_mult) "
         << identity_pair(0.5f,2.f, float_mult)  << endl << endl;

    cout << "ordered_algebraic_division(32.,2., float_mult) "
         << ordered_algebraic_division(32.f,2.f, float_mult)  << endl;
    cout << "ordered_algebraic_division(33.f,2.f, float_mult) "
         << ordered_algebraic_division(33.f,2.f, float_mult)  << endl;
    cout << "ordered_algebraic_division(33.f,2.f, float_add) "
         << ordered_algebraic_division(33.f,2.f, float_add)  << endl;
    cout << "ordered_algebraic_division(31.,2., float_mult) "
         << ordered_algebraic_division(31.f,2.f, float_mult)  << endl;
    cout << "ordered_algebraic_division(0.125,2, float_mult) "
         << ordered_algebraic_division(0.125f,2.f, float_mult)  << endl;

  return 0;
}


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

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

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing minmod capabilities

    Return the minimum of the two entries
    if they have the same sign, otherwise 0

    @par semantic:
    For any given value @c x,  @c y of type @c T:

    @code
    T r = minmod(x, y);
    @endcode

    is similar to:

    @code
    T r =  x*y > 0 ? min(x, y) : 0;
    @endcode

  **/
  const boost::dispatch::functor<tag::minmod_> minmod = {};
} }
#endif

#include <boost/simd/function/scalar/minmod.hpp>
#include <boost/simd/function/simd/minmod.hpp>

#endif