/*
 * Copyright (c) 2009 Carnegie Mellon University.
 *     All rights reserved.
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing,
 *  software distributed under the License is distributed on an "AS
 *  IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
 *  express or implied.  See the License for the specific language
 *  governing permissions and limitations under the License.
 *
 * For more about this software visit:
 *
 *      http://www.graphlab.ml.cmu.edu
 *
 */

#include <boost/unordered_set.hpp>
#include <graphlab.hpp>
#include <graphlab/ui/metrics_server.hpp>
#include <graphlab/macros_def.hpp>
/**
 *
 * In this program we implement the "hash-table" version of the
 * "edge-iterator" algorithm described in
 *
 *    T. Schank. Algorithmic Aspects of Triangle-Based Network Analysis.
 *    Phd in computer science, University Karlsruhe, 2007.
 *
 * The procedure is quite straightforward:
 *   - each vertex maintains a list of all of its neighbors in a hash table.
 *   - For each edge (u,v) in the graph, count the number of intersections
 *     of the neighbor set on u and the neighbor set on v.
 *   - We store the size of the intersection on the edge.
 *
 * This will count every triangle exactly 3 times. Summing across all the
 * edges and dividing by 3 gives the desired result.
 *
 * The preprocessing stage take O(|E|) time, and it has been shown that this
 * algorithm takes $O(|E|^(3/2))$ time.
 *
 * If we only require total counts, we can introduce a optimization that is
 * similar to the "forward" algorithm
 * described in thesis above. Instead of maintaining a complete list of all
 * neighbors, each vertex only maintains a list of all neighbors with
 * ID greater than itself. This implicitly generates a topological sort
 * of the graph.
 *
 * Then you can see that each triangle
 *
 * \verbatim

     A----->C
     |     ^
     |   /
     v /
     B

 * \endverbatim
 * Must be counted only once. (Only when processing edge AB, can one
 * observe that A and B have intersecting out-neighbor sets).
 *
 *
 * \note The implementation here is built to be easy to understand
 * and not necessarily optimal. In particular the unordered_set is slow
 * for small number of entries. There is a much more efficient
 * (and substantially more complicated) version in undirected_triangle_count.cpp
 */

/*
 * Each vertex maintains a list of all its neighbors.
 * and a final count for the number of triangles it is involved in
 */
struct vertex_data_type {
  vertex_data_type():num_triangles(0) { }
  // A list of all its neighbors
  boost::unordered_set<graphlab::vertex_id_type> vid_set;
  // The number of triangles this vertex is involved it.
  // only used if "per vertex counting" is used
  size_t num_triangles;

  void save(graphlab::oarchive &oarc) const {
    oarc << vid_set << num_triangles;
  }
  void load(graphlab::iarchive &iarc) {
    iarc >> vid_set >> num_triangles;
  }
};

/*
 * Each edge is simply a counter of triangles
 */
typedef size_t edge_data_type;

// To collect the set of neighbors, we need a message type which is
// basically a set of vertex IDs

bool PER_VERTEX_COUNT = false;

/*
 * This is the gathering type which accumulates an (unordered) set of
 * all neighboring vertices.
 * It is a simple wrapper around a boost::unordered_set with
 * an operator+= which simply performs a set union.
 *
 * This struct can be significantly accelerated for small sets.
 * Small collections of vertex IDs should not require the overhead
 * of the unordered_set.
 */
struct set_union_gather {
  boost::unordered_set<graphlab::vertex_id_type> vid_set;

  /*
   * Combining with another collection of vertices.
   * Union it into the current set.
   */
  set_union_gather& operator+=(const set_union_gather& other) {
    foreach(graphlab::vertex_id_type othervid, other.vid_set) {
      vid_set.insert(othervid);
    }
    return *this;
  }

  // serialize
  void save(graphlab::oarchive& oarc) const {
    oarc << vid_set;
  }

  // deserialize
  void load(graphlab::iarchive& iarc) {
    iarc >> vid_set;
  }
};

/*
 * Define the type of the graph
 */
typedef graphlab::distributed_graph<vertex_data_type,
                                    edge_data_type> graph_type;

/*
 * This class implements the triangle counting algorithm as described in
 * the header. On gather, we accumulate a set of all adjacent vertices.
 * If per_vertex output is not necessary, we can use the optimization
 * where each vertex only accumulates neighbors with greater vertex IDs.
 */
class triangle_count :
      public graphlab::ivertex_program<graph_type,
                                      set_union_gather>,
      /* I have no data. Just force it to POD */
      public graphlab::IS_POD_TYPE  {
public:
  // Gather on all edges
  edge_dir_type gather_edges(icontext_type& context,
                             const vertex_type& vertex) const {
    return graphlab::ALL_EDGES;
  }

  /*
   * For each edge, figure out the ID of the "other" vertex
   * and accumulate a set of the neighborhood vertex IDs.
   */
  gather_type gather(icontext_type& context,
                     const vertex_type& vertex,
                     edge_type& edge) const {
    set_union_gather gather;
    // Insert the opposite end of the edge IF the opposite end has
    // ID greater than the current vertex
    // If we are getting per vertex counts, we need the entire neighborhood
    vertex_id_type otherid = edge.source().id() == vertex.id() ?
                             edge.target().id() : edge.source().id();
    if (PER_VERTEX_COUNT ||
        otherid > vertex.id()) gather.vid_set.insert(otherid);
    return gather;
  }

  /*
   * the gather result now contains the vertex IDs in the neighborhood.
   * store it on the vertex.
   */
  void apply(icontext_type& context, vertex_type& vertex,
             const gather_type& neighborhood) {
    vertex.data().vid_set = neighborhood.vid_set;
  } // end of apply

  /*
   * Scatter over all edges to compute the intersection.
   * I only need to touch each edge once, so if I scatter just on the
   * out edges, that is sufficient.
   */
  edge_dir_type scatter_edges(icontext_type& context,
                              const vertex_type& vertex) const {
    return graphlab::OUT_EDGES;
  }

  /*
   * Computes the size of the intersection of two unordered sets
   */
  static size_t count_set_intersect(
               const boost::unordered_set<vertex_id_type>& smaller_set,
               const boost::unordered_set<vertex_id_type>& larger_set) {
    size_t count = 0;
    foreach(vertex_id_type vid, smaller_set) {
      count += larger_set.count(vid);
    }
    return count;
  }

  /*
   * For each edge, count the intersection of the neighborhood of the
   * adjacent vertices. This is the number of triangles this edge is involved
   * in.
   */
  void scatter(icontext_type& context,
              const vertex_type& vertex,
              edge_type& edge) const {
    const vertex_data_type& srclist = edge.source().data();
    const vertex_data_type& targetlist = edge.target().data();
    if (srclist.vid_set.size() >= targetlist.vid_set.size()) {
      edge.data() = count_set_intersect(targetlist.vid_set, srclist.vid_set);
    }
    else {
      edge.data() = count_set_intersect(srclist.vid_set, targetlist.vid_set);
    }
  }
};

/*
 * This class is used in a second engine call if per vertex counts are needed.
 * The number of triangles a vertex is involved in can be computed easily
 * by summing over the number of triangles each adjacent edge is involved in
 * and dividing by 2.
 */
class get_per_vertex_count :
      public graphlab::ivertex_program<graph_type, size_t>,
      /* I have no data. Just force it to POD */
      public graphlab::IS_POD_TYPE  {
public:
  // Gather on all edges
  edge_dir_type gather_edges(icontext_type& context,
                             const vertex_type& vertex) const {
    return graphlab::ALL_EDGES;
  }
  // We gather the number of triangles each edge is involved in
  size_t gather(icontext_type& context,
                     const vertex_type& vertex,
                     edge_type& edge) const {
    return edge.data();
  }

  /* the gather result is the total sum of the number of triangles
   * each adjacent edge is involved in . Dividing by 2 gives the
   * desired result.
   */
  void apply(icontext_type& context, vertex_type& vertex,
             const gather_type& num_triangles) {
    vertex.data().num_triangles = num_triangles / 2;
  }

  // No scatter
  edge_dir_type scatter_edges(icontext_type& context,
                             const vertex_type& vertex) const {
    return graphlab::NO_EDGES;
  }

};

/* Used to sum over all the edges in the graph in a
 * map_reduce_edges call
 * to get the total number of triangles
 */
size_t get_edge_data(const graph_type::edge_type& e) {
  return e.data();
}

/*
 * A saver which saves a file where each line is a vid / # triangles pair
 */
struct save_triangle_count{
  std::string save_vertex(graph_type::vertex_type v) {
    return graphlab::tostr(v.id()) + "\t" +
           graphlab::tostr(v.data().num_triangles) + "\n";
  }
  std::string save_edge(graph_type::edge_type e) {
    return "";
  }
};

int main(int argc, char** argv) {
  std::cout << "This program counts the exact number of triangles in the "
            "provided graph.\n\n";

  graphlab::command_line_options clopts("Exact Triangle Counting. "
    "Given a graph, this program computes the total number of triangles "
    "in the graph. An option (per_vertex) is also provided which "
    "computes for each vertex, the number of triangles it is involved in."
    "The algorithm assumes that each undirected edge appears exactly once "
    "in the graph input. If edges may appear more than once, this procedure "
    "will over count.");
  std::string prefix, format;
  std::string per_vertex;
  clopts.attach_option("graph", prefix,
                       "Graph input. reads all graphs matching prefix*");
  clopts.attach_option("format", format,
                       "The graph format");
  clopts.attach_option("per_vertex", per_vertex,
                       "If not empty, will count the number of "
                       "triangles each vertex belongs to and "
                       "save to file with prefix \"[per_vertex]\". "
                       "The algorithm used is slightly different "
                       "and thus will be a little slower");

  if(!clopts.parse(argc, argv)) return EXIT_FAILURE;
  if (prefix == "") {
    std::cout << "--graph is not optional\n";
    clopts.print_description();
    return EXIT_FAILURE;
  }
  else if (format == "") {
    std::cout << "--format is not optional\n";
    clopts.print_description();
    return EXIT_FAILURE;
  }

  if (per_vertex != "") PER_VERTEX_COUNT = true;
  // Initialize control plane using mpi
  graphlab::mpi_tools::init(argc, argv);
  graphlab::distributed_control dc;

  graphlab::launch_metric_server();
  // load graph
  graph_type graph(dc, clopts);
  graph.load_format(prefix, format);
  graph.finalize();
  dc.cout() << "Number of vertices: " << graph.num_vertices() << std::endl
            << "Number of edges:    " << graph.num_edges() << std::endl;

  graphlab::timer ti;

  // create engine to count the number of triangles
  dc.cout() << "Counting Triangles..." << std::endl;
  graphlab::synchronous_engine<triangle_count> engine(dc, graph, clopts);
  engine.signal_all();
  engine.start();

  dc.cout() << "Counted in " << ti.current_time() << " seconds" << std::endl;

  if (PER_VERTEX_COUNT == false) {
    size_t count = graph.map_reduce_edges<size_t>(get_edge_data);
    dc.cout() << count << " Triangles"  << std::endl;
  }
  else {
    graphlab::synchronous_engine<get_per_vertex_count> engine(dc, graph, clopts);
    engine.signal_all();
    engine.start();
    graph.save(per_vertex,
            save_triangle_count(),
            false, /* no compression */
            true, /* save vertex */
            false, /* do not save edge */
            1); /* one file per machine */

  }

  graphlab::stop_metric_server();

  graphlab::mpi_tools::finalize();
  return EXIT_SUCCESS;
} // End of main


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

/**
 * \file measurement.cpp
 * \brief Contains tests for the measurement member functions
 *
 */

#include <gtest/gtest.h>
#include <qsl/qubits.hpp>
#include <qsl/concepts.hpp>
#include <armadillo>
#include <complex>
#include <list>
#include "test-utils.hpp"

template<typename T>
struct SimWrapper
{
    using Sim = T;
};

#define UNCERTAINTY 0.1

/**
 * \brief Typed test suite for one-qubit gates
 */
template <typename T>
class Measurements : public testing::Test {
public:
    using List = std::list<T>;
    static T shared_;
    T value_;
};

template <typename T>
class NPMeasurements : public testing::Test {
public:
    using List = std::list<T>;
    static T shared_;
    T value_;
};

/// List of simulator types to check
using Sim1 = SimWrapper<qsl::Qubits<qsl::Type::Default, float>>;
using Sim2 = SimWrapper<qsl::Qubits<qsl::Type::Default, double>>;
using Sim3 = SimWrapper<qsl::Qubits<qsl::Type::Omp, float>>;
using Sim4 = SimWrapper<qsl::Qubits<qsl::Type::Omp, double>>;
using Sim5 = SimWrapper<qsl::Qubits<qsl::Type::Resize, float>>;
using Sim6 = SimWrapper<qsl::Qubits<qsl::Type::Resize, double>>;
using SimTypes = ::testing::Types<Sim1, Sim2, Sim3, Sim4, Sim5, Sim6>;

