// Copyright 2020 Erik Teichmann <kontakt.teichmann@gmail.com>

#ifndef INCLUDE_SAM_SYSTEM_RK4_NETWORK_HPP_
#define INCLUDE_SAM_SYSTEM_RK4_NETWORK_HPP_

#include <vector>

#include <boost/numeric/odeint/integrate/check_adapter.hpp>
#include <boost/numeric/odeint/integrate/integrate_n_steps.hpp>
#include <boost/numeric/odeint/integrate/null_observer.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>

#include "./generic_network.hpp"

namespace sam {

/*! \brief A network that is integrated with a 4th order Runge-Kutta method.
 *
 * A network that is integrated with a Runge-Kutta method of 4th order. The
 * integrator is implemented by the odeint library in boost
 * (https://www.boost.org/doc/libs/1_72_0/libs/numeric/odeint/doc/html/index.html).
 */
template<typename ODE, typename data_type = double>
class RK4Network: public GenericNetwork<ODE, data_type> {
 public:
  using typename GenericNetwork<ODE, data_type>::node_size_type;
  using typename GenericNetwork<ODE, data_type>::state_type;
  using typename GenericNetwork<ODE, data_type>::matrix_type;

  template<typename... Ts>
  explicit RK4Network(node_size_type node_sizes, unsigned int dimension,
                     Ts... parameters);

  template<typename observer_type = boost::numeric::odeint::null_observer>
  void Integrate(double dt, unsigned int number_steps,
                 observer_type observer
                     = boost::numeric::odeint::null_observer());

 private:
  boost::numeric::odeint::runge_kutta4<state_type> stepper_;
};

// Implementation

template<typename ODE, typename data_type>
template<typename... Ts>
RK4Network<ODE, data_type>::RK4Network(node_size_type node_sizes,
                                      unsigned int dimension,
                                      Ts... parameters)
    : GenericNetwork<ODE, data_type>(node_sizes, dimension, parameters...) {}

template<typename ODE, typename data_type>
template<typename observer_type>
void RK4Network<ODE, data_type>::Integrate(double dt, unsigned int number_steps,
                                           observer_type observer) {
      this->t_ = boost::numeric::odeint::integrate_n_steps(
          stepper_, *(this->ode_), this->x_, this->t_, dt, number_steps,
          observer);
}

}  // namespace sam

#endif  // INCLUDE_SAM_SYSTEM_RK4_NETWORK_HPP_


#include "RevoluteJoint.h"
#include <Eigen/Geometry>

#include "RigidBodyManipulator.h" // todo: remove this when I remove setupOldKinematicTree

using namespace Eigen;

RevoluteJoint::RevoluteJoint(const std::string& name, const Isometry3d& transform_to_parent_body, const Vector3d& rotation_axis) :
    FixedAxisOneDoFJoint(name, transform_to_parent_body, spatialJointAxis(rotation_axis)), rotation_axis(rotation_axis)
{
  assert(abs(rotation_axis.norm()-1)<1e-10);
}

RevoluteJoint::~RevoluteJoint()
{
  // empty
}

Isometry3d RevoluteJoint::jointTransform(const Eigen::Ref<const VectorXd>& q) const
{
  Isometry3d ret(AngleAxisd(q[0], rotation_axis));
  ret.makeAffine();
  return ret;
}

Matrix<double, TWIST_SIZE, 1> RevoluteJoint::spatialJointAxis(const Vector3d& rotation_axis)
{
  Matrix<double, TWIST_SIZE, 1> ret;
  ret.topRows<3>() = rotation_axis;
  ret.bottomRows<3>() = Vector3d::Zero();
  return ret;
}

void RevoluteJoint::setupOldKinematicTree(RigidBodyManipulator* model, int body_ind, int position_num_start, int velocity_num_start) const
{
  FixedAxisOneDoFJoint::setupOldKinematicTree(model,body_ind,position_num_start,velocity_num_start);
  model->bodies[body_ind]->pitch = 0.0;

  Vector3d z_axis(0.0,0.0,1.0);
  if (rotation_axis.dot(z_axis)<1-1e-4) {
//    std::cout << "T_body_to_joint (before) = " << std::endl << model->bodies[body_ind]->T_body_to_joint << std::endl;
    Vector4d a;
    a << rotation_axis.cross(z_axis), acos(rotation_axis.dot(z_axis));
    if ((std::abs(a(0))<1e-4) && (std::abs(a(1))<1e-4) && (std::abs(a(2))<1e-4))
      a.head(3) << 0.0, 1.0, 0.0;
//    std::cout << "axis_angle = " << a.transpose() << std::endl;
    model->bodies[body_ind]->T_body_to_joint.topLeftCorner(3,3) = axis2rotmat(a);
//    std::cout << "T_body_to_joint (after) = " << std::endl << model->bodies[body_ind]->T_body_to_joint << std::endl;
  }
}


//---------------------------------------------------------------------------//
//!
//! \file   Geometry_AdvancedModel.hpp
//! \author Alex Robinson
//! \brief  The advanced geometry model base class declaration
//!
//---------------------------------------------------------------------------//

#ifndef GEOMETRY_ADVANCED_MODEL_HPP
#define GEOMETRY_ADVANCED_MODEL_HPP

// Boost Includes
#include <boost/units/systems/cgs/area.hpp>

// FRENSIE Includes
#include "Geometry_Model.hpp"

namespace Geometry{

//! The advanced model base class
class AdvancedModel : public Model
{

public:

  //! The surface id set type
  typedef std::set<EntityId> SurfaceIdSet;

  //! The surface id array type
  typedef std::vector<EntityId> SurfaceIdArray;

  //! The surface estimator data type
  typedef std::tuple<EstimatorType,ParticleType,SurfaceIdArray> SurfaceEstimatorData;

  //! surface estimator id data map type
  typedef std::map<EstimatorId,SurfaceEstimatorData> SurfaceEstimatorIdDataMap;

  //! The area unit
  typedef boost::units::cgs::area AreaUnit;

  //! The area quantity
  typedef boost::units::quantity<AreaUnit> Area;

  //! Constructor
  AdvancedModel()
  { /* ... */ }

  //! Destructor
  virtual ~AdvancedModel()
  { /* ... */ }

  //! Check if this is an advanced model
  bool isAdvanced() const override
  { return true; }

  //! Check if the model has surface estimator data
  virtual bool hasSurfaceEstimatorData() const = 0;

  //! Get the surfaces
  virtual void getSurfaces( SurfaceIdSet& surfaces ) const = 0;

  //! Get the surface estimator data
  virtual void getSurfaceEstimatorData( SurfaceEstimatorIdDataMap& surface_estimator_id_data_map ) const = 0;

  //! Check if a surface exists
  virtual bool doesSurfaceExist(
                            const EntityId surface_id ) const = 0;

  //! Get the surface area
  virtual Area getSurfaceArea(
                            const EntityId surface_id ) const = 0;

  //! Check if the surface is a reflecting surface
  virtual bool isReflectingSurface(
                            const EntityId surface_id ) const = 0;

private:

  // Save the model to an archive
  template<typename Archive>
  void save( Archive& ar, const unsigned version ) const
  { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP( Model ); }

  // Load the model from an archive
  template<typename Archive>
  void load( Archive& ar, const unsigned version )
  { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP( Model ); }

  BOOST_SERIALIZATION_SPLIT_MEMBER();

  // Declare the boost serialization access object as a friend
  friend class boost::serialization::access;
};

} // end Geometry namespace

BOOST_SERIALIZATION_ASSUME_ABSTRACT_CLASS( AdvancedModel, Geometry );
BOOST_SERIALIZATION_CLASS_VERSION( AdvancedModel, Geometry, 0 );

#endif // end GEOMETRY_ADVANCED_MODEL_HPP

//---------------------------------------------------------------------------//
// end Geometry_AdvancedModel.hpp
//---------------------------------------------------------------------------//


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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
//==============================================================================
#ifndef BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_SSE_SSE2_ALL_HPP_INCLUDED
#define BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_SSE_SSE2_ALL_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT

#include <boost/simd/reduction/functions/all.hpp>
#include <boost/simd/include/functions/simd/genmask.hpp>
#include <boost/simd/sdk/simd/logical.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::all_, boost::simd::tag::sse2_,
                              (A0),
                              ((simd_<type16_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename meta::scalar_of<A0>::type sA0;
    typedef typename meta::as_logical<sA0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return result_type(_mm_movemask_epi8(genmask(a0)) == 0xFFFF);
    }
  };
} } }

#endif
#endif


#include <voxelized_geometry_tools/device_pointcloud_voxelization.hpp>

#if defined(_OPENMP)
#include <omp.h>
#endif

#include <algorithm>
#include <atomic>
#include <cmath>
#include <cstdint>
#include <fstream>
#include <memory>
#include <stdexcept>
#include <vector>

#include <Eigen/Geometry>
#include <voxelized_geometry_tools/collision_map.hpp>
#include <voxelized_geometry_tools/cuda_voxelization_helpers.h>
#include <voxelized_geometry_tools/opencl_voxelization_helpers.h>
#include <voxelized_geometry_tools/pointcloud_voxelization_interface.hpp>

namespace voxelized_geometry_tools
{
namespace pointcloud_voxelization
{
VoxelizerRuntime DevicePointCloudVoxelizer::DoVoxelizePointClouds(
    const CollisionMap& static_environment, const double step_size_multiplier,
    const PointCloudVoxelizationFilterOptions& filter_options,
    const std::vector<PointCloudWrapperSharedPtr>& pointclouds,
    CollisionMap& output_environment) const
{
  EnforceAvailable();

  const std::chrono::time_point<std::chrono::steady_clock> start_time =
      std::chrono::steady_clock::now();

  // Allocate device-side memory for tracking grids. Note that at least one grid
  // is always allocated so that filtering is consistent, even if no points are
  // raycast.
  const size_t num_tracking_grids =
      std::max(pointclouds.size(), static_cast<size_t>(1));

  std::unique_ptr<TrackingGridsHandle> tracking_grids =
      helper_interface_->PrepareTrackingGrids(
          static_environment.GetTotalCells(),
          static_cast<int32_t>(num_tracking_grids));
  if (tracking_grids->GetNumTrackingGrids() != num_tracking_grids)
  {
    throw std::runtime_error("Failed to allocate device tracking grid");
  }

  // Get X_GW, the transform from grid origin to world
  const Eigen::Isometry3d& X_GW =
      static_environment.GetInverseOriginTransform();

  // Prepare grid data
  const float inverse_step_size =
      static_cast<float>(1.0 /
          (static_environment.GetResolution() * step_size_multiplier));
  const float inverse_cell_size =
      static_cast<float>(static_environment.GetGridSizes().InvCellXSize());
  const int32_t num_x_cells =
      static_cast<int32_t>(static_environment.GetNumXCells());
  const int32_t num_y_cells =
      static_cast<int32_t>(static_environment.GetNumYCells());
  const int32_t num_z_cells =
      static_cast<int32_t>(static_environment.GetNumZCells());

  // Do raycasting of the pointclouds
#if defined(_OPENMP)
#pragma omp parallel for
#endif
  for (size_t idx = 0; idx < pointclouds.size(); idx++)
  {
    const PointCloudWrapperSharedPtr& pointcloud = pointclouds.at(idx);

    // Only do work if the pointcloud is non-empty, to avoid passing empty
    // arrays into the device interface.
    if (pointcloud->Size() > 0)
    {
      // Get X_WC, the transform from world to the origin of the pointcloud
      const Eigen::Isometry3d& X_WC =
          pointcloud->GetPointCloudOriginTransform();
      // X_GC, transform from grid origin to the origin of the pointcloud
      const Eigen::Isometry3f grid_pointcloud_transform_float =
          (X_GW * X_WC).cast<float>();

      const float max_range = static_cast<float>(pointcloud->MaxRange());

      // Copy pointcloud
      std::vector<float> raw_points(pointcloud->Size() * 3, 0.0);
      for (int64_t point = 0; point < pointcloud->Size(); point++)
      {
        pointcloud->CopyPointLocationIntoVectorFloat(
            point, raw_points, point * 3);
      }

      // Raycast
      helper_interface_->RaycastPoints(
          raw_points, max_range, grid_pointcloud_transform_float.data(),
          inverse_step_size, inverse_cell_size, num_x_cells, num_y_cells,
          num_z_cells, *tracking_grids, idx);
    }
  }

  const std::chrono::time_point<std::chrono::steady_clock> raycasted_time =
      std::chrono::steady_clock::now();

  // Filter
  const float percent_seen_free =
      static_cast<float>(filter_options.PercentSeenFree());
  const int32_t outlier_points_threshold =
      filter_options.OutlierPointsThreshold();
  const int32_t num_cameras_seen_free =
      filter_options.NumCamerasSeenFree();

  std::unique_ptr<FilterGridHandle> filter_grid =
      helper_interface_->PrepareFilterGrid(
          static_environment.GetTotalCells(),
          static_environment.GetImmutableRawData().data());

  helper_interface_->FilterTrackingGrids(
      *tracking_grids, percent_seen_free, outlier_points_threshold,
      num_cameras_seen_free, *filter_grid);

  // Retrieve & return
  helper_interface_->RetrieveFilteredGrid(
      *filter_grid, output_environment.GetMutableRawData().data());

  const std::chrono::time_point<std::chrono::steady_clock> done_time =
      std::chrono::steady_clock::now();

  return VoxelizerRuntime(
      std::chrono::duration<double>(raycasted_time - start_time).count(),
      std::chrono::duration<double>(done_time - raycasted_time).count());
}

CudaPointCloudVoxelizer::CudaPointCloudVoxelizer(
    const std::map<std::string, int32_t>& options)
{
  device_name_ = "CudaPointCloudVoxelizer";
  helper_interface_ = std::unique_ptr<DeviceVoxelizationHelperInterface>(
      cuda_helpers::MakeCudaVoxelizationHelper(options));
  EnforceAvailable();
}

OpenCLPointCloudVoxelizer::OpenCLPointCloudVoxelizer(
    const std::map<std::string, int32_t>& options)
{
  device_name_ = "OpenCLPointCloudVoxelizer";
  helper_interface_ = std::unique_ptr<DeviceVoxelizationHelperInterface>(
      opencl_helpers::MakeOpenCLVoxelizationHelper(options));
  EnforceAvailable();
}
}  // namespace pointcloud_voxelization
}  // namespace voxelized_geometry_tools


