// Copyright © 2016-2019 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#include <vinecopulib/misc/tools_stats.hpp>
#include <boost/math/constants/constants.hpp>

namespace vinecopulib {
inline StudentBicop::StudentBicop()
{
    family_ = BicopFamily::student;
    parameters_ = Eigen::VectorXd(2);
    parameters_lower_bounds_ = Eigen::VectorXd(2);
    parameters_upper_bounds_ = Eigen::VectorXd(2);
    parameters_ << 0, 50;
    parameters_lower_bounds_ << -1, 2;
    parameters_upper_bounds_ << 1, 50;
}

inline Eigen::VectorXd StudentBicop::pdf_raw(
    const Eigen::Matrix<double, Eigen::Dynamic, 2> &u
)
{
    double rho = double(this->parameters_(0));
    double nu = double(this->parameters_(1));
    Eigen::VectorXd f = Eigen::VectorXd::Ones(u.rows());
    Eigen::Matrix<double, Eigen::Dynamic, 2> tmp = tools_stats::qt(u, nu);

    f = tmp.col(0).cwiseAbs2() + tmp.col(1).cwiseAbs2() -
        (2 * rho) * tmp.rowwise().prod();
    f /= nu * (1.0 - pow(rho, 2.0));
    f = f + Eigen::VectorXd::Ones(u.rows());
    f = f.array().pow(-(nu + 2.0) / 2.0);
    f = f.cwiseQuotient(tools_stats::dt(tmp, nu).rowwise().prod());
    f *= boost::math::tgamma_ratio((nu + 2.0) / 2.0, nu / 2.0);
    f /= (nu * constant::pi * sqrt(1.0 - pow(rho, 2.0)));

    return f;
}

inline Eigen::VectorXd StudentBicop::cdf(
    const Eigen::Matrix<double, Eigen::Dynamic, 2> &u
)
{
    using namespace tools_stats;

    double rho = double(this->parameters_(0));
    double nu = double(this->parameters_(1));

    // for integer nu, just use pbvt
    // otherwise, interpolate linearly between floor(nu) and ceil(nu)
    if (nu == round(nu)) {
        int inu = static_cast<int>(nu);
        return pbvt(qt(u, inu), inu, rho);
    } else {
        int nu1 = static_cast<int>(std::floor(nu));
        int nu2 = static_cast<int>(std::ceil(nu));
        double weight = (nu - static_cast<double>(nu1)) /
            (static_cast<double>(nu2) - static_cast<double>(nu1));
        return pbvt(qt(u, nu1), nu1, rho) * (1 - weight) +
            pbvt(qt(u, nu2), nu2, rho) * weight;
    }
}

inline Eigen::VectorXd StudentBicop::hfunc1(
    const Eigen::Matrix<double, Eigen::Dynamic, 2> &u
)
{
    double rho = double(this->parameters_(0));
    double nu = double(this->parameters_(1));
    Eigen::VectorXd h = Eigen::VectorXd::Ones(u.rows());
    Eigen::Matrix<double, Eigen::Dynamic, 2> tmp = tools_stats::qt(u, nu);
    h = nu * h + tmp.col(0).cwiseAbs2();
    h *= (1.0 - pow(rho, 2)) / (nu + 1.0);
    h = h.cwiseSqrt().cwiseInverse().cwiseProduct(
        tmp.col(1) - rho * tmp.col(0));
    h = tools_stats::pt(h, nu + 1.0);

    return h;
}

inline Eigen::VectorXd StudentBicop::hinv1(
    const Eigen::Matrix<double, Eigen::Dynamic, 2> &u
)
{
    double rho = double(this->parameters_(0));
    double nu = double(this->parameters_(1));
    Eigen::VectorXd hinv = Eigen::VectorXd::Ones(u.rows());
    Eigen::VectorXd tmp = u.col(1);
    Eigen::VectorXd tmp2 = u.col(0);
    tmp = tools_stats::qt(tmp, nu + 1.0);
    tmp2 = tools_stats::qt(tmp2, nu);

    hinv = nu * hinv + tmp2.cwiseAbs2();
    hinv *= (1.0 - pow(rho, 2)) / (nu + 1.0);
    hinv = hinv.cwiseSqrt().cwiseProduct(tmp) + rho * tmp2;
    hinv = tools_stats::pt(hinv, nu);

    return hinv;
}

inline Eigen::VectorXd StudentBicop::get_start_parameters(const double tau)
{
    Eigen::VectorXd parameters = get_parameters();
    parameters(0) = std::sin(tau * constant::pi / 2);;
    parameters(1) = 5;
    return parameters;
}

inline Eigen::MatrixXd StudentBicop::tau_to_parameters(const double &tau)
{
    return no_tau_to_parameters(tau);
}
}


#include <iostream>
#include <time.h>
#include <boost/graph/bellman_ford_shortest_paths.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/random.hpp>
#include <LEDA/graph/graph.h>
#include <LEDA/graph/shortest_path.h>

using namespace std;
using namespace boost;
using namespace leda;

// Define the boost edge weight property
typedef property<edge_weight_t, int> EdgeWeightProperty;

// Define the boost directed graph: std::vector, std::vector, directed, no vertex property, int edge property, no graph property, std::list
typedef adjacency_list<vecS, vecS, directedS, no_property, EdgeWeightProperty, no_property, listS> DirectedGraph;

// Define the vertex class as vertex_desciptor
typedef graph_traits<DirectedGraph>::vertex_descriptor Vertex;

// Define the edge class as edge_desciptor
typedef graph_traits<DirectedGraph>::edge_descriptor Edge;

// Define the edge iterator as edge_iterator
typedef graph_traits<DirectedGraph>::edge_iterator EdgeIterator;

// Define the edge iterator as edge_iterator
typedef graph_traits<DirectedGraph>::vertex_iterator VertexIterator;

// Define the edge weight map as a property map
typedef property_map<DirectedGraph, edge_weight_t>::type EdgeWeightMap;

/**
 * Copies @param LedaGraph to @param BoostDirectedGraph, using the @param LedaEdgeWeightMap
 * @param BoostDirectedGraph The boost directed graph
 * @param LedaGraph The leda directed graph
 * @param LedaEdgeWeightMap The edge array that contain the leda directed graph edges weights
*/
void CopyLedaGraphToBoostGraph(DirectedGraph& BoostDirectedGraph, leda::graph& LedaGraph, edge_array<int>& LedaEdgeWeightMap)
{
	// Create a new boost directed graph containing the smae number of nodes as the leda directed graph
	DirectedGraph boostGraph(LedaGraph.number_of_nodes());

	// Leda edge that will be used for iteration
	leda::edge tempEdge;

	// For all edges in the leda directed graph
	forall_edges(tempEdge, LedaGraph)
	{
		// Get the source node of the edge
		node source = LedaGraph.source(tempEdge);

		// Get the target node of the edge
		node target = LedaGraph.target(tempEdge);

		// Get the weight of the edge
		int edgeWeight = LedaEdgeWeightMap[tempEdge];

		// Add the edge in the boost directed graph
		add_edge(LedaGraph.index(source), LedaGraph.index(target), edgeWeight, boostGraph);
	}

	// Update the boost directed graph
	BoostDirectedGraph = boostGraph;
}

/**
 * Applies the Bellman Ford algorithm to the @param directedGraph, using @param startingnode
 * @param directedGraph The inserted boost directed graph
 * @param startingNode The node that will be used as the minimum path's starting node
 * @return False if the graph contains a negative weight circle or true otherwise
 */
bool BellmanFord(DirectedGraph& directedGraph, Vertex startingVertex)
{
	// Initialize a node map containg the node minimum path cost
	std::map<Vertex, int> nodeCostMap;

	// Initialize the property map that contain the edges's weights
	EdgeWeightMap boostEdgeWeightMap = get(edge_weight, directedGraph);

	// Initialise the boost vertex iterators
	VertexIterator vertexIteratorBegin, vertexIteratorEnd;

	// For every vertex in the boost directed graph...
	for(tie(vertexIteratorBegin, vertexIteratorEnd) = vertices(directedGraph); vertexIteratorBegin != vertexIteratorEnd; vertexIteratorBegin++)
	{
		// Set the vertex initial cost to INT_MAX
		nodeCostMap.insert(pair<Vertex,int>(*vertexIteratorBegin, INT_MAX));
	}

	// Set the starting node cost to 0
	nodeCostMap[startingVertex] = 0;

	// Initialise the boost edge iterators
	EdgeIterator edgeIteratorBegin, edgeIteratorEnd;

	// For every vertex in the boost directed graph...
	for(tie(vertexIteratorBegin, vertexIteratorEnd) = vertices(directedGraph); vertexIteratorBegin != vertexIteratorEnd; vertexIteratorBegin++)
	{
		// For every out edge of the current vertex...
		for(tie(edgeIteratorBegin, edgeIteratorEnd) = edges(directedGraph); edgeIteratorBegin != edgeIteratorEnd; edgeIteratorBegin++)
		{
			// Get the current edge's source node
			int sourceNodeCost = nodeCostMap[source(*edgeIteratorBegin, directedGraph)];

			// Get the current edge's target node
			int targetNodeCost = nodeCostMap[target(*edgeIteratorBegin, directedGraph)];

			// Get the current edge's weight
			int edgeWeight = boostEdgeWeightMap[*edgeIteratorBegin];

			// If the current edge verifies the triangular inequality and has already been accessed...
			if(sourceNodeCost != INT_MAX && (sourceNodeCost + edgeWeight < targetNodeCost))
			{
				nodeCostMap[target(*edgeIteratorBegin, directedGraph)] = sourceNodeCost + edgeWeight;
			}
	 	}
	}

	// For all edges in the boost directed graph...
	for(tie(edgeIteratorBegin, edgeIteratorEnd) = edges(directedGraph); edgeIteratorBegin != edgeIteratorEnd; edgeIteratorBegin++)
	{
		// Get the current edge's source node
		int sourceNodeCost = nodeCostMap[source(*edgeIteratorBegin, directedGraph)];

		// Get the current edge's target node
		int targetNodeCost = nodeCostMap[target(*edgeIteratorBegin, directedGraph)];

		// Get the current edge's weight
		int edgeWeight = boostEdgeWeightMap[*edgeIteratorBegin];

		// If negative cycle is detected...
		if(sourceNodeCost != INT_MAX && (sourceNodeCost + edgeWeight < targetNodeCost))
		{
			// Return false if a negative weight cycle is present in the directed graph
			return false;
		}
	}

	// Return true since the directed graph doesn't contain a negative weight cycle
	return true;
}

