//
//  Rate_and_State_Aging.hpp
//  hybrid_fem_bie
//
//  Created by Max on 2/10/18.
//
//

#ifndef Rate_and_State_Aging_hpp
#define Rate_and_State_Aging_hpp

#include <stdio.h>
#include <iostream>
#include <Eigen/Eigen>
#include <math.h>
#include "maplocal.hpp"
#include "NR_solve_eq10.hpp"
using namespace Eigen;

void Rate_and_State_Aging(VectorXd &M_global_vec, ArrayXi &top_surf_index, ArrayXi &bot_surf_index, VectorXd &fe_global, double dt, double dx,double dy, int nx, VectorXd &delt_u_n, VectorXd &delt_v_n,VectorXd &T_0, VectorXd &tau_s, VectorXd a, VectorXd b, double L, double V_0, double f_0, int Ndofn, double M, VectorXd &F_fault, VectorXd &T_c, VectorXd &theta_n, VectorXd &theta_dot_n);

//void Rate_and_State_Aging(VectorXd &M_global_vec, ArrayXi &top_surf_index, ArrayXi &bot_surf_index, VectorXd &fe_global, double dt, double dx,double dy, int nx, ArrayXd &delt_u_n, ArrayXd &delt_v_n, ArrayXd &T_0, ArrayXd &tau_s, VectorXd a, VectorXd b, double L, double V_0, double f_0, int Ndofn, double M, VectorXd &F_fault, ArrayXd &T_c, VectorXd &theta_n, VectorXd &theta_dot_n);

#endif /* Rate_and_State_Aging_hpp */


/******************************************************************************

  Markov random field model for regular 2D lattice.

  Copyright (c) 2009 - 2012
  Alexander Rukletsov <rukletsov@gmail.com>
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions
  are met:
  1.  Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
  2.  Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.

  THIS SOFTWARE IS PROVIDED BY THE AUTHOR 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 AUTHOR 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.

*******************************************************************************/

#ifndef MRF_2D_HPP_1140ED81_1E3E_4AEB_AFBB_6CA70FE3EF9B_
#define MRF_2D_HPP_1140ED81_1E3E_4AEB_AFBB_6CA70FE3EF9B_

#include <cstddef>
#include <cmath>
#include <stdexcept>
#include <boost/random.hpp>
#include <boost/assert.hpp>
#include <boost/noncopyable.hpp>
#include <boost/function.hpp>

#include "bo/core/raw_image_2d.hpp"

namespace bo {
namespace mrf {

// A class representing a Markov random field on a regular 2D lattice. Operates on
// random field configuration using given observation and clique functions. Only first
// order (pairwise) neighbourhood is supproted. No copies of an instance are allowed.
template <typename NodeType, typename DataType, typename RealType>
class MRF2D: public boost::noncopyable
{
public:
    typedef NodeType& reference;
    typedef const NodeType& const_reference;

    typedef RawImage2D<NodeType> RandomLattice;
    typedef RawImage2D<DataType> DataLattice;
    typedef boost::function<RealType (DataType, NodeType)> LikelihoodEnergy;
    typedef boost::function<RealType (NodeType, NodeType)> PriorEnergy;

    MRF2D(const RandomLattice& initial_configuration, const DataLattice& observation,
          LikelihoodEnergy likelihood, PriorEnergy prior);

    RealType compute_full_energy() const;
    RealType compute_local_energy(NodeType val, std::size_t col, std::size_t row) const;
    RealType compute_local_likelihood(NodeType val, std::size_t col, std::size_t row) const;

    const_reference operator()(std::size_t col, std::size_t row) const;
    reference operator()(std::size_t col, std::size_t row);

    std::size_t width() const;
    std::size_t height() const;

private:
    RealType right_clique_(NodeType val, std::size_t col, std::size_t row) const;
    RealType down_clique_(NodeType val, std::size_t col, std::size_t row) const;
    RealType left_clique_(NodeType val, std::size_t col, std::size_t row) const;
    RealType up_clique_(NodeType val, std::size_t col, std::size_t row) const;

    RealType prior_fun_(NodeType val, std::size_t neigh_col, std::size_t neigh_row) const;
    RealType likelihood_fun_(NodeType val, std::size_t col, std::size_t row) const;

private:
    RandomLattice configuration_;
    DataLattice observation_;
    LikelihoodEnergy likelihood_;
    PriorEnergy prior_;
};

template <typename NodeType, typename DataType, typename RealType>
MRF2D<NodeType, DataType, RealType>::MRF2D(const RandomLattice& initial_configuration,
    const DataLattice& observation, LikelihoodEnergy likelihood, PriorEnergy prior):
    configuration_(initial_configuration),
    observation_(observation),
    likelihood_(likelihood),
    prior_(prior)
{
    // Check if the dimensions of observation and configuration are the same.
    if ((configuration_.width() != observation_.width()) ||
        (configuration_.height() != configuration_.height()))
    {
        BOOST_ASSERT(false && "Dimensions of the configuration and observation do not coincide.");
        throw std::logic_error("MRF2D: dimensions of the random field lattice and the "
                               "observation data must be the same.");
    }
}

// Computes full energy of the current MRF state. It takes sum over all zero order
// (single node) and first order (every two adjacent nodes) cliques using corresponding
// clique functions. In order to iterate over all first order cliques, for every
// node we can consider only right and down neighbour except for some border nodes.
template <typename NodeType, typename DataType, typename RealType>
RealType MRF2D<NodeType, DataType, RealType>::compute_full_energy() const
{
    RealType energy(0);

    for (std::size_t col = 0; col < configuration_.width(); ++col) {
        for (std::size_t row = 0; row < configuration_.height(); ++row) {
            NodeType value = configuration_(col, row);
            energy += (likelihood_fun_(value, col, row) +
                       right_clique_(value, col, row) +
                       down_clique_(value, col, row));
    }   }

    return energy;
}

// Computes a local energy of the neighbourhood of the given node with provided value.
// Border checks are done inside corresponding clique functions.
template <typename NodeType, typename DataType, typename RealType>
RealType MRF2D<NodeType, DataType, RealType>::compute_local_energy(NodeType val,
    std::size_t col, std::size_t row) const
{
    // Compute likelihood energy for the given value at the given position.
    RealType energy = likelihood_fun_(val, col, row);

    // Compute prior energy of the given node given its neighbours.
    energy += (right_clique_(val, col, row) + down_clique_(val, col, row) +
               left_clique_(val, col, row) + up_clique_(val, col, row));

    return energy;
}

// Computes a likelihood function for the given node with provided value.
template <typename NodeType, typename DataType, typename RealType>
RealType MRF2D<NodeType, DataType, RealType>::compute_local_likelihood(NodeType val,
    std::size_t col, std::size_t row) const
{
    return
        likelihood_fun_(val, col, row);
}

template <typename NodeType, typename DataType, typename RealType> inline
typename MRF2D<NodeType, DataType, RealType>::const_reference
MRF2D<NodeType, DataType, RealType>::operator()(std::size_t col, std::size_t row) const
{
    return configuration_(col, row);
}

template <typename NodeType, typename DataType, typename RealType> inline
typename MRF2D<NodeType, DataType, RealType>::reference
MRF2D<NodeType, DataType, RealType>::operator()(std::size_t col, std::size_t row)
{
    return configuration_(col, row);
}

template <typename NodeType, typename DataType, typename RealType> inline
std::size_t MRF2D<NodeType, DataType, RealType>::width() const
{
    return configuration_.width();
}

template <typename NodeType, typename DataType, typename RealType> inline
std::size_t MRF2D<NodeType, DataType, RealType>::height() const
{
    return configuration_.height();
}

// Next four functions represent cliques and neighbourhood relations in the model.
// They check for border overrun and call the associated clique function.
template <typename NodeType, typename DataType, typename RealType> inline
RealType MRF2D<NodeType, DataType, RealType>::right_clique_(NodeType val,
    std::size_t col, std::size_t row) const
{
    return
        (col < configuration_.width() - 1) ? prior_fun_(val, col + 1, row) : RealType(0);
}

template <typename NodeType, typename DataType, typename RealType> inline
RealType MRF2D<NodeType, DataType, RealType>::down_clique_(NodeType val,
    std::size_t col, std::size_t row) const
{
    return
        (row < configuration_.height() - 1) ? prior_fun_(val, col, row + 1) : RealType(0);
}

template <typename NodeType, typename DataType, typename RealType> inline
RealType MRF2D<NodeType, DataType, RealType>::left_clique_(NodeType val,
    std::size_t col, std::size_t row) const
{
    return
        (col > 0) ? prior_fun_(val, col - 1, row) : RealType(0);
}

template <typename NodeType, typename DataType, typename RealType> inline
RealType MRF2D<NodeType, DataType, RealType>::up_clique_(NodeType val,
    std::size_t col, std::size_t row) const
{
    return
        (row > 0) ? prior_fun_(val, col, row - 1) : RealType(0);
}

// Computes prior energy for two nodes.
template <typename NodeType, typename DataType, typename RealType> inline
RealType MRF2D<NodeType, DataType, RealType>::prior_fun_(NodeType val,
    std::size_t neigh_col, std::size_t neigh_row) const
{
    return prior_(val, configuration_(neigh_col, neigh_row));
}

// Computes likelihood energy for the given node.
template <typename NodeType, typename DataType, typename RealType> inline
RealType MRF2D<NodeType, DataType, RealType>::likelihood_fun_(NodeType val,
    std::size_t col, std::size_t row) const
{
    return likelihood_(observation_(col, row), val);
}

} // namespace mrf
} // namespace bo

#endif // MRF_2D_HPP_1140ED81_1E3E_4AEB_AFBB_6CA70FE3EF9B_


#ifndef EDGE_PLANE_PARALLEL_HPP
#define EDGE_PLANE_PARALLEL_HPP

#include <Eigen/Dense>
#include <g2o/core/base_binary_edge.h>
#include <g2o/types/slam3d_addons/vertex_plane.h>

namespace g2o {

class EdgePlaneParallel : public BaseBinaryEdge<3, Eigen::Vector3d, VertexPlane, VertexPlane> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  EdgePlaneParallel() : BaseBinaryEdge<3, Eigen::Vector3d, VertexPlane, VertexPlane>() {
    _information.setIdentity();
    _error.setZero();
  }

  void computeError() override {
    const VertexPlane* v1 = static_cast<const VertexPlane*>(_vertices[0]);
    const VertexPlane* v2 = static_cast<const VertexPlane*>(_vertices[1]);

    Eigen::Vector3d normal1 = v1->estimate().normal();
    Eigen::Vector3d normal2 = v2->estimate().normal();

    if (normal1.dot(normal2) < 0.0) {
      normal2 = -normal2;
    }

    _error = (normal2 - normal1) - _measurement;
  }
  virtual bool read(std::istream& is) override {
    Eigen::Vector3d v;
    for (int i = 0; i < 3; ++i) {
      is >> v[i];
    }

    setMeasurement(v);
    for(int i = 0; i < information().rows(); ++i) {
      for(int j = i; j < information().cols(); ++j) {
        is >> information()(i, j);
        if(i != j) {
          information()(j, i) = information()(i, j);
        }
      }
    }

    return true;
  }

  virtual bool write(std::ostream& os) const override {
    for(int i = 0; i < 3; ++i) {
      os << _measurement[i] << " ";
    }

    for(int i = 0; i < information().rows(); ++i) {
      for(int j = i; j < information().cols(); ++j) {
        os << " " << information()(i, j);
      };
    }
    return os.good();
  }

  virtual void setMeasurement(const Eigen::Vector3d& m) override { _measurement = m; }

  virtual int measurementDimension() const override { return 3; }
};

