/**
 * 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;
    }
}


/*=========================================================================
 *
 *  Copyright David Doria 2012 daviddoria@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.txt
 *
 *  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 QuadrantHistogramCompareAcceptanceVisitor_HPP
#define QuadrantHistogramCompareAcceptanceVisitor_HPP

#include <boost/graph/graph_traits.hpp>

// Parent class
#include "Visitors/AcceptanceVisitors/AcceptanceVisitorParent.h"

// Submodules
#include <Mask/Mask.h>
#include <Mask/MaskOperations.h>
#include <ITKHelpers/ITKHelpers.h>
#include "Utilities/Histogram/Histogram.h"

// ITK
#include "itkImage.h"
#include "itkImageRegion.h"

/**
  Accept a patch if the score of the histogram comparison of the unmasked region of a specified quadrant is
  less than a specified threshold.
 */
template <typename TGraph, typename TImage>
struct QuadrantHistogramCompareAcceptanceVisitor : public AcceptanceVisitorParent<TGraph>
{
  TImage* Image;
  Mask* MaskImage;

  const unsigned int HalfWidth;

  const unsigned int Quadrant;

  std::vector<float> Mins;
  std::vector<float> Maxs;

  float DifferenceThreshold;

  typedef typename boost::graph_traits<TGraph>::vertex_descriptor VertexDescriptorType;

  QuadrantHistogramCompareAcceptanceVisitor(TImage* const image, Mask* const mask, const unsigned int halfWidth,
                                            const unsigned int quadrant, const std::vector<float>& mins,
                                            const std::vector<float>& maxs, const float differenceThreshold = 100.0f) :
  Image(image), MaskImage(mask), HalfWidth(halfWidth), Quadrant(quadrant), Mins(mins), Maxs(maxs),
  DifferenceThreshold(differenceThreshold)
  {

  }

  bool AcceptMatch(VertexDescriptorType target, VertexDescriptorType source) const
  {
    float computedEnergy;
    return AcceptMatch(target, source, computedEnergy);
  }

  bool AcceptMatch(VertexDescriptorType target, VertexDescriptorType source, float& computedEnergy) const override
  {
    itk::Index<2> targetPixel = ITKHelpers::CreateIndex(target);
    itk::ImageRegion<2> targetRegion = ITKHelpers::GetRegionInRadiusAroundPixel(targetPixel, this->HalfWidth);

    itk::Index<2> sourcePixel = ITKHelpers::CreateIndex(source);
    itk::ImageRegion<2> sourceRegion = ITKHelpers::GetRegionInRadiusAroundPixel(sourcePixel, this->HalfWidth);

    itk::ImageRegion<2> targetRegionQuadrant = ITKHelpers::GetQuadrant(targetRegion, this->Quadrant);
    itk::ImageRegion<2> sourceRegionQuadrant = ITKHelpers::GetQuadrant(sourceRegion, this->Quadrant);

    std::vector<typename TImage::PixelType> validPixelsTargetRegion =
              MaskOperations::GetValidPixelsInRegion(this->Image, this->MaskImage, targetRegionQuadrant);

    // If less than 20 percent of the quadrant has valid pixels, the comparison is too specific
    // and the energy will often be erroneously high
    float quadrantCoverage = static_cast<float>(validPixelsTargetRegion.size()) /
                             static_cast<float>(targetRegionQuadrant.GetNumberOfPixels());
    if(quadrantCoverage < .2)
    {
      std::cout << "Skipping quadrant " << this->Quadrant << " (coverage " << quadrantCoverage << ")" << std::endl;
      computedEnergy = 0.0f;
      return true;
    }
    std::vector<itk::Offset<2> > validOffsets = this->MaskImage->GetValidOffsetsInRegion(targetRegionQuadrant);
    std::vector<itk::Index<2> > sourcePatchValidPixelIndices =
            ITKHelpers::OffsetsToIndices(validOffsets, sourceRegionQuadrant.GetIndex());
    std::vector<typename TImage::PixelType> validPixelsSourceRegion =
            ITKHelpers::GetPixelValues(Image, sourcePatchValidPixelIndices);

    assert(validPixelsSourceRegion.size() == validPixelsTargetRegion.size());

    unsigned int numberOfPixels = validPixelsTargetRegion.size();

//    std::cout << "There are " << numberOfPixels << " pixels." << std::endl;

    float totalHistogramDifference = 0.0f;
//     std::cout << "There are " << Image->GetNumberOfComponentsPerPixel() << " components in the image." << std::endl;
//     std::cout << "There are " << validPixelsTargetRegion[0].Size() << " components in the target pixels." << std::endl;
//     std::cout << "There are " << validPixelsSourceRegion[0].Size() << " components in the source pixels." << std::endl;

    for(unsigned int component = 0; component < Image->GetNumberOfComponentsPerPixel(); ++component)
    {
      // std::cout << "QuadrantHistogramCompareAcceptanceVisitor component " << component << "..." << std::endl;
      std::vector<float> targetValues(numberOfPixels);
      std::vector<float> sourceValues(numberOfPixels);

      for(unsigned int pixelId = 0; pixelId < numberOfPixels; ++pixelId)
      {
        targetValues[pixelId] = validPixelsTargetRegion[pixelId][component];
        sourceValues[pixelId] = validPixelsSourceRegion[pixelId][component];
      }

      typedef HistogramGenerator<float> HistogramGeneratorType;
      typedef HistogramGeneratorType::HistogramType HistogramType;
      unsigned int numberOfBins = 20;

      HistogramType targetHistogram = HistogramGeneratorType::ScalarHistogram(targetValues, numberOfBins,
                                                                              this->Mins[component], this->Maxs[component]);
      HistogramType sourceHistogram = HistogramGeneratorType::ScalarHistogram(sourceValues, numberOfBins,
                                                                              this->Mins[component], this->Maxs[component]);

//       std::cout << "targetHistogram size " << targetHistogram.size() << std::endl;
//       std::cout << "sourceHistogram size " << sourceHistogram.size() << std::endl;

      // We normalize the histograms because the magnitude of the histogram difference
      // should not change based on the number of pixels that were in the valid region of the patches.
      Helpers::NormalizeVector(targetHistogram);
      Helpers::NormalizeVector(sourceHistogram);

      float channelHistogramDifference = Helpers::VectorSumOfAbsoluteDifferences(targetHistogram, sourceHistogram);
      totalHistogramDifference += channelHistogramDifference;
    }

    // Compute the difference
    computedEnergy = totalHistogramDifference;
    std::cout << "QuadrantHistogramCompareAcceptanceVisitor Energy: " << computedEnergy << std::endl;

    if(computedEnergy < this->DifferenceThreshold)
      {
      std::cout << "QuadrantHistogramCompareAcceptanceVisitor: Match accepted (less than "
                << this->DifferenceThreshold << ")" << std::endl;
      return true;
      }
    else
      {
      std::cout << "QuadrantHistogramCompareAcceptanceVisitor: Match rejected (greater than "
                << this->DifferenceThreshold << ")" << std::endl;
      return false;
      }
  }

};

#endif