// Main function
int main()
{
	#pragma region Initialization

	// Create an empty boost directed graph
	DirectedGraph boostDirectedGraph;

	// Create an empty leda directed graph
	leda::graph ledaDirectedGraph;

	//Create an empty edge array
	edge_array<int> ledaEdgeWeightArray;

	// User graph option
	std::string graphOption;

	// Number of nodes
	int numberOfNodes;

	cout << "Choose the testing graph between grid or random." << endl;

	// Read the graph type
	cin >> graphOption;

	cout << "Enter the number of nodes." << endl;

	// Read the number of nodes
	cin >> numberOfNodes;

	// If the grid graph is selected...
	if(graphOption == "grid")
	{
		// Create a grid graph
		grid_graph(ledaDirectedGraph, numberOfNodes);

		// Intialise an edge array that will contain the leda graph edges weights
		edge_array<int> edgeWeightArray(ledaDirectedGraph);

		// Copy the edge array
		ledaEdgeWeightArray = edgeWeightArray;

		// Initialise a random seed
		srand(time(NULL));

		// Edge that will be used for the iteration
		leda::edge tempEdge;

		// For every edge in the undirected graph...
		forall_edges(tempEdge, ledaDirectedGraph)
		{
			// Get the current edge source node index
			int tempEdgeSourceNodeIndex = ledaDirectedGraph.index(ledaDirectedGraph.source(tempEdge));

			// Get the current edge target node index
			int tempEdgeTargetNodeIndex = ledaDirectedGraph.index(ledaDirectedGraph.target(tempEdge));

			// Get the point representation of the edge's source node index
			div_t tempEdgeSourceNodeIndexDivResult = div(tempEdgeSourceNodeIndex, numberOfNodes);

			// Get the point representation of the edge's target node index
			div_t tempEdgeTargetNodeIndexDivResult = div(tempEdgeTargetNodeIndex, numberOfNodes);

			// Check if the edge is a vertical edge that belong in the third quarter
			bool verticalEdgeThirdQuarterPresence = (tempEdgeSourceNodeIndexDivResult.quot >= (numberOfNodes/2)) && (tempEdgeSourceNodeIndexDivResult.rem <= (numberOfNodes/2)) && (tempEdgeTargetNodeIndexDivResult.quot > (numberOfNodes/2)) && (tempEdgeTargetNodeIndexDivResult.rem < (numberOfNodes/2));

			// Check if the edge is a horizontal edge that belong in the third quarter
			bool horizontalEdgeThirdQuarterPresence = (tempEdgeSourceNodeIndexDivResult.quot > (numberOfNodes/2)) && (tempEdgeSourceNodeIndexDivResult.rem < (numberOfNodes/2)) && (tempEdgeTargetNodeIndexDivResult.quot >= (numberOfNodes/2)) && (tempEdgeTargetNodeIndexDivResult.rem <= (numberOfNodes/2));

			// If the edge belongs into the third quarter...
			if(verticalEdgeThirdQuarterPresence || horizontalEdgeThirdQuarterPresence)
			{
				// If the current edge, was randomly chosen to reversed...
				if((rand() % 2) == 0)
				{
					// Reverse the current edge
					ledaDirectedGraph.rev_edge(tempEdge);
				}

				// Check if the edge is the special horizontal edge
				bool specialThirdQuarterHorizontalEdge = (tempEdgeSourceNodeIndexDivResult.quot == (numberOfNodes/2 + 1)) && (tempEdgeSourceNodeIndexDivResult.rem == (numberOfNodes/2 - 1)) && (tempEdgeTargetNodeIndexDivResult.quot == (numberOfNodes/2 + 1)) && (tempEdgeTargetNodeIndexDivResult.rem == (numberOfNodes/2));

				// Check if the edge is the special vertical edge
				bool specialThirdQuarterVerticalEdge = (tempEdgeSourceNodeIndexDivResult.quot == (numberOfNodes/2)) && (tempEdgeSourceNodeIndexDivResult.rem == (numberOfNodes/2 - 1)) && (tempEdgeTargetNodeIndexDivResult.quot == (numberOfNodes/2 + 1)) && (tempEdgeTargetNodeIndexDivResult.rem == (numberOfNodes/2 - 1));

				// If the edge is either the special negative weight vertical edge or the special negative weight horizontal edge...
				if(specialThirdQuarterVerticalEdge || specialThirdQuarterHorizontalEdge)
				{
					// Assign -100000 as weight to the current special edge
					ledaEdgeWeightArray[tempEdge] = -100000;
				}
			}
			else
			{
				// Assign random integer values as costs between 0 and 10000
				ledaEdgeWeightArray[tempEdge] = (rand() % 10000);
			}
		}
		// Get the grid graph number of nodes
		numberOfNodes = ledaDirectedGraph.number_of_nodes();

		// Copy the leda directed graph to the boost directed graph
		CopyLedaGraphToBoostGraph(boostDirectedGraph, ledaDirectedGraph, ledaEdgeWeightArray);

		// Initialise a property map that contain the boost graph edges weights
		EdgeWeightMap boostEdgeWeightMap = get(edge_weight, boostDirectedGraph);
	}
	else
	{
		// If the random graph is selected...
		if(graphOption == "random")
		{
			// Calculate the number of edges
			int numberOfEdges = ceil(20 * numberOfNodes * log2(numberOfNodes));

			// Generate a random directed graph
			random_graph(ledaDirectedGraph, numberOfNodes, numberOfEdges, false, true, true);

			// Make the graph cohesive
			Make_Connected(ledaDirectedGraph);

			// Intialise an edge array that will contain the leda graph edges weights
			edge_array<int> edgeWeightArray(ledaDirectedGraph);

			// Copy the edge array
			ledaEdgeWeightArray = edgeWeightArray;

			// Initialise a random seed
			srand(time(NULL));

			// Edge that will be used for the iteration
			leda::edge tempEdge;

			// For every edge in the undirected graph...
			forall_edges(tempEdge, ledaDirectedGraph)
			{
				// Assign random integer values as costs between 10 and 10000
				ledaEdgeWeightArray[tempEdge] = (rand() % 10100) - 100;
			}

			// Copy the leda directed graph to the boost directed graph
			CopyLedaGraphToBoostGraph(boostDirectedGraph, ledaDirectedGraph, ledaEdgeWeightArray);

			// Initialise a property map that contain the boost graph edges weights
			EdgeWeightMap boostEdgeWeightMap = get(edge_weight, boostDirectedGraph);
		}
		else
		{
			cout << "Choose between grid or random." << endl;

			// Exit if another option is selected
			exit(0);
		}
	}

	// Choose a random node from the Leda directed graph
	node randomLedaNode = ledaDirectedGraph.choose_node();

	// Get the index of the Boost directed graph vertex that correspond to the appropriate Leda random chosen node
	Vertex randomBoostVertex = vertex(ledaDirectedGraph.index(randomLedaNode), boostDirectedGraph);

	// Initialise a node array the will contain the last edge on a shortest path from the starting node to a node
	node_array<leda::edge> ledaPredecessorNodeArray(ledaDirectedGraph);

	// Initialise a node array the will contain the shortest path langth from the starting node to a node
	node_array<int> ledaDistanceNodeArray(ledaDirectedGraph);

	// Initialize a property map that contain the edges's weights
	EdgeWeightMap boostEdgeWeightMap = get(edge_weight, boostDirectedGraph);

	// Initialize a vector that will contain the distance to each node and set the initial distance to INT_MAX
	std::vector<int> boostDistanceVector(numberOfNodes,INT_MAX);

	// Set the distance of the chosen vertex to 0
	boostDistanceVector[randomBoostVertex] = 0;

	// Initialize a vector that will contain the predeccesor of each node
	std::vector<std::size_t> boostPredeccesorVector(numberOfNodes);

	// For every vertex in the boost directed graph...
	for(int index = 0; index < numberOfNodes; index++)
	{
		// Set the current vertex predeccesor to itself
		boostPredeccesorVector[index] = index;
	}

	#pragma endregion Initialization

	#pragma region Simulation

	// Initialise the starting CPU time
	float CPUTime = used_time();

	// Execute the user defined Bellman Ford algorithm for the boost directed graph using the random vertex
	bool negativeWeightCircleNotFound = BellmanFord(boostDirectedGraph, randomBoostVertex);

	// Print the user defined Bellman Ford function execution time
	cout << "User defined Bellman Ford function execution time: " << used_time(CPUTime) << " seconds."<< endl;

	// Execute the Leda Bellman Ford algorithm for the Leda directed graph using the random node and the defined arrays
	negativeWeightCircleNotFound = BELLMAN_FORD(ledaDirectedGraph, randomLedaNode, ledaEdgeWeightArray, ledaDistanceNodeArray, ledaPredecessorNodeArray);

	// Print the Leda Bellman Ford function execution time
	cout << "Leda Bellman Ford function execution time: " << used_time(CPUTime) << " seconds."<< endl;

	// If the directed graph doesn't contain a negative weight circle...
	if(negativeWeightCircleNotFound)
	{
		// Execute the Boost Bellman Ford algorithm for the boost directed graph using the defined maps
		bellman_ford_shortest_paths(boostDirectedGraph, numberOfNodes, weight_map(boostEdgeWeightMap).distance_map(&boostDistanceVector[0]).distance_map(&boostPredeccesorVector[0]));

		// Print the Boost Bellman Ford function execution time
		cout << "Boost Bellman Ford function execution time: " << used_time(CPUTime) << " seconds."<< endl;

		cout << "The directed graph doesn't contain a negative weight cycle." << endl;
	}
	else
	{
		cout << "The directed graph contains a negative weight cycle." << endl;
	}

	#pragma endregion Simulation

	// Return 0
	return 0;
}

Project Τεχνολογίες Υλοποίησης Αλγορίθμων/2η Άσκηση/Ergasia_2.cpp


/*****************************************************************************
 * fixed.cpp        Blitz++ array using a custom type
 * $Id$
 * This example illustrates how simple it is to create Blitz++ arrays
 * using a custom type.
 *****************************************************************************/

#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

// A simple fixed point arithmetic class which represents a point
// in the interval [0,1].
class FixedPoint {

public:
    typedef unsigned int T_mantissa;

    FixedPoint() { }

    explicit FixedPoint(T_mantissa mantissa)
    {
        mantissa_ = mantissa;
    }

    FixedPoint(double value)
    {
        assert((value >= 0.0) && (value <= 1.0));
        mantissa_ = T_mantissa(value * huge(T_mantissa()));
    }

    FixedPoint operator+(FixedPoint x)
    { return FixedPoint(mantissa_ + x.mantissa_); }

    double value() const
    { return mantissa_ / double(huge(T_mantissa())); }

private:
    T_mantissa mantissa_;
};

ostream& operator<<(ostream& os, const FixedPoint& a)
{
    os << a.value();
    return os;
}

int main()
{
    // Now create an array using the FixedPoint class:

    Array<FixedPoint, 2> A(4,4), B(4,4);

    A = 0.5, 0.3, 0.8, 0.2,
        0.1, 0.3, 0.2, 0.9,
        0.0, 1.0, 0.7, 0.4,
        0.2, 0.3, 0.8, 0.4;

    B = A + 0.05;

    cout << "B = " << B << endl;

    return 0;
}

// Program output:
// B = 4 x 4
//      0.55      0.35      0.85      0.25
//      0.15      0.35      0.25      0.95
//      0.05      0.05      0.75      0.45
//      0.25      0.35      0.85      0.45

/*
 * Note: Just because Array<T,N> supports all possible operators doesn't
 * mean that a user-defined class has to.  You only need to define the
 * operators you actually use on the array.  This works because the ISO/ANSI
 * draft standard forbids instantiation of unused member functions:
 *
 * [temp.inst, paragraph 7]
 * An implementation shall not implicitly instantiate  a  function,  non-
 * virtual  member  function,  class  or  member  template  that does not
 * require instantiation.  It is unspecified whether or not an  implemen-
 * tation implicitly instantiates a virtual member function that does not
 * require specialization.
 */


