/**
 * Some portion of the source code is from the Rodinia benchmark suite.
 *
 * These are the original authors:
 * 2009; Amittai Aviram; entire code written in C;
 * 2010; Jordan Fix and Andrew Wilkes; code converted to CUDA;
 * 2011.10; Lukasz G. Szafaryn; code converted to portable form, to C, OpenMP, CUDA, PGI versions;
 * 2011.12; Lukasz G. Szafaryn; Split different versions for Rodinia.
 * 2011.12; Lukasz G. Szafaryn; code converted to OpenCL;
 * 2012.10; Ke Wang; Change it to non-interactive mode. Use command option read command from file. And also add output for easy verification among different platforms and devices.Merged into Rodinia main distribution 2.2.
 */

#include <boost/program_options.hpp>
#include <cstdlib>
#include <cstdio>
#include <iostream>
#include <sys/time.h>

#include "../device_hopper/core.h"

using namespace device_hopper;

// TODO the following define and structs should be in the common.h header
// but the source to source cl translation needs them in the benchmark

#define  DEFAULT_ORDER 256

typedef struct knode {
	int location;
	int indices [DEFAULT_ORDER + 1];
	int  keys [DEFAULT_ORDER + 1];
	bool is_leaf;
	int num_keys;
} knode;

typedef struct record {
	int value;
} record;

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

#include "./rodinia_src/btree/common.h"
#include "./rodinia_src/btree/kernel_gpu_cuda_wrapper.h"

#define PREFERRED_DEVICE GPU

using namespace std::chrono;

knode *knodes;
record *krecords;
char *mem;
long freeptr;
long malloc_size;
long size;
long maxheight;
int order = DEFAULT_ORDER;
node *queue = NULL;

void *kmalloc(int size) {
    //printf("size: %d, current offset: %p\n",size,freeptr);
    void *r = (void *) freeptr;
    freeptr += size;
    if (freeptr > malloc_size + (long) mem) {
        printf("Memory Overflow\n");
        exit(1);
    }
    return r;
}

void enqueue(node *new_node) {
    node *c;
    if (queue == NULL) {
        queue = new_node;
        queue->next = NULL;
    } else {
        c = queue;
        while (c->next != NULL) {
            c = c->next;
        }
        c->next = new_node;
        new_node->next = NULL;
    }
}

node *dequeue(void) {
    node *n = queue;
    queue = queue->next;
    n->next = NULL;
    return n;
}

node *insert(node *root, int key, int value) {
    record *pointer;
    node *leaf;

    /* The current implementation ignores duplicates. */
    if (find(root, key, false) != NULL)
        return root;

    /* Create a new record for the value. */
    pointer = make_record(value);

    /* Case: the tree does not exist yet. Start a new tree. */
    if (root == NULL)
        return start_new_tree(key, pointer);

    /* Case: the tree already exists. (Rest of function body.) */
    leaf = find_leaf(root, key, false);

    /* Case: leaf has room for key and pointer. */
    if (leaf->num_keys < order - 1) {
        leaf = insert_into_leaf(leaf, key, pointer);
        return root;
    }

    /* Case:  leaf must be split. */
    return insert_into_leaf_after_splitting(root, leaf, key, pointer);
}

long transform_to_cuda(node *root, bool verbose) {
    struct timeval one, two;
    double time;
    gettimeofday(&one, NULL);
    long max_nodes = (long) (pow(order, log(size) / log(order / 2.0) - 1) + 1);
    malloc_size = size * sizeof(record) + max_nodes * sizeof(knode);
    mem = (char *) malloc(malloc_size);
    if (mem == NULL) {
        printf("Initial malloc error\n");
        exit(1);
    }
    freeptr = (long) mem;

    krecords = (record *) kmalloc(size * sizeof(record));
    // printf("%d records\n", size);
    knodes = (knode *) kmalloc(max_nodes * sizeof(knode));
    // printf("%d knodes\n", max_nodes);

    queue = NULL;
    enqueue(root);
    node *n;
    knode *k;
    int i;
    long nodeindex = 0;
    long recordindex = 0;
    long queueindex = 0;
    knodes[0].location = nodeindex++;

    while (queue != NULL) {
        n = dequeue();
        k = &knodes[queueindex];
        k->location = queueindex++;
        k->is_leaf = n->is_leaf;
        k->num_keys = n->num_keys + 2;
        //start at 1 because 0 is set to INT_MIN
        k->keys[0] = INT_MIN;
        k->keys[k->num_keys - 1] = INT_MAX;
        for (i = k->num_keys; i < order; i++)k->keys[i] = INT_MAX;
        if (!k->is_leaf) {
            k->indices[0] = nodeindex++;
            // if(k->indices[0]>3953){
            // printf("ERROR: %d\n", k->indices[0]);
            // }
            for (i = 1; i < k->num_keys - 1; i++) {
                k->keys[i] = n->keys[i - 1];
                enqueue((node *) n->pointers[i - 1]);
                k->indices[i] = nodeindex++;
                // if(k->indices[i]>3953){
                // printf("ERROR 1: %d\n", k->indices[i]);
                // }
                //knodes[nodeindex].location = nodeindex++;
            }
            //for final point of n
            enqueue((node *) n->pointers[i - 1]);
        } else {
            k->indices[0] = 0;
            for (i = 1; i < k->num_keys - 1; i++) {
                k->keys[i] = n->keys[i - 1];
                krecords[recordindex].value = ((record *) n->pointers[i - 1])->value;
                k->indices[i] = recordindex++;
                // if(k->indices[i]>3953){
                // printf("ERROR 2: %d\n", k->indices[i]);
                // }
            }
        }

        k->indices[k->num_keys - 1] = queueindex;
        // if(k->indices[k->num_keys-1]>3953){
        // printf("ERROR 3: %d\n", k->indices[k->num_keys-1]);
        // }

        if (verbose) {
            printf("Successfully created knode with index %d\n", k->location);
            printf("Is Leaf: %d, Num Keys: %d\n", k->is_leaf, k->num_keys);
            printf("Pointers: ");
            for (i = 0; i < k->num_keys; i++)
                printf("%d | ", k->indices[i]);
            printf("\nKeys: ");
            for (i = 0; i < k->num_keys; i++)
                printf("%d | ", k->keys[i]);
            printf("\n\n");
        }
    }
    long mem_used = size * sizeof(record) + (nodeindex) * sizeof(knode);
    if (verbose) {
        for (i = 0; i < size; i++)
            printf("%d ", krecords[i].value);
        printf("\nNumber of records = %d, sizeof(record)=%d, total=%d\n", size, sizeof(record), size * sizeof(record));
        printf("Number of knodes = %d, sizeof(knode)=%d, total=%d\n", nodeindex, sizeof(knode),
               (nodeindex) * sizeof(knode));
        printf("\nDone Transformation. Mem used: %d\n", mem_used);
    }
    gettimeofday(&two, NULL);
    double oneD = one.tv_sec + (double) one.tv_usec * .000001;
    double twoD = two.tv_sec + (double) two.tv_usec * .000001;
    time = twoD - oneD;
    printf("Tree transformation took %f\n", time);

    return mem_used;
}

/* Utility function to give the height of the tree, which length in number of edges of the path from the root to any leaf. */
int height(node *root) {
    int h = 0;
    node *c = root;
    while (!c->is_leaf) {
        c = (node *) c->pointers[0];
        h++;
    }
    return h;
}

/* Traces the path from the root to a leaf, searching by key.  Displays information about the path if the verbose flag is set. Returns the leaf containing the given key. */
node *find_leaf(node *root, int key, bool verbose) {
    int i = 0;
    node *c = root;
    if (c == NULL) {
        if (verbose)
            printf("Empty tree.\n");
        return c;
    }
    while (!c->is_leaf) {
        if (verbose) {
            printf("[");
            for (i = 0; i < c->num_keys - 1; i++)
                printf("%d ", c->keys[i]);
            printf("%d] ", c->keys[i]);
        }
        i = 0;
        while (i < c->num_keys) {
            if (key >= c->keys[i])
                i++;
            else
                break;
        }
        if (verbose)
            printf("%d ->\n", i);
        c = (node *) c->pointers[i];
    }
    if (verbose) {
        printf("Leaf [");
        for (i = 0; i < c->num_keys - 1; i++)
            printf("%d ", c->keys[i]);
        printf("%d] ->\n", c->keys[i]);
    }
    return c;

}

/* Finds and returns the record to which a key refers. */
record *find(node *root, int key, bool verbose) {
    int i = 0;
    node *c = find_leaf(root, key, verbose);
    if (c == NULL)
        return NULL;
    for (i = 0; i < c->num_keys; i++)
        if (c->keys[i] == key)
            break;
    if (i == c->num_keys)
        return NULL;
    else
        return (record *) c->pointers[i];
}

/* Finds the appropriate place to split a node that is too big into two. */
int cut(int length) {
    if (length % 2 == 0)
        return length / 2;
    else
        return length / 2 + 1;
}

//======================================================================================================================================================150
// INSERTION
//======================================================================================================================================================150

/* Creates a new record to hold the value to which a key refers. */
record *make_record(int value) {
    record *new_record = (record *) malloc(sizeof(record));
    if (new_record == NULL) {
        perror("Record creation.");
        exit(EXIT_FAILURE);
    } else {
        new_record->value = value;
    }
    return new_record;
}

/* Creates a new general node, which can be adapted to serve as either a leaf or an internal node. */
node *make_node(void) {
    node *new_node;
    new_node = (node *) malloc(sizeof(node));
    if (new_node == NULL) {
        perror("Node creation.");
        exit(EXIT_FAILURE);
    }
    new_node->keys = (int *) malloc((order - 1) * sizeof(int));
    if (new_node->keys == NULL) {
        perror("New node keys array.");
        exit(EXIT_FAILURE);
    }
    new_node->pointers = (void **) malloc(order * sizeof(void *));
    if (new_node->pointers == NULL) {
        perror("New node pointers array.");
        exit(EXIT_FAILURE);
    }
    new_node->is_leaf = false;
    new_node->num_keys = 0;
    new_node->parent = NULL;
    new_node->next = NULL;
    return new_node;
}

/* Creates a new leaf by creating a node and then adapting it appropriately. */
node *make_leaf(void) {
    node *leaf = make_node();
    leaf->is_leaf = true;
    return leaf;
}

