// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2011 The University of Sydney
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT
// HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
// BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
// OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
// IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <vector>
#include <iostream>
#include <iterator>
#include <math.h>
#include <boost/optional.hpp>
#include <fftw3.h>
#include <comma/visiting/traits.h>
#include <comma/application/command_line_options.h>
#include <comma/application/verbose.h>
#include <comma/csv/options.h>
#include <comma/csv/stream.h>
#include "detail/shuffle-tied.h"

std::size_t input_size=0;
bool filter_input=true;
bool logarithmic_output=true;
bool magnitude=false;
bool real=false;
bool split=false;
std::size_t bin_size=0;
boost::optional<double> bin_overlap;
bool tied=true;

void usage(bool detail)
{
    std::cerr<<"    perform fft on input data" << std::endl;
    std::cerr << std::endl;
    std::cerr<< "usage: " << comma::verbose.app_name() << " [ <options> ]" << std::endl;
    std::cerr << std::endl;
    std::cerr<< "    input fields: data; an array of double, size is specified with --size"  << std::endl;
    std::cerr<< "    output: array of pair (real, complex) of double; use --output-size to get size of array of doubles with the specified options"  << std::endl;
    std::cerr << std::endl;
    std::cerr << "options" << std::endl;
    std::cerr << "    --help,-h: show help" << std::endl;
    std::cerr << "    --verbose,-v: show detailed messages" << std::endl;
    std::cerr << std::endl;
    std::cerr << "    --bin-size=[<size>]: cut data into several bins of this size and perform fft on each bin, when not speificed calculates fft on the whole data" << std::endl;
    std::cerr << "    --bin-overlap=[<overlap>]: if specified, each bin will contain this portion of the last bin's data, range: 0 (no overlap) to 1"<<std::endl;
    std::cerr << "    --linear: output as linear; when not specified, output will be scaled to lograithm of 10 for magnitude or real part (phase is not affected)" << std::endl;
    std::cerr << "    --no-filter: when not specified, filters input using a cut window to get limited output" << std::endl;
    std::cerr << "    --size=<size>: size of input vector" << std::endl;
    std::cerr << std::endl;
    std::cerr << "  output options:" << std::endl;
    std::cerr << "    --magnitude: output magnitude only" << std::endl;
    std::cerr<< "        output is binary array of double with half the size of input"  << std::endl;
    std::cerr << "    --real: output real part only" << std::endl;
    std::cerr<< "        output is binary array of double with half the size of input"  << std::endl;
    std::cerr << "    --split: output array of real followed by array of complex part; when not specified real and complex parts are interleaved" << std::endl;
    std::cerr << std::endl;
    std::cerr << "    --shuffle,--shuffle-fields,--shuffled-fields=[<csv_fields>]: comma separated list of input fields to be written to stdout; if not specified prepend all input fields to the output" << std::endl;
    std::cerr << "    --untied: only write output (doesn't write input to stdout)" << std::endl;
    std::cerr << std::endl;
    std::cerr << "  utility options:" << std::endl;
    std::cerr << "    --output-fields: print output fields and exit; depends on input fields and size" << std::endl;
    std::cerr << "    --output-format: print binary format of output and exit; depends on input fields and size" << std::endl;
    std::cerr << "    --output-size: print size of output data array and exit; e.g. if output format is t,ui,256d, then output size is 256" << std::endl;
    std::cerr << std::endl;
    if(detail)
    {
        std::cerr << "csv options:" << std::endl;
        std::cerr<< comma::csv::options::usage() << std::endl;
        std::cerr << std::endl;
    }
    else
    {
        std::cerr << "use -v or --verbose to see more detail" << std::endl;
        std::cerr << std::endl;
    }
    std::cerr << "example" << std::endl;
    std::cerr << "      " << comma::verbose.app_name() << " --binary=\"t,16000f\" --fields=t,data --size=16000" << std::endl;
    std::cerr << std::endl;
    exit(0);
}

// math-fft --fields ,,data --format t,ui,16000f
// math-fft --fields ,,,,,data --format t,ui,s[$(( 16000 * 4  ))],t,ui,16000f

struct input_t
{
    std::vector<double> data;
    input_t() : data(input_size) {}
};

struct output_t
{
    std::vector<double> data;
    output_t()
    {
        std::size_t len=bin_size;
        if(magnitude || real) { len/=2; }
        data.resize(len);
    }
    void reset()
    {
        memset(&data[0],0,data.size()*sizeof(data[0]));
    }
};

namespace comma { namespace visiting {

template <> struct traits< input_t >
{
    template< typename K, typename V > static void visit( const K& k, input_t& p, V& v )
    {
        v.apply( "data", p.data );
    }
    template< typename K, typename V > static void visit( const K& k, const input_t& p, V& v )
    {
        v.apply( "data", p.data );
    }
};

template <> struct traits< output_t >
{
    template< typename K, typename V > static void visit( const K& k, const output_t& p, V& v )
    {
        v.apply( "output", p.data );
    }
};

} } // namespace comma { namespace visiting {

template<typename T>
T* allocate_fftw_array(std::size_t size)
{
    return reinterpret_cast<T*>(fftw_malloc(sizeof(T)*size));
}

// a quick and dirty helper class
struct fft
{
    std::vector<double> h;
    double* input;
    fftw_complex* output;
    fftw_plan plan;

    fft(std::size_t size) : h( size ), input(allocate_fftw_array<double>(size)) ,
        output(allocate_fftw_array<fftw_complex>(size / 2 + 1 )) ,
        plan( fftw_plan_dft_r2c_1d( size, input, output, 0 ) )
    {
        for( std::size_t i = 0; i < size; ++i ) { h[i] = 0.54 - 0.46 * std::cos( M_PI * 2 * i / size ); }
    }
    ~fft()
    {
        fftw_destroy_plan(plan);
        fftw_free( input ); // seems that fftw_destroy_plan() releases it
        fftw_free( output ); // seems that fftw_destroy_plan() releases it
    }
    void calculate() { fftw_execute( plan); }
    std::size_t output_size() const { return h.size() / 2; }
};

void calculate(const double* data, std::size_t size, std::vector<double>& output)
{
    fft fft(size);
    if(filter_input)
    {
        for(std::size_t i=0;i<size;i++) { fft.input[i] = fft.h[i] * data[i]; }
    }
    else { memcpy(fft.input, data, size * sizeof(double)); }

    fft.calculate();

    if(magnitude)
    {
        if(fft.output_size()>output.size()) { COMMA_THROW(comma::exception, "size mismatch, output "<<output.size()<<" fft output "<<fft.output_size()); }
        for(std::size_t j=0;j<fft.output_size();j++)
        {
            double a= std::abs( std::complex<double>( fft.output[j][0], fft.output[j][1] ) );
            if(logarithmic_output) { a = (a == 0) ? 0 : (std::log10(a)); }
            output[j]=a;
        }
    }
    else if(real)
    {
        if(fft.output_size()>output.size()) { COMMA_THROW(comma::exception, "size mismatch, output "<<output.size()<<" fft output "<<fft.output_size()); }
        for(std::size_t j=0;j<fft.output_size();j++)
        {
            double a= fft.output[j][0];
            if(logarithmic_output) { a = (a == 0) ? 0 : (std::log10(a)); }
            output[j]=a;
        }
    }
    else
    {
        std::size_t k=0;
        std::size_t step=2;
        std::size_t off=1;
        if(split)
        {
            step=1;
            off=output.size()/2;
        }
        if(2*fft.output_size()>output.size()) { COMMA_THROW(comma::exception, "size mismatch, output "<<output.size()<<" fft output "<<fft.output_size()); }
        for(std::size_t j=0; j<fft.output_size(); j++, k+=step)
        {
            double a= fft.output[j][0];
            if(logarithmic_output) { a = (a == 0) ? 0 : (std::log10(a)); }
            output[k]=a;
            output[k+off]=fft.output[j][1];
        }
    }
}

struct app
{
    app()
    {
    }
    void process(const comma::csv::options& csv, const boost::optional<std::string>& shuffle_fields)
    {
        if(bin_overlap && int(bin_size*(1-*bin_overlap)) <= 0) { COMMA_THROW( comma::exception, "bin size and overlap don't work" ); }
        input_t sample;
        comma::csv::input_stream<input_t> is(std::cin, csv, sample);
        comma::csv::output_stream<output_t> os(std::cout, csv.binary(), true);
        std::string array_sizes="data=";
        array_sizes+=boost::lexical_cast<std::string>(sample.data.size());
        ::shuffle_tied<input_t,output_t> shuffle_tied(is, os, csv, shuffle_fields,array_sizes);
        output_t output;
        while(std::cin.good())
        {
            //read a record
            const input_t* input=is.read();
            if(!input) { break; }
            //calculate
            for(std::size_t bin_offset=0;bin_offset<input_size;bin_offset+=(bin_overlap ? bin_size*(1-*bin_overlap) : bin_size))
            {
                output.reset();
                calculate(&input->data[bin_offset], std::min(bin_size,input_size-bin_offset), output.data);
                //write output
                if(tied)
                    shuffle_tied.append(output);
                else
                    os.write(output);
            }
        }
    }
    std::size_t get_output_size()
    {
        return output_t().data.size();
    }
    void output_format()
    {
        std::cout<<output_t().data.size()<<"d"<<std::endl;
    }
    void output_fields()
    {
        std::cout<<comma::join(comma::csv::names< output_t >(true),',')<<std::endl;
    }
};

template<typename T>
std::ostream& operator<< (std::ostream& o, const std::vector<T>& v)
{
    std::copy(v.begin(), v.end(), std::ostream_iterator<T>(o, " "));
    return o;
}
template<typename T>
void range_check(T value, T min, T max, const char* label)
{
    if(value<min || value>max) { COMMA_THROW(comma::exception, label<<" out of range "<<min<<" to "<<max ); }
}

int main( int argc, char** argv )
{
    comma::command_line_options options( argc, argv, usage );
    try
    {
        comma::csv::options csv(options,"data");
        filter_input= ! options.exists("--no-filter");
        logarithmic_output= ! options.exists("--linear");
        input_size=options.value<std::size_t>("--size");
        magnitude=options.exists("--magnitude");
        real=options.exists("--real");
        split=options.exists("--split");
        bin_size=options.value<std::size_t>("--bin-size", input_size);
        range_check<std::size_t>(bin_size,0,input_size,"bin_size");
        bin_overlap=options.optional<double>("--bin-overlap");
        if(bin_overlap) { range_check<double>(*bin_overlap,0,1,"bin_overlap"); }
        boost::optional<std::string> shuffle_fields=options.optional<std::string>("--shuffle,--shuffle-fields,--shuffled-fields");
        tied=!options.exists("--untied");
        if(shuffle_fields&&!tied) { COMMA_THROW(comma::exception,"--shuffle only works with tied stream (can't specify both --untied and --shuffle"); }
        std::vector<std::string> unnamed=options.unnamed("--verbose,-v,--output-size,--output-format,--output-fields,--no-filter,--linear,--magnitude,--real,--split,--untied",
                                                         "--binary,-b,--fields,-f,--delimiter,-d,--size,--bin-size,--bin-overlap,--shuffle,--shuffle-fields,--shuffled-fields");
        if(unnamed.size() != 0) { COMMA_THROW(comma::exception, "invalid option(s): " << unnamed ); }
        app app;
        if(options.exists("--output-size")) { std::cout<< app.get_output_size() << std::endl; return 0; }
        if(options.exists("--output-format")) { app.output_format(); return 0; }
        if(options.exists("--output-fields")) { app.output_fields(); return 0; }
        app.process(csv,shuffle_fields);
        return 0;
    }
    catch( std::exception& ex )
    {
        std::cerr << comma::verbose.app_name() << ": " << ex.what() << std::endl;
    }
    catch( ... )
    {
        std::cerr << comma::verbose.app_name() << ": " << "unknown exception" << std::endl;
    }
    return 1;
}


/**
 * \file dcs/algorithm/subset.hpp
 *
 * \brief Generate the power set of a set in lexicographic order.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 *

 *
 * Copyright 2013 Marco Guazzone (marco.guazzone@gmail.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.
 */

#ifndef DCS_COMMONS_ALGORITHM_SUBSET_HPP
#define DCS_COMMONS_ALGORITHM_SUBSET_HPP

#include <algorithm>
#include <boost/dynamic_bitset.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <cstddef>
#include <dcs/assert.hpp>
#include <dcs/debug.hpp>
#include <dcs/exception.hpp>
#include <iostream>
#include <iterator>
#include <stdexcept>
#include <vector>

