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

/**
 * @file arm_transform.cpp
 * @author Cyril Jourdan
 * @date Sep 26, 2017
 * @version 0.1.0
 * @brief Implementation file for the arm transform
 *
 * Contact: contact@therobotstudio.com
 * Created on : Jul 17, 2012
 */

#include <ros/ros.h>
#include <string>
#include <sensor_msgs/JointState.h>
#include <tf/transform_listener.h>
//#include <boost/lexical_cast.hpp>
#include "std_msgs/Int16.h"
#include <std_msgs/Float32.h>

#include <math.h>

using namespace std;

double calculateAlpha(tf::StampedTransform tf_hand, tf::StampedTransform tf_elbow, tf::StampedTransform tf_shoulder)
{
	double rightHand[3] = {tf_hand.getOrigin().x(), tf_hand.getOrigin().y(), tf_hand.getOrigin().z()};
	double rightElbow[3] = {tf_elbow.getOrigin().x(), tf_elbow.getOrigin().y(), tf_elbow.getOrigin().z()};
	double rightShoulder[3] = {tf_shoulder.getOrigin().x(), tf_shoulder.getOrigin().y(), tf_shoulder.getOrigin().z()};

	//Al-Kashi triangle formula
	double num =  pow(rightHand[0]-rightElbow[0], 2) + pow(rightHand[1]-rightElbow[1], 2) + pow(rightHand[2]-rightElbow[2], 2)
				+ pow(rightShoulder[0]-rightElbow[0], 2) + pow(rightShoulder[1]-rightElbow[1], 2) + pow(rightShoulder[2]-rightElbow[2], 2)
				- pow(rightShoulder[0]-rightHand[0], 2) - pow(rightShoulder[1]-rightHand[1], 2) - pow(rightShoulder[2]-rightHand[2], 2);

	double den = 2 * sqrt(pow(rightHand[0]-rightElbow[0], 2) + pow(rightHand[1]-rightElbow[1], 2) + pow(rightHand[2]-rightElbow[2], 2)) * sqrt(pow(rightShoulder[0]-rightElbow[0], 2) + pow(rightShoulder[1]-rightElbow[1], 2) + pow(rightShoulder[2]-rightElbow[2], 2));

	return acos(num/den);
}

double calculateBeta(tf::StampedTransform tf_elbow, tf::StampedTransform tf_neck, tf::StampedTransform tf_shoulder)
{
	double rightElbow[3] = {tf_elbow.getOrigin().x(), tf_elbow.getOrigin().y(), tf_elbow.getOrigin().z()};
	double neck[3] = {tf_neck.getOrigin().x(), tf_neck.getOrigin().y(), tf_neck.getOrigin().z()};
	double rightShoulder[3] = {tf_shoulder.getOrigin().x(), tf_shoulder.getOrigin().y(), tf_shoulder.getOrigin().z()};

	//Al-Kashi triangle formula
	double num =  pow(rightElbow[0]-rightShoulder[0], 2) + pow(rightElbow[1]-rightShoulder[1], 2) + pow(rightElbow[2]-rightShoulder[2], 2)
						+ pow(rightShoulder[0]-neck[0], 2) + pow(rightShoulder[1]-neck[1], 2) + pow(rightShoulder[2]-neck[2], 2)
						- pow(neck[0]-rightElbow[0], 2) - pow(neck[1]-rightElbow[1], 2) - pow(neck[2]-rightElbow[2], 2);

	double den = 2 * sqrt(pow(rightElbow[0]-rightShoulder[0], 2) + pow(rightElbow[1]-rightShoulder[1], 2) + pow(rightElbow[2]-rightShoulder[2], 2)) * sqrt(pow(rightShoulder[0]-neck[0], 2) + pow(rightShoulder[1]-neck[1], 2) + pow(rightShoulder[2]-neck[2], 2));

	return acos(num/den);
}

double calculateTheta(tf::StampedTransform tf_elbow, tf::StampedTransform tf_neck, tf::StampedTransform tf_shoulder,  tf::StampedTransform tf_torso)
{
	double rightElbow[3] = {tf_elbow.getOrigin().x(), tf_elbow.getOrigin().y(), tf_elbow.getOrigin().z()};
	double neck[3] = {tf_neck.getOrigin().x(), tf_neck.getOrigin().y(), tf_neck.getOrigin().z()};
	double rightShoulder[3] = {tf_shoulder.getOrigin().x(), tf_shoulder.getOrigin().y(), tf_shoulder.getOrigin().z()};
	double torso[3] = {tf_torso.getOrigin().x(), tf_torso.getOrigin().y(), tf_torso.getOrigin().z()};

	////Theta is the angle between two planes : TNE and SNE
	double a1 = (rightShoulder[1]-torso[1])*(neck[2]-torso[2]) - (rightShoulder[2]-torso[2])*(neck[1]-torso[1]);
	double b1 = (rightShoulder[2]-torso[2])*(neck[0]-torso[0]) - (rightShoulder[0]-torso[0])*(neck[2]-torso[2]);
	double c1 = (rightShoulder[0]-torso[0])*(neck[1]-torso[1]) - (rightShoulder[1]-torso[1])*(neck[0]-torso[0]);

	double a2 = (rightShoulder[1]-rightElbow[1])*(neck[2]-rightElbow[2]) - (rightShoulder[2]-rightElbow[2])*(neck[1]-rightElbow[1]);
	double b2 = (rightShoulder[2]-rightElbow[2])*(neck[0]-rightElbow[0]) - (rightShoulder[0]-rightElbow[0])*(neck[2]-rightElbow[2]);
	double c2 = (rightShoulder[0]-rightElbow[0])*(neck[1]-rightElbow[1]) - (rightShoulder[1]-rightElbow[1])*(neck[0]-rightElbow[0]);

	double num = a1*a2 + b1*b2 + c1*c2;

	double den = sqrt((a1*a1+b1*b1+c1*c1)*(a2*a2+b2*b2+c2*c2));

	return acos(num/den);
}

double calculatePhi(tf::StampedTransform tf_hand, tf::StampedTransform tf_neck, tf::StampedTransform tf_shoulder,  tf::StampedTransform tf_elbow)
{
	double H[3] = {tf_hand.getOrigin().x(), tf_hand.getOrigin().y(), tf_hand.getOrigin().z()};
	double N[3] = {tf_neck.getOrigin().x(), tf_neck.getOrigin().y(), tf_neck.getOrigin().z()};
	double S[3] = {tf_shoulder.getOrigin().x(), tf_shoulder.getOrigin().y(), tf_shoulder.getOrigin().z()};
	double E[3] = {tf_elbow.getOrigin().x(), tf_elbow.getOrigin().y(), tf_elbow.getOrigin().z()};

	//Phi is the angle between two planes : SNE and SHE
	float n1[3] = {(N[1]-S[1])*(E[2]-S[2])-(N[2]-S[2])*(E[1]-S[1]),
					(N[2]-S[2])*(E[0]-S[0])-(N[0]-S[0])*(E[2]-S[2]),
					(N[0]-S[0])*(E[1]-S[1])-(N[1]-S[1])*(E[0]-S[0])}; //normal plane vector : SNE

	float n2[3] = {(H[1]-S[1])*(E[2]-S[2])-(H[2]-S[2])*(E[1]-S[1]),
						(H[2]-S[2])*(E[0]-S[0])-(H[0]-S[0])*(E[2]-S[2]),
						(H[0]-S[0])*(E[1]-S[1])-(H[1]-S[1])*(E[0]-S[0])}; //normal plane vector : SHE

	float a1,b1,c1;
	a1 = n1[0];
	b1 = n1[1];
	c1 = n1[2];

	float a2,b2,c2;
	a2 = n2[0];
	b2 = n2[1];
	c2 = n2[2];

	double angle_phi = acos(fabs((a1*a2 + b1*b2 + c1*c2)/(sqrt((a1*a1+b1*b1+c1*c1)*(a2*a2+b2*b2+c2*c2)))));

	//std::cerr << "angle_phi" << angle_phi << std::endl;

	return angle_phi;

	//right
	/*
	double a1 = (rightShoulder[1]-rightHand[1])*(rightElbow[2]-rightHand[2]) - (rightShoulder[2]-rightHand[2])*(rightElbow[1]-rightHand[1]);
	double b1 = (rightShoulder[2]-rightHand[2])*(rightElbow[0]-rightHand[0]) - (rightShoulder[0]-rightHand[0])*(rightElbow[2]-rightHand[2]);
	double c1 = (rightShoulder[0]-rightHand[0])*(rightElbow[1]-rightHand[1]) - (rightShoulder[1]-rightHand[1])*(rightElbow[0]-rightHand[0]);

	double a2 = (rightShoulder[1]-rightHand[1])*(neck[2]-rightHand[2]) - (rightShoulder[2]-rightHand[2])*(neck[1]-rightHand[1]);
	double b2 = (rightShoulder[2]-rightHand[2])*(neck[0]-rightHand[0]) - (rightShoulder[0]-rightHand[0])*(neck[2]-rightHand[2]);
	double c2 = (rightShoulder[0]-rightHand[0])*(neck[1]-rightHand[1]) - (rightShoulder[1]-rightHand[1])*(neck[0]-rightHand[0]);

	double num = a1*a2 + b1*b2 + c1*c2;

	double den = sqrt((a1*a1+b1*b1+c1*c1)*(a2*a2+b2*b2+c2*c2));

	return acos(num/den);
	*/
}
/*
double calculateLambda(tf::StampedTransform tf_hand, tf::StampedTransform tf_elbow, tf::StampedTransform tf_shoulder)
{
	double leftHand[3] = {tf_hand.getOrigin().x(), tf_hand.getOrigin().y(), tf_hand.getOrigin().z()};
	double leftElbow[3] = {tf_elbow.getOrigin().x(), tf_elbow.getOrigin().y(), tf_elbow.getOrigin().z()};
	double leftShoulder[3] = {tf_shoulder.getOrigin().x(), tf_shoulder.getOrigin().y(), tf_shoulder.getOrigin().z()};

	//Al-Kashi triangle formula
	double num =  pow(rightHand[0]-rightElbow[0], 2) + pow(rightHand[1]-rightElbow[1], 2) + pow(rightHand[2]-rightElbow[2], 2)
				+ pow(rightShoulder[0]-rightElbow[0], 2) + pow(rightShoulder[1]-rightElbow[1], 2) + pow(rightShoulder[2]-rightElbow[2], 2)
				- pow(rightShoulder[0]-rightHand[0], 2) - pow(rightShoulder[1]-rightHand[1], 2) - pow(rightShoulder[2]-rightHand[2], 2);

	double den = 2 * sqrt(pow(rightHand[0]-rightElbow[0], 2) + pow(rightHand[1]-rightElbow[1], 2) + pow(rightHand[2]-rightElbow[2], 2)) * sqrt(pow(rightShoulder[0]-rightElbow[0], 2) + pow(rightShoulder[1]-rightElbow[1], 2) + pow(rightShoulder[2]-rightElbow[2], 2));

	return acos(num/den);
}
*/
//sensor_msgs::JointState prev_joint_state;
/*
string playerNb_str = "1";
string frame_str = "/openni_depth_frame";
*/
string playerNb_str = "0";
string frame_str = "/camera_depth_optical_frame";
/*
void playerNumber_cb(const std_msgs::Int16ConstPtr& number)
{
	playerNb_str = boost::lexical_cast<string>(number->data);

	//ROS_INFO("Player Number is %s", playerNb_str);
	//cout << "number = " << playerNb_str << endl;
	ROS_INFO("number = %d", number->data);

	if(number->data == 0)
	{
		frame_str = "/camera_depth_optical_frame";
	}
	else
	{
		frame_str = "/openni_depth_frame";
	}
}
*/
int main(int argc, char** argv)
{
	ros::init(argc, argv, "arm_transform");
	ros::NodeHandle n;
	ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states_kinect", 100);
	ros::Publisher test_pub = n.advertise<std_msgs::Float32>("/test", 1);

	ros::Rate loop_rate(50);
	//ros::Rate loop_rate(10);

	tf::TransformListener listener_neck, listener_torso, listener_shoulder, listener_elbow, listener_hand;
	tf::StampedTransform transform_neck, transform_torso, transform_shoulder, transform_elbow, transform_hand;

	//left part
	tf::TransformListener listener_leftShoulder, listener_leftElbow, listener_leftHand;
	tf::StampedTransform transform_leftShoulder, transform_leftElbow, transform_leftHand;

	//read the player number : 0 used for the posture generated by IR markers, other numbers used by openni_tracker
	//ros::Subscriber sub_playerNumber = n.subscribe("/playerNumber", 1, &playerNumber_cb);

	bool error = false;

	//tf::TransformBroadcaster transform;
	//transform.sendTransform(tf::StampedTransform(null, ros::Time::now(), ));

	// message declarations
	sensor_msgs::JointState joint_state;
/*
	//init joint state
	//update joint_state
	joint_state.header.stamp = ros::Time::now();
	joint_state.name.resize(6);
	joint_state.position.resize(6);
	joint_state.name[0] ="elbow_alpha_joint";
	joint_state.position[0] = M_PI/2;
	joint_state.name[1] ="shoulder_beta_joint";
	joint_state.position[1] = M_PI/2;
	joint_state.name[2] ="shoulder_theta_joint";
	joint_state.position[2] = M_PI/2;
	joint_state.name[3] ="shoulder_phi_joint";
	joint_state.position[3] = 3*M_PI/4;
	joint_state.name[4] ="elbow_twist_joint";
	joint_state.position[4] = M_PI/2;
	joint_state.name[5] ="wrist_joint";
	joint_state.position[5] = M_PI/2;

	prev_joint_state = joint_state;
*/
	//uint32_t begin, end, loopTime;

	while (ros::ok())
	{/*
		begin = ros::WallTime::now().nsec;
		error = false;
*/

		//grab tf skeleton joints
		try
		{
			listener_neck.lookupTransform(frame_str, "/neck_" + playerNb_str, ros::Time(0), transform_neck);
		}
		catch (tf::TransformException ex)
		{
			//ROS_ERROR("%s",ex.what());
			error = true;
		}

		try
		{
			listener_torso.lookupTransform(frame_str, "/torso_" + playerNb_str, ros::Time(0), transform_torso);
		}
		catch (tf::TransformException ex)
		{
			//ROS_ERROR("%s",ex.what());
			error = true;
		}

		try
		{
			listener_shoulder.lookupTransform(frame_str, "/left_shoulder_" + playerNb_str, ros::Time(0), transform_shoulder);
		}
		catch (tf::TransformException ex)
		{
			//ROS_ERROR("%s",ex.what());
			error = true;
		}

		try
		{
			listener_elbow.lookupTransform(frame_str, "/left_elbow_" + playerNb_str, ros::Time(0), transform_elbow);
		}
		catch (tf::TransformException ex)
		{
			//ROS_ERROR("%s",ex.what());
			error = true;
		}

		try
		{
			listener_hand.lookupTransform(frame_str, "/left_hand_" + playerNb_str, ros::Time(0), transform_hand);
		}
		catch (tf::TransformException ex)
		{
			//ROS_ERROR("%s",ex.what());
			error = true;
		}
/*
		//get the left arm transform
		try
		{
			listener_leftShoulder.lookupTransform(frame_str, "/right_shoulder_" + playerNb_str, ros::Time(0), transform_leftShoulder);
		}
		catch (tf::TransformException ex)
		{
			//ROS_ERROR("%s",ex.what());
			error = true;
		}

		try
		{
			listener_leftElbow.lookupTransform(frame_str, "/right_elbow_" + playerNb_str, ros::Time(0), transform_leftElbow);
		}
		catch (tf::TransformException ex)
		{
			//ROS_ERROR("%s",ex.what());
			error = true;
		}

		try
		{
			listener_leftHand.lookupTransform(frame_str, "/right_hand_" + playerNb_str, ros::Time(0), transform_leftHand);
		}
		catch (tf::TransformException ex)
		{
			//ROS_ERROR("%s",ex.what());
			error = true;
		}
*/
		//ROS_INFO("transform_han %f", transform_hand.getOrigin().x());

		//update joint_state
		joint_state.header.stamp = ros::Time::now();
		joint_state.name.resize(4);//6
		joint_state.position.resize(4);//6
		joint_state.name[0] ="elbow_alpha_joint";
		joint_state.position[0] = M_PI - calculateAlpha(transform_hand, transform_elbow, transform_shoulder);
		joint_state.name[1] ="shoulder_beta_joint";
		joint_state.position[1] = M_PI - calculateBeta(transform_elbow, transform_neck, transform_shoulder);
		joint_state.name[2] ="shoulder_theta_joint";
		joint_state.position[2] = calculateTheta(transform_elbow, transform_neck, transform_shoulder, transform_torso) - M_PI/2;
		joint_state.name[3] ="shoulder_phi_joint";
		joint_state.position[3] = calculatePhi(transform_hand, transform_neck, transform_shoulder, transform_elbow);

		/*
		joint_state.name[4] ="elbow_twist_joint";
		joint_state.position[4] = 2*calculatePhi(transform_leftHand, transform_neck, transform_leftShoulder, transform_leftElbow) - 3*M_PI/4;
		joint_state.name[5] ="wrist_joint";
		joint_state.position[5] = M_PI - calculateAlpha(transform_leftHand, transform_leftElbow, transform_leftShoulder);
*/
		//test
		std_msgs::Float32 test;
		test.data = M_PI;//(float)transform_hand.getOrigin().x();

		test_pub.publish(test);

		//send the joint state and transform
		joint_pub.publish(joint_state);
/*
			prev_joint_state = joint_state;
		}
		else
		{
			//std::cerr << "Error" << std::endl;
			joint_pub.publish(prev_joint_state);
		}
*/

		ros::spinOnce(); //reasign a new player if need

		// This will adjust as needed per iteration
		loop_rate.sleep();

		//if(error) ROS_INFO("Error !!!");
/*
		end = ros::WallTime::now().nsec;
		loopTime = end - begin;
		ROS_INFO("loopTime =  %d", loopTime);
	*/
	}

	return 0;
}


