/**********************************************************************************************************************
This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
Authors:  Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
Licensed under Apache2 license (see LICENSE file in main directory)
**********************************************************************************************************************/

#include <memory>
#include <array>

#include <Eigen/Dense>
#include <gtest/gtest.h>

#include <ct/rbd/rbd.h>

#include "../../models/testIrb4600/RobCoGenTestIrb4600.h"
#include "ct/rbd/robot/Dynamics.h"
#include "ct/rbd/robot/Kinematics.h"

using namespace ct::rbd;

TEST(DynamicsTestIrb4600, forward_dynamics_test)
{
    std::shared_ptr<TestIrb4600::Kinematics> kyn(new TestIrb4600::Kinematics);

    TestIrb4600::Dynamics testdynamics(kyn);

    using control_vector_t = typename TestIrb4600::Dynamics::control_vector_t;
    using ForceVector_t = typename TestIrb4600::Dynamics::ForceVector_t;
    using RBDState_t = typename TestIrb4600::Dynamics::RBDState_t;
    using RBDAcceleration_t = typename TestIrb4600::Dynamics::RBDAcceleration_t;
    using JointState_t = typename TestIrb4600::Dynamics::JointState_t;
    using JointAcceleration_t = typename TestIrb4600::Dynamics::JointAcceleration_t;
    using ExtLinkForces_t = typename TestIrb4600::Dynamics::ExtLinkForces_t;
    using EE_in_contact_t = typename TestIrb4600::Dynamics::EE_in_contact_t;

    JointState_t irb_state;
    irb_state.setZero();

    control_vector_t torque_u = control_vector_t::Zero();
    ExtLinkForces_t ext_forces;
    ext_forces = ForceVector_t::Zero();

    JointAcceleration_t irb_xd;

    testdynamics.FixBaseForwardDynamics(irb_state, torque_u, ext_forces, irb_xd);

    JointAcceleration_t qdd;
    testdynamics.FixBaseID(irb_state, qdd, ext_forces, torque_u);

    // EE_in_contact_t ee_contact=true;

    // testdynamics.ProjectedForwardDynamics(ee_contact , irb_state ,torque_u, irb_xd );

    // testdynamics.ProjectedInverseDynamics(ee_contact , irb_state , irb_xd , torque_u);
}

int main(int argc, char **argv)
{
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_SWAR_FUNCTIONS_COMPLEX_SIMD_COMMON_COMPARATOR_HPP_INCLUDED
#define NT2_TOOLBOX_SWAR_FUNCTIONS_COMPLEX_SIMD_COMMON_COMPARATOR_HPP_INCLUDED

#include <nt2/toolbox/swar/functions/comparator.hpp>
#include <nt2/include/functions/min.hpp>
#include <nt2/include/functions/max.hpp>
#include <nt2/include/functions/simd/any.hpp>
#include <nt2/include/functions/is_not_equal_with_equal_nans.hpp>
#include <boost/fusion/include/vector.hpp>
#include <nt2/sdk/complex/meta/as_complex.hpp>
#include <nt2/sdk/complex/meta/as_real.hpp>
#include <nt2/include/functions/imag.hpp>
#include <nt2/include/functions/real.hpp>
#include <nt2/sdk/complex/meta/as_dry.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::comparator_, tag::cpu_,
                             (A0)(A1)(X),
                             ((simd_<complex_ <arithmetic_<A0> >,X>))
                             ((simd_<complex_ <arithmetic_<A0> >,X>))
                             ((scalar_<integer_<A1> >))
                             ((simd_<complex_ <arithmetic_<A0> >,X>))
                             ((simd_<complex_ <arithmetic_<A0> >,X>))
                            )
  {
    typedef bool result_type;
    inline result_type operator()(A0 const& a0,A0 const& a1,const A1 & a2, A0 & r0, A0 & r1) const
    {
      if (a2)
      {
        r1 =  nt2::min(a0, a1);
        r0 =  nt2::max(a0, a1);
      }
      else
      {
        r0 =  nt2::min(a0, a1);
        r1 =  nt2::max(a0, a1);
      }
      return nt2::any(is_not_equal_with_equal_nans(a0, r0));
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::comparator_, tag::cpu_,
                             (A0)(A1)(X),
                             ((simd_<complex_ < arithmetic_<A0> >,X>))
                             ((simd_<complex_ < arithmetic_<A0> >,X>))
                             ((scalar_<integer_<A1> >))
                            )
  {
    typedef boost::fusion::vector<A0,A0,bool>           result_type;
    inline result_type operator()(A0 const& a0,A0 const & a1,const A1 & a2) const
    {
      result_type res;
      boost::fusion::at_c<2>(res) = nt2::comparator(a0, a1, a2, boost::fusion::at_c<0>(res),  boost::fusion::at_c<1>(res));
      return res;
    }
  };
} }
#endif


#ifndef BART_SRC_FRAMEWORK_BUILDER_FRAMEWORK_BUILDER_HPP_
#define BART_SRC_FRAMEWORK_BUILDER_FRAMEWORK_BUILDER_HPP_

#include "framework/builder/framework_builder_i.hpp"

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

#include "framework/builder/framework_validator.hpp"
#include "instrumentation/port.hpp"
#include "system/system_helper.hpp"
#include "utility/has_description.h"

namespace bart::framework::builder {

namespace data_port {
struct BuilderStatus;
using StatusDataPort = instrumentation::Port<std::pair<std::string, utility::Color>, BuilderStatus>;
}

template <int dim>
class FrameworkBuilder : public data_port::StatusDataPort, public FrameworkBuilderI<dim> {
 public:
  // New using types from refactor
  using typename FrameworkBuilderI<dim>::AngularFluxIntegrator;
  using typename FrameworkBuilderI<dim>::CrossSections;
  using typename FrameworkBuilderI<dim>::DiffusionFormulation;
  using typename FrameworkBuilderI<dim>::DriftDiffusionFormulation;
  using typename FrameworkBuilderI<dim>::Domain;
  using typename FrameworkBuilderI<dim>::FiniteElement;
  using typename FrameworkBuilderI<dim>::FrameworkI;
  using typename FrameworkBuilderI<dim>::GroupSolution;
  using typename FrameworkBuilderI<dim>::GroupSolveIteration;
  using typename FrameworkBuilderI<dim>::Initializer;
  using typename FrameworkBuilderI<dim>::KEffectiveUpdater;
  using typename FrameworkBuilderI<dim>::MomentCalculator;
  using typename FrameworkBuilderI<dim>::MomentConvergenceChecker;
  using typename FrameworkBuilderI<dim>::MomentMapConvergenceChecker;
  using typename FrameworkBuilderI<dim>::OuterIteration;
  using typename FrameworkBuilderI<dim>::ParameterConvergenceChecker;
  using typename FrameworkBuilderI<dim>::QuadratureSet;
  using typename FrameworkBuilderI<dim>::Stamper;
  using typename FrameworkBuilderI<dim>::Subroutine;
  using typename FrameworkBuilderI<dim>::SubroutineName;
  using typename FrameworkBuilderI<dim>::SAAFFormulation;
  using typename FrameworkBuilderI<dim>::SphericalHarmonicMoments;
  using typename FrameworkBuilderI<dim>::SingleGroupSolver;
  using typename FrameworkBuilderI<dim>::System;
  using typename FrameworkBuilderI<dim>::Validator;

  using typename FrameworkBuilderI<dim>::UpdaterPointers;
  using typename FrameworkBuilderI<dim>::BoundaryConditionsUpdater;
  using typename FrameworkBuilderI<dim>::FissionSourceUpdater;
  using typename FrameworkBuilderI<dim>::FixedTermUpdater;
  using typename FrameworkBuilderI<dim>::ScatteringSourceUpdater;

  using typename FrameworkBuilderI<dim>::DiffusionFormulationImpl;
  using typename FrameworkBuilderI<dim>::MomentCalculatorImpl;
  using typename FrameworkBuilderI<dim>::InitializerName;

  using typename FrameworkBuilderI<dim>::ColorStatusPair;
  using typename FrameworkBuilderI<dim>::ColorStatusInstrument;
  using typename FrameworkBuilderI<dim>::ConvergenceInstrument;
  using typename FrameworkBuilderI<dim>::StatusInstrument;

  using typename FrameworkBuilderI<dim>::AngularFluxStorage;

  FrameworkBuilder(std::unique_ptr<Validator> validator_ptr);
  ~FrameworkBuilder() = default;

  [[nodiscard]] auto BuildAngularFluxIntegrator(
      const std::shared_ptr<QuadratureSet>) -> std::unique_ptr<AngularFluxIntegrator> override;

  [[nodiscard]] auto BuildDiffusionFormulation(
      const std::shared_ptr<FiniteElement>&,
      const std::shared_ptr<data::cross_sections::CrossSectionsI>&,
      const DiffusionFormulationImpl implementation = DiffusionFormulationImpl::kDefault)
  -> std::unique_ptr<DiffusionFormulation> override;

  [[nodiscard]] auto BuildDriftDiffusionFormulation(
      const std::shared_ptr<AngularFluxIntegrator>&,
      const std::shared_ptr<FiniteElement>&,
      const std::shared_ptr<data::cross_sections::CrossSectionsI>&) -> std::unique_ptr<DriftDiffusionFormulation> override;

  [[nodiscard]] auto BuildDomain(const FrameworkParameters::DomainSize,
                                 const FrameworkParameters::NumberOfCells,
                                 const std::shared_ptr<FiniteElement>&,
                                 const std::string material_mapping) -> std::unique_ptr<Domain> override;
  [[nodiscard]] auto BuildFiniteElement(
      const problem::CellFiniteElementType finite_element_type,
      const problem::DiscretizationType discretization_type,
      const FrameworkParameters::PolynomialDegree polynomial_degree) -> std::unique_ptr<FiniteElement> override;
  [[nodiscard]] auto BuildGroupSolution(const int n_angles) -> std::unique_ptr<GroupSolution> override;
  [[nodiscard]] auto BuildGroupSolveIteration(
      std::unique_ptr<SingleGroupSolver>,
      std::unique_ptr<MomentConvergenceChecker>,
      std::unique_ptr<MomentCalculator>,
      const std::shared_ptr<GroupSolution>&,
      const UpdaterPointers& updater_ptrs,
      std::unique_ptr<MomentMapConvergenceChecker>) -> std::unique_ptr<GroupSolveIteration> override;
  [[nodiscard]] auto BuildInitializer(const std::shared_ptr<FixedTermUpdater>&,
                                      const int total_groups,
                                      const int total_angles) -> std::unique_ptr<Initializer> override;
  [[nodiscard]] auto BuildInitializer(const std::shared_ptr<FixedTermUpdater>&,
                                      const int total_groups,
                                      const int total_angles,
                                      const InitializerName) -> std::unique_ptr<Initializer> override;
  [[nodiscard]] auto BuildKEffectiveUpdater() -> std::unique_ptr<KEffectiveUpdater> override;
  [[nodiscard]] auto BuildKEffectiveUpdater(
      const std::shared_ptr<FiniteElement>&,
      const std::shared_ptr<CrossSections>&,
      const std::shared_ptr<Domain>&) -> std::unique_ptr<KEffectiveUpdater> override;
  [[nodiscard]] auto BuildMomentCalculator(
      MomentCalculatorImpl implementation = MomentCalculatorImpl::kScalarMoment)
  -> std::unique_ptr<MomentCalculator> override;
  [[nodiscard]] auto BuildMomentCalculator(
      std::shared_ptr<QuadratureSet>,
      MomentCalculatorImpl implementation = MomentCalculatorImpl::kZerothMomentOnly)
  -> std::unique_ptr<MomentCalculator> override;
  [[nodiscard]] auto  BuildMomentConvergenceChecker(
      double max_delta,
      int max_iterations) -> std::unique_ptr<MomentConvergenceChecker> override;
  [[nodiscard]] auto BuildMomentMapConvergenceChecker(
      double max_delta,
      int max_iterations) -> std::unique_ptr<MomentMapConvergenceChecker> override;
  [[nodiscard]] auto BuildOuterIteration(
      std::unique_ptr<GroupSolveIteration>,
      std::unique_ptr<ParameterConvergenceChecker>,
      const std::string& output_filename_base) -> std::unique_ptr<OuterIteration> override;
  [[nodiscard]] auto BuildOuterIteration(
      std::unique_ptr<GroupSolveIteration>,
      std::unique_ptr<ParameterConvergenceChecker>,
      std::unique_ptr<KEffectiveUpdater>,
      const std::shared_ptr<FissionSourceUpdater>&,
      const std::string& output_filename_base) -> std::unique_ptr<OuterIteration> override;
  [[nodiscard]] auto BuildParameterConvergenceChecker(
      double max_delta,
      int max_iterations) -> std::unique_ptr<ParameterConvergenceChecker> override;
  [[nodiscard]] auto BuildQuadratureSet(
      const problem::AngularQuadType,
      const FrameworkParameters::AngularQuadratureOrder) -> std::shared_ptr<QuadratureSet> override;
  [[nodiscard]] auto BuildSAAFFormulation(
      const std::shared_ptr<FiniteElement>&,
      const std::shared_ptr<data::cross_sections::CrossSectionsI>&,
      const std::shared_ptr<QuadratureSet>&,
      const formulation::SAAFFormulationImpl implementation = formulation::SAAFFormulationImpl::kDefault)
  -> std::unique_ptr<SAAFFormulation> override;
  [[nodiscard]] auto BuildSingleGroupSolver(
      const int max_iterations,
      const double convergence_tolerance) -> std::unique_ptr<SingleGroupSolver> override;
  [[nodiscard]] auto BuildStamper(const std::shared_ptr<Domain>&) -> std::unique_ptr<Stamper> override;
  [[nodiscard]] auto BuildSubroutine(std::unique_ptr<FrameworkI>,
                                     const SubroutineName) -> std::unique_ptr<Subroutine> override;
  [[nodiscard]] auto BuildSystem(const int n_groups,
                                 const int n_angles,
                                 const Domain& domain,
                                 const std::size_t solution_size,
                                 bool is_eigenvalue_problem,
                                 bool need_rhs_boundary_condition) -> std::unique_ptr<System> override;

