//-----------------------------------------------------------------------------
#include <utility>
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <vector>
#include <queue>
#include <set>
#include <map>
#include <string>
#include <cstdio>
#include <cstdlib>
#include <cmath>
#include <cstring>
#include <omp.h>
//-----------------------------------------------------------------------------
#include <ilcplex/ilocplex.h>
//-----------------------------------------------------------------------------
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/planar_face_traversal.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>
//-----------------------------------------------------------------------------
#define pb push_back
#define mp make_pair
#define MAXSTR 1024
#define MAX 100
#define EPS 1e-6

using namespace std;
using namespace boost;

#include "combinadic.h"

typedef pair<int, int> ii;
typedef adjacency_list< vecS, vecS, undirectedS,
    property<vertex_index_t, int>,
    property<edge_index_t, int> > Graph;
typedef vector< graph_traits<Graph>::edge_descriptor > vec_t;

bool found = false;
char errmsg[MAXSTR];

int nNodes = 0;
int nCols  = 0;
int cbSize = 4;

vector<vector<int>> graphIn;
map<vector<int>, int> colsId;

int sgn(double a) { return ((a > EPS) ? (1) : ((a < -EPS) ? (-1) : (0))); }
int cmp(double a, double b = 0.0) { return sgn(a - b); }

/*
    Print elapsed time.
    */
void printElapsedTime(double start, double stop)
{
    double elapsed = stop - start;
    printf("Elapsed time: %.3lfs.\n", elapsed);
}
//-----------------------------------------------------------------------------
// Mac
#ifdef __MACH__
#include <mach/clock.h>
#include <mach/mach.h>
#endif
//-----------------------------------------------------------------------------
/*  Get clock time.
    */
void current_utc_time(struct timespec *ts)
{
    #ifdef __MACH__ // OS X does not have clock_gettime, use clock_get_time
        clock_serv_t cclock;
        mach_timespec_t mts;
        host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock);
        clock_get_time(cclock, &mts);
        mach_port_deallocate(mach_task_self(), cclock);
        ts->tv_sec = mts.tv_sec;
        ts->tv_nsec = mts.tv_nsec;
    #else
        clock_gettime(CLOCK_REALTIME, ts);
    #endif
}

double getTime()
{
    timespec ts;
    current_utc_time(&ts);
    return double(ts.tv_sec) + double(ts.tv_nsec) / 1e9;
}
//-----------------------------------------------------------------------------
int faces_count = 0;
map< vector<int>, int > hasFace;
// Some planar face traversal visitors that will
// print the vertices and edges on the faces
struct output_visitor : public planar_face_traversal_visitor
{
    vector<int> faceVertices;
    void begin_face(){ printf("New face: "); }
    void end_face(){
        printf("\n");
        sort(faceVertices.begin(), faceVertices.end());
        hasFace[faceVertices] = 1;
        faceVertices.clear();
        faces_count++;
    }
};

struct vertex_output_visitor : public output_visitor
{
    template <typename Vertex>
    void next_vertex(Vertex v)
    {
        std::cout << v + 1 << " ";
        faceVertices.pb((int)v);
    }
};

