// 2019 Team AobaZero
// This is a work derived from Leela Zero (May 1, 2019).
/*
    This file is part of Leela Zero.
    Copyright (C) 2017-2019 Gian-Carlo Pascutto and contributors

    Leela Zero is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    Leela Zero is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with Leela Zero.  If not, see <http://www.gnu.org/licenses/>.

    Additional permission under GNU GPL version 3 section 7

    If you modify this Program, or any covered work, by linking or
    combining it with NVIDIA Corporation's libraries from the
    NVIDIA CUDA Toolkit and/or the NVIDIA CUDA Deep Neural
    Network library and/or the NVIDIA TensorRT inference library
    (or a modified version of those libraries), containing parts covered
    by the terms of the respective license agreement, the licensors of
    this Program grant you additional permission to convey the resulting
    work.
*/

#include "config.h"

#ifdef USE_OPENCL
#include <algorithm>
#include <array>
#include <cassert>
#include <iostream>
#include <sstream>
#include <string>
#include <map>
#include <random>
#include <cmath>
#include <fstream>
#ifndef USE_BLAS
#include <Eigen/Dense>
#endif

#include "GTP.h"
#include "OpenCL.h"
#include "Tuner.h"
#include "Utils.h"
#include "Random.h"

//const auto TUNER_FILE_LOCAL = std::string("leelaz_opencl_tuning");
const auto TUNER_FILE_LOCAL = std::string("aobaz_opencl_tuning");

template <typename net_t>
std::vector<std::string> Tuner<net_t>::tuned_devices;

#ifndef USE_BLAS
// Eigen helpers
template <typename T>
using EigenMatrixMap =
    Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>>;
template <typename T>
using ConstEigenMatrixMap =
    Eigen::Map<const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>>;
#endif

template <typename net_t> static std::string getTunerKernel();
template <typename net_t> static float getTunerMaxError();

template <> std::string getTunerKernel<float>() {
    return std::string("XgemmBatched");
}

template <> float getTunerMaxError<float>() {
    return 1e-4f;
}

#ifdef USE_HALF
template <> std::string getTunerKernel<half_float::half>() {
    return std::string("XgemmBatchedHalf");
}

template <> float getTunerMaxError<half_float::half>() {
    return 1e-1f;
}
#endif

using namespace Utils;

template <typename net_t>
static void sgemmBatched_ref(const std::vector<net_t>& a,
                             const std::vector<net_t>& b,
                             std::vector<net_t>& c,
                             const int m, const int n, const int k,
                             const int batch_size) {
    std::vector<float> ar(a.size());
    std::vector<float> br(b.size());
    std::vector<float> cr(c.size());

    std::copy(begin(a), end(a), begin(ar));
    std::copy(begin(b), end(b), begin(br));

    for (auto batch = 0; batch < batch_size; batch++) {
        auto offset_u = batch * m * k;
        auto offset_v = batch * n * k;
        auto offset_m = batch * m * n;
#ifdef USE_BLAS
        // Calculates C = transpose(tranpose(A) * B) in row major, or
        // C = A * transpose(B) in column major.
        for (auto i = 0; i < m; i++) {
            for (auto j = 0; j < n; j++) {
                auto acc = 0.0f;
                for (auto l = 0; l < k; l++) {
                    acc += ar[l * m + i + offset_u] * br[l * n + j + offset_v];
                }
                cr[j * m + i + offset_m] = acc;
            }
        }
#else
        auto C = EigenMatrixMap<float>(cr.data() + offset_m, m, n);
        auto A = ConstEigenMatrixMap<float>(ar.data() + offset_u, m, k);
        auto B = ConstEigenMatrixMap<float>(br.data() + offset_v, n, k);
        C.noalias() = (A * B.transpose());
#endif
    }

    std::copy(begin(cr), end(cr), begin(c));
}

static bool IsMultiple(const size_t a, const size_t b) {
    return (a % b == 0);
}

template <typename net_t>
bool Tuner<net_t>::valid_config_sgemm(Parameters p, bool exhaustive) {
    if (p["TCE"] == 0) {
        if (!IsMultiple(p["MWG"], p["MDIMC"]*p["VWM"])) {
            return false;
        }
        if (!IsMultiple(p["NWG"], p["NDIMC"]*p["VWN"])) {
            return false;
        }
        if (!IsMultiple(p["MWG"], p["MDIMA"]*p["VWM"])) {
            return false;
        }
        if (!IsMultiple(p["NWG"], p["NDIMB"]*p["VWN"])) {
            return false;
        }
        if (!IsMultiple(p["KWG"], p["MDIMC"]*p["NDIMC"]/p["MDIMA"])) {
            return false;
        }
        if (!IsMultiple(p["KWG"], p["MDIMC"]*p["NDIMC"]/p["NDIMB"])) {
            return false;
        }
        // Extra restrictions for a fast tuning run
        if (!exhaustive) {
            if (p["MDIMC"] != p["MDIMA"]) {
                return false;
            }
            if (p["NDIMC"] != p["NDIMB"]) {
                return false;
            }
            if (p["SA"] != p["SB"]) {
                return false;
            }
        }
    } else {
        if (!m_use_tensorcore) {
            return false;
        }

        // In Tensor Core implementations, MDIMA and NDIMB represents the
        // wmmv multiplication dimensions, that is,
        // m16n16k16 / m32n8k16 / m8n32k16.  Thus m * n is fixed to 256.
        if (p["MDIMA"] * p["NDIMB"] != 256) {
            return false;
        }
        if (p["MWG"] < p["MDIMC"]) {
            return false;
        }
        if (p["NWG"] < p["NDIMC"]) {
            return false;
        }
        if (p["MDIMC"] < p["MDIMA"]) {
            return false;
        }
        if (p["NDIMC"] < p["NDIMB"]) {
            return false;
        }
        // VWM / VWN has no meaning if we don't do SA / SB.
        // Only test VWM / VWN == 2
        if (p["SA"] == 0 && p["VWM"] != 2) {
            return false;
        }
        if (p["SB"] == 0 && p["VWN"] != 2) {
            return false;
        }
    }
    return true;
}

template <typename net_t>
Parameters Tuner<net_t>::get_parameters_by_int(
    const std::vector<Configurations>& opts, const int n) {

    Parameters param;
    std::vector<size_t> choices(opts.size());

    auto cfgs = 1;
    for (auto c = size_t{0}; c < opts.size(); c++) {
        choices[c] = opts[c].second.size();
        cfgs *= choices[c];
    }
    auto j = n;

    for (auto c = size_t{0}; c < opts.size(); c++) {
        auto o = opts[c];
        auto s = o.first;
        auto v = o.second[j % choices[c]];
        j /= choices[c];
        param[s] = v;
    }

    return param;
}

template <typename net_t>
std::string Tuner<net_t>::parameters_to_defines(const Parameters& p) {
    std::string s;
    for (auto const& x : p) {
        s += " -D" + x.first + "=" + std::to_string(x.second);
    }
    return s;
}

template <typename net_t>
std::string Tuner<net_t>::parameters_to_string(const Parameters& p) {
    std::string s;
    for (auto const& x : p) {
        s += x.first + "=" + std::to_string(x.second) + " ";
    }
    if (s.size() > 0) {
        s.resize(s.size() - 1);
    }
    return s;
}

static size_t next_power_of_two(const size_t x) {
    return 2 << size_t(std::ceil(std::log2(x)) - 1);
}

template <typename net_t>
static void sgemm_generate_data(std::vector<net_t> &x,
                                const int m, const int n,
                                const int batch_size,
                                const int m_ceil, const int n_ceil) {
    for (auto batch = 0; batch < batch_size; batch++) {
        for (auto i = 0; i < n_ceil; i++) {
            if (i < n) {
                for (auto j = 0; j < m; j++) {
                    x[batch*n_ceil*m_ceil + i*m_ceil + j] =
                        (( (i ^ j) + batch - 128) % 256) / 256.0f;
                }
                for (auto j = m; j < m_ceil; j++) {
                    x[batch*n_ceil*m_ceil + i*m_ceil + j] = 0.0f;
                }
            } else {
                for (auto j = 0; j < m_ceil; j++) {
                    x[batch*n_ceil*m_ceil + i*m_ceil + j] = 0.0f;
                }
            }
        }
    }
}

template <typename net_t>
static float compare_ref(std::vector<net_t> &x, std::vector<net_t> &ref,
                         const int m, const int n, const int batch_size,
                         const int m_ceil, const int n_ceil) {
    auto sum = 0.0f;
    for (auto batch = 0; batch < batch_size; batch++) {
        for (auto j = 0; j < m; j++) {
            for (auto i = 0; i < n; i++) {
                auto r = ref[batch*n*m + j*n + i];
                auto y = x[batch*n_ceil*m_ceil + j*n_ceil + i];

                sum += (r - y) * (r - y);
            }
        }
    }
    return sum / (m * n * batch_size);
}

template <typename net_t>
std::vector<Parameters> Tuner<net_t>::build_valid_params() {
    auto opts = std::vector<Configurations>();
    if (cfg_sgemm_exhaustive) {
        opts = {
            {"MWG", {16, 32, 64}},
            {"NWG", {16, 32, 64}},
            {"KWG", {16, 32}},
            {"MDIMC", {8, 16, 32}},
            {"NDIMC", {8, 16, 32}},
            {"MDIMA", {8, 16, 32}},
            {"NDIMB", {8, 16, 32}},
            {"KWI", {2, 8}},
            {"VWM", {1, 2, 4, 8}},
            {"VWN", {1, 2, 4, 8}},
            {"STRM", {0, 1}},
            {"STRN", {0, 1}},
            {"SA", {0, 1}},
            {"SB", {0, 1}},
        };
    } else {
        opts = {
            {"MWG", {16, 32, 64}},
            {"NWG", {16, 32, 64}},
            {"KWG", {16, 32}},
            {"MDIMC", {8, 16, 32}},
            {"NDIMC", {8, 16, 32}},
            {"MDIMA", {8, 16, 32}},
            {"NDIMB", {8, 16, 32}},
            {"KWI", {2, 8}},
            {"VWM", {2, 4}},
            {"VWN", {2, 4}},
            {"STRM", {0}},
            {"STRN", {0}},
            {"SA", {1}},
            {"SB", {1}},
        };
    }
    // Tensor Core options
    auto topts = std::vector<Configurations>();
    if (cfg_sgemm_exhaustive) {
        topts = {
            {"MWG", {32, 64, 128, 256}},
            {"NWG", {8, 16, 32, 64}},
            {"KWG", {16, 32, 64}},
            {"MDIMC", {8, 16, 32, 64}},
            {"NDIMC", {8, 16, 32, 64}},
            {"MDIMA", {8, 16, 32}},
            {"NDIMB", {8, 16, 32}},
            {"KWI", {2}},
            {"VWM", {2, 4, 8}},
            {"VWN", {2, 4, 8}},
            {"STRM", {0}},
            {"STRN", {0}},
            {"SA", {0, 1}},
            {"SB", {0, 1}},
        };
    } else {
        topts = {
            {"MWG", {32, 64, 128}},
            {"NWG", {8, 16, 32}},
            {"KWG", {16, 32}},
            {"MDIMC", {8, 16, 32}},
            {"NDIMC", {8, 16, 32}},
            {"MDIMA", {8, 16, 32}},
            {"NDIMB", {8, 16, 32}},
            {"KWI", {2}},
            {"VWM", {2}},
            {"VWN", {2}},
            {"STRM", {0}},
            {"STRN", {0}},
            {"SA", {0}},
            {"SB", {0}},
        };
    }

    auto valid_params = std::vector<Parameters>{};
    auto build_from = [this, &valid_params](std::vector<Configurations> & opts, int tce) {
        auto cfgs = 1;
        for (auto c = size_t{0}; c < opts.size(); c++) {
            cfgs *= opts[c].second.size();
        }
        for (auto i = 0; i < cfgs; i++) {
            Parameters param = get_parameters_by_int(opts, i);
            param["TCE"] = tce;
            if (valid_config_sgemm(param, cfg_sgemm_exhaustive)) {
                valid_params.push_back(param);
            }
        }
    };
    build_from(opts, 0);
    build_from(topts, 1);

    // Don't use thread RNG or determinism will depend on whether tuner ran.
    auto rng = Random{0};
    std::shuffle(begin(valid_params), end(valid_params), rng);

    if (cfg_sgemm_exhaustive) {
        // Likely too many valid params, cut out some of them
        valid_params.resize(valid_params.size() / 16);
    }

    return valid_params;
}

