/*
(c) 2012 Fengtao Fan
*/
#include "SimpleGraph.h"
#include "SimpleMesh.h"
#include "canonical_loops.h"
#include "edge_annotations_gauss.h"
#include "CycleOptimization/Annotation/AnnotationComputation.h"
#include "psbmReebGraph.h"

#include <boost/progress.hpp>

#include <fstream>
#include <sstream>

#include <map>

//_simpGraph_vec meshGraph;
//_SimpleMesh cycMesh;
//std::vector<Vector3>   meshNormal;
void LoadCycleData(psbmReebGraph &reebGraph, std::vector<std::vector<int> > &basis_h_loops,
                   std::vector<std::vector<int> > &basis_v_loops, std::vector<int> &base_pt_vec) {
    //
    std::vector<bool> initVerticalLoopsType;
    // set the flag bit for handle or tunnel loops
    for (unsigned int i = 0; i < reebGraph.initVerticalLoops->size(); i++) {
        if ((*reebGraph.initVerticalLoops)[i].pathType) {// this is a vertical loop
            initVerticalLoopsType.push_back(true);
        } else {
            initVerticalLoopsType.push_back(false);
        }
    }
    // Store the critical points
    base_pt_vec.reserve(2 * reebGraph.criticalPairing->size());
    for (unsigned int j = 0; j < reebGraph.criticalPairing->size(); j++) {
        base_pt_vec.push_back((*reebGraph.criticalPairing)[j].first);
        base_pt_vec.push_back((*reebGraph.criticalPairing)[j].second);
    }
    //
    const int tot_genus = (int) reebGraph.vecVerticalLoopEdgePathOnMesh_Vertex.size();
    basis_v_loops.reserve(tot_genus);
    basis_h_loops.reserve(tot_genus);
    // store v-basis
    for (int i = 0; i < tot_genus; i++) {
        std::vector<int> tempIntVec;
        int edges_num = 0;
        int row_num = i;
        //
        if (!initVerticalLoopsType[i])
            row_num += tot_genus;
        //
        for (std::list<std::pair<int, int> >::iterator listIter = (reebGraph.invLinkNumbersMatrix)[row_num].begin();
             listIter != (reebGraph.invLinkNumbersMatrix)[row_num].end(); listIter++) {
            int col_num = listIter->first;
            int counter = abs(listIter->second);
            if (col_num >= tot_genus) {// it is a horizontal loop
                edges_num +=
                        counter * (int) (reebGraph.vecHorizontalLoopEdgePathOnMesh_Edge)[col_num - tot_genus].size();
            } else {
                edges_num += counter * (int) (reebGraph.vecVerticalLoopEdgePathOnMesh_Edge)[col_num].size();
            }
        }
        //
        //sstr << edges_num << std::endl;
        tempIntVec.reserve(edges_num + 2);
        for (std::list<std::pair<int, int> >::iterator listIter = (reebGraph.invLinkNumbersMatrix)[row_num].begin();
             listIter != (reebGraph.invLinkNumbersMatrix)[row_num].end(); listIter++) {
            int col_num = listIter->first;
            int counter = abs(listIter->second);
            if (col_num >= tot_genus) {// it is a horizontal loop
                for (int j = 0; j < counter; j++) {
                    for (std::vector<int>::iterator vIter = (reebGraph.vecHorizontalLoopEdgePathOnMesh_Edge)[col_num -
                                                                                tot_genus].begin();
                         vIter !=
                         (reebGraph.vecHorizontalLoopEdgePathOnMesh_Edge)[col_num - tot_genus].end(); vIter++) {
                        //sstr << *vIter << " ";
                        tempIntVec.push_back(*vIter);
                    }
                }
            } else {
                for (int j = 0; j < counter; j++) {
                    for (std::vector<int>::iterator vIter = (reebGraph.vecVerticalLoopEdgePathOnMesh_Edge)[col_num].begin();
                         vIter != (reebGraph.vecVerticalLoopEdgePathOnMesh_Edge)[col_num].end(); vIter++) {
                        //sstr << *vIter << " ";
                        tempIntVec.push_back(*vIter);
                    }
                }
            }
        }
        //
        basis_v_loops.push_back(tempIntVec);
        //sstr << std::endl;
    }
    //store h-basis
    for (int i = 0; i < tot_genus; i++) {
        std::vector<int> hTempIntVec;
        int edges_num = 0;
        int row_num = i;
        //
        if (initVerticalLoopsType[i])
            row_num += tot_genus;
        //
        for (std::list<std::pair<int, int> >::iterator listIter = (reebGraph.invLinkNumbersMatrix)[row_num].begin();
             listIter != (reebGraph.invLinkNumbersMatrix)[row_num].end(); listIter++) {
            int col_num = listIter->first;
            int counter = abs(listIter->second);
            if (col_num >= tot_genus) {// it is a horizontal loop
                edges_num +=
                        counter * (int) (reebGraph.vecHorizontalLoopEdgePathOnMesh_Edge)[col_num - tot_genus].size();
            } else {
                edges_num += counter * (int) (reebGraph.vecVerticalLoopEdgePathOnMesh_Edge)[col_num].size();
            }
        }
        //
        //sstr << edges_num << std::endl;
        hTempIntVec.reserve(edges_num + 2);
        for (std::list<std::pair<int, int> >::iterator listIter = (reebGraph.invLinkNumbersMatrix)[row_num].begin();
             listIter != (reebGraph.invLinkNumbersMatrix)[row_num].end(); listIter++) {
            int col_num = listIter->first;
            int counter = abs(listIter->second);
            if (col_num >= tot_genus) {// it is a horizontal loop
                for (int j = 0; j < counter; j++) {
                    for (std::vector<int>::iterator vIter = (reebGraph.vecHorizontalLoopEdgePathOnMesh_Edge)[col_num -
                                                                                tot_genus].begin();
                         vIter !=
                         (reebGraph.vecHorizontalLoopEdgePathOnMesh_Edge)[col_num - tot_genus].end(); vIter++) {
                        //sstr << *vIter << " ";
                        hTempIntVec.push_back(*vIter);
                    }
                }
            } else {
                for (int j = 0; j < counter; j++) {
                    for (std::vector<int>::iterator vIter = (reebGraph.vecVerticalLoopEdgePathOnMesh_Edge)[col_num].begin();
                         vIter != (reebGraph.vecVerticalLoopEdgePathOnMesh_Edge)[col_num].end(); vIter++) {
                        //sstr << *vIter << " ";
                        hTempIntVec.push_back(*vIter);
                    }
                }
            }
        }
        //sstr << std::endl;
        basis_h_loops.push_back(hTempIntVec);
    }
    return;
}

void AssembleMeshGraph(_simpGraph_vec &meshGraph, _SimpleMesh &mesh, std::vector<float> &sqEdgeLength,
                       std::set<int> &extraVertices, const float fScaleRatio) {
    meshGraph.InitNodes((int) mesh.vecVertex.size());
    //
    std::vector<bool> vertexFlag(mesh.vecVertex.size(), false);
    //
    for (std::set<int>::iterator sIter = extraVertices.begin();
         sIter != extraVertices.end(); sIter++) {
        vertexFlag[*sIter] = true;
    }
    //
    std::vector<int> nonExistEdges;
    //
    float totEdgeLen = 0.0;
    for (unsigned int eid = 0; eid < mesh.vecEdge.size(); eid++) {
        int v_a = mesh.vecEdge[eid].v0;
        int v_b = mesh.vecEdge[eid].v1;

        Vector3 p_a(mesh.vecVertex[v_a].x, mesh.vecVertex[v_a].y, mesh.vecVertex[v_a].z);
        Vector3 p_b(mesh.vecVertex[v_b].x, mesh.vecVertex[v_b].y, mesh.vecVertex[v_b].z);

        //
        sqEdgeLength.push_back(norm(p_a - p_b) * fScaleRatio);
        //

        meshGraph.AddEdge(v_a, v_b, eid);
        //
        totEdgeLen += sqEdgeLength.back();
        //
        if (vertexFlag[v_a] || vertexFlag[v_b]) {
            nonExistEdges.push_back(eid);
        }
    }
    //
    for (unsigned int i = 0; i < nonExistEdges.size(); i++) {
        //
        sqEdgeLength[nonExistEdges[i]] = totEdgeLen;// std::numeric_limits<float>::max(); // totEdgeLen* 2;
    }
    return;
}

void AssembleMeshGraph(_simpGraph_vec &meshGraph, _SimpleMesh &mesh, std::vector<float> &sqEdgeLength,
                       const float fScaleRatio) {
    meshGraph.InitNodes((int) mesh.vecVertex.size());
    //
    for (unsigned int eid = 0; eid < mesh.vecEdge.size(); eid++) {
        int v_a = mesh.vecEdge[eid].v0;
        int v_b = mesh.vecEdge[eid].v1;

        Vector3 p_a(mesh.vecVertex[v_a].x, mesh.vecVertex[v_a].y, mesh.vecVertex[v_a].z);
        Vector3 p_b(mesh.vecVertex[v_b].x, mesh.vecVertex[v_b].y, mesh.vecVertex[v_b].z);

        //
        sqEdgeLength.push_back(norm(p_a - p_b) * fScaleRatio);
        //

        meshGraph.AddEdge(v_a, v_b, eid);
    }
}

float BasisWeight(std::vector<std::set<int> > &basis, std::vector<float> sqEdgeLen) {
    float tot_len = 0.0;
    for (unsigned int i = 0; i < basis.size(); i++) {
        for (std::set<int>::iterator sIter = basis[i].begin();
             sIter != basis[i].end(); sIter++) {
            tot_len += sqEdgeLen[*sIter];
        }
    }
    return tot_len;
}

float BasisWeight(std::vector<std::vector<int> > &basis, std::vector<float> sqEdgeLen) {
    float tot_len = 0.0;
    for (unsigned int i = 0; i < basis.size(); i++) {
        for (std::vector<int>::iterator vIter = basis[i].begin();
             vIter != basis[i].end(); vIter++) {
            tot_len += sqEdgeLen[*vIter];
        }
    }
    return tot_len;
}