struct edge_output_visitor : public output_visitor
{
    template <typename Edge>
    void next_edge(Edge e)
    {
        std::cout << e << " ";
    }
};
//-----------------------------------------------------------------------------
CPXENVptr env = NULL;
CPXLPptr  lp  = NULL;
//-----------------------------------------------------------------------------
void readData()
{
    printf("---------- Input graph ----------\n");
    //- - - - - - - - - - - - - - - - - - -
    scanf("%d", &nNodes);
    printf("%d\n", nNodes);
    // cbSize = nNodes-1;
    //- - - - - - - - - - - - - - - - - - -

    graphIn.resize( nNodes + 1 );
    for (int i = 1; i <= nNodes; i++)
    {
        graphIn[i].resize( nNodes + 1 );
    }

    //- - - - - - - - - - - - - - - - - - -
    for (int i = 1; i < nNodes; i++)
    {
        for (int j = i + 1; j <= nNodes; j++)
        {
            scanf("%d", &graphIn[i][j]);
            graphIn[j][i] = graphIn[i][j];
            printf("%d ", graphIn[i][j]);
        }
        printf("\n");
    }
    printf("---------------------------------\n\n");
    //- - - - - - - - - - - - - - - - - - -
}
//-----------------------------------------------------------------------------
int findColID( int i, int j, int k )
{
    map< vector<int>, int >::iterator itm;

    vector< int > v(3);
    v[0] = i;
    v[1] = j;
    v[2] = k;

    itm = colsId.find( v );
    if (itm == colsId.end())
    {
        return -1;
        // cout << "Cannot find colId x_" << j << '(' << h << ')' << '_' << k << '\n';
        // abort();
    }
    else
    {
        return itm->second;
    }
}
//-----------------------------------------------------------------------------
void buildModel()
{
    int status = 0;

    //- - - - - - - - - - - - - - - - - - -
    CPXchgobjsen( env, lp, CPX_MAX );

    nCols  = nNodes * (nNodes - 1) / 2;
    printf("nCols = %d\n", nCols);
    nCols += nNodes * (nNodes * nNodes - 3 * nNodes + 2) / 6;
    printf("nCols = %d\n", nCols);
    //- - - - - - - - - - - - - - - - - - -

    //- - - - - - - - - - - - - - - - - - -
    char    sense[2];
    int     rmatbeg[2];
    double  rhs[2];

    char*   ctype    = new char[nCols + 1];
    char**  cname    = new char*[nCols + 1];
    char**  rname    = new char*[1];
    rname[0] = new char[20];
    int*    rmatind  = new int[nCols + 1];
    double* rmatval  = new double[nCols + 1];
    double* obj      = new double[nCols + 1];
    double* lb       = new double[nCols + 1];
    double* ub       = new double[nCols + 1];
    //- - - - - - - - - - - - - - - - - - -

    //- - - - - - - - - - - - - - - - - - -
    int col = 0;
    for (int i = 1; i < nNodes; i++)
    {
        for (int j = i + 1; j <= nNodes; j++, col++)
        {
            vector<int> v(3);
            v[0] = i;
            v[1] = j;
            v[2] = 0;
            colsId[v] = col;

            cname[col] = new char[20];
            sprintf(cname[col], "x_%d_%d", i, j);
            lb[col]    = 0.0;
            ub[col]    = 1.0;
            ctype[col] = 'B';
            obj[col]   = graphIn[i][j];
        }
    }
    //- - - - - - - - - - - - - - - - - - -
    printf("col = %d\n", col);

    //- - - - - - - - - - - - - - - - - - -
    for (int i = 1; i < nNodes - 1; i++)
    {
        for (int j = i + 1; j < nNodes; j++)
        {
            for (int k = j + 1; k <= nNodes; k++, col++)
            {
                vector<int> v(3);
                v[0] = i;
                v[1] = j;
                v[2] = k;
                colsId[v] = col;

                cname[col] = new char[20];
                sprintf(cname[col], "f_%d_%d_%d", i, j, k);
                lb[col]    = 0.0;
                ub[col]    = 1.0;
                ctype[col] = 'B';
                obj[col]   = 0.0;
            }
        }
    }
    //- - - - - - - - - - - - - - - - - - -
    printf("col = %d\n", col);

    //- - - - - - - - - - - - - - - - - - -
    if (status = CPXnewcols( env, lp, nCols, obj, lb, ub, ctype, (char **) cname ))
    {
        printf("CPXnewcols: Could not add new columns, error %d\n", status);
        abort();
    }
    //- - - - - - - - - - - - - - - - - - -
    printf("CCCCCC\n");

    //R7 - - - - - - - - - - - - - - - - - - -
    rhs[0]     = 3.0 * (nNodes - 2);
    sense[0]   = 'E';
    rmatbeg[0] = 0;

    int p = 0;
    for (int i = 1; i < nNodes; i++)
    {
        for (int j = i + 1; j <= nNodes; j++, p++)
        {
            rmatind[p] = p;
            rmatval[p] = 1.0;
        }
    }
    sprintf(rname[0], "R7");

    if (status = CPXaddrows( env, lp,
                            (int) 0, (int) 1, p,
                            (const double *) rhs, (const char *) sense,
                            (const int *) rmatbeg, (const int *) rmatind,
                            (const double *) rmatval,
                            NULL, (char **) rname ))
    {
        printf("CPXaddrows: Could not add new rows (%s)\n", rname);
        abort();
    }
    printf("777777\n");
    //R7 - - - - - - - - - - - - - - - - - - -

    //R8 - - - - - - - - - - - - - - - - - - -
    rhs[0]   = 0.0;
    sense[0] = 'E';
    rmatbeg[0] = 0;

    for (int i = 1; i < nNodes; i++)
    {
        for (int j = i + 1; j <= nNodes; j++)
        {
            p = 0;
            for (int k = 1; k < i; k++, p++)
            {
                rmatind[p] = findColID( k, i, j );;
                rmatval[p] = 1.0;
            }
            for (int k = i + 1; k < j; k++, p++)
            {
                rmatind[p] = findColID( i, k, j );;
                rmatval[p] = 1.0;
            }
            for (int k = j + 1; k <= nNodes; k++, p++)
            {
                rmatind[p] = findColID( i, j, k );;
                rmatval[p] = 1.0;
            }
            rmatind[p] = findColID( i, j, 0 );;
            rmatval[p] = -2.0;
            p++;

            sprintf(rname[0], "R8_%d_%d", i, j);

            if (status = CPXaddrows( env, lp,
                                    (int) 0, (int) 1, p,
                                    (const double *) rhs, (const char *) sense,
                                    (const int *) rmatbeg, (const int *) rmatind,
                                    (const double *) rmatval,
                                    NULL, (char **) rname ))
            {
                printf("CPXaddrows: Could not add new rows (%s)\n", rname);
                abort();
            }
        }
    }
    printf("888888\n");
    //R8 - - - - - - - - - - - - - - - - - - -

    //R9 - - - - - - - - - - - - - - - - - - -
    rhs[0]     = 3.0;
    sense[0]   = 'G';
    rmatbeg[0] = 0;

    for (int i = 1; i <= nNodes; i++)
    {
        p = 0;
        for (int j = 1; j < i; j++, p++)
        {
            rmatind[p] = findColID( j, i, 0 );
            rmatval[p] = 1.0;
        }
        for (int j = nNodes; j > i; j--, p++)
        {
            rmatind[p] = findColID( i, j, 0 );
            rmatval[p] = 1.0;
        }
        sprintf(rname[0], "R9_%d", i);

        if (status = CPXaddrows( env, lp,
                                (int) 0, (int) 1, p,
                                (const double *) rhs, (const char *) sense,
                                (const int *) rmatbeg, (const int *) rmatind,
                                (const double *) rmatval,
                                NULL, (char **) rname ))
        {
            printf("CPXaddrows: Could not add new rows (%s)\n", rname);
            abort();
        }
    }
    printf("999999\n");
    //R9  - - - - - - - - - - - - - - - - - - -

    //R10 - - - - - - - - - - - - - - - - - - -
    rhs[0]     = 0.0;
    sense[0]   = 'L';
    rmatbeg[0] = 0;

    for (int i = 1; i < nNodes - 1; i++)
    {
        for (int j = i + 1; j < nNodes; j++)
        {
            for (int k = j + 1; k <= nNodes; k++)
            {
                p = 0;
                rmatind[p] = findColID( i, j, k );;
                rmatval[p] = 1.0;
                p++;
                rmatind[p] = findColID( i, j, 0 );;
                rmatval[p] = -1.0;
                p++;

                sprintf(rname[0], "R10_%d_%d_%d", i, j, k);

                if (status = CPXaddrows( env, lp,
                                        (int) 0, (int) 1, p,
                                        (const double *) rhs, (const char *) sense,
                                        (const int *) rmatbeg, (const int *) rmatind,
                                        (const double *) rmatval,
                                        NULL, (char **) rname ))
                {
                    printf("CPXaddrows: Could not add new rows (%s)\n", rname);
                    abort();
                }
            }
        }
    }
    printf("101010\n");
    //R10 - - - - - - - - - - - - - - - - - - -

    //R11 - - - - - - - - - - - - - - - - - - -
    rhs[0]     = 0.0;
    sense[0]   = 'L';
    rmatbeg[0] = 0;

    for (int i = 1; i < nNodes - 1; i++)
    {
        for (int j = i + 1; j < nNodes; j++)
        {
            for (int k = j + 1; k <= nNodes; k++)
            {
                p = 0;
                rmatind[p] = findColID( i, j, k );;
                rmatval[p] = 1.0;
                p++;
                rmatind[p] = findColID( j, k, 0 );;
                rmatval[p] = -1.0;
                p++;

                sprintf(rname[0], "R11_%d_%d_%d", i, j, k);

                if (status = CPXaddrows( env, lp,
                                        (int) 0, (int) 1, p,
                                        (const double *) rhs, (const char *) sense,
                                        (const int *) rmatbeg, (const int *) rmatind,
                                        (const double *) rmatval,
                                        NULL, (char **) rname ))
                {
                    printf("CPXaddrows: Could not add new rows (%s)\n", rname);
                    abort();
                }
            }
        }
    }
    printf("111111\n");
    //R11 - - - - - - - - - - - - - - - - - - -

    //R12 - - - - - - - - - - - - - - - - - - -
    rhs[0]     = 0.0;
    sense[0]   = 'L';
    rmatbeg[0] = 0;

    for (int i = 1; i < nNodes - 1; i++)
    {
        for (int j = i + 1; j < nNodes; j++)
        {
            for (int k = j + 1; k <= nNodes; k++)
            {
                p = 0;
                rmatind[p] = findColID( i, j, k );;
                rmatval[p] = 1.0;
                p++;
                rmatind[p] = findColID( i, k, 0 );;
                rmatval[p] = -1.0;
                p++;

                sprintf(rname[0], "R12_%d_%d_%d", i, j, k);

                if (status = CPXaddrows( env, lp,
                                        (int) 0, (int) 1, p,
                                        (const double *) rhs, (const char *) sense,
                                        (const int *) rmatbeg, (const int *) rmatind,
                                        (const double *) rmatval,
                                        NULL, (char **) rname ))
                {
                    printf("CPXaddrows: Could not add new rows (%s)\n", rname);
                    abort();
                }
            }
        }
    }
    printf("121212\n");
    //R12 - - - - - - - - - - - - - - - - - - -

    //- - - - - - - - - - - - - - - - - - -
    delete[] rmatind;

    delete[] rmatval;
    delete[] obj;
    delete[] lb;
    delete[] ub;
    delete[] ctype;

    for (int i = 0; i <= nCols; i++)
    {
        delete[] cname[i];
    }
    delete[] cname;

    delete[] rname[0];
    delete[] rname;
    //- - - - - - - - - - - - - - - - - - -
}
//-----------------------------------------------------------------------------
void openCplex()
{
    int status = 0;

    if (!(env = CPXopenCPLEX( &status )))
    {
        printf("Could not open CPLEX environment.\n");
        CPXgeterrorstring( env, status, errmsg );
        printf("%s\n", errmsg);
        abort();
    }

    if (status = CPXsetintparam( env, CPX_PARAM_SCRIND, CPX_ON ))
    {
        printf("PARAM_SCRIND: Failure to turn on screen indicator, error %d\n",
            status);
        abort();
    }

    if (status = CPXsetintparam (env, CPX_PARAM_SIMDISPLAY, 2))
    {
        printf("PARAM_SIMDISPLAY: Failed to turn up simplex display level.\n");
        abort();
    }

    if (status = CPXsetintparam( env, CPX_PARAM_DATACHECK, CPX_ON ))
    {
        printf("PARAM_DATACHECK: Failure to turn on data checking, error %d\n",
            status);
        abort();
    }

    if (!(lp = CPXcreateprob( env, &status, "PLANAR" )))
    {
        printf("Failed to create LP PLANAR.\n");
        abort();
    }
}
//-----------------------------------------------------------------------------
void writeSolutions()
{
    int    solStat  = 0;
    double objValue = 0.0;

    double* x = new double[nCols + 1];
    if (x == NULL)
    {
        printf("Could not allocate memory for solution.\n");
        abort();
    }

    int status = CPXsolution( env, lp, &solStat, &objValue, x, NULL, NULL, NULL);
    if (status)
    {
        printf("Failed to obtain cplex solution.\n");
        abort();
    }

    printf("\nSolution status = %d\n", solStat);
    printf("Solution value  = %.0lf\n\n", objValue);

    //- - - - - - - - - - - - - - - - - - -
    int col = 0;
    for (int i = 1; i < nNodes; i++)
    {
        for (int j = i + 1; j <= nNodes; j++, col++)
        {
            if (cmp(x[col]) == 1)
            {
                printf("x(%d,%d) = %0.lf\n", i, j, x[col]);
            }
        }
    }

    for (int i = 1; i < nNodes - 1; i++)
    {
        for (int j = i + 1; j < nNodes; j++)
        {
            for (int k = j + 1; k <= nNodes; k++, col++)
            {
                if (cmp(x[col]) == 1)
                {
                    printf("f(%d,%d, %d) = %0.lf\n", i, j, k, x[col]);
                }
            }
        }
    }
    //- - - - - - - - - - - - - - - - - - -
    delete[] x;
    //- - - - - - - - - - - - - - - - - - -
}
//-----------------------------------------------------------------------------
void addCut( vector<int>& S )
{
    int status = 0;
    static int r13 = 1;

    //- - - - - - - - - - - - - - - - - - -
    char    sense[2];
    int     rmatbeg[2];
    double  rhs[2];

    char**  rname    = new char*[1];
    rname[0]         = new char[20];
    int*    rmatind  = new int[nCols + 1];
    double* rmatval  = new double[nCols + 1];
    //- - - - - - - - - - - - - - - - - - -

    //- - - - - - - - - - - - - - - - - - -
    rhs[0]   = 2 * (S.size() - 2) - 1;
    sense[0] = 'L';
    rmatbeg[0] = 0;

    int p = 0;
    for (int i = 1; i < S.size() - 1; i++)
    {
        for (int j = i + 1; j < S.size(); j++)
        {
            for (int k = j + 1; k <= S.size(); k++)
            {
                vector<int> tmpVertices;
                tmpVertices.pb(S[i-1]); tmpVertices.pb(S[j-1]); tmpVertices.pb(S[k-1]);
                if (hasFace.count(tmpVertices))
                {
                    rmatind[p] = findColID( S[i-1]+1, S[j-1]+1, S[k-1]+1 );
                    rmatval[p] = 1.0;
                    p++;
                }
            }
        }
    }

    sprintf(rname[0], "R13_%d", r13++);

    if (status = CPXaddrows( env, lp,
                            (int) 0, (int) 1, p,
                            (const double *) rhs, (const char *) sense,
                            (const int *) rmatbeg, (const int *) rmatind,
                            (const double *) rmatval,
                            NULL, (char **) rname ))
    {
        printf("CPXaddrows: Could not add new rows (%s)\n", rname);
        abort();
    }
    //- - - - - - - - - - - - - - - - - - -

    //- - - - - - - - - - - - - - - - - - -
    delete[] rmatind;
    delete[] rmatval;
    delete[] rname[0];
    delete[] rname;
    //- - - - - - - - - - - - - - - - - - -

}
//-----------------------------------------------------------------------------
//vector<ii> getEdges()
map<ii, bool> getEdges()
{
    int    solStat  = 0;
    double objValue = 0.0;

    double* x = new double[nCols + 1];
    if (x == NULL)
    {
        printf("Could not allocate memory for solution.\n");
        abort();
    }

    int status = CPXsolution(env, lp, &solStat, &objValue, x, NULL, NULL, NULL);
    if (status)
    {
        printf("Failed to obtain cplex solution.\n");
        abort();
    }

    // vector<ii> resp;
    map<ii, bool> resp;
    //- - - - - - - - - - - - - - - - - - -
    int col = 0;
    for (int i = 1; i < nNodes; i++)
    {
        for (int j = i + 1; j <= nNodes; j++, col++)
        {
            if (cmp(x[col]) == 1)
            {
                // resp.pb(mp(i, j));
                resp[mp(i, j)] = true;
            }
        }
    }
    delete [] x;
    return resp;
}
//-----------------------------------------------------------------------------
//Solution obtained from the relaxed model.
map<ii, bool> sol;
//Was this combination of edges used as a restriction already?
map<vector<ii>, int> cutFound;
//A list having MPGs used as restrictions.
map<int64, vector<ii>> edgesListM;    //smallest
vector<vector<ii>> edgesListV;        //smpg