template <typename net_t>
std::string Tuner<net_t>::tune_sgemm(const int m, const int n, const int k,
                              const int batch_size, const int runs) {
    // This needs to be at minimum the maximum (MNK/WG) values above.
    auto m_max = std::max(64, m);
    auto n_max = std::max(64, n);
    auto k_max = std::max(32, k);

    auto at_size = batch_size
        * next_power_of_two(k_max) * next_power_of_two(m_max);
    auto b_size = batch_size
        * next_power_of_two(k_max) * next_power_of_two(n_max);
    auto c_size = batch_size
        * next_power_of_two(m_max) * next_power_of_two(n_max);

    auto total_flops = batch_size * 2.0 * m * n * k;

    auto at = std::vector<net_t>(at_size);
    auto b = std::vector<net_t>(b_size);
    auto c = std::vector<net_t>(c_size);
    auto c_ref = std::vector<net_t>(c_size);

    sgemm_generate_data(at, k, m, batch_size, k, m);
    sgemm_generate_data(b, n, k, batch_size, n, k);

    sgemmBatched_ref(at, b, c_ref, m, n, k, batch_size);

    auto aBuffer = cl::Buffer(
        m_context,
        CL_MEM_READ_WRITE, sizeof(net_t) * at_size, nullptr, nullptr);
    auto bBuffer = cl::Buffer(
        m_context,
        CL_MEM_READ_WRITE, sizeof(net_t) * b_size, nullptr, nullptr);
    auto cBuffer = cl::Buffer(
        m_context,
        CL_MEM_READ_WRITE, sizeof(net_t) * c_size, nullptr, nullptr);

    myprintf("\nStarted OpenCL SGEMM tuner.\n");

    auto valid_params = build_valid_params();

    myprintf("Will try %zu valid configurations.\n", valid_params.size());

    std::string best_params;
    auto best_time = unsigned{0};

    auto queue = cl::CommandQueue(m_context,
                                  m_device,
                                  CL_QUEUE_PROFILING_ENABLE);
    auto event = cl::Event();
    auto program = cl::Program(m_context, sourceCode_common + sourceCode_sgemm);

    auto m_ceil_prev = 0;
    auto n_ceil_prev = 0;
    auto k_ceil_prev = 0;
    auto param_counter = size_t{0};
    auto min_error = 100.0f;
    auto failed_compile = 0;
    auto failed_enqueue = 0;
    auto failed_error = 0;

    for (auto & p : valid_params) {
        param_counter++;

        auto defines = parameters_to_defines(p);

        try {
            auto args = m_opencl.m_cl_args + " " + defines;
            program.build(args.c_str());
        } catch (const cl::Error&) {
            // Failed to compile, get next parameter
            failed_compile++;
            continue;
        }

        auto sgemm_kernel = cl::Kernel(program, "XgemmBatched");

        auto m_ceil = int(ceilMultiple(ceilMultiple(m, p["MWG"]), p["VWM"]));
        auto n_ceil = int(ceilMultiple(ceilMultiple(n, p["NWG"]), p["VWN"]));
        auto k_ceil = int(ceilMultiple(ceilMultiple(k, p["KWG"]), p["VWM"]));

        if (m_ceil != m_ceil_prev
            || n_ceil != n_ceil_prev
            || k_ceil != k_ceil_prev) {
            m_ceil_prev = m_ceil;
            n_ceil_prev = n_ceil;
            k_ceil_prev = k_ceil;

            sgemm_generate_data(at, k, m, batch_size, k_ceil, m_ceil);
            sgemm_generate_data(b, n, k, batch_size, n_ceil, k_ceil);

            queue.enqueueWriteBuffer(aBuffer, CL_FALSE, 0,
                                     at_size * sizeof(net_t), at.data());
            queue.enqueueWriteBuffer(bBuffer, CL_FALSE, 0,
                                     b_size * sizeof(net_t), b.data());
            queue.finish();
        }

        sgemm_kernel.setArg(0, m_ceil);
        sgemm_kernel.setArg(1, n_ceil);
        sgemm_kernel.setArg(2, k_ceil);
        sgemm_kernel.setArg(3, aBuffer);
        sgemm_kernel.setArg(4, bBuffer);
        sgemm_kernel.setArg(5, cBuffer);

        cl::NDRange local_sgemm = {p["MDIMC"], p["NDIMC"], 1};

        cl::NDRange size_sgemm = {(m_ceil * p["MDIMC"]) / p["MWG"],
                                  (n_ceil * p["NDIMC"]) / p["NWG"],
                                  size_t(batch_size)};
        // Tensor Core implementation uses a different dimension.
        if (p["TCE"]) {
            local_sgemm = {32 * p["MDIMC"] / p["MDIMA"], p["NDIMC"] / p["NDIMB"], 1};
            size_sgemm = {32 * m_ceil / p["MDIMA"] * p["MDIMC"] / p["MWG"],
                          n_ceil / p["NDIMB"] * p["NDIMC"] / p["NWG"],
                          size_t(batch_size)};
        }

        auto sum = 0.0f;
        auto error = 0.0f;

        for (auto r = 0; r < runs; r++) {
            try {
                queue.enqueueNDRangeKernel(sgemm_kernel, cl::NullRange,
                                           size_sgemm, local_sgemm,
                                           nullptr, &event);
                queue.finish();
                event.wait();

                queue.enqueueReadBuffer(cBuffer, CL_FALSE, 0,
                                        c_size * sizeof(net_t), c.data());
                queue.finish();

                auto this_error = compare_ref(c, c_ref, n, m, batch_size,
                                              n_ceil, m_ceil);
                error = std::max(error, this_error);

                auto elapsed =
                    event.getProfilingInfo<CL_PROFILING_COMMAND_END>() -
                    event.getProfilingInfo<CL_PROFILING_COMMAND_START>();

                sum += elapsed;
            } catch (const cl::Error&) {
                // Failed to enqueue kernel. Set error to some big number.
                failed_enqueue++;
                error = std::numeric_limits<float>::max();
                // This failure will be counted to be failed due to error,
                // so preemptively subtract one from that count.
                failed_error--;
                break;
            }
        }

        min_error = std::min(min_error, error);

        if (error >= getTunerMaxError<net_t>()) {
            failed_error++;
        }

        if (error < getTunerMaxError<net_t>() && (best_time == 0 || sum < best_time)) {
            auto param_str = parameters_to_string(p);
            auto kernel_ms = 1e-6f * (sum / runs);
            // Timing is in nanoseconds (10^-9), Giga = 10^9, so this works out
            auto kernel_gflops = total_flops / (sum / runs);
            myprintf("(%u/%u) %s %.4f ms (%.1f GFLOPS)\n",
               param_counter, valid_params.size(), param_str.c_str(),
               kernel_ms, kernel_gflops);
            best_time = sum;
            best_params = defines;
        }
    }
    if (best_time == 0) {
        if (failed_compile > 0) {
            myprintf_error("Failed to compile: %d kernels.\n", failed_compile);
        }
        if (failed_enqueue > 0) {
            myprintf_error("Failed to enqueue: %d kernels\n", failed_enqueue);
        }
        if (failed_error > 0) {
            myprintf_error("Too high error: %d kernels\n", failed_error);
        }
        myprintf_error("Failed to find a working configuration.\nCheck your OpenCL drivers.\n");
        myprintf_error("Minimum error: %f. Error bound: %f\n", min_error, getTunerMaxError<net_t>());
        throw std::runtime_error("Tuner failed to find working configuration.");
    }
    return best_params;
}

template <typename net_t>
void Tuner<net_t>::store_sgemm_tuners(const int m, const int n, const int k,
                               const int batch_size, std::string tuners) {
    auto tuner_file = leelaz_file(TUNER_FILE_LOCAL);
    auto file_contents = std::vector<std::string>();
    {
        // Read the previous contents to string
        auto file = std::ifstream{tuner_file};
        if (file.good()) {
            auto line = std::string{};
            while (std::getline(file, line)) {
                file_contents.emplace_back(line);
            }
        }
    }
    auto file = std::ofstream{tuner_file};

    auto device_name = m_opencl.get_device_name();
    auto tuning_params = std::stringstream{};
    tuning_params << m << ";" << n << ";" << k << ";" << batch_size;

    auto tuning_line_prefix = std::to_string(TUNER_VERSION) + ";"
        + getTunerKernel<net_t>() + ";" + tuning_params.str() + ";";
    auto tuning_line = tuning_line_prefix + tuners + ";" + device_name;

    // Write back previous data as long as it's not the device and
    // tuning we just tuned
    for (const auto& line : file_contents) {
        if (line.find(tuning_line_prefix) == std::string::npos
            || line.find(device_name) == std::string::npos) {
            file << line << std::endl;
        }
    }

    // Write new tuning
    file << tuning_line << std::endl;

    if (file.fail()) {
        myprintf("Could not save the tuning result.\n");
        myprintf("Do I have write permissions on %s?\n",
            tuner_file.c_str());
    }
}

template <typename net_t>
std::string Tuner<net_t>::sgemm_tuners_from_line(std::string line,
                                          const int m, const int n, const int k,
                                          const int batch_size) {
    auto s = std::vector<std::string>{};
    auto ss = std::stringstream{line};
    auto item = std::string{};

    while (std::getline(ss, item, ';')) {
        s.emplace_back(item);
    }

    if (s.size() != 8) {
        return "";
    }

    if (s[0] != std::to_string(TUNER_VERSION)) {
        return "";
    }

    if (s[1] != getTunerKernel<net_t>()) {
        return "";
    }

    if (s[2] != std::to_string(m)) {
        return "";
    }

    if (s[3] != std::to_string(n)) {
        return "";
    }

    if (s[4] != std::to_string(k)) {
        return "";
    }

    if (s[5] != std::to_string(batch_size)) {
        return "";
    }

    if (s[7] != m_opencl.get_device_name()) {
        return "";
    }

    return s[6];
}

template <typename net_t>
std::string Tuner<net_t>::load_sgemm_tuners(const int m, const int n, const int k,
                                     const int batch_size) {
    auto tuner_file = leelaz_file(TUNER_FILE_LOCAL);
    auto file = std::ifstream{tuner_file};

    auto try_prior_tuning = file.good();

    // If we want full tuning, don't reuse previously tuned results
    // except if the tuning was created from this run from a different
    // GPU instance with the same name.  This prevents the tuner running
    // for multiple times if the system has multiple same GPUs.
    if (try_prior_tuning && cfg_sgemm_exhaustive) {
        auto dev = m_opencl.get_device_name();
        try_prior_tuning = std::any_of(
            begin(tuned_devices),
            end(tuned_devices),
            [&dev](const std::string & x) { return dev == x; }
        );
    }
    tuned_devices.emplace_back(m_opencl.get_device_name());

    if (try_prior_tuning) {
        auto line = std::string{};
        while (std::getline(file, line)) {
            auto tuners = sgemm_tuners_from_line(line, m, n, k, batch_size);
            if (tuners.size() != 0) {
                myprintf("Loaded existing SGEMM tuning.\n");
                return tuners;
            }
        }
    }
    auto tuners = tune_sgemm(m, n, k, batch_size);
    store_sgemm_tuners(m, n, k, batch_size, tuners);
    return tuners;
}

template <typename net_t>
void Tuner<net_t>::enable_tensorcore() {}
#ifdef USE_HALF
template <>
void Tuner<half_float::half>::enable_tensorcore() {
    m_use_tensorcore = true;
}
#endif

template class Tuner<float>;
#ifdef USE_HALF
template class Tuner<half_float::half>;
#endif

#endif


// Copyright (C) 2011-2012 by the BEM++ Authors
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include "bempp/common/config_ahmed.hpp"
#include "bempp/common/config_trilinos.hpp"

#include "aca_global_assembler.hpp"

#include "assembly_options.hpp"
#include "cluster_construction_helper.hpp"
#include "index_permutation.hpp"
#include "discrete_boundary_operator_composition.hpp"
#include "discrete_sparse_boundary_operator.hpp"

#include "../common/armadillo_fwd.hpp"
#include "../common/auto_timer.hpp"
#include "../common/boost_shared_array_fwd.hpp"
#include "../common/chunk_statistics.hpp"
#include "../fiber/explicit_instantiation.hpp"
#include "../fiber/local_assembler_for_operators.hpp"
#include "../fiber/serial_blas_region.hpp"
#include "../fiber/scalar_traits.hpp"
#include "../space/space.hpp"

#include <stdexcept>
#include <iostream>

#include <boost/type_traits/is_complex.hpp>

#include <tbb/atomic.h>
#include <tbb/parallel_for.h>
#include <tbb/task_scheduler_init.h>
#include <tbb/concurrent_queue.h>

#ifdef WITH_AHMED
#include "ahmed_aux.hpp"
#include "discrete_aca_boundary_operator.hpp"
#include "scattered_range.hpp"
#include "weak_form_aca_assembly_helper.hpp"
#endif

namespace Bempp
{

// Body of parallel loop
namespace
{

#ifdef WITH_AHMED
template <typename BasisFunctionType, typename ResultType>
class AcaWeakFormAssemblerLoopBody
{
    typedef typename Fiber::ScalarTraits<ResultType>::RealType CoordinateType;
    typedef AhmedDofWrapper<CoordinateType> AhmedDofType;
    typedef bemblcluster<AhmedDofType, AhmedDofType> AhmedBemBlcluster;
    typedef mblock<typename AhmedTypeTraits<ResultType>::Type> AhmedMblock;
public:
    typedef tbb::concurrent_queue<size_t> LeafClusterIndexQueue;