namespace dcs { namespace algorithm {

/**
 * \brief Traits class for subset types
 *
 * \tparam ValueT The value type
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template <typename ValueT>
struct subset_traits
{
	typedef ValueT value_type;
	typedef ::std::vector<value_type> subset_container;///< DEPRECATED
	typedef typename subset_container::iterator subset_container_iterator;///< DEPRECATED
	typedef typename subset_container::const_iterator subset_container_const_iterator;///< DEPRECATED
	typedef ::std::vector<value_type> element_container;
	typedef typename element_container::iterator element_iterator;
	typedef typename element_container::const_iterator element_const_iterator;
}; // subset_traits

/**
 * \brief Class to generate in lexicographic order all subsets
 *
 * Given a set N={0,1,...,n-1} of n elements, this class iteratively generates
 * all subset S of N, possibly included the empty set, in lexicographic order,
 * that is, to generate a subset containing the i-th element we generate all
 * subset containing the preceding 0,1,...(i-1)-th elements.
 * For instance, for a set of 4 elements, the subset generation in lexicographic
 * order produces the following sequence:
 *

 *  \emptyset,
 *  {0},
 *  {1},
 *  {0,1},
 *  {2},
 *  {0,2},
 *  {1,2},
 *  {0,1,2},
 *  {3},
 *  {0,3},
 *  {1,3},
 *  {2,3},
 *  {0,1,3},
 *  {0,2,3}
 *  {1,2,3},
 *  {0,1,2,3}
 *

 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
class lexicographic_subset
{
	public: class const_iterator;

	friend class const_iterator;

	private: typedef lexicographic_subset self_type;
	private: typedef ::boost::dynamic_bitset<> impl_type;
	private: typedef typename impl_type::size_type size_type;
	private: typedef unsigned long word_type;

	public: explicit lexicographic_subset(::std::size_t n, bool empty_set=true)
	: n_(n),
	  empty_set_(empty_set),
	  bits_(n, empty_set ? 0 : 1),
	  has_prev_(false),
	  has_next_(n_ > 0 ? true : false)
	{
		DCS_ASSERT(n_ > 0,
				   DCS_EXCEPTION_THROW(::std::invalid_argument,
									   "Number of elements must be positive"));
	}

	public: ::std::size_t max_size() const
	{
		return n_;
	}

	public: ::std::size_t size() const
	{
		return bits_.count();
	}

	public: ::std::size_t count() const
	{
		const ::std::size_t c = 1 << n_; // 2^n

		return empty_set_ ? c : (c-1);
	}

	public: self_type& operator++()
	{
		DCS_ASSERT(has_next_,
				   DCS_EXCEPTION_THROW(::std::overflow_error,
									   "No following subsets"));

		has_next_ = bits_.count() < n_;

		if (has_next_)
		{
			bits_ = impl_type(n_, bits_.to_ulong()+1);
		}

		has_prev_ = bits_.to_ulong() > (empty_set_ ? 0 : 1);

		return *this;
	}

	public: bool has_next() const
	{
		return has_next_;
	}

	public: self_type& operator--()
	{
		DCS_ASSERT(has_prev_,
				   DCS_EXCEPTION_THROW(::std::underflow_error,
									   "No preceding subsets"));

		has_prev_ = bits_.to_ulong() > (empty_set_ ? 0 : 1);

		if (has_prev_)
		{
			bits_ = impl_type(n_, bits_.to_ulong()-1);
		}

		has_next_ = bits_.to_ulong() < n_;

		return *this;
	}

	public: bool has_prev() const
	{
		return has_prev_;
	}

	public: typename ::std::vector<size_type> operator()() const
	{
		::std::vector<size_type> subset;

		for (size_type pos = bits_.find_first();
			 pos != impl_type::npos;
			 pos = bits_.find_next(pos))
		{
			subset.push_back(pos);
		}

		return subset;
	}

	//public: template <typename ElemT, typename IterT>
	public: template <typename ElemT>
			typename subset_traits<ElemT>::element_container operator()(::std::vector<ElemT> const& v) const
	{
		DCS_ASSERT(v.size() == n_,
				   DCS_EXCEPTION_THROW(::std::invalid_argument,
									   "Size does not match"));

		typename subset_traits<ElemT>::element_container subset;

		for (size_type pos = bits_.find_first();
			 pos != impl_type::npos;
			 pos = bits_.find_next(pos))
		{
			subset.push_back(v[pos]);
		}

		return subset;
	}

	public: template <typename IterT>
			typename subset_traits< typename ::std::iterator_traits<IterT>::value_type >::element_container operator()(IterT first, IterT last) const
	{
			return this->operator()(::std::vector<typename ::std::iterator_traits<IterT>::value_type>(first, last));
	}

	public: const_iterator begin() const
	{
		return const_iterator(this, bits_.any() ? bits_.find_first() : impl_type::npos);
	}

	public: const_iterator end() const
	{
		return const_iterator(this, impl_type::npos);
	}

	public: class const_iterator: public ::std::iterator< ::std::bidirectional_iterator_tag,
														 typename lexicographic_subset::impl_type::size_type const>
	{
		private: typedef ::std::iterator< ::std::bidirectional_iterator_tag,
										  typename lexicographic_subset::impl_type::size_type const> base_type;
		private: typedef lexicographic_subset container_type;
		private: typedef typename container_type::impl_type bitset_type;
		private: typedef typename container_type::size_type size_type;
		public: typedef typename base_type::value_type value_type;
		public: typedef typename base_type::difference_type difference_type;
		public: typedef typename base_type::pointer pointer;
		public: typedef typename base_type::reference reference;
		public: typedef typename base_type::iterator_category iterator_category;

		public: const_iterator(container_type const* p_subset, size_type pos)
		: p_sub_(p_subset),
		  pos_(pos)
		{
		}

		public: reference operator*() const
		{
			return pos_;
		}

		public: pointer operator->() const
		{
			return &(operator*());
		}

		public: const_iterator& operator++()
		{
			pos_ = p_sub_->bits_.find_next(pos_);

			return *this;
		}

		public: const_iterator operator++(int)
		{
			const_iterator tmp = *this;

			operator++();

			return tmp;
		}

		public: const_iterator& operator--()
		{
			size_type pos = bitset_type::npos;
			for (size_type pos2 = p_sub_->bits_.find_first();
				 pos2 != pos_;
				 pos2 = p_sub_->bits_.find_next(pos2))
			{
				pos = pos2;
			}

			pos_ = pos;

			return *this;
		}

		public: const_iterator operator--(int)
		{
			const_iterator tmp = *this;

			operator--();

			return tmp;
		}

		public: const_iterator operator+(difference_type n) const
		{
			const_iterator it = *this;

			if (n > 0)
			{
				while (n--)
				{
					++it;
				}
			}
			else
			{
				while (n++)
				{
					--it;
				}
			}

			return it;
		}

		public: const_iterator operator-(difference_type n) const
		{
			return operator+(-n);
		}

		public: const_iterator& operator+=(difference_type n)
		{
			if (n > 0)
			{
				while (n--)
				{
					operator++();
				}
			}
			else
			{
				while (n++)
				{
					operator--();
				}
			}

			return *this;
		}

		public: const_iterator& operator-=(difference_type n)
		{
			return operator+=(-n);
		}

		friend
		bool operator==(const_iterator const& lhs, const_iterator const& rhs)
		{
			return lhs.p_sub_ == rhs.p_sub_ && lhs.pos_ == rhs.pos_;
		}

		friend
		bool operator!=(const_iterator const& lhs, const_iterator const& rhs)
		{
			return !(lhs == rhs);
		}

		private: container_type const* p_sub_;
		private: size_type pos_;
	}; // const_iterator

	private: ::std::size_t n_; ///< The max number of elements
	private: bool empty_set_; ///< Flag to enable or disable the inclusion of the empty set
	private: impl_type bits_; ///< The subset implementation
	private: bool has_prev_;
	private: bool has_next_;
}; // lexicographic_subset

/**
 * \brief Class to generate in lexicographic order all subsets of a specific size
 *
 * Given a set N={0,1,...,n-1} of n elements, this class iteratively generates
 * all subset S of N of size 0<=k<=n, in lexicographic order.
 * For instance, for a set of 4 elements, the generation of subset of size 2 in
 * lexicographic order produces the following sequence:
 *

 *  \emptyset,
 *  {0,1},
 *  {0,2},
 *  {1,2},
 *  {0,3},
 *  {1,3},
 *  {2,3}
 *

 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
class lexicographic_k_subset
{
	public: class const_iterator;

	friend class const_iterator;

	private: typedef lexicographic_k_subset self_type;
	private: typedef ::boost::dynamic_bitset<> impl_type;
	private: typedef typename impl_type::size_type size_type;
	private: typedef unsigned long word_type;

	public: explicit lexicographic_k_subset(::std::size_t n, ::std::size_t k)
	: n_(n),
	  k_(k),
	  bits_(n, (1 << k_)-1),
	  has_prev_(false),
	  has_next_(n_ > 0 && k_ > 0 ? true : false)
	{
		DCS_ASSERT(n_ > 0,
				   DCS_EXCEPTION_THROW(::std::invalid_argument,
									   "Number of elements must be positive"));
		DCS_ASSERT(n_ >= k,
				   DCS_EXCEPTION_THROW(::std::invalid_argument,
									   "Size of subset must be non negative"));
	}

	public: ::std::size_t max_size() const
	{
		return k_;
	}

	public: ::std::size_t size() const
	{
		return bits_.count();
	}

	public: ::std::size_t count() const
	{
		return static_cast<std::size_t>(boost::math::binomial_coefficient<double>(n_, k_));
	}

	public: self_type& operator++()
	{
		DCS_ASSERT(has_next_,
				   DCS_EXCEPTION_THROW(::std::overflow_error,
									   "No following subsets"));

		if (k_ > 0)
		{
			unsigned long v = bits_.to_ulong();

			const unsigned long lo = v & ~(v - 1);       // lowest one bit
			const unsigned long lz = (v + lo) & ~v;      // lowest zero bit above lo
			v |= lz;                     // add lz to the set
			v &= ~(lz - 1);              // reset bits below lz
			v |= (lz / lo / 2) - 1;      // put back right number of bits at end

			has_next_ = !(v & 1 << n_);

			if (has_next_)
			{
				bits_ = impl_type(n_, v);
			}

			has_prev_ = (bits_.to_ulong() != static_cast<unsigned long>((1 << k_)-1));
		}

		return *this;
	}

	public: bool has_next() const
	{
		return has_next_;
	}

	public: self_type& operator--()
	{
		DCS_ASSERT(has_prev_,
				   DCS_EXCEPTION_THROW(::std::underflow_error,
									   "No preceding subsets"));

		if (k_ > 0)
		{
			const unsigned long w = bits_.to_ulong();
			const unsigned long min = (1 << k_)-1;
			unsigned long v = min;
			unsigned long p = v;

			while (v != w)
			{
				p = v;

				const unsigned long lo = v & ~(v - 1);       // lowest one bit
				const unsigned long lz = (v + lo) & ~v;      // lowest zero bit above lo
				v |= lz;                     // add lz to the set
				v &= ~(lz - 1);              // reset bits below lz
				v |= (lz / lo / 2) - 1;      // put back right number of bits at end
			}

			has_prev_ = (p != min) || (w != min);

//			if (has_prev_)
//			{
				bits_ = impl_type(n_, p);
//			}

			has_next_ = !(p & 1 << n_);
		}

		return *this;
	}

	public: bool has_prev() const
	{
		return has_prev_;
	}

	//public: template <typename ElemT, typename IterT>
	public: template <typename ElemT>
			typename subset_traits<ElemT>::element_container operator()(::std::vector<ElemT> const& v) const
	{
		DCS_ASSERT(v.size() == n_,
				   DCS_EXCEPTION_THROW(::std::invalid_argument,
									   "Size does not match"));

		typename subset_traits<ElemT>::element_container subset;

		for (size_type pos = bits_.find_first();
			 pos != impl_type::npos;
			 pos = bits_.find_next(pos))
		{
			subset.push_back(v[pos]);
		}

		return subset;
	}

	public: template <typename IterT>
			typename subset_traits< typename ::std::iterator_traits<IterT>::value_type >::element_container operator()(IterT first, IterT last) const
	{
			return this->operator()(::std::vector<typename ::std::iterator_traits<IterT>::value_type>(first, last));
	}

	public: const_iterator begin() const
	{
		return const_iterator(this, bits_.any() ? bits_.find_first() : impl_type::npos);
	}

	public: const_iterator end() const
	{
		return const_iterator(this, impl_type::npos);
	}

	public: class const_iterator: public ::std::iterator< ::std::bidirectional_iterator_tag,
														 typename lexicographic_k_subset::impl_type::size_type const>
	{
		private: typedef ::std::iterator< ::std::bidirectional_iterator_tag,
										  typename lexicographic_k_subset::impl_type::size_type const> base_type;
		private: typedef lexicographic_k_subset container_type;
		private: typedef typename container_type::impl_type bitset_type;
		private: typedef typename container_type::size_type size_type;
		public: typedef typename base_type::value_type value_type;
		public: typedef typename base_type::difference_type difference_type;
		public: typedef typename base_type::pointer pointer;
		public: typedef typename base_type::reference reference;
		public: typedef typename base_type::iterator_category iterator_category;

		public: const_iterator(container_type const* p_subset, size_type pos)
		: p_sub_(p_subset),
		  pos_(pos)
		{
		}

		public: reference operator*() const
		{
			return pos_;
		}

		public: pointer operator->() const
		{
			return &(operator*());
		}

		public: const_iterator& operator++()
		{
			pos_ = p_sub_->bits_.find_next(pos_);

			return *this;
		}

		public: const_iterator operator++(int)
		{
			const_iterator tmp = *this;

			operator++();

			return tmp;
		}

		public: const_iterator& operator--()
		{
			size_type pos = bitset_type::npos;
			for (size_type pos2 = p_sub_->bits_.find_first();
				 pos2 != pos_;
				 pos2 = p_sub_->bits_.find_next(pos2))
			{
				pos = pos2;
			}

			pos_ = pos;

			return *this;
		}

		public: const_iterator operator--(int)
		{
			const_iterator tmp = *this;

			operator--();

			return tmp;
		}

		public: const_iterator operator+(difference_type n) const
		{
			const_iterator it = *this;

			if (n > 0)
			{
				while (n--)
				{
					++it;
				}
			}
			else
			{
				while (n++)
				{
					--it;
				}
			}

			return it;
		}

		public: const_iterator operator-(difference_type n) const
		{
			return operator+(-n);
		}

		public: const_iterator& operator+=(difference_type n)
		{
			if (n > 0)
			{
				while (n--)
				{
					operator++();
				}
			}
			else
			{
				while (n++)
				{
					operator--();
				}
			}

			return *this;
		}

		public: const_iterator& operator-=(difference_type n)
		{
			return operator+=(-n);
		}

		friend
		bool operator==(const_iterator const& lhs, const_iterator const& rhs)
		{
			return lhs.p_sub_ == rhs.p_sub_ && lhs.pos_ == rhs.pos_;
		}

		friend
		bool operator!=(const_iterator const& lhs, const_iterator const& rhs)
		{
			return !(lhs == rhs);
		}

		private: container_type const* p_sub_;
		private: size_type pos_;
	}; // const_iterator

	private: ::std::size_t n_; ///< The number of elements of the set
	private: ::std::size_t k_; ///< The max number of elements of the subset
	private: impl_type bits_; ///< The subset implementation
	private: bool has_prev_;
	private: bool has_next_;
}; // lexicographic_k_subset

template <typename CharT, typename CharTraitsT>
::std::basic_ostream<CharT,CharTraitsT>& operator<<(::std::basic_ostream<CharT,CharTraitsT>& os,
													lexicographic_subset const& subset)
{
	os << '(';

	if (subset.size() > 0)
	{
		if (subset.size() > 1)
		{
			::std::copy(subset.begin(),
						subset.end()-1,
						::std::ostream_iterator< ::std::size_t >(os, " "));
		}

		os << *(subset.end()-1);
	}

	os << ')';

	return os;
}

template <typename CharT, typename CharTraitsT>
::std::basic_ostream<CharT,CharTraitsT>& operator<<(::std::basic_ostream<CharT,CharTraitsT>& os,
													lexicographic_k_subset const& subset)
{
	os << '(';

	if (subset.size() > 0)
	{
		if (subset.size() > 1)
		{
			::std::copy(subset.begin(),
						subset.end()-1,
						::std::ostream_iterator< ::std::size_t >(os, " "));
		}

		os << *(subset.end()-1);
	}

	os << ')';

	return os;
}

template <typename BidiIterT, typename SubsetT>
inline
typename subset_traits< typename ::std::iterator_traits<BidiIterT>::value_type >::element_container
next_subset(BidiIterT first,
			BidiIterT last,
			SubsetT& subset)
{
	typedef typename ::std::iterator_traits<BidiIterT>::value_type value_type;
	typedef typename subset_traits<value_type>::element_container element_container;

	element_container subs = subset(first, last);

	++subset;

	return subs;
}

template <typename BidiIterT, typename SubsetT>
inline
typename subset_traits< typename ::std::iterator_traits<BidiIterT>::value_type >::element_container
prev_subset(BidiIterT first,
			BidiIterT last,
			SubsetT& subset)
{
	typedef typename ::std::iterator_traits<BidiIterT>::value_type value_type;
	typedef typename subset_traits<value_type>::element_container element_container;

	element_container subs = subset(first, last);

	--subset;

	return subs;
}

/*
inline
::std::size_t count_subsets(n)
{
	const ::std::size_t m = n >> 1;
	const bool flag = ((m << 1) - n) > 0;

	::std::size_t c = 0;
	for (std::size_t i = 0; i < m; ++i)
	{
		c +=
	}
}
*/

}} // Namespace dcs::algorithm