/* Helper function used in insert_into_parent to find the index of the parent's pointer to the node to the left of the key to be inserted. */
int get_left_index(node *parent, node *left) {
    int left_index = 0;
    while (left_index <= parent->num_keys &&
           parent->pointers[left_index] != left)
        left_index++;
    return left_index;
}

/* Inserts a new pointer to a record and its corresponding key into a leaf. Returns the altered leaf. */
node *insert_into_leaf(node *leaf, int key, record *pointer) {

    int i, insertion_point;

    insertion_point = 0;
    while (insertion_point < leaf->num_keys && leaf->keys[insertion_point] < key)
        insertion_point++;

    for (i = leaf->num_keys; i > insertion_point; i--) {
        leaf->keys[i] = leaf->keys[i - 1];
        leaf->pointers[i] = leaf->pointers[i - 1];
    }
    leaf->keys[insertion_point] = key;
    leaf->pointers[insertion_point] = pointer;
    leaf->num_keys++;
    return leaf;
}

/* Inserts a new key and pointer to a new record into a leaf so as to exceed the tree's order, causing the leaf to be split in half. */
node *insert_into_leaf_after_splitting(node *root,
                                       node *leaf,
                                       int key,
                                       record *pointer) {

    node *new_leaf;
    int *temp_keys;
    void **temp_pointers;
    int insertion_index, split, new_key, i, j;

    new_leaf = make_leaf();

    temp_keys = (int *) malloc(order * sizeof(int));
    if (temp_keys == NULL) {
        perror("Temporary keys array.");
        exit(EXIT_FAILURE);
    }

    temp_pointers = (void **) malloc(order * sizeof(void *));
    if (temp_pointers == NULL) {
        perror("Temporary pointers array.");
        exit(EXIT_FAILURE);
    }

    insertion_index = 0;
    while (leaf->keys[insertion_index] < key && insertion_index < order - 1)
        insertion_index++;

    for (i = 0, j = 0; i < leaf->num_keys; i++, j++) {
        if (j == insertion_index) j++;
        temp_keys[j] = leaf->keys[i];
        temp_pointers[j] = leaf->pointers[i];
    }

    temp_keys[insertion_index] = key;
    temp_pointers[insertion_index] = pointer;

    leaf->num_keys = 0;

    split = cut(order - 1);

    for (i = 0; i < split; i++) {
        leaf->pointers[i] = temp_pointers[i];
        leaf->keys[i] = temp_keys[i];
        leaf->num_keys++;
    }

    for (i = split, j = 0; i < order; i++, j++) {
        new_leaf->pointers[j] = temp_pointers[i];
        new_leaf->keys[j] = temp_keys[i];
        new_leaf->num_keys++;
    }

    free(temp_pointers);
    free(temp_keys);

    new_leaf->pointers[order - 1] = leaf->pointers[order - 1];
    leaf->pointers[order - 1] = new_leaf;

    for (i = leaf->num_keys; i < order - 1; i++)
        leaf->pointers[i] = NULL;
    for (i = new_leaf->num_keys; i < order - 1; i++)
        new_leaf->pointers[i] = NULL;

    new_leaf->parent = leaf->parent;
    new_key = new_leaf->keys[0];

    return insert_into_parent(root, leaf, new_key, new_leaf);
}

/* Inserts a new key and pointer to a node into a node into which these can fit without violating the B+ tree properties. */
node *insert_into_node(node *root,
                       node *n,
                       int left_index,
                       int key,
                       node *right) {

    int i;

    for (i = n->num_keys; i > left_index; i--) {
        n->pointers[i + 1] = n->pointers[i];
        n->keys[i] = n->keys[i - 1];
    }
    n->pointers[left_index + 1] = right;
    n->keys[left_index] = key;
    n->num_keys++;
    return root;
}

/* Inserts a new key and pointer to a node into a node, causing the node's size to exceed the order, and causing the node to split into two. */
node *insert_into_node_after_splitting(node *root,
                                       node *old_node,
                                       int left_index,
                                       int key,
                                       node *right) {
    int i, j, split, k_prime;
    node *new_node, *child;
    int *temp_keys;
    node **temp_pointers;

    /* First create a temporary set of keys and pointers
    * to hold everything in order, including
    * the new key and pointer, inserted in their
    * correct places.
    * Then create a new node and copy half of the
    * keys and pointers to the old node and
    * the other half to the new.
    */

    temp_pointers = (node **) malloc((order + 1) * sizeof(node *));
    if (temp_pointers == NULL) {
        perror("Temporary pointers array for splitting nodes.");
        exit(EXIT_FAILURE);
    }
    temp_keys = (int *) malloc(order * sizeof(int));
    if (temp_keys == NULL) {
        perror("Temporary keys array for splitting nodes.");
        exit(EXIT_FAILURE);
    }

    for (i = 0, j = 0; i < old_node->num_keys + 1; i++, j++) {
        if (j == left_index + 1) j++;
        temp_pointers[j] = (node *) old_node->pointers[i];
    }

    for (i = 0, j = 0; i < old_node->num_keys; i++, j++) {
        if (j == left_index) j++;
        temp_keys[j] = old_node->keys[i];
    }

    temp_pointers[left_index + 1] = right;
    temp_keys[left_index] = key;

    /* Create the new node and copy
    * half the keys and pointers to the
    * old and half to the new.
    */
    split = cut(order);
    new_node = make_node();
    old_node->num_keys = 0;
    for (i = 0; i < split - 1; i++) {
        old_node->pointers[i] = temp_pointers[i];
        old_node->keys[i] = temp_keys[i];
        old_node->num_keys++;
    }
    old_node->pointers[i] = temp_pointers[i];
    k_prime = temp_keys[split - 1];
    for (++i, j = 0; i < order; i++, j++) {
        new_node->pointers[j] = temp_pointers[i];
        new_node->keys[j] = temp_keys[i];
        new_node->num_keys++;
    }
    new_node->pointers[j] = temp_pointers[i];
    free(temp_pointers);
    free(temp_keys);
    new_node->parent = old_node->parent;
    for (i = 0; i <= new_node->num_keys; i++) {
        child = (node *) new_node->pointers[i];
        child->parent = new_node;
    }

    /* Insert a new key into the parent of the two
* nodes resulting from the split, with
* the old node to the left and the new to the right.
*/

    return insert_into_parent(root, old_node, k_prime, new_node);
}

/* Inserts a new node (leaf or internal node) into the B+ tree. Returns the root of the tree after insertion. */
node *
insert_into_parent(node *root,
                   node *left,
                   int key,
                   node *right) {

    int left_index;
    node *parent;

    parent = left->parent;

    /* Case: new root. */

    if (parent == NULL)
        return insert_into_new_root(left, key, right);

    /* Case: leaf or node. (Remainder of
* function body.)
*/

    /* Find the parent's pointer to the left
* node.
*/

    left_index = get_left_index(parent, left);

    /* Simple case: the new key fits into the node.
*/

    if (parent->num_keys < order - 1)
        return insert_into_node(root, parent, left_index, key, right);

    /* Harder case:  split a node in order
* to preserve the B+ tree properties.
*/

    return insert_into_node_after_splitting(root, parent, left_index, key, right);
}

/* Creates a new root for two subtrees and inserts the appropriate key into the new root. */
node *
insert_into_new_root(node *left,
                     int key,
                     node *right) {

    node *root = make_node();
    root->keys[0] = key;
    root->pointers[0] = left;
    root->pointers[1] = right;
    root->num_keys++;
    root->parent = NULL;
    left->parent = root;
    right->parent = root;
    return root;
}

/* First insertion: start a new tree. */
node *
start_new_tree(int key,
               record *pointer) {

    node *root = make_leaf();
    root->keys[0] = key;
    root->pointers[0] = pointer;
    root->pointers[order - 1] = NULL;
    root->parent = NULL;
    root->num_keys++;
    return root;
}

//======================================================================================================================================================150
// DELETION
//======================================================================================================================================================150

/* Utility function for deletion. Retrieves the index of a node's nearest neighbor (sibling) to the left if one exists.  If not (the node is the leftmost child), returns -1 to signify this special case. */
int
get_neighbor_index(node *n) {

    int i;

    /* Return the index of the key to the left
* of the pointer in the parent pointing
* to n.
* If n is the leftmost child, this means
* return -1.
*/
    for (i = 0; i <= n->parent->num_keys; i++)
        if (n->parent->pointers[i] == n)
            return i - 1;

    // Error state.
    printf("Search for nonexistent pointer to node in parent.\n");
    //printf("Node:  %#x\n", (unsigned int)n);
    exit(EXIT_FAILURE);
}

/*   */
node *
remove_entry_from_node(node *n,
                       int key,
                       node *pointer) {

    int i, num_pointers;

    // Remove the key and shift other keys accordingly.
    i = 0;
    while (n->keys[i] != key)
        i++;
    for (++i; i < n->num_keys; i++)
        n->keys[i - 1] = n->keys[i];

    // Remove the pointer and shift other pointers accordingly.
    // First determine number of pointers.
    num_pointers = n->is_leaf ? n->num_keys : n->num_keys + 1;
    i = 0;
    while (n->pointers[i] != pointer)
        i++;
    for (++i; i < num_pointers; i++)
        n->pointers[i - 1] = n->pointers[i];

    // One key fewer.
    n->num_keys--;

    // Set the other pointers to NULL for tidiness.
    // A leaf uses the last pointer to point to the next leaf.
    if (n->is_leaf)
        for (i = n->num_keys; i < order - 1; i++)
            n->pointers[i] = NULL;
    else
        for (i = n->num_keys + 1; i < order; i++)
            n->pointers[i] = NULL;

    return n;
}

/*   */
node *
adjust_root(node *root) {

    node *new_root;

    /* Case: nonempty root.
* Key and pointer have already been deleted,
* so nothing to be done.
*/

    if (root->num_keys > 0)
        return root;

    /* Case: empty root.
*/

    // If it has a child, promote
    // the first (only) child
    // as the new root.

    if (!root->is_leaf) {
        new_root = (node *) root->pointers[0];
        new_root->parent = NULL;
    }

        // If it is a leaf (has no children),
        // then the whole tree is empty.

    else
        new_root = NULL;

    free(root->keys);
    free(root->pointers);
    free(root);

    return new_root;
}

