/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2013, PAL Robotics, S.L.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the PAL Robotics nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/*
 * Author: Luca Marchionni
 * Author: Bence Magyar
 * Author: Enrique Fernández
 * Author: Paul Mathieu
 */

#include "odometry.h"

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

#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/command-direct-getter.h>
#include <dynamic-graph/command-direct-setter.h>
#include <dynamic-graph/factory.h>

namespace dynamicgraph
{
namespace details
{
  namespace bacc = boost::accumulators;

  Odometry::Odometry(size_t velocity_rolling_window_size)
  : x_(0.0)
  , y_(0.0)
  , heading_(0.0)
  , linear_(0.0)
  , angular_(0.0)
  , wheelSeparation_(0.0)
  , left_wheelRadius_(0.0)
  , right_wheelRadius_(0.0)
  , left_wheel_old_pos_(0.0)
  , right_wheel_old_pos_(0.0)
  , velocity_rolling_window_size_(velocity_rolling_window_size)
  , linear_acc_(RollingWindow::window_size = velocity_rolling_window_size)
  , angular_acc_(RollingWindow::window_size = velocity_rolling_window_size)
  , integrate_fun_(boost::bind(&Odometry::integrateExact, this, _1, _2))
  {
  }

  void Odometry::init()
  {
    // Reset accumulators:
    resetAccumulators();
  }

  bool Odometry::update(double left_pos, double right_pos, double dt)
  {
    /// Get current wheel joint positions:
    const double left_wheel_cur_pos  = left_pos  * left_wheelRadius_;
    const double right_wheel_cur_pos = right_pos * right_wheelRadius_;

    /// Estimate velocity of wheels using old and current position:
    const double left_wheel_est_vel  = left_wheel_cur_pos  - left_wheel_old_pos_;
    const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_;

    /// Update old position with current:
    left_wheel_old_pos_  = left_wheel_cur_pos;
    right_wheel_old_pos_ = right_wheel_cur_pos;

    /// Compute linear and angular diff:
    const double linear  = (right_wheel_est_vel + left_wheel_est_vel) * 0.5 ;
    const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheelSeparation_;

    /// Integrate odometry:
    integrate_fun_(linear, angular);

    /// We cannot estimate the speed with very small time intervals:
    if (dt < 0.0001)
      return false; // Interval too small to integrate with

    /// Estimate speeds using a rolling mean to filter them out:
    linear_acc_(linear/dt);
    angular_acc_(angular/dt);

    linear_ = bacc::rolling_mean(linear_acc_);
    angular_ = bacc::rolling_mean(angular_acc_);

    return true;
  }

  void Odometry::updateOpenLoop(double linear, double angular, double dt)
  {
    /// Save last linear and angular velocity:
    linear_ = linear;
    angular_ = angular;

    /// Integrate odometry:
    integrate_fun_(linear * dt, angular * dt);
  }

  void Odometry::setWheelParams(double wheelSeparation, double left_wheelRadius, double right_wheelRadius)
  {
    wheelSeparation_   = wheelSeparation;
    left_wheelRadius_  = left_wheelRadius;
    right_wheelRadius_ = right_wheelRadius;
  }

  void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
  {
    velocity_rolling_window_size_ = velocity_rolling_window_size;

    resetAccumulators();
  }

  void Odometry::integrateRungeKutta2(double linear, double angular)
  {
    const double direction = heading_ + angular * 0.5;

    /// Runge-Kutta 2nd order integration:
    x_       += linear * cos(direction);
    y_       += linear * sin(direction);
    heading_ += angular;
  }

  /**
   * \brief Other possible integration method provided by the class
   * \param linear
   * \param angular
   */
  void Odometry::integrateExact(double linear, double angular)
  {
    if (fabs(angular) < 1e-6)
      integrateRungeKutta2(linear, angular);
    else
    {
      /// Exact integration (should solve problems when angular is zero):
      const double heading_old = heading_;
      const double r = linear/angular;
      heading_ += angular;
      x_       +=  r * (sin(heading_) - sin(heading_old));
      y_       += -r * (cos(heading_) - cos(heading_old));
    }
  }

  void Odometry::resetAccumulators()
  {
    linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
    angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
  }

} // namespace details

  namespace bacc = boost::accumulators;

  Odometry::Odometry(const std::string& name)
  : Entity (name)
  , wheelsPositionSIN (NULL, "Odometry("+name+")::input(vector)::wheelsPosition")
  , baseVelocityEstimationSOUT (boost::bind (&Odometry::computeVelocityEstimation, this, _1, _2),
      wheelsPositionSIN, "Odometry("+name+")::output(vector)::baseVelocityEstimation")
  , baseVelocityFilteredEstimationSOUT (boost::bind (&Odometry::computeVelocityFilteredEstimation, this, _1, _2),
      baseVelocityEstimationSOUT, "Odometry("+name+")::output(vector)::baseVelocityFilteredEstimation")
  , baseVelocitySIN (NULL, "Odometry("+name+")::input(vector)::baseVelocity")
  , baseConfigSOUT (boost::bind (&Odometry::computeBaseConfig, this, _1, _2),
      baseVelocitySIN, "Odometry("+name+")::output(vector)::baseConfig")
  , basePoseSOUT (boost::bind (&Odometry::computeBasePose, this, _1, _2),
      baseConfigSOUT, "Odometry("+name+")::output(matrixHomogeneous)::basePose")

  , dt_ (1.)
  , x_(0.0)
  , y_(0.0)
  , heading_(0.0)
  , wheelSeparationInv_(0.0)
  , wheelRadii_(0.0, 0.0)
  , wheelOldPos_ (0.0, 0.0)
  , rollingWindowSize_ (10)
  , linear_acc_(RollingWindow::window_size = rollingWindowSize_)
  , angular_acc_(RollingWindow::window_size = rollingWindowSize_)
  {
    signalRegistration (
        wheelsPositionSIN <<
        baseVelocityEstimationSOUT <<
        baseVelocityFilteredEstimationSOUT <<
        baseVelocitySIN <<
        baseConfigSOUT <<
        basePoseSOUT);

    // By default, use the velocity estimation
    // (i.e. loop closed with wheel position sensors).
    baseVelocitySIN.plug (&baseVelocityEstimationSOUT);

    addCommand ("setWheelParams",
        command::makeCommandVoid3 (*this, &Odometry::setWheelParams,
          command::docCommandVoid3 ("Sets the wheel parameters: radius and separation",
            "double: Separation between left and right wheels [m]",
            "double: Left wheel radius [m]",
            "double: Right wheel radius [m]"))
        );
    addCommand ("setBasePose",
        command::makeCommandVoid3 (*this, &Odometry::setBasePose,
          command::docCommandVoid3 ("Set the base pose to integrate from.",
            "double: x [m]",
            "double: y [m]",
            "double: heading [rad]"))
        );
    addCommand ("setPeriod", command::makeDirectSetter (*this, &dt_,
          "Set period (only affects the velocity estimation and nothing else)."));
    addCommand ("getPeriod", command::makeDirectGetter (*this, &dt_,
          "the period"));
  }

  Vector& Odometry::computeVelocityEstimation (Vector& vel, int time)
  {
    vel.resize(2);
    const Vector& wheelCurAng = wheelsPositionSIN (time);

    /// Get current wheel joint positions:
    Eigen::Array2d wheelCurPos = wheelCurAng.array() * wheelRadii_;

    /// Estimate velocity of wheels using old and current position:
    Eigen::Array2d wheelEstVel = ( wheelCurPos - wheelOldPos_ ) / dt_;

    /// Update old position with current:
    wheelOldPos_ = wheelCurPos;

    /// Compute linear and angular diff:
    vel[0] = (wheelEstVel[1] + wheelEstVel[0]) * 0.5                ;
    vel[1] = (wheelEstVel[1] - wheelEstVel[0]) * wheelSeparationInv_;

    /// Estimate speeds using a rolling mean to filter them out:
    linear_acc_ (vel[0]);
    angular_acc_(vel[1]);

    return vel;
  }

  Vector& Odometry::computeVelocityFilteredEstimation (Vector& vel, int time)
  {
    baseVelocityEstimationSOUT.recompute (time);

    vel.resize(2);
    vel[0] = bacc::rolling_mean(linear_acc_);
    vel[1] = bacc::rolling_mean(angular_acc_);

    return vel;
  }

  Vector& Odometry::computeBaseConfig (Vector& base, int time)
  {
    base.resize(3);
    const Vector& vel = baseVelocitySIN (time);
    integrateExact (vel[0] * dt_, vel[1] * dt_);

    base << x_, y_, heading_;
    return base;
  }

  sot::MatrixHomogeneous& Odometry::computeBasePose (sot::MatrixHomogeneous& M, int time)
  {
    const Vector& base = baseConfigSOUT (time);
    M.setIdentity();
    M.translation().head<2>() = base.head<2>();
    double c = cos(base(2)), s = sin(base(2));
    M.linear().topLeftCorner<2,2>() <<
      c, -s,
      s,  c;
    return M;
  }

  void Odometry::setBasePose (const double& x, const double& y, const double& heading)
  {
    x_ = x;
    y_ = y;
    heading_ = heading;

    resetAccumulators ();
  }

  void Odometry::setWheelParams(const double& wheelSeparation, const double& leftWheelRadius, const double& rightWheelRadius)
  {
    wheelSeparationInv_   = 1/wheelSeparation;
    wheelRadii_  << leftWheelRadius, rightWheelRadius;
  }

  void Odometry::setVelocityRollingWindowSize(const size_t& rollingWindowSize)
  {
    rollingWindowSize_ = rollingWindowSize;
    resetAccumulators ();
  }

  void Odometry::integrateRungeKutta2(double linear, double angular)
  {
    const double direction = heading_ + angular * 0.5;

    /// Runge-Kutta 2nd order integration:
    x_       += linear * cos(direction);
    y_       += linear * sin(direction);
    heading_ += angular;
  }

  /**
   * \brief Other possible integration method provided by the class
   * \param linear
   * \param angular
   */
  void Odometry::integrateExact(double linear, double angular)
  {
    if (fabs(angular) < 1e-6)
      integrateRungeKutta2(linear, angular);
    else
    {
      /// Exact integration (should solve problems when angular is zero):
      const double heading_old = heading_;
      const double r = linear/angular;
      heading_ += angular;
      x_       +=  r * (sin(heading_) - sin(heading_old));
      y_       += -r * (cos(heading_) - cos(heading_old));
    }
  }

  void Odometry::resetAccumulators()
  {
    linear_acc_  = RollingMeanAcc(RollingWindow::window_size = rollingWindowSize_);
    angular_acc_ = RollingMeanAcc(RollingWindow::window_size = rollingWindowSize_);
  }

  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (Odometry, "Odometry");

} // namespace diff_drive_controller