#endif // DCS_COMMONS_ALGORITHM_SUBSET_HPP


///////////////////////////////////////////////////////////////////////////////
/// \file numeric.hpp
/// Defined the mathematical operator s on time series
//
//  Copyright 2006 Eric Niebler. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_TIME_SERIES_NUMERIC_NUMERIC_EAN_04_27_2006
#define BOOST_TIME_SERIES_NUMERIC_NUMERIC_EAN_04_27_2006

#include <utility>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/numeric/functional.hpp>
#include <boost/range_run_storage/algorithm/transform.hpp>
#include <boost/time_series/time_series_fwd.hpp>
#include <boost/time_series/concepts.hpp>
#include <boost/time_series/traits/generate_series.hpp>
#include <boost/time_series/traits/promotion.hpp>
#include <boost/time_series/scaled_series.hpp>
#include <boost/time_series/numeric/detail/zero.hpp>
#include <boost/detail/construct.hpp>

namespace boost { namespace time_series
{
    namespace detail
    {
        namespace rrs = range_run_storage;

        using concepts::Run;
        using concepts::TimeSeries;
        using concepts::Mutable_TimeSeries;

        template<typename Out, typename Left, typename Right, typename BinOp>
        Out &run_bin_op_union(Out &out, Left &left, Right &right, BinOp const &binop)
        {
            typename Mutable_TimeSeries<Out>::ordered_inserter_type
                o(rrs::ordered_inserter(out));

            rrs::commit(
                rrs::transform(left, right, binop, o)
            );

            return out;
        }

        // For operations that are only defined
        template<typename Out, typename Left, typename Right, typename BinOp>
        Out &run_bin_op_intersection(Out &out, Left &left, Right &right, BinOp const &binop)
        {
            typename Mutable_TimeSeries<Out>::ordered_inserter_type
                o(rrs::ordered_inserter(out));

            rrs::commit(
                rrs::transform(left, right, binop, rrs::skip, rrs::skip, o)
            );

            return out;
        }

        // operation traits
        template<typename S1, typename S2, typename Gen, typename Op, typename Disc>
        struct operation_traits_base
        {
            typedef typename TimeSeries<S1>::storage_category storage1_tag;
            typedef typename TimeSeries<S2>::storage_category storage2_tag;
            typedef typename mpl::apply<Gen, storage1_tag, storage2_tag>::type storage_category;

            typedef typename TimeSeries<S1>::value_type value1_type;
            typedef typename TimeSeries<S2>::value_type value2_type;
            typedef typename result_of<Op(value1_type, value2_type)>::type value_type;

            typedef typename TimeSeries<S1>::offset_type offset1_type;
            typedef typename TimeSeries<S2>::offset_type offset2_type;
            BOOST_MPL_ASSERT((is_same<offset1_type, offset2_type>)); // is there a better option?
            typedef offset1_type offset_type;

            typedef typename traits::generate_series<
                storage_category, value_type, Disc, offset_type
            >::type result_type;
        };

        template<typename S1, typename S2, typename Gen, typename Op
          , typename Disc1 = typename TimeSeries<S1>::discretization_type
          , typename Disc2 = typename TimeSeries<S2>::discretization_type
        >
        struct operation_traits
        {};

        template<typename S1, typename S2, typename Gen, typename Op, typename Disc>
        struct operation_traits<S1, S2, Gen, Op, time_series::any, Disc>
          : operation_traits_base<S1, S2, Gen, Op, Disc>
        {};

        template<typename S1, typename S2, typename Gen, typename Op, typename Disc>
        struct operation_traits<S1, S2, Gen, Op, Disc, time_series::any>
          : operation_traits_base<S1, S2, Gen, Op, Disc>
        {};

        template<typename S1, typename S2, typename Gen, typename Op>
        struct operation_traits<S1, S2, Gen, Op, time_series::any, time_series::any>
          : operation_traits_base<S1, S2, Gen, Op, time_series::any>
        {};

        template<typename S1, typename S2, typename Gen, typename Op, typename Disc>
        struct operation_traits<S1, S2, Gen, Op, Disc, Disc>
          : operation_traits_base<S1, S2, Gen, Op, Disc>
        {};

        template<typename Left, typename Right, typename Op>
        typename result_of<Op(
            typename TimeSeries<Left>::value_type const &
          , typename TimeSeries<Right>::value_type const &
        )>::type
        make_zero(Left &left, Right &right, Op op)
        {
            typename TimeSeries<Left>::value_type const &left_zero = rrs::zero(left);
            typename TimeSeries<Right>::value_type const &right_zero = rrs::zero(right);
            return op(left_zero, right_zero);
        }

        template<typename Discretization>
        Discretization make_discretization(Discretization left, Discretization right)
        {
            BOOST_ASSERT(left == right);
            return left;
        }

        template<typename Discretization>
        Discretization make_discretization(Discretization left, time_series::any)
        {
            return left;
        }

        template<typename Discretization>
        Discretization make_discretization(time_series::any, Discretization right)
        {
            return right;
        }

        inline time_series::any make_discretization(time_series::any, time_series::any)
        {
            return time_series::any();
        }

    } // namespace detail

    using mpl::_;

    ///////////////////////////////////////////////////////////////////////////
    // operator +
    template<typename Left, typename Right>
    typename detail::operation_traits<Left, Right, traits::plus<_, _>, numeric::op::plus>::result_type
    operator +(time_series_base<Left> const &left, time_series_base<Right> const &right)
    {
        typedef detail::operation_traits<Left, Right, traits::plus<_, _>, numeric::op::plus> op_traits;
        typedef typename op_traits::result_type result_type;

        result_type result(constructors::construct<result_type>(
            time_series::discretization
              = detail::make_discretization(left.cast().discretization(), right.cast().discretization())
          , time_series::zero
              = detail::make_zero(left.cast(), right.cast(), numeric::plus)
        ));

        return detail::run_bin_op_union(result, left.cast(), right.cast(), numeric::plus);
    }

    ///////////////////////////////////////////////////////////////////////////
    // operator -
    template<typename Left, typename Right>
    typename detail::operation_traits<Left, Right, traits::minus<_, _>, numeric::op::minus>::result_type
    operator -(time_series_base<Left> const &left, time_series_base<Right> const &right)
    {
        typedef detail::operation_traits<Left, Right, traits::minus<_, _>, numeric::op::minus> op_traits;
        typedef typename op_traits::result_type result_type;

        result_type result(constructors::construct<result_type>(
            time_series::discretization
              = detail::make_discretization(left.cast().discretization(), right.cast().discretization())
          , time_series::zero
              = detail::make_zero(left.cast(), right.cast(), numeric::minus)
        ));

        return detail::run_bin_op_union(result, left.cast(), right.cast(), numeric::minus);
    }

    ///////////////////////////////////////////////////////////////////////////
    // operator *
    template<typename Left, typename Right>
    typename detail::operation_traits<Left, Right, traits::multiplies<_, _>, numeric::op::multiplies>::result_type
    operator *(time_series_base<Left> const &left, time_series_base<Right> const &right)
    {
        typedef detail::operation_traits<Left, Right, traits::multiplies<_, _>, numeric::op::multiplies> op_traits;
        typedef typename op_traits::result_type result_type;

        result_type result(constructors::construct<result_type>(
            time_series::discretization
              = detail::make_discretization(left.cast().discretization(), right.cast().discretization())
          , time_series::zero
              = detail::make_zero(left.cast(), right.cast(), numeric::multiplies)
        ));

        return detail::run_bin_op_intersection(result, left.cast(), right.cast(), numeric::multiplies);
    }

    ///////////////////////////////////////////////////////////////////////////
    // operator /
    template<typename Left, typename Right>
    typename detail::operation_traits<Left, Right, traits::divides<_, _>, numeric::op::divides>::result_type
    operator /(time_series_base<Left> const &left, time_series_base<Right> const &right)
    {
        typedef detail::operation_traits<Left, Right, traits::divides<_, _>, numeric::op::divides> op_traits;
        typedef typename op_traits::result_type result_type;

        // BUGBUG possible divide by zero, so use multiplies instead of divides here ...
        result_type result(constructors::construct<result_type>(
            time_series::discretization
              = detail::make_discretization(left.cast().discretization(), right.cast().discretization())
          , time_series::zero
              = detail::make_zero(left.cast(), right.cast(), numeric::multiplies)
        ));

        return detail::run_bin_op_intersection(result, left.cast(), right.cast(), numeric::divides);
    }

    ///////////////////////////////////////////////////////////////////////////
    // operator +=
    template<typename Left, typename Right>
    Left &operator +=(time_series_base<Left> &left, time_series_base<Right> const &right)
    {
        BOOST_MPL_ASSERT((is_same<
            typename concepts::TimeSeries<Left>::discretization_type
          , typename concepts::TimeSeries<Right>::discretization_type
        >));

        // TODO also add the series zero
        BOOST_ASSERT(left.cast().discretization() == right.cast().discretization());
        return detail::run_bin_op_union(left.cast(), left.cast(), right.cast(), numeric::plus);
    }