class EdgePlanePerpendicular : public BaseBinaryEdge<1, Eigen::Vector3d, VertexPlane, VertexPlane> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  EdgePlanePerpendicular() : BaseBinaryEdge<1, Eigen::Vector3d, VertexPlane, VertexPlane>() {
    _information.setIdentity();
    _error.setZero();
  }

  void computeError() override {
    const VertexPlane* v1 = static_cast<const VertexPlane*>(_vertices[0]);
    const VertexPlane* v2 = static_cast<const VertexPlane*>(_vertices[1]);

    Eigen::Vector3d normal1 = v1->estimate().normal().normalized();
    Eigen::Vector3d normal2 = v2->estimate().normal().normalized();

    _error[0] = normal1.dot(normal2);
  }
  virtual bool read(std::istream& is) override {
    Eigen::Vector3d v;
    for (int i = 0; i < 3; ++i) {
      is >> v[i];
    }

    setMeasurement(v);
    for(int i = 0; i < information().rows(); ++i) {
      for(int j = i; j < information().cols(); ++j) {
        is >> information()(i, j);
        if (i != j) {
          information()(j, i) = information()(i, j);
        }
      }
    }

    return true;
  }

  virtual bool write(std::ostream& os) const override {
    for (int i = 0; i < 3; ++i) {
      os << _measurement[i] << " ";
    }

    for(int i = 0; i < information().rows(); ++i) {
      for(int j = i; j < information().cols(); ++j) {
        os << " " << information()(i, j);
      };
    }
    return os.good();
  }

  virtual void setMeasurement(const Eigen::Vector3d& m) override { _measurement = m; }

  virtual int measurementDimension() const override { return 3; }
};

}  // namespace g2o

#endif  // EDGE_PLANE_PARALLEL_HPP


/*
 * @Description: g2o vertex for LIO extended pose
 * @Author: Ge Yao
 * @Date: 2020-11-29 15:47:49
 */
#ifndef LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_VERTEX_VERTEX_PRVAG_HPP_
#define LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_VERTEX_VERTEX_PRVAG_HPP_

#include <iostream>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <sophus/so3.hpp>

#include <g2o/core/base_vertex.h>

#include <mutex>

namespace g2o {

struct PRVAG {
    static const int INDEX_POS = 0;
    static const int INDEX_ORI = 3;
    static const int INDEX_VEL = 6;
    static const int INDEX_B_A = 9;
    static const int INDEX_B_G = 12;

    PRVAG() {}

    explicit PRVAG(const double *data) {
        pos = Eigen::Vector3d(data[INDEX_POS + 0], data[INDEX_POS + 1], data[INDEX_POS + 2]);
        ori = Sophus::SO3d::exp(
              Eigen::Vector3d(data[INDEX_ORI + 0], data[INDEX_ORI + 1], data[INDEX_ORI + 2])
        );
        vel = Eigen::Vector3d(data[INDEX_VEL + 0], data[INDEX_VEL + 1], data[INDEX_VEL + 2]);
        b_a = Eigen::Vector3d(data[INDEX_B_A + 0], data[INDEX_B_A + 1], data[INDEX_B_A + 2]);
        b_g = Eigen::Vector3d(data[INDEX_B_G + 0], data[INDEX_B_G + 1], data[INDEX_B_G + 2]);
    }

    void write(double *data) const {
        // get orientation in so3:
        auto log_ori = ori.log();

        for (size_t i = 0; i < 3; ++i) {
            data[INDEX_POS + i] = pos(i);
            data[INDEX_ORI + i] = log_ori(i);
            data[INDEX_VEL + i] = vel(i);
            data[INDEX_B_A + i] = b_a(i);
            data[INDEX_B_G + i] = b_g(i);
        }
    }

    double time = 0.0;

    Eigen::Vector3d pos = Eigen::Vector3d::Zero();
    Sophus::SO3d ori = Sophus::SO3d();
    Eigen::Vector3d vel = Eigen::Vector3d::Zero();
    Eigen::Vector3d b_a = Eigen::Vector3d::Zero();
    Eigen::Vector3d b_g = Eigen::Vector3d::Zero();
};

class VertexPRVAG : public g2o::BaseVertex<15, PRVAG> {
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    virtual void setToOriginImpl() override {
        _estimate = PRVAG();
    }

    virtual void oplusImpl(const double *update) override {
        //
        // TODO: do update
        //
    }

    bool isUpdated(void) const { return _is_updated; }

    void updateDeltaBiases(
        const Eigen::Vector3d &d_b_a_i,
        const Eigen::Vector3d &d_b_g_i
    ) {
        std::lock_guard<std::mutex> l(_m);

        _is_updated = true;

        _d_b_a_i += d_b_a_i;
        _d_b_g_i += d_b_g_i;
    }

    void getDeltaBiases(Eigen::Vector3d &d_b_a_i, Eigen::Vector3d &d_b_g_i) {
        std::lock_guard<std::mutex> l(_m);

        d_b_a_i = _d_b_a_i;
        d_b_g_i = _d_b_g_i;

        _d_b_a_i = _d_b_g_i = Eigen::Vector3d::Zero();

        _is_updated = false;
    }

    virtual bool read(std::istream &in) { return true; }

    virtual bool write(std::ostream &out) const { return true; }

private:
    std::mutex _m;
    bool _is_updated = false;

    Eigen::Vector3d _d_b_a_i = Eigen::Vector3d::Zero();
    Eigen::Vector3d _d_b_g_i = Eigen::Vector3d::Zero();
};

} // namespace g2o

#endif // LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_VERTEX_VERTEX_PRVAG_HPP_


#include <iomanip>
#include <memory>
#include <vector>

#include <Eigen/Core>
#include <aslam/common/feature-descriptor-ref.h>
#include <aslam/common/memory.h>
#include <aslam/common/statistics/statistics.h>
#include <aslam/frames/visual-nframe.h>
#include <eigen-checks/gtest.h>
#include <glog/logging.h>
#include <gtest/gtest.h>
#include <maplab-common/feature-descriptor-ref.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>

#include "simulation/generic-path-generator.h"
#include "simulation/visual-nframe-simulator-channels.h"
#include "simulation/visual-nframe-simulator.h"

namespace simulation {

class VisualNFrameSimulatorTest
    : public ::testing::TestWithParam<test_trajectory_gen::Path> {
 protected:
  virtual void SetUp() {
    num_cameras_ = 1;
    settings_.imu_sigmas.acc_bias_random_walk_noise_density = 0.1;
    settings_.imu_sigmas.acc_noise_density = 0.1;
    settings_.gravity_meter_by_second2 = 9.81;
    settings_.imu_sigmas.gyro_bias_random_walk_noise_density = 0.1;
    settings_.imu_sigmas.gyro_noise_density = 0.1;
    settings_.imu_noise_bias_seed = 10u;
    settings_.num_of_path_constraints = 8;
    settings_.landmark_seed = 5u;
    settings_.landmark_variance_meter = 0.25;
    settings_.num_of_landmarks = 1500;
    settings_.sampling_time_second = 0.1;
    settings_.distance_to_keypoints_meter = 7.5;

    for (size_t i = 0; i < 10; ++i) {
      settings_.pose_waypoints.emplace_back(i / 5.0, i / 5.0, 0.0, 0.0);
    }

    // Create the camera rig.
    camera_rig_ = aslam::NCamera::createTestNCamera(num_cameras_);
  }
  test_trajectory_gen::PathAndLandmarkSettings settings_;
  std::shared_ptr<test_trajectory_gen::GenericPathGenerator> path_generator_;
  aslam::NCamera::Ptr camera_rig_;
  simulation::VisualNFrameSimulator nframe_simulator_;
  aslam::TransformationVector ground_truth_transformations_;

  Eigen::Matrix3Xd G_landmarks_;

  size_t num_cameras_;
  size_t num_landmarks_;
};

// Selects the desired trajectory
// - path from circle equation:  mode = 1
// - path from elliptical equation: mode = 2
// - rotation-only path: mode = 3
// - translation-only path: mode = 4
// - path from waypoints (file): mode = 5
// - path from waypoints (ctor): mode = 6.
INSTANTIATE_TEST_CASE_P(
    PathModes, VisualNFrameSimulatorTest,
    ::testing::Values(
        test_trajectory_gen::Path::kCircular,
        test_trajectory_gen::Path::kElliptical,
        test_trajectory_gen::Path::kRotationOnly,
        test_trajectory_gen::Path::kTranslationOnly,
        test_trajectory_gen::Path::kFromFile,
        test_trajectory_gen::Path::kFromCtor));

TEST_P(VisualNFrameSimulatorTest, testSimulateNFrames_noNoise) {
  settings_.mode = GetParam();
  path_generator_ =
      aligned_shared<test_trajectory_gen::GenericPathGenerator>(settings_);

  path_generator_->generatePath();
  path_generator_->generateLandmarks();

  G_landmarks_ = path_generator_->getLandmarks();
  num_landmarks_ = static_cast<size_t>(G_landmarks_.cols());
  path_generator_->getGroundTruthTransformations(
      &ground_truth_transformations_);

  aslam::VisualNFrame::PtrVector nframes;
  bool add_noise_to_keypoints = false;
  size_t num_bits_to_flip = 0;
  const double kKeypointSigma = 0.8;

  aslam::TransformationVector T_G_Bs;
  path_generator_->getGroundTruthTransformations(&T_G_Bs);
  aslam::TransformationVector poses_without_keypoints;
  nframe_simulator_.simulateVisualNFrames(
      path_generator_->getTimestampsInSeconds(), T_G_Bs,
      path_generator_->getLandmarks(), camera_rig_, kKeypointSigma,
      add_noise_to_keypoints, num_bits_to_flip, &nframes,
      &poses_without_keypoints);
  EXPECT_TRUE(poses_without_keypoints.empty());

  const aslam::VisualFrame::DescriptorsT& ground_truth_descriptors =
      nframe_simulator_.getGroundTruthDescriptors();
  ASSERT_EQ(ground_truth_descriptors.cols(), static_cast<int>(num_landmarks_));
  const size_t num_frames = nframes.size();
  ASSERT_EQ(ground_truth_transformations_.size(), nframes.size());
  const Eigen::VectorXd& ground_truth_landmark_scores =
      nframe_simulator_.getGroundTruthLandmarkScores();

  for (size_t nframe_idx = 0; nframe_idx < num_frames; ++nframe_idx) {
    const aslam::VisualNFrame::Ptr& nframe = nframes[nframe_idx];
    const aslam::Transformation& T_G_B =
        ground_truth_transformations_[nframe_idx];
    ASSERT_EQ(camera_rig_->getNumCameras(), num_cameras_);

    for (size_t camera_idx = 0; camera_idx < num_cameras_; ++camera_idx) {
      const aslam::VisualFrame& frame = nframe->getFrame(camera_idx);
      const aslam::Camera& camera = nframe->getCamera(camera_idx);
      const aslam::Transformation& T_C_B = nframe->get_T_C_B(camera_idx);

      const Eigen::VectorXi& landmark_id_channel =
          nframe_channels::getGroundTruthLandmarkIds(frame);

      size_t num_keypoints = frame.getNumKeypointMeasurements();
      for (size_t keypoint_idx = 0; keypoint_idx < num_keypoints;
           ++keypoint_idx) {
        // Get the id of the associated landmark.s
        int keypoint_landmarkid = landmark_id_channel(keypoint_idx);
        EXPECT_LT(keypoint_landmarkid, G_landmarks_.cols());

        // Reproject the groundtruth landmark and compare to stored keypoint.
        Eigen::Vector2d keypoint_groundtruth;
        Eigen::Vector3d G_landmark = G_landmarks_.col(keypoint_landmarkid);
        Eigen::Vector3d C_landmark = T_C_B * T_G_B.inverse() * G_landmark;
        aslam::ProjectionResult res =
            camera.project3(C_landmark, &keypoint_groundtruth);

        EXPECT_EQ(
            res.getDetailedStatus(), aslam::ProjectionResult::KEYPOINT_VISIBLE);
        EXPECT_NEAR_EIGEN(
            frame.getKeypointMeasurement(keypoint_idx), keypoint_groundtruth,
            1e-12);

        double score_groundtruth =
            ground_truth_landmark_scores(keypoint_landmarkid);
        EXPECT_EQ(frame.getKeypointScore(keypoint_idx), score_groundtruth);
        EXPECT_EQ(
            frame.getKeypointMeasurementUncertainty(keypoint_idx),
            kKeypointSigma * kKeypointSigma);

        // Test descriptor.
        aslam::common::FeatureDescriptorConstRef noisy_descriptor(
            frame.getDescriptor(keypoint_idx),
            VisualNFrameSimulator::kDescriptorSizeBytes);
        aslam::common::FeatureDescriptorConstRef ground_truth_descriptor(
            &ground_truth_descriptors.coeffRef(0, keypoint_landmarkid),
            VisualNFrameSimulator::kDescriptorSizeBytes);

        EXPECT_EQ(
            aslam::common::GetNumBitsDifferent(
                noisy_descriptor, ground_truth_descriptor),
            0u);
      }
      EXPECT_GT(frame.getNumKeypointMeasurements(), 50u)
          << "Less than 50 landmarks seen in the "
             "frame. This indicates that something might be wrong (camera "
             "extrinsics, rotation "
             "conventions, etc.). Note: this is soft lower bound. The actual "
             "number of visible "
             "landmarks per frame should be around 80, fluctuating with noise.";
    }
  }
}

TEST_P(VisualNFrameSimulatorTest, testSimulateNFrames_withNoise) {
  settings_.mode = GetParam();
  path_generator_ =
      aligned_shared<test_trajectory_gen::GenericPathGenerator>(settings_);

  path_generator_->generatePath();
  path_generator_->generateLandmarks();

  G_landmarks_ = path_generator_->getLandmarks();
  num_landmarks_ = static_cast<size_t>(G_landmarks_.cols());
  path_generator_->getGroundTruthTransformations(
      &ground_truth_transformations_);

  aslam::VisualNFrame::PtrVector nframes;
  bool add_noise_to_keypoints = true;
  size_t num_bits_to_flip = 5;
  const double kKeypointSigma = 0.8;

  aslam::TransformationVector T_G_Bs;
  path_generator_->getGroundTruthTransformations(&T_G_Bs);
  aslam::TransformationVector poses_without_keypoints;
  nframe_simulator_.simulateVisualNFrames(
      path_generator_->getTimestampsInSeconds(), T_G_Bs,
      path_generator_->getLandmarks(), camera_rig_, kKeypointSigma,
      add_noise_to_keypoints, num_bits_to_flip, &nframes,
      &poses_without_keypoints);
  EXPECT_TRUE(poses_without_keypoints.empty());

  const aslam::VisualFrame::DescriptorsT& ground_truth_descriptors =
      nframe_simulator_.getGroundTruthDescriptors();
  ASSERT_EQ(ground_truth_descriptors.cols(), static_cast<int>(num_landmarks_));
  const size_t num_frames = nframes.size();
  ASSERT_EQ(ground_truth_transformations_.size(), nframes.size());

  statistics::StatsCollectorImpl stat_keypoint_noise_u("keypoint_noise_u");
  statistics::StatsCollectorImpl stat_keypoint_noise_v("keypoint_noise_v");
  for (size_t nframe_idx = 0; nframe_idx < num_frames; ++nframe_idx) {
    const aslam::VisualNFrame::Ptr& nframe = nframes[nframe_idx];
    const aslam::Transformation& T_G_B =
        ground_truth_transformations_[nframe_idx];
    ASSERT_EQ(camera_rig_->getNumCameras(), num_cameras_);

    for (size_t camera_idx = 0; camera_idx < num_cameras_; ++camera_idx) {
      const aslam::VisualFrame& frame = nframe->getFrame(camera_idx);
      const aslam::Camera& camera = nframe->getCamera(camera_idx);
      const aslam::Transformation& T_C_B = nframe->get_T_C_B(camera_idx);

      const Eigen::VectorXi& landmark_id_channel =
          nframe_channels::getGroundTruthLandmarkIds(frame);

      size_t num_keypoints = frame.getNumKeypointMeasurements();
      for (size_t keypoint_idx = 0; keypoint_idx < num_keypoints;
           ++keypoint_idx) {
        // Get the id of the associated landmark.s
        int keypoint_landmarkid = landmark_id_channel(keypoint_idx);
        EXPECT_LT(keypoint_landmarkid, G_landmarks_.cols());

        // Reproject the groundtruth landmark and compare to stored keypoint.
        Eigen::Vector2d keypoint_groundtruth;
        Eigen::Vector3d G_landmark = G_landmarks_.col(keypoint_landmarkid);
        Eigen::Vector3d C_landmark = T_C_B * T_G_B.inverse() * G_landmark;
        aslam::ProjectionResult res =
            camera.project3(C_landmark, &keypoint_groundtruth);
        EXPECT_TRUE(
            res.getDetailedStatus() !=
                aslam::ProjectionResult::PROJECTION_INVALID ||
            res.getDetailedStatus() !=
                aslam::ProjectionResult::POINT_BEHIND_CAMERA);
        Eigen::Vector2d keypoint_noise =
            keypoint_groundtruth - frame.getKeypointMeasurement(keypoint_idx);

        stat_keypoint_noise_u.AddSample(keypoint_noise(0));
        stat_keypoint_noise_v.AddSample(keypoint_noise(1));

        // Test descriptor.
        aslam::common::FeatureDescriptorConstRef noisy_descriptor(
            frame.getDescriptor(keypoint_idx),
            VisualNFrameSimulator::kDescriptorSizeBytes);
        aslam::common::FeatureDescriptorConstRef ground_truth_descriptor(
            &ground_truth_descriptors.coeffRef(0, keypoint_landmarkid),
            VisualNFrameSimulator::kDescriptorSizeBytes);

        EXPECT_EQ(
            aslam::common::GetNumBitsDifferent(
                noisy_descriptor, ground_truth_descriptor),
            num_bits_to_flip);
      }
    }
  }

  double keypoint_var = kKeypointSigma * kKeypointSigma;
  EXPECT_NEAR(
      statistics::Statistics::GetVariance("keypoint_noise_u"), keypoint_var,
      1e-1);
  EXPECT_NEAR(
      statistics::Statistics::GetVariance("keypoint_noise_v"), keypoint_var,
      3e-1);
}

}  // namespace simulation

