/**
 *  This file is part of dvo.
 *
 *  Copyright 2012 Christian Kerl <christian.kerl@in.tum.de> (Technical University of Munich)
 *  For more information see <http://vision.in.tum.de/data/software/dvo>.
 *
 *  dvo 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.
 *
 *  dvo 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 dvo.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <dvo/dense_tracking.h>

#include <assert.h>
#include <sophus/se3.h>

#include <Eigen/Core>
#include <Eigen/Eigenvalues>

#include <dvo/core/datatypes.h>
#include <dvo/util/revertable.h>
#include <dvo/util/stopwatch.h>
#include <dvo/visualization/visualizer.h>

#include <tbb/task_scheduler_init.h>
#include <tbb/parallel_reduce.h>
#include <tbb/blocked_range.h>

namespace dvo
{

using namespace dvo::core;
using namespace dvo::util;

static inline bool isfinite(const float& v)
{
  return std::isfinite(v);
}

const DenseTracker::Config& DenseTracker::getDefaultConfig()
{
  static Config defaultConfig;

  return defaultConfig;
}

static const Eigen::IOFormat YamlArrayFmt(Eigen::FullPrecision, Eigen::DontAlignCols, ",", ",", "", "", "[", "]");

DenseTracker::DenseTracker(IntrinsicMatrix& intrinsics, const Config& cfg) :
    cfg(cfg),
    weight_calculation_(),
    itctx_(cfg)
{
  intrinsics_.push_back(intrinsics);

  configure();
}

void DenseTracker::configure()
{
  assert(cfg.IsSane());

  IntrinsicMatrix current = intrinsics_.back();

  for(size_t idx = intrinsics_.size(); idx <= cfg.FirstLevel; ++idx)
  {
    current.scale(0.5f);

    intrinsics_.push_back(current);
  }

  if(cfg.UseWeighting)
  {
    weight_calculation_
      .scaleEstimator(ScaleEstimators::get(cfg.ScaleEstimatorType))
      .scaleEstimator()->configure(cfg.ScaleEstimatorParam);

    weight_calculation_
      .influenceFunction(InfluenceFunctions::get(cfg.InfluenceFuntionType))
      .influenceFunction()->configure(cfg.InfluenceFunctionParam);
  }
  else
  {
    weight_calculation_
      .scaleEstimator(ScaleEstimators::get(ScaleEstimators::Unit))
      .influenceFunction(InfluenceFunctions::get(InfluenceFunctions::Unit));
  }
}

const IntrinsicMatrix& DenseTracker::intrinsics(size_t level)
{
  return intrinsics_.at(level);
}

void DenseTracker::updateLastTransform(Eigen::Affine3d& last_transformation)
{
  last_xi_ = Sophus::SE3(last_transformation.rotation(), last_transformation.translation());
}

void DenseTracker::getCovarianceEstimate(Eigen::Matrix<double, 6, 6>& covariance) const
{
  covariance = last_a_.cast<double>().lu().inverse();
}

bool DenseTracker::match(RgbdImagePyramid& reference, RgbdImagePyramid& current, Eigen::Affine3d& transformation)
{
  reference.compute(cfg.getNumLevels());
  current.compute(cfg.getNumLevels());

  bool success = true;

  if(!cfg.UseInitialEstimate)
  {
    transformation.setIdentity();
  }

  // our first increment is the given guess
  Sophus::SE3 inc(transformation.rotation(), transformation.translation());

  Revertable<Sophus::SE3> old(last_xi_);
  Revertable<Sophus::SE3> initial(inc);
  Revertable<AffineTransform> estimate(AffineTransform::Identity());

  bool accept = true;

  static stopwatch_collection sw_level(5, "l", 100);
  static stopwatch_collection sw_it(5, "it@l", 500);

  for(itctx_.Level = cfg.FirstLevel; itctx_.Level >= cfg.LastLevel; --itctx_.Level)
  {
    // reset error after every pyramid level? yes because errors from different levels are not comparable
    itctx_.Iteration = 0;
    itctx_.Error = 1;

    RgbdImage& ref = reference.level(itctx_.Level);
    RgbdImage& cur = current.level(itctx_.Level);
    const IntrinsicMatrix& intrinsics = intrinsics_[itctx_.Level];

    NormalEquationsLeastSquares ls;
    Vector6 x;

    Vector6 last_x;
    sw_level[itctx_.Level].start();
    do
    {
      sw_it[itctx_.Level].start();
      estimate.update() = inc.matrix().cast<NumType>() * estimate().matrix();

      if(cfg.UseTemporalSmoothing())
      {
        old.update() = inc.inverse() * old();
      }

      if(cfg.UseEstimateSmoothing())
      {
        initial.update() = inc.inverse() * initial();
      }

      computeLeastSquaresEquationsInverseCompositional(ref, cur, intrinsics, estimate(), ls);

      itctx_.LastError = itctx_.Error;
      itctx_.Error = ls.error + 0.5 * cfg.Lambda * old().log().squaredNorm() + 0.5 * cfg.Mu * initial().log().squaredNorm();

      // accept the last increment?
      accept = itctx_.ErrorDiff() > 0.0;

      //ROS_DEBUG_STREAM_COND(!accept, itctx_);

      // if we get a worse result, we revert the increment and try our luck on the next pyramid level
      if(!accept)
      {
        old.revert();
        initial.revert();
        estimate.revert();
        inc = Sophus::SE3::exp(Vector6::Zero().cast<double>());

        break;
      }

      // calculate new increment
      Matrix6x6 A_diagonal = Matrix6x6::Identity();

      if(cfg.UseTemporalSmoothing())
      {
        ls.A += cfg.Lambda * A_diagonal;
        ls.b += cfg.Lambda * old().log().cast<NumType>();
      }

      if(cfg.UseEstimateSmoothing())
      {
        ls.A += cfg.Mu * A_diagonal;
        ls.b += cfg.Mu * initial().log().cast<NumType>();
      }

      // first estimate rotation on lowest level
      //if(itctx_.IsFirstLevel())
      //{
      //  Eigen::Vector3f rot = ls.A.bottomRightCorner(3, 3).ldlt().solve(ls.b.tail(3));
      //  x.setZero();
      //  x.tail<3>() = rot;
      //}
      //else
      //{
      //  ls.solve(x);
      //}

      ls.solve(x);
      if(itctx_.IsLastLevel())
      {
        // TODO: should only be used if we also accept the solution
        last_a_ = ls.A;
      }

      //ROS_DEBUG_STREAM_COND(accept, itctx_ << ", Increment: " << x.format(YamlArrayFmt));

      inc = Sophus::SE3::exp(x.cast<double>());

      itctx_.Iteration++;
      itctx_.NumConstraints = ls.num_constraints;

      sw_it[itctx_.Level].stopAndPrint();
    }
    while(accept && itctx_.ErrorDiff() > cfg.Precision && !itctx_.IterationsExceeded());

    sw_level[itctx_.Level].stopAndPrint();
  }

  // if last increment wasn't incorporated because of iterations exceeded, incorporate it
  if(itctx_.IterationsExceeded())
  {
    estimate.update() = inc.matrix().cast<NumType>() * estimate().matrix();
  }

  // log reason for termination on last level
  //ROS_DEBUG_STREAM_COND_NAMED(!itctx_.IterationsExceeded() && itctx_.ErrorDiff() > 0.0 && itctx_.ErrorDiff() <= cfg.Precision, "abort", "error_precision");
  //ROS_DEBUG_STREAM_COND_NAMED(!itctx_.IterationsExceeded() && itctx_.ErrorDiff() < 0.0, "abort", "error_increase");
  //ROS_DEBUG_STREAM_COND_NAMED(itctx_.IterationsExceeded(), "abort", "iteration_exceeded");

  if(success)
  {
    last_xi_ = Sophus::SE3(estimate().rotation().cast<double>(), estimate().translation().cast<double>());
  }

  transformation = estimate().inverse().cast<double>();

  return success;
}

void DenseTracker::computeLeastSquaresEquationsForwardAdditive(dvo::core::RgbdImage& ref, dvo::core::RgbdImage& cur, const dvo::core::IntrinsicMatrix& intrinsics, const dvo::core::AffineTransform& transformation, dvo::core::LeastSquaresInterface& ls)
{
  RgbdImage cur_warped;
  RgbdImage::PointCloud ref_transformed;
  cv::Mat residuals, cur_dx, cur_dy;

  ref.buildPointCloud(intrinsics);
  cur.warpIntensitySse(transformation, ref.pointcloud, intrinsics, cur_warped, ref_transformed);

  // compute I_{2,x} and I_{2,y}
  cur_warped.calculateIntensityDerivatives();
  cur_dx = cur_warped.intensity_dx * intrinsics.fx();
  cur_dy = cur_warped.intensity_dy * intrinsics.fy();

  // compute residuals
  residuals = cur_warped.intensity - ref.intensity;

  computeLeastSquaresEquationsGeneric(residuals, cur_dx, cur_dy, ref_transformed, ls);
}

void DenseTracker::computeLeastSquaresEquationsForwardCompositional(dvo::core::RgbdImage& ref, dvo::core::RgbdImage& cur, const dvo::core::IntrinsicMatrix& intrinsics, const dvo::core::AffineTransform& transformation, dvo::core::LeastSquaresInterface& ls)
{
  RgbdImage cur_warped;
  cv::Mat residuals, cur_dx, cur_dy;

  ref.buildPointCloud(intrinsics);
  cur.warpIntensitySse(transformation, ref.pointcloud, intrinsics, cur_warped);

  // compute I_{2,x} and I_{2,y}
  cur_warped.calculateIntensityDerivatives();
  cur_dx = cur_warped.intensity_dx * intrinsics.fx();
  cur_dy = cur_warped.intensity_dy * intrinsics.fy();

  // compute residuals
  residuals = cur_warped.intensity - ref.intensity;

  computeLeastSquaresEquationsGeneric(residuals, cur_dx, cur_dy, ref.pointcloud, ls);
}

void DenseTracker::computeLeastSquaresEquationsForwardCompositionalESM(dvo::core::RgbdImage& ref, dvo::core::RgbdImage& cur, const dvo::core::IntrinsicMatrix& intrinsics, const dvo::core::AffineTransform& transformation, dvo::core::LeastSquaresInterface& ls)
{
  //static stopwatch_collection sw_warp(5, "warp@l"), sw_eqs(5, "eqs@l"), sw_total(5, "total@l");
  //sw_total[itctx_.Level].start();
  RgbdImage cur_warped;
  cv::Mat residuals, dx, dy;

  ref.buildPointCloud(intrinsics);
  //sw_warp[itctx_.Level].start();
  cur.warpIntensitySse(transformation, ref.pointcloud, intrinsics, cur_warped);
  //sw_warp[itctx_.Level].stopAndPrint();

  // compute I_{1,x} and I_{1,y}
  ref.calculateIntensityDerivatives();
  // compute I_{2,x} and I_{2,y}
  cur_warped.calculateIntensityDerivatives();

  dx = (ref.intensity_dx + cur_warped.intensity_dx) * 0.5f * intrinsics.fx();
  dy = (ref.intensity_dy + cur_warped.intensity_dy) * 0.5f * intrinsics.fy();

  // compute residuals
  residuals = cur_warped.intensity - ref.intensity;

  //sw_eqs[itctx_.Level].start();
  computeLeastSquaresEquationsGeneric(residuals, dx, dy, ref.pointcloud, ls);
  //sw_eqs[itctx_.Level].stopAndPrint();
  //sw_total[itctx_.Level].stopAndPrint();
}

void DenseTracker::computeLeastSquaresEquationsInverseCompositional(dvo::core::RgbdImage& ref, dvo::core::RgbdImage& cur, const dvo::core::IntrinsicMatrix& intrinsics, const dvo::core::AffineTransform& transformation, dvo::core::LeastSquaresInterface& ls)
{
  RgbdImage cur_warped;
  cv::Mat residuals, ref_dx, ref_dy;

  ref.buildPointCloud(intrinsics);
  cur.warpIntensitySse(transformation, ref.pointcloud, intrinsics, cur_warped);

  // compute I_{1,x} and I_{1,y}
  ref.calculateIntensityDerivatives();
  ref_dx = ref.intensity_dx * intrinsics.fx();
  ref_dy = ref.intensity_dy * intrinsics.fy();

  // compute residuals
  residuals = cur_warped.intensity - ref.intensity;

  computeLeastSquaresEquationsGeneric(residuals, ref_dx, ref_dy, ref.pointcloud, ls);
}

struct LeastSquaresEquationsReduction
{
  dvo::core::NormalEquationsLeastSquares* ls;
  const cv::Mat& residuals/*, weights*/, Jix, Jiy;
  const dvo::core::RgbdImage::PointCloud& points;
  const dvo::core::WeightCalculation& weights;
  LeastSquaresEquationsReduction(const cv::Mat& residuals,const dvo::core::WeightCalculation& weights, const cv::Mat& Jix, const cv::Mat& Jiy, const dvo::core::RgbdImage::PointCloud& points) :
    ls(new dvo::core::NormalEquationsLeastSquares()),
    residuals(residuals),
    weights(weights),
    Jix(Jix),
    Jiy(Jiy),
    points(points)
  {
    ls->initialize(1);
  }

  LeastSquaresEquationsReduction(LeastSquaresEquationsReduction& other, tbb::split) :
    ls(new dvo::core::NormalEquationsLeastSquares()),
    residuals(other.residuals),
    weights(other.weights),
    Jix(other.Jix),
    Jiy(other.Jiy),
    points(other.points)
  {
    ls->initialize(1);
  }

  ~LeastSquaresEquationsReduction()
  {
    delete ls;
  }

  void operator()(const tbb::blocked_range<size_t>& r)
  {
    const float *jix_ptr = Jix.ptr<float>() + r.begin(), *jiy_ptr = Jiy.ptr<float>() + r.begin(), *residual_ptr = residuals.ptr<float>() + r.begin();//, *weights_ptr = weights.ptr<float>() + r.begin();
    Matrix1x2 Ji;
    Matrix2x6 Jw;

    dvo::core::NormalEquationsLeastSquares* tmp = ls;//new dvo::core::NormalEquationsLeastSquares();

    for (size_t idx = r.begin(); idx != r.end(); ++idx, ++jix_ptr, ++jiy_ptr, ++residual_ptr)//, ++weights_ptr)
    {
      if(!isfinite(*jix_ptr) || !isfinite(*jiy_ptr) || !isfinite(*residual_ptr)) continue;

      Ji << *jix_ptr, *jiy_ptr;

      const Vector4& p3d = points.col(idx).cast<NumType>();
      DenseTracker::computeJacobianOfProjectionAndTransformation(p3d, Jw);

      tmp->update(Ji * Jw, *residual_ptr, weights.calculateWeight(*residual_ptr));
    }

    ls = tmp;
  }

  void join(LeastSquaresEquationsReduction& other)
  {
    ls->combine(*other.ls);
  }
};