/*!
@file
Forward declares `boost::hana::Enumerable`.

@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_HANA_FWD_ENUMERABLE_HPP
#define BOOST_HANA_FWD_ENUMERABLE_HPP

#include <boost/hana/config.hpp>
#include <boost/hana/detail/std/forward.hpp>
#include <boost/hana/fwd/core/datatype.hpp>
#include <boost/hana/fwd/core/models.hpp>

namespace boost { namespace hana {
    //! @ingroup group-concepts
    //! Represents data types whose values can be enumerated.
    //!
    //! `Enumerable` provides the `succ` and `pred` methods, whose names
    //! come from the [successor][1] and predecessor functions used when
    //! defining the natural numbers with the Peano axioms. Those functions
    //! allow the values of a data type to be enumerated. Note that an
    //! `Enumerable` data type `E` may contain infinitely many values;
    //! `pred` and `succ` can be implemented as long as the infinity is
    //! [countable][2] (in its mathematical sense).
    //!
    //!
    //! Laws
    //! ----
    //! For any `Enumerable x`, the following laws must be satisfied:
    //! @code
    //!     succ(pred(x)) == x
    //!     pred(succ(x)) == x
    //! @endcode
    //!
    //!
    //! Minimal complete definition
    //! ---------------------------
    //! `succ` and `pred` satisfying the above laws
    //!
    //!
    //! Provided models
    //! ---------------
    //! 1. For non-boolean arithmetic data types\n
    //! A data type `T` is arithmetic if `std::is_arithmetic<T>::%value` is
    //! true. For a non-boolean arithmetic data type `T`, a model of
    //! `Enumerable` is automatically defined by setting
    //! @code
    //!     succ(x) = ++x
    //!     pred(x) = --x
    //! @endcode
    //!
    //!
    //! [1]: http://en.wikipedia.org/wiki/Successor_function
    //! [2]: http://en.wikipedia.org/wiki/Countable_set
    struct Enumerable { };

    //! Returns the successor of a value.
    //! @relates Enumerable
    //!
    //!
    //! Example
    //! -------
    //! @snippet example/enumerable.cpp succ
#ifdef BOOST_HANA_DOXYGEN_INVOKED
    constexpr auto succ = [](auto&& num) -> decltype(auto) {
        return tag-dispatched;
    };
#else
    template <typename E, typename = void>
    struct succ_impl;

    struct _succ {
        template <typename E>
        constexpr decltype(auto) operator()(E&& num) const {
#ifdef BOOST_HANA_CONFIG_CHECK_DATA_TYPES
        static_assert(_models<Enumerable, typename datatype<E>::type>{},
        "hana::succ(n) requires n to be Enumerable");
#endif

            return succ_impl<
                typename datatype<E>::type
            >::apply(detail::std::forward<E>(num));
        }
    };

    constexpr _succ succ{};
#endif

    //! Returns the predecessor of a value.
    //! @relates Enumerable
    //!
    //!
    //! Example
    //! -------
    //! @snippet example/enumerable.cpp pred
#ifdef BOOST_HANA_DOXYGEN_INVOKED
    constexpr auto pred = [](auto&& num) -> decltype(auto) {
        return tag-dispatched;
    };
#else
    template <typename E, typename = void>
    struct pred_impl;

    struct _pred {
        template <typename E>
        constexpr decltype(auto) operator()(E&& num) const {
#ifdef BOOST_HANA_CONFIG_CHECK_DATA_TYPES
        static_assert(_models<Enumerable, typename datatype<E>::type>{},
        "hana::pred(n) requires n to be Enumerable");
#endif

            return pred_impl<
                typename datatype<E>::type
            >::apply(detail::std::forward<E>(num));
        }
    };

    constexpr _pred pred{};
#endif
}} // end namespace boost::hana

#endif // !BOOST_HANA_FWD_ENUMERABLE_HPP


/* (c) Copyright 2012 Felipe Magno de Almeida
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef MORBID_PRIMITIVE_TYPES_H
#define MORBID_PRIMITIVE_TYPES_H

#include <boost/integer.hpp>

namespace morbid {

typedef bool Boolean;
typedef boost::uint_t<16u>::least UShort;
typedef boost::int_t<16u>::least Short;
typedef boost::uint_t<32u>::least ULong;
typedef boost::int_t<32u>::least Long;
typedef boost::uint_t<64u>::least ULongLong;
typedef boost::int_t<64u>::least LongLong;
typedef double Double;
typedef float Float;
typedef char Char;
typedef wchar_t WChar;
typedef unsigned char Octet;

}

#endif


#include <Eigen/Dense>

// Problem to solve


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> test_float_D_D(
    const Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic>& M) {
    return M;
}
void export_float_D_D() { boost::python::def("test_float_D_D", test_float_D_D); }


#include "biggles/detail/random.hpp"
#include "biggles/mh_moves/mh_moves.hpp"
#include "biggles/simulate.hpp"
#include "biggles/tools/sundries.hpp"
#include <boost/foreach.hpp>
#include <iomanip>
#include <iostream>
#include <fstream>
#include <string>

// include this last to stop pre-processor macros breaking things
extern "C" {
#include <ccan/tap/tap.h>
}

using namespace biggles;

int main(int argc, char** argv)
{
    // seed the PRNG to ensure consistent results across runs.
    biggles::detail::seed_prng(0xfacedead);

    plan_tests(4 + 28);

    float sigma_R = 0.1f;

    // initialise model parameters
    model::parameters params;
    model::mean_new_tracks_per_frame(params)           = 1.5f;
    model::mean_false_observations_per_frame(params)   = 2.f;
    model::frame_to_frame_survival_probability(params) = 0.95f;
    model::generate_observation_probability(params)    = 0.8f;
    model::observation_error_covariance(params)       << sigma_R * sigma_R, 0, 0, sigma_R * sigma_R;

    // simulate full data
    partition_ptr_t partition_ptr;
    partition_ptr_t target_ptr;
    simulate::generate_partition(partition_ptr, params, 0, 128 + 128, 128.f, 128.f);

    diag("Generated %zu new tracks.", partition_ptr->tracks().size());
    diag("Generated %zu clutter observations.", partition_ptr->clutter().size());

    // crop partition to a specific region
    simulate::crop_partition(partition_ptr, partition_ptr, 0.f, 128.f, 0.f, 128.f, 64, 64 + 128);

    diag("Cropped to %zu tracks.", partition_ptr->tracks().size());
    diag("Cropped to %zu clutter observations.", partition_ptr->clutter().size());

    // create a synthetic dataset from the partition
    simulate::demote_all_tracks_from_partition(partition_ptr, partition_ptr);

    // check we have some data and no tracks
    ok1(partition_ptr->clutter().size() > 0);
    ok1(partition_ptr->tracks().size() == 0);

    diag("Final data set consists of %zu observations.", partition_ptr->clutter().size());
    size_t input_obs_count(partition_ptr->clutter().size());

    // try some birth moves
    int n_succeeded(0), n_tried(0);
    for(int birth_idx = 0; birth_idx < 300; ++birth_idx)
    {
        float log_pdf(1337.f);
        ++n_tried;
        if(mh_moves::birth(partition_ptr, partition_ptr, log_pdf))
        {
            ++n_succeeded;
            diag("birth move succeeded with log( P(backward) / P(forward) ): %f", log_pdf);
        }
    }
    diag("%i birth move(s) succeeded out of %i attempt(s) (~%i%%)",
         n_succeeded, n_tried, (100 * n_succeeded) / n_tried);
    ok(n_succeeded >= 2, "at least two birth moves succeeded");

    // check that total number of observations hasn't changed
    size_t new_obs_count(partition_ptr->clutter().size());
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_p, partition_ptr->tracks())
    {
        new_obs_count += t_p->size();
    }
    ok(new_obs_count == input_obs_count, "birth moves did not change overall observation count");

    // try some split moves
    //size_t n_to_try = std::min(2 * partition_ptr->tracks().size(), size_t(200));
    size_t n_to_try = 50;
    ok1(n_to_try > 0);

    size_t pre_split_track_count(partition_ptr->tracks().size());
    ok1(pre_split_track_count > 0);

    n_succeeded = 0;
    n_tried = 0;
    for(size_t split_idx = 0; split_idx < n_to_try; ++split_idx)
    {
        float log_pdf(1337.f);
        ++n_tried;
        if(mh_moves::split(partition_ptr, partition_ptr, log_pdf))
        {
            ++n_succeeded;
            diag("split move succeeded with log( P(backward) / P(forward) ): %f", log_pdf);
        }
    }
    diag("%i split move(s) succeeded out of %i attempt(s) (~%i%%)",
         n_succeeded, n_tried, (100 * n_succeeded) / n_tried);
    ok(n_succeeded >= 2, "at least two split moves succeeded");

    ok(partition_ptr->tracks().size() == pre_split_track_count + n_succeeded,
       "each split move added one track");

    // try some death moves
    //n_to_try = std::min(partition_ptr->tracks().size() >> 1, size_t(200));
    ok1(n_to_try > 0);

    size_t pre_death_track_count(partition_ptr->tracks().size());
    ok1(pre_death_track_count > 0);

    n_succeeded = 0;
    n_tried = 0;
    for(size_t death_idx = 0; death_idx < n_to_try; ++death_idx)
    {
        float log_pdf(1337.f);
        ++n_tried;
        if(mh_moves::death(partition_ptr, partition_ptr, log_pdf))
        {
            ++n_succeeded;
            diag("death move succeeded with log( P(backward) / P(forward) ): %f", log_pdf);
        }
    }
    diag("%i death move(s) succeeded out of %i attempt(s) (~%i%%)",
         n_succeeded, n_tried, (100 * n_succeeded) / n_tried);
    ok(n_succeeded >= 2, "at least two death moves succeeded");

    ok(partition_ptr->tracks().size() == pre_death_track_count - n_succeeded,
       "each death move removed one track");

    // check that total number of observations hasn't changed
    new_obs_count = partition_ptr->clutter().size();
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_p, partition_ptr->tracks())
    {
        new_obs_count += t_p->size();
    }
    ok(new_obs_count == input_obs_count, "split moves did not change overall observation count");

    // try some merge moves
    //n_to_try = std::min(partition_ptr->tracks().size() >> 1, size_t(200));
    ok1(n_to_try > 0);

    size_t pre_merge_track_count(partition_ptr->tracks().size());
    ok1(pre_merge_track_count > 0);

    n_succeeded = 0;
    n_tried = 0;
    for(size_t merge_idx = 0; merge_idx < n_to_try; ++merge_idx)
    {
        float log_pdf(1337.f);
        ++n_tried;
        if(mh_moves::merge(partition_ptr, partition_ptr, log_pdf))
        {
            ++n_succeeded;
            diag("merge move succeeded with log( P(backward) / P(forward) ): %f", log_pdf);
        }
    }
    diag("%i merge move(s) succeeded out of %i attempt(s) (~%i%%)",
         n_succeeded, n_tried, (100 * n_succeeded) / n_tried);
    ok(n_succeeded >= 2, "at least two merge moves succeeded");

    ok(partition_ptr->tracks().size() == pre_merge_track_count - n_succeeded,
       "each merge move removed one track");

    // check that total number of observations hasn't changed
    new_obs_count = partition_ptr->clutter().size();
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_p, partition_ptr->tracks())
    {
        new_obs_count += t_p->size();
    }
    ok(new_obs_count == input_obs_count, "merge moves did not change overall observation count");

    // try some extend moves
    //n_to_try = 100;

    size_t pre_extend_track_count = partition_ptr->tracks().size();
    size_t pre_extend_clutter_count = partition_ptr->clutter().size();
    ok1(pre_extend_track_count > 0);

    n_succeeded = 0;
    n_tried = 0;
    for(size_t extend_idx = 0; extend_idx < n_to_try; ++extend_idx)
    {
        float log_pdf(1337.f);
        ++n_tried;
        if(mh_moves::extend(partition_ptr, partition_ptr, log_pdf))
        {
            ++n_succeeded;
            diag("extend move succeeded with log( P(backward) / P(forward) ): %f", log_pdf);
        }
    }
    diag("%i extend move(s) succeeded out of %i attempt(s) (~%i%%)",
         n_succeeded, n_tried, (100 * n_succeeded) / n_tried);
    ok(n_succeeded >= 2, "at least two extend moves succeeded");

    ok(partition_ptr->tracks().size() == pre_extend_track_count, "extend moves retained track count");

    // check that total number of observations hasn't changed
    new_obs_count = partition_ptr->clutter().size();
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_p, partition_ptr->tracks())
    {
        new_obs_count += t_p->size();
    }
    ok(new_obs_count == input_obs_count, "extend moves did not change overall observation count");

    ok(partition_ptr->clutter().size() <= pre_extend_clutter_count - n_succeeded,
       "each extend move updated the clutter by at least one");

    // try some reduce moves
    //n_to_try = 100;

    size_t pre_reduce_track_count = partition_ptr->tracks().size();
    size_t pre_reduce_clutter_count = partition_ptr->clutter().size();
    ok1(pre_reduce_track_count > 0);

    n_succeeded = 0;
    n_tried = 0;
    partition_ptr_t proposed_part_ptr;
    for(size_t reduce_idx = 0; reduce_idx < n_to_try; ++reduce_idx)
    {
        float log_pdf(1337.f);
        ++n_tried;
        if(mh_moves::reduce(partition_ptr, proposed_part_ptr, log_pdf))
        {
            ++n_succeeded;
            diag("reduce move succeeded with log( P(backward) / P(forward) ): %f", log_pdf);
            partition_ptr = proposed_part_ptr;
        }
    }
    diag("%i reduce move(s) succeeded out of %i attempt(s) (~%i%%)",
         n_succeeded, n_tried, (100 * n_succeeded) / n_tried);
    ok(n_succeeded >= 2, "at least two reduce moves succeeded");

    ok(partition_ptr->tracks().size() == pre_reduce_track_count, "reduce moves retained track count");

    // check that total number of observations hasn't changed
    new_obs_count = partition_ptr->clutter().size();
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_p, partition_ptr->tracks())
    {
        new_obs_count += t_p->size();
    }
    ok(new_obs_count == input_obs_count, "reduce moves did not change overall observation count");

    ok(partition_ptr->clutter().size() >= pre_reduce_clutter_count + n_succeeded,
       "each reduce move increased the clutter by at least one");

    // try some update moves
    //n_to_try = 100;

    size_t pre_update_track_count = partition_ptr->tracks().size();
    ok1(pre_update_track_count > 0);

    n_succeeded = 0;
    n_tried = 0;
    for(size_t update_move_idx = 0; update_move_idx < n_to_try; ++update_move_idx)
    {
        float log_pdf(1337.f);
        ++n_tried;
        if(mh_moves::update(partition_ptr, partition_ptr, log_pdf))
        {
            ++n_succeeded;
        }
    }
    diag("%i update move(s) succeeded out of %i attempt(s) (~%i%%)",
         n_succeeded, n_tried, (100 * n_succeeded) / n_tried);
    ok(n_succeeded >= 2, "at least two update moves succeeded");

    ok(partition_ptr->tracks().size() == pre_update_track_count, "update moves retained track count");

    // check that total number of observations hasn't changed
    new_obs_count = partition_ptr->clutter().size();
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_p, partition_ptr->tracks())
    {
        new_obs_count += t_p->size();
    }
    ok(new_obs_count == input_obs_count, "update moves did not change overall observation count");

    diag("Using propose() to perform one move of each type.");
    float ratio(0.f);
    //mh_moves::propose(mh_moves::NONE, partition_ptr, partition_ptr, ratio);

    for (int i = 1; i != int(mh_moves::IDENTITY); ++i) {
        mh_moves::move_type mv = mh_moves::move_type(i);
        diag("Move type = %s", mh_moves::move_name(mv).c_str());
        mh_moves::propose(mv, partition_ptr, proposed_part_ptr, ratio);
    }
    /*
    mh_moves::propose(mh_moves::BIRTH, partition_ptr, partition_ptr, ratio);
    mh_moves::propose(mh_moves::DEATH, partition_ptr, partition_ptr, ratio);
    mh_moves::propose(mh_moves::EXTEND, partition_ptr, partition_ptr, ratio);
    mh_moves::propose(mh_moves::REDUCE, partition_ptr, partition_ptr, ratio);
    mh_moves::propose(mh_moves::SPLIT, partition_ptr, partition_ptr, ratio);
    mh_moves::propose(mh_moves::MERGE, partition_ptr, partition_ptr, ratio);
    mh_moves::propose(mh_moves::UPDATE, partition_ptr, partition_ptr, ratio);
    */

    return exit_status();
}


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/functional/curry.hpp>
namespace hana = boost::hana;

