#include <iostream>

#include "gflags/gflags.h"
#include "sndfile.h"
#include "tbb/tbb.h"
#include "tbb/pipeline.h"
#include "tbb/scalable_allocator.h"
#include "tbb/cache_aligned_allocator.h"
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>
#include "bakuage/sndfile_wrapper.h"
#include "bakuage/dft.h"
#include "bakuage/memory.h"
#include "bakuage/utils.h"
#include "bakuage/window_func.h"
#include "bakuage/clear_mixer_filter.h"
#include "bakuage/clear_mixer_filter2.h"
#include "bakuage/clear_mixer_filter3.h"
#include "bakuage/clear_mixer_filter4.h"

DEFINE_string(input, "", "comma separated input wav file paths");
DEFINE_string(output, "", "output wav file path");
DEFINE_double(window_sec, 0.02, "dft window size in sec");
DEFINE_double(mean_sec, 0.02, "energy lowpass mean time in sec");
DEFINE_double(ratio, 2, "ratio");
DEFINE_double(noise_reduction_threshold, -20, "noise reduction threshold in dB");
DEFINE_bool(normalize, false, "whether if normalize is done before output");
DEFINE_string(mode, "3", "clear mixer mode (1/2sparse/2expander/3/gen_test_signal)");

namespace {
typedef float Float;

Float **Alloc2DFloat(int a, int b) {
    Float **result = new Float*[a];
    for (int i = 0; i < a; i++) {
        result[i] = new Float[b];
    }
    return result;
}

Float ***Alloc3DFloat(int a, int b, int c) {
    Float ***result = new Float**[a];
    for (int i = 0; i < a; i++) {
        result[i] = Alloc2DFloat(b, c);
    }
    return result;
}

// 動画の長さが音源の長さ以上になるようにする
void CalculateClearMixer(const std::vector<std::vector<Float>> &input, int channels, int samples, int sample_freq, Float *output) {

    bakuage::ClearMixerFilter<Float>::Config config;
    config.num_tracks = input.size();
    config.num_channels = channels;
    config.sample_rate = sample_freq;
    config.window_sec = FLAGS_window_sec;
    config.mean_sec = FLAGS_window_sec;
    bakuage::ClearMixerFilter<Float> clear_mixer_filter(config);

    bakuage::ClearMixerFilter2<Float>::Config config2;
    config2.num_tracks = input.size();
    config2.num_channels = channels;
    config2.sample_rate = sample_freq;
    config2.overlap = 2;
    config2.window_samples = config2.overlap * (int)((FLAGS_window_sec * sample_freq) / config2.overlap);
    config2.algorithm = FLAGS_mode == "2sparse" ? bakuage::ClearMixerFilter2<Float>::kAlgorithmSparse : bakuage::ClearMixerFilter2<Float>::kAlgorithmExpander;
    bakuage::ClearMixerFilter2<Float> clear_mixer_filter2(config2);
    clear_mixer_filter2.set_ratio(FLAGS_ratio);

    bakuage::ClearMixerFilter3<Float>::Config config3;
    config3.num_tracks = input.size();
    config3.num_channels = channels;
    config3.sample_rate = sample_freq;
    config3.fir_samples = 2 * (int)(0.020 * sample_freq / 2) + 1;
    config3.gain_decimation = bakuage::CeilPowerOf2(sample_freq / 1500);
    config3.energy_mean_sec = 0.020;
    config3.scale_mean_sec = 0.020;
    config3.filter = bakuage::ClearMixerFilter3<Float>::kFilterFir;
    config3.noise_reduction = bakuage::ClearMixerFilter3<Float>::kNoiseReductionFixedSpectrumLearn;
    config3.noise_reduction_threshold = std::pow(10, FLAGS_noise_reduction_threshold / 10.0);
    bakuage::ClearMixerFilter3<Float> clear_mixer_filter3_learn(config3);
    clear_mixer_filter3_learn.set_ratio(FLAGS_ratio);

    bakuage::ClearMixerFilter4<Float>::Config config4;
    config4.num_tracks = input.size();
    config4.num_channels = channels;
    config4.sample_rate = sample_freq;
    config4.fir_samples = 2 * (int)(0.020 * sample_freq / 2) + 1;
    config4.gain_decimation = bakuage::CeilPowerOf2(sample_freq / 1500);
    config4.energy_mean_sec = 0.020;
    config4.scale_mean_sec = 0.020;
    config4.filter = bakuage::ClearMixerFilter3<Float>::kFilterFir;
    config4.noise_reduction = bakuage::ClearMixerFilter3<Float>::kNoiseReductionFixedSpectrumLearn;
    config4.noise_reduction_threshold = std::pow(10, FLAGS_noise_reduction_threshold / 10.0);
    bakuage::ClearMixerFilter4<Float> clear_mixer_filter4_learn(config4);
    clear_mixer_filter4_learn.set_ratio(FLAGS_ratio);

    constexpr int process_size = 4096; //44100 * 0.004;
    Float ***input_buf = Alloc3DFloat(input.size(), channels, process_size);
    Float **output_buf = Alloc2DFloat(channels, process_size);

    // learn noise
    for (int base_frame = 0; base_frame < samples + clear_mixer_filter.delay_samples(); base_frame += process_size) {
        const int this_frames = std::min<int>(samples + clear_mixer_filter.delay_samples() - base_frame, process_size);

#if 1
        // input
        for (int track = 0; track < input.size(); track++) {
            for (int ch = 0; ch < channels; ch++) {
                for (int frame = 0; frame < this_frames; frame++) {
                    const int f = frame + base_frame;
                    input_buf[track][ch][frame] = f < samples ? input[track][channels * f + ch] : 0;
                }
            }
        }
#endif

        // process
#if 1
        if (FLAGS_mode == "1") {
        } else if (FLAGS_mode == "2sparse" || FLAGS_mode == "2expander") {
        } else if (FLAGS_mode == "3") {
            clear_mixer_filter3_learn.Clock(input_buf, this_frames, output_buf);
        } else if (FLAGS_mode == "4") {
            clear_mixer_filter4_learn.Clock(input_buf, this_frames, output_buf);
        } else {
            std::stringstream ss;
            ss << "unknown mode " << FLAGS_mode;
            throw std::logic_error(ss.str());
        }
#endif
    }
    config3.noise_reduction = bakuage::ClearMixerFilter3<Float>::kNoiseReductionFixedSpectrum;
    config3.noise_reduction_fixed_spectrum_profile = clear_mixer_filter3_learn.CalculateNoiseReductionFixedSpectrumProfile();
    bakuage::ClearMixerFilter3<Float> clear_mixer_filter3(config3);
    clear_mixer_filter3.set_ratio(FLAGS_ratio);

    config4.noise_reduction = bakuage::ClearMixerFilter4<Float>::kNoiseReductionFixedSpectrum;
    config4.noise_reduction_fixed_spectrum_profile = clear_mixer_filter4_learn.CalculateNoiseReductionFixedSpectrumProfile();
    bakuage::ClearMixerFilter4<Float> clear_mixer_filter4(config4);
    clear_mixer_filter4.set_ratio(FLAGS_ratio);

    bakuage::StopWatch stop_watch;
    for (int base_frame = 0; base_frame < samples + clear_mixer_filter.delay_samples(); base_frame += process_size) {
        // 最初の一回は飛ばす
        if (base_frame == process_size) stop_watch.Start();

        const int this_frames = std::min<int>(samples + clear_mixer_filter.delay_samples() - base_frame, process_size);

#if 1
        // input
        for (int track = 0; track < input.size(); track++) {
            for (int ch = 0; ch < channels; ch++) {
                for (int frame = 0; frame < this_frames; frame++) {
                    const int f = frame + base_frame;
                    input_buf[track][ch][frame] = f < samples ? input[track][channels * f + ch] : 0;
                }
            }
        }
#endif

        // process
#if 1
        if (FLAGS_mode == "1") {
            clear_mixer_filter.Clock(input_buf, this_frames, output_buf);
        } else if (FLAGS_mode == "2sparse" || FLAGS_mode == "2expander") {
            clear_mixer_filter2.Clock(input_buf, this_frames, output_buf);
        } else if (FLAGS_mode == "3") {
            clear_mixer_filter3.Clock(input_buf, this_frames, output_buf);
        } else if (FLAGS_mode == "4") {
            clear_mixer_filter4.Clock(input_buf, this_frames, output_buf);
        } else {
            std::stringstream ss;
            ss << "unknown mode " << FLAGS_mode;
            throw std::logic_error(ss.str());
        }
#endif

        // output
#if 1
        for (int ch = 0; ch < channels; ch++) {
            for (int frame = 0; frame < this_frames; frame++) {
                const int f = frame + base_frame - clear_mixer_filter.delay_samples();
                if (f >= 0) {
                    output[channels * f + ch] = output_buf[ch][frame];
                }
            }
        }
#endif
    }

    std::cerr << "clear mixer: " << stop_watch.time() << std::endl;
}

template <class Float>
void SaveFloatWave(const std::vector<Float> &wave, const std::string &filename) {
    bakuage::SndfileWrapper snd_file;
    SF_INFO sfinfo = { 0 };
    std::memset(&sfinfo, 0, sizeof(sfinfo));

    sfinfo.channels = 2;
    sfinfo.format = SF_FORMAT_WAV | SF_FORMAT_FLOAT;
    int frames = wave.size() / sfinfo.channels;
    sfinfo.frames = frames;
    sfinfo.samplerate = 44100;

    if ((snd_file.set(sf_open(filename.c_str(), SFM_WRITE, &sfinfo))) == NULL) {
        std::stringstream message;
        message << "Not able to open output file " << filename << ", "
        << sf_strerror(NULL);
        throw std::logic_error(message.str());
    }

    sf_count_t size;
    if (sizeof(Float) == 4) {
        size = sf_writef_float(snd_file.get(), (float *)wave.data(), frames);
    } else {
        size = sf_writef_double(snd_file.get(), (double *)wave.data(), frames);
    }
    if (size != frames) {
        std::stringstream message;
        message << "sf_writef_float error: " << size;
        throw std::logic_error(message.str());
    }
}
}

// wavを受け取って、標準出力にrawvideoを出力
int main(int argc, char* argv[]) {
    gflags::SetVersionString("1.0.0");
    gflags::ParseCommandLineFlags(&argc, &argv, true);

    if (FLAGS_mode == "gen_test_signal") {
        for (int shifts = 1; shifts <= 16; shifts++) {
            {
                std::vector<Float> wave(44100);
                for (int i = 0; i < 22050; i++) {
                    double x = 0;
                    for (int j = 0; j < shifts; j++) {
                        const double t = 50.0 * i / 44100 - 1.0 * j / 16;
                        const double theta = 2 * M_PI * t;
                        x += (0 <= t && t < 1) ? 0.5 - 0.5 * std::cos(theta) : 0;
                    }
                    wave[2 * i + 0] = wave[2 * i + 1] = x;
                }
                std::stringstream ss;
                ss << "/tmp/hanning50hz" << shifts << "shift.wav";
                SaveFloatWave(wave, ss.str());
            }
            {
                std::vector<Float> wave(44100);
                for (int i = 0; i < 22050; i++) {
                    double x = 0;
                    for (int j = 0; j < shifts; j++) {
                        const double t = 50.0 * i / 44100 - 1.0 * j / 16;
                        const double theta = 2 * M_PI * t;
                        x += (0 <= t && t < 1) ? 0.35875 - 0.48829 * std::cos(theta) + 0.14128 * std::cos(2 * theta) - 0.01168 * std::cos(3 * theta) : 0;
                    }
                    wave[2 * i + 0] = wave[2 * i + 1] = x;
                }
                std::stringstream ss;
                ss << "/tmp/blackman_harris50hz" << shifts << "shift.wav";
                SaveFloatWave(wave, ss.str());
            }
        }
        return 0;
    }

    bakuage::SndfileWrapper infile;
    SF_INFO sfinfo = { 0 };
    int frames = 1 << 30;

    // カンマ区切りを分解して、load wave (全てが同じフォーマットである必要がある)
    std::vector<std::vector<float>> buffers;
    std::vector<std::string> input_paths;
    boost::algorithm::split(input_paths, FLAGS_input, boost::is_any_of(","));
    for (const auto &input_file_path: input_paths) {
        if ((infile.set(sf_open (input_file_path.c_str(), SFM_READ, &sfinfo))) == NULL) {
            fprintf(stderr, "Not able to open input file %s.\n", input_file_path.c_str());
            fprintf(stderr, "%s\n", sf_strerror(NULL));
            return 1;
        }

        // check format
        fprintf(stderr, "sfinfo.format 0x%08x.\n", sfinfo.format);
        switch (sfinfo.format & SF_FORMAT_TYPEMASK) {
            case SF_FORMAT_WAV:
            case SF_FORMAT_WAVEX:
                break;
            default:
                fprintf(stderr, "Not supported sfinfo.format 0x%08x.\n", sfinfo.format);
                return 2;
        }

        std::vector<float> buffer(sfinfo.channels * sfinfo.frames);
        int read_size = sf_readf_float(infile.get(), buffer.data(), sfinfo.frames);
        fprintf(stderr, "%d samples read.\n", read_size);
        if (read_size != sfinfo.frames) {
            fprintf(stderr, "sf_readf_float error: %d %d\n", read_size, (int)sfinfo.frames);
            return 3;
        }
        buffers.emplace_back(buffer);

        frames = std::min<int>(frames, sfinfo.frames);
    }

    // calculate spectrogram (energy)
    std::vector<float> output(frames * sfinfo.channels);
    CalculateClearMixer(buffers, sfinfo.channels, frames, sfinfo.samplerate, output.data());

    // normalize
    if (FLAGS_normalize) {
        double peak = 1e-37;
        for (int i = 0; i < output.size(); i++) {
            peak = std::max<double>(peak, std::abs(output[i]));
        }
        for (int i = 0; i < output.size(); i++) {
            output[i] /= peak;
        }
    }

    // save output
    SaveFloatWave(output, FLAGS_output);
}