#include <aslam/backend/DesignVariable.hpp>
#include <aslam/backend/ErrorTerm.hpp>
#include <aslam/backend/MEstimatorPolicies.hpp>
#include <boost/shared_ptr.hpp>
#include <numpy_eigen/boost_python_headers.hpp>
using namespace boost::python;
using namespace aslam::backend;

void exportMEstimators() {
    class_<MEstimator, boost::shared_ptr<MEstimator>, boost::noncopyable>("MEstimator", no_init)
        .def("getWeight", &MEstimator::getWeight)
        .def("name", &MEstimator::name);

    class_<NoMEstimator, boost::shared_ptr<NoMEstimator>, bases<MEstimator> >("NoMEstimator", init<>());

    class_<GemanMcClureMEstimator, boost::shared_ptr<GemanMcClureMEstimator>, bases<MEstimator> >(
        "GemanMcClureMEstimator", init<double>());

    class_<CauchyMEstimator, boost::shared_ptr<CauchyMEstimator>, bases<MEstimator> >("CauchyMEstimator",
                                                                                init<double>());

    class_<HuberMEstimator, boost::shared_ptr<HuberMEstimator>, bases<MEstimator> >("HuberMEstimator", init<double>());

    class_<BlakeZissermanMEstimator, boost::shared_ptr<BlakeZissermanMEstimator>, bases<MEstimator> >(
        "BlakeZissermanMEstimator", init<double>("BlakeZissermanMEstimator( dimensionOfErrorTerm)"))
        .def(init<double, double, double>(
            "BlakeZissermanMEstimator(dim, pCut, wCut)\n* @param df    The dimension of your error term\n* @param pCut "
            " The probability of error that you want to down-weight. 0.99 means\n*              that the 99% most "
            "likely squared errors will not be much down-weighted\n*              and the remaining 1% will be "
            "smoothly cut to zero weight.\n* @param wCut  The weight (between 0 and 1 not inclusive) that you want the "
            "above probability to have."))

        ;
}


// Copyright 2016, 2017 Peter Dimov.
//
// Distributed under the Boost Software License, Version 1.0.
//
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt

#include <boost/mp11/map.hpp>
#include <boost/mp11/list.hpp>
#include <boost/mp11/integral.hpp>
#include <boost/core/lightweight_test_trait.hpp>
#include <type_traits>
#include <tuple>
#include <utility>

using boost::mp11::mp_int;

template<class T, class U> using inc = mp_int<U::value + 1>;

int main()
{
    using boost::mp11::mp_map_update;
    using boost::mp11::mp_list;

    using M1 = mp_list<>;

    using M2 = mp_map_update< M1, std::pair<char, mp_int<0>>, inc >;
    BOOST_TEST_TRAIT_TRUE((std::is_same<M2, mp_list<std::pair<char, mp_int<0>>>>));

    using M3 = mp_map_update< M2, std::pair<char, mp_int<0>>, inc >;
    BOOST_TEST_TRAIT_TRUE((std::is_same<M3, mp_list<std::pair<char, mp_int<1>>>>));

    using M4 = mp_map_update< M3, std::pair<int, mp_int<0>>, inc >;
    BOOST_TEST_TRAIT_TRUE((std::is_same<M4, mp_list<std::pair<char, mp_int<1>>, std::pair<int, mp_int<0>>>>));

    using M5 = mp_map_update< M4, std::pair<long, mp_int<0>>, inc >;
    BOOST_TEST_TRAIT_TRUE((std::is_same<M5, mp_list<std::pair<char, mp_int<1>>, std::pair<int, mp_int<0>>, std::pair<long, mp_int<0>>>>));

    using M6 = mp_map_update< M5, std::pair<long, mp_int<0>>, inc >;
    BOOST_TEST_TRAIT_TRUE((std::is_same<M6, mp_list<std::pair<char, mp_int<1>>, std::pair<int, mp_int<0>>, std::pair<long, mp_int<1>>>>));

    using M7 = mp_map_update< M6, std::pair<char, mp_int<0>>, inc >;
    BOOST_TEST_TRAIT_TRUE((std::is_same<M7, mp_list<std::pair<char, mp_int<2>>, std::pair<int, mp_int<0>>, std::pair<long, mp_int<1>>>>));

    return boost::report_errors();
}


/* The Image Registration Toolkit (IRTK)
 *
 * Copyright 2008-2015 Imperial College London
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. */

// MUST be included first due to nasty IRTK "round" macro
#include <boost/random.hpp>

#include <irtkPointSamples.h>

typedef boost::mt19937 RandomNumberGeneratorType;

// =============================================================================
// Construction/Destruction
// =============================================================================

// -----------------------------------------------------------------------------
irtkPointSamples::irtkPointSamples(int n, int seed)
:
  irtkPointSet(n), _RandomNumberGenerator(new RandomNumberGeneratorType())
{
  RandomNumberGeneratorType &rng = *(RandomNumberGeneratorType*)_RandomNumberGenerator;
  if (seed < 0) rng.seed(static_cast<unsigned int>(std::time(0)));
  else          rng.seed(seed);
}

// -----------------------------------------------------------------------------
irtkPointSamples::~irtkPointSamples()
{
  delete (RandomNumberGeneratorType*)_RandomNumberGenerator;
}