// GMTL is (C) Copyright 2001-2010 by Allen Bierbaum
// Distributed under the GNU Lesser General Public License 2.1 with an
// addendum covering inlined code. (See accompanying files LICENSE and
// LICENSE.addendum or http://www.gnu.org/copyleft/lesser.txt)

// This file was originally part of PyJuggler.

// PyJuggler is (C) Copyright 2002, 2003 by Patrick Hartling
// Distributed under the GNU Lesser General Public License 2.1.  (See
// accompanying file COPYING.txt or http://www.gnu.org/copyleft/lesser.txt)

// Includes ====================================================================
#include <boost/python.hpp>
#include <gmtl/Math.h>

// Using =======================================================================
using namespace boost::python;

// Declarations ================================================================

namespace gmtlWrappers
{
   template<typename T, typename U>
   T lerp(const U& lerp, const T& a, const T& b)
   {
      T result;
      gmtl::Math::lerp(result, lerp, a, b);
      return result;
   }

   template<typename T>
   tuple quadraticFormula(const T& a, const T& b, const T& c)
   {
      T r1, r2;
      bool result = gmtl::Math::quadraticFormula(r1, r2, a, b, c);
      return make_tuple(result, r1, r2);
   }
}

namespace
{

class Fake : public boost::noncopyable
{
};

}

// Module ======================================================================
void _Export_gmtl_Math_h()
{
    // Retained (temporarily) for backwards compatibility.
    def("deg2Rad", (double (*)(double))&gmtl::Math::deg2Rad);
    def("deg2Rad", (float (*)(float))&gmtl::Math::deg2Rad);
    def("rad2Deg", (float (*)(float))&gmtl::Math::rad2Deg);
    def("rad2Deg", (double (*)(double))&gmtl::Math::rad2Deg);

    scope* gmtl_Math_scope = new scope(
    class_< Fake, boost::noncopyable >("Math", no_init)
        .def("sign", (int (*)(int)) &gmtl::Math::sign)
        .def("sign", (int (*)(float)) &gmtl::Math::sign)
        .def("sign", (int (*)(double)) &gmtl::Math::sign)
        .def("fastInvSqrt", &gmtl::Math::fastInvSqrt)
        .def("fastInvSqrt2", &gmtl::Math::fastInvSqrt2)
        .def("fastInvSqrt3", &gmtl::Math::fastInvSqrt3)
        .def("deg2Rad", (double (*)(double))&gmtl::Math::deg2Rad)
        .def("deg2Rad", (float (*)(float))&gmtl::Math::deg2Rad)
        .def("rad2Deg", (float (*)(float))&gmtl::Math::rad2Deg)
        .def("rad2Deg", (double (*)(double))&gmtl::Math::rad2Deg)
        .def("factorial", (double (*)(double)) &gmtl::Math::factorial)
        .def("factorial", (float (*)(float)) &gmtl::Math::factorial)
        .def("factorial", (int (*)(int)) &gmtl::Math::factorial)
        .def("lerp",
            (double (*)(const double&, const double&, const double&)) &gmtlWrappers::lerp)
        .def("lerp",
            (float (*)(const float&, const float&, const float&)) &gmtlWrappers::lerp)
        .def("quadraticFormula",
            (tuple (*)(const double&, const double&, const double&)) &gmtlWrappers::quadraticFormula)
        .def("quadraticFormula",
            (tuple (*)(const float&, const float&, const float&)) &gmtlWrappers::quadraticFormula)
        .staticmethod("sign")
        .staticmethod("fastInvSqrt")
        .staticmethod("fastInvSqrt2")
        .staticmethod("fastInvSqrt3")
        .staticmethod("deg2Rad")
        .staticmethod("rad2Deg")
        .staticmethod("factorial")
        .staticmethod("lerp")
        .staticmethod("quadraticFormula")
    );

    delete gmtl_Math_scope;
}


#include <math.h>
#include <uWS/uWS.h>
#include <chrono>
#include <iostream>
#include <thread>
#include <vector>
#include <Eigen/Core>
#include <Eigen/QR>
#include "MPC.h"
#include "json.hpp"
#include "tools.hpp"

// for convenience
using json = nlohmann::json;

const int N_ref = 15; // number of points for reference trajectory
const double wp_delta = 5.0; // delta between waypoints in x direction
const double latency_s = 0.07; // estimated latency in seconds for future state prediction

// This value assumes the model presented in the classroom is used.
// It was obtained by measuring the radius formed by running the vehicle in the
// simulator around in a circle with a constant steering angle and velocity on a
// flat terrain.
//
// Lf was tuned until the the radius formed by the simulating the model
// presented in the classroom matched the previous radius.
//
// This is the length from front to CoG that has a similar radius.
const double Lf = 2.67;

// Reference velocity
const double ref_v = 120.0;

int main() {
  uWS::Hub h;

  // MPC is initialized here!
  MPC mpc(ref_v, Lf, 0.1, -0.6, 1.0, 200.);

  // save steering angle and throttle values in case optimizer failed
  double steer_value = 0.0;
  double throttle_value = 1.0;

  h.onMessage([&mpc, &steer_value, &throttle_value](uWS::WebSocket<uWS::SERVER> ws,
                                                    char *data, size_t length, uWS::OpCode opCode) {
    // "42" at the start of the message means there's a websocket message event.
    // The 4 signifies a websocket message
    // The 2 signifies a websocket event
    std::chrono::high_resolution_clock::time_point begin = std::chrono::high_resolution_clock::now();
    string sdata = string(data).substr(0, length);
    if (sdata.size() > 2 && sdata[0] == '4' && sdata[1] == '2') {
      string s = hasData(sdata);
      if (s != "") {
        auto j = json::parse(s);
        string event = j[0].get<string>();
        if (event == "telemetry") {
          // j[1] is the data JSON object
          vector<double> waypts_x = j[1]["ptsx"];
          vector<double> waypts_y = j[1]["ptsy"];
          double px               = j[1]["x"];
          double py               = j[1]["y"];
          double psi              = j[1]["psi"];
          double v                = j[1]["speed"];
          double cte, epsi;
          bool success = false;

          //Display the waypoints/reference line
          vector<double> next_x_vals(N_ref);
          vector<double> next_y_vals(N_ref);
          //Display the MPC predicted trajectory
          vector<double> mpc_x_vals;
          vector<double> mpc_y_vals;

          // Transform waypoints from global coordinate system into the local car coordinate system
          // where px and py are the cars translational offset and -psi the rotational offset
          std::vector<double> waypts_x_trans;
          std::vector<double> waypts_y_trans;
          Tranform2d(waypts_x_trans, waypts_y_trans, waypts_x, waypts_y, px, py, -psi);
          Eigen::Map<Eigen::VectorXd> wp_x(waypts_x_trans.data(), waypts_x_trans.size());
          Eigen::Map<Eigen::VectorXd> wp_y(waypts_y_trans.data(), waypts_y_trans.size());

          // Fit a polynomial of order 3 to the above x and y coordinates
          auto coeffs = polyfit(wp_x, wp_y, 3);

          for (int x = 0; x < (int) next_x_vals.size(); x++) {
            next_x_vals[x] = wp_delta*x;
            next_y_vals[x] = polyeval(coeffs, next_x_vals[x]);
          }

          // Calculate the cross-track-error
          // current CTE is fitted polynomial (road curve), evaluated at px = 0.0
          cte = coeffs[0];

          // Current heading error epsi is the tangent (derivative) to the road curve,
          // also evaluated at px = 0.0.
          // f = ax^3 + bx + c --> f'(x)=2ax+b --> f'(0)=b
          // y(x)   = a*x^3 + b*x^2 + c*x + d --->  y'(x) = 3*a*x^2 + 2*b*x + c
          // coeffs = [d, c, b, a]
          // epsi = arctan(y'(0)) = arctan(coeffs[1])
          epsi = -atan(coeffs[1]);

          // In initial state for trajectory planning x, y, and psi are 0
          // because they now represent the position and orientation in
          // local car coordinates.
          px = py = psi = 0.0;
          // Predicting offset to initial state by taking actuation delays into account
          if  (latency_s > 0.0) {
            px    += v * cos(-psi) * latency_s;
            py    += v * sin(-psi) * latency_s;
            psi    = fmod(psi + v * steer_value / Lf * latency_s, 2*M_PI);
            cte   += v * sin(epsi) * latency_s;
            epsi  += v * steer_value / Lf * latency_s;
            v     += throttle_value * latency_s;
          }

          Eigen::VectorXd state(6);
          state << px, py, psi, v, cte, epsi;

          auto solution = mpc.Solve(state, coeffs, mpc_x_vals, mpc_y_vals, success);

          // Calculate steering angle and throttle using MPC.
          // Both are in between [-1, 1].
          // But only update values if solver succeeded, otherwise keep old values!
          if (success) {
            steer_value    = solution[0];
            throttle_value = solution[1];
          } else {
            std::cout << "Optimizer failed to find solution!" << std::endl;
          }

          json msgJson;

          // Note:
          // - If steering is positive we rotate counter-clockwise, or turn left.
          //   In the simulator however, a positive value implies a right turn and a negative value implies a left turn.
          //   Therefore we change directions here.
          // - Remember to divide by deg2rad(25) before you send the steering value back.
          //   Otherwise the values will be in between [-deg2rad(25), deg2rad(25] instead of [-1, 1].
          msgJson["steering_angle"] = -steer_value / deg2rad(25);
          msgJson["throttle"] = throttle_value;

          //.. add (x,y) points to list here, points are in reference to the vehicle's coordinate system
          // the points in the simulator are connected by a Green line
          msgJson["mpc_x"] = mpc_x_vals;
          msgJson["mpc_y"] = mpc_y_vals;

          //.. add (x,y) points to list here, points are in reference to the vehicle's coordinate system
          // the points in the simulator are connected by a Yellow line
          msgJson["next_x"] = next_x_vals;
          msgJson["next_y"] = next_y_vals;

          auto msg = "42[\"steer\"," + msgJson.dump() + "]";
          //std::cout << msg << std::endl;

          // Latency
          // The purpose is to mimic real driving conditions where
          // the car does actuate the commands instantly.
          //
          // Feel free to play around with this value but should be to drive
          // around the track with 100ms latency.
          //
          // NOTE: REMEMBER TO SET THIS TO 100 MILLISECONDS BEFORE
          // SUBMITTING.
          chrono::high_resolution_clock::time_point end= chrono::high_resolution_clock::now();
          std::cout << "elapsed seconds: " <<
                       1e-3*chrono::duration_cast<chrono::milliseconds>(end-begin).count() << endl;

          this_thread::sleep_for(chrono::milliseconds(100));
          ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
        }
      } else {
        // Manual driving
        std::string msg = "42[\"manual\",{}]";
        ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
      }
    }
  });

  // We don't need this since we're not using HTTP but if it's removed the
  // program
  // doesn't compile :-(
  h.onHttpRequest([](uWS::HttpResponse *res, uWS::HttpRequest req, char *data,
                     size_t, size_t) {
    const std::string s = "
Hello world!
";
    if (req.getUrl().valueLength == 1) {
      res->end(s.data(), s.length());
    } else {
      // i guess this should be done more gracefully?
      res->end(nullptr, 0);
    }
  });

  h.onConnection([&h](uWS::WebSocket<uWS::SERVER> ws, uWS::HttpRequest req) {
    std::cout << "Connected!!!" << std::endl;
  });

  h.onDisconnection([&h](uWS::WebSocket<uWS::SERVER> ws, int code,
                         char *message, size_t length) {
    ws.close();
    std::cout << "Disconnected" << std::endl;
  });

  int port = 4567;
  if (h.listen(port)) {
    std::cout << "Listening to port " << port << std::endl;
  } else {
    std::cerr << "Failed to listen to port" << std::endl;
    return -1;
  }
  h.run();
}