/* Coalesces a node that has become too small after deletion with a neighboring node that can accept the additional entries without exceeding the maximum. */
node *
coalesce_nodes(node *root,
               node *n,
               node *neighbor,
               int neighbor_index,
               int k_prime) {

    int i, j, neighbor_insertion_index, n_start, n_end, new_k_prime;
    node *tmp;
    bool split;

    /* Swap neighbor with node if node is on the
* extreme left and neighbor is to its right.
*/

    if (neighbor_index == -1) {
        tmp = n;
        n = neighbor;
        neighbor = tmp;
    }

    /* Starting point in the neighbor for copying
* keys and pointers from n.
* Recall that n and neighbor have swapped places
* in the special case of n being a leftmost child.
*/

    neighbor_insertion_index = neighbor->num_keys;

    /*
* Nonleaf nodes may sometimes need to remain split,
* if the insertion of k_prime would cause the resulting
* single coalesced node to exceed the limit order - 1.
* The variable split is always false for leaf nodes
* and only sometimes set to true for nonleaf nodes.
*/

    split = false;

    /* Case:  nonleaf node.
* Append k_prime and the following pointer.
* If there is room in the neighbor, append
* all pointers and keys from the neighbor.
* Otherwise, append only cut(order) - 2 keys and
* cut(order) - 1 pointers.
*/

    if (!n->is_leaf) {

        /* Append k_prime.
    */

        neighbor->keys[neighbor_insertion_index] = k_prime;
        neighbor->num_keys++;

        /* Case (default):  there is room for all of n's keys and pointers
    * in the neighbor after appending k_prime.
    */

        n_end = n->num_keys;

        /* Case (special): k cannot fit with all the other keys and pointers
    * into one coalesced node.
    */
        n_start = 0; // Only used in this special case.
        if (n->num_keys + neighbor->num_keys >= order) {
            split = true;
            n_end = cut(order) - 2;
        }

        for (i = neighbor_insertion_index + 1, j = 0; j < n_end; i++, j++) {
            neighbor->keys[i] = n->keys[j];
            neighbor->pointers[i] = n->pointers[j];
            neighbor->num_keys++;
            n->num_keys--;
            n_start++;
        }

        /* The number of pointers is always
    * one more than the number of keys.
    */

        neighbor->pointers[i] = n->pointers[j];

        /* If the nodes are still split, remove the first key from
    * n.
    */
        if (split) {
            new_k_prime = n->keys[n_start];
            for (i = 0, j = n_start + 1; i < n->num_keys; i++, j++) {
                n->keys[i] = n->keys[j];
                n->pointers[i] = n->pointers[j];
            }
            n->pointers[i] = n->pointers[j];
            n->num_keys--;
        }

        /* All children must now point up to the same parent.
    */

        for (i = 0; i < neighbor->num_keys + 1; i++) {
            tmp = (node *) neighbor->pointers[i];
            tmp->parent = neighbor;
        }
    }

        /* In a leaf, append the keys and pointers of
    * n to the neighbor.
    * Set the neighbor's last pointer to point to
    * what had been n's right neighbor.
    */

    else {
        for (i = neighbor_insertion_index, j = 0; j < n->num_keys; i++, j++) {
            neighbor->keys[i] = n->keys[j];
            neighbor->pointers[i] = n->pointers[j];
            neighbor->num_keys++;
        }
        neighbor->pointers[order - 1] = n->pointers[order - 1];
    }

    if (!split) {
        root = delete_entry(root, n->parent, k_prime, n);
        free(n->keys);
        free(n->pointers);
        free(n);
    } else
        for (i = 0; i < n->parent->num_keys; i++)
            if (n->parent->pointers[i + 1] == n) {
                n->parent->keys[i] = new_k_prime;
                break;
            }

    return root;

}

/* Redistributes entries between two nodes when one has become too small after deletion but its neighbor is too big to append the small node's entries without exceeding the maximum */
node *
redistribute_nodes(node *root,
                   node *n,
                   node *neighbor,
                   int neighbor_index,
                   int k_prime_index,
                   int k_prime) {

    int i;
    node *tmp;

    /* Case: n has a neighbor to the left.
* Pull the neighbor's last key-pointer pair over
* from the neighbor's right end to n's left end.
*/

    if (neighbor_index != -1) {
        if (!n->is_leaf)
            n->pointers[n->num_keys + 1] = n->pointers[n->num_keys];
        for (i = n->num_keys; i > 0; i--) {
            n->keys[i] = n->keys[i - 1];
            n->pointers[i] = n->pointers[i - 1];
        }
        if (!n->is_leaf) {
            n->pointers[0] = neighbor->pointers[neighbor->num_keys];
            tmp = (node *) n->pointers[0];
            tmp->parent = n;
            neighbor->pointers[neighbor->num_keys] = NULL;
            n->keys[0] = k_prime;
            n->parent->keys[k_prime_index] = neighbor->keys[neighbor->num_keys - 1];
        } else {
            n->pointers[0] = neighbor->pointers[neighbor->num_keys - 1];
            neighbor->pointers[neighbor->num_keys - 1] = NULL;
            n->keys[0] = neighbor->keys[neighbor->num_keys - 1];
            n->parent->keys[k_prime_index] = n->keys[0];
        }
    }

        /* Case: n is the leftmost child.
    * Take a key-pointer pair from the neighbor to the right.
    * Move the neighbor's leftmost key-pointer pair
    * to n's rightmost position.
    */

    else {
        if (n->is_leaf) {
            n->keys[n->num_keys] = neighbor->keys[0];
            n->pointers[n->num_keys] = neighbor->pointers[0];
            n->parent->keys[k_prime_index] = neighbor->keys[1];
        } else {
            n->keys[n->num_keys] = k_prime;
            n->pointers[n->num_keys + 1] = neighbor->pointers[0];
            tmp = (node *) n->pointers[n->num_keys + 1];
            tmp->parent = n;
            n->parent->keys[k_prime_index] = neighbor->keys[0];
        }
        for (i = 0; i < neighbor->num_keys; i++) {
            neighbor->keys[i] = neighbor->keys[i + 1];
            neighbor->pointers[i] = neighbor->pointers[i + 1];
        }
        if (!n->is_leaf)
            neighbor->pointers[i] = neighbor->pointers[i + 1];
    }

    /* n now has one more key and one more pointer;
* the neighbor has one fewer of each.
*/

    n->num_keys++;
    neighbor->num_keys--;

    return root;
}

/* Deletes an entry from the B+ tree. Removes the record and its key and pointer from the leaf, and then makes all appropriate changes to preserve the B+ tree properties. */
node *
delete_entry(node *root,
             node *n,
             int key,
             void *pointer) {

    int min_keys;
    node *neighbor;
    int neighbor_index;
    int k_prime_index, k_prime;
    int capacity;

    // Remove key and pointer from node.

    n = remove_entry_from_node(n, key, (node *) pointer);

    /* Case:  deletion from the root.
*/

    if (n == root)
        return adjust_root(root);

    /* Case:  deletion from a node below the root.
* (Rest of function body.)
*/

    /* Determine minimum allowable size of node,
* to be preserved after deletion.
*/

    min_keys = n->is_leaf ? cut(order - 1) : cut(order) - 1;

    /* Case:  node stays at or above minimum.
* (The simple case.)
*/

    if (n->num_keys >= min_keys)
        return root;

    /* Case:  node falls below minimum.
* Either coalescence or redistribution
* is needed.
*/

    /* Find the appropriate neighbor node with which
* to coalesce.
* Also find the key (k_prime) in the parent
* between the pointer to node n and the pointer
* to the neighbor.
*/

    neighbor_index = get_neighbor_index(n);
    k_prime_index = neighbor_index == -1 ? 0 : neighbor_index;
    k_prime = n->parent->keys[k_prime_index];
    neighbor = neighbor_index == -1 ? (node *) n->parent->pointers[1] :
               (node *) n->parent->pointers[neighbor_index];

    capacity = n->is_leaf ? order : order - 1;

    /* Coalescence. */

    if (neighbor->num_keys + n->num_keys < capacity)
        return coalesce_nodes(root, n, neighbor, neighbor_index, k_prime);

        /* Redistribution. */

    else
        return redistribute_nodes(root, n, neighbor, neighbor_index, k_prime_index, k_prime);
}

/*   */
void
destroy_tree_nodes(node *root) {
    int i;
    if (root->is_leaf)
        for (i = 0; i < root->num_keys; i++)
            free(root->pointers[i]);
    else
        for (i = 0; i < root->num_keys + 1; i++)
            destroy_tree_nodes((node *) root->pointers[i]);
    free(root->pointers);
    free(root->keys);
    free(root);
}

/**
* Verify the results calculated using the device hopper programming
* model using the original benchmark cuda kernel.
*/
bool
verify_results(
        record *records,
        long records_mem,
        knode *knodes,
        long knodes_elem,
        long knodes_mem,

        int order,
        long maxheight,
        int count,

        long *currKnode,
        long *offset,
        int *keys,
        record *dhopper_ans) {

    // Compute reference results
    // Zero out buffers for intermediate results
    memset(currKnode, 0, count * sizeof(long));
    memset(offset, 0, count * sizeof(long));

    // OUTPUT: ans CPU allocation
    record *referenceAns = (record *) malloc(sizeof(record) * count);
    // OUTPUT: ans CPU initialization
    for (int i = 0; i < count; i++) referenceAns[i].value = -1;

    kernel_gpu_cuda_wrapper(
            records,
            records_mem,
            knodes,
            knodes_elem,
            knodes_mem,
            order,
            maxheight,
            count,
            currKnode,
            offset,
            keys,
            referenceAns);

    bool success = true;
    for (int i = 0; i < count; ++i) {
        if (referenceAns[i].value != dhopper_ans[i].value) {
            std::cout << "Error at index: " << i << " ";
            std::cout << "Expected value: "
                      << referenceAns[i].value
                      << " Computed value: "
                      << dhopper_ans[i].value << std::endl;
            success = false;
            break;
        }
    }

    free(referenceAns);

    return success;
}

#define REPOSITORY_PATH std::string(std::getenv("PLASTICITY_ROOT"))