MAPLAB_UNITTEST_ENTRYPOINT


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

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

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

See LICENSE.txt or http://www.mitk.org for details.

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

// Blueberry
#include <berryISelectionService.h>
#include <berryIWorkbenchWindow.h>

// Qmitk
#include "QmitkFiberQuantificationView.h"
#include <QmitkStdMultiWidget.h>

// Qt
#include <QMessageBox>

// MITK
#include <mitkNodePredicateProperty.h>
#include <mitkImageCast.h>
#include <mitkPointSet.h>
#include <mitkPlanarCircle.h>
#include <mitkPlanarPolygon.h>
#include <mitkPlanarRectangle.h>
#include <mitkPlanarFigureInteractor.h>
#include <mitkGlobalInteraction.h>
#include <mitkImageAccessByItk.h>
#include <mitkDataNodeObject.h>
#include <mitkDiffusionImage.h>
#include <mitkTensorImage.h>

// ITK
#include <itkResampleImageFilter.h>
#include <itkGaussianInterpolateImageFunction.h>
#include <itkImageRegionIteratorWithIndex.h>
#include <itkTractsToFiberEndingsImageFilter.h>
#include <itkTractDensityImageFilter.h>
#include <itkImageRegion.h>
#include <itkTractsToRgbaImageFilter.h>
#include <itkTractsToVectorImageFilter.h>

#include <math.h>
#include <boost/lexical_cast.hpp>

const std::string QmitkFiberQuantificationView::VIEW_ID = "org.mitk.views.fiberquantification";
const std::string id_DataManager = "org.mitk.views.datamanager";
using namespace mitk;

QmitkFiberQuantificationView::QmitkFiberQuantificationView()
    : QmitkFunctionality()
    , m_Controls( 0 )
    , m_MultiWidget( NULL )
    , m_UpsamplingFactor(5)
{

}

// Destructor
QmitkFiberQuantificationView::~QmitkFiberQuantificationView()
{

}

void QmitkFiberQuantificationView::CreateQtPartControl( QWidget *parent )
{
    // build up qt view, unless already done
    if ( !m_Controls )
    {
        // create GUI widgets from the Qt Designer's .ui file
        m_Controls = new Ui::QmitkFiberQuantificationViewControls;
        m_Controls->setupUi( parent );

        connect( m_Controls->m_ProcessFiberBundleButton, SIGNAL(clicked()), this, SLOT(ProcessSelectedBundles()) );
        connect( m_Controls->m_ExtractFiberPeaks, SIGNAL(clicked()), this, SLOT(CalculateFiberDirections()) );
    }
}

void QmitkFiberQuantificationView::StdMultiWidgetAvailable (QmitkStdMultiWidget &stdMultiWidget)
{
    m_MultiWidget = &stdMultiWidget;
}

void QmitkFiberQuantificationView::StdMultiWidgetNotAvailable()
{
    m_MultiWidget = NULL;
}

void QmitkFiberQuantificationView::CalculateFiberDirections()
{
    typedef itk::Image<unsigned char, 3>                                            ItkUcharImgType;
    typedef itk::Image< itk::Vector< float, 3>, 3 >                                 ItkDirectionImage3DType;
    typedef itk::VectorContainer< unsigned int, ItkDirectionImage3DType::Pointer >  ItkDirectionImageContainerType;

    // load fiber bundle
    mitk::FiberBundleX::Pointer inputTractogram = dynamic_cast<mitk::FiberBundleX*>(m_SelectedFB.back()->GetData());

    itk::TractsToVectorImageFilter<float>::Pointer fOdfFilter = itk::TractsToVectorImageFilter<float>::New();
    if (m_SelectedImage.IsNotNull())
    {
        ItkUcharImgType::Pointer itkMaskImage = ItkUcharImgType::New();
        mitk::CastToItkImage<ItkUcharImgType>(m_SelectedImage, itkMaskImage);
        fOdfFilter->SetMaskImage(itkMaskImage);
    }

    // extract directions from fiber bundle
    fOdfFilter->SetFiberBundle(inputTractogram);
    fOdfFilter->SetAngularThreshold(cos(m_Controls->m_AngularThreshold->value()*M_PI/180));
    fOdfFilter->SetNormalizeVectors(m_Controls->m_NormalizeDirectionsBox->isChecked());
    fOdfFilter->SetUseWorkingCopy(true);
    fOdfFilter->SetCreateDirectionImages(m_Controls->m_DirectionImagesBox->isChecked());
    fOdfFilter->SetSizeThreshold(m_Controls->m_PeakThreshold->value());
    fOdfFilter->SetMaxNumDirections(m_Controls->m_MaxNumDirections->value());
    fOdfFilter->Update();

    QString name = m_SelectedFB.back()->GetName().c_str();

    if (m_Controls->m_VectorFieldBox->isChecked())
    {
        float minSpacing = 1;
        if (m_SelectedImage.IsNotNull())
        {
            mitk::Vector3D outImageSpacing = m_SelectedImage->GetGeometry()->GetSpacing();

            if(outImageSpacing[0]<outImageSpacing[1] && outImageSpacing[0]<outImageSpacing[2])
                minSpacing = outImageSpacing[0];
            else if (outImageSpacing[1] < outImageSpacing[2])
                minSpacing = outImageSpacing[1];
            else
                minSpacing = outImageSpacing[2];
        }

        mitk::FiberBundleX::Pointer directions = fOdfFilter->GetOutputFiberBundle();
        mitk::DataNode::Pointer node = mitk::DataNode::New();
        node->SetData(directions);
        node->SetName((name+"_vectorfield").toStdString().c_str());
        node->SetProperty("Fiber2DSliceThickness", mitk::FloatProperty::New(minSpacing));
        node->SetProperty("Fiber2DfadeEFX", mitk::BoolProperty::New(false));
        node->SetProperty("color", mitk::ColorProperty::New(1.0f, 1.0f, 1.0f));

        GetDefaultDataStorage()->Add(node, m_SelectedFB.back());
    }

    if (m_Controls->m_NumDirectionsBox->isChecked())
    {
        mitk::Image::Pointer mitkImage = mitk::Image::New();
        mitkImage->InitializeByItk( fOdfFilter->GetNumDirectionsImage().GetPointer() );
        mitkImage->SetVolume( fOdfFilter->GetNumDirectionsImage()->GetBufferPointer() );

        mitk::DataNode::Pointer node = mitk::DataNode::New();
        node->SetData(mitkImage);
        node->SetName((name+"_numdirections").toStdString().c_str());
        GetDefaultDataStorage()->Add(node, m_SelectedFB.back());
    }

    if (m_Controls->m_DirectionImagesBox->isChecked())
    {
        ItkDirectionImageContainerType::Pointer directionImageContainer = fOdfFilter->GetDirectionImageContainer();
        for (unsigned int i=0; i<directionImageContainer->Size(); i++)
        {
            itk::TractsToVectorImageFilter<float>::ItkDirectionImageType::Pointer itkImg = directionImageContainer->GetElement(i);

            if (itkImg.IsNull())
                return;

            mitk::Image::Pointer mitkImage = mitk::Image::New();
            mitkImage->InitializeByItk( itkImg.GetPointer() );
            mitkImage->SetVolume( itkImg->GetBufferPointer() );

            mitk::DataNode::Pointer node = mitk::DataNode::New();
            node->SetData(mitkImage);
            node->SetName( (name+"_direction_"+boost::lexical_cast<std::string>(i).c_str()).toStdString().c_str());
            node->SetVisibility(false);
            GetDefaultDataStorage()->Add(node, m_SelectedFB.back());
        }
    }
}