#ifndef STAN_MATH_PRIM_MAT_PROB_CATEGORICAL_LPMF_HPP
#define STAN_MATH_PRIM_MAT_PROB_CATEGORICAL_LPMF_HPP

#include <stan/math/prim/mat/err/check_simplex.hpp>
#include <stan/math/prim/scal/err/check_bounded.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/mat/fun/sum.hpp>
#include <stan/math/prim/mat/meta/index_type.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/variate_generator.hpp>
#include <cmath>
#include <vector>

namespace stan {
  namespace math {

    // Categorical(n|theta)  [0 < n <= N;   0 <= theta[n] <= 1;  SUM theta = 1]
    template <bool propto,
              typename T_prob>
    typename boost::math::tools::promote_args<T_prob>::type
    categorical_lpmf(int n,
                    const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>& theta) {
      static const char* function("categorical_lpmf");

      using boost::math::tools::promote_args;
      using std::log;

      int lb = 1;

      check_bounded(function, "Number of categories", n, lb, theta.size());
      check_simplex(function, "Probabilities parameter", theta);

      if (include_summand<propto, T_prob>::value)
        return log(theta(n - 1));
      return 0.0;
    }

    template <typename T_prob>
    inline
    typename boost::math::tools::promote_args<T_prob>::type
    categorical_lpmf(const typename
                    math::index_type<Eigen::Matrix<T_prob,
                    Eigen::Dynamic, 1> >::type n,
                    const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>& theta) {
      return categorical_lpmf<false>(n, theta);
    }

    template <bool propto,
              typename T_prob>
    typename boost::math::tools::promote_args<T_prob>::type
    categorical_lpmf(const std::vector<int>& ns,
                    const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>& theta) {
      static const char* function("categorical_lpmf");

      using boost::math::tools::promote_args;
      using std::log;

      int lb = 1;

      for (size_t i = 0; i < ns.size(); ++i)
        check_bounded(function, "element of outcome array", ns[i],
                      lb, theta.size());

      check_simplex(function, "Probabilities parameter", theta);

      if (!include_summand<propto, T_prob>::value)
        return 0.0;

      if (ns.size() == 0)
        return 0.0;

      Eigen::Matrix<T_prob, Eigen::Dynamic, 1> log_theta(theta.size());
      for (int i = 0; i < theta.size(); ++i)
        log_theta(i) = log(theta(i));

      Eigen::Matrix<typename boost::math::tools::promote_args<T_prob>::type,
                    Eigen::Dynamic, 1> log_theta_ns(ns.size());
      for (size_t i = 0; i < ns.size(); ++i)
        log_theta_ns(i) = log_theta(ns[i] - 1);

      return sum(log_theta_ns);
    }

    template <typename T_prob>
    inline
    typename boost::math::tools::promote_args<T_prob>::type
    categorical_lpmf(const std::vector<int>& ns,
                    const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>& theta) {
      return categorical_lpmf<false>(ns, theta);
    }

  }
}
#endif


// MIT License
//
// Copyright (c) 2020 Lennart Braun
//
// 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 "tensor_op.h"

#include <boost/functional/hash.hpp>
#include <cassert>

namespace MOTION::tensor {

bool Conv2DOp::verify() const noexcept {
  bool result = true;
  result = result && (output_shape_ == compute_output_shape());
  result = result && strides_[0] > 0 && strides_[1] > 0;
  // maybe add more checks here
  return result;
}

std::array<std::size_t, 3> Conv2DOp::compute_output_shape() const noexcept {
  const auto compute_output_dimension = [](auto input_size, auto kernel_size, auto padding_begin,
                                           auto padding_end, auto stride) {
    assert(stride != 0);
    return (input_size - kernel_size + padding_begin + padding_end + stride) / stride;
  };

  std::array<std::size_t, 3> output_shape;
  output_shape[0] = kernel_shape_[0];
  output_shape[1] =
      compute_output_dimension(input_shape_[1], kernel_shape_[2], pads_[0], pads_[2], strides_[0]);
  output_shape[2] =
      compute_output_dimension(input_shape_[2], kernel_shape_[3], pads_[1], pads_[3], strides_[1]);
  return output_shape;
}

std::size_t Conv2DOp::compute_output_size() const noexcept {
  assert(verify());
  auto output_shape = compute_output_shape();
  return output_shape[0] * output_shape[1] * output_shape[2];
}

std::size_t Conv2DOp::compute_input_size() const noexcept {
  assert(verify());
  return input_shape_[0] * input_shape_[1] * input_shape_[2];
}

std::size_t Conv2DOp::compute_kernel_size() const noexcept {
  assert(verify());
  return kernel_shape_[0] * kernel_shape_[1] * kernel_shape_[2] * kernel_shape_[3];
}

std::size_t Conv2DOp::compute_bias_size() const noexcept {
  assert(verify());
  return kernel_shape_[0];
}

std::pair<std::size_t, std::size_t> Conv2DOp::compute_input_matrix_shape() const noexcept {
  assert(verify());
  std::size_t num_rows = kernel_shape_[1] * kernel_shape_[2] * kernel_shape_[3];
  std::size_t num_columns = output_shape_[1] * output_shape_[2];
  return {num_rows, num_columns};
}

std::pair<std::size_t, std::size_t> Conv2DOp::compute_kernel_matrix_shape() const noexcept {
  assert(verify());
  std::size_t num_rows = kernel_shape_[0];
  std::size_t num_columns = kernel_shape_[1] * kernel_shape_[2] * kernel_shape_[3];
  return {num_rows, num_columns};
}

std::pair<std::size_t, std::size_t> Conv2DOp::compute_output_matrix_shape() const noexcept {
  assert(verify());
  std::size_t num_rows = kernel_shape_[0];
  std::size_t num_columns = output_shape_[1] * output_shape_[2];
  return {num_rows, num_columns};
}

TensorDimensions Conv2DOp::get_input_tensor_dims() const noexcept {
  assert(verify());
  return {.batch_size_ = 1,
          .num_channels_ = input_shape_[0],
          .height_ = input_shape_[1],
          .width_ = input_shape_[2]};
}

TensorDimensions Conv2DOp::get_kernel_tensor_dims() const noexcept {
  assert(verify());
  return {.batch_size_ = kernel_shape_[0],
          .num_channels_ = kernel_shape_[1],
          .height_ = kernel_shape_[2],
          .width_ = kernel_shape_[3]};
}

TensorDimensions Conv2DOp::get_output_tensor_dims() const noexcept {
  assert(verify());
  return {.batch_size_ = 1,
          .num_channels_ = output_shape_[0],
          .height_ = output_shape_[1],
          .width_ = output_shape_[2]};
}

bool Conv2DOp::operator==(const Conv2DOp& other) const noexcept {
  assert(verify());
  assert(other.verify());
  bool result = true;
  result = result && kernel_shape_ == other.kernel_shape_;
  result = result && input_shape_ == other.input_shape_;
  result = result && output_shape_ == other.output_shape_;
  result = result && dilations_ == other.dilations_;
  result = result && pads_ == other.pads_;
  result = result && strides_ == other.strides_;
  return result;
}

bool GemmOp::verify() const noexcept {
  bool result = true;
  std::size_t m = input_A_shape_[0];
  std::size_t k = input_A_shape_[1];
  if (transA_) {
    std::swap(m, k);
  }
  std::size_t n = input_B_shape_[transB_ ? 0 : 1];
  result = result && (k == input_B_shape_[transB_ ? 1 : 0]);
  result = result && (m == output_shape_[0]);
  result = result && (n == output_shape_[1]);
  // maybe add more checks here
  return result;
}

std::array<std::size_t, 2> GemmOp::compute_output_shape() const noexcept {
  return {input_A_shape_[transA_ ? 1 : 0], input_B_shape_[transB_ ? 0 : 1]};
}

std::size_t GemmOp::compute_output_size() const noexcept {
  assert(verify());
  auto output_shape = compute_output_shape();
  return output_shape[0] * output_shape[1];
}

std::size_t GemmOp::compute_input_A_size() const noexcept {
  assert(verify());
  return input_A_shape_[0] * input_A_shape_[1];
}

std::size_t GemmOp::compute_input_B_size() const noexcept {
  assert(verify());
  return input_B_shape_[0] * input_B_shape_[1];
}

TensorDimensions GemmOp::get_input_A_tensor_dims() const noexcept {
  assert(verify());
  return {.batch_size_ = 1,
          .num_channels_ = 1,
          .height_ = input_A_shape_[0],
          .width_ = input_A_shape_[1]};
}

TensorDimensions GemmOp::get_input_B_tensor_dims() const noexcept {
  assert(verify());
  return {.batch_size_ = 1,
          .num_channels_ = 1,
          .height_ = input_B_shape_[0],
          .width_ = input_B_shape_[1]};
}

TensorDimensions GemmOp::get_output_tensor_dims() const noexcept {
  assert(verify());
  return {.batch_size_ = 1,
          .num_channels_ = 1,
          .height_ = output_shape_[0],
          .width_ = output_shape_[1]};
}

bool GemmOp::operator==(const GemmOp& other) const noexcept {
  assert(verify());
  assert(other.verify());
  bool result = true;
  result = result && input_A_shape_ == other.input_A_shape_;
  result = result && input_B_shape_ == other.input_B_shape_;
  result = result && output_shape_ == other.output_shape_;
  return result;
}

TensorDimensions flatten(const TensorDimensions& dims, std::size_t axis) {
  std::size_t height = 1;
  std::size_t width = 1;
  switch (axis) {
    case 0:
      width *= dims.batch_size_;
      [[fallthrough]];
    case 1:
      width *= dims.num_channels_;
      [[fallthrough]];
    case 2:
      width *= dims.height_;
      [[fallthrough]];
    case 3:
      width *= dims.width_;
  }
  switch (axis) {
    case 4:
      height *= dims.width_;
      [[fallthrough]];
    case 3:
      height *= dims.height_;
      [[fallthrough]];
    case 2:
      height *= dims.num_channels_;
      [[fallthrough]];
    case 1:
      height *= dims.batch_size_;
  }
  return {.batch_size_ = 1, .num_channels_ = 1, .height_ = height, .width_ = width};
}

bool MaxPoolOp::verify() const noexcept {
  bool result = true;
  result = result && (output_shape_ == compute_output_shape());
  result = result && strides_[0] > 0 && strides_[1] > 0;
  result = kernel_shape_[0] <= input_shape_[1] && kernel_shape_[1] <= input_shape_[2];
  // maybe add more checks here
  return result;
}

std::array<std::size_t, 3> MaxPoolOp::compute_output_shape() const noexcept {
  const auto compute_output_dimension = [](auto input_size, auto kernel_size, auto stride) {
    assert(stride != 0);
    return (input_size - kernel_size + stride) / stride;
  };

  std::array<std::size_t, 3> output_shape;
  output_shape[0] = input_shape_[0];
  output_shape[1] =
      compute_output_dimension(input_shape_[1], kernel_shape_[2], strides_[0]);
  output_shape[2] =
      compute_output_dimension(input_shape_[2], kernel_shape_[3], strides_[1]);
  return output_shape;
}

std::size_t MaxPoolOp::compute_kernel_size() const noexcept {
  assert(verify());
  return kernel_shape_[0] * kernel_shape_[1];
}

std::size_t MaxPoolOp::compute_input_size() const noexcept {
  assert(verify());
  return input_shape_[0] * input_shape_[1] * input_shape_[2];
}

std::size_t MaxPoolOp::compute_output_size() const noexcept {
  assert(verify());
  return output_shape_[0] * output_shape_[1] * output_shape_[2];
}

TensorDimensions MaxPoolOp::get_input_tensor_dims() const noexcept {
  assert(verify());
  return {.batch_size_ = 1,
          .num_channels_ = input_shape_[0],
          .height_ = input_shape_[1],
          .width_ = input_shape_[2]};
}

TensorDimensions MaxPoolOp::get_output_tensor_dims() const noexcept {
  assert(verify());
  return {.batch_size_ = 1,
          .num_channels_ = output_shape_[0],
          .height_ = output_shape_[1],
          .width_ = output_shape_[2]};
}

}  // namespace MOTION::tensor