inline void DenseTracker::computeLeastSquaresEquationsGeneric(const cv::Mat& residuals, const cv::Mat& Jix, const cv::Mat& Jiy, const dvo::core::RgbdImage::PointCloud& points, dvo::core::LeastSquaresInterface& ls)
{
  //static stopwatch_collection sw_weights(5, "weights@l"), sw_reduce(5, "reduce@l");

  cv::Mat weights;

  // compute weights
  //sw_weights[itctx_.Level].start();
  computeWeights(residuals, weights);
  //sw_weights[itctx_.Level].stopAndPrint();
  LeastSquaresEquationsReduction body(residuals, weight_calculation_, Jix, Jiy, points);

  //sw_reduce[itctx_.Level].start();
  tbb::parallel_reduce(tbb::blocked_range<size_t>(0, residuals.size().area()), body);
  //sw_reduce[itctx_.Level].stopAndPrint();

  NormalEquationsLeastSquares& xls = (NormalEquationsLeastSquares&)ls;
  xls = *body.ls;
  xls.finish();
}

inline void DenseTracker::computeWeights(const cv::Mat& residuals, cv::Mat& weights)
{
  if(itctx_.IsFirstLevel() || itctx_.IsFirstIterationOnLevel())
  {
    weight_calculation_.calculateScale(residuals);
  }
}

// jacobian computation
inline void DenseTracker::computeJacobianOfProjectionAndTransformation(const Vector4& p, Matrix2x6& j)
{
  NumType z = 1.0f / p(2);
  NumType z_sqr = 1.0f / (p(2) * p(2));

  j(0, 0) =  z;
  j(0, 1) =  0.0f;
  j(0, 2) = -p(0) * z_sqr;
  j(0, 3) = j(0, 2) * p(1);//j(0, 3) = -p(0) * p(1) * z_sqr;
  j(0, 4) = 1.0f - j(0, 2) * p(0);//j(0, 4) =  (1.0 + p(0) * p(0) * z_sqr);
  j(0, 5) = -p(1) * z;

  j(1, 0) =  0.0f;
  j(1, 1) =  z;
  j(1, 2) = -p(1) * z_sqr;
  j(1, 3) = -1.0f + j(1, 2) * p(1); //j(1, 3) = -(1.0 + p(1) * p(1) * z_sqr);
  j(1, 4) = -j(0, 3); //j(1, 4) =  p(0) * p(1) * z_sqr;
  j(1, 5) =  p(0) * z;
}

void DenseTracker::compute3rdRowOfJacobianOfTransformation(Vector4& p, Vector6& j)
{
  j(0) = 0.0;
  j(1) = 0.0;
  j(2) = 1.0;
  j(3) = p(1);
  j(4) = -p(0);
  j(5) = 0.0;
}

} /* namespace dvo */


/*
  Tencent is pleased to support the open source community by making
  Plato available.
  Copyright (C) 2019 THL A29 Limited, a Tencent company.
  All rights reserved.

  Licensed under the BSD 3-Clause License (the "License"); you may
  not use this file except in compliance with the License. You may
  obtain a copy of the License at

  https://opensource.org/licenses/BSD-3-Clause

  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.

  See the AUTHORS file for names of contributors.
*/

#ifndef __PLATO_ALGO_FAST_UNFOLDING_LOUVAIN_DENSITY_HPP__
#define __PLATO_ALGO_FAST_UNFOLDING_LOUVAIN_DENSITY_HPP__

#include <cstdint>
#include <cstdlib>
#include <unordered_map>
#include <algorithm>
#include <utility>
#include <vector>
#include <cmath>
#include <unordered_set>
#include <boost/format.hpp>
#include <boost/lockfree/queue.hpp>

#include "glog/logging.h"

#include "plato/util/perf.hpp"
#include "plato/util/atomic.hpp"
#include "plato/graph/graph.hpp"
#include "plato/engine/dualmode.hpp"
#include "plato/algo/fast_unfolding/louvain.hpp"

namespace plato { namespace algo {

template<typename GRAPH>
class louvain_density_epoch_t {
public:
  using partition_t = typename GRAPH::partition_t;
  using edge_value_t = typename GRAPH::edata_t;
  using louvain_value_t = plato::dense_state_t<edge_value_t, partition_t>;
  using adj_unit_list_spec_t = typename GRAPH::adj_unit_list_spec_t;
  struct epoch_msg_type_t {
    vid_t v_i;
    vid_t from;
    vid_t to;
    edge_value_t ki;
    edge_value_t ki_in_from;
    edge_value_t ki_in_to;
    uint32_t in_from_edges;
    uint32_t in_to_edges;
  };

  struct sync_val_msg_type_t{
    vid_t v_i;
    edge_value_t val;
    uint32_t edges;
    edge_value_t in_sum;
  };

public:
  /**
   * @brief
   * @param graph
   * @param graph_info
   * @param m
   * @param opts
   */
  explicit louvain_density_epoch_t(
    std::shared_ptr<GRAPH> graph, const graph_info_t& graph_info, double m,
    const louvain_opts_t& opts = louvain_opts_t());

  /**
   * @brief
   */
  ~louvain_density_epoch_t();

  /**
   * @brief
   */
  void compute();

  /**
   * @brief getter
   * @return
   */
  std::shared_ptr<GRAPH> graph() { return graph_; };

