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

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

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

#include <boost/simd/detail/assert_utils.hpp>
#include <boost/simd/function/is_nltz.hpp>
#include <boost/simd/function/sqrt.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/assert.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( realsqrt_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG(boost::simd::assert_all(is_nltz(a0)),
                       "realsqrt cannot produce complex result.");
      return sqrt(a0);
    }
  };
} } }

#endif


#include <boost/test/unit_test.hpp>
#include "acc_data_input.hpp"
#include "expected_prediction.h"
#include "../src/svm_impl/classifiers_generator.h"
#include "../src/svm_impl/svm_model.h"

namespace
{

// arguments for the training
const constexpr auto coat_value = 32.0;
const constexpr auto gamma_value = 0.0078125;

}   // end of namespace

BOOST_AUTO_TEST_CASE(test_svm_acc_model_classifier)
{
    using namespace mlmodels;
    // we would test if we are generating the same results as
    // we are getting from running libsvm tools - please note that the
    // parameters above as well as the expected results where generated by using
    // tools from libsvm which includes running
    // ./svm-scale -l -1 -u 1 -s range1 train_input > train_input.scale
    // ./svm-scale -r range1 test_input > test_input.scale
    // tools/grid.py train_input.scale
    // ./svm-train -c 32 -g 0.0078125 train_input.scale
    // ./svm-predict test_input.scale train_input.scale.model prediction
    // to generate the data input we generate 400 lines for input for train and 283 lines of test
    // from the file dataset_libsvm in this directory
    auto model = svm::classifier::create_model(svm::classifier::c_rbf_train{
                                                        svm::classifier::c_rbf_train::base_1
                                                            {
                                                                svm::rbf_args{gamma_value},
                                                                svm::base_train_params{}
                                                        },
                                                        svm::classifier::c_rbf_train::base_2{
                                                            coat_value, 0, 0
                                                        }
                                                },
                                                train_features, acc_train_labels
                                            );
    BOOST_TEST_REQUIRE((model.get() != nullptr), "failed to generate C SVM model, with train data");
    // run run prediction
    auto predictions = svm::predict(model, test_features);
    BOOST_TEST_REQUIRE(not predictions.empty(), "failed to get predictions!!");
    BOOST_TEST_REQUIRE(predictions.size() == expected_results_prediction.size(), "number of predictions do not match expetection: "
            <<predictions.size()<<" != "<<expected_results_prediction.size());
    // now compare results
    auto expted_begin = std::begin(expected_results_prediction);
    unsigned int at = 0;
    for (auto our_results : predictions) {
        BOOST_TEST(our_results == *expted_begin, "["<<at<<"] our results "<<our_results<<" do not match expected results "<<*expted_begin);
        ++expted_begin;
        ++at;
    }
//    svm::save(model, "/tmp/ut_csvm_rbf.model");
}


// Copyright (C) 2014 The Regents of the University of California (Regents).
// All rights reserved.
//
// 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.
//
//     * Neither the name of The Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/filter_view_graph_cycles_by_rotation.h"

#include <Eigen/Core>
#include <ceres/rotation.h>
#include <glog/logging.h>
#include <unordered_map>
#include <vector>

#include "theia/math/graph/triplet_extractor.h"
#include "theia/math/util.h"
#include "theia/sfm/twoview_info.h"
#include "theia/sfm/types.h"
#include "theia/sfm/view_graph/view_graph.h"
#include "theia/sfm/view_triplet.h"
#include "theia/util/hash.h"

namespace theia {
namespace {

double ComputeLoopRotationError(const TwoViewInfo& info_one_two,
                                const TwoViewInfo& info_one_three,
                                const TwoViewInfo& info_two_three) {
  // Get relative rotation matrices.
  Eigen::Matrix3d rotation1_2, rotation1_3, rotation2_3;
  ceres::AngleAxisToRotationMatrix(
      info_one_two.rotation_2.data(),
      ceres::ColumnMajorAdapter3x3(rotation1_2.data()));
  ceres::AngleAxisToRotationMatrix(
      info_one_three.rotation_2.data(),
      ceres::ColumnMajorAdapter3x3(rotation1_3.data()));
  ceres::AngleAxisToRotationMatrix(
      info_two_three.rotation_2.data(),
      ceres::ColumnMajorAdapter3x3(rotation2_3.data()));

  // Compute loop rotation.
  const Eigen::Matrix3d loop_rotation =
      rotation2_3 * rotation1_2 * rotation1_3.transpose();
  Eigen::Vector3d loop_rotation_angle_axis;
  ceres::RotationMatrixToAngleAxis(
      ceres::ColumnMajorAdapter3x3(loop_rotation.data()),
      loop_rotation_angle_axis.data());

  // Return the angle of the loop rotation which is the error of the
  // concatenated triplet rotation.
  return RadToDeg(loop_rotation_angle_axis.norm());
}

void RemoveTripletEdgesFromInvalidViewPairs(
    const ViewIdTriplet& triplet,
    std::unordered_set<ViewIdPair>* invalid_view_pairs) {
  const ViewIdPair pair_01(std::get<0>(triplet), std::get<1>(triplet));
  const ViewIdPair pair_02(std::get<0>(triplet), std::get<2>(triplet));
  const ViewIdPair pair_12(std::get<1>(triplet), std::get<2>(triplet));
  invalid_view_pairs->erase(pair_01);
  invalid_view_pairs->erase(pair_02);
  invalid_view_pairs->erase(pair_12);
}

}  // namespace

void FilterViewGraphCyclesByRotation(const double max_loop_error_degrees,
                                     ViewGraph* view_graph) {
  const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs =
      view_graph->GetAllEdges();

  // Initialize a list of invalid view pairs to all view pairs. View pairs
  // deemed valid will be removed from this list.
  std::unordered_set<ViewIdPair> invalid_view_pairs;
  invalid_view_pairs.reserve(view_pairs.size());
  for (const auto& view_pair : view_pairs) {
    invalid_view_pairs.insert(view_pair.first);
  }

  // Find all triplets.
  TripletExtractor<ViewId> triplet_extractor;
  std::vector<std::vector<ViewIdTriplet> > connected_triplets;
  CHECK(triplet_extractor.ExtractTriplets(invalid_view_pairs,
                                          &connected_triplets))
      << "Could not extract triplets from view pairs.";

  // Examine the cycles of size 3 to determine invalid view pairs from the
  // rotations.
  for (const auto& triplets : connected_triplets) {
    for (const ViewIdTriplet& triplet : triplets) {
      const TwoViewInfo& info_one_two =
          *view_graph->GetEdge(std::get<0>(triplet), std::get<1>(triplet));
      const TwoViewInfo& info_one_three =
          *view_graph->GetEdge(std::get<0>(triplet), std::get<2>(triplet));
      const TwoViewInfo& info_two_three =
          *view_graph->GetEdge(std::get<1>(triplet), std::get<2>(triplet));

      // Compute loop rotation error.
      const double loop_rotation_error_degrees = ComputeLoopRotationError(
          info_one_two, info_one_three, info_two_three);

      // Add the view pairs to the list of valid view pairs if the loop error is
      // within the designated tolerance.
      if (loop_rotation_error_degrees < max_loop_error_degrees) {
        RemoveTripletEdgesFromInvalidViewPairs(triplet, &invalid_view_pairs);
      }
    }
  }

  VLOG(1) << "Removing " << invalid_view_pairs.size() << " of "
          << view_pairs.size() << " view pairs from loop rotation filtering.";

  // Remove any view pairs not in the list of valid edges.
  for (const ViewIdPair& view_pair_id : invalid_view_pairs) {
    view_graph->RemoveEdge(view_pair_id.first, view_pair_id.second);
  }
}

}  // namespace theia


//=======================================================================
// Copyright (c) 2013 Piotr Wygocki, Piotr Godlewski
//
// 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)
//=======================================================================
/**
 * @file glp.hpp
 * @brief
 * @author Piotr Godlewski, Piotr Wygocki
 * @version 2.0
 * @date 2014-05-19
 */
#ifndef PAAL_GLP_HPP
#define PAAL_GLP_HPP

#include "paal/data_structures/bimap.hpp"
#include "paal/lp/constraints.hpp"
#include "paal/lp/ids.hpp"
#include "paal/lp/lp_base.hpp"
#include "paal/lp/problem_type.hpp"
#include "paal/utils/irange.hpp"

#include <boost/range/iterator_range.hpp>
#include <boost/range/adaptor/indexed.hpp>

#include <glpk.h>

namespace paal {
namespace lp {

namespace detail {

/**
 * @class glp_impl
 * @brief LP implementation using GLPK.
 */
class glp_impl {
    using Ids = std::vector<int>;
    using Vals = std::vector<double>;
    using IdsVals = std::vector<std::pair<row_id, double>>;
    using RowsInColumnIterator = IdsVals::const_iterator;

  public:
    /**
     * Constructor.
     */
    glp_impl()
        : m_lp(glp_create_prob()), m_total_col_nr(0), m_total_row_nr(0),
          m_idx_cols_tmp(1), m_idx_rows_tmp(1), m_val_cols_tmp(1),
          m_val_rows_tmp(1), m_rows_tmp(1) {
        glp_term_out(GLP_OFF);
        glp_init_smcp(&m_glpk_control);
        glp_load_matrix(m_lp, 0, NULL, NULL, NULL);
        m_glpk_control.msg_lev = GLP_MSG_OFF;
    }

    /**
     * Destructor.
     */
    ~glp_impl() { glp_delete_prob(m_lp); }

    /**
     * Frees GLPK resources, common for all LP instances.
     * Should be called after all LP instances are destructed.
     */
    static void free_env() { glp_free_env(); }

    /**
     * Sets the problem optimization type (min/max).
     */
    void set_optimization_type(optimization_type opt_type) {
        glp_set_obj_dir(m_lp, opt_type == MINIMIZE ? GLP_MIN : GLP_MAX);
    }

    /**
     * Adds a new column to the LP.
     *
     * @param cost_coef coefficient of the column in the objective function
     * @param lb column lower bound value
     * @param ub column upper bound value
     *
     * @return column identifier
     */
    col_id add_column(double cost_coef, double lb, double ub) {
        int colNr = glp_add_cols(m_lp, 1);

        glp_set_col_bnds(m_lp, colNr, bounds_to_glp_type(lb, ub), lb, ub);
        glp_set_obj_coef(m_lp, colNr, cost_coef);
        glp_set_mat_col(m_lp, colNr, 0, NULL, NULL);

        resize_col_tmp();
        ++m_total_col_nr;
        m_col_idx.add(m_total_col_nr - 1);
        return col_id(m_total_col_nr - 1);
    }

    /**
     * Adds a new row to the LP.
     *
     * @param constraint constraint being added
     *
     * @return row identifier
     */
    row_id add_row(const double_bounded_expression &constraint) {
        int rowNr = glp_add_rows(m_lp, 1);
        auto lb = constraint.get_lower_bound();
        auto ub = constraint.get_upper_bound();
        glp_set_row_bnds(m_lp, rowNr, bounds_to_glp_type(lb, ub), lb, ub);

        resize_row_tmp();
        ++m_total_row_nr;
        m_row_idx.add(m_total_row_nr - 1);
        auto row = row_id(m_total_row_nr - 1);

        set_row_expression(row, constraint.get_expression());

        return row;
    }

    /**
     * Sets the lower bound of an existing LP column.
     *
     * @param col column identifier
     * @param lb row lower bound value
     */
    void set_col_lower_bound(col_id col, double lb) {
        auto ub = get_col_upper_bound(col);
        glp_set_col_bnds(m_lp, get_col(col), bounds_to_glp_type(lb, ub), lb,
                         ub);
    }

