/*
 * PowerStateGraph.cpp
 *
 *  Created on: 3 Aug 2011
 *      Author: jack
 */

#include "PowerStateGraph.h"
#include "AggregateData.h"
#include <iostream>
#include <list>
#include <boost/graph/graphviz.hpp>
#include <cstdio> // sprintf

using namespace std;

PowerStateGraph::PowerStateGraph()
: totalCount(0), aggData(0)
{
    using namespace boost;

    // add a vertex to represent "off"
    // uses Statistic's default constructor to make a Statistic with all-zeros
    offVertex = add_vertex(powerStateGraph);

}

void PowerStateGraph::addItemToEdgeHistory(
        const PSGraph::edge_descriptor& edge
        )
{
    // Check if edge histories are disabled
    if (EDGE_HISTORY_SIZE == 0)
        return;

    if (edgeHistory.size() >= EDGE_HISTORY_SIZE)
        edgeHistory.erase( edgeHistory.begin() );

    edgeHistory.push_back(edge);
}

const Statistic< double >& PowerStateGraph::getEnergyConsumption() const
{
    return energyConsumption;
}

/**
 * @brief Update or initialise Power State Graph.
 */
void PowerStateGraph::update(
        const Signature& sig,
        const bool verbose
        )
{
    const double energyConsumptionFromSig = sig.getEnergyConsumption();
    energyConsumption.update( energyConsumptionFromSig );
    cout << "Energy consumption from sig" << sig.getID() << " = "
         << energyConsumptionFromSig / J_PER_KWH << " kWh" << endl;

    edgeHistory.clear();

    // get the gradient spikes for the signature
    list<Signature::Spike> spikes = sig.getDeltaSpikes();
    list<Signature::Spike>::iterator spike;
    PSGraph::vertex_descriptor targetVertex, sourceVertex=offVertex;
    size_t indexOfLastAcceptedSpike = 0;
    size_t start=0, end=0;

    // for each spike, locate the samples immediately before and immediately after the spike
    const size_t WINDOW = 8; // how far either side of the spike will we look?

    // take just the top TOP_SLICE_SIZE (whilst ordered by absolute value)
    const size_t TOP_SLICE_SIZE = 10;
    if (spikes.size() > TOP_SLICE_SIZE) {
        list<Signature::Spike>::iterator it = spikes.begin();
        advance( it, TOP_SLICE_SIZE );
        spikes.erase( it, spikes.end() );
    }

    // re-order by index (i.e. by time)
    spikes.sort( Signature::Spike::compareIndexAsc );

    for (spike = spikes.begin(); spike!=spikes.end(); spike++) {

        // calculate the start index for the 8 pre-spike samples
        start = ((spike->index > WINDOW) ? (spike->index - WINDOW) : 0 );

        // calculate the end index for the 8 post-spike samples
        end = spike->index + WINDOW + spike->n + 1;
        if (end > sig.getSize())
            end = sig.getSize();

        // Create statistics for pre- and post-spike
        Statistic<Sample_t> preSpikePowerState(sig, start, spike->index);
        Statistic<Sample_t> postSpikePowerState(sig, (spike->index + spike->n + 1), end);

        // check to see if spikes needs to be rejected and that the spikes aren't too close
        if (rejectSpike(preSpikePowerState, postSpikePowerState) || start < indexOfLastAcceptedSpike) {
            if (verbose) cout << " REJECT";
        } else {

            // Create inter-spike stats
            Statistic<Sample_t> betweenSpikesPowerState(
                    sig,
                    (spike->index + spike->n + 1),
                    indexOfNextSpike( spikes, spike, sig )
                    );

            targetVertex =
                    updateOrInsertVertex(
                            sig,
                            postSpikePowerState,
                            betweenSpikesPowerState
                            );

            if (sourceVertex != targetVertex) {
                updateOrInsertEdge( sourceVertex, targetVertex,
                        (spike->index - indexOfLastAcceptedSpike), spike->delta );
            }

            if (verbose)
                printSpikeInfo( spike, start, end, preSpikePowerState, postSpikePowerState, sig);

            sourceVertex = targetVertex;
            indexOfLastAcceptedSpike = spike->index;

        }
    }
}

/**
 * @brief Useful for diagnostics.
 */
void PowerStateGraph::printSpikeInfo(
        const list<Signature::Spike>::iterator spike,
        const size_t start,
        const size_t end,
        const Statistic<Sample_t>& preSpikePowerState,
        const Statistic<Sample_t>& postSpikePowerState,
        const Signature& sig
        ) const
{

    cout << endl
            << "SPIKE: index=" << spike->index
            << ", delta=" << spike->delta
            << ", duration=" << spike->n
            << endl
            << " KEEP" << endl
            << "Before=" << preSpikePowerState << endl
            << "After =" << postSpikePowerState  << endl;

    for (size_t i=start; i<end; i++ ) {
        cout << i << "\t" << sig[i];
        if (i < spike->index)
            cout << " before";
        else if (i == spike->index)
            cout << " <--SPIKE";
        else if (i < (spike->index + spike->n))
            cout << " <--Spike continues";
        else
            cout << " after";
        cout << endl;
    }

}

/**
 * @brief Determine the index of the next spike after @c spike
 */
const size_t PowerStateGraph::indexOfNextSpike(
        const list<Signature::Spike>& spikes,
        list<Signature::Spike>::iterator spike,
        const Signature& sig
        ) const
{
    list<Signature::Spike>::iterator spikePlusOne = spike;
    spikePlusOne++;
    if (spikePlusOne != spikes.end())
        return spikePlusOne->index;
    else
        return sig.getSize() - 1;
}

/**
 * @brief Decides whether or not to reject this spike based on the
 * @c before and @c after statistics.
 */
const bool PowerStateGraph::rejectSpike(
        const Statistic<Sample_t>& before,
        const Statistic<Sample_t>& after,
        const bool verbose
        ) const
{
    // check the stdev isn't too large
    if ( (before.stdev > fabs(before.mean)) ||
         ( after.stdev > fabs(after.mean))   ) {
        if (verbose) cout << "stdev too big";
        return true;
    }

    // check the means aren't too close
    if (Utils::within(before.mean, after.mean, Utils::highest(before.stdev, after.stdev)*2) ||
        Utils::within(before.mean, after.mean, Utils::highest(before.mean, after.mean)*0.2)     ) {
        if (verbose) cout << "means too close";
        return true;
    }

    // if we get to here then don't reject
    return false;
}

/**
 * @brief Attempts to find an existing vertex which is
 * statistically similar to @c stat.  If an existing similar
 * vertex is found then the vertex's stats are updated
 * with the new data points. If a similar vertex is not found,
 * and new vertex is inserted.  Either way, a vertex_descriptor
 * is returned to the new or existing similar vertex.
 */
PowerStateGraph::PSGraph::vertex_descriptor PowerStateGraph::updateOrInsertVertex(
        const Signature& sig, /**< source of the raw data */
        const Statistic<Sample_t>& postSpikePowerState,
        const Statistic<Sample_t>& betweenSpikesPowerState,
        const bool verbose  /**< cout debugging messages? */
    )
{
    bool foundSimilar; // return param for mostSimilarVertex()
    PSGraph::vertex_descriptor vertex = mostSimilarVertex( &foundSimilar, postSpikePowerState );
    // mostSimilarVertex() uses a T-Test to find the powerStateGraph
    // vertex with a mean most similar to postSpikePowerState.

    if (verbose)
        cout << " mostSimlar = " << powerStateGraph[vertex].postSpike << endl;

    if ( foundSimilar ) {
        if (vertex != offVertex) {
            // Update an existing vertex

            if (verbose) {
                cout << endl
                        << "Updating existing vertex: " << endl
                        << "    existing vertex = " << powerStateGraph[vertex].postSpike << endl
                        << "    new stat        = " << postSpikePowerState << endl;
            }

            powerStateGraph[vertex].postSpike.update( postSpikePowerState );
            powerStateGraph[vertex].betweenSpikes.update( betweenSpikesPowerState );

            if (verbose)
                cout << "    merged          = " << powerStateGraph[vertex].postSpike << endl;
        }
    } else {
        // Add a new vertex
        vertex = add_vertex(powerStateGraph);
        powerStateGraph[vertex].postSpike = postSpikePowerState;
        powerStateGraph[vertex].betweenSpikes = betweenSpikesPowerState;

        if (verbose) {
            cout << endl
                 << "Adding new vertex: " << powerStateGraph[vertex].postSpike << endl;
        }

    }
    return vertex;

}

/**
 * @brief Find the vertex statistically most similarto @c stat.
 *
 * @return vertex descriptor of best fit.
 *         @c success is also used as a return parameter.
 *
 * @todo remove commented-out code if it proves to be of no use
 */
PowerStateGraph::PSGraph::vertex_descriptor PowerStateGraph::mostSimilarVertex(
        bool * success, /**< return parameter.  Did we find a satisfactory match? */
        const Statistic<Sample_t>& stat, /**< stat to find in graph vertices */
        const double ALPHA /**< significance level (what constitutes as a "satisfactory" match?) */
    ) const
{
    PSGraph::vertex_descriptor vertex=0;
    std::pair<PSG_vertex_iter, PSG_vertex_iter> vp;
    double tTest, highestTTest=0;

    // Find the best fit
    for (vp = boost::vertices(powerStateGraph); vp.first != vp.second; ++vp.first) {
        // t test
        tTest = stat.tTest( powerStateGraph[*vp.first].postSpike );
        if (tTest > highestTTest) {
            highestTTest = tTest;
            vertex = *vp.first;
        }

    }

    // Check whether the best fit is satisfactory
    if ( (highestTTest > (ALPHA/2)) // T-Test
       ||  ( Utils::within
                 (
                 powerStateGraph[vertex].postSpike.mean,
                 stat.mean,
                 (Utils::highest(powerStateGraph[vertex].postSpike.mean, stat.mean) * 0.2)
                 ) // check if the means are within 20% of each other
         )
      )
        *success = true;
    else
        *success = false;

    return vertex;
}