#include "generator/streets/street_geometry.hpp"

#include "generator/boost_helpers.hpp"

#include "indexer/feature_algo.hpp"

#include "geometry/mercator.hpp"

#include "base/exception.hpp"

#include <algorithm>
#include <iterator>
#include <utility>

#include <boost/geometry.hpp>

namespace generator
{
namespace streets
{
Pin StreetGeometry::GetOrChoosePin() const
{
  if (m_pin)
    return *m_pin;

  if (m_highwayGeometry)
    return m_highwayGeometry->ChoosePin();

  if (m_bindingsGeometry)
    return m_bindingsGeometry->GetCentralBinding();

  UNREACHABLE();
}

m2::RectD StreetGeometry::GetBbox() const
{
  if (m_highwayGeometry)
    return m_highwayGeometry->GetBbox();

  if (m_bindingsGeometry)
    return m_bindingsGeometry->GetBbox();

  if (m_pin)
    return {m_pin->m_position, m_pin->m_position};

  UNREACHABLE();
}

void StreetGeometry::SetPin(Pin && pin)
{
  m_pin = std::move(pin);
}

void StreetGeometry::AddHighwayLine(base::GeoObjectId const & osmId, std::vector<m2::PointD> const & line)
{
  if (!m_highwayGeometry)
    m_highwayGeometry = std::make_unique<HighwayGeometry>();

  m_highwayGeometry->AddLine(osmId, line);
}

void StreetGeometry::AddHighwayArea(base::GeoObjectId const osmId, std::vector<m2::PointD> const & border)
{
  if (!m_highwayGeometry)
    m_highwayGeometry = std::make_unique<HighwayGeometry>();

  m_highwayGeometry->AddArea(osmId, border);
}

void StreetGeometry::AddBinding(base::GeoObjectId const & osmId, m2::PointD const & point)
{
  if (!m_bindingsGeometry)
    m_bindingsGeometry = std::make_unique<BindingsGeometry>();

  m_bindingsGeometry->Add(osmId, point);
}

// HighwayGeometry -------------------------------------------------------------------------------------------

Pin HighwayGeometry::ChoosePin() const
{
  if (!m_areaParts.empty())
    return ChooseAreaPin();

  return ChooseMultilinePin();
}

Pin HighwayGeometry::ChooseMultilinePin() const
{
  auto const & lines = m_multiLine.m_lines;
  CHECK(!lines.empty(), ());
  auto longestLine = lines.cbegin();
  auto longestLineLength = longestLine->CalculateLength();
  for (auto l = std::next(longestLine), end = lines.cend(); l != end; ++l)
  {
    auto const length = l->CalculateLength();
    if (longestLineLength < length)
    {
      longestLine = l;
      longestLineLength = length;
    }
  }

  return ChooseLinePin(*longestLine, longestLineLength / 2);
}

Pin HighwayGeometry::ChooseLinePin(Line const & line, double disposeDistance) const
{
  double length = 0.0;

  for (auto const & segment : line.m_segments)
  {
    auto const & points = segment.m_points;
    CHECK_GREATER_OR_EQUAL(points.size(), 2, ());
    for (auto p = points.cbegin(), end = std::prev(points.cend()); p != end; ++p)
    {
      auto const & p1 = *p;
      auto const & p2 = *std::next(p);
      length += MercatorBounds::DistanceOnEarth(p1, p2);
      if (disposeDistance < length)
        return {p1.Mid(p2), segment.m_osmId};
    }
  }

  UNREACHABLE();
}

Pin HighwayGeometry::ChooseAreaPin() const
{
  CHECK(!m_areaParts.empty(), ());
  auto const largestPart = std::max_element(m_areaParts.cbegin(), m_areaParts.cend(),
                                            base::LessBy(&AreaPart::m_area));
  return {largestPart->m_center, largestPart->m_osmId};
}

void HighwayGeometry::AddLine(base::GeoObjectId const & osmId, std::vector<m2::PointD> const & line)
{
  m_multiLine.Add({osmId, line});
  ExtendLimitRect(line);
}

void HighwayGeometry::AddArea(base::GeoObjectId const & osmId, std::vector<m2::PointD> const & border)
{
  m_areaParts.emplace_back(osmId, border);
  ExtendLimitRect(border);
}

m2::RectD const & HighwayGeometry::GetBbox() const
{
  return m_limitRect;
}

void HighwayGeometry::ExtendLimitRect(std::vector<m2::PointD> const & points)
{
  feature::CalcRect(points, m_limitRect);
}

// HighwayGeometry::MultiLine --------------------------------------------------------------------------------

void HighwayGeometry::MultiLine::Add(LineSegment && segment)
{
  for (auto line = m_lines.begin(), end = m_lines.end(); line != end; ++line)
  {
    if (line->Add(std::move(segment)))
    {
      if (Recombine(std::move(*line)))
        m_lines.erase(line);
      return;
    }
  }

  m_lines.emplace_back(Line{{std::move(segment)}});
}

bool HighwayGeometry::MultiLine::Recombine(Line && line)
{
  for (auto & l : m_lines)
  {
    if (l.Concatenate(std::move(line)))
      return true;
  }

  return false;
}

// HighwayGeometry::Line -------------------------------------------------------------------------------------

bool HighwayGeometry::Line::Concatenate(Line && other)
{
  // Ignore self-addition.
  if (&other == this)
    return false;

  CHECK(!m_segments.empty(), ());
  CHECK(!m_segments.front().m_points.empty() && !m_segments.back().m_points.empty(), ());
  auto const & thisStart = m_segments.front().m_points.front();
  auto const & thisEnd = m_segments.back().m_points.back();

  CHECK(!other.m_segments.empty(), ());
  CHECK(!other.m_segments.front().m_points.empty() && !other.m_segments.back().m_points.empty(), ());
  auto const & otherStart = other.m_segments.front().m_points.front();
  auto const & otherEnd = other.m_segments.back().m_points.back();

  if (AlmostEqualAbs(thisEnd, otherStart, kCoordEqualityEps))
  {
    m_segments.splice(m_segments.end(), std::move(other.m_segments));
    return true;
  }

  if (AlmostEqualAbs(thisStart, otherEnd, kCoordEqualityEps))
  {
    m_segments.splice(m_segments.begin(), std::move(other.m_segments));
    return true;
  }

  if (AlmostEqualAbs(thisStart, otherStart, kCoordEqualityEps))
  {
    other.Reverse();
    m_segments.splice(m_segments.begin(), std::move(other.m_segments));
    return true;
  }

  if (AlmostEqualAbs(thisEnd, otherEnd, kCoordEqualityEps))
  {
    other.Reverse();
    m_segments.splice(m_segments.end(), std::move(other.m_segments));
    return true;
  }

  return false;
}

void HighwayGeometry::Line::Reverse()
{
  for (auto & segment : m_segments)
    std::reverse(segment.m_points.begin(), segment.m_points.end());
  std::reverse(m_segments.begin(), m_segments.end());
}

bool HighwayGeometry::Line::Add(LineSegment && segment)
{
  CHECK(!m_segments.empty(), ());
  auto const & startSegment = m_segments.front();
  auto const & endSegment = m_segments.back();
  CHECK(!startSegment.m_points.empty(), ());
  auto const & lineStart = startSegment.m_points.front();
  CHECK(!endSegment.m_points.empty(), ());
  auto const & lineEnd = endSegment.m_points.back();

  // Ignore self-addition.
  if (startSegment.m_osmId == segment.m_osmId || endSegment.m_osmId == segment.m_osmId)
    return false;

  CHECK(!segment.m_points.empty(), ());
  auto const & segmentStart = segment.m_points.front();
  auto const & segmentEnd = segment.m_points.back();

  if (AlmostEqualAbs(lineEnd, segmentStart, kCoordEqualityEps))
  {
    m_segments.push_back(std::move(segment));
    return true;
  }

  if (AlmostEqualAbs(lineEnd, segmentEnd, kCoordEqualityEps))
  {
    std::reverse(segment.m_points.begin(), segment.m_points.end());
    m_segments.push_back(std::move(segment));
    return true;
  }

  if (AlmostEqualAbs(lineStart, segmentEnd, kCoordEqualityEps))
  {
    m_segments.push_front(std::move(segment));
    return true;
  }

  if (AlmostEqualAbs(lineStart, segmentStart, kCoordEqualityEps))
  {
    std::reverse(segment.m_points.begin(), segment.m_points.end());
    m_segments.push_front(std::move(segment));
    return true;
  }

  return false;
}

double HighwayGeometry::Line::CalculateLength() const noexcept
{
  double length = 0.0;
  for (auto const & segment : m_segments)
    length += segment.CalculateLength();
  return length;
}

// HighwayGeometry::LineSegment ------------------------------------------------------------------------------

HighwayGeometry::LineSegment::LineSegment(base::GeoObjectId const & osmId,
                                          std::vector<m2::PointD> const & points)
  : m_osmId{osmId}, m_points{points}
{
  CHECK_GREATER_OR_EQUAL(m_points.size(), 2, ());
}

double HighwayGeometry::LineSegment::CalculateLength() const noexcept
{
  if (m_points.size() < 2)
    return 0.0;

  double length = 0.0;
  for (auto p1 = m_points.cbegin(), p2 = std::next(p1); p2 != m_points.cend(); p1 = p2, ++p2)
    length += MercatorBounds::DistanceOnEarth(*p1, *p2);

  return length;
}

// HighwayGeometry::AreaPart ---------------------------------------------------------------------------------

HighwayGeometry::AreaPart::AreaPart(base::GeoObjectId const & osmId, std::vector<m2::PointD> const & polygon)
  : m_osmId{osmId}
{
  CHECK_GREATER_OR_EQUAL(polygon.size(), 3, ());

  auto boostPolygon = boost_helpers::BoostPolygon{};
  for (auto const & p : polygon)
    boost::geometry::append(boostPolygon, boost_helpers::BoostPoint{p.x, p.y});
  boost::geometry::correct(boostPolygon);

  boost_helpers::BoostPoint center{};
  boost::geometry::centroid(boostPolygon, center);
  m_center = {center.get<0>(), center.get<1>()};

  m_area = boost::geometry::area(boostPolygon);
}

// BindingsGeometry ------------------------------------------------------------------------------------------

Pin BindingsGeometry::GetCentralBinding() const
{
  CHECK(m_centralBinding, ("no bindings"));
  return *m_centralBinding;
}

m2::RectD const & BindingsGeometry::GetBbox() const
{
  CHECK(m_centralBinding, ("no bindings"));
  return m_limitRect;
}

void BindingsGeometry::ExtendLimitRect(m2::PointD const & point)
{
  m_limitRect.Add(point);
}

void BindingsGeometry::Add(base::GeoObjectId const & osmId, m2::PointD const & point)
{
  ExtendLimitRect(point);

  if (!m_centralBinding)
  {
    m_centralBinding = Pin{point, osmId};
    return;
  }

  auto const bboxCenter = GetBbox().Center();
  auto const centralBindingDistance = MercatorBounds::DistanceOnEarth(m_centralBinding->m_position, bboxCenter);
  auto const pointDistance = MercatorBounds::DistanceOnEarth(point, bboxCenter);
  if (pointDistance < centralBindingDistance)
    m_centralBinding = Pin{point, osmId};
}
}  // namespace streets
}  // namespace generator