/*
    Add a set of edges that generates a cut.
    edgesIdx    ---> Set of edges
    t           ---> Iteration number
*/
void findCut(vector<ii> edgesIdx, int t)
{
    //get num of vertices
    //m = 3n - 6; n = (m+6)/3
    int graphSize = (edgesIdx.size()+6)/3;
    vector<int> S;
    set<int> V;

    //create the graph which will generate a restriction 13
    Graph tmp_planar(graphSize);
    for (int i = 0; i < edgesIdx.size(); ++i)
    {
        int u = edgesIdx[i].first, v = edgesIdx[i].second;
        if (!V.count(u))
        {
            S.pb(u);
            V.insert(u);
        }
        if (!V.count(v))
        {
            S.pb(v);
            V.insert(v);
        }
        add_edge(u, v, tmp_planar);
    }
    //Necessary.
    sort(S.begin(), S.end());

    //Initialize the interior edge index; necessary for face traversal.
    property_map<Graph, edge_index_t>::type e_index = get(edge_index, tmp_planar);
    graph_traits<Graph>::edges_size_type edge_count = 0;
    graph_traits<Graph>::edge_iterator ei, ei_end;
    for (tie(ei, ei_end) = edges(tmp_planar); ei != ei_end; ++ei)
        put(e_index, *ei, edge_count++);

    vector<vec_t> embedding(num_vertices(tmp_planar));
    if (boyer_myrvold_planarity_test(tmp_planar, &embedding[0]))
    {
        //Clear the face set.
        hasFace.clear();

        vertex_output_visitor v_vis;
        printf("---------- Adding cut ----------\n");
        planar_face_traversal(tmp_planar, &embedding[0], v_vis);
        printf("Number of faces = %d\n", faces_count);
        printf("Number of edges = %d\n", num_edges(tmp_planar));
        printf("--------------------------------\n");
        faces_count = 0;

        cutFound[edgesIdx]++;
        addCut(S);
    }
}
//-----------------------------------------------------------------------------
/*
    n       ---> Size of the input array
    k       ---> Size of the combination
    app     ---> Approach option
*/
void combine(int n, int k, int app)
{
    Combination c(n, k);
    int64 ub = c.choose(n, k);
    #pragma omp parallel for shared(edgesListM, edgesListV)
    for (int64 i = 0; i < ub; i++)
    {
        Graph tmp_planar(k);

        vector<int> S = c.element(i).getArray();
        vector<ii> edges;

        for (int vertex = 0; vertex < S.size()-1; vertex++)
        {
            int u = S[vertex];
            for (int adj = vertex+1; adj < S.size(); adj++)
            {
                int v = S[adj];
                //add an edge if (u, v) belongs to the solution found
                if (!sol.count(mp(u+1, v+1))) continue;
                edges.pb(mp(u, v));
                add_edge(u, v, tmp_planar);
            }
        }
        //Was this restriction inserted already?
        if (cutFound.count(edges)) continue;

        //Cannot be maximal
        if (num_edges(tmp_planar) != 3*S.size()-6) continue;

        //Is it planar?
        if (boyer_myrvold_planarity_test(tmp_planar))
        {
            #pragma omp critical
            {
                if (app) edgesListM[i] = edges;
                else edgesListV.pb(edges);
            }
        }
    }
}
//-----------------------------------------------------------------------------
void bcSmallest(int app)
{
    // mipBasis();
    int t = 0;
    while (true)
    {
        if (CPXmipopt( env, lp ))
        {
            printf("CPXmipopt: Failed to optimize LP.\n");
            abort();
        }
        char name[20];
        sprintf(name, "planar-%d.lp", t);
        CPXwriteprob(env, lp, name, NULL);

        sol = getEdges();
        map<ii, bool>::iterator ed;
        writeSolutions();

        //Build the solution graph.
        Graph planar;
        for (ed = sol.begin(); ed != sol.end(); ed++)
        {
            ii at = ed->first;
            int a = at.first-1, b = at.second-1;
            add_edge(a, b, planar);
        }

        if (boyer_myrvold_planarity_test(planar))
        {
            printf("Maximal Planar Subgraph found!\n");
            break;
        }
        else
        {
            //S initial size must be at least 4.
            edgesListM.clear();
            cbSize = 4;
            found = true;
            while (cbSize < nNodes)
            {
                double st = getTime();
                printf("Combination size = %d\n", cbSize);
                combine(nNodes, cbSize, app);
                double fn = getTime();
                printElapsedTime(st, fn);

                if (edgesListM.size()) break;
                cbSize++;
            }
            if (edgesListM.size() == 0)
            {
                printf("There is no MPG for restriction 13.\n");
                return;
            }
            //Use the first that was found.
            findCut(edgesListM.begin()->second, t);
        }
        // char m_name[20];
        // sprintf(m_name, "MIPS-%d", t);
        // CPXwritemipstarts(env, lp, m_name, 0, 0);
        t++;
    }
}
//-----------------------------------------------------------------------------
void bcSMPG(int app)
{
    // mipBasis();
    int t = 0;
    while (true)
    {
        if (CPXmipopt( env, lp ))
        {
            printf("CPXmipopt: Failed to optimize LP.\n");
            abort();
        }
        char name[20];
        sprintf(name, "planar-%d.lp", t);
        CPXwriteprob(env, lp, name, NULL);

        sol = getEdges();
        map<ii, bool>::iterator ed;
        writeSolutions();

        //Build the solution graph.
        Graph planar;
        for (ed = sol.begin(); ed != sol.end(); ed++)
        {
            ii at = ed->first;
            int a = at.first-1, b = at.second-1;
            add_edge(a, b, planar);
        }

        if (boyer_myrvold_planarity_test(planar))
        {
            printf("Maximal Planar Subgraph found!\n");
            break;
        }
        else
        {
            edgesListV.clear();
            cbSize = 4;
            found = true;
            while (cbSize < nNodes)
            {
                double st = getTime();
                printf("Combination size = %d\n", cbSize);
                combine(nNodes, cbSize, app);
                double fn = getTime();
                printElapsedTime(st, fn);
                cbSize++;
            }
            if (edgesListV.size() == 0)
            {
                printf("There is no MPG for restriction 13.\n");
                return;
            }

            //Having all MPGs generated, we have to get the smallest one
            //which is not a subgraph of any other.
            int idx = edgesListV.size()-1;
            vector<ii>::iterator vit;
            for (int i = 0; i < edgesListV.size()-1; i++)
            {
                found = true;
                for (int j = i+1; j < edgesListV.size(); j++)
                {
                    if (edgesListV[i].size() == edgesListV[j].size()) continue;

                    vector<ii> tmp(edgesListV[j].size());
                    vit = set_intersection(edgesListV[i].begin(), edgesListV[i].end(),
                        edgesListV[j].begin(), edgesListV[j].end(), tmp.begin());

                    //If the smallest subgraph is a subgraph of any other,
                    //then it cannot be a restriction.
                    int sz = (int)(vit-tmp.begin());
                    if (sz == edgesListV[i].size())
                    {
                        found = false;
                        break;
                    }
                }
                if (found)
                {
                    idx = i;
                    break;
                }
            }
            //Use the smallest as the best cut.
            findCut(edgesListV[idx], t);
        }
        t++;
    }
}
//-----------------------------------------------------------------------------
int main(int argv, char** argc)
{
    // ios::sync_with_stdio(false);
    if (argv < 2)
    {
        printf("ERROR! Required num. of arguments: 2\n");
        printf("Try:\nsmallest - ./a.out 1\nsmpg     - ./a.out 0\n");
        return 0;
    }

    int status;

    readData();
    openCplex();

    if (argv == 2)
    {
        buildModel();
    }
    else if (argv == 3)
    {
        if (status = CPXreadcopyprob(env, lp, argc[2], "LP"))
        {
            printf("Error! Could not read file!\n");
            exit(1);
        }
    }

    int option = atoi(argc[1]);

    if (option) bcSmallest(option);
    else bcSMPG(option);

    if (status = CPXwriteprob(env, lp, "planar.lp", NULL))
    {
        printf("CPXwriteprob: Failed to write LP to disk, error %d\n",
            status);
    }

    writeSolutions();

    if (status = CPXfreeprob(env, &lp))
    {
        printf("CPXfreeprob failed, error code %d\n", status);
    }

    if (status = CPXcloseCPLEX(&env))
    {
        CPXgeterrorstring(env, status, errmsg);
        printf("CPXcloseCPLEX: Could not close CPLEX environment.\n");
        printf("%s\n", errmsg);
    }
    //- - - - - - - - - - - - - - - - - - -

    return 0;
}
//-----------------------------------------------------------------------------
void mipBasis()
{
    int status = 0;

    // int cstat[nCols + 1];
    // int rstat[nRows + 1];

    int nFaces, x, nzcnt = 0, effortlevel = 0;
    int beg = 0;

    vector<int> varindices;
    vector<double> values;
    for (int i = 1; i < nNodes; i++)
    {
        for (int j = i + 1; j <= nNodes; j++)
        {
            scanf("%d", &x);
            vector<int> tmp;
            tmp.pb(i); tmp.pb(j); tmp.pb(0);

            varindices.pb(colsId[tmp]);
            values.pb(x ? 1.0 : 0.0);
            nzcnt++;
        }
    }

    map< vector<int>, int > sFaces;
    scanf("%d", &nFaces);
    for (int i = 0; i < nFaces; i++)
    {
        vector<int> sF;
        for (int j = 0; j < 3; j++)
        {
            scanf("%d", &x);
            sF.pb(x+1);
        }
        sort(sF.begin(), sF.end());
        sFaces[sF] = 1;
    }

    for (int i = 1; i < nNodes - 1; i++)
    {
        for (int j = i + 1; j < nNodes; j++)
        {
            for (int k = j + 1; k <= nNodes; k++)
            {
                vector<int> tmp;
                tmp.pb(i); tmp.pb(j); tmp.pb(k);

                varindices.pb(colsId[tmp]);
                values.pb(sFaces.count(tmp) ? 1.0 : 0.0);
                nzcnt++;
            }
        }
    }
    printf("Result: %d\n", nzcnt);

    int arr_varindices[nzcnt];
    double arr_values[nzcnt];
    copy(varindices.begin(), varindices.end(), arr_varindices);
    copy(values.begin(), values.end(), arr_values);

    /* Now copy the mip start */

    CPXsetintparam( env, CPX_PARAM_ADVIND, 1 );

    if (status = CPXaddmipstarts (env, lp, 1, nzcnt, &beg, arr_varindices,
        arr_values, &effortlevel, NULL))
    {
        printf("CPXaddmipstarts: Could not add mip start.\n");
        abort();
    }

    CPXwritemipstarts(env, lp, "MIPS", 0, 0);
}
//-----------------------------------------------------------------------------