/**
 * @brief Update or Insert a new edge into powerStateGraph.
 *
 * - If an edge already exists between beforeVertex and afterVertex then
 *     - check all out-edges from beforeVertex
 *     - if any out-edge has the same history as the current history
 *           and the same sign delta then
 *           and and is within 3 stdevs of the existing delta then:
 *         - update that edge's statistics
 *     - else
 *         - create a new edge.
 * - else
 *     - create a new edge
 */
void PowerStateGraph::updateOrInsertEdge(
        const PSGraph::vertex_descriptor& beforeVertex,
        const PSGraph::vertex_descriptor& afterVertex,
        const size_t samplesSinceLastSpike,
        const double spikeDelta,
        const bool verbose
        )
{
    totalCount++;

    if (verbose) cout << "-------------------" << endl;

    PSGraph::edge_descriptor existingEdge, newEdge;
    bool edgeExistsAlready;
    tie(existingEdge, edgeExistsAlready) = boost::edge(beforeVertex, afterVertex, powerStateGraph);

    if (verbose) {
        if ( edgeExistsAlready ) {
            cout << "edgeExistsAlready" << endl;
        } else {
            cout << "edge does not Exist Already" << endl;
        }
    }

    if ( edgeExistsAlready ) {

        if (verbose) {
            for (list<PSGraph::edge_descriptor>::iterator edge=edgeHistory.begin(); edge!=edgeHistory.end(); edge++) {
                cout << "                              edgeHistory = " << *edge << endl;
            }
            for (list<PSGraph::edge_descriptor>::iterator edge=powerStateGraph[existingEdge].edgeHistory.begin();
                    edge!=powerStateGraph[existingEdge].edgeHistory.end(); edge++) {
                cout << "powerStateGraph[existingEdge].edgeHistory = " << *edge << endl;
            }
            cout << endl;
        }

        // check if any of the out edges from beforeVertex have the same
        // history as our current history... if so, update that edge.
        PSGraph::out_edge_iterator out_e_i, out_e_end;
        tie(out_e_i, out_e_end) = out_edges(beforeVertex, powerStateGraph);
        for (; out_e_i != out_e_end; out_e_i++) {
            // check if the edge has the same history and same sign delta and is within 3 stdevs of the existing delta
            if ( edgeListsAreEqual(powerStateGraph[*out_e_i].edgeHistory, edgeHistory) &&
                    Utils::sameSign(powerStateGraph[*out_e_i].delta.mean, spikeDelta ) &&
                    Utils::within(powerStateGraph[*out_e_i].delta.mean,
                            spikeDelta, powerStateGraph[*out_e_i].delta.nonZeroStdev()*3 )) {

                if (verbose) {
                    cout << "edge histories the same. merging with" << *out_e_i
                            << powerStateGraph[*out_e_i].delta << endl;
                }

                // update existing edge's stats
                powerStateGraph[*out_e_i].delta.update( spikeDelta );
                powerStateGraph[*out_e_i].duration.update( samplesSinceLastSpike );
                powerStateGraph[*out_e_i].count++;

                addItemToEdgeHistory( *out_e_i );

                return;
            }
        }
    }

    // if we get to here then we know that either:
    //    an edge doesn't already exist between these 2 vertices
    // or
    //    an edge does exist but it doesn't share our history.
    // either way, we need to add a new edge:

    tie(newEdge, edgeExistsAlready) = boost::add_edge(beforeVertex, afterVertex, powerStateGraph);
    powerStateGraph[newEdge].delta    = Statistic<double>( spikeDelta );
    powerStateGraph[newEdge].duration = Statistic<size_t>( samplesSinceLastSpike );
    powerStateGraph[newEdge].count    = 1;
    powerStateGraph[newEdge].edgeHistory = edgeHistory;

    if (verbose) {
        cout << "adding new edge" << newEdge << endl
            << "   delta stats    = " << powerStateGraph[newEdge].delta << endl
            << "   duration stats = " << powerStateGraph[newEdge].duration << endl;
    }

    addItemToEdgeHistory( newEdge );
}

/**
 * @brief Check if edge list @c a and edge list @c b are equal.
 */
const bool PowerStateGraph::edgeListsAreEqual(
        const list< PSGraph::edge_descriptor >& a,
        const list< PSGraph::edge_descriptor >& b,
        const bool verbose
        ) const
{
    // Check if edge histories are disabled
    if (EDGE_HISTORY_SIZE == 0)
        return true;

    //******************* DIAGNOSTICS *******************
    if (verbose ) {
        cout << "************" << endl
                << "comparing lists:" << endl;

        for (list< PSGraph::edge_descriptor >::const_iterator i=a.begin(); i!=a.end(); i++) {
            cout << *i << endl;
        }
        cout << "----" << endl;
        for (list< PSGraph::edge_descriptor >::const_iterator i=b.begin(); i!=b.end(); i++) {
            cout << *i << endl;
        }
    }//___________________________________________________

    if (a.size() != b.size()) {
        if (verbose) cout << "sizes not equal" << endl;
        return false;
    }

    if (verbose) cout << "************" << endl;

    list<PSGraph::edge_descriptor>::const_iterator a_i, b_i;

    a_i = a.begin();
    b_i = b.begin();

    while (a_i != a.end() && b_i != b.end()) {

        if (verbose) cout << "*a_i=" << *a_i << " *b_i=" << *b_i << endl;

        if ( source(*a_i, powerStateGraph) != source(*b_i, powerStateGraph) &&
             target(*a_i, powerStateGraph) != target(*b_i, powerStateGraph)   ) {

            if (verbose) cout << "edge list are not equal" << endl << endl;

            return false;
        }

        a_i++;
        b_i++;
    }

    return true;
}

/**
 * @brief Update (or create) directional edges between power states (vertices)
 *        so the edges are consistent with the observed transitions between
 *        power states in @c sig.
 *
 * Basic strategy is to:
 *

 *
retrieve gradient spikes from @c sig, extract the most salient
 *      and put the spikes into temporal order.

 *
Create stats for the data points between each spike. Use mostSimilarVertex()
 *      to determine which of the existing power state vertices this belongs to.

 *
Check to see if there's an existing edge representing the power state transition,
 *      if so then check the @c delta and @c duration stats for the edge and update if necessary.
 *      If no edge exists then create one with the necessary @c delta and @c duration,
 *      with a hard-coded stdev.

 *

 *
 *  @deprecated just leaving this here to illustrate one of the strategies I tried.
 *  This has been superseded by update() (which used to be updateVertices) and updateOrInsertEdge()
 *
 */
void PowerStateGraph::updateEdges( const Signature& sig )
{
    Statistic<Sample_t> nextPowerState, prevPowerState;
    PSGraph::vertex_descriptor mostSimVertex, previousVertex;
    pair<PSGraph::edge_descriptor, bool> addedEdge;
    size_t previousIndex;

    // get the gradient spikes for the first signature
    list<Signature::Spike> spikes = sig.getDeltaSpikes();

    // take just the top ten (whilst ordered by absolute value)
    if (spikes.size() > 10) {
        list<Signature::Spike>::iterator it = spikes.begin();
        advance( it, 10 );
        spikes.erase( it, spikes.end() );
    }

    // re-order by index (i.e. by time)
    spikes.sort( Signature::Spike::compareIndexAsc );

    // calculate stats for the signature's values between start and first spike
    // as long as the first spike->index > 1
    previousVertex = offVertex;
    previousIndex  = 0;

    list<Signature::Spike>::iterator spike, prevSpike;
    prevSpike = spike = spikes.begin();
    advance(spike, 1);
    for (; spike!=spikes.end(); spike++ ) {

        nextPowerState = Statistic<Sample_t>( sig, prevSpike->index+1, prevSpike->index+10); // spike->index);

        bool foundSimilar;
        mostSimVertex = mostSimilarVertex( &foundSimilar, nextPowerState, 0.1 );

        if (foundSimilar && previousVertex!=mostSimVertex) {
            /* Add an edge from previousVertex to mostSimVertex.
             * If an edge already exists then boost::add_edge will
             * return an edge_descriptor to that edge. */
            addedEdge = boost::add_edge(previousVertex, mostSimVertex, powerStateGraph);

            if (addedEdge.second) { // then there was not an edge already in the graph
                powerStateGraph[addedEdge.first].delta =
                        Statistic<double>( prevSpike->delta );
                powerStateGraph[addedEdge.first].duration =
                        Statistic<size_t>( prevSpike->index - previousIndex );
                powerStateGraph[addedEdge.first].count = 1;
                totalCount++;
            } else { // there was an edge already in the graph so update that edge
                powerStateGraph[addedEdge.first].delta.update( prevSpike->delta );
                powerStateGraph[addedEdge.first].duration.update( prevSpike->index - previousIndex );
                powerStateGraph[addedEdge.first].count++;
                totalCount++;
            }

            previousIndex  = prevSpike->index;
            previousVertex = mostSimVertex;
        }
        prevSpike = spike;
    }

    // add the last edge back to "off"
    addedEdge = boost::add_edge(previousVertex, offVertex, powerStateGraph);
    if (addedEdge.second) { // then there was not an edge already in the graph
        powerStateGraph[addedEdge.first].delta =
                Statistic<double>( prevSpike->delta );
        powerStateGraph[addedEdge.first].duration =
                Statistic<size_t>( prevSpike->index - previousIndex );
        powerStateGraph[addedEdge.first].count = 1;
        totalCount++;
    } else { // there was an edge already in the graph so update that edge
        powerStateGraph[addedEdge.first].delta.update( prevSpike->delta );
        powerStateGraph[addedEdge.first].duration.update( prevSpike->index - previousIndex );
        powerStateGraph[addedEdge.first].count++;
        totalCount++;
    }

}

/**
 * @brief Produce a graphviz output
 */
void PowerStateGraph::writeGraphViz(ostream& out)
{
    write_graphviz(
            out,
            powerStateGraph,
            PSG_vertex_writer(powerStateGraph),
            PSG_edge_writer(powerStateGraph)
            );
}