void QmitkFiberQuantificationView::UpdateGui()
{
    m_Controls->m_ProcessFiberBundleButton->setEnabled(!m_SelectedFB.empty());
    m_Controls->m_ExtractFiberPeaks->setEnabled(!m_SelectedFB.empty());
}

void QmitkFiberQuantificationView::OnSelectionChanged( std::vector<mitk::DataNode*> nodes )
{
    //reset existing Vectors containing FiberBundles and PlanarFigures from a previous selection
    m_SelectedFB.clear();
    m_SelectedSurfaces.clear();
    m_SelectedImage = NULL;

    for( std::vector<mitk::DataNode*>::iterator it = nodes.begin(); it != nodes.end(); ++it )
    {
        mitk::DataNode::Pointer node = *it;
        if ( dynamic_cast<mitk::FiberBundleX*>(node->GetData()) )
        {
            m_SelectedFB.push_back(node);
        }
        else if (dynamic_cast<mitk::Image*>(node->GetData()))
            m_SelectedImage = dynamic_cast<mitk::Image*>(node->GetData());
        else if (dynamic_cast<mitk::Surface*>(node->GetData()))
        {
            m_SelectedSurfaces.push_back(dynamic_cast<mitk::Surface*>(node->GetData()));
        }
    }
    UpdateGui();
    GenerateStats();
}

void QmitkFiberQuantificationView::Activated()
{

}

void QmitkFiberQuantificationView::GenerateStats()
{
    if ( m_SelectedFB.empty() )
        return;

    QString stats("");

    for( int i=0; i<m_SelectedFB.size(); i++ )
    {
        mitk::DataNode::Pointer node = m_SelectedFB[i];
        if (node.IsNotNull() && dynamic_cast<mitk::FiberBundleX*>(node->GetData()))
        {
            if (i>0)
                stats += "\n-----------------------------\n";
            stats += QString(node->GetName().c_str()) + "\n";
            mitk::FiberBundleX::Pointer fib = dynamic_cast<mitk::FiberBundleX*>(node->GetData());
            stats += "Number of fibers: "+ QString::number(fib->GetNumFibers()) + "\n";
            stats += "Number of points: "+ QString::number(fib->GetNumberOfPoints()) + "\n";
            stats += "Min. length:         "+ QString::number(fib->GetMinFiberLength(),'f',1) + " mm\n";
            stats += "Max. length:         "+ QString::number(fib->GetMaxFiberLength(),'f',1) + " mm\n";
            stats += "Mean length:         "+ QString::number(fib->GetMeanFiberLength(),'f',1) + " mm\n";
            stats += "Median length:       "+ QString::number(fib->GetMedianFiberLength(),'f',1) + " mm\n";
            stats += "Standard deviation:  "+ QString::number(fib->GetLengthStDev(),'f',1) + " mm\n";
        }
    }
    this->m_Controls->m_StatsTextEdit->setText(stats);
}

void QmitkFiberQuantificationView::ProcessSelectedBundles()
{
    if ( m_SelectedFB.empty() ){
        QMessageBox::information( NULL, "Warning", "No fibe bundle selected!");
        MITK_WARN("QmitkFiberQuantificationView") << "no fibe bundle selected";
        return;
    }

    int generationMethod = m_Controls->m_GenerationBox->currentIndex();

    for( int i=0; i<m_SelectedFB.size(); i++ )
    {
        mitk::DataNode::Pointer node = m_SelectedFB[i];
        if (node.IsNotNull() && dynamic_cast<mitk::FiberBundleX*>(node->GetData()))
        {
            mitk::FiberBundleX::Pointer fib = dynamic_cast<mitk::FiberBundleX*>(node->GetData());
            QString name(node->GetName().c_str());
            DataNode::Pointer newNode = NULL;
            switch(generationMethod){
            case 0:
                newNode = GenerateTractDensityImage(fib, false, true);
                name += "_TDI";
                break;
            case 1:
                newNode = GenerateTractDensityImage(fib, false, false);
                name += "_TDI";
                break;
            case 2:
                newNode = GenerateTractDensityImage(fib, true, false);
                name += "_envelope";
                break;
            case 3:
                newNode = GenerateColorHeatmap(fib);
                break;
            case 4:
                newNode = GenerateFiberEndingsImage(fib);
                name += "_fiber_endings";
                break;
            case 5:
                newNode = GenerateFiberEndingsPointSet(fib);
                name += "_fiber_endings";
                break;
            }
            if (newNode.IsNotNull())
            {
                newNode->SetName(name.toStdString());
                GetDataStorage()->Add(newNode);
            }
        }
    }
}

// generate pointset displaying the fiber endings
mitk::DataNode::Pointer QmitkFiberQuantificationView::GenerateFiberEndingsPointSet(mitk::FiberBundleX::Pointer fib)
{
    mitk::PointSet::Pointer pointSet = mitk::PointSet::New();
    vtkSmartPointer<vtkPolyData> fiberPolyData = fib->GetFiberPolyData();
    vtkSmartPointer<vtkCellArray> vLines = fiberPolyData->GetLines();
    vLines->InitTraversal();

    int count = 0;
    int numFibers = fib->GetNumFibers();
    for( int i=0; i<numFibers; i++ )
    {
        vtkIdType   numPoints(0);
        vtkIdType*  points(NULL);
        vLines->GetNextCell ( numPoints, points );

        if (numPoints>0)
        {
            double* point = fiberPolyData->GetPoint(points[0]);
            itk::Point<float,3> itkPoint;
            itkPoint[0] = point[0];
            itkPoint[1] = point[1];
            itkPoint[2] = point[2];
            pointSet->InsertPoint(count, itkPoint);
            count++;
        }
        if (numPoints>2)
        {
            double* point = fiberPolyData->GetPoint(points[numPoints-1]);
            itk::Point<float,3> itkPoint;
            itkPoint[0] = point[0];
            itkPoint[1] = point[1];
            itkPoint[2] = point[2];
            pointSet->InsertPoint(count, itkPoint);
            count++;
        }
    }

    mitk::DataNode::Pointer node = mitk::DataNode::New();
    node->SetData( pointSet );
    return node;
}

// generate image displaying the fiber endings
mitk::DataNode::Pointer QmitkFiberQuantificationView::GenerateFiberEndingsImage(mitk::FiberBundleX::Pointer fib)
{
    typedef unsigned char OutPixType;
    typedef itk::Image<OutPixType,3> OutImageType;

    typedef itk::TractsToFiberEndingsImageFilter< OutImageType > ImageGeneratorType;
    ImageGeneratorType::Pointer generator = ImageGeneratorType::New();
    generator->SetFiberBundle(fib);
    generator->SetUpsamplingFactor(m_Controls->m_UpsamplingSpinBox->value());
    if (m_SelectedImage.IsNotNull())
    {
        OutImageType::Pointer itkImage = OutImageType::New();
        CastToItkImage(m_SelectedImage, itkImage);
        generator->SetInputImage(itkImage);
        generator->SetUseImageGeometry(true);
    }
    generator->Update();

    // get output image
    OutImageType::Pointer outImg = generator->GetOutput();
    mitk::Image::Pointer img = mitk::Image::New();
    img->InitializeByItk(outImg.GetPointer());
    img->SetVolume(outImg->GetBufferPointer());

    // init data node
    mitk::DataNode::Pointer node = mitk::DataNode::New();
    node->SetData(img);
    return node;
}

// generate rgba heatmap from fiber bundle
mitk::DataNode::Pointer QmitkFiberQuantificationView::GenerateColorHeatmap(mitk::FiberBundleX::Pointer fib)
{
    typedef itk::RGBAPixel<unsigned char> OutPixType;
    typedef itk::Image<OutPixType, 3> OutImageType;
    typedef itk::TractsToRgbaImageFilter< OutImageType > ImageGeneratorType;
    ImageGeneratorType::Pointer generator = ImageGeneratorType::New();
    generator->SetFiberBundle(fib);
    generator->SetUpsamplingFactor(m_Controls->m_UpsamplingSpinBox->value());
    if (m_SelectedImage.IsNotNull())
    {
        itk::Image<unsigned char, 3>::Pointer itkImage = itk::Image<unsigned char, 3>::New();
        CastToItkImage(m_SelectedImage, itkImage);
        generator->SetInputImage(itkImage);
        generator->SetUseImageGeometry(true);
    }
    generator->Update();

    // get output image
    typedef itk::Image<OutPixType,3> OutType;
    OutType::Pointer outImg = generator->GetOutput();
    mitk::Image::Pointer img = mitk::Image::New();
    img->InitializeByItk(outImg.GetPointer());
    img->SetVolume(outImg->GetBufferPointer());

    // init data node
    mitk::DataNode::Pointer node = mitk::DataNode::New();
    node->SetData(img);
    return node;
}

// generate tract density image from fiber bundle
mitk::DataNode::Pointer QmitkFiberQuantificationView::GenerateTractDensityImage(mitk::FiberBundleX::Pointer fib, bool binary, bool absolute)
{
    typedef float OutPixType;
    typedef itk::Image<OutPixType, 3> OutImageType;

    itk::TractDensityImageFilter< OutImageType >::Pointer generator = itk::TractDensityImageFilter< OutImageType >::New();
    generator->SetFiberBundle(fib);
    generator->SetBinaryOutput(binary);
    generator->SetOutputAbsoluteValues(absolute);
    generator->SetUpsamplingFactor(m_Controls->m_UpsamplingSpinBox->value());
    if (m_SelectedImage.IsNotNull())
    {
        OutImageType::Pointer itkImage = OutImageType::New();
        CastToItkImage(m_SelectedImage, itkImage);
        generator->SetInputImage(itkImage);
        generator->SetUseImageGeometry(true);

    }
    generator->Update();

    // get output image
    typedef itk::Image<OutPixType,3> OutType;
    OutType::Pointer outImg = generator->GetOutput();
    mitk::Image::Pointer img = mitk::Image::New();
    img->InitializeByItk(outImg.GetPointer());
    img->SetVolume(outImg->GetBufferPointer());

    // init data node
    mitk::DataNode::Pointer node = mitk::DataNode::New();
    node->SetData(img);
    return node;
}


// Copyright (C) 2008-today The SG++ project
// This file is part of the SG++ project. For conditions of distribution and
// use, please see the copyright notice provided with SG++ or at
// sgpp.sparsegrids.org

/**
 * \page example_optimization_cpp optimization.cpp
 *
 * On this page, we look at an example application of the sgpp::optimization module.
 * Versions of the example are given in all languages
 * currently supported by SG++: C++, Python, Java, and MATLAB.
 *
 * The example interpolates a bivariate test function with B-splines instead
 * of piecewise linear basis functions to obtain a smoother interpolant.
 * The resulting sparse grid function is then minimized with the method of steepest descent.
 * For comparison, we also minimize the objective function with Nelder-Mead's method.
 */

/**
 * First, we include all the necessary headers, including those of the sgpp::base and
 * sgpp::optimization module.
 */
#include <sgpp_base.hpp>
#include <sgpp/base/grid/Grid.hpp>
#include <sgpp/base/datatypes/DataMatrix.hpp>
#include <sgpp/datadriven/DatadrivenOpFactory.hpp>
#include <sgpp/datadriven/application/KernelDensityEstimator.hpp>
#include <sgpp/datadriven/application/LearnerSGDE.hpp>
#include <sgpp/datadriven/configuration/RegularizationConfiguration.hpp>
#include <sgpp/datadriven/tools/ARFFTools.hpp>
#include <sgpp_optimization.hpp>
#include <sgpp/globaldef.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/ini_parser.hpp>