    ///////////////////////////////////////////////////////////////////////////
    // operator -=
    template<typename Left, typename Right>
    Left &operator -=(time_series_base<Left> &left, time_series_base<Right> const &right)
    {
        BOOST_MPL_ASSERT((is_same<
            typename concepts::TimeSeries<Left>::discretization_type
          , typename concepts::TimeSeries<Right>::discretization_type
        >));

        // TODO also subtract the series zero
        BOOST_ASSERT(left.cast().discretization() == right.cast().discretization());
        return detail::run_bin_op_union(left.cast(), left.cast(), right.cast(), numeric::minus);
    }

    ///////////////////////////////////////////////////////////////////////////
    // operator *=
    template<typename Left, typename Right>
    Left &operator *=(time_series_base<Left> &left, time_series_base<Right> const &right)
    {
        BOOST_MPL_ASSERT((is_same<
            typename concepts::TimeSeries<Left>::discretization_type
          , typename concepts::TimeSeries<Right>::discretization_type
        >));

        BOOST_ASSERT(left.cast().discretization() == right.cast().discretization());

        // BUGBUG not optimal
        Left result(constructors::construct<Left>(
            time_series::discretization
              = detail::make_discretization(left.cast().discretization(), right.cast().discretization())
          , time_series::zero
              = detail::make_zero(left.cast(), right.cast(), numeric::multiplies)
        ));

        left.cast().swap(detail::run_bin_op_intersection(result, left.cast(), right.cast(), numeric::multiplies));
        return left.cast();
    }

    ///////////////////////////////////////////////////////////////////////////
    // operator /=
    template<typename Left, typename Right>
    Left &operator /=(time_series_base<Left> &left, time_series_base<Right> const &right)
    {
        BOOST_MPL_ASSERT((is_same<
            typename concepts::TimeSeries<Left>::discretization_type
          , typename concepts::TimeSeries<Right>::discretization_type
        >));

        BOOST_MPL_ASSERT((traits::is_valid_divisor<
            typename concepts::TimeSeries<Right>::storage_category
        >));

        BOOST_ASSERT(left.cast().discretization() == right.cast().discretization());

        // BUGBUG not optimal
        // BUGBUG possible divide by zero, so use multiplies instead of divides here ...
        Left result(constructors::construct<Left>(
            time_series::discretization
              = detail::make_discretization(left.cast().discretization(), right.cast().discretization())
          , time_series::zero
              = detail::make_zero(left.cast(), right.cast(), numeric::multiplies)
        ));

        left.cast().swap(detail::run_bin_op_intersection(result, left.cast(), right.cast(), numeric::divides));
        return left.cast();
    }

    ///////////////////////////////////////////////////////////////////////////
    // operator * (with scalar)
    template<typename Series, typename Factor>
    typename disable_if<
        traits::is_time_series<Factor>
      , scaled_series<Series const, Factor> const
    >::type
    operator *(time_series_base<Series> const &left, Factor const &right)
    {
        return scaled_series<Series const, Factor>(left.cast(), right);
    }

    ///////////////////////////////////////////////////////////////////////////
    // operator * (with scalar)
    template<typename Factor, typename Series>
    typename disable_if<
        traits::is_time_series<Factor>
      , scaled_series<Series const, Factor> const
    >::type
    operator *(Factor const &left, time_series_base<Series> const &right)
    {
        return scaled_series<Series const, Factor>(right.cast(), left);
    }

    ///////////////////////////////////////////////////////////////////////////
    // operator *= (with scalar)
    template<typename Series, typename Factor>
    typename disable_if<
        traits::is_time_series<Factor>
      , Series &
    >::type
    operator *=(time_series_base<Series> &left, Factor const &right)
    {
        left.cast() = left.cast() * right;
        return left.cast();
    }

}} // namespace boost::time_series

#endif


#include "lenslet_graph.h"

#include <Eigen/Dense>

namespace lightfields {

namespace {

struct Vec2Compare {
	bool operator()(const cv::Vec2f& v1, const cv::Vec2f& v2) const {
		if(v1[0] != v2[0])
			return v1[0] < v2[0];
		return v1[1] < v2[1];
	}
};

struct Lenslet {
	Lenslet(const cv::Vec2f& c) : center(c), neighbourCount(0), id(INT_MIN, INT_MIN) {
	}

	void addNeighbour(std::size_t index) {
		if(neighbourCount >= 6)
			throw std::runtime_error(
			    "Lenslets should be organized in a hexagonal grid, which does not seem to be the case!");

		neighbours[neighbourCount] = index;
		++neighbourCount;
	}

	cv::Vec2f center;
	std::array<std::size_t, 6> neighbours;
	unsigned char neighbourCount;
	cv::Vec2i id;
};

}  // namespace

/////////////

struct LensletGraph::Pimpl {
	Pimpl() : fitted(false) {
	}

	Imath::V2i sensorSize;
	int exclusionBorder;

	std::vector<Lenslet> lenslets;

	cv::Matx<double, 3, 3> fittedMatrix;
	bool fitted;
};

/////////////

LensletGraph::LensletGraph(Imath::V2i sensorSize, int exclusionBorder) : m_pimpl(new Pimpl()) {
	m_pimpl->sensorSize = sensorSize;
	m_pimpl->exclusionBorder = exclusionBorder;
}

LensletGraph::~LensletGraph() {
}

void LensletGraph::addLenslet(const cv::Vec2f& center) {
	m_pimpl->lenslets.push_back(Lenslet(center));
}

namespace {

/// computes relative integer offset between two lenslets (assumed to be neighbouring edges of a graph)
cv::Vec2i offset(const cv::Vec2f& l1, const cv::Vec2f& l2) {
	const float angle = atan2(l2[1] - l1[1], l2[0] - l1[0]) / M_PI * 6.0f;
	assert(angle >= -6.0f && angle <= 6.0f);

	if(angle < -5.0f || angle > 5.0f)
		return cv::Vec2i(-1, 0);

	if(angle < -3.0f)
		return cv::Vec2i(0, -1);

	if(angle > 3.0f)
		return cv::Vec2i(-1, 1);

	if(angle < -1.0f)
		return cv::Vec2i(1, -1);

	if(angle > 1.0f)
		return cv::Vec2i(0, 1);

	return cv::Vec2i(1, 0);
}

}  // namespace

void LensletGraph::fit() {
	assert(!m_pimpl->fitted);

	// build the subdiv and index
	std::map<cv::Vec2f, std::size_t, Vec2Compare> index;

	cv::Subdiv2D subdiv(cv::Rect(0, 0, m_pimpl->sensorSize[0], m_pimpl->sensorSize[1]));
	for(auto& l : m_pimpl->lenslets) {
		subdiv.insert(cv::Point2f(l.center[0], l.center[1]));
		index.insert(std::make_pair(cv::Point2f(l.center[0], l.center[1]), index.size()));
	}

	// collect and process the edges
	{
		std::vector<cv::Vec4f> srcEdgeList;
		subdiv.getEdgeList(srcEdgeList);

		for(auto eit = srcEdgeList.begin(); eit != srcEdgeList.end(); ++eit) {
			auto& e = *eit;

			if(e[0] > m_pimpl->exclusionBorder && e[0] < m_pimpl->sensorSize[0] - m_pimpl->exclusionBorder &&
			   e[1] > m_pimpl->exclusionBorder && e[1] < m_pimpl->sensorSize[1] - m_pimpl->exclusionBorder &&
			   e[2] > m_pimpl->exclusionBorder && e[2] < m_pimpl->sensorSize[0] - m_pimpl->exclusionBorder &&
			   e[3] > m_pimpl->exclusionBorder && e[3] < m_pimpl->sensorSize[1] - m_pimpl->exclusionBorder) {
				auto it1 = index.find(cv::Point2f(e[0], e[1]));
				auto it2 = index.find(cv::Point2f(e[2], e[3]));
				assert(it1 != index.end() && it2 != index.end());

				m_pimpl->lenslets[it1->second].addNeighbour(it2->second);
				m_pimpl->lenslets[it2->second].addNeighbour(it1->second);
			}
		}
	}

	// propagate IDs
	{
		// find first connected lenslet
		auto it = m_pimpl->lenslets.begin();
		while(it != m_pimpl->lenslets.end() && it->neighbourCount == 0)
			++it;

		if(it != m_pimpl->lenslets.end()) {
			// this lenslet is the "origin" now
			it->id = cv::Vec2i(0, 0);

			// initialise the edges (to-be-processed) list
			std::vector<std::pair<std::size_t, std::size_t>> edges;
			for(std::size_t n = 0; n < it->neighbourCount; ++n)
				edges.push_back(std::make_pair(it - m_pimpl->lenslets.begin(), it->neighbours[n]));

			while(!edges.empty()) {
				// get the processed edge
				auto edge = edges.back();
				edges.pop_back();

				const Lenslet& source = m_pimpl->lenslets[edge.first];
				Lenslet& target = m_pimpl->lenslets[edge.second];

				// assert the "origin" lenslet, which should have been processed already
				assert(source.id[0] > INT_MIN && source.id[1] > INT_MIN);

				// get the "offset" based on edge direction
				const cv::Vec2i off = offset(source.center, target.center);

				// either set the target, or assert its value is right
				if(target.id[0] > INT_MIN) {
					assert(target.id[0] == source.id[0] + off[0]);
					assert(target.id[1] == source.id[1] + off[1]);
				}

				else {
					target.id[0] = source.id[0] + off[0];
					target.id[1] = source.id[1] + off[1];

					// recursively continue around all edges of the target
					for(std::size_t n = 0; n < target.neighbourCount; ++n)
						edges.push_back(std::make_pair(edge.second, target.neighbours[n]));
				}
			}
		}
	}

	// least squares fit
	// Ref: Fitting a Transformation: Feature-based Alignment, Kristen Grauman, lecture notes for UT Austin
	// A = [ ...
	//       xi, yi,  0,  0, 1, 0,
	//        0,  0, xi, yi, 0, 1,
	//       ... ]
	// x = [m1, m2, m3, m4, t1, t2]^T
	// b = [ ... x'i, y'i, ... ]^T

	// count the useful lenslets
	std::size_t lensletCount = 0;
	for(auto& l : m_pimpl->lenslets)
		if(l.id[0] > INT_MIN)
			++lensletCount;

	Eigen::MatrixXd A(2 * lensletCount, 6);
	Eigen::VectorXd b(2 * lensletCount);
	lensletCount = 0;
	for(auto& l : m_pimpl->lenslets)
		if(l.id[0] > INT_MIN) {
			std::size_t rowId = lensletCount * 2;

			A(rowId, 0) = l.center[0];
			A(rowId, 1) = l.center[1];
			A(rowId, 2) = 0;
			A(rowId, 3) = 0;
			A(rowId, 4) = 1;
			A(rowId, 5) = 0;

			b(rowId) = l.id[0];

			++rowId;

			A(rowId, 0) = 0;
			A(rowId, 1) = 0;
			A(rowId, 2) = l.center[0];
			A(rowId, 3) = l.center[1];
			A(rowId, 4) = 0;
			A(rowId, 5) = 1;

			b(rowId) = l.id[1];

			++lensletCount;
		}

	Eigen::VectorXd x = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
	assert(x.size() == 6);

	m_pimpl->fitted = true;
	m_pimpl->fittedMatrix = cv::Matx<double, 3, 3>(x[0], x[2], 0, x[1], x[3], 0, x[4], x[5], 1);
}

double LensletGraph::lensPitch() const {
	std::size_t counter = 0;
	double average = 0.0;

	for(auto& l : m_pimpl->lenslets)
		if(l.id[0] > INT_MIN) {
			for(unsigned char ni = 0; ni < l.neighbourCount; ++ni) {
				const Lenslet& neighbour = m_pimpl->lenslets[l.neighbours[ni]];
				if(neighbour.id[0] > INT_MIN) {
					++counter;
					average += cv::norm(l.center - neighbour.center);
				}
			}
		}

	assert(counter > 0);

	return average / (double)counter;
}

const Imath::V2i& LensletGraph::sensorResolution() const {
	return m_pimpl->sensorSize;
}

const cv::Matx<double, 3, 3>& LensletGraph::fittedMatrix() const {
	assert(m_pimpl->fitted);
	return m_pimpl->fittedMatrix;
}

void LensletGraph::drawCenters(cv::Mat& target) const {
	assert(target.rows == m_pimpl->sensorSize[0] && target.cols == m_pimpl->sensorSize[1]);
	assert(target.type() == CV_8UC1);

	for(auto& l : m_pimpl->lenslets)
		target.at<unsigned char>(l.center[1], l.center[0]) = 255;
}

void LensletGraph::drawEdges(cv::Mat& target) const {
	assert(target.rows == m_pimpl->sensorSize[0] && target.cols == m_pimpl->sensorSize[1]);
	assert(target.type() == CV_8UC1);

	for(auto& l1 : m_pimpl->lenslets) {
		for(unsigned char n = 0; n < l1.neighbourCount; ++n) {
			auto& l2 = m_pimpl->lenslets[l1.neighbours[n]];

			const float a = atan((l2.center[1] - l1.center[1]) / (l2.center[0] - l1.center[0])) / M_PI * 2.0;

			float color = 155.0;
			if(a < -0.33)
				color = 55.0;
			if(a > 0.33)
				color = 255;

			cv::line(target, cv::Point2i(l1.center[0], l1.center[1]), cv::Point2i(l2.center[0], l2.center[1]),
			         cv::Scalar(color));
		}
	}
}

void LensletGraph::drawFit(cv::Mat& target) const {
	assert(target.rows == m_pimpl->sensorSize[0] && target.cols == m_pimpl->sensorSize[1]);
	assert(target.type() == CV_8UC1);

	for(auto& l : m_pimpl->lenslets)
		cv::circle(target, cv::Point(l.center[0], l.center[1]), 2, (l.id[0] + 2560) % 256, 2);
}

}  // namespace lightfields