/**
 * @brief This is the main public interface to the disaggregation algorithm
 *
 * Each candidate solution is represented as a
 * tree.  Each vertex
 * on this tree is a spike found in the aggregate data.
 * The edge weights are the mean of the probability density
 * functions for the spike size and the timing.  If an
 * edge described in PowerStateGraph (from training) cannot be found
 * in the aggregate data then this candidate is discarded.  If
 * we successfully get from vertex0 back to vertex0 then the
 * shortest path through the tree is calculated and saved.
 *
 *

 *
Retrieve edge @c e which connects @c vertex0 (offVertex)
 *     to vertex1.  Look through the AggregateData
 *     searching for any spike within a certain number of
 *     standard deviations of @c e.delta.

 *
When a spike is found, start a tree structure and
 *     start looking for the subsequent edges learnt during training.

 *
Look for the delta corresponding to each out edge from current vertex.
 *     Store the UNIX timestamp of each candidate.

 *

 *
 * @return a list of UNIX times when the device starts
 */
const list<PowerStateGraph::Fingerprint> PowerStateGraph::disaggregate(
        const AggregateData& aggregateData, /**< A populated array of AggregateData */
        const bool keep_overlapping, /**< Should we keep or remove overlapping candidates? */
        const bool verbose
        )
{
    cout << endl << "***** TRAINING FINISHED. DISAGGREGATION STARTING. *****" << endl << endl;
    cout << "Finding start deltas...";
    cout.flush();

    if (num_vertices( powerStateGraph ) < 2) {
        Utils::fatalError( "powerStateGraph is empty. Cannot continue with disaggregation." );
    }

    // Store a pointer to aggregateData for use later.
    aggData = &aggregateData;

    /* Fingerprint is a struct for bundling start
     * timestamp, duration, energy and avLikelihood */
    list<Fingerprint> fingerprintList; // what we return
    Fingerprint candidateFingerprint;

    /* Load the stats from the first PowerStateGraph edge.
     * The Boost Graph Lib provides an out_edges() function which returns
     * a pair of edge iterators: the first iterator points to the first edge;
     * the second iterator points 1 past the last edge. Dereferencing an
     * edge iterator returns an edge descriptor. tie() allows easy
     * access to each element of the pair of edge iterators. */
    PSG_out_edge_iter out_i, out_end;
    tie(out_i, out_end) = out_edges(offVertex, powerStateGraph);
    PowerStateEdge firstEdgeStats = powerStateGraph[*out_i];

    // Search through aggregateData for possible start spikes
    list<AggregateData::FoundSpike> posStartSpikes
        = aggregateData.findSpike(firstEdgeStats.delta);

    cout << " found " << posStartSpikes.size() << " possible start deltas. Following through... " << endl;

    // For each possible start spike in the delta aggregate, attempt
    // to find all the subsequent spikes specified by powerStateGraph edges.
    for (list<AggregateData::FoundSpike>::const_iterator posStartSpike=posStartSpikes.begin();
            posStartSpike!=posStartSpikes.end();
            posStartSpike++) {

        // Attempt to recursively "follow through" from this posStartSpike.
        // initTraceToEnd() calls the recursive function traceToEnd().
        candidateFingerprint = initTraceToEnd(
                *posStartSpike,
                ( posStartSpike->timestamp - firstEdgeStats.duration.mean ) // time the device probably started
                );

        // If this start spike was successfully traced all the way to
        // an off power state then add this item to fingerprintList.
        if ( candidateFingerprint.avLikelihood != -1 ) {
            fingerprintList.push_back( candidateFingerprint );
            if (verbose)
                cout << endl << "candidate found at " << endl << candidateFingerprint << endl;
            else {
                cout << ".";
                cout.flush();
            }
        }
    }

    cout << endl;

    if ( fingerprintList.empty() ) {
        cout << "No signatures found." << endl;
    } else {
        if (!keep_overlapping) {
            removeOverlapping( &fingerprintList );
        }

        displayAndPlotFingerprintList( fingerprintList, aggregateData.getFilename() );
    }

    return fingerprintList;
}

void PowerStateGraph::displayAndPlotFingerprintList(
        const list< Fingerprint >& fingerprintList,
        const string& aggDataFilename
        ) const
{
    cout << "Displaying disaggregation data and dumping to file for plotting. " << endl;

    fstream fs;
    Utils::openFile(fs, DATA_OUTPUT_PATH + "disagg.dat", fstream::out);
    list<Fingerprint>::const_iterator disagItem;
    size_t count = 0;
    for (disagItem=fingerprintList.begin(); disagItem!=fingerprintList.end(); disagItem++) {
        cout << endl << "Candidate fingerprint found: " << endl << *disagItem << endl;
        count++;
        for (list<TimeAndPower>::const_iterator tap_i=disagItem->timeAndPower.begin();
                tap_i!=disagItem->timeAndPower.end(); tap_i++) {
            fs << tap_i->timestamp << "\t" << tap_i->meanPower << endl;
        }
    }
    cout << endl
            << "Found " << count << " candidate" << (count==1 ? ". " : "s." ) << endl;

    // Calculate size of x-axis border
    const size_t begOfFirstFingerprint = fingerprintList.front().timestamp;
    const size_t endOfLastFingerprint  = fingerprintList.back().timestamp + fingerprintList.back().duration;
    const size_t BORDER = (endOfLastFingerprint - begOfFirstFingerprint) / 10;

    // Set Plot variables
    GNUplot::PlotVars pv;
    pv.inFilename  = "disagg";
    pv.outFilename = "disagg";
    pv.title       = "Automatic disaggregation for " + deviceName;
    pv.xlabel      = "time";
    pv.ylabel      = "power (kW)";

    // set sensible xrange, but only if we're not dealing with synthetic data with a low timecode
    if (fingerprintList.front().timestamp > BORDER) {
        const size_t dstOffset = 3600; // to correct for BST
        pv.plotArgs    = "[\"" + Utils::size_t_to_s( fingerprintList.front().timestamp - BORDER + dstOffset) + "\":\""
                + Utils::size_t_to_s( fingerprintList.back().timestamp + fingerprintList.back().duration + BORDER + dstOffset) + "\"]";
    }

    pv.data.push_back(
            GNUplot::PlotData(
                    "disagg", "Automatically determined device fingerprint", "DISAGG"));
    pv.data.push_back(
            GNUplot::PlotData(
                      aggDataFilename, "Aggregate data", "AGGDATA", false));

    GNUplot::plot( pv );
}

/**
 * Remove any overlapping list entries and leave the one with the highest likelihood
 */
void PowerStateGraph::removeOverlapping(
        list<Fingerprint> * fingerprintList, /**< Input and output parameter */
        const bool verbose
        )
{
    list<Fingerprint>::iterator currentDisagItem, prevDisagItem;
    size_t count = 0;

    currentDisagItem = prevDisagItem = fingerprintList->begin();
    advance( currentDisagItem, 1 );
    while ( currentDisagItem != fingerprintList->end() ) {

        // check to see whether the two disag items overlap
        if ( (prevDisagItem->timestamp + prevDisagItem->duration) >= currentDisagItem->timestamp ) {

            count++;

            if (verbose) {
                cout << "Overlap detected between items with start timestamps "
                    << prevDisagItem->timestamp << " and " << currentDisagItem->timestamp << " ...";
            }

            // they overlap.  so find which needs to be replaced
            if (prevDisagItem->avLikelihood < currentDisagItem->avLikelihood) {
                if (verbose) cout << "erasing item with timestamp " << prevDisagItem->timestamp << endl;
                fingerprintList->erase( prevDisagItem );
                prevDisagItem = currentDisagItem++;
            } else {
                if (verbose) cout << "erasing item with timestamp " << currentDisagItem->timestamp << endl;
                fingerprintList->erase( currentDisagItem++ );
                // don't change prevDisagItem
            }

        } else {
            prevDisagItem = currentDisagItem++;
        }
    }

    if (count == 0) {
        cout << "No candidate fingerprints overlap." << endl;
    } else {
        cout << "Removed " << count << " overlapping candidate fingerprints." << endl;
    }
}

/**
 *
 * @return DisaggregatedStruct.likelihood will be set to -1 if
 * this looks like it's not a good candidate match.
 */
const PowerStateGraph::Fingerprint PowerStateGraph::initTraceToEnd(
        const AggregateData::FoundSpike& spike,
        const size_t deviceStart, /**< The possible time the device started. */
        const bool verbose
        )
{
    DisagTree disagTree;

    // make the first vertex (which represents "off")
    DisagTree::vertex_descriptor disagOffVertex = add_vertex(disagTree);
    disagTree[disagOffVertex].timestamp = deviceStart;
    disagTree[disagOffVertex].meanPower = 0; // this is "off"
    disagTree[disagOffVertex].psgVertex = offVertex;

    // add a vertex to represent the first true power state
    DisagTree::vertex_descriptor firstVertex = add_vertex(disagTree);

    // retrieve info for firstVertex and for edge between disagOffVertex and firstVertex
    PSG_out_edge_iter out_e_i, out_e_end;
    PSG_vertex_iter v_i, v_end;
    tie(out_e_i, out_e_end) = out_edges(offVertex, powerStateGraph);
    PowerStateEdge firstEdgeStats = powerStateGraph[*out_e_i];
    tie(v_i, v_end) = vertices(powerStateGraph);
    v_i++;

    disagTree[firstVertex].timestamp = spike.timestamp;
    disagTree[firstVertex].meanPower = powerStateGraph[*v_i].betweenSpikes.mean;
    disagTree[firstVertex].psgVertex = *v_i;
    disagTree[firstVertex].psgEdge   = *out_e_i;
    if (EDGE_HISTORY_SIZE)
        disagTree[firstVertex].edgeHistory.push_back(*out_e_i);

    // add an edge between disagOffVertex and firstVertex
    DisagTree::edge_descriptor edge;
    bool existingEdge;
    tie(edge, existingEdge) = add_edge(
            disagOffVertex,   // source vertex
            firstVertex,      // target vertex
            spike.likelihood, // edge value
            disagTree);

    // now recursively trace from this edge to the end
    traceToEnd( &disagTree, firstVertex, deviceStart );

    if (verbose) {
        write_graphviz(cout, disagTree,
            Disag_vertex_writer(disagTree), Disag_edge_writer(disagTree));
    }

    // find route through the tree with highest average edge likelihoods
    listOfPaths.clear();

    LikelihoodAndVertex nextLAV;
    nextLAV.vertex = firstVertex;
    nextLAV.likelihood = disagTree[edge];

    findListOfPathsThroughDisagTree(
            disagTree,
            disagOffVertex,
            nextLAV);

    // Return the most confident path through the disagTree
    return findBestPath( disagTree, deviceStart );

}