using Sim7 = SimWrapper<qsl::Qubits<qsl::Type::NP, float>>;
using Sim8 = SimWrapper<qsl::Qubits<qsl::Type::NP, double>>;
using Sim9 = SimWrapper<qsl::Qubits<qsl::Type::OmpNP, float>>;
using Sim10 = SimWrapper<qsl::Qubits<qsl::Type::OmpNP, double>>;

using NPSimTypes = ::testing::Types<Sim7,Sim8,Sim9,Sim10>;

TYPED_TEST_SUITE(Measurements, SimTypes);
TYPED_TEST_SUITE(NPMeasurements, NPSimTypes);

TYPED_TEST(Measurements, ProbTest)
{
    using Sim = TypeParam::Sim;
    using Fp = TypeParam::Sim::Fp_type;
    const unsigned num_qubits{ 8 };

    // Make a random state
    Sim q{num_qubits};
    const std::vector<qsl::complex<Fp>> state
	= qsl::makeRandomState<Fp>(num_qubits);
    q.setState(state);

    // Set an armadillo vector to the same state
    arma::Col<std::complex<Fp>> v{ toArmaState(q) };

    // Find the probability of measurement 0 or 1 on every qubit
    for (unsigned n = 0; n < num_qubits; n++) {

	// Find the probability of getting either zero or one
	// from the simulator
	Fp p0 = q.prob(n, 0);
	Fp p1 = q.prob(n, 1);

	// Calculate the projector for the outcome |1), which is
	// the observable that has eigenvalue 0 for |0) and eigenvalue
	// 1 for |1). The expectation value of this observable in a
	// state is the probability of getting 1 on measurement.
	arma::SpMat<std::complex<Fp>> P = projector<Fp>(num_qubits, n, 1);

	// calculate the probability of zero from the projector P
	Fp p1_arma = probability(P,v);
	Fp p0_arma = 1 - p1_arma;

	EXPECT_NEAR(p0, p0_arma, 1e-6);
	EXPECT_NEAR(p1, p1_arma, 1e-6);

	// Read qubit state into armadillo to check the state hasn't changed
	arma::Col<std::complex<Fp>> qubit_v{ toArmaState(q) };
	EXPECT_TRUE(arma::approx_equal(v, qubit_v, "both", 1e-6, 1e-8));
    }

}

TYPED_TEST(NPMeasurements, ProbTest)
{
    using Sim = TypeParam::Sim;
    using Fp = TypeParam::Sim::Fp_type;
    const unsigned num_qubits{ 8 };
    const unsigned num_ones{ 5 };

    // Make a random state
    Sim q{num_qubits};
    const std::vector<qsl::complex<Fp>> state
	= qsl::makeRandomNPState<Fp>(num_qubits, num_ones);
    q.setState(state);

    // Set an armadillo vector to the same state
    arma::Col<std::complex<Fp>> v{ toArmaState(q) };

    // Find the probability of measurement 0 or 1 on every qubit
    for (unsigned n = 0; n < num_qubits; n++) {

	// Find the probability of getting either zero or one
	// from the simulator
	Fp p0 = q.prob(n, 0);
	Fp p1 = q.prob(n, 1);

	// Calculate the projector for the outcome |1), which is
	// the observable that has eigenvalue 0 for |0) and eigenvalue
	// 1 for |1). The expectation value of this observable in a
	// state is the probability of getting 1 on measurement.
	arma::SpMat<std::complex<Fp>> P = projector<Fp>(num_qubits, n, 1);

	// calculate the probability of zero from the projector P
	Fp p1_arma = probability(P,v);
	Fp p0_arma = 1 - p1_arma;

	EXPECT_NEAR(p0, p0_arma, 1e-6);
	EXPECT_NEAR(p1, p1_arma, 1e-6);

	// Read qubit state into armadillo to check the state hasn't changed
	arma::Col<std::complex<Fp>> qubit_v{ toArmaState(q) };
	EXPECT_TRUE(arma::approx_equal(v, qubit_v, "both", 1e-6, 1e-8));
    }

}

TYPED_TEST(Measurements, PostselectTest)
{
    using Sim = TypeParam::Sim;
    using Fp = TypeParam::Sim::Fp_type;
    const unsigned num_qubits{ 8 };

    // Make a random state
    Sim q{num_qubits};
    const std::vector<qsl::complex<Fp>> state
	= qsl::makeRandomState<Fp>(num_qubits);
    q.setState(state);

    // Make a copy to re-initialise the state
    const Sim q_copy{q};

    // Set an armadillo vector to the same state
    arma::Col<std::complex<Fp>> v{ toArmaState(q) };

    for (unsigned n = 0; n < num_qubits; n++) {

	// Reset the state to the copy
	q = q_copy;

	// Postselect on the 0 outcome
	Fp p0 = q.postselect(n, 0);

	// Calculate the projector for outcome 0
	arma::SpMat<std::complex<Fp>> P0 = projector<Fp>(num_qubits, n, 0);

	// Check that the state collapses to the correct thing
	arma::Col<std::complex<Fp>> state_1{ toArmaState(q) };
	arma::Col<std::complex<Fp>> state_2{ applyProjector(P0, v) };
	EXPECT_TRUE(arma::approx_equal(state_1, state_2, "both", 1e-6, 1e-8));

	// Check that the probability returned by postselect is correct
	Fp p0_arma = probability(P0,v);
	EXPECT_NEAR(p0, p0_arma, 1e-6);

	// Reset the state to the copy
	q = q_copy;

	// Postselect on the 1 outcome
	Fp p1 = q.postselect(n, 1);

	// Calculate the projector for outcome 1
	arma::SpMat<std::complex<Fp>> P1 = projector<Fp>(num_qubits, n, 1);

	// Check that the state collapses to the correct thing
	state_1 = toArmaState(q);
	state_2 = applyProjector(P1, v);
	EXPECT_TRUE(arma::approx_equal(state_1, state_2, "both", 1e-6, 1e-8));

	// Check that the probability returned by postselect is correct
	Fp p1_arma = probability(P1,v);
	EXPECT_NEAR(p1, p1_arma, 1e-6);
    }
}

TYPED_TEST(NPMeasurements, PostselectTest)
{
    using Sim = TypeParam::Sim;
    using Fp = TypeParam::Sim::Fp_type;
    const unsigned num_qubits{ 8 };
    const unsigned num_ones{ 5 };

    // Make a random state
    Sim q{num_qubits};
    const std::vector<qsl::complex<Fp>> state
	= qsl::makeRandomNPState<Fp>(num_qubits, num_ones);
    q.setState(state);

    // Make a copy to re-initialise the state
    const Sim q_copy{q};

    // Set an armadillo vector to the same state
    arma::Col<std::complex<Fp>> v{ toArmaState(q) };

    for (unsigned n = 0; n < num_qubits; n++) {

	// Reset the state to the copy
	q = q_copy;

	// Postselect on the 0 outcome
	Fp p0 = q.postselect(n, 0);

	// Calculate the projector for outcome 0
	arma::SpMat<std::complex<Fp>> P0 = projector<Fp>(num_qubits, n, 0);

	// Check that the state collapses to the correct thing
	arma::Col<std::complex<Fp>> state_1{ toArmaState(q) };
	arma::Col<std::complex<Fp>> state_2{ applyProjector(P0, v) };
	EXPECT_TRUE(arma::approx_equal(state_1, state_2, "both", 1e-6, 1e-8));

	// Check that the probability returned by postselect is correct
	Fp p0_arma = probability(P0,v);
	EXPECT_NEAR(p0, p0_arma, 1e-6);

	// Reset the state to the copy
	q = q_copy;

	// Postselect on the 1 outcome
	Fp p1 = q.postselect(n, 1);

	// Calculate the projector for outcome 1
	arma::SpMat<std::complex<Fp>> P1 = projector<Fp>(num_qubits, n, 1);

	// Check that the state collapses to the correct thing
	state_1 = toArmaState(q);
	state_2 = applyProjector(P1, v);
	EXPECT_TRUE(arma::approx_equal(state_1, state_2, "both", 1e-6, 1e-8));

	// Check that the probability returned by postselect is correct
	Fp p1_arma = probability(P1,v);
	EXPECT_NEAR(p1, p1_arma, 1e-6);
    }
}

TYPED_TEST(Measurements, MeasureTest)
{
    using Sim = TypeParam::Sim;
    using Fp = TypeParam::Sim::Fp_type;
    const unsigned num_qubits{ 8 };

    // Make a random state
    Sim q{num_qubits};
    const std::vector<qsl::complex<Fp>> state
	= qsl::makeRandomState<Fp>(num_qubits);
    q.setState(state);

    // Make a copy to re-initialise the state
    const Sim q_copy{q};

    // Set an armadillo vector to the same state
    arma::Col<std::complex<Fp>> v{ toArmaState(q) };

    for (unsigned n = 0; n < num_qubits; n++) {

	// Make the projectors for both outcomes
	// The vector is indexed by outcome
	std::vector<arma::SpMat<std::complex<Fp>>> P {
	    projector<Fp>(num_qubits, n, 0),
	    projector<Fp>(num_qubits, n, 1)
	};

	// Store the running average measured outcome
	arma::running_stat<double> X;

	// Sample the measure function many times
	std::size_t samples{ 1000 };
	for (std::size_t s = 0; s < samples; s++) {

	    // Reset the state to the copy
	    q = q_copy;

	    // Measure, get outcome, and collapse the state
	    unsigned outcome = q.measure(n);

	    // Update the statistics
	    X(outcome);

	    // Check that the state collapsed to the correct thing
	    arma::Col<std::complex<Fp>> state_1{ toArmaState(q) };
	    arma::Col<std::complex<Fp>> state_2{ applyProjector(P[outcome], v) };
	    EXPECT_TRUE(arma::approx_equal(state_1, state_2, "both", 1e-6, 1e-8));
	}

	// Compute the probability of getting 1
	Fp p1_arma = probability(P[1],v);
	Fp p1 = X.mean();

	// Compare with the true mean. This test will succeed if the
	// estimated probability is within 5% of the true value.
	///\todo We need to figure out a legitimate way to test
	/// whether the probability is correct.
	EXPECT_NEAR(p1, p1_arma, UNCERTAINTY);
    }
}

TYPED_TEST(NPMeasurements, MeasureTest)
{
    using Sim = TypeParam::Sim;
    using Fp = TypeParam::Sim::Fp_type;
    const unsigned num_qubits{ 8 };
    const unsigned num_ones{ 5 };

    // Make a random state
    Sim q{num_qubits};
    const std::vector<qsl::complex<Fp>> state
	= qsl::makeRandomNPState<Fp>(num_qubits, num_ones);
    q.setState(state);

    // Make a copy to re-initialise the state
    const Sim q_copy{q};

    // Set an armadillo vector to the same state
    arma::Col<std::complex<Fp>> v{ toArmaState(q) };

    for (unsigned n = 0; n < num_qubits; n++) {

	// Make the projectors for both outcomes
	// The vector is indexed by outcome
	std::vector<arma::SpMat<std::complex<Fp>>> P {
	    projector<Fp>(num_qubits, n, 0),
	    projector<Fp>(num_qubits, n, 1)
	};

	// Store the running average measured outcome
	arma::running_stat<double> X;

	// Sample the measure function many times
	std::size_t samples{ 1000 };
	for (std::size_t s = 0; s < samples; s++) {

	    // Reset the state to the copy
	    q = q_copy;

	    // Measure, get outcome, and collapse the state
	    unsigned outcome = q.measure(n);

	    // Update the statistics
	    X(outcome);

	    // Check that the state collapsed to the correct thing
	    arma::Col<std::complex<Fp>> state_1{ toArmaState(q) };
	    arma::Col<std::complex<Fp>> state_2{ applyProjector(P[outcome], v) };
	    EXPECT_TRUE(arma::approx_equal(state_1, state_2, "both", 1e-6, 1e-8));
	}

	// Compute the probability of getting 1
	Fp p1_arma = probability(P[1],v);
	Fp p1 = X.mean();

	// Compare with the true mean. This test will succeed if the
	// estimated probability is within 5% of the true value.
	///\todo We need to figure out a legitimate way to test
	/// whether the probability is correct.
	EXPECT_NEAR(p1, p1_arma, UNCERTAINTY);

    }
}