#include <memory>
#include <Eigen/MatrixFunctions>
#include "Basis.hh"
#include "Dispatcher.hh"
#include "io/manipulators.hh"

void Basis::twoElectron(const Eigen::MatrixXd& D, Eigen::MatrixXd& G) const
{
	if (!_status.test(ELEC_REP_CURRENT))
		calcElectronRepulsion();

	int n = D.rows();
#ifdef DEBUG
	if (D.cols() != n)
		throw Li::Exception("Density matrix is not square");
#endif
	G.resize(n, n);
	G.setZero();
	int idx = 0;
	double e;
	for (int i = 0; i < n; ++i)
	{
		double Dii = D(i,i);
		for (int j = 0; j < i; ++j)
		{
			double Dij = D(i,j);
			double Djj = D(j,j);
			for (int k = 0; k < j; ++k)
			{
				double Dik = D(i,k);
				double Djk = D(j,k);
				for (int l = 0; l < k; ++l)
				{
					e = _elec_rep(idx++);
					G(i,j) += 2 * D(k,l) * e;
					G(i,k) -= 0.5 * D(j,l) * e;
					G(i,l) -= 0.5 * Djk * e;
					G(j,k) -= 0.5 * D(i,l) * e;
					G(j,l) -= 0.5 * Dik * e;
					G(k,l) += 2 * Dij * e;
				}
				e = _elec_rep(idx++);
				G(i,j) += D(k,k) * e;
				G(i,k) -= 0.5 * Djk * e;
				G(j,k) -= 0.5 * Dik * e;
				G(k,k) += 2 * Dij * e;
			}
			for (int l = 0; l < j; ++l)
			{
				e = _elec_rep(idx++);
				G(i,j) += 1.5 * D(j,l) * e;
				G(i,l) -= 0.5 * Djj * e;
				G(j,j) -= D(i,l) * e;
				G(j,l) += 1.5 * Dij * e;
			}
			e = _elec_rep(idx++);
			G(i,j) += 0.5 * Djj * e;
			G(j,j) += Dij * e;
			for (int k = j+1; k < i; ++k)
			{
				double Dik = D(i,k);
				double Dkj = D(k,j);
				for (int l = 0; l < j; ++l)
				{
					e = _elec_rep(idx++);
					G(i,j) += 2 * D(k,l) * e;
					G(i,k) -= 0.5 * D(j,l) * e;
					G(i,l) -= 0.5 * Dkj * e;
					G(j,l) -= 0.5 * Dik * e;
					G(k,j) -= 0.5 * D(i,l) * e;
					G(k,l) += 2 * Dij * e;
				}
				e = _elec_rep(idx++);
				G(i,j) += 1.5 * Dkj * e;
				G(i,k) -= 0.5 * Djj * e;
				G(j,j) -= Dik * e;
				G(k,j) += 1.5 * Dij * e;
				for (int l = j+1; l < k; ++l)
				{
					e = _elec_rep(idx++);
					G(i,j) += 2 * D(k,l) * e;
					G(i,k) -= 0.5 * D(l,j) * e;
					G(i,l) -= 0.5 * Dkj * e;
					G(k,l) += 2 * Dij * e;
					G(k,j) -= 0.5 * D(i,l) * e;
					G(l,j) -= 0.5 * Dik * e;
				}
				e = _elec_rep(idx++);
				G(i,j) += D(k,k) * e;
				G(i,k) -= 0.5 * Dkj * e;
				G(k,j) -= 0.5 * Dik * e;
				G(k,k) += 2 * Dij * e;
			}
			for (int l = 0; l < j; ++l)
			{
				e = _elec_rep(idx++);
				G(i,i) -= D(j,l) * e;
				G(i,j) += 1.5 * D(i,l) * e;
				G(i,l) += 1.5 * Dij * e;
				G(j,l) -= 0.5 * Dii * e;
			}
			e = _elec_rep(idx++);
			G(i,i) -= 0.5 * Djj * e;
			G(i,j) += 1.5 * Dij * e;
			G(j,j) -= 0.5 * Dii * e;
		}

		for (int k = 0; k < i; ++k)
		{
			double Dik = D(i, k);
			for (int l = 0; l < k; ++l)
			{
				e = _elec_rep(idx++);
				G(i,i) += 2 * D(k,l) * e;
				G(i,k) -= 0.5 * D(i,l) * e;
				G(i,l) -= 0.5 * Dik * e;
				G(k,l) += Dii * e;
			}
			e = _elec_rep(idx++);
			G(i,i) += D(k,k) * e;
			G(i,k) -= 0.5 * Dik * e;
			G(k,k) += Dii * e;
		}
		for (int l = 0; l < i; ++l)
		{
			e = _elec_rep(idx++);
			G(i,i) += D(i,l) * e;
			G(i,l) += 0.5 * Dii * e;
		}
		G(i,i) += 0.5 * Dii * _elec_rep(idx++);
	}

	for (int i = 0; i < n; ++i)
	{
		for (int j = 0; j < i; ++j)
			G(j,i) = G(i,j);
	}
}

void Basis::twoElectron(const Eigen::MatrixXd& D, Eigen::MatrixXd& J,
	Eigen::MatrixXd& K) const
{
	if (!_status.test(ELEC_REP_CURRENT))
		calcElectronRepulsion();

	int n = D.rows();
#ifdef DEBUG
	if (D.cols() != n)
		throw Li::Exception("Density matrix is not square");
#endif
	J.resize(n, n);
	J.setZero();
	K.resize(n, n);
	K.setZero();
	int idx = 0;
	double e;
	for (int i = 0; i < n; ++i)
	{
		double Dii = D(i,i);
		for (int j = 0; j < i; ++j)
		{
			double Dij = D(i,j);
			double Djj = D(j,j);
			for (int k = 0; k < j; ++k)
			{
				double Dik = D(i,k);
				double Djk = D(j,k);
				for (int l = 0; l < k; ++l)
				{
					e = _elec_rep(idx++);
					J(i,j) += 2 * D(k,l) * e;
					J(k,l) += 2 * Dij * e;
					K(i,k) += D(j,l) * e;
					K(i,l) += Djk * e;
					K(j,k) += D(i,l) * e;
					K(j,l) += Dik * e;
				}
				e = _elec_rep(idx++);
				J(i,j) += D(k,k) * e;
				J(k,k) += 2 * Dij * e;
				K(i,k) += Djk * e;
				K(j,k) += Dik * e;
			}
			for (int l = 0; l < j; ++l)
			{
				e = _elec_rep(idx++);
				J(i,j) += 2 * D(j,l) * e;
				J(j,l) += 2 * Dij * e;
				K(i,j) += D(j,l) * e;
				K(i,l) += Djj * e;
				K(j,j) += 2 * D(i,l) * e;
				K(j,l) += Dij * e;
			}
			e = _elec_rep(idx++);
			J(i,j) += Djj * e;
			J(j,j) += 2 * Dij * e;
			K(i,j) += Djj * e;
			K(j,j) += 2 * Dij * e;
			for (int k = j+1; k < i; ++k)
			{
				double Dik = D(i,k);
				double Dkj = D(k,j);
				for (int l = 0; l < j; ++l)
				{
					e = _elec_rep(idx++);
					J(i,j) += 2 * D(k,l) * e;
					J(k,l) += 2 * Dij * e;
					K(i,k) += D(j,l) * e;
					K(i,l) += Dkj * e;
					K(j,l) += Dik * e;
					K(k,j) += D(i,l) * e;
				}
				e = _elec_rep(idx++);
				J(i,j) += 2 * Dkj * e;
				J(k,j) += 2 * Dij * e;
				K(i,k) += Djj * e;
				K(i,j) += Dkj * e;
				K(j,j) += 2 * Dik * e;
				K(k,j) += Dij * e;
				for (int l = j+1; l < k; ++l)
				{
					e = _elec_rep(idx++);
					J(i,j) += 2 * D(k,l) * e;
					J(k,l) += 2 * Dij * e;
					K(i,k) += D(l,j) * e;
					K(i,l) += Dkj * e;
					K(k,j) += D(i,l) * e;
					K(l,j) += Dik * e;
				}
				e = _elec_rep(idx++);
				J(i,j) += D(k,k) * e;
				J(k,k) += 2 * Dij * e;
				K(i,k) += Dkj * e;
				K(k,j) += Dik * e;
			}
			for (int l = 0; l < j; ++l)
			{
				e = _elec_rep(idx++);
				J(i,j) += 2 * D(i,l) * e;
				J(i,l) += 2 * Dij * e;
				K(i,i) += 2 * D(j,l) * e;
				K(i,j) += D(i,l) * e;
				K(i,l) += Dij * e;
				K(j,l) += Dii * e;
			}
			e = _elec_rep(idx++);
			J(i,j) += 2 * Dij * e;
			K(i,i) += Djj * e;
			K(i,j) += Dij * e;
			K(j,j) += Dii * e;
		}
		for (int k = 0; k < i; ++k)
		{
			double Dik = D(i,k);
			for (int l = 0; l < k; ++l)
			{
				e = _elec_rep(idx++);
				J(i,i) += 2 * D(k,l) * e;
				J(k,l) += Dii * e;
				K(i,k) += D(i,l) * e;
				K(i,l) += Dik * e;
			}
			e = _elec_rep(idx++);
			J(i,i) += D(k,k) * e;
			J(k,k) += Dii * e;
			K(i,k) += Dik * e;
		}
		for (int l = 0; l < i; ++l)
		{
			e = _elec_rep(idx++);
			J(i,i) += 2 * D(i,l) * e;
			J(i,l) += Dii * e;
			K(i,i) += D(i,l) * e;
			K(i,l) += Dii * e;
			K(i,i) += D(i,l) * e;
		}
		e = _elec_rep(idx++);
		J(i,i) += Dii * e;
		K(i,i) += Dii * e;
	}

	for (int i = 0; i < n; ++i)
	{
		for (int j = 0; j < i; ++j)
		{
			J(j,i) = J(i,j);
			K(j,i) = K(i,j);
		}
	}
}

std::ostream& Basis::print(std::ostream& os) const
{
	os << "Basis (\n" << indent;
	for (BasisFunList::const_iterator it = _funs.begin();
		it != _funs.end(); ++it)
	{
		os << **it << "\n";
	}
	os << dedent << ")";
	return os;
}

void Basis::setPairs() const
{
	const Dispatcher& dispatcher = Dispatcher::singleton();

	_pairs.clear();
	for (BasisFunList::const_iterator iit = _funs.begin(); iit != _funs.end(); ++iit)
	{
		for (BasisFunList::const_iterator jit = _funs.begin(); jit <= iit; ++jit)
		{
			AbstractBFPair *pair = dispatcher.pair(**iit, **jit);
			_pairs.push_back(PairPtr(pair));
		}
	}

	_status.set(PAIRS_CURRENT);
}

void Basis::setQuads() const
{
	if (!_status.test(PAIRS_CURRENT))
		setPairs();

	const Dispatcher& dispatcher = Dispatcher::singleton();

	_quads.clear();
	for (PairList::const_iterator iit = _pairs.begin(); iit != _pairs.end(); ++iit)
	{
		for (PairList::const_iterator jit = _pairs.begin(); jit <= iit; ++jit)
		{
			AbstractBFQuad *quad = dispatcher.quad(**iit, **jit, _quad_pool);
			_quads.push_back(QuadPtr(quad, _quad_pool.deleter()));
		}
	}

	_status.set(QUADS_CURRENT);
}

void Basis::calcOverlap() const
{
	if (!_status.test(PAIRS_CURRENT))
		setPairs();

	PairList::const_iterator pit = _pairs.begin();
	int n = size();
	_overlap.resize(n, n);
	for (int i = 0; i < n; ++i)
	{
		for (int j = 0; j < i; ++j, ++pit)
			_overlap(i, j) = _overlap(j, i) = (*pit)->overlap();
		_overlap(i, i) = (*pit)->overlap();
		++pit;
	}

	_status.set(OVERLAP_CURRENT);
}

void Basis::calcKinetic() const
{
	if (!_status.test(PAIRS_CURRENT))
		setPairs();

	PairList::const_iterator pit = _pairs.begin();
	int n = size();
	_kinetic.resize(n, n);
	for (int i = 0; i < n; ++i)
	{
		for (int j = 0; j < i; ++j, ++pit)
			_kinetic(i, j) = _kinetic(j, i) = (*pit)->kineticEnergy();
		_kinetic(i, i) = (*pit)->kineticEnergy();
		++pit;
	}

	_status.set(KINETIC_CURRENT);
}