namespace std {

std::size_t std::hash<MOTION::tensor::Conv2DOp>::operator()(
    const MOTION::tensor::Conv2DOp& op) const noexcept {
  std::size_t seed = 0;
  boost::hash_combine(seed,
                      boost::hash_range(std::begin(op.kernel_shape_), std::end(op.kernel_shape_)));
  boost::hash_combine(seed,
                      boost::hash_range(std::begin(op.input_shape_), std::end(op.input_shape_)));
  boost::hash_combine(seed, boost::hash_range(std::begin(op.dilations_), std::end(op.dilations_)));
  boost::hash_combine(seed, boost::hash_range(std::begin(op.pads_), std::end(op.pads_)));
  boost::hash_combine(seed, boost::hash_range(std::begin(op.strides_), std::end(op.strides_)));
  return seed;
}

std::size_t std::hash<MOTION::tensor::GemmOp>::operator()(
    const MOTION::tensor::GemmOp& op) const noexcept {
  std::size_t seed = 0;
  boost::hash_combine(
      seed, boost::hash_range(std::begin(op.input_A_shape_), std::end(op.input_A_shape_)));
  boost::hash_combine(
      seed, boost::hash_range(std::begin(op.input_B_shape_), std::end(op.input_B_shape_)));
  return seed;
}

}  // namespace std


/*
 * ex5.cpp
 *
 * 	\brief     5. und 7. Aufgabe
 *  \details   Liesst eine .gph Datei ein und berechnet den laengsten kuerzesten pfad von jeder Ecke zum Root des Graphen.
 *  \author    Christopher Wyczisk
 *  \date      20.07.2017
 */

#include <fstream>
#include <iostream>
#include <vector>
#include <string>
#include <sstream>
#include "dijkstra.h"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/timer/timer.hpp>

using namespace std;
using namespace boost::timer;

// helper
vector<pair<int, int>> kanten;
vector<int> gewichte;
unsigned int anzahlEckenGlobal;

/**
 *\typedef Adjancen des Graphen
 */
typedef boost::adjacency_list<boost::listS, boost::vecS,
	boost::undirectedS, boost::no_property,
	boost::property<boost::edge_weight_t, int>> graph;

/**
 * \typedef Die Informationen ueber die Ecken des Graphen
 */
typedef boost::graph_traits <graph>::vertex_descriptor eckenDto;

/**
 * Einlesen der Graph Datei und Fehlerbehandlung.
 *
 * \param argc
 * \param argv
 * \param infile input Graph-File
 */
bool graphEinlesen(int argc, char** argv, ifstream& file) {
	if(argc <= 1) {
		cout << "Sie haben keinen Pfad zu einer .gph Datei angegeben." << endl;
		return false;
	} else {
		file.open(argv[1], ios::in);
	}

	if (!file) {
		cout << "Die von Ihnen angegebene Datei konnte nicht geoeffnet werden." << endl;
		return false;
	}

	return true;
}

/**
 * Konsolen-Ausgabe der Anzahl an Ecken und des Gewichts.
 *
 * \param anzahlEcken
 * \param laegsterKuerzesterPfadGewicht
 */
void ergebnisAusgabe(int anzahlEcken, int laegsterKuerzesterPfadGewicht) {
	cout << "RESULT VERTEX " << anzahlEcken << endl;
	cout << "RESULT DIST " << laegsterKuerzesterPfadGewicht << endl;
}

/**
 * Erstellt den Graphen.
 *
 * \param file
 */
bool graphErstellen(ifstream& file) {
	stringstream str;
	string zeile;
	int anzahlKanten;
	int start;
	int end;
	int gewicht;

	if (getline(file, zeile)) {
		str.str(zeile);
		str >> anzahlEckenGlobal >> anzahlKanten;
		anzahlEckenGlobal++;
	} else {
		cerr << "Das .gph File ist leer." << endl;
		return false;
	}

	while (getline(file, zeile)) {
		str.clear();
		str.str(zeile);
		str >> start >> end >> gewicht;
		kanten.push_back(make_pair(start, end));
		gewichte.push_back(gewicht);
	}

	return true;
}

/**
 * Berechnet das Gewicht des longest-shortest Pfad.
 *
 * \param graph der graph
 * \param anzahlEcken
 * \param laengsterKuerzesterPfadGewicht
 */
void berechneLongesteShortesWeigth(graph graph, int& anzahlEcken, int& laegsterKuerzesterPfadGewicht) {
	vector<eckenDto> pfadRichtungen(anzahlEckenGlobal);
	vector<int> gewichteList(anzahlEckenGlobal);

	boost::dijkstra_shortest_paths(
		graph, 1,
		boost::predecessor_map(	boost::make_iterator_property_map(pfadRichtungen.begin(), get(boost::vertex_index, graph)))
			.distance_map(
					boost::make_iterator_property_map(gewichteList.begin(), get(boost::vertex_index, graph
					))));

	// berechnet das Gewicht
	for(int i = 1; i < anzahlEckenGlobal; i++) {
		if (gewichteList[i + 1] > laegsterKuerzesterPfadGewicht) {
			laegsterKuerzesterPfadGewicht = gewichteList[i + 1];
			anzahlEcken = i + 1;
		}
	}
}

string konsolenabfrageWelcheMethode() {
	string eingabe = "";
	while(eingabe.compare("a") && eingabe.compare("b")) {
		cout << "Geben Sie ein, welcher Algorithmus verwendet werden soll (a=Boots, b=eigene Impelementierung):" << endl;
		cin >> eingabe;
	}
	return eingabe;
}

/**
 * Main.
 */
int main(int argc, char* argv[]) {
	ifstream file;
	bool fileEingelesen = graphEinlesen(argc, argv, file);

	string eingabe = konsolenabfrageWelcheMethode();

	boost::timer::cpu_timer t;
	if(fileEingelesen) {
		//erstelle den Graph
		bool erstellt = graphErstellen(file);
		if(erstellt && !eingabe.compare("a")) {
			graph graph{kanten.begin(), kanten.end(), gewichte.begin(), anzahlEckenGlobal};

			// berechnen des gewichts des longesten shortesten pfad
			int laegsterKuerzesterPfadGewicht = -1;
			int anzahlEcken = -1;
			berechneLongesteShortesWeigth(graph, anzahlEcken, laegsterKuerzesterPfadGewicht);

			ergebnisAusgabe(anzahlEcken, laegsterKuerzesterPfadGewicht);
		}
		else if(erstellt) {
			vector<int> mapGewichte(anzahlEckenGlobal);
	    	dijkstra dijkstra(gewichte, kanten, anzahlEckenGlobal);
			mapGewichte = dijkstra.berechneKuerzestenPfad(1);

			// berechnen des gewichts des longesten shortesten pfad
			int laegsterKuerzesterPfadGewicht = -1;
			int anzahlEcken = -1;
			int absGewichte;
			for(unsigned int i = 2; i < anzahlEckenGlobal; i++) {
				absGewichte = mapGewichte[i];
				if (absGewichte > laegsterKuerzesterPfadGewicht) {
					laegsterKuerzesterPfadGewicht = absGewichte;
					anzahlEcken = i;
				}
			}

			ergebnisAusgabe(anzahlEcken, laegsterKuerzesterPfadGewicht);
		}
	}

	boost::timer::cpu_times zeit = t.elapsed();
	std::cout << "WALL-CLOCK " << zeit.wall / 1e9 << "s" << std::endl;
	std::cout << "USER TIME " << zeit.user / 1e9 << "s" << std::endl;
	return 0;
}


/*
 * TriangleElement.hpp
 *
 *  Created on: Nov 18, 2012
 *      Author: petr
 */
#pragma once
#ifndef TRIANGLEELEMENT_HPP_
#define TRIANGLEELEMENT_HPP_

#undef max
#undef min

#include <cmath>
#include <limits>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <vector>
#include <iostream>
#include "BoundingBox.hpp"
#include "utility.h"
#include "tribox3.hpp"

// #include <boost/archive/basic_binary_oarchive.hpp>
// #include <boost/archive/basic_binary_iarchive.hpp>

template<typename type>
struct SignedDistance {
	type            dist;
	type            angle;
	int             sign;
	Eigen::Vector3d minPoint;
};

template <typename type>
class TriangleElement {

public:

	/// Stores vertices of an element.
	/// This implementation uses triangle element, but in future, there will be support for multiple element types.
	std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > vertices;

	/// Face normal
	Eigen::Vector3d normal;

	/// element ID used to identify element
	long int ID;

	TriangleElement(): ID(-1) {};

	/// Triangle element contructor, triangle normal is computed automaticaly
	/// Normal is computed standard way : n = (v1 - v0) x (v2 - v0), where x is cross product
	TriangleElement(Eigen::Vector3d& v0, Eigen::Vector3d& v1, Eigen::Vector3d& v2, long int id);

	TriangleElement(const Eigen::Vector3d& v) {
		this->vertices.reserve(3);

		this->vertices.push_back(v);
		this->vertices.push_back(v);
		this->vertices.push_back(v);

		this->ID = -1;
	}

	/// Constructor to create the triangle element from complete list of vertices with an ID
	TriangleElement(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> >& vertices, long int id);

	TriangleElement(const TriangleElement<type> & element) : vertices(element.vertices), normal(element.normal), ID(element.ID) {};

	/// Test whether element is inside the rectangular! region
	bool isInsideRegion(const Box<type, 3>& bb) const;

	/// Test whether element is partly inside the rectangular! region
	bool isPartlyInRegion(const Box<type, 3>& bb) const;

	/// Return centroid of element
	Eigen::Vector3d centroid();

	static SignedDistance<type> computeDistance(const TriangleElement<type>& el, const Eigen::Vector3d& point);

	static type computePerpendicularDistance(const TriangleElement<type>& el, const Eigen::Vector3d& point);

	/// Return tight Axis Aligned Bounding Box around element
	BoundingBox<type, 3, TriangleElement<double> > getBoundingBox() const;

};

template<typename type>
TriangleElement<type>::TriangleElement(Eigen::Vector3d& v0, Eigen::Vector3d& v1, Eigen::Vector3d& v2, long int id) {

	this->vertices.reserve(3);

	this->vertices.push_back(v0);
	this->vertices.push_back(v1);
	this->vertices.push_back(v2);

	Eigen::Vector3d e0 = v1 - v0;
	Eigen::Vector3d e1 = v2 - v0;

	this->normal = e0.cross(e1);
	this->normal.normalize();

	this->ID = id;

}

template<typename type>
TriangleElement<type>::TriangleElement(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> >& vertices, long int id) {

	this->vertices = vertices;

	Eigen::Vector3d e0 = vertices[1] - vertices[0];
	Eigen::Vector3d e1 = vertices[2] - vertices[0];

	this->normal = e0.cross(e1);
	this->normal.normalize();

	this->ID = id;

}

template<typename type>
inline BoundingBox<type, 3, TriangleElement<double> > TriangleElement<type>::getBoundingBox() const {

	double minX[3], maxX[3];
	minX[0] =  std::numeric_limits<type>::max();
	minX[1] =  std::numeric_limits<type>::max();
	minX[2] =  std::numeric_limits<type>::max();

	maxX[0] = -std::numeric_limits<type>::max();
	maxX[1] = -std::numeric_limits<type>::max();
	maxX[2] = -std::numeric_limits<type>::max();

	for (auto vertex: vertices) {

		// find lower bound
		if ( vertex[0] < minX[0] ) // for x
			minX[0] = vertex[0];
		if ( vertex[1] < minX[1] ) // for y
			minX[1] = vertex[1];
		if ( vertex[2] < minX[2] ) // for z
			minX[2] = vertex[2];

		// find upper bound
		if ( vertex[0] > maxX[0] ) // for x
			maxX[0] = vertex[0];
		if ( vertex[1] > maxX[1] ) // for y
			maxX[1] = vertex[1];
		if ( vertex[2] > maxX[2] ) // for z
			maxX[2] = vertex[2];

	}

	return BoundingBox<type, 3, TriangleElement<double> >(minX, maxX, this);

}

template<typename type>
inline bool TriangleElement<type>::isInsideRegion(const Box<type, 3>& bb) const {

	bool vertex_0 = bb.pointInside(vertices[0].array());
	bool vertex_1 = bb.pointInside(vertices[1].array());
	bool vertex_2 = bb.pointInside(vertices[2].array());

	return vertex_0 && vertex_1 && vertex_2;

}

/// \brief Detects if triangle element collide with cuboid region. Tests for partial collisions
///        which could pose a tricky problem: long thin triangles, one vertex collision,
///        one edge collision, edge to edge collision.
///
/// The algorithm is taken from book: (Morgan Kaufmann series in  Interactive 3d technology)
///                                   Realtime collision detection - Christer Ericson
///
template<typename type>
inline bool TriangleElement<type>::isPartlyInRegion(const Box<type, 3>& bb) const {

	float trivert[3][3] = { {(float)vertices[0][0], (float)vertices[0][1], (float)vertices[0][2]},
							{(float)vertices[1][0], (float)vertices[1][1], (float)vertices[1][2]},
							{(float)vertices[2][0], (float)vertices[2][1], (float)vertices[2][2]}};

	float center[3] = {(float)bb.center(0), (float)bb.center(1), (float)bb.center(2)};

	float extent[3] = {(float)bb.extent(0), (float)bb.extent(1), (float)bb.extent(2)};

	// utilizes code I found on the internet, check the functionality
	return triBoxOverlap(center, extent, trivert);

}

template<typename type>
inline SignedDistance<type> TriangleElement<type>::computeDistance(const TriangleElement<type>& el, const Eigen::Vector3d &point) {
	// point is supposed to have right dimension, so do it nice and clear :)
	// this function computed the distance between triangle element and point in 3D
	// complete description of a method could be found here :
	//  	http://www.geometrictools.com/Documentation/DistancePoint3Triangle3.pdf
	// sample implementation could be found here :
	//		http://www.geometrictools.com/LibMathematics/Distance/Wm5DistPoint3Triangle3.cpp
	// and here :
	// 		http://www.mathworks.com/matlabcentral/fileexchange/22857-distance-between-a-point-and-a-triangle-in-3d/

	//double E0[3] = {el.vertex_x[1] - el.vertex_x[0], el.vertex_y[1] - el.vertex_y[0], el.vertex_z[1] - el.vertex_z[0]};
	//double E1[3] = {el.vertex_x[2] - el.vertex_x[0], el.vertex_y[2] - el.vertex_y[0], el.vertex_z[2] - el.vertex_z[0]};

	Eigen::Vector3d E0 = el.vertices[1] - el.vertices[0];
	Eigen::Vector3d E1 = el.vertices[2] - el.vertices[0];

	//double D[3]  = {el.vertex_x[0] - point[0], el.vertex_y[0] - point[1], el.vertex_z[0] - point[2]};

	Eigen::Vector3d D = el.vertices[0] - point;

//	double a00_ = E0[0]*E0[0] + E0[1]*E0[1] + E0[2]*E0[2];
	type a00 = E0.dot(E0);

//	std::cout << (a00_ - a00) << std::endl;

//	double a01_ = E0[0]*E1[0] + E0[1]*E1[1] + E0[2]*E1[2];
	type a01 = E0.dot(E1);

//	std::cout << (a01_ - a01) << std::endl;

//	double a11_ = E1[0]*E1[0] + E1[1]*E1[1] + E1[2]*E1[2];
	type a11 = E1.dot(E1);

//	std::cout << (a11_ - a11) << std::endl;

//	double b0_  = E0[0]*D[0]  + E0[1]*D[1]  + E0[2]*D[2];
	type b0 = E0.dot(D);

//	std::cout << (b0_ - b0) << std::endl;

//	double b1_  = E1[0]*D[0]  + E1[1]*D[1]  + E1[2]*D[2];
	type b1 = E1.dot(D);

//	std::cout << (b0_ - b0) << std::endl;

//	double c_   = D[0]*D[0]   + D[1]*D[1]   + D[2]*D[2];
	type c = D.dot(D);

//	std::cout << (c_ - c) << std::endl;

	type det = std::abs(a00*a11 - a01*a01);

	type s   = a01*b1 - a11*b0;
	type t   = a01*b0 - a00*b1;

	type sqrDistance = 0;

	if ( (s+t) <= det ) {
		if ( s < 0 ) {
			if ( t < 0 ) {
				// region 4
				if ( b0 < 0 ) {
					t = 0;
					if ( -b0 >= a00 ) {
						s = 1;
						sqrDistance = a00 + 2*b0 + c;
					} else {
						s = -b0/a00;
						sqrDistance = b0*s + c;
					}
				} else {
					s = 0;
					if ( b1 >= 0 ) {
						t = 0;
						sqrDistance = c;
					} else if ( -b1 >= a11 ) {
						t = 1;
						sqrDistance = a11 + 2*b1 + c;
					} else {
						t = -b1/a11;
						sqrDistance = b1*t + c;
					}
				}
				// end of region 4
			} else {
				// region 3
				s = 0;
				if ( b1 >= 0 ) {
					t = 0;
					sqrDistance = c;
				} else if ( -b1 >= a11 ) {
					t = 1;
					sqrDistance = a11 + 2*b1 + c;
				} else {
					t = -b1/a11;
					sqrDistance = b1*t + c;
				}
				// end of region 3
			}
		} else if ( t < 0) {
			// region 5
			t = 0;
			if ( b0 >= 0 ) {
				s = 0;
				sqrDistance = c;
			} else if ( -b0 >= a00 ) {
				s = 1;
				sqrDistance = a00 + 2*b0 + c;
			} else {
				s = -b0/a00;
				sqrDistance = b0*s + c;
			}
			// end of region 5
		} else {
			// region 0
			double invDet = 1 / det;
			s *= invDet;
			t *= invDet;
			sqrDistance = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
			// end of region 0
		}
	} else {
		if ( s < 0 ) {
			// region 2
			double tmp0 = a01 + b0;
			double tmp1 = a11 + b1;
			if ( tmp1 > tmp0 ) { // minimum on edge s+t=1
				double numer = tmp1 - tmp0;
				double denom = a00 - 2*a01 + a11;
				if ( numer >= denom ) {
					s = 1;
					t = 0;
					sqrDistance = a00 + 2*b0 + c;
				} else {
					s = numer/denom;
					t = 1 - s;
					sqrDistance = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
				}
			} else { // minimum on edge s=0
				s = 0;
				if ( tmp1 <= 0 ) {
					t = 1;
					sqrDistance = a11 + 2*b1 + c;
				} else if ( b1 >= 0 ) {
					t = 0;
					sqrDistance = c;
				} else {
					t = -b1/a11;
					sqrDistance = b1*t + c;
				}
			}
			// end of region 2
		} else if ( t < 0 ) {
			// region 6
			double tmp0 = a01 + b1;
			double tmp1 = a00 + b0;
			if ( tmp1 > tmp0 ) {
				double numer = tmp1 - tmp0;
				double denom = a00 - 2*a01 + a11;
				if ( numer >= denom ) {
					t = 1;
					s = 0;
					sqrDistance = a11 + 2*b1 + c;
				} else {
					t = numer/denom;
					s = 1 - t;
					sqrDistance = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
				}
			} else {
				t = 0;
				if ( tmp1 <= 0 ) {
					s = 1;
					sqrDistance = a00 + 2*b0 + c;
				} else if ( b0 >= 0 ) {
					s = 0;
					sqrDistance = c;
				} else {
					s = -b0/a00;
					sqrDistance = b0*s + c;
				}
			}
			// end of region 6
		} else {
			// region 1
			double numer = a11 + b1 - a01 - b0;
			if ( numer <= 0 ) {
				s = 0;
				t = 1;
				sqrDistance = a11 + 2*b1 + c;
			} else {
				double denom = a00 - 2*a01 + a11;
				if ( numer >= denom ) {
					s = 1;
					t = 0;
					sqrDistance = a00 + 2*b0 + c;
				} else {
					s = numer / denom;
					t = 1 - s;
					sqrDistance = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
				}
			}
			// end of region 1
		}
	}

	double eeps = my_eps;

	if (sqrDistance <= eeps)
		sqrDistance = 0.0;

	SignedDistance<type> dist;

	dist.dist = std::sqrt(sqrDistance);

	dist.minPoint = el.vertices[0] + s*E0 + t*E1;

//	dist.minPoint[0] = (el.vertex_x[0] + s*E0[0] + t*E1[0]);
//	dist.minPoint[1] = (el.vertex_y[0] + s*E0[1] + t*E1[1]);
//	dist.minPoint[2] = (el.vertex_z[0] + s*E0[2] + t*E1[2]);

//	double PP0[3] = { point[0] - dist.minPoint[0],
//					  point[1] - dist.minPoint[1],
//					  point[2] - dist.minPoint[2] };

//	double nrm = sqrt(PP0[0]*PP0[0] + PP0[1]*PP0[1] + PP0[2]*PP0[2]);

	// compute on which side of the triangle we are
	Eigen::Vector3d PP0 = point - dist.minPoint;

	if ( PP0.isZero(eeps) ) {
		// PP0 = Eigen::Vector3d::Zero();
	} else {
		PP0.normalize();
	}

	//double nrm_normal = sqrt(el.normal_x*el.normal_x + el.normal_y*el.normal_y + el.normal_z*el.normal_z);

//	double dprod = (el.normal_x/nrm_normal) * (PP0[0]/nrm) +
//				   (el.normal_y/nrm_normal) * (PP0[1]/nrm) +
//				   (el.normal_z/nrm_normal) * (PP0[2]/nrm);

	type dprod = el.normal.dot(PP0);

//	std::cout << el.normal.transpose() << " DOT " << PP0.transpose() << " = " << dprod << std::endl;

	int sgn = 0;
	if (dprod > eeps)
		sgn = 1;
	else if (dprod < -eeps)
		sgn = -1;

	dist.angle = dprod;
	dist.sign  = sgn;

	return dist;

}

template<typename type>
inline Eigen::Vector3d TriangleElement<type>::centroid() {

	Eigen::Vector3d c;

	for (auto vertex: vertices) {
		c += vertex;
	}

	return c*(1.0/3.0);

}

template<typename type>
inline type TriangleElement<type>::computePerpendicularDistance(const TriangleElement& el, const Eigen::Vector3d& point) {
	// compute projection onto surface defined by the triangle element
	Eigen::Vector3d vec;
	vec << point - el.vertices[0];

	type ortDist = std::abs( vec.dot(el.normal) );

	return ortDist;
}

// ostream operators

template <typename type>
std::ostream& operator<<(std::ostream& os, const TriangleElement<type>& el) {
	os << "vert_0: " << el.vertices[0].transpose() << std::endl;
	os << "vert_1: " << el.vertices[1].transpose() << std::endl;
	os << "vert_2: " << el.vertices[2].transpose() << std::endl;
	os << "normal: " << el.normal.transpose();

    return os;
}

struct TriangleCompare {

	bool operator() (spatial::dimension_type dim, const TriangleElement<double>& a, const TriangleElement<double>& b) const {

		switch (dim) {
			case 0:
//				a.vertices[0]
				return (a.vertices[0][0] < b.vertices[0][0]) &&
					   (a.vertices[1][0] < b.vertices[1][0]) &&
					   (a.vertices[2][0] < b.vertices[2][0]);
				break;
			case 1:
				return (a.vertices[0][1] < b.vertices[0][1]) &&
					   (a.vertices[1][1] < b.vertices[1][1]) &&
					   (a.vertices[2][1] < b.vertices[2][1]);
//				return (a._tr[0] < b._tr[0]);
				break;
			case 2:
				return (a.vertices[0][2] < b.vertices[0][2]) &&
					   (a.vertices[1][2] < b.vertices[1][2]) &&
					   (a.vertices[2][2] < b.vertices[2][2]);
//				return (a._bl[1] < b._bl[1]);
				break;

		}

		return false;
	}

};

struct TrianglePredicate {
	const Box<double, 3>& box;

	TrianglePredicate(const Box<double, 3>& b) : box(b) {}

	spatial::relative_order
	operator() (spatial::dimension_type dim, spatial::dimension_type, const TriangleElement<double>& t ) const {

		Box<double, 3> b = t.getBoundingBox();

		switch (dim) {
			case 0:
				return (b._bl[0] > box._tr[0] && b._tr[0] > box._tr[0]) ? spatial::above :
					   (b._tr[0] < box._bl[0] && b._bl[0] < box._bl[0]) ? spatial::below :
						spatial::matching;
				break;
			case 1:
				return (b._bl[1] > box._tr[1] && b._tr[1] > box._tr[1]) ? spatial::above :
					   (b._tr[1] < box._bl[1] && b._bl[1] < box._bl[1]) ? spatial::below :
						spatial::matching;
				break;
			case 2:
				return (b._bl[2] > box._tr[2] && b._tr[2] > box._tr[2]) ? spatial::above :
					   (b._tr[2] < box._bl[2] && b._bl[2] < box._bl[2]) ? spatial::below :
						spatial::matching;
				break;
		}

	}
};

struct TriangleMetric {
    // Check that DistanceType is a fundamental floating point type
    typedef double distance_type;

    /**
     *  Compute the distance between the point of \c origin and the \c key.
     *  \return The resulting square distance.
     */
    distance_type
    distance_to_key(spatial::dimension_type rank,
                    const TriangleElement<double>& origin,
                    const TriangleElement<double>& key) const {

    	distance_type result = distance_type(0.0);

//    	std::cout << "===============================================" << std::endl;
//    	std::cout << origin << std::endl;
//    	std::cout << "--------------" << std::endl;
//    	std::cout << key << std::endl;
//    	std::cout << "--------------" << std::endl;

    	Eigen::Array3d co = (origin.vertices[0] + origin.vertices[1] + origin.vertices[2]).array() / 3.0;
    	Eigen::Array3d ck = (key.vertices[0] + key.vertices[1] + key.vertices[2]).array() / 3.0;

    	// distance_type tmp = 0.0

		for (int ii = 0; ii < 3; ++ii) {
			double d = std::numeric_limits<double>::max();
			for (spatial::dimension_type i = 0; i < rank; ++i) {
				double tmp = std::abs( key.vertices[i][ii] - origin.vertices[i][ii] );
				d = (tmp < d) ? tmp : d;
			}

			result += d * d;
		}

//    	std::cout << "dist = " << result << std::endl;
//    	std::cout << "===============================================" << std::endl;
    	return result;

    }

    /**
     *  The distance between the point of \c origin and the closest point to
     *  the plane orthogonal to the axis of dimension \c dim and crossing \c
     *  key.
     *  \return The resulting square distance.
     */
    distance_type
    distance_to_plane(spatial::dimension_type,
    			      spatial::dimension_type dim,
                      const TriangleElement<double>& origin,
                      const TriangleElement<double>& key) const {

    	distance_type d = std::numeric_limits<double>::max();
		for (spatial::dimension_type i = 0; i < 3; ++i) {
			double tmp = std::abs( key.vertices[i][dim] - origin.vertices[i][dim] );
			d = (tmp < d) ? tmp : d;
		}

		return d * d;

    }

};

#endif /* TRIANGLEELEMENT_HPP_ */


#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <Eigen/Dense>

namespace py=pybind11;
typedef Eigen::MatrixXd Matrix;
typedef Eigen::VectorXd Vector;

Eigen::Ref<Matrix> distance_table(
  Eigen::Ref<const Matrix>& pos, double lbox)
{
  const int natom = pos.rows();
  const int ndim = pos.cols();
  Matrix dtable = Matrix::Zero(natom, natom);
  Vector drij(ndim);
  for (int i=0;i<natom;i++)
  {
    for (int j=i; j<natom; j++)
    {
      drij = pos.row(i) - pos.row(j);
      for (int idim=0;idim<ndim;idim++)
      {
        drij(idim) -= lbox*std::round(drij(idim)/lbox);
      }
      dtable(i, j) = drij.norm();
    }
  }
  return dtable;
}

PYBIND11_MODULE(example, m)
{
  //m.def("distance_table", &distance_table, "PBC distances");
  m.def("distance_table", &distance_table, "PBC distances",
        py::return_value_policy::reference_internal);
}


#include <QTime>
#include <QApplication>
#include <QAction>
#include <QMainWindow>
#include <QStringList>

#include "opengl_tools.h"
#include "Scene_polyhedron_item.h"
#include "Scene_surface_mesh_item.h"
#include "Scene_points_with_normal_item.h"
#include "Scene_polylines_item.h"
#include "Scene_polyhedron_selection_item.h"
#include "Polyhedron_type.h"

#include <CGAL/Three/Polyhedron_demo_plugin_interface.h>

#include <CGAL/convex_hull_3.h>
#include <CGAL/boost/graph/copy_face_graph.h>
#include <boost/iterator/transform_iterator.hpp>
using namespace CGAL::Three;
class Polyhedron_demo_convex_hull_plugin :
  public QObject,
  public Polyhedron_demo_plugin_interface
{
  Q_OBJECT
  Q_INTERFACES(CGAL::Three::Polyhedron_demo_plugin_interface)
  Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.PluginInterface/1.0")
public:
    void init(QMainWindow*mw,
              Scene_interface* scene_interface,
              Messages_interface*)
    {
        scene = scene_interface;
        this->mw = mw;
        QAction *actionConvexHull = new QAction("Convex Hull", mw);
        actionConvexHull->setProperty("subMenuName","3D Convex Hulls");
        connect(actionConvexHull, SIGNAL(triggered()), this, SLOT(on_actionConvexHull_triggered()));
        _actions <<actionConvexHull;
    }

  QList<QAction*> actions()const {return _actions;}

  bool applicable(QAction*) const {
    return
      qobject_cast<Scene_polyhedron_item*>(scene->item(scene->mainSelectionIndex())) ||
      qobject_cast<Scene_surface_mesh_item*>(scene->item(scene->mainSelectionIndex())) ||
      qobject_cast<Scene_polylines_item*>(scene->item(scene->mainSelectionIndex())) ||
      qobject_cast<Scene_points_with_normal_item*>(scene->item(scene->mainSelectionIndex())) ||
      qobject_cast<Scene_polyhedron_selection_item*>(scene->item(scene->mainSelectionIndex()));
  }

public Q_SLOTS:
  void on_actionConvexHull_triggered();
private:
  QList<QAction*> _actions;
  Scene_interface* scene;
  QMainWindow* mw;
}; // end Polyhedron_demo_convex_hull_plugin

// for transform iterator
struct Get_point {
  typedef const Polyhedron::Point_3& result_type;
  result_type operator()(const Polyhedron::Vertex_handle v) const
  { return v->point(); }
};

void Polyhedron_demo_convex_hull_plugin::on_actionConvexHull_triggered()
{
  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();

  Scene_polyhedron_item* poly_item =
    qobject_cast<Scene_polyhedron_item*>(scene->item(index));

  Scene_points_with_normal_item* pts_item =
    qobject_cast<Scene_points_with_normal_item*>(scene->item(index));

  Scene_polylines_item* lines_item =
    qobject_cast<Scene_polylines_item*>(scene->item(index));

  Scene_polyhedron_selection_item* selection_item =
    qobject_cast<Scene_polyhedron_selection_item*>(scene->item(index));

  Scene_surface_mesh_item* sm_item =
    qobject_cast<Scene_surface_mesh_item*>(scene->item(index));

  if(poly_item || pts_item || lines_item || selection_item || sm_item)
  {
    // wait cursor
    QApplication::setOverrideCursor(Qt::WaitCursor);

    QTime time;
    time.start();
    std::cout << "Convex hull...";

    // add convex hull as new polyhedron
    SMesh *pConvex_hull  = new SMesh;
    if(selection_item) {
      CGAL::convex_hull_3(
        boost::make_transform_iterator(selection_item->selected_vertices.begin(), Get_point()),
        boost::make_transform_iterator(selection_item->selected_vertices.end(), Get_point()),
        *pConvex_hull);
    }
    else if ( poly_item ){
      Polyhedron* pMesh = poly_item->polyhedron();
      CGAL::convex_hull_3(pMesh->points_begin(),pMesh->points_end(),*pConvex_hull);
    }
    else if ( sm_item ){
      SMesh* pMesh = sm_item->polyhedron();
      typedef boost::property_map<SMesh,CGAL::vertex_point_t>::type Vpmap;

      typedef CGAL::Property_map_to_unary_function<Vpmap> Vpmap_fct;
      Vpmap vpm = get(CGAL::vertex_point,*pMesh);

      Vpmap_fct v2p(vpm);
      boost::graph_traits<SMesh>::vertex_iterator b,e;
      boost::tie(b,e) = vertices(*pMesh);

      CGAL::convex_hull_3(boost::make_transform_iterator(b,v2p),
                          boost::make_transform_iterator(e,v2p),
                          *pConvex_hull);

    }
    else{
      if (pts_item)
        CGAL::convex_hull_3(pts_item->point_set()->points().begin(),
                            pts_item->point_set()->points().end(),
                            *pConvex_hull);
      else{
        std::size_t nb_points=0;
        for(std::list<std::vector<Kernel::Point_3> >::const_iterator it = lines_item->polylines.begin();
            it != lines_item->polylines.end();
            ++it)  nb_points+=it->size();

        std::vector<Kernel::Point_3> all_points;
        all_points.reserve( nb_points );

        for(std::list<std::vector<Kernel::Point_3> >::const_iterator it = lines_item->polylines.begin();
            it != lines_item->polylines.end();
            ++it)  std::copy(it->begin(), it->end(),std::back_inserter( all_points ) );

        CGAL::convex_hull_3(all_points.begin(),all_points.end(),*pConvex_hull);
      }
    }
    std::cout << "ok (" << time.elapsed() << " ms)" << std::endl;

    if(mw->property("is_polyhedron_mode").toBool()){
      Polyhedron *poly = new Polyhedron;
      CGAL::copy_face_graph(*pConvex_hull,*poly);
      delete pConvex_hull;

      Scene_polyhedron_item* new_item = new Scene_polyhedron_item(poly);
      new_item->setName(tr("%1 (convex hull)").arg(scene->item(index)->name()));
      new_item->setColor(Qt::magenta);
      new_item->setRenderingMode(FlatPlusEdges);
      scene->addItem(new_item);
    } else {
       Scene_surface_mesh_item* new_item = new Scene_surface_mesh_item(pConvex_hull);
       new_item->setName(tr("%1 (convex hull)").arg(scene->item(index)->name()));
       new_item->setColor(Qt::magenta);
       new_item->setRenderingMode(FlatPlusEdges);
       scene->addItem(new_item);
    }

    // default cursor
    QApplication::restoreOverrideCursor();
  }

}

#include "Convex_hull_plugin.moc"


#ifndef constraint_hpp
#define constraint_hpp

#include <memory>
#include <vector>
#include <Eigen/Core>

namespace elasty
{
    struct Particle;

    enum class ConstraintType { Bilateral, Unilateral };

    class Constraint
    {
    public:

        Constraint(const std::vector<std::shared_ptr<Particle>>& particles,
                   const double stiffness) :
        m_stiffness(stiffness),
        m_particles(particles)
        {
        }

        /// \brief Calculates the constraint function value C(x).
        virtual double calculateValue() = 0;

        /// \brief Calculate the derivative of the constraint function
        /// grad C(x).
        /// \details As constraints can have different vector sizes, it will
        /// store the result to the passed raw buffer that should be allocated
        /// in the caller, rather than returning a dynamically allocated
        /// variable-length vector. This method does not check whether the
        /// buffer is adequately allocated, or not.
        virtual void calculateGrad(double* grad_C) = 0;

        /// \brief Manipulate the associated particles by projecting them to the
        /// constraint manifold.
        /// \details This method should be called by the core engine. As this
        /// method directly updates the predicted positions of the associated
        /// particles, it is intended to be used in a Gauss-Seidel-style solver.
        virtual void projectParticles() = 0;

        /// \brief Return the constraint type (i.e., either unilateral or
        /// bilateral).
        virtual ConstraintType getType() = 0;

        /// \brief Stiffness of this constraint, which should be in [0, 1].
        double m_stiffness;

    protected:

        /// \brief Associated particles.
        /// \details The number of particles is (in most cases) solely
        /// determined in each constraint. For example, a distance constraint
        /// need to have exactly two particles. Some special constraints
        /// (e.g., shape-matching constraint) could have a variable number of
        /// particles.
        std::vector<std::shared_ptr<Particle>> m_particles;
    };

    class BendingConstraint final : public Constraint
    {
    public:

        BendingConstraint(const std::shared_ptr<Particle> p_0,
                          const std::shared_ptr<Particle> p_1,
                          const std::shared_ptr<Particle> p_2,
                          const std::shared_ptr<Particle> p_3,
                          const double stiffness,
                          const double dihedral_angle);

        double calculateValue() override;
        void calculateGrad(double* grad_C) override;
        void projectParticles() override;
        ConstraintType getType() override { return ConstraintType::Bilateral; }

    private:

        const Eigen::Matrix<double, 12, 1> m_inv_M;
        const double m_dihedral_angle;
    };

    class DistanceConstraint final : public Constraint
    {
    public:

        DistanceConstraint(const std::shared_ptr<Particle> p_0,
                           const std::shared_ptr<Particle> p_1,
                           const double stiffness,
                           const double d);

        double calculateValue() override;
        void calculateGrad(double* grad_C) override;
        void projectParticles() override;
        ConstraintType getType() override { return ConstraintType::Bilateral; }

    private:

        const Eigen::Matrix<double, 6, 1> m_inv_M;
        const double m_d;
    };

    class EnvironmentalCollisionConstraint final : public Constraint
    {
    public:

        EnvironmentalCollisionConstraint(const std::shared_ptr<Particle> p_0,
                                         const double stiffness,
                                         const Eigen::Vector3d& n,
                                         const double d);

        double calculateValue() override;
        void calculateGrad(double* grad_C) override;
        void projectParticles() override;
        ConstraintType getType() override { return ConstraintType::Unilateral; }

    private:

        const Eigen::Matrix<double, 3, 1> m_inv_M;
        const Eigen::Vector3d m_n;
        const double m_d;
    };

    class FixedPointConstraint final : public Constraint
    {
    public:

        FixedPointConstraint(const std::shared_ptr<Particle> p_0,
                             const double stiffness,
                             const Eigen::Vector3d& point);

        double calculateValue() override;
        void calculateGrad(double* grad_C) override;
        void projectParticles() override;
        ConstraintType getType() override { return ConstraintType::Bilateral; }

    private:

        const Eigen::Matrix<double, 3, 1> m_inv_M;
        const Eigen::Vector3d m_point;
    };

    class IsometricBendingConstraint final : public Constraint
    {
    public:

        IsometricBendingConstraint(const std::shared_ptr<Particle> p_0,
                                   const std::shared_ptr<Particle> p_1,
                                   const std::shared_ptr<Particle> p_2,
                                   const std::shared_ptr<Particle> p_3,
                                   const double stiffness);

        double calculateValue() override;
        void calculateGrad(double* grad_C) override;
        void projectParticles() override;
        ConstraintType getType() override { return ConstraintType::Bilateral; }

    private:

        const Eigen::Matrix<double, 12, 1> m_inv_M;
        Eigen::Matrix4d m_Q;
    };
}

#endif /* constraint_hpp */


#include <iostream>
#include <fstream>
#include <string>
#include <math.h>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/program_options.hpp>

#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_3.h>
#include <CGAL/Triangulation_data_structure_3.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Delaunay_triangulation_3<K>                   Triangulation;
typedef Triangulation::Point                                Point;

// using a global for this is not good. I really should be passing it
// some other way.
double scale_z=1.0;

// these two functions are for my readvtk function.
Triangulation::Vertex_handle
add_vertex(Triangulation &T, std::string& vname, double x, double y, double z)
{
  // the exp(z) scaling shouldn't be done here.
  // max value of z is 1000. in that case, z/1000 =1.
  // which leaves us with exp(log(1000) which == 1000
  double newz = scale_z*exp(log(1000.0)*z/1000.0);;
  return T.insert(Point(x,y,newz));
}

void add_edge(Triangulation &T,
               Triangulation::Vertex_handle s,
               Triangulation::Vertex_handle t)
{
  // no edges to add. CGAL does that.
}

#include <readvtk.hxx>

struct VertexData {
  std::string name;
  double x,y,z;
};

struct EdgeData {
  double distance;
};

typedef boost::adjacency_list<boost::vecS, boost::vecS,
                              boost::undirectedS,
                              VertexData,
                              boost::property<boost::edge_weight_t, double, EdgeData>
                              > MyGraphType;

// these are needed by readvtk
typedef typename boost::graph_traits<MyGraphType>::vertex_descriptor vertex_descriptor;
typedef typename boost::graph_traits<MyGraphType>::edge_descriptor   edge_descriptor;
typename boost::graph_traits<MyGraphType>::vertex_descriptor
add_vertex(MyGraphType &G, std::string& vname, double x, double y, double z)
{
  typedef typename boost::graph_traits<MyGraphType>::vertex_descriptor vertex_descriptor;
  vertex_descriptor v = add_vertex(G);
  G[v].x = x;
  G[v].y = y;
  G[v].z = z;
  return v;
}

inline
double distance(MyGraphType &G,
                typename boost::graph_traits<MyGraphType>::vertex_descriptor v1,
                typename boost::graph_traits<MyGraphType>::vertex_descriptor v2)
{
  return sqrt((G[v1].x - G[v2].x)*(G[v1].x - G[v2].x) +
              (G[v1].y - G[v2].y)*(G[v1].y - G[v2].y) +
              (G[v1].z - G[v2].z)*(G[v1].z - G[v2].z));
}

typename boost::graph_traits<MyGraphType>::edge_descriptor
add_edge(MyGraphType &G,
         typename boost::graph_traits<MyGraphType>::vertex_descriptor v1,
         typename boost::graph_traits<MyGraphType>::vertex_descriptor v2)
{
  typedef typename boost::graph_traits<MyGraphType>::edge_descriptor edge_descriptor;
  edge_descriptor e = add_edge(v1, v2, G).first;
  boost::property_map<MyGraphType, boost::edge_weight_t>::type weightmap = get(boost::edge_weight, G);
  weightmap[e] = distance(G, v1, v2);
  return e;
}

#include <readvtk.hxx>

enum MSTAlgorithm { PRIM, KRUSKAL };
std::istream& operator>>(std::istream& in, MSTAlgorithm &format)
{
    std::string token;
    in >> token;
    if (token == "prim")
        format = PRIM;
    else if (token == "kruskal")
        format = KRUSKAL;
    else
        in.setstate(std::ios_base::failbit);
    return in;
}

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

  namespace po = boost::program_options;
  po::options_description desc("Usage");

  std::string filename;
  desc.add_options()
    ("help", "produce help message")
    ("filename", po::value<std::string>(&filename)->default_value(""),
     "filename containing input points");

  MSTAlgorithm mst_algorithm;
    desc.add_options()
        ("algorithm", po::value<MSTAlgorithm>(&mst_algorithm)->default_value(KRUSKAL),
         "which output format");

    desc.add_options()
        ("scale", po::value<double>(&scale_z)->default_value(1.0),
         "scale factor");

  po::variables_map opts;
  po::store(po::parse_command_line(argc, argv, desc), opts);

  try {
    po::notify(opts);
  } catch (std::exception& e) {
    std::cerr << "Error: " << e.what() << "\n";
    return 1;
  }

  if (filename == "") {
    std::cerr << "please provide a vtk file with the --filename <file> option" << std::endl;
    exit(-1);
  }
  std::ifstream input(filename.c_str());

  std::cerr << "using scale factor " << scale_z << std::endl;

  Triangulation T;
  readvtk<Triangulation,Triangulation::Vertex_handle>(input, T);
  assert( T.is_valid() ); // checking validity of T

  // at this point, the points have been loaded and the triangulation is done.

  MyGraphType G;

  std::map<Point, MyGraphType::vertex_descriptor> vertex_map;

  Triangulation::Finite_vertices_iterator viter;
  for (viter =  T.finite_vertices_begin();
       viter != T.finite_vertices_end();
       viter++) {
    Triangulation::Triangulation_data_structure::Vertex v = *viter;
    Point p = v.point();
    double x = CGAL::to_double(p.x());
    double y = CGAL::to_double(p.y());
    double z = CGAL::to_double(p.z());
    std::string d("");
    auto boost_vertex = vertex_map[v.point()] = add_vertex(G, d, x,y,z);

  }

  Triangulation::Finite_edges_iterator iter;
  for(iter =  T.finite_edges_begin();
      iter != T.finite_edges_end();
      iter++) {
    // edges are not represented as edges in CGAL triangulation graphs.
    // Instead, they are stored in faces/cells.

    Triangulation::Triangulation_data_structure::Edge e = *iter;
    Triangulation::Triangulation_data_structure::Cell_handle c = e.first;
    int i = e.second;
    int j = e.third;
    auto boost_edge = add_edge(G, vertex_map[c->vertex(i)->point()], vertex_map[c->vertex(j)->point()]);
  }

  std::cerr << "running prim" << std::endl;
  std::vector<vertex_descriptor> mst_prim(num_vertices(G));

  std::cout << "# vtk DataFile Version 1.0\n";
  std::cout << "3D triangulation data\n";
  std::cout << "ASCII\n";
  std::cout << std::endl;
  std::cout << "DATASET POLYDATA\n";

  std::cout << "POINTS " << num_vertices(G) << " float\n";
  for(int i=0; i<num_vertices(G); i++) {
    std::cout << G[i].x  << " " << G[i].y << " " << G[i].z << std::endl;
  }

  std::cout << "LINES " << (num_vertices(G)-1) << " " << (num_vertices(G)-1)*3 << std::endl;
  if (mst_algorithm == PRIM) {

    // the not particularly helpful doc for iterator_property_map:
    // http://www.boost.org/doc/libs/1_64_0/libs/property_map/doc/iterator_property_map.html
    // iterator_property_map<RandomAccessIterator, OffsetMap, T, R>
    //
    typedef boost::property_map<MyGraphType, boost::vertex_index_t>::type IdMap;
    boost::iterator_property_map<std::vector<vertex_descriptor>::iterator,
                                 IdMap,
                                 vertex_descriptor,
                                 vertex_descriptor&>
      predmap(mst_prim.begin(), get(boost::vertex_index, G));

    boost::prim_minimum_spanning_tree(G, predmap);

    for(int i=0; i<num_vertices(G); i++) {
      if (i == mst_prim[i]) {
        std::cerr << "skipping " << i << std::endl;
        continue;
      }
      std::cout << "2 " << i << " " << mst_prim[i] << std::endl;
    }
  }

  if (mst_algorithm == KRUSKAL) {
    std::cerr << "running kruskal" << std::endl;
    std::list<boost::graph_traits<MyGraphType>::edge_descriptor> mst_kruskal;
    boost::kruskal_minimum_spanning_tree(G, std::back_inserter(mst_kruskal));

    for(auto iter=mst_kruskal.begin();
        iter != mst_kruskal.end();
        iter++) {
      std::cout << "2 " << source(*iter, G) << " " << target(*iter, G) <<std::endl;
    }
  }

}


#include <iostream>
#include <stdio.h>
#include <sstream>
#include <deque>
#include <boost/multiprecision/gmp.hpp>

void require(bool cond, const char* msg)
{
    if (!cond) {
        std::cerr << "ERROR: " << msg << "!" << std::endl;
        exit(1);
    }
}

enum Op { LEQ, LT, GT, GEQ, EQ, ADD, SUB, MUL, DIV, AND, OR, NOT };

struct StackItem
{
    typedef boost::multiprecision::mpq_rational val_t;

    enum Kind { OP, VAL, BV } kind;
    Op op;
    val_t val;
    bool bv;

    StackItem(Op o) : kind(OP), op(o) {}
    StackItem(val_t v) : kind(VAL), val(v) {}
    StackItem(bool b) : kind(BV), bv(b) {}

    friend std::ostream& operator<< (std::ostream& stream, const StackItem& si);
};

std::ostream& operator<< (std::ostream& stream, const StackItem& si)
{
    switch (si.kind) {
        case StackItem::OP:
            stream << "op: " << si.op;
            break;
        case StackItem::VAL:
            stream << si.val;
            break;
        case StackItem::BV:
            stream << (si.bv ? "true" : "false");
            break;
    }
    return stream;
}

#define CHECK_BIN(type)\
            require(operands.size() == 2, "Invalid number of arguments for binary operator");\
            require(operands[0].kind == StackItem::type, "Invalid first argument type");\
            require(operands[1].kind == StackItem::type, "Invalid second argument type");

#define CHECK_UN(type)\
            require(operands.size() == 1, "Invalid number of arguments for unary operator");\
            require(operands[0].kind == StackItem::type, "Invalid argument type");

StackItem eval(Op o, const std::deque<StackItem>& operands)
{
    switch  (o) {
        case LEQ: CHECK_BIN(VAL);
            return StackItem(operands[0].val <= operands[1].val);
        case GEQ: CHECK_BIN(VAL);
            return StackItem(operands[0].val >= operands[1].val);
        case LT: CHECK_BIN(VAL);
            return StackItem(operands[0].val < operands[1].val);
        case GT: CHECK_BIN(VAL);
            return StackItem(operands[0].val > operands[1].val);
        case EQ: CHECK_BIN(VAL);
            return StackItem(operands[0].val == operands[1].val);
        case ADD: CHECK_BIN(VAL);
            return StackItem(operands[0].val + operands[1].val);
        case SUB:
            if (operands.size() == 1) {
                CHECK_UN(VAL);
                return StackItem(- operands[0].val);
            } else if (operands.size() == 2) {
                CHECK_BIN(VAL);
                return StackItem(operands[0].val - operands[1].val);
            } else {
                require(false, "Invalid number of arguments for operator '-'");
            }
        case MUL: CHECK_BIN(VAL);
            return StackItem(operands[0].val * operands[1].val);
        case DIV: CHECK_BIN(VAL);
            require(operands[1].val != 0, "Division by zero");
            return StackItem(operands[0].val / operands[1].val);
        case AND: CHECK_BIN(BV);
            return StackItem(operands[0].bv && operands[1].bv);
        case OR: CHECK_BIN(BV);
            return StackItem(operands[0].bv || operands[1].bv);
        case NOT: CHECK_UN(BV);
            return StackItem(!operands[0].bv);
        default:
            require(false, "Invalid operation!");
    }
}

#define EXPECT(x) require(std::getchar() == x, "Expected " #x);

int main(int argc, char *argv[])
{
    std::deque<StackItem> stack;
    char chr = std::getchar();
    while (chr != EOF) {
        switch (chr) {
            case '<':
                chr = std::getchar();
                if (chr == '=') {
                    stack.emplace_back(Op::LEQ);
                } else {
                    stack.emplace_back(Op::LT);
                    continue;
                }
                break;
            case '>':
                chr = std::getchar();
                if (chr == '=') {
                    stack.emplace_back(Op::GEQ);
                } else {
                    stack.emplace_back(Op::GT);
                    continue;
                }
                break;
            case '=':
                stack.emplace_back(Op::EQ);
                break;
            case 'a': EXPECT('n'); EXPECT('d');
                stack.emplace_back(Op::AND);
                break;
            case 'o': EXPECT('r');
                stack.emplace_back(Op::OR);
                break;
            case 'n': EXPECT('o'); EXPECT('t');
                stack.emplace_back(Op::NOT);
                break;
            case '+':
                stack.emplace_back(Op::ADD);
                break;
            case '-':
                stack.emplace_back(Op::SUB);
                break;
            case '*':
                stack.emplace_back(Op::MUL);
                break;
            case '/':
                stack.emplace_back(Op::DIV);
                break;
            case '0': case '1': case '2': case '3': case '4':
            case '5': case '6': case '7': case '8': case '9': {
                    std::stringstream sb;
                    sb << chr;
                    chr = getchar();
                    while (chr >= '0' && chr <= '9') {
                        sb << chr;
                        chr = getchar();
                    }
                    stack.emplace_back(StackItem::val_t(sb.str()));
                    continue;
                }
                break;
            case ')': {
                    std::deque<StackItem> v;
                    while (stack.back().kind != StackItem::OP) {
                        v.push_front(stack.back());
                        stack.pop_back();
                    }
                    auto o = stack.back().op;
                    stack.pop_back();
                    stack.push_back(eval(o, v));
                }
                break;
            case ' ': case '\n': case '\t': case '(':
                break;
            default:
                require(false, "Invalid input");
        }
        chr = getchar();
    }

    for (const auto& si : stack) {
        std::cout << "> " << si << std::endl;
    }

    return 0;
}


/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

#include <jni.h>

#include <iostream>

#include <Eigen/Core>
#include <unsupported/Eigen/MatrixFunctions>

#include "drake/math/discrete_algebraic_riccati_equation.h"
#include "edu_wpi_first_wpiutil_math_DrakeJNI.h"
#include "wpi/jni_util.h"

using namespace wpi::java;

extern "C" {

/*
 * Class:     edu_wpi_first_wpiutil_math_DrakeJNI
 * Method:    discreteAlgebraicRiccatiEquation
 * Signature: ([D[D[D[DII[D)V
 */
JNIEXPORT void JNICALL
Java_edu_wpi_first_wpiutil_math_DrakeJNI_discreteAlgebraicRiccatiEquation
  (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
   jdoubleArray R, jint states, jint inputs, jdoubleArray S)
{
  jdouble* nativeA = env->GetDoubleArrayElements(A, nullptr);
  jdouble* nativeB = env->GetDoubleArrayElements(B, nullptr);
  jdouble* nativeQ = env->GetDoubleArrayElements(Q, nullptr);
  jdouble* nativeR = env->GetDoubleArrayElements(R, nullptr);

  Eigen::Map<
      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
      Amat{nativeA, states, states};
  Eigen::Map<
      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
      Bmat{nativeB, states, inputs};
  Eigen::Map<
      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
      Qmat{nativeQ, states, states};
  Eigen::Map<
      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
      Rmat{nativeR, inputs, inputs};

  Eigen::MatrixXd result =
      drake::math::DiscreteAlgebraicRiccatiEquation(Amat, Bmat, Qmat, Rmat);

  env->ReleaseDoubleArrayElements(A, nativeA, 0);
  env->ReleaseDoubleArrayElements(B, nativeB, 0);
  env->ReleaseDoubleArrayElements(Q, nativeQ, 0);
  env->ReleaseDoubleArrayElements(R, nativeR, 0);

  env->SetDoubleArrayRegion(S, 0, states * states, result.data());
}

}  // extern "C"


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <boost/random.hpp>
#include <boost/make_shared.hpp>

#include "Tudat/Mathematics/Statistics/boostProbabilityDistributions.h"

namespace tudat
{

namespace statistics
{

//! Function to create a random variable class of BoostContinuousProbabilityDistribution type
std::shared_ptr< InvertibleContinuousProbabilityDistribution< double > > createBoostRandomVariable(
        const ContinuousBoostStatisticalDistributions boostDistribution, const std::vector< double >& parameters )
{
    using namespace boost::math;

    std::shared_ptr< InvertibleContinuousProbabilityDistribution< double > > continuousRandomVariable;

    // Check which distribution type is requested
    switch( boostDistribution )
    {
    case uniform_boost_distribution:
    {
        // Check number of provided parameters
        if ( parameters.size( ) != 2 )
        {
            throw std::runtime_error( "Uniform distribution requires two parameters." );
        }
        else
        {
            // Create uniform distribution. parameters 0: lower bound, 1: upper bound
            uniform_distribution< > uniformDistribution( parameters.at( 0 ), parameters.at( 1 ) );
            continuousRandomVariable =
                    std::make_shared< BoostContinuousProbabilityDistribution< uniform_distribution< > > >(
                        uniformDistribution );
        }
        break;
    }
    case normal_boost_distribution:
    {
        // Check number of provided parameters
        if ( parameters.size( ) != 2 )
        {
            throw std::runtime_error( "Normal distribution requires two parameters." );
        }
        else
        {
            // Create uniform distribution. parameters 0: mean, 1: standard deviation
            normal_distribution< > normalDistribution( parameters.at( 0 ), parameters.at( 1 ) );
            continuousRandomVariable =
                    std::make_shared< BoostContinuousProbabilityDistribution< normal_distribution< > > >(
                        normalDistribution );
        }
        break;
    }
    case exponential_boost_distribution:
    {
        // Check number of provided parameters
        if ( parameters.size( ) != 1 )
        {
            throw std::runtime_error( "Exponential distribution requires one parameter." );
        }
        else
        {
            // Create uniform distribution. parameters 0: lambda parameter
            exponential_distribution< > exponentialDistribution( parameters.at( 0 ) );
            continuousRandomVariable =
                    std::make_shared< BoostContinuousProbabilityDistribution< exponential_distribution< > > >(
                        exponentialDistribution );
        }
        break;
    }
    case gamma_boost_distribution:
    {
        // Check number of provided parameters
        if ( parameters.size( ) != 2 )
        {
            throw std::runtime_error( "Gamma distribution requires two parameters." );
        }
        else
        {
            // Create uniform distribution. parameters 0: shape parameter, parameter 1: scale parameter
            gamma_distribution< > gammaDistribution( parameters.at( 0 ), parameters.at( 1 ) );
            continuousRandomVariable =
                    std::make_shared< BoostContinuousProbabilityDistribution< gamma_distribution< > > >(
                        gammaDistribution );
        }
        break;
    }
    case lognormal_boost_distribution:
    {
        // Check number of provided parameters
        if ( parameters.size( ) != 2 )
        {
            throw std::runtime_error( "Log-normal distribution requires two parameters." );
        }
        else
        {
            // Create uniform distribution. parameters 0: location parameter, parameter 1: scale parameter
            lognormal_distribution< > logNormalDistribution( parameters.at( 0 ), parameters.at( 1 ) );
            continuousRandomVariable =
                    std::make_shared< BoostContinuousProbabilityDistribution< lognormal_distribution< > > >(
                        logNormalDistribution );
        }
        break;
    }
    case beta_boost_distribution:
    {
        // Check number of provided parameters
        if ( parameters.size( ) != 2 )
        {
            throw std::runtime_error( "Beta distribution requires two parameters." );
        }
        else
        {
            // Create uniform distribution. parameters 0: alpha parameter, parameter 1: beta parameter
            beta_distribution< > betaDistribution( parameters.at( 0 ), parameters.at( 1 ) );
            continuousRandomVariable =
                    std::make_shared< BoostContinuousProbabilityDistribution< beta_distribution< > > >(
                        betaDistribution );
        }
        break;
    }
    default:
        throw std::runtime_error( "Boost distribution not recognized." );
    }
    return continuousRandomVariable;
}

}

}


#include <CGAL/trace.h>
#include <CGAL/Timer.h>
#include <iostream>
#include <string>
#include <fstream>

#include <Eigen/Eigen>

// #define USE_SCALAR_IMPLEMENTATION
#define USE_SSE_IMPLEMENTATION
#define COMPUTE_V_AS_MATRIX
#define COMPUTE_U_AS_MATRIX

#include <CGAL/internal/Surface_mesh_deformation/auxiliary/Singular_Value_Decomposition_Preamble.hpp>

int main() {

  std::ifstream file;
  file.open("SVD_benchmark");
  if (!file)
  {
    CGAL_TRACE_STREAM << "Error loading file!\n";
    return 0;
  }

  int ite = 200000;
  Eigen::JacobiSVD<Eigen::Matrix3d> svd;
  Eigen::Matrix3d u, v, m, r;
  Eigen::Vector3d w;

  int matrix_idx = rand()%200;
  for (int i = 0; i < matrix_idx; i++)
  {
    for (int j = 0; j < 3; j++)
    {
      for (int k = 0; k < 3; k++)
      {
        file >> m(j, k);
      }
    }
  }

  CGAL::Timer task_timer;

  std::cout << "Start SVD decomposition...";
  task_timer.start();
  for (int i = 0; i < ite; i++)
  {

    #include <CGAL/internal/Surface_mesh_deformation/auxiliary/Singular_Value_Decomposition_Kernel_Declarations.hpp>

    ENABLE_SCALAR_IMPLEMENTATION(Sa11.f=m(0,0);)
    ENABLE_SCALAR_IMPLEMENTATION(Sa21.f=m(1,0);)
    ENABLE_SCALAR_IMPLEMENTATION(Sa31.f=m(2,0);)
    ENABLE_SCALAR_IMPLEMENTATION(Sa12.f=m(0,1);)
    ENABLE_SCALAR_IMPLEMENTATION(Sa22.f=m(1,1);)
    ENABLE_SCALAR_IMPLEMENTATION(Sa32.f=m(2,1);)
    ENABLE_SCALAR_IMPLEMENTATION(Sa13.f=m(0,2);)
    ENABLE_SCALAR_IMPLEMENTATION(Sa23.f=m(1,2);)
    ENABLE_SCALAR_IMPLEMENTATION(Sa33.f=m(2,2);)

    ENABLE_SSE_IMPLEMENTATION(Va11=_mm_set1_ps(m(0,0));)
    ENABLE_SSE_IMPLEMENTATION(Va21=_mm_set1_ps(m(1,0));)
    ENABLE_SSE_IMPLEMENTATION(Va31=_mm_set1_ps(m(2,0));)
    ENABLE_SSE_IMPLEMENTATION(Va12=_mm_set1_ps(m(0,1));)
    ENABLE_SSE_IMPLEMENTATION(Va22=_mm_set1_ps(m(1,1));)
    ENABLE_SSE_IMPLEMENTATION(Va32=_mm_set1_ps(m(2,1));)
    ENABLE_SSE_IMPLEMENTATION(Va13=_mm_set1_ps(m(0,2));)
    ENABLE_SSE_IMPLEMENTATION(Va23=_mm_set1_ps(m(1,2));)
    ENABLE_SSE_IMPLEMENTATION(Va33=_mm_set1_ps(m(2,2));)

    #include <CGAL/internal/Surface_mesh_deformation/auxiliary/Singular_Value_Decomposition_Main_Kernel_Body.hpp>

    std::pair<Eigen::Matrix3d, Eigen::Matrix3d> solver;

#ifdef USE_SCALAR_IMPLEMENTATION
    solver.first(0,0) = Su11.f;
    solver.first(1,0) = Su21.f;
    solver.first(2,0) = Su31.f;
    solver.first(0,1) = Su12.f;
    solver.first(1,1) = Su22.f;
    solver.first(2,1) = Su32.f;
    solver.first(0,2) = Su13.f;
    solver.first(1,2) = Su23.f;
    solver.first(2,2) = Su33.f;

    solver.second(0,0) = Sv11.f;
    solver.second(1,0) = Sv21.f;
    solver.second(2,0) = Sv31.f;
    solver.second(0,1) = Sv12.f;
    solver.second(1,1) = Sv22.f;
    solver.second(2,1) = Sv32.f;
    solver.second(0,2) = Sv13.f;
    solver.second(1,2) = Sv23.f;
    solver.second(2,2) = Sv33.f;
#endif

#ifdef USE_SSE_IMPLEMENTATION
    float buf[4];
    _mm_storeu_ps(buf,Vu11);solver.first(0,0)=buf[0];
    _mm_storeu_ps(buf,Vu21);solver.first(1,0)=buf[0];
    _mm_storeu_ps(buf,Vu31);solver.first(2,0)=buf[0];
    _mm_storeu_ps(buf,Vu12);solver.first(0,1)=buf[0];
    _mm_storeu_ps(buf,Vu22);solver.first(1,1)=buf[0];
    _mm_storeu_ps(buf,Vu32);solver.first(2,1)=buf[0];
    _mm_storeu_ps(buf,Vu13);solver.first(0,2)=buf[0];
    _mm_storeu_ps(buf,Vu23);solver.first(1,2)=buf[0];
    _mm_storeu_ps(buf,Vu33);solver.first(2,2)=buf[0];

    _mm_storeu_ps(buf,Vv11);solver.second(0,0)=buf[0];
    _mm_storeu_ps(buf,Vv21);solver.second(1,0)=buf[0];
    _mm_storeu_ps(buf,Vv31);solver.second(2,0)=buf[0];
    _mm_storeu_ps(buf,Vv12);solver.second(0,1)=buf[0];
    _mm_storeu_ps(buf,Vv22);solver.second(1,1)=buf[0];
    _mm_storeu_ps(buf,Vv32);solver.second(2,1)=buf[0];
    _mm_storeu_ps(buf,Vv13);solver.second(0,2)=buf[0];
    _mm_storeu_ps(buf,Vv23);solver.second(1,2)=buf[0];
    _mm_storeu_ps(buf,Vv33);solver.second(2,2)=buf[0];
#endif

    r = solver.first * solver.second.transpose();
  }
  task_timer.stop();
  file.close();

  std::cout << "done: " << task_timer.time() << "s\n";
  return 0;
}


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// 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 <boost/numeric/odeint.hpp>
#include <smooth/bundle.hpp>
#include <smooth/compat/odeint.hpp>
#include <smooth/feedback/asif.hpp>
#include <smooth/feedback/mpc.hpp>
#include <smooth/se2.hpp>
#include <smooth/se3.hpp>

#include <chrono>

#ifdef ENABLE_PLOTTING
#include <matplot/matplot.h>
#endif

#ifdef ENABLE_ROS
#include <gazebo_msgs/srv/set_entity_state.hpp>
#include <geometry_msgs/msg/accel.hpp>
#include <rclcpp/rclcpp.hpp>
#include <smooth/compat/ros.hpp>
#endif

using namespace std::chrono_literals;
using namespace boost::numeric::odeint;

using Time = std::chrono::duration<double>;

template<typename S>
using X = smooth::Bundle<smooth::SE2<S>, Eigen::Matrix<S, 3, 1>>;
template<typename S>
using U = Eigen::Matrix<S, 2, 1>;

using Xd = X<double>;
using Ud = U<double>;

using Tangentd = smooth::Tangent<Xd>;

int main()
{
  // dynamics
  auto f = []<typename S>(const X<S> & x, const U<S> & u) -> smooth::Tangent<X<S>> {
    return {
      x.template part<1>().x(),
      x.template part<1>().y(),
      x.template part<1>().z(),
      -S(0.2) * x.template part<1>().x() + u.x(),
      S(0),
      -S(0.4) * x.template part<1>().z() + u.y(),
    };
  };

  auto cr = []<typename S>(const X<S> &, const U<S> & u) -> Eigen::Vector<S, 2> { return u; };
  Eigen::Vector2d crl{-0.5, -0.5};
  Eigen::Vector2d cru{0.5, 0.5};

  // simulation time step
  const auto dt = 25ms;

  ////////////////////
  //// SET UP MPC ////
  ////////////////////

  smooth::feedback::MPC<Time, Xd, Ud, decltype(f), decltype(cr)> mpc{
    f,
    cr,
    crl,
    cru,
    {.K = 30, .tf = 5},
  };

  // define desired trajectory
  auto xdes = []<typename S>(S t) -> X<S> {
    const Eigen::Vector3d vdes{1, 0, 0.4};
    return X<S>{
      smooth::SE2<S>(smooth::SO2<S>(M_PI_2), Eigen::Vector2<S>(2.5, 0)) + (t * vdes),
      vdes,
    };
  };

  mpc.set_weights({
    .Q   = Eigen::Matrix<double, 6, 6>::Identity(),
    .Qtf = 0.1 * Eigen::Matrix<double, 6, 6>::Identity(),
    .R   = Eigen::Matrix2d::Identity(),
  });

  // set desired trajectory in MPC
  mpc.set_xdes_rel(xdes);
  mpc.set_udes_rel([]<typename S>(S) -> U<S> { return U<S>::Zero(); });

  /////////////////////
  //// SET UP ASIF ////
  /////////////////////

  // safe set
  auto h = []<typename S>(S, const X<S> & x) -> Eigen::Matrix<S, 1, 1> {
    const Eigen::Vector2<S> dir = x.template part<0>().r2() - Eigen::Vector2<S>{0, -2.3};
    const Eigen::Vector2d e_dir = dir.template cast<double>().normalized();
    return Eigen::Matrix<S, 1, 1>(dir.dot(e_dir) - 0.7);
  };

  // backup controller
  auto bu = []<typename S>(S, const X<S> & x) -> U<S> {
    return {0.2 * x.template part<1>().x(), -0.5};
  };

  const smooth::feedback::ManifoldBounds<Ud> ulim{
    .A = Eigen::Matrix2d{{1, 0}, {0, 1}},
    .c = Ud::Zero(),
    .l = Eigen::Vector2d(-0.2, -0.5),
    .u = Eigen::Vector2d(0.5, 0.5),
  };

  // parameters
  smooth::feedback::ASIFilterParams<Ud> asif_prm{
    .T        = 2.5,
    .nh       = 1,
    .u_weight = Eigen::Vector2d{20, 1},
    .ulim     = ulim,
    .asif =
      {
        .K          = 200,
        .alpha      = 5,
        .dt         = 0.01,
        .relax_cost = 100,
      },
    .qp =
      {
        .polish = false,
      },
  };

  smooth::feedback::ASIFilter<Xd, Ud, decltype(f)> asif(f, asif_prm);

  /////////////////////////
  //// CREATE ROS NODE ////
  /////////////////////////

#ifdef ENABLE_ROS
  rclcpp::init(0, nullptr);
  auto node       = std::make_shared<rclcpp::Node>("se2_example");
  auto ses_client = node->create_client<gazebo_msgs::srv::SetEntityState>("/set_entity_state");
  auto u_mpc_pub =
    node->create_publisher<geometry_msgs::msg::Accel>("u_mpc", rclcpp::SystemDefaultsQoS{});
  auto u_asif_pub =
    node->create_publisher<geometry_msgs::msg::Accel>("u_asif", rclcpp::SystemDefaultsQoS{});

  rclcpp::Rate rate(25ms);
#endif

  /////////////////////////////////////
  //// SIMULATE CLOSED-LOOP SYSTEM ////
  /////////////////////////////////////

  // system variables
  Xd x = Xd::Identity();
  Ud u;

  // prepare for integrating the closed-loop system
  runge_kutta4<Xd, double, Tangentd, double, vector_space_algebra> stepper{};
  const auto ode = [&f, &u](const Xd & x, Tangentd & d, double) { d = f(x, u); };
  std::vector<double> tvec, xvec, yvec, u1vec, u2vec, u1mpcvec, u2mpcvec;

  // integrate closed-loop system
  for (std::chrono::milliseconds t = 0s; t < 30s; t += dt) {
    // compute MPC input
    const auto [u_mpc, mpc_code] = mpc(t, x);
    if (mpc_code != smooth::feedback::QPSolutionStatus::Optimal) {
      std::cerr << "MPC failed with mpc_code " << static_cast<int>(mpc_code) << std::endl;
    }

    // filter input with ASIF
    const auto [u_asif, asif_code] = asif(x, u_mpc, h, bu);
    if (asif_code != smooth::feedback::QPSolutionStatus::Optimal) {
      std::cerr << "ASIF solver failed with asif_code " << static_cast<int>(asif_code) << std::endl;
    }

    // select input
    u = u_asif;

    // store data
    tvec.push_back(duration_cast<Time>(t).count());
    xvec.push_back(x.template part<0>().r2().x());
    yvec.push_back(x.template part<0>().r2().y());

    u1mpcvec.push_back(u_mpc(0));
    u2mpcvec.push_back(u_mpc(1));
    u1vec.push_back(u_asif(0));
    u2vec.push_back(u_asif(1));

#ifdef ENABLE_ROS
    auto req        = std::make_shared<gazebo_msgs::srv::SetEntityState::Request>();
    req->state.name = "bus::link";
    smooth::Map<geometry_msgs::msg::Pose>(req->state.pose) = x.template part<0>().lift_se3();
    req->state.pose.position.x                             = 8 * req->state.pose.position.x;
    req->state.pose.position.y                             = 8 * req->state.pose.position.y;
    ses_client->async_send_request(req);

    geometry_msgs::msg::Accel u_mpc_msg;
    u_mpc_msg.linear.x  = u_mpc.x();
    u_mpc_msg.angular.z = u_mpc.y();
    u_mpc_pub->publish(u_mpc_msg);

    geometry_msgs::msg::Accel u_asif_msg;
    u_asif_msg.linear.x  = u_asif.x();
    u_asif_msg.angular.z = u_asif.y();
    u_asif_pub->publish(u_asif_msg);

    rate.sleep();
#endif
    // step dynamics
    stepper.do_step(
      ode, x, 0, std::chrono::duration_cast<std::chrono::duration<double>>(dt).count());
  }

#ifdef ENABLE_PLOTTING
  matplot::figure();
  matplot::hold(matplot::on);
  matplot::title("Path");

  matplot::plot(xvec, yvec)->line_width(2);
  matplot::plot(
    matplot::transform(tvec, [&](auto t) { return xdes(t).template part<0>().r2().x(); }),
    matplot::transform(tvec, [&](auto t) { return xdes(t).template part<0>().r2().y(); }),
    "k--")
    ->line_width(2);
  matplot::legend({"actual", "desired"});

  matplot::figure();
  matplot::hold(matplot::on);
  matplot::title("Inputs");
  matplot::plot(tvec, u1vec, "r")->line_width(2);
  matplot::plot(tvec, u2vec, "b")->line_width(2);
  matplot::plot(tvec, u1mpcvec, "--r")->line_width(2);
  matplot::plot(tvec, u2mpcvec, "--b")->line_width(2);
  matplot::legend({"u1", "u2", "u1des", "u2des"});

  matplot::show();
#else
  std::cout << "TRAJECTORY:" << std::endl;
  for (auto i = 0u; i != tvec.size(); ++i) {
    std::cout << "t=" << tvec[i] << ": x=" << xvec[i] << ", y=" << yvec[i] << std::endl;
  }
#endif

#ifdef ENABLE_ROS
  rclcpp::shutdown();
#endif

  return EXIT_SUCCESS;
}