#ifndef POTOK_HPACK_DECODE_HPP_
#define POTOK_HPACK_DECODE_HPP_

#include <potok/hpack/common.hpp>
#include <potok/hpack/error.hpp>

#include <potok/stdint.hpp>

#include <boost/asio/buffer.hpp>
#include <boost/asio/buffers_iterator.hpp>

#include <boost/system/error_code.hpp>

#include <boost/assert.hpp>

namespace potok {
namespace hpack {

//  decode I from the next N bits
//  if I < 2^N - 1, return I
//  else
//      M = 0
//      repeat
//          B = next octet
//          I = I + (B & 127) * 2^M
//          M = M + 7
//      while B & 128 == 128
//      return I

struct integer_decoder {
  enum class state { first, continuation, final, done };

  u64      v_               = 0;
  u64      M_               = 0;
  state    state_           = state::first;
  u8 const num_prefix_bits_ = 0;

  integer_decoder()                       = delete;
  integer_decoder(integer_decoder const&) = default;
  integer_decoder(integer_decoder&&)      = default;

  integer_decoder(u8 const num_prefix_bits)
      : num_prefix_bits_{num_prefix_bits}
  {
    BOOST_ASSERT(num_prefix_bits_ >= 1 && num_prefix_bits_ <= 8);
  }

  template <class ConstBufferSequence>
  auto operator()(ConstBufferSequence        const_buf_seq,    //
                  u64&                       v,                //
                  boost::system::error_code& ec) -> usize
  {
    constexpr auto const u64_max = u64{0xffffffff};

    auto bytes_read = usize{0};
    if (state_ == state::done) { return bytes_read; }

    auto pos = boost::asio::buffers_begin(const_buf_seq);
    auto end = boost::asio::buffers_end(const_buf_seq);

    if (pos == end) {
      ec = error::needs_more;
      return bytes_read;
    }

    switch (state_) {
      case state::first: {
        auto const max_prefix_value = get_max_prefix_value(num_prefix_bits_);

        v_ = *pos++ & max_prefix_value;
        ++bytes_read;

        if (v_ < max_prefix_value) {
          state_ = state::done;
          break;
        }

        state_ = state::continuation;
      }

      case state::continuation: {
        auto B = u64{128};
        for (; (pos != end) && ((B & 128) == 128); ++pos, ++bytes_read, M_ += 7) {
          B = *pos;

          auto const tmp = (B & u64{127}) * (u64{1} << M_);
          if (v_ > u64_max - tmp) {
            ec = error::too_large;
            break;
          }

          v_ += tmp;
        }

        if (!ec && ((B & 128) == 128)) {
          BOOST_ASSERT(pos == end);
          ec = error::needs_more;
          break;
        }

        state_ = state::done;
      }

      case state::done:
      default:
        break;
    }

    if (!ec && state_ == state::done) { v = v_; }
    return bytes_read;
  }
};

template <class ConstBufferSequence>
auto decode_integer(u8 const            num_prefix_bits,    //
                    ConstBufferSequence const_buf_seq,      //
                    u64&                out,                //
                    boost::system::error_code&) -> usize
{
  BOOST_ASSERT(num_prefix_bits >= 1 && num_prefix_bits <= 8);

  auto pos = boost::asio::buffers_begin(const_buf_seq);
  auto end = boost::asio::buffers_end(const_buf_seq);

  u64 const max_prefix_value = get_max_prefix_value(num_prefix_bits);

  auto num_parsed = u64{0};

  auto I = *pos & max_prefix_value;
  ++num_parsed;

  if (I < max_prefix_value) {
    out = I;
    return num_parsed;
  }

  auto M = u64{0};
  ++pos;

  auto B = u64{128};
  while ((pos != end) && ((B & 128) == 128)) {
    B = *pos++;
    I = I + (B & u64{127}) * (u64{1} << M);
    M += 7;
    ++num_parsed;
  }

  out = I;

  return num_parsed;
}

}    // namespace hpack
}    // namespace potok

#endif    // POTOK_HPACK_ENCODE_HPP_


//
// Copyright 2012 Christian Henning, Andreas Pokorny, Lubomir Bourdev
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
#ifndef BOOST_GIL_EXTENSION_TOOLBOX_COLOR_CONVERTERS_RGB_TO_LUMINANCE_HPP
#define BOOST_GIL_EXTENSION_TOOLBOX_COLOR_CONVERTERS_RGB_TO_LUMINANCE_HPP

#include <boost/gil/color_convert.hpp>

namespace boost{ namespace gil { namespace detail {

/// - performance specialization double
/// - to eliminate compiler warning 4244
template <typename GrayChannelValue>
struct rgb_to_luminance_fn< double, double, double, GrayChannelValue >
{
    GrayChannelValue operator()( const double& red
                               , const double& green
                               , const double& blue    ) const
   {
      return channel_convert<GrayChannelValue>( red * 0.30 + green * 0.59 + blue * 0.11 );
   }
};

} // namespace detail
} // namespace gil
} // namespace boost

#endif


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

// File generated at Sat 27 Aug 2016 12:40:14

#ifndef NSM_TWO_SCALE_susy_parameters_H
#define NSM_TWO_SCALE_susy_parameters_H

#include "betafunction.hpp"
#include "NSM_input_parameters.hpp"

#include <iosfwd>
#include <string>
#include <vector>
#include <Eigen/Core>

namespace flexiblesusy {

#ifdef TRACE_STRUCT_TYPE
   #undef TRACE_STRUCT_TYPE
#endif
#define TRACE_STRUCT_TYPE Susy_traces

class NSM_susy_parameters : public Beta_function {
public:
   explicit NSM_susy_parameters(const NSM_input_parameters& input_ = NSM_input_parameters());
   NSM_susy_parameters(double scale_, double loops_, double thresholds_, const NSM_input_parameters& input_, double g1_, double g2_, double g3_, double Lambda2_, double Lambda3_,
   double Lambda1_, const Eigen::Matrix<double,3,3>& Yu_, const Eigen::Matrix<
   double,3,3>& Yd_, const Eigen::Matrix<double,3,3>& Ye_
);
   virtual ~NSM_susy_parameters() {}
   virtual Eigen::ArrayXd beta() const;
   virtual Eigen::ArrayXd get() const;
   virtual void print(std::ostream&) const;
   virtual void set(const Eigen::ArrayXd&);
   const NSM_input_parameters& get_input() const;
   NSM_input_parameters& get_input();
   void set_input_parameters(const NSM_input_parameters&);

   NSM_susy_parameters calc_beta() const;
   virtual void clear();

   void set_g1(double g1_) { g1 = g1_; }
   void set_g2(double g2_) { g2 = g2_; }
   void set_g3(double g3_) { g3 = g3_; }
   void set_Lambda2(double Lambda2_) { Lambda2 = Lambda2_; }
   void set_Lambda3(double Lambda3_) { Lambda3 = Lambda3_; }
   void set_Lambda1(double Lambda1_) { Lambda1 = Lambda1_; }
   void set_Yu(const Eigen::Matrix<double,3,3>& Yu_) { Yu = Yu_; }
   void set_Yu(int i, int k, double value) { Yu(i,k) = value; }
   void set_Yd(const Eigen::Matrix<double,3,3>& Yd_) { Yd = Yd_; }
   void set_Yd(int i, int k, double value) { Yd(i,k) = value; }
   void set_Ye(const Eigen::Matrix<double,3,3>& Ye_) { Ye = Ye_; }
   void set_Ye(int i, int k, double value) { Ye(i,k) = value; }

   double get_g1() const { return g1; }
   double get_g2() const { return g2; }
   double get_g3() const { return g3; }
   double get_Lambda2() const { return Lambda2; }
   double get_Lambda3() const { return Lambda3; }
   double get_Lambda1() const { return Lambda1; }
   const Eigen::Matrix<double,3,3>& get_Yu() const { return Yu; }
   double get_Yu(int i, int k) const { return Yu(i,k); }
   const Eigen::Matrix<double,3,3>& get_Yd() const { return Yd; }
   double get_Yd(int i, int k) const { return Yd(i,k); }
   const Eigen::Matrix<double,3,3>& get_Ye() const { return Ye; }
   double get_Ye(int i, int k) const { return Ye(i,k); }

protected:
   double g1;
   double g2;
   double g3;
   double Lambda2;
   double Lambda3;
   double Lambda1;
   Eigen::Matrix<double,3,3> Yu;
   Eigen::Matrix<double,3,3> Yd;
   Eigen::Matrix<double,3,3> Ye;

   NSM_input_parameters input;

private:
   static const int numberOfParameters = 33;

   struct Susy_traces {
      double traceYdAdjYd;
      double traceYeAdjYe;
      double traceYuAdjYu;
      double traceYdAdjYdYdAdjYd;
      double traceYdAdjYuYuAdjYd;
      double traceYeAdjYeYeAdjYe;
      double traceYuAdjYuYuAdjYu;
      double traceYdAdjYdYdAdjYdYdAdjYd;
      double traceYdAdjYdYdAdjYuYuAdjYd;
      double traceYdAdjYuYuAdjYdYdAdjYd;
      double traceYdAdjYuYuAdjYuYuAdjYd;
      double traceYeAdjYeYeAdjYeYeAdjYe;
      double traceYuAdjYuYuAdjYuYuAdjYu;

   };
   void calc_susy_traces(Susy_traces&) const;

   double calc_beta_g1_one_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g1_two_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g1_three_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g2_one_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g2_two_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g2_three_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g3_one_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g3_two_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g3_three_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_Lambda2_one_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_Lambda2_two_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_Lambda2_three_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_Lambda3_one_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_Lambda3_two_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_Lambda3_three_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_Lambda1_one_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_Lambda1_two_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_Lambda1_three_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Yu_one_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Yu_two_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Yu_three_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Yd_one_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Yd_two_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Yd_three_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Ye_one_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Ye_two_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Ye_three_loop(const TRACE_STRUCT_TYPE&) const;

};

std::ostream& operator<<(std::ostream&, const NSM_susy_parameters&);

#undef TRACE_STRUCT_TYPE

} // namespace flexiblesusy

#endif


//
// Copyright (c) 2017 CNRS
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//

#ifndef GLPKWRAPPER_HH_
#define GLPKWRAPPER_HH_

#include <hpp/bezier-com-traj/solver/solver-abstract.hpp>

#include <Eigen/Dense>

namespace solvers {

// min g'x
// st  CIx <= ci0
//     CEx  = ce0
int solveglpk(const VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0,
              solvers::Cref_vectorX minBounds, solvers::Cref_vectorX maxBounds, VectorXd& x, double& cost);

} /* namespace solvers */

#endif /* GLPKWRAPPER_HH_ */


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2015 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph_filtering.hh"
#include "graph_selectors.hh"
#include "graph_properties.hh"
#include "graph.hh"

#include "graph_augment.hh"

#include <boost/mpl/if.hpp>
#include <boost/mpl/or.hpp>
namespace std
{
using namespace boost;

// we need a min() function with arguments of different types

template <class T1, class T2>
typename boost::mpl::if_<
    typename boost::mpl::or_<typename std::is_floating_point<T1>::type,
                             typename std::is_floating_point<T2>::type>::type,
    double, int>::type
min(const T1& v1, const T2& v2)
{
    if (v1 <= T1(v2))
        return v1;
    else
        return v2;
}
}

#include <boost/bind.hpp>

using namespace graph_tool;
using namespace boost;

#if (BOOST_VERSION >= 104400)
# include <boost/graph/boykov_kolmogorov_max_flow.hpp>
# define KOLMOGOROV_MAX_FLOW boost::boykov_kolmogorov_max_flow
#else
# include <boost/graph/kolmogorov_max_flow.hpp>
# define KOLMOGOROV_MAX_FLOW boost::kolmogorov_max_flow
#endif

struct get_kolmogorov_max_flow
{
    template <class Graph, class EdgeIndex, class VertexIndex,
              class CapacityMap, class ResidualMap>
    void operator()(Graph& g, EdgeIndex edge_index, size_t,
                    VertexIndex vertex_index, size_t src, size_t sink,
                    CapacityMap cm, ResidualMap res) const
    {
        typedef typename graph_traits<Graph>::edge_descriptor edge_t;
        checked_vector_property_map<uint8_t,EdgeIndex>
            augmented(edge_index);
        checked_vector_property_map<edge_t,EdgeIndex>
            reverse_map(edge_index);
        unchecked_vector_property_map<edge_t,VertexIndex>
            pred_map(vertex_index, num_vertices(g));
        unchecked_vector_property_map<size_t,VertexIndex>
            color_map(vertex_index, num_vertices(g));
        unchecked_vector_property_map<size_t,VertexIndex>
            dist_map(vertex_index, num_vertices(g));

        typedef typename std::remove_const<Graph>::type GT;
        GT& u = const_cast<GT&>(g);

        augment_graph(u, augmented, cm, reverse_map, res, true);

        KOLMOGOROV_MAX_FLOW(g, cm, res, reverse_map, pred_map, color_map,
                            dist_map, vertex_index, vertex(src, g),
                            vertex(sink, g));

        deaugment_graph(u, augmented);

    }
};

void kolmogorov_max_flow(GraphInterface& gi, size_t src, size_t sink,
                         boost::any capacity, boost::any res)
{
    run_action<graph_tool::detail::always_directed, boost::mpl::true_>()
        (gi, std::bind(get_kolmogorov_max_flow(),
                       placeholders::_1, gi.GetEdgeIndex(), gi.GetMaxEdgeIndex(),
                       gi.GetVertexIndex(), src, sink,  placeholders::_2,
                       placeholders::_3),
         writable_edge_scalar_properties(), writable_edge_scalar_properties())
        (capacity,res);
}


/*
*  Template of a compute module
*/

#include <QApplication>

#include <hxcore/HxMessage.h>
#include <hxcore/internal/HxWorkArea.h>
#include <hxfield/HxUniformScalarField3.h>

#include "CCPiXtekNoShiftReconstruction.h"
#include "base_types.hpp"
#include "mpi.hpp"
#include "utils.hpp"
#include "instruments.hpp"
#include "algorithms.hpp"
#include "cgls.hpp"
#include "sirt.hpp"
#include "mlem.hpp"
#include "results.hpp"
#include "voxels.hpp"
#include "ui_messages.hpp"
#include <boost/multi_array.hpp>

HX_INIT_CLASS(CCPiXtekNoShiftReconstruction, HxCompModule)

	CCPiXtekNoShiftReconstruction::CCPiXtekNoShiftReconstruction() :
HxCompModule(HxUniformScalarField3::getClassTypeId()),
	portAction(this,"action",QApplication::translate("CCPiXtekNoShiftReconstruction",
	"Action")),
	algorithm(this, "algorithm",
	QApplication::translate("CCPiXtekNoShiftReconstruction",
	"Reconstruction Algorithm")),
	iterations(this, "number of iterations",
	QApplication::translate("CCPiXtekNoShiftReconstruction", "Iterations")),
	resolution(this, "resolution", QApplication::translate("CCPiXtekNoShiftReconstruction", "Pixels per Voxel")),
	beam_harden(this, "beam harden",
	QApplication::translate("CCPiXtekNoShiftReconstruction", "Beam Hardening")),
	regularize(this, "regularize", QApplication::translate("CCPiXtekNoShiftReconstruction", "Regularization parameter"))
{
	portAction.setLabel(0,"DoIt");
	algorithm.setNum(5);
	algorithm.setLabel(0, "CGLS");
	algorithm.setLabel(1, "SIRT");
	algorithm.setLabel(2, "MLEM");
	algorithm.setLabel(3, "CGLS/Tikhonov");
	algorithm.setLabel(4, "CGLS/TV reg");
	algorithm.setValue(0);
	iterations.setMinMax(5, 30);
	iterations.setValue(20);
	resolution.setMinMax(1, 8);
	resolution.setValue(1);
	beam_harden.setValue(false);
	regularize.setNum(1);
	regularize.setMinMax(0.00001, 0.2);
	regularize.setValue(0.01);
	regularize.hide();
}

CCPiXtekNoShiftReconstruction::~CCPiXtekNoShiftReconstruction()
{
}

void CCPiXtekNoShiftReconstruction::compute()
{
	// Check whether the action port button was clicked
  if (!portAction.wasHit()) {
    if (algorithm.isNew()) {
      if (algorithm.getValue() < 3)
	regularize.hide();
      else
	regularize.show();
    }
    return;
  }
	ccpi_recon::do_progress = false;
	ccpi_recon::messages = theMsg;
	ccpi_recon::progress = theWorkArea;
	HxUniformScalarField3* field = (HxUniformScalarField3*) portData.getSource();
	// Check whether the input port is connected
	if (field == 0)
	{
		theMsg->stream()<<"Is not connected to uniform scalar field source"<<std::endl;
		return;
	}
	float *angles = new float[field->lattice().getDims()[2]];
	int result = field->parameters.findReal("Angles", field->lattice().getDims()[2], angles);
	//Check the Rotation Angle
	if (result!=1)
	{
		theMsg->stream()<<"Angles parameter not found in input data"<<result<<std::endl;
		return;
	}
	// Todo - check of optional shifts
	float *pixel_size = new float[2];
	result = field->parameters.findReal("DetectorPixelSize", 2,pixel_size);
	if(result!=1)
	{
		theMsg->stream()<<"DetectorPixelSize parameter not found in input data"<<result<<std::endl;
		return;
	}
	double srcToObject;
	result = field->parameters.findReal("SourceToObject", srcToObject);
	if(result!=1)
	{
		theMsg->stream()<<"SourceToObject parameter not found in input data"<<std::endl;
		return;
	}
	double srcToDetector;
	result = field->parameters.findReal("SourceToDetector", srcToDetector);
	if(result!=1)
	{
		theMsg->stream()<<"SourceToDetector parameter not found in input data"<<std::endl;
		return;
	}

	// Todo - check that its a float field?
	run_reconstruction();

	// end progress area
	if (ccpi_recon::do_progress) {
		theWorkArea->stopWorking();
		theWorkArea->undivide();
	}
}

void CCPiXtekNoShiftReconstruction::run_reconstruction()
{
	HxUniformScalarField3 *field = (HxUniformScalarField3 *) portData.getSource();
	McDim3l fdims = field->lattice().getDims();
	boost::multi_array_ref<float, 3>
		pixels_ref((float *)field->lattice().dataPtr(),
		boost::extents[fdims[0]][fdims[1]][fdims[2]],
		boost::fortran_storage_order());

	//Convert pixels to correct order
	pixel_3d pixels(boost::extents[fdims[2]][fdims[1]][fdims[0]]);
	for (int i = 0; i < fdims[2]; i++)
		for (int j = 0; j < fdims[1]; j++)
			for (int k = 0; k < fdims[0]; k++)
				pixels[i][j][k] = pixels_ref[k][j][i];

	float *angles_float = new float[field->lattice().getDims()[2]];
	field->parameters.findReal("Angles", field->lattice().getDims()[2], angles_float);
	float *pixel_size = new float[2];
	field->parameters.findReal("DetectorPixelSize", 2,pixel_size);
	double srcToObject;
	field->parameters.findReal("SourceToObject", srcToObject);
	double srcToDetector;
	field->parameters.findReal("SourceToDetector", srcToDetector);
	double MaskRadius;
	field->parameters.findReal("MaskRadius", MaskRadius);
	boost::multi_array_ref<float, 1> angles(angles_float,
						boost::extents[fdims[2]]);

	real h_size = pixel_size[0];
	real v_size = pixel_size[1];

	real source_x = - srcToObject;
	real detector_x = srcToDetector + source_x;

	int pixels_per_voxel = resolution.getValue();
	int niterations = iterations.getValue();
	bool beam_hardening = beam_harden.getValue();
	float reg_param = regularize.getValue();

	CCPi::instrument *instrument = new CCPi::Nikon_XTek();

	CCPi::reconstruction_alg *recon_algorithm = 0;
	switch (algorithm.getValue()) {
	case 0:
		//if (blocking_factor > 0 and instrument->supports_blocks())
		//  recon_algorithm = new CCPi::cgls_2d(niterations, pixels_per_voxel);
		recon_algorithm = new CCPi::cgls_3d(niterations);
		break;
	case 1:
		recon_algorithm = new CCPi::sirt(niterations);
		break;
	case 2:
		recon_algorithm = new CCPi::mlem(niterations);
		break;
	case 3:
	  recon_algorithm = new CCPi::cgls_tikhonov(niterations, reg_param);
		break;
	case 4:
	  recon_algorithm = new CCPi::cgls_tv_reg(niterations, reg_param);
		break;
	}

	machine::initialise(0);
	// instrument setup from pixels/angles will probably copy
	boost::multi_array<float, 1> h_offsets(boost::extents[1]);
	boost::multi_array<float, 1> v_offsets(boost::extents[1]);
	real vox_origin[3];
	real vox_size[3];
	voxel_data *voxels = reconstruct(instrument, recon_algorithm, pixels,
					 angles, h_offsets, v_offsets,
					 pixels_per_voxel, source_x, detector_x,
					 h_size, v_size, MaskRadius,
					 beam_hardening, vox_origin, vox_size,
					 false, false);
	theMsg->stream() << "completed the reconstruction: " << voxels<< std::endl;

	machine::exit();
	delete recon_algorithm;
	delete instrument;
	if (voxels != 0) {
		int dims[3];

		dims[0] = voxels->shape()[0];
		dims[1] = voxels->shape()[1];
		dims[2] = voxels->shape()[2];
		HxUniformScalarField3* output =
			new HxUniformScalarField3(dims, McPrimType::MC_FLOAT);
		for (int i = 0; i < dims[0]; i++)
			for (int j = 0; j < dims[1]; j++)
				for (int k = 0; k < dims[2]; k++)
					output->set(i, j, k, (*voxels)[i][j][k]);
		delete voxels;
		HxUniformCoord3 *coords =(HxUniformCoord3 *) output->lattice().coords();
		McBox3f bx = coords->getBoundingBox();
		bx[0] = vox_origin[0];
		bx[1] = vox_origin[0] + float(dims[0]) * vox_size[0];
		bx[2] = vox_origin[1];
		bx[3] = vox_origin[1] + float(dims[1]) * vox_size[1];
		bx[4] = vox_origin[2];
		bx[5] = vox_origin[2] + float(dims[2]) * vox_size[2];
		// publish reconstruction
		setResult(output);
	}
}


// from ros-control meta packages
#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>

#include <pluginlib/class_list_macros.h>
#include <std_msgs/Float64MultiArray.h>

#include <urdf/model.h>

// from kdl packages
#include <kdl/tree.hpp>
#include <kdl/kdl.hpp>
#include <kdl/chain.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/chaindynparam.hpp>              // inverse dynamics
#include <kdl/chainjnttojacsolver.hpp>        // jacobian
#include <kdl/chainfksolverpos_recursive.hpp> // forward kinematics
// #include <kdl/chainfksolvervel_recursive.hpp> // forward kinematics

#include <boost/scoped_ptr.hpp>
#include <boost/lexical_cast.hpp>

//
#include <math.h>
#include <Eigen/LU>
#include <utils/pseudo_inversion.h>
#include <utils/skew_symmetric.h>

#define PI 3.141592
#define D2R PI / 180.0
#define R2D 180.0 / PI
#define SaveDataMax 97
#define num_taskspace 6
#define A 0.1
#define b 2.5
#define f 1
#define t_set 1

namespace arm_controllers
{
class motion_control : public controller_interface::Controller<hardware_interface::EffortJointInterface>
{
public:
        bool init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &n)
        {
                // ********* 1. Get joint name / gain from the parameter server *********
                // 1.0 Control objective & Inverse Kinematics mode
                if (!n.getParam("ctr_obj", ctr_obj_))
                {
                        ROS_ERROR("Could not find control objective");
                        return false;
                }

                if (!n.getParam("ik_mode", ik_mode_))
                {
                        ROS_ERROR("Could not find control objective");
                        return false;
                }

                // 1.1 Joint Name
                if (!n.getParam("joints", joint_names_))
                {
                        ROS_ERROR("Could not find joint name");
                        return false;
                }
                n_joints_ = joint_names_.size();

                if (n_joints_ == 0)
                {
                        ROS_ERROR("List of joint names is empty.");
                        return false;
                }
                else
                {
                        ROS_INFO("Found %d joint names", n_joints_);
                        for (int i = 0; i < n_joints_; i++)
                        {
                                ROS_INFO("%s", joint_names_[i].c_str());
                        }
                }

                // 1.2 Gain
                // 1.2.1 Joint Controller
                Kp_.resize(n_joints_);
                Kd_.resize(n_joints_);
                Ki_.resize(n_joints_);

                std::vector<double> Kp(n_joints_), Ki(n_joints_), Kd(n_joints_);

                for (size_t i = 0; i < n_joints_; i++)
                {
                        std::string si = boost::lexical_cast<std::string>(i + 1);
                        if (n.getParam("/elfin/motion_control/gains/elfin_joint" + si + "/pid/p", Kp[i]))
                        {
                                Kp_(i) = Kp[i];
                        }
                        else
                        {
                                std::cout << "/elfin/motion_control/gains/elfin_joint" + si + "/pid/p" << std::endl;
                                ROS_ERROR("Cannot find pid/p gain");
                                return false;
                        }

                        if (n.getParam("/elfin/motion_control/gains/elfin_joint" + si + "/pid/i", Ki[i]))
                        {
                                Ki_(i) = Ki[i];
                        }
                        else
                        {
                                ROS_ERROR("Cannot find pid/i gain");
                                return false;
                        }

                        if (n.getParam("/elfin/motion_control/gains/elfin_joint" + si + "/pid/d", Kd[i]))
                        {
                                Kd_(i) = Kd[i];
                        }
                        else
                        {
                                ROS_ERROR("Cannot find pid/d gain");
                                return false;
                        }
                }

                // 1.2.2 Closed-loop Inverse Kinematics Controller
                if (ctr_obj_ == 1)
                {
                        if (!n.getParam("/elfin/motion_control/clik_gain/K_regulation", K_regulation_))
                        {
                                ROS_ERROR("Cannot find clik regulation gain");
                                return false;
                        }
                }

                else if (ctr_obj_ == 2)
                {
                        if (!n.getParam("/elfin/motion_control/clik_gain/K_tracking", K_tracking_))
                        {
                                ROS_ERROR("Cannot find clik tracking gain");
                                return false;
                        }
                }

                // 2. ********* urdf *********
                urdf::Model urdf;
                if (!urdf.initParam("robot_description"))
                {
                        ROS_ERROR("Failed to parse urdf file");
                        return false;
                }
                else
                {
                        ROS_INFO("Found robot_description");
                }

                // 3. ********* Get the joint object to use in the realtime loop [Joint Handle, URDF] *********
                for (int i = 0; i < n_joints_; i++)
                {
                        try
                        {
                                joints_.push_back(hw->getHandle(joint_names_[i]));
                        }
                        catch (const hardware_interface::HardwareInterfaceException &e)
                        {
                                ROS_ERROR_STREAM("Exception thrown: " << e.what());
                                return false;
                        }

                        urdf::JointConstSharedPtr joint_urdf = urdf.getJoint(joint_names_[i]);
                        if (!joint_urdf)
                        {
                                ROS_ERROR("Could not find joint '%s' in urdf", joint_names_[i].c_str());
                                return false;
                        }
                        joint_urdfs_.push_back(joint_urdf);
                }

                // 4. ********* KDL *********
                // 4.1 kdl parser
                if (!kdl_parser::treeFromUrdfModel(urdf, kdl_tree_))
                {
                        ROS_ERROR("Failed to construct kdl tree");
                        return false;
                }
                else
                {
                        ROS_INFO("Constructed kdl tree");
                }

                // 4.2 kdl chain
                std::string root_name, tip_name;
                if (!n.getParam("root_link", root_name))
                {
                        ROS_ERROR("Could not find root link name");
                        return false;
                }
                if (!n.getParam("tip_link", tip_name))
                {
                        ROS_ERROR("Could not find tip link name");
                        return false;
                }
                if (!kdl_tree_.getChain(root_name, tip_name, kdl_chain_))
                {
                        ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
                        ROS_ERROR_STREAM("  " << root_name << " --> " << tip_name);
                        ROS_ERROR_STREAM("  Tree has " << kdl_tree_.getNrOfJoints() << " joints");
                        ROS_ERROR_STREAM("  Tree has " << kdl_tree_.getNrOfSegments() << " segments");
                        ROS_ERROR_STREAM("  The segments are:");

                        KDL::SegmentMap segment_map = kdl_tree_.getSegments();
                        KDL::SegmentMap::iterator it;

                        for (it = segment_map.begin(); it != segment_map.end(); it++)
                                ROS_ERROR_STREAM("    " << (*it).first);

                        return false;
                }
                else
                {
                        ROS_INFO("Got kdl chain");
                }

                // 4.3 inverse dynamics solver 초기화
                gravity_ = KDL::Vector::Zero();
                gravity_(2) = -9.81; // 0: x-axis 1: y-axis 2: z-axis

                id_solver_.reset(new KDL::ChainDynParam(kdl_chain_, gravity_));

                // 4.4 jacobian solver 초기화
                jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));

                // 4.5 forward kinematics solver 초기화
                fk_pos_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));

                // ********* 5. 각종 변수 초기화 *********

                // 5.1 KDL Vector 초기화 (사이즈 정의 및 값 0)
                tau_d_.data = Eigen::VectorXd::Zero(n_joints_);
                x_cmd_.data = Eigen::VectorXd::Zero(num_taskspace);

                qd_.data = Eigen::VectorXd::Zero(n_joints_);
                qd_dot_.data = Eigen::VectorXd::Zero(n_joints_);
                qd_ddot_.data = Eigen::VectorXd::Zero(n_joints_);
                qd_old_.data = Eigen::VectorXd::Zero(n_joints_);

                q_.data = Eigen::VectorXd::Zero(n_joints_);
                qdot_.data = Eigen::VectorXd::Zero(n_joints_);

                e_.data = Eigen::VectorXd::Zero(n_joints_);
                e_dot_.data = Eigen::VectorXd::Zero(n_joints_);
                e_int_.data = Eigen::VectorXd::Zero(n_joints_);

                // 5.2 KDL Matrix 초기화 (사이즈 정의 및 값 0)
                J_.resize(kdl_chain_.getNrOfJoints());
                // J_inv_.resize(kdl_chain_.getNrOfJoints());
                M_.resize(kdl_chain_.getNrOfJoints());
                C_.resize(kdl_chain_.getNrOfJoints());
                G_.resize(kdl_chain_.getNrOfJoints());

                // ********* 6. ROS 명령어 *********
                // 6.1 publisher
                pub_qd_ = n.advertise<std_msgs::Float64MultiArray>("qd", 1000);
                pub_q_ = n.advertise<std_msgs::Float64MultiArray>("q", 1000);
                pub_e_ = n.advertise<std_msgs::Float64MultiArray>("e", 1000);

                pub_xd_ = n.advertise<std_msgs::Float64MultiArray>("xd", 1000);
                pub_x_ = n.advertise<std_msgs::Float64MultiArray>("x", 1000);
                pub_ex_ = n.advertise<std_msgs::Float64MultiArray>("ex", 1000);

                pub_SaveData_ = n.advertise<std_msgs::Float64MultiArray>("SaveData", 1000); // 뒤에 숫자는?

                // 6.2 subsriber
                sub_x_cmd_ = n.subscribe<std_msgs::Float64MultiArray>("command", 1, &motion_control::commandCB, this);
                event = 0; // subscribe 받기 전: 0
                // subscribe 받은 후: 1

                return true;
        }

        void commandCB(const std_msgs::Float64MultiArrayConstPtr &msg)
        {
                if (msg->data.size() != 3*num_taskspace)
                {
                        ROS_ERROR_STREAM("Dimension of command (" << msg->data.size() << ") does not match DOF of Task Space (" << 2 << ")! Not executing!");
                        return;
                }

                for (int i = 0; i < 3*num_taskspace; i++)
                {
                        x_cmd_(i) = msg->data[i];
                }

                event = 1; // subscribe 받기 전(Before receiving): 0
                // subscribe 받은 후(after receiving): 1
        }

        void starting(const ros::Time &time)
        {
                t = 0.0;
                ROS_INFO("Starting Computed Torque Controller with Closed-Loop Inverse Kinematics");
        }

        void update(const ros::Time &time, const ros::Duration &period)
        {
                // ********* 0. Get states from gazebo *********
                // 0.1 sampling time

                double dt = period.toSec();
                t = t + 0.0001;

                // 0.2 joint state
                for (int i = 0; i < n_joints_; i++)
                {
                        q_(i) = joints_[i].getPosition();
                        qdot_(i) = joints_[i].getVelocity();
                }

                // 0.3 end-effector state by Compute forward kinematics (x_,xdot_)
                fk_pos_solver_->JntToCart(q_, x_);
                xdot_ = J_.data * qdot_.data;

                // ********* 1. Desired Trajectory Generation in task space *********

                // *** 1.1 Desired Trajectory in taskspace ***
                if (ctr_obj_ == 1)
                {
                        // (1) Set Point Regulation
                        if (event == 0) // initial command
                        {
//                                xd_.p(0) = x_.p(0);
//                                xd_.p(1) = x_.p(1);
//                                xd_.p(2) = x_.p(2);
//                                xd_.M = x_.M;

                                xd_.p(0) = 0.0;
                                xd_.p(1) = -0.32;
                                xd_.p(2) = 0.56;
                                xd_.M = KDL::Rotation(KDL::Rotation::RPY(0, 0, 0));

                                xd_dot_(0) = 0;
                                xd_dot_(1) = 0;
                                xd_dot_(2) = 0;
                                xd_dot_(3) = 0;
                                xd_dot_(4) = 0;
                                xd_dot_(5) = 0;

                                xd_ddot_(0) = 0;
                                xd_ddot_(1) = 0;
                                xd_ddot_(2) = 0;
                                xd_ddot_(3) = 0;
                                xd_ddot_(4) = 0;
                                xd_ddot_(5) = 0;

                        }
                        else if (event == 1) // command from ros subscriber
                        {
                                xd_.p(0) = x_cmd_(0);
                                xd_.p(1) = x_cmd_(1);
                                xd_.p(2) = x_cmd_(2);
                                xd_.M = KDL::Rotation(KDL::Rotation::RPY(x_cmd_(3), x_cmd_(4), x_cmd_(5)));

                                xd_dot_(0) = x_cmd_(6);
                                xd_dot_(1) = x_cmd_(7);
                                xd_dot_(2) = x_cmd_(8);
                                xd_dot_(3) = 0;
                                xd_dot_(4) = 0;
                                xd_dot_(5) = 0;

                                xd_ddot_(0) = x_cmd_(9);
                                xd_ddot_(1) = x_cmd_(10);
                                xd_ddot_(2) = x_cmd_(11);
                                xd_ddot_(3) = 0;
                                xd_ddot_(4) = 0;
                                xd_ddot_(5) = 0;
                        }

                }
                else if (ctr_obj_ == 2)
                {
                        // (2) Tracking
                        xd_.p(0) = 0.1;
                        xd_.p(1) = A * sin(f * M_PI * (t - t_set)) + b;
                        xd_.p(2) = 0.5;
                        xd_.M = KDL::Rotation(KDL::Rotation::RPY(0, 0, 0));

                        xd_dot_(0) = 0;
                        xd_dot_(1) = (f * M_PI) * A * cos(f * M_PI * (t - t_set));
                        xd_dot_(3) = 0;
                        xd_dot_(4) = 0;
                        xd_dot_(5) = 0;

                        xd_ddot_(0) = 0;
                        xd_ddot_(1) = -1 * (f * M_PI) * (f * M_PI) * A * sin(f * M_PI * (t - t_set));
                        xd_ddot_(2) = 0;
                        xd_ddot_(3) = 0;
                        xd_ddot_(4) = 0;
                        xd_ddot_(5) = 0;
                }

                // ********* 2. Inverse Kinematics *********
                // *** 2.0 Error Definition in Task Space ***
                ex_temp_ = diff(x_, xd_);

                ex_(0) = ex_temp_(0);
                ex_(1) = ex_temp_(1);
                ex_(2) = ex_temp_(2);
                ex_(3) = ex_temp_(3);
                ex_(4) = ex_temp_(4);
                ex_(5) = ex_temp_(5);

                ex_dot_ = xd_dot_ - xdot_;
                // ex_int_ = xd_ - x_; // (To do: e_int 업데이트 필요요)

                // *** 2.1 computing Jacobian J(q) ***
                jnt_to_jac_solver_->JntToJac(q_, J_);

                //kkk
                xdot_ =J_.data * qdot_.data;
                ex_dot_ = xd_dot_ - xdot_;

                // *** 2.2 computing Jacobian transpose/inversion ***
                J_transpose_ = J_.data.transpose();
                J_inv_ = J_.data.inverse();
                // pseudo_inverse(J_.data,J_inv_.data,false);

                // *** 2.3 computing desired joint state from Open-loop/Closed-loop Inverse Kinematics ***
                // 2.3.1 Regulation Case
                if (ctr_obj_ == 1)
                {

                        if (t < t_set)
                        {
                                qd_.data = qd_old_.data;
                        }
                        else
                        {
                                qd_.data = qd_old_.data + J_transpose_ * K_regulation_ * ex_ * dt;
                                qd_old_.data = qd_.data;
                        }
                }
                // 2.3.2 Tracking Case
                else if (ctr_obj_ == 2)
                {
                        if (t < t_set)
                        {
                                qd_.data = qd_old_.data;
                        }
                        else
                        {
                                if (ik_mode_ == 1) // Open-loop Inverse Kinematics
                                {
                                        qd_.data = qd_old_.data + J_inv_ * xd_dot_ * dt;
                                        qd_old_.data = qd_.data;
                                }
                                else if (ik_mode_ == 2) // Closed-loop Inverse Kinematics
                                {
                                        qd_.data = qd_old_.data + J_inv_ * (xd_dot_ + K_tracking_ * ex_) * dt;
                                        qd_old_.data = qd_.data;
                                }
                        }
                }

                // ********* 3. Motion Controller in Joint Space*********
                // *** 3.1 Error Definition in Joint Space ***
                e_.data = qd_.data - q_.data;
                e_dot_.data = qd_dot_.data - qdot_.data;
                e_int_.data = qd_.data - q_.data; // (To do: e_int 업데이트 필요요 (Update required))

                // *** 3.2 Compute model(M,C,G) ***
                id_solver_->JntToMass(q_, M_);
                id_solver_->JntToCoriolis(q_, qdot_, C_);
                id_solver_->JntToGravity(q_, G_); // output은 머지? , id_solver는 어디에서?

                // *** 3.3 Apply Torque Command to Actuator ***
                //                aux_d_.data = M_.data * (qd_ddot_.data + Kp_.data.cwiseProduct(e_.data) + Kd_.data.cwiseProduct(e_dot_.data));
                //                comp_d_.data = C_.data + G_.data;
                //                tau_d_.data = aux_d_.data + comp_d_.data;

                //                for (int i = 0; i < n_joints_; i++)
                //                {
                //                        joints_[i].setCommand(tau_d_(i));
                //                        // joints_[i].setCommand(0.0);
                //                }

                // *********** 3'. Operational Space Control ***************

                //aux_d_.data =M_.data*(J_inv_*(Kp_.data.cwiseProduct(ex_)));
                aux_d_.data =M_.data*(J_inv_*(xd_ddot_ + Kd_.data.cwiseProduct(ex_dot_)+Kp_.data.cwiseProduct(ex_)));
                comp_d_.data = C_.data + G_.data;
                tau_d_.data = aux_d_.data+ G_.data;

                for (int i = 0; i < n_joints_; i++)
                {
                        joints_[i].setCommand(tau_d_(i));
                        //joints_[i].setCommand(0.0);
                }

                // ********* 4. data 저장 *********
                save_data();

                // ********* 5. state 출력 *********
                print_state();
        }

        void stopping(const ros::Time &time)
        {
        }

        void save_data()
        {
                // // 1
                // // Simulation time (unit: sec)

                SaveData_[0] = t;

                // // Desired position in joint space (unit: rad)
                // SaveData_[1] = qd_(0);
                // SaveData_[2] = qd_(1);
                // SaveData_[3] = qd_(2);
                // SaveData_[4] = qd_(3);
                // SaveData_[5] = qd_(4);
                // SaveData_[6] = qd_(5);

                // // Desired velocity in joint space (unit: rad/s)
                // SaveData_[7] = qd_dot_(0);
                // SaveData_[8] = qd_dot_(1);
                // SaveData_[9] = qd_dot_(2);
                // SaveData_[10] = qd_dot_(3);
                // SaveData_[11] = qd_dot_(4);
                // SaveData_[12] = qd_dot_(5);

                // // Desired acceleration in joint space (unit: rad/s^2)
                // SaveData_[13] = qd_ddot_(0);
                // SaveData_[14] = qd_ddot_(1);
                // SaveData_[15] = qd_ddot_(2);
                // SaveData_[16] = qd_ddot_(3);
                // SaveData_[17] = qd_ddot_(4);
                // SaveData_[18] = qd_ddot_(5);

                // // Actual position in joint space (unit: rad)
                // SaveData_[19] = q_(0);
                // SaveData_[20] = q_(1);
                // SaveData_[21] = q_(2);
                // SaveData_[22] = q_(3);
                // SaveData_[23] = q_(4);
                // SaveData_[24] = q_(5);

                // // Actual velocity in joint space (unit: rad/s)
                // SaveData_[25] = qdot_(0);
                // SaveData_[26] = qdot_(1);
                // SaveData_[27] = qdot_(2);
                // SaveData_[28] = qdot_(3);
                // SaveData_[29] = qdot_(4);
                // SaveData_[30] = qdot_(5);

                // // Error position in joint space (unit: rad)
                // SaveData_[31] = e_(0);
                // SaveData_[32] = e_(1);
                // SaveData_[33] = e_(2);
                // SaveData_[34] = e_(3);
                // SaveData_[35] = e_(4);
                // SaveData_[36] = e_(5);

                // // Error velocity in joint space (unit: rad/s)
                // SaveData_[37] = e_dot_(0);
                // SaveData_[38] = e_dot_(1);
                // SaveData_[39] = e_dot_(3);
                // SaveData_[40] = e_dot_(4);
                // SaveData_[41] = e_dot_(5);
                // SaveData_[42] = e_dot_(6);

                // // Error intergal value in joint space (unit: rad*sec)
                // SaveData_[43] = e_int_(0);
                // SaveData_[44] = e_int_(1);
                // SaveData_[45] = e_int_(2);
                // SaveData_[46] = e_int_(3);
                // SaveData_[47] = e_int_(4);
                // SaveData_[48] = e_int_(5);

                 // Desired position in task space (unit: m)
                 SaveData_[49] = xd_.p(0);
                 SaveData_[50] = xd_.p(1);
                 SaveData_[51] = xd_.p(2);
        //         SaveData_[52] = xd_.p(3);
        //         SaveData_[53] = xd_.p(4);
        //         SaveData_[54] = xd_.p(5);

                 // Desired velocity in task space (unit: m/s)
                 SaveData_[55] = xd_dot_(0);
                 SaveData_[56] = xd_dot_(1);
                 SaveData_[57] = xd_dot_(2);
        //         SaveData_[58] = xd_dot_(3);
        //         SaveData_[59] = xd_dot_(4);
        //         SaveData_[60] = xd_dot_(5);

                // // Desired acceleration in task space (unit: m/s^2)
                // SaveData_[61] = xd_ddot_(0);
                // SaveData_[62] = xd_ddot_(1);
                // SaveData_[63] = xd_ddot_(2);
                // SaveData_[64] = xd_ddot_(3);
                // SaveData_[65] = xd_ddot_(4);
                // SaveData_[66] = xd_ddot_(5);

                 // Actual position in task space (unit: m)
                 SaveData_[67] = x_.p(0);
                 SaveData_[68] = x_.p(1);
                 SaveData_[69] = x_.p(2);
        //         SaveData_[70] = x_.p(3);
        //         SaveData_[71] = x_.p(4);
        //         SaveData_[72] = x_.p(5);

                 // Actual velocity in task space (unit: m/s)
                 SaveData_[73] = xdot_(0);
                 SaveData_[74] = xdot_(1);
                 SaveData_[75] = xdot_(2);
        //         SaveData_[76] = xdot_(3);
        //         SaveData_[77] = xdot_(4);
        //         SaveData_[78] = xdot_(5);

                // // Error position in task space (unit: m)
                // SaveData_[79] = ex_(0);
                // SaveData_[80] = ex_(1);
                // SaveData_[81] = ex_(2);
                // SaveData_[82] = ex_(3);
                // SaveData_[83] = ex_(4);
                // SaveData_[84] = ex_(5);

                // // Error velocity in task space (unit: m/s)
                // SaveData_[85] = ex_dot_(0);
                // SaveData_[86] = ex_dot_(1);
                // SaveData_[87] = ex_dot_(2);
                // SaveData_[88] = ex_dot_(3);
                // SaveData_[89] = ex_dot_(4);
                // SaveData_[90] = ex_dot_(5);

                // // Error intergal value in task space (unit: m*sec)
                // SaveData_[91] = ex_int_(0);
                // SaveData_[92] = ex_int_(1);
                // SaveData_[93] = ex_int_(2);
                // SaveData_[94] = ex_int_(3);
                // SaveData_[95] = ex_int_(4);
                // SaveData_[96] = ex_int_(5);

                // // 2
                // msg_qd_.data.clear();
                // msg_q_.data.clear();
                // msg_e_.data.clear();

                // msg_xd_.data.clear();
                // msg_x_.data.clear();
                // msg_ex_.data.clear();

                 msg_SaveData_.data.clear();

                // // 3
                // for (int i = 0; i < n_joints_; i++)
                // {
                //     msg_qd_.data.push_back(qd_(i));
                //     msg_q_.data.push_back(q_(i));
                //     msg_e_.data.push_back(e_(i));
                // }

                // for (int i = 0; i < num_taskspace; i++)
                // {
                //     msg_xd_.data.push_back(xd_(i));
                //     msg_x_.data.push_back(x_(i));
                //     msg_ex_.data.push_back(ex_(i));
                // }

                 for (int i = 0; i < SaveDataMax; i++)
                 {
                     msg_SaveData_.data.push_back(SaveData_[i]);
                 }

                // // 4
                // pub_qd_.publish(msg_qd_);
                // pub_q_.publish(msg_q_);
                // pub_e_.publish(msg_e_);

                // pub_xd_.publish(msg_xd_);
                // pub_x_.publish(msg_x_);
                // pub_ex_.publish(msg_ex_);

                 pub_SaveData_.publish(msg_SaveData_);
        }

        void print_state()
        {
                static int count = 0;
                if (count > 99)
                {
                        printf("*********************************************************\n\n");
                        printf("*** Simulation Time (unit: sec)  ***\n");
                        printf("t = %f\n", t);
                        printf("\n");

                        printf("*** Command from Subscriber in Task Space (unit: m) ***\n");
                        if (event == 0)
                        {
                                printf("No Active!!!\n");
                        }
                        else
                        {
                                printf("Active!!!\n");
                        }
                        printf("x_cmd: %f, ", x_cmd_(0));
                        printf("y_cmd: %f, ", x_cmd_(1));
                        printf("z_cmd: %f, ", x_cmd_(2));
                        printf("r_cmd: %f, ", x_cmd_(3));
                        printf("p_cmd: %f, ", x_cmd_(4));
                        printf("y_cmd: %f\n", x_cmd_(5));
                        printf("\n");

                        printf("*** Desired Position in Joint Space (unit: deg) ***\n");
                        printf("qd(1): %f, ", qd_(0) * R2D);
                        printf("qd(2): %f, ", qd_(1) * R2D);
                        printf("qd(3): %f, ", qd_(2) * R2D);
                        printf("qd(4): %f, ", qd_(3) * R2D);
                        printf("qd(5): %f, ", qd_(4) * R2D);
                        printf("qd(6): %f\n", qd_(5) * R2D);
                        printf("\n");

                        printf("*** Actual Position in Joint Space (unit: deg) ***\n");
                        printf("q(1): %f, ", q_(0) * R2D);
                        printf("q(2): %f, ", q_(1) * R2D);
                        printf("q(3): %f, ", q_(2) * R2D);
                        printf("q(4): %f, ", q_(3) * R2D);
                        printf("q(5): %f, ", q_(4) * R2D);
                        printf("q(6): %f\n", q_(5) * R2D);
                        printf("\n");

                        printf("*** Desired Position in Task Space (unit: m) ***\n");
                        printf("xd: %f, ", xd_.p(0));
                        printf("yd: %f, ", xd_.p(1));
                        printf("zd: %f\n", xd_.p(2));
                        printf("\n");

                        printf("*** Actual Position in Task Space (unit: m) ***\n");
                        printf("x: %f, ", x_.p(0));
                        printf("y: %f, ", x_.p(1));
                        printf("z: %f\n", x_.p(2));
                        printf("\n");

                        // printf("*** Desired Orientation in Task Space (unit: ??) ***\n");
                        // printf("xd_(0): %f, ", xd_.M(KDL::Rotation::RPY(0));
                        // printf("xd_(1): %f, ", xd_.M(KDL::Rotation::RPY(1));
                        // printf("xd_(2): %f\n", xd_.M(KDL::Rotation::RPY(2));
                        // printf("\n");

                        // printf("*** Actual Orientation in Task Space (unit: ??) ***\n");
                        // printf("x_(0): %f, ", x_.M(KDL::Rotation::RPY(0));
                        // printf("x_(1): %f, ", x_.M(KDL::Rotation::RPY(1));
                        // printf("x_(2): %f\n", x_.M(KDL::Rotation::RPY(2));
                        // printf("\n");

                        printf("*** Desired Translation Velocity in Task Space (unit: m/s) ***\n");
                        printf("xd_dot: %f, ", xd_dot_(0));
                        printf("yd_dot: %f, ", xd_dot_(1));
                        printf("zd_dot: %f\n", xd_dot_(2));
                        printf("\n");

                        printf("*** Actual Translation Velocity in Task Space (unit: m/s) ***\n");
                        printf("xdot: %f, ", xdot_(0));
                        printf("ydot: %f  ", xdot_(1));
                        printf("zdot: %f\n", xdot_(2));
                        printf("\n");

                        printf("*** Desired Angular Velocity in Task Space (unit: rad/s) ***\n");
                        printf("rd_dot: %f, ", xd_dot_(3));
                        printf("pd_dot: %f, ", xd_dot_(4));
                        printf("yd_dot: %f\n", xd_dot_(5));
                        printf("\n");

                        printf("*** Actual Angular Velocity in Task Space (unit: rad/s) ***\n");
                        printf("r_dot: %f, ", xdot_(3));
                        printf("p_dot: %f  ", xdot_(4));
                        printf("y_dot: %f\n", xdot_(5));
                        printf("\n");

                        printf("*** Desired Rotation Matrix of end-effector ***\n");
                        printf("%f, ",xd_.M(0,0));
                        printf("%f, ",xd_.M(0,1));
                        printf("%f\n",xd_.M(0,2));
                        printf("%f, ",xd_.M(1,0));
                        printf("%f, ",xd_.M(1,1));
                        printf("%f\n",xd_.M(1,2));
                        printf("%f, ",xd_.M(2,0));
                        printf("%f, ",xd_.M(2,1));
                        printf("%f\n",xd_.M(2,2));
                        printf("\n");

                        printf("*** Actual Rotation Matrix of end-effector ***\n");
                        printf("%f, ",x_.M(0,0));
                        printf("%f, ",x_.M(0,1));
                        printf("%f\n",x_.M(0,2));
                        printf("%f, ",x_.M(1,0));
                        printf("%f, ",x_.M(1,1));
                        printf("%f\n",x_.M(1,2));
                        printf("%f, ",x_.M(2,0));
                        printf("%f, ",x_.M(2,1));
                        printf("%f\n",x_.M(2,2));
                        printf("\n");

                        printf("*** Joint Space Error (unit: deg)  ***\n");
                        printf("q1: %f, ", R2D * e_(0));
                        printf("q2: %f, ", R2D * e_(1));
                        printf("q3: %f, ", R2D * e_(2));
                        printf("q4: %f, ", R2D * e_(3));
                        printf("q5: %f, ", R2D * e_(4));
                        printf("q6: %f\n", R2D * e_(5));
                        printf("\n");

                        printf("*** Task Space Position Error (unit: mm) ***\n");
                        printf("x: %f, ", ex_(0)*1000);
                        printf("y: %f, ", ex_(1)*1000);
                        printf("z: %f\n", ex_(2)*1000);
                        printf("\n");

                        printf("*** Task Space Orientation Error ?? (unit: deg) ***\n");
                        printf("r: %f, ", ex_(3)*R2D);
                        printf("p: %f, ", ex_(4)*R2D);
                        printf("y: %f\n", ex_(5)*R2D);
                        printf("\n");

                        count = 0;
                }
                count++;
        }

private:
        // others
        double t;
        int ctr_obj_;
        int ik_mode_;
        int event;

        //Joint handles
        unsigned int n_joints_;                               // joint 숫자
        std::vector<std::string> joint_names_;                // joint name ??
        std::vector<hardware_interface::JointHandle> joints_; // ??
        std::vector<urdf::JointConstSharedPtr> joint_urdfs_;  // ??

        // kdl
        KDL::Tree kdl_tree_;   // tree?
        KDL::Chain kdl_chain_; // chain?

        // kdl M,C,G
        KDL::JntSpaceInertiaMatrix M_; // intertia matrix
        KDL::JntArray C_;              // coriolis
        KDL::JntArray G_;              // gravity torque vector
        KDL::Vector gravity_;

        // kdl and Eigen Jacobian
        KDL::Jacobian J_;
        // KDL::Jacobian J_inv_;
        // Eigen::Matrix<double, num_taskspace, num_taskspace> J_inv_;
        Eigen::MatrixXd J_inv_;
        Eigen::Matrix<double, num_taskspace, num_taskspace> J_transpose_;

        // kdl solver
        boost::scoped_ptr<KDL::ChainFkSolverPos_recursive> fk_pos_solver_; //Solver to compute the forward kinematics (position)
        // boost::scoped_ptr<KDL::ChainFkSolverVel_recursive> fk_vel_solver_; //Solver to compute the forward kinematics (velocity)
        boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_; //Solver to compute the jacobian
        boost::scoped_ptr<KDL::ChainDynParam> id_solver_;               // Solver To compute the inverse dynamics

        // Joint Space State
        KDL::JntArray qd_, qd_dot_, qd_ddot_;
        KDL::JntArray qd_old_;
        KDL::JntArray q_, qdot_;
        KDL::JntArray e_, e_dot_, e_int_;

        // Task Space State
        // ver. 01
        KDL::Frame xd_; // x.p: frame position(3x1), x.m: frame orientation (3x3)
        KDL::Frame x_;
        KDL::Twist ex_temp_;

        // KDL::Twist xd_dot_, xd_ddot_;
        Eigen::Matrix<double, num_taskspace, 1> ex_;
        Eigen::Matrix<double, num_taskspace, 1> xd_dot_, xd_ddot_;
        Eigen::Matrix<double, num_taskspace, 1> xdot_;
        Eigen::Matrix<double, num_taskspace, 1> ex_dot_, ex_int_;

        // ver. 02
        // Eigen::Matrix<double, num_taskspace, 1> xd_, xd_dot_, xd_ddot_;
        // Eigen::Matrix<double, num_taskspace, 1> x_, xdot_;
        // KDL::Frame x_temp_;
        // Eigen::Matrix<double, num_taskspace, 1> ex_, ex_dot_, ex_int_;

        // Input
        KDL::JntArray x_cmd_;

        // Torque
        KDL::JntArray aux_d_;
        KDL::JntArray comp_d_;
        KDL::JntArray tau_d_;

        // gains
        KDL::JntArray Kp_, Ki_, Kd_;
        double K_regulation_, K_tracking_;

        // save the data
        double SaveData_[SaveDataMax];

        // ros subscriber
        ros::Subscriber sub_x_cmd_;

        // ros publisher
        ros::Publisher pub_qd_, pub_q_, pub_e_;
        ros::Publisher pub_xd_, pub_x_, pub_ex_;
        ros::Publisher pub_SaveData_;

        // ros message
        std_msgs::Float64MultiArray msg_qd_, msg_q_, msg_e_;
        std_msgs::Float64MultiArray msg_xd_, msg_x_, msg_ex_;
        std_msgs::Float64MultiArray msg_SaveData_;
};
}; // namespace arm_controllers
PLUGINLIB_EXPORT_CLASS(arm_controllers::motion_control, controller_interface::ControllerBase)


#ifndef HEAD_HPP_
#define HEAD_HPP_

#include <iostream>
#include <fstream>
#include <algorithm>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <vector>
#include <map>
#include <math.h>
#include <stdlib.h>
#include <set>
#include <vector>
#include <string.h>
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include <Eigen/SparseCore>
#include <cstdlib>
#include <time.h>
#include <omp.h>

using namespace std;
using namespace boost;
using namespace Eigen;

typedef Eigen::Matrix<double, Dynamic, 1> ColumnVector;
typedef Eigen::Matrix<double, 1, Dynamic> RowVector;
typedef Eigen::Matrix<double, Dynamic, Dynamic> MatrixXd;

double pall_dot(RowVector feature, ColumnVector beta){
	double sum=0.0; int nf=beta.rows();
	omp_set_num_threads(36);
	#pragma omp parallel
	{
		#pragma omp for reduction(+:sum)
		for(int i=0;i<nf;i++)
			sum +=feature[i]*beta[i];
	}
		return sum;
}

#endif /* HEAD_HPP_ */


#include <memory>
#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <vector>
#include <thread>
#include <chrono>
#include <exception>

#include <armadillo>

#include <ros/ros.h>
#include <ros/package.h>

#include <io_lib/io_lib.h>
#include <ur_robot/ur_robot.h>

#include <io_utils.h>

using namespace as64_;

class Worker
{
public:
  Worker(const std::string &ip, int port)
  {
    robot.reset(new ur_::Robot(ip, port));
  }

  void setFreeDriveMode()
  {
    robot->setMode(ur_::Mode::FREEDRIVE_MODE);
  }

  void printJointsPos()
  {
    std::thread([this]()
    {
      arma::vec jpos = robot->getJointsPosition();
      std::cout << "jpos = " << jpos.t() << "\n";
    }).detach();
  }

  bool setJointsTrajectory(const arma::vec &qT, double duration)
  {
    // keep last known robot mode
    ur_::Mode prev_mode = robot->getMode();
    // start controller
    robot->setMode(ur_::Mode::VELOCITY_CONTROL);

    // waits for the next tick
    robot->waitNextCycle();

    arma::vec q0 = robot->getJointsPosition();
    arma::vec q = q0;
    arma::vec qref = q0;
    arma::vec qref_dot;

    robot->startLogging();

    double t = 0.0;
    double click = 0.0;
    // the main while
    while (t < duration)
    {
      if (!robot->isOk())
      {
        std::cerr << robot->getErrMsg();
        return false;
      }

      // compute time now
      t += robot->getCtrlCycle();
      // update trajectory
      arma::mat ref_traj =  get5thOrder(t, q0, qT, duration);
      qref = ref_traj.col(0);
      qref_dot = ref_traj.col(1);

      arma::vec q_dot_cmd = qref_dot + click*(qref-q);

      robot->setJointsVelocity(q_dot_cmd, qref);

      // waits for the next tick
      robot->waitNextCycle();

      q = robot->getJointsPosition();
    }

    robot->stopLogging();

    // reset last known robot mode
    robot->setMode(prev_mode);

    robot->saveLoggedData(ros::package::getPath("ur_test") + "/data/logged_data.bin");

    return true;
  }

  void executeTrajectory()
  {
    arma::vec q;
    arma::vec q_start = {-0.7827,  -1.2511,  -1.4126,  -1.6371,   1.6009,   0.0254};
    arma::vec q_end = {0.3761,  -1.8148,  -1.4127,  -2.7396,   1.6009,   0.0254};

    // Set the robot in position control mode
    robot->setMode(ur_::Mode::POSITION_CONTROL);
    std::cout << io_::bold << io_::green << "=== The robot is in position control ===\n" << io_::reset;

    if (!ros::ok()) exit(-1);

    robot->waitNextCycle();
    q = robot->getJointsPosition();

    double duration = 6.5;
    std::cout << io_::bold << io_::cyan << "The robot will move to its initial pose in " << duration << " sec.\n" << io_::reset;
    robot->setJointsTrajectory(q_start, duration);
    std::cout << io_::bold << io_::cyan << "Initial pose reached!\n" << io_::reset;
    if (!ros::ok()) exit(-1);

    robot->waitNextCycle();
    q = robot->getJointsPosition();
    duration = 6.5;
    std::cout << io_::bold << io_::cyan << "The robot will move to its final pose in " << duration << " sec.\n" << io_::reset;
    robot->startLogging();
    robot->setJointsTrajectory(q_end, duration);
    robot->stopLogging();
    std::cout << io_::bold << io_::cyan << "*** Final pose reached! ***\n" << io_::reset;

    robot->saveLoggedData(ros::package::getPath("ur_test") + "/data/logged_data.bin");
  }

  std::shared_ptr<ur_::Robot> robot;

private:

  arma::mat get5thOrder(double t, arma::vec p0, arma::vec pT, double totalTime)
  {
    arma::mat retTemp = arma::zeros<arma::mat>(p0.n_rows, 3);

    if (t < 0)
    {
      // before start
      retTemp.col(0) = p0;
    }
    else if (t > totalTime)
    {
      // after the end
      retTemp.col(0) = pT;
    }
    else
    {
      // somewhere betweeen ...
      // position
      retTemp.col(0) = p0 +
                       (pT - p0) * (10 * pow(t / totalTime, 3) -
                       15 * pow(t / totalTime, 4) +
                       6 * pow(t / totalTime, 5));
      // vecolity
      retTemp.col(1) = (pT - p0) * (30 * pow(t, 2) / pow(totalTime, 3) -
                       60 * pow(t, 3) / pow(totalTime, 4) +
                       30 * pow(t, 4) / pow(totalTime, 5));
      // acceleration
      retTemp.col(2) = (pT - p0) * (60 * t / pow(totalTime, 3) -
                       180 * pow(t, 2) / pow(totalTime, 4) +
                       120 * pow(t, 3) / pow(totalTime, 5));
    }

    // return vector
    return retTemp;
  }
};

int main(int argc, char** argv)
{
  // ===========  Initialize the ROS node  ==================
  ros::init(argc, argv, "test_ur");

  ros::NodeHandle nh("~");
  std::string robot_ip;
  if (!nh.getParam("robot_ip",robot_ip)) throw std::runtime_error("Failed to load param \"robot_ip\"...");

  // ===========  Create robot  ==================
  Worker worker(robot_ip, 50001);
  worker.setFreeDriveMode();

  arma::vec q1 = {-0.7827,  -1.2511,  -1.4126,  -1.6371,   1.6009,   0.0254};
  arma::vec q2 = {0.3761,  -1.8148,  -1.4127,  -2.7396,   1.6009,   0.0254};

  while (ros::ok())
  {
    worker.robot->waitNextCycle();
    char ch = ' ';
    ch = getch();
    if (ch == 'f') worker.setFreeDriveMode();
    if (ch == 't') worker.executeTrajectory();
    if (ch == '1') worker.setJointsTrajectory(q1, 6);  //worker.robot->movej(q1, 1.4, 1.05, 8); //
    if (ch == '2') worker.setJointsTrajectory(q2, 6); //worker.robot->movej(q1, 1.4, 1.05, 8); //
    if (ch == 'j') worker.printJointsPos();
    if (ch == 'q') throw std::runtime_error("Program termination request!");
  }

  // ===========  Shutdown ROS node  ==================
  // ros::shutdown();

  return 0;
}


// Copyright (c) 2020, Oracle and/or its affiliates.
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License, version 2.0,
// as published by the Free Software Foundation.
//
// This program is also distributed with certain software (including
// but not limited to OpenSSL) that is licensed under separate terms,
// as designated in a particular file or component or in included license
// documentation.  The authors of MySQL hereby grant you an additional
// permission to link the program and your derivative works with the
// separately licensed software that they have included with MySQL.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License, version 2.0, for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301  USA.

/// @file
///
/// This file implements the discrete Hausdorff distance functor and function.

#include <cmath>  // std::isfinite

#include <boost/geometry.hpp>
#include "my_inttypes.h"                            // MYF
#include "my_sys.h"                                 // my_error
#include "mysqld_error.h"                           // Error codes
#include "sql/dd/types/spatial_reference_system.h"  // dd::Spatial_reference_system
#include "sql/gis/gc_utils.h"
#include "sql/gis/geometries.h"
#include "sql/gis/geometries_traits.h"
#include "sql/gis/hausdorff_distance.h"
#include "sql/gis/hausdorff_distance_functor.h"
#include "sql/sql_exception_handler.h"  // handle_gis_exception

namespace bg = boost::geometry;
namespace bgs = boost::geometry::srs;
namespace bgsd = boost::geometry::strategy::distance;

namespace gis {

Hausdorff_distance::Hausdorff_distance(double major, double minor)
    : m_geographic_strategy(
          new bgsd::geographic<boost::geometry::strategy::andoyer,
                               bgs::spheroid<double>>(
              bgs::spheroid<double>(major, minor))) {}

double Hausdorff_distance::operator()(const Geometry *g1,
                                      const Geometry *g2) const {
  return apply(*this, g1, g2);
}

double Hausdorff_distance::eval(const Geometry *g1, const Geometry *g2) const {
  throw not_implemented_exception::for_non_projected(*g1, *g2);
}

double Hausdorff_distance::eval(const Cartesian_point *g1,
                                const Cartesian_multipoint *g2) const {
  return bg::discrete_hausdorff_distance(*g1, *g2);
}

double Hausdorff_distance::eval(const Cartesian_multipoint *g1,
                                const Cartesian_point *g2) const {
  return bg::discrete_hausdorff_distance(*g2, *g1);
}

double Hausdorff_distance::eval(const Cartesian_linestring *g1,
                                const Cartesian_linestring *g2) const {
  return bg::discrete_hausdorff_distance(*g1, *g2);
}

double Hausdorff_distance::eval(const Cartesian_multipoint *g1,
                                const Cartesian_multipoint *g2) const {
  return bg::discrete_hausdorff_distance(*g1, *g2);
}

double Hausdorff_distance::eval(const Cartesian_linestring *g1,
                                const Cartesian_multilinestring *g2) const {
  return bg::discrete_hausdorff_distance(*g1, *g2);
}

double Hausdorff_distance::eval(const Cartesian_multilinestring *g1,
                                const Cartesian_linestring *g2) const {
  return bg::discrete_hausdorff_distance(*g2, *g1);
}

double Hausdorff_distance::eval(const Cartesian_multilinestring *g1,
                                const Cartesian_multilinestring *g2) const {
  return bg::discrete_hausdorff_distance(*g1, *g2);
}

double Hausdorff_distance::eval(const Geographic_point *g1,
                                const Geographic_multipoint *g2) const {
  return bg::discrete_hausdorff_distance(*g1, *g2, *m_geographic_strategy);
}

double Hausdorff_distance::eval(const Geographic_multipoint *g1,
                                const Geographic_point *g2) const {
  return bg::discrete_hausdorff_distance(*g2, *g1, *m_geographic_strategy);
}

double Hausdorff_distance::eval(const Geographic_linestring *g1,
                                const Geographic_linestring *g2) const {
  return bg::discrete_hausdorff_distance(*g1, *g2, *m_geographic_strategy);
}

double Hausdorff_distance::eval(const Geographic_multipoint *g1,
                                const Geographic_multipoint *g2) const {
  return bg::discrete_hausdorff_distance(*g1, *g2, *m_geographic_strategy);
}

double Hausdorff_distance::eval(const Geographic_linestring *g1,
                                const Geographic_multilinestring *g2) const {
  return bg::discrete_hausdorff_distance(*g1, *g2, *m_geographic_strategy);
}

double Hausdorff_distance::eval(const Geographic_multilinestring *g1,
                                const Geographic_linestring *g2) const {
  return bg::discrete_hausdorff_distance(*g2, *g1, *m_geographic_strategy);
}

double Hausdorff_distance::eval(const Geographic_multilinestring *g1,
                                const Geographic_multilinestring *g2) const {
  return bg::discrete_hausdorff_distance(*g1, *g2, *m_geographic_strategy);
}

/////////////////////////////////////////////////////////////////////////////

bool hausdorff_distance(const dd::Spatial_reference_system *srs,
                        const Geometry *g1, const Geometry *g2,
                        const char *func_name, double *hausdorff_distance,
                        bool *is_null) noexcept {
  try {
    DBUG_ASSERT(g1->coordinate_system() == g2->coordinate_system());
    DBUG_ASSERT(srs == nullptr ||
                ((srs->is_cartesian() &&
                  g1->coordinate_system() == Coordinate_system::kCartesian) ||
                 (srs->is_geographic() &&
                  g1->coordinate_system() == Coordinate_system::kGeographic)));

    if ((*is_null = (g1->is_empty() || g2->is_empty()))) return false;

    Hausdorff_distance hd(srs ? srs->semi_major_axis() : 0.0,
                          srs ? srs->semi_minor_axis() : 0.0);
    *hausdorff_distance = hd(g1, g2);
  } catch (...) {
    handle_gis_exception(func_name);
    return true;
  }

  if (!std::isfinite(*hausdorff_distance) || *hausdorff_distance < 0.0) {
    my_error(ER_DATA_OUT_OF_RANGE, MYF(0), "Hausdorff_distance", func_name);
    return true;
  }

  return false;
}

}  // namespace gis


#ifndef CELL_HPP
#define CELL_HPP

#include "vector"
#include "array"
#include "iostream"

#include <Eigen/Dense>
#include "mpi.h"

using namespace Eigen;

/// 4x1 vector FIXME multiple definitions of the same type also in pic.cpp
typedef Array<double, 4, 1> vec4;
// typedef Array<double, 3, 1> vec3;

/// Full 4x4 GR vector
typedef Array<double, 4, 4> mat4;

// Define cell types for different boundary conditions
#define NORMAL_CELL 0               ///< Cell in a normal state
#define METAL_BOUNDARY_X_CELL 1     ///< Cell with a conducting X boundary
#define METAL_BOUNDARY_Y_CELL 2     ///< Cell with a conducting Y boundary

// Enumarete particle populations
enum Population
{
    ELECTRONS, ///< Electron particle population
    POSITRONS,  ///< positron particle population
    N_POPULATIONS
};

/// Modular Cell class for mpiGrid
/**
Cell class to handle MPI communications
*/

class Cell
{
public:

    /// current number of electrons inside the Cell
	// unsigned int number_of_electrons = 0;

    /// current number of positrons inside the Cell
	// unsigned int number_of_positrons = 0;

    std::array<uint64_t , Population::N_POPULATIONS> particles_in_population;

    uint64_t& number_of(Population particle_population)
    {
        return particles_in_population[particle_population];
    };

    /// Particle 6D phase space populations
    ///@{
	/// 6D phase space coordinates of electrons in this cell
	std::vector<std::array<double, 6> > electrons;

	/// 6D phase space coordinates of positrons in this cell
	std::vector<std::array<double, 6> > positrons;
    ///@}

    /// Particle population switcher TODO fix/check performance
    std::vector<std::array<double, 6> >& particles(Population particle_population)
    {
        switch(particle_population) {
            case Population::ELECTRONS:
                return this->electrons;
                break;
            case Population::POSITRONS:
                return this->positrons;
                break;
            default:
                    std::cerr << __FILE__ << ":" << __LINE__
                    << " Invalid population switch: " << particle_population
                    << std::endl;
                abort();
                break;
        }
    };

    /// 4-current vector containing (rho, Jx, Jy, Jz)
    vec4 J;

    /// standard 3-vector for currents & field vectors at Yee lattice staggeration
    /**
        XXX Here we use 3+1 formalism which is more easily
        expressed via E and B, not F tensor
    */
    Vector3d JY, BY, EY;

    /* Yee staggered field tensor
        note that E and B elements are defined in different locations
        due to the staggering
     mat4 FY = mat4::Zero();
    */

    // Yee lattice/mesh stuff array
    /*
        0 - charge density rho
        DONE 1 2 3    - current vector on nodal points Jx, Jy, Jz
        DONE 4 5 6    - current vector on Yee lattice  JxY, JyY, JzY
        7 8 9    - E field on Yee lattice ExY, EyY, EzY
        10 11 12 - B field on Yee lattice BxY, ByY, BzY

        13 14 15 - E field on nodal lattice Ex, Ey, Ez
        16 17 18 - B field on nodal lattice Bx, By, Bz
    std::array<double, 19> field = {{0.0,
                                     0.0, 0.0, 0.0,
                                     0.0, 0.0, 0.0,
                                     0.0, 0.0, 0.0,
                                     0.0, 0.0, 0.0,
                                     0.0, 0.0, 0.0,
                                     0.0, 0.0, 0.0}};
    */

    /// incoming currents from other processes
    /**  3x3x3 cube with four-vector elements = 108 values */
    std::array<double, 108> incoming_currents;

    /** @name Auxiliary functions
     *  auxiliary functions to help disentangle lattice values
     *
     *  note how we always give normal and const definition
     *  this makes sure the variable pointed to by the returned
     *  pointer & won't be alterable and that the method does not
     *  alter the variable pointed to by the given pointer. (phew!)
    */

    /*
    ///@{
    /// overload array operator for object
    double& operator [](const std::size_t i){return this->field[i];}
    const double& operator [](const std::size_t i) const {return this->field[i];}

    /// charge density
    double& rho(){ return this->field[0]; }
    const double& rho() const { return this->field[0]; }

    /// nodal current x
    double& Jx(){ return this->field[1]; }
    const double& Jx() const { return this->field[1]; }

    /// nodal current y
    double& Jy(){ return this->field[2]; }
    const double& Jy() const { return this->field[2]; }

    /// nodal current z
    double& Jz(){ return this->field[3]; }
    const double& Jz() const { return this->field[3]; }

    /// Yee currents x
    double& JxY(){ return this->field[4]; }
    const double& JxY() const { return this->field[4]; }

    /// Yee currents y
    double& JyY(){ return this->field[5]; }
    const double& JyY() const { return this->field[5]; }

    /// Yee currents z
    double& JzY(){ return this->field[6]; }
    const double& JzY() const { return this->field[6]; }

    /// Yee E x
    double& ExY(){ return this->field[7]; }
    const double& ExY() const { return this->field[7]; }

    /// Yee E y
    double& EyY(){ return this->field[8]; }
    const double& EyY() const { return this->field[8]; }

    /// Yee E z
    double& EzY(){ return this->field[9]; }
    const double& EzY() const { return this->field[9]; }

    /// Yee B x
    double& BxY(){ return this->field[10]; }
    const double& BxY() const { return this->field[10]; }

    /// Yee B y
    double& ByY(){ return this->field[11]; }
    const double& ByY() const { return this->field[11]; }

    /// Yee B z
    double& BzY(){ return this->field[12]; }
    const double& BzY() const { return this->field[12]; }

    /// nodal E field vector x
    double& Ex(){ return this->field[13]; }
    const double& Ex() const { return this->field[13]; }

    /// nodal E field vector y
    double& Ey(){ return this->field[14]; }
    const double& Ey() const { return this->field[14]; }

    /// nodal E field vector z
    double& Ez(){ return this->field[15]; }
    const double& Ez() const { return this->field[15]; }

    /// nodal B x
    double& Bx(){ return this->field[16]; }
    const double& Bx() const { return this->field[16]; }

    /// nodal B y
    double& By(){ return this->field[17]; }
    const double& By() const { return this->field[17]; }

    /// nodal B z
    double& Bz(){ return this->field[18]; }
    const double& Bz() const { return this->field[18]; }
    ///@}
    */

    /// Cell type XXX: use me!
    double cell_type = 0;

    /// list of remote neighbors
    std::array<uint64_t, 27>
        remote_neighbor_list = {{0, 0, 0, 0, 0, 0, 0, 0, 0,
                                 0, 0, 0, 0, 0, 0, 0, 0, 0,
                                 0, 0, 0, 0, 0, 0, 0, 0, 0
                                }};

    /// transfer switch
	/**
     Defines what is transferred over MPI
	*/
	static int transfer_mode;

    // Enumarete data types
    enum {
        INIT,                 /// < data related to initialization
        NUMBER_OF_ELECTRONS,  /// < Number of electrons to expect
        NUMBER_OF_POSITRONS,  /// < Number of positrons to expect
        ELECTRONS,            /// < 6D electron phase phases
        POSITRONS,            /// < 6D positron phase phases
        FIELDS,               /// < data of EM fields
		REMOTE_NEIGHBOR_LIST, /// < List of remote neighbors sending to me
		INCOMING_CURRENTS,    /// < data of incoming currents
        CURRENT,              /// < current four vector
        YEE_CURRENT,          /// < staggered current four vector
        YEE_B,                /// < Staggered B field
        YEE_E,                /// < Staggered E field
		TYPE                  /// < Cell type
    };

    /// handle the MPI calls depending on cell state
	std::tuple<void*, int, MPI_Datatype> get_mpi_datatype();

	/// reserves space for electron phase space data coming over MPI
    /// TODO make ambigious of population
    void resize_population(Population particle_population)
    {
        switch(particle_population) {
            case Population::ELECTRONS:
                this->electrons.resize(this->number_of(Population::ELECTRONS));
                break;
            case Population::POSITRONS:
                this->positrons.resize(this->number_of(Population::POSITRONS));
                break;
            default:
                    std::cerr << __FILE__ << ":" << __LINE__
                    << " Invalid population switch: " << particle_population
                    << std::endl;
                abort();
                break;
        }
    };

};

#endif


#include <map>
#include <numeric>

#include <Eigen/Dense>

#include <BingoCpp/backend.h>
#include <BingoCpp/backend_nodes.h>
#include <BingoCpp/constants.h>

namespace bingo {
namespace backend {
namespace {

Eigen::ArrayXXd reverse_eval(const std::pair<int, int> &deriv_shape,
                             const int deriv_wrt_node,
                             const std::vector<Eigen::ArrayXXd> &forward_eval,
                             const Eigen::ArrayX3i &stack);

Eigen::ArrayXXd reverse_eval_with_mask(const std::pair<int, int> &deriv_shape,
                                       const int deriv_wrt_node,
                                       const std::vector<Eigen::ArrayXXd> &forward_eval,
                                       const Eigen::ArrayX3i &stack,
                                       const std::vector<bool> &mask);

std::vector<Eigen::ArrayXXd> forward_eval(
    const Eigen::ArrayX3i &stack,
    const Eigen::ArrayXXd &x,
    const Eigen::VectorXd &constants);

std::vector<Eigen::ArrayXXd> forward_eval_with_mask(
    const Eigen::ArrayX3i &stack,
    const Eigen::ArrayXXd &x,
    const Eigen::VectorXd &constants,
    const std::vector<bool> &mask);

EvalAndDerivative evaluate_with_derivative(
    const Eigen::ArrayX3i &stack,
    const Eigen::ArrayXXd &x,
    const Eigen::VectorXd &constants,
    const bool param_x_or_c);

EvalAndDerivative evaluate_with_derivative_and_mask(
    const Eigen::ArrayX3i &stack,
    const Eigen::ArrayXXd &x,
    const Eigen::VectorXd &constants,
    const std::vector<bool> &mask,
    const bool param_x_or_c);
} // namespace

Eigen::ArrayXXd Evaluate(const Eigen::ArrayX3i &stack,
                         const Eigen::ArrayXXd &x,
                         const Eigen::VectorXd &constants) {
  std::vector<Eigen::ArrayXXd> _forward_eval = forward_eval(
      stack, x, constants);
  return _forward_eval.back();
}

std::pair<Eigen::ArrayXXd, Eigen::ArrayXXd> EvaluateWithDerivative(
    const Eigen::ArrayX3i &stack,
    const Eigen::ArrayXXd &x,
    const Eigen::VectorXd &constants,
    const bool param_x_or_c) {
  return evaluate_with_derivative(
    stack, x, constants, param_x_or_c);
}

Eigen::ArrayXXd SimplifyAndEvaluate(const Eigen::ArrayX3i &stack,
                                    const Eigen::ArrayXXd &x,
                                    const Eigen::VectorXd &constants) {
  std::vector<bool> mask = GetUtilizedCommands(stack);
  std::vector<Eigen::ArrayXXd> forward_eval = forward_eval_with_mask(
      stack, x, constants, mask);
  return forward_eval.back();
}

EvalAndDerivative SimplifyAndEvaluateWithDerivative(
    const Eigen::ArrayX3i &stack,
    const Eigen::ArrayXXd &x,
    const Eigen::VectorXd &constants,
    const bool param_x_or_c) {
  std::vector<bool> mask = GetUtilizedCommands(stack);
  return evaluate_with_derivative_and_mask(stack, x, constants, mask, param_x_or_c);
}

std::vector<bool> GetUtilizedCommands(const Eigen::ArrayX3i &stack) {
  std::vector<bool> used_commands(stack.rows());
  used_commands.back() = true;
  int stack_size = stack.rows();
  for (int i = 1; i < stack_size; i++) {
    int row = stack_size - i;
    int node = stack(row, ArrayProps::kNodeIdx);
    int param1 = stack(row, ArrayProps::kOp1);
    int param2 = stack(row, ArrayProps::kOp2);
    if (used_commands[row] && node > Op::LOAD_C) {
      used_commands[param1] = true;
      if (AGraph::HasArityTwo(node)) {
        used_commands[param2] = true;
      }
    }
  }
  return used_commands;
}

Eigen::ArrayX3i SimplifyStack(const Eigen::ArrayX3i &stack) {
  std::vector<bool> used_command = GetUtilizedCommands(stack);
  std::map<int, int> reduced_param_map;
  int num_commands = 0;
  num_commands = std::accumulate(used_command.begin(), used_command.end(), 0);
  Eigen::ArrayX3i new_stack(num_commands, 3);

  for (int i = 0, j = 0; i < stack.rows(); ++i) {
    if (used_command[i]) {
      new_stack(j, ArrayProps::kNodeIdx) = stack(i, ArrayProps::kNodeIdx);
      if (AGraph::IsTerminal(new_stack(j, ArrayProps::kNodeIdx))) {
        new_stack(j, ArrayProps::kOp1) = stack(i, ArrayProps::kOp1);
        new_stack(j, ArrayProps::kOp2) = stack(i, ArrayProps::kOp2);
      } else {
        new_stack(j, ArrayProps::kOp1) = reduced_param_map[stack(i, ArrayProps::kOp1)];
        if (AGraph::HasArityTwo(new_stack(j, ArrayProps::kNodeIdx))) {
          new_stack(j, ArrayProps::kOp2) = reduced_param_map[stack(i, ArrayProps::kOp2)];
        } else {
          new_stack(j, ArrayProps::kOp2) = new_stack(j, ArrayProps::kOp2);
        }
      }
      reduced_param_map[i] = j;
      ++j;
    }
  }
  return new_stack;
}

namespace {

Eigen::ArrayXXd reverse_eval(const std::pair<int, int> &deriv_shape,
                             const int deriv_wrt_node,
                             const std::vector<Eigen::ArrayXXd> &forward_eval,
                             const Eigen::ArrayX3i &stack) {
  int num_samples = deriv_shape.first;
  int num_features = deriv_shape.second;
  int stack_depth = stack.rows();

  Eigen::ArrayXXd derivative = Eigen::ArrayXXd::Zero(num_samples, num_features);
  std::vector<Eigen::ArrayXXd> reverse_eval(stack_depth);
  for (int row = 0; row < stack_depth; row++) {
      reverse_eval[row] = Eigen::ArrayXd::Zero(num_samples);
  }

  reverse_eval[stack_depth-1] = Eigen::ArrayXd::Ones(num_samples);
  for (int i = stack_depth - 1; i >= 0; i--) {
    int node = stack(i, ArrayProps::kNodeIdx);
    int param1 = stack(i, ArrayProps::kOp1);
    int param2 = stack(i, ArrayProps::kOp2);
    if (node == deriv_wrt_node) {
      derivative.col(param1) += reverse_eval[i];
    } else {
      ReverseEvalFunction(node, i, param1, param2, forward_eval, reverse_eval);
    }
  }
  return derivative;
}

Eigen::ArrayXXd reverse_eval_with_mask(const std::pair<int, int> &deriv_shape,
                                       const int deriv_wrt_node,
                                       const std::vector<Eigen::ArrayXXd> &forward_eval,
                                       const Eigen::ArrayX3i &stack,
                                       const std::vector<bool> &mask) {
  int num_samples = deriv_shape.first;
  int num_features = deriv_shape.second;
  int stack_depth = stack.rows();

  Eigen::ArrayXXd derivative = Eigen::ArrayXXd::Zero(num_samples, num_features);
  std::vector<Eigen::ArrayXXd> reverse_eval(stack_depth);
  for (int row = 0; row < stack_depth; row++) {
    if (mask[row]) {
      reverse_eval[row] = Eigen::ArrayXd::Zero(num_samples);
    }
  }

  reverse_eval[stack_depth-1] = Eigen::ArrayXd::Ones(num_samples);
  for (int i = stack_depth - 1; i >= 0; i--) {
    if (mask[i]) {
      int node = stack(i, ArrayProps::kNodeIdx);
      int param1 = stack(i, ArrayProps::kOp1);
      int param2 = stack(i, ArrayProps::kOp2);
      if (node == deriv_wrt_node) {
        derivative.col(param1) += reverse_eval[i];
      } else {
        ReverseEvalFunction(node, i, param1, param2, forward_eval, reverse_eval);
      }
    }
  }
  return derivative;
}

std::vector<Eigen::ArrayXXd> forward_eval(
    const Eigen::ArrayX3i &stack,
    const Eigen::ArrayXXd &x,
    const Eigen::VectorXd &constants) {
  std::vector<Eigen::ArrayXXd> _forward_eval(stack.rows());

  for (int i = 0; i < stack.rows(); ++i) {
    int node = stack(i, ArrayProps::kNodeIdx);
    int op1 = stack(i, ArrayProps::kOp1);
    int op2 = stack(i, ArrayProps::kOp2);
    _forward_eval[i] = ForwardEvalFunction(
      node, op1, op2, x, constants, _forward_eval);
  }
  return _forward_eval;
}

std::vector<Eigen::ArrayXXd> forward_eval_with_mask(
    const Eigen::ArrayX3i &stack,
    const Eigen::ArrayXXd &x,
    const Eigen::VectorXd &constants,
    const std::vector<bool> &mask) {
  std::vector<Eigen::ArrayXXd> _forward_eval(stack.rows());

  for (int i = 0; i < stack.rows(); ++i) {
    if (mask[i]) {
      int node = stack(i, ArrayProps::kNodeIdx);
      int op1 = stack(i, ArrayProps::kOp1);
      int op2 = stack(i, ArrayProps::kOp2);
      _forward_eval[i] = ForwardEvalFunction(
        node, op1, op2, x, constants, _forward_eval);
    }
  }
  return _forward_eval;
}

EvalAndDerivative evaluate_with_derivative(
    const Eigen::ArrayX3i &stack,
    const Eigen::ArrayXXd &x,
    const Eigen::VectorXd &constants,
    const bool param_x_or_c) {
  std::vector<Eigen::ArrayXXd> _forward_eval = forward_eval(
      stack, x, constants);

  std::pair<int, int> deriv_shape;
  int deriv_wrt_node;
  if (param_x_or_c) {  // true = x
    deriv_shape = std::make_pair(x.rows(), x.cols());
    deriv_wrt_node = Op::LOAD_X;
  } else {  // false = c
    deriv_shape = std::make_pair(x.rows(), constants.size());
    deriv_wrt_node = Op::LOAD_C;
  }

  Eigen::ArrayXXd derivative = reverse_eval(
      deriv_shape, deriv_wrt_node, _forward_eval, stack);
  return std::make_pair(_forward_eval.back(), derivative);
}

EvalAndDerivative evaluate_with_derivative_and_mask(
    const Eigen::ArrayX3i &stack,
    const Eigen::ArrayXXd &x,
    const Eigen::VectorXd &constants,
    const std::vector<bool> &mask,
    const bool param_x_or_c) {
  std::vector<Eigen::ArrayXXd> forward_eval = forward_eval_with_mask(
      stack, x, constants, mask);

  std::pair<int, int> deriv_shape;
  int deriv_wrt_node;
  if (param_x_or_c) {  // true = x
    deriv_shape = std::make_pair(x.rows(), x.cols());
    deriv_wrt_node = Op::LOAD_X;
  } else {  // false = c
    deriv_shape = std::make_pair(x.rows(), constants.size());
    deriv_wrt_node = Op::LOAD_C;
  }

  Eigen::ArrayXXd derivative = reverse_eval_with_mask(
      deriv_shape, deriv_wrt_node, forward_eval, stack, mask);
  return std::make_pair(forward_eval.back(), derivative);
}
} // namespace (anonymous)
} // namespace backend
} // namespace bingo


#include <base/cgal_typedefs.h>
#include <IO/fileIO.h>
#include <util/helper.h>
#include <util/geometricOperations.h>

#include <processing/field.h>

#include <CGAL/point_generators_3.h>
#include <CGAL/Random.h>
#include <CGAL/boost/graph/convert_nef_polyhedron_to_polygon_mesh.h>
#include <CGAL/boost/graph/graph_traits_Polyhedron_3.h>
#include <boost/foreach.hpp>
#include <CGAL/Polygon_mesh_processing/distance.h>
#include <CGAL/polygon_mesh_processing.h>
#include <CGAL/optimal_bounding_box.h>
#include <CGAL/Side_of_triangle_mesh.h>

#include <util/helper.h>

#include <random>

using namespace std;

int makeFieldOnDelaunayVertices(dataHolder& data, string mode){

    auto start = std::chrono::high_resolution_clock::now();

    cout << "\nMaking field..." << endl;
    cout << "\t-using mode: " << mode << endl;

    if(data.gt_poly.size_of_vertices() == 0){
        cout << "\nERROR: you need to load a ground truth polygon (with -g) for scanning it" << endl;
        return 1;
    }
    if(!data.gt_poly.is_closed()){
        cout << "\nERROR: ground truth is not closed" << endl;
        return 1;
    }

    // Initialize the point-in-polyhedron tester
    // Construct AABB tree with a KdTree
    SurfaceMesh smesh;
    CGAL::copy_face_graph(data.gt_poly, smesh);
    Tree tree(faces(smesh).first, faces(smesh).second, smesh);
    tree.accelerate_distance_queries();
    // Initialize the point-in-polyhedron tester
    Point p;
    double val;
    Delaunay::Finite_vertices_iterator aci;

    const Point_inside inside_tester(tree);

    for(aci = data.Dt.finite_vertices_begin(); aci != data.Dt.finite_vertices_end(); aci++){

        p = aci->point();

        if(inside_tester(p) == CGAL::ON_BOUNDED_SIDE){
            val=-1.0;
        }
        else if(inside_tester(p) == CGAL::ON_UNBOUNDED_SIDE){
            val=1.0;
        }
        else{
            val=0.0;
        }

        if(mode == "occ"){
            aci->info().field_value = val;
        }
        else if(mode == "sdf"){
            aci->info().field_value = sqrt(tree.squared_distance(p))*val;
        }
        else{
            cout << "\nERROR: not a valid field mode!" << endl;
            return 1;
        }
    }

    auto stop = chrono::high_resolution_clock::now();
    auto duration = chrono::duration_cast<std::chrono::seconds>(stop - start);
    cout << "\t-in "<<duration.count() << "s" << endl;

    return 0;
}