    /**
     * Sets the upper bound of an existing LP column.
     *
     * @param col column identifier
     * @param ub row upper bound value
     */
    void set_col_upper_bound(col_id col, double ub) {
        auto lb = get_col_lower_bound(col);
        glp_set_col_bnds(m_lp, get_col(col), bounds_to_glp_type(lb, ub), lb,
                         ub);
    }

    /**
     * Sets the cost coefficient of an existing LP column.
     *
     * @param col column identifier
     * @param cost_coef new cost coefficient
     */
    void set_col_cost(col_id col, double cost_coef) {
        glp_set_obj_coef(m_lp, get_col(col), cost_coef);
    }

    /**
     * Sets the lower bound of an existing LP row.
     *
     * @param row row identifier
     * @param lb row lower bound value
     */
    void set_row_lower_bound(row_id row, double lb) {
        auto ub = get_row_upper_bound(row);
        glp_set_row_bnds(m_lp, get_row(row), bounds_to_glp_type(lb, ub), lb,
                         ub);
    }

    /**
     * Sets the upper bound of an existing LP row.
     *
     * @param row row identifier
     * @param ub row upper bound value
     */
    void set_row_upper_bound(row_id row, double ub) {
        auto lb = get_row_lower_bound(row);
        glp_set_row_bnds(m_lp, get_row(row), bounds_to_glp_type(lb, ub), lb,
                         ub);
    }

    /**
     * Sets the expression of an existing row.
     *
     * @param row row identifier
     * @param expr new expression
     */
    void set_row_expression(row_id row, const linear_expression &expr) {
        auto elems = expr.get_elements();
        int expr_size = boost::distance(elems);

        for (auto elem : elems | boost::adaptors::indexed(1)) {
            m_idx_cols_tmp[elem.index()] = get_col(elem.value().first);
            m_val_cols_tmp[elem.index()] = elem.value().second;
        }

        glp_set_mat_row(m_lp, get_row(row), expr_size, &m_idx_cols_tmp[0],
                        &m_val_cols_tmp[0]);
    }

    /**
     * Removes a column form the LP.
     *
     * @param col ID of the column to be removed
     */
    void delete_col(col_id col) {
        int arr[2];
        arr[1] = get_col(col);
        m_col_idx.erase(col.get());
        glp_del_cols(m_lp, 1, arr);
    }

    /**
     * Removes a row form the LP.
     *
     * @param row ID of the row to be removed
     */
    void delete_row(row_id row) {
        int arr[2];
        arr[1] = get_row(row);
        m_row_idx.erase(row.get());
        glp_del_rows(m_lp, 1, arr);
    }

    /**
     * Solves the LP using the primal simplex method.
     *
     * @param type simplex type (primal / dual)
     *
     * @return solution status
     */
    problem_type solve_simplex(simplex_type type = PRIMAL) {
        return run_simplex(type, false);
    }

    /**
     * Resolves the LP (starting from the previously found solution)
     * using the primal simplex method.
     *
     * @param type simplex type (primal / dual)
     *
     * @return solution status
     */
    problem_type resolve_simplex(simplex_type type = PRIMAL) {
        return run_simplex(type, true);
    }

    /**
     * Returns the found objective function value.
     * Should be called only after the LP has been solved and if it
     * wasn't modified afterwards.
     */
    double get_obj_value() const { return glp_get_obj_val(m_lp); }

    /**
     * Returns column primal value.
     * Should be called only after the LP has been solved and if it
     * wasn't modified afterwards.
     */
    double get_col_value(col_id col) const {
        return glp_get_col_prim(m_lp, get_col(col));
    }

    /**
     * Returns the column cost function coefficient.
     */
    double get_col_coef(col_id col) const {
        return glp_get_obj_coef(m_lp, get_col(col));
    }

    /**
     * Returns the column lower bound.
     */
    double get_col_lower_bound(col_id col) const {
        return get_col_bounds(col).first;
    }

    /**
     * Returns the column upper bound.
     */
    double get_col_upper_bound(col_id col) const {
        return get_col_bounds(col).second;
    }

    /**
     * Returns row dual value.
     * Should be called only after the LP has been solved and if it
     * wasn't modified afterwards.
     */
    double get_row_dual_value(row_id row) const {
       return glp_get_row_dual(m_lp, get_row(row));
    }

    /**
     * Returns the row lower bound.
     */
    double get_row_lower_bound(row_id row) const {
        return get_row_bounds(row).first;
    }

    /**
     * Returns the row upper bound.
     */
    double get_row_upper_bound(row_id row) const {
        return get_row_bounds(row).second;
    }

    /**
     * Returns the expression of an existing row.
     */
    linear_expression get_row_expression(row_id row) const {
        linear_expression exp;
        int size = glp_get_mat_row(m_lp, get_row(row), &m_idx_cols_tmp[0],
                                   &m_val_cols_tmp[0]);
        for (auto i : irange(1, size + 1)) {
            exp += m_val_cols_tmp[i] * get_col_id(m_idx_cols_tmp[i]);
        }
        return exp;
    }

    /**
     * Returns the identifiers and coefficients of all rows in a given column,
     * which constraint matrix coefficient is non-zero (as an iterator range).
     */
    boost::iterator_range<RowsInColumnIterator>
    get_rows_in_column(col_id col) const {
        int size = glp_get_mat_col(m_lp, get_col(col), &m_idx_rows_tmp[0],
                                   &m_val_rows_tmp[0]);
        for (auto i : irange(1, size + 1)) {
            m_rows_tmp[i].first = get_row_id(m_idx_rows_tmp[i]);
            m_rows_tmp[i].second = m_val_rows_tmp[i];
        }
        return boost::make_iterator_range(m_rows_tmp.begin() + 1,
                                          m_rows_tmp.begin() + size + 1);
    }

  private:
    glp_impl(glp_impl &&) {}
    glp_impl(const glp_impl &) {}

    /**
     * Determines the GLPK bound type based on the lower and upper bound values.
     */
    static int bounds_to_glp_type(double lb, double ub) {
        if (lb == ub) {
            return GLP_FX;
        } else if (lb == lp_traits::MINUS_INF) {
            if (ub == lp_traits::PLUS_INF) {
                return GLP_FR;
            } else {
                return GLP_UP;
            }
        } else {
            if (ub == lp_traits::PLUS_INF) {
                return GLP_LO;
            } else {
                return GLP_DB;
            }
        }
    }

    /**
     * Determines the lower and upper bound values based on the GLPK bound type
     * and GLPK returned values.
     */
    static std::pair<double, double> glp_type_to_bounds(double lb, double ub,
                                                        int type) {
        switch (type) {
        case GLP_FX:
            return { lb, lb };
        case GLP_FR:
            return { lp_traits::MINUS_INF, lp_traits::PLUS_INF };
        case GLP_UP:
            return { lp_traits::MINUS_INF, ub };
        case GLP_LO:
            return { lb, lp_traits::PLUS_INF };
        case GLP_DB:
            return { lb, ub };
        default:
            assert(false);
            return { 0, 0 };
        }
    }

    /**
     * Determines the GLPK simplex type based on the paal::lp simplex type.
     */
    static int simplex_type_to_glp(simplex_type type) {
        switch (type) {
        case PRIMAL:
            return GLP_PRIMAL;
        case DUAL:
            return GLP_DUAL;
        default:
            assert(false);
            return GLP_PRIMAL;
        }
    }

    /**
     * Returns column bounds.
     */
    std::pair<double, double> get_col_bounds(col_id col) const {
        auto column = get_col(col);
        return glp_type_to_bounds(glp_get_col_lb(m_lp, column),
                                  glp_get_col_ub(m_lp, column),
                                  glp_get_col_type(m_lp, column));
    }

    /**
     * Returns row bounds.
     */
    std::pair<double, double> get_row_bounds(row_id row) const {
        auto row_num = get_row(row);
        return glp_type_to_bounds(glp_get_row_lb(m_lp, row_num),
                                  glp_get_row_ub(m_lp, row_num),
                                  glp_get_row_type(m_lp, row_num));
    }

    /**
     * Converts column identifier to the GLPK column index.
     */
    int get_col(col_id col) const { return m_col_idx.get_idx(col.get()) + 1; }

    /**
     * Converts row identifier to the GLPK row index.
     */
    int get_row(row_id row) const { return m_row_idx.get_idx(row.get()) + 1; }

    /**
     * Converts GLPK column index to the column identifier.
     */
    col_id get_col_id(int col) const {
        return col_id(m_col_idx.get_val(col - 1));
    }

    /**
     * Converts GLPK row index to the row identifier.
     */
    row_id get_row_id(int row) const {
        return row_id(m_row_idx.get_val(row - 1));
    }

    /**
     * Optimizes the LP using the simplex method.
     *
     * @return solution status
     */
    problem_type run_simplex(simplex_type type, bool resolve) {
        m_glpk_control.meth = simplex_type_to_glp(type);
        static const std::string buggy_version = "4.52";
        if (!resolve || glp_version() == buggy_version) {
            //TODO waiting for response to on
            //http://lists.gnu.org/archive/html/bug-glpk/2014-06/msg00000.html
            glp_adv_basis(m_lp, 0);
        }
        int ret = glp_simplex(m_lp, &m_glpk_control);
        if (resolve && ret != 0) {
            // if basis is not valid, create basis and try again
            glp_adv_basis(m_lp, 0);
            ret = glp_simplex(m_lp, &m_glpk_control);
        }
        assert(ret == 0);
        return get_primal_type();
    }

    /**
     * Converts the GLPK soltion status into paal::lp::problem_type.
     *
     * @return solution status
     */
    problem_type get_primal_type() {
        if (glp_get_status(m_lp) == GLP_OPT) {
            return OPTIMAL;
        }

        switch (glp_get_prim_stat(m_lp)) {
        case GLP_UNDEF:
            return UNDEFINED;
        case GLP_NOFEAS:
            return INFEASIBLE;
        case GLP_FEAS:
        case GLP_INFEAS:
            if (glp_get_dual_stat(m_lp) == GLP_NOFEAS) {
                return UNBOUNDED;
            } else {
                return UNDEFINED;
            }
        default:
            assert(false);
            return UNDEFINED;
        }
    }

    void resize_col_tmp() {
        m_idx_cols_tmp.push_back(0);
        m_val_cols_tmp.push_back(0);
    }

    void resize_row_tmp() {
        m_idx_rows_tmp.push_back(0);
        m_val_rows_tmp.push_back(0);
        m_rows_tmp.push_back({ row_id(0), 0 });
    }

    /// GLPK problem object
    glp_prob *m_lp;
    glp_smcp m_glpk_control;

    /// mapping between GLPK column numbers and column IDs
    data_structures::eraseable_bimap<int> m_col_idx;
    /// mapping between GLPK row numbers and column IDs
    data_structures::eraseable_bimap<int> m_row_idx;

    int m_total_col_nr;
    int m_total_row_nr;

    mutable Ids m_idx_cols_tmp;
    mutable Ids m_idx_rows_tmp;
    mutable Vals m_val_cols_tmp;
    mutable Vals m_val_rows_tmp;
    mutable IdsVals m_rows_tmp;
};
} // detail

using glp = detail::lp_base<detail::glp_impl>;

} // lp
} // paal

#endif // PAAL_GLP_HPP


#include <boost/math/special_functions/log1p.hpp>