  /**
   * @brief getter
   * @return
   */
  std::vector<vid_t>& labels() { return labels_; };

private:
  std::shared_ptr<GRAPH> graph_;
  graph_info_t graph_info_;
  louvain_value_t ki_;
  std::vector<vid_t> labels_;
  std::vector<edge_value_t> sigma_tot_;
  bitmap_t<> local_bit_;
  double m_;
  std::vector<edge_value_t> sigma_in_;
  std::vector<uint32_t>  c_vertices_;
  std::vector<uint32_t>  c_edges_;
  louvain_opts_t opts_;
};

template <typename GRAPH>
louvain_density_epoch_t<GRAPH>::louvain_density_epoch_t(
  std::shared_ptr<GRAPH> graph,
  const graph_info_t& graph_info, double m, const louvain_opts_t& opts) :
  graph_(graph), graph_info_(graph_info), ki_(graph_info.max_v_i_, graph->partitioner()),
  labels_(graph_info.max_v_i_ + 1), sigma_tot_(graph_info.max_v_i_ + 1, 0),
  local_bit_(graph_info.max_v_i_ + 1), m_(2 * m), sigma_in_(graph_info.max_v_i_ + 1, 0),
  c_vertices_(graph_info.max_v_i_ + 1, 1),c_edges_(graph_info.max_v_i_ + 1, 0),opts_(opts) {

  auto& cluster_info = plato::cluster_info_t::get_instance();
  plato::stop_watch_t watch;

  {// init labels
    watch.mark("t0");
#pragma omp parallel for num_threads(cluster_info.threads_)
    for (vid_t v_i = 0; v_i <= graph_info.max_v_i_; ++v_i) {
      labels_[v_i] = v_i;
    }
    LOG(INFO) << "epoch init labels: " << watch.show("t0") / 1000.0;
  }

  {//calc ki_ and sync sigma_tot_
    int cnt =0;
    bitmap_t<> active_all(graph_info.max_v_i_ + 1);
    active_all.fill();
    auto active_view_all = plato::create_active_v_view(graph->partitioner()->self_v_view(), active_all);
    watch.mark("t0");
    edge_value_t max_ki = 0;
    using push_context_t = plato::template mepa_bc_context_t<sync_val_msg_type_t>;
    plato::broadcast_message<sync_val_msg_type_t, vid_t>(
      active_view_all,
      [&](const push_context_t& context, vid_t v_i) {
        edge_value_t local_sum = 0;
        edge_value_t self_sum =0;
        uint32_t self_num =0;
        auto neighbours = graph->neighbours(v_i);
        for (auto it = neighbours.begin_; neighbours.end_ != it; ++it) {
          local_sum += it->edata_;
          if(it->neighbour_ == v_i) {
            self_num++;
            self_sum += it->edata_;
          }
        }
        if (neighbours.begin_ != neighbours.end_) {
          ki_[v_i] = local_sum;
          local_bit_.set_bit(v_i);
          plato::write_max(&max_ki, local_sum);
          context.send(sync_val_msg_type_t{ v_i, local_sum, self_num, self_sum});
        }
      },
      [&](int p_i, sync_val_msg_type_t& msg) {
        sigma_tot_[msg.v_i] = msg.val;
        sigma_in_[msg.v_i]  = msg.in_sum;
        c_edges_[msg.v_i]   = msg.edges;
        return 0;
      }
    );
    LOG(INFO) << "epoch sync sigma_tot" << watch.show("t0") / 1000.0;
    MPI_Allreduce(MPI_IN_PLACE, &max_ki, 1, get_mpi_data_type<edge_value_t>(), MPI_MAX, MPI_COMM_WORLD);
    LOG(INFO) << "max-ki is: " << max_ki;
  }
}

template <typename GRAPH>
louvain_density_epoch_t<GRAPH>::~louvain_density_epoch_t() {
}

template <typename GRAPH>
void louvain_density_epoch_t<GRAPH>::compute() {
  auto& cluster_info = plato::cluster_info_t::get_instance();
  uint32_t graph_vertices= graph_info_.vertices_;
  double p_all = 1.0;
  if(graph_vertices > 1) { p_all = graph_info_.edges_*2.0 / ((graph_vertices - 1) * graph_vertices); }

  auto try_change =
    [&](vid_t v_i, vid_t from, vid_t to, edge_value_t ki_in_from, edge_value_t ki_in_to, int32_t in_from_edges,  int32_t in_to_edges) {
      //p = 2mc / nc(nc -1) - M*2 / N(N - 1)
      //Q = [p*in/2m + p^2 * tot^2 / 4m^2 ]

      double p_from = 1.0 - p_all, p_to = 1.0 - p_all, p_from_change = 1.0 - p_all, p_to_change = 1.0 - p_all;
      if(c_vertices_[from] > 1) {
        p_from = c_edges_[from]*2.0 / (c_vertices_[from] * (c_vertices_[from] - 1)) - p_all;
        if(c_vertices_[from] > 2) { p_from_change= (c_edges_[from] - in_from_edges) * 2.0 / ((c_vertices_[from] - 1) * (c_vertices_[from] - 2)) - p_all;}
      }
      if(c_vertices_[to] > 1) {
        p_to = c_edges_[to] * 2.0 / (c_vertices_[to] * (c_vertices_[to] - 1)) - p_all;
      }
      p_to_change = (c_edges_[to] + in_to_edges) * 2.0 / (c_vertices_[to] * (c_vertices_[to] + 1)) - p_all;

      double x1 = (p_from_change - p_from) * sigma_in_[from] - p_from_change * ki_in_from + (p_to_change - p_to) * sigma_in_[to] + p_to_change * ki_in_to;

      p_from_change *= p_from_change;
      p_from *= p_from;
      p_to_change  *= p_to_change;
      p_to *= p_to;

      double x2 = p_from_change * (sigma_tot_[from] * sigma_tot_[from] - 2 * ki_[v_i] * sigma_tot_[from] + ki_[v_i] * ki_[v_i]) - p_from * sigma_tot_[from] * sigma_tot_[from];
      x2 += p_to_change * (sigma_tot_[to] * sigma_tot_[to] + 2 * ki_[v_i] * sigma_tot_[to] + ki_[v_i] * ki_[v_i]) - p_to * sigma_tot_[to] * sigma_tot_[to];
      return x1 - x2 / m_ ;
    };

  auto do_change =
    [&](epoch_msg_type_t& msg) {
      labels_[msg.v_i] = msg.to;
      plato::write_add(&sigma_tot_[msg.from], -msg.ki);
      plato::write_add(&sigma_tot_[msg.to], msg.ki);

      plato::write_add(&sigma_in_[msg.from], -msg.ki_in_from);
      plato::write_add(&sigma_in_[msg.to], msg.ki_in_to);
      plato::write_add(&c_vertices_[msg.from], (uint32_t)-1);
      plato::write_add(&c_vertices_[msg.to], (uint32_t)1);
      plato::write_add(&c_edges_[msg.from],-msg.in_from_edges);
      plato::write_add(&c_edges_[msg.to],msg.in_to_edges);
    };

  auto active_view = plato::create_active_v_view(graph_->partitioner()->self_v_view(), local_bit_);
  plato::stop_watch_t watch;
  using push_context_t = plato::template mepa_bc_context_t<epoch_msg_type_t>;
  for (int try_time = 0; try_time < opts_.inner_iteration_; ++try_time){
    watch.mark("t0");
    auto exec_once =
      [&](std::function<bool(vid_t, vid_t)> condition) {
        plato::broadcast_message<epoch_msg_type_t, vid_t>(
          active_view,
          [&](const push_context_t& context, plato::vid_t v_i) {
            vid_t target = (vid_t)-1;
            vid_t from = labels_[v_i];
            edge_value_t ki_in_from = 0;
            std::unordered_map<vid_t, edge_value_t> ki_in_map;
            std::unordered_map<vid_t, uint32_t>  ki_in_edges;
            auto neighbours = graph_->neighbours(v_i);
            edge_value_t self_cycle = 0;
            uint32_t self_cycle_edges = 0;
            for (auto it = neighbours.begin_; neighbours.end_ != it; ++it) {
              vid_t to = labels_[it->neighbour_];
              if (condition(from, to)) {
                ki_in_map[to] += it->edata_;
                ki_in_edges[to]++;
              } else if (to == from) {
                ki_in_from += it->edata_;
                ki_in_edges[from]++;
              }
              if (it->neighbour_ == v_i) {
                self_cycle_edges++;
                self_cycle = it->edata_;
              }
            }
            edge_value_t cur_kto,cur_kfrom;
            double best_delta = 0;
            for (auto& it: ki_in_map) {
              double delta = try_change(v_i, from, it.first, ki_in_from, it.second + self_cycle, ki_in_edges[from], ki_in_edges[it.first] + self_cycle_edges);
              if (delta > best_delta) {
                target = it.first;
                best_delta = delta;
                cur_kfrom = ki_in_from;
                cur_kto   = it.second + self_cycle;
              }
            }

            if (target != (vid_t)-1) {

              epoch_msg_type_t msg = epoch_msg_type_t{ v_i, from, target, ki_[v_i],cur_kfrom,cur_kto,ki_in_edges[from],ki_in_edges[target] + self_cycle_edges};
              context.send(msg);
              do_change(msg);
            }
          },
          [&](int p_i, epoch_msg_type_t& msg) {
            if (p_i != cluster_info.partition_id_) {
              do_change(msg);
            }
            return 0;
          }
        );
      };

    exec_once(
      [&](vid_t from, vid_t to){
      if (from < to) return true;
      return false;
    });
    exec_once(
      [&](vid_t from, vid_t to){
      if (from > to) return true;
      return false;
    });
    LOG(INFO) << "try_time: "  << try_time << " cost: " << watch.show("t0") / 1000.0;
  }
}

template<typename GRAPH>
class louvain_density_fast_unfolding_t {
public:
  using partition_t = typename GRAPH::partition_t;
  using edge_value_t = typename GRAPH::edata_t;
  using adj_unit_list_spec_t = plato::adj_unit_list_t<edge_value_t>;
  using louvain_value_t = plato::dense_state_t<vid_t, partition_t>;

  struct edge_sync_msg_type_t {
    vid_t src;
    vid_t dst;
    edge_value_t data;
  };

  struct degree_sync_msg_type_t {
    vid_t src;
    vid_t degree;
  };

public:
  /**
   * @brief
   * @param graph
   * @param graph_info
   * @param opts
   */
  explicit louvain_density_fast_unfolding_t(
    std::shared_ptr<GRAPH> graph, const graph_info_t& graph_info,
    const louvain_opts_t& opts = louvain_opts_t());

  ~louvain_density_fast_unfolding_t();

  /**
   * @brief
   */
  void compute();

  /**
   * @brief
   * @tparam Callback
   * @param callback
   */
  template <typename Callback>
  void save(Callback&& callback);

private:
  /**
   * @brief
   * @param graph
   * @param labels
   * @return
   */
  std::shared_ptr<GRAPH> rebuild(std::shared_ptr<GRAPH> graph, std::vector<vid_t>& labels);

  /**
   * @brief
   * @param labels
   */
  void update_local_label(std::vector<vid_t>& labels);
private:
  std::shared_ptr<GRAPH> graph_;
  graph_info_t graph_info_;
  graph_info_t cur_graph_info_;
  louvain_value_t local_label_;
  louvain_value_t local_comm_size_;
  louvain_opts_t opts_;
};

template<typename GRAPH>
louvain_density_fast_unfolding_t<GRAPH>::louvain_density_fast_unfolding_t(
  std::shared_ptr<GRAPH> graph, const graph_info_t& graph_info,
  const louvain_opts_t& opts)
  : graph_(graph), graph_info_(graph_info), cur_graph_info_(graph_info),
    local_label_(graph_info.max_v_i_, graph->partitioner()),
    local_comm_size_(graph_info.max_v_i_, graph->partitioner()), opts_(opts) {
}

template<typename GRAPH>
louvain_density_fast_unfolding_t<GRAPH>::~louvain_density_fast_unfolding_t() {
}

template<typename GRAPH>
void louvain_density_fast_unfolding_t<GRAPH>::compute() {
  //first calc m and init label
  plato::stop_watch_t watch;
  watch.mark("t0");
  double m = local_label_.template foreach<double> (
    [&](vid_t v_i, vid_t* pval) {
      *pval = v_i;
      double local_sum = 0;
      auto neighbours = graph_->neighbours(v_i);
      for (auto it = neighbours.begin_; neighbours.end_ != it; ++it) {
        local_sum += it->edata_;
      }
      return local_sum;
    }
  );
  m /= 2; //undirected
  LOG(INFO) << "calc m: " << m << " cost: " << watch.show("t0") / 1000.0;

  //epoch
  louvain_density_epoch_t<GRAPH>* cur = new louvain_density_epoch_t<GRAPH>(graph_, cur_graph_info_, m, opts_);
  for (int epoch = 0; epoch < opts_.outer_iteration_; epoch++) {
    LOG(INFO) << "epoch " << epoch << " begin!";
    watch.mark("compute");
    cur->compute();
    LOG(INFO) << "compute cost: " << watch.show("compute") / 1000.0;
    update_local_label(cur->labels());
    if (epoch == opts_.outer_iteration_ - 1) {
      delete cur;
      break;
    }
    //rebuild from current graph
    watch.mark("rebuild");
    graph_info_t graph_info_next(cur_graph_info_);
    graph_info_next.is_directed_ = true;
    auto graph_next = rebuild(cur->graph(), cur->labels());
    louvain_density_epoch_t<GRAPH>* nxt = new louvain_density_epoch_t<GRAPH>(graph_next, graph_info_next, m, opts_);
    LOG(INFO) << "rebuild cost: " << watch.show("rebuild") / 1000.0;
    delete cur;
    cur = nxt;
  }
}

template<typename GRAPH>
std::shared_ptr<GRAPH> louvain_density_fast_unfolding_t<GRAPH>::rebuild(std::shared_ptr<GRAPH> graph, std::vector<vid_t>& labels) {
  plato::stop_watch_t watch;
  watch.mark("t0");
  eid_t total_edge = local_label_.template foreach<eid_t> (
    [&](vid_t v_i, vid_t* pval) {
      auto neighbours = graph->neighbours(v_i);
      return (eid_t)(neighbours.end_ - neighbours.begin_);
    }
  );
  LOG(INFO) << "rebuild before total edge: " << total_edge << " cost: " << watch.show("t0") / 1000.0;

  watch.mark("t0");
  auto& cluster_info = plato::cluster_info_t::get_instance();
  vid_t v_begin = graph->partitioner()->offset_[cluster_info.partition_id_];
  vid_t v_end = graph->partitioner()->offset_[cluster_info.partition_id_ + 1];
  std::vector<eid_t> edge_idx(v_end - v_begin + 1, 0);
  std::vector<eid_t> tmp_idx(v_end - v_begin + 1, 0);

  bitmap_t<> active_all(graph_info_.max_v_i_ + 1);
  active_all.fill();
  auto active_view_all = plato::create_active_v_view(graph->partitioner()->self_v_view(), active_all);

  {
    //first, calc degree
    using push_context_t = plato::template mepa_sd_context_t<degree_sync_msg_type_t>;
    plato::spread_message<degree_sync_msg_type_t, vid_t>(
      active_view_all,
      [&](const push_context_t& context, vid_t v_i) {
        auto neighbours = graph->neighbours(v_i);
        if (neighbours.begin_ == neighbours.end_) return;
        vid_t src = labels[v_i];
        auto send_to = graph->partitioner()->get_partition_id(src);
        context.send(send_to, degree_sync_msg_type_t { src, (vid_t)(neighbours.end_ - neighbours.begin_) } );
      },
      [&](degree_sync_msg_type_t& msg) {
        vid_t pos = msg.src - v_begin + 1;
        plato::write_add(&edge_idx[pos], (eid_t)msg.degree);
        return 0;
      }
    );

    for (int i = 1; i < (int)edge_idx.size(); ++i) {
      edge_idx[i] = edge_idx[i - 1] + edge_idx[i];
      tmp_idx[i] = edge_idx[i];
    }

    LOG(INFO) << "rebuild calc degree cost: " << watch.show("t0") / 1000.0;
  }
  eid_t total_local_edge = edge_idx[edge_idx.size() - 1];
  std::vector<std::pair<vid_t, edge_value_t> > edges(total_local_edge);
  LOG(INFO) << "rebuild local edge: " << edges.size();
  watch.mark("t0");
  {
    //second, transfer edge
    using push_context_t = plato::template mepa_sd_context_t<edge_sync_msg_type_t>;
    plato::spread_message<edge_sync_msg_type_t, vid_t>(
      active_view_all,
      [&](const push_context_t& context, vid_t v_i) {
        auto neighbours = graph->neighbours(v_i);
        if (neighbours.begin_ == neighbours.end_) return;
        vid_t src = labels[v_i];
        auto send_to = graph->partitioner()->get_partition_id(src);
        for (auto it = neighbours.begin_; neighbours.end_ != it; ++it) {
          vid_t dst = labels[it->neighbour_];
          context.send(send_to, edge_sync_msg_type_t{ src, dst, it->edata_ });
        }
      },
      [&](edge_sync_msg_type_t& msg) {
        vid_t pos = msg.src - v_begin;
        vid_t idx = __sync_fetch_and_add(&tmp_idx[pos], (eid_t)1);
        edges[idx].first = msg.dst;
        edges[idx].second = msg.data;
        return 0;
      }
    );
    LOG(INFO) << "rebuild transfer edge cost: " << watch.show("t0") / 1000.0;
  }

  watch.mark("t0");
  bitmap_t<> v_bitmap(std::numeric_limits<vid_t>::max());
  edge_cache_t<edge_value_t> edge_cache;
  {
    //third, sort and aggregate
#pragma omp parallel for num_threads(cluster_info.threads_)
    for (vid_t v_i = v_begin; v_i < v_end; ++v_i) {
      vid_t pos = v_i - v_begin;
      eid_t e_start = edge_idx[pos];
      eid_t e_end = edge_idx[pos + 1];
      if (e_start == e_end) continue;
      std::sort(edges.begin() + e_start, edges.begin() + e_end);
      vid_t pre = (vid_t)-1;
      edge_value_t local_sum = 0;
      for (eid_t e = e_start; e < e_end; ++e) {
        if (pre != edges[e].first) {
          if (pre != (vid_t)-1) {
            v_bitmap.set_bit(v_i);v_bitmap.set_bit(pre);
            edge_cache.push_back(edge_unit_t<edge_value_t> { v_i, pre, local_sum });
          }
          pre = edges[e].first;
          local_sum = 0;
        }
        local_sum += edges[e].second;
      }
      if (pre != (vid_t)-1) {
        v_bitmap.set_bit(v_i);v_bitmap.set_bit(pre);
        edge_cache.push_back(edge_unit_t<edge_value_t> { v_i, pre, local_sum });
      }
    }

    eid_t edge_num_new = edge_cache.size();
    MPI_Allreduce(MPI_IN_PLACE, &edge_num_new, 1, get_mpi_data_type<eid_t>(), MPI_SUM, MPI_COMM_WORLD);
    LOG(INFO) << "rebuild new edge num: " << edge_num_new;
    cur_graph_info_.edges_ = edge_num_new;
    LOG(INFO) << "rebuild aggregate edge cost: " << watch.show("t0") / 1000.0;
  }

  watch.mark("t0");
  graph_info_t graph_info_next(graph_info_);
  graph_info_next.is_directed_ = true;
  std::shared_ptr<GRAPH> pgraph(new GRAPH(graph->partitioner()));
  pgraph->load_from_cache(graph_info_next, edge_cache);
  LOG(INFO) << "rebuild load cache cost is: " << watch.show("t0") / 1000.0;

  cur_graph_info_.vertices_ = v_bitmap.count();
  return pgraph;
}

template<typename GRAPH>
void louvain_density_fast_unfolding_t<GRAPH>::update_local_label(std::vector<vid_t>& labels) {
  local_label_.template foreach<int> (
    [&] (vid_t v_i, vid_t* pval) {
      if (labels[*pval] != *pval) {
        *pval = labels[*pval];
      }
      return 0;
    }
  );
}

template<typename GRAPH>
template<typename Callback>
void louvain_density_fast_unfolding_t<GRAPH>::save(Callback&& callback) {
  // traverse
  local_label_.template foreach<int> (
    [&] (vid_t v_i, vid_t* pval) {
      callback(v_i, *pval);
      return 0;
    }
  );
}

}  // namespace plato
}  // namespace algo
#endif