TYPED_TEST(Measurements, SampleTest)
{
    using Sim = TypeParam::Sim;
    using Fp = TypeParam::Sim::Fp_type;
    const unsigned num_qubits{ 8 };

    // Make a random state
    Sim q{num_qubits};
    const std::vector<qsl::complex<Fp>> state
	= qsl::makeRandomState<Fp>(num_qubits);
    q.setState(state);

    // Set an armadillo vector to the same state
    arma::Col<std::complex<Fp>> v{ toArmaState(q) };

    for (unsigned n = 0; n < num_qubits; n++) {

	// Make the projector onto the |0) state
	arma::SpMat<std::complex<Fp>> P0 { projector<Fp>(num_qubits, n, 0) };

	// Sample the measure function many times
	std::size_t samples{ 1000 };

	// Sample outcomes from the state vector
	std::vector<std::size_t> outcomes = q.sample(n, samples);

	// Compute the probability of getting 0
	Fp p0 = probability(P0,v);
	Fp p0_arma = static_cast<double>(outcomes[0])/(outcomes[0] + outcomes[1]);

	// Compare with the true mean. This test will succeed if the
	// estimated probability is within 5% of the true value.
	///\todo We need to figure out a legitimate way to test
	/// whether the probability is correct.
	EXPECT_NEAR(p0, p0_arma, UNCERTAINTY);
    }
}

TYPED_TEST(NPMeasurements, SampleTest)
{
    using Sim = TypeParam::Sim;
    using Fp = TypeParam::Sim::Fp_type;
    const unsigned num_qubits{ 8 };
    const unsigned num_ones{ 5 };

    // Make a random state
    Sim q{num_qubits};
    const std::vector<qsl::complex<Fp>> state
	= qsl::makeRandomNPState<Fp>(num_qubits, num_ones);
    q.setState(state);

    // Set an armadillo vector to the same state
    arma::Col<std::complex<Fp>> v{ toArmaState(q) };

    for (unsigned n = 0; n < num_qubits; n++) {

	// Make the projector onto the |0) state
	arma::SpMat<std::complex<Fp>> P0 { projector<Fp>(num_qubits, n, 0) };

	// Sample the measure function many times
	std::size_t samples{ 1000 };

	// Sample outcomes from the state vector
	std::vector<std::size_t> outcomes = q.sample(n, samples);

	// Compute the probability of getting 0
	Fp p0 = probability(P0,v);
	Fp p0_arma = static_cast<double>(outcomes[0])/(outcomes[0] + outcomes[1]);

	// Compare with the true mean. This test will succeed if the
	// estimated probability is within 5% of the true value.
	///\todo We need to figure out a legitimate way to test
	/// whether the probability is correct.
	EXPECT_NEAR(p0, p0_arma, UNCERTAINTY);
    }
}

TYPED_TEST(Measurements, MeasureAllTest)
{
    using Sim = TypeParam::Sim;
    using Fp = TypeParam::Sim::Fp_type;
    const unsigned num_qubits{ 4 };

    // Make a random state
    Sim q{num_qubits};
    const std::vector<qsl::complex<Fp>> state
	= qsl::makeRandomState<Fp>(num_qubits);
    q.setState(state);

    // Make a copy to re-initialise the state
    const Sim q_copy{q};

    // Set an armadillo vector to the same state
    arma::Col<std::complex<Fp>> v{ toArmaState(q) };

    // Sample the measure function many times
    std::size_t samples{ 1000 };

    // Record how many times each outcome was measured
    std::map<std::size_t, unsigned> results;

    for (std::size_t s = 0; s < samples; s++) {

	// Reset the state to the copy
	q = q_copy;

	// Measure, get outcome bitstring, and collapse to basis state
	std::size_t outcome = q.measureAll();
	results[outcome]++;

	// Make the resulting basis state
	const std::size_t dim{ 1ULL << num_qubits  };
	arma::Col<std::complex<Fp>> state_2{ dim, arma::fill::zeros };
	state_2(outcome) = 1;

	// Check that the state collapsed to the correct thing
	arma::Col<std::complex<Fp>> state_1{ toArmaState(q) };
	EXPECT_TRUE(arma::approx_equal(state_1, state_2, "both", 1e-6, 1e-8));
    }

    // Check that the probability of each outcome is correct
    for (const auto & [outcome, s] : results) {

	// Only check stats if there are enough samples
	if (s > 50) {

	    // Compute the probability estimate
	    double p_est = static_cast<double>(s)/samples;

	    // Compute the true probability
	    double p_true = std::abs(v[outcome]) * std::abs(v[outcome]);

	    // Check that the estimate is near the true value (with 5%)
	    ///\todo Find a legitimate statistical test.
	    EXPECT_NEAR(p_est, p_true, UNCERTAINTY);

	}

    }
}

TYPED_TEST(NPMeasurements, MeasureAllTest)
{
    using Sim = TypeParam::Sim;
    using Fp = TypeParam::Sim::Fp_type;
    const unsigned num_qubits{ 4 };
    const unsigned num_ones{ 2 };

    // Make a random state
    Sim q{num_qubits};
    const std::vector<qsl::complex<Fp>> state
	= qsl::makeRandomNPState<Fp>(num_qubits, num_ones);
    q.setState(state);

    // Make a copy to re-initialise the state
    const Sim q_copy{q};

    // Set an armadillo vector to the same state
    arma::Col<std::complex<Fp>> v{ toArmaState(q) };

    // Sample the measure function many times
    std::size_t samples{ 1000 };

    // Record how many times each outcome was measured
    std::map<std::size_t, unsigned> results;

    for (std::size_t s = 0; s < samples; s++) {

	// Reset the state to the copy
	q = q_copy;

	// Measure, get outcome bitstring, and collapse to basis state
	std::size_t outcome = q.measureAll();
	results[outcome]++;

	// Make the resulting basis state
	const std::size_t dim{ 1ULL << num_qubits  };
	arma::Col<std::complex<Fp>> state_2{ dim, arma::fill::zeros };
	state_2(outcome) = 1;

	// Check that the state collapsed to the correct thing
	arma::Col<std::complex<Fp>> state_1{ toArmaState(q) };
	EXPECT_TRUE(arma::approx_equal(state_1, state_2, "both", 1e-6, 1e-8));
    }

    // Check that the probability of each outcome is correct
    for (const auto & [outcome, s] : results) {

	// Only check stats if there are enough samples
	if (s > 50) {

	    // Compute the probability estimate
	    double p_est = static_cast<double>(s)/samples;

	    // Compute the true probability
	    double p_true = std::abs(v[outcome]) * std::abs(v[outcome]);

	    // Check that the estimate is near the true value (with 5%)
	    ///\todo Find a legitimate statistical test.
	    EXPECT_NEAR(p_est, p_true, UNCERTAINTY);

	}

    }
}

TYPED_TEST(Measurements, SampleAllTest)
{
    using Sim = TypeParam::Sim;
    using Fp = TypeParam::Sim::Fp_type;
    const unsigned num_qubits{ 4 };

    // Make a random state
    Sim q{num_qubits};
    const std::vector<qsl::complex<Fp>> state
	= qsl::makeRandomState<Fp>(num_qubits);
    q.setState(state);

    // Make a copy to re-initialise the state
    const Sim q_copy{q};

    // Set an armadillo vector to the same state
    arma::Col<std::complex<Fp>> v{ toArmaState(q) };

    // Sample the measure function many times
    std::size_t samples{ 1000 };

    // Record how many times each outcome was measured
    std::map<std::size_t, std::size_t> results = q.sampleAll(samples);

    // Check that there are the right number of samples
    std::size_t num_samples = 0;
    for (const auto & item : results) {
	num_samples += item.second;
    }
    EXPECT_EQ(num_samples, samples);

    // Check that the probability of each outcome is correct
    for (const auto & [outcome, s] : results) {

	// Only check stats if there are enough samples
	if (s > 50) {

	    // Compute the probability estimate
	    double p_est = static_cast<double>(s)/samples;

	    // Compute the true probability
	    double p_true = std::abs(v[outcome]) * std::abs(v[outcome]);

	    // Check that the estimate is near the true value (with 5%)
	    ///\todo Find a legitimate statistical test.
	    EXPECT_NEAR(p_est, p_true, UNCERTAINTY);

	}

    }
}

TYPED_TEST(NPMeasurements, SampleAllTest)
{
    using Sim = TypeParam::Sim;
    using Fp = TypeParam::Sim::Fp_type;
    const unsigned num_qubits{ 4 };
    const unsigned num_ones{ 2 };

    // Make a random state
    Sim q{num_qubits};
    const std::vector<qsl::complex<Fp>> state
	= qsl::makeRandomNPState<Fp>(num_qubits, num_ones);
    q.setState(state);

    // Make a copy to re-initialise the state
    const Sim q_copy{q};

    // Set an armadillo vector to the same state
    arma::Col<std::complex<Fp>> v{ toArmaState(q) };

    // Sample the measure function many times
    std::size_t samples{ 1000 };

    // Record how many times each outcome was measured
    std::map<std::size_t, std::size_t> results = q.sampleAll(samples);

    // Check that there are the right number of samples
    std::size_t num_samples = 0;
    for (const auto & item : results) {
	num_samples += item.second;
    }
    EXPECT_EQ(num_samples, samples);

    // Check that the probability of each outcome is correct
    for (const auto & [outcome, s] : results) {

	// Only check stats if there are enough samples
	if (s > 50) {

	    // Compute the probability estimate
	    double p_est = static_cast<double>(s)/samples;

	    // Compute the true probability
	    double p_true = std::abs(v[outcome]) * std::abs(v[outcome]);

	    // Check that the estimate is near the true value (with 5%)
	    ///\todo Find a legitimate statistical test.
	    EXPECT_NEAR(p_est, p_true, UNCERTAINTY);

	}

    }
}

TEST(SampleAll2Test, DefaultDouble)
{
    using Sim = qsl::Qubits<qsl::Type::Default, double>;
    using Fp = Sim::Fp_type;
    const unsigned num_qubits{ 4 };

    // Make a random state
    Sim q{num_qubits};
    const std::vector<qsl::complex<Fp>> state
	= qsl::makeRandomState<Fp>(num_qubits);
    q.setState(state);

    // Make a copy to re-initialise the state
    const Sim q_copy{q};

    // Set an armadillo vector to the same state
    arma::Col<std::complex<Fp>> v{ toArmaState(q) };

    // Sample the measure function many times
    std::size_t samples{ 1000 };

    // Record how many times each outcome was measured
    std::map<std::size_t, std::size_t> results = q.sampleAll2(samples);

    // Check that the probability of each outcome is correct
    for (const auto & [outcome, s] : results) {

	// Only check stats if there are enough samples
	if (s > 50) {

	    // Compute the probability estimate
	    double p_est = static_cast<double>(s)/samples;

	    // Compute the true probability
	    double p_true = std::abs(v[outcome]) * std::abs(v[outcome]);

	    // Check that the estimate is near the true value (with 5%)
	    ///\todo Find a legitimate statistical test.
	    EXPECT_NEAR(p_est, p_true, UNCERTAINTY);

	}

   }
}

TEST(SampleAll2Test, ResizeDouble)
{
    using Sim = qsl::Qubits<qsl::Type::Resize, double>;
    using Fp = Sim::Fp_type;
    const unsigned num_qubits{ 4 };

    // Make a random state
    Sim q{num_qubits};
    const std::vector<qsl::complex<Fp>> state
	= qsl::makeRandomState<Fp>(num_qubits);
    q.setState(state);

    // Make a copy to re-initialise the state
    const Sim q_copy{q};

    // Set an armadillo vector to the same state
    arma::Col<std::complex<Fp>> v{ toArmaState(q) };

    // Sample the measure function many times
    std::size_t samples{ 1000 };

    // Record how many times each outcome was measured
    std::map<std::size_t, std::size_t> results = q.sampleAll2(samples);

    // Check that the probability of each outcome is correct
    for (const auto & [outcome, s] : results) {

	// Only check stats if there are enough samples
	if (s > 50) {

	    // Compute the probability estimate
	    double p_est = static_cast<double>(s)/samples;

	    // Compute the true probability
	    double p_true = std::abs(v[outcome]) * std::abs(v[outcome]);

	    // Check that the estimate is near the true value (with 5%)
	    ///\todo Find a legitimate statistical test.
	    EXPECT_NEAR(p_est, p_true, UNCERTAINTY);

	}

    }
}

TEST(SampleAll2Test, NPDouble)
{
    using Sim = qsl::Qubits<qsl::Type::NP, double>;
    using Fp = Sim::Fp_type;
    const unsigned num_qubits{ 4 };
    const unsigned num_ones{ 2 };
    // Make a random state
    Sim q{num_qubits};
    const std::vector<qsl::complex<Fp>> state
	= qsl::makeRandomNPState<Fp>(num_qubits, num_ones);
    q.setState(state);

    // Make a copy to re-initialise the state
    const Sim q_copy{q};

    // Set an armadillo vector to the same state
    arma::Col<std::complex<Fp>> v{ toArmaState(q) };

    // Sample the measure function many times
    std::size_t samples{ 1000 };

    // Record how many times each outcome was measured
    std::map<std::size_t, std::size_t> results = q.sampleAll2(samples);

    // Check that the probability of each outcome is correct
    for (const auto & [outcome, s] : results) {

	// Only check stats if there are enough samples
	if (s > 50) {

	    // Compute the probability estimate
	    double p_est = static_cast<double>(s)/samples;

	    // Compute the true probability
	    double p_true = std::abs(v[outcome]) * std::abs(v[outcome]);

	    // Check that the estimate is near the true value (with 5%)
	    ///\todo Find a legitimate statistical test.
	    EXPECT_NEAR(p_est, p_true, UNCERTAINTY);

	}

    }
}


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#define NT2_UNIT_MODULE "nt2 arithmetic toolbox - correct_fma/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// Test behavior of arithmetic components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created by jt the 28/11/2010
/// modified by jt the 23/03/2011
///
#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/memory/buffer.hpp>
#include <nt2/sdk/constant/real.hpp>
#include <nt2/sdk/constant/infinites.hpp>
#include <nt2/include/functions/max.hpp>
#include <nt2/toolbox/arithmetic/include/correct_fma.hpp>
// specific includes for arity 3 tests
#include <nt2/sdk/constant/eps_related.hpp>