  [[nodiscard]] auto BuildUpdaterPointers(std::unique_ptr<DiffusionFormulation>,
                                    std::unique_ptr<DriftDiffusionFormulation>,
                                    std::shared_ptr<Stamper>,
                                    std::shared_ptr<AngularFluxIntegrator>,
                                    std::shared_ptr<SphericalHarmonicMoments>,
                                    AngularFluxStorage&,
                                    const std::map<problem::Boundary, bool>&) -> UpdaterPointers override;
  [[nodiscard]] auto BuildUpdaterPointers(
      std::unique_ptr<DiffusionFormulation>,
      std::unique_ptr<Stamper>,
      const std::map<problem::Boundary, bool>& reflective_boundaries) -> UpdaterPointers override;
  [[nodiscard]] auto BuildUpdaterPointers(std::unique_ptr<SAAFFormulation>,
                                          std::unique_ptr<Stamper>,
                                          const std::shared_ptr<QuadratureSet>&) -> UpdaterPointers override;
  [[nodiscard]] auto BuildUpdaterPointers(std::unique_ptr<SAAFFormulation>,
                                          std::unique_ptr<Stamper>,
                                          const std::shared_ptr<QuadratureSet>&,
                                          const std::map<problem::Boundary, bool>& reflective_boundaries,
                                          const AngularFluxStorage&) -> UpdaterPointers override;

  // Instrumentation
  auto set_color_status_instrument_ptr(
      const std::shared_ptr<ColorStatusInstrument>& to_set) -> FrameworkBuilderI<dim>& override {
    AssertThrow(to_set != nullptr, dealii::ExcMessage("Error setting instrument, pointer is null"))
    color_status_instrument_ptr_ = to_set;
    return *this;
  };
  auto set_convergence_status_instrument_ptr(
      const std::shared_ptr<ConvergenceInstrument>& to_set) -> FrameworkBuilderI<dim>& override {
    AssertThrow(to_set != nullptr, dealii::ExcMessage("Error setting instrument, pointer is null"))
    convergence_status_instrument_ptr_ = to_set;
    return *this;
  };
  auto set_status_instrument_ptr(const std::shared_ptr<StatusInstrument>& to_set) -> FrameworkBuilderI<dim>& override{
    AssertThrow(to_set != nullptr, dealii::ExcMessage("Error setting instrument, pointer is null"))
    status_instrument_ptr_ = to_set;
    return *this;
  };

  auto color_status_instrument_ptr() const -> std::shared_ptr<ColorStatusInstrument> override {
    return color_status_instrument_ptr_; };
  auto convergence_status_instrument_ptr() const -> std::shared_ptr<ConvergenceInstrument> override {
    return convergence_status_instrument_ptr_; };
  auto status_instrument_ptr() const -> std::shared_ptr<StatusInstrument> override {
    return status_instrument_ptr_; };

  auto validator_ptr() -> Validator* override { return validator_ptr_.get(); };

 private:
  void ReportBuildingComponant(std::string componant) {
    if (!build_report_closed_) {
      data_port::StatusDataPort::Expose({"\n", utility::Color::kReset});
    }
    data_port::StatusDataPort::Expose(
        {"Building " + componant + ": ", utility::Color::kReset});
    build_report_closed_ = false;
  }

  void ReportBuildSuccess(std::string description = "") {
    data_port::StatusDataPort::Expose(
        {"Built " + description + "\n", utility::Color::kGreen});
    build_report_closed_ = true;
  }

  void Report(std::string to_report, utility::Color color) {
    data_port::StatusDataPort::Expose({to_report, color});
  }

  void ReportBuildError(std::string description = "") {
    data_port::StatusDataPort::Expose(
        {"Error: " + description + "\n", utility::Color::kRed});
    build_report_closed_ = true;
  }

  void Validate() const;

  template <typename T>
  inline std::shared_ptr<T> Shared(std::unique_ptr<T> to_convert_ptr) {
    return to_convert_ptr;
  }
  std::unique_ptr<Validator> validator_ptr_{ nullptr };
  std::shared_ptr<StatusInstrument> status_instrument_ptr_{nullptr};
  std::shared_ptr<ColorStatusInstrument> color_status_instrument_ptr_{nullptr};
  std::shared_ptr<ConvergenceInstrument> convergence_status_instrument_ptr_{ nullptr };
  const system::SystemHelper<dim> system_helper_;
  bool build_report_closed_ = true;
};

} // namespace bart::framework::builder

#endif //BART_SRC_FRAMEWORK_BUILDER_FRAMEWORK_BUILDER_HPP_


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_REDUCTION_REDUCTION_HPP_INCLUDED
#define BOOST_SIMD_REDUCTION_REDUCTION_HPP_INCLUDED

//<include> please don't modify between these tags
#include <boost/simd/reduction/include/functions/hmsb.hpp>
#include <boost/simd/reduction/include/functions/sum.hpp>
#include <boost/simd/reduction/include/functions/second.hpp>
#include <boost/simd/reduction/include/functions/prod.hpp>
#include <boost/simd/reduction/include/functions/posmin.hpp>
#include <boost/simd/reduction/include/functions/posmax.hpp>
#include <boost/simd/reduction/include/functions/inbtrue.hpp>
#include <boost/simd/reduction/include/functions/minimum.hpp>
#include <boost/simd/reduction/include/functions/maximum.hpp>
#include <boost/simd/reduction/include/functions/first.hpp>
#include <boost/simd/reduction/include/functions/dot.hpp>
#include <boost/simd/reduction/include/functions/all.hpp>
#include <boost/simd/reduction/include/functions/any.hpp>
//<\include>

#endif


#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <control_toolbox/pid.h>
#include <realtime_tools/realtime_buffer.h>

#include <pluginlib/class_list_macros.h>
#include <std_msgs/Float64MultiArray.h>
#include <angles/angles.h>
#include <geometry_msgs/WrenchStamped.h>

#include <urdf/model.h>

#include <kdl/tree.hpp>
#include <kdl/kdl.hpp>
#include <kdl/chain.hpp>
#include <kdl/chaindynparam.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainiksolvervel_pinv.hpp>
#include <kdl/chainiksolverpos_nr_jl.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <tf_conversions/tf_kdl.h>

#include <boost/scoped_ptr.hpp>

#define PI 3.141592
#define D2R PI/180.0
#define R2D 180.0/PI
#define JointMax 6
#define SaveDataMax 7

namespace arm_controllers{

	class AdaptiveImpedanceController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
	{
		public:
		~AdaptiveImpedanceController() {sub_q_cmd_.shutdown(); sub_forcetorque_sensor_.shutdown();}

		bool init(hardware_interface::EffortJointInterface* hw, ros::NodeHandle &n)
  		{
			// List of controlled joints
    		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;
			}

			// urdf
			urdf::Model urdf;
			if (!urdf.initParam("elfin/robot_description"))
			{
				ROS_ERROR("Failed to parse urdf file");
            	return false;
			}