#ifndef HERMIT_SPIRIT_KARMA_IPV6_HPP
#define HERMIT_SPIRIT_KARMA_IPV6_HPP

#include <cstdint>
#include <utility>
#include <boost/spirit/include/karma.hpp>
#include <boost/spirit/include/phoenix.hpp>
#include <boost/fusion/adapted/std_pair.hpp>
#include <boost/fusion/include/std_pair.hpp>

#include <hermit/ip.hpp>

namespace hermit {
  namespace spirit {
    namespace karma {
    template< typename Iterator >
      class ipv6 : public boost::spirit::karma::grammar<
        Iterator,
        hermit::ipv6()
        > {
      public:
        ipv6() : ipv6::base_type( root ) {
          namespace karma = boost::spirit::karma;
          namespace phx = boost::phoenix;
          root = ( hex_p << ':' << hex_p << ':' << hex_p << ':' << hex_p << ':' <<
                   hex_p << ':' << hex_p << ':' << hex_p << ':' << hex_p )[
            karma::_1 = phx::static_cast_< uint16_t >( phx::at_c< 0 >( karma::_val ) / 0x1000000000000ull ),
            karma::_2 = phx::static_cast_< uint16_t >( phx::at_c< 0 >( karma::_val ) / 0x100000000ull ),
            karma::_3 = phx::static_cast_< uint16_t >( phx::at_c< 0 >( karma::_val ) / 0x10000ull ),
            karma::_4 = phx::static_cast_< uint16_t >( phx::at_c< 0 >( karma::_val ) ),
            karma::_5 = phx::static_cast_< uint16_t >( phx::at_c< 1 >( karma::_val ) / 0x1000000000000ull ),
            karma::_6 = phx::static_cast_< uint16_t >( phx::at_c< 1 >( karma::_val ) / 0x100000000ull ),
            karma::_7 = phx::static_cast_< uint16_t >( phx::at_c< 1 >( karma::_val ) / 0x10000ull ),
            karma::_8 = phx::static_cast_< uint16_t >( phx::at_c< 1 >( karma::_val ) )
          ];
        }
      private:
        boost::spirit::karma::uint_generator<uint16_t, 16> hex_p;
        boost::spirit::karma::rule< Iterator, hermit::ipv6() > root;
      };
    }
  }
}

#endif


#include <Eigen/Geometry>
#include <nav_msgs/Odometry.h>
#include <nodelet/nodelet.h>
#include <quadrotor_msgs/SO3Command.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Float64.h>
#include <tf/transform_datatypes.h>
#include "snav/snapdragon_navigator.h"

class SO3CmdToSnav : public nodelet::Nodelet
{
 public:
  void onInit(void);

 private:
  void so3_cmd_callback(const quadrotor_msgs::SO3Command::ConstPtr &msg);
  void odom_callback(const nav_msgs::Odometry::ConstPtr &odom);
  void imu_callback(const sensor_msgs::Imu::ConstPtr &pose);
  void so3_cmd_to_qc_interface(const quadrotor_msgs::SO3Command::ConstPtr &msg);
  void motors_on();
  void motors_off();

  //controller state
  SnavCachedData *snav_cached_data_struct;

  bool odom_set_, imu_set_, so3_cmd_set_;
  Eigen::Quaterniond odom_q_, imu_q_;

  ros::Subscriber so3_cmd_sub_;
  ros::Subscriber odom_sub_;
  ros::Subscriber imu_sub_;

  int motor_status_;

  double so3_cmd_timeout_;
  ros::Time last_so3_cmd_time_;
  quadrotor_msgs::SO3Command last_so3_cmd_;
};

void SO3CmdToSnav::odom_callback(const nav_msgs::Odometry::ConstPtr &odom)
{
  if(!odom_set_)
    odom_set_ = true;

  odom_q_ = Eigen::Quaterniond(
      odom->pose.pose.orientation.w, odom->pose.pose.orientation.x,
      odom->pose.pose.orientation.y, odom->pose.pose.orientation.z);
    if(so3_cmd_set_ &&
     ((ros::Time::now() - last_so3_cmd_time_).toSec() >= so3_cmd_timeout_))
  {
    ROS_INFO("so3_cmd timeout. %f seconds since last command",
             (ros::Time::now() - last_so3_cmd_time_).toSec());
    const auto last_so3_cmd_ptr =
        boost::make_shared<quadrotor_msgs::SO3Command>(last_so3_cmd_);

    so3_cmd_callback(last_so3_cmd_ptr);
  }
}

void SO3CmdToSnav::imu_callback(const sensor_msgs::Imu::ConstPtr &pose)
{
  if(!imu_set_)
    imu_set_ = true;

  imu_q_ = Eigen::Quaterniond(pose->orientation.w, pose->orientation.x,
                              pose->orientation.y, pose->orientation.z);

  if(so3_cmd_set_ &&
     ((ros::Time::now() - last_so3_cmd_time_).toSec() >= so3_cmd_timeout_))
  {
    ROS_INFO("so3_cmd timeout. %f seconds since last command",
             (ros::Time::now() - last_so3_cmd_time_).toSec());
    const auto last_so3_cmd_ptr =
        boost::make_shared<quadrotor_msgs::SO3Command>(last_so3_cmd_);

    so3_cmd_callback(last_so3_cmd_ptr);
  }
}

void SO3CmdToSnav::motors_on(){

  int ret = sn_spin_props();
  if(ret == -1)
    ROS_ERROR("INIT: not able to send spinning command");
  else
    motor_status_ = 1;

  if(snav_cached_data_struct->general_status.props_state == SN_PROPS_STATE_SPINNING)
    ROS_ERROR("INIT: all the propellers are spinnig");
  else{
    ROS_ERROR("INIT 5: all the propellers are not spinnig");
    motor_status_ = 0;
  }

/*
    int counter_motors = 0;
		do{
		//call the update 0 success
		int res_update = sn_update_data();
		if(res_update ==  -1){
		ROS_ERROR("\nINIT 1: flight software non functional\n");
		return;
		}
		//send minimum thrust and identity attitude
		sn_send_thrust_att_ang_vel_command (0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);

		//run the props 0 success
		//int r = sn_spin_props();
		//if(r == -1)
		  //ROS_ERROR("\nINIT 2: not able to send spinning command\n");

		//controller state
		//int size_cached_struct;
		counter_motors++;

		}
		while(counter_motors > 500);//snav_cached_data_struct->general_status.props_state != SN_PROPS_STATE_SPINNING);

		//sn_send_thrust_att_ang_vel_command (0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
		for(int i = 0; i<10; i++){
		int r = sn_spin_props();
		if(r == -1)
		  ROS_ERROR("\nINIT 2: not able to send spinning command\n");
		else
			motor_status_ = 1;
		  }
		if(snav_cached_data_struct->general_status.props_state == SN_PROPS_STATE_SPINNING)
		  ROS_ERROR("\nINIT 5: all the propellers are spinnig\n");
		else
		  ROS_ERROR("\nINIT 5: all the propellers are not spinnig\n");
*/
}