NT2_TEST_CASE_TPL ( correct_fma_real__3_0,  NT2_REAL_TYPES)
{

  using nt2::correct_fma;
  using nt2::tag::correct_fma_;
  typedef typename nt2::meta::as_integer<T>::type iT;
  typedef typename nt2::meta::call<correct_fma_(T,T,T)>::type r_t;
  typedef typename nt2::meta::upgrade<T>::type u_t;
  typedef typename boost::result_of<nt2::meta::arithmetic(T,T,T)>::type wished_r_t;

  // return type conformity test
  NT2_TEST( (boost::is_same < r_t, wished_r_t >::value) );
  std::cout << std::endl;
  double ulpd;
  ulpd=0.0;

  // specific values tests
  NT2_TEST_ULP_EQUAL(correct_fma(nt2::Inf<T>(), nt2::Inf<T>(), nt2::Inf<T>()), nt2::Inf<T>(), 0);
  NT2_TEST_ULP_EQUAL(correct_fma(nt2::Minf<T>(), nt2::Minf<T>(), nt2::Minf<T>()), nt2::Nan<T>(), 0);
  NT2_TEST_ULP_EQUAL(correct_fma(nt2::Mone<T>(), nt2::Mone<T>(), nt2::Mone<T>()), nt2::Zero<T>(), 0);
  NT2_TEST_ULP_EQUAL(correct_fma(nt2::Nan<T>(), nt2::Nan<T>(), nt2::Nan<T>()), nt2::Nan<T>(), 0);
  NT2_TEST_ULP_EQUAL(correct_fma(nt2::One<T>(), nt2::One<T>(), nt2::One<T>()), nt2::Two<T>(), 0);
  NT2_TEST_ULP_EQUAL(correct_fma(nt2::One<T>()+nt2::Eps<T>(), nt2::One<T>()-nt2::Eps<T>(),nt2::Mone<T>()), -nt2::Eps<T>()*nt2::Eps<T>(), 0);
  NT2_TEST_ULP_EQUAL(correct_fma(nt2::Zero<T>(), nt2::Zero<T>(), nt2::Zero<T>()), nt2::Zero<T>(), 0);
} // end of test for real_

NT2_TEST_CASE_TPL ( correct_fma_signed_int__3_0,  NT2_INTEGRAL_SIGNED_TYPES)
{

  using nt2::correct_fma;
  using nt2::tag::correct_fma_;
  typedef typename nt2::meta::as_integer<T>::type iT;
  typedef typename nt2::meta::call<correct_fma_(T,T,T)>::type r_t;
  typedef typename nt2::meta::upgrade<T>::type u_t;
  typedef typename boost::result_of<nt2::meta::arithmetic(T,T,T)>::type wished_r_t;

  // return type conformity test
  NT2_TEST( (boost::is_same < r_t, wished_r_t >::value) );
  std::cout << std::endl;
  double ulpd;
  ulpd=0.0;

  // specific values tests
  NT2_TEST_ULP_EQUAL(correct_fma(nt2::Mone<T>(), nt2::Mone<T>(), nt2::Mone<T>()), nt2::Zero<T>(), 0);
  NT2_TEST_ULP_EQUAL(correct_fma(nt2::One<T>(), nt2::One<T>(), nt2::One<T>()), nt2::Two<T>(), 0);
  NT2_TEST_ULP_EQUAL(correct_fma(nt2::Zero<T>(), nt2::Zero<T>(), nt2::Zero<T>()), nt2::Zero<T>(), 0);
} // end of test for signed_int_

NT2_TEST_CASE_TPL ( correct_fma_unsigned_int__3_0,  NT2_UNSIGNED_TYPES)
{

  using nt2::correct_fma;
  using nt2::tag::correct_fma_;
  typedef typename nt2::meta::as_integer<T>::type iT;
  typedef typename nt2::meta::call<correct_fma_(T,T,T)>::type r_t;
  typedef typename nt2::meta::upgrade<T>::type u_t;
  typedef typename boost::result_of<nt2::meta::arithmetic(T,T,T)>::type wished_r_t;

  // return type conformity test
  NT2_TEST( (boost::is_same < r_t, wished_r_t >::value) );
  std::cout << std::endl;
  double ulpd;
  ulpd=0.0;

  // specific values tests
  NT2_TEST_ULP_EQUAL(correct_fma(nt2::One<T>(), nt2::One<T>(), nt2::One<T>()), nt2::Two<T>(), 0);
  NT2_TEST_ULP_EQUAL(correct_fma(nt2::Zero<T>(), nt2::Zero<T>(), nt2::Zero<T>()), nt2::Zero<T>(), 0);
} // end of test for unsigned_int_


/* Copyright 2020 Oinam Romesh Meitei

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
 */

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <unsupported/Eigen/MatrixFunctions>
#include <cmath>
#include <vector>
#include <typeinfo>
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <iomanip>

#include "getham.h"
#include "agradc.h"

gradc grad_ana(
	       std::vector<double> &tlist,
	       std::vector<std::complex<double> > &ini_vec,
	       pulsec pobj,
	       std::vector< std::vector< Eigen::SparseMatrix
	       <double,0,ptrdiff_t> > > hdrive,
	       std::vector< std::complex<double> > dsham,
	       std::vector< int> &states,
	       Eigen::MatrixXcd &cHam){

  Eigen::SparseMatrix<std::complex<double> > hamdr;
  Eigen::SparseMatrix<std::complex<double> > hamdR;
  std::complex<double> hcoef; //, hcoefc;

  int i,j;
  int dsham_len = dsham.size();
  int tlen = tlist.size();
  int nstate = states.size();
  int &nqubit = pobj.nqubit;
  double energy;

  std::complex<double> psit_g;

  Eigen::SparseMatrix<std::complex<double> >
    matexp_(dsham_len, dsham_len);

  Eigen::SparseMatrix<std::complex<double> > hD, Hd_;
  Eigen::MatrixXcd H1_, P_;
  Eigen::VectorXcd psi_;
  Eigen::VectorXcd states_(nstate);

  std::vector< Eigen::VectorXcd > O_(nqubit,
				     Eigen::VectorXcd (dsham_len));

  std::vector< Eigen::VectorXcd > g1__(nqubit,
				       Eigen::VectorXcd (dsham_len));

  std::vector< Eigen::VectorXcd > g1_(nqubit,
				       Eigen::VectorXcd (nstate));

  Eigen::Map<Eigen::VectorXcd> ket_(ini_vec.data(), ini_vec.size());
  std::vector< Eigen::SparseMatrix<std::complex<double> > > hamd(nqubit);
  std::vector<std::vector<double> > t_grad(nqubit,
					   std::vector<double> (tlen));

  double tau = tlist[tlen-1] / tlen;
  std::complex<double> im(0.0,-tau);
  std::complex<double> imp(0.0,tau);

  for (int t=0; t<tlen; t++){

    hD = getham2(tlist[t], t, pobj, hdrive, dsham, dsham_len, matexp_, hamd, hamdr, hamdR, hcoef);
    H1_ = im * Eigen::MatrixXcd(hD);
    ket_ = H1_.exp() * ket_;
  }

  for (i=0; i<nqubit; i++){
    g1__[i] = hamd[i] * ket_;
  }

  for (i=0; i<nstate; i++){
    states_[i] = ket_[states[i]];
  }
  double nrm =  states_.norm();

  for (int i = 0; i<nqubit; i++){
    for (int j=0; j<nstate; j++){
      g1_[i][j] = g1__[i][states[j]];
    }
  }

  psi_ = cHam * states_;

  std::complex<double> energy_ = states_.conjugate().transpose() * psi_;
  energy = energy_.real();

  psi_ = psi_.conjugate();
  Eigen::Transpose<Eigen::VectorXcd> psi1_ = psi_.transpose();
  P_ = Eigen::MatrixXcd::Identity(dsham_len, dsham_len);

  double tau1 = 2.0 * tau;

  for (i=0; i<nqubit; i++){

    psit_g = psi1_ * g1_[i];
    t_grad[i][tlen-1] = tau1 * psit_g.imag();
  }

  for (int idx=tlen-1; idx > 0; idx--){
    Hd_ = getham3( tlist[idx], idx, pobj, hdrive, dsham, dsham_len, matexp_, hamdr, hamdR, hcoef);
    hD = getham2(tlist[idx-1], idx-1, pobj, hdrive, dsham, dsham_len,
		 matexp_, hamd, hamdr, hamdR, hcoef);

    H1_ = (imp*Eigen::MatrixXcd(Hd_)).exp();
    ket_ = H1_ * ket_;
    P_ = P_ * H1_.conjugate().transpose();

    for (i=0; i<nqubit; i++){
      O_[i] = P_ * hamd[i] * ket_;
    }

    for (i=0; i<nqubit; i++){
      for (j=0; j<nstate; j++){
	g1_[i][j] = O_[i][states[j]];
      }
    }

    for (i=0; i<nqubit; i++){
      psit_g = psi1_ * g1_[i];
      t_grad[i][idx-1] = tau1 * psit_g.imag();
    }
  }
  gradc g1(energy, nrm, t_grad);

  return g1;
}

gradc grad_ana_normalized(
                               std::vector<double> &tlist,
                               std::vector<std::complex<double> > &ini_vec,
                               pulsec pobj,
                               std::vector< std::vector< Eigen::SparseMatrix
			       <double,0,ptrdiff_t> > > hdrive,
                               std::vector< std::complex<double> > dsham,
			       std::vector< int> &states,
			       Eigen::MatrixXcd &cHam){

  Eigen::SparseMatrix<std::complex<double> > hamdr;
  Eigen::SparseMatrix<std::complex<double> > hamdR;
  std::complex<double> hcoef;

  int i,j;
  int dsham_len = dsham.size();
  int tlen = tlist.size();
  int nstate = states.size();
  int &nqubit = pobj.nqubit;
  double energy,nrm2, nrm4;

  std::complex<double> psit_g,psit_gN;

  Eigen::SparseMatrix<std::complex<double> >
    matexp_(dsham_len, dsham_len);

  Eigen::SparseMatrix<std::complex<double> > hD, Hd_;
  Eigen::MatrixXcd H1_, P_;
  Eigen::VectorXcd psi_,psi_N;
  Eigen::VectorXcd states_(nstate);

  std::vector< Eigen::VectorXcd > O_(nqubit,
				     Eigen::VectorXcd (dsham_len));

  std::vector< Eigen::VectorXcd > g1__(nqubit,
				       Eigen::VectorXcd (dsham_len));

  std::vector< Eigen::VectorXcd > g1_(nqubit,
				       Eigen::VectorXcd (nstate));

  Eigen::Map<Eigen::VectorXcd> ket_(ini_vec.data(), ini_vec.size());
  std::vector< Eigen::SparseMatrix<std::complex<double> > > hamd(nqubit);
  std::vector<std::vector<double> > t_grad(nqubit,
					   std::vector<double> (tlen));

  double tau = tlist[tlen-1] / tlen;
  std::complex<double> im(0.0,-tau);
  std::complex<double> imp(0.0,tau);

  for (int t=0; t<tlen; t++){

    hD = getham2(tlist[t], t, pobj, hdrive, dsham, dsham_len, matexp_, hamd, hamdr, hamdR, hcoef);
    H1_ = im * Eigen::MatrixXcd(hD);
    ket_ = H1_.exp() * ket_;
  }

  for (i=0; i<nqubit; i++){
    g1__[i] = hamd[i] * ket_;
  }

  for (i=0; i<nstate; i++){
    states_[i] = ket_[states[i]];
  }
  double nrm =  states_.norm();
  nrm2 = nrm*nrm;
  nrm4 = nrm2*nrm2;

  for (int i = 0; i<nqubit; i++){
    for (int j=0; j<nstate; j++){
      g1_[i][j] = g1__[i][states[j]];
    }
  }

  psi_ = cHam * states_;
  psi_N = states_;

  std::complex<double> energy_ = states_.conjugate().transpose() * psi_;
  energy = energy_.real();

  psi_ = psi_.conjugate();
  Eigen::Transpose<Eigen::VectorXcd> psi1_ = psi_.transpose();
  psi_N = psi_N.conjugate();
  Eigen::Transpose<Eigen::VectorXcd> psi1_N = psi_N.transpose();
  P_ = Eigen::MatrixXcd::Identity(dsham_len, dsham_len);
  double tau1 = 2.0 * tau;
  nrm4 = tau1/nrm4;

  for (i=0; i<nqubit; i++){

    psit_g = psi1_ * g1_[i];
    psit_gN = psi1_N * g1_[i];
    t_grad[i][tlen-1] = (psit_g.imag()*nrm2 - energy*psit_gN.imag())*nrm4;
  }

  for (int idx=tlen-1; idx > 0; idx--){
    Hd_ = getham3( tlist[idx], idx, pobj, hdrive, dsham, dsham_len, matexp_, hamdr, hamdR, hcoef);
    hD = getham2(tlist[idx-1], idx-1, pobj, hdrive, dsham, dsham_len,
		 matexp_, hamd, hamdr, hamdR, hcoef);

    H1_ = (imp*Eigen::MatrixXcd(Hd_)).exp();
    ket_ = H1_ * ket_;
    P_ = P_ * H1_.conjugate().transpose();

    for (i=0; i<nqubit; i++){
      O_[i] = P_ * hamd[i] * ket_;
    }

    for (i=0; i<nqubit; i++){
      for (j=0; j<nstate; j++){
	g1_[i][j] = O_[i][states[j]];
      }
    }

    for (i=0; i<nqubit; i++){
      psit_g = psi1_ * g1_[i];
      psit_gN = psi1_N * g1_[i];
      t_grad[i][idx-1] = (psit_g.imag()*nrm2 - energy*psit_gN.imag())*nrm4;
    }
  }
  energy = energy/nrm2;
  gradc g1(energy, nrm, t_grad);

  return g1;
}