void Basis::calcOneElectron() const
{
	if (!_status.test(PAIRS_CURRENT))
		setPairs();

	PairList::const_iterator pit = _pairs.begin();
	int n = size();
	_overlap.resize(n, n);
	_kinetic.resize(n, n);
	for (int i = 0; i < n; ++i)
	{
		for (int j = 0; j < i; ++j, ++pit)
		{
			(*pit)->oneElectron(_overlap(i,j), _kinetic(i,j));
			_overlap(j,i) = _overlap(i,j);
			_kinetic(j,i) = _kinetic(i,j);
		}
		(*pit)->oneElectron(_overlap(i,i), _kinetic(i,i));
		++pit;
	}

	_status.set(OVERLAP_CURRENT);
	_status.set(KINETIC_CURRENT);
}

void Basis::calcNuclearAttraction(const Eigen::MatrixXd& nuc_pos,
	const Eigen::VectorXd& nuc_charge) const
{
	if (!_status.test(PAIRS_CURRENT))
		setPairs();

	PairList::const_iterator pit = _pairs.begin();
	int n = size();
	_nuc_attr.resize(n, n);
	for (int i = 0; i < n; ++i)
	{
		for (int j = 0; j < i; ++j, ++pit)
			_nuc_attr(i, j) = _nuc_attr(j, i)
				= (*pit)->nuclearAttraction(nuc_pos, nuc_charge);
		_nuc_attr(i, i) = (*pit)->nuclearAttraction(nuc_pos, nuc_charge);
		++pit;
	}

	_status.set(NUC_ATTR_CURRENT);
}

void Basis::calcElectronRepulsion() const
{
	if (!_status.test(QUADS_CURRENT))
		setQuads();

	_elec_rep.resize(_quads.size());
	for (unsigned int i = 0; i < _quads.size(); ++i)
		_elec_rep[i] = _quads[i]->electronRepulsion();

	_status.set(ELEC_REP_CURRENT);
}

void Basis::calcOrtho() const
{
	if (!_status.test(OVERLAP_CURRENT))
		calcOverlap();
	_ortho = _overlap.sqrt().inverse();
	_status.set(ORTHO_CURRENT);
}


#include"main.hpp"
#include"Option.hpp"
#include"Pre.hpp"
#include <boost/algorithm/string.hpp>
using boost::lexical_cast;
typedef multimap<string,Row*>::const_iterator mmit;
typedef multimap<string,const Row*>::const_iterator cmmit;

class pred_mc
{
    const double d_mc_cut;
    public:
    pred_mc(const double mc_cut) : d_mc_cut(mc_cut) { }
    bool operator() (const Row& tmprow) const { return tmprow.m_cor<d_mc_cut; }
};

class pred_g
{
    const set<string> d_set;
    public:
    pred_g(const set<string>& tmpset) : d_set(tmpset) {}
    bool operator() (const Row& tmprow) const { return d_set.end()==d_set.find(tmprow.pid); }
};

class pred_nr
{
    const int d_nr;
    public:
    pred_nr(const int nr) : d_nr(nr) {}
    bool operator() (const Row& tmpco) const { return tmpco.nrepsi<d_nr; }
};

void Pre::setMc()
{
    multimap<string,Row*> pid_mm;
    for (list<Row>::iterator it=d_pdata.begin();it!=d_pdata.end();it++) {
        pid_mm.insert(make_pair(it->pid,&(*it)));
    }
    for (mmit end1,itp=pid_mm.begin();itp!=pid_mm.end();itp=end1) {
        end1=pid_mm.upper_bound(itp->first);
        vector<vector<double> > ww;
        for (mmit it=itp;it!=end1;it++) ww.push_back(it->second->in);
        if (ww.size()==1) {
            itp->second->m_cor=1;
            continue;
        }
        mmit it=itp;
        for (unsigned f=0;f<ww.size();f++,it++) {
            vector<double> cor;
            for (unsigned g=0;g<ww.size();g++) if (f != g) {
                vector<double> center(2);
                vector<double> sd(center.size());
                int ncol0=0;
                for (int l=0;l<op().nc();l++) if (obs(ww.at(f).at(l)) and obs(ww.at(g).at(l))) {
                    center.at(0) += ww.at(f).at(l);
                    center.at(1) += ww.at(g).at(l);
                    ncol0++;
                }
                if (ncol0<2) continue;
                center.at(0) /= ncol0;
                center.at(1) /= ncol0;
                for (int l=0;l<op().nc();l++) if (obs(ww.at(f).at(l)) and obs(ww.at(g).at(l))) {
                    sd.at(0) += pow(ww.at(f).at(l)-center.at(0),2);
                    sd.at(1) += pow(ww.at(g).at(l)-center.at(1),2);
                }
                sd.at(0) = sqrt(sd.at(0));
                sd.at(1) = sqrt(sd.at(1));
                double cortmp=0;
                for (int l=0;l<op().nc();l++) if (obs(ww.at(f).at(l)) and obs(ww.at(g).at(l))) {
                    cortmp += (ww.at(f).at(l)-center.at(0))*(ww.at(g).at(l)-center.at(1));
                }
                if (sd.at(0)==0 or sd.at(1)==0) continue;
                cortmp /= sd.at(0);
                cortmp /= sd.at(1);
                cor.push_back(cortmp);
            }
            if (cor.size()==0) {
                it->second->m_cor=-1;
            } else {
                it->second->m_cor=median(cor.begin(),cor.end());
            }
        }
    }
}

void Pre::norm_data()
{
    if (op().xbool()) {
        for (int p=0;p<nprot();p++) for (int j=0;j<op().nrep();j++) {//centering by reps
            vector<double> xvec;
            for (int t=0; t<op().nt(); t++) if (obs(xx().at(p).at(j*op().nt()+t))) {
                xvec.push_back(xx().at(p).at(j*op().nt()+t));
            }
            double center_x=median(xvec.begin(),xvec.end());
            for (int t=0; t<op().nt(); t++) if (obs(xx().at(p).at(j*op().nt()+t))) {
                d_xx.at(p).at(j*op().nt()+t) -= center_x;
            }
        }
    }
    for (int p=0;p<nprot();p++) {//centering by reps
        for (unsigned q=0;q<yy().at(p).size();q++) for (int j=0;j<op().nrep();j++) {
            vector<double> yvec;
            for (int t=0; t<op().nt(); t++) if (obs(yy().at(p).at(q).at(j*op().nt()+t))) {
                yvec.push_back(yy().at(p).at(q).at(j*op().nt()+t));
            }
            double center_y=median(yvec.begin(),yvec.end());
            for (int t=0; t<op().nt(); t++) if (obs(yy().at(p).at(q).at(j*op().nt()+t))) {
                d_yy.at(p).at(q).at(j*op().nt()+t) -= center_y;
            }
        }
    }

}

Pre::Pre(const Option& op) : d_op(op)
{
    read();
    setAll();
}

void Pre::read()
{
    ifstream y_ifs(op().filey().c_str());
    ifstream x_ifs(op().filex().c_str());

    string str0;
    istringstream iss;
    getline(y_ifs,str0);
    split(d_yh, str0, boost::is_any_of("\t"));
    if (int(yh().size())<op().nc()) throw runtime_error("file_y");

    getline(x_ifs,str0);
    split(d_xh, str0, boost::is_any_of("\t"));

    for (int i=2;getline(x_ifs,str0) and (not str0.empty());i++) {
        Row tmp_f(op().nc());
        iss.clear(); iss.str(str0+"\t");
        getline(iss,tmp_f.pid,'\t'); //tmp_f.pid+="_"+lexical_cast<string>(i);
        for (int l=0;l<op().nc();l++) {
            if (not getline(iss,str0,'\t')) throw runtime_error("FILE_X unequal columns: row "+lexical_cast<string>(i));
            if (str0=="NA" or str0.empty() or lexical_cast<double>(str0)==0) {
                tmp_f.in.at(l)=NAN;
            } else {
                try {
                    if (op().log_x()) tmp_f.in.at(l)=log(lexical_cast<double>(str0));
                    else tmp_f.in.at(l)=(lexical_cast<double>(str0));
                } catch (boost::bad_lexical_cast& e) {
                    cerr<<e.what()<<"FILE_X : row "<<i<<", column "<<l+1+op().level();
                    exit(1);
                }
            }
        }
        d_gdata0.push_back(tmp_f);
    }

    for (int i=2;getline(y_ifs,str0) and (not str0.empty());i++) {
        Row tmp_f(op().nc());
        iss.clear(); iss.str(str0+"\t");
        getline(iss,tmp_f.pid,'\t'); //tmp_f.pid+="_"+lexical_cast<string>(i);
        if (op().level()==1) tmp_f.qid=tmp_f.pid;
        else if (op().level()==2) getline(iss,tmp_f.qid,'\t');
        for (int l=0;l<op().nc();l++) {
            if (not getline(iss,str0,'\t')) throw runtime_error("FILE_Y unequal columns: row "+lexical_cast<string>(i));
            if (str0=="NA" or lexical_cast<double>(str0)==0 or str0.empty()) {
                tmp_f.in.at(l)=NAN;
            } else {
                try {
                    if (op().log_y()) tmp_f.in.at(l)=log(lexical_cast<double>(str0));
                    else tmp_f.in.at(l)=(lexical_cast<double>(str0));
                } catch (boost::bad_lexical_cast& e) {
                    cerr<<e.what()<<"FILE_Y : row "<<i<<", column "<<l+1+op().level();
                    exit(1);
                }
            }
        }
        d_pdata0.push_back(tmp_f);
    }
}

void Pre::setAll()
{
    multimap<string,const Row*> gid_mm;
    for (list<Row>::const_iterator it=gdata0().begin();it!=gdata0().end();it++) {
        gid_mm.insert(make_pair(it->pid,&(*it)));
    }
    for (cmmit end1,itp=gid_mm.begin();itp!=gid_mm.end();itp=end1) {
        end1=gid_mm.upper_bound(itp->first);
        Row tmp_f(op().nc());
        tmp_f.pid=itp->first;
        vector<int> cobs(op().nc());
        for (cmmit it=itp;it!=end1;it++) {
            for (int i=0;i<op().nc();i++) {
                if (obs(it->second->in.at(i))) {
                    tmp_f.in.at(i)+=it->second->in.at(i);
                    cobs.at(i)++;
                }
            }
        }
        for (int i=0;i<op().nc();i++) {
            if (cobs.at(i)>0) tmp_f.in.at(i) /= cobs.at(i); else tmp_f.in.at(i)=NAN;
        }
        d_gdata.push_back(tmp_f);
    }

    multimap<string,const Row*> pqid_mm;
    for (list<Row>::const_iterator it=pdata0().begin();it!=pdata0().end();it++) {
        pqid_mm.insert(make_pair(it->pid+it->qid,&(*it)));
    }
    for (cmmit end1,itp=pqid_mm.begin();itp!=pqid_mm.end();itp=end1) {
        end1=pqid_mm.upper_bound(itp->first);
        Row tmp_f(op().nc());
        tmp_f.pid=itp->second->pid;
        tmp_f.qid=itp->second->qid;
        vector<int> cobs(op().nc());
        for (cmmit it=itp;it!=end1;it++) {
            for (int i=0;i<op().nc();i++) {
                if (obs(it->second->in.at(i))) {
                    tmp_f.in.at(i)+=it->second->in.at(i);
                    cobs.at(i)++;
                }
            }
        }
        for (int i=0;i<op().nc();i++) {
            if (cobs.at(i)>0) tmp_f.in.at(i) /= cobs.at(i); else tmp_f.in.at(i)=NAN;
        }
        d_pdata.push_back(tmp_f);
    }

    for (list<Row>::iterator it=d_gdata.begin();it!=d_gdata.end();it++) {
        for (int l=0;l<op().nc();l++) {
            if (not obs(it->in.at(l))) {it->nrepsi--; /*break;*/ }
        }
    }
    for (list<Row>::iterator it=d_pdata.begin();it!=d_pdata.end();it++) {
        for (int l=0;l<op().nc();l++) {
            if (not obs(it->in.at(l))) {it->nrepsi--; /*break;*/ }
        }
    }

    //d_gdata.remove_if(pred_nr(op().nc()-2));
    //d_pdata.remove_if(pred_nr(op().nc()-2));

    setMc();
    d_pdata.remove_if(pred_mc(op().min_correl()));

    set<string> gset;
    for (list<Row>::const_iterator it=gdata().begin();it!=gdata().end();it++) {
        gset.insert(it->pid);
    }
    d_pdata.remove_if(pred_g(gset));

    gset.clear();
    for (list<Row>::const_iterator it=pdata().begin();it!=pdata().end();it++) {
        gset.insert(it->pid);
    }
    d_gdata.remove_if(pred_g(gset));

    map<string,const Row*> gid_m;
    for (list<Row>::const_iterator it=gdata().begin();it!=gdata().end();it++) {
        gid_m[it->pid]=&(*it);
    }

    multimap<string,const Row*> pid_mm;
    for (list<Row>::const_iterator it=pdata().begin();it!=pdata().end();it++) {
        pid_mm.insert(make_pair(it->pid,&(*it)));
    }

    cout<<"nprot = "<<nprot()<<'\n';
    d_xid.resize(nprot());
    d_pid.resize(nprot());
    d_qid.resize(nprot());
    d_xx.resize(nprot(),vector<double>(op().nc()));
    d_yy.resize(nprot());
    int p=0;
    multimap<string,const Row*>::const_iterator end1,itp=pid_mm.begin();
    for (map<string,const Row*>::const_iterator itg=gid_m.begin();itg!=gid_m.end();itg++,itp=end1,p++) {
        d_xid.at(p)=itg->second->pid;
        d_pid.at(p)=itp->second->pid;
        for (int l=0;l<op().nc();l++) d_xx.at(p).at(l)=itg->second->in.at(l);
        end1=pid_mm.upper_bound(itp->first);
        for (cmmit it=itp;it!=end1;it++) {
            d_qid.at(p).push_back(it->second->qid);
            d_yy.at(p).push_back(it->second->in);
        }
    }
    if (not op().xbool()) {
        for (int g=0;g<nprot();g++) for (int l=0;l<op().nc();l++) d_xx.at(g).at(l)=0;
    }

}