    AcaWeakFormAssemblerLoopBody(
            WeakFormAcaAssemblyHelper<BasisFunctionType, ResultType>& helper,
            AhmedLeafClusterArray& leafClusters,
            boost::shared_array<AhmedMblock*> blocks,
            const AcaOptions& options,
            tbb::atomic<size_t>& done,
            bool verbose,
            LeafClusterIndexQueue& leafClusterIndexQueue,
            bool symmetric,
            std::vector<ChunkStatistics>& stats) :
        m_helper(helper),
        m_leafClusters(leafClusters), m_blocks(blocks),
        m_options(options), m_done(done), m_verbose(verbose),
        m_leafClusterIndexQueue(leafClusterIndexQueue),
        m_symmetric(symmetric),
        m_stats(stats)
    {
    }

    template <typename Range>
    void operator() (const Range& r) const {
        const char* TEXT = "Approximating ... ";
        for (typename Range::const_iterator i = r.begin(); i != r.end(); ++i) {
            size_t leafClusterIndex = 0;
            if (!m_leafClusterIndexQueue.try_pop(leafClusterIndex)) {
                std::cerr << "AcaWeakFormAssemblerLoopBody::operator(): "
                             "Warning: try_pop failed; this shouldn't happen!"
                          << std::endl;
                continue;
            }
            m_stats[leafClusterIndex].valid = true;
            m_stats[leafClusterIndex].chunkStart = r.begin();
            m_stats[leafClusterIndex].chunkSize = r.size();
            m_stats[leafClusterIndex].startTime = tbb::tick_count::now();

            AhmedBemBlcluster* cluster =
                    dynamic_cast<AhmedBemBlcluster*>(m_leafClusters[leafClusterIndex]);
            if (m_symmetric)
                apprx_sym(m_helper, m_blocks[cluster->getidx()],
                          cluster, m_options.eps, m_options.maximumRank,
                          true /* complex_sym */);
            else
                apprx_unsym(m_helper, m_blocks[cluster->getidx()],
                            cluster, m_options.eps, m_options.maximumRank);
            m_stats[leafClusterIndex].endTime = tbb::tick_count::now();
            // TODO: recompress
            const int HASH_COUNT = 20;
            if (m_verbose)
                progressbar(std::cout, TEXT, (++m_done) - 1,
                            m_leafClusters.size(), HASH_COUNT, true);
        }

    }

private:
    WeakFormAcaAssemblyHelper<BasisFunctionType, ResultType>& m_helper;
    AhmedLeafClusterArray& m_leafClusters;
    boost::shared_array<AhmedMblock*> m_blocks;
    const AcaOptions& m_options;
    tbb::atomic<size_t>& m_done;
    bool m_verbose;
    LeafClusterIndexQueue& m_leafClusterIndexQueue;
    bool m_symmetric;
    std::vector<ChunkStatistics>& m_stats;
};

void reallyGetClusterIds(const cluster& clusterTree,
                         const std::vector<unsigned int>& p2oDofs,
                         std::vector<unsigned int>& clusterIds,
                         unsigned int& id)
{
    if (clusterTree.isleaf())
        for (unsigned int nDof = clusterTree.getnbeg(); nDof < clusterTree.getnend(); ++nDof)
            clusterIds[p2oDofs[nDof]] = id;
    else
        for (unsigned int nSon = 0; nSon < clusterTree.getns(); ++nSon)
            reallyGetClusterIds(*clusterTree.getson(nSon), p2oDofs, clusterIds, ++id);
}

void getClusterIds(const cluster& clusterTree,
                   const std::vector<unsigned int>& p2oDofs,
                   std::vector<unsigned int>& clusterIds)
{
    clusterIds.resize(p2oDofs.size());
    unsigned int id = 0;
    reallyGetClusterIds(clusterTree, p2oDofs, clusterIds, id);
}
#endif

} // namespace

template <typename BasisFunctionType, typename ResultType>
std::auto_ptr<DiscreteBoundaryOperator<ResultType> >
AcaGlobalAssembler<BasisFunctionType, ResultType>::assembleDetachedWeakForm(
        const Space<BasisFunctionType>& testSpace,
        const Space<BasisFunctionType>& trialSpace,
        const std::vector<LocalAssembler*>& localAssemblers,
        const std::vector<const DiscreteBndOp*>& sparseTermsToAdd,
        const std::vector<ResultType>& denseTermsMultipliers,
        const std::vector<ResultType>& sparseTermsMultipliers,
        const AssemblyOptions& options,
        int symmetry)
{
#ifdef WITH_AHMED
    typedef AhmedDofWrapper<CoordinateType> AhmedDofType;
    typedef ExtendedBemCluster<AhmedDofType> AhmedBemCluster;
    typedef bemblcluster<AhmedDofType, AhmedDofType> AhmedBemBlcluster;
    typedef DiscreteAcaBoundaryOperator<ResultType> DiscreteAcaLinOp;

    const AcaOptions& acaOptions = options.acaOptions();
    const bool indexWithGlobalDofs = acaOptions.globalAssemblyBeforeCompression;
    const bool verbosityAtLeastDefault =
            (options.verbosityLevel() >= VerbosityLevel::DEFAULT);
    const bool verbosityAtLeastHigh =
            (options.verbosityLevel() >= VerbosityLevel::HIGH);

    // Currently we don't support Hermitian ACA operators. This is because we
    // don't have the means to really test them -- we would need complex-valued
    // basis functions for that. (Assembly of such a matrix would be very easy
    // -- just change complex_sym from true to false in the call to apprx_sym()
    // in AcaWeakFormAssemblerLoopBody::operator() -- but operations on
    // symmetric/Hermitian matrices are not always trivial and we do need to be
    // able to test them properly.)
    bool symmetric = symmetry & SYMMETRIC;
    if (symmetry & HERMITIAN && !(symmetry & SYMMETRIC) &&
            verbosityAtLeastDefault)
        std::cout << "Warning: assembly of non-symmetric Hermitian H-matrices "
                     "is not supported yet. A general H-matrix will be assembled"
                  << std::endl;

#ifndef WITH_TRILINOS
    if (!indexWithGlobalDofs)
        throw std::runtime_error("AcaGlobalAssembler::assembleDetachedWeakForm(): "
                                 "ACA assembly with globalAssemblyBeforeCompression "
                                 "set to false requires BEM++ to be linked with "
                                 "Trilinos");
#endif // WITH_TRILINOS

    const size_t testDofCount = indexWithGlobalDofs ?
                testSpace.globalDofCount() : testSpace.flatLocalDofCount();
    const size_t trialDofCount = indexWithGlobalDofs ?
                trialSpace.globalDofCount() : trialSpace.flatLocalDofCount();

    if (symmetric && testDofCount != trialDofCount)
        throw std::invalid_argument("AcaGlobalAssembler::assembleDetachedWeakForm(): "
                                    "you cannot generate a symmetric weak form "
                                    "using test and trial spaces with different "
                                    "numbers of DOFs");

    // o2p: map of original indices to permuted indices
    // p2o: map of permuted indices to original indices
    typedef ClusterConstructionHelper<BasisFunctionType> CCH;
    shared_ptr<AhmedBemCluster> testClusterTree;
    shared_ptr<IndexPermutation> test_o2pPermutation, test_p2oPermutation;
    CCH::constructBemCluster(testSpace, indexWithGlobalDofs, acaOptions,
                             testClusterTree,
                             test_o2pPermutation, test_p2oPermutation);
    shared_ptr<AhmedBemCluster> trialClusterTree;
    shared_ptr<IndexPermutation> trial_o2pPermutation, trial_p2oPermutation;
    if (symmetric || &testSpace == &trialSpace) {
        trialClusterTree = testClusterTree;
        trial_o2pPermutation = test_o2pPermutation;
        trial_p2oPermutation = test_p2oPermutation;
    } else
        CCH::constructBemCluster(trialSpace, indexWithGlobalDofs, acaOptions,
                                 trialClusterTree,
                                 trial_o2pPermutation, trial_p2oPermutation);

//    // Export VTK plots showing the disctribution of leaf cluster ids
//    std::vector<unsigned int> testClusterIds;
//    getClusterIds(*testClusterTree, test_p2oPermutation->permutedIndices(), testClusterIds);
//    testSpace.dumpClusterIds("testClusterIds", testClusterIds,
//                             indexWithGlobalDofs ? GLOBAL_DOFS : FLAT_LOCAL_DOFS);
//    std::vector<unsigned int> trialClusterIds;
//    getClusterIds(*trialClusterTree, trial_p2oPermutation->permutedIndices(), trialClusterIds);
//    trialSpace.dumpClusterIds("trialClusterIds", trialClusterIds,
//                              indexWithGlobalDofs ? GLOBAL_DOFS : FLAT_LOCAL_DOFS);

    if (verbosityAtLeastHigh)
        std::cout << "Test cluster count: " << testClusterTree->getncl()
                  << "\nTrial cluster count: " << trialClusterTree->getncl()
                  << std::endl;

    unsigned int blockCount = 0;
    shared_ptr<AhmedBemBlcluster> bemBlclusterTree(
                CCH::constructBemBlockCluster(acaOptions, symmetric,
                                              *testClusterTree, *trialClusterTree,
                                              blockCount).release());

    if (verbosityAtLeastHigh)
        std::cout << "Mblock count: " << blockCount << std::endl;

    std::vector<unsigned int> p2oTestDofs =
        test_p2oPermutation->permutedIndices();
    std::vector<unsigned int> p2oTrialDofs =
        trial_p2oPermutation->permutedIndices();
    WeakFormAcaAssemblyHelper<BasisFunctionType, ResultType>
        helper(testSpace, trialSpace, p2oTestDofs, p2oTrialDofs,
               localAssemblers, sparseTermsToAdd,
               denseTermsMultipliers, sparseTermsMultipliers, options);

    typedef mblock<typename AhmedTypeTraits<ResultType>::Type> AhmedMblock;
    boost::shared_array<AhmedMblock*> blocks =
            allocateAhmedMblockArray<ResultType>(bemBlclusterTree.get());

    // matgen_sqntl(helper, AhmedBemBlclusterTree.get(), AhmedBemBlclusterTree.get(),
    //              acaOptions.recompress, acaOptions.eps,
    //              acaOptions.maximumRank, blocks.get());

    // matgen_omp(helper, blockCount, AhmedBemBlclusterTree.get(),
    //            acaOptions.eps, acaOptions.maximumRank, blocks.get());

    // // Dump mblocks
    // const int mblockCount = AhmedBemBlclusterTree->nleaves();
    // for (int i = 0; i < mblockCount; ++i)
    //     if (blocks[i]->isdns())
    //     {
    //         char  buffer[1024];
    //         sprintf(buffer, "mblock-dns-%d-%d.txt",
    //                 blocks[i]->getn1(), blocks[i]->getn2());
    //         arma::Col<ResultType> block((ResultType*)blocks[i]->getdata(),
    //                                     blocks[i]->nvals());
    //         arma::diskio::save_raw_ascii(block, buffer);
    //     }
    //     else
    //     {
    //         char buffer[1024];
    //         sprintf(buffer, "mblock-lwr-%d-%d.txt",
    //                 blocks[i]->getn1(), blocks[i]->getn2());
    //         arma::Col<ResultType> block((ResultType*)blocks[i]->getdata(),
    //                                     blocks[i]->nvals());
    //         arma::diskio::save_raw_ascii(block, buffer);
    //     }

    AhmedLeafClusterArray leafClusters(bemBlclusterTree.get());
    leafClusters.sortAccordingToClusterSize();
    const size_t leafClusterCount = leafClusters.size();

    const ParallelizationOptions& parallelOptions =
            options.parallelizationOptions();
    int maxThreadCount = 1;
    if (!parallelOptions.isOpenClEnabled())
    {
        if (parallelOptions.maxThreadCount() == ParallelizationOptions::AUTO)
            maxThreadCount = tbb::task_scheduler_init::automatic;
        else
            maxThreadCount = parallelOptions.maxThreadCount();
    }
    tbb::task_scheduler_init scheduler(maxThreadCount);
    tbb::atomic<size_t> done;
    done = 0;

    std::vector<ChunkStatistics> chunkStats(leafClusterCount);

    //    typedef AcaWeakFormAssemblerLoopBody<BasisFunctionType, ResultType> Body;
    //    // std::cout << "Loop start" << std::endl;
    //    tbb::tick_count loopStart = tbb::tick_count::now();
    // //    tbb::parallel_for(tbb::blocked_range<size_t>(0, leafClusterCount),
    // //                      Body(helper, leafClusters, blocks, acaOptions, done
    // //                           , chunkStats));
    //    tbb::parallel_for(ScatteredRange(0, leafClusterCount),
    //                      Body(helper, leafClusters, blocks, acaOptions, done
    //                           , chunkStats));
    //    tbb::tick_count loopEnd = tbb::tick_count::now();
    //    // std::cout << "Loop end" << std::endl;

    typedef AcaWeakFormAssemblerLoopBody<BasisFunctionType, ResultType> Body;
    typename Body::LeafClusterIndexQueue leafClusterIndexQueue;
    for (size_t i = 0; i < leafClusterCount; ++i)
        leafClusterIndexQueue.push(i);

    if (verbosityAtLeastDefault)
        std::cout << "About to start the ACA assembly loop" << std::endl;
    tbb::tick_count loopStart = tbb::tick_count::now();
    {
        Fiber::SerialBlasRegion region; // if possible, ensure that BLAS is single-threaded
        tbb::parallel_for(tbb::blocked_range<size_t>(0, leafClusterCount),
                          Body(helper, leafClusters, blocks, acaOptions, done,
                               verbosityAtLeastDefault,
                               leafClusterIndexQueue, symmetric, chunkStats));
    }
    tbb::tick_count loopEnd = tbb::tick_count::now();
    if (verbosityAtLeastDefault) {
        std::cout << "\n"; // the progress bar doesn't print the final \n
        std::cout << "ACA loop took " << (loopEnd - loopStart).seconds() << " s"
                  << std::endl;
    }

    // TODO: parallelise!
    if (acaOptions.recompress) {
        if (verbosityAtLeastDefault)
            std::cout << "About to start ACA agglomeration" << std::endl;
        agglH(bemBlclusterTree.get(), blocks.get(),
              acaOptions.eps, acaOptions.maximumRank);
        if (verbosityAtLeastDefault)
            std::cout << "Agglomeration finished" << std::endl;
    }

    // // Dump timing data of individual chunks
    //    std::cout << "\nChunks:\n";
    //    for (int i = 0; i < leafClusterCount; ++i)
    //        if (chunkStats[i].valid) {
    //            int blockIndex = leafClusters[i]->getidx();
    //            std::cout << chunkStats[i].chunkStart << "\t"
    //                      << chunkStats[i].chunkSize << "\t"
    //                      << (chunkStats[i].startTime - loopStart).seconds() << "\t"
    //                      << (chunkStats[i].endTime - loopStart).seconds() << "\t"
    //                      << (chunkStats[i].endTime - chunkStats[i].startTime).seconds() << "\t"
    //                      << blocks[blockIndex]->getn1() << "\t"
    //                      << blocks[blockIndex]->getn2() << "\t"
    //                      << blocks[blockIndex]->islwr() << "\t"
    //                      << (blocks[blockIndex]->islwr() ? blocks[blockIndex]->rank() : 0) << "\n";
    //        }

    {
        size_t origMemory = sizeof(ResultType) * testDofCount * trialDofCount;
        size_t ahmedMemory = sizeH(bemBlclusterTree.get(), blocks.get());
        int maximumRank = Hmax_rank(bemBlclusterTree.get(), blocks.get());
        if (verbosityAtLeastDefault)
            std::cout << "\nNeeded storage: "
                      << ahmedMemory / 1024. / 1024. << " MB.\n"
                      << "Without approximation: "
                      << origMemory / 1024. / 1024. << " MB.\n"
                      << "Compressed to "
                      << (100. * ahmedMemory) / origMemory << "%.\n"
                      << "Maximum rank: " << maximumRank << ".\n"
                      << std::endl;

        if (acaOptions.outputPostscript) {
            if (verbosityAtLeastDefault)
                std::cout << "Writing matrix partition ..." << std::flush;
            std::ofstream os(acaOptions.outputFname.c_str());
            if (symmetric) // seems valid also for Hermitian matrices
                psoutputHeH(os, bemBlclusterTree.get(), testDofCount, blocks.get());
            else
                psoutputGeH(os, bemBlclusterTree.get(), testDofCount, blocks.get());
            os.close();
            if (verbosityAtLeastDefault)
                std::cout << " done." << std::endl;
        }
    }

    int outSymmetry = NO_SYMMETRY;
    if (symmetric) {
        outSymmetry = SYMMETRIC;
        if (!boost::is_complex<ResultType>())
            outSymmetry |= HERMITIAN;
    }
    std::auto_ptr<DiscreteAcaLinOp> acaOp(
                new DiscreteAcaLinOp(testDofCount, trialDofCount,
                                     acaOptions.eps,
                                     acaOptions.maximumRank,
                                     outSymmetry,
                                     bemBlclusterTree, blocks,
                                     *trial_o2pPermutation,
                                     *test_o2pPermutation,
                                     parallelOptions));

    std::auto_ptr<DiscreteBndOp> result;
    if (indexWithGlobalDofs)
        result = acaOp;
    else {
#ifdef WITH_TRILINOS
        // without Trilinos, this code will never be reached -- an exception
        // will be thrown earlier in this function
        typedef DiscreteBoundaryOperatorComposition<ResultType> DiscreteBndOpComp;
        shared_ptr<DiscreteBndOp> acaOpShared(acaOp.release());
        shared_ptr<DiscreteBndOp> trialGlobalToLocal =
                constructOperatorMappingGlobalToFlatLocalDofs<
                BasisFunctionType, ResultType>(trialSpace);
        shared_ptr<DiscreteBndOp> testLocalToGlobal =
                constructOperatorMappingFlatLocalToGlobalDofs<
                BasisFunctionType, ResultType>(testSpace);
        shared_ptr<DiscreteBndOp> tmp(
                    new DiscreteBndOpComp(acaOpShared, trialGlobalToLocal));
        result.reset(new DiscreteBndOpComp(testLocalToGlobal, tmp));
#endif // WITH_TRILINOS
    }
    return result;

#else // without Ahmed
    throw std::runtime_error("AcaGlobalAssembler::assembleDetachedWeakForm(): "
                             "To enable assembly in ACA mode, recompile BEM++ "
                             "with the symbol WITH_AHMED defined.");
#endif // WITH_AHMED
}

template <typename BasisFunctionType, typename ResultType>
std::auto_ptr<DiscreteBoundaryOperator<ResultType> >
AcaGlobalAssembler<BasisFunctionType, ResultType>::assembleDetachedWeakForm(
        const Space<BasisFunctionType>& testSpace,
        const Space<BasisFunctionType>& trialSpace,
        LocalAssembler& localAssembler,
        const AssemblyOptions& options,
        int symmetry)
{
    std::vector<LocalAssembler*> localAssemblers(1, &localAssembler);
    std::vector<const DiscreteBndOp*> sparseTermsToAdd;
    std::vector<ResultType> denseTermsMultipliers(1, 1.0);
    std::vector<ResultType> sparseTermsMultipliers;

    return assembleDetachedWeakForm(testSpace, trialSpace, localAssemblers,
                            sparseTermsToAdd,
                            denseTermsMultipliers,
                            sparseTermsMultipliers,
                            options, symmetry);
}

FIBER_INSTANTIATE_CLASS_TEMPLATED_ON_BASIS_AND_RESULT(AcaGlobalAssembler);

} // namespace Bempp