// SPDX-License-Identifier: NASA-1.3
/*******************************************************************************
 *
 * Implementation of bignums based on GMP, the Gnu Multiple Precision Arithmetic
 * Library (http://gmplib.org).
 *
 * Author: Arnaud J. Venet (arnaud.j.venet@nasa.gov)
 *
 * Contributors: Jorge A. Navas (jorge.navas@sri.com)
 *
 * Notices:
 *
 * Copyright (c) 2011-2014 United States Government as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All Rights Reserved.
 *
 * Disclaimers:
 *
 * No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF
 * ANY KIND, EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED
 * TO, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS,
 * ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE,
 * OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL BE
 * ERROR FREE, OR ANY WARRANTY THAT DOCUMENTATION, IF PROVIDED, WILL CONFORM TO
 * THE SUBJECT SOFTWARE. THIS AGREEMENT DOES NOT, IN ANY MANNER, CONSTITUTE AN
 * ENDORSEMENT BY GOVERNMENT AGENCY OR ANY PRIOR RECIPIENT OF ANY RESULTS,
 * RESULTING DESIGNS, HARDWARE, SOFTWARE PRODUCTS OR ANY OTHER APPLICATIONS
 * RESULTING FROM USE OF THE SUBJECT SOFTWARE.  FURTHER, GOVERNMENT AGENCY
 * DISCLAIMS ALL WARRANTIES AND LIABILITIES REGARDING THIRD-PARTY SOFTWARE,
 * IF PRESENT IN THE ORIGINAL SOFTWARE, AND DISTRIBUTES IT "AS IS."
 *
 * Waiver and Indemnity:  RECIPIENT AGREES TO WAIVE ANY AND ALL CLAIMS AGAINST
 * THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS, AS WELL
 * AS ANY PRIOR RECIPIENT.  IF RECIPIENT'S USE OF THE SUBJECT SOFTWARE RESULTS
 * IN ANY LIABILITIES, DEMANDS, DAMAGES, EXPENSES OR LOSSES ARISING FROM SUCH
 * USE, INCLUDING ANY DAMAGES FROM PRODUCTS BASED ON, OR RESULTING FROM,
 * RECIPIENT'S USE OF THE SUBJECT SOFTWARE, RECIPIENT SHALL INDEMNIFY AND HOLD
 * HARMLESS THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS,
 * AS WELL AS ANY PRIOR RECIPIENT, TO THE EXTENT PERMITTED BY LAW.
 * RECIPIENT'S SOLE REMEDY FOR ANY SUCH MATTER SHALL BE THE IMMEDIATE,
 * UNILATERAL TERMINATION OF THIS AGREEMENT.
 *
 ******************************************************************************/

#pragma once

#include <climits>
#include <iostream>
#include <sstream>
#include <string>

#include <boost/functional/hash.hpp>
#include <utility>

#include <gmpxx.h>

#include "debug.hpp"

namespace crab {

// GMP can convert directly from/to signed/unsigned long and
// signed/unsigned int. However, the C++11 standard only guarantees:
//
//  - unsigned/signed int  >= 16 bits.
//  - unsigned/signed long >= 32 bits.
//

// TODO/FIXME:
//
// We don't have a conversion from GMP numbers to 64-bit integers,
// because GMP cannot convert directly from/to int64_t or
// uint64_t. For that, we need to use mpz_export and mpz_import but
// they are significantly more expensive.
//
// Note that the actual size of **long** integer varies depending on
// the architecture and OS (see e.g.,
// https://en.cppreference.com/w/cpp/language/types). For instance,
// both Linux and mac OS on an Intel 64, the size of long integers is
// 8 bytes. But for Windows on Intel 64, the size is 4 bytes.

class z_number final {
  private:
    mpz_class _n{0};

  public:
    z_number() = default;
    explicit z_number(mpz_class n) : _n(std::move(n)) {}

    z_number(signed long long int n) : _n((signed long int)n) {
        if (n > LONG_MAX) {
            CRAB_ERROR(n, " cannot fit into a signed long int: use another mpz_class constructor");
        }
    }

    explicit z_number(const std::string& s) {
        try {
            _n = s;
        } catch (std::invalid_argument& e) {
            CRAB_ERROR("z_number: invalid string in constructor", s);
        }
    }

    // overloaded typecast operators
    explicit operator long() const {
        if (_n.fits_slong_p()) {
            return _n.get_si();
        } else {
            CRAB_ERROR("mpz_class ", _n.get_str(), " does not fit into a signed long integer");
        }
    }

    explicit operator int() const {
        if (_n.fits_sint_p()) {
            // get_si returns a signed long so we cast it to int
            return (int)_n.get_si();
        } else {
            CRAB_ERROR("mpz_class ", _n.get_str(), " does not fit into a signed integer");
        }
    }

    explicit operator mpz_class() const { return _n; }

    [[nodiscard]] std::size_t hash() const {
        boost::hash<std::string> hasher;
        return hasher(_n.get_str());
    }

    [[nodiscard]] bool fits_sint() const { return _n.fits_sint_p(); }

    [[nodiscard]] bool fits_slong() const { return _n.fits_slong_p(); }

    z_number operator+(const z_number& x) const {
        mpz_class r = _n + x._n;
        return z_number(r);
    }

    z_number operator+(int x) const { return operator+(z_number(x)); }

    z_number operator*(const z_number& x) const {
        mpz_class r = _n * x._n;
        return z_number(r);
    }

    z_number operator*(int x) const { return operator*(z_number(x)); }

    z_number operator-(const z_number& x) const {
        mpz_class r = _n - x._n;
        return z_number(r);
    }

    z_number operator-(int x) const { return operator-(z_number(x)); }

    z_number operator-() const {
        mpz_class r = -_n;
        return z_number(r);
    }

    z_number operator/(const z_number& x) const {
        if (x._n == 0) {
            CRAB_ERROR("z_number: division by zero [1]");
        } else {
            mpz_class r = _n / x._n;
            return z_number(r);
        }
    }

    z_number operator/(int x) const { return operator/(z_number(x)); }

    z_number operator%(const z_number& x) const {
        if (x._n == 0) {
            CRAB_ERROR("z_number: division by zero [2]");
        } else {
            mpz_class r = _n % x._n;
            return z_number(r);
        }
    }

    z_number operator%(int x) const { return operator%(z_number(x)); }

    z_number& operator+=(const z_number& x) {
        _n += x._n;
        return *this;
    }

    z_number& operator+=(int x) { return operator+=(z_number(x)); }

    z_number& operator*=(const z_number& x) {
        _n *= x._n;
        return *this;
    }

    z_number& operator*=(int x) { return operator*=(z_number(x)); }

    z_number& operator-=(const z_number& x) {
        _n -= x._n;
        return *this;
    }

    z_number& operator-=(int x) { return operator-=(z_number(x)); }

    z_number& operator/=(const z_number& x) {
        if (x._n == 0) {
            CRAB_ERROR("z_number: division by zero [3]");
        } else {
            _n /= x._n;
            return *this;
        }
    }

    z_number& operator/=(int x) { return operator/=(z_number(x)); }

    z_number& operator%=(const z_number& x) {
        if (x._n == 0) {
            CRAB_ERROR("z_number: division by zero [4]");
        } else {
            _n %= x._n;
            return *this;
        }
    }

    z_number& operator%=(int x) { return operator%=(z_number(x)); }

    z_number& operator--() & {
        --(_n);
        return *this;
    }

    z_number& operator++() & {
        ++(_n);
        return *this;
    }

    z_number operator++(int) & {
        z_number r(*this);
        ++(*this);
        return r;
    }

    z_number operator--(int) & {
        z_number r(*this);
        --(*this);
        return r;
    }

    bool operator==(const z_number& x) const { return _n == x._n; }
    bool operator==(int x) const { return operator==(z_number(x)); }