//==================================================================================================
/*!

  Copyright 2015 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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/div.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>

STF_CASE_TPL (" div real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using bs::div;
  using r_t = decltype(div(T(), T()));

  STF_IEEE_EQUAL(div(bs::Inf<T>(), bs::Inf<T>(), bs::round2even), bs::Nan<r_t>());
  STF_IEEE_EQUAL(div(bs::Minf<T>(), bs::Minf<T>(), bs::round2even), bs::Nan<r_t>());
  STF_EQUAL(div(bs::Mone<T>(), bs::Mone<T>(), bs::round2even), bs::One<r_t>());
  STF_IEEE_EQUAL(div(bs::Nan<T>(), bs::Nan<T>(), bs::round2even), bs::Nan<r_t>());
  STF_EQUAL(div(bs::One<T>(), bs::One<T>(), bs::round2even), bs::One<r_t>());
} // end of test for floating_

STF_CASE_TPL (" div unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;

  using bs::div;
  using r_t = decltype(div(T(), T()));

  STF_EQUAL(div(bs::One<T>(), bs::One<T>(), bs::round2even), bs::One<r_t>());
  STF_EQUAL(div(T(5), T(2), bs::round2even), T(2));
  STF_EQUAL(div(T(7), T(2), bs::round2even), T(4));
  STF_EQUAL(div(T(9), T(3), bs::round2even), T(3));
  STF_EQUAL(div(T(10), T(3), bs::round2even), T(3));
  STF_EQUAL(div(T(11), T(3), bs::round2even), T(4));
  STF_EQUAL(div(T(12), T(3), bs::round2even), T(4));
  STF_EQUAL(div(T(18), T(6), bs::round2even), T(3));
  STF_EQUAL(div(T(20), T(6), bs::round2even), T(3));
  STF_EQUAL(div(T(22), T(6), bs::round2even), T(4));
  STF_EQUAL(div(T(24), T(6), bs::round2even), T(4));
  STF_EQUAL(div(bs::Valmax<T>(),bs::Two<T>(), bs::round2even), bs::Valmax<T>()/bs::Two<T>()+bs::One<T>());

} // end of test for unsigned_int_

STF_CASE_TPL (" div signed_int", STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using bs::div;
  using r_t = decltype(div(T(), T()));

  STF_EQUAL(div(bs::Mone<T>(), bs::Mone<T>(), bs::round2even), bs::One<r_t>());
  STF_EQUAL(div(bs::One<T>(), bs::One<T>(), bs::round2even), bs::One<r_t>());
  STF_EQUAL(div(T(5), T(2), bs::round2even), T(2));
  STF_EQUAL(div(T(7), T(2), bs::round2even), T(4));
  STF_EQUAL(div(T(-5), T(2), bs::round2even), T(-2));
  STF_EQUAL(div(T(-7), T(2), bs::round2even), T(-4));
  STF_EQUAL(div(T(-4),T(0), bs::round2even), bs::Valmin<r_t>());
  STF_EQUAL(div(T(4),T(0), bs::round2even), bs::Valmax<r_t>());
  STF_EQUAL(div(T(4),T(3), bs::round2even), T(1));
  STF_EQUAL(div(T(-4),T(-3), bs::round2even), T(1));
  STF_EQUAL(div(T(4),T(-3), bs::round2even), T(-1));
  STF_EQUAL(div(T(-4),T(3), bs::round2even), T(-1));
  STF_EQUAL(div(T(5),T(3), bs::round2even), T(2));
  STF_EQUAL(div(T(-5),T(-3), bs::round2even), T(2));
  STF_EQUAL(div(T(5),T(-3), bs::round2even), T(-2));
  STF_EQUAL(div(T(-5),T(3), bs::round2even), T(-2));

  STF_EQUAL(div(T(5),T(4), bs::round2even), T(1));
  STF_EQUAL(div(T(-5),T(-4), bs::round2even), T(1));
  STF_EQUAL(div(T(5),T(-4), bs::round2even), T(-1));
  STF_EQUAL(div(T(-5),T(4), bs::round2even), T(-1));
  STF_EQUAL(div(T(6),T(4), bs::round2even), T(2));
  STF_EQUAL(div(T(-6),T(-4), bs::round2even), T(2));
  STF_EQUAL(div(T(6),T(-4), bs::round2even), T(-2));
  STF_EQUAL(div(T(-6),T(4), bs::round2even), T(-2));
  STF_EQUAL(div(T(8),T(4), bs::round2even), T(2));
  STF_EQUAL(div(T(-8),T(-4), bs::round2even), T(2));
  STF_EQUAL(div(T(8),T(-4), bs::round2even), T(-2));
  STF_EQUAL(div(T(-8),T(4), bs::round2even), T(-2));
  STF_EQUAL(div(T(9),T(4), bs::round2even), T(2));
  STF_EQUAL(div(T(-9),T(-4), bs::round2even), T(2));
  STF_EQUAL(div(T(9),T(-4), bs::round2even), T(-2));
  STF_EQUAL(div(T(-9),T(4), bs::round2even), T(-2));
  STF_EQUAL(div(T(10),T(4), bs::round2even), T(2));
  STF_EQUAL(div(T(-10),T(-4), bs::round2even), T(2));
  STF_EQUAL(div(T(10),T(-4), bs::round2even), T(-2));
  STF_EQUAL(div(T(-10),T(4), bs::round2even), T(-2));

} // end of test for signed_int_


//
//		HSP3.0 plugin sample
//		onion software/onitama 2004/9
//

#define WIN32_LEAN_AND_MEAN		// Exclude rarely-used stuff from Windows headers
#define NOMINMAX
#include <Windows.h>
#include <cstdio>
#include <cstdlib>
#include "../HSP_Includes/hsp3plugin.h"

#include "Boost-LongInt.h"

#include <boost/multiprecision/cpp_int.hpp>
/*------------------------------------------------------------*/
/*
controller
*/
/*------------------------------------------------------------*/
typedef boost::multiprecision::cpp_int boost_longint;
static boost_longint ref_fval;						// 返値のための変数
static double dp1;

static void *reffunc(int *type_res, int cmd)
{
	//		関数・システム変数の実行処理 (値の参照時に呼ばれます)
	//
	//			'('で始まるかを調べる
	//
	if (*type != TYPE_MARK) puterror(HSPERR_INVALID_FUNCPARAM);
	if (*val != '(') puterror(HSPERR_INVALID_FUNCPARAM);
	code_next();

	int cp;
	switch (cmd) {							// サブコマンドごとの分岐

	case 0x00:								// boost_longint関数

		cp = code_getprm();
		switch (cp) {
		case PARAM_DEFAULT:
		case PARAM_END:
		case PARAM_ENDSPLIT:
			ref_fval = boost_longint();
			break;
		default:
			switch (mpval->flag) {
			case 2:
				ref_fval = boost_longint((char*)mpval->pt);
				break;
			case 3:
				ref_fval = boost_longint(*(double*)mpval->pt);
				break;
			case 4:
				ref_fval = boost_longint(*(int*)mpval->pt);
				break;
			default:
				ref_fval = boost_longint(0);
			}
		}
		break;

	default:
		puterror(HSPERR_UNSUPPORTED_FUNCTION);
	}

	//			'('で終わるかを調べる
	//
	if (*type != TYPE_MARK) puterror(HSPERR_INVALID_FUNCPARAM);
	if (*val != ')') puterror(HSPERR_INVALID_FUNCPARAM);
	code_next();

	*type_res = HspVarBoost_Longint_typeid();		// 返値のタイプを指定する
	return (void *)&ref_fval;
}

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

static int termfunc(int option)
{
	//		終了処理 (アプリケーション終了時に呼ばれます)
	//
	return 0;
}

/*------------------------------------------------------------*/
/*
interface
*/
/*------------------------------------------------------------*/

int WINAPI DllMain(HINSTANCE hInstance, DWORD fdwReason, PVOID pvReserved)
{
	//		DLLエントリー (何もする必要はありません)
	//
	return TRUE;
}

EXPORT void WINAPI hsp3cmdinit(HSP3TYPEINFO *info)
{
	//		プラグイン初期化 (実行・終了処理を登録します)
	//
	hsp3sdk_init(info);		// SDKの初期化(最初に行なって下さい)

	info->reffunc = reffunc;		// 参照関数(reffunc)の登録
	info->termfunc = termfunc;		// 終了関数(termfunc)の登録

	registvar(-1, HspVarBoost_Longint_Init);		// 新しい型の追加
}

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


// Copyright Stephan T. Lavavej, http://nuwen.net .
// Distributed under the Boost Software License, Version 1.0.
// See accompanying file LICENSE_1_0.txt or copy at
// http://boost.org/LICENSE_1_0.txt .

#ifndef PHAM_HUFF_HH
#define PHAM_HUFF_HH

#include "compiler.hh"

#ifdef NUWEN_PLATFORM_MSVC
    #pragma once
#endif

// LJM developed the ideas behind the automata.

#ifdef PHAM_AUTOMATON_TIMING
    #include "clock.hh"
#endif

#include "gluon.hh"
#include "typedef.hh"
#include "vector.hh"

#include "external_begin.hh"
    #include <algorithm>
    #include <numeric>
    #include <queue>
    #include <stdexcept>
    #include <vector>
    #include <boost/utility.hpp>
#include "external_end.hh"

namespace nuwen {
    inline vuc_t huff(const vuc_t& v);
    inline vuc_t puff(const vuc_t& v);
}

namespace pham {
    namespace huff {
        typedef nuwen::vuc_s_t freq_t;
        typedef std::vector<freq_t> vfreq_t;

        class node {
        public:
            node() : m_freq(0), m_byte(0), m_left(NULL), m_right(NULL) { }

            node(const freq_t f, const nuwen::uc_t b)
                : m_freq(f), m_byte(b), m_left(NULL), m_right(NULL) { }

            // The m_byte of internal nodes is ignored.
            node(const node * const l, const node * const r)
                : m_freq(l->m_freq + r->m_freq), m_byte(0), m_left(l), m_right(r) { }

            freq_t        freq() const { return m_freq;  }
            nuwen::uc_t   byte() const { return m_byte;  }
            const node *  left() const { return m_left;  }
            const node * right() const { return m_right; }

            void set_left( const node * const l) { m_left  = l; }
            void set_right(const node * const r) { m_right = r; }

        private:
            freq_t       m_freq;
            nuwen::uc_t  m_byte;
            const node * m_left;
            const node * m_right;
        };

        inline void read_codelengths_helper(nuwen::vuc_t& v, const node * const p, const nuwen::uc_t h) {
            if (p->left()) {
                read_codelengths_helper(v, p->left(),  static_cast<nuwen::uc_t>(h + 1));
                read_codelengths_helper(v, p->right(), static_cast<nuwen::uc_t>(h + 1));
            } else {
                v[p->byte()] = h;
            }
        }

        inline nuwen::vuc_t read_codelengths(const node * const root) {
            nuwen::vuc_t ret(256);
            read_codelengths_helper(ret, root, 0);
            return ret;
        }

        struct comparator {
            bool operator()(const node * const l, const node * const r) const {
                return l->freq() > r->freq();
            }
        };

        inline vfreq_t frequencies(const nuwen::vuc_t& v) {
            vfreq_t freqs(256, 0);

            for (nuwen::vuc_ci_t i = v.begin(); i != v.end(); ++i) {
                ++freqs[*i];
            }

            return freqs;
        }

        inline nuwen::vuc_t make_codelengths(const vfreq_t& freqs) {
            std::vector<node> nodes;
            nodes.reserve(511);

            std::priority_queue<const node *, std::vector<const node *>, comparator> pq;

            for (nuwen::us_t i = 0; i < 256; ++i) {
                nodes.push_back(node(freqs[i], static_cast<nuwen::uc_t>(i)));
                pq.push(&nodes.back());
            }

            while (pq.size() > 1) {
                const node * const first = pq.top();
                pq.pop();
                const node * const second = pq.top();
                pq.pop();

                nodes.push_back(node(first, second));
                pq.push(&nodes.back());
            }

            return read_codelengths(pq.top());
        }

        inline nuwen::vuc_t make_codes(const nuwen::vuc_t& codelengths) {
            nuwen::vuc_t codes(256);

            nuwen::uc_t start = 0;

            for (int i = 255; i > 0; --i) {             // Fill in codes that have codelength i.
                for (nuwen::us_t j = 0; j < 256; ++j) {
                    if (codelengths[j] == i) {
                        codes[j] = start++;
                    }
                }

                start = static_cast<nuwen::uc_t>(start >> 1);
            }

            return codes;
        }