#include"Eigen/Dense"

double Pre::Kfn(const vector<double>&theta,const double xp,const double xq,const int e=1)
{
    if (not op().smooth()) return exp(-pow((xp-xq),2)/2);
    const static double ll = op().smoothing().at(1);
    return pow(op().smoothing().at(0),2)*exp(-pow((xp-xq)/ll,2)/2)+e*(xp==xq);
}

double Pre::Mfn(const vector<double>& m,const double xs,const vector<double>& adjdiff)
{
    if (op().timei().front()>xs or xs>op().timei().back()) throw runtime_error("xs range");
    int fxs=-1;
    if (xs==op().timei().back()) return m.back();
    for (int i=op().nt()-2;i>=0;i--) if (op().timei().at(i)<=xs) {fxs=i; break;}
    return m.at(fxs)+(m.at(fxs+1)-m.at(fxs))/adjdiff.at(fxs+1)*(xs-op().timei().at(fxs));
}

typedef multimap<int,int>::const_iterator cmmii;

void Pre::impute_x()
{
    double** data = new double*[nprot()];
    int** mask = new int*[nprot()];
    for (int i = 0; i < nprot(); i++) {
        data[i] = new double[op().nc()];
        mask[i] = new int[op().nc()];
    }

    vector<vector<double> > mvec(nprot(),vector<double>(op().nrep()));
    vector<vector<double> > vvec(nprot(),vector<double>(op().nrep()));
    for (int p=0;p<nprot();p++) for (int j=0;j<op().nrep();j++) {
        int nj=0;
        const vector<double>& xxp=xx().at(p);
        for (int t=0;t<op().nt();t++) if (obs(xxp.at(j*op().nt()+t))) {
            mvec.at(p).at(j)+=xxp.at(j*op().nt()+t);
            nj++;
        }
        mvec.at(p).at(j) /= nj;
        for (int t=0;t<op().nt();t++) if (obs(xxp.at(j*op().nt()+t))) {
            vvec.at(p).at(j) += pow(xxp.at(j*op().nt()+t)-mvec.at(p).at(j),2);
        }
        vvec.at(p).at(j) /= nj-1;
        if (vvec.at(p).at(j)==0) vvec.at(p).at(j)=1;
        for (int t=0;t<op().nt();t++) if (obs(xxp.at(j*op().nt()+t))) {
            d_xx.at(p).at(j*op().nt()+t) -= mvec.at(p).at(j);
            d_xx.at(p).at(j*op().nt()+t) /= sqrt(vvec.at(p).at(j));
        }
    }

    for (int p=0;p<nprot();p++) for (int l=0; l<op().nc(); l++) {
        data[p][l]=0;
        int nq=0;
        if (obs(xx().at(p).at(l))) {
            data[p][l] += xx().at(p).at(l);
            nq++;
        }
        if (nq>0) {
            data[p][l] /= nq;
            mask[p][l] = 1;
        } else {
            data[p][l] = NAN;
            mask[p][l] = 0;
        }
    }

    vector<int> clusterid(nprot());

    multimap<int,int> c_mm;
    for (int p=0;p<nprot();p++) c_mm.insert(make_pair(clusterid[p],p));

    vector<vector<Eigen::VectorXd> > xax,yy_;
    xax.resize(xx().size());
    yy_.resize(xx().size());
    for (int p=0;p<nprot();p++) {
        xax.at(p).resize(op().nrep());
        yy_.at(p).resize(op().nrep());
        for (int j=0;j<op().nrep();j++) {
            vector<double> x,y;
            for (int t=0;t<op().nt();t++) {
                if (obs(xx().at(p).at(j*op().nt()+t))) {
                    x.push_back(op().timei().at(t));
                    y.push_back(xx().at(p).at(j*op().nt()+t));
                }
            }
            xax.at(p).at(j).resize(x.size());
            yy_.at(p).at(j).resize(y.size());
            for (unsigned i=0;i<x.size();i++) {
                xax.at(p).at(j)(i)=x.at(i);
                yy_.at(p).at(j)(i)=y.at(i);
            }
        }
    }

    const int Ns=100;
    const double step=double(op().timei().back()-op().timei().front())/Ns;
    Eigen::VectorXd xs(Ns);
    for (int i=0;i<Ns;i++) xs(i)=op().timei().at(0)+step/2+i*step;

    ofstream ofs1("EfsX.txt");
    ofs1<<"p\tq\tj";
    for (unsigned i=0;i<xs.size();i++) ofs1<<'\t'<<xs(i);
    ofs1<<'\n';

    int ijk=0;
    vector<double> adjdiff(op().timei().size());
    adjacent_difference(op().timei().begin(),op().timei().end(),adjdiff.begin());

    for (cmmii end1,itc=c_mm.begin();itc!=c_mm.end()/*,ijk<8*/;itc=end1,ijk++) {
        end1=c_mm.upper_bound(itc->first);

        vector<vector<double> > m(op().nrep(),vector<double>(op().nt()));

        vector<double> theta(3,1);

        for (cmmii it=itc;it!=end1;it++) {
            const int p=it->second;
            for (int j=0;j<op().nrep();j++) {
                Eigen::VectorXd& y = yy_.at(p).at(j);
                Eigen::VectorXd& x = xax.at(p).at(j);
                const int N = y.size();
                Eigen::MatrixXd Ky(N,N);
                for (int i=0;i<N;i++) for (int k=0;k<N;k++) Ky(i,k)=Kfn(theta,x(i),x(k));
                Eigen::VectorXd ym(y);
                for (int i=0;i<N;i++) ym(i)-=Mfn(m.at(j),x(i),adjdiff);
                Eigen::MatrixXd invKy=Ky.inverse();
                ofs1<<p<<"\t0\t"<<j;
                for (unsigned i=0;i<xs.size();i++) {
                    Eigen::VectorXd ks(N);
                    for (int k=0;k<N;k++) ks(k)=Kfn(theta,xs(i),x(k),0);
                    const double Efs=ks.transpose()*invKy*ym+Mfn(m.at(j),xs(i),adjdiff);
                    ofs1<<'\t'<<Efs*sqrt(vvec.at(p).at(j))+mvec.at(p).at(j);
                }
                ofs1<<'\n';

                for (int t=0;t<op().nt();t++) {
                    Eigen::VectorXd ks(N);
                    for (int k=0;k<N;k++) ks(k)=Kfn(theta,op().timei().at(t),x(k),0);
                    d_xx.at(p).at(j*op().nt()+t)=
                        ks.transpose()*invKy*ym+Mfn(m.at(j),op().timei().at(t),adjdiff);
                    d_xx.at(p).at(j*op().nt()+t)*=sqrt(vvec.at(p).at(j));
                    d_xx.at(p).at(j*op().nt()+t)+=mvec.at(p).at(j);
                }
            }
        }
    }
}

void Pre::impute_y()
{
    double** data = new double*[nprot()];
    int** mask = new int*[nprot()];
    for (int i = 0; i < nprot(); i++) {
        data[i] = new double[op().nc()];
        mask[i] = new int[op().nc()];
    }

    vector<vector<vector<double> > > mvec(nprot());
    vector<vector<vector<double> > > vvec(nprot());
    for (int p=0;p<nprot();p++) {
        mvec.at(p).resize(yy().at(p).size(),vector<double>(op().nrep()));
        vvec.at(p).resize(yy().at(p).size(),vector<double>(op().nrep()));
        for (unsigned q=0;q<yy().at(p).size();q++) for (int j=0;j<op().nrep();j++) {
            int nj=0;
            const vector<double>& yypq=yy().at(p).at(q);
            for (int t=0;t<op().nt();t++) if (obs(yypq.at(j*op().nt()+t))) {
                mvec.at(p).at(q).at(j)+=yypq.at(j*op().nt()+t);
                nj++;
            }
            mvec.at(p).at(q).at(j) /= nj;
            for (int t=0;t<op().nt();t++) if (obs(yypq.at(j*op().nt()+t))) {
                vvec.at(p).at(q).at(j) += pow(yypq.at(j*op().nt()+t)-mvec.at(p).at(q).at(j),2);
            }
            vvec.at(p).at(q).at(j) /= nj-1;
            if (vvec.at(p).at(q).at(j)==0) vvec.at(p).at(q).at(j)=1;
            for (int t=0;t<op().nt();t++) if (obs(yypq.at(j*op().nt()+t))) {
                d_yy.at(p).at(q).at(j*op().nt()+t) -= mvec.at(p).at(q).at(j);
                d_yy.at(p).at(q).at(j*op().nt()+t) /= sqrt(vvec.at(p).at(q).at(j));
            }
        }
    }

    for (int p=0;p<nprot();p++) for (int l=0; l<op().nc(); l++) {
        data[p][l]=0;
        int nq=0;
        for (unsigned q=0;q<yy().at(p).size();q++) if (obs(yy().at(p).at(q).at(l))) {
            data[p][l] += yy().at(p).at(q).at(l);
            nq++;
        }
        if (nq>0) {
            data[p][l] /= nq;
            mask[p][l] = 1;
        } else {
            data[p][l] = NAN;
            mask[p][l] = 0;
        }
    }

    //double* weight = new double[op().nc()];
    //for (int i = 0; i < op().nc(); i++) weight[i] = 1.0;
    //Node* tree = treecluster(nprot(), op().nc(), data, mask, weight, 0, 'c', 'a', 0);
    //delete[] weight;
    //if (!tree) { /* Indication that the treecluster routine failed */
    //    cout<< "treecluster routine failed due to insufficient memory\n";
    //    return;
    //}
    //int* clusterid = new int[nprot()];
    //cuttree(nprot(), tree, nprot()/100, clusterid);
    //delete[] tree;

    vector<int> clusterid(nprot());

    multimap<int,int> c_mm;
    for (int p=0;p<nprot();p++) c_mm.insert(make_pair(clusterid[p],p));

    vector<vector<vector<Eigen::VectorXd> > > xax,yy_;
    xax.resize(yy().size());
    yy_.resize(yy().size());
    for (int p=0;p<nprot();p++) {
        xax.at(p).resize(yy().at(p).size());
        yy_.at(p).resize(yy().at(p).size());
        for (unsigned q=0;q<yy().at(p).size();q++) {
            xax.at(p).at(q).resize(op().nrep());
            yy_.at(p).at(q).resize(op().nrep());
            for (int j=0;j<op().nrep();j++) {
                vector<double> x,y;
                for (int t=0;t<op().nt();t++) {
                    if (obs(yy().at(p).at(q).at(j*op().nt()+t))) {
                        x.push_back(op().timei().at(t));
                        y.push_back(yy().at(p).at(q).at(j*op().nt()+t));
                    }
                }
                xax.at(p).at(q).at(j).resize(x.size());
                yy_.at(p).at(q).at(j).resize(y.size());
                for (unsigned i=0;i<x.size();i++) {
                    xax.at(p).at(q).at(j)(i)=x.at(i);
                    yy_.at(p).at(q).at(j)(i)=y.at(i);
                }
            }
        }
    }

    const int Ns=100;
    const double step=double(op().timei().back()-op().timei().front())/Ns;
    Eigen::VectorXd xs(Ns);
    for (int i=0;i<Ns;i++) xs(i)=op().timei().at(0)+step/2+i*step;

    ofstream ofs1("EfsY.txt");
    ofs1<<"p\tq\tj";
    for (unsigned i=0;i<xs.size();i++) ofs1<<'\t'<<xs(i);
    ofs1<<'\n';

    int ijk=0;
    vector<double> adjdiff(op().timei().size());
    adjacent_difference(op().timei().begin(),op().timei().end(),adjdiff.begin());

    for (cmmii end1,itc=c_mm.begin();itc!=c_mm.end()/*,ijk<8*/;itc=end1,ijk++) {
        end1=c_mm.upper_bound(itc->first);

        vector<vector<double> > m(op().nrep(),vector<double>(op().nt()));

        vector<double> theta(3,1);

        for (cmmii it=itc;it!=end1;it++) {
            const int p=it->second;
            for (unsigned q=0;q<yy_.at(p).size();q++) for (int j=0;j<op().nrep();j++) {
                Eigen::VectorXd& y = yy_.at(p).at(q).at(j);
                Eigen::VectorXd& x = xax.at(p).at(q).at(j);
                const int N = y.size();

                Eigen::MatrixXd Ky(N,N);
                for (int i=0;i<N;i++) for (int k=0;k<N;k++) Ky(i,k)=Kfn(theta,x(i),x(k));
                Eigen::VectorXd ym(y);
                for (int i=0;i<N;i++) ym(i)-=Mfn(m.at(j),x(i),adjdiff);
                Eigen::MatrixXd invKy=Ky.inverse();
                ofs1<<p<<'\t'<<q<<'\t'<<j;
                for (unsigned i=0;i<xs.size();i++) {
                    Eigen::VectorXd ks(N);
                    for (int k=0;k<N;k++) ks(k)=Kfn(theta,xs(i),x(k),0);
                    const double Efs=ks.transpose()*invKy*ym+Mfn(m.at(j),xs(i),adjdiff);
                    ofs1<<'\t'<<Efs*sqrt(vvec.at(p).at(q).at(j))+mvec.at(p).at(q).at(j);
                }
                ofs1<<'\n';

                for (int t=0;t<op().nt();t++) {
                    Eigen::VectorXd ks(N);
                    for (int k=0;k<N;k++) ks(k)=Kfn(theta,op().timei().at(t),x(k),0);
                    d_yy.at(p).at(q).at(j*op().nt()+t)=
                        ks.transpose()*invKy*ym+Mfn(m.at(j),op().timei().at(t),adjdiff);
                    d_yy.at(p).at(q).at(j*op().nt()+t)*=sqrt(vvec.at(p).at(q).at(j));
                    d_yy.at(p).at(q).at(j*op().nt()+t)+=mvec.at(p).at(q).at(j);
                }
            }
        }
    }
}