#include <ql/quantlib.hpp>

#include <boost/assign/std/vector.hpp>

using namespace boost::assign;

using namespace QuantLib;

int main() {

    // set the evaluation date

    Date refDate(13, Oct, 2014);
    Settings::instance().evaluationDate() = refDate;

    std::cerr << "Evaluation Date " << Settings::instance().evaluationDate()
              << "\n";

    // set the EUR USD exchange rate

    Real exRate = atof(getenv("SPOT"));
    ExchangeRate eurusd(EURCurrency(), USDCurrency(), exRate);
    ExchangeRateManager::instance().add(eurusd);

    // build an EUR and USD curve

    Handle<YieldTermStructure> eurYts(
        boost::make_shared<FlatForward>(0, TARGET(), 0.01, Actual365Fixed()));
    Handle<YieldTermStructure> usdYts(
        boost::make_shared<FlatForward>(0, TARGET(), 0.02, Actual365Fixed()));

    // build an ECB EUR-USD fx fixing index

    JointCalendar TgtNy((TARGET()), (UnitedStates()));

    boost::shared_ptr<FxIndex> ecbIndex = boost::make_shared<FxIndex>(
        "ECB", 2, EURCurrency(), USDCurrency(), TgtNy, eurYts, usdYts);

    // add historic fixings

    // ecbIndex->addFixing(Date(13, Nov, 2014), 1.04);
    // ecbIndex->addFixing(Date(11, Dec, 2014), 1.07);
    // ecbIndex->addFixing(Date(13, Jan, 2015), 1.05);
    // ecbIndex->addFixing(Date(12, Feb, 2015), 1.06);
    // ecbIndex->addFixing(Date(12, Mar, 2015), 1.02);
    // ecbIndex->addFixing(Date(13, Apr, 2015), 1.08);

    // set up a tarf deal

    Schedule sched(Date(15, Oct, 2014), Date(15, Oct, 2015), 1 * Months, TgtNy,
                   Following, Following, DateGeneration::Forward, false);

    std::cerr << "tarf schedule:" << std::endl;
    for (Size i = 1; i < sched.size(); ++i) {
        Date fixing = ecbIndex->fixingDate(sched[i]);
        std::cerr << fixing << ";" << sched[i] << ";"
                  << fixing.serialNumber() - refDate.serialNumber()
                  << std::endl;
    }

    double target = atof(getenv("TARGET"));
    int samples = atoi(getenv("SAMPLES"));
    int samples2 = atoi(getenv("SAMPLES2"));
    int steps = atoi(getenv("STEPS"));
    int seed = atoi(getenv("SEED"));
    char *type = getenv("TYPE");
    double shortLeverage = atof(getenv("LEV"));
    FxTarf::CouponType cpType;
    switch (type[0]) {
    case 'f':
        cpType = FxTarf::full;
        break;
    case 'c':
        cpType = FxTarf::capped;
        break;
    case 'n':
        cpType = FxTarf::none;
        break;
    default:
        QL_FAIL("unknown cl option " << type[0]);
        break;
    }

    double initialAm = atof(getenv("ACC"));
    boost::shared_ptr<SimpleQuote> initialAmQ =
        boost::make_shared<SimpleQuote>(initialAm);
    Handle<Quote> initialAcc(initialAmQ);

    FxTarf tarf(sched, ecbIndex, 100000000.0,
                boost::make_shared<PlainVanillaPayoff>(Option::Put, 1.21),
                boost::make_shared<PlainVanillaPayoff>(Option::Call, 1.21),
                target, cpType, shortLeverage, 1.0, initialAcc);

    boost::shared_ptr<SimpleQuote> fx1 =
        boost::make_shared<SimpleQuote>(eurusd.rate());
    RelinkableHandle<Quote> fxQuote(fx1);

    Handle<BlackVolTermStructure> volTs(boost::make_shared<BlackConstantVol>(
        0, TARGET(), 0.20, Actual365Fixed()));
    boost::shared_ptr<GarmanKohlagenProcess> gkProcess =
        boost::make_shared<GarmanKohlagenProcess>(fxQuote, eurYts, usdYts,
                                                  volTs);
    // boost::shared_ptr<ExtendedBlackScholesMertonProcess> gkProcess =
    //     boost::make_shared<ExtendedBlackScholesMertonProcess>(fxQuote,
    //     eurYts, usdYts,
    //                                               volTs);

    // test path generation
    // std::vector<Real> required;
    // required += 0.0, 0.05, 0.1, 0.15, 0.25, 0.47, 0.68, 0.91;
    // TimeGrid gridTmp(required.begin(),required.end(),1);
    // Real result = 0.0;
    // size_t N = 1000000;
    // PseudoRandom::rsg_type rsg =
    // PseudoRandom::make_sequence_generator(gridTmp.size(),42);
    // for(Size i=0;i<N;++i) {
    //     Real x = fxQuote->value();
    //     PseudoRandom::rsg_type::sample_type seq=rsg.nextSequence();
    //     for(Size j=1;j<gridTmp.size();++j) {
    //         x=gkProcess->evolve(gridTmp[j-1],x,gridTmp.dt(j-1),seq.value[j-1]);
    //     }
    //     result += x;
    // }
    // std::cout << "test mean = " << std::setprecision(12) << (result/N) <<
    // std::endl;
    // return 0;
    // end test path generation

    // full pricing engine and creation of proxy

    boost::shared_ptr<PricingEngine> mcEngine = MakeMcFxTarfEngine<>(gkProcess)
                                                    .withStepsPerYear(steps)
                                                    .withSamples(samples)
                                                    .withSeed(seed)
                                                    .withProxy();
    // same, but without proxy pricing
    boost::shared_ptr<PricingEngine> mcEngine2 = MakeMcFxTarfEngine<>(gkProcess)
                                                     .withStepsPerYear(steps)
                                                     .withSamples(samples2)
                                                     .withSeed(seed);
    tarf.setPricingEngine(mcEngine);
    std::cerr << "spot=" << exRate << " steps=" << steps
              << " samples=" << samples << " samples2=" << samples2
              << " type=" << type[0] << " target=" << target
              << " shortLev=" << shortLeverage << " initialAm=" << initialAm
              << std::endl;
    std::cerr << "MCEngine NPV = " << tarf.NPV() << " error est "
              << tarf.errorEstimate() << std::endl;

    // return 0; //test

    // transport the proxy to the proxy engine

    int interpolate = atoi(getenv("INT"));

    boost::shared_ptr<PricingEngine> proxyEngine =
        boost::make_shared<ProxyFxTarfEngine>(
            tarf.proxy(), fxQuote, gkProcess->riskFreeRate(), interpolate);

    tarf.setPricingEngine(proxyEngine);

    std::cerr << "ProxyEngine NPV (orig eval date and spot)= " << tarf.NPV()
              << std::endl;

    // estimate NPVs and compare to full MC pricing

    int shiftDays = atoi(getenv("DAYSHIFT"));
    Settings::instance().evaluationDate() = refDate + shiftDays;

    std::cerr << "output simulated npvs with refDate "
              << Settings::instance().evaluationDate()
              << " (DayShift=" << shiftDays << ")" << std::endl;

    double newAccAmount = atof(getenv("ACCPROXY"));
    initialAmQ->setValue(newAccAmount); // accumulated amount
    int stepOver = 0;
    for (Real x = 0.50; x <= 2.00; x += 0.01) {
        fx1->setValue(x);
        std::cout << x << " " << tarf.NPV() << " "
                  << tarf.result<Real>("coreRegionMin") << " "
                  << tarf.result<Real>("coreRegionMax") << " ";
        if (!stepOver--) {
            stepOver = 9;
            tarf.setPricingEngine(mcEngine2);
            std::cout << tarf.NPV() << " " << tarf.errorEstimate();
            tarf.setPricingEngine(proxyEngine);
        } else
            std::cout << "0.0 0.0";
        std::cout << std::endl;
    }

    return 0;
}


#include "RefElement.h"
#include "basis/Functions.h"
#include "basis/Nodal.h"
#include "quadrules/AutoRule.h"
#include "tensor/EigenMap.h"
#include "tensor/Reshape.h"
#include "tensor/TensorBase.h"
#include "util/Combinatorics.h"
#include "util/Enumerate.h"
#include "util/MultiIndex.h"

#include <Eigen/Core>
#include <Eigen/LU>

#include <cassert>
#include <cstddef>