#include "highgui.h"
#include "imgproc.h"
#include "feature.h"
#include "draw.h"
#include <armadillo>
#include <iostream>
#include <opencv2/highgui/highgui.hpp>

using namespace arma;
using namespace std;

int main() {
  srand(getpid());
  //cube I = load_image("600px-Line_with_outliers.svg.png");
  cube I = load_image("twolines.bmp");
  //cube I = load_image("Hough_Lines_Tutorial_Original_Image.jpg");
  mat E = canny2(rgb2gray(I), 7, 3.0);
  E = ((E % (E > 0.01)) > 0) % ones(E.n_rows, E.n_cols);

  // after we get the image, try to resize it
  double scale_factor = 1.0;
  double cc = E(0,0);
  uvec ind = find(E != cc);
  vector<vec> A;
  for (uword i : ind) {
    vec index = { (double)(i % E.n_rows), (double)(i / E.n_rows) };
    A.push_back(index);
  }

  disp_image("original", I);
  disp_image("canny", E);
  disp_wait();

  mat R = lines2(E, A);

  // filter out to only get 0.5 or higher radius
  int max_count = -1;
  for (uword i = 0; i < R.n_cols; i++) {
    if (R(0,i) < 0.5) {
      break;
    }
    max_count = (int)i;
  }
  if (max_count > -1) {
    R = R.cols(0,max_count);
  } else {
    R = mat();
  }

  // select the highest one and show it
  cube L = gray2rgb(E);

  for (int k = 0; k < R.n_cols; k++) {
    double theta = R(2, k) * M_PI / 180.0;
    double b = R(1, k) / sin(theta);
    double m = -cos(theta) / sin(theta);
    double y1 = b;
    double y2 = m*I.n_cols + b;
    draw_line(L, vec({ 0, 0, 1 }), {y1, 0}, {y2, (double)I.n_cols});
  }

  disp_image("hough_lines", L);
  disp_wait();
  return 0;
}