void SO3CmdToSnav::motors_off(){
	do{
		//call the update 0 success
		int res_update = sn_update_data();
	  //stop the props 0 success
		int ret = sn_stop_props();
		if(ret == -1)
		  ROS_ERROR("Not able to send switch off propellers");

		//send minimum thrust and identity attitude
		sn_send_thrust_att_ang_vel_command (0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
		motor_status_ = 0;
	}
	while(snav_cached_data_struct->general_status.props_state == SN_PROPS_STATE_SPINNING);

	//check the propellers status
	if(snav_cached_data_struct->general_status.props_state == SN_PROPS_STATE_SPINNING)
	  ROS_ERROR("All the propellers are still spinnig");
	else
	  ROS_ERROR("All the propellers are now off");
}

void SO3CmdToSnav::so3_cmd_to_qc_interface(
  const quadrotor_msgs::SO3Command::ConstPtr &msg){
  // grab desired forces and rotation from so3
  const Eigen::Vector3d f_des(msg->force.x, msg->force.y, msg->force.z);

  const Eigen::Quaterniond q_des(msg->orientation.w, msg->orientation.x,
                                 msg->orientation.y, msg->orientation.z);
  const Eigen::Vector3d ang_vel(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z);

  // convert to tf::Quaternion
  tf::Quaternion imu_tf =
      tf::Quaternion(imu_q_.x(), imu_q_.y(), imu_q_.z(), imu_q_.w());
  tf::Quaternion odom_tf =
      tf::Quaternion(odom_q_.x(), odom_q_.y(), odom_q_.z(), odom_q_.w());

  const Eigen::Matrix3d R_cur(odom_q_);

  //const float Psi =
  //    0.5f * (3.0f - (R_des(0, 0) * R_cur(0, 0) + R_des(1, 0) * R_cur(1, 0) +
  //                    R_des(2, 0) * R_cur(2, 0) + R_des(0, 1) * R_cur(0, 1) +
  //                    R_des(1, 1) * R_cur(1, 1) + R_des(2, 1) * R_cur(2, 1) +
  //                    R_des(0, 2) * R_cur(0, 2) + R_des(1, 2) * R_cur(1, 2) +
   //                   R_des(2, 2) * R_cur(2, 2)));

  double throttle = 0.0;
  //if(Psi < 1.0f) // Position control stability guaranteed only when Psi < 1
  //{
  throttle = f_des(0) * R_cur(0, 2) + f_des(1) * R_cur(1, 2) +
               f_des(2) * R_cur(2, 2);

  //convert throttle in grams
  throttle = throttle*1000/9.81;

  int res_update = sn_update_data();
  if(res_update ==  -1){
    ROS_ERROR("\nTRAJ 1: flight software non functional\n");
    return;
  }

  int r = sn_send_thrust_att_ang_vel_command (throttle, q_des.w(), q_des.x(), q_des.y(), q_des.z(), ang_vel(0), ang_vel(1), ang_vel(2));
  if(r == -1)
    ROS_ERROR("\nTRAJ 2: control command not send\n");
}

void SO3CmdToSnav::so3_cmd_callback(
    const quadrotor_msgs::SO3Command::ConstPtr &msg)
{
  if(!so3_cmd_set_)
    so3_cmd_set_ = true;

  //switch on motors
  if(msg->aux.enable_motors && !motor_status_)
  	motors_on();
  else if(!msg->aux.enable_motors)
  	motors_off();

  so3_cmd_to_qc_interface(msg);

  // save last so3_cmd
  last_so3_cmd_ = *msg;
  last_so3_cmd_time_ = ros::Time::now();
}

void SO3CmdToSnav::onInit(void)
{
  ros::NodeHandle priv_nh(getPrivateNodeHandle());

  // get param for so3 command timeout duration
  priv_nh.param("so3_cmd_timeout", so3_cmd_timeout_, 0.25);

  odom_set_ = false;
  imu_set_ = false;
  so3_cmd_set_ = false;
  motor_status_ = 0;
	snav_cached_data_struct = NULL;
  if (sn_get_flight_data_ptr(sizeof(SnavCachedData), &snav_cached_data_struct) != 0)
  {
    ROS_ERROR("\nFailed to get flight data pointer!\n");
    return;
  }

  so3_cmd_sub_ =
      priv_nh.subscribe("so3_cmd", 10, &SO3CmdToSnav::so3_cmd_callback, this,
                        ros::TransportHints().tcpNoDelay());

  odom_sub_ = priv_nh.subscribe("odom", 10, &SO3CmdToSnav::odom_callback,
                                this, ros::TransportHints().tcpNoDelay());

  imu_sub_ = priv_nh.subscribe("imu", 10, &SO3CmdToSnav::imu_callback, this,
                               ros::TransportHints().tcpNoDelay());
}

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(SO3CmdToSnav, nodelet::Nodelet);


// Copyright (c) 2017-2019 The Multiverse developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "crypto.h"

#include <boost/test/unit_test.hpp>

#include "test_fnfn.h"

using namespace multiverse::crypto;

BOOST_FIXTURE_TEST_SUITE(crypto_tests, BasicUtfSetup)

BOOST_AUTO_TEST_CASE(multisign)
{
    srand(time(0));

    int64_t keyCount = 0;
    int64_t signCount = 0, signTime = 0, verifyTime = 0;
    int count = 100;
    for (int i = 0; i < count; i++)
    {
        uint32_t nKey = CryptoGetRand32() % 16 + 1;
        uint32_t nPartKey = CryptoGetRand32() % nKey + 1;

        keyCount += nKey;

        CCryptoKey* keys = new CCryptoKey[nKey];
        std::set<uint256> setPubKey;
        for (int i = 0; i < nKey; i++)
        {
            CryptoMakeNewKey(keys[i]);
            setPubKey.insert(keys[i].pubkey);
        }

        uint256 anchor;
        CryptoGetRand256(anchor);
        uint256 msg;
        CryptoGetRand256(msg);

        std::vector<uint8_t> vchSig;
        for (int i = 0; i < nPartKey; i++)
        {
            boost::posix_time::ptime t0 = boost::posix_time::microsec_clock::universal_time();
            BOOST_CHECK(CryptoMultiSign(setPubKey, keys[i], anchor.begin(), anchor.size(), msg.begin(), msg.size(), vchSig));
            boost::posix_time::ptime t1 = boost::posix_time::microsec_clock::universal_time();
            signTime = (t1 - t0).ticks();
            ++signCount;
        }

        std::set<uint256> setPartKey;
        boost::posix_time::ptime t0 = boost::posix_time::microsec_clock::universal_time();
        BOOST_CHECK(CryptoMultiVerify(setPubKey, anchor.begin(), anchor.size(), msg.begin(), msg.size(), vchSig, setPartKey) && (setPartKey.size() == nPartKey));
        boost::posix_time::ptime t1 = boost::posix_time::microsec_clock::universal_time();
        verifyTime += (t1 - t0).ticks();
    }

    std::cout << "multisign key count : " << keyCount << std::endl;
    std::cout << "multisign sign count : " << signCount << "; average time : " << signTime / signCount << "us." << std::endl;
    std::cout << "multisign verify count : " << count << "; time per count : " << verifyTime / count << "us."
              << " time per key count: " << verifyTime / keyCount << "us." << std::endl;
}

BOOST_AUTO_TEST_SUITE_END()


#include "mtwrap.h"
//#include <cstdint>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>

static boost::random::mt19937 rng(42u);
static boost::random::uniform_int_distribution<> dist(1, 6);

extern "C" {
int roll_die(void)
{
    return dist(rng);
}

uint32_t mt_rand(void)
{
    return rng();
}
void mt_write_random(uint8_t *bytes, size_t len)
{
    while (len >= 4) {
        uint32_t tmp = mt_rand();
        memcpy(bytes, &tmp, sizeof(uint32_t));
        bytes += 4;
        len -= 4;
    }
    if (len) {
        uint32_t tmp = mt_rand();
        memcpy(bytes, &tmp, len);
    }
}

}


// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2018.
//
// This software is released under a three-clause BSD license:
//  * 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 any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// 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 ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS 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.
//
// --------------------------------------------------------------------------
// $Maintainer: Timo Sachsenberg $
// $Authors: David Wojnar $
// --------------------------------------------------------------------------
//
#include <OpenMS/MATH/STATISTICS/PosteriorErrorProbabilityModel.h>

#include <OpenMS/CONCEPT/Constants.h>
#include <OpenMS/CONCEPT/LogStream.h>
#include <OpenMS/DATASTRUCTURES/String.h>
#include <OpenMS/DATASTRUCTURES/ListUtils.h>
#include <OpenMS/FORMAT/TextFile.h>
#include <OpenMS/MATH/STATISTICS/StatisticFunctions.h>
#include <OpenMS/METADATA/PeptideIdentification.h>
#include <OpenMS/METADATA/ProteinIdentification.h>
#include <OpenMS/METADATA/PeptideHit.h>

#include <QDir>

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

#include <algorithm>

using namespace std;

namespace OpenMS
{
  namespace Math
  {
    PosteriorErrorProbabilityModel::PosteriorErrorProbabilityModel() :
      DefaultParamHandler("PosteriorErrorProbabilityModel"),
      incorrectly_assigned_fit_param_(GaussFitter::GaussFitResult(-1, -1, -1)),
      correctly_assigned_fit_param_(GaussFitter::GaussFitResult(-1, -1, -1)),
      negative_prior_(0.5), max_incorrectly_(0), max_correctly_(0), smallest_score_(0)
    {
      defaults_.setValue("out_plot", "", "If given, the some output files will be saved in the following manner: <out_plot>_scores.txt for the scores and <out_plot> which contains the fitted values for each step of the EM-algorithm, e.g., out_plot = /usr/home/OMSSA123 leads to /usr/home/OMSSA123_scores.txt, /usr/home/OMSSA123 will be written. If no directory is specified, e.g. instead of '/usr/home/OMSSA123' just OMSSA123, the files will be written into the working directory.", ListUtils::create<String>("advanced,output file"));
      defaults_.setValue("number_of_bins", 100, "Number of bins used for visualization. Only needed if each iteration step of the EM-Algorithm will be visualized", ListUtils::create<String>("advanced"));
      defaults_.setValue("incorrectly_assigned", "Gumbel", "for 'Gumbel', the Gumbel distribution is used to plot incorrectly assigned sequences. For 'Gauss', the Gauss distribution is used.", ListUtils::create<String>("advanced"));
      defaults_.setValue("max_nr_iterations", 1000, "Bounds the number of iterations for the EM algorithm when convergence is slow.", ListUtils::create<String>("advanced"));
      defaults_.setValidStrings("incorrectly_assigned", ListUtils::create<String>("Gumbel,Gauss"));
      defaultsToParam_();
      getNegativeGnuplotFormula_ = &PosteriorErrorProbabilityModel::getGumbelGnuplotFormula;
      getPositiveGnuplotFormula_ = &PosteriorErrorProbabilityModel::getGaussGnuplotFormula;
    }

    PosteriorErrorProbabilityModel::~PosteriorErrorProbabilityModel()
    {
    }

    bool PosteriorErrorProbabilityModel::fit(std::vector<double>& search_engine_scores)
    {
      // nothing to fit?
      if (search_engine_scores.empty()) { return false; }

      //-------------------------------------------------------------
      // Initializing Parameters
      //-------------------------------------------------------------
      sort(search_engine_scores.begin(), search_engine_scores.end());

      smallest_score_ = search_engine_scores[0];

      vector<double> x_scores{search_engine_scores};
      for (double & d : x_scores) { d += fabs(smallest_score_) + 0.001; }

      negative_prior_ = 0.7;
      if (param_.getValue("incorrectly_assigned") == "Gumbel")
      {
        incorrectly_assigned_fit_param_.x0 = Math::mean(x_scores.begin(), x_scores.begin() + ceil(0.5 * x_scores.size())) + x_scores[0];
        incorrectly_assigned_fit_param_.sigma = Math::sd(x_scores.begin(), x_scores.end(), incorrectly_assigned_fit_param_.x0);
        incorrectly_assigned_fit_param_.A = 1   / sqrt(2 * Constants::PI * pow(incorrectly_assigned_fit_param_.sigma, 2));
        //TODO: Currently, the fit is calculated using the Gauss.
        getNegativeGnuplotFormula_ = &PosteriorErrorProbabilityModel::getGumbelGnuplotFormula;
      }
      else
      {
        incorrectly_assigned_fit_param_.x0 = Math::mean(x_scores.begin(), x_scores.begin() + ceil(0.5 * x_scores.size())) + x_scores[0];
        incorrectly_assigned_fit_param_.sigma = Math::sd(x_scores.begin(), x_scores.end(), incorrectly_assigned_fit_param_.x0);
        incorrectly_assigned_fit_param_.A = 1   / sqrt(2 * Constants::PI * pow(incorrectly_assigned_fit_param_.sigma, 2));
        getNegativeGnuplotFormula_ = &PosteriorErrorProbabilityModel::getGaussGnuplotFormula;
      }
      getPositiveGnuplotFormula_ = &PosteriorErrorProbabilityModel::getGaussGnuplotFormula;

      Size x_score_start = std::min(x_scores.size() - 1, (Size) ceil(x_scores.size() * 0.7)); // if only one score is present, ceil(...) will yield 1, which is an invalid index
      correctly_assigned_fit_param_.x0 = Math::mean(x_scores.begin() + x_score_start, x_scores.end()) + x_scores[x_score_start]; //(gauss_scores.begin()->getX() + (gauss_scores.end()-1)->getX())/2;
      correctly_assigned_fit_param_.sigma = incorrectly_assigned_fit_param_.sigma;
      correctly_assigned_fit_param_.A = 1.0   / sqrt(2 * Constants::PI * pow(correctly_assigned_fit_param_.sigma, 2));

      vector<double> incorrect_density, correct_density;
      fillDensities(x_scores, incorrect_density, correct_density);

      double maxlike = computeMaxLikelihood(incorrect_density, correct_density);

      //-------------------------------------------------------------
      // create files for output
      //-------------------------------------------------------------
      bool output_plots  = (param_.getValue("out_plot").toString().trim().length() > 0);
      TextFile file;
      if (output_plots)
      {
        // create output directory (if not already present)
        QDir dir(param_.getValue("out_plot").toString().toQString());
        if (!dir.cdUp())
        {
          LOG_ERROR << "Could not navigate to output directory for plots from '" << String(dir.dirName()) << "'." << std::endl;
          return false;
        }
        if (!dir.exists() && !dir.mkpath("."))
        {
          LOG_ERROR << "Could not create output directory for plots '" << String(dir.dirName()) << "'." << std::endl;
          return false;
        }
        //
        file = initPlots(x_scores);
      }

      //-------------------------------------------------------------
      // Estimate Parameters - EM algorithm
      //-------------------------------------------------------------
      bool stop_em_init = false;
      Int max_itns = param_.getValue("max_nr_iterations");
      int delta = 6;
      int itns = 0;

      do
      {
        //-------------------------------------------------------------
        // E-STEP
        double one_minus_sum_posterior = one_minus_sum_post(incorrect_density, correct_density);
        double sum_posterior = sum_post(incorrect_density, correct_density);

        // new mean
        double sum_positive_x0 = sum_pos_x0(x_scores, incorrect_density, correct_density);
        double sum_negative_x0 = sum_neg_x0(x_scores, incorrect_density, correct_density);

        double positive_mean = sum_positive_x0 / one_minus_sum_posterior;
        double negative_mean = sum_negative_x0 / sum_posterior;

        //i new standard deviation
        double sum_positive_sigma = sum_pos_sigma(x_scores, incorrect_density, correct_density, positive_mean);
        double sum_negative_sigma = sum_neg_sigma(x_scores, incorrect_density, correct_density, negative_mean);

        // update parameters
        correctly_assigned_fit_param_.x0 = positive_mean;
        if (sum_positive_sigma  != 0)
        {
          correctly_assigned_fit_param_.sigma = sqrt(sum_positive_sigma / one_minus_sum_posterior);
          correctly_assigned_fit_param_.A = 1 / sqrt(2 * Constants::PI * pow(correctly_assigned_fit_param_.sigma, 2));
        }

        incorrectly_assigned_fit_param_.x0 = negative_mean;
        if (sum_negative_sigma  != 0)
        {
          incorrectly_assigned_fit_param_.sigma = sqrt(sum_negative_sigma / sum_posterior);
          incorrectly_assigned_fit_param_.A = 1 / sqrt(2 * Constants::PI * pow(incorrectly_assigned_fit_param_.sigma, 2));
        }

        // compute new prior probabilities negative peptides
        fillDensities(x_scores, incorrect_density, correct_density);
        sum_posterior = sum_post(incorrect_density, correct_density);
        negative_prior_ = sum_posterior / x_scores.size();

        double new_maxlike(computeMaxLikelihood(incorrect_density, correct_density));
        if (boost::math::isnan(new_maxlike - maxlike)
          || new_maxlike < maxlike)
        {
          return false;
        }

        // check termination criterium
        if ((new_maxlike - maxlike) < pow(10.0, -delta) || itns >= max_itns)
        {
          if (itns >= max_itns)
          {
            LOG_WARN << "Number of iterations exceeded. Convergence criterion not met. Last likelihood increase: " << (new_maxlike - maxlike) << endl;
            LOG_WARN << "Algorithm returns probabilites for suboptimal fit. You might want to try raising the max. number of iterations and have a look at the distribution." << endl;
          }
          stop_em_init = true;
          sum_posterior = sum_post(incorrect_density, correct_density);
          negative_prior_ = sum_posterior / x_scores.size();

        }

        if (output_plots)
        {
          String formula1, formula2, formula3;
          formula1 = ((this)->*(getNegativeGnuplotFormula_))(incorrectly_assigned_fit_param_) + "* " + String(negative_prior_); //String(incorrectly_assigned_fit_param_.A) +" * exp(-(x - " + String(incorrectly_assigned_fit_param_.x0) + ") ** 2 / 2 / (" + String(incorrectly_assigned_fit_param_.sigma) + ") ** 2)"+ "*" + String(negative_prior_);
          formula2 = ((this)->*(getPositiveGnuplotFormula_))(correctly_assigned_fit_param_) + "* (1 - " + String(negative_prior_) + ")"; //String(correctly_assigned_fit_param_.A) +" * exp(-(x - " + String(correctly_assigned_fit_param_.x0) + ") ** 2 / 2 / (" + String(correctly_assigned_fit_param_.sigma) + ") ** 2)"+ "* (1 - " + String(negative_prior_) + ")";
          formula3 = getBothGnuplotFormula(incorrectly_assigned_fit_param_, correctly_assigned_fit_param_);
          // important: use single quotes for paths, since otherwise backslashes will not be accepted on Windows!
          file.addLine("plot '" + (String)param_.getValue("out_plot") + "_scores.txt' with boxes, " + formula1 + " , " + formula2 + " , " + formula3);
        }
        //update maximum likelihood
        maxlike = new_maxlike;
        ++itns;
      } while (!stop_em_init);

      //-------------------------------------------------------------
      // Finished fitting
      //-------------------------------------------------------------
      if (param_.getValue("incorrectly_assigned") == "Gumbel")
      {
        max_incorrectly_ = getGumbel_(incorrectly_assigned_fit_param_.x0, incorrectly_assigned_fit_param_);
      }
      else
      {
        max_incorrectly_ = incorrectly_assigned_fit_param_.eval(incorrectly_assigned_fit_param_.x0);
      }
      max_correctly_ = correctly_assigned_fit_param_.eval(correctly_assigned_fit_param_.x0);

      if (output_plots)
      {
        String formula1 = ((this)->*(getNegativeGnuplotFormula_))(incorrectly_assigned_fit_param_) + "*" + String(negative_prior_); //String(incorrectly_assigned_fit_param_.A) +" * exp(-(x - " + String(incorrectly_assigned_fit_param_.x0) + ") ** 2 / 2 / (" + String(incorrectly_assigned_fit_param_.sigma) + ") ** 2)"+ "*" + String(negative_prior_);
        String formula2 = ((this)->*(getPositiveGnuplotFormula_))(correctly_assigned_fit_param_) + "* (1 - " + String(negative_prior_) + ")"; // String(correctly_assigned_fit_param_.A) +" * exp(-(x - " + String(correctly_assigned_fit_param_.x0) + ") ** 2 / 2 / (" + String(correctly_assigned_fit_param_.sigma) + ") ** 2)"+ "* (1 - " + String(negative_prior_) + ")";
        String formula3 = getBothGnuplotFormula(incorrectly_assigned_fit_param_, correctly_assigned_fit_param_);
        // important: use single quotes for paths, since otherwise backslashes will not be accepted on Windows!
        file.addLine("plot '" + (String)param_.getValue("out_plot") + "_scores.txt' with boxes, " + formula1 + " , " + formula2 + " , " + formula3);
        file.store((String)param_.getValue("out_plot"));
        tryGnuplot((String)param_.getValue("out_plot"));
      }
      return true;
    }

    bool PosteriorErrorProbabilityModel::fit(std::vector<double>& search_engine_scores, vector<double>& probabilities)
    {
      bool return_value = fit(search_engine_scores);

      if (!return_value) return false;

      probabilities = std::vector<double>(search_engine_scores);
      for (double & p : probabilities) { p = computeProbability(p); }

      return true;
    }

    void PosteriorErrorProbabilityModel::fillDensities(vector<double>& x_scores, vector<double>& incorrect_density, vector<double>& correct_density)
    {
      if (incorrect_density.size() != x_scores.size())
      {
        incorrect_density.resize(x_scores.size());
        correct_density.resize(x_scores.size());
      }
      vector<double>::iterator incorrect(incorrect_density.begin());
      vector<double>::iterator correct(correct_density.begin());
      for (double const & score : x_scores)
      {
        // TODO: incorrect is currently filled with gauss as fitting gumble is not supported
        *incorrect = incorrectly_assigned_fit_param_.eval(score);
        *correct = correctly_assigned_fit_param_.eval(score);
        ++incorrect;
        ++correct;
      }
    }

    double PosteriorErrorProbabilityModel::computeMaxLikelihood(vector<double>& incorrect_density, vector<double>& correct_density)
    {
      double maxlike(0);
      vector<double>::iterator incorrect = incorrect_density.begin();
      for (vector<double>::iterator correct = correct_density.begin(); correct < correct_density.end(); ++correct, ++incorrect)
      {
        maxlike += log10(negative_prior_ * (*incorrect) + (1 - negative_prior_) * (*correct));
      }
      return maxlike;
    }

    double PosteriorErrorProbabilityModel::one_minus_sum_post(vector<double>& incorrect_density, vector<double>& correct_density)
    {
      double one_min(0);
      vector<double>::iterator incorrect = incorrect_density.begin();
      for (vector<double>::iterator correct = correct_density.begin(); correct < correct_density.end(); ++correct, ++incorrect)
      {
        one_min +=  1  - ((negative_prior_ * (*incorrect)) / ((negative_prior_ * (*incorrect)) + (1 - negative_prior_) * (*correct)));
      }
      return one_min;
    }

    double PosteriorErrorProbabilityModel::sum_post(vector<double>& incorrect_density, vector<double>& correct_density)
    {
      double post(0);
      vector<double>::iterator incorrect = incorrect_density.begin();
      for (vector<double>::iterator correct = correct_density.begin(); correct < correct_density.end(); ++correct, ++incorrect)
      {
        post += ((negative_prior_ * (*incorrect)) / ((negative_prior_ * (*incorrect)) + (1 - negative_prior_) * (*correct)));
      }
      return post;
    }

    double PosteriorErrorProbabilityModel::sum_pos_x0(vector<double>& x_scores, vector<double>& incorrect_density, vector<double>& correct_density)
    {
      double pos_x0(0);
      vector<double>::iterator the_x = x_scores.begin();
      vector<double>::iterator incorrect = incorrect_density.begin();
      for (vector<double>::iterator correct = correct_density.begin(); correct < correct_density.end(); ++correct, ++incorrect, ++the_x)
      {
        pos_x0 += ((1  - ((negative_prior_ * (*incorrect)) / ((negative_prior_ * (*incorrect)) + (1 - negative_prior_) * (*correct)))) * (*the_x));
      }
      return pos_x0;
    }

    double PosteriorErrorProbabilityModel::sum_neg_x0(vector<double>& x_scores, vector<double>& incorrect_density, vector<double>& correct_density)
    {
      double neg_x0(0);
      vector<double>::iterator the_x = x_scores.begin();
      vector<double>::iterator correct = correct_density.begin();
      for (vector<double>::iterator incorrect = incorrect_density.begin(); incorrect < incorrect_density.end(); ++correct, ++incorrect, ++the_x)
      {
        neg_x0 += ((((negative_prior_ * (*incorrect)) / ((negative_prior_ * (*incorrect)) + (1 - negative_prior_) * (*correct)))) * (*the_x));
      }
      return neg_x0;
    }

    double PosteriorErrorProbabilityModel::sum_pos_sigma(vector<double>& x_scores, vector<double>& incorrect_density, vector<double>& correct_density, double positive_mean)
    {
      double pos_sigma(0);
      vector<double>::iterator the_x = x_scores.begin();
      vector<double>::iterator incorrect = incorrect_density.begin();
      for (vector<double>::iterator correct = correct_density.begin(); correct < correct_density.end(); ++correct, ++incorrect, ++the_x)
      {
        pos_sigma += ((1  - ((negative_prior_ * (*incorrect)) / ((negative_prior_ * (*incorrect)) + (1 - negative_prior_) * (*correct)))) * pow((*the_x) - positive_mean, 2));
      }
      return pos_sigma;
    }

    double PosteriorErrorProbabilityModel::sum_neg_sigma(vector<double>& x_scores, vector<double>& incorrect_density, vector<double>& correct_density, double positive_mean)
    {
      double neg_sigma(0);
      vector<double>::iterator the_x = x_scores.begin();
      vector<double>::iterator incorrect = incorrect_density.begin();
      for (vector<double>::iterator correct = correct_density.begin(); correct < correct_density.end(); ++correct, ++incorrect, ++the_x)
      {
        neg_sigma += ((((negative_prior_ * (*incorrect)) / ((negative_prior_ * (*incorrect)) + (1 - negative_prior_) * (*correct)))) * pow((*the_x) - positive_mean, 2));
      }
      return neg_sigma;
    }

    double PosteriorErrorProbabilityModel::computeProbability(double score) const
    {
      score = score + fabs(smallest_score_) + 0.001;
      double x_neg, x_pos;

      // the score is smaller than the peak of incorrectly assigned sequences. To ensure that the probabilities wont rise again use the incorrectly assigned peak for computation
      if (score < incorrectly_assigned_fit_param_.x0)
      {
        x_neg = max_incorrectly_;
        x_pos = correctly_assigned_fit_param_.eval(score);
      }
      // same as above. However, this time to ensure that probabilities wont drop again.
      else if (score > correctly_assigned_fit_param_.x0)
      {
        x_neg = getGumbel_(score, incorrectly_assigned_fit_param_);
        x_pos = max_correctly_;
      }
      // if its in between use the normal formula
      else
      {
        x_neg = getGumbel_(score, incorrectly_assigned_fit_param_);
        x_pos = correctly_assigned_fit_param_.eval(score);
      }
      return (negative_prior_ * x_neg) / ((negative_prior_ * x_neg) + (1 - negative_prior_) * x_pos);
    }

    TextFile PosteriorErrorProbabilityModel::initPlots(vector<double>& x_scores)
    {
      std::vector<DPosition<2> > points;
      Int number_of_bins = param_.getValue("number_of_bins");
      points.resize(number_of_bins);
      DPosition<2> temp;
      double dividing_score = (x_scores.back() - x_scores[0]) / number_of_bins;

      temp.setX(dividing_score / 2);
      temp.setY(0);
      Int bin = 0;
      points[bin] = temp;
      double temp_divider = dividing_score;
      for (std::vector<double>::iterator it = x_scores.begin(); it < x_scores.end(); ++it)
      {
        if (temp_divider - *it >= 0 && bin < number_of_bins - 1)
        {
          points[bin].setY(points[bin].getY() + 1);
        }
        else if (bin  == number_of_bins - 1)
        {
          points[bin].setY(points[bin].getY() + 1);
        }
        else
        {
          temp.setX((temp_divider + temp_divider + dividing_score) / 2);
          temp.setY(1);
          ++bin;
          points[bin] = temp;
          temp_divider += dividing_score;
        }
      }

      TextFile data_points;
      for (vector<DPosition<2> >::iterator it = points.begin(); it < points.end(); ++it)
      {
        it->setY(it->getY() / (x_scores.size()  * dividing_score));
        data_points << (String(it->getX()) + "\t" + it->getY());
      }
      data_points.store((String)param_.getValue("out_plot") + "_scores.txt");

      TextFile file;
      file << "set terminal pdf color solid linewidth 2.0 rounded";
      //file<<"set style empty solid 0.5 border -1";
      //file<<"set style function lines";
      file << "set xlabel \"discriminant score\"";
      file << "set ylabel \"density\"";
      //TODO: file<<"set title ";
      file << "set key off";
      // important: use single quotes for paths, since otherwise backslashes will not be accepted on Windows!
      file <<  "set output '" + (String)param_.getValue("out_plot") + ".pdf'";
      String formula1 = ((this)->*(getNegativeGnuplotFormula_))(incorrectly_assigned_fit_param_) + "* " + String(negative_prior_); //String(incorrectly_assigned_fit_param_.A) +" * exp(-(x - " + String(incorrectly_assigned_fit_param_.x0) + ") ** 2 / 2 / (" + String(incorrectly_assigned_fit_param_.sigma) + ") ** 2)"+ "*" + String(negative_prior_);
      String formula2 = ((this)->*(getPositiveGnuplotFormula_))(correctly_assigned_fit_param_) + "* (1 - " + String(negative_prior_) + ")"; //String(correctly_assigned_fit_param_.A) +" * exp(-(x - " + String(correctly_assigned_fit_param_.x0) + ") ** 2 / 2 / (" + String(correctly_assigned_fit_param_.sigma) + ") ** 2)"+ "* (1 - " + String(negative_prior_) + ")";
      // important: use single quotes for paths, since otherwise backslashes will not be accepted on Windows!
      file << ("plot '" + (String)param_.getValue("out_plot") + "_scores.txt' with boxes, " + formula1 + " , " + formula2);
      return file;
    }

    const String PosteriorErrorProbabilityModel::getGumbelGnuplotFormula(const GaussFitter::GaussFitResult& params) const
    {
      // build a formula with the fitted parameters for gnuplot
      stringstream formula;
      formula << "(1/" << params.sigma << ") * " << "exp(( " << params.x0 << "- x)/" << params.sigma << ") * exp(-exp((" << params.x0 << " - x)/" << params.sigma << "))";
      return formula.str();
    }

    const String PosteriorErrorProbabilityModel::getGaussGnuplotFormula(const GaussFitter::GaussFitResult& params) const
    {
      stringstream formula;
      formula << params.A << " * exp(-(x - " << params.x0 << ") ** 2 / 2 / (" << params.sigma << ") ** 2)";
      return formula.str();
    }

    const String PosteriorErrorProbabilityModel::getBothGnuplotFormula(const GaussFitter::GaussFitResult& incorrect, const GaussFitter::GaussFitResult& correct) const
    {
      stringstream formula;
      formula << negative_prior_ << "*" <<  ((this)->*(getNegativeGnuplotFormula_))(incorrect) << " + (1-" << negative_prior_ << ")*" << ((this)->*(getPositiveGnuplotFormula_))(correct);
      return formula.str();
    }

    void PosteriorErrorProbabilityModel::plotTargetDecoyEstimation(vector<double>& target, vector<double>& decoy)
    {
      if (target.size() == 0 || decoy.size() == 0)
      {
        StringList empty;
        if (target.size() == 0) empty.push_back("target");
        if (decoy.size() == 0) empty.push_back("decoy");
        LOG_WARN << "Target-Decoy plot was called, but '" << ListUtils::concatenate(empty, "' and '") << "' has no data! Unable to create a target-decoy plot." << std::endl;
        return;
      }
      Int number_of_bins = param_.getValue("number_of_bins");
      std::vector<DPosition<3> > points(number_of_bins);
      DPosition<3> temp;

      sort(target.begin(), target.end());
      sort(decoy.begin(), decoy.end());

      double dividing_score = (max(target.back(), decoy.back()) /*scores.back()*/ - min(target[0], decoy[0]) /*scores[0]*/) / number_of_bins;

      temp[0] = (dividing_score / 2);
      temp[1] = 0;
      temp[2] = 0;
      Int bin = 0;
      points[bin] = temp;
      double temp_divider = dividing_score;
      for (std::vector<double>::iterator it = target.begin(); it < target.end(); ++it)
      {
        *it = *it + fabs(smallest_score_) + 0.001;
        if (temp_divider - *it >= 0 && bin < number_of_bins - 1)
        {
          points[bin][1] += 1;
        }
        else if (bin  == number_of_bins - 1)
        {
          points[bin][1] += 1;
        }
        else
        {
          temp[0] = ((temp_divider + temp_divider + dividing_score) / 2);
          temp[1] = 1;
          ++bin;
          points[bin] = temp;
          temp_divider += dividing_score;
        }
      }

      bin = 0;
      temp_divider = dividing_score;
      for (std::vector<double>::iterator it = decoy.begin(); it < decoy.end(); ++it)
      {
        *it = *it + fabs(smallest_score_) + 0.001;
        if (temp_divider - *it >= 0 && bin < number_of_bins - 1)
        {
          points[bin][2] += 1;
        }
        else if (bin  == number_of_bins - 1)
        {
          points[bin][2] += 1;
        }
        else
        {
          // temp[0] = ((temp_divider + temp_divider + dividing_score)/2);
          // temp[2] = 1;
          ++bin;
          points[bin][2] = 1;
          temp_divider += dividing_score;
        }
      }

      TextFile data_points;
      for (vector<DPosition<3> >::iterator it = points.begin(); it < points.end(); ++it)
      {
        (*it)[1] = ((*it)[1] / ((decoy.size() + target.size())  * dividing_score));
        (*it)[2] = ((*it)[2] / ((decoy.size() + target.size())  * dividing_score));
        String temp_ = (*it)[0];
        temp_ += "\t";
        temp_ += (*it)[1];
        temp_ += "\t";
        temp_ += (*it)[2];
        data_points << temp_;
      }
      data_points.store((String)param_.getValue("out_plot") + "_target_decoy_scores.txt");
      TextFile file;
      file << "set terminal pdf color solid linewidth 2.0 rounded";
      //file<<"set style empty solid 0.5 border -1";
      //file<<"set style function lines";
      file << "set xlabel \"discriminant score\"";
      file << "set ylabel \"density\"";
      //TODO: file<<"set title ";
      file << "set key off";
      // important: use single quotes for paths, since otherwise backslashes will not be accepted on Windows!
      file << String("set output '") +  (String)param_.getValue("out_plot") + "_target_decoy.pdf'";
      String formula1, formula2;
      formula1 = getGumbelGnuplotFormula(getIncorrectlyAssignedFitResult()) + "* " + String(getNegativePrior()); //String(incorrectly_assigned_fit_param_.A) +" * exp(-(x - " + String(incorrectly_assigned_fit_param_.x0) + ") ** 2 / 2 / (" + String(incorrectly_assigned_fit_param_.sigma) + ") ** 2)"+ "*" + String(negative_prior_);
      formula2 = getGaussGnuplotFormula(getCorrectlyAssignedFitResult()) + "* (1 - " + String(getNegativePrior()) + ")"; //String(correctly_assigned_fit_param_.A) +" * exp(-(x - " + String(correctly_assigned_fit_param_.x0) + ") ** 2 / 2 / (" + String(correctly_assigned_fit_param_.sigma) + ") ** 2)"+ "* (1 - " + String(negative_prior_) + ")";
      // important: use single quotes for paths, since otherwise backslashes will not be accepted on Windows!
      file << ("plot '" + (String)param_.getValue("out_plot") + "_target_decoy_scores.txt'   using 1:3  with boxes fill solid 0.8 noborder, \"" + (String)param_.getValue("out_plot") + "_target_decoy_scores.txt\"  using 1:2  with boxes, " + formula1 + " , " + formula2);
      file.store((String)param_.getValue("out_plot") + "_target_decoy");
      tryGnuplot((String)param_.getValue("out_plot") + "_target_decoy");
    }

    void PosteriorErrorProbabilityModel::tryGnuplot(const String& gp_file)
    {
      LOG_INFO << "Attempting to call 'gnuplot' ...";
      String cmd = String("gnuplot \"") + gp_file + "\"";
      if (system(cmd.c_str()))  // 0 is success!
      {
        LOG_WARN << "Calling 'gnuplot' on '" << gp_file << "' failed. Please create plots manually." << std::endl;
      }
      else LOG_INFO << " success!" << std::endl;

    }

    double PosteriorErrorProbabilityModel::transformScore_(const String & engine, const PeptideHit & hit)
    {
      // Set fixed e-value threshold
      const double smallest_e_value_ = numeric_limits<double>::denorm_min();

      if (engine == "OMSSA")
      {
        return (-1) * log10(max(hit.getScore(), smallest_e_value_));
      }
      else if (engine == "MYRIMATCH" )
      {
        return hit.getScore();
      }
      else if (engine == "XTANDEM")
      {
        return (-1) * log10(max((double)hit.getMetaValue("E-Value"), smallest_e_value_));
      }
      else if (engine == "MASCOT")
      {
        // issue #740: unable to fit data with score 0
        if (hit.getScore() == 0.0)
        {
          return numeric_limits<double>::quiet_NaN();
        }
        // end issue #740
        if (hit.metaValueExists("EValue"))
        {
          return (-1) * log10(max((double)hit.getMetaValue("EValue"), smallest_e_value_));
        }
        if (hit.metaValueExists("expect"))
        {
          return (-1) * log10(max((double)hit.getMetaValue("expect"), smallest_e_value_));
        }
      }
      else if (engine == "SPECTRAST")
      {
        return 100 * hit.getScore(); // f-val
      }
      else if (engine == "SIMTANDEM")
      {
        if (hit.metaValueExists("E-Value"))
        {
          return (-1) * log10(max((double)hit.getMetaValue("E-Value"), smallest_e_value_));
        }
      }
      else if ((engine == "MSGFPLUS") || (engine == "MS-GF+"))
      {
        if (hit.metaValueExists("MS:1002053"))  // name: MS-GF:EValue
        {
          return (-1) * log10(max((double)hit.getMetaValue("MS:1002053"), smallest_e_value_));
        }
        else if (hit.metaValueExists("expect"))
        {
          return (-1) * log10(max((double)hit.getMetaValue("expect"), smallest_e_value_));
        }
      }
      else if (engine == "COMET")
      {
        if (hit.metaValueExists("MS:1002257")) // name: Comet:expectation value
        {
          return (-1) * log10(max((double)hit.getMetaValue("MS:1002257"), smallest_e_value_));
        }
        else if (hit.metaValueExists("expect"))
        {
          return (-1) * log10(max((double)hit.getMetaValue("expect"), smallest_e_value_));
        }
      }

      throw Exception::UnableToFit(__FILE__, __LINE__, OPENMS_PRETTY_FUNCTION, "No parameters for chosen search engine", "The chosen search engine is currently not supported");
    }

    map<String, vector<vector<double>>> PosteriorErrorProbabilityModel::extractAndTransformScores(
      const vector<ProteinIdentification> & protein_ids,
      const vector<PeptideIdentification> & peptide_ids,
      const bool split_charge,
      const bool top_hits_only,
      const bool target_decoy_available,
      const double fdr_for_targets_smaller)
    {
      std::set<Int> charges;
      const StringList search_engines = ListUtils::create<String>("XTandem,OMSSA,MASCOT,SpectraST,MyriMatch,SimTandem,MSGFPlus,MS-GF+,Comet");

      if (split_charge)
      {  // determine different charges in data
        for (PeptideIdentification const & pep_id : peptide_ids)
        {
          const vector<PeptideHit>& hits = pep_id.getHits();
          for (PeptideHit const & hit : hits) { charges.insert(hit.getCharge()); }
        }
        if (charges.empty())
        {
          throw Exception::Precondition(__FILE__, __LINE__, OPENMS_PRETTY_FUNCTION, "'split_charge' is set, but the list of charge states is empty");
        }
      }

      set<Int>::iterator charge_it = charges.begin(); // charges can be empty, no problem if split_charge is not set
      map<String, vector<vector<double> > > all_scores;
      char splitter = ','; // to split the engine from the charge state later on
      do
      {
        vector<double> scores, decoy, target;
        for (String supported_engine : search_engines)
        {
          supported_engine.toUpper();
          for (ProteinIdentification const & prot : protein_ids)
          {
            String search_engine = prot.getSearchEngine();
            search_engine.toUpper();

            if (supported_engine == search_engine)
            {
              for (PeptideIdentification pep : peptide_ids)
              {
                // make sure we are comparing peptide and proteins of the same search run
                if (prot.getIdentifier() == pep.getIdentifier())
                {
                  pep.sort();
                  vector<PeptideHit>& hits = pep.getHits();
                  if (top_hits_only)
                  {
                    if (!hits.empty() && (!split_charge || hits[0].getCharge() == *charge_it))
                    {
                      double score = PosteriorErrorProbabilityModel::transformScore_(supported_engine, hits[0]);
                      if (!boost::math::isnan(score)) // issue #740: ignore scores with 0 values, otherwise you will get the error "unable to fit data"
                      {
                        scores.push_back(score);

                        if (target_decoy_available)
                        {
                          if (hits[0].getScore() < fdr_for_targets_smaller)
                          {
                            target.push_back(score);
                          }
                          else
                          {
                            decoy.push_back(score);
                          }
                        }
                      }
                    }
                  }
                  else
                  {
                    for (PeptideHit const & hit : hits)
                    {
                      if (!split_charge || (hit.getCharge() == *charge_it))
                      {
                        double score = PosteriorErrorProbabilityModel::transformScore_(supported_engine, hit);
                        if (!boost::math::isnan(score)) // issue #740: ignore scores with 0 values, otherwise you will get the error "unable to fit data"
                        {
                          scores.push_back(score);
                        }
                      }
                    }
                  }
                }
              }
            }
          }

          if (scores.size() > 2)
          {
            vector<vector<double> > tmp;
            tmp.push_back(scores);
            tmp.push_back(target);
            tmp.push_back(decoy);

            if (split_charge)
            {
              String engine_with_charge_state = supported_engine + String(splitter) + String(*charge_it);
              all_scores.insert(make_pair(engine_with_charge_state, tmp));
            }
            else
            {
              all_scores.insert(make_pair(supported_engine, tmp));
            }
          }

          scores.clear();
          target.clear();
          decoy.clear();
        }

        if (split_charge) { ++charge_it; }
      } while (charge_it != charges.end());
      return all_scores;
    }

    void PosteriorErrorProbabilityModel::updateScores(
      const PosteriorErrorProbabilityModel & PEP_model,
      const String & search_engine,
      const Int charge,
      const bool prob_correct,
      const bool split_charge,
      vector<ProteinIdentification> & protein_ids,
      vector<PeptideIdentification> & peptide_ids,
      bool & unable_to_fit_data,
      bool & data_might_not_be_well_fit)
    {
      String engine(search_engine);
      unable_to_fit_data = true;
      data_might_not_be_well_fit = true;

      engine.toUpper();
      for (ProteinIdentification & prot : protein_ids)
      {
        String search_engine = prot.getSearchEngine();
        search_engine.toUpper();

        if (engine == search_engine)
        {
          for (PeptideIdentification & pep : peptide_ids)
          {
            if (prot.getIdentifier() == pep.getIdentifier())
            {
              String score_type = pep.getScoreType() + "_score";
              vector<PeptideHit> hits = pep.getHits();
              for (PeptideHit & hit : hits)
              {
                if (!split_charge || (hit.getCharge() == charge))
                {
                  double score;
                  hit.setMetaValue(score_type, hit.getScore());
                  score = PosteriorErrorProbabilityModel::transformScore_(engine, hit);

                  if (boost::math::isnan(score)) // issue #740: ignore scores with 0 values, otherwise you will get the error "unable to fit data"
                  {
                    score = 1.0;
                  }
                  else
                  {
                    score = PEP_model.computeProbability(score);

                    // invalid score? invalid fit!
                    if ((score > 0.0) && (score < 1.0)) unable_to_fit_data = false;
                    if ((score > 0.2) && (score < 0.8)) data_might_not_be_well_fit = false;
                  }
                  hit.setScore(score);
                  if (prob_correct)
                  {
                    hit.setScore(1.0 - score);
                  }
                  else
                  {
                    hit.setScore(score);
                  }
                }
              }
              pep.setHits(hits);
            }
            if (prob_correct)
            {
              pep.setScoreType("Posterior Probability");
              pep.setHigherScoreBetter(true);
            }
            else
            {
              pep.setScoreType("Posterior Error Probability");
              pep.setHigherScoreBetter(false);
            }
          }
        }
      }
    }
  } // namespace Math
} // namespace OpenMS


#include <Rcpp.h>
#include <array>
#include <boost/container/flat_map.hpp>

// Was using C++17 for std::map::try_emplace
// Very much required for boost::container::flat_map - 10x speed gain
// [[Rcpp::plugins(cpp17)]]
//
// Using boost::container::flat_map as it is >3x faster than any of std::*
// [[Rcpp::depends(BH)]]
// sparse_map - https://www.codeproject.com/Articles/866996/Fast-Implementations-of-Maps-with-Integer-Keys-in
// can actually be the fastest map for the task, however I didn't try it yet
// because it requires me to include non-standard header and change the way I
// emplace new items.

// CX report, vectorised, summarising, context-aware, linearly scalable
// PRE-SORTED DATASET IS A REQUIREMENT.
//
// Parses XM tags and outputs summarised CX report only if most frequent context
// is observed in more than 50% of reads (not including +-) and is within ctx
// string parameter.
// Output report is a data.frame with six columns and rows for every cytosine:
// rname (factor), strand (factor), pos, ctx (char), meth, unmeth
//
// 1) all XM positions counted in int[16]: index is equal to char+2>>2&00001111
// 2) when gap in reads or another chr - spit map to res, clear map
// 3) spit if within context and same context in more than 50% of the reads
//
// Here's the ctx_to_idx convertion:
// ctx  bin       +2        >>2&15  idx
// +    00101011  00101101  1011    11
// -    00101101  00101111  1011    11
// .    00101110  00110000  1100    12
// H    01001000  01001010  0010    2
// U    01010101  01010111  0101    5
// X    01011000  01011010  0110    6
// Z    01011010  01011100  0111    7
// h    01101000  01101010  1010    10
// u    01110101  01110111  1101    13
// x    01111000  01111010  1110    14
// z    01111010  01111100  1111    15
//
// [[Rcpp::export("rcpp_cx_report")]]
Rcpp::DataFrame rcpp_cx_report(Rcpp::DataFrame &df,                             // data frame with BAM data
                               Rcpp::LogicalVector &pass,                       // does it pass the threshold
                               std::string ctx)                                 // context string for bases to report
{
  // walking trough bunch of reads <- filling the map
  // pos<<2|strand -> {0: rname,  1: pos,       2: 'H',  3: '',    4: '',   5: 'U',  6: 'X',  7: 'Z',
  //                   8: strand, 9: coverage, 10: 'h', 11: '+-', 12: '.', 13: 'u', 14: 'x', 15: 'z'}
  // boost::container::flat_map<uint64_t, std::array<int,16>>

  Rcpp::IntegerVector rname   = df["rname"];                                    // template rname
  Rcpp::IntegerVector strand  = df["strand"];                                   // template strand
  Rcpp::IntegerVector start   = df["start"];                                    // template start
  Rcpp::IntegerVector templid = df["templid"];                                  // template id, effectively holds indexes of corresponding std::string in std::vector

  // Rcpp::CharacterVector xm    = df["XM"];                                       // merged refspaced template XMs
  Rcpp::XPtr<std::vector<std::string>> xm((SEXP)df.attr("xm_xptr"));            // merged refspaced template XMs, as a pointer to std::vector<std::string>

  // main typedefs
  typedef uint64_t T_key;                                                       // {62bit:pos, 2bit:strand}
  typedef std::array<int,16> T_val;                                             // {0:rname, 1:pos, 8:strand, 9:coverage, and 10 more for 11 valid chars}
  typedef boost::container::flat_map<T_key, T_val> T_cx_fmap;                   // attaboy

  // macros
  #define ctx_to_idx(c) ((c+2)>>2) & 15
  #define spit_results {                                                       \
    for (it=cx_map.begin(); it!=cx_map.end(); it++) {                          \
      it->second[9] /= 2;                              /* half the coverage */ \
      if (it->second[12] > it->second[9])             /* skip if most are . */ \
        continue;                                                              \
      else if ((it->second[2] + it->second[10]) > it->second[9])               \
        max_freq_ctx='H', max_freq_idx=2;                                      \
      else if ((it->second[6] + it->second[14]) > it->second[9])               \
        max_freq_ctx='X', max_freq_idx=6;                                      \
      else if ((it->second[7] + it->second[15]) > it->second[9])               \
        max_freq_ctx='Z', max_freq_idx=7;                                      \
      else continue;                               /* skip if none is > 50% */ \
      if (ctx.find(max_freq_ctx)!=std::string::npos) {     /* if within ctx */ \
        res_rname.push_back(it->second[0]);                        /* rname */ \
        res_strand.push_back(it->second[8]);                      /* strand */ \
        res_pos.push_back(it->second[1]);                            /* pos */ \
        res_ctx.push_back(max_freq_idx);                         /* context */ \
        res_meth.push_back(it->second[max_freq_idx]);               /* meth */ \
        res_unmeth.push_back(it->second[max_freq_idx | 8]);       /* unmeth */ \
      }                                                                        \
    }                                                                          \
    cx_map.clear();                                                            \
    hint = cx_map.end();                                                       \
  }

  // result
  // { (rname, strand, pos, ctx, meth, unmeth) * n }
  std::vector<int> res_rname, res_strand, res_pos, res_ctx, res_meth, res_unmeth;
  size_t nitems = rname.size()*pow(ctx.size()<<2,2);
  // reserving space makes it only slow?
  res_rname.reserve(nitems); res_strand.reserve(nitems);
  res_pos.reserve(nitems); res_ctx.reserve(nitems);
  res_meth.reserve(nitems); res_unmeth.reserve(nitems);

  // iterating over XM vector, saving the results when necessary
  T_cx_fmap cx_map;
  T_cx_fmap::iterator it, hint;
  T_key map_key;
  T_val map_val = {0};
  unsigned int idx_to_increase, max_freq_ctx, max_freq_idx, pass_x;

  cx_map.reserve(100000);                                                       // reserving helps?
  for (unsigned int x=0; x<rname.size(); x++) {
    // checking for the interrupt
    if ((x & 0xFFFF) == 0) Rcpp::checkUserInterrupt();                          // every ~65k reads

    if ((start[x]>map_val[1]) || (rname[x]!=map_val[0])) {
      spit_results;
    }
    map_val[0] = rname[x];
    map_val[8] = strand[x];
    pass_x = (!pass[x])<<3;                                                     // should we lowercase this XM (TRUE==0, FALSE==8)
    for (unsigned int i=0; i<xm->at(templid[x]).size(); i++) {                  // xm->at(templid[x]) is a reference to a corresponding XM string
      map_val[1] = start[x]+i;
      map_key = ((T_key)map_val[1] << 2) | map_val[8];
      idx_to_increase = ctx_to_idx(xm->at(templid[x])[i]);                      // see the table above
      if (idx_to_increase==11) continue;                                        // skip +-
      idx_to_increase |= pass_x;                                                // if not pass - lowercase
      hint = cx_map.try_emplace(hint, map_key, map_val);
      hint->second[idx_to_increase]++;
      hint->second[9]++;                                                        // total coverage
    }
  }
  spit_results;

  Rcpp::DataFrame res = Rcpp::DataFrame::create(                                // final CX report
    Rcpp::Named("rname") = res_rname,                                           // numeric ids (factor) for reference names
    Rcpp::Named("strand") = res_strand,                                         // numeric ids (factor) for reference strands
    Rcpp::Named("pos") = res_pos,                                               // position of cytosine
    Rcpp::Named("context") = res_ctx,                                           // cytosine context
    Rcpp::Named("meth") = res_meth,                                             // number of methylated
    Rcpp::Named("unmeth") = res_unmeth                                          // number of unmethylated
  );

  return res;
}

