/*

Software License Agreement (BSD License)

Copyright (c) 2016--, Liana Bertoni (liana.bertoni@gmail.com)
  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(s) 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.
Contact GitHub API Training Shop Blog About
*/

#include <ros/ros.h>
#include <ros/package.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>

#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>

#include <boost/scoped_ptr.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>

#include <kdl_parser/kdl_parser.hpp>
#include <urdf/model.h>

#include <iostream>
#include <fstream>
#include <sstream>
#include <cstdlib>
#include <string>

#include <math.h>
#include <stdio.h>
#include <ctime>
#include <time.h>

#include "quality.h"
#include "PGR_5.h"
#include "normal_component_box_surface.h"

using namespace std;
using namespace Eigen;
using namespace KDL;
const double PPI_  = 3.141592653589793238463;

const double PI_  = 3.141592653589793238463;

//////////////////////////////////	BOX

std::vector<double> box;
//////////////////////////////////////////////

/////////////////////////////////// CONTACT POINTS

std::vector<double> contact_points;
///////////////////////////////////////

////////////////////////////////////  CONTACT FORCE

std::vector<double> contact_forces;
/////////////////////////////////////////////

////////////////////////////////////// JOINTS

Eigen::VectorXd joints(10);
////////////////////////////////////////////

int n_c = 0;
int n_q = 0;
double quality_i = 0;
double contact_stiffness = 100;
double joint_stiffness = 1;
double mu = 1.5;
double f_i_max = 200;

//////////////////////////////////////////// ROBOT SCTRUCTURE

KDL::Tree hand_tree("RootName");

	KDL::Chain chain_palm;
	KDL::Chain chain_finger_right;
	KDL::Chain chain_finger_left;

	KDL::Chain chain_palm_finger_left;
	KDL::Chain chain_palm_finger_right;

  KDL::JntArray jointpositions_right = JntArray(8);
  KDL::JntArray jointpositions_left = JntArray(8);

	KDL::Jacobian jacobian_right;
	KDL::Jacobian jacobian_left;

	boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_right;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_left;

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

//------------------------------------------------------------------------------------------
void createStructureKDL()
{
  	chain_palm.addSegment(Segment("palm_trasl_x_link",Joint(Joint::TransX),Frame(Vector(0.0,0.0,0.0))));
   	chain_palm.addSegment(Segment("palm_trasl_y_link",Joint(Joint::TransY),Frame(Vector(0.0,0.0,0.0))));
	  chain_palm.addSegment(Segment("palm_trasl_z_link",Joint(Joint::TransZ),Frame(Vector(0.0,0.0,0.0))));
   	chain_palm.addSegment(Segment("palm_rot_x_link",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.0))));
   	chain_palm.addSegment(Segment("palm_rot_y_link",Joint(Joint::RotY),Frame(Vector(0.0,0.0,0.0))));
   	chain_palm.addSegment(Segment("palm_rot_z_link",Joint(Joint::RotZ),Frame(Vector(0.0,0.0,0.0))));

	chain_finger_right.addSegment(Segment("finger_right_proximal_link",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.03))));
	chain_finger_right.addSegment(Segment("finger_right_distal_link",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.03))));

	chain_finger_left.addSegment(Segment("finger_left_proximal_link",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.3))));
	chain_finger_left.addSegment(Segment("finger_left_distal_link",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.03))));

	hand_tree.addChain(chain_palm,"RootName");
	hand_tree.addChain(chain_finger_right,"palm_rot_z_link");
	hand_tree.addChain(chain_finger_left, "palm_rot_z_link");

	hand_tree.getChain("RootName", "finger_right_distal_link", chain_palm_finger_right);
	hand_tree.getChain("RootName", "finger_left_distal_link", chain_palm_finger_left);

}
//-------------------------------------------------------------------------------------------

//-------------------------------------------------------------------------------------------
void setJoints()
{
	for(int i=0 ; i < 6 ; i++)
   		jointpositions_right(i) = jointpositions_left(i) = joints[i];

  	jointpositions_right(6) = joints[6];
  	jointpositions_right(7) = joints[7];

  	jointpositions_left(6) = joints[8];
  	jointpositions_left(7) = joints[9];

	jointpositions_right.resize(chain_palm_finger_right.getNrOfJoints());
  jointpositions_left.resize(chain_palm_finger_left.getNrOfJoints());
}

void initJacobian()
{
	  jacobian_right.resize(chain_palm_finger_right.getNrOfJoints());
  	jacobian_left.resize(chain_palm_finger_left.getNrOfJoints());

  	jnt_to_jac_right.reset(new KDL::ChainJntToJacSolver(chain_palm_finger_right));
  	jnt_to_jac_left.reset(new KDL::ChainJntToJacSolver(chain_palm_finger_left));
}

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

	ros::init(argc, argv, "Test_file");
	ros::NodeHandle nh;

  std::vector<double> joints_;

	nh.param<std::vector<double>>("box", box, std::vector<double>{1, 1, 1});
	nh.param<std::vector<double>>("contact_points", contact_points, std::vector<double>{1,1,1,1,1,1});
	nh.param<std::vector<double>>("contact_forces", contact_forces, std::vector<double>{1,1,1,1,1,1});
	nh.param<std::vector<double>>("joints", joints_, std::vector<double>{0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
  nh.param<double>("mu", mu, 0.5);
  nh.param<double>("f_i_max", f_i_max, 1);
	nh.param<int>("n_c", n_c, 0);
	nh.param<int>("n_q", n_q, 0);
cout << "joints_.size()" << endl << joints_.size() << endl;

  for(int i =0 ; i < joints_.size(); i++)
      joints(i) = joints_[i]*(PI_/180);

	if(n_c <= 0)
		return 0;

	createStructureKDL();
	setJoints();
	initJacobian();

  Eigen::MatrixXd Skew_Matrix1(3,3);
  Eigen::MatrixXd Skew_Matrix2(3,3);

  Skew_Matrix1(0,0) = Skew_Matrix1(1,1) = Skew_Matrix1(2,2) = 0;
      Skew_Matrix1(0,1) = - contact_points[2]; // -rz
      Skew_Matrix1(0,2) = contact_points[1];   // ry
      Skew_Matrix1(1,0) = contact_points[2];   // rz
      Skew_Matrix1(2,0) = - contact_points[1]; // -ry
      Skew_Matrix1(1,2) = - contact_points[0]; // -rx
      Skew_Matrix1(2,1) = contact_points[0];   // rx

 Skew_Matrix2(0,0) = Skew_Matrix2(1,1) = Skew_Matrix2(2,2) = 0;
      Skew_Matrix2(0,1) = - contact_points[5]; // -rz
      Skew_Matrix2(0,2) = contact_points[4];   // ry
      Skew_Matrix2(1,0) = contact_points[5];   // rz
      Skew_Matrix2(2,0) = - contact_points[4]; // -ry
      Skew_Matrix2(1,2) = - contact_points[3]; // -rx
      Skew_Matrix2(2,1) = contact_points[3];   // rx

Eigen::MatrixXd G  = MatrixXd::Zero(6, 12);
Eigen::MatrixXd G1 = MatrixXd::Zero(6,6);
Eigen::MatrixXd G2 = MatrixXd::Zero(6,6);

Eigen::MatrixXd R1(3,3);
Eigen::MatrixXd R2(3,3);

R1  << 1, 0, 0,
       0, cos(-90*PPI_/180), -sin(-90*PPI_/180),
       0, sin(-90*PPI_/180), cos(-90*PPI_/180);

R2 << 1, 0, 0,
      0, cos(90*PPI_/180), -sin(90*PPI_/180),
      0, sin(90*PPI_/180), cos(90*PPI_/180);

G1.block<3,3>(0,0) = R1.transpose();
G1.block<3,3>(3,3) = R1.transpose();
G1.block<3,3>(3,0) = Skew_Matrix1 * R1.transpose();

G2.block<3,3>(0,0) = R2.transpose();
G2.block<3,3>(3,3) = R2.transpose();
G2.block<3,3>(3,0) = Skew_Matrix2 * R2.transpose();

G.block<6,6>(0,0) = G1;
G.block<6,6>(0,6) = G2;

Eigen::MatrixXd Jacobian = MatrixXd::Zero(6*n_c, n_q);

jnt_to_jac_right->JntToJac(jointpositions_right, jacobian_right);
jnt_to_jac_left->JntToJac(jointpositions_left, jacobian_left);

Jacobian.block<6,8>(0,0) = jacobian_right.data;
Jacobian.block<6,8>(6,0) = jacobian_left.data;

Eigen::MatrixXd R_c = MatrixXd::Zero(3*n_c,3*n_c);
    int k = 0;
    for(int i = 0 ; i < n_c ; i++)
    {
      R_c.block<3,3>(k,k) = MatrixXd::Identity(3,3);
      k += 3;
    }

    Eigen::VectorXd f_c_(contact_forces.size());
    for(int i = 0 ; i < contact_forces.size() ; i++ )
      f_c_(i) = contact_forces[i];

    Eigen::MatrixXd Contact_Stiffness_Matrix = MatrixXd::Zero(3,3);   // Kis
      for(int i = 0 ; i < Contact_Stiffness_Matrix.rows() ; i++) //Kis
          Contact_Stiffness_Matrix(i,i) = contact_stiffness;

    Eigen::MatrixXd Joint_Stiffness_Matrix = MatrixXd::Zero(n_q,n_q);    // Kp
      for(int j = 0 ; j < Joint_Stiffness_Matrix.rows() ; j++) //Kp
          Joint_Stiffness_Matrix(j,j) = joint_stiffness;

  //quality_i = quality_pcr_pgr_5(f_c_, G_c, Jacobian, R_c, Contact_Stiffness_Matrix, Joint_Stiffness_Matrix, mu, f_i_max);
 // quality_i = quality_pcr_pgr_5(f_c_, G_b, Jacobian, R_c, Contact_Stiffness_Matrix, Joint_Stiffness_Matrix, mu, f_i_max);

  quality_i = quality_pcr_pgr_5(f_c_, G, Jacobian, R_c, Contact_Stiffness_Matrix, Joint_Stiffness_Matrix, mu, f_i_max);

  cout << " quality : " << quality_i << endl;

	ros::spinOnce();
	return 0;
}


#include <iostream>
#include <cassert>
#include <string>
#include <fstream>
#include <set>
#include <vector>
#include <map>
#include <random>
#include <chrono>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>

using namespace std;
using namespace boost;

// Typedefs for Input and output graphs
struct vertex_p
{
    int name;
    float color;
    string fillcolor;
};

struct edge_p
{
    double weight;
    unsigned penwidth;
};

typedef adjacency_list <listS, vecS, undirectedS, vertex_p, edge_p, no_property > graph_p;

typedef graph_traits<graph_p>::vertex_descriptor vd_p;
typedef graph_traits<graph_p>::edge_descriptor ed_p;

// Typedefs for intermediate graphs

struct vertex_q
{
    unsigned name;
    unsigned label;
    float color;
};

struct edge_q
{
    double weight;
};

typedef adjacency_list <listS, vecS, undirectedS, vertex_q, edge_q, no_property > graph_q;
typedef graph_traits<graph_q>::vertex_descriptor vd_q;
typedef graph_traits<graph_q>::edge_descriptor ed_q;

// Global Variables
string input_filename, output_filename;
float multicast_fraction = 0.0;
unsigned vertex_count = 0;
set<unsigned> multicast_vertices;

int main(int argc, char* argv[])
{
    if(argc != 4)
    {
        cerr << "Usage: " << argv[0] << " <Multicast Fraction> <Input.dot> <Output.dot>\n";
        return 1;
    }

    try
    {
        multicast_fraction = stof(string(argv[1]));
        if(multicast_fraction < 0 || multicast_fraction > 1)
            throw invalid_argument("Fraction should be > 0 and < 1.");
    }
    catch(std::exception &err)
    {
        cerr << err.what() << endl;
        cerr << "Invalid fraction argument\n";
        return 1;
    }

    input_filename.assign(argv[2]);
    output_filename.assign(argv[3]);

#ifdef DEBUG
    cout << "Running in debug mode\n";
    ifstream input_file("steiner.dot", ios::in);
    ofstream output_file("debug.dot", ios::out);
#else
    ifstream input_file(input_filename.c_str(), ios::in);
    ofstream output_file(output_filename.c_str(), ios::out);
#endif

    assert(output_file.is_open() && input_file.is_open() && "Error opening files");

    graph_p network(0);
    dynamic_properties dp;
    auto name = get(&vertex_p::name, network);
    dp.property("node_id", name);
    auto fillcolor = get(&vertex_p::fillcolor, network);
    dp.property("fillcolor", fillcolor);
    auto weight = get(&edge_p::weight, network);
    dp.property("weight", weight);
    auto penwidth = get(&edge_p::penwidth, network);
    dp.property("penwidth", penwidth);

    try
    {
        read_graphviz(input_file, network, dp, "node_id");
    }
    catch(std::exception &err)
    {
        cerr << err.what() << endl;
        cerr << "read_graphviz failed for " << input_filename << "\n";
        return 1;
    }

    // Initialize all dynamic properties

    BGL_FORALL_VERTICES(v, network, graph_p)
    {
        fillcolor[v] = "white";
    }

    BGL_FORALL_EDGES(e, network, graph_p)
    {
        penwidth[e] = 1;
    }

    // TODO: Add timing code

    vertex_count = num_vertices(network);
    unsigned multicast_count = unsigned(multicast_fraction * vertex_count);

#ifndef DEBUG
    // Assume that vertices are numbered 0 -- N-1 (generated by BRITE)
    // Randomly select multicast nodes using uniform distribution

    std::default_random_engine generator;
    std::uniform_int_distribution<unsigned> distribution(0, multicast_count - 1);

    multicast_count = multicast_count == 0 ? 1 : multicast_count;

    do
    {
        // Insert vd_p Names ( == vd_p descriptors )
        unsigned value = distribution(generator) % multicast_count;
        multicast_vertices.insert(value);
    } while( multicast_vertices.size() < multicast_count);

#else
    // debug for steiner.dot
    multicast_count = 4;
    cout << "Num vd_p: " << vertex_count << endl;
    cout << "Num MCast: " << multicast_count << endl;
    multicast_vertices.insert(0);
    multicast_vertices.insert(1);
    multicast_vertices.insert(2);
    multicast_vertices.insert(3);

    // Debug
    ostream dbg(std::cout.rdbuf());
    write_graphviz_dp(dbg, network, dp);
#endif

    graph_q G1(0);
    map<unsigned, vd_q> vd_p_to_vd_q_map;

    for(auto &V : multicast_vertices)
    {
        vd_q Vx = add_vertex(G1);
        //cout << Vx << " " << V << "\n";
        G1[Vx].label = V;
        G1[Vx].name = Vx;
        vd_p_to_vd_q_map.insert(make_pair(V,Vx));
    }

#ifdef DEBUG
    // Debug
    dynamic_properties dp_q;
    auto name_q = get(&vertex_q::name, G1);
    dp_q.property("node_id", name_q);
    auto label_q = get(&vertex_q::label, G1);
    dp_q.property("label", label_q);
    auto weight_q = get(&edge_q::weight, G1);
    dp_q.property("weight", weight_q);
    write_graphviz_dp(dbg, G1, dp_q);
    cout << "Step1-----------------------------------\n";
#endif

    map<vd_p, vector<vd_p> > AllPreds;
    map<vd_p, vector<unsigned> > AllDistances;

    auto start_step_1 = chrono::steady_clock::now();

    // Step 1 -- Construct undirected distance graph G1, G and S.
    for(auto V = multicast_vertices.begin(), E = multicast_vertices.end(); V != E; V++)
    {
        vd_p Vx = *V;
        // Get the shortest path between Vx,All
        vector<unsigned> D(vertex_count);

        AllPreds.insert(pair<vd_p,vector<vd_p> >(Vx, vector<vd_p>(vertex_count)));
        AllDistances.insert(pair<vd_p,vector<unsigned> >(Vx, vector<unsigned>(vertex_count)));

        dijkstra_shortest_paths(network, Vx,
                weight_map(get(&edge_p::weight,network))
                .distance_map(make_iterator_property_map(AllDistances[Vx].begin(), get(vertex_index, network)))
                .predecessor_map(make_iterator_property_map(AllPreds[Vx].begin(), get(vertex_index, network))));

        for(auto W = std::next(V); W != E; W++)
        {
            vd_p Wx = *W;
            //// Debug
            //cout << "Adding ed_p (Vx, Wx, D[Wx]): " << Vx << "," << Wx << "," << D[Wx] << endl;
            ed_q e; bool found;
            tie(e,found) = add_edge(vd_p_to_vd_q_map[*V], vd_p_to_vd_q_map[*W], G1);
            G1[e].weight = AllDistances[Vx][Wx];
        }
    }

    auto end_step_1 = chrono::steady_clock::now();
    cout << "[KMB] Step1: "<< chrono::duration <double, milli> (end_step_1-start_step_1).count() << " ms" << endl;

#ifdef DEBUG
    // Debug
    write_graphviz_dp(dbg, G1, dp_q);
    cout << "Step2-----------------------------------\n";
#endif

    // Step 2a -- Construct Minimum Spanning Tree from G1

    auto start_step_2 = chrono::steady_clock::now();

    vector<ed_q> st;
    kruskal_minimum_spanning_tree(G1, back_inserter(st),
            weight_map(get(&edge_q::weight, G1)));

    // Step 2b -- Trim G1 based on MST

    set<vd_q> vKeep, vRemove;
    set<ed_q> eKeep, eRemove;

    for(auto &e : st)
    {
        vKeep.insert(source(e, G1)); vKeep.insert(target(e, G1));
        eKeep.insert(e);
    }

    auto end_step_2 = chrono::steady_clock::now();
    cout << "[KMB] Step2: "<< chrono::duration <double, milli> (end_step_2-start_step_2).count() << " ms" << endl;

#ifdef DEBUG
    BGL_FORALL_VERTICES(v, G1, graph_q)
    {
        if(vKeep.count(v) == 0)
            vRemove.insert(v);
    }

    BGL_FORALL_EDGES(e, G1, graph_q)
    {
        if(eKeep.count(e) == 0)
            eRemove.insert(e);
    }

    for(auto Ex : eRemove) remove_edge(Ex, G1);
    for(auto Vx : vRemove) remove_vertex(Vx, G1);

    write_graphviz_dp(dbg, G1, dp_q);
    cout << "Step3-----------------------------------\n";
#endif

    auto start_step_3 = chrono::steady_clock::now();
    // Step 3 -- Add the shortest paths back to G1 (expand each edge)

    graph_q G2(0);

    // For each edge in the MST, get the path we saved earlier
    // For each vertex in path, if vertex is not added, add vertex, add edge
    // else check if edge is added else add edge. Set weight

    set<unsigned> added_labels;
    map<unsigned, vd_q> label_map;

    for(auto &Ex : eKeep)
    {

#ifdef DEBUG
        cout << "Source: " << G1[source(Ex,G1)].label << "\n";
        cout << "Target: " << G1[target(Ex,G1)].label << "\n";
#endif

        // Traverse from sink to source

        auto predecessor_map = AllPreds[G1[source(Ex,G1)].label];
        vd_p v = G1[target(Ex, G1)].label;
        for(vd_p u = predecessor_map[v];          // Start by setting 'u' to the destintaion node's predecessor
                u != v;                          // Keep tracking the path until we get to the source
                v = u, u = predecessor_map[v])   // Set the current vertex to the current predecessor, and the predecessor to one level up
        {
            vd_q Vx, Ux;
            if(added_labels.count(v) == 0)
            {
                Vx = add_vertex(G2);
                G2[Vx].label = v;
                G2[Vx].name = Vx;
                added_labels.insert(v);
                label_map.insert(make_pair(v,Vx));
            }
            else
            {
                Vx = label_map[v];
            }

            if(added_labels.count(u) == 0)
            {
                Ux = add_vertex(G2);
                G2[Ux].label = u;
                G2[Ux].name = Ux;
                added_labels.insert(u);
                label_map.insert(make_pair(u,Ux));
            }
            else
            {
                Ux = label_map[u];
            }

            ed_q eq; bool fq = false;
            tie(eq, fq) = edge(Vx, Ux, G2);
            if(!fq)
            {
                ed_p e; bool f = false;
                tie(e, f) = edge(u, v, network);
                assert(f && "Could not find edge in original graph!");
                double wt = network[e].weight;

                tie(eq, fq) = add_edge(Vx, Ux, G2);
                G2[eq].weight = wt;
            }
        }
    }

    auto end_step_3 = chrono::steady_clock::now();
    cout << "[KMB] Step3: "<< chrono::duration <double, milli> (end_step_3-start_step_3).count() << " ms" << endl;

#ifdef DEBUG
    dynamic_properties dp_q2;
    auto name_q2 = get(&vertex_q::name, G2);
    dp_q2.property("node_id", name_q2);
    auto label_q2 = get(&vertex_q::label, G2);
    dp_q2.property("label", label_q2);
    auto weight_q2 = get(&edge_q::weight, G2);
    dp_q2.property("weight", weight_q2);

    write_graphviz_dp(dbg, G2, dp_q2);

    cout << "Step4-----------------------------------\n";
#endif

    auto start_step_4 = chrono::steady_clock::now();
    // Step 4a -- Construct Minimum Spanning Tree from G2

    vector<ed_q> st2;
    kruskal_minimum_spanning_tree(G2, back_inserter(st2),
            weight_map(get(&edge_q::weight, G2)));

    // Step 4b -- Trim G2 based on MST

    set<vd_q> vKeep2, vRemove2;
    set<ed_q> eKeep2, eRemove2;

    for(auto &e : st2)
    {
        vKeep2.insert(source(e, G2)); vKeep2.insert(target(e, G2));
        eKeep2.insert(e);
    }

    BGL_FORALL_VERTICES(v, G2, graph_q)
    {
        if(vKeep2.count(v) == 0)
            vRemove2.insert(v);
    }

    BGL_FORALL_EDGES(e, G2, graph_q)
    {
        if(eKeep2.count(e) == 0)
            eRemove2.insert(e);
    }

    for(auto Ex : eRemove2) remove_edge(Ex, G2);
    for(auto Vx : vRemove2) remove_vertex(Vx, G2);

    auto end_step_4 = chrono::steady_clock::now();
    cout << "[KMB] Step4: "<< chrono::duration <double, milli> (end_step_4-start_step_4).count() << " ms" << endl;

#ifdef DEBUG
    write_graphviz_dp(dbg, G2, dp_q2);
    cout << "Step5-----------------------------------\n";
#endif

    auto start_step_5 = chrono::steady_clock::now();

    // Remove non-steiner leaf nodes
    // Possible optimization : Start from leaf node and delete upwards instead
    // of iterating over and over the graph and examining each time.

    set<vd_q> to_remove;
    do
    {
        for(auto v : to_remove)
        {
            clear_vertex(v, G2);
            remove_vertex(v, G2);
        }

        to_remove.clear();

        BGL_FORALL_VERTICES(v, G2, graph_q)
        {
            if(multicast_vertices.count(G2[v].label) == 0 && degree(v, G2) == 1)
            {
                //cout << "Removing: " << v << " " << G2[v].label << "\n";
                to_remove.insert(v);
            }
        }
    } while(to_remove.size() > 0);

    auto end_step_5 = chrono::steady_clock::now();
    cout << "[KMB] Step5: "<< chrono::duration <double, milli> (end_step_5-start_step_5).count() << " ms" << endl;

#ifdef DEBUG
    write_graphviz_dp(dbg, G2, dp_q2);
    cout << "Step6-----------------------------------\n";
#endif

    // Update the fillcolor and penwidth in the original graph and print it out.

    for(auto &m : multicast_vertices)
    {
        network[m].fillcolor = string("black");
    }

    BGL_FORALL_EDGES(ed, G2, graph_q)
    {
        vd_p vx = G2[source(ed, G2)].label;
        vd_p vy = G2[target(ed, G2)].label;

        //cout << vx << " " << vy << "\n";
        ed_p e; bool f = false;
        tie(e,f) = edge(vx, vy, network);
        assert(f && "ed_p not found in original graph!");
        network[e].penwidth = 5;
    }

    try
    {
        write_graphviz_dp(output_file, network, dp);
    }
    catch(std::exception &err)
    {
        cerr << err.what() << endl;
        cerr << "write_graphviz failed for " << output_filename << "\n";
        return 1;
    }

    return 0;
}


//
// Copyright (c) 2019 Vinnie Falco (vinnie.falco@gmail.com)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Official repository: https://github.com/cppalliance/json
//

#include <boost/json/number_cast.hpp>
#include <boost/json/parse.hpp>
#include <boost/json/value.hpp>

#include "test_suite.hpp"

#ifdef _MSC_VER
# pragma warning(push)
# pragma warning(disable: 4101)
# pragma warning(disable: 4189) // local variable is initialized but not referenced
#elif defined(__clang__)
# pragma clang diagnostic push
# pragma clang diagnostic ignored "-Wunused"
# pragma clang diagnostic ignored "-Wunused-variable"
#elif defined(__GNUC__)
# pragma GCC diagnostic push
# pragma GCC diagnostic ignored "-Wunused"
# pragma GCC diagnostic ignored "-Wunused-variable"
#endif

BOOST_JSON_NS_BEGIN

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

static void set1() {

//----------------------------------------------------------
{
//[doc_using_numbers_1
// construction from int
value jv1 = 1;

assert( jv1.is_int64() );

// construction from unsigned int
value jv2 = 2u;

assert( jv2.is_uint64() );

// construction from double
value jv3 = 3.0;

assert( jv3.is_double() );
//]
}
//----------------------------------------------------------
{
//[doc_using_numbers_2
struct convert_int64
{
    value jv;

    operator int() const
    {
        return number_cast< int >( this->jv );
    }
};
//]
}
//----------------------------------------------------------
{
//[doc_using_numbers_3
try
{
    value jv1 = 404;

    assert( jv1.is_int64() );

    // ok, identity conversion
    std::int64_t r1 = number_cast< std::int64_t >( jv1 );

    // loss of data, throws system_error
    char r2 = number_cast< char >( jv1 );

    // ok, no loss of data
    double r3 = number_cast< double >( jv1 );

    value jv2 = 1.23;

    assert( jv1.is_double() );

    // ok, same as static_cast<float>( jv2.get_double() )
    float r4 = number_cast< float >( jv2 );

    // not exact, throws system_error
    int r5 = number_cast< int >( jv2 );

    value jv3 = {1, 2, 3};

    assert( ! jv3.is_number() );

    // not a number, throws system_error
    int r6 = number_cast< int >( jv3 );
}
catch(...)
{
}
//]
}
//----------------------------------------------------------
{
//[doc_using_numbers_4
value jv = parse("[-42, 100, 10.25, -299999999999999999998, 2e32]");

array ja = jv.as_array();

// represented by std::int64_t
assert( ja[0].is_int64() );

// represented by std::int64_t
assert( ja[1].is_int64() );

// contains decimal point, represented as double
assert( ja[2].is_double() );

// less than INT64_MIN, represented as double
assert( ja[3].is_double() );

// contains exponent, represented as double
assert( ja[4].is_double() );
//]
}
//----------------------------------------------------------
{
//[doc_using_numbers_5
//]
}
//----------------------------------------------------------

} // set1

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

class doc_using_numbers_test
{
public:
    void
    run()
    {
        (void)&set1;
    }
};

TEST_SUITE(doc_using_numbers_test, "boost.json.doc_using_numbers");

BOOST_JSON_NS_END


#pragma once
#include <trajopt_utils/macros.h>
#include <trajopt_utils/basic_array.hpp>
TRAJOPT_IGNORE_WARNINGS_PUSH
#include <Eigen/Core>
#include <map>
#include <vector>
TRAJOPT_IGNORE_WARNINGS_POP

#include <trajopt_sco/modeling.hpp>
#include <trajopt_sco/modeling_utils.hpp>

namespace trajopt
{
typedef util::BasicArray<sco::Var> VarArray;
typedef util::BasicArray<sco::AffExpr> AffArray;
typedef util::BasicArray<sco::Cnt> CntArray;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> DblMatrix;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> TrajArray;
typedef sco::DblVec DblVec;
typedef sco::IntVec IntVec;

/**  @brief Adds plotting to the CostFromErrFunc class in trajopt_sco */
class TrajOptCostFromErrFunc : public sco::CostFromErrFunc
{
public:
  /// supply error function, obtain derivative numerically
  TrajOptCostFromErrFunc(sco::VectorOfVectorPtr f, const sco::VarVector& vars, const Eigen::VectorXd& coeffs,
                         sco::PenaltyType pen_type, const std::string& name)
    : CostFromErrFunc(f, vars, coeffs, pen_type, name)
  {
  }

  /// supply error function and gradient
  TrajOptCostFromErrFunc(sco::VectorOfVectorPtr f, sco::MatrixOfVectorPtr dfdx, const sco::VarVector& vars,
                         const Eigen::VectorXd& coeffs, sco::PenaltyType pen_type, const std::string& name)
    : CostFromErrFunc(f, dfdx, vars, coeffs, pen_type, name)
  {
  }
};

/**  @brief Adds plotting to the ConstraintFromFunc class in trajopt_sco */
class TrajOptConstraintFromErrFunc : public sco::ConstraintFromErrFunc
{
public:
  /// supply error function, obtain derivative numerically
  TrajOptConstraintFromErrFunc(sco::VectorOfVectorPtr f, const sco::VarVector& vars, const Eigen::VectorXd& coeffs,
                               sco::ConstraintType type, const std::string& name)
    : ConstraintFromErrFunc(f, vars, coeffs, type, name)
  {
  }

  /// supply error function and gradient
  TrajOptConstraintFromErrFunc(sco::VectorOfVectorPtr f, sco::MatrixOfVectorPtr dfdx, const sco::VarVector& vars,
                               const Eigen::VectorXd& coeffs, sco::ConstraintType type, const std::string& name)
    : ConstraintFromErrFunc(f, dfdx, vars, coeffs, type, name)
  {
  }
};
}


#ifndef HGaussianProcessKernelBaseClass
#define HGaussianProcessKernelBaseClass

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

//! Interface for Gaussian kernel defintion
class GaussianProcessKernelBaseClass {
  public:
    //! Virtual member function for the estimation of hyper parameters of the kernel
    virtual void estimate_hyper_parameters ( std::vector< std::vector<double> > const&,
                                             std::vector<double> const&,
                                             std::vector<double> const& ) = 0;
    //! Virtual membber function for the evaluation of the kernel
    virtual double evaluate_kernel ( std::vector<double> const&,
                                     std::vector<double> const& ) = 0;
};

#endif


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/ext/boost/fusion.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/integral.hpp>

#include "../helper.hpp"
using namespace boost::hana;

int main() {
    with_nonassociative_forward_sequences([](auto container) {
        BOOST_HANA_CONSTANT_ASSERT(length(container()) == size_t<0>);
        BOOST_HANA_CONSTANT_ASSERT(length(container(1)) == size_t<1>);
        BOOST_HANA_CONSTANT_ASSERT(length(container(1, '2')) == size_t<2>);
        BOOST_HANA_CONSTANT_ASSERT(length(container(1, '2', 3.3)) == size_t<3>);
    });
}