/*! \file
  \brief Functors to convert data to doubles.
  \details SVG plot assumes all data are convertible to double or uncertain value type unc before being plotted.
    The functors are used to convert both 1D and 2D (pairs of data values) to be converted.
    Note that uncertain value class unc only holds double precision so higher precision data type
    will therefore lose information.  This seems a reasonable design decision as any real data
    to be plotted is unlikely to have more than double precision (about 16 decimal digits).

    "svg_plot\example\convertible_to_double.cpp" demonstrates that built-in types
    @c float, @c double, @c long double @ Boost.Multiprecision @c cpp_bin_float_quad
    work as expected, as well as a sample User Defined Type (UDT) a fixed-point type.

    Types that cannot be converted to double nor constructible from double provoke a compile-time message:
    "Uncertain types must be convertible to double!"

    (This uses checks using http://www.cplusplus.com/reference/type_traits/is_constructible/
      @c BOOST_STATIC_ASSERT_MSG(std::is_constructible<T, double>::value, "Uncertain types must be convertible to double!");
     ).
*/

// Copyright Jacob Voytko 2007
// Copyright Paul A. Bristow 2009, 2012, 2013, 2018, 2021

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

#ifndef BOOST_SVG_DETAIL_FUNCTORS_HPP
#define BOOST_SVG_DETAIL_FUNCTORS_HPP

#include <boost/quan/unc.hpp>
#include <boost/quan/meas.hpp>

namespace boost {
namespace svg {
namespace detail
{
  using namespace boost::quan;

  /*! \class boost::svg::detail::double_1d_convert
     \brief This functor allows any 1D data that can be converted to @c double to be plotted.
 */
class double_1d_convert
{
public:
    typedef double result_type; //!< result type is @c double.

    //! To convert a single data value to double.
    //! \tparam T Any type that can be converted to double.
    //! \returns A single @c double data value.
    template <class T>
    double operator()(T val) const
    {
      BOOST_STATIC_ASSERT_MSG(std::is_constructible<T, double>::value, "Uncertain types must be convertible to double!");

       return static_cast<double>(val); //! \return Value that has been converted to double.
    }
}; // class double_1d_convert

 /*! \class boost::svg::detail::unc_1d_convert
      \brief This functor allows any 1D data that can be converted to @c boost::quan::unc (uncertain double) to be plotted.
      \details Defaults provided by the @c unc class constructor ensure that
        uncertainty, degrees of freedom information, and uncertain type are suitably set too.
*/
template <bool correlated>
class unc_1d_convert
{
public:
    typedef unc<correlated> result_type; //!< @c result_type is an uncertain floating-point type.

    /*!< Convert to uncertain type,
      providing defaults for uncertainty,  degrees of freedom information, and type (meaning undefined).
      \return value including uncertainty information.
       \tparam T Any data type with a value that can be converted to double, for example: @c double, @c uncun, @c Meas.
      */
    template <class T>
    unc<correlated> operator()(T val) const
    {
      return (result_type)val;
      /*! \return uncertain type (uncertainty, degrees of freedom information, and type meaning undefined). */
    }
}; // template <bool correlated> class default_1d_convert

/*! \class boost::svg::detail::meas_1d_convert
      \brief This functor allows any 1D data that can be converted to measurements
      (with uncertain doubles) to be plotted.
      \details Defaults provided by the meas class constructor ensure that
        uncertainty, degrees of freedom information, type, and order, timestamp and id are suitably set too.
    \tparam T Any data type with a value that can be converted to double, for example: double, unc, Meas.
    \return uncertain type (uncertainty, degrees of freedom information, and type meaning undefined).
*/
class meas_1d_convert
{
public:
    typedef Meas result_type; //!< result type includes an uncertain floating-point type.

    template <class T>
    Meas operator()(T val) const
    /*!< Convert to Meas type,
      providing defaults for uncertainty, degrees of freedom information, and uncertain type.
      \return value including uncertainty and other information.
    */
    {
      BOOST_STATIC_ASSERT_MSG(std::is_constructible<T, double>::value, "Uncertain types must be that can be converted to double!");
      return static_cast<result_type>(val);
    }
}; // class default_1d_convert

class pair_double_2d_convert
{ /*! \class boost::svg::detail::pair_double_2d_convert
      \brief This functor allows any 2-D data that can be converted to type @c std::pair<double, double> to be plotted.
     Convert a pair of X and Y (whose types can be converted to double values) to a pair of @c doubles.
     \tparam T type whose value can be converted to @c double.
     \tparam U type whose value can be converted to @c  double.
     \returns @c std::pair of @c double data point values.
  */
public:
    typedef std::pair<double, double> result_type; //!< result type is a pair (X and Y) of doubles.

    double i; //!< Current value, 1st set by start(double i0).
    void start(double i0)
    { //! Set a start value.
      i = i0;
    }

    template <typename T, typename U>
    std::pair<double, double> operator()(const std::pair<T, U>& a) const
    { //! Assumes that a conversion from double yields just the value component of the uncertain value.
      BOOST_STATIC_ASSERT_MSG(std::is_constructible<T, double>::value, "Uncertain types must be convertible to double!");
      BOOST_STATIC_ASSERT_MSG(std::is_constructible<U, double>::value, "Uncertain types must be convertible to double!");
      return std::pair<double, double>(static_cast<double>(a.first), static_cast<double>(a.second));
    }