// =============================================================================
// Uniform grid sampling
// =============================================================================

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleGrid(const irtkPoint &p1, const irtkPoint &p2,
                                  int nx, int ny, int nz)
{
  SampleGrid(p1._x, p1._y, p1._z, p2._x, p2._y, p2._z, nx, ny, nz);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleGrid(const irtkPoint &p1, const irtkPoint &p2,
                                  double dx, double dy, double dz)
{
  SampleGrid(p1._x, p1._y, p1._z, p2._x, p2._y, p2._z, dx, dy, dz);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleGrid(double x1, double y1, double z1,
                                  double x2, double y2, double z2,
                                  int    nx, int    ny, int    nz)
{
  if (x2 < x1) swap(x1, x2);
  if (y2 < y1) swap(y1, y2);
  if (z2 < z1) swap(z1, z2);
  if (nx <= 0) nx = 1;
  if (ny <= 0) ny = 1;
  if (nz <= 0) nz = 1;
  const double sx = x2 - x1;
  const double sy = x2 - x1;
  const double sz = x2 - x1;
  const double dx = sx / nx;
  const double dy = sy / ny;
  const double dz = sz / nz;
  Size(nx * ny * nz);
  int n = 0;
  for (int k = 0; k < nz; ++k)
  for (int j = 0; j < ny; ++j)
  for (int i = 0; i < nx; ++i, ++n) {
    _data[n] = irtkPoint(x1 + i * dx, y1 + j * dy, z1 + k * dz);
  }
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleGrid(double x1, double y1, double z1,
                                  double x2, double y2, double z2,
                                  double dx, double dy, double dz)
{
  if (x2 < x1) swap(x1, x2);
  if (y2 < y1) swap(y1, y2);
  if (z2 < z1) swap(z1, z2);
  if (dx < 0) dx = .0;
  if (dy < 0) dy = .0;
  if (dz < 0) dz = .0;
  const double sx = x2 - x1;
  const double sy = x2 - x1;
  const double sz = x2 - x1;
  const int    nx = (dx > .0 ? round(sx / dx) : 1);
  const int    ny = (dy > .0 ? round(sy / dy) : 1);
  const int    nz = (dz > .0 ? round(sz / dz) : 1);
  Size(nx * ny * nz);
  int n = 0;
  for (int k = 0; k < nz; ++k)
  for (int j = 0; j < ny; ++j)
  for (int i = 0; i < nx; ++i, ++n) {
    _data[n] = irtkPoint(x1 + i * dx, y1 + j * dy, z1 + k * dz);
  }
}

// =============================================================================
// Uniform spherical distribution
// =============================================================================

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleSphere(double r)
{
  SampleSphere(.0, r);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleSphere(double c, double r)
{
  SampleSphere(c, c, c, r, r, r);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleSphere(const irtkPoint & c, double r)
{
  SampleSphere(c._x, c._y, c._z, r, r, r);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleSphere(const irtkPoint & c, double rx, double ry, double rz)
{
  SampleSphere(c._x, c._y, c._z, rx, ry, rz);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleSphere(double cx, double cy, double cz, double r)
{
  SampleSphere(cx, cy, cz, r, r, r);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleSphere(double cx, double cy, double cz,
                                    double rx, double ry, double rz)
{
  typedef boost::uniform_on_sphere<double>                                   Distribution;
  typedef boost::variate_generator<RandomNumberGeneratorType&, Distribution> Generator;

  RandomNumberGeneratorType &rng = *(RandomNumberGeneratorType*)_RandomNumberGenerator;

  Distribution dist(3);
  Generator next(rng, dist);

  vector<double> p(3);
  for (int i = 0; i < _n; ++i) {
    p = next();
    _data[i]._x = cx + rx * p[0];
    _data[i]._y = cy + ry * p[1];
    _data[i]._z = cz + rz * p[2];
  }
}

// =============================================================================
// Normal distribution
// =============================================================================

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleGaussian(double s)
{
  SampleGaussian(.0, s);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleGaussian(double m, double s)
{
  SampleGaussian(m, m, m, s, s, s);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleGaussian(const irtkPoint & m, double s)
{
  SampleGaussian(m._x, m._y, m._z, s, s, s);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleGaussian(const irtkPoint & m, double sx, double sy, double sz)
{
  SampleGaussian(m._x, m._y, m._z, sx, sy, sz);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleGaussian(double mx, double my, double mz, double s)
{
  SampleGaussian(mx, my, mz, s, s, s);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleGaussian(double mx, double my, double mz,
                                      double sx, double sy, double sz)
{
  typedef boost::normal_distribution<double>                                 Distribution;
  typedef boost::variate_generator<RandomNumberGeneratorType&, Distribution> Generator;

  RandomNumberGeneratorType &rng = *(RandomNumberGeneratorType*)_RandomNumberGenerator;

  Distribution distx(mx, sx);
  Distribution disty(my, sy);
  Distribution distz(mz, sz);

  Generator x(rng, distx);
  Generator y(rng, disty);
  Generator z(rng, distz);

  for (int i = 0; i < _n; ++i) {
    _data[i]._x = x();
    _data[i]._y = y();
    _data[i]._z = z();
  }
}


//==============================================================================
//         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_IEEE_FUNCTIONS_GENERIC_FREXP_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_GENERIC_FREXP_HPP_INCLUDED
#include <boost/simd/ieee/functions/frexp.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/fusion/include/std_pair.hpp>
#include <boost/type_traits/is_same.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION_IF( boost::simd::tag::frexp_, tag::cpu_
                                      , (A0)(A2)
                                      , ( boost::is_same
                                          < typename dispatch::meta
                                                      ::as_integer<A0>::type
                                          , A2
                                          >
                                        )
                                      , ((generic_< floating_<A0> >))
                                        ((generic_< integer_<A2> >))
                            )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0,A2 & a2) const
    {
      A0 a1;
      boost::simd::frexp(a0, a1, a2);
      return a1;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::frexp_, tag::cpu_
                                    , (A0)
                                    , ((generic_<floating_<A0> >))
                                    )
  {
    typedef typename dispatch::meta::as_integer<A0, signed>::type exponent;
    typedef std::pair<A0,exponent>                                result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      result_type res;
      boost::simd::frexp( a0, res.first, res.second );
      return res;
    }
  };
} } }

#endif


#include "common.h"

#include "densemarkerbuilder.h"
#include "eigensparsemarkerbuilder.h"
#include "raggedsparsemarkerbuilder.h"

#include <cassert>
#include <iostream>

#include <Eigen/Eigen>
#include <vector>
#include <fstream>
#include <type_traits>

using namespace Eigen;

std::ostream &operator<<(std::ostream &os, const AnalysisType &obj)
{
    os << static_cast<std::underlying_type<AnalysisType>::type>(obj);
    return os;
}

std::ostream &operator<<(std::ostream &os, const InputType &obj)
{
    os << static_cast<std::underlying_type<InputType>::type>(obj);
    return os;
}

std::ostream &operator<<(std::ostream &os, const PreprocessDataType &obj)
{
   os << static_cast<std::underlying_type<PreprocessDataType>::type>(obj);
   return os;
}

MarkerBuilder *builderForType(const PreprocessDataType type)
{
    switch (type)
    {
    case PreprocessDataType::Dense:
        return new DenseMarkerBuilder;

    case PreprocessDataType::SparseEigen:
        return new EigenSparseMarkerBuilder;

    case PreprocessDataType::SparseRagged:
        return new RaggedSparseMarkerBuilder;

    case PreprocessDataType::None:
        // Fall through
    default:
        std::cerr << "builderForType - unhandled DataType:" << type << std::endl;
        assert(false);
        return nullptr;
    }
}

std::string fileWithoutExtension(const std::string &dataFile)
{
    auto const pos = dataFile.find_last_of('.');
    if (pos == std::string::npos) {
        std::cout << "Could remove file extension: " << dataFile << std::endl;
        return dataFile;
    }
    return dataFile.substr(0, pos);
}

std::string fileWithSuffix(const std::string &dataFile, const std::string &suffix)
{
    return fileWithoutExtension(dataFile) + suffix;
}

std::string ppFileForType(PreprocessDataType type, const std::string &dataFile)
{
    const auto fileName = fileWithoutExtension(dataFile);

    switch (type) {
    case PreprocessDataType::Dense:
        return fileName + ".ppbed";

    case PreprocessDataType::SparseEigen:
        return fileName +  ".eigen.sparsebed";

    case PreprocessDataType::SparseRagged:
        return fileName +  ".ragged.sparsebed";

    default:
        std::cerr << "ppFileForType - unsupported DataType: "
             << type
             << std::endl;
        assert(false);
        return {};
    }
}

std::string ppIndexFileForType(PreprocessDataType type, const std::string &dataFile)
{
    const auto fileName = fileWithoutExtension(dataFile);

    switch (type) {
    case PreprocessDataType::Dense:
        return fileName +  ".ppbedindex";

    case PreprocessDataType::SparseEigen:
        return fileName +  ".eigen.sparsebedindex";

    case PreprocessDataType::SparseRagged:
        return fileName + ".ragged.sparsebedindex";

    default:
        std::cerr << "ppIndexFileForType - unsupported DataType: "
             << type
             << std::endl;
        assert(false);
        return {};
    }
}

InputType getInputType(const std::string &dataFile)
{
    auto endsWith = [](std::string const & value, std::string const & ending)
    {
        if (ending.size() > value.size())
            return false;

        return std::equal(ending.rbegin(), ending.rend(), value.rbegin());
    };

    if (endsWith(dataFile, ".bed"))
        return InputType::BED;
    else if (endsWith(dataFile, ".csv"))
        return InputType::CSV;
    else
        return InputType::Unknown;
}

template<typename M>
M load_csv (const std::string & path) {
    std::ifstream indata;
    indata.open(path);
    std::string line;
    std::vector<double> values;
    uint rows = 0;
    while (std::getline(indata, line)) {
        std::stringstream lineStream(line);
        std::string cell;
        while (std::getline(lineStream, cell, ',')) {
            values.push_back(std::stod(cell));
        }
        ++rows;
    }
    return Map<const Matrix<typename M::Scalar, M::RowsAtCompileTime, M::ColsAtCompileTime, RowMajor>>(values.data(), rows, values.size()/rows);
}


// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is licensed under the Apache License, Version 2.0 (the
//  "License"); you may not use this file except in compliance with the
//  License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

#include <asp/Sessions/StereoSessionDGMapRPC.h>
#include <asp/Camera/RPC_XML.h>
#include <asp/Camera/RPCModel.h>
#include <boost/scoped_ptr.hpp>
#include <test/Helpers.h>

#include <vw/Stereo/StereoModel.h>

using namespace vw;
using namespace asp;
using namespace vw::test;

TEST(StereoSessionDGMapRPC, TransformCycle) {
  // Generate GeoReference projection that matches something that the
  // RPC models in dg_example1 and dg_example4 see.
  cartography::GeoReference lowres_georef; // WGS84
  lowres_georef.set_equirectangular( 37.745, -103.29, 37 );
  Matrix<double> tx = math::identity_matrix<3>();
  tx(0,0) = tx(1,1) = 500;
  tx(0,2) = tx(1,2) = -1000;
  lowres_georef.set_transform( tx );

  // Make low res images. We have to overcrop, otherwise the bicubic
  // interpolation of the DEM will have edge effects.
  cartography::GeoReference lowres_overcrop_georef = lowres_georef;
  tx(0,2) = tx(1,2) = -2000;
  lowres_overcrop_georef.set_transform( tx );
  ImageView<float> lowres_overcrop_image(8,8);
  double dem_height = 2287; // Average height of this area
  fill( lowres_overcrop_image, dem_height );
  UnlinkName lowres_dem_name( "lowres_dem.tif" );
  write_georeferenced_image( lowres_dem_name, lowres_overcrop_image, lowres_overcrop_georef );

  // Make hires images
  cartography::GeoReference hires_georef = lowres_georef;
  tx(0,0) = tx(1,1) = 100;
  tx(0,2) = tx(1,2) = -1000;
  hires_georef.set_transform( tx );
  ImageView<float> hires_image(20,20);
  fill( hires_image, 1 );
  ImageView<float> lowres_image(4,4);
  fill( lowres_image, 2 );
  UnlinkName left_image_name("faked_left.tif"), right_image_name("faked_right.tif");
  write_georeferenced_image( left_image_name, lowres_image, lowres_georef );
  write_georeferenced_image( right_image_name, hires_image, hires_georef );

  // Create session
  vw::cartography::GdalWriteOptions opt;
  StereoSessionDGMapRPC session;
  session.initialize( opt, left_image_name, right_image_name,
                      "dg_example1.xml", "dg_example4.xml",
                      "debug/debug", lowres_dem_name);
  RPCXML left_xml, right_xml;
  left_xml.read_from_file( "dg_example1.xml" );
  right_xml.read_from_file( "dg_example4.xml" );
  StereoSessionDGMapRPC::tx_type left_tx = session.tx_left();
  StereoSessionDGMapRPC::tx_type right_tx = session.tx_right();
  cartography::Datum datum;

  // Verify that LX matches what we got by hand.
  for ( size_t j = 0; j < 4; j++ ) {
    for ( size_t i = 0; i < 4; i++ ) {
      Vector3 lonlatheight;
      subvector(lonlatheight,0,2) = lowres_georef.pixel_to_lonlat( Vector2(i,j) );
      lonlatheight.z() = dem_height;
      Vector3 xyz = datum.geodetic_to_cartesian( lonlatheight );
      EXPECT_VECTOR_NEAR( left_xml.rpc_ptr()->point_to_pixel( xyz ),
                          left_tx.reverse( Vector2(i,j) ), 1e-1 );
    }
  }

  // Verify that RX matches what we got by hand
  for ( size_t j = 0; j < 20; j++ ) {
    for ( size_t i = 0; i < 20; i++ ) {
      Vector3 lonlatheight;
      subvector(lonlatheight,0,2) = hires_georef.pixel_to_lonlat( Vector2(i,j) );
      lonlatheight.z() = dem_height;
      Vector3 xyz = datum.geodetic_to_cartesian( lonlatheight );
      EXPECT_VECTOR_NEAR( right_xml.rpc_ptr()->point_to_pixel( xyz ),
                          right_tx.reverse( Vector2(i,j) ), 1e-1 );
    }
  }
}


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

#include <boost/test/unit_test.hpp>

#include <graphene/chain/database.hpp>
#include <graphene/chain/protocol/protocol.hpp>
#include <graphene/chain/exceptions.hpp>

#include <graphene/chain/account_object.hpp>
#include <graphene/chain/asset_object.hpp>
#include <graphene/chain/committee_member_object.hpp>
#include <graphene/chain/proposal_object.hpp>

#include <graphene/db/simple_index.hpp>

#include <fc/crypto/digest.hpp>
#include "../common/database_fixture.hpp"

using namespace graphene::chain;

BOOST_FIXTURE_TEST_SUITE( confidential_tests, database_fixture )
BOOST_AUTO_TEST_CASE( confidential_test )
{ try {
   ACTORS( (dan)(nathan) )
   const asset_object& core = asset_id_type()(db);

   transfer(account_id_type()(db), dan, core.amount(1000000));

   transfer_to_blind_operation to_blind;
   to_blind.amount = core.amount(1000);
   to_blind.from   = dan.id;

   auto owner1_key = fc::ecc::private_key::generate();
   auto owner1_pub = owner1_key.get_public_key();
   auto owner2_key = fc::ecc::private_key::generate();
   auto owner2_pub = owner2_key.get_public_key();

   blind_output out1, out2;
   out1.owner = authority( 1, public_key_type(owner1_pub), 1 );
   out2.owner = authority( 1, public_key_type(owner2_pub), 1 );

   auto InB1  = fc::sha256::hash("InB1");
   auto InB2  = fc::sha256::hash("InB2");
   auto OutB  = fc::sha256::hash("InB2");
   auto nonce1 = fc::sha256::hash("nonce");
   auto nonce2 = fc::sha256::hash("nonce2");

   out1.commitment  = fc::ecc::blind(InB1,250);
   out1.range_proof = fc::ecc::range_proof_sign( 0, out1.commitment, InB1, nonce1, 0, 0, 250 );

   out2.commitment = fc::ecc::blind(InB2,750);
   out2.range_proof = fc::ecc::range_proof_sign( 0, out2.commitment, InB1, nonce2, 0, 0, 750 );

   to_blind.blinding_factor = fc::ecc::blind_sum( {InB1,InB2}, 2 );
   to_blind.outputs = {out2,out1};

   trx.operations = {to_blind};
   sign( trx,  dan_private_key  );
   db.push_transaction(trx);
   trx.signatures.clear();

   BOOST_TEST_MESSAGE( "Transfering from blind to blind with change address" );
   auto Out3B  = fc::sha256::hash("Out3B");
   auto Out4B  = fc::ecc::blind_sum( {InB2,Out3B}, 1 ); // add InB2 - Out3b
   blind_output out3, out4;
   out3.commitment = fc::ecc::blind(Out3B,300);
   out3.range_proof = fc::ecc::range_proof_sign( 0, out3.commitment, InB1, nonce1, 0, 0, 300 );
   out4.commitment = fc::ecc::blind(Out4B,750-300-10);
   out4.range_proof = fc::ecc::range_proof_sign( 0, out3.commitment, InB1, nonce1, 0, 0, 750-300-10 );

   blind_transfer_operation blind_tr;
   blind_tr.fee = core.amount(10);
   blind_tr.inputs.push_back( {out2.commitment, out2.owner} );
   blind_tr.outputs = {out3,out4};
   blind_tr.validate();
   trx.operations = {blind_tr};
   sign( trx,  owner2_key  );
   db.push_transaction(trx);

   BOOST_TEST_MESSAGE( "Attempting to double spend the same commitments" );
   blind_tr.fee = core.amount(11);

   Out4B  = fc::ecc::blind_sum( {InB2,Out3B}, 1 ); // add InB2 - Out3b
   out4.commitment = fc::ecc::blind(Out4B,750-300-11);
   auto out4_amount = 750-300-10;
   out4.range_proof = fc::ecc::range_proof_sign( 0, out3.commitment, InB1, nonce1, 0, 0, 750-300-11 );
   blind_tr.outputs = {out4,out3};
   trx.operations = {blind_tr};
   BOOST_REQUIRE_THROW( db.push_transaction(trx, ~0), graphene::chain::blind_transfer_unknown_commitment );

   BOOST_TEST_MESSAGE( "Transfering from blind to nathan public" );
   out4.commitment = fc::ecc::blind(Out4B,750-300-10);

   transfer_from_blind_operation from_blind;
   from_blind.fee = core.amount(10);
   from_blind.to  = nathan.id;
   from_blind.amount = core.amount( out4_amount - 10 );
   from_blind.blinding_factor = Out4B;
   from_blind.inputs.push_back( {out4.commitment, out4.owner} );
   trx.operations = {from_blind};
   trx.signatures.clear();
   db.push_transaction(trx);

   BOOST_REQUIRE_EQUAL( get_balance( nathan, core ), 750-300-10-10 );

} FC_LOG_AND_RETHROW() }

BOOST_AUTO_TEST_SUITE_END()


#include "parameterparser.h"
#include "CBCT_acquisition.h"
#include "CBCT_binning.h"
#include "hoCuConebeamProjectionOperator.h"
#include "hoNDArray_fileio.h"
#include "hoCuNDArray_math.h"
#include "vector_td_utilities.h"
#include "GPUTimer.h"

#include <boost/program_options.hpp>
#include "dicomWriter.h"
#include <iostream>
#include <algorithm>
#include <sstream>
#include <proton/hdf5_utils.h>

using namespace Gadgetron;
using namespace std;
namespace po = boost::program_options;

int main(int argc, char** argv)
{
  std::string acquisition_filename;
  std::string image_filename;
	floatd2 scale_factor;
	uintd3 imageSize;
	floatd3 is_dims_in_mm;
	int device;
	std::string binning_filename;
 // Parse command line
  //
po::options_description desc("Allowed options");
 desc.add_options()
    ("help", "produce help message")
    ("acquisition,a", po::value<string>(&acquisition_filename)->default_value("acquisition.hdf5"), "Acquisition data")
    ("samples,n",po::value<unsigned int>(),"Number of samples per ray")
    ("output,f", po::value<string>(&image_filename)->default_value("fdk4f.hdf5"), "Output filename")
    ("size,s",po::value<uintd3>(&imageSize)->default_value(uintd3(512,512,1)),"Image size in pixels")
    ("binning,b",po::value<string>(&binning_filename)->default_value("binning.hdf5"),"Binning file for 3d reconstruction (used to exclude projections)")
    ("SAG","Use exact SAG correction if present")
    ("dimensions,d",po::value<floatd3>(&is_dims_in_mm)->default_value(floatd3(256,256,256)),"Image dimensions in mm. Overwrites voxelSize.")
    ("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
    ("downsample,D",po::value<floatd2>(&scale_factor)->default_value(floatd2(1,1)),"Downsample projections this factor")
    ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

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

  std::cout << "Command line options:" << std::endl;
  for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
    boost::any a = it->second.value();
    std::cout << it->first << ": ";
    if (a.type() == typeid(std::string)) std::cout << it->second.as<std::string>();
    else if (a.type() == typeid(int)) std::cout << it->second.as<int>();
    else if (a.type() == typeid(unsigned int)) std::cout << it->second.as<unsigned int>();
    else if (a.type() == typeid(float)) std::cout << it->second.as<float>();
    else if (a.type() == typeid(vector_td<float,3>)) std::cout << it->second.as<vector_td<float,3> >();
    else if (a.type() == typeid(vector_td<int,3>)) std::cout << it->second.as<vector_td<int,3> >();
    else if (a.type() == typeid(vector_td<unsigned int,3>)) std::cout << it->second.as<vector_td<unsigned int,3> >();
    else std::cout << "Unknown type" << std::endl;
    std::cout << std::endl;
  }

	// Load acquisition data
	//

	boost::shared_ptr<CBCT_acquisition> acquisition( new CBCT_acquisition() );
	acquisition->load(acquisition_filename);

	// Downsample projections if requested
	//

    if (scale_factor[0] != 1 || scale_factor[1] != 1)
	{
		GPUTimer timer("Downsampling projections");
        acquisition->downsample(scale_factor[0],scale_factor[1]);
	}

	// Configuring...
	//

	uintd2 ps_dims_in_pixels( acquisition->get_projections()->get_size(0),
			acquisition->get_projections()->get_size(1) );

	floatd2 ps_dims_in_mm( acquisition->get_geometry()->get_FOV()[0],
			acquisition->get_geometry()->get_FOV()[1] );

	float SDD = acquisition->get_geometry()->get_SDD();
	float SAD = acquisition->get_geometry()->get_SAD();

	boost::shared_ptr<CBCT_binning> ps_bd4d(  new CBCT_binning());

	ps_bd4d->load(binning_filename);
	ps_bd4d->print(std::cout);

	// Load the binning data
		//

		boost::shared_ptr<CBCT_binning> binning=ps_bd4d->get_3d_binning() ;

	// Allocate array to hold the result
	//

	std::vector<size_t> is_dims{imageSize[0],imageSize[1],imageSize[2]};

	hoCuNDArray<float> fdk_3d(&is_dims);

	//
	// Standard 3D FDK reconstruction
	//

	boost::shared_ptr< hoCuConebeamProjectionOperator > E( new hoCuConebeamProjectionOperator() );

	E->setup( acquisition, binning, is_dims_in_mm );
	E->set_use_filtered_backprojection(true);

	hoCuNDArray<float> projections(*acquisition->get_projections());
    hoCuNDArray<float> result;
    {
        GPUTimer timer("Running 3D FDK reconstruction");
        E->mult_MH(&projections, &fdk_3d);

        write_nd_array<float>(&fdk_3d, "fdk.real");

        /*4D FDK-MB algorithm starts here. McKinnon GC, RHT Bates,
         *
         *"Towards Imaging the Beating Heart Usefully with a Conventional CT Scanner,"
         *" Biomedical Engineering, IEEE Transactions on , vol.BME-28, no.2, pp.123,127, Feb. 1981
         * doi: 10.1109/TBME.1981.324785
         */

        size_t numBins = ps_bd4d->get_number_of_bins();
        is_dims.push_back(numBins);
        boost::shared_ptr<hoCuConebeamProjectionOperator>
                E4D(new hoCuConebeamProjectionOperator());
        E4D->setup(acquisition, ps_bd4d, is_dims_in_mm);
        E4D->set_use_filtered_backprojection(true);
        E4D->set_domain_dimensions(&is_dims);

        hoCuNDArray<float> fdk(*expand(&fdk_3d, numBins));
        hoCuNDArray<float> diff_proj(projections.get_dimensions());

        E4D->mult_M(&fdk, &diff_proj);
        float scaler = dot(&diff_proj, &projections) / dot(&diff_proj, &diff_proj);
        std::cout << "Scale: " << scaler << std::endl;
        diff_proj *= scaler;
        projections -= diff_proj;
//        projections *= -1.0f;

        result = hoCuNDArray<float>(&is_dims);
        E4D->mult_MH(&projections, &result);
        //result *= 10.0f;
//	axpy(scaler,&fdk,&result);
        result += fdk;
        result *= scaler;
    }
	//write_nd_array<float>( &result, image_filename.c_str() );
	write_dicom(&result,"",is_dims_in_mm);
	saveNDArray2HDF5(&result,image_filename.c_str(),is_dims_in_mm,floatd3(0,0,0),"",0);
	return 0;
}


#ifndef _MAX_FLOW_DOUBLE_HPP_
#define _MAX_FLOW_DOUBLE_HPP_

#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_utility.hpp>
using namespace std;
using namespace boost;

class Network {
public:
  typedef adjacency_list_traits < vecS, vecS, directedS > Traits;
  typedef adjacency_list < vecS, vecS, directedS,
    property < vertex_name_t, std::string,
    property < vertex_index_t, long,
    property < vertex_color_t, boost::default_color_type,
    property < vertex_distance_t, long,
    property < vertex_predecessor_t, Traits::edge_descriptor > > > > >,

    property < edge_capacity_t, double,
    property < edge_residual_capacity_t, double,
    property < edge_reverse_t, Traits::edge_descriptor > > > > Graph;
  typedef Traits::vertex_descriptor Vertex;
  typedef Traits::edge_descriptor Edge;
  Network();
  property_map<Graph, edge_capacity_t>::type capacity;
  property_map<Graph, edge_residual_capacity_t>::type residual_capacity;
  property_map<Graph, vertex_color_t>::type color;
  Vertex AddVertex();
  Edge AddEdge(Vertex &v1, Vertex &v2, const double capacity);
  double MaxFlow(Vertex &s, Vertex &t);
  void BfsOnResidualGraph(Vertex &s);

private:
  Graph g;
  property_map < Graph, edge_reverse_t >::type rev;
  struct NonSaturatedEdges;
};

#endif


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

    @Copyright 2016 Numscale SAS
    @copyright 2016 J.T.Lapreste

    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_DIVIDES_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_DIVIDES_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#if defined(BOOST_SIMD_COMPILER_GCC) && BOOST_SIMD_GCC_VERSION < 40603
#include <boost/simd/function/simd/if_nan_else.hpp>
#include <boost/simd/function/simd/is_eqz_else.hpp>
#include <boost/simd/function/simd/logical_and.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( divides_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::double_<A0>, bs::sse_>
                          , bs::pack_<bd::double_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0
                                    , const A0 & a1 ) const BOOST_NOEXCEPT
    {
//      A0 const that = _mm_div_pd(a0,a1);
#if defined(BOOST_SIMD_COMPILER_GCC) && BOOST_SIMD_GCC_VERSION < 40603
      // workaround for GCC bug #50396 fixed in 4.6.3
      return if_nan_else(logical_and(is_eqz(a0), is_eqz(a1)),  _mm_div_pd(a0,a1));
#else
      return  _mm_div_pd(a0,a1);
#endif
    }
  };

} } }

#endif


//---------------------------------------------------------------------------//
//!
//! \file   DataGen_AdjointElectroionizationSubshellCrossSectionEvaluator.cpp
//! \author Luke Kersting
//! \brief  Adjoint electroionization subshell cross section evaluator definition
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <limits>

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

// FRENSIE Includes
#include "DataGen_AdjointElectroionizationSubshellCrossSectionEvaluator.hpp"
#include "Utility_GaussKronrodIntegrator.hpp"
#include "Utility_PhysicalConstants.hpp"
#include "Utility_ContractException.hpp"
#include "MonteCarlo_TwoDDistributionHelpers.hpp"

namespace DataGen{

// Constructor
AdjointElectroionizationSubshellCrossSectionEvaluator::AdjointElectroionizationSubshellCrossSectionEvaluator(
    const double& binding_energy,
    const Teuchos::RCP<MonteCarlo::ElectroatomicReaction>&
                                     electroionization_subshell_reaction,
    const Teuchos::RCP<const MonteCarlo::ElectroionizationSubshellElectronScatteringDistribution>&
                                     knock_on_distribution )
  : d_binding_energy( binding_energy ),
    d_electroionization_subshell_reaction( electroionization_subshell_reaction ),
    d_knock_on_distribution( knock_on_distribution )
{
  // Make sure the data is valid
  testPrecondition( !d_electroionization_subshell_reaction.is_null() );
  testPrecondition( !d_knock_on_distribution.is_null() );
 // testPrecondition( d_knock_on_distribution.size() > 0 );
}

// Evaluate the differential adjoint electroionization subshell cross section (dc/dx)
double AdjointElectroionizationSubshellCrossSectionEvaluator::evaluateDifferentialCrossSection(
	  const double incoming_energy,
          const double outgoing_energy ) const
{
  // Make sure the energies are valid
  testPrecondition( incoming_energy > 0.0 );
  testPrecondition( outgoing_energy > 0.0 );

  // Evaluate the forward cross section at the incoming energy
  double forward_cs = d_electroionization_subshell_reaction->getCrossSection(
                                                              incoming_energy );

  // Evaluate the knock on electron pdf value at a given incoming and outgoing energy
  double knock_on_pdf = d_knock_on_distribution->evaluatePDF( incoming_energy,
                                                              outgoing_energy );

  /* Calculate the energy of a knock on electron from a primary electron with
     outgoing energy = outgoing_energy */
  double knock_on_energy = incoming_energy - outgoing_energy - d_binding_energy;

  // Evaluate the primary electron pdf value at a given incoming and outgoing energy
  double primary_pdf = d_knock_on_distribution->evaluatePDF( incoming_energy,
                                                             knock_on_energy );

/*
  // Evaluate the primary electron pdf value at a given incoming and outgoing energy
  double primary_pdf = MonteCarlo::evaluateTwoDDistributionCorrelatedPDF(
                                     incoming_energy,
                                     knock_on_energy,
                                     d_knock_on_distribution );
*/
  return forward_cs*( knock_on_pdf + primary_pdf );
}

// Return the cross section value at a given energy
double AdjointElectroionizationSubshellCrossSectionEvaluator::evaluateCrossSection(
                               const double energy,
			       const double precision ) const
{
  // Make sure the energies are valid
  testPrecondition( energy > 0.0 );

  double cross_section = 0.0;

  // Create boost rapper function for the adjoint electroionization subshell differential cross section
  boost::function<double (double x)> diff_adjoint_subshell_wrapper =
    boost::bind<double>( &AdjointElectroionizationSubshellCrossSectionEvaluator::evaluateDifferentialCrossSection,
                         boost::cref( *this ),
                         _1,
                         energy );

    double abs_error;

    Utility::GaussKronrodIntegrator integrator( precision );

    integrator.integrateAdaptively<15>(
					diff_adjoint_subshell_wrapper,
					d_knock_on_distribution->getMinEnergy(),
					d_knock_on_distribution->getMaxEnergy(),
					cross_section,
					abs_error );
/*
    integrator.integrateAdaptively<15>(
					diff_adjoint_subshell_wrapper,
					d_knock_on_distribution.front().first,
					d_knock_on_distribution.back().first,
					cross_section,
					abs_error );
*/
}

// Return the max outgoing adjoint energy for a given energy
double AdjointElectroionizationSubshellCrossSectionEvaluator::getMaxOutgoingEnergyAtEnergy(
                                const double energy )
{
  return d_knock_on_distribution->getMaxIncomingEnergyAtOutgoingEnergy( energy );
}

} // end DataGen namespace

//---------------------------------------------------------------------------//
// end DataGen_AdjointElectroionizationSubshellCrossSectionEvaluator.cpp
//---------------------------------------------------------------------------//


//
//  handlers.hpp
//  Jdlib
//
//  Created by Taha Emara on 12/18/20.
//

#ifndef handlers_hpp
#define handlers_hpp

#include <stdio.h>
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/image_processing.h>
#include <dlib/dnn.h>
using namespace dlib;

template <template <int, template<typename>class, int, typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual = add_prev1<block<N, BN, 1, tag1<SUBNET>>>;

template <template <int, template<typename>class, int, typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual_down = add_prev2<avg_pool<2, 2, 2, 2, skip1<tag2<block<N, BN, 2, tag1<SUBNET>>>>>>;

template <int N, template <typename> class BN, int stride, typename SUBNET>
using block = BN<con<N, 3, 3, 1, 1, relu<BN<con<N, 3, 3, stride, stride, SUBNET>>>>>;

template <int N, typename SUBNET> using ares = relu<residual<block, N, affine, SUBNET>>;
template <int N, typename SUBNET> using ares_down = relu<residual_down<block, N, affine, SUBNET>>;

template <typename SUBNET> using alevel0 = ares_down<256, SUBNET>;
template <typename SUBNET> using alevel1 = ares<256, ares<256, ares_down<256, SUBNET>>>;
template <typename SUBNET> using alevel2 = ares<128, ares<128, ares_down<128, SUBNET>>>;
template <typename SUBNET> using alevel3 = ares<64, ares<64, ares<64, ares_down<64, SUBNET>>>>;
template <typename SUBNET> using alevel4 = ares<32, ares<32, ares<32, SUBNET>>>;

typedef  loss_metric<fc_no_bias<128, avg_pool_everything<
  alevel0<
  alevel1<
  alevel2<
  alevel3<
  alevel4<
  max_pool<3, 3, 2, 2, relu<affine<con<32, 7, 7, 2, 2,
  input_rgb_image_sized<150>
  >>>>>>>>>>>>  anet_type;

class FaceDetectorHandler {
public:
  static FaceDetectorHandler *getFaceDetectorHandler();
  dlib::frontal_face_detector getFaceDetector();

  FaceDetectorHandler(FaceDetectorHandler const&) = delete;
  void operator=(FaceDetectorHandler const&) = delete;

private:
  FaceDetectorHandler();
  dlib::frontal_face_detector face_detector;
};

class ShapePredictorHandler {
public:
  static ShapePredictorHandler *getShapePredictorHandler(const std::string & model_path);
  dlib::shape_predictor getShapePredictorModel();

  ShapePredictorHandler(ShapePredictorHandler const&) = delete;
  void operator=(ShapePredictorHandler const&) = delete;

private:
  ShapePredictorHandler(const std::string & model_path);
  dlib::shape_predictor shape_predictor_model;
  std::string model_path;
};

class FaceEmbeddingHandler {
public:
  static FaceEmbeddingHandler *getFaceEmbeddingHandler(const std::string & model_path);

  anet_type getFaceEmbeddingModel();

  FaceEmbeddingHandler(FaceEmbeddingHandler const&) = delete;
  void operator=(FaceEmbeddingHandler const&) = delete;

private:
  FaceEmbeddingHandler(const std::string & model_path);
  anet_type face_embedding_model;
  std::string model_path;

};

#endif /* handlers_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_ARITHMETIC_FUNCTIONS_SCALAR_SUBS_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_SUBS_HPP_INCLUDED

#include <boost/simd/arithmetic/functions/subs.hpp>
#include <boost/simd/include/functions/scalar/saturate.hpp>
#include <boost/simd/include/functions/scalar/min.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/dispatch/meta/as_unsigned.hpp>
#include <boost/dispatch/meta/as_signed.hpp>
#include <boost/dispatch/meta/upgrade.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( subs_, tag::cpu_
                                    , (A0)
                                    , (scalar_< floating_<A0> >)
                                      (scalar_< floating_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return a0-a1;
    }
  };

  // for int8/int16
  BOOST_DISPATCH_IMPLEMENT          ( subs_, tag::cpu_
                                    , (A0)
                                    , (scalar_< integer_<A0> >)
                                      (scalar_< integer_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::upgrade<A0>::type utype;
      typedef typename dispatch::meta::as_signed<utype>::type stype;
      return static_cast<result_type>(boost::simd::saturate<A0>(stype(a0)-stype(a1)));
    }
  };

  BOOST_DISPATCH_IMPLEMENT_IF         ( subs_, tag::cpu_
                                      , (A0)
                                      , (mpl::bool_<sizeof(A0) == 4 || sizeof(A0) == 8>)
                                      , (scalar_< uint_<A0> >)
                                        (scalar_< uint_<A0> >)
                                      )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      A0 res = a0 - a1;
      res &= -(res <= a0);

      return res;
    }
  };

  BOOST_DISPATCH_IMPLEMENT_IF         ( subs_, tag::cpu_
                                      , (A0)
                                      , (mpl::bool_<sizeof(A0) == 4 || sizeof(A0) == 8>)
                                      , (scalar_<int_<A0> >)
                                        (scalar_<int_<A0> >)
                                      )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::as_unsigned<A0>::type utype;

      utype ux = a0;
      utype uy = a1;
      utype res = ux - uy;

      ux = (ux >> (sizeof(A0)*CHAR_BIT-1)) + Valmax<A0>();

      if(A0((ux ^ uy) & (ux ^ res)) < Zero<A0>())
        res = ux;

      return res;
    }
  };
} } }

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          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_IEEE_FUNCTIONS_SIMD_COMMON_PREDECESSOR_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SIMD_COMMON_PREDECESSOR_HPP_INCLUDED