void CycleLocalOptimization_bdry(_SimpleMesh &locMesh, psbmReebGraph &reebgraph, std::vector<int> &OrientTriangles,
                                 std::vector<std::set<int> > &out_v_basis_loops,
                                 std::vector<std::set<int> > &out_h_basis_loops,
                                 std::set<int> extraVertices,
                                 const float fScaleRatio) {//
    _simpGraph_vec locMeshGraph;
    //
    edge_annotation_computing edge_anno_proxy;
    //
    //
    ComputeAnnotation(locMesh, edge_anno_proxy.edge_annotations, OrientTriangles);
    edge_anno_proxy.vec_size = (int) edge_anno_proxy.edge_annotations[0].size();
    //
    if (edge_anno_proxy.vec_size == 0) {
        std::cout << "TRIVIAL LOOPS" << std::endl;
        exit(0);
    }
    std::vector<float> sqEdgeLen;
    AssembleMeshGraph(locMeshGraph, locMesh, sqEdgeLen, extraVertices, fScaleRatio);
    //
    edge_anno_proxy.SetMeshPtr(&locMesh);
    //edge_anno_proxy.ReadEdgeAnnotationsFromFile(argv[2]);
//{//
//	//
//	edge_annotation_computing edge_anno_proxy;
//	//
//	//
//	LoadData(argv[1], edge_anno_proxy);
//	//
//	std::set<int> extraVertices;
//	ReadBoundaries(argv[argc - 1], extraVertices);
//	//
//	std::vector<float> sqEdgeLen;
//	AssembleMeshGraph(meshGraph, cycMesh, sqEdgeLen, extraVertices);
//	//
//	edge_anno_proxy.SetMeshPtr(&cycMesh);
//	//edge_anno_proxy.ReadEdgeAnnotationsFromFile(argv[2]);
    //
    std::vector<std::vector<int> > basis_h_loops;
    std::vector<std::vector<int> > basis_v_loops;
    //
    std::vector<Annotation_Type> basis_h_annotations;
    std::vector<Annotation_Type> basis_v_annotations;
    //
    std::vector<int> base_pt_vec;
    //
    LoadCycleData(reebgraph, basis_h_loops, basis_v_loops, base_pt_vec);
    //ReadBasisLoops(argv[4], basis_h_loops);
    //ReadBasisLoops(argv[5], basis_v_loops);

    edge_anno_proxy.CheckTwoGroupVectorOrthogonality(basis_v_loops, basis_v_annotations, basis_h_loops,
                                                     basis_h_annotations);
    //
    canonical_loops_computing cano_loop_proxy;
    //
    //
    //std::set<int> base_pt_set;
    //for (unsigned int i = 0; i < basis_v_loops.size(); i++)
    //{
    //	for (unsigned int j = 0; j < basis_v_loops[i].size(); j++)
    //	{
    //		base_pt_set.insert(mesh.vecEdge[basis_v_loops[i][j]].v0);
    //		base_pt_set.insert(mesh.vecEdge[basis_v_loops[i][j]].v1);
    //	}
    //}
    //std::vector<int> base_pt_vec(base_pt_set.begin(), base_pt_set.end());//
    //
    //std::vector<int> base_pt_vec;
    //ReadBasisCriticalPoints(argv[3], base_pt_vec);
    std::set<int> base_pt_set(base_pt_vec.begin(), base_pt_vec.end());
    //
    //std::cout << "old base_pt_vec " << base_pt_vec.size() << std::endl;
    //
    cano_loop_proxy.SetGraphPtr(&locMeshGraph);
    cano_loop_proxy.SetEdgeWeights(sqEdgeLen);
    cano_loop_proxy.SetBasePointArray(base_pt_vec);
    cano_loop_proxy.SetMeshPtr(&locMesh);
    cano_loop_proxy.SetMeshGenus(edge_anno_proxy.vec_size / 2);
    //
    //cano_loop_proxy.ResetBaseVertices(basis_h_loops, edge_anno_proxy);
    //std::cout << "new base_pt_vec " << cano_loop_proxy.base_pt_index.size() << std::endl;
    //
    //bool bPartialBasis = false;
    std::cout << "Time for shortening handle and tunnel loops : " << std::endl;
    float cur_basis_h_len = BasisWeight(basis_h_loops, sqEdgeLen);
    float cur_basis_v_len = BasisWeight(basis_v_loops, sqEdgeLen);
    std::vector<std::set<int> > tmp_short_h_basis_loop;
    std::vector<std::set<int> > tmp_short_v_basis_loop;
    {
        // starting measuring the time
        boost::progress_timer t;
        //
        std::vector<std::set<int> > cano_loops;
        std::vector<int> non_tree_edges;
        std::vector<Annotation_Type> cano_loop_annotations;
        std::set<std::pair<float, int>, myFloatIntPairLessThan> sorted_cano_loops;
        std::vector<int> non_tree_edges_for_sorted_cano_loops;
        //
        float pre_basis_h_len = 0.0;
        float pre_basis_v_len = 0.0;

        //
        int iterNumber = 0;
        // optimize the all loops together
        do {
            tmp_short_h_basis_loop.clear();
            tmp_short_h_basis_loop.reserve(basis_h_loops.size());
            tmp_short_v_basis_loop.clear();
            tmp_short_v_basis_loop.reserve(basis_v_loops.size());
            base_pt_vec.clear();
            //
            cano_loop_proxy.compute_canonical_loops_fibo(cano_loops, non_tree_edges, cano_loop_annotations,
                                                         sorted_cano_loops, edge_anno_proxy);
            //
            if (edge_anno_proxy.ComputeShortestCanonicalLoops(basis_h_loops, basis_h_annotations, cano_loops,
                                                              non_tree_edges,
                                                              cano_loop_annotations, sorted_cano_loops,
                                                              tmp_short_h_basis_loop,
                                                              non_tree_edges_for_sorted_cano_loops)) {
                std::cout << ".";//ompletly use short loops" << std::endl;
            } else {
                std::cout << "*"; //artially use short loops" << std::endl;
                //bPartialBasis = true;
            }
            /**/
            cano_loop_proxy.base_pt_index.clear();//
            cano_loop_proxy.base_pt_index.reserve(non_tree_edges_for_sorted_cano_loops.size() * 3);
            for (unsigned int i = 0; i < non_tree_edges_for_sorted_cano_loops.size(); i++) {
                int edge_idx = non_tree_edges_for_sorted_cano_loops[i];
                //std::cout << edge_idx << std::endl;
                if ((edge_idx >= 0) && base_pt_set.find(locMesh.vecEdge[edge_idx].v0) == base_pt_set.end()) {
                    base_pt_set.insert(locMesh.vecEdge[edge_idx].v0);
                    cano_loop_proxy.base_pt_index.push_back(locMesh.vecEdge[edge_idx].v0);
                }
                //if (base_pt_set.find(mesh.vecEdge[edge_idx].v1) == base_pt_set.end())
                //{
                //	base_pt_set.insert(mesh.vecEdge[edge_idx].v1);
                //	cano_loop_proxy.base_pt_index.push_back(mesh.vecEdge[edge_idx].v1);
                //}
            }

            if (edge_anno_proxy.ComputeShortestCanonicalLoops(basis_v_loops, basis_v_annotations, cano_loops,
                                                              non_tree_edges,
                                                              cano_loop_annotations, sorted_cano_loops,
                                                              tmp_short_v_basis_loop,
                                                              non_tree_edges_for_sorted_cano_loops)) {
                std::cout << "."; //ompletly use short loops" << std::endl;
            } else {
                std::cout << "*"; //artially use short loops" << std::endl;
                //bPartialBasis = true;
            }
            //
            pre_basis_v_len = cur_basis_v_len;
            pre_basis_h_len = cur_basis_h_len;
            cur_basis_v_len = BasisWeight(tmp_short_v_basis_loop, sqEdgeLen);
            cur_basis_h_len = BasisWeight(tmp_short_h_basis_loop, sqEdgeLen);
            //
            for (unsigned int i = 0; i < non_tree_edges_for_sorted_cano_loops.size(); i++) {
                int edge_idx = non_tree_edges_for_sorted_cano_loops[i];
                //std::cout << edge_idx << std::endl;
                if ((edge_idx >= 0) && base_pt_set.find(locMesh.vecEdge[edge_idx].v0) == base_pt_set.end()) {
                    base_pt_set.insert(locMesh.vecEdge[edge_idx].v0);
                    cano_loop_proxy.base_pt_index.push_back(locMesh.vecEdge[edge_idx].v0);
                }
                //if (base_pt_set.find(mesh.vecEdge[edge_idx].v1) == base_pt_set.end())
                //{
                //	base_pt_set.insert(mesh.vecEdge[edge_idx].v1);
                //	cano_loop_proxy.base_pt_index.push_back(mesh.vecEdge[edge_idx].v1);
                //}
            }
            //std::cout << iterNumber << "\t";//<< " new base pt set size " << base_pt_set.size() << std::endl;
            //std::cout << iterNumber << " new base pt size " << cano_loop_proxy.base_pt_index.size() << std::endl;
            iterNumber++;
            if (cano_loop_proxy.base_pt_index.empty())
                break;
            if (iterNumber > 2000)
                break;

        } while (abs(cur_basis_h_len - pre_basis_h_len) > 0.000001 ||
                 abs(cur_basis_v_len - pre_basis_v_len) > 0.000001);
        //
        std::cout << std::endl;
    }
    //
    //
    out_h_basis_loops = tmp_short_h_basis_loop;
    //
    out_v_basis_loops = tmp_short_v_basis_loop;
    return;
}

