//------------------------------------------------------------------------------
/// \file
/// \brief
/// \copyright (C) Copyright Aquaveo 2018. Distributed under FreeBSD License
/// (See accompanying file LICENSE or https://aqaveo.com/bsd/license.txt)
//------------------------------------------------------------------------------

//----- Included files ---------------------------------------------------------
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <boost/shared_ptr.hpp>
#include <xmscore/python/misc/PyUtils.h>
#include <xmscore/misc/DynBitset.h>
#include <xmscore/stl/vector.h>
#include <xmsgrid/ugrid/XmUGrid.h>
#include <xmsgridtrace/gridtrace/XmGridTrace.h>
#include <xmsextractor/extractor/XmUGrid2dDataExtractor.h>

//----- Namespace declaration --------------------------------------------------
namespace py = pybind11;

//----- Python Interface -------------------------------------------------------
PYBIND11_DECLARE_HOLDER_TYPE(T, boost::shared_ptr<T>);

void initXmGridTrace(py::module &m) {
  const char* XmGridTrace_doc = R"pydoc(
        Class in which the flow trace of a point between 2 velocity vector time
        steps on an XmGrid is calculated and the velocity vectors can be
        assigned to either the points or to the cells.
    )pydoc";
    py::class_<xms::XmGridTrace, boost::shared_ptr<xms::XmGridTrace>> gridtrace(
      m, "GridTrace", XmGridTrace_doc);

  // ---------------------------------------------------------------------------
  // function: __init__
  // ---------------------------------------------------------------------------
  gridtrace.def(py::init([](boost::shared_ptr<xms::XmUGrid> ugrid,
                py::object vector_multiplier, py::object max_tracing_time,
                py::object max_tracing_distance, py::object min_delta_time,
                py::object max_change_distance, py::object max_change_velocity,
                py::object max_change_direction_in_radians)
      {
        boost::shared_ptr<xms::XmGridTrace> rval(xms::XmGridTrace::New(ugrid));
        if (!vector_multiplier.is_none())
        {
          rval->SetVectorMultiplier(vector_multiplier.cast<double>());
        }
        if (!max_tracing_time.is_none())
        {
          rval->SetMaxTracingTime(max_tracing_time.cast<double>());
        }
        if (!max_tracing_distance.is_none())
        {
          rval->SetMaxTracingDistance(max_tracing_distance.cast<double>());
        }
        if (!min_delta_time.is_none())
        {
          rval->SetMinDeltaTime(min_delta_time.cast<double>());
        }
        if (!max_change_distance.is_none())
        {
          rval->SetMaxChangeDistance(max_change_distance.cast<double>());
        }
        if (!max_change_velocity.is_none())
        {
          rval->SetMaxChangeVelocity(max_change_velocity.cast<double>());
        }
        if (!max_change_direction_in_radians.is_none())
        {
          rval->SetMaxChangeDirectionInRadians(max_change_direction_in_radians.cast<double>());
        }
        return rval;
      }), py::arg("ugrid"), py::arg("vector_multiplier") = py::none(), py::arg("max_tracing_time") = py::none(),
          py::arg("max_tracing_distance") = py::none(), py::arg("min_delta_time") = py::none(),
          py::arg("max_change_distance") = py::none(), py::arg("max_change_velocity") = py::none(),
          py::arg("max_change_direction_in_radians") = py::none());

  // -------------------------------------------------------------------------
  // function: __repr__
  // -------------------------------------------------------------------------
  gridtrace.def("__repr__", [](xms::XmGridTrace &self) {
    std::stringstream ss;
    ss << "vector_multiplier: " << self.GetVectorMultiplier() << "\n";
    ss << "max_tracing_time: " << self.GetMaxTracingTime() << "\n";
    ss << "max_tracing_distance: " << self.GetMaxTracingDistance() << "\n";
    ss << "min_delta_time: " << self.GetMinDeltaTime() << "\n";
    ss << "max_change_distance: " << self.GetMaxChangeDistance() << "\n";
    ss << "max_change_velocity: " << self.GetMaxChangeVelocity() << "\n";
    ss << "max_change_direction_in_radians: " << self.GetMaxChangeDirectionInRadians();
    return ss.str();
  });

  // ---------------------------------------------------------------------------
  // property: get_vector_multiplier
  // ---------------------------------------------------------------------------
  const char* vector_multiplier_doc = R"pydoc(
      The vector multiplier.
  )pydoc";
  gridtrace.def_property("vector_multiplier",
      [](xms::XmGridTrace &self) -> double
      {
        return self.GetVectorMultiplier();
      },
      [](xms::XmGridTrace &self, double vector_multiplier)
      {
        self.SetVectorMultiplier(vector_multiplier);
      },
      vector_multiplier_doc);

  // ---------------------------------------------------------------------------
  // property: max_tracing_time
  // ---------------------------------------------------------------------------
  const char* max_tracing_time_doc = R"pydoc(
      The max tracing time.
  )pydoc";
  gridtrace.def_property("max_tracing_time",
      [](xms::XmGridTrace &self) -> double
      {
        return self.GetMaxTracingTime();
      },
      [](xms::XmGridTrace &self, double max_tracing_time)
      {
        self.SetMaxTracingTime(max_tracing_time);
      },
      max_tracing_time_doc);

  // ---------------------------------------------------------------------------
  // property: max_tracing_distance
  // ---------------------------------------------------------------------------
  const char* max_tracing_distance_doc = R"pydoc(
      The max tracing distance.
  )pydoc";
  gridtrace.def_property("max_tracing_distance",
      [](xms::XmGridTrace &self) -> double
      {
        return self.GetMaxTracingDistance();
      },
      [](xms::XmGridTrace &self, double max_tracing_distance)
      {
        self.SetMaxTracingDistance(max_tracing_distance);
      },
      max_tracing_distance_doc);

  // ---------------------------------------------------------------------------
  // property: min_delta_time
  // ---------------------------------------------------------------------------
  const char* min_delta_time_doc = R"pydoc(
      The min delta time.
  )pydoc";
  gridtrace.def_property("min_delta_time",
      [](xms::XmGridTrace &self) -> double
      {
        return self.GetMinDeltaTime();
      },
      [](xms::XmGridTrace &self, double min_delta_time)
      {
        self.SetMinDeltaTime(min_delta_time);
      },
      min_delta_time_doc);

  // ---------------------------------------------------------------------------
  // property: max_change_distance
  // ---------------------------------------------------------------------------
  const char* max_change_distance_doc = R"pydoc(
      The max change distance.
  )pydoc";
  gridtrace.def_property("max_change_distance",
      [](xms::XmGridTrace &self) -> double
      {
        return self.GetMaxChangeDistance();
      },
      [](xms::XmGridTrace &self, double max_change_distance)
      {
        self.SetMaxChangeDistance(max_change_distance);
      },
      max_change_distance_doc);

  // ---------------------------------------------------------------------------
  // property: max_change_velocity
  // ---------------------------------------------------------------------------
  const char* max_change_velocity_doc = R"pydoc(
      The max change velocity.
  )pydoc";
  gridtrace.def_property("max_change_velocity",
      [](xms::XmGridTrace &self) -> double
      {
        return self.GetMaxChangeVelocity();
      },
      [](xms::XmGridTrace &self, double max_change_velocity)
      {
        self.SetMaxChangeVelocity(max_change_velocity);
      },
      max_change_velocity_doc);

  // ---------------------------------------------------------------------------
  // property: max_change_direction_in_radians
  // ---------------------------------------------------------------------------
  const char* max_change_direction_in_radians_doc = R"pydoc(
      The max change direction in radians.
  )pydoc";
  gridtrace.def_property("max_change_direction_in_radians",
      [](xms::XmGridTrace &self) -> double
      {
        return self.GetMaxChangeDirectionInRadians();
      },
      [](xms::XmGridTrace &self, double max_change_direction_in_radians)
      {
        self.SetMaxChangeDirectionInRadians(max_change_direction_in_radians);
      },
      max_change_direction_in_radians_doc);

  // ---------------------------------------------------------------------------
  // function: add_grid_scalars_at_time
  // ---------------------------------------------------------------------------
  const char* add_grid_scalars_at_time_doc = R"pydoc(
      Assigns velocity vectors to each point or cell for a time step,
      keeping the previous step, and dropping the one before that
      for a maximum of two time steps.

      Args:
          scalars (iterable): The velocity vectors.

          scalar_loc (string): Whether the vectors are assigned to cells or points.

          cell_activity (iterable): Whether each cell or point is active.

          activity_loc (string): Whether the activities are assigned to cells or points.

          time (float): The time of the scalars.
  )pydoc";
  gridtrace.def("add_grid_scalars_at_time", [](xms::XmGridTrace &self,
          py::iterable vel_scalars,
          std::string scalar_loc,
          py::iterable cell_activity,
          std::string activity_loc,
          double time) {

            xms::DataLocationEnum scalar_loc_e;
            if (scalar_loc == "points")
              scalar_loc_e = xms::DataLocationEnum::LOC_POINTS;
            else if (scalar_loc == "cells")
              scalar_loc_e = xms::DataLocationEnum::LOC_CELLS;
            else if (scalar_loc == "unknown")
              scalar_loc_e = xms::DataLocationEnum::LOC_UNKNOWN;
            else
            {
              std::string msg = "nodal_func_type string must be one of 'points', 'cells', "
                                "'unknown' not " + scalar_loc;
              throw py::value_error(msg);
            }

            xms::DataLocationEnum activity_loc_e;
            if (activity_loc == "points")
              activity_loc_e = xms::DataLocationEnum::LOC_POINTS;
            else if (activity_loc == "cells")
              activity_loc_e = xms::DataLocationEnum::LOC_CELLS;
            else if (activity_loc == "unknown")
              activity_loc_e = xms::DataLocationEnum::LOC_UNKNOWN;
            else
            {
              std::string msg = "nodal_func_type string must be one of 'points', 'cells', "
                                "'unknown' not " + activity_loc;
              throw py::value_error(msg);
            }

            boost::shared_ptr<xms::VecPt3d> scalars =
              xms::VecPt3dFromPyIter(vel_scalars);
            xms::DynBitset activity = xms::DynamicBitsetFromPyIter(cell_activity);
            self.AddGridScalarsAtTime(*scalars, scalar_loc_e, activity,
              activity_loc_e, time);
            return;
          }, add_grid_scalars_at_time_doc, py::arg("scalars"),
          py::arg("scalar_loc"), py::arg("cell_activity"), py::arg("activity_loc"),
           py::arg("time"));
  // ---------------------------------------------------------------------------
  // function: trace_point
  // ---------------------------------------------------------------------------
  const char* trace_point_doc = R"pydoc(
      Runs the Grid Trace for a point.

      Args:
          pt (iterable): The starting point of the trace.

          pt_time (float): The starting time of the trace.

      Returns:
          iterable: Contains the resultant positions at each step and the resultant times at each step.
  )pydoc";
  gridtrace.def("trace_point", [](xms::XmGridTrace &self, py::iterable pt,
    double pt_time) -> py::iterable {
          xms::Pt3d point = xms::Pt3dFromPyIter(pt);
          xms::VecPt3d outTrace;
          xms::VecDbl outTimes;
          self.TracePoint(point, pt_time, outTrace, outTimes);
          py::iterable resultTrace = xms::PyIterFromVecPt3d(outTrace);
          py::iterable resultTimes = xms::PyIterFromVecDbl(outTimes);
          return py::make_tuple(resultTrace, resultTimes);
        }, trace_point_doc, py::arg("pt"), py::arg("pt_time"));
  // ---------------------------------------------------------------------------
  // function: get_exit_message
  // ---------------------------------------------------------------------------
  const char* get_exit_message_doc = R"pydoc(
      Returns a message describing what caused trace to exit.

      Returns:
          str: The exit message of the last TracePoint operation.
  )pydoc";
  gridtrace.def("get_exit_message", &xms::XmGridTrace::GetExitMessage,
    get_exit_message_doc);

    // DataLocationEnum
    py::enum_<xms::DataLocationEnum>(m, "data_location_enum",
                    "data_location_enum location mapping for dataset values")
        .value("LOC_POINTS", xms::LOC_POINTS)
        .value("LOC_CELLS", xms::LOC_CELLS)
        .value("LOC_UNKNOWN", xms::LOC_UNKNOWN);
}


// Copyright Louis Dionne 2013-2017
// 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/hana/ap.hpp>
#include <boost/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/optional.hpp>
#include <boost/hana/tuple.hpp>

#include <functional>
namespace hana = boost::hana;

int main() {
    // with tuples
    static_assert(
        hana::ap(hana::make_tuple(std::plus<>{}), hana::make_tuple(1, 2),
                                                  hana::make_tuple(3, 4, 5))
            ==
        hana::make_tuple(
            1 + 3,      1 + 4,      1 + 5,
            2 + 3,      2 + 4,      2 + 5
        )
    , "");

    // with optional values
    BOOST_HANA_CONSTEXPR_LAMBDA auto multiply = [](auto a, auto b, auto c) {
        return a * b * c;
    };

    BOOST_HANA_CONSTEXPR_CHECK(
        hana::ap(hana::just(multiply), hana::just(1),
                                       hana::just(2),
                                       hana::just(3))
            ==
        hana::just(1 * 2 * 3)
    );

    BOOST_HANA_CONSTANT_CHECK(
        hana::ap(hana::just(multiply), hana::just(1),
                                       hana::nothing,
                                       hana::just(3))
            ==
        hana::nothing
    );
}


#include "Scene_polyhedron_item.h"
#include "Polyhedron_type.h"
#include "Scene_surface_mesh_item.h"

#include <QMainWindow>
#include <QObject>
#include <CGAL/Three/Polyhedron_demo_io_plugin_interface.h>
#include <CGAL/Three/Polyhedron_demo_plugin_interface.h>
#include <CGAL/Three/Polyhedron_demo_plugin_helper.h>
#include <CGAL/Three/Scene_group_item.h>

#include <CGAL/IO/read_surf_trianglemesh.h>

#include <CGAL/Bbox_3.h>
#include <CGAL/array.h>

#include "Color_map.h"
#include <fstream>
#include <boost/container/flat_set.hpp>

using namespace CGAL::Three;
class Surf_io_plugin:
    public QObject,
    public Polyhedron_demo_io_plugin_interface,
    public Polyhedron_demo_plugin_helper
{
  Q_OBJECT
  Q_INTERFACES(CGAL::Three::Polyhedron_demo_plugin_interface CGAL::Three::Polyhedron_demo_io_plugin_interface)
  Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.PluginInterface/1.0")
  Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.IOPluginInterface/1.0")

public:
  void init(QMainWindow* mainWindow,
            CGAL::Three::Scene_interface* scene_interface,
            Messages_interface*) {
    //get the references
    this->scene = scene_interface;
    this->mw = mainWindow;
  }
  QList<QAction*> actions() const {
    return QList<QAction*>();
  }
  bool applicable(QAction*) const { return false;}
  QString name() const { return "surf_io_plugin"; }
  QString nameFilters() const { return "Amira files (*.surf)"; }
  bool canLoad() const{ return true; }
  template<class FaceGraphItem>
  CGAL::Three::Scene_item* actual_load(QFileInfo fileinfo);
  CGAL::Three::Scene_item* load(QFileInfo fileinfo);

  bool canSave(const CGAL::Three::Scene_item*) { return false; }
  bool save(const CGAL::Three::Scene_item*, QFileInfo) { return false; }
};

CGAL::Three::Scene_item* Surf_io_plugin::load(QFileInfo fileinfo)
{
  if(mw->property("is_polyhedron_mode").toBool())
    return actual_load<Scene_polyhedron_item>(fileinfo);
  else
    return actual_load<Scene_surface_mesh_item>(fileinfo);
}
template< class FaceGraphItem>
CGAL::Three::Scene_item* Surf_io_plugin::actual_load(QFileInfo fileinfo)
{
  typedef typename FaceGraphItem::Face_graph FaceGraph;
  typedef typename boost::property_traits<
      typename boost::property_map<FaceGraph, boost::vertex_point_t>::type
      >::value_type Point_3;
  // Open file
  std::ifstream in(fileinfo.filePath().toUtf8());
  if(!in) {
    std::cerr << "Error! Cannot open file " << (const char*)fileinfo.filePath().toUtf8() << std::endl;
    return NULL;
  }

  std::vector<FaceGraph> patches;
  std::vector<MaterialData> material_data;
  CGAL::Bbox_3 grid_box;
  CGAL::cpp11::array<unsigned int, 3> grid_size = {{1, 1, 1}};
  boost::container::flat_set<Point_3> duplicated_points;
  read_surf(in, patches, material_data, grid_box, grid_size
          , std::inserter(duplicated_points, duplicated_points.end()));

  for(std::size_t i=0; i<material_data.size(); ++i)
  {
   std::cout<<"The patch #"<<i<<":\n  -inner region : material's id = "<<material_data[i].innerRegion.first<<" material's name = "
           <<material_data[i].innerRegion.second<<"\n  -outer region: material's id = "<<material_data[i].outerRegion.first<<" material's name = "
             <<material_data[i].outerRegion.second<<std::endl;
  }
  if (!duplicated_points.empty())
    std::cout << duplicated_points.size() << " points have been duplicated." << std::endl;

  std::vector<QColor> colors_;
  compute_color_map(QColor(100, 100, 255), static_cast<unsigned>(patches.size()),
                    std::back_inserter(colors_));
  Scene_group_item* group = new Scene_group_item(fileinfo.completeBaseName());
  for(std::size_t i=0; i<patches.size(); ++i)
  {
    FaceGraphItem *patch = new FaceGraphItem(patches[i]);
    patch->setName(QString("Patch #%1").arg(i));
    patch->setColor(colors_[i]);
    scene->addItem(patch);
    scene->changeGroup(patch, group);
  }
  return group;
}

#include "Surf_io_plugin.moc"


/**
 * $Id$
 *
 * Copyright (C)
 * 2014 - $Date$
 *     Martin Wolf <ndhist@martin-wolf.org>
 *
 * This file is distributed under the BSD 2-Clause Open Source License
 * (See LICENSE file).
 *
 */
#ifndef NDHIST_AXES_CONSTANT_BIN_WIDTH_AXIS_HPP_INCLUDED
#define NDHIST_AXES_CONSTANT_BIN_WIDTH_AXIS_HPP_INCLUDED 1

#include <cmath>
#include <string>
#include <sstream>

#include <boost/shared_ptr.hpp>

#include <boost/numpy/iterators/flat_iterator.hpp>

#include <ndhist/axis.hpp>
#include <ndhist/error.hpp>

namespace bn = boost::numpy;

namespace ndhist {
namespace axes {

/**
 * @class ConstantBinWidthAxis
 *     This class provides an axis with underlaying constant bin widths. To the
 *     user the bin widths could still vary. But as long as there is a value
 *     transformation available that transforms the bin edge values, which
 *     contuct the non-constant bin widths, into a linear scale with constant
 *     bin widths, this class can be used as axis with good performance.
 *
 *     The ValueTransform class must provide two static methods:
 *
 *         AxisValueType transform(AxisValueType const value)
 *
 *     and
 *
 *         AxisValueType back_transform(AxisValueType const value)
 *
 *     for transforming an edge value into linear scale and back.
 */
template <typename AxisValueType, typename ValueTransform>
class ConstantBinWidthAxis
  : public Axis
{
  public:
    typedef AxisValueType
            axis_value_type;

    typedef ValueTransform
            value_transform_type;

    typedef bn::iterators::single_value<axis_value_type>
            axis_value_type_traits;

    typedef Axis
            base;

    typedef ConstantBinWidthAxis<axis_value_type, value_transform_type>
            type;

  protected:
    /// The number of bins, including the possible under- and overflow bins.
    intptr_t n_bins_;

    /// The constant width of the bins. Remember, that the widths of the
    /// possible under- and overflow bins could be different from this value.
    axis_value_type bin_width_;

    /// The lower edge of the first bin with constant bin width, i.e. excluding
    /// the underflow bin.
    axis_value_type min_;

    /// The lower edge of the underflow bin.
    axis_value_type underflow_edge_;

    /// The upper edge of the overflow bin.
    axis_value_type overflow_edge_;

    /// The axis value type traits object, needed as temporary storage for
    /// dereferencing value pointers.
    axis_value_type_traits mutable avtt_;

  public:
    ConstantBinWidthAxis(
        bn::ndarray const & edges
      , std::string const & label
      , std::string const & name
      , bool has_underflow_bin
      , bool has_overflow_bin
      , bool is_extendable
      , intptr_t extension_max_fcap
      , intptr_t extension_max_bcap
    )
      : Axis(
            edges.get_size()-1
          , edges.get_dtype()
          , label
          , name
          , has_underflow_bin
          , has_overflow_bin
          , is_extendable
          , extension_max_fcap
          , extension_max_bcap
        )
    {
        // Set up the axis's function pointers.
        create_fct_                     = &type::create;
        get_bin_index_fct_              = &type::get_bin_index;
        get_binedges_ndarray_fct_       = &type::get_binedges_ndarray;
        get_lower_binedges_ndarray_fct_ = &base::get_lower_binedges_ndarray<type>;
        get_upper_binedges_ndarray_fct_ = &base::get_upper_binedges_ndarray<type>;
        get_bincenters_ndarray_fct_     = &base::get_bincenters_ndarray<type>;
        get_binwidths_ndarray_fct_      = &base::get_binwidths_ndarray<type>;
        get_n_bins_fct_                 = &type::get_n_bins;
        request_extension_fct_          = &type::request_extension;
        extend_fct_                     = &type::extend;
        create_axis_slice_fct_          = &base::create_axis_slice<type>;
        deepcopy_fct_                   = &type::deepcopy;

        //std::cout << "edges.shape(0): "<< edges.shape(0)<<std::endl;
        n_bins_ = edges.shape(0) - 1;
        //std::cout << "n_bins_: "<< n_bins_<<std::endl;
        if(n_bins_ <= 0)
        {
            std::stringstream ss;
            ss << "The edges array does not contain enough edges! It must "
               << "contain at least two edges! Remember "
               << "that the edges of the out-of-range bins, need to be "
               << "specified as well, when the axis is not extendable and is "
               << "supposed to have such bins.";
            throw ValueError(ss.str());
        }

        bn::iterators::flat_iterator< axis_value_type_traits > edges_iter(edges);

        // Set and skip the underflow edge.
        if(has_underflow_bin_)
        {
            underflow_edge_ = value_transform_type::transform(*edges_iter);
            ++edges_iter;
        }

        min_ = value_transform_type::transform(*edges_iter);
        ++edges_iter;
        if(   edges_iter.is_end()
           || (has_overflow_bin_ && edges_iter.get_iter_index() == n_bins_)
          )
        {
            // Only one bin (ie. the underflow or overflow bin) was given. So
            // we set the bin width to one.
            // Note: The bin_width_ property is used only for internal
            //       calculation. The bin width seen by the user is still
            //       calculated through the actual bin edges.
            bin_width_ = axis_value_type(1);
        }
        else
        {
            axis_value_type const value = value_transform_type::transform(*edges_iter);
            bin_width_ = value - min_;
        }

        // Set the overflow edge.
        if(has_overflow_bin_)
        {
            edges_iter.advance(edges_iter.distance_to(edges_iter.end()) - 1);
            overflow_edge_ = value_transform_type::transform(*edges_iter);
        }
    }

    /**
     * Copy constructor.
     */
    ConstantBinWidthAxis(ConstantBinWidthAxis const & other)
      : Axis(other)
      , n_bins_((*static_cast<type const *>(&other.get_axis_base())).n_bins_)
      , bin_width_((*static_cast<type const *>(&other.get_axis_base())).bin_width_)
      , min_((*static_cast<type const *>(&other.get_axis_base())).min_)
      , underflow_edge_((*static_cast<type const *>(&other.get_axis_base())).underflow_edge_)
      , overflow_edge_((*static_cast<type const *>(&other.get_axis_base())).overflow_edge_)
    {
        //std::cout << "ConstantBinWidthAxis:: Copy constructor." << std::endl<<std::flush;
    }

    static
    boost::shared_ptr<Axis>
    create(
        boost::numpy::ndarray const & edges
      , std::string const & label
      , std::string const & name
      , bool has_underflow_bin
      , bool has_overflow_bin
      , bool is_extendable
      , intptr_t extension_max_fcap
      , intptr_t extension_max_bcap
    )
    {
        return boost::shared_ptr<Axis>(new type(
            edges
          , label
          , name
          , has_underflow_bin
          , has_overflow_bin
          , is_extendable
          , extension_max_fcap
          , extension_max_bcap
        ));
    }

    static
    intptr_t
    get_n_bins(Axis const & axisbase)
    {
        type const & axis = *static_cast<type const *>(&axisbase);
        //std::cout << "constant_bin_width_axis: get_n_bins, axis_ptr: "<< &axis<< ", axis.n_bins_="<<axis.n_bins_ <<std::endl;
        return axis.n_bins_;
    }

    static
    bn::ndarray
    get_binedges_ndarray(Axis const & axisbase)
    {
        type const & axis = *static_cast<type const *>(&axisbase);

        intptr_t shape[1];
        shape[0] = axis.n_bins_ + 1;
        bn::ndarray edges_arr = bn::empty(1, shape, bn::dtype::get_builtin<axis_value_type>());
        bn::iterators::flat_iterator< axis_value_type_traits > iter(edges_arr);
        if(axis.has_underflow_bin_)
        {
            // Set the underflow edge.
            iter.set_value(value_transform_type::back_transform(axis.underflow_edge_));
            ++iter;
        }
        intptr_t idx = 0;
        while(! iter.is_end())
        {
            axis_value_type const value = axis.min_ + idx*axis.bin_width_;
            iter.set_value(value_transform_type::back_transform(value));

            ++idx;
            ++iter;
        }
        if(axis.has_overflow_bin_)
        {
            // Set the overflow edge.
            iter.advance(-1);
            iter.set_value(value_transform_type::back_transform(axis.overflow_edge_));
        }

        return edges_arr;
    }

    static
    intptr_t
    get_bin_index(Axis const & axisbase, char * value_ptr, axis::out_of_range_t & oor_flag)
    {
        type const & axis = *static_cast<type const *>(&axisbase);

        typename axis_value_type_traits::value_cref_type value_cref = axis_value_type_traits::dereference(axis.avtt_, value_ptr);
        axis_value_type const value = value_transform_type::transform(value_cref);
        //std::cout << "Got value = "<<value<<std::endl;

        if(axis.has_underflow_bin_)
        {
            if(value < axis.underflow_edge_)
            {
                // The value falls even left to the underflow bin.
                oor_flag = axis::OOR_UNDERFLOW;
                return -1;
            }
            if(value < axis.min_)
            {
                // The value falls into the underflow bin.
                //std::cout << "Got value in underflow bin"<< std::endl;
                oor_flag = axis::OOR_NONE;
                return 0;
            }
        }
        else
        {
            if(value < axis.min_)
            {
                //std::cout << "underflow: " << value << ", min = "<< data.min_ << std::endl;
                oor_flag = axis::OOR_UNDERFLOW;
                return -1;
            }
        }

        // The value is >= min_.
        intptr_t const idx = (value - axis.min_)/axis.bin_width_;

        if(axis.has_overflow_bin_)
        {
            if(idx < axis.n_bins_-2)
            {
                // The value falls into the axis range (excluding the overflow
                // bin).
                //std::cout << "Got value in normal bin"<< std::endl;
                oor_flag = axis::OOR_NONE;
                return idx + axis.has_underflow_bin_;
            }
            if(value < axis.overflow_edge_)
            {
                // The value falls into the overflow bin.
                oor_flag = axis::OOR_NONE;
                return axis.n_bins_-1;
            }

            // The value falls even right to the overflow bin.
            oor_flag = axis::OOR_OVERFLOW;
            return -1;
        }
        else
        {
            if(idx >= axis.n_bins_)
            {
                //std::cout << "overflow: " << value << ", idx = "<< idx << std::endl;
                oor_flag = axis::OOR_OVERFLOW;
                return -1;
            }
            //std::cout << "value " << value << " at " << idx << std::endl;
            oor_flag = axis::OOR_NONE;
            return idx + axis.has_underflow_bin_;
        }
    }

    // Determines the number of extra bins needed to the left (negative number
    // returned) or to the right (positive number returned) of the current axis
    // range.
    static
    intptr_t
    request_extension(Axis const & axisbase, char * value_ptr, axis::out_of_range_t const oor_flag)
    {
        type const & axis = *static_cast< type const *>(&axisbase);

        axis_value_type_traits avtt;
        typename axis_value_type_traits::value_ref_type value_cref = axis_value_type_traits::dereference(avtt, value_ptr);
        axis_value_type const value = value_transform_type::transform(value_cref);

        if(oor_flag == axis::OOR_UNDERFLOW)
        {
            intptr_t const n_extra_bins = std::ceil((std::abs(value - axis.min_) / axis.bin_width_));
            //std::cout << "request_autoscale (underflow): " << n_extra_bins << " extra bins." << std::endl<< std::flush;
            return -n_extra_bins;
        }
        else if(oor_flag == axis::OOR_OVERFLOW)
        {
            intptr_t const n_extra_bins = intptr_t((value - axis.min_)/axis.bin_width_) - (axis.n_bins_-1);
            //std::cout << "request_autoscale (overflow): " << n_extra_bins << " extra bins." << std::endl<< std::flush;
            return n_extra_bins;
        }

        return 0;
    }

    static
    void
    extend(Axis & axisbase, intptr_t f_n_extra_bins, intptr_t b_n_extra_bins)
    {
        type & axis = *static_cast< type *>(&axisbase);

        if(f_n_extra_bins > 0)
        {
            axis.n_bins_ += f_n_extra_bins;
            axis.min_    -= f_n_extra_bins * axis.bin_width_;
        }
        if(b_n_extra_bins > 0)
        {
            axis.n_bins_ += b_n_extra_bins;
        }
    }

    static
    boost::shared_ptr<Axis>
    deepcopy(Axis const & axisbase)
    {
        type const & axis = *static_cast<type const *>(&axisbase);
        return boost::shared_ptr<Axis>(new type(axis));
    }
};

}//namespace axes
}//namespace ndhist

#endif // NDHIST_AXES_CONSTANT_BIN_WIDTH_AXIS_HPP_INCLUDED


/*
	Authors: Darya Filippova, Geet Duggal, Rob Patro
	dfilippo | geet | robp @cs.cmu.edu
        See LICENSE.txt included with this distribution.
*/

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

#include <boost/range/irange.hpp>
#include <boost/iostreams/filtering_streambuf.hpp>
#include <boost/iostreams/copy.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <cmath>
#include "ArmatusUtil.hpp"
#include "ArmatusParams.hpp"
#include "IntervalScheduling.hpp"
#include "ArmatusParams.hpp"
#include "ArmatusDAG.hpp"

MatrixProperties parseGZipMatrix(string path, int resolution, string chrom) {
	MatrixProperties prop;

    prop.matrix = std::make_shared<SparseMatrix>();

	ifstream file(path, ios_base::in | ios_base::binary);
    if (!file.good()) {
        std::cerr << "Couldn't read file " << path << std::endl;
        std::exit(1);
    }
    assert(file.good());
    boost::iostreams::filtering_streambuf<boost::iostreams::input> in;
    in.push(boost::iostreams::gzip_decompressor());
    in.push(file);

    string line;
    std::istream incoming(&in);
    bool firstLine = true;
    size_t i = 0;
    double tot = 0.0;
    size_t nedge = 0;
    while ( getline(incoming, line) ) {
    	vector<string> parts;
        boost::trim(line);
		boost::split(parts, line, boost::is_any_of("\t"));

        if (firstLine) {
            //if (parts.size() - 3 < 100) {
            if (parts.size() < 100) {
                cerr << "[INFO] Matrix is smaller than the recommended minimum size of 101." << endl;
                // exit(1);
            }
            prop.matrix->resize(parts.size(), parts.size(), false);
            prop.chrom = chrom;
            prop.resolution = resolution;
            cerr << prop.chrom << " at resolution " << prop.resolution << "bp" << endl;
            firstLine = false;
        }

        if (parts.size() != prop.matrix->size2()) {
            std::cerr << "Error: row " << i << " has " << parts.size()
                      << " entries, but I was expecting " << prop.matrix->size2() << std::endl;
            std::exit(3);
        }

        if (i >= prop.matrix->size2()) {
            std::cerr << "Error: I was expecting " << prop.matrix->size2()
                      << " rows, but there are more than that in the matrix file." << std::endl;
            std::exit(3);
        }

        for (size_t j : boost::irange(i, parts.size())) {
            double e  = stod(parts[j]);
            if (e > 0.0) {
                size_t row = j;
                prop.matrix->insert_element(i, row, e);
                tot += e;
                nedge++;
            }
        }

    	++i;
        if ( i % 1000 == 0 ) { std::cerr << "line " << i << "\n"; }
        if (incoming.eof()) break;
    }

    // check that we read # of rows = to the number of columns in the matrix
    if (prop.matrix->size2() != i) {
        std::cerr << "Error: it doesn't look like your matrix file had enough rows." << std::endl;
        std::cerr << "Error: expecting " << prop.matrix->size2() << " but saw " << i << std::endl;
        std::exit(1);
    }
    return prop;
}

MatrixProperties parseRaoMatrix(string path, int resolution, string chrom, bool noNormalization) {
	MatrixProperties prop;
    int n;
    int M = 0;
    auto rawCountPath = path + ".RAWobserved";

    {
	    ifstream file(rawCountPath);
        if (!file.good()) {
            std::cerr << "Couldn't read file: " << rawCountPath << std::endl;
            std::exit(1);
        }
        assert(file.good());

        string line;
        int v, w;
        double count;
        int maxv = 0;
        while ( file >> v >> w >> count )  {
            if (v > maxv) maxv = v;
            if (w > maxv) maxv = w;
            M += 1;
        }
        n = maxv/resolution+1;
        prop.chrom = chrom;
        prop.resolution = resolution;
        cerr << "Building matrix for chromosome " << prop.chrom << " at resolution " << prop.resolution << "bp with " << n << " rows." << endl;
    }

    vector<double> KRnormalization;
    auto KRPath = path + ".KRnorm";
    if (!noNormalization)
    {
        cerr << "Reading KR normalization counts" << endl;
	    ifstream file(KRPath);
        string line;
        while( getline(file, line) ) {
            KRnormalization.push_back(stod(line));
        }
    }
    cout << "Initializing matrix to zero elements" << endl;
    prop.matrix = std::make_shared<SparseMatrix>(n,n,M);
    for (int i = 0; i < n; i++) {
        for (int j =0; j < n; j++) {
            prop.matrix->insert_element(i,j, 0.0);
        }
    }
    {
	    ifstream file(rawCountPath);

        string line;
        int v, w;
        double count;
        int m = 0;
        while ( file >> v >> w >> count )  {
            size_t i = v/resolution;
            size_t j = w/resolution;
            if (noNormalization) {
                prop.matrix->insert_element(i, j, log(count));
            }
            else {
                double ki = KRnormalization[i];
                double kj = KRnormalization[j];
                if (!std::isnan(ki) and !std::isnan(kj) and ki > 0 and kj > 0) {
                    prop.matrix->insert_element(i, j, log(count/(ki*kj)));
                    // cout << i << "\t" << j << "\t" << count << "\t" << count/(ki*kj) << endl;
                }
                else {
                    // Maybe write NaN or inf issue to log/warning file
                }
            }
            m++;
            if ( m % 100000 == 0 ) { std::cerr << "" << float(m)/M*100 << "%\n"; }
        }
    }

    return prop;
}

MatrixProperties parseSparseMatrix(string filepath, int resolution, string chrom) {
  MatrixProperties prop;
  int n;
  int M=0;

  {
    ifstream file(filepath);
    if (!file.good()) {
      std::cerr << "Couldn't read file: " << filepath << std::endl;
      std::exit(1);
    }
    assert(file.good());

    string line;
    int v, w;
    double count;
    int maxv = 0;
    while ( file >> v >> w >> count ) {
      if (v > maxv) maxv = v;
      if (w > maxv) maxv = w;
      M += 1;
    }
    n = maxv/resolution+1;
    prop.chrom = chrom;
    prop.resolution = resolution;
    cerr << "Building matrix for chromosome " << prop.chrom << " at resolution " << prop.resolution << "bp with " << n << " rows." << endl;
  }

  cout << "Initializing matrix to zero elements" << endl;
  prop.matrix = std::make_shared<SparseMatrix>(n,n,M);
  for (int i=0; i < n; i++) {
    for (int j=0; j < n; j++) {
      prop.matrix->insert_element(i,j,0.0);
    }
  }

  {
    ifstream file(filepath);

    string line;
    int v,w;
    double count;
    int m= 0;
    while ( file >> v >> w >> count ) {
      size_t i = v/resolution;
      size_t j = w/resolution;
      prop.matrix->insert_element(i,j, log(count));
      m++;
      if ( m % 100000 == 0 ) { std::cerr << "" << float(m)/M*100 << "%\n";}
    }
  }
  return prop;
}

// domain size
double d(size_t const & i, size_t const & j) {return j - i + 1;}

Domain::Domain(size_t s, size_t e) : start(s), end(e) { }

DomainSet consensusDomains(WeightedDomainEnsemble& dEnsemble) {
    using PersistenceMap = map<Domain, double>;
    PersistenceMap pmap;

    for (auto dSetIdx : boost::irange(size_t{0}, dEnsemble.domainSets.size())) {
        auto& dSet = dEnsemble.domainSets[dSetIdx];
        auto weight = dEnsemble.weights[dSetIdx];
        for (auto& domain : dSet) {
            if ( pmap.find(domain) == pmap.end() ) pmap[domain] = 0;
            pmap[domain] += weight;
        }
    }

    Intervals ivals;

    for (auto domainPersistence : pmap) {
        auto domain = domainPersistence.first;
        auto persistence = domainPersistence.second;
        ivals.push_back(WeightedInterval(domain.start, domain.end, persistence));
    }

    IntervalScheduler scheduler(ivals);
    scheduler.computeSchedule();

    DomainSet dSet;
    for (auto ival : scheduler.extractIntervals()) {
        dSet.push_back(Domain(ival.start, ival.end));
    }

    sort(dSet.begin(), dSet.end());

    return dSet;
}

WeightedDomainEnsemble multiscaleDomains(std::shared_ptr<SparseMatrix> A,
    float gammaMax, double stepSize, int k, int minMeanSamples, bool justThisGamma) {

    WeightedDomainEnsemble dEnsemble;
    double eps = 1e-5;
    double gamma =0.0;
    if (justThisGamma)  {
        gamma = gammaMax;
    }

    for (; gamma <= gammaMax+eps; gamma+=stepSize) {

        cerr << "gamma=" << gamma << endl;

        ArmatusParams params(A, gamma, k, minMeanSamples); // k parameter is not used for anything in Params
        ArmatusDAG G(params); // but is used in the DAG
        G.build();
        G.computeTopK();

        auto domainEnsemble = G.extractTopK();
        auto& domains = domainEnsemble.domainSets;
        auto& weights = domainEnsemble.weights;
        dEnsemble.domainSets.insert(dEnsemble.domainSets.end(), domains.begin(), domains.end());
        dEnsemble.weights.insert(dEnsemble.weights.end(), weights.begin(), weights.end());
        for (int i = 0; i<k; i++) {
            dEnsemble.resolutions.push_back(gamma);
            dEnsemble.optidx.push_back(i);
        }
    }

    return dEnsemble;
}

void outputDomains(DomainSet dSet, string fname, MatrixProperties matProp) {
    ofstream file;
    file.open(fname);
    int res = matProp.resolution;
    for (auto d : dSet) {
        file << matProp.chrom << "\t" << (d.start)*res << "\t" << (d.end+1)*res-1 << endl;
    }
    file.close();
}

void sanityCheck(WeightedDomainEnsemble e) {
    for (auto dset : e.domainSets) {
    }
}


// -*- c++-mode -*-
// Copyright 2010 François Bleibel Thomas Moulard, Olivier Stasse, Nicolas Mansard
//

#include <boost/date_time/posix_time/posix_time.hpp>
#include <dynamic-graph/signal-caster.h>
#include <dynamic-graph/signal-cast-helper.h>
#include <dynamic-graph/dynamic-graph-api.h>
#include <exception>
#include <boost/lambda/bind.hpp>
#include <string>
#include <sstream>
#include <algorithm>
#include <dynamic-graph/exception-signal.h>
#include <dynamic-graph/linear-algebra.h>

namespace dynamicgraph
{

  // Define a custom implementation of the DefaultCastRegisterer
  // to workaround the limitations of the stream based approach.
  // When dealing with double: displaying a double on a stream
  // is *NOT* the opposite of reading a double from a stream.
  //
  // In practice, it means that there is no way to read
  // a NaN, +inf, -inf from a stream!
  //
  // To workaround this problem, parse special values manually
  // (the strings used are the one produces by displaying special
  // values on a stream).

  template <>
  inline boost::any
  DefaultCastRegisterer<double>::cast (std::istringstream& iss)
    {
      std::string tmp;
      iss >> tmp;

      if (tmp == "nan")
	return std::numeric_limits<double>::quiet_NaN ();
      else if (tmp == "inf" || tmp == "+inf")
	return std::numeric_limits<double>::infinity ();
      else if (tmp == "-inf")
	return -1. * std::numeric_limits<double>::infinity ();

      try
	{
	  return boost::lexical_cast<double> (tmp);
	}
      catch (boost::bad_lexical_cast&)
	{
	  boost::format fmt ("failed to serialize %s (to double)");
	  fmt % tmp;
	  throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str ());
	}
    }

  /* Specialize Matrix and Vector traces. */

  template <>
  void
  DefaultCastRegisterer<dynamicgraph::Vector>::
  trace(const boost::any& object, std::ostream& os)
  {
    const dynamicgraph::Vector & v = boost::any_cast<dynamicgraph::Vector> (object);
    for( int i=0;i<v.size();++i )
      { os << "\t" << v(i); }
  }
  template <>
  void
  DefaultCastRegisterer<dynamicgraph::Matrix>::
  trace(const boost::any& object, std::ostream& os)
  {
    const dynamicgraph::Matrix & m = boost::any_cast<dynamicgraph::Matrix> (object);
    for(int i=0;i<m.rows();++i )
      for(int j=0;j<m.cols();++j )
	{ os << "\t" << m(i,j); }
  }

  /// Registers useful casts
  namespace
  {
    DefaultCastRegisterer<double> double_reg;
    DefaultCastRegisterer<int> int_reg;
    DefaultCastRegisterer<unsigned int> uint_reg;
    DefaultCastRegisterer<bool> bool_reg;
    DefaultCastRegisterer<dynamicgraph::Vector> vectorCastRegisterer;
    DefaultCastRegisterer<dynamicgraph::Matrix> matrixCastRegisterer;
    DefaultCastRegisterer <boost::posix_time::ptime> ptimeCastRegisterer;
  } // end of anonymous namespace.

} // namespace dynamicgraph


//
// Created by kerin on 2019-02-28.
//

#include "eigen_utils.hpp"
#include "mpi_utils.hpp"

#include "tools/eigen3.3/Dense"
#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/device/file.hpp>
#include <boost/iostreams/filter/gzip.hpp>

#include <iostream>
#include <cmath>
#include <map>
#include <vector>
#include <string>
#include <set>

namespace boost_io = boost::iostreams;

Eigen::MatrixXd EigenUtils::subset_matrix(const Eigen::MatrixXd &orig,
                                          const std::vector<int> &valid_points) {
	long n_cols = orig.cols(), n_rows = valid_points.size();
	Eigen::MatrixXd subset(n_rows, n_cols);

	for(int kk = 0; kk < n_rows; kk++) {
		for(int jj = 0; jj < n_cols; jj++) {
			subset(kk, jj) = orig(valid_points[kk], jj);
		}
	}
	return subset;
}

template <typename EigenMat>
void EigenUtils::write_matrix(boost_io::filtering_ostream& outf,
                              EigenMat& M,
                              std::vector<std::string >& col_names,
                              std::vector<std::string>& row_names){
	if(!col_names.empty()) {
		if (row_names.size() > 0) assert(col_names.size() == M.cols() + 1);
		if (row_names.size() == 0) assert(col_names.size() == M.cols());
		for (int jj = 0; jj < col_names.size(); jj++) {
			outf << col_names[jj];
			if (jj < col_names.size() - 1) {
				outf << " ";
			}
		}
		outf << std::endl;
	}

	if(!row_names.empty()) {
		assert(row_names.size() == M.rows());
		for (long ii = 0; ii < M.rows(); ii++) {
			outf << row_names[ii] << " ";
			for (long jj = 0; jj < M.cols(); jj++) {
				outf << M(ii, jj);
				if (jj < M.cols() - 1) {
					outf << " ";
				}
			}
			outf << std::endl;
		}
	} else {
		outf << M;
	}
}

template <typename EigenMat>
void EigenUtils::read_matrix( const std::string& filename,
                              EigenMat& M){
	std::vector<std::string> placeholder;
	EigenUtils::read_matrix(filename, M, placeholder);
}

template <typename EigenMat>
void EigenUtils::read_matrix(const std::string &filename,
                             EigenMat &M,
                             std::vector <std::string> &col_names){
	std::map<long, bool> incomplete_row;
	EigenUtils::read_matrix(filename, M, col_names, incomplete_row);
}

template <typename EigenMat>
void EigenUtils::read_matrix(const std::string &filename,
                             EigenMat &M,
                             std::vector <std::string> &col_names,
                             std::map<long, bool> &incomplete_row) {
	/* Read txt file into martix. Files can be gzipped. */

	boost_io::filtering_istream fg;
	std::string gz_str = ".gz";
	if (filename.find(gz_str) != std::string::npos) {
		fg.push(boost_io::gzip_decompressor());
	}
	fg.push(boost_io::file_source(filename));
	if (!fg) {
		throw std::runtime_error(filename+" could not be opened.");
	}

	// Read file twice to acertain number of lines
	std::string line;
	int n_rows = 0;
	getline(fg, line);
	while (getline(fg, line)) {
		n_rows++;
	}
	fg.reset();
	if (filename.find(gz_str) != std::string::npos) {
		fg.push(boost_io::gzip_decompressor());
	}
	fg.push(boost_io::file_source(filename));

	// Reading column names
	if (!getline(fg, line)) {
		throw std::runtime_error(filename+" contains zero lines.");
	}
	std::stringstream ss;
	std::string s1;
	int n_cols = 0;
	ss.clear();
	ss.str(line);
	while (ss >> s1) {
		++n_cols;
		col_names.push_back(s1);
	}
	std::cout << "Reading matrix of size " << n_rows << " x " << n_cols << " from " << filename << std::endl;

	// Write remainder of file to Eigen matrix M
	incomplete_row.clear();
	M.resize(n_rows, n_cols);
	int i = 0;
	double tmp_d;
	while (getline(fg, line)) {
		if (i >= n_rows) {
			throw std::runtime_error("ERROR: could not convert txt file (too many lines).");
		}
		ss.clear();
		ss.str(line);
		for (int k = 0; k < n_cols; k++) {
			std::string sss;
			ss >> sss;
			if (sss == "NA" || sss == "NAN" || sss == "NaN" || sss == "nan") {
				tmp_d = 0;
				incomplete_row[i] = true;
			} else {
				try {
					tmp_d = stod(sss);
				} catch (const std::invalid_argument &exc) {
					std::cout << sss << " on line " << i << std::endl;
					throw;
				}
			}
			M(i, k) = tmp_d;
		}
		i++;
	}
}

void EigenUtils::read_matrix_and_skip_cols(const std::string &filename,
                                           const int& n_skip_cols,
                                           Eigen::MatrixXd &M,
                                           std::vector <std::string> &col_names) {
	/* Assumptions:
	   - dimensions unknown
	   - assume no missing values
	 */

	boost_io::filtering_istream fg;
	std::string gz_str = ".gz";
	if (filename.find(gz_str) != std::string::npos) {
		fg.push(boost_io::gzip_decompressor());
	}
	fg.push(boost_io::file_source(filename));
	if (!fg) {
		throw std::runtime_error(filename+" could not be opened.");
	}

	// Read file twice to acertain number of lines
	std::string line;
	int n_rows = 0;
	getline(fg, line);
	while (getline(fg, line)) {
		n_rows++;
	}
	fg.reset();
	if (filename.find(gz_str) != std::string::npos) {
		fg.push(boost_io::gzip_decompressor());
	}
	fg.push(boost_io::file_source(filename));

	// Reading column names
	if (!getline(fg, line)) {
		throw std::runtime_error(filename+" contains zero lines.");
	}
	std::stringstream ss;
	std::string s1;
	int n_cols = 0;
	ss.clear();
	ss.str(line);
	while (ss >> s1) {
		++n_cols;
		col_names.push_back(s1);
	}
	std::cout << " Reading matrix of size " << n_rows << " x " << n_cols << " from " << filename << std::endl;
	assert(n_skip_cols < n_cols);

	// Write remainder of file to Eigen matrix M
	M.resize(n_rows, n_cols - n_skip_cols);
	int i = 0;
	double tmp_d;
	while (getline(fg, line)) {
		if (i >= n_rows) {
			throw std::runtime_error("ERROR: could not convert txt file (too many lines).");
		}
		ss.clear();
		ss.str(line);
		for (int k = 0; k < n_cols; k++) {
			std::string s;
			ss >> s;
			if (k >= n_skip_cols) {
				try {
					tmp_d = stod(s);
				} catch (const std::invalid_argument &exc) {
					std::cout << s << " on line " << i << std::endl;
					throw;
				}
				M(i, k - n_skip_cols) = tmp_d;
			}
		}
		i++;
	}
	if (i < n_rows) {
		throw std::runtime_error("ERROR: could not convert txt file (too few lines).");
	}
}

Eigen::MatrixXf EigenUtils::solve(const Eigen::MatrixXf &A, const Eigen::MatrixXf &b) {
	Eigen::MatrixXf x = A.colPivHouseholderQr().solve(b);
	double check = fabs((double)((A * x - b).norm()/b.norm()));
	if (check > 1e-6) {
		// std::string ms = "ERROR: could not solve covariate scatter matrix (Check = " +
		// std::to_string(check) + ").";
		// throw std::runtime_error(ms);
		std::cout << "WARNING: Error in solving covariate scatter matrix is: " << check << std::endl;
	}
	return x;
}

Eigen::MatrixXd EigenUtils::solve(const Eigen::MatrixXd &A, const Eigen::MatrixXd &b) {
	Eigen::MatrixXd x = A.colPivHouseholderQr().solve(b);
	double check = fabs((double)((A * x - b).norm()/b.norm()));
	if (check > 1e-8) {
		std::string ms = "ERROR: could not solve covariate scatter matrix (Check = " +
		                 std::to_string(check) + ").";
		throw std::runtime_error(ms);
	}
	return x;
}

template <typename EigenMat>
void EigenUtils::scale_matrix_and_remove_constant_cols(EigenMat &M,
                                                       long &n_cols,
                                                       std::vector<std::string> &col_names){
	// Scale eigen matrix passed by reference.
	// Removes columns with zero variance + updates col_names.
	// Only call on matrixes which have been reduced to complete cases,
	// as no check for incomplete rows.
	double Nlocal = M.rows();
	double Nglobal = mpiUtils::mpiReduce_inplace(&Nlocal);

	std::vector<std::size_t> keep;
	std::vector<std::string> keep_names;
	std::vector<std::string> reject_names;
	for (std::size_t k = 0; k < n_cols; k++) {
		double sum_mik2 = M.col(k).array().square().sum();
		sum_mik2 = mpiUtils::mpiReduce_inplace(&sum_mik2);
//		double sigma = 0.0;
//		double count = 0;
//		for (int i = 0; i < n_rows; i++) {
//			double val = M(i, k);
//			sigma += val * val;
//			count += 1;
//		}
		double sigma = std::sqrt(sum_mik2 / (Nglobal - 1));

//		sigma = sqrt(sigma/(count - 1));
		if (sigma > 1e-12) {
			M.col(k).array() /= sigma;
//			for (int i = 0; i < n_rows; i++) {
//				M(i, k) /= sigma;
//			}
			keep.push_back(k);
			keep_names.push_back(col_names[k]);
		} else {
			reject_names.push_back(col_names[k]);
		}
	}

	if (keep.size() != n_cols) {
		std::cout << " Removing " << (n_cols - keep.size())  << " column(s) with zero variance:" << std::endl;
		for(auto name : reject_names) {
			std::cout << name << std::endl;
		}
		// subset cols
		for (std::size_t i = 0; i < keep.size(); i++) {
			M.col(i) = M.col(keep[i]);
		}
		M.conservativeResize(M.rows(), keep.size());

		n_cols = keep.size();
		col_names = keep_names;
	}

	if (n_cols == 0) {
		throw std::runtime_error("ERROR: No columns left with nonzero variance after scale_matrix()");
	}
}

template <typename EigenMat>
void EigenUtils::center_matrix(EigenMat& M){
	// Center eigen matrix passed by reference.
	// Only call on matrixes which have been reduced to complete cases,
	// as no check for incomplete rows.
	long n_cols = M.cols();
	long n_rows = M.rows();
	double Nlocal = M.rows();
	double Nglobal = mpiUtils::mpiReduce_inplace(&Nlocal);

	for (int k = 0; k < n_cols; k++) {
		double sum_mik = 0.0;
		for (int i = 0; i < n_rows; i++) {
			sum_mik += M(i, k);
		}

		sum_mik = mpiUtils::mpiReduce_inplace(&sum_mik);
		double mu = sum_mik / Nglobal;
		for (int i = 0; i < n_rows; i++) {
			M(i, k) -= mu;
		}
	}
}

Eigen::MatrixXd EigenUtils::project_out_covars(Eigen::Ref<Eigen::MatrixXd> rhs, const Eigen::Ref<const Eigen::MatrixXd> &C,
                                               const Eigen::Ref<const Eigen::MatrixXd> &CtC_inv) {
	assert(CtC_inv.cols() == C.cols());
	assert(CtC_inv.rows() == C.cols());
	assert(C.rows() == rhs.rows());
	Eigen::MatrixXd CtRHS = C.transpose() * rhs;
	CtRHS = mpiUtils::mpiReduce_inplace(CtRHS);
	Eigen::MatrixXd beta = CtC_inv * CtRHS;
	Eigen::MatrixXd yhat = C * beta;
	Eigen::MatrixXd res = rhs - yhat;
	return res;
}

// Explicit instantiation
// https://stackoverflow.com/questions/2152002/how-do-i-force-a-particular-instance-of-a-c-template-to-instantiate

template void EigenUtils::read_matrix(const std::string&,
                                      Eigen::MatrixXf&, std::vector<std::string>&, std::map<long, bool>&);
template void EigenUtils::read_matrix(const std::string&,
                                      Eigen::MatrixXd&, std::vector<std::string>&, std::map<long, bool>&);
template void EigenUtils::read_matrix(const std::string&,
                                      Eigen::MatrixXf&, std::vector<std::string>&);
template void EigenUtils::read_matrix(const std::string&,
                                      Eigen::MatrixXd&, std::vector<std::string>&);
template void EigenUtils::read_matrix(const std::string&, Eigen::MatrixXf&);
template void EigenUtils::read_matrix(const std::string&, Eigen::MatrixXd&);
template void EigenUtils::write_matrix(boost_io::filtering_ostream&,
                                       Eigen::VectorXd&,
                                       std::vector<std::string>&,
                                       std::vector<std::string>&);
template void EigenUtils::write_matrix(boost_io::filtering_ostream&,
                                       Eigen::MatrixXd&,
                                       std::vector<std::string>&,
                                       std::vector<std::string>&);
template void EigenUtils::center_matrix(Eigen::MatrixXd&);
template void EigenUtils::center_matrix(Eigen::MatrixXf&);
template void EigenUtils::center_matrix(Eigen::VectorXd&);
template void EigenUtils::center_matrix(Eigen::VectorXf&);
template void EigenUtils::scale_matrix_and_remove_constant_cols(Eigen::MatrixXf&,
                                                                long&, std::vector<std::string>&);
template void EigenUtils::scale_matrix_and_remove_constant_cols(Eigen::MatrixXd&,
                                                                long&, std::vector<std::string>&);
template void EigenUtils::scale_matrix_and_remove_constant_cols(Eigen::VectorXd&,
                                                                long&, std::vector<std::string>&);


// Boost.Geometry
//
// Copyright (c) 2015 Mats Taraldsvik.
//
// This file was modified by Oracle on 2020.
// Modifications copyright (c) 2020, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_IO_WKB_DETAIL_WRITER_HPP
#define BOOST_GEOMETRY_IO_WKB_DETAIL_WRITER_HPP

#include <algorithm>
#include <cassert>
#include <cstddef>
#include <iterator>
#include <limits>

#include <boost/concept_check.hpp>
#include <boost/cstdint.hpp>
#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>
#include <boost/range/size.hpp>
#include <boost/static_assert.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/exterior_ring.hpp>
#include <boost/geometry/core/interior_rings.hpp>
#include <boost/geometry/extensions/gis/io/wkb/detail/endian.hpp>
#include <boost/geometry/extensions/gis/io/wkb/detail/ogc.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace wkb
{

    template <typename T>
    struct value_writer
    {
        typedef T value_type;

        template <typename OutputIterator>
        static bool write(T const& value,
                          OutputIterator& iter,
                          byte_order_type::enum_t byte_order)
        {
            endian::endian_value<T> parsed_value(value);

// 				if (byte_order_type::xdr == byte_order)
// 				{
// 					parsed_value.template store<endian::big_endian_tag>(iter);
// 				}
// 				else if (byte_order_type::ndr == byte_order)
// 				{
// 					parsed_value.template store<endian::little_endian_tag>(iter);
// 				}
// 				else
// 				{
                parsed_value.template store<endian::native_endian_tag>(iter);
// 				}

            return true;
        }
    };

    template
    <
        typename Point,
        std::size_t I = 0,
        std::size_t N = dimension<Point>::value
    >
    struct writer_assigner
    {
        template <typename OutputIterator>
        static void run(Point const& point,
                        OutputIterator& iter,
                        byte_order_type::enum_t byte_order)
        {
            // NOTE: coordinates of any type are converted to double

            value_writer<double>::write(geometry::get<I>(point),
                                        iter,
                                        byte_order);

            writer_assigner<Point, I+1, N>::run(point, iter, byte_order);
        }
    };

    template <typename Point, std::size_t N>
    struct writer_assigner<Point, N, N>
    {
        template <typename OutputIterator>
        static void run(Point const& /*point*/,
                        OutputIterator& /*iter*/,
                        byte_order_type::enum_t /*byte_order*/)
        {
            // terminate
        }
    };

    template <typename Point>
    struct point_writer
    {
        template <typename OutputIterator>
        static bool write(Point const& point,
                          OutputIterator& iter,
                          byte_order_type::enum_t byte_order)
        {
            // write endian type
            value_writer<uint8_t>::write(byte_order, iter, byte_order);

            // write geometry type
            uint32_t type = geometry_type<Point>::get();
            value_writer<uint32_t>::write(type, iter, byte_order);

            // write point's x, y, z
            writer_assigner<Point>::run(point, iter, byte_order);

            return true;
        }
    };

    template <typename Linestring>
    struct linestring_writer
    {
        template <typename OutputIterator>
        static bool write(Linestring const& linestring,
                          OutputIterator& iter,
                          byte_order_type::enum_t byte_order)
        {
            // write endian type
            value_writer<uint8_t>::write(byte_order, iter, byte_order);

            // write geometry type
            uint32_t type = geometry_type<Linestring>::get();
            value_writer<uint32_t>::write(type, iter, byte_order);

            // write num points
            uint32_t num_points = boost::size(linestring);
            value_writer<uint32_t>::write(num_points, iter, byte_order);

            for(typename boost::range_iterator<Linestring const>::type
                    point_iter = boost::begin(linestring);
                point_iter != boost::end(linestring);
                ++point_iter)
            {
                // write point's x, y, z
                writer_assigner<typename point_type<Linestring>::type>
                    ::run(*point_iter, iter, byte_order);
            }

            return true;
        }
    };

    template <typename Polygon>
    struct polygon_writer
    {
        template <typename OutputIterator>
        static bool write(Polygon const& polygon,
                          OutputIterator& iter,
                          byte_order_type::enum_t byte_order)
        {
            // write endian type
            value_writer<uint8_t>::write(byte_order, iter, byte_order);

            // write geometry type
            uint32_t type = geometry_type<Polygon>::get();
            value_writer<uint32_t>::write(type, iter, byte_order);

            // write num rings
            uint32_t num_rings = 1 + geometry::num_interior_rings(polygon);
            value_writer<uint32_t>::write(num_rings, iter, byte_order);

            // write exterior ring
            typedef typename geometry::ring_type<Polygon const>::type
                ring_type;

            typename geometry::ring_return_type<Polygon const>::type
                exterior_ring = geometry::exterior_ring(polygon);

            value_writer<uint32_t>::write(geometry::num_points(exterior_ring),
                                          iter,
                                          byte_order);

            for(typename boost::range_iterator<ring_type const>::type
                    point_iter = boost::begin(exterior_ring);
                point_iter != boost::end(exterior_ring);
                ++point_iter)
            {
                // write point's x, y, z
                writer_assigner<typename point_type<Polygon>::type>
                    ::run(*point_iter, iter, byte_order);
            }

            // write interor rings
            typedef typename geometry::interior_type<Polygon const>::type
                interior_rings_type;

            typename geometry::interior_return_type<Polygon const>::type
                interior_rings = geometry::interior_rings(polygon);

            for(typename boost::range_iterator<interior_rings_type const>::type
                    ring_iter = boost::begin(interior_rings);
                ring_iter != boost::end(interior_rings);
                ++ring_iter)
            {
                value_writer<uint32_t>::write(geometry::num_points(*ring_iter),
                                              iter,
                                              byte_order);

                for(typename boost::range_iterator<ring_type const>::type
                        point_iter = boost::begin(*ring_iter);
                    point_iter != boost::end(*ring_iter);
                    ++point_iter)
                {
                    // write point's x, y, z
                    writer_assigner<typename point_type<Polygon>::type>
                        ::run(*point_iter, iter, byte_order);
                }
            }

            return true;
        }
    };

}} // namespace detail::wkb
#endif // DOXYGEN_NO_IMPL

}} // namespace boost::geometry
#endif // BOOST_GEOMETRY_IO_WKB_DETAIL_WRITER_HPP


/*
   Copyright 2018 Simon Vogl <svogl@voxel.at>
                  Angel Merino-Sastre <amerino@voxel.at>

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/
#include <string>
#include <iostream>
#include <fstream>
#include <limits.h>

#define _USE_MATH_DEFINES
#include <cmath>

#if OCV_VERSION_MAJOR >= 3
#  include <opencv2/core.hpp>
#  include <opencv2/calib3d.hpp>
#else
#  include <opencv2/core/core.hpp>
#  include <opencv2/calib3d/calib3d.hpp>
#endif

#include <boost/log/trivial.hpp>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string/case_conv.hpp>

#include "toffy/base/polar2cart.hpp"
#include "toffy/common/filenodehelper.hpp"

using namespace toffy;
using namespace toffy::filters;
using namespace cv;
using namespace std;
namespace logging = boost::log;
namespace fs = boost::filesystem;

#ifdef CM_DEBUG
const bool dbg=true;
#else
const bool dbg=false;
#endif

std::size_t Polar2Cart::_filter_counter = 1;
const std::string Polar2Cart::id_name = "polar2cart";

Polar2Cart::Polar2Cart(): Filter(Polar2Cart::id_name,_filter_counter),
    _fovx(-1), _fovy(-1),_in_img("img"), _out_img(_in_img)
{
    _filter_counter++;
}

void Polar2Cart::updateConfig(const boost::property_tree::ptree &pt) {
    BOOST_LOG_TRIVIAL(debug) << __FUNCTION__ <<  " " << id();

    using namespace boost::property_tree;

    Filter::updateConfig(pt);

    _fovx = pt.get<double>("options.fovx",_fovx);
    _fovy = pt.get<double>("options.fovy",_fovy);

    boost::optional<const boost::property_tree::ptree& > ocvo =
	    pt.get_child_optional( "options.cameraMatrix" );
    if (ocvo.is_initialized()) {
	if( toffy::commons::checkOCVNone(*ocvo) ) {
	    boost::property_tree::ptree os;
	    os.put_child("cameraMatrix",*ocvo);
	    cv::FileStorage fs = commons::loadOCVnode(os);
	    cout << fs.getFirstTopLevelNode().name() << endl;
	    fs.getFirstTopLevelNode() >> _cameraMatrix;
	    fs.release();
	} else BOOST_LOG_TRIVIAL(debug) << __FUNCTION__;
	BOOST_LOG_TRIVIAL(debug) << "Node cameraMatrix is not opencv.";
    } else
	BOOST_LOG_TRIVIAL(debug) << "Node options.cameraMatrix not found.";

    _in_img = pt.get<string>("inputs.img",_in_img);
    _in_fovx = pt.get<string>("inputs.fovx",_in_fovx);
    _in_fovy = pt.get<string>("inputs.fovy",_in_fovy);

    _out_img = pt.get<string>("outputs.img",_out_img);

}

boost::property_tree::ptree Polar2Cart::getConfig() const
{
    boost::property_tree::ptree pt;

    pt = Filter::getConfig();
    pt.put("options.x_ang", _fovx);
    pt.put("options.y_ang", _fovy);

    pt.put("inputs.img", _in_img);
    pt.put("inputs.fovx", _in_fovx);
    pt.put("inputs.fovy", _in_fovy);

    pt.put("outputs.img", _out_img);

    return pt;
}

bool Polar2Cart::filter(const Frame &in, Frame& out) {
    BOOST_LOG_TRIVIAL(debug) << __FUNCTION__ << " " << id();

    boost::shared_ptr<cv::Mat> img;
    try {
	img = boost::any_cast<boost::shared_ptr<cv::Mat> >(in.getData(_in_img));
    } catch(const boost::bad_any_cast &) {
	BOOST_LOG_TRIVIAL(error) << "Could not cast input " <<
				    _in_img << ", filter  " <<
				    id() <<" not applied.";
	return false;
    }

    if (_cameraMatrix.data) {
	double noV, apertureWidth = (45/1000)*img->size().width, apertureHeight = (45/1000)*img->size().height;
	Point2d np;
	calibrationMatrixValues(_cameraMatrix, img->size(), apertureWidth, apertureHeight, _fovx, _fovy, noV, np, noV);
    }

    if (!_in_fovx.empty()) {
	try {
	    _fovx= boost::any_cast<double>(in.getData(_in_fovx));
	} catch(const boost::bad_any_cast &) {
	    BOOST_LOG_TRIVIAL(warning) <<
					  "Could not read input " << _in_fovx;
	}
    }
    if (!_in_fovy.empty()) {
	try {
	    _fovy= boost::any_cast<double>(in.getData(_in_fovy));
	} catch(const boost::bad_any_cast &) {
	    BOOST_LOG_TRIVIAL(warning) <<
					  "Could not read input " << _in_fovy;
	}
    }

    if (_fovx <= 0. || _fovy <= 0.) {
	BOOST_LOG_TRIVIAL(warning) <<
				      "No FoV data, filter " << id() <<" not applied.";
	return false;
    }

    //TODO we need new data at the output
    //TODO handle aux vars creation
    Mat new_img = *img;

    float coef_h = (M_PI*_fovx/180)/new_img.cols,
	    coef_v = (M_PI*_fovy/180)/new_img.rows;

    for (int i= 0 ; i < new_img.rows; i++) {
	for(int j=0; j < new_img.cols; j++) {
	    if (new_img.at<float>(i,j) < 65. && new_img.at<float>(i,j) > 0. &&  new_img.at<float>(i,j) == new_img.at<float>(i,j)) {
		//TODO check type and channels
		new_img.at<float>(i,j) *=
			(cos(coef_h*(abs(j-((new_img.cols/2)-1))))) *
			(cos(coef_v*(abs(i-((new_img.rows/2)-1)))));
	    }
	}

    }

    return true;
}


///////////////////////////////////////////////////////////////////////////////
// statistics::detail::accumulator::keyword::threshold.hpp                   //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_ACCUMULATOR_STATISTICS_KEYWORD_THRESHOLD_HPP_2009
#define BOOST_STATISTICS_DETAIL_ACCUMULATOR_STATISTICS_KEYWORD_THRESHOLD_HPP_2009
#include <boost/parameter/name.hpp>
#include <boost/parameter/keyword.hpp>
#include <boost/statistics/detail/accumulator/statistics/tag/threshold.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace accumulator{
namespace keyword{
    namespace
    {
        boost::parameter::keyword<tag::threshold>& threshold
            = boost::parameter::keyword<tag::threshold>::get();
    }
}// keyword
}// accumulator
}// detail
}// statistics
}// boost

#endif


#pragma once

#include <string>
#include <vector>

#include <boost/filesystem.hpp>

#include <Eigen/Core>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

const double kPi = 3.14159265359;

struct KittiOxts_t
{
    union
    {
        struct
        {
            double lat, lon, alt;
            double roll, pitch, yaw;

            double vn, ve, vf, vl, vu;
            double ax, ay, az, af, al, au;
            double wx, wy, wz, wf, wl, wu;

            double posacc, velacc;
        };
        double data[25];
    };

    union
    {
        struct
        {
            int navstat, numstats, posmode, velmode, orimode;
        };
        int info[5];
    };

    KittiOxts_t(const std::vector<std::string>& oxtsData)
    {
        std::size_t i = 0;
        for (; i < 25; i++)
            data[i] = std::stod(oxtsData[i]);

        for (; i < 30; i++)
            info[i - 25] = std::stoi(oxtsData[i]);
    }
};

Eigen::Matrix4f computePoseFromOxts(
    const KittiOxts_t& oxts,
    const float scale);

std::vector<boost::filesystem::path> getDirectoryContents(
    const boost::filesystem::path& kPath,
    bool findDirs,
    const std::string& fileExt="");

KittiOxts_t readOxtsData(
    const std::string& kOxtsPath);

void readVelodyneBin(
    const std::string& kBinPath,
    pcl::PointCloud<pcl::PointXYZI>& cloud);


///////////////////////////////////////////////////////////////////////////////
// cpp-next_bug.hpp
//
//  Copyright 2012 Eric Niebler. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <vector>
#include <boost/proto/proto.hpp>
#include <boost/test/unit_test.hpp>
namespace mpl = boost::mpl;
namespace proto = boost::proto;
using proto::_;

namespace linear_algebra
{
    // A trait that returns true only for std::vector
    template<typename T>
    struct is_std_vector
      : mpl::false_
    {};

    template<typename T, typename A>
    struct is_std_vector<std::vector<T, A> >
      : mpl::true_
    {};

    // A type used as a domain for linear algebra expressions
    struct linear_algebra_domain
      : proto::domain<>
    {};

    // Define all the operator overloads for combining std::vectors
    BOOST_PROTO_DEFINE_OPERATORS(is_std_vector, linear_algebra_domain)

    // Take any expression and turn each node
    // into a subscript expression, using the
    // state as the RHS.
    struct Distribute
      : proto::or_<
            proto::when<proto::terminal<_>, proto::_make_subscript(_, proto::_state)>
          , proto::plus<Distribute, Distribute>
        >
    {};

    struct Optimize
      : proto::or_<
            proto::when<
                proto::subscript<Distribute, proto::terminal<_> >,
                Distribute(proto::_left, proto::_right)
            >
          , proto::plus<Optimize, Optimize>
          , proto::terminal<_>
        >
    {};
}

static const int celems = 4;
static int const value[celems] = {1,2,3,4};
std::vector<int> A(value, value+celems), B(A);

void test1()
{
    using namespace linear_algebra;
    proto::_default<> eval;
    BOOST_CHECK_EQUAL(8, eval(Optimize()((A + B)[3])));
}

using namespace boost::unit_test;
///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("test for a problem reported on the cpp-next.com blog");

    test->add(BOOST_TEST_CASE(&test1));

    return test;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/sdk/meta/as_pack.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE_TPL(as_pack, BOOST_SIMD_TYPES)
{
  using boost::simd::meta::as_pack;
  using boost::simd::pack;

  NT2_TEST_TYPE_IS( (typename as_pack<T,1 >::type), T            );
  NT2_TEST_TYPE_IS( (typename as_pack<T,2 >::type), (pack<T,2>)  );
  NT2_TEST_TYPE_IS( (typename as_pack<T,4 >::type), (pack<T,4>)  );
  NT2_TEST_TYPE_IS( (typename as_pack<T,8 >::type), (pack<T,8>)  );
  NT2_TEST_TYPE_IS( (typename as_pack<T,16>::type), (pack<T,16>) );
  NT2_TEST_TYPE_IS( (typename as_pack<T,32>::type), (pack<T,32>) );
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.Dimension. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_POINT_ON_BORDER_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_POINT_ON_BORDER_HPP

#include <cstddef>

#include <boost/range.hpp>

#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/core/ring_type.hpp>

#include <boost/geometry/geometries/concepts/check.hpp>

#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/algorithms/detail/convert_point_to_point.hpp>
#include <boost/geometry/algorithms/detail/disjoint.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace point_on_border
{

template<typename Point>
struct get_point
{
    static inline bool apply(Point& destination, Point const& source, bool)
    {
        destination = source;
        return true;
    }
};

template<typename Point, std::size_t Dimension, std::size_t DimensionCount>
struct midpoint_helper
{
    template <typename InputPoint>
    static inline bool apply(Point& p, InputPoint const& p1, InputPoint const& p2)
    {
        typename coordinate_type<Point>::type const two = 2;
        set<Dimension>(p,
                    (get<Dimension>(p1) + get<Dimension>(p2)) / two);
        return midpoint_helper<Point, Dimension + 1, DimensionCount>::apply(p, p1, p2);
    }
};

template <typename Point, std::size_t DimensionCount>
struct midpoint_helper<Point, DimensionCount, DimensionCount>
{
    template <typename InputPoint>
    static inline bool apply(Point& , InputPoint const& , InputPoint const& )
    {
        return true;
    }
};

template<typename Point, typename Range>
struct point_on_range
{
    static inline bool apply(Point& point, Range const& range, bool midpoint)
    {
        const std::size_t n = boost::size(range);
        if (midpoint && n > 1)
        {
            typedef typename boost::range_iterator
                <
                    Range const
                >::type iterator;

            iterator it = boost::begin(range);
            iterator prev = it++;
            while (it != boost::end(range)
                && detail::equals::equals_point_point(*it, *prev))
            {
                prev = it++;
            }
            if (it != boost::end(range))
            {
                return midpoint_helper
                    <
                        Point,
                        0, dimension<Point>::value
                    >::apply(point, *prev, *it);
            }
        }

        if (n > 0)
        {
            geometry::detail::conversion::convert_point_to_point(*boost::begin(range), point);
            return true;
        }
        return false;
    }
};

template<typename Point, typename Polygon>
struct point_on_polygon
{
    static inline bool apply(Point& point, Polygon const& polygon, bool midpoint)
    {
        return point_on_range
            <
                Point,
                typename ring_type<Polygon>::type
            >::apply(point, exterior_ring(polygon), midpoint);
    }
};

template<typename Point, typename Box>
struct point_on_box
{
    static inline bool apply(Point& point, Box const& box, bool midpoint)
    {
        if (midpoint)
        {
            Point p1, p2;
            detail::assign::assign_box_2d_corner<min_corner, min_corner>(box, p1);
            detail::assign::assign_box_2d_corner<max_corner, min_corner>(box, p2);
            midpoint_helper
                <
                    Point,
                    0, dimension<Point>::value
                >::apply(point, p1, p2);
        }
        else
        {
            detail::assign::assign_box_2d_corner<min_corner, min_corner>(box, point);
        }

        return true;
    }
};

}} // namespace detail::point_on_border
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename GeometryTag,
    typename Point,
    typename Geometry

>
struct point_on_border
{};

template<typename Point>
struct point_on_border<point_tag, Point, Point>
    : detail::point_on_border::get_point<Point>
{};

template<typename Point, typename Linestring>
struct point_on_border<linestring_tag, Point, Linestring>
    : detail::point_on_border::point_on_range<Point, Linestring>
{};

template<typename Point, typename Ring>
struct point_on_border<ring_tag, Point, Ring>
    : detail::point_on_border::point_on_range<Point, Ring>
{};

template<typename Point, typename Polygon>
struct point_on_border<polygon_tag, Point, Polygon>
    : detail::point_on_border::point_on_polygon<Point, Polygon>
{};

template<typename Point, typename Box>
struct point_on_border<box_tag, Point, Box>
    : detail::point_on_border::point_on_box<Point, Box>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

/*!
\brief Take point on a border
\ingroup overlay
\tparam Geometry geometry type. This also defines the type of the output point
\param point to assign
\param geometry geometry to take point from
\param midpoint boolean flag, true if the point should not be a vertex, but some point
    in between of two vertices
\return TRUE if successful, else false.
    It is only false if polygon/line have no points
\note for a polygon, it is always a point on the exterior ring
\note for take_midpoint, it is not taken from two consecutive duplicate vertices,
    (unless there are no other).
 */
template <typename Point, typename Geometry>
inline bool point_on_border(Point& point,
            Geometry const& geometry,
            bool midpoint = false)
{
    concept::check<Point>();
    concept::check<Geometry const>();

    typedef typename point_type<Geometry>::type point_type;

    return dispatch::point_on_border
            <
                typename tag<Geometry>::type,
                Point,
                Geometry
            >::apply(point, geometry, midpoint);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_POINT_ON_BORDER_HPP


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

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

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

/**
 *  @file DiscreteConditional.cpp
 *  @date Feb 14, 2011
 *  @author Duy-Nguyen Ta
 *  @author Frank Dellaert
 */

#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/discrete/Signature.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/debug.h>

#include <boost/make_shared.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>

#include <vector>
#include <algorithm>
#include <stdexcept>

using namespace std;

namespace gtsam {

  /* ******************************************************************************** */
  DiscreteConditional::DiscreteConditional(const size_t nrFrontals,
      const DecisionTreeFactor& f) :
      IndexConditional(f.keys(), nrFrontals), Potentials(
          f / (*f.sum(nrFrontals))) {
  }

  /* ******************************************************************************** */
  DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint,
      const DecisionTreeFactor& marginal) :
      IndexConditional(joint.keys(), joint.size() - marginal.size()), Potentials(
          ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal) {
    if (ISDEBUG("DiscreteConditional::DiscreteConditional"))
      cout << (firstFrontalKey()) << endl;  //TODO Print all keys
  }

  /* ******************************************************************************** */
  DiscreteConditional::DiscreteConditional(const Signature& signature) :
      IndexConditional(signature.indices(), 1), Potentials(
          signature.discreteKeysParentsFirst(), signature.cpt()) {
  }

  /* ******************************************************************************** */
  void DiscreteConditional::print(const std::string& s, const IndexFormatter& formatter) const {
    std::cout << s << std::endl;
    IndexConditional::print(s, formatter);
    Potentials::print(s);
  }

  /* ******************************************************************************** */
  bool DiscreteConditional::equals(const DiscreteConditional& other, double tol) const {
    return IndexConditional::equals(other, tol)
        && Potentials::equals(other, tol);
  }

  /* ******************************************************************************** */
  Potentials::ADT DiscreteConditional::choose(
      const Values& parentsValues) const {
    ADT pFS(*this);
    BOOST_FOREACH(Index key, parents())
      try {
        Index j = (key);
        size_t value = parentsValues.at(j);
        pFS = pFS.choose(j, value);
      } catch (exception&) {
        throw runtime_error(
            "DiscreteConditional::choose: parent value missing");
      };
    return pFS;
  }

  /* ******************************************************************************** */
  void DiscreteConditional::solveInPlace(Values& values) const {
    // TODO: Abhijit asks: is this really the fastest way? He thinks it is.

    ADT pFS = choose(values); // P(F|S=parentsValues)

    // Initialize
    Values mpe;
    double maxP = 0;

    DiscreteKeys keys;
    BOOST_FOREACH(Index idx, frontals()) {
      DiscreteKey dk(idx,cardinality(idx));
      keys & dk;
    }
    // Get all Possible Configurations
    vector<Values> allPosbValues = cartesianProduct(keys);

    // Find the MPE
    BOOST_FOREACH(Values& frontalVals, allPosbValues) {
      double pValueS = pFS(frontalVals); // P(F=value|S=parentsValues)
      // Update MPE solution if better
      if (pValueS > maxP) {
        maxP = pValueS;
        mpe = frontalVals;
      }
    }

    //set values (inPlace) to mpe
    BOOST_FOREACH(Index j, frontals()) {
      values[j] = mpe[j];
    }
  }

  /* ******************************************************************************** */
  void DiscreteConditional::sampleInPlace(Values& values) const {
    assert(nrFrontals() == 1);
    Index j = (firstFrontalKey());
    size_t sampled = sample(values); // Sample variable
    values[j] = sampled; // store result in partial solution
  }

  /* ******************************************************************************** */
  size_t DiscreteConditional::solve(const Values& parentsValues) const {

    // TODO: is this really the fastest way? I think it is.
    ADT pFS = choose(parentsValues); // P(F|S=parentsValues)

    // Then, find the max over all remaining
    // TODO, only works for one key now, seems horribly slow this way
    size_t mpe = 0;
    Values frontals;
    double maxP = 0;
    assert(nrFrontals() == 1);
    Index j = (firstFrontalKey());
    for (size_t value = 0; value < cardinality(j); value++) {
      frontals[j] = value;
      double pValueS = pFS(frontals); // P(F=value|S=parentsValues)
      // Update MPE solution if better
      if (pValueS > maxP) {
        maxP = pValueS;
        mpe = value;
      }
    }
    return mpe;
  }

  /* ******************************************************************************** */
  size_t DiscreteConditional::sample(const Values& parentsValues) const {

    using boost::uniform_real;
    static boost::mt19937 gen(2); // random number generator

    bool debug = ISDEBUG("DiscreteConditional::sample");

    // Get the correct conditional density
    ADT pFS = choose(parentsValues); // P(F|S=parentsValues)
    if (debug) GTSAM_PRINT(pFS);

    // get cumulative distribution function (cdf)
    // TODO, only works for one key now, seems horribly slow this way
    assert(nrFrontals() == 1);
    Index j = (firstFrontalKey());
    size_t nj = cardinality(j);
    vector<double> cdf(nj);
    Values frontals;
    double sum = 0;
    for (size_t value = 0; value < nj; value++) {
      frontals[j] = value;
      double pValueS = pFS(frontals); // P(F=value|S=parentsValues)
      sum += pValueS; // accumulate
      if (debug) cout << sum << " ";
      if (pValueS == 1) {
        if (debug) cout << "--> " << value << endl;
        return value; // shortcut exit
      }
      cdf[value] = sum;
    }

    // inspired by http://www.boost.org/doc/libs/1_46_1/doc/html/boost_random/tutorial.html
    uniform_real<> dist(0, cdf.back());
    boost::variate_generator<boost::mt19937&, uniform_real<> > die(gen, dist);
    size_t sampled = lower_bound(cdf.begin(), cdf.end(), die()) - cdf.begin();
    if (debug) cout << "-> " << sampled << endl;

    return sampled;

    return 0;
  }

  /* ******************************************************************************** */
  void DiscreteConditional::permuteWithInverse(const Permutation& inversePermutation){
    IndexConditional::permuteWithInverse(inversePermutation);
    Potentials::permuteWithInverse(inversePermutation);
  }

/* ******************************************************************************** */

} // namespace