//  Copyright John Maddock 2007.
//  Copyright Paul A. Bristow 2007, 2010.

//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifdef _MSC_VER
# pragma warning (disable : 4305) // 'initializing' : truncation from 'long double' to 'const eval_type'
# pragma warning (disable : 4244) //  conversion from 'long double' to 'const eval_type'
#endif

#include <iostream>
using std::cout; using std::endl;

//[policy_eg_3

#include <boost/math/distributions/binomial.hpp>
using boost::math::binomial_distribution;

// Begin by defining a policy type, that gives the behaviour we want:

//using namespace boost::math::policies; or explicitly
using boost::math::policies::policy;

using boost::math::policies::promote_float;
using boost::math::policies::discrete_quantile;
using boost::math::policies::integer_round_nearest;

typedef policy<
   promote_float<false>, // Do not promote to double.
   discrete_quantile<integer_round_nearest> // Round result to nearest integer.
> mypolicy;
//
// Then define a new distribution that uses it:
typedef boost::math::binomial_distribution<float, mypolicy> mybinom;

//  And now use it to get the quantile:

int main()
{
   cout << "quantile(mybinom(200, 0.25), 0.05) is: " <<
      quantile(mybinom(200, 0.25), 0.05) << endl;
}

//]

/*

Output:

  quantile(mybinom(200, 0.25), 0.05) is: 40

*/


#include <math.h>
#include <EigenUnsupported/Eigen/KroneckerProduct>
#include "Core/Utilities/QProgInfo/QCircuitInfo.h"
#include "Core/Utilities/Tools/MatrixDecomposition.h"
#include <chrono>
#include "Core/Utilities/QProgInfo/Visualization/QVisualization.h"
#include "QAlg/Base_QCircuit/AmplitudeEncode.h"

USING_QPANDA
using namespace std;
using namespace chrono;

#define PRINT_TRACE 0
#if PRINT_TRACE
#define PTrace printf
#define PTraceMat(mat) (std::cout << (mat) << endl)
#define PTraceCircuit(cir) (std::cout << cir << endl)
#else
#define PTrace
#define PTraceMat(mat)
#define PTraceCircuit(cir)
#endif

#define MAX_MATRIX_PRECISION 1e-10

using MatrixSequence = std::vector<MatrixUnit>;
using DecomposeEntry = std::pair<int, MatrixSequence>;

using ColumnOperator = std::vector<DecomposeEntry>;
using MatrixOperator = std::vector<ColumnOperator>;

using SingleGateUnit = std::pair<MatrixSequence, QStat>;

static void upper_partition(int order, MatrixOperator &entries)
{
	auto index = (int)std::log2(entries.size() + 1) - (int)std::log2(order) - 1;

	for (auto cdx = 0; cdx < order - 1; ++cdx)
	{
		for (auto rdx = 0; rdx < order - cdx - 1; ++rdx)
		{
			auto entry = entries[cdx][rdx];

			entry.first += order;
			entry.second[index] = MatrixUnit::SINGLE_P1;

			entries[cdx + order].emplace_back(entry);
		}
	}

    return;
}

static bool entry_requirement(const MatrixSequence& units, int udx, int cdx)
{
	int lj = ((cdx - 1) >> (udx - 1)) & 1;

	int M = 1;
	while (cdx)
	{
		cdx >>= 1;
		M += cdx ? 1 : 0;
	}

	//if 1 ≤ j ≤ m and cj = lj' = 1 , return true
	auto mat = units[units.size() - udx];
	return udx >= 1
		&& udx <= M
		&& lj
		&& mat == MatrixUnit::SINGLE_P1;
}

static bool steps_requirement(const MatrixSequence& units, int udx, int cdx)
{
	int M = 1;
	while (cdx)
	{
		cdx >>= 1;
		M += cdx ? 1 : 0;
	}

	//if j = n and none of cn...cm+1 is 1 , return true
	if (units.size() != udx)
	{
		return false;
	}
	else
	{
		auto iter = std::find(units.begin(), units.end() - M, MatrixUnit::SINGLE_P1);
		return (units.end() - M) == iter;
	}
}

static void under_partition(int order, MatrixOperator& entries)
{
	auto qubits = (int)std::log2(entries.size() + 1);

	for (auto cdx = 1; cdx < order; ++cdx)
	{
		if (cdx & 1)
		{
			for (auto rdx = 0; rdx < order; ++rdx)
			{
				auto value = entries[0][rdx + order - 1].first ^ cdx;
				auto entry = make_pair(value, entries[cdx - 1][rdx + order - cdx].second);

				entries[cdx].emplace_back(entry);
			}

			auto &units = entries[cdx].back().second;
			for (auto idx = 0; idx < (int)std::log2(order); ++idx)
			{
				units[qubits - idx - 1] = ((cdx >> idx) & 1) ?
					MatrixUnit::SINGLE_P1 : MatrixUnit::SINGLE_I2;
			}
		}
		else
		{
			for (auto rdx = 0; rdx < order; ++rdx)
			{
				auto range = (int)std::log2(order) + 1;
				auto refer = entries[0][rdx + order - 1].second;
				auto entry = entries[0][rdx + order - 1].first ^ cdx;

				MatrixSequence units(refer.begin() + qubits - range, refer.end());

				for (auto udx = 1; udx <= range; ++udx)  /*udx = j , cdx = L*/
				{
					bool steps_accord = steps_requirement(units, udx, cdx + 1);
					bool entry_accord = entry_requirement(units, udx, cdx + 1);

					units[range - udx] = steps_accord ? MatrixUnit::SINGLE_P1 :
						entry_accord ? MatrixUnit::SINGLE_P0 : units[range - udx];
				}

				for (auto idx = 0; idx < qubits - range; ++idx)
				{
					units.insert(units.begin(), MatrixUnit::SINGLE_I2);
				}

				entries[cdx].emplace_back(make_pair(entry, units));
			}

			auto refer_opt = entries[0][2 * order - 2].second;
			for (auto idx = 0; idx < qubits; ++idx)
			{
				if ((cdx >> idx) & 1)
				{
					refer_opt[qubits - idx - 1] = MatrixUnit::SINGLE_P1;
				}
			}

			entries[cdx].back().second = refer_opt;
		}
	}

    return;
}

static void controller(MatrixSequence &sequence, const EigenMatrix2c U2, EigenMatrixXc &matrix)
{
	EigenMatrix2c P0;
	EigenMatrix2c P1;
	EigenMatrix2c I2;

	P0 << Eigen::dcomplex(1, 0), Eigen::dcomplex(0, 0),
		  Eigen::dcomplex(0, 0), Eigen::dcomplex(0, 0);
	P1 << Eigen::dcomplex(0, 0), Eigen::dcomplex(0, 0),
		  Eigen::dcomplex(0, 0), Eigen::dcomplex(1, 0);
	I2 << Eigen::dcomplex(1, 0), Eigen::dcomplex(0, 0),
		  Eigen::dcomplex(0, 0), Eigen::dcomplex(1, 0);

	std::map<MatrixUnit, std::function<EigenMatrix2c()>> mapping =
	{
		{ MatrixUnit::SINGLE_P0, [&]() {return P0; } },
		{ MatrixUnit::SINGLE_P1, [&]() {return P1; } },
		{ MatrixUnit::SINGLE_I2, [&]() {return I2; } },
		{ MatrixUnit::SINGLE_V2, [&]() {return U2 - I2; } }
	};

	auto order = sequence.size();
	EigenMatrixXc Un = EigenMatrixXc::Identity(1, 1);
	EigenMatrixXc In = EigenMatrixXc::Identity(1ull << order, 1ull << order);

	for (const auto &val : sequence)
	{
		EigenMatrix2c M2 = mapping.find(val)->second();
		Un = Eigen::kroneckerProduct(Un, M2).eval();
	}

	matrix = In + Un;
    return;
}

static void recursive_partition(const EigenMatrixXc& sub_matrix, MatrixOperator &entries)
{
    Eigen::Index order = sub_matrix.rows();
    if (1 == order)
    {
        return;
    }
    else
    {
        EigenMatrixXc corner = sub_matrix.topLeftCorner(order / 2, order / 2);

        recursive_partition(corner, entries);

        upper_partition(order / 2, entries);
        under_partition(order / 2, entries);
    }

    return;
}

static void decomposition(EigenMatrixXc& matrix, MatrixOperator& entries, std::vector<SingleGateUnit>& cir_units)
{
	for (auto cdx = 0; cdx < entries.size(); ++cdx)
	{
		auto opts = entries[cdx].size();
		for (auto idx = 0; idx < opts; ++idx)
		{
			auto rdx = entries[cdx][idx].first;
			auto opt = entries[cdx][idx].second;

			if ((((abs(matrix(rdx, cdx).real()) < MAX_MATRIX_PRECISION) && (abs(matrix(rdx, cdx).imag()) < MAX_MATRIX_PRECISION)) && (idx != opts - 1)) ||
				(((abs(matrix(cdx + 1, cdx).real() - 1.0) < MAX_MATRIX_PRECISION) && (abs(matrix(cdx + 1, cdx).imag()) < MAX_MATRIX_PRECISION)) && (idx == opts - 1)))
			/*if ((EigenComplexT(0, 0) == matrix(rdx, cdx) && (idx != opts - 1)) ||
				(EigenComplexT(1, 0) == matrix(cdx + 1, cdx) && (idx == opts - 1)))*/
			{
				continue;
			}
			else
			{
				EigenMatrix2c C2; /*placeholder*/
				C2 << EigenComplexT(0, 1), EigenComplexT(0, 1),
					EigenComplexT(0, 1), EigenComplexT(0, 1);

				EigenMatrixXc Cn;
				controller(opt, C2, Cn);

				Qnum indices(2);
				for (Eigen::Index index = 0; index < (1ull << opt.size()); ++index)
				{
					if (Cn(rdx, index) != EigenComplexT(0, 0))
					{
						indices[index == rdx] = index;
					}
				}

				EigenComplexT C0 = matrix(indices[0], cdx);  /*The entry to be eliminated */
				EigenComplexT C1 = matrix(indices[1], cdx);  /*The corresponding entry */

				EigenComplexT V11, V12, V21, V22;

				if (indices[0] < indices[1])
				{
					V11 = std::conj(C0) / std::sqrt(std::norm(C0) + std::norm(C1));
					V12 = std::conj(C1) / std::sqrt(std::norm(C0) + std::norm(C1));
					V21 = C1 / std::sqrt(std::norm(C0) + std::norm(C1));
					V22 = -C0 / std::sqrt(std::norm(C0) + std::norm(C1));
				}
				else
				{
					V11 = -C0 / std::sqrt(std::norm(C0) + std::norm(C1));
					V12 = C1 / std::sqrt(std::norm(C0) + std::norm(C1));
					V21 = std::conj(C1) / std::sqrt(std::norm(C0) + std::norm(C1));
					V22 = std::conj(C0) / std::sqrt(std::norm(C0) + std::norm(C1));
				}

				EigenMatrix2c V2;
				V2 << V11, V12, V21, V22;

				EigenMatrixXc Un;
				controller(opt, V2, Un);

				matrix = Un * matrix;

				QStat M2 = { (qcomplex_t)V11 ,(qcomplex_t)V12 ,(qcomplex_t)V21 ,(qcomplex_t)V22 };
				cir_units.insert(cir_units.begin(), std::make_pair(opt, M2));
			}
		}
	}

	EigenMatrix2c V2 = matrix.bottomRightCorner(2, 2);
	if(!V2.isApprox(EigenMatrixXc::Identity(2, 2), MAX_MATRIX_PRECISION))
	//if (EigenMatrixXc::Identity(2, 2) != V2)
	{
		QStat M2 = { (qcomplex_t)((EigenComplexT)1.0 / V2(0,0)), (qcomplex_t)(V2(0,1)),
					 (qcomplex_t)(V2(1,0)) , (qcomplex_t)((EigenComplexT)1.0 / V2(1,1))};

		auto entry = entries.back().back().second;
		cir_units.insert(cir_units.begin(), std::make_pair(entry, M2));
	}
}