			// joint handle
			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);
			}

			// kdl parser
			if (!kdl_parser::treeFromUrdfModel(urdf, kdl_tree_)){
				ROS_ERROR("Failed to construct kdl tree");
				return false;
			}

			// 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;
			}

			gravity_ = KDL::Vector::Zero();
			gravity_(2) = -9.81;
			G_.resize(n_joints_);

			// inverse dynamics solver
			id_solver_.reset( new KDL::ChainDynParam(kdl_chain_, gravity_) );
			fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
			ik_vel_solver_.reset(new KDL::ChainIkSolverVel_pinv(kdl_chain_));
			//ik_pos_solver_.reset(new KDL::ChainIkSolverPos_NR_JL(kdl_chain_, fk_solver_, ik_vel_solver_));

			// command and state
			tau_cmd_.data = Eigen::VectorXd::Zero(n_joints_);
			tau_cmd_old_.data = Eigen::VectorXd::Zero(n_joints_);
			q_cmd_sp_.data = Eigen::VectorXd::Zero(n_joints_);
			q_cmd_.data = Eigen::VectorXd::Zero(n_joints_);
			q_cmd_old_.data = Eigen::VectorXd::Zero(n_joints_);
			qdot_cmd_.data = Eigen::VectorXd::Zero(n_joints_);
			qdot_cmd_old_.data = Eigen::VectorXd::Zero(n_joints_);
			qddot_cmd_.data = Eigen::VectorXd::Zero(n_joints_);
			q_cmd_end_.data = Eigen::VectorXd::Zero(n_joints_);

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

			for (size_t i = 0; i < 6; i++)
			{
				Xc_dot_(i) = 0.0;
				Xc_dot_old_(i) = 0.0;
				Xc_ddot_(i) = 0.0;
			}

			// gains
			Mbar_.resize(n_joints_);
			Mbar_dot_.resize(n_joints_);
			Ramda_.resize(n_joints_);
			Alpha_.resize(n_joints_);
			Omega_.resize(n_joints_);

			Xr_dot_ = 0.0;
			Xe_dot_ = 0.0;
			Xe_ddot_ = 0.0;
			Fd_ = 0.0;
			Fd_temp_ = 0.0;
			Fd_old_ = 0.0;
			Fe_ = 0.0;
			Fe_old_ = 0.0;
			M_ = 0.0;
			B_ = 0.0;
			del_B_ = 0.0;
			B_buffer_ = 0.0;
			PI_ = 0.0;
			PI_old_ = 0.0;

			filt_old_ = 0.0;
			filt_ = 0.0;
			tau_ = 1.0/(2*PI*9.0);

			f_cur_buffer_ = 0.0;

			experiment_mode_ = 0;

			std::vector<double> Mbar(n_joints_), Ramda(n_joints_), Alpha(n_joints_), Omega(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/adaptive_impedance_controller/joint" + si + "/tdc/mbar", Mbar[i]) )
				{
					Mbar_(i) = Mbar[i];
				}
				else
				{
					std::cout << "/elfin/adaptive_impedance_controller/joint" + si + "/tdc/mbar" << std::endl;
					ROS_ERROR("Cannot find tdc/mbar gain");
					return false;
				}

				if ( n.getParam("/elfin/adaptive_impedance_controller/joint" + si + "/tdc/r", Ramda[i]) )
				{
					Ramda_(i) = Ramda[i];
				}
				else
				{
					ROS_ERROR("Cannot find tdc/r gain");
					return false;
				}

				if ( n.getParam("/elfin/adaptive_impedance_controller/joint" + si + "/tdc/a", Alpha[i]) )
				{
					Alpha_(i) = Alpha[i];
				}
				else
				{
					ROS_ERROR("Cannot find tdc/a gain");
					return false;
				}

				if ( n.getParam("/elfin/adaptive_impedance_controller/joint" + si + "/tdc/w", Omega[i]) )
				{
					Omega_(i) = Omega[i];
				}
				else
				{
					ROS_ERROR("Cannot find tdc/w gain");
					return false;
				}
			}

			if (!n.getParam("/elfin/adaptive_impedance_controller/aic/fd", Fd_temp_))
			{
				ROS_ERROR("Cannot find aci/fd");
				return false;
			}

			if (!n.getParam("/elfin/adaptive_impedance_controller/aic/m", M_))
			{
				ROS_ERROR("Cannot find aci/m");
				return false;
			}

			if (!n.getParam("/elfin/adaptive_impedance_controller/aic/b", B_))
			{
				ROS_ERROR("Cannot find aci/b");
				return false;
			}

			if (!n.getParam("/elfin/adaptive_impedance_controller/mode", experiment_mode_))
			{
				ROS_ERROR("Cannot find mode");
				return false;
			}

			// command
			sub_q_cmd_ = n.subscribe("command", 1, &AdaptiveImpedanceController::commandCB, this);
			sub_forcetorque_sensor_ = n.subscribe<geometry_msgs::WrenchStamped>("/elfin/elfin/ft_sensor_topic", 1, &AdaptiveImpedanceController::updateFTsensor, this);

			pub_SaveData_ = n.advertise<std_msgs::Float64MultiArray>("SaveData", 1000);

   			return true;
  		}

		void starting(const ros::Time& time)
		{
			// get joint positions
			for(size_t i=0; i<n_joints_; i++)
			{
				ROS_INFO("JOINT %d", (int)i);
				q_(i) = joints_[i].getPosition();
				q_init_(i) = q_(i);
				qdot_(i) = joints_[i].getVelocity();
			}

			time_ = 0.0;
			total_time_ = 0.0;

			ROS_INFO("Starting Adaptive Impedance Controller");
		}

		void commandCB(const std_msgs::Float64MultiArrayConstPtr &msg)
		{
			if(msg->data.size()!=n_joints_)
			{
				ROS_ERROR_STREAM("Dimension of command (" << msg->data.size() << ") does not match number of joints (" << n_joints_ << ")! Not executing!");
				return;
			}

    		for (unsigned int i = 0; i<n_joints_; i++)
    			q_cmd_sp_(i) = msg->data[i];
		}

		//void updateFTsensor(const geometry_msgs::WrenchStamped::ConstPtr &msg)
		void updateFTsensor(const geometry_msgs::WrenchStamped::ConstPtr &msg)
		{
			// Convert Wrench msg to KDL wrench
			geometry_msgs::Wrench f_meas = msg->wrench;

			f_cur_[0] = f_meas.force.x;
			f_cur_buffer_ = f_meas.force.y;
			f_cur_[2] = f_meas.force.z;
			f_cur_[3] = f_meas.torque.x;
			f_cur_[4] = f_meas.torque.y;
			f_cur_[5] = f_meas.torque.z;

			if (experiment_mode_ == 1 || experiment_mode_ == 2)
				f_cur_[1] = first_order_lowpass_filter();
		}

		// load gain is not permitted during controller loading?
		void loadGainCB()
		{

		}

  		void update(const ros::Time& time, const ros::Duration& period)
  		{
			// simple trajectory interpolation from joint command setpoint
			dt_ = period.toSec();

			if(total_time_ < 5.0)
			{
				task_init();
			}
			else if(total_time_ >= 5.0 && total_time_ < 6.0)
			{
				task_via();
			}
			else if(total_time_ >= 6.0 && total_time_ < 16.0)
			{
				task_ready();
			}
			else if (total_time_ >= 16.0 && total_time_ < 17.0)
			{
				task_via();
			}
			else if (total_time_ >= 17.0 && total_time_ < 27.0)
			{
				task_freespace();
			}
			else if (total_time_ >= 27.0 && total_time_ < 28.0)
			{
				task_via();
			}
			else if (total_time_ >= 28.0 && total_time_ < 48.0)
			{
				task_contactspace();
			}
			else if (total_time_ >= 48.0 && total_time_ < 49.0)
			{
				task_via();
			}
			else if (total_time_ >= 49.0 && total_time_ < 59.0)
			{
				task_homming();
			}

			// get joint states
			for (size_t i=0; i<n_joints_; i++)
			{
				q_(i) = joints_[i].getPosition();
				qdot_(i) = joints_[i].getVelocity();
			}

			qddot_.data = (qdot_.data - qdot_old_.data) / dt_;

			// error
			KDL::JntArray q_error;
			KDL::JntArray q_error_dot;
			KDL::JntArray ded;
			KDL::JntArray tde;
			KDL::JntArray s;

			q_error.data = Eigen::VectorXd::Zero(n_joints_);
			q_error_dot.data = Eigen::VectorXd::Zero(n_joints_);
			ded.data = Eigen::VectorXd::Zero(n_joints_);
			tde.data = Eigen::VectorXd::Zero(n_joints_);
			s.data = Eigen::VectorXd::Zero(n_joints_);

			double db = 0.001;

			q_error.data = q_cmd_.data - q_.data;
			q_error_dot.data = qdot_cmd_.data - qdot_.data;

			for(size_t i=0; i<n_joints_; i++)
			{
				s(i) = q_error_dot(i) + Ramda_(i)*q_error(i);

				if(db < Mbar_(i) && Mbar_(i) > Ramda_(i)/Alpha_(i))
				{
					Mbar_dot_(i) = Alpha_(i)*s(i)*s(i) - Alpha_(i)*Omega_(i)*Mbar_(i);
				}

				if(Mbar_(i) < db)
				{
					Mbar_(i) = db;
				}

				if(Mbar_(i) > Ramda_(i)/Alpha_(i))
				{
					Mbar_(i) = Ramda_(i)/Alpha_(i) - db;
				}

				Mbar_(i) = Mbar_(i) + dt_*Mbar_dot_(i);
			}

			// torque command
			for(size_t i=0; i<n_joints_; i++)
			{
				ded(i) = qddot_cmd_(i) + 2.0 * Ramda_(i)*q_error(i) + Ramda_(i)*Ramda_(i)*q_error_dot(i);
				tde(i) = tau_cmd_old_(i) - Mbar_(i)*qddot_(i);
				tau_cmd_(i) = Mbar_(i)*ded(i) + tde(i);
			}

			for(size_t i=0; i<n_joints_; i++)
			{
				joints_[i].setCommand(tau_cmd_(i));
			}

			tau_cmd_old_.data = tau_cmd_.data;
			qdot_old_.data = qdot_.data;

			KDL::Frame Xdes;

			fk_solver_->JntToCart(q_cmd_, Xdes);

			SaveData_[0] = total_time_;
			SaveData_[1] = 0.122;
			SaveData_[2] = Xdes.p(2);
			SaveData_[3] = Fd_;
			SaveData_[4] = f_cur_(1);
			SaveData_[5] = f_cur_buffer_;
			SaveData_[6] = B_buffer_;

			msg_SaveData_.data.clear();

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

			pub_SaveData_.publish(msg_SaveData_);

			time_ = time_ + dt_;
			total_time_ = total_time_ + dt_;
  		}

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

		void task_via()
		{
			time_ = 0.0;

			for (size_t i = 0; i < n_joints_; i++)
			{
				q_init_(i) = joints_[i].getPosition();
				q_cmd_(i) = q_cmd_end_(i);
				qdot_cmd_(i) = 0.0;
				qddot_cmd_(i) = 0.0;
			}
		}

		void task_init()
		{
			for (size_t i=0; i<n_joints_; i++)
			{
				q_cmd_(i) = 0.0;
				qdot_cmd_(i) = 0.0;
				qddot_cmd_(i) = 0.0;
			}

			q_cmd_end_.data = q_cmd_.data;
		}

		void task_ready()
		{
			for (size_t i=0; i<n_joints_; i++)
			{
				if (i == 2 || i == 4)
				{
					q_cmd_(i) = trajectory_generator_pos(q_init_(i), PI/2.0, 10.0);
					qdot_cmd_(i) = trajectory_generator_vel(q_init_(i), PI/2.0, 10.0);
					qddot_cmd_(i) = trajectory_generator_acc(q_init_(i), PI/2.0, 10.0);
				}
				else
				{
					q_cmd_(i) = q_init_(i);
					qdot_cmd_(i) = 0.0;
					qddot_cmd_(i) = 0.0;
				}
			}

			q_cmd_end_.data = q_cmd_.data;
		}

		void task_freespace()
		{
			KDL::Frame start;
			KDL::Twist target_vel;
			KDL::JntArray cart_cmd;

			cart_cmd.data = Eigen::VectorXd::Zero(3);

			fk_solver_->JntToCart(q_init_, start);

			for (size_t i=0; i<6; i++)
			{
				if(i == 2)
				{
					target_vel(i) = trajectory_generator_vel(start.p(i), 0.122, 10.0);
				}
				else
				{
					target_vel(i) = 0.0;
				}
			}

			ik_vel_solver_->CartToJnt(q_cmd_, target_vel, qdot_cmd_);

			for (size_t i=0; i<n_joints_; i++)
			{
				q_cmd_(i) = q_cmd_(i) + qdot_cmd_(i)*dt_;
				qddot_cmd_(i) = (qdot_cmd_(i) - qdot_cmd_old_(i))/dt_;
				qdot_cmd_old_(i) = qdot_cmd_(i);
			}

			q_cmd_end_.data = q_cmd_.data;
		}

		void task_contactspace()
		{
			KDL::Frame start;

			if (experiment_mode_ == 0 || experiment_mode_ == 1)
				Fd_ = Fd_temp_;
			else if (experiment_mode_ == 2)
				Fd_ = Fd_temp_ * 1 / 5 * sin(2 * PI * 0.2 * total_time_) + Fd_temp_;

				fk_solver_->JntToCart(q_init_, start);

			for (size_t i = 0; i < 6; i++)
			{
				if (i == 0)
				{
					Xc_dot_(i) = trajectory_generator_vel(start.p(i), 0.5, 20.0);
				}
				else if (i == 2)
				{
					Fe_ = f_cur_[1];
					PI_ = PI_old_ + dt_ * (Fd_old_ - Fe_old_) / B_;
					B_buffer_ = B_ + del_B_;
					Xc_ddot_(i) = 1/M_*((Fe_ - Fd_) - (B_ + del_B_)*Xc_dot_old_(i));
					Xc_dot_(i) = Xc_dot_old_(i) + Xc_ddot_(i)*dt_;
					del_B_ = B_ / Xc_dot_(i) * PI_;
					Xc_dot_old_(i) = Xc_dot_(i);
					Fd_old_ = Fd_;
					Fe_old_ = Fe_;
					PI_old_ = PI_;
				}
				else
				{
					Xc_ddot_(i) = 0.0;
					Xc_dot_(i) = 0.0;
				}
			}

			ik_vel_solver_->CartToJnt(q_cmd_, Xc_dot_, qdot_cmd_);

			for (size_t i=0; i<n_joints_; i++)
			{
				q_cmd_(i) = q_cmd_(i) + qdot_cmd_(i)*dt_;
				qddot_cmd_(i) = (qdot_cmd_(i) - qdot_cmd_old_(i))/dt_;
				qdot_cmd_old_(i) = qdot_cmd_(i);
			}

			q_cmd_end_.data = q_cmd_.data;
		}

		void task_homming()
		{
			for (size_t i = 0; i < n_joints_; i++)
			{
				q_cmd_(i) = trajectory_generator_pos(q_init_(i), 0.0, 10.0);
				qdot_cmd_(i) = trajectory_generator_vel(q_init_(i), 0.0, 10.0);
				qddot_cmd_(i) = trajectory_generator_acc(q_init_(i), 0.0, 10.0);
			}

			q_cmd_end_.data = q_cmd_.data;
		}

		double trajectory_generator_pos(double dStart, double dEnd, double dDuration)
		{
			double dA0 = dStart;
			double dA3 = (20.0*dEnd - 20.0*dStart) / (2.0*dDuration*dDuration*dDuration);
			double dA4 = (30.0*dStart - 30*dEnd) / (2.0*dDuration*dDuration*dDuration*dDuration);
			double dA5 = (12.0*dEnd - 12.0*dStart) / (2.0*dDuration*dDuration*dDuration*dDuration*dDuration);

			return dA0 + dA3*time_*time_*time_ + dA4*time_*time_*time_*time_ + dA5*time_*time_*time_*time_*time_;
		}

		double trajectory_generator_vel(double dStart, double dEnd, double dDuration)
		{
			double dA0 = dStart;
			double dA3 = (20.0*dEnd - 20.0*dStart) / (2.0*dDuration*dDuration*dDuration);
			double dA4 = (30.0*dStart - 30*dEnd) / (2.0*dDuration*dDuration*dDuration*dDuration);
			double dA5 = (12.0*dEnd - 12.0*dStart) / (2.0*dDuration*dDuration*dDuration*dDuration*dDuration);

			return 3.0*dA3*time_*time_ + 4.0*dA4*time_*time_*time_ + 5.0*dA5*time_*time_*time_*time_;
		}

		double trajectory_generator_acc(double dStart, double dEnd, double dDuration)
		{
			double dA0 = dStart;
			double dA3 = (20.0*dEnd - 20.0*dStart) / (2.0*dDuration*dDuration*dDuration);
			double dA4 = (30.0*dStart - 30*dEnd) / (2.0*dDuration*dDuration*dDuration*dDuration);
			double dA5 = (12.0*dEnd - 12.0*dStart) / (2.0*dDuration*dDuration*dDuration*dDuration*dDuration);

			return 6.0*dA3*time_ + 12.0*dA4*time_*time_ + 20.0*dA5*time_*time_*time_;
		}

		double first_order_lowpass_filter()
		{
			filt_ = (tau_ * filt_old_ + dt_*f_cur_buffer_)/(tau_ + dt_);
			filt_old_ = filt_;

			return filt_;
		}

	private:
		// joint handles
		unsigned int n_joints_;
		std::vector<std::string> joint_names_;
  		std::vector<hardware_interface::JointHandle> joints_;
		std::vector<urdf::JointConstSharedPtr> joint_urdfs_;

		// kdl
		KDL::Tree 	kdl_tree_;
		KDL::Chain	kdl_chain_;
		boost::scoped_ptr<KDL::ChainDynParam> id_solver_;
		boost::scoped_ptr<KDL::ChainFkSolverPos> fk_solver_;
		boost::scoped_ptr<KDL::ChainIkSolverVel_pinv> ik_vel_solver_;
		boost::scoped_ptr<KDL::ChainIkSolverPos_NR_JL> ik_pos_solver_;
		KDL::JntArray G_;
		KDL::Vector gravity_;

		// tdc gain
		KDL::JntArray Mbar_, Mbar_dot_, Ramda_, Alpha_, Omega_;

		// cmd, state
		KDL::JntArray q_init_;
		KDL::JntArray tau_cmd_, tau_cmd_old_;
		KDL::JntArray q_cmd_, q_cmd_old_, qdot_cmd_, qdot_cmd_old_, qddot_cmd_;
		KDL::JntArray q_cmd_end_;
		KDL::JntArray q_cmd_sp_;
		KDL::JntArray q_, qdot_, qdot_old_, qddot_;
		KDL::Wrench f_cur_;
		double f_cur_buffer_;

		KDL::Twist Xc_dot_, Xc_dot_old_, Xc_ddot_;

		double Xr_dot_, Xe_dot_;
		double Xe_ddot_;

		double Fd_, Fd_temp_, Fd_old_, Fe_, Fe_old_;
		double M_, B_, del_B_, B_buffer_;
		double PI_, PI_old_;

		double dt_;
		double time_;
		double total_time_;

		double filt_old_;
		double filt_;
		double tau_;

		int experiment_mode_;

		// topic
		ros::Subscriber sub_q_cmd_;
		ros::Subscriber sub_forcetorque_sensor_;

		double SaveData_[SaveDataMax];

		ros::Publisher pub_SaveData_;

		std_msgs::Float64MultiArray msg_SaveData_;
	};
}

PLUGINLIB_EXPORT_CLASS(arm_controllers::AdaptiveImpedanceController, controller_interface::ControllerBase)


/**
 * Copyright (c) 2013 Jonan Cruz-Martin
 *
 * Distributed under the terms of the MIT license, see the accompanying
 * file COPYING or http://opensource.org/licenses/MIT.
 *
 * @file
 * @brief Class definitions for domains of triangles.
 */

#ifndef DOMAINS_TRIANGULAR_HPP
#define DOMAINS_TRIANGULAR_HPP

#include "types.hpp"
#include "../points/cartesian.hpp"
#include "../points/cylindrical.hpp"
#include "../points/polar.hpp"
#include "../points/spherical.hpp"

#include "../vertices/triangular.hpp"
#include "../cells/triangular.hpp"

#include <boost/python.hpp>
using namespace boost::python;

/**
 * Domain of lines in the cartesian 2D space.
 *
 * @since 0.1.0
 */
class TriangularCartesian2D_Domain {
	/**
	 * ViennaGrid domain object.
	 * @since 0.1.0
	 */
	TriangularCartesian2D_Domain_t    domain;
public:
	TriangularCartesian2D_Domain() {};

	/**
	 * Return the number of vertices contained in the domain.
	 *
	 * @return unsigned integer
	 *
	 * @since 0.1.0
	 */
	unsigned int num_vertices();

	/**
	 * Create a new vertex in the domain based on the given point.
	 *
	 * @param point Point that represents the vertex. The point object will be copied.
	 *
	 * @since 0.1.0
	 */
	void make_vertex(PointCartesian2D point);