#include <algorithm>
#include <iostream>
#include <iterator>
#include <random>
#include <string>

using sgpp::base::DataMatrix;
using sgpp::base::DataVector;
using sgpp::base::Grid;
using sgpp::base::GridGenerator;
using sgpp::base::GridStorage;
using boost::property_tree::ptree;

/**
 * The function \f$f\colon [0, 1]^d \to \mathbb{R}\f$ to be minimized
 * is called <i>objective function</i> and has to derive from
 * sgpp::optimization::ScalarFunction.
 * In the constructor, we give the dimensionality of the domain
 * (in this case \f$d = 2\f$).
 * The eval method evaluates the objective function and returns the function
 * value \f$f(\vec{x})\f$ for a given point \f$\vec{x} \in [0, 1]^d\f$.
 * The clone method returns a std::unique_ptr to a clone of the object
 * and is used for parallelization (in case eval is not thread-safe).
 */
class ExampleFunction : public sgpp::optimization::ScalarFunction {
 public:
  sgpp::base::DataVector featureVector;
  sgpp::datadriven::LearnerSGDE& learner;

  // our optimization problem is 3dim
  ExampleFunction(sgpp::datadriven::LearnerSGDE& _learner) : sgpp::optimization::ScalarFunction(3), learner(_learner) {}

  // x is 3dim, we need to supply the rest (feature vector)
  double eval(const sgpp::base::DataVector& x) {

        sgpp::base::DataVector xNew(x);

        //xNew.append(featureVector.get(0));
        //xNew.append(featureVector.get(1));
        //xNew.append(featureVector.get(2));
        //xNew.append(featureVector.get(3));

        xNew.append(featureVector.get(0));
        xNew.append(featureVector.get(1));
        xNew.append(featureVector.get(2));
        xNew.append(featureVector.get(3));

        return learner.pdf(xNew);
  }

  virtual void clone(std::unique_ptr<sgpp::optimization::ScalarFunction>& clone) const {
    clone = std::unique_ptr<sgpp::optimization::ScalarFunction>(new ExampleFunction(*this));
  }

  void setParamters(const sgpp::base::DataVector& reqFeatureVector){
    featureVector=reqFeatureVector;
  }
};

/**
 * Now, we can start with the \c main function.
 */
void printLine() {
  std::cout << "----------------------------------------"
               "----------------------------------------\n";
}

int main(int argc, const char* argv[]) {
  (void)argc;
  (void)argv;

  double featureVectorCoeff_0 = atof(argv[3]);
  double featureVectorCoeff_1 = atof(argv[4]);
  double featureVectorCoeff_2 = atof(argv[5]);
  double featureVectorCoeff_3 = atof(argv[6]);

  std::string filename = "../" + std::string(argv[1]);
  std::string ininame = "../" + std::string(argv[2]);

  std::cout << "# loading file: " << filename << std::endl;
  sgpp::datadriven::Dataset dataset =
    sgpp::datadriven::ARFFTools::readARFFFromFile(filename);
  sgpp::base::DataMatrix& samples = dataset.getData();

  /**
   * Configure the sparse grid of level 3 with linear basis functions and the same dimension as the
   * given test data.
   * Alternatively load a sparse grid that has been saved to a file, see the commented line.
   */
  std::cout << "# create grid config" << std::endl;
  sgpp::base::RegularGridConfiguration gridConfig;
  gridConfig.dim_ = dataset.getDimension();
  gridConfig.level_ = 3;
  gridConfig.type_ = sgpp::base::GridType::Linear; //TODO: BSplines not available, but does not matter
  //  gridConfig.filename_ = "/tmp/sgde-grid-4391dc6e-54cd-4ca2-9510-a9c02a2889ec.grid";

  /**
   * Configure the adaptive refinement. Therefore the number of refinements and the number of points
   * are specified.
   */
  std::cout << "# create adaptive refinement config" << std::endl;
  sgpp::base::AdaptivityConfiguration adaptConfig;
  adaptConfig.numRefinements_ = 0;
  adaptConfig.noPoints_ = 10;

  /**
   * Configure the solver. The solver type is set to the conjugent gradient method and the maximum
   * number of iterations, the tolerance epsilon and the threshold are specified.
   */
  std::cout << "# create solver config" << std::endl;
  sgpp::solver::SLESolverConfiguration solverConfig;
  solverConfig.type_ = sgpp::solver::SLESolverType::CG;
  solverConfig.maxIterations_ = 1000;
  solverConfig.eps_ = 1e-14;
  solverConfig.threshold_ = 1e-14;

  /**
   * Configure the regularization for the laplacian operator.
   */
  std::cout << "# create regularization config" << std::endl;
  sgpp::datadriven::RegularizationConfiguration regularizationConfig;
  regularizationConfig.type_ = sgpp::datadriven::RegularizationType::Laplace;

  /**
   * Configure the learner by specifying:
   * - an initial value for the lagrangian multiplier \f$\lambda\f$ and the interval
   *   \f$ [\lambda_{Start} , \lambda_{End}] \f$ in which \f$\lambda\f$ will be searched,
   * - whether a logarithmic scale is used,
   * - the parameters shuffle and an initial seed for the random value generation,
   * - whether parts of the output shall be kept off.
   */
  std::cout << "# create learner config" << std::endl;
  sgpp::datadriven::CrossvalidationConfiguration crossvalidationConfig;
  crossvalidationConfig.enable_ = false;
  crossvalidationConfig.kfold_ = 3;
  crossvalidationConfig.lambda_ = 3.16228e-06;
  crossvalidationConfig.lambdaStart_ = 1e-1;
  crossvalidationConfig.lambdaEnd_ = 1e-10;
  crossvalidationConfig.lambdaSteps_ = 3;
  crossvalidationConfig.logScale_ = true;
  crossvalidationConfig.shuffle_ = true;
  crossvalidationConfig.seed_ = 1234567;
  crossvalidationConfig.silent_ = false;

  /**
   * Create the learner using the configuratons set above. Then initialize it with the data read
   * from the file in the first step and train the learner.
   */
  std::cout << "# creating the learner" << std::endl;
  sgpp::datadriven::LearnerSGDE learner(gridConfig, adaptConfig, solverConfig, regularizationConfig,
                                        crossvalidationConfig);
  learner.initialize(samples);
  learner.train();
  /**
   * Estimate the probability density function (pdf) via a gaussian kernel density estimation (KDE)
   * and print the corresponding values.
   */
  sgpp::datadriven::KernelDensityEstimator kde(samples);
  sgpp::base::DataVector x(learner.getDim());
  // samples.getRow(samples.getNrows()/2, x);
  samples.getRow(1, x);

  std::cout << "--------------------------------------------------------\n";
  std::cout << "x= " << x.toString() << "\n";
  std::cout << "SGDE dimensions " << learner.getDim() << "\n";
  std::cout << "SGDE sample count " << learner.getNsamples() << "\n";
  std::cout << learner.getSurpluses()->getSize() << " -> " << learner.getSurpluses()->sum() << "\n";
  std::cout << "pdf_SGDE(x) = " << learner.pdf(x) << " ~ " << kde.pdf(x) << " =pdf_KDE(x)\n";
  std::cout << "mean_SGDE(x) = " << learner.mean() << " ~ " << kde.mean() << " = mean_KDE(x)\n";
  std::cout << "var_SGDE(x) = " << learner.variance() << " ~ " << kde.variance() << "=var_KDE(x)\n";

  std::cout << "sgpp::optimization example program started.\n\n";
  // increase verbosity of the output
  sgpp::optimization::Printer::getInstance().setVerbosity(2);

  /**
   * Here, we define some parameters: objective function, dimensionality,
   * B-spline degree, maximal number of grid points, and adaptivity.
   */
  // objective function
  ExampleFunction f(learner);
  sgpp::base::DataVector testFV;
  testFV.append(featureVectorCoeff_0);
  testFV.append(featureVectorCoeff_1);
  testFV.append(featureVectorCoeff_2);
  testFV.append(featureVectorCoeff_3);

  f.setParamters(testFV);
  // dimension of domain
  const size_t d = f.getNumberOfParameters();
  // B-spline degree
  const size_t p = 3;
  // maximal number of grid points
  const size_t N = 30;
  // adaptivity of grid generation
  const double gamma = 0.95;

  /**
   * First, we define a grid with modified B-spline basis functions and
   * an iterative grid generator, which can generate the grid adaptively.
   */
  sgpp::base::ModBsplineGrid grid(d, p);
  sgpp::optimization::IterativeGridGeneratorRitterNovak gridGen(f, grid, N, gamma);

  /**
   * With the iterative grid generator, we generate adaptively a sparse grid.
   */
  printLine();
  std::cout << "Generating grid...\n\n";

  if (!gridGen.generate()) {
    std::cout << "Grid generation failed, exiting.\n";
    return 1;
  }

  /**
   * Then, we hierarchize the function values to get hierarchical B-spline
   * coefficients of the B-spline sparse grid interpolant
   * \f$\tilde{f}\colon [0, 1]^d \to \mathbb{R}\f$.
   */
  printLine();
  std::cout << "Hierarchizing...\n\n";
  sgpp::base::DataVector functionValues(gridGen.getFunctionValues());
  sgpp::base::DataVector coeffs(functionValues.getSize());
  sgpp::optimization::HierarchisationSLE hierSLE(grid);
  sgpp::optimization::sle_solver::Auto sleSolver;

  // solve linear system
  if (!sleSolver.solve(hierSLE, functionValues, coeffs)) {
    std::cout << "Solving failed, exiting.\n";
    return 1;
  }

  /**
   * We define the interpolant \f$\tilde{f}\f$ and its gradient
   * \f$\nabla\tilde{f}\f$ for use with the gradient method (steepest descent).
   * Of course, one can also use other optimization algorithms from
   * sgpp::optimization::optimizer.
   */
  printLine();
  std::cout << "Optimizing smooth interpolant...\n\n";
  sgpp::optimization::InterpolantScalarFunction ft(grid, coeffs);
  sgpp::optimization::InterpolantScalarFunctionGradient ftGradient(grid, coeffs);
  sgpp::optimization::optimizer::GradientDescent gradientDescent(ft, ftGradient);
  sgpp::base::DataVector x0(d);
  double fX0;
  double ftX0;

  /**
   * The gradient method needs a starting point.
   * We use a point of our adaptively generated sparse grid as starting point.
   * More specifically, we use the point with the smallest
   * (most promising) function value and save it in x0.
   */
  {
    sgpp::base::GridStorage& gridStorage = grid.getStorage();

    // index of grid point with minimal function value
    size_t x0Index =
        std::distance(functionValues.getPointer(),
                      std::min_element(functionValues.getPointer(),
                                       functionValues.getPointer() + functionValues.getSize()));

    //x0 = gridStorage.getCoordinates(gridStorage[x0Index]);

    x0.set(0, samples.get(0,0)/2);
    x0.set(1, samples.get(0,1));
    x0.set(2, samples.get(0,2));

    fX0 = functionValues[x0Index];
    ftX0 = ft.eval(x0);
  }

  std::cout << "Gradient starting point:\n";
  std::cout << "x0 = " << x0.toString() << "\n";
  std::cout << "f(x0) = " << fX0 << ", ft(x0) = " << ftX0 << "\n\n";

  /**
   * We apply the gradient method and print the results.
   */
  gradientDescent.setStartingPoint(x0);
  gradientDescent.optimize();
  const sgpp::base::DataVector& xOpt = gradientDescent.getOptimalPoint();
  const double ftXOpt = gradientDescent.getOptimalValue();
  const double fXOpt = f.eval(xOpt);

  std::cout << "\noptimal control point:\n";
  std::cout << "for feature vector" << testFV.toString() << "\n";
  std::cout << "xOpt = " << xOpt.toString() << "\n";
  std::cout << "f(xOpt) = " << fXOpt << ", ft(xOpt) = " << ftXOpt << "\n\n";

  /**
   * We modify the control point inside the .ini file
   */

   ptree pt;

   boost::property_tree::ini_parser::read_ini( ininame, pt );

   pt.put("Simulation.cpX", xOpt.get(0));
   pt.put("Simulation.cpY", xOpt.get(1));
   pt.put("Simulation.cpZ", xOpt.get(2));

   boost::property_tree::write_ini( ininame, pt );

  return 0;
}


/*
 *    File:        datset.hpp
 *    Author(s):   Saswati Ray
 *    Created:     Wed Aug 30 10:19:52 EDT 2017
 *    Description:
 *    Copyright (c) 2017 Carnegie Mellon University
 *    */

#ifndef DATSET_H_
#define DATSET_H_

#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION

#include <boost/python.hpp>
#include <numpy/arrayobject.h>
namespace p = boost::python;

#include <boost/numeric/ublas/matrix.hpp>

using namespace boost::numeric::ublas;

/*
 * Class representing user data.
 * Consists of input feature space (matrix) and output vector.
 * Datset can be created by loading from csv file or by feeding in values from numpy arrays
 */
class Datset {
 private:
  matrix<double> *darray;
  std::vector<int> *output_class;
  std::vector<int> *training_rows;
  std::vector<double> *output_regress;
  int rows, cols, num_classes;
  bool is_classifier;

 public:
  Datset();
  Datset(PyObject *array);

  void fill_datset_output_for_classification(PyObject *array);
  void fill_datset_output_for_regression(PyObject *array);
  void set_training_rows(PyObject *array);
  std::vector<int> *get_training_rows() {
    return training_rows;
  }

  double ds_real_ref(int i, int j);
  double ds_output_ref(int i);
  double get_default_value();

  int get_num_classes() {
    return num_classes;
  }

  ~Datset();

  bool is_classification() {
    return is_classifier;
  }

  bool is_valid() {
    return (darray && (output_class || output_regress));
  }

  int get_rows() {
    return rows;
  }
  int get_cols() {
    return cols;
  }
};

#endif


#include "iotbx/xplor/map_reader.h"

#include <fstream>
#include <cstdlib>
#include <cstdio>
#include <iotbx/error.h>
#include <scitbx/array_family/accessors/c_grid.h>
#include <boost/algorithm/string/trim.hpp>

namespace iotbx { namespace xplor {

namespace af = scitbx::af;

map_reader::map_reader(
  std::string const& file_name,
  std::size_t n_header_lines,
  af::flex_grid<> const& grid)
:
  data(grid, 0)
{
  this->load(file_name, n_header_lines, grid);
}

map_reader::map_reader(const std::string &file_name, bool header_only)
{
  std::ifstream f(file_name.c_str());
  this->read(f, header_only);
  f.close();
}

void map_reader::read(std::istream &f, bool header_only)
{
  f.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
  std::string line;
  std::getline(f, line);
  boost::algorithm::trim(line);
  int ntitle=std::atoi(line.substr(0, line.find_first_of('!')).c_str());
  while( ntitle-- )
  {
    std::getline(f,line);
    this->title_lines.push_back(line);
  }
  std::getline(f,line);
  std::list<int> values;
  for(short d=0; d<3; ++d)
  {
    int p=d*8*3;
    this->grid_size[d]     = std::atoi(line.substr(p,8).c_str());
    IOTBX_ASSERT( this->grid_size[d]>0 );
    this->grid_first[d] = std::atoi(line.substr(p+8,8).c_str());
    this->grid_last[d]  = std::atoi(line.substr(p+8*2,8).c_str());
  }
  std::getline(f,line);
  for(short p=0; p<6; ++p)
    this->unit_cell_parameters[p] = std::atof(line.substr(p*12,12).c_str());
  std::getline(f,line);
  boost::algorithm::trim(line);
  IOTBX_ASSERT( line=="ZYX" );
  af::flex_grid<> grid(af::adapt(this->grid_first), af::adapt(this->grid_last),
    false);
  if( !header_only )
  {
    data.resize(grid, 0.);
    this->read(f, 0, grid);
  }
}

void map_reader::load(std::string const& file_name, std::size_t n_header_lines,
  af::flex_grid<> const& grid)
{
  std::ifstream cin(file_name.c_str());
  this->read(cin, n_header_lines, grid);
  cin.close();
}

void map_reader::read(std::istream &cin, std::size_t n_header_lines,
  af::flex_grid<> const& grid)
{
  IOTBX_ASSERT(grid.nd() == 3);
  IOTBX_ASSERT(grid.all().all_gt(0));
  for (std::size_t i=0;i<n_header_lines;i++) {
    cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
  }
  std::string line;
  af::ref<double, af::c_grid<3> > data_ref(
    data.begin(),
    af::c_grid<3>(af::adapt(data.accessor().all())));
  for(std::size_t iz=0;iz<data_ref.accessor()[2];iz++) {
    std::getline(cin, line); // reads section number
    std::size_t i_fld = 6;
    for(std::size_t iy=0;iy<data_ref.accessor()[1];iy++) {
      for(std::size_t ix=0;ix<data_ref.accessor()[0];ix++) {
        if (i_fld == 6) {
          std::getline(cin, line);
          i_fld = 0;
        }
        data_ref(ix,iy,iz) = std::atof(line.substr(i_fld*12,12).c_str());
        i_fld++;
      }
    }
  }
  std::getline(cin, line);
  if (line.size() == 0) {
    average = -1;
    standard_deviation = -1;
  }
  else {
    int expected_9999 = std::atoi(line.substr(0,8).c_str());
    IOTBX_ASSERT(expected_9999 == -9999);
    std::getline(cin, line);
    average = std::atof(line.substr(0,12).c_str());
    standard_deviation = std::atof(line.substr(12,12).c_str());
  }
}

}}


/*
 * attenuation_coefficient.cc
 *
 *  Copyright (C) 2013 Diamond Light Source
 *
 *  Author: James Parkhurst
 *
 *  This code is distributed under the BSD license, a copy of which is
 *  included in the root directory of this package.
 */
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <cctbx/eltbx/attenuation_coefficient.h>

namespace cctbx { namespace eltbx { namespace attenuation_coefficient {
    namespace boost_python {

  using namespace boost::python;

  void init_module()
  {
    double (table::*energy_single)(std::size_t) const = &table::energy;
    double (table::*mu_rho_single)(std::size_t) const = &table::mu_rho;
    double (table::*mu_en_rho_single)(std::size_t) const = &table::mu_en_rho;
    flex_double (table::*energy_list)() const = &table::energy;
    flex_double (table::*mu_rho_list)() const = &table::mu_rho;
    flex_double (table::*mu_en_rho_list)() const = &table::mu_en_rho;

    class_<table>("table", no_init)
      .def(init<std::size_t>((arg("z"))))
      .def("size", &table::size)
      .def("density", &table::density)
      .def("min_energy", &table::min_energy)
      .def("max_energy", &table::max_energy)
      .def("energy", energy_single)
      .def("energy", energy_list)
      .def("mu_rho", mu_rho_single)
      .def("mu_rho", mu_rho_list)
      .def("mu_en_rho", mu_en_rho_single)
      .def("mu_en_rho", mu_en_rho_list)
      .def("mu_rho_at_ev", &table::mu_rho_at_ev)
      .def("mu_rho_at_kev", &table::mu_rho_at_kev)
      .def("mu_rho_at_angstrom", &table::mu_rho_at_angstrom)
      .def("mu_en_rho_at_ev", &table::mu_en_rho_at_ev)
      .def("mu_en_rho_at_kev", &table::mu_en_rho_at_kev)
      .def("mu_en_rho_at_angstrom", &table::mu_en_rho_at_angstrom)
      .def("mu_at_ev", &table::mu_at_ev)
      .def("mu_at_kev", &table::mu_at_kev)
      .def("mu_at_angstrom", &table::mu_at_angstrom)
      .def("lambda_at_ev", &table::lambda_at_ev)
      .def("lambda_at_kev", &table::lambda_at_kev)
      .def("lambda_at_angstrom", &table::lambda_at_angstrom);
  }

}}}} // namespace = cctbx::eltbx::attenuation_coefficient::boost_python

BOOST_PYTHON_MODULE(cctbx_eltbx_attenuation_coefficient_ext)
{
  cctbx::eltbx::attenuation_coefficient::boost_python::init_module();
}


//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are 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)

//
// This is not a complete header file, it is included by beta.hpp
// after it has defined it's definitions.  This inverts the incomplete
// beta functions ibeta and ibetac on the first parameters "a"
// and "b" using a generic root finding algorithm (TOMS Algorithm 748).
//

#ifndef BOOST_MATH_SP_DETAIL_BETA_INV_AB
#define BOOST_MATH_SP_DETAIL_BETA_INV_AB

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/tools/toms748_solve.hpp>
#include <boost/cstdint.hpp>

namespace boost{ namespace math{ namespace detail{

template <class T, class Policy>
struct beta_inv_ab_t
{
   beta_inv_ab_t(T b_, T z_, T p_, bool invert_, bool swap_ab_) : b(b_), z(z_), p(p_), invert(invert_), swap_ab(swap_ab_) {}
   T operator()(T a)
   {
      return invert ?
         p - boost::math::ibetac(swap_ab ? b : a, swap_ab ? a : b, z, Policy())
         : boost::math::ibeta(swap_ab ? b : a, swap_ab ? a : b, z, Policy()) - p;
   }
private:
   T b, z, p;
   bool invert, swap_ab;
};

template <class T, class Policy>
T inverse_negative_binomial_cornish_fisher(T n, T sf, T sfc, T p, T q, const Policy& pol)
{
   BOOST_MATH_STD_USING
   // mean:
   T m = n * (sfc) / sf;
   T t = sqrt(n * (sfc));
   // standard deviation:
   T sigma = t / sf;
   // skewness
   T sk = (1 + sfc) / t;
   // kurtosis:
   T k = (6 - sf * (5+sfc)) / (n * (sfc));
   // Get the inverse of a std normal distribution:
   T x = boost::math::erfc_inv(p > q ? 2 * q : 2 * p, pol) * constants::root_two<T>();
   // Set the sign:
   if(p < 0.5)
      x = -x;
   T x2 = x * x;
   // w is correction term due to skewness
   T w = x + sk * (x2 - 1) / 6;
   //
   // Add on correction due to kurtosis.
   //
   if(n >= 10)
      w += k * x * (x2 - 3) / 24 + sk * sk * x * (2 * x2 - 5) / -36;

   w = m + sigma * w;
   if(w < tools::min_value<T>())
      return tools::min_value<T>();
   return w;
}

template <class T, class Policy>
T ibeta_inv_ab_imp(const T& b, const T& z, const T& p, const T& q, bool swap_ab, const Policy& pol)
{
   BOOST_MATH_STD_USING  // for ADL of std lib math functions
   //
   // Special cases first:
   //
   BOOST_MATH_INSTRUMENT_CODE("b = " << b << " z = " << z << " p = " << p << " q = " << " swap = " << swap_ab);
   if(p == 0)
   {
      return swap_ab ? tools::min_value<T>() : tools::max_value<T>();
   }
   if(q == 0)
   {
      return swap_ab ? tools::max_value<T>() : tools::min_value<T>();
   }
   //
   // Function object, this is the functor whose root
   // we have to solve:
   //
   beta_inv_ab_t<T, Policy> f(b, z, (p < q) ? p : q, (p < q) ? false : true, swap_ab);
   //
   // Tolerance: full precision.
   //
   tools::eps_tolerance<T> tol(policies::digits<T, Policy>());
   //
   // Now figure out a starting guess for what a may be,
   // we'll start out with a value that'll put p or q
   // right bang in the middle of their range, the functions
   // are quite sensitive so we should need too many steps
   // to bracket the root from there:
   //
   T guess = 0;
   T factor = 5;
   //
   // Convert variables to parameters of a negative binomial distribution:
   //
   T n = b;
   T sf = swap_ab ? z : 1-z;
   T sfc = swap_ab ? 1-z : z;
   T u = swap_ab ? p : q;
   T v = swap_ab ? q : p;
   if(u <= pow(sf, n))
   {
      //
      // Result is less than 1, negative binomial approximation
      // is useless....
      //
      if((p < q) != swap_ab)
      {
         guess = (std::min)(T(b * 2), T(1));
      }
      else
      {
         guess = (std::min)(T(b / 2), T(1));
      }
   }
   if(n * n * n * u * sf > 0.005)
      guess = 1 + inverse_negative_binomial_cornish_fisher(n, sf, sfc, u, v, pol);

   if(guess < 10)
   {
      //
      // Negative binomial approximation not accurate in this area:
      //
      if((p < q) != swap_ab)
      {
         guess = (std::min)(T(b * 2), T(10));
      }
      else
      {
         guess = (std::min)(T(b / 2), T(10));
      }
   }
   else
      factor = (v < sqrt(tools::epsilon<T>())) ? 2 : (guess < 20 ? 1.2f : 1.1f);
   BOOST_MATH_INSTRUMENT_CODE("guess = " << guess);
   //
   // Max iterations permitted:
   //
   boost::uintmax_t max_iter = policies::get_max_root_iterations<Policy>();
   std::pair<T, T> r = bracket_and_solve_root(f, guess, factor, swap_ab ? true : false, tol, max_iter, pol);
   if(max_iter >= policies::get_max_root_iterations<Policy>())
      return policies::raise_evaluation_error<T>("boost::math::ibeta_invab_imp<%1%>(%1%,%1%,%1%)", "Unable to locate the root within a reasonable number of iterations, closest approximation so far was %1%", r.first, pol);
   return (r.first + r.second) / 2;
}

} // namespace detail

template <class RT1, class RT2, class RT3, class Policy>
typename tools::promote_args<RT1, RT2, RT3>::type
      ibeta_inva(RT1 b, RT2 x, RT3 p, const Policy& pol)
{
   typedef typename tools::promote_args<RT1, RT2, RT3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   static const char* function = "boost::math::ibeta_inva<%1%>(%1%,%1%,%1%)";
   if(p == 0)
   {
      return policies::raise_overflow_error<result_type>(function, 0, Policy());
   }
   if(p == 1)
   {
      return tools::min_value<result_type>();
   }

   return policies::checked_narrowing_cast<result_type, forwarding_policy>(
      detail::ibeta_inv_ab_imp(
         static_cast<value_type>(b),
         static_cast<value_type>(x),
         static_cast<value_type>(p),
         static_cast<value_type>(1 - static_cast<value_type>(p)),
         false, pol),
      function);
}

template <class RT1, class RT2, class RT3, class Policy>
typename tools::promote_args<RT1, RT2, RT3>::type
      ibetac_inva(RT1 b, RT2 x, RT3 q, const Policy& pol)
{
   typedef typename tools::promote_args<RT1, RT2, RT3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   static const char* function = "boost::math::ibetac_inva<%1%>(%1%,%1%,%1%)";
   if(q == 1)
   {
      return policies::raise_overflow_error<result_type>(function, 0, Policy());
   }
   if(q == 0)
   {
      return tools::min_value<result_type>();
   }

   return policies::checked_narrowing_cast<result_type, forwarding_policy>(
      detail::ibeta_inv_ab_imp(
         static_cast<value_type>(b),
         static_cast<value_type>(x),
         static_cast<value_type>(1 - static_cast<value_type>(q)),
         static_cast<value_type>(q),
         false, pol),
      function);
}

template <class RT1, class RT2, class RT3, class Policy>
typename tools::promote_args<RT1, RT2, RT3>::type
      ibeta_invb(RT1 a, RT2 x, RT3 p, const Policy& pol)
{
   typedef typename tools::promote_args<RT1, RT2, RT3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   static const char* function = "boost::math::ibeta_invb<%1%>(%1%,%1%,%1%)";
   if(p == 0)
   {
      return tools::min_value<result_type>();
   }
   if(p == 1)
   {
      return policies::raise_overflow_error<result_type>(function, 0, Policy());
   }

   return policies::checked_narrowing_cast<result_type, forwarding_policy>(
      detail::ibeta_inv_ab_imp(
         static_cast<value_type>(a),
         static_cast<value_type>(x),
         static_cast<value_type>(p),
         static_cast<value_type>(1 - static_cast<value_type>(p)),
         true, pol),
      function);
}

template <class RT1, class RT2, class RT3, class Policy>
typename tools::promote_args<RT1, RT2, RT3>::type
      ibetac_invb(RT1 a, RT2 x, RT3 q, const Policy& pol)
{
   static const char* function = "boost::math::ibeta_invb<%1%>(%1%, %1%, %1%)";
   typedef typename tools::promote_args<RT1, RT2, RT3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   if(q == 1)
   {
      return tools::min_value<result_type>();
   }
   if(q == 0)
   {
      return policies::raise_overflow_error<result_type>(function, 0, Policy());
   }

   return policies::checked_narrowing_cast<result_type, forwarding_policy>(
      detail::ibeta_inv_ab_imp(
         static_cast<value_type>(a),
         static_cast<value_type>(x),
         static_cast<value_type>(1 - static_cast<value_type>(q)),
         static_cast<value_type>(q),
         true, pol),
         function);
}

template <class RT1, class RT2, class RT3>
inline typename tools::promote_args<RT1, RT2, RT3>::type
         ibeta_inva(RT1 b, RT2 x, RT3 p)
{
   return boost::math::ibeta_inva(b, x, p, policies::policy<>());
}

template <class RT1, class RT2, class RT3>
inline typename tools::promote_args<RT1, RT2, RT3>::type
         ibetac_inva(RT1 b, RT2 x, RT3 q)
{
   return boost::math::ibetac_inva(b, x, q, policies::policy<>());
}

template <class RT1, class RT2, class RT3>
inline typename tools::promote_args<RT1, RT2, RT3>::type
         ibeta_invb(RT1 a, RT2 x, RT3 p)
{
   return boost::math::ibeta_invb(a, x, p, policies::policy<>());
}

template <class RT1, class RT2, class RT3>
inline typename tools::promote_args<RT1, RT2, RT3>::type
         ibetac_invb(RT1 a, RT2 x, RT3 q)
{
   return boost::math::ibetac_invb(a, x, q, policies::policy<>());
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_SP_DETAIL_BETA_INV_AB


/// \file
/// Maintainer: Felice Serena
///
///

#include "hog_labeling.h"

#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/objdetect/objdetect.hpp>

#include <boost/log/trivial.hpp>

#include <classifier/knn.h>

namespace MouseTrack {

/// Returns a n x 2 matrix of (x,y) pairs of valid corner coordinates for
/// sliding windows of the desired size such that they are within the given
/// image dimensions.
///
std::vector<cv::Point> slidingWindows(int imgWidth, int imgHeight, int stepSize,
                                      int windowWidth, int windowHeight) {
  std::vector<cv::Point> result;
  for (int x = 0; x < imgWidth - windowWidth; x += stepSize) {
    for (int y = 0; y < imgHeight - windowHeight; y += stepSize) {
      result.push_back(cv::Point(x, y));
    }
  }
  return result;
} // namespace MouseTrack

int HogLabeling::slidingWindowWidth() const {
    return _windowWidth;
}

void HogLabeling::slidingWindowWidth(int _new) {
    _windowWidth = _new;
}

int HogLabeling::slidingWindowHeight() const{
    return _windowHeight;
}

void HogLabeling::slidingWindowHeight(int _new){
    _windowHeight = _new;
}

/// Get the number of pixels between two neighboring sliding windows (used along both axis)
int HogLabeling::slidingWindowStride() const {
    return _stepSize;
}

/// Set the number of pixels between two neighboring sliding windows (used along both axis)
void HogLabeling::slidingWindowStride(int _new) {
    _stepSize = _new;
}

std::unique_ptr<Classifier>& HogLabeling::classifier(){
  return _classifier;
}

void HogLabeling::train(const Mat &X_train, const Vec &y_train) {
  if(_classifier.get() == nullptr){
    auto ptr = std::make_unique<KnnClassifier>();
    ptr->k(11);
    _classifier = std::move(ptr);
  }
  _classifier->fit(X_train, y_train);
  _numLabels = y_train.maxCoeff() + 1;
}

FrameWindow HogLabeling::operator()(const FrameWindow &window) const {
  if (window.frames().empty()) {
    return window;
  }
  if (_classifier.get() == nullptr) {
    BOOST_LOG_TRIVIAL(warning)
        << "HOG classifier not trained, no labeling performed.";
    return window;
  }
  FrameWindow result = window;
  // create matrices for labels
  for (Frame &f : result.frames()) {
    f.labels.resize(_numLabels);
    for (auto &l : f.labels) {
      l.setZero(f.referencePicture.rows(), f.referencePicture.cols());
    }
  }

  // hog settings
  cv::Size windowSize(_windowWidth, _windowHeight);
  cv::Size blockSize(_blockWidth, _blockHeight);
  cv::Size blockStride(_blockStrideWidth, _blockStrideHeight);
  cv::Size cellSize(_cellWidth, _cellHeight);

  // create hog descriptor: it is able to tansform images patches to feature
  // vectors
  cv::HOGDescriptor hog(windowSize, blockSize, blockStride, cellSize, _nbins);

  // compute settings
  cv::Size windowStride;
  cv::Size padding;

  // build locations for sliding window
  const auto &first = result.frames()[0];
  std::vector<cv::Point> locations = slidingWindows(
      first.referencePicture.cols(), first.referencePicture.rows(), _stepSize,
      _windowWidth, _windowHeight);

  BOOST_LOG_TRIVIAL(trace) << "Created " << locations.size()
                           << " sliding window locations to check.";

  for (size_t f = 0; f < result.frames().size(); ++f) {
    BOOST_LOG_TRIVIAL(trace) << "Checking frame " << f;
    Frame &frame = result.frames()[f];
    cv::Mat img;
    PictureI eig = (frame.referencePicture * 255.0).cast<PictureI::Scalar>();
    cv::eigen2cv(eig, img);

    // holds `locations.size()` descriptors of size hog.getDescriptorSize()
    std::vector<float> descriptors;
    hog.compute(img, descriptors, windowStride, padding, locations);
    BOOST_LOG_TRIVIAL(trace)
        << "Found " << (descriptors.size() / hog.getDescriptorSize())
        << " HOG descriptors of size " << hog.getDescriptorSize() << " at "
        << locations.size() << " locations in frame " << f;
    Eigen::Map<Eigen::MatrixXf> map(descriptors.data(), hog.getDescriptorSize(),
                                    locations.size());
    Classifier::Mat tmp = map.cast<double>();
    BOOST_LOG_TRIVIAL(trace) << "Classifying...";
    auto labels = _classifier->predictProbabilities(tmp);
    BOOST_LOG_TRIVIAL(trace) << "Assigning...";
    // apply labels of windows to frame.labels
    for (size_t w = 0; w < locations.size(); ++w) {
      // iterate over pixels within window w
      for (int x = locations[w].x; x < locations[w].x + _windowWidth; ++x) {
        for (int y = locations[w].y; y < locations[w].y + _windowHeight; ++y) {
          // distribute labels
          for (int l = 0; l < _numLabels; ++l) {
            frame.labels[l](y, x) += labels(l, w);
          }
        }
      }
    }
    // normalize label weights
    if (_normalizeRange) {
      for (auto &l : frame.labels) {
        auto min = l.minCoeff();
        l = l.array() - min;
        auto max = l.maxCoeff();
        l = l.array() / max;
      }
    }

    // normalize accross labels
    if (_normalizeAccross) {
      PictureD sum;
      sum.setZero(frame.referencePicture.rows(), frame.referencePicture.cols());
      for (int l = 0; l < _numLabels; ++l) {
        sum += frame.labels[l];
      }
      // avoid division by zero
      sum = sum.array() + 0.0000001;
      sum = sum.array().inverse();
      for (int l = 0; l < _numLabels; ++l) {
        frame.labels[l] = frame.labels[l].array() * sum.array();
      }
    }
    BOOST_LOG_TRIVIAL(trace) << "Frame " << f << " finished.";
  }
  return result;
}

} // namespace MouseTrack


//
//  TestHelpers.hpp
//  KinFu
//
//  Created by Dave on 16/05/2016.
//  Copyright © 2016 Sindesso. All rights reserved.
//

#ifndef TestHelpers_h
#define TestHelpers_h

#include "TSDFVolumeCPU.hpp"
#include "Camera.hpp"
#include "PngWrapper.hpp"

#include <Eigen/Dense>

#pragma mark - helpers

void create_sphere_in_TSDF( TSDFVolumeCPU & volume, float radius );
void create_wall_in_TSDF( TSDFVolumeCPU & volume, float depth );

uint16_t * make_sphere_depth_map( uint16_t width, uint16_t height, uint16_t radius, uint16_t min_depth, uint16_t max_depth ) ;
uint16_t * make_wall_depth_map( uint16_t width, uint16_t height, uint16_t max_depth, uint16_t min_depth, uint16_t wall_depth );

void save_depth_map( std::string file_name, uint16_t width, uint16_t height, uint16_t * pixels);

Eigen::Matrix4f make_y_axis_rotation( float theta, Eigen::Vector3f pos );
Eigen::Matrix4f make_x_axis_rotation( float theta, Eigen::Vector3f pos );
Eigen::Matrix4f make_z_axis_rotation( float theta, Eigen::Vector3f pos );

void helper_move_look( float vars[7], Eigen::Vector3f & move_to, Eigen::Vector3f & look_at );

#endif /* TestHelpers_h */


#include "../sdp_read.hxx"
#include "../sdp_convert/write_vector.hxx"

#include <boost/filesystem.hpp>

void write_functions(
  const boost::filesystem::path &output_path,
  const std::vector<El::BigFloat> &objectives,
  const std::vector<El::BigFloat> &normalization,
  const std::vector<Positive_Matrix_With_Prefactor> &matrices)
{
  boost::filesystem::ofstream output_stream(output_path);
  set_stream_precision(output_stream);
  output_stream << "{\n  \"objective\":\n";
  write_vector(output_stream, objectives, "    ");
  output_stream << ",\n  \"normalization\":\n";
  write_vector(output_stream, normalization, "  ");
  output_stream << ",\n  \"functions\":\n  [\n";

  const Boost_Float pi(boost::math::constants::pi<Boost_Float>());
  const El::BigFloat zero(0);

  size_t block_number(0);
  for(auto block(matrices.begin()); block != matrices.end(); ++block)
    {
      if(block != matrices.begin())
        {
          output_stream << ",\n";
        }
      output_stream << "    [\n";

      const size_t num_chebyshev_points([&block]() {
        size_t max(0);
        for(auto &row : block->polynomials)
          for(auto &column : row)
            for(auto &poly : column)
              {
                max = std::max(max, poly.coefficients.size());
              }
        return max;
      }());

      const El::BigFloat max_delta(6 * num_chebyshev_points);
      std::vector<El::BigFloat> chebyshev_zeros(num_chebyshev_points, 0);
      for(size_t index(0); index != chebyshev_zeros.size(); ++index)
        {
          chebyshev_zeros[index]
            = 0.5 * max_delta
              * El::BigFloat(
                to_string(1
                          + cos((pi * (num_chebyshev_points - index - 0.5))
                                / num_chebyshev_points)));
        }

      const size_t num_rows(block->polynomials.size()),
        num_columns(block->polynomials.front().size());
      std::vector<int64_t> max_degree(num_rows * num_columns, 0),
        min_degree(max_degree.size(), std::numeric_limits<int64_t>::max());
      for(size_t row(0); row != num_rows; ++row)
        {
          for(size_t column(0); column != num_columns; ++column)
            {
              for(auto &poly : block->polynomials[row][column])
                {
                  // Sometimes, the highest degree coefficients are zero
                  for(size_t index(0); index != poly.coefficients.size();
                      ++index)
                    {
                      const int64_t degree(poly.coefficients.size() - 1
                                           - index);
                      if(poly.coefficients.at(degree) != zero)
                        {
                          max_degree.at(row + column * num_rows) = std::max(
                            max_degree.at(row + column * num_rows), degree);
                          min_degree.at(row + column * num_rows) = std::min(
                            min_degree.at(row + column * num_rows), degree);
                        }
                    }
                }
            }
        }
      // Fix max_degree so that we have the correct limiting
      // determinant
      switch(num_rows)
        {
        case 1: break;
          case 2: {
            const int64_t first(max_degree.at(0) + max_degree.at(3)),
              second(2 * max_degree.at(1));
            if(first > second)
              {
                max_degree.at(1) = 0;
                max_degree.at(2) = 0;
              }
            else if(first < second)
              {
                max_degree.at(0) = 0;
                max_degree.at(3) = 0;
              }
          }
          break;
        default:
          throw std::runtime_error(
            "Too large a dimension.  Only 1x1 and 2x2 supported: "
            + std::to_string(num_rows));
        }

      for(size_t row(0); row != num_rows; ++row)
        {
          if(row != 0)
            {
              output_stream << ",\n";
            }
          output_stream << "      [\n";
          for(size_t column(0); column != num_columns; ++column)
            {
              if(column != 0)
                {
                  output_stream << ",\n";
                }
              output_stream << "        [\n";
              size_t poly_number(0);
              for(auto poly(block->polynomials[row][column].begin());
                  poly != block->polynomials[row][column].end(); ++poly)
                {
                  if(poly != block->polynomials[row][column].begin())
                    {
                      output_stream << ",\n";
                    }
                  output_stream << "          {\n"
                                << "            \"max_delta\": \"" << max_delta
                                << "\",\n"
                                << "            \"infinity_value\": \"";
                  if(poly->degree() < max_degree.at(row + column * num_rows))
                    {
                      output_stream << "0";
                    }
                  else
                    {
                      output_stream << poly->coefficients.at(
                        max_degree.at(row + column * num_rows));
                    }
                  output_stream << "\",\n"
                                << "            \"epsilon_value\": \"";
                  if(poly->degree() < min_degree.at(row + column * num_rows))
                    {
                      output_stream << "0";
                    }
                  else
                    {
                      output_stream << poly->coefficients.at(
                        min_degree.at(row + column * num_rows));
                    }
                  output_stream << "\",\n"
                                << "            \"chebyshev_values\":\n"
                                << "            [\n";

                  for(auto point(chebyshev_zeros.begin());
                      point != chebyshev_zeros.end(); ++point)
                    {
                      if(point != chebyshev_zeros.begin())
                        {
                          output_stream << ",\n";
                        }
                      output_stream << "              \"" << (*poly)(*point)
                                    << "\"";
                    }
                  output_stream << "\n            ]"
                                << "\n          }";
                  ++poly_number;
                }
              output_stream << "\n        ]";
            }
          output_stream << "\n      ]";
        }
      output_stream << "\n    ]";
      ++block_number;
    }
  output_stream << "\n  ]\n}\n";
}


/**
 * @Author: Julien Vial-Detambel <l3ninj>
 * @Date:   2018-06-12T11:23:27+01:00
 * @Email: julien.vial-detambel@epitech.eu
 * @Project: CUDA-Based Simulator of Quantum Systems
 * @Filename: Simulator.cpp
 * @Last modified by:   l3ninj
 * @Last modified time: 2018-08-23T12:13:08+02:00
 * @License: MIT License
 */

#include <iostream>
#include <cmath>
#include <random>

#include <boost/foreach.hpp>

#include "MatrixStore.hpp"
#include "Worker/Simulator.hpp"
#include "Logger.hpp"

using namespace TaskGraph;
using namespace MeasurementResultsTree;

Simulator::Simulator(SimulateCircuitTask &task, IMeasurementResultsTree &measurementsTree, Matrix const &state)
: m_task(task), m_measurementsTree(measurementsTree), m_state(state) {
  // Computing the offsets for each qregisters,
  // and total qubits number of the system.
  m_size = 0;
  for (auto &reg: m_task.circuit.qreg) {
    m_qRegOffsets.insert(make_pair(reg.name, m_size));
    m_size += reg.size;
  }
}

Simulator::StepVisitor::StepVisitor(Simulator& simulator) :
m_simulator(simulator) {}

void Simulator::StepVisitor::operator()(const Circuit::UGate& value) {
  // Computing the offset of the target qubit
  Circuit::Qubit target = value.target;
  int id = m_simulator.m_qRegOffsets.find(target.registerName)->second;
  id += target.element;

  // Creating a gate according to the phi, theta and lambda parameters and
  // setting it as the target qubit transformation gate.
  using namespace std::complex_literals;
  m_simulator.m_gates[id] = Matrix(std::shared_ptr<Tvcplxd>(new Tvcplxd({exp(-1i * (value.phi + value.lambda) / 2.0)
    * cos(value.theta / 2.0),
    -exp(-1i * (value.phi - value.lambda) / 2.0) * sin(value.theta / 2.0),
    exp(1i * (value.phi - value.lambda) / 2.0) * sin(value.theta / 2.0),
    exp(1i * (value.phi + value.lambda) / 2.0) * cos(value.theta / 2.0)
  })), 2, 2);

  // Debug logs
  LOG(Logger::DEBUG, "Applying a U Gate:" << "\nU Gate:\n\ttheta: "
    << value.theta << ", phi: " << value.phi << ", lambda: " << value.lambda
    << "\n\ttarget: " << value.target.registerName << "["
    << value.target.element << "]\n" << std::endl);
}

void Simulator::StepVisitor::operator()(const Circuit::CXGate& value) {
  // Apparently this method requires us to normalize the state afterward
  m_simulator.m_shouldNormalize = true;
  m_simulator.m_shouldAddSecondMatrix = true;

  // Computing the offset of the control qubit.
  Circuit::Qubit control = value.control;
  int controlId = m_simulator.m_qRegOffsets.find(control.registerName)->second;
  controlId += control.element;

  // Computing the offset of the target qubit.
  Circuit::Qubit target = value.target;
  int targetId = m_simulator.m_qRegOffsets.find(target.registerName)->second;
  targetId += target.element;

  m_simulator.m_gates[controlId] = MatrixStore::pk0;
  m_simulator.m_extraGates[controlId] = MatrixStore::pk1;
  m_simulator.m_extraGates[targetId] = MatrixStore::x;

  LOG(Logger::DEBUG, "Applying a CX Gate:" << "\nCX Gate:\n\tcontrol: "
    << value.control.registerName << "[" << value.control.element
    << "]\n\ttarget: " << value.target.registerName << "["
    << value.target.element << "]\n" << std::endl);
}

void Simulator::StepVisitor::operator()(const Circuit::Measurement& value) {
  (void)(value);
  LOG(Logger::ERROR, "Measurement should not be present in the circuit anymore");
  assert(true == false);
}

void Simulator::StepVisitor::operator()(const Circuit::Barrier& __attribute__((unused)) value) {
  // Nothing to be done for a barrier. It is the same as an identity, computationaly-wise
  (void)value;
}

void Simulator::StepVisitor::operator()(const Circuit::Reset& value) {
  m_simulator.m_shouldNormalize = true;
  // Computing the offset of the target qubit
  Circuit::Qubit target = value.target;
  int id = m_simulator.m_qRegOffsets.find(target.registerName)->second;
  id += target.element;

  m_simulator.m_gates[id] = MatrixStore::pk0;
}
void Simulator::StepVisitor::operator()(const Circuit::ConditionalGate& cGate) {
  const auto value = m_simulator.m_measurementsTree.getCregValueAtNode(cGate.testedRegister, m_simulator.m_task.measurementNodeId);
  if (value == cGate.expectedValue) { cGate.gate.apply_visitor(*this); }
}

Matrix Simulator::simulate() {
  auto visitor = StepVisitor(*this);
  // Looping through each steps of the circuit.
  for (std::vector<Circuit::Step>::iterator it = m_task.circuit.steps.begin();
    it != m_task.circuit.steps.end(); ++it) {
      // LOG(Logger::DEBUG, "State before step:" << m_state);
      m_shouldNormalize = false;
      m_shouldAddSecondMatrix = false;
    // Initializing the gate vectors
    m_gates = std::vector<Matrix>(m_size, MatrixStore::i2);
    m_extraGates = std::vector<Matrix>(m_size, MatrixStore::i2);
    for (auto &substep: *it) {
      // Applying defined tranformations in the visitor.
      boost::apply_visitor(visitor, substep);
    }
    // Computing the new state as the dot product between the kroenecker product
    // of the transformation gates for each qubits and the actual simulator state.
    Matrix op = Matrix::kron(m_gates);
    if (m_shouldAddSecondMatrix) op = op + Matrix::kron(m_extraGates);
    // for (auto const &g : m_extraGates) {
    //   LOG(Logger::ERROR, g);
    // }
    // LOG(Logger::DEBUG, "Applying matrix:" << op);
    // LOG(Logger::DEBUG, "Submatrices:" << Matrix::kron(m_gates) << Matrix::kron(m_extraGates));
    m_state = op * m_state;

    if (m_shouldNormalize) m_state = m_state.normalize();
      // LOG(Logger::DEBUG, "State after step:" << m_state);
  }
  return m_state;
}