        // Despite occupying 128 KB, this results in a massive speedup.
        class byte_decoder : public boost::noncopyable {
        public:
            static const nuwen::us_t INVALID = 256;

            byte_decoder(const nuwen::vuc_t& codelengths, const nuwen::vuc_t& codes) {
                for (int i = 0; i < 256; ++i) {
                    for (int j = 0; j < 256; ++j) {
                        m_table[i][j] = INVALID;
                    }
                }

                for (nuwen::us_t i = 0; i < 256; ++i) {
                    m_table[codelengths[i]][codes[i]] = i;
                }
            }

            nuwen::us_t operator()(const nuwen::uc_t codelength, const nuwen::uc_t code) const {
                return m_table[codelength][code];
            }

        private:
            nuwen::us_t m_table[256][256];
        };

        inline const node * regen_tree_helper(std::vector<node>& nodes, const byte_decoder& decoder, nuwen::uc_t length, nuwen::uc_t code) {
            nodes.push_back(node());

            node * const p = &nodes.back();

            ++length;
            code = static_cast<nuwen::uc_t>(code << 1);

            const nuwen::us_t leftbyte = decoder(length, code);

            if (leftbyte == byte_decoder::INVALID) {
                p->set_left(regen_tree_helper(nodes, decoder, length, code));
            } else {
                p->set_left(&nodes[leftbyte]);
            }

            code |= 1;

            const nuwen::us_t rightbyte = decoder(length, code);

            if (rightbyte == byte_decoder::INVALID) {
                p->set_right(regen_tree_helper(nodes, decoder, length, code));
            } else {
                p->set_right(&nodes[rightbyte]);
            }

            return p;
        }

        // Given codelengths and codes, regenerates the Huffman tree and places it into nodes.
        // nodes will contain 511 elements.
        // The first 256 elements will be the leaf nodes. The leaf node for byte B will be at index B.
        // The next 255 elements will be the internal nodes, starting with the root (at index 256).
        // (All nodes will have zero frequencies; this information has been lost and doesn't matter.)
        // Returns a pointer to the root.
        inline const node * regen_tree(std::vector<node>& nodes, const nuwen::vuc_t& codelengths, const nuwen::vuc_t& codes) {
            if (!nodes.empty()) {
                throw std::logic_error("LOGIC ERROR: pham::huff::regen_tree() - nodes should be empty.");
            }

            nodes.reserve(511);

            for (int i = 0; i < 256; ++i) {
                nodes.push_back(node(0, static_cast<nuwen::uc_t>(i)));
            }

            const byte_decoder decoder(codelengths, codes);

            return regen_tree_helper(nodes, decoder, 0, 0);
        }

        inline nuwen::vuc_t huff_bits(const nuwen::vuc_t& v) {
            using namespace nuwen;

            const vfreq_t freqs       = frequencies(v);
            const vuc_t   codelengths = make_codelengths(freqs);
            const vuc_t   codes       = make_codes(codelengths);

            pack::packed_bits encoded;

            for (vuc_ci_t i = v.begin(); i != v.end(); ++i) {
                const uc_t byte   = *i;
                const uc_t code   = codes[byte];
                      uc_t length = codelengths[byte];

                while (length > 8) {
                    --length;
                    encoded.push_back(0);
                }

                while (length > 0) {
                    --length;
                    encoded.push_back(code >> length & 1);
                }
            }

            return vec(cat(codelengths)(encoded.vuc()));
        }

        inline nuwen::vuc_t puff_bits(const nuwen::vuc_t& v) {
            using namespace nuwen;

            const vuc_t codelengths(v.begin(), v.begin() + 256);
            const vuc_t codes = make_codes(codelengths);

            const byte_decoder decoder(codelengths, codes);

            vuc_t decompressed;

            uc_t mrs8b = 0; // Most Recently Seen 8 Bits
            uc_t nbs   = 0; // Number Of Bits Seen

            for (ull_t i = 2048; i < static_cast<ull_t>(v.size()) * 8; ++i) {
                mrs8b = static_cast<uc_t>(mrs8b << 1);                 // Make space for another bit.
                mrs8b = static_cast<uc_t>(mrs8b | bit_from_vuc(v, i)); // Load in another bit.
                ++nbs;                                                 // We've seen another bit.

                const us_t byte = decoder(nbs, mrs8b);

                if (byte != byte_decoder::INVALID) {
                    decompressed.push_back(static_cast<uc_t>(byte));
                    mrs8b = 0;
                    nbs = 0;
                }
            }

            return decompressed;
        }

        class huff_automaton : public boost::noncopyable {
        private:
            struct entry {
                nuwen::uc_t numemit;     // The number of bytes to output, [0, 6 or 12 or 32]. (See end of class.)
                nuwen::uc_t new_numbits; // The new number of compressed bits not yet output, [0, 7].
                nuwen::uc_t firstpart;   // The high byte of the shifted code.
                nuwen::uc_t secondpart;  // The low byte of the shifted code.
            };

            // When numemit is 0:         Emit nothing.                              Then m_bits |= secondpart.
            // When numemit is 1:         Emit m_bits | firstpart.                   Then m_bits = secondpart.
            // When numemit is 2 or more: Emit m_bits, numemit - 2 zeros, firstpart. Then m_bits = secondpart.

            entry          m_table[256][8]; // Indexed by byte, m_numbits.
            nuwen::uc_t    m_bits;          // LEFT ALIGNED compressed bits not yet output.
            nuwen::uc_t    m_numbits;       // The number of compressed bits not yet output, [0, 7].
            nuwen::vuc_i_t m_dest;          // Output destination.

        public:
            huff_automaton(const nuwen::vuc_t& codelengths, const nuwen::vuc_t& codes, const nuwen::vuc_i_t dest)
                : m_bits(0), m_numbits(0), m_dest(dest) {

                using namespace nuwen;

                for (nuwen::us_t possible_byte = 0; possible_byte < 256; ++possible_byte) {
                    const uc_t length = codelengths[possible_byte];
                    const uc_t code = codes[possible_byte];

                    for (int possible_numbits = 0; possible_numbits < 8; ++possible_numbits) {
                        entry * const ent = &m_table[possible_byte][possible_numbits];

                        // The total number of bits we have to deal with, [1, 51 or 97 or 262].
                        const us_t total_bits = static_cast<us_t>(possible_numbits + length);

                        ent->numemit     = static_cast<uc_t>(total_bits / 8);
                        ent->new_numbits = static_cast<uc_t>(total_bits % 8);

                        // The number of empty bits at the right of m_bits, [1, 8].
                        const uc_t empty_bits = static_cast<uc_t>(8 - ent->new_numbits);

                        // We break the code into two parts on a byte boundary.
                        const us_t shiftedcode = static_cast<us_t>(code << empty_bits);

                        ent->firstpart = static_cast<uc_t>(shiftedcode >> 8);
                        ent->secondpart = static_cast<uc_t>(shiftedcode & 0xFF);
                    }
                }
            }

            void operator()(nuwen::vuc_ci_t byte, const nuwen::vuc_ci_t end) {
                for (; byte != end; ++byte) {
                    const entry * const p = &m_table[*byte][m_numbits];
                    m_numbits = p->new_numbits;

                    switch (p->numemit) {
                        case 0:
                            m_bits = static_cast<nuwen::uc_t>(m_bits | p->secondpart);
                            break;
                        case 1:
                            *m_dest++ = static_cast<nuwen::uc_t>(m_bits | p->firstpart);
                            m_bits = p->secondpart;
                            break;
                        case 2:
                            *m_dest++ = m_bits;
                            *m_dest++ = p->firstpart;
                            m_bits = p->secondpart;
                            break;
                        default:
                            *m_dest = m_bits;
                            m_dest += p->numemit - 1;
                            *m_dest++ = p->firstpart;
                            m_bits = p->secondpart;
                            break;
                    }
                }

                if (m_numbits != 0) {
                    *m_dest++ = m_bits;
                }
            }

            // For inputs of less than 2^32 bytes, the maximum possible codelength is 44.
            // For inputs of less than 2^64 bytes, the maximum possible codelength is 90.
            // In the completely general case, the maximum possible codelength is 255.

            // We may have 7 compressed bits not yet output when we encounter a byte with the maximum possible codelength.
            // That gives us 51 or 97 or 262 bits, which is 6 or 12 or 32 full bytes to emit.

            // These upper bounds are nice to know, but aren't actually helpful.
        };

        inline nuwen::vuc_t huff_auto(const nuwen::vuc_t& v
            #ifdef PHAM_AUTOMATON_TIMING
                , double * const ctor_time = NULL
            #endif
        ) {
            using namespace nuwen;

            const vfreq_t freqs       = frequencies(v);
            const vuc_t   codelengths = make_codelengths(freqs);
            const vuc_t   codes       = make_codes(codelengths);

            const ull_t bits = std::inner_product(freqs.begin(), freqs.end(), codelengths.begin(), static_cast<ull_t>(0));

            vuc_t ret(static_cast<vuc_s_t>(256 + bytes_from_bits(bits)), 0);

            std::copy(codelengths.begin(), codelengths.end(), ret.begin());

            #ifdef PHAM_AUTOMATON_TIMING
                const nuwen::chrono::watch w;
            #endif

            huff_automaton automaton(codelengths, codes, ret.begin() + 256);

            #ifdef PHAM_AUTOMATON_TIMING
                if (ctor_time) {
                    *ctor_time = w.seconds();
                }
            #endif

            automaton(v.begin(), v.end());

            return ret;
        }

        class puff_automaton : public boost::noncopyable {
        private:
            // We begin each step on one of the 255 internal nodes,
            // reading any of the 256 possible compressed bytes.
            // The main table has an entry for what we should do.

            struct entry {
                nuwen::uc_t aux_id;       // See below.
                nuwen::uc_t first_symbol; // The first symbol to emit.
                nuwen::uc_t dest_node;    // The index of the internal node that we land on.
            };

            static const nuwen::uc_t ID_EMIT_NO_SYMBOLS = 254;
            static const nuwen::uc_t ID_EMIT_ONE_SYMBOL = 255;

            // Each step will emit [0, 8] symbols.
            // When the number of symbols to emit is 0, aux_id will be ID_EMIT_NO_SYMBOLS and first_symbol will be ignored.
            // When the number of symbols to emit is 1, aux_id will be ID_EMIT_ONE_SYMBOL and first_symbol will be used.
            // When the number of symbols to emit is [2, 8]:
            // After emitting the first symbol, we return to the root. This puts us into a known state. What happens next
            // (i.e. which other symbols we emit, as well as which internal node we finally land on) is determined solely
            // by the remaining bits of the compressed byte.
            // There could be as many as 7 bits (when the first bit causes us to emit the first symbol), or as few as 1.
            // (If there are 0 bits remaining when we emit the first symbol, we definitely won't emit a second symbol.)
            // 2^7 + 2^6 + ... + 2^1 = 254, so we will construct an ID in [0, 254) which corresponds to the remaining bits
            // of the compressed byte. This ID will be stored in aux_id and used to index into auxiliary tables with 254
            // entries that tell us what happens next.
            // We build the auxiliary tables first in order to speed up the construction of the main table. When building
            // the main table, as soon as we emit one symbol, we can look up what happens next and thereby avoid doing any
            // unnecessary computation.
            // m_aux_numemit[id] says how many more symbols will be emitted after the first. It can be any value in [0, 7].
            // m_aux_table[id][N] says what the Nth additional symbol is.
            // aux_dest[id] says what internal node we finally land on; it is used during construction of the main table.