	/**
	 * Return the vertex identified by the given index.
	 *
	 * @param index Numerical vertex ID as assigned by ViennaGrid on vertex creation.
	 *
	 * @since 0.1.0
	 */
	TriangularCartesian2D_Vertex get_vertex(unsigned int index);

	/**
	 * Return a Python list containing all the vertices of the domain.
	 *
	 * @return Python list
	 *
	 * @since 0.1.0
	 */
	list get_vertices();

	/**
	 * Return the number of cells contained in the segment.
	 *
	 * @return unsigned integer
	 *
	 * @since 0.1.0
	 */
	unsigned int num_cells();

	/**
	 * Create a new cell in the segment.
	 *
	 * @param vertex1
	 * @param vertex2
	 * @param vertex3
	 *
	 * @return The newly created cell.
	 *
	 * @since 0.1.0
	 */
	TriangularCartesian2D_Cell make_cell(TriangularCartesian2D_Vertex vertex1, TriangularCartesian2D_Vertex vertex2, TriangularCartesian2D_Vertex vertex3);

	/**
	 * Return a Python list containing all the cells contained in the segment.
	 *
	 * @return Python list
	 *
	 * @since 0.1.0
	 */
	list get_cells();

	/**
	 * Get a reference to the ViennaGrid domain object.
	 *
	 * @return Reference to the ViennaGrid domain object.
	 * @since 0.1.0
	 */
	TriangularCartesian2D_Domain_t & get_domain();
};

////////////////////////////
// Triangle, cartesian 3D //
////////////////////////////

/**
 * Domain of lines in the cartesian 3D space.
 *
 * @since 0.1.0
 */
class TriangularCartesian3D_Domain {
	/**
	 * ViennaGrid domain object.
	 * @since 0.1.0
	 */
	TriangularCartesian3D_Domain_t    domain;
public:
	TriangularCartesian3D_Domain() {};

	/**
	 * Return the number of vertices contained in the domain.
	 *
	 * @return unsigned integer
	 *
	 * @since 0.1.0
	 */
	unsigned int num_vertices();

	/**
	 * Create a new vertex in the domain based on the given point.
	 *
	 * @param point Point that represents the vertex. The point object will be copied.
	 *
	 * @since 0.1.0
	 */
	void make_vertex(PointCartesian3D point);

	/**
	 * Return the vertex identified by the given index.
	 *
	 * @param index Numerical vertex ID as assigned by ViennaGrid on vertex creation.
	 *
	 * @since 0.1.0
	 */
	TriangularCartesian3D_Vertex get_vertex(unsigned int index);

	/**
	 * Return a Python list containing all the vertices of the domain.
	 *
	 * @return Python list
	 *
	 * @since 0.1.0
	 */
	list get_vertices();

	/**
	 * Return the number of cells contained in the segment.
	 *
	 * @return unsigned integer
	 *
	 * @since 0.1.0
	 */
	unsigned int num_cells();

	/**
	 * Create a new cell in the segment.
	 *
	 * @param vertex1
	 * @param vertex2
	 * @param vertex3
	 *
	 * @return The newly created cell.
	 *
	 * @since 0.1.0
	 */
	TriangularCartesian3D_Cell make_cell(TriangularCartesian3D_Vertex vertex1, TriangularCartesian3D_Vertex vertex2, TriangularCartesian3D_Vertex vertex3);

	/**
	 * Return a Python list containing all the cells contained in the segment.
	 *
	 * @return Python list
	 *
	 * @since 0.1.0
	 */
	list get_cells();

	/**
	 * Get a reference to the ViennaGrid domain object.
	 *
	 * @return Reference to the ViennaGrid domain object.
	 * @since 0.1.0
	 */
	TriangularCartesian3D_Domain_t & get_domain();
};

////////////////////////////////
// Triangle, cylindrical (3D) //
////////////////////////////////

/**
 * Domain of lines in the cylindrical space.
 *
 * @since 0.1.0
 */
class TriangularCylindrical3D_Domain {
	/**
	 * ViennaGrid domain object.
	 * @since 0.1.0
	 */
	TriangularCylindrical3D_Domain_t    domain;
public:
	TriangularCylindrical3D_Domain() {};

	/**
	 * Return the number of vertices contained in the domain.
	 *
	 * @return unsigned integer
	 *
	 * @since 0.1.0
	 */
	unsigned int num_vertices();

	/**
	 * Create a new vertex in the domain based on the given point.
	 *
	 * @param point Point that represents the vertex. The point object will be copied.
	 *
	 * @since 0.1.0
	 */
	void make_vertex(PointCylindrical3D point);

	/**
	 * Return the vertex identified by the given index.
	 *
	 * @param index Numerical vertex ID as assigned by ViennaGrid on vertex creation.
	 *
	 * @since 0.1.0
	 */
	TriangularCylindrical3D_Vertex get_vertex(unsigned int index);

	/**
	 * Return a Python list containing all the vertices of the domain.
	 *
	 * @return Python list
	 *
	 * @since 0.1.0
	 */
	list get_vertices();

	/**
	 * Return the number of cells contained in the segment.
	 *
	 * @return unsigned integer
	 *
	 * @since 0.1.0
	 */
	unsigned int num_cells();

	/**
	 * Create a new cell in the segment.
	 *
	 * @param vertex1
	 * @param vertex2
	 * @param vertex3
	 *
	 * @return The newly created cell.
	 *
	 * @since 0.1.0
	 */
	TriangularCylindrical3D_Cell make_cell(TriangularCylindrical3D_Vertex vertex1, TriangularCylindrical3D_Vertex vertex2, TriangularCylindrical3D_Vertex vertex3);

	/**
	 * Return a Python list containing all the cells contained in the segment.
	 *
	 * @return Python list
	 *
	 * @since 0.1.0
	 */
	list get_cells();

	/**
	 * Get a reference to the ViennaGrid domain object.
	 *
	 * @return Reference to the ViennaGrid domain object.
	 * @since 0.1.0
	 */
	TriangularCylindrical3D_Domain_t & get_domain();
};

//////////////////////////
// Triangle, polar (2D) //
//////////////////////////

/**
 * Domain of lines in the polar space.
 *
 * @since 0.1.0
 */
class TriangularPolar2D_Domain {
	/**
	 * ViennaGrid domain object.
	 * @since 0.1.0
	 */
	TriangularPolar2D_Domain_t    domain;
public:
	TriangularPolar2D_Domain() {};

	/**
	 * Return the number of vertices contained in the domain.
	 *
	 * @return unsigned integer
	 *
	 * @since 0.1.0
	 */
	unsigned int num_vertices();

	/**
	 * Create a new vertex in the domain based on the given point.
	 *
	 * @param point Point that represents the vertex. The point object will be copied.
	 *
	 * @since 0.1.0
	 */
	void make_vertex(PointPolar2D point);

	/**
	 * Return the vertex identified by the given index.
	 *
	 * @param index Numerical vertex ID as assigned by ViennaGrid on vertex creation.
	 *
	 * @since 0.1.0
	 */
	TriangularPolar2D_Vertex get_vertex(unsigned int index);

	/**
	 * Return a Python list containing all the vertices of the domain.
	 *
	 * @return Python list
	 *
	 * @since 0.1.0
	 */
	list get_vertices();

	/**
	 * Return the number of cells contained in the segment.
	 *
	 * @return unsigned integer
	 *
	 * @since 0.1.0
	 */
	unsigned int num_cells();

	/**
	 * Create a new cell in the segment.
	 *
	 * @param vertex1
	 * @param vertex2
	 * @param vertex3
	 *
	 * @return The newly created cell.
	 *
	 * @since 0.1.0
	 */
	TriangularPolar2D_Cell make_cell(TriangularPolar2D_Vertex vertex1, TriangularPolar2D_Vertex vertex2, TriangularPolar2D_Vertex vertex3);

	/**
	 * Return a Python list containing all the cells contained in the segment.
	 *
	 * @return Python list
	 *
	 * @since 0.1.0
	 */
	list get_cells();

	/**
	 * Get a reference to the ViennaGrid domain object.
	 *
	 * @return Reference to the ViennaGrid domain object.
	 * @since 0.1.0
	 */
	TriangularPolar2D_Domain_t & get_domain();
};

//////////////////////////////
// Triangle, spherical (3D) //
//////////////////////////////

/**
 * Domain of lines in the spherical space.
 *
 * @since 0.1.0
 */
class TriangularSpherical3D_Domain {
	/**
	 * ViennaGrid domain object.
	 * @since 0.1.0
	 */
	TriangularSpherical3D_Domain_t    domain;
public:
	TriangularSpherical3D_Domain() {};

	/**
	 * Return the number of vertices contained in the domain.
	 *
	 * @return unsigned integer
	 *
	 * @since 0.1.0
	 */
	unsigned int num_vertices();

	/**
	 * Create a new vertex in the domain based on the given point.
	 *
	 * @param point Point that represents the vertex. The point object will be copied.
	 *
	 * @since 0.1.0
	 */
	void make_vertex(PointSpherical3D point);

	/**
	 * Return the vertex identified by the given index.
	 *
	 * @param index Numerical vertex ID as assigned by ViennaGrid on vertex creation.
	 *
	 * @since 0.1.0
	 */
	TriangularSpherical3D_Vertex get_vertex(unsigned int index);

	/**
	 * Return a Python list containing all the vertices of the domain.
	 *
	 * @return Python list
	 *
	 * @since 0.1.0
	 */
	list get_vertices();

	/**
	 * Return the number of cells contained in the segment.
	 *
	 * @return unsigned integer
	 *
	 * @since 0.1.0
	 */
	unsigned int num_cells();

	/**
	 * Create a new cell in the segment.
	 *
	 * @param vertex1
	 * @param vertex2
	 * @param vertex3
	 *
	 * @return The newly created cell.
	 *
	 * @since 0.1.0
	 */
	TriangularSpherical3D_Cell make_cell(TriangularSpherical3D_Vertex vertex1, TriangularSpherical3D_Vertex vertex2, TriangularSpherical3D_Vertex vertex3);

	/**
	 * Return a Python list containing all the cells contained in the segment.
	 *
	 * @return Python list
	 *
	 * @since 0.1.0
	 */
	list get_cells();

	/**
	 * Get a reference to the ViennaGrid domain object.
	 *
	 * @return Reference to the ViennaGrid domain object.
	 * @since 0.1.0
	 */
	TriangularSpherical3D_Domain_t & get_domain();
};

#endif /* end of include guard: DOMAINS_TRIANGULAR_HPP */


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE Basic0LogicalIf

#include <../test_runner.hpp>
#include <boost/test/unit_test.hpp>
#include <filereader.hpp>
#include <popart/builder.hpp>
#include <popart/inputshapeinfo.hpp>
#include <popart/op/add.hpp>
#include <popart/op/identity.hpp>
#include <popart/op/if.hpp>
#include <popart/op/l1.hpp>
#include <popart/tensorinfo.hpp>
#include <popart/tensornames.hpp>
#include <popart/tensors.hpp>

using namespace popart;

BOOST_AUTO_TEST_CASE(LogicalIf_basic0) {
  TensorInfo info{"FLOAT", std::vector<int64_t>{2, 2}};
  TensorInfo infoBool{"BOOL", std::vector<int64_t>{}};
  std::vector<TestTensor> inputs;
  std::vector<TestTensor> outputs;

  TestRunner runner;
  runner.patterns.enableInPlace(false);

  runner.buildModel([&inputs, &outputs, info, infoBool](Builder &builder) {
    auto aiOnnx       = builder.aiOnnxOpset9();
    auto in0          = builder.addInputTensor(info);
    auto in1          = builder.addInputTensor(info);
    auto in_condition = builder.addInputTensor(infoBool);

    auto &then_branch = [in0, in1](Builder &parent_builder) -> Builder & {
      Builder &builder = parent_builder.createSubgraphBuilder();
      auto aiOnnx      = builder.aiOnnxOpset9();
      builder.addInputTensorFromParentGraph(in0);
      builder.addInputTensorFromParentGraph(in1);
      auto out = aiOnnx.add({in0, in1});
      builder.addOutputTensor(out);
      return builder;
    }(builder);

    auto &else_branch = [in0, in1](Builder &parent_builder) -> Builder & {
      Builder &builder = parent_builder.createSubgraphBuilder();
      auto aiOnnx      = builder.aiOnnxOpset9();
      builder.addInputTensorFromParentGraph(in0);
      builder.addInputTensorFromParentGraph(in1);
      auto out = aiOnnx.sub({in0, in1});
      builder.addOutputTensor(out);
      return builder;
    }(builder);

    auto out =
        aiOnnx.logical_if({in_condition}, 1, else_branch, then_branch)[0];
    builder.addOutputTensor(out);

    inputs.push_back(
        TestTensor::create<float>(in0, {1, 2, 3, 4}, info.shape()));
    inputs.push_back(
        TestTensor::create<float>(in1, {2, 3, 4, 5}, info.shape()));
    inputs.push_back(TestTensor::create<bool>(in_condition, infoBool.shape()));
    outputs.push_back(TestTensor::create<float>(out, info.shape()));

    return out;
  });

  // Check true branch
  inputs.back().setData<char>({1});
  runner.checkResult(
      [](TestTensor &result) {
        auto data = result.getDataCopy<float>();
        std::vector<float> expected{3, 5, 7, 9};
        BOOST_CHECK(data == expected);
      },
      inputs,
      outputs);

  // Check false branch
  inputs.back().setData<char>({0});
  runner.checkResult(
      [](TestTensor &result) {
        auto data = result.getDataCopy<float>();
        std::vector<float> expected{-1, -1, -1, -1};
        BOOST_CHECK(data == expected);
      },
      inputs,
      outputs);
}