    bool operator!=(const z_number& x) const { return _n != x._n; }
    bool operator!=(int x) const { return operator!=(z_number(x)); }

    bool operator<(const z_number& x) const { return _n < x._n; }

    bool operator<(int x) const { return operator<(z_number(x)); }

    bool operator<=(const z_number& x) const { return _n <= x._n; }
    bool operator<=(int x) const { return operator<=(z_number(x)); }

    bool operator>(const z_number& x) const { return _n > x._n; }
    bool operator>(int x) const { return operator>(z_number(x)); }

    bool operator>=(const z_number& x) const { return _n >= x._n; }
    bool operator>=(int x) const { return operator>=(z_number(x)); }

    z_number operator&(const z_number& x) const { return z_number(_n & x._n); }
    z_number operator&(int x) const { return operator&(z_number(x)); }

    z_number operator|(const z_number& x) const { return z_number(_n | x._n); }
    z_number operator|(int x) const { return operator|(z_number(x)); }

    z_number operator^(const z_number& x) const { return z_number(_n ^ x._n); }
    z_number operator^(int x) const { return operator^(z_number(x)); }

    z_number operator<<(z_number x) const {
        mpz_t tmp;
        mpz_init(tmp);
        mpz_mul_2exp(tmp, _n.get_mpz_t(), mpz_get_ui(x._n.get_mpz_t()));
        mpz_class result(tmp);
        return z_number(result);
    }

    z_number operator<<(int x) const { return operator<<(z_number(x)); }

    z_number operator>>(z_number x) const {
        mpz_class tmp(_n);
        return z_number(tmp.operator>>=(mpz_get_ui(x._n.get_mpz_t())));
    }
    z_number operator>>(int x) const { return operator>>(z_number(x)); }

    [[nodiscard]] z_number fill_ones() const {
        assert(_n >= 0);
        if (_n == 0) {
            return z_number(0);
        }

        mpz_class result;
        for (result = 1; result < _n; result = 2 * result + 1)
            ;
        return z_number(result);
    }

    friend std::ostream& operator<<(std::ostream& o, const z_number& z) { return o << z._n.get_str(); }

}; // class z_number

using number_t = z_number;

inline std::size_t hash_value(const z_number& z) { return z.hash(); }

} // namespace crab


#ifndef STAN_VARIATIONAL_ADVI_HPP
#define STAN_VARIATIONAL_ADVI_HPP

#include <stan/math.hpp>
#include <stan/callbacks/logger.hpp>
#include <stan/callbacks/writer.hpp>
#include <stan/callbacks/stream_writer.hpp>
#include <stan/io/dump.hpp>
#include <stan/services/error_codes.hpp>
#include <stan/variational/print_progress.hpp>
#include <stan/variational/families/normal_fullrank.hpp>
#include <stan/variational/families/normal_meanfield.hpp>
#include <boost/circular_buffer.hpp>
#include <boost/lexical_cast.hpp>
#include <algorithm>
#include <limits>
#include <numeric>
#include <ostream>
#include <vector>
#include <queue>
#include <string>

namespace stan {

namespace variational {

/**
 * Automatic Differentiation Variational Inference
 *
 * Implements "black box" variational inference using stochastic gradient
 * ascent to maximize the Evidence Lower Bound for a given model
 * and variational family.
 *
 * @tparam Model class of model
 * @tparam Q class of variational distribution
 * @tparam BaseRNG class of random number generator
 */
template <class Model, class Q, class BaseRNG>
class advi {
 public:
  /**
   * Constructor
   *
   * @param[in] m stan model
   * @param[in] cont_params initialization of continuous parameters
   * @param[in,out] rng random number generator
   * @param[in] n_monte_carlo_grad number of samples for gradient computation
   * @param[in] n_monte_carlo_elbo number of samples for ELBO computation
   * @param[in] eval_elbo evaluate ELBO at every "eval_elbo" iters
   * @param[in] n_posterior_samples number of samples to draw from posterior
   * @throw std::runtime_error if n_monte_carlo_grad is not positive
   * @throw std::runtime_error if n_monte_carlo_elbo is not positive
   * @throw std::runtime_error if eval_elbo is not positive
   * @throw std::runtime_error if n_posterior_samples is not positive
   */
  advi(Model& m, Eigen::VectorXd& cont_params, BaseRNG& rng,
       int n_monte_carlo_grad, int n_monte_carlo_elbo, int eval_elbo,
       int n_posterior_samples)
      : model_(m),
        cont_params_(cont_params),
        rng_(rng),
        n_monte_carlo_grad_(n_monte_carlo_grad),
        n_monte_carlo_elbo_(n_monte_carlo_elbo),
        eval_elbo_(eval_elbo),
        n_posterior_samples_(n_posterior_samples) {
    static const char* function = "stan::variational::advi";
    math::check_positive(function,
                         "Number of Monte Carlo samples for gradients",
                         n_monte_carlo_grad_);
    math::check_positive(function, "Number of Monte Carlo samples for ELBO",
                         n_monte_carlo_elbo_);
    math::check_positive(function, "Evaluate ELBO at every eval_elbo iteration",
                         eval_elbo_);
    math::check_positive(function, "Number of posterior samples for output",
                         n_posterior_samples_);
  }

  /**
   * Calculates the Evidence Lower BOund (ELBO) by sampling from
   * the variational distribution and then evaluating the log joint,
   * adjusted by the entropy term of the variational distribution.
   *
   * @param[in] variational variational approximation at which to evaluate
   * the ELBO.
   * @param logger logger for messages
   * @return the evidence lower bound.
   * @throw std::domain_error If, after n_monte_carlo_elbo_ number of draws
   * from the variational distribution all give non-finite log joint
   * evaluations. This means that the model is severly ill conditioned or
   * that the variational distribution has somehow collapsed.
   */
  double calc_ELBO(const Q& variational, callbacks::logger& logger) const {
    static const char* function = "stan::variational::advi::calc_ELBO";

    double elbo = 0.0;
    int dim = variational.dimension();
    Eigen::VectorXd zeta(dim);

    int n_dropped_evaluations = 0;
    for (int i = 0; i < n_monte_carlo_elbo_;) {
      variational.sample(rng_, zeta);
      try {
        std::stringstream ss;
        double log_prob = model_.template log_prob<false, true>(zeta, &ss);
        if (ss.str().length() > 0)
          logger.info(ss);
        stan::math::check_finite(function, "log_prob", log_prob);
        elbo += log_prob;
        ++i;
      } catch (const std::domain_error& e) {
        ++n_dropped_evaluations;
        if (n_dropped_evaluations >= n_monte_carlo_elbo_) {
          const char* name = "The number of dropped evaluations";
          const char* msg1 = "has reached its maximum amount (";
          const char* msg2
              = "). Your model may be either severely "
                "ill-conditioned or misspecified.";
          stan::math::domain_error(function, name, n_monte_carlo_elbo_, msg1,
                                   msg2);
        }
      }
    }
    elbo /= n_monte_carlo_elbo_;
    elbo += variational.entropy();
    return elbo;
  }

  /**
   * Calculates the "black box" gradient of the ELBO.
   *
   * @param[in] variational variational approximation at which to evaluate
   * the ELBO.
   * @param[out] elbo_grad gradient of ELBO with respect to variational
   * approximation.
   * @param logger logger for messages
   */
  void calc_ELBO_grad(const Q& variational, Q& elbo_grad,
                      callbacks::logger& logger) const {
    static const char* function = "stan::variational::advi::calc_ELBO_grad";

    stan::math::check_size_match(
        function, "Dimension of elbo_grad", elbo_grad.dimension(),
        "Dimension of variational q", variational.dimension());
    stan::math::check_size_match(
        function, "Dimension of variational q", variational.dimension(),
        "Dimension of variables in model", cont_params_.size());

    variational.calc_grad(elbo_grad, model_, cont_params_, n_monte_carlo_grad_,
                          rng_, logger);
  }

  /**
   * Heuristic grid search to adapt eta to the scale of the problem.
   *
   * @param[in] variational initial variational distribution.
   * @param[in] adapt_iterations number of iterations to spend doing stochastic
   * gradient ascent at each proposed eta value.
   * @param[in,out] logger logger for messages
   * @return adapted (tuned) value of eta via heuristic grid search
   * @throw std::domain_error If either (a) the initial ELBO cannot be
   * computed at the initial variational distribution, (b) all step-size
   * proposals in eta_sequence fail.
   */
  double adapt_eta(Q& variational, int adapt_iterations,
                   callbacks::logger& logger) const {
    static const char* function = "stan::variational::advi::adapt_eta";

    stan::math::check_positive(function, "Number of adaptation iterations",
                               adapt_iterations);

    logger.info("Begin eta adaptation.");

    // Sequence of eta values to try during adaptation
    const int eta_sequence_size = 5;
    double eta_sequence[eta_sequence_size] = {100, 10, 1, 0.1, 0.01};

    // Initialize ELBO tracking variables
    double elbo = -std::numeric_limits<double>::max();
    double elbo_best = -std::numeric_limits<double>::max();
    double elbo_init;
    try {
      elbo_init = calc_ELBO(variational, logger);
    } catch (const std::domain_error& e) {
      const char* name
          = "Cannot compute ELBO using the initial "
            "variational distribution.";
      const char* msg1
          = "Your model may be either "
            "severely ill-conditioned or misspecified.";
      stan::math::domain_error(function, name, "", msg1);
    }

    // Variational family to store gradients
    Q elbo_grad = Q(model_.num_params_r());

    // Adaptive step-size sequence
    Q history_grad_squared = Q(model_.num_params_r());
    double tau = 1.0;
    double pre_factor = 0.9;
    double post_factor = 0.1;

    double eta_best = 0.0;
    double eta;
    double eta_scaled;

    bool do_more_tuning = true;
    int eta_sequence_index = 0;
    while (do_more_tuning) {
      // Try next eta
      eta = eta_sequence[eta_sequence_index];

      int print_progress_m;
      for (int iter_tune = 1; iter_tune <= adapt_iterations; ++iter_tune) {
        print_progress_m = eta_sequence_index * adapt_iterations + iter_tune;
        variational ::print_progress(print_progress_m, 0,
                                     adapt_iterations * eta_sequence_size,
                                     adapt_iterations, true, "", "", logger);

        // (ROBUST) Compute gradient of ELBO. It's OK if it diverges.
        // We'll try a smaller eta.
        try {
          calc_ELBO_grad(variational, elbo_grad, logger);
        } catch (const std::domain_error& e) {
          elbo_grad.set_to_zero();
        }

        // Update step-size
        if (iter_tune == 1) {
          history_grad_squared += elbo_grad.square();
        } else {
          history_grad_squared = pre_factor * history_grad_squared
                                 + post_factor * elbo_grad.square();
        }
        eta_scaled = eta / sqrt(static_cast<double>(iter_tune));
        // Stochastic gradient update
        variational
            += eta_scaled * elbo_grad / (tau + history_grad_squared.sqrt());
      }

      // (ROBUST) Compute ELBO. It's OK if it has diverged.
      try {
        elbo = calc_ELBO(variational, logger);
      } catch (const std::domain_error& e) {
        elbo = -std::numeric_limits<double>::max();
      }

      // Check if:
      // (1) ELBO at current eta is worse than the best ELBO
      // (2) the best ELBO hasn't gotten worse than the initial ELBO
      if (elbo < elbo_best && elbo_best > elbo_init) {
        std::stringstream ss;
        ss << "Success!"
           << " Found best value [eta = " << eta_best << "]";
        if (eta_sequence_index < eta_sequence_size - 1)
          ss << (" earlier than expected.");
        else
          ss << ".";
        logger.info(ss);
        logger.info("");
        do_more_tuning = false;
      } else {
        if (eta_sequence_index < eta_sequence_size - 1) {
          // Reset
          elbo_best = elbo;
          eta_best = eta;
        } else {
          // No more eta values to try, so use current eta if it
          // didn't diverge or fail if it did diverge
          if (elbo > elbo_init) {
            std::stringstream ss;
            ss << "Success!"
               << " Found best value [eta = " << eta_best << "].";
            logger.info(ss);
            logger.info("");
            eta_best = eta;
            do_more_tuning = false;
          } else {
            const char* name = "All proposed step-sizes";
            const char* msg1
                = "failed. Your model may be either "
                  "severely ill-conditioned or misspecified.";
            stan::math::domain_error(function, name, "", msg1);
          }
        }
        // Reset
        history_grad_squared.set_to_zero();
      }
      ++eta_sequence_index;
      variational = Q(cont_params_);
    }
    return eta_best;
  }