// test code in R
//

/*** R
# microbenchmark::microbenchmark(rcpp_cx_report(rname, strand, start, xm, pass, ctx), times=10)
*/

// Sourcing:
// Rcpp::sourceCpp("rcpp_cx_report.cpp")

// #############################################################################


#ifndef SPHERE_HPP
#define SPHERE_HPP

#include <algorithm>
#include <cmath>
#include <string>

#include <Eigen/Core>

#include "image.hpp"
#include "surface.hpp"
#include "ray.hpp"

class Sphere : public Surface {
  public:
    Sphere(double radius, Eigen::Vector4d centerPoint, std::string texture = "");

    void setRadius(double radius);
    double getRadius();
    void setCenterPoint(Eigen::Vector4d centerPoint);
    Eigen::Vector4d getCenterPoint();
    Eigen::Vector2d calculateUVMapping(Eigen::Vector4d intersectionPoint);

    std::string getType();
    double calculateIntersectionTime(Ray ray);
    Eigen::Vector4d calculateIntersectionNormal(Eigen::Vector4d intersectionPoint);
    std::shared_ptr<Color> getTextureColor(Eigen::Vector4d intersectionPoint);

  private:
    Image texture;
    double radius;
    Eigen::Vector4d centerPoint;
};

#endif // SPHERE_HPP