int main() {
    BOOST_HANA_CONSTEXPR_LAMBDA auto add = [](auto x, auto y, auto z) {
        return x + y + z;
    };

    BOOST_HANA_CONSTEXPR_CHECK(hana::curry<3>(add)(1)(2)(3) == 1 + 2 + 3);
    BOOST_HANA_CONSTEXPR_CHECK(hana::curry<3>(add)(1)(2, 3) == hana::curry<3>(add)(1)(2)(3));
    BOOST_HANA_CONSTEXPR_CHECK(hana::curry<3>(add)(1, 2, 3) == hana::curry<3>(add)(1)(2)(3));

    // curry with a nullary function
    BOOST_HANA_CONSTEXPR_LAMBDA auto two = []() {
        return 2;
    };

    BOOST_HANA_CONSTEXPR_CHECK(hana::curry<0>(two)() == two());
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, Eigen::Dynamic, 2> test_int_D_2(const Eigen::Matrix<int, Eigen::Dynamic, 2>& M) { return M; }
void export_int_D_2() { boost::python::def("test_int_D_2", test_int_D_2); }


#include <string>
#include <algorithm>
#include <vector>
#include <cctype>
#include <list>
#include <cstdlib>
#include "DFA.h"
#include "DFAenumerator.h"
#include "DFAexhaustenumAux_0.h"
#include "DFAreisenumAux_0.h"
#include "DFAreisenum_0.h"
#include "DFArenderer.h"
#include "Inf_ExistsPattern.h"
#include "InformantEnum.h"
#include "Even1s_infenum.h"
#include "Inf_Random.h"
#include "RawInformant.h"
#include "ExhaustiveScientist.h"
#include <fstream>
#include <ostream>
#include <sstream>
#include "gvc.h"
#include "SDL.h"
#include "SDL_image.h"
#include "SDL_thread.h"
#include "Inf_RepeatingPattern.h"
#include <boost/algorithm/string.hpp>
#include "misc.h"

enum ProgramStates
{
	START,
	ENUM
};

using namespace std;

std::string input;
bool hasinput = false;

int thread(void*a) {
	getline(std::cin, input);
	hasinput = true;
	cout << "\n";
	return 0;
}

void clean_result(DFA*& res, DFArenderer*& resrend)
{
	if (res)
	{
		delete res;
		res = NULL;
	}
	if (resrend)
	{
		delete resrend;
		resrend = NULL;
	}
}

void close_window(SDL_Window* gWindow, SDL_Renderer* gRenderer, bool& openWindow)
{
	openWindow = false;
	if (gRenderer)
	{
		SDL_DestroyRenderer(gRenderer);
		gRenderer = NULL;
	}
	if (gWindow)
	{
		SDL_DestroyWindow(gWindow);
		gWindow = NULL;
	}
}

int max_states = 7;
int symbol_nr = 2;
int timeout = 25;

bool running = true;
bool openWindow = false;
bool updateGraph = false;

SDL_Window* gWindow = NULL;
SDL_Renderer* gRenderer = NULL;
SDL_Texture* gTexture = NULL;
SDL_Surface* loadedSurface = NULL;

std::string* loaded_file = NULL;

SDL_Rect topLeftViewport;

SDL_Event e;

GVC_t *gvc = NULL;

DFAenumerator_0* en = new DFAreisenum_0(max_states, symbol_nr);
InformantEnum* inf = NULL;

DFA* res = NULL;
DFArenderer* resrend = NULL;

int conjecture()
{
	if (!inf)
	{
		cout << "Please provide an informant! Type 'informant' for more information. \n \n \n";
		return 0;
	}
	else if (!en)
	{
		cout << "Please provide a DFA enumeration! Type 'enum' for more information. \n \n \n";
		return 0;
	}
	else
	{
		Uint32 ti = SDL_GetTicks();

		inf->reset();
		en->reset();

		DFA_0 guess = en->getCur();
		bool changed = false;
		int total_tested = 0;
		int max_words = inf->max() < 0 ? timeout : min(timeout, inf->max());
		while (!en->isLast())
		{
			++total_tested;
			int f = guess.compatible(inf, max_words);
			if (f > 0)
			{
				if (res)
					delete res;
				res = new DFA(guess.getTransitions(), f);
				changed = true;
				break;
			}
			inf->reset();
			en->next();
			guess = en->getCur();
		}
		Uint32 tf = SDL_GetTicks();

		Uint32 tms = tf - ti;

		Uint32 ms = tms % 1000;
		Uint32 tsec = (tms - ms) / 1000;

		Uint32 sec = tsec % 60;
		Uint32 min = (tsec - sec) / 60;

		if (changed)
		{
			cout << "SUCCESS \n";
			cout << "Type \"dfa draw\" to open a graph of the DFA. \n \n";
		}
		else
		{
			cout << "No automata found. Try increasing number of states.  \n \n";
		}
		cout << "Tested a total of " << total_tested << " DFAs.\n";
		cout << "Ran for " << min << ":" << sec << ":" << ms << "\n \n \n";

		if (resrend)
		{
			delete resrend;
			resrend = NULL;
		}

		updateGraph = true;
		return 1;
	}
}

int load_new_file()
{
	if (loaded_file)
	{
		ifstream load(*loaded_file);

		if (!load.is_open())
		{
			cout << "Error opening file. Check filename and whether it exists. \n\n\n";
			return 0;
		}
		else
		{
			bool bad_file = false;
			string buffer;
			vector<string> load_split;
			vector<int> acc;
			vector<vector<int>> wlist;

			while (getline(load, buffer))
			{
				boost::split(load_split, buffer, [](char c) {return isspace(c); }, boost::token_compress_on);
				//load_split.erase(std::remove_if(load_split.begin(), load_split.end(),
				//	[](std::string const& s) { return s.size() == 0; }),
				//	load_split.end());

				if (load_split.size() == 2)
				{
					if (load_split[0] == "0")
					{
						acc.push_back(0);
					}
					else if (load_split[0] == "1")
					{
						acc.push_back(1);
					}
					else
					{
						bad_file = true;
					}
					if (!bad_file)
					{
						vector<int> w;

						for (string::iterator it = load_split[1].begin(); it != load_split[1].end(); it++)
						{
							if (*it == '0')
							{
								w.push_back(0);
							}
							else if (*it == '1')
							{
								w.push_back(1);
							}
							else
							{
								bad_file = true;
								break;
							}
						}
						if (!bad_file)
							wlist.push_back(w);
					}
				}
			}
			if (bad_file)
			{
				cout << "Invalid file structure. \n\n\n";
				return 0;
			}
			else
			{
				if (inf)
					delete inf;

				clean_result(res, resrend);
				close_window(gWindow, gRenderer, openWindow);

				inf = new RawInformant(acc, wlist);
				cout << "Informant loaded successfully. \n";
				return 1;
			}
		}
	}
	else
	{
		std::cout << "No file specified.";
		return 0;
	}
}

int rendergraph()
{
	if (res)
	{
		if (openWindow)
		{
			close_window(gWindow, gRenderer, openWindow);
		}

		if (updateGraph)
		{
			if (!resrend)
			{
				resrend = new DFArenderer(*res, gvc);
			}
			resrend->render("temp.png", gvc);

			if (loadedSurface)
			{
				SDL_FreeSurface(loadedSurface);
			}
			loadedSurface = IMG_Load("temp.png");
			updateGraph = false;
		}

		SDL_DisplayMode DM;
		SDL_GetCurrentDisplayMode(0, &DM);

		float sw = 1;
		float sh = 1;
		float smin = 1;

		if (loadedSurface->w > DM.w * 0.85)
			sw = (((float)DM.w) / loadedSurface->w) * 0.85;

		if (loadedSurface->h > DM.h * 0.85)
			sh = (((float)DM.h) / loadedSurface->h) * 0.85;

		smin = sw > sh ? sh : sw;

		topLeftViewport.x = 0;
		topLeftViewport.y = 0;
		topLeftViewport.w = loadedSurface->w * smin;
		topLeftViewport.h = loadedSurface->h * smin;

		gWindow = SDL_CreateWindow("", DM.w / 4, DM.h / 4,
			loadedSurface->w * smin, loadedSurface->h * smin, SDL_WINDOW_SHOWN);
		gRenderer = SDL_CreateRenderer(gWindow, -1, SDL_RENDERER_ACCELERATED);
		SDL_SetRenderDrawColor(gRenderer, 0xFF, 0xFF, 0xFF, 0xFF);
		SDL_RenderClear(gRenderer);

		gTexture = SDL_CreateTextureFromSurface(gRenderer, loadedSurface);

		openWindow = true;

		cout << "Graph successfully rendered.\n\n\n";
		return 1;
	}
	else
	{
		cout << "A DFA is yet to be conjectured! \n \n \n";
		return 0;
	}
}

int main(int argc, char* argv[])
{
	if (SDL_Init(SDL_INIT_VIDEO) < 0)
	{
		printf("SDL could not initialize! SDL_Error: %s\n", SDL_GetError());
	}

	int imgFlags = IMG_INIT_PNG;
	if (!(IMG_Init(imgFlags) & imgFlags))
	{
	cout << "SDL_image could not initialize! SDL_image Error:" << IMG_GetError() << "\n";
	}

	cout <<
		R"(===============================================================================
				GOLD v. 0.1
===============================================================================
Type 'about' for a short description of the application.
Type 'help' for a list of commands
The source code can be found at https://github.com/gamatos/gold
Please submit bug reports to gabriel.matos@tecnico.ulisboa.pt or use github
-------------------------------------------------------------------------------)" "\n \n";

	gvc = gvContext();

	SDL_CreateThread(thread, "InputThread", NULL);

	while (running)
	{
		//Handle events on queue
		while (SDL_PollEvent(&e) != 0)
		{
			//User requests quit
			if (e.type == SDL_WINDOWEVENT)
			{
				switch (e.window.event)
				{
				case SDL_WINDOWEVENT_CLOSE:
					openWindow = false;
					if (gRenderer)
					{
						SDL_DestroyRenderer(gRenderer);
						gRenderer = NULL;
					}
					if (gWindow)
					{
						SDL_DestroyWindow(gWindow);
						gWindow = NULL;
					}
					break;
				}
			}
		}

		// process input HERE
		if (hasinput)
		{
			std::vector<std::string> results;
			boost::split(results, input, [](char c) {return c == ' '; });
			/*results.erase(std::remove_if(results.begin(), results.end(),
				[](std::string const& s) { return s.size() == 0; }),
				results.end());*/
			bool valid = false;

			if (!results.empty())
			{
				if (results[0] == "about" && results.size() == 1)
				{
					cout << R"(This application illustrates the concept of a scientist as introduced by E. Mark Gold in his 1967 seminal paper "Language identification in the limit".
It attempts to identify a regular language accepting/rejecting a given finite set of data and return a deterministic finite-state automata (DFA) associated with it.
It works by exhaustively searching all deterministic finite-state automata up to a certain number of states until it finds a suitable one.
Type 'libraries' for the list of libraries used)" "\n \n \n";
					valid = true;

				}
				if (results[0] == "libraries" && results.size() == 1)
				{
					cout << R"(SDL2 - https://www.libsdl.org/
graphviz - http://www.graphviz.org/
boost - http://www.boost.org/)" "\n \n \n";
					valid = true;

				}
				else if (results[0] == "help" && results.size() == 1)
				{
					cout << R"(
informant - shows currently selected and available informants
informant 'x' - changes informant to 'x'
print - Prints current informant data to console
identify - identify DFA associated with selected informant
dfa draw - draws last identified DFA
dfa save 'filename' - saves DFA structure in the graphviz format to the designated file
dfa savegraph 'filename' - saves DFA graph to the designated file
informant save 'filename' - saves data from current informant to the designated file, up to max_words elements
informant load 'filename' - creates informant from data in designated file
update - reloads loaded file then attempts to identify an automaton and draw its graph
enum - shows currently selected and available DFA enumerations
enum 'x' - changes enumeration to 'x'
max_words 'n' - changes maximum number of tested words to 'n'
max_states 'n' - changes maximum number of tested DFA states to 'n'
)" "\n \n \n";

					valid = true;
				}
				else if (results[0] == "enum")
				{
					if (results.size() == 1)
					{
						if (en)
						{
							cout << "Current enumeration is " << en->name() << ".\n\n";
						}
						else
						{
							cout << "There currently is no enumeration set.\n\n";
						}

						cout << R"(AVAILABLE ENUMERATIONS:
naive - naive enumeration
reis - enumeration by Rogerio Reis

Type "enum 'x'" to set the current informant to 'x'. You can also load the data from a file by typing "informant load 'filename'")" "\n\n\n";

						valid = true;
					}
					if (results.size() == 2)
					{
						bool validenum = false;
						if (results[1] == "naive")
						{
							if (en)
								delete en;
							en = new DFAexhaustenumAux_0(max_states);
							cout << "Enumeration successfully set to naive \n";
							cout << "Warning: Selected enumeration is inefficient. Use a small number of maximum states.\n\n\n";
							validenum = true;
						}
						else if (results[1] == "reis")
						{
							if (en)
								delete en;

							en = new DFAreisenum_0(max_states, symbol_nr);
							cout << "Enumeration successfully set to reis \n\n\n";
							validenum = true;
						}

						if (validenum)
						{
							en->setMaxStates(max_states);
							clean_result(res, resrend);
							close_window(gWindow, gRenderer, openWindow);
						}
						else
						{
							cout << "Requested enumeration not recognized. \n \n \n";
						}
						valid = true;
					}
				}
				else if (results[0] == "informant")
				{
					bool validinf = false;
					if (results.size() == 1)
					{
						if (inf)
						{
							cout << "Current informant is " << inf->name() << ".\n\n";
						}
						else
						{
							cout << "There currently is no informant set.\n\n";
						}

						cout << R"(AVAILABLE INFORMANTS:
even1s - informant for language that have even number of ones
random - informant based on a randomly generated sequence of 32-bit words
ex_pattern 'p' - informant for language that accepts words containing p
rep_pattern 'p' - informant for language that accepts words constructed by repeating sequence p

Type "informant 'x'" to set the current informant to 'x'. You can also load the data from a file by typing "informant 'filename'")" "\n\n\n";
						valid = true;
					}

					else if (results.size() == 2)
					{
						if (results[1] == "even1s")
						{
							if (loaded_file)
							{
								delete loaded_file;
								loaded_file = NULL;
							}

							if (inf)
								delete inf;

							inf = new Even1s_infenum;
							validinf = true;
							cout << "Informant even1s set. \n \n \n";
						}
						else if (results[1] == "random")
						{
							if (loaded_file)
							{
								delete loaded_file;
								loaded_file = NULL;
							}
							if (inf)
								delete inf;

							inf = new Inf_Random;
							validinf = true;
							cout << "Informant random set. \n \n \n";
						}

						else
						{
							cout << "Requested informant not recognized. \n \n \n";
						}
						valid = true;
					}
					else if (results.size() == 3)
					{
						if (results[1] == "rep_pattern")
						{
							if (loaded_file)
							{
								delete loaded_file;
								loaded_file = NULL;
							}
							int param = strtointbin(results[2]);
							if (param < 0)
							{
								cout << "Invalid pattern. \n \n \n";
							}
							else
							{
								if (inf)
								delete inf;

								inf = new Inf_RepeatingPattern(param, results[2].size());
								validinf = true;
								cout << "Informant rep_pattern set. \n";
							}
							valid = true;
						}
						if (results[1] == "ex_pattern")
						{
							if (loaded_file)
							{
								delete loaded_file;
								loaded_file = NULL;
							}
							int param = strtointbin(results[2]);
							if (param < 0)
							{
								cout << "Invalid pattern. \n \n \n";
							}
							else
							{
								if (inf)
									delete inf;

								inf = new Inf_ExistsPattern(param, results[2].size());
								validinf = true;
								cout << "Informant ex_pattern set. \n";
							}
							valid = true;
						}
						if (results[1] == "load")
						{
							if (loaded_file)
							{
								delete loaded_file;
								loaded_file = NULL;
							}
							loaded_file = new std::string(results[2]);
							if (load_new_file())
							{
								validinf = true;
							}
							valid = true;
						}
						else if (results[1] == "save")
						{
							if (inf)
							{
								ofstream save(results[2]);
								if (!save.is_open())
								{
									cout << "Error opening file. Check filename and whether it exists. \n\n\n";
								}
								else
								{
									inf->print(save, timeout);
									cout << "Informant successfully saved to " << results[2] << ".\n";
								}
							}
							else
								cout << "No informant is currently active!\n\n\n";
							valid = true;
						}
					}
					if (validinf)
					{
						if (inf->max() > 0)
						{
							timeout = inf->max();
							cout << "Maximum number of words set to " << inf->max() << "\n\n\n";
						}
						clean_result(res, resrend);
						close_window(gWindow, gRenderer, openWindow);
					}
				}
				else if (results[0] == "max_states")
				{
					if (results.size() == 1)
					{
						cout << "Current maximum number of states is set to " << max_states << ". \n\n\n";
					}
					if (results.size() == 2)
					{
						try {
							max_states = stoi(results[1]);
							cout << "Maximum number of states successfully set to " << max_states << ". \n\n\n";
							if (en)
							{
								en->setMaxStates(max_states);
							}
						}
						catch (std::invalid_argument& e) {
							cout << "Invalid argument. \n \n \n";
						}
						catch (std::out_of_range& e) {
							cout << "Requested number out of integer range. \n \n \n";
						}
						catch (...) {
							cout << "Unable to set maximum states. \n \n \n";
						}
					}

					valid = true;
				}
				else if (results[0] == "max_words")
				{
					if (results.size() == 1)
					{
						cout << "Current maximum test words is set to " << timeout << ". \n\n\n";
					}
					if (results.size() == 2)
					{
						try
						{
							timeout = stoi(results[1]);
							close_window(gWindow, gRenderer, openWindow);
							clean_result(res, resrend);
							if (inf && timeout < inf->max())
							{
								cout << "Warning: Current max. nr. of words truncates informant, which has " << inf->max() << " words. \n";
							}
							cout << "Max. test words set to " << timeout << ". \n\n\n";
						}
						catch (std::invalid_argument& e) {
							cout << "Invalid argument. \n \n \n";
						}
						catch (std::out_of_range& e) {
							cout << "Requested number out of integer range. \n \n \n";
						}
						catch (...) {
							cout << "Unable to set max. test words. \n \n \n";
						}
					}

					valid = true;
				}
				else if (results[0] == "identify" && results.size() == 1)
				{
					conjecture();
					valid = true;
				}
				else if (results[0] == "update" && results.size() == 1)
				{
					if (loaded_file)
					{
						if (load_new_file())
						{
							timeout = inf->max();
							cout << "Maximum number of words set to " << inf->max() << "\n\n\n";
							if (conjecture())
							{
								rendergraph();
							}
						}
					}
					else
					{
						cout << "Currently there is no informant supplied from a file! \n\n\n";
					}
					valid = true;
				}
				else if (results[0] == "print" && results.size() == 1)
				{
					if (inf)
					{
						inf->print(cout, timeout);
						cout << "\n \n";
					}
					else
					{
						cout << "Please provide an informant! Type 'informant' for more information. \n \n \n";
					}
					valid = true;
				}
				else if (results[0] == "dfa")
				{
					if (results.size() == 2)
					{
						if (results[1] == "draw")
						{
							rendergraph();

							valid = true;
						}
					}
					if (results.size() == 3)
					{
						if (results[1] == "savegraph")
						{
							valid = true;
							if (res)
							{
								if (!resrend)
								{
									resrend = new DFArenderer(*res, gvc);
								}

								resrend->render(results[2], gvc);
								cout << "DFA graph successfully saved to file " << results[2] << ". \n \n \n";
							}
							else
							{
								cout << "A DFA is yet to be conjectured!\n\n\n";
							}
						}

						if (results[1] == "save")
						{
							valid = true;
							if (res)
							{
								if (!resrend)
								{
									resrend = new DFArenderer(*res, gvc);
								}

								resrend->save(results[2], gvc);
								cout << "DFA successfully saved to file " << results[2] << ". \n \n \n";
							}
							else
							{
								cout << "A DFA is yet to be conjectured!\n\n\n";
							}
						}
					}
				}

				if (!valid)
				{
					cout << "Command '" << input << "' not recognized \n\n\n";
				}
			}
			hasinput = false;
			SDL_CreateThread(thread, "InputThread", NULL);
		}

		if (openWindow)
		{
			//Clear screen
			SDL_RenderClear(gRenderer);

			SDL_RenderSetViewport(gRenderer, &topLeftViewport);

			//Render texture to screen
			SDL_RenderCopy(gRenderer, gTexture, NULL, NULL);

			//Update screen
			SDL_RenderPresent(gRenderer);
		}
	}
	return 0;
}