            entry         m_main_table[255][256]; // Indexed by m_curr_node, byte. See above.
            nuwen::uc_t   m_aux_numemit[254];     // Indexed by aux_id. See above.
            nuwen::uc_t   m_aux_table[254][7];    // Indexed by aux_id. See above.
            nuwen::uc_t   m_curr_node;            // The index of the internal node that we are currently on.
            nuwen::vuc_t& m_dest;                 // Output destination.

        public:
            puff_automaton(const nuwen::vuc_t& codelengths, const nuwen::vuc_t& codes, nuwen::vuc_t& dest)
                : m_curr_node(0), m_dest(dest) {

                using namespace nuwen;

                // The bitstrings 0, 1, 00, 01, 10, 11, 000, 001, ... are assigned the IDs 0, 1, 2, 3, 4, 5, 6, 7, ...
                // This corresponds to (1 << numbits) - 2 + masked_byte, where masked_byte is (byte & (1 << numbits) - 1).
                // Looking up id_table[numbits][byte] is faster than computing the ID.

                uc_t id_table[8][256];

                for (int numbits = 1; numbits <= 7; ++numbits) {
                    for (int byte = 0; byte < 256; ++byte) {
                        id_table[numbits][byte] = static_cast<uc_t>((1 << numbits) - 2 + (byte & ((1 << numbits) - 1)));
                    }
                }

                std::vector<node> nodes;

                const node * const root = regen_tree(nodes, codelengths, codes);

                // Build the auxiliary tables.

                uc_t aux_dest[254];

                for (int numbits = 1; numbits <= 7; ++numbits) {
                    for (uc_t masked_byte = 0; masked_byte < (1 << numbits); ++masked_byte) {
                        // Although we traverse the bitstrings in ID order, using id_table is simpler and isn't slower.
                        const uc_t id = id_table[numbits][masked_byte];

                        m_aux_numemit[id] = 0;

                        const node * p = root; // We have just emitted a symbol and have returned to the root.

                        // Process the bits of masked_byte from left to right.
                        for (int k = numbits - 1; k >= 0; --k) {
                            const bool bit = masked_byte >> k & 1;

                            p = bit ? p->right() : p->left();

                            if (p->left() == NULL) {
                                m_aux_table[id][m_aux_numemit[id]++] = p->byte();

                                p = root;
                            }
                        }

                        aux_dest[id] = static_cast<uc_t>(p - root);
                    }
                }

                // Build the main table.

                for (int possible_node = 0; possible_node < 255; ++possible_node) {
                    for (int possible_byte = 0; possible_byte < 256; ++possible_byte) {
                        entry * const ent = &m_main_table[possible_node][possible_byte];

                        ent->aux_id = ID_EMIT_NO_SYMBOLS;

                        const node * p = root + possible_node;

                        // Process the bits of possible_byte from left to right.
                        for (int k = 7; k >= 0; --k) {
                            const bool bit = possible_byte >> k & 1;

                            p = bit ? p->right() : p->left();

                            if (p->left() == NULL) {
                                ent->aux_id = ID_EMIT_ONE_SYMBOL;
                                ent->first_symbol = p->byte();

                                if (k == 0) {
                                    p = root;
                                } else {
                                    const uc_t id = id_table[k][possible_byte];

                                    p = root + aux_dest[id];

                                    if (m_aux_numemit[id] > 0) {
                                        ent->aux_id = id;
                                    }
                                }

                                break;
                            }
                        }

                        ent->dest_node = static_cast<uc_t>(p - root);
                    }
                }
            }

            void operator()(nuwen::vuc_ci_t byte, const nuwen::vuc_ci_t end) {
                for (; byte != end; ++byte) {
                    const entry * const p = &m_main_table[m_curr_node][*byte];
                    m_curr_node = p->dest_node;

                    switch (p->aux_id) {
                        case ID_EMIT_NO_SYMBOLS:
                            break;
                        case ID_EMIT_ONE_SYMBOL:
                            m_dest.push_back(p->first_symbol);
                            break;
                        default:
                            m_dest.push_back(p->first_symbol);

                            for (int i = 0; i < m_aux_numemit[p->aux_id]; ++i) {
                                m_dest.push_back(m_aux_table[p->aux_id][i]);
                            }

                            break;
                    }
                }
            }
        };

        inline nuwen::vuc_t puff_auto(const nuwen::vuc_t& v
            #ifdef PHAM_AUTOMATON_TIMING
                , double * const ctor_time = NULL
            #endif
        ) {
            const nuwen::vuc_t codelengths(v.begin(), v.begin() + 256);
            const nuwen::vuc_t codes = make_codes(codelengths);

            nuwen::vuc_t ret;

            ret.reserve(v.size() * 2); // A reasonable heuristic.

            #ifdef PHAM_AUTOMATON_TIMING
                const nuwen::chrono::watch w;
            #endif

            puff_automaton automaton(codelengths, codes, ret);

            #ifdef PHAM_AUTOMATON_TIMING
                if (ctor_time) {
                    *ctor_time = w.seconds();
                }
            #endif

            automaton(v.begin() + 256, v.end());

            return ret;
        }
    }
}

inline nuwen::vuc_t nuwen::huff(const vuc_t& v) {
    // huff_automaton construction is so fast, we may as well always use it.
    return pham::huff::huff_auto(v);
}

inline nuwen::vuc_t nuwen::puff(const vuc_t& v) {
    if (v.size() < 256) {
        throw std::runtime_error("RUNTIME ERROR: nuwen::puff() - Insufficient data to decompress.");
    }

    // For compressed data smaller than 6 KB, bitwise decompression is faster.
    return v.size() < 6144 ? pham::huff::puff_bits(v) : pham::huff::puff_auto(v);
}

#endif // Idempotency


/*
 * Copyright (C) 2018 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.
 *
*/

#ifndef IGNITION_PHYSICS_GEOMETRY_HH_
#define IGNITION_PHYSICS_GEOMETRY_HH_

#include <Eigen/Geometry>

#define DETAIL_IGN_PHYSICS_MAKE_BOTH_PRECISIONS(Type, Dim) \
  using Type ## Dim ## d = Type<double, Dim>; \
  using Type ## Dim ## f = Type<float, Dim>;

/// \brief This macro defines the following types:
/// Type2d // 2-dimensional version of Type with double precision
/// Type2f // 2-dimensional version of Type with float precision
/// Type3d // 3-dimensional version of Type with double precision
/// Type3f // 3-dimensional version of Type with float precision
#define IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(Type) \
  DETAIL_IGN_PHYSICS_MAKE_BOTH_PRECISIONS(Type, 2) \
  DETAIL_IGN_PHYSICS_MAKE_BOTH_PRECISIONS(Type, 3)

namespace ignition
{
  namespace physics
  {
    /// \brief This is used by ignition-physics to represent rigid body
    /// transforms in 2D or 3D simulations. The precision can be chosen as
    /// float or scalar.
    template <typename Scalar, std::size_t Dim>
    using Pose = Eigen::Transform<Scalar, Dim, Eigen::Isometry>;
    IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(Pose)

    template <typename Scalar, std::size_t Dim>
    using Vector = Eigen::Matrix<Scalar, Dim, 1>;
    IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(Vector)

    template <typename Scalar, std::size_t Dim>
    using LinearVector = Vector<Scalar, Dim>;
    IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(LinearVector)

    template <typename Scalar, std::size_t Dim>
    using AngularVector = Vector<Scalar, (Dim*(Dim-1))/2>;
    IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(AngularVector)

    template <typename Scalar, std::size_t Dim>
    struct Wrench
    {
      AngularVector<Scalar, Dim> torque;
      LinearVector<Scalar, Dim> force;
    };
    IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(Wrench)

    template <typename Scalar, std::size_t Dim>
    using AlignedBox = Eigen::AlignedBox<Scalar, Dim>;
    IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(AlignedBox)

    /////////////////////////////////////////////////
    /// \brief This struct is used to conveniently convert from a policy to a
    /// geometric type. Example usage:
    ///
    /// using AngularVector = FromPolicy<FeaturePolicy3d>::To<AngularVector>;
    template <typename PolicyT>
    struct FromPolicy
    {
      using Scalar = typename PolicyT::Scalar;
      enum { Dim = PolicyT::Dim };

      template<template <typename, std::size_t> class Type>
      using Use = Type<Scalar, Dim>;
    };

    template<typename Scalar>
    Eigen::Rotation2D<Scalar> Rotate(
        const Scalar &_angle,
        const AngularVector<Scalar, 2> &_axis)
    {
      return Eigen::Rotation2D<Scalar>(_angle*_axis[0]);
    }

    template <typename Scalar>
    Eigen::AngleAxis<Scalar> Rotate(
        const Scalar &_angle,
        const AngularVector<Scalar, 3> &_axis)
    {
      return Eigen::AngleAxis<Scalar>(_angle, _axis);
    }
  }
}

#endif


#include "metrics.h"
#include "mxutils.h"

#include <mshadow/tensor.h>
#include <Eigen/Dense>

void RCNNAccMetric::Update(mxnet::cpp::NDArray labels,
                           mxnet::cpp::NDArray preds) {
  auto preds_shape = preds.GetShape();
  auto labels_shape = labels.GetShape();
  mx_uint batches = labels_shape.front();
  mx_uint len = labels_shape.back() * batches;
  mx_uint classes_num = preds_shape.back();
  std::vector<mx_float> pred_data(len);
  std::vector<mx_float> label_data(len);
  preds.Reshape(mxnet::cpp::Shape(static_cast<mx_uint>(-1), classes_num))
      .ArgmaxChannel()
      .SyncCopyToCPU(&pred_data, pred_data.size());
  labels.Reshape(mxnet::cpp::Shape(static_cast<mx_uint>(-1)))
      .SyncCopyToCPU(&label_data, label_data.size());
  CheckMXnetError("RCNNAccMetric");
  for (mx_uint i = 0; i < len; ++i) {
    sum_metric +=
        (static_cast<int>(pred_data[i]) == static_cast<int>(label_data[i])) ? 1
                                                                            : 0;
    num_inst += 1;
  }
}

void RCNNLogLossMetric::Update(mxnet::cpp::NDArray labels,
                               mxnet::cpp::NDArray preds) {
  auto preds_shape = preds.GetShape();
  auto labels_shape = labels.GetShape();
  mx_uint batches = labels_shape.front();
  mx_uint len = labels_shape.back() * batches;
  mx_uint classes_num = preds_shape.back();
  std::vector<mx_float> pred_data(preds.Size());
  std::vector<mx_float> label_data(labels.Size());
  preds.SyncCopyToCPU(&pred_data, pred_data.size());
  labels.SyncCopyToCPU(&label_data, label_data.size());
  CheckMXnetError("RCNNLogLossMetric");

  auto pred = Eigen::Map<const Eigen::Matrix<float, Eigen::Dynamic,
                                             Eigen::Dynamic, Eigen::RowMajor>>(
      pred_data.data(), len, classes_num);

  auto label = Eigen::Map<const Eigen::Matrix<float, Eigen::Dynamic,
                                              Eigen::Dynamic, Eigen::RowMajor>>(
      label_data.data(), len, 1);

  Eigen::MatrixXf cls(len, 1);
  for (Eigen::Index r = 0; r < len; ++r) {
    cls(r, 0) = pred(r, static_cast<Eigen::Index>(label(r, 0)));
  }

  cls.array() += 1e-14f;
  auto cls_loss = -1 * cls.array().log();
  auto value = cls_loss.sum();
  assert(!std::isnan(value));
  sum_metric += value;
  num_inst += label.rows();
}