#ifndef GRID_SENSOR_HPP_I9SAOOSJ
#define GRID_SENSOR_HPP_I9SAOOSJ

#include <boost/function.hpp>
#include <boost/bind.hpp>

#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_broadcaster.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/common/transforms.h>
#include <pcl_conversions/pcl_conversions.h>

#include <gridmapping2/proxy_gridmap.hpp>

#include <scrollgrid/scrollgrid3.hpp>
#include <scrollgrid/raycasting.hpp>
#include <scrollgrid/occ_raycasting.hpp>
#include <scrollgrid/scrolling_strategies.hpp>
#include <scrollgrid/grid_util.hpp>

#include <opencv/cv.h>
#include <opencv2/core/core.hpp>

#include <Eigen/Dense>

#include <sensor_msgs/Image.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <nav_msgs/Odometry.h>

#include "densecrf.h"

typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixXf_row;

using namespace Eigen;
using namespace std;

namespace ca {

typedef Eigen::Matrix<mem_ix_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixXf_men;

class GridSensor {
 public:

  typedef pcl::PointCloud<pcl::PointXYZ> PointCloudXYZ;
  typedef pcl::PointCloud<pcl::PointXYZRGB> CloudXYZRGB;

  //Parameters specific for rgbd grid  static const uint8_t CA_SG_UNKNOWN;
  uint8_t CA_SG_UNKNOWN;
  int32_t CA_SG_COMPLETELY_FREE;
  int32_t CA_SG_COMPLETELY_OCCUPIED;
  int32_t CA_SG_BARELY_FREE;
  int32_t CA_SG_BARELY_OCCUPIED;
  int32_t CA_SG_BELIEF_UPDATE_POS; // when hit
  int32_t CA_SG_BELIEF_UPDATE_NEG;