/**

Copyright (c) 2016, Borella Jocelyn, Hanselmann Fabian, Heller Florian, Heizmann Heinrich, Kübler Marcel, Mehlhaus Jonas, Meißner Pascal, Qattan Mohamad, Reckling Reno, Stroh Daniel
All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.

3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>
#include <std_msgs/Float64MultiArray.h>
#include <std_msgs/Empty.h>
#include <std_msgs/UInt32.h>
#include <std_msgs/UInt32MultiArray.h>
#include <std_msgs/String.h>

#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <ISM/common_type/Point.hpp>
#include <ISM/common_type/Track.hpp>
#include <ISM/common_type/Tracks.hpp>
#include <ISM/common_type/Object.hpp>
#include "ISM/recognizer/VotingSpace.hpp"
#include "ISM/recorder/Recorder.hpp"
#include <ISM/utility/TableHelper.hpp>
#include <ISM/utility/MathHelper.hpp>

#include <asr_ism/recordGenConfig.h>

#include <asr_ism_visualizations/ObjectModelVisualizerRVIZ.hpp>
#include "asr_ism_visualizations/VizHelperRVIZ.hpp"

#include <boost/filesystem/path.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/algorithm/string.hpp>

//#include <eigen3/Eigen/Geometry.hpp>
#include <eigen3/unsupported/Eigen/Splines>
#include <Eigen/Geometry>

//#include "visStuff.hpp"

#include <boost/math/constants/constants.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/random.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/uniform_real.hpp>
#include <random>
#include <iostream>
#include <fstream>
#include <sstream>

#include <math.h>
#include <stdint.h>
#include <tuple>

using visualization_msgs::MarkerArray;
using namespace VIZ;

double frand(double min, double max) {
	double f = (double) rand() / RAND_MAX;
	return min + f * (max - min);
}

enum class RecGenPointType
{
	Fix = 0,
	MidSpline = 1,
	MidSplineAlt = 2,
	Undefined = 3
};

namespace H
{
    template<class C, class T>
    auto contains(const C& v, const T& x)
    -> decltype(end(v), true)
    {
	return end(v) != std::find(begin(v), end(v), x);
    }
}

RecGenPointType recGenPointTypeFromString(const std::string& s)
{
	if(s == "Fix")
	{
		return RecGenPointType::Fix;
	} else if(s == "MidSpline")
	{
		return RecGenPointType::MidSpline;
	} else if(s == "MidSplineAlt")
	{
		return RecGenPointType::MidSplineAlt;
	}

	return RecGenPointType::Undefined;
}

enum class OrientCalcType
{
	Fix = 0,
    Random = 1,
    Linear = 2,
    Undefined = 3
};

OrientCalcType orientCalcTypeFromString(const std::string& s)
{
	if(s == "Fix")
	{
		return OrientCalcType::Fix;
	} else if (s == "Random")
	{
		return OrientCalcType::Random;
	} else if (s == "Linear")
	{
		return OrientCalcType::Linear;
	}
	return OrientCalcType::Undefined;
}

class RecordGenPoint
{
public:
	RecordGenPoint(std::string name, int32_t id, RecGenPointType type, double posX, double posY,
			double posZ, double orientW, double orientX, double orientY, double orientZ,
			OrientCalcType orientCalcType, double posErr, double orientDerivation, unsigned int from,
			unsigned int to, double linearDegX, double linearDegY, double linearDegZ):
		mTimestep(0),
		mCurrentApplied(false),
		mName(name),
		mId(id),
		mType(type),
		mCurrentPosX(posX),
		mCurrentPosY(posY),
		mCurrentPosZ(posZ),
		mCurrentOrientW(orientW),
		mCurrentOrientX(orientX),
		mCurrentOrientY(orientY),
		mCurrentOrientZ(orientZ),
	    mInitOrientW(orientW),
	    mInitOrientX(orientX),
	    mInitOrientY(orientY),
	    mInitOrientZ(orientZ),
		mOrientCalcType(orientCalcType),
		mPosErr(posErr),
		mOrientDer(orientDerivation),
		mFrom(from),
		mTo(to),
		mLinearDegX(linearDegX),
		mLinearDegY(linearDegY),
		mLinearDegZ(linearDegZ),
		mUniform(-1,1),
		mNormal(0, mOrientDer)

	{
		if(mOrientCalcType == OrientCalcType::Undefined)
		{
			throw std::runtime_error("Type of OrientCalc is undefined");
		}
		if(mType == RecGenPointType::Undefined)
		{
			throw std::runtime_error("Type of RecGenPoint is undefined");
		}
        mTrack.reset(new ISM::Track(name, boost::lexical_cast<std::string>(id)));
		std::random_device rd;
		mRandomGen.seed(rd());
		mRandomGenOrient.seed(rd());
		mVariantGen = new boost::variate_generator<boost::mt19937, boost::uniform_real<>>
				(mRandomGen, mUniform);
		mVarGenOrient = new boost::variate_generator<boost::mt19937, boost::normal_distribution<>>
				(mRandomGenOrient, mNormal);
	}
	virtual ~RecordGenPoint()
	{
	}
	virtual bool isVisible() = 0;
	virtual void calcCurrObPos() = 0;
	virtual void calcCurrObOrient()
	{
		ISM::PointPtr currPos;
        if(mCurrentO && mCurrentO->pose)
		{
            currPos = mCurrentO->pose->point;
		}

		switch(mOrientCalcType)
		{
		case OrientCalcType::Fix:
		{
            mCurrentO.reset(new ISM::Object(mName,
                            new ISM::Pose(currPos,
                                    new ISM::Quaternion(mCurrentOrientW, mCurrentOrientX,
                                    		mCurrentOrientY, mCurrentOrientZ)),
                                    		boost::lexical_cast<std::string>(mId)));

			break;
		}
		case OrientCalcType::Random:
		{
			mCurrentOrientW = (*mVariantGen)();
			mCurrentOrientX = (*mVariantGen)();
			mCurrentOrientY = (*mVariantGen)();
			mCurrentOrientZ = (*mVariantGen)();

            mCurrentO.reset(new ISM::Object(mName,
                            new ISM::Pose(mCurrentO->pose->point,
                                    new ISM::Quaternion(mCurrentOrientW, mCurrentOrientX,
                                    		mCurrentOrientY, mCurrentOrientZ)),
                                    		boost::lexical_cast<std::string>(mId)));
			break;
		}
		case OrientCalcType::Linear:
		{
			if(mTimestep > 0 && mFrom < mTo)
			{
			    const double pi = boost::math::constants::pi<double>();
				double totSpan = mTo - mFrom;
				double currentLinEulX = mLinearDegX * ((double) mTimestep) / totSpan
						*pi / 180.0;
				double currentLinEulY = mLinearDegY * ((double) mTimestep) / totSpan
						*pi / 180.0;
				double currentLinEulZ = mLinearDegZ * ((double) mTimestep) / totSpan
						*pi / 180.0;

			    Eigen::Quaterniond linQ = Eigen::AngleAxisd(currentLinEulX, Eigen::Vector3d::UnitX())
			     * Eigen::AngleAxisd(currentLinEulY, Eigen::Vector3d::UnitY())
			     * Eigen::AngleAxisd(currentLinEulZ, Eigen::Vector3d::UnitZ());
			    Eigen::Quaterniond q(mInitOrientW, mInitOrientX, mInitOrientY, mInitOrientZ);
			    Eigen::Quaterniond totalQ = q * linQ;

			    mCurrentOrientW = totalQ.w();
			    mCurrentOrientX = totalQ.x();
			    mCurrentOrientY = totalQ.y();
			    mCurrentOrientZ = totalQ.z();
			}
			mCurrentO.reset(new ISM::Object(mName,
                    new ISM::Pose(mCurrentO->pose->point,
						new ISM::Quaternion(mCurrentOrientW, mCurrentOrientX,
								mCurrentOrientY, mCurrentOrientZ)),
								boost::lexical_cast<std::string>(mId)));
			break;
		}
		case OrientCalcType::Undefined:
		{
			throw std::runtime_error("OrientCalcType is Undefined");
			break;
		}
		}
	}
	virtual void forceInvisible(bool) = 0;
	virtual void apply()
	{
        if(!mCurrentO || !mCurrentO->pose->point ||
                !mCurrentO->pose->quat)
			throw "RecordGenPoint: Current Object must always be initialized";
		if(!mCurrentApplied)
		{
            auto point = mCurrentO->pose->point;
            auto quat = mCurrentO->pose->quat;
            point->eigen.x() = point->eigen.x() + (*mVariantGen)() * mPosErr;
            point->eigen.y() = point->eigen.y() + (*mVariantGen)() * mPosErr;
            point->eigen.z() = point->eigen.z() + (*mVariantGen)() * mPosErr;
            if(point->eigen.x() < -1.0 || point->eigen.x() > 1.0
                    || point->eigen.y() < -1.0 || point->eigen.y() > 1.0
                    ||point->eigen.z() < -1.0 || point->eigen.z() > 1.0 )
			{
				std::cerr << "Warning Point does not lie in range: " << point << std::endl
						<< "type: " << mName << " id: " << mId << std::endl
						<< "at timestep: " << mTimestep << std::endl;
				std::cin.ignore();
			}
			const double pi = boost::math::constants::pi<double>();

			Eigen::Matrix3d m;
			m = Eigen::AngleAxisd(pi * (*mVarGenOrient)(), Eigen::Vector3d::UnitX())
			 * Eigen::AngleAxisd(pi * (*mVarGenOrient)(), Eigen::Vector3d::UnitY())
			 * Eigen::AngleAxisd(pi * (*mVarGenOrient)(), Eigen::Vector3d::UnitZ());

			Eigen::Quaterniond errorQ(m);
			Eigen::Quaterniond q(quat->eigen.w(), quat->eigen.x(), quat->eigen.y(), quat->eigen.z());
	    Eigen::Quaterniond totalQ = q * errorQ;

	    quat->eigen.w() = totalQ.w();
            quat->eigen.x() = totalQ.x();
            quat->eigen.y() = totalQ.y();
            quat->eigen.z() = totalQ.z();
			/*
            quat->setW(quat->w);
            quat->setX(quat->x);
            quat->setY(quat->y);
            quat->setZ(quat->z);
			*/

            if(isVisible())
            {
		//TODO not in current ws
            	//mCurrentO->mTimestep = mTimestep;
                mTrack->objects.push_back(mCurrentO);
            }
            else
            {
                mTrack->objects.push_back(ISM::ObjectPtr());
                std::cout << mName << " is not visible" << std::endl;
            }

            mCurrentApplied = true;
		}
	}
	virtual void applyAndGotoNextTimestep()
	{
		apply();
		++mTimestep;
		mCurrentApplied = false;
		calcCurrObPos();
		calcCurrObOrient();
	}

	unsigned int mTimestep;
	bool mCurrentApplied;
	std::string mName;
	int32_t mId;
	ISM::TrackPtr mTrack;
    ISM::ObjectPtr mCurrentO;
    RecGenPointType mType;
	double mCurrentPosX;
	double mCurrentPosY;
	double mCurrentPosZ;
	double mCurrentOrientW;
	double mCurrentOrientX;
	double mCurrentOrientY;
	double mCurrentOrientZ;
	double mInitOrientW;
	double mInitOrientX;
	double mInitOrientY;
	double mInitOrientZ;
	OrientCalcType mOrientCalcType;
	double mPosErr;
	double mOrientDer;
	unsigned int mFrom;
	unsigned int mTo;
	double mLinearDegX;
	double mLinearDegY;
	double mLinearDegZ;
	boost::mt19937 mRandomGen;
	boost::mt19937 mRandomGenOrient;
	boost::uniform_real<> mUniform;
	boost::normal_distribution<> mNormal;
    boost::variate_generator<boost::mt19937, boost::uniform_real<>>* mVariantGen;
    boost::variate_generator<boost::mt19937, boost::normal_distribution<>>* mVarGenOrient;
    //ISM::PosePtr mCurrentPose;
};

class MidSplinePoint : public RecordGenPoint
{
private:
	void setCol(Eigen::MatrixXd &m, int col, Eigen::Vector3d &v, double time)
	{
		m(0, col) = time;
		m(1, col) = v.x();
		m(2, col) = v.y();
		m(3, col) = v.z();
	}
public:
	void calcSpline()
	{
		Eigen::Vector3d s = Eigen::Vector3d(mXs, mYs , mZs);
		Eigen::Vector3d e = Eigen::Vector3d(mXe, mYe , mZe);
		Eigen::Vector3d sToe = e - s;
        Eigen::Vector3d m = s + sToe/2;
        Eigen::Vector3d temp = sToe;
        temp.normalize();
        temp = temp * 2;
        Eigen::Vector3d orthSToe = temp.unitOrthogonal();
        orthSToe.normalize();
        orthSToe *= (sToe.norm()/2) * tan(mDegree * (boost::math::constants::pi<double>() / 180.0));
        Eigen::Vector3d p3 = m + orthSToe;

        //dimension, num points
        int numPoints = 3;
        double timeToNextPoint = mTimeSpan / (double) numPoints;
        Eigen::MatrixXd points(4, numPoints);
        setCol(points, 0, s, 0);
        setCol(points, 1, p3, timeToNextPoint);
        setCol(points, 2, e, mTimeSpan);
        mSpline = Eigen::SplineFitting<Eigen::Spline<double, 4>>::Interpolate(points, numPoints - 1);
	}
	MidSplinePoint(std::string name, int32_t id, double xs, double ys, double zs,
			double xe, double ye, double ze, double degree, double timeSpan,
			double orientW, double orientX, double orientY, double orientZ,
			OrientCalcType orientCalcType, double posErr, double orientErr,
			unsigned int from, unsigned int to,
			 double linearDegX, double linearDegY, double linearDegZ) :
				RecordGenPoint(name, id, RecGenPointType::MidSpline, xs, ys, zs, orientW,
						orientX, orientY, orientZ, orientCalcType, posErr,
						orientErr, from, to,
						linearDegX, linearDegY, linearDegZ),
						mIsInvisible(false),
                mXs(xs), mYs(ys), mZs(zs), mXe(xe), mYe(ye), mZe(ze), mDegree(degree), mTimeSpan(timeSpan)
	{
		calcSpline();
		calcCurrObPos();
		calcCurrObOrient();
	}

	void setNewParams(double xs, double ys, double zs,
			double xe, double ye, double ze, double degree, double timeSpan)
	{
		mXs = xs;
		mYs = ys;
		mZs = zs;

		mXe = xe;
		mYe = ye;
		mZe = ze;

		mDegree = degree;
		mTimeSpan = timeSpan;
		calcSpline();
		calcCurrObPos();
	}

	virtual ~MidSplinePoint()
	{
	}

	virtual bool isVisible()
	{
		return !mIsInvisible;
	}

	virtual void forceInvisible(bool b)
	{
		mIsInvisible = b;
	}

	virtual void calcCurrObPos()
	{
		ISM::QuaternionPtr currOrient;
        if(mCurrentO && mCurrentO->pose)
		{
            currOrient = mCurrentO->pose->quat;
		}

        Eigen::Vector4d values;
		if(mTimestep > mTimeSpan)
          values = mSpline(1);
		else
          values = mSpline(mTimestep/mTimeSpan);

		mCurrentPosX = values(1);
		mCurrentPosY = values(2);
		mCurrentPosZ = values(3);
        mCurrentO.reset(new ISM::Object(mName,
                        new ISM::Pose(new ISM::Point(mCurrentPosX, mCurrentPosY, mCurrentPosZ),
                                currOrient), boost::lexical_cast<std::string>(mId)));

	}
	bool mIsInvisible;
	double mXs;
	double mYs;
	double mZs;
	double mXe;
	double mYe;
	double mZe;
	Eigen::Spline<double, 4> mSpline;
	double mDegree;
	double mTimeSpan;
};

class MidSplinePointAlt : public MidSplinePoint
{
public:
	MidSplinePointAlt(std::string name, int32_t id, double xs, double ys, double zs,
			double xe, double ye, double ze, double degree, double timeSpan,
			double orientW, double orientX, double orientY, double orientZ,
			OrientCalcType orientCalcType, double posErr, double orientErr,
			unsigned int from, unsigned int to , double linearDegX, double linearDegY, double linearDegZ) :
          MidSplinePoint(name, id, xs, ys, zs,
                          xe, ye, ze, degree, timeSpan, orientW, orientX, orientY, orientZ,
                          orientCalcType, posErr, orientErr, from, to, linearDegX, linearDegY, linearDegZ)
    {

    }
	virtual void calcCurrObPos()
	{
		ISM::QuaternionPtr currOrient;
        if(mCurrentO && mCurrentO->pose)
		{
            currOrient = mCurrentO->pose->quat;
		}

        Eigen::Vector4d values;
        int timesTimeSpan = mTimestep / mTimeSpan;
		if((timesTimeSpan % 2) == 0)
          values = mSpline((mTimestep - (timesTimeSpan * mTimeSpan)) / mTimeSpan);
		else
          values = mSpline(1 - (mTimestep - (timesTimeSpan * mTimeSpan)) / mTimeSpan);

		mCurrentPosX = values(1);
		mCurrentPosY = values(2);
		mCurrentPosZ = values(3);
        mCurrentO.reset(new ISM::Object(mName,
                        new ISM::Pose(new ISM::Point(mCurrentPosX, mCurrentPosY, mCurrentPosZ),
                                currOrient), boost::lexical_cast<std::string>(mId)));

	}
};
class FixRecordGenPoint : public RecordGenPoint
{
public:
	FixRecordGenPoint(std::string name, int32_t id, double x, double y, double z,
			double orientW, double orientX, double orientY, double orientZ,
			OrientCalcType orientCalcType, double posErr, double orientErr,
			unsigned int from, unsigned int to, double linearDegX, double linearDegY, double linearDegZ):
		RecordGenPoint(name, id, RecGenPointType::Fix, x, y, z, orientW, orientX, orientY, orientZ,
				orientCalcType, posErr, orientErr, from, to, linearDegX, linearDegY, linearDegZ),
		mIsInvisible(false)
	{
		calcCurrObPos();
		calcCurrObOrient();
#if 0
        if(mTimestep > 0)
        {
        	unsigned int desTs = mTimestep;
        	mTimestep = 0;
        	forceInvisible(true);
            for(unsigned int i = 0; i < desTs; ++i)
            {
            	applyAndGotoNextTimestep();
            }
            forceInvisible(false);
        }
#endif
	}

	virtual ~FixRecordGenPoint()
	{
	}

	void setNewParams(double x, double y, double z)
	{
		mCurrentPosX = x;
		mCurrentPosY = y;
		mCurrentPosZ = z;
		calcCurrObPos();
	}

	virtual bool isVisible()
	{
		return !mIsInvisible;
	}

	virtual void forceInvisible(bool b)
	{
		mIsInvisible = b;
	}

	virtual void calcCurrObPos()
	{
		ISM::QuaternionPtr currOrient;
        if(mCurrentO && mCurrentO->pose)
		{
            currOrient = mCurrentO->pose->quat;
		}
        mCurrentO.reset(new ISM::Object(mName,
                        new ISM::Pose(new ISM::Point(mCurrentPosX, mCurrentPosY, mCurrentPosZ),
                                        currOrient), boost::lexical_cast<std::string>(mId)));

	}

	bool mIsInvisible;
};

using boost::filesystem::path;
typedef boost::shared_ptr<RecordGenPoint> RecordGenPointPtr;
typedef std::tuple<uint32_t,uint32_t,RecordGenPointPtr> FromToPointTuple;
namespace pt = boost::property_tree;

struct cmpFromToPointTuple {
    bool operator()(const FromToPointTuple& a, const FromToPointTuple& b) const {
    	if(std::get<2>(a)->mName == std::get<2>(b)->mName &&
    			std::get<2>(a)->mId == std::get<2>(b)->mId)
    	{
    		return std::get<1>(a) < std::get<0>(b);
    	} else
    	{
    		return (uintptr_t)(std::get<2>(a).get()) < (uintptr_t)(std::get<2>(b).get());
    	}
    }
};

class RecordGenerator
{
public:
	void parseXml(path xmlFile)
	{
		pt::ptree tree;

		pt::read_xml(xmlFile.string(), tree);

		for(auto& fstLvlNode : tree)
		{
			if(fstLvlNode.first == "object")
			{
				std::string name = fstLvlNode.second.get<std::string>("<xmlattr>.name");
				int32_t id = fstLvlNode.second.get<int32_t>("<xmlattr>.id");
                ISM::TrackPtr sharedTrack = ISM::TrackPtr(new ISM::Track(name,
                		boost::lexical_cast<std::string>(id)));
                mTracks.push_back(sharedTrack);
				for(auto& pointNode : fstLvlNode.second)
				{
                    if(pointNode.first == "point")
                    {
                        auto& pointAttrs = pointNode.second;

                        RecGenPointType pointType = recGenPointTypeFromString
                                        (pointAttrs.get<std::string>("<xmlattr>.type", ""));
                        int from = pointAttrs.get<uint32_t>("from");
                        int to = pointAttrs.get<uint32_t>("to");
                        if(from > to)
                        {
                        	std::stringstream ss;
                        	ss << "At: " << name << "," << id << " from must be less than to";
                        	throw std::runtime_error(ss.str());
                        }
                        double posX = pointAttrs.get<double>("posX", 0.0);
                        double posY = pointAttrs.get<double>("posY", 0.0);
                        double posZ = pointAttrs.get<double>("posZ", 0.0);
                        double orientW = pointAttrs.get<double>("orientW", 0.0);
                        double orientX = pointAttrs.get<double>("orientX", 0.0);
                        double orientY = pointAttrs.get<double>("orientY", 0.0);
                        double orientZ = pointAttrs.get<double>("orientZ", 0.0);
                        double xs = pointAttrs.get<double>("xs", 0.0);
                        double ys = pointAttrs.get<double>("ys", 0.0);
                        double zs = pointAttrs.get<double>("zs", 0.0);
                        double xe = pointAttrs.get<double>("xe", 0.0);
                        double ye = pointAttrs.get<double>("ye", 0.0);
                        double ze = pointAttrs.get<double>("ze", 0.0);
                        double degree = pointAttrs.get<double>("degree", 0.0);
                        double timeSpan = pointAttrs.get<double>("timeSpan", 0.0);
                        double posErr = pointAttrs.get<double>("posErr", 0.0);
                        double orientErr = pointAttrs.get<double>("orientErr", 0.0);
                        double linearDegX = pointAttrs.get<double>("linearDegX", 0.0);
                        double linearDegY = pointAttrs.get<double>("linearDegY", 0.0);
                        double linearDegZ = pointAttrs.get<double>("linearDegZ", 0.0);
                        OrientCalcType oCT = orientCalcTypeFromString(pointAttrs.get<std::string>("orientCalcType", ""));
                        std::vector<unsigned int> timestampVisible;
                        for(auto& pointAttr : pointAttrs)
                        {
                                if(pointAttr.first == "visible")
                                {
                                        std::string rangeString = pointAttr.second.get_value("");
                                        std::vector<std::string> range;
                                        boost::split(range, rangeString, boost::is_any_of("-"));
                                        if(range.size() == 2)
                                        {
                                                for(unsigned int i = boost::lexical_cast<unsigned int>(range[0]);
                                                                i <= boost::lexical_cast<unsigned int>(range[1]); ++i)
                                                {
                                                        timestampVisible.push_back(i);
                                                }
                                        } else
                                        {
                                                timestampVisible.push_back(boost::lexical_cast<unsigned int>(range[0]));
                                        }
                                }
                        }
                        RecordGenPointPtr p;
                        switch(pointType)
                        {
                        case RecGenPointType::Fix:
                        {
                                p = RecordGenPointPtr(new FixRecordGenPoint(name, id, posX, posY, posZ, orientW,
                                                orientX, orientY, orientZ, oCT, posErr, orientErr, from, to
                                                ,linearDegX, linearDegY, linearDegZ));
                                break;
                        }
                        case RecGenPointType::MidSpline:
                        {
                                p = RecordGenPointPtr(new MidSplinePoint(name, id, xs, ys, zs, xe, ye, ze, degree,
                                                timeSpan, orientW, orientX, orientY, orientZ, oCT, posErr, orientErr, from, to,
                                                 linearDegX, linearDegY, linearDegZ));
                                break;
                        }
                        case RecGenPointType::MidSplineAlt:
                        {
                                p = RecordGenPointPtr(new MidSplinePointAlt(name, id, xs, ys, zs, xe, ye, ze, degree,
                                        timeSpan, orientW, orientX, orientY, orientZ, oCT, posErr, orientErr, from, to,
                                         linearDegX, linearDegY, linearDegZ));
                                break;
                        }

                        default:
                                throw std::runtime_error("Error in parsing xml: RecGenPointType not valid");
                                break;
                        }
                        mTimestampVisibleXmlPoints[p] = timestampVisible;
                        p->mTrack = sharedTrack;
                        auto itToSucceed = mPoints.insert(std::make_tuple(from,to,p));
                        if(!itToSucceed.second)
                        {
							std::stringstream ss;
                        	ss << "At: " << name << "," << id << " point intervals overlapping";
                        	throw std::runtime_error(ss.str());
                        }
                    }
				}
			}

		}
	}
    void dynamicReconfCallback(asr_ism::recordGenConfig &config, uint32_t level)
    {
    	mShowFrom = config.from;
    	mShowTo = config.to;
    }

	RecordGenerator(std::string dbFile,
			std::string patternName = "pattern0", bool doPublish = false):
		mNextId(0),
		mTimestep(0),
		mDbFile(dbFile),
		mFilterViaVgOut(false),
		mPatternName(patternName),
		mShowCurrent(false),
		mDoPublish(doPublish),
		mShowFrom(0),
		mShowTo(0)
	{
            dynamic_reconfigure::Server<asr_ism::recordGenConfig>::CallbackType f = boost::bind(&RecordGenerator::dynamicReconfCallback, this, _1, _2);
	    mDynReconfServer.setCallback(f);
		rec.reset(new ISM::Recorder(mDbFile));
		//TODO drop tables?
        //rec->dropTables();
	}

	void filterViaVgOut(boost::filesystem::path vgOutFile)
	{
		mFilterViaVgOut = true;
		std::ifstream is(vgOutFile.string());
		std::string line;
		std::string type;
		std::string pattern;
		bool foundRep = true;
		while(std::getline(is, line))
		{
			if(line.find("Ref is:") != std::string::npos)
			{
				line.erase(line.begin(), line.begin() + line.find_first_of(":") + 1);
				mPatternToRef[pattern] = line;
			}
			if(line.find("Id for pattern:") != std::string::npos)
			{
				line.erase(line.begin(), line.begin() + line.find_first_of(":") + 1);
				pattern = line;
			} else if(line.find("Id for type:") != std::string::npos)
			{
				line.erase(line.begin(), line.begin() + line.find_first_of(":") + 1);
				type = line;
			} else if(line.find("These trackIds are in the same Orient Grid:") != std::string::npos)
			{
				foundRep = false;
			} else if(!foundRep)
			{
				uint32_t rep = 0;
				try
				{
                    rep = boost::lexical_cast<uint32_t>(line);
				} catch(boost::bad_lexical_cast& e)
                {
					continue;
                }
				mPatternTypeToVoxelReps[std::make_pair(pattern,type)].insert(rep);
				foundRep = true;
			}
		}
		for(auto& patternTypeToVoxelReps : mPatternTypeToVoxelReps)
		{
			std::string pattern = patternTypeToVoxelReps.first.first;
			std::string type = patternTypeToVoxelReps.first.second;
			for(uint32_t rep : patternTypeToVoxelReps.second)
			{
			    std::cout << "Rep for p: " << pattern << " t: " << type << " is: " << rep << std::endl;
			}
		}
	}

	void applyAndGotoNextTimestep()
	{
        for(auto& fromToPoint : mPoints)
        {
		    if(std::get<0>(fromToPoint) <= mTimestep &&
			    std::get<1>(fromToPoint) >= mTimestep)
		    {
			    auto& p = std::get<2>(fromToPoint);
			    std::vector<unsigned int>& visTs = mTimestampVisibleXmlPoints[p];

			    if(std::find(visTs.begin(), visTs.end(), mTimestep) != visTs.end())
			    {
				    p->forceInvisible(false);
			    } else
			    {
				    p->forceInvisible(true);
			    }
			    p->applyAndGotoNextTimestep();
		    }
        }
	    mTimestep++;
	}

	void apply()
	{
        for(auto& fromToPoint : mPoints)
        {
		    if(std::get<0>(fromToPoint) <= mTimestep &&
			    std::get<1>(fromToPoint) >= mTimestep)
		    {
			    auto& p = std::get<2>(fromToPoint);
			    std::vector<unsigned int>& visTs = mTimestampVisibleXmlPoints[p];

			    if(std::find(visTs.begin(), visTs.end(), mTimestep) != visTs.end())
			    {
				    p->forceInvisible(false);
			    } else
			    {
				    p->forceInvisible(true);
			    }
			    p->apply();
		    }
        }
	}

	void applyAndGotoNextTimestep(const uint32_t n)
	{
		for(unsigned int i = 0; i < n; ++i)
		{
			applyAndGotoNextTimestep();
		}
	}

	std::vector<ISM::ObjectPtr> getCurrentVisibleObjects()
    {
		std::vector<ISM::ObjectPtr> result;
		for(auto& fromToPoint : mPoints)
		{
		    if(std::get<0>(fromToPoint) <= mTimestep &&
			    std::get<1>(fromToPoint) >= mTimestep)
		    {
			    auto& p = std::get<2>(fromToPoint);
			    if(p->isVisible())
			    {
				    result.push_back(p->mCurrentO);
			    }
		    }
		}
		return result;
    }

	std::vector<ISM::ObjectPtr> getCurrentInvisibleObjects()
    {
		std::vector<ISM::ObjectPtr> result;
		for(auto& fromToPoint : mPoints)
		{
		    if(std::get<0>(fromToPoint) <= mTimestep &&
			    std::get<1>(fromToPoint) >= mTimestep)
		    {
			    auto& p = std::get<2>(fromToPoint);
			    if(!p->isVisible())
			    {
				    result.push_back(p->mCurrentO);
			    }
		    }
		}
		return result;
    }

	//void showFromTo(std_msgs::UInt32MultiArray::ConstPtr& msg)
	void showFromTo(const std_msgs::Float64MultiArray::ConstPtr& msg)
	{
	    mShowFrom = *msg->data.begin();
	    mShowTo = *(msg->data.begin() + 1);
	    mShowCurrent = false;
	}

    std::vector<ISM::ObjectPtr> getObjects(unsigned int from, unsigned int to)
    {
	std::vector<ISM::ObjectPtr> result;
    	static unsigned int oldTo = to;
    	if(!mDoPublish)
    	{
            if(to == oldTo)
            {
                    return result;
            } else
            {
                    oldTo = to;
            }
            ROS_INFO_STREAM("from is " << from << " << to: " <<to );
    	}
    	if(mTimestep < to)
    	{
    		//TODO
    		applyAndGotoNextTimestep(to - mTimestep);
    	}

    	if(mTimestep == to)
    	{
		    apply();
    	}
		if(from <= to)
		{
			for(auto& t : mTracks)
			{
				bool isRef = false;
                if(mFilterViaVgOut)
                {
                	std::vector<std::string> isRefInPatterns;
                	for(auto& patternToRef : mPatternToRef)
                	{
                		if(patternToRef.second == t->type)
                		{
                			isRefInPatterns.push_back(patternToRef.first);
                		}
                	}
                	if(isRefInPatterns.size() > 0)
                	{
                		isRef = true;
                		for(auto& patternTypeToReps : mPatternTypeToVoxelReps)
                		{
                			if(patternTypeToReps.first.second != t->type
                					&& H::contains(isRefInPatterns, patternTypeToReps.first.first))
                			{
                				for(uint32_t repTs : patternTypeToReps.second)
                				{
								    if(from <= repTs && repTs <= to && !H::contains(result,
								    		t->objects[repTs]))
								    {
								    	result.push_back(t->objects[repTs]);
								    }
                				}
                			}
                		}
                	}
                }
                if((!isRef && mFilterViaVgOut) || !mFilterViaVgOut)
                {
				    if(t->objects.size() > to)
				    {
					    {
							//TODO why did i do this?
						    result.insert(result.end(), t->objects.begin() + from, t->objects.begin() + to + 1);
					    }
				    } else if(t->objects.size() > from)
				    {
					    result.insert(result.end(), t->objects.begin() + from, t->objects.end());
				    }
                }
			}
		}
		/*
		ROS_INFO_STREAM("-------");
		for(auto& o : result)
		{
			if(o)
			{
				ROS_INFO_STREAM(o);
			}
		}
		ROS_INFO_STREAM("-------");
		*/
		return result;
    }

	std::vector<ISM::ObjectPtr> getOldObjects()
    {
		if(mTimestep == 0)
		{
			return std::vector<ISM::ObjectPtr>();
		}
		return getObjects(0, mTimestep -1);
    }

	void writeToDb()
	{
		uint32_t maxTimestep = std::get<1>(*mPoints.rbegin());
		if(maxTimestep > mTimestep)
		{
			applyAndGotoNextTimestep(maxTimestep - mTimestep);
			apply();
		}
        ROS_INFO_STREAM("Writing to database");
		apply();
        ISM::TracksPtr tracks(new ISM::Tracks(mTracks));
        auto objectSets = tracks->toObjectSetVector();
        for (auto objectSet : objectSets) {
            rec->insert(objectSet, mPatternName);
        }
        ROS_INFO_STREAM("Finished");
	}

	void writeToDb(const std_msgs::Empty)
	{
		writeToDb();
	}

private:
    int32_t mNextId;
    std::vector<int32_t> mIdXmlPoints;
    std::map<RecordGenPointPtr, std::vector<unsigned int> > mTimestampVisibleXmlPoints;
    dynamic_reconfigure::Server<asr_ism::recordGenConfig> mDynReconfServer;

public:
	unsigned int mTimestep;
	std::string mDbFile;
	bool mFilterViaVgOut;
	std::map<std::pair<std::string, std::string>, std::set<uint32_t>> mPatternTypeToVoxelReps;
	std::map<std::string, std::string> mPatternToRef;
	std::string mPatternName;

	std::set<std::tuple<uint32_t,uint32_t,RecordGenPointPtr>, cmpFromToPointTuple> mPoints;
	std::vector<ISM::TrackPtr> mTracks;
	ISM::RecorderPtr rec;
	bool mShowCurrent;
	bool mDoPublish;
	uint32_t mShowFrom;
	uint32_t mShowTo;
};

std_msgs::ColorRGBA getColorForTypeId(std::map<std::pair<std::string,std::string>, std_msgs::ColorRGBA >&
    typeIdToColor, std::vector<std_msgs::ColorRGBA>& allColors, uint32_t& availColorIndex,
    std::string& type, std::string& id)
{
	std_msgs::ColorRGBA result;
	if(typeIdToColor.find(std::make_pair(type,id)) != typeIdToColor.end())
	{
		result = typeIdToColor[std::make_pair(type,id)];
	} else
	{
		if(allColors.size() < availColorIndex + 1)
		{
			std::uniform_real_distribution<float> urd;
			std::default_random_engine re;
			result = VIZ::VizHelperRVIZ::createColorRGBA(urd(re), urd(re), urd(re), 1);
			allColors.push_back(result);
		} else
		{
			result = allColors[availColorIndex];
		}
		typeIdToColor[std::make_pair(type, id)] = result;
		++availColorIndex;
	}
	return result;
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "recordGen");
	ros::NodeHandle n("~");
	ros::Rate r(1);
	std::string baseFrame = "/camera_left_frame";
	std::string visualizationTopic = "/visualization_marker";
	std::string dbFile;//("/media/share/data/own/gen1.sqlite");
    double bin_size = 0.03;
	double maxAngleDeviation = 10;
	bool useXml = false;
	bool useVgOut = false;
	boost::filesystem::path xmlFile;
	boost::filesystem::path vgOutFile;
    bool detailedBinNS;
	bool doPublish;

    if (!n.getParam("dbfilename", dbFile)) {
    	throw std::runtime_error("dbfilename not set");
    }
    ROS_INFO_STREAM("dbfilename: " << dbFile);

    std::string temp;
    if (n.getParam("xml", temp)) {
    	useXml = true;
    	xmlFile = temp;
    }
    xmlFile = temp;
    ROS_INFO_STREAM("xml: " << temp);

    if (n.getParam("vgOut", temp)) {
    	vgOutFile = temp;
    	if(boost::filesystem::is_regular_file(vgOutFile))
    	{
		    useVgOut = true;
    	} else
    	{
    		useVgOut = false;
    	}
    }
    vgOutFile = temp;
    if(useVgOut)
    {
	    ROS_INFO_STREAM("vgOut: " << vgOutFile);
    }

    if (!n.getParam("detailedBinNS", detailedBinNS)) {
        detailedBinNS = false;
    }
    ROS_INFO_STREAM("detailedBinNS: " << detailedBinNS);

    if (!n.getParam("doPublish", doPublish)) {
    	doPublish = false;
    }
    ROS_INFO_STREAM("doPublish: " << doPublish);

    if (!n.getParam("bin_size", bin_size)) {
        bin_size = 0.03;
    }
    ROS_INFO_STREAM("bin_size: " << bin_size);

    RecordGenerator rg(dbFile, "pattern0", doPublish);
    if(useXml)
    {
    	rg.parseXml(xmlFile);
    }
    if(useVgOut)
    {
    	rg.filterViaVgOut(vgOutFile);
    }

	ros::Publisher visPub = n.advertise<visualization_msgs::MarkerArray>(visualizationTopic, 100);

    ISM::VotingSpace vs = ISM::VotingSpace(bin_size, maxAngleDeviation);
    //std_msgs::ColorRGBA  regularBinColor = VizHelperRVIZ::createColorRGBA(0.0, 1.0, 0.0, 1.0);
    //std_msgs::ColorRGBA  regularObjectColor = VizHelperRVIZ::createColorRGBA(0.0, 1.0, 0.0, 1.0);
    //const double sphereRadius = 0.005;
    //const double markerLifetime = ros::Duration().toSec();

    /*
    ros::Subscriber s0 = n.subscribe<std_msgs::Float64MultiArray>("addPoint", 1000, &RecordGenerator::addPoint, &rg);
    ros::Subscriber s1 = n.subscribe<std_msgs::UInt32>("applyAndGotoNextTimestep2", 1000, &RecordGenerator::applyAndGotoNextTimestep, &rg);
    ros::Subscriber s3 = n.subscribe<std_msgs::Float64MultiArray>("changeParameter", 1000, &RecordGenerator::changeParameter, &rg);
    ros::Subscriber s4 = n.subscribe<std_msgs::String>("toogleInvis", 1000, &RecordGenerator::toogleInvisible, &rg);
    */
    ros::Subscriber s1 = n.subscribe<std_msgs::Float64MultiArray>("showFromTo", 1000, &RecordGenerator::showFromTo, &rg);
    //ros::Subscriber s1 = n.subscribe<std_msgs::UInt32MultiArray>("showFromTo", 1000, &RecordGenerator::showFromTo, &rg);
    ros::Subscriber s2 = n.subscribe<std_msgs::Empty>("writeToDb", 1000, &RecordGenerator::writeToDb, &rg);

    std::vector<std_msgs::ColorRGBA> allColors;
    allColors.push_back(VIZ::VizHelperRVIZ::createColorRGBA(0.0,0.0,0.0,1.0));
    allColors.push_back(VIZ::VizHelperRVIZ::createColorRGBA(1.0,0.0,0.0,1.0));
    allColors.push_back(VIZ::VizHelperRVIZ::createColorRGBA(0.0,1.0,0.0,1.0));
    allColors.push_back(VIZ::VizHelperRVIZ::createColorRGBA(0.0,0.0,1.0,1.0));
    allColors.push_back(VIZ::VizHelperRVIZ::createColorRGBA(0.5,0.2,0.9,1.0));
    allColors.push_back(VIZ::VizHelperRVIZ::createColorRGBA(0.3,0.8,0.8,1.0));
    allColors.push_back(VIZ::VizHelperRVIZ::createColorRGBA(0.3,0.5,0.5,1.0));
    allColors.push_back(VIZ::VizHelperRVIZ::createColorRGBA(0.5,0.3,0.1,1.0));

    std::map<std::pair<std::string,std::string>, std_msgs::ColorRGBA > typeIdToColor;
    //uint32_t availColorIndex = 0;

    /*
    boost::function< geometry_msgs::Point (int,int,int) > pointF = boost::bind(genCuboidPoint, _1, _2, _3, x, y, z, xwitdh, ywidth, zwidth);
std_msgs::ColorRGBA getColor(std::map<std::pair<std::string,std::string>, std_msgs::ColorRGBA >&
    typeIdToColor, std::vector<std_msgs::ColorRGBA>& allColors, uint32_t& availColorIndex,
    const std::string& type, const std::string& id)
    */

 /*   boost::function < std_msgs::ColorRGBA (const std::string,const std::string) > getColor =
                boost::bind(getColorForTypeId, boost::ref(typeIdToColor), boost::ref(allColors),
                                boost::ref(availColorIndex), _1, _2);
*/
    while (ros::ok()) {

        ///deleted for visualization refectoring. Bins now drawn at ism_voting_visualizer
    /*    std::map<std::tuple<int,int,int>,bool> binDrawn;

    	MarkerArray oBs;
    	MarkerArray bins;
        ros::spinOnce();
        //TODO
        if(rg.mShowCurrent)
        {
			for(ISM::ObjectPtr& o : rg.getCurrentVisibleObjects())
			{
                ISM::PointPtr point = o->pose->point;
				std::stringstream ss;
                ss << o->getType() << " With X:" << point->eigen.x() << "Y:" << point->eigen.y() << "Z:" << point->eigen.z();
				oBs.markers.push_back(VIZ::VizHelperRVIZ::createSphereMarker(point, baseFrame, ss.str(),
						boost::lexical_cast<int32_t>(o->getId()), sphereRadius, regularObjectColor, markerLifetime));
                //TODO: look for better solution than hard-code binSize; Problem introduced because of deletion of calcBinNum in VotingSpace
                int x = vs.discretizeToBins(point->eigen.x(), 0.1);
                int y = vs.discretizeToBins(point->eigen.y(), 0.1);
                int z = vs.discretizeToBins(point->eigen.z(), 0.1);
			    std::tuple<int,int,int> t = std::make_tuple(x, y, z);
			    if(binDrawn[t] == false)
			    {
				    std::stringstream ss;
				    ss << "\nObject" << o->getId() << "Is in Bin: "<< "X:" << x << " Y:" << y << " Z:" << z;
				    bins.markers.push_back(VIZ::VizHelperRVIZ::getBinMarker(x, y, z, bin_size, ss.str(), 0, baseFrame,
						    regularBinColor));
				    binDrawn[t] = true;
			    }
			}
			for(ISM::ObjectPtr& o : rg.getCurrentInvisibleObjects())
			{
                ISM::PointPtr point = o->pose->point;
				std::stringstream ss;
                ss << "Invis: " <<  o->getType() << " With X:" << point->eigen.x() << "Y:" << point->eigen.y() << "Z:" << point->eigen.z();
				visualization_msgs::Marker s = VizHelperRVIZ::createSphereMarker(point, baseFrame, ss.str(),
						boost::lexical_cast<int32_t>(o->getId()), sphereRadius, regularObjectColor, markerLifetime);
				s.color.g = 0.0f;
				s.color.b = 1.0f;
				oBs.markers.push_back(s);
                //TODO: look for better solution than hard-code binSize; Problem introduced because of deletion of calcBinNum in VotingSpace
                int x = vs.discretizeToBins(point->eigen.x(), 0.1);
                int y = vs.discretizeToBins(point->eigen.y(), 0.1);
                int z = vs.discretizeToBins(point->eigen.z(), 0.1);
			    std::tuple<int,int,int> t = std::make_tuple(x, y, z);
			    if(binDrawn[t] == false)
			    {
				    std::stringstream ss;
				    ss << "\nObject" << o->getId() << "Is in Bin: "<< "X:" << x << " Y:" << y << " Z:" << z;
				    bins.markers.push_back(VizHelperRVIZ::getBinMarker(x,y,z,bin_size,ss.str(), 0, baseFrame,
						    regularBinColor));
				    binDrawn[t] = true;
			    }
			}
			int id = 0;
			for(ISM::ObjectPtr& o : rg.getOldObjects())
			{
				if(!o)
					continue;
                ISM::PointPtr point = o->pose->point;
				std::stringstream ss;
                //ss << "Old " << o->getType() << " With X:" << point->eigen.x() << "Y:" << point->eigen.y() << "Z:" << point->eigen.z();
				ss << "Old " << o->getType();
				//visualization_msgs::Marker m = objectToSphere(o, baseFrame, ss.str(), boost::lexical_cast<int32_t>(o->getId()));
				visualization_msgs::Marker m = VizHelperRVIZ::createSphereMarker(point, baseFrame, ss.str(),
						boost::lexical_cast<int32_t>(id++), sphereRadius, regularObjectColor, markerLifetime);
				m.color.r = 1.0f;
				oBs.markers.push_back(m);
                //TODO: look for better solution than hard-code binSize; Problem introduced because of deletion of calcBinNum in VotingSpace
                int x = vs.discretizeToBins(point->eigen.x(), 0.1);
                int y = vs.discretizeToBins(point->eigen.y(), 0.1);
                int z = vs.discretizeToBins(point->eigen.z(), 0.1);
			    std::tuple<int,int,int> t = std::make_tuple(x, y, z);
			    if(binDrawn[t] == false)
			    {
				    std::stringstream ss;
				    ss << "\nObject" << o->getId() << "Is in Bin: "<< "X:" << x << " Y:" << y << " Z:" << z;
				    bins.markers.push_back(VizHelperRVIZ::getBinMarker(x,y,z,bin_size,ss.str(),0,baseFrame,
						    regularBinColor));
				    binDrawn[t] = true;
			    }
			}
        } else
        {
			int objectMarkerId = 0;
			int binMarkerId = 0;

			for(ISM::ObjectPtr& o : rg.getObjects(rg.mShowFrom, rg.mShowTo))
			{
				if(!rg.mDoPublish)
				{
					break;
				}
				if(!o)
				{
					continue;
				}
                ISM::PointPtr point = o->pose->point;
				std::stringstream ss;
                //ss << o->getType() << " With X:" << point->eigen.x() << "Y:" << point->eigen.y() << "Z:" << point->eigen.z();
				ss << o->getType() << " " << o->getId();
				auto sphereWithOrient = VIZ::VizHelperRVIZ::createSphereMarkerWithOrientation(
                        o->pose, baseFrame, ss.str(), objectMarkerId + 1, sphereRadius,
						getColor(o->getType(), o->getId()), markerLifetime, objectMarkerId + 2);
				objectMarkerId += 4;
				oBs.markers.insert(oBs.markers.end(), sphereWithOrient.markers.begin(),
						sphereWithOrient.markers.end());
                //TODO: look for better solution than hard-code binSize; Problem introduced because of deletion of calcBinNum in VotingSpace
                int x = vs.discretizeToBins(point->eigen.x(), 0.1);
                int y = vs.discretizeToBins(point->eigen.y(), 0.1);
                int z = vs.discretizeToBins(point->eigen.z(), 0.1);
			    std::tuple<int,int,int> t = std::make_tuple(x, y, z);
			    if(binDrawn[t] == false)
			    {
				    std::stringstream ss;
				    //ss << "\nObject" << o->getId() << "Is in Bin: "<< "X:" << x << " Y:" << y << " Z:" << z;
				    if(detailedBinNS)
				    {
                        ss << "Bin: "<< "X:" << x << " Y:" << y << " Z:" << z;
				    } else
				    {
                        ss << "Bins";
				    }
				    bins.markers.push_back(VIZ::VizHelperRVIZ::getBinMarker(x, y, z, bucketSize, ss.str(),
				    		binMarkerId++, baseFrame, regularBinColor));
				    binDrawn[t] = true;
			    }
			}

        }
        visPub.publish(oBs);
        visPub.publish(bins);*/

        r.sleep();
    }

}


/**************************************************************************\
|
|    Copyright (C) 2009 Marc Stevens
|
|    This program is free software: you can redistribute it and/or modify
|    it under the terms of the GNU General Public License as published by
|    the Free Software Foundation, either version 3 of the License, or
|    (at your option) any later version.
|
|    This program is distributed in the hope that it will be useful,
|    but WITHOUT ANY WARRANTY; without even the implied warranty of
|    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
|    GNU General Public License for more details.
|
|    You should have received a copy of the GNU General Public License
|    along with this program.  If not, see <http://www.gnu.org/licenses/>.
|
\**************************************************************************/

#include <stdexcept>
#include <iostream>
#include <fstream>
#include <vector>
#include <string>

#include <boost/program_options.hpp>

#include "main.hpp"

#include <hashclash/md5detail.hpp>
#include <hashclash/timer.hpp>

using namespace hashclash;
using namespace std;
namespace po = boost::program_options;
namespace fs = boost::filesystem;

std::string workdir;

int main(int argc, char** argv)
{
	int result = 0;
	timer runtime(true);

	cout <<
		"Birthday search for MD5 chosen-prefix collisions\n"
		"Copyright (C) 2009 Marc Stevens\n"
		"http://homepages.cwi.nl/~stevens/\n"
		<< endl;

	try {
		birthday_parameters parameters;

		// Define program options
		po::options_description
			desc("Allowed options"),
			all("Allowed options");

		desc.add_options()
			("help,h", "Show options.")
			("mod,m"
				, po::value<unsigned>(&parameters.modn)->default_value(1)
				, "Do only 1/m of all work.")

			("index,i"
				, po::value<unsigned>(&parameters.modi)->default_value(0)
				, "Do i-th part of all work (i=0,...,m-1).")

			("workdir,w"
				, po::value<string>(&workdir)->default_value("./data")
				, "Set working directory.")

			("inputfile1"
				, po::value<string>(&parameters.inputfile1)
				, "Use specified inputfile as first message.")

			("inputfile2"
				, po::value<string>(&parameters.inputfile2)
				, "Use specified inputfile as second message.")

			("outputfile1"
				, po::value<string>(&parameters.outputfile1)->default_value("file1.bin")
				, "Use specified outputfile for first message.")

			("outputfile2"
				, po::value<string>(&parameters.outputfile2)->default_value("file2.bin")
				, "Use specified outputfile for second message.")

			("distribution"
				, po::bool_switch(&parameters.distribution)
				, "Show workdistribution.\nDepends on hybridbits and pathtyperange.")

			("hybridbits"
				, po::value<unsigned>(&parameters.hybridbits)->default_value(0)
				, "Set 0 for 64-bit, 32 for 96-bit birthdaying.")

			("pathtyperange"
				, po::value<unsigned>(&parameters.pathtyperange)->default_value(2)
				, "Increases potential # diffs eliminated per n.c.")

			("maxblocks"
				, po::value<unsigned>(&parameters.maxblocks)->default_value(9)
				, "Upperbound on the amount of near-collisions.")

			("logtraillength"
				, po::value<int>(&parameters.logpathlength)->default_value(-1)
				, "Specify avg. trail length in bits.")

			("maxmemory"
				, po::value<unsigned>(&parameters.maxmemory)->default_value(100)
				, "Max. memory in MB used for storing trails.")

			("memhardlimit"
				, po::bool_switch(&parameters.memhardlimit)
				, "Hard limit max. memory instead of average.")
			("threads"
				, po::value<unsigned>(&parameters.threads)->default_value(0)
				, "Number of computing threads to start.")
			;

#ifdef HAVE_CUDA
		desc.add_options()
			("cuda_dev_query", "Query CUDA devices")
			("cuda_enable", po::bool_switch(&parameters.cuda_enabled), "Enable CUDA")
			;
#else
		all.add_options()
			("cuda_dev_query", "Query CUDA devices")
			("cuda_enable", po::bool_switch(&parameters.cuda_enabled), "Enable CUDA")
			;
#endif

		all.add(desc);

		// Parse program options
		po::variables_map vm;
		po::store(po::parse_command_line(argc, argv, all), vm);
		{
			std::ifstream ifs("md5birthdaysearch.cfg");
			if (ifs) po::store(po::parse_config_file(ifs, all), vm);
		}
		po::notify(vm);

		// Process program options
		if (vm.count("help")) {
			cout << desc << endl;
			return 0;
		}
		if (parameters.distribution) {
			determine_nrblocks_distribution(parameters);
			return 0;
		}

		if (parameters.modi >= parameters.modn) {
			cerr << "Error: i must be strictly less than m!" << endl;
			return 1;
		}
		if (parameters.modn != 1)
			cout << "System " << parameters.modi << " of " << parameters.modn << endl;
		if (parameters.inputfile1.size() == 0)
		{
			cerr << "Error: inputfile1 must be given!" << endl;
			return 2;
		}
		if (parameters.inputfile2.size() == 0)
		{
			cerr << "Error: inputfile2 must be given!" << endl;
			return 2;
		}
		if (parameters.logpathlength < 0 && parameters.maxmemory == 0)
		{
			cerr << "Cannot determine logtraillength when maxmemory is unspecified!" << endl;
			return 3;
		}
		if (parameters.modi == 0)
		{
			if (parameters.outputfile1.size() == 0)
			{
				cerr << "Error: outputfile1 must be given!" << endl;
				return 2;
			}
			if (parameters.outputfile2.size() == 0)
			{
				cerr << "Error: outputfile2 must be given!" << endl;
				return 2;
			}
		}
#ifdef HAVE_CUDA
		if (vm.count("cuda_dev_query")) {
			cuda_device_query();
			return 0;
		}
#endif // CUDA
		result = dostep(parameters);
	} catch (exception& e) {
		cout << "Runtime: " << runtime.time() << endl;
		cerr << "Caught exception!!:" << endl << e.what() << endl;
		throw;
	} catch (...) {
		cout << "Runtime: " << runtime.time() << endl;
		cerr << "Unknown exception caught!!" << endl;
		throw;
	}
	cout << "Runtime: " << runtime.time() << endl;
	return result;
}


#include <boost/test/unit_test.hpp>
#include "EnumAsFlagsMacro.h"

enum class TestFlags : uint64_t {
    none = 0,
    flag0 = 1 << 0,
    flag1 = 1 << 1,
    flag2 = 1 << 2,
    flag3 = 1 << 3,
    flag4 = 1 << 4,
    flag5 = 1 << 5,
    flag6 = 1 << 6,
    flag7 = 1 << 7,
    flag8 = 1 << 8,
    flag9 = 1 << 9,
    flag10 = 1 << 10
};
MAKE_ENUM_SUPPORT_FLAG_OPERATORS(TestFlags);
MAKE_ENUM_SUPPORT_OSTREAM_OPERATOR(TestFlags);

BOOST_AUTO_TEST_CASE(EnumAsFlagsMacroTestCase)
{
    BOOST_REQUIRE_EQUAL(sizeof(std::underlying_type<TestFlags>::type), 8);

    TestFlags f = TestFlags::none;
    BOOST_REQUIRE_EQUAL(f, TestFlags::none);
    BOOST_REQUIRE_NE(f, TestFlags::flag0);
    BOOST_REQUIRE_EQUAL(f | TestFlags::flag0, TestFlags::flag0);
    f |= TestFlags::flag10;
    BOOST_REQUIRE_NE(f, TestFlags::none);
    BOOST_REQUIRE_EQUAL(f, TestFlags::flag10);
    BOOST_REQUIRE_EQUAL(f | TestFlags::flag0, TestFlags::flag10 | TestFlags::flag0);
    f |= TestFlags::flag0;
    f |= TestFlags::flag1;
    f |= TestFlags::flag2;
    BOOST_REQUIRE_EQUAL(f, TestFlags::flag0 | TestFlags::flag1 | TestFlags::flag2 | TestFlags::flag10);
    f &= TestFlags::flag1 | TestFlags::flag2 | TestFlags::flag3;
    BOOST_REQUIRE_EQUAL(f, TestFlags::flag1 | TestFlags::flag2);
    f = f | TestFlags::flag0;
    f = f | TestFlags::flag10;
    BOOST_REQUIRE_EQUAL(f, TestFlags::flag0 | TestFlags::flag1 | TestFlags::flag2 | TestFlags::flag10);
    f &= ~TestFlags::flag10;
    BOOST_REQUIRE_EQUAL(f, TestFlags::flag0 | TestFlags::flag1 | TestFlags::flag2);
    f ^= TestFlags::flag10;
    BOOST_REQUIRE_EQUAL(f, TestFlags::flag0 | TestFlags::flag1 | TestFlags::flag2 | TestFlags::flag10);
    f ^= TestFlags::flag10;
    BOOST_REQUIRE_EQUAL(f, TestFlags::flag0 | TestFlags::flag1 | TestFlags::flag2);
    BOOST_REQUIRE_EQUAL(TestFlags::flag0 & TestFlags::flag0, TestFlags::flag0);
    BOOST_REQUIRE_EQUAL(TestFlags::flag0 & TestFlags::flag1, TestFlags::none);
    BOOST_REQUIRE_EQUAL(TestFlags::flag0 ^ TestFlags::flag0, TestFlags::none);
    BOOST_REQUIRE_EQUAL(TestFlags::flag0 ^ TestFlags::flag0 ^ TestFlags::flag0, TestFlags::flag0);
    BOOST_REQUIRE_EQUAL(TestFlags::flag0 ^ TestFlags::flag1, TestFlags::flag0 | TestFlags::flag1);
}