#include <cassert>

// Before doing anything else, grab in the definitions of the test
// space and trial spaces that make up the finite element system.
#include <qdove/base/test_space.h>
#include <qdove/base/trial_space.h>
#include <qdove/materials/constants.h>

// Use the *solution* to a fick equation as a material function
#include <qdove/models/fick.h>

// Start by solving Schroedinger's problem
#include <qdove/models/schroedinger.h>

// and next by solving Poissons's problem
#include <qdove/models/poisson.h>

// Fermi-Dirac statistic play a role later here
#include <qdove/models/statistics.h>

// Also make use of the predefined generalised eigenspectrum system -
// we need this for the Schroedinger problem.
#include <qdove/generic_linear_algebra/eigenspectrum_system.h>

// Also make use of the predefined linear algebra system - we need
// this for the Poisson problem.
#include <qdove/generic_linear_algebra/linear_algebra_system.h>

// Next up are some deal.II objects that have not been generalised
// away yet...
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/numerics/data_out.h>

// The purpose of this example is to have a working (dimensionless)
// Schroedinger-Poisson solver that can be generalised piecewise to
// the qdove library.
//
template<int dim>
class SelfConsistentProblem
{
public:
  SelfConsistentProblem (dealii::Triangulation<dim> &triangulation);
  ~SelfConsistentProblem ();

  void write_gnuplot (const dealii::PETScWrappers::Vector &vector,
          const std::string                   &name,
          const unsigned int                   cycle);
  void run ();

private:
  // The description of the finite element basis
  qdove::TrialSpace<dim> trial_space;

  // Geometry description and boundary constraints
  qdove::TestSpace<dim> test_space;

  // Fick's problem, which is used to obtain a material profile.
  qdove::Fick::Solution<1> fick_solution;

  // The eigenspectrum system that will be solved by SLEPc
  qdove::Schroedinger::Problem<dim> schroedinger_problem;

  // The system of equations that will be solved by PETSc
  qdove::Poisson::Problem<dim> poisson_problem;

  // The eigenpairs from schroedinger's problem
  std::vector<dealii::PETScWrappers::Vector> eigenvectors;
  std::vector<double>                        eigenvalues;

  // The solution from poisson's problem
  dealii::PETScWrappers::Vector solution;

};

template<int dim>
SelfConsistentProblem<dim>::SelfConsistentProblem (dealii::Triangulation<dim> &triangulation)
  :
  trial_space (triangulation),
  test_space (trial_space),
  fick_solution (trial_space, test_space),
  schroedinger_problem (trial_space, test_space, 10),
  poisson_problem (trial_space, test_space)
{}

template<int dim>
SelfConsistentProblem<dim>::~SelfConsistentProblem ()
{}

template<int dim>
void
SelfConsistentProblem<dim>::write_gnuplot (const dealii::PETScWrappers::Vector &vector,
             const std::string                   &name,
             const unsigned int                   cycle)
{
  // Output a vector to gnuplot style file.
  std::ostringstream filename;
  filename << "solution-" << name << "-" << cycle << ".gpl";
  std::ofstream output (filename.str ().c_str ());

  dealii::DataOut<dim> data_out;
  data_out.attach_dof_handler (test_space.dofs ());
  data_out.add_data_vector (vector, name);

  // generate default patches and output.
  data_out.build_patches ();
  data_out.write_gnuplot (output);
}

template<int dim>
void
SelfConsistentProblem<dim>::run ()
{
  std::cout << "Test space:" << std::endl
      << "   Finite element type:          "
      << test_space.fe ().get_name ()
      << std::endl
      << "   Number of degrees of freedom: "
      << test_space.n_dofs ()
      << std::endl;

  const double radius       = 50e-10;
  const double band_edge    = 0.5 * qdove::E0;
  const double fermi_energy = band_edge * 0.95; //for simplicity we take the bond energy to be 5% below the band edge
  const double doping       = 1e25;

  // Setup a material function:
  fick_solution.reinit ();
  fick_solution.set_initial_length (0.5*radius);
  fick_solution.set_initial_height (1.);
  fick_solution.set_initial_rate(1e-10);

  dealii::PETScWrappers::Vector material_function (test_space.n_dofs ());
  fick_solution.interpolate_analytic_solution (material_function);

  // Setup an initial potential based on the material function:
  dealii::PETScWrappers::Vector initial_potential (test_space.n_dofs ());
  for (std::size_t i=0; i<initial_potential.size(); ++i)
    initial_potential[i] = -material_function[i] * band_edge + band_edge;
  write_gnuplot (initial_potential, "initial_potential", 0);

  // set up the effective mass, to keep things simple assume a constant effective mass
  dealii::PETScWrappers::Vector eff_mass (test_space.n_dofs ());
  for (std::size_t i=0; i<eff_mass.size(); ++i)
    eff_mass[i] = qdove::mstar_GaAs;
  write_gnuplot (eff_mass, "effective_mass", 0);

  // Setup the kinetic energy term
  dealii::PETScWrappers::Vector kinetic (test_space.n_dofs ());
  for (std::size_t i=0; i<eff_mass.size(); ++i)
    kinetic[i] = (qdove::HBAR*qdove::HBAR) / (2.*eff_mass[i]*qdove::M0);
  write_gnuplot (kinetic, "kinetic", 0);

  // set up the initial guess for the electron potential (i.e. electrostatic potential multiplied by elementary charge)
  dealii::PETScWrappers::Vector potential (test_space.n_dofs ());
  potential = initial_potential;
  potential *= 0.9; //start with no space charge region

  //
  // Main iteration Schroedinger <-> Poisson
  //
  for (std::size_t cycle = 0; cycle < 100; ++cycle)
  {
    write_gnuplot (potential, "potential", cycle);

    // Get started on Schroedinger's problem
    std::cout << "Schroedinger's problem:" << std::endl;
    schroedinger_problem.reinit ();
    schroedinger_problem.assemble (kinetic, potential);
    schroedinger_problem.solve ();
    schroedinger_problem.get_solution_eigenpairs (eigenvalues, eigenvectors);
    write_gnuplot (eigenvectors[0], "electron_function", cycle);

    // output
    std::cout << "   Eigenvalues:                  ";
    for (unsigned int i=0; i<eigenvalues.size (); ++i)
      std::cout << eigenvalues[i] << " ";
    std::cout << std::endl;

    std::cout << "   Scaled values (meV):           ";
    for (unsigned int i=0; i<eigenvalues.size (); ++i)
      std::cout << eigenvalues[i]/(1e-03*qdove::E0) << " ";
    std::cout << std::endl;

    // Make the density of states (dos)
    dealii::PETScWrappers::Vector dos (test_space.n_dofs ());
    qdove::FermiDirac::compute_density_of_states (eff_mass, dos);

    // and then the density
    dealii::PETScWrappers::Vector density (test_space.n_dofs());
    qdove::FermiDirac::compute_number_density (eigenvectors, eigenvalues, fermi_energy, dos, density);
    write_gnuplot (density, "density", cycle);

    // Set up RHS for Poisson's equation.
    //
    // First copy the density over, add the doping profile, and thten
    // multiply by 4*\pi*e*e/permittivity
    dealii::PETScWrappers::Vector rho (density);
    for (std::size_t i=0; i<solution.size (); ++i)
      if (potential[i] > fermi_energy)
        rho[i] = -doping;
    rho *= 4.0 * qdove::PI * qdove::E0 * qdove::E0 / (qdove::permittivity_GaAs);
    write_gnuplot (rho, "rho", cycle);

    // Solve Poisson:
    std::cout << "Poisson's problem:" << std::endl;
    poisson_problem.reinit ();
    poisson_problem.assemble (rho);
    poisson_problem.solve ();
    poisson_problem.get_solution_vector (solution);
    write_gnuplot (solution, "poisson_solution", cycle);

    // Update potential:
    double max_update = 0;
    for (std::size_t i=0; i<solution.size(); ++i)
    {
      double old_value = potential[i] - initial_potential[i];
      max_update = std::max(max_update, std::fabs(solution[i] - old_value));
    }

    double cutoff_update_value = 0.02 * qdove::E0;
    double alpha = (max_update > cutoff_update_value)
      ? cutoff_update_value / max_update
      : 1.0; // update by at most 10 mV for stability reasons

    alpha = 0.1;
    std::cout << "alpha: " << alpha << std::endl;

    for (std::size_t i=0; i<potential.size(); ++i)
    {
      double old_value = potential[i] - 0.9 * initial_potential[i];
      double update    = solution[i] - old_value;

      potential[i] += alpha * update;
    }
  }
}

int main (int argc, char **argv)
{
  try
    {
      dealii::Utilities::MPI::MPI_InitFinalize mpi_initialization (argc, argv, 1);
      {
  // Create a grid
  dealii::Triangulation<1> triangulation;
  dealii::GridGenerator::hyper_cube (triangulation, -50e-10, 50e-10);
  triangulation.refine_global (9);

  // Run Schroedinger's problem on that grid
  SelfConsistentProblem<1> self_consistent_problem (triangulation);
  self_consistent_problem.run ();
      }
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


#include <ceres/ceres.h>
#include <Eigen/Core>
#include <unsupported/Eigen/MatrixFunctions>

#include "wave/optimization/ceres/odom_gp/point_to_plane_gp.hpp"
#include "wave/optimization/ceres/odom_gp/point_to_line_gp.hpp"
#include "wave/wave_test.hpp"
#include "wave/utils/math.hpp"
#include "wave/geometry_og/transformation.hpp"
#include "wave/kinematics/constant_velocity_gp_prior.hpp"

// This is a numerical check for some residuals where ceres gradient checker is not helpful

namespace wave {

TEST(point_to_line, jacobian) {

    double ptA[3] = {1, 1, 0};
    double ptB[3] = {1, 3, -4};
    double pt[3] = {1, 2, -4};

    const double delta_T = 0.5;
    const double **params;
    params = new const double *[4];

    Transformation<Eigen::Matrix<double, 3, 4>, false> T_k, T_kp1;
    Vec6 vel_k, vel_kp1;

    params[0] = T_k.storage.data();
    params[1] = T_kp1.storage.data();
    params[2] = vel_k.data();
    params[3] = vel_kp1.data();

    T_k.setIdentity();
    vel_k << 0.1, -0.1, 0.1, 5, 1, -1;
    vel_kp1 = vel_k;
    T_kp1 = T_k;
    T_kp1.manifoldPlus(delta_T * vel_k);

    double zero = 0;
    double tau = 0.34;
    Mat6 Qc = Mat6::Identity();
    Mat6 inv_Qc = Qc.inverse();

    wave_kinematics::ConstantVelocityPrior motion_prior(zero, delta_T, &tau, Qc, inv_Qc);

    Eigen::Matrix<double, 12, 12> hat, candle;

    SE3PointToLineGPObjects objects;

    motion_prior.calculateStuff(hat, candle);

    objects.hat = hat;
    objects.candle = candle;

    ceres::CostFunction *cost_function = new SE3PointToLineGP(pt,
                                                              ptA,
                                                              ptB,
                                                              objects,
                                                              Mat3::Identity(),
                                                              true);
    double **jacobian;
    jacobian = new double *[4];
    jacobian[0] = new double[24];
    jacobian[1] = new double[24];
    jacobian[2] = new double[12];
    jacobian[3] = new double[12];

    Vec2 op_result;

    cost_function->Evaluate(params, op_result.data(), jacobian);

    double const step_size = 1e-9;
    Transformation<Eigen::Matrix<double, 3, 4>, false> Tk_perturbed, Tkp1_perturbed;
    Vec6 vel_k_perturbed, vel_kp1_perturbed;

    Tk_perturbed = T_k;
    Tkp1_perturbed = T_kp1;
    vel_k_perturbed = vel_k;
    vel_kp1_perturbed = vel_kp1;

    params[0] = Tk_perturbed.storage.data();
    params[1] = Tkp1_perturbed.storage.data();
    params[2] = vel_k_perturbed.data();
    params[3] = vel_kp1_perturbed.data();

    std::vector<Eigen::Matrix<double, 2, 6>> an_jacs, num_jacs;
    an_jacs.resize(4);
    num_jacs.resize(4);

    Vec6 delta;
    delta.setZero();

    Vec2 result;
    Vec2 diff;

    double inv_step = 1.0 / step_size;

    for (uint32_t i = 0; i < 6; i++) {
        delta(i) = step_size;
        // First parameter
        Tk_perturbed.manifoldPlus(delta);
        cost_function->Evaluate(params, result.data(), nullptr);
        diff = result - op_result;
        num_jacs.at(0).block<2,1>(0,i) = inv_step * diff;
        Tk_perturbed = (T_k);
        // Second parameter
        Tkp1_perturbed.manifoldPlus(delta);
        cost_function->Evaluate(params, result.data(), nullptr);
        diff = result - op_result;
        num_jacs.at(1).block<2,1>(0,i) = inv_step * diff;
        Tkp1_perturbed = (T_kp1);
        // Third parameter
        vel_k_perturbed = vel_k_perturbed + delta;
        cost_function->Evaluate(params, result.data(), nullptr);
        diff = result - op_result;
        num_jacs.at(2).block<2,1>(0,i) = inv_step * diff;
        vel_k_perturbed = vel_k;
        // Fourth parameter
        vel_kp1_perturbed = vel_kp1_perturbed + delta;
        cost_function->Evaluate(params, result.data(), nullptr);
        diff = result - op_result;
        num_jacs.at(3).block<2,1>(0,i) = inv_step * diff;
        vel_kp1_perturbed = vel_kp1;

        delta.setZero();
    }

    // now get the analytical jacobians
    Eigen::Map<Eigen::Matrix<double, 2, 12, Eigen::RowMajor>> an_jac_1(jacobian[0], 2, 12);
    an_jacs.at(0) = an_jac_1.block<2,6>(0,0);

    Eigen::Map<Eigen::Matrix<double, 2, 12, Eigen::RowMajor>> an_jac_2(jacobian[1], 2, 12);
    an_jacs.at(1) = an_jac_2.block<2,6>(0,0);

    Eigen::Map<Eigen::Matrix<double, 2, 6, Eigen::RowMajor>> an_jac_3(jacobian[2], 2, 6);
    an_jacs.at(2) = an_jac_3;

    Eigen::Map<Eigen::Matrix<double, 2, 6, Eigen::RowMajor>> an_jac_4(jacobian[3], 2, 6);
    an_jacs.at(3) = an_jac_4;

    for (uint32_t i = 0; i < 4; i++) {
        double err = (num_jacs.at(i) - an_jacs.at(i)).norm();
        if (err > 1e-10) {
            std::cout << "Index " << i << " with error = " << err << std::endl
                     << "Numerical: " << std::endl << num_jacs.at(i) << std::endl
                     << "Analytical:" << std::endl << an_jacs.at(i) << std::endl << std::endl;
        }
        EXPECT_NEAR(err, 0.0, 1e-6);
    }
}

TEST(point_to_plane, jacobian) {

    double ptA[3] = {1, 1, 0};
    double ptB[3] = {1, 3, -4};
    double ptC[3] = {4, -1, 0};
    double pt[3] = {1, 2, -4};

    const double delta_T = 0.5;
    const double **params;
    params = new const double *[4];

    Transformation<Eigen::Matrix<double, 3, 4>, false> T_k, T_kp1;
    Vec6 vel_k, vel_kp1;

    params[0] = T_k.storage.data();
    params[1] = T_kp1.storage.data();
    params[2] = vel_k.data();
    params[3] = vel_kp1.data();

    T_k.setIdentity();
    vel_k << 0.1, -0.1, 0.2, 5, 1, -1;
    vel_kp1 = vel_k;
    T_kp1 = (T_k);
    T_kp1.manifoldPlus(delta_T * vel_k);

    double zero = 0;
    double tau = 0.34;
    Mat6 Qc = Mat6::Identity();
    Mat6 inv_Qc = Qc.inverse();

    wave_kinematics::ConstantVelocityPrior motion_prior(zero, delta_T, &tau, Qc, inv_Qc);

    Eigen::Matrix<double, 12, 12> hat, candle;

    motion_prior.calculateStuff(hat, candle);

    SE3PointToPlaneGPObjects objects;
    objects.hat = hat;
    objects.candle = candle;

    ceres::CostFunction *cost_function = new SE3PointToPlaneGP(pt,
                                                              ptA,
                                                              ptB,
                                                              ptC,
                                                              objects,
                                                              Mat3::Identity(),
                                                              false);
    double **jacobian;
    jacobian = new double *[4];
    jacobian[0] = new double[12];
    jacobian[1] = new double[12];
    jacobian[2] = new double[6];
    jacobian[3] = new double[6];

    Eigen::Matrix<double, 1, 1> op_result;

    cost_function->Evaluate(params, op_result.data(), jacobian);

    double const step_size = 1e-9;
    Transformation<Eigen::Matrix<double, 3, 4>> Tk_perturbed, Tkp1_perturbed;
    Vec6 vel_k_perturbed, vel_kp1_perturbed;

    Tk_perturbed = (T_k);
    Tkp1_perturbed = (T_kp1);
    vel_k_perturbed = vel_k;
    vel_kp1_perturbed = vel_kp1;

    params[0] = Tk_perturbed.storage.data();
    params[1] = Tkp1_perturbed.storage.data();
    params[2] = vel_k_perturbed.data();
    params[3] = vel_kp1_perturbed.data();

    std::vector<Eigen::Matrix<double, 1, 6>> an_jacs, num_jacs;
    an_jacs.resize(4);
    num_jacs.resize(4);

    Vec6 delta;
    delta.setZero();

    Eigen::Matrix<double, 1, 1> diff, result;

    double inv_step = 1.0 / step_size;

    for (uint32_t i = 0; i < 6; i++) {
        delta(i) = step_size;
        // First parameter
        Tk_perturbed.manifoldPlus(delta);
        cost_function->Evaluate(params, result.data(), nullptr);
        diff = result - op_result;
        num_jacs.at(0).block<1,1>(0,i) = inv_step * diff;
        Tk_perturbed = (T_k);
        // Second parameter
        Tkp1_perturbed.manifoldPlus(delta);
        cost_function->Evaluate(params, result.data(), nullptr);
        diff = result - op_result;
        num_jacs.at(1).block<1,1>(0,i) = inv_step * diff;
        Tkp1_perturbed = (T_kp1);
        // Third parameter
        vel_k_perturbed = vel_k_perturbed + delta;
        cost_function->Evaluate(params, result.data(), nullptr);
        diff = result - op_result;
        num_jacs.at(2).block<1,1>(0,i) = inv_step * diff;
        vel_k_perturbed = vel_k;
        // Fourth parameter
        vel_kp1_perturbed = vel_kp1_perturbed + delta;
        cost_function->Evaluate(params, result.data(), nullptr);
        diff = result - op_result;
        num_jacs.at(3).block<1,1>(0,i) = inv_step * diff;
        vel_kp1_perturbed = vel_kp1;

        delta.setZero();
    }

    // now get the analytical jacobians
    Eigen::Map<Eigen::Matrix<double, 1, 12, Eigen::RowMajor>> an_jac_1(jacobian[0], 1, 12);
    an_jacs.at(0) = an_jac_1.block<1,6>(0,0);

    Eigen::Map<Eigen::Matrix<double, 1, 12, Eigen::RowMajor>> an_jac_2(jacobian[1], 1, 12);
    an_jacs.at(1) = an_jac_2.block<1,6>(0,0);

    Eigen::Map<Eigen::Matrix<double, 1, 6, Eigen::RowMajor>> an_jac_3(jacobian[2], 1, 6);
    an_jacs.at(2) = an_jac_3;

    Eigen::Map<Eigen::Matrix<double, 1, 6, Eigen::RowMajor>> an_jac_4(jacobian[3], 1, 6);
    an_jacs.at(3) = an_jac_4;

    for (uint32_t i = 0; i < 4; i++) {
        double err = (num_jacs.at(i) - an_jacs.at(i)).norm();
        std::cout << "Index " << i << " has error: " << err << std::endl
                  << "Numerical: " << std::endl << num_jacs.at(i) << std::endl
                  << "Analytical:" << std::endl << an_jacs.at(i) << std::endl << std::endl;
        EXPECT_NEAR(err, 0.0, 1e-6);
    }
}

}


#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <limits>
#include <mimkl/data_structures.hpp>
#include <mimkl/definitions.hpp>
#include <mimkl/io.hpp>
#include <mimkl/kernels.hpp>
#include <mimkl/linear_algebra.hpp>
#include <mimkl/models/umkl_knn.hpp>
#include <spdlog/spdlog.h>
//#include <mimkl/models.hpp>
#include <mimkl/kernels_handler.hpp>
#include <mimkl/utilities.hpp>
#include <numeric>
#include <spdlog/fmt/ostr.h>

using dlib::mat;
using mimkl::data_structures::DataFrame;
using mimkl::data_structures::indexing_from_vector;
using mimkl::data_structures::range;
using mimkl::definitions::Indexing;
using mimkl::utilities::check_invocable;
using mimkl::utilities::print_type;

using mimkl::kernels_handler::KernelsHandler;
using mimkl::models::UMKLKNN;

//#define SPDLOG_DEBUG_ON
//#define SPDLOG_TRACE_ON

// Compile time log levels
// define SPDLOG_DEBUG_ON or SPDLOG_TRACE_ON
// SPDLOG_TRACE(console, "Enabled only #ifdef SPDLOG_TRACE_ON..{} ,{}",
// 1, 3.23);  SPDLOG_DEBUG(console, "Enabled only #ifdef SPDLOG_DEBUG_ON.. {}
// ,{}", 1, 3.23);

typedef std::function<MATRIX(double)(const MATRIX(double) &, const MATRIX(double) &)>
InducedFunction;
typedef std::vector<InducedFunction> Function_vec;
typedef std::vector<MATRIX(double)> Matrix_vec;

MATRIX(double)
test_umkl_knn(const Function_vec function_vec,
              const Matrix_vec matrix_vec,
              const Matrix_vec matrix_vec_test,
              const bool precompute,
              const bool trace_normalization,
              std::shared_ptr<spdlog::logger> console,
              MATRIX(double) X,
              MATRIX(double) Y)
{
    console->info("\nRun with precompute = {} an trace_normalization = {}\n\n",
                  precompute, trace_normalization);

    console->info("rows X,Y : {},{}", X.rows(), Y.rows());
    console->info("sizes matric_vec : {},{}, sizes matric_vec_test : {},{}",
                  matrix_vec[0].rows(), matrix_vec[0].cols(),
                  matrix_vec_test[0].rows(), matrix_vec_test[0].cols());

    console->info("\nMatricial construction\n");

    UMKLKNN<double, InducedFunction> umkl_mat =
    UMKLKNN<double, InducedFunction>(matrix_vec, precompute, trace_normalization);
    console->info("constructor of umkl_mat done");

    umkl_mat.set_k(5);
    umkl_mat.fit(matrix_vec);
    MATRIX(double) beta_mat_1 = umkl_mat.get_beta();
    try
    {
        umkl_mat.fit(matrix_vec_test);
        MATRIX(double) beta_mat_2 = umkl_mat.get_beta();
        console->error("not caught error on matricial non-squared.");
    }
    catch (const std::exception &e)
    {
        console->warn("caught error on matricial non-squared. message: \n{}",
                      e.what());
    };
    try
    {
        umkl_mat.fit(X);
        MATRIX(double) beta_mat_fun = umkl_mat.get_beta();
        console->error("not caught error on matricial set_lhs.");
    }
    catch (const std::exception &e)
    {
        console->warn("caught error on matricial set_lhs. message: \n{}",
                      e.what());
    };
    // retrain
    umkl_mat.fit(matrix_vec);

    console->info("\nFunctional construction\n");
    UMKLKNN<double, InducedFunction> umkl(function_vec, precompute,
                                          trace_normalization);
    console->info("constructor of umkl_fun done");

    umkl.set_k(5);
    umkl.fit(matrix_vec);
    MATRIX(double) beta_fun_mat = umkl.get_beta();
    umkl.fit(X);
    MATRIX(double) beta_fun_1 = umkl.get_beta();
    // retrain
    umkl.fit(Y);
    MATRIX(double) beta_fun_2 = umkl.get_beta();
    try
    {
        umkl_mat.fit(matrix_vec_test);
        MATRIX(double) beta_fun_mat_2 = umkl_mat.get_beta();
        console->error(
        "not caught error on functional with non-squared matricial.");
    }
    catch (const std::exception &e)
    {
        console->warn("caught error on functional with non-squared matricial. "
                      "message: \n{}",
                      e.what());
    };

    assert(((beta_mat_1 - beta_fun_mat).norm() == 0) && "beta_mat_1");
    assert(((beta_fun_1 - beta_fun_mat).norm() == 0) && "beta_fun_1");

    return beta_fun_mat;
}

int main(int argc, char **argv)
{

    // Runtime log levels
    spdlog::set_level(spdlog::level::debug); // Set global log level to info
    auto console = spdlog::stdout_color_mt("console");

    const Index rows = 6; // 3 to reproduce single member in class  error
    const Index dims = 2;

    //
    //  ////
    //  MATRIX(double) X; //(rows, 2)

    //  std::string path = "../../data/simple_csv2.csv";
    //  console->info("path: {}",path);
    //  X = mimkl::io::eigen_matrix_from_csv<MATRIX(double)>(path, ','); //
    //  weird
    //  bug maybe here: floating point error that doesn't throw??
    //  ///////

    MATRIX(double) X(rows, dims);
    //  //	X << 1., 1., 3., 1., 1., 2.;
    //  X << 1., 1., 3., 1., 1., 4., 3., 2.;
    X << 1., 1., 3., 1., 1., 4., 3., 2., 1., -1., 3., -1.;
    //  ///////
    console->info("X\n{}", X);

    std::vector<std::string> labels;
    labels.reserve(rows);
    labels.push_back("a");
    labels.push_back("b");
    labels.push_back("a");
    labels.push_back("b");
    labels.push_back("c");
    labels.push_back("c");

    Eigen::SparseMatrix<double> L(2, 2);
    mimkl::linear_algebra::fill_sparse_diagonal(L, 1.0);

    Eigen::SparseMatrix<double> L1(dims, dims);
    typedef Eigen::Triplet<double> TripletDouble; // (row,col,coef)
    std::vector<TripletDouble> triplet_list;
    triplet_list.reserve(4);
    triplet_list.push_back(TripletDouble(0, 1, 1.));
    //  triplet_list.push_back(TripletDouble(1, 2, 1.));
    triplet_list.push_back(TripletDouble(1, 0, 1.));
    //  triplet_list.push_back(TripletDouble(2, 1, 1.));
    L1.setFromTriplets(triplet_list.begin(), triplet_list.end());

    std::vector<Eigen::SparseMatrix<double>> inducer_vec;
    inducer_vec.reserve(2);
    inducer_vec.push_back(L);
    inducer_vec.push_back(L1);

    typedef std::function<MATRIX(double)(const MATRIX(double) &, const MATRIX(double) &,
                                         const Eigen::SparseMatrix<double>)>
    InducerFunction;
    typedef std::function<MATRIX(double)(const MATRIX(double) &,
                                         const MATRIX(double) &)>
    InducedFunction;

    const double degree = 1.;
    const double offset = 0.;

    auto lambda_logger = spdlog::stdout_color_mt("lambda");
    InducerFunction k_poly =
    [degree, offset](const MATRIX(double) & lhs, const MATRIX(double) & rhs,
                     const Eigen::SparseMatrix<double> inducer) {
        //	 spdlog::get("lambda")->debug("before actual inducer function
        // call" 			 "lhs rows {}  cols {} \n"
        //			 "rhs rows {}  cols {} \n"
        //			 "kernel rows {}  cols {} \n"
        //			 "inducer rows {}  cols {} \n",
        //			 lhs.rows(), lhs.cols(),
        //			 rhs.rows(), rhs.cols(),
        //			 kernel_matrix.rows(), kernel_matrix.cols(),
        //			 inducer.rows(), inducer.cols()
        //			 );
        return mimkl::induction::induce_polynomial_kernel<MATRIX(double)>(
        lhs, rhs, inducer, degree, offset);
    };

    std::vector<InducedFunction> function_vec =
    mimkl::induction::inducer_combination(k_poly, inducer_vec);
    //// 	==inducer_combination:
    //	  for (Eigen::SparseMatrix<double> inducer : inducer_vec) {
    //		  function_vec.push_back(
    //			[&,inducer](const MATRIX(double) &lhs,
    //					  const MATRIX(double)&rhs,
    //					  const MATRIX(double) &kernel_matrix) {
    //			  k_poly(lhs, rhs, kernel_matrix, inducer);
    //			});
    //
    //	  }
    //
    //  MATRIX(double) K2(rows, rows);
    //  function_vec[0](X, X, K2);
    //  console->info("a kernel from function:\n{}", K2);
    //
    //  MATRIX(double) K3(rows, rows);
    //  function_vec[1](X, X, K3);
    //  console->info("another kernel from function:\n{}", K3);

    //  COLUMN(bool) all_true_I_swear = COLUMN(bool)::Constant(rows, true);
    //  console->info("bool:\n{}",all_true_I_swear);
    //  COLUMN(double) double_trouble = all_true_I_swear.cast<double>();
    //  console->info("double:\n{}",double_trouble);
    //
    //  console->info("mult by
    //  bool:\n{}",all_true_I_swear.cast<double>().array()
    //  * X.col(0).array());
    ////  console->info("mult by bool:\n{}",all_true_I_swear.cast<int>().array()
    ///*
    /// X.col(0).array());

    //////////
    MATRIX(double) K2 = function_vec[0](X, X);
    console->info("a kernel from function:\n{}", K2);

    MATRIX(double) K3 = function_vec[1](X, X);
    console->info("another kernel from function:\n{}", K3);

    std::vector<MATRIX(double)> matrix_vec(2);
    matrix_vec[0] = K2;
    matrix_vec[1] = K3;

    MATRIX(double) Y(7, 2);
    Y << 1., 2., 3., 4., 5., 6., 7., 8., 9., 10., 11., 12., 13., 14.;
    console->info("Y\n{}", Y);
    std::vector<MATRIX(double)> matrix_vec_test(2);

    matrix_vec_test[0] = function_vec[0](X, Y);
    matrix_vec_test[1] = function_vec[1](X, Y);

    //////////
    //  UMKLKNN<double, InducedFunction> umkl_mat(matrix_vec,true, true);
    //  UMKLKNN<double, InducedFunction> umkl_mfun(function_vec,true, true);

    MATRIX(double)
    tt = test_umkl_knn(function_vec, matrix_vec, matrix_vec_test, true, true,
                       console, X, Y);
    MATRIX(double)
    tf = test_umkl_knn(function_vec, matrix_vec, matrix_vec_test, true, false,
                       console, X, Y);
    MATRIX(double)
    ft = test_umkl_knn(function_vec, matrix_vec, matrix_vec_test, false, true,
                       console, X, Y);
    MATRIX(double)
    ff = test_umkl_knn(function_vec, matrix_vec, matrix_vec_test, false, false,
                       console, X, Y);

    assert(((tt - ft).norm() == 0) && "trace_normalization true");
    assert(((ff - tf).norm() == 0) && "trace_normalization false");

    console->info("trace_normalization true: \n{}", tt);
    console->info("trace_normalization false: \n{}", tf);
    return EXIT_SUCCESS;
}


#include <iostream>
#include <opencv2/opencv.hpp>
#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <vector>

using namespace std;
using namespace cv;

// 相机内参
#define L_FX 7.188560000000e+02
#define L_FY 7.188560000000e+02
#define L_CX 6.071928000000e+02
#define L_CY 1.852157000000e+02

#define R_FX 7.188560000000e+02
#define R_FY 7.188560000000e+02
#define R_CX 6.071928000000e+02
#define R_CY 1.852157000000e+02

#define FB   3.861448000000e+02

#define IMG_W 1024.0
#define IMG_H 768.0

void prefilterXSobel(const cv::Mat& src, cv::Mat& dst, int ftzero);

template <typename T> void filterSpecklesImpl(cv::Mat& img, int newVal, int maxSpeckleSize, int maxDiff, cv::Mat& _buf);

void initColor(void);

typedef struct _Color
{
//public:
    unsigned char r;
    unsigned char g;
    unsigned char b;

    /*
    _Color(unsigned char _r,unsigned char _g,unsigned char _b)
    {    r=_r;g=_g;b=_b;  }
    */

}Color;

vector<Color> HueCircle;

int main(int argc, char* argv[])
{
    cout<<"Kitti Streo Test."<<endl;
    cout<<"Complied at "<<__TIME__<<", "<<__DATE__<<"."<<endl;

    if(argc!=4)
    {
        cout<<"Usage: "<<argv[0]<<" img_left img_right img_dis"<<endl;
        return 1;
    }

    // 读入左右双目图像
    Mat imgLeft =imread(argv[1],IMREAD_GRAYSCALE);
    Mat imgRight=imread(argv[2],IMREAD_GRAYSCALE);

    if(imgLeft.empty())
    {
        cout<<"Error: img_left "<<argv[1]<<" is empty!"<<endl;
        return 2;
    }

    if(imgRight.empty())
    {
        cout<<"Error: img_left "<<argv[1]<<" is empty!"<<endl;
        return 2;
    }

    imshow("img_left",imgLeft);
    imshow("img_right",imgRight);

    waitKey(100);

    Mat imgLefted,imgRighted;

    // 最小视差
    int mindisparity = 0;
    // 视差搜索范围长度
	int ndisparities = 64;
    // SAD代价计算窗口大小
	int SADWindowSize = 11;
	//SGBM
	cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(mindisparity, ndisparities, SADWindowSize);

    // 能量函数参数
	int P1 = 8 * imgLeft.channels() * SADWindowSize* SADWindowSize;
    // 能量函数参数
	int P2 = 32 * imgRight.channels() * SADWindowSize* SADWindowSize;

    // 下面就是各种配置了
	sgbm->setP1(P1);
	sgbm->setP2(P2);
	sgbm->setPreFilterCap(15);
	sgbm->setUniquenessRatio(10);
	sgbm->setSpeckleRange(2);
	sgbm->setSpeckleWindowSize(100);
	sgbm->setDisp12MaxDiff(1);
	//sgbm->setMode(cv::StereoSGBM::MODE_HH);

    // 对原始图像进行预处理
    // imgLeft.copyTo(imgLefted);
    // imgRight.copyTo(imgRighted);
    // prefilterXSobel(imgLeft, imgLefted, sgbm->getPreFilterCap());
    // prefilterXSobel(imgRight, imgRighted, sgbm->getPreFilterCap());

    // imshow("img_left",imgLefted);
    // imshow("img_right",imgRighted);

    // waitKey(0);

    //
    Mat disp;
	sgbm->compute(imgLeft, imgRight, disp);
	disp.convertTo(disp, CV_32F, 1.0 / 16);                //除以16得到真实视差值
	Mat disp8U = Mat(disp.rows, disp.cols, CV_8UC1);       //显示
	normalize(disp, disp8U, 0, 255, NORM_MINMAX, CV_8UC1);

    // display
    imshow("dis",disp8U);

    waitKey(100);

	imwrite(argv[3], disp8U);

    // 生成相机内参数矩阵
    Eigen::Matrix3d K;
    K<<L_FX,    0.0,    L_CX,
       0.0,     L_FY,   L_CY,
       0.0,     0.0,    1.0;
    Eigen::Matrix3d K_inv=K.inverse();

    // 准备深度着色
    double min_d,max_d;
    int max_id[2],min_id[2];
    minMaxIdx(disp,&min_d,&max_d,min_id,max_id);

    min_d=min_d<5 ? 5:min_d;

    double max_z=FB/min_d,min_z=FB/max_d;

    double factor=1.0*1536/(max_z-min_z);

    initColor();

    // ================================== 准备可视化 =================================
    pangolin::CreateWindowAndBind(
        "PointClouds",     //窗口标题
        IMG_W,        //窗口尺寸
        IMG_H);       //窗口尺寸
    glEnable(GL_DEPTH_TEST);

    // Define Projection and initial ModelView matrix
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(
            IMG_W,IMG_H,            //相机图像的长和宽
            L_FX,L_FY,L_CX,L_CY,    //相机的内参,fu fv u0 v0
            0.2,1000),           //相机所能够看到的最浅和最深的像素
        pangolin::ModelViewLookAt(
            -2,2,-2,            //相机光心位置,NOTICE z轴不要设置为0
            0,0,0,              //相机要看的位置
            pangolin::AxisY)    //和观察的方向有关
    );

    // Create Interactive View in window
    pangolin::Handler3D handler(s_cam);
    pangolin::View& d_cam = pangolin::CreateDisplay()
            .SetBounds(
                0.0, 1.0, 0.0, 1.0,     //表示整个窗口都可以观测到
                -IMG_W/IMG_H)         //窗口的比例
            .SetHandler(&handler);

    while( !pangolin::ShouldQuit() )
    {
        // Clear screen and activate view to render into
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);

        glClearColor(1,1,1,0.0);

        // Render OpenGL Cube
        //pangolin::glDrawColouredCube();

        //尝试按照谢晓佳的视频中给出的代码绘制
        // pangolin::glDrawAxis(3);

        glPointSize(1.0f);
        glBegin(GL_POINTS);

        // 绘制当前帧点云
        for(int x=0;x<disp.rows;++x)
        {
            for(int y=0;y<disp.cols;++y)
            {
                float d=disp.at<float>(x,y);

                if(d>=min_d)
                {
                    double z=FB/d;

                    Eigen::Vector3d position=z*K_inv*Eigen::Vector3d(y,x,1);
                    // cout<<"Debug: disp8U.at<uint8_t>(x,y)="<<(int)(disp8U.at<uint8_t>(x,y))<<endl;
                    // Color c=HueCircle[(int)(disp8U.at<char>(x,y))];
                    // if((size_t)(z*1000)<1535)
                    {
                        // cout<<"debug: z="<<z<<endl;
                        // Color c=HueCircle[(size_t)(z*10)];
                        // cout<<"debug: r="<<(int)c.r<<"\tg="<<(int)c.g<<"\tb="<<(int)c.b<<"\t(size_t)(z*10)="<<(size_t)(z*10)<<endl;
                        size_t index=(z-min_z)*factor;
                        Color c=HueCircle[index];

                        // 画点
                        glColor3f(0,0,0);
                        // glColor3f(c.r/255.0,c.g/255.0,c.b/255.0);
                        // glColor3f(c.r,c.g,c.b);
                        glVertex3f(-position[0],-position[1],position[2]);
                    }

                }

            }
        }

        glEnd();
        //不要忘记了这个东西!!!
        glFlush();

        // Swap frames and Process Events
        pangolin::FinishFrame();

        waitKey(1);

    }

    return 0;
}

void initColor(void)
{

     //颜色环初始化
    HueCircle.reserve(1536);
    HueCircle.resize(1536);

    for (int i = 0;i < 255;i++)
	{
		HueCircle[i].r = 255;
		HueCircle[i].g = i;
		HueCircle[i].b = 0;

		HueCircle[i+255].r = 255-i;
		HueCircle[i+255].g = 255;
		HueCircle[i+255].b = 0;

		HueCircle[i+511].r = 0;
		HueCircle[i+511].g = 255;
		HueCircle[i+511].b = i;

		HueCircle[i+767].r = 0;
		HueCircle[i+767].g = 255-i;
		HueCircle[i+767].b = 255;

		HueCircle[i+1023].r = i;
		HueCircle[i+1023].g = 0;
		HueCircle[i+1023].b = 255;

		HueCircle[i+1279].r = 255;
		HueCircle[i+1279].g = 0;
		HueCircle[i+1279].b = 255-i;
	}

	HueCircle[1534].r = 0;
	HueCircle[1534].g = 0;
	HueCircle[1534].b = 0;

	HueCircle[1535].r = 255;
	HueCircle[1535].g = 255;
	HueCircle[1535].b = 255;

    // while(1)
    // {
    //     int index;
    //     cout<<"Index? ";
    //     cin>>index;
    //     cout<<"\tr="<<()HueCircle[index].r
    //         <<"\tg="<<HueCircle[index].g
    //         <<"\tb="<<HueCircle[index].b<<endl;
    // }

}


#ifndef ADAGRAD_HPP
#define ADAGRAD_HPP

#define EIGEN_MPL2_ONLY

#include <Eigen/Dense>
#include <vector>

using namespace Eigen;
using namespace std;

class Adagrad{
private:
  int N;
  int d;
  MatrixXd X;
  VectorXd label;
  double C;
  double eta;
  int iteration;
  VectorXd E;
  double sigma(const MatrixXd& _x, int i);
  double sigmoid(double z);

public:
  VectorXd w;
  vector<double> iterscores;
  Adagrad(int _N,int _d,MatrixXd _x,VectorXd _label,double _C,double _eta,int iter);
  double Acc(vector<double>& pred,VectorXd &l);
  void train();
  void predict(MatrixXd& _x,VectorXd& _l,vector<double>& ret);
};

#endif


/* ============================================================================
 * Copyright 2021 The University of Utah
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright notice, this
 * list of conditions and the following disclaimer in the documentation and/or
 * other materials provided with the distribution.
 *
 * Neither the name of BlueQuartz Software, the US Air Force, the University of Utah nor the names of its contributors may be used to endorse or promote products derived from this software without
 * specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
 * OF SUCH DAMAGE.
 *
 * The code contained herein was partially funded by the following contracts:
 *
 *
 * This code contained herein is based upon work supported by the following grants:
 *    DOE Office of Nuclear Energy's Nuclear Energy University Program Grant No.: DE-NE0008799
 *    DOD Office of Economic Adjustment Grant No.: ST1605-19-03
 *
 * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

#include "ComputeFeatureEigenstrains.h"

#include <cmath>

#include <QtCore/QTextStream>

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

#include "EbsdLib/Core/Orientation.hpp"
#include "EbsdLib/Core/OrientationTransformation.hpp"

#include "SIMPLib/Common/Constants.h"
#include "SIMPLib/DataContainers/DataContainer.h"
#include "SIMPLib/DataContainers/DataContainerArray.h"
#include "SIMPLib/FilterParameters/AbstractFilterParametersReader.h"
#include "SIMPLib/FilterParameters/DataArraySelectionFilterParameter.h"
#include "SIMPLib/FilterParameters/FloatFilterParameter.h"
#include "SIMPLib/FilterParameters/LinkedBooleanFilterParameter.h"
#include "SIMPLib/FilterParameters/LinkedPathCreationFilterParameter.h"
#include "SIMPLib/FilterParameters/SeparatorFilterParameter.h"
#include "SIMPLib/Geometry/ImageGeom.h"
#include "SIMPLib/Math/SIMPLibMath.h"

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

#include "DREAM3DReview/DREAM3DReviewFilters/util/EigenstrainsHelper.hpp"

namespace SIMPLMath = SIMPLib::Constants;

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
ComputeFeatureEigenstrains::ComputeFeatureEigenstrains() = default;

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

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::initialize()
{
  clearErrorCode();
  clearWarningCode();
  setCancel(false);
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setupFilterParameters()
{
  FilterParameterVectorType parameters;

  // Poisson's ratio user input
  parameters.push_back(SIMPL_NEW_FLOAT_FP("Poisson's Ratio", PoissonRatio, FilterParameter::Category::Parameter, ComputeFeatureEigenstrains));

  std::vector<QString> linkedProps;
  linkedProps.push_back("AxisLengthsArrayPath");
  linkedProps.push_back("AxisEulerAnglesArrayPath");

  parameters.push_back(
      SIMPL_NEW_LINKED_BOOL_FP("Use Ellipsoidal Grains (versus spherical assumption)", UseEllipsoidalGrains, FilterParameter::Category::Parameter, ComputeFeatureEigenstrains, linkedProps));

  // Correctional matrix beta user inputs
  linkedProps.clear();
  linkedProps.push_back("Beta11");
  linkedProps.push_back("Beta22");
  linkedProps.push_back("Beta33");
  linkedProps.push_back("Beta23");
  linkedProps.push_back("Beta13");
  linkedProps.push_back("Beta12");
  parameters.push_back(SIMPL_NEW_LINKED_BOOL_FP("Use Correctional Matrix", UseCorrectionalMatrix, FilterParameter::Category::Parameter, ComputeFeatureEigenstrains, linkedProps));

  parameters.push_back(SIMPL_NEW_FLOAT_FP("Beta11", Beta11, FilterParameter::Category::Parameter, ComputeFeatureEigenstrains));
  parameters.push_back(SIMPL_NEW_FLOAT_FP("Beta22", Beta22, FilterParameter::Category::Parameter, ComputeFeatureEigenstrains));
  parameters.push_back(SIMPL_NEW_FLOAT_FP("Beta33", Beta33, FilterParameter::Category::Parameter, ComputeFeatureEigenstrains));
  parameters.push_back(SIMPL_NEW_FLOAT_FP("Beta23", Beta23, FilterParameter::Category::Parameter, ComputeFeatureEigenstrains));
  parameters.push_back(SIMPL_NEW_FLOAT_FP("Beta13", Beta13, FilterParameter::Category::Parameter, ComputeFeatureEigenstrains));
  parameters.push_back(SIMPL_NEW_FLOAT_FP("Beta12", Beta12, FilterParameter::Category::Parameter, ComputeFeatureEigenstrains));

  // Axis lengths and euler angles 3xN feature arrays
  parameters.push_back(SeparatorFilterParameter::Create("Cell Feature Data", FilterParameter::Category::RequiredArray));

  {
    DataArraySelectionFilterParameter::RequirementType req =
        DataArraySelectionFilterParameter::CreateRequirement(SIMPL::TypeNames::Float, 3, AttributeMatrix::Type::CellFeature, IGeometry::Type::Image);
    parameters.push_back(SIMPL_NEW_DA_SELECTION_FP("Axis Lengths", AxisLengthsArrayPath, FilterParameter::Category::RequiredArray, ComputeFeatureEigenstrains, req));
  }

  {
    DataArraySelectionFilterParameter::RequirementType req =
        DataArraySelectionFilterParameter::CreateRequirement(SIMPL::TypeNames::Float, 3, AttributeMatrix::Type::CellFeature, IGeometry::Type::Image);
    parameters.push_back(SIMPL_NEW_DA_SELECTION_FP("Axis Euler Angles", AxisEulerAnglesArrayPath, FilterParameter::Category::RequiredArray, ComputeFeatureEigenstrains, req));
  }

  // Elastic strain 6xN feature array
  {
    DataArraySelectionFilterParameter::RequirementType req =
        DataArraySelectionFilterParameter::CreateRequirement(SIMPL::TypeNames::Float, 6, AttributeMatrix::Type::CellFeature, IGeometry::Type::Image);
    parameters.push_back(SIMPL_NEW_DA_SELECTION_FP("Elastic Strains (Voigt Notation)", ElasticStrainsArrayPath, FilterParameter::Category::RequiredArray, ComputeFeatureEigenstrains, req));
  }

  // Output 6xN eigenstrain feature array
  parameters.push_back(SeparatorFilterParameter::Create("Cell Feature Data", FilterParameter::Category::CreatedArray));
  parameters.push_back(
      SIMPL_NEW_DA_WITH_LINKED_AM_FP("Eigenstrains", EigenstrainsArrayName, ElasticStrainsArrayPath, ElasticStrainsArrayPath, FilterParameter::Category::CreatedArray, ComputeFeatureEigenstrains));

  setFilterParameters(parameters);
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::dataCheck()
{
  clearErrorCode();
  clearWarningCode();
  initialize();
  DataArrayPath tempPath;

  // Negative Poisson's ratio works in the calculations but warn
  if(getPoissonRatio() < 0.0f)
  {
    QString ss = QObject::tr("Poisson's ratio is negative");
    setWarningCondition(-94001, ss);
  }

  // Incompressible v=0.5 results in singular matrix
  if(getPoissonRatio() > 0.49999999f)
  {
    QString ss = QObject::tr("Poisson's ratio cannot be 0.5 or greater");
    setErrorCondition(-94002, ss);
  }

  // Check correction values and warn if they are large (potential typos)
  if(m_UseCorrectionalMatrix)
  {
    if(getBeta11() < 0.5f || getBeta11() > 2.0f)
    {
      QString ss = QObject::tr("Beta11 correction is pretty large; this may be a typo");
      setWarningCondition(-94003, ss);
    }

    if(getBeta22() < 0.5f || getBeta22() > 2.0f)
    {
      QString ss = QObject::tr("Beta22 correction is pretty large; this may be a typo");
      setWarningCondition(-94004, ss);
    }

    if(getBeta33() < 0.5f || getBeta33() > 2.0f)
    {
      QString ss = QObject::tr("Beta33 correction is pretty large; this may be a typo");
      setWarningCondition(-94005, ss);
    }

    if(getBeta23() < 0.5f || getBeta23() > 2.0f)
    {
      QString ss = QObject::tr("Beta23 correction is pretty large; this may be a typo");
      setWarningCondition(-94006, ss);
    }

    if(getBeta13() < 0.5f || getBeta13() > 2.0f)
    {
      QString ss = QObject::tr("Beta13 correction is pretty large; this may be a typo");
      setWarningCondition(-94007, ss);
    }

    if(getBeta12() < 0.5f || getBeta12() > 2.0f)
    {
      QString ss = QObject::tr("Beta12 correction is pretty large; this may be a typo");
      setWarningCondition(-94008, ss);
    }
  }

  // Check Required Objects
  std::vector<size_t> cDims(1, 1);
  if(m_UseEllipsoidalGrains)
  {
    cDims[0] = 3;
    m_AxisLengthsPtr = getDataContainerArray()->getPrereqArrayFromPath<FloatArrayType>(this, getAxisLengthsArrayPath(), cDims);
    if(getErrorCode() < 0)
    {
      return;
    }

    cDims[0] = 3;
    m_AxisEulerAnglesPtr = getDataContainerArray()->getPrereqArrayFromPath<FloatArrayType>(this, getAxisEulerAnglesArrayPath(), cDims);
    if(getErrorCode() < 0)
    {
      return;
    }
  }

  cDims[0] = 6;
  m_ElasticStrainsPtr = getDataContainerArray()->getPrereqArrayFromPath<FloatArrayType>(this, getElasticStrainsArrayPath(), cDims);
  if(getErrorCode() < 0)
  {
    return;
  }

  // Check Eigenstrain output
  cDims[0] = 6;
  tempPath.update(getElasticStrainsArrayPath().getDataContainerName(), getElasticStrainsArrayPath().getAttributeMatrixName(), getEigenstrainsArrayName());
  m_EigenstrainsPtr = getDataContainerArray()->createNonPrereqArrayFromPath<FloatArrayType>(this, tempPath, 0, cDims, "", 1);
  if(getErrorCode() < 0)
  {
    return;
  }
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::execute()
{
  initialize();
  dataCheck();
  if(getErrorCode() < 0)
  {
    return;
  }

  if(getCancel())
  {
    return;
  }

  find_eigenstrains();
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::find_eigenstrains()
{
  size_t numfeatures = m_ElasticStrainsPtr.lock()->getNumberOfTuples();

  double phi1 = 0.0;
  double theta = 0.0;
  double phi2 = 0.0;

  double semiAxisA = 0.0;
  double semiAxisB = 0.0;
  double semiAxisC = 0.0;

  double E11 = 0.0;
  double E22 = 0.0;
  double E33 = 0.0;
  double E23 = 0.0;
  double E13 = 0.0;
  double E12 = 0.0;

  Eigen::Matrix<double, 3, 3> beta;
  beta.setOnes(3, 3); // Default no correction
  if(m_UseCorrectionalMatrix)
  {
    // clang-format off
    beta << m_Beta11, m_Beta12, m_Beta13,
            m_Beta12, m_Beta22, m_Beta23,
            m_Beta13, m_Beta23, m_Beta33;
    // clang-format on
  }

  OrientationD orientationMatrix;
  Eigen::Matrix<double, 3, 3> OM;
  Eigen::Matrix<double, 3, 3> OMT;
  OM.setIdentity(3, 3); // Defaults to no orientation identity
  OMT.setIdentity(3, 3);

  Eigen::Matrix<double, 3, 3> elasticStrainTensor;
  Eigen::Matrix<double, 3, 3> elasticStrainTensorRot;

  EigenstrainsHelper::Tensor4DType eshelbyTensor;
  EigenstrainsHelper::Tensor4DType eshelbyInverse;

  Eigen::Matrix<double, 9, 9> eshelbyTensor99;
  Eigen::Matrix<double, 9, 9> eshelbyInverse99;
  Eigen::Matrix<double, 9, 9> I9;
  I9.setIdentity(9, 9);

  Eigen::Matrix<double, 3, 3> eigenstrainTensorRot;
  Eigen::Matrix<double, 3, 3> eigenstrainTensor;
  Eigen::Matrix<double, 3, 3> eigenstrainTensorCorrected;

  FloatArrayType& axisEulerAngles = *(m_AxisEulerAnglesPtr.lock().get());
  FloatArrayType& axisLengths = *(m_AxisLengthsPtr.lock().get());
  FloatArrayType& elasticStrains = *(m_ElasticStrainsPtr.lock().get());
  FloatArrayType& eigenstrains = *(m_EigenstrainsPtr.lock().get());

  // small eps term added to euler angle bounds check as FindFeatureShapes has some small noise outside the bounds
  double eps = 0.001;
  double eulerPhiMax = 2 * SIMPLMath::k_PiD + eps;
  double eulerThetaMax = SIMPLMath::k_PiD + eps;
  double eulerMin = 0 - eps;

  for(size_t feature = 0; feature < numfeatures; feature++)
  {
    if(m_UseEllipsoidalGrains)
    {
      phi1 = axisEulerAngles[feature * 3 + 0];
      theta = axisEulerAngles[feature * 3 + 1];
      phi2 = axisEulerAngles[feature * 3 + 2];

      if(std::isnan(phi1) || std::isnan(theta) || std::isnan(phi2))
      {
        QString ss = QObject::tr("NaN Axis Euler angle found in feature ID #%1, skipping").arg(feature);
        notifyStatusMessage(ss);
        eigenstrains[feature * 6 + 0] = 0;
        eigenstrains[feature * 6 + 1] = 0;
        eigenstrains[feature * 6 + 2] = 0;
        eigenstrains[feature * 6 + 3] = 0;
        eigenstrains[feature * 6 + 4] = 0;
        eigenstrains[feature * 6 + 5] = 0;
        continue;
      }

      if(phi1 > eulerPhiMax || phi1 < eulerMin)
      {
        QString ss = QObject::tr("Feature %1 euler angle phi1=%2 out of bounds 2pi >= phi1 >= 0. Euler angles may be in degrees").arg(feature).arg(phi1);
        setErrorCondition(-94000, ss);
        return;
      }

      if(theta > eulerThetaMax || theta < eulerMin)
      {
        QString ss = QObject::tr("Feature %1 euler angle theta=%2 out of bounds pi >= theta >= 0. Euler angles may be in degrees").arg(feature).arg(theta);
        setErrorCondition(-94000, ss);
        return;
      }

      if(phi2 > eulerPhiMax || phi2 < eulerMin)
      {
        QString ss = QObject::tr("Feature %1 euler angle phi2=%2 out of bounds 2pi >= phi2 >= 0. Euler angles may be in degrees").arg(feature).arg(phi2);
        setErrorCondition(-94000, ss);
        return;
      }

      semiAxisA = axisLengths[feature * 3 + 0];
      semiAxisB = axisLengths[feature * 3 + 1];
      semiAxisC = axisLengths[feature * 3 + 2];

      if(std::isnan(semiAxisA) || std::isnan(semiAxisB) || std::isnan(semiAxisC))
      {
        QString ss = QObject::tr("NaN Axis length found in feature ID #%1, skipping").arg(feature);
        notifyStatusMessage(ss);
        eigenstrains[feature * 6 + 0] = 0;
        eigenstrains[feature * 6 + 1] = 0;
        eigenstrains[feature * 6 + 2] = 0;
        eigenstrains[feature * 6 + 3] = 0;
        eigenstrains[feature * 6 + 4] = 0;
        eigenstrains[feature * 6 + 5] = 0;
        continue;
      }

      if(semiAxisB > semiAxisA)
      {
        QString ss = QObject::tr("Feature %1 semi-axis b=%2 is greater than semi-axis a=%3. Criteria a>=b>=c must be satisfied").arg(feature).arg(semiAxisB).arg(semiAxisA);
        setErrorCondition(-94000, ss);
        return;
      }

      if(semiAxisC > semiAxisB)
      {
        QString ss = QObject::tr("Feature %1 semi-axis c=%2 is greater than semi-axis b=%3. Criteria a>=b>=c must be satisfied").arg(feature).arg(semiAxisC).arg(semiAxisB);
        setErrorCondition(-94000, ss);
        return;
      }
    }

    E11 = elasticStrains[feature * 6 + 0];
    E22 = elasticStrains[feature * 6 + 1];
    E33 = elasticStrains[feature * 6 + 2];
    E23 = elasticStrains[feature * 6 + 3];
    E13 = elasticStrains[feature * 6 + 4];
    E12 = elasticStrains[feature * 6 + 5];

    // clang-format off
    elasticStrainTensor << E11, E12, E13,
                           E12, E22, E23,
                           E13, E23, E33;
    // clang-format on

    // Check if the elastic strains are zero (or negligible) then so are the eigenstrains
    if(elasticStrainTensor.isMuchSmallerThan(1e-10))
    {
      eigenstrains[feature * 6 + 0] = 0;
      eigenstrains[feature * 6 + 1] = 0;
      eigenstrains[feature * 6 + 2] = 0;
      eigenstrains[feature * 6 + 3] = 0;
      eigenstrains[feature * 6 + 4] = 0;
      eigenstrains[feature * 6 + 5] = 0;
      continue;
    }

    if(m_UseEllipsoidalGrains)
    {
      orientationMatrix = OrientationTransformation::eu2om<OrientationD, OrientationD>({phi1, theta, phi2});

      // clang-format off
      OM << orientationMatrix[0], orientationMatrix[1], orientationMatrix[2],
            orientationMatrix[3], orientationMatrix[4], orientationMatrix[5],
            orientationMatrix[6], orientationMatrix[7], orientationMatrix[8];
      OMT = OM.transpose();
      // clang-format on
    }

    // Change basis into ellipsoid reference frame | e' = Q e Q^T
    elasticStrainTensorRot = OM * elasticStrainTensor * OMT;

    // Calculate Eshelby tensor | S = f(a, b, c, nu)
    eshelbyTensor = EigenstrainsHelper::find_eshelby(semiAxisA, semiAxisB, semiAxisC, m_PoissonRatio, m_UseEllipsoidalGrains);

    // Map Eshelby tensor into 9x9 matrix
    size_t col = 0;
    size_t row = 0;
    for(size_t i = 0; i < 3; i++)
    {
      for(size_t j = 0; j < 3; j++)
      {
        for(size_t k = 0; k < 3; k++)
        {
          for(size_t l = 0; l < 3; l++)
          {
            eshelbyTensor99(col, row) = eshelbyTensor(i, j, k, l);
            col++;
          }
        }
        row++;
        col = 0;
      }
    }

    // Calculate inverse | (S-I)^-1
    eshelbyInverse99 = (eshelbyTensor99 - I9).inverse();

    // Remap inverse back into a 3x3x3x3 tensor
    col = 0;
    row = 0;
    for(size_t i = 0; i < 3; i++)
    {
      for(size_t j = 0; j < 3; j++)
      {
        for(size_t k = 0; k < 3; k++)
        {
          for(size_t l = 0; l < 3; l++)
          {
            eshelbyInverse(i, j, k, l) = eshelbyInverse99(col, row);
            col++;
          }
        }
        row++;
        col = 0;
      }
    }

    // Calculate eigenstrain tensor | e*' = (S-I)^-1 e'
    eigenstrainTensorRot.setZero(3, 3);
    for(size_t i = 0; i < 3; i++)
    {
      for(size_t j = 0; j < 3; j++)
      {
        for(size_t k = 0; k < 3; k++)
        {
          for(size_t l = 0; l < 3; l++)
          {
            eigenstrainTensorRot(i, j) += eshelbyInverse(i, j, k, l) * elasticStrainTensorRot(k, l);
          }
        }
      }
    }

    // Change basis back to global reference frame | e* = Q^T e*' Q
    eigenstrainTensor = OMT * eigenstrainTensorRot * OM;

    // Add correction | e*c = B e*
    eigenstrainTensorCorrected = eigenstrainTensor.cwiseProduct(beta);

    eigenstrains[feature * 6 + 0] = eigenstrainTensorCorrected(0, 0);
    eigenstrains[feature * 6 + 1] = eigenstrainTensorCorrected(1, 1);
    eigenstrains[feature * 6 + 2] = eigenstrainTensorCorrected(2, 2);
    eigenstrains[feature * 6 + 3] = eigenstrainTensorCorrected(1, 2);
    eigenstrains[feature * 6 + 4] = eigenstrainTensorCorrected(0, 2);
    eigenstrains[feature * 6 + 5] = eigenstrainTensorCorrected(0, 1);
  }
}

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

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

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

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

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString ComputeFeatureEigenstrains::getGroupName() const
{
  return DREAM3DReviewConstants::FilterGroups::DREAM3DReviewFilters;
}

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

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString ComputeFeatureEigenstrains::getHumanLabel() const
{
  return "Compute Eigenstrains by Feature (Grain/Inclusion)";
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QUuid ComputeFeatureEigenstrains::getUuid() const
{
  return QUuid("{879e1eb8-40dc-5a5b-abe5-7e0baa77ed73}");
}

// -----------------------------------------------------------------------------
ComputeFeatureEigenstrains::Pointer ComputeFeatureEigenstrains::NullPointer()
{
  return Pointer(static_cast<Self*>(nullptr));
}

// -----------------------------------------------------------------------------
std::shared_ptr<ComputeFeatureEigenstrains> ComputeFeatureEigenstrains::New()
{
  struct make_shared_enabler : public ComputeFeatureEigenstrains
  {
  };
  std::shared_ptr<make_shared_enabler> val = std::make_shared<make_shared_enabler>();
  val->setupFilterParameters();
  return val;
}

// -----------------------------------------------------------------------------
QString ComputeFeatureEigenstrains::getNameOfClass() const
{
  return QString("ComputeFeatureEigenstrains");
}

// -----------------------------------------------------------------------------
QString ComputeFeatureEigenstrains::ClassName()
{
  return QString("ComputeFeatureEigenstrains");
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setPoissonRatio(float value)
{
  m_PoissonRatio = value;
}

// -----------------------------------------------------------------------------
float ComputeFeatureEigenstrains::getPoissonRatio() const
{
  return m_PoissonRatio;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setUseEllipsoidalGrains(bool value)
{
  m_UseEllipsoidalGrains = value;
}

// -----------------------------------------------------------------------------
bool ComputeFeatureEigenstrains::getUseEllipsoidalGrains() const
{
  return m_UseEllipsoidalGrains;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setUseCorrectionalMatrix(bool value)
{
  m_UseCorrectionalMatrix = value;
}

// -----------------------------------------------------------------------------
bool ComputeFeatureEigenstrains::getUseCorrectionalMatrix() const
{
  return m_UseCorrectionalMatrix;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setBeta11(float value)
{
  m_Beta11 = value;
}

// -----------------------------------------------------------------------------
float ComputeFeatureEigenstrains::getBeta11() const
{
  return m_Beta11;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setBeta22(float value)
{
  m_Beta22 = value;
}

// -----------------------------------------------------------------------------
float ComputeFeatureEigenstrains::getBeta22() const
{
  return m_Beta22;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setBeta33(float value)
{
  m_Beta33 = value;
}

// -----------------------------------------------------------------------------
float ComputeFeatureEigenstrains::getBeta33() const
{
  return m_Beta33;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setBeta23(float value)
{
  m_Beta23 = value;
}

// -----------------------------------------------------------------------------
float ComputeFeatureEigenstrains::getBeta23() const
{
  return m_Beta23;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setBeta13(float value)
{
  m_Beta13 = value;
}

// -----------------------------------------------------------------------------
float ComputeFeatureEigenstrains::getBeta13() const
{
  return m_Beta13;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setBeta12(float value)
{
  m_Beta12 = value;
}

// -----------------------------------------------------------------------------
float ComputeFeatureEigenstrains::getBeta12() const
{
  return m_Beta12;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setAxisLengthsArrayPath(const DataArrayPath& value)
{
  m_AxisLengthsArrayPath = value;
}

// -----------------------------------------------------------------------------
DataArrayPath ComputeFeatureEigenstrains::getAxisLengthsArrayPath() const
{
  return m_AxisLengthsArrayPath;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setAxisEulerAnglesArrayPath(const DataArrayPath& value)
{
  m_AxisEulerAnglesArrayPath = value;
}

// -----------------------------------------------------------------------------
DataArrayPath ComputeFeatureEigenstrains::getAxisEulerAnglesArrayPath() const
{
  return m_AxisEulerAnglesArrayPath;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setElasticStrainsArrayPath(const DataArrayPath& value)
{
  m_ElasticStrainsArrayPath = value;
}

// -----------------------------------------------------------------------------
DataArrayPath ComputeFeatureEigenstrains::getElasticStrainsArrayPath() const
{
  return m_ElasticStrainsArrayPath;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setEigenstrainsArrayName(const QString& value)
{
  m_EigenstrainsArrayName = value;
}

// -----------------------------------------------------------------------------
QString ComputeFeatureEigenstrains::getEigenstrainsArrayName() const
{
  return m_EigenstrainsArrayName;
}


#include <ros/ros.h>
#include <iostream>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/String.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>
#include <boost/foreach.hpp>
#include <pcl/console/parse.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/sample_consensus/sac_model_cylinder.h>
#include <pcl/segmentation/sac_segmentation.h>

using std::cout;
using std::endl;

std_msgs::String str;
ros::Publisher pub;
int found;

void callback(const sensor_msgs::PointCloud2ConstPtr& cloud)
{
  ///Detection part was inspired from here -> http://answers.ros.org/question/229784/detecting-spheres-using-ransac-in-pcl/
  pcl::PCLPointCloud2 pcl_pc2;
  pcl_conversions::toPCL(*cloud,pcl_pc2);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::fromPCLPointCloud2(pcl_pc2,*temp_cloud);

  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  pcl::SACSegmentation<pcl::PointXYZRGB> segmentation;
  segmentation.setInputCloud(temp_cloud);
  ///Shapes that are available for RANSAC -> http://docs.pointclouds.org/1.7.0/group__sample__consensus.html
  segmentation.setModelType(pcl::SACMODEL_SPHERE);
  segmentation.setMethodType(pcl::SAC_RANSAC);
  segmentation.setDistanceThreshold(1000);
  segmentation.setOptimizeCoefficients(true);
  segmentation.setRadiusLimits(0.4, 1);
  segmentation.setMaxIterations(1000);

  pcl::PointIndices inlierIndices;
  segmentation.segment(inlierIndices, *coefficients);

  if (inlierIndices.indices.size() != 0)
  {
    ROS_INFO("RANSAC found shape with [%d] points", (int)inlierIndices.indices.size());
    cout << "SPHERE!" << endl;
    str.data = "found";
    int size = inlierIndices.indices.size();
    if((int)temp_cloud->points[inlierIndices.indices[size/2]].r>(int)temp_cloud->points[inlierIndices.indices[size/2]].g && (int)temp_cloud->points[inlierIndices.indices[size/2]].r>(int)temp_cloud->points[inlierIndices.indices[size/2]].g)
    {
      cout << "RED SPHERE!" << endl;
      str.data = "found red";
      found = 1;
      }
    }

    std_msgs::String nstr;
    nstr.data = str.data;
    pub.publish(nstr);
}

void callback2(const sensor_msgs::PointCloud2ConstPtr& cloud)
{
  pcl::PCLPointCloud2 pcl_pc2;
  pcl_conversions::toPCL(*cloud,pcl_pc2);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::fromPCLPointCloud2(pcl_pc2,*temp_cloud);

  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  pcl::SACSegmentation<pcl::PointXYZRGB> segmentation;
  segmentation.setOptimizeCoefficients(true);
  segmentation.setModelType(pcl::SACMODEL_PLANE);
  segmentation.setMethodType(pcl::SAC_RANSAC);
  segmentation.setDistanceThreshold(0.01);
  segmentation.setMaxIterations(200);
  segmentation.setInputCloud(temp_cloud);
  segmentation.setEpsAngle (0.1);

  pcl::PointIndices inlierIndices;
  segmentation.segment(inlierIndices, *coefficients);

  if (inlierIndices.indices.size() != 0)
  {
    int size = (int)inlierIndices.indices.size();
    ROS_INFO("RANSAC found shape with [%d] points", size);
    float y1 = temp_cloud->points[inlierIndices.indices[0]].y;
    float z1 = temp_cloud->points[inlierIndices.indices[0]].z;
    float y2 =temp_cloud->points[inlierIndices.indices[size/2]].y;
    float z2 =temp_cloud->points[inlierIndices.indices[size/2]].z;
    float transformedy1 = y1*cos(-1)-z1*sin(-1);
    float transformedy2 = y2*cos(-1)-z2*sin(-1);

    cout << temp_cloud->points[inlierIndices.indices[0]].x << "  " << y1 << "  " << temp_cloud->points[inlierIndices.indices[0]].z << endl;
    cout << temp_cloud->points[inlierIndices.indices[size/2]].x << "  " << y2 << "  " << temp_cloud->points[inlierIndices.indices[size/2]].z << endl;
    cout << transformedy1 << endl;
    cout << transformedy2 << endl;
    cout << (int)temp_cloud->points[inlierIndices.indices[size/2]].r << endl;
    cout << (int)temp_cloud->points[inlierIndices.indices[size/2]].g << endl;
    cout << (int)temp_cloud->points[inlierIndices.indices[size/2]].b << endl;

    if(fabs(transformedy2- transformedy1) < 0.1 && transformedy1 < 1.3)
    {
      cout << "CUBE!" << endl;
      str.data = "found cube";
      cout << "blabla" << endl;
      if((int)temp_cloud->points[inlierIndices.indices[size/2]].g>(int)temp_cloud->points[inlierIndices.indices[size/2]].b && (int)temp_cloud->points[inlierIndices.indices[size/2]].g>(int)temp_cloud->points[inlierIndices.indices[size/2]].r)
      {
        cout << "GREEN CUBE!" << endl;
        str.data = "found green";
      }
    }
  }
}

void callback0(const sensor_msgs::PointCloud2ConstPtr& cloud)
{
  callback(cloud);
  callback2(cloud);
  std_msgs::String nstr;
  nstr.data = str.data;
  pub.publish(nstr);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "subscribed");
  ros::NodeHandle nh("/");
  ros::Subscriber sub;
  str.data = "nothing";
  found = 0;

  sub =nh.subscribe<sensor_msgs::PointCloud2>("/camera/depth/points", 1, callback0);

  ros::init(argc, argv, "publish");
  pub = nh.advertise<std_msgs::String>("detectionresult", 1000);

  ros::Rate spin_rate(1);
  while(ros::ok())
    ros::spin();

  return 0;
}


/*
 * This example reproduces one of the tests with Eigen in the
 * Boost odeint repository.
 *
 * Check here: https://github.com/boostorg/odeint/blob/develop/test_external/eigen/runge_kutta4.cpp
 */

#include <iostream>
#include <Eigen/Core>
#include <boost/numeric/odeint.hpp>

#include <boost/numeric/odeint/algebra/vector_space_algebra.hpp>
#include <boost/numeric/odeint/external/eigen/eigen_resize.hpp>

using namespace boost::numeric::odeint;
using namespace Eigen;

typedef Matrix< double , Dynamic , 1 > state_type;

// System to be solved
struct sys
{
    template<class State, class Deriv>
    void operator()(const State& x, Deriv& dxdt, double t) const
    {
        dxdt[0] = 1.0;
    }
};

int main()
{
    state_type x( 1 );
    x[0] = 10.0;
    runge_kutta4< state_type , double , state_type , double , vector_space_algebra > rk4;
    rk4.do_step( sys() , x , 0.0 , 0.1 );

    std::cout << x << std::endl;
}


/**
 * File: BRIEF.cpp
 * Author: Dorian Galvez
 * Date: September 2010
 * Description: implementation of BRIEF (Binary Robust Independent
 *   Elementary Features) descriptor by
 *   Michael Calonder, Vincent Lepetitand Pascal Fua
 *   + close binary tests (by Dorian Galvez-Lopez)
 * License: see the LICENSE.txt file
 *
 */

#include "BRIEF.h"
#include "../src/DUtils.h"
#include <boost/dynamic_bitset.hpp>
#include <vector>

using namespace std;
using namespace DVision;

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

BRIEF::BRIEF(int nbits, int patch_size, Type type):
  m_bit_length(nbits), m_patch_size(patch_size), m_type(type)
{
  assert(patch_size > 1);
  assert(nbits > 0);
  generateTestPoints();
}

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

BRIEF::~BRIEF()
{
}

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

void BRIEF::compute(const cv::Mat &image,
    const std::vector<cv::KeyPoint> &points,
    vector<bitset> &descriptors,
    bool treat_image) const
{
  const float sigma = 2.f;
  const cv::Size ksize(9, 9);

  cv::Mat im;
  if(treat_image)
  {
    cv::Mat aux;
    if(image.depth() == 3)
    {
      cv::cvtColor(image, aux, CV_RGB2GRAY);
      //cv::cvtColor(image, aux, cv::COLOR_RGB2GRAY);
    }
    else
    {
      aux = image;
    }

    cv::GaussianBlur(aux, im, ksize, sigma, sigma);

  }
  else
  {
    im = image;
  }

  assert(im.type() == CV_8UC1);
  assert(im.isContinuous());

  // use im now
  const int W = im.cols;
  const int H = im.rows;

  descriptors.resize(points.size());
  std::vector<bitset>::iterator dit;

  std::vector<cv::KeyPoint>::const_iterator kit;

  int x1, y1, x2, y2;

  dit = descriptors.begin();
  for(kit = points.begin(); kit != points.end(); ++kit, ++dit)
  {
    dit->resize(m_bit_length);
    dit->reset();

    for(unsigned int i = 0; i < m_x1.size(); ++i)
    {
      x1 = (int)(kit->pt.x + m_x1[i]);
      y1 = (int)(kit->pt.y + m_y1[i]);
      x2 = (int)(kit->pt.x + m_x2[i]);
      y2 = (int)(kit->pt.y + m_y2[i]);

      if(x1 >= 0 && x1 < W && y1 >= 0 && y1 < H
        && x2 >= 0 && x2 < W && y2 >= 0 && y2 < H)
      {
        if( im.ptr<unsigned char>(y1)[x1] < im.ptr<unsigned char>(y2)[x2] )
        {
          dit->set(i);
        }
      } // if (x,y)_1 and (x,y)_2 are in the image

    } // for each (x,y)
  } // for each keypoint
}

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

void BRIEF::generateTestPoints()
{
  m_x1.resize(m_bit_length);
  m_y1.resize(m_bit_length);
  m_x2.resize(m_bit_length);
  m_y2.resize(m_bit_length);

  const float g_mean = 0.f;
  const float g_sigma = 0.2f * (float)m_patch_size;
  const float c_sigma = 0.08f * (float)m_patch_size;

  float sigma2;
  if(m_type == RANDOM)
    sigma2 = g_sigma;
  else
    sigma2 = c_sigma;

  const int max_v = m_patch_size / 2;

  DUtils::Random::SeedRandOnce();

  for(int i = 0; i < m_bit_length; ++i)
  {
    int x1, y1, x2, y2;

    do
    {
      x1 = DUtils::Random::RandomGaussianValue(g_mean, g_sigma);
    } while( x1 > max_v || x1 < -max_v);

    do
    {
      y1 = DUtils::Random::RandomGaussianValue(g_mean, g_sigma);
    } while( y1 > max_v || y1 < -max_v);

    float meanx, meany;
    if(m_type == RANDOM)
      meanx = meany = g_mean;
    else
    {
      meanx = x1;
      meany = y1;
    }

    do
    {
      x2 = DUtils::Random::RandomGaussianValue(meanx, sigma2);
    } while( x2 > max_v || x2 < -max_v);

    do
    {
      y2 = DUtils::Random::RandomGaussianValue(meany, sigma2);
    } while( y2 > max_v || y2 < -max_v);

    m_x1[i] = x1;
    m_y1[i] = y1;
    m_x2[i] = x2;
    m_y2[i] = y2;
  }

}

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


#include "Tudat/Astrodynamics/Relativity/jw_acceleration.h"

#include <iostream>

#include <Eigen/Geometry>

namespace tudat {

namespace gnv {

    Eigen::Vector3d ext_pot_acceleration(
        const Eigen::Vector3d& relativePosition,
        const Eigen::Vector3d& relativePosition_cb_wrt_primary,
        const double commonCorrectionTerm,
        const double mu_primary
    ) {
        double rab = relativePosition_cb_wrt_primary.norm();
        return (commonCorrectionTerm*mu_primary/rab)*relativePosition;
    }

    Eigen::Vector3d kinetic_acceleration(
        Eigen::Vector6d state_subject,
        Eigen::Vector6d state_actor,
        double mu_actor
    ) {
        Eigen::Vector6d state_sa = state_subject - state_actor;
        Eigen::Vector3d pos_sa   = state_sa.segment(0,3);

        Eigen::Vector3d vel_a    = state_actor.segment(3,3);

        const double c = tudat::physical_constants::SPEED_OF_LIGHT;

        double beta_a = vel_a.norm()/c;

        double r = pos_sa.norm();

        return -(2.0*beta_a*beta_a*mu_actor/(r*r*r))*pos_sa;

    }

    Eigen::Vector3d schwarzschild_acceleration(
        Eigen::Vector6d state_subject,
        Eigen::Vector6d state_actor,
        double mu_actor,
        double ppn_gamma
    ) {
        Eigen::Vector6d state_sa = state_subject - state_actor;
        Eigen::Vector3d pos_sa = state_sa.segment(0,3);
        Eigen::Vector3d vel_sa = state_sa.segment(3,3);

        const double c = tudat::physical_constants::SPEED_OF_LIGHT;
        double r = pos_sa.norm();

        double mu_c2_r3 = mu_actor / (c*c*r*r*r);

        return mu_c2_r3 * (
            (4.0*mu_actor/r - vel_sa.dot(vel_sa)) * pos_sa +
            4*(pos_sa.dot(vel_sa)) * vel_sa
        );

//        Eigen::Vector3d output_vector;
//        output_vector.setZero();
//        return output_vector;
    }

    Eigen::Vector3d cb_velocity_acceleration(
        Eigen::Vector6d state_subject,
        Eigen::Vector6d state_actor,
        double mu_actor
    ) {
        Eigen::Vector6d state_sa = state_subject - state_actor;
        Eigen::Vector3d pos_sa = state_sa.segment(0,3);
        //Eigen::Vector3d vel_sa = state_sa.segment(3,3);
        Eigen::Vector3d vel_a  = state_actor.segment(3,3);

        double r = pos_sa.norm();
        double r_dot_v = pos_sa.dot(vel_a);
        double c = tudat::physical_constants::SPEED_OF_LIGHT;
        double pre_factor = (mu_actor*r_dot_v)/(2.0*c*c*r*r*r);

        return pre_factor * ( ((3.0*r_dot_v)/(r*r)) * pos_sa - 2.0*vel_a);
    }

    Eigen::Vector3d lense_thirring_acceleration(
        Eigen::Vector6d state_subject,
        Eigen::Vector6d state_actor,
        Eigen::Vector3d angular_momentum_actor
    ) {
//        std::cout << __FILE__ << "\n Line " << __LINE__ << std::endl;
//        throw std::runtime_error("Lense Thirring function not implemented");
//        return Eigen::Vector3d::Zero();

        Eigen::Vector6d state_sa = state_subject - state_actor;
        Eigen::Vector3d pos_sa = state_sa.segment(0,3);
        Eigen::Vector3d vel_sa = state_sa.segment(3,3);

        double r = pos_sa.norm();

        double G = tudat::physical_constants::GRAVITATIONAL_CONSTANT;
        double c = tudat::physical_constants::SPEED_OF_LIGHT;

        return (2*G/(c*c*r*r*r)) * (
            (3.0/(r*r)) * pos_sa.dot(angular_momentum_actor) * pos_sa.cross(vel_sa) +
            vel_sa.cross(angular_momentum_actor)
        );
    }

    Eigen::Vector3d wavi_acceleration(
        Eigen::Vector6d state_subject,
        Eigen::Vector6d state_actor,
        double mu_actor
    ) {
//        std::cout << __FILE__ << "\n Line " << __LINE__ << std::endl;
//        throw std::runtime_error("Wavi function not implemented");
//        return Eigen::Vector3d::Zero();

        Eigen::Vector6d state_sa = state_subject - state_actor;
        Eigen::Vector3d pos_sa   = state_sa.segment(0,3);
        Eigen::Vector3d vel_sa   = state_sa.segment(3,3);
        Eigen::Vector3d vel_a    = state_actor.segment(3,3);

        const double c = tudat::physical_constants::SPEED_OF_LIGHT;

        double r = pos_sa.norm();

        return 4*mu_actor / (c*c*r*r*r) * (
            vel_a.dot(vel_sa) * pos_sa -
            pos_sa.dot(vel_sa) * vel_a
        );
    }

    Eigen::Vector3d de_sitter_acceleration(
        Eigen::Vector6d state_subject,
        Eigen::Vector6d state_actor,
        Eigen::Vector6d state_primary,
        double mu_primary
    ) {
//        std::cout << __FILE__ << "\n Line " << __LINE__ << std::endl;
//        throw std::runtime_error("de Sitter function not implemented");
//        return Eigen::Vector3d::Zero();
        Eigen::Vector6d state_sa = state_subject - state_actor;
        Eigen::Vector6d state_ap = state_actor - state_primary;

        Eigen::Vector3d v_sa = state_sa.segment(3,3);
        Eigen::Vector3d r_ap = state_ap.segment(0,3);
        Eigen::Vector3d v_ap = state_ap.segment(3,3);

        double c = tudat::physical_constants::SPEED_OF_LIGHT;

        //double r = r_sa.norm();
        double R = r_ap.norm();

        double pre_factor = 3.0*mu_primary/(c*c*R*R*R);

        return pre_factor * (
            (v_ap.cross(r_ap)).cross(v_sa)
        );
    }

}

}


#include<iostream>
//#define EIGEN_USE_MKL_ALL
#include"basis.hpp"
#include"operators.hpp"
#include"diag.h"
#include"tpoperators.hpp"
#include"files.hpp"
#include"timeev.hpp"
#include"ETH.hpp"
#include<iomanip>
#include <boost/program_options.hpp>

using namespace boost::program_options;
int main(int argc, char *argv[])
{
  using namespace Eigen;
using namespace std;
using namespace Many_Body;
  using Fermi_HubbardBasis= TensorProduct<ElectronBasis, ElectronBasis>;
   using HolsteinBasis= TensorProduct<ElectronBasis, PhononBasis>;
  using Mat= Operators::Mat;
   using boost::program_options::value;
   int M{};
  int L{};
    double T{};
  double t0{};
  double omega{};
  double gamma{};
  double cutoff{};
  bool PB{};

  std::string sM{};
  std::string sL{};
  std::string star{};
  std::string sT{};
  std::string st0{};
  std::string somega{};
  std::string sgamma{};
  std::string starget{};
  std::string scutoff{};
  std::string sPB{};
  std::string filename="exT";
  try
  {
    boost::program_options::options_description desc{"Options"};
    desc.add_options()
      ("help,h", "Help screen")
      ("L", value(&L)->default_value(4), "L")
      ("T", value(&T)->default_value(0.1), "T")
      ("M,m", value(&M)->default_value(2), "M")
      ("t0", value(&t0)->default_value(1.), "t0")
      ("gam", value(&gamma)->default_value(std::sqrt(2)), "gamma")
      ("omg", value(&omega)->default_value(1.), "omega")
      ("pb", value(&PB)->default_value(0), "pb");
  boost::program_options::variables_map vm;
  boost::program_options::store(parse_command_line(argc, argv, desc), vm);
  boost::program_options::notify(vm);

  if (vm.count("help"))
      {std::cout << desc << '\n'; return 0;}
    else{
      if (vm.count("L"))
      {      std::cout << "L: " << vm["L"].as<int>() << '\n';
      	sL="L"+std::to_string(vm["L"].as<int>());
	filename+=sL;
      }
         if (vm.count("M"))
      {      std::cout << "M: " << vm["M"].as<int>() << '\n';
      	sM="M"+std::to_string(vm["M"].as<int>());
	filename+=sM;
      }
      	 if (vm.count("t0"))
      {      std::cout << "t0: " << vm["t0"].as<double>() << '\n';
      	st0="t0"+std::to_string(vm["t0"].as<double>()).substr(0, 3);
      	filename+=st0;
      }
      	 	 if (vm.count("omg"))
      {      std::cout << "omega: " << vm["omg"].as<double>() << '\n';
      	somega="omg"+std::to_string(vm["omg"].as<double>()).substr(0, 3);
      		filename+=somega;
      }

      		 if (vm.count("gam"))
      {      std::cout << "gamma: " << vm["gam"].as<double>() << '\n';
      	sgamma="gam"+std::to_string(vm["gam"].as<double>()).substr(0, 3);
      		filename+=sgamma;
      }
      // 		 if (vm.count("T"))
      // {      std::cout << "T: " << vm["T"].as<double>() << '\n';
      // 	sT="T"+std::to_string(vm["T"].as<double>()).substr(0, 3);
      // 	filename+=sT;
      // }
		 if (vm.count("pb"))
      {      std::cout << "PB: " << vm["pb"].as<bool>() << '\n';
      	sPB="PB"+std::to_string(vm["pb"].as<bool>());
      		filename+=sPB;
      }
      }
    }
  catch (const boost::program_options::error &ex)
  {
    std::cerr << ex.what() << '\n';
    return 0;
  }
  filename+=".bin";
  double mean=0.5*L*M*omega;
  double dt=0.1;
  ElectronState estate1(L, 1);
  ElectronBasis e( L, 1);
  std::cout<< e<<std::endl;

  PhononBasis ph(L, M);
  //                 std::cout<< ph<<std::endl;
      HolsteinBasis TP(e, ph);
      std::cout<< TP.dim << std::endl;
         Eigen::VectorXcd inistate(TP.dim);

   	 Mat O=Operators::NumberOperator(TP, ph, 1,  PB);
   inistate.setZero();

   inistate[0]=1;
   	          Eigen::VectorXcd i0=inistate;

   		  std::cout<< TP.dim << std::endl;
      Mat E1=Operators::EKinOperatorL(TP, e, t0,PB);
       Mat Ebdag=Operators::NBosonCOperator(TP, ph, gamma, PB);
       Mat Eb=Operators::NBosonDOperator(TP, ph, gamma, PB);
      Mat Eph=Operators::NumberOperator(TP, ph, omega,  PB);

      Eigen::VectorXd eigenVals(TP.dim);
      Mat H=E1+Eb+Eph+Ebdag;
   auto J=Operators::CurOperatorL(TP, e, t0, PB);
Eigen::MatrixXcd Jcp=J*std::complex<double>(0,1);

    Eigen::MatrixXd HH=Eigen::MatrixXd(H);

    //Eigen::MatrixXcd HHcp=HH;
    Eigen::MatrixXcd HHcp=Eigen::MatrixXd(HH);
             Eigen::MatrixXcd M2=HHcp*Jcp;
             Eigen::MatrixXcd M1=Jcp*HHcp;
	     Eigen::MatrixXcd COMM=M1-M2;
	     //                  std::cout<< M1<<std::endl;
	     //           std::cout<< COMM<<std::endl;
	     //   std::cout<< M2<<std::endl;
	       COMM*=Jcp;

		  //           std::cout<< HHcp<<std::endl;
		  //           std::cout<< Jcp<<std::endl;
        Eigen::MatrixXd N=Eigen::MatrixXd(Eph);
	  Many_Body::diagMat(HH, eigenVals);
     std::cout<<"MIN E "<<std::setprecision(15)<< eigenVals(0)<< std::endl;
     std::cout<< "MAX "<<eigenVals(eigenVals.size()-1)<< std::endl;
    		      std::cout<<endl<<eigenVals.mean()-mean<<std::endl;
         Eigen::VectorXd energy(TP.dim);
    	   std::vector<double> obs(TP.dim);
    	   std::vector<double> obs2(TP.dim);
	   std::vector<double> obs3(TP.dim);
	   std::vector<double> obs4(TP.dim);
	   std::vector<double> obs5(TP.dim);
	   std::vector<double> obs6(TP.dim);
	   //std::vector<double> obs(TP.dim);
    	   std::vector<double> ensvec(TP.dim);

	   Eigen::MatrixXd PHD2=HH.adjoint()*N*HH;
	   Eigen::MatrixXd EKIN=HH.adjoint()*E1*HH;
	   Eigen::MatrixXd EC=HH.adjoint()*(Ebdag+Eb)*HH;
	   Eigen::MatrixXcd Jmat=HH.adjoint()*(Jcp*HH);
	   Eigen::MatrixXcd JJmat=Jmat*Jmat;
	   Eigen::MatrixXcd COMMmat=HH.adjoint()*(COMM*HH);
	   //	   std::complex<double> J_in=(newIn.adjoint()*(Jcp*newIn))(0);
	   // std::complex<double> JJsq_in=(newIn.adjoint()*(Jcp*Jcp*newIn))(0);
	    Eigen::VectorXd v=PHD2.diagonal();
	    Eigen::VectorXd v2=EKIN.diagonal();
	    Eigen::VectorXd v3=EC.diagonal();
	    Eigen::VectorXd v4=(JJmat.diagonal().real());
	     Eigen::VectorXd v5=(Jmat.diagonal().real());
	     Eigen::VectorXd v6=(COMMmat.diagonal().real());
	    std::cout<< "GS "<<eigenVals(0)<<std::endl;
    	   for(int i=0; i<energy.size(); i++)
    {
      //obs[i]=real(obstot[i]);

      ensvec[i]=eigenVals(i);
      obs[i]=v(i);
      obs2[i]=v2(i);
      obs3[i]=v3(i);
      obs4[i]=v4(i);
      obs5[i]=v5(i);
      obs6[i]=v6(i);
      std::cout<< COMMmat(i,i)<<std::endl;
      if(gamma!=0)
	{	obs3[i]/=gamma;}

      //std::cout<< ensvec[i]<<'\n';
    }// 	   	    std::cout<< "gS "<<   eigenVals[0] <<std::endl;
    		    std::cout<< "mean "<<   eigenVals.mean() <<std::endl;
    	   std::vector<double> Ovec;
	    std::vector<double> Ovec2;
	     std::vector<double> Ovec3;
	     std::vector<double> Ovec4;
	     	     std::vector<double> Ovec5;

   	    	   std::vector<double> Evec;
   	    	   //		   std::vector<double> Tr={0.01, 0.05, 0.1, 0.15, 0.2, 0.25};
   	    	   		   std::vector<double> Tr;
   	    	   for(int i=1; i<22; i++)
   	    	     {
   	    	       Tr.push_back(i*0.05);

   	    	     }

   	    	  	  for(auto t: Tr){

   	    	 // //	      std::cout<< l<<std::endl;
   	         // bin_write(filename, l);
   	    	 // n++;
			    double e1=expvalCan(ensvec, ensvec,  t);
			    double o=expvalCan(ensvec, obs,  t);
			    double o2=expvalCan(ensvec, obs2,  t);
			    double o3=expvalCan(ensvec, obs3,  t);
			    double o4=expvalCan(ensvec, obs4,  t);
			     double o5=expvalCan(ensvec, obs5,  t);
			     double o6=expvalCan(ensvec, obs6,  t);
   	    		    std::cout<< " at T "<< t<<std::endl;
			    std::cout<<std::setprecision(15)<<e1 <<"  "<< o+o2+o3*gamma <<" Nph "<<o << " JJ " <<o4 << "o 6 2 "<< o6<< std::endl;
			    Ovec.push_back(o);
			    Ovec2.push_back(o2);
			    Ovec3.push_back(o3);
			    Ovec4.push_back(o4);
			    			    Ovec5.push_back(o5);
			    Evec.push_back(e1);

   	    }

   			  int pb=PB;
   		  	  std::string Hs="H";
			  std::string Ts="T";
			  std::string phds="PHD";

		      bin_write("E"+filename, Evec);
		      bin_write("Nph"+filename, Ovec);
		      bin_write("EK"+filename, Ovec2);
		      bin_write("nX"+filename, Ovec3);
		      bin_write("JJ"+filename, Ovec4);
		      bin_write("J"+filename, Ovec5);
		      bin_write("temp"+filename, Tr);
  return 0;
}


/*
 * Copyright 2010,2019
 *  CNRS/AIST
 * François Bleibel, Olivier Stasse, François Bailly
 *
 */

#ifndef __SOT_MATRIX_GEOMETRY_H__
#define __SOT_MATRIX_GEOMETRY_H__

/* --- Matrix --- */
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <dynamic-graph/eigen-io.h>
#include <dynamic-graph/linear-algebra.h>
#include <sot/core/api.hh>

#define MRAWDATA(x) x.data()

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

namespace dynamicgraph {
namespace sot {

#define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix)                \
  /** \ingroup matrixtypedefs */                                               \
  typedef Eigen::Matrix<Type, Size, Size> Matrix##SizeSuffix##TypeSuffix;      \
  /** \ingroup matrixtypedefs */                                               \
  typedef Eigen::Matrix<Type, Size, 1> Vector##SizeSuffix##TypeSuffix;         \
  /** \ingroup matrixtypedefs */                                               \
  typedef Eigen::Matrix<Type, 1, Size> RowVector##SizeSuffix##TypeSuffix;

#define EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, Size)                      \
  /** \ingroup matrixtypedefs */                                               \
  typedef Eigen::Matrix<Type, Size, Eigen::Dynamic>                            \
      Matrix##Size##X##TypeSuffix;                                             \
  /** \ingroup matrixtypedefs */                                               \
  typedef Eigen::Matrix<Type, Eigen::Dynamic, Size> Matrix##X##Size##TypeSuffix;

#define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix)                        \
  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 1, 1)                                  \
  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 5, 5)                                  \
  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 6, 6)                                  \
  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 7, 7)                                  \
  EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 1)                               \
  EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 5)                               \
  EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 6)                               \
  EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 7)

EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int, i)
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(float, f)
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(double, d)
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<float>, cf)
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<double>, cd)

#undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES
#undef EIGEN_MAKE_TYPEDEFS

typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
    MatrixRXd;
typedef Eigen::Map<MatrixRXd> SigMatrixXd;
typedef Eigen::Map<Eigen::VectorXd> SigVectorXd;
typedef const Eigen::Map<const MatrixRXd> const_SigMatrixXd;
typedef const Eigen::Map<const Eigen::VectorXd> const_SigVectorXd;

typedef Eigen::Ref<Eigen::VectorXd> RefVector;
typedef const Eigen::Ref<const Eigen::VectorXd> &ConstRefVector;
typedef Eigen::Ref<Eigen::MatrixXd> RefMatrix;
typedef const Eigen::Ref<const Eigen::MatrixXd> ConstRefMatrix;

typedef Eigen::Transform<double, 3, Eigen::Affine> SOT_CORE_EXPORT
    MatrixHomogeneous;
typedef Eigen::Matrix<double, 3, 3> SOT_CORE_EXPORT MatrixRotation;
typedef Eigen::AngleAxis<double> SOT_CORE_EXPORT VectorUTheta;
typedef Eigen::Quaternion<double> SOT_CORE_EXPORT VectorQuaternion;
typedef Eigen::Vector3d SOT_CORE_EXPORT VectorRotation;
typedef Eigen::Vector3d SOT_CORE_EXPORT VectorRollPitchYaw;
typedef Eigen::Matrix<double, 6, 6> SOT_CORE_EXPORT MatrixForce;
typedef Eigen::Matrix<double, 6, 6> SOT_CORE_EXPORT MatrixTwist;

typedef Eigen::Matrix<double, 7, 1> SOT_CORE_EXPORT Vector7;
typedef Eigen::Quaternion<double> SOT_CORE_EXPORT Quaternion;
typedef Eigen::Map<Quaternion> SOT_CORE_EXPORT QuaternionMap;

inline void buildFrom(const MatrixHomogeneous &MH, MatrixTwist &MT) {

  Eigen::Vector3d _t = MH.translation();
  MatrixRotation R(MH.linear());
  Eigen::Matrix3d Tx;
  Tx << 0, -_t(2), _t(1), _t(2), 0, -_t(0), -_t(1), _t(0), 0;
  Eigen::Matrix3d sk;
  sk = Tx * R;

  MT.block<3, 3>(0, 0) = R;
  MT.block<3, 3>(0, 3) = sk;
  MT.block<3, 3>(3, 0).setZero();
  MT.block<3, 3>(3, 3) = R;
}

} // namespace sot
} // namespace dynamicgraph

#endif /* #ifndef __SOT_MATRIX_GEOMETRY_H__ */