 public:

  GridSensor(ros::NodeHandle& n):
	initialized_(false),
	counter(0) {

	CA_SG_UNKNOWN = 128;  //128   255
	CA_SG_COMPLETELY_FREE = 0;
	CA_SG_COMPLETELY_OCCUPIED = 255;//255  254
	CA_SG_BARELY_FREE = 117;
	CA_SG_BARELY_OCCUPIED = 135; //129  128  //change grid_visualization.hpp accordingly
	CA_SG_BELIEF_UPDATE_POS = 10; // when hit  20
	CA_SG_BELIEF_UPDATE_NEG = 4;  // when pass through  2 (raw)

	n.param<int>("Grid_SG_BARELY_FREE", CA_SG_BARELY_FREE, CA_SG_BARELY_FREE);
	n.param<int>("Grid_SG_BARELY_OCCUPIED", CA_SG_BARELY_OCCUPIED, CA_SG_BARELY_OCCUPIED);
	n.param<int>("Grid_SG_BELIEF_UPDATE_POS", CA_SG_BELIEF_UPDATE_POS, CA_SG_BELIEF_UPDATE_POS);
	n.param<int>("Grid_SG_BELIEF_UPDATE_NEG", CA_SG_BELIEF_UPDATE_NEG, CA_SG_BELIEF_UPDATE_NEG);

        n.param<std::string>("/grid_sensor/pointCloudFrame", point_cloud_frame_, "/camera_rgb_optical_frame");
        n.param<std::string>("/grid_sensor/worldFrame", world_frame_, "/world_frame");
        n.param<std::string>("/grid_sensor/sharedGridIdentifer",shared_grid_identifier_,"shared_grid_map");
	raw_cloud_pub = n.advertise<CloudXYZRGB> ("/rgbd_grid/raw_stereo_cloud", 50);
	raw_img_pub=n.advertise<sensor_msgs::Image>("/raw_img",10);
	raw_label_img_pub=n.advertise<sensor_msgs::Image>("/raw_label_img",10);
	raw_superpixel_img_pub = n.advertise<sensor_msgs::Image>("/superpixel_img",10);

	odom_pub=n.advertise<nav_msgs::Odometry>( "/odom_pose", 10 );

	proxy_=new ca::ProxyGridMap(shared_grid_identifier_);
        ROS_INFO_STREAM("Grid sensor :"<<point_cloud_frame_<<" , "<<world_frame_);

	n.param ("crf_iterations", crf_iterations, crf_iterations);
	n.param ("use_high_order", use_high_order, use_high_order);
	n.param ("use_high_order", use_hierarchical_inference, use_hierarchical_inference);

	n.param ("smooth_xy_stddev", smooth_xy_stddev, smooth_xy_stddev);
	n.param ("smooth_z_stddev", smooth_z_stddev, smooth_z_stddev);
	n.param ("smooth_weight", smooth_weight, smooth_weight);
	n.param ("appear_xy_stddev", appear_xy_stddev, appear_xy_stddev);
	n.param ("appear_z_stddev", appear_z_stddev, appear_z_stddev);
	n.param ("appear_rgb_stddev", appear_rgb_stddev, appear_rgb_stddev);
	n.param ("appear_weight", appear_weight, appear_weight);

	n.param ("depth_scaling", depth_scaling, depth_scaling);
	n.param ("depth_ignore_thres", depth_ignore_thres, depth_ignore_thres);
      }