#include <boost/simd/ieee/functions/predecessor.hpp>
#include <boost/simd/include/functions/simd/if_allbits_else.hpp>
#include <boost/simd/include/functions/simd/seladd.hpp>
#include <boost/simd/include/functions/simd/selsub.hpp>
#include <boost/simd/include/functions/simd/is_not_equal.hpp>
#include <boost/simd/include/functions/simd/is_less.hpp>
#include <boost/simd/include/functions/simd/is_nan.hpp>
#include <boost/simd/include/functions/simd/subs.hpp>
#include <boost/simd/include/functions/simd/minusone.hpp>
#include <boost/simd/include/functions/simd/bitinteger.hpp>
#include <boost/simd/include/functions/simd/bitfloating.hpp>
#include <boost/simd/include/functions/simd/is_gez.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/operator/functions/details/assert_utils.hpp>
#include <boost/assert.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( predecessor_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<arithmetic_<A0>,X>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return seladd(neq(a0, Valmin<A0>()), a0, Mone<A0>());
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( predecessor_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<floating_<A0>,X>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return if_nan_else(is_nan(a0), bitfloating(minusone(bitinteger(a0))));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( predecessor_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<integer_<A0>,X>))
                                      ((simd_<integer_<A0>,X>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      BOOST_ASSERT_MSG(assert_all(is_gez(a1)), "predecessor rank must be non negative");
      return selsub( le(Valmin<A0>()+a1, a0), a0, a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( predecessor_, tag::cpu_
                                    , (A0)(A1)(X)
                                    , ((simd_<floating_<A0>,X>))
                                      ((simd_<integer_<A1>,X>))
                                    )
  {

    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(2)
    {
      BOOST_ASSERT_MSG(assert_all(is_gez(a1)), "predecessor rank must be non negative");
      return if_nan_else(is_nan(a0), bitfloating(subs(bitinteger(a0), a1)));
    }
  };

} } }