#include "engine.hpp"

#include "functional.hpp"

#include <cassert>
#include <vector>

#include <ranges>

#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/algebra/fusion_algebra.hpp>
#include <boost/numeric/odeint/algebra/fusion_algebra_dispatcher.hpp>

namespace EvolutionaryWalker::Physics
{

namespace Impl
{

    class Engine
    {
    public:
        constexpr static const std::size_t number_of_dimensions = 2;
        using VelocityVector = LibUtils::Point<VelocityQuantity, number_of_dimensions>;
        using AccelerationVector = LibUtils::Point<AccelerationQuantity, number_of_dimensions>;
        using state_type = /*std::vector<*/ boost::fusion::vector<LengthQuantity, VelocityQuantity> /*>*/;
        using deriv_type = /*std::vector<*/ boost::fusion::vector<VelocityQuantity, AccelerationQuantity> /*>*/;
        using StepperType = boost::numeric::odeint::runge_kutta_dopri5<state_type, double, deriv_type, TimeQuantity>;

        struct Node
        {
            bool is_fixed;
            MassQuantity mass;
            Point2d position;
            VelocityVector speed;
        };

        struct Spring
        {
            std::array<NodeIndex, 2> nodes;
            SpringCharacteristics characteristics;
        };

        Engine(AccelerationQuantity gravity)
            : m_gravity(std::move(gravity))
        {
        }

        ~Engine() = default;

        void init()
        {
            m_stepper = std::make_unique<StepperType>();
        }

        void step(TimeQuantity time_step)
        {
            assert(numberOfFreeNodes() == 1); // for now
            assert(m_stepper);
            auto x = currentState();
            m_stepper->do_step(std::cref(*this), x, TimeQuantity{}, time_step);
            applyNewState(x);
        }

        void operator()(const state_type& x, deriv_type& dxdt, const TimeQuantity& /* t */) const
        {
            // assert(x.size() == dxdt.size());
            // for(std::size_t i = 0; i < x.size(); i++)
            // {
            //     // dxdt[i].first = x[i].second;
            //     // dxdt[i].second = -x[0] - gam * x[1];
            //
            //     // fusion::at_c<0>(dxdt) = fusion::at_c<1>(x);
            //     // fusion::at_c<1>(dxdt) = -m_omega * m_omega * fusion::at_c<0>(x);
            // }

            // using boost::fusion::at_c;
            at_c<0>(dxdt) = at_c<1>(x);
            at_c<1>(dxdt) = -m_gravity; //-m_omega * m_omega * at_c<0>(x);
        }

        static VelocityVector nullVelocity()
        {
            return {{0 * boost::units::si::meters_per_second, 0 * boost::units::si::meters_per_second}};
        }

        NodeIndex addFixedNode(Point2d position)
        {
            m_nodes.emplace_back(true, 0 * boost::units::si::kilograms, std::move(position), nullVelocity());
            return NodeIndex{m_nodes.size() - 1};
        }

        NodeIndex addNode(Point2d position, MassQuantity mass)
        {
            m_nodes.emplace_back(false, std::move(mass), std::move(position), nullVelocity());
            return NodeIndex{m_nodes.size() - 1};
        }

        SpringIndex addSpring(const NodeIndex node1, const NodeIndex node2, SpringCharacteristics characteristics)
        {
            m_springs.push_back({{node1, node2}, std::move(characteristics)});
            return SpringIndex{m_nodes.size() - 1};
        }

        Point2d node(const NodeIndex i) const
        {
            return m_nodes[i].position;
        }

        std::size_t numberOfFreeNodes() const
        {
            return freeNodes().size();
        }

    private:
        const AccelerationQuantity m_gravity;
        std::vector<Node> m_nodes;
        std::vector<Spring> m_springs;
        std::unique_ptr<StepperType> m_stepper = nullptr;

        state_type currentState() const
        {
            // state_type res;
            // for(const auto& node : m_nodes)
            // {
            //     if(node.is_fixed) continue;
            //     res.emplace_back(node.position, node.speed);
            // }
            // return res;

            const auto nodes = freeNodes();
            state_type res;
            at_c<0>(res) = nodes.front().position[1];
            at_c<1>(res) = nodes.front().speed[1];
            return res;
        }

        void applyNewState(const state_type& state)
        {
            // TODO do better
            m_nodes.back().position[1] = at_c<0>(state);
            m_nodes.back().speed[1] = at_c<1>(state);
        }

        std::vector<Node> freeNodes() const
        {
            using LibUtils::Functional::Not;
            // TODO try with ranges?
            // return m_nodes | std::views::filter(LibUtils::Functional::Not(&Node::is_fixed));
            decltype(m_nodes) res;
            std::copy_if(begin(m_nodes), end(m_nodes), std::back_inserter(res), Not(std::mem_fn(&Node::is_fixed)));
            return res;
        }
    };

} // namespace Impl

Engine::Engine(AccelerationQuantity gravity)
    : m_pimpl(std::make_unique<Impl::Engine>(std::move(gravity)))
{
}

Engine::~Engine() = default;

void Engine::init()
{
    m_pimpl->init();
}

void Engine::step(TimeQuantity time_step)
{
    m_pimpl->step(time_step);
}

NodeIndex Engine::addFixedNode(Point2d position)
{
    return m_pimpl->addFixedNode(std::move(position));
}

NodeIndex Engine::addNode(Point2d position, MassQuantity mass)
{
    return m_pimpl->addNode(std::move(position), std::move(mass));
}

SpringIndex Engine::addSpring(const NodeIndex node1, const NodeIndex node2, SpringCharacteristics characteristics)
{
    return m_pimpl->addSpring(node1, node2, std::move(characteristics));
}

Point2d Engine::node(const NodeIndex i) const
{
    return m_pimpl->node(i);
}

} // namespace EvolutionaryWalker::Physics


///////////////////////////////////////////////////////////////////////////////
// cells.hpp                                                                 //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_CELLS_CELLS_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_CELLS_CELLS_HPP_ER_2010
#include <boost/type_traits/remove_reference.hpp>
#include <boost/type_traits/remove_const.hpp>

#include <boost/math/policies/policy.hpp>

#include <boost/fusion/include/at_key.hpp>
#include <boost/fusion/container/map/detail/extract_keys.hpp> // needed?
#include <boost/fusion/container/map/detail/subset_traits.hpp>
#include <boost/fusion/container/map/detail/hashable_map.hpp>

#include <boost/unordered_map.hpp>

#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/parameters/accumulator.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/framework/parameters/weight.hpp>
#include <boost/accumulators/framework/detail/unpack_depends_on.hpp>
#include <boost/accumulators/framework/detail/parameters/policy.hpp>

#include <boost/statistics/detail/non_parametric/contingency_table/detail/raise_domain_error.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/factor/levels.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/factor/vec_levels.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/factor/domain_error_logger.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/factor/check_against_domain.hpp>

// Usage:
//  typedef cells<mpl::vector<X,Y,Z> > cells_
// This is a Boost.Accumulator feature specifying a contingency table asso-
// ciated with variables X, Y, and Z.

namespace boost {
namespace statistics{
namespace detail{
namespace contingency_table{
namespace cells_aux{

   template<typename Keys>
   struct depends_on{
        typedef typename contingency_table::vec_levels<Keys>::type vec_levels_;
        typedef typename boost::accumulators::detail::unpack_depends_on<
            vec_levels_
        >::type type;
   };

    template<typename T,typename Keys>
    struct traits{
        typedef boost::fusion::detail::map_subset_traits<T,Keys> inner_traits;
        typedef typename inner_traits::map subsample_;
        typedef boost::fusion::detail::hashable_map<
            subsample_> hashable_subsample_;
        typedef typename boost::unordered_map<
            hashable_subsample_,std::size_t> map_;

    };
}// cells_aux
namespace impl{

    template<typename T,typename Keys>
    class cells : public boost::accumulators::accumulator_base{
        typedef boost::accumulators::dont_care dont_care_;
        typedef std::size_t size_;

        typedef contingency_table::cells_aux::traits<T,Keys> traits_;
        typedef typename traits_::hashable_subsample_ hashable_subsample_;
        typedef typename traits_::map_ map_;

        public:

        typedef T 		sample_type;
        typedef size_ 	size_type;

        typedef map_& result_type; // not const bec map[] is non-const

        cells(dont_care_){}

        //template<typename A1,typename A2>
        //void operator()(const A1& a1,const A2& a2){
        //
        //}

        template<typename Args>
        void operator()(const Args& args){
            typedef boost::math::policies::policy<> pol_;
            this->update_if(
                hashable_subsample_( args[ boost::accumulators::sample ] ),
                args[ boost::accumulators::accumulator ],
                args[ boost::accumulators::detail::_policy |  pol_() ],
                args[ boost::accumulators::weight ]
            );
        }

        result_type result(dont_care_)const{
            return this->map;
        }

		private:
        template<typename V,typename N>
        void update(
            const V& s,
            const N& size
        ){
            ( this->map )[ s ] += size;
        }

        typedef ::boost::math::policies::domain_error<
             boost::math::policies::ignore_error
        > ignore_policy_;
        template<typename V,typename A,typename N>
        void update_if(
            const V& s,
            const A& acc,
            const ignore_policy_& policy,
            const N& size
        ){
            this->update(s,acc,size);
        }

        template<typename V,typename A,typename P,typename N>
        void update_if(
            const V& s,
            const A& acc,
            const P& policy,
            const N& size
        ){
            namespace ct = contingency_table;
            this->error_logger.reset();
            boost::fusion::for_each(
                s,
                ct::make_check_against_domain( acc, this->error_logger)
            );
            if(!this->error_logger.is_error())
            {
                this->update( s, size );
            }else{
                static const char* fun = "impl::cells::update_if %1%";
                ct::raise_domain_error<V>(
                  fun,
                  this->error_logger().c_str(),
                  policy
                );
            }
        }

        mutable map_ map;
        typedef contingency_table::domain_error_logger error_logger_;
        error_logger_ error_logger;
	};

}// impl
namespace tag
{

    template<typename Keys>
    struct cells
        : contingency_table::cells_aux::depends_on<Keys>::type
    {
        typedef typename contingency_table::par_spec< Keys >::type par_spec_;

        struct impl{
            template<typename T,typename W>
            struct apply{
        		typedef contingency_table::impl::cells<T,Keys> type;
            };
        };
    };

}// tag
namespace result_of{
namespace extract{

    template<typename Keys,typename AccSet>
    struct cells
    : boost::accumulators::detail::extractor_result<
        AccSet,
        contingency_table::tag::cells<Keys>
    >{};

}// extract
}// result_of
namespace extract
{
  	template<typename Keys,typename AccSet>
    typename contingency_table::result_of::extract::template
        cells<Keys,AccSet>::type
  	cells(AccSet const& acc)
    {
    	typedef contingency_table::tag::cells<Keys> the_tag;
        return boost::accumulators::extract_result<the_tag>(acc);
  	}

}// extract

using extract::cells;

}// contingency_table
}// detail
}// statistics
}// boost

#endif