#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <Eigen/Dense>
#include <motor_publisher.h>
#include <sstream>

struct cstr{ char s[100]; };

class JointToActuator {
 public:
  JointToActuator();

 private:
  void callback(const sensor_msgs::JointState::ConstPtr& joint_state);

  ros::NodeHandle nh_;
  MotorPublisher<cstr> motor_pub_ = {"wrist_commands"};
  ros::Subscriber joint_sub_;
  //std::vector<std::string> joint_names_ = {"tool_rotate_joint", "proximal_wrist_joint", "distal_wrist_joint", "jaw_a_joint"};
  std::vector<std::string> joint_names_ = {"j1", "j2", "j3", "j4"};
};

JointToActuator::JointToActuator() {
  nh_.getParam("joint_names", joint_names_);
  std::vector<double> actuator_transform;
  joint_sub_ = nh_.subscribe<sensor_msgs::JointState>("joint_states", 10, &JointToActuator::callback, this);
}

void JointToActuator::callback(const sensor_msgs::JointState::ConstPtr& joint_state) {
  sensor_msgs::JointState actuator_state;
  Eigen::VectorXd joint_position(joint_names_.size());
  for (int i=0; i<joint_names_.size(); i++) {
      auto joint_name = joint_names_[i];
      auto it = std::find(joint_state->name.begin(), joint_state->name.end(), joint_name);
      if (it == joint_state->name.end()) {
          return;
      }
      joint_position(i) = joint_state->position[it - joint_state->name.begin()];
  }
  std::ostringstream oss;
  oss << joint_position.transpose();
  cstr tmp = {};
  std::strncpy(tmp.s, oss.str().c_str(), sizeof(tmp));
  motor_pub_.publish(tmp);
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "joint_to_actuator");
  JointToActuator joint_to_actuator;

  ros::spin();
}


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

#include "open3d/t/geometry/PointCloud.h"

#include <Eigen/Core>
#include <string>
#include <unordered_map>

#include "open3d/core/EigenConverter.h"
#include "open3d/core/ShapeUtil.h"
#include "open3d/core/Tensor.h"
#include "open3d/core/hashmap/Hashmap.h"
#include "open3d/core/linalg/Matmul.h"
#include "open3d/t/geometry/TensorMap.h"
#include "open3d/t/geometry/kernel/PointCloud.h"

namespace open3d {
namespace t {
namespace geometry {

PointCloud::PointCloud(const core::Device &device)
    : Geometry(Geometry::GeometryType::PointCloud, 3),
      device_(device),
      point_attr_(TensorMap("points")) {
    ;
}

PointCloud::PointCloud(const core::Tensor &points)
    : PointCloud(points.GetDevice()) {
    points.AssertShapeCompatible({utility::nullopt, 3});
    SetPoints(points);
}

PointCloud::PointCloud(const std::unordered_map<std::string, core::Tensor>
                               &map_keys_to_tensors)
    : Geometry(Geometry::GeometryType::PointCloud, 3),
      point_attr_(TensorMap("points")) {
    if (map_keys_to_tensors.count("points") == 0) {
        utility::LogError("\"points\" attribute must be specified.");
    }
    device_ = map_keys_to_tensors.at("points").GetDevice();
    map_keys_to_tensors.at("points").AssertShapeCompatible(
            {utility::nullopt, 3});
    point_attr_ = TensorMap("points", map_keys_to_tensors.begin(),
                            map_keys_to_tensors.end());
}

core::Tensor PointCloud::GetMinBound() const { return GetPoints().Min({0}); }

core::Tensor PointCloud::GetMaxBound() const { return GetPoints().Max({0}); }

core::Tensor PointCloud::GetCenter() const { return GetPoints().Mean({0}); }

PointCloud PointCloud::To(const core::Device &device, bool copy) const {
    if (!copy && GetDevice() == device) {
        return *this;
    }
    PointCloud pcd(device);
    for (auto &kv : point_attr_) {
        pcd.SetPointAttr(kv.first, kv.second.To(device, /*copy=*/true));
    }
    return pcd;
}

PointCloud PointCloud::Clone() const { return To(GetDevice(), /*copy=*/true); }

PointCloud &PointCloud::Transform(const core::Tensor &transformation) {
    transformation.AssertShape({4, 4});
    transformation.AssertDevice(device_);

    if (transformation.AllClose(core::Tensor::Eye(
                4, transformation.GetDtype(), transformation.GetDevice()))) {
        return *this;
    }

    core::Tensor R = transformation.Slice(0, 0, 3).Slice(1, 0, 3);
    core::Tensor t = transformation.Slice(0, 0, 3).Slice(1, 3, 4);
    // TODO: Make it more generalised [4x4][4xN] transformation.

    // TODO: Consider adding a new op extending MatMul to support `AB + C`
    // GEMM operation. Also, a parallel joint optimimsed kernel for
    // independent MatMul operation with common matrix like AB and AC
    // with fusion based cache optimisation.
    core::Tensor &points = GetPoints();
    points = (R.Matmul(points.T())).Add_(t).T();

    if (HasPointNormals()) {
        core::Tensor &normals = GetPointNormals();
        normals = (R.Matmul(normals.T())).T();
    }
    return *this;
}

PointCloud &PointCloud::Translate(const core::Tensor &translation,
                                  bool relative) {
    translation.AssertShape({3});
    translation.AssertDevice(device_);

    core::Tensor transform = translation;
    if (!relative) {
        transform -= GetCenter();
    }
    GetPoints() += transform;
    return *this;
}

PointCloud &PointCloud::Scale(double scale, const core::Tensor &center) {
    center.AssertShape({3});
    center.AssertDevice(device_);

    core::Tensor points = GetPoints();
    points.Sub_(center).Mul_(scale).Add_(center);
    return *this;
}

PointCloud &PointCloud::Rotate(const core::Tensor &R,
                               const core::Tensor &center) {
    R.AssertShape({3, 3});
    R.AssertDevice(device_);
    center.AssertShape({3});
    center.AssertDevice(device_);

    core::Tensor Rot = R;
    core::Tensor &points = GetPoints();
    points = ((Rot.Matmul((points.Sub_(center)).T())).T()).Add_(center);

    if (HasPointNormals()) {
        core::Tensor &normals = GetPointNormals();
        normals = (Rot.Matmul(normals.T())).T();
    }
    return *this;
}

PointCloud PointCloud::VoxelDownSample(
        double voxel_size, const core::HashmapBackend &backend) const {
    if (voxel_size <= 0) {
        utility::LogError("voxel_size must be positive.");
    }
    core::Tensor points_voxeld = GetPoints() / voxel_size;
    core::Tensor points_voxeli = points_voxeld.Floor().To(core::Dtype::Int64);

    core::Hashmap points_voxeli_hashmap(points_voxeli.GetLength(),
                                        core::Dtype::Int64, core::Dtype::Int32,
                                        {3}, {1}, device_, backend);

    core::Tensor addrs, masks;
    points_voxeli_hashmap.Activate(points_voxeli, addrs, masks);

    PointCloud pcd_down(GetPoints().GetDevice());
    for (auto &kv : point_attr_) {
        if (kv.first == "points") {
            pcd_down.SetPointAttr(kv.first, points_voxeli.IndexGet({masks}).To(
                                                    GetPoints().GetDtype()) *
                                                    voxel_size);
        } else {
            pcd_down.SetPointAttr(kv.first, kv.second.IndexGet({masks}));
        }
    }

    return pcd_down;
}

PointCloud PointCloud::CreateFromDepthImage(const Image &depth,
                                            const core::Tensor &intrinsics,
                                            const core::Tensor &extrinsics,
                                            float depth_scale,
                                            float depth_max,
                                            int stride) {
    core::Dtype dtype = depth.AsTensor().GetDtype();
    if (dtype != core::Dtype::UInt16 && dtype != core::Dtype::Float32) {
        utility::LogError(
                "Unsupported dtype for CreateFromDepthImage, expected UInt16 "
                "or Float32, but got {}.",
                dtype.ToString());
    }

    core::Tensor points;
    kernel::pointcloud::Unproject(depth.AsTensor(), utility::nullopt, points,
                                  utility::nullopt, intrinsics, extrinsics,
                                  depth_scale, depth_max, stride);
    return PointCloud(points);
}

PointCloud PointCloud::CreateFromRGBDImage(const RGBDImage &rgbd_image,
                                           const core::Tensor &intrinsics,
                                           const core::Tensor &extrinsics,
                                           float depth_scale,
                                           float depth_max,
                                           int stride) {
    auto dtype = rgbd_image.depth_.AsTensor().GetDtype();
    if (dtype != core::Dtype::UInt16 && dtype != core::Dtype::Float32) {
        utility::LogError(
                "Unsupported dtype for CreateFromRGBDImage, expected UInt16 "
                "or Float32, but got {}.",
                dtype.ToString());
    }

    core::Tensor image_colors =
            rgbd_image.color_.To(core::Dtype::Float32, /*copy=*/false)
                    .AsTensor();

    core::Tensor points, colors;
    kernel::pointcloud::Unproject(rgbd_image.depth_.AsTensor(), image_colors,
                                  points, colors, intrinsics, extrinsics,
                                  depth_scale, depth_max, stride);
    return PointCloud({{"points", points}, {"colors", colors}});
}

PointCloud PointCloud::FromLegacyPointCloud(
        const open3d::geometry::PointCloud &pcd_legacy,
        core::Dtype dtype,
        const core::Device &device) {
    geometry::PointCloud pcd(device);
    if (pcd_legacy.HasPoints()) {
        pcd.SetPoints(core::eigen_converter::EigenVector3dVectorToTensor(
                pcd_legacy.points_, dtype, device));
    } else {
        utility::LogWarning("Creating from an empty legacy PointCloud.");
    }
    if (pcd_legacy.HasColors()) {
        pcd.SetPointColors(core::eigen_converter::EigenVector3dVectorToTensor(
                pcd_legacy.colors_, dtype, device));
    }
    if (pcd_legacy.HasNormals()) {
        pcd.SetPointNormals(core::eigen_converter::EigenVector3dVectorToTensor(
                pcd_legacy.normals_, dtype, device));
    }
    return pcd;
}

open3d::geometry::PointCloud PointCloud::ToLegacyPointCloud() const {
    open3d::geometry::PointCloud pcd_legacy;
    if (HasPoints()) {
        pcd_legacy.points_ =
                core::eigen_converter::TensorToEigenVector3dVector(GetPoints());
    }
    if (HasPointColors()) {
        pcd_legacy.colors_ = core::eigen_converter::TensorToEigenVector3dVector(
                GetPointColors());
    }
    if (HasPointNormals()) {
        pcd_legacy.normals_ =
                core::eigen_converter::TensorToEigenVector3dVector(
                        GetPointNormals());
    }
    return pcd_legacy;
}

}  // namespace geometry
}  // namespace t
}  // namespace open3d


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_IS_GREATER_EQUAL_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_GREATER_EQUAL_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-predicates
    This function object returns @ref True or @ref False
    according x is greater or equal to y or not.

    Infix notation can be used with operator '>='.

    @par Header <boost/simd/function/is_greater_equal.hpp>

    @par Note

      Using `is_greater_equal(x,y)` is equivalent to `x >= y`

    @see  is_greater, is_gtz

    @par Example:

      @snippet is_greater_equal.cpp is_greater_equal

    @par Possible output:

      @snippet is_greater_equal.txt is_greater_equal

  **/
  as_logical_t<Value> is_greater_equal(Value const& x, Value const& y);
} }
#endif

#include <boost/simd/function/scalar/is_greater_equal.hpp>
#include <boost/simd/function/simd/is_greater_equal.hpp>

#endif