#endif


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

#ifndef NMSSM_TWOLOOPHIGGS_H
#define NMSSM_TWOLOOPHIGGS_H

#include <Eigen/Core>

/**
 * @file nmssm_twoloophiggs.hpp
 * @brief function declarations for 2-loop NMSSM Higgs self-energies
 *        and tadpoles
 *
 * Notation:
 *
 * mt2    : squared DR-bar top mass in the NMSSM
 * mb2    : squared DR-bar bottom mass in the NMSSM
 * mtau2  : squared DR-bar tau mass in the NMSSM
 * mg     : DR-bar gluino mass in the NMSSM
 * mA2    : squared DR-bar CP-odd Higgs mass in the NMSSM
 * mst12  : squared DR-bar lightest stop mass
 * mst22  : squared DR-bar heaviest stop mass
 * msb12  : squared DR-bar lightest sbottom mass
 * msb22  : squared DR-bar heaviest sbottom mass
 * mstau12: squared DR-bar lightest stau mass
 * mstau22: squared DR-bar heaviest stau mass
 *
 * sxt    : sine of DR-bar stop mixing angle in the NMSSM
 * cxt    : cosine of DR-bar stop mixing angle in the NMSSM
 * sxb    : sine of DR-bar sbottom mixing angle in the NMSSM
 * cxb    : cosine of DR-bar sbottom mixing angle in the NMSSM
 * sintau : sine of DR-bar stau mixing angle in the NMSSM
 * costau : cosine of DR-bar stau mixing angle in the NMSSM
 *
 * gs     : DR-bar strong gauge coupling g3 in the NMSSM
 * mu     : DR-bar mu-parameter in the NMSSM (arXiv:0907.4682)
 * tanb   : DR-bar tan(beta) = vu/vd in the NMSSM
 * cotb   : DR-bar 1/tan(beta) in the NMSSM
 * vev2   : squared DR-bar vev^2 = (vu^2 + vd^2) in the NMSSM
 * lam    : DR-bar lambda in the NNMSSM
 * svev   : DR-bar singlet VEV = mu_eff / lam
 *
 * scheme : DR-bar scheme (0) or on-shell scheme (1)
 */