/**
 * @brief Trace the tree downwards from @c vertex recursively finding
 * every path which successfully completes (i.e. reaches an off state).
 */
void PowerStateGraph::findListOfPathsThroughDisagTree(
        const DisagTree& disagTree,
        const DisagTree::vertex_descriptor vertex,
        const LikelihoodAndVertex lav,
        list<PowerStateGraph::LikelihoodAndVertex> path /**< Deliberately called-by-value because we want a copy. */
    )
{
    path.push_back( lav );

    // base case = we're at the end
    if ( vertex != 0 && // check we're not at the first vertex
            disagTree[ vertex ].meanPower == 0 ) {
        listOfPaths.push_back( path );
        return;
    }

    // iterate through each out-edge
    Disag_out_edge_iter out_e_i, out_e_end;
    tie(out_e_i, out_e_end) = out_edges(vertex, disagTree);

    for (; out_e_i!=out_e_end; out_e_i++) {

        DisagTree::vertex_descriptor downstreamVertex = target(*out_e_i, disagTree);

        LikelihoodAndVertex nextLav;
        nextLav.vertex = downstreamVertex;
        nextLav.likelihood = disagTree[*out_e_i];

        // we haven't hit the end yet so recursively follow tree downwards.
        findListOfPathsThroughDisagTree(
                disagTree,
                downstreamVertex,
                nextLav,
                path );

    }
    return;
}

/**
 * Trace from startVertex to the off state in PSGraph
 */
void PowerStateGraph::traceToEnd(
        DisagTree * disagTree_p, /**< input and output parameter */
        const DisagTree::vertex_descriptor& disagVertex,
        const size_t prevTimestamp, /**< timestamp of previous vertex */
        const bool verbose
        ) const
{

    if (verbose) cout << "***traceToEnd... prevTimestamp=" << prevTimestamp << " DisagTree startVertex=" << disagVertex << endl;

    list<AggregateData::FoundSpike> foundSpikes;

    // A handy reference to make the code more readable
    DisagTree& disagTree = *disagTree_p;

    // base case
    if ( disagTree[disagVertex].psgVertex == offVertex ) {
        return;
    }

    // For each out-edge from disagVertex.psgVertex, retrieve a list of
    // spikes which match and create a new DisagTree vertex for each match.
    PSG_out_edge_iter psg_out_i, psg_out_end;
    tie(psg_out_i, psg_out_end) =
            out_edges(disagTree[disagVertex].psgVertex, powerStateGraph);

    const size_t WINDOW_FRAME = 8; // number of seconds to widen window by

    for (; psg_out_i!=psg_out_end; psg_out_i++ ) {

        // Commented out alternative strategy for getting edge history. Almost certainly slower than
        // using disagTree[disagVertex].edgeHistory
//        std::list< PSGraph::edge_descriptor > eHistory = getEdgeHistoryForVertex(disagTree, disagVertex);

        if ( ! edgeListsAreEqual(
//                eHistory,
                disagTree[disagVertex].edgeHistory,
                powerStateGraph[*psg_out_i].edgeHistory ) ) {
            if (verbose) cout << "edge histories not equal" << endl;
            continue;
        }

        size_t begOfSearchWindow, endOfSearchWindow;

        size_t e = powerStateGraph[*psg_out_i].duration.nonZeroStdev();

        if (verbose)  cout << "disagTree[startVertex].timestamp=" << disagTree[disagVertex].timestamp << endl;

        begOfSearchWindow = (disagTree[disagVertex].timestamp + powerStateGraph[*psg_out_i].duration.min)
                - WINDOW_FRAME - e;

        endOfSearchWindow = (disagTree[disagVertex].timestamp + powerStateGraph[*psg_out_i].duration.max)
                + WINDOW_FRAME + e;

        //********************** DIAGNOSTICS *************************
        if (verbose) {
            cout << "endOfSearchWindow=" << endOfSearchWindow
                    << " disagTree[startVertex].timestamp=" << disagTree[disagVertex].timestamp
                    << " powerStateGraph[*psg_out_i].duration.max=" << powerStateGraph[*psg_out_i].duration.max
                    << " powerStateGraph[*psg_out_i].duration.mean=" << powerStateGraph[*psg_out_i].duration.mean
                    << " powerStateGraph[*psg_out_i].duration.stdev=" << powerStateGraph[*psg_out_i].duration.stdev
                    << endl;
        }//__________________________________________________________

        // ensure we're not looking backwards in time
        if (begOfSearchWindow <= disagTree[disagVertex].timestamp) {
            begOfSearchWindow =
                    disagTree[disagVertex].timestamp
                    + powerStateGraph[*psg_out_i].duration.min
                    - (powerStateGraph[*psg_out_i].duration.min / 10);
        }

        // check that we're not looking past the end of aggData
        if (endOfSearchWindow > (*aggData)[ (*aggData).getSize() - 1 ].timestamp ) {
            continue; // we can't process this if we're trying to look past the end of the aggData
        }

        //************************************************************//
        // get a list of candidate spikes matching this PSG-out-edge  //

        foundSpikes.clear();
        foundSpikes = aggData->findSpike(
                powerStateGraph[*psg_out_i].delta,  // spike stats
                begOfSearchWindow,
                endOfSearchWindow
                );

        //***************************************************************//
        // for each candidate spike, create a new vertex in disagTree   //
        // and recursively trace this to the end                         //

        for (list<AggregateData::FoundSpike>::const_iterator spike=foundSpikes.begin();
                spike!=foundSpikes.end();
                spike++) {

            // ensure that the absolute value of the aggregate data signal
            // does not drop below the minimum for our current power state
            if (aggData->readingGoesBelowPowerState(
                    disagTree[disagVertex].timestamp,
                    spike->timestamp,
                    powerStateGraph[ disagTree[disagVertex].psgVertex ].betweenSpikes ) ) {

                continue;
            }

            double normalisedLikelihoodForTime = powerStateGraph[*psg_out_i].duration.normalisedLikelihood(
                    spike->timestamp - disagTree[disagVertex].timestamp);

            // merge probability for time and for spike delta
            double avLikelihood = ( normalisedLikelihoodForTime + spike->likelihood ) / 2;

            // create new vertex
            DisagTree::vertex_descriptor newVertex=add_vertex( disagTree );

            // create new edge
            DisagTree::edge_descriptor newEdge;
            bool existingEdge;
            tie(newEdge, existingEdge) =
                    add_edge( disagVertex, newVertex, avLikelihood, disagTree );

            // add details to newVertex
            disagTree[newVertex].timestamp = spike->timestamp;
            // get vertex that *psg_out_i points to
            disagTree[newVertex].psgVertex = target(*psg_out_i, powerStateGraph);
            disagTree[newVertex].psgEdge   = *psg_out_i;
            disagTree[newVertex].meanPower =
                    powerStateGraph[disagTree[newVertex].psgVertex].betweenSpikes.mean;

            if (EDGE_HISTORY_SIZE) {
                disagTree[newVertex].edgeHistory = disagTree[disagVertex].edgeHistory;
                disagTree[newVertex].edgeHistory.push_back(*psg_out_i);
                if (disagTree[newVertex].edgeHistory.size() > EDGE_HISTORY_SIZE) {
                    disagTree[newVertex].edgeHistory.erase( disagTree[newVertex].edgeHistory.begin() );
                }
            }

            // recursively trace to end.
            traceToEnd(disagTree_p, newVertex, disagTree[disagVertex].timestamp);
        }
    }
}

/**
 * @brief Iterates through each path in @c listOfPaths
 *        to find the one with the highest likelihood.
 *
 * To be called after @c listOfPaths has been populated by findListOfPathsThroughDisagTree().
 *
 * @return details of the best path.  Confidence is set to -1 if no paths are available.
 */
const PowerStateGraph::Fingerprint PowerStateGraph::findBestPath(
        const DisagTree& disagTree,
        const size_t deviceStart,
        const bool verbose
        )
{
    Fingerprint fingerprint;
    fingerprint.timestamp = deviceStart;

    // first check that listOfPaths is populated
    if ( listOfPaths.empty() ) {
        fingerprint.avLikelihood = -1; // return error code
        return fingerprint;
    }

    fingerprint.avLikelihood = 0;

    list< list<LikelihoodAndVertex> >::const_iterator path_i, bestPath_i;
    list< LikelihoodAndVertex >::const_iterator lav_i;

    if (verbose) cout << "path dump:" << endl;

    double likelihoodAccumulator, avLikelihood;
    bool foundGoodPath = false;

    // iterate through each path to find the one with the highest likelihood
    for (path_i=listOfPaths.begin(); path_i!=listOfPaths.end(); path_i++) {

        likelihoodAccumulator = 0;
        for ( lav_i=path_i->begin(); lav_i!=path_i->end(); lav_i++ ) {
            if (verbose) cout << "vertex=" << lav_i->vertex << " conf=" << lav_i->likelihood << ", ";
            likelihoodAccumulator += lav_i->likelihood;
        }

        avLikelihood = likelihoodAccumulator / path_i->size();

        // if this is the most confident path we've seen yet then record its details.
        if ( avLikelihood >= fingerprint.avLikelihood ) {
            fingerprint.avLikelihood = avLikelihood;
            bestPath_i = path_i;
            foundGoodPath = true;
        }

        if (verbose) cout << endl << endl;
    }

    // now get energy usage and duration from bestPath_i
    size_t prevTimestamp, duration;
    double prevMeanPower = 0;

    fingerprint.energy = 0;
    fingerprint.duration = 0;
    duration = 0;
    prevTimestamp = deviceStart;
    size_t count = 0;
    if (foundGoodPath) {
        for (lav_i=bestPath_i->begin(); lav_i != bestPath_i->end(); lav_i++) {

            // Add to timeAndPower list for read-out later when we plot the power states
            if ( lav_i != bestPath_i->begin() ) {
                fingerprint.timeAndPower.push_back( TimeAndPower(
                        disagTree[ lav_i->vertex ].timestamp-1 ,
                        (count==1 ? 0 : prevMeanPower)
                ) );
                fingerprint.timeAndPower.push_back( TimeAndPower(
                        disagTree[ lav_i->vertex ].timestamp,
                        disagTree[ lav_i->vertex ].meanPower) );
            }

            // Now calculate duration and energy consumption
            duration = disagTree[ lav_i->vertex ].timestamp - prevTimestamp;
            fingerprint.energy += prevMeanPower * duration;

            fingerprint.duration += duration;

            prevTimestamp = disagTree[ lav_i->vertex ].timestamp;
            prevMeanPower = disagTree[ lav_i->vertex ].meanPower;
            count++;
        }

        fingerprint.avLikelihood = ( fingerprint.avLikelihood +
                energyConsumption.normalisedLikelihood(fingerprint.energy) ) / 2;
    } else {
        fingerprint.avLikelihood = -1;
    }

    return fingerprint;
}