#ifndef RNDCMP_INCLUDE_FLOATSR_HPP_
#define RNDCMP_INCLUDE_FLOATSR_HPP_

#include <type_traits>
#include <cmath>
#include <random>
#include <iostream>
#include <Eigen/Core>

namespace rndcmp {
    // Мask for 29 low-cut bits for double
    constexpr int64_t res32_mask = 0x1fffffff;
    // Epsilon for double to float conversion
    constexpr int64_t eps32 = res32_mask + 1;

    constexpr float float_max = std::numeric_limits<float>::max();
    constexpr float float_min = std::numeric_limits<float>::lowest();

    static std::random_device floating_rd;
    static std::mt19937 int_generator = std::mt19937(floating_rd());
    static std::uniform_int_distribution<int64_t> dist32 = std::uniform_int_distribution<int64_t>(0, res32_mask);

    class FloatSR {
    public:
        FloatSR() = default;

        FloatSR(float v): value(v) {}

        template<typename T>
        FloatSR(T v, std::enable_if_t<std::is_floating_point<T>::value, bool> = true) {
            double double_val = static_cast<double>(v);
            round(double_val);
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        operator T() const {
            return static_cast<T>(value);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        operator T() const {
            return static_cast<T>(value);
        }

        /* + operators */

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        FloatSR operator+(const T& rhs) const {
            double v = static_cast<double>(value) + static_cast<double>(rhs);
            return FloatSR(v);
        }

        FloatSR operator+(const FloatSR& rhs) const {
            double v =  static_cast<double>(value) + static_cast<double>(rhs);
            return FloatSR(v);
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend FloatSR operator+(T lhs, const FloatSR& rhs) {
            double v = static_cast<double>(lhs) + static_cast<double>(rhs);
            return FloatSR(v);
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        FloatSR& operator+=(const T& rhs) {
            double v = static_cast<double>(*this) + static_cast<double>(rhs);
            round(v);
            return *this;
        }

        FloatSR& operator+=(const FloatSR& rhs) {
            double v = static_cast<double>(*this) + static_cast<double>(rhs);
            round(v);
            return *this;
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        FloatSR operator+(const T& rhs) const {
            double v = static_cast<double>(*this) + rhs;
            return FloatSR(v);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend FloatSR operator+(T lhs, const FloatSR& rhs) {
            double v = lhs + static_cast<double>(rhs);
            return FloatSR(v);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        FloatSR& operator+=(const T& rhs) {
            double v = static_cast<double>(*this) + rhs;
            round(v);
            return *this;
        }

        /* - operators */

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        FloatSR operator-(const T& rhs) const {
            double v = static_cast<double>(*this) - static_cast<double>(rhs);
            return FloatSR(v);
        }

        FloatSR operator-(const FloatSR& rhs) const {
            double v = static_cast<double>(*this) - static_cast<double>(rhs);
            return FloatSR(v);
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        FloatSR& operator-=(const T& rhs) {
            double v = static_cast<double>(*this) - static_cast<double>(rhs);
            round(v);
            return *this;
        }

        FloatSR& operator-=(const FloatSR& rhs) {
            double v = static_cast<double>(value) - static_cast<double>(rhs);
            round(v);
            return *this;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend FloatSR operator-(T lhs, const FloatSR& rhs) {
            double v = static_cast<double>(lhs) - static_cast<double>(rhs);
            return FloatSR(v);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        FloatSR operator-(const T& rhs) const {
            double v = static_cast<double>(*this) - static_cast<double>(rhs);
            return FloatSR(v);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend FloatSR operator-(T lhs, const FloatSR& rhs) {
            double v = static_cast<double>(lhs) - static_cast<double>(rhs);
            return FloatSR(v);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        FloatSR& operator-=(const T& rhs) {
            double v = static_cast<double>(*this) - rhs;
            round(v);
            return *this;
        }

        // Unary minus
        FloatSR operator-() const {
            return FloatSR(-value);
        }

        /* multiply operators */

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        FloatSR operator*(const T& rhs) const {
            double v = static_cast<double>(*this) * static_cast<double>(rhs);
            return FloatSR(v);
        }

        FloatSR operator*(const FloatSR& rhs) const {
            double v = static_cast<double>(*this) * static_cast<double>(rhs);
            return FloatSR(v);
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        FloatSR& operator*=(const T& rhs) {
            double v = static_cast<double>(*this) * static_cast<double>(rhs);
            round(v);
            return *this;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend FloatSR operator*(T lhs, const FloatSR& rhs) {
            double v = static_cast<double>(lhs) * static_cast<double>(rhs);
            return FloatSR(v);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        FloatSR operator*(const T& rhs) const {
            double v = static_cast<double>(*this) * rhs;
            return FloatSR(v);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend FloatSR operator*(T lhs, const FloatSR& rhs) {
            double v = lhs * static_cast<double>(rhs);
            return FloatSR(v);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        FloatSR& operator*=(const T& rhs) {
            double v = static_cast<double>(*this) * rhs;
            round(v);
            return *this;
        }

        FloatSR& operator*=(const FloatSR& rhs) {
            double v = static_cast<double>(value) * static_cast<double>(rhs);
            round(v);
            return *this;
        }

        /* divide operators */

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        FloatSR operator/(const T& rhs) const {
            double v = static_cast<double>(*this) / static_cast<double>(rhs);
            return FloatSR(v);
        }

        FloatSR operator/(const FloatSR& rhs) const {
            double v = static_cast<double>(*this) / static_cast<double>(rhs);
            return FloatSR(v);
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        FloatSR& operator/=(const T& rhs) {
            double v = static_cast<double>(*this) / static_cast<double>(rhs);
            round(v);
            return *this;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend FloatSR operator/(T lhs, const FloatSR& rhs) {
            double v = static_cast<double>(lhs) / static_cast<double>(rhs);
            return FloatSR(v);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        FloatSR operator/(const T& rhs) const {
            double v = static_cast<double>(*this) / rhs;
            return FloatSR(v);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend FloatSR operator/(T lhs, const FloatSR& rhs) {
            double v = lhs / static_cast<double>(rhs);
            return FloatSR(v);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        FloatSR& operator/=(const T& rhs) {
            double v = static_cast<double>(*this) / rhs;
            round(v);
            return *this;
        }

        FloatSR& operator/=(const FloatSR& rhs) {
            double v = static_cast<double>(value) / static_cast<double>(rhs);
            round(v);
            return *this;
        }

        /* comparison operators */

        /* less */

        bool operator<(const FloatSR& rhs) const {
            return value < rhs.value;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bool operator<(const T& rhs) const {
            return value < rhs;
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bool operator<(const T& rhs) const {
            return value < rhs;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend bool operator<(const T& rhs, const FloatSR& lhs) {
            return static_cast<double>(rhs) < static_cast<double>(lhs);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend bool operator<(const T& rhs, const FloatSR& lhs) {
            return static_cast<double>(rhs) < static_cast<double>(lhs);
        }

        /* greater */

        bool operator>(const FloatSR& rhs) const {
            return value > rhs.value;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bool operator>(const T& rhs) const {
            return value > rhs;
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bool operator>(const T& rhs) const {
            return value > rhs;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend bool operator>(const T& rhs, const FloatSR& lhs) {
            return static_cast<double>(rhs) > static_cast<double>(lhs);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend bool operator>(const T& rhs, const FloatSR& lhs) {
            return static_cast<double>(rhs) > static_cast<double>(lhs);
        }

        /* equal */

        bool operator==(const FloatSR& rhs) const {
            return value == rhs.value;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bool operator==(const T& rhs) const {
            return value == rhs;
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bool operator==(const T& rhs) const {
            return value == rhs;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend bool operator==(const T& rhs, const FloatSR& lhs) {
            return static_cast<double>(rhs) == static_cast<double>(lhs);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend bool operator==(const T& rhs, const FloatSR& lhs) {
            return static_cast<double>(rhs) == static_cast<double>(lhs);
        }

        /* not equal */

        bool operator!=(const FloatSR& rhs) const {
            return value != rhs.value;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bool operator!=(const T& rhs) const {
            return value != rhs;
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bool operator!=(const T& rhs) const {
            return value != rhs;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend bool operator!=(const T& rhs, const FloatSR& lhs) {
            return static_cast<double>(rhs) != static_cast<double>(lhs);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend bool operator!=(const T& rhs, const FloatSR& lhs) {
            return static_cast<double>(rhs) != static_cast<double>(lhs);
        }

        /* less or equal */

        bool operator<=(const FloatSR& rhs) const {
            return value <= rhs.value;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bool operator<=(const T& rhs) const {
            return value <= rhs;
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bool operator<=(const T& rhs) const {
            return value <= rhs;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend bool operator<=(const T& rhs, const FloatSR& lhs) {
            return static_cast<double>(rhs) <= static_cast<double>(lhs);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend bool operator<=(const T& rhs, const FloatSR& lhs) {
            return static_cast<double>(rhs) <= static_cast<double>(lhs);
        }

        /* greater or equal */

        bool operator>=(const FloatSR& rhs) const {
            return value >= rhs.value;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bool operator>=(const T& rhs) const {
            return value >= rhs;
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bool operator>=(const T& rhs) const {
            return value >= rhs;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend bool operator>=(const T& rhs, const FloatSR& lhs) {
            return static_cast<double>(rhs) >= static_cast<double>(lhs);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend bool operator>=(const T& rhs, const FloatSR& lhs) {
            return static_cast<double>(rhs) >= static_cast<double>(lhs);
        }

        /* ostream overload */
        friend std::ostream& operator<<(std::ostream& os, const FloatSR& v) {
            os << float(v);
            return os;
        }

        /* Trigonometric functions */
        friend inline FloatSR cos(const FloatSR&  x)  { return cos(static_cast<double>(x)); }
        friend inline FloatSR sin(const FloatSR&  x)  { return sin(static_cast<double>(x)); }
        friend inline FloatSR tan(const FloatSR&  x)  { return tan(static_cast<double>(x)); }
        friend inline FloatSR acos(const FloatSR&  x)  { return acos(static_cast<double>(x)); }
        friend inline FloatSR asin(const FloatSR&  x)  { return asin(static_cast<double>(x)); }
        friend inline FloatSR atan(const FloatSR&  x)  { return atan(static_cast<double>(x)); }

        /* Hyperbolic functions */
        friend inline FloatSR cosh(const FloatSR&  x)  { return cosh(static_cast<double>(x)); }
        friend inline FloatSR sinh(const FloatSR&  x)  { return sinh(static_cast<double>(x)); }
        friend inline FloatSR tanh(const FloatSR&  x)  { return tanh(static_cast<double>(x)); }
        friend inline FloatSR acosh(const FloatSR&  x)  { return acosh(static_cast<double>(x)); }
        friend inline FloatSR asinh(const FloatSR&  x)  { return asinh(static_cast<double>(x)); }
        friend inline FloatSR atanh(const FloatSR&  x)  { return atanh(static_cast<double>(x)); }

        /* Exponential and logarithmic functions */
        friend inline FloatSR exp(const FloatSR&  x)  { return exp(static_cast<double>(x)); }
        friend inline FloatSR log(const FloatSR&  x)  { return log(static_cast<double>(x)); }
        friend inline FloatSR log10(const FloatSR&  x)  { return log10(static_cast<double>(x)); }
        friend inline FloatSR logb(const FloatSR&  x)  { return logb(static_cast<double>(x)); }

        /* Power functions */
        friend inline FloatSR pow(const FloatSR&  base, double exponent)  { return pow(static_cast<double>(base), exponent); }
        friend inline FloatSR sqrt(const FloatSR&  x)  { return sqrt(static_cast<double>(x)); }
        friend inline FloatSR cbrt(const FloatSR&  x)  { return cbrt(static_cast<double>(x)); }

        friend inline FloatSR scalbn(const FloatSR&  x, int n)  { return scalbn(static_cast<double>(x), n); }

        /* Other functions */
        friend inline FloatSR abs(const FloatSR&  x)  { return abs(static_cast<double>(x)); }
        friend inline FloatSR fabs(const FloatSR&  x)  { return fabs(static_cast<double>(x)); }
        friend inline FloatSR abs2(const FloatSR& x)  { return x*x; }

        friend inline FloatSR copysign(const FloatSR&  x1, const FloatSR& x2)  { return copysign(static_cast<double>(x1), static_cast<double>(x2)); }
        friend inline FloatSR fmax(const FloatSR&  x1, const FloatSR&  x2) { return x1 < x2 ? x1 : x2; }
        friend inline bool isfinite(const FloatSR& x) { return true; }

    private:
        void round(double x) {
            // Interpret double as int64 for better bits manipulating
            int64_t x_int = reinterpret_cast<int64_t&>(x);
            // Truncate lower bits
            int64_t x_tr = x_int & ~res32_mask;
            // Generate random value
            int64_t p = dist32(int_generator);
            if (p < (x_int & res32_mask)) {
                x_tr += eps32;
            }
            double result = reinterpret_cast<double&>(x_tr);

            // Check for overflow
            if (result > float_max) {
                value = float_max;
            } else if (result < float_min) {
                value = float_min;
            } else {
                value = static_cast<float>(result);
            }
        }

        float value;
    };
}

namespace Eigen {
    template<> struct NumTraits<rndcmp::FloatSR>: NumTraits<float> {
        typedef rndcmp::FloatSR Real;
        typedef rndcmp::FloatSR NonInteger;
        typedef rndcmp::FloatSR Nested;

        enum {
            IsComplex = 0,
            IsInteger = 0,
            IsSigned = 1,
            RequireInitialization = 1,
            ReadCost = 2,
            AddCost = 6,
            MulCost = 8
        };
    };
}

#endif  // RNDCMP_INCLUDE_FLOATSR_HPP_


/*
 * OBMoleculeAnalytic.cpp
 *
 *  Created on: Mar 14, 2012
 *      Author: dkoes
 */

#include "OBMoleculeAnalytic.h"
#include <boost/multi_array.hpp>
#include "CommandLine2/CommandLine.h"

extern cl::opt<bool> Verbose;
using namespace boost;
//maintain a bounding box
struct BoundingBox
{
	double minx;
	double maxx;
	double miny;
	double maxy;
	double minz;
	double maxz;

	BoundingBox() :
			minx(HUGE_VAL), maxx(-HUGE_VAL), miny(HUGE_VAL), maxy(-HUGE_VAL), minz(
					HUGE_VAL), maxz(-HUGE_VAL)
	{
	}

	void update(double x, double y, double z)
	{
		if (x < minx)
			minx = x;
		if (x > maxx)
			maxx = x;
		if (y < miny)
			miny = y;
		if (y > maxy)
			maxy = y;
		if (z < minz)
			minz = z;
		if (z > maxz)
			maxz = z;
	}

	//add amount to all dimensions
	void extend(double amount)
	{
		minx -= amount;
		maxx += amount;
		miny -= amount;
		maxy += amount;
		minz -= amount;
		maxz += amount;
	}

	//x,y,z within box
	bool contains(double x, double y, double z) const
	{
		if (x >= minx && x <= maxx && y >= miny && y <= maxy && z >= minz
				&& z <= maxz)
		{
			return true;
		}
		return false;
	}
};

struct AtomPoint
{
	double x;
	double y;
	double z;
	unsigned interactingCnt;

	AtomPoint() :
			x(0), y(0), z(0), interactingCnt(0)
	{
	}
	AtomPoint(double _x, double _y, double _z) :
			x(_x), y(_y), z(_z), interactingCnt(0)
	{
	}

	double distSq(double X, double Y, double Z) const
	{
		return (x - X) * (x - X) + (y - Y) * (y - Y) + (z - Z) * (z - Z);
	}

};

typedef pair<unsigned, vector<unsigned> > ClusterAdjList;

static bool neighSorter(const ClusterAdjList& lhs, const ClusterAdjList& rhs)
{
	return lhs.second.size() < rhs.second.size();
}

//checks that adding c to cluster won't violate maxClusterDist
static bool goodForCluster(const vector<unsigned>& cluster, unsigned c, const multi_array<double, 2>& distances, double maxdistSq)
{
	for(unsigned i = 0, n = cluster.size(); i < n; i++)
	{
		unsigned index = cluster[i];
		if(distances[index][c] > maxdistSq)
		{
			return false;
		}
	}
	return true;
}

//create clusters of points; clusters contains indices into points
static void clusterPoints(const vector<AtomPoint>& points,
		double maxClusterDist, vector<vector<unsigned> >& clusters)
{
	unsigned N = points.size();
	multi_array<double, 2> distances(extents[N][N]);

	//compute distances array and count close neighbors
	vector<ClusterAdjList> closeNeighbors(N);
	double maxdistSq = maxClusterDist * maxClusterDist;

	for (unsigned i = 0; i < N; i++)
	{
		closeNeighbors[i].first = i;
		for (unsigned j = 0; j < N; j++)
		{
			distances[i][j] = points[i].distSq(points[j].x, points[j].y,
					points[j].z);

			if (i != j && distances[i][j] <= maxdistSq)
			{
				closeNeighbors[i].second.push_back(j);
			}
		}
	}

	//sort point indices by how many neighbors are within the cutoff
	sort(closeNeighbors.begin(), closeNeighbors.end(), neighSorter);

	clusters.clear();
	vector<int> clusterAssign(N, -1);
	//greedily assign to clusters
	for (unsigned i = 0; i < N; i++)
	{
		unsigned index = closeNeighbors[i].first;
		if (clusterAssign[index] < 0) //unassigned, create new cluster
		{
			unsigned cluster = clusters.size();
			clusters.push_back(vector<unsigned>());
			clusters.back().push_back(index);

			//now consider all close neighbors for inclusion in cluster
			for (unsigned j = 0, m = closeNeighbors[i].second.size(); j < m;
					j++)
			{
				unsigned neigh = closeNeighbors[i].second[j];
				if(clusterAssign[neigh] < 0 && goodForCluster(clusters.back(), neigh, distances, maxdistSq))
				{
					clusters.back().push_back(neigh);
					clusterAssign[neigh] = cluster;
				}
			}

		}
	}
}

//computes a set of "interaction points" spatial location indicative of the ligand protein interface
void OBAMolecule::computeInteractionPoints(OBMol& rmol, vector<Eigen::Vector3d>& respoints,
		double interactionDist, double maxClusterDist, unsigned minClusterPoints)
{
	respoints.clear();
	//first construct a bounding box for the ligand while assembling a
	//vector of atomic coordinates
	BoundingBox ligandBox;
	vector<AtomPoint> points;
	points.reserve(mol.NumAtoms());
	for (OBAtomIterator aitr = mol.BeginAtoms(); aitr != mol.EndAtoms(); ++aitr)
	{
		OBAtom* atom = *aitr;
		points.push_back(AtomPoint(atom->x(), atom->y(), atom->z()));
		ligandBox.update(atom->x(), atom->y(), atom->z());
	}
	ligandBox.extend(interactionDist);

	//then identify all coordinates that are interacting
	double idistSq = interactionDist * interactionDist;

	for (OBAtomIterator aitr = rmol.BeginAtoms(); aitr != rmol.EndAtoms();
			++aitr)
	{
		OBAtom* a = *aitr;
		if (ligandBox.contains(a->x(), a->y(), a->z()))
		{
			for (unsigned i = 0, n = points.size(); i < n; i++)
			{
				if (points[i].distSq(a->x(), a->y(), a->z()) <= idistSq)
				{
					points[i].interactingCnt++;
				}
			}
		}
	}

	//prune out non-interacting points
	vector<AtomPoint> tmp;
	tmp.reserve(points.size());
	for (unsigned i = 0, n = points.size(); i < n; i++)
	{
		if (points[i].interactingCnt > 0)
			tmp.push_back(points[i]);
	}
	points.swap(tmp);
	tmp.clear();

	//cluster these coordinates
	vector<vector<unsigned> > clusters;
	clusterPoints(points, maxClusterDist, clusters);

	//make the cluster centers the interaction grid points
	for (unsigned i = 0, n = clusters.size(); i < n; i++)
	{
		double xtot = 0, ytot = 0, ztot = 0;
		unsigned npts = clusters[i].size();

		if (npts >= minClusterPoints)
		{
			for (unsigned j = 0; j < npts; j++)
			{
				xtot += points[clusters[i][j]].x;
				ytot += points[clusters[i][j]].y;
				ztot += points[clusters[i][j]].z;
			}
			double xave = xtot / (double) npts;
			double yave = ytot / (double) npts;
			double zave = ztot / (double) npts;

			respoints.push_back(Eigen::Vector3d(xave,yave,zave));
		}
	}
}

//computes a set of solitary grid points that represent the interaction between
//this ligand and the provided receptor in some way
void OBAMolecule::computeInteractionGridPoints(OBMol& rmol,
		MGrid& grid, double interactionDist,
		double maxClusterDist,
		unsigned minClusterPoints, double interactionPointRadius)
{
	using namespace Eigen;
	grid.clear();

	vector<Vector3d> points;
	computeInteractionPoints(rmol, points, interactionDist, maxClusterDist, minClusterPoints);

	for(unsigned i = 0, n = points.size(); i < n; i++)
	{
		if(interactionPointRadius >= 0)
		{
			Vector3d pt = points[i];
			grid.markXYZSphere(pt.x(),pt.y(),pt.z(),interactionPointRadius);
		}
	}
}


/*
 * img_align.cpp
 *
 *  Created on: Aug 22, 2012
 *      Author: cforster
 */

#include <vector>
#include <string>
#include <stdint.h>
#include <stdio.h>
#include <iostream>
#include <math.h>
#include <Eigen/Core>
#include <Eigen/StdVector>
#include <opencv2/opencv.hpp>
#include <vikit/math_utils.h>
#include <vikit/vision.h>
#include <vikit/pinhole_camera.h>
#include <vikit/nlls_solver.h>
#include <vikit/performance_monitor.h>
#include <vikit/img_align.h>
#include <sophus/se3.h>

namespace vk
{

/*******************************************************************************
 * Forward Compositional
 */
ForwardCompositionalSE3::
ForwardCompositionalSE3 ( vector<PinholeCamera>& cam_pyr,
                          vector<cv::Mat>& depth_pyr,
                          vector<cv::Mat>& img_pyr,
                          vector<cv::Mat>& tpl_pyr,
                          vector<cv::Mat>& img_pyr_dx,
                          vector<cv::Mat>& img_pyr_dy,
                          SE3& init_model,
                          int n_levels,
                          int n_iter,
                          float res_thresh,
                          bool display,
                          Method method,
                          int test_id ) :
    cam_pyr_ ( cam_pyr ),
    depth_pyr_ ( depth_pyr ),
    img_pyr_ ( img_pyr ),
    tpl_pyr_ ( tpl_pyr ),
    img_pyr_dx_ ( img_pyr_dx ),
    img_pyr_dy_ ( img_pyr_dy ),
    display_ ( display ),
    log_ ( test_id < 0 ),
    res_thresh_ ( res_thresh )
{
    n_iter_ = n_iter;
    method_ = method;

    // Init Performance Monitor
#if 0
    if ( log_ )
    {
        permon_.init ( "forward", ros::package::getPath ( "rpl_examples" ) + "/trace/img_align/data",
                       test_id, true );
        permon_.addLog ( "iter" );
        permon_.addLog ( "level" );
        permon_.addLog ( "mu" );
        permon_.addLog ( "chi2" );
        permon_.addLog ( "trials" );
    }
#endif

    runOptimization ( init_model );

}

ForwardCompositionalSE3::
ForwardCompositionalSE3 ( vector<PinholeCamera>& cam_pyr,
                          vector<cv::Mat>& depth_pyr,
                          vector<cv::Mat>& img_pyr,
                          vector<cv::Mat>& tpl_pyr,
                          vector<cv::Mat>& img_pyr_dx,
                          vector<cv::Mat>& img_pyr_dy,
                          int n_levels,
                          int n_iter,
                          float res_thresh,
                          bool display,
                          Method method,
                          int test_id ) :
    cam_pyr_ ( cam_pyr ),
    depth_pyr_ ( depth_pyr ),
    img_pyr_ ( img_pyr ),
    tpl_pyr_ ( tpl_pyr ),
    img_pyr_dx_ ( img_pyr_dx ),
    img_pyr_dy_ ( img_pyr_dy ),
    display_ ( display ),
    log_ ( test_id < 0 ),
    res_thresh_ ( res_thresh )
{
    n_iter_ = n_iter;
    method_ = method;

    // Init Performance Monitor
#if 0
    if ( log_ )
    {
        permon_.init ( "forward", ros::package::getPath ( "rpl_examples" ) + "/trace/img_align/data",
                       test_id, true );
        permon_.addLog ( "iter" );
        permon_.addLog ( "level" );
        permon_.addLog ( "mu" );
        permon_.addLog ( "chi2" );
        permon_.addLog ( "trials" );
    }
#endif

}

void ForwardCompositionalSE3::
runOptimization ( SE3& model, int levelBegin, int levelEnd )
{
    if ( levelBegin < 0 || levelBegin > n_levels_-1 )
        levelBegin = n_levels_-1;
    if ( levelEnd < 0 )
        levelEnd = 1;

    // Perform Pyramidal optimization
    for ( level_=levelBegin; level_>=levelEnd; --level_ )
    {
        mu_ = 0.1;
        cout << endl << "PYRAMID LEVEL " << level_
             << endl << "---------------" << endl;
        optimize ( model );
    }
}

double ForwardCompositionalSE3::
computeResiduals ( const SE3& model, bool linearize_system, bool compute_weight_scale )
{
    // Warp the image such that it aligns with the template image
    double chi2 = 0;
    size_t n_pixels = 0;

    if ( linearize_system )
        resimg_ = cv::Mat ( tpl_pyr_[level_].size(), CV_32F, cv::Scalar ( 1 ) );

    for ( int v=0; v<depth_pyr_[level_].rows; ++v )
    {
        for ( int u=0; u<depth_pyr_[level_].cols; ++u )
        {
            // compute pixel location in new img
            cv::Vec3f cv_float3 = depth_pyr_[level_].at<cv::Vec3f> ( v,u );
            Vector3d xyz_tpl ( cv_float3[0], cv_float3[1], cv_float3[2] );
            Vector3d xyz_img ( model*xyz_tpl );
            Vector2f uv_img_pyr = cam_pyr_[level_].world2cam ( xyz_img ).cast<float>(); // apply cam model
            if ( cam_pyr_[level_].isInFrame ( uv_img_pyr.cast<int>(), 2 ) )
            {
                // compare image values
                float intensity_tpl =
                    tpl_pyr_[level_].at<float> ( v,u );
                float intensity_img =
                    interpolateMat_32f ( img_pyr_[level_], uv_img_pyr[0], uv_img_pyr[1] );

                // compute residual (opposite to 2d case because of other jacobian)
                float res = intensity_tpl-intensity_img;

                // robustification
                if ( res > res_thresh_ )  res = res_thresh_;
                if ( res < -res_thresh_ ) res = -res_thresh_;
                chi2 += res*res;
                n_pixels++;

                if ( linearize_system )
                {
                    // get gradient of warped image (~gradient at warped position)
                    float dx = 0.5*interpolateMat_32f ( img_pyr_dx_[level_], uv_img_pyr[0], uv_img_pyr[1] );
                    float dy = 0.5*interpolateMat_32f ( img_pyr_dy_[level_], uv_img_pyr[0], uv_img_pyr[1] );

                    // evaluate jacobian
                    Matrix<double,2,6> frame_jac;
                    frameJac_xyz2uv ( xyz_img, cam_pyr_[level_].fx(), frame_jac );

                    // compute steppest descent images
                    Vector6d J = dx*frame_jac.row ( 0 ) + dy*frame_jac.row ( 1 );

                    // compute Hessian and
                    H_ += J*J.transpose();
                    Jres_ += J*res;

                    resimg_.at<float> ( v,u ) = -res;
                }
            }
        }
    }
    chi2 /= n_pixels;
    return chi2;
}

int ForwardCompositionalSE3::
solve()
{
    x_ = H_.ldlt().solve ( -Jres_ );
    if ( ( bool ) std::isnan ( ( double ) x_[0] ) )
        return 0;
    return 1;
}

void ForwardCompositionalSE3::
update ( const ModelType& old_model,  ModelType& new_model )
{
    new_model = SE3::exp ( x_ ) * ( old_model );
}

void ForwardCompositionalSE3::
startIteration()
{
#if 0
    if ( log_ )
        permon_.newMeasurement();
#endif
}

void ForwardCompositionalSE3::
finishIteration()
{
#if 0
    if ( log_ )
    {
        permon_.log ( "iter", iter_ );
        permon_.log ( "level", level_ );
        permon_.log ( "mu", mu_ );
        permon_.log ( "chi2", chi2_ );
        permon_.log ( "trials", n_trials_ );
    }
#endif

    if ( display_ )
    {
        cv::namedWindow ( "residuals", CV_WINDOW_AUTOSIZE );
        cv::imshow ( "residuals", resimg_*3 );
        cv::waitKey ( 0 );
    }
}

/*******************************************************************************
 * Efficient Second Order Minimization (ESM)
 */
SecondOrderMinimisationSE3::
SecondOrderMinimisationSE3 ( vector<PinholeCamera>& cam_pyr,
                             vector<cv::Mat>& depth_pyr,
                             vector<cv::Mat>& img_pyr,
                             vector<cv::Mat>& tpl_pyr,
                             vector<cv::Mat>& img_pyr_dx,
                             vector<cv::Mat>& img_pyr_dy,
                             vector<cv::Mat>& tpl_pyr_dx,
                             vector<cv::Mat>& tpl_pyr_dy,
                             SE3& init_model,
                             int n_levels,
                             int n_iter,
                             float res_thresh,
                             bool display,
                             Method method,
                             int test_id ) :
    cam_pyr_ ( cam_pyr ),
    depth_pyr_ ( depth_pyr ),
    img_pyr_ ( img_pyr ),
    tpl_pyr_ ( tpl_pyr ),
    img_pyr_dx_ ( img_pyr_dx ),
    img_pyr_dy_ ( img_pyr_dy ),
    tpl_pyr_dx_ ( tpl_pyr_dx ),
    tpl_pyr_dy_ ( tpl_pyr_dy ),
    display_ ( display ),
    log_ ( test_id < 0 ),
    res_thresh_ ( res_thresh )
{
    n_iter_ = n_iter;
    method_ = method;
    verbose_ = false;

#if 0
    if ( log_ )
    {
        // Init Performance Monitor
        permon_.init ( "esm", ros::package::getPath ( "rpl_examples" ) + "/trace/img_align/data",
                       test_id, true );
        permon_.addLog ( "iter" );
        permon_.addLog ( "level" );
        permon_.addLog ( "mu" );
        permon_.addLog ( "chi2" );
        permon_.addLog ( "trials" );
    }
#endif

    // perform pyramidal optimization
    for ( level_=n_levels-1; level_>2; --level_ )
        //level_ = n_levels-1;
    {
        // Optimize
        mu_ = 0.01f;
        if ( display_ )
        {
            cout << endl << "PYRAMID LEVEL " << level_
                 << endl << "patch-width = " << img_pyr_[level_].cols
                 << endl << "---------------" << endl;
        }
        optimize ( init_model );
    }
}

double SecondOrderMinimisationSE3::
computeResiduals ( const SE3& model, bool linearize_system, bool compute_weight_scale )
{
    // Warp the image such that it aligns with the template image
    double chi2 = 0;
    size_t n_pixels = 0;

    // TODO: to improve access speed, use a pointer and increment every iteration

    // Compute Warp
    cv::Mat mask = cv::Mat_<bool> ( tpl_pyr_[level_].rows, tpl_pyr_[level_].cols, false );
    cv::Mat img_warped = cv::Mat_<float> ( tpl_pyr_[level_].rows, tpl_pyr_[level_].cols, 1.0 );

    for ( int v=0; v<depth_pyr_[level_].rows; ++v )
    {
        for ( int u=0; u<depth_pyr_[level_].cols; ++u )
        {
            // compute pixel location in new img
            cv::Vec3f cv_float3 = depth_pyr_[level_].at<cv::Vec3f> ( v,u );
            Vector3d xyz_tpl ( cv_float3[0], cv_float3[1], cv_float3[2] );
            Vector3d xyz_img ( model*xyz_tpl );
            Vector2f uv_img_pyr = cam_pyr_[level_].world2cam ( xyz_img ).cast<float>(); // apply cam model
            if ( cam_pyr_[level_].isInFrame ( uv_img_pyr.cast<int>(), 1 ) )
            {
                img_warped.at<float> ( v,u ) = interpolateMat_32f ( img_pyr_[level_], uv_img_pyr[0], uv_img_pyr[1] );

                if ( cam_pyr_[level_].isInFrame ( uv_img_pyr.cast<int>(), 2 ) )
                    mask.at<bool> ( v,u ) = true;
            }
        }
    }

    // Compute Warp derivative
    cv::Mat img_warped_dx, img_warped_dy;
    cv::Sobel ( img_warped, img_warped_dx, CV_32F, 1, 0, 1 );
    cv::Sobel ( img_warped, img_warped_dy, CV_32F, 0, 1, 1 );

    // Compute Jacobian
    if ( linearize_system )
        resimg_ = cv::Mat_<float> ( tpl_pyr_[level_].size(), 1.0 );

    for ( int v=0; v<depth_pyr_[level_].rows; ++v )
    {
        for ( int u=0; u<depth_pyr_[level_].cols; ++u )
        {
            if ( mask.at<bool> ( v,u ) )
            {
                // compare image values
                float intensity_tpl = tpl_pyr_[level_].at<float> ( v,u );
                float intensity_img = img_warped.at<float> ( v,u );

                // compute residual  (opposite to 2d case because of other jacobian)
                float res = intensity_tpl-intensity_img;

                // robustification
                if ( res > res_thresh_ )  res = res_thresh_;
                if ( res < -res_thresh_ ) res = -res_thresh_;
                chi2 += res*res;
                n_pixels++;

                if ( linearize_system )
                {
                    // 0.25 because we have two 0.5 factors. First from adding the two gradients
                    // and the second when we compute the sobel mask
                    float dx = 0.25* ( tpl_pyr_dx_[level_].at<float> ( v,u ) + img_warped_dx.at<float> ( v,u ) );
                    float dy = 0.25* ( tpl_pyr_dy_[level_].at<float> ( v,u ) + img_warped_dy.at<float> ( v,u ) );

                    // evaluate jacobian
                    cv::Vec3f cv_float3 = depth_pyr_[level_].at<cv::Vec3f> ( v,u );
                    Vector3d xyz_tpl ( cv_float3[0], cv_float3[1], cv_float3[2] );
                    Vector3d xyz_img ( model*xyz_tpl );
                    Matrix<double,2,6> frame_jac;
                    frameJac_xyz2uv ( xyz_tpl, cam_pyr_[level_].fx(), frame_jac );

                    // compute steppest descent images
                    Vector6d J = dx*frame_jac.row ( 0 ) + dy*frame_jac.row ( 1 );

                    // compute Hessian
                    H_ += J*J.transpose();
                    Jres_ += J*res;
                    resimg_.at<float> ( v,u ) = res;
                }
            }
        }
    }
    chi2 /= n_pixels;
    return chi2;
}

int SecondOrderMinimisationSE3::
solve()
{
    x_ = H_.ldlt().solve ( -Jres_ );
    if ( ( bool ) std::isnan ( ( double ) x_[0] ) )
        return 0;
    return 1;
}

void SecondOrderMinimisationSE3::
update ( const ModelType& old_model,  ModelType& new_model )
{
    new_model = SE3::exp ( x_ ) *old_model;
}

void SecondOrderMinimisationSE3::
startIteration()
{
#if 0
    if ( log_ )
        permon_.newMeasurement();
#endif
}

void SecondOrderMinimisationSE3::
finishIteration()
{
#if 0
    if ( log_ )
    {
        permon_.log ( "iter", iter_ );
        permon_.log ( "level", level_ );
        permon_.log ( "mu", mu_ );
        permon_.log ( "chi2", chi2_ );
        permon_.log ( "trials", n_trials_ );
    }
#endif

    if ( display_ )
    {
        cv::namedWindow ( "residuals", CV_WINDOW_AUTOSIZE );
        cv::imshow ( "residuals", resimg_*3 );
        cv::waitKey ( 0 );
    }
}

} // end namespace vk


#include <hdl_graph_slam/graph_slam.hpp>

#include <boost/format.hpp>
#include <g2o/stuff/macros.h>
#include <g2o/core/factory.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/linear_solver.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/robust_kernel_factory.h>
#include <g2o/core/optimization_algorithm_factory.h>
#include <g2o/solvers/pcg/linear_solver_pcg.h>
#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/types/slam3d/edge_se3_pointxyz.h>
#include <g2o/types/slam3d_addons/types_slam3d_addons.h>
#include <g2o/edge_se3_plane.hpp>
#include <g2o/edge_se3_priorxy.hpp>
#include <g2o/edge_se3_priorxyz.hpp>
#include <g2o/edge_se3_priorvec.hpp>
#include <g2o/edge_se3_priorquat.hpp>

G2O_USE_OPTIMIZATION_LIBRARY(pcg)
G2O_USE_OPTIMIZATION_LIBRARY(cholmod)
G2O_USE_OPTIMIZATION_LIBRARY(csparse)

namespace g2o {
  G2O_REGISTER_TYPE(EDGE_SE3_PLANE, EdgeSE3Plane)
  G2O_REGISTER_TYPE(EDGE_SE3_PRIORXY, EdgeSE3PriorXY)
  G2O_REGISTER_TYPE(EDGE_SE3_PRIORXYZ, EdgeSE3PriorXYZ)
  G2O_REGISTER_TYPE(EDGE_SE3_PRIORVEC, EdgeSE3PriorVec)
  G2O_REGISTER_TYPE(EDGE_SE3_PRIORQUAT, EdgeSE3PriorQuat)
}

namespace hdl_graph_slam {

/**
 * @brief constructor
 */
GraphSLAM::GraphSLAM(const std::string& solver_type) {
  graph.reset(new g2o::SparseOptimizer());

  std::cout << "construct solver... " << std::endl;
  g2o::OptimizationAlgorithmFactory* solver_factory = g2o::OptimizationAlgorithmFactory::instance();
  g2o::OptimizationAlgorithmProperty solver_property;
  g2o::OptimizationAlgorithm* solver = solver_factory->construct(solver_type, solver_property);
  graph->setAlgorithm(solver);

  if (!graph->solver()) {
    std::cerr << std::endl;
    std::cerr << "error : failed to allocate solver!!" << std::endl;
    solver_factory->listSolvers(std::cerr);
    std::cerr << "-------------" << std::endl;
    std::cin.ignore(1);
    return;
  }
  std::cout << "done" << std::endl;

  robust_kernel_factory = g2o::RobustKernelFactory::instance();
}

/**
 * @brief destructor
 */
GraphSLAM::~GraphSLAM() {
  graph.reset();
}

//下面三个函数设定g2o的节点

g2o::VertexSE3* GraphSLAM::add_se3_node(const Eigen::Isometry3d& pose) {
  g2o::VertexSE3* vertex(new g2o::VertexSE3());             //设定g2o的节点
  vertex->setId(static_cast<int>(graph->vertices().size()));
  vertex->setEstimate(pose);
  graph->addVertex(vertex);

  return vertex;
}

g2o::VertexPlane* GraphSLAM::add_plane_node(const Eigen::Vector4d& plane_coeffs) {
  g2o::VertexPlane* vertex(new g2o::VertexPlane());
  vertex->setId(static_cast<int>(graph->vertices().size()));
  vertex->setEstimate(plane_coeffs);
  graph->addVertex(vertex);

  return vertex;
}

g2o::VertexPointXYZ* GraphSLAM::add_point_xyz_node(const Eigen::Vector3d& xyz) {
  g2o::VertexPointXYZ* vertex(new g2o::VertexPointXYZ());
  vertex->setId(static_cast<int>(graph->vertices().size()));
  vertex->setEstimate(xyz);
  graph->addVertex(vertex);

  return vertex;
}

//这个应该用作里程计的边
g2o::EdgeSE3* GraphSLAM::add_se3_edge(g2o::VertexSE3* v1, g2o::VertexSE3* v2, const Eigen::Isometry3d& relative_pose, const Eigen::MatrixXd& information_matrix) {
  g2o::EdgeSE3* edge(new g2o::EdgeSE3());
  edge->setMeasurement(relative_pose);
  edge->setInformation(information_matrix);
  edge->vertices()[0] = v1;
  edge->vertices()[1] = v2;
  graph->addEdge(edge);

  return edge;
}

g2o::EdgeSE3Plane* GraphSLAM::add_se3_plane_edge(g2o::VertexSE3* v_se3, g2o::VertexPlane* v_plane, const Eigen::Vector4d& plane_coeffs, const Eigen::MatrixXd& information_matrix) {
  g2o::EdgeSE3Plane* edge(new g2o::EdgeSE3Plane());
  edge->setMeasurement(plane_coeffs);
  edge->setInformation(information_matrix);
  edge->vertices()[0] = v_se3;
  edge->vertices()[1] = v_plane;
  graph->addEdge(edge);

  return edge;
}

//g2o::VertexPointXYZ是路标点的坐标(每一帧中的路标点)
g2o::EdgeSE3PointXYZ* GraphSLAM::add_se3_point_xyz_edge(g2o::VertexSE3* v_se3, g2o::VertexPointXYZ* v_xyz, const Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix) {
  g2o::EdgeSE3PointXYZ* edge(new g2o::EdgeSE3PointXYZ());
  edge->setMeasurement(xyz);
  edge->setInformation(information_matrix);
  edge->vertices()[0] = v_se3;
  edge->vertices()[1] = v_xyz;
  graph->addEdge(edge);

  return edge;
}

g2o::EdgeSE3PriorXY* GraphSLAM::add_se3_prior_xy_edge(g2o::VertexSE3* v_se3, const Eigen::Vector2d& xy, const Eigen::MatrixXd& information_matrix) {
  g2o::EdgeSE3PriorXY* edge(new g2o::EdgeSE3PriorXY());       //gps边的形式
  edge->setMeasurement(xy);
  edge->setInformation(information_matrix);
  edge->vertices()[0] = v_se3;
  graph->addEdge(edge);

  return edge;
}

g2o::EdgeSE3PriorXYZ* GraphSLAM::add_se3_prior_xyz_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix) {
  g2o::EdgeSE3PriorXYZ* edge(new g2o::EdgeSE3PriorXYZ());
  edge->setMeasurement(xyz);
  edge->setInformation(information_matrix);
  edge->vertices()[0] = v_se3;
  graph->addEdge(edge);

  return edge;
}

//imu的加入加速度和角度的边的形式不一样,如下所示:
g2o::EdgeSE3PriorVec* GraphSLAM::add_se3_prior_vec_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& direction, const Eigen::Vector3d& measurement, const Eigen::MatrixXd& information_matrix) {
  Eigen::Matrix<double, 6, 1> m;
  m.head<3>() = direction;
  m.tail<3>() = measurement;

  g2o::EdgeSE3PriorVec* edge(new g2o::EdgeSE3PriorVec());    //图优化中加速度边的形式
  edge->setMeasurement(m);
  edge->setInformation(information_matrix);
  edge->vertices()[0] = v_se3;
  graph->addEdge(edge);

  return edge;
}

g2o::EdgeSE3PriorQuat* GraphSLAM::add_se3_prior_quat_edge(g2o::VertexSE3* v_se3, const Eigen::Quaterniond& quat, const Eigen::MatrixXd& information_matrix) {
  g2o::EdgeSE3PriorQuat* edge(new g2o::EdgeSE3PriorQuat());    //角度边的形式
  edge->setMeasurement(quat);
  edge->setInformation(information_matrix);
  edge->vertices()[0] = v_se3;
  graph->addEdge(edge);

  return edge;
}

void GraphSLAM::add_robust_kernel(g2o::OptimizableGraph::Edge* edge, const std::string& kernel_type, double kernel_size) {
  if(kernel_type == "NONE") {
    return;
  }

  g2o::RobustKernel* kernel = robust_kernel_factory->construct(kernel_type);
  if(kernel == nullptr) {
    std::cerr << "warning : invalid robust kernel type: " << kernel_type << std::endl;
    return;
  }

  kernel->setDelta(kernel_size);
  edge->setRobustKernel(kernel);
}

void GraphSLAM::optimize(int num_iterations) {
  if(graph->edges().size() < 10) {
    return;
  }

  std::cout << std::endl;
  std::cout << "--- pose graph optimization ---" << std::endl;
  std::cout << "nodes: " << graph->vertices().size() << "   edges: " << graph->edges().size() << std::endl;
  std::cout << "optimizing... " << std::flush;

  graph->initializeOptimization();
  graph->computeInitialGuess();
  graph->computeActiveErrors();
  graph->setVerbose(false);

  double chi2 = graph->chi2();

  auto t1 = ros::Time::now();
  int iterations = graph->optimize(num_iterations);

  auto t2 = ros::Time::now();
  std::cout << "done" << std::endl;
  std::cout << "iterations: " << iterations << " / " << num_iterations << std::endl;
  std::cout << "chi2: (before)" << chi2 << " -> (after)" << graph->chi2() << std::endl;
  std::cout << "time: " << boost::format("%.3f") % (t2 - t1).toSec() << "[sec]" << std::endl;
}

void GraphSLAM::save(const std::string& filename) {
  std::ofstream ofs(filename);
  graph->save(ofs);
}

}


#include <cassert>
#include <ctime>
#include <cctype>
#include <string>
#include <random>
#include <chrono>
#include <iostream>
#include <sstream>
#include <filesystem>

#include <type_alias.h>
#include <types.h>
#include <input_parser.h>
#include <go_types.h>
#include <splitmix.h>
#include <null_distribution.h>
#include <pytorch_util.h>
#include <go_action_encoder.h>
#include <go_zero_encoder.h>
#include <model_base.h>
#include <zero_model_small.h>
#include <zero_agent.h>

#include <torch/torch.h>

#include <boost/program_options.hpp>

#include <Wt/WApplication.h>
#include <Wt/WServer.h>
#include <Wt/WContainerWidget.h>
#include <Wt/WText.h>
#include <Wt/WPushButton.h>
#include <Wt/WLineEdit.h>

namespace s = std;
namespace t = torch;
namespace R = rlgames;
namespace w = Wt;
namespace po= boost::program_options;

constexpr ubyte SZ = 9;
constexpr uint action_size = R::ZeroGoActionEncoder<SZ>::action_size();
using ModelContainerType = R::ModelContainer<R::ZeroModelSmall, R::ZeroGoStateEncoder<SZ>, R::ZeroGoActionEncoder<SZ>, t::optim::Adam>;
using AgentType = R::ZeroAgent<ModelContainerType, R::null_distribution<action_size>, R::Splitmix, R::GoBoard<SZ>, R::GoGameState<SZ>, action_size>;

s::string model_config;
s::string model_params;
s::string optimizer_params;

s::string wrap_string(const s::string& msg){
  s::string ret("
");
  ret += msg;
  ret += "
";
  return ret;
}

s::string board_to_html(const s::string& board_str){
  s::string ret("
");
  for (uint i = 0; i < board_str.length(); ++i){
    if (board_str[i] != '\n')
      ret += board_str[i];
    else
      ret += "

";
  }
  ret += "
";
  return ret;
}

class GoGameWidget : public w::WContainerWidget {
public:
  GoGameWidget():
    mGoGameState(),
    mDevice(t::Device(t::kCPU)),
    mModelContainer(
      R::ZeroModelSmall(R::ZeroGoStateEncoder<SZ>().state_size(), action_size, R::load_model_option<R::ZeroModelSmallOptions>(model_config)),
      R::ZeroGoStateEncoder<SZ>(),
      R::ZeroGoActionEncoder<SZ>(),
      1E-5F),
    mAgent(mModelContainer, mDevice, 1200, 0, 0., 0., rand()){

    // setup widget
    mBoard = addWidget(s::make_unique<w::WText>(""));
    addWidget(s::make_unique<w::WBreak>());
    mUserInput = addWidget(s::make_unique<w::WLineEdit>());
    mUserInput->setMaxLength(3);
    mPlayButton = addWidget(s::make_unique<w::WPushButton>("play"));
    mResetButton = addWidget(s::make_unique<w::WPushButton>("restart"));
    addWidget(s::make_unique<w::WBreak>());
    mStatus = addWidget(s::make_unique<w::WText>(""));

    mPlayButton->clicked().connect(this, &GoGameWidget::play);
    mResetButton->clicked().connect(this, &GoGameWidget::reset);

    display_board();

    R::load_model(mModelContainer, model_params, optimizer_params, mDevice);
    mModelContainer.model->to(mDevice);
  }
private:
  R::GoGameState<SZ> mGoGameState;
  t::Device          mDevice;
  ModelContainerType mModelContainer;
  AgentType          mAgent;

  w::WText*       mBoard;
  w::WText*       mStatus;
  w::WLineEdit*   mUserInput;
  w::WPushButton* mPlayButton;
  w::WPushButton* mResetButton;

  void play(){
    s::string input_str = mUserInput->valueText().toUTF8();
    mUserInput->setValueText("");
    mStatus->setText(wrap_string("processing..."));

    if (mGoGameState.is_over())
      return;

    R::Move move = R::string_to_move(input_str, SZ);
    if (move.mty == R::M::Unknown){
      mStatus->setText(w::WString(wrap_string("invalid move input. format: E5")));
      return;
    }

    if (not mGoGameState.is_valid_move(move)){
      mStatus->setText(w::WString(wrap_string("move is invalid on this board. please retry")));
      return;
    }

    mGoGameState.apply_move(move);

    R::Move agent_move = mAgent.select_move(mGoGameState);

    mGoGameState.apply_move(agent_move);

    display_board();
  }

  void reset(){
    mGoGameState = R::GoGameState<SZ>();
    display_board();
  }

  void display_board(){
    s::stringstream ss;
    mGoGameState.board().print(ss);
    s::string s = ss.str();
    mBoard->setText(w::WString(board_to_html(s)));
  }
};

s::unique_ptr<w::WApplication> createApplication(const w::WEnvironment& env){
  auto app = w::cpp14::make_unique<w::WApplication>(env);

  app->setTitle("Go");

  app->root()->addWidget(w::cpp14::make_unique<GoGameWidget>());
  app->useStyleSheet("css/go.css");
  return app;
}

int main(int argc, char* argv[]){
  po::options_description desc("Parameters");
  try {
    srand(time(nullptr));

    desc.add_options()
      ("model_config", po::value<s::string>()->required(), "model hyperparameter configuration file")
      ("model_params", po::value<s::string>()->required(), "model parameters file")
      ("optimizer_params", po::value<s::string>()->required(), "optimizer parameters file")
      ("docroot", po::value<s::string>()->required(), "root to the document path")
      ("http-address", po::value<s::string>()->required(), "http server address")
      ("http-port", po::value<int>()->required(), "http server port");
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    model_config = vm["model_config"].as<s::string>();
    model_params = vm["model_params"].as<s::string>();
    optimizer_params = vm["optimizer_params"].as<s::string>();

    w::WServer server(argc, argv, WTHTTP_CONFIGURATION);
    server.addEntryPoint(w::EntryPointType::Application, createApplication);

    server.run();
  } catch (po::error& e){
    s::cerr << e.what() << s::endl;
    s::cerr << desc << s::endl;
  } catch (w::WServer::Exception& e){
    s::cerr << e.what() << s::endl;
  } catch (s::exception& e){
    s::cerr << "exception: " << e.what() << s::endl;
  }
}


#include "felzenszwalbsegmentation.h"
#include "implementation/segment-image.h"
#include "implementation/image.h"
#include "implementation/misc.h"
#include "commands/felzenszwalbconfig.h"
#include <iostream>
#include <fstream>
#include <opencv2/highgui/highgui.hpp>
#include <boost/serialization/tracking.hpp>

namespace superpixels
{

FelzenszwalbSegmentation::FelzenszwalbSegmentation(vole::FelzenszwalbConfig config)
	: SuperpixelSegmentation(std::string("felzenszwalb"))
{
	localCfg = new vole::FelzenszwalbConfig();

	localCfg->input_file  = config.input_file;
	localCfg->input_file_16_bit  = config.input_file_16_bit;
	localCfg->min_intensity = config.min_intensity;
	localCfg->max_intensity = config.max_intensity;
	localCfg->output_file = config.output_file;
	localCfg->isGraphical = config.isGraphical;
	localCfg->kThreshold  = config.kThreshold;
	localCfg->sigma       = config.sigma;
	localCfg->min_size    = config.min_size;

	this->config = localCfg;
}

FelzenszwalbSegmentation::FelzenszwalbSegmentation(int min_size, int kThreshold, double sigma) :
	SuperpixelSegmentation(std::string("felzenszwalb"))
{
	localCfg = new vole::FelzenszwalbConfig();

	localCfg->input_file   = ""; // Problem: when hashing over the configuration,
							  // the hash values differ from runs where input
							  // and output files are known by using the other
							  // constructor
	localCfg->input_file_16_bit = "";
	localCfg->min_intensity = 0;
	localCfg->max_intensity = 1;
	localCfg->output_file = "";
	localCfg->isGraphical = false;
	localCfg->kThreshold  = kThreshold;
	localCfg->sigma       = sigma;
	localCfg->min_size    = min_size;

	this->config = localCfg;
}

FelzenszwalbSegmentation::~FelzenszwalbSegmentation()
{
	// superclass is deleting config in its destructor, hence also localCfg. We don't need to delete here. Weird/ugly but true.
}

std::vector<Superpixel> FelzenszwalbSegmentation::superpixels(const cv::Mat_<cv::Vec3b>& image)
{
	unsigned long int hashKey = 0;
	if (openCache(cache_dir) && isCaching()) {
		hashKey = localCfg->configHash();
		readFromCache(hashKey);
		//std::vector<superpixels::SuperpixelSegmentation::Superpixel> segmentation = readFromCache(hashKey);
		if (_superpixels.size() > 0) return _superpixels;
	}

	height = image.rows;
	width  = image.cols;

	felzenszwalb::image<felzenszwalb::rgb>* inputImage = new felzenszwalb::image<felzenszwalb::rgb>(width, height);

	for (int y = 0; y < height; y++) {
		for (int  x = 0; x < width; x++) {
			imRef(inputImage, x, y).r = image(y, x)[2];
			imRef(inputImage, x, y).g = image(y, x)[1];
			imRef(inputImage, x, y).b = image(y, x)[0];
		}
	}

	int numSegments;
	felzenszwalb::image<felzenszwalb::rgb>* segmentation
		= felzenszwalb::segment_image(inputImage,
			localCfg->sigma,
			localCfg->kThreshold,
			localCfg->min_size,
			&numSegments);

	_superpixels = std::vector<Superpixel>(numSegments);
	std::vector<Superpixel>::iterator sup;
	for (sup = _superpixels.begin(); sup != _superpixels.end(); sup++) {
		sup->bbox = cv::Rect(INT_MAX, INT_MAX, INT_MIN, INT_MIN);
	}

	std::map<int, int> labels;
	int label = 0;

	for (int y = 0; y < height; y++) {
		for (int  x = 0; x < width; x++) {
			cv::Point coord(x, y);
			int value = (imRef(segmentation, x, y).r << 16) + (imRef(segmentation, x, y).g << 8) + imRef(segmentation, x, y).b;

			if (labels.find(value) == labels.end()) {
				if (label >= numSegments) {
					std::cerr << "Error in felzenszwalb superpixel segmentation!";
					_superpixels.clear();
					labels.clear();
					break;
				}

				labels[value] = label;
				label++;
			}

			Superpixel &sup = _superpixels[labels[value]];
			sup.coordinates.push_back(coord);
			sup.bbox.x      = std::min(sup.bbox.x,      coord.x);
			sup.bbox.width  = std::max(sup.bbox.width,  coord.x);
			sup.bbox.y      = std::min(sup.bbox.y,      coord.y);
			sup.bbox.height = std::max(sup.bbox.height, coord.y);
		}
	}

	for (sup = _superpixels.begin(); sup != _superpixels.end(); sup++) {
		sup->bbox.width  = sup->bbox.width  - sup->bbox.x + 1;
		sup->bbox.height = sup->bbox.height - sup->bbox.y + 1;
	}

	delete inputImage;
	delete segmentation;

	if (isCaching()) {
		writeToCache(hashKey);
	}

	return _superpixels;
}

std::vector<Superpixel> FelzenszwalbSegmentation::superpixels(const cv::Mat_<cv::Vec3d>& image)
{
	cv::Mat_<cv::Vec3b> inputImage;
	image.convertTo(inputImage, 8, 255);
	return superpixels(inputImage);
}

/*
std::string FelzenszwalbSegmentation::getCacheIdentifier()
{
	return std::string("felzenszwalb");
}
*/

/*
std::string FelzenszwalbSegmentation::getStorageFilename(std::string dir, unsigned long int hashKey)
{
	std::stringstream s;
	s << dir << "/" << "felzenszwalb_" << hashKey << ".png";
	return s.str();
//	return localCfg->output_file;
}
*/

void FelzenszwalbSegmentation::addCachingInfo(std::string input_file, std::string input_file_16_bit, std::string output_file, bool isGraphical)
{
	localCfg->input_file = input_file;
	localCfg->input_file_16_bit = input_file_16_bit;
	localCfg->output_file = output_file;
	localCfg->isGraphical = isGraphical;
}

bool FelzenszwalbSegmentation::writeToCache(unsigned long int hashKey)
{
	// open file streams, serialize
	std::ofstream out;
	out.open(getStorageFilename(cache_dir, hashKey).c_str());
	if (!out.good()) return false;

	{
		boost::archive::text_oarchive oa(out);
		oa << *this;
	} // stream is closed when archive is destructed

	return true;
}

bool FelzenszwalbSegmentation::readFromCache(unsigned long int hashKey)
{
	// open file streams, serialize
	std::ifstream in;
	in.open(getStorageFilename(cache_dir, hashKey).c_str());
	if (!in.good()) return false;

	{
		boost::archive::text_iarchive ia(in);
		ia >> *this;
	} // stream is closed when archive is destructed

	return true;
}

} // namespace superpixels


#include "atomsciflow/parser/cube.h"

#include <fstream>
#include <iostream>
#include <regex>
#include <cstdlib>
//#include <numbers>
#include <set>
#include <map>
#include <armadillo>
#include <iomanip>
#include <string>
#include <cmath>
#include <boost/algorithm/string.hpp>

#ifdef _OPENMP
#include <omp.h>
#endif // _OPENMP

#include "atomsciflow/base/crystal.h"
#include "atomsciflow/base/atom.h"
#include "atomsciflow/base/element.h"

namespace atomsciflow {

void CubeElectronDensity::read_cube(std::string filepath) {

    int i, j, k;

    std::ifstream cubefile;

    cubefile.open(filepath);

    std::string line;
    std::vector<std::string> cube;

    while (std::getline(cubefile, line)) {
        // ignore empty lines
        std::istringstream iss(line);
        std::vector<std::string> line_split((std::istream_iterator<std::string>(iss)), std::istream_iterator<std::string>());
        if (line_split.size() != 0) {
            cube.push_back(line);
        }
    }
    cubefile.close();

    double bohr_to_angstrom = 0.529177249;
    // read structure info
    std::vector<std::string> vec_str;
    boost::split(vec_str, cube[2], boost::is_space(), boost::token_compress_on);
    int natom = int(std::atof(vec_str[1].c_str())); // it might be negative, if MO infor are included in cube file

    arma::mat latcell(3, 3);
    this->crystal.cell.clear();
    for ( i = 0; i < 3; i++) {
        boost::split(vec_str, cube[i+3], boost::is_space(), boost::token_compress_on);
        for (j = 0; j < 3; j++) {
            std::cout << vec_str[1] << " + " << vec_str[j+2] << " " << std::endl;
            latcell(i, j) = int(std::atof(vec_str[1].c_str())) * std::atof(vec_str[j+2].c_str()) * bohr_to_angstrom;
        }
        std::cout << "split string" << cube[i+3] << "\n";
        this->crystal.cell.push_back(arma::conv_to<std::vector<double>>::from(latcell.row(i)));
    }

    std::cout << this->crystal.cell[0][0] << " " << this->crystal.cell[0][1] << " " <<this->crystal.cell[0][2] << std::endl;
    std::cout << this->crystal.cell[1][0] << " " << this->crystal.cell[1][1] << " " <<this->crystal.cell[1][2] << std::endl;
    std::cout << this->crystal.cell[2][0] << " " << this->crystal.cell[2][1] << " " <<this->crystal.cell[2][2] << std::endl;

    std::string label;
    std::map<std::string, Element> ele_num_map = get_element_number_map();
    for (i = 0; i < natom; i++) {
        boost::split(vec_str, cube[i+6], boost::is_space(), boost::token_compress_on);
        int atomic_number = int(std::atof(vec_str[0].c_str()));
        for (auto e : ele_num_map) {
            if (ele_num_map[e.first].number == atomic_number) {
                label = e.first;
            }
        }
        Atom atm;
        atm.set_name(label);
        boost::split(vec_str, cube[i+6], boost::is_space(), boost::token_compress_on);
        atm.set_xyz(
            std::atof(vec_str[2].c_str()) * bohr_to_angstrom,
            std::atof(vec_str[3].c_str()) * bohr_to_angstrom,
            std::atof(vec_str[4].c_str()) * bohr_to_angstrom
        );
        this->crystal.atoms.push_back(atm);
    }

    double a = arma::norm(latcell.row(0), 2);
    double b = arma::norm(latcell.row(1), 2);
    double c = arma::norm(latcell.row(2), 2);

    // assume three cube file have the same ngridx ngridy and ngridz
    boost::split(vec_str, cube[3], boost::is_space(), boost::token_compress_on);
    int ngridx = int(std::atof(vec_str[1].c_str()));
    boost::split(vec_str, cube[4], boost::is_space(), boost::token_compress_on);
    int ngridy = int(std::atof(vec_str[1].c_str()));
    boost::split(vec_str, cube[5], boost::is_space(), boost::token_compress_on);
    int ngridz = int(std::atof(vec_str[1].c_str()));

    this->ngridx = ngridx;
    this->ngridy = ngridy;
    this->ngridz = ngridz;

    std::cout << "ngrid: " << this->ngridx << " " << this->ngridy << " " << this->ngridz << std::endl;

    // read grid value
    std::string tmp;
    vec_str.clear();

    // std::cout << "natom: " << natom << std::endl;
    #pragma omp parallel for private(tmp)
    for (i = natom + 6; i < cube.size(); i++) {
        tmp = cube[i];
        // boost::algorithm::replace_all(tmp, "\n", ";");
        boost::algorithm::replace_all(tmp, "\n", " ");
        vec_str.push_back(tmp);
    }

    std::string tmp_str = boost::algorithm::join(vec_str, " ");

    arma::vec dat_vec(tmp_str);

    this->data.set_size(ngridx, ngridy, ngridz);
    // charge data in cube file is in shape (ngridx, ngridy, ngridz)
    // while charge in *CHG* file is in shape (ngzf, ngyf, ngxf)
    // they are different!
    // grid data in cube is iterated in different compared to *CHG* of vasp
    // data = dat.reshape(ngridx, ngridy, ngridz);
    for (i = 0; i < ngridx; i++) {
        for (j = 0; j < ngridy; j++) {
            for (k = 0; k < ngridz; k++) {
                //this->data.at(i, j, k) = dat_vec.at(i*ngridx + j*ngridy + k*ngridz); // this is wrong
                this->data.at(i, j, k) = dat_vec.at(i*ngridy*ngridz + j*ngridx + k);
                // std::cout << this->data.at(i, j, k) << "\n";
            }
        }
    }

    std::cout << "data: last" << std::endl;
    std::cout << this->data.at(ngridx-1, ngridy-1, ngridz-1) << std::endl;

}

} // namespace atomsciflow


///////////////////////////////////////////////////////////////////////////////////////////
// survival::test::model::functions.cpp                                                  //
//                                                                                //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                            //
//  Software License, Version 1.0. (See accompanying file                                //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)                     //
///////////////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_TEST_MODEL_FUNCTIONS_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_TEST_MODEL_FUNCTIONS_HPP_ER_2009
#include <ostream>
#include <boost/concept/assert.hpp>
#include <boost/fusion/container/map.hpp>
#include <boost/fusion/include/map.hpp>
#include <boost/fusion/include/map_fwd.hpp>

#include <boost/statistics/detail/distribution/survival/response/types/right_truncated/include.hpp>
#include <boost/statistics/detail/distribution/survival/models/exponential/scalar/model.hpp>
#include <boost/statistics/detail/distribution/survival/models/exponential/scalar/function/log_unnormalized_pdf.hpp>

// Concepts must come after distributions
#include <boost/statistics/detail/distribution_common/concept/unary/log_unnormalized_pdf.hpp>
//#include <boost/statistics/detail/distribution_common/concept/unary/derivative_log_unnormalized_pdf.hpp>

#include <boost/statistics/detail/distribution_common/meta/wrapper/distribution.hpp>
#include <boost/statistics/detail/distribution_common/meta/wrapper/variable.hpp>
#include <boost/statistics/detail/distribution_common/meta/wrapper/probability.hpp>

#include <boost/statistics/detail/distribution/survival/response/types/right_truncated/is_event.hpp>

#include <boost/fusion/sequence/intrinsic/at_key.hpp>
#include <boost/fusion/include/at_key.hpp>

template <typename T>
void test_model_functions(std::ostream& os)
{
    os << "test_functions - >" << std::endl;
    using namespace boost;

    namespace stat      = boost::statistics::detail;
    namespace dist		= stat::distribution;
    namespace dist_mo   = dist::model;
    namespace dist_su   = dist::survival;

    typedef fusion::map<
        fusion::pair<dist_mo::tag::covariate,T>,
        fusion::pair<
            dist_mo::tag::response,
            dist_su::response::event<T>
        >
    > unit_;

    typedef dist::meta::wrapper_variable<unit_> u_;
    typedef dist::meta::wrapper_probability<T,u_> p_u_;

    {
        typedef dist_su::exponential_model<T>              			m_;
        typedef dist::meta::wrapper_distribution<m_,p_u_>          	w_;

        BOOST_CONCEPT_ASSERT((
            dist::concept::LogUnnormalizedPdf<w_>
        ));
    }

}

#endif


// Copyright 2014, D. E. Shaw Research.
// 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 )

#include "m128_traits.hpp"
#include <boost/random/counter_based_engine.hpp>
#include <boost/timer.hpp>
#include <iostream>

using namespace boost::random;

// This is the ARS pseudo-random function from Salmon et al. 2011,
// implemented using the AESNI _mm_aesenc_si128 hardware intrinsics.
// On Intel hardware with the AESNI instructions, it's the fastest
// Crush-resistant RNG we know of.  It runs at 2.7nsec/uint64
// on a 3.07GHz Xeon, i.e., just about 1 cycle-per-byte.
//
// This is in examples/ rather than boost/random because these
// intrinsics (_mm_aesenc_si128) and data types (__m128i) are too
// machine/compiler-specific for the main 'boost' tree.  Nevertheless,
// this example demonstrates that it's possible (and not that hard) to
// create hardware-dependent Prfs  that can be used by the standard
// counter_based_engine.
template <unsigned R=7>
struct ars_prf{
    typedef __m128i domain_type;
    typedef __m128i range_type;
    typedef __m128i key_type ;

    range_type operator()(domain_type v){
        __m128i kweyl = _mm_set_epi64x(UINT64_C(0xBB67AE8584CAA73B), /* sqrt(3) - 1.0 */
                                       UINT64_C(0x9E3779B97F4A7C15)); /* golden ratio */
        __m128i kk = k;
        if( R>1 ){
            kk = _mm_add_epi64(kk, kweyl);
            v = _mm_aesenc_si128(v, kk);
        }
        if( R>2 ){
            kk = _mm_add_epi64(kk, kweyl);
            v = _mm_aesenc_si128(v, kk);
        }
        if( R>3 ){
            kk = _mm_add_epi64(kk, kweyl);
            v = _mm_aesenc_si128(v, kk);
        }
        if( R>4 ){
            kk = _mm_add_epi64(kk, kweyl);
            v = _mm_aesenc_si128(v, kk);
        }
        if( R>5 ){
            kk = _mm_add_epi64(kk, kweyl);
            v = _mm_aesenc_si128(v, kk);
        }
        if( R>6 ){
            kk = _mm_add_epi64(kk, kweyl);
            v = _mm_aesenc_si128(v, kk);
        }
        if( R>7 ){
            kk = _mm_add_epi64(kk, kweyl);
            v = _mm_aesenc_si128(v, kk);
        }
        if( R>8 ){
            kk = _mm_add_epi64(kk, kweyl);
            v = _mm_aesenc_si128(v, kk);
        }
        if( R>9 ){
            kk = _mm_add_epi64(kk, kweyl);
            v = _mm_aesenc_si128(v, kk);
        }
        kk = _mm_add_epi64(kk, kweyl);
        v = _mm_aesenclast_si128(v, kk);
        return v;
    }

    ars_prf(key_type _k) : k(_k){
        //std::cerr << "ars_prf(key_type)\n";
    }

    ars_prf() { k = key_type(); }

    ars_prf(const ars_prf& v) : k(v.k) {
        //std::cerr << "ars_prf(const ars_prf&)\n";
    }

    void setkey(key_type _k){ k = _k; }
    key_type getkey() const { return k; }

    bool operator==(const ars_prf& rhs) const{
        return boost::random::detail::counter_traits<__m128i>::is_equal(k, rhs.k);
    }

    bool operator!=(const ars_prf& rhs) const{
        return !(k == rhs.k);
    }
protected:
    key_type k;
};

static const double cpu_frequency = 3.07e9;

void show_elapsed(double end, int iter, const std::string & name)
{
  double usec = end/iter*1e6;
  double cycles = usec * cpu_frequency/1e6;
  std::cout << name << ": "
            << usec*1e3 << " nsec/loop = "
            << cycles << " CPU cycles"
            << std::endl;
}

int main(int argc, char **argv){
    counter_based_engine<uint32_t, ars_prf<5> > e;
    std::cout << std::hex;
    std::cout << e() << "\n";
    std::cout << e() << "\n";
    std::cout << e() << "\n";
    std::cout << e() << "\n";
    std::cout << e() << "\n";
    std::cout << e() << "\n";
    std::cout << e() << "\n";
    std::cout << e() << "\n";

    std::cout << std::dec;
    std::cout << "state of the generator: " << e << "\n";

    counter_based_engine<uint32_t, ars_prf<5> > f;
    BOOST_ASSERT(e != f);

    // Aggressive parameters:  5 rounds is "Crush-resistant"
    // but with no safety margin.
    // 64-bit output minimizes loop-overhead.
    counter_based_engine<uint64_t, ars_prf<5> > g;
    uint32_t ss[4] = {1, 2, 3, 4};
    uint32_t *ss0 = &ss[0];
    g.seed(ss0, &ss[4]);
    size_t N = 100000000;
    boost::timer t;
    uint64_t tmp = 0;
    for(size_t i=0; i<N; ++i){
        tmp ^= g();
    }
    show_elapsed(t.elapsed(), N, "ars<5/64>");
    if(tmp == 0)
        std::cout << "tmp==0.  That's odd...";

    std::cout << "state of the generator: " << g << "\n";
    return 0;
}


#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <iostream>
#include <string>

namespace client{
  namespace qi = boost::spirit::qi;
  namespace ascii = boost::spirit::ascii;
  namespace phoenix = boost::phoenix;

  using qi::double_;
  using qi::_1;
  using ascii::space;
  using phoenix::ref;

  template <typename Iterator>
    bool adder(Iterator first, Iterator last, double& n){
      bool r = qi::phrase_parse(first, last,
          (
            double_[ref(n) = _1] >> *(',' >> double_[ref(n) += _1])
          ),
          space
          );

      if(first != last)
        return false;
      return r;
    }
}

int main(){
  std::string str;
  while(getline(std::cin, str)){
    if(str.empty() || str[0] == 'q' || str[0] == 'Q')
      break;

    double n;
    if(client::adder(str.begin(), str.end(), n)){
      std::cout << "Parsing succeeded\n";
      std::cout << "sum = " << n << std::endl;
    }
    else{
      std::cout << "Parsing failed\n";
    }
  }

  return 0;
}


#include "cli/humanize.h"
#include "cli/verbose.h"
#include <boost/ut.hpp>

using namespace boost::ut;

suite human_duration = [] {
    using namespace std::chrono;
    using namespace std::literals::chrono_literals;
    using namespace angonoka::cli;

    "very short duration"_test = [] {
        constexpr auto d = humanize{5s};
        const auto text = fmt::format("{}", d);
        expect(text == "a few seconds");
    };

    "15 seconds"_test = [] {
        constexpr auto d = humanize{15s};
        const auto text = fmt::format("{}", d);
        expect(text == "15 seconds");
    };

    "short duration"_test = [] {
        constexpr auto d = humanize{1min + 15s};
        const auto text = fmt::format("{}", d);
        expect(text == "about a minute");
    };

    "almost 2 min"_test = [] {
        constexpr auto d = humanize{1min + 50s};
        const auto text = fmt::format("{}", d);
        expect(text == "2 minutes");
    };

    "medium duration"_test = [] {
        constexpr auto d = humanize{1h + 15s};
        const auto text = fmt::format("{}", d);
        expect(text == "about an hour");
    };

    "2 hours"_test = [] {
        constexpr auto d = humanize{2h};
        const auto text = fmt::format("{}", d);
        expect(text == "2 hours");
    };

    "a day"_test = [] {
        constexpr auto d = humanize{23h};
        const auto text = fmt::format("{}", d);
        expect(text == "about a day");
    };

    "long duration"_test = [] {
        constexpr auto d = humanize{24h * 12 + 1h + 15s};
        const auto text = fmt::format("{}", d);
        expect(text == "12 days");
    };

    "very long duration"_test = [] {
        constexpr auto d = humanize{24h * 35};
        const auto text = fmt::format("{}", d);
        expect(text == "about a month");
    };

    "2 months"_test = [] {
        constexpr auto d = humanize{24h * 70};
        const auto text = fmt::format("{}", d);
        expect(text == "2 months");
    };

    "0 duration"_test = [] {
        constexpr auto d = humanize{0s};
        const auto text = fmt::format("{}", d);
        expect(text == "a few seconds");
    };
};

suite verbose_duration = [] {
    using namespace std::chrono;
    using namespace std::literals::chrono_literals;
    using namespace angonoka::cli;

    "very short duration"_test = [] {
        constexpr auto d = verbose{5s};
        const auto text = fmt::format("{}", d);
        expect(text == "5s");
    };

    "short duration"_test = [] {
        constexpr auto d = verbose{1min + 15s};
        const auto text = fmt::format("{}", d);
        expect(text == "1m 15s");
    };

    "almost 2 min"_test = [] {
        constexpr auto d = verbose{1min + 50s};
        const auto text = fmt::format("{}", d);
        expect(text == "1m 50s");
    };

    "medium duration"_test = [] {
        constexpr auto d = verbose{1h + 15s};
        const auto text = fmt::format("{}", d);
        expect(text == "1h 15s");
    };

    "long duration"_test = [] {
        constexpr auto d = verbose{24h * 12 + 1h + 15s};
        const auto text = fmt::format("{}", d);
        expect(text == "12d 1h 15s");
    };

    "very long duration"_test = [] {
        constexpr auto d = verbose{24h * 35};
        const auto text = fmt::format("{}", d);
        expect(text == "1mo 4d 13h 30m 54s");
    };

    "0 duration"_test = [] {
        constexpr auto d = verbose{0s};
        const auto text = fmt::format("{}", d);
        expect(text == "0s");
    };
};


/*=============================================================================
    Copyright (c) 2017 Paul Fultz II
    fix.cpp
    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)
==============================================================================*/
#include <boost/hof/fix.hpp>
#include <boost/hof/static.hpp>
#include <boost/hof/reveal.hpp>
#include <boost/hof/result.hpp>
#include "test.hpp"

#include <memory>

struct factorial_t
{
    template<class Self, class T>
    T operator()(Self s, T x) const noexcept
    {
        return x == 0 ? 1 : x * s(x-1);
    }
};

struct factorial_constexpr_t
{
    template<class Self, class T>
    constexpr T operator()(Self s, T x) const noexcept
    {
        return x == 0 ? 1 : x * s(x-1);
    }
};

struct factorial_move_t
{
    std::unique_ptr<int> i;
    factorial_move_t() : i(new int(1))
    {}
    template<class Self, class T>
    T operator()(const Self& s, T x) const
    {
        return x == 0 ? *i : x * s(x-1);
    }
};

static constexpr boost::hof::fix_adaptor<factorial_t> factorial = {};
static constexpr boost::hof::fix_adaptor<factorial_constexpr_t> factorial_constexpr = {};
static constexpr boost::hof::static_<boost::hof::fix_adaptor<factorial_move_t> > factorial_move = {};

#if BOOST_HOF_HAS_NOEXCEPT_DEDUCTION
BOOST_HOF_TEST_CASE()
{
    static_assert(noexcept(factorial(5)), "noexcept fix");
}
#endif

BOOST_HOF_TEST_CASE()
{
    const int r = factorial(5);
    BOOST_HOF_TEST_CHECK(r == 5*4*3*2*1);
}

BOOST_HOF_TEST_CASE()
{
    const int r = boost::hof::reveal(factorial)(5);
    BOOST_HOF_TEST_CHECK(r == 5*4*3*2*1);
}

#if !BOOST_HOF_NO_EXPRESSION_SFINAE
BOOST_HOF_TEST_CASE()
{
    const int r = boost::hof::fix(boost::hof::result<int>(factorial_constexpr_t()))(5);
    BOOST_HOF_TEST_CHECK(r == 5*4*3*2*1);
}

BOOST_HOF_TEST_CASE()
{
    const int r = boost::hof::result<int>(factorial_constexpr)(5);
    BOOST_HOF_TEST_CHECK(r == 5*4*3*2*1);
}
BOOST_HOF_TEST_CASE()
{
    BOOST_HOF_STATIC_TEST_CHECK(boost::hof::fix(boost::hof::result<int>(factorial_constexpr_t()))(5) == 5*4*3*2*1);
    BOOST_HOF_STATIC_TEST_CHECK(boost::hof::result<int>(factorial_constexpr)(5) == 5*4*3*2*1);
}
#endif
BOOST_HOF_TEST_CASE()
{
#if BOOST_HOF_HAS_GENERIC_LAMBDA
    auto factorial_ = boost::hof::fix([](auto s, auto x) -> decltype(x) { return x == 0 ? 1 : x * s(x-1); });
    int r = boost::hof::result<int>(factorial_)(5);
    BOOST_HOF_TEST_CHECK(r == 5*4*3*2*1);
#endif
}

BOOST_HOF_TEST_CASE()
{
    const int r = factorial_move(5);
    BOOST_HOF_TEST_CHECK(r == 5*4*3*2*1);
    BOOST_HOF_TEST_CHECK(boost::hof::fix(factorial_move_t())(5) == 5*4*3*2*1);
}


#include "ros_package_template/algorithm.hpp"

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <utility>

namespace ros_package_template
{

using namespace boost::accumulators;

struct Algorithm::Data
{
  accumulator_set<double, features<tag::mean, tag::count>> acc;
};

Algorithm::Algorithm() {data_ = std::make_unique<Data>();}

Algorithm::~Algorithm() = default;

void Algorithm::addData(const double data) {data_->acc(data);}

void Algorithm::addData(const Eigen::VectorXd & data)
{
  for (auto i = 0; i < data.size(); ++i) {
    addData(data[i]);
  }
}

double Algorithm::getAverage() const {return count(data_->acc) ? mean(data_->acc) : 0;}

}  // namespace ros_package_template


/*
 * serialize.cpp
 * Date: 2016-02-29
 * Author: Karsten Ahnert (karsten.ahnert@gmx.de)
 * Copyright: Karsten Ahnert
 *
 * 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)
 */

#include "serialize.hpp"

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

#include <sstream>
#include <functional>
#include <unordered_map>

#include <iostream>

namespace dynsys {

std::string serialize_winner( std::array< dynsys::tree_type , dim > const& winner , norm_type const& xnorm , norm_type const& ynorm )
{
    using namespace std;

    boost::property_tree::ptree pt;
    pt.put( "winner.w_1" , gpcxx::polish_string( winner[0] ) );
    pt.put( "winner.w_2" , gpcxx::polish_string( winner[1] ) );
    pt.put( "winner.w_3" , gpcxx::polish_string( winner[2] ) );

    pt.put( "norm.x_1.mean" , xnorm[0].first );
    pt.put( "norm.x_1.stdev" , xnorm[0].second );
    pt.put( "norm.x_2.mean" , xnorm[1].first );
    pt.put( "norm.x_2.stdev" , xnorm[1].second );
    pt.put( "norm.x_3.mean" , xnorm[2].first );
    pt.put( "norm.x_3.stdev" , xnorm[2].second );

    pt.put( "norm.y_1.mean" , ynorm[0].first );
    pt.put( "norm.y_1.stdev" , ynorm[0].second );
    pt.put( "norm.y_2.mean" , ynorm[1].first );
    pt.put( "norm.y_2.stdev" , ynorm[1].second );
    pt.put( "norm.y_3.mean" , ynorm[2].first );
    pt.put( "norm.y_3.stdev" , ynorm[2].second );

    std::ostringstream str;
    boost::property_tree::write_json( str , pt );
    return str.str();
}

deserialized_system deserialize_winner(const std::string& str )
{
    using namespace gpcxx;

    using mapped_func = std::pair< size_t , std::function< node_type() > > ;
    using map_type = std::unordered_map< std::string , mapped_func > ;
    // using mapper_type = std::function< mapped_func( std::string const& ) > ;

    auto map = map_type {
        std::make_pair( "+" , std::make_pair( 2 , []() { return node_type( plus_func {} , "+" ); } ) ) ,
        std::make_pair( "-" , std::make_pair( 2 , []() { return node_type( minus_func {} , "-" ); } ) ) ,
        std::make_pair( "*" , std::make_pair( 2 , []() { return node_type( multiplies_func {} , "*" ); } ) ) ,
        std::make_pair( "/" , std::make_pair( 2 , []() { return node_type( divides_func {} , "/" ); } ) ) ,
        std::make_pair( "c" , std::make_pair( 1 , []() { return node_type( cos_func {} , "c" ); } ) ) ,
        std::make_pair( "s" , std::make_pair( 1 , []() { return node_type( sin_func {} , "s" ); } ) ) ,
        std::make_pair( "e" , std::make_pair( 1 , []() { return node_type( exp_func {} , "e" ); } ) ) ,
        std::make_pair( "l" , std::make_pair( 1 , []() { return node_type( log_func {} , "l" ); } ) ) ,
        std::make_pair( "x" , std::make_pair( 0 , []() { return node_type( array_terminal<0> {} , "x" ); } ) ) ,
        std::make_pair( "y" , std::make_pair( 0 , []() { return node_type( array_terminal<1> {} , "y" ); } ) ) ,
        std::make_pair( "z" , std::make_pair( 0 , []() { return node_type( array_terminal<2> { } , "z" ); } ) )
    };

    auto mapper = [map]( std::string const& str ) {
        auto iter = map.find( str );
        if( iter != map.end() )
        {
            return iter->second;
        }
        else
        {
            auto value = std::stod( str );
            auto func = [str,value]() { return node_type( double_terminal< double > { value } , str ); };
            return mapped_func { std::make_pair( 0 , func ) };
        }
    };

    deserialized_system winner;
    boost::property_tree::ptree pt;
    std::istringstream istr { str };
    boost::property_tree::read_json( istr , pt );
    gpcxx::read_polish( pt.get< std::string >( "winner.w_1" ) , winner.trees[0] , mapper );
    gpcxx::read_polish( pt.get< std::string >( "winner.w_2" ) , winner.trees[1] , mapper );
    gpcxx::read_polish( pt.get< std::string >( "winner.w_3" ) , winner.trees[2] , mapper );

    winner.xnorm[0].first = pt.get< double >( "norm.x_1.mean" );
    winner.xnorm[0].second = pt.get< double >( "norm.x_1.stdev" );
    winner.xnorm[1].first = pt.get< double >( "norm.x_2.mean" );
    winner.xnorm[1].second = pt.get< double >( "norm.x_2.stdev" );
    winner.xnorm[2].first = pt.get< double >( "norm.x_3.mean" );
    winner.xnorm[2].second = pt.get< double >( "norm.x_3.stdev" );

    winner.ynorm[0].first = pt.get< double >( "norm.y_1.mean" );
    winner.ynorm[0].second = pt.get< double >( "norm.y_1.stdev" );
    winner.ynorm[1].first = pt.get< double >( "norm.y_2.mean" );
    winner.ynorm[1].second = pt.get< double >( "norm.y_2.stdev" );
    winner.ynorm[2].first = pt.get< double >( "norm.y_3.mean" );
    winner.ynorm[2].second = pt.get< double >( "norm.y_3.stdev" );

    return winner;
}

} // namespace dynsys