DEVICE_HOPPER_MAIN(int argc, char* argv[]) {
    DEVICE_HOPPER_SETUP

    boost::program_options::options_description desc("Options");
    desc.add_options()("problem-size", boost::program_options::value<size_t>(), "Sample count");
    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);

    size_t problem_size = 0;
    if (vm.count("problem-size") == 0) {
        std::cerr << "Error: Problem size is missing." << std::endl;
        std::exit(EXIT_FAILURE);
    } else {
        problem_size = vm["problem-size"].as<size_t>();
    }

    std::string path_to_input_file;
    int count = 0;
    if (problem_size == 3) {
        count = 4500000;
        path_to_input_file = REPOSITORY_PATH + "/library/benchmarks/input_data/rodinia/b+tree/4500k.txt";
    } else if (problem_size == 2) {
        count = 3000000;
        path_to_input_file = REPOSITORY_PATH + "/library/benchmarks/input_data/rodinia/b+tree/3000k.txt";
    } else if (problem_size == 1) {
        count = 1500000;
        path_to_input_file = REPOSITORY_PATH + "/library/benchmarks/input_data/rodinia/b+tree/1500k.txt";
    } else {
        std::cerr << "Error: Unknown problem size" << std::endl;
        std::exit(EXIT_FAILURE);
    }

    // Load inputs
    node *root = NULL;
    // open input file
    FILE *file_pointer = fopen(path_to_input_file.c_str(), "r");
    if (file_pointer == NULL) {
        std::cerr << "Error: Failure to open input file." << std::endl;
        std::exit(EXIT_FAILURE);
    }

    // get # of numbers in the file
    fscanf(file_pointer, "%d\n", &size);
    int input = 0;
    while (!feof(file_pointer)) {
        fscanf(file_pointer, "%d\n", &input);
        root = insert(root, input, input);
    }
    fclose(file_pointer);

    // NOTE DH: this is done to ease handling the tree data with
    // CUDA but it can also be seen as a flattening of the tree and is
    // not necessarily CUDA specific.
    long mem_used = transform_to_cuda(root, 0);
    maxheight = height(root);
    printf("Max height: %d\n", maxheight);
    long rootLoc = (long) knodes - (long) mem;

    // Allocate memory
    // INPUT: records CPU allocation (setting pointer in mem variable)
    record *records_initial = (record *) mem;
    long records_elem = (long) rootLoc / sizeof(record);
    long records_mem = (long) rootLoc;
    printf("records_elem=%d, records_unit_mem=%d, records_mem=%d\n", (int) records_elem, (int) sizeof(record), (int) records_mem);

    // INPUT: knodes CPU allocation (setting pointer in mem variable)
    knode *knodes_initial = (knode *) ((long) mem + (long) rootLoc);
    long knodes_elem = ((long) (mem_used) - (long) rootLoc) / sizeof(knode);
    long knodes_mem = (long) (mem_used) - (long) rootLoc;
    printf("knodes_elem=%d, knodes_unit_mem=%d, knodes_mem=%d\n", (int) knodes_elem, (int) sizeof(knode), (int) knodes_mem);

    // NOTE DH: buffers are copied to independent memory locations to
    // integrate better with the current device hopper API.
    // An alternative and likely prettier approach is not to use
    // the common memory location in the first place.
    record *records = (record *) device_hopper::use_existing_buffer(records_elem, sizeof(record), records_initial);
    knode *knodes = (knode *) device_hopper::use_existing_buffer(knodes_elem, sizeof(knode), knodes_initial);

    // INPUT: currKnode CPU allocation
    long *currKnode = (long *) device_hopper::malloc(count, sizeof(long));
    // INPUT: offset CPU initialization
    memset(currKnode, 0, count * sizeof(long));

    // INPUT: offset CPU allocation
    long *offset = (long *) device_hopper::malloc(count, sizeof(long));
    // INPUT: offset CPU initialization
    memset(offset, 0, count * sizeof(long));

    // INPUT: keys CPU allocation
    int *keys = (int *) device_hopper::malloc(count, sizeof(int));
    // INPUT: keys CPU initialization
    for (int i = 0; i < count; i++) keys[i] = (rand() / (float) RAND_MAX) * size;

    // OUTPUT: ans CPU allocation
    record *ans = (record *) device_hopper::malloc(count, sizeof(record));
    // OUTPUT: ans CPU initialization
    for (int i = 0; i < count; i++) ans[i].value = -1;

    // // Create parallel for
    parallel_for pf(0, count*order, [=]DEVICE_HOPPER_LAMBDA() {
        // private thread IDs
        int thid = GET_ITERATION_WITHIN_BATCH();
        int bid  = GET_BATCH_ID();

        // processtree levels
        int i;
        for(i = 0; i < maxheight; i++){

            // if value is between the two keys
            if((knodes[currKnode[bid]].keys[thid]) <= keys[bid] && (knodes[currKnode[bid]].keys[thid+1] > keys[bid])){
                // this conditional statement is inserted to avoid crush due to but in original code
                // "offset[bid]" calculated below that addresses knodes[] in the next iteration goes outside of its bounds cause segmentation fault
                // more specifically, values saved into knodes->indices in the main function are out of bounds of knodes that they address
                if(knodes[offset[bid]].indices[thid] < knodes_elem){
                    offset[bid] = knodes[offset[bid]].indices[thid];
                }
            }

            device_hopper::batch_barrier();

            // set for next tree level
            if(thid==0){
                currKnode[bid] = offset[bid];
            }

            device_hopper::batch_barrier();
        }

        //At this point, we have a candidate leaf node which may contain
        //the target record.  Check each key to hopefully find the record
        if(knodes[currKnode[bid]].keys[thid] == keys[bid]){
            ans[bid].value = records[knodes[currKnode[bid]].indices[thid]].value;
        }
    });
    // Register buffers and specify access patterns
    pf.add_buffer_access_patterns(
        device_hopper::buf(knodes,    direction::IN, pattern::ALL_OR_ANY),
        device_hopper::buf(records,   direction::IN, pattern::ALL_OR_ANY),
        device_hopper::buf(currKnode, direction::IN_OUT, data_kind::INTERIM_RESULTS, pattern::SUCCESSIVE_SUBSECTIONS(1)),
        device_hopper::buf(offset,    direction::IN_OUT, data_kind::INTERIM_RESULTS, pattern::SUCCESSIVE_SUBSECTIONS(1)),
        device_hopper::buf(keys,      direction::IN,     pattern::SUCCESSIVE_SUBSECTIONS(1)),
        device_hopper::buf(ans,       direction::IN_OUT, pattern::SUCCESSIVE_SUBSECTIONS(1)));
    // Add scalar kernel parameters
    pf.add_scalar_parameters(maxheight, knodes_elem);
    // Set optional tuning parameters and call run()
    pf.opt_set_simple_indices(true).opt_set_batch_size(256).run();

    bool success = verify_results(
                        records,
                        records_mem,
                        knodes,
                        knodes_elem,
                        knodes_mem,
                        order,
                        maxheight,
                        count,
                        currKnode,
                        offset,
                        keys,
                        ans);

    free(currKnode);
    free(offset);
    free(keys);
    free(ans);
    free(mem);

    if(success) {
        std::cout << "Info: The results are correct" << std::endl;
        return EXIT_SUCCESS;
    } else {
        std::cout << "Error: The results are incorrect" << std::endl;
        return EXIT_FAILURE;
    }
}


/**
 * \file dcs/testbed/rao2013_dynaqos_application_manager.hpp
 *
 * \brief Application manager based on the work by (RAO et al., 2013)
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2014   Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_TESTBED_RAO2013_DYNAQOS_APPLICATION_MANAGER_HPP
#define DCS_TESTBED_RAO2013_DYNAQOS_APPLICATION_MANAGER_HPP

#include <boost/smart_ptr.hpp>
#include <cmath>
#include <cstddef>
#include <dcs/assert.hpp>
#include <dcs/debug.hpp>
#include <dcs/exception.hpp>
#include <dcs/logging.hpp>
#include <dcs/macro.hpp>
#include <dcs/math/traits/float.hpp>
#include <dcs/testbed/application_performance_category.hpp>
#include <dcs/testbed/base_application_manager.hpp>
#include <dcs/testbed/data_smoothers.hpp>
#include <dcs/testbed/virtual_machine_performance_category.hpp>
#include <fl/Headers.h>
#include <fstream>
#include <limits>
#include <map>
#include <stdexcept>
#include <string>
#include <vector>

namespace dcs { namespace testbed {

/**
 * \brief Application manager based on the work by (Rao et al., 2013)
 *
 * \tparam Traits The traits type
 *
 * This class implements the DynaQoS framework proposed in [1,2].
 *
 * References:
 * -# J. Rao and Y. Wei and J. Gong and C.-Z. Xu,
 *    "QoS Guarantees and Service Differentiation for Dynamic Cloud Applications,"
 *    IEEE Transactions on Network and Service Management 10(1):43-55, 2013.
 * -# J. Wei and C.-Z. Xu,
 *    "eQoS: Provisioning of Client-Perceived End-to-End QoS Guarantees in Web Servers,"
 *    IEEE Transactions on Computers 55(12):1543-1556, 2006.
 * .
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template <typename TraitsT>
class rao2013_dynaqos_application_manager: public base_application_manager<TraitsT>
{
	private: typedef base_application_manager<TraitsT> base_type;
	public: typedef typename base_type::traits_type traits_type;
	public: typedef typename traits_type::real_type real_type;
	private: typedef typename base_type::app_type app_type;
	private: typedef typename base_type::app_pointer app_pointer;
	private: typedef typename base_type::vm_identifier_type vm_identifier_type;
	private: typedef typename app_type::sensor_type sensor_type;
	private: typedef typename app_type::sensor_pointer sensor_pointer;
	private: typedef ::std::vector<real_type> observation_container;
	private: typedef ::std::map<application_performance_category,observation_container> observation_map;
	private: typedef ::std::map<application_performance_category,sensor_pointer> app_sensor_map;
	private: typedef ::std::map<virtual_machine_performance_category,::std::map<vm_identifier_type,sensor_pointer> > vm_sensor_map;

	private: static const ::std::string alpha_fuzzy_var_name;
	private: static const ::std::string e_fuzzy_var_name;
	private: static const ::std::string de_fuzzy_var_name;
	private: static const ::std::string du_fuzzy_var_name;

	public: rao2013_dynaqos_application_manager()
	: gamma_(0.8),
	  Ke_(0),
	  Kde_(0),
	  p_rc_fuzzy_eng_(new ::fl::Engine()),
	  p_sfc_fuzzy_eng_(new ::fl::Engine())
	{
		init();
	}

	public: void discount_factor(real_type value)
	{
		gamma_ = value;
	}

	public: real_type discount_factor() const
	{
		return gamma_;
	}

	public: void export_data_to(::std::string const& fname)
	{
		dat_fname_ = fname;
	}

	private: void init()
	{
		DCS_DEBUG_ASSERT( p_rc_fuzzy_eng_ );
		DCS_DEBUG_ASSERT( p_sfc_fuzzy_eng_ );

		const real_type one_third = 1.0/3.0;
		const real_type two_third = 2.0/3.0;
		const real_type one_sixth = 1.0/6.0;
		const real_type five_sixth = 5.0/6.0;

		::fl::InputVariable* p_iv = 0;
		::fl::OutputVariable* p_ov = 0;
		::fl::RuleBlock* p_rules = 0;

		// Setup the Resource Controller
		//   Membership functions taken from (Wei et al.,2006) [2]

		for (::std::size_t i = 0; i < 2; ++i)
		{
			p_iv = new ::fl::InputVariable();
			p_iv->setEnabled(true);
			switch (i)
			{
				case 0: // \Delta e(k)
					p_iv->setName(de_fuzzy_var_name);
					break;
				case 1: // e(k)
					p_iv->setName(e_fuzzy_var_name);
					break;
			}
			p_iv->setRange(-1, 1);
			p_iv->addTerm(new ::fl::Ramp("NL", -two_third, -1));
			p_iv->addTerm(new ::fl::Triangle("NM", -1, -two_third, -one_third));
			p_iv->addTerm(new ::fl::Triangle("NS", -two_third, -one_third, 0));
			p_iv->addTerm(new ::fl::Triangle("ZE", -one_third, 0, one_third));
			p_iv->addTerm(new ::fl::Triangle("PS", 0, one_third, two_third));
			p_iv->addTerm(new ::fl::Triangle("PM", one_third, two_third, 1));
			p_iv->addTerm(new ::fl::Ramp("PL", two_third, 1));
			p_rc_fuzzy_eng_->addInputVariable(p_iv);
		}

		p_ov = new ::fl::OutputVariable();
		p_ov->setEnabled(true);
		p_ov->setName(du_fuzzy_var_name);
		p_ov->setRange(-1, 1);
#if defined(FL_VERSION) // Until fuzzylite v. 5.x (the FL_VERSION macro was removed since fuzzylite 6.x)
		p_ov->fuzzyOutput()->setAccumulation(new ::fl::Maximum());
#else // Since fuzzylite v. 6.x
		p_ov->fuzzyOutput()->setAggregation(new ::fl::Maximum());
#endif // FL_VERSION
		p_ov->setDefuzzifier(new ::fl::Centroid());
		p_ov->setDefaultValue(::fl::nan);
		p_ov->setLockPreviousValue(false);
		p_ov->addTerm(new ::fl::Ramp("NL", -two_third, -1));
		p_ov->addTerm(new ::fl::Triangle("NM", -1, -two_third, -one_third));
		p_ov->addTerm(new ::fl::Triangle("NS", -two_third, -one_third, 0));
		p_ov->addTerm(new ::fl::Triangle("ZE", -one_third, 0, one_third));
		p_ov->addTerm(new ::fl::Triangle("PS", 0, one_third, two_third));
		p_ov->addTerm(new ::fl::Triangle("PM", one_third, two_third, 1));
		p_ov->addTerm(new ::fl::Ramp("PL", two_third, 1));
		p_rc_fuzzy_eng_->addOutputVariable(p_ov);

		p_rules = new ::fl::RuleBlock();
		p_rules->setEnabled(true);
		p_rules->setConjunction(new ::fl::Minimum());
		p_rules->setDisjunction(new ::fl::Maximum());
		p_rules->setImplication(new ::fl::Minimum());
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is NL then " + du_fuzzy_var_name + " is PL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is NM then " + du_fuzzy_var_name + " is PL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is NS then " + du_fuzzy_var_name + " is PL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is ZE then " + du_fuzzy_var_name + " is PL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is PS then " + du_fuzzy_var_name + " is PM", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is PM then " + du_fuzzy_var_name + " is PS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is PL then " + du_fuzzy_var_name + " is ZE", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is NL then " + du_fuzzy_var_name + " is PL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is NM then " + du_fuzzy_var_name + " is PL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is NS then " + du_fuzzy_var_name + " is PL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is ZE then " + du_fuzzy_var_name + " is PM", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is PS then " + du_fuzzy_var_name + " is PS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is PM then " + du_fuzzy_var_name + " is ZE", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is PL then " + du_fuzzy_var_name + " is NS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is NL then " + du_fuzzy_var_name + " is PL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is NM then " + du_fuzzy_var_name + " is PL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is NS then " + du_fuzzy_var_name + " is PM", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is ZE then " + du_fuzzy_var_name + " is PS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is PS then " + du_fuzzy_var_name + " is ZE", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is PM then " + du_fuzzy_var_name + " is NS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is PL then " + du_fuzzy_var_name + " is NM", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is NL then " + du_fuzzy_var_name + " is PL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is NM then " + du_fuzzy_var_name + " is PM", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is NS then " + du_fuzzy_var_name + " is PS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is ZE then " + du_fuzzy_var_name + " is ZE", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is PS then " + du_fuzzy_var_name + " is NS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is PM then " + du_fuzzy_var_name + " is NM", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is PL then " + du_fuzzy_var_name + " is NL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is NL then " + du_fuzzy_var_name + " is PM", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is NM then " + du_fuzzy_var_name + " is PS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is NS then " + du_fuzzy_var_name + " is ZE", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is ZE then " + du_fuzzy_var_name + " is NS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is PS then " + du_fuzzy_var_name + " is NM", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is PM then " + du_fuzzy_var_name + " is NL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is PL then " + du_fuzzy_var_name + " is NL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is NL then " + du_fuzzy_var_name + " is PS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is NM then " + du_fuzzy_var_name + " is ZE", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is NS then " + du_fuzzy_var_name + " is NS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is ZE then " + du_fuzzy_var_name + " is NM", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is PS then " + du_fuzzy_var_name + " is NL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is PM then " + du_fuzzy_var_name + " is NL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is PL then " + du_fuzzy_var_name + " is NL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is NL then " + du_fuzzy_var_name + " is ZE", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is NM then " + du_fuzzy_var_name + " is NS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is NS then " + du_fuzzy_var_name + " is NM", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is ZE then " + du_fuzzy_var_name + " is NL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is PS then " + du_fuzzy_var_name + " is NL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is PM then " + du_fuzzy_var_name + " is NL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is PL then " + du_fuzzy_var_name + " is NL", p_rc_fuzzy_eng_.get()));
		p_rc_fuzzy_eng_->addRuleBlock(p_rules);

		// Setup the Scaling Factor Controller
		//   Membership functions taken from (Wei et al.,2006) [2]

		for (::std::size_t i = 0; i < 2; ++i)
		{
			p_iv = new ::fl::InputVariable();
			p_iv->setEnabled(true);
			switch (i)
			{
				case 0: // \Delta e(k)
					p_iv->setName(de_fuzzy_var_name);
					break;
				case 1: // e(k)
					p_iv->setName(e_fuzzy_var_name);
					break;
			}
			p_iv->setRange(-1, 1);
			p_iv->addTerm(new ::fl::Ramp("NL", -two_third, -1));
			p_iv->addTerm(new ::fl::Triangle("NM", -1, -two_third, -one_third));
			p_iv->addTerm(new ::fl::Triangle("NS", -two_third, -one_third, 0));
			p_iv->addTerm(new ::fl::Triangle("ZE", -one_third, 0, one_third));
			p_iv->addTerm(new ::fl::Triangle("PS", 0, one_third, two_third));
			p_iv->addTerm(new ::fl::Triangle("PM", one_third, two_third, 1));
			p_iv->addTerm(new ::fl::Ramp("PL", two_third, 1));
			p_sfc_fuzzy_eng_->addInputVariable(p_iv);
		}

		p_ov = new ::fl::OutputVariable();
		p_ov->setEnabled(true);
		p_ov->setName(alpha_fuzzy_var_name);
		p_ov->setRange(0, 1);
#if defined(FL_VERSION) // Until fuzzylite v. 5.x (the FL_VERSION macro was removed since fuzzylite 6.x)
		p_ov->fuzzyOutput()->setAccumulation(new ::fl::Maximum());
#else // Since fuzzylite v. 6.x
		p_ov->fuzzyOutput()->setAggregation(new ::fl::Maximum());
#endif // FL_VERSION
		p_ov->setDefuzzifier(new ::fl::Centroid());
		p_ov->setDefaultValue(::fl::nan);
		p_ov->setLockPreviousValue(false);
		p_ov->addTerm(new ::fl::Ramp("ZE", one_sixth, 0));
		p_ov->addTerm(new ::fl::Triangle("VS", 0, one_sixth, one_third));
		p_ov->addTerm(new ::fl::Triangle("SM", one_sixth, one_third, 0.5));
		p_ov->addTerm(new ::fl::Triangle("SL", one_third, 0.5, two_third));
		p_ov->addTerm(new ::fl::Triangle("ML", 0.5, two_third, five_sixth));
		p_ov->addTerm(new ::fl::Triangle("LG", two_third, five_sixth, 1));
		p_ov->addTerm(new ::fl::Ramp("VL", five_sixth, 1));
		p_sfc_fuzzy_eng_->addOutputVariable(p_ov);

		p_rules = new ::fl::RuleBlock();
		p_rules->setEnabled(true);
		p_rules->setConjunction(new ::fl::Minimum());
		p_rules->setDisjunction(new ::fl::Maximum());
		p_rules->setImplication(new ::fl::Minimum());
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is NL then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is NM then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is NS then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is ZE then " + alpha_fuzzy_var_name + " is SM", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is PS then " + alpha_fuzzy_var_name + " is VS", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is PM then " + alpha_fuzzy_var_name + " is VS", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is PL then " + alpha_fuzzy_var_name + " is ZE", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is NL then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is NM then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is NS then " + alpha_fuzzy_var_name + " is LG", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is ZE then " + alpha_fuzzy_var_name + " is SL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is PS then " + alpha_fuzzy_var_name + " is SM", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is PM then " + alpha_fuzzy_var_name + " is SM", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is PL then " + alpha_fuzzy_var_name + " is SM", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is NL then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is NM then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is NS then " + alpha_fuzzy_var_name + " is LG", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is ZE then " + alpha_fuzzy_var_name + " is ML", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is PS then " + alpha_fuzzy_var_name + " is VS", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is PM then " + alpha_fuzzy_var_name + " is SM", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is PL then " + alpha_fuzzy_var_name + " is SL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is NL then " + alpha_fuzzy_var_name + " is LG", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is NM then " + alpha_fuzzy_var_name + " is ML", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is NS then " + alpha_fuzzy_var_name + " is SL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is ZE then " + alpha_fuzzy_var_name + " is ZE", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is PS then " + alpha_fuzzy_var_name + " is SL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is PM then " + alpha_fuzzy_var_name + " is ML", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is PL then " + alpha_fuzzy_var_name + " is LG", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is NL then " + alpha_fuzzy_var_name + " is SL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is NM then " + alpha_fuzzy_var_name + " is SM", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is NS then " + alpha_fuzzy_var_name + " is VS", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is ZE then " + alpha_fuzzy_var_name + " is ML", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is PS then " + alpha_fuzzy_var_name + " is LG", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is PM then " + alpha_fuzzy_var_name + " is LG", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is PL then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is NL then " + alpha_fuzzy_var_name + " is SM", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is NM then " + alpha_fuzzy_var_name + " is SM", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is NS then " + alpha_fuzzy_var_name + " is SM", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is ZE then " + alpha_fuzzy_var_name + " is SL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is PS then " + alpha_fuzzy_var_name + " is LG", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is PM then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is PL then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is NL then " + alpha_fuzzy_var_name + " is ZE", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is NM then " + alpha_fuzzy_var_name + " is VS", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is NS then " + alpha_fuzzy_var_name + " is VS", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is ZE then " + alpha_fuzzy_var_name + " is SM", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is PS then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is PM then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is PL then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_sfc_fuzzy_eng_->addRuleBlock(p_rules);
	}

	private: void do_reset()
	{
		typedef typename base_type::target_value_map::const_iterator target_iterator;
		typedef typename app_type::vm_pointer vm_pointer;

		const ::std::vector<vm_pointer> vms = this->app().vms();
		const ::std::size_t nvms = this->app().num_vms();

		// Reset output sensors
		app_sensors_.clear();
		const target_iterator tgt_end_it = this->target_values().end();
		for (target_iterator tgt_it = this->target_values().begin();
			 tgt_it != tgt_end_it;
			 ++tgt_it)
		{
			const application_performance_category cat(tgt_it->first);

			app_sensors_[cat] = this->app().sensor(cat);
			es_[cat] = 0;
		}

		// Reset input sensors
		vm_sensors_.clear();
		for (::std::size_t i = 0; i < nvms; ++i)
		{
			const virtual_machine_performance_category cat = cpu_util_virtual_machine_performance;
			vm_pointer p_vm = vms[i];

			vm_sensors_[cat][p_vm->id()] = p_vm->sensor(cat);
		}

		// Reset counters
		ctl_count_ = ctl_skip_count_
				   = ctl_fail_count_
				   = 0;

		// Reset fuzzy controllers
		p_rc_fuzzy_eng_->restart();
		p_sfc_fuzzy_eng_->restart();

		// Reset input scaling factors
		Ke_ = Kde_ = 0;

		// Reset VCPU util estimator and smoother
		for (::std::size_t i = 0; i < nvms; ++i)
		{
			//this->data_estimator(cpu_util_virtual_machine_performance, vms[i]->id(), ::boost::make_shared< testbed::mean_estimator<real_type> >());
			this->data_smoother(cpu_util_virtual_machine_performance, vms[i]->id(), ::boost::make_shared< testbed::brown_single_exponential_smoother<real_type> >(0.9));
			//this->data_smoother(cpu_util_virtual_machine_performance, vms[i]->id(), ::boost::make_shared< testbed::holt_winters_double_exponential_smoother<real_type> >(beta_));
		}

		// Reset output data file and write header
		if (p_dat_ofs_ && p_dat_ofs_->is_open())
		{
			p_dat_ofs_->close();
		}
		p_dat_ofs_.reset();
		if (!dat_fname_.empty())
		{
			p_dat_ofs_ = ::boost::make_shared< ::std::ofstream >(dat_fname_.c_str());
			if (!p_dat_ofs_->good())
			{
				::std::ostringstream oss;
				oss << "Cannot open output data file '" << dat_fname_ << "'";

				DCS_EXCEPTION_THROW(::std::runtime_error, oss.str());
			}

			*p_dat_ofs_ << "\"ts\"";

			const ::std::size_t nvms = this->app().num_vms();
			for (::std::size_t i = 0; i < nvms; ++i)
			{
				*p_dat_ofs_ << ",\"Cap_{" << vms[i]->id() << "}\",\"Share_{" << vms[i]->id() << "}\",\"Util_{" << vms[i]->id() << "}\"";
			}
			for (target_iterator tgt_it = this->target_values().begin();
				 tgt_it != tgt_end_it;
				 ++tgt_it)
			{
				const application_performance_category cat = tgt_it->first;

				*p_dat_ofs_ << ",\"y_{" << cat << "}\",\"r_{" << cat << "}\"";
			}
			*p_dat_ofs_ << ",\"alpha\",\"Delta u\",\"K_e\",\"K_{Delta e}\",\"# Controls\",\"# Skip Controls\",\"# Fail Controls\"";
			*p_dat_ofs_ << ::std::endl;
		}
	}

	private: void do_sample()
	{
		typedef typename vm_sensor_map::const_iterator vm_sensor_iterator;
		typedef typename app_sensor_map::const_iterator app_sensor_iterator;
		typedef ::std::vector<typename sensor_type::observation_type> obs_container;
		typedef typename obs_container::const_iterator obs_iterator;

		DCS_DEBUG_TRACE("(" << this << ") BEGIN Do SAMPLE - Count: " << ctl_count_ << "/" << ctl_skip_count_ << "/" << ctl_fail_count_);

		// Collect VM values
		const vm_sensor_iterator vm_sens_end_it = vm_sensors_.end();
		for (vm_sensor_iterator vm_sens_it = vm_sensors_.begin();
			 vm_sens_it != vm_sens_end_it;
			 ++vm_sens_it)
		{
			const virtual_machine_performance_category cat = vm_sens_it->first;

			const typename vm_sensor_map::mapped_type::const_iterator vm_end_it = vm_sens_it->second.end();
			for (typename vm_sensor_map::mapped_type::const_iterator vm_it = vm_sens_it->second.begin();
				 vm_it != vm_end_it;
				 ++vm_it)
			{
				const vm_identifier_type vm_id = vm_it->first;
				sensor_pointer p_sens = vm_it->second;

				// check: p_sens != null
				DCS_DEBUG_ASSERT( p_sens );

				p_sens->sense();
				if (p_sens->has_observations())
				{
					const obs_container obs = p_sens->observations();
					const obs_iterator end_it = obs.end();
					for (obs_iterator it = obs.begin();
						 it != end_it;
						 ++it)
					{
						//this->data_estimator(cat, vm_id).collect(it->value());
						this->data_smoother(cat, vm_id).smooth(it->value());
					}
				}
			}
		}

		// Collect App values
		const app_sensor_iterator app_sens_end_it = app_sensors_.end();
		for (app_sensor_iterator app_sens_it = app_sensors_.begin();
			 app_sens_it != app_sens_end_it;
			 ++app_sens_it)
		{
			const application_performance_category cat(app_sens_it->first);

			sensor_pointer p_sens(app_sens_it->second);

			// check: p_sens != null
			DCS_DEBUG_ASSERT( p_sens );

			p_sens->sense();
			if (p_sens->has_observations())
			{
				const obs_container obs = p_sens->observations();
				const obs_iterator end_it = obs.end();
				for (obs_iterator it = obs.begin();
					 it != end_it;
					 ++it)
				{
					this->data_estimator(cat).collect(it->value());
				}
			}
		}

		DCS_DEBUG_TRACE("(" << this << ") END Do SAMPLE - Count: " << ctl_count_ << "/" << ctl_skip_count_ << "/" << ctl_fail_count_);
	}

	private: void do_control()
	{
		typedef typename base_type::target_value_map::const_iterator target_iterator;
		typedef typename app_type::vm_pointer vm_pointer;

		DCS_DEBUG_TRACE("(" << this << ") BEGIN Do CONTROL - Count: " << ctl_count_ << "/" << ctl_skip_count_ << "/" << ctl_fail_count_);

		++ctl_count_;

		bool skip_ctl = false;

		::std::vector<vm_pointer> vms = this->app().vms();
		const ::std::size_t nvms = vms.size();

		//std::map<application_performance_category,real_type> es_;
		std::map<application_performance_category,real_type> des;

		const target_iterator tgt_end_it = this->target_values().end();
		for (target_iterator tgt_it = this->target_values().begin();
			 tgt_it != tgt_end_it;
			 ++tgt_it)
		{
			const application_performance_category cat(tgt_it->first);

			// Compute a summary statistics of collected observation
			if (this->data_estimator(cat).count() > 0)
			{
				const real_type y = this->data_estimator(cat).estimate();
				const real_type r = this->target_value(cat);

				real_type e = 0;
				switch (cat)
				{
					case response_time_application_performance:
						if (dcs::math::float_traits<real_type>::approximately_less_equal(y, 2.0*r))
						{
							e = (r-y)/r;
						}
						else
						{
							e = -1;
						}
						break;
					case throughput_application_performance:
						if (dcs::math::float_traits<real_type>::approximately_greater_equal(y, 0.5*r))
						{
							e = (y-r)/r;
						}
						else
						{
							e = -1;
						}
						break;
				}
				if (es_.count(cat) > 0)
				{
					des[cat] = e-es_.at(cat);
				}
				else
				{
					des[cat] = e;
				}
				es_[cat] = e;
DCS_DEBUG_TRACE("APP Performance Category: " << cat << " - Y(k): " << y << " - R: " << r << " -> E(k+1): " << es_.at(cat) << " - DeltaE(k+1): " << des.at(cat));//XXX
			}
			else
			{
				// No observation collected during the last control interval
				DCS_DEBUG_TRACE("No output observation collected during the last control interval -> Skip control");
				skip_ctl = true;
				break;
			}
#ifdef DCSXX_TESTBED_EXP_APP_MGR_RESET_ESTIMATION_EVERY_INTERVAL
			this->data_estimator(cat).reset();
#endif // DCSXX_TESTBED_EXP_APP_MGR_RESET_ESTIMATION_EVERY_INTERVAL
		}

        if (!skip_ctl)
        {
			//FIXME: actually we only handle SISO systems
			DCS_ASSERT(es_.size() == 1,
					   DCS_EXCEPTION_THROW(::std::runtime_error,
					   "Only SISO system are currently managed"));

			// Compute the input to the RC fuzzy controller
			const real_type Ke = (ctl_count_ > 1) ? ::std::abs(Ke_) : 1.0;
			const real_type Kde = (ctl_count_ > 1) ? ::std::abs(Kde_) : 1.0;
			const real_type e = Ke*es_.begin()->second;
			const real_type de = Kde*des.begin()->second;

			// Update input scaling factors
			Ke_ = (1-gamma_)*Ke_ + gamma_*es_.begin()->second;
			Kde_ = (1-gamma_)*Kde_ - gamma_*des.begin()->second;

			// Perform fuzzy control
			bool ok = false;
			real_type du = 0;
			real_type alpha = 0;

			try
			{
				// Compute \Delta u(k)
				p_rc_fuzzy_eng_->setInputValue(e_fuzzy_var_name, e);
				p_rc_fuzzy_eng_->setInputValue(de_fuzzy_var_name, de);
				p_rc_fuzzy_eng_->process();
				du = p_rc_fuzzy_eng_->getOutputValue(du_fuzzy_var_name);

				// Compute the \alpha(k) scaling factor
				p_sfc_fuzzy_eng_->setInputValue(e_fuzzy_var_name, e);
				p_sfc_fuzzy_eng_->setInputValue(de_fuzzy_var_name, de);
				p_sfc_fuzzy_eng_->process();
				alpha = p_sfc_fuzzy_eng_->getOutputValue(alpha_fuzzy_var_name);

				ok = true;
			}
			catch (::fl::Exception const& fe)
			{
				DCS_DEBUG_TRACE( "Caught exception: " << fe.what() );

				::std::ostringstream oss;
				oss << "Unable to compute optimal control: " << fe.what();
				::dcs::log_warn(DCS_LOGGING_AT, oss.str());
			}
			catch (::std::exception const& se)
			{
				DCS_DEBUG_TRACE( "Caught exception: " << se.what() );

				::std::ostringstream oss;
				oss << "Unable to compute optimal control: " << se.what();
				::dcs::log_warn(DCS_LOGGING_AT, oss.str());
			}

			// Apply fuzzy control results
			if (ok)
			{
				for (::std::size_t i = 0; i < nvms; ++i)
				{
					vm_pointer p_vm = vms[i];

					const real_type c = p_vm->cpu_share();

					// Compute the output amplifier
					const real_type Kdu = c*0.5*::std::abs(e);

					const real_type u = ::std::max(::std::min(c+alpha*Kdu*du, 1.0), 0.0);

					DCS_DEBUG_TRACE("VM '" << p_vm->id() << "' - old-share: " << c << " - new-share: " << u);

					if (::std::isfinite(u) && !::dcs::math::float_traits<real_type>::essentially_equal(c, u))
					{
						p_vm->cpu_share(u);
DCS_DEBUG_TRACE("VM " << vms[i]->id() << ", Alpha: " << alpha << ", DeltaU: " << du << ", K_{DeltaU}: " << Kdu << " -> U(k+1): " << u);//XXX
					}
				}
DCS_DEBUG_TRACE("Optimal control applied");//XXX
			}
			else
			{
				++ctl_fail_count_;

				::std::ostringstream oss;
				oss << "Control not applied: failed to solve the control problem";
				::dcs::log_warn(DCS_LOGGING_AT, oss.str());
			}
		}
		else
		{
			++ctl_skip_count_;
		}

		// Export to file
		if (p_dat_ofs_)
		{
			*p_dat_ofs_ << ::std::time(0) << ",";
			for (::std::size_t i = 0; i < nvms; ++i)
			{
				const vm_pointer p_vm = vms[i];

				// check: p_vm != null
				DCS_DEBUG_ASSERT( p_vm );

				if (i != 0)
				{
					*p_dat_ofs_ << ",";
				}
				*p_dat_ofs_ << p_vm->cpu_cap() << "," << p_vm->cpu_share() << "," << this->data_smoother(cpu_util_virtual_machine_performance, p_vm->id()).forecast(0);
			}
			*p_dat_ofs_ << ",";
			const target_iterator tgt_end_it = this->target_values().end();
			for (target_iterator tgt_it = this->target_values().begin();
				 tgt_it != tgt_end_it;
				 ++tgt_it)
			{
				const application_performance_category cat = tgt_it->first;

				if (tgt_it != this->target_values().begin())
				{
					*p_dat_ofs_ << ",";
				}
				const real_type y = this->data_estimator(cat).estimate();
				const real_type r = tgt_it->second;
				*p_dat_ofs_ << y << "," << r;
			}
			if (skip_ctl)
			{
				const real_type nan = ::std::numeric_limits<real_type>::quiet_NaN();

				*p_dat_ofs_ << "," << nan << "," << nan;
			}
			else
			{
				const real_type alpha = p_sfc_fuzzy_eng_->getOutputValue(alpha_fuzzy_var_name);
				const real_type du = p_rc_fuzzy_eng_->getOutputValue(du_fuzzy_var_name);

				*p_dat_ofs_ << "," << alpha << "," << du;
			}
			*p_dat_ofs_ << "," << Ke_ << "," << Kde_;
			*p_dat_ofs_ << "," << ctl_count_ << "," << ctl_skip_count_ << "," << ctl_fail_count_;
			*p_dat_ofs_ << ::std::endl;
		}

		DCS_DEBUG_TRACE("(" << this << ") END Do CONTROL - Count: " << ctl_count_ << "/" << ctl_skip_count_ << "/" << ctl_fail_count_);
	}

	private: real_type gamma_; ///< The EWMA smoothing factor for Cres
	private: real_type Ke_;
	private: real_type Kde_;
	private: ::boost::shared_ptr< ::fl::Engine > p_rc_fuzzy_eng_; ///< The fuzzy resource control engine
	private: ::boost::shared_ptr< ::fl::Engine > p_sfc_fuzzy_eng_; ///< The fuzzy scaling factor control engine
	private: ::std::map<application_performance_category,real_type> es_; ///< The e(k) variable, grouped by application performance category
	private: ::std::size_t ctl_count_; ///< Number of times control function has been invoked
	private: ::std::size_t ctl_skip_count_; ///< Number of times control has been skipped
	private: ::std::size_t ctl_fail_count_; ///< Number of times control has failed
	private: vm_sensor_map vm_sensors_;
	private: app_sensor_map app_sensors_;
	private: ::std::string dat_fname_;
	private: ::boost::shared_ptr< ::std::ofstream > p_dat_ofs_;
}; // rao2013_dynaqos_application_manager

template <typename T>
const ::std::string rao2013_dynaqos_application_manager<T>::alpha_fuzzy_var_name = "alpha";

template <typename T>
const ::std::string rao2013_dynaqos_application_manager<T>::de_fuzzy_var_name = "DeltaE";

template <typename T>
const ::std::string rao2013_dynaqos_application_manager<T>::du_fuzzy_var_name = "DeltaU";

template <typename T>
const ::std::string rao2013_dynaqos_application_manager<T>::e_fuzzy_var_name = "E";

}} // Namespace dcs::testbed

#endif // DCS_TESTBED_RAO2013_DYNAQOS_APPLICATION_MANAGER_HPP


/**
 * @file   CamPose.cpp
 * @brief  Implementation of CamPose class for camera pose representation.
 * @author Charlie Li
 * @date   2019.08.23
 */

#include "CamPose.hpp"

#include <iostream>

#include <opencv2/calib3d.hpp> // cv::Rodrigues()
#include <opencv2/core.hpp>
#include <opencv2/core/eigen.hpp> // cv::cv2eigen()
#include <Eigen/Core>
#include <Eigen/Geometry> // for Matrix3f::eulerAngles(), Quaternion

namespace SLAM_demo {

using std::endl;
using cv::Mat;

CamPose::CamPose() : mTcw(Mat(3, 4, CV_32FC1)), mTwc(Mat(3, 4, CV_32FC1))
{
    Mat I = Mat::eye(3, 3, CV_32FC1);
    Mat zero = Mat::zeros(3, 1, CV_32FC1);
    I.copyTo(mTcw.rowRange(0, 3).colRange(0, 3));
    zero.copyTo(mTcw.rowRange(0, 3).col(3));
    setPoseInv();
}

CamPose::CamPose(const cv::Mat& Tcw) :
    mTcw(Tcw.clone()), mTwc(Mat(3, 4, CV_32FC1))
{
    setPoseInv();
}

CamPose::CamPose(const cv::Mat& Rcw, const cv::Mat& tcw) :
    mTcw(Mat(3, 4, CV_32FC1)), mTwc(Mat(3, 4, CV_32FC1))
{
    Rcw.copyTo(mTcw.rowRange(0, 3).colRange(0, 3));
    tcw.copyTo(mTcw.rowRange(0, 3).col(3));
    setPoseInv();
}

CamPose::CamPose(const CamPose& pose)
{
    mTcw = pose.mTcw.clone(); // clone the cv::Mat!
    mTwc = pose.mTwc.clone();
}

CamPose& CamPose::operator=(const CamPose& pose)
{
    mTcw = pose.mTcw.clone(); // clone the cv::Mat!
    mTwc = pose.mTwc.clone();
    return *this;
}

void CamPose::setPose(const cv::Mat& Tcw)
{
    mTcw = Tcw.clone();
    setPoseInv();
}

void CamPose::setPose(const cv::Mat& Rcw, const cv::Mat& tcw)
{
    setRotation(Rcw);
    setTranslation(tcw);
    setPoseInv();
}

cv::Mat CamPose::getRotationAngleAxis() const
{
    Mat RcwAA; // rotation in angle-axis representation
    Mat Rcw = getRotation();
    cv::Rodrigues(Rcw, RcwAA, cv::noArray());
    return RcwAA;
}

//cv::Mat CamPose::getRotationInvAngleAxis() const
//{
//    Mat Rwc = getRotationInv();
//    Mat RwcAA = cv::Rodrigues(Rcw, RcwAA. cv::noArray());
//    return RwcAA;
//}

cv::Mat CamPose::getTranslationSS() const
{
    Mat tx = Mat::zeros(3, 3, CV_32FC1);
    Mat t = getTranslation();
    tx.at<float>(0, 1) = -t.at<float>(2);
    tx.at<float>(0, 2) =  t.at<float>(1);
    tx.at<float>(1, 2) = -t.at<float>(0);
    tx.at<float>(1, 0) = -tx.at<float>(0, 1);
    tx.at<float>(2, 0) = -tx.at<float>(0, 2);
    tx.at<float>(2, 1) = -tx.at<float>(1, 2);
    return tx;
}

Eigen::Matrix<float, 3, 1> CamPose::getREulerAngleEigen() const
{
    Eigen::Matrix<float, 3, 1> ea;
    Eigen::Matrix<float, 3, 3> R;
    cv::cv2eigen(getRotation(), R);
    // construct corresponding Euler angles from rotation matrix representation
    // (yaw, pitch, roll)^T
    ea = R.eulerAngles(2, 1, 0);
    // convert unit from radian to degree
    ea *= 180.f / M_PI;
    return ea;
}

Eigen::Quaternion<float> CamPose::getRQuatEigen() const
{
    Eigen::Matrix<float, 3, 3> R;
    cv::cv2eigen(getRotation(), R);
    Eigen::Quaternionf q(R);
    return q;
}

Eigen::Quaternion<float> CamPose::getRInvQuatEigen() const
{
    Eigen::Matrix<float, 3, 3> RInv;
    cv::cv2eigen(getRotation().t(), RInv);
    Eigen::Quaternionf q(RInv);
    return q;
}

CamPose& CamPose::operator*=(const CamPose& rhs)
{
    Mat RcwL = getRotation();
    Mat tcwL = getTranslation();
    Mat RcwR = rhs.getRotation();
    Mat tcwR = rhs.getTranslation();
    // (4*4 matrix) T_L * T_R = [R_L*R_R, R_L*t_R + t_L; 0^T, 1]
    this->setPose(RcwL*RcwR, RcwL*tcwR + tcwL);
    return *this;
}

const CamPose CamPose::operator*(const CamPose& rhs) const
{
    CamPose ret = *this;
    ret *= rhs;
    return ret;
}

void CamPose::setRotation(const cv::Mat& Rcw)
{
    Rcw.copyTo(mTcw.rowRange(0, 3).colRange(0, 3));
}

void CamPose::setTranslation(const cv::Mat& tcw)
{
    tcw.copyTo(mTcw.rowRange(0, 3).col(3));
}

void CamPose::setPoseInv()
{
    Mat Rcw = getRotation();
    Mat tcw = getTranslation();
    Mat Rwc = Rcw.t();
    Mat twc = -Rwc*tcw;
    Rwc.copyTo(mTwc.rowRange(0, 3).colRange(0, 3));
    twc.copyTo(mTwc.rowRange(0, 3).col(3));
}

std::ostream& operator<<(std::ostream& os, const CamPose& pose)
{
    //os << "Pose Tcw = [Rcw | tcw] = " << endl << pose.getPose() << endl;
    Eigen::Vector3f ea = pose.getREulerAngleEigen();
    os << "Camera origin = " << pose.getCamOrigin().t() << "; ";
    os << "Rotation {yaw, pitch, roll} = {"
       << ea(0) << ", " << ea(1) << ", " << ea(2) << "} (deg); ";
    //os << "Translation tcw = " << pose.getTranslation().t();
    return os;
}

} // namespace SLAM_demo


// Copyright 2004, 2005 The Trustees of Indiana University.

// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//  Authors: Jeremiah Willcock
//           Douglas Gregor
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_ERDOS_RENYI_GENERATOR_HPP
#define BOOST_GRAPH_ERDOS_RENYI_GENERATOR_HPP

#include <cassert>
#include <iterator>
#include <utility>
#include <boost/shared_ptr.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/random/geometric_distribution.hpp>
#include <boost/type_traits/is_base_and_derived.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/config/no_tr1/cmath.hpp>

namespace boost {

  template<typename RandomGenerator, typename Graph>
  class erdos_renyi_iterator
  {
    typedef typename graph_traits<Graph>::directed_category directed_category;
    typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
    typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

    BOOST_STATIC_CONSTANT
      (bool,
       is_undirected = (is_base_and_derived<undirected_tag,
                                            directed_category>::value
                        || is_same<undirected_tag, directed_category>::value));

  public:
    typedef std::input_iterator_tag iterator_category;
    typedef std::pair<vertices_size_type, vertices_size_type> value_type;
    typedef const value_type& reference;
    typedef const value_type* pointer;
    typedef void difference_type;

    erdos_renyi_iterator() : gen(), n(0), edges(0), allow_self_loops(false) {}
    erdos_renyi_iterator(RandomGenerator& gen, vertices_size_type n,
                         double fraction = 0.0, bool allow_self_loops = false)
      : gen(&gen), n(n), edges(edges_size_type(fraction * n * n)),
        allow_self_loops(allow_self_loops)
    {
      if (is_undirected) edges = edges / 2;
      next();
    }

    erdos_renyi_iterator(RandomGenerator& gen, vertices_size_type n,
                         edges_size_type m, bool allow_self_loops = false)
      : gen(&gen), n(n), edges(m),
        allow_self_loops(allow_self_loops)
    {
      next();
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    erdos_renyi_iterator& operator++()
    {
      --edges;
      next();
      return *this;
    }

    erdos_renyi_iterator operator++(int)
    {
      erdos_renyi_iterator temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const erdos_renyi_iterator& other) const
    { return edges == other.edges; }

    bool operator!=(const erdos_renyi_iterator& other) const
    { return !(*this == other); }

  private:
    void next()
    {
      uniform_int<vertices_size_type> rand_vertex(0, n-1);
      current.first = rand_vertex(*gen);
      do {
        current.second = rand_vertex(*gen);
      } while (current.first == current.second && !allow_self_loops);
    }

    RandomGenerator* gen;
    vertices_size_type n;
    edges_size_type edges;
    bool allow_self_loops;
    value_type current;
  };

  template<typename RandomGenerator, typename Graph>
  class sorted_erdos_renyi_iterator
  {
    typedef typename graph_traits<Graph>::directed_category directed_category;
    typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
    typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

    BOOST_STATIC_CONSTANT
      (bool,
       is_undirected = (is_base_and_derived<undirected_tag,
                                            directed_category>::value
                        || is_same<undirected_tag, directed_category>::value));

  public:
    typedef std::input_iterator_tag iterator_category;
    typedef std::pair<vertices_size_type, vertices_size_type> value_type;
    typedef const value_type& reference;
    typedef const value_type* pointer;
    typedef void difference_type;

    sorted_erdos_renyi_iterator()
      : gen(), rand_vertex(0.5), n(0), allow_self_loops(false),
    src((std::numeric_limits<vertices_size_type>::max)()), tgt(0), prob(0) {}
    sorted_erdos_renyi_iterator(RandomGenerator& gen, vertices_size_type n,
                    double prob = 0.0,
                                bool allow_self_loops = false)
      : gen(),
        // The "1.0 - prob" in the next line is to work around a Boost.Random
        // (and TR1) bug in the specification of geometric_distribution.  It
        // should be replaced by "prob" when the issue is fixed.
        rand_vertex(1.0 - prob),
    n(n), allow_self_loops(allow_self_loops), src(0), tgt(0), prob(prob)
    {
      this->gen.reset(new uniform_01<RandomGenerator>(gen));

      if (prob == 0.0) {src = (std::numeric_limits<vertices_size_type>::max)(); return;}
      next();
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    sorted_erdos_renyi_iterator& operator++()
    {
      next();
      return *this;
    }

    sorted_erdos_renyi_iterator operator++(int)
    {
      sorted_erdos_renyi_iterator temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const sorted_erdos_renyi_iterator& other) const
    { return src == other.src && tgt == other.tgt; }

    bool operator!=(const sorted_erdos_renyi_iterator& other) const
    { return !(*this == other); }

  private:
    void next()
    {
      using std::sqrt;
      using std::floor;

      // In order to get the edges from the generator in sorted order, one
      // effective (but slow) procedure would be to use a
      // bernoulli_distribution for each legal (src, tgt) pair.  Because of the
      // O(n^2) cost of that, a geometric distribution is used.  The geometric
      // distribution tells how many times the bernoulli_distribution would
      // need to be run until it returns true.  Thus, this distribution can be
      // used to step through the edges which are actually present.  Everything
      // beyond "tgt += increment" is done to effectively convert linear
      // indexing (the partial sums of the geometric distribution output) into
      // graph edges.
      assert (src != (std::numeric_limits<vertices_size_type>::max)());
      vertices_size_type increment = rand_vertex(*gen);
      tgt += increment;
      if (is_undirected) {
    // Update src and tgt based on position of tgt
    // Basically, we want the greatest src_increment such that (in \bbQ):
    // src_increment * (src + allow_self_loops + src_increment - 1/2) <= tgt
    // The result of the LHS of this, evaluated with the computed
    // src_increment, is then subtracted from tgt
    double src_minus_half = (src + allow_self_loops) - 0.5;
    double disc = src_minus_half * src_minus_half + 2 * tgt;
    double src_increment_fp = floor(sqrt(disc) - src_minus_half);
    vertices_size_type src_increment = vertices_size_type(src_increment_fp);
    if (src + src_increment >= n) {
      src = n;
    } else {
      tgt -= (src + allow_self_loops) * src_increment +
         src_increment * (src_increment - 1) / 2;
      src += src_increment;
    }
      } else {
    // Number of out edge positions possible from each vertex in this graph
    vertices_size_type possible_out_edges = n - (allow_self_loops ? 0 : 1);
    src += (std::min)(n - src, tgt / possible_out_edges);
    tgt %= possible_out_edges;
      }
      // Set end of graph code so (src, tgt) will be the same as for the end
      // sorted_erdos_renyi_iterator
      if (src >= n) {src = (std::numeric_limits<vertices_size_type>::max)(); tgt = 0;}
      // Copy (src, tgt) into current
      current.first = src;
      current.second = tgt;
      // Adjust for (src, src) edge being forbidden
      if (!allow_self_loops && tgt >= src) ++current.second;
    }

    shared_ptr<uniform_01<RandomGenerator> > gen;
    geometric_distribution<vertices_size_type> rand_vertex;
    vertices_size_type n;
    bool allow_self_loops;
    vertices_size_type src, tgt;
    value_type current;
    double prob;
  };

} // end namespace boost

#endif // BOOST_GRAPH_ERDOS_RENYI_GENERATOR_HPP