void RCNNL1LossMetric::Update(mxnet::cpp::NDArray labels,
                              mxnet::cpp::NDArray preds) {
  std::vector<mx_float> pred_data(preds.Size());
  std::vector<mx_float> label_data(labels.Size());
  preds.SyncCopyToCPU(&pred_data, pred_data.size());
  labels.SyncCopyToCPU(&label_data, label_data.size());
  CheckMXnetError("RCNNL1LossMetric");

  auto bbox_loss =
      Eigen::Map<const Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic,
                                     Eigen::RowMajor>>(
          pred_data.data(), static_cast<Eigen::Index>(pred_data.size()), 1);

  auto label = Eigen::Map<const Eigen::Matrix<float, Eigen::Dynamic,
                                              Eigen::Dynamic, Eigen::RowMajor>>(
      label_data.data(), static_cast<Eigen::Index>(label_data.size()), 1);

  sum_metric += bbox_loss.array().sum();
  num_inst += (label.array() != 0).count();
}

void RPNL1LossMetric::Update(mxnet::cpp::NDArray labels,
                             mxnet::cpp::NDArray preds) {
  auto preds_shape = preds.GetShape();
  auto labels_shape = labels.GetShape();
  std::vector<mx_float> pred_data(preds.Size());
  std::vector<mx_float> label_data(labels.Size());
  preds.SyncCopyToCPU(&pred_data, pred_data.size());
  labels.SyncCopyToCPU(&label_data, label_data.size());
  CheckMXnetError("RPNL1LossMetric");

  auto bbox_loss =
      Eigen::Map<const Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic,
                                     Eigen::RowMajor>>(
          pred_data.data(), static_cast<Eigen::Index>(pred_data.size()), 1);

  auto bbox_weight =
      Eigen::Map<const Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic,
                                     Eigen::RowMajor>>(
          label_data.data(), static_cast<Eigen::Index>(label_data.size()), 1);

  // calculate num_inst(average on those fg anchors)
  auto num = (bbox_weight.array() > 0).count() / 4;
  auto val = bbox_loss.array().sum();
  sum_metric += val;
  num_inst += num;
}

void RPNAccMetric::Update(mxnet::cpp::NDArray labels,
                          mxnet::cpp::NDArray preds) {
  auto preds_shape = preds.GetShape();
  auto labels_shape = labels.GetShape();
  std::vector<mx_float> pred_data(labels.Size());
  std::vector<mx_float> label_data(labels.Size());
  preds.ArgmaxChannel().SyncCopyToCPU(&pred_data, pred_data.size());
  labels.SyncCopyToCPU(&label_data, label_data.size());
  CheckMXnetError("RPNAccMetric");
  for (mx_uint i = 0; i < labels.Size(); ++i) {
    if (static_cast<long>(label_data[i]) != -1) {
      sum_metric +=
          (static_cast<int>(pred_data[i]) == static_cast<int>(label_data[i]))
              ? 1
              : 0;
      num_inst += 1;
    }
  }
}

void RPNLogLossMetric::Update(mxnet::cpp::NDArray labels,
                              mxnet::cpp::NDArray preds) {
  using namespace mxnet::cpp;
  using namespace mshadow;
  std::vector<mx_float> label_data(labels.Size());
  labels.SyncCopyToCPU(&label_data, label_data.size());

  std::vector<mx_float> pred_data(preds.Size());
  preds.SyncCopyToCPU(&pred_data, pred_data.size());
  CheckMXnetError("RPNLogLossMetric");

  index_t labels_len = static_cast<index_t>(label_data.size());
  index_t len = static_cast<index_t>(pred_data.size());
  index_t dim3 = len / (preds.GetShape()[0] * preds.GetShape()[1]);
  auto out_shape = Shape2(labels_len, len / labels_len);
  Tensor<cpu, 2, float> pred(pred_data.data(), out_shape);
  {
    TensorContainer<cpu, 2, float> x3(out_shape);
    {
      Tensor<cpu, 4, float> pred_map(
          pred_data.data(), Shape4(preds.GetShape()[0], preds.GetShape()[1],
                                   preds.GetShape()[2], preds.GetShape()[3]));
      auto x1 = expr::reshape(
          pred_map, Shape3(preds.GetShape()[0], preds.GetShape()[1], dim3));
      auto x2 = expr::transpose(x1, Shape3(0, 2, 1));
      x3 = expr::reshape(x2, out_shape);
    }
    Copy(pred, x3);
  }

  // filter with labels
  std::vector<float> cls_data;
  cls_data.reserve(labels_len);
  for (index_t i = 0; i < labels_len; ++i) {
    if (static_cast<long>(label_data[i]) != -1) {
      cls_data.push_back(pred[i][static_cast<index_t>(label_data[i])]);
    }
  }

  Eigen::MatrixXf cls =
      Eigen::Map<const Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic,
                                     Eigen::RowMajor>>(
          cls_data.data(), static_cast<Eigen::Index>(cls_data.size()), 1);

  cls.array() += 1e-14f;
  auto cls_loss = -1 * cls.array().log();
  sum_metric += cls_loss.sum();
  num_inst += cls_data.size();
}


#ifndef DIJKSTRA_HPP
#define DIJKSTRA_HPP

#include <boost/assert.hpp>

#include "routing_base.hpp"
#include "../DynamicServer/DataStructures/InternalDataFacade.h"
#include "../data_structures/search_engine_data.hpp"
#include "../Util/integer_range.hpp"
#include "../Util/timing_util.hpp"
#include "../typedefs.h"

template <class DataFacadeT> class BasicDijkstraRouting final : public BasicRoutingInterface<DataFacadeT>
{
	typedef typename DataFacadeT::EdgeData EdgeData;

	using super = BasicRoutingInterface<DataFacadeT>;
	using QueryHeap = SearchEngineData::QueryHeap;
	SearchEngineData &engine_working_data;

public:
	BasicDijkstraRouting(DataFacadeT *facade, SearchEngineData &engine_working_data)
		: super(facade), engine_working_data(engine_working_data)
	{
	}

	~BasicDijkstraRouting() {}

	void operator()(const std::vector<PhantomNodes> &phantom_nodes_vector,
					const std::vector<bool> &uturn_indicators,
					RawRouteData &raw_route_data) const
	{
		TIMER_START(query);

		int distance = 0;
		int setteled_nodes = 0;
		std::vector<NodeID> path;

		QueryHeap dijkstra_heap(super::facade->GetNumberOfNodes());
		NodeID source = phantom_nodes_vector[0].source_phantom.forward_node_id;
		NodeID target = SPECIAL_NODEID;
		NodeID f_target = phantom_nodes_vector[0].target_phantom.forward_node_id;
		NodeID r_target = phantom_nodes_vector[0].target_phantom.reverse_node_id;
		NodeID current = SPECIAL_NODEID;

		dijkstra_heap.Clear();

		const bool allow_u_turn = false;

		if (source != SPECIAL_NODEID)
		{
			dijkstra_heap.Insert(
						phantom_nodes_vector[0].source_phantom.forward_node_id,
					(allow_u_turn ? 0 : distance) - phantom_nodes_vector[0].source_phantom.GetForwardWeightPlusOffset(),
					phantom_nodes_vector[0].source_phantom.forward_node_id);
			dijkstra_heap.Insert(
						phantom_nodes_vector[0].source_phantom.reverse_node_id,
					(allow_u_turn ? 0 : distance) - phantom_nodes_vector[0].source_phantom.GetReverseWeightPlusOffset(),
					phantom_nodes_vector[0].source_phantom.reverse_node_id);

			while ( !dijkstra_heap.Empty() )
			{
				current = dijkstra_heap.DeleteMin();
				distance = dijkstra_heap.GetKey(current);

				//SimpleLogger().Write(logDEBUG) << "expanding node " << current;

				if ( current == f_target || current == r_target )
				{
					target = current;
					SimpleLogger().Write(logDEBUG) << "found road to target " << current << " of distance " << distance << std::endl
												   << "setteled nodes " << setteled_nodes;
					break;
				}

				for (const auto edge : super::facade->GetAdjacentEdgeRange(current))
				{
					const EdgeData &data = super::facade->GetEdgeData(edge);
					const int edge_weight = data.distance;
					BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid");

					if ( data.forward )
					{
						// New Node discovered -> Add to Heap + Node Info Storage
						const NodeID to = super::facade->GetTarget(edge);
						const int to_distance = distance + edge_weight;
						if (!dijkstra_heap.WasInserted(to))
						{
							dijkstra_heap.Insert(to, to_distance, current);
							//SimpleLogger().Write(logDEBUG) << "discovering node " << to << " with distance " << to_distance;
						}
						// Found a shorter Path -> Update distance
						else if (to_distance < dijkstra_heap.GetKey(to))
						{
							// new parent
							dijkstra_heap.GetData(to).parent = current;
							dijkstra_heap.DecreaseKey(to, to_distance);
							//SimpleLogger().Write(logDEBUG) << "improving road to " << to << " with " << to_distance;
						}
					}
				}
				++setteled_nodes;
			}
		}

		BOOST_ASSERT_MSG(target != SPECIAL_NODEID, "edge_weight invalid");
		// Did we found anything ?
		if ( current != target )
		{
			raw_route_data.shortest_path_length = INVALID_EDGE_WEIGHT;
			return;
		}

		NodeID node = target;
		while (node != dijkstra_heap.GetData(node).parent)
		{
			path.emplace_back(node);
			node = dijkstra_heap.GetData(node).parent;
		}
		path.emplace_back(node);

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

		raw_route_data.unpacked_path_segments.resize(phantom_nodes_vector.size());
		super::UnpackPath(
					// -- packed input
					path,
					// -- start and end of route
					phantom_nodes_vector[0],
				// -- unpacked output
				raw_route_data.unpacked_path_segments[0]);

		raw_route_data.source_traversed_in_reverse.push_back(
					(path.front() != phantom_nodes_vector[0].source_phantom.forward_node_id));
		raw_route_data.target_traversed_in_reverse.push_back(
					(path.back() != phantom_nodes_vector[0].target_phantom.forward_node_id));
		raw_route_data.shortest_path_length = ( dijkstra_heap.GetKey(target) < 0 ) ? 0 :
																				  dijkstra_heap.GetKey(target);

		TIMER_STOP(query);
		SimpleLogger().Write() << "Qeury : " << TIMER_SEC(query) << " seconds";
	}
};

#endif /* DIJKSTRA_HPP */


#include "ridgeTraversalCenterlineExtraction.hpp"
#include <queue>
#include <vector>
#include <list>
#include "eigenanalysisOfHessian.hpp"
#include "timing.hpp"
#ifdef CPP11
#include <unordered_set>
using std::unordered_set;
#else
#include <boost/unordered_set.hpp>
using boost::unordered_set;
#endif

typedef struct point {
    float value;
    int x,y,z;
} point;

class PointComparison {
    public:
    bool operator() (const point &lhs, const point &rhs) const {
        return (lhs.value < rhs.value);
    }
};

float sign(float a) {
    return a < 0 ? -1.0f: 1.0f;
}

#define LPOS(a,b,c) (a)+(b)*(size.x)+(c)*(size.x*size.y)
#define POS(pos) pos.x+pos.y*size.x+pos.z*size.x*size.y
#define M(a,b,c) 1-sqrt(pow(T.Fx[a+b*size.x+c*size.x*size.y],2.0f) + pow(T.Fy[a+b*size.x+c*size.x*size.y],2.0f) + pow(T.Fz[a+b*size.x+c*size.x*size.y],2.0f))
#define SQR_MAG(pos) sqrt(pow(T.Fx[pos.x+pos.y*size.x+pos.z*size.x*size.y],2.0f) + pow(T.Fy[pos.x+pos.y*size.x+pos.z*size.x*size.y],2.0f) + pow(T.Fz[pos.x+pos.y*size.x+pos.z*size.x*size.y],2.0f))
#define SQR_MAG_SMALL(pos) sqrt(pow(T.FxSmall[pos.x+pos.y*size.x+pos.z*size.x*size.y],2.0f) + pow(T.FySmall[pos.x+pos.y*size.x+pos.z*size.x*size.y],2.0f) + pow(T.FzSmall[pos.x+pos.y*size.x+pos.z*size.x*size.y],2.0f))