namespace flexiblesusy {
namespace nmssm_twoloophiggs {

Eigen::Matrix<double, 3, 1> tadpole_higgs_2loop_at_as_nmssm(
   double mt2, double mg, double mst12, double mst22,
   double sxt, double cxt, double scale2,
   double mu, double tanb, double vev2, double gs, double svev);

Eigen::Matrix<double, 3, 1> tadpole_higgs_2loop_ab_as_nmssm(
   double mb2, double mg, double msb12, double msb22,
   double sxb, double cxb, double scale2,
   double mu, double cotb, double vev2, double gs, double svev);

Eigen::Matrix<double, 3, 3> self_energy_higgs_2loop_at_as_nmssm(
   double rmt, double mg, double mst12, double mst22,
   double sxt, double cxt, double scale2, double tanb, double vev2,
   double lam, double svev, double as, double mu);

Eigen::Matrix<double, 3, 3> self_energy_higgs_2loop_ab_as_nmssm(
   double rmb, double mg, double msb12, double msb22,
   double sxb, double cxb, double scale2, double cotb, double vev2,
   double lam, double svev, double as, double mu);

Eigen::Matrix<double, 3, 3> self_energy_pseudoscalar_2loop_at_as_nmssm(
   double rmt, double mg, double mst12, double mst22,
   double sxt, double cxt, double scale2, double tanb, double vev2,
   double lam, double svev, double as, double mu);

Eigen::Matrix<double, 3, 3> self_energy_pseudoscalar_2loop_ab_as_nmssm(
   double rmb, double mg, double msb12, double msb22,
   double sxb, double cxb, double scale2, double cotb, double vev2,
   double lam, double svev, double as, double mu);

Eigen::Matrix<double, 3, 3> self_energy_higgs_2loop_at_as_nmssm_with_tadpoles(
   double rmt, double mg, double mst12, double mst22,
   double sxt, double cxt, double scale2, double tanb, double vev2,
   double lam, double svev, double as);

Eigen::Matrix<double, 3, 3> self_energy_higgs_2loop_ab_as_nmssm_with_tadpoles(
   double rmb, double mg, double msb12, double msb22,
   double sxb, double cxb, double scale2, double cotb, double vev2,
   double lam, double svev, double as);

Eigen::Matrix<double, 3, 3> self_energy_pseudoscalar_2loop_at_as_nmssm_with_tadpoles(
   double rmt, double mg, double mst12, double mst22,
   double sxt, double cxt, double scale2, double tanb, double vev2,
   double lam, double svev, double as);

Eigen::Matrix<double, 3, 3> self_energy_pseudoscalar_2loop_ab_as_nmssm_with_tadpoles(
   double rmb, double mg, double msb12, double msb22,
   double sxb, double cxb, double scale2, double cotb, double vev2,
   double lam, double svev, double as);

} // namespace nmssm_twoloophiggs
} // namespace flexiblesusy

#endif


#ifndef __IS_DRIVER_ATSP_HPP__
#define __IS_DRIVER_ATSP_HPP__

#include <is/is.hpp>
#include <is/logger.hpp>
#include <is/msgs/common.hpp>
#include <is/msgs/geometry.hpp>
#include <is/msgs/robot.hpp>

#include <chrono>
#include <cmath>
#include <memory>
#include <string>

#include <atomic>
#include <condition_variable>
#include <mutex>
#include <thread>

#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>

namespace is {
namespace driver {

using namespace std::chrono;
using namespace is::msg::robot;
using namespace is::msg::common;
using namespace is::msg::geometry;

constexpr double pi() {
  return std::atan(1) * 4;
}
auto deg2rad = [](double deg) { return deg * (pi() / 180.0); };
auto rad2deg = [](double rad) { return rad * (180.0 / pi()); };

struct ATSP {
  is::Connection is;
  std::string broker;
  std::string mqtt_entity;

  Pose pose;
  Speed speed;
  Timestamp last_timestamp;
  std::atomic<int64_t> period_ms;
  std::atomic<int64_t> delay_ms;

  bool ready;
  std::mutex mutex;
  std::condition_variable condition;
  std::thread thread;
  std::atomic<bool> running;

  ATSP(std::string const& broker, std::string const& mqtt_entity) : is(broker) {
    this->broker = broker;
    this->mqtt_entity = mqtt_entity;
    this->period_ms = 500;
    initialize();
  }

  virtual ~ATSP() {
    is::log::info("Stopping publish");
    is.publish(mqtt_entity + ".cmd", BasicMessage::Create("#stopData;"));
    running = false;
    thread.join();
  }

  void set_speed(Speed const& speed) {
    std::unique_lock<std::mutex> lock(mutex);
    is.publish(mqtt_entity + ".cmd", BasicMessage::Create("#driveRobot(" + std::to_string(speed.linear) + "," +
                                                          std::to_string(rad2deg(speed.angular)) + "/0)" + ";"));
  }

  Speed get_speed() {
    std::unique_lock<std::mutex> lock(mutex);
    return this->speed;
  }

  Pose get_pose() {
    std::unique_lock<std::mutex> lock(mutex);
    return this->pose;
  }

  void set_pose(Pose const& pose) {
    std::unique_lock<std::mutex> lock(mutex);
    is.publish(mqtt_entity + ".cmd", BasicMessage::Create("#setPose(" + std::to_string(pose.position.x) + "," +
                                                          std::to_string(pose.position.y) + "/" +
                                                          std::to_string(rad2deg(pose.heading)) + ")" + ";"));
  }

  void set_sample_rate(SamplingRate const& rate) {
    std::unique_lock<std::mutex> lock(mutex);
    if (rate.period) {
      period_ms = std::max<int64_t>(500, *rate.period);
    } else if (rate.rate) {
      period_ms = std::max<int64_t>(500, 1000.0 / *rate.rate);
    }
  }

  SamplingRate get_sample_rate() {
    std::unique_lock<std::mutex> lock(mutex);
    SamplingRate rate;
    rate.period = period_ms;
    rate.rate = static_cast<double>(1000.0 / period_ms);
    return rate;
  }

  void set_delay(Delay const& delay) {
    std::unique_lock<std::mutex> lock(mutex);
    if (delay.milliseconds <= period_ms) {
      delay_ms = delay.milliseconds;
    }
  }

  Timestamp get_last_timestamp() {
    std::unique_lock<std::mutex> lock(mutex);
    return last_timestamp;
  }

  void wait() {
    std::unique_lock<std::mutex> lock(mutex);
    condition.wait(lock, [this] { return ready; });
    ready = false;
  }

