/* Author: Wolfgang Bangerth, University of Heidelberg, 1999 */

/*    $Id: step-2.cc 27657 2012-11-21 13:19:08Z bangerth $       */
/*                                                                */
/*    Copyright (C) 1999-2003, 2006, 2008-2012 by the deal.II authors */
/*                                                                */
/*    This file is subject to QPL and may not be  distributed     */
/*    without copyright and license information. Please refer     */
/*    to the file deal.II/doc/license.html for the  text  and     */
/*    further information on this license.                        */

// The first few includes are just like in the previous program, so do not
// require additional comments:
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_boundary_lib.h>

// However, the next file is new. We need this include file for the
// association of degrees of freedom ("DoF"s) to vertices, lines, and cells:
#include <deal.II/dofs/dof_handler.h>

// The following include contains the description of the bilinear finite
// element, including the facts that it has one degree of freedom on each
// vertex of the triangulation, but none on faces and none in the interior of
// the cells.
//
// (In fact, the file contains the description of Lagrange elements in
// general, i.e. also the quadratic, cubic, etc versions, and not only for 2d
// but also 1d and 3d.)
#include <deal.II/fe/fe_q.h>
// In the following file, several tools for manipulating degrees of freedom
// can be found:
#include <deal.II/dofs/dof_tools.h>
// We will use a sparse matrix to visualize the pattern of nonzero entries
// resulting from the distribution of degrees of freedom on the grid. That
// class can be found here:
#include <deal.II/lac/sparse_matrix.h>
// We will also need to use an intermediate sparsity patter structure, which
// is found in this file:
#include <deal.II/lac/compressed_sparsity_pattern.h>

// We will want to use a special algorithm to renumber degrees of freedom. It
// is declared here:
#include <deal.II/dofs/dof_renumbering.h>

// And this is again needed for C++ output:
#include <fstream>

// Finally, as in step-1, we import the deal.II namespace into the global
// scope:
using namespace dealii;

// @sect3{Mesh generation}

// This is the function that produced the circular grid in the previous step-1
// example program. The sole difference is that it returns the grid it
// produces via its argument.
//
// The details of what the function does are explained in step-1. The only
// thing we would like to comment on is this:
//
// Since we want to export the triangulation through this function's
// parameter, we need to make sure that the boundary object lives at least as
// long as the triangulation does. However, in step-1, the boundary object is
// a local variable, and it would be deleted at the end of the function, which
// is too early. We avoid the problem by declaring it 'static' which makes
// sure that the object is initialized the first time control the program
// passes this point, but at the same time assures that it lives until the end
// of the program.
void make_grid (Triangulation<2> &triangulation)
{
  const Point<2> center (1,0);
  const double inner_radius = 0.5,
               outer_radius = 1.0;
  GridGenerator::hyper_shell (triangulation,
                              center, inner_radius, outer_radius,
                              10);

  static const HyperShellBoundary<2> boundary_description(center);
  triangulation.set_boundary (0, boundary_description);

  for (unsigned int step=0; step<5; ++step)
    {
      Triangulation<2>::active_cell_iterator
      cell = triangulation.begin_active(),
      endc = triangulation.end();

      for (; cell!=endc; ++cell)
        for (unsigned int v=0;
             v < GeometryInfo<2>::vertices_per_cell;
             ++v)
          {
            const double distance_from_center
              = center.distance (cell->vertex(v));

            if (std::fabs(distance_from_center - inner_radius) < 1e-10)
              {
                cell->set_refine_flag ();
                break;
              }
          }

      triangulation.execute_coarsening_and_refinement ();
    }
}

// @sect3{Creation of a DoFHandler}

// Up to now, we only have a grid, i.e. some geometrical (the position of the
// vertices) and some topological information (how vertices are connected to
// lines, and lines to cells, as well as which cells neighbor which other
// cells). To use numerical algorithms, one needs some logic information in
// addition to that: we would like to associate degree of freedom numbers to
// each vertex (or line, or cell, in case we were using higher order elements)
// to later generate matrices and vectors which describe a finite element
// field on the triangulation.
//
// This function shows how to do this. The object to consider is the
// DoFHandler class template.  Before we do so, however, we first
// need something that describes how many degrees of freedom are to be
// associated to each of these objects. Since this is one aspect of the
// definition of a finite element space, the finite element base class stores
// this information. In the present context, we therefore create an object of
// the derived class FE_Q that describes Lagrange elements. Its
// constructor takes one argument that states the polynomial degree of the
// element, which here is one (indicating a bi-linear element); this then
// corresponds to one degree of freedom for each vertex, while there are none
// on lines and inside the quadrilateral. A value of, say, three given to the
// constructor would instead give us a bi-cubic element with one degree of
// freedom per vertex, two per line, and four inside the cell. In general,
// FE_Q denotes the family of continuous elements with complete
// polynomials (i.e. tensor-product polynomials) up to the specified order.
//
// We first need to create an object of this class and then pass it on to the
// DoFHandler object to allocate storage for the degrees of
// freedom (in deal.II lingo: we distribute degrees of
// freedom). Note that the DoFHandler object will store a reference to
// this finite element object, so we have to make sure its lifetime is at
// least as long as that of the DoFHandler; one way to make sure
// this is so is to make it static as well, in order to prevent its preemptive
// destruction. (However, the library would warn us if we forgot about this
// and abort the program if that occured. You can check this, if you want, by
// removing the 'static' declaration.)
void distribute_dofs (DoFHandler<2> &dof_handler)
{
  // As described above, let us first create a finite element object, and then
  // use it to allocate degrees of freedom on the triangulation with which the
  // dof_handler object is associated:
  static const FE_Q<2> finite_element(1);
  dof_handler.distribute_dofs (finite_element);

  // Now that we have associated a degree of freedom with a global number to
  // each vertex, we wonder how to visualize this?  There is no simple way to
  // directly visualize the DoF number associated with each vertex. However,
  // such information would hardly ever be truly important, since the
  // numbering itself is more or less arbitrary. There are more important
  // factors, of which we will demonstrate one in the following.
  //
  // Associated with each vertex of the triangulation is a shape
  // function. Assume we want to solve something like Laplace's equation, then
  // the different matrix entries will be the integrals over the gradient of
  // each pair of such shape functions. Obviously, since the shape functions
  // are nonzero only on the cells adjacent to the vertex they are associated
  // with, matrix entries will be nonzero only if the supports of the shape
  // functions associated to that column and row %numbers intersect. This is
  // only the case for adjacent shape functions, and therefore only for
  // adjacent vertices. Now, since the vertices are numbered more or less
  // randomly by the above function (DoFHandler::distribute_dofs), the pattern
  // of nonzero entries in the matrix will be somewhat ragged, and we will
  // take a look at it now.
  //
  // First we have to create a structure which we use to store the places of
  // nonzero elements. This can then later be used by one or more sparse
  // matrix objects that store the values of the entries in the locations
  // stored by this sparsity pattern. The class that stores the locations is
  // the SparsityPattern class. As it turns out, however, this class has some
  // drawbacks when we try to fill it right away: its data structures are set
  // up in such a way that we need to have an estimate for the maximal number
  // of entries we may wish to have in each row. In two space dimensions,
  // reasonable values for this estimate are available through the
  // DoFHandler::max_couplings_between_dofs() function, but in three
  // dimensions the function almost always severely overestimates the true
  // number, leading to a lot of wasted memory, sometimes too much for the
  // machine used, even if the unused memory can be released immediately after
  // computing the sparsity pattern. In order to avoid this, we use an
  // intermediate object of type CompressedSparsityPattern that uses a
  // different %internal data structure and that we can later copy into the
  // SparsityPattern object without much overhead. (Some more information on
  // these data structures can be found in the @ref Sparsity module.) In order
  // to initialize this intermediate data structure, we have to give it the
  // size of the matrix, which in our case will be square with as many rows
  // and columns as there are degrees of freedom on the grid:
  CompressedSparsityPattern compressed_sparsity_pattern(dof_handler.n_dofs(),
                                                        dof_handler.n_dofs());

  // We then fill this object with the places where nonzero elements will be
  // located given the present numbering of degrees of freedom:
  DoFTools::make_sparsity_pattern (dof_handler, compressed_sparsity_pattern);

  // Now we are ready to create the actual sparsity pattern that we could
  // later use for our matrix. It will just contain the data already assembled
  // in the CompressedSparsityPattern.
  SparsityPattern sparsity_pattern;
  sparsity_pattern.copy_from (compressed_sparsity_pattern);

  // With this, we can now write the results to a file:
  std::ofstream out ("sparsity_pattern.1");
  sparsity_pattern.print_gnuplot (out);
  // The result is in GNUPLOT format, where in each line of the output file,
  // the coordinates of one nonzero entry are listed. The output will be shown
  // below.
  //
  // If you look at it, you will note that the sparsity pattern is
  // symmetric. This should not come as a surprise, since we have not given
  // the DoFTools::make_sparsity_pattern any information that
  // would indicate that our bilinear form may couple shape functions in a
  // non-symmetric way. You will also note that it has several distinct
  // region, which stem from the fact that the numbering starts from the
  // coarsest cells and moves on to the finer ones; since they are all
  // distributed symmetrically around the origin, this shows up again in the
  // sparsity pattern.
}

// @sect3{Renumbering of DoFs}

// In the sparsity pattern produced above, the nonzero entries extended quite
// far off from the diagonal. For some algorithms, for example for incomplete
// LU decompositions or Gauss-Seidel preconditioners, this is unfavorable, and
// we will show a simple way how to improve this situation.
//
// Remember that for an entry $(i,j)$ in the matrix to be nonzero, the
// supports of the shape functions i and j needed to intersect (otherwise in
// the integral, the integrand would be zero everywhere since either the one
// or the other shape function is zero at some point). However, the supports
// of shape functions intersected only if they were adjacent to each other, so
// in order to have the nonzero entries clustered around the diagonal (where
// $i$ equals $j$), we would like to have adjacent shape functions to be
// numbered with indices (DoF numbers) that differ not too much.
//
// This can be accomplished by a simple front marching algorithm, where one
// starts at a given vertex and gives it the index zero. Then, its neighbors
// are numbered successively, making their indices close to the original
// one. Then, their neighbors, if not yet numbered, are numbered, and so on.
//
// One algorithm that adds a little bit of sophistication along these lines is
// the one by Cuthill and McKee. We will use it in the following function to
// renumber the degrees of freedom such that the resulting sparsity pattern is
// more localized around the diagonal. The only interesting part of the
// function is the first call to DoFRenumbering::Cuthill_McKee,
// the rest is essentially as before:
void renumber_dofs (DoFHandler<2> &dof_handler)
{
  DoFRenumbering::Cuthill_McKee (dof_handler);

  CompressedSparsityPattern compressed_sparsity_pattern(dof_handler.n_dofs(),
                                                        dof_handler.n_dofs());
  DoFTools::make_sparsity_pattern (dof_handler, compressed_sparsity_pattern);

  SparsityPattern sparsity_pattern;
  sparsity_pattern.copy_from (compressed_sparsity_pattern);

  std::ofstream out ("sparsity_pattern.2");
  sparsity_pattern.print_gnuplot (out);
}

// Again, the output is shown below. Note that the nonzero entries are
// clustered far better around the diagonal than before. This effect is even
// more distinguished for larger matrices (the present one has 1260 rows and
// columns, but large matrices often have several 100,000s).

// It is worth noting that the DoFRenumbering class offers a
// number of other algorithms as well to renumber degrees of freedom. For
// example, it would of course be ideal if all couplings were in the lower or
// upper triangular part of a matrix, since then solving the linear system
// would among to only forward or backward substitution. This is of course
// unachievable for symmetric sparsity patterns, but in some special
// situations involving transport equations, this is possible by enumerating
// degrees of freedom from the inflow boundary along streamlines to the
// outflow boundary. Not surprisingly, DoFRenumbering also has
// algorithms for this.

// @sect3{The main function}

// Finally, this is the main program. The only thing it does is to allocate
// and create the triangulation, then create a DoFHandler object
// and associate it to the triangulation, and finally call above two functions
// on it:
int main ()
{
  Triangulation<2> triangulation;
  make_grid (triangulation);

  DoFHandler<2> dof_handler (triangulation);

  distribute_dofs (dof_handler);
  renumber_dofs (dof_handler);
}


#include <iostream>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/moment.hpp>

/***********extract **********/
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/max.hpp>

using namespace boost::accumulators;

int main()
{
	///
	accumulator_set<double, stats<tag::mean, tag::moment<2> > > acc;

	//push in some data

	acc(1.2);
	acc(2.3);acc(3.4);
	acc(4.5);acc(5.6);

	std::cout << "Mean: " << mean(acc) << std::endl;
	//we do need't accumulator field descriptor for ----accumulators::moment or
	/// boost namespace
	std::cout << "Moment: " << moment<2>(acc) << std::endl;

	/*********extract************/
	accumulator_set<int, features<tag::min, tag::max> > iacc;
	iacc(2);iacc(-1);iacc(1);
	std::cout << "min: " << min(iacc) << ", max: " << max(iacc) << std::endl;

	//extract_result be more convenient
	std::cout << "extract result min: " << extract_result<tag::min>(iacc) \
	<< ", extract result max: " << extract_result<tag::max>(iacc) << std::endl;

	//more interesting that we can define extractor

	extractor<tag::min> _min;
	extractor<tag::max> _max;
	//now call defined
	std::cout << "defined. min: " << _min(iacc) << ", max: " << _max(iacc) << std::endl;

	return 0;
}


///
/// @file maximumClique.hpp
/// @author Jxtopher
/// @version 0.1
/// @date 2020-02-16
/// @brief Problème de la clique maximale
///        see : https://fr.wikipedia.org/wiki/Problème_de_la_clique
///              http://www.csplib.org/Problems/prob074/
/// compilation g++ -Os maximumClique.cpp
///
///

#include <cstdlib>
#include <iostream>
#include <string>
#include <istream>
#include <sstream>
#include <fstream>      // std::filebuf

#include <boost/graph/graph_utility.hpp> // print_graph
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graphviz.hpp>

using namespace std;
using namespace boost;

//-----------------------------------------------------------------------------
// Définitions des types pour le graphe
//-----------------------------------------------------------------------------

///
/// @brief Définition du type pour les noeuds graphe
///
struct VertexProperties {
    float d;
    int predecessor;
    VertexProperties() : d(std::numeric_limits<float>::infinity()), predecessor(-1) {}
    VertexProperties(float d, int predecessor) : d(d), predecessor(-1) {}
};

///
/// @brief Définition du type pour les liens graphe
///
struct EdgeProperties {
    int weight;
    EdgeProperties() : weight(0) { }
    EdgeProperties(int weight) : weight(weight) { }
};

struct EdgeInfoPropertyTag {
    typedef edge_property_tag kind;
    static std::size_t const num; // ???
};

std::size_t const EdgeInfoPropertyTag::num = (std::size_t)&EdgeInfoPropertyTag::num;
typedef property<EdgeInfoPropertyTag, EdgeProperties> edge_info_prop_type;

///
/// @brief Type de graphe
///
typedef adjacency_list<
    boost::vecS, boost::vecS, boost::undirectedS,
    VertexProperties,                               // Type vertex
    edge_info_prop_type                             // Type edge
> Graph;

//-----------------------------------------------------------------------------
//
//-----------------------------------------------------------------------------

///
/// @brief source https://stackoverflow.com/questions/30415388/how-to-read-dimacs-vertex-coloring-graphs-in-c
///
/// @param dimacs
/// @param g
/// @return true
/// @return false
///
bool read_dimacs(std::istream& dimacs, Graph& g) {
    size_t vertices = 0, edges = 0;

    std::string line;
    while (getline(dimacs, line))
    {
        std::istringstream iss(line);
        char ch;
        if (iss >> ch)
        {
            size_t from, to;
            std::string format;

            switch(ch) {
                case 'c': break;
                case 'p':
                    if (vertices||edges) return false;
                    if (iss >> format >> vertices >> edges) {
                        if ("edge" != format && "col" != format ) return false;
                    }
                    break;
                case 'e':
                    if (edges-- && (iss >> from >> to) && (add_edge(from-1, to-1, g).second))
                        break;
                default:
                    return false;
            }
        }
    }
    return !(edges || !dimacs.eof());
}

// // Algorithme naif
// bool is_clique(const Graph& g, const unsigned int num_main_node) {
//     // Returns the number of edges leaving vertex u.
//     // unsigned int number_of_edges = boost::out_degree(num_main_node, g);

//     // Pas terrible
//     vector<unsigned int> list;
//     auto main_neighbours = boost::adjacent_vertices(num_main_node, g);
//     for (auto v : make_iterator_range(main_neighbours))
//         list.push_back(v);
//     list.push_back(num_main_node);

//     std::cout<<num_main_node<<" "<<list.size()<<std::endl;

//     //
//     for (unsigned int i = 0 ; i < list.size() ; i++) {
//         for (unsigned int j = i+1 ; j < list.size() ; j++) {
//             // check edge beteween list[i] and list[j]
//             auto neighbours = boost::adjacent_vertices(list[i], g);
//             auto x = make_iterator_range(neighbours);
//             auto p = std::find (x.begin(), x.end(), list[j]);
//             if (p == x.end()) {// Element not found
//                 return false;
//             }
//         }
//     }
//     return true;
// }

// void find_clique(const Graph& g) {
//     unsigned int degree_vertex =  0;
//     unsigned int num_vertex = -1;
//     for (unsigned int i = 0 ; i < boost::num_vertices(g) ; i++) {
//         if (is_clique(g, i) && degree_vertex < out_degree(i, g)) {
//             degree_vertex = out_degree(i, g);
//             num_vertex = i;
//         }
//     }

//     if (degree_vertex != 0) {
//         std::cout<<"Noeud principale : "<<num_vertex<<std::endl;
//         std::cout<<"Taille de la clique : "<<degree_vertex + 1<<std::endl;
//     } else {
//         cout<<"Not found clique"<<endl;
//     }

// }

bool is_clique(const Graph& g, const vector<unsigned int> &list_of_nodes) {
    for (unsigned int i = 0 ; i < list_of_nodes.size() ; i++) {
        for (unsigned int j = i+1 ; j < list_of_nodes.size() ; j++) {
            // check edge beteween list[i] and list[j]
            auto neighbours = boost::adjacent_vertices(list_of_nodes[i], g);
            auto x = make_iterator_range(neighbours);
            auto p = std::find (x.begin(), x.end(), list_of_nodes[j]);
            if (p == x.end()) {// Element not found
                return false;
            }
        }
    }
    return true;
}

class CombinationGenerator {
  public:
	void operator()(const unsigned int nbDigit, unsigned int len_string, void (*f)(unsigned int, unsigned int *, unsigned int)) {
		unsigned int nbCall = 0;
		unsigned int *string = new unsigned int[len_string];

		for (unsigned int i = 0; i < len_string; i++)
			string[i] = 0;

		bool x = false;
		unsigned int i = 0;

		f(nbCall++, string, len_string);

		while (string[i] == (nbDigit - 1)) {
			i++;
			x = true;
		}

		while (i < (len_string)) {
			string[i]++;

			if (x) {
				for (unsigned int j = 0; j < i; j++)
					string[j] = 0;
				i = 0;
			}

			f(nbCall++, string, len_string);

			while (string[i] == (nbDigit - 1)) {
				i++;
				x = true;
			}
		}

		delete[] string;
	}

	static void f(unsigned int nbCall, unsigned int *string, unsigned int size) {
		std::cout << nbCall << " ";
		for (unsigned int i = 0; i < size; i++)
			std::cout << string[i] << " ";
		std::cout << std::endl;
	}
};

void find_clique(const Graph& g) {
    unsigned int degree_vertex =  0;
    unsigned int num_vertex = -1;

    vector<unsigned int> clique;
    vector<unsigned int> list_of_nodes;

    for (unsigned int i = 0 ; i < boost::num_vertices(g) ; i++) {
        clique.push_back(i);
    }
}

void sec(std::vector<unsigned int> s) {
    for (unsigned int i = 0 ; i < s.size() ; i++) {
        for (unsigned int j = i+1 ; j < s.size() ; j++) {

        }
    }
}

int main(int argc, char **argv) {
    string path_file = "benchmark-perso/c6.clq";
    if (argc == 2) {
        path_file = string(argv[1]);
    }
    // Crée un graphe g
    Graph g;

    // Chargement du graphe depuis un ficher
    std::cout<<"[+] Loading file : "<<path_file<<std::endl;
    std::filebuf fb;
    if (fb.open (path_file,std::ios::in)) {
        std::istream is(&fb);
        read_dimacs(is, g);
        fb.close();
    } else {
        std::cerr<<"ERROR"<<std::endl;
        return EXIT_FAILURE;
    }

    std::cout<<"[+] Nombre de noeuds : "<<boost::num_vertices(g)<<std::endl;

    // find_clique(g);
    // std::cout<<edge(0, 4, g).first<<std::endl;

    // cout<<boost::out_degree(0, g)<<endl;

    // auto neighbours = boost::adjacent_vertices(0, g);

    // auto p = std::find (make_iterator_range(neighbours).begin(), make_iterator_range(neighbours).end(), 3);
    // if (p != make_iterator_range(neighbours).end()) {
    //     std::cout << "Element found in myints: ";
    // } else {
    //     std::cout << "Element not found in myints\n";
    // }

    // // View
    // print_graph(g);

    /// dot -Tpdf filename.dot -o outfile.pdf
    // std::ofstream f("filename.dot");
    // boost::write_graphviz(f, g);
    // f.close();
    // boost::write_graphviz(std::cout, g);
    return EXIT_SUCCESS;
}


// Copyright (c) 2019 Vsevolod Vlaskine

/// @author vsevolod vlaskine

#include <iostream>
#include <sstream>
#include <type_traits>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/version.hpp>
#if defined( CV_VERSION_EPOCH ) && CV_VERSION_EPOCH == 2
#include <opencv2/calib3d/calib3d.hpp>
#else
#include <opencv2/calib3d.hpp>
#endif
#include <tbb/parallel_for.h>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/tuple/tuple.hpp>
#include <comma/base/exception.h>
#include <comma/csv/ascii.h>
#include <comma/visiting/traits.h>
#include "../../../math/range_bearing_elevation.h"
#include "../../../visiting/eigen.h"
#include "../../equirectangular.h"
#include "equirectangular_map.h"

namespace snark { namespace cv_calc { namespace equirectangular_map {

static cv::Mat rotation_matrix( double x,double y,double z ) // quick and dirty
{
    cv::Mat r_x = ( cv::Mat_<double>(3,3) <<
        1,            0,            0,
        0,  std::cos(x), -std::sin(x),
        0,  std::sin(x),  std::cos(x) );
    cv::Mat r_y = ( cv::Mat_<double>(3,3) <<
        std::cos(y),    0,  std::sin(y),
                  0,    1,            0,
       -std::sin(y),    0,  std::cos(y) );
    cv::Mat r_z = ( cv::Mat_<double>(3,3) <<
        std::cos(z), -std::sin(z),      0,
        std::sin(z),  std::cos(z),      0,
                  0,            0,      1 );
    return r_z * r_y * r_x;
}

// see: inverse formula for spherical projection, Szeliski, "Computer Vision: Algorithms and Applications" p439.
struct calculator
{
    calculator( const Eigen::Vector3d& orientation, cv::Mat K, unsigned int w, unsigned int h )
        : RK( rotation_matrix( orientation.y(), orientation.z(), orientation.x() ) * K.inv() ) // todo: validate euler angle order
        , w( w )
        , h( h )
    {
        xyz = ( cv::Mat_< double >( 3, 1 ) << 0, 0, 1 );
    }

    std::pair< double, double > projected_pixel( unsigned int x, unsigned int y )
    {
        xyz.at< double >( 0, 0 ) = x;
        xyz.at< double >( 1, 0 ) = y;
        cv::Mat ray3d = RK * ( xyz / cv::norm( xyz ) );
        double xp = ray3d.at< double >( 0, 0 );
        double yp = ray3d.at< double >( 0, 1 );
        double zp = ray3d.at< double >( 0, 2 );
        double phi = std::atan2( xp, zp );
        double sx = ( phi / ( M_PI * 2 ) + 0.5 ) * w;
        if( sx >= w - 0.00001 ) { sx = 0; }
        double theta = std::atan2( yp, std::sqrt( xp * xp + zp * zp ) );
        double sy = ( theta / M_PI + 0.5 ) * h;
        return std::make_pair( sx, sy );
    };

    cv::Mat RK;
    cv::Mat xyz;
    unsigned int w;
    unsigned int h;
};

std::string options()
{
    std::ostringstream oss;
    oss << "        --cubes,--cube-size=[<width>]:" << std::endl;
    oss << "            direct: output map width for top,back,left,front,right,bottom cubes with a given original orientation" << std::endl;
    oss << "            reverse: cube size" << std::endl;
    oss << "        --focal-length=<pixels>; camera focal length" << std::endl;
    oss << "        --map-size,--size=[<width>,<height>]; output map size" << std::endl;
    oss << "        --orientation=<roll>,<pitch>,<yaw>; default=0,0,0; orientation in radians" << std::endl;
    oss << "        --reverse,--from-cubes; generate reverse map, i.e. mapping cubes to spherical images; if no --cube-size given, spherical width / 4 used" << std::endl;
    oss << "        --spherical-size=<width>[,<height>]; spherical image size" << std::endl;
    oss << std::endl;
    return oss.str();
}

int run( const comma::command_line_options& options )
{
    static_assert( sizeof( float ) == 4, "expected float of size 4" );
    options.assert_mutually_exclusive( "--reverse", "--focal-length,--map-size,--size,--orientation" );
    unsigned int spherical_width, spherical_height; // todo? make doubles?
    const auto& s = comma::split( options.value< std::string >( "--spherical-size" ), ',' );
    switch( s.size() )
    {
        case 1: spherical_width = boost::lexical_cast< unsigned int >( s[0] ); spherical_height = spherical_width / 2; break;
        case 2: spherical_width = boost::lexical_cast< unsigned int >( s[0] ); spherical_height = boost::lexical_cast< unsigned int >( s[1] ); break;
        default: std::cerr << "cv-calc: equirectangular-map: expected spherical size as <width>[,<height>]; got '" << comma::join( s, ',' ) << "'" << std::endl; return 1;
    }
    if( spherical_width != spherical_height * 2 ) { std::cerr << "cv-calc: equirectangular-map: expected spherical height half of width; got width: " << spherical_width << " height: " << spherical_height << std::endl; }
    if( options.exists( "--reverse" ) )
    {
        unsigned int cube_size = options.value< unsigned int >( "--cubes,--cube-size", spherical_width / 4 ); // quick and dirty
        cv::Mat x( spherical_height, spherical_width, CV_32F );
        cv::Mat y( spherical_height, spherical_width, CV_32F );
        tbb::parallel_for( tbb::blocked_range< std::size_t >( 0, spherical_height ), [&]( const tbb::blocked_range< std::size_t >& r )
        {
            for( unsigned int v = r.begin(); v < r.end(); ++v )
            {
                for( unsigned int u = 0; u < spherical_width; ++u )
                {
                    auto pixel = snark::equirectangular::to_cube( Eigen::Vector2d( u, v ), spherical_width );
                    pixel.first.y() += pixel.second; // quick and dirty
                    pixel.first *= cube_size;
                    x.at< float >( v, u ) = pixel.first.x();
                    y.at< float >( v, u ) = pixel.first.y();
                }
            }
        } );
        std::cout.write( reinterpret_cast< const char* >( x.datastart ), x.dataend - x.datastart );
        std::cout.write( reinterpret_cast< const char* >( y.datastart ), y.dataend - y.datastart );
        std::cout.flush();
    }
    else // todo? reimplement using equirectangular::... methods, which potentially may speed it up 2-3 times
    {
        options.assert_mutually_exclusive( "--focal-length", "--cubes,--cube-size" ); // quick and dirty for now
        auto focal_length = options.optional< double >( "--focal-length" );
        unsigned int map_width, map_height;
        bool cubes = options.exists( "--cubes,--cube-size" );
        if( cubes ) { map_width = map_height = options.value< unsigned int >( "--cubes" ); }
        else { boost::tie( map_width, map_height ) = comma::csv::ascii< std::pair< unsigned int, unsigned int > >().get( options.value< std::string >( "--map-size,--size" ) ); }
        if( !focal_length )
        {
            if( map_width != map_height ) { std::cerr << "cv-calc: equirectangular-map: please specify --focal-length" << std::endl; return 1; }
            focal_length = map_width / 2;
        }
        cv::Mat camera = ( cv::Mat_< double >( 3, 3 ) << *focal_length,             0,  map_width / 2,
                                                                        0, *focal_length, map_height / 2,
                                                                        0,             0,              1 );
        auto orientation = comma::csv::ascii< Eigen::Vector3d >().get( options.value< std::string >( "--orientation", "0,0,0" ) );
        auto make_map = [&]( const Eigen::Vector3d& o ) -> std::pair< cv::Mat, cv::Mat >
        {
            cv::Mat x( map_height, map_width, CV_32F ); // quick and dirty; if output to stdout has problems, use serialisation class
            cv::Mat y( map_height, map_width, CV_32F ); // quick and dirty; if output to stdout has problems, use serialisation class
            tbb::parallel_for( tbb::blocked_range< std::size_t >( 0, map_height ), [&]( const tbb::blocked_range< std::size_t >& r )
            {
                calculator calc( o, camera, spherical_width, spherical_height );
                for( unsigned int v = r.begin(); v < r.end(); ++v )
                {
                    for( unsigned int u = 0; u < map_width; ++u )
                    {
                        boost::tie( x.at< float >( v, u ), y.at< float >( v, u ) ) = calc.projected_pixel( u, v );
                    }
                }
            } );
            return std::make_pair( x, y );
        };
        auto face = make_map( orientation );
        if( !cubes )
        {
            std::cout.write( reinterpret_cast< const char* >( face.first.datastart ), face.first.dataend - face.first.datastart );
            std::cout.write( reinterpret_cast< const char* >( face.second.datastart ), face.second.dataend - face.second.datastart );
            std::cout.flush();
            return 0;
        }
        if( orientation != Eigen::Vector3d::Zero() ) { std::cerr << "cv-calc: equirectangular-map: for --cubes, expected --orientation 0,0,0; got: " << options.value< std::string >( "--orientation" ) << " (not supported)" << std::endl; return 1; }
        cv::Mat x( map_height * 6, map_width, CV_32F ); // quick and dirty; if output to stdout has problems, use serialisation class
        cv::Mat y( map_height * 6, map_width, CV_32F ); // quick and dirty; if output to stdout has problems, use serialisation class
        tbb::parallel_for( tbb::blocked_range< std::size_t >( 0, 6 ), [&]( const tbb::blocked_range< std::size_t >& r )
        {
            for( unsigned int face = r.begin(); face < r.end(); ++face )
            {
                unsigned int offset = face * map_width;
                for( unsigned int v = 0; v < map_width; ++v )
                {
                    for( unsigned int u = 0; u < map_width; ++u )
                    {
                        auto p = snark::equirectangular::from_cube( Eigen::Vector2d( u, v ) / map_width
                                                                  , static_cast< equirectangular::cube::faces::values >( face ) ) * spherical_width;
                        x.at< float >( v + offset, u ) = p.x();
                        y.at< float >( v + offset, u ) = p.y();
                    }
                }
            }
        } );
        std::cout.write( reinterpret_cast< const char* >( x.datastart ), x.dataend - x.datastart );
        std::cout.write( reinterpret_cast< const char* >( y.datastart ), y.dataend - y.datastart );
    }
//     auto top = make_map( Eigen::Vector3d( 0, M_PI / 2, orientation.z() ) );
//     auto bottom = make_map( Eigen::Vector3d( 0, -M_PI / 2, orientation.z() ) );
//     std::cout.write( reinterpret_cast< const char* >( top.first.datastart ), top.first.dataend - top.first.datastart );
//     int width = spherical_width / 4;
//     for( int i = -2; i < 2; ++i )
//     {
//         cv::Mat f = face.first + width * i; // todo? waste to allocate it each time (currently, the only reason for it is the back face
//         if( i == -2 ) { cv::Mat( f, cv::Rect( 0, 0, f.cols / 2, f.rows ) ) += spherical_width; }
//         std::cout.write( reinterpret_cast< const char* >( f.datastart ), f.dataend - f.datastart );
//     }
//     std::cout.write( reinterpret_cast< const char* >( bottom.first.datastart ), bottom.first.dataend - bottom.first.datastart );
//     std::cout.write( reinterpret_cast< const char* >( top.second.datastart ), top.second.dataend - top.second.datastart );
//     for( unsigned int i = 0; i < 4; ++i ) { std::cout.write( reinterpret_cast< const char* >( face.second.datastart ), face.second.dataend - face.second.datastart ); }
//     std::cout.write( reinterpret_cast< const char* >( bottom.second.datastart ), bottom.second.dataend - bottom.second.datastart );
//     std::cout.flush();
    return 0;
}

} } } // namespace snark { namespace cv_calc { namespace equirectangular_map {


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE DegreeTwoReducerTests
#include <boost/test/unit_test.hpp>

#include <vector>
#include <set>

#include "graph/PBQPGraph.hpp"
#include "graph/Vector.hpp"
#include "graph/PBQPNode.hpp"
#include "graph/PBQPEdge.hpp"
#include "reduction/PBQPReduction.hpp"
#include "reduction/degree/DegreeTwoReducer.hpp"
#include "graph/PBQPSolution.hpp"

#include "util/TestUtils.hpp"

namespace pbqppapa {

BOOST_AUTO_TEST_CASE(emptyGraphTest) {
	//make sure this doesnt explode
	PBQPGraph<int> graph;
	DegreeTwoReducer<int> twoReducer(&graph);
	std::vector<PBQPGraph<int>*> result = twoReducer.reduce();
}

BOOST_AUTO_TEST_CASE(simpleCalculation) {
	PBQPGraph<int> graph = PBQPGraph<int>();
	int node1Arr[] { 1, 2 };
	int node2Arr[] { 5, 3 };
	int node3Arr[] { 10, 2 };
	Vector<int> vek1(2, node1Arr);
	Vector<int> vek2(2, node2Arr);
	Vector<int> vek3(2, node3Arr);
	PBQPNode<int>* first = graph.addNode(vek1);
	PBQPNode<int>* second = graph.addNode(vek2);
	PBQPNode<int>* third = graph.addNode(vek3);
	unsigned long firstIndex = first->getIndex();
	unsigned long secondIndex = second->getIndex();
	unsigned long thirdIndex = third->getIndex();
	int edge1Arr[] { 3, 1, 8, 5 };
	Matrix<int> mat1(2, 2, edge1Arr);
	int edge2Arr[] { 0, 7, 2, 8 };
	Matrix<int> mat2(2, 2, edge2Arr);
	graph.addEdge(first, second, mat1);
	graph.addEdge(second, third, mat2);
	//second one gets removed
	DegreeTwoReducer<int> twoReducer(&graph);
	std::vector<PBQPGraph<int>*> result = twoReducer.reduce();
	BOOST_CHECK_EQUAL(result.size(), 1);
	PBQPGraph<int>* resultGraph = result[0];
	BOOST_CHECK_EQUAL(resultGraph->getNodeCount(), 2);
	BOOST_CHECK_EQUAL(resultGraph->getEdgeCount(), 1);
	PBQPEdge<int>* edge = *(resultGraph->getEdgeBegin());
	BOOST_CHECK_EQUAL(edge->getMatrix().get(0, 0), 6);
	BOOST_CHECK_EQUAL(edge->getMatrix().get(0, 1), 12);
	BOOST_CHECK_EQUAL(edge->getMatrix().get(1, 0), 10);
	BOOST_CHECK_EQUAL(edge->getMatrix().get(1, 1), 16);
	PBQPSolution<int> sol(3);
	sol.setSolution(firstIndex, 0);
	sol.setSolution(thirdIndex, 0);
	twoReducer.solve(sol);
	BOOST_CHECK_EQUAL(sol.getSolution(secondIndex), 1);
}

}


// Functions for reading, encoding and normalizeing image features
//
// @author: Bingqing Qu
//
// For implentation details, refer to:
//
// Jinjun Wang; Jianchao Yang; Kai Yu; Fengjun Lv; Huang, T.;
// Yihong Gong, "Locality-constrained Linear Coding for image
// classification, " Computer Vision and Pattern Recognition (CVPR),
// 2010 IEEE Conference on , vol., no., pp.3360,3367, 13-18 June 2010
//
// Copyright (C) 2014-2015  Bingqing Qu <sylar.qu@gmail.com>
//
// @license: See LICENSE at root directory

#ifndef SIREEN_IMAGE_FEATURE_EXTRACT_H_
#define SIREEN_IMAGE_FEATURE_EXTRACT_H_
#include <fstream>
#include <string.h>
#include <queue>
#include <stdexcept>
#include <algorithm>

// opencv header
#include <opencv2/opencv.hpp>

// Eigen Linear Algebra
#include <Eigen/Dense>

using namespace cv;
using namespace std;
using namespace Eigen;

extern "C" {
#include <vl/generic.h>
#include <vl/sift.h>
#include <vl/dsift.h>
};

// Image Coder Class
// Sample Usage:
//    ImageCoder icoder;
//    Mat src_image = cv::imread(path_to_image,0);
//    std::string result = icoder.LLCDescriptor(src_image);
class ImageCoder
{

private:
    /** DSIFT MEMBERS */
    // standard resize width
    int std_width_;
    // standard resize height
    int std_height_;
    // sift sampleing step size
    unsigned int step_;
    // sift bin size
    unsigned int bin_size_;
    // dsift filter
    VlDsiftFilter* dsift_filter_;

    // image data buffer, in vlfeat, vl_sift_pix is general used
    // vl_sift_pix is infact a symbolic link to float
    // the buffer always contains current image pixel values
    float* image_data_;

    /** SIFT MEMBERS */
    // sift filter
    VlSiftFilt* sift_filter_;

    /**
     * set parameters for ImageCoder
     *
     * @param std_width  standard image resize frame width
     * @param std_height standard image resize frame height
     * @param step       VlDsiftFilter step parameter
     * @param bin_size   VlDsiftFilter binSize parameter
     */
    void set_params(int, int, int, int);
    /**
     * decode image to graylevel resized values by row-order.
     *
     * @param src_image opencv Mat image
     *
     * @return image data values
     */
    float* decode_image(Mat);
    /**
     * compute linear local constraint coding descriptor from dsift
     * descriptors
     *
     * @param dsift_descr dsift descriptors
     * @param codebook    codebook from sift-kmeans
     * @param ncb         dimension of codebook
     * @param k           get top k nearest codes
     * @param out         output vector will take the llc result
     *
     * @return Eigen vector take the llc valuex
     */
    VectorXf llc_process(float*, float*, const int, const int, const int, const int);

public:
    /** Default Constructor*/
    ImageCoder(void);
    /**
     * Constructor Overloading
     * @param std_width  standard image resize frame width
     * @param std_height standard image resize frame height
     * @param step       VlDsiftFilter step parameter
     * @param bin_size   VlDsiftFilter binSize parameter
     */
    ImageCoder(int,int,int,int);
    /**
     * Constructor Overloading
     * @param dsift_filter VlDsiftFilter*
     */
    ImageCoder(VlDsiftFilter*);
    /** Destructor */
    ~ImageCoder(void);
    /**
     * encode dense-sift descriptors
     *
     * @param image pixel values in row-major order
     * @return the dense sift float-point descriptors
     */
    float* dsift_descriptor(float*);
    void sift_descriptor(float*, int&, vector<float>&);
    /**
     * compute linear local constraint coding descriptor
     *
     * @param dsift_descr dsift descriptors
     * @param codebook  codebook from sift-kmeans
     * @param ncb       dimension of codebook
     * @param k         get top k nearest codes
     * @param out       output vector will take the llc result
     *
     * @return a conversion from llc feature to string
     */
    string llc_dense_sift(float* , float*, const int, const int, vector<float> &);
    /**
     * compute linear local constraint coding descriptor
     *
     * @param src_image source image in opencv mat format
     * @param codebook  codebook from sift-kmeans
     * @param ncb       dimension of codebook
     * @param k         get top k nearest codes
     *
     * @return a conversion from llc feature to string
     */
    string llc_dense_sift(Mat, float*, const int, const int);
    string llc_sift(Mat, float*, const int, const int);

    /**
     * Optimized sift feature improvement and normalization
     *
     * @param descriptors sift descriptors
     * @param row         number of rows
     * @param col         number of column
     * @param normalized  flag for normalized input
     *
     * @return MatrixXf normalized dsift descripters in Eigen::MatrixXf form
     */
    Eigen::MatrixXf norm_sift(float *, int, int, const bool);

};
#endif //SIREEN_IMAGE_FEATURE_EXTRACT_H_


#ifndef __earthenvironment_HH__
#define __earthenvironment_HH__
/********************************* TRICK HEADER *******************************
PURPOSE:
      (Describe the EarthEnvironment Module Variables and Algorithm)
LIBRARY DEPENDENCY:
      ((../src/EarthEnvironment.cpp))
PROGRAMMERS:
      (((Chun-Hsu Lai) () () () ))
*******************************************************************************/
#include <armadillo>
#include <functional>
#include "Module.hh"
#include "aux.hh"
#include "global_constants.hh"

#include "env/atmosphere.hh"
#include "env/atmosphere76.hh"
#include "env/atmosphere_nasa2002.hh"
#include "env/atmosphere_weatherdeck.hh"

#include "env/wind.hh"
#include "env/wind_constant.hh"
#include "env/wind_no.hh"
#include "env/wind_tabular.hh"

#include "Time_management.hh"
#include "dm_delta_ut.hh"

class EarthEnvironment : public Dynamics {
  TRICK_INTERFACE(EarthEnvironment);

 public:
  EarthEnvironment(Data_exchang& input);

  EarthEnvironment(const EarthEnvironment& other);
  ~EarthEnvironment();

  EarthEnvironment& operator=(const EarthEnvironment& other);

  void atmosphere_use_public();
  void atmosphere_use_nasa();
  void atmosphere_use_weather_deck(char* filename);

  void set_no_wind();
  void set_constant_wind(double dvae, double dir, double twind,
                         double vertical_wind);
  void set_tabular_wind(char* filename, double twind, double vertical_wind);

  void set_no_wind_turbulunce();
  void set_wind_turbulunce(double turb_length, double turb_sigma, double taux1,
                           double taux1d, double taux2, double taux2d,
                           double tau, double gauss_value);

  virtual void init();
  virtual void algorithm(double int_step);

  void set_RNP();

  // double get_rho();
  // double get_vmach();
  // double get_pdynmc();
  // double get_tempk();
  // double get_dvba();
  // double get_grav();
  // double get_press();

  // arma::vec3 get_GRAVG();
  // arma::vec3 get_VAED();
  // arma::mat33 get_TEI();

 private:
  time_management* time;

  /* Constants */
  cad::Atmosphere* atmosphere;
  cad::Wind* wind;

  /* Variable declaration */
  VECTOR(GRAVG, 3);  /* *o (m/s2) Earth gravity acceleration in inertia frame */
  VECTOR(VBAB, 3);   /* *o (m/s)  Vehicle speed wrt air speed in body frame */
  MATRIX(TEI, 3, 3); /* *o (--)  T.M. from ECI to ECEF */

  double vmach;            /* *o (--)  vehicle mach number */
  double pdynmc;           /* *o (pa)  Dynamic pressure */
  double dvba;             /* *o (m/s) Vehicle air speed magnitude */
  double gravg;            /* *o (m/s2)  Earth gravity acceleration magnitude */
  double tempc;            /* *o (c)  Atmosphere temperature in Celsius */
  double DM_sidereal_time; /* *o  (--)  Sidereal time */
  double DM_Julian_century; /* *o  (--)  Julian_century */

  /* Function declaration */
  void RNP();
  arma::vec AccelHarmonic(arma::vec3 SBII, int n_max, int m_max);
};

#endif  // __earthenvironment_HH__


#include <iostream>

#include <boost/graph/grid_graph.hpp>

#include "NearestNeighbor/topological_search.hpp"

int main(int argc, char *argv[])
{
  typedef boost::grid_graph<2> GraphType;

  const unsigned int dimension = 5;
  boost::array<std::size_t, 2> lengths = { { dimension, dimension } };
  GraphType graph(lengths);

  typedef boost::graph_traits<GraphType>::vertex_descriptor VertexDescriptor;

  VertexDescriptor v = { { 0, 1 } };

  typedef boost::hypercube_topology<6, boost::minstd_rand> TopologyType;
  TopologyType myTopology;
  typedef TopologyType::point_type PointType;

  std::vector<PointType> vertexData(dimension * dimension);

  // This is an "exterior property" of the grid_graph
  typedef boost::property_map<GraphType, boost::vertex_index_t>::const_type IndexMapType;

  IndexMapType indexMap(get(boost::vertex_index, graph));

  typedef boost::iterator_property_map<std::vector<PointType>::iterator, IndexMapType> MapType;
  MapType myMap(vertexData.begin(), indexMap);

  typedef linear_neighbor_search<> SearchType;

  // Add vertices to the graph and corresponding points increasin integer points to the tree.
  // The experiment here is to query the nearest neighbor of a point like (5.2, 5.2, 5.1, 5.3, 5.2, 5.1)
  // and ensure we get back (5,5,5,5,5,5)
  unsigned int numberOfVertices = 100;
  for(unsigned int vertexId = 0; vertexId < numberOfVertices; ++vertexId)
  {
    PointType p;
    for(unsigned int dim = 0; dim < dimension; ++dim)
      {
      p[dim] = vertexId;
      }
    boost::put(myMap, v, p);
  };

  PointType queryPoint;
  for(unsigned int dim = 0; dim < dimension; ++dim)
    {
    queryPoint[dim] = 5.2;
    }

  SearchType search;

  //VertexDescriptor nearestNeighbor = search(queryPoint, graph, myTopology, myMap);
  VertexDescriptor nearestNeighbor = search.operator()<GraphType, TopologyType, MapType>(queryPoint, graph, myTopology, myMap);
  std::cout << "nearestNeighbor[0]: " << nearestNeighbor[0] << std::endl;

  return 0;
}


#include <boost/random/poisson_distribution.hpp>


#include "cellogram/image_reader.h"

#include <Eigen/Dense>
#include <iostream>

int main(int argc, char** argv)
{
	Eigen::MatrixXd img;
	cellogram::read_image(".../detection_test.tif", img);

	std::cout<<img<<std::endl;

	return 0;
}


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2020 Scientific Computing and Imaging Institute,
   University of Utah.

   Permission is hereby granted, free of charge, to any person obtaining a
   copy of this software and associated documentation files (the "Software"),
   to deal in the Software without restriction, including without limitation
   the rights to use, copy, modify, merge, publish, distribute, sublicense,
   and/or sell copies of the Software, and to permit persons to whom the
   Software is furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included
   in all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
   THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
   FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
   DEALINGS IN THE SOFTWARE.
*/

#include <Core/Algorithms/Math/ComputeTensorUncertaintyAlgorithm.h>
#include <Core/Algorithms/Base/AlgorithmPreconditions.h>
#include <Core/Datatypes/Dyadic3DTensor.h>
#include <Core/Datatypes/Legacy/Field/FieldInformation.h>
#include <Core/Datatypes/Mesh/MeshFacade.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/Legacy/Field/VMesh.h>
#include <Core/Datatypes/Legacy/Field/VField.h>
#include <Core/GeometryPrimitives/Point.h>
#include <boost/tuple/tuple.hpp>
#include <unsupported/Eigen/MatrixFunctions>

using namespace SCIRun;
using namespace Core;
using namespace Core::Datatypes;
using namespace Core::Algorithms;
using namespace Core::Algorithms::Math;
using namespace Core::Geometry;

AlgorithmOutputName ComputeTensorUncertaintyAlgorithm::MeanTensorField("MeanTensorField");
AlgorithmOutputName ComputeTensorUncertaintyAlgorithm::CovarianceMatrix("CovarianceMatrix");

ALGORITHM_PARAMETER_DEF(Math, MeanInvariantMethod);
ALGORITHM_PARAMETER_DEF(Math, MeanOrientationMethod);

// TODO move helpers
Dyadic3DTensor scirunTensorToEigenTensor(const Geometry::Tensor& t)
{
  Dyadic3DTensor newTensor(t.xx(), t.xy(), t.xz(), t.yy(), t.yz(), t.zz());
  return newTensor;
}

Tensor eigenTensorToScirunTensor(const Dyadic3DTensor& t)
{
  return Tensor(t(0, 0), t(1, 0), t(2, 0), t(1, 1), t(2, 1), t(2, 2));
}

// TODO move to separate algo
enum FieldDataType
{ node, edge, face, cell };

class ComputeTensorUncertaintyAlgorithmImpl
{
public:
  ComputeTensorUncertaintyAlgorithmImpl();
  void run(const FieldList& fields);
  void setInvariantMethod(AlgoOption method);
  void setOrientationMethod(AlgoOption method);
  void getPoints(const FieldList& fields);
  void getPointsForFields(FieldHandle field, std::vector<int>& indices, std::vector<Point>& points);
  FieldHandle getMeanTensors() const;
  MatrixHandle getCovarianceMatrices() const;
private:
  size_t fieldCount_ = 0;
  size_t fieldSize_ = 0;
  std::vector<std::vector<Point>> points_;
  std::vector<std::vector<int>> indices_;
  std::vector<std::vector<Dyadic3DTensor>> tensors_;
  std::vector<Dyadic3DTensor> meanTensors_;
  std::vector<DyadicTensor<6>> covarianceMatrices_;
  AlgoOption invariantMethod_;
  AlgoOption orientationMethod_;

  void verifyData(const FieldList& fields);
  void getTensors(const FieldList& fields);
  void computeCovarianceMatrices();
  void computeMeanTensors();
  Dyadic3DTensor computeMeanMatrixAverage(int index) const;
  Dyadic3DTensor computeMeanLinearInvariant(int t) const;
  Dyadic3DTensor computeMeanLogEuclidean(int t) const;
  void computeMeanTensorsSameMethod();
  void computeMeanTensorsDifferentMethod();
};

void ComputeTensorUncertaintyAlgorithmImpl::setInvariantMethod(AlgoOption method)
{
  invariantMethod_ = method;
}

void ComputeTensorUncertaintyAlgorithmImpl::setOrientationMethod(AlgoOption method)
{
  orientationMethod_ = method;
}

FieldHandle ComputeTensorUncertaintyAlgorithmImpl::getMeanTensors() const
{
  FieldInformation ofinfo("PointCloudMesh", 0, "Tensor");
  auto ofield = CreateField(ofinfo);
  auto mesh = ofield->vmesh();
  auto field = ofield->vfield();

  std::vector<VMesh::index_type> meshIndices(fieldSize_);
  for (size_t i = 0; i < fieldSize_; ++i)
    meshIndices[i] = mesh->add_point(points_[0][i]);

  field->resize_fdata();

  for (size_t i = 0; i < fieldSize_; ++i)
    field->set_value(eigenTensorToScirunTensor(meanTensors_[i]), meshIndices[i]);

  return ofield;
}

void ComputeTensorUncertaintyAlgorithmImpl::computeMeanTensorsSameMethod()
{
  if (orientationMethod_.option_ == "Matrix Average")
  {
    for (size_t t = 0; t < fieldSize_; ++t)
      meanTensors_[t] = computeMeanMatrixAverage(t);
  }
  else if (orientationMethod_.option_ == "Log-Euclidean")
  {
    for (size_t t = 0; t < fieldSize_; ++t)
      meanTensors_[t] = computeMeanLogEuclidean(t);
  }
}

void ComputeTensorUncertaintyAlgorithmImpl::computeMeanTensorsDifferentMethod()
{
  std::vector<Dyadic3DTensor> orientationTensors(fieldSize_);
  std::vector<Dyadic3DTensor> invariantTensors(fieldSize_);
  if (orientationMethod_.option_ == "Matrix Average")
  {
    for (size_t t = 0; t < fieldSize_; ++t)
      orientationTensors[t] = computeMeanMatrixAverage(t);
  }
  else if (orientationMethod_.option_ == "Log-Euclidean")
  {
    for (size_t t = 0; t < fieldSize_; ++t)
      orientationTensors[t] = computeMeanLogEuclidean(t);
  }

  if (invariantMethod_.option_ == "Matrix Average")
  {
    for (size_t t = 0; t < fieldSize_; ++t)
      invariantTensors[t] = computeMeanMatrixAverage(t);
  }
  else if (invariantMethod_.option_ == "Log-Euclidean")
  {
    for (size_t t = 0; t < fieldSize_; ++t)
      invariantTensors[t] = computeMeanLogEuclidean(t);
  }
  else if (invariantMethod_.option_ == "Linear Invariant")
  {
    for (size_t t = 0; t < fieldSize_; ++t)
      invariantTensors[t] = computeMeanLinearInvariant(t);
  }

  for (size_t t = 0; t < fieldSize_; ++t)
  {
    invariantTensors[t].setDescendingRHSOrder();
    orientationTensors[t].setDescendingRHSOrder();
    meanTensors_[t] = Dyadic3DTensor(orientationTensors[t].getEigenvectors(),
                                     invariantTensors[t].getEigenvalues());
    meanTensors_[t].setDescendingRHSOrder();
  }
}

void ComputeTensorUncertaintyAlgorithmImpl::computeMeanTensors()
{
  meanTensors_ = std::vector<Dyadic3DTensor>(fieldSize_);
  if (invariantMethod_.option_ == orientationMethod_.option_)
    computeMeanTensorsSameMethod();
  else
    computeMeanTensorsDifferentMethod();
}

Dyadic3DTensor ComputeTensorUncertaintyAlgorithmImpl::computeMeanMatrixAverage(int t) const
{
  Dyadic3DTensor sum = Dyadic3DTensor();
  for (size_t f = 0; f < fieldCount_; ++f) {
    sum += tensors_[f][t];
  }
  sum = sum / (double)fieldCount_;

  return sum;
}

// Mean calculation using Linear Invariant interpolation
Dyadic3DTensor ComputeTensorUncertaintyAlgorithmImpl::computeMeanLinearInvariant(int t) const
{
  const static double oneThird = 1.0 / 3.0;
  const static double sqrtThreeHalves = sqrt(1.5);
  const static double threeSqrtSix = 3.0 * sqrt(6.0);

  const static Dyadic3DTensor identity = Dyadic3DTensor(Eigen::Vector3d(1, 0, 0),
                                                        Eigen::Vector3d(0, 1, 0),
                                                        Eigen::Vector3d(0, 0, 1));
  const static Dyadic3DTensor identityThird = oneThird * identity;

  double K1 = 0.0;
  double R2 = 0.0;
  double R3 = 0.0;

  for (size_t f = 0; f < fieldCount_; ++f)
  {
    const double trace = tensors_[f][t].trace();
    Dyadic3DTensor trThird = trace * identityThird;
    K1 += trace;

    const double fro = tensors_[f][t].frobeniusNorm();
    Dyadic3DTensor anisotropicDeviation = tensors_[f][t] - trThird;
    anisotropicDeviation.setDescendingRHSOrder();
    const double anisotropicDeviationFro = anisotropicDeviation.frobeniusNorm();

    R2 += sqrtThreeHalves * anisotropicDeviationFro / fro;
    R3 += threeSqrtSix * (anisotropicDeviation / anisotropicDeviationFro).asMatrix().determinant();
  }

  // Equally weight all of the coeffecients
  K1 /= static_cast<double>(fieldCount_);
  R2 /= static_cast<double>(fieldCount_);
  R3 /= static_cast<double>(fieldCount_);

  // Clamp to avoid nan results with acos
  if (R3 > 1.0) R3 = 1.0;
  if (R3 < -1.0) R3 = -1.0;

  const double arccosR3 = std::acos(R3);
  Eigen::Vector3d eigvals;
  const double x = oneThird * K1;
  const double y = (2.0 * K1 * R2) / (3.0 * sqrt(3.0 - 2.0 * std::pow(R2, 2.0)));
  eigvals(0) = x + y * std::cos(arccosR3 * oneThird);
  eigvals(1) = x + y * std::cos((arccosR3 - 2.0 * M_PI) * oneThird);
  eigvals(2) = x + y * std::cos((arccosR3 + 2.0 * M_PI) * oneThird);

  // Using axis aligned orientation because this mean method is only for invariants
  Dyadic3DTensor ret = Dyadic3DTensor({Eigen::Vector3d({1,0,0}), Eigen::Vector3d({0,1,0}), Eigen::Vector3d({0,0,1})}, eigvals);
  return ret;
}

Dyadic3DTensor ComputeTensorUncertaintyAlgorithmImpl::computeMeanLogEuclidean(int t) const
{
  Eigen::Matrix3d sum = Eigen::Matrix3d::Zero();
  for (size_t f = 0; f < fieldCount_; ++f)
  {
    Dyadic3DTensor tensor = tensors_[f][t];
    tensor.setDescendingRHSOrder();
    Eigen::Vector3d eigvalsLog = tensor.getEigenvalues();
    Eigen::Matrix3d rotation = tensor.getEigenvectorsAsMatrix();
    for (size_t i = 0; i < eigvalsLog.size(); ++i)
      eigvalsLog[i] = std::log(eigvalsLog[i]);
    sum += rotation * eigvalsLog.asDiagonal() * rotation.transpose();
  }
  sum /= (double)fieldCount_;
  Eigen::Matrix3d mean = sum.exp();
  Dyadic3DTensor ret = Dyadic3DTensor(mean);
  return ret;
}

ComputeTensorUncertaintyAlgorithmImpl::ComputeTensorUncertaintyAlgorithmImpl()
{}

void ComputeTensorUncertaintyAlgorithmImpl::computeCovarianceMatrices()
{
  covarianceMatrices_ = std::vector<DyadicTensor<6>>(fieldSize_);

  Eigen::Matrix<double, 6, 6> covarianceMat;
  for (size_t t = 0; t < fieldSize_; ++t)
  {
    covarianceMat.fill(0.0);
    for (size_t f = 0; f < fieldCount_; ++f)
    {
      Eigen::Matrix<double, 6, 1> diffTensor = (tensors_[f][t] - meanTensors_[t]).mandel();
      covarianceMat += diffTensor * diffTensor.transpose();
    }
    covarianceMat /= static_cast<double>(fieldCount_);
    covarianceMatrices_[t] = DyadicTensor<6>(covarianceMat);
  }
}

void ComputeTensorUncertaintyAlgorithmImpl::getPointsForFields(
    FieldHandle field, std::vector<int>& indices, std::vector<Point>& points)
{
  // Collect indices and points from facades
  FieldDataType fieldLocation;
  FieldInformation finfo(field);
  if (finfo.is_point() || finfo.is_linear())
    fieldLocation = FieldDataType::node;
  else if (finfo.is_line())
    fieldLocation = FieldDataType::edge;
  else if (finfo.is_surface())
    fieldLocation = FieldDataType::face;
  else
    fieldLocation = FieldDataType::cell;

  auto mesh = field->vmesh();
  auto primaryFacade = field->mesh()->getFacade();
  switch (fieldLocation)
  {
  case FieldDataType::node:
    for (const auto& node : primaryFacade->nodes())
    {
      indices.push_back(node.index());
      Point p;
      mesh->get_center(p, node.index());
      points.push_back(p);
    }
    break;
  case FieldDataType::edge:
    for (const auto& edge : primaryFacade->edges())
    {
      indices.push_back(edge.index());
      Point p;
      mesh->get_center(p, edge.index());
      points.push_back(p);
    }
    break;
  case FieldDataType::face:
    for (const auto& face : primaryFacade->faces())
    {
      indices.push_back(face.index());
      Point p;
      mesh->get_center(p, face.index());
      points.push_back(p);
    }
    break;
  case FieldDataType::cell:
    for (const auto& cell : primaryFacade->cells())
    {
      indices.push_back(cell.index());
      Point p;
      mesh->get_center(p, cell.index());
      points.push_back(p);
    }
    break;
  }
}

void ComputeTensorUncertaintyAlgorithmImpl::getPoints(const FieldList& fields)
{
  fieldCount_ = fields.size();

  indices_ = std::vector<std::vector<int>>(fieldCount_);
  points_ = std::vector<std::vector<Point>>(fieldCount_);

  for (size_t f = 0; f < fieldCount_; ++f)
    getPointsForFields(fields[f], indices_[f], points_[f]);
}

void ComputeTensorUncertaintyAlgorithmImpl::verifyData(const FieldList& fields)
{
  fieldSize_ = indices_[0].size();
  for (size_t f = 1; f < fieldCount_; ++f)
    if (indices_[f].size() != fieldSize_)
      THROW_ALGORITHM_INPUT_ERROR_SIMPLE("All field inputs must have the same size.");

  // Verify all are tensors
  for (auto field : fields)
  {
    FieldInformation finfo(field);
    if (!finfo.is_tensor())
      THROW_ALGORITHM_INPUT_ERROR_SIMPLE("This module only supports tensor fields.");
  }
}

void ComputeTensorUncertaintyAlgorithmImpl::getTensors(const FieldList& fields)
{
  tensors_ = std::vector<std::vector<Dyadic3DTensor>>(fieldCount_);
  for (size_t f = 0; f < fieldCount_; ++f)
  {
    tensors_[f] = std::vector<Dyadic3DTensor>(fieldSize_);
    auto vfield = fields[f]->vfield();
    Tensor temp;
    for (size_t v = 0; v < fieldSize_; ++v)
    {
      vfield->get_value(temp, v);
      tensors_[f][v] = scirunTensorToEigenTensor(temp);
      tensors_[f][v].setDescendingRHSOrder();
    }
  }
}

MatrixHandle ComputeTensorUncertaintyAlgorithmImpl::getCovarianceMatrices() const
{
  auto m = std::make_shared<DenseMatrix>(21, fieldSize_);
  for (size_t i = 0; i < fieldSize_; ++i)
  {
    m->col(i) = covarianceMatrices_[i].mandel();
    Eigen::Matrix<double,21,1> recov_m = m->col(i);
    DyadicTensor<6> recov = DyadicTensor<6>(recov_m);
  }
  return m;
}

void ComputeTensorUncertaintyAlgorithmImpl::run(const FieldList& fields)
{
  getPoints(fields);
  verifyData(fields);
  getTensors(fields);
  computeMeanTensors();
  computeCovarianceMatrices();
}

ComputeTensorUncertaintyAlgorithm::ComputeTensorUncertaintyAlgorithm()
{
  addOption(Parameters::MeanInvariantMethod, "Linear Invariant",
            "Linear Invariant|Log-Euclidean|Matrix Average");
  addOption(Parameters::MeanOrientationMethod, "Log-Euclidean",
            "Log-Euclidean|Matrix Average");
}

AlgorithmOutput ComputeTensorUncertaintyAlgorithm::run(const AlgorithmInput& input) const
{
  auto fields = input.getList<Field>(Variables::InputFields);
  auto invariantMethod = get(Parameters::MeanInvariantMethod).toOption();
  auto orientMethod = get(Parameters::MeanOrientationMethod).toOption();
  auto data = runImpl(fields, invariantMethod, orientMethod);

  AlgorithmOutput output;
  output[MeanTensorField] = data.get<0>();
  output[CovarianceMatrix] = data.get<1>();
  return output;
}

boost::tuple<FieldHandle, MatrixHandle> ComputeTensorUncertaintyAlgorithm::runImpl(const FieldList& fields, const AlgoOption& invariantMethod, const AlgoOption& orientationOption) const
{
  if (fields.empty())
    THROW_ALGORITHM_INPUT_ERROR("No input fields given");

  auto impl = ComputeTensorUncertaintyAlgorithmImpl();
  impl.setInvariantMethod(invariantMethod);
  impl.setOrientationMethod(orientationOption);
  impl.run(fields);

  return boost::make_tuple(impl.getMeanTensors(), impl.getCovarianceMatrices());
}


#include "worldengine/images/simple_elevation_image.h"

#include <boost/log/trivial.hpp>

namespace WorldEngine
{
SimpleElevationImage::SimpleElevationImage(const World& world) :
    Image(world, false)
{
}
SimpleElevationImage::~SimpleElevationImage() {}

void SimpleElevationImage::DrawImage(boost::gil::rgb8_image_t::view_t& target)
{
   const ElevationArrayType& e     = world_.GetElevationData();
   const OceanArrayType&     ocean = world_.GetOceanData();

   float seaLevel    = world_.oceanLevel();
   bool  hasOcean    = (seaLevel != NAN) && (!ocean.empty());
   float minElevLand = 10.0f;
   float maxElevLand = -10.0f;
   float minElevSea  = 10.0f;
   float maxElevSea  = -10.0f;

   for (uint32_t y = 0; y < world_.height(); y++)
   {
      for (uint32_t x = 0; x < world_.width(); x++)
      {
         if (hasOcean && ocean[y][x])
         {
            if (minElevSea > e[y][x])
            {
               minElevSea = e[y][x];
            }
            if (maxElevSea < e[y][x])
            {
               maxElevSea = e[y][x];
            }
         }
         else
         {

            if (minElevLand > e[y][x])
            {
               minElevLand = e[y][x];
            }
            if (maxElevLand < e[y][x])
            {
               maxElevLand = e[y][x];
            }
         }
      }
   }

   if (hasOcean)
   {
      BOOST_LOG_TRIVIAL(debug) << "minElevSea = " << minElevSea;
      BOOST_LOG_TRIVIAL(debug) << "maxElevSea = " << maxElevSea;
   }

   BOOST_LOG_TRIVIAL(debug) << "minElevLand = " << minElevLand;
   BOOST_LOG_TRIVIAL(debug) << "maxElevLand = " << maxElevLand;

   float elevDeltaLand = (maxElevLand - minElevLand) / 11.0f;
   float elevDeltaSea  = (maxElevSea - minElevSea);
   float elevation;

   for (uint32_t y = 0; y < world_.height(); y++)
   {
      for (uint32_t x = 0; x < world_.width(); x++)
      {
         if (hasOcean && ocean[y][x])
         {
            elevation = ((e[y][x] - minElevSea) / elevDeltaSea);
         }
         else
         {
            elevation = ((e[y][x] - minElevLand) / elevDeltaLand) + 1;
         }

         target(x, y) = ElevationColor(elevation, seaLevel);
      }
   }
}

boost::gil::rgb8_pixel_t SimpleElevationImage::ElevationColor(float elevation,
                                                              float seaLevel)
{
   float r, g, b;
   std::tie(r, g, b) = ElevationColorF(elevation, seaLevel);
   SatureColorComponent(r);
   SatureColorComponent(g);
   SatureColorComponent(b);

   boost::gil::rgb8_pixel_t color(static_cast<uint8_t>(r * 255),
                                  static_cast<uint8_t>(g * 255),
                                  static_cast<uint8_t>(b * 255));

   return color;
}

std::tuple<float, float, float>
SimpleElevationImage::ElevationColorF(float elevation, float seaLevel)
{
   float colorStep = 1.5f;

   if (seaLevel == NAN)
   {
      seaLevel = -1.0f;
   }

   if (elevation < seaLevel / 2.0f)
   {
      elevation /= seaLevel;
      return std::make_tuple(0.0f, 0.0f, 0.75f + 0.5f * elevation);
   }
   if (elevation < seaLevel)
   {
      elevation /= seaLevel;
      return std::make_tuple(0.0f, 2.0f * (elevation - 0.5f), 1.0f);
   }

   elevation -= seaLevel;

   if (elevation < 1.0f * colorStep)
   {
      return std::make_tuple(0.0f, 0.5f + 0.5f * elevation / colorStep, 0.0f);
   }
   if (elevation < 1.5f * colorStep)
   {
      return std::make_tuple(
         2.0f * (elevation - 1.0f * colorStep) / colorStep, 1.0f, 0.0f);
   }
   if (elevation < 2.0f * colorStep)
   {
      return std::make_tuple(
         1.0f, 1.0f - (elevation - 1.5f * colorStep) / colorStep, 0.0f);
   }
   if (elevation < 3.0f * colorStep)
   {
      return std::make_tuple(
         1.0f - 0.5f * (elevation - 2.0f * colorStep) / colorStep,
         0.5f - 0.25f * (elevation - 2.0f * colorStep) / colorStep,
         0.0f);
   }
   if (elevation < 5.0f * colorStep)
   {
      return std::make_tuple(
         0.5f - 0.125f * (elevation - 3.0f * colorStep) / (2.0f * colorStep),
         0.25f + 0.125f * (elevation - 3.0f * colorStep) / (2.0f * colorStep),
         0.375f * (elevation - 3.0f * colorStep) / 2.0f * colorStep);
   }
   if (elevation < 8.0f * colorStep)
   {
      return std::make_tuple(
         0.375f + 0.625f * (elevation - 5.0f * colorStep) / (3.0f * colorStep),
         0.375f + 0.625f * (elevation - 5.0f * colorStep) / (3.0f * colorStep),
         0.375f + 0.625f * (elevation - 5.0f * colorStep) / (3.0f * colorStep));
   }

   elevation -= 8.0f * colorStep;
   while (elevation > 2.0f * colorStep)
   {
      elevation -= 2.0f * colorStep;
   }
   return std::make_tuple(1.0f, 1.0f - elevation / 4.0f, 1.0f);
}

void SimpleElevationImage::SatureColorComponent(float& component)
{
   if (component < 0.0f)
   {
      component = 0.0f;
   }
   if (component > 1.0f)
   {
      component = 1.0f;
   }
}
} // namespace WorldEngine


/**
 * @file NetTests.cpp
 *
 * @breif High level tests of the Net class
 *
 * @date 12/17/17
 * @author Ben Caine
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE NetTests

#include <boost/test/unit_test.hpp>
#include <chrono>
#include "Net.h"

BOOST_AUTO_TEST_CASE(test_relu) {
    std::cout << "Testing Relu" << std::endl;
    nn::Relu<float, 2> relu;

    int dim1 = 1;
    int dim2 = 10;
    Eigen::Tensor<float, 2> input(dim1, dim2);
    input.setRandom();
    input = input * input.constant(-1.0f);

    Eigen::Tensor<float, 2> result = relu.forward(input);
    for (unsigned int ii = 0; ii < dim1; ++ii) {
        for (unsigned int jj = 0; jj < dim2; ++jj) {
            BOOST_REQUIRE_MESSAGE(result(ii, jj) == 0, "Element in result does not equal zero");
        }
    }

    // Make a few elements positive
    input(0, 5) = 10.0;
    input(0, 3) = 150.0;

    result = relu.forward(input);
    for (unsigned int ii = 0; ii < dim1; ++ii) {
        for (unsigned int jj = 0; jj < dim2; ++jj) {
            BOOST_REQUIRE_MESSAGE(result(ii, jj) >= 0, "Element in result does is negative");
        }
    }
}

BOOST_AUTO_TEST_CASE(test_relu_back) {
    std::cout << "Testing Relu backwards" << std::endl;
    nn::Relu<float, 2> relu;

    int dim1 = 1;
    int dim2 = 10;
    Eigen::Tensor<float, 2> input(dim1, dim2);
    input.setValues({{-10, -7, -5, -3, 0, 1, 3, 5, 7, 10}});

    Eigen::Tensor<float, 2> accumulatedGrad(dim1, dim2);
    accumulatedGrad.setValues({{1, -4, 7, -10, 13, -16, 19, -22, 25, -28}});

    Eigen::Tensor<float, 2> forwardResult = relu.forward(input);
    Eigen::Tensor<float, 2> backwardResult = relu.backward(accumulatedGrad);

    std::vector<float> expectedOutput({0, 0, 0, 0, 0, -16, 19, -22, 25, -28});
    for (unsigned ii = 0; ii < dim2; ++ii) {
        BOOST_REQUIRE_MESSAGE(backwardResult(0, ii) == expectedOutput[ii], "Output of relu.backward did not match");
    }
}

BOOST_AUTO_TEST_CASE(test_softmax) {
    std::cout << "Testing Softmax" << std::endl;
    nn::Softmax<float, 2> softmax;

    int inputBatchSize = 2;
    Eigen::Tensor<float, 2> input(inputBatchSize, 2);
    input.setValues({{5, 5},
                     {-100, 100}});

    Eigen::Tensor<float, 2> result = softmax.forward(input);

    BOOST_REQUIRE_MESSAGE(result(0, 0) == 0.5, "Result(0, 0) did not match");
    BOOST_REQUIRE_MESSAGE(result(0, 1) == 0.5, "Result(0, 1) did not match");
    BOOST_REQUIRE_MESSAGE(result(1, 0) == 0, "Result (1, 0) did not match");
    BOOST_REQUIRE_MESSAGE(result(1, 1) == 1, "Result (1, 1) did not match");

    inputBatchSize = 1;
    int inputSize = 100;
    Eigen::Tensor<float, 2> input2(inputBatchSize, inputSize);
    input2.setRandom();

    Eigen::Tensor<float, 2> result2 = softmax.forward(input2);

    float sum = 0;
    for (unsigned int ii = 0; ii < inputSize; ++ii) {
        sum += result2(0, ii);
    }

    BOOST_REQUIRE_CLOSE(sum, 1.0, 1e-3);
}

BOOST_AUTO_TEST_CASE(test_softmax_back) {
    std::cout << "Testing Softmax backwards" << std::endl;
    nn::Softmax<float, 2> softmax;

    int inputBatchSize = 2;
    Eigen::Tensor<float, 2> input(inputBatchSize, 2);
    input.setValues({{5, 7},
                     {-100, 100}});

    Eigen::Tensor<float, 2> result = softmax.forward(input);

    Eigen::Tensor<float, 2> labels(inputBatchSize, 2);
    labels.setValues({{0, 1},
                      {0, 1}});

    // Already has state from forward
    Eigen::Tensor<float, 2> backwardsResult = softmax.backward(labels);
    // TODO: Add actual tests. Test against TF/Pytorch?
}

BOOST_AUTO_TEST_CASE(test_net1) {
    std::cout << "Testing net creation" << std::endl;
    nn::Net<float> net;

    // TODO: output of previous should match input of next. Can we auto-infer in some nice way?
    int batchSize = 1;
    net.add(new nn::Dense<float, 2>(batchSize, 28 * 28, 100, true))
       .add(new nn::Dense<float, 2>(batchSize, 100, 100, true))
       .add(new nn::Dense<float, 2>(batchSize, 100, 10, true));

    Eigen::Tensor<float, 2> input(batchSize, 28 * 28);
    input.setRandom();
    Eigen::Tensor<float, 2> result = net.forward<2, 2>(input);
    BOOST_REQUIRE_MESSAGE(result.dimensions()[0] == batchSize, "Result dimension 0 did not match batch size");
    BOOST_REQUIRE_MESSAGE(result.dimensions()[1] == 10, "Result dimension 1 did not match last dense layer");
}

BOOST_AUTO_TEST_CASE(test_net2) {
    std::cout << "Testing net creation" << std::endl;
    nn::Net<> net;

    int batchSize = 64;
    int inputX = 28;
    int inputY = 28;
    int numClasses = 10;
    bool useBias = true;
    // Basic MLP for testing MNSIT
    net.add(new nn::Dense<>(batchSize, inputX * inputY, 100, useBias))
       .add(new nn::Relu<>())
       .add(new nn::Dense<>(batchSize, 100, 100, useBias))
       .add(new nn::Relu<>())
       .add(new nn::Dense<>(batchSize, 100, 10, useBias))
       .add(new nn::Relu<>())
       .add(new nn::Softmax<>());

    Eigen::Tensor<float, 2> input(batchSize, 28 * 28);
    input.setRandom();

    auto startTime = std::chrono::system_clock::now();
    Eigen::Tensor<float, 2> result = net.forward<2, 2>(input);
    auto endTime = std::chrono::system_clock::now();

    std::chrono::duration<double> duration = endTime - startTime;
    std::cout << "A single forward of size: [" << batchSize << ", 28, 28] took: " << duration.count() << "s"
              << std::endl;

    Eigen::Tensor<float, 2> fakeLabels(batchSize, numClasses);
    fakeLabels.setZero();
    fakeLabels.setValues({{0, 0, 0, 1, 0, 0, 0, 0, 0, 0},
                          {0, 1, 0, 0, 0, 0, 0, 0, 0, 1}});

    net.backward<2>(fakeLabels);
}

BOOST_AUTO_TEST_CASE(test_regression) {
    std::cout << "Testing linear regression" << std::endl;
    nn::Net<> net;

    net.add(new nn::Dense<float, 2>(1, 1, 10, true));
    net.add(new nn::Relu<float, 2>());
    net.add(new nn::Dense<float, 2>(1, 10, 1, true));

    Eigen::Tensor<float, 2> input(1, 1);
    input.setRandom();

    auto startTime = std::chrono::system_clock::now();
    auto result = net.forward<2, 2>(input);
    auto endTime = std::chrono::system_clock::now();

    std::chrono::duration<double> duration = endTime - startTime;
    std::cout << "Regression took: " << duration.count() << "s" << std::endl;
}


// Copyright (C) 2015  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/matrix.h>
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <vector>
#include "../dnn/tensor_tools.h"

#include "tester.h"

// We only do these tests if CUDA is available to test in the first place.
#ifdef DLIB_USE_CUDA

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.cublas");

    void test_inv()
    {
        tt::tensor_rand rnd;
        dlib::tt::inv tinv;
        dlib::cuda::inv cinv;
        resizable_tensor minv1, minv2;
        for (int n = 1; n < 20; ++n)
        {
            print_spinner();
            resizable_tensor m(n,n);
            rnd.fill_uniform(m);

            tinv(m, minv1);
            cinv(m, minv2);
            matrix<float> mref = inv(mat(m));
            DLIB_TEST_MSG(mean(abs(mref-mat(minv1)))/mean(abs(mref)) < 1e-5, mean(abs(mref-mat(minv1)))/mean(abs(mref)) <<"  n: " << n);
            DLIB_TEST_MSG(mean(abs(mref-mat(minv2)))/mean(abs(mref)) < 1e-5, mean(abs(mref-mat(minv2)))/mean(abs(mref)) <<"  n: " << n);
        }
    }

    class cublas_tester : public tester
    {
    public:
        cublas_tester (
        ) :
            tester ("test_cublas",
                    "Runs tests on the cuBLAS bindings.")
        {}

        void perform_test (
        )
        {
            test_inv();
            {
                resizable_tensor a(4,3), b(3,4), c(3,3);

                c = 1;
                a = matrix_cast<float>(gaussian_randm(a.num_samples(),a.size()/a.num_samples()));
                b = matrix_cast<float>(gaussian_randm(b.num_samples(),b.size()/b.num_samples()));

                matrix<float> truth = 2*mat(c)+trans(mat(a))*trans(mat(b));

                a.async_copy_to_device(); b.async_copy_to_device(); c.async_copy_to_device();
                cuda::gemm(2, c, 1, a, true, b, true);
                DLIB_TEST(max(abs(truth-mat(c))) < 1e-6);
            }
            {
                resizable_tensor a(4,3), b(4,3), c(3,3);

                c = 1;
                a = matrix_cast<float>(gaussian_randm(a.num_samples(),a.size()/a.num_samples()));
                b = matrix_cast<float>(gaussian_randm(b.num_samples(),b.size()/b.num_samples()));

                matrix<float> truth = 2*mat(c)+trans(mat(a))*mat(b);

                a.async_copy_to_device(); b.async_copy_to_device(); c.async_copy_to_device();
                cuda::gemm(2, c, 1, a, true, b, false);
                DLIB_TEST(max(abs(truth-mat(c))) < 1e-6);
            }
            {
                resizable_tensor a(3,4), b(3,4), c(3,3);

                c = 1;
                a = matrix_cast<float>(gaussian_randm(a.num_samples(),a.size()/a.num_samples()));
                b = matrix_cast<float>(gaussian_randm(b.num_samples(),b.size()/b.num_samples()));

                matrix<float> truth = 2*mat(c)+mat(a)*trans(mat(b));

                a.async_copy_to_device(); b.async_copy_to_device(); c.async_copy_to_device();
                cuda::gemm(2, c, 1, a, false, b, true);
                DLIB_TEST(max(abs(truth-mat(c))) < 1e-6);
            }
            {
                resizable_tensor a(3,4), b(3,4), c(3,3);

                c = 1;
                a = matrix_cast<float>(gaussian_randm(a.num_samples(),a.size()/a.num_samples()));
                b = matrix_cast<float>(gaussian_randm(b.num_samples(),b.size()/b.num_samples()));

                matrix<float> truth = mat(c)+mat(a)*trans(mat(b));

                a.async_copy_to_device(); b.async_copy_to_device(); c.async_copy_to_device();
                cuda::gemm(1, c, 1, a, false, b, true);
                DLIB_TEST(max(abs(truth-mat(c))) < 1e-6);
            }
            {
                resizable_tensor a(3,4), b(4,3), c(3,3);

                c = 1;
                a = matrix_cast<float>(gaussian_randm(a.num_samples(),a.size()/a.num_samples()));
                b = matrix_cast<float>(gaussian_randm(b.num_samples(),b.size()/b.num_samples()));

                matrix<float> truth = 2*mat(c)+mat(a)*mat(b);

                a.async_copy_to_device(); b.async_copy_to_device(); c.async_copy_to_device();
                cuda::gemm(2, c, 1, a, false, b, false);
                DLIB_TEST(max(abs(truth-mat(c))) < 1e-6);
            }
            {
                resizable_tensor a(3,4), b(4,3), c(3,3);

                c = std::numeric_limits<float>::infinity();
                a = matrix_cast<float>(gaussian_randm(a.num_samples(),a.size()/a.num_samples()));
                b = matrix_cast<float>(gaussian_randm(b.num_samples(),b.size()/b.num_samples()));
                a.async_copy_to_device(); b.async_copy_to_device(); c.async_copy_to_device();

                matrix<float> truth = mat(a)*mat(b);

                cuda::gemm(0, c, 1, a, false, b, false);
                DLIB_TEST(max(abs(truth-mat(c))) < 1e-6);
            }
            {
                resizable_tensor a(3,4), b(4,4), c(3,4);

                c = 1;
                a = matrix_cast<float>(gaussian_randm(a.num_samples(),a.size()/a.num_samples()));
                b = matrix_cast<float>(gaussian_randm(b.num_samples(),b.size()/b.num_samples()));

                matrix<float> truth = 2*mat(c)+mat(a)*mat(b);

                cuda::gemm(2, c, 1, a, false, b, false);
                DLIB_TEST(get_rect(truth) == get_rect(mat(c)));
                DLIB_TEST(max(abs(truth-mat(c))) < 1e-6);
            }
            {
                resizable_tensor a(4,3), b(4,4), c(3,4);

                c = 1;
                a = matrix_cast<float>(gaussian_randm(a.num_samples(),a.size()/a.num_samples()));
                b = matrix_cast<float>(gaussian_randm(b.num_samples(),b.size()/b.num_samples()));

                matrix<float> truth = 2*mat(c)+trans(mat(a))*mat(b);

                cuda::gemm(2, c, 1, a, true, b, false);
                DLIB_TEST(get_rect(truth) == get_rect(mat(c)));
                DLIB_TEST(max(abs(truth-mat(c))) < 1e-6);
            }
            {
                resizable_tensor a(4,3), b(4,5), c(3,5);

                c = 1;
                a = matrix_cast<float>(gaussian_randm(a.num_samples(),a.size()/a.num_samples()));
                b = matrix_cast<float>(gaussian_randm(b.num_samples(),b.size()/b.num_samples()));

                matrix<float> truth = 2*mat(c)+trans(mat(a))*mat(b);

                cuda::gemm(2, c, 1, a, true, b, false);
                DLIB_TEST(get_rect(truth) == get_rect(mat(c)));
                DLIB_TEST(max(abs(truth-mat(c))) < 1e-6);
            }
            {
                resizable_tensor a(4,3), b(4,5), c(3,5);

                c = std::numeric_limits<float>::infinity();
                a = matrix_cast<float>(gaussian_randm(a.num_samples(),a.size()/a.num_samples()));
                b = matrix_cast<float>(gaussian_randm(b.num_samples(),b.size()/b.num_samples()));

                matrix<float> truth = trans(mat(a))*mat(b);

                cuda::gemm(0, c, 1, a, true, b, false);
                DLIB_TEST(get_rect(truth) == get_rect(mat(c)));
                DLIB_TEST(max(abs(truth-mat(c))) < 1e-6);
            }
        }
    } a;

}

#endif // DLIB_USE_CUDA


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019, University of Stuttgart
 *  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 the University of Stuttgart 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 OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Andreas Orthey */

#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/base/spaces/SO3StateSpace.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/base/SpaceInformation.h>
#include <ompl/base/StateSpace.h>
#include <ompl/multilevel/planners/qrrt/QRRT.h>
#include <iostream>
#include <boost/math/constants/constants.hpp>

namespace ob = ompl::base;
namespace og = ompl::geometric;
namespace om = ompl::multilevel;

using SE3State = ob::ScopedState<ob::SE3StateSpace>;
using SO3State = ob::ScopedState<ob::SO3StateSpace>;
using R3State = ob::ScopedState<ob::RealVectorStateSpace>;
const double pi = boost::math::constants::pi<double>();

// Path Planning on fiber bundle SE3 \rightarrow R3

bool isInCollision(double *val)
{
    const double &x = val[0] - 0.5;
    const double &y = val[1] - 0.5;
    const double &z = val[2] - 0.5;
    double d = sqrt(x * x + y * y + z * z);
    return (d > 0.2);
}

bool isStateValid_SE3(const ob::State *state)
{
    static auto SO3(std::make_shared<ob::SO3StateSpace>());
    static SO3State SO3id(SO3);
    SO3id->setIdentity();

    const auto *SE3state = state->as<ob::SE3StateSpace::StateType>();
    const auto *R3state = SE3state->as<ob::RealVectorStateSpace::StateType>(0);
    const ob::State *SO3state = SE3state->as<ob::SO3StateSpace::StateType>(1);
    const ob::State *SO3stateIdentity = SO3id->as<ob::SO3StateSpace::StateType>();

    double d = SO3->distance(SO3state, SO3stateIdentity);
    return isInCollision(R3state->values) && (d < pi / 4.0);
}

bool isStateValid_R3(const ob::State *state)
{
    const auto *R3 = state->as<ob::RealVectorStateSpace::StateType>();
    return isInCollision(R3->values);
}

int main()
{
    //############################################################################
    // Step 1: Setup planning problem using several quotient-spaces
    //############################################################################
    // Setup SE3
    auto SE3(std::make_shared<ob::SE3StateSpace>());
    ob::RealVectorBounds bounds(3);
    bounds.setLow(0);
    bounds.setHigh(1);
    SE3->setBounds(bounds);
    ob::SpaceInformationPtr si_SE3(std::make_shared<ob::SpaceInformation>(SE3));
    si_SE3->setStateValidityChecker(isStateValid_SE3);

    // Setup Quotient-Space R2
    auto R3(std::make_shared<ob::RealVectorStateSpace>(3));
    R3->setBounds(0, 1);
    ob::SpaceInformationPtr si_R3(std::make_shared<ob::SpaceInformation>(R3));
    si_R3->setStateValidityChecker(isStateValid_R3);

    // Create vector of spaceinformationptr (last one is original cspace)
    std::vector<ob::SpaceInformationPtr> si_vec;
    si_vec.push_back(si_R3);
    si_vec.push_back(si_SE3);

    // Define Planning Problem
    SE3State start_SE3(SE3);
    SE3State goal_SE3(SE3);
    start_SE3->setXYZ(0, 0, 0);
    start_SE3->rotation().setIdentity();
    goal_SE3->setXYZ(1, 1, 1);
    goal_SE3->rotation().setIdentity();

    ob::ProblemDefinitionPtr pdef = std::make_shared<ob::ProblemDefinition>(si_SE3);
    pdef->setStartAndGoalStates(start_SE3, goal_SE3);

    //############################################################################
    // Step 2: Do path planning as usual but with a sequence of
    // spaceinformationptr
    //############################################################################
    auto planner = std::make_shared<ompl::multilevel::QRRT>(si_vec);

    // Planner can be used as any other OMPL algorithm
    planner->setProblemDefinition(pdef);
    planner->setup();

    ob::PlannerStatus solved = planner->ob::Planner::solve(1.0);

    if (solved)
    {
        std::cout << std::string(80, '-') << std::endl;
        std::cout << "Bundle-Space Path (SE3):" << std::endl;
        std::cout << std::string(80, '-') << std::endl;
        pdef->getSolutionPath()->print(std::cout);

        std::cout << std::string(80, '-') << std::endl;
        std::cout << "Base-Space Path (R3)   :" << std::endl;
        std::cout << std::string(80, '-') << std::endl;
        const ob::ProblemDefinitionPtr pdefR3 = planner->getProblemDefinition(0);
        pdefR3->getSolutionPath()->print(std::cout);
    }
    return 0;
}


#ifndef MUSE_MCL_2D_GRIDMAPS_LASER_CONVEX_HULL_HPP
#define MUSE_MCL_2D_GRIDMAPS_LASER_CONVEX_HULL_HPP

#include <cslibs_plugins_data/types/laserscan.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

namespace muse_mcl_2d_gridmaps {
namespace utilty {
namespace bg = boost::geometry;

struct Point
{
    Point(double x = 0, double y = 0, std::size_t index = 0) :
        x(x),
        y(y),
        index(index)
    {
    }

    Point(const Eigen::Vector2d &p, const std::size_t index) :
      x(p(0)),
      y(p(1)),
      index(index)
    {
    }

    double      x;
    double      y;
    std::size_t index;
} __attribute__ ((aligned (32)));

using polygon_t = boost::geometry::model::polygon<Point>;
using rays_t    = cslibs_plugins_data::types::Laserscan2::rays_t;
using ray_t     = cslibs_plugins_data::types::Laserscan2::Ray;
using laserscan_t = cslibs_plugins_data::types::Laserscan2;

inline void convexHull(const laserscan_t& scan,
                       std::vector<std::size_t> &indices)
{
    const rays_t rays = scan.getRays();
    const std::size_t size = rays.size();
    const double range_min = scan.getLinearMin();
    const double range_max = scan.getLinearMax();
    const double angle_min = scan.getAngularMin();
    const double angle_max = scan.getAngularMax();

    auto valid = [range_min, range_max, angle_min, angle_max](const ray_t &r){
        return r.valid()
            && r.angle >= angle_min && r.angle <= angle_max
            && r.range >= range_min && r.range <= range_max;
    };

    polygon_t scan_poly;
    polygon_t hull_poly;
    scan_poly.outer().reserve(size);
    for(std::size_t i = 0 ; i < size ; ++i) {
        const auto &r = rays[i];
        if(valid(r)) {
            scan_polygon.outer().emplace_back(Point(r.end_point(), i));
        }
    }
    boost::geometry::convex_hull(scan_poly, hull_poly);

    indices.reserve(hull_poly.outer().size());
    for(const auto &p : hull_poly.outer()) {
      indices.emplace_back(p.index);
    }
}
}

#endif // MUSE_MCL_2D_GRIDMAPS_LASER_CONVEX_HULL_HPP