#include <openvslam_ros.h>

#include <chrono>

#include <opencv2/core/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <openvslam/publish/map_publisher.h>
#include <Eigen/Geometry>

namespace openvslam_ros {
system::system(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path)
    : SLAM_(cfg, vocab_file_path), cfg_(cfg), private_nh_("~"), it_(nh_), tp_0_(std::chrono::steady_clock::now()),
      mask_(mask_img_path.empty() ? cv::Mat{} : cv::imread(mask_img_path, cv::IMREAD_GRAYSCALE)),
      pose_pub_(private_nh_.advertise<geometry_msgs::PoseStamped>("camera_pose", 1)) {}

void system::publish_pose() {
    // SLAM get the motion matrix publisher
    auto cam_pose_wc = SLAM_.get_map_publisher()->get_current_cam_pose();

    // Extract rotation matrix and translation vector from
    Eigen::Matrix3d rot = cam_pose_wc.block<3, 3>(0, 0);
    Eigen::Vector3d trans = cam_pose_wc.block<3, 1>(0, 3);
    Eigen::Matrix3d cv_to_ros;
    cv_to_ros << 0, 0, -1,
                 1, 0, 0,
                 0, 1, 0;

    // Transform from CV coordinate system to ROS coordinate system on camera coordinates
    Eigen::Quaterniond quat(cv_to_ros * rot * cv_to_ros.transpose());
    trans = cv_to_ros * trans;

    // Create odometry message and update it with current camera pose
    geometry_msgs::PoseStamped pose_msg;
    pose_msg.header.stamp = ros::Time::now();
    pose_msg.header.frame_id = "map";
    pose_msg.pose.orientation.x = quat.x();
    pose_msg.pose.orientation.y = quat.y();
    pose_msg.pose.orientation.z = quat.z();
    pose_msg.pose.orientation.w = quat.w();
    pose_msg.pose.position.x = trans(0);
    pose_msg.pose.position.y = trans(1);
    pose_msg.pose.position.z = trans(2);
    pose_pub_.publish(pose_msg);
}

mono::mono(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path)
    : system(cfg, vocab_file_path, mask_img_path) {
    sub_ = it_.subscribe("camera/image_raw", 1, &mono::callback, this);
}
void mono::callback(const sensor_msgs::ImageConstPtr& msg) {
    const auto tp_1 = std::chrono::steady_clock::now();
    const auto timestamp = std::chrono::duration_cast<std::chrono::duration<double>>(tp_1 - tp_0_).count();

    // input the current frame and estimate the camera pose
    SLAM_.feed_monocular_frame(cv_bridge::toCvShare(msg)->image, timestamp, mask_);

    const auto tp_2 = std::chrono::steady_clock::now();

    const auto track_time = std::chrono::duration_cast<std::chrono::duration<double>>(tp_2 - tp_1).count();
    track_times_.push_back(track_time);
}

stereo::stereo(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path,
               const bool rectify)
    : system(cfg, vocab_file_path, mask_img_path),
      rectifier_(rectify ? std::make_shared<openvslam::util::stereo_rectifier>(cfg) : nullptr),
      left_sf_(it_, "camera/left/image_raw", 1),
      right_sf_(it_, "camera/right/image_raw", 1),
      sync_(SyncPolicy(10), left_sf_, right_sf_) {
    sync_.registerCallback(&stereo::callback, this);
}

void stereo::callback(const sensor_msgs::ImageConstPtr& left, const sensor_msgs::ImageConstPtr& right) {
    auto leftcv = cv_bridge::toCvShare(left)->image;
    auto rightcv = cv_bridge::toCvShare(right)->image;
    if (leftcv.empty() || rightcv.empty()) {
        return;
    }

    if (rectifier_) {
        rectifier_->rectify(leftcv, rightcv, leftcv, rightcv);
    }

    const auto tp_1 = std::chrono::steady_clock::now();
    const auto timestamp = std::chrono::duration_cast<std::chrono::duration<double>>(tp_1 - tp_0_).count();

    // input the current frame and estimate the camera pose
    SLAM_.feed_stereo_frame(leftcv, rightcv, timestamp, mask_);

    const auto tp_2 = std::chrono::steady_clock::now();

    const auto track_time = std::chrono::duration_cast<std::chrono::duration<double>>(tp_2 - tp_1).count();
    track_times_.push_back(track_time);
}

rgbd::rgbd(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path)
    : system(cfg, vocab_file_path, mask_img_path),
      color_sf_(it_, "camera/color/image_raw", 1),
      depth_sf_(it_, "camera/aligned_depth_to_color/image_raw", 1),
      sync_(SyncPolicy(10), color_sf_, depth_sf_) {
    sync_.registerCallback(&rgbd::callback, this);
}

void rgbd::callback(const sensor_msgs::ImageConstPtr& color, const sensor_msgs::ImageConstPtr& depth) {
    auto colorcv = cv_bridge::toCvShare(color)->image;
    auto depthcv = cv_bridge::toCvShare(depth)->image;
    if (colorcv.empty() || depthcv.empty()) {
        return;
    }

    const auto tp_1 = std::chrono::steady_clock::now();
    const auto timestamp = std::chrono::duration_cast<std::chrono::duration<double>>(tp_1 - tp_0_).count();

    // input the current frame and estimate the camera pose
    SLAM_.feed_RGBD_frame(colorcv, depthcv, timestamp, mask_);

    const auto tp_2 = std::chrono::steady_clock::now();

    const auto track_time = std::chrono::duration_cast<std::chrono::duration<double>>(tp_2 - tp_1).count();
    track_times_.push_back(track_time);
}
} // namespace openvslam_ros


#include <boost/multiprecision/cpp_int/limits.hpp>


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

#include "../check_arrays_are_close.hpp"
#include "../type_template.hpp"

#include "assembly/assembly_options.hpp"
#include "assembly/discrete_boundary_operator.hpp"
#include "assembly/boundary_operator.hpp"
#include "assembly/context.hpp"
#include "assembly/identity_operator.hpp"
#include "assembly/numerical_quadrature_strategy.hpp"

#include "assembly/modified_helmholtz_3d_single_layer_boundary_operator.hpp"

#include "common/boost_make_shared_fwd.hpp"

#include "grid/grid_factory.hpp"
#include "grid/grid.hpp"

#include "space/piecewise_linear_continuous_scalar_space.hpp"
#include "space/piecewise_constant_scalar_space.hpp"

#include <algorithm>
#include "common/armadillo_fwd.hpp"
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/version.hpp>
#include <complex>

// Tests

using namespace Bempp;

namespace
{

template <typename T>
T initMultiplier();

template <>
float initMultiplier()
{
    return 1.2f;
}

template <>
double initMultiplier()
{
    return 1.2;
}

template <>
std::complex<float> initMultiplier()
{
    return std::complex<float>(1.2f, 0.7f);
}

template <>
std::complex<double> initMultiplier()
{
    return std::complex<double>(1.2, 0.7);
}

} // namespace

BOOST_AUTO_TEST_SUITE(ScaledDiscreteDenseBoundaryOperator)

BOOST_AUTO_TEST_CASE_TEMPLATE(builtin_apply_works_correctly_for_alpha_equal_to_2_and_beta_equal_to_0_and_y_initialized_to_nans, ResultType, result_types)
{
    typedef ResultType RT;
    typedef typename Fiber::ScalarTraits<RT>::RealType BFT;
    typedef typename Fiber::ScalarTraits<RT>::RealType CT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;

    const int dimGrid = 2;
    typedef double ctype;
    arma::Col<double> lowerLeft(dimGrid);
    arma::Col<double> upperRight(dimGrid);
    arma::Col<unsigned int> nElements(dimGrid);
    lowerLeft.fill(0);
    upperRight.fill(1);
    nElements(0) = 2;
    nElements(1) = 3;

    shared_ptr<Grid> grid =
        Bempp::GridFactory::createStructuredGrid(
            params, lowerLeft, upperRight, nElements);

    PiecewiseLinearContinuousScalarSpace<BFT> pwiseLinears(grid);
    PiecewiseConstantScalarSpace<BFT> pwiseConstants(grid);

    AssemblyOptions assemblyOptions;
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    NumericalQuadratureStrategy<BFT, RT> quadStrategy;

    Context<BFT, RT> context(make_shared_from_ref(quadStrategy), assemblyOptions);

    const RT multiplier = initMultiplier<RT>();

    BoundaryOperator<BFT, RT> id =
        identityOperator<BFT, RT>(
        make_shared_from_ref(context),
        make_shared_from_ref(pwiseConstants),
        make_shared_from_ref(pwiseConstants),
        make_shared_from_ref(pwiseLinears));

    BoundaryOperator<BFT, RT> op = multiplier * id;

    shared_ptr<const DiscreteBoundaryOperator<RT> > dop = op.weakForm();

    RT alpha = static_cast<RT>(2.);
    RT beta = static_cast<RT>(0.);

    arma::Col<RT> x(dop->columnCount());
    x.fill(1.);
    arma::Col<RT> y(dop->rowCount());
    y.fill(std::numeric_limits<CT>::quiet_NaN());

    arma::Col<RT> expected = alpha * dop->asMatrix() * x;

    dop->apply(NO_TRANSPOSE, x, y, alpha, beta);

    BOOST_CHECK(y.is_finite());
    BOOST_CHECK(check_arrays_are_close<RT>(y, expected,
                                           10. * std::numeric_limits<CT>::epsilon()));
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <Eigen/Dense>

namespace pd::ecs::cmp::root
{
	// ---------------------------------------------------------------------------------------------------------
	class Input
	{
	public:
		enum class eMouseButton
		{
			LEFT = 0,
			MIDDLE,
			RIGHT,

			COUNT
		};

		using ButtonStateArrayType = std::array<bool, static_cast<int>(eMouseButton::COUNT)>;

		bool isPressed(eMouseButton button) const { return m_currentButtonState[static_cast<int>(button)]; }

		bool wasJustPressed(eMouseButton button) const
		{
			return m_currentButtonState[static_cast<int>(button)] == true
				&& m_lastButtonState[static_cast<int>(button)] == false;
		}

		bool wasJustReleased(eMouseButton button) const
		{
			return m_currentButtonState[static_cast<int>(button)] == false
				&& m_lastButtonState[static_cast<int>(button)] == true;
		}

		Eigen::Vector2i getCursorPos() const { return m_cursorPos; }
		Eigen::Vector2i getCursorDiff() const { return m_cursorDiff; }

		void setNewButtonState(ButtonStateArrayType newState)
		{
			m_lastButtonState = std::move(m_currentButtonState);
			m_currentButtonState = std::move(newState);
		}

		void setNewCursorPos(Eigen::Vector2i newCursorPos)
		{
			m_cursorDiff = newCursorPos - m_cursorPos;
			m_cursorPos = std::move(newCursorPos);
		}

	private:
		ButtonStateArrayType m_currentButtonState = { false };
		ButtonStateArrayType m_lastButtonState = { false };

		Eigen::Vector2i m_cursorPos;
		Eigen::Vector2i m_cursorDiff;

	};
}


#include <cap/energy_storage_device.h>
#include <cap/mp_values.h>
#include <cap/default_inspector.h>
#include <cap/supercapacitor.h>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/info_parser.hpp>
#include <boost/mpi/environment.hpp>
#include <boost/mpi/timer.hpp>
#include <iostream>

int main(int argc, char *argv[])
{
  boost::mpi::environment env(argc, argv);
  boost::mpi::communicator comm;
  boost::mpi::timer timer;
  if (comm.rank() == 0)
    std::cout << "Number of processors: " << comm.size() << std::endl;

  // Parse input file
  boost::property_tree::ptree device_database;
  boost::property_tree::info_parser::read_info("super_capacitor.info",
                                               device_database);

  std::shared_ptr<cap::EnergyStorageDevice> device =
      cap::EnergyStorageDevice::build(device_database, comm);

  unsigned int const n_time_steps = 10;
  double const time_step = 0.1;
  double const charge_voltage = 2.1;
  for (unsigned int i = 0; i < n_time_steps; ++i)
    device->evolve_one_time_step_constant_voltage(time_step, charge_voltage);

  if (comm.rank() == 0)
  {
    cap::DefaultInspector inspector;
    inspector.inspect(device.get());
    auto data = inspector.get_data();
    std::cout << "n dofs: " << data["n_dofs"] << std::endl;
    std::cout << "Elapsed time: " << timer.elapsed() << std::endl;
  }

  cap::SuperCapacitorInspector<2> supercap_inspector;
  supercap_inspector.inspect(device.get());

  return 0;
}


/***************************************************************************
 * Copyright 1998-2018 by authors (see AUTHORS.txt)                        *
 *                                                                         *
 *   This file is part of LuxCoreRender.                                   *
 *                                                                         *
 * Licensed under the Apache License, Version 2.0 (the "License");         *
 * you may not use this file except in compliance with the License.        *
 * You may obtain a copy of the License at                                 *
 *                                                                         *
 *     http://www.apache.org/licenses/LICENSE-2.0                          *
 *                                                                         *
 * Unless required by applicable law or agreed to in writing, software     *
 * distributed under the License is distributed on an "AS IS" BASIS,       *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.*
 * See the License for the specific language governing permissions and     *
 * limitations under the License.                                          *
 ***************************************************************************/

#include <limits>
#include <iostream>

#include <boost/thread.hpp>
#include <boost/filesystem/operations.hpp>

#include <luxcore/luxcore.h>

using namespace std;
using namespace luxrays;
using namespace luxcore;

class UV {
public:
	UV(float _u = 0.f, float _v = 0.f)
	: u(_u), v(_v) {
	}

	UV(const float v[2]) : u(v[0]), v(v[1]) {
	}

	float u, v;
};

class Point {
public:
	Point(float _x = 0.f, float _y = 0.f, float _z = 0.f)
	: x(_x), y(_y), z(_z) {
	}

	Point(const float v[3]) : x(v[0]), y(v[1]), z(v[2]) {
	}

	float x, y, z;
};

class Triangle {
public:
	Triangle() { }
	Triangle(const unsigned int v0, const unsigned int v1, const unsigned int v2) {
		v[0] = v0;
		v[1] = v1;
		v[2] = v2;
	}

	unsigned int v[3];
};

class BBox {
public:
	// BBox Public Methods

	BBox() {
		pMin = Point(numeric_limits<float>::infinity(),
				numeric_limits<float>::infinity(),
				numeric_limits<float>::infinity());
		pMax = Point(-numeric_limits<float>::infinity(),
				-numeric_limits<float>::infinity(),
				-numeric_limits<float>::infinity());
	}

	BBox(const Point &p1, const Point &p2) {
		pMin = p1;
		pMax = p2;
	}

	Point pMin, pMax;
};

static void CreateBox(Scene *scene, const string &objName, const string &meshName,
		const string &matName, const bool enableUV, const BBox &bbox) {
	Point *p = (Point *)Scene::AllocVerticesBuffer(24);
	// Bottom face
	p[0] = Point(bbox.pMin.x, bbox.pMin.y, bbox.pMin.z);
	p[1] = Point(bbox.pMin.x, bbox.pMax.y, bbox.pMin.z);
	p[2] = Point(bbox.pMax.x, bbox.pMax.y, bbox.pMin.z);
	p[3] = Point(bbox.pMax.x, bbox.pMin.y, bbox.pMin.z);
	// Top face
	p[4] = Point(bbox.pMin.x, bbox.pMin.y, bbox.pMax.z);
	p[5] = Point(bbox.pMax.x, bbox.pMin.y, bbox.pMax.z);
	p[6] = Point(bbox.pMax.x, bbox.pMax.y, bbox.pMax.z);
	p[7] = Point(bbox.pMin.x, bbox.pMax.y, bbox.pMax.z);
	// Side left
	p[8] = Point(bbox.pMin.x, bbox.pMin.y, bbox.pMin.z);
	p[9] = Point(bbox.pMin.x, bbox.pMin.y, bbox.pMax.z);
	p[10] = Point(bbox.pMin.x, bbox.pMax.y, bbox.pMax.z);
	p[11] = Point(bbox.pMin.x, bbox.pMax.y, bbox.pMin.z);
	// Side right
	p[12] = Point(bbox.pMax.x, bbox.pMin.y, bbox.pMin.z);
	p[13] = Point(bbox.pMax.x, bbox.pMax.y, bbox.pMin.z);
	p[14] = Point(bbox.pMax.x, bbox.pMax.y, bbox.pMax.z);
	p[15] = Point(bbox.pMax.x, bbox.pMin.y, bbox.pMax.z);
	// Side back
	p[16] = Point(bbox.pMin.x, bbox.pMin.y, bbox.pMin.z);
	p[17] = Point(bbox.pMax.x, bbox.pMin.y, bbox.pMin.z);
	p[18] = Point(bbox.pMax.x, bbox.pMin.y, bbox.pMax.z);
	p[19] = Point(bbox.pMin.x, bbox.pMin.y, bbox.pMax.z);
	// Side front
	p[20] = Point(bbox.pMin.x, bbox.pMax.y, bbox.pMin.z);
	p[21] = Point(bbox.pMin.x, bbox.pMax.y, bbox.pMax.z);
	p[22] = Point(bbox.pMax.x, bbox.pMax.y, bbox.pMax.z);
	p[23] = Point(bbox.pMax.x, bbox.pMax.y, bbox.pMin.z);

	Triangle *vi = (Triangle *)Scene::AllocTrianglesBuffer(12);
	// Bottom face
	vi[0] = Triangle(0, 1, 2);
	vi[1] = Triangle(2, 3, 0);
	// Top face
	vi[2] = Triangle(4, 5, 6);
	vi[3] = Triangle(6, 7, 4);
	// Side left
	vi[4] = Triangle(8, 9, 10);
	vi[5] = Triangle(10, 11, 8);
	// Side right
	vi[6] = Triangle(12, 13, 14);
	vi[7] = Triangle(14, 15, 12);
	// Side back
	vi[8] = Triangle(16, 17, 18);
	vi[9] = Triangle(18, 19, 16);
	// Side back
	vi[10] = Triangle(20, 21, 22);
	vi[11] = Triangle(22, 23, 20);

	// Define the Mesh
	if (!enableUV) {
		// Define the object
		scene->DefineMesh(meshName, 24, 12, (float *)p, (unsigned int *)vi, NULL, NULL, NULL, NULL);
	} else {
		UV *uv = new UV[24];
		// Bottom face
		uv[0] = UV(0.f, 0.f);
		uv[1] = UV(1.f, 0.f);
		uv[2] = UV(1.f, 1.f);
		uv[3] = UV(0.f, 1.f);
		// Top face
		uv[4] = UV(0.f, 0.f);
		uv[5] = UV(1.f, 0.f);
		uv[6] = UV(1.f, 1.f);
		uv[7] = UV(0.f, 1.f);
		// Side left
		uv[8] = UV(0.f, 0.f);
		uv[9] = UV(1.f, 0.f);
		uv[10] = UV(1.f, 1.f);
		uv[11] = UV(0.f, 1.f);
		// Side right
		uv[12] = UV(0.f, 0.f);
		uv[13] = UV(1.f, 0.f);
		uv[14] = UV(1.f, 1.f);
		uv[15] = UV(0.f, 1.f);
		// Side back
		uv[16] = UV(0.f, 0.f);
		uv[17] = UV(1.f, 0.f);
		uv[18] = UV(1.f, 1.f);
		uv[19] = UV(0.f, 1.f);
		// Side front
		uv[20] = UV(0.f, 0.f);
		uv[21] = UV(1.f, 0.f);
		uv[22] = UV(1.f, 1.f);
		uv[23] = UV(0.f, 1.f);

		// Define the object
		scene->DefineMesh(meshName, 24, 12, (float *)p, (unsigned int *)vi, NULL, (float *)uv, NULL, NULL);
	}

	// Add the object to the scene
	Properties props;
	props.SetFromString(
		"scene.objects." + objName + ".shape = " + meshName + "\n"
		"scene.objects." + objName + ".material = " + matName + "\n"
		);
	scene->Parse(props);
}

static void DoRendering(RenderSession *session) {
	const unsigned int haltTime = session->GetRenderConfig().GetProperties().Get(Property("batch.halttime")(0)).Get<unsigned int>();
	const unsigned int haltSpp = session->GetRenderConfig().GetProperties().Get(Property("batch.haltspp")(0)).Get<unsigned int>();

	char buf[512];
	const Properties &stats = session->GetStats();
	while (!session->HasDone()) {
		boost::this_thread::sleep(boost::posix_time::millisec(1000));

		session->UpdateStats();
		const double elapsedTime = stats.Get("stats.renderengine.time").Get<double>();
		const unsigned int pass = stats.Get("stats.renderengine.pass").Get<unsigned int>();
		const float convergence = stats.Get("stats.renderengine.convergence").Get<unsigned int>();

		// Print some information about the rendering progress
		sprintf(buf, "[Elapsed time: %3d/%dsec][Samples %4d/%d][Convergence %f%%][Avg. samples/sec % 3.2fM on %.1fK tris]",
				int(elapsedTime), int(haltTime), pass, haltSpp, 100.f * convergence,
				stats.Get("stats.renderengine.total.samplesec").Get<double>() / 1000000.0,
				stats.Get("stats.dataset.trianglecount").Get<double>() / 1000.0);

		LC_LOG(buf);
	}

	// Save the rendered image
	session->GetFilm().SaveOutputs();
}

int main(int argc, char *argv[]) {
	try {
		luxcore::Init();

		cout << "LuxCore " << LUXCORE_VERSION_MAJOR << "." << LUXCORE_VERSION_MINOR << "\n" ;

		//----------------------------------------------------------------------
		// Build the scene to render
		//----------------------------------------------------------------------

		Scene *scene = Scene::Create();

		// Setup the camera
		scene->Parse(
				Property("scene.camera.lookat.orig")(1.f , 6.f , 3.f) <<
				Property("scene.camera.lookat.target")(0.f , 0.f , .5f) <<
				Property("scene.camera.fieldofview")(60.f));

		// Define texture maps
		const unsigned int size = 500;
		vector<unsigned char> img(size * size * 3);
		unsigned char *ptr = &img[0];
		for (unsigned int y = 0; y < size; ++y) {
			for (unsigned int x = 0; x < size; ++x) {
				if ((x % 50 < 25) ^ (y % 50 < 25)) {
					*ptr++ = 255;
					*ptr++ = 0;
					*ptr++ = 0;
				} else {
					*ptr++ = 255;
					*ptr++ = 255;
					*ptr++ = 0;
				}
			}
		}

		scene->DefineImageMap<unsigned char>("check_texmap", &img[0], 1.f, 3, size, size, Scene::DEFAULT);
		scene->Parse(
			Property("scene.textures.map.type")("imagemap") <<
			Property("scene.textures.map.file")("check_texmap") <<
			Property("scene.textures.map.gamma")(1.f)
			);

		// Setup materials
		scene->Parse(
			Property("scene.materials.whitelight.type")("matte") <<
			Property("scene.materials.whitelight.emission")(1000000.f, 1000000.f, 1000000.f) <<
			Property("scene.materials.mat_white.type")("matte") <<
			Property("scene.materials.mat_white.kd")("map") <<
			Property("scene.materials.mat_red.type")("matte") <<
			Property("scene.materials.mat_red.kd")(0.75f, 0.f, 0.f) <<
			Property("scene.materials.mat_glass.type")("glass") <<
			Property("scene.materials.mat_glass.kr")(0.9f, 0.9f, 0.9f) <<
			Property("scene.materials.mat_glass.kt")(0.9f, 0.9f, 0.9f) <<
			Property("scene.materials.mat_glass.exteriorior")(1.f) <<
			Property("scene.materials.mat_glass.interiorior")(1.4f) <<
			Property("scene.materials.mat_gold.type")("metal2") <<
			Property("scene.materials.mat_gold.preset")("gold")
			);

		// Create the ground
		CreateBox(scene, "ground", "mesh-ground", "mat_white", true, BBox(Point(-3.f, -3.f, -.1f), Point(3.f, 3.f, 0.f)));
		// Create the red box
		CreateBox(scene, "box01", "mesh-box01", "mat_red", false, BBox(Point(-.5f, -.5f, .2f), Point(.5f, .5f, .7f)));
		// Create the glass box
		CreateBox(scene, "box02", "mesh-box02", "mat_glass", false, BBox(Point(1.5f, 1.5f, .3f), Point(2.f, 1.75f, 1.5f)));
		// Create the light
		CreateBox(scene, "box03", "mesh-box03", "whitelight", false, BBox(Point(-1.75f, 1.5f, .75f), Point(-1.5f, 1.75f, .5f)));
		// Create a monkey from ply-file
		Properties props;
		props.SetFromString(
			"scene.shapes.monkey.type = mesh\n"
			"scene.shapes.monkey.ply = samples/luxcorescenedemo/suzanne.ply\n"	// load the ply-file
			"scene.objects.monkey.shape = monkey\n"		// set shape
			"scene.objects.monkey.material = mat_gold\n"		// set material
			"scene.objects.monkey.transformation = \
						0.4 0.0 0.0 0.0 \
						0.0 0.4 0.0 0.0 \
						0.0 0.0 0.4 0.0 \
					    0.0 2.0 0.3 1.0\n"						//scale and translate
			);
		scene->Parse(props);

		// Create a SkyLight & SunLight
		scene->Parse(
				Property("scene.lights.skyl.type")("sky2") <<
				Property("scene.lights.skyl.dir")(0.166974f, 0.59908f, 0.783085f) <<
				Property("scene.lights.skyl.turbidity")(2.2f) <<
				Property("scene.lights.skyl.gain")(0.8f, 0.8f, 0.8f) <<
				Property("scene.lights.sunl.type")("sun") <<
				Property("scene.lights.sunl.dir")(0.166974f, 0.59908f, 0.783085f) <<
				Property("scene.lights.sunl.turbidity")(2.2f) <<
				Property("scene.lights.sunl.gain")(0.8f, 0.8f, 0.8f)
				);

		//----------------------------------------------------------------------
		// Do the render
		//----------------------------------------------------------------------

		RenderConfig *config = RenderConfig::Create(
				Property("renderengine.type")("PATHCPU") <<
				Property("sampler.type")("RANDOM") <<
				Property("opencl.platform.index")(-1) <<
				Property("opencl.cpu.use")(false) <<
				Property("opencl.gpu.use")(true) <<
				Property("batch.halttime")(10.f) <<
				Property("film.outputs.1.type")("RGB_IMAGEPIPELINE") <<
				Property("film.outputs.1.filename")("image.png"),
				scene);
		RenderSession *session = RenderSession::Create(config);

		//----------------------------------------------------------------------
		// Start the rendering
		//----------------------------------------------------------------------

		session->Start();

		DoRendering(session);
		boost::filesystem::rename("image.png", "image0.png");

		//----------------------------------------------------------------------
		// Edit a texture
		//----------------------------------------------------------------------

		LC_LOG("Editing a texture...");
		session->BeginSceneEdit();
		scene->Parse(
			Property("scene.textures.map.type")("constfloat3") <<
			Property("scene.textures.map.value")(0.f, 0.f, 1.f));
		session->EndSceneEdit();

		// And redo the rendering
		DoRendering(session);
		boost::filesystem::rename("image.png", "image1.png");

		//----------------------------------------------------------------------
		// Edit a material
		//----------------------------------------------------------------------

		LC_LOG("Editing a material...");
		session->BeginSceneEdit();
		scene->Parse(
			Property("scene.materials.mat_white.type")("mirror") <<
			Property("scene.materials.mat_white.kr")(.9f, .9f, .9f));
		session->EndSceneEdit();

		// And redo the rendering
		DoRendering(session);
		boost::filesystem::rename("image.png", "image2.png");

		//----------------------------------------------------------------------
		// Edit an object
		//----------------------------------------------------------------------

		LC_LOG("Editing a material and an object...");
		session->BeginSceneEdit();

		scene->Parse(
			Property("scene.materials.mat_white.type")("matte") <<
			Property("scene.materials.mat_white.kr")(.7f, .7f, .7f));
		CreateBox(scene, "box03", "mesh-box03", "mat_red", false, BBox(Point(-2.75f, 1.5f, .75f), Point(-.5f, 1.75f, .5f)));

		// Translate the monkey
		const float mat[16] = {
			.4f, 0.f, 0.f, 0.f,
			0.f, .4f, 0.f, 0.f,
			0.f, 0.f, .4f, 0.f,
			1.f, 2.f, .3f, 1.f
		};
		scene->UpdateObjectTransformation("monkey", mat);

		session->EndSceneEdit();

		// And redo the rendering
		DoRendering(session);
		boost::filesystem::rename("image.png", "image3.png");

		//----------------------------------------------------------------------
		// Add a strands object
		//----------------------------------------------------------------------

		LC_LOG("Adding a strands object...");
		session->BeginSceneEdit();

		{
			const string fileName = "scenes/strands/straight.hair";
			luxrays::cyHairFile strandsFile;
			const int strandsCount = strandsFile.LoadFromFile(fileName.c_str());
			if (strandsCount <= 0)
				throw runtime_error("Unable to read strands file: " + fileName);
			scene->DefineStrands("hairs_shape", strandsFile, Scene::TESSEL_RIBBON,
					0, 0.f, 0, false, false, true);

			// Add the object to the scene
			Properties props;
			props.SetFromString(
				"scene.objects.hairs_obj.shape = hairs_shape\n"
				"scene.objects.hairs_obj.material = mat_white\n"
				"scene.objects.hairs_obj.transformation = 0.01 0.0 0.0 0.0  0.0 0.01 0.0 0.0  0.0 0.0 0.01 0.0  -1.5 0.0 0.3 1.0\n"
				);
			scene->Parse(props);
		}

		session->EndSceneEdit();

		// And redo the rendering
		DoRendering(session);
		boost::filesystem::rename("image.png", "image4.png");

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

		// Stop the rendering
		session->Stop();

		delete session;
		delete config;
		delete scene;

		LC_LOG("Done.");
	} catch (runtime_error &err) {
		LC_LOG("RUNTIME ERROR: " << err.what());
		return EXIT_FAILURE;
	} catch (exception &err) {
		LC_LOG("ERROR: " << err.what());
		return EXIT_FAILURE;
	}

	return EXIT_SUCCESS;
}


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include <boost/test/unit_test.hpp>
#include "armnnTfParser/ITfParser.hpp"
#include "ParserPrototxtFixture.hpp"

BOOST_AUTO_TEST_SUITE(TensorflowParser)

struct DivisionFixture : public armnnUtils::ParserPrototxtFixture<armnnTfParser::ITfParser>
{
    DivisionFixture()
    {
        m_Prototext = "node { \n"
                      "    name: \"graphInput\" \n"
                      "    op: \"Placeholder\" \n"
                      "    attr { \n"
                      "      key: \"dtype\" \n"
                      "      value { \n"
                      "        type: DT_FLOAT \n"
                      "      } \n"
                      "    } \n"
                      "    attr { \n"
                      "      key: \"shape\" \n"
                      "      value { \n"
                      "        shape { \n"
                      "        } \n"
                      "      } \n"
                      "    } \n"
                      "  } \n"
                      "  node { \n"
                      "    name: \"softmax1\" \n"
                      "    op: \"Softmax\" \n"
                      "    input: \"graphInput\" \n"
                      "    attr { \n"
                      "      key: \"T\" \n"
                      "      value { \n"
                      "        type: DT_FLOAT \n"
                      "      } \n"
                      "    } \n"
                      "  }\n"
                      "  node {\n"
                      "    name: \"softmax2\"\n"
                      "    op : \"Softmax\"\n"
                      "    input: \"graphInput\"\n"
                      "    attr { \n"
                      "      key: \"T\" \n"
                      "      value { \n"
                      "        type: DT_FLOAT \n"
                      "      } \n"
                      "    } \n"
                      "  }\n"
                      "  node {\n"
                      "    name: \"division\"\n"
                      "    op : \"RealDiv\"\n"
                      "    input: \"softmax1\"\n"
                      "    input: \"softmax2\"\n"
                      "    attr { \n"
                      "      key: \"T\" \n"
                      "      value { \n"
                      "        type: DT_FLOAT \n"
                      "      } \n"
                      "    } \n"
                      "  }\n";

        SetupSingleInputSingleOutput({ 4, 1 }, "graphInput", "division");
    }
};

BOOST_FIXTURE_TEST_CASE(ParseDivision, DivisionFixture)
{
    RunTest<2>({ 2, 1.0f, 3, 1 }, { 1, 1.0f, 1, 1});
}

struct DivisionBroadcastFixture : public armnnUtils::ParserPrototxtFixture<armnnTfParser::ITfParser>
{
    DivisionBroadcastFixture(const armnn::TensorShape& inputShape0, const armnn::TensorShape& inputShape1)
    {
        m_Prototext = R"(
                 node {
                   name: "input0"
                   op: "Placeholder"
                   attr {
                     key: "dtype"
                     value {
                       type: DT_FLOAT
                     }
                   }
                   attr {
                     key: "shape"
                     value {
                       shape {
                       }
                     }
                   }
                 }
                 node {
                   name: "input1"
                   op: "Placeholder"
                   attr {
                     key: "dtype"
                     value {
                       type: DT_FLOAT
                     }
                   }
                   attr {
                     key: "shape"
                     value {
                       shape {
                       }
                     }
                   }
                 }
                 node {
                   name: "output"
                   op: "RealDiv"
                   input: "input0"
                   input: "input1"
                   attr {
                     key: "T"
                     value {
                       type: DT_FLOAT
                     }
                   }
                 }
                 )";

        Setup({ { "input0", inputShape0 },
                { "input1", inputShape1 } },
              { "output" });
    }
};
struct DivisionBroadcastFixture4D1D : public DivisionBroadcastFixture
{
    DivisionBroadcastFixture4D1D() : DivisionBroadcastFixture({ 1, 2, 2, 3 }, { 1 }) {}
};

BOOST_FIXTURE_TEST_CASE(ParseDivisionBroadcast4D1D, DivisionBroadcastFixture4D1D)
{
    RunTest<4>({ { "input0", { 0.0f, 100.0f, 2.0f,
                               3.0f, 250.0f, 15.0f,
                               33.0f, 60.0f, 5.0f,
                               35.0f, 10.0f, 55.0f } },
                 { "input1", { 5.0f } } },
               { { "output", { 0, 20.0f, 0.4f,
                               0.6f, 50.0f, 3.0f,
                               6.6f, 12.0f, 1.0f,
                               7.0f, 2.0f, 11.0f } } });
}

BOOST_FIXTURE_TEST_CASE(ParseDivideByZeroBroadcast4D1D, DivisionBroadcastFixture4D1D)
{
    float Inf = std::numeric_limits<float>::infinity();
    float NaN = std::numeric_limits<float>::quiet_NaN();

    RunTest<4>({ { "input0", { 0.0f,  -100.0f,  2.0f,
                               3.0f,  -250.0f,  15.0f,
                               33.0f,  -0,  5.0f,
                               35.0f, -10.0f, 55.0f } },
                 { "input1", { 0 } } },
               { { "output", { NaN, -Inf, Inf,
                               Inf, -Inf, Inf,
                               Inf, NaN, Inf,
                               Inf, -Inf, Inf } } });
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright (c) 2014-2016 The Bitcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "chainparams.h"
#include "validation/validation.h"
#include "net.h"
#include "unity/signals.h"

#include "test/test.h"

#include <boost/signals2/signal.hpp>
#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(main_tests, TestingSetup)

BOOST_AUTO_TEST_CASE(block_subsidy_test)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    uint64_t nP4First = chainParams->GetConsensus().pow2Phase4FirstBlockHeight;
    BOOST_CHECK_EQUAL(GetBlockSubsidy(0).total,             (994'744'000*COIN) + (10*CENT));
    BOOST_CHECK_EQUAL(GetBlockSubsidy(1000).total,          10*CENT*2);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(1000).witness,        10*CENT);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(99999).total,         10*CENT*2);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(99999).witness,       10*CENT);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(100000).total,        (10*CENT));
    BOOST_CHECK_EQUAL(GetBlockSubsidy(100000).witness,      (75*CENT/10));
    BOOST_CHECK_EQUAL(GetBlockSubsidy(100001).total,        (10*CENT));
    BOOST_CHECK_EQUAL(GetBlockSubsidy(100001).witness,      (75*CENT/10));
    BOOST_CHECK_EQUAL(GetBlockSubsidy(499999).total,        (10*CENT));
    BOOST_CHECK_EQUAL(GetBlockSubsidy(500000).total,        (10*CENT)/2);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(500001).total,        (10*CENT)/2);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(899999).total,        (10*CENT)/2);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(900000).total,        (10*CENT)/2/2);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(900001).total,        (10*CENT)/2/2);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(1299999).total,       (10*CENT)/2/2);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(1300000).total,       (10*CENT)/2/2/2);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(1300001).total,       (10*CENT)/2/2/2);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(9300001).total,       0);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(12000000).total,      0);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(42000000).total,      0);

}

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    CAmount nSumTotal = 0;
    CAmount nSumTotalMining = 0;
    CAmount nSumTotalWitness = 0;
    for (int nHeight = 0; nHeight < 9300001; nHeight++)
    {
        CAmount nSubsidy = GetBlockSubsidy(nHeight).total;
        nSumTotal += nSubsidy;
        BOOST_CHECK(MoneyRange(nSumTotal));
        CAmount nSubsidyWitness = GetBlockSubsidy(nHeight).witness;
        nSumTotalWitness += nSubsidyWitness;
        BOOST_CHECK(MoneyRange(nSumTotalWitness));
    }
    BOOST_CHECK_EQUAL(nSumTotal, 99484399982800000LL);
    BOOST_CHECK_EQUAL(nSumTotalWitness, 6999996000000LL);
}

bool ReturnFalse() { return false; }
bool ReturnTrue() { return true; }

BOOST_AUTO_TEST_CASE(test_combiner_all)
{
    boost::signals2::signal<bool (), BooleanAndAllReturnValues> Test;
    BOOST_CHECK(Test());
    Test.connect(&ReturnFalse);
    BOOST_CHECK(!Test());
    Test.connect(&ReturnTrue);
    BOOST_CHECK(!Test());
    Test.disconnect(&ReturnFalse);
    BOOST_CHECK(Test());
    Test.disconnect(&ReturnTrue);
    BOOST_CHECK(Test());
}
BOOST_AUTO_TEST_SUITE_END()


#include <string>
#include <vector>
#include <math.h>
#include <gtest/gtest.h>
#include <ros/ros.h>
#include <tf/tf.h>
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <ros/package.h>

#include <cslibs_kdl/dynamic_model.h>
#include <cslibs_kdl/kdl_conversion.h>

using namespace cslibs_kdl;

DynamicModel jaco2KDL;

TEST(Jaco2KinematicsDynamicsToolTests, converttest)
{
    KDL::JntArray q(6);
    std::vector<double> qvec;
    for(int i = 0; i< 6; ++i){
        q(i) = i;
    }
    convert(q,qvec);
    for(int i =0; i<6; ++i){
        EXPECT_EQ(q(i),qvec[i]);
    }
    KDL::JntArray q2;
    convert(qvec,q2);
    for(int i =0; i<6; ++i){
        EXPECT_EQ(q2(i),qvec[i]);
    }
}

TEST(Jaco2KinematicsDynamicsToolTests, chaintest)
{
    EXPECT_TRUE(jaco2KDL.getRootLink().find("jaco_link_base")!=std::string::npos);
    EXPECT_TRUE(jaco2KDL.getTipLink().find("jaco_link_hand")!=std::string::npos);
    EXPECT_EQ(jaco2KDL.getNrOfJoints(),6);
    EXPECT_EQ(jaco2KDL.getNrOfSegments(),6);
    EXPECT_EQ(jaco2KDL.getKDLSegmentIndex("jaco_link_base"), -1);
    EXPECT_EQ(jaco2KDL.getKDLSegmentIndex("jaco_link_1"),0);
    EXPECT_EQ(jaco2KDL.getKDLSegmentIndex("jaco_link_5"),4);
    EXPECT_EQ(jaco2KDL.getKDLSegmentIndex("jaco_link_hand"), 5);
    EXPECT_EQ(jaco2KDL.getKDLSegmentIndexFK("jaco_link_5"), 5);
}

TEST(Jaco2KinematicsDynamicsToolTests, DynParam)
{
    Eigen::Vector3d comLink1 = jaco2KDL.getLinkCoM("jaco_link_1");
    EXPECT_NEAR(comLink1(0), 0, 1e-4);
    EXPECT_NEAR(comLink1(1),  0.0086, 1e-4);
    EXPECT_NEAR(comLink1(2), -0.1064, 1e-4);
    EXPECT_NEAR(jaco2KDL.getLinkMass("jaco_link_1"),0.7417, 1e-4);
    Eigen::Matrix3d inertia = jaco2KDL.getLinkInertia("jaco_link_1");
    EXPECT_NEAR(inertia(0,0), 0.0094, 1e-4);
    EXPECT_NEAR(inertia(0,1), 0.0000, 1e-4);
    EXPECT_NEAR(inertia(0,2), 0.0000, 1e-4);
    EXPECT_NEAR(inertia(1,1), 0.0522, 1e-4);
    EXPECT_NEAR(inertia(1,2), 0.0006, 1e-4);
    EXPECT_NEAR(inertia(2,2), 0.0004, 1e-4);

    Eigen::Vector3d comLink3 = jaco2KDL.getLinkCoM("jaco_link_3");
    EXPECT_NEAR(comLink3(0),  0.1624, 1e-4);
    EXPECT_NEAR(comLink3(1),  0.0000, 1e-4);
    EXPECT_NEAR(comLink3(2), -0.0135, 1e-4);
    EXPECT_NEAR(jaco2KDL.getLinkMass("jaco_link_3"), 0.7847, 1e-4);
    inertia = jaco2KDL.getLinkInertia("jaco_link_3");
    EXPECT_NEAR(inertia(0,0),  0.0004, 1e-4);
    EXPECT_NEAR(inertia(0,1),  0.0000, 1e-4);
    EXPECT_NEAR(inertia(0,2),  0.0015, 1e-4);
    EXPECT_NEAR(inertia(1,1),  0.0243, 1e-4);
    EXPECT_NEAR(inertia(1,2),  0.0000, 1e-4);
    EXPECT_NEAR(inertia(2,2),  0.0243, 1e-4);

}

TEST(Jaco2KinematicsDynamicsToolTests, KinParam)
{
    Eigen::Vector3d trans = jaco2KDL.getLinkFixedTranslation("jaco_link_1");
    Eigen::Matrix3d rot = jaco2KDL.getLinkFixedRotation("jaco_link_1");
    EXPECT_NEAR(trans(0), 0, 1e-4);
    EXPECT_NEAR(trans(1), 0, 1e-4);
    EXPECT_NEAR(trans(2), 0.1535, 1e-4);
    EXPECT_NEAR(rot(0,0),  1, 1e-4);
    EXPECT_NEAR(rot(0,1),  0, 1e-4);
    EXPECT_NEAR(rot(0,2),  0, 1e-4);
    EXPECT_NEAR(rot(1,0),  0, 1e-4);
    EXPECT_NEAR(rot(1,1), -1, 1e-4);
    EXPECT_NEAR(rot(1,2),  0, 1e-4);
    EXPECT_NEAR(rot(2,0),  0, 1e-4);
    EXPECT_NEAR(rot(2,1),  0, 1e-4);
    EXPECT_NEAR(rot(2,2), -1, 1e-4);
}

//TEST(Jaco2KinematicsDynamicsToolTests, robotModleFileTest)
//{
//   std::string file = "/home/zwiener/workspace/jaco_ws/src/jaco2_ros/jaco2_description/robots/standalone_arm.urdf";

//   Jaco2KinematicModel kinmodel(file, "jaco_link_base","jaco_link_hand");
//   std::vector<std::string> names = kinmodel.getLinkNames();
//   EXPECT_EQ(names.size(), 6);
//}

TEST(Jaco2KinematicsDynamicsToolTests, getRotationAxisTest)
{
    Eigen::Vector3d j1;
    jaco2KDL.getRotationAxis("jaco_link_1", j1);
    EXPECT_NEAR(j1(0), 0, 1e-8);
    EXPECT_NEAR(j1(1), 0, 1e-8);
    EXPECT_NEAR(j1(2), 1, 1e-8);
    Eigen::Vector3d j2;
    jaco2KDL.getRotationAxis("jaco_link_2", j2);
    EXPECT_NEAR(j2(0), 0, 1e-8);
    EXPECT_NEAR(j2(1), 0, 1e-8);
    EXPECT_NEAR(j2(2), 1, 1e-8);
    Eigen::Vector3d j3;
    jaco2KDL.getRotationAxis("jaco_link_3", j3);
    EXPECT_NEAR(j3(0), 0, 1e-8);
    EXPECT_NEAR(j3(1), 0, 1e-8);
    EXPECT_NEAR(j3(2), 1, 1e-8);
    Eigen::Vector3d j4;
    jaco2KDL.getRotationAxis("jaco_link_4", j4);
    EXPECT_NEAR(j4(0), 0, 1e-8);
    EXPECT_NEAR(j4(1), 0, 1e-8);
    EXPECT_NEAR(j4(2), 1, 1e-8);
    Eigen::Vector3d j5;
    jaco2KDL.getRotationAxis("jaco_link_5", j5);
    EXPECT_NEAR(j5(0), 0, 1e-8);
    EXPECT_NEAR(j5(1), 0, 1e-8);
    EXPECT_NEAR(j5(2), 1, 1e-8);
    Eigen::Vector3d j6;
    jaco2KDL.getRotationAxis("jaco_link_hand", j6);
    EXPECT_NEAR(j6(0), 0, 1e-8);
    EXPECT_NEAR(j6(1), 0, 1e-8);
    EXPECT_NEAR(j6(2), 1, 1e-8);
}

TEST(Jaco2DynamicsTests,inverseDynamics)
{
    std::vector<double> q = {0, M_PI, M_PI, 0, 0, M_PI};
    std::vector<double> qDot;
    std::vector<double> torques;
    qDot.resize(6,0);
    torques.resize(6,0);
    int ec = jaco2KDL.getTorques(q,qDot,qDot,torques);
    EXPECT_TRUE(ec>=0);
    for(int i = 0; i <6; ++i)
    {
        EXPECT_NEAR(torques[i], 0, 1e-4);
        //        std::cout << "torques(" << i <<") = " << torques[i] <<  std::endl;
    }
    std::vector<double> q2 = {4.776098185246001, 2.9779051856135985, 1.0370221453036228, -2.089493953881068, 1.3567380962770526, 1.3811624792663872};
    std::vector<double> qDot2;
    qDot2.resize(6,0.1);
    ec = jaco2KDL.getTorques(q2,qDot,qDot,torques);
    for(int i = 1; i <6; ++i)
    {
        EXPECT_TRUE(fabs(torques[i]) > 1e-4);
    }
}

TEST(Jaco2KinematicsTests, fk)
{
    std::vector<double> q = {0, M_PI, M_PI, 0, 0, M_PI};
    tf::Vector3 posH(-0.000, 0.0629, 1.018);
    tf::Quaternion rotH(-0.707, 0.707, -0.000, 0.000);
    tf::Pose res;
    int ec = jaco2KDL.getFKPose(q,res,"jaco_link_hand");
    EXPECT_TRUE(ec>=0);
    EXPECT_NEAR(posH.getX(), res.getOrigin().getX(), 1e-3);
    EXPECT_NEAR(posH.getY(), res.getOrigin().getY(), 1e-3);
    EXPECT_NEAR(posH.getZ(), res.getOrigin().getZ(), 1e-3);
    EXPECT_NEAR(rotH.getX(), res.getRotation().getX(), 1e-3);
    EXPECT_NEAR(rotH.getY(), res.getRotation().getY(), 1e-3);
    EXPECT_NEAR(rotH.getZ(), res.getRotation().getZ(), 1e-3);
    EXPECT_NEAR(rotH.getW(), res.getRotation().getW(), 1e-3);

    tf::Vector3 pos5(-0.000, 0.024, 0.9549);
    tf::Quaternion rot5(-0.612, 0.612, -0.354, -0.354);
    tf::Pose res5;
    ec = jaco2KDL.getFKPose(q,res5,"jaco_link_5");
    EXPECT_TRUE(ec>=0);
    EXPECT_NEAR(pos5.getX(), res5.getOrigin().getX(), 1e-3);
    EXPECT_NEAR(pos5.getY(), res5.getOrigin().getY(), 1e-3);
    EXPECT_NEAR(pos5.getZ(), res5.getOrigin().getZ(), 1e-3);
    EXPECT_NEAR(rot5.getX(), res5.getRotation().getX(), 1e-3);
    EXPECT_NEAR(rot5.getY(), res5.getRotation().getY(), 1e-3);
    EXPECT_NEAR(rot5.getZ(), res5.getRotation().getZ(), 1e-3);
    EXPECT_NEAR(rot5.getW(), res5.getRotation().getW(), 1e-3);
    KDL::Frame T1;
    ec = jaco2KDL.getFKPose(q,T1,"jaco_link_1");
    EXPECT_TRUE(ec>=0);
}

TEST(Jaco2DynamicsToolsTests, changeDynParam)
{
    std::string linkName("jaco_link_hand");
    Eigen::Vector3d com(1.1, 0.1, 0.1);
    Eigen::Matrix3d mat;
    mat << 1, 2, 3,
            4, 5, 6,
            7, 8, 9;
    jaco2KDL.changeDynamicParams(linkName,jaco2KDL.getLinkMass(linkName), com, mat);
    double mass = jaco2KDL.getLinkMass(linkName);
    Eigen::Vector3d v = jaco2KDL.getLinkCoM(linkName);
    Eigen::Matrix3d mat2 = jaco2KDL.getLinkInertia(linkName);
    EXPECT_NEAR(v(0),com(0), 1e-6);
    EXPECT_NEAR(v(1),com(1), 1e-6);
    EXPECT_NEAR(v(2),com(2), 1e-6);
    EXPECT_EQ(mat2(0,0), mat(0,0) + mass*(com(1)*com(1) + com(2) * com(2)));
    EXPECT_EQ(mat2(0,1), mat(0,1) - mass*com(0)*com(1));
    EXPECT_EQ(mat2(0,2), mat(0,2) - mass*com(0)*com(2));
    EXPECT_EQ(mat2(1,1), mat(1,1) + mass*(com(0)*com(0) + com(2) * com(2)));
    EXPECT_EQ(mat2(1,2), mat(1,2) - mass*com(1)*com(2));
    EXPECT_EQ(mat2(2,2), mat(2,2) + mass*(com(0)*com(0) + com(1) * com(1)));

    Eigen::Matrix3d mat3 = jaco2KDL.getLinkInertiaCoM(linkName);
    EXPECT_EQ(mat3(0,0), mat(0,0));
    EXPECT_EQ(mat3(0,1), mat(0,1));
    EXPECT_EQ(mat3(0,2), mat(0,2));
    EXPECT_EQ(mat3(1,1), mat(1,1));
    EXPECT_EQ(mat3(1,2), mat(1,2));
    EXPECT_EQ(mat3(2,2), mat(2,2));

    com = Eigen::Vector3d(1,2,3);
    std::vector<double> torques,t2;
    std::vector<double> qDot = {0.8, 0.1, 0.1, 0.1, 0.2, 0.1};
    torques.resize(6,0);
    std::vector<double> q2 = {4.776098185246001, 2.9779051856135985, 1.0370221453036228, -2.089493953881068, 1.3567380962770526, 1.3811624792663872};
    int  ec = jaco2KDL.getTorques(q2,qDot,qDot,torques);
    jaco2KDL.useUrdfDynamicParams();
    ec = jaco2KDL.getTorques(q2,qDot,qDot,t2);
    for(int i = 0; i <6; ++i)
    {
        //        std::cout << "t2(" << i <<") = " << t2[i] << " | torques(" << i <<") = " << torques[i] <<  std::endl;
        EXPECT_TRUE(t2[i] != torques[i]);
    }

}

TEST(Jaco2KinematicsTests, IK)
{
    try{
        std::vector<double> zero;
        std::vector<double> jointAngles  = {0, M_PI, M_PI, 0, 0, M_PI};
        zero.resize(6,0);
        int nrTests = 10000;
        int fails = 0;
        for(int i = 0; i < nrTests; ++i){
            tf::Pose fk_pose;
            jaco2KDL.getRandomConfig(jointAngles);
            int ec = jaco2KDL.getFKPose(jointAngles,fk_pose,"jaco_link_hand");
            EXPECT_TRUE(ec>=0);
            if(ec>=0)
            {
                std::vector<double> ik_solution;
                int ecIK = jaco2KDL.getIKSolution(fk_pose,ik_solution,zero);
                if(ecIK < 0){
                    //                geometry_msgs::Pose msg;
                    //                tf::poseTFToMsg(fk_pose,msg);
                    //                std::cout << "number of test" << i<< std::endl << "Pose: " << msg << std::endl;

                    //                for(auto phi : jointAngles) {
                    //                    std::cout << phi << std::endl;
                    //                }
                    ++fails;
                }
                //            EXPECT_TRUE(ecIK >= 0);
                if(ecIK >= 0){
                    tf::Pose ik_pose;
                    ecIK = jaco2KDL.getFKPose(ik_solution,ik_pose,"jaco_link_hand");
                    EXPECT_NEAR(ik_pose.getOrigin().getX(), fk_pose.getOrigin().getX(), 1e-3);
                    EXPECT_NEAR(ik_pose.getOrigin().getY(), fk_pose.getOrigin().getY(), 1e-3);
                    EXPECT_NEAR(ik_pose.getOrigin().getZ(), fk_pose.getOrigin().getZ(), 1e-3);
                    EXPECT_NEAR(ik_pose.getRotation().getX(), fk_pose.getRotation().getX(), 1e-3);
                    EXPECT_NEAR(ik_pose.getRotation().getY(), fk_pose.getRotation().getY(), 1e-3);
                    EXPECT_NEAR(ik_pose.getRotation().getZ(), fk_pose.getRotation().getZ(), 1e-3);
                    EXPECT_NEAR(ik_pose.getRotation().getW(), fk_pose.getRotation().getW(), 1e-3);
                }
            }
        }
        double successRate = 1.0 - ((double)fails)/((double)nrTests);
        EXPECT_NEAR(successRate,0.99, 1e-2);
        std::cout << "success rate: " <<  successRate << std::endl;
    } catch(const std::exception& e){
        std::cout << e.what() << std::endl;
    }
}

TEST(Jaco2DynamicsToolsTests, kdlEigenConversion)
{
    KDL::Vector vec1(1,2,3);
    KDL::Vector vec2(3,4,5);
    KDL::Vector kdlcross = vec1*vec2;

    Eigen::Vector3d vec2_eigen(3,4,5);
    Eigen::Matrix3d mat = skewSymMat(vec1);
    Eigen::Vector3d crossprod = mat * vec2_eigen;

    KDL::Rotation r(0.707107,0.707107,0,
                    0,0,1,
                    0.707107,-0.707107,0);

    KDL::Vector rot_vec2 = r * vec2;

    Eigen::Vector3d rot_vec2_e= convert2Eigen(r) * vec2_eigen;

    KDL::Vector t(0.1,0.2,0.3);
    KDL::Frame frame(r,t);
    KDL::Twist twist(vec1, vec2);
    KDL::Twist twist_trans = frame*twist;

    Eigen::Matrix<double, 6, 1> vec1_spatial;
    vec1_spatial << vec2(0), vec2(1), vec2(2), vec1(0), vec1(1), vec1(2);
    Eigen::Matrix<double, 6, 6> eframe = convert2EigenTwistTransform(frame);
    Eigen::Matrix<double, 6, 1> etrans = eframe *vec1_spatial;

    KDL::Wrench w_t = frame * KDL::Wrench(vec1,vec2);
    Eigen::Matrix<double, 6, 1> wrench;
    wrench << vec2(0), vec2(1), vec2(2), vec1(0), vec1(1), vec1(2);
    KDL::Frame i_frame = frame.Inverse();
    Eigen::Matrix<double, 6, 1> wrench_t = convert2EigenTwistTransform(i_frame).transpose() * wrench;
    Eigen::Matrix<double, 6, 1> wrench_t2 = convert2EigenWrenchTransform(frame) * wrench;

    Eigen::Matrix<double, 3, 6> in_prod = inertiaProductMat(KDL::Vector(1,2,3));

    for(int i = 0; i < 3; ++i) {
        EXPECT_EQ(kdlcross(i), crossprod(i));
        EXPECT_EQ(rot_vec2(i), rot_vec2_e(i));
        EXPECT_NEAR(twist_trans.rot(i), etrans(i), 1e-6);
        EXPECT_NEAR(twist_trans.vel(i), etrans(i+3), 1e-6);
        EXPECT_NEAR(w_t.torque(i), wrench_t(i), 1e-6);
        EXPECT_NEAR(w_t.force(i), wrench_t(i+3), 1e-6);
        EXPECT_NEAR(w_t.torque(i), wrench_t2(i), 1e-6);
        EXPECT_NEAR(w_t.force(i), wrench_t2(i+3), 1e-6);
        EXPECT_EQ(in_prod(0,i+3),0);
    }
    EXPECT_EQ(in_prod(0,0),1);
    EXPECT_EQ(in_prod(0,1),2);
    EXPECT_EQ(in_prod(0,2),3);
    EXPECT_EQ(in_prod(1,0),0);
    EXPECT_EQ(in_prod(1,1),1);
    EXPECT_EQ(in_prod(1,2),0);
    EXPECT_EQ(in_prod(1,3),2);
    EXPECT_EQ(in_prod(1,4),3);
    EXPECT_EQ(in_prod(1,5),0);
    EXPECT_EQ(in_prod(2,0),0);
    EXPECT_EQ(in_prod(2,1),0);
    EXPECT_EQ(in_prod(2,2),1);
    EXPECT_EQ(in_prod(2,3),0);
    EXPECT_EQ(in_prod(2,4),2);
    EXPECT_EQ(in_prod(2,5),3);
}

TEST(Jaco2DynamicsTests, getRigidBodyRegressionMatrix)
{
    //    std::vector<double> q = {4.77, 3.14, 3.14, 0.0, 0, 3.14};
    std::vector<double> q = {4.74, 2.96, 1.04, -2.08, 0.37, 1.37};
    //    std::vector<double> qDot = {0.1, 0.04, 0.05, 0.06, 0.03, 0.1};
    //    std::vector<double> qDotDot = {0.1, 0, 0, 0, -0.1, 0.4};
    std::vector<double> qDot = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    std::vector<double> qDotDot = {0.0, 0, 0, 0, 0.0, 0.0};
    std::vector<double> torque;

    int num_stat_tests = 100;
    double accuracy = 1e-9;

    Eigen::Matrix<double, 1, 10> matrix;
    double tau6;
    Eigen::Matrix<double, 2, 20> mat56;
    Eigen::Matrix<double, 2, 1> tau56;
    Eigen::Matrix<double, 60, 1> param_vec;
    Eigen::Matrix<double, 6, 60> full_matrix;
    Eigen::Matrix<double, 6, 1> tau_full;

    jaco2KDL.useUrdfDynamicParams();
    int id = 0;
    for(auto link : jaco2KDL.getLinkNames()) {
        param_vec(id) = jaco2KDL.getLinkMass(link);
        Eigen::Vector3d mc = jaco2KDL.getLinkMass(link) * jaco2KDL.getLinkCoM(link);
        param_vec(id+1) = mc(0);
        param_vec(id+2) = mc(1);
        param_vec(id+3) = mc(2);
        param_vec(id+4) = jaco2KDL.getLinkInertia(link)(0,0);
        param_vec(id+5) = jaco2KDL.getLinkInertia(link)(0,1);
        param_vec(id+6) = jaco2KDL.getLinkInertia(link)(0,2);
        param_vec(id+7) = jaco2KDL.getLinkInertia(link)(1,1);
        param_vec(id+8) = jaco2KDL.getLinkInertia(link)(1,2);
        param_vec(id+9) = jaco2KDL.getLinkInertia(link)(2,2);
        id += 10;
    }

    Eigen::Matrix<double, 10,1> param_vec6 = Eigen::Matrix<double, 10, 1>::Zero();
    param_vec6 = param_vec.block<10,1>(50,0);

    double gx,gy,gz;
    jaco2KDL.getGravity(gx, gy, gz);

    for(int i = 0; i < num_stat_tests; ++i ) {
        jaco2KDL.getRandomConfig(q);

        jaco2KDL.getTorques(q, qDot, qDotDot, torque);

        matrix = jaco2KDL.getRigidBodyRegressionMatrix("jaco_link_hand", "jaco_link_hand",q, qDot, qDotDot, gx, gy, gz);
        tau6 = matrix*param_vec6;
        EXPECT_NEAR(torque[5], tau6, accuracy);

        mat56 = jaco2KDL.getRigidBodyRegressionMatrix("jaco_link_5", "jaco_link_hand",q, qDot, qDotDot, gx, gy, gz);
        tau56 = mat56*param_vec.block<20,1>(40,0);
        EXPECT_NEAR(torque[4], tau56(0), 1e-9);
        EXPECT_NEAR(torque[5], tau56(1), 1e-9);

        full_matrix = jaco2KDL.getRigidBodyRegressionMatrix("jaco_link_1", "jaco_link_hand",q, qDot, qDotDot, gx, gy, gz);
        tau_full = full_matrix*param_vec;

        for(int i = 0; i <6; ++i) {
            EXPECT_NEAR(torque[i], tau_full(i), accuracy);
        }
    }

    qDot = {0.1, 1.04, 1.05, 1.06, 1.03, 0.1};
    qDotDot = {0.1, 1.0, 1.0, 1.0, -1.1, -0.4};
    jaco2KDL.getTorques(q, qDot, qDotDot, torque);

    matrix = jaco2KDL.getRigidBodyRegressionMatrix("jaco_link_hand", "jaco_link_hand",q, qDot, qDotDot, gx, gy, gz);
    tau6 = matrix*param_vec6;
    EXPECT_NEAR(torque[5], tau6, accuracy);

    mat56 = jaco2KDL.getRigidBodyRegressionMatrix("jaco_link_5", "jaco_link_hand",q, qDot, qDotDot, gx, gy, gz);
    tau56 = mat56*param_vec.block<20,1>(40,0);
    EXPECT_NEAR(torque[4], tau56(0), accuracy);
    EXPECT_NEAR(torque[5], tau56(1), accuracy);

    full_matrix = jaco2KDL.getRigidBodyRegressionMatrix("jaco_link_1", "jaco_link_hand",q, qDot, qDotDot, gx, gy, gz);
    tau_full = full_matrix*param_vec;

    for(int i = 0; i <6; ++i) {
        EXPECT_NEAR(torque[i], tau_full(i), accuracy);
    }

    //Somehow this test does not work
    // test if we can solve the parameters torque =  getRigidBodyRegressionMatrix * parameters
    //     parameres = svd(getRigidBodyRegressionMatrix).solve(torque)

    std::size_t n_samples = 2000;
    std::size_t n_links = 6;
    std::size_t n_params = 10 * n_links;
    std::size_t rows = n_samples * n_links + n_params;
    Eigen::MatrixXd reg_mat = Eigen::MatrixXd::Zero(rows, n_params);
    Eigen::MatrixXd samples_tau = Eigen::MatrixXd::Zero(rows, 1);
    Eigen::MatrixXd reg_tau = Eigen::MatrixXd::Zero(rows, 1);

    double scale = 0.0001;
    Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(n_params, n_params);
    for(int i = 0; i < n_samples; ++i) {
        jaco2KDL.getRandomConfig(q);
        jaco2KDL.setGravity(gx,gy,gz);
        jaco2KDL.getTorques(q, qDot, qDotDot, torque);

        Eigen::MatrixXd mat =
                jaco2KDL.getRigidBodyRegressionMatrix("jaco_link_1", "jaco_link_hand",q, qDot, qDotDot, gx, gy, gz);

        Eigen::Matrix<double, 6, 1> torque_mat;
        torque_mat << torque[0], torque[1], torque[2], torque[3], torque[4], torque[5];
        Eigen::MatrixXd tau_test = mat * param_vec;

        for(int i = 0; i < n_links; ++i) {
            EXPECT_NEAR(tau_test(i), torque_mat(i), accuracy);
        }

        reg_mat.block(i * n_links, 0, n_links, n_params) = mat;
        //        samples_tau.block<6,1>(i * 6,0) = torque_mat;
        samples_tau.block<6,1>(i * 6,0) = torque_mat;
        reg_tau.block<6,1>(i * 6,0) = torque_mat - mat * param_vec;
    }
    reg_mat.block(n_samples * n_links, 0, n_params, n_params) = scale * identity;

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(reg_mat, Eigen::ComputeThinU | Eigen::ComputeThinV | Eigen::FullPivHouseholderQRPreconditioner);
    Eigen::MatrixXd param = svd.solve(reg_tau);
    param += param_vec;

    Eigen::MatrixXd tau2 = reg_mat * param;

    //        std::cout << tau2 << std::endl;
    for(int i = 0; i < tau2.size(); ++i) {
        EXPECT_NEAR(samples_tau(i), tau2(i), scale);
    }

}

TEST(Jaco2DynamicsTests, getStaticRigidBodyRegressionMatrix)
{
    std::vector<double> q = {4.74, 2.96, 1.04, -2.08, 0.37, 1.37};
    std::vector<double> qDot = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    std::vector<double> qDotDot = {0.0, 0, 0, 0, 0.0, 0.0};
    std::vector<double> torque;

    int num_stat_tests = 100;
    double accuracy = 1e-9;

    Eigen::Matrix<double, 1, 4> matrix;
    double tau6;
    Eigen::Matrix<double, 2, 8> mat56;
    Eigen::Matrix<double, 2, 1> tau56;
    Eigen::Matrix<double, 24, 1> param_vec;
    Eigen::Matrix<double, 6, 24> full_matrix;
    Eigen::Matrix<double, 6, 1> tau_full;

    jaco2KDL.useUrdfDynamicParams();
    int id = 0;
    for(auto link : jaco2KDL.getLinkNames()) {
        param_vec(id) = jaco2KDL.getLinkMass(link);
        Eigen::Vector3d mc = jaco2KDL.getLinkMass(link) * jaco2KDL.getLinkCoM(link);
        param_vec(id+1) = mc(0);
        param_vec(id+2) = mc(1);
        param_vec(id+3) = mc(2);
        id += 4;
    }

    Eigen::Matrix<double, 4,1> param_vec6 = Eigen::Matrix<double, 4, 1>::Zero();
    param_vec6 = param_vec.block<4,1>(20,0);

    double gx,gy,gz;
    jaco2KDL.getGravity(gx, gy, gz);

    for(int i = 0; i < num_stat_tests; ++i ) {
        jaco2KDL.getRandomConfig(q);

        jaco2KDL.getTorques(q, qDot, qDotDot, torque);

        matrix = jaco2KDL.getStaticRigidBodyRegressionMatrix("jaco_link_hand", "jaco_link_hand",q, gx, gy, gz);
        tau6 = matrix*param_vec6;
        EXPECT_NEAR(torque[5], tau6, accuracy);

        mat56 = jaco2KDL.getStaticRigidBodyRegressionMatrix("jaco_link_5", "jaco_link_hand",q, gx, gy, gz);
        tau56 = mat56*param_vec.block<8,1>(16,0);
        EXPECT_NEAR(torque[4], tau56(0), 1e-9);
        EXPECT_NEAR(torque[5], tau56(1), 1e-9);

        full_matrix = jaco2KDL.getStaticRigidBodyRegressionMatrix("jaco_link_1", "jaco_link_hand",q, gx, gy, gz);
        tau_full = full_matrix*param_vec;

        for(int i = 0; i <6; ++i) {
            EXPECT_NEAR(torque[i], tau_full(i), accuracy);
        }
    }
}

TEST(Jaco2DynamicsTests, dynParamMatrices)
{
    //    std::vector<double> q = {4.77, 3.14, 3.14, 0.0, 0, 3.14};
    std::vector<double> q = {4.74, 2.96, 1.04, -2.08, 0.37, 1.37};
    std::vector<double> qDot = {0.1, 0.04, 0.05, 0.06, 0.03, 0.1};
    std::vector<double> qDotDot = {0.1, 0, 0, 0, -0.1, 0.4};
    //    std::vector<double> qDot = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    //    std::vector<double> qDotDot = {0.0, 0, 0, 0, 0.0, 0.0};
    std::vector<double> torque;

    Eigen::MatrixXd H;
    Eigen::VectorXd C;
    Eigen::VectorXd G;
    for(std::size_t i = 0; i <10 ; ++ i){
        jaco2KDL.getRandomConfig(q);
        jaco2KDL.getRandomConfig(qDot);
        jaco2KDL.getRandomConfig(qDotDot);
        jaco2KDL.getTorques(q,qDot,qDotDot,torque);

        Eigen::VectorXd alpha(6);
        alpha << qDotDot[0], qDotDot[1], qDotDot[2], qDotDot[3], qDotDot[4], qDotDot[5];
        jaco2KDL.getChainDynParam(q, qDot, H, C, G );

        Eigen::VectorXd  tau = H*alpha + C + G;

        double accuracy = 1e-3;
        for(int i = 0; i < torque.size(); ++i) {
            EXPECT_NEAR(torque[i], tau(i), accuracy);
        }
    }
}

TEST(Jaco2DynamicsTests, modifiedRNE)
{
    std::vector<double> q = {4.74, 2.96, 1.04, -2.08, 0.37, 1.37};
    std::vector<double> qDot = {1.1, 1.04, 1.05, 1.06, 1.03, 1.1};
    std::vector<double> qDotDot = {1.0, 1, 1, 1, 1.0, 1.0};

    std::vector<double> torques;
    Eigen::MatrixXd mrne_res(6,1);
    jaco2KDL.setGravity(0,0,9.81);
    for(std::size_t i = 0; i <50 ; ++ i){

        jaco2KDL.modifiedRNE(0,0,9.81,q,qDot,qDot,qDotDot,mrne_res);
        jaco2KDL.getTorques(q, qDot,qDotDot,torques);
        jaco2KDL.getRandomConfig(q);
        jaco2KDL.getRandomConfig(qDot);
        jaco2KDL.getRandomConfig(qDotDot);

        for(int i = 0; i < torques.size(); ++i) {
            EXPECT_NEAR(torques[i], mrne_res(i), 1e-10);
        }

    }
}

TEST(Jaco2DynamicsTests, matrixC)
{
    std::vector<double> q = {4.74, 2.96, 1.04, -2.08, 0.37, 1.37};
    std::vector<double> qDot = {1.1, 1.04, 1.05, 1.06, 1.03, 1.1};
    //    std::vector<double> qDotDot = {0.0, 0, 0, 0, 0.0, 0.0};
    //        std::vector<double> qDot = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    std::vector<double> qDotDot = {1.0, 1, 1, 1, 1.0, 1.0};

    //    std::vector<double> torques;
    //    Eigen::MatrixXd mrne_res(6,1);
    Eigen::MatrixXd H;
    Eigen::MatrixXd C;
    Eigen::VectorXd c;
    Eigen::VectorXd g;

    for(std::size_t i = 0; i < 50 ; ++ i){

        jaco2KDL.getChainDynParam(q, qDot, H, c, g );
        jaco2KDL.getMatrixC(q, qDot, C);

        Eigen::VectorXd omega(6);
        omega << qDot[0], qDot[1], qDot[2], qDot[3], qDot[4], qDot[5];

        Eigen::VectorXd c2 = C * omega;

        for(int i = 0; i < 6; ++i) {
            EXPECT_NEAR(c(i), c2(i), 1e-10);

        }

        jaco2KDL.getRandomConfig(q);
        jaco2KDL.getRandomConfig(qDot);
        jaco2KDL.getRandomConfig(qDotDot);

    }
}
TEST(Jaco2DynamicsTests, forwadInverseDynamics)
{
    std::size_t ntests = 50;
    std::vector<double> q,qDot,qDotDot,tau, accFD, tau2;
    Eigen::VectorXd run_times = Eigen::VectorXd::Zero(ntests);
    for(std::size_t i = 0; i < ntests ; ++ i){

        jaco2KDL.getRandomConfig(q);
        jaco2KDL.getRandomConfig(qDot);
        jaco2KDL.getRandomConfig(qDotDot);

        jaco2KDL.getTorques(q,qDot,qDotDot,tau);
        ros::Time start = ros::Time::now();
        jaco2KDL.getJointAcceleration(q, qDot, tau, accFD);
        ros::Time end = ros::Time::now();
        run_times(i) = (end -start).toSec();
        jaco2KDL.getTorques(q,qDot,accFD,tau2);

        for(int i = 0; i < 6; ++i) {
            EXPECT_NEAR(tau[i], tau2[i], 1e-10);
        }
    }
    std::cout << "mean FD runtime: " << run_times.mean() * 1000 << " ms." << std::endl;
}

TEST(Jaco2DynamicsTests, moreValuesInn)
{
    std::vector<double> q = {4.8089, 2.9226, 1.0028, 4.2031, 1.4448 , 1.3206, 0, 0, 0};
    std::vector<double> qDot = {0, 0, 0, 0, 0 , 06, 0, 0, 0};
    std::vector<double> qDotDot = {0, 0, 0, 0, 0 , 06, 0, 0, 0};
    std::vector<double> tau;
    int ec = jaco2KDL.getTorques(q,qDot,qDotDot,tau);
    EXPECT_TRUE(ec>=0);

}

int main(int argc, char *argv[])
{
    testing::InitGoogleTest(&argc, argv);

    ros::init(argc, argv, "test_jaco2_dynamics");
    ros::NodeHandle node("~");
    std::string robot_desc_string;
    //    node.getParam("/robot_description", robot_desc_string);//, std::string(""));
//    std::string urdf_param("robot_description");
    std::string urdf_path = ros::package::getPath("cslibs_kdl");
    urdf_path += "/test/jaco.urdf";
    jaco2KDL = DynamicModel(urdf_path,"jaco_link_base","jaco_link_hand");

    return RUN_ALL_TESTS();
}


/* perceptron.cc
   Jeremy Barnes, 16 June 2003
   Copyright (c) 2003 Jeremy Barnes.  All rights reserved.
   $Source$

   Wrapper around the MLP class to go with the rest of the classifier
   framework.  Implementation.
*/

#include "perceptron.h"
#include "jml/boosting/classifier_persist_impl.h"
#include "jml/utils/profile.h"
#include "jml/utils/environment.h"
#include "jml/algebra/matrix_ops.h"
#include "jml/boosting/training_index.h"
#include "jml/algebra/irls.h"
#include "jml/utils/vector_utils.h"
#include <iomanip>
#include <boost/random/lagged_fibonacci.hpp>
#include <boost/random/uniform_01.hpp>
#include "jml/utils/parse_context.h"
#include "jml/stats/distribution_simd.h"
#include "jml/stats/distribution_ops.h"
#include "jml/boosting/evaluation.h"
#include "jml/arch/simd_vector.h"
#include "jml/algebra/lapack.h"
#include "jml/boosting/config_impl.h"
#include "jml/utils/worker_task.h"
#include "jml/utils/guard.h"
#include <boost/bind.hpp>
#include "jml/arch/backtrace.h"
#include "dense_layer.h"

using namespace std;
using namespace DB;

namespace ML {

namespace {

Env_Option<bool> profile("PROFILE_PERCEPTRON", false);

double t_predict = 0.0, t_accuracy = 0.0, t_decorrelate = 0.0;

struct Stats {
    ~Stats()
    {
        if (profile) {
            cerr << "perceptron runtime profile: " << endl;
            cerr << "  decorrelate:    " << t_decorrelate << "s" << endl;
            cerr << "  predict:        " << t_predict     << "s" << endl;
            cerr << "  accuracy:       " << t_accuracy    << "s" << endl;
        }
    }
} stats;

} // file scope

/*****************************************************************************/
/* PERCEPTRON                                                                */
/*****************************************************************************/

Perceptron::Perceptron()
{
}

Perceptron::
Perceptron(const std::shared_ptr<const Feature_Space> & feature_space,
               const Feature & predicted)
    : Classifier_Impl(feature_space, predicted)
{
}

Perceptron::
Perceptron(DB::Store_Reader & reader,
           const std::shared_ptr<const Feature_Space> & feature_space)
{
    this->reconstitute(reader, feature_space);
}

Perceptron::
Perceptron(const std::shared_ptr<const Feature_Space> & feature_space,
           const Feature & predicted,
           size_t label_count)
    : Classifier_Impl(feature_space, predicted, label_count)
{
}

Perceptron::
Perceptron(const Perceptron & other)
    : Classifier_Impl(other), features(other.features),
      layers(other.layers, Deep_Copy_Tag()), output(other.output)
{
}

Perceptron &
Perceptron::
operator = (const Perceptron & other)
{
    Perceptron new_me(other);
    swap(new_me);
    return *this;
}

float
Perceptron::
predict(int label, const Feature_Set & features,
        PredictionContext * context) const
{
    return predict(features).at(label);
}

distribution<float>
Perceptron::
predict(const Feature_Set & fs,
        PredictionContext * context) const
{
    PROFILE_FUNCTION(t_predict);

    float input[layers.inputs()];
    extract_features(fs, input);

    distribution<float> output(layers.outputs());
    layers.apply(input, &output[0]);

    return this->output.decode(output);
}

std::string
Perceptron::
print() const
{
    string result = format("{ Perceptron: %zd layers, %zd inputs, %zd outputs\n",
                           layers.size(), features.size(),
                           layers.outputs());

    result += "  features:\n";
    for (unsigned f = 0;  f < features.size();  ++f)
        result += format("    %d %s\n", f,
                         feature_space()->print(features[f]).c_str());
    result += "\n";

    for (unsigned i = 0;  i < layers.size();  ++i)
        result += format("  layer %d\n", i) + layers[i].print();

    result += "}";

    return result;
}

std::vector<ML::Feature>
Perceptron::
all_features() const
{
    return features;
}

Output_Encoding
Perceptron::
output_encoding() const
{
    return OE_PM_ONE;
}

std::vector<int>
Perceptron::
parse_architecture(const std::string & arch)
{
    Parse_Context context(arch, &arch[0], &arch[0] + arch.size());

    vector<int> result;
    while (context) {
        if (context.match_literal('%')) {
            if (context.match_literal('i'))
                result.push_back(-1);
            else context.exception("expected i after %");
        }
        else result.push_back(context.expect_unsigned());

        if (!context.match_literal('_')) context.expect_eof();
    }

    return result;
}

void Perceptron::add_layer(const std::shared_ptr<Layer> & layer)
{
    layer->validate();
    layers.add(layer);
}

void Perceptron::clear()
{
    layers.clear();
    features.clear();
}

size_t Perceptron::parameters() const
{
    size_t result = 0;
    for (unsigned l = 1;  l < layers.size();  ++l)
        result += layers[l].parameter_count();
    return result;
}

namespace {

static const std::string PERCEPTRON_MAGIC = "PERCEPTRON";
static const compact_size_t PERCEPTRON_VERSION = 2;

} // file scope

void Perceptron::serialize(DB::Store_Writer & store) const
{
    store << PERCEPTRON_MAGIC << PERCEPTRON_VERSION
          << compact_size_t(label_count());
    feature_space()->serialize(store, predicted_);

    store << compact_size_t(features.size());
    for (unsigned i = 0;  i < features.size();  ++i)
        feature_space()->serialize(store, features[i]);

    /* Now the layers... */
    layers.serialize(store);
    output.serialize(store);

    store << string("END PERCEPTRON");
}

void
Perceptron::
reconstitute(DB::Store_Reader & store,
             const std::shared_ptr<const Feature_Space> & feature_space)
{
    /* Implement the strong exception guarantee, except for the store. */
    string magic;
    compact_size_t version;
    store >> magic >> version;
    if (magic != PERCEPTRON_MAGIC)
        throw Exception("Attempt to reconstitute \"" + magic
                                + "\" with perceptrons reconstitutor");
    if (version != PERCEPTRON_VERSION)
        throw Exception(format("Attemp to reconstitute perceptrons "
                               "version %zd, only <= %zd supported",
                               version.size_,
                               PERCEPTRON_VERSION.size_));

    compact_size_t label_count(store);

    predicted_ = MISSING_FEATURE;
    feature_space->reconstitute(store, predicted_);

    Perceptron new_me(feature_space, predicted_, label_count);

    compact_size_t nfeat(store);
    new_me.features.resize(nfeat);
    for (unsigned i = 0;  i < nfeat;  ++i)
        feature_space->reconstitute(store, new_me.features[i]);

    /* Now the layers... */
    new_me.layers.reconstitute(store);
    new_me.output.reconstitute(store);

    string s;
    store >> s;
    if (s != "END PERCEPTRON")
        throw Exception("problem with perceptron reconstitution");

    swap(new_me);
}

Perceptron * Perceptron::make_copy() const
{
    return new Perceptron(*this);
}

boost::multi_array<float, 2>
Perceptron::
decorrelate(const Training_Data & data) const
{
    PROFILE_FUNCTION(t_decorrelate);

    if (layers.empty())
        throw Exception("Perceptron::decorrelate(): need to train decorrelation "
                        "first");

    size_t nx = data.example_count();
    size_t nf = features.size();

    boost::multi_array<float, 2> result(boost::extents[nx][nf]);

    float input[nf];

    for (unsigned x = 0;  x < nx;  ++x) {
        extract_features(data[x], &input[0]);
        layers[0].apply(input, &result[x][0]);
    }

    return result;
}

/*****************************************************************************/
/* REGISTRATION                                                              */
/*****************************************************************************/

namespace {

Register_Factory<Classifier_Impl, Perceptron>
    PERCEPTRON_REGISTER("PERCEPTRON");

} // file scope

} // namespace ML


/*
 * Copyright 2020 Tier IV, Inc. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <pcl/filters/voxel_grid.h>
#include <tf2/utils.h>
#include <tf2_eigen/tf2_eigen.h>
#include <boost/assert.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <obstacle_stop_planner/node.hpp>
#include <vector>
#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
#include <Eigen/Geometry>
namespace
{
template <class T>
T getParam(const ros::NodeHandle & nh, const std::string & key, const T & default_value)
{
  T value;
  nh.param<T>(key, value, default_value);
  return value;
}

template <class T>
T waitForParam(const ros::NodeHandle & nh, const std::string & key)
{
  T value;
  ros::Rate rate(1.0);

  while (ros::ok()) {
    const auto result = nh.getParam(key, value);
    if (result) {
      return value;
    }

    ROS_WARN("waiting for parameter `%s` ...", key.c_str());
    rate.sleep();
  }

  return {};
}
double getYawFromGeometryMsgsQuaternion(const geometry_msgs::Quaternion & quat)
{
  tf2::Quaternion tf2_quat(quat.x, quat.y, quat.z, quat.w);
  double roll, pitch, yaw;
  tf2::Matrix3x3(tf2_quat).getRPY(roll, pitch, yaw);

  return yaw;
}
}  // namespace

namespace motion_planning
{
namespace bg = boost::geometry;
using Point = bg::model::d2::point_xy<double>;
using Polygon = bg::model::polygon<Point, false>;
using Line = bg::model::linestring<Point>;

ObstacleStopPlannerNode::ObstacleStopPlannerNode() : nh_(), pnh_("~"), tf_listener_(tf_buffer_)
{
  // Subscribers
  obstacle_pointcloud_sub_ = pnh_.subscribe(
    "input/pointcloud", 1, &ObstacleStopPlannerNode::obstaclePointcloudCallback, this);
  path_sub_ = pnh_.subscribe("input/trajectory", 1, &ObstacleStopPlannerNode::pathCallback, this);
  // Publishers
  path_pub_ = pnh_.advertise<autoware_planning_msgs::Trajectory>("output/trajectory", 1);

  // Vehicle Parameters
  wheel_base_ = waitForParam<double>(pnh_, "/vehicle_info/wheel_base");
  front_overhang_ = waitForParam<double>(pnh_, "/vehicle_info/front_overhang");
  rear_overhang_ = waitForParam<double>(pnh_, "/vehicle_info/rear_overhang");
  left_overhang_ = waitForParam<double>(pnh_, "/vehicle_info/left_overhang");
  right_overhang_ = waitForParam<double>(pnh_, "/vehicle_info/right_overhang");
  vehicle_width_ = waitForParam<double>(pnh_, "/vehicle_info/vehicle_width");
  // Parameters
  stop_margin_ = getParam<double>(pnh_, "stop_margin", 5.0);
  min_behavior_stop_margin_ = getParam<double>(pnh_, "min_behavior_stop_margin", 2.0);
  stop_margin_ += wheel_base_ + front_overhang_;
  min_behavior_stop_margin_ += wheel_base_ + front_overhang_;
  debug_ptr_ = std::make_shared<ObstacleStopPlannerDebugNode>(wheel_base_ + front_overhang_);
}

void ObstacleStopPlannerNode::obstaclePointcloudCallback(
  const sensor_msgs::PointCloud2::ConstPtr & input_msg)
{
  obstacle_ros_pointcloud_ptr_ = boost::make_shared<sensor_msgs::PointCloud2>();
  pcl::VoxelGrid<pcl::PointXYZ> filter;
  pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr no_height_pointcloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr no_height_filtered_pointcloud_ptr(
    new pcl::PointCloud<pcl::PointXYZ>);

  pcl::fromROSMsg(*input_msg, *pointcloud_ptr);

  for (const auto & point : pointcloud_ptr->points)
    no_height_pointcloud_ptr->push_back(pcl::PointXYZ(point.x, point.y, 0.0));
  filter.setInputCloud(no_height_pointcloud_ptr);
  filter.setLeafSize(0.05f, 0.05f, 100000.0f);
  filter.filter(*no_height_filtered_pointcloud_ptr);
  pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_);
  obstacle_ros_pointcloud_ptr_->header = input_msg->header;
}
void ObstacleStopPlannerNode::pathCallback(
  const autoware_planning_msgs::Trajectory::ConstPtr & input_msg)
{
  if (obstacle_ros_pointcloud_ptr_ == nullptr) return;

  autoware_planning_msgs::Trajectory output_msg = *input_msg;
  const double epsilon = 0.00001;
  /*
   * trim trajectory from self pose
   */
  geometry_msgs::Pose self_pose;
  getSelfPose(input_msg->header, tf_buffer_, self_pose);
  autoware_planning_msgs::Trajectory trim_trajectory;
  size_t trajectory_trim_index;
  trimTrajectoryWithIndexFromSelfPose(
    *input_msg, self_pose, trim_trajectory, trajectory_trim_index);

  /*
   * decimate trajectory for calculation cost
   */
  autoware_planning_msgs::Trajectory decimate_trajectory;
  const double step_length = 1.0;
  std::map<size_t /* decimate */, size_t /* origin */> decimate_trajectory_index_map;
  decimateTrajectory(
    trim_trajectory, step_length, decimate_trajectory, decimate_trajectory_index_map);

  autoware_planning_msgs::Trajectory & trajectory = decimate_trajectory;

  /*
   * search candidate obstacle pointcloud
   */
  pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_candidate_pointcloud_ptr(
    new pcl::PointCloud<pcl::PointXYZ>);
  {
    // transform pointcloud
    geometry_msgs::TransformStamped transform_stamped = tf_buffer_.lookupTransform(
      trajectory.header.frame_id, obstacle_ros_pointcloud_ptr_->header.frame_id,
      obstacle_ros_pointcloud_ptr_->header.stamp, ros::Duration(0.5));
    Eigen::Matrix4f affine_matrix =
      tf2::transformToEigen(transform_stamped.transform).matrix().cast<float>();
    sensor_msgs::PointCloud2 transformed_obstacle_ros_pointcloud;
    pcl_ros::transformPointCloud(
      affine_matrix, *obstacle_ros_pointcloud_ptr_, transformed_obstacle_ros_pointcloud);
    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_obstacle_pointcloud_ptr(
      new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(transformed_obstacle_ros_pointcloud, *transformed_obstacle_pointcloud_ptr);
    // search obstacle candidate pointcloud to reduce calculation cost
    const double search_range =
      step_length + std::hypot((vehicle_width_ / 2.0), (wheel_base_ + front_overhang_));
    searchPointcloudNearTrajectory(
      trajectory, search_range, transformed_obstacle_pointcloud_ptr,
      obstacle_candidate_pointcloud_ptr);
  }

  /*
   * check collision
   */
  bool is_collision = false;
  size_t decimate_trajectory_collision_index;
  pcl::PointXYZ nearest_collision_point;
  for (int i = 0; i < (int)(trajectory.points.size()) - 1; ++i) {
    /*
     * create one step polygon
     */
    std::vector<cv::Point2d> one_step_move_vehicle_polygon;
    createOneStepPolygon(
      trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose,
      one_step_move_vehicle_polygon);
    debug_ptr_->pushPolygon(one_step_move_vehicle_polygon, trajectory.points.at(i).pose.position.z);

    /*
     * collision check obstacle pointcloud and polygon
     */
    // convert boost polygon
    Polygon boost_one_step_move_vehicle_polygon;
    for (const auto & point : one_step_move_vehicle_polygon) {
      boost_one_step_move_vehicle_polygon.outer().push_back(bg::make<Point>(point.x, point.y));
    }
    boost_one_step_move_vehicle_polygon.outer().push_back(bg::make<Point>(
      one_step_move_vehicle_polygon.front().x, one_step_move_vehicle_polygon.front().y));
    // check within polygon
    pcl::PointCloud<pcl::PointXYZ>::Ptr collision_pointcloud_ptr(
      new pcl::PointCloud<pcl::PointXYZ>);
    for (size_t j = 0; j < obstacle_candidate_pointcloud_ptr->size(); ++j) {
      Point point(
        obstacle_candidate_pointcloud_ptr->at(j).x, obstacle_candidate_pointcloud_ptr->at(j).y);
      if (bg::within(point, boost_one_step_move_vehicle_polygon)) {
        collision_pointcloud_ptr->push_back(obstacle_candidate_pointcloud_ptr->at(j));
        is_collision = true;
        debug_ptr_->pushCollisionPolygon(
          one_step_move_vehicle_polygon, trajectory.points.at(i).pose.position.z);
      }
    }

    /*
     * search nearest collision point by begining of path
     */
    if (is_collision) {
      getNearestPoint(
        *collision_pointcloud_ptr, trajectory.points.at(i).pose, nearest_collision_point);
      debug_ptr_->pushStopObstaclePoint(nearest_collision_point);
      decimate_trajectory_collision_index = i;
      break;
    }
  }

  /*
   * insert stop point
   */
  if (is_collision) {
    for (int i = decimate_trajectory_index_map.at(decimate_trajectory_collision_index);
         i < (int)output_msg.points.size(); ++i) {
      Eigen::Vector2d trajectory_vec;
      {
        const double yaw =
          getYawFromGeometryMsgsQuaternion(output_msg.points.at(i).pose.orientation);
        trajectory_vec << std::cos(yaw), std::sin(yaw);
      }
      Eigen::Vector2d collision_point_vec;
      collision_point_vec << nearest_collision_point.x - output_msg.points.at(i).pose.position.x,
        nearest_collision_point.y - output_msg.points.at(i).pose.position.y;

      if (
        trajectory_vec.dot(collision_point_vec) < 0.0 ||
        (i + 1 == output_msg.points.size() && 0.0 < trajectory_vec.dot(collision_point_vec))) {
        Eigen::Vector2d max_dist_stop_point;
        // search insert point
        size_t max_dist_stop_point_idx = 0;
        {
          double length_sum = 0.0;
          length_sum += trajectory_vec.normalized().dot(collision_point_vec);
          Eigen::Vector2d line_start_point, line_end_point;
          {
            line_start_point << output_msg.points.at(0).pose.position.x,
              output_msg.points.at(0).pose.position.y;
            const double yaw =
              getYawFromGeometryMsgsQuaternion(output_msg.points.at(0).pose.orientation);
            line_end_point << std::cos(yaw), std::sin(yaw);
          }
          for (size_t j = i; 0 < j; --j) {
            line_start_point << output_msg.points.at(j - 1).pose.position.x,
              output_msg.points.at(j - 1).pose.position.y;
            line_end_point << output_msg.points.at(j).pose.position.x,
              output_msg.points.at(j).pose.position.y;
            if (stop_margin_ < length_sum) {
              max_dist_stop_point_idx = j;
              break;
            }
            length_sum += (line_end_point - line_start_point).norm();
          }
          getBackwordPointFromBasePoint(
            line_start_point, line_end_point, line_start_point, length_sum - stop_margin_,
            max_dist_stop_point);
        }
        Eigen::Vector2d min_dist_stop_point;
        size_t min_dist_stop_point_idx = 0;
        {
          double length_sum = 0.0;
          length_sum += trajectory_vec.normalized().dot(collision_point_vec);
          Eigen::Vector2d line_start_point, line_end_point;
          {
            line_start_point << output_msg.points.at(0).pose.position.x,
              output_msg.points.at(0).pose.position.y;
            const double yaw =
              getYawFromGeometryMsgsQuaternion(output_msg.points.at(0).pose.orientation);
            line_end_point << std::cos(yaw), std::sin(yaw);
          }
          for (size_t j = i; 0 < j; --j) {
            line_start_point << output_msg.points.at(j - 1).pose.position.x,
              output_msg.points.at(j - 1).pose.position.y;
            line_end_point << output_msg.points.at(j).pose.position.x,
              output_msg.points.at(j).pose.position.y;
            if (min_behavior_stop_margin_ < length_sum) {
              min_dist_stop_point_idx = j;
              break;
            }
            length_sum += (line_end_point - line_start_point).norm();
          }
          getBackwordPointFromBasePoint(
            line_start_point, line_end_point, line_start_point,
            length_sum - min_behavior_stop_margin_, min_dist_stop_point);
        }

        // check already insert stop point
        bool is_inserted_already_stop_point = false;
        for (int j = max_dist_stop_point_idx - 1; j < (int)i; ++j) {
          if (output_msg.points.at(std::max(j, 0)).twist.linear.x == 0.0) {
            is_inserted_already_stop_point = true;
            break;
          }
        }
        // insert stop point
        const size_t insert_stop_point_index =
          !is_inserted_already_stop_point ? max_dist_stop_point_idx : min_dist_stop_point_idx;
        const Eigen::Vector2d stop_point =
          !is_inserted_already_stop_point ? max_dist_stop_point : min_dist_stop_point;
        autoware_planning_msgs::TrajectoryPoint stop_trajectory_point =
          output_msg.points.at(std::max((int)(insert_stop_point_index)-1, 0));
        stop_trajectory_point.pose.position.x = stop_point.x();
        stop_trajectory_point.pose.position.y = stop_point.y();
        stop_trajectory_point.twist.linear.x = 0.0;
        output_msg.points.insert(
          output_msg.points.begin() + insert_stop_point_index, stop_trajectory_point);
        for (size_t j = insert_stop_point_index; j < output_msg.points.size(); ++j) {
          output_msg.points.at(j).twist.linear.x = 0.0;
        }
        debug_ptr_->pushStopPose(stop_trajectory_point.pose);
        break;
      }
    }
  }
  path_pub_.publish(output_msg);
  debug_ptr_->publish();
}

bool ObstacleStopPlannerNode::decimateTrajectory(
  const autoware_planning_msgs::Trajectory & input_trajectory, const double step_length,
  autoware_planning_msgs::Trajectory & output_trajectory)
{
  std::map<size_t /* decimate */, size_t /* origin */> index_map;
  decimateTrajectory(input_trajectory, step_length, output_trajectory, index_map);
}

bool ObstacleStopPlannerNode::decimateTrajectory(
  const autoware_planning_msgs::Trajectory & input_trajectory, const double step_length,
  autoware_planning_msgs::Trajectory & output_trajectory,
  std::map<size_t /* decimate */, size_t /* origin */> & index_map)
{
  output_trajectory.header = input_trajectory.header;
  double trajectory_length_sum = 0.0;
  double next_length = 0.0;
  const double epsilon = 0.001;
  Eigen::Vector2d point1, point2;

  for (int i = 0; i < (int)(input_trajectory.points.size()) - 1; ++i) {
    if (next_length <= trajectory_length_sum + epsilon) {
      Eigen::Vector2d line_start_point, line_end_point, interporated_point;
      line_start_point << input_trajectory.points.at(i).pose.position.x,
        input_trajectory.points.at(i).pose.position.y;
      line_end_point << input_trajectory.points.at(i + 1).pose.position.x,
        input_trajectory.points.at(i + 1).pose.position.y;
      getBackwordPointFromBasePoint(
        line_start_point, line_end_point, line_end_point,
        -1.0 * (trajectory_length_sum - next_length), interporated_point);
      autoware_planning_msgs::TrajectoryPoint trajectory_point;
      trajectory_point = input_trajectory.points.at(i);
      trajectory_point.pose.position.x = interporated_point.x();
      trajectory_point.pose.position.y = interporated_point.y();
      output_trajectory.points.push_back(trajectory_point);
      index_map.insert(std::make_pair(output_trajectory.points.size() - 1, size_t(i)));
      next_length += step_length;
      continue;
    }
    const double x = input_trajectory.points.at(i).pose.position.x -
                     input_trajectory.points.at(i + 1).pose.position.x;
    const double y = input_trajectory.points.at(i).pose.position.y -
                     input_trajectory.points.at(i + 1).pose.position.y;
    const double distance = std::sqrt(x * x + y * y);

    trajectory_length_sum += distance;
  }
  if (!input_trajectory.points.empty()) {
    output_trajectory.points.push_back(input_trajectory.points.back());
    index_map.insert(
      std::make_pair(output_trajectory.points.size() - 1, input_trajectory.points.size() - 1));
  }
  return true;
}

bool ObstacleStopPlannerNode::trimTrajectoryWithIndexFromSelfPose(
  const autoware_planning_msgs::Trajectory & input_trajectory, const geometry_msgs::Pose self_pose,
  autoware_planning_msgs::Trajectory & output_trajectory, size_t & index)
{
  double min_distance;
  size_t min_dintance_index = 0;
  bool is_init = false;
  for (size_t i = 0; i < input_trajectory.points.size(); ++i) {
    const double x = input_trajectory.points.at(i).pose.position.x - self_pose.position.x;
    const double y = input_trajectory.points.at(i).pose.position.y - self_pose.position.y;
    const double squared_distance = x * x + y * y;
    if (!is_init || squared_distance < min_distance * min_distance) {
      is_init = true;
      min_distance = std::sqrt(squared_distance);
      min_dintance_index = i;
    }
  }
  for (size_t i = min_dintance_index; i < input_trajectory.points.size(); ++i) {
    output_trajectory.points.push_back(input_trajectory.points.at(i));
  }
  output_trajectory.header = input_trajectory.header;
  index = min_dintance_index;
  return true;
}

bool ObstacleStopPlannerNode::trimTrajectoryFromSelfPose(
  const autoware_planning_msgs::Trajectory & input_trajectory, const geometry_msgs::Pose self_pose,
  autoware_planning_msgs::Trajectory & output_trajectory)
{
  size_t index;
  return trimTrajectoryWithIndexFromSelfPose(
    output_trajectory, self_pose, output_trajectory, index);
}

bool ObstacleStopPlannerNode::searchPointcloudNearTrajectory(
  const autoware_planning_msgs::Trajectory & trajectory, const double radius,
  const pcl::PointCloud<pcl::PointXYZ>::Ptr input_pointcloud_ptr,
  pcl::PointCloud<pcl::PointXYZ>::Ptr output_pointcloud_ptr)
{
  const double squared_radius = radius * radius;
  for (const auto & trajectory_point : trajectory.points) {
    for (const auto & point : input_pointcloud_ptr->points) {
      const double x = trajectory_point.pose.position.x - point.x;
      const double y = trajectory_point.pose.position.y - point.y;
      const double squared_distance = x * x + y * y;
      if (squared_distance < squared_radius) output_pointcloud_ptr->points.push_back(point);
    }
  }
  return true;
}

void ObstacleStopPlannerNode::createOneStepPolygon(
  const geometry_msgs::Pose base_stap_pose, const geometry_msgs::Pose next_step_pose,
  std::vector<cv::Point2d> & polygon)
{
  std::vector<cv::Point2d> one_step_move_vehicle_corner_points;
  // start step
  {
    double yaw = getYawFromGeometryMsgsQuaternion(base_stap_pose.orientation);
    one_step_move_vehicle_corner_points.push_back(cv::Point2d(
      base_stap_pose.position.x + std::cos(yaw) * (wheel_base_ + front_overhang_) -
        std::sin(yaw) * (vehicle_width_ / 2.0),
      base_stap_pose.position.y + std::sin(yaw) * (wheel_base_ + front_overhang_) +
        std::cos(yaw) * (vehicle_width_ / 2.0)));
    one_step_move_vehicle_corner_points.push_back(cv::Point2d(
      base_stap_pose.position.x + std::cos(yaw) * (wheel_base_ + front_overhang_) -
        std::sin(yaw) * (-vehicle_width_ / 2.0),
      base_stap_pose.position.y + std::sin(yaw) * (wheel_base_ + front_overhang_) +
        std::cos(yaw) * (-vehicle_width_ / 2.0)));
    one_step_move_vehicle_corner_points.push_back(cv::Point2d(
      base_stap_pose.position.x + std::cos(yaw) * (-rear_overhang_) -
        std::sin(yaw) * (-vehicle_width_ / 2.0),
      base_stap_pose.position.y + std::sin(yaw) * (-rear_overhang_) +
        std::cos(yaw) * (-vehicle_width_ / 2.0)));
    one_step_move_vehicle_corner_points.push_back(cv::Point2d(
      base_stap_pose.position.x + std::cos(yaw) * (-rear_overhang_) -
        std::sin(yaw) * (vehicle_width_ / 2.0),
      base_stap_pose.position.y + std::sin(yaw) * (-rear_overhang_) +
        std::cos(yaw) * (vehicle_width_ / 2.0)));
  }
  // next step
  {
    double yaw = getYawFromGeometryMsgsQuaternion(next_step_pose.orientation);
    one_step_move_vehicle_corner_points.push_back(cv::Point2d(
      next_step_pose.position.x + std::cos(yaw) * (wheel_base_ + front_overhang_) -
        std::sin(yaw) * (vehicle_width_ / 2.0),
      next_step_pose.position.y + std::sin(yaw) * (wheel_base_ + front_overhang_) +
        std::cos(yaw) * (vehicle_width_ / 2.0)));
    one_step_move_vehicle_corner_points.push_back(cv::Point2d(
      next_step_pose.position.x + std::cos(yaw) * (wheel_base_ + front_overhang_) -
        std::sin(yaw) * (-vehicle_width_ / 2.0),
      next_step_pose.position.y + std::sin(yaw) * (wheel_base_ + front_overhang_) +
        std::cos(yaw) * (-vehicle_width_ / 2.0)));
    one_step_move_vehicle_corner_points.push_back(cv::Point2d(
      next_step_pose.position.x + std::cos(yaw) * (-rear_overhang_) -
        std::sin(yaw) * (-vehicle_width_ / 2.0),
      next_step_pose.position.y + std::sin(yaw) * (-rear_overhang_) +
        std::cos(yaw) * (-vehicle_width_ / 2.0)));
    one_step_move_vehicle_corner_points.push_back(cv::Point2d(
      next_step_pose.position.x + std::cos(yaw) * (-rear_overhang_) -
        std::sin(yaw) * (vehicle_width_ / 2.0),
      next_step_pose.position.y + std::sin(yaw) * (-rear_overhang_) +
        std::cos(yaw) * (vehicle_width_ / 2.0)));
  }
  convexHull(one_step_move_vehicle_corner_points, polygon);

  return;
}

bool ObstacleStopPlannerNode::convexHull(
  const std::vector<cv::Point2d> pointcloud, std::vector<cv::Point2d> & polygon_points)
{
  cv::Point2d centroid;
  centroid.x = 0;
  centroid.y = 0;
  for (const auto & point : pointcloud) {
    centroid.x += point.x;
    centroid.y += point.y;
  }
  centroid.x = centroid.x / (double)pointcloud.size();
  centroid.y = centroid.y / (double)pointcloud.size();

  std::vector<cv::Point> normalized_pointcloud;
  std::vector<cv::Point> normalized_polygon_points;
  for (size_t i = 0; i < pointcloud.size(); ++i) {
    normalized_pointcloud.push_back(cv::Point(
      (pointcloud.at(i).x - centroid.x) * 1000.0, (pointcloud.at(i).y - centroid.y) * 1000.0));
  }
  cv::convexHull(normalized_pointcloud, normalized_polygon_points);

  for (size_t i = 0; i < normalized_polygon_points.size(); ++i) {
    cv::Point2d polygon_point;
    polygon_point.x = (normalized_polygon_points.at(i).x / 1000.0 + centroid.x);
    polygon_point.y = (normalized_polygon_points.at(i).y / 1000.0 + centroid.y);
    polygon_points.push_back(polygon_point);
  }
  return true;
}

bool ObstacleStopPlannerNode::getSelfPose(
  const std_msgs::Header & header, const tf2_ros::Buffer & tf_buffer,
  geometry_msgs::Pose & self_pose)
{
  try {
    geometry_msgs::TransformStamped transform;
    transform =
      tf_buffer.lookupTransform(header.frame_id, "base_link", header.stamp, ros::Duration(0.1));
    self_pose.position.x = transform.transform.translation.x;
    self_pose.position.y = transform.transform.translation.y;
    self_pose.position.z = transform.transform.translation.z;
    self_pose.orientation.x = transform.transform.rotation.x;
    self_pose.orientation.y = transform.transform.rotation.y;
    self_pose.orientation.z = transform.transform.rotation.z;
    self_pose.orientation.w = transform.transform.rotation.w;
    return true;
  } catch (tf2::TransformException & ex) {
    return false;
  }
}
bool ObstacleStopPlannerNode::getBackwordPointFromBasePoint(
  const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2,
  const Eigen::Vector2d & base_point, const double backward_length, Eigen::Vector2d & output_point)
{
  Eigen::Vector2d line_vec = line_point2 - line_point1;
  Eigen::Vector2d backward_vec = backward_length * line_vec.normalized();
  output_point = base_point + backward_vec;
  return true;
}

void ObstacleStopPlannerNode::getNearestPoint(
  const pcl::PointCloud<pcl::PointXYZ> & pointcloud, const geometry_msgs::Pose & base_pose,
  pcl::PointXYZ & nearest_collision_point)
{
  double min_norm;
  bool is_init = false;
  const double yaw = getYawFromGeometryMsgsQuaternion(base_pose.orientation);
  Eigen::Vector2d base_pose_vec;
  base_pose_vec << std::cos(yaw), std::sin(yaw);

  for (size_t i = 0; i < pointcloud.size(); ++i) {
    Eigen::Vector2d pointcloud_vec;
    pointcloud_vec << pointcloud.at(i).x - base_pose.position.x,
      pointcloud.at(i).y - base_pose.position.y;
    double norm = base_pose_vec.dot(pointcloud_vec);
    if (norm < min_norm || !is_init) {
      min_norm = norm;
      nearest_collision_point = pointcloud.at(i);
      is_init = true;
    }
  }
}

}  // namespace motion_planning