char * runRidgeTraversal(TubeSegmentation &T, SIPL::int3 size, paramList &parameters, std::stack<CenterlinePoint> centerlineStack) {

    float Thigh = getParam(parameters, "tdf-high"); // 0.6
    int Dmin = getParam(parameters, "min-distance");
    float Mlow = getParam(parameters, "m-low"); // 0.2
    float Tlow = getParam(parameters, "tdf-low"); // 0.4
    int maxBelowTlow = getParam(parameters, "max-below-tdf-low"); // 2
    float minMeanTube = getParam(parameters, "min-mean-tdf"); //0.6
    int TreeMin = getParam(parameters, "min-tree-length"); // 200
    const int totalSize = size.x*size.y*size.z;

    int * centerlines = new int[totalSize]();
    INIT_TIMER

    // Create queue
    std::priority_queue<point, std::vector<point>, PointComparison> queue;

    START_TIMER
    // Collect all valid start points
    #pragma omp parallel for
    for(int z = 2; z < size.z-2; z++) {
        for(int y = 2; y < size.y-2; y++) {
            for(int x = 2; x < size.x-2; x++) {
                if(T.TDF[LPOS(x,y,z)] < Thigh)
                    continue;

                int3 pos(x,y,z);
                bool valid = true;
                for(int a = -1; a < 2; a++) {
                    for(int b = -1; b < 2; b++) {
                        for(int c = -1; c < 2; c++) {
                            int3 nPos(x+a,y+b,z+c);
                            if(SQR_MAG(nPos) < SQR_MAG(pos)) {
                                valid = false;
                                break;
                            }
                        }
                    }
                }

                if(valid) {
                    point p;
                    p.value = T.TDF[LPOS(x,y,z)];
                    p.x = x;
                    p.y = y;
                    p.z = z;
                    #pragma omp critical
                    queue.push(p);
                }
            }
        }
    }

    std::cout << "Processing " << queue.size() << " valid start points" << std::endl;
    if(queue.size() == 0) {
    	throw SIPL::SIPLException("no valid start points found", __LINE__, __FILE__);
    }
    STOP_TIMER("finding start points")
    START_TIMER
    int counter = 1;
    T.TDF[0] = 0;
    T.Fx[0] = 1;
    T.Fy[0] = 0;
    T.Fz[0] = 0;

    // Create a map of centerline distances
    unordered_map<int, int> centerlineDistances;

    // Create a map of centerline stacks
    unordered_map<int, std::stack<CenterlinePoint> > centerlineStacks;

    while(!queue.empty()) {
        // Traverse from new start point
        point p = queue.top();
        queue.pop();

        // Has it been handled before?
        if(centerlines[LPOS(p.x,p.y,p.z)] == 1)
            continue;

        unordered_set<int> newCenterlines;
        newCenterlines.insert(LPOS(p.x,p.y,p.z));
        int distance = 1;
        int connections = 0;
        int prevConnection = -1;
        int secondConnection = -1;
        float meanTube = T.TDF[LPOS(p.x,p.y,p.z)];

        // Create new stack for this centerline
        std::stack<CenterlinePoint> stack;
        CenterlinePoint startPoint;
        startPoint.pos.x = p.x;
        startPoint.pos.y = p.y;
        startPoint.pos.z = p.z;

        stack.push(startPoint);

        // For each direction
        for(int direction = -1; direction < 3; direction += 2) {
            int belowTlow = 0;
            int3 position(p.x,p.y,p.z);
            float3 t_i = getTubeDirection(T, position, size);
            t_i.x *= direction;
            t_i.y *= direction;
            t_i.z *= direction;
            float3 t_i_1;
            t_i_1.x = t_i.x;
            t_i_1.y = t_i.y;
            t_i_1.z = t_i.z;

            // Traverse
            while(true) {
                int3 maxPoint(0,0,0);

                // Check for out of bounds
                if(position.x < 3 || position.x > size.x-3 || position.y < 3 || position.y > size.y-3 || position.z < 3 || position.z > size.z-3)
                    break;

                // Try to find next point from all neighbors
                for(int a = -1; a < 2; a++) {
                    for(int b = -1; b < 2; b++) {
                        for(int c = -1; c < 2; c++) {
                            int3 n(position.x+a,position.y+b,position.z+c);
                            if((a == 0 && b == 0 && c == 0) || T.TDF[POS(n)] == 0.0f)
                                continue;

                            float3 dir((float)(n.x-position.x),(float)(n.y-position.y),(float)(n.z-position.z));
                            dir = dir.normalize();
                            if( (dir.x*t_i.x+dir.y*t_i.y+dir.z*t_i.z) <= 0.1)
                                continue;

                            if(T.radius[POS(n)] >= 1.5f) {
                                if(M(n.x,n.y,n.z) > M(maxPoint.x,maxPoint.y,maxPoint.z))
                                maxPoint = n;
                            } else {
                                if(T.TDF[LPOS(n.x,n.y,n.z)]*M(n.x,n.y,n.z) > T.TDF[POS(maxPoint)]*M(maxPoint.x,maxPoint.y,maxPoint.z))
                                maxPoint = n;
                            }

                        }
                    }
                }

                if(maxPoint.x+maxPoint.y+maxPoint.z > 0) {
                    // New maxpoint found, check it!
                    if(centerlines[LPOS(maxPoint.x,maxPoint.y,maxPoint.z)] > 0) {
                        // Hit an existing centerline
                        if(prevConnection == -1) {
                            prevConnection = centerlines[LPOS(maxPoint.x,maxPoint.y,maxPoint.z)];
                        } else {
                            if(prevConnection ==centerlines[LPOS(maxPoint.x,maxPoint.y,maxPoint.z)]) {
                                // A loop has occured, reject this centerline
                                connections = 5;
                            } else {
                                secondConnection = centerlines[LPOS(maxPoint.x,maxPoint.y,maxPoint.z)];
                            }
                        }
                        break;
                    } else if(M(maxPoint.x,maxPoint.y,maxPoint.z) < Mlow || (belowTlow > maxBelowTlow && T.TDF[LPOS(maxPoint.x,maxPoint.y,maxPoint.z)] < Tlow)) {
                        // New point is below thresholds
                        break;
                    } else if(newCenterlines.count(LPOS(maxPoint.x,maxPoint.y,maxPoint.z)) > 0) {
                        // Loop detected!
                        break;
                    } else {
                        // Point is OK, proceed to add it and continue
                        if(T.TDF[LPOS(maxPoint.x,maxPoint.y,maxPoint.z)] < Tlow) {
                            belowTlow++;
                        } else {
                            belowTlow = 0;
                        }

                        // Update direction
                        //float3 e1 = getTubeDirection(T, maxPoint,size.x,size.y,size.z);

                        //TODO: check if all eigenvalues are negative, if so find the egeinvector that best matches
                        float3 lambda, e1, e2, e3;
                        doEigen(T, maxPoint, size, &lambda, &e1, &e2, &e3);
                        if((lambda.x < 0 && lambda.y < 0 && lambda.z < 0)) {
                            if(fabs(t_i.dot(e3)) > fabs(t_i.dot(e2))) {
                                if(fabs(t_i.dot(e3)) > fabs(t_i.dot(e1))) {
                                    e1 = e3;
                                }
                            } else if(fabs(t_i.dot(e2)) > fabs(t_i.dot(e1))) {
                                e1 = e2;
                            }
                        }

                        float maintain_dir = sign(e1.dot(t_i));
                        float3 vec_sum;
                        vec_sum.x = maintain_dir*e1.x + t_i.x + t_i_1.x;
                        vec_sum.y = maintain_dir*e1.y + t_i.y + t_i_1.y;
                        vec_sum.z = maintain_dir*e1.z + t_i.z + t_i_1.z;
                        vec_sum = vec_sum.normalize();
                        t_i_1 = t_i;
                        t_i = vec_sum;

                        // update position
                        position = maxPoint;
                        distance ++;
                        newCenterlines.insert(LPOS(maxPoint.x,maxPoint.y,maxPoint.z));
                        meanTube += T.TDF[LPOS(maxPoint.x,maxPoint.y,maxPoint.z)];

                        // Create centerline point
                        CenterlinePoint p;
                        p.pos = position;
                        p.next = &(stack.top()); // add previous
                        if(T.radius[POS(p.pos)] > 3.0f) {
                            p.large = true;
                        } else {
                            p.large = false;
                        }

                        // Add point to stack
                        stack.push(p);
                    }
                } else {
                    // No maxpoint found, stop!
                    break;
                }

            } // End traversal
        } // End for each direction

        // Check to see if new traversal can be added
        //std::cout << "Finished. Distance " << distance << " meanTube: " << meanTube/distance << std::endl;
        if(distance > Dmin && meanTube/distance > minMeanTube && connections < 2) {
            //std::cout << "Finished. Distance " << distance << " meanTube: " << meanTube/distance << std::endl;
            //std::cout << "------------------- New centerlines added #" << counter << " -------------------------" << std::endl;

            unordered_set<int>::iterator usit;
            if(prevConnection == -1) {
                // No connections
                for(usit = newCenterlines.begin(); usit != newCenterlines.end(); usit++) {
                    centerlines[*usit] = counter;
                }
                centerlineDistances[counter] = distance;
                centerlineStacks[counter] = stack;
                counter ++;
            } else {
                // The first connection

                std::stack<CenterlinePoint> prevConnectionStack = centerlineStacks[prevConnection];
                while(!stack.empty()) {
                    prevConnectionStack.push(stack.top());
                    stack.pop();
                }

                for(usit = newCenterlines.begin(); usit != newCenterlines.end(); usit++) {
                    centerlines[*usit] = prevConnection;
                }
                centerlineDistances[prevConnection] += distance;
                if(secondConnection != -1) {
                    // Two connections, move secondConnection to prevConnection
                    std::stack<CenterlinePoint> secondConnectionStack = centerlineStacks[secondConnection];
                    centerlineStacks.erase(secondConnection);
                    while(!secondConnectionStack.empty()) {
                        prevConnectionStack.push(secondConnectionStack.top());
                        secondConnectionStack.pop();
                    }

                    #pragma omp parallel for
                    for(int i = 0; i < totalSize;i++) {
                        if(centerlines[i] == secondConnection)
                            centerlines[i] = prevConnection;
                    }
                    centerlineDistances[prevConnection] += centerlineDistances[secondConnection];
                    centerlineDistances.erase(secondConnection);
                }

                centerlineStacks[prevConnection] = prevConnectionStack;
            }
        } // end if new point can be added
    } // End while queue is not empty
    std::cout << "Finished traversal" << std::endl;
    STOP_TIMER("traversal")
    START_TIMER

    if(centerlineDistances.size() == 0) {
        //throw SIPL::SIPLException("no centerlines were extracted");
        char * returnCenterlines = new char[totalSize]();
        return returnCenterlines;
    }

    // Find largest connected tree and all trees above a certain size
    unordered_map<int, int>::iterator it;
    int max = centerlineDistances.begin()->first;
    std::list<int> trees;
    for(it = centerlineDistances.begin(); it != centerlineDistances.end(); it++) {
        if(it->second > centerlineDistances[max])
            max = it->first;
        if(it->second > TreeMin)
            trees.push_back(it->first);
    }
    std::list<int>::iterator it2;
    // TODO: if use the method with TreeMin have to add them to centerlineStack also
    centerlineStack = centerlineStacks[max];
    for(it2 = trees.begin(); it2 != trees.end(); it2++) {
        while(!centerlineStacks[*it2].empty()) {
            centerlineStack.push(centerlineStacks[*it2].top());
            centerlineStacks[*it2].pop();
        }
    }

    char * returnCenterlines = new char[totalSize]();
    // Mark largest tree with 1, and rest with 0
    #pragma omp parallel for
    for(int i = 0; i < totalSize;i++) {
        if(centerlines[i] == max) {
        //if(centerlines[i] > 0) {
            returnCenterlines[i] = 1;
        } else {
            bool valid = false;
            for(it2 = trees.begin(); it2 != trees.end(); it2++) {
                if(centerlines[i] == *it2) {
                    returnCenterlines[i] = 1;
                    valid = true;
                    break;
                }
            }
            if(!valid)
                returnCenterlines[i] = 0;

        }
    }
    STOP_TIMER("finding largest tree")

	delete[] centerlines;
    return returnCenterlines;
}