static void initialize(EigenMatrixXc& matrix, MatrixOperator& entries)
{
    auto qubits = (int)std::log2(matrix.rows());

    MatrixSequence Cns(qubits, MatrixUnit::SINGLE_I2);
    Cns.back() = MatrixUnit::SINGLE_V2;
    entries.front().emplace_back(make_pair(1, Cns));

    ColumnOperator& column = entries.front();
    for (auto idx = 1; idx < qubits; ++idx)
    {
        size_t path = 1ull << idx;
        for (auto opt = 0; opt < (1 << idx) - 1; ++opt)
        {
            auto entry = column[opt].first;
            auto units = column[opt].second;

            // 1 : none of cn−1, . . . , c1 equals 1
            // * : otherwise
            auto iter = std::find(units.end() - idx, units.end(), MatrixUnit::SINGLE_P1);

            units[units.size() - 1 - idx] = (units.end() == iter) ?
                MatrixUnit::SINGLE_P1 : MatrixUnit::SINGLE_I2;

            column.emplace_back(make_pair(entry + path, units));
        }

        MatrixSequence Lns(qubits, MatrixUnit::SINGLE_I2);
        Lns[qubits - idx - 1] = MatrixUnit::SINGLE_V2;

        column.emplace_back(make_pair((1ull << idx), Lns));
    }

    return;
}

static void general_scheme(EigenMatrixXc& matrix, std::vector<SingleGateUnit>& cir_units)
{
	MatrixOperator entries;
	for (auto idx = 1; idx < matrix.cols(); ++idx)
	{
		ColumnOperator Co;
		entries.emplace_back(Co);
	}

	initialize(matrix, entries);
 	recursive_partition(matrix, entries);
	decomposition(matrix, entries, cir_units);

    return;
}

static void circuit_insert(QVec& qubits, std::vector<SingleGateUnit>& cir_units, QCircuit &circuit)
{
	std::sort(qubits.begin(), qubits.end(), [&](Qubit *a, Qubit *b)
	{
		return a->getPhysicalQubitPtr()->getQubitAddr()
			 < b->getPhysicalQubitPtr()->getQubitAddr();
	});

	auto rank = qubits.size();
	for (auto &val : cir_units)
	{
		QVec control;
		QCircuit cir;

		for (auto qdx = 0; qdx < rank; qdx++)
		{
			if (MatrixUnit::SINGLE_P0 == val.first[qdx])
			{
				cir << X(qubits[qdx]);
				control.emplace_back(qubits[qdx]);
			}
			else if (MatrixUnit::SINGLE_P1 == val.first[qdx])
			{
				control.emplace_back(qubits[qdx]);
			}
			else
			{}
		}

		for (auto qdx = 0; qdx < rank; qdx++)
		{
			if (MatrixUnit::SINGLE_V2 == val.first[qdx])
			{
				circuit << cir
					    << U4(val.second, qubits[qdx]).control(control).dagger()
						<< cir;

				break;
			}
		}
	}
}

/*******************************************************************
*                      class DiagonalMatrixDecompose
********************************************************************/
class DiagonalMatrixDecompose
{
public:
	DiagonalMatrixDecompose() {}
	~DiagonalMatrixDecompose() {}

	QCircuit decompose(const QVec& qubits, const QStat& src_mat)
	{
		//check param
		if (!is_unitary_matrix(src_mat))
		{
			QCERR_AND_THROW_ERRSTR(run_fail, "Error: failed on HQRDecompose, the input matrix is not a unitary-matrix.");
		}

		const auto mat_dimension = sqrt(src_mat.size());
		const auto need_qubits_num = ceil(log2(mat_dimension));
		if (need_qubits_num > qubits.size())
		{
			QCERR_AND_THROW_ERRSTR(run_fail, "Error: failed on HQRDecompose, no enough qubits.");
		}

		QCircuit decompose_result_cir;
		m_qubits = qubits;
		QVec controlqvec = qubits;
		controlqvec.pop_back();
		QStat tmp_mat22; //2*2 unitary matrix
		const size_t tmp_base_unitary_cnt = mat_dimension / 2;
		long pre_index = -1;
		for (size_t i = 0; i < tmp_base_unitary_cnt; ++i)
		{
			tmp_mat22.clear();
			const size_t tmp_row = (2 * i * mat_dimension) + (2 * i);
			tmp_mat22.push_back(src_mat[tmp_row]);
			tmp_mat22.push_back(src_mat[tmp_row + 1]);
			tmp_mat22.push_back(src_mat[tmp_row + mat_dimension]);
			tmp_mat22.push_back(src_mat[tmp_row + mat_dimension + 1]);
			QGate tmp_u4 = U4(tmp_mat22, qubits.back()).control(controlqvec);
			QGATE_SPACE::U4* p_gate = dynamic_cast<QGATE_SPACE::U4*>(tmp_u4.getQGate());
			if ((abs(p_gate->getAlpha()) < MAX_MATRIX_PRECISION)
				&& (abs(p_gate->getBeta()) < MAX_MATRIX_PRECISION)
				&& (abs(p_gate->getGamma()) < MAX_MATRIX_PRECISION)
				&& (abs(p_gate->getDelta()) < MAX_MATRIX_PRECISION))
			{
				continue;
			}

			if (0 == i)
			{
				QCircuit index_cir_zero = index_to_circuit(0, controlqvec);
				decompose_result_cir << index_cir_zero;
			}
			else
			{
				QCircuit index_cir = index_to_merge_circuit(i, pre_index, controlqvec);
				decompose_result_cir << index_cir;
			}

			decompose_result_cir << tmp_u4;
			pre_index = i;
		}

		return decompose_result_cir;
	}

protected:
	QCircuit index_to_circuit(size_t index, QVec& controlqvec)
	{
		QCircuit ret_cir;
		size_t data_qubits_cnt = controlqvec.size();
		for (size_t i = 0; i < data_qubits_cnt; ++i)
		{
			if (0 == index % 2)
			{
				ret_cir << X(controlqvec[data_qubits_cnt - i - 1]);
			}

			index /= 2;
		}

		return ret_cir;
	}

	QCircuit index_to_merge_circuit(size_t index, QVec& controlqvec)
	{
		if (0 == index)
		{
			QCERR_AND_THROW_ERRSTR(run_fail, "Error: failed to build merge-index-circuit, the index must be >0.");
		}

		size_t pre_index = index - 1;
		QCircuit ret_cir;
		size_t data_qubits_cnt = controlqvec.size();
		for (size_t i = 0; i < data_qubits_cnt; ++i)
		{
			if ((index % 2) != (pre_index % 2))
			{
				ret_cir << X(controlqvec[data_qubits_cnt - i - 1]);
			}

			index /= 2;
			pre_index /= 2;
		}

		return ret_cir;
	}

	QCircuit index_to_merge_circuit(size_t index, long pre_index, QVec& controlqvec)
	{
		if (0 == index)
		{
			QCERR_AND_THROW_ERRSTR(run_fail, "Error: failed to build merge-index-circuit, the index must be >0.");
		}

		size_t tmp_pre_index = pre_index;
		if (pre_index < 0)
		{
			tmp_pre_index = 1;
		}

		QCircuit ret_cir;
		size_t data_qubits_cnt = controlqvec.size();
		for (size_t i = 0; i < data_qubits_cnt; ++i)
		{
			if ((index % 2) != (tmp_pre_index % 2))
			{
				ret_cir << X(controlqvec[data_qubits_cnt - i - 1]);
			}

			index /= 2;

			if (pre_index > 0)
			{
				tmp_pre_index /= 2;
			}
		}

		return ret_cir;
	}

private:
	QVec m_qubits;
};

/*******************************************************************
*                      class HQRDecompose
* Householder QR-decompose
* refer to <Quantum circuits synthesis using Householder transformations>(https://arxiv.org/abs/2004.07710v1)
********************************************************************/
class HQRDecompose
{
public:
	HQRDecompose()
		:m_dimension(0)
	{}
	~HQRDecompose() {}

	QCircuit decompose(QVec qubits, const EigenMatrixXc& src_mat)
	{
		//check param
		if (!src_mat.isUnitary(MAX_MATRIX_PRECISION))
		{
			QCERR_AND_THROW_ERRSTR(run_fail, "Error: failed on HQRDecompose, the input matrix is not a unitary-matrix.");
		}

		const auto mat_dimension = src_mat.rows();
		const auto need_qubits_num = ceil(log2(mat_dimension));
		if (need_qubits_num > qubits.size())
		{
			QCERR_AND_THROW_ERRSTR(run_fail, "Error: failed on HQRDecompose, no enough qubits.");
		}

		// do Householder QR_decompose
		m_dimension = mat_dimension;
		m_qubits = qubits;
		auto start = system_clock::now();
		Householder_QR_decompose(src_mat);

#if PRINT_TRACE
		auto end = system_clock::now();
		auto duration = duration_cast<microseconds>(end - start);
		PTrace("Total used: %f s",
			double(duration.count()) * microseconds::period::num / microseconds::period::den);
#endif

		return m_result_cir;
	}

protected:
	void print_vec(const QStat& vec)
	{
		for (auto i = 0; i < vec.size(); ++i)
		{
			printf("(%-g, %-g), ", vec[i].real(), vec[i].imag());
		}

		printf("\n");
	}

	void Householder_QR_decompose(EigenMatrixXc src_mat)
	{
		const auto& lines = m_dimension;
		QStat tmp_mat_R(m_dimension * m_dimension);
		QCircuit cir_Q;
		for (size_t cur_col = 0; cur_col < lines; ++cur_col)
		{
			//printf("On column %ld\n", cur_col);
			const auto cur_col_size = lines - cur_col;
			QStat cur_col_vec(cur_col_size);
			QStat cur_col_vec_dagger(cur_col_size);
			qstate_type norm = 0;
			for (size_t cur_row = cur_col; cur_row < lines; ++cur_row)
			{
				cur_col_vec[cur_row - cur_col] = -src_mat(cur_row, cur_col);
				norm += (cur_col_vec[cur_row - cur_col].real() * cur_col_vec[cur_row - cur_col].real() +
					cur_col_vec[cur_row - cur_col].imag() * cur_col_vec[cur_row - cur_col].imag());
			}

			norm = sqrt(norm);
			const double angle = arg(cur_col_vec[0]);
			cur_col_vec[0] -= exp(qcomplex_t(0, angle)); // ?
			//vec[0] = exp(complex_t(0, 1.0 * angle)) * (sqrt(vec[0].real() * vec[0].real() + vec[0].imag() * vec[0].imag()) - tmp_x);

			norm = 0.0;
			for (size_t i = 0; i < cur_col_size; ++i)
			{
				norm += (cur_col_vec[i].real() * cur_col_vec[i].real() + cur_col_vec[i].imag() * cur_col_vec[i].imag());
			}

			if (norm > 1e-7)
			{
				// vec Dagger
				for (size_t i = 0; i < cur_col_size; ++i)
				{
					qstate_type real_v = cur_col_vec[i].real();
					qstate_type imag_v = cur_col_vec[i].imag();
					//cur_col_vec[i] = qcomplex_t(real_v, imag_v);
					cur_col_vec_dagger[i] = qcomplex_t(real_v, -imag_v);
				}
#if PRINT_TRACE
				printf("The vec:\n");
				print_vec(cur_col_vec);
				printf("The vec_dagger:\n");
				print_vec(cur_col_vec_dagger);
				printf(":::::::::::::::::::::::::::::\n");
#endif
				//sestavit matici P
				EigenMatrixXc matrix_p(cur_col_size, cur_col_size);
				for (size_t k = 0; k < cur_col_size; ++k)
				{
					for (size_t h = 0; h < cur_col_size; ++h)
					{
						if (k == h)
						{
							matrix_p(k, k) = qcomplex_t(1, 0) - qcomplex_t(2, 0) * cur_col_vec[k] * cur_col_vec_dagger[h] / norm;

						}
						else
						{
							matrix_p(k, h) = -qcomplex_t(2, 0) * cur_col_vec[k] * cur_col_vec_dagger[h] / norm;
						}
					}
				}
				PTrace("-----tmp matrixP:\n");
				PTraceMat(matrix_p);
				PTrace("tmp matrixP end -----------:\n");

				auto norm_sqr = sqrt(norm);
				for (size_t i = 0; i < cur_col_size; ++i)
				{
					cur_col_vec[i] /= norm_sqr;
				}
				cir_Q << build_cir_Pi(cur_col_vec);
				/*using testMat = Eigen::Matrix<qcomplex_t, -1, -1, Eigen::RowMajor>;
				testMat mat_test_1 = testMat::Map(&matrix_p[0], cur_col_size, cur_col_size);*/
				/*testMat mat_test_2 = testMat::Map(&src_mat[0], m_dimension, m_dimension);*/
				EigenMatrixXc mat_test_3 = src_mat.bottomRightCorner(m_dimension - cur_col, m_dimension - cur_col);
				matrix_p *= mat_test_3;
				//src_mat.block(cur_col, cur_col, m_dimension - cur_col, m_dimension - cur_col) *= matrix_p;
				//src_mat.bottomRightCorner(m_dimension - cur_col, m_dimension - cur_col) = matrix_p;
				src_mat.block(cur_col, cur_col, m_dimension - cur_col, m_dimension - cur_col) = matrix_p;
				PTrace("-----tmp matrixA:\n");
				PTraceMat(src_mat);
				PTrace("tmp matrixA end -----------:\n");
			}
		}

		//QCircuit last_cir_D = matrix_decompose(m_qubits, src_mat);
		QStat mat_r(src_mat.data(), src_mat.data() + src_mat.size());
		QCircuit last_cir_D = diagonal_matrix_decompose(m_qubits, mat_r);
		m_result_cir << last_cir_D << cir_Q.dagger();
	}

	QCircuit build_cir_Pi(const QStat& cur_col_vec)
	{
		QStat full_cur_vec(m_dimension - cur_col_vec.size(), qcomplex_t(0, 0));
		full_cur_vec.insert(full_cur_vec.end(), cur_col_vec.begin(), cur_col_vec.end());
		if (full_cur_vec.size() != m_dimension)
		{
			QCERR_AND_THROW_ERRSTR(run_fail, "Error: current vector size error on HQRDecompose.");
		}

		QCircuit cir_swap_qubits;
		for (size_t i = 0; (i * 2) < (m_qubits.size() - 1); ++i)
		{
			cir_swap_qubits << SWAP(m_qubits[i], m_qubits[m_qubits.size() - 1 - i]);
		}

		std::vector<double> ui_mod(m_dimension);
		std::vector<double> ui_angle(m_dimension);
		double tatal = 0.0;
		for (size_t i = 0; i < m_dimension; ++i)
		{
			auto tmp_m = full_cur_vec[i].real() * full_cur_vec[i].real() + full_cur_vec[i].imag() * full_cur_vec[i].imag();
			tatal += tmp_m;
			ui_mod[i] = sqrt(tmp_m);
			ui_angle[i] = arg(full_cur_vec[i]);
		}

		QStat mat_d(m_dimension * m_dimension, qcomplex_t(0, 0));
		for (size_t i = 0; i < m_dimension; ++i)
		{
			mat_d[i + i * m_dimension] = exp(qcomplex_t(0, ui_angle[i]));
		}

		QCircuit cir_d = diagonal_matrix_decompose(m_qubits, mat_d);

		PTrace("cir_d:\n");
		PTraceCircuit(cir_d);

#if PRINT_TRACE
		const auto mat_test_d = getCircuitMatrix(cir_d);
		PTrace("mat_test_d:\n");
		PTraceCircuit(mat_test_d);
		if (mat_test_d == mat_d)
		{
			cout << "matrix decompose okkkkkkkkkkkkkkk" << endl;
		}
		else
		{
			cout << "ffffffffffffffffffffailed on matrix decompose." << endl;
		}
#endif

		QCircuit cir_y = build_cir_b(m_qubits, ui_mod);
		QCircuit cir_P;
		cir_P << cir_y << cir_swap_qubits  << cir_d << cir_swap_qubits;

		QCircuit cir_DG = zero_phase_shift_cir();
		QCircuit cir_pi;
		cir_pi << cir_swap_qubits  << cir_P.dagger() << cir_DG << cir_P << cir_swap_qubits;

		return cir_pi;
	}

	QCircuit zero_phase_shift_cir()
	{
		QCircuit cir_DG;
		QVec tmp_qubits = m_qubits;
		tmp_qubits.pop_back();
		cir_DG << applyQGate(m_qubits, X) << Z(m_qubits.back()).control(tmp_qubits) << applyQGate(m_qubits, X);

		return cir_DG;
	}

	QCircuit build_cir_b(QVec qubits, const std::vector<double>& b)
	{
		return amplitude_encode(qubits, b);
	}

private:
	QVec m_qubits;
	size_t m_dimension;
	QCircuit m_result_cir;
};

static QCircuit Householder_qr_matrix_decompose(QVec qubits, const EigenMatrixXc& src_mat)
{
	return HQRDecompose().decompose(qubits, src_mat);
}

/*******************************************************************
*                      public interface
********************************************************************/
QCircuit QPanda::matrix_decompose(QVec qubits, const QStat& src_mat, DecompositionMode de_mode/* = HOUSEHOLDER_QR*/)
{
	auto order = std::sqrt(src_mat.size());
	EigenMatrixXc tmp_mat = EigenMatrixXc::Map(&src_mat[0], order, order);

	return matrix_decompose(qubits, tmp_mat, de_mode);
}

QCircuit QPanda::matrix_decompose(QVec qubits, EigenMatrixXc& src_mat, DecompositionMode de_mode/* = HOUSEHOLDER_QR*/)
{
	if (!src_mat.isUnitary(MAX_MATRIX_PRECISION))
	{
		QCERR_AND_THROW_ERRSTR(invalid_argument, "Non-unitary matrix.");
	}

	if (qubits.size() != log2(src_mat.cols()))
	{
		QCERR_AND_THROW_ERRSTR(invalid_argument, "The qubits number is error.");
	}

	QCircuit output_circuit;
	switch (de_mode)
	{
	case HOUSEHOLDER_QR:
		output_circuit = Householder_qr_matrix_decompose(qubits, src_mat);
		break;

	default:
	{
		//QR decompose
		std::vector<SingleGateUnit> cir_units;
		general_scheme(src_mat, cir_units);
		circuit_insert(qubits, cir_units, output_circuit);
	}
		break;
	}

	return output_circuit;
}

QCircuit QPanda::diagonal_matrix_decompose(const QVec& qubits, const QStat& src_mat)
{
	return DiagonalMatrixDecompose().decompose(qubits, src_mat);
}


#include "Angle.hpp"
#include <boost/format.hpp>
#include <base/Eigen.hpp>

namespace base {

Angle Angle::vectorToVector(const Vector3d& a, const Vector3d& b)
{
    double dot = a.dot(b);
    double norm = a.norm() * b.norm();
    return fromRad(acos(dot / norm));
}

Angle Angle::vectorToVector(const Vector3d& a, const Vector3d& b, const Vector3d& positive)
{
    double cos = a.dot(b) / (a.norm() * b.norm());

    bool is_positive = (a.cross(b).dot(positive) > 0);
    if (is_positive)
        return fromRad(acos(cos));
    else
        return fromRad(-acos(cos));
}

std::ostream& operator << (std::ostream& os, Angle angle)
{
    os << angle.getRad() << boost::format("[%3.1fdeg]") % angle.getDeg();
    return os;
}

AngleSegment::AngleSegment(): width(0), startRad(0), endRad(0)
{
}

AngleSegment::AngleSegment(const Angle &start, double _width): width(_width), startRad(start.getRad()), endRad(startRad + width)
{
    if(width < 0)
        throw std::runtime_error("Error got segment with negative width");
}

bool AngleSegment::isInside(const Angle& angle) const
{
    double angleRad = angle.getRad();
    if(angleRad < startRad)
        angleRad += 2*M_PI;

    if(angleRad <= endRad) //startRad <= angleRad &&
        return true;

    return false;
}

bool AngleSegment::isInside(const AngleSegment& segment) const
{
    double otherStart = segment.startRad;
    if(otherStart < startRad)
        otherStart += 2*M_PI;

    double otherEnd = otherStart + segment.width;

    if(otherEnd <= endRad)
        return true;

    return false;
}

std::vector< AngleSegment > AngleSegment::getIntersections(const AngleSegment& b) const
{
    std::vector<AngleSegment> ret;
    //special case, this segment is a whole circle
    if(width >= 2*M_PI)
    {
        ret.push_back(b);
        return ret;
    }

    //special case, other segment is a whole circle
    if(b.width >= 2*M_PI)
    {
        ret.push_back(*this);
        return ret;
    }

    double startA = startRad;
    double startB = b.startRad;
    double widthA = width;
    double widthB = b.width;

    //make A the smaller angle
    if(startA > startB)
    {
        std::swap(startA, startB);
        std::swap(widthA, widthB);
    }
    double endA = startA + widthA;
    double endB = startB + widthB;

    //test if segemnts do not intersect at all
    if(endA < startB)
    {
        //wrap case
        if(endB > M_PI)
        {
            //check if segments intersect after wrap correction
            if(startA < endB - 2*M_PI)
            {
                //this means the start of A is inside of B
                //drop first part of B and realign it to -M_PI
                //also switch A and B as B is now the 'lower' one
                double newWidthA = widthB - (M_PI - startB);
                startB = startA;
                widthB = widthA;
                startA = - M_PI;
                widthA = newWidthA;
                endA = startA + widthA;
                endB = startB + widthB;
                //no return, still need
                //to check for intersection
            }
            else
                //no intersection
                return ret;
        } else
                //no intersection
            return ret;
    }

    //normal case, no wrap around
    double newStart = startB;
    double newEnd = 0;

    if(endA < endB)
    {
        newEnd = endA;
    }
    else
    {
        newEnd = endB;
    }

    double newWidth = newEnd - newStart;

    //filter invalid segments
    if(newWidth > 1e-10)
        ret.push_back(AngleSegment(Angle::fromRad(newStart), newWidth));

    newStart = endB - 2*M_PI;
    if(newStart > startA)
    {
        newWidth = newStart - startA;
        //filter invalid segments
        if(newWidth > 1e-10)
            ret.push_back(AngleSegment(Angle::fromRad(startA), newWidth));
    }

    return ret;
}

Angle AngleSegment::getStart() const
{
    return Angle::fromRad(startRad);
}

Angle AngleSegment::getEnd() const
{
    return Angle::fromRad(endRad);
}

std::ostream& operator << (std::ostream& os, AngleSegment seg)
{
    os << " Segmend start " << seg.startRad/M_PI *180.0 << " end  " << seg.endRad/M_PI * 180.0 << " width " << seg.width /M_PI * 180.0;
    return os;
}

} //end namespace base


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RW_MATH_QUATERNION_HPP
#define RW_MATH_QUATERNION_HPP

/**
 * @file Quaternion.hpp
 */

#if !defined(SWIG)
#include <rw/common/Serializable.hpp>
#include <rw/math/Rotation3D.hpp>
#include <rw/math/Rotation3DVector.hpp>

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

namespace rw { namespace math {

    /** @addtogroup math */
    /*@{*/

    /**
     * @brief A Quaternion @f$ \mathbf{q}\in \mathbb{R}^4 @f$ a complex
     * number used to describe rotations in 3-dimensional space.
     * @f$ q_w+{\bf i}\ q_x+ {\bf j} q_y+ {\bf k}\ q_z @f$
     *
     * Quaternions can be added and multiplied in a similar way as usual
     * algebraic numbers. Though there are differences. Quaternion
     * multiplication is not commutative which means
     * \f$ Q\cdot P \neq P\cdot Q \f$
     */
    template< class T = double > class Quaternion : public rw::math::Rotation3DVector< T >
    {
      private:
        typedef Eigen::Quaternion< T > EigenQuaternion;

      public:

        //! Value type.
        typedef T value_type;

        /**
         * @brief constuct Quaterinion of {0,0,0,1}
         */
        Quaternion () : _q (0, 0, 0, 1) {}

        /**
         * @brief Creates a Quaternion
         * @param qx [in] @f$ q_x @f$
         * @param qy [in] @f$ q_y @f$
         * @param qz [in] @f$ q_z @f$
         * @param qw  [in] @f$ q_w @f$
         */
        Quaternion (T qx, T qy, T qz, T qw) : _q (qw, qx, qy, qz) {}

        /**
         * @brief Creates a Quaternion from another Quaternion
         * @param quat [in] Quaternion
         */
        Quaternion (const Quaternion< T >& quat) : _q (quat._q) {}

        /**
         * @brief Creates a Quaternion from another Rotation3DVector type
         * @param rot [in] The Rotation3DVector type
         */
        Quaternion (const rw::math::Rotation3DVector< T >& rot)
        {
            setRotation (rot.toRotation3D ());
        }

        /**
         * @brief Extracts a Quaternion from Rotation matrix using
         * setRotation(const Rotation3D<R>& rot)
         * @param rot [in] A 3x3 rotation matrix @f$ \mathbf{rot} @f$
         */
        Quaternion (const rw::math::Rotation3D< T >& rot) { setRotation (rot); }

        /**
         * @brief Creates a Quaternion from a Eigen quaternion
         * @param r [in] a boost quaternion
         */
        Quaternion (const Eigen::Quaternion< T >& r) : _q (r) {}

        /**
         * @brief Creates a Quaternion from vector_expression
         *
         * @param r [in] an Eigen Vector
         */
        template< class R >
        explicit Quaternion (const Eigen::MatrixBase< R >& r) :
            _q (r.row (3) (0), r.row (0) (0), r.row (1) (0), r.row (2) (0))
        {}

        // ###################################################
        // #                Acces Operators                  #
        // ###################################################

        /**
         * @brief get method for the x component
         * @return the x component of the quaternion
         */
        inline T getQx () const { return _q.x (); }

        /**
         * @brief get method for the y component
         * @return the y component of the quaternion
         */
        inline T getQy () const { return _q.y (); }

        /**
         * @brief get method for the z component
         * @return the z component of the quaternion
         */
        inline T getQz () const { return _q.z (); }

        /**
         * @brief get method for the w component
         * @return the w component of the quaternion
         */
        inline T getQw () const { return _q.w (); }

#if !defined(SWIG)
        /**
         * @brief Returns reference to Quaternion element
         * @param i [in] index in the quaternion \f$i\in \{0,1,2,3\} \f$
         * @return const reference to element
         */
        inline T operator() (size_t i) const
        {
            switch (i) {
                case 0: return _q.x ();
                case 1: return _q.y ();
                case 2: return _q.z ();
                case 3: return _q.w ();
                default: assert (0); return _q.x ();
            }
        }

        /**
         * @brief Returns reference to Quaternion element
         * @param i [in] index in the quaternion \f$i\in \{0,1,2,3\} \f$
         * @return reference to element
         */
        inline T& operator() (size_t i)
        {
            switch (i) {
                case 0: return _q.x ();
                case 1: return _q.y ();
                case 2: return _q.z ();
                case 3: return _q.w ();
                default: assert (0); return _q.x ();
            }
        }

        /**
         * @brief Returns reference to Quaternion element
         * @param i [in] index in the quaternion \f$i\in \{0,1,2,3\} \f$
         * @return reference to element
         */
        inline T& operator[] (size_t i)
        {
            switch (i) {
                case 0: return _q.x ();
                case 1: return _q.y ();
                case 2: return _q.z ();
                case 3: return _q.w ();
                default: assert (0); return _q.x ();
            }
        }

        /**
         * @brief Returns reference to Quaternion element
         * @param i [in] index in the quaternion \f$i\in \{0,1,2,3\} \f$
         * @return reference to element
         */
        inline T operator[] (size_t i) const
        {
            switch (i) {
                case 0: return _q.x ();
                case 1: return _q.y ();
                case 2: return _q.z ();
                case 3: return _q.w ();
                default: assert (0); return _q.x ();
            }
        }
#else
        ARRAYOPERATOR (T);
#endif
#if !defined(SWIGJAVA)

        /**
         * @brief Calculates the @f$ 3\times 3 @f$ Rotation matrix
         *
         * @return A 3x3 rotation matrix @f$ \mathbf{rot} @f$
         * @f$
         * \mathbf{rot} =
         *  \left[
         *   \begin{array}{ccc}
         *      1-2(q_y^2-q_z^2) & 2(q_x\ q_y+q_z\ q_w)& 2(q_x\ q_z-q_y\ q_w) \\
         *      2(q_x\ q_y-q_z\ q_w) & 1-2(q_x^2-q_z^2) & 2(q_y\ q_z+q_x\ q_w)\\
         *      2(q_x\ q_z+q_y\ q_w) & 2(q_y\ q_z-q_x\ q_z) & 1-2(q_x^2-q_y^2)
         *    \end{array}
         *  \right]
         * @f$
         *
         */
#endif
        inline const rw::math::Rotation3D< T > toRotation3D () const
        {
            const T qx = _q.x ();
            const T qy = _q.y ();
            const T qz = _q.z ();
            const T qw = _q.w ();

            return rw::math::Rotation3D< T > (1 - 2 * qy * qy - 2 * qz * qz,
                                              2 * (qx * qy - qz * qw),
                                              2 * (qx * qz + qy * qw),
                                              2 * (qx * qy + qz * qw),
                                              1 - 2 * qx * qx - 2 * qz * qz,
                                              2 * (qy * qz - qx * qw),
                                              2 * (qx * qz - qy * qw),
                                              2 * (qy * qz + qx * qw),
                                              1 - 2 * qx * qx - 2 * qy * qy);
        }

        /** @brief Converts a Rotation3D to a Quaternion and saves the Quaternion
         * in this.
         *
         * @param rot [in] A 3x3 rotation matrix @f$ \mathbf{R} @f$
         *
         * @f$
         * \begin{array}{c}
         * q_x\\ q_y\\ q_z\\ q_w
         * \end{array}
         * =
         *  \left[
         *   \begin{array}{c}
         *      \\
         *      \\
         *
         *    \end{array}
         *  \right]
         * @f$
         *
         * The conversion method is proposed by Henrik Gordon Petersen. The switching between
         * different cases occur well before numerical instabilities, hence the solution should be
         * more robust, than many of the methods proposed elsewhere.
         *
         */
        template< class R > void setRotation (const rw::math::Rotation3D< R >& rot)
        {
            // The method
            const T min  = (T) (-0.9);
            const T min1 = (T) (min / 3.0);

            const T tr = rot (0, 0) + rot (1, 1) + rot (2, 2);

            if (tr > min) {
                const T s = static_cast< T > (0.5) / static_cast< T > (sqrt (tr + 1.0));
                _q.w ()   = static_cast< T > (0.25) / s;
                _q.x ()   = static_cast< T > (rot (2, 1) - rot (1, 2)) * s;
                _q.y ()   = static_cast< T > (rot (0, 2) - rot (2, 0)) * s;
                _q.z ()   = static_cast< T > (rot (1, 0) - rot (0, 1)) * s;
            }
            else {
                if (rot (0, 0) > min1) {
                    const T sa =
                        static_cast< T > (sqrt (rot (0, 0) - rot (1, 1) - rot (2, 2) + 1.0));
                    _q.x ()   = static_cast< T > (0.5) * sa;
                    const T s = static_cast< T > (0.25) / _q.x ();
                    _q.y ()   = static_cast< T > (rot (0, 1) + rot (1, 0)) * s;
                    _q.z ()   = static_cast< T > (rot (0, 2) + rot (2, 0)) * s;
                    _q.w ()   = static_cast< T > (rot (2, 1) - rot (1, 2)) * s;
                }
                else if (rot (1, 1) > min1) {
                    const T sb = static_cast< T > (sqrt (rot (1, 1) - rot (2, 2) - rot (0, 0) + 1));
                    _q.y ()    = static_cast< T > (0.5) * sb;

                    const T s = static_cast< T > (0.25) / _q.y ();
                    _q.x ()   = static_cast< T > (rot (0, 1) + rot (1, 0)) * s;
                    _q.z ()   = static_cast< T > (rot (1, 2) + rot (2, 1)) * s;
                    _q.w ()   = static_cast< T > (rot (0, 2) - rot (2, 0)) * s;
                }
                else {
                    const T sc = static_cast< T > (sqrt (rot (2, 2) - rot (0, 0) - rot (1, 1) + 1));
                    _q.z ()    = static_cast< T > (0.5) * sc;

                    const T s = static_cast< T > (0.25) / _q.z ();
                    _q.x ()   = static_cast< T > (rot (0, 2) + rot (2, 0)) * s;
                    _q.y ()   = static_cast< T > (rot (1, 2) + rot (2, 1)) * s;
                    _q.w ()   = static_cast< T > (rot (1, 0) - rot (0, 1)) * s;
                }
            }
        }

        /**
         * @brief The dimension of the quaternion (i.e. 4).
         * This method is provided to help support generic algorithms using
         * size() and operator[].
         */
        size_t size () const { return 4; }

        /**
         * @brief Convert to an Eigen Quaternion.
         * @return Eigen Quaternion representation.
         */
        Eigen::Quaternion< T >& e () { return _q; }

        //! @copydoc e()
        const Eigen::Quaternion< T >& e () const { return _q; }

        /**
         * @brief convert to Eigen Vector
         * @return eigen Vector of quaternion
         */
        Eigen::Matrix< T, 4, 1 > toEigenVector () const
        {
            return Eigen::Matrix< T, 4, 1 > (_q.x (), _q.y (), _q.z (), _q.w ());
        }

        // ###################################################
        // #                 Math Operators                  #
        // ###################################################

        // ############ Quaternion Operations

        /**
         * @brief Unary minus.
         */
        Quaternion< T > operator- () const
        {
            return Quaternion (-_q.x (), -_q.y (), -_q.z (), -_q.w ());
        }

        /**
         * @brief Unary plus.
         */
        Quaternion< T > operator+ () const { return Quaternion (*this); }

        /**
         * @brief Subtraction.
         */
        inline const Quaternion< T > operator- (const Quaternion< T >& v)
        {
            return Quaternion< T > (
                (*this) (0) - v (0), (*this) (1) - v (1), (*this) (2) - v (2), (*this) (3) - v (3));
        }

        /**
         * @brief Multiply-from operator
         */
        inline Quaternion< T > operator* (const Quaternion< T >& r) const
        {
            Quaternion q = Quaternion (_q * r.e ());
            return q;
        }

        /**
           @brief Addition of two quaternions
         */
        inline const Quaternion< T > operator+ (const Quaternion< T >& v) const
        {
            return Quaternion< T > (
                (*this) (0) + v (0), (*this) (1) + v (1), (*this) (2) + v (2), (*this) (3) + v (3));
        }

        // ############ Scalar Operations

        /**
         * @brief Scalar multiplication.
         */
        inline const Quaternion< T > operator* (T s) const
        {
            return Quaternion< T > (_q.x () * s, _q.y () * s, _q.z () * s, _q.w () * s);
        }
#if !defined(SWIGPYTHON)
        /**
         * @brief Scalar multiplication.
         */
        inline friend const Quaternion< T > operator* (T s, const Quaternion< T >& v)
        {
            return v * s;
        }
#endif
        /**
         * @brief element whise division
         * @param lhs [in] the scalar to devide with
         * @return the result of elementwise devision
         */
        Quaternion< T > elemDivide (const T& lhs) const;

        // ############ Math Operations

        /**
         * @brief get length of quaternion
         * @f$ \sqrt{q_x^2+q_y^2+q_z^2+q_w^2} @f$
         * @return the length og this quaternion
         */
        inline T getLength () const { return _q.norm (); }

        /**
         * @brief get squared length of quaternion
         * @f$ q_x^2+q_y^2+q_z^2+q_w^2 @f$
         * @return the length og this quaternion
         */
        inline T getLengthSquared () const { return _q.squaredNorm (); }

        /**
         * @brief normalizes this quaternion so that
         * @f$ normalze(Q)=\frac{Q}{\sqrt{q_x^2+q_y^2+q_z^2+q_w^2}} @f$
         */
        inline void normalize () { _q.normalize (); };

        /**
         * @brief Calculates a slerp interpolation between \b this and \b v.
         *
         * The slerp interpolation ensures a constant velocity across the interpolation.
         * For \f$ t=0\f$ the result is \b this and for \f$ t=1\f$ it is \b v.
         *
         * @note Algorithm and implementation is thanks to euclideanspace.com
         */
        inline const Quaternion< T > slerp (const Quaternion< T >& v, const T t) const
        {
            return Quaternion (_q.slerp (t, v.e ()));
        }

        /*
         * @brief this will return the exponential of this quaternion \f$ e^Quaternion \f$
         * @return the exponential of this quaternion
         */
        Quaternion< T > exp () const;

        /**
         * @brief Calculate the inverse Quaternion
         * @return the inverse quaternion
         */
        Quaternion< T > inverse () const;

        /**
         * @brief calculates the natural logerithm of this quaternion
         * @return natural logetihm
         */
        Quaternion< T > ln () const;

        /**
         * @brief calculates the quaternion lifted to the power of \b power
         * @param power [in] the power the quaternion is lifted to
         * @return \f$ Quaternion^power \f$
         */
        Quaternion< T > pow (double power) const;

        // ###################################################
        // #             assignement Operators               #
        // ###################################################

        /**
         * @brief copy a boost quaternion to this Quaternion
         * @param r [in] - boost quaternion
         */
        inline void operator= (const Eigen::Quaternion< T >& r) { _q = r; }

        /**
           @brief Scalar multiplication.
         */
        inline const Quaternion< T > operator*= (T s)
        {
            _q.x () *= s;
            _q.y () *= s;
            _q.z () *= s;
            _q.w () *= s;
            return *this;
        }

        /**
         * @brief Multiply with operator
         */
        inline const Quaternion< T > operator*= (const Quaternion< T >& r)
        {
            *this = (*this) * r;
            return *this;
        }

        /**
         *@brief Add-to operator
         */
        inline const Quaternion< T > operator+= (const Quaternion< T >& r)
        {
            _q.x () += r (0);
            _q.y () += r (1);
            _q.z () += r (2);
            _q.w () += r (3);
            return *this;
        }

        /**
         * @brief Subtract-from operator
         */
        inline const Quaternion< T > operator-= (const Quaternion< T >& r)
        {
            _q.x () -= r (0);
            _q.y () -= r (1);
            _q.z () -= r (2);
            _q.w () -= r (3);
            return *this;
        }

        /**
         * @brief copyfrom rotaion matrix, same as setRotation.
         * @param rhs [in] the rotation that will be copyed
         */
        Quaternion< T >& operator= (const rw::math::Rotation3D<>& rhs)
        {
            this->setRotation (rhs);
            return (*this);
        }

        // ###################################################
        // #              Comparison Operators               #
        // ###################################################

        /**
         * @brief Comparison (equals) operator
         */
        inline bool operator== (const Quaternion< T >& r) const
        {
            return (*this) (0) == r (0) && (*this) (1) == r (1) && (*this) (2) == r (2) &&
                   (*this) (3) == r (3);
        }

        /**
         * @brief Comparison (not equals) operator
         */
        inline bool operator!= (const Quaternion< T >& r) const { return !((*this) == r); }

#if defined(SWIG)
        TOSTRING ();
#endif
      private:
        Eigen::Quaternion< T > _q;
    };

    /**
       @brief Streaming operator.

       @relates Quaternion
    */
    template< class T > std::ostream& operator<< (std::ostream& out, const Quaternion< T >& v)
    {
        return out << "Quaternion {" << v (0) << ", " << v (1) << ", " << v (2) << ", " << v (3)
                   << "}";
    }

    /**
     * @brief calculates the natural logerithm of this quaternion
     * @param q [in] the quaternion being operated on
     * @return natural logetihm
     */
    template< class T > Quaternion< T > ln (const Quaternion< T >& q) { return q.ln (); }

    /**
     * @brief this will return the exponential of this quaternion \f$ e^Quaternion \f$
     * @param q [in] the quaternion being operated on
     * @return the exponential of this quaternion
     */
    template< class T > Quaternion< T > exp (const Quaternion< T >& q) { return q.exp (); }

    /**
     * @brief Calculate the inverse Quaternion
     * @param q [in] the quaternion being operated on
     * @return the inverse quaternion
     */
    template< class T > Quaternion< T > inverse (const Quaternion< T >& q) { return q.inverse (); }

    /**
     * @brief calculates the quaternion lifted to the power of \b power
     * @param q [in] the quaternion being operated on
     * @param power [in] the power the quaternion is lifted to
     * @return \f$ Quaternion^power \f$
     */
    template< class T > Quaternion< T > pow (const Quaternion< T >& q, double power)
    {
        return q.pow (power);
    }

    /**
     * @brief Casts Quaternion<T> to Quaternion<Q>
     * @param quaternion [in] Quarternion with type T
     * @return Quaternion with type Q
     */
    template< class Q, class T >
    inline const Quaternion< Q > cast (const Quaternion< T >& quaternion)
    {
        return Quaternion< Q > (static_cast< Q > (quaternion (0)),
                                static_cast< Q > (quaternion (1)),
                                static_cast< Q > (quaternion (2)),
                                static_cast< Q > (quaternion (3)));
    }
#if !defined(SWIG)
    extern template class rw::math::Quaternion< double >;
    extern template class rw::math::Quaternion< float >;
#else

#if SWIG_VERSION < 0x040000
    SWIG_DECLARE_TEMPLATE (Quaternion_d, rw::math::Quaternion< double >);
    ADD_DEFINITION (Quaternion_d, Quaternion)
#else
    SWIG_DECLARE_TEMPLATE (Quaternion, rw::math::Quaternion< double >);
#endif
    SWIG_DECLARE_TEMPLATE (Quaternion_f, rw::math::Quaternion< float >);
#endif
    using Quaterniond = Quaternion< double >;
    using Quaternionf = Quaternion< float >;

    /*@}*/
}}    // namespace rw::math

namespace rw { namespace common {
    class OutputArchive;
    class InputArchive;
    namespace serialization {
        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Quaternion
         */
        template<>
        void write (const rw::math::Quaternion< double >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Quaternion
         */
        template<>
        void write (const rw::math::Quaternion< float >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Quaternion
         */
        template<>
        void read (rw::math::Quaternion< double >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Quaternion
         */
        template<>
        void read (rw::math::Quaternion< float >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);
    }    // namespace serialization
}}       // namespace rw::common

#endif    // end include guard


#include <vector>
#include <Eigen/Dense>
#include "../simple_lib/include/simple_activation.h"
#include <matplotlibcpp.h>

namespace plt = matplotlibcpp;
using namespace Eigen;

int main(){
    using std::vector;

    int n = 201;
    VectorXd x = VectorXd::LinSpaced(n, -10, 10);
    VectorXd y;

    y = x.unaryExpr([](double p){return MyDL::relu<double>(p);});

    // plotのために、STLコンテナに詰め替え
    vector<double> xs(n), ys(n);
    Map<VectorXd>(&xs[0], n) = x;
    Map<VectorXd>(&ys[0], n) = y;

    plt::named_plot("ReLU", xs, ys, "--b");
    plt::grid(true);
    plt::legend();
    plt::save("ch3/images/relu.png");

    return 0;
}