  /**
   * Runs stochastic gradient ascent with an adaptive stepsize sequence.
   *
   * @param[in,out] variational initia variational distribution
   * @param[in] eta stepsize scaling parameter
   * @param[in] tol_rel_obj relative tolerance parameter for convergence
   * @param[in] max_iterations max number of iterations to run algorithm
   * @param[in,out] logger logger for messages
   * @param[in,out] diagnostic_writer writer for diagnostic information
   * @throw std::domain_error If the ELBO or its gradient is ever
   * non-finite, at any iteration
   */
  void stochastic_gradient_ascent(Q& variational, double eta,
                                  double tol_rel_obj, int max_iterations,
                                  callbacks::logger& logger,
                                  callbacks::writer& diagnostic_writer) const {
    static const char* function
        = "stan::variational::advi::stochastic_gradient_ascent";

    stan::math::check_positive(function, "Eta stepsize", eta);
    stan::math::check_positive(
        function, "Relative objective function tolerance", tol_rel_obj);
    stan::math::check_positive(function, "Maximum iterations", max_iterations);

    // Gradient parameters
    Q elbo_grad = Q(model_.num_params_r());

    // Stepsize sequence parameters
    Q history_grad_squared = Q(model_.num_params_r());
    double tau = 1.0;
    double pre_factor = 0.9;
    double post_factor = 0.1;
    double eta_scaled;

    // Initialize ELBO and convergence tracking variables
    double elbo(0.0);
    double elbo_best = -std::numeric_limits<double>::max();
    double elbo_prev = -std::numeric_limits<double>::max();
    double delta_elbo = std::numeric_limits<double>::max();
    double delta_elbo_ave = std::numeric_limits<double>::max();
    double delta_elbo_med = std::numeric_limits<double>::max();

    // Heuristic to estimate how far to look back in rolling window
    int cb_size
        = static_cast<int>(std::max(0.1 * max_iterations / eval_elbo_, 2.0));
    boost::circular_buffer<double> elbo_diff(cb_size);

    logger.info("Begin stochastic gradient ascent.");
    logger.info(
        "  iter"
        "             ELBO"
        "   delta_ELBO_mean"
        "   delta_ELBO_med"
        "   notes ");

    // Timing variables
    clock_t start = clock();
    clock_t end;
    double delta_t;

    // Main loop
    bool do_more_iterations = true;
    for (int iter_counter = 1; do_more_iterations; ++iter_counter) {
      // Compute gradient using Monte Carlo integration
      calc_ELBO_grad(variational, elbo_grad, logger);

      // Update step-size
      if (iter_counter == 1) {
        history_grad_squared += elbo_grad.square();
      } else {
        history_grad_squared = pre_factor * history_grad_squared
                               + post_factor * elbo_grad.square();
      }
      eta_scaled = eta / sqrt(static_cast<double>(iter_counter));

      // Stochastic gradient update
      variational
          += eta_scaled * elbo_grad / (tau + history_grad_squared.sqrt());

      // Check for convergence every "eval_elbo_"th iteration
      if (iter_counter % eval_elbo_ == 0) {
        elbo_prev = elbo;
        elbo = calc_ELBO(variational, logger);
        if (elbo > elbo_best)
          elbo_best = elbo;
        delta_elbo = rel_difference(elbo, elbo_prev);
        elbo_diff.push_back(delta_elbo);
        delta_elbo_ave
            = std::accumulate(elbo_diff.begin(), elbo_diff.end(), 0.0)
              / static_cast<double>(elbo_diff.size());
        delta_elbo_med = circ_buff_median(elbo_diff);
        std::stringstream ss;
        ss << "  " << std::setw(4) << iter_counter << "  " << std::setw(15)
           << std::fixed << std::setprecision(3) << elbo << "  "
           << std::setw(16) << std::fixed << std::setprecision(3)
           << delta_elbo_ave << "  " << std::setw(15) << std::fixed
           << std::setprecision(3) << delta_elbo_med;

        end = clock();
        delta_t = static_cast<double>(end - start) / CLOCKS_PER_SEC;

        std::vector<double> print_vector;
        print_vector.clear();
        print_vector.push_back(iter_counter);
        print_vector.push_back(delta_t);
        print_vector.push_back(elbo);
        diagnostic_writer(print_vector);

        if (delta_elbo_ave < tol_rel_obj) {
          ss << "   MEAN ELBO CONVERGED";
          do_more_iterations = false;
        }

        if (delta_elbo_med < tol_rel_obj) {
          ss << "   MEDIAN ELBO CONVERGED";
          do_more_iterations = false;
        }

        if (iter_counter > 10 * eval_elbo_) {
          if (delta_elbo_med > 0.5 || delta_elbo_ave > 0.5) {
            ss << "   MAY BE DIVERGING... INSPECT ELBO";
          }
        }

        logger.info(ss);

        if (do_more_iterations == false
            && rel_difference(elbo, elbo_best) > 0.05) {
          logger.info(
              "Informational Message: The ELBO at a previous "
              "iteration is larger than the ELBO upon "
              "convergence!");
          logger.info(
              "This variational approximation may not "
              "have converged to a good optimum.");
        }
      }

      if (iter_counter == max_iterations) {
        logger.info(
            "Informational Message: The maximum number of "
            "iterations is reached! The algorithm may not have "
            "converged.");
        logger.info(
            "This variational approximation is not "
            "guaranteed to be meaningful.");
        do_more_iterations = false;
      }
    }
  }

  /**
   * Runs ADVI and writes to output.
   *
   * @param[in] eta eta parameter of stepsize sequence
   * @param[in] adapt_engaged boolean flag for eta adaptation
   * @param[in] adapt_iterations number of iterations for eta adaptation
   * @param[in] tol_rel_obj relative tolerance parameter for convergence
   * @param[in] max_iterations max number of iterations to run algorithm
   * @param[in,out] logger logger for messages
   * @param[in,out] parameter_writer writer for parameters
   *   (typically to file)
   * @param[in,out] diagnostic_writer writer for diagnostic information
   */
  int run(double eta, bool adapt_engaged, int adapt_iterations,
          double tol_rel_obj, int max_iterations, callbacks::logger& logger,
          callbacks::writer& parameter_writer,
          callbacks::writer& diagnostic_writer) const {
    diagnostic_writer("iter,time_in_seconds,ELBO");

    // Initialize variational approximation
    Q variational = Q(cont_params_);

    if (adapt_engaged) {
      eta = adapt_eta(variational, adapt_iterations, logger);
      parameter_writer("Stepsize adaptation complete.");
      std::stringstream ss;
      ss << "eta = " << eta;
      parameter_writer(ss.str());
    }

    stochastic_gradient_ascent(variational, eta, tol_rel_obj, max_iterations,
                               logger, diagnostic_writer);

    // Write posterior mean of variational approximations.
    cont_params_ = variational.mean();
    std::vector<double> cont_vector(cont_params_.size());
    for (int i = 0; i < cont_params_.size(); ++i)
      cont_vector.at(i) = cont_params_(i);
    std::vector<int> disc_vector;
    std::vector<double> values;

    std::stringstream msg;
    model_.write_array(rng_, cont_vector, disc_vector, values, true, true,
                       &msg);
    if (msg.str().length() > 0)
      logger.info(msg);

    // The first row of lp_, log_p, and log_g.
    values.insert(values.begin(), {0, 0, 0});
    parameter_writer(values);

    // Draw more from posterior and write on subsequent lines
    logger.info("");
    std::stringstream ss;
    ss << "Drawing a sample of size " << n_posterior_samples_
       << " from the approximate posterior... ";
    logger.info(ss);
    double log_p = 0;
    double log_g = 0;
    // Draw posterior sample. log_g is the log normal densities.
    for (int n = 0; n < n_posterior_samples_; ++n) {
      variational.sample_log_g(rng_, cont_params_, log_g);
      for (int i = 0; i < cont_params_.size(); ++i) {
        cont_vector.at(i) = cont_params_(i);
      }
      std::stringstream msg2;
      model_.write_array(rng_, cont_vector, disc_vector, values, true, true,
                         &msg2);
      //  log_p: Log probability in the unconstrained space
      log_p = model_.template log_prob<false, true>(cont_params_, &msg2);
      if (msg2.str().length() > 0)
        logger.info(msg2);
      // Write lp__, log_p, and log_g.
      values.insert(values.begin(), {0, log_p, log_g});
      parameter_writer(values);
    }
    logger.info("COMPLETED.");
    return stan::services::error_codes::OK;
  }

  // TODO(akucukelbir): move these things to stan math and test there

  /**
   * Compute the median of a circular buffer.
   *
   * @param[in] cb circular buffer with some number of values in it.
   * @return median of values in circular buffer.
   */
  double circ_buff_median(const boost::circular_buffer<double>& cb) const {
    // FIXME: naive implementation; creates a copy as a vector
    std::vector<double> v;
    for (boost::circular_buffer<double>::const_iterator i = cb.begin();
         i != cb.end(); ++i) {
      v.push_back(*i);
    }

    size_t n = v.size() / 2;
    std::nth_element(v.begin(), v.begin() + n, v.end());
    return v[n];
  }

  /**
   * Compute the relative difference between two double values.
   *
   * @param[in] prev previous value
   * @param[in] curr current value
   * @return  absolutely value of relative difference
   */
  double rel_difference(double prev, double curr) const {
    return std::fabs((curr - prev) / prev);
  }

 protected:
  Model& model_;
  Eigen::VectorXd& cont_params_;
  BaseRNG& rng_;
  int n_monte_carlo_grad_;
  int n_monte_carlo_elbo_;
  int eval_elbo_;
  int n_posterior_samples_;
};
}  // namespace variational
}  // namespace stan
#endif


/* Copyright (c) 2021, the adamantine authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#include <experimental_data.hh>
#include <utils.hh>

#include <deal.II/arborx/bvh.h>
#include <deal.II/base/symmetric_tensor.h>
#include <deal.II/base/tensor.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/mapping_q1.h>
#include <deal.II/grid/filtered_iterator.h>
#include <deal.II/grid/reference_cell.h>

#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>

#include <Kokkos_HostSpace.hpp>

#include <cstdlib>
#include <fstream>
#include <limits>
#include <optional>
#include <regex>
#include <sstream>

#include <ArborX_Ray.hpp>

namespace adamantine
{
/**
 * This class implements a predicate for the intersection of Ray and
 * BoundingBox
 */
class RayIntersectPredicate
{
public:
  /**
   * Constructor. @p points is a list of points which we are interested in
   * knowing if they intersect ArborXWrappers::BVH bounding boxes.
   */
  RayIntersectPredicate(std::vector<Ray<3>> const &rays) : _rays(rays) {}

  /**
   * Number of rays stored in the structure.
   */
  std::size_t size() const { return _rays.size(); }

  /**
   * Return the `i`th Ray stored in the object.
   */
  Ray<3> const &get(unsigned int i) const { return _rays[i]; }

private:
  std::vector<Ray<3>> _rays;
};
} // namespace adamantine

namespace ArborX
{
template <>
struct AccessTraits<adamantine::RayIntersectPredicate, PredicatesTag>
{
  using memory_space = Kokkos::HostSpace;

  static std::size_t
  size(adamantine::RayIntersectPredicate const &ray_intersect)
  {
    return ray_intersect.size();
  }

  static auto get(adamantine::RayIntersectPredicate const &ray_intersect,
                  std::size_t i)
  {
    auto const &ray = ray_intersect.get(i);
    auto const &origin = ray.origin;
    auto const &direction = ray.direction;
    ArborX::Experimental::Ray arborx_ray = {
        ArborX::Point{(float)origin[0], (float)origin[1], (float)origin[2]},
        ArborX::Experimental::Ray::Vector{
            (float)direction[0], (float)direction[1], (float)direction[2]}};
    return intersects(arborx_ray);
  }
};
} // namespace ArborX

namespace adamantine
{
template <int dim>
std::vector<PointsValues<dim>> read_experimental_data_point_cloud(
    MPI_Comm const &communicator,
    boost::property_tree::ptree const &experiment_database)
{
  // Format of the file names: the format is pretty arbitrary, #frame and
  // #camera are replaced by the frame and the camera number.
  // PropertyTreeInput experiment.file
  std::string data_filename = experiment_database.get<std::string>("file");
  // PropertyTreeInput experiment.first_frame
  unsigned int first_frame = experiment_database.get("first_frame", 0);
  // PropertyTreeInput experiment.last_frame
  unsigned int last_frame = experiment_database.get<unsigned int>("last_frame");
  // PropertyTreeInput experiment.first_camera_id
  unsigned int first_camera_id =
      experiment_database.get<unsigned int>("first_camera_id");
  // PropertyTreeInput experiment.last_camera_id
  unsigned int last_camera_id = experiment_database.get<int>("last_camera_id");
  // PropertyTreeInput experiment.data_columns
  std::string data_columns =
      experiment_database.get<std::string>("data_columns");

  std::vector<PointsValues<dim>> points_values_all_frames(last_frame + 1 -
                                                          first_frame);
  for (unsigned int frame = first_frame; frame < last_frame + 1; ++frame)
  {
    PointsValues<dim> points_values;
    for (unsigned int camera_id = first_camera_id;
         camera_id < last_camera_id + 1; ++camera_id)
    {
      // Use regex to get the next file to read
      std::regex camera_regex("#camera");
      std::regex frame_regex("#frame");
      auto regex_filename =
          std::regex_replace((std::regex_replace(data_filename, camera_regex,
                                                 std::to_string(camera_id))),
                             frame_regex, std::to_string(frame));
      ASSERT(boost::filesystem::exists(regex_filename),
             "The file " + regex_filename + " does not exist.");
      std::string filename("data_" + std::to_string(frame) + "_" +
                           std::to_string(camera_id) + ".csv");

      // Use bash to create a new file that only contains the columns that we
      // care about. For large files this divides by four the time to parse the
      // files. It also simplifies reading the files. Only rank zero renames
      // the file.
      if (dealii::Utilities::MPI::this_mpi_process(communicator) == 0)
      {
        std::string cut_command("cut -d, -f" + data_columns + " " +
                                regex_filename + " > " + filename);
        std::system(cut_command.c_str());
      }

      // Wait for rank zero before reading the file.
      MPI_Barrier(communicator);

      // Read and parse the file
      std::ifstream file;
      file.open(filename);
      std::string line;
      std::getline(file, line);
      while (std::getline(file, line))
      {
        std::size_t pos = 0;
        std::size_t last_pos = 0;
        std::size_t line_length = line.length();
        unsigned int i = 0;
        dealii::Point<dim> point;
        double value = 0.;
        while (last_pos < line_length + 1)
        {
          pos = line.find_first_of(",", last_pos);
          // If no comma was found that we read until the end of the file
          if (pos == std::string::npos)
          {
            pos = line_length;
          }

          if (pos != last_pos)
          {
            char *end = line.data() + pos;
            if (i < dim)
            {
              point[i] = std::strtod(line.data() + last_pos, &end);
            }
            else
            {
              value = std::strtod(line.data() + last_pos, &end);
            }

            ++i;
          }

          last_pos = pos + 1;
        }

        points_values.points.push_back(point);
        points_values.values.push_back(value);
      }

      // Wait for every rank to be done reading the temporary stripped file and
      // then remove it.
      MPI_Barrier(communicator);
      if (dealii::Utilities::MPI::this_mpi_process(communicator) == 0)
      {
        std::string rm_command("rm " + filename);
        std::system(rm_command.c_str());
      }
    }
    points_values_all_frames[frame - first_frame] = points_values;
  }

  return points_values_all_frames;
}

template <int dim>
std::pair<std::vector<int>, std::vector<int>> set_with_experimental_data(
    PointsValues<dim> const &points_values,
    dealii::DoFHandler<dim> const &dof_handler,
    dealii::LinearAlgebra::distributed::Vector<double> &temperature)
{
  // First we need to get all the supports points and the associated dof
  // indices
  std::map<dealii::types::global_dof_index, dealii::Point<dim>> indices_points;
  dealii::DoFTools::map_dofs_to_support_points(
      dealii::StaticMappingQ1<dim>::mapping, dof_handler, indices_points);
  // Change the format to something that can be used by ArborX
  std::vector<dealii::types::global_dof_index> dof_indices(
      indices_points.size());
  std::vector<dealii::Point<dim>> support_points(indices_points.size());
  unsigned int pos = 0;
  for (auto map_it = indices_points.begin(); map_it != indices_points.end();
       ++map_it, ++pos)
  {
    dof_indices[pos] = map_it->first;
    support_points[pos] = map_it->second;
  }

  // Perform the search
  dealii::ArborXWrappers::BVH bvh(support_points);
  dealii::ArborXWrappers::PointNearestPredicate pt_nearest(points_values.points,
                                                           1);
  auto [indices, offset] = bvh.query(pt_nearest);

  // Fill in the temperature
  unsigned int const n_queries = points_values.points.size();
  for (unsigned int i = 0; i < n_queries; ++i)
  {
    for (int j = offset[i]; j < offset[i + 1]; ++j)
    {
      temperature[dof_indices[indices[j]]] = points_values.values[i];
    }
  }

  temperature.compress(dealii::VectorOperation::insert);

  return {indices, offset};
}

std::vector<std::vector<double>>
read_frame_timestamps(boost::property_tree::ptree const &experiment_database)
{
  // PropertyTreeInput experiment.log_filename
  std::string log_filename =
      experiment_database.get<std::string>("log_filename");

  ASSERT(boost::filesystem::exists(log_filename),
         "The file " + log_filename + " does not exist.");

  // PropertyTreeInput experiment.first_frame_temporal_offset
  double first_frame_offset =
      experiment_database.get("first_frame_temporal_offset", 0.0);

  // PropertyTreeInput experiment.first_frame
  unsigned int first_frame =
      experiment_database.get<unsigned int>("first_frame", 0);
  // PropertyTreeInput experiment.last_frame
  unsigned int last_frame = experiment_database.get<unsigned int>("last_frame");

  // PropertyTreeInput experiment.first_camera_id
  unsigned int first_camera_id =
      experiment_database.get<unsigned int>("first_camera_id");
  // PropertyTreeInput experiment.last_camera_id
  unsigned int last_camera_id =
      experiment_database.get<unsigned int>("last_camera_id");

  unsigned int num_cameras = last_camera_id - first_camera_id + 1;
  std::vector<std::vector<double>> time_stamps(num_cameras);

  std::vector<double> first_frame_value(num_cameras);

  // Read and parse the file
  std::ifstream file;
  file.open(log_filename);
  std::string line;
  std::getline(file, line);
  while (std::getline(file, line))
  {
    unsigned int entry_index = 0;
    std::stringstream s_stream(line);
    bool frame_of_interest = false;
    unsigned int frame = std::numeric_limits<unsigned int>::max();
    while (s_stream.good())
    {
      std::string substring;
      std::getline(s_stream, substring, ',');
      boost::trim(substring);

      if (entry_index == 0)
      {
        ASSERT(std::stoi(substring) - frame == 1 ||
                   frame == std::numeric_limits<unsigned int>::max(),
               "The file " + log_filename +
                   " does not have consecutive frame indices.");
        frame = std::stoi(substring);
        if (frame >= first_frame && frame <= last_frame)
          frame_of_interest = true;
      }
      else
      {
        if (frame == first_frame && substring.size() > 0)
          first_frame_value[entry_index - 1] = std::stod(substring);

        if (frame_of_interest && substring.size() > 0)
          time_stamps[entry_index - 1].push_back(
              std::stod(substring) - first_frame_value[entry_index - 1] +
              first_frame_offset);
      }
      entry_index++;
    }
  }
  return time_stamps;
}

RayTracing::RayTracing(boost::property_tree::ptree const &experiment_database)
{

  // Format of the file names: the format is pretty arbitrary, #frame and
  // #camera are replaced by the frame and the camera number.
  // PropertyTreeInput experiment.file
  std::string data_filename = experiment_database.get<std::string>("file");
  // PropertyTreeInput experiment.first_frame
  unsigned int first_frame = experiment_database.get("first_frame", 0);
  // PropertyTreeInput experiment.last_frame
  unsigned int last_frame = experiment_database.get<unsigned int>("last_frame");
  // PropertyTreeInput experiment.first_camera_id
  unsigned int first_camera_id =
      experiment_database.get<unsigned int>("first_camera_id");
  // PropertyTreeInput experiment.last_camera_id
  unsigned int last_camera_id = experiment_database.get<int>("last_camera_id");

  _rays_all_frames.resize(last_frame + 1 - first_frame);
  _values_all_frames.resize(last_frame + 1 - first_frame);
  for (unsigned int frame = first_frame; frame < last_frame + 1; ++frame)
  {
    std::vector<Ray<dim>> rays_one_frame;
    std::vector<double> values_one_frame;
    for (unsigned int camera_id = first_camera_id;
         camera_id < last_camera_id + 1; ++camera_id)
    {
      // Use regex to get the next file to read
      std::regex camera_regex("#camera");
      std::regex frame_regex("#frame");
      auto filename =
          std::regex_replace((std::regex_replace(data_filename, camera_regex,
                                                 std::to_string(camera_id))),
                             frame_regex, std::to_string(frame));
      ASSERT(boost::filesystem::exists(filename),
             "The file " + filename + " does not exist.");

      // Read and parse the file
      std::ifstream file;
      file.open(filename);
      std::string line;
      while (std::getline(file, line))
      {
        std::size_t pos = 0;
        std::size_t last_pos = 0;
        std::size_t line_length = line.length();
        unsigned int i = 0;
        dealii::Point<dim> point;
        dealii::Tensor<1, dim> direction;
        double value = 0.;
        while (last_pos < line_length + 1)
        {
          pos = line.find_first_of(",", last_pos);
          // If no comma was found that we read until the end of the file
          if (pos == std::string::npos)
          {
            pos = line_length;
          }

          if (pos != last_pos)
          {
            char *end = line.data() + pos;
            if (i < dim)
            {
              point[i] = std::strtod(line.data() + last_pos, &end);
            }
            else if (i < 2 * dim)
            {
              direction[i - dim] = std::strtod(line.data() + last_pos, &end);
            }
            else
            {
              value = std::strtod(line.data() + last_pos, &end);
            }

            ++i;
          }

          last_pos = pos + 1;
        }

        Ray<dim> ray{point, direction};
        rays_one_frame.push_back(ray);
        values_one_frame.push_back(value);
      }
    }
    _rays_all_frames[frame - first_frame] = rays_one_frame;
    _values_all_frames[frame - first_frame] = values_one_frame;
  }
}

PointsValues<3>
RayTracing::get_intersection(dealii::DoFHandler<3> const &dof_handler,
                             unsigned int frame)
{
  PointsValues<dim> points_values;

  // Perform the ray tracing to get the cells that are intersected by rays

  // Create the bounding boxes associated to the locally owned cells with FE
  // index = 0
  std::vector<dealii::BoundingBox<dim>> bounding_boxes;
  std::vector<typename dealii::DoFHandler<dim>::active_cell_iterator>
      cell_iterators;
  for (auto const &cell : dealii::filter_iterators(
           dof_handler.active_cell_iterators(),
           dealii::IteratorFilters::LocallyOwnedCell(),
           dealii::IteratorFilters::ActiveFEIndexEqualTo(0)))
  {
    bounding_boxes.push_back(cell->bounding_box());
    cell_iterators.push_back(cell);
  }

  dealii::ArborXWrappers::BVH bvh(bounding_boxes);
  RayIntersectPredicate ray_intersect(_rays_all_frames[frame]);
  auto [indices, offset] = bvh.query(ray_intersect);

  // Find the exact intersections points
  // See https://en.wikipedia.org/wiki/Line%E2%80%93plane_intersection
  // NOTE that we assume that the faces are flat. If the faces are curved,
  // this is wrong.
  unsigned int const n_rays = _rays_all_frames[frame].size();
  unsigned int n_intersections = 0;
  for (unsigned int i = 0; i < n_rays; ++i)
  {
    if (offset[i] != offset[i + 1])
    {
      ++n_intersections;
    }
  }
  std::vector<double> distances(n_intersections,
                                std::numeric_limits<double>::max());
  points_values.points.resize(n_intersections);
  points_values.values.resize(n_intersections);
  auto constexpr reference_cell = dealii::ReferenceCells::get_hypercube<dim>();
  double constexpr tolerance = 1e-12;
  unsigned int ii = 0;
  for (unsigned int i = 0; i < n_rays; ++i)
  {
    points_values.values[ii] = _values_all_frames[frame][i];
    for (int j = offset[i]; j < offset[i + 1]; ++j)
    {
      auto const &cell = cell_iterators[indices[j]];
      // We know that the ray intersects the bounding box but we don't know
      // where it intersects the cells. We need to check the intersection of
      // the ray with each face of the cell.
      for (unsigned int f = 0; f < reference_cell.n_faces(); ++f)
      {
        // First we check if the ray is parallel to the face. If this is the
        // case, either the ray misses the face or the ray hits the edge of
        // the face. In that last case, the ray is also orthogonal to another
        // face and it is safe to discard all rays parallel to a face.
        auto const point_0 = cell->face(f)->vertex(0);
        auto const point_1 = cell->face(f)->vertex(1);
        auto const point_2 = cell->face(f)->vertex(2);
        dealii::Tensor<1, dim> edge_01({point_1[0] - point_0[0],
                                        point_1[1] - point_0[1],
                                        point_1[2] - point_0[2]});
        dealii::Tensor<1, dim> edge_02({point_2[0] - point_0[0],
                                        point_2[1] - point_0[1],
                                        point_2[2] - point_0[2]});
        auto const &ray_direction = _rays_all_frames[frame][i].direction;
        dealii::Tensor<2, dim> matrix(
            {{-ray_direction[0], -ray_direction[1], -ray_direction[2]},
             {edge_01[0], edge_01[1], edge_01[2]},
             {edge_02[0], edge_02[1], edge_02[2]}});
        double det = dealii::determinant(matrix);
        // If determinant is close to zero, the ray is parallel to the face
        // and we go to the next face.
        if (std::abs(det) < tolerance)
          continue;
        // Compute the distance along the ray direction between the origin of
        // the ray and the intersection point.
        auto const cross_product = dealii::cross_product_3d(edge_01, edge_02);
        auto const &ray_origin = _rays_all_frames[frame][i].origin;
        dealii::Tensor<1, dim> p0_ray({ray_origin[0] - point_0[0],
                                       ray_origin[1] - point_0[1],
                                       ray_origin[2] - point_0[2]});
        double d = cross_product * p0_ray / det;
        // We can finally compute the intersection point. It is possible that
        // a ray intersects multiple faces. For instance if the mesh is a cube
        // the ray will get into the cube from one face and it will get out of
        // the cube by the opposite face. The correct intersection point is
        // the one with the smallest distance.
        if (d < distances[ii])
        {
          // The point intersects the plane of the face but maybe not the face
          // itself. Check that the point is on the face.
          // NOTE: We assume that the face is an axis-aligned rectangle.
          dealii::Point<dim> intersection = ray_origin + d * ray_direction;
          std::vector<double> min(dim, std::numeric_limits<double>::max());
          std::vector<double> max(dim, std::numeric_limits<double>::lowest());
          for (unsigned int coord = 0; coord < dim; ++coord)
          {
            if (point_0[coord] < min[coord])
              min[coord] = point_0[coord];
            if (point_0[coord] > max[coord])
              max[coord] = point_0[coord];

            if (point_1[coord] < min[coord])
              min[coord] = point_1[coord];
            if (point_1[coord] > max[coord])
              max[coord] = point_1[coord];

            if (point_2[coord] < min[coord])
              min[coord] = point_2[coord];
            if (point_2[coord] > max[coord])
              max[coord] = point_2[coord];
          }

          bool on_the_face = true;
          for (unsigned int coord = 0; coord < 3; ++coord)
          {
            // NOTE: We could add a tolerance if the intersection point is on
            // the edge. Currently, we may lose some rays but I don't think it
            // matters. The mesh does not match exactly the real object
            // anyway.
            if ((intersection[coord] < min[coord]) ||
                (intersection[coord] > max[coord]))
            {
              on_the_face = false;
              break;
            }
          }

          if (on_the_face)
          {
            points_values.points[ii] = intersection;
            distances[ii] = d;
          }
        }
      }
    }
    if (offset[i] != offset[i + 1])
      ++ii;
  }

  return points_values;
}

} // namespace adamantine

//-------------------- Explicit Instantiations --------------------//
namespace adamantine
{
template std::vector<PointsValues<2>> read_experimental_data_point_cloud(
    MPI_Comm const &communicator,
    boost::property_tree::ptree const &experiment_database);
template std::vector<PointsValues<3>> read_experimental_data_point_cloud(
    MPI_Comm const &communicator,
    boost::property_tree::ptree const &experiment_database);

template std::pair<std::vector<int>, std::vector<int>>
set_with_experimental_data(
    PointsValues<2> const &points_values,
    dealii::DoFHandler<2> const &dof_handler,
    dealii::LinearAlgebra::distributed::Vector<double> &temperature);
template std::pair<std::vector<int>, std::vector<int>>
set_with_experimental_data(
    PointsValues<3> const &points_values,
    dealii::DoFHandler<3> const &dof_handler,
    dealii::LinearAlgebra::distributed::Vector<double> &temperature);
} // namespace adamantine


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

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

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

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

#ifndef BOOST_GEOMETRY_CORE_POINT_ORDER_HPP
#define BOOST_GEOMETRY_CORE_POINT_ORDER_HPP

#include <boost/mpl/assert.hpp>
#include <boost/range.hpp>
#include <boost/type_traits/remove_const.hpp>

#include <boost/geometry/core/ring_type.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>

namespace boost { namespace geometry
{

/*!
\brief Enumerates options for the order of points within polygons
\ingroup enum
\details The enumeration order_selector describes options for the order of
    points within a polygon. Polygons can be ordered either clockwise or
    counterclockwise. The specific order of a polygon type is defined by the
    point_order metafunction. The point_order metafunction defines a value,
    which is one of the values enumerated in the order_selector

\qbk{
[heading See also]
[link geometry.reference.core.point_order The point_order metafunction]
}
*/
enum order_selector
{
    /// Points are ordered clockwise
    clockwise = 1,
    /// Points are ordered counter clockwise
    counterclockwise = 2,
    /// Points might be stored in any order, algorithms will determine it on the
    /// fly (not yet supported)
    order_undetermined = 0
};

namespace traits
{

/*!
\brief Traits class indicating the order of contained points within a
    ring or (multi)polygon, clockwise, counter clockwise or not known.
\ingroup traits
\tparam Ring ring
*/
template <typename Ring>
struct point_order
{
    static const order_selector value = clockwise;
};

} // namespace traits

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace point_order
{

struct clockwise
{
    static const order_selector value = geometry::clockwise;
};

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

#ifndef DOXYGEN_NO_DISPATCH
namespace core_dispatch
{

template <typename Tag, typename Geometry>
struct point_order
{
    BOOST_MPL_ASSERT_MSG
        (
            false, NOT_IMPLEMENTED_FOR_THIS_GEOMETRY_TYPE
            , (types<Geometry>)
        );
};

template <typename Point>
struct point_order<point_tag, Point>
    : public detail::point_order::clockwise {};

template <typename Segment>
struct point_order<segment_tag, Segment>
    : public detail::point_order::clockwise {};

template <typename Box>
struct point_order<box_tag, Box>
    : public detail::point_order::clockwise {};

template <typename LineString>
struct point_order<linestring_tag, LineString>
    : public detail::point_order::clockwise {};

template <typename Ring>
struct point_order<ring_tag, Ring>
{
    static const order_selector value
        = geometry::traits::point_order<Ring>::value;
};

// Specialization for polygon: the order is the order of its rings
template <typename Polygon>
struct point_order<polygon_tag, Polygon>
{
    static const order_selector value = core_dispatch::point_order
        <
            ring_tag,
            typename ring_type<polygon_tag, Polygon>::type
        >::value ;
};

} // namespace core_dispatch
#endif // DOXYGEN_NO_DISPATCH

/*!
\brief \brief_meta{value, point order (clockwise\, counterclockwise),
    \meta_geometry_type}
\tparam Geometry \tparam_geometry
\ingroup core

\qbk{[include reference/core/point_order.qbk]}
*/
template <typename Geometry>
struct point_order
{
    static const order_selector value = core_dispatch::point_order
        <
            typename tag<Geometry>::type,
            typename boost::remove_const<Geometry>::type
        >::value;
};

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_CORE_POINT_ORDER_HPP


/*
 *  Copyright 2010-2011 Anders Wallin (anders.e.e.wallin "at" gmail.com)
 *
 *  This file is part of OpenCAMlib.
 *
 *  OpenCAMlib 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.
 *
 *  OpenCAMlib 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 OpenCAMlib.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <boost/foreach.hpp>

#include "fiber.hpp"

namespace ocl
{

Fiber::Fiber(const Point &p1in, const Point &p2in) {
    p1=p1in;
    p2=p2in;
    calcDir();
}

void Fiber::calcDir() {
    dir = p2 - p1;
    assert( dir.z == 0.0 );
    dir.normalize();
}

bool Fiber::contains(Interval& i) const {
    BOOST_FOREACH( Interval fi, ints) {
        if ( i.inside( fi ) )
            return true;
    }
    return false;
}

bool Fiber::missing(Interval& i) const {
    bool result = true;
    BOOST_FOREACH( Interval fi, ints) {
        if ( !i.outside( fi ) ) // all existing ints must be non-overlapping
            result = false;
    }
    return result;
}

void Fiber::addInterval(Interval& i) {
    if (i.empty())
        return; // do nothing.

    if (ints.empty()) { // empty fiber case
        ints.push_back(i);
        return;
    } else if ( this->contains(i)  ) { // if fiber already contains i
        return; // do nothing
    } else if ( this->missing(i) ) { // if fiber doesn't contain i
        ints.push_back(i);
        return;
    } else {
        // this is the messier general case with partial overlap
        std::vector<Interval>::iterator itr;
        itr = ints.begin();
        std::vector<Interval> overlaps;
        while (itr!=ints.end()) { // loop through all intervals
            if ( ! (itr->outside( i )) ) {
                overlaps.push_back(*itr); // add overlaps here
                itr = ints.erase(itr); // erase overlaps from ints
            } else {
                ++itr;
            }
        }
        overlaps.push_back(i);
        // now build a new interval from i and the overlaps
        Interval sumint;
        BOOST_FOREACH(Interval intr, overlaps) {
            sumint.updateLower( intr.lower, intr.lower_cc );
            sumint.updateUpper( intr.upper, intr.upper_cc );
        }
        ints.push_back(sumint); // add the sum-interval to ints
        return;
    }
}

double Fiber::tval(Point& p) const {
    // fiber is  f = p1 + t * (p2-p1)
    // t = (f-p1).dot(p2-p1) / (p2-p1).dot(p2-p1)
    return  (p-p1).dot(p2-p1) / (p2-p1).dot(p2-p1);
}

Point Fiber::point(double t) const {
    Point p = p1 + t*(p2-p1);
    return p;
}

void Fiber::printInts() const {
    int n=0;
    BOOST_FOREACH( Interval i, ints) {
        std::cout << n << ": [ " << i.lower << " , " << i.upper << " ]" << "\n";
        ++n;
    }
}

std::ostream& operator<<(std::ostream &stream, const Fiber& f) {
  stream << " fiber dir=" << f.dir << " and " << f.ints.size() << " intervals\n";
  stream << " fiber.p1=" << f.p1 << " fiber.p2 " << f.p2 ;
  return stream;
}

} // end namespace
// end file fiber.cpp