#include <boost/graph/copy.hpp>
#include <iostream>

#include "PrioritizedStrategicSolver.h"
#include "GraphUtil.h"
#include "ChainSolver.h"

PrioritizedStrategicSolver::PrioritizedStrategicSolver()
    : queue()
{
}

void PrioritizedStrategicSolver::FinishPartialSolution(Tree& bucket, std::vector<int>& depthLookup)
{
    for(Node& n : queue)
    {
        int naiveDepth = GenerateNaiveSolution(*n.Subgraph, n.ParentNum, bucket, depthLookup);
        bucket.Depth = std::max(bucket.Depth, n.Depth + naiveDepth);
    }
}

void PrioritizedStrategicSolver::Decompose(const UndirectedGraph& graph, Tree& bucket)
{
    Init(new UndirectedGraph(graph));
    std::vector<int> depthLookup(num_vertices(graph));
    while(queue.size() != 0)
    {
        Step(bucket, depthLookup);
    }
    return;
}

void PrioritizedStrategicSolver::Init(UndirectedGraph* graph)
{
    for(int i = 0; i < queue.size(); i++)
    {
        delete queue[i].Subgraph;
    }

    queue.clear();
    queue.emplace_back(graph, 0, -1);
}

void PrioritizedStrategicSolver::Step(Tree& bucket, std::vector<int>& depthLookup)
{
    Node next = queue.front();
    std::pop_heap(queue.begin(), queue.end());
    queue.pop_back();

    // Prune branches where the naive solution cant make the overall solution worse
    int numVertices = num_vertices(*next.Subgraph);
    int naiveDepth = numVertices + next.Depth;
    if(numVertices == 1 || naiveDepth <= bucket.Depth)
    {
        naiveDepth = GenerateNaiveSolution(*next.Subgraph, next.ParentNum, bucket, depthLookup);
        bucket.Depth = std::max(bucket.Depth, next.Depth + naiveDepth);
        delete next.Subgraph;
        return;
    }

    // Remove a vertex
    VIndex child = ChooseNodeBrandes(*next.Subgraph, 30);
    int childNum = (*next.Subgraph)[child].num;
    clear_vertex(child, *next.Subgraph);
    remove_vertex(child, *next.Subgraph);
    next.Depth += 1;
    bucket.Depth = std::max(bucket.Depth, next.Depth + depthLookup[childNum]);

    // Update tree
    bucket.ParentLookup[childNum] = next.ParentNum;
    bucket.Depth = std::max(bucket.Depth, next.Depth + depthLookup[childNum]);

    // Search for connected components
    auto components = ConnectedComponents(*next.Subgraph);
    if(components.size() == 1)
    {
        // We didnt split the graph, keep the node
        next.ParentNum = childNum;
        queue.push_back(next);
        std::push_heap(queue.begin(), queue.end());
    }
    else
    {
        // We've split the graph, add all components to the queue
        for(ComponentGraph& c: components)
        {
            UndirectedGraph* new_subgraph = new UndirectedGraph();
            copy_graph(c, *new_subgraph);
            queue.emplace_back(new_subgraph, next.Depth, childNum);
            std::push_heap(queue.begin(), queue.end());
        }
        delete next.Subgraph;
    }
}


/**
 ** Copyright (c) 2014 Illumina, Inc.
 **
 ** This file is part of Illumina's Enhanced Artificial Genome Engine (EAGLE),
 ** covered by the "BSD 2-Clause License" (see accompanying LICENSE file)
 **
 ** \author Lilian Janin
 **/

#include <numeric>
#include <boost/foreach.hpp>
#include <boost/format.hpp>

#include "common/Exceptions.hh"
#include "model/FragmentLengthDist.hh"

using namespace std;

namespace eagle
{
namespace model
{

FragmentLengthDist::FragmentLengthDist( const boost::filesystem::path& filename )
    : min_(0)
    , max_(0)
{
    // Parse template length table
    io::DsvReader tsvReader( filename );
    vector<string> tokens;
    while ( tsvReader.getNextLineFields<'\t'>(tokens) )
    {
        if ( tokens.size() == 0 ) { continue; }
        assert ( tokens.size() == 2 && "There should be 2 entries per line" );
        vector<unsigned long> values;
        unsigned int templateLength;
        double templateLengthCount;
        try
        {
            templateLength = boost::lexical_cast<unsigned long,std::string>( tokens[0] );
            templateLengthCount = boost::lexical_cast<double,std::string>  ( tokens[1] );
        }
        catch (const boost::bad_lexical_cast &e)
        {
            EAGLE_ERROR("Error while reading template length table: a numerical field seems to contain non-numerical characters");
        }

        if (min_ == 0)
        {
            min_ = templateLength;
        }
        if (templateLength < min_)
        {
            EAGLE_ERROR("Error: the template length table should be sorted by template length (first column)");
        }
        if (templateLength > max_)
        {
            max_ = templateLength;
            templateLengthDist_.resize( max_ - min_ + 1 );
        }
        assert( templateLength >= min_ );
        assert( templateLength - min_ < templateLengthDist_.size() );
        templateLengthDist_[ templateLength - min_ ] = templateLengthCount;
    }

    // Normalise table
    double sum = std::accumulate( templateLengthDist_.begin(), templateLengthDist_.end(), 0.0 );
    if (sum == 0)
    {
        EAGLE_ERROR("Error: the template length table counts (column 2) adds up to zero");
    }
    BOOST_FOREACH( double& val, templateLengthDist_ )
    {
        val /= sum;
    }
}

void FragmentLengthProbabilityMatrix::phase1( const unsigned long chrLength, const FragmentLengthDist& fragmentLengthDist )
{
    // For each position, use GC and CNV to establish the probability of a fragment covering this pos:
    // P(pos) = GC_trans[GC%(pos)] * CNV(pos)
    // Both GC% and CNV are faster to calculate if we keep the value for the previous position
    GcPercentage gcPercentage;
    CNV cnv;
    GcTrans gcTrans;
    vector<float> P( chrLength );
    for (unsigned int globalPos = 0; globalPos < chrLength; ++globalPos)
    {
        double gcPercentage_pos = gcPercentage[ globalPos ];
        double gcMultiplier_pos = gcTrans[ gcPercentage_pos ];
        double cnvMultiplier_pos = cnv[ globalPos ];
        double P_pos = gcMultiplier_pos * cnvMultiplier_pos;
        P[globalPos] = P_pos;
    }

#define COUNT_ONLY
#ifdef COUNT_ONLY
    P_FL_pos->resize( fragmentLengthDist.size() );
#else
    P_FL_pos->resize( fragmentLengthDist.size() * chrLength );
#endif  //ifdef COUNT_ONLY
    sum_P_FL_pos = 0.0;
    for (unsigned int globalPos = 0; globalPos < chrLength; ++globalPos)
    {
        double P_FL_pos_tmp = 1.0;
        unsigned int P_FL_pos_index = 0;//globalPos * fragmentLengthDist.size();
        unsigned int FLmax = min<unsigned int>( fragmentLengthDist.max(), chrLength - globalPos );
        if (globalPos%100000 == 0)
        {
            cout << "calculating for globalPos=" << globalPos << ", FLmax=" << FLmax << endl;
        }
        for (unsigned int FL = fragmentLengthDist.min(); FL <= FLmax; ++FL, ++P_FL_pos_index)
        {
/*
            P_FL_pos[FL][globalPos] = 1.0;
            for (unsigned int x = 0; x < FL; ++x)
            {
                P_FL_pos[FL][globalPos] *= P[globalPos + x];
            }
*/
            P_FL_pos_tmp *= P[globalPos + FL];
            P_FL_pos->operator[](P_FL_pos_index) = P_FL_pos_tmp;
        }
        P_FL_pos_index = 0;//globalPos * fragmentLengthDist.size();
        for (unsigned int FL = fragmentLengthDist.min(); FL <= FLmax; ++FL, ++P_FL_pos_index)
        {
            double tmp = P_FL_pos->operator[](P_FL_pos_index) * fragmentLengthDist[FL];
            sum_P_FL_pos += tmp;
        }
//        cout << "sum_P_FL_pos=" << sum_P_FL_pos << endl;
    }
    cout << "Final enumeration: " << sum_P_FL_pos << endl;
    exit(0);
/*
    // Validity check
    {
        double sum = 0;
        for (unsigned int i=0; i<P_FL_pos->size(); ++i)
        {
            sum += P_FL_pos[i];
        }
        cout << "sum=" << sum << endl;
    }
*/
}

} // namespace model
} // namespace eagle


/* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */

#include "bqresample/Resampler.h"

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include <stdexcept>
#include <vector>
#include <cmath>
#include <iostream>

using namespace std;
using namespace breakfastquay;

BOOST_AUTO_TEST_SUITE(TestResampler)

#define LEN(a) (int(sizeof(a)/sizeof(a[0])))

static vector<float>
sine(double samplerate, double frequency, int nsamples)
{
    vector<float> v(nsamples, 0.f);
    for (int i = 0; i < nsamples; ++i) {
        v[i] = sin ((i * 2.0 * M_PI * frequency) / samplerate);
    }
    return v;
}

#define COMPARE_N(a, b, n)                    \
    for (int cmp_i = 0; cmp_i < n; ++cmp_i) { \
        BOOST_CHECK_SMALL((a)[cmp_i] - (b)[cmp_i], 1e-4f);      \
    }

static const float guard_value = -999.f;

BOOST_AUTO_TEST_CASE(interpolated_sine_1ch_interleaved)
{
    // Interpolating a sinusoid should give us a sinusoid, once we've
    // dropped the first few samples
    vector<float> in = sine(8, 2, 1000); // 2Hz wave at 8Hz: [ 0, 1, 0, -1 ] etc
    vector<float> expected = sine(16, 2, 2000);
    vector<float> out(in.size() * 2 + 1, guard_value);
    Resampler r(Resampler::Parameters(), 1);
    int returned = r.resampleInterleaved
        (out.data(), out.size(), in.data(), in.size(), 2, true);

    // because final was true, we expect to have exactly the right
    // number of samples back
    BOOST_CHECK_EQUAL(returned, int(in.size() * 2));
    BOOST_CHECK_NE(out[returned-1], guard_value);
    BOOST_CHECK_EQUAL(out[returned], guard_value);

    // and they should match the expected data, at least in the middle
    const float *outf = out.data() + 200, *expectedf = expected.data() + 200;
    COMPARE_N(outf, expectedf, 600);
}

BOOST_AUTO_TEST_CASE(interpolated_sine_1ch_noninterleaved)
{
    // Interpolating a sinusoid should give us a sinusoid, once we've
    // dropped the first few samples
    vector<float> in = sine(8, 2, 1000); // 2Hz wave at 8Hz: [ 0, 1, 0, -1 ] etc
    vector<float> expected = sine(16, 2, 2000);
    vector<float> out(in.size() * 2 + 1, guard_value);
    const float *in_data = in.data();
    float *out_data = out.data();
    Resampler r(Resampler::Parameters(), 1);
    int returned = r.resample
        (&out_data, out.size(), &in_data, in.size(), 2, true);

    // because final was true, we expect to have exactly the right
    // number of samples back
    BOOST_CHECK_EQUAL(returned, int(in.size() * 2));
    BOOST_CHECK_NE(out[returned-1], guard_value);
    BOOST_CHECK_EQUAL(out[returned], guard_value);

    // and they should match the expected data, at least in the middle
    const float *outf = out.data() + 200, *expectedf = expected.data() + 200;
    COMPARE_N(outf, expectedf, 600);
}

BOOST_AUTO_TEST_CASE(overrun_interleaved)
{
    // Check that the outcount argument is correctly used: any samples
    // already in the out buffer beyond outcount*channels must be left
    // untouched. We test this with short buffers (likely to be
    // shorter than the resampler filter length) and longer ones, with
    // resampler ratios that reduce, leave unchanged, and raise the
    // sample rate, and with all quality settings.

    // Options are ordered from most normal/likely to least.

    int channels = 2;

    int lengths[] = { 6000, 6 };
    int constructionBufferSizes[] = { 0, 1000 };
    double ratios[] = { 1.0, 10.0, 0.1 };
    Resampler::Quality qualities[] = {
        Resampler::FastestTolerable, Resampler::Best, Resampler::Fastest
    };

    bool failed = false;

    for (int li = 0; li < LEN(lengths); ++li) {
        for (int cbi = 0; cbi < LEN(constructionBufferSizes); ++cbi) {
            for (int ri = 0; ri < LEN(ratios); ++ri) {
                for (int qi = 0; qi < LEN(qualities); ++qi) {

                    int length = lengths[li];
                    double ratio = ratios[ri];
                    Resampler::Parameters parameters;
                    parameters.quality = qualities[qi];
                    parameters.maxBufferSize = constructionBufferSizes[cbi];
                    Resampler r(parameters, channels);

                    float *inbuf = new float[length * channels];
                    for (int i = 0; i < length; ++i) {
                        for (int c = 0; c < channels; ++c) {
                            inbuf[i*channels+c] =
                                sinf((i * 2.0 * M_PI * 440.0) / 44100.0);
                        }
                    }

                    int outcount = int(round(length * ratio));
                    outcount -= 10;
                    if (outcount < 1) outcount = 1;
                    int outbuflen = outcount + 10;
                    float *outbuf = new float[outbuflen * channels];
                    for (int i = 0; i < outbuflen; ++i) {
                        for (int c = 0; c < channels; ++c) {
                            outbuf[i*channels+c] = guard_value;
                        }
                    }
/*
                    cerr << "\nTesting with length = " << length << ", ratio = "
                         << ratio << ", outcount = " << outcount << ", final = false"
                         << endl;
*/
                    int returned = r.resampleInterleaved
                        (outbuf, outcount, inbuf, length, ratio, false);

                    BOOST_CHECK_LE(returned, outcount);

                    for (int i = returned; i < outbuflen; ++i) {
                        for (int c = 0; c < channels; ++c) {
                            BOOST_CHECK_EQUAL(outbuf[i*channels+c], guard_value);
                            if (outbuf[i*channels+c] != guard_value) {
                                failed = true;
                            }
                        }
                    }

                    if (failed) {
                        cerr << "Test failed, abandoning remaining loop cycles"
                             << endl;
                        break;
                    }
/*
                    cerr << "\nContinuing with length = " << length << ", ratio = "
                         << ratio << ", outcount = " << outcount << ", final = true"
                         << endl;
*/
                    returned = r.resampleInterleaved
                        (outbuf, outcount, inbuf, length, ratio, true);

                    BOOST_CHECK_LE(returned, outcount);

                    for (int i = returned; i < outbuflen; ++i) {
                        for (int c = 0; c < channels; ++c) {
                            BOOST_CHECK_EQUAL(outbuf[i*channels+c], guard_value);
                            if (outbuf[i*channels+c] != guard_value) {
                                failed = true;
                            }
                        }
                    }

                    delete[] outbuf;
                    delete[] inbuf;

                    if (failed) {
                        cerr << "Test failed, abandoning remaining loop cycles"
                             << endl;
                        break;
                    }
                }

                if (failed) break;
            }
            if (failed) break;
        }
        if (failed) break;
    }
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <boost/pending/disjoint_sets.hpp>
#include "nifty/xtensor/xtensor.hxx"
#include "nifty/graph/undirected_list_graph.hxx"

namespace nifty {
namespace carving {

    // we provide implementations with kruskal and prim
    template<class GRAPH>
    class CarvingSegmenter {
        public:

            // TODO for now I made sorting edges optional, we could also just always
            // do this, but I still want to check how much time this costs
            template<class EDGES>
            CarvingSegmenter(const GRAPH & graph,
                             const EDGES & edgeWeights,
                             const bool sortEdges) : graph_(graph),
                                                     nNodes_(graph.numberOfNodes()){
                // check that the number of edges and len of edges agree
                NIFTY_CHECK_OP(edgeWeights.size(), ==, graph_.numberOfEdges(), "Number of edges does not agree");

                // copy the edge weights
                edgeWeights_.resize(edgeWeights.size());
                std::copy(edgeWeights.begin(), edgeWeights.end(), edgeWeights_.begin());

                if(sortEdges) {
                    sortEdgeIndices();
                }
            }

            template<class NODES>
            inline void operator()(NODES & seeds,
                                   const double bias,
                                   const double noBiasBelow) const {
                // check that the number of nodes agree
                NIFTY_CHECK_OP(seeds.size(), ==, nNodes_, "Number of nodes does not agree");

                // check if we can use kruskal: we don't have a bias and edges were pre-sorted
                const bool useKruskal = (bias == 1.) && (edgesSorted_.size() == graph_.numberOfEdges());
                if(useKruskal) {
                    // std::cout << "run kruskal" << std::endl;
                    runKruskal(seeds);
                }
                // otherwise we need to run prim
                else {
                    // std::cout << "run prim" << std::endl;
                    runPrim(seeds, bias, noBiasBelow);
                }

            }

            inline std::size_t nNodes() const {
                return graph_.numberOfNodes();
            }

            inline const std::vector<std::size_t> & edgesSorted() const {
                return edgesSorted_;
            }

        private:
            // argsort the edges
            inline void sortEdgeIndices() {
                // we sort edge indices in ascending order
                edgesSorted_.resize(graph_.numberOfEdges());
                std::iota(edgesSorted_.begin(), edgesSorted_.end(), 0);
                std::sort(edgesSorted_.begin(), edgesSorted_.end(), [&](const std::size_t a,
                                                                        const std::size_t b){
                    return edgeWeights_[a] < edgeWeights_[b];}
                );
            }

            template<class NODES>
            inline void runPrim(NODES & seeds,
                                const double bias,
                                const double noBiasBelow) const {
                typedef typename NODES::value_type NodeType;
                typedef float WeightType;
                const NodeType backgroundSeedLabel = 1;

                // initialize the priority queue
                typedef std::pair<std::size_t, WeightType> PQElement; // PQElement contains the edge-id and the weight
                auto pqCompare = [](PQElement left, PQElement right) {return left.second < right.second;};
                typedef std::priority_queue<PQElement, std::vector<PQElement>, decltype(pqCompare)> PriorityQueue;
                PriorityQueue pq(pqCompare);

                // put edges from seed nodes on the pq
                for(std::size_t nodeId = 0; nodeId < nNodes_; ++nodeId) {
                    const NodeType seedId = seeds[nodeId];
                    if(seedId != 0) {

                        // check if this is a background seed and we use bias
                        const bool needBias = seedId == backgroundSeedLabel;

                        // iterate over the edges going from this node
                        // and put them on the pq
                        for(auto adjIt = graph_.adjacencyBegin(nodeId); adjIt != graph_.adjacencyEnd(nodeId); ++adjIt) {
                            const std::size_t edgeId = adjIt->edge();

                            // don't put on pq if the connected node has a seed
                            const std::size_t node = adjIt->node();
                            if(seeds[node] != 0) {
                                continue;
                            }

                            WeightType weight = edgeWeights_[edgeId];
                            if(needBias && (weight > noBiasBelow)) {
                                weight *= bias;
                            }

                            pq.push(std::make_pair(edgeId, weight));
                        }
                    }
                }

                // run prim
                while(!pq.empty()) {
                    // extract next element from the queue
                    const PQElement elem = pq.top();
                    pq.pop();

                    const std::size_t edgeId = elem.first;

                    const auto u = graph_.u(edgeId);
                    const auto v = graph_.v(edgeId);
                    const NodeType lu = seeds[u];
                    const NodeType lv = seeds[v];

                    // check for seeds
                    if(lu == 0 && lv == 0){
                        throw std::runtime_error("both have no labels");
                    }
                    else if(lu != 0 && lv != 0){
                        continue;
                    }

                    const auto unlabeledNode = lu == 0 ? u : v;
                    const NodeType seedId = lu == 0 ? lv : lu;

                    // assign seedId to unlabeled node
                    seeds[unlabeledNode] = seedId;

                    // check if this is a background seed and thus we use bias
                    const bool needBias = seedId == backgroundSeedLabel;

                    // put outgoing edges on the pq
                    for(auto adjIt = graph_.adjacencyBegin(unlabeledNode); adjIt != graph_.adjacencyEnd(unlabeledNode); ++adjIt) {
                        const std::size_t nextEdge = adjIt->edge();

                        // check that this is not the ingoing edge
                        if(nextEdge == edgeId) {
                            continue;
                        }
                        // check that the node is not labeled
                        const auto nextNode = adjIt->node();
                        if(seeds[nextNode] != 0) {
                            continue;
                        }

                        WeightType weight = edgeWeights_[nextEdge];
                        if(needBias && weight > noBiasBelow) {
                            weight *= bias;
                        }
                        pq.push(std::make_pair(nextEdge, weight));
                    }

                }
            }

            template<class NODES>
            inline void runKruskal(NODES & seeds) const {
                typedef typename NODES::value_type NodeType;
                // make union find and map seeds to reperesentatives
                std::vector<uint64_t> ranks(nNodes_);
                std::vector<uint64_t> parents(nNodes_);
                boost::disjoint_sets<uint64_t*, uint64_t*> ufd(&ranks[0], &parents[0]);

                for(std::size_t node = 0; node < nNodes_; ++node) {
                    ufd.make_set(node);
                }

                // run kruskal
                for(const std::size_t edgeId : edgesSorted_) {
                    // get the nodes connected by this edge
                    // and the representatives
                    const uint64_t u = graph_.u(edgeId);
                    const uint64_t v = graph_.v(edgeId);
                    const uint64_t ru = ufd.find_set(u);
                    const uint64_t rv = ufd.find_set(v);

                    // if the representatives are the same, continue
                    if(ru == rv) {
                        continue;
                    }

                    // get the seeds for our reperesentatives
                    const NodeType lu = seeds[ru];
                    const NodeType lv = seeds[rv];

                    // if we have two seeded regions (both values different from 0) continue
                    if(lu !=0 && lv != 0) {
                        continue;
                    }

                    // otherwise link the two representatives
                    ufd.link(ru, rv);

                    // if we have a seed, propagate it
                    if(lu != 0) {
                        seeds[rv] = lu;
                    }
                    if(lv != 0) {
                        seeds[ru] = lv;
                    }
                }

                // write all seeds
                for(std::size_t node = 0; node < nNodes_; ++node) {
                    auto & seed = seeds[node];
                    if(seed == 0) {
                        seed = seeds[ufd.find_set(node)];
                    }
                }
            }

        private:
            const GRAPH & graph_;
            std::size_t nNodes_;
            std::vector<float> edgeWeights_;
            std::vector<std::size_t> edgesSorted_;
    };

}
}


#ifndef PARAMETRIZER_H_
#define PARAMETRIZER_H_
#include <atomic>
#include <condition_variable>
#ifdef WITH_TBB
#include <tbb/tbb.h>
#endif

#include <Eigen/Core>
#include <Eigen/Dense>
#include <list>
#include <map>
#include <set>
#include <unordered_set>
#include "adjacent-matrix.hpp"
#include "disajoint-tree.hpp"
#include "field-math.hpp"
#include "hierarchy.hpp"
#include "post-solver.hpp"
#include "serialize.hpp"

namespace qflow {

using namespace Eigen;

typedef std::pair<unsigned int, unsigned int> Edge;
typedef std::map<int, std::pair<int, int>> SingDictionary;

struct ExpandInfo {
    ExpandInfo() {}
    int current_v;
    int singularity;
    int step;
    int edge_id;
    int prev;
};

class Parametrizer {
   public:
    Parametrizer() {}
    // Mesh Initialization
    void Load(const char* filename);
    void NormalizeMesh();
    void ComputeMeshStatus();
    void ComputeSmoothNormal();
    void ComputeSharpEdges();
    void ComputeSharpO();
    void ComputeVertexArea();
    void Initialize(int faces);

    // Singularity and Mesh property
    void AnalyzeValence();
    void ComputeOrientationSingularities();
    void ComputePositionSingularities();

    // Integer Grid Map Pipeline
    void ComputeIndexMap(int with_scale = 0);
    void BuildEdgeInfo();
    void ComputeMaxFlow();
    void MarkInteger();
    void BuildIntegerConstraints();

    // Fix Flip
    void FixFlipHierarchy();
    void FixFlipSat();
    void FixHoles();
    void FixHoles(std::vector<int>& loop_vertices);
    void FixValence();
    double QuadEnergy(std::vector<int>& loop_vertices, std::vector<Vector4i>& res_quads,
                      int level);

    // Quadmesh and IO
    void AdvancedExtractQuad();
    void BuildTriangleManifold(DisajointTree& disajoint_tree, std::vector<int>& edge,
                               std::vector<int>& face, std::vector<DEdge>& edge_values,
                               std::vector<Vector3i>& F2E, std::vector<Vector2i>& E2F,
                               std::vector<Vector2i>& EdgeDiff, std::vector<Vector3i>& FQ);
    void OutputMesh(const char* obj_name);

    std::map<int, int> singularities;  // map faceid to valence (1 (valence=3) or 3(valence=5))
    std::map<int, Vector2i> pos_sing;
    MatrixXi pos_rank;   // pos_rank(i, j) i \in [0, 3) jth face ith vertex  rotate by its value so
                         // that all thress vertices are in the same orientation
    MatrixXi pos_index;  // pos_index(i x 2 + dim, j) i \in [0, 6) jth face ith vertex's
                         // (t_ij-t_ji)'s dim's dimenstion in the paper
    // input mesh
    MatrixXd V;
    MatrixXd N;
    MatrixXd Nf;
    MatrixXd FS;
    MatrixXd FQ;
    MatrixXi F;

    double normalize_scale;
    Vector3d normalize_offset;

    // data structures
    VectorXd rho;
    VectorXi V2E;
    VectorXi E2E;
    VectorXi boundary;
    VectorXi nonManifold;  // nonManifold vertices, in boolean
    AdjacentMatrix adj;
    Hierarchy hierarchy;

    // Mesh Status;
    double surface_area;
    double scale;
    double average_edge_length;
    double max_edge_length;
    VectorXd A;

    // just for test
    DisajointTree disajoint_tree;

    int compact_num_v;
    std::vector<std::vector<int>> Vset;
    std::vector<Vector3d> O_compact;
    std::vector<Vector3d> Q_compact;
    std::vector<Vector3d> N_compact;
    std::vector<Vector4i> F_compact;
    std::set<std::pair<int, int>> Quad_edges;
    std::vector<int> V2E_compact;
    std::vector<int> E2E_compact;
    VectorXi boundary_compact;
    VectorXi nonManifold_compact;

    std::vector<int> bad_vertices;
    std::vector<double> counter;
    std::vector<int>
        sharp_edges;  // sharp_edges[deid]: whether deid is a sharp edge that should be preserved
    std::vector<int> allow_changes;   // allow_changes[variable_id]: whether var can be changed
                                      // based on sharp edges
    std::vector<Vector2i> edge_diff;  // edge_diff[edgeIds[i](j)]:  t_ij+t_ji under
                                      // edge_values[edgeIds[i](j)].x's Q value
    std::vector<DEdge> edge_values;   // see above
    std::vector<Vector3i>
        face_edgeIds;  // face_edgeIds[i](j): ith face jth edge's "undirected edge ID"

    // face_edgeOrients[i](j): Rotate from edge_diff space
    //    (a) initially, to F(0, i)'s Q space
    //    (b) later on, to a global Q space where some edges are fixed
    std::vector<Vector3i> face_edgeOrients;

    // variable[i].first: indices of the two equations corresponding to variable i
    // variable[i].second: number of positive minus negative of variables' occurances
    std::vector<std::pair<Vector2i, int>> variables;

    struct QuadInfo {
        QuadInfo() : patchId(-1), coordinate(0x10000000, 0x10000000), singular(0), edge(0) {}
        int patchId;
        Vector2i coordinate;
        int singular;
        int edge;
    };
    std::vector<QuadInfo> quad_info;

    // scale
    void ComputeInverseAffine();
    void EstimateSlope();
    std::vector<MatrixXd> triangle_space;

    // flag
    int flag_preserve_sharp = 0;
    int flag_preserve_border = 0;
    int flag_adaptive_scale = 0;
    int flag_aggresive_sat = 0;
    int flag_minimum_cost_flow = 0;
};

extern void generate_adjacency_matrix_uniform(const MatrixXi& F, const VectorXi& V2E,
                                              const VectorXi& E2E, const VectorXi& nonManifold,
                                              AdjacentMatrix& adj);

} // namespace qflow

#endif


#include <iostream>
#include <fstream>
#include <vector>
#include <time.h>
#include "Circuit.h"
#include <boost/graph/graphviz.hpp>

int main(int argc, char* argv[]){
    std::pair<std::set<int>, std::set<int>> XY;
    Circuit C;

    time_t start, end;
    time(&start);

    C.input_to_objects(argv[1]);
    C.move_obstacles_points();
    C.generate_hanan_grid(false);
    C.spanning_tree(false);
    C.close_components_cycles();
    C.remove_one_degree_vertices();
    C.generate_output();

    string p = "print";
    if (argv[2] == p) {
        C.g.print_Vertex_map();
    }

    time(&end);
    double dif = difftime(end, start);
    std::cout << "\n\nTempo de execução: " << dif << " segundos\n";

    return 0;
}


#include <boost/test/unit_test.hpp>
#include <vector>
#include <utility>
#include "algorithms/search/find_two_elements.hpp"

BOOST_AUTO_TEST_SUITE(TestFindTwoElements)

auto areAllPairsEqual = [](const std::vector<std::pair<int, int>>& first,
        const std::vector<std::pair<int, int>>& second)
{
    if (first.size() != second.size())
    {
        return false;
    }

    for (size_t i = 0; i < first.size(); ++i)
    {
        std::pair<int, int> orig = first[i];
        std::pair<int, int> inv = {first[i].second, first[i].first};
        auto func = [&orig, &inv](const std::pair<int, int>& p)
        {
            return p == orig || p == inv;
        };

        if (!std::any_of(second.begin(), second.end(), func))
        {
            return false;
        }
    }

    return true;
};

BOOST_AUTO_TEST_CASE(empty_data)
{
    std::vector<int> data;
    int value = 10;
    std::vector<std::pair<int, int>> expected;

    BOOST_CHECK(areAllPairsEqual(
                    expected, Algo::Search::FindTwoElementsNaive(data, value)));

    BOOST_CHECK(areAllPairsEqual(
                    expected, Algo::Search::FindTwoElementsBS(data, value)));

    BOOST_CHECK(areAllPairsEqual(
                    expected, Algo::Search::FindTwoElementsHash(data, value)));
    {
        std::vector<int> expectedIndexes;
        BOOST_CHECK(expectedIndexes ==
                    Algo::Search::TwoSum::SearchOrdered(data, value));
    }
}

BOOST_AUTO_TEST_CASE(one_pair)
{
    std::vector<int> data = {99, -1, 0, 8, 3, 4, 15, 100};
    const int value = 11;
    const std::vector<std::pair<int, int>> expected({ {8, 3} });

    BOOST_CHECK(areAllPairsEqual(
                    expected, Algo::Search::FindTwoElementsNaive(data, value)));

    BOOST_CHECK(areAllPairsEqual(
                    expected, Algo::Search::FindTwoElementsBS(data, value)));

    BOOST_CHECK(areAllPairsEqual(
                    expected, Algo::Search::FindTwoElementsHash(data, value)));

    {
        std::sort(data.begin(), data.end());
        std::vector<int> expectedIndexes({2, 4});
        BOOST_CHECK(expectedIndexes ==
                    Algo::Search::TwoSum::SearchOrdered(data, value));
    }
}

BOOST_AUTO_TEST_CASE(several_pairs)
{
    std::vector<int> data = {99, -1, 7, 0, 8, 3, 4, 15, 6, 17, 100, 1, 9, 16};
    int value = 16;
    std::vector<std::pair<int, int>> expected(
        { {1, 15}, {-1, 17}, {7, 9}, {0, 16} });

    BOOST_CHECK(areAllPairsEqual(
                    expected, Algo::Search::FindTwoElementsNaive(data, value)));

    BOOST_CHECK(areAllPairsEqual(
                    expected, Algo::Search::FindTwoElementsBS(data, value)));

    BOOST_CHECK(areAllPairsEqual(
                    expected, Algo::Search::FindTwoElementsHash(data, value)));

    {
        std::sort(data.begin(), data.end());
        std::vector<int> expectedIndexes({0, 11});
        BOOST_CHECK(expectedIndexes ==
                    Algo::Search::TwoSum::SearchOrdered(data, value));
    }
}

BOOST_AUTO_TEST_CASE(no_pairs)
{
    std::vector<int> data = {99, -1, 7, 0, 8, 3, 4, 15, 6, 17, 100, 1, 9, 16};
    int value = 1123;
    std::vector<std::pair<int, int>> expected;

    BOOST_CHECK(areAllPairsEqual(
                    expected, Algo::Search::FindTwoElementsNaive(data, value)));

    BOOST_CHECK(areAllPairsEqual(
                    expected, Algo::Search::FindTwoElementsBS(data, value)));

    BOOST_CHECK(areAllPairsEqual(
                    expected, Algo::Search::FindTwoElementsHash(data, value)));

    {
        std::sort(data.begin(), data.end());
        std::vector<int> expectedIndexes;
        BOOST_CHECK(expectedIndexes ==
                    Algo::Search::TwoSum::SearchOrdered(data, value));
    }
}

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2014   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_MAGMA_MCSNE_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_MAGMA_MCSNE_HPP_INCLUDED

#if defined(NT2_USE_MAGMA)

#include <nt2/linalg/functions/mcsne.hpp>
#include <nt2/sdk/memory/cuda/buffer.hpp>
#include <nt2/include/functions/lange.hpp>
#include <nt2/include/functions/width.hpp>
#include <nt2/include/functions/height.hpp>
#include <nt2/include/functions/of_size.hpp>
#include <nt2/include/functions/trsm.hpp>
#include <nt2/include/functions/qr.hpp>
#include <nt2/include/functions/linsolve.hpp>
#include <nt2/include/functions/mtimes.hpp>
#include <nt2/include/functions/transpose.hpp>
#include <nt2/include/functions/maximum.hpp>
#include <nt2/include/functions/colon.hpp>
#include <nt2/include/functions/abs.hpp>
#include <nt2/include/functions/cast.hpp>
#include <nt2/linalg/details/utility/f77_wrapper.hpp>
#include <nt2/include/constants/eps.hpp>
#include <boost/dispatch/meta/downgrade.hpp>
#include <nt2/sdk/meta/as_real.hpp>

#include <nt2/core/container/table/table.hpp>
#include <magma.h>

 namespace nt2{ namespace ext
 {
   BOOST_DISPATCH_IMPLEMENT  ( mcsne_, nt2::tag::cuda_<site>
                             , (A0)(A1)(site)
                             , ((ast_<A0, nt2::container::domain>))
                               ((ast_<A1, nt2::container::domain>))
                             )
   {
     typedef typename A0::value_type                   type_t;
     typedef typename nt2::container::table<type_t>    t_t;
     typedef  t_t                                      result_type;

      BOOST_FORCEINLINE result_type operator()(A0 const& a, A1 const& b) const
     {
       nt2_la_int lda = a.leading_size();
       nt2_la_int m = nt2::height(a);
       nt2_la_int na = nt2::width(a);
       nt2_la_int nhrs = nt2::width(b);
       nt2_la_int ldb = nt2::height(b);
       nt2_la_int nb = magma_get_dgeqrf_nb(m);
       nt2_la_int lwkopt = (2*std::min(m,na)+ ((na+31)/32)*32)*nb;
       nt2_la_int info;
       double one = 1;
       double negone = -1;

       t_t x(nt2::of_size(ldb,nhrs));
       nt2::container::table<float> tau(nt2::of_size(std::min(m,na),1));

       nt2::memory::cuda_buffer<double>      dA(m*na);
       nt2::memory::copy(a,dA);

       nt2::memory::cuda_buffer<double>      dB(ldb*nhrs);
       nt2::memory::copy(b,dB);

       nt2::memory::cuda_buffer<double>      dX(ldb*nhrs);
       nt2::memory::cuda_buffer<double>      dE(ldb*nhrs);
       nt2::memory::cuda_buffer<double>      temp(ldb*nhrs);
       nt2::memory::cuda_buffer<float>       dSR(m*na);
       nt2::memory::cuda_buffer<float>       dSX(ldb*nhrs);
       nt2::memory::cuda_buffer<float>       dT(lwkopt*nhrs);

       double eps = boost::simd::Eps<double>();
       double anrm = magmablas_dlange(MagmaInfNorm, m, na, dA.data(), m, dE.data() );
       double cte = anrm*eps*nt2::sqrt( type_t(na));
       double xnrm, rnrm;

       magmablas_dlag2s(m,na,dA.data(),m,dSR.data(),m, &info);

       magmablas_dlag2s(ldb,nhrs,dB.data(),ldb,dSX.data(),ldb,&info);

       magmablas_sgemv( MagmaTrans, m, na, one, dSR.data(), m, dSX.data(), one, 0, dSX.data(), one );

       magma_sgeqrf2_gpu( m, na, dSR.data(), m, tau.data(), &info );

       magmablas_strsm(MagmaLeft,MagmaUpper,MagmaTrans,MagmaNonUnit,na,nhrs,(float)one,dSR.data(),m,dSX.data(), na);

       magmablas_strsm(MagmaLeft,MagmaUpper,MagmaNoTrans,MagmaNonUnit,na,nhrs,(float)one,dSR.data(),m,dSX.data(), na);

       magmablas_slag2d(ldb,nhrs,dSX.data(),ldb,dX.data(),ldb,&info);

       magmablas_dlacpy(MagmaFull, ldb, nhrs, dB.data(), ldb, dE.data(), ldb);

       magmablas_dgemv( MagmaNoTrans, lda, na, negone, dA.data(), lda, dX.data(), one, one, dE.data(), one );

       for(size_t i = 1; i<=10;++i)
       {
         magmablas_dgemv( MagmaTrans, lda, na, one, dA.data(), lda, dE.data(), one, 0, temp.data(), one );
         magmablas_dlag2s(ldb,nhrs,temp.data(),ldb,dSX.data(),ldb,&info);

         magmablas_strsm(MagmaLeft,MagmaUpper,MagmaTrans,MagmaNonUnit,na,nhrs,(float)one,dSR.data(),m,dSX.data(), na);

         magmablas_strsm(MagmaLeft,MagmaUpper,MagmaNoTrans,MagmaNonUnit,na,nhrs,(float)one,dSR.data(),m,dSX.data(), na);

         magmablas_slag2d(na,nhrs,dSX.data(),ldb,dE.data(),ldb,&info);

         nt2::memory::copy(dE,x);
         rnrm = nt2::maximum(nt2::abs(x(_)));

         magmablas_dgeadd(na,nhrs,one,dE.data(),ldb,dX.data(),ldb);

         nt2::memory::copy(dX,x);
         xnrm = nt2::maximum(nt2::abs(x(_)));

         magmablas_dlacpy(MagmaFull, ldb, nhrs, dB.data(), ldb, dE.data(), ldb);
         magmablas_dgemv( MagmaNoTrans, lda, na, negone, dA.data(), lda, dX.data(), one, one, dE.data(), one );

         if(rnrm < xnrm*cte) { break; }
       }

        nt2::memory::copy(dX,x);

        return x(_(1,na));
     }
   };

 } }

 #endif

 #endif

// namespace nt2{ namespace ext
// {
//   BOOST_DISPATCH_IMPLEMENT  ( mcsne_, nt2::tag::cuda_<site>
//                             , (A0)(A1)(site)
//                             , ((ast_<A0, nt2::container::domain>))
//                               ((ast_<A1, nt2::container::domain>))
//                             )
//   {
//     typedef typename A0::value_type ctype_t;
//     typedef typename nt2::meta::as_real<ctype_t>::type   type_t;
//     typedef typename boost::dispatch::meta::downgrade<ctype_t>::type dtype;
//     typedef typename nt2::container::table<ctype_t> t_ct;
//     typedef  t_ct                                      result_type;

//      BOOST_FORCEINLINE result_type operator()(A0 const& a, A1 const& b) const
//     {
//       nt2_la_int lda = a.leading_size();
//       nt2_la_int na = nt2::width(a);

//       t_ct e,x;

//       double eps = boost::simd::Eps<double>();
//       double anrm = nt2::lange( boost::proto::value(a),MagmaInfNorm);
//       double cte = anrm*eps*nt2::sqrt( double(na));
//       double xnrm, rnrm;

//       nt2::container::table<dtype> sa,sb,sx;
//       nt2::container::table<dtype,nt2::upper_triangular_> sr,sr1(nt2::of_size(na,na));
//       sa = nt2::cast<dtype>(a);
//       sr = nt2::qr(sa,nt2::no_pivot_);

//       size_t size_n = sizeof(dtype)*(na);

//       for(nt2_la_int i=0; i<na; i++)
//       {
//         std::memcpy(sr1.data()+i*na, sr.data()+i*lda, size_n);
//       }

//       boost::proto::value(sr).swap(boost::proto::value(sr1) );

//       sx= nt2::mtimes(nt2::trans(sa),nt2::cast<dtype>(b));

//       sx = nt2::linsolve(nt2::trans(sr),sx);
//       sx = nt2::linsolve(sr,sx);

//       x = nt2::cast<ctype_t>(sx);
//       e = b - nt2::mtimes(a,x);

//       for( size_t i = 1; i<=10;++i)
//       {
//         sx = nt2::cast<dtype> (nt2::mtimes(nt2::trans(a),e) ) ;

//         sx = nt2::linsolve(nt2::trans(sr),sx);
//         sx = nt2::linsolve(sr,sx);

//         e = nt2::cast<ctype_t>(sx);
//         rnrm = nt2::maximum(nt2::abs(e(_)));
//         x = x + e;
//         e = b - nt2::mtimes(a,x);

//         xnrm = nt2::maximum(nt2::abs(x(_)));

//         if(rnrm < xnrm*cte) { break; }
//       }
//       return x;
//     }
//   };

// } }

// #endif

// #endif


/*
* Copyright (c) 2021 The Foundation for Research on Information Technologies in Society (IT'IS).
*
* This file is part of iSEG
* (see https://github.com/ITISFoundation/osparc-iseg).
*
* This software is released under the MIT License.
*  https://opensource.org/licenses/MIT
*/
#include <boost/test/unit_test.hpp>

#include "../ConnectedShapeBasedInterpolation.h"

#include <itkMinimumMaximumImageCalculator.h>

#include <string>

namespace iseg {

BOOST_AUTO_TEST_SUITE(iSeg_suite);
BOOST_AUTO_TEST_SUITE(Core_suite);
BOOST_AUTO_TEST_SUITE(Interpolation_suite);

// TestRunner.exe --run_test=iSeg_suite/Interpolation_suite/Interpolation_test --log_level=message
BOOST_AUTO_TEST_CASE(Interpolation_test)
{
	using image_type = itk::Image<float, 2>;

	image_type::IndexType start;
	start.Fill(0);
	image_type::SizeType size;
	size.Fill(512);
	image_type::RegionType region(start, size);

	auto slice1 = image_type::New();
	slice1->SetRegions(region);
	slice1->Allocate();
	slice1->FillBuffer(0);
	itk::Index<2> idx = {200, 200};
	slice1->SetPixel(idx, 1);

	auto slice2 = image_type::New();
	slice2->SetRegions(region);
	slice2->Allocate();
	slice2->FillBuffer(0);
	idx[0] = 250;
	slice2->SetPixel(idx, 1);

	ConnectedShapeBasedInterpolation myinterpolator;
	auto slices = myinterpolator.Interpolate(slice1, slice2, 2);

	for (const auto& slice : slices)
	{
		auto calc = itk::MinimumMaximumImageCalculator<image_type>::New();
		calc->SetImage(slice);
		calc->Compute();
		BOOST_CHECK_EQUAL(calc->GetMaximum(), 255);
		BOOST_CHECK_GT(calc->GetIndexOfMaximum()[0], 210);
		BOOST_CHECK_LT(calc->GetIndexOfMaximum()[0], 240);
		BOOST_CHECK_EQUAL(calc->GetIndexOfMaximum()[1], 200);
	}
}

// --run_test=iSeg_suite/Interpolation_suite/ResampleFilterBug --log_level=message
BOOST_AUTO_TEST_CASE(ResampleFilterBug)
{
	using image_type = itk::Image<float, 3>;

	image_type::IndexType input_start = {0, 0, 0};
	image_type::SizeType input_size = {512, 512, 3};
	image_type::RegionType input_region(input_start, input_size);
	image_type::SpacingType input_spacing;
	input_spacing.Fill(1.0);
	image_type::PointType input_origin;
	input_origin.Fill(0.0);

	auto input = image_type::New();
	input->SetSpacing(input_spacing);
	input->SetOrigin(input_origin);
	input->SetRegions(input_region);
	input->Allocate();
	input->FillBuffer(3.f);
	input->SetObjectName("My Input");

	auto id = itk::IdentityTransform<double, 3>::New();
	auto origin = input_origin;
	auto spacing = input_spacing;
	auto size = input_region.GetSize();
	size[2] = 2;
	spacing[2] = 1.0 / (size[2] + 1);
	origin[2] = spacing[2];

	auto interpolator = itk::LinearInterpolateImageFunction<image_type, double>::New();

	auto resample_filter = itk::ResampleImageFilter<image_type, image_type>::New();
	resample_filter->SetTransform(id.GetPointer());
	resample_filter->SetInterpolator(interpolator);
	resample_filter->SetOutputOrigin(origin);
	resample_filter->SetOutputSpacing(spacing);
	resample_filter->SetOutputStartIndex(input_region.GetIndex());
	resample_filter->SetSize(size);
	resample_filter->SetInput(input);
	resample_filter->Update();
}

BOOST_AUTO_TEST_SUITE_END();
BOOST_AUTO_TEST_SUITE_END();
BOOST_AUTO_TEST_SUITE_END();

} // namespace iseg


/*
 * Bullet OSG Framework.
 * Robudog trunk module.
 *
 * Copyright (c) 2015 Jérémie Decock <jd.jdhp@gmail.com>
 *
 * www.jdhp.org
 */

#include "robudog_trunk.h"

#include "joint_slots/fixed_slot.h"
#include "joint_slots/hinge_slot.h"
#include "joint_slots/point_to_point_slot.h"

#include "parts/box.h"

#include <Eigen/Dense>

#include <iostream>
#include <string>

/*
 * PART'S PROPERTIES
 */

static const Eigen::Vector3d initial_dimension = Eigen::Vector3d(20., 7.5, 5.);
//static const Eigen::Vector3d initial_position = Eigen::Vector3d(0., 0., 5.);
//static const Eigen::Vector4d initial_angle = Eigen::Vector4d(0., 0., 0., 1.);
static const Eigen::Vector3d initial_velocity = Eigen::Vector3d(0., 0., 0.);
static const Eigen::Vector3d initial_angular_velocity = Eigen::Vector3d(0., 0., 0.);
static const Eigen::Vector3d initial_inertia = Eigen::Vector3d(0., 0., 0.);
static const double mass = 5.;
static double friction = 0.5;
static double rolling_friction = 0.;
static double restitution = 0.;

botsim::Part * botsim::make_robudog_trunk(const Eigen::Vector3d & initial_position,
                                          const Eigen::Vector4d & initial_angle,
                                          const std::string & name) {

    // Make the part
    botsim::Part * p_part = new botsim::Box(initial_dimension,
                                            initial_position,
                                            initial_angle,
                                            initial_velocity,
                                            initial_angular_velocity,
                                            initial_inertia,
                                            mass,
                                            friction,
                                            rolling_friction,
                                            restitution,
                                            name);

    const double dim_x = initial_dimension[0];
    const double dim_y = initial_dimension[1];
    const double dim_z = initial_dimension[2];

    const double dim_ax12_x = 2.5; // TODO !!!
    const double dim_ax12_y = 3.5 + 0.5; // TODO !!!
    const double dim_ax12_z = 5.;  // TODO !!!

    // Add left shoulder joint slot
    Eigen::Vector3d left_shoulder_pivot(dim_x/2. - dim_ax12_x/2., dim_y/2. + dim_ax12_y/2., 0.);
    Eigen::Vector3d left_shoulder_axis(0., 1., 0.);
    botsim::HingeSlot * p_left_shoulder_joint_slot = new botsim::HingeSlot(left_shoulder_pivot, left_shoulder_axis);

    // Add right shoulder joint slot
    Eigen::Vector3d right_shoulder_pivot(dim_x/2. - dim_ax12_x/2., -dim_y/2. - dim_ax12_y/2., 0.);
    Eigen::Vector3d right_shoulder_axis(0., 1., 0.);
    botsim::HingeSlot * p_right_shoulder_joint_slot = new botsim::HingeSlot(right_shoulder_pivot, right_shoulder_axis);

    // Add left hip joint slot
    Eigen::Vector3d left_hip_pivot(-dim_x/2. + dim_ax12_x/2., dim_y/2. + dim_ax12_y/2., 0.);
    Eigen::Vector3d left_hip_axis(0., 1., 0.);
    botsim::HingeSlot * p_left_hip_joint_slot = new botsim::HingeSlot(left_hip_pivot, left_hip_axis);

    // Add right hip joint slot
    Eigen::Vector3d right_hip_pivot(-dim_x/2. + dim_ax12_x/2., -dim_y/2. - dim_ax12_y/2., 0.);
    Eigen::Vector3d right_hip_axis(0., 1., 0.);
    botsim::HingeSlot * p_right_hip_joint_slot = new botsim::HingeSlot(right_hip_pivot, right_hip_axis);

    // Add crane (machine) joint slot
    Eigen::Vector3d crane_pivot(0., 0., dim_z/2.);
    //botsim::PointToPointSlot * p_crane_joint_slot = new botsim::PointToPointSlot(crane_pivot);
    botsim::FixedSlot * p_crane_joint_slot = new botsim::FixedSlot(crane_pivot);

    p_part->addJointSlot(std::string("left_shoulder"), p_left_shoulder_joint_slot);
    p_part->addJointSlot(std::string("right_shoulder"), p_right_shoulder_joint_slot);
    p_part->addJointSlot(std::string("left_hip"), p_left_hip_joint_slot);
    p_part->addJointSlot(std::string("right_hip"), p_right_hip_joint_slot);
    p_part->addJointSlot(std::string("crane"), p_crane_joint_slot);

    return p_part;
}


#include <boost/test/unit_test.hpp>

#include <smtrat-modules/SATModule/mcsat/BaseBackend.h>
#include <smtrat-common/smtrat-common.h>

using namespace smtrat;

BOOST_AUTO_TEST_SUITE(Test_AssignmentFinder);

/**
 * Tests Example 3 from the NLSAT-Paper
 */
BOOST_AUTO_TEST_CASE(Test_NLSATPaper_Ex3)
{

	carl::Variable x = carl::freshRealVariable("x");
	carl::Variable y = carl::freshRealVariable("y");

	// Original constraints
	FormulaT c1(Poly(x)+Rational(1), carl::Relation::LEQ);
	FormulaT c2(Poly(x)-Rational(1), carl::Relation::GEQ);
	FormulaT c3(Poly(x)+y, carl::Relation::GREATER);
	FormulaT c4(Poly(x)-y, carl::Relation::GREATER);
	// Learnt from first conflict
	FormulaT c5(Poly(x), carl::Relation::LESS);

	// Explanation for first conflict
	FormulaT ex1(carl::FormulaType::OR, { c3.negated(), c4.negated(), c5.negated() });
	// Explanation for second conflict
	FormulaT ex2(carl::FormulaType::OR, { c1.negated(), c5 });

	mcsat::MCSATBackend<mcsat::MCSATSettingsNL> nlsat;

	// Decide x+1<=0
	SMTRAT_LOG_INFO("smtrat.test.nlsat", "Decide " << c1);
	nlsat.pushConstraint(c1);

	// T-Decide
	SMTRAT_LOG_INFO("smtrat.test.nlsat", "T-Decide " << x << " = -2");
	{
		auto res = nlsat.findAssignment(x);
		BOOST_CHECK(carl::variant_is_type<mcsat::ModelValues>(res));
		auto value = boost::get<mcsat::ModelValues>(res);
		BOOST_CHECK(value.size() == 1);
		BOOST_CHECK(value[0].first == x);
		BOOST_CHECK(value[0].second == ModelValue(Rational(-1)));
	}
	FormulaT fx(Poly(x) + Rational(2), carl::Relation::EQ);
	nlsat.pushAssignment(x, Rational(-2), fx);

	// Propagate x+y>0
	SMTRAT_LOG_INFO("smtrat.test.nlsat", "Propagate " << c3);
	nlsat.pushConstraint(c3);

	// Check whether x-y>0 is feasible, propagate x-y<=0
	SMTRAT_LOG_INFO("smtrat.test.nlsat", "T-Propagate " << c4.negated());
	{
		auto res = nlsat.isInfeasible(y, c4);
		BOOST_CHECK(carl::variant_is_type<FormulasT>(res));
		auto r = boost::get<FormulasT>(res);
		auto explanation = nlsat.explain(y, r);
		BOOST_CHECK(boost::get<FormulaT>(&explanation) != nullptr);
		BOOST_CHECK(ex1 == boost::get<FormulaT>(explanation));
	}
	nlsat.pushConstraint(c4.negated());

	// Backtrack assignment
	SMTRAT_LOG_INFO("smtrat.test.nlsat", "Backtrack " << x);
	nlsat.popAssignment(x);

	// Check whether x>0 is feasible, propagate x<=0
	SMTRAT_LOG_INFO("smtrat.test.nlsat", "T-Propagate " << c5);
	{
		auto res = nlsat.isInfeasible(x, c5.negated());
		BOOST_CHECK(carl::variant_is_type<FormulasT>(res));
		auto r = boost::get<FormulasT>(res);
		auto explanation = nlsat.explain(x, r);
		BOOST_CHECK(boost::get<FormulaT>(&explanation) != nullptr);
		BOOST_CHECK(ex2 == boost::get<FormulaT>(explanation));
	}
}

BOOST_AUTO_TEST_CASE(CoverComputation)
{
	carl::Variable a = carl::freshRealVariable("a");
	carl::Variable b = carl::freshRealVariable("b");
	Poly p = Poly(20)*a*a*b - Poly(a)*a*a*a*b - Poly(120)*b;
	carl::UnivariatePolynomial<Rational> q(a, {
		Rational(900),
		Rational(0),
		Rational(-2550),
		Rational(0),
		Rational(1055)/2,
		Rational(0),
		Rational(-40),
		Rational(0),
		Rational(1)
	});
	carl::Interval<Rational> i(Rational(633)/1025, carl::BoundType::STRICT, Rational(2533)/4096, carl::BoundType::STRICT);
	FormulaT f(p, carl::Relation::LESS);

	std::cout << "Constructing RAN on " << q << " / " << i << std::endl;
	carl::RealAlgebraicNumber<Rational> ran(q, i);
	std::cout << "-> " << ran << std::endl;

	Model model;
	model.assign(a, ran);
	model.assign(b, Rational(-3));

	auto res = carl::model::evaluate(f, model);
	std::cout << f << " on " << model << " -> " << res << std::endl;

	model.assign(b, Rational(1));
	res = carl::model::evaluate(f, model);
	std::cout << f << " on " << model << " -> " << res << std::endl;
}

BOOST_AUTO_TEST_SUITE_END();


// Boost.Geometry (aka GGL, Generic Geometry Library)
// This file is manually converted from PROJ4

// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Geometry Library by Barend Gehrels (Geodan, Amsterdam)

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_ELL_SET_HPP
#define BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_ELL_SET_HPP

#include <string>
#include <vector>

#include <boost/geometry/util/math.hpp>

#include <boost/geometry/extensions/gis/projections/impl/pj_ellps.hpp>
#include <boost/geometry/extensions/gis/projections/impl/pj_param.hpp>

namespace boost { namespace geometry { namespace projections {

namespace detail {

/* set ellipsoid parameters a and es */
static const double SIXTH =  .1666666666666666667; /* 1/6 */
static const double RA4 = .04722222222222222222; /* 17/360 */
static const double RA6 = .02215608465608465608; /* 67/3024 */
static const double RV4 = .06944444444444444444; /* 5/72 */
static const double RV6 = .04243827160493827160; /* 55/1296 */

/* initialize geographic shape parameters */
inline void pj_ell_set(std::vector<pvalue>& parameters, double &a, double &es)
{
    double b = 0.0;
    double e = 0.0;
    std::string name;

    /* check for varying forms of ellipsoid input */
    a = es = 0.;

    /* R takes precedence */
    if (pj_param(parameters, "tR").i)
        a = pj_param(parameters, "dR").f;
    else { /* probable elliptical figure */

        /* check if ellps present and temporarily append its values to pl */
        name = pj_param(parameters, "sellps").s;
        if (! name.empty())
        {
            const int n = sizeof(pj_ellps) / sizeof(pj_ellps[0]);
            int index = -1;
            for (int i = 0; i < n && index == -1; i++)
            {
                if(pj_ellps[i].id == name)
                {
                    index = i;
                }
            }

            if (index == -1) { throw proj_exception(-9); }

            parameters.push_back(pj_mkparam(pj_ellps[index].major));
            parameters.push_back(pj_mkparam(pj_ellps[index].ell));
        }
        a = pj_param(parameters, "da").f;
        if (pj_param(parameters, "tes").i) /* eccentricity squared */
            es = pj_param(parameters, "des").f;
        else if (pj_param(parameters, "te").i) { /* eccentricity */
            e = pj_param(parameters, "de").f;
            es = e * e;
        } else if (pj_param(parameters, "trf").i) { /* recip flattening */
            es = pj_param(parameters, "drf").f;
            if (!es) {
                throw proj_exception(-10);
            }
            es = 1./ es;
            es = es * (2. - es);
        } else if (pj_param(parameters, "tf").i) { /* flattening */
            es = pj_param(parameters, "df").f;
            es = es * (2. - es);
        } else if (pj_param(parameters, "tb").i) { /* minor axis */
            b = pj_param(parameters, "db").f;
            es = 1. - (b * b) / (a * a);
        }     /* else es == 0. and sphere of radius a */
        if (!b)
            b = a * sqrt(1. - es);
        /* following options turn ellipsoid into equivalent sphere */
        if (pj_param(parameters, "bR_A").i) { /* sphere--area of ellipsoid */
            a *= 1. - es * (SIXTH + es * (RA4 + es * RA6));
            es = 0.;
        } else if (pj_param(parameters, "bR_V").i) { /* sphere--vol. of ellipsoid */
            a *= 1. - es * (SIXTH + es * (RV4 + es * RV6));
            es = 0.;
        } else if (pj_param(parameters, "bR_a").i) { /* sphere--arithmetic mean */
            a = .5 * (a + b);
            es = 0.;
        } else if (pj_param(parameters, "bR_g").i) { /* sphere--geometric mean */
            a = sqrt(a * b);
            es = 0.;
        } else if (pj_param(parameters, "bR_h").i) { /* sphere--harmonic mean */
            a = 2. * a * b / (a + b);
            es = 0.;
        } else {
            int i = pj_param(parameters, "tR_lat_a").i;
            if (i || /* sphere--arith. */
                pj_param(parameters, "tR_lat_g").i) { /* or geom. mean at latitude */
                double tmp;

                tmp = sin(pj_param(parameters, i ? "rR_lat_a" : "rR_lat_g").f);
                if (geometry::math::abs(tmp) > geometry::math::half_pi<double>()) {
                    throw proj_exception(-11);
                }
                tmp = 1. - es * tmp * tmp;
                a *= i ? .5 * (1. - es + tmp) / ( tmp * sqrt(tmp)) :
                    sqrt(1. - es) / tmp;
                es = 0.;
            }
        }
    }

    /* some remaining checks */
    if (es < 0.)
        { throw proj_exception(-12); }
    if (a <= 0.)
        { throw proj_exception(-13); }
}

} // namespace detail
}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_ELL_SET_HPP


/*
 *  Copyright (c) 2012-2015 Evgeny Proydakov <lord.tiran@gmail.com>
 *
 *  Permission is hereby granted, free of charge, to any person obtaining a copy
 *  of this software and associated documentation files (the "Software"), to deal
 *  in the Software without restriction, including without limitation the rights
 *  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 *  copies of the Software, and to permit persons to whom the Software is
 *  furnished to do so, subject to the following conditions:
 *
 *  The above copyright notice and this permission notice shall be included in
 *  all copies or substantial portions of the Software.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 *  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 *  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 *  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 *  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 *  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 *  THE SOFTWARE.
 */

#include "maze.h"
#include <fstream>
#include <boost/graph/graphviz.hpp>

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

vertex_hash::vertex_hash(const grid& graph) :
    m_graph(graph)
{
}

std::size_t vertex_hash::operator()(const vertex_descriptor& v) const
{
    std::size_t seed = 0;
    boost::hash_combine(seed, m_graph[v].x);
    boost::hash_combine(seed, m_graph[v].y);
    return seed;
}

// Exception thrown when the goal vertex is found
struct found_goal {};

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

distance_t euclidean_distance(distance_t dx, distance_t dy)
{
    return static_cast<distance_t>(std::sqrt(dx * dx + dy * dy));
}

distance_t euclidean_squared_distance(distance_t dx, distance_t dy)
{
    return dx * dx + dy * dy;
}

distance_t manhattan_distance(distance_t dx, distance_t dy)
{
    return dx + dy;
}

distance_t diagonal_distance(distance_t dx, distance_t dy)
{
    return static_cast<distance_t>(dx + dy + (14 - 20) / 10.0 * std::min(dx, dy));
}

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

//
// Heuristic for a grid
//
// This calculates the Euclidean distance between a vertex and a goal
// vertex.
//
template<class graph_t, class heuristic_impl>
class euclidean_heuristic : public boost::astar_heuristic<graph_t, distance_t>
{
public:
    euclidean_heuristic(const graph_t& graph, const heuristic_impl& impl, vertex_vector& tested, const vertex_descriptor& goal) :
        m_graph(graph),
        m_impl(impl),
        m_tested(tested),
        m_goal(goal)
    {
    }

    distance_t operator() (vertex_descriptor v) {
        m_tested.push_back(v);

        distance_t dx = std::abs(m_graph[m_goal].x - m_graph[v].x) * 10;
        distance_t dy = std::abs(m_graph[m_goal].y - m_graph[v].y) * 10;

        distance_t result = m_impl(dx, dy);
        return result;
    }

private:
    const graph_t& m_graph;
    const heuristic_impl m_impl;
    vertex_vector& m_tested;
    vertex_descriptor m_goal;
};

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

// Visitor that terminates when we find the goal vertex
template<class graph_t>
struct astar_goal_visitor : public boost::default_astar_visitor
{
    astar_goal_visitor(const vertex_descriptor& goal) :
        m_goal(goal)
    {
    }

    void examine_vertex(const vertex_descriptor& u, const graph_t&) {
        if (u == m_goal) {
            throw found_goal();
        }
    }

private:
    vertex_descriptor m_goal;
};

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

maze::maze(std::size_t width, std::size_t height) :
    m_random_generator(boost::mt19937()),
    m_width(width),
    m_height(height),
    m_heuristic_type(heuristic_type::euclidean),
    m_barriers(width * height, vertex_hash(m_grid)),
    m_barrier_grid(create_barrier_grid())
{
    m_random_generator.seed(static_cast<unsigned>(std::time(0)));

    const size_t size = width * height;
    m_name.resize(size);

    // init horizontal edge
    const distance_t horizontal_size = 10;
    for(size_t x = 0; x < width - 1; x++) {
        for(size_t y = 0; y < height; y++) {
            const size_t index = calc_vertex_index(x, y);
            boost::add_edge(index, index + 1, horizontal_size, m_grid);
        }
    }

    // init vertical edge
    const distance_t vertical_size = 10;
    for(size_t x = 0; x < width; x++) {
        for(size_t y = 0; y < height - 1; y++) {
            const size_t index = calc_vertex_index(x, y);
            boost::add_edge(index, index + width, vertical_size, m_grid);
        }
    }

    // init diagonal edge
    const distance_t diagonal_size = 14;
    for(size_t x = 0; x < width - 1; x++) {
        for(size_t y = 0; y < height - 1; y++) {
            const size_t index = calc_vertex_index(x, y);
            boost::add_edge(index, index + width + 1, diagonal_size, m_grid);
            boost::add_edge(index + 1, index + width, diagonal_size, m_grid);
        }
    }

    // fill name and vertex descriptin
    for(size_t x = 0; x < width; x++) {
        for(size_t y = 0; y < height; y++) {
            const size_t index = calc_vertex_index(x, y);
            vertex_descriptor descriptor(index);
            m_grid[descriptor].x = static_cast<distance_t>(x);
            m_grid[descriptor].y = static_cast<distance_t>(y);
            std::stringstream sstream;
            sstream << "(" << x << ", " << y << ")";
            m_name[index] = sstream.str();
        }
    }

    set_heuristic(m_heuristic_type);
}

void maze::set_heuristic(heuristic_type h)
{
    heuristic_t heuristic;

    switch (h) {
    case heuristic_type::euclidean:
        heuristic = heuristic_t(std::bind(&euclidean_distance, std::placeholders::_1, std::placeholders::_2));
        break;

    case heuristic_type::euclidean_squared:
        heuristic = heuristic_t(std::bind(&euclidean_squared_distance, std::placeholders::_1, std::placeholders::_2));
        break;

    case heuristic_type::manhattan:
        heuristic = heuristic_t(std::bind(&manhattan_distance, std::placeholders::_1, std::placeholders::_2));
        break;

    case heuristic_type::diagonal:
        heuristic = heuristic_t(std::bind(&diagonal_distance, std::placeholders::_1, std::placeholders::_2));
        break;

    default:
        assert(false);
        break;
    }
    if (heuristic) {
        m_heuristic = heuristic;
        m_heuristic_type = h;
    }
}

vertices_size_type maze::length(std::size_t d) const
{
    vertices_size_type size = 0;

    switch(d) {
        case 0:
            size = m_width;
            break;

        case 1:
            size = m_height;
            break;

        default:
            assert(false);
            break;
    }
    return size;
}

vertex_descriptor maze::source_vertex() const
{
    return m_soure;
}

vertex_descriptor maze::goal_vertex() const
{
    return m_goal;
}

// Solve the maze using A-star search.  Return true if a solution was found.
bool maze::solve()
{
    const vertex_descriptor& source = source_vertex();
    const vertex_descriptor& goal = goal_vertex();
    vertex_vector& solution = m_solution;

    const size_t solution_assessment_size = m_width + m_height;
    vertex_hash hash(m_grid);

    // The predecessor map is a vertex-to-vertex mapping.
    typedef boost::unordered_map<vertex_descriptor, vertex_descriptor, vertex_hash> pred_map;
    pred_map predecessor(solution_assessment_size, hash);
    boost::associative_property_map<pred_map> pred_pmap(predecessor);

    // The distance map is a vertex-to-distance mapping.
    typedef boost::unordered_map<vertex_descriptor, distance_t, vertex_hash> dist_map;
    dist_map distance(solution_assessment_size, hash);
    boost::associative_property_map<dist_map> dist_pmap(distance);

    euclidean_heuristic<grid, heuristic_t> heuristic(m_grid, m_heuristic, m_tested, goal);
    astar_goal_visitor<filtered_grid>      visitor  (goal);

    bool result = false;
    const int iters = 1;

    std::cout << "astar search... ";

    std::chrono::high_resolution_clock clock;
    auto start = clock.now();

    for(int i = 0; i < iters; i++) {
        m_tested.clear();
        solution.clear();
        predecessor.clear();
        distance.clear();
        try {
            boost::astar_search(m_barrier_grid, source, heuristic,
                                boost::visitor(visitor).
                                predecessor_map(pred_pmap).
                                distance_map(dist_pmap)
                                );
        }
        catch(const found_goal&) {
            // Walk backwards from the goal through the predecessor chain adding
            // vertices to the solution path.
            for (vertex_descriptor u = goal; u != source; u = predecessor[u]) {
                solution.push_back(u);
            }
            solution.push_back(source);
            std::reverse(solution.begin(), solution.end());
            result = true;
        }
    }
    auto end = clock.now();

    distance_t dist = distance[goal];
    std::cout << "solve. result: " << result << " size: " << dist
        << " heuristic: " << heuristic_type_2_string(m_heuristic_type)
        << " process time: "
        << std::chrono::duration_cast<std::chrono::microseconds>(end - start).count() / iters << " us"
        << std::endl;

    dump_to_dot();

    return result;
}

bool maze::solved() const
{
    return !m_solution.empty();
}

const vertex_vector& maze::get_heuristic_tested() const
{
    return m_tested;
}

const vertex_vector& maze::get_solution() const
{
    return m_solution;
}

const vertex_set& maze::get_barriers() const
{
    return m_barriers;
}

const grid::vertex_bundled& maze::get_vertex_property(vertex_descriptor vertex) const
{
    return m_grid[vertex];
}

bool maze::has_barrier(vertex_descriptor u) const
{
    return m_barriers.find(u) != m_barriers.end();
}

vertex_descriptor maze::next(vertex_descriptor u, std::size_t direction)
{
    size_t delta_x = 0;
    size_t delta_y = 0;

    switch(direction) {
        case 0:
            delta_x = 1;
            break;

        case 1:
            delta_y = 1;
            break;

        default:
            assert(false);
            break;
    }
    const size_t x = static_cast<size_t>(m_grid[u].x);
    const size_t y = static_cast<size_t>(m_grid[u].y);

    const size_t new_x = x + delta_x;
    const size_t new_y = y + delta_y;

    if(new_x >= m_width || new_y >= m_height) {
        return u;
    }
    vertex_descriptor new_u = calc_vertex_index(new_x, new_y);
    return new_u;
}

void maze::dump_to_dot()
{
    std::ofstream dot("astar.dot");
    boost::write_graphviz(dot, m_grid, boost::make_label_writer(&m_name[0]));
    dot.close();
}

filtered_grid maze::create_barrier_grid()
{
    return boost::make_vertex_subset_complement_filter(m_grid, m_barriers);
}

size_t maze::calc_vertex_index(size_t x, size_t y)
{
    return x + y * m_width;
}

std::string maze::heuristic_type_2_string(heuristic_type h) const
{
    std::string text;
    switch (h) {
    case heuristic_type::euclidean:
        text = "euclidean_distance";
        break;

    case heuristic_type::euclidean_squared:
        text = "euclidean_squared_distance";
        break;

    case heuristic_type::manhattan:
        text = "manhattan_distance";
        break;

    case heuristic_type::diagonal:
        text = "diagonal_distance";
        break;

    default:
        assert(false);
        break;
    }
    return text;
}

// Return a random integer in the interval [a, b].
std::size_t maze::random_int(std::size_t a, std::size_t b)
{
    if (b < a) {
        b = a;
    }
    boost::uniform_int<> dist(static_cast<int>(a), static_cast<int>(b));
    boost::variate_generator<boost::mt19937&, boost::uniform_int<> >
    generate(m_random_generator, dist);
    return static_cast<size_t>(generate());
}

// Generate a maze with a random assignment of barriers.
std::shared_ptr<maze> random_maze(std::size_t x, std::size_t y)
{
    auto m = std::make_shared<maze>(x, y);

    vertex_descriptor s = m->calc_vertex_index( 0, 0 );
    vertex_descriptor g = m->calc_vertex_index( x - 1, y -1 );
    m->m_soure = s;
    m->m_goal  = g;

    vertices_size_type n = num_vertices(m->m_grid);

    // One quarter of the cells in the maze should be barriers.
    int barriers = static_cast<int>(pow(n, 0.75));
    while (barriers > 0) {
        // Choose horizontal or vertical direction.
        std::size_t direction = m->random_int(0, 1);
        // Walls range up to one quarter the dimension length in this direction.
        vertices_size_type wall = m->random_int(1, m->length(direction) / 4);
        // Create the wall while decrementing the total barrier count.
        vertex_descriptor u = vertex(m->random_int(0, n - 1), m->m_grid);
        while (wall) {
            // Start and goal spaces should never be barriers.
            if (u != s && u != g) {
                wall--;
                if (!m->has_barrier(u)) {
                    m->m_barriers.insert(u);
                    barriers--;
                }
            }
            vertex_descriptor v = m->next(u, direction);
            // Stop creating this wall if we reached the maze's edge.
            if (u == v) {
                break;
            }
            u = v;
        }
    }
    return m;
}

std::shared_ptr<maze> empty_maze(std::size_t width, std::size_t height)
{
    auto m = std::make_shared<maze>(width, height);

    vertex_descriptor s = m->calc_vertex_index( 0, 0 );
    vertex_descriptor g = m->calc_vertex_index( width - 1, height -1 );
    m->m_soure = s;
    m->m_goal  = g;

    return m;
}

std::shared_ptr<maze> fixed()
{
    auto m = std::make_shared<maze>(9, 9);

    vertex_descriptor u1 = m->calc_vertex_index(4, 2);
    vertex_descriptor u2 = m->calc_vertex_index(4, 3);
    vertex_descriptor u3 = m->calc_vertex_index(4, 4);
    vertex_descriptor u4 = m->calc_vertex_index(4, 5);

    m->m_barriers.insert(u1);
    m->m_barriers.insert(u2);
    m->m_barriers.insert(u3);
    m->m_barriers.insert(u4);

    vertex_descriptor source = m->calc_vertex_index(2, 4);
    m->m_soure = source;

    vertex_descriptor goal = m->calc_vertex_index(6, 4);
    m->m_goal = goal;

    return m;
}


#include <costar_task_plan/dmp_trajectory_distribution.h>

#include <Eigen/Dense>
#include <trajectory_msgs/JointTrajectoryPoint.h>

using trajectory_msgs::JointTrajectoryPoint;
using namespace Eigen;

#define SHOW_SAMPLED_VALUES 0
#define DEFAULT_SIGMA 0.1

namespace costar {

  /**
   * Initialize a trajectory distribution with given params
   */
  CartDmpTrajectoryDistribution::DmpTrajectoryDistribution(unsigned int dim_, unsigned int nbasis_, RobotKinematicsPointer robot_)
    : dim(dim_),
    nbasis(nbasis_),
    robot(robot_),
    dist((dim_*nbasis_) + POSE_FEATURES_SIZE,1),
    verbose(false),
    dmp_list(dim_),
    dmp_goal(dim_),
    k_gain(100),
    d_gain(20),
    tau(1.0),
    goal_threshold(dim_,0.01),
    dmp_velocity_multiplier(0.1)
  {

    assert(dim == robot->getDegreesOfFreedom());

    // set up anything else?
    nvars = dist.ns[0].mu.size();

    for (unsigned int i = 0; i < dim; ++i) {
      dmp_list[i].k_gain = k_gain;
      dmp_list[i].d_gain = d_gain;
      dmp_list[i].weights.resize(nbasis);
    }
  }

  /**
   * initialize
   * set up the distribution based on a skill and an environment
   */
  void CartDmpTrajectoryDistribution::initialize(TestFeatures &features, const Skill &skill, std::vector<double> sigma) {
    //Pose p0 = features.lookup(AGENT);
    Pose p1 = features.lookup(skill.getInitializationFeature()) * skill.getInitializationFinalPose();

    unsigned int idx = 0;

    dist.ns[0].mu[POSE_FEATURE_X] = p1.p.x();
    dist.ns[0].mu[POSE_FEATURE_Y] = p1.p.y();
    dist.ns[0].mu[POSE_FEATURE_Z] = p1.p.z();

    double roll, pitch, yaw;
    p1.M.GetRPY(roll, pitch, yaw);

    dist.ns[0].mu[POSE_FEATURE_YAW] = yaw;
    dist.ns[0].mu[POSE_FEATURE_PITCH] = pitch;
    dist.ns[0].mu[POSE_FEATURE_ROLL] = roll;

    for (int j = POSE_RPY_SIZE; j < nvars; ++j) {
      dist.ns[0].mu[j] = 0;
    }

    /*for (unsigned int j = 0; j < dist.ns[0].mu.size(); ++j) {
      std::cout<<dist.ns[0].mu(j)<<"\n";
      }*/

    if (sigma.size() < nvars) {
      if (verbose) {
        std::cerr << "[GRID/TRAJECTORY DISTRIBUTION] Noise argument for trajectory search initialization was the wrong size!" << std::endl;
        std::cerr << "[GRID/TRAJECTORY DISTRIBUTION] Should be: " << dim << std::endl;
        std::cerr << "[GRID/TRAJECTORY DISTRIBUTION] Was: " << sigma.size() << std::endl;
      }
      for (int j = 0; j < nvars; ++j) {
        dist.ns[0].P(j,j) = DEFAULT_SIGMA;
      }

    } else {
      for (int j = 0; j < nvars; ++j) {
        dist.ns[0].P(j,j) = sigma[j];
      }
    }
    dist.Update();

  }

  /**
   * sample
   * Pull a random trajectory from the gmm
   * Convert it into a KDL trajectory
   * NON-CONST becuse Gmm::sample is likewise non-const
   */
  void CartDmpTrajectoryDistribution::sample(std::vector<EigenVectornd> &params,std::vector<JointTrajectory> &trajs) {

    using KDL::Vector;
    using KDL::Rotation;

    unsigned int nsamples = params.size();
    trajs.resize(nsamples);

    for (int sample = 0; sample < nsamples; ++sample) {

      EigenVectornd vec;
      vec.resize(nvars);
      dist.Sample(vec);

      params[sample] = vec;

#if SHOW_SAMPLED_VALUES
      std::cout << "Sampled: ";
      for (int j = 0; j < dim; ++j) {
        std::cout << vec[j] << " ";
      }
      std::cout << std::endl;
#endif

      // convert the first six into a pose
      Vector v1 = Vector(vec[POSE_FEATURE_X],vec[POSE_FEATURE_Y],vec[POSE_FEATURE_Z]);
      Rotation r1 = Rotation::RPY(vec[POSE_FEATURE_ROLL],vec[POSE_FEATURE_PITCH],vec[POSE_FEATURE_YAW]);
      Pose p(r1,v1);

      robot->IkPos(p,q);
      unsigned int idx = 0;
      //std::cout << "GOAL = ";
      for (; idx < dim; ++idx) {
        dmp_goal[idx] = q(idx);
        //std::cout << q(idx) << " ";
      }
      //std::cout << std::endl;

      for (unsigned int i = 0; i < dim; ++i) {
        for (unsigned int j = 0; j < nbasis; ++j) {
          dmp_list[i].weights[j] = vec[idx++];
        }
      }

      unsigned char at_goal;
      dmp::DMPTraj plan;

      dmp::generatePlan(dmp_list,robot->getJointPos(),robot->getJointVel(),0,dmp_goal,goal_threshold,-1,tau,0.1,5,plan,at_goal);

      if (verbose) {
        std::cout << "--------------------------" << std::endl;

        std::cout << "Using joints = ";
        for (const double &q: robot->getJointPos()) {
          std::cout << q << " ";
        }
        std::cout << std::endl;        std::cout << "at goal: " << (unsigned int)at_goal << std::endl;
        std::cout << "points: " << plan.points.size() << std::endl;
      }

      trajs[sample].points.resize(plan.points.size());
      for (unsigned int j = 0; j < plan.points.size(); ++j) {
        trajs[sample].points[j].positions = plan.points[j].positions;
        trajs[sample].points[j].velocities = plan.points[j].velocities;
        for (double &v: trajs[sample].points[j].velocities) {
          v *= dmp_velocity_multiplier;
        }
      }
    }
  }

  /* ==================================================================== */
  /* ==================================================================== */
  /*             BELOW THIS: SAME AS TRAJECTORY DISTRIBUTION              */
  /* ==================================================================== */
  /* ==================================================================== */

  /**
   * update
   * take a set of trajectories and samples
   * use the trajectories to reweight the distribution
   */
  void CartDmpTrajectoryDistribution::update(
      std::vector<EigenVectornd> &params,
      std::vector<double> &ps,
      double diagonal_noise)
  {
    update(params,ps,diagonal_noise,def_step_size);
  }

  /**
   * update
   * take a set of trajectories and samples
   * use the trajectories to reweight the distribution
   */
  void CartDmpTrajectoryDistribution::update(
      std::vector<EigenVectornd> &params,
      std::vector<double> &ps)
  {
    update(params,ps,diagonal_sigma,def_step_size);
  }

  /**
   * update
   * take a set of trajectories and samples
   * use the trajectories to reweight the distribution
   */
  void CartDmpTrajectoryDistribution::update(
      std::vector<EigenVectornd> &params,
      std::vector<double> &ps,
      double diagonal_noise,
      double step_size)
  {

    double psum = 0;
    for (double &d: ps) {
      psum += d;
    }

    if (dist.k == 1) {

      // one cluster only
      // compute mean

      dist.ns[0].mu *= (1 - step_size); //setZero();
      dist.ns[0].P *= (1 - step_size); //setZero();

      for (unsigned int i = 0; i < params.size(); ++i) {
        //std::cout << "mu rows = " << dist.ns[0].mu.rows() << ", vec rows = " << vec.rows() << std::endl;
        //std::cout << "mu cols = " << dist.ns[0].mu.cols() << ", vec cols = " << vec.cols() << std::endl;
        double wt = step_size * ps[i] / psum;
        dist.ns[0].mu += params[i] * wt;
      }

      for (unsigned int i = 0; i < params.size(); ++i) {
        double wt = step_size * ps[i] / psum;
        //std::cout << wt << ", " << ps[i] << ", " << psum << std::endl;
        dist.ns[0].P += wt * (params[i] - dist.ns[0].mu) * (params[i] - dist.ns[0].mu).transpose();
      }

    } else {

      // set up weighted data
      // and then fit GMM again

      std::vector<std::pair<EigenVectornd,double> > data(ps.size());
      for (unsigned int i = 0; i < ps.size(); ++i) {
        data[0].first = params[i];
        data[0].second = ps[i] / psum;
      }

      dist.Fit(data);

    }

    for (unsigned int i = 0; i < dist.k; ++i) {
      dist.ns[0].P += diagonal_noise * Matrix<double,Dynamic,Dynamic>::Identity(nvars,nvars);
    }

    dist.Update();
  }
};


#ifndef BASE_H_
#define BASE_H_

#include <cmath>
#include <vector>
#include <map>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <sstream>
#include <string>
#include <algorithm>

#include <boost/array.hpp>
#include <boost/multi_array.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/uniform_int_distribution.hpp>

typedef std::vector<std::string> string_vector;
typedef std::vector<bool> bool_vector;

typedef boost::multi_array<double,1> darray1;
typedef boost::multi_array<double,2> darray2;
typedef boost::multi_array<double,3> darray3;
typedef boost::multi_array<double,4> darray4;

typedef boost::multi_array<bool,2> barray2;
typedef boost::multi_array<bool,3> barray3;
typedef boost::multi_array<bool,4> barray4;

const int nstate = 20;
const int nstate_match = 20;
const int nstate_delete = 1;
const int nstate_core = nstate_match;
const int nstate_insert = 20;
const int terminal_state = nstate_match;
const char terminal_residue = '-';

const double match_fraction = 0.75;

const double neg_infinity = -std::numeric_limits<double>::infinity();
const double pos_infinity = std::numeric_limits<double>::infinity();

//                            01234567890123456789
const std::string amino1_core =  "ACDEFGHIKLMNPQRSTVWY-";
const std::string amino1_insert = "acdefghiklmnpqrstvwy";
int find_amino1_core(const char a);
int find_amino1_insert(const char a);

bool amino1_core_p(const char a);
bool amino1_insert_p(const char a);

double log_add(const double a, const double b);
double log_dot(darray1& a, darray1& b, const int n);
void log_mat_mult(darray2& a, darray2& b, darray2& c,
		  const int l, const int m, const int n);

#endif //BASE_H_


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2000, 2001, 2002, 2003 RiskMap srl
 Copyright (C) 2003, 2004, 2005, 2006, 2007 StatPro Italia srl

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file instrument.hpp
    \brief Abstract instrument class
*/

#ifndef quantlib_instrument_hpp
#define quantlib_instrument_hpp

#include <ql/patterns/lazyobject.hpp>
#include <ql/pricingengine.hpp>
#include <ql/utilities/null.hpp>
#include <ql/time/date.hpp>
#include <boost/any.hpp>
#include <map>
#include <string>

namespace QuantLib {

    //! Abstract instrument class
    /*! This class is purely abstract and defines the interface of concrete
        instruments which will be derived from this one.

        \test observability of class instances is checked.
    */
    class Instrument : public LazyObject {
      public:
        class results;
        Instrument();
        //! \name Inspectors
        //@{

        //! returns the net present value of the instrument.
        Real NPV() const;
        //! returns the error estimate on the NPV when available.
        Real errorEstimate() const;
        //! returns the date the net present value refers to.
        const Date& valuationDate() const;

        //! returns any additional result returned by the pricing engine.
        template <typename T> T result(const std::string& tag) const;
        //! returns all additional result returned by the pricing engine.
        const std::map<std::string,boost::any>& additionalResults() const;

        //! returns whether the instrument might have value greater than zero.
        virtual bool isExpired() const = 0;
        //@}
        //! \name Modifiers
        //@{
        //! set the pricing engine to be used.
        /*! \warning calling this method will have no effects in
                     case the performCalculation method
                     was overridden in a derived class.
        */
        void setPricingEngine(const ext::shared_ptr<PricingEngine>&);
        //@}
        /*! When a derived argument structure is defined for an
            instrument, this method should be overridden to fill
            it. This is mandatory in case a pricing engine is used.
        */
        virtual void setupArguments(PricingEngine::arguments*) const;
        /*! When a derived result structure is defined for an
            instrument, this method should be overridden to read from
            it. This is mandatory in case a pricing engine is used.
        */
        virtual void fetchResults(const PricingEngine::results*) const;
      protected:
        //! \name Calculations
        //@{
        void calculate() const;
        /*! This method must leave the instrument in a consistent
            state when the expiration condition is met.
        */
        virtual void setupExpired() const;
        /*! In case a pricing engine is not used, this
            method must be overridden to perform the actual
            calculations and set any needed results. In case
            a pricing engine is used, the default implementation
            can be used.
        */
        virtual void performCalculations() const;
        //@}
        /*! \name Results
            The value of this attribute and any other that derived
            classes might declare must be set during calculation.
        */
        //@{
        mutable Real NPV_, errorEstimate_;
        mutable Date valuationDate_;
        mutable std::map<std::string,boost::any> additionalResults_;
        //@}
        ext::shared_ptr<PricingEngine> engine_;
    };

    class Instrument::results : public virtual PricingEngine::results {
      public:
        void reset() {
            value = errorEstimate = Null<Real>();
            valuationDate = Date();
            additionalResults.clear();
        }
        Real value;
        Real errorEstimate;
        Date valuationDate;
        std::map<std::string,boost::any> additionalResults;
    };

    // inline definitions

    inline Instrument::Instrument()
    : NPV_(Null<Real>()), errorEstimate_(Null<Real>()),
      valuationDate_(Date()) {}

    inline void Instrument::setPricingEngine(
                                  const ext::shared_ptr<PricingEngine>& e) {
        if (engine_ != 0)
            unregisterWith(engine_);
        engine_ = e;
        if (engine_ != 0)
            registerWith(engine_);
        // trigger (lazy) recalculation and notify observers
        update();
    }

    inline void Instrument::setupArguments(PricingEngine::arguments*) const {
        QL_FAIL("Instrument::setupArguments() not implemented");
    }

    inline void Instrument::calculate() const {
        if (!calculated_) {
            if (isExpired()) {
                setupExpired();
                calculated_ = true;
            } else {
                LazyObject::calculate();
            }
        }
    }

    inline void Instrument::setupExpired() const {
        NPV_ = errorEstimate_ = 0.0;
        valuationDate_ = Date();
        additionalResults_.clear();
    }

    inline void Instrument::performCalculations() const {
        QL_REQUIRE(engine_, "null pricing engine");
        engine_->reset();
        setupArguments(engine_->getArguments());
        engine_->getArguments()->validate();
        engine_->calculate();
        fetchResults(engine_->getResults());
    }

    inline void Instrument::fetchResults(
                                      const PricingEngine::results* r) const {
        const Instrument::results* results =
            dynamic_cast<const Instrument::results*>(r);
        QL_ENSURE(results != 0,
                  "no results returned from pricing engine");

        NPV_ = results->value;
        errorEstimate_ = results->errorEstimate;
        valuationDate_ = results->valuationDate;

        additionalResults_ = results->additionalResults;
    }

    inline Real Instrument::NPV() const {
        calculate();
        QL_REQUIRE(NPV_ != Null<Real>(), "NPV not provided");
        return NPV_;
    }

    inline Real Instrument::errorEstimate() const {
        calculate();
        QL_REQUIRE(errorEstimate_ != Null<Real>(),
                   "error estimate not provided");
        return errorEstimate_;
    }

    inline const Date& Instrument::valuationDate() const {
        calculate();
        QL_REQUIRE(valuationDate_ != Date(),
                   "valuation date not provided");
        return valuationDate_;
    }

    template <class T>
    inline T Instrument::result(const std::string& tag) const {
        calculate();
        std::map<std::string,boost::any>::const_iterator value =
            additionalResults_.find(tag);
        QL_REQUIRE(value != additionalResults_.end(),
                   tag << " not provided");
        return boost::any_cast<T>(value->second);
    }

    inline const std::map<std::string,boost::any>&
    Instrument::additionalResults() const {
        return additionalResults_;
    }

}

#endif