namespace tndm {

template <std::size_t D> Managed<Matrix<double>> ModalRefElement<D>::massMatrix() const {
    auto rule = simplexQuadratureRule<D>(2 * this->degree());
    std::ptrdiff_t nbf = this->numBasisFunctions();
    Managed<Matrix<double>> M({nbf, nbf}, this->alignment());
    auto E = evaluateBasisAt(rule.points(), {0, 1});
    for (std::ptrdiff_t i = 0; i < M.shape(0); ++i) {
        for (std::ptrdiff_t j = 0; j < M.shape(1); ++j) {
            M(i, j) = 0.0;
            // The basis is orthogonal, therefore we only need to compute the diagonal
            if (i == j) {
                for (std::size_t q = 0; q < rule.size(); ++q) {
                    M(i, j) += rule.weights()[q] * E(i, q) * E(j, q);
                }
            }
        }
    }

    return M;
}

template <std::size_t D> Managed<Matrix<double>> ModalRefElement<D>::inverseMassMatrix() const {
    auto Minv = massMatrix();
    for (std::ptrdiff_t i = 0; i < Minv.shape(0); ++i) {
        Minv(i, i) = 1.0 / Minv(i, i);
    }
    return Minv;
}

template <std::size_t D>
Managed<Matrix<double>>
ModalRefElement<D>::evaluateBasisAt(std::vector<std::array<double, D>> const& points,
                                    std::array<unsigned, 2> const& permutation) const {
    using index_t = Matrix<double>::index_t;
    auto shape =
        permute(permutation, make_index<index_t>(this->numBasisFunctions(), points.size()));
    Managed<Matrix<double>> E(shape, this->alignment());
    for (std::size_t p = 0; p < points.size(); ++p) {
        for (auto&& [bf, j] : enumerate(AllIntegerSums<D>(this->degree()))) {
            auto index = permute(permutation, make_index<index_t>(bf, p));
            E(index) = DubinerP(j, points[p]);
        }
    }
    return E;
}

template <std::size_t D>
Managed<Tensor<double, 3u>>
ModalRefElement<D>::evaluateGradientAt(std::vector<std::array<double, D>> const& points,
                                       std::array<unsigned, 3> const& permutation) const {
    using index_t = Matrix<double>::index_t;
    auto shape =
        permute(permutation, make_index<index_t>(this->numBasisFunctions(), D, points.size()));
    Managed<Tensor<double, 3u>> grad(shape, this->alignment());
    for (std::size_t p = 0; p < points.size(); ++p) {
        for (auto&& [bf, j] : enumerate(AllIntegerSums<D>(this->degree()))) {
            auto dphi = gradDubinerP(j, points[p]);
            for (std::size_t d = 0; d < D; ++d) {
                auto index = permute(permutation, make_index<index_t>(bf, d, p));
                grad(index) = dphi[d];
            }
        }
    }
    return grad;
}

template <std::size_t D>
NodalRefElement<D>::NodalRefElement(unsigned degree, NodesFactory<D> const& nodesFactory,
                                    std::size_t alignment)
    : RefElement<D>(degree, alignment), refNodes_(nodesFactory(degree)) {
    assert(this->numBasisFunctions() == refNodes_.size());
    vandermonde_ = Vandermonde(this->degree(), refNodes_);
    vandermondeInv_ = vandermonde_.inverse();
}

template <std::size_t D> Managed<Matrix<double>> NodalRefElement<D>::massMatrix() const {
    std::ptrdiff_t nbf = this->numBasisFunctions();
    Managed<Matrix<double>> M({nbf, nbf}, this->alignment());

    Managed<Matrix<double>> modalM =
        ModalRefElement<D>(this->degree(), this->alignment()).massMatrix();
    EigenMap(M) = vandermondeInv_.transpose() * EigenMap(modalM) * vandermondeInv_;

    return M;
}

template <std::size_t D> Managed<Matrix<double>> NodalRefElement<D>::inverseMassMatrix() const {
    std::ptrdiff_t nbf = this->numBasisFunctions();
    Managed<Matrix<double>> Minv({nbf, nbf}, this->alignment());

    Managed<Matrix<double>> modalMinv =
        ModalRefElement<D>(this->degree(), this->alignment()).inverseMassMatrix();
    EigenMap(Minv) = vandermonde_ * EigenMap(modalMinv) * vandermonde_.transpose();

    return Minv;
}

template <std::size_t D>
Managed<Matrix<double>>
NodalRefElement<D>::evaluateBasisAt(std::vector<std::array<double, D>> const& points,
                                    std::array<unsigned, 2> const& permutation) const {
    Managed<Matrix<double>> E =
        ModalRefElement<D>(this->degree(), this->alignment()).evaluateBasisAt(points, permutation);
    auto Emap = EigenMap(E);
    if (permutation[0] == 0 && permutation[1] == 1) {
        Emap = vandermondeInv_.transpose() * Emap;
    } else if (permutation[0] == 1 && permutation[1] == 0) {
        Emap = Emap * vandermondeInv_;
    } else {
        assert(false);
    }
    return E;
}

template <std::size_t D>
Managed<Tensor<double, 3u>>
NodalRefElement<D>::evaluateGradientAt(std::vector<std::array<double, D>> const& points,
                                       std::array<unsigned, 3> const& permutation) const {
    Managed<Tensor<double, 3u>> gradE = ModalRefElement<D>(this->degree(), this->alignment())
                                            .evaluateGradientAt(points, permutation);

    assert(vandermondeInv_.cols() == vandermondeInv_.rows());
    // 0,1,2 F_idq = V_ji E_jdq => F_i(dq) = V^T E_j(dq)
    // 0,2,1 F_iqd = V_ji E_jqd => F_i(qd) = V^T E_j(qd)
    if (permutation[0] == 0) {
        assert((permutation[1] == 1 && permutation[2] == 2) ||
               (permutation[1] == 2 && permutation[2] == 1));
        auto mat = reshape(gradE, vandermondeInv_.rows(), D * points.size());
        EigenMap(mat) = vandermondeInv_.transpose() * EigenMap(mat);
    }
    // 1,2,0 F_dqi = V_ji E_dqj => F_(dq)i = E_(dq)j V
    // 2,1,0 F_qdi = V_ji E_qdj => F_(qd)i = E_(qd)j V
    else if (permutation[2] == 0) {
        assert((permutation[0] == 1 && permutation[1] == 2) ||
               (permutation[0] == 2 && permutation[1] == 1));
        auto mat = reshape(gradE, D * points.size(), vandermondeInv_.rows());
        EigenMap(mat) = EigenMap(mat) * vandermondeInv_;
    }
    // 1,0,2 F_diq = V_ji E_djq => F_di[q] = E_dj[q] V
    // 2,0,1 F_qid = V_ji E_qjd => F_qi[d] = E_qj[d] V
    else {
        assert((permutation[0] == 1 && permutation[2] == 2) ||
               (permutation[0] == 2 && permutation[2] == 1));
        for (decltype(gradE)::index_t i = 0; i < gradE.shape(2); ++i) {
            auto mat = gradE.subtensor(slice{}, slice{}, i);
            EigenMap(mat) = EigenMap(mat) * vandermondeInv_;
        }
    }

    return gradE;
}

template class ModalRefElement<1ul>;
template class ModalRefElement<2ul>;
template class ModalRefElement<3ul>;
template class NodalRefElement<1ul>;
template class NodalRefElement<2ul>;
template class NodalRefElement<3ul>;

} // namespace tndm


#include <ros/ros.h>
#include <vector>
#include <math.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <boost/foreach.hpp>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/kdtree/kdtree.h>
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <fstream>
#include <string.h>

typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;

boost::shared_ptr<pcl::visualization::PCLVisualizer>
simpleVis(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud, pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr plane)
{
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer("3D Viewer"));
  viewer->setBackgroundColor(0, 0, 0);
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> rgb1(cloud, 0, 0, 255);
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> rgb2(plane, 255, 0, 0);
  viewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb1, "sample cloud1");
  viewer->addPointCloud<pcl::PointXYZRGB> (plane, rgb2, "sample cloud2");
  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud1");
  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud2");
  viewer->initCameraParameters ();
  return (viewer);
}

void callback(const PointCloud::ConstPtr& msg)
{
  ROS_INFO("I heard: [%d] [%d]",msg->width, msg->height);
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
  pcl::VoxelGrid<pcl::PointXYZRGB> vg;
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered (new pcl::PointCloud<pcl::PointXYZRGB>());
  vg.setInputCloud(msg);
  vg.setLeafSize(0.03f, 0.03f, 0.03f);
  vg.filter(*filtered);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
  std::vector<int> inl;
  pcl::SampleConsensusModelPlane<pcl::PointXYZRGB>::Ptr model_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZRGB> (filtered));
  pcl::RandomSampleConsensus<pcl::PointXYZRGB> ransac(model_p);
  ransac.setDistanceThreshold(0.01);
  ransac.computeModel();
  ransac.getInliers(inl);
  pcl::copyPointCloud<pcl::PointXYZRGB>(*filtered, inl, *cloud);
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  pcl::SACSegmentation<pcl::PointXYZRGB> seg;
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZRGB> ()), cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);
  seg.setOptimizeCoefficients(true);
  seg.setModelType(pcl::SACMODEL_PLANE);
  seg.setMethodType(pcl::SAC_RANSAC);
  //seg.setMaxIterations (100);
  seg.setDistanceThreshold(0.01);
  int i = 0, nr_points = (int)filtered->points.size();
  while(filtered->points.size() > 0.6*nr_points)
  {
    seg.setInputCloud(filtered);
    seg.segment(*inliers, *coefficients);
    pcl::ExtractIndices<pcl::PointXYZRGB> extract;
    extract.setInputCloud(filtered);
    extract.setIndices(inliers);
    extract.setNegative(false);
    extract.filter (*cloud_plane);
    //std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << nr_points << std::endl;
    extract.setNegative (true);
    extract.filter (*cloud_f);
    *filtered = *cloud_f;
  }
  // Creating the KdTree object for the search method of the extraction
  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
  tree->setInputCloud (filtered);
  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
  ec.setClusterTolerance (0.05); // 2cm
  ec.setMinClusterSize (50);
  ec.setMaxClusterSize (10000);
  ec.setSearchMethod (tree);
  ec.setInputCloud (filtered);
  ec.extract (cluster_indices);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
  int j = 0;
  std::vector<std::vector<float> > moi, xcor;
  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
  {
    j++;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr clusters (new pcl::PointCloud<pcl::PointXYZRGB>);
    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
    {
      clusters->points.push_back (filtered->points[*pit]); //*
      cloud_cluster->points.push_back (filtered->points[*pit]); //*
      //cloud_cluster->width = 0;
      //cloud_cluster->height = 0;
    }
    pcl::MomentOfInertiaEstimation <pcl::PointXYZRGB> feature_extractor;
    feature_extractor.setInputCloud (clusters);
    feature_extractor.compute ();
    std::vector <float> moment_of_inertia;
    std::vector <float> eccentricity;
    pcl::PointXYZRGB min_point_AABB;
    pcl::PointXYZRGB max_point_AABB;
    pcl::PointXYZRGB min_point_OBB;
    pcl::PointXYZRGB max_point_OBB;
    pcl::PointXYZRGB position_OBB;
    Eigen::Matrix3f rotational_matrix_OBB;
    float major_value, middle_value, minor_value;
    Eigen::Vector3f major_vector, middle_vector, minor_vector;
    Eigen::Vector3f mass_center;
    feature_extractor.getMomentOfInertia (moment_of_inertia);
    feature_extractor.getEccentricity (eccentricity);
    feature_extractor.getAABB (min_point_AABB, max_point_AABB);
    feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
    feature_extractor.getEigenValues (major_value, middle_value, minor_value);
    feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
    feature_extractor.getMassCenter (mass_center);
    //for(int i=j-1; i<=j; j++)
    //{
    std::vector<float> row1;
    for(int k=0;k<2;k++)
      row1.push_back(mass_center[k]);
    moi.push_back(row1);
    //}
     //for(int i=j-1; i<=j; j++)
    //{
    std::vector<float> row2;
    row2.push_back(min_point_AABB.x);
    row2.push_back(max_point_AABB.x);
    row2.push_back(min_point_AABB.z);
    row2.push_back(max_point_AABB.z);
    row2.push_back(min_point_AABB.y);
    row2.push_back(max_point_AABB.y);
    xcor.push_back(row2);
    //}
  }
  std::cout<<endl<<j<<endl;
  //for(int i=0;i<j;i++)
  //std::cout<<moi[i][0]<<" "<<moi[i][1]<<endl;
  ofstream outfile;
  outfile.open("kinect.txt");
  for(int i=0; i<j; i++)
      {
        if(xcor[i][2]<2.0 && xcor[i][5]-xcor[i][4]>0.15)
        {
          std::cout<<xcor[i][5]<<"  "<<xcor[i][4]<<" diff= "<<xcor[i][5]-xcor[i][4]<<endl;
          if(moi[i][0]>=0)
	  {
            std::cout<<"left\n";
	    outfile <<"left";
	    goto label;
	  }
          else //if(fabs(xcor[i][0])>fabs(xcor[i][1]))
	  {
            std::cout<<"right\n";
    	    outfile <<"right";
	    goto label;
	  }
        }
	//else
       }
	std::cout<<"straight\n";
      label:
	std::cout<<"";
	outfile.close();
  /*viewer = simpleVis(cloud_cluster, cloud);
  while(1)//!viewer->wasStopped()
  {
     viewer->spinOnce(100);
     boost::this_thread::sleep(boost::posix_time::microseconds(100000));
  }*/
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "planar");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("/camera/depth/points", 1, callback);
  ros::spin();
  return 0;
}


#include "mex.h"
#include <math.h>
#include <Eigen/Dense>

using namespace std;

void constraints(mxArray* c_out, mxArray* ceq_out, mxArray* dc_out, mxArray* dceq_out, const Eigen::MatrixXd& x, size_t nc, size_t nceq, size_t nv, size_t nsteps)
{
  Eigen::MatrixXd steps = x.block(0, 0, 6, nsteps);
  Eigen::MatrixXd rel_steps = x.block(6, 0, 6, nsteps);

  Eigen::MatrixXd c(nc, 1);
  c = Eigen::MatrixXd::Zero(nc, 1);
  Eigen::VectorXd ceq(nceq);
  ceq = Eigen::VectorXd::Zero(nceq);
  Eigen::MatrixXd dc(nv, nsteps);
  dc = Eigen::MatrixXd::Zero(nv, nc);
  Eigen::MatrixXd dceq(nv, nceq);
  dceq = Eigen::MatrixXd::Zero(nv, nceq);
  Eigen::Matrix2d R;
  Eigen::Vector2d dxy;
  Eigen::Vector2d proj;
  Eigen::Vector2d u;
  Eigen::Vector2d al;

  // ceq.segment(0,2) = steps.block(0,0,2,1) - rel_steps.block(0,0,2,1);
  // dceq.block(0,0,2,2) << 1, 0, 0, 1;
  // dceq.block(6,0,2,2) << -1, 0, 0, -1;

  int j;
  int x1_ndx;
  int dx_ndx;
  int x2_ndx;
  int con_ndx;
  int con_dndx;
  double dx, dy, si, co;

  for (j = 2; j <= nsteps; j++) {
    con_ndx = (j-1)*2;
    con_dndx = 2;
    si = sin(steps(5,j-2));
    co = cos(steps(5,j-2));
    R << co, -si, si, co;
    dxy = R * rel_steps.block(0,j-1,2,1);
    proj = steps.block(0,j-2,2,1) + dxy;
    ceq.segment(con_ndx, con_dndx) = steps.block(0,j-1,2,1) - proj;
    x1_ndx = (j-2)*12;
    dx_ndx = (j-1)*12+6;
    x2_ndx = (j-1)*12;
    dceq.block(x2_ndx,con_ndx,2,con_dndx) << 1, 0, 0, 1;
    dceq.block(x1_ndx,con_ndx,2,con_dndx) << -1, 0, 0, -1;
    dx = rel_steps(0,j-1);
    dy = rel_steps(1,j-1);
    dceq.block(x1_ndx+5,con_ndx,1,con_dndx) << dx*si + dy*co, -dx*co + dy*si;
    dceq.block(dx_ndx,con_ndx,1,con_dndx) << -co, -si;
    dceq.block(dx_ndx+1,con_ndx,1,con_dndx) << si, -co;
  }

  memcpy(mxGetPr(c_out), c.data(), sizeof(double)*c.rows()*c.cols());
  memcpy(mxGetPr(dc_out), dc.data(), sizeof(double)*dc.rows()*dc.cols());
  memcpy(mxGetPr(ceq_out), ceq.data(), sizeof(double)*ceq.rows()*ceq.cols());
  memcpy(mxGetPr(dceq_out), dceq.data(), sizeof(double)*dceq.rows()*dceq.cols());

  return;
}

void mexFunction( int nlhs, mxArray *plhs[],
                  int nrhs, const mxArray *prhs[] )
{
  // [c_mex, ceq_mex, dc_mex, dceq_mex] = stepCollocationConstraintsMex(x);

  size_t nv = mxGetM(prhs[0]);
  size_t nsteps = nv / 12;
  size_t nceq = 2 * nsteps;
  size_t nc = 0;

  Eigen::MatrixXd x = Eigen::Map<Eigen::MatrixXd>(mxGetPr(prhs[0]), 12, nsteps);

  /* Create matrices for the return arguments. */
  plhs[0] = mxCreateDoubleMatrix((mwSize)nc, (mwSize)1, mxREAL); // c
  plhs[1] = mxCreateDoubleMatrix((mwSize)nceq, (mwSize)1, mxREAL); // ceq
  plhs[2] = mxCreateDoubleMatrix((mwSize)nv, (mwSize)nc, mxREAL); // dc
  plhs[3] = mxCreateDoubleMatrix((mwSize)nv, (mwSize)nceq, mxREAL); // dceq

  /* Assign pointers to each output. */
  mxArray* c = plhs[0];
  mxArray* ceq = plhs[1];
  mxArray* dc = plhs[2];
  mxArray* dceq = plhs[3];

  constraints(c, ceq, dc, dceq, x, nc, nceq, nv, nsteps);
}


/*
Copyright (c) 2015 - 2016, Tianwei Shen
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of libvot nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

/** \file svt.cpp
 *	\brief singular value threshold (immature)
 */
#include <iostream>
#include <vector>
#include <sstream>
#include <Eigen/Dense>
#include <random>

#include "utils/io_utils.h"

using namespace std;
using namespace Eigen;

float Fnorm(Eigen::MatrixXf &f)
{
	float norm = 0;
	for (int i = 0; i < f.rows(); i++) {
		for (int j = 0; j < f.cols(); j++) {
			norm += f(i, j) * f(i, j);
		}
	}
	return sqrt(norm);
}

double Fnorm(Eigen::MatrixXd &d)
{
	double norm = 0;
	for (int i = 0; i < d.rows(); i++) {
		for (int j = 0; j < d.cols(); j++) {
			norm += d(i, j) * d(i, j);
		}
	}
	return sqrt(norm);
}

// sample_set.size() = d.rows();
Eigen::MatrixXd MatrixProjection(Eigen::MatrixXd &d, std::vector<std::vector<int> > sample_set)
{
	Eigen::MatrixXd pd = Eigen::MatrixXd::Constant(d.rows(), d.cols(), 0);
	for (int i = 0; i < d.rows(); i++) {
		for (int j = 0; j < sample_set[i].size(); j++) {
			pd(i, sample_set[i][j]) = d(i, sample_set[i][j]);
		}
	}
	return pd;
}

int main(int argc, char ** argv)
{
	if (argc != 2) {
		cout << "Usage: " << argv[0] << " <mat_file>\n";
		exit(-1);
	}
	const char *mat_file = argv[1];

	// here we assume that the input matrix is a square matrix
	vector<string> mat_string;
	tw::IO::ExtractLines(mat_file, mat_string);
	int mat_size = mat_string.size();
	MatrixXd input_mat = MatrixXd::Constant(mat_size, mat_size, 0);
	for (int i = 0; i < mat_size; i++) {
		stringstream ss;
		ss << mat_string[i];
		float temp;
		for (int j = 0; j < mat_size; j++) {
			ss >> temp;
			input_mat(i, j) = temp;
		}
	}

	MatrixXd sample_mat = MatrixXd::Constant(mat_size, mat_size, 0);
	const float sample_ratio = 0.4;
	const int sample_size = mat_size * sample_ratio;
	vector<vector<int> > sample_set;
	sample_set.resize(mat_size);

	// sample without replacement
	default_random_engine e(0);
	uniform_int_distribution<int> uni_rand(0, mat_size-1);
	for (int i = 0; i < mat_size; i++) {
		for (int j = 0; j < sample_size; j++) {
			int curr_sample = uni_rand(e);
			int k;
			for (k = 0; k < j; k++) {
				if (curr_sample == sample_set[i][k]) {
					j--; k = -1;
					break;
				}
			}
			if (k != -1) {
				sample_set[i].push_back(curr_sample);
			}
		}
	}

	for (int i = 0; i < mat_size; i++) {
		for (int j = 0; j < sample_set[i].size(); j++) {
			sample_mat(i, sample_set[i][j]) = input_mat(i, sample_set[i][j]);
		}
	}

	cout << sample_mat << endl;

	// singular value thresholding
	float step_size = 1.5;//1.2 * mat_size / sample_ratio;
	float tau = 5 * mat_size;
	int k0 = tau / (step_size * Fnorm(sample_mat)) + 1;
	int max_iter = 200;
	MatrixXd Y = k0 * step_size * sample_mat;
	MatrixXd X;
	double stop_threshold = 1e-4;
	for (int i = 0; i < max_iter; i++) {
		JacobiSVD<MatrixXd> svd(Y, ComputeThinU | ComputeThinV);
		MatrixXd U = svd.matrixU();
		MatrixXd V = svd.matrixV();
		VectorXd singular_vector = svd.singularValues();
		MatrixXd S = MatrixXd::Constant(mat_size, mat_size, 0);
		for (int i = 0; i < mat_size; i++) {
			if (singular_vector[i] > tau) {
				S(i, i) = singular_vector[i] - tau;
			}
		}
		X = U * S * V.transpose();

		// compute error
		MatrixXd residual_mat = X - sample_mat;
		MatrixXd residual_proj = MatrixProjection(residual_mat, sample_set);
		double error = Fnorm(residual_proj);
		double sample_mat_norm = Fnorm(sample_mat);
		if (error / sample_mat_norm < stop_threshold) {
			cout << "break at iter " << i << endl;
			break;
		}

		// refresh Y
		for (int i = 0; i < sample_mat.rows(); i++) {
			for (int j = 0; j < sample_set[i].size(); j++) {
				Y(i, sample_set[i][j]) = Y(i, sample_set[i][j]) + step_size * (sample_mat(i, sample_set[i][j]) - X(i, sample_set[i][j]));
			}
		}
	}

	// compute completion error
	float completion_error = 0;
	float max_error = 0;
	float error_bound = 10;
	int within_bound_count = 0;
	for (int i = 0; i < mat_size; i++) {
		for (int j = 0; j < mat_size; j++) {
			completion_error += (X(i, j) - input_mat(i, j)) * (X(i, j) - input_mat(i, j));
			if (max_error < abs(X(i, j) - input_mat(i, j)) ) {
				max_error = abs(X(i, j) - input_mat(i, j));
			}
			if (abs(X(i, j) - input_mat(i, j)) < error_bound) {
				within_bound_count++;
			}
		}
	}
	MatrixXd error_mat = X - input_mat;
	float relative_error = Fnorm(error_mat) / Fnorm(input_mat);
	cout << "completion_error " << completion_error << endl;
	cout << "max_error " << max_error << endl;
	cout << "within_bound_count " << within_bound_count << endl;
	cout << "relative_error " << relative_error << endl;

	return 0;
}


//
//  share_header.hpp
//  hybrid_fem_bie
//
//  Created by Max on 2/15/18.
//
//

#ifndef share_header_hpp
#define share_header_hpp

#include <stdio.h>
#include <Eigen/Eigen>

using namespace Eigen;

typedef Eigen::Matrix<int, -1, -1,Eigen::RowMajor> MatrixXi_rm;

#endif /* share_header_hpp */


/**
  *
  * @file ConvertStdVector.cpp
  *
  **/

#include <Eigen/Geometry>

#include "StdVector.hpp"

namespace Tools {
	namespace Converter {
		namespace StdVector {
			template <>
			Eigen::Matrix<int, Eigen::Dynamic, 1> to_vector(std::vector<int> &vector_stl) {
				return Eigen::Map<Eigen::Matrix<int, Eigen::Dynamic, 1>, Eigen::Unaligned>(vector_stl.data(), vector_stl.size()).matrix();
			}

			template <>
			Eigen::Matrix<float, Eigen::Dynamic, 1> to_vector(std::vector<float> &vector_stl) {
				return Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, 1>, Eigen::Unaligned>(vector_stl.data(), vector_stl.size()).matrix();
			}

			template <>
			Eigen::Matrix<double, Eigen::Dynamic, 1> to_vector(std::vector<double> &vector_stl) {
				return Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, 1>, Eigen::Unaligned>(vector_stl.data(), vector_stl.size()).matrix();
			}
		}
	}
}


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

  @copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ALGORITHM_COUNT_HPP_INCLUDED
#define BOOST_SIMD_ALGORITHM_COUNT_HPP_INCLUDED

#include <boost/simd/range/segmented_aligned_range.hpp>
#include <boost/simd/function/nbtrue.hpp>
#include <boost/simd/pack.hpp>
#include <algorithm>

namespace boost { namespace simd
{
  /*!
    @ingroup group-algo

    Returns the number of elements in the ContiguousRange [first,last) that compare equal to val.

    @param first  Beginning of the range of elements to count
    @param last   End of the range of elements to count
    @param val    Value to search for in the range.

    @par Requirement

      - @c first, @c last and @c out must be pointers to Vectorizable type.
      - @c val must be a scalar value convertible to the pointee type of first.

    @par Example:

      @snippet count.cpp count

    @par Possible output:

      @snippet count.txt count

  **/
  template<typename T, typename U>
  typename std::iterator_traits<const T*>::difference_type
  count(T const* first, T const* last, U const & val)
  {
    pack<T> pval(val);
    auto pr = segmented_aligned_range(first,last);

    auto c = std::count(pr.head.begin(), pr.head.end(), val);
    for(pack<T> x : pr.body)   c += nbtrue(x == pval);
    c += std::count(pr.tail.begin(), pr.tail.end(), val);

    return c;
  }
} }

#endif


/**
 * @file coretype.hpp
 * @brief Definition of common types.
 *
 * @author Maxim <john.jasper.doe@gmail.com>
 * @date 2020
 */

#ifndef CORE_CORETYPE_HPP_
#define CORE_CORETYPE_HPP_

#include <dlib/clustering.h>
#include <vector>

namespace coretypes {
/** @brief The namespace of the "Core" */
namespace core {

// Here we declare that our samples will be 2 dimensional column vectors.
// (Note that if you don't know the dimensionality of your vectors at compile time
// you can change the 2 to a 0 and then set the size at runtime)
const size_t COLUMNS = 8;
using sample_t = dlib::matrix<double, COLUMNS - 1, 1>;

/** @brief Input data type. */
using datas_t = std::vector<sample_t>;

// Now we are making a typedef for the kind of kernel we want to use.  I picked the
// radial basis kernel because it only has one parameter and generally gives good
// results without much fiddling.
using kernel_type = dlib::radial_basis_kernel<sample_t>;

using column_ix_t = size_t;
using row_ix_t = size_t;

/** @brief The Output data struct. */
struct clust_data {
  double x{.0};
  double y{.0};
  std::size_t clust{0};
};
using clust_datas_t = std::vector<core::clust_data>;

} /* coretypes:: */
} /* core:: */

#endif /* CORE_CORETYPE_HPP_ */


//=======================================================================
// Copyright (c) 2013 Piotr Wygocki
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file prune_restrictions_to_tree.hpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2013-07-05
 */
#ifndef PAAL_PRUNE_RESTRICTIONS_TO_TREE_HPP
#define PAAL_PRUNE_RESTRICTIONS_TO_TREE_HPP

#include "paal/utils/functors.hpp"
#include "paal/utils/irange.hpp"

#include <boost/function_output_iterator.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>

namespace paal {

using RestrictionsVector = std::vector<std::pair<unsigned, unsigned>>;

/**
 * @brief Returns a list of restrictions, made of the edges of a maximum
* spanning tree
 * in a clique with edge weights equal to restriction values between the edges.
 *
 * @tparam Restrictions
 * @param res restrictions
 * @param N number of vertices
 *
 * @return A minimum set of restrictions needed to be checked by the oracle.
 */
template <typename Restrictions>
RestrictionsVector prune_restrictions_to_tree(Restrictions res, int N) {
    using Dist = decltype(std::declval<Restrictions>()(0, 0));
    using EdgeProp = boost::property<boost::edge_weight_t, Dist>;
    using TGraph =
        boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
                              boost::no_property, EdgeProp>;
    using Edge = typename boost::graph_traits<TGraph>::edge_descriptor;

    RestrictionsVector res_vec;
    TGraph g(N);
    for (auto i : irange(N)) {
        for (auto j : irange(i + 1, N)) {
            add_edge(i, j, EdgeProp(-std::max(res(i, j), res(j, i))), g);
        }
    }

    auto add_edge_to_graph = [&](Edge e) {
        res_vec.push_back(std::make_pair(source(e, g), target(e, g)));
    };

    boost::kruskal_minimum_spanning_tree(
        g, boost::make_function_output_iterator(
               utils::make_assignable_functor(add_edge_to_graph)));
    return res_vec;
}
} //! paal

#endif // PAAL_PRUNE_RESTRICTIONS_TO_TREE_HPP


// (C) Copyright David James Pearce 2003. Permission to copy, use,
// modify, sell and distribute this software is granted provided this
// copyright notice appears in all copies. This software is provided
// "as is" without express or implied warranty, and with no claim as
// to its suitability for any purpose.
//
// Email: david.pearce@mcs.vuw.ac.nz
//
// Notes
// -----
// This algorithm corresponds to that used in the following works:
//
// [1] David J. Pearce and Paul H. J. Kelly. A dynamic algorithm for
//     topologically sorting directed acyclic graphs, In Proceedings
//     of the Workshop on Efficient and experimental Algorithms (WEA04).
//
// [2] David J. Pearce and Paul H. J. Kelly. A dynamic topological sort
//     algorithm for directed acyclic graphs, submitted to the journal
//     of Experimental Algorithmics (JEA), 2005.
//
// [3] David J. Pearce. Some directed graph algorithms and their
//     application to pointer analysis. Ph.D. Thesis, Imperial College
///    of Science, Technology and Medicine, University of London,
//     February 2005.

#ifndef POTO1_ONLINE_TOPOLOGICAL_ORDER_HPP
#define POTO1_ONLINE_TOPOLOGICAL_ORDER_HPP

#include <stdexcept>
#include <boost/property_map.hpp>
#include <boost/graph/topological_sort.hpp>
#include "oto_tags.hpp"

// these globals are not strictly needed
// they are used to generate the metrics
#ifdef POTO1_GENERATE_STATS
extern unsigned int poto1_dxy;
extern unsigned int poto1_ddxy;
extern unsigned int poto1_ninvalid;
#endif

template<class T, class N2I>
class poto1_online_topological_order;

template<class N2I>
class poto1_oto_comp {
private:
  N2I _n2i;
public:
  poto1_oto_comp(N2I p)
    : _n2i(p) {
  }

  bool operator()(unsigned int x, unsigned int y) const {
    return _n2i[x] < _n2i[y];
  }
};

template<class T, class N2I>
bool poto1_oto_fwd_dfs(typename T::vertex_descriptor n,
		       typename T::vertex_descriptor ub,
		       std::vector<unsigned int> &reachable,
		       typename boost::property_map<T, N2I>::type n2i,
		       poto1_online_topological_order<T,N2I> &g) {
  typedef typename T::out_edge_iterator out_iterator;

  reachable.push_back(n);
  g._visited[n]=true;
#ifdef POTO1_GENERATE_STATS
  ++poto1_ddxy;
#endif
  out_iterator i,iend;
  for(tie(i,iend) = out_edges(n,g); i!=iend;++i) {
    unsigned int w(target(*i,g));
    unsigned int wn2i(n2i[w]);
    if(wn2i == ub) {
      // this is the special case
      // where a cycle has been detected
      return true;
    } else if(wn2i < ub && !g._visited[w]) {
      poto1_oto_fwd_dfs(w,ub,reachable,n2i,g);
    }
#ifdef POTO1_GENERATE_STATS
    ++poto1_ddxy;
#endif
  }
  return false;
}

template<class T, class N2I>
void poto1_oto_back_dfs(typename T::vertex_descriptor n,
			typename T::vertex_descriptor lb,
			std::vector<unsigned int> &reaching,
			typename boost::property_map<T, N2I>::type n2i,
			poto1_online_topological_order<T,N2I> &g) {

  typedef typename T::in_edge_iterator in_iterator;

  reaching.push_back(n);
  g._visited[n]=true;
#ifdef POTO1_GENERATE_STATS
  ++poto1_ddxy;
#endif
  in_iterator i,iend;
  for(tie(i,iend) = in_edges(n,g); i!=iend;++i) {
    unsigned int w(source(*i,g));
    unsigned int wn2i(n2i[w]);
    if(wn2i > lb && !g._visited[w]) {
      poto1_oto_back_dfs(w,lb,reaching,n2i,g);
    }
#ifdef POTO1_GENERATE_STATS
    ++poto1_ddxy;
#endif
  }
}

template<class T, class N2I>
void poto1_oto_reorder(std::vector<unsigned int> &reachable,
		   std::vector<unsigned int> &reaching,
		   typename boost::property_map<T, N2I>::type n2i,
		   poto1_online_topological_order<T,N2I> &g) {
  std::vector<typename T::vertex_descriptor> tmp;
  tmp.reserve(reaching.size() + reachable.size());
  std::vector<unsigned int>::iterator iend(reaching.end());
  for(std::vector<unsigned int>::iterator i(reaching.begin());
      i!=iend;++i) {
    tmp.push_back(*i);
    g._visited[*i]=false;
    *i = n2i[*i]; // dirty trick
  }
  iend=reachable.end();
  for(std::vector<unsigned int>::iterator i(reachable.begin());
      i!=iend;++i) {
    tmp.push_back(*i);
    g._visited[*i]=false;
    *i = n2i[*i]; // dirty trick
  }
  std::vector<unsigned int>::iterator i(reachable.begin());
  std::vector<unsigned int>::iterator j(reaching.begin());
  std::vector<unsigned int>::iterator jend(reaching.end());
  unsigned int index(0);
  while(i != iend || j != jend) {
    unsigned int w;
    if(j == jend || (i != iend && *i < *j)) {
      w=*i;++i;
    } else {
      w=*j;++j;
    }
    typename T::vertex_descriptor n(tmp[index++]);
    // allocate n at w
    n2i[n]=w;
  }
}

template<class T, class N2I>
std::pair<typename T::edge_descriptor, bool>
add_edge(typename poto1_online_topological_order<T,N2I>::vertex_descriptor t,
	 typename poto1_online_topological_order<T,N2I>::vertex_descriptor h,
	 poto1_online_topological_order<T,N2I> &g) {
  std::pair<typename T::edge_descriptor, bool> r;
  r = add_edge(static_cast<typename T::vertex_descriptor>(t),
	       static_cast<typename T::vertex_descriptor>(h),
	       static_cast<T&>(g));

  typedef typename boost::property_map<T, N2I>::type N2iMap;
  N2iMap n2i = get(N2I(),g);

  unsigned int hn2i(n2i[h]);
  unsigned int tn2i(n2i[t]);

  if(r.second && hn2i < tn2i) {
    // need to reorder
    std::vector<unsigned int> reaching, reachable;
    if(poto1_oto_fwd_dfs<T,N2I>(h,tn2i,reachable,n2i,g)) {
      throw std::runtime_error("loop detected");
    } else {
      poto1_oto_back_dfs<T,N2I>(t,hn2i,reaching,n2i,g);
      poto1_oto_comp<N2iMap> pc(n2i);
      sort(reaching.begin(),reaching.end(),pc);
      sort(reachable.begin(),reachable.end(),pc);
      poto1_oto_reorder(reachable,reaching,n2i,g);
#ifdef POTO1_GENERATE_STATS
      ++poto1_ninvalid;
      poto1_dxy += reaching.size() + reachable.size();
#endif
    }
  }
}

template<class InputIter, class T, class N2I>
void add_edges(InputIter b, InputIter e,
	       poto1_online_topological_order<T,N2I> &g) {
  for(;b!=e;++b) {
    add_edge(b->first,b->second,g);
  }
}

template<class T, class N2I = n2i_t>
class poto1_online_topological_order : public T {
public:
  typedef poto1_online_topological_order<T> self;
  friend std::pair<typename T::edge_descriptor, bool> add_edge<T>(typename self::vertex_descriptor,
								  typename self::vertex_descriptor,
								  self &);
  friend void poto1_oto_reorder<T,N2I>(std::vector<unsigned int> &,
					  std::vector<unsigned int> &,
					  typename boost::property_map<T, N2I>::type n2i,
					  poto1_online_topological_order<T,N2I> &g);

  friend bool poto1_oto_fwd_dfs<T,N2I>(typename self::vertex_descriptor n,
				       typename self::vertex_descriptor ub,
				       std::vector<unsigned int> &reachable,
				       typename boost::property_map<T, N2I>::type n2i,
				       self &g);

  friend void poto1_oto_back_dfs<T,N2I>(typename self::vertex_descriptor n,
					typename self::vertex_descriptor lb,
					std::vector<unsigned int> &reaching,
					typename boost::property_map<T, N2I>::type n2i,
					self &g);
private:
  std::vector<bool> _visited; // useful and not too expensive.
public:
  poto1_online_topological_order(T const &g, unsigned int acc = 1)
    : T(g), _visited(num_vertices(g),false) {

    // use the topological sort function to
    // order the vertices correctly ...
    std::vector<unsigned int> tmp;
    tmp.reserve(num_vertices(g));
    topological_sort(g,std::back_inserter(tmp));
    // finally, setup the n2i map correctly
    typedef typename boost::property_map<T, N2I>::type N2iMap;
    N2iMap n2i = get(N2I(),*this);

    unsigned int index(0);
    for(std::vector<unsigned int>::reverse_iterator i(tmp.rbegin());i!=tmp.rend();
	++i,++index) {
      n2i[*i]=index;
    }
  }

  poto1_online_topological_order(typename T::vertices_size_type n,
			       unsigned int acc = 1)
    : T(n), _visited(n, false) {
    typedef typename boost::property_map<T, N2I>::type N2iMap;
    N2iMap n2i = get(N2I(),*this);

    unsigned int counter(0);
    typename T::vertex_iterator i,iend;
    for(tie(i,iend) = vertices(*this);i!=iend;++i) {
      n2i[*i]=counter++;
    }
  }
};

#endif


/*
 * Simulation.cpp
 *
 *  Created on: May 30, 2017
 *      Author: schutzekatze
 */

#include "Simulation.h"

#include "history/History.h"
using simulation::history::History;

#include "State.h"

#include "src/physics/Object.h"
#include <src/simulation/ObjectManager.h>
using physics::Object;

#include <thread>
using std::this_thread::sleep_for;

#include <Eigen/Dense>
using Eigen::Vector3d;

#include "src/physics/Collision.h"
using physics::Collision;

#include <cstdlib>
using std::rand;

#include <cmath>
using std::cos;
using std::sin;

namespace simulation {

thread *Simulation::main_thread(nullptr);
atomic<bool> Simulation::stop_flag(false);

void Simulation::start() {
	State::initialize();
	History::initialize();

	main_thread = new thread(simulation_main);
}

void Simulation::stop() {
	stop_flag = true;
	main_thread->join();
	delete main_thread;

	History::finalize();
}

void Simulation::simulation_main() {
	Vector3d origin, velocity;
	double phi, theta;
	const double pi = 3.14159265359, distance = 30000, velocity_magnitude = 30000;

	while (!stop_flag) {
		phi = 2 * ((double)rand()) / RAND_MAX * pi;
		theta = 2 * ((double)rand()) / RAND_MAX * pi;

		origin = Vector3d(
				sin(theta) * cos(phi) * distance,
				sin(theta) * sin(phi) * distance,
				cos(theta) * distance
				);

		velocity = Vector3d(Vector3d(0, 0, 0) - origin);
		velocity /= velocity.norm();
		velocity *= velocity_magnitude;

		State::acquire();

		Object* object = ObjectManager::create(
				origin,
				Object::DEFAULT_RADIUS,
				velocity);

		Vector3d collision_point =
				Collision::find_collision_point(State::blob_objects, *State::new_object);

		while (!stop_flag) {
			if ((collision_point - object->position).norm() <= object->velocity.norm()) {
				ObjectManager::move(object, collision_point);
				State::blob_objects.push_back(object);
				State::new_object = nullptr;
				break;
			}
			else {
				ObjectManager::apply_velocity(object);
			}
		}

		State::release();
	}
}

} /* namespace simulation */