/**
 * @brief Trace the disagTree backwards.
 */
list< PowerStateGraph::PSGraph::edge_descriptor > PowerStateGraph::getEdgeHistoryForVertex(
        const DisagTree& disagTree,
        const DisagTree::vertex_descriptor& startVertex
        ) const
{
    list< PSGraph::edge_descriptor > eHistory;
    DisagTree::vertex_descriptor disagVertex = startVertex;

    DisagTree::in_edge_iterator in_e_i, in_e_end;

    while (disagTree[disagVertex].psgVertex != offVertex && eHistory.size() < EDGE_HISTORY_SIZE) {
        eHistory.push_front( disagTree[disagVertex].psgEdge );
        tie(in_e_i, in_e_end) = in_edges(disagVertex, disagTree);
        disagVertex = source( *in_e_i, disagTree ); // the vertex upstream from in_e_i
    }

    return eHistory;
}

void PowerStateGraph::setDeviceName(const string& _deviceName)
{
    deviceName = _deviceName;
}

std::ostream& operator<<( std::ostream& o, const PowerStateGraph& psg )
{
    PowerStateGraph::PSG_vertex_index_map index = boost::get(boost::vertex_index, psg.powerStateGraph);

    o << "vertices(graph) = " << std::endl;
    std::pair<PowerStateGraph::PSG_vertex_iter, PowerStateGraph::PSG_vertex_iter> vp;
    for (vp = boost::vertices(psg.powerStateGraph); vp.first != vp.second; ++vp.first) {
        o << "vertex" << index[*vp.first] << " = {" << psg.powerStateGraph[*vp.first].postSpike <<  "}";
        if ( *vp.first == psg.offVertex ) {
            o << "\n          (offVertex)" << std::endl;
        }
        o << std::endl;
    }

    return o;
}


/******************************************************************************
 * Copyright (C) 2020-2021 by
 * The Salk Institute for Biological Studies
 *
 * Use of this source code is governed by an MIT-style
 * license that can be found in the LICENSE file or at
 * https://opensource.org/licenses/MIT.
******************************************************************************/

#include <iostream>
#include <sstream>
#include <map>

#include "bng/ast.h"
#include "bng/bng_engine.h"
#include "bng/elem_mol_type.h"
#include "bng/elem_mol.h"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/vf2_sub_graph_iso.hpp>
#include <boost/graph/connected_components.hpp>

#define NAUTY_CPU_DEFINED // silence a warning
#include "nauty/traces.h"
#include "nauty/nausparse.h"

#include "debug_config.h"

#include "bng/cplx.h"
#include "bng/bngl_names.h"
#include "bng/semantic_analyzer.h" // only for insert_compartment_id_to_set_based_on_type

using namespace boost;
using namespace std;