// train.cpp
// Copyright Laurence Emms 2017

#include <cmath>
#include <iostream>
#include <array>
#include <numeric>
#include <algorithm>
#include <list>
#include <random>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
#include <opencv2/opencv.hpp>

#include <mlpclassifier.h>

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

template <typename Classifier>
bool train(Classifier& classifier,
           const std::vector<bool>& marked,
           const std::vector<int>& subset,
           const std::string& input_path,
           const int w,
           const int h,
           const int f,
           const bool verbose)
{
    if (subset.empty())
    {
        std::cerr << "Subset is empty\n";
        return false;
    }
    cv::VideoCapture cap(input_path);
    if (!cap.isOpened())
    {
        std::cerr << "Failed to open video capture\n";
        return false;
    }

    const double fourcc_d = cap.get(CV_CAP_PROP_FOURCC);
    const char* fourcc = reinterpret_cast<const char*>(&fourcc_d);
    std::cout << "FourCC: " << fourcc << "\n";
    int frame_width = static_cast<int>(cap.get(CV_CAP_PROP_FRAME_WIDTH));
    int frame_height = static_cast<int>(cap.get(CV_CAP_PROP_FRAME_HEIGHT));
    std::cout << "Frame width: " << frame_width << "\n";
    std::cout << "Frame height: " << frame_height << "\n";
    std::cout << "FPS: " << static_cast<int>(cap.get(CV_CAP_PROP_FPS)) << "\n";
    std::cout << "Frame format: " << static_cast<int>(cap.get(CV_CAP_PROP_FORMAT)) << "\n";
    std::cout << "ISO Speed: " << static_cast<int>(cap.get(CV_CAP_PROP_ISO_SPEED)) << "\n";

    // count frames
    cap.set(CV_CAP_PROP_POS_AVI_RATIO, 1);
    int frame_count = static_cast<int>(cap.get(CV_CAP_PROP_POS_FRAMES));
    cap.set(CV_CAP_PROP_POS_AVI_RATIO, 0);
    std::cout << "Frame count (approx): " << frame_count << "\n";

    std::list<cv::Mat> prev_frames;
    int subset_index = 0;
    for (int fn = 0; fn < frame_count; ++fn)
    {
        cv::Mat frame;
        if (!cap.read(frame))
        {
            std::cout << "Frame empty: "<< fn << "\n";
            continue;
        }
        prev_frames.push_front(frame.clone());
        if (fn == 0)
        {
            // preload f frames
            for (int i = 0; i < f - 1; ++i)
            {
                prev_frames.push_front(frame.clone());
            }
        }
        while (static_cast<int>(prev_frames.size()) > f)
        {
            prev_frames.pop_back();
        }
        if (fn <= subset[subset_index])
        {
            continue;
        }
        if (verbose)
        {
            std::cout << "Frame number: " << fn << " / " << frame_count << "\n";
            double msec = cap.get(CV_CAP_PROP_POS_MSEC);
            double seconds = std::floor(msec / 1000.0);
            double minutes = std::floor(seconds / 60.0);
            double hours = std::floor(minutes / 60.0);
            minutes -= hours * 60.0;
            seconds -= minutes * 60.0;
            msec -= seconds * 1000.0;
            std::cout << "Time: " << std::setfill('0') << std::setw(2) << static_cast<int>(hours) << ":" << std::setw(2) << static_cast<int>(minutes) << ":" << std::setw(2) << static_cast<int>(seconds) << ":" << std::setw(4) << static_cast<int>(msec) << "\n";
        }
        float target_value = 0.0f;
        if (marked[fn])
        {
            target_value = 1.0f;
        }
        std::vector<float> target_vec;
        target_vec.push_back(target_value);
        int w_offset = w / 2;
        int h_offset = h / 2;
        int fw = frame.cols;
        int fh = frame.rows;

        std::vector<float> input_vector(w * f * h + 1);
        input_vector[w * h * f] = -1.0f; // bias node
        int stride = w;
        for (int y = h_offset; y + h_offset < fh; y += stride)
        {
            for (int x = w_offset; x + w_offset < fw; x += stride)
            {
                std::list<cv::Mat>::iterator current_frame = prev_frames.begin();

                int it_f = 0;
                while (current_frame != prev_frames.end())
                {
                    int it_y = 0;
                    for (int y_o = -h_offset; y_o < h_offset; ++y_o)
                    {
                        int it_x = 0;
                        for (int x_o = -w_offset; x_o < w_offset; ++x_o)
                        {
                            cv::Vec3b texel = current_frame->at<cv::Vec3b>(y + y_o, x + x_o);
                            float luminance = (0.2126f * static_cast<float>(texel[0]) + 0.7512f * static_cast<float>(texel[1]) + 0.0722f * static_cast<float>(texel[2])) / 255.0f;
                            input_vector[it_x + it_y * w + it_f * w * h] = luminance;
                            it_x++;
                        }
                        it_y++;
                    }
                    current_frame++;
                    it_f++;
                }

                classifier.train(input_vector, target_vec);
            }
        }
        if (verbose)
        {
            std::cout << "Frame trained\n";
        }
        subset_index++;
    }
    cap.release();
    std::cout << "Training complete\n";
    return true;
}

int main(int argc, char** argv)
{
    std::srand(std::time(0));
    std::cout << "Train\n";
    std::cout << "by Laurence Emms\n";

    po::options_description desc("Options");
    desc.add_options()
        ("help,h", "Print help message")
        ("version,v", "Print version number")
        ("input,i", po::value<std::string>(), "Input video file")
        ("classifier,c", po::value<std::string>(), "Classifier file")
        ("marked", po::value<std::string>(), "Marked frames file")
        ("subset", po::value<std::string>(), "Subset file")
        ("verbose", "Force verbose output")
        ;
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help"))
    {
        std::cout << desc << "\n";
        return 0;
    }

    if (vm.count("version"))
    {
        std::cout << "Train 1.0\n";
        return 0;
    }

    if (vm.count("input") == 0)
    {
        std::cerr << "Input file not specified\n";
        return 1;
    }

    if (vm.count("classifier") == 0)
    {
        std::cerr << "Classifier file not specified\n";
        return 1;
    }

    if (vm.count("marked") == 0)
    {
        std::cerr << "Marked file not specified\n";
        return 1;
    }

    if (vm.count("subset") == 0)
    {
        std::cerr << "Subset file not specified\n";
        return 1;
    }

    fs::path input_path(vm["input"].as<std::string>());
    fs::path classifier_path(vm["classifier"].as<std::string>());
    fs::path marked_path(vm["marked"].as<std::string>());
    fs::path subset_path(vm["subset"].as<std::string>());

    if (!fs::exists(input_path.string()))
    {
        std::cerr << "Input file does not exist: " << input_path.string() << "\n";
        return 1;
    }

    if (!fs::exists(marked_path.string()))
    {
        std::cerr << "Marked file does not exist: " << marked_path.string() << "\n";
        return 1;
    }

    std::cout << "Reading input file: " << input_path.string() << "\n";
    cv::VideoCapture cap(input_path.string());
    if (!cap.isOpened())
    {
        std::cerr << "Failed to open video capture\n";
        return 1;
    }

    int w = 8;
    int h = 8;
    int f = 4;

    classifiers::MLPClassifier classifier;
    if (fs::exists(classifier_path.string()))
    {
        std::cout << "Reading classifier file: " << classifier_path.string() << "\n";
        std::ifstream classifier_file(classifier_path.string().c_str());
        classifier.read(classifier_file);
        int layers = classifier.num_layers();
        if (layers <= 0)
        {
            std::cerr << "Error: Classifier has no layers\n";
            return -1;
        }
        std::cout << "Read classifier with " << layers << " layers\n";
        for (int l = 0; l < layers; ++l)
        {
            std::cout << l << ": " << classifier.layer_size(l) << "\n";
        }
    }
    else
    {
        std::vector<int> layer_sizes;
        layer_sizes.push_back(w * h * f + 1);
        layer_sizes.push_back(w * h * f + 1);
        layer_sizes.push_back(w * h * f + 1);
        layer_sizes.push_back(1);
        classifier.init(layer_sizes);
    }

    std::cout << "Input format:\n";
    const int fourcc_i = static_cast<int>(cap.get(CV_CAP_PROP_FOURCC));
    const char* fourcc = reinterpret_cast<const char*>(&fourcc_i);
    std::cout << "FourCC: " << fourcc << "\n";
    int frame_width = static_cast<int>(cap.get(CV_CAP_PROP_FRAME_WIDTH));
    int frame_height = static_cast<int>(cap.get(CV_CAP_PROP_FRAME_HEIGHT));
    std::cout << "Frame width: " << frame_width << "\n";
    std::cout << "Frame height: " << frame_height << "\n";
    double fps = cap.get(CV_CAP_PROP_FPS);
    std::cout << "FPS: " << fps << "\n";
    double estimated_frame_count = cap.get(CV_CAP_PROP_FRAME_COUNT);
    std::cout << "Estimated frame count: " << estimated_frame_count << "\n";
    std::cout << "Frame format: " << static_cast<int>(cap.get(CV_CAP_PROP_FORMAT)) << "\n";
    std::cout << "ISO Speed: " << static_cast<int>(cap.get(CV_CAP_PROP_ISO_SPEED)) << "\n";

    // count frames
    cap.set(CV_CAP_PROP_POS_AVI_RATIO, 1);
    int frame_count = static_cast<int>(cap.get(CV_CAP_PROP_POS_FRAMES));
    cap.set(CV_CAP_PROP_POS_AVI_RATIO, 0);
    std::cout << "Frame count (approx): " << frame_count << "\n";
    cap.release();

    std::cout << "Reading marked data from: " << marked_path.string() << "\n";
    std::vector<bool> marked(frame_count, false);
    if (fs::exists(marked_path))
    {
        std::ifstream marked_file(marked_path.string().c_str());
        int frame = 0;
        while (marked_file >> frame)
        {
            marked[frame] = true;
        }
    }

    // choose a subset of the frames to train with
    float subset_percentage = 0.1f;
    size_t subset_size = static_cast<size_t>(static_cast<float>(frame_count) * subset_percentage);
    std::vector<int> indices(frame_count);
    std::iota(indices.begin(), indices.end(), 0);
    std::random_shuffle(indices.begin(), indices.end());
    std::vector<int> subset(subset_size);
    std::copy(indices.begin(), indices.begin() + subset_size, subset.begin());
    std::sort(subset.begin(), subset.end());

    bool verbose = vm.count("verbose") != 0;

    std::cout << "Training on input file: " << input_path.string() << "\n";
    if (!train(classifier,
               marked,
               subset,
               input_path.string(),
               w,
               h,
               f,
               verbose))
    {
        std::cerr << "Failed to train on video: " << input_path.string() << "\n";
        return 1;
    }

    std::cout << "Writing classifier data to: " << marked_path.string() << "\n";
    std::ofstream classifier_file(classifier_path.string().c_str());
    classifier.write(classifier_file);

    std::cout << "Finished training on video: " << input_path.string() << "\n";

    cv::waitKey(0);

    return 0;
}