/* test_arrays.cpp - Test Adept's array functionality

    Copyright (C) 2016-2017 European Centre for Medium-Range Weather Forecasts

    Author: Robin Hogan <r.j.hogan@ecmwf.int>

  Copying and distribution of this file, with or without modification,
  are permitted in any medium without royalty provided the copyright
  notice and this notice are preserved.  This file is offered as-is,
  without any warranty.
*/

#include <iostream>

#define ADEPT_BOUNDS_CHECKING 1

#include <adept_arrays.h>
#include <adept/FixedArray.h>

// The following controls whether to use active variables or not
//#define ALL_ACTIVE 1
//#define MARVEL_STYLE 1

using namespace adept;

int
main(int argc, const char** argv) {
  using namespace adept;
  Stack stack;

#define HEADING(MESSAGE) \
  std::cout << "====================================================================\n"	\
	    << "   TESTING " << MESSAGE << "\n"

#define EVAL(MESSAGE, TYPE, X, EXPR)					\
  std::cout << "--------------------------------------------------------------------\n" \
	    << "### " << MESSAGE << "\n### " << #EXPR << "\n";	\
  try {								\
    TYPE X;								\
    X = test. X;							\
    std::cout << #TYPE << " " << #X << " = " << X << "\n";		\
    std::cout << "Evaluating " << #EXPR << "\n";			\
    std::cout.flush();						\
    EXPR;								\
    std::cout << "Result: " << #X << " = " << X << "\n";		\
    if (should_fail) { std::cout << "*** INCORRECT OUTCOME\n";	\
      anomalous_results++;						\
    }									\
  } catch (const adept::exception& e) {					\
    std::cout << "*** Failed with: " << e.what() << "\n";		\
    if (!should_fail) { std::cout << "*** INCORRECT OUTCOME\n";		\
      anomalous_results++;						\
    }									\
    else {								\
      std::cout << "*** Correct behaviour\n";				\
    }									\
  }

#define EVAL2(MESSAGE, TYPEX, X, TYPEY, Y, EXPR)			\
  std::cout << "--------------------------------------------------------------------\n" \
	    << "### " << MESSAGE << "\n### " << #EXPR << "\n";	\
  try {									\
    TYPEX X;								\
    X = test. X;							\
    std::cout << #TYPEX << " " << #X << " = " << X << "\n";		\
    TYPEY Y; Y = test. Y;						\
    std::cout << #TYPEY << " " << #Y << " = " << Y << "\n";		\
    std::cout << "Evaluating " << #EXPR << "\n";			\
    std::cout.flush();							\
    EXPR;								\
    std::cout << "Result: " << #X << " = " << X << "\n";		\
    if (should_fail) { std::cout << "*** INCORRECT OUTCOME\n";	        \
      anomalous_results++;						\
    }									\
  } catch (const adept::exception& e) {					\
    std::cout << "*** Failed with: " << e.what() << "\n";		\
    if (!should_fail) { std::cout << "*** INCORRECT OUTCOME\n";		\
      anomalous_results++;						\
    }									\
    else {								\
      std::cout << "*** Correct behaviour\n";				\
    }									\
  }

#define EVAL3(MESSAGE, TYPEX, X, TYPEY, Y, TYPEZ, Z, EXPR)		\
  std::cout << "--------------------------------------------------------------------\n" \
	    << "### " << MESSAGE << "\n### " << #EXPR << "\n";	\
  try {									\
    TYPEX X;								\
    X = test. X;							\
    std::cout << #TYPEX << " " << #X << " = " << X << "\n";		\
    TYPEY Y; Y = test. Y;						\
    TYPEZ Z; Z = test. Z;						\
    std::cout << #TYPEY << " " << #Y << " = " << Y << "\n";		\
    std::cout << #TYPEZ << " " << #Z << " = " << Z << "\n";		\
    std::cout << "Evaluating " << #EXPR << "\n";			\
    std::cout.flush();							\
    EXPR;								\
    std::cout << "Result: " << #X << " = " << X << "\n";		\
    if (should_fail) { std::cout << "*** INCORRECT OUTCOME\n";	        \
      anomalous_results++;						\
    }									\
  } catch (const adept::exception& e) {					\
    std::cout << "*** Failed with: " << e.what() << "\n";		\
    if (!should_fail) { std::cout << "*** INCORRECT OUTCOME\n";		\
      anomalous_results++;						\
    }									\
    else {								\
      std::cout << "*** Correct behaviour\n";				\
    }									\
  }

#define EVAL_NO_TRAP(MESSAGE, TYPE, X, EXPR)				\
  std::cout << "--------------------------------------------------------------------\n" \
	    << "### " << MESSAGE << "\n### " << #EXPR << "\n";	\
  {									\
    TYPE X;								\
    X = test. X;							\
    std::cout << #TYPE << " " << #X << " = " << X << "\n";		\
    std::cout << "Evaluating " << #EXPR << "\n";			\
    std::cout.flush();						\
    EXPR;								\
    std::cout << "Result: " << #X << " = " << X << "\n";		\
    if (should_fail) { std::cout << "*** INCORRECT OUTCOME\n";	\
      anomalous_results++;						\
    }									\
  }

#define EVAL2_NO_TRAP(MESSAGE, TYPEX, X, TYPEY, Y, EXPR)			\
  std::cout << "--------------------------------------------------------------------\n" \
	    << "### " << MESSAGE << "\n### " << #EXPR << "\n";	\
  {									\
    TYPEX X;								\
    X = test. X;							\
    std::cout << #TYPEX << " " << #X << " = " << X << "\n";		\
    TYPEY Y; Y = test. Y;						\
    std::cout << #TYPEY << " " << #Y << " = " << Y << "\n";		\
    std::cout << "Evaluating " << #EXPR << "\n";			\
    std::cout.flush();							\
    EXPR;								\
    std::cout << "Result: " << #X << " = " << X << "\n";		\
    if (should_fail) { std::cout << "*** INCORRECT OUTCOME\n";	\
      anomalous_results++;						\
    }									\
  }

#ifdef ALL_ACTIVE
#define IS_ACTIVE true
#else
#define IS_ACTIVE false
#endif

  typedef FixedArray<double,IS_ACTIVE,2> myVector2;
  typedef FixedArray<double,IS_ACTIVE,3> myVector3;
  typedef FixedArray<double,IS_ACTIVE,1,2> myMatrix12;
  typedef FixedArray<double,IS_ACTIVE,3,3> myMatrix33;
  typedef FixedArray<double,IS_ACTIVE,2,3> myMatrix23;
  typedef FixedArray<double,IS_ACTIVE,3,2> myMatrix32;
  typedef FixedArray<double,IS_ACTIVE,2,2> myMatrix22;

#ifndef ALL_ACTIVE
  typedef Real myReal;
  typedef SymmMatrix mySymmMatrix;
  typedef DiagMatrix myDiagMatrix;
  typedef TridiagMatrix myTridiagMatrix;
  typedef LowerMatrix myLowerMatrix;
  typedef UpperMatrix myUpperMatrix;
#else
  typedef aReal myReal;
  typedef aSymmMatrix mySymmMatrix;
  typedef aDiagMatrix myDiagMatrix;
  typedef aTridiagMatrix myTridiagMatrix;
  typedef aLowerMatrix myLowerMatrix;
  typedef aUpperMatrix myUpperMatrix;
#endif

  //  typedef SpecialMatrix<Real,SymmEngine<ROW_UPPER_COL_LOWER>,false> mySymmMatrix;
  //  typedef SpecialMatrix<Real,BandEngine<COL_MAJOR,0,0>,false> myDiagMatrix;
  //  typedef SpecialMatrix<Real,BandEngine<COL_MAJOR,1,1>,false> myTridiagMatrix;

  struct Test {

    myReal x;
    myVector2 z;
    myVector3 v, w;
    myMatrix12 K;
    myMatrix23 M, N;
    myMatrix33 S, C;
    myMatrix32 A;
    myMatrix22 B;

    mySymmMatrix O, P;
    myDiagMatrix D, E;
    myTridiagMatrix T, TT;
    myLowerMatrix L, LL;
    myUpperMatrix U, UU;

    intVector index;

    Test() {
      x = -2;

      O.resize(3);
      //      Q.resize(5);
      index.resize(2);
      v(0) = 2; v(1) = 3; v(2) = 5;
      w(0) = 7; w(1) = 11; w(2) = 13;
      M(0,0) = 2; M(0,1) = 3; M(0,2) = 5;
      M(1,0) = 7; M(1,1) = 11; M(1,2) = 13;
      N(0,0) = 17; N(0,1) = 19; N(0,2) = 23;
      N(1,0) = 29; N(1,1) = 31; N(1,2) = 37;
      S(0,0) = 2; S(0,1) = 3; S(0,2) = 5;
      S(1,0) = 7; S(1,1) = 11; S(1,2) = 13;
      S(2,0) = 17; S(2,1) = 19; S(2,2) = 23;

      K << 57, 59;
      z << 37, 47;

      A << 21,22,23,24,25,26;
      B << 31,32,33,34;

      //      O = -M.T();

      O(0,0) = 7;
      O(1,0) = 2; O(1,1) = 11;
      O(2,0) = 3; O(2,1) = 5; O(2,2) = 13;
      /*

      P = 14-O;

      Q.diag_vector(-2) = 1;
      Q.diag_vector(-1) = 2;
      Q.diag_vector(0)  = 3;
      Q.diag_vector(1)  = 4;
      */

      C = 0;
      D = S;
      T = S;
      L = S;
      U = S;
      index << 1, 0;
    }
  };

  stack.new_recording();

  Test test;

  bool should_fail=false;
  int anomalous_results=0;

#ifdef ALL_ACTIVE
  std::cout << "Testing ACTIVE arrays\n";
#else
  std::cout << "Testing INACTIVE arrays\n";
#endif

  HEADING("BASIC EXPRESSIONS");
  EVAL2("Vector assignment to vector", myVector3, v, myVector3, w, v = w);
  EVAL2("Vector assignment to expression", myVector3, v, myVector3, w, v = log(w) + 1.0);

  EVAL("Matrix *= operator", myMatrix23, M, M *= 0.5);
  EVAL2("Matrix = scalar", myMatrix23, M, myReal, x, M = x);

  EVAL2("Matrix = scalar expression", myMatrix23, M, myReal, x, M = (10.0*x));
  HEADING("BASIC FUNCTIONS");
  EVAL2("max", myVector3, v, myVector3, w, v = max(v,w/3.0));
  EVAL2("min", myVector3, v, myVector3, w, v = min(v,w/3.0));

  HEADING("ARRAY SLICING");
  EVAL2("Array indexing rvalue", myReal, x, myMatrix23, M, x = M(1,end-1));

  should_fail=true;
  EVAL2("Array indexing rvalue out of range (SHOULD FAIL)", myReal, x, myMatrix23, M, x = M(1,3));
  should_fail=false;

  EVAL("Array indexing lvalue", myMatrix23, M, M(1,end-1) *= -1.0);

  EVAL2("contiguous subarray rvalue", myVector3, v, myMatrix23, M, v = M(end,__));
  EVAL("contiguous subarray lvalue", myMatrix23, M, M(end-1,__) /= 2.0);
  EVAL2("contiguous subarray rvalue using range", myVector2, z, myMatrix23, M, z = 2.0 * M(1,range(1,2)));
  EVAL2("contiguous subarray lvalue using range", myMatrix23, M, myVector3, v, M(end-1,range(0,1)) = log(v(range(1,2))));
  EVAL2("contiguous subarray rvalue using subset", myMatrix12, K, myMatrix23, N, K = 2.0 * N.subset(1,1,1,2));
  EVAL("contiguous subarray lvalue using subset", myVector3, v, v.subset(end-1,end) *= 10.0);

  EVAL2("regular subarray rvalue", myVector3, v, myVector3, w, v = w(stride(end,0,-1)));
  EVAL2("regular subarray lvalue", myMatrix23, M, myVector3, w, M(0,stride(0,end,2)) *= w(stride(end,0,-2)));
  EVAL("irregular subarray rvalue", myMatrix23, M, M(stride(1,0,-1),find(M(0,__)>4)) = 0);
  EVAL("slice leading dimension", myMatrix23, M, M[end] = 0);
  EVAL("slice two dimensions", myMatrix23, M, M[end][0] = 0);
  EVAL2("diag_vector member function as rvalue", myVector2, z, myMatrix33, S, z = diag_vector(S,1));
  EVAL2("diag_vector member function as lvalue", myMatrix33, S, myVector3, v, S.diag_vector() += v);
  EVAL2("diag_matrix member function", myMatrix33, S, myVector3, v, S = v.diag_matrix());
  EVAL2("diag_matrix external function", myMatrix33, S, myVector3, v, S = diag_matrix(v));
  EVAL2("transpose as rvalue via T member function", myMatrix32, A, myMatrix23, M, A = 2 * M.T());
  EVAL2("transpose as rvalue via permute member function", myMatrix32, A, myMatrix23, M, A = 2 * M.permute(1,0));
  //  EVAL3("2D arbitrary index as rvalue", myMatrix22, B, myMatrix23, N, intVector, index, B = const_cast<const myMatrix23&>(N)(index,index));
  EVAL3("2D arbitrary index as rvalue", myMatrix22, B, myMatrix23, N, intVector, index, B = N(index,index));
  EVAL3("2D arbitrary index as lvalue", myMatrix23, M, myMatrix23, N, intVector, index, M(index,index) = N(__,range(1,2)));
  EVAL2("2D arbitrary index as lvalue with assign-multiply operator", myMatrix23, M, intVector, index, M(index,index) *= 10.0);
  EVAL2("2D arbitrary index as lvalue with aliased right-hand-side", myMatrix23, M, intVector, index, M(index,index) += M(__,range(1,2)));

  HEADING("REDUCTION OPERATIONS");
  EVAL2("full reduction", myReal, x, myMatrix23, M, x = sum(M));
  EVAL2("1-dimension reduction", myVector3, v, myMatrix23, M, v = 0.5 * mean(M,0));
  EVAL2("1-dimension reduction", myVector2, z, myMatrix23, M, z = norm2(M,1));
  EVAL2("maxval", myVector2, z, myMatrix23, M, z = maxval(M,1));
  EVAL2("minval", myVector2, z, myMatrix23, M, z = minval(M,1));
  EVAL2("dot product", myReal, x, myVector3, w, x = dot_product(w,w(stride(end,0,-1))));
  //  EVAL2("1D interpolation", myVector3, v, myVector3, w, (v = interp<double,double,true,double>(value(v), w, value(w)/3.0) ));
  EVAL2("1D interpolation", myVector3, v, myVector3, w, v = interp(value(v), w, value(w)/3.0));
  HEADING("CONDITIONAL OPERATIONS");
  EVAL2("where construct, scalar right-hand-side", myMatrix23, M, myMatrix23, N, M.where(N > 20) = 0);
  EVAL2("where construct, expression right-hand-side", myMatrix23, M, myMatrix23, N, M.where(N > 20) = -N);
  EVAL2("where construct, scalar either-or right-hand-side", myMatrix23, M, myMatrix23, N, M.where(N > 20) = either_or(0,1));
  EVAL2("where construct, expression either-or right-hand-side", myMatrix23, M, myMatrix23, N, M.where(N > 20) = either_or(-N,N));
  EVAL("find construct, scalar right-hand-side", myVector3, v, v(find(v > 3.5)) = 0);
  EVAL("find construct, expression right-hand-side", myVector3, v, v(find(v > 3.5)) = -v(range(end,end)));
  EVAL("find construct, multiply-assign right-hand-side", myVector3, v, v(find(v != 5.0)) *= 10.0);

  HEADING("SPECIAL SQUARE MATRICES");
  EVAL2("SymmMatrix assign from fixed matrix", mySymmMatrix, O, myMatrix33, S, O = S);
  EVAL2("DiagMatrix assign from dense matrix", myDiagMatrix, D, myMatrix33, S, D = S);
  EVAL2("TridiagMatrix assign from dense matrix", myTridiagMatrix, T, myMatrix33, S, T = S);
  EVAL2("LowerMatrix assign from dense matrix", myLowerMatrix, L, myMatrix33, S, L = S);
  EVAL2("UpperMatrix assign from dense matrix", myUpperMatrix, U, myMatrix33, S, U = S);
  EVAL2("SymmMatrix as rvalue", myMatrix33, S, mySymmMatrix, O, S = O);
  EVAL2("DiagMatrix as rvalue", myMatrix33, S, myDiagMatrix, D, S = D);
  EVAL2("TridiagMatrix as rvalue", myMatrix33, S, myTridiagMatrix, T, S = T);
  EVAL2("LowerMatrix as rvalue", myMatrix33, S, myLowerMatrix, L, S = L);
  EVAL2("UpperMatrix as rvalue", myMatrix33, S, myUpperMatrix, U, S = U);

  EVAL2("Array submatrix_on_diagonal member function", myMatrix22, B, myMatrix33, S, B = S.submatrix_on_diagonal(1,2));
  EVAL("Array submatrix_on_diagonal member function as lvalue", myMatrix33, S, S.submatrix_on_diagonal(0,1) = 0);

  should_fail = true;
  EVAL2("Array submatrix_on_diagonal member function to non-square matrix", myMatrix22, B, myMatrix33, N, B = N.submatrix_on_diagonal(1,2));
  should_fail = false;

#ifndef MARVEL_STYLE
  if (adept::have_matrix_multiplication()) {
    HEADING("MATRIX MULTIPLICATION");
    EVAL2("Matrix-Matrix multiplication", myMatrix33, S, myMatrix23, M, S = M.T() ** M);
    EVAL2("Matrix-Matrix multiplication with matmul", myMatrix33, S, myMatrix23, M, S = matmul(M.T(), M));

    should_fail = true;
    EVAL2("Matrix-Matrix multiplication with inner dimension mismatch", myMatrix33, S, myMatrix23, M, S = M ** M);
    should_fail = false;

    // TESTING!
    EVAL2("Matrix-Matrix-Vector multiplication", myVector3, v, myMatrix33, S, v = S ** S ** v);

    EVAL2("Matrix-Matrix-Vector multiplication", myVector3, v, myMatrix33, S, v = S ** log(S) ** S(0,__));
    EVAL2("Vector-Matrix multiplication", myVector3, v, myMatrix33, S, v = v ** S);
    EVAL2("Vector-Matrix multiplication with matmul", myVector3, v, myMatrix33, S, v = matmul(v, S));
    EVAL2("SymmMatrix-Vector multiplication", myVector3, v, mySymmMatrix, O, v = O ** v);
    EVAL2("SymmMatrix-Matrix multiplication", myMatrix33, S, mySymmMatrix, O, S = O ** S);
    EVAL2("Vector-SymmMatrix multiplication", myVector3, v, mySymmMatrix, O, v = v ** O);
    EVAL2("Matrix-SymmMatrix multiplication", myMatrix23, M, mySymmMatrix, O, M = M ** O);
    EVAL2("DiagMatrix-Vector multiplication", myVector3, v, myDiagMatrix, D, v = D ** v);
    EVAL2("TridiagMatrix-Vector multiplication", myVector3, v, myTridiagMatrix, T, v = T ** v);
    EVAL2("TridiagMatrix-Matrix multiplication", myMatrix33, S, myTridiagMatrix, T, S = T ** S);
    EVAL2("Vector-TridiagMatrix multiplication", myVector3, v, myTridiagMatrix, T, v = v ** T);
    EVAL2("Matrix-TridiagMatrix multiplication", myMatrix23, M, myTridiagMatrix, T, M = M ** T);
  }
  else {
    std::cout << "NO MATRIX MULTIPLICATION TESTS PERFORMED BECAUSE ADEPT COMPILED WITHOUT LAPACK\n";
  }

#ifndef ALL_ACTIVE
  if (adept::have_linear_algebra()) {
    HEADING("LINEAR ALGEBRA");
    EVAL2("Solving general linear equations Ax=b", myVector3, v, myMatrix33, S, v = solve(S,v));

    EVAL2("Solving general linear equations AX=B", myMatrix23, M, myMatrix33, S, M.T() = solve(S,M.T()));
    EVAL2("Solving linear equations Ax=b with symmetric A", myVector3, v, mySymmMatrix, O, v = solve(O,v));
    EVAL2("Solving linear equations AX=B with symmetric A", myMatrix23, M, mySymmMatrix, O, M.T() = solve(O,M.T()));
    EVAL2("Invert general matrix", myMatrix33, C, myMatrix33, S, C = inv(S));
  }
  else {
    std::cout << "NO LINEAR ALGEBRA TESTS PERFORMED BECAUSE ADEPT COMPILED WITHOUT LAPACK\n";
  }
#else
    std::cout << "NO LINEAR ALGEBRA TESTS PERFORMED BECAUSE ACTIVE ARRAYS NOT YET SUPPORTED\n";
#endif
#else
    std::cout << "NO MATRIX TESTS PERFORMED BECAUSE USING MARVEL-STYLE ACTIVE ARRAYS\n";
#endif

  HEADING("FILLING ARRAYS");
  EVAL("Fill vector with \"<<\"", myVector3, v, (v << 0.1, 0.2));

  should_fail = true;
  EVAL("Overfill vector with \"<<\"", myVector3, v, (v << 0.1, 0.2, 0.3, 0.4));
  should_fail = false;

  EVAL("Underfill matrix with \"<<\"", myMatrix23, M, (M << 0.1, 0.2, 0.3, 0.4, 0.5));
  EVAL("Fill matrix with \"<<\"", myMatrix23, M, (M << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6));

  should_fail = true;
  EVAL("Overfill matrix with \"<<\"", myMatrix23, M, (M << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0));
  should_fail = false;

  EVAL("Fill vector with vectors using \"<<\"", myVector3, v, v << v(range(1,2)) << 0.1);
  EVAL2("Fill matrix with vector using \"<<\"", myMatrix23, M, myVector3, v, M << 0.1 << 0.2 << 0.3 << v);
  EVAL2("Fill matrix with vector using \"<<\"", myMatrix33, S, myVector3, v, S << v << v << v);
  EVAL("Assign array using range", myVector3, v, v = range(3,5));

#ifdef ADEPT_BOUNDS_CHECKING
  HEADING("BOUNDS CHECKING");
  should_fail = true;
  EVAL("Access vector out of bounds", myVector3, v, v(0) = v(4));
  EVAL("Access vector out of bounds", myVector3, v, v(0) = v(end-4));
  EVAL("Access matrix out of bounds", myMatrix23, M, M(0,0) = M(0,-1));
  EVAL("Access matrix out of bounds", myMatrix23, M, M(0,0) = M(end+1,1));
  should_fail = false;
#endif

  std::cout << "====================================================================\n";
  if (anomalous_results > 0) {
    std::cout << "*** In terms of run-time errors, there were " << anomalous_results << " incorrect results\n";
    return 1;
  }
  else {
    std::cout << "In terms of run-time errors, all tests were passed\n";
    return 0;
  }
}


#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/tuple.hpp>
#include <boost/python/list.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <boost/python/return_by_value.hpp>

#include <cctbx/sgtbx/select_generators.h>

namespace cctbx { namespace sgtbx { namespace boost_python {

  namespace {

    struct any_generator_set_wrapper
    {
      typedef select_generators::any wt;

      static boost::python::tuple generators(
        tr_vec inv_t, rt_mx const *gen, int n_gen)
      {
        using namespace boost::python;
        list result;
        if (inv_t.is_valid()) {
          result.append(rt_mx(rot_mx(1,-1), inv_t));
        }
        for(int i=0; i < n_gen; i++) result.append(gen[i]);
        return tuple(result);
      }

      static boost::python::tuple z_gen(wt const& self) {
        return generators(self.z_inv_t, self.z_gen, self.n_gen);
      }

      static boost::python::tuple p_gen(wt const& self) {
        return generators(self.p_inv_t, self.p_gen, self.n_gen);
      }

      static void wrap() {
        using namespace boost::python;
        typedef return_value_policy<return_by_value> rbv;
        typedef return_value_policy<copy_const_reference> ccr;
        class_<wt>("any_generator_set", no_init)
          .def(init<space_group const&, int, int>((
              arg("space_group"),
              arg("z2p_r_den")=cb_r_den,
              arg("z2p_t_den")=cb_t_den)))
          .def("z_gen", z_gen)
          .def("p_gen", p_gen)
          .def("set_primitive", &wt::set_primitive)
        ;
      }
    };

  } // anonymous namespace

  void wrap_select_generators()
  {
    any_generator_set_wrapper::wrap();
  }

}}} // namespace cctbx::sgtbx::select_generators::boost_python


#ifndef _MARSUPIAL_HPP_
#define _MARSUPIAL_HPP_

#include "g2o/core/optimization_algorithm_gauss_newton.h"
#include "g2o/types/icp/types_icp.h"

#include "g2o/stuff/command_args.h"

#include <Eigen/StdVector>
#include <random>
#include <stdint.h>

#include "g2o/solvers/dense/linear_solver_dense.h"

//g2o
#include <g2o/core/block_solver.h>
#include <g2o/core/factory.h>
#include <g2o/core/linear_solver.h>
#include <g2o/core/optimization_algorithm_factory.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/sparse_optimizer_terminate_action.h>

#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>

#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/solvers/pcg/linear_solver_pcg.h>

#include <g2o/stuff/macros.h>

#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/types/slam3d_addons/types_slam3d_addons.h>

#include "marsupial_optimizer/g2o_edge_distance_vertex.h"
#include "marsupial_optimizer/g2o_edge_obstacles.h"
#include "marsupial_optimizer/g2o_edge_obstacles_through.h"
#include "marsupial_optimizer/g2o_edge_distance_xyz.h"
#include "marsupial_optimizer/g2o_edge_kinematics.h"
#include "marsupial_optimizer/g2o_edge_equi_distance.h"
#include "marsupial_optimizer/g2o_edge_time.h"
#include "marsupial_optimizer/g2o_edge_velocity.h"
#include "marsupial_optimizer/g2o_edge_acceleration.h"

#include "marsupial_optimizer/g2o_vertex_timediff.h"
#include "g2o/types/slam3d/vertex_pointxyz.h"

// #include "marsupial_optimizer/obstacles.hpp"
#include "marsupial_optimizer/nearNeighbor.hpp"
#include "marsupial_optimizer/marsupial_cfg.hpp"

#include <iostream>
#include <fstream>
#include <vector>
#include <iterator>
#include <string>
#include <algorithm>
#include <boost/algorithm/string.hpp>

//ROS
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <marsupial_actions/OptimizationTrajectoryAction.h>
#include <visualization_msgs/MarkerArray.h>
#include <geometry_msgs/Point.h>
#include <sensor_msgs/PointCloud2.h>

#include <tf/transform_broadcaster.h>

using namespace Eigen;
using namespace std;
using namespace g2o;

class Marsupial
{

public:

	// ============= Global Variables ================
	ofstream file_in_pos,file_in_time ,file_in_velocity, file_in_difftime, file_in_acceleration;
	ofstream file_out_pos, file_out_time, file_out_velocity, file_out_difftime, file_out_acceleration;
	string path= "/home/simon/";
	double X1,Y1,Z1,X2,Y2,Z2;
	int n_iter_opt;	//Iterations Numbers of Optimizer
	int n_vert_unit; //number of Vertex per unit of lenght
	double w_alpha, w_beta, w_iota, w_gamma, w_delta, w_epsilon, w_zeta, w_eta, w_theta;

	double bound, velocity , angle, acceleration;

	MarsupialCfg mcfg;

	NearNeighbor nn_;

	SparseOptimizer optimizer;
	g2o::VertexPointXYZ* vertexPose;
	g2o::VertexTimeDiff* vertexTime;

	g2o::G2ODistanceVertexEdge* edgeDistanceVertex;
	g2o::G2ODistanceXYZEdge* edgeDistanceXYZ;
	g2o::G2OKinematicsEdge* edgeKinetics;
	g2o::G2OObstaclesEdge* edgeObstaclesNear;
	g2o::G2OThroughObstaclesEdge* edgeThroughObstacles;
	g2o::G2OEquiDistanceEdge* edgeEquiDistance;

	//! Manage Data Vertices and Edges
	// vector<Eigen::Vector3d> vecObs;
	vector<Eigen::Vector3d> vecOpt;
	vector<Eigen::Vector3d> near_obs;
	vector<float> mXYZ_;
	//! Create obstacles object
	// Obstacles _Obs1;
	// Obstacles _Obs2;
	// Obstacles _Obs3;

	actionlib::SimpleActionServer<marsupial_actions::OptimizationTrajectoryAction> optimization_trajectory_action_server_;
	marsupial_actions::OptimizationTrajectoryFeedback feedback_; //variable stores the feedback/intermediate results
    marsupial_actions::OptimizationTrajectoryResult result_; //variable stores the final output

	// bool *finish;
	// bool aux;

	std::string action_name_;
	std::string robot_base_frame_id_;

	ros::Subscriber read_octomap_sub_;
	ros::Publisher init_traj_pub_,final_traj_pub_, line_traj_pub_, main_obstacle_pub_;

	geometry_msgs::Point obs_oct;

	int n_time_optimize;
	bool continue_optimizing;
	double td_;

	double initial_pos_robot_x, initial_pos_robot_y, initial_pos_robot_z;

	// =========== Function declarations =============
	Marsupial(ros::NodeHandle &nh, ros::NodeHandle &pnh, std::string name);
	// ~Marsupial();
	void setupOptimization();
	void executeOptimization(const marsupial_actions::OptimizationTrajectoryGoalConstPtr &goal);

	void initializeSubscribers(ros::NodeHandle &nh);
	void initializePublishers(ros::NodeHandle &nh);
	void readOctomapCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);
	void getMarkerArray(visualization_msgs::MarkerArray &_marker, Eigen::Vector3d _p);
	visualization_msgs::Marker getMarker(double _size , Eigen::Vector3d _vo );
	void tfBroadcaster(void);

};

Marsupial::Marsupial(ros::NodeHandle &nh, ros::NodeHandle &pnh,std::string name):
			mcfg(),
			optimization_trajectory_action_server_(name,boost::bind(&Marsupial::executeOptimization, this,_1),false),
			action_name_(name){

	pnh.param<double>("X1", X1, 25.0);
  	pnh.param<double>("Y1", Y1, 40.0);
	pnh.param<double>("Z1", Z1, 50.0);
	pnh.param<double>("X2", X2, 50.0);
  	pnh.param<double>("Y2", Y2, 15.0);
  	pnh.param<double>("Z2", Z2, 40.0);

	pnh.param<double>("initial_pos_robot_x", initial_pos_robot_x, 0.0);
  	pnh.param<double>("initial_pos_robot_y", initial_pos_robot_y, 0.0);
  	pnh.param<double>("initial_pos_robot_z", initial_pos_robot_z, 0.0);

	pnh.param<double>("w_alpha", w_alpha,0.1);
	pnh.param<double>("w_beta",	w_beta,0.1);
	pnh.param<double>("w_iota",	w_iota,0.1);
  	pnh.param<double>("w_gamma", w_gamma,0.1);
  	pnh.param<double>("w_delta", w_delta,0.1);
  	pnh.param<double>("w_epsilon", w_epsilon,0.1);
  	pnh.param<double>("w_zeta", w_zeta,0.1);
  	pnh.param<double>("w_eta", w_eta,0.1);
  	pnh.param<double>("w_theta", w_theta,0.1);

	pnh.param<int>("n_iter_opt", n_iter_opt,200);
  	pnh.param<int>("n_vert_unit", n_vert_unit,4);
  	pnh.param<double>("bound", bound,2.0);
  	pnh.param<double>("velocity", velocity,1.0);
  	pnh.param<double>("acceleration", acceleration,0.0);
  	pnh.param<double>("angle", angle, M_PI / 15.0);

  	pnh.param<int>("n_time_optimize", n_time_optimize, 500);
  	pnh.param<double>("td_", td_, 0.5);

  	pnh.param("robot_base_frame_id_", robot_base_frame_id_, (std::string)"/base_link");

	continue_optimizing = false;

	initializeSubscribers(nh);
	initializePublishers(nh);

	setupOptimization();
	optimization_trajectory_action_server_.start();

	ROS_INFO("Parameters loaded, ready to Initialize Optimization Proccess !!");
}

void Marsupial::initializeSubscribers(ros::NodeHandle &nh)
{
    read_octomap_sub_ = nh.subscribe( "octomap_point_cloud_centers", 2,  &Marsupial::readOctomapCallback, this);
    ROS_INFO("Subscribers Initialized marsupial_node");
}

void Marsupial::initializePublishers(ros::NodeHandle &nh)
{
    init_traj_pub_ = nh.advertise<visualization_msgs::MarkerArray>("initial_trajectory", 2);
    final_traj_pub_ = nh.advertise<visualization_msgs::MarkerArray>("final_trajectory", 2);
    line_traj_pub_ = nh.advertise<visualization_msgs::MarkerArray>("line_trajectory", 2);
	main_obstacle_pub_ = nh.advertise<visualization_msgs::Marker>("main_obstacle", 2);

    ROS_INFO("Publishers Initialized marsupial_node");
}

void Marsupial::readOctomapCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
	  nn_.setInput(*msg);
	//   printf("1  nn_.obs_points=[%lu]",nn_.obs_points->size());
}

void Marsupial::setupOptimization()
{
	mcfg.setNumVertUnit(n_vert_unit);
	mcfg.setVelocity(velocity);
	// mcfg.setDistance3D(X1,Y1,Z1,X2,Y2,Z2);
	// mcfg.setNumPoints(mcfg.d3D_,mcfg.n_vert_unit);

	//! Create the linear solver (Ax = b)
	auto linearSolver = g2o::make_unique<LinearSolverCSparse<BlockSolverX::PoseMatrixType>>();
    // auto linearSolver = g2o::make_unique<LinearSolverDense<BlockSolverX::PoseMatrixType>>(); // Linear equation solver

	auto blockSolver = g2o::make_unique<BlockSolverX>(move(linearSolver));

	OptimizationAlgorithmLevenberg* optimizationAlgorithm = new OptimizationAlgorithmLevenberg(move(blockSolver));
	// OptimizationAlgorithmGaussNewton* optimizationAlgorithm = new g2o::OptimizationAlgorithmGaussNewton(move(blockSolver));
    // OptimizationAlgorithmDogleg* optimizationAlgorithm = new OptimizationAlgorithmDogleg(move(blockSolver));

	// optimizationAlgorithm->setMaxTrialsAfterFailure(3);
	optimizationAlgorithm->setUserLambdaInit(0.00001);

	optimizer.clear();
	optimizer.setAlgorithm(optimizationAlgorithm);
    optimizer.setVerbose(true);

	//! Create the vectors with initial poses and distances between vertex
	ROS_INFO("Loading Trajectory Points");
	mcfg.setStraightTrajectory(X1,Y1,Z1,X2,Y2,Z2,n_vert_unit);
	ROS_INFO("X1=[%f] Y1=[%f] Z1=[%f] X2=[%f] Y2=[%f] Z2=[%f]\n",X1,Y1,Z1,X2,Y2,Z2);
	ROS_INFO("alpha=[%f] beta=[%f] gamma=[%f] delta=[%f] epsilon=[%f] zeta=[%f] eta=[%f] theta=[%f]",w_alpha,w_beta,w_gamma,w_delta,w_epsilon,w_zeta,w_eta,w_theta);

	//! Create Objet for kdtree
	mcfg.getSlopXYZAxes(mXYZ_);
}

void Marsupial::executeOptimization(const marsupial_actions::OptimizationTrajectoryGoalConstPtr &goal)
{

	for (int j_ = 0; j_ < n_time_optimize; j_++)
	{
		if (!continue_optimizing)
			printf("Preparing to execute optimization!!\n");
			// printf("Preparing to execute optimization [%i]!!\n",j_);
		// delete vertexPose;
		vertexPose = new g2o::VertexPointXYZ;
		vertexTime = new g2o::VertexTimeDiff;

		visualization_msgs::MarkerArray initial_marker, final_marker , line_strip;
		visualization_msgs::Marker main_obstacle_marker_;

		initial_marker.markers.clear();
		final_marker.markers.clear();
		line_strip.markers.clear();
		initial_marker.markers.resize(mcfg.pose_vec_.size());
		final_marker.markers.resize(mcfg.pose_vec_.size());
		line_strip.markers.resize(mcfg.pose_vec_.size());

		/*
		*Create Vertex. Points of the trajectory
		*/
		//! To save points cable before and after optimization
		for (size_t i = 0; i < mcfg.pose_vec_.size(); ++i)
		{
			vertexPose = new g2o::VertexPointXYZ();
			Eigen::Vector3d xyz;
			if(!continue_optimizing){
				xyz = mcfg.pose_vec_[i].position;
				initial_marker.markers[i].header.frame_id = "/map";
				initial_marker.markers[i].header.stamp = ros::Time::now();
				initial_marker.markers[i].ns = "points";
				initial_marker.markers[i].id = i;
				// initial_marker.markers[i].action = visualization_msgs::Marker::DELETEALL;
				initial_marker.markers[i].type = visualization_msgs::Marker::SPHERE;
				initial_marker.markers[i].lifetime = ros::Duration(0);
				initial_marker.markers[i].pose.position.x = xyz.x();
				initial_marker.markers[i].pose.position.y = xyz.y();
				initial_marker.markers[i].pose.position.z = xyz.z();
				initial_marker.markers[i].pose.orientation.x = 0.0;
				initial_marker.markers[i].pose.orientation.y = 0.0;
				initial_marker.markers[i].pose.orientation.z = 0.0;
				initial_marker.markers[i].pose.orientation.w = 1.0;
				initial_marker.markers[i].scale.x = 0.1;
				initial_marker.markers[i].scale.y = 0.1;
				initial_marker.markers[i].scale.z = 0.1;
				initial_marker.markers[i].color.a = 1.0;
				initial_marker.markers[i].color.r = 0.0;
				initial_marker.markers[i].color.g = 0.9;
				initial_marker.markers[i].color.b = 0.0;

				vertexPose->setId(mcfg.pose_vec_[i].id);
				vertexPose->setEstimate(xyz);
				//First and last vertex are fixed.
				if (i == 0 || i == mcfg.pose_vec_.size()-1)
					vertexPose->setFixed(true);
			}
			else{
				vertexPose->setId(mcfg.pose_vec_[i].id);
				vertexPose->setEstimate(vecOpt[i]);
					//First and last vertex are fixed.
				if (i == 0 || i == mcfg.pose_vec_.size()-1)
					vertexPose->setFixed(true);
			}
			optimizer.addVertex(vertexPose);

		}
		if (!continue_optimizing)
			init_traj_pub_.publish(initial_marker);

		//! I. Add edges. These are the distances between the poses.
		for (size_t i = 0; i < mcfg.dist_vec_.size(); ++i)
		{
			edgeDistanceVertex = new g2o::G2ODistanceVertexEdge();
			edgeDistanceVertex->vertices()[0] = optimizer.vertices()[mcfg.dist_vec_[i].id_from];
			edgeDistanceVertex->vertices()[1] = optimizer.vertices()[mcfg.dist_vec_[i].id_to];
			edgeDistanceVertex->setMeasurement(mcfg.d3D_/mcfg.n_points);
			edgeDistanceVertex->setInformation(G2ODistanceVertexEdge::InformationType::Identity()*w_alpha);
			optimizer.addEdge(edgeDistanceVertex);
		}
		//! II. Add edges. These are the XYZ distances between Vertices.
		for (size_t i = 0; i < mcfg.dist_vec_.size(); ++i)
		{
			edgeDistanceXYZ = new g2o::G2ODistanceXYZEdge();
			edgeDistanceXYZ->vertices()[0] = optimizer.vertices()[mcfg.dist_vec_[i].id_from];
			edgeDistanceXYZ->vertices()[1] = optimizer.vertices()[mcfg.dist_vec_[i].id_to];
			edgeDistanceXYZ->setMeasurement(mXYZ_);
			edgeDistanceXYZ->setInformation(G2ODistanceXYZEdge::InformationType::Identity()*w_gamma);
			optimizer.addEdge(edgeDistanceXYZ);
		}
		// ! III. Add edges. This is Equi-distance among vertices.
		for (size_t i = 0; i < mcfg.pose_vec_.size()-3; ++i)
		{
			edgeEquiDistance = new g2o::G2OEquiDistanceEdge;
			edgeEquiDistance->vertices()[0] = optimizer.vertices()[i];
			edgeEquiDistance->vertices()[1] = optimizer.vertices()[i+1];
			edgeEquiDistance->vertices()[2] = optimizer.vertices()[i+2];
			edgeEquiDistance->setInformation(G2OEquiDistanceEdge::InformationType::Identity()*w_epsilon);
			optimizer.addEdge(edgeEquiDistance);
		}
		//! IV. Add edges. These are the distances from vertex to obstacles poses.
		for (size_t i = 0; i < mcfg.pose_vec_.size(); ++i)
		{
			edgeObstaclesNear = new g2o::G2OObstaclesEdge();
			edgeObstaclesNear->vertices()[0] = optimizer.vertices()[i];
			edgeObstaclesNear->readKDTree(nn_.kdtree,nn_.obs_points);
			edgeObstaclesNear->setMeasurement(bound);
			edgeObstaclesNear->setInformation(G2OObstaclesEdge::InformationType::Identity()*w_beta);
			optimizer.addEdge(edgeObstaclesNear);
		}
		//! V. Add edges. These are the distances from vertex to obstacles poses.
		for (size_t i = 0; i < mcfg.dist_vec_.size(); ++i)
		{
			edgeThroughObstacles = new g2o::G2OThroughObstaclesEdge();
			edgeThroughObstacles->vertices()[0] = optimizer.vertices()[mcfg.dist_vec_[i].id_from];
			edgeThroughObstacles->vertices()[1] = optimizer.vertices()[mcfg.dist_vec_[i].id_to];
			edgeThroughObstacles->setRobustKernel(new g2o::RobustKernelCauchy());
            edgeThroughObstacles->robustKernel()->setDelta(1.0);
			edgeThroughObstacles->readKDTree(nn_.kdtree,nn_.obs_points);
			edgeThroughObstacles->setInformation(G2OThroughObstaclesEdge::InformationType::Identity()*w_iota);
			optimizer.addEdge(edgeThroughObstacles);
		}
		// VI. Add edges. This is kinematic of trajectory.
		for (size_t i = 0; i < mcfg.pose_vec_.size()-3; ++i)
		{
			edgeKinetics = new g2o::G2OKinematicsEdge;
			edgeKinetics->vertices()[0] = optimizer.vertices()[i];
			edgeKinetics->vertices()[1] = optimizer.vertices()[i+1];
			edgeKinetics->vertices()[2] = optimizer.vertices()[i+2];
			edgeKinetics->setMeasurement(angle);
			edgeKinetics->setInformation(G2OKinematicsEdge::InformationType::Identity()*w_delta);
			optimizer.addEdge(edgeKinetics);
		}
		/*
		*Create Vertex. Time between consecutive configuration
		*/
		//! To save temporal state before optimization
		// file_in_time.open ((path+"initial_time.txt"));
		// file_in_velocity.open ((path+"initial_velocity.txt"));
		// file_in_difftime.open ((path+"initial_difftime.txt"));
		// file_in_acceleration.open ((path+"initial_acceleration.txt"));
		// double _dT = 0.0;
		// double _sum_dT = 0.0;
		// double _sum_dist = 0.0;
		// for (size_t i = 0; i < mcfg.dist_vec_.size(); ++i)
		// {
		// 	_dT = mcfg.time_vec_[i].time;
		// 	_sum_dT = _dT + _sum_dT;
		// 	_sum_dist = mcfg.dist_vec_[i].dist + _sum_dist;
		// 	vertexPose = new g2o::VertexTimeDiff();
		// 	if (i == 0)	//First vertex is fixed.
		// 		vertexTime->setFixed(true);
		// 		// vertexTime = new g2o::VertexTimeDiff(_dT,true);
		// 	else
		// 		vertexTime->setFixed(false);
		// 		// vertexTime = new g2o::VertexTimeDiff(_dT,false);
		// 	vertexTime->setId(i+mcfg.dist_vec_.size()+1);
		// 	vertexTime->setEstimate(_dT);
		// 	optimizer.addVertex(vertexTime);
		// 	file_in_time << setprecision(6) << _sum_dist << ";" << _sum_dT << endl;
		// 	file_in_velocity  << setprecision(6) << _sum_dist << ";" << velocity << endl;
		// 	file_in_difftime << setprecision(6) << _sum_dist << ";" << _dT << endl;
		// 	if (i > 0)
		// 		file_in_acceleration << setprecision(6) << _sum_dist << ";" << acceleration << endl;
		// }
		// file_in_time.close();
		// file_in_velocity.close();
		// file_in_difftime.close();
		// file_in_acceleration.close();

		//! VI. Add edges. These are time between Vertices.
		// for (unsigned i=mcfg.dist_vec_.size()+1; i < mcfg.dist_vec_.size()*2.0; ++i)
		// {
		// 	G2OTimeEdge* edge = new G2OTimeEdge;
		// 	edge->vertices()[0] = optimizer.vertices()[i];
		// 	edge->setMeasurement(mcfg.time_vec_[i-mcfg.dist_vec_.size()-1].time);
		// 	edge->setInformation(G2OTimeEdge::InformationType::Identity()*w_zeta);
		// 	optimizer.addEdge(edge);
		// }

		//! VII. Add edges. These are velocities between Vertices.
		// for (size_t i = 0; i < mcfg.dist_vec_.size(); ++i)
		// {
		// 	G2OVelocityEdge* edge = new G2OVelocityEdge;
		// 	edge->vertices()[0] = optimizer.vertices()[mcfg.dist_vec_[i].id_from];
		// 	edge->vertices()[1] = optimizer.vertices()[mcfg.dist_vec_[i].id_to];
		// 	edge->vertices()[2] = optimizer.vertices()[i+mcfg.dist_vec_.size()+1];
		// 	edge->setMeasurement(velocity);
		// 	edge->setInformation(G2OVelocityEdge::InformationType::Identity()*w_eta);
		// 	optimizer.addEdge(edge);
		// }

		//! VIII. Add edges. This is aceleration between Vertices.
		// for (size_t i = 0; i < mcfg.dist_vec_.size()-1; ++i)
		// {
		// 	G2OAccelerationEdge* edge = new G2OAccelerationEdge;
		// 	edge->vertices()[0] = optimizer.vertices()[i];
		// 	edge->vertices()[1] = optimizer.vertices()[i+1];
		// 	edge->vertices()[2] = optimizer.vertices()[i+2];
		// 	edge->vertices()[3] = optimizer.vertices()[i+mcfg.dist_vec_.size()+1];
		// 	edge->vertices()[4] = optimizer.vertices()[i+mcfg.dist_vec_.size()+2];
		// 	edge->setMeasurement(acceleration);
		// 	edge->setInformation(G2OAccelerationEdge::InformationType::Identity()*w_theta);
		// 	optimizer.addEdge(edge);
		// }

		if (!continue_optimizing)
			std::cout << "Optimization  started !!" << std::endl;
		// optimizer.save("antithing_before.g2o");
		optimizer.initializeOptimization();
		optimizer.setVerbose(true);
		optimizer.optimize(n_iter_opt);
		// optimizer.save("antithing_after.g2o");

		//! Save Optimized Trajectory in File.txt
		// file_out_pos.open (path+"optimized_trajectory.txt");
		for (unsigned i = 0; i < mcfg.pose_vec_.size(); i++)
		{
			structPose vOp;
			g2o::VertexPointXYZ *pose = dynamic_cast<g2o::VertexPointXYZ*>(optimizer.vertex(i));
			vOp.position = pose->estimate();
			vOp.id = i;
			// file_out_pos << setprecision(6) << vOp.position.x() << ";" << vOp.position.y() << ";" << vOp.position.z()<< endl;

			final_marker.markers[i].header.frame_id = "/map";
			final_marker.markers[i].header.stamp = ros::Time::now();
			final_marker.markers[i].ns = "points";
			final_marker.markers[i].id = i;
			// final_marker.markers[i].action = visualization_msgs::Marker::ADD;
			if (i % 5 == 0)
				final_marker.markers[i].type = visualization_msgs::Marker::CUBE;
			else
				final_marker.markers[i].type = visualization_msgs::Marker::SPHERE;
			if (j_+1 == n_time_optimize)
				final_marker.markers[i].lifetime = ros::Duration(0);
			else
				final_marker.markers[i].lifetime = ros::Duration(2.0);
			final_marker.markers[i].pose.position.x = vOp.position.x();
			final_marker.markers[i].pose.position.y = vOp.position.y();
			final_marker.markers[i].pose.position.z = vOp.position.z();
			final_marker.markers[i].pose.orientation.x = 0.0;
			final_marker.markers[i].pose.orientation.y = 0.0;
			final_marker.markers[i].pose.orientation.z = 0.0;
			final_marker.markers[i].pose.orientation.w = 1.0;
			final_marker.markers[i].scale.x = 0.1;
			final_marker.markers[i].scale.y = 0.1;
			final_marker.markers[i].scale.z = 0.1;
			final_marker.markers[i].color.a = 1.0;
			final_marker.markers[i].color.r = 0.0;
			final_marker.markers[i].color.g = 0.0;
			final_marker.markers[i].color.b = 0.9;

			// geometry_msgs::Point _p1, _p2;
			// if (i > 0)
			// {
			// 	_p2.x= vOp.position.x();
			// 	_p2.y= vOp.position.y();
			// 	_p2.z= vOp.position.z();
			// 	line_strip.markers[i].header.frame_id = "/map";
			// 	line_strip.markers[i].header.stamp = ros::Time::now();
			// 	line_strip.markers[i].ns = "points";
			// 	line_strip.markers[i].id = i;
			// 	line_strip.markers[i].type = visualization_msgs::Marker::LINE_STRIP;
			// 	if (j_+1 == n_time_optimize)
			// 		line_strip.markers[i].lifetime = ros::Duration(0);
			// 	else
			// 		line_strip.markers[i].lifetime = ros::Duration(1.0);
			// 	line_strip.markers[i].points.push_back(_p1);
			// 	line_strip.markers[i].points.push_back(_p2);
			// 	line_strip.markers[i].pose.orientation.x = 0.0;
			// 	line_strip.markers[i].pose.orientation.y = 0.0;
			// 	line_strip.markers[i].pose.orientation.z = 0.0;
			// 	line_strip.markers[i].pose.orientation.w = 1.0;
			// 	line_strip.markers[i].scale.x = 0.02;
			// 	line_strip.markers[i].color.a = 1.0;
			// 	line_strip.markers[i].color.r = 0.0;
			// 	line_strip.markers[i].color.g = 0.0;
			// 	line_strip.markers[i].color.b = 0.9;
			// }
			// _p1.x= vOp.position.x();
			// _p1.y= vOp.position.y();
			// _p1.z= vOp.position.z();
		}
		// file_out_pos.close();

		// printf("size=[%i] main_obstacle_marker=[%f %f %f]\n",edgeThroughObstacles->_size_vector_marker,edgeThroughObstacles->_main_obstacle_marker.x()
		// ,edgeThroughObstacles->_main_obstacle_marker.y(),edgeThroughObstacles->_main_obstacle_marker.z());
		// main_obstacle_marker_ = getMarker(0.2*edgeThroughObstacles->_size_vector_marker,edgeThroughObstacles->_main_obstacle_marker);
		// main_obstacle_pub_.publish(main_obstacle_marker_);
		final_traj_pub_.publish(final_marker);
		// line_traj_pub_.publish(line_strip);

		vecOpt.clear();
		for (size_t i = 0; i < mcfg.pose_vec_.size(); i++)
		{
			g2o::VertexPointXYZ *pose = dynamic_cast<g2o::VertexPointXYZ*>(optimizer.vertex(i));
			vecOpt.push_back(pose->estimate());

			optimizer.removeVertex(optimizer.vertex(i));
		}
		continue_optimizing = true;

		//! Save Optimized Time in File.txt
		// file_out_time.open (path+"optimized_time.txt");
		// file_out_velocity.open (path+"optimized_velocity.txt");
		// file_out_difftime.open (path+"optimized_difftime.txt");
		// double sum_dispos = 0.0;
		// double sum_diffTime = 0.0;
		// double velocity = 0.0;
		// for (size_t i = 0; i < mcfg.dist_vec_.size(); i++)
		// {
		// 	g2o::VertexPointXYZ *pose1Op = dynamic_cast<g2o::VertexPointXYZ*>(optimizer.vertex(i));
		// 	g2o::VertexPointXYZ *pose2Op = dynamic_cast<g2o::VertexPointXYZ*>(optimizer.vertex(i+1));
		// 	g2o::VertexTimeDiff *difftime = dynamic_cast<g2o::VertexTimeDiff*>(optimizer.vertex(i+mcfg.dist_vec_.size()+1));
		// 	double distance = (pose2Op->estimate()-pose1Op->estimate()).norm();
		// 	sum_dispos = distance + sum_dispos;
		// 	sum_diffTime = difftime->estimate()+ sum_diffTime;
		// 	velocity = distance / difftime->estimate();
		// 	file_out_time << setprecision(6) << sum_dispos << ";" << sum_diffTime << endl;
		// 	file_out_velocity << setprecision(6) << sum_dispos << ";" << velocity << endl;
		// 	file_out_difftime << setprecision(6) << sum_dispos << ";" << difftime->estimate() << endl;
		// 	// printf("p1=[%f %f %f]  p2=[%f %f %f] d=%f Sum_d=%f\n",pose1Op->estimate().x(),pose1Op->estimate().y(),pose1Op->estimate().z(),pose2Op->estimate().x(),pose2Op->estimate().y(),pose2Op->estimate().z(),distance,sum_dispos);
		// }
		// file_out_time.close();
		// file_out_velocity.close();
		// file_out_difftime.close();

		// file_out_acceleration.open (path+"optimized_acceleration.txt");
		// double sumdis = 0.0;
		// for (size_t i = 0; i < mcfg.dist_vec_.size()-1; i++)
		// {
		// 	g2o::VertexPointXYZ *pose1Op = dynamic_cast<g2o::VertexPointXYZ*>(optimizer.vertex(i));
		// 	g2o::VertexPointXYZ *pose2Op = dynamic_cast<g2o::VertexPointXYZ*>(optimizer.vertex(i+1));
		// 	g2o::VertexPointXYZ *pose3Op = dynamic_cast<g2o::VertexPointXYZ*>(optimizer.vertex(i+2));
		// 	g2o::VertexTimeDiff *difftime1 = dynamic_cast<g2o::VertexTimeDiff*>(optimizer.vertex(i+mcfg.dist_vec_.size()+1));
		// 	g2o::VertexTimeDiff *difftime2 = dynamic_cast<g2o::VertexTimeDiff*>(optimizer.vertex(i+mcfg.dist_vec_.size()+1));

		// 	double distance1 = (pose2Op->estimate()-pose1Op->estimate()).norm();
		// 	double distance2 = (pose3Op->estimate()-pose2Op->estimate()).norm();
		// 	if (i==0)
		// 		sumdis = distance1;
		// 	sumdis = sumdis + distance2;
		// 	double diffTime = difftime1->estimate()+ difftime2->estimate();
		// 	double velocity1 = distance1 / difftime1->estimate();
		// 	double velocity2 = distance2 / difftime2->estimate();
		// 	double acceleration = (velocity2-velocity1)/diffTime;
		// 	file_out_acceleration << setprecision(6) << sumdis << ";" << acceleration << endl;
		// }
		// file_out_acceleration.close();

		ros::Duration(td_).sleep();
	}
	if (optimization_trajectory_action_server_.isPreemptRequested() || !ros::ok())
    {
        optimizer.clear();
        ROS_INFO("%s: Preempted", action_name_.c_str());
        result_.optimization_completed = false;
        optimization_trajectory_action_server_.setPreempted(result_);
    }

	std::cout << "Optimization Proccess  Completed !!!" << std::endl;
	result_.optimization_completed = true;
	ROS_INFO("%s: Succeeded", action_name_.c_str());
	optimization_trajectory_action_server_.setSucceeded(result_);
	optimizer.clear();
}

void Marsupial::tfBroadcaster(void){
  static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(initial_pos_robot_x,initial_pos_robot_y,initial_pos_robot_z) );
  tf::Quaternion q;
  q.setRPY(0, 0, 0);
  transform.setRotation(q);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", robot_base_frame_id_));
}

void Marsupial::getMarkerArray(visualization_msgs::MarkerArray &_marker, Eigen::Vector3d _p)
{
	for (size_t i = 0; i < mcfg.pose_vec_.size(); ++i)
	{
		_marker.markers[i].header.frame_id = "map";
		_marker.markers[i].header.stamp = ros::Time::now();
		_marker.markers[i].ns = "points";
		_marker.markers[i].id = i;
		// _marker.markers[i].action = visualization_msgs::Marker::ADD;
		_marker.markers[i].type = visualization_msgs::Marker::SPHERE;
		_marker.markers[i].lifetime = ros::Duration(0);
		_marker.markers[i].pose.position.x = _p.x();
		_marker.markers[i].pose.position.y = _p.y();
		_marker.markers[i].pose.position.z = _p.z();
		_marker.markers[i].pose.orientation.x = 0.0;
		_marker.markers[i].pose.orientation.y = 0.0;
		_marker.markers[i].pose.orientation.z = 0.0;
		_marker.markers[i].pose.orientation.w = 1.0;
		_marker.markers[i].scale.x = 0.1;
		_marker.markers[i].scale.y = 0.1;
		_marker.markers[i].scale.z = 0.1;
		_marker.markers[i].color.a = 1.0;
		_marker.markers[i].color.r = 0.0;
		_marker.markers[i].color.g = 0.9;
		_marker.markers[i].color.b = 0.0;
	}
}

visualization_msgs::Marker Marsupial::getMarker(double _size , Eigen::Vector3d _vo )
{
	visualization_msgs::Marker marker_;
	marker_.header.frame_id = "map";
	marker_.header.stamp = ros::Time::now();
	marker_.ns = "points";
	marker_.id = 1;
	// _marker.action = visualization_msgs::Marker::ADD;
	marker_.type = visualization_msgs::Marker::SPHERE;
	marker_.lifetime = ros::Duration(0);
	marker_.pose.position.x = _vo.x();
	marker_.pose.position.y = _vo.y();
	marker_.pose.position.z = _vo.z();
	marker_.pose.orientation.x = 0.0;
	marker_.pose.orientation.y = 0.0;
	marker_.pose.orientation.z = 0.0;
	marker_.pose.orientation.w = 1.0;
	marker_.scale.x = _size;
	marker_.scale.y = _size;
	marker_.scale.z = _size;
	marker_.color.a = 1.0;
	marker_.color.r = 0.9;
	marker_.color.g = 0.0;
	marker_.color.b = 0.0;

	return marker_;
}

#endif


/* Copyright (c) 2016 - 2021, the adamantine authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#include <Geometry.hh>
#include <ThermalPhysics.hh>

#include <deal.II/base/quadrature_lib.h>

template <typename MemorySpaceType>
void thermal_2d(boost::property_tree::ptree &database, double time_step)
{
  MPI_Comm communicator = MPI_COMM_WORLD;

  // Geometry database
  boost::property_tree::ptree geometry_database;
  geometry_database.put("import_mesh", false);
  geometry_database.put("length", 12e-3);
  geometry_database.put("length_divisions", 4);
  geometry_database.put("height", 6e-3);
  geometry_database.put("height_divisions", 5);
  // Build Geometry
  adamantine::Geometry<2> geometry(communicator, geometry_database);
  // Material property
  database.put("materials.property_format", "polynomial");
  database.put("materials.n_materials", 1);
  database.put("materials.material_0.solid.density", 1.);
  database.put("materials.material_0.powder.density", 1.);
  database.put("materials.material_0.liquid.density", 1.);
  database.put("materials.material_0.solid.specific_heat", 1.);
  database.put("materials.material_0.powder.specific_heat", 1.);
  database.put("materials.material_0.liquid.specific_heat", 1.);
  database.put("materials.material_0.solid.thermal_conductivity", 1.);
  database.put("materials.material_0.powder.thermal_conductivity", 1.);
  database.put("materials.material_0.liquid.thermal_conductivity", 1.);
  // Source database
  database.put("sources.n_beams", 1);
  database.put("sources.beam_0.depth", 1e100);
  database.put("sources.beam_0.diameter", 1e100);
  database.put("sources.beam_0.max_power", 1e300);
  database.put("sources.beam_0.absorption_efficiency", 0.1);
  database.put("sources.beam_0.type", "electron_beam");
  database.put("sources.beam_0.scan_path_file",
               "scan_path_test_thermal_physics.txt");
  database.put("sources.beam_0.scan_path_file_format", "segment");
  // Boundary database
  database.put("boundary.type", "adiabatic");

  // Build ThermalPhysics
  adamantine::ThermalPhysics<2, 2, MemorySpaceType, dealii::QGauss<1>> physics(
      communicator, database, geometry);
  physics.setup_dofs();
  physics.compute_inverse_mass_matrix();

  dealii::LA::distributed::Vector<double, MemorySpaceType> solution;
  physics.initialize_dof_vector(solution);
  physics.get_state_from_material_properties();
  std::vector<adamantine::Timer> timers(adamantine::Timing::n_timers);
  double time = 0;
  while (time < 0.1)
    time = physics.evolve_one_time_step(time, time_step, solution, timers);

  double const tolerance = 1e-3;
  BOOST_CHECK(time == 0.1);
  BOOST_CHECK_CLOSE(solution.l2_norm(), 0.291705, tolerance);

  physics.initialize_dof_vector(1000., solution);
  BOOST_CHECK(solution.l1_norm() == 1000. * solution.size());
}

template <typename MemorySpaceType>
void thermal_2d_manufactured_solution()
{
  MPI_Comm communicator = MPI_COMM_WORLD;

  // Geometry database
  boost::property_tree::ptree geometry_database;
  geometry_database.put("import_mesh", false);
  geometry_database.put("length", 1e3);
  geometry_database.put("length_divisions", 4);
  geometry_database.put("height", 6e3);
  geometry_database.put("height_divisions", 5);
  // Build Geometry
  adamantine::Geometry<2> geometry(communicator, geometry_database);

  boost::property_tree::ptree database;
  // Material property
  database.put("materials.property_format", "polynomial");
  database.put("materials.n_materials", 1);
  database.put("materials.material_0.solid.density", 1.);
  database.put("materials.material_0.powder.density", 1.);
  database.put("materials.material_0.liquid.density", 1.);
  database.put("materials.material_0.solid.specific_heat", 1.);
  database.put("materials.material_0.powder.specific_heat", 1.);
  database.put("materials.material_0.liquid.specific_heat", 1.);
  database.put("materials.material_0.solid.thermal_conductivity", 1.);
  database.put("materials.material_0.powder.thermal_conductivity", 1.);
  database.put("materials.material_0.liquid.thermal_conductivity", 1.);
  // Source database
  database.put("sources.n_beams", 1);
  database.put("sources.beam_0.depth", 1e100);
  database.put("sources.beam_0.diameter", 1e100);
  database.put("sources.beam_0.max_power", 1e300);
  database.put("sources.beam_0.absorption_efficiency",
               0.1 / 0.29317423955177113);
  database.put("sources.beam_0.scan_path_file",
               "scan_path_test_thermal_physics.txt");
  database.put("sources.beam_0.type", "electron_beam");
  database.put("sources.beam_0.scan_path_file_format", "segment");
  // Boundary database
  database.put("boundary.type", "adiabatic");

  // Time-stepping database
  database.put("time_stepping.method", "rk_fourth_order");
  // Build ThermalPhysics
  adamantine::ThermalPhysics<2, 2, MemorySpaceType, dealii::QGauss<1>> physics(
      communicator, database, geometry);
  physics.setup_dofs();
  physics.compute_inverse_mass_matrix();

  dealii::LA::distributed::Vector<double, MemorySpaceType> solution;
  std::vector<adamantine::Timer> timers(adamantine::Timing::n_timers);
  physics.initialize_dof_vector(solution);
  physics.get_state_from_material_properties();
  double time = physics.evolve_one_time_step(0., 0.1, solution, timers);

  double const tolerance = 1e-5;

  BOOST_CHECK(time == 0.1);

  BOOST_CHECK_SMALL(std::abs(physics.get_current_source_height() - 0.0),
                    tolerance);

  if (std::is_same<MemorySpaceType, dealii::MemorySpace::Host>::value)
  {
    for (unsigned int i = 0; i < solution.locally_owned_size(); ++i)
      BOOST_CHECK_CLOSE(solution.local_element(i), 0.1, tolerance);
  }
  else
  {
    dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host>
        solution_host(solution.get_partitioner());
    solution_host.import(solution, dealii::VectorOperation::insert);
    for (unsigned int i = 0; i < solution.size(); ++i)
      BOOST_CHECK_CLOSE(solution_host[i], 0.1, tolerance);
  }
}

template <typename MemorySpaceType>
void initial_temperature()
{
  MPI_Comm communicator = MPI_COMM_WORLD;

  // Geometry database
  boost::property_tree::ptree geometry_database;
  geometry_database.put("import_mesh", false);
  geometry_database.put("length", 12e-3);
  geometry_database.put("length_divisions", 1);
  geometry_database.put("height", 6e-3);
  geometry_database.put("height_divisions", 1);
  // Build Geometry
  adamantine::Geometry<2> geometry(communicator, geometry_database);
  boost::property_tree::ptree database;
  // Material property
  database.put("materials.property_format", "polynomial");
  database.put("materials.n_materials", 1);
  database.put("materials.material_0.solid.density", 1.);
  database.put("materials.material_0.powder.density", 10.);
  database.put("materials.material_0.liquid.density", 1.);
  database.put("materials.material_0.solid.specific_heat", 1.);
  database.put("materials.material_0.powder.specific_heat", 2.);
  database.put("materials.material_0.liquid.specific_heat", 1.);
  database.put("materials.material_0.solid.thermal_conductivity", 1.);
  database.put("materials.material_0.powder.thermal_conductivity", 1.);
  database.put("materials.material_0.liquid.thermal_conductivity", 1.);
  // Source database
  database.put("sources.n_beams", 1);
  database.put("sources.beam_0.depth", 1e100);
  database.put("sources.beam_0.diameter", 1e100);
  database.put("sources.beam_0.max_power", 1e300);
  database.put("sources.beam_0.scan_path_file", "scan_path.txt");
  database.put("sources.beam_0.absorption_efficiency", 0.3);
  database.put("sources.beam_0.type", "electron_beam");
  database.put("sources.beam_0.scan_path_file",
               "scan_path_test_thermal_physics.txt");
  database.put("sources.beam_0.scan_path_file_format", "segment");
  // Boundary database
  database.put("boundary.type", "adiabatic");

  // Time-stepping database
  database.put("time_stepping.method", "rk_fourth_order");
  // Build ThermalPhysics
  adamantine::ThermalPhysics<2, 2, MemorySpaceType, dealii::QGauss<1>> physics(
      communicator, database, geometry);
  physics.setup_dofs();
  physics.compute_inverse_mass_matrix();

  dealii::LA::distributed::Vector<double, MemorySpaceType> solution;
  physics.initialize_dof_vector(1000., solution);
  physics.get_state_from_material_properties();
  BOOST_CHECK(solution.l1_norm() == 1000. * solution.size());
}

template <typename MemorySpaceType>
void energy_conservation()
{
  MPI_Comm communicator = MPI_COMM_WORLD;

  // Geometry database
  boost::property_tree::ptree geometry_database;
  geometry_database.put("import_mesh", false);
  geometry_database.put("length", 10);
  geometry_database.put("length_divisions", 10);
  geometry_database.put("height", 10);
  geometry_database.put("height_divisions", 10);
  // Build Geometry
  adamantine::Geometry<2> geometry(communicator, geometry_database);
  boost::property_tree::ptree database;
  // Material property
  database.put("materials.property_format", "polynomial");
  database.put("materials.n_materials", 1);
  database.put("materials.material_0.solid.density", 0.5);
  database.put("materials.material_0.powder.density", 0.5);
  database.put("materials.material_0.liquid.density", 0.5);
  database.put("materials.material_0.solid.specific_heat", 4.);
  database.put("materials.material_0.powder.specific_heat", 4.);
  database.put("materials.material_0.liquid.specific_heat", 4.);
  database.put("materials.material_0.solid.thermal_conductivity", 2.);
  database.put("materials.material_0.powder.thermal_conductivity", 2.);
  database.put("materials.material_0.liquid.thermal_conductivity", 2.);
  // Source database
  database.put("sources.n_beams", 1);
  database.put("sources.beam_0.type", "cube");
  database.put("sources.beam_0.start_time", 0);
  database.put("sources.beam_0.end_time", 5);
  database.put("sources.beam_0.value", 5);
  database.put("sources.beam_0.min_x", 4);
  database.put("sources.beam_0.min_y", 4);
  database.put("sources.beam_0.max_x", 6);
  database.put("sources.beam_0.max_y", 6);
  // Time-stepping database
  database.put("time_stepping.method", "forward_euler");
  // Boundary database
  database.put("boundary.type", "adiabatic");
  // Build ThermalPhysics
  adamantine::ThermalPhysics<2, 2, dealii::MemorySpace::Host, dealii::QGauss<1>>
      physics(communicator, database, geometry);
  physics.setup_dofs();
  physics.compute_inverse_mass_matrix();

  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> solution;
  double constexpr initial_temperature = 10;
  double constexpr final_temperature = 10.5;
  physics.initialize_dof_vector(initial_temperature, solution);
  physics.get_state_from_material_properties();
  std::vector<adamantine::Timer> timers(adamantine::Timing::n_timers);
  double time = 0;
  while (time < 100)
  {
    time = physics.evolve_one_time_step(time, 0.05, solution, timers);
  }

  double max = -1;
  double min = 1e4;
  if (std::is_same<MemorySpaceType, dealii::MemorySpace::Host>::value)
  {
    for (auto v : solution)
    {
      if (max < v)
        max = v;
      if (min > v)
        min = v;
    }
  }
  else
  {
    dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host>
        solution_host(solution.get_partitioner());
    solution_host.import(solution, dealii::VectorOperation::insert);
    for (auto v : solution_host)
    {
      if (max < v)
        max = v;
      if (min > v)
        min = v;
    }
  }

  double constexpr tolerance = 1e-9;
  BOOST_CHECK_CLOSE(solution.mean_value(), final_temperature, tolerance);
  BOOST_CHECK_CLOSE(min, max, tolerance);
}

template <typename MemorySpaceType>
void radiation_bcs()
{
  MPI_Comm communicator = MPI_COMM_WORLD;

  // Geometry database
  boost::property_tree::ptree geometry_database;
  geometry_database.put("import_mesh", false);
  geometry_database.put("length", 5);
  geometry_database.put("length_divisions", 5);
  geometry_database.put("height", 5);
  geometry_database.put("height_divisions", 5);
  // Build Geometry
  adamantine::Geometry<2> geometry(communicator, geometry_database);
  boost::property_tree::ptree database;
  // Material property
  database.put("materials.property_format", "polynomial");
  database.put("materials.n_materials", 1);
  database.put("materials.material_0.solid.density", 1.);
  database.put("materials.material_0.powder.density", 1.);
  database.put("materials.material_0.liquid.density", 1.);
  database.put("materials.material_0.solid.specific_heat", 1.);
  database.put("materials.material_0.powder.specific_heat", 1.);
  database.put("materials.material_0.liquid.specific_heat", 1.);
  database.put("materials.material_0.solid.thermal_conductivity", 1.);
  database.put("materials.material_0.powder.thermal_conductivity", 1.);
  database.put("materials.material_0.liquid.thermal_conductivity", 1.);
  database.put("materials.material_0.solid.emissivity", 1.);
  database.put("materials.material_0.powder.emissivity", 1.);
  database.put("materials.material_0.liquid.emissivity", 1.);
  database.put("materials.material_0.solid.radiation_heat_transfer_coef", 1.);
  database.put("materials.material_0.powder.radiation_heat_transfer_coef", 1.);
  database.put("materials.material_0.liquid.radiation_heat_transfer_coef", 1.);
  database.put("materials.material_0.solid.convection_heat_transfer_coef", 1.);
  database.put("materials.material_0.powder.convection_heat_transfer_coef", 1.);
  database.put("materials.material_0.liquid.convection_heat_transfer_coef", 1.);
  database.put("materials.material_0.radiation_temperature_infty", 20.0);
  database.put("materials.material_0.convection_temperature_infty", 0.0);
  // Source database
  database.put("sources.n_beams", 0);
  // Time-stepping database
  database.put("time_stepping.method", "forward_euler");
  // Boundary database
  database.put("boundary.type", "radiative");
  // Build ThermalPhysics
  adamantine::ThermalPhysics<2, 2, dealii::MemorySpace::Host, dealii::QGauss<1>>
      physics(communicator, database, geometry);
  physics.setup_dofs();
  physics.compute_inverse_mass_matrix();

  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> solution;
  double constexpr initial_temperature = 10;
  physics.initialize_dof_vector(initial_temperature, solution);
  physics.get_state_from_material_properties();
  std::vector<adamantine::Timer> timers(adamantine::Timing::n_timers);
  double time = 0;
  while (time < 100)
  {
    time = physics.evolve_one_time_step(time, 0.05, solution, timers);
  }

  double max = -1;
  double min = 1e4;
  if (std::is_same<MemorySpaceType, dealii::MemorySpace::Host>::value)
  {
    for (auto v : solution)
    {
      if (max < v)
        max = v;
      if (min > v)
        min = v;
    }
  }
  else
  {
    dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host>
        solution_host(solution.get_partitioner());
    solution_host.import(solution, dealii::VectorOperation::insert);
    for (auto v : solution_host)
    {
      if (max < v)
        max = v;
      if (min > v)
        min = v;
    }
  }

  BOOST_CHECK(min >= 10. && min <= 20.);
  BOOST_CHECK(max > 10. && max <= 20.);
}

template <typename MemorySpaceType>
void convection_bcs()
{
  MPI_Comm communicator = MPI_COMM_WORLD;

  // Geometry database
  boost::property_tree::ptree geometry_database;
  geometry_database.put("import_mesh", false);
  geometry_database.put("length", 5);
  geometry_database.put("length_divisions", 5);
  geometry_database.put("height", 5);
  geometry_database.put("height_divisions", 5);
  // Build Geometry
  adamantine::Geometry<2> geometry(communicator, geometry_database);
  boost::property_tree::ptree database;
  // Material property
  database.put("materials.property_format", "polynomial");
  database.put("materials.n_materials", 1);
  database.put("materials.material_0.solid.density", 1.);
  database.put("materials.material_0.powder.density", 1.);
  database.put("materials.material_0.liquid.density", 1.);
  database.put("materials.material_0.solid.specific_heat", 1.);
  database.put("materials.material_0.powder.specific_heat", 1.);
  database.put("materials.material_0.liquid.specific_heat", 1.);
  database.put("materials.material_0.solid.thermal_conductivity", 1.);
  database.put("materials.material_0.powder.thermal_conductivity", 1.);
  database.put("materials.material_0.liquid.thermal_conductivity", 1.);
  database.put("materials.material_0.solid.emissivity", 1.);
  database.put("materials.material_0.powder.emissivity", 1.);
  database.put("materials.material_0.liquid.emissivity", 1.);
  database.put("materials.material_0.solid.radiation_heat_transfer_coef", 1.);
  database.put("materials.material_0.powder.radiation_heat_transfer_coef", 1.);
  database.put("materials.material_0.liquid.radiation_heat_transfer_coef", 1.);
  database.put("materials.material_0.solid.convection_heat_transfer_coef", 1.);
  database.put("materials.material_0.powder.convection_heat_transfer_coef", 1.);
  database.put("materials.material_0.liquid.convection_heat_transfer_coef", 1.);
  database.put("materials.material_0.radiation_temperature_infty", 0.0);
  database.put("materials.material_0.convection_temperature_infty", 20.0);
  // Source database
  database.put("sources.n_beams", 0);
  // Time-stepping database
  database.put("time_stepping.method", "forward_euler");
  // Boundary database
  database.put("boundary.type", "convective");
  // Build ThermalPhysics
  adamantine::ThermalPhysics<2, 2, dealii::MemorySpace::Host, dealii::QGauss<1>>
      physics(communicator, database, geometry);
  physics.setup_dofs();
  physics.compute_inverse_mass_matrix();

  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> solution;
  double constexpr initial_temperature = 10;
  physics.initialize_dof_vector(initial_temperature, solution);
  physics.get_state_from_material_properties();
  std::vector<adamantine::Timer> timers(adamantine::Timing::n_timers);
  double time = 0;
  while (time < 10)
  {
    time = physics.evolve_one_time_step(time, 0.05, solution, timers);
  }

  double max = -1;
  double min = 1e4;
  if (std::is_same<MemorySpaceType, dealii::MemorySpace::Host>::value)
  {
    for (auto v : solution)
    {
      if (max < v)
        max = v;
      if (min > v)
        min = v;
    }
  }
  else
  {
    dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host>
        solution_host(solution.get_partitioner());
    solution_host.import(solution, dealii::VectorOperation::insert);
    for (auto v : solution_host)
    {
      if (max < v)
        max = v;
      if (min > v)
        min = v;
    }
  }

  BOOST_CHECK(min >= 10. && min <= 20.);
  BOOST_CHECK(max > 10. && max <= 20.);
}


/*
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2015-, Open Perception, Inc.
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * * Redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer.
 * * Redistributions in binary form must reproduce the above
 * copyright notice, this list of conditions and the following
 * disclaimer in the documentation and/or other materials provided
 * with the distribution.
 * * Neither the name of the copyright holder(s) nor the names of its
 * contributors may be used to endorse or promote products derived
 * from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Filippo Basso [bassofil@dei.unipd.it]
 *         Matteo Munaro [matteo.munaro@dei.unipd.it]
 *
 */

#include <ros/ros.h>
#include <opt_msgs/TrackArray.h>
#include <visualization_msgs/MarkerArray.h>
#include <Eigen/Dense>
#include <pcl/point_cloud.h>

#include <open_ptrack/tracking/track.h>

#include <dynamic_reconfigure/server.h>
#include <tracking/MovingAverageSmootherConfig.h>

namespace open_ptrack
{
namespace tracking
{

typedef ::tracking::MovingAverageSmootherConfig Config;
typedef ::dynamic_reconfigure::Server<Config> ReconfigureServer;

struct TrackPositionNode
{
  typedef boost::shared_ptr<TrackPositionNode> Ptr;

  TrackPositionNode(const Eigen::Array2d & position, const ros::Time & time) : position_(position), time_(time) {}

  const Eigen::Array2d position_;
  const ros::Time time_;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

class TrackFilter
{
public:

  typedef boost::shared_ptr<TrackFilter> Ptr;

  TrackFilter(int window_size) : window_size_(window_size), last_positions_sum_(Eigen::Array2d::Zero()) {}

  Eigen::Array2d movingAveragePosition()
  {
    return last_positions_sum_ / last_positions_.size();
  }

  void addPosition(const Eigen::Array2d & position, const ros::Time & time)
  {
    TrackPositionNode::Ptr node = boost::make_shared<TrackPositionNode>(position, time);
    if (last_positions_.size() < window_size_)
    {
      last_positions_.push_front(node);
      last_positions_sum_ += node->position_;
    }
    else
    {
      const TrackPositionNode::Ptr & remove_node = last_positions_.back();
      last_positions_sum_ -= remove_node->position_;
      last_positions_.pop_back();
      last_positions_.push_front(node);
      last_positions_sum_ += node->position_;
    }
  }

  void setWindowSize(int new_size)
  {
    if (last_positions_.size() > new_size)
    {
      while (last_positions_.size() > new_size)
      {
        const TrackPositionNode::Ptr & remove_node = last_positions_.back();
        last_positions_sum_ -= remove_node->position_;
        last_positions_.pop_back();
      }
    }
    window_size_ = new_size;
  }

  void removeOldPositions(const ros::Time & min_allowed_time)
  {
    bool removed = false;
    while (not removed)
    {
      const TrackPositionNode::Ptr & test_node = last_positions_.back();
      if (test_node->time_ < min_allowed_time and last_positions_.size() > 1)
      {
        last_positions_sum_ -= test_node->position_;
        last_positions_.pop_back();
      }
      else
      {
        removed = true;
      }
    }
  }

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

protected:

  std::list<TrackPositionNode::Ptr> last_positions_;
  int window_size_;

  Eigen::Array2d last_positions_sum_;

};

struct TrackInfo
{
  TrackFilter::Ptr track_;
  opt_msgs::Track::Ptr last_msg_;
  ros::Time last_msg_time_;
};

class MovingAverageSmoother
{

public:

  MovingAverageSmoother(ros::NodeHandle & node_handle)
    : node_handle_(node_handle),
      reconfigure_server_(new ReconfigureServer(config_mutex_, node_handle_)),
      rate_(ros::Rate(30.0))
  {
    double rate_d, heartbeat_time;

    node_handle_.param("rate", rate_d, 30.0);
    node_handle_.param("publish_empty", publish_empty_, true);
    node_handle_.param("heartbeat_time", heartbeat_time, 5.0);
    node_handle_.param("max_history_size", max_history_size_, 1000);

    node_handle_.param("window_size", window_size_, 5);

    double position_lifetime_d, track_lifetime_d;
    node_handle_.param("track_lifetime_with_no_detections", track_lifetime_d, 5.0);
    node_handle_.param("position_lifetime", position_lifetime_d, 0.5);
    track_lifetime_ = ros::Duration(track_lifetime_d);
    position_lifetime_ = ros::Duration(position_lifetime_d);

    generateColors();
    history_cloud_ = boost::make_shared<pcl::PointCloud<pcl::PointXYZRGB> >();
    history_cloud_index_ = 0;
    history_cloud_->header.frame_id = "world";

    heartbeat_time_duration_ = ros::Duration(heartbeat_time);

    tracking_sub_ = node_handle_.subscribe<opt_msgs::TrackArray>("input", 100, &MovingAverageSmoother::trackingCallback, this);
    tracking_pub_ = node_handle_.advertise<opt_msgs::TrackArray>("output", 1);
    marker_array_pub_ = node_handle_.advertise<visualization_msgs::MarkerArray>("markers_array", 1);
    history_pub_ = node_handle_.advertise<pcl::PointCloud<pcl::PointXYZRGB> >("history", 1);

    rate_ = ros::Rate(rate_d);

    ReconfigureServer::CallbackType callback = boost::bind(&MovingAverageSmoother::configCallback, this, _1, _2);
    reconfigure_server_->setCallback(callback);

  }

  void configCallback(Config & config, uint32_t level)
  {
    //rate_ = ros::Rate(config.rate);

    config.track_lifetime_with_no_detections = std::max(config.track_lifetime_with_no_detections, rate_.expectedCycleTime().toSec());
    track_lifetime_ = ros::Duration(config.track_lifetime_with_no_detections);

    heartbeat_time_duration_ = ros::Duration(config.heartbeat_time);
    publish_empty_ = config.publish_empty;

    window_size_ = config.window_size;
    for (std::map<int, TrackInfo>::iterator it = track_map_.begin(); it != track_map_.end(); ++it)
    {
      TrackInfo & track_info = it->second;
      track_info.track_->setWindowSize(window_size_);
    }

    config.position_lifetime = std::max(config.position_lifetime, rate_.expectedCycleTime().toSec());
    position_lifetime_ = ros::Duration(config.position_lifetime);

    max_history_size_ = config.max_history_size;

  }

  void trackingCallback(const opt_msgs::TrackArray::ConstPtr & msg)
  {
    for (size_t i = 0; i < msg->tracks.size(); ++i)
    {
      const opt_msgs::Track & track_msg = msg->tracks[i];

      std::map<int, TrackInfo>::iterator it = track_map_.find(track_msg.id);

      if (it == track_map_.end()) // Track does not exist
      {
        TrackInfo track_info;
        track_info.track_ = boost::make_shared<TrackFilter>(window_size_);
        track_map_[track_msg.id] = track_info;
      }

      TrackInfo & track_info = track_map_[track_msg.id];

      track_info.track_->addPosition(Eigen::Array2d(track_msg.x, track_msg.y), msg->header.stamp);
      track_info.last_msg_ = boost::make_shared<opt_msgs::Track>(track_msg);
      if (track_info.last_msg_->visibility < 2)
        track_info.last_msg_time_ = msg->header.stamp;
    }
  }

  void spin()
  {
    ros::Time last_heartbeat_time = ros::Time::now();

    while (ros::ok())
    {
      ros::spinOnce();

      opt_msgs::TrackArray track_msg;
      visualization_msgs::MarkerArray marker_msg;

      int n = createMsg(track_msg, marker_msg);
      ros::Time current_time = ros::Time::now();

      config_mutex_.lock();
      if (publish_empty_ or n > 0)
      {
        tracking_pub_.publish(track_msg);
        marker_array_pub_.publish(marker_msg);
        history_pub_.publish(history_cloud_);
        last_heartbeat_time = current_time;
      }
      else if (not publish_empty_)
      {
        // Publish a heartbeat message every 'heartbeat_time' seconds
        if ((current_time - last_heartbeat_time) > heartbeat_time_duration_)
        {
          opt_msgs::TrackArray heartbeat_msg;
          heartbeat_msg.header.stamp = current_time;
          heartbeat_msg.header.frame_id = "heartbeat";
          tracking_pub_.publish(heartbeat_msg);
          marker_array_pub_.publish(marker_msg);
          history_pub_.publish(history_cloud_);
          last_heartbeat_time = current_time;
        }
      }
      config_mutex_.unlock();

      rate_.sleep();
    }
  }

private:

  void generateColors()
  {
    for (size_t i = 0; i <= 4; ++i)
      for (size_t j = 0; j <= 4; ++j)
        for (size_t k = 0; k <= 4; ++k)
          color_set_.push_back(Eigen::Vector3f(i * 0.25f, j * 0.25f, k * 0.25f));

    std::random_shuffle(color_set_.begin(), color_set_.end());
  }

  void appendToHistory(const opt_msgs::Track & track_msg)
  {
//    if (data.last_msg.tracks[0].visibility == opt_msgs::Track::NOT_VISIBLE)
//      return;

    config_mutex_.lock();
    if (history_cloud_->size() < max_history_size_)
    {
      pcl::PointXYZRGB point;
      history_cloud_->push_back(point);
    }
    config_mutex_.unlock();

    toPointXYZRGB(history_cloud_->points[history_cloud_index_], track_msg);
    history_cloud_index_ = (history_cloud_index_ + 1) % max_history_size_;
  }

  int createMsg(opt_msgs::TrackArray & track_msg,
                visualization_msgs::MarkerArray & marker_msg)
  {
    int added = 0;
    ros::Time now = ros::Time::now();

    track_msg.header.stamp = now;
    track_msg.header.frame_id = "world";

    history_cloud_->header.stamp = now.toNSec() / 1000;
    std::vector<int> to_remove;

    for (std::map<int, TrackInfo>::iterator it = track_map_.begin(); it != track_map_.end(); ++it)
    {
      TrackInfo & track_info = it->second;

      config_mutex_.lock();
      bool ok = (now - track_info.last_msg_time_) < track_lifetime_;
      config_mutex_.unlock();

      if (ok)
      {
        track_info.track_->removeOldPositions(now - position_lifetime_);

        Eigen::Array2d position = track_info.track_->movingAveragePosition();
        track_info.last_msg_->x = position[0];
        track_info.last_msg_->y = position[1];

        track_msg.tracks.push_back(*track_info.last_msg_);
        createMarker(marker_msg, *track_info.last_msg_, track_msg.header);
        appendToHistory(*track_info.last_msg_);
        ++added;

      }
      else
      {
        to_remove.push_back(it->first);
      }
    }

    for (size_t i = 0; i < to_remove.size(); ++i)
      track_map_.erase(to_remove[i]);

    return added;
  }

  void createMarker(visualization_msgs::MarkerArray & msg,
                    const opt_msgs::Track & track_msg,
                    const std_msgs::Header & header)
  {
//    if(track.visibility == Track::NOT_VISIBLE)
//      return;

    visualization_msgs::Marker marker;

    marker.header.frame_id = header.frame_id;
    marker.header.stamp = header.stamp;

    marker.ns = "people";
    marker.id = track_msg.id;

    marker.type = visualization_msgs::Marker::SPHERE;
    marker.action = visualization_msgs::Marker::ADD;

    marker.pose.position.x = track_msg.x;
    marker.pose.position.y = track_msg.y;
    marker.pose.position.z = 3 * track_msg.height / 4;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;

    marker.scale.x = 0.1;
    marker.scale.y = 0.1;
    marker.scale.z = 0.1;

    marker.color.r = color_set_[track_msg.id % color_set_.size()](2);
    marker.color.g = color_set_[track_msg.id % color_set_.size()](1);
    marker.color.b = color_set_[track_msg.id % color_set_.size()](0);
    marker.color.a = 1.0;

    marker.lifetime = ros::Duration(0.2);

    msg.markers.push_back(marker);

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

    visualization_msgs::Marker text_marker;

    text_marker.header.frame_id = header.frame_id;
    text_marker.header.stamp = header.stamp;

    text_marker.ns = "numbers";
    text_marker.id = track_msg.id;

    text_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    text_marker.action = visualization_msgs::Marker::ADD;

    std::stringstream ss;
    ss << track_msg.id;
    text_marker.text = ss.str();

    text_marker.pose.position.x = track_msg.x;
    text_marker.pose.position.y = track_msg.y;
    text_marker.pose.position.z = track_msg.height + 0.1;
    text_marker.pose.orientation.x = 0.0;
    text_marker.pose.orientation.y = 0.0;
    text_marker.pose.orientation.z = 0.0;
    text_marker.pose.orientation.w = 1.0;

    text_marker.scale.x = 0.2;
    text_marker.scale.y = 0.2;
    text_marker.scale.z = 0.2;

    text_marker.color.r = color_set_[track_msg.id % color_set_.size()](2);
    text_marker.color.g = color_set_[track_msg.id % color_set_.size()](1);
    text_marker.color.b = color_set_[track_msg.id % color_set_.size()](0);
    text_marker.color.a = 1.0;

    text_marker.lifetime = ros::Duration(0.2);

    msg.markers.push_back(text_marker);
  }

  void toPointXYZRGB(pcl::PointXYZRGB & p,
                     const opt_msgs::Track & track_msg)
  {
//    if(track.visibility == Track::NOT_VISIBLE)
//      return;

    p.x = float(track_msg.x);
    p.y = float(track_msg.y);
    p.z = float(3 * track_msg.height / 4);
    uchar * rgb_ptr = reinterpret_cast<uchar *>(&p.rgb);
    *rgb_ptr++ = uchar(color_set_[track_msg.id % color_set_.size()](0) * 255.0f);
    *rgb_ptr++ = uchar(color_set_[track_msg.id % color_set_.size()](1) * 255.0f);
    *rgb_ptr++ = uchar(color_set_[track_msg.id % color_set_.size()](2) * 255.0f);
  }

  ros::NodeHandle node_handle_;

  boost::recursive_mutex config_mutex_;
  boost::shared_ptr<ReconfigureServer> reconfigure_server_;

  ros::Subscriber tracking_sub_;
  ros::Publisher tracking_pub_;
  ros::Publisher marker_array_pub_;
  ros::Publisher history_pub_;

  bool publish_empty_;
  ros::Duration heartbeat_time_duration_;

  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > color_set_;

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr history_cloud_;
  int max_history_size_;
  size_t history_cloud_index_;

  ros::Rate rate_;
  int window_size_;

  ros::Duration track_lifetime_;
  ros::Duration position_lifetime_;

  std::map<int, TrackInfo> track_map_;

};

} // namespace tracking
} // namespace open_ptrack

using open_ptrack::tracking::MovingAverageSmoother;

int main(int argc, char **argv)
{
  // Initialization:
  ros::init(argc, argv, "tracking_filter");
  ros::NodeHandle nh("~");

  MovingAverageSmoother smoother(nh);
  smoother.spin();

  return 0;
}


/************************************************************
 *
 * Copyright (c) 2021, University of California, Los Angeles
 *
 * Authors: Kenny J. Chen, Brett T. Lopez
 * Contact: kennyjchen@ucla.edu, btlopez@ucla.edu
 *
 ***********************************************************/

/***********************************************************************
 * BSD 3-Clause License
 *
 * Copyright (c) 2020, SMRT-AIST
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder 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.
 *************************************************************************/

#ifndef NANO_GICP_NANO_GICP_HPP
#define NANO_GICP_NANO_GICP_HPP

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/registration/registration.h>

#include <nano_gicp/lsq_registration.hpp>
#include <nano_gicp/gicp/gicp_settings.hpp>
#include <nano_gicp/nanoflann.hpp>

namespace nano_gicp {

template<typename PointSource, typename PointTarget>
class NanoGICP : public LsqRegistration<PointSource, PointTarget> {
public:
  using Scalar = float;
  using Matrix4 = typename pcl::Registration<PointSource, PointTarget, Scalar>::Matrix4;

  using PointCloudSource = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudSource;
  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;

  using PointCloudTarget = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudTarget;
  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;

protected:
  using pcl::Registration<PointSource, PointTarget, Scalar>::reg_name_;
  using pcl::Registration<PointSource, PointTarget, Scalar>::input_;
  using pcl::Registration<PointSource, PointTarget, Scalar>::target_;
  using pcl::Registration<PointSource, PointTarget, Scalar>::corr_dist_threshold_;

public:
  NanoGICP();
  virtual ~NanoGICP() override;

  void setNumThreads(int n);
  void setCorrespondenceRandomness(int k);
  void setRegularizationMethod(RegularizationMethod method);

  virtual void swapSourceAndTarget() override;
  virtual void clearSource() override;
  virtual void clearTarget() override;

  virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override;
  virtual void setSourceCovariances(const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covs);
  virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override;
  virtual void setTargetCovariances(const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covs);

  virtual void registerInputSource(const PointCloudSourceConstPtr& cloud);

  const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& getSourceCovariances() const {
    return source_covs_;
  }

  const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& getTargetCovariances() const {
    return target_covs_;
  }

protected:
  virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override;

  virtual void update_correspondences(const Eigen::Isometry3d& trans);

  virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix<double, 6, 6>* H, Eigen::Matrix<double, 6, 1>* b) override;

  virtual double compute_error(const Eigen::Isometry3d& trans) override;

  template<typename PointT>
  bool calculate_covariances(const typename pcl::PointCloud<PointT>::ConstPtr& cloud, nanoflann::KdTreeFLANN<PointT>& kdtree, std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covariances);

public:
  std::shared_ptr<nanoflann::KdTreeFLANN<PointSource>> source_kdtree_;
  std::shared_ptr<nanoflann::KdTreeFLANN<PointTarget>> target_kdtree_;

  std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> source_covs_;
  std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> target_covs_;

protected:
  int num_threads_;
  int k_correspondences_;

  RegularizationMethod regularization_method_;

  std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> mahalanobis_;

  std::vector<int> correspondences_;
  std::vector<float> sq_distances_;
};
}  // namespace nano_gicp

#endif


//
// Copyright (c) 2016 - 2017 Mesh Consultants Inc.
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//

#include "Vector_ClosestPoint.h"

#include <assert.h>

#include <Urho3D/Math/Vector3.h>

#include <Eigen/Core>

using namespace Urho3D;

String Vector_ClosestPoint::iconTexture = "Textures/Icons/Vector_ClosestPoint.png";

Vector_ClosestPoint::Vector_ClosestPoint(Context* context) : IoComponentBase(context, 2, 3)
{
	SetName("ClosePoint");
	SetFullName("Closest Point");
	SetDescription("Which point in list is closest");
	SetGroup(IoComponentGroup::VECTOR);
	SetSubgroup("Point");

	inputSlots_[0]->SetName("Point");
	inputSlots_[0]->SetVariableName("P");
	inputSlots_[0]->SetDescription("Query point");
	inputSlots_[0]->SetVariantType(VariantType::VAR_VECTOR3);
	inputSlots_[0]->SetDefaultValue(Vector3(0.0f, 0.0f, 0.0f));
	inputSlots_[0]->DefaultSet();

	inputSlots_[1]->SetName("Point List");
	inputSlots_[1]->SetVariableName("L");
	inputSlots_[1]->SetDescription("List of points");
	inputSlots_[1]->SetVariantType(VariantType::VAR_VECTOR3);
	inputSlots_[1]->SetDataAccess(DataAccess::LIST);
	inputSlots_[1]->SetDefaultValue(Vector3(0.0f, 0.0f, 0.0f));
	inputSlots_[1]->DefaultSet();

	outputSlots_[0]->SetName("Closest Point");
	outputSlots_[0]->SetVariableName("C");
	outputSlots_[0]->SetDescription("Closest point in list to P");
	outputSlots_[0]->SetVariantType(VariantType::VAR_VECTOR3);

	outputSlots_[1]->SetName("Index");
	outputSlots_[1]->SetVariableName("I");
	outputSlots_[1]->SetDescription("Index into L of closest point");
	outputSlots_[1]->SetVariantType(VariantType::VAR_INT);

	outputSlots_[2]->SetName("Distance");
	outputSlots_[2]->SetVariableName("D");
	outputSlots_[2]->SetDescription("Distance to closest point");
	outputSlots_[2]->SetVariantType(VariantType::VAR_FLOAT);
}

void Vector_ClosestPoint::SolveInstance(
	const Vector<Variant>& inSolveInstance,
	Vector<Variant>& outSolveInstance
)
{
	///////////////////
	// EXTRACT & VERIFY

	// query point
	Variant queryPointVar = inSolveInstance[0];
	if (queryPointVar.GetType() != VariantType::VAR_VECTOR3) {
		URHO3D_LOGWARNING("P must be a valid Vector3");
		outSolveInstance[0] = Variant();
		outSolveInstance[1] = Variant();
		outSolveInstance[2] = Variant();
		return;
	}
	Vector3 q = queryPointVar.GetVector3();

	// point list
	Variant pointListVar = inSolveInstance[1];
	if (pointListVar.GetType() != VariantType::VAR_VARIANTVECTOR) {
		URHO3D_LOGWARNING("L must be a list of Vector3's");
		outSolveInstance[0] = Variant();
		outSolveInstance[1] = Variant();
		outSolveInstance[2] = Variant();
		return;
	}
	Vector<Variant> pointList = pointListVar.GetVariantVector();

	//////////////////////////////////////////////
	// COMPONENT'S WORK & VERIFICATION DURING LOOP

	// output holders needed now to store data as we loop
	int index = -1; // index of closest point
	Vector3 r; // closest point
	float distance = -1.0f;

	/* due to the nature of this computation, we unpack and compute at the same time */
	if (pointList.Size() > 0) {

		Variant pointVar = pointList[0];
		if (pointVar.GetType() != VariantType::VAR_VECTOR3) {
			URHO3D_LOGWARNING("L must be a list of Vector3's only");
			outSolveInstance[0] = Variant();
			outSolveInstance[1] = Variant();
			outSolveInstance[2] = Variant();
			return;
		}
		Vector3 p = pointVar.GetVector3();
		float minDistanceSquared = (p - q).LengthSquared();
		index = 0;
		r = p;

		for (unsigned i = 1; i < pointList.Size(); ++i) {
			pointVar = pointList[i];
			if (pointVar.GetType() != VariantType::VAR_VECTOR3) {
				URHO3D_LOGWARNING("L must be a list of Vector3's only");
				outSolveInstance[0] = Variant();
				outSolveInstance[1] = Variant();
				outSolveInstance[2] = Variant();
				return;
			}
			p = pointVar.GetVector3();
			float distanceSquared = (p - q).LengthSquared();
			if (distanceSquared < minDistanceSquared) {
				minDistanceSquared = distanceSquared;
				index = i;
				r = p;
			}
		}

		distance = sqrt(minDistanceSquared);
	}

	/////////////////
	// ASSIGN OUTPUTS

	if (index > -1) {
		// inputs were good
		outSolveInstance[0] = Variant(r);
		outSolveInstance[1] = Variant((int)index);
		outSolveInstance[2] = Variant(distance);
	}
	else {
		// inputs were bad -- null sets?
		outSolveInstance[0] = Variant();
		outSolveInstance[1] = Variant();
		outSolveInstance[2] = Variant();
	}
}