namespace BNG {

// ------------------------------------ CplxInstance -------------------------

bool Cplx::is_fully_qualified() const {
  for (const ElemMol& mi: elem_mols) {
    if (!mi.is_fully_qualified(*bng_data)) {
      return false;
    }
  }
  return true;
}

bool Cplx::is_connected() const {
  assert(is_finalized());

  // simply count connected components
  size_t num_vertices = boost::num_vertices(graph);
  vector<int> component_per_vertex(num_vertices);
  int num_components = boost::connected_components(
      graph,
      boost::make_iterator_property_map(
          component_per_vertex.begin(), boost::get(boost::vertex_index, graph), component_per_vertex[0]
      )
  );
  assert(num_components > 0);
  return num_components == 1;
}

void Cplx::finalize_cplx(const bool init_flags_and_compartments) {
  if (elem_mols.empty()) {
    return; // empty complex, ignoring finalization
  }

  if (init_flags_and_compartments) {
    // volume or surface type
    bool surf_type = false;
    bool reactive_surf_type = false;
    for (ElemMol& mp: elem_mols) {
      // need to finalize flags - copy them from molecule type
      mp.finalize_flags_and_sort_components(*bng_data);
      // if at least one is a surface molecule then the whole cplx is surface molecule
      if (mp.is_surf()) {
        surf_type = true;
      }

      // if at least one is a reactive surface then the whole cplx is reactive surface
      if (mp.is_reactive_surface()) {
        reactive_surf_type = true;
      }
    }
    if (surf_type) {
      release_assert(!reactive_surf_type && "Species cannot be both reactive surface and surface molecule.");
      set_flag(SPECIES_CPLX_MOL_FLAG_SURF);
    }
    else if (reactive_surf_type) {
      set_flag(SPECIES_CPLX_MOL_FLAG_REACTIVE_SURFACE);
    }

    // set flag SPECIES_CPLX_FLAG_ONE_MOL_NO_COMPONENTS
    bool is_simple = true;
    if (elem_mols.size() > 1) {
      is_simple = false;
    }
    if (is_simple) {
      for (ElemMol& mi: elem_mols) {
        if (!mi.components.empty()) {
          is_simple = false;
          break;
        }
      }
    }
    set_flag(SPECIES_CPLX_FLAG_ONE_MOL_NO_COMPONENTS, is_simple);
  }

  // we need graphs even for simple complexes because they can be used in reaction patterns
  graph.clear();
  create_graph();

  set_finalized();
}

void Cplx::create_graph() {
  graph.clear();

  // convert molecule instances and their bonds into the boost graph representation

  map<bond_value_t, small_vector<Graph::vertex_descriptor>> bonds_to_vertices_map;

  // reserve some space for vertices, edges are stored using list
  graph.m_vertices.reserve(elem_mols.size() * 8);

  // add all molecules with their components and remember how they should be bound
  for (ElemMol& mi: elem_mols) {
    Graph::vertex_descriptor mol_desc = boost::add_vertex(MtVertexProperty(Node(&mi)), graph);

    for (Component& ci: mi.components) {
      // for patterns, only components that were explicitly listed are in component instances
      Graph::vertex_descriptor comp_desc = boost::add_vertex(MtVertexProperty(Node(&ci)), graph);

      // connect the component to its molecule
      // TODO OPTIMIZATION:
      //  - boost::add_edge is pretty slow (25% of time for the SynGAP model)
      //  - it almost seems that we would need a different graph representation to optimize it,
      //    the internal adjacency_list's EdgeList must be a list and it cannot be pre-allocated
      boost::add_edge(mol_desc, comp_desc, graph);

      // and remember its bond
      if (ci.bond_has_numeric_value()) {
        bonds_to_vertices_map[ci.bond_value].push_back(comp_desc);
      }
    }
  }

  // connect components
  for (auto it: bonds_to_vertices_map) {
    release_assert(it.second.size() == 2 && "There must be exactly pairs of components connected with numbered bonds.");
    boost::add_edge(it.second[0], it.second[1], graph);
  }
}

void Cplx::get_used_compartments(uint_set<compartment_id_t>& compartments) const {
  compartments.clear();

  assert(elem_mols.size() > 0);
  for (const ElemMol& em: elem_mols) {
    compartments.insert(em.compartment_id);
  }
}

compartment_id_t Cplx::get_complex_compartment_id(const bool dont_know_elem_mol_types) const {
#ifndef NDEBUG
  // consistency check of compartments
  // similar as in SemanticAnalyzer::convert_cplx
  bool ignored;
  bool all_are_none_or_inout = true;
  uint_set<compartment_id_t> vol_compartments;
  uint_set<compartment_id_t> surf_compartments;

  assert(elem_mols.size() > 0);
  for (const ElemMol& em: elem_mols) {
    insert_compartment_id_to_set_based_on_type(
        bng_data, em.compartment_id,
        all_are_none_or_inout, ignored, vol_compartments, surf_compartments);
  }

  // we might not know whether our compartment is surf or vol, so the check is weaker
  assert(
      all_are_none_or_inout ||
      (vol_compartments.size() == 1 && surf_compartments.empty()) ||
      (vol_compartments.size() <= 2 && surf_compartments.size() == 1));
#endif

  if (dont_know_elem_mol_types) {
    // get the first surface compartment because we do not know the types of elementary molecules
    // if surface compartment is not used, get the first volume compartment
    compartment_id_t vol_cid = COMPARTMENT_ID_NONE;
    for (const ElemMol& em: elem_mols) {
      compartment_id_t cid = em.compartment_id;
      if (is_specific_compartment_id(cid)) {
        if (!bng_data->get_compartment(cid).is_3d) {
          return cid;
        }
        else {
          if (vol_cid == COMPARTMENT_ID_NONE) {
            vol_cid = cid;
          }
        }
      }
    }
    // no specific surface compartment was set, using the volume compartment if found
    return vol_cid;
  }
  else if (is_surf()) {
    // get the first set surface elem mol
    for (const ElemMol& em: elem_mols) {
      if (em.is_surf() && em.compartment_id != COMPARTMENT_ID_NONE) {
        return em.compartment_id;
      }
    }
    // no compartment is set
    return COMPARTMENT_ID_NONE;
  }
  else {
    // all are volume and must have the same compartment but in patterns,
    // not all need to be set
    // get the first set surface elem mol
    for (const ElemMol& em: elem_mols) {
      if (em.compartment_id != COMPARTMENT_ID_NONE) {
        return em.compartment_id;
      }
    }
    // no compartment is set
    return COMPARTMENT_ID_NONE;
  }
}

bool Cplx::matches_complex_pattern_ignore_orientation(const Cplx& pattern) const {

#ifdef DEBUG_CPLX_MATCHING
  cout << "** matches_complex_pattern_ignore_orientation:\n";
  cout << "pattern:\n";
  pattern.dump(false); cout << "\n";
  pattern.dump(true); cout << "\n";
  dump_graph(pattern.graph, bng_data);
  cout << "this instance:\n";
  dump(false); cout << "\n";
  dump(true); cout << "\n";
  dump_graph(graph, bng_data);
#endif
  // this result cannot be cached because it might not be and applicable for other equivalent complexes,
  // we might need to impose some ordering on elementary molecules and then we can reuse the result when
  // creating products
  VertexMappingVector mappings;
  get_subgraph_isomorphism_mappings(pattern.graph, graph, true, mappings);
  assert((mappings.size() == 0 || mappings.size() == 1) && "We are searching only for the first match");

#ifdef DEBUG_CPLX_MATCHING
  cout << "** result: " << !mappings.empty() << "\n";
#endif
  // we need at least one match
  return !mappings.empty();
}

// sets compartment to all contained elementary molecules
void Cplx::set_compartment_id(const compartment_id_t cid, const bool override_only_compartment_none) {
  // TODO: here could be some extra checks related to orientation
  // set compartment to all elementary molecules
  assert(!elem_mols.empty());
  for (ElemMol& em: elem_mols) {
    // we cannot check here whether compartment type (2d/3d matches surf/vol) because
    // we do not necessarily know the the of the elementary molecules at all times
    if (!override_only_compartment_none || em.compartment_id == COMPARTMENT_ID_NONE) {
      em.compartment_id = cid;
    }
  }
}

uint Cplx::get_pattern_num_matches(const Cplx& pattern) const {
  assert(is_finalized() && pattern.is_finalized());
  VertexMappingVector mappings;
  get_subgraph_isomorphism_mappings(pattern.graph, graph, false, mappings);
  return mappings.size();
}

bool Cplx::matches_complex_fully_ignore_orientation(const Cplx& other) const {
  if (graph.m_vertices.size() != other.graph.m_vertices.size()) {
    // we need full match
    return false;
  }

  VertexMappingVector mappings;
  get_subgraph_isomorphism_mappings(other.graph, graph, true, mappings);
  assert((mappings.size() == 0 || mappings.size()) == 1 && "We are searching only for the first match");

  if (mappings.size() != 1 || mappings[0].size() != graph.m_vertices.size()) {
    // no mapping found or not all nodes match
    return false;
  }

  // we must also check that compartments are the same,
  // this must be done separately because the Graph object Node allows only
  // one type of comparison and the one used considers one graph a pattern,
  // however we must compare for equality here
  VertexNameMap graph1_index = boost::get(boost::vertex_name, graph);
  VertexNameMap graph2_index = boost::get(boost::vertex_name, other.graph);

  // for each molecule instance
  typedef boost::graph_traits<Graph>::vertex_iterator vertex_iter;
  std::pair<vertex_iter, vertex_iter> graph1_mol_it;
  for (graph1_mol_it = boost::vertices(graph); graph1_mol_it.first != graph1_mol_it.second; ++graph1_mol_it.first) {
    vertex_descriptor_t graph1_mol_desc = *graph1_mol_it.first;

    const Node& graph1_mol = graph1_index[graph1_mol_desc];
    if (!graph1_mol.is_mol) {
      continue;
    }

    // get corresponding molecule from the 2nd graph
    auto graph2_mol_it = mappings[0].find(graph1_mol_desc);
    assert(graph2_mol_it != mappings[0].end() && "Mapping must exist");
    vertex_descriptor_t graph2_mol_desc = graph2_mol_it->second;

    const Node& graph2_mol = graph2_index[graph2_mol_desc];
    assert(graph2_mol.is_mol);

    if (graph1_mol.mol->compartment_id != graph2_mol.mol->compartment_id) {
      return false;
    }
  }
  return true;
}

void Cplx::renumber_bonds() {
  map<bond_value_t, bond_value_t> new_bond_values;

  for (ElemMol& mi: elem_mols) {
    for (Component& ci: mi.components) {
      if (ci.bond_has_numeric_value()) {
        auto it = new_bond_values.find(ci.bond_value);
        if (it == new_bond_values.end()) {
          // new bond value, numbering from 1
          bond_value_t new_value = new_bond_values.size() + 1;
          new_bond_values[ci.bond_value] = new_value;
          ci.bond_value = new_value;
        }
        else {
          // we have already seen this bond value, use the new value
          ci.bond_value = it->second;
        }
      }
    }
  }
}

// https://computationalcombinatorics.wordpress.com/2012/09/20/canonical-labelings-with-nauty/
void Cplx::canonicalize(const bool sort_components_by_name_do_not_finalize) {
  if (elem_mols.size() == 1) {
    canonicalize_w_single_elem_mol(sort_components_by_name_do_not_finalize);
  }
  else {
    canonicalize_complex(sort_components_by_name_do_not_finalize);
  }

  // update the boost graph representation
  if (!sort_components_by_name_do_not_finalize) {
    finalize_cplx();
  }

  set_flag(SPECIES_CPLX_FLAG_IS_CANONICAL);
  name = "";
  to_str(name);
}

void Cplx::canonicalize_w_single_elem_mol(const bool sort_components_by_name_do_not_finalize) {
  assert(elem_mols.size() == 1);

  if (!sort_components_by_name_do_not_finalize) {
    // sort components in molecules to their prescribed form
    // and in a way that the state name is ascending (we have no bonds here)
    elem_mols[0].canonicalize(*bng_data);
  }
  else {
    elem_mols[0].sort_components_by_name(*bng_data);
  }
}

void Cplx::canonicalize_complex(const bool sort_components_by_name_do_not_finalize) {

  // we use nauty/traces to construct a canonical version of the graph
  // we are using only the base BNG API, not the boost graphs to stay independent

  // 1) construct mapping vertex - index -> molecule or component
  //    and also store information on bonds and other details
  vector<Node> nodes;
  // map bond index -> vertex indices
  map<bond_value_t, vector<int>> bond_index_to_components;
  // mapping to know to which molecule a component belongs
  map<int, int> component_to_mol_index;
  // indexed by index, pair is (molecule index, component index) in this cplx
  // second value is -1 if the index represents a molecule
  vector<pair<int, int>> index_to_mol_and_component_index;
  int index = 0;
  for (int m_index = 0; m_index < (int)elem_mols.size(); m_index++) {
    ElemMol& mi = elem_mols[m_index];
    nodes.push_back(Node(&mi));
    int mol_index = index;
    index_to_mol_and_component_index.push_back(make_pair(m_index, -1));
    index++;

    for (int c_index = 0; c_index < (int)mi.components.size(); c_index++) {
      Component& ci = mi.components[c_index];
      nodes.push_back(Node(&ci));
      component_to_mol_index[index] = mol_index;
      index_to_mol_and_component_index.push_back(make_pair(m_index, c_index));
      if (ci.bond_has_numeric_value()) {
        bond_index_to_components[ci.bond_value].push_back(index);
      }
      index++;
    }
  }

  // 2) create nauty graph representation
  vector<size_t> v_edge_indices;
  vector<int> d_out_degrees;
  vector<int> e_neighbors;
  map<string, vector<int>> color_classes;

  for (int index = 0; index < (int)nodes.size(); index++) {
    const Node& n = nodes[index];

    // the index for this node for neighbors simply starts where the last ended
    v_edge_indices.push_back(e_neighbors.size());

    // neighbors
    int num_neighbors = 0;
    if (n.is_mol) {
      // for a molecule - neighbors are all edges - the indices directly follow ours
      for (int i = 0; i < (int)n.mol->components.size(); i++) {
        e_neighbors.push_back(index + i + 1); // need +1 because the first component is the next one
        num_neighbors++;
      }

      // also remember color
      // use name instead of id so that we are not dependent on the order of declatation in the BNG file
      color_classes["M:" + bng_data->get_elem_mol_type(n.mol->elem_mol_type_id).name].push_back(index);
    }
    else {
      // index of the component's molecule
      assert(component_to_mol_index.count(index) != 0);
      e_neighbors.push_back(component_to_mol_index[index]);
      num_neighbors++;

      // index of the second component, if connected
      if (n.component->bond_has_numeric_value()) {
        assert(bond_index_to_components.count(n.component->bond_value) != 0);
        vector<int>& bonds = bond_index_to_components[n.component->bond_value];
        assert(bonds.size() == 2);
        int second_index = -1;
        if (bonds[0] == index) {
          second_index = bonds[1];
        }
        else if (bonds[1] == index) {
          second_index = bonds[0];
        }
        else {
          assert(false);
        }
        e_neighbors.push_back(second_index);
        num_neighbors++;
      }

      // also remember 'color', we need to distinguish states as well
      string component_label = "C:" + bng_data->get_component_type(n.component->component_type_id).name;
      if (n.component->state_is_set()) {
        component_label += "~" + bng_data->get_state_name(n.component->state_id);
      }
      color_classes[component_label].push_back(index);
    }

    d_out_degrees.push_back(num_neighbors);
  }

  // define coloring, nauty has a weird way of assigning colors to nodes:
  // libs/nauty/nug27.pdf, p. 18:
  // if ptn[i] = 0, then a cell (colour class) ends at position i.
  // so let's say I have these data:
  //   lab: 2 3 5 6 1 0 4 7 8 all vertices in some order
  //   ptn: 0 0 1 1 1 0 1 1 0 cells end where the zeros are (non-zero value specifies continuation)
  // it defines these classes
  //   [{2}, {3}, {0, 1, 5, 6}, {4, 7, 8}].

  vector<int> labels; // vertex indices
  vector<int> permutations; // ptn in nauty

#ifdef DEBUG_CANONICALIZATION
  cout << "Before " << to_str(*bng_data) << "\n";
#endif

  // labels.push_back(index); // index of vertices in the colors array
  for (auto it_color: color_classes) {
    vector<int>& indices = it_color.second;
    for (size_t i = 0; i < indices.size(); i++) {
      labels.push_back(indices[i]);
      // 1 - there are more, 0 - last of this class
      if (i != indices.size() - 1) {
        permutations.push_back(1);
      }
      else {
        permutations.push_back(0);
      }
    }
  }

  // setup sparse graph representation
  SG_DECL(sg1);
  int num_verts = v_edge_indices.size();
  sg1.nde = e_neighbors.size();
  sg1.nv = num_verts;
  sg1.d = d_out_degrees.data();
  sg1.dlen = d_out_degrees.size();
  sg1.v = v_edge_indices.data();
  sg1.vlen = num_verts;
  sg1.e = e_neighbors.data();
  sg1.elen = e_neighbors.size();

  // 3) get canonical mapping
  SG_DECL(cg1);
  //DEFAULTOPTIONS_SPARSEGRAPH(options);
  //statsblk stats;
  DEFAULTOPTIONS_TRACES(options);
  options.getcanon = TRUE;
  options.defaultptn = FALSE;
  options.digraph = FALSE;
  TracesStats stats;

  int* orbits = new int[num_verts]; // unused but must be allocated

  // - do the actual canonicalization, labels define how to reorder molecules and components
  //   using function Traces instead of sparsenauty or nauty because it does not leave so much
  //   unfreed memory
  // - WARNING: Threads function may not be thread safe, nauty uses many globals
  // - overwrites contents of labels and permutations
#ifdef DEBUG_CANONICALIZATION
  dump_container(labels, "labels before");
  dump_container(permutations, "permutations before");
#endif

  Traces(&sg1, labels.data(), permutations.data(), orbits, &options, &stats, &cg1);

#ifdef DEBUG_CANONICALIZATION
  dump_container(labels, "labels after");
  dump_container(permutations, "permutations after");
#endif

  SG_FREE( cg1 );
  delete [] orbits;
  nausparse_freedyn(); // frees allocated thread local storage memory

  // 4) create molecules and components in this complex from scratch
  // the numeric bonds are still ok

  ElemMolVector new_mol_instances(elem_mols.size());
  map<int, int> old_to_new_mol_index;
  // now go by the ordered reverse mapping and create mols
  int new_mol_index = 0;
  for (int index: labels) {
    // is this node a molecule?
    pair<int, int> orig_mci = index_to_mol_and_component_index[index];
    if (orig_mci.second == -1) {
      // copy everything and clear components, they will be added later
      new_mol_instances[new_mol_index] = elem_mols[orig_mci.first];
      new_mol_instances[new_mol_index].components.clear();
      old_to_new_mol_index[orig_mci.first] = new_mol_index;
      new_mol_index++;
    }
  }

  // once we have mols, add also the components
  for (int index: labels) {
    // is this node a component?
    pair<int, int> orig_mci = index_to_mol_and_component_index[index];
    if (orig_mci.second != -1) {
      int new_mol_index = old_to_new_mol_index[orig_mci.first];
      assert(new_mol_index < (int)new_mol_instances.size());
      // copy components
      new_mol_instances[new_mol_index].components.push_back(
          elem_mols[orig_mci.first].components[orig_mci.second]
      );
    }
  }

  // and overwrite
  elem_mols = new_mol_instances;

  // 5) renumber bonds so that they follow the new molecule ordering
  renumber_bonds();

  // 6) sort components in molecules back to their prescribed form
  // and in a way that the bond index is increasing
  for (ElemMol& mi: elem_mols) {
    if (!sort_components_by_name_do_not_finalize) {
      // sort components in molecules to their prescribed form
      // and in a way that the state name is ascending (we have no bonds here)
      mi.canonicalize(*bng_data);
    }
    else {
      mi.sort_components_by_name(*bng_data);
    }
  }

  // 7) and renumber bonds again
  renumber_bonds();

#ifdef DEBUG_CANONICALIZATION
  cout << "After " << to_str(*bng_data) << "\n";
#endif
}

void Cplx::remove_compartment_from_elem_mols(BNG::compartment_id_t cid) {
  if (cid == BNG::COMPARTMENT_ID_NONE) {
    return;
  }
  release_assert(!BNG::is_in_out_compartment_id(cid));
  for (auto& em: elem_mols) {
    if (em.compartment_id == cid) {
      em.compartment_id = BNG::COMPARTMENT_ID_NONE;
    }
  }
}

std::string Cplx::to_str(bool in_surf_reaction, const bool with_orientation) const {
  std::string res;
  to_str(res, in_surf_reaction, with_orientation);
  return res;
}

void Cplx::to_str(std::string& res, const bool in_surf_reaction, const bool with_orientation) const {

  uint_set<compartment_id_t> used_compartments;
  get_used_compartments(used_compartments);
  bool use_individual_compartments = !(used_compartments.size() == 1) || elem_mols.size() == 1;

  for (size_t i = 0; i < elem_mols.size(); i++) {
    elem_mols[i].to_str(*bng_data, res, use_individual_compartments);

    if (i != elem_mols.size() - 1) {
      res += ".";
    }
  }

  if (used_compartments.size() == 1 && *used_compartments.begin() == COMPARTMENT_ID_NONE) {
    if (with_orientation) {
      if (orientation == ORIENTATION_UP) {
        res += "'";
      }
      else if (orientation == ORIENTATION_DOWN) {
        res += ",";
      }
      else if (in_surf_reaction && orientation == ORIENTATION_NONE) {
        res += ";";
      }
    }
  }
  else if (!use_individual_compartments) {
    compartment_id_t single_compartment_id = *used_compartments.begin();
    // single compartment is used as prefix when all compartments are the same
    if (is_in_out_compartment_id(single_compartment_id)) {
      res = "@" + compartment_id_to_str(single_compartment_id) + ":" + res;
    }
    else {
      const string& compartment_name = bng_data->get_compartment(single_compartment_id).name;
      if (compartment_name != DEFAULT_COMPARTMENT_NAME) {
        res = "@" + bng_data->get_compartment(single_compartment_id).name  + ":" + res;
      }
    }
  }
}

void Cplx::dump(const bool for_diff, const std::string ind) const {
  if (!for_diff) {
    cout << ind << to_str();
  }
  else {
    cout << ind << "orientation: " << orientation << "\n";
    cout << ind << "mol_instances:\n";
    for (size_t i = 0; i < elem_mols.size(); i++) {
      cout << ind << i << ":\n";
      elem_mols[i].dump(*bng_data, true, ind + "  ");
    }
  }
}

} /* namespace BNG */