  virtual ~GridSensor() {
   delete proxy_;
  }

 public:

  void preprocess_pose(ros::Time timnow,const cv::Mat& rgb_img, const cv::Mat& label_img,const cv::Mat& superpixel_img,
		       const Eigen::Matrix4f& transToWorld);
  void AddDepthImg(const cv::Mat& rgb_img,const cv::Mat& label_rgb_img,const cv::Mat& depth_img, const cv::Mat& superpixel_img,
			  const Eigen::Matrix4f pose,MatrixXf_row& frame_label_prob); // pose is 12*1    possibly add label data here
  void reproject_to_images(int current_index);

  MatrixXf_men pixels_to_gridmem;  // store each pixel's correspondence to 3D occupancy grid memory index
  int crf_iterations=0;
  bool use_high_order=false;
  bool use_hierarchical_inference=false;
  void CRF_optimization(const string superpixel_bin_file);

  void ScrollGrid();
  void ClearGrid(const ca::Vec3Ix& start, const ca::Vec3Ix& finish);
  void Init();

  void UpdateProbability(int x,int y,int z, bool endpt){
    bool isadd=false,isremove=false;
    if(!grid3_->is_inside_grid(x,y,z))ROS_INFO_STREAM("updateprob:isinsideGrid"<<x<<" , "<<y<<" , "<<z);
    mem_ix_t mem_ix = grid3_->grid_to_mem(x, y, z);
    int32_t old_value = occ_array3_[mem_ix];
    if(!endpt){
      int32_t new_value = static_cast<int32_t>(occ_array3_[mem_ix])-CA_SG_BELIEF_UPDATE_NEG;
      new_value=std::max(CA_SG_COMPLETELY_FREE, new_value);
      occ_array3_[mem_ix] = static_cast<uint8_t>(new_value);
      isremove = (old_value>=CA_SG_BARELY_FREE && new_value<=CA_SG_BARELY_FREE); //REMOVE point from list
    }
    else{
      int32_t new_value = static_cast<int32_t>(occ_array3_[mem_ix])+CA_SG_BELIEF_UPDATE_POS;
      new_value=std::min(CA_SG_COMPLETELY_OCCUPIED, new_value);
      occ_array3_[mem_ix] = static_cast<uint8_t>(new_value);
      isadd = (old_value<CA_SG_BARELY_FREE && new_value>=CA_SG_BARELY_OCCUPIED);  //ADD point from list
    }
  }

  Eigen::MatrixXi label_to_color_mat;
  int sky_label=1;

  // calibration related
  Eigen::Matrix3f calibration_mat;
  int im_height, im_width;
  void set_up_calibration(const Eigen::Matrix3f& calibration_mat_in,const int im_height_in,const int im_width_in);

  // for reprojection evaluation
  void set_up_reprojections(const int reprojection_frames);
  std::vector<cv::Mat> reproject_depth_imgs; // depth buffer for images
  std::vector<cv::Mat> actual_depth_imgs;
  std::vector<cv::Mat> reproj_label_colors;
  std::vector<cv::Mat> reproj_label_maps;
  std::vector<Eigen::Matrix4f> all_WorldToBodys; // all the frame poses need to project
  std::vector<Eigen::Matrix4f> all_BodyToWorlds; // all the frame poses need to project
  Eigen::VectorXi reproj_frame_inds;

 private:

  std::string point_cloud_frame_;
  std::string world_frame_;
  std::string shared_grid_identifier_;

  ca::ProxyGridMap* proxy_;  //occupancy grids
  ca::ScrollGrid3f* grid3_;
  ca::DenseArray3<uint8_t> occ_array3_;  // true data fields of occupancy value
  ca::DenseArray3<Vector3f> rgb_array3_;  // true data fields of rgb value
  ca::DenseArray3<Vector_Xxf> label_array3_;  // true data fields of label probability
  ca::DenseArray3<int> count_array3_;

  int counter;
  ca::ScrollGrid3f::Vec3 sensor_pos_;

  ros::Publisher raw_cloud_pub,raw_img_pub, raw_label_img_pub, raw_superpixel_img_pub,odom_pub;

  //Scrolling
  ca::ScrollForBaseFrame<float> scroll_strategy_;
  ca::Vec3Ix scroll_cells_;
  ca::Vec3Ix clear_i_min, clear_i_max, clear_j_min, clear_j_max, clear_k_min, clear_k_max;

  //Raycasting
  ca::ScrollGrid3f::Vec3 ray_end_pos_;
  ca::Vec3Ix sensor_pos_ijk_;
  ca::Vec3Ix ray_end_pos_ijk_;

  //Input point cloud is provided in local frame and therefore converted to point_cloud_global
  //If point cloud is global, assign it directly to point_cloud_global //TODO
  tf::TransformListener tf_listener_;
  PointCloudXYZ point_cloud_global_;
  tf::StampedTransform w_to_sensor_transform_;

  bool initialized_;

  // depth to cloud
  cv::Mat_<float> matx_to3d_, maty_to3d_;
  float depth_scaling=1000;
  float depth_ignore_thres=20.0;

  // CRF related
  float smooth_xy_stddev=3;
  float smooth_z_stddev=3;
  float smooth_weight=8;

  float appear_xy_stddev=160;
  float appear_z_stddev=40;
  float appear_rgb_stddev=4;
  float appear_weight=10;

};

}
#endif