void CycleGlobalOptimization(_SimpleMesh &locMesh, psbmReebGraph &reebgraph, std::vector<int> &OrientTriangles,
                             std::vector<std::set<int> > &out_v_basis_loops,
                             std::vector<std::set<int> > &out_h_basis_loops,
                             const float fScaleRatio) {//
    _simpGraph_vec locMeshGraph;
    //
    edge_annotation_computing edge_anno_proxy;
    //
    //
    ComputeAnnotation(locMesh, edge_anno_proxy.edge_annotations, OrientTriangles);
    edge_anno_proxy.vec_size = (int) edge_anno_proxy.edge_annotations[0].size();
    //
    if (edge_anno_proxy.vec_size == 0) {
        std::cout << "TRIVIAL LOOPS" << std::endl;
        exit(0);
    }
    std::vector<float> sqEdgeLen;
    AssembleMeshGraph(locMeshGraph, locMesh, sqEdgeLen, fScaleRatio);
    //
    edge_anno_proxy.SetMeshPtr(&locMesh);
    //edge_anno_proxy.ReadEdgeAnnotationsFromFile(argv[2]);
//{
//	//
//	edge_annotation_computing edge_anno_proxy;
//	//
//	//std::vector<int> meshTriangles;
//	LoadData(argv[1], edge_anno_proxy);
//	//
//	std::vector<float> sqEdgeLen;
//	AssembleMeshGraph(meshGraph, cycMesh, sqEdgeLen);
//	//
//	edge_anno_proxy.SetMeshPtr(&cycMesh);
//	//edge_anno_proxy.ReadEdgeAnnotationsFromFile(argv[2]);
//	//
//	//std::vector<int> fvs;
//	//ComputeFeedbackVertexSet(edge_anno_proxy.edge_annotations, fvs);

    std::vector<std::vector<int> > basis_h_loops;
    std::vector<std::vector<int> > basis_v_loops;
    std::vector<Annotation_Type> basis_h_annotations;
    std::vector<Annotation_Type> basis_v_annotations;
    //
    std::vector<int> base_pt_vec;
    //
    LoadCycleData(reebgraph, basis_h_loops, basis_v_loops, base_pt_vec);
    //
    edge_anno_proxy.CheckTwoGroupVectorOrthogonality(basis_v_loops, basis_v_annotations, basis_h_loops,
                                                     basis_h_annotations);
    //
    canonical_loops_computing cano_loop_proxy;
    //

    //ReadBasisCriticalPoints(argv[3], base_pt_vec);
    std::set<int> base_pt_set(base_pt_vec.begin(), base_pt_vec.end());
    //
    std::cout << "old base_pt_vec " << base_pt_vec.size() << std::endl;
    //
    cano_loop_proxy.SetGraphPtr(&locMeshGraph);
    cano_loop_proxy.SetEdgeWeights(sqEdgeLen);
    cano_loop_proxy.SetBasePointArray(base_pt_vec);
    cano_loop_proxy.SetMeshPtr(&locMesh);
    cano_loop_proxy.SetMeshGenus(edge_anno_proxy.vec_size / 2);
    //
    //cano_loop_proxy.ResetBaseVertices(basis_h_loops, edge_anno_proxy);
    //std::cout << "new base_pt_vec " << cano_loop_proxy.base_pt_index.size() << std::endl;
    //
    std::cout << "in cano loops computation" << std::endl;
    // starting measuring time
    boost::progress_timer t;
    //
    std::vector<std::set<int> > cano_loops;
    std::vector<int> non_tree_edges;
    std::vector<Annotation_Type> cano_loop_annotations;
    std::set<std::pair<float, int>, myFloatIntPairLessThan> sorted_cano_loops;
    std::vector<int> non_tree_edges_for_sorted_cano_loops;
    //
    float pre_basis_h_len = 0.0;
    float pre_basis_v_len = 0.0;
    float cur_basis_h_len = BasisWeight(basis_h_loops, sqEdgeLen);
    float cur_basis_v_len = BasisWeight(basis_v_loops, sqEdgeLen);
    std::vector<std::set<int> > tmp_short_h_basis_loop;
    std::vector<std::set<int> > tmp_short_v_basis_loop;
    //
    int iterNumber = 0;
    // compute the vertical loops first
    do {
        tmp_short_h_basis_loop.clear();
        tmp_short_h_basis_loop.reserve(basis_h_loops.size());
        tmp_short_v_basis_loop.clear();
        tmp_short_v_basis_loop.reserve(basis_v_loops.size());
        base_pt_vec.clear();
        //
        cano_loop_proxy.compute_canonical_loops_fibo(cano_loops, non_tree_edges, cano_loop_annotations,
                                                     sorted_cano_loops, edge_anno_proxy);
        //
        if (edge_anno_proxy.ComputeShortestCanonicalLoops(basis_h_loops, basis_h_annotations, cano_loops,
                                                          non_tree_edges,
                                                          cano_loop_annotations, sorted_cano_loops,
                                                          tmp_short_h_basis_loop,
                                                          non_tree_edges_for_sorted_cano_loops)) {
            std::cout << "c \t ";//ompletly use short loops" << std::endl;
        } else {
            std::cout << "p \t"; //artially use short loops" << std::endl;
        }
        /**/
        cano_loop_proxy.base_pt_index.clear();//
        cano_loop_proxy.base_pt_index.reserve(non_tree_edges_for_sorted_cano_loops.size() * 3);
        for (unsigned int i = 0; i < non_tree_edges_for_sorted_cano_loops.size(); i++) {
            int edge_idx = non_tree_edges_for_sorted_cano_loops[i];
            //std::cout << edge_idx << std::endl;
            if ((edge_idx >= 0) && base_pt_set.find(locMesh.vecEdge[edge_idx].v0) == base_pt_set.end()) {
                base_pt_set.insert(locMesh.vecEdge[edge_idx].v0);
                cano_loop_proxy.base_pt_index.push_back(locMesh.vecEdge[edge_idx].v0);
            }
            //if (base_pt_set.find(mesh.vecEdge[edge_idx].v1) == base_pt_set.end())
            //{
            //	base_pt_set.insert(mesh.vecEdge[edge_idx].v1);
            //	cano_loop_proxy.base_pt_index.push_back(mesh.vecEdge[edge_idx].v1);
            //}
        }

        if (edge_anno_proxy.ComputeShortestCanonicalLoops(basis_v_loops, basis_v_annotations, cano_loops,
                                                          non_tree_edges,
                                                          cano_loop_annotations, sorted_cano_loops,
                                                          tmp_short_v_basis_loop,
                                                          non_tree_edges_for_sorted_cano_loops)) {
            std::cout << "c \t"; //ompletly use short loops" << std::endl;
        } else {
            std::cout << "p \t"; //artially use short loops" << std::endl;
        }
        //
        pre_basis_v_len = cur_basis_v_len;
        pre_basis_h_len = cur_basis_h_len;
        cur_basis_v_len = BasisWeight(tmp_short_v_basis_loop, sqEdgeLen);
        cur_basis_h_len = BasisWeight(tmp_short_h_basis_loop, sqEdgeLen);
        //
        for (unsigned int i = 0; i < non_tree_edges_for_sorted_cano_loops.size(); i++) {
            int edge_idx = non_tree_edges_for_sorted_cano_loops[i];
            //std::cout << edge_idx << std::endl;
            if ((edge_idx >= 0) && base_pt_set.find(locMesh.vecEdge[edge_idx].v0) == base_pt_set.end()) {
                base_pt_set.insert(locMesh.vecEdge[edge_idx].v0);
                cano_loop_proxy.base_pt_index.push_back(locMesh.vecEdge[edge_idx].v0);
            }
            //if (base_pt_set.find(mesh.vecEdge[edge_idx].v1) == base_pt_set.end())
            //{
            //	base_pt_set.insert(mesh.vecEdge[edge_idx].v1);
            //	cano_loop_proxy.base_pt_index.push_back(mesh.vecEdge[edge_idx].v1);
            //}
        }
        std::cout << iterNumber << "\t";//<< " new base pt set size " << base_pt_set.size() << std::endl;
        //std::cout << iterNumber << " new base pt size " << cano_loop_proxy.base_pt_index.size() << std::endl;
        iterNumber++;
        if (cano_loop_proxy.base_pt_index.empty())
            break;
        if (iterNumber > 2000)
            break;

    } while (abs(cur_basis_h_len - pre_basis_h_len) > 0.000001 || abs(cur_basis_v_len - pre_basis_v_len) > 0.000001);
    //
    // use optimzed vertical loops to optimize horizontal loops
    std::set<int> tmpSet;
    cano_loop_proxy.base_pt_index.clear();
    for (unsigned int i = 0; i < tmp_short_v_basis_loop.size(); i++) {
        for (std::set<int>::iterator sIter = tmp_short_v_basis_loop[i].begin();
             sIter != tmp_short_v_basis_loop[i].end(); sIter++) {
            base_pt_set.insert(locMesh.vecEdge[*sIter].v0);
            base_pt_set.insert(locMesh.vecEdge[*sIter].v1);
            tmpSet.insert(locMesh.vecEdge[*sIter].v0);
        }
    }
    //
    for (unsigned int i = 0; i < tmp_short_h_basis_loop.size(); i++) {
        for (std::set<int>::iterator sIter = tmp_short_h_basis_loop[i].begin();
             sIter != tmp_short_h_basis_loop[i].end(); sIter++) {
            base_pt_set.insert(locMesh.vecEdge[*sIter].v0);
            base_pt_set.insert(locMesh.vecEdge[*sIter].v1);
            tmpSet.insert(locMesh.vecEdge[*sIter].v0);
        }
    }
    //
    cano_loop_proxy.base_pt_index.assign(tmpSet.begin(), tmpSet.end());
    //
    tmp_short_h_basis_loop.clear();
    tmp_short_h_basis_loop.reserve(basis_h_loops.size());
    tmp_short_v_basis_loop.clear();
    tmp_short_v_basis_loop.reserve(basis_v_loops.size());
    //
    cano_loop_proxy.compute_canonical_loops_fibo(cano_loops, non_tree_edges, cano_loop_annotations, sorted_cano_loops,
                                                 edge_anno_proxy);
    //

    if (edge_anno_proxy.ComputeShortestCanonicalLoops(basis_v_loops, basis_v_annotations, cano_loops, non_tree_edges,
                                                      cano_loop_annotations, sorted_cano_loops, tmp_short_v_basis_loop,
                                                      non_tree_edges_for_sorted_cano_loops)) {
        std::cout << "c \t"; //ompletly use short loops" << std::endl;
    } else {
        std::cout << "p \t"; //artially use short loops" << std::endl;
    }
    //
    if (edge_anno_proxy.ComputeShortestCanonicalLoops(basis_h_loops, basis_h_annotations, cano_loops, non_tree_edges,
                                                      cano_loop_annotations, sorted_cano_loops, tmp_short_h_basis_loop,
                                                      non_tree_edges_for_sorted_cano_loops)) {
        std::cout << "completly use short loops" << std::endl;
    } else {
        std::cout << "partially use short loops" << std::endl;
    }
    //
    out_h_basis_loops = tmp_short_h_basis_loop;
    //
    out_v_basis_loops = tmp_short_v_basis_loop;
    return;
}
//
//int mainx(int argc, char** argv)
//{
//	int optMethodOption;
//	stringstream sstr(stringstream::in | stringstream::out);
//	if (argc == 8)
//	{
//		sstr.str(argv[7]);
//		sstr >> optMethodOption;
//		sstr.clear();
//		std::cout << optMethodOption << std::endl;
//		if (optMethodOption)
//		{
//			//GlobalOptimization(argc, argv);
//		}
//		else
//		{
//			LocalOptimization(argc, argv);
//		}
//	}
//	else
//	{
//		if (argc == 9)
//			LocalOptimization_bdry(argc, argv);
//		else
//			LocalOptimization(argc, argv);
//	}
//	return 1;
//	//
//}