#include <functional>
#include <mutex>
#include <thread>
#include <string>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <map_msgs/OccupancyGridUpdate.h>
#include <nav_msgs/OccupancyGrid.h>
#include <queue>
#include <unordered_map>
// #include <boost>
#include <time.h>
#include <random>

#define INF INT_MAX
using namespace std;

namespace frontier_detection
{
    class frontierdetection
    {
        public:
            frontierdetection(ros::NodeHandle private_nh)
            {
                getCostmap = false;
                NODE = private_nh;
                costmap_sub_global_ = private_nh.subscribe<nav_msgs::OccupancyGrid>("/map", 1, &frontierdetection::map_sub, this);
                costmap_pub_global_=private_nh.advertise<nav_msgs::OccupancyGrid>("/sampling_partles_map", 1);
                costmap_sub_global_ = private_nh.subscribe<nav_msgs::OccupancyGrid>("/sampling_partles_map", 1, &frontierdetection::CostmapSubCallback_global, this);
                // frontier_pub_=private_nh.advertise<geometry_msgs::PoseStamped>("/frontier/goal", 1);
                // robot_position_pub_=private_nh.advertise<geometry_msgs::PoseStamped>("/robot_position", 1);
            }

        private:
            void map_sub(const nav_msgs::OccupancyGrid::ConstPtr& map)
            {
                cout<<"get map"<<endl;
                nav_msgs::OccupancyGrid map_temp=*map;
                int x_step=10;
                int y_step=10;
                for(int i=0;i<map_temp.info.height;i++)
                {
                    if(i%y_step==0)
                    {
                    for(int j=0;j<map_temp.info.width;j++)
                        {
                            if(j%x_step==0&&map_temp.data.at(i*map_temp.info.width+j)==0)
                                map_temp.data.at(i*map_temp.info.width+j)=80;
                                particle_indexes.push_back(i*map_temp.info.width+j);
                        }
                    }
                }
                while(1)
                {
                    sleep(1);
                    costmap_pub_global_.publish(map_temp);
                }
            }

            void CostmapSubCallback_global(const nav_msgs::OccupancyGrid::ConstPtr& map)
            {
                nav_msgs::OccupancyGrid map_temp=*map;
                partle_move(map_temp,particle_indexes);
            }
            void partle_move(const nav_msgs::OccupancyGrid& map, std::vector<int> indexes)
            {
                for(auto index:indexes)
                {
                    int mx,my;
                    indexTomap(map,index,mx,my);

                }
            }

            void collion_check()
            {

            }
            void indexTomap(const nav_msgs::OccupancyGrid& map,int index,int& x,int& y)
            {
                y=index/map.info.width;
                x=index%map.info.width;
            }

    // void find_closest_frontier(const nav_msgs::OccupancyGrid& occupancy_map,int start_idx,int &frontier_idx)
    // {
    //     int a[4]={1,(int)occupancy_map.info.width,-1,-(int)occupancy_map.info.width};
    //     std::unordered_map<int, int> visited, unvisited;
    //     std::queue<int> id;
    //     int current;
    //     if(occupancy_map.data[start_idx]<97)
    //     {
    //         id.push(start_idx);
    //         while (id.size())
    //         {
    //             current = id.front();
    //             id.pop();
    //             for(int i=0;i<4;i++)
    //             {
    //                 int neighbor_id=current+a[i];
    //                 if(neighbor_id>=0&&neighbor_id<occupancy_map.info.width*occupancy_map.info.height)
    //                     {
    //                             if((occupancy_map.data[neighbor_id]<97)&&!(visited[neighbor_id]))
    //                             {
    //                                 if(occupancy_map.data[current]==-1)
    //                                 {
    //                                     frontier_idx=current;
    //                                     id=queue<int>();
    //                                     break;
    //                                 }
    //                                     id.push(neighbor_id);
    //                                     ++visited[neighbor_id];
    //                             }
    //                     }
    //             }
    //         }
    //     }
    // }