 private:
  void initialize() {
    is.publish(mqtt_entity + ".cmd", BasicMessage::Create("#stopAll;"));
    is.publish(mqtt_entity + ".cmd", BasicMessage::Create("#sendData;"));

    auto loop = [this]() {
      is::Connection is_loop(broker);
      auto mqtt_topics = is_loop.subscribe(mqtt_entity + ".Pose");
      running = true;
      while (running) {
        auto mqtt_messages = is_loop.consume_for(mqtt_topics, std::chrono::milliseconds(5 * period_ms));

        if (mqtt_messages == nullptr) {
          is::log::info("Waiting subscription...");
          auto talk_topic = is_loop.subscribe(mqtt_entity + ".talk");
          do {
            is_loop.publish(mqtt_entity + ".cmd.Lua",
                            BasicMessage::Create("mqttClient:publish('" + mqtt_entity + "/talk','Connected!',0,0)"));
          } while (is_loop.consume_for(talk_topic, 1s) == nullptr);

          is::log::info("Connected!");
          is_loop.publish(mqtt_entity + ".cmd", BasicMessage::Create("#stopAll;"));
          is_loop.publish(mqtt_entity + ".cmd", BasicMessage::Create("#sendData;"));
        } else {
          auto timestamp = Timestamp();
          Pose pose;
          Speed speed;
          std::vector<std::string> fields;
          std::string message = mqtt_messages->Message()->Body();
          boost::split(fields, message, boost::is_any_of("/"), boost::token_compress_on);
          if (fields.size() == 5) {
            pose = {.position = {stod(fields[0]), stod(fields[1])}, .heading = deg2rad(stod(fields[2]))};
            speed = {.linear = stod(fields[3]), .angular = stod(fields[4])};
          }
          {
            std::unique_lock<std::mutex> lock(mutex);
            this->pose = pose;
            this->speed = speed;
            last_timestamp = timestamp;
            ready = true;
          }
          condition.notify_one();
        }
      }
    };

    thread = std::thread(loop);
  }
};

}  // ::driver
}  // ::is

#endif  // __IS_DRIVER_PIONEER_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_BITWISE_FUNCTIONS_SIMD_COMMON_CLZ_HPP_INCLUDED
#define BOOST_SIMD_BITWISE_FUNCTIONS_SIMD_COMMON_CLZ_HPP_INCLUDED

#include <boost/simd/bitwise/functions/clz.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/minusone.hpp>
#include <boost/simd/include/functions/simd/reversebits.hpp>
#include <boost/simd/include/functions/simd/ffs.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/operator/functions/details/assert_utils.hpp>
#include <boost/assert.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::clz_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))
                            )
  {

    typedef typename dispatch::meta::as_integer<A0, unsigned>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      result_type t =  bitwise_cast<result_type>(a0);
      typedef typename meta::scalar_of<A0>::type s_type;

      BOOST_ASSERT_MSG( assert_all(t), "clz not defined for 0" );
      return minusone(boost::simd::ffs(boost::simd::reversebits(t)));
    }
  };
} } }

#endif


#pragma once

#include <rj_geometry/point.hpp>
#include <boost/circular_buffer.hpp>
#include <rj_vision_filter/ball/camera_ball.hpp>
#include <rj_vision_filter/filter/kalman_filter_2d.hpp>

namespace vision_filter {
class WorldBall;

/**
 * Filtered ball estimation for a single camera
 */
class KalmanBall {
public:
    /**
     * Checks the previous_world_ball to see if it's valid
     *
     * @param camera_id ID of the camera this filter belongs to
     * @param creation_time Time this filter is created
     * @param init_measurement Initial ball measurement we are creating the
     * filter at
     * @param previous_world_ball Previous prediction of ball location to
     * initialize the velocity smartly
     */
    KalmanBall(unsigned int camera_id, RJ::Time creation_time,
               CameraBall init_measurement, const WorldBall& previous_world_ball);

    /**
     * Predicts one time step forward
     *
     * @param current_time Time at the current frame
     *
     * @note Call either this OR predict_and_update once a frame
     */
    void predict(RJ::Time current_time);

    /**
     * Predicts one time step forward then triangulates towards the measurement
     *
     * @param current_time Current time of the prediction/update step
     * @param update_ball Ball measurement that we are using as feedback to the
     * filters
     *
     * @note Call either this OR predict once a frame
     */
    void predict_and_update(RJ::Time current_time, CameraBall update_ball);

    /**
     * @return Returns true when the filter hasn't been updated in a while etc
     * and should be deleted
     */
    bool is_unhealthy() const;

    /**
     * @return The camera id this belongs to
     */
    unsigned int get_camera_id() const;

    /**
     * @return How healthy this filter is. AKA How often it's been updated
     */
    int get_health() const;

    /**
     * @return Best estimate of the position of the ball
     */
    rj_geometry::Point get_pos() const;

    /**
     * @return Best estimate of the velocity of the ball
     */
    rj_geometry::Point get_vel() const;

    /**
     * @return Covariance in X and Y direction of the position of the ball
     */
    rj_geometry::Point get_pos_cov() const;

    /**
     * @return Covariance in X and Y direction of the velocity of the ball
     */
    rj_geometry::Point get_vel_cov() const;

    /**
     * @return List of previous camera ball measurements for kick
     * detection/estimation
     */
    const boost::circular_buffer<CameraBall>& get_prev_measurements() const;

    /**
     * @param new_vel new velocity to insert into the kalman filter
     *
     * Note: Only used to set the velocity when we think the ball will bounce
     * off another robot
     */
    void set_vel(rj_geometry::Point new_vel);

private:
    RJ::Time last_update_time_;
    RJ::Time last_predict_time_;

    // Keeps track of this for kick detection stuff
    boost::circular_buffer<CameraBall> previous_measurements_;

    KalmanFilter2D filter_;

    int health_;

    unsigned int camera_id_;
};
}  // namespace vision_filter


/*=============================================================================

  NifTK: A software platform for medical image computing.

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

=============================================================================*/
#include "mitkTrackerAnalysis.h"
#include "mitkProjectPointsOnStereoVideo.h"
#include <mitkCameraCalibrationFacade.h>
#include <mitkUltrasoundPinCalibration.h>
#include <mitkOpenCVMaths.h>
#include <niftkMathsUtils.h>

#include <boost/lexical_cast.hpp>

#include <sstream>
#include <fstream>
#include <cstdlib>

namespace mitk
{
//---------------------------------------------------------------------------
TrackerAnalysis::TrackerAnalysis ()
{}

//---------------------------------------------------------------------------
TrackerAnalysis::~TrackerAnalysis ()
{}

//---------------------------------------------------------------------------
void TrackerAnalysis::TemporalCalibration(std::string calibrationfilename ,
    int windowLow, int windowHigh, bool visualise, std::string fileout)
{
  if ( !m_Ready )
  {
    MITK_ERROR << "Initialise video tracker matcher before attempting temporal calibration";
    return;
  }

  std::vector < std::ofstream* > fout;
  if ( fileout.length() != 0 )
  {
    for ( unsigned int i = 0 ; i <  GetTrackingMatricesSize() ; i ++ )
    {
      std::string thisfileout = fileout + boost::lexical_cast<std::string>(i) + ".txt";
      std::ofstream* thisfout = new std::ofstream();
      thisfout->open ( thisfileout.c_str() );
      if ( !thisfout )
      {
        MITK_WARN << "Failed to open output file for temporal calibration " << thisfileout;
      }
      fout.push_back(thisfout);
    }
  }

  std::vector < mitk::WorldPointsWithTimingError >  pointsInLensCS;
  pointsInLensCS.clear();
  std::vector < mitk::ProjectedPointPairsWithTimingError> onScreenPoints;
  onScreenPoints.clear();
  pointsInLensCS = ReadPointsInLensCSFile(calibrationfilename, 1 , &onScreenPoints);

  if ( pointsInLensCS.size() != m_FrameNumbers.size() )
  {
    MITK_ERROR << "Temporal calibration file has wrong number of frames, " << pointsInLensCS.size()  << " != " << m_FrameNumbers.size() ;
    return;
  }

  mitk::ProjectPointsOnStereoVideo::Pointer projector = mitk::ProjectPointsOnStereoVideo::New();
  projector->Initialise(m_Directory,m_CalibrationDirectory);

  std::vector < std::vector < cv::Point3d > > reconstructedPointSD (GetTrackingMatricesSize());
  std::vector < std::vector < std::pair <double, double > > > projectedErrorRMS (GetTrackingMatricesSize());
  for ( int videoLag = windowLow; videoLag <= windowHigh ; videoLag ++ )
  {
    if ( videoLag < 0 )
    {
      SetVideoLagMilliseconds ( (unsigned long long) (videoLag * -1) , true, -1 );
    }
    else
    {
      SetVideoLagMilliseconds ( (unsigned long long) (videoLag ) , false, -1  );
    }

    for ( unsigned int trackerIndex = 0 ; trackerIndex < GetTrackingMatricesSize() ; trackerIndex++ )
    {
      std::vector <cv::Point3d> worldPoints;
      worldPoints.clear();
      for ( unsigned int frame = 0 ; frame < pointsInLensCS.size() ; frame++ )
      {
        int framenumber = frame;
        worldPoints.push_back (GetCameraTrackingMatrix(framenumber, NULL , trackerIndex ) *
            pointsInLensCS[frame].m_Points[0].m_Point);
      }

      cv::Point3d pointSpread;
      cv::Point3d worldCentre = mitk::GetCentroid (worldPoints, true,  &pointSpread);

      reconstructedPointSD[trackerIndex].push_back(pointSpread);
      std::vector <mitk::WorldPoint > worldPoint(1);
      worldPoint[0] = mitk::WorldPoint (worldCentre);
      projector->ClearWorldPoints();
      projector->AppendWorldPoints(worldPoint);
      projector->SetTrackerIndex(trackerIndex);
      projector->Project(this);
      std::vector < mitk::ProjectedPointPairsWithTimingError > projectedPoints =
        projector->GetProjectedPoints();

      std::pair <double, double> projectedRMS = mitk::RMSError ( projectedPoints,  onScreenPoints );
      projectedErrorRMS[trackerIndex].push_back(projectedRMS);

    }
  }

  if ( fileout.length() != 0 )
  {
    for ( unsigned int trackerIndex = 0 ; trackerIndex < GetTrackingMatricesSize() ; trackerIndex++ )
    {
      *fout[trackerIndex] << "#lag SDx SDy SDz RMSLeft RMSRight" << std::endl;
      for ( int videoLag = windowLow; videoLag <= windowHigh ; videoLag ++ )
      {
        *fout[trackerIndex] << videoLag << " " <<
        reconstructedPointSD[trackerIndex][videoLag - windowLow].x << " " <<
        reconstructedPointSD[trackerIndex][videoLag - windowLow].y << " " <<
        reconstructedPointSD[trackerIndex][videoLag - windowLow].z << " " <<
        projectedErrorRMS[trackerIndex][videoLag - windowLow].first << " " <<
        projectedErrorRMS[trackerIndex][videoLag - windowLow].second << std::endl;
      }
      fout[trackerIndex]->close();
    }

  }

  for ( unsigned int trackerIndex = 0 ; trackerIndex < GetTrackingMatricesSize() ; trackerIndex++ )
  {
    std::pair < unsigned int , unsigned int > minIndexes;
    std::pair < double , double > minValues = niftk::FindMinimumValues ( projectedErrorRMS[trackerIndex], &minIndexes );
    MITK_INFO << "Tracker Index " << trackerIndex << " min left RMS " << minValues.first << " at " << (int)minIndexes.first + windowLow << " ms ";  ;
    MITK_INFO << "Tracker Index " << trackerIndex << " min right RMS " << minValues.second << " at " << (int)minIndexes.second + windowLow << " ms ";  ;
  }

}
//---------------------------------------------------------------------------
void TrackerAnalysis::OptimiseHandeyeCalibration(std::string calibrationfilename ,
    bool visualise, std::string fileout)
{
  MITK_ERROR << "TrackerAnalysis::OptimiseHandeyeCalibration is currently broken, do not use";
  return;
  if ( !m_Ready )
  {
    MITK_ERROR << "Initialise video tracker matcher before attempting temporal calibration";
    return;
  }

  std::ofstream fout;
  if ( fileout.length() != 0 )
  {
    fout.open(fileout.c_str());
    if ( !fout )
    {
      MITK_WARN << "Failed to open output file for temporal calibration " << fileout;
    }
  }

  std::vector < mitk::WorldPointsWithTimingError > pointsInLensCS;
  pointsInLensCS.clear();
  std::vector < mitk::ProjectedPointPairsWithTimingError >* onScreenPoints;
  onScreenPoints->clear();
  pointsInLensCS = ReadPointsInLensCSFile(calibrationfilename, 1, onScreenPoints);

  if ( pointsInLensCS.size() != m_FrameNumbers.size() )
  {
    MITK_ERROR << "Temporal calibration file has wrong number of frames, " << pointsInLensCS.size()  << " != " << m_FrameNumbers.size() ;
    return;
  }

  for ( unsigned int trackerIndex = 0 ; trackerIndex < GetTrackingMatricesSize() ; trackerIndex++ )
  {
    std::vector<cv::Mat> cameraMatrices;
    cameraMatrices.clear();
    for ( unsigned int frame = 0 ; frame < pointsInLensCS.size() ; frame++ )
    {
      int framenumber = frame ;
      if ( ! ( pointsInLensCS[frame].m_Points[0].IsNaN() ))
      {
        cameraMatrices.push_back (GetCameraTrackingMatrix(framenumber, NULL , trackerIndex ));
      }
      else
      {
       pointsInLensCS.erase(pointsInLensCS.begin() + frame);
       frame -- ;
      }
    }
    //bool optimiseScaling = false;
    //bool optimiseInvariantPoint = true;
    std::vector<double> rigidBodyTransformation;
    cv::Point3d invariantPoint;
    //initial values for point and transform. Could use known handeye and reconstructed point,
    //but for the minute let's use ID and 0 0 0
    rigidBodyTransformation.clear();
    for ( int i = 0 ; i < 6 ; i ++ )
    {
      rigidBodyTransformation.push_back(0.0);
    }
    invariantPoint.x=0.0;
    invariantPoint.y=0.0;
    invariantPoint.z=0.0;
    cv::Point2d millimetresPerPixel;
    //mm per pixel has no meaning in this application as the point is already defined in mm
    millimetresPerPixel.x = 1.0;
    millimetresPerPixel.y = 1.0;

    cv::Matx44d outputMatrix;
    double residualError;

    mitk::UltrasoundPinCalibration::Pointer invPointCal = mitk::UltrasoundPinCalibration::New();
    //invPointCal->Calibrate(cameraMatrices, pointsInLensCS,
    //    optimiseScaling, optimiseInvariantPoint, rigidBodyTransformation,
    //    invariantPoint, millimetresPerPixel,
    //    outputMatrix, residualError);
    MITK_INFO << "Tracker Index " << trackerIndex << ": After optimisation, handeye = " ;
    for ( int i = 0 ; i < 4 ; i ++ )
    {
      MITK_INFO << outputMatrix(i,0) << "," << outputMatrix(i,1) <<  " , " << outputMatrix(i,2) << " , " <<  outputMatrix (i,3);
    }
    MITK_INFO << "Invariant point = " << invariantPoint << " [ " << residualError << " SD ].";
    MITK_INFO << "Millimetres per pixel = " << millimetresPerPixel;
  }

}
//---------------------------------------------------------------------------
void TrackerAnalysis::HandeyeSensitivityTest(std::string calibrationfilename ,
    double windowLow, double windowHigh , double stepSize, std::string fileout)
{
  if ( !m_Ready )
  {
    MITK_ERROR << "Initialise video tracker matcher before attempting handeye sensitivity";
    return;
  }

  std::vector < std::ofstream* > fout;
  if ( fileout.length() != 0 )
  {
    for ( unsigned int i = 0 ; i <  GetTrackingMatricesSize() ; i ++ )
    {
      std::string thisfileout = fileout + boost::lexical_cast<std::string>(i) + ".txt";
      std::ofstream* thisfout = new std::ofstream();
      thisfout->open ( thisfileout.c_str() );
      if ( !thisfout )
      {
        MITK_WARN << "Failed to open output file for handeye sensitivity " << thisfileout;
      }
      fout.push_back(thisfout);
    }
  }

  std::vector < mitk::WorldPointsWithTimingError >  pointsInLensCS;
  pointsInLensCS.clear();
  std::vector < mitk::ProjectedPointPairsWithTimingError > onScreenPoints;
  onScreenPoints.clear();
  pointsInLensCS = ReadPointsInLensCSFile(calibrationfilename, 1 , &onScreenPoints);

  if ( pointsInLensCS.size()  != m_FrameNumbers.size() )
  {
    MITK_ERROR << "Handeye sensitivity file has wrong number of frames, " << pointsInLensCS.size() << " != " << m_FrameNumbers.size() ;
    return;
  }

  mitk::ProjectPointsOnStereoVideo::Pointer projector = mitk::ProjectPointsOnStereoVideo::New();
  projector->Initialise(m_Directory,m_CalibrationDirectory);

  std::vector < std::vector < cv::Point3d > > reconstructedPointSD (GetTrackingMatricesSize());
  std::vector < std::vector < std::pair <double, double > > > projectedErrorRMS (GetTrackingMatricesSize());
  std::vector < std::vector <double> > stateVector;
  for ( double tx = windowLow; tx <= windowHigh ; tx += stepSize )
  {
    MITK_INFO << "tx=" << tx;
    for ( double ty = windowLow; ty <= windowHigh ; ty += stepSize )
    {
      for ( double tz = windowLow; tz <= windowHigh ; tz += stepSize )
      {
        for ( double rx = windowLow; rx <= windowHigh ; rx += stepSize )
        {
          MITK_INFO << "rx="<< rx;
          for ( double ry = windowLow; ry <= windowHigh ; ry += stepSize )
          {
            for ( double rz = windowLow; rz <= windowHigh ; rz += stepSize )
            {
              std::vector<double> state;
              state.push_back(tx);
              state.push_back(ty);
              state.push_back(tz);
              state.push_back(rx);
              state.push_back(ry);
              state.push_back(rz);

              stateVector.push_back( state );
              for ( unsigned int trackerIndex = 0 ; trackerIndex < GetTrackingMatricesSize() ; trackerIndex++ )
              {
                std::vector <cv::Point3d> worldPoints;
                worldPoints.clear();
                for ( unsigned int frame = 0 ; frame < pointsInLensCS.size() ; frame++ )
                {
                  int framenumber = frame ;
                  worldPoints.push_back (GetCameraTrackingMatrix(framenumber, NULL , trackerIndex, &state ) *
                      pointsInLensCS[frame].m_Points[0].m_Point);
                }

                cv::Point3d pointSpread;
                cv::Point3d worldCentre = mitk::GetCentroid (worldPoints, true,  &pointSpread);
                reconstructedPointSD[trackerIndex].push_back(pointSpread);
                std::vector < mitk::WorldPoint > worldPoint(1);
                worldPoint[0] = mitk::WorldPoint(worldCentre);
                projector->ClearWorldPoints();
                projector->AppendWorldPoints(worldPoint);
                projector->SetTrackerIndex(trackerIndex);
                projector->Project(this, &state);
                std::vector < mitk::ProjectedPointPairsWithTimingError > projectedPoints =
                  projector->GetProjectedPoints();

                std::pair <double, double> projectedRMS = mitk::RMSError ( projectedPoints, onScreenPoints );
                projectedErrorRMS[trackerIndex].push_back(projectedRMS);

              }
            }
          }
        }
      }
    }
  }

  if ( fileout.length() != 0 )
  {
    for ( unsigned int trackerIndex = 0 ; trackerIndex < GetTrackingMatricesSize() ; trackerIndex++ )
    {
      *fout[trackerIndex] << "#lag SDx SDy SDz RMSLeft RMSRight" << std::endl;
      for ( unsigned int i = 0 ; i  < stateVector.size() ; i ++ )
      {
        *fout[trackerIndex] << stateVector[i][0] << " " <<
        stateVector[i][1] << " " <<
        stateVector[i][2] << " " <<
        stateVector[i][3] << " " <<
        stateVector[i][4] << " " <<
        stateVector[i][5] << " : " <<
        reconstructedPointSD[trackerIndex][i].x << " " <<
        reconstructedPointSD[trackerIndex][i].y << " " <<
        reconstructedPointSD[trackerIndex][i].z << " " <<
        projectedErrorRMS[trackerIndex][i].first << " " <<
        projectedErrorRMS[trackerIndex][i].second << std::endl;
      }
      fout[trackerIndex]->close();
    }
  }

  for ( unsigned int trackerIndex = 0 ; trackerIndex < GetTrackingMatricesSize() ; trackerIndex++ )
  {
    std::pair < unsigned int , unsigned int > minIndexes;
    std::pair < double , double > minValues = niftk::FindMinimumValues ( projectedErrorRMS[trackerIndex], &minIndexes );
    MITK_INFO << "Tracker Index " << trackerIndex << " min left RMS " << minValues.first << " at " << (int)minIndexes.first + windowLow << " ms ";  ;
    MITK_INFO << "Tracker Index " << trackerIndex << " min right RMS " << minValues.second << " at " << (int)minIndexes.second + windowLow << " ms ";  ;
  }

}
} // namespace