void CycleLocalOptimization(_SimpleMesh &locMesh, psbmReebGraph &reebgraph, std::vector<int> &OrientTriangles,
                            std::vector<std::set<int> > &out_v_basis_loops,
                            std::vector<std::set<int> > &out_h_basis_loops,
                            const float fScaleRatio) {//
    _simpGraph_vec locMeshGraph;
    //
    edge_annotation_computing edge_anno_proxy;
    //
    //std::cout << "tris size" << OrientTriangles.size() << std::endl;
    //
    ComputeAnnotation(locMesh, edge_anno_proxy.edge_annotations, OrientTriangles);
    edge_anno_proxy.vec_size = (int) edge_anno_proxy.edge_annotations[0].size();
    //
    if (edge_anno_proxy.vec_size == 0) {
        std::cout << "TRIVIAL LOOPS" << std::endl;
        exit(0);
    }
    //std::cout << "before assembe graph " << edge_anno_proxy.vec_size << std::endl;
    std::vector<float> sqEdgeLen;
    AssembleMeshGraph(locMeshGraph, locMesh, sqEdgeLen, fScaleRatio);
    //
    edge_anno_proxy.SetMeshPtr(&locMesh);
    //edge_anno_proxy.ReadEdgeAnnotationsFromFile(argv[2]);
    //
    std::vector<int> basis_h_LowestOnePos;
    std::vector<int> basis_v_LowestOnePos;
    std::vector<Annotation_Type> reduced_basis_h_annotations;
    std::vector<Annotation_Type> reduced_basis_v_annotations;
    //
    std::vector<std::vector<int> > basis_h_loops;
    std::vector<std::vector<int> > basis_v_loops;
    std::vector<Annotation_Type> basis_h_annotations;
    std::vector<Annotation_Type> basis_v_annotations;
    //
    std::vector<int> base_pt_vec;
    //
    LoadCycleData(reebgraph, basis_h_loops, basis_v_loops, base_pt_vec);

    edge_anno_proxy.CheckTwoGroupVectorOrthogonality(basis_v_loops, basis_v_annotations, reduced_basis_v_annotations,
                                                     basis_v_LowestOnePos,
                                                     basis_h_loops, basis_h_annotations, reduced_basis_h_annotations,
                                                     basis_h_LowestOnePos);
    //
    canonical_loops_computing cano_loop_proxy;
    //
    //
    //std::set<int> base_pt_set;
    //for (unsigned int i = 0; i < basis_v_loops.size(); i++)
    //{
    //	for (unsigned int j = 0; j < basis_v_loops[i].size(); j++)
    //	{
    //		base_pt_set.insert(mesh.vecEdge[basis_v_loops[i][j]].v0);
    //		base_pt_set.insert(mesh.vecEdge[basis_v_loops[i][j]].v1);
    //	}
    //}
    //std::vector<int> base_pt_vec(base_pt_set.begin(), base_pt_set.end());//
    //

    //ReadBasisCriticalPoints(argv[3], base_pt_vec);
    std::set<int> base_pt_set(base_pt_vec.begin(), base_pt_vec.end());
    //
    //std::cout << "old base_pt_vec " << base_pt_vec.size() << std::endl;
    //
    cano_loop_proxy.SetGraphPtr(&locMeshGraph);
    cano_loop_proxy.SetEdgeWeights(sqEdgeLen);
    cano_loop_proxy.SetBasePointArray(base_pt_vec);
    cano_loop_proxy.SetMeshPtr(&locMesh);
    cano_loop_proxy.SetMeshGenus(edge_anno_proxy.vec_size / 2);
    //
    //cano_loop_proxy.ResetBaseVertices(basis_h_loops, edge_anno_proxy);
    //std::cout << "new base_pt_vec " << cano_loop_proxy.base_pt_index.size() << std::endl;
    //
    std::cout << "Time for shortening handle and tunnel loops : " << std::endl;
    float cur_basis_h_len = BasisWeight(basis_h_loops, sqEdgeLen);
    float cur_basis_v_len = BasisWeight(basis_v_loops, sqEdgeLen);
    std::vector<std::set<int> > tmp_short_h_basis_loop;
    std::vector<std::set<int> > tmp_short_v_basis_loop;
    {
        // starting measuring the time
        boost::progress_timer t;
        //
        std::vector<std::set<int> > cano_loops;
        std::vector<int> non_tree_edges;
        std::vector<Annotation_Type> cano_loop_annotations;
        std::set<std::pair<float, int>, myFloatIntPairLessThan> sorted_cano_loops;
        std::vector<int> non_tree_edges_for_sorted_cano_loops;
        //
        float pre_basis_h_len = 0.0;
        float pre_basis_v_len = 0.0;

        //
        int iterNumber = 0;
        // optimize the all loops together
        do {
            tmp_short_h_basis_loop.clear();
            tmp_short_h_basis_loop.reserve(basis_h_loops.size());
            tmp_short_v_basis_loop.clear();
            tmp_short_v_basis_loop.reserve(basis_v_loops.size());
            base_pt_vec.clear();
            //

            cano_loop_proxy.compute_canonical_loops_fibo(cano_loops, non_tree_edges, cano_loop_annotations,
                                                         sorted_cano_loops, edge_anno_proxy);

            //
            //std::cout << "in cano 1" << std::endl;
            if (edge_anno_proxy.ComputeShortestCanonicalLoops(basis_h_loops, basis_h_annotations,
                                                              reduced_basis_h_annotations, basis_h_LowestOnePos,
                                                              cano_loops, non_tree_edges,
                                                              cano_loop_annotations, sorted_cano_loops,
                                                              tmp_short_h_basis_loop,
                                                              non_tree_edges_for_sorted_cano_loops)) {
                std::cout << ".";//ompletly use short loops" << std::endl;
            } else {
                std::cout << "*"; //artially use short loops" << std::endl;
            }
            //std::cout << "out cano 1" << std::endl;
            /**/
            cano_loop_proxy.base_pt_index.clear();//
            cano_loop_proxy.base_pt_index.reserve(non_tree_edges_for_sorted_cano_loops.size() * 3);
            for (unsigned int i = 0; i < non_tree_edges_for_sorted_cano_loops.size(); i++) {
                int edge_idx = non_tree_edges_for_sorted_cano_loops[i];
                //std::cout << edge_idx << std::endl;
                if ((edge_idx >= 0) && base_pt_set.find(locMesh.vecEdge[edge_idx].v0) == base_pt_set.end()) {
                    base_pt_set.insert(locMesh.vecEdge[edge_idx].v0);
                    cano_loop_proxy.base_pt_index.push_back(locMesh.vecEdge[edge_idx].v0);
                }
                //if (base_pt_set.find(mesh.vecEdge[edge_idx].v1) == base_pt_set.end())
                //{
                //	base_pt_set.insert(mesh.vecEdge[edge_idx].v1);
                //	cano_loop_proxy.base_pt_index.push_back(mesh.vecEdge[edge_idx].v1);
                //}
            }

            //std::cout << "in cano 2" << std::endl;
            if (edge_anno_proxy.ComputeShortestCanonicalLoops(basis_v_loops, basis_v_annotations,
                                                              reduced_basis_v_annotations, basis_v_LowestOnePos,
                                                              cano_loops, non_tree_edges,
                                                              cano_loop_annotations, sorted_cano_loops,
                                                              tmp_short_v_basis_loop,
                                                              non_tree_edges_for_sorted_cano_loops)) {
                std::cout << "."; //ompletly use short loops" << std::endl;
            } else {
                std::cout << "*"; //artially use short loops" << std::endl;
            }
            //std::cout << "out cano 2" << std::endl;
            //
            pre_basis_v_len = cur_basis_v_len;
            pre_basis_h_len = cur_basis_h_len;
            cur_basis_v_len = BasisWeight(tmp_short_v_basis_loop, sqEdgeLen);
            cur_basis_h_len = BasisWeight(tmp_short_h_basis_loop, sqEdgeLen);
            //
            for (unsigned int i = 0; i < non_tree_edges_for_sorted_cano_loops.size(); i++) {
                int edge_idx = non_tree_edges_for_sorted_cano_loops[i];
                //std::cout << edge_idx << std::endl;
                if ((edge_idx >= 0) && base_pt_set.find(locMesh.vecEdge[edge_idx].v0) == base_pt_set.end()) {
                    base_pt_set.insert(locMesh.vecEdge[edge_idx].v0);
                    cano_loop_proxy.base_pt_index.push_back(locMesh.vecEdge[edge_idx].v0);
                }
                //if (base_pt_set.find(mesh.vecEdge[edge_idx].v1) == base_pt_set.end())
                //{
                //	base_pt_set.insert(mesh.vecEdge[edge_idx].v1);
                //	cano_loop_proxy.base_pt_index.push_back(mesh.vecEdge[edge_idx].v1);
                //}
            }
            //std::cout << iterNumber << "\t";//<< " new base pt set size " << base_pt_set.size() << std::endl;
            //std::cout << iterNumber << " new base pt size " << cano_loop_proxy.base_pt_index.size() << std::endl;
            iterNumber++;
            if (cano_loop_proxy.base_pt_index.empty())
                break;
            if (iterNumber > 2000)
                break;

        } while (fabs(cur_basis_h_len - pre_basis_h_len) > 0.000001 ||
                 fabs(cur_basis_v_len - pre_basis_v_len) > 0.000001);
        //
        std::cout << std::endl;
    }
    //
    out_h_basis_loops = tmp_short_h_basis_loop;
    //
    //
    out_v_basis_loops = tmp_short_v_basis_loop;
    //
    return;
}


#include <string>
#include <iostream>
#include <fstream>
#include <ios>
#include <stdexcept>
#include <limits>
#include <urdf_model/model.h>
#include <urdf_parser/urdf_parser.h>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/frames_io.hpp>
#include <kdl/chain.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainiksolvervel_pinv.hpp>
#include <kdl/chainiksolverpos_nr_jl.hpp>
#include <Eigen/Core>
// TODO make dependency for timing optional
#include <boost/date_time/posix_time/posix_time.hpp>
// TODO correct paths
#include "cppoptlib/problem.h"
#include "cppoptlib/solver/lbfgsbsolver.h"

void chainFromUrdf(const std::string& file, const std::string& root,
                   const std::string& tip, KDL::Chain& chain,
                   Eigen::VectorXd& qlo, Eigen::VectorXd& qhi, bool verbose=0)
{
  std::ifstream f(file.c_str());
  if(!f.good())
    throw std::ios_base::failure("Cannot read from file '" + file + "'.");

  boost::shared_ptr<urdf::ModelInterface> robot_model =
      urdf::parseURDFFile(file);

  KDL::Tree tree;
#ifdef ROCK // uses an old version of kdl_parser
  if(!kdl_parser::treeFromUrdfModel(*robot_model, tree))
#else
  if(!kdl_parser::treeFromUrdfModel(robot_model, tree))
#endif
    throw std::runtime_error("Could not extract kdl tree");
  if(verbose >= 1)
    std::cout << "[IK] KDL::Tree (" << tree.getNrOfJoints() << " joints, "
        << tree.getNrOfSegments() << " segments)" << std::endl;

  tree.getChain(root, tip, chain);
  if(verbose >= 1)
    std::cout << "[IK] KDL::Chain (" << chain.getNrOfJoints() << " joints, "
        << chain.getNrOfSegments() << " segments)" << std::endl;

  std::list<double> lo, hi;
  for(unsigned int i = 0; i < chain.getNrOfSegments(); i++)
  {
    KDL::Segment segment = chain.getSegment(i);
    KDL::Joint joint = segment.getJoint();
    if(joint.getType() != KDL::Joint::None)
    {
      std::string name = joint.getName();
      const double lower = robot_model->getJoint(name)->limits->lower;
      const double upper = robot_model->getJoint(name)->limits->upper;
      if(verbose >= 1)
      {
        std::cout << "  joint #" << lo.size() << ": '" << name << "'" << std::endl
            << "    limits: [" << lower << ", " << upper << "]" << std::endl;
      }
      lo.push_back(lower);
      hi.push_back(upper);
    }
  }

  const unsigned int numJoints = chain.getNrOfJoints();
  if(numJoints != lo.size() || numJoints != hi.size())
    throw std::runtime_error(
        "Number of joints in chain and limits is not equal!");

  qlo.resize(numJoints);
  qhi.resize(numJoints);

  std::list<double>::iterator loIt = lo.begin(), hiIt = hi.begin();
  for(unsigned int i = 0; i < numJoints; i++, loIt++, hiIt++)
  {
    qlo(i) = *loIt;
    qhi(i) = *hiIt;
  }
}

template <typename Derived>
KDL::JntArray jntEigen2KDL(const Eigen::MatrixBase<Derived>& qin)
{
  KDL::JntArray qout;
  qout.data = qin;
  return qout;
}

template <typename Derived>
void poseKDL2Eigen(const KDL::Frame& frame, Eigen::MatrixBase<Derived>& pose)
{
  for(unsigned int i = 0; i < 3; i++)
    pose(i) = frame.p(i);
  frame.M.GetQuaternion(pose(4), pose(5), pose(6), pose(3)); // x, y, z, w
}

template <typename Derived>
void poseEigen2KDL(const Eigen::MatrixBase<Derived>& pose, KDL::Frame& frame)
{
  for(unsigned int i = 0; i < 3; i++)
    frame.p(i) = pose(i);
  frame.M = KDL::Rotation::Quaternion(pose(4), pose(5), pose(6), pose(3));
}

class ForwardKinematics
{
  const int verbose;
  unsigned int numJoints;
  KDL::JntArray q;
  KDL::Frame p;
public:
  KDL::Chain chain;
  Eigen::VectorXd qlo, qhi;
  KDL::ChainFkSolverPos_recursive* fk;

  ForwardKinematics(const std::string& file, const std::string& root,
                    const std::string& tip, int verbose=0)
    : verbose(verbose)
  {
    chainFromUrdf(file, root, tip, chain, qlo, qhi, verbose);
    numJoints = chain.getNrOfJoints();
    fk = new KDL::ChainFkSolverPos_recursive(chain);
  }

  ~ForwardKinematics()
  {
    delete fk;
  }

  const unsigned int getNumJoints()
  {
    return numJoints;
  }

  template <typename Derived1, typename Derived2>
  bool JntToCart(const Eigen::MatrixBase<Derived1>& q,
                 Eigen::MatrixBase<Derived2>& p)
  {
    this->q.data = q;
    const bool result = fk->JntToCart(this->q, this->p) >= 0;
    poseKDL2Eigen(this->p, p);
    return result;
  }
};

class IKProblem : public cppoptlib::Problem<double>
{
  ForwardKinematics& fk;
  const Eigen::VectorXd* desiredPose;
  Eigen::VectorXd pose;
  const double positionWeight;
  const double rotationWeight;
public:
  IKProblem(ForwardKinematics& fk, double positionWeight=1.0,
            double rotationWeight=1.0)
    : fk(fk), desiredPose(0), pose(7),
      positionWeight(positionWeight), rotationWeight(rotationWeight)
  {
  }

  void setDesiredPose(const Eigen::VectorXd& newPose)
  {
    desiredPose = &newPose;
  }

  double value(const cppoptlib::Vector<double>& q)
  {
    const bool success = fk.JntToCart(q, pose);
    if(success)
    {
      const double sqPositionDist = (pose.head<3>() - desiredPose->head<3>()).squaredNorm();
      const Eigen::Quaterniond q1(pose.tail<4>().data());
      const Eigen::Quaterniond q2(desiredPose->tail<4>());
      const double angle = 2.0 * std::acos((q1 * q2.conjugate()).w());
      const double angularDist = std::min(angle, 2.0 * M_PI - angle);
      //const double angularDist = q1.angularDistance(q2);
      const double sqAngularDist = angularDist * angularDist;

      return positionWeight * sqPositionDist + rotationWeight * sqAngularDist;
    }
    else
    {
      return std::numeric_limits<double>::max();
    }
  }
};

class ApproximateInverseKinematics
{
protected:
  ForwardKinematics fk;
  IKProblem objective;
  unsigned int maxiter;
  const int verbose;
  // TODO make optional
  boost::posix_time::ptime start_time;
  boost::posix_time::ptime stop_time;
public:
  ApproximateInverseKinematics(
      const std::string& file, const std::string& root,
      const std::string& tip, double positionWeight=1.0,
      double rotationWeight=1.0, unsigned int maxiter=100000,
      int verbose=0)
    : fk(file, root, tip, verbose),
      objective(fk, positionWeight, rotationWeight), maxiter(maxiter),
      verbose(verbose)
  {
  }

  virtual ~ApproximateInverseKinematics()
  {
  }

  const unsigned int getNumJoints()
  {
    return fk.getNumJoints();
  }

  template <typename Derived1, typename Derived2>
  void JntToCart(const Eigen::MatrixBase<Derived1>& q,
                 Eigen::MatrixBase<Derived2>& pose)
  {
    fk.JntToCart(q, pose);
  }

  void JntToCart(double* q, double* pose)
  {
    Eigen::Map<Eigen::VectorXd> qvec(q, fk.getNumJoints());
    Eigen::Map<Eigen::VectorXd> pvec(pose, 7);
    fk.JntToCart(qvec, pvec);
  }

  template <typename Derived1, typename Derived2>
  bool CartToJnt(const Eigen::MatrixBase<Derived1>& pose,
                 Eigen::MatrixBase<Derived2>& currentQ)
  {
    // TODO ctor args
    cppoptlib::ISolver<double, 1>::Info stopControl;
    stopControl.rate = 0.0;
    stopControl.iterations = maxiter;
    stopControl.gradNorm = 0.0;
    stopControl.m = 10;
    // TODO check whether we can make the solver a member
    cppoptlib::LbfgsbSolver<double> solver(stopControl);

    // We cannot pass currentQ directly to solver.minimize() because the
    // interface is not generic enough
    Eigen::VectorXd q = currentQ;
    setProblemBounds(&q);  // must be done before any clipping is performed
    clipToLimits(q);

    Eigen::VectorXd p = pose;
    objective.setDesiredPose(p);

    if(verbose >= 2)
    {
      std::cout << "[IK] Limits:" << std::endl
          << "  " << objective.lowerBound().transpose() << std::endl
          << "  " << objective.upperBound().transpose() << std::endl;
    }

    start_time = boost::posix_time::microsec_clock::local_time(); // TODO optional
    solver.minimize(objective, q);

    // Workaround for a serious problem: for some poses we are completely stuck
    // in a local minimum
    if(q == currentQ)
    {
      if(verbose >= 2)
        std::cout << "[IK] Workaround is active" << std::endl;
      p(0) += 0.001;
      p(1) -= 0.001;
      p(2) += 0.001;
      objective.setDesiredPose(p);
      solver.minimize(objective, q);
      p(0) -= 0.001;
      p(1) += 0.001;
      p(2) -= 0.001;
      objective.setDesiredPose(p);
      solver.minimize(objective, q);
    }

    stop_time = boost::posix_time::microsec_clock::local_time(); // TODO optional
    // TODO check why limits are sometimes violated
    clipToLimits(q);

    currentQ = q;
    if(verbose >= 1)
    {
      std::cout << "[IK] Optimized joints:" << std::endl
          << "  " << currentQ.transpose() << std::endl
          << "  Objective: " << objective(currentQ) << std::endl;
      if(verbose >= 2)
      {
        Eigen::VectorXd actualPose(7);
        fk.JntToCart(currentQ, actualPose);
        std::cout << "[IK] Desired pose:" << std::endl
            << "  " << pose.transpose() << std::endl;
        std::cout << "[IK] Actual pose:" << std::endl
            << "  " << actualPose.transpose() << std::endl;
      }
    }

    return true;
  }

  void CartToJnt(double* pose, double* q)
  {
    Eigen::Map<Eigen::VectorXd> pvec(pose, 7);
    Eigen::Map<Eigen::VectorXd> qvec(q, fk.getNumJoints());
    CartToJnt(pvec, qvec);
  }

  // TODO optional
  double getLastTiming() // in seconds
  {
    return (stop_time - start_time).total_nanoseconds() / 1000000000.0;
  }

protected:
  virtual void setProblemBounds(Eigen::VectorXd* currentQ = 0)
  {
    objective.setBoxConstraint(fk.qlo, fk.qhi);
  }

  template <typename Derived>
  void clipToLimits(Eigen::MatrixBase<Derived>& q)
  {
    q = q.cwiseMax(objective.lowerBound()).cwiseMin(objective.upperBound());
  }
};

class ApproximateLocalInverseKinematics : public ApproximateInverseKinematics
{
  bool initialized;
  double maxJump;
public:
  ApproximateLocalInverseKinematics(
      const std::string& file, const std::string& root,
      const std::string& tip, double positionWeight=1.0,
      double rotationWeight=1.0, double maxJump=1.0,
      unsigned int maxiter=100000, int verbose=0)
    : ApproximateInverseKinematics(file, root, tip, positionWeight,
                                   rotationWeight, maxiter, verbose),
      initialized(false), maxJump(maxJump)
  {
  }

  void reset()
  {
    initialized = false;
  }
protected:
  virtual void setProblemBounds(Eigen::VectorXd* currentQ)
  {
    if(!initialized)
    {
      ApproximateInverseKinematics::setProblemBounds();
      initialized = true;
      return;
    }

    Eigen::VectorXd lo = (currentQ->array() - maxJump).matrix().cwiseMax(fk.qlo);
    Eigen::VectorXd hi = (currentQ->array() + maxJump).matrix().cwiseMin(fk.qhi);
    objective.setBoxConstraint(lo, hi);
  }
};

class ExactInverseKinematics
{
  ForwardKinematics fk;
  KDL::ChainIkSolverVel_pinv velIk;
  KDL::ChainIkSolverPos_NR_JL posIk;
  const int verbose;
  KDL::JntArray qout;
  KDL::Frame frame;
  // TODO make optional
  boost::posix_time::ptime start_time;
  boost::posix_time::ptime stop_time;
public:
  ExactInverseKinematics(
      const std::string& file, const std::string& root,
      const std::string& tip, double eps=1e-5, int maxiter=150,
      int verbose=0)
    : fk(file, root, tip, verbose), velIk(fk.chain, eps, maxiter),
      posIk(fk.chain, jntEigen2KDL(fk.qlo), jntEigen2KDL(fk.qhi), *fk.fk,
            velIk, maxiter, eps), verbose(verbose)
  {
    qout.resize(fk.getNumJoints());
  }

  const unsigned int getNumJoints()
  {
    return fk.getNumJoints();
  }

  template <typename Derived1, typename Derived2>
  void JntToCart(const Eigen::MatrixBase<Derived1>& q,
                 Eigen::MatrixBase<Derived2>& pose)
  {
    fk.JntToCart(q, pose);
  }

  void JntToCart(double* q, double* pose)
  {
    Eigen::Map<Eigen::VectorXd> qvec(q, fk.getNumJoints());
    Eigen::Map<Eigen::VectorXd> pvec(pose, 7);
    fk.JntToCart(qvec, pvec);
  }

  template <typename Derived1, typename Derived2>
  bool CartToJnt(const Eigen::MatrixBase<Derived1>& pose,
                 Eigen::MatrixBase<Derived2>& currentQ)
  {
    poseEigen2KDL(pose, frame);
    start_time = boost::posix_time::microsec_clock::local_time(); // TODO optional
    const int result = posIk.CartToJnt(jntEigen2KDL(currentQ), frame, qout);
    stop_time = boost::posix_time::microsec_clock::local_time(); // TODO optional
    currentQ = qout.data;

    if(verbose >= 1)
    {
      if(result == -3)
        std::cout << "[IK] FAILED: Maximum number of iterations reached!"
            << std::endl;
      std::cout << "[IK] Optimized joints:" << std::endl
          << "  " << currentQ.transpose() << std::endl;
      if(verbose >= 2)
      {
        Eigen::VectorXd actualPose(7);
        fk.JntToCart(currentQ, actualPose);
        std::cout << "[IK] Desired pose:" << std::endl
            << "  " << pose.transpose() << std::endl;
        std::cout << "[IK] Actual pose:" << std::endl
            << "  " << actualPose.transpose() << std::endl;
      }
    }
    return result >= 0;
  }

  bool CartToJnt(double* pose, double* q)
  {
    Eigen::Map<Eigen::VectorXd> pvec(pose, 7);
    Eigen::Map<Eigen::VectorXd> qvec(q, fk.getNumJoints());
    return CartToJnt(pvec, qvec);
  }

  // TODO optional
  double getLastTiming() // in seconds
  {
    return (stop_time - start_time).total_nanoseconds() / 1000000000.0;
  }
};


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "PointwiseFunctions/AnalyticSolutions/GeneralRelativity/Tov.hpp"

// Need Boost MultiArray because it is used internally by ODEINT
#include "DataStructures/BoostMultiArray.hpp"  // IWYU pragma: keep

#include <algorithm>
#include <array>
#include <boost/numeric/odeint.hpp>  // IWYU pragma: keep
#include <cmath>
#include <cstddef>
#include <functional>
#include <ostream>
#include <pup.h>
#include <vector>

#include "DataStructures/DataVector.hpp"
#include "DataStructures/Tensor/EagerMath/Magnitude.hpp"
#include "Utilities/ConstantExpressions.hpp"
#include "Utilities/ContainerHelpers.hpp"
#include "Utilities/ErrorHandling/Assert.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/MakeWithValue.hpp"

// IWYU pragma: no_include <boost/numeric/odeint/integrate/integrate_adaptive.hpp>
// IWYU pragma: no_include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
// IWYU pragma: no_include <boost/numeric/odeint/stepper/dense_output_runge_kutta.hpp>
// IWYU pragma: no_include <boost/numeric/odeint/stepper/generation/make_dense_output.hpp>
// IWYU pragma: no_include <boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp>
// IWYU pragma: no_include <complex>

// IWYU pragma: no_forward_declare boost::numeric::odeint::controlled_runge_kutta
// IWYU pragma: no_forward_declare EquationsOfState::EquationOfState
// IWYU pragma: no_forward_declare Tensor

namespace {

void lindblom_rhs(const gsl::not_null<std::array<double, 2>*> dvars,
                  const std::array<double, 2>& vars, const double log_enthalpy,
                  const EquationsOfState::EquationOfState<true, 1>&
                      equation_of_state) noexcept {
  const double& radius_squared = vars[0];
  const double& mass_over_radius = vars[1];
  double& d_radius_squared = (*dvars)[0];
  double& d_mass_over_radius = (*dvars)[1];
  const double specific_enthalpy = std::exp(log_enthalpy);
  const double rest_mass_density =
      get(equation_of_state.rest_mass_density_from_enthalpy(
          Scalar<double>{specific_enthalpy}));
  const double pressure = get(equation_of_state.pressure_from_density(
      Scalar<double>{rest_mass_density}));
  const double energy_density =
      specific_enthalpy * rest_mass_density - pressure;

  // At the center of the star: (u,v) = (0,0)
  if (UNLIKELY((radius_squared == 0.0) and (mass_over_radius == 0.0))) {
    d_radius_squared = -3.0 / (2.0 * M_PI * (energy_density + 3.0 * pressure));
    d_mass_over_radius =
        -2.0 * energy_density / (energy_density + 3.0 * pressure);
  } else {
    const double common_factor =
        (1.0 - 2.0 * mass_over_radius) /
        (4.0 * M_PI * radius_squared * pressure + mass_over_radius);
    d_radius_squared = -2.0 * radius_squared * common_factor;
    d_mass_over_radius =
        -(4.0 * M_PI * radius_squared * energy_density - mass_over_radius) *
        common_factor;
  }
}

class Observer {
 public:
  void operator()(const std::array<double, 2>& vars,
                  const double current_log_enthalpy) noexcept {
    radius.push_back(std::sqrt(vars[0]));
    mass_over_radius.push_back(vars[1]);
    log_enthalpy.push_back(current_log_enthalpy);
  }
  std::vector<double> radius;
  std::vector<double> mass_over_radius;
  std::vector<double> log_enthalpy;
};

template <typename DataType>
RelativisticEuler::Solutions::TovStar<
    gr::Solutions::TovSolution>::RadialVariables<DataType>
interior_solution(
    const EquationsOfState::EquationOfState<true, 1>& equation_of_state,
    const DataType& radius, const DataType& mass_over_radius,
    const DataType& log_specific_enthalpy,
    const double log_lapse_at_outer_radius) noexcept {
  RelativisticEuler::Solutions::TovStar<
      gr::Solutions::TovSolution>::RadialVariables<DataType>
      result(radius);
  result.specific_enthalpy = Scalar<DataType>{exp(log_specific_enthalpy)};
  result.rest_mass_density = equation_of_state.rest_mass_density_from_enthalpy(
      result.specific_enthalpy);
  result.pressure =
      equation_of_state.pressure_from_density(result.rest_mass_density);
  result.specific_internal_energy =
      equation_of_state.specific_internal_energy_from_density(
          result.rest_mass_density);
  result.metric_time_potential =
      log_lapse_at_outer_radius - log_specific_enthalpy;
  result.dr_metric_time_potential =
      (mass_over_radius / radius + 4.0 * M_PI * get(result.pressure) * radius) /
      (1.0 - 2.0 * mass_over_radius);
  result.metric_radial_potential = -0.5 * log(1.0 - 2.0 * mass_over_radius);
  result.dr_metric_radial_potential =
      (4.0 * M_PI * radius *
           (get(result.specific_enthalpy) * get(result.rest_mass_density) -
            get(result.pressure)) -
       mass_over_radius / radius) /
      (1.0 - 2.0 * mass_over_radius);
  result.metric_angular_potential = make_with_value<DataType>(radius, 0.0);
  result.dr_metric_angular_potential = make_with_value<DataType>(radius, 0.0);
  return result;
}

template <typename DataType>
RelativisticEuler::Solutions::TovStar<
    gr::Solutions::TovSolution>::RadialVariables<DataType>
vacuum_solution(const DataType& radius, const double total_mass) noexcept {
  RelativisticEuler::Solutions::TovStar<
      gr::Solutions::TovSolution>::RadialVariables<DataType>
      result(radius);
  result.specific_enthalpy = make_with_value<Scalar<DataType>>(radius, 1.0);
  result.rest_mass_density = make_with_value<Scalar<DataType>>(radius, 0.0);
  result.pressure = make_with_value<Scalar<DataType>>(radius, 0.0);
  result.specific_internal_energy =
      make_with_value<Scalar<DataType>>(radius, 0.0);
  const DataType one_minus_two_m_over_r = 1.0 - 2.0 * total_mass / radius;
  result.metric_time_potential = 0.5 * log(one_minus_two_m_over_r);
  result.dr_metric_time_potential =
      total_mass / square(radius) / one_minus_two_m_over_r;
  result.metric_radial_potential = -result.metric_time_potential;
  result.dr_metric_radial_potential = -result.dr_metric_time_potential;
  result.metric_angular_potential = make_with_value<DataType>(radius, 0.0);
  result.dr_metric_angular_potential = make_with_value<DataType>(radius, 0.0);
  return result;
}

}  // namespace

namespace gr::Solutions {

TovSolution::TovSolution(
    const EquationsOfState::EquationOfState<true, 1>& equation_of_state,
    const double central_mass_density,
    const double log_enthalpy_at_outer_radius, const double absolute_tolerance,
    const double relative_tolerance) {
  std::array<double, 2> u_and_v = {{0.0, 0.0}};
  std::array<double, 2> dudh_and_dvdh{};
  const double central_log_enthalpy =
      std::log(get(equation_of_state.specific_enthalpy_from_density(
          Scalar<double>{central_mass_density})));
  lindblom_rhs(&dudh_and_dvdh, u_and_v, central_log_enthalpy,
               equation_of_state);
  const double initial_step = -std::min(std::abs(1.0 / dudh_and_dvdh[0]),
                                        std::abs(1.0 / dudh_and_dvdh[1]));
  using StateDopri5 =
      boost::numeric::odeint::runge_kutta_dopri5<std::array<double, 2>>;
  boost::numeric::odeint::dense_output_runge_kutta<
      boost::numeric::odeint::controlled_runge_kutta<StateDopri5>>
      dopri5 = make_dense_output(absolute_tolerance, relative_tolerance,
                                 StateDopri5{});
  Observer observer{};
  boost::numeric::odeint::integrate_adaptive(
      dopri5,
      [&equation_of_state](const std::array<double, 2>& lindblom_u_and_v,
                           std::array<double, 2>& lindblom_dudh_and_dvdh,
                           const double lindblom_enthalpy) noexcept {
        return lindblom_rhs(&lindblom_dudh_and_dvdh, lindblom_u_and_v,
                            lindblom_enthalpy, equation_of_state);
      },
      u_and_v, central_log_enthalpy, log_enthalpy_at_outer_radius, initial_step,
      std::ref(observer));
  outer_radius_ = observer.radius.back();
  const double total_mass_over_radius = observer.mass_over_radius.back();
  total_mass_ = total_mass_over_radius * outer_radius_;
  log_lapse_at_outer_radius_ = 0.5 * log(1.0 - 2.0 * total_mass_over_radius);
  mass_over_radius_interpolant_ =
      intrp::BarycentricRational(observer.radius, observer.mass_over_radius, 5);
  // log_enthalpy(radius) is almost linear so an interpolant of order 3
  // maximizes precision
  log_enthalpy_interpolant_ =
      intrp::BarycentricRational(observer.radius, observer.log_enthalpy, 3);
}

double TovSolution::outer_radius() const noexcept { return outer_radius_; }

double TovSolution::mass_over_radius(const double r) const noexcept {
  ASSERT(r >= 0.0 and r <= outer_radius_,
         "Invalid radius: " << r << " not in [0.0, " << outer_radius_ << "]\n");
  return mass_over_radius_interpolant_(r);
}

double TovSolution::mass(const double r) const noexcept {
  return mass_over_radius(r) * r;
}

double TovSolution::log_specific_enthalpy(const double r) const noexcept {
  ASSERT(r >= 0.0 and r <= outer_radius_,
         "Invalid radius: " << r << " not in [0.0, " << outer_radius_ << "]\n");
  return log_enthalpy_interpolant_(r);
}

template <>
RelativisticEuler::Solutions::TovStar<TovSolution>::RadialVariables<double>
TovSolution::radial_variables(
    const EquationsOfState::EquationOfState<true, 1>& equation_of_state,
    const tnsr::I<double, 3>& x) const noexcept {
  // add small number to avoid FPEs at origin
  const double radius = get(magnitude(x)) + 1.e-30 * outer_radius_;
  if (radius >= outer_radius_) {
    return vacuum_solution(radius, total_mass_);
  }
  return interior_solution(equation_of_state, radius, mass_over_radius(radius),
                           log_specific_enthalpy(radius),
                           log_lapse_at_outer_radius_);
}

template <>
RelativisticEuler::Solutions::TovStar<TovSolution>::RadialVariables<DataVector>
TovSolution::radial_variables(
    const EquationsOfState::EquationOfState<true, 1>& equation_of_state,
    const tnsr::I<DataVector, 3>& x) const noexcept {
  // add small number to avoid FPEs at origin
  const DataVector radius = get(magnitude(x)) + 1.e-30 * outer_radius_;
  if (min(radius) >= outer_radius_) {
    return vacuum_solution(radius, total_mass_);
  }
  if (max(radius) <= outer_radius_) {
    DataVector mass_over_radius_data(radius.size());
    DataVector log_of_specific_enthalpy(radius.size());
    for (size_t i = 0; i < get_size(radius); i++) {
      const double r = get_element(radius, i);
      get_element(mass_over_radius_data, i) = mass_over_radius(r);
      get_element(log_of_specific_enthalpy, i) = log_specific_enthalpy(r);
    }
    return interior_solution(equation_of_state, radius, mass_over_radius_data,
                             log_of_specific_enthalpy,
                             log_lapse_at_outer_radius_);
  }
  RelativisticEuler::Solutions::TovStar<TovSolution>::RadialVariables<
      DataVector>
      result(radius);
  for (size_t i = 0; i < radius.size(); i++) {
    const double r = radius[i];
    auto radial_vars_at_r =
        (r <= outer_radius_
             ? interior_solution(equation_of_state, r, mass_over_radius(r),
                                 log_specific_enthalpy(r),
                                 log_lapse_at_outer_radius_)
             : vacuum_solution(r, total_mass_));
    get(result.rest_mass_density)[i] = get(radial_vars_at_r.rest_mass_density);
    get(result.pressure)[i] = get(radial_vars_at_r.pressure);
    get(result.specific_internal_energy)[i] =
        get(radial_vars_at_r.specific_internal_energy);
    get(result.specific_enthalpy)[i] = get(radial_vars_at_r.specific_enthalpy);
    result.metric_time_potential[i] = radial_vars_at_r.metric_time_potential;
    result.dr_metric_time_potential[i] =
        radial_vars_at_r.dr_metric_time_potential;
    result.metric_radial_potential[i] =
        radial_vars_at_r.metric_radial_potential;
    result.dr_metric_radial_potential[i] =
        radial_vars_at_r.dr_metric_radial_potential;
  }
  result.metric_angular_potential = make_with_value<DataVector>(radius, 0.0);
  result.dr_metric_angular_potential = make_with_value<DataVector>(radius, 0.0);
  return result;
}

void TovSolution::pup(PUP::er& p) noexcept {  // NOLINT
  p | outer_radius_;
  p | total_mass_;
  p | log_lapse_at_outer_radius_;
  p | mass_over_radius_interpolant_;
  p | log_enthalpy_interpolant_;
}

}  // namespace gr::Solutions


// JKC superblocks list
// Superblocks - Version 0.4.3
//
// explorer: http://block.al.tcoin.info/chain/JKCCoin
// explorer: http://jkc.cryptocoinexplorer.com/

#include <iostream>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
using namespace std;
using namespace boost;

int list_length = 100000;

int main() {

	cout << "[JKC] Junkcoin - Superblock list" << endl << endl;
	cout << "BLOCK, REWARD" << endl;
	for (int nHeight = 0; nHeight <= list_length; nHeight++) {
		random::mt19937 gen(nHeight);
		random::uniform_int_distribution<> dist(1, 100000);
		int rand = dist(gen);
		if(rand > 99990) {
			cout << nHeight << ", 1000" << endl;
		} else if (rand < 1001) {
			cout << nHeight << ", 150" << endl;
		}
	}
	return 0;
}


// merge VMD files generated by readfacevmd and another tool
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <fstream>
#include <iostream>
#include <string>
#include <map>
#include <vector>
#include "VMD.h"
#include "MMDFileIOUtil.h"
#include "interpolate.h"
//#include "reducevmd.h"

using namespace Eigen;
using namespace MMDFileIOUtil;
using namespace std;

void usage(char *prog)
{
  cerr << "usage: " << prog << " body.vmd face.vmd out.vmd\n";
}

Quaternionf rot_frame(const vector<VMD_Frame>& frames, uint32_t n)
{
  VMD_Frame f_old;
  f_old.rotation = Quaternionf::Identity();
  for (const VMD_Frame& f : frames) {
    if (f.number == n) {
      return f.rotation;
    } else if (f.number > n) {
      VMD_Frame f_i = interpolate_frame(f_old, f, n, false);
      return f_i.rotation;
    }
    f_old = f;
  }
  return f_old.rotation;
}

void merge_vmd(VMD& body, VMD& face)
{
  // キーフレームをボーンごとに分ける
  map<string, vector<VMD_Frame>> body_frame_map;
  for (VMD_Frame& f : body.frame) {
    string name;
    sjis_to_utf8(f.bonename, name, f.bonename_len);
    body_frame_map[name].push_back(f);
  }
  map<string, vector<VMD_Frame>> face_frame_map;
  for (VMD_Frame& f : face.frame) {
    string name;
    sjis_to_utf8(f.bonename, name, f.bonename_len);
    face_frame_map[name].push_back(f);
  }

  // フレーム番号順にソートしておく
  for (auto iter = body_frame_map.begin(); iter != body_frame_map.end(); iter++) {
    vector<VMD_Frame>& fv = iter->second;
    sort(fv.begin(), fv.end());
  }
  for (auto iter = face_frame_map.begin(); iter != face_frame_map.end(); iter++) {
    vector<VMD_Frame>& fv = iter->second;
    sort(fv.begin(), fv.end());
  }

  // body motion と face motionを接続するボーンを決める
  string connector_bone = u8"頭";
  decltype(face_frame_map)::iterator it = face_frame_map.find(u8"首");
  if (it != face_frame_map.end()) {
    connector_bone = u8"首";
  }
  vector<string> head_hierarchy = {u8"全ての親", u8"センター", u8"上半身", u8"上半身2", u8"首"};

  // ボーンキーフレームをbody.frame に格納する
  body.frame.clear();
  for (auto iter = body_frame_map.begin(); iter != body_frame_map.end(); iter++) {
    string name = iter->first;
    if (name == u8"頭" || name == connector_bone) {
      continue;
    }
    vector<VMD_Frame>& fv = iter->second;
    for (VMD_Frame& f : fv) {
      body.frame.push_back(f);
    }
  }
  for (auto iter = face_frame_map.begin(); iter != face_frame_map.end(); iter++) {
    string name = iter->first;
    if (name == u8"センター" || name == u8"全ての親") {
      continue;
    }
    vector<VMD_Frame>& fv = iter->second;
    for (VMD_Frame& f : fv) {
      if (name == connector_bone) {
	Quaternionf rot = Quaternionf::Identity();
	for (string bonename : head_hierarchy) {
	  if (bonename != connector_bone) {
	    rot = rot * rot_frame(body_frame_map[bonename], f.number);
	  }
	}
	f.rotation = rot.inverse() * f.rotation;
      }
      body.frame.push_back(f);
    }
  }

  // 表情キーフレームをマージする
  for (VMD_Morph& m : face.morph) {
    body.morph.push_back(m);
  }
}

int main(int argc, char* argv[])
{
  if (argc < 4) {
    usage(argv[0]);
    return 1;
  }

  string fname_vmdl = argv[1];
  string fname_rfv = argv[2];
  string fname_out = argv[3];

  ifstream in1(fname_vmdl, ios::binary);
  VMD body;
  body.input(in1);
  in1.close();

  ifstream in2(fname_rfv, ios::binary);
  VMD face;
  face.input(in2);
  in2.close();

  merge_vmd(body, face);

  ofstream out(fname_out, ios::binary);
  body.output(out);
  out.close();

  return 0;
}


#include <string>
#include <pthread.h>
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>

// #include <unitree_legged_msgs/LowCmd.h>
// #include <unitree_legged_msgs/LowState.h>

#include "unitree_legged_msgs/msg/low_cmd.hpp"
#include "unitree_legged_msgs/msg/low_state.hpp"

// #include "convert.h"
#include "unitree_legged_real/convert.h"
#include "rclcpp/rclcpp.hpp"

using namespace UNITREE_LEGGED_SDK;

template<typename TLCM>
void* update_loop(void* param)
{
    TLCM *data = (TLCM *)param;
    // while(ros::ok){
    while(rclcpp::ok()){
        data->Recv();
        usleep(2000);
    }
}

double jointLinearInterpolation(double initPos, double targetPos, double rate)
{
    double p;
    rate = std::min(std::max(rate, 0.0), 1.0);
    p = initPos*(1-rate) + targetPos*rate;
    return p;
}

template<typename TCmd, typename TState, typename TLCM>
int mainHelper(int argc, char *argv[], TLCM &roslcm)
{
    std::cout << "WARNING: Control level is set to LOW-level." << std::endl
              << "Make sure the robot is hung up." << std::endl
              << "Press Enter to continue..." << std::endl;
    std::cin.ignore();

    // ros::NodeHandle n;
    auto node = rclcpp::Node::make_shared("simple_node");

    // ros::Rate loop_rate(500);
    rclcpp::WallRate loop_rate(500);  // Hz

    long motiontime = 0;
    int rate_count = 0;
    int sin_count = 0;
    float qInit[3]={0};
    float qDes[3]={0};
    float sin_mid_q[3] = {0.0, 1.2, -2.0};
    float kp[3] = {0};
    float kd[3] = {0};
    TCmd SendLowLCM = {0};
    TState RecvLowLCM = {0};
    unitree_legged_msgs::msg::LowCmd SendLowROS;
    unitree_legged_msgs::msg::LowState RecvLowROS;

    bool initiated_flag = false;  // initiate need time
    int count = 0;

    roslcm.SubscribeState();

    pthread_t tid;
    pthread_create(&tid, NULL, update_loop<TLCM>, &roslcm);

    SendLowROS.level_flag = LOWLEVEL;
    for(int i = 0; i<12; i++){
        SendLowROS.motor_cmd[i].mode = 0x0A;   // motor switch to servo (PMSM) mode
        SendLowROS.motor_cmd[i].q = PosStopF;        // 禁止位置环
        SendLowROS.motor_cmd[i].kp = 0;
        SendLowROS.motor_cmd[i].dq = VelStopF;        // 禁止速度环
        SendLowROS.motor_cmd[i].kd = 0;
        SendLowROS.motor_cmd[i].tau = 0;
    }

    // while (ros::ok()){
    while (rclcpp::ok()){
        roslcm.Get(RecvLowLCM);
        RecvLowROS = ToRos(RecvLowLCM);
        printf("FR_2 position: %f\n",  RecvLowROS.motor_state[FR_2].q);

        if(initiated_flag == true){
            motiontime++;

            SendLowROS.motor_cmd[FR_0].tau = -0.65f;
            SendLowROS.motor_cmd[FL_0].tau = +0.65f;
            SendLowROS.motor_cmd[RR_0].tau = -0.65f;
            SendLowROS.motor_cmd[RL_0].tau = +0.65f;

            // printf("%d\n", motiontime);
            // printf("%d %f %f %f\n", FR_0, RecvLowROS.motor_state[FR_0].q, RecvLowROS.motor_state[FR_1].q, RecvLowROS.motor_state[FR_2].q);
            // printf("%f %f \n",  RecvLowROS.motor_state[FR_0].mode, RecvLowROS.motor_state[FR_1].mode);
            if( motiontime >= 0){
                // first, get record initial position
                // if( motiontime >= 100 && motiontime < 500){
                if( motiontime >= 0 && motiontime < 10){
                    qInit[0] = RecvLowROS.motor_state[FR_0].q;
                    qInit[1] = RecvLowROS.motor_state[FR_1].q;
                    qInit[2] = RecvLowROS.motor_state[FR_2].q;
                }
                if( motiontime >= 10 && motiontime < 400){
                    // printf("%f %f %f\n", );
                    rate_count++;
                    double rate = rate_count/200.0;                       // needs count to 200
                    kp[0] = 5.0; kp[1] = 5.0; kp[2] = 5.0;
                    kd[0] = 1.0; kd[1] = 1.0; kd[2] = 1.0;

                    qDes[0] = jointLinearInterpolation(qInit[0], sin_mid_q[0], rate);
                    qDes[1] = jointLinearInterpolation(qInit[1], sin_mid_q[1], rate);
                    qDes[2] = jointLinearInterpolation(qInit[2], sin_mid_q[2], rate);
                }
                double sin_joint1, sin_joint2;
                // last, do sine wave
                if( motiontime >= 400){
                    sin_count++;
                    sin_joint1 = 0.6 * sin(3*M_PI*sin_count/1000.0);
                    sin_joint2 = -0.6 * sin(1.8*M_PI*sin_count/1000.0);
                    qDes[0] = sin_mid_q[0];
                    qDes[1] = sin_mid_q[1];
                    qDes[2] = sin_mid_q[2] + sin_joint2;
                    // qDes[2] = sin_mid_q[2];
                }

                SendLowROS.motor_cmd[FR_0].q = qDes[0];
                SendLowROS.motor_cmd[FR_0].dq = 0;
                SendLowROS.motor_cmd[FR_0].kp = kp[0];
                SendLowROS.motor_cmd[FR_0].kd = kd[0];
                SendLowROS.motor_cmd[FR_0].tau = -0.65f;

                SendLowROS.motor_cmd[FR_1].q = qDes[1];
                SendLowROS.motor_cmd[FR_1].dq = 0;
                SendLowROS.motor_cmd[FR_1].kp = kp[1];
                SendLowROS.motor_cmd[FR_1].kd = kd[1];
                SendLowROS.motor_cmd[FR_1].tau = 0.0f;

                SendLowROS.motor_cmd[FR_2].q =  qDes[2];
                SendLowROS.motor_cmd[FR_2].dq = 0;
                SendLowROS.motor_cmd[FR_2].kp = kp[2];
                SendLowROS.motor_cmd[FR_2].kd = kd[2];
                SendLowROS.motor_cmd[FR_2].tau = 0.0f;
            }
        }

        SendLowLCM = ToLcm(SendLowROS, SendLowLCM);
        roslcm.Send(SendLowLCM);

        // ros::spinOnce();
        rclcpp::spin_some(node);

        loop_rate.sleep();

        count++;
        if(count > 10){
            count = 10;
            initiated_flag = true;
        }
    }
    return 0;
}

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

    // ros::init(argc, argv);
    rclcpp::init(argc, argv);

    UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL);
    mainHelper<UNITREE_LEGGED_SDK::LowCmd, UNITREE_LEGGED_SDK::LowState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm);
}


//
// Copyright 2005-2007 Adobe Systems Incorporated
//
// 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
//
#ifndef BOOST_GIL_EXTENSION_NUMERIC_ALGORITHM_HPP
#define BOOST_GIL_EXTENSION_NUMERIC_ALGORITHM_HPP

#include <boost/gil/extension/numeric/pixel_numeric_operations.hpp>

#include <boost/gil/metafunctions.hpp>
#include <boost/gil/pixel_iterator.hpp>

#include <boost/assert.hpp>

#include <algorithm>
#include <iterator>
#include <numeric>
#include <type_traits>

namespace boost { namespace gil {

/// \brief Reference proxy associated with a type that has a \p "reference" member type alias.
///
/// The reference proxy is the reference type, but with stripped-out C++ reference.
/// Models PixelConcept.
template <typename T>
struct pixel_proxy : std::remove_reference<typename T::reference> {};

/// \brief std::for_each for a pair of iterators
template <typename Iterator1, typename Iterator2, typename BinaryFunction>
BinaryFunction for_each(Iterator1 first1, Iterator1 last1, Iterator2 first2, BinaryFunction f)
{
    while (first1 != last1)
        f(*first1++, *first2++);
    return f;
}

template <typename SrcIterator, typename DstIterator>
inline
auto assign_pixels(SrcIterator src, SrcIterator src_end, DstIterator dst) -> DstIterator
{
    for_each(src, src_end, dst,
        pixel_assigns_t
        <
            typename pixel_proxy<typename std::iterator_traits<SrcIterator>::value_type>::type,
            typename pixel_proxy<typename std::iterator_traits<DstIterator>::value_type>::type
        >());
    return dst + (src_end - src);
}

namespace detail {

template <std::size_t Size>
struct inner_product_k_t
{
    template
    <
        class InputIterator1,
        class InputIterator2,
        class T,
        class BinaryOperation1,
        class BinaryOperation2
    >
    static T apply(
        InputIterator1 first1,
        InputIterator2 first2, T init,
        BinaryOperation1 binary_op1,
        BinaryOperation2 binary_op2)
    {
        init = binary_op1(init, binary_op2(*first1, *first2));
        return inner_product_k_t<Size - 1>::template apply(
            first1 + 1, first2 + 1, init, binary_op1, binary_op2);
    }
};

template <>
struct inner_product_k_t<0>
{
    template
    <
        class InputIterator1,
        class InputIterator2,
        class T,
        class BinaryOperation1,
        class BinaryOperation2
    >
    static T apply(
        InputIterator1 first1,
        InputIterator2 first2,
        T init,
        BinaryOperation1 binary_op1,
        BinaryOperation2 binary_op2)
    {
        return init;
    }
};

} // namespace detail

/// static version of std::inner_product
template
<
    std::size_t Size,
    class InputIterator1,
    class InputIterator2,
    class T,
    class BinaryOperation1,
    class BinaryOperation2
>
BOOST_FORCEINLINE
T inner_product_k(
    InputIterator1 first1,
    InputIterator2 first2,
    T init,
    BinaryOperation1 binary_op1,
    BinaryOperation2 binary_op2)
{
    return detail::inner_product_k_t<Size>::template apply(
        first1, first2, init, binary_op1, binary_op2);
}

/// \brief 1D un-guarded cross-correlation with a variable-size kernel
template
<
    typename PixelAccum,
    typename SrcIterator,
    typename KernelIterator,
    typename Size,
    typename DstIterator
>
inline
auto correlate_pixels_n(
    SrcIterator src_begin,
    SrcIterator src_end,
    KernelIterator kernel_begin,
    Size kernel_size,
    DstIterator dst_begin)
    -> DstIterator
{
    using src_pixel_ref_t = typename pixel_proxy
        <
            typename std::iterator_traits<SrcIterator>::value_type
        >::type;
    using dst_pixel_ref_t = typename pixel_proxy
        <
            typename std::iterator_traits<DstIterator>::value_type
        >::type;
    using kernel_value_t = typename std::iterator_traits<KernelIterator>::value_type;

    PixelAccum accum_zero;
    pixel_zeros_t<PixelAccum>()(accum_zero);
    while (src_begin != src_end)
    {
        pixel_assigns_t<PixelAccum, dst_pixel_ref_t>()(
            std::inner_product(
                src_begin,
                src_begin + kernel_size,
                kernel_begin,
                accum_zero,
                pixel_plus_t<PixelAccum, PixelAccum, PixelAccum>(),
                pixel_multiplies_scalar_t<src_pixel_ref_t, kernel_value_t, PixelAccum>()),
            *dst_begin);

        ++src_begin;
        ++dst_begin;
    }
    return dst_begin;
}

/// \brief 1D un-guarded cross-correlation with a fixed-size kernel
template
<
    std::size_t Size,
    typename PixelAccum,
    typename SrcIterator,
    typename KernelIterator,
    typename DstIterator
>
inline
auto correlate_pixels_k(
    SrcIterator src_begin,
    SrcIterator src_end,
    KernelIterator kernel_begin,
    DstIterator dst_begin)
    -> DstIterator
{
    using src_pixel_ref_t = typename pixel_proxy
        <
            typename std::iterator_traits<SrcIterator>::value_type
        >::type;
    using dst_pixel_ref_t = typename pixel_proxy
        <
            typename std::iterator_traits<DstIterator>::value_type
        >::type;
    using kernel_type = typename std::iterator_traits<KernelIterator>::value_type;

    PixelAccum accum_zero;
    pixel_zeros_t<PixelAccum>()(accum_zero);
    while (src_begin != src_end)
    {
        pixel_assigns_t<PixelAccum, dst_pixel_ref_t>()(
            inner_product_k<Size>(
                src_begin,
                kernel_begin,
                accum_zero,
                pixel_plus_t<PixelAccum, PixelAccum, PixelAccum>(),
                pixel_multiplies_scalar_t<src_pixel_ref_t, kernel_type, PixelAccum>()),
            *dst_begin);

        ++src_begin;
        ++dst_begin;
    }
    return dst_begin;
}

/// \brief destination is set to be product of the source and a scalar
/// \tparam PixelAccum - TODO
/// \tparam SrcView Models ImageViewConcept
/// \tparam DstView Models MutableImageViewConcept
template <typename PixelAccum, typename SrcView, typename Scalar, typename DstView>
inline
void view_multiplies_scalar(SrcView const& src_view, Scalar const& scalar, DstView const& dst_view)
{
    static_assert(std::is_scalar<Scalar>::value, "Scalar is not scalar");
    BOOST_ASSERT(src_view.dimensions() == dst_view.dimensions());
    using src_pixel_ref_t = typename pixel_proxy<typename SrcView::value_type>::type;
    using dst_pixel_ref_t = typename pixel_proxy<typename DstView::value_type>::type;
    using y_coord_t = typename SrcView::y_coord_t;

    y_coord_t const height = src_view.height();
    for (y_coord_t y = 0; y < height; ++y)
    {
        typename SrcView::x_iterator it_src = src_view.row_begin(y);
        typename DstView::x_iterator it_dst = dst_view.row_begin(y);
        typename SrcView::x_iterator it_src_end = src_view.row_end(y);
        while (it_src != it_src_end)
        {
            pixel_assigns_t<PixelAccum, dst_pixel_ref_t>()(
                pixel_multiplies_scalar_t<src_pixel_ref_t, Scalar, PixelAccum>()(*it_src, scalar),
                *it_dst);

            ++it_src;
            ++it_dst;
        }
    }
}

}} // namespace boost::gil

#endif


/*=============================================================================
    Copyright (c) 2001-2007 Joel de Guzman
    Copyright (c) 2001-2009 Hartmut Kaiser
    http://spirit.sourceforge.net/

    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)
=============================================================================*/
#if !defined(SPIRIT_REAL_IMPL_APR_18_2006_0901AM)
#define SPIRIT_REAL_IMPL_APR_18_2006_0901AM

#include <boost/config/no_tr1/cmath.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/spirit/home/support/unused.hpp>

namespace boost { namespace spirit { namespace qi  { namespace detail
{
    namespace
    {
        template <typename T>
        inline void
        scale_number(T const& exp, T& n)
        {
            using namespace std;    // allow for ADL to find the correct overload
            n *= pow(T(10), exp);
        }

        inline void
        scale_number(unused_type /*exp*/, unused_type /*n*/)
        {
            // no-op for unused_type
        }

        template <typename T>
        inline void
        scale_number(T const& exp, int frac, T& n)
        {
            scale_number(exp - T(frac), n);
        }

        inline void
        scale_number(unused_type /*exp*/, int /*frac*/, unused_type /*n*/)
        {
            // no-op for unused_type
        }

        template <typename T>
        inline T
        negate_number(bool neg, T const& n)
        {
            return neg ? -n : n;
        }

        inline unused_type
        negate_number(bool /*neg*/, unused_type n)
        {
            // no-op for unused_type
            return n;
        }

        template <typename T>
        inline bool
        number_equal_to_one(T const& value)
        {
            return value == 1.0;
        }

        inline bool
        number_equal_to_one(unused_type)
        {
            // no-op for unused_type
            return false;
        }
    }

#if BOOST_WORKAROUND(BOOST_MSVC, >= 1400)
# pragma warning(push)
# pragma warning(disable: 4100)   // 'p': unreferenced formal parameter
# pragma warning(disable: 4127)   // conditional expression is constant
#endif

    template <typename T, typename RealPolicies>
    struct real_impl
    {
        template <typename Iterator, typename Attribute>
        static bool
        parse(Iterator& first, Iterator const& last, Attribute& attr,
            RealPolicies const& p)
        {
            if (first == last)
                return false;
            Iterator save = first;

            // Start by parsing the sign. neg will be true if
            // we got a "-" sign, false otherwise.
            bool neg = p.parse_sign(first, last);

            // Now attempt to parse an integer
            Attribute n = 0;
            bool got_a_number = p.parse_n(first, last, n);

            // If we did not get a number it might be a NaN, Inf or a leading
            // dot.
            if (!got_a_number)
            {
                // Check whether the number to parse is a NaN or Inf
                if (p.parse_nan(first, last, attr) ||
                    p.parse_inf(first, last, attr))
                {
                    // If we got a negative sign, negate the number
                    attr = negate_number(neg, attr);
                    return true;    // got a NaN or Inf, return early
                }

                // If we did not get a number and our policies do not
                // allow a leading dot, fail and return early (no-match)
                if (!p.allow_leading_dot)
                {
                    first = save;
                    return false;
                }
            }

            bool e_hit = false;
            int frac_digits = 0;

            // Try to parse the dot ('.' decimal point)
            if (p.parse_dot(first, last))
            {
                // We got the decimal point. Now we will try to parse
                // the fraction if it is there. If not, it defaults
                // to zero (0) only if we already got a number.
                Iterator savef = first;
                if (p.parse_frac_n(first, last, n))
                {
                    // Optimization note: don't compute frac_digits if T is
                    // an unused_type. This should be optimized away by the compiler.
                    if (!is_same<T, unused_type>::value)
                        frac_digits =
                            static_cast<int>(std::distance(savef, first));
                }
                else if (!got_a_number || !p.allow_trailing_dot)
                {
                    // We did not get a fraction. If we still haven't got a
                    // number and our policies do not allow a trailing dot,
                    // return no-match.
                    first = save;
                    return false;
                }

                // Now, let's see if we can parse the exponent prefix
                e_hit = p.parse_exp(first, last);
            }
            else
            {
                // No dot and no number! Return no-match.
                if (!got_a_number)
                {
                    first = save;
                    return false;
                }

                // If we must expect a dot and we didn't see an exponent
                // prefix, return no-match.
                e_hit = p.parse_exp(first, last);
                if (p.expect_dot && !e_hit)
                {
                    first = save;
                    return false;
                }
            }

            if (e_hit)
            {
                // We got the exponent prefix. Now we will try to parse the
                // actual exponent. It is an error if it is not there.
                Attribute exp = 0;
                if (p.parse_exp_n(first, last, exp))
                {
                    // Got the exponent value. Scale the number by
                    // exp-frac_digits.
                    scale_number(exp, frac_digits, n);
                }
                else
                {
                    // Oops, no exponent, return no-match.
                    first = save;
                    return false;
                }
            }
            else if (frac_digits)
            {
                // No exponent found. Scale the number by -frac_digits.
                scale_number(Attribute(-frac_digits), n);
            }
            else if (number_equal_to_one(n))
            {
                // There is a chance of having to parse one of the 1.0#...
                // styles some implementations use for representing NaN or Inf.

                // Check whether the number to parse is a NaN or Inf
                if (p.parse_nan(first, last, attr) ||
                    p.parse_inf(first, last, attr))
                {
                    // If we got a negative sign, negate the number
                    attr = negate_number(neg, attr);
                    return true;    // got a NaN or Inf, return immediately
                }
            }

            // If we got a negative sign, negate the number
            attr = negate_number(neg, n);

            // Success!!!
            return true;
        }
    };

#if BOOST_WORKAROUND(BOOST_MSVC, >= 1400)
# pragma warning(pop)
#endif

}}}}

#endif