    //         void GetRobotpose(std::string iden_frame_id,geometry_msgs::PoseStamped& global_pose, ros::Time timestamp)
    //         {
    //         tf::StampedTransform transform;
    //         geometry_msgs::PoseStamped iden_pose;
    //         iden_pose.header.frame_id = iden_frame_id;
    //         iden_pose.header.stamp = ros::Time::now();
    //         iden_pose.pose.orientation.w = 1;
    //         tf_listener_.waitForTransform("/robot0/map",iden_frame_id, ros::Time(0), ros::Duration(2.0));
    //         tf_listener_.lookupTransform( "/robot0/map",iden_frame_id, ros::Time(0), transform);
    //         global_pose.pose.position.x=transform.getOrigin().x();
    //         global_pose.pose.position.y=transform.getOrigin().y();
    //         global_pose.pose.position.z=transform.getOrigin().z();
    //         }

            bool getCostmap;
            std::vector<int> particle_indexes;
            ros::NodeHandle NODE;
            ros::Subscriber costmap_sub_global_;
            ros::Publisher frontier_pub_;
            ros::Publisher robot_position_pub_;
            // ros::Publisher frontier_map_pub_;
            ros::Publisher costmap_pub_global_;
            nav_msgs::OccupancyGrid global_costmap;
            nav_msgs::OccupancyGrid local_costmap;
            geometry_msgs::PoseStamped global_pose;
            geometry_msgs::PoseStamped closest_frontier_point;

            std::mutex lock_costmap;
            std::thread tf_thread_;
            tf::TransformListener tf_listener_;
            int robot_index;
            int seed;
            int frontier_temp=0;
            int frontier_count=0;
            int closest_frontier_idx=0;
            int resolution=0.2;

            // protected:

    };
}
int main(int argc, char** argv)
{
    ros::init(argc, argv, "frontier_detection");
    ros::NodeHandle private_nh1("~");
    frontier_detection::frontierdetection ta(private_nh1);
    ros::MultiThreadedSpinner spinner;
    spinner.spin();
    return 0;
}


// Disable warnings for dlib
//
// Note: All additional dlib submodule dependencies should
// be included here, and all modules dependent on dlib
// should include this file. The dlib compilation process
// emits numerous pedantic and variadic macro errors, so this
// method of header inclusion is used to suppress them
#pragma GCC system_header

#include <dlib/matrix.h>
#include <dlib/svm.h>


#include "tag_hierarchy/tag_hierarchy.h"

#include "models/models.h"

#include <boost/graph/adj_list_serialize.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/iostreams/stream.hpp>
#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/serialization/map.hpp>
#include <boost/serialization/variant.hpp>
#include <boost/serialization/vector.hpp>

#include <functional>
#include <tag_hierarchy/commands/command.h>

using VertexIterator = boost::graph_traits<TagHierarchyGraph>::vertex_iterator;
using InEdgeIterator = TagHierarchyGraph::in_edge_iterator;
using OutEdgeIterator = TagHierarchyGraph::out_edge_iterator;

using DispatchFunction =
    std::function<std::vector<NodeType>(std::vector<NodeType>&)>;

class TagHierarchyImpl {
private:
    TagHierarchyGraph graph_;
    std::unordered_map<std::string, VertexT> vertices_;
    std::map<std::string, DispatchFunction> command_func_dispatch_;
    EdgeLabelMap edge_labels_;
    VertexT root_;

public:

    TagHierarchyGraph& GetGraph() {
        return graph_;
    }

    std::unordered_map<std::string, VertexT>& GetVertices() {
        return vertices_;
    }

    EdgeLabelMap& GetEdgeLabels() {
        return edge_labels_;
    }

    VertexT& GetRoot() {
        return root_;
    }

    std::map<std::string, DispatchFunction> GetDispatchMap() {
        return command_func_dispatch_;
    }

    std::vector<NodeType>
    Store(std::vector<NodeType>& message) {
        auto retval = std::vector<NodeType>();
        std::ostringstream stream;
        {
            boost::iostreams::filtering_stream<boost::iostreams::output> f;
            f.push(boost::iostreams::gzip_compressor());
            f.push(stream);
            boost::archive::binary_oarchive archive(f);
            archive << *this;
        }
        retval.push_back({{"serialized_graph", stream.str()}});
        return retval;
    }

    std::vector<NodeType>
    Restore(std::vector<NodeType>& message) {
        ClearGraph();
        auto retval = std::vector<NodeType>();
        const std::string graph_state =
            boost::get<std::string>(message[0]["serialized_hierarchy"]);
        std::istringstream buffer(graph_state);
        boost::iostreams::filtering_stream<boost::iostreams::input> f;
        f.push(boost::iostreams::gzip_decompressor());
        f.push(buffer);
        boost::archive::binary_iarchive archive(f);
        archive >> *this;
        retval.push_back({{"success", true}});
        return retval;
    }

    std::vector<NodeType>
    HealthCheck(std::vector<NodeType>& message) {
        auto retval = std::vector<NodeType>();
        if (root_ == std::numeric_limits<VertexT>::max()) {
            retval.push_back({{std::string("ok"), false},
                              {std::string("error"), std::string("Cache not populated")}});
            return retval;
        }
        retval.push_back({{std::string("ok"), true}});
        return retval;
    }

    void
    ClearGraph() {
        graph_.clear();
        vertices_.clear();
        root_ = std::numeric_limits<VertexT>::max();
    }

    std::vector<NodeType>
    Handle(std::vector<NodeType> &message)
    {
        auto command_map = message.at(0);
        std::string command = boost::get<std::string>(command_map["command"]);
        auto func = command_func_dispatch_[command];
        auto retval = func(message);
        return retval; //command_func_dispatch_[command](message);
    }

    TagHierarchyImpl() : command_func_dispatch_({
                             {"store", [this](std::vector<NodeType> &nodes) -> std::vector<NodeType> {
                                  return this->Store(nodes);
                              }},
                             {"restore", [this](std::vector<NodeType> &nodes) -> std::vector<NodeType> {
                                  return this->Restore(nodes);
                              }},
                             {"flush", [this](std::vector<NodeType> &nodes) -> std::vector<NodeType> {
                                  this->ClearGraph();
                                  return {{{std::string("success"), std::string("hierarchy flushed")}}};
                              }},
                             {"healthcheck", [this](std::vector<NodeType> &nodes) -> std::vector<NodeType> {
                                  return this->HealthCheck(nodes);
                              }},
                         }),
                         edge_labels_({{{"Physical hierarchy", 0}}}),
                         root_(std::numeric_limits<VertexT>::max())
    {
    };
    TagHierarchyImpl(const TagHierarchyImpl& in) : command_func_dispatch_(in.command_func_dispatch_) {}

    void Register(Command& in, std::string name) {
        auto func = in.Function();
        command_func_dispatch_[name] = func;
    }

    template<typename Archive>
    void save(Archive& ar, const unsigned int version) const {
        ar & graph_;
        ar & edge_labels_;
    }

    template<typename Archive>
    void load(Archive& ar, const unsigned int version) {
        ar & graph_;
        ar & edge_labels_;
        auto [start, end] = boost::vertices(graph_);
        for (auto iter = start; iter != end; ++iter) {
            if (graph_[*iter].id == "root") {
                root_ = *iter;
            }
            vertices_[graph_[*iter].id] = *iter;
        }
    }
    BOOST_SERIALIZATION_SPLIT_MEMBER()
};

std::vector<NodeType>
TagHierarchy::Handle(std::vector<NodeType>& message) {
    return GetTagHierarchy().Handle(message);
}

TagHierarchyImpl&
TagHierarchy::GetTagHierarchy() {
    static TagHierarchyImpl taghierarchy_impl;
    return taghierarchy_impl;
}

TagHierarchyGraph&
TagHierarchy::GetGraph() {
    return GetTagHierarchy().GetGraph();
}

std::unordered_map<std::string, VertexT>&
TagHierarchy::GetVertices() {
    return GetTagHierarchy().GetVertices();
}

EdgeLabelMap &
TagHierarchy::GetEdgeLabels() {
    return GetTagHierarchy().GetEdgeLabels();
}

VertexT&
TagHierarchy::GetRoot() {
    return GetTagHierarchy().GetRoot();
}

void
TagHierarchy::Register(Command &in, std::string name) {
    GetTagHierarchy().Register(in, name);
}


#include <merely3d/app.hpp>
#include <merely3d/window.hpp>

#include <Eigen/Geometry>

#include "../demo/example_model.hpp"

using merely3d::Window;
using merely3d::WindowBuilder;
using merely3d::Frame;
using merely3d::Material;
using merely3d::Color;
using merely3d::Line;

using merely3d::renderable;
using merely3d::Rectangle;
using merely3d::Box;
using merely3d::Sphere;

using Eigen::Vector2f;
using Eigen::Vector3f;
using Eigen::Quaternionf;
using Eigen::AngleAxisf;

merely3d::StaticMesh load_example_model()
{
    // Create a static mesh which we can use when rendering
    const auto & vn = merely3d::example_model::vertices_and_normals;
    const auto & idx = merely3d::example_model::indices;
    const auto model_vertices_normals = std::vector<float>(vn.begin(), vn.end());
    const auto model_indices = std::vector<unsigned int>(idx.begin(), idx.end());
    return merely3d::StaticMesh(model_vertices_normals, model_indices);
}

/// This relatively minimal example is used mainly to test parts of merely3d
/// for memory errors or undefined behavior with sanitizers such as
/// -fsanitize=address and/or -fsanitize=undefined
int main()
{
    // Constructing the app first is essential: it makes sure that
    // GLFW is set up properly. Note that as an alternative, you can call
    // glfw init/terminate yourself directly, but you must be careful that
    // any windows are destroyed before calling terminate(). App automatically
    // takes care of this as long as it outlives any windows.
    merely3d::App app;

    {
        auto window = WindowBuilder()
                .dimensions(1024, 768)
                .title("Hello merely3d!")
                .multisampling(8)
                .build();

        for (int i = 0; i < 100 && !window.should_close(); ++i)
        {
            window.render_frame([] (Frame & frame)
            {
                frame.draw(renderable(Rectangle(0.5, 0.5))
                            .with_position(1.0, 0.0, 0.5)
                            .with_orientation(AngleAxisf(0.78, Vector3f(1.0f, 0.0f, 0.0f)))
                            .with_material(Material().with_color(Color(0.5, 0.3, 0.3))));

                frame.draw_line(Line(Vector3f(0.0, 0.0, 0.0), Vector3f(10.0, -5.0, 10.0)));
                frame.draw(renderable(load_example_model()));
            });
        }
    }

    return 0;
}