    template <typename T>
    std::pair<double, double> operator()(T a)
    {  //! Convert a pair of X and Y values to a @c std::pair of @c doubles.
       //! \return @c std::pair of doubles.
        return std::pair<double, double>(i++, static_cast<double>(a));
    }
}; // class pair_double_2d_convert

template <bool correlated>
class pair_unc_2d_convert
{ /*! \class boost::svg::detail::pair_unc_2d_convert
      \brief This functor allows any 2D data that can be converted to type @c std::pair<unc, unc> to be plotted.
*/
public:
    typedef std::pair<unc<correlated>, unc<correlated> > result_type; //!< result type is pair of uncertain values.

    unc<correlated> i;  //!< Current uncertain value, 1st set by start(double i0).

    void start(unc<correlated> i0)
    { //!< Set a start value.
       i = i0;
    }

    //!< \tparam T type that can be converted to double.
    //!< \tparam U type that can be converted to double.
    //! \returns A @c std::pair of double data point values.
    template <class T, class U>
    std::pair<unc<correlated>, unc<correlated> > operator()(const std::pair<T, U>& a) const
    { //!< Convert a pair of X and Y uncertain type values to a pair of @c doubles.
      //! Cast to double so that can use with float, long double and UDTs.
      //! \return @c std::pair of uncs.
      BOOST_STATIC_ASSERT_MSG(std::is_constructible<T, double>::value, "Uncertain types must be convertible to double!");
      BOOST_STATIC_ASSERT_MSG(std::is_constructible<U, double>::value, "Uncertain types must be convertible to double!");
       return std::pair<unc<correlated>, unc<correlated> >(
         (unc<correlated>)(a.first), (unc<correlated>)(static_cast<double>(a.second))
         );
    }

    template <typename T>    //!< \tparam T Any type that can be converted to double.
    std::pair<unc<correlated>, unc<correlated> > operator()(T a)
    {  //!< Convert a pair of X and Y uncertain type values to a @c std::pair of @c unc.
      BOOST_STATIC_ASSERT_MSG(std::is_constructible<T, double>::value, "Uncertain types must be convertible to double!");
      return std::pair<unc <correlated>, unc<correlated> >(i++, (unc<correlated>)a); //! \return pair of unc.
    }
}; // class pair_unc_2d_convert

 /*! This functor allows any 2D data that can be converted to type @c std::pair<Meas, unc> to be plotted.
   \tparam correlated @c true if the uncertainties are correlated (for example, adding to a constant value).
   */
template <bool correlated>
class pair_Meas_2d_convert
{
public:
    typedef std::pair<Meas, unc<correlated> > result_type;
    //!< result type is pair of Meas (uncertain including datetime etc) and an uncertain value.

    Meas i; //!< Current Meas (uncun + datetime etc) value.

    void start(Meas i0)
    { //!< Set a start value.
       i = i0;
    }
    //!< Convert a pair of X and Y uncertain type values to a pair of uncertain types uncun.
    //! \return pair of Meas (an unc including time, ID and order info) & an unc.
    //! Cast to double so that can potentially use with @c float, @c long double.
    //!< \tparam T type that can be converted to double.
    //!< \tparam U type that can be converted to uncertain type @c unc.
    //! \returns A @c std::pair of double precision data-point values.
    template <typename T, typename U>
    std::pair<Meas, unc<correlated> > operator()(const std::pair<T, U>& a) const
    {
      BOOST_STATIC_ASSERT_MSG(std::is_constructible<T, double>::value, "Uncertain types must be convertible to uncun!");
      BOOST_STATIC_ASSERT_MSG(std::is_constructible<U, double>::value, "Uncertain types must be convertible to uncun!");
      return std::pair<Meas, unc<correlated> >((Meas)(static_cast<Meas>(a.first)), (unc<correlated>)(static_cast<unc<correlated>>(a.second)));
    }

    template <typename T>  //!< \tparam T Any type that can be converted to uncun.
    std::pair<Meas, unc<correlated> > operator()(T a)
    {  //!< Convert a pair of X and Y uncertain type values to a pair of Meas & unc.
        return std::pair<Meas, unc<correlated> >(i++, (unc<correlated>)(static_cast<double>(a)));
        //! \return pair of Meas & unc.
    }
}; // class pair_Meas_2d_convert

 /*! This functor allows any 2D data that can be converted to type double to be plotted.
   \tparam correlated @c true if the uncertainties are correlated (for example, adding up to a constant value).
   */
template <bool correlated>
class pair_Meas_2d_double_convert
{
public:
 //typedef std::pair<Meas, unc<correlated> > result_type;
  typedef std::pair<double, double > result_type;
  //!< result type is pair of double values.

  Meas i; //!< Current Meas (uncun + datetime etc) value.

  void start(Meas i0)
  { //!< Set a start value.
    i = i0;
  }
  //!< Convert a pair of X and Y uncertain type values to a pair of doubles.
  //! \return pair of Meas (unc including time, ID and order info) & an unc.
  //! Cast to double so that can potentially use with float, long double.
  //!< \tparam T type that can be converted to double.
  //!< \tparam U type that can be converted to double.
  //! \returns A @c std::pair of @c double precision data-point values.
  template <typename T, typename U>
  std::pair<Meas, unc<correlated> > operator()(const std::pair<T, U>& a) const
  {
    BOOST_STATIC_ASSERT_MSG(std::is_constructible<T, double>::value, "Uncertain types must be convertible to double!");
    BOOST_STATIC_ASSERT_MSG(std::is_constructible<U, double>::value, "Uncertain types must be convertible to double!");
     return std::pair<Meas, unc<correlated> >((Meas)(static_cast<Meas>(a.first)), (unc<correlated>)(static_cast<uncun>(a.second))); // OK uncertain values.
     // but fails for \boost\libs\svg_plot\example\convertible_to_double.cpp and this compiles
     //return std::pair<Meas, unc<correlated> >((Meas)(static_cast<double>(a.first)), (unc<correlated>)(static_cast<double>(a.second)));
     // but does not work for uncertainty displays.  Not fully understood.
 }

  //! Convert a pair of X and Y uncertain type values to a pair of @c Meas & @c unc.
  //! \tparam T Any type that can be converted to @c double.
  template <typename T>
  std::pair<Meas, unc<correlated> > operator()(T a)
  {
    return std::pair<Meas, unc<correlated> >(i++, (unc<correlated>)(static_cast<double>(a)));
    //! \return pair of Meas & unc.
  }
}; // class pair_Meas_2d_double_convert

} // namespace detail
} // namespace svg
} // namespace boost

#endif // BOOST_SVG_DETAIL_FUNCTORS_HPP


#include <gtest/gtest.h>
#include "humanoids/rave_to_rbdl.hpp"
#include <openrave-core.h>
#include "trajopt/configuration_space.hpp"
#include "trajopt/rave_utils.hpp"
#include "trajopt/utils.hpp"
#include <boost/foreach.hpp>
#include "utils/eigen_conversions.hpp"
#include "utils/stl_to_string.hpp"
#include <iostream>
#include "osgviewer/osgviewer.hpp"
#include <Eigen/Geometry>
#include <rbdl_utils.h>

using namespace std;
using namespace OpenRAVE;
using namespace trajopt;
using namespace util;
using namespace Eigen;

Vector3d axisAngleToEulerZYX(const Vector3d& in) {
  double rx = in(0), ry = in(1), rz = in(2);
  Matrix3d mat = toMatrix3d(OpenRAVE::geometry::matrixFromAxisAngle(OpenRAVE::Vector(rx, ry, rz)));
  return mat.eulerAngles(2,1,0);
}
Vector3d eulerZYXtoAxisAngle(const Vector3d& in) {
  double rz = in(0), ry = in(1), rx = in(2);
  Matrix3d mat; mat = AngleAxisd(rz, Vector3d::UnitZ())
               * AngleAxisd(ry, Vector3d::UnitY())
               * AngleAxisd(rx, Vector3d::UnitX());
  OpenRAVE::TransformMatrix tm;
  tm.rotfrommat(mat(0,0), mat(0,1), mat(0,2), mat(1,0), mat(1,1), mat(1,2), mat(2,0), mat(2,1), mat(2,2));
  return toVector3d(OpenRAVE::geometry::axisAngleFromMatrix(tm));

}