BOOST_AUTO_TEST_CASE(LogicalIf_scopes0) {
  TensorInfo info{"FLOAT", std::vector<int64_t>{2, 2}};
  TensorInfo infoBool{"BOOL", std::vector<int64_t>{}};
  std::vector<TestTensor> inputs;
  std::vector<TestTensor> outputs;

  TestRunner runner;
  runner.patterns.enableInPlace(false);

  runner.buildModel([&inputs, &outputs, info, infoBool](Builder &builder) {
    auto aiOnnx       = builder.aiOnnxOpset9();
    auto in0          = builder.addInputTensor(info);
    auto in1          = builder.addInputTensor(info);
    auto in_condition = builder.addInputTensor(infoBool);

    auto &then_branch = [in0, in1](Builder &parent_builder) -> Builder & {
      Builder &builder = parent_builder.createSubgraphBuilder();
      auto aiOnnx      = builder.aiOnnxOpset9();
      builder.addInputTensorFromParentGraph(in0);
      builder.addInputTensorFromParentGraph(in1);
      auto out = aiOnnx.add({in0, in1});
      builder.addOutputTensor(out);
      return builder;
    }(builder);

    auto &else_branch = [in0, in1](Builder &parent_builder) -> Builder & {
      Builder &builder = parent_builder.createSubgraphBuilder();
      auto aiOnnx      = builder.aiOnnxOpset9();
      builder.addInputTensorFromParentGraph(in0);
      builder.addInputTensorFromParentGraph(in1);
      auto out = aiOnnx.add({in0, in1});
      builder.addOutputTensor(out);
      return builder;
    }(builder);

    auto out =
        aiOnnx.logical_if({in_condition}, 1, else_branch, then_branch)[0];
    builder.addOutputTensor(out);

    inputs.push_back(
        TestTensor::create<float>(in0, {1, 2, 3, 4}, info.shape()));
    inputs.push_back(
        TestTensor::create<float>(in1, {2, 3, 4, 5}, info.shape()));
    inputs.push_back(TestTensor::create<bool>(in_condition, infoBool.shape()));
    outputs.push_back(TestTensor::create<float>(out, info.shape()));

    return out;
  });

  // Check true branch
  inputs.back().setData<char>({1});
  runner.checkResult(
      [](TestTensor &result) {
        auto data = result.getDataCopy<float>();
        std::vector<float> expected{3, 5, 7, 9};
        logging::debug("data: {}", data);
        logging::debug("expected: {}", expected);
        BOOST_CHECK(data == expected);
      },
      inputs,
      outputs);

  // Check false branch
  inputs.back().setData<char>({0});
  runner.checkResult(
      [](TestTensor &result) {
        auto data = result.getDataCopy<float>();
        std::vector<float> expected{3, 5, 7, 9};
        logging::debug("data: {}", data);
        logging::debug("expected: {}", expected);
        BOOST_CHECK(data == expected);
      },
      inputs,
      outputs);
}

BOOST_AUTO_TEST_CASE(LogicalIf_scopes1) {
  TensorInfo info{"FLOAT", std::vector<int64_t>{2, 2}};
  TensorInfo infoBool{"BOOL", std::vector<int64_t>{}};
  std::vector<TestTensor> inputs;
  std::vector<TestTensor> outputs;

  TestRunner runner;
  runner.patterns.enableInPlace(false);

  runner.buildModel([&inputs, &outputs, info, infoBool](Builder &builder) {
    auto aiOnnx       = builder.aiOnnxOpset9();
    auto in0          = builder.addInputTensor(info);
    auto in1          = builder.addInputTensor(info);
    auto in_condition = builder.addInputTensor(infoBool);

    auto &then_branch = [in0, in1](Builder &parent_builder) -> Builder & {
      Builder &builder = parent_builder.createSubgraphBuilder();
      auto aiOnnx      = builder.aiOnnxOpset9();
      builder.addInputTensorFromParentGraph(in0);
      builder.addInputTensorFromParentGraph(in1);
      auto out = aiOnnx.add({in0, in1});
      out      = aiOnnx.add({out, out});
      builder.addOutputTensor(out);
      return builder;
    }(builder);

    auto &else_branch = [in0, in1](Builder &parent_builder) -> Builder & {
      Builder &builder = parent_builder.createSubgraphBuilder();
      auto aiOnnx      = builder.aiOnnxOpset9();
      builder.addInputTensorFromParentGraph(in0);
      builder.addInputTensorFromParentGraph(in1);
      auto out = aiOnnx.add({in0, in1});
      builder.addOutputTensor(out);
      return builder;
    }(builder);

    auto out =
        aiOnnx.logical_if({in_condition}, 1, else_branch, then_branch)[0];
    builder.addOutputTensor(out);

    inputs.push_back(
        TestTensor::create<float>(in0, {1, 2, 3, 4}, info.shape()));
    inputs.push_back(
        TestTensor::create<float>(in1, {2, 3, 4, 5}, info.shape()));
    inputs.push_back(TestTensor::create<bool>(in_condition, infoBool.shape()));
    outputs.push_back(TestTensor::create<float>(out, info.shape()));

    return out;
  });

  // Check true branch
  inputs.back().setData<char>({1});
  runner.checkResult(
      [](TestTensor &result) {
        auto data = result.getDataCopy<float>();
        std::vector<float> expected{6, 10, 14, 18};
        logging::debug("data: {}", data);
        logging::debug("expected: {}", expected);
        BOOST_CHECK(data == expected);
      },
      inputs,
      outputs);

  // Check false branch
  inputs.back().setData<char>({0});
  runner.checkResult(
      [](TestTensor &result) {
        auto data = result.getDataCopy<float>();
        std::vector<float> expected{3, 5, 7, 9};
        logging::debug("data: {}", data);
        logging::debug("expected: {}", expected);
        BOOST_CHECK(data == expected);
      },
      inputs,
      outputs);
}

// Check that the generation of IfGradOp and backward graphs works
BOOST_AUTO_TEST_CASE(LogicalIf_train0) {
  TensorInfo info{"FLOAT", std::vector<int64_t>{2, 2}};
  TensorInfo infoBool{"BOOL", std::vector<int64_t>{}};
  std::vector<TestTensor> inputs;
  std::vector<TestTensor> outputs;

  float in0Data[4]        = {1, 2, 3, 4};
  ConstVoidData in0CVData = {in0Data, info};

  float in1Data[4]        = {2, 3, 4, 5};
  ConstVoidData in1CVData = {in1Data, info};

  TestRunner runner;
  runner.patterns.enableInPlace(false);
  runner.isTraining = true;

  TensorId in0;
  TensorId in1;
  TensorId out;

  runner.buildModel([&inputs,
                     &outputs,
                     &runner,
                     &in0CVData,
                     &in1CVData,
                     &in0,
                     &in1,
                     &out,
                     info,
                     infoBool](Builder &builder) {
    auto aiOnnx       = builder.aiOnnxOpset9();
    in0               = builder.addInitializedInputTensor(in0CVData);
    in1               = builder.addInitializedInputTensor(in1CVData);
    auto in_condition = builder.addInputTensor(infoBool);

    auto &then_branch = [in0, in1](Builder &parent_builder) -> Builder & {
      Builder &builder = parent_builder.createSubgraphBuilder();
      auto aiOnnx      = builder.aiOnnxOpset9();
      builder.addInputTensorFromParentGraph(in0);
      builder.addInputTensorFromParentGraph(in1);
      auto out = aiOnnx.add({in0, in1});
      builder.addOutputTensor(out);
      return builder;
    }(builder);

    auto &else_branch = [in0, in1](Builder &parent_builder) -> Builder & {
      Builder &builder = parent_builder.createSubgraphBuilder();
      auto aiOnnx      = builder.aiOnnxOpset9();
      builder.addInputTensorFromParentGraph(in0);
      builder.addInputTensorFromParentGraph(in1);
      auto out = aiOnnx.add({in0, in1});
      builder.addOutputTensor(out);
      return builder;
    }(builder);

    out = aiOnnx.logical_if({in_condition}, 1, else_branch, then_branch)[0];
    auto l1 =
        builder.aiGraphcoreOpset1().l1loss({out}, 0.1, ReductionType::Sum);

    runner.anchors.insert({getGradId(in0), AnchorReturnType("All")});
    runner.anchors.insert({getGradId(in1), AnchorReturnType("All")});

    inputs.push_back(TestTensor::create<bool>(in_condition, infoBool.shape()));
    outputs.push_back(TestTensor::create<float>(out, info.shape()));
    outputs.push_back(TestTensor::create<float>(getGradId(in0), info.shape()));
    outputs.push_back(TestTensor::create<float>(getGradId(in1), info.shape()));

    runner.loss = l1;

    return out;
  });

  // Reference was generated with the below python
  // >>> a = torch.tensor([[1, 2], [3, 4]], dtype=torch.float32,
  // requires_grad=True)
  // >>> b = torch.tensor([[2, 3], [4, 5]], dtype=torch.float32,
  // requires_grad=True)
  // >>> o = a + b
  // >>> o
  // tensor([[3., 5.],
  //         [7., 9.]], grad_fn=<AddBackward0>)
  // >>> o.backward(torch.ones(2, 2) * 0.1)
  // >>> a.grad
  // tensor([[0.1000, 0.1000],
  //         [0.1000, 0.1000]])
  // >>> b.grad
  // tensor([[0.1000, 0.1000],
  //         [0.1000, 0.1000]])
  auto resultChecker = [&in0, &in1, &out](std::vector<TestTensor> &results) {
    BOOST_CHECK_EQUAL(results.size(), 3);
    std::map<TensorId, std::vector<float>> expected{
        {out, {3, 5, 7, 9}},
        {getGradId(in0), {0.1, 0.1, 0.1, 0.1}},
        {getGradId(in1), {0.1, 0.1, 0.1, 0.1}}};

    for (auto &t : results) {
      auto data = t.getDataCopy<float>();
      auto e    = expected.at(t.id);
      BOOST_CHECK(data == e);
    }
  };

  // Check true branch
  inputs.back().setData<char>({1});
  runner.checkResults(resultChecker, inputs, outputs);

  // Check false branch
  inputs.back().setData<char>({0});
  runner.checkResults(resultChecker, inputs, outputs);
}

// Same as LogicalIf_train0, but then and else branchs are now different
BOOST_AUTO_TEST_CASE(LogicalIf_train1) {
  TensorInfo info{"FLOAT", std::vector<int64_t>{2, 2}};
  TensorInfo infoBool{"BOOL", std::vector<int64_t>{}};
  std::vector<TestTensor> inputs;
  std::vector<TestTensor> outputs;

  float in0Data[4]        = {1, 2, 3, 4};
  ConstVoidData in0CVData = {in0Data, info};

  float in1Data[4]        = {2, 3, 4, 5};
  ConstVoidData in1CVData = {in1Data, info};

  TestRunner runner;
  runner.opts.dotChecks.insert("Final");
  runner.opts.separateCallOpPdfs = false;
  runner.patterns.enableInPlace(false);
  runner.patterns.enableSubtractArg1GradOp(true);
  runner.isTraining = true;

  TensorId in0;
  TensorId in1;
  TensorId out;

  runner.buildModel([&inputs,
                     &outputs,
                     &runner,
                     &in0CVData,
                     &in1CVData,
                     &in0,
                     &in1,
                     &out,
                     &info,
                     infoBool](Builder &builder) {
    auto aiOnnx       = builder.aiOnnxOpset9();
    in0               = builder.addInitializedInputTensor(in0CVData);
    in1               = builder.addInitializedInputTensor(in1CVData);
    auto in_condition = builder.addInputTensor(infoBool);

    auto &then_branch = [in0, in1](Builder &parent_builder) -> Builder & {
      Builder &builder = parent_builder.createSubgraphBuilder();
      auto aiOnnx      = builder.aiOnnxOpset9();
      builder.addInputTensorFromParentGraph(in0);
      builder.addInputTensorFromParentGraph(in1);
      auto out = aiOnnx.add({in0, in1});
      builder.addOutputTensor(out);
      return builder;
    }(builder);

    auto &else_branch = [in0, in1](Builder &parent_builder) -> Builder & {
      Builder &builder = parent_builder.createSubgraphBuilder();
      auto aiOnnx      = builder.aiOnnxOpset9();
      builder.addInputTensorFromParentGraph(in0);
      builder.addInputTensorFromParentGraph(in1);
      auto out = aiOnnx.sub({in0, in1});
      builder.addOutputTensor(out);
      return builder;
    }(builder);

    out = aiOnnx.logical_if({in_condition}, 1, else_branch, then_branch)[0];
    auto l1 =
        builder.aiGraphcoreOpset1().l1loss({out}, 0.1, ReductionType::Sum);

    runner.anchors.insert({getGradId(in0), AnchorReturnType("All")});
    runner.anchors.insert({getGradId(in1), AnchorReturnType("All")});

    inputs.push_back(TestTensor::create<bool>(in_condition, infoBool.shape()));
    outputs.push_back(TestTensor::create<float>(out, info.shape()));
    outputs.push_back(TestTensor::create<float>(getGradId(in0), info.shape()));
    outputs.push_back(TestTensor::create<float>(getGradId(in1), info.shape()));

    runner.loss = l1;

    return out;
  });

  // Check true branch
  // Reference was generated with the below python
  // >>> a = torch.tensor([[1, 2], [3, 4]], dtype=torch.float32,
  // requires_grad=True)
  // >>> b = torch.tensor([[2, 3], [4, 5]], dtype=torch.float32,
  // requires_grad=True)
  // >>> o = a + b
  // >>> o
  // tensor([[3., 5.],
  //         [7., 9.]], grad_fn=<AddBackward0>)
  // >>> o.backward(torch.ones(2, 2) * 0.1)
  // >>> a.grad
  // tensor([[0.1000, 0.1000],
  //         [0.1000, 0.1000]])
  // >>> b.grad
  // tensor([[0.1000, 0.1000],
  //         [0.1000, 0.1000]])
  inputs.back().setData<char>({1});
  runner.checkResults(
      [&in0, &in1, &out](std::vector<TestTensor> &results) {
        BOOST_CHECK_EQUAL(results.size(), 3);
        std::map<TensorId, std::vector<float>> expected{
            {out, {3, 5, 7, 9}},
            {getGradId(in0), {0.1, 0.1, 0.1, 0.1}},
            {getGradId(in1), {0.1, 0.1, 0.1, 0.1}}};

        for (auto &t : results) {
          auto data = t.getDataCopy<float>();
          auto e    = expected.at(t.id);
          logging::debug("BOOST_CHECK({} == {});", data, e);
          BOOST_CHECK(data == e);
        }
      },
      inputs,
      outputs);

  // Check false branch
  // Reference was generated with the below python
  // >>> a = torch.tensor([[1, 2], [3, 4]], dtype=torch.float32,
  // requires_grad=True)
  // >>> b = torch.tensor([[2, 3], [4, 5]], dtype=torch.float32,
  // requires_grad=True)
  // >>> o = a - b
  // >>> o
  // tensor([[-1., -1.],
  //         [-1., -1.]], grad_fn=<SubBackward0>)
  // >>> o.backward(torch.ones(2, 2) * -0.1)
  // >>> a.grad
  // tensor([[-0.1000, -0.1000],
  //         [-0.1000, -0.1000]])
  // >>> b.grad
  // tensor([[0.1000, 0.1000],
  //         [0.1000, 0.1000]])
  inputs.back().setData<char>({0});
  runner.checkResults(
      [&in0, &in1, &out](std::vector<TestTensor> &results) {
        BOOST_CHECK_EQUAL(results.size(), 3);
        std::map<TensorId, std::vector<float>> expected{
            {out, {-1, -1, -1, -1}},
            {getGradId(in0), {-0.1, -0.1, -0.1, -0.1}},
            {getGradId(in1), {0.1, 0.1, 0.1, 0.1}}};

        for (auto &t : results) {
          auto data = t.getDataCopy<float>();
          auto e    = expected.at(t.id);
          logging::debug("Checking for t.id {}", t.id);
          logging::debug("BOOST_CHECK({} == {});", data, e);
          BOOST_CHECK(data == e);
        }
      },
      inputs,
      outputs);
}

// Same as LogicalIf_train0, but one branch requires inputs of forward branch
// for computing gradient.
BOOST_AUTO_TEST_CASE(LogicalIf_train2) {
  TensorInfo info{"FLOAT", std::vector<int64_t>{2, 2}};
  TensorInfo infoBool{"BOOL", std::vector<int64_t>{}};
  std::vector<TestTensor> inputs;
  std::vector<TestTensor> outputs;

  float in0Data[4]        = {1, 2, 3, 4};
  ConstVoidData in0CVData = {in0Data, info};

  float in1Data[4]        = {2, 3, 4, 5};
  ConstVoidData in1CVData = {in1Data, info};

  TestRunner runner;
  runner.patterns.enableInPlace(false);
  runner.patterns.enableMulArgGradOp(true);
  runner.isTraining = true;

  TensorId in0;
  TensorId in1;
  TensorId out;

  runner.buildModel([&inputs,
                     &outputs,
                     &runner,
                     &in0CVData,
                     &in1CVData,
                     &in0,
                     &in1,
                     &out,
                     info,
                     infoBool](Builder &builder) {
    auto aiOnnx       = builder.aiOnnxOpset9();
    in0               = builder.addInitializedInputTensor(in0CVData);
    in1               = builder.addInitializedInputTensor(in1CVData);
    auto in_condition = builder.addInputTensor(infoBool);

    auto &then_branch = [in0, in1](Builder &parent_builder) -> Builder & {
      Builder &builder = parent_builder.createSubgraphBuilder();
      auto aiOnnx      = builder.aiOnnxOpset9();
      builder.addInputTensorFromParentGraph(in0);
      builder.addInputTensorFromParentGraph(in1);
      auto out = aiOnnx.add({in0, in1});
      builder.addOutputTensor(out);
      return builder;
    }(builder);

    auto &else_branch = [in0, in1](Builder &parent_builder) -> Builder & {
      Builder &builder = parent_builder.createSubgraphBuilder();
      auto aiOnnx      = builder.aiOnnxOpset9();
      builder.addInputTensorFromParentGraph(in0);
      builder.addInputTensorFromParentGraph(in1);
      auto out = aiOnnx.mul({in0, in1});
      builder.addOutputTensor(out);
      return builder;
    }(builder);

    out = aiOnnx.logical_if({in_condition}, 1, else_branch, then_branch)[0];
    auto l1 =
        builder.aiGraphcoreOpset1().l1loss({out}, 0.1, ReductionType::Sum);

    runner.anchors.insert({getGradId(in0), AnchorReturnType("All")});
    runner.anchors.insert({getGradId(in1), AnchorReturnType("All")});

    inputs.push_back(TestTensor::create<bool>(in_condition, infoBool.shape()));
    outputs.push_back(TestTensor::create<float>(out, info.shape()));
    outputs.push_back(TestTensor::create<float>(getGradId(in0), info.shape()));
    outputs.push_back(TestTensor::create<float>(getGradId(in1), info.shape()));

    runner.loss = l1;

    return out;
  });

  // Check true branch
  // Reference was generated with the below python
  // >>> a = torch.tensor([[1, 2], [3, 4]], dtype=torch.float32,
  // requires_grad=True)
  // >>> b = torch.tensor([[2, 3], [4, 5]], dtype=torch.float32,
  // requires_grad=True)
  // >>> o = a + b
  // >>> o
  // tensor([[3., 5.],
  //         [7., 9.]], grad_fn=<AddBackward0>)
  // >>> o.backward(torch.ones(2, 2) * 0.1)
  // >>> a.grad
  // tensor([[0.1000, 0.1000],
  //         [0.1000, 0.1000]])
  // >>> b.grad
  // tensor([[0.1000, 0.1000],
  //         [0.1000, 0.1000]])
  inputs.back().setData<char>({1});
  runner.checkResults(
      [&in0, &in1, &out](std::vector<TestTensor> &results) {
        BOOST_CHECK_EQUAL(results.size(), 3);
        std::map<TensorId, std::vector<float>> expected{
            {out, {3, 5, 7, 9}},
            {getGradId(in0), {0.1, 0.1, 0.1, 0.1}},
            {getGradId(in1), {0.1, 0.1, 0.1, 0.1}}};

        for (auto &t : results) {
          auto data = t.getDataCopy<float>();
          auto e    = expected.at(t.id);
          logging::debug("BOOST_CHECK({} == {});", data, e);
          BOOST_CHECK(data == e);
        }
      },
      inputs,
      outputs);

  // Check false branch
  // Reference was generated with the below python
  // >>> a = torch.tensor([[1, 2], [3, 4]], dtype=torch.float32,
  // requires_grad=True)
  // >>> b = torch.tensor([[2, 3], [4, 5]], dtype=torch.float32,
  // requires_grad=True)
  // >>> o = a * b
  // >>> o
  // tensor([[ 2.,  6.],
  //         [12., 20.]], grad_fn=<MulBackward0>)
  // >>> o.backward(torch.ones(2, 2) * 0.1)
  // >>> a.grad
  // tensor([[0.2000, 0.3000],
  //         [0.4000, 0.5000]])
  // >>> b.grad
  // tensor([[0.1000, 0.2000],
  //         [0.3000, 0.4000]])
  inputs.back().setData<char>({0});
  runner.checkResults(
      [&in0, &in1, &out](std::vector<TestTensor> &results) {
        BOOST_CHECK_EQUAL(results.size(), 3);
        std::map<TensorId, std::vector<float>> expected{
            {out, {2, 6, 12, 20}},
            {getGradId(in0), {0.2, 0.3, 0.4, 0.5}},
            {getGradId(in1), {0.1, 0.2, 0.3, 0.4}}};

        for (auto &t : results) {
          auto data = t.getDataCopy<float>();
          auto e    = expected.at(t.id);
          logging::debug("BOOST_CHECK({} == {});", data, e);
          BOOST_CHECK(data == e);
        }
      },
      inputs,
      outputs);
}

// Same as LogicalIf_train0, but then and else branchs have different number of
// inputs
BOOST_AUTO_TEST_CASE(LogicalIf_train3) {
  TensorInfo info{"FLOAT", std::vector<int64_t>{1}};
  TensorInfo infoBool{"BOOL", std::vector<int64_t>{}};
  std::vector<TestTensor> inputs;
  std::vector<TestTensor> outputs;

  float in0Data[1]        = {1};
  ConstVoidData in0CVData = {in0Data, info};

  float in1Data[1]        = {2};
  ConstVoidData in1CVData = {in1Data, info};

  TestRunner runner;
  runner.patterns.enableInPlace(false);
  runner.isTraining = true;

  TensorId in0;
  TensorId in1;
  TensorId out;

  runner.buildModel([&inputs,
                     &outputs,
                     &runner,
                     &in0CVData,
                     &in1CVData,
                     &in0,
                     &in1,
                     &out,
                     info,
                     infoBool](Builder &builder) {
    auto aiOnnx       = builder.aiOnnxOpset9();
    in0               = builder.addInitializedInputTensor(in0CVData);
    in1               = builder.addInitializedInputTensor(in1CVData);
    auto in_condition = builder.addInputTensor(infoBool);

    auto &then_branch = [in0, in1](Builder &parent_builder) -> Builder & {
      Builder &builder = parent_builder.createSubgraphBuilder();
      auto aiOnnx      = builder.aiOnnxOpset9();
      builder.addInputTensorFromParentGraph(in0);
      builder.addInputTensorFromParentGraph(in1);
      auto out = aiOnnx.add({in0, in1});
      builder.addOutputTensor(out);
      return builder;
    }(builder);

    auto &else_branch = [in0, in1](Builder &parent_builder) -> Builder & {
      Builder &builder = parent_builder.createSubgraphBuilder();
      auto aiOnnx      = builder.aiOnnxOpset9();
      builder.addInputTensorFromParentGraph(in0);
      auto out = aiOnnx.add({in0, in0});
      builder.addOutputTensor(out);
      return builder;
    }(builder);

    out = aiOnnx.logical_if({in_condition}, 1, else_branch, then_branch)[0];
    auto l1 =
        builder.aiGraphcoreOpset1().l1loss({out}, 0.1, ReductionType::Sum);

    runner.anchors.insert({getGradId(in0), AnchorReturnType("All")});
    runner.anchors.insert({getGradId(in1), AnchorReturnType("All")});

    inputs.push_back(TestTensor::create<bool>(in_condition, infoBool.shape()));
    outputs.push_back(TestTensor::create<float>(out, info.shape()));
    outputs.push_back(TestTensor::create<float>(getGradId(in0), info.shape()));
    outputs.push_back(TestTensor::create<float>(getGradId(in1), info.shape()));

    runner.loss = l1;

    return out;
  });

  auto resultChecker = [](std::vector<TestTensor> &results) {
    logging::debug("Results:");
    for (auto &t : results) {
      auto data = t.getDataCopy<float>();
      logging::debug("  {}: {}", t.id, data);
    }
  };

  (void)resultChecker;

  // Check true branch
  inputs.back().setData<char>({1});
  runner.checkResults(
      [&in0, &in1, &out](std::vector<TestTensor> &results) {
        BOOST_CHECK_EQUAL(results.size(), 3);
        std::map<TensorId, std::vector<float>> expected{
            {out, {3}}, {getGradId(in0), {0.1}}, {getGradId(in1), {0.1}}};

        for (auto &t : results) {
          auto data = t.getDataCopy<float>();
          auto e    = expected.at(t.id);
          logging::debug("BOOST_CHECK({} == {});", data, e);
          BOOST_CHECK(data == e);
        }
      },
      inputs,
      outputs);

  // Check false branch
  inputs.back().setData<char>({0});
  runner.checkResults(
      [&in0, &in1, &out](std::vector<TestTensor> &results) {
        BOOST_CHECK_EQUAL(results.size(), 3);
        std::map<TensorId, std::vector<float>> expected{
            {out, {2}}, {getGradId(in0), {0.2}}, {getGradId(in1), {0.0}}};

        for (auto &t : results) {
          auto data = t.getDataCopy<float>();
          auto e    = expected.at(t.id);
          logging::debug("BOOST_CHECK({} == {});", data, e);
          BOOST_CHECK(data == e);
        }
      },
      inputs,
      outputs);
}

BOOST_AUTO_TEST_CASE(LogicalIf_inputs_differ0) {
  TensorInfo info{"FLOAT", std::vector<int64_t>{2, 2}};
  TensorInfo infoBool{"BOOL", std::vector<int64_t>{}};
  std::vector<TestTensor> inputs;
  std::vector<TestTensor> outputs;

  TestRunner runner;
  runner.patterns.enableInPlace(false);

  runner.buildModel([&inputs, &outputs, info, infoBool](Builder &builder) {
    auto aiOnnx       = builder.aiOnnxOpset9();
    auto in0          = builder.addInputTensor(info);
    auto in1          = builder.addInputTensor(info);
    auto in_condition = builder.addInputTensor(infoBool);

    auto &then_branch = [in0, in1](Builder &parent_builder) -> Builder & {
      Builder &builder = parent_builder.createSubgraphBuilder();
      // auto builder = Builder::create();
      auto aiOnnx = builder.aiOnnxOpset9();
      builder.addInputTensorFromParentGraph(in0);
      auto out = aiOnnx.add({in0, in0});
      builder.addOutputTensor(out);
      return builder;
    }(builder);

    auto &else_branch = [in0, in1](Builder &parent_builder) -> Builder & {
      Builder &builder = parent_builder.createSubgraphBuilder();
      // auto builder = Builder::create();
      auto aiOnnx = builder.aiOnnxOpset9();
      builder.addInputTensorFromParentGraph(in0);
      builder.addInputTensorFromParentGraph(in1);
      auto out = aiOnnx.add({in0, in1});
      builder.addOutputTensor(out);
      return builder;
    }(builder);

    auto out =
        aiOnnx.logical_if({in_condition}, 1, else_branch, then_branch)[0];
    builder.addOutputTensor(out);

    inputs.push_back(
        TestTensor::create<float>(in0, {1, 2, 3, 4}, info.shape()));
    inputs.push_back(
        TestTensor::create<float>(in1, {2, 3, 4, 5}, info.shape()));
    inputs.push_back(TestTensor::create<bool>(in_condition, infoBool.shape()));
    outputs.push_back(TestTensor::create<float>(out, info.shape()));

    return out;
  });

  // Check true branch
  inputs.back().setData<char>({1});
  runner.checkResult(
      [](TestTensor &result) {
        auto data = result.getDataCopy<float>();
        std::vector<float> expected{2, 4, 6, 8};
        BOOST_CHECK(data == expected);
      },
      inputs,
      outputs);

  // Check false branch
  inputs.back().setData<char>({0});
  runner.checkResult(
      [](TestTensor &result) {
        auto data = result.getDataCopy<float>();
        std::vector<float> expected{3, 5, 7, 9};
        BOOST_CHECK(data == expected);
      },
      inputs,
      outputs);
}

BOOST_AUTO_TEST_CASE(LogicalIf_inputs_differ_train0) {
  TensorInfo info{"FLOAT", std::vector<int64_t>{2, 2}};
  TensorInfo infoBool{"BOOL", std::vector<int64_t>{}};
  std::vector<TestTensor> inputs;
  std::vector<TestTensor> outputs;

  float in0Data[4]        = {1, 2, 3, 4};
  ConstVoidData in0CVData = {in0Data, info};

  float in1Data[4]        = {2, 3, 4, 5};
  ConstVoidData in1CVData = {in1Data, info};

  TestRunner runner;
  runner.patterns.enableInPlace(false);
  runner.patterns.enableSubtractArg1GradOp(true);
  runner.isTraining = true;

  runner.buildModel(
      [&inputs, &outputs, &runner, &in0CVData, &in1CVData, info, infoBool](
          Builder &builder) {
        auto aiOnnx       = builder.aiOnnxOpset9();
        auto in0          = builder.addInitializedInputTensor(in0CVData);
        auto in1          = builder.addInitializedInputTensor(in1CVData);
        auto in_condition = builder.addInputTensor(infoBool);

        auto &then_branch = [in0, in1](Builder &parent_builder) -> Builder & {
          Builder &builder = parent_builder.createSubgraphBuilder();
          auto aiOnnx      = builder.aiOnnxOpset9();
          builder.addInputTensorFromParentGraph(in0);
          auto out = aiOnnx.add({in0, in0});
          builder.addOutputTensor(out);
          return builder;
        }(builder);

        auto &else_branch = [in0, in1](Builder &parent_builder) -> Builder & {
          Builder &builder = parent_builder.createSubgraphBuilder();
          auto aiOnnx      = builder.aiOnnxOpset9();
          builder.addInputTensorFromParentGraph(in0);
          builder.addInputTensorFromParentGraph(in1);
          auto out = aiOnnx.add({in0, in1});
          builder.addOutputTensor(out);
          return builder;
        }(builder);

        auto out =
            aiOnnx.logical_if({in_condition}, 1, else_branch, then_branch)[0];
        auto l1 =
            builder.aiGraphcoreOpset1().l1loss({out}, 0.1, ReductionType::Sum);

        inputs.push_back(
            TestTensor::create<bool>(in_condition, infoBool.shape()));
        outputs.push_back(TestTensor::create<float>(out, info.shape()));

        runner.loss = l1;

        return out;
      });

  // Check true branch
  inputs.back().setData<char>({1});
  runner.checkResult(
      [](TestTensor &result) {
        auto data = result.getDataCopy<float>();
        std::vector<float> expected{2, 4, 6, 8};
        BOOST_CHECK(data == expected);
      },
      inputs,
      outputs);

  // Check false branch
  inputs.back().setData<char>({0});
  runner.checkResult(
      [](TestTensor &result) {
        auto data = result.getDataCopy<float>();
        std::vector<float> expected{3, 5, 7, 9};
        BOOST_CHECK(data == expected);
      },
      inputs,
      outputs);
}


// SPDX-License-Identifier: MIT
// Copyright (c) 2021 ETH Zurich, Luc Grosheintz-Laval

#include <boost/program_options.hpp>
#include <iostream>
#include <string>
#include <tuple>

#if ZISA_HAS_METIS == 1
#include <metis.h>
#endif

#if ZISA_HAS_MPI == 1
#include <mpi.h>
#endif

#include <zisa/grid/grid.hpp>
#include <zisa/io/format_as_list.hpp>
#include <zisa/io/hdf5_serial_writer.hpp>
#include <zisa/memory/array_view.hpp>
#include <zisa/parallelization/domain_decomposition.hpp>
#include <zisa/reconstruction/stencil_family.hpp>

#include <thread>
#include <zisa/utils/timer.hpp>

namespace po = boost::program_options;

namespace zisa {
#if ZISA_HAS_METIS == 1
using metis_idx_t = ::idx_t;
#endif

std::mutex work_queue_mutex;

void save_partitioned_grid(const std::string &dirname,
                           const Grid &grid,
                           const StencilParams &stencil_params,
                           int n_parts,
                           int n_workers) {

  LOG_ERR_IF(n_parts <= 1, "You need 2 or more parts.");

  //  auto sharp_stencil_params = StencilFamilyParams{
  //      {3, 2, 2, 2, 2}, {"c", "b", "b", "b", "b"},
  //      {2.0, 1.5, 1.5, 1.5, 1.5}};
  //
  //  auto stencil_timer = Timer();
  //  auto effective_stencils
  //      = compute_effective_stencils(grid, sharp_stencil_params);
  //  PRINT(stencil_timer.elapsed_seconds());
  //
  //  auto metis_timer = Timer();
  //  //  auto partitioned_grid = compute_partitioned_grid(grid, n_parts);
  //  auto partitioned_grid
  //      = compute_partitioned_grid(grid, effective_stencils, n_parts);
  //  PRINT(metis_timer.elapsed_seconds());

  auto partitioned_grid = compute_partitioned_grid_by_sfc(grid, n_parts);

  const auto &permutation = partitioned_grid.permutation;
  const auto &partition = partitioned_grid.partition;
  const auto &boundaries = partitioned_grid.boundaries;

  std::vector<std::thread> workers;
  workers.reserve(n_workers);

  std::vector<int> work_queue(n_parts, 0);

  auto job = [&](int p) {
    auto [local_vertex_indices, local_vertices, global_cell_indices]
        = extract_subgrid(grid, partitioned_grid, stencil_params, p);

    int_t n_cells_local = local_vertex_indices.shape(0);

    auto local_partition = array<int_t, 1>(n_cells_local);
    for (int_t i = 0; i < n_cells_local; ++i) {
      local_partition[i] = partition[global_cell_indices[i]];
    }

    auto filename = dirname + string_format("/subgrid-%04d.msh.h5", p);
    auto hdf5_writer = HDF5SerialWriter(filename);

    hdf5_writer.write_scalar(grid.n_dims(), "n_dims");
    save(hdf5_writer, local_vertex_indices, "vertex_indices");
    save(hdf5_writer, local_vertices, "vertices");
    save(hdf5_writer, local_partition, "partition");
    save(hdf5_writer, global_cell_indices, "global_cell_indices");
  };

  auto scheduling_loop = [&work_queue, job]() {
    while (true) {
      int p = -1;
      {
        auto lock = std::unique_lock(work_queue_mutex);

        int n_parts = work_queue.size();
        for (int pp = 0; pp < n_parts; ++pp) {
          if (work_queue[pp] == 0) {
            work_queue[pp] = 1;
            p = pp;
            break;
          }
        }

        if (p == -1) {
          return; // no more work to be had.
        }
      }

      job(p);
    }
  };

  for (int w = 0; w < n_workers; ++w) {
    workers.push_back(std::thread(scheduling_loop));
  }

  for (int w = 0; w < n_workers; ++w) {
    workers[w].join();
  }

  // auto filename = dirname + "/grid.h5";
  // auto hdf5_writer = HDF5SerialWriter(filename);
  // save(hdf5_writer, grid);
}
}

int main(int argc, char *argv[]) {
#if ZISA_HAS_MPI == 1
  MPI_Init(&argc, &argv);
#endif

  po::variables_map options;

  // generic options
  po::options_description generic("Generic options");

  // clang-format off
  generic.add_options()
      ("help,h", "produce this message")
      ("grid", po::value<std::string>(), "GMSH grid to partition.")
      ("output,o", po::value<std::string>(), "Name of the folder in which to place the partitioned grids.")
      ("partitions,n", po::value<int>(), "Number of partitions.")
      ("workers,k", po::value<int>(), "Number of workers.")
      ;
  // clang-format on

  // first parse cmdline and check what config file to use
  po::store(po::parse_command_line(argc, argv, generic), options);

  if (options.count("help") != 0) {
    std::cout << generic << "\n";
    std::exit(EXIT_SUCCESS);
  }

  if (options.count("grid") == 0) {
    std::cout << "Missing argument `--gmsh GMSH`.\n";
    std::exit(EXIT_FAILURE);
  }

  if (options.count("partitions") == 0) {
    std::cout << "Missing argument `-n N`.\n";
    std::exit(EXIT_FAILURE);
  }

  if (options.count("workers") == 0) {
    std::cout << "Missing argument `-k K`.\n";
    std::exit(EXIT_FAILURE);
  }

  if (options.count("output") == 0) {
    std::cout << "Missing argument `--output OUTPUT`.\n";
    std::exit(EXIT_FAILURE);
  }

  auto n_parts = options["partitions"].as<int>();
  auto n_workers = options["workers"].as<int>();
  auto gmsh_file = options["grid"].as<std::string>();
  auto part_file = options["output"].as<std::string>();
  auto grid = zisa::load_grid(gmsh_file);

  auto stencil_params = [&grid]() {
    if (grid->n_dims() == 2) {
      return zisa::StencilParams(5, "c", 8.0);
    } else if (grid->n_dims() == 3) {
      return zisa::StencilParams(4, "c", 8.0);
    }
    LOG_ERR("Broken logic.");
  }();

  zisa::save_partitioned_grid(
      part_file, *grid, stencil_params, n_parts, n_workers);

#if ZISA_HAS_MPI == 1
  MPI_Finalize();
#endif
}


//
// Copyright (c) 2019 INRIA
//

#include "pinocchio/serialization/archive.hpp"
#include "pinocchio/serialization/spatial.hpp"

#include "pinocchio/serialization/frame.hpp"

#include "pinocchio/serialization/joints.hpp"
#include "pinocchio/serialization/model.hpp"

#include "pinocchio/parsers/sample-models.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

template<typename T>
void generic_test(const T & object,
                  const std::string & filename,
                  const std::string & tag_name)
{
  using namespace pinocchio::serialization;

  // Load and save as TXT
  const std::string txt_filename = filename + ".txt";
  saveToText(object,txt_filename);

  {
    T object_loaded;
    loadFromText(object_loaded,txt_filename);

    // Check
    BOOST_CHECK(object_loaded == object);
  }

  // Load and save as XML
  const std::string xml_filename = filename + ".xml";
  saveToXML(object,xml_filename,tag_name);

  {
    T object_loaded;
    loadFromXML(object_loaded,xml_filename,tag_name);

    // Check
    BOOST_CHECK(object_loaded == object);
  }

  // Load and save as binary
  const std::string bin_filename = filename + ".bin";
  saveToBinary(object,bin_filename);

  {
    T object_loaded;
    loadFromBinary(object_loaded,bin_filename);

    // Check
    BOOST_CHECK(object_loaded == object);
  }
}

BOOST_AUTO_TEST_CASE(test_spatial_serialization)
{
  using namespace pinocchio;

  SE3 M(SE3::Random());
  generic_test(M,TEST_SERIALIZATION_FOLDER"/SE3","SE3");

  Motion m(Motion::Random());
  generic_test(m,TEST_SERIALIZATION_FOLDER"/Motion","Motion");

  Force f(Force::Random());
  generic_test(f,TEST_SERIALIZATION_FOLDER"/Force","Force");

  Symmetric3 S(Symmetric3::Random());
  generic_test(S,TEST_SERIALIZATION_FOLDER"/Symmetric3","Symmetric3");

  Inertia I(Inertia::Random());
  generic_test(I,TEST_SERIALIZATION_FOLDER"/Inertia","Inertia");
}

BOOST_AUTO_TEST_CASE(test_multibody_serialization)
{
  using namespace pinocchio;

  Frame frame("frame",0,0,SE3::Random(),SENSOR);
  generic_test(frame,TEST_SERIALIZATION_FOLDER"/Frame","Frame");
}

struct TestJoint
{
  template <typename T>
  void operator()(const T) const
  {
    T jmodel;
    jmodel.setIndexes(0,0,0);

    test(jmodel);
  }

  void operator()(const pinocchio::JointModelComposite & ) const
  {
    pinocchio::JointModelComposite jmodel((pinocchio::JointModelRX()));
    jmodel.addJoint(pinocchio::JointModelRY());
    jmodel.setIndexes(0,0,0);

    test(jmodel);
  }

  void operator()(const pinocchio::JointModelRevoluteUnaligned & ) const
  {
    pinocchio::JointModelRevoluteUnaligned jmodel(1.5, 1., 0.);
    jmodel.setIndexes(0,0,0);

    test(jmodel);
  }

  void operator()(const pinocchio::JointModelPrismaticUnaligned & ) const
  {
    pinocchio::JointModelPrismaticUnaligned jmodel(1.5, 1., 0.);
    jmodel.setIndexes(0,0,0);

    test(jmodel);
  }

  template<typename JointType>
  static void test(JointType & jmodel)
  {
    generic_test(jmodel,TEST_SERIALIZATION_FOLDER"/Joint",jmodel.shortname());
  }

};

BOOST_AUTO_TEST_CASE(test_multibody_joints_serialization)
{
  using namespace pinocchio;
  boost::mpl::for_each<JointModelVariant::types>(TestJoint());
}

BOOST_AUTO_TEST_CASE(test_model_serialization)
{
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  generic_test(model,TEST_SERIALIZATION_FOLDER"/Model","Model");
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/graph/dominator_tree.hpp>


#include "Point.h"

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

#include "Optimizer.h"
#include "Util.h"

using namespace std;
using namespace MosesTuning;

BOOST_AUTO_TEST_CASE(point_operators)
{
  const unsigned int dim = 5;
  vector<float> init(dim);
  init[0] = 1.0f;
  init[1] = 1.0f;
  init[2] = 0.3f;
  init[3] = 0.2f;
  init[4] = 0.3f;

  vector<float> min(dim, 0.0f);
  vector<float> max(dim, 0.0f);

  Point::setdim(dim);
  BOOST_REQUIRE(dim == Point::getdim());

  // Test operator '+'
  {
    Point p1(init, min, max);
    Point p2(init, min, max);
    Point p3 = p1 + p2;
    for (size_t i = 0; i < p3.size(); ++i) {
      BOOST_CHECK(IsAlmostEqual(init[i] * 2.0f, p3[i]));
    }
    BOOST_CHECK_EQUAL(p3.GetScore(), kMaxFloat);
  }

  // Test operator '+='
  {
    Point p1(init, min, max);
    Point p2(init, min, max);
    p1 += p2;

    for (size_t i = 0; i < p1.size(); ++i) {
      BOOST_CHECK(IsAlmostEqual(init[i] * 2.0f, p1[i]));
    }
    BOOST_CHECK_EQUAL(p1.GetScore(), kMaxFloat);
  }

  // Test operator '*'
  {
    Point p1(init, min, max);
    const Point p2 = p1 * 2.0;

    BOOST_REQUIRE(p1.size() == p2.size());
    for (size_t i = 0; i < p2.size(); ++i) {
      BOOST_CHECK(IsAlmostEqual(init[i] * 2.0f, p2[i]));
    }
    BOOST_CHECK_EQUAL(p2.GetScore(), kMaxFloat);
  }
}


// Boost.Geometry
// Unit Test

// Copyright (c) 2014 Adam Wulkiewicz, Lodz, Poland.

// Use, modification and distribution is subject to 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)

#include <geometry_test_common.hpp>

#include <boost/assign.hpp>
#include <boost/range.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/geometries/register/point.hpp>

#include <boost/geometry/algorithms/num_points.hpp>
#include <boost/geometry/multi/algorithms/num_points.hpp>
#include <boost/geometry/algorithms/num_geometries.hpp>

typedef std::pair<float, float> pt_pair_t;
BOOST_GEOMETRY_REGISTER_POINT_2D(pt_pair_t, float, bg::cs::cartesian, first, second)
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

template <typename P>
void test_default()
{
    typedef bg::model::multi_point<P> mpt;
    typedef bg::model::linestring<P> ls;
    typedef bg::model::multi_linestring<ls> mls;
    typedef bg::model::ring<P> ring;
    typedef bg::model::polygon<P> poly;
    typedef bg::model::multi_polygon<poly> mpoly;

    // multi_point()
    mpt mptd;
    BOOST_CHECK(bg::num_points(mptd) == 0);
    // linestring()
    ls lsd;
    BOOST_CHECK(bg::num_points(lsd) == 0);
    // multi_linestring()
    mls mlsd;
    BOOST_CHECK(bg::num_points(mlsd) == 0);
    // ring()
    ring rd;
    BOOST_CHECK(bg::num_points(rd) == 0);
    // polygon()
    poly pd;
    BOOST_CHECK(bg::num_points(pd) == 0);
    // multi_polygon()
    mpoly mpd;
    BOOST_CHECK(bg::num_points(mpd) == 0);
}

template <typename P>
void test_boost_assign_2d()
{
    typedef bg::model::multi_point<P> mpt;
    typedef bg::model::linestring<P> ls;
    typedef bg::model::ring<P> ring;

    // using Boost.Assign
    mpt mpt2 = boost::assign::list_of(P(0, 0))(P(1, 0));
    BOOST_CHECK(bg::num_points(mpt2) == 2);
    mpt2 = boost::assign::list_of(P(0, 0))(P(1, 0));
    BOOST_CHECK(bg::num_points(mpt2) == 2);

    // using Boost.Assign
    ls ls2 = boost::assign::list_of(P(0, 0))(P(1, 0))(P(1, 1));
    BOOST_CHECK(bg::num_points(ls2) == 3);
    ls2 = boost::assign::list_of(P(0, 0))(P(1, 0))(P(1, 1));
    BOOST_CHECK(bg::num_points(ls2) == 3);

    // using Boost.Assign
    ring r2 = boost::assign::list_of(P(0, 0))(P(0, 1))(P(1, 1))(P(1, 0))(P(0, 0));
    BOOST_CHECK(bg::num_points(r2) == 5);
    r2 = boost::assign::list_of(P(0, 0))(P(0, 1))(P(1, 1))(P(1, 0))(P(0, 0));
    BOOST_CHECK(bg::num_points(r2) == 5);
}

void test_boost_assign_pair_2d()
{
    typedef std::pair<float, float> pt;

    test_boost_assign_2d<pt>();

    typedef bg::model::multi_point<pt> mpt;

    // using Boost.Assign
    mpt mpt2 = boost::assign::pair_list_of(0, 0)(1, 0);
    BOOST_CHECK(bg::num_points(mpt2) == 2);
    mpt2 = boost::assign::pair_list_of(0, 0)(1, 0);
    BOOST_CHECK(bg::num_points(mpt2) == 2);
}

void test_boost_assign_tuple_2d()
{
    typedef boost::tuple<float, float> pt;

    test_boost_assign_2d<pt>();

    typedef bg::model::multi_point<pt> mpt;

    // using Boost.Assign
    mpt mpt2 = boost::assign::tuple_list_of(0, 0)(1, 0);
    BOOST_CHECK(bg::num_points(mpt2) == 2);
    mpt2 = boost::assign::tuple_list_of(0, 0)(1, 0);
    BOOST_CHECK(bg::num_points(mpt2) == 2);
}

template <typename P>
void test_initializer_list_2d()
{
#if !defined(BOOST_NO_CXX11_HDR_INITIALIZER_LIST) && !defined(BOOST_NO_CXX11_UNIFIED_INITIALIZATION_SYNTAX)

    typedef bg::model::multi_point<P> mpt;
    typedef bg::model::linestring<P> ls;
    typedef bg::model::multi_linestring<ls> mls;
    typedef bg::model::ring<P> ring;
    typedef bg::model::polygon<P> poly;
    typedef bg::model::multi_polygon<poly> mpoly;

    // multi_point(initializer_list<Point>)
    mpt mpt1 = {{0, 0}, {1, 0}, {2, 0}};
    BOOST_CHECK(bg::num_geometries(mpt1) == 3);
    BOOST_CHECK(bg::num_points(mpt1) == 3);
    // multi_point::operator=(initializer_list<Point>)
    mpt1 = {{0, 0}, {1, 0}, {2, 0}, {3, 0}};
    BOOST_CHECK(bg::num_points(mpt1) == 4);

    // linestring(initializer_list<Point>)
    ls ls1 = {{0, 0}, {1, 0}, {2, 0}};
    BOOST_CHECK(bg::num_geometries(ls1) == 1);
    BOOST_CHECK(bg::num_points(ls1) == 3);
    // linestring::operator=(initializer_list<Point>)
    ls1 = {{0, 0}, {1, 0}, {2, 0}, {3, 0}};
    BOOST_CHECK(bg::num_points(ls1) == 4);

    // multi_linestring(initializer_list<Linestring>)
    mls mls1 = {{{0, 0}, {1, 0}, {2, 0}}, {{3, 0}, {4, 0}}};
    BOOST_CHECK(bg::num_geometries(mls1) == 2);
    BOOST_CHECK(bg::num_points(mls1) == 5);
    // multi_linestring::operator=(initializer_list<Linestring>)
    mls1 = {{{0, 0}, {1, 0}, {2, 0}}, {{3, 0}, {4, 0}, {5, 0}}};
    BOOST_CHECK(bg::num_points(mls1) == 6);

    // ring(initializer_list<Point>)
    ring r1 = {{0, 0}, {0, 1}, {1, 1}, {1, 0}, {0, 0}};
    BOOST_CHECK(bg::num_geometries(r1) == 1);
    BOOST_CHECK(bg::num_points(r1) == 5);
    // ring::operator=(initializer_list<Point>)
    r1 = {{0, 0}, {0, 1}, {1, 2}, {2, 1}, {1, 0}, {0, 0}};
    BOOST_CHECK(bg::num_points(r1) == 6);

    // polygon(initializer_list<ring_type>)
    poly p1 = {{{0, 0}, {0, 9}, {9, 9}, {9, 0}, {0, 0}}, {{1, 1}, {2, 1}, {2, 2}, {1, 2}, {1, 1}}};
    BOOST_CHECK(bg::num_geometries(p1) == 1);
    BOOST_CHECK(bg::num_points(p1) == 10);
    BOOST_CHECK(boost::size(bg::interior_rings(p1)) == 1);
    // polygon::operator=(initializer_list<ring_type>)
    p1 = {{{0, 0}, {0, 8}, {8, 9}, {9, 8}, {8, 0}, {0, 0}}, {{1, 1}, {2, 1}, {2, 2}, {1, 2}, {1, 1}}};
    BOOST_CHECK(bg::num_points(p1) == 11);
    BOOST_CHECK(boost::size(bg::interior_rings(p1)) == 1);
    p1 = {{{0, 0}, {0, 9}, {9, 9}, {9, 0}, {0, 0}}};
    BOOST_CHECK(bg::num_points(p1) == 5);
    BOOST_CHECK(boost::size(bg::interior_rings(p1)) == 0);
    // polygon(initializer_list<ring_type>)
    poly p2 = {{{0, 0}, {0, 9}, {9, 9}, {9, 0}, {0, 0}}};
    BOOST_CHECK(bg::num_geometries(p2) == 1);
    BOOST_CHECK(bg::num_points(p2) == 5);
    BOOST_CHECK(boost::size(bg::interior_rings(p1)) == 0);
    // polygon::operator=(initializer_list<ring_type>)
    p2 = {{{0, 0}, {0, 8}, {8, 9}, {9, 8}, {8, 0}, {0, 0}}};
    BOOST_CHECK(bg::num_points(p2) == 6);

    // multi_polygon(initializer_list<Polygon>)
    mpoly mp1 = {{{{0, 0}, {0, 1}, {1, 1}, {1, 0}, {0, 0}}}, {{{2, 2}, {2, 3}, {3, 3}, {3, 2}, {2, 2}}}};
    BOOST_CHECK(bg::num_geometries(mp1) == 2);
    BOOST_CHECK(bg::num_points(mp1) == 10);
    // multi_polygon::operator=(initializer_list<Polygon>)
    mp1 = {{{{0, 0}, {0, 1}, {1, 2}, {2, 1}, {1, 0}, {0, 0}}}, {{{2, 2}, {2, 3}, {3, 3}, {3, 2}, {2, 2}}}};
    BOOST_CHECK(bg::num_points(mp1) == 11);

#endif
}

template <typename P>
void test_all_2d()
{
    test_default<P>();
    test_boost_assign_2d<P>();
    test_initializer_list_2d<P>();
}

template <typename T>
struct test_point
{
    test_point(T = T(), T = T()) {}
};

template <typename T>
struct test_range
{
    test_range() {}
    template <typename It>
    test_range(It, It) {}
#if !defined(BOOST_NO_CXX11_HDR_INITIALIZER_LIST)
    test_range(std::initializer_list<T>) {}
    //test_range & operator=(std::initializer_list<T>) { return *this; }
#endif
};

void test_sanity_check()
{
    typedef test_point<float> P;
    typedef test_range<P> R;
    typedef std::vector<P> V;

#if !defined(BOOST_NO_CXX11_HDR_INITIALIZER_LIST) && !defined(BOOST_NO_CXX11_UNIFIED_INITIALIZATION_SYNTAX)
    {
        R r = {{1, 1},{2, 2},{3, 3}};
        r = {{1, 1},{2, 2},{3, 3}};

        V v = {{1, 1},{2, 2},{3, 3}};
        v = {{1, 1},{2, 2},{3, 3}};
    }
#endif
    {
        R r = boost::assign::list_of(P(1, 1))(P(2, 2))(P(3, 3));
        r = boost::assign::list_of(P(1, 1))(P(2, 2))(P(3, 3));

        V v = boost::assign::list_of(P(1, 1))(P(2, 2))(P(3, 3));
        //v = boost::assign::list_of(P(1, 1))(P(2, 2))(P(3, 3));
        v.empty();
    }
}

int test_main(int, char* [])
{
    test_all_2d< bg::model::point<float, 2, bg::cs::cartesian> >();
    test_all_2d< bg::model::d2::point_xy<float> >();

    test_boost_assign_pair_2d();
    test_boost_assign_tuple_2d();

    test_sanity_check();

    return 0;
}


#ifndef HOG_DETECTOR_H
#define HOG_DETECTOR_H

#include <opencv2/objdetect.hpp>
#include <opencv2/imgproc.hpp>
#include <Eigen/Core>
#include <iostream>
#include "utils.hpp"
using namespace cv;
using namespace std;
using namespace Eigen;

class HOGDetector
{
public:
	HOGDetector();
	HOGDetector(int group_threshold, double hit_threshold);
	vector<Rect> detect(Mat &frame);
	void draw();
	//vector<Rect> getDetections();
	MatrixXd getFeatureValues(Mat &frame, vector<MyTarget> detections);
	VectorXd getDetectionWeights();

private:
	HOGDescriptor hog;
	vector<Rect> detections;
	VectorXd weights;
	Mat frame;
	int group_threshold;
	double hit_threshold;
};

#endif


// <uuids> -*- C++ -*-

#pragma once

#include <chrono>
#include <random>

#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>

namespace simdb {

//! \note There is a known issue with the Boost
//! UUID library that causes valgrind to fail.
//! Some Boost versions intentionally read from
//! uninitialized memory in order to increase
//! randomness of their UUID algorithms. This
//! occurs in the default constructor of:
//!
//! boost::uuids::basic_random_generator<T>::ctor()
//!
//! The Boost documentation notes related to
//! boost::uuid and valgrind suggest to get
//! around the valgrind failure by either
//! suppressing these valgrind errors, or
//! by using a non-default constructor for
//! the basic_random_generator<T> class.
inline std::string generateUUID()
{
    std::mt19937 ran;
    auto seed = std::chrono::high_resolution_clock::now().
        time_since_epoch().count();

    ran.seed(static_cast<uint64_t>(seed));

    boost::uuids::basic_random_generator<std::mt19937> gen(&ran);
    boost::uuids::uuid uuid = gen();

    std::ostringstream oss;
    oss << uuid;
    return oss.str();
}

} // namespace simdb