#include <Eigen/Core>
#include <glog/logging.h>

#include <aslam/cameras/camera-pinhole.h>
#include <aslam/cameras/distortion-fisheye.h>
#include <aslam/cameras/ncamera.h>
#include <aslam/frames/visual-frame.h>
#include <maplab-common/accessors.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/quaternion-math.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>
#include <vi-map/landmark-index.h>
#include <vi-map/mission-baseframe.h>
#include <vi-map/unique-id.h>
#include <vi-map/vi-mission.h>

#include "map-optimization-legacy/landmark-geometry-verification.h"

namespace map_optimization_legacy {

class ViwlsGraph : public ::testing::Test {
 protected:
  typedef aslam::FisheyeDistortion DistortionType;
  typedef aslam::PinholeCamera CameraType;

  virtual void SetUp() {
    constructCamera();
    createMission();
    populatePosegraphAndLandmarks();
    landmark_geometry_ = std::shared_ptr<LandmarkGeometryVerification>(
        new LandmarkGeometryVerification(
            posegraph_, landmarks_, missions_, mission_base_frames_,
            sensor_manager_));
  }

  void constructCamera();
  const vi_map::MissionId createMission();
  void prepareVertex(
      const Eigen::Quaterniond& G_q_I, const Eigen::Matrix<double, 3, 1>& G_p_I,
      const vi_map::MissionId& mission_id,
      vi_map::Vertex::UniquePtr* vertex_ptr);
  const vi_map::LandmarkId addLandmarkToVertex(
      const Eigen::Vector3d& G_p_fi, vi_map::Vertex* vertex);
  void addKeypointToVertex(
      const Eigen::Vector3d& G_p_fi, const vi_map::LandmarkId& landmark_id,
      vi_map::Vertex* vertex_ptr);
  void populatePosegraphAndLandmarks();
  Eigen::Vector2d calculateErrors();

  std::unordered_map<vi_map::LandmarkId, pose_graph::VertexId> landmarks_;
  vi_map::MissionMap missions_;
  vi_map::MissionBaseFrameMap mission_base_frames_;
  vi_map::PoseGraph posegraph_;
  vi_map::SensorManager sensor_manager_;

  std::shared_ptr<LandmarkGeometryVerification> landmark_geometry_;

  vi_map::LandmarkId map_landmark_id_;
  vi_map::LandmarkId vertex_landmark_id_;

  aslam::NCamera::Ptr cameras_;

  vi_map::LandmarkId landmark_ids_[2];

  Eigen::Vector3d G_p_fi0_;
  Eigen::Vector3d G_p_fi1_;

  // C_T_G transformations for observer vertices.
  aslam::Transformation C0_T_G_;
  aslam::Transformation C1_T_G_;

  static constexpr int kDescriptorBytes = 48;
  static constexpr int kNumCameras = 1;
  static constexpr int kVisualFrameIndex = 0;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

void ViwlsGraph::constructCamera() {
  Eigen::VectorXd distortion_param(1);
  distortion_param << 0.94;
  double fu = 120;
  double fv = 90;
  int res_u = 640;
  int res_v = 480;
  double cu = res_u / 2.0;
  double cv = res_v / 2.0;

  aslam::Distortion::UniquePtr distortion(new DistortionType(distortion_param));

  Eigen::VectorXd intrinsics(4);
  intrinsics << fu, fv, cu, cv;

  aslam::CameraId camera_id;
  common::generateId(&camera_id);
  aslam::Camera::Ptr camera = std::shared_ptr<CameraType>(
      new CameraType(intrinsics, res_u, res_v, distortion));
  camera->setId(camera_id);

  std::vector<aslam::Camera::Ptr> camera_vector;
  camera_vector.push_back(camera);
  aslam::TransformationVector T_C_B_vector;
  aslam::Transformation T_C_B(
      Eigen::Quaterniond(1, 0, 0.2, 0).normalized(), Eigen::Vector3d(0, 1, 2));
  T_C_B_vector.push_back(T_C_B);
  aslam::NCameraId n_camera_id;
  common::generateId(&n_camera_id);
  cameras_.reset(
      new aslam::NCamera(
          n_camera_id, T_C_B_vector, camera_vector, "Test camera rig"));
}

const vi_map::MissionId ViwlsGraph::createMission() {
  vi_map::MissionBaseFrame baseframe;
  vi_map::MissionBaseFrameId baseframe_id;
  common::generateId(&baseframe_id);
  baseframe.setId(baseframe_id);

  baseframe.set_p_G_M(Eigen::Matrix<double, 3, 1>::Zero());
  baseframe.set_q_G_M(Eigen::Quaterniond::Identity());

  vi_map::VIMission::UniquePtr mission_ptr(new vi_map::VIMission);
  vi_map::MissionId mission_id;
  generateId(&mission_id);
  mission_ptr->setId(mission_id);
  mission_ptr->setBaseFrameId(baseframe_id);

  missions_.emplace(mission_ptr->id(), std::move(mission_ptr));
  mission_base_frames_.emplace(baseframe.id(), baseframe);

  sensor_manager_.addNCamera(cameras_, mission_id);

  return mission_id;
}

void ViwlsGraph::prepareVertex(
    const Eigen::Quaterniond& G_q_I, const Eigen::Matrix<double, 3, 1>& G_p_I,
    const vi_map::MissionId& mission_id,
    vi_map::Vertex::UniquePtr* vertex_ptr) {
  CHECK_NOTNULL(vertex_ptr);
  vertex_ptr->reset(new vi_map::Vertex(cameras_));

  pose_graph::VertexId vertex_id;
  generateId(&vertex_id);
  (*vertex_ptr)->setId(vertex_id);

  (*vertex_ptr)->set_p_M_I(G_p_I);
  Eigen::Quaterniond G_q_I_positive = G_q_I;
  if (G_q_I_positive.w() < 0.0) {
    G_q_I_positive.coeffs() = -G_q_I_positive.coeffs();
  }
  (*vertex_ptr)->set_q_M_I(G_q_I_positive);

  CHECK_GT(missions_.count(mission_id), 0u);
  (*vertex_ptr)->setMissionId(mission_id);

  aslam::VisualFrame::Ptr frame(new aslam::VisualFrame);
  aslam::FrameId frame_id;
  common::generateId(&frame_id);
  frame->setId(frame_id);
  frame->setCameraGeometry(cameras_->getCameraShared(kVisualFrameIndex));
  (*vertex_ptr)->getVisualNFrame().setFrame(kVisualFrameIndex, frame);
}

const vi_map::LandmarkId ViwlsGraph::addLandmarkToVertex(
    const Eigen::Vector3d& G_p_fi, vi_map::Vertex* vertex_ptr) {
  CHECK_NOTNULL(vertex_ptr);

  std::shared_ptr<vi_map::Landmark> landmark_ptr(new vi_map::Landmark);
  vi_map::LandmarkId landmark_id;
  generateId(&landmark_id);
  landmark_ptr->setId(landmark_id);

  pose::Transformation G_T_I = vertex_ptr->get_T_M_I();

  Eigen::Vector3d I_p_fi = G_T_I.inverse() * G_p_fi;
  landmark_ptr->set_p_B(I_p_fi);
  vertex_ptr->getLandmarks().addLandmark(*landmark_ptr);

  CHECK(vertex_ptr->id().isValid());
  landmarks_.emplace(landmark_id, vertex_ptr->id());
  return landmark_id;
}

void ViwlsGraph::addKeypointToVertex(
    const Eigen::Vector3d& p_G_fi, const vi_map::LandmarkId& landmark_id,
    vi_map::Vertex* vertex_ptr) {
  CHECK_NOTNULL(vertex_ptr);

  const vi_map::MissionId& mission_id = vertex_ptr->getMissionId();
  CHECK_GT(missions_.count(mission_id), 0u);
  const pose::Transformation T_C_I = cameras_->get_T_C_B(kVisualFrameIndex);

  pose::Transformation T_M_I = vertex_ptr->get_T_M_I();
  const Eigen::Vector3d p_C_fi = T_C_I * T_M_I.inverse() * p_G_fi;

  Eigen::Vector2d reprojected_keypoint;
  aslam::ProjectionResult result =
      vertex_ptr->getCamera(kVisualFrameIndex)
          ->project3(p_C_fi, &reprojected_keypoint);

  ASSERT_EQ(result, aslam::ProjectionResult::KEYPOINT_VISIBLE);

  Eigen::Matrix2Xd measurements;
  measurements.resize(Eigen::NoChange, 1);
  measurements = reprojected_keypoint;
  vertex_ptr->getVisualFrame(kVisualFrameIndex)
      .setKeypointMeasurements(measurements);
  CHECK(
      vertex_ptr->getVisualFrame(kVisualFrameIndex).hasKeypointMeasurements());

  vertex_ptr->addObservedLandmarkId(kVisualFrameIndex, landmark_id);

  CHECK_GT(landmarks_.count(landmark_id), 0u);
  const pose_graph::VertexId& landmark_vertex_id =
      common::getChecked(landmarks_, landmark_id);
  CHECK(landmark_vertex_id.isValid());
  vi_map::Vertex& landmark_vertex =
      posegraph_.getVertexPtrMutable(landmark_vertex_id)
          ->getAs<vi_map::Vertex>();

  unsigned int keypoint_idx =
      vertex_ptr->observedLandmarkIdsSize(kVisualFrameIndex) - 1;
  landmark_vertex.getLandmarks()
      .getLandmark(landmark_id)
      .addObservation(vertex_ptr->id(), kVisualFrameIndex, keypoint_idx);
}

void ViwlsGraph::populatePosegraphAndLandmarks() {
  CHECK(cameras_);

  vi_map::MissionId mission_id = createMission();

  vi_map::Vertex::UniquePtr vertex0;
  vi_map::Vertex::UniquePtr vertex1;
  vi_map::Vertex::UniquePtr vertex2;
  vi_map::Vertex::UniquePtr vertex3;

  // vertex0: 1st observer
  // vertex1: 2nd observer
  // vertex2: base for initial landmark position
  // vertex3: base for final landmark position
  Eigen::Vector3d G_p_I0(0, 0, 0);
  Eigen::Quaterniond G_q_I0(1, 0, 0, 0);
  Eigen::Vector3d G_p_I1(1, 0, 0);
  Eigen::Quaterniond G_q_I1(1, 0, 0, 0);
  Eigen::Vector3d G_p_I2(0.43, 0.2, 5);
  Eigen::Quaterniond G_q_I2(0, 0, 1, 0);
  Eigen::Vector3d G_p_I3(-0.46, -0.14, 8);
  Eigen::Quaterniond G_q_I3(sqrt(2) / 2, 0, sqrt(2) / 2, 0);

  prepareVertex(G_q_I0, G_p_I0, mission_id, &vertex0);
  prepareVertex(G_q_I1, G_p_I1, mission_id, &vertex1);
  prepareVertex(G_q_I2, G_p_I2, mission_id, &vertex2);
  prepareVertex(G_q_I3, G_p_I3, mission_id, &vertex3);

  const unsigned int kCameraIndex = 0;
  const aslam::Transformation& C_T_I = cameras_->get_T_C_B(kCameraIndex);
  C0_T_G_ = C_T_I * aslam::Transformation(G_q_I0, G_p_I0).inverse();
  C1_T_G_ = C_T_I * aslam::Transformation(G_q_I1, G_p_I1).inverse();

  // Create landmarks.
  G_p_fi0_ << 0, 0.1, 1;
  G_p_fi1_ << 0, 0.9, 10;
  landmark_ids_[0] = addLandmarkToVertex(G_p_fi0_, vertex2.get());
  landmark_ids_[1] = addLandmarkToVertex(G_p_fi1_, vertex3.get());

  posegraph_.addVertex(std::move(vertex2));
  posegraph_.addVertex(std::move(vertex3));

  // Reproject landmark[0].
  addKeypointToVertex(G_p_fi0_, landmark_ids_[0], vertex0.get());
  addKeypointToVertex(G_p_fi0_, landmark_ids_[0], vertex1.get());

  posegraph_.addVertex(std::move(vertex0));
  posegraph_.addVertex(std::move(vertex1));
}

Eigen::Vector2d ViwlsGraph::calculateErrors() {
  Eigen::Vector2d error;

  Eigen::Vector3d C0_p_fi0 = C0_T_G_ * G_p_fi0_;
  Eigen::Vector3d C0_p_fi1 = C0_T_G_ * G_p_fi1_;
  Eigen::Vector3d C1_p_fi0 = C1_T_G_ * G_p_fi0_;
  Eigen::Vector3d C1_p_fi1 = C1_T_G_ * G_p_fi1_;

  Eigen::Vector2d reprojectionC0_fi0, reprojectionC0_fi1, reprojectionC1_fi0,
      reprojectionC1_fi1;

  cameras_->getCamera(kVisualFrameIndex)
      .project3(C0_p_fi0, &reprojectionC0_fi0);
  cameras_->getCamera(kVisualFrameIndex)
      .project3(C1_p_fi0, &reprojectionC1_fi0);
  cameras_->getCamera(kVisualFrameIndex)
      .project3(C0_p_fi1, &reprojectionC0_fi1);
  cameras_->getCamera(kVisualFrameIndex)
      .project3(C1_p_fi1, &reprojectionC1_fi1);

  error(0) = (reprojectionC0_fi0 - reprojectionC0_fi1).norm();
  error(1) = (reprojectionC1_fi0 - reprojectionC1_fi1).norm();

  return error;
}

TEST_F(ViwlsGraph, LandmarkGeometryVerification) {
  std::vector<double> errors_before;
  std::vector<double> errors_after;

  landmark_geometry_->getMergeReprojectionErrors(
      landmark_ids_[0], landmark_ids_[1], &errors_before, &errors_after);

  EXPECT_EQ(errors_before.size(), 2u);
  EXPECT_EQ(errors_after.size(), 2u);

  EXPECT_DOUBLE_EQ(errors_before[0], 0.0);
  EXPECT_DOUBLE_EQ(errors_before[1], 0.0);

  calculateErrors();

  EXPECT_NEAR(errors_after[0], calculateErrors()(0), 1e-10);
  EXPECT_NEAR(errors_after[1], calculateErrors()(1), 1e-10);
}

TEST_F(ViwlsGraph, LandmarkGeometryVerificationSameLandmark) {
  std::vector<double> errors_before;
  std::vector<double> errors_after;

  landmark_geometry_->getMergeReprojectionErrors(
      landmark_ids_[0], landmark_ids_[0], &errors_before, &errors_after);

  EXPECT_EQ(errors_before.size(), 2u);
  EXPECT_EQ(errors_after.size(), 2u);

  EXPECT_DOUBLE_EQ(errors_before[0], 0.0);
  EXPECT_DOUBLE_EQ(errors_before[1], 0.0);
  EXPECT_DOUBLE_EQ(errors_after[0], 0.0);
  EXPECT_DOUBLE_EQ(errors_after[1], 0.0);
}

}  // namespace map_optimization_legacy

MAPLAB_UNITTEST_ENTRYPOINT


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017.
// Modifications copyright (c) 2017, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_MULTI_ALGORITHMS_DISSOLVE_HPP
#define BOOST_GEOMETRY_EXTENSIONS_MULTI_ALGORITHMS_DISSOLVE_HPP

#include <vector>

#include <boost/range.hpp>

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

#include <boost/geometry/algorithms/union.hpp>

#include <boost/geometry/extensions/algorithms/dissolve.hpp>
#include <boost/geometry/extensions/algorithms/detail/overlay/dissolver.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace dissolve
{

template <typename Multi, typename GeometryOut, bool Reverse>
struct dissolve_multi
{
    template
    <
        typename RescalePolicy, typename OutputIterator,
        typename Strategy, typename Visitor
    >
    static inline OutputIterator apply(Multi const& multi,
            RescalePolicy const& rescale_policy, OutputIterator out,
            Strategy const& strategy, Visitor& visitor)
    {
        typedef typename boost::range_value<Multi>::type polygon_type;
        typedef typename boost::range_iterator<Multi const>::type iterator_type;

        // Step 1: dissolve all polygons in the multi-polygon, independantly
        std::vector<GeometryOut> step1;
        for (iterator_type it = boost::begin(multi);
            it != boost::end(multi);
            ++it)
        {
            dissolve_polygon
                <
                    polygon_type,
                    GeometryOut,
                    Reverse
                >::apply(*it, rescale_policy, std::back_inserter(step1),
                         strategy, visitor);
        }

        // Step 2: remove mutual overlap
        {
            std::vector<GeometryOut> step2; // TODO avoid this, output to "out", if possible
            detail::dissolver::dissolver_generic
                <
                    detail::dissolver::plusmin_policy
                >::apply(step1, rescale_policy, step2, strategy);
            for (typename std::vector<GeometryOut>::const_iterator it = step2.begin();
                it != step2.end(); ++it)
            {
                *out++ = *it;
            }
        }

        return out;
    }
};

// Dissolving multi-linestring is currently moved to extensions/algorithms/connect,
// because it is actually different from dissolving of polygons.
// To be decided what the final behaviour/name is.

}} // namespace detail::dissolve
#endif

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template<typename Multi, typename GeometryOut, bool Reverse>
struct dissolve<Multi, GeometryOut, Reverse, multi_polygon_tag, polygon_tag>
    : detail::dissolve::dissolve_multi<Multi, GeometryOut, Reverse>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_MULTI_ALGORITHMS_DISSOLVE_HPP


#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <cassert>
#include <cfenv>

#include <complex>
#include <iomanip>
#include <iostream>
#include <vector>

#include <armadillo>

#ifdef HAVE_OMP
#include <omp.h>
#endif

#include "pme.h"
#include "abort_unless.h"
#include "energies.h"

using namespace std;
using namespace arma;

const complex<double> I(0.0, 1.0);

ewald::exclusion_vector exclusions;

static bool excluded(size_t i, size_t j);

int main(int argc, char* argv[]) {
  if (argc < 8) {
    cout << "Usage: " << argv[0]
         << " dimension cell-file positions-file charges-file cut-off tolerance num-shells\n";
    return EXIT_FAILURE;
  }

  const long dim0 = atoi(argv[1]);
  const long dim1 = dim0;
  const long dim2 = dim0;

  mat::fixed<3, 3> L;
  L.load(argv[2]);
  const mat::fixed<3, 3> LinvT = trans(inv(L));
  L.print();

  mat r;
  r.load(argv[3]);
  abort_unless(r.n_rows == 3);
  const mat x = inv(L) * r;

  vec qq;
  qq.load(argv[4]);
  std::vector<double> q(r.n_cols);
  {
    for (size_t k = 0; k < q.size(); ++k)
      q[k] = qq(k);
  }

  const size_t num_particles = q.size();
  clog << "Read " << num_particles << " particles into memory.\n";

  std::vector<double> coor_x(num_particles);
  std::vector<double> coor_y(num_particles);
  std::vector<double> coor_z(num_particles);
  {
    for (size_t k = 0; k < num_particles; ++k) {
      coor_x[k] = r(0, k);
      coor_y[k] = r(1, k);
      coor_z[k] = r(2, k);
    }
  }

  const double cut_off = atof(argv[5]);
  const double cut_off2 = cut_off * cut_off;
  const double tolerance = atof(argv[6]);
  clog << "Real space cut-off: " << cut_off << "\n"
       << "Real space tolerance: " << tolerance << "\n\n";

  exclusions = ewald::exclusion_vector(num_particles);
  {                                     // Populate exclusion vector.
/*
    for (size_t i = 0; i < num_particles; ++i)
      for (size_t j = 0; j < num_particles; ++j)
        // if (i != j && ((i % 2) != (j % 2))) {
        if (abs(int(i) - int(j)) <= 2) {
          exclusions[i].push_back(j);
        }
*/
    /*
    for (size_t i = 0; i < num_particles; ++i) {
      std::cout << i << ": ";
      ewald::index_vector const& is = exclusions[i];
      for (size_t j = 0; j < is.size(); ++j)
        std::cout << is[j] << " ";
      std::cout << std::endl;
    }
    */
  }

  const double max_shell = int(atof(argv[7]));

  mat forces = zeros<mat>(3, num_particles);
  mat force_cutoff = zeros<mat>(3, num_particles);
  mat force_direct = zeros<mat>(3, num_particles);
  mat force_recip = zeros<mat>(3, num_particles);
  mat force_excluded = zeros<mat>(3, num_particles);
  mat force_excluded_pme = zeros<mat>(3, num_particles);

  std::vector<double> force_x(num_particles, 0.0);
  std::vector<double> force_y(num_particles, 0.0);
  std::vector<double> force_z(num_particles, 0.0);
  mat force_recip_pme = zeros<mat>(3, num_particles);
  double LL[3 * 3];
  for (size_t i = 0; i < 3; ++i)
    for (size_t j = 0; j < 3; ++j)
      LL[3*i+j] = L(i, j);

  energies energy_pme, energy_ewald;

  wall_clock timer;
  timer.tic();

#ifdef HAVE_OMP
  ewald::pme p(dim0, dim1, dim2, LL, num_particles, &q[0], cut_off, tolerance, omp_get_max_threads());
#else
  ewald::pme p(dim0, dim1, dim2, LL, num_particles, &q[0], cut_off, tolerance, 1);
#endif
  double seconds = timer.toc();

  clog << "PME initialization took " << seconds << " seconds.\n\n";

  const double t = p.get_threshold();
  if (std::isnan(t)) {
    cerr << "Unable to run PME for the prescribed values of cut off and tolerance.\n";
    return EXIT_FAILURE;
  }

  timer.tic();

  energy_pme.recip = p.energy(&coor_x[0], &coor_y[0], &coor_z[0],
                              &force_x[0], &force_y[0], &force_z[0]);

  for (size_t k = 0; k < num_particles; ++k) {
    force_recip_pme(0, k) = force_x[k];
    force_recip_pme(1, k) = force_y[k];
    force_recip_pme(2, k) = force_z[k];
  }

  seconds = timer.toc();
  clog << "PME took " << seconds << " seconds.\n\n";

  std::vector<double> force_excl_x(num_particles, 0.0);
  std::vector<double> force_excl_y(num_particles, 0.0);
  std::vector<double> force_excl_z(num_particles, 0.0);

  energy_pme.masked = p.excluded(exclusions,
                                 &coor_x[0],
                                 &coor_y[0],
                                 &coor_z[0],
                                 &force_excl_x[0],
                                 &force_excl_y[0],
                                 &force_excl_z[0]);

  for (size_t k = 0; k < num_particles; ++k) {
    force_excluded_pme(0, k) = force_excl_x[k];
    force_excluded_pme(1, k) = force_excl_y[k];
    force_excluded_pme(2, k) = force_excl_z[k];
  }

  energy_ewald.extra = energy_pme.extra = p.energy_extra();
  energy_ewald.self = energy_pme.self = p.energy_self();

  double Eexhaustive = 0.0;
  double Ecutoff = 0.0;

  mat force_ewald, force_pme;

  double elapsed = 0.0;

  for (int n = 0; n <= max_shell; n++) {
    wall_clock timer;
    timer.tic();
    for (int nx = -n; nx <= n; nx++) {
      for (int ny = -n; ny <= n; ny++) {
        for (int nz = -n; nz <= n; nz++) {
          if (abs(nx) != n && abs(ny) != n && abs(nz) != n)
            continue;

          const bool origin = (n == 0);

          const vec::fixed<3> m = { double(nx), double(ny), double(nz) };
          const vec::fixed<3> Lm = L * m;
          const vec::fixed<3> LinvTm = LinvT * m;

          // Compute direct space contribution.
          for (size_t i = 0; i < num_particles; i++) {
            for (size_t j = 0; j < num_particles; j++) {
              if (origin && i == j)
                continue;

              const vec::fixed<3> v = r.col(i) - r.col(j) + Lm;
              const double r2 = dot(v, v);
              const double r6 = r2 * r2 * r2;
              const double r8 = r6 * r2;

              const double qij = q[i] * q[j];

              const bool is_excluded = excluded(i, j);

              const double ener = 0.5 * qij / r6;
              const vec::fixed<3> f = 6.0 * qij / r8 * v;

              if (origin) {
                if (!is_excluded) {     // Unmasked pair.
                  Eexhaustive  += ener;
                  forces.col(i) += f;

                  if (r2 < cut_off2) {
                    Ecutoff += ener;
                    force_cutoff.col(i) += f;

                    vec::fixed<3> dv;
                    const double pot = 0.5 * qij
                        * p.direct_convergence_term(v.memptr(), dv.memptr());
                    energy_pme.direct += pot;
                    energy_ewald.direct += pot;
                    force_direct.col(i) += -qij * dv;
                  }
                } else {                // Masked pair.
                  // Note that we don't do cut offs here because we
                  // want to cancel the Fourier space contribution,
                  // which does not involve cut offs.
                  vec::fixed<3> dv;
                  energy_ewald.masked += (ener - 0.5 * qij
                                          * p.direct_convergence_term(v.memptr(),
                                                                      dv.memptr()));
                  force_excluded.col(i) += f + qij * dv;
                }
              } else {                  // Outer shells.
                Eexhaustive  += ener;
                forces.col(i) += f;

                if (r2 < cut_off2) {
                  Ecutoff += ener;
                  force_cutoff.col(i) += f;

                  vec::fixed<3> dv;
                  const double pot = 0.5 * qij
                      * p.direct_convergence_term(v.memptr(), dv.memptr());
                  energy_pme.direct += pot;
                  energy_ewald.direct += pot;
                  force_direct.col(i) += -qij * dv;
                }
              }
            }
          }

          // Compute contribution from Fourier space.
          if (!origin) {
            complex<double> rho(0.0, 0.0);

            for (size_t i = 0; i < num_particles; i++)
              rho += q[i] * exp(2.0 * M_PI * I * dot(m, x.col(i)));

            const double h2 = dot(LinvTm, LinvTm);
            energy_ewald.recip += p.recip_convergence_term(h2, rho);

            // force_recip.col(0) = -4.0 * M_PI * q[0] * q[1] * p.psi(h2) * sin(2.0 * M_PI * dot(LinvTm, r.col(0) - r.col(1))) * (-m);
            // force_recip.col(1) = -4.0 * M_PI * q[0] * q[1] * p.psi(h2) * sin(2.0 * M_PI * dot(LinvTm, r.col(0) - r.col(1))) * m;
          }
        }
      }
    }

    elapsed += timer.toc();

    force_ewald = force_direct + force_recip;
    force_pme   = force_direct + force_recip_pme - force_excluded_pme;

    {
      const double pme_total   = energy_pme.total();
      const double ewald_total = energy_ewald.total();

      const double rel_error_ewald_vs_pme    = fabsl(ewald_total - pme_total) / fabsl(ewald_total);
      const double rel_error_ewald_vs_cutoff = fabsl(ewald_total - Ecutoff) / fabsl(ewald_total);

      const double rel_error_exhaustive_vs_cutoff = fabsl(Eexhaustive - Ecutoff) / fabsl(Eexhaustive);
      const double rel_error_exhaustive_vs_ewald  = fabsl(Eexhaustive - ewald_total) / fabsl(Eexhaustive);
      const double rel_error_exhaustive_vs_pme    = fabsl(Eexhaustive - pme_total) / fabsl(Eexhaustive);

      cout << "==============================================\n"
           << fixed << setprecision(4)
           << "Shell " << n << " (" << elapsed << " seconds)" << "\n"
           << "==============================================\n"
           << scientific << setprecision(9)
           << "\n"
           << "Eexhaustive = " << Eexhaustive << "\n"
           << "Ecutoff     = " << Ecutoff << "\n"
           << "Ewald       = " << energy_ewald << "\n"
           << "PME         = " << energy_pme << "\n"
           << "\n"
           << "Abs. error in excluded forces = " << norm(force_excluded - force_excluded_pme, "inf") << "\n"
           << "\n"
           << "Abs. error in forces (Exhaustive vs. cutoff) = " << norm(forces - force_cutoff, "inf") << "\n"
           << "Abs. error in forces (Exhaustive vs. PME)    = " << norm(forces - force_pme, "inf") << "\n"
           << "\n"
           << "Rel. error in total energies (Ewald vs. cutoff) = " << rel_error_ewald_vs_cutoff << "\n"
           << "Rel. error in total energies (Ewald vs. PME)    = " << rel_error_ewald_vs_pme << "\n"
           << "\n"
           << "Rel. error in total energies (Exhaustive vs. cutoff) = " << rel_error_exhaustive_vs_cutoff << "\n"
           << "Rel. error in total energies (Exhaustive vs. Ewald)  = " << rel_error_exhaustive_vs_ewald << "\n"
           << "Rel. error in total energies (Exhaustive vs. PME)    = " << rel_error_exhaustive_vs_pme << "\n"
           << endl;
    }
  }

  return 0;
}

bool excluded(size_t i, size_t j) {
  // assert(i < num_particles);
  // assert(j < num_particles);

  ewald::index_vector const& is = exclusions[i];
  for (size_t k = 0; k < is.size(); ++k)
    if (j == is[k])
      return true;

  return false;
}


// Copyright 2016 Michel Koenen (Falcons)
// SPDX-License-Identifier: Apache-2.0
/*
 * cObjectPath.cpp
 *
 *  Created on: Feb 18, 2015
 *      Author: Michel Koenen
 */

#include "int/cObjectPath.hpp"

#include <boost/geometry/algorithms/intersection.hpp>

#include "int/stores/fieldDimensionsStore.hpp"

using namespace teamplay;

cObjectPath::cObjectPath(Point3D &position, Vector3D &speed)
{
    myPosition.x=position.x;
    myPosition.y=position.y;
    mySpeed.x=speed.x;
    mySpeed.y=speed.y;

    // y= slope*x + intercept;
    // intercept= y - slope*x
    slope=mySpeed.y/mySpeed.x;
    intercept=myPosition.y - slope * myPosition.x;
}

float cObjectPath::getX()
{
	return myPosition.x;
}

float cObjectPath::getY()
{
	return myPosition.y;
}

float cObjectPath::getSpeedX()
{
	return mySpeed.x;
}

float cObjectPath::getSpeedY()
{
	return mySpeed.y;
}

float cObjectPath::calcX( float forY )
{
	// x= (y-intercept)/slope
	return ( ( forY - intercept )/slope );
}

float cObjectPath::calcY( float forX )
{
    // y= slope*x + intercept;
	return ( slope * forX + intercept );
}

float cObjectPath::getYIntercept()
{
	return intercept;
}

float cObjectPath::getSlope()
{
	return slope;
}

/*! return the extrapolated coordinate (1 meter) outside the field where the object is moving to
 *
 * @return coordinate
 */
Point2D cObjectPath::getTowardsCoordinate()
{
	float objectTowardsX, objectTowardsY;

    auto fieldWidth = fieldDimensionsStore::getFieldDimensions().getWidth();
    auto fieldLength = fieldDimensionsStore::getFieldDimensions().getLength();

    if( mySpeed.x < 0.0 )
    {
    	objectTowardsX = -fieldWidth/2.0 - 1.0;
    	objectTowardsY = calcY( objectTowardsX );
    }
    else if ( mySpeed.x > 0.0 )
    {
    	objectTowardsX =  fieldWidth/2.0 + 1.0;
    	objectTowardsY = calcY( objectTowardsX );
    }
    else
    {
    	objectTowardsX = myPosition.x;
    	if ( mySpeed.y > 0.0 )
    	{
    		objectTowardsY = fieldLength/2.0 + 1.0;
    	}
    	else if ( mySpeed.y < 0.0)
    	{
    		objectTowardsY = -fieldLength/2.0 - 1.0;
    	}
    	else
    	{
    		objectTowardsY = myPosition.y;
    	}
    }

    Point2D objectMovingTowardsCoordinate( objectTowardsX, objectTowardsY);

    return objectMovingTowardsCoordinate;

}

/*! check if the line segment x1,y1 to x2,y2 intersects with the line which is followed by the object
 *
 * @param[in] lineStart coord of line segment start
 * @param[in] lineEnd coord of line segment end
 * @return true if the two linesegments intersect
 */
bool cObjectPath::intersectCheck( Point2D lineStart, Point2D lineEnd)
{
	bool b;

    Point2D oMTP = getTowardsCoordinate();

    //translate to boost variable types Point and Segment
    Point objectMovingTowardsPosition( oMTP.x, oMTP.y );

    Segment objectLine( Point( myPosition.x, myPosition.y), objectMovingTowardsPosition);
    Segment line( Point(lineStart.x,lineStart.y), Point(lineEnd.x,lineEnd.y));

	b = boost::geometry::intersects(objectLine, line);
	return b;

}

/*! check if the line segment x1,y1 to x2,y2 intersects with the line which is followed by the object
 *
 * @param[in] lineStart coord of line segment start
 * @param[in] lineEnd coord of line segment end
 * @param[out] intersect coord of intersection point, only valid when function returns true!
 * @return true if the two linesegments intersect
 */
bool cObjectPath::intersectCheck( Point2D lineStart, Point2D lineEnd, Point2D &intersectCoord)
{
    Point2D oMTP = getTowardsCoordinate();
    //translate to boost variable types Point and Segment
    Point objectMovingTowardsPosition( oMTP.x, oMTP.y );

    Segment objectLine( Point( myPosition.x, myPosition.y), objectMovingTowardsPosition);
    Segment line( Point(lineStart.x,lineStart.y), Point(lineEnd.x,lineEnd.y));

	std::vector<Point> output;
	boost::geometry::intersection(objectLine, line, output);

	if ( output.size() == 1 )
	{
		intersectCoord.x=boost::geometry::get<0>( output[0] ); // get first field representing x of intersect
		intersectCoord.y=boost::geometry::get<1>( output[0] ); // get second field representing y of intersect
		return true;
	}
	return false;

}

/*! return the point on objectLine which is perpendicular to the position of otherObject
 *
 * @param[in] otherObject coord of the other object which wants to have a coordinate for interception
 * @param[out] valid true if the coordinate is on the line in the moving direction of the object
 * @return coordinate target on line
 */
Point2D cObjectPath::getPerpendicularInterceptionPoint( Point2D otherObject,  bool &valid )
{
    Point2D objectMovingTowardsPosition= getTowardsCoordinate();

    // formula found on http://arstechnica.com/civis/viewtopic.php?f=26&t=149128
    float u=( (otherObject.x - myPosition.x ) * ( objectMovingTowardsPosition.x - myPosition.x) +
		      (otherObject.y - myPosition.y ) * ( objectMovingTowardsPosition.y - myPosition.y) );
	float dist=calc_distance( Point2D( myPosition.x,myPosition.y), objectMovingTowardsPosition);

	u= u/(dist*dist);

	Point2D target(0.0, 0.0);

	target.x=myPosition.x + u * ( objectMovingTowardsPosition.x - myPosition.x);
	target.y=myPosition.y + u * ( objectMovingTowardsPosition.y - myPosition.y);

    valid=true;

	if ( mySpeed.x > 0.0 )
	{
		if ( target.x < myPosition.x )
			valid=false;
	}
	else if ( mySpeed.x < 0.0)
	{
		if ( target.x > myPosition.x )
			valid=false;
	}

	if( valid )
	{
		if ( mySpeed.y > 0.0)
		{
			if ( target.y < myPosition.y )
				valid=false;
		}
		else if ( mySpeed.y < 0.0)
		{
			if ( target.y > myPosition.y)
				valid=false;
		}
	}

    return target;
}


/* ============================================================================
* Copyright (c) 2009-2016 BlueQuartz Software, LLC
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of BlueQuartz Software, the US Air Force, nor the names of its
* contributors may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
* USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
* The code contained herein was partially funded by the followig contracts:
*    United States Air Force Prime Contract FA8650-07-D-5800
*    United States Air Force Prime Contract FA8650-10-D-5210
*    United States Prime Contract Navy N00173-07-C-2068
*
* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

#include "ComputeUmeyamaTransform.h"

#include "SIMPLib/Common/Constants.h"
#include "SIMPLib/FilterParameters/AbstractFilterParametersReader.h"
#include "SIMPLib/FilterParameters/BooleanFilterParameter.h"
#include "SIMPLib/FilterParameters/DataContainerSelectionFilterParameter.h"
#include "SIMPLib/FilterParameters/LinkedPathCreationFilterParameter.h"
#include "SIMPLib/FilterParameters/SeparatorFilterParameter.h"
#include "SIMPLib/FilterParameters/StringFilterParameter.h"
#include "SIMPLib/Geometry/EdgeGeom.h"
#include "SIMPLib/Geometry/IGeometry2D.h"
#include "SIMPLib/Geometry/IGeometry3D.h"
#include "SIMPLib/Geometry/VertexGeom.h"

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include "DREAM3DReview/DREAM3DReviewConstants.h"
#include "DREAM3DReview/DREAM3DReviewVersion.h"

/* Create Enumerations to allow the created Attribute Arrays to take part in renaming */
enum createdPathID : RenameDataPath::DataID_t
{
  AttributeMatrixID21 = 21,

  DataArrayID30 = 30,
  DataArrayID31 = 31,
};

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
ComputeUmeyamaTransform::ComputeUmeyamaTransform()
: m_SourcePointSet("")
, m_DestPointSet("")
, m_UseScaling(false)
, m_TransformationAttributeMatrixName("TransformationData")
, m_TransformationMatrixName("TransformationMatrix")
{
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
ComputeUmeyamaTransform::~ComputeUmeyamaTransform()
= default;

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void ComputeUmeyamaTransform::setupFilterParameters()
{
  FilterParameterVectorType parameters;
  parameters.push_back(SIMPL_NEW_BOOL_FP("Use Scaling", UseScaling, FilterParameter::Parameter, ComputeUmeyamaTransform));
  DataContainerSelectionFilterParameter::RequirementType dcReq;
  dcReq.dcGeometryTypes = {IGeometry::Type::Vertex, IGeometry::Type::Edge, IGeometry::Type::Triangle, IGeometry::Type::Quad, IGeometry::Type::Tetrahedral};
  parameters.push_back(SIMPL_NEW_DC_SELECTION_FP("Moving Geometry", SourcePointSet, FilterParameter::RequiredArray, ComputeUmeyamaTransform, dcReq));
  parameters.push_back(SIMPL_NEW_DC_SELECTION_FP("Fixed Geometry", DestPointSet, FilterParameter::RequiredArray, ComputeUmeyamaTransform, dcReq));
  parameters.push_back(SeparatorFilterParameter::New("Transformation Data", FilterParameter::CreatedArray));
  parameters.push_back(SIMPL_NEW_AM_WITH_LINKED_DC_FP("Transformation Attribute Matrix", TransformationAttributeMatrixName, SourcePointSet, FilterParameter::CreatedArray, ComputeUmeyamaTransform));
  parameters.push_back(SIMPL_NEW_DA_WITH_LINKED_AM_FP("Transformation Matrix", TransformationMatrixName, SourcePointSet, TransformationAttributeMatrixName, FilterParameter::CreatedArray, ComputeUmeyamaTransform));
  setFilterParameters(parameters);
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void ComputeUmeyamaTransform::readFilterParameters(AbstractFilterParametersReader* reader, int index)
{
  reader->openFilterGroup(this, index);
  setSourcePointSet(reader->readDataArrayPath("SourcePointSet", getSourcePointSet()));
  setDestPointSet(reader->readDataArrayPath("DestPointSet", getDestPointSet()));
  setUseScaling(reader->readValue("UseScaling", getUseScaling()));
  setTransformationAttributeMatrixName(reader->readString("TransformationAttributeMatrixName", getTransformationAttributeMatrixName()));
  setTransformationMatrixName(reader->readString("TransformationMatrixName", getTransformationMatrixName()));
  reader->closeFilterGroup();
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void ComputeUmeyamaTransform::initialize()
{
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void ComputeUmeyamaTransform::dataCheck()
{
  clearErrorCode();
  clearWarningCode();

  IGeometry::Pointer movingGeom = getDataContainerArray()->getPrereqGeometryFromDataContainer<IGeometry, AbstractFilter>(this, getSourcePointSet());
  IGeometry::Pointer fixedGeom = getDataContainerArray()->getPrereqGeometryFromDataContainer<IGeometry, AbstractFilter>(this, getDestPointSet());

  if(getErrorCode() < 0)
  {
    return;
  }

  if(!std::dynamic_pointer_cast<IGeometry2D>(movingGeom) && !std::dynamic_pointer_cast<IGeometry3D>(movingGeom) && !std::dynamic_pointer_cast<VertexGeom>(movingGeom) &&
     !std::dynamic_pointer_cast<EdgeGeom>(movingGeom))
  {
    QString ss = QObject::tr("Moving Geometry must be an unstructured geometry (Vertex, Edge, Triangle, Quadrilateral, or Tetrahedral), but the type is %1").arg(movingGeom->getGeometryTypeAsString());
    setErrorCondition(-702, ss);
  }

  if(!std::dynamic_pointer_cast<IGeometry2D>(fixedGeom) && !std::dynamic_pointer_cast<IGeometry3D>(fixedGeom) && !std::dynamic_pointer_cast<VertexGeom>(fixedGeom) &&
     !std::dynamic_pointer_cast<EdgeGeom>(fixedGeom))
  {
    QString ss = QObject::tr("Fixed Geometry must be an unstructured geometry (Vertex, Edge, Triangle, Quadrilateral, or Tetrahedral), but the type is %1").arg(fixedGeom->getGeometryTypeAsString());
    setErrorCondition(-702, ss);
  }

  size_t numMovingVertices = 0;
  size_t numFixedVertices = 0;

  if(IGeometry2D::Pointer igeom2D = std::dynamic_pointer_cast<IGeometry2D>(movingGeom))
  {
    numMovingVertices = igeom2D->getNumberOfVertices();
  }
  else if(IGeometry3D::Pointer igeom3D = std::dynamic_pointer_cast<IGeometry3D>(movingGeom))
  {
    numMovingVertices = igeom3D->getNumberOfVertices();
  }
  else if(VertexGeom::Pointer vertex = std::dynamic_pointer_cast<VertexGeom>(movingGeom))
  {
    numMovingVertices = vertex->getNumberOfVertices();
  }
  else if(EdgeGeom::Pointer edge = std::dynamic_pointer_cast<EdgeGeom>(movingGeom))
  {
    numMovingVertices = edge->getNumberOfVertices();
  }

  if(IGeometry2D::Pointer igeom2D = std::dynamic_pointer_cast<IGeometry2D>(fixedGeom))
  {
    numFixedVertices = igeom2D->getNumberOfVertices();
  }
  else if(IGeometry3D::Pointer igeom3D = std::dynamic_pointer_cast<IGeometry3D>(fixedGeom))
  {
    numFixedVertices = igeom3D->getNumberOfVertices();
  }
  else if(VertexGeom::Pointer vertex = std::dynamic_pointer_cast<VertexGeom>(fixedGeom))
  {
    numFixedVertices = vertex->getNumberOfVertices();
  }
  else if(EdgeGeom::Pointer edge = std::dynamic_pointer_cast<EdgeGeom>(fixedGeom))
  {
    numFixedVertices = edge->getNumberOfVertices();
  }

  if(numMovingVertices != numFixedVertices)
  {
    QString ss = QObject::tr("The moving and fixed Geometries must have the same number of Vertices; the number of moving Vertices is %1 and the number of fixed Vertices is %2")
                     .arg(numMovingVertices)
                     .arg(numFixedVertices);
    setErrorCondition(-11000, ss);
  }

  DataContainer::Pointer m = getDataContainerArray()->getPrereqDataContainer(this, getSourcePointSet());

  if(getErrorCode() < 0)
  {
    return;
  }

  std::vector<size_t> tDims(1, 1);
  m->createNonPrereqAttributeMatrix(this, getTransformationAttributeMatrixName(), tDims, AttributeMatrix::Type::Generic, AttributeMatrixID21);

  std::vector<size_t> cDims(2, 4);
  DataArrayPath path(getSourcePointSet().getDataContainerName(), getTransformationAttributeMatrixName(), getTransformationMatrixName());

  m_TransformationMatrixPtr = getDataContainerArray()->createNonPrereqArrayFromPath<DataArray<float>, AbstractFilter, float>(this, path, 0, cDims, "", DataArrayID31);
  if(nullptr != m_TransformationMatrixPtr.lock()) /* Validate the Weak Pointer wraps a non-nullptr pointer to a DataArray<T> object */
  {
    m_TransformationMatrix = m_TransformationMatrixPtr.lock()->getPointer(0);
  } /* Now assign the raw pointer to data from the DataArray<T> object */
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void ComputeUmeyamaTransform::preflight()
{
  // These are the REQUIRED lines of CODE to make sure the filter behaves correctly
  setInPreflight(true);              // Set the fact that we are preflighting.
  emit preflightAboutToExecute();    // Emit this signal so that other widgets can do one file update
  emit updateFilterParameters(this); // Emit this signal to have the widgets push their values down to the filter
  dataCheck();                       // Run our DataCheck to make sure everthing is setup correctly
  emit preflightExecuted();          // We are done preflighting this filter
  setInPreflight(false);             // Inform the system this filter is NOT in preflight mode anymore.
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void ComputeUmeyamaTransform::execute()
{
  clearErrorCode();
  clearWarningCode();
  dataCheck();
  if(getErrorCode() < 0)
  {
    return;
  }

  IGeometry::Pointer movingGeom = getDataContainerArray()->getDataContainer(m_SourcePointSet)->getGeometry();
  IGeometry::Pointer fixedGeom = getDataContainerArray()->getDataContainer(m_DestPointSet)->getGeometry();

  float* movingPointsPtr = nullptr;
  float* fixedPointsPtr = nullptr;
  size_t numMovingVertices = 0;
  size_t numFixedVertices = 0;

  if(IGeometry2D::Pointer igeom2D = std::dynamic_pointer_cast<IGeometry2D>(movingGeom))
  {
    movingPointsPtr = igeom2D->getVertexPointer(0);
    numMovingVertices = igeom2D->getNumberOfVertices();
  }
  else if(IGeometry3D::Pointer igeom3D = std::dynamic_pointer_cast<IGeometry3D>(movingGeom))
  {
    movingPointsPtr = igeom3D->getVertexPointer(0);
    numMovingVertices = igeom3D->getNumberOfVertices();
  }
  else if(VertexGeom::Pointer vertex = std::dynamic_pointer_cast<VertexGeom>(movingGeom))
  {
    movingPointsPtr = vertex->getVertexPointer(0);
    numMovingVertices = vertex->getNumberOfVertices();
  }
  else if(EdgeGeom::Pointer edge = std::dynamic_pointer_cast<EdgeGeom>(movingGeom))
  {
    movingPointsPtr = edge->getVertexPointer(0);
    numMovingVertices = edge->getNumberOfVertices();
  }

  if(IGeometry2D::Pointer igeom2D = std::dynamic_pointer_cast<IGeometry2D>(fixedGeom))
  {
    fixedPointsPtr = igeom2D->getVertexPointer(0);
    numFixedVertices = igeom2D->getNumberOfVertices();
  }
  else if(IGeometry3D::Pointer igeom3D = std::dynamic_pointer_cast<IGeometry3D>(fixedGeom))
  {
    fixedPointsPtr = igeom3D->getVertexPointer(0);
    numFixedVertices = igeom3D->getNumberOfVertices();
  }
  else if(VertexGeom::Pointer vertex = std::dynamic_pointer_cast<VertexGeom>(fixedGeom))
  {
    fixedPointsPtr = vertex->getVertexPointer(0);
    numFixedVertices = vertex->getNumberOfVertices();
  }
  else if(EdgeGeom::Pointer edge = std::dynamic_pointer_cast<EdgeGeom>(fixedGeom))
  {
    fixedPointsPtr = edge->getVertexPointer(0);
    numFixedVertices = edge->getNumberOfVertices();
  }

  // Eigen does best with things in column major, but we'll ultimately want
  // the matrix as row major; perform the actual computation in column
  // major and then transpose when we're ready to move it into a DataArray
  typedef Eigen::Matrix<float, 3, Eigen::Dynamic, Eigen::ColMajor> PointCloud;
  typedef Eigen::Matrix<float, 4, 4, Eigen::ColMajor> UmeyamaTransform;

  // Map our DataArray pointers directly to Eigen objects in memory to avoid making copies
  Eigen::Map<PointCloud> moving(movingPointsPtr, 3, numMovingVertices);
  Eigen::Map<PointCloud> fixed(fixedPointsPtr, 3, numFixedVertices);

  UmeyamaTransform transformMatrix = Eigen::umeyama(moving, fixed, m_UseScaling);

  // The matrix is in column major, transpose it in place to make it row major
  transformMatrix.transposeInPlace();
  float* umeyamaArray = &transformMatrix(0);

  // Copy the transformation matrix over to our DataArray
  // We know the size will be exactly sixteen...
  for(size_t i = 0; i < 16; i++)
  {
    m_TransformationMatrix[i] = umeyamaArray[i];
  }

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
AbstractFilter::Pointer ComputeUmeyamaTransform::newFilterInstance(bool copyFilterParameters) const
{
  ComputeUmeyamaTransform::Pointer filter = ComputeUmeyamaTransform::New();
  if(copyFilterParameters)
  {
    copyFilterParameterInstanceVariables(filter.get());
  }
  return filter;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
const QString ComputeUmeyamaTransform::getCompiledLibraryName() const
{
  return DREAM3DReviewConstants::DREAM3DReviewBaseName;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
const QString ComputeUmeyamaTransform::getBrandingString() const
{
  return "DREAM3DReview";
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
const QString ComputeUmeyamaTransform::getFilterVersion() const
{
  QString version;
  QTextStream vStream(&version);
  vStream << DREAM3DReview::Version::Major() << "." << DREAM3DReview::Version::Minor() << "." << DREAM3DReview::Version::Patch();
  return version;
}
// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
const QString ComputeUmeyamaTransform::getGroupName() const
{
  return DREAM3DReviewConstants::FilterGroups::DREAM3DReviewFilters;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
const QUuid ComputeUmeyamaTransform::getUuid()
{
  return QUuid("{3192d494-d1ec-5ee7-a345-e9963f02aaab}");
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
const QString ComputeUmeyamaTransform::getSubGroupName() const
{
  return DREAM3DReviewConstants::FilterSubGroups::RegistrationFilters;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
const QString ComputeUmeyamaTransform::getHumanLabel() const
{
  return "Compute Umeyama Transform";
}


#include <train_utils.h>
#include <visualization.h>
#include <opencv2/opencv.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>
#include <boost/functional/hash.hpp>
#include <limits>
#include <math.h>
#include <picosha2.h>
#ifdef __ANDROID__
#include <android/log.h>
#include <omp.h>
#else
#include <randomkit.h>
#endif

using namespace std;
using namespace caffe;
using namespace cv;

Mat mean_init();

Mat core50_mean_image = mean_init(); // 128*128*3 BGR (8 bit per canale)

Mat mean_init() {
    Mat mean(128, 128, CV_32FC3);
    auto mean_data = (float*) mean.data;

    for (int size = 0; size < 3 * 128 * 128; size += 3) { // 3 = channels
        mean_data[size + 0] = 104;
        mean_data[size + 1] = 117;
        mean_data[size + 2] = 123;
    }

    return mean;
}

void extract_minibatch_size_from_prototxt_with_input_layers(
        const boost::shared_ptr<Net<float>> &train_net,
        const boost::shared_ptr<Net<float>> &test_net,
        int *train_minibatch_size,
        int *test_minibatch_size) {
    if (train_minibatch_size != nullptr) {
        *train_minibatch_size = train_net->input_blobs()[0]->shape()[0];
    }

    if (test_minibatch_size != nullptr) {
        *test_minibatch_size = test_net->input_blobs()[0]->shape()[0];
    }
}

bool net_use_target_vectors(const boost::shared_ptr<Net<float>> &net) {
    return std::find(net->layer_names().begin(), net->layer_names().end(), "target") != net->layer_names().end();
}

void get_data(const string &fpath,
              const string &bpath,
              vector<Mat> &x,
              vector<int> &y,
              const Mat &mean_image) {
    path fp{fpath};
    path bp{bpath};

    x.clear();
    y.clear();
    auto file_relative_paths = read_file_list(fp.make_preferred().string());
    x.reserve(file_relative_paths.size());
    y.reserve(file_relative_paths.size());

    int loaded_n = 0;

    for (auto &line : file_relative_paths) {
        path image_path = bp / line.first;
        Mat image;
        if(!file_exists(image_path.make_preferred().string())) {
            throw "Image not found";
        }
        Mat imagex = imread(image_path.make_preferred().string(), IMREAD_COLOR); // BGR, 8-bit per canale
        imagex.convertTo(image, CV_32FC3); // BGR, 32 bit (float) per canale
        preprocess_image(image, mean_image);
        x.push_back(image);
        y.push_back(line.second);
        loaded_n++;
    }
}

void get_data(const string &fpath,
              const string &bpath,
              vector<Mat> &x,
              vector<int> &y,
              int resizeW, int resizeH,
              const Mat &mean_image) {
    path fp{fpath};
    path bp{bpath};

    x.clear();
    y.clear();
    auto file_relative_paths = read_file_list(fp.make_preferred().string());
    x.reserve(file_relative_paths.size());
    y.reserve(file_relative_paths.size());

    int loaded_n = 0;

    for (auto &line : file_relative_paths) {
        path image_path = bp / line.first;
        //cout << "Image path:" << image_path.make_preferred().string() << endl;
        if(!file_exists(image_path.make_preferred().string())) {
            throw "Image not found";
        }
        Mat image, resized;
        Mat imagex = imread(image_path.make_preferred().string(), IMREAD_COLOR); // BGR, 8-bit per canale
        imagex.convertTo(image, CV_32FC3); // BGR, 32 bit (float) per canale
        resize(image, resized, Size(resizeW, resizeH));

        //cout << resized.rows << " " << resized.cols << " vs " << mean_image.rows << " " << mean_image.cols << endl;
        preprocess_image(resized, mean_image);
        x.push_back(resized);
        y.push_back(line.second);
        loaded_n++;
    }
}

vector<string> read_lines(const string &file_path) {
    vector<string> result;
    ifstream infile(file_path);
    string line;

    while (getline(infile, line)) {
        result.push_back(line);
    }

    return result;
}

vector<pair<string, int>> read_file_list(const string& file_path) {
    vector<string> lines = read_lines(file_path);
    vector<pair<string, int>> result;
    result.reserve(lines.size());

    for(auto & line : lines) {
        string path, label;
        istringstream f(line);
        getline(f, path, ' ');
        getline(f, label, ' ');

        string platform_path = path;
        std::replace(platform_path.begin(), platform_path.end(), '\\', '/');

        result.emplace_back(platform_path, atoi(label.c_str()));
    }

    return result;
}

int count_lines(const std::string &file_path) {
    int result = 0;
    std::ifstream infile(file_path);
    std::string line;
    while (std::getline(infile, line)) {
        result++;
    }

    return result;
}

void preprocess_image(Mat &img, const Mat &mean_image) {
    //Input = immagine BGR, 3 canali, 32 bit per canale, range 0-255
    //# scale each pixel to 255 img = img * 255 -> No
    //# Swap RGB to BRG img = img[:, :, ::-1] -> No
    img -= mean_image;

    //# Swap channel dimension to fit the caffe format (c, h, w)
    //img = np.transpose(img, (2, 0, 1)) -> Non si può fare!
}

void pad_data(std::vector<cv::Mat> &x, std::vector<int> &y, int &it, int mb_size) {
    // computing test_iters
    int n_missing = x.size() % mb_size;
    int surplus;
    if (n_missing > 0) {
        surplus = 1;
    } else {
        surplus = 0;

    }
    it = ((int) x.size() / mb_size) + surplus;

    // # padding data to fix batch dimentions
    if (n_missing > 0) {
        int n_to_add = mb_size - n_missing;
        while(n_to_add > 0) {
            int addition = n_to_add % x.size();
            vector<Mat> x_insert(&x[0], &x[addition]);
            vector<int> y_insert(&y[0], &y[addition]);

            x.insert(x.begin(), x_insert.begin(), x_insert.end());
            y.insert(y.begin(), y_insert.begin(), y_insert.end());
            n_to_add -= addition;
        }
    }
}

vector<int> count_lines_in_batches(int batch_count, const std::string &fpath) {
    vector<int> lines(batch_count);
    for (int batch = 0; batch < batch_count; batch++) {
        string batch_filelist = fpath;
        string replacement = std::to_string(batch);
        if (replacement.size() < 2) {
            replacement = "0" + replacement;
        }

        boost::replace_all(batch_filelist, "XX", replacement);
        lines[batch] = count_lines(batch_filelist);
    }

    return lines;
}

std::vector<std::vector<float>> compute_one_hot_vectors(
        const std::vector<int> &y,
        const boost::shared_ptr<caffe::Net<float>> &net,
        const std::string &prediction_layer) {
    boost::shared_ptr<Blob<float>> prediction_blob = net->blob_by_name(prediction_layer);
    int class_count = prediction_blob->shape()[1];
    return compute_one_hot_vectors_from_class_count(y, class_count);
}

std::vector<std::vector<float>> compute_one_hot_vectors(
        const std::vector<std::shared_ptr<std::vector<int>>> &y,
        const boost::shared_ptr<caffe::Net<float>> &net,
        const std::string &prediction_layer) {
    std::vector<int> y_data;
    for(int i = 0; i < y.size(); i++) {
        y_data.push_back(y[i]->at(0));
    }

    boost::shared_ptr<Blob<float>> prediction_blob = net->blob_by_name(prediction_layer);
    int class_count = prediction_blob->shape()[1];
    return compute_one_hot_vectors_from_class_count(y_data, class_count);
}

vector<vector<float>> compute_one_hot_vectors_from_class_count(const std::vector<int> &y, int class_count) {
    vector<vector<float>> target_y(y.size(), vector<float>(class_count, 0.0f));
    for (int i = 0; i < y.size(); i++) {
        target_y[i][y[i]] = 1.0f;
    }

    return target_y;
}

#ifdef __ANDROID__
void shuffle_in_unison(vector<std::shared_ptr<vector<float>>> &train_x,
                       vector<int> &train_y,
                       vector<vector<float>> &target_y,
                       unsigned long seed) {
    std::mt19937 generator(seed);

    //https://github.com/numpy/numpy/blob/master/numpy/random/mtrand/mtrand.pyx
    for (int i = train_x.size()-1; i >= 1; i--) {
        int j = (int) (generator() % i);
        if (i == j) {
            continue;
        }

        std::shared_ptr<vector<float>> tmpTrainX = train_x[j];
        train_x[j] = train_x[i];
        train_x[i] = tmpTrainX;

        int tmpTrainY = train_y[j];
        train_y[j] = train_y[i];
        train_y[i] = tmpTrainY;

        vector<float> tmpTargetY = target_y[j];
        target_y[j] = target_y[i];
        target_y[i] = tmpTargetY;
    }
}

void shuffle_in_unison(vector<Mat> &train_x,
                       vector<int> &train_y,
                       vector<vector<float>> &target_y,
                       unsigned long seed) {
    std::mt19937 generator(seed);

    //https://github.com/numpy/numpy/blob/master/numpy/random/mtrand/mtrand.pyx
    for (int i = train_x.size()-1; i >= 1; i--) {
        int j = (int) (generator() % i);
        if (i == j) {
            continue;
        }

        auto tmpTrainX = train_x[j];
        train_x[j] = train_x[i];
        train_x[i] = tmpTrainX;

        int tmpTrainY = train_y[j];
        train_y[j] = train_y[i];
        train_y[i] = tmpTrainY;

        vector<float> tmpTargetY = target_y[j];
        target_y[j] = target_y[i];
        target_y[i] = tmpTargetY;
    }
}
#else
void shuffle_in_unison(vector<Mat> &train_x,
                       vector<int> &train_y,
                       vector<vector<float>> &target_y,
                       unsigned long seed) {
    auto state = (rk_state*) malloc(sizeof(rk_state));
    rk_seed(seed, state);
    /*rk_state* rng_state = (rk_state*) malloc(sizeof(rk_state));
    memcpy(rng_state, state, sizeof(rk_state));*/

    //https://github.com/numpy/numpy/blob/master/numpy/random/mtrand/mtrand.pyx
    for (int i = train_x.size()-1; i >= 1; i--) {
        int j = rk_interval(i, state);
        if (i == j) {
            continue;
        }

        Mat tmpTrainX = train_x[j];
        train_x[j] = train_x[i];
        train_x[i] = tmpTrainX;

        int tmpTrainY = train_y[j];
        train_y[j] = train_y[i];
        train_y[i] = tmpTrainY;

        vector<float> tmpTargetY = target_y[j];
        target_y[j] = target_y[i];
        target_y[i] = tmpTargetY;
    }

    free(state);
}

void shuffle_in_unison(std::vector<std::shared_ptr<cv::Mat>> &train_x,
                       std::vector<int> &train_y,
                       std::vector<std::vector<float>> &target_y,
                       unsigned long seed) {
    auto state = (rk_state*) malloc(sizeof(rk_state));
    rk_seed(seed, state);
    /*rk_state* rng_state = (rk_state*) malloc(sizeof(rk_state));
    memcpy(rng_state, state, sizeof(rk_state));*/

    //https://github.com/numpy/numpy/blob/master/numpy/random/mtrand/mtrand.pyx
    for (int i = train_x.size()-1; i >= 1; i--) {
        int j = rk_interval(i, state);
        if (i == j) {
            continue;
        }

        auto tmpTrainX = train_x[j];
        train_x[j] = train_x[i];
        train_x[i] = tmpTrainX;

        int tmpTrainY = train_y[j];
        train_y[j] = train_y[i];
        train_y[i] = tmpTrainY;

        vector<float> tmpTargetY = target_y[j];
        target_y[j] = target_y[i];
        target_y[i] = tmpTargetY;
    }

    free(state);
}

void shuffle_in_unison(vector<std::shared_ptr<vector<float>>> &train_x,
                       vector<int> &train_y,
                       vector<vector<float>> &target_y,
                       unsigned long seed) {
    auto state = (rk_state*) malloc(sizeof(rk_state));
    rk_seed(seed, state);
    /*rk_state* rng_state = (rk_state*) malloc(sizeof(rk_state));
    memcpy(rng_state, state, sizeof(rk_state));*/

    //https://github.com/numpy/numpy/blob/master/numpy/random/mtrand/mtrand.pyx
    for (int i = train_x.size()-1; i >= 1; i--) {
        int j = rk_interval(i, state);
        if (i == j) {
            continue;
        }

        auto tmpTrainX = train_x[j];
        train_x[j] = train_x[i];
        train_x[i] = tmpTrainX;

        int tmpTrainY = train_y[j];
        train_y[j] = train_y[i];
        train_y[i] = tmpTrainY;

        vector<float> tmpTargetY = target_y[j];
        target_y[j] = target_y[i];
        target_y[i] = tmpTargetY;
    }

    free(state);
}

void shuffle_in_unison(std::vector<std::shared_ptr<std::vector<float>>> &train_x,
                       std::vector<std::shared_ptr<std::vector<int>>> &train_y,
                       std::vector<std::vector<float>> &target_y, unsigned long seed) {
    auto state = (rk_state*) malloc(sizeof(rk_state));
    rk_seed(seed, state);
    /*rk_state* rng_state = (rk_state*) malloc(sizeof(rk_state));
    memcpy(rng_state, state, sizeof(rk_state));*/

    //https://github.com/numpy/numpy/blob/master/numpy/random/mtrand/mtrand.pyx
    for (int i = train_x.size()-1; i >= 1; i--) {
        int j = rk_interval(i, state);
        if (i == j) {
            continue;
        }

        auto tmpTrainX = train_x[j];
        train_x[j] = train_x[i];
        train_x[i] = tmpTrainX;

        auto tmpTrainY = train_y[j];
        train_y[j] = train_y[i];
        train_y[i] = tmpTrainY;

        auto tmpTargetY = target_y[j];
        target_y[j] = target_y[i];
        target_y[i] = tmpTargetY;
    }

    free(state);
}
#endif

void feed_image_layer(const boost::shared_ptr<caffe::Net<float>> &net, const std::string &layer_name,
                      const std::vector<cv::Mat> &images, int from, int to, int dest_from) {
    boost::shared_ptr<Blob<float>> input_layer = net->blob_by_name(layer_name);
    int width = input_layer->width();
    int height = input_layer->height();
    int offset_range = to - from;

    #pragma omp parallel for
    for (int input_offset = 0; input_offset < offset_range; input_offset++) {
        int i = input_offset + from;

        vector<Mat> input_channels;
        int inputImageOffset = input_layer->offset(input_offset + dest_from, 0, 0, 0);
        float *input_data = &input_layer->mutable_cpu_data()[inputImageOffset];
        for (int j = 0; j < input_layer->channels(); ++j) {
            Mat channel(height, width, CV_32FC1, input_data);
            input_channels.push_back(channel);
            input_data += width * height;
        }

        split(images[i], input_channels);
    }
}

void feed_label_layer(const boost::shared_ptr<Net<float>> &net, const string &layer_name,
                      const std::vector<int> &labels, int from, int to, int dest_from) {
    boost::shared_ptr<Blob<float>> input_layer = net->blob_by_name(layer_name);

    for (int i = from, input_offset = 0; i < to; i++, input_offset++) {
        input_layer->mutable_cpu_data()[input_offset + dest_from] = (float) labels[i];
    }
}

void feed_label_layer(const boost::shared_ptr<Net<float>> &net, const string &layer_name,
                      const std::vector<std::shared_ptr<std::vector<int>>> &labels, int from, int to, int dest_from) {
    boost::shared_ptr<Blob<float>> input_layer = net->blob_by_name(layer_name);

    for (int i = from, input_offset = 0; i < to; i++, input_offset++) {
        input_layer->mutable_cpu_data()[input_offset + dest_from] = (float) labels[i]->at(0);
    }
}

void feed_feature_layer(const boost::shared_ptr<caffe::Net<float>>& net,
                        const std::string& layer_name,
                        const std::vector<std::shared_ptr<std::vector<float>>>& data,
                        int from,
                        int to,
                        int dest_from) {
    boost::shared_ptr<Blob<float>> input_layer = net->blob_by_name(layer_name);
    int offset_range = to - from;

    /*#pragma omp parallel for
    for (int input_offset = 0; input_offset < offset_range; input_offset++) {
        int i = input_offset + from;
        std::shared_ptr<std::vector<float>> features = data[i];
        for (int j = 0; j < features->size(); j++) {
            int input_target_offset = input_layer->offset({dest_from + input_offset, j});
            input_layer->mutable_cpu_data()[input_target_offset] = features.get()[0][j];
        }
    }*/

    #pragma omp parallel for
    for (int input_offset = 0; input_offset < offset_range; input_offset++) {
        int i = input_offset + from;
        std::shared_ptr<std::vector<float>> features = data[i];
        float* features_ptr = features.get()->data();
        int input_target_initial_offset = input_layer->offset({dest_from + input_offset, 0});
        float* layer_data = &input_layer->mutable_cpu_data()[input_target_initial_offset];
        memcpy(layer_data, features_ptr, features->size() * sizeof(float));
    }
}

void feed_label_layer(const boost::shared_ptr<Net<float>> &net, const string &layer_name,
                      const vector<float> &labels, int from, int to, int dest_from) {
    boost::shared_ptr<Blob<float>> input_layer = net->blob_by_name(layer_name);

    for (int i = from, input_offset = 0; i < to; i++, input_offset++) {
        input_layer->mutable_cpu_data()[input_offset + dest_from] = labels[i];
    }
}

void feed_target_layer(const boost::shared_ptr<Net<float>> &net, const string &layer_name,
                       const vector<vector<float>> &targets, int from, int to, int dest_from) {
    boost::shared_ptr<Blob<float>> input_layer = net->blob_by_name(layer_name);

    for (int i = from, input_offset = 0; i < to; i++, input_offset++) {
        for (int j = 0; j < targets[i].size(); j++) {
            int input_target_offset = input_layer->offset({input_offset + dest_from, j});
            input_layer->mutable_cpu_data()[input_target_offset] = targets[i][j];
        }
    }
}

void test_network_with_accuracy_layer(Solver<float>* solver,
                                      const vector<Mat> &test_x, const vector<int> &test_y,
                                      int test_iterat, int test_minibatch_size,
                                      const string &final_prediction_layer, double &test_acc, double &test_loss,
                                      vector<int> &pred_y, bool compute_test_loss, bool return_prediction) {
    test_acc = test_loss = 0;
    pred_y = vector<int>(test_y.size(), 0);
    boost::shared_ptr<Net<float>> test_net = solver->test_nets()[0];

    for (int test_it = 0; test_it < test_iterat; test_it++) {
        int start = test_it * test_minibatch_size;
        int end = (test_it + 1) * test_minibatch_size;

        feed_image_layer(test_net, "data", test_x, start, end);
        feed_label_layer(test_net, "label", test_y, start, end);
        test_net->Forward();

        float acc = test_net->blob_by_name("accuracy")->data_at({});
        test_acc += (double) acc;

        if (compute_test_loss) {
            float loss = test_net->blob_by_name("loss")->data_at({});
            test_loss += loss;
        }

        if (return_prediction) {
            for (int i = start; i < end; i++) {
                boost::shared_ptr<Blob<float>> pred_layer = test_net->blob_by_name(final_prediction_layer);

                int max_pred_class = 0;
                float max_pred_val = std::numeric_limits<float>::min();

                int row = i - start;
                for (int column = 0; column < pred_layer->shape(1); column++) {
                    float current_pred_val = pred_layer->data_at({row, column});
                    if (current_pred_val > max_pred_val) {
                        max_pred_val = current_pred_val;
                        max_pred_class = column;
                    }
                }

                pred_y[i] = max_pred_class;
            }
        }
    }

    test_acc /= (double) test_iterat;
    test_loss /= (double) test_iterat;
}

void partial_param_update(const boost::shared_ptr<caffe::Net<float>>& net, int from, int to) {
    partial_param_update(net.get(), from, to);
}

void partial_param_update(caffe::Net<float>* net, int from, int to) {
    if(to < 0) {
        to = net->learnable_params().size();
    }

    for (int i = from; i < to; ++i) {
        net->learnable_params_[i]->Update();
    }
}

void partial_apply_update(caffe::SGDSolver<float>* solver, int from, int to) {
    if(to < 0) {
        to = solver->net_->learnable_params().size();
    }
    float rate = solver->GetLearningRate();

    solver->ClipGradients();
    for (int param_id = from; param_id < to; ++param_id) {
        solver->Normalize(param_id);
        solver->Regularize(param_id);
        solver->ComputeUpdateValue(param_id, rate);
    }
    partial_param_update(solver->net_, from, to);
    //solver->net_->Update();

    // Increment the internal iter_ counter -- its value should always indicate
    // the number of times the weights have been updated.
    ++solver->iter_;
}

void caffenet_config(const string &dataset_variation, bool reduced_test_set,
                     string &selected_configuration, string &train_filelists, string &test_filelist, string &db_path,
                     string &solver_file_first_batch, string &solver_file,
                     string &class_labels_txt, string &tmp_weights_file,
                     string &init_weights_file, string &cwr_serialization_file,
                     vector<string> &cwr_layers_Model, string &prediction_level_Model) {
    string project_root;
    cwr_layers_Model = {"mid_fc8"};
    prediction_level_Model = "mid_fc8";

    // Mio PC uni
    project_root = "/home/lorenzo/Desktop/caffe_cwr_to_cpp";
    selected_configuration = "PC Uni Lorenzo";
    train_filelists = project_root + "/batch_filelists/NIC_inc1" + dataset_variation + "/run0/train_batch_XX_filelist.txt"; // conf['train_filelists']
    test_filelist =  project_root + "/batch_filelists/NIC_inc1" + dataset_variation + "/run0/test_filelist.txt"; // conf['test_filelist']
    db_path = "/home/lorenzo/Desktop/preparazione raptor01/core50_128x128"; // conf['db_path']
    solver_file_first_batch = project_root + "/NIC/NIC1/NIC_solver_CaffeNet_first_batch.prototxt"; // conf['solver_file_first_batch']
    solver_file = project_root + "/NIC/NIC1/NIC_solver_CaffeNet.prototxt"; // conf['solver_file']
    class_labels_txt = "/home/lorenzo/Desktop/preparazione raptor01/core50_labels.txt"; //conf['class_labels']
    tmp_weights_file = project_root + "/tmp_exp_data/CaffeNet.caffemodel";
    init_weights_file = project_root + "/models/CaffeNet.caffemodel"; // conf['init_weights_file']
    cwr_serialization_file = project_root + "/tmp_exp_data/cwrdata";
    if(reduced_test_set) {
        test_filelist =  project_root + "/batch_filelists/test_filelist_20.txt"; // conf['test_filelist']
    }

    if(boost::filesystem::exists(test_filelist) && boost::filesystem::exists(class_labels_txt)) {
        return;
    }

    // Mio PC casa
    project_root = "/home/lorenzo/Desktop/caffe_cwr_to_cpp";
    selected_configuration = "PC casa Lorenzo";
    train_filelists = project_root + "/batch_filelists/NIC_inc1" + dataset_variation + "/run0/train_batch_XX_filelist.txt"; // conf['train_filelists']
    test_filelist =  project_root + "/batch_filelists/NIC_inc1" + dataset_variation + "/run0/test_filelist.txt"; // conf['test_filelist']
    db_path = "/home/lorenzo/Desktop/datasets/core50/core50_128x128"; // conf['db_path']
    solver_file_first_batch = project_root + "/NIC/NIC1/NIC_solver_CaffeNet_first_batch.prototxt"; // conf['solver_file_first_batch']
    solver_file = project_root + "/NIC/NIC1/NIC_solver_CaffeNet.prototxt"; // conf['solver_file']
    class_labels_txt = "/home/lorenzo/Desktop/datasets/core50/core50_labels.txt"; //conf['class_labels']
    tmp_weights_file = project_root + "/tmp_exp_data/CaffeNet.caffemodel";
    init_weights_file = project_root + "/models/CaffeNet.caffemodel"; // conf['init_weights_file']
    cwr_serialization_file = project_root + "/tmp_exp_data/cwrdata";
    if(reduced_test_set) {
        test_filelist =  project_root + "/batch_filelists/test_filelist_20.txt"; // conf['test_filelist']
    }

    if(boost::filesystem::exists(test_filelist) && boost::filesystem::exists(class_labels_txt)) {
        return;
    }

    // Raptor01
    project_root = "/home/lpellegrini/remote_mirror/clion_projects/caffe_cwr_to_cpp";
    selected_configuration = "Raptor01 Lorenzo";
    train_filelists = project_root + "/batch_filelists/NIC_inc1" + dataset_variation + "/run0/train_batch_XX_filelist.txt"; // conf['train_filelists']
    test_filelist =  project_root + "/batch_filelists/NIC_inc1" + dataset_variation + "/run0/test_filelist.txt"; // conf['test_filelist']
    db_path = "/home/lpellegrini/datasets/core50/core50_128x128"; // conf['db_path']
    solver_file_first_batch = project_root + "/NIC/NIC1/NIC_solver_CaffeNet_first_batch.prototxt"; // conf['solver_file_first_batch']
    solver_file = project_root + "/NIC/NIC1/NIC_solver_CaffeNet.prototxt"; // conf['solver_file']
    class_labels_txt = "/home/lpellegrini/datasets/core50/core50_labels.txt"; //conf['class_labels']
    tmp_weights_file = project_root + "/tmp_exp_data/CaffeNet.caffemodel";
    init_weights_file = project_root + "/models/CaffeNet.caffemodel"; // conf['init_weights_file']
    cwr_serialization_file = project_root + "/tmp_exp_data/cwrdata";
    if(reduced_test_set) {
        test_filelist =  project_root + "/batch_filelists/test_filelist_20.txt"; // conf['test_filelist']
    }

    if(boost::filesystem::exists(test_filelist) && boost::filesystem::exists(class_labels_txt)) {
        return;
    }

    // Docker
    project_root = "/opt/project";
    selected_configuration = "Immagine docker";
    train_filelists = project_root + "/batch_filelists/NIC_inc1" + dataset_variation + "/run0/train_batch_XX_filelist.txt"; // conf['train_filelists']
    test_filelist =  project_root + "/batch_filelists/NIC_inc1" + dataset_variation + "/run0/test_filelist.txt"; // conf['test_filelist']
    db_path = "/datasets/core50/core50_128x128"; // conf['db_path']
    solver_file_first_batch = project_root + "/NIC/NIC1/NIC_solver_CaffeNet_first_batch.prototxt"; // conf['solver_file_first_batch']
    solver_file = project_root + "/NIC/NIC1/NIC_solver_CaffeNet.prototxt"; // conf['solver_file']
    class_labels_txt = "/datasets/core50/core50_labels.txt"; //conf['class_labels']
    tmp_weights_file = project_root + "/tmp_exp_data/CaffeNet.caffemodel";
    init_weights_file = project_root + "/models/CaffeNet.caffemodel"; // conf['init_weights_file']
    cwr_serialization_file = project_root + "/tmp_exp_data/cwrdata";
    if(reduced_test_set) {
        test_filelist =  project_root + "/batch_filelists/test_filelist_20.txt"; // conf['test_filelist']
    }

    if(boost::filesystem::exists(test_filelist) && boost::filesystem::exists(class_labels_txt)) {
        return;
    }

    throw std::runtime_error("[caffenet] Can't find paths configuration. This PC is not Lorenzo's PC(s), Raptor01 or Docker image!");
}

void mobilenet_config(const string &dataset_variation, bool reduced_test_set, int nicv2_scenario,
                     string &selected_configuration, string &train_filelists, string &test_filelist, string &db_path,
                     string &solver_file_first_batch, string &solver_file,
                     string &class_labels_txt, string &tmp_weights_file,
                     string &init_weights_file, string &cwr_serialization_file,
                      vector<string> &cwr_layers_Model, string &prediction_level_Model) {
    string project_root;
    cwr_layers_Model = {"mid_fc7"};
    prediction_level_Model = "mid_fc7";
    string filelist_folder_name("sIII_v2_");
    filelist_folder_name += std::to_string(nicv2_scenario);
    filelist_folder_name += dataset_variation;

            // Mio PC uni
    project_root = "/home/lorenzo/Desktop/caffe_cwr_to_cpp";
    selected_configuration = "PC Uni Lorenzo";
    train_filelists = project_root + "/batch_filelists/" + filelist_folder_name + "/run0/train_batch_XX_filelist.txt"; // conf['train_filelists']
    test_filelist =  project_root + "/batch_filelists/" + filelist_folder_name + "/run0/test_filelist.txt"; // conf['test_filelist']
    db_path = "/home/lorenzo/Desktop/preparazione raptor01/core50_128x128"; // conf['db_path']
    solver_file_first_batch = project_root + "/mobilenet/nic_solver_mobilenet_first_batch.prototxt"; // conf['solver_file_first_batch']
    solver_file = project_root + "/mobilenet/nic_solver_mobilenet.prototxt"; // conf['solver_file']
    class_labels_txt = "/home/lorenzo/Desktop/preparazione raptor01/core50_labels.txt"; //conf['class_labels']
    tmp_weights_file = project_root + "/tmp_exp_data/MobileNetV1.caffemodel";
    init_weights_file = project_root + "/mobilenet/MobileNetV1_50_classes.caffemodel"; // conf['init_weights_file']
    cwr_serialization_file = project_root + "/tmp_exp_data/MobileNetV1cwrdata";
    if(reduced_test_set) {
        test_filelist =  project_root + "/batch_filelists/test_filelist_20.txt"; // conf['test_filelist']
    }

    if(boost::filesystem::exists(test_filelist) && boost::filesystem::exists(class_labels_txt)) {
        return;
    }

    // Mio PC casa
    project_root = "/home/lorenzo/Desktop/caffe_cwr_to_cpp";
    selected_configuration = "PC casa Lorenzo";
    train_filelists = project_root + "/batch_filelists/" + filelist_folder_name + "/run0/train_batch_XX_filelist.txt"; // conf['train_filelists']
    test_filelist =  project_root + "/batch_filelists/" + filelist_folder_name + "/run0/test_filelist.txt"; // conf['test_filelist']
    db_path = "/home/lorenzo/Desktop/datasets/core50/core50_128x128"; // conf['db_path']
    solver_file_first_batch = project_root + "/mobilenet/nic_solver_mobilenet_first_batch.prototxt"; // conf['solver_file_first_batch']
    solver_file = project_root + "/mobilenet/nic_solver_mobilenet.prototxt"; // conf['solver_file']
    class_labels_txt = "/home/lorenzo/Desktop/datasets/core50/core50_labels.txt"; //conf['class_labels']
    tmp_weights_file = project_root + "/tmp_exp_data/MobileNetV1.caffemodel";
    init_weights_file = project_root + "/mobilenet/MobileNetV1_50_classes.caffemodel"; // conf['init_weights_file']
    cwr_serialization_file = project_root + "/tmp_exp_data/MobileNetV1cwrdata";
    if(reduced_test_set) {
        test_filelist =  project_root + "/batch_filelists/test_filelist_20.txt"; // conf['test_filelist']
    }

    if(boost::filesystem::exists(test_filelist) && boost::filesystem::exists(class_labels_txt)) {
        return;
    }

    // Raptor01
    project_root = "/home/lpellegrini/remote_mirror/clion_projects/caffe_cwr_to_cpp";
    selected_configuration = "Raptor01 Lorenzo";
    train_filelists = project_root + "/batch_filelists/" + filelist_folder_name + "/run0/train_batch_XX_filelist.txt"; // conf['train_filelists']
    test_filelist =  project_root + "/batch_filelists/" + filelist_folder_name + "/run0/test_filelist.txt"; // conf['test_filelist']
    db_path = "/home/lpellegrini/datasets/core50/core50_128x128"; // conf['db_path']
    solver_file_first_batch = project_root + "/mobilenet/nic_solver_mobilenet_first_batch.prototxt"; // conf['solver_file_first_batch']
    solver_file = project_root + "/mobilenet/nic_solver_mobilenet.prototxt"; // conf['solver_file']
    class_labels_txt = "/home/lpellegrini/datasets/core50/core50_labels.txt"; //conf['class_labels']
    tmp_weights_file = project_root + "/tmp_exp_data/MobileNetV1.caffemodel";
    init_weights_file = project_root + "/mobilenet/MobileNetV1_50_classes.caffemodel"; // conf['init_weights_file']
    cwr_serialization_file = project_root + "/tmp_exp_data/MobileNetV1cwrdata";
    if(reduced_test_set) {
        test_filelist =  project_root + "/batch_filelists/test_filelist_20.txt"; // conf['test_filelist']
    }

    if(boost::filesystem::exists(test_filelist) && boost::filesystem::exists(class_labels_txt)) {
        return;
    }

    // Docker
    project_root = "/opt/project";
    selected_configuration = "Immagine docker";
    train_filelists = project_root + "/batch_filelists/" + filelist_folder_name + "/run0/train_batch_XX_filelist.txt"; // conf['train_filelists']
    test_filelist =  project_root + "/batch_filelists/" + filelist_folder_name + "/run0/test_filelist.txt"; // conf['test_filelist']
    db_path = "/datasets/core50/core50_128x128"; // conf['db_path']
    solver_file_first_batch = project_root + "/mobilenet/nic_solver_mobilenet_first_batch.prototxt"; // conf['solver_file_first_batch']
    solver_file = project_root + "/mobilenet/nic_solver_mobilenet.prototxt"; // conf['solver_file']
    class_labels_txt = "/datasets/core50/core50_labels.txt"; //conf['class_labels']
    tmp_weights_file = project_root + "/tmp_exp_data/MobileNetV1.caffemodel";
    init_weights_file = project_root + "/mobilenet/MobileNetV1_50_classes.caffemodel"; // conf['init_weights_file']
    cwr_serialization_file = project_root + "/tmp_exp_data/MobileNetV1cwrdata";
    if(reduced_test_set) {
        test_filelist =  project_root + "/batch_filelists/test_filelist_20.txt"; // conf['test_filelist']
    }

    if(boost::filesystem::exists(test_filelist) && boost::filesystem::exists(class_labels_txt)) {
        return;
    }

    throw std::runtime_error("[mobilenet] Can't find paths configuration. This PC is not Lorenzo's PC(s), Raptor01 or Docker image!");
}

void choose_file_paths_config(const string &dataset_variation, const string &model, bool reduced_test_set,
                              int nicv2_scenario, string &selected_configuration, string &train_filelists, string &test_filelist, string &db_path,
                              string &solver_file_first_batch, string &solver_file,
                              string &class_labels_txt, string &tmp_weights_file,
                              string &init_weights_file, string &cwr_serialization_file,
                              vector<string> &cwr_layers_Model, string &prediction_level_Model) {
    if(model == "caffenet") {
        cout << "Caffenet is deprecated. This configuration may not work..." << endl;
        caffenet_config(dataset_variation, reduced_test_set, selected_configuration, train_filelists, test_filelist, db_path,
                solver_file_first_batch, solver_file,
                class_labels_txt, tmp_weights_file,
                init_weights_file, cwr_serialization_file,
                cwr_layers_Model, prediction_level_Model);
    } else if(model == "mobilenet") {
        mobilenet_config(dataset_variation, reduced_test_set, nicv2_scenario,
                selected_configuration, train_filelists, test_filelist, db_path,
                solver_file_first_batch, solver_file,
                class_labels_txt, tmp_weights_file,
                init_weights_file, cwr_serialization_file,
                cwr_layers_Model, prediction_level_Model);
    } else {
        throw std::runtime_error("Invalid model, must be caffenet or mobilenet!");
    }
}

string hash_generic(void* pointer, size_t size_in_bytes) {
    return picosha2::hash256_hex_string((uchar*)pointer, ((uchar*)pointer)+size_in_bytes);
}

string hash_caffe_blob(const boost::shared_ptr<caffe::Blob<float>> &blob) {
    const float* data = blob->cpu_data();
    int byte_size = blob->count() * sizeof(float);
    return hash_generic((void*)data, byte_size);
}

string hash_caffe_blob(const boost::shared_ptr<caffe::Net<float>> &net, const std::string &blob_name) {
    auto blob = net->blob_by_name(blob_name);
    return hash_caffe_blob(blob);
}

string hash_net_params(const boost::shared_ptr<Net<float>> &net) {
    string result;
    vector<string> names = net->layer_names();
    std::sort(names.begin(), names.end());

    for(auto& name : names) {
        auto blobs = net->layer_by_name(name)->blobs();
        if(!blobs.empty()) {
            if(blobs.size() <= 2) {
                result += name;
                result += "\t";
                result += hash_caffe_blob(blobs[0]);
                if (blobs.size() == 2) {
                    result += "\nBias: ";
                    result += hash_caffe_blob(blobs[1]);
                }
                result += "\n";
            } else {
                result += name;
                result += ":\n";
                for(int i = 0; i < blobs.size(); i++) {
                    result += "\t";
                    result += hash_caffe_blob(blobs[i]);
                    result += "\n";
                }
            }
        }
    }

    return result;
}

string hash_net(const boost::shared_ptr<Net<float>> &net, bool include_lrs) {
    picosha2::hash256_one_by_one hasher;

    vector<string> names = net->layer_names();
    std::sort(names.begin(), names.end());

    for(auto& name : names) {
        auto blobs = net->layer_by_name(name)->blobs();
        if(!blobs.empty()) {
            for(int i = 0; i < blobs.size(); i++) {
                const float* blob_data = blobs[i]->cpu_data();
                int byte_size = blobs[i]->count() * sizeof(float);
                hasher.process((uchar*)blob_data, ((uchar*)blob_data)+byte_size);
            }
        }
    }

    if(include_lrs) {
        size_t size_in_bytes = net->params_lr_.size() * sizeof(float);
        hasher.process((uchar*) net->params_lr_.data(), ((uchar*) net->params_lr_.data()) + size_in_bytes);
    }

    hasher.finish();

    std::string hex_str = picosha2::get_hash_hex_string(hasher);

    return hex_str;
}

std::string hash_vector(const std::vector<float> &vec) {
    size_t size_in_bytes = vec.size() * sizeof(float);
    return hash_generic((void*) vec.data(), size_in_bytes);
}

std::string hash_vector(const std::vector<int> &vec) {
    size_t size_in_bytes = vec.size() * sizeof(int);
    return hash_generic((void*) vec.data(), size_in_bytes);
}

std::string hash_vector_as_int64(const std::vector<int> &vec) {
    std::vector<int64_t> vec_64(vec.size());
    for(int i = 0; i < vec.size(); i++) {
        vec_64[i] = vec[i];
    }

    size_t size_in_bytes = vec_64.size() * sizeof(int64_t);
    return hash_generic((void*) vec_64.data(), size_in_bytes);
}

std::string hash_rehearsal_memory(const std::vector<std::shared_ptr<std::vector<float>>> &vec) {
    //picosha2::hash256_one_by_one hasher;
    std::vector<float> unique_byte_array;

    for(int i = 0; i < vec.size(); i++) {
        //std::vector<float> my_vec = *vec[i].get();

        //size_t byte_size = my_vec.size() * sizeof(float);
        //hasher.process((uchar*)my_vec.data(), ((uchar*)my_vec.data())+byte_size);

        unique_byte_array.insert( unique_byte_array.end(), vec[i]->begin(), vec[i]->end() );
    }

    return hash_vector(unique_byte_array);

    /*hasher.finish();

    std::string hex_str = picosha2::get_hash_hex_string(hasher);

    return hex_str;*/
}

vector<float> inference_on_single_pattern(Solver<float>* solver, const string &pattern,
        const string &final_prediction_layer) {

    boost::shared_ptr<Net<float>> test_net = solver->test_nets()[0];

    Mat image;
    image = imread(pattern, cv::IMREAD_COLOR);

    cv::Mat img2;
    image.convertTo(img2, CV_32FC3);
    cv::Mat sample_normalized;
    sample_normalized = img2 - core50_mean_image;

    vector<float> label_input(1, 0.0f);
    feed_image_layer(test_net, "data", {sample_normalized}, 0, 1);
    //feed_label_layer(test_net, "label", label_input, 0, 1);
    test_net->Forward();

    vector<float> result;
    boost::shared_ptr<Blob<float>> pred_layer = test_net->blob_by_name(final_prediction_layer);

    for (int column = 0; column < pred_layer->shape(1); column++) {
        result.push_back(pred_layer->data_at({0, column}));
    }

    return result;
}

void log_android_debug(std::stringstream &debug_stream) {
    int buffer_len;
    streamsize read;
    buffer_len = 500;
    std::string prev_remaining = "", actual_remaining = "";

    char buffer[buffer_len];

    debug_stream.read(buffer, buffer_len);
    read = debug_stream.gcount();
    while(!(debug_stream.eof() && read <= 0)) {
        std::string out_str(buffer, read);

        size_t lf = out_str.find_last_of('\n');
        if(lf != std::string::npos && lf != (read-1) && lf != 0) {
            actual_remaining = std::string(&out_str.c_str()[lf+1]);
            out_str = out_str.substr(0, lf);
        } else {
            actual_remaining = "";
        }

#ifdef __ANDROID__
        __android_log_print(ANDROID_LOG_DEBUG, "NativePrint", "%s%s", prev_remaining.c_str(), out_str.c_str());
#else
        cout << "[NativePrint]" << prev_remaining << out_str << endl;
#endif

        debug_stream.read(buffer, buffer_len - actual_remaining.length());
        read = debug_stream.gcount();
        prev_remaining = actual_remaining;
    }

    debug_stream.str("");
    debug_stream.clear();
}

std::unordered_map<std::string, int> generate_layers_name_to_id_mapping(caffe::Net<float>* net) {
    std::unordered_map<std::string, int> layer_names_to_id;

    const auto &layers = net->layers();

    for (int layer_id = 0; layer_id < net->layer_names_.size(); ++layer_id) {
        layer_names_to_id.insert(std::pair<string, int>(net->layer_names_[layer_id], layer_id));
        //layer_names_index_[layer_names_[layer_id]] = layer_id;
    }

    /*for(int layer_index = 0; layer_index < layers.size(); layer_index++) {
        layer_names_to_id.insert(std::pair<string, int>(layers[layer_index]->layer_param().name(), layer_index));
    }*/

    return layer_names_to_id;
}

//TODO: remove duplicate in cwr.cpp
std::unordered_map<std::string, int> generate_layers_name_to_param_id_mapping(const boost::shared_ptr<caffe::Net<float>> &net) {
    return generate_layers_name_to_param_id_mapping(net.get());
}

std::unordered_map<std::string, int> generate_layers_name_to_param_id_mapping(caffe::Net<float>* net) {
    std::unordered_map<std::string, int> layer_names_to_param_id;

    const auto &layers = net->layers();

    //int layer_index = 0;
    int param_entry = 0;
    for(int i = 0; i < layers.size(); i++) {
        auto lr = layers[i];
        int blobs_sz = lr->blobs().size();

        if(blobs_sz > 0) {
            layer_names_to_param_id.insert(std::pair<std::string, int>(net->layer_names()[i], param_entry));
        }

        param_entry += blobs_sz;
    }

    return layer_names_to_param_id;
}

std::unordered_map<std::string, int> generate_layers_name_to_id_mapping(const boost::shared_ptr<caffe::Net<float>> &net) {
    return generate_layers_name_to_id_mapping(net.get());
}

int get_layer_id_from_name(const boost::shared_ptr<caffe::Net<float>> &net, const std::string &layer_name) {
    const auto &layers = net->layers();

    for (int layer_id = 0; layer_id < net->layer_names_.size(); ++layer_id) {
        if(layer_name == net->layer_names_[layer_id]) {
            return layer_id;
        }
    }

    return -1;
}

std::vector<std::shared_ptr<std::vector<float>>>
get_layer_features(const boost::shared_ptr<caffe::Net<float>> &net, const std::string &layer_name, int from, int to) {
    std::vector<std::shared_ptr<std::vector<float>>> result;

    boost::shared_ptr<Blob<float>> blob = net->blob_by_name(layer_name);
    int pattern_feature_size = 1;
    for(int i = 1; i < blob->shape().size(); i++) {
        pattern_feature_size *= blob->shape()[i];
    }

    if(to < 0) {
        to = blob->shape()[0];
    }

    for (int i = from; i < to; i++) {
        const float* pattern_data_start = &blob->cpu_data()[i * pattern_feature_size];
        const float* pattern_data_end = &blob->cpu_data()[(i+1) * pattern_feature_size];

        result.push_back(std::make_shared<std::vector<float>>(pattern_data_start, pattern_data_end));
    }

    return result;
}

std::vector<float> per_class_accuracy(int classes, const std::vector<int> &pred_y, const std::vector<int> &truth) {
    vector<float> result(classes, 0.0f);
    vector<int> correct(classes, 0);
    vector<int> all(classes, 0);
    int i, prediction, correct_label;

    for(i = 0; i < pred_y.size(); i++) {
        prediction = pred_y[i];
        correct_label = truth[i];

        all[correct_label]++;

        if(prediction == correct_label) {
            correct[prediction]++;
        }
    }

    for(i = 0; i < classes; i++) {
        if(all[i] == 0) {
            result[i] = NAN;
        } else {
            result[i] = (double) correct[i] /  (double) all[i];
        }
        //cout << "Class " << i << " " << correct[i] << " / " << all[i] << " = " << result[i] << endl;
    }

    return result;
}

std::string hash_net_lrs(const boost::shared_ptr<caffe::Net<float>> &net) {
    return hash_vector(net->params_lr_);
}

bool file_exists(const std::string& name) {
    if (FILE *file = fopen(name.c_str(), "r")) {
        fclose(file);
        return true;
    } else {
        return false;
    }
}

vector<int> indexes_range(int min, int max, int n_to_generate) {
    vector<int> result;

    if((max - min) + 1 < n_to_generate) {
        for(int i = min; i <= max; i++) {
            result.push_back(i);
        }
    } else {
        double delta = ((double)(max - min)) / (double)(n_to_generate - 1);
        for (int i = 0; i < n_to_generate; i++) {
            result.push_back((int) std::lround((double) min + i * delta));
        }
    }

    return result;
}


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern operation_xml parser
#include <boost/test/unit_test.hpp>
#include "fern/core/exception.h"
#include "fern/language/io/uncertml2/uncertml2_parser.h"
#include "fern/language/uncertainty/normal_distribution.h"

BOOST_AUTO_TEST_CASE(parse)
{
    fern::UncertML2Parser parser;
    std::string xml;
    std::shared_ptr<fern::Uncertainty> uncertainty;

    // Example from the uncertml reference.
    {
        xml =
            "<?xml version=\"1.0\"?>\n"
            "<un:NormalDistribution xmlns:un=\"http://www.uncertml.org/2.0\">\n"
              "<un:mean>3.14</un:mean>\n"
              "<un:variance>3.14</un:variance>\n"
            "</un:NormalDistribution>\n"
            ;
        uncertainty = parser.parse(xml);
        BOOST_REQUIRE(uncertainty);
        std::shared_ptr<fern::NormalDistribution<double>>
            normal_distribution(
                std::dynamic_pointer_cast<fern::NormalDistribution<double>>(
                uncertainty));
        BOOST_REQUIRE(normal_distribution);
        BOOST_CHECK_CLOSE(normal_distribution->mean(), 3.14, 0.001);
        BOOST_CHECK_CLOSE(normal_distribution->standard_deviation(),
            std::sqrt(3.14), 0.001);
    }

    // Empty file.
    {
        xml = "";
        BOOST_CHECK_THROW(parser.parse(xml), fern::detail::ParseError);
    }

    // Parse error.
    {
        xml = "abc";
        BOOST_CHECK_THROW(parser.parse(xml), fern::detail::ParseError);
    }
}