VectorXd toRBDLOrder(const DblVec& _x) {
  VectorXd x = toVectorXd(_x);
  int n = x.size();
  VectorXd y(n);
  y.topRows(3) = x.middleRows(n-6,3);
  y.middleRows(3,3) = axisAngleToEulerZYX(x.bottomRows(3));
  y.bottomRows(n-6) = x.topRows(n-6);
  return y;
}
DblVec toRaveOrder(const VectorXd& x) {
  int n = x.size();
  VectorXd y(x.size());
  y.topRows(n-6) = x.bottomRows(n-6);
  y.middleRows(n-6,3) = x.topRows(3);
  y.bottomRows(3) = eulerZYXtoAxisAngle(x.middleRows(3,3));
  return toDblVec(y);
}

#define EXPECT_MATRIX_NEAR(_mat0, _mat1, abstol) do {\
    Eigen::MatrixXd mat0=_mat0, mat1=_mat1;\
    ASSERT_TRUE(mat0.rows() == mat1.rows());\
    ASSERT_TRUE(mat0.cols() == mat1.cols());\
    bool fail = false;\
    for (int i=0; i < mat0.rows(); ++i) {\
      for (int j=0; j < mat0.cols(); ++j) {\
        if (fabs(mat0(i,j) - mat1(i,j)) > abstol) {\
          fail = true;\
        }\
      }\
    }\
    if (fail) {\
      char msg[1000];\
      sprintf(msg, "%s !=\n %s    (tol %.2e) at %s:%i\n", CSTR(mat0), CSTR(mat1), abstol, __FILE__, __LINE__);\
      GTEST_NONFATAL_FAILURE_(msg);\
    }\
} while(0)

TEST(math, rotation_conversion) {
  {
    Vector3d x = Vector3d::Random();
    EXPECT_MATRIX_NEAR(x, axisAngleToEulerZYX(eulerZYXtoAxisAngle(x)),1e-6);
    EXPECT_MATRIX_NEAR(x, eulerZYXtoAxisAngle(axisAngleToEulerZYX(x)),1e-6);
  }
  {
    VectorXd x = VectorXd::Random(40);
    EXPECT_MATRIX_NEAR(x, toVectorXd(toRaveOrder(toRBDLOrder(toDblVec(x)))),1e-6);
    EXPECT_MATRIX_NEAR(x, toRBDLOrder(toRaveOrder(x)),1e-6);
  }
}

TEST(fixed_base, kinematics) {
  /**
   *
   * Check to see that OpenRAVE kinematics gives the same result as RBDL kinematics on fixed-base robot
   */
  EnvironmentBasePtr env = RaveCreateEnvironment();
  env->StopSimulation();
  env->Load("robots/pr2-beta-static.zae");
  RobotBasePtr robot = GetRobot(*env);
  vector<int> inds;
  for (int i=0; i < robot->GetDOF(); ++i) inds.push_back(i);
  RobotAndDOFPtr rad(new RobotAndDOF(robot, inds));
  DblVec dofvals = rad->RandomDOFValues();

  robot->SetDOFValues(dofvals,true);

  std::map<KinBody::LinkPtr, unsigned> link2id;
  boost::shared_ptr<rbd::Model> model = MakeRBDLModel(robot, false, link2id);
  rbd::Math::VectorNd Q = toVectorXd(dofvals);
  rbd::UpdateKinematicsCustom(*model, &Q, NULL, NULL);

  cout << "model hier\n" << rbd::Utils::GetModelHierarchy (*model) << endl;
  cout << "model dof overview\n" << rbd::Utils::GetModelDOFOverview (*model) << endl;
  cout << "named bodies\n" << rbd::Utils::GetNamedBodyOriginsOverview(*model) << endl;

  BOOST_FOREACH(KinBody::JointPtr joint, robot->GetJoints()) {
    KinBody::LinkPtr childLink = joint->GetHierarchyChildLink();
    int i = link2id[childLink];
    cout << childLink->GetName() << " " << childLink->GetTransform() <<  endl;
    DblVec vals;
    joint->GetValues(vals);
    cout << "vasls: " << vals[0] << endl;
    EXPECT_MATRIX_NEAR(toVector3d((toRave(model->X_base[i]) * joint->GetInternalHierarchyRightTransform()).trans),
        toVector3d(childLink->GetTransform().trans), 1e-4);
    EXPECT_MATRIX_NEAR(toMatrix3d(toRave(model->X_base[i]) * joint->GetInternalHierarchyRightTransform()),
        toMatrix3d(childLink->GetTransform()), 1e-4);

  }
  RaveDestroy();
}

TEST(floating_base, kinematics) {

  EnvironmentBasePtr env = RaveCreateEnvironment();
  env->StopSimulation();
  env->Load(string(BIGDATA_DIR) + "/atlas.xml");
  RobotBasePtr robot = GetRobot(*env);
  DblVec lower(robot->GetDOF(), -1000);
  DblVec upper(robot->GetDOF(), 1000);
  robot->SetDOFLimits(lower, upper);

  vector<int> inds;
  for (int i=0; i < robot->GetDOF(); ++i) inds.push_back(i);
  RobotAndDOFPtr rad(new RobotAndDOF(robot, inds, OpenRAVE::DOF_XYZ | OpenRAVE::DOF_Rotation3D));
  DblVec dofvals = rad->RandomDOFValues();
  rad->SetDOFValues(dofvals);

  std::map<KinBody::LinkPtr, unsigned> link2id;
  boost::shared_ptr<rbd::Model> model = MakeRBDLModel(robot, true, link2id);
  rbd::Math::VectorNd Q = toRBDLOrder(dofvals);
  rbd::UpdateKinematicsCustom(*model, &Q, NULL, NULL);

  cout << Q.transpose() << endl;
  cout << Str(rad->GetDOFValues()) << endl;

  BOOST_FOREACH(KinBody::JointPtr joint, robot->GetJoints()) {
    KinBody::LinkPtr childLink = joint->GetHierarchyChildLink();
    int i = link2id[childLink];
    cout << childLink->GetName() << endl;
    EXPECT_MATRIX_NEAR(toVector3d((toRave(model->X_base[i]) * joint->GetInternalHierarchyRightTransform()).trans),
        toVector3d(childLink->GetTransform().trans), 1e-4);
    EXPECT_MATRIX_NEAR(toMatrix3d(toRave(model->X_base[i]) * joint->GetInternalHierarchyRightTransform()),
        toMatrix3d(childLink->GetTransform()), 1e-4);
  }
  RaveDestroy();
}

TEST(fixed_base, dynamics) {
  /**
   *
   * Simulate fixed-base robot
   */
  EnvironmentBasePtr env = RaveCreateEnvironment();
  env->StopSimulation();
  env->Load("robots/puma.robot.xml");
  RobotBasePtr robot = GetRobot(*env);

  std::map<KinBody::LinkPtr, unsigned> link2id;
  boost::shared_ptr<rbd::Model> model = MakeRBDLModel(robot, false, link2id);

  VectorXd Q = VectorXd::Zero(model->dof_count);
  VectorXd QD = VectorXd::Zero(model->dof_count);
  VectorXd QDD = VectorXd::Zero(model->dof_count);
  VectorXd Tau = VectorXd::Zero(model->dof_count);

  OSGViewerPtr viewer = OSGViewer::GetOrCreate(env);

  float dt = .01;
  for (int i=0; i < 100; ++i) {
    rbd::ForwardDynamics(*model, Q, QD, Tau, QDD, NULL);
    Q += QD * dt;
    QD += QDD * dt;
    robot->SetDOFValues(toDblVec(Q), false);
    viewer->Idle();
  }
  RaveDestroy();

}

TEST(floating_base, dynamics) {
  /**
   *
   * Simulate fixed-base robot
   */
  EnvironmentBasePtr env = RaveCreateEnvironment();
  env->StopSimulation();
  env->Load(string(BIGDATA_DIR)+"/atlas.xml");
  RobotBasePtr robot = GetRobot(*env);
  DblVec lower(robot->GetDOF(), -10000);
  DblVec upper(robot->GetDOF(), 10000);
  robot->SetDOFLimits(lower, upper);

  vector<int> inds;
  for (int i=0; i < robot->GetDOF(); ++i) inds.push_back(i);
  RobotAndDOFPtr rad(new RobotAndDOF(robot, inds, OpenRAVE::DOF_XYZ | OpenRAVE::DOF_Rotation3D));

  std::map<KinBody::LinkPtr, unsigned> link2id;
  boost::shared_ptr<rbd::Model> model = MakeRBDLModel(robot, true, link2id);

  VectorXd Q = VectorXd::Zero(model->dof_count);
  VectorXd QD = VectorXd::Zero(model->dof_count);
  VectorXd QDD = VectorXd::Zero(model->dof_count);
  VectorXd Tau = VectorXd::Zero(model->dof_count);

  OSGViewerPtr viewer = OSGViewer::GetOrCreate(env);

  vector<rbdmath::SpatialVector> f_ext(model->f.size(), rbdmath::SpatialVectorZero);

  KinBody::LinkPtr l_foot = robot->GetLink("l_foot");

  float dt = .005;
  for (int i=0; i < 1000; ++i) {

    OpenRAVE::Transform body_frame = robot->GetTransform();
    Vector3d l_foot_pos = toVector3d( (body_frame.inverse()*l_foot->GetTransform()).trans);
    Vector3d l_foot_force = toMatrix3d(body_frame).inverse() * Vector3d(0,0,10);
    cout << l_foot_force.transpose() << endl;
    cout << l_foot_pos.transpose() << endl;
//    f_ext[link2id[l_foot]] = concat(l_foot_force, l_foot_force.cross(l_foot_pos));
    Vector3d l_foot_force2(0,0,10);
    f_ext[link2id[l_foot]] = concat(l_foot_force2,toVector3d(l_foot->GetTransform().trans).cross( l_foot_force2)) ;

    Tau = -QD*.1;
    rbd::ForwardDynamics(*model, Q, QD, Tau, QDD, &f_ext);
    Q += QD * dt;
    QD += QDD * dt;
    rad->SetDOFValues(toRaveOrder(Q));
    viewer->Idle();
  }
  RaveDestroy();

}

TEST(fixed_base, invdynamics) {

  EnvironmentBasePtr env = RaveCreateEnvironment();
  env->StopSimulation();
  env->Load("robots/puma.robot.xml");
  RobotBasePtr robot = GetRobot(*env);

  std::map<KinBody::LinkPtr, unsigned> link2id;
  boost::shared_ptr<rbd::Model> model = MakeRBDLModel(robot, false, link2id);

  VectorXd Q = VectorXd::Zero(robot->GetDOF());
  VectorXd QDot = VectorXd::Zero(robot->GetDOF());
  VectorXd QDDot = VectorXd::Zero(robot->GetDOF());
  VectorXd Tau = VectorXd::Zero(robot->GetDOF());
  rbd::InverseDynamics(*model, Q, QDot, QDDot, Tau, NULL);
  DblVec tau_rave;
  robot->SetDOFVelocities(toDblVec(QDot));
  robot->ComputeInverseDynamics(tau_rave, toDblVec(QDDot));
  EXPECT_MATRIX_NEAR(toVectorXd(tau_rave), Tau, 1e-4);
}

int main(int argc, char** argv)
{
  ::testing::InitGoogleTest(&argc, argv);
  RaveInitialize(false);

  return RUN_ALL_TESTS();
}


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

#include <gtest/gtest.h>
#include <NTL/ZZX.h>
#include <helib/Context.h>
#include <helib/PolyModRing.h>
#include <helib/EncryptedArray.h>
#include <helib/exceptions.h>

namespace {

TEST(TestPolyModRing, canBeConstructed)
{
  const long p = 5;
  const long r = 3;
  NTL::ZZX G;
  NTL::SetCoeff(G, 1, 1);
  NTL::SetCoeff(G, 0, -1);
  helib::PolyModRing poly(p, r, G);
  EXPECT_EQ(poly.p, p);
  EXPECT_EQ(poly.r, r);
  EXPECT_EQ(poly.G, G);
  EXPECT_EQ(poly.p2r, pow(p, r));
}

TEST(TestPolyModRing, canBeCopyConstructed)
{
  const long p = 5;
  const long r = 3;
  NTL::ZZX G;
  NTL::SetCoeff(G, 1, 1);
  NTL::SetCoeff(G, 0, -1);
  helib::PolyModRing poly(p, r, G);
  helib::PolyModRing copy(poly);
  EXPECT_EQ(poly, copy);
}

TEST(TestPolyModRing, canBeMoveConstructed)
{
  const long p = 5;
  const long r = 3;
  NTL::ZZX G;
  NTL::SetCoeff(G, 1, 1);
  NTL::SetCoeff(G, 0, -1);
  helib::PolyModRing poly(p, r, G);
  helib::PolyModRing copied(poly);
  helib::PolyModRing moved(std::move(poly));
  EXPECT_EQ(copied, moved);
}

TEST(TestPolyModRing, equalsAndNotEqualsAreCorrect)
{
  const long p = 5;
  const long r = 3;
  NTL::ZZX G;
  NTL::SetCoeff(G, 1, 1);
  NTL::SetCoeff(G, 0, -1);
  helib::PolyModRing poly1(p, r, G);
  helib::PolyModRing poly2(p + 2, r, G);
  helib::PolyModRing poly3(p, r + 2, G);
  helib::PolyModRing poly4(p, r, G + 2);
  helib::PolyModRing polyA(p, r, G);

  EXPECT_EQ(poly1, polyA);
  EXPECT_FALSE(poly1 != polyA);

  EXPECT_NE(poly1, poly2);
  EXPECT_NE(poly1, poly3);
  EXPECT_NE(poly1, poly4);

  EXPECT_FALSE(poly1 == poly2);
  EXPECT_FALSE(poly1 == poly3);
  EXPECT_FALSE(poly1 == poly4);
}

} // namespace


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, Indiana University,
// Bloomington, IN 47405.
//
// Permission to modify the code and to distribute the code is
// granted, provided the text of this NOTICE is retained, a notice if
// the code was modified is included with the above COPYRIGHT NOTICE
// and with the COPYRIGHT NOTICE in the LICENSE file, and that the
// LICENSE file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
#include <boost/config.hpp>
#include <vector>
#include <deque>
#include <boost/graph/topological_sort.hpp>
#include <boost/graph/adjacency_list.hpp>
int
main()
{
  using namespace boost;
  const char *tasks[] = {
    "pick up kids from school",
    "buy groceries (and snacks)",
    "get cash at ATM",
    "drop off kids at soccer practice",
    "cook dinner",
    "pick up kids from soccer",
    "eat dinner"
  };
  const int n_tasks = sizeof(tasks) / sizeof(char *);

  adjacency_list < listS, vecS, directedS > g(n_tasks);

  add_edge(0, 3, g);
  add_edge(1, 3, g);
  add_edge(1, 4, g);
  add_edge(2, 1, g);
  add_edge(3, 5, g);
  add_edge(4, 6, g);
  add_edge(5, 6, g);

  std::deque < int >topo_order;

  topological_sort(g, std::front_inserter(topo_order),
                   vertex_index_map(identity_property_map()));

  int n = 1;
  for (std::deque < int >::iterator i = topo_order.begin();
       i != topo_order.end(); ++i, ++n)
    std::cout << tasks[*i] << std::endl;

  return EXIT_SUCCESS;
}