// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2011 The University of Sydney
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. 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.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  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.

#include <cmath>
#include <Eigen/Geometry>
#include <boost/format.hpp>
#include <comma/base/exception.h>
#include <comma/math/compare.h>
#include "../angle.h"
#include "coordinates.h"
#include <iomanip>

namespace {

    static const double eighty_degrees = 80 * M_PI / 180.0;

    static const double scaled_cos_eighty_degrees = 2.0 / M_PI * std::cos( eighty_degrees );

    bool precise_is_far( const snark::spherical::coordinates & l, const snark::spherical::coordinates & r, double _epsilon )
    {
        return Eigen::AngleAxis< double >( Eigen::Quaternion< double >::FromTwoVectors( l.to_cartesian(), r.to_cartesian() ) ).angle() > _epsilon;
    }

    bool is_far( const snark::spherical::coordinates & l, const snark::spherical::coordinates & r, double _epsilon )
    {
        // for very short arcs skip shortcuts and enforce thorough comparison because of numerical stability issues in Eigen
        // the actual value is about 6 m / earth radius; enough to make AngleAxis::angle() non-zero
        double threshold = std::max( snark::spherical::coordinates::epsilon, _epsilon );
        // make quick decisions first; for latitude shortcut no explanations needed
        if ( std::abs( l.latitude - r.latitude ) > threshold ) { return true; }
        double delta_longitude = std::abs( l.longitude - r.longitude );
        // nearly 360 delta means very close; take the shorter of the two possible arcs
        delta_longitude = delta_longitude > M_PI ? 2 * M_PI - delta_longitude : delta_longitude;
        // longitude shortcut:
        //     ignore near-Pole regions
        //     take a lower limit on great circle arc length:
        //         the arc at the higher (by modulo) of the two latitudes is shorter than the real arc (lower estimate)
        //             proof not given here; differentiate the exact expression by one of the latitudes; the derivative sign is
        //             defined by the other latitude: negative in the Northern hemisphere, positive in the Southern; therefore,
        //             if the fixed latitude (not the one being differentiated upon) is chosen as the max by modulo, the function
        //             may only increase as the other latitude moves either South or North, respectively
        //         for the same latitudes, use the lower estimate of the great circle arc length:
        //             exact length = 2 * asin( cos(latitude) * sin( delta_longitude / 2 ) )
        //             estimates: asin(x) >= x for any x in [0, 1]
        //                        sin(y) >= 2/pi * y for any y in [0, pi/2]
        //             thus exact length >= 2 cos(latitude) * sin( delta_longitude / 2 ) >= cos(latitude) * 2/pi * delta_longitude >= cos(80 degrees) * 2/pi * delta_longitude
        //     if the lower limit is above the threshold, the real arc length is above the threshold as well
        double max_latitude = std::max( std::abs( l.latitude ), std::abs( r.latitude ) );
        if ( max_latitude < eighty_degrees && delta_longitude * scaled_cos_eighty_degrees > threshold ) { return true; }
        // shortcuts failed, do lengthy computations
        return precise_is_far( l, r, threshold );
    }
}

namespace snark { namespace spherical {

const double coordinates::epsilon = 1e-6;

bool coordinates::operator==( const coordinates& rhs ) const
{
    return !is_far( *this, rhs, epsilon );
}

bool coordinates::is_near( const coordinates& c, double epsilon ) const
{
    return !is_far( *this, c, epsilon );
}

bool coordinates::is_near( const coordinates & l, const coordinates & r, double _epsilon )
{
    return !is_far( l, r, _epsilon );
}

/// limits longitude to [-PI, PI)
static double limit( const double longitude )
{
    // todo: this is just a quick patch, not a real fix; what if longitude equals e.g. -3*M_PI?
    // todo: seriously quick and dirty, to fix anti merridian crossing
    return longitude < -M_PI ? longitude + 2 * M_PI : longitude >= M_PI ? longitude - 2 * M_PI : longitude;
}

coordinates::coordinates( const double latitude, const double longitude ) : latitude( latitude ), longitude( limit( longitude ) )
{
}

coordinates::coordinates( const snark::bearing_elevation& be ) : latitude( be.elevation() ), longitude( limit( be.bearing() ) )
{
}

coordinates::coordinates( const Eigen::Vector3d& xyz )
{
    snark::range_bearing_elevation rbe( xyz );
    latitude = rbe.elevation();
    longitude = limit( rbe.bearing() );
}

Eigen::Vector3d to_navigation_frame( const coordinates& c, const Eigen::Vector3d& v )
{
    const Eigen::Matrix3d& r1 = Eigen::AngleAxis< double >( -c.longitude, Eigen::Vector3d( 0, 0, 1 ) ).toRotationMatrix();
    const Eigen::Matrix3d& r2 = Eigen::AngleAxis< double >( c.latitude + M_PI / 2, Eigen::Vector3d( 0, 1, 0 ) ).toRotationMatrix();
    return r2 * r1 * v;
}

coordinates& coordinates::operator+=( const coordinates& rhs )
{
    double d = latitude + rhs.latitude;
    static const double epsilon = 0.00005;
    if( comma::math::equal( d, M_PI / 2, epsilon ) ) { d = M_PI / 2; }
    else if( comma::math::equal( d, -M_PI / 2, epsilon ) ) { d = -M_PI / 2; }
    else if( d > M_PI / 2 || d < -M_PI / 2 ) { COMMA_THROW( comma::exception, "adding " << ( latitude * 180 / M_PI ) << " and " << ( rhs.latitude * 180 / M_PI ) << " gives invalid latitude of " << ( d * 180 / M_PI ) << " degress" ); }
    latitude = d;
    longitude += rhs.longitude;
    while( longitude < -M_PI ) { longitude += 2 * M_PI; } // brutal, but mod() is slower in most cases, i think
    while( this->longitude >= M_PI ) { longitude -= 2 * M_PI; } // brutal, but mod() is slower in most cases, i think
    return *this;
}

coordinates::operator std::string() const
{
    const std::pair< double, double >& c = to_degrees();
    return (boost::format("%.2f") % c.first ).str() + "," + (boost::format("%.2f") % c.second).str();
}

bool coordinates::is_near( const Eigen::Vector3d& c, double _epsilon ) const
{
    return ( to_cartesian() - c ).lpNorm<Eigen::Infinity>() < _epsilon;
}

coordinates coordinates::from_degrees(double latitude, double longitude)
{
    return coordinates( snark::math::radians( snark::math::degrees( latitude ) ).value,
                        snark::math::radians( snark::math::degrees( longitude ) ).value );
}

std::pair< double, double > coordinates::to_degrees() const
{
    return std::make_pair( snark::math::degrees( snark::math::radians( latitude ) ).value
                         , snark::math::degrees( snark::math::radians( longitude ) ).value );
}

} } // namespace snark { namespace spherical {


#ifndef BIGGLES_SERVER_STATE_HPP__
#define BIGGLES_SERVER_STATE_HPP__

#include "../mh_moves/mh_moves.hpp"
#include "../model.hpp"
#include "../partition.hpp"
#include "../tracker.hpp"

#include <boost/shared_ptr.hpp>
#include <boost/thread/thread.hpp>

namespace biggles
{

/// @brief Support for writing a Bigglers tracking server.
///
/// This namespace contains a tracking engine suitable for embedding into a server. The engine itself runs the tracking
/// in a separate thread and allows querying the current state of the tracker.
namespace server
{

/// @brief A snapshot of the state of a Biggles tracking process.
///
/// @sa server::engine
struct tracking_state
{
    /// @brief The number of samples which have been drawn.
    size_t sample_count;

    /// @brief The model parameters corresponding to the last drawn sample.
    biggles::model::parameters current_model_parameters;

    /// @brief The partition corresponding to the last drawn sample.
    biggles::partition_ptr_t current_partition;
    /// \brief This is for the python interface
    biggles::partition current_partition_fun() const {return *current_partition;}

    /// @brief The proposal move type by which the current partition was proposed.
    biggles::mh_moves::move_type current_move_type;

    /// @brief The computed log PDF of the last drawn sample.
    /// is either last_proposal_density (if proposal was accepted)
    /// or last_sample_density (if proposal was rejected)
    float current_log_pdf;

    /// @brief the target density of the last proposal
    float last_proposal_density;

    /// @brief the target density of the last sample in comparison
    float last_sample_density;

    /// @brief The model parameters associated with the sample with lowest PDF value.
    biggles::model::parameters best_model_parameters;

    /// @brief The partition associated with the sample with lowest PDF value.
    biggles::partition_ptr_t best_partition;
    /// \brief This is for the python interface
    biggles::partition best_partition_fun() const {return *best_partition;}

    /// @brief The lowest PDF value of any sample drawn so far.
    float best_log_pdf;

    /// @brief The acceptance rate for all samples of the Metropolis Hastings part of the tracker.
    float acceptance_rate;

    /// @brief The acceptance rate for the last samples of the Metropolis Hastings part of the tracker.
    float recent_acceptance_rate;

    bool accepted; // was the last move accepted

    /// \brief the last proposal partition; 1st component of last_proposal
    biggles::partition_ptr_t last_proposed_partition;
    /// \brief This is for the python interface
    biggles::partition last_proposed_partition_fun() const {return *last_proposed_partition;}

    /// \brief last PDR
    float last_pdr;

    /// \brief the last proposal move; 2st component of last_proposal
    biggles::mh_moves::move_type last_proposed_move;

    /// @brief A histogram of accepted move types.
    std::deque<uint64_t> move_histogram;

    /// @brief A record for the move statisicis (a/r/i).
    std::deque<uint64_t> moves_accepted;
    std::deque<uint64_t> moves_rejected;
    std::deque<uint64_t> moves_identity;

    std::deque<uint64_t> internals;

    //tracking_state();
};

/// @brief A threaded engine for a Biggles server.
///
/// This class encapsulates the logic required to implement Biggles as a server keeping the tracking process within a
/// separate thread. It can be used to expose the Biggles tracker via almost any RPC mechanism with an appropriate
/// adaptor.
class engine
{
public:
    engine();

    ~engine();

    /// @brief Copy constructor
    ///
    /// @param e
    engine(const engine& e);

    /// @brief Set the tracker's initial conditions.
    ///
    /// @note This will implicitly cause any current tracking to be stopped via stop().
    ///
    /// @param params
    /// @param partition
    void set_initial_conditions(const biggles::model::parameters& params, const biggles::partition_ptr_t& partition_ptr);
    void set_initial_conditions_part(const biggles::model::parameters& params,const biggles::partition& part);

    /// @brief Obtain the initial model parameters used by the tracker.
    const biggles::model::parameters& initial_model_parameters() const { return initial_parameters_; }

    /// @brief Obtain the initial partition used by the tracker.
    const biggles::partition_ptr_t& initial_partition() const { return initial_partition_; }

    /// @brief Return a shared pointer to the current tracking state.
    ///
    /// The contents are only useful if the tracking is currently running. If the tracking is not running, this is the
    /// tracking state at the time the last tracker stopped or an empty initial value if there was no prior tracking.
    boost::shared_ptr<server::tracking_state> tracking_state_ptr() const;

    /// @brief Start the tracker thread running.
    ///
    /// @note Calls to start() when the thread is already running are NOPs.
    void start();

    /// @brief Stop the tracker thread running.
    ///
    /// This member function will block until the thread has stopped.
    ///
    /// @note Calls to stop() when the thread is not running are NOPs.
    void stop();

    void record_state(const biggles::tracker& track);

protected:
    /// @brief Set to \c true iff the tracking thread should stop.
    bool should_stop_;

    /// @brief Initial model parameters for the tracker.
    biggles::model::parameters initial_parameters_;

    /// @brief An initial partition for the tracker.
    biggles::partition_ptr_t initial_partition_;

    /// @brief The current tracking thread. Can be NULL if no tracking is currently running.
    boost::shared_ptr<boost::thread> tracking_thread_ptr_;

    /// @brief The current state of the tracks.
    boost::shared_ptr<tracking_state> tracking_state_ptr_;

    /// @brief A mutex protecting tracking_state_ptr_
    ///
    /// This is needed because also shared_ptr allows simultaneous reads *or* simultaneous writes, a simultaneous write
    /// and read is undefined.
    mutable boost::mutex tracking_state_mutex_;

    /// @brief The tracking loop implementation.
    ///
    /// This does not return until \c should_stop_ is \c true.
    void track_loop();
};

}

}

#endif // BIGGLES_SERVER_STATE_HPP__


/**
 * @file      MeshFitterImpl.cpp
 *
 * @brief     Implementation file for MeshFitter::Impl
 *
 * @author    Stefan Reinhold
 * @copyright Copyright (C) 2018 Stefan Reinhold  -- All Rights Reserved.
 *            You may use, distribute and modify this code under the terms of
 *            the AFL 3.0 license; see LICENSE for full license details.
 */

#include "MeshFitterImpl.h"

#include "CommonMath.h"
#include "DiscreteRangeDecorators.h"
#include "DisplacementOptimizer.h"
#include "EigenAdaptors.h"
#include "MeshAdaptors.h"
#include "MeshFitterHiddenState.h"
#include "MeshHelpers.h"
#include "Sampler.h"
#include "WeightedARAPFitter.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <gsl/gsl>

namespace CortidQCT {

using namespace Internal;

/**********************
 * Helper Functions
 */

// MARK: -
// MARK: Helper Function

/**
 * @brief Returns a matrix containing positions to sample a voxel volume at
 *
 * Let \f$N := |V|\f$ and let \f$M := |R|\f$, where \f$R\f$ represent
 * `model.samplingRange`, then the returned NMx3 matrix contains N*M positions,
 * where each M consecutive postitions represent a line through a vertex in `V`
 * sampled along the surface normal in `N`.
 *
 * @param V Nx3 matrix with vertex positions
 * @param N Nx3 matrix of per-vertex surface normals
 * @param model MeasurementModel instance
 * @return NMx3 matrix containing sampling positions
 */
template <class DerivedV, class DerivedN>
Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 3>
samplingPoints(Eigen::MatrixBase<DerivedV> const &V,
               Eigen::MatrixBase<DerivedN> const &N,
               MeasurementModel const &model) {
  using Scalar = typename DerivedV::Scalar;
  using Eigen::Dynamic;
  using Eigen::Index;
  using Eigen::Matrix;
  using gsl::narrow_cast;

  auto const t = Internal::discreteRangeElementVector(model.samplingRange);
  Matrix<Scalar, Dynamic, 3> samples(V.rows() * t.rows(), 3);

  for (auto i = 0; i < V.rows(); ++i) {
    auto const iStart = i * t.rows();
    samples.block(iStart, 0, t.rows(), 3).colwise() = -t;
    samples.block(iStart, 0, t.rows(), 3).array().rowwise() *= N.row(i).array();
    samples.block(iStart, 0, t.rows(), 3).rowwise() += V.row(i);
  }

  return samples;
}

/***********************************
 * MeshFitter::Impl Implementation
 */

// MARK: -
// MARK: MeshFitter::Impl Implementation

MeshFitter::Result MeshFitter::Impl::fit(VoxelVolume const &volume) const {

  auto state = init(volume);

  VertexMatrix<float> Vlast =
      Adaptor::vertexMap(state.deformedMesh).transpose();

  while (!state.converged) {

    fitOneIteration(state);

    auto V = Adaptor::vertexMap(state.deformedMesh);

    auto const diff = (V.transpose() - Vlast).norm() / V.norm();
    Vlast = V.transpose();

    auto disNorm = Adaptor::map(state.displacementVector).norm() /
                   static_cast<float>(state.displacementVector.size());

    std::cout << "Converged after iteration " << state.iteration << ": "
              << std::boolalpha << state.converged << " (" << diff << " | "
              << disNorm << " | " << state.nonDecreasing << ")" << std::endl;
  }

  state.success = true;

  return std::move(state);
}

MeshFitter::State MeshFitter::Impl::init(VoxelVolume const &volume) const {
  using Eigen::Dynamic;
  using Eigen::Index;
  using Eigen::Map;
  using Eigen::Matrix;
  using Eigen::MatrixXf;
  using Eigen::Vector3f;
  using Eigen::VectorXf;
  using gsl::narrow_cast;

  MeshFitter::State state;

  auto const &conf = fitter_.configuration;
  // auto const &model = fitter_.configuration.model;

  // Init result state
  state.referenceMesh = conf.referenceMesh;

  auto const nVertices = narrow_cast<Index>(conf.referenceMesh.vertexCount());

  // Apply initial transofrmation on the vertices of the reference mesh
  auto const translation = Adaptor::vec(conf.meshTranslation(volume));
  Vector3f const rotInRad = Adaptor::vec(conf.referenceMeshRotation) *
                            static_cast<float>(M_PI) / 180.f;
  auto V0 = Adaptor::vertexMap(state.referenceMesh);

  // Apply transformation to vertices of reference mesh
  V0 = ((Eigen::Translation<float, 3>{translation} *
         Eigen::AngleAxisf(rotInRad[0], Vector3f::UnitX()) *
         Eigen::AngleAxisf(rotInRad[1], Vector3f::UnitY()) *
         Eigen::AngleAxisf(rotInRad[2], Vector3f::UnitZ())) *
        Adaptor::vec(conf.referenceMeshScale).asDiagonal() * V0);

  // Init vertex normals
  state.referenceMesh.updatePerVertexNormals();
  // This will be removed in versino 2.0:
  state.vertexNormals.resize(narrow_cast<std::size_t>(nVertices));
  Adaptor::map(state.vertexNormals) =
      Adaptor::vertexNormalMap(state.referenceMesh);

  // Init deformed mesh with reference mesh
  state.deformedMesh = state.referenceMesh;

  // Init displacement vector
  state.displacementVector =
      std::vector<float>(narrow_cast<std::size_t>(nVertices), .0f);

  // Init weight vector
  state.weights = std::vector<float>(narrow_cast<std::size_t>(nVertices),
                                     1 / static_cast<float>(nVertices));

  // Init vertex normals
  state.vertexNormals.resize(narrow_cast<std::size_t>(nVertices));

  // Init hidden state
  state.hiddenState_ = std::make_unique<State::HiddenState>(
      volume, DisplacementOptimizer{conf},
      WeightedARAPFitter<float>{V0.transpose(), facetMatrix(conf.referenceMesh),
                                narrow_cast<float>(conf.sigmaE)},
      facetMatrix(conf.referenceMesh));

  // Init volume sampling positions
  auto const nSamples = conf.model.samplingRange.numElements() *
                        narrow_cast<std::size_t>(nVertices);
  state.volumeSamplingPositions.resize(nSamples);

  // Init volume samples
  state.volumeSamples.resize(nSamples);

  sampleVolume(state);

  return state;
} // namespace CortidQCT

void MeshFitter::Impl::fitOneIteration(MeshFitter::State &state) const {

  if (state.hiddenState_ == nullptr) {
    throw std::invalid_argument("Invalid state argument, call init() first!");
  }

  findOptimalDisplacements(state);
  findOptimalDeformation(state);
  sampleVolume(state);
  computeLogLikelihood(state);
  checkConvergence(state);

  ++state.iteration;
}

void MeshFitter::Impl::findOptimalDisplacements(
    MeshFitter::State &state) const {
  if (state.hiddenState_ == nullptr) {
    throw std::invalid_argument("Invalid state argument, call init() first!");
  }

  // Get maps to state variables
  auto const labels = Adaptor::labelMap(state.referenceMesh);
  auto const N = Adaptor::vertexNormalMap(state.deformedMesh);
  auto const volumeSamples = Adaptor::map(state.volumeSamples);
  auto optimalDisplacements = Adaptor::map(state.displacementVector);
  auto gamma = Adaptor::map(state.weights);

  // Find optimal displacements
  std::tie(optimalDisplacements, gamma) =
      state.hiddenState_->displacementOptimizer(
          N.transpose(), labels, volumeSamples, state.nonDecreasing,
          state.effectiveSigmaS);
}

void MeshFitter::Impl::findOptimalDeformation(MeshFitter::State &state) const {
  if (state.hiddenState_ == nullptr) {
    throw std::invalid_argument("Invalid state argument, call init() first!");
  }

  auto const N = Adaptor::vertexNormalMap(state.deformedMesh);
  auto const optimalDisplacements = Adaptor::map(state.displacementVector);
  auto const gamma = Adaptor::map(state.weights);
  auto V = Adaptor::vertexMap(state.deformedMesh);

  VertexMatrix<> const Y =
      (V - (N.array().rowwise() * optimalDisplacements.array().transpose())
               .matrix())
          .transpose();

  // Fit mesh
  V = state.hiddenState_->meshFitter.fit(Y, N.transpose(), gamma).transpose();
  // Update normals
  state.deformedMesh.updatePerVertexNormals();
  // This will be removed in v2.0:
  Adaptor::map(state.vertexNormals) =
      Adaptor::vertexNormalMap(state.deformedMesh);
}

void MeshFitter::Impl::sampleVolume(MeshFitter::State &state) const {

  using Eigen::Map;
  using Eigen::MatrixXf;
  using Eigen::VectorXf;

  if (state.hiddenState_ == nullptr) {
    throw std::invalid_argument("Invalid state argument, call init() first!");
  }

  auto const &conf = fitter_.configuration;
  auto const V = Adaptor::vertexMap(state.deformedMesh);
  auto const nVertices = V.cols();
  auto N = Adaptor::vertexNormalMap(state.deformedMesh);
  auto volumeSamplingPositions = Adaptor::map(state.volumeSamplingPositions);
  auto volumeSamples = Adaptor::map(state.volumeSamples);

  // Copmute new sampling positions
  volumeSamplingPositions =
      samplingPoints(V.transpose(), N.transpose(), conf.model).transpose();

  // Sample the volume
  auto const volumeSampler = VolumeSampler{
      state.hiddenState_->volume, conf.ignoreExteriorSamples
                                      ? std::numeric_limits<float>::quiet_NaN()
                                      : 0.f};
  volumeSampler(volumeSamplingPositions.transpose(), volumeSamples,
                fitter_.configuration.calibrationSlope,
                fitter_.configuration.calibrationIntercept);

  // Reorder samples
  state.hiddenState_->volumeSamplesMatrix =
      Map<MatrixXf const>{volumeSamples.data(),
                          volumeSamples.rows() / nVertices, nVertices}
          .transpose();

  volumeSamples = Map<VectorXf const>{
      state.hiddenState_->volumeSamplesMatrix.data(), volumeSamples.rows()};
}

void MeshFitter::Impl::computeLogLikelihood(MeshFitter::State &state) const {
  using Eigen::Map;
  using Eigen::VectorXf;
  using gsl::narrow_cast;

  if (state.hiddenState_ == nullptr) {
    throw std::invalid_argument("Invalid state argument, call init() first!");
  }

  auto const labels = Adaptor::labelMap(state.referenceMesh);
  auto const N = Adaptor::vertexNormalMap(state.deformedMesh);
  auto const volumeSamples = Adaptor::map(state.volumeSamples);

  auto const llVec =
      state.hiddenState_->displacementOptimizer.logLikelihoodVector(
          N.transpose(), labels, volumeSamples);
  auto const LL = llVec.sum();

  state.logLikelihood = LL;
  state.perVertexLogLikelihood.resize(narrow_cast<std::size_t>(llVec.size()));

  Map<VectorXf>{state.perVertexLogLikelihood.data(), llVec.rows(), 1} = llVec;
}

void MeshFitter::Impl::checkConvergence(MeshFitter::State &state) const {
  if (state.hiddenState_ == nullptr) {
    throw std::invalid_argument("Invalid state argument, call init() first!");
  }

  auto const optimalDisplacements = Adaptor::map(state.displacementVector);
  auto const &conf = fitter_.configuration;

  state.converged = state.iteration >= conf.maxIterations ||
                    (optimalDisplacements.norm() < 1e-3f);

  auto disNorm = optimalDisplacements.norm() / optimalDisplacements.rows();
  if (disNorm < state.minDisNorm) {
    state.minDisNorm = disNorm;
    state.nonDecreasing = 0;
  } else {
    ++state.nonDecreasing;
  }
}

} // namespace CortidQCT


#include <boost/compute/random/threefry_engine.hpp>


#include "geodesic.hh"
#include "Geo/entity.hh"
#include "Geo/polynomial_solver.hh"
#include "Topology/topology.hh"
#include "Topology/impl.hh"
#include "Topology/iterator.hh"
#include "Utils/error_handling.hh"

#include <boost/math/tools/roots.hpp>

#include <bitset>
#include <functional>
#include <fstream>

#if 1

#include <map>
#include <queue>
#include <set>

namespace Offset
{
struct EdgeDistance;
using EdgeDistances = std::multimap<Topo::Wrap<Topo::Type::EDGE>, EdgeDistance>;
using EdgeAndDistance = EdgeDistances::value_type;

struct EdgeDistance
{
  EdgeDistance()
  {
    static int id_seq = 0;
    id_ = id_seq++;
  }
  ~EdgeDistance()
  {}

  void init(const Topo::Wrap<Topo::Type::FACE>& _f,
            const EdgeAndDistance* _parent)
  {
    if (x_ < 0)
    {
      x_ *= -1;
      y_ed_[0] *= -1;
      y_ed_[1] *= -1;
    }
    update_parameter();
    origin_face_ = _f;
    parent_ = _parent;
  }

  void update_parameter()
  {
    y_[0] = y_ed_[0] + (y_ed_[1] - y_ed_[0]) * param_range_[0];
    y_[1] = y_ed_[0] + (y_ed_[1] - y_ed_[0]) * param_range_[1];
    distance_range_.set_empty();
    distance_range_.add(sqrt(Geo::sq(x_) + Geo::sq(y_[0])) + dist0_);
    distance_range_.add(sqrt(Geo::sq(x_) + Geo::sq(y_[1])) + dist0_);
    if ((y_[0] > 0) != (y_[1] > 0))
      distance_range_.add(x_ + dist0_);
  }

  double get_distance(double _par,  double* _first_der = nullptr) const
  {
    auto dy = y_ed_[1] - y_ed_[0];
    auto y_part = y_ed_[0] + _par * dy;
    auto dist = sqrt(Geo::sq(x_) + Geo::sq(y_part));
    if (_first_der != nullptr)
      *_first_der = dy * y_part / dist;
    return dist0_ + dist;
  }

  size_t parameters(const double _dist, std::array<double, 2>& _pars)
  {
    if (!distance_range_.contain_close(_dist))
      return 0;
    auto y = std::sqrt(Geo::sq(_dist - dist0_) - Geo::sq(x_));
    size_t sol_nmbr = 0;
    auto get_solution = [this, &sol_nmbr, &_pars](double _y)
    {
      _pars[sol_nmbr] = (_y - y_ed_[0]) / (y_ed_[1] - y_ed_[0]);
      if (_pars[sol_nmbr] >= 0 && _pars[sol_nmbr] < 1)
        ++sol_nmbr;
    };
    get_solution(y);
    get_solution(-y);
    return sol_nmbr;
  }

  enum class Status { Keep = 0, Skip = 1, Remove = 2 };

  int id_;
  Topo::Wrap<Topo::Type::FACE> origin_face_; // Face used to get there
  double x_;
  double y_ed_[2];
  double dist0_ = 0;
  Geo::Interval<double> param_range_;  // Portion of edges
  Status status_ = Status::Keep;
  double y_[2];
  Geo::Interval<double> distance_range_;             // range of distances
  std::array<EdgeAndDistance*, 2> children_ = {nullptr};
  const EdgeAndDistance* parent_ = nullptr;
};

void check(const EdgeAndDistance& _edd, const Geo::Point& _orig)
{
  auto t = _edd.second.param_range_.interpolate(0.5);
  Topo::Iterator<Topo::Type::EDGE, Topo::Type::VERTEX> ev(_edd.first);
  Geo::Point pt, pt1;
  ev.get(0)->geom(pt);
  ev.get(1)->geom(pt1);
  pt = Geo::interpolate(pt, pt1, t);
  auto dist = Geo::angle(_orig, pt) * 5;
  if (!_edd.second.distance_range_.contain_close(dist)) dist; // std::cout << "error";
}

// Return 0 if _apr is a root, else
// < 0 ==> _a < _b
// > 0 ==> _b < _a

int find_minimum(const EdgeDistance& _a, const EdgeDistance& _b,
                 double& _par, Geo::Interval<double> _range)
{
  auto func = [&_a, &_b](const double& _t)
  {
    double der_a, der_b;
    auto val_a = _a.get_distance(_t, &der_a);
    auto val_b = _b.get_distance(_t, &der_b);
    return std::make_tuple(val_a - val_b, der_a - der_b);
  };
  boost::uintmax_t max_iter = 20;
  _par = boost::math::tools::newton_raphson_iterate(
    func, (_range[0] + _range[1]) / 2, _range[0], _range[1],
    std::numeric_limits<double>::digits - 4, max_iter);
  if (max_iter < 20)
    return 0;
  return std::get<0>(func(_range.interpolate(0.5))) > 0 ? 1 : -1;
}

struct GeodesicDistance: public IGeodesic
{
  Geo::Point origin_;
  // First smaller distances. Priority queue process first big elements.
  struct CompareEdgeDistancePtr
  {
    bool operator()(const EdgeAndDistance* _a, const EdgeAndDistance* _b) const
    {
      return _a->second.distance_range_[1] > _b->second.distance_range_[1];
    }
  };

  EdgeDistances edge_distances_;
  std::priority_queue<const EdgeAndDistance*, std::vector<const EdgeAndDistance*>,
    CompareEdgeDistancePtr> priority_queue_;

  bool compute(const Topo::Wrap<Topo::Type::VERTEX>& _v) override;

  bool find_graph(
    double _dist,
    std::vector<Geo::VectorD3>& _pts,
    std::vector<std::array<size_t, 2>>& _inds) override;

private:
  void advance(const EdgeAndDistance* _ed_span,
               const Topo::Wrap<Topo::Type::FACE>& _f);
  void merge(Topo::Wrap<Topo::Type::EDGE> _ed,
             EdgeDistance& _ed_dist);

  void insert_element(const Topo::Wrap<Topo::Type::EDGE> _ed,
                      EdgeDistance& _ed_dist)
  {
    auto it = edge_distances_.emplace(_ed, _ed_dist);
    check(*it, origin_);
    priority_queue_.push(&(*it));
  }
};

std::shared_ptr<IGeodesic> IGeodesic::make()
{
  return std::make_shared<GeodesicDistance>();
}

bool GeodesicDistance::compute(const Topo::Wrap<Topo::Type::VERTEX>& _v)
{
  _v->geom(origin_);
  Topo::Iterator<Topo::Type::VERTEX, Topo::Type::EDGE> vert_it(_v);
  for (auto e : vert_it)
  {
    EdgeDistance ed_dist;
    Topo::Iterator<Topo::Type::EDGE, Topo::Type::VERTEX> ed_vert_it(e);
    auto v1 = ed_vert_it.get(1);
    auto inv = v1 == _v;
    if (inv)
      v1 = ed_vert_it.get(0);
    Geo::Point pt;
    v1->geom(pt);
    ed_dist.x_ = 0;
    ed_dist.y_ed_[0] = 0;
    ed_dist.y_ed_[1] = Geo::length(pt - origin_);
    if (inv)
      std::swap(ed_dist.y_ed_[0], ed_dist.y_ed_[1]);
    ed_dist.param_range_ = { 0, 1 };
    ed_dist.init(Topo::Wrap<Topo::Type::FACE>(), nullptr);
    ed_dist.status_ = EdgeDistance::Status::Skip;
    insert_element(e, ed_dist);
  }
  Topo::Iterator<Topo::Type::VERTEX, Topo::Type::FACE> face_it(_v);
  for (auto f : face_it)
  {
    Topo::Iterator<Topo::Type::FACE, Topo::Type::EDGE> ed_it(f);
    Topo::Wrap<Topo::Type::EDGE> curr_ed;
    for (auto e : ed_it)
    {
      auto it = edge_distances_.equal_range(e);
      if (it.first == it.second)
      {
        curr_ed = e;
        break;
      }
    }
    if (curr_ed.get() == nullptr)
      continue;
    EdgeDistance ed_dist;
    Topo::Iterator<Topo::Type::EDGE, Topo::Type::VERTEX> ed_vert(curr_ed);
    Geo::Segment seg;
    ed_vert.get(0)->geom(seg[0]);
    ed_vert.get(1)->geom(seg[1]);
    double dist_sq, t;
    Geo::VectorD3 proj;
    if (!Gen::closest_point<double, 3, true>(seg, origin_, &proj, &t, &dist_sq))
      return false;
    ed_dist.x_ = sqrt(dist_sq);
    ed_dist.y_ed_[0] = Geo::length(seg[0] - proj);
    ed_dist.y_ed_[1] = Geo::length(seg[1] - proj);
    if (t < 1 && t > 0)
      ed_dist.y_ed_[ed_dist.y_ed_[1] < ed_dist.y_ed_[0]] *= -1;
    ed_dist.param_range_ = { 0, 1 };
    ed_dist.init(f, nullptr);
    insert_element(curr_ed, ed_dist);
  }
  while (!priority_queue_.empty())
  {
    auto edge_span = priority_queue_.top();
    priority_queue_.pop();
    if (edge_span->second.status_ != EdgeDistance::Status::Keep)
      continue;
    Topo::Iterator<Topo::Type::EDGE, Topo::Type::FACE> fe_it(edge_span->first);
    for (auto f : fe_it)
    {
      if (f != edge_span->second.origin_face_)
        advance(edge_span, f);
    }
  }
  for (auto it = edge_distances_.begin(); it != edge_distances_.end();)
  {
    if (it->second.status_ == EdgeDistance::Status::Remove)
      it = edge_distances_.erase(it);
    else
      ++it;
  }
  std::ofstream outf("c:/t/out.txt");
  outf << "V0 V1 Id Px Py Pz Qx Qy Qz x y0 y1 interval0 interval1 distance0  distance1 dist0 par_id \n";
  for (auto& es : edge_distances_)
  {
    Geo::Segment seg;
    es.first->geom(seg);
    Topo::Iterator<Topo::Type::EDGE, Topo::Type::VERTEX> ed_it(es.first);
    outf << ed_it.get(0)->id() << " " << ed_it.get(1)->id() << " ";
    outf << es.second.id_ << " " << seg[0] << " " << seg[1] << " ";
    outf << es.second.x_ << " ";
    outf << es.second.y_ed_[0] << " " << es.second.y_ed_[1] << " ";
    outf << es.second.param_range_[0] << " " << es.second.param_range_[1] << " ";
    outf << es.second.distance_range_[0] << " " << es.second.distance_range_[1] << " ";
    outf << es.second.dist0_ << " ";
    if (es.second.parent_ == nullptr)
      outf << -1;
    else
      outf << es.second.parent_->second.id_;
    outf  << std::endl;
  }
  outf << "SIZE = " << edge_distances_.size() << "\n";
  return true;
}

void GeodesicDistance::advance(
  const EdgeAndDistance* _parent, const Topo::Wrap<Topo::Type::FACE>& _f)
{
  Topo::Iterator<Topo::Type::EDGE, Topo::Type::VERTEX> it_ev_pa(_parent->first);
  Topo::Wrap<Topo::Type::VERTEX> verts[2] = { it_ev_pa.get(0), it_ev_pa.get(1) };
  Geo::VectorD3 p_parent[2];
  Geo::iterate_forw<2>::eval([&verts, &p_parent](int _i) { verts[_i]->geom(p_parent[_i]); });
  Geo::VectorD3 v_parent = p_parent[1] - p_parent[0];
  Topo::Iterator<Topo::Type::FACE, Topo::Type::EDGE> it_fe(_f);
  struct OtherEdge
  {
    Topo::Wrap<Topo::Type::EDGE> ed_;
    bool inv_;
    Geo::VectorD3 v_;
  };
  Gen::Segment<double, 2> parent_seg{ {
    { _parent->second.x_, _parent->second.y_[0] },
    { _parent->second.x_, _parent->second.y_[1] } } };
  Gen::Segment<double, 2> parent_trace_segs[2];
  for (int i = 0; i < 2; ++i)
  {
    parent_trace_segs[i][0] = {};
    parent_trace_segs[i][1] = parent_seg[i];
  }

  OtherEdge oth_eds[2];
  auto dy_par = _parent->second.y_ed_[1] - _parent->second.y_ed_[0];
  for (auto& fe : it_fe)
  {
    if (fe == _parent->first)
      continue;
    EdgeDistance new_edd;
    Topo::Iterator<Topo::Type::EDGE, Topo::Type::VERTEX> it_ev(fe);
    bool idx = it_ev.get(1) == verts[1] || it_ev.get(0) == verts[1];
    auto inv = it_ev.get(1) == verts[idx];
    oth_eds[idx].ed_ = fe;
    oth_eds[idx].inv_ = inv;
    Geo::VectorD3 p[2];
    Geo::iterate_forw<2>::eval([&it_ev, &p](int _i) { it_ev.get(_i)->geom(p[_i]); });
    auto v0 = v_parent;
    if (dy_par < 0) v0 *= -1.;
    v0 /= Geo::length(v0);
    auto v1 = oth_eds[idx].v_ = p[1] - p[0];
    if (inv) v1 *= -1.;
    auto dy = v0 * v1;
    //if (dy * dy_par < 0)
    //  continue;
    auto dx = Geo::length(v0 % v1);
    Gen::Segment<double, 2> v2 = { {
      { _parent->second.x_, _parent->second.y_ed_[idx] },
      { _parent->second.x_ + dx, _parent->second.y_ed_[idx] + dy } } };
    if (inv)
      std::swap(v2[0], v2[1]);
    auto dist_3d = Geo::length(p[0] - p[1]);

    auto compute_x_y_ed = [dist_3d](const Gen::Segment<double, 2>& _v, EdgeDistance& _new_ed)
    {
      Geo::VectorD2 proj;
      double dist_sq, par;
      Gen::closest_point<double, 2, true>(_v, Geo::VectorD2{},
                                          &proj, &par, &dist_sq);
      _new_ed.x_ = sqrt(dist_sq);
      _new_ed.y_ed_[0] = Geo::length(_v[0] - proj);
      _new_ed.y_ed_[1] = Geo::length(_v[1] - proj);
      if (par > 0 && par < 1)
        _new_ed.y_ed_[_new_ed.y_ed_[0] >_new_ed.y_ed_[1]] *= -1;
      THROW_IF((fabs(_new_ed.y_ed_[1] - _new_ed.y_ed_[0]) - dist_3d) > 1e-8,
               "Wrong y reange");
    };
    compute_x_y_ed(v2, new_edd);

    Geo::Interval<double> tmp;
    for (int i = 0; i < 2; ++i)
    {
      bool done = false;
      for (int j = 0; j < 2; ++j)
      {
        double t, dist_sq;
        Gen::closest_point<double, 2, true>(
          parent_trace_segs[i], v2[j], nullptr, &t, &dist_sq);
        done = dist_sq < Geo::epsilon_sq(v2[j]);
        if (done)
        {
          tmp.add(j);
          break;
        }
      }
      if (done)
        continue;

      double pars[2], dist_sq;
      Gen::closest_point<double, 2, true, true>(
        parent_trace_segs[i], v2, nullptr, pars, &dist_sq);
      auto dir = v2[1] - v2[0];
      double cr_pr = fabs(parent_trace_segs[i][1] % dir);
      if (cr_pr < 1e-12)
        pars[1] = Geo::length_square(v2[1]) > Geo::length_square(v2[0]) ? 1: 0;
      //auto val_sq = std::max(Geo::length_square(v2[0]), Geo::length_square(v2[1]));
      else if (pars[0] <= 1e-12)
      {
        if (pars[1] <= 0) pars[1] = 1;
        else if (pars[1] >= 1) pars[1] = 0;
        else
          THROW("Bad par");

      }
      else if (pars[0] <= 1 - 1e-12)
        continue;
      tmp.add(std::clamp(pars[1], 0., 1.));
    }

    if (!tmp.empty())
    {
      new_edd.param_range_ = tmp;
      new_edd.dist0_ = _parent->second.dist0_;
      new_edd.init(_f, _parent);
      merge(fe, new_edd);
    }

    size_t par_bndr_to_fill = idx;
    if (tmp.empty() &&
        (_parent->second.param_range_[par_bndr_to_fill] == par_bndr_to_fill))
    {
      EdgeDistance new_edd2;
      new_edd2.dist0_ = _parent->second.get_distance(static_cast<double>(par_bndr_to_fill));
      auto orig = Geo::VectorD2{ _parent->second.x_, _parent->second.y_ed_[par_bndr_to_fill] };
      Gen::Segment<double, 2> new_seg;
      new_seg[0] = v2[0] - orig;
      new_seg[1] = v2[1] - orig;
      compute_x_y_ed(new_seg, new_edd2);
      new_edd2.param_range_ = { 0., 1. };
      new_edd2.init(_f, _parent);
      merge(fe, new_edd2);
    }
    par_bndr_to_fill = !idx;
    size_t new_par = !inv;
    if (!tmp.empty() && tmp[new_par] != new_par &&
      (_parent->second.param_range_[par_bndr_to_fill] == par_bndr_to_fill))
    {
      EdgeDistance new_edd2;
      new_edd2.dist0_ = _parent->second.get_distance(static_cast<double>(par_bndr_to_fill));
      auto orig = Geo::VectorD2{ _parent->second.x_, _parent->second.y_ed_[par_bndr_to_fill] };
      Gen::Segment<double, 2> new_seg;
      new_seg[0] = v2[0] - orig;
      new_seg[1] = v2[1] - orig;
      compute_x_y_ed(new_seg, new_edd2);
      if (inv)
        new_edd2.param_range_ = Geo::Interval<double>(0., tmp[0]);
      else
        new_edd2.param_range_ = Geo::Interval<double>(tmp[1], 1.);
      new_edd2.init(_f, _parent);
      merge(fe, new_edd2);
    }

  }
  THROW_IF(!oth_eds[0].ed_.get() || !oth_eds[1].ed_.get(), "");
}

static std::bitset<2> intersect(EdgeDistance& _a, EdgeDistance& _b,
                                Geo::Interval<double> _extras[2])
{
  auto inters_par_range = _a.param_range_ * _b.param_range_;
  if (inters_par_range.length() < 1e-12)
    return 0;
  double root;
  auto a_minus_b = find_minimum(_a, _b, root, inters_par_range);
  std::bitset<2> res;
  if (a_minus_b > 0)
    res[0] = _a.param_range_.subtract(inters_par_range, _extras[0]);
  else if (a_minus_b < 0)
    res[1] = _b.param_range_.subtract(inters_par_range, _extras[1]);
  else
  {
    auto snap_to_boundary = [](double& _root, const Geo::Interval<double>& _interv)
    {
      for (int i = 0; i < 2; ++i)
        if (fabs(_root - _interv[i]) < 1e-8)
        {
          _root = _interv[i];
          return true;
        }
      return false;
    };
    snap_to_boundary(root, _a.param_range_) || snap_to_boundary(root, _b.param_range_);
    Geo::Interval<double> near_a(Geo::Interval<double>::min(), root);
    Geo::Interval<double> near_b(root, Geo::Interval<double>::max());
    if (_a.get_distance(root - 1) > _b.get_distance(root - 1))
      std::swap(near_a, near_b);

    near_b *= _b.param_range_;
    res[0] = _a.param_range_.subtract(near_b, _extras[0]);
    near_a *= _a.param_range_;
    res[1] = _b.param_range_.subtract(near_a, _extras[1]);
  }
  return res;
}

void GeodesicDistance::merge(Topo::Wrap<Topo::Type::EDGE> _ed,
                             EdgeDistance& _ed_dist)
{
  auto range = edge_distances_.equal_range(_ed);
  Topo::Iterator<Topo::Type::EDGE, Topo::Type::VERTEX> edv(_ed);
  if (edv.get(0)->id() == 395 && edv.get(1)->id() == 436)
  {
    std::cout << "Here we are";
  }
  std::vector<EdgeDistance*> old_ed_dists;
  for (auto it = range.first; it != range.second; ++it)
    old_ed_dists.push_back(&(it->second));
  std::vector<EdgeDistance> new_edds;
  new_edds.push_back(_ed_dist);
  for (auto old_edd : old_ed_dists)
  {
    //if (it->second.status_ != EdgeDistance::Status::Keep)
    if (old_edd->status_ == EdgeDistance::Status::Remove)
      continue;
    auto size = new_edds.size();
    for (size_t i = 0; i < size; ++i)
    {
      auto& new_edd = new_edds[i];
      Geo::Interval<double> extras[2];
      auto int_res = intersect(new_edd, *old_edd, extras);
      if (!int_res.any())
        continue;
      if (int_res[1])
      {
        if (old_edd->param_range_.empty())
        {
          std::function<void(EdgeDistance* _ed_dist)> skip_children =
            [&](EdgeDistance* _ed_dist)
          {
            _ed_dist->status_ = EdgeDistance::Status::Remove;
            for (auto child : _ed_dist->children_)
              if (child != nullptr)
                skip_children(&child->second);
          };
          skip_children(old_edd);
        }
        else
        {
          old_edd->update_parameter();
          if (!extras[1].empty())
          {
            auto split_piece = *old_edd;
            split_piece.param_range_ = extras[1];
            split_piece.update_parameter();
            insert_element(_ed, split_piece);
          }
        }
      }
      if (int_res[0])
      {
        new_edd.update_parameter();
        if (!extras[0].empty())
        {
          auto new_el = new_edds.emplace_back(new_edd);
          new_el.param_range_ = extras[0];
          new_el.update_parameter();
        }
      }
    }
  }
  for (auto& new_edd : new_edds)
    if (!new_edd.param_range_.empty())
      insert_element(_ed, new_edd);
}

bool GeodesicDistance::find_graph(
  double _dist,
  std::vector<Geo::VectorD3>& _pts,
  std::vector<std::array<size_t, 2>>& _inds)
{
  using FacePointMap = std::map<Topo::Wrap<Topo::Type::FACE>, std::vector<size_t>>;
  FacePointMap fp_map;
  for (auto& ed_dist : edge_distances_)
  {
    std::array<double, 2> pars;
    auto sol_num = ed_dist.second.parameters(_dist, pars);
    if (sol_num == 0)
      continue;
    Topo::Iterator<Topo::Type::EDGE, Topo::Type::VERTEX> vert_it(ed_dist.first);
    Geo::Segment seg;
    vert_it.get(0)->geom(seg[0]);
    vert_it.get(1)->geom(seg[1]);
    Topo::Iterator<Topo::Type::EDGE, Topo::Type::FACE> face_it(ed_dist.first);
    for (int i = 0; i < sol_num; ++i)
    {
      auto pt = Geo::evaluate(seg, pars[i]);
      for (auto f : face_it)
        fp_map[f].push_back(_pts.size());
      _pts.push_back(pt);
    }
  }
  for (auto& fp : fp_map)
  {
    for (auto it = fp.second.begin(); it != fp.second.end(); ++it)
      for (auto it1 = it; ++it1 != fp.second.end(); )
        _inds.push_back({ *it, *it1 });
  }
  return true;
}

} // namespace Offset

#endif


#include "Sh3FixedPoint.h"

#include <boost/multiprecision/cpp_int.hpp>
#include <cryptoTools/Common/BitVector.h>
namespace aby3
{
	template<typename T, Decimal D>
	fp<T, D> fp<T, D>::operator*(const fp<T, D>& rhs) const
	{
		boost::multiprecision::int128_t v0 = mValue, v1 = rhs.mValue;

		v0 = v0 * v1;
		v0 = v0 / (1ull << mDecimal);

		return {
			static_cast<i64>(v0),
			monostate{}
		};
	}

	//template<typename T, Decimal D>
	//std::ostream& operator<<(std::ostream& o, const fp<T, D>& f)
	//{
	//	auto mask = ((1ull << f.mDecimal) - 1);
	//	auto print = [mask](T v) {
	//		std::stringstream s;
	//		oc::BitVector bv;
	//		bv.append((u8*)& v, D);
	//		s << bv;

	//		s << ".";
	//		bv = {};
	//		bv.append((u8*)& v, 64 - D, D);
	//		s << bv;
	//		return s.str();
	//	};

	//	std::stringstream ss;
	//	//auto& ss = o;
	//	auto vv = (f.mValue / double(1 << f.mDecimal));
	//	o << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
	//		<< vv << std::endl;
	//	o << print(f.mValue) << std::endl;

	//	u64 v = 0;
	//	if (f.mValue >= 0)
	//		v = f.mValue;
	//	else
	//	{
	//		ss << '-';
	//		v =-f.mValue;
	//	}

	//	ss << (v >> f.mDecimal) << ".";

	//	v &= mask;

	//	int i = 0;
	//	if (v)
	//	{
	//		o << "["<<i<<"]  " << print(v) << std::endl;
	//		while (v & mask)
	//		{
	//			++i;
	//			v *= 10;
	//			ss << (v >> f.mDecimal);
	//			o << "[" << i << "]  " << print(v) << std::endl;

	//			v &= mask;
	//			o << "[" << i << "]* " << print(v) << std::endl;
	//		}
	//	}
	//	else
	//	{
	//		ss << '0';
	//	}
	//	o << ss.str();

	//	return o;
	//}

	//template std::ostream& operator<<<i64, D8>(std::ostream& o, const fp<i64, D8>& f);
	//template std::ostream& operator<<<i64, D16>(std::ostream& o, const fp<i64, D16>& f);
	//template std::ostream& operator<<<i64, D32>(std::ostream& o, const fp<i64, D32>& f);

	template struct fp<i64, D0>;
	template struct fp<i64, D8>;
	template struct fp<i64, D16>;
	template struct fp<i64, D32>;

	template struct sf64<D0>;
	template struct sf64<D8>;
	template struct sf64<D16>;
	template struct sf64<D32>;

	template struct sf64Matrix<D0>;
	template struct sf64Matrix<D8>;
	template struct sf64Matrix<D16>;
	template struct sf64Matrix<D32>;

}


/***************************************************************************************************************************
 * kcf_tracker.cpp
 * Author: Jario
 * Update Time: 2020.1.14
 *
 * 说明: KCF目标跟踪程序
 *      1. 【订阅】图像话题 (默认来自web_cam)
 *         /prometheus/camera/rgb/image_raw
 *      2. 【发布】目标位置，发布话题见 Prometheus/Modules/msgs/msg/DetectionInfo.msg
 *         /prometheus/object_detection/kcf_tracker
***************************************************************************************************************************/
#include <math.h>
#include <string>
#include <vector>
#include <iostream>
#include <pthread.h>
#include <thread>
#include <chrono>
#include <boost/thread/mutex.hpp>
#include <boost/thread/shared_mutex.hpp>

#include <ros/ros.h>
#include <ros/package.h>
#include <yaml-cpp/yaml.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <prometheus_msgs/DetectionInfo.h>
#include <geometry_msgs/Pose.h>
#include <std_msgs/Bool.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/ml.hpp>

#include "kcftracker.hpp"
#include "message_utils.h"

using namespace std;
using namespace cv;

static const std::string RGB_WINDOW = "RGB Image window";

// 相机话题中的图像同步相关变量
int frame_width, frame_height;
std_msgs::Header image_header;
cv::Mat cam_image_copy;
boost::shared_mutex mutex_image_callback;
bool image_status = false;
boost::shared_mutex mutex_image_status;

//【订阅】输入开关量
ros::Subscriber switch_subscriber;
// 接收消息，允许暂停检测
bool is_suspanded = false;
// 使用cout打印消息
bool local_print = false;
// 使用prometheus_msgs::Message打印消息
bool message_print = true;
//【发布】调试消息
ros::Publisher message_pub;
std::string msg_node_name;

// 图像接收回调函数，接收web_cam的话题，并将图像保存在cam_image_copy中
void cameraCallback(const sensor_msgs::ImageConstPtr& msg)
{
    if (local_print)
        ROS_DEBUG("[KCFTracker] USB image received.");

    cv_bridge::CvImagePtr cam_image;

    try {
        cam_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
        image_header = msg->header;
    } catch (cv_bridge::Exception& e) {
        if (local_print)
            ROS_ERROR("cv_bridge exception: %s", e.what());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::ERROR, msg_node_name, "cv_bridge exception");
        return;
    }

    if (cam_image) {
        {
            boost::unique_lock<boost::shared_mutex> lockImageCallback(mutex_image_callback);
            cam_image_copy = cam_image->image.clone();
        }
        {
            boost::unique_lock<boost::shared_mutex> lockImageStatus(mutex_image_status);
            image_status = true;
        }
        frame_width = cam_image->image.size().width;
        frame_height = cam_image->image.size().height;
    }
    return;
}

// 用此函数查看是否收到图像话题
bool getImageStatus(void)
{
    boost::shared_lock<boost::shared_mutex> lock(mutex_image_status);
    return image_status;
}

//! ROS subscriber and publisher.
//【订阅】输入图像
image_transport::Subscriber imageSubscriber_;
//【发布】目标位置
ros::Publisher pose_pub;
prometheus_msgs::DetectionInfo pose_now;

cv::Rect selectRect;
cv::Point origin;
cv::Rect result;

bool select_flag = false;
bool bRenewROI = false;  // the flag to enable the implementation of KCF algorithm for the new chosen ROI
bool bBeginKCF = false;

void onMouse(int event, int x, int y, int, void*)
{
    if (select_flag)
    {
        selectRect.x = MIN(origin.x, x);
        selectRect.y = MIN(origin.y, y);
        selectRect.width = abs(x - origin.x);
        selectRect.height = abs(y - origin.y);
        selectRect &= cv::Rect(0, 0, frame_width, frame_height);
    }
    if (event == CV_EVENT_LBUTTONDOWN)
    {
        bBeginKCF = false;
        select_flag = true;
        origin = cv::Point(x, y);
        selectRect = cv::Rect(x, y, 0, 0);
    }
    else if (event == CV_EVENT_LBUTTONUP)
    {
        if (selectRect.width*selectRect.height < 64)
        {
            ;
        }
        else
        {
            select_flag = false;
            bRenewROI = true;
        }
    }
}

void switchCallback(const std_msgs::Bool::ConstPtr& msg)
{
    is_suspanded = !(bool)msg->data;
    // cout << is_suspanded << endl;
}

bool HOG = true;
bool FIXEDWINDOW = false;
bool MULTISCALE = true;
bool SILENT = true;
bool LAB = false;

// Create KCFTracker object
KCFTracker tracker(HOG, FIXEDWINDOW, MULTISCALE, LAB);

int main(int argc, char **argv)
{
    ros::init(argc, argv, "kcf_tracker");
    ros::NodeHandle nh("~");
    image_transport::ImageTransport it(nh);
    ros::Rate loop_rate(30);

    // 发布调试消息
    msg_node_name = "/prometheus/message/kcf_tracker";
    message_pub = nh.advertise<prometheus_msgs::Message>(msg_node_name, 10);

    std::string camera_topic, camera_info;
    if (nh.getParam("camera_topic", camera_topic)) {
        if (local_print)
            ROS_INFO("camera_topic is %s", camera_topic.c_str());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "camera_topic is" + camera_topic);
    } else {
        if (local_print)
            ROS_WARN("didn't find parameter camera_topic");
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::WARN, msg_node_name, "didn't find parameter camera_topic");
        camera_topic = "/prometheus/camera/rgb/image_raw";
    }

    if (nh.getParam("camera_info", camera_info)) {
        if (local_print)
            ROS_INFO("camera_info is %s", camera_info.c_str());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "camera_info is" + camera_info);
    } else {
        if (local_print)
            ROS_WARN("didn't find parameter camera_info");
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::WARN, msg_node_name, "didn't find parameter camera_info");
        camera_info = "camera_param.yaml";
    }

    bool switch_state = is_suspanded;
    // 接收开关话题
    switch_subscriber = nh.subscribe("/prometheus/switch/kcf_tracker", 10, switchCallback);

    // 接收图像的话题
    imageSubscriber_ = it.subscribe(camera_topic.c_str(), 1, cameraCallback);

    // 跟踪结果，xyz
    pose_pub = nh.advertise<prometheus_msgs::DetectionInfo>("/prometheus/object_detection/kcf_tracker", 1);

    sensor_msgs::ImagePtr msg_ellipse;

    std::string ros_path = ros::package::getPath("prometheus_detection");
    if (local_print)
        cout << "DETECTION_PATH: " << ros_path << endl;
    if (message_print)
        pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "DETECTION_PATH: " + ros_path);
    // 读取参数文档camera_param.yaml中的参数值；
    YAML::Node camera_config = YAML::LoadFile(ros_path + "/config/" + camera_info);
    // 相机内部参数
    double fx = camera_config["fx"].as<double>();
    double fy = camera_config["fy"].as<double>();
    double cx = camera_config["x0"].as<double>();
    double cy = camera_config["y0"].as<double>();
    // 相机畸变系数
    double k1 = camera_config["k1"].as<double>();
    double k2 = camera_config["k2"].as<double>();
    double p1 = camera_config["p1"].as<double>();
    double p2 = camera_config["p2"].as<double>();
    double k3 = camera_config["k3"].as<double>();

    double kcf_tracker_h = camera_config["kcf_tracker_h"].as<double>();

    const auto wait_duration = std::chrono::milliseconds(2000);

    cv::namedWindow(RGB_WINDOW);
    cv::setMouseCallback(RGB_WINDOW, onMouse, 0);
    float last_x(0), last_y(0), last_z(0), last_ax(0), last_ay(0);

    while (ros::ok())
    {
        while (!getImageStatus() && ros::ok())
        {
            if (local_print)
                cout << "Waiting for image." << endl;
            if (message_print)
                pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "Waiting for image.");
            std::this_thread::sleep_for(wait_duration);
            ros::spinOnce();
        }

        if (switch_state != is_suspanded)
        {
            switch_state = is_suspanded;
            if (!is_suspanded)
            {
                if (local_print)
                    cout << "Start Detection." << endl;
                if (message_print)
                    pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "Start Detection.");
            }
            else
            {
                if (local_print)
                    cout << "Stop Detection." << endl;
                if (message_print)
                    pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "Stop Detection.");
            }
        }

        if (!is_suspanded)
        {
        Mat frame;
        {
            boost::unique_lock<boost::shared_mutex> lockImageCallback(mutex_image_callback);
            frame = cam_image_copy.clone();
        }
        static bool need_tracking_det = false;

        bool detected = false;
        if (bRenewROI)
        {
            tracker.init(selectRect, frame);
            cv::rectangle(frame, selectRect, cv::Scalar(255, 0, 0), 2, 8, 0);
            bRenewROI = false;
            bBeginKCF = true;
        }
        else if (bBeginKCF)
        {
            result = tracker.update(frame);
            cv::rectangle(frame, result, cv::Scalar(255, 0, 0), 2, 8, 0);

            // 将解算后的位置发给控制端
            detected = true;
            pose_now.header.stamp = ros::Time::now();
            pose_now.detected = true;
            pose_now.frame = 0;
            double depth = kcf_tracker_h / result.height * fy;
            double cx = result.x + result.width / 2 - frame.cols / 2;
            double cy = result.y + result.height / 2 - frame.rows / 2;
            pose_now.position[0] = depth * cx / fx;
            pose_now.position[1] = depth * cy / fy;
            pose_now.position[2] = depth;

            pose_now.sight_angle[0] = cx / (frame.cols / 2) * atan((frame.cols / 2) / fx);
            pose_now.sight_angle[1] = cy / (frame.rows / 2) * atan((frame.rows / 2) / fy);

            last_x = pose_now.position[0];
            last_y = pose_now.position[1];
            last_z = pose_now.position[2];
            last_ax = pose_now.sight_angle[0];
            last_ay = pose_now.sight_angle[1];
        }

        if (!detected)
        {
            pose_now.header.stamp = ros::Time::now();
            pose_now.detected = false;
            pose_now.frame = 0;
            pose_now.position[0] = last_x;
            pose_now.position[1] = last_y;
            pose_now.position[2] = last_z;
            pose_now.sight_angle[0] = last_ax;
            pose_now.sight_angle[1] = last_ay;
        }

        pose_pub.publish(pose_now);

        imshow(RGB_WINDOW, frame);
        }

        waitKey(5);
        ros::spinOnce();
        loop_rate.sleep();
    }
}


#include <map>
#include <memory>
#include <boost/random/exponential_distribution.hpp>
#include <boost/random/discrete_distribution.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>

#include "../globals.h"

#ifndef TENANTGENERATOR_HPP_
#define TENANTGENERATOR_HPP_

class TenantGenerator
	: public AbstractGenerator,
	  public etcd_helper {

	public:
		typedef std::shared_ptr<TenantGenerator> ptr;

		virtual ~TenantGenerator() {};

		TenantGenerator(std::string basep,
				        uint32_t rate,
				        std::vector<OFPT::OFPT_T> of_types,
				        std::vector<float> weights)
			: etcd_helper(basep),
			  _weights(weights),
			  _of_types(of_types),
			  _rate(rate),
			  _rng(std::time(NULL)),
			  _nd(rate),
			  _var_exp(new VarGenMtExp(_rng, _nd)),
			  _discrete_nd(_weights),
			  _var_disc(new VarGenMtDis(_rng, _discrete_nd))
			  {

			_next_msg += boost::chrono::microseconds(static_cast<uint64_t>((*_var_exp.get())() * 1000000.));
		};

		time_point_t peek() {

			auto tp = base_time_ + _next_msg;

			return tp;
		};

		ScheduledMsg next() {

			_idx++;

			auto next_type = _of_types[(*_var_disc.get())()];

			AbstractOFMsg::ptr ofmsg_ptr;

			switch(next_type) {

				case OFPT::FEATURE_REQUEST:
				{
					ofmsg_ptr = FeatureRequestOFMsg::gen(_idx);
				}
				break;
				case OFPT::ECHO_REQUEST:
				{
					ofmsg_ptr = EchoRequestOFMsg::gen(_idx);
				}
				break;
				case OFPT::STATS_REQUEST_PORT:
				{
					ofmsg_ptr = StatsRequestPortOFMsg::gen(_idx);
				}
				break;
				case OFPT::STATS_REQUEST_FLOW:
				{
					ofmsg_ptr = StatsRequestFlowOFMsg::gen(_idx);
				}
				break;
				case OFPT::PACKET_OUT_UDP:
				{
					ofmsg_ptr = PacketOutUDPOFMsg::gen(_idx);
				}
				break;
				case OFPT::FLOW_MOD:
				{
					ofmsg_ptr = FlowModOFMsg::gen(_idx);
				}
				break;
				default:
					BOOST_ASSERT(false);
			}

			auto scheduledm = ScheduledMsg(base_time_ + _next_msg, ofmsg_ptr);

			_next_msg += boost::chrono::microseconds(static_cast<uint64_t>((*_var_exp.get())() * 1000000.));

			return count(scheduledm);
		};

		void update(std::string key, std::string value) {

			std::cout << "UPDATE " << key << " to " << value << std::endl;

			if (key == "avg_rate") {

				//FIXME: This is not thread-safe!
				_nd = boost::exponential_distribution<>(boost::lexical_cast<uint32_t>(value));
				_var_exp = std::shared_ptr<VarGenMtExp>(new VarGenMtExp(_rng, _nd));
			}

			if (OFPT::RSTR.find(key) == OFPT::RSTR.end()) {

				std::cerr << "Unknown message type " << key << "!" <<std::endl;
				return;
			}

			auto it = std::find(this->_of_types.begin(), this->_of_types.end(), OFPT::RSTR.at(key));

			if (it == this->_of_types.end()) {

				std::cerr << "ERR: Updated message weight for message type " << key << " was not in the original message type set!" <<std::endl;
				return;

			} else {

				auto index = std::distance(this->_of_types.begin(), it);

				this->_weights[index] = boost::lexical_cast<float>(value);

				this->_discrete_nd = boost::random::discrete_distribution<>(_weights);

				this->_var_disc = std::make_shared<VarGenMtDis>(this->_rng, this->_discrete_nd);

				std::cout << "Weight of " << key << " updated successfully to " << value << "." << std::endl;
			}
		}

	private:
		std::vector<float>        _weights;
		std::vector<OFPT::OFPT_T> _of_types;
		uint32_t& _rate;

		boost::mt19937 _rng;

		// Inter-arrival distribution
		boost::exponential_distribution<> _nd;

		typedef boost::variate_generator<boost::mt19937&,
		         	 	 	 	 	 	 boost::exponential_distribution<> > VarGenMtExp;

		std::shared_ptr<VarGenMtExp> _var_exp;

		// Message type distribution
		boost::random::discrete_distribution<> _discrete_nd;

		typedef boost::variate_generator<boost::mt19937&,
					             boost::random::discrete_distribution<> > VarGenMtDis;

		std::shared_ptr<VarGenMtDis> _var_disc;

		std::map<OFPT::OFPT_T, AbstractOFMsgFactory::ptr> _msg_factories;

		boost::chrono::microseconds _next_msg;

		uint32_t _idx = 0;
};

enum TTYPES {

	TT_MISUSE_FEATURE_REQ = 0,
	TT_MISUSE_ECHO_REQ = 1,
	TT_EQUAL = 2,
	TT_STATS = 3,
	TT_PACKET_OUT_UDP = 4,
	TT_FLOW_MOD = 5
};

/*
typedef std::pair<std::vector<OFPT::OFPT_T>, std::vector<float> > OFPT_WEIGHTS_PAIR_T;

class TenantFactory {

	public:
		TenantFactory() {};

		static TenantGenerator::ptr create(const uint32_t avgr) {

			boost::mt19937 rng(std::time(NULL));

			std::vector<TTYPES> tt_types        = {TT_MISUSE_FEATURE_REQ, TT_MISUSE_ECHO_REQ, TT_EQUAL, TT_STATS, TT_PACKET_OUT_UDP, TT_FLOW_MOD};
			std::vector<float> tt_stats_weights = {                  0.1,                0.1,      0.2,      0.2,               0.2,         0.2};

			boost::random::discrete_distribution<> discrete_nd(tt_stats_weights);
			boost::variate_generator<boost::mt19937&,
						             boost::random::discrete_distribution<> > var_disc(rng, discrete_nd);

			const uint32_t avg_rate = avgr;
			const uint32_t std_rate = 50;
			const uint32_t min_rate = 10;
			const uint32_t max_rate = avg_rate * 2;

			boost::normal_distribution<> normal_nd(avg_rate, std_rate);
			boost::variate_generator<boost::mt19937&,
						 boost::normal_distribution<> > var_nor(rng, normal_nd);

			auto tenant_desc = getByTT(tt_types[var_disc()]);

			auto rate = std::max(min_rate, std::min(max_rate, static_cast<uint32_t>(var_nor())));

			auto tenant = TenantGenerator::ptr(new TenantGenerator(rate, tenant_desc.first, tenant_desc.second));

			return tenant;
		};

		static OFPT_WEIGHTS_PAIR_T getByTT(TTYPES type) {

			std::vector<OFPT::OFPT_T> msg_types;
			std::vector<float> msg_weights;

			switch(type){
				case TT_MISUSE_FEATURE_REQ:
				{
					msg_types   = {OFPT::FEATURE_REQUEST, OFPT::ECHO_REQUEST, OFPT::STATS_REQUEST_PORT, OFPT::STATS_REQUEST_FLOW, OFPT::PACKET_OUT_UDP, OFPT::FLOW_MOD};
					msg_weights =                   {0.5,                0.1,                      0.2,                      0.1,                  0.1,            0.1};
				}
				break;
				case TT_MISUSE_ECHO_REQ:
				{
					msg_types   = {OFPT::FEATURE_REQUEST, OFPT::ECHO_REQUEST, OFPT::STATS_REQUEST_PORT, OFPT::STATS_REQUEST_FLOW, OFPT::PACKET_OUT_UDP, OFPT::FLOW_MOD};
					msg_weights =                   {0.1,                0.3,                      0.4,                      0.1,                  0.1,            0.1};
				}
				break;
				case TT_EQUAL:
				{
					msg_types   = {OFPT::FEATURE_REQUEST, OFPT::ECHO_REQUEST, OFPT::STATS_REQUEST_PORT, OFPT::STATS_REQUEST_FLOW, OFPT::PACKET_OUT_UDP, OFPT::FLOW_MOD};
					msg_weights =                   {0.3,                0.3,                      0.2,                      0.1,                  0.1,            0.1};
				}
				break;
				case TT_STATS:
				{
					msg_types   = {OFPT::FEATURE_REQUEST, OFPT::ECHO_REQUEST, OFPT::STATS_REQUEST_PORT, OFPT::STATS_REQUEST_FLOW, OFPT::PACKET_OUT_UDP, OFPT::FLOW_MOD};
					msg_weights =                  {0.05,               0.04,                     0.45,                     0.45,                  0.1,            0.1};
				}
				break;
				case TT_PACKET_OUT_UDP:
				{
					msg_types   = {OFPT::FEATURE_REQUEST, OFPT::ECHO_REQUEST, OFPT::STATS_REQUEST_PORT, OFPT::STATS_REQUEST_FLOW, OFPT::PACKET_OUT_UDP, OFPT::FLOW_MOD};
					msg_weights =                  {   0,                  0,                        0,                        0,                    1,            0.};
				}
				break;
				case TT_FLOW_MOD:
				{
					msg_types   = {OFPT::FEATURE_REQUEST, OFPT::ECHO_REQUEST, OFPT::STATS_REQUEST_PORT, OFPT::STATS_REQUEST_FLOW, OFPT::PACKET_OUT_UDP, OFPT::FLOW_MOD};
					msg_weights =                  { 0.1,                0.1,                      0.1,                      0.1,                  0.1,            0.5};
				}
				break;
				default:
					BOOST_ASSERT(false);
			}

			return OFPT_WEIGHTS_PAIR_T(msg_types, msg_weights);
		}
};
*/
#endif /* TENANTGENERATOR_HPP_ */