// Copyright (c) 2014 The Bitcoin Core developers
// Copyright (c) 2014-2015 The Dash developers
// Copyright (c) 2015-2017 The PIVX developers
// Copyright (c) 2015-2017 The ALQO developers
// Copyright (C) 2019 The ICC developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "primitives/transaction.h"
#include "main.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(main_tests)

CAmount nMoneySupplyPoWEnd = 5000 * COIN;

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    CAmount nSum = 0;
    for (int nHeight = 0; nHeight < 1; nHeight += 1) {
        /* premine in block 1 */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 200000 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 1; nHeight < 501; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 0 * COIN);
        nSum += nSubsidy;
    }
    for (int nHeight = 501; nHeight < 20001; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 10 * COIN);
        nSum += nSubsidy;
    }
    for (int nHeight = 20001; nHeight < 40001; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 15 * COIN);
        nSum += nSubsidy;
    }
    for (int nHeight = 40001; nHeight < 262801; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 20 * COIN);
        nSum += nSubsidy;
    }
    for (int nHeight = 262801; nHeight < 525601; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 10 * COIN);
        nSum += nSubsidy;
    }
    for (int nHeight = 525601; nHeight < 788401; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 8 * COIN);
        nSum += nSubsidy;
    }
    for (int nHeight = 788401; nHeight < 1051201; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 6 * COIN);
        nSum += nSubsidy;
    }
    for (int nHeight = 1051201; nHeight < 1314001; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 4 * COIN);
        nSum += nSubsidy;
    }
	BOOST_CHECK(nSum > 0 && nSum <= nMoneySupplyPoWEnd);
}

BOOST_AUTO_TEST_SUITE_END()


#include <fstream>
#include <iostream>
#include <algorithm>
#include <thread>
#include <numeric>

#include "succinct/mapper.hpp"

#include "configuration.hpp"
#include "util.hpp"
#include "verify_collection.hpp"
#include "index_build_utils.hpp"

#include <boost/iostreams/filtering_streambuf.hpp>
#include <boost/iostreams/copy.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <boost/algorithm/string.hpp>

#include "clustered_index_types.hpp"
#include "clustered_binary_freq_collection.hpp"
#include "partitioned_sequence.hpp"
#include "reference_selector.hpp"

using ds2i::logger;

void create_clustered_collection(ds2i::clustered_binary_freq_collection& input,
                                 ds2i::global_parameters const& params,
                                 const char* cluster_filename,
                                 const char* output_filename,
                                 bool check,
                                 uint32_t MAX_REF_SIZE,
                                 uint32_t divisor)
{
    using namespace ds2i;
    uint64_t universe = input.num_docs();
    logger() << "Processing " << universe << " documents" << std::endl;
    double tick = get_time_usecs();
    double user_tick = get_user_time_usecs();
    clustered_opt_index::builder builder(universe, params);
    progress_logger plog;

    std::ifstream file(cluster_filename, std::ios_base::in | std::ios_base::binary);
    boost::iostreams::filtering_streambuf<boost::iostreams::input> inbuf;
    inbuf.push(boost::iostreams::gzip_decompressor());
    inbuf.push(file);
    std::istream instream(&inbuf);

    std::string line;
    uint32_t c = 1;
    std::vector<sequence_t> partitions;
    std::vector<pos_t> docs_positions;
    std::vector<pos_t> freqs_positions;
    while (std::getline(instream, line))
    {
        std::vector<std::string> data;
        boost::split(data, line, boost::is_any_of(" "));
        uint32_t cluster_size = std::atoi(data.front().data());

        std::cout << "cluster_size: " << cluster_size << std::endl;

        docs_positions.reserve(cluster_size);
        freqs_positions.reserve(cluster_size);

        std::for_each(++data.begin(), data.end(),
            [&docs_positions, &freqs_positions](const std::string& s)
            {
                pos_t p = std::stoull(s.data());
                docs_positions.push_back(p);
                freqs_positions.push_back(p - 2);
            });

        input.add_positions(docs_positions, freqs_positions);
        input.set_positions();

        partitions.clear();
        partitions.reserve(cluster_size);

        for (auto const& plist : input)
        {
            auto const& docs = plist.docs;
            partitions.push_back(
                std::move(
                    ds2i::partitioned_sequence<>::compute_partition(docs.begin(),
                                                                    universe,
                                                                    docs.size(),
                                                                    params)
                        )
                );
        }

        reference_selector rs(input, partitions, params, cluster_size, MAX_REF_SIZE);
        sequence_t reference = rs.select_reference(divisor);

        std::cout << "reference size: " << reference.size() << std::endl;

        builder.add_reference(reference.size(), reference.begin());
        uint32_t i = 0;
        for (auto const& plist : input)
        {
            uint64_t freqs_sum =
                std::accumulate(plist.freqs.begin(),
                                plist.freqs.end(),
                                uint64_t(0));
            builder.add_posting_list(plist.docs,
                                     plist.freqs,
                                     freqs_sum,
                                     partitions[i++]);
            plog.done_sequence(plist.docs.size());
        }
        logger() << "cluster-" << c++ << " encoded" << std::endl;

        docs_positions.clear();
        freqs_positions.clear();
    }

    file.close();
    plog.log();
    clustered_opt_index coll;
    builder.build(coll);
    double elapsed_secs = (get_time_usecs() - tick) / 1000000;
    double user_elapsed_secs = (get_user_time_usecs() - user_tick) / 1000000;
    logger() << "clustered_opt collection built in "
             << elapsed_secs << " seconds" << std::endl;

    stats_line()
        ("type", "clustered_opt_greedy")
        ("worker_threads", configuration::get().worker_threads)
        ("construction_time", elapsed_secs)
        ("construction_user_time", user_elapsed_secs)
        ;

    dump_stats(coll, "clustered_opt", plog.postings);

    if (output_filename) {
        succinct::mapper::freeze(coll, output_filename);
        if (check) {
            verify_clustered_collection(input, output_filename);
        }
    }
}

int main(int argc, const char** argv)
{
    using namespace ds2i;

    if (argc < 5) {
        std::cerr << "Usage: " << argv[0]
                  << " <collection basename> <cluster filename> <MAX_REF_SIZE> <divisor> [<output filename>] [--check]"
                  << std::endl;
        std::cerr << "<divisor> represents the amount by which the number of candidates is divided to define the length of an apoch"
                  << std::endl;
        return 1;
    }

    const char* input_basename = argv[1];
    const char* cluster_filename = argv[2];
    const uint32_t MAX_REF_SIZE = std::atoi(argv[3]);
    const uint32_t divisor = std::atoi(argv[4]);

    const char* output_filename = nullptr;
    if (argc > 5) {
        output_filename = argv[5];
    }

    bool check = false;
    if (argc > 6 && std::string(argv[6]) == "--check") {
        check = true;
    }

    clustered_binary_freq_collection input(input_basename, check);
    global_parameters params;
    params.log_partition_size = configuration::get().log_partition_size;
    create_clustered_collection(
        input, params, cluster_filename,
        output_filename, check, MAX_REF_SIZE, divisor
    );

    return 0;
}


#include "conditional_algorithm.h"

#include <Eigen/Dense>

#include "base_algorithm.h"
#include "src/collectors/base_collector.h"
#include "src/mixings/conditional_mixing.h"

void ConditionalAlgorithm::initialize() {
  BaseAlgorithm::initialize();
  cond_mixing = std::dynamic_pointer_cast<ConditionalMixing>(mixing);
}

//! \param grid Grid of points in matrix form to evaluate the density on
//! \param coll Collector containing the algorithm chain
Eigen::MatrixXd ConditionalAlgorithm::eval_lpdf(
    BaseCollector *const collector, const Eigen::MatrixXd &grid,
    const Eigen::MatrixXd &hier_covariates /*= Eigen::MatrixXd(0, 0)*/,
    const Eigen::MatrixXd &mix_covariates /*= Eigen::MatrixXd(0, 0)*/) {
  return Eigen::MatrixXd::Zero(1, 1);  // TODO
}


//
// Copyright (c) 2011-2012 Krzysztof Jusiak (krzysztof at jusiak dot net)
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef QFSM_BACK_DETAIL_ORTHOGONALREGIONS_HPP
#define QFSM_BACK_DETAIL_ORTHOGONALREGIONS_HPP

#include <boost/mpl/int.hpp>
#include <boost/mpl/is_sequence.hpp>
#include <boost/mpl/size.hpp>

namespace QFsm
{
namespace Back
{

template<typename TDerived>
class OrthogonalRegions
{
public:
    typedef boost::mpl::int_<1> OneRegion;
    typedef boost::mpl::int_<0> FirstRegion;

    QFSM_UML_EXTENSIONS
    (
        struct GetNumberOfOrthogonalRegions : boost::mpl::if_
            <
                boost::mpl::is_sequence<typename TDerived::InitialState>,
                boost::mpl::size<typename TDerived::InitialState>,
                boost::mpl::int_<1>
            >
        { };

        typedef boost::mpl::int_<GetNumberOfOrthogonalRegions::type::value> NumberOfOrthogonalRegions;
    )

    QFSM_UML_EXTENSIONS_ELSE(
        typedef OneRegion NumberOfOrthogonalRegions;
    )

    template<typename Size = NumberOfOrthogonalRegions, typename Region = FirstRegion, bool dummy = false> class ForEachRegion
    {
        typedef boost::mpl::int_<Region::value + OneRegion::value> NextRegion;

    public:
        template<typename T, typename Arg1> static void execute(Arg1& p_arg1)
        {
            T::template execute<Region>(p_arg1);
            ForEachRegion<Size, NextRegion>::template execute<T>(p_arg1);
        }

        template<typename T, typename Arg1, typename Arg2> static void execute(Arg1& p_arg1, Arg2& p_arg2)
        {
            T::template execute<Region>(p_arg1, p_arg2);
            ForEachRegion<Size, NextRegion>::template execute<T>(p_arg1, p_arg2);
        }

        template<typename T, typename Arg1, typename Arg2, typename Arg3> static void execute(Arg1& p_arg1, Arg2& p_arg2, Arg3& p_arg3)
        {
            T::template execute<Region>(p_arg1, p_arg2, p_arg3);
            ForEachRegion<Size, NextRegion>::template execute<T>(p_arg1, p_arg2, p_arg3);
        }
    };

    template<bool dummy> class ForEachRegion<OneRegion, FirstRegion, dummy>
    {
    public:
        template<typename T, typename Arg1> static void execute(Arg1& p_arg1)
        {
            T::template execute<FirstRegion>(p_arg1);
        }

        template<typename T, typename Arg1, typename Arg2> static void execute(Arg1& p_arg1, Arg2& p_arg2)
        {
            T::template execute<FirstRegion>(p_arg1, p_arg2);
        }

        template<typename T, typename Arg1, typename Arg2, typename Arg3> static void execute(Arg1& p_arg1, Arg2& p_arg2, Arg3& p_arg3)
        {
            T::template execute<FirstRegion>(p_arg1, p_arg2, p_arg3);
        }
    };

    template<bool dummy> class ForEachRegion<NumberOfOrthogonalRegions, NumberOfOrthogonalRegions, dummy>
    {
    public:
        template<typename T, typename Arg1> static void execute(Arg1&) { }
        template<typename T, typename Arg1, typename Arg2> static void execute(Arg1&, Arg2&) { }
        template<typename T, typename Arg1, typename Arg2, typename Arg3> static void execute(Arg1&, Arg2&, Arg3&) { }
    };
};

} // namespace Back
} // namespace QFsm

#endif


/* ============================================================================================== *
 *                                                                                *
 *                                     Galaxia Blockchain                                         *
 *                                                                                *
 * ---------------------------------------------------------------------------------------------- *
 * This file is part of the Xi framework.                                                         *
 * ---------------------------------------------------------------------------------------------- *
 *                                                                                *
 * Copyright 2018-2019 Xi Project Developers <support.xiproject.io>                               *
 *                                                                                *
 * 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 <https://www.gnu.org/licenses/>.                                                   *
 *                                                                                *
 * ============================================================================================== */

#include "Xi/Crypto/PasswordContainer.h"

#include <algorithm>
#include <limits>

#include <Xi/ExternalIncludePush.h>
#include <openssl/evp.h>
#include <boost/random/random_device.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <Xi/ExternalIncludePop.h>

Xi::Crypto::PasswordContainer::PasswordContainer(const std::string &password) {
  generateConfig();
  m_hash = computeHash(password);
}

bool Xi::Crypto::PasswordContainer::validate(const std::string &password) const {
  return hashEqual(m_hash, computeHash(password));
}

Xi::Crypto::PasswordContainer::hash_t Xi::Crypto::PasswordContainer::computeHash(const std::string &password) const {
  hash_t hash;
  PKCS5_PBKDF2_HMAC_SHA1(password.data(), static_cast<int>(password.size()),
                         reinterpret_cast<const uint8_t *>(m_salt.data()), static_cast<int>(m_salt.size()),
                         static_cast<int>(m_iterations), static_cast<int>(hash.size()), hash.data());
  return hash;
}

void Xi::Crypto::PasswordContainer::generateConfig() {
  boost::random::random_device rng;
  rng.generate(m_salt.begin(), m_salt.end());
  boost::random::uniform_int_distribution<uint32_t> dist{std::numeric_limits<uint16_t>::max(),
                                                         2u * std::numeric_limits<uint16_t>::max()};
  m_iterations = dist(rng);
}

bool Xi::Crypto::PasswordContainer::hashEqual(const PasswordContainer::hash_t &lhs,
                                              const PasswordContainer::hash_t &rhs) {
  return std::equal(lhs.begin(), lhs.end(), rhs.begin(), rhs.end());
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#ifndef BOOST_SIMD_FUNCTION_SCALAR_TAN_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SCALAR_TAN_HPP_INCLUDED

#include <boost/simd/function/definition/tan.hpp>
#include <boost/simd/arch/common/scalar/function/tan.hpp>

#endif


#include "mwoibn/hierarchical_control/controllers/wheels.h"
#include "mwoibn/hierarchical_control/tasks/castor_angle_task.h"
#include <boost/range/adaptor/reversed.hpp>

bool mwoibn::hierarchical_control::controllers::Wheels::_checkStack()
{
        _last_state.noalias() = _state;
        bool stack = true;
        for (int i = 0; i < _size; i++)
        {
//    bool high =
//        _inversers_ptrs[_id_high]->getJacobian().row(i).isApproxToConstant(
//            0, 1e-3);
//    bool low =
//        _inversers_ptrs[_id_low]->getJacobian().row(i).isApproxToConstant(0,
//                                                                          1e-3);

                if (_state[i] && _inversers_ptrs[_id_high]->getJacobian().row(i).cwiseAbs().maxCoeff() < 6e-3)
                {
                        //      std::cout << "!high\t" <<
                        //      _inversers_ptrs[_id_high]->getJacobian().row(i) << std::endl;
                        _state[i] = 0;
//      _values[i] = 0;
                }
                else if (!_state[i] && _inversers_ptrs[_id_high]->getJacobian().row(i).cwiseAbs().maxCoeff() > 8e-3)
                {
                        //      std::cout << "!high\t" <<
                        //      _inversers_ptrs[_id_high]->getJacobian().row(i) << std::endl;
                        _state[i] = 1;
//      _values[i] = 0;
                }
//    else if ()_state[i] = 1;
//    else if (low)
//    {
//      //      std::cout << "!low\t" <<
//      //      _inversers_ptrs[_id_low]->getJacobian().row(i) << std::endl;

//      _state[i] = 1;
//      _values[i] = 100;
//    }
//    else
//    {
//      //      std::cout << "compare" << std::endl;
//      double max_high =
//          _inversers_ptrs[_id_high]->getJacobian().row(i).cwiseAbs().maxCoeff();
//      double max_low =
//          _inversers_ptrs[_id_low]->getJacobian().row(i).cwiseAbs().maxCoeff();

                //      std::cout << "max_high" << max_high << std::endl;
                //      std::cout << "max_low" << max_low<< std::endl;

//      double value = max_high / max_low;
//      if (value > 1.05)
//        _state[i] = 1;
//      else if (value < 0.95)
//        _state[i] = 0;

//      _values[i] = value;

//          std::cout << "high\t" << high << "\t" <<
//      //              <<
//      //              << "\t" <<
//                    _inversers_ptrs[_id_high]->getJacobian().row(i)
//                    << std::endl;
//          std::cout << "jacobian\t" << high << "\t" <<
//      //              <<
//      //              << "\t" <<
//                    _task_high.getJacobian().row(i)
//                    << std::endl;

//          std::cout << "low\t" << low << "\t"
//                    <<
//                    _inversers_ptrs[_id_low]->getJacobian().row(i)  << std::endl;
//    }

                if(_state[i] != _last_state[i]) {
                        tasks::CastorAngleTask& cast = dynamic_cast<tasks::CastorAngleTask&>(_task_low);
                        if(!_state[i])
                                cast.setReference(i, cast.getCurrent()[i]);
//      else
//        cast.setReference(i, -mwoibn::PI);
                }

                stack = stack && (_state[i] == _last_state[i]);
        }

////  if (!stack)
////  {
//    std::cout << _values.transpose() << ",\t" << _state.transpose()
//              << std::endl;
        std::cout << _state.transpose() << "\t";
//                << std::endl;
        for (int i = 0; i < _size; i++) {
                std::cout
                << "high: "
                << _inversers_ptrs[_id_high]->getJacobian().row(i).cwiseAbs().maxCoeff()
                << ",\t"
                << " low: "
                << _inversers_ptrs[_id_low]->getJacobian().row(i).cwiseAbs().maxCoeff()         << ",\t";
        }
//    }
//    std::cout << std::endl;
//  //  std::cout << stack << std::endl;

        return stack;
}

void mwoibn::hierarchical_control::controllers::Wheels::_findTasks()
{
        auto it = std::find_if(_tasks.begin(), _tasks.end(), [this](auto task){ return &task.get() == &(this->_task_high); });

        if (it == _tasks.end())
                throw(std::invalid_argument("mwoibn::hierarchical_control::controllers::Wheels: couldn't find a "
                                            "task_high in a tasks stack."));
        _id_high = it - _tasks.begin();

        it = std::find_if(_tasks.begin(), _tasks.end(), [this](auto task){ return &task.get() == &(this->_task_low); });
        // it = std::find(_tasks.begin(), _tasks.end(), _task_low);

        if (it == _tasks.end())
                throw(std::invalid_argument("mwoibn::hierarchical_control::controllers::Wheels: couldn't find a "
                                            "task_low in a tasks stack."));
        _id_low = it - _tasks.begin();
}

//void mwoibn::hierarchical_control::HierarchicalControllerWheels::_updateTask(
//    int i, mwoibn::hierarchical_control::BasicTask* task)
//{
//  _errors[i].noalias() = -(_gains[i].asDiagonal() * task->getError());
//  _errors[i].noalias() -= task->getJacobian() * _command;

//  if (_errors[i].size())
//  {
//    _inversers_ptrs[i]->compute(task->getJacobian(), _P);
//    _command.noalias() += _inversers_ptrs[i]->getInverse() * _errors[i];
//  }
//}

void mwoibn::hierarchical_control::controllers::Wheels::compute()
{
        _command.setZero();
        _P.setIdentity();

        mwoibn::Matrix P_1 = _P, P_2 = _P;
        int i = 0;

        for (auto& task : _tasks)
        {
                if (_errors[i].size())
                {
                        if(&task.get() == &_task_high) {
                                std::cout << i << std::endl;
                                P_1 = _P; // just for now
                        }
                        if(&task.get() == &_task_low)
                                P_2 = _P;  // just for now
                        _inversers_ptrs[i]->compute(task.get().getJacobian(), _P);
                }
                ++i;
        }

        if (!_checkStack()) {
                _P = P_2;
                _task_low.update();
                _inversers_ptrs[_id_low]->compute(_task_low.getJacobian(), _P);

                _last_stack_change = 0;
                std::cout << "stack change detected" << std::endl;
        }

//  i = 0;

        for (int k = 0; k < _tasks.size()-2; k++)
        {
                auto& task = _tasks[k];

                _errors[k].noalias() = -(_gains[k].asDiagonal() * task.get().getError());
                _errors[k].noalias() -= task.get().getJacobian() * _command;

                _command.noalias() += _inversers_ptrs[k]->getInverse() * _errors[k];

        }

//  i = _tasks_ptr.size()-2;
        _errors[_id_high].noalias() = -(_gains[_id_high].asDiagonal() * _task_high.getError());
        _errors[_id_high].noalias() -= _task_high.getJacobian() * _command;
        _command.noalias() += _inversers_ptrs[_id_high]->getInverse() * _errors[_id_high];

        _errors[_id_low].noalias() = -(_gains[_id_low].asDiagonal() * _task_low.getError());
        _errors[_id_low].noalias() -= _task_low.getJacobian() * _command;

        for (int j = 0; j < _task_low.getTaskSize(); j++) {
                if(_state[j])
                        _errors[_id_low][j] = 0;
        }

        _command.noalias() += _inversers_ptrs[_id_low]->getInverse() * _errors[_id_low];

        if (!_last_stack_change)
                _resetCorrection();

        _g.setZero();
        int cols = _g.cols();
        _g_it.setIdentity();
        i = _tasks.size()-1;
        for (auto& task : boost::adaptors::reverse(_tasks))
        {
                cols -= task.get().getTaskSize();

                __n_by_n1.setIdentity();
                __n_by_n2.noalias() = _inversers_ptrs[i]->getInverse() * task.get().getJacobian();
                __n_by_n1.noalias() -= __n_by_n2;
                __n_by_n2.noalias() = _g_it * __n_by_n1;

                _g_it.noalias() = __n_by_n2;

                _g.block(0, cols, _robot.getDofs(), task.get().getTaskSize()) =
                        _g_it * _inversers_ptrs[i]->getInverse();
                i--;
        }

        ++_last_stack_change;

        std::cout << "exp" << exp(-_mu * _last_stack_change *_robot.rate()) << std::endl;
//   change with respect to the original
        _command.noalias() +=
                _g * _e * exp(-_mu * _last_stack_change *
                              _robot.rate()); //! \todo ** frequency should be an outside argument
}


/**
 * @file envelope_generator.cc
 * Computes reverberation envelopes from eigenverbs.
 */
#include <usml/eigenverb/envelope_generator.h>
#include <usml/eigenverb/eigenverb.h>
#include <usml/sensors/beam_pattern_map.h>
#include <usml/sensors/beam_pattern_model.h>
#include <usml/threads/smart_ptr.h>
#include <boost/foreach.hpp>

using namespace usml::eigenverb ;
using namespace usml::sensors ;

/**
 * Minimum intensity level for valid reverberation contributions (dB).
 */
double envelope_generator::intensity_threshold = -300.0 ;

/**
 * Maximum distance between the peaks of the source and receiver eigenverbs.
 */
double envelope_generator::distance_threshold = 6.0 ;

/**
 * The mutex for static properties.
 */
read_write_lock envelope_generator::_property_mutex;

/**
 * Time axis for reverberation calculation.  Defaults to
 * a linear sequence out to 400 sec with a sampling period of 0.1 sec.
 */
unique_ptr<const seq_vector> envelope_generator::_travel_time( new seq_linear(0.0,0.1,400.0) ) ;

/**
 * Copies envelope computation parameters from static memory into
 * this specific task.
 */
envelope_generator::envelope_generator(
	sensor_pair* sensor_pair,
	double initial_time,
	size_t src_freq_first,
	size_t num_azimuths
):
    _done(false),
    _initial_time(initial_time),
    _ocean( ocean_shared::current() ),
    _sensor_pair(sensor_pair),
    _src_eigenverbs(sensor_pair->source()->eigenverbs()),
    _rcv_eigenverbs(sensor_pair->receiver()->eigenverbs()),
    _eigenverb_interpolator(sensor_pair->receiver()->frequencies(),sensor_pair->frequencies())
{
    write_lock_guard guard(_property_mutex);

    // Get source_params for reverb_duration and pulse_length, and beam_list size
    source_params::reference src_params = _sensor_pair->source()->source();
    _src_beam_list = src_params->beam_list();

    // Get receiver_params for beam_list size
    sensor_params::id_type rcv_params_ID = _sensor_pair->receiver()->paramsID();
    receiver_params::reference rcv_params =
        receiver_params_map::instance()->find(rcv_params_ID);

    _rcv_beam_list = rcv_params->beam_list();

    add_envelope_listener(_sensor_pair);

    _envelopes = envelope_collection::reference( new envelope_collection(
    	_sensor_pair->frequencies(),
        src_freq_first,
        _travel_time.get(),
        src_params->reverb_duration(),
        src_params->pulse_length(),
        pow(10.0,intensity_threshold/10.0),
        num_azimuths,
        _src_beam_list.size(),
        _rcv_beam_list.size(),
        _initial_time,
        _sensor_pair->source()->sensorID(),
        _sensor_pair->receiver()->sensorID(),
        _sensor_pair->source()->position(),
        _sensor_pair->receiver()->position() ) ) ;
}

/**
 * Executes the Eigenverb reverberation model.
 */
void envelope_generator::run() {

	// create memory for work products

    const seq_vector* freq = _envelopes->envelope_freq() ;
    const size_t num_freq = freq->size() ;

	vector<double> scatter( num_freq, 1.0 ) ;
	matrix<double> src_beam( num_freq, _envelopes->num_src_beams(), 1.0 ) ;
	matrix<double> rcv_beam( num_freq, _envelopes->num_rcv_beams(), 1.0 ) ;

	eigenverb rcv_verb ;
	rcv_verb.frequencies = freq ;
	rcv_verb.power = vector<double>( num_freq ) ;

	// loop through eigenrays for each interface

	for ( size_t interface=0 ; interface < _rcv_eigenverbs->num_interfaces() ; ++interface) {

		BOOST_FOREACH( eigenverb verb, _rcv_eigenverbs->eigenverbs(interface) ) {
			_eigenverb_interpolator.interpolate(verb,&rcv_verb) ;

			// Cull eigenverbs down with rtree.query
			std::vector<value_pair> result_s;
			_src_eigenverbs->query_rtree(interface, rcv_verb, result_s);

			BOOST_FOREACH( value_pair const& vp, result_s ) {

				eigenverb src_verb = *(vp.second);

				// determine relative range and bearing between the projected Gaussians
				// skip this combo if source peak too far away

			    double bearing ;
			    const double range = rcv_verb.position.gc_range( src_verb.position, &bearing ) ;
			    if ( range > distance_threshold * max(rcv_verb.length,rcv_verb.width)) continue ;

			    if ( range < 1e-6 ) bearing = 0 ;	// fixes bearing = NaN
			    bearing -= rcv_verb.direction ;		// relative bearing

			    const double ys = range * cos( bearing ) ;
			    const double ys2 = ys * ys ;
			    if ( abs(ys) > distance_threshold * rcv_verb.length ) continue ;

			    const double xs = range * sin( bearing ) ;
			    const double xs2 = xs * xs ;
			    if ( abs(xs) > distance_threshold * rcv_verb.width ) continue ;

				// compute interface scattering strength
			    // skip this combo if scattering strength is trivial

				if ( ! scattering( interface,
					 rcv_verb.position, *freq,
					 src_verb.grazing, rcv_verb.grazing,
					 src_verb.direction, rcv_verb.direction,
					 &scatter ) ) continue ;

				// compute beam levels

				src_beam = beam_gain(_src_beam_list, freq, src_verb.source_de,
					src_verb.source_az, _sensor_pair->source()->orient());
				rcv_beam = beam_gain(_rcv_beam_list, freq, rcv_verb.source_de,
					rcv_verb.source_az, _sensor_pair->receiver()->orient());

				// create envelope contribution

				_envelopes->add_contribution( src_verb, rcv_verb,
						src_beam, rcv_beam, scatter, xs2, ys2 ) ;
			}
		}
	}
	this->notify_envelope_listeners(_envelopes) ;
}

/**
 * Computes the beam_gain
 */
matrix<double> envelope_generator::beam_gain(
    sensor_params::beam_pattern_list beam_list,
    const seq_vector* freq, double de_rad, double az_rad, orientation orient)
{
    const vector<double> frequencies = *freq;	// beam_level requires ublas vector
    matrix<double> beam_matrix( freq->size(), beam_list.size() ) ;
    vector<double> level( freq->size(), 0.0 ) ;
    BOOST_FOREACH( beam_pattern_model::id_type id, beam_list) {
        beam_pattern_model::reference bp = beam_pattern_map::instance()->find(id);
        bp->beam_level(de_rad, az_rad, orient, frequencies, &level ) ;

        std::copy(level.begin(), level.end(), beam_matrix.begin1());
    }
    return beam_matrix;
}

/**
 * Computes the broadband scattering strength for a specific interface.
 */
bool envelope_generator::scattering( size_t interface, const wposition1& location,
		const seq_vector& frequencies, double de_incident,
		double de_scattered, double az_incident, double az_scattered,
		vector<double>* amplitude)
{
	switch ( interface ) {
	case eigenverb::BOTTOM:
		_ocean->bottom().scattering(location,frequencies,
				de_incident,de_scattered,az_incident,az_scattered,
				amplitude) ;
		break;
	case eigenverb::SURFACE:
		_ocean->surface().scattering(location,frequencies,
				de_incident,de_scattered,az_incident,az_scattered,
				amplitude) ;
		break;
	default:
		size_t layer = (size_t) floor( (interface-2.0)/2.0 ) ;
		_ocean->volume( layer ).scattering(location,frequencies,
				de_incident,de_scattered,az_incident,az_scattered,
				amplitude) ;
		break;
	}

	// check to see that scattering strenght is not trivial

	BOOST_FOREACH (double amp, *amplitude){
        if (amp >= intensity_threshold ) {
            return true;
        }
    }
    return true;
}


/* Copyright (C) 2010 Ion Torrent Systems, Inc. All Rights Reserved */
#include "EvaluateKey.h"
#include "IonH5Eigen.h"
#include "ZeromerMatDiff.h"
#include <malloc.h>
#include <Eigen/Dense>
#include <Eigen/LU>

using namespace Eigen;
#define NUM_STATS_FIELDS 9
#define SNR_IDX 0
#define MAD_IDX 1
#define OK_IDX 2
#define TRACE_SD_IDX 3
#define PROJ_RES_IDX 4
#define PEAK_IDX 5
#define TAUE_IDX 6
#define TAUB_IDX 7
#define MEAN_SIG_IDX 8
#define MIN_CRAZY_PEAK_VAL 0
#define MAX_CRAZY_PEAK_VAL 2000
#define MIN_INTEGRATION_WINDOW 15
#define DEFAULT_INTEGRATION_WINDOW 20
#define MIN_SAMPLES_FOR_STATS .05
#define MIN_SNR_FOR_STATS 8
#define MIN_PEAK_FOR_STATS 35
#define MAX_PEAK_FOR_STATS 1000
#define MAX_MAD_FOR_STATS 12
#define MIN_PEAK_SEARCH_END 15
#define MAX_PEAK_WINDOW 2

EvaluateKey::EvaluateKey() {
  Init();
}

void EvaluateKey::Init() {
  m_debug = false;
  m_trace_data = m_ref_data = m_zeromer_est = m_shifted_ref = NULL;
  m_avg_0mer = m_sd_0mer = m_avg_1mer = m_sd_1mer = NULL;
  m_num_wells = m_num_well_flows = m_num_frames = m_num_flows = 0;
  m_integration_start = m_integration_end = m_peak_start = m_peak_end = 0;
  m_min_integration = 0;
  m_flow_avg_1mer = NULL;
  m_flow_count_1mer = NULL;
  m_doing_darkmatter = m_use_projection = m_peak_signal_frames = m_integration_width = m_normalize_1mers = false;
  fill(m_flow_key_avg.begin(), m_flow_key_avg.end(), 0.0f);
  m_flow_key_avg.resize(0);
}

void EvaluateKey::Alloc(size_t num_well_flows, size_t num_frames, size_t num_flows, int num_wells) {
  size_t total_size = num_well_flows * num_frames;
  if (m_num_well_flows != num_well_flows || m_num_frames != num_frames) {
    if (m_trace_data != NULL) {
      Cleanup();
    }
    m_num_wells = num_wells;
    m_num_well_flows = num_well_flows;
    m_num_frames = num_frames;
    m_num_flows = num_flows;
    m_trace_data = (float *) memalign(32, sizeof(float) * total_size);
    m_ref_data = (float *) memalign(32, sizeof(float) * total_size);
    m_zeromer_est = (float *) memalign(32, sizeof(float) * total_size);
    m_shifted_ref = (float *) memalign(32, sizeof(float) * total_size);
    m_avg_0mer = (float *) memalign(32, sizeof(float) * num_frames);
    m_sd_0mer = (float *) memalign(32, sizeof(float) * num_frames);
    m_avg_1mer = (float *) memalign(32, sizeof(float) * num_frames);
    m_sd_1mer = (float *) memalign(32, sizeof(float) * num_frames);
    m_flow_avg_1mer = (float *) memalign(32, sizeof(float) * num_frames * num_flows);
    m_flow_count_1mer = (int *) memalign(32, sizeof(int) * num_flows);
    // these are small, just for sanity initialize them
    memset(m_avg_0mer,0,sizeof(float) * num_frames);
    memset(m_sd_0mer,0,sizeof(float) * num_frames);
    memset(m_avg_1mer,0,sizeof(float) * num_frames);
    memset(m_sd_1mer,0,sizeof(float) * num_frames);
    memset(m_flow_avg_1mer, 0, sizeof(float) * num_frames * num_flows);
    memset(m_flow_count_1mer, 0, sizeof(int) * num_flows);
  }
}

void EvaluateKey::Cleanup() {
    if (m_trace_data != NULL) {
      free(m_trace_data);
      free(m_ref_data);
      free(m_zeromer_est);
      free(m_shifted_ref);
      free(m_avg_0mer);
      free(m_sd_0mer);
      free(m_avg_1mer);
      free(m_sd_1mer);
      free(m_flow_avg_1mer);
      free(m_flow_count_1mer);
      Init();
    }
  }

void EvaluateKey::SetSizes(int row_start, int row_end,
                           int col_start, int col_end,
                           int flow_start, int flow_end,
                           int frame_start, int frame_end) {
  size_t num_wells = (row_end - row_start) * (col_end - col_start);
  size_t num_well_flows = num_wells * (flow_end - flow_start);
  size_t num_frames = frame_end - frame_start;
  size_t num_flows = flow_end - flow_start;
  Alloc( num_well_flows, num_frames, num_flows, num_wells);
  m_peak_start = m_integration_start = 0;
  m_min_integration = min((size_t)DEFAULT_INTEGRATION_WINDOW, num_frames);
  m_integration_end = min(m_num_frames, (size_t)DEFAULT_INTEGRATION_WINDOW);
  m_peak_end = min(m_num_frames, (size_t)MIN_PEAK_SEARCH_END);
  // these are small, just for sanity initialize them
  memset(m_avg_0mer,0,sizeof(float) * num_frames);
  memset(m_sd_0mer,0,sizeof(float) * num_frames);
  memset(m_avg_1mer,0,sizeof(float) * num_frames);
  memset(m_sd_1mer,0,sizeof(float) * num_frames);
}

void EvaluateKey::SetUpMatrices(TraceStoreCol &trace_store,
                                int col_stride, int flow_stride,
                                int row_start, int row_end, int col_start, int col_end,
                                int flow_start, int flow_end,
                                int frame_start, int frame_end,
                                float *trace_data,
                                float *ref_data) {
  int row_size = row_end - row_start;
  int col_size = col_end - col_start;
  int flow_size = flow_end - flow_start;
  int frame_size = frame_end - frame_start;
  size_t local_flow_stride = row_size * col_size;
  size_t total_rows = local_flow_stride * flow_size;

  std::vector<float> trace(trace_store.GetNumFrames());
  std::vector<float> ref_trace(trace_store.GetNumFrames());

  for (int flow_ix = flow_start; flow_ix < flow_end; flow_ix++) {
    for (int row_ix = row_start; row_ix < row_end; row_ix++) {
      for (int col_ix = col_start; col_ix < col_end; col_ix++) {
        int well_ix =  row_ix * col_stride + col_ix;
        int local_well_ix = (flow_ix - flow_start) * local_flow_stride + (row_ix - row_start) * col_size + (col_ix - col_start);
        trace_store.GetReferenceTrace(well_ix, flow_ix, &ref_trace[0]);
        for (int frame_ix = frame_start; frame_ix < frame_end; frame_ix++) {
          ref_data[local_well_ix + (frame_ix - frame_start) * total_rows] = ref_trace[frame_ix];
        }
      }
      for (int frame_ix = frame_start; frame_ix < frame_end; frame_ix++) {
        int well_ix = row_ix * col_stride + col_start;
        int local_well_ix = (row_ix - row_start) * col_size;
        int16_t *__restrict store_start = trace_store.GetMemPtr() + flow_ix * trace_store.mFrameStride + frame_ix * trace_store.mFlowFrameStride + well_ix;
        int16_t *__restrict store_end = store_start + col_size;
        float *__restrict out_start = trace_data + (frame_ix - frame_start) * total_rows + (flow_ix-flow_start) * local_flow_stride + local_well_ix;
        while(store_start != store_end) {
          *out_start++ = *store_start++;
        }
      }
    }
  }
}

void EvaluateKey::FitTauB(KeySeq &key, const float *time, float taue_est, int frame_start, int frame_end, float *__restrict taub) {
  if (m_flow_order.size() >= m_num_flows) {
    ZeromerMatDiff::FitTauBNuc(&key.zeroFlows[0], key.zeroFlows.size(),
                               m_trace_data, m_shifted_ref,
                               &m_flow_order[0],
                               m_num_wells, m_num_flows, m_num_well_flows,
                               m_num_frames, taue_est, taub);
  }
  else {
    ZeromerMatDiff::FitTauB(&key.zeroFlows[0], key.zeroFlows.size(),
                            m_trace_data, m_shifted_ref,
                            m_num_wells, m_num_flows, m_num_well_flows,
                            m_num_frames, taue_est, taub);
  }
}

void EvaluateKey::PredictZeromersVec(const float *time, float taue_est, float * __restrict taub) {
  ZeromerMatDiff::PredictZeromersSignal(time, m_num_frames,
                                        m_trace_data, m_shifted_ref, m_zeromer_est,
                                        m_num_wells, m_num_flows, m_num_well_flows,
                                        taue_est, taub, "");
}

void EvaluateKey::ScoreKeySignals(KeySeq &key, float *__restrict key_signal_ptr,
                                  int integration_start, int integration_end,
                                  int peak_start, int peak_end,
                                  float *onemer_norm,
                                  float *__restrict key_results_ptr,
                                  int num_result_cols, float *__restrict taub, float taue_est) {
  Map<MatrixXf, Aligned> key_signal(key_signal_ptr, m_num_well_flows, m_num_frames);
  Map<MatrixXf, Aligned> key_results(key_results_ptr, m_num_wells, num_result_cols);

  // Calculate basics per well for integral, mad, max for each well/flow
  VectorXf key_integrals(m_num_well_flows), key_mad(m_num_well_flows), key_max(m_num_well_flows);
  key_integrals.setZero();
  key_mad.setZero();
  key_max.setZero();
  for (int frame_ix = peak_start; frame_ix < peak_end; frame_ix++) {
    float *__restrict key_sig_start = key_signal.data() + frame_ix * m_num_well_flows;
    //    float *__restrict one_forward = key_signal.data() + min(frame_ix+1,(int)m_num_frames-1) * m_num_well_flows;
    float *__restrict one_backward = key_signal.data() + max(frame_ix-1,0) * m_num_well_flows;
    float *__restrict key_sig_end = key_sig_start + m_num_well_flows;
    float *__restrict peak = key_max.data();
    while (key_sig_start != key_sig_end) {
      float value = (*key_sig_start + *one_backward) / 2;
      *peak = max(*peak, value);
      peak++;
      one_backward++;
      key_sig_start++;
    }
  }

  for (int frame_ix = integration_start; frame_ix < integration_end; frame_ix++) {
    key_integrals.array() += key_signal.col(frame_ix).array();
    key_mad.array() += key_signal.col(frame_ix).array().abs();
  }

  // Our summary statistics for onemers and zeromers.
  VectorXf onemer_mean(m_num_wells), onemer_m2(m_num_wells), zeromer_mean(m_num_wells), zeromer_m2(m_num_wells);
  VectorXf all_mean(m_num_wells), all_m2(m_num_wells);

  all_mean.setZero();
  all_m2.setZero();
  onemer_mean.setZero();
  onemer_m2.setZero();
  zeromer_mean.setZero();
  zeromer_m2.setZero();

  int n_onemer = 0;
  // loop through all the onemers and calculate the mean and sd
  for (size_t i = 0; i < key.onemerFlows.size(); i++) {
    n_onemer++;
    int flow_ix = key.onemerFlows[i];
    float norm_factor = 1.0f;
    if (onemer_norm != NULL) {
      norm_factor = onemer_norm[i];
    }
    size_t offset = m_num_wells * flow_ix;
    float *__restrict integral_start = key_integrals.data() + offset;
    float *__restrict integral_end = integral_start + m_num_wells;
    float *__restrict max_key = key_max.data() + offset;
    float *__restrict mean = onemer_mean.data();
    float *__restrict m2 = onemer_m2.data();
    float *__restrict mean_peak = key_results.col(PEAK_IDX).data();
    float delta;
    while (integral_start != integral_end) {
      *integral_start = *integral_start * norm_factor;
      delta = (*integral_start) - *mean;
      *mean += delta/n_onemer;
      *m2 += delta * (*integral_start - *mean);
      *mean_peak++ += *max_key++;
      integral_start++;
      mean++;
      m2++;
    }
  }

  int n_flow = 0;
  for (size_t flow_ix = 0; flow_ix < key.usableKeyFlows; flow_ix++) {
    n_flow++;
    size_t offset = m_num_wells * flow_ix;
    float *__restrict integral_start = key_integrals.data() + offset;
    float *__restrict integral_end = integral_start + m_num_wells;
    float *__restrict mean = all_mean.data();
    float *__restrict m2 = all_m2.data();
    float delta;
    while (integral_start != integral_end) {
      delta = *integral_start - *mean;
      *mean += delta/n_flow;
      *m2 += delta * (*integral_start - *mean);
      integral_start++;
      mean++;
      m2++;
    }
  }

  int num_col = m_num_frames;
  int n_zeromer = 0;
  // loop through all the zeromers and calculate the mean and sd
  for (size_t i = 0; i < key.zeroFlows.size(); i++) {
    n_zeromer++;
    int flow_ix = key.zeroFlows[i];
    size_t offset = m_num_wells * flow_ix;
    float *__restrict integral_start = key_integrals.data() + offset;
    float *__restrict integral_end = integral_start + m_num_wells;
    float *__restrict mad = key_mad.data() + offset;
    float *__restrict mean = zeromer_mean.data();
    float *__restrict m2 = zeromer_m2.data();
    float *__restrict mean_mad = key_results.col(MAD_IDX).data();

    float delta;
    while (integral_start != integral_end) {
      delta = *integral_start - *mean;
      *mean += delta/n_zeromer;
      *m2 += delta * (*integral_start - *mean);
      *mean_mad++ += *mad++ / num_col;
      integral_start++;
      mean++;
      m2++;
    }
  }

  /* combine all the stats into final summaries. */
  float *__restrict snr_start = key_results.col(SNR_IDX).data();
  float *__restrict snr_end = snr_start + m_num_wells;
  float *__restrict o_mean = onemer_mean.data();
  float *__restrict o_m2 = onemer_m2.data();
  float *__restrict z_mean = zeromer_mean.data();
  float *__restrict z_m2 = zeromer_m2.data();
  float *__restrict a_m2 = all_m2.data();
  float *__restrict ok = key_results.col(OK_IDX).data();
  float *__restrict trace_sd = key_results.col(TRACE_SD_IDX).data();
  float *__restrict taub_out = key_results.col(TAUB_IDX).data();
  float *__restrict taue_out = key_results.col(TAUE_IDX).data();
  float *__restrict mean_mad = key_results.col(MAD_IDX).data();
  float *__restrict mean_peak = key_results.col(PEAK_IDX).data();
  float *__restrict onemer_sig = key_results.col(MEAN_SIG_IDX).data();
  while(snr_start != snr_end) {
    float sig = *o_mean++ - *z_mean++;
    float o_sd = sqrt(*o_m2++/n_onemer);
    float z_sd = sqrt(*z_m2++/n_zeromer);
    *onemer_sig++ = sig;
    *snr_start = sig/((o_sd +z_sd)/2.0f);
    *ok = isfinite(*snr_start) ? 1 : 0;
    *trace_sd++ = sqrt(*a_m2++ / n_flow);
    //    *trace_sd++ = sig;
    *taub_out++ = *taub++;
    *taue_out++ = taue_est;
    ok++;
    snr_start++;
    *mean_peak /= n_onemer;
    *mean_mad /= n_zeromer;
    mean_peak++;
    mean_mad++;
  }
}

void PickBestKey(std::vector<Eigen::MatrixXf> &key_results, std::vector<KeySeq> &keys, int local_index, int global_index, KeyFit &fit) {
  fit.keyIndex = -1;
  for (size_t key_ix = 0; key_ix < keys.size(); key_ix++) {
    Eigen::MatrixXf &results = key_results[key_ix];
    float snr = results.coeff(local_index, SNR_IDX);
    float peak = results.coeff(local_index, PEAK_IDX);
    if (key_ix == 0 || ((snr > fit.snr && snr > keys[key_ix].minSnr))) { // || (peak > keys[key_ix].good_enough_peak && snr > keys[key_ix].good_enough_snr))) {
      fit.snr = results.coeff(local_index, SNR_IDX);
      fit.mad = results.coeff(local_index, MAD_IDX);
      fit.ok = results.coeff(local_index, OK_IDX);
      fit.peakSig = results.coeff(local_index, PEAK_IDX);
      fit.sd = results.coeff(local_index, TRACE_SD_IDX);
      fit.tauE = results.coeff(local_index, TAUE_IDX);
      fit.tauB = results.coeff(local_index, TAUB_IDX);
      fit.onemerAvg = results.coeff(local_index, MEAN_SIG_IDX);
    }
    if (isfinite(snr) && (snr >= fit.snr && snr >= keys[key_ix].minSnr && peak >= keys[key_ix].minPeak)) { // || (peak > keys[key_ix].good_enough_peak && snr > keys[key_ix].good_enough_snr))) {
      fit.keyIndex = key_ix;
      fit.snr = snr;
    }
  }
}

void CalculateIntegrationWindow(float *avg_1mer, float *sd_1mer, float *avg_0mer, float *sd_0mer, int n_frames,
                                int min_integration, size_t &integration_start, size_t &integration_end) {
  integration_start = 0;
  integration_end = min_integration;
  while (integration_end < (size_t)n_frames) {
    float signal = avg_1mer[integration_end] - avg_0mer[integration_end];
    float noise_0mer = sd_0mer[integration_end]; // @todo cws - should we have a multiple on noise or avg with 1mer noise?
    float noise_1mer = sd_1mer[integration_end]; // @todo cws - should we have a multiple on noise or avg with 1mer noise?
    if (signal < (noise_0mer + noise_1mer) || (integration_end + 1) == (size_t) n_frames) {
      break;
    }
    integration_end++;
  }
  //  fprintf(stdout, "Finishing on frame %d with signal %.2f and noise %.2f 1mer noise %.2f 0mer signal %.2f\n", integration_end, avg_1mer[integration_end] - avg_0mer[integration_end], sd_0mer[integration_end], sd_1mer[integration_end], avg_0mer[integration_end]);
}

// @todo - cws don't use our soft filtred wells for this
int CalculateIncorporationStats(std::vector<Eigen::MatrixXf> &key_results,
                                std::vector<Eigen::MatrixXf> &key_signals,
                                std::vector<KeySeq> &keys,
                                int num_wells, int num_flows, int num_well_flows, int num_frames,
                                float min_snr, float min_peak,
                                float *avg_0mer, float *sd_0mer, float *avg_1mer,
                                float *sd_1mer,
                                float *flow_avg_1mer, int *flow_count_1mer) {
  Eigen::Map<Eigen::MatrixXf, Eigen::Aligned> flow_avg(flow_avg_1mer, num_flows, num_frames);
  VectorXf flow_onemer_mean(num_frames), onemer_mean(num_frames), onemer_m2(num_frames), zeromer_mean(num_frames), zeromer_m2(num_frames);
  VectorXi flow_onemer_counts(num_frames), onemer_counts(num_frames), zeromer_counts(num_frames);
  VectorXi well_counts(num_wells);
  memset(flow_count_1mer, 0, sizeof(int) * num_flows);
  onemer_mean.setZero();
  onemer_m2.setZero();
  zeromer_mean.setZero();
  zeromer_m2.setZero();
  onemer_counts.setZero();
  zeromer_counts.setZero();
  flow_avg.setZero();
  well_counts.setZero();
  int too_low_snr = 0, too_low_peak = 0, too_high_peak = 0, too_mad = 0;
  for (int flow_ix = 0; flow_ix < num_flows; flow_ix++) {
    flow_onemer_mean.setZero();
    flow_onemer_counts.setZero();
    for (int frame_ix = 0; frame_ix < num_frames; frame_ix++) {
      for (size_t key_ix = 0; key_ix < keys.size(); key_ix++) {
        float *m2 = NULL;
        float *mean = NULL;
        int *count = NULL;
        float *flow_mean = NULL;
        int *flow_count = NULL;
        KeySeq &key = keys[key_ix];
        if (key.flows[flow_ix] == 0) {
          mean = &zeromer_mean[frame_ix];
          m2 = &zeromer_m2[frame_ix];
          count = &zeromer_counts[frame_ix];
        }
        else if (key.flows[flow_ix] == 1) {
          mean = &onemer_mean[frame_ix];
          m2 = &onemer_m2[frame_ix];
          count = &onemer_counts[frame_ix];
          flow_mean = &flow_onemer_mean[frame_ix];
          flow_count = &flow_onemer_counts[frame_ix];
        }
        else {
          continue;
        }
        MatrixXf &results = key_results[key_ix];
        float *__restrict signal_start = key_signals[key_ix].data() + frame_ix * num_well_flows + num_wells * flow_ix;
        float *__restrict signal_end = signal_start + num_wells;
        float *__restrict snr = results.col(SNR_IDX).data();
        float *__restrict peak = results.col(PEAK_IDX).data();
        float *__restrict mad = results.col(MAD_IDX).data();
        int *__restrict well_counts_start =well_counts.data();
        float delta;
        while (signal_start != signal_end) {
          if (*snr >= key.minSnr ) {
            if (*snr >= min_snr && *peak >= min_peak && *peak < MAX_PEAK_FOR_STATS && *mad < MAX_MAD_FOR_STATS) {
              if (flow_mean != NULL) {
                *flow_mean += *signal_start;
                *flow_count += 1;
              }
              if (frame_ix == 0 && flow_ix == 0) {
                (*well_counts_start)++;
              }
              (*count)++;
              delta = *signal_start - *mean;
              *mean += delta / *count;
              *m2 += delta * (*signal_start - *mean);
            }
          }
          well_counts_start++;
          mad++;
          signal_start++;
          snr++;
          peak++;
        }
      }
    }
    for (int frame_ix = 0; frame_ix < num_frames; frame_ix++) {
      flow_avg(flow_ix, frame_ix) = flow_onemer_counts.coeff(frame_ix) > 0 ? flow_onemer_mean.coeff(frame_ix)/flow_onemer_counts.coeff(frame_ix) : 0.0f;
    }
    flow_count_1mer[flow_ix] = flow_onemer_counts[0];
  }
  //  fprintf(stdout, "Filters for summary stats: %d low_snr %d low_peak %d high_peak %d mad\n",too_low_snr , too_low_peak, too_high_peak, too_mad);
  for (int frame_ix = 0; frame_ix < num_frames; frame_ix++) {
    avg_0mer[frame_ix] = zeromer_mean[frame_ix];
    sd_0mer[frame_ix] = sqrt(zeromer_m2[frame_ix]/zeromer_counts[frame_ix]);
    avg_1mer[frame_ix] = onemer_mean[frame_ix];
    sd_1mer[frame_ix] = sqrt(onemer_m2[frame_ix]/onemer_counts[frame_ix]);
  }
  // let the calling function set thresholds for using resulting statistics
  return well_counts.sum();
}

void EvaluateKey::Calculate1MerNormalization(size_t num_flows, size_t num_frames,
                                             std::vector<int> &onemer_flows,
                                             float *flow_1mer_avg, int *flow_1mer_count,
                                             float *norm_factors,
                                             size_t integration_start, size_t integration_end) {

  Eigen::Map<Eigen::MatrixXf, Eigen::Aligned> flow_avg(flow_1mer_avg, num_flows, num_frames);
  std::fill(norm_factors, norm_factors + onemer_flows.size(), 1.0f);
  int onemer_count = onemer_flows.size();
  float integration_sum[onemer_count];
  float mean_integration = 0;
  int threshold = MIN_SAMPLES_FOR_STATS * m_num_wells;
  int seen = 0;
  std::fill(integration_sum, integration_sum + onemer_count, 0);
  for (int i = 0; i < onemer_count; i++) {
    if (flow_1mer_count[onemer_flows[i]] < threshold) {
      continue;
    }
    for (int frame_ix = integration_start; frame_ix < (int)integration_end; frame_ix++) {
      integration_sum[i] += flow_avg(onemer_flows[i], frame_ix);
    }
  }

  for (int i = 0; i < onemer_count; i++) {
    mean_integration += integration_sum[i];
  }

  mean_integration = mean_integration / onemer_count;

  if (mean_integration > 0) {
    for (int i = 0; i < onemer_count; i++) {
      norm_factors[i] = mean_integration / integration_sum[i];
    }
  }
}

void EvaluateKey::FindBestKey(int row_start, int row_end, int col_start, int col_end,
                              int frame_start, int frame_end,
                              int flow_stride, int col_stride,
                              const float *time, std::vector<KeySeq> &keys, float taue_est,
                              float shift, char *bad_wells,
                              std::vector<KeyFit> &key_fits) {
  Eigen::Matrix<float, Dynamic, Dynamic, AutoAlign | ColMajor> zeromer_buffer;
  // allocate some scratch buffers for our intermediate results
  std::vector<Eigen::MatrixXf> key_results(keys.size());
  std::vector<Eigen::MatrixXf> key_signals(keys.size());
  std::vector<Eigen::VectorXf> key_taub(keys.size());
  std::vector<std::vector<float> > key_norm_signals(keys.size());
  for (size_t key_ix = 0; key_ix < keys.size(); key_ix++) {
    // 8 for snr, mad, ok, traceSd, projResid, peakSig, taue, taub
    key_results[key_ix].resize(m_num_wells, NUM_STATS_FIELDS);
    key_results[key_ix].setZero(); // clean out.
    key_signals[key_ix].resize(m_num_well_flows, m_num_frames); // this will be initialized every time.
    key_taub[key_ix].resize(m_num_well_flows);

  }
  // for each key calculate our estimated signals m_num_well_flows, a no-op currently as we don't shift reference
  ZeromerMatDiff::ShiftReference(m_num_frames, m_num_well_flows, shift, m_ref_data, m_shifted_ref);
  Eigen::Map<Eigen::MatrixXf, Eigen::Aligned> trace_data(m_trace_data, m_num_well_flows, m_num_frames);
  Eigen::Map<Eigen::MatrixXf, Eigen::Aligned> zeromer_est(m_zeromer_est, m_num_well_flows, m_num_frames);
  Eigen::Map<Eigen::MatrixXf, Eigen::Aligned> shifted_ref(m_shifted_ref, m_num_well_flows, m_num_frames);
  for (size_t key_ix = 0; key_ix < keys.size(); key_ix++) {
    // Fit taub for each well assuming that key 0mers are 0mers
    FitTauB(keys[key_ix], time, taue_est, frame_start, frame_end, key_taub[key_ix].data());
    // Predict the zeromers
    PredictZeromersVec(time, taue_est, key_taub[key_ix].data());
    // Calculate our "signal" after subtracting estimated zeromer
    key_signals[key_ix] = trace_data - zeromer_est;
    // Normalization default to 1.0 (no normalization)
    key_norm_signals[key_ix].resize(keys[key_ix].onemerFlows.size());
    fill(key_norm_signals[key_ix].begin(), key_norm_signals[key_ix].end(), 1.0f);
    // Gather summary stats about each key fit
    ScoreKeySignals(keys[key_ix], key_signals[key_ix].data(),
                    m_integration_start, m_integration_end,
                    m_peak_start, m_peak_end,
                    &key_norm_signals[key_ix][0],
                    key_results[key_ix].data(), key_results[key_ix].cols(),
                    key_taub[key_ix].data(), taue_est);

  }

  // If we're doing any modifications to fitting try rescoring with new modifications
  if (m_doing_darkmatter || m_use_projection || m_peak_signal_frames || m_integration_width) {
    int num_samples = CalculateIncorporationStats(key_results, key_signals, keys,
                                                  m_num_wells, m_num_flows, m_num_well_flows, m_num_frames,
                                                  MIN_SNR_FOR_STATS, MIN_PEAK_FOR_STATS,
                                                  m_avg_0mer, m_sd_0mer, m_avg_1mer, m_sd_1mer, m_flow_avg_1mer,
                                                  m_flow_count_1mer);
    //    fprintf(stdout, "Num good samples: %d\n", num_samples);
    if (num_samples >= MIN_SAMPLES_FOR_STATS * m_num_wells) {
      Eigen::MatrixXf proj_signal;
      if (m_integration_width) {
        CalculateIntegrationWindow(m_avg_1mer, m_sd_1mer, m_avg_0mer, m_sd_0mer, m_num_frames,
                                   MIN_INTEGRATION_WINDOW,m_integration_start, m_integration_end);
      }
      for (size_t key_ix = 0; key_ix < keys.size(); key_ix++) {
        key_results[key_ix].setZero(); // reset anything learned last time...

        if (m_doing_darkmatter) {
          for (int i = 0; i < key_signals[key_ix].cols(); i++) {
            key_signals[key_ix].col(i).array() -= m_avg_0mer[i];
          }
        }
        if (m_use_projection) {
          Eigen::VectorXf proj_1mer(m_num_frames);
          std::copy(m_avg_1mer, m_avg_1mer + m_num_frames, proj_1mer.data());
          proj_1mer.normalize();
          Eigen::VectorXf coefficients(key_signals[key_ix].rows());
          coefficients.setZero();
          // Should be faster to do this at the column level than row by row
          for (int i = 0; i < key_signals[key_ix].cols(); i++) {
            coefficients.array() += key_signals[key_ix].col(i).array() * proj_1mer[i];
          }
          // Replace signal with coefficient projection of 1mer
          proj_signal.resize(key_signals[key_ix].rows(),key_signals[key_ix].cols());
          for (int i = 0; i < key_signals[key_ix].cols(); i++) {
            //key_signals[key_ix].col(i).array() = coefficients.array() * proj_1mer[i];
            proj_signal.col(i).array() = coefficients.array() * proj_1mer[i];
          }
        }
        if (m_peak_signal_frames) {
          int max_frame = 0;
          float max_value = 0;
          for (int i = 0; i < (int)m_num_frames; i++) {
            if (max_value < m_avg_1mer[i]) {
              max_value = m_avg_1mer[i];
              max_frame = i;
            }
          }
          m_peak_start = max(max_frame - MAX_PEAK_WINDOW,0);
          m_peak_end = min((size_t)(max_frame+ MAX_PEAK_WINDOW),m_num_frames);
        }
        if (m_normalize_1mers) {
          float * norm_1mer = &key_norm_signals[key_ix][0];
          Calculate1MerNormalization(m_num_flows, m_num_frames,
                                     keys[key_ix].onemerFlows,
                                     m_flow_avg_1mer, m_flow_count_1mer,
                                     norm_1mer,
                                     m_integration_start,
                                     m_integration_end);
        }
        Eigen::MatrixXf *signal = &key_signals[key_ix];
        if (m_use_projection) {
          signal = &proj_signal;
        }
        // rescore the key signals after all modifications
        ScoreKeySignals(keys[key_ix], signal->data(),
                        m_integration_start, m_integration_end,
                        m_peak_start, m_peak_end,
                        &(key_norm_signals[key_ix][0]),
                        key_results[key_ix].data(), key_results[key_ix].cols(),
                        key_taub[key_ix].data(), taue_est);
      }
    }
  }

  // For each well pick the best key...
  int col_size = col_end - col_start;
  int sum_flow_frame_stride = m_num_frames * m_num_flows;
  std::vector<float> key_flow_avg_sum(keys.size() * sum_flow_frame_stride, 0);
  m_key_counts.resize(keys.size(), 0);
  std::fill(m_key_counts.begin(), m_key_counts.end(), 0);
  m_flow_key_avg.resize(keys.size() * m_num_flows * m_num_frames);
  std::fill(m_flow_key_avg.begin(), m_flow_key_avg.end(), 0.0f);
  Eigen::Map<Eigen::VectorXf, Eigen::Aligned> dark_matter(m_avg_0mer, m_num_frames);
  for (int row_ix = row_start; row_ix < row_end; row_ix++) {
    for (int col_ix = col_start; col_ix < col_end; col_ix++) {
      int global_index = row_ix * col_stride + col_ix;
      int local_index = (row_ix - row_start) * col_size + (col_ix - col_start);
      key_fits[global_index].wellIdx = global_index;
      // @todo cws copy in the best signal here as well, with library or best key if not called though?
      PickBestKey(key_results, keys, local_index, global_index, key_fits[global_index]);
      // store the preferred zeromer and stats for chosen key if we're doing a dump of data
      int key_ix = key_fits[global_index].keyIndex;
      bool no_nan = true;
      if (key_ix >= 0) {
        for (size_t frame_ix = 0; frame_ix < m_num_frames; frame_ix++) {
          for (size_t flow_ix = 0; flow_ix < m_num_flows; flow_ix++) {
            size_t z_ix = flow_ix * m_num_wells + local_index;
            if (!isfinite(key_signals[key_ix].coeff(z_ix, frame_ix))) {
              no_nan = false;
            }
          }
        }
      }
      if (no_nan) {
        if (key_ix >= 0 && bad_wells[global_index] == 0 &&
            key_results[key_ix].coeff(local_index, PEAK_IDX) > MIN_CRAZY_PEAK_VAL &&
            key_results[key_ix].coeff(local_index, PEAK_IDX) < MAX_CRAZY_PEAK_VAL) {
          m_key_counts[key_ix]++;
          int sum_index = key_ix * sum_flow_frame_stride;
          for (size_t frame_ix = 0; frame_ix < m_num_frames; frame_ix++) {
            for (size_t flow_ix = 0; flow_ix < m_num_flows; flow_ix++) {
              size_t z_ix = flow_ix * m_num_wells + local_index;
              key_flow_avg_sum[sum_index + flow_ix * m_num_frames + frame_ix] += key_signals[key_ix].coeff(z_ix, frame_ix);
            }
          }
        }
      }
      if (m_debug) {
        key_ix = max(0,key_ix);
        for (size_t flow_ix = 0; flow_ix < m_num_flows; flow_ix++) {
          size_t z_ix = flow_ix * m_num_wells + local_index;
          zeromer_est.row(z_ix).array()  = trace_data.row(z_ix).array() - key_signals[key_ix].row(z_ix).array();
          zeromer_est.row(z_ix).array() -= dark_matter.array(); // set
        }
        // for (size_t frame_ix = 0; frame_ix < m_num_frames; frame_ix++) {
        //   zeromer_est.col(frame_ix).array() = zeromer_est.col(frame_ix).array() + m_avg_0mer[frame_ix]; // add back the dark matter, should be zeros if not using
        // }
      }
    }
  }
  for (size_t key_ix = 0; key_ix < keys.size(); key_ix++) {
    if (m_key_counts[key_ix] > 0) {
      int sum_index = key_ix * sum_flow_frame_stride;
      int sum_index_end = sum_index + sum_flow_frame_stride;
      for (int i = sum_index; i < sum_index_end; i++) {
        m_flow_key_avg[i] = key_flow_avg_sum[i] / m_key_counts[key_ix];
      }
    }
  }
}


//
// Created by judith on 08.03.19.
//

#include "bitbots_localization/map.h"
#include <boost/filesystem.hpp>
#include <ros/package.h>

namespace fs = boost::filesystem;

Map::Map(const std::string& name, const std::string& type, const bl::LocalizationConfig &config) {
  // Set config
  config_ = config;
  //get package path
  std::string package_path = ros::package::getPath("bitbots_localization");
  //make boost path
  fs::path map_path = fs::path("config/fields") / fs::path(name) / fs::path(type);
  //convert to absolute path
  fs::path absolute_map_path = fs::absolute(map_path, package_path);
  //load map
  map = cv::imread(absolute_map_path.string(), cv::IMREAD_GRAYSCALE);
  if (!map.data) {
    ROS_ERROR("No image data '%s'", map_path.c_str());
  }
}

double Map::get_occupancy(double x, double y) {
  //get dimensions of field
  int mapWidth = map.cols;
  int mapHeight = map.rows;

  // m to pixel (=cm)
  x = x * 100;
  y = y * 100;

  // ursprung in feldmitte
  x = std::round(x + mapWidth / 2.0); //assuming lines are centered on map
  y = std::round(y + mapHeight / 2.0);

  double occupancy = -config_.measurement_out_of_map_punishment; // punish points outside the map

  if (x < mapWidth && x >= 0 && y < mapHeight && y >= 0) {
    occupancy = 100 - map.at<uchar>(y, x);
  }
  return occupancy;
}

std::vector<double> Map::provideRating(const RobotState &state,
                                       const std::vector<std::pair<double, double>> &observations) {
  std::vector<double> rating;
  for (const std::pair<double, double> &observation : observations) {
    //lines are in polar form!
    std::pair<double, double> lineRelative;

    // get rating per line
    lineRelative = observationRelative(observation, state.getXPos(), state.getYPos(), state.getTheta());
    double occupancy = get_occupancy(lineRelative.first, lineRelative.second);

    rating.push_back(occupancy);
  }
  return rating;
}

std::pair<double, double> Map::observationRelative(std::pair<double, double> observation,
                                                   double stateX,
                                                   double stateY,
                                                   double stateT) { // todo rename to a more correct name like observationonmap?
  // transformes observation from particle to map (assumes particle is correct)
  // input: obsservation relative in polar coordinates and particle
  // output: hypothetical observation on map

  // add theta and convert back to cartesian
  std::pair<double, double> observationWithTheta = polarToCartesian(observation.first + stateT, observation.second);

  // add to particle
  std::pair<double, double>
      observationRelative = std::make_pair(stateX + observationWithTheta.first, stateY + observationWithTheta.second);

  //alternativ:
  //Thrun 6.32, seite 169
  // but both equivalent
  //double xGlobal = stateX + observation.second * (cos(stateT + observation.first));
  //double yGlobal = stateY + observation.second * (sin(stateT + observation.first));

  //std::pair<double, double> observationRelative = std::make_pair(xGlobal, yGlobal);

  return observationRelative; // in cartesian

}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#include <boost/simd/arch/x86/tags.hpp>
#include <boost/simd/arch/common/limits.hpp>
#include <boost/simd/detail/brigand.hpp>

namespace boost { namespace simd
{
  template<> struct limits<boost::simd::avx_>
  {
    struct largest_integer
    {
      template<typename Sign> struct apply : boost::dispatch::make_integer<8,Sign> {};
    };

    struct smallest_integer
    {
      template<typename Sign> struct apply : boost::dispatch::make_integer<1,Sign> {};
    };

    using parent = boost::simd::sse2_;

    using smallest_real = float;
    using largest_real  = double;

    enum { bits = 256, bytes = 32 };
  };
} }

#endif


/*
 * Copyright (C) 2012 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#include <algorithm>
#include <fstream>
#include <sstream>
#include <memory>

#include <boost/algorithm/string.hpp>
#include <ignition/math/Helpers.hh>
#include <ignition/math/Vector3.hh>
#include <ignition/math/Matrix4.hh>

#include "gazebo/common/CommonIface.hh"
#include "gazebo/common/BVHLoader.hh"
#include "gazebo/common/SystemPaths.hh"
#include "gazebo/common/Skeleton.hh"
#include "gazebo/common/SkeletonAnimation.hh"
#include "gazebo/common/Console.hh"

using namespace gazebo;
using namespace common;

/////////////////////////////////////////////////
BVHLoader::BVHLoader()
{
}

/////////////////////////////////////////////////
BVHLoader::~BVHLoader()
{
}

/////////////////////////////////////////////////
Skeleton *BVHLoader::Load(const std::string &_filename, double _scale)
{
  std::string fullname = common::find_file(_filename);
  if (fullname.empty())
    return nullptr;

  std::unique_ptr<Skeleton> skeleton;
  std::ifstream file;
  file.open(fullname.c_str());
  std::vector<SkeletonNode*> nodes;
  std::vector<std::vector<std::string> > nodeChannels;
  unsigned int totalChannels = 0;
  std::string line;
  if (file.is_open())
  {
    getline(file, line);
    if (line.find("HIERARCHY") == std::string::npos)
    {
      file.close();
      return nullptr;
    }

    SkeletonNode *parent = nullptr;
    SkeletonNode *node = nullptr;
    while (!file.eof())
    {
      getline(file, line);
      std::vector<std::string> words;
      boost::trim(line);
      boost::split(words, line, boost::is_any_of(" \t"));
      if (words[0] == "ROOT" || words[0] == "JOINT")
      {
        if (words.size() < 2)
        {
          file.close();
          return nullptr;
        }
        SkeletonNode::SkeletonNodeType type = SkeletonNode::JOINT;
        std::string name = words[1];
        node = new SkeletonNode(parent, name, name, type);
        if (words[0] != "End")
          nodes.push_back(node);
      }
      else
        if (words[0] == "OFFSET")
        {
          if (words.size() < 4)
          {
            file.close();
            return nullptr;
          }
          ignition::math::Vector3d offset = ignition::math::Vector3d(
              ignition::math::parseFloat(words[1]) * _scale,
              ignition::math::parseFloat(words[2]) * _scale,
              ignition::math::parseFloat(words[3]) * _scale);
          ignition::math::Matrix4d transform(
              ignition::math::Matrix4d::Identity);
          transform.SetTranslation(offset);
          node->SetTransform(transform);
        }
        else
          if (words[0] == "CHANNELS")
          {
            if (words.size() < 3 ||
                static_cast<size_t>(ignition::math::parseInt(words[1]) + 2) >
                 words.size())
            {
              file.close();
              return nullptr;
            }
            nodeChannels.push_back(words);
            totalChannels += ignition::math::parseInt(words[1]);
          }
          else
            if (words[0] == "{")
              parent = node;
            else
              if (words[0] == "}")
                parent = parent->GetParent();
              else
                if (words.size() == 2 && words[0] == "End"
                        && words[1] == "Site")
                {
                  /// ignore End Sites
                  getline(file, line);  /// read {
                  getline(file, line);  /// read OFFSET
                  getline(file, line);  /// read }
                }
                else
                {
                  if (nodes.empty())
                  {
                    file.close();
                    return nullptr;
                  }
                  skeleton.reset(new Skeleton(nodes[0]));
                  break;
                }
    }
  }
  getline(file, line);
  std::vector<std::string> words;
  boost::trim(line);
  boost::split(words, line, boost::is_any_of(" \t"));
  unsigned int frameCount = 0;
  double frameTime = 0.0;
  if (words[0] != "Frames:" || words.size() < 2)
  {
    file.close();
    return nullptr;
  }
  else
    frameCount = ignition::math::parseInt(words[1]);

  getline(file, line);
  words.clear();
  boost::trim(line);
  boost::split(words, line, boost::is_any_of(" \t"));

  if (words.size() < 3 || words[0] != "Frame" || words[1] != "Time:")
  {
    file.close();
    return nullptr;
  }
  else
    frameTime = ignition::math::parseFloat(words[2]);

  double time = 0.0;
  unsigned int frameNo = 0;

  SkeletonAnimation *animation = new SkeletonAnimation(_filename);

  while (!file.eof())
  {
    getline(file, line);
    words.clear();
    boost::trim(line);
    boost::split(words, line, boost::is_any_of(" \t"));
    if (words.size() < totalChannels)
    {
      gzwarn << "Frame " << frameNo << " invalid.\n";
      frameNo++;
      time += frameTime;
      continue;
    }

    unsigned int cursor = 0;
    for (unsigned int i = 0; i < nodes.size(); ++i)
    {
      SkeletonNode *node = nodes[i];
      std::vector<std::string> channels = nodeChannels[i];
      ignition::math::Vector3d translation = node->Transform().Translation();
      ignition::math::Vector3d xAxis(1, 0, 0);
      ignition::math::Vector3d yAxis(0, 1, 0);
      ignition::math::Vector3d zAxis(0, 0, 1);
      double xAngle = 0.0;
      double yAngle = 0.0;
      double zAngle = 0.0;
      ignition::math::Matrix4d transform(ignition::math::Matrix4d::Identity);
      std::vector<ignition::math::Matrix4d> mats;
      unsigned int chanCount = ignition::math::parseInt(channels[1]);
      for (unsigned int j = 2; j < (2 + chanCount); ++j)
      {
        double value = ignition::math::parseFloat(words[cursor]);
        cursor++;
        std::string channel = channels[j];
        if (channel == "Xposition")
          translation.X(value * _scale);
        else
          if (channel == "Yposition")
            translation.Y(value * _scale);
          else
          {
            if (channel == "Zposition")
            {
              translation.Z(value * _scale);
            }
            else
            {
              if (channel == "Zrotation")
              {
                zAngle = IGN_DTOR(value);
                mats.push_back(ignition::math::Matrix4d(
                      ignition::math::Quaterniond(zAxis, zAngle)));
              }
              else
              {
                if (channel == "Xrotation")
                {
                  xAngle = IGN_DTOR(value);
                  mats.push_back(ignition::math::Matrix4d(
                    ignition::math::Quaterniond(xAxis, xAngle)));
                }
                else
                {
                  if (channel == "Yrotation")
                  {
                    yAngle = IGN_DTOR(value);
                    mats.push_back(ignition::math::Matrix4d(
                      ignition::math::Quaterniond(yAxis, yAngle)));
                  }
                }
              }
            }
          }
      }
      while (!mats.empty())
      {
        transform = mats.back() * transform;
        mats.pop_back();
      }
      ignition::math::Matrix4d pos(ignition::math::Matrix4d::Identity);
      pos.SetTranslation(translation);
      transform = pos * transform;
      animation->AddKeyFrame(node->GetName(), time, transform);
    }

    frameNo++;
    time += frameTime;
    if (frameNo == frameCount)
      break;
  }
  if (frameNo < frameCount - 1)
    gzwarn << "BVH file ended unexpectedly.\n";

  skeleton->AddAnimation(animation);

  file.close();
  return skeleton.release();
}


/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  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.
 *
 * 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 <numeric>
#include <sstream>

#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>

#include "dbglog/dbglog.hpp"

#include "utility/binaryio.hpp"
#include "utility/streams.hpp"

#include "math/math.hpp"

#include "../vts/tileset/driver.hpp"
#include "../vts/opencv/atlas.hpp"
#include "../vts/tileset/config.hpp"

#include "tmptileset.hpp"

namespace fs = boost::filesystem;
namespace bin = utility::binaryio;

namespace vtslibs { namespace vts { namespace tools {

namespace {

// mesh proper
const char MAGIC[2] = { 'S', 'M' };
const std::uint16_t VERSION_ORIGINAL = 1;
const std::uint16_t VERSION_ZINDEX = 2;
const std::uint16_t VERSION = VERSION_ZINDEX;

bool isShort(std::size_t size) {
    return size <= std::numeric_limits<std::uint16_t>::max();
}

void saveSimpleMesh(std::ostream &out, const Mesh &mesh)
{
    // helper functions
    auto saveVertexComponent([&out](double v, double o, double s) -> void
    {
        bin::write
            (out, std::uint32_t
             (std::round
              (((v - o) * std::numeric_limits<std::uint32_t>::max()) / s)));
    });

    auto saveTexCoord([&out](double v)
    {
        v = std::round(math::clamp(v, 0.0, 1.0)
                       * std::numeric_limits<std::uint32_t>::max());
        bin::write(out, std::uint32_t(v));
    });

    // write header
    bin::write(out, MAGIC);
    bin::write(out, std::uint16_t(VERSION));

    bin::write(out, std::uint16_t(mesh.submeshes.size()));

    // write submeshes
    for (const auto &sm : mesh) {
        // compute extents
        const auto bbox(extents(sm));
        const math::Point3d bbsize(bbox.ur - bbox.ll);

        // write extents
        bin::write(out, bbox.ll(0));
        bin::write(out, bbox.ll(1));
        bin::write(out, bbox.ll(2));
        bin::write(out, bbox.ur(0));
        bin::write(out, bbox.ur(1));
        bin::write(out, bbox.ur(2));

        // write vertices
        bin::write(out, std::uint32_t(sm.vertices.size()));
        const bool shortVertices(isShort(sm.vertices.size()));
        for (const auto &vertex : sm.vertices) {
            saveVertexComponent(vertex(0), bbox.ll(0), bbsize(0));
            saveVertexComponent(vertex(1), bbox.ll(1), bbsize(1));
            saveVertexComponent(vertex(2), bbox.ll(2), bbsize(2));
        }

        // write tc
        bin::write(out, std::uint32_t(sm.tc.size()));
        const bool shortTc(isShort(sm.tc.size()));
        for (const auto &tc : sm.tc) {
            saveTexCoord(tc(0));
            saveTexCoord(tc(1));
        }

        // save faces
        bin::write(out, std::uint32_t(sm.faces.size()));

        auto ifacesTc(sm.facesTc.begin());
        for (auto &face : sm.faces) {
            // face
            if (shortVertices) {
                bin::write(out, std::uint16_t(face(0)));
                bin::write(out, std::uint16_t(face(1)));
                bin::write(out, std::uint16_t(face(2)));
            } else {
                bin::write(out, std::uint32_t(face(0)));
                bin::write(out, std::uint32_t(face(1)));
                bin::write(out, std::uint32_t(face(2)));
            }

            // tc face
            if (shortTc) {
                bin::write(out, std::uint16_t((*ifacesTc)(0)));
                bin::write(out, std::uint16_t((*ifacesTc)(1)));
                bin::write(out, std::uint16_t((*ifacesTc)(2)));
            } else {
                bin::write(out, std::uint32_t((*ifacesTc)(0)));
                bin::write(out, std::uint32_t((*ifacesTc)(1)));
                bin::write(out, std::uint32_t((*ifacesTc)(2)));
            }

            ++ifacesTc;
        }

        // write zIndex
        bin::write(out, std::uint32_t(sm.zIndex));
    }
}

Mesh loadSimpleMesh(std::istream &in, const fs::path &path)
{
    // helper functions
    auto loadVertexComponent([&in](double o, double s) -> double
    {
        std::uint32_t v;
        bin::read(in, v);
        return o + ((v * s) / std::numeric_limits<std::uint32_t>::max());
    });

    auto loadTexCoord([&in]() -> double
    {
        std::uint32_t v;
        bin::read(in, v);
        return (double(v) / std::numeric_limits<std::uint32_t>::max());
    });

    // Load mesh headers first
    char magic[sizeof(MAGIC)];
    std::uint16_t version;

    bin::read(in, magic);
    bin::read(in, version);

    LOG(info1) << "Mesh version: " << version;

    if (std::memcmp(magic, MAGIC, sizeof(MAGIC))) {
        LOGTHROW(err1, storage::BadFileFormat)
            << "File " << path << " is not a VTS simplemesh file.";
    }
    if (version > VERSION) {
        LOGTHROW(err1, storage::VersionError)
            << "File " << path
            << " has unsupported version (" << version << ").";
    }

    auto versionedSize([&]() -> std::uint32_t
    {
        if (version > 0) {
            return bin::read<std::uint32_t>(in);
        }

        return bin::read<std::uint16_t>(in);
    });

    std::uint16_t subMeshCount;
    bin::read(in, subMeshCount);

    Mesh mesh;

    mesh.submeshes.resize(subMeshCount);
    for (auto &sm : mesh) {
        // load sub-mesh bounding box
        math::Extents3 bbox;
        bin::read(in, bbox.ll(0));
        bin::read(in, bbox.ll(1));
        bin::read(in, bbox.ll(2));
        bin::read(in, bbox.ur(0));
        bin::read(in, bbox.ur(1));
        bin::read(in, bbox.ur(2));

        const math::Point3d bbsize(bbox.ur - bbox.ll);

        // load vertices
        auto vertexCount(versionedSize());
        const bool shortVertices(isShort(vertexCount));
        sm.vertices.resize(vertexCount);
        for (auto &vertex : sm.vertices) {
            vertex(0) = loadVertexComponent(bbox.ll(0), bbsize(0));
            vertex(1) = loadVertexComponent(bbox.ll(1), bbsize(1));
            vertex(2) = loadVertexComponent(bbox.ll(2), bbsize(2));
        }

        // load tc
        auto tcCount(versionedSize());
        const bool shortTc(isShort(tcCount));
        sm.tc.resize(tcCount);
        for (auto &tc : sm.tc) {
            tc(0) = loadTexCoord();
            tc(1) = loadTexCoord();
        }

        // load faces
        std::uint32_t faceCount(versionedSize());
        sm.faces.resize(faceCount);
        sm.facesTc.resize(faceCount);
        auto ifacesTc(sm.facesTc.begin());

        for (auto &face : sm.faces) {
            if (shortVertices) {
                face(0) = bin::read<std::uint16_t>(in);
                face(1) = bin::read<std::uint16_t>(in);
                face(2) = bin::read<std::uint16_t>(in);
            } else {
                face(0) = bin::read<std::uint32_t>(in);
                face(1) = bin::read<std::uint32_t>(in);
                face(2) = bin::read<std::uint32_t>(in);
            }

            if (shortTc) {
                (*ifacesTc)(0) = bin::read<std::uint16_t>(in);
                (*ifacesTc)(1) = bin::read<std::uint16_t>(in);
                (*ifacesTc)(2) = bin::read<std::uint16_t>(in);
            } else {
                (*ifacesTc)(0) = bin::read<std::uint32_t>(in);
                (*ifacesTc)(1) = bin::read<std::uint32_t>(in);
                (*ifacesTc)(2) = bin::read<std::uint32_t>(in);
            }

            ++ifacesTc;
        }

        if (version >= VERSION_ZINDEX) {
            sm.zIndex = bin::read<std::uint32_t>(in);
        }
    }

    return mesh;
}

} // namespace

class TmpTileset::Slice {
public:
    typedef std::shared_ptr<Slice> pointer;
    struct OpenTag {};

    Slice(const fs::path &root)
        : driver_(Driver::create(root, driver::PlainOptions(5), {}))
    {}

    Slice(const fs::path &root, const OpenTag&)
        : driver_(Driver::open(root, Driver::BareConfigTag{}, {}))
    {
        index_.load(driver_->input(File::tileIndex)->get());
    }

    bool hasTile(const TileId &tileId) const {
        return index_.get(tileId);
    }

    TileIndex::Flag::value_type getTile(const TileId &tileId) const {
        return index_.get(tileId);
    }

    void setTile(const TileId &tileId
                 , const TileIndex::Flag::value_type extraFlags)
    {
        index_.set(tileId
                   , (extraFlags | TileIndex::Flag::mesh
                      | TileIndex::Flag::atlas));
    }

    Driver::pointer driver() { return driver_; }

    Driver::pointer driver() const { return driver_; }

    const TileIndex& index() { return index_; }

    void flush() {
        {
            auto f(driver_->output(File::tileIndex));
            index_.save(f->get());
            f->close();
        }
        {
            auto f(driver_->output(File::config));
            vts::tileset::saveDriver(f->get(), driver_->options());
            f->close();
        }
        driver_->flush();
    }

    void saveMesh(const TileId &tileId, const Mesh &mesh);
    void saveAtlas(const TileId &tileId, const Atlas &atlas);

    IStream::pointer input(const TileId &tileId, TileFile type) {
        std::unique_lock<std::mutex> lock(mutex_);
        return driver_->input(tileId, type);
    }

private:
    std::mutex mutex_;
    TileIndex index_;
    Driver::pointer driver_;
};

void TmpTileset::Slice::saveMesh(const TileId &tileId, const Mesh &mesh)
{
    std::stringstream tmp;
    saveSimpleMesh(tmp, mesh);

    {
        std::unique_lock<std::mutex> lock(mutex_);
        auto os(driver_->output(tileId, storage::TileFile::mesh));
        copyFile(tmp, os);
        os->close();
    }
}

void TmpTileset::Slice::saveAtlas(const TileId &tileId, const Atlas &atlas)
{
    std::stringstream tmp;
    atlas.serialize(tmp);
    {
        std::unique_lock<std::mutex> lock(mutex_);
        auto os(driver_->output(tileId, storage::TileFile::atlas));
        copyFile(tmp, os);
        os->close();
    }
}

TmpTileset::TmpTileset(const boost::filesystem::path &root
                       , bool create)
    : root_(root), keep_(false)
{
    if (create) {
        // make room for tilesets
        fs::remove_all(root_);
        // create root for tilesets
        fs::create_directories(root_);
        return;
    }

    for (int i(0);; ++i) {
        auto path(root_ / boost::lexical_cast<std::string>(i));
        if (!exists(path)) { break; }
        slices_.push_back(std::make_shared<Slice>(path, Slice::OpenTag{}));
    }

    if (slices_.empty()) {
        LOGTHROW(err1, std::runtime_error)
            << "No tileset slice found in temporary tileset " << root_ << ".";
    }
}

TmpTileset::~TmpTileset()
{
    // cleanup
    if (!keep_) {
        fs::remove_all(root_);
    }
}

void TmpTileset::store(const TileId &tileId, const Mesh &mesh
                       , const Atlas &atlas
                       , const TileIndex::Flag::value_type extraFlags)
{
    LOG(debug)
        << tileId << " Storing mesh with "
        << std::accumulate(mesh.begin(), mesh.end(), std::size_t(0)
                           , [](std::size_t v, const SubMesh &sm) {
                               return v + sm.faces.size();
                           })
        << " faces.";

    // get driver for tile
    auto slice([&]() -> Slice::pointer
    {
        std::unique_lock<std::mutex> lock(mutex_);
        for (auto &slice : slices_) {
            // TODO: make get/set in one pass
            if (!slice->hasTile(tileId)) {
                slice->setTile(tileId, extraFlags);
                return slice;
            }
        }

        // no available slice for this tile, create new
        // path
        auto path(root_ / boost::lexical_cast<std::string>(slices_.size()));
        LOG(info3) << "Creating temporary tileset at " << path << ".";
        slices_.push_back(std::make_shared<Slice>(path));
        auto &slice(slices_.back());
        slice->setTile(tileId, extraFlags);
        return slice;
    }());

    slice->saveMesh(tileId, mesh);
    slice->saveAtlas(tileId, atlas);
}

TmpTileset::Tile
TmpTileset::load(const TileId &tileId, int quality) const
{
    Tile tile;
    auto &mesh(std::get<0>(tile));
    auto &atlas(std::get<1>(tile));
    auto &flags(std::get<2>(tile));

    flags = 0;

    for (const auto &slice : slices_) {
        auto sliceFlags(slice->getTile(tileId));
        if (!sliceFlags) { continue; }

        // remember flags
        flags |= sliceFlags;

        auto driver(slice->driver());

        auto is(slice->input(tileId, storage::TileFile::mesh));
        Mesh m(loadSimpleMesh(is->get(), is->name()));

        opencv::HybridAtlas a(quality);
        {
            auto is(slice->input(tileId, storage::TileFile::atlas));
            a.deserialize(is->get(), is->name());
        }

        if (!mesh) {
            mesh = std::make_shared<Mesh>(m);
        } else {
            mesh->submeshes.insert(mesh->submeshes.end(), m.submeshes.begin()
                                   , m.submeshes.end());
        }

        if (!atlas) {
            atlas = std::make_shared<opencv::HybridAtlas>(a);
        } else {
            atlas->append(a);
        }
    }

    return tile;
}

void TmpTileset::flush()
{
    // unlocked!
    for (const auto &slice : slices_) {
        slice->flush();
    }
}

TileIndex TmpTileset::tileIndex() const
{
    TileIndex ti;
    for (const auto &slice : slices_) {
        ti = unite(ti, slice->index());
    }
    return ti;
}

} } } // namespace vtslibs::vts::tools