#include <iostream>
#include <fstream>
#include <stdlib.h>
#include <memory>
#include <functional>
#include <atomic>

#include <Eigen/Core>

#include <opencv2/opencv.hpp>
#include <okvis/VioParametersReader.hpp>
#include <okvis/ThreadedKFVio.hpp>

#include <boost/filesystem.hpp>

#include <ros/ros.h>
#include <ros/console.h>
#include <geometry_msgs/PoseStamped.h>

void publishFullStateAsCallback(
    const okvis::Time &t,
    const okvis::kinematics::Transformation &T_WS,
    const Eigen::Matrix<double, 9, 1> &speedAndBiases,
    const Eigen::Matrix<double, 3, 1> &omega_S);

ros::Publisher _pose_pub;
geometry_msgs::PoseStamped _pose;

int main(int argc, char **argv) {
  ros::NodeHandle nh;
  _pose_pub = nh.advertise
            <geometry_msgs::PoseStamped>
            ("/okvis/pose", 10);

  std::string configFilename;
  if (!nh.getParam("/okvis_config", configFilename)) {
    ROS_ERROR("Must pass okvis config file");
  }

  okvis::VioParametersReader vio_parameters_reader(configFilename);
  okvis::VioParameters parameters;
  vio_parameters_reader.getParameters(parameters);

  okvis::ThreadedKFVio okvis_estimator(parameters);

  okvis_estimator.setFullStateCallback(
    std::bind(&publishFullStateAsCallback,
      std::placeholders::_1,std::placeholders::_2,
      std::placeholders::_3,std::placeholders::_4));

  okvis_estimator.setBlocking(true);

  return 0;
}

void publishFullStateAsCallback(
    const okvis::Time &t,
    const okvis::kinematics::Transformation &T_WS,
    const Eigen::Matrix<double, 9, 1> &speedAndBiases,
    const Eigen::Matrix<double, 3, 1> &omega_S) {
  Eigen::Vector3d r = T_WS.r();
  ROS_INFO("Translation\n\tx:\t%f\n\ty:\t%f\n\tz:\t%f",
      r[0],r[1],r[2]);

  _pose.pose.position.x = r[0];
  _pose.pose.position.y = r[1];
  _pose.pose.position.z = r[2];
  _pose_pub.publish(_pose);
}


#include <stdlib.h>
#include <iostream>
#include <thread>
#include <string>
#include <cmath>

#include <inttypes.h>

#include <boost/tr1/random.hpp>
#include "utils.h"

#include "WordEmbed.h"

#define EXP_TABLE_SIZE 1000
#define MAX_EXP 6
#define MAX_CODE_LENGTH 40
#define MAX_SENTENCE_LENGTH 1000

class Word2Vec : public WordEmbed {
    private:
        unsigned int layer1_size;
        int window;
        real sample;
        int hs, negative;
        unsigned int num_threads;
        unsigned int iter;
        real alpha;
        unsigned int classes;
        int  cbow;
        std::string wordvector_file;

        int64_t word_count_actual;

        real starting_alpha;

        std::array<real,(EXP_TABLE_SIZE + 1)> expTable;

        clock_t start;

        std::vector<real> syn0, syn1, syn1neg;

        // Tables
        std::array<int, table_size> table;
    public:
        Word2Vec (
                std::string train_file, //Use text data from <file> to train the model
                std::string output_file = "", //Use <file> to save the resulting word vectors / word clusters
                unsigned int min_count = 5, //This will discard words that appear less than <int> times; default is 5
                int debug_mode = 2, //Set the debug mode (default = 2 = more info during training)
                int binary = 0, //Save the resulting vectors in binary moded; default is 0 (off)
                std::string save_vocab_file = "", //The vocabulary will be saved to <file>
                std::string read_vocab_file = "", //The vocabulary will be read from <file>, not constructed from the training data
                unsigned int layer1_size = 100, //Set size of word vectors; default is 100
                unsigned int window = 5, //Set max skip length between words; default is 5
                real sample = 1e-3, //Set threshold for occurrence of words. Those that appear with higher frequency in the training data will be randomly down-sampled; default is 1e-3, useful range is (0, 1e-5)
                int hs = 0, //Use Hierarchical Softmax; default is 0 (not used)
                int negative = 5, //Number of negative examples; default is 5, common values are 3 - 10 (0 = not used)
                unsigned int num_threads = 12, //Use <int> threads (default 12)
                int iter = 5, //Run more training iterations (default 5)
                real alpha = 0.05, //Set the starting learning rate; default is 0.025 for skip-gram and 0.05 for CBOW
                unsigned int classes = 0,//Output word classes rather than word vectors; default number of classes is 0 (vectors are written)
                int cbow = 0, //Use the continuous bag of words model; default is 1 (use 0 for skip-gram model)
                std::string wordvector_file = "" //Read the trained word vector file
                ):
            WordEmbed (train_file, output_file, min_count, debug_mode, binary,save_vocab_file,read_vocab_file),
            layer1_size (layer1_size),
            window (window),
            sample (sample),
            hs (hs),
            negative (negative),
            num_threads (num_threads),
            iter (iter),
            alpha (alpha),
            classes (classes),
            cbow (cbow),
            wordvector_file (wordvector_file)
        {
            min_reduce = 1,
            vocab_size = 0;
            train_words = 0;
            word_count_actual=0;
            file_size = 0;

            ready_to_train = false;
            if( CheckReady() ) ready_to_train = true;
        };

        void TrainModel ();

        void CreateBinaryTree ();

        void InitUnigramTable ();
        void InitNet ();
        void TrainModelThread (int tid);
};

//vocab cn word codelen point code

// Create binary Huffman tree using the word counts
// Frequent words will have short unique binary codes
void Word2Vec::CreateBinaryTree() {
    // Words are not sorted at the beginning
    int64_t a, b, i, min1i, min2i, pos1, pos2;
    int64_t point[MAX_CODE_LENGTH];
    char code[MAX_CODE_LENGTH];

    std::vector<int64_t> count;
    std::vector<int64_t> binary;
    std::vector<int64_t> parent_node;

    count.reserve(vocab_size * 2 + 1);
    binary.reserve(vocab_size * 2 + 1);
    parent_node.reserve(vocab_size * 2 +1);

    for(a = 0; a < vocab_size; a++) count[a] = vocab[a].cn; // count word frequency
    for(a = vocab_size; a < 2 * vocab_size; a++) count[a] = 1e15; // should be larger than any number in the vocabulary count
    pos1 = vocab_size - 1;
    pos2 = vocab_size; //

    // following algorithm constructs the Huffman tree by adding one node at a time
    for(a = 0; a < vocab_size - 1; a++) {
        // First, find two smallest nodes 'min1, min2'
        if(pos1 >= 0) {
            if(count[pos1] < count[pos2]) {
                min1i = pos1;
                pos1--;
            } else {
                min1i = pos2;
                pos2++;
            }
        } else {
            min1i = pos2;
            pos2++;
        }
        if(pos1 >= 0) {
            if(count[pos1] < count[pos2]) {
                min2i = pos1;
                pos1--;
            } else {
                min2i = pos2;
                pos2++;
            }
        } else {
            min2i = pos2;
            pos2++;
        }
        count[vocab_size + a] = count[min1i] + count[min2i]; // Creating parent node with summed frequency
        parent_node[min1i] = vocab_size + a;
        parent_node[min2i] = vocab_size + a; // Note that having a common parent node
        binary[min2i] = 1; // min2i will point to root node at the final state
    }
    // Now assign binary code to each vocabulary word
    for(a = 0; a < vocab_size; a++) {
        b = a;
        i = 0;
        while(1) {
            code[i] = binary[b];
            point[i] = b;
            i++;
            b = parent_node[b];
            if(b == vocab_size * 2 - 2) break;
        }
        vocab[a].codelen = i;
        vocab[a].point[0] = vocab_size - 2;
        for(b = 0; b < i; b++) {
            vocab[a].code[i - b - 1] = code[b];
            vocab[a].point[i - b] = point[b] - vocab_size;
        }
    }
}

// Begin of Learning Net

void Word2Vec::InitUnigramTable() {
    unsigned int i;
    double train_words_pow = 0;
    double d1, power = 0.75;
    int64_t sum = 0;
    for (unsigned int a = 0; a < vocab_size; a++) train_words_pow += pow(vocab[a].cn, power);
    for (unsigned int a = 0; a < vocab_size; a++) sum += vocab[a].cn;
    std::cout << sum << std::endl;
    i = 0;
    d1 = pow(vocab[i].cn, power) / train_words_pow;
    for (unsigned int a = 0; a < table_size; a++) {
        table[a] = i;
        if (a / (double)table_size > d1) {
            i++;
            d1 += pow(vocab[i].cn, power) / train_words_pow;
        }
        if (i >= vocab_size) i = vocab_size - 1;
    }
}

void Word2Vec::InitNet() {
    // The name EXP_TABLE is totally mis-guiding.
    for (int i = 0; i < EXP_TABLE_SIZE; i++) {
        expTable[i] = exp((i / (real)EXP_TABLE_SIZE * 2 - 1) * MAX_EXP);
        expTable[i] = expTable[i] / (expTable[i] + 1);
    }

    syn0.reserve((int64_t)vocab_size * layer1_size);
    if(hs) {
        syn1.reserve((int64_t)vocab_size * layer1_size);
        for(unsigned int a = 0; a < vocab_size; a++)
            for(unsigned int b = 0; b < layer1_size; b++)
                syn1[a * layer1_size + b] = 0;
    }
    if(negative > 0) {
        syn1neg.reserve((int64_t)vocab_size * layer1_size);
        for(unsigned int a = 0; a < vocab_size; a++)
            for(unsigned int b = 0; b < layer1_size; b++)
                syn1neg[a * layer1_size + b] = 0;
    }

    for(unsigned int a = 0; a < vocab_size; a++)
        for(unsigned int b = 0; b < layer1_size; b++)
            syn0[a * layer1_size + b] = (rand()/(double)RAND_MAX - 0.5) / layer1_size;

    //CreateBinaryTree();
}

void Word2Vec::TrainModelThread(int tid){
    int64_t a, b, d, word, last_word, sentence_length = 0, sentence_position = 0;
    int64_t word_count = 0, last_word_count = 0, sen[MAX_SENTENCE_LENGTH + 1];
    int64_t l1, l2, c, target, label, local_iter = iter;
    uint64_t next_random;
    real f, g;
    clock_t now;

    boost::mt19937 rand_engine_int;    // rand engine
    boost::uniform_int<> rand_int(0, table_size);    // set range.
    boost::variate_generator<boost::mt19937, boost::uniform_int<>> rand_gen_int(rand_engine_int, rand_int);

    boost::mt19937 rand_engine_double;  // rand engine
    boost::uniform_real<> rand_double(0.0, 1.0);
    boost::variate_generator<boost::mt19937, boost::uniform_real<>> rand_gen_double(rand_engine_double, rand_double);

    std::vector<real> neu1;
    std::vector<real> neu1e;

    neu1.reserve(layer1_size);
    neu1e.reserve(layer1_size);

    std::ifstream inFile(train_file, std::ifstream::in | std::ifstream::binary);
    inFile.seekg(file_size / (int64_t)num_threads * (int64_t)tid);
    while(1) {
        if(word_count - last_word_count > 10000) {
            word_count_actual += word_count - last_word_count;
            last_word_count = word_count;
            if((debug_mode > 1)) {
                now = clock();
                printf("%cAlpha: %f  Progress: %.2f%%  Words/thread/sec: %.2fk   ", 13, alpha,
                        word_count_actual / (real)(iter * train_words + 1) * 100,
                        word_count_actual / ((real)(now - start + 1) / (real)CLOCKS_PER_SEC * 1000));
                fflush(stdout);
            }
            alpha = starting_alpha * (1 - word_count_actual / (real)(iter * train_words + 1));
            if(alpha < starting_alpha * 0.0001) alpha = starting_alpha * 0.0001;
        }
        if(sentence_length == 0) {
            while(1) {
                word = ReadWordIndex(inFile);
                if(inFile.eof()) break;
                if(word == -1) continue;
                word_count++;
                if(word == 0) break;
                // The subsampling randomly discards frequent words while keeping the ranking same
                if(sample > 0) {
                    real ran = (sqrt(vocab[word].cn / (sample * train_words)) + 1) * (sample * train_words) / vocab[word].cn;
                    if(ran < rand_gen_double()) continue;
                }
                sen[sentence_length] = word;
                sentence_length++;
                if(sentence_length >= MAX_SENTENCE_LENGTH) break;
            }
            sentence_position = 0;
        }
        if(inFile.eof() || (word_count > train_words / num_threads )) {
            word_count_actual += word_count - last_word_count;
            local_iter--;
            if(local_iter == 0) break;
            word_count = 0;
            last_word_count = 0;
            sentence_length = 0;
	    if(inFile.eof()) {
	      inFile.clear();
	      inFile.seekg(file_size / (int64_t)num_threads * (int64_t)tid);
	    }
	    else {
	      inFile.seekg(file_size / (int64_t)num_threads * (int64_t)tid);
	    }
            continue;
        }
        word = sen[sentence_position];
        if(word == -1) continue;
        // Network Initialization
        for(c = 0; c < layer1_size; c++) neu1[c] = 0;
        for(c = 0; c < layer1_size; c++) neu1e[c] = 0;

        b = rand_gen_int() % window;
        // Skip-gram
        {
            for(a = b; a < window * 2 + 1 - b; a++) if(a != window) {
                c = sentence_position - window + a;
                if(c < 0) continue;
                if(c >= sentence_length) continue;
                last_word = sen[c];
                if(last_word == -1) continue;
                l1 = last_word * layer1_size;
                // Hierarchical Softmax
                if(hs) for(d = 0; d < vocab[word].codelen; d++) {
                    f = 0;
                    l2 = vocab[word].point[d] * layer1_size;
                    // Propagate hidden -> output
                    for(c = 0; c < layer1_size; c++) f += syn0[c + l1] * syn1[c + l2];
                    if(f <= -MAX_EXP) continue;
                    else if(f >= MAX_EXP) continue;
                    else f = expTable[(int)((f + MAX_EXP) * (EXP_TABLE_SIZE / MAX_EXP / 2))];
                    // g is the gradient multiplied by the learning rate
                    g = (1 - vocab[word].code[d] - f) * alpha;
                    // Backpropagate errors output -> hidden
                    for(c = 0; c < layer1_size; c++) neu1e[c] += g * syn1[c + l2];
                    // Learn weights hidden -> output
                    for(c = 0; c < layer1_size; c++) syn1[c + l2] += g * syn0[c +l1];
                }
                // Negative Sampling
                if(negative > 0) for (d = 0; d < negative + 1 ; d++) {
                    if(d == 0) {
		      target = word;
		      label = 1;
                    } else {
		      next_random = rand_gen_int();
		      target = table[next_random % table_size];
		      if(target == 0) target = next_random % (vocab_size - 1) + 1;
		      if(target == word) continue;
		      label = 0;
                    }
                    l2 = target * layer1_size;
                    f = 0;
                    for(c = 0; c < layer1_size; c++) f += syn0[c + l1] * syn1neg[c + l2];
                    if(f > MAX_EXP) g = (label - 1) * alpha;
                    else if(f < -MAX_EXP) g = (label - 0) * alpha;
                    else g = (label - expTable[(int)((f + MAX_EXP) * (EXP_TABLE_SIZE / MAX_EXP / 2))]) * alpha;
                    for (c = 0; c < layer1_size; c++) neu1e[c] += g * syn1neg[c + l2];
                    for (c = 0; c < layer1_size; c++) syn1neg[c + l2] += g * syn0[c + l1];
                }
                // Learn weights input -> hidden
                for(c = 0; c < layer1_size; c++) syn0[c + l1] += neu1e[c];
            }
        }
        sentence_position++;
        if(sentence_position >= sentence_length) {
            sentence_length = 0;
            continue;
        }
    }
    inFile.close();
}

void Word2Vec::TrainModel() {
    std::ofstream outFile;

    std::vector<std::thread> th;
    starting_alpha = alpha;

    std::cout << "Starting training using file " << train_file << std::endl;
    if(read_vocab_file != "") ReadVocab();
    else ExtractVocabFromTrainFile();
    if(save_vocab_file != "") SaveVocab();
    if(output_file[0] == 0) return;

    // Initialization
    InitNet();
    if(negative > 0)
        InitUnigramTable();

    start = clock(); // start to measure time

    for(int i = 0; i < num_threads; i++) {
      th.push_back(std::thread(&Word2Vec::TrainModelThread, this, i));
    }

    for(auto &t : th) {
      t.join();
    }

    //TrainModelThread(); // For the single thread

    outFile.open(output_file, std::ofstream::out | std::ofstream::binary);
    if(classes == 0) {
        outFile << vocab_size << " " << layer1_size << std::endl;
        for(unsigned int a = 0; a < vocab_size; a++) {
            outFile << vocab[a].word << " ";
            if(binary) for(unsigned int b = 0; b < layer1_size; b++) outFile << syn0[a * layer1_size + b] << " ";
            else for(unsigned int b = 0; b < layer1_size; b++) outFile << syn0[a * layer1_size + b] << " ";
            outFile << std::endl;
        }
    } else {
        // Run K-means on the word vectors
        unsigned int clcn = classes, iter = 10, closeid;
        real closev, x;

        std::vector<int> centcn, cl;
        std::vector<real> cent;

        centcn.reserve(classes);
        cl.reserve(vocab_size);
        cent.reserve(classes * layer1_size);

        for (unsigned int a = 0; a < vocab_size; a++) cl[a] = a % clcn;
        for (unsigned int a = 0; a < iter; a++) {
            for (unsigned int b = 0; b < clcn * layer1_size; b++) cent[b] = 0;
            for (unsigned int b = 0; b < clcn; b++) centcn[b] = 1;
            for (unsigned int c = 0; c < vocab_size; c++) {
                for (unsigned int d = 0; d < layer1_size; d++) cent[layer1_size * cl[c] + d] += syn0[c * layer1_size + d];
                centcn[cl[c]]++;
            }
            for (unsigned int b = 0; b < clcn; b++) {
                closev = 0;
                for (unsigned int c = 0; c < layer1_size; c++) {
                    cent[layer1_size * b + c] /= centcn[b];
                    closev += cent[layer1_size * b + c] * cent[layer1_size * b + c];
                }
                closev = sqrt(closev);
                for (unsigned int c = 0; c < layer1_size; c++) cent[layer1_size * b + c] /= closev;
            }
            for (unsigned int c = 0; c < vocab_size; c++) {
                closev = -10;
                closeid = 0;
                for (unsigned int d = 0; d < clcn; d++) {
                    x = 0;
                    for (unsigned int b = 0; b < layer1_size; b++) x += cent[layer1_size * d + b] * syn0[c * layer1_size + b];
                    if (x > closev) {
                        closev = x;
                        closeid = d;
                    }
                }
                cl[c] = closeid;
            }
        }
        // Save the K-means classes
        for (unsigned int a = 0; a < vocab_size; a++) outFile << vocab[a].word << "    " << cl[a] << std::endl;

    }
    outFile.close();
}

// End of Learning Net

// main function arguments

void printHelp() {
    std::cout << "c++ word2vector implementation \n\n";
    std::cout << "Options:\n";
    std::cout << "Parameters for training:\n";
    std::cout << "\t-train <file>\n";
    std::cout << "\t\tUse text data from <file> to train the model\n";
    std::cout << "\t-output <file>\n";
    std::cout << "\t\tUse <file> to save the resulting word vectors / word clusters\n";
    std::cout << "\t-size <int>\n";
    std::cout << "\t\tSet size of word vectors; default is 100\n";
    std::cout << "\t-window <int>\n";
    std::cout << "\t\tSet max skip length between words; default is 5\n";
    std::cout << "\t-sample <float>\n";
    std::cout << "\t\tSet threshold for occurrence of words. Those that appear with higher frequency in the training data\n";
    std::cout << "\t\twill be randomly down-sampled; default is 1e-3, useful range is (0, 1e-5)\n";
    std::cout << "\t-hs <int>\n";
    std::cout << "\t\tUse Hierarchical Softmax; default is 0 (not used)\n";
    std::cout << "\t-negative <int>\n";
    std::cout << "\t\tNumber of negative examples; default is 5, common values are 3 - 10 (0 = not used)\n";
    std::cout << "\t-threads <int>\n";
    std::cout << "\t\tUse <int> threads (default 12)\n";
    std::cout << "\t-iter <int>\n";
    std::cout << "\t\tRun more training iterations (default 5)\n";
    std::cout << "\t-min-count <int>\n";
    std::cout << "\t\tThis will discard words that appear less than <int> times; default is 5\n";
    std::cout << "\t-alpha <float>\n";
    std::cout << "\t\tSet the starting learning rate; default is 0.025 for skip-gram and 0.05 for CBOW\n";
    std::cout << "\t-classes <int>\n";
    std::cout << "\t\tOutput word classes rather than word vectors; default number of classes is 0 (vectors are written)\n";
    std::cout << "\t-debug <int>\n";
    std::cout << "\t\tSet the debug mode (default = 2 = more info during training)\n";
    std::cout << "\t-binary <int>\n";
    std::cout << "\t\tSave the resulting vectors in binary moded; default is 0 (off)\n";
    std::cout << "\t-save-vocab <file>\n";
    std::cout << "\t\tThe vocabulary will be saved to <file>\n";
    std::cout << "\t-read-vocab <file>\n";
    std::cout << "\t\tThe vocabulary will be read from <file>, not constructed from the training data\n";
    std::cout << "\t-cbow <int>\n";
    std::cout << "\t\tUse the continuous bag of words model; default is 1 (use 0 for skip-gram model)\n";
    std::cout << "\t-wordvector <file>\n";
    std::cout << "\t\tRead the trained word vector file\n";
    std::cout << "\nExamples:\n";
    std::cout << "./word2vec -train data.txt -output vec.txt -size 200 -window 5 -sample 1e-4 -negative 5 -hs 0 -binary 0 -cbow 1 -iter 3\n\n";

}

int argpos(const char *str, int argc, char **argv) {
    std::string s_str(str);
    std::vector<std::string> s_argv(argv,argv+argc);

    int i=0;
    while (i<argc) {
        if(s_str == s_argv[i]) break;
        i++;
    }
    if(i == argc) i=0;

    return i;
}

Word2Vec *arg_to_w2v(int argc, char **argv) {
    int i;

    int layer1_size = 100;
    std::string train_file ="", save_vocab_file = "", read_vocab_file = "";
    int debug_mode =2, binary = 0, cbow = 0;
    real alpha = 0.05;
    std::string output_file = "";
    int window = 5;
    real sample = 1e-3;
    int hs = 0, negative = 5, num_threads = 12;
    int64_t iter = 5;
    int min_count = 5;
    int64_t classes = 0;
    std::string wordvector_file = "";

    if ((i = argpos("-size", argc, argv)) > 0) layer1_size = atoi(argv[i + 1]);
    if ((i = argpos("-train", argc, argv)) > 0) train_file = argv[i + 1];
    if ((i = argpos("-save-vocab", argc, argv)) > 0) save_vocab_file = argv[i + 1];
    if ((i = argpos("-read-vocab", argc, argv)) > 0) read_vocab_file = argv[i + 1];
    if ((i = argpos("-debug", argc, argv)) > 0) debug_mode = atoi(argv[i + 1]);
    if ((i = argpos("-binary", argc, argv)) > 0) binary = atoi(argv[i + 1]);
    if ((i = argpos("-cbow", argc, argv)) > 0) cbow = atoi(argv[i + 1]);
    if (cbow) alpha = 0.05; else alpha = 0.025;
    if ((i = argpos("-alpha", argc, argv)) > 0) alpha = atof(argv[i + 1]);
    if ((i = argpos("-output", argc, argv)) > 0) output_file = argv[i + 1];
    if ((i = argpos("-window", argc, argv)) > 0) window = atoi(argv[i + 1]);
    if ((i = argpos("-sample", argc, argv)) > 0) sample = atof(argv[i + 1]);
    if ((i = argpos("-hs", argc, argv)) > 0) hs = atoi(argv[i + 1]);
    if ((i = argpos("-negative", argc, argv)) > 0) negative = atoi(argv[i + 1]);
    if ((i = argpos("-threads", argc, argv)) > 0) num_threads = atoi(argv[i + 1]);
    if ((i = argpos("-iter", argc, argv)) > 0) iter = atoi(argv[i + 1]);
    if ((i = argpos("-min-count", argc, argv)) > 0) min_count = atoi(argv[i + 1]);
    if ((i = argpos("-classes", argc, argv)) > 0) classes = atoi(argv[i + 1]);
    if ((i = argpos("-wordvector", argc, argv)) > 0) wordvector_file = argv[i + 1];

    Word2Vec *w2v = new Word2Vec (
                train_file,
                output_file,
                min_count,
                debug_mode,
                binary,
                save_vocab_file,
                read_vocab_file,
                layer1_size,
                window,
                sample,
                hs,
                negative,
                num_threads,
                iter,
                alpha,
                classes,
                cbow,
                wordvector_file );

    return w2v;
}

int main(int argc, char **argv) {

    std::srand(11596521);
    Word2Vec *w2v;

    if(argc < 2) { printHelp(); return 1; }
    else {
        w2v = arg_to_w2v(argc, argv);
    }

    w2v->TrainModel();

    return 0;
}


#include <exception>

// ROS
#include <ros/console.h>

#include <Eigen/Dense>
#include <drake_robot_control/task_space_streaming_plan.h>
#include <drake_robot_control/utils.h>

namespace drake {
namespace robot_plan_runner {

typedef drake::TwistVector<double> TwistVectord;

// Current robot state x = [q,v]
// Current time t
void TaskSpaceStreamingPlan::Step(
    const Eigen::Ref<const Eigen::VectorXd> &x,
    const Eigen::Ref<const Eigen::VectorXd> &tau_external, double t,
    Eigen::VectorXd *const q_commanded, Eigen::VectorXd *const v_commanded,
    Eigen::VectorXd *const tau_commanded) {
  double dt = t - last_control_update_t_;
  last_control_update_t_ = t;
  dt = std::max(std::min(dt, 0.01), 0.0001);

  PlanStatus not_started_status = PlanStatus::NOT_STARTED;
  PlanStatus running_status = PlanStatus::RUNNING;

  plan_status_.compare_exchange_strong(not_started_status, PlanStatus::RUNNING);

  // Lock so we can play with the measured state cache
  std::lock_guard<std::mutex> lock(goal_mutex_);

  // do kinematics so we can check the force guards
  Eigen::VectorXd q = x.head(this->get_num_positions());
  Eigen::VectorXd v = x.tail(this->get_num_positions());
  cache_measured_state_.initialize(q, v);
  tree_->doKinematics(cache_measured_state_);

  // check if the plan has been stopped
  // if so just echo the last command
  if (this->is_stopped() || !this->have_goal_) {
    *q_commanded = q_commanded_prev_;
    *tau_commanded = Eigen::VectorXd::Zero(this->get_num_positions());
    return;
  }

  // Check the external force guards
  if (guard_container_) {
    std::pair<bool, std::pair<double, std::shared_ptr<ForceGuard>>> result =
                                          guard_container_->EvaluateGuards(cache_measured_state_, q,
                                                                           tau_external);

    bool guard_triggered = result.first;

    if (guard_triggered) {
      std::cout << "Force Guard Triggered, commanding in direction of measured position" << std::endl;
      std::cout << "ForceGuardType: " << result.second.second->get_type()
                << std::endl;
      //plan_status_ = PlanStatus::STOPPED_BY_FORCE_GUARD;
      //this->SetPlanFinished();
      *q_commanded = q_commanded_prev_*0.99 + q*0.01;
      q_commanded_prev_ = *q_commanded;
      *tau_commanded = Eigen::VectorXd::Zero(this->get_num_positions());
      return;
    }
  }

  // if we reach here then plan should be in state RUNNING
  // COPIED LARGELY FROM TASK_SPACE_TRAJECTORY_PLAN
  // TODO(gizatt) Refactor to reduce this repeated code.
  // Unfortunately it depends on a large number of internal parameters
  // (lots of frames, gains, and goals), so breaking it out into
  // a function is pretty ugly.
  // Instead of the actual q, use the last commanded q
  Eigen::VectorXd q_measured = x.head(this->get_num_positions());
  q = q_commanded_prev_;

  // std::cout << "Starting control in measured config " << q_measured << " but commanded " << q << std::endl;
  // std::cout << "xyz_ee_goal: " << xyz_ee_goal_ << std::endl;
  // std::cout << "xyz_d_ee_goal: " << xyz_d_ee_goal_ << std::endl;
  // std::cout << "quat_ee_goal_: " << quat_ee_goal_.matrix() << std::endl;

  cache_.initialize(q, v);
  tree_->doKinematics(cache_);

  math::RotationMatrixd R_WEr(quat_ee_goal_);
  math::RotationMatrixd R_ErW = R_WEr.inverse();

  J_ee_E_ = tree_->geometricJacobian(cache_, 0, body_index_ee_frame_, body_index_ee_frame_);
  J_ee_W_ = tree_->geometricJacobian(cache_, 0, body_index_ee_frame_, 0);

  H_WEr_.set_rotation(R_WEr);
  H_WEr_.set_translation(xyz_ee_goal_);
  Eigen::Isometry3d H_WEr = H_WEr_.GetAsIsometry3();

  std::cout << "H_WEr: " << H_WEr.matrix() << std::endl;

  if (body_index_ee_frame_ >= 0){
    H_WE_ = tree_->CalcBodyPoseInWorldFrame(cache_, tree_->get_body(body_index_ee_frame_));
  } else {
    // frames start at -2 and count down.
    H_WE_ = tree_->CalcFramePoseInWorldFrame(cache_, *tree_->get_frames()[-body_index_ee_frame_ - 2]);
  }
  std::cout << "H_WE_: " << H_WE_.matrix() << std::endl;
  Eigen::Isometry3d H_EW = H_WE_.inverse();
  Eigen::Isometry3d H_EEr = H_EW * H_WEr;
  std::cout << "HEEr: " << H_EEr.matrix() << std::endl;

  // Compute the PD part of the control
  // K_{p,w} log_{SO(3)}(R_EEr)
  Eigen::Vector3d log_R_EEr =
      spartan::drake_robot_control::utils::LogSO3(H_EEr.linear());

  TwistVectord twist_pd;
  // we need to do elementwise multiplication, hence the use of array instead
  // of Vector
  twist_pd.head(3) = this->kp_rotation_.array() * log_R_EEr.array();
  twist_pd.tail(3) =
      this->kp_translation_.array() * H_EEr.translation().array();

  // Compute the feed forward part of the control
  // Need twist of reference trajectory with respect to world
  // easiest to compute this as expressed in Er frame,
  // then transform that twist to E frame using adjoint
  TwistVectord T_WEr_Er;

  T_WEr_Er.head(3) = Eigen::Vector3d::Zero(); // hack for now
  T_WEr_Er.tail(3) = R_WEr * xyz_d_ee_goal_;

  Eigen::Matrix<double, 6, 6> Ad_H_EEr =
      spartan::drake_robot_control::utils::AdjointSE3(H_EEr.linear(),
                                                      H_EEr.translation());
  TwistVectord T_WEr_E = Ad_H_EEr * T_WEr_Er;

  // Total desired twist
  // Can add these this twists since both expressed in frame E
  TwistVectord T_WE_E_cmd = twist_pd + T_WEr_E;

  // q_dot_cmd = J_ee.pseudo_inverse()*T_WE_E_cmd
  auto svd = J_ee_E_.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
  // When computing the pseudoinverse, this ignores all singular
  // values smaller than this threshold. This is raised
  // from the Eigen default to create less jerky movements near
  // singularities.
  svd.setThreshold(0.01);
  Eigen::VectorXd q_dot_cmd = svd.solve(T_WE_E_cmd);
  std::cout << "Final q dot cmd: " << q_dot_cmd << std::endl;
  *q_commanded = q + q_dot_cmd * dt;
  *v_commanded = q_dot_cmd; // This is ignored when constructing iiwa_command.

  bool unsafe_command = Eigen::isnan(q_commanded->array()).any();
  if (unsafe_command) {
    std::cout << "\n\nunsafe command caught inside Step()" << std::endl;
    std::cout << "q_commanded:\n" << *q_commanded << std::endl;
    std::cout << "q_dot_cmd:\n" << q_dot_cmd << std::endl;
    std::cout << "q:\n" << q << std::endl;

    std::cout << "\nT_WE_E_cmd:\n" << T_WE_E_cmd << std::endl;
  }

  bool debug = false;
  if (debug) {
    std::cout << "\n-------\n";
    std::cout << "\nT_WE_E_cmd:\n" << T_WE_E_cmd << std::endl;

    Eigen::Isometry3d H_ErE = H_EEr.inverse();
    std::cout << "H_ErE.translation() " << H_ErE.translation() << std::endl;
  }
  // debugging prints for when things are nan . . .
}

void TaskSpaceStreamingPlan::HandleSetpoint(
    const robot_msgs::CartesianGoalPoint::ConstPtr& msg) {
  if (this->is_stopped()){
    std::cout << "In callback, but plan is stopped... forcefully unregistering." << std::endl;
    this->setpoint_subscriber_->shutdown();
  }
  std::lock_guard<std::mutex> lock(goal_mutex_);
  //std::cout << "Starting to handle setpoint... " << std::endl;
  // Extract the body index in the RBT that this msg
  // is referring to.
  // These will throw if the frame isn't unique or doesn't exist.
  body_index_ee_goal_ = tree_->findFrame(
    msg->xyz_point.header.frame_id)->get_frame_index();
  body_index_ee_frame_ = tree_->findFrame(msg->ee_frame_id)->get_frame_index();

  tree_->doKinematics(cache_measured_state_);

  xyz_ee_goal_ = Eigen::Vector3d(msg->xyz_point.point.x,
                                 msg->xyz_point.point.y,
                                 msg->xyz_point.point.z);
  // Transform to world frame at last observed robot posture
  auto R = tree_->relativeTransform(
    cache_measured_state_, 0, body_index_ee_goal_)
    .matrix().block<3, 3>(0, 0);
  xyz_ee_goal_ = tree_->transformPoints(
    cache_measured_state_, xyz_ee_goal_, body_index_ee_goal_, 0);
  xyz_d_ee_goal_ = Eigen::Vector3d(msg->xyz_d_point.x,
                                   msg->xyz_d_point.y,
                                   msg->xyz_d_point.z);
  xyz_d_ee_goal_ = R*xyz_d_ee_goal_;
  quat_ee_goal_ =  Eigen::Quaterniond(msg->quaternion.w,
                                     msg->quaternion.x,
                                     msg->quaternion.y,
                                     msg->quaternion.z);
  quat_ee_goal_ =
    (drake::math::RotationMatrixd(R)*
     drake::math::RotationMatrixd(quat_ee_goal_)).ToQuaternion();

  kp_rotation_ = Eigen::Vector3d(msg->gain.rotation.x,
                                 msg->gain.rotation.y,
                                 msg->gain.rotation.z);
  kp_translation_ = Eigen::Vector3d(msg->gain.translation.x,
                                    msg->gain.translation.y,
                                    msg->gain.translation.z);
  have_goal_ = true;
  //std::cout << "Finished handling setpoint from config " << q_commanded_prev_ << std::endl;
}

} // namespace robot_plan_runner
} // namespace drake


/*
 * Aircraft.cpp
 *
 *  Created on: 11-Jul-2008
 *      Author: malem303
 */

#include "Aircraft.h"
#include "Constants.h"
#include "Utils.h"
#include "ControlRigging.h"
#include "AircraftFactory.h"

#include <math.h>
#include <boost/numeric/ublas/storage.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <list>

Aircraft::Aircraft(bool usePhi) : usePhi(usePhi)
{
	atmosphericConditions = new AtmosphericConditions();
	centerOfGravity = AircraftFactory::createCenterOfGravity(Utils::toVec3(10, 0, 5), Constants::ZERO_VEC_3, Constants::ZERO_VEC_3, Constants::ZERO_VEC_3);
    fuselage = AircraftFactory::createFuselage();
	horizontalStabilizer = AircraftFactory::createHorizontalStabilizer();
	verticalStabilizer = AircraftFactory::createVerticalStabilizer();
	mainRotor = AircraftFactory::createMainRotor();
	tailRotor = AircraftFactory::createTailRotor();
}

Aircraft::~Aircraft() {

	delete atmosphericConditions;
	delete fuselage;
	delete mainRotor;
	delete tailRotor;
	delete horizontalStabilizer;
	delete verticalStabilizer;
	delete centerOfGravity;
}
void Aircraft::updateControls(double collectivePitchStickPosition, double longitudinalSwashplateStickPosition, double lateralSwashplateStickPosition, double tailRotorCollectivePitchStick)
{
	mainRotor->setCollectivePitchStickPosition(collectivePitchStickPosition);
	mainRotor->setLongitudinalSwashplateStickPosition(longitudinalSwashplateStickPosition);
	mainRotor->setLateralSwashplateStickPosition(lateralSwashplateStickPosition);
	tailRotor->setCollectivePitchStickPosition(tailRotorCollectivePitchStick);
}

void Aircraft::updatePhysics()
{

	updateAtmosphericConditions();

	centerOfGravity->resetForces();
	centerOfGravity->updateVelocities();

	fuselage->updatePhysics(*centerOfGravity, atmosphericConditions->getDensity());

	horizontalStabilizer->updatePhysics(*centerOfGravity, atmosphericConditions->getDensity());

	verticalStabilizer->updatePhysics(*centerOfGravity, atmosphericConditions->getDensity());

	mainRotor->updatePhysics(*centerOfGravity, atmosphericConditions->getDensity());

	tailRotor->updatePhysics(*centerOfGravity, atmosphericConditions->getDensity());

	updateWashVelocities();

	centerOfGravity->addGravityForces();

}

void Aircraft::updateWashVelocities()
{
	// TODO : find a less hard-coded way to compute that

	// Mutual influences :

	double density = atmosphericConditions->getDensity();
	vector<double> externalWashAgularVelocity = Constants::ZERO_VEC_3;
	vector<double> externalWashVelocity(3);

	// mainRotor on fuselage
	externalWashVelocity = mainRotor->computeExternalWashInfluence(fuselage, density);
	fuselage->updtateWashVelocities(density, externalWashVelocity, externalWashAgularVelocity);

	// fuselage on mainRotor
	vector<double> fuselageSelfInducedWashVelocity = fuselage->computeSelfInducedWashVelocity(density);

	externalWashVelocity = 	Utils::toVec3(0.0, 0.0, fuselageSelfInducedWashVelocity(2));
	mainRotor->updtateWashVelocities(density, externalWashVelocity, externalWashAgularVelocity);

	// mainRotor and fuselage on horizontalStabilizer
	externalWashVelocity = mainRotor->computeExternalWashInfluence(horizontalStabilizer, density) + Utils::toVec3(fuselageSelfInducedWashVelocity(0), 0.0, 0.0);
	horizontalStabilizer->updtateWashVelocities(density, externalWashVelocity, externalWashAgularVelocity);

	// verticalStabilizer on tailRotor
	vector<double> verticalStabilizerSelfInducedVelocity = verticalStabilizer->computeSelfInducedWashVelocity(density);

	externalWashVelocity = 	Utils::toVec3(0.0, verticalStabilizerSelfInducedVelocity(1), 0.0);
	tailRotor->updtateWashVelocities(density, externalWashVelocity, externalWashAgularVelocity);

	// tailRotor on verticalStabilizer
	vector<double> tailRotorSelfInducedWashVelocity = tailRotor->computeSelfInducedWashVelocity(density);

	externalWashVelocity = 	Utils::toVec3(0.0, tailRotorSelfInducedWashVelocity(1), 0.0);
	verticalStabilizer->updtateWashVelocities(density, externalWashVelocity, externalWashAgularVelocity);

}

void Aircraft::updateAtmosphericConditions()
{
	// TODO: this is a stub!
	// The atmospheric condition change according to meteo (pressure, wind, temperature) and altitude, see Dreier 2007, chap 5
}

void Aircraft::printForces()
{
	using namespace std;

	cout << "----------------------------" << endl;

	cout << "fext" << centerOfGravity->getForcesIELA() << endl;
	cout << "fuse" << fuselage->getForcesIELA() << endl;
	cout << "wing" << horizontalStabilizer->getForcesIELA() << endl;
	cout << "fin " << verticalStabilizer->getForcesIELA() << endl;
	cout << "main " << mainRotor->getForcesIELA() << endl;
	cout << "tail " << tailRotor->getForcesIELA() << endl;

	cout << "----------------------------" << endl;
}

void Aircraft::printWashes()
{
	using namespace std;
	cout << "----------Wash velocities ------------------" << endl;

	cout << "CoG" << centerOfGravity->getWashVelocityIELA() << endl;
	cout << "fuse" << fuselage->getWashVelocityIELA() << endl;
	cout << "wing" << horizontalStabilizer->getWashVelocityIELA() << endl;
	cout << "fin " << verticalStabilizer->getWashVelocityIELA() << endl;
	cout << "main " << mainRotor->getWashVelocityIELA() << endl;
	cout << "tail " << tailRotor->getWashVelocityIELA() << endl;

	cout << "----------------------------" << endl;
}
vector<double> Aircraft::buildTrimVector()
{
	vector<double> trimVector(6);

	trimVector(0) = mainRotor->getCollectivePitch();
	trimVector(1) = mainRotor->getLongitudinalSwashplateAngle();
	trimVector(2) = mainRotor->getLateralSwashplateAngle();
	trimVector(3) = tailRotor->getCollectivePitch();

	vector<double> angularPositionEarthAxes = centerOfGravity->getAngularPositionEarthAxes();
	if(usePhi)
		trimVector(4) =  angularPositionEarthAxes(0);
	else
		trimVector(4) =  angularPositionEarthAxes(2);

	trimVector(5) =  angularPositionEarthAxes(1);

	return trimVector;
}

void Aircraft::applyTrimVector(vector<double> trimVector)
{
	mainRotor->setCollectivePitch(trimVector(0));
	mainRotor->setLongitudinalSwashplateAngle(trimVector(1));
	mainRotor->setLateralSwashplateAngle(trimVector(2));
	tailRotor->setCollectivePitch(trimVector(3));

	vector<double> angularPositionEarthAxes = centerOfGravity->getAngularPositionEarthAxes();
	if(usePhi)
		angularPositionEarthAxes(0) = trimVector(4);
	else
		angularPositionEarthAxes(2) = trimVector(4);

	angularPositionEarthAxes(1) = trimVector(5);

	updatePhysics();
	centerOfGravity->updateDerivatives();

}

vector<double> Aircraft::buildAccelerationVector()
{
	vector<double> invertialVelocityDerivative = centerOfGravity->getInertialVelocityDerivativeCenterOfGravity();
	vector<double> invertialAngularVelocityDerivative = centerOfGravity->getInertialAngularVelocityDerivativeCenterOfGravity();

	vector<double> accelerationVector(6);

	for(int i = 0; i < 3; i++)
	{
		accelerationVector(i) = invertialVelocityDerivative(i);
		accelerationVector(i + 3) = invertialAngularVelocityDerivative(i);
	}
	return accelerationVector;

}

double Aircraft::jacobianTrim(vector<double> initialPositionEarthAxes, vector<double> initialAngularPositionEarthAxes, vector<double> initialVelocityEarthAxes, vector<double> initialAngularVelocityEarthAxes)
{
	centerOfGravity->setPositionEarthAxes(initialPositionEarthAxes);
	centerOfGravity->setAngularPositionEarthAxes(initialAngularPositionEarthAxes);
	centerOfGravity->setVelocityEarthAxes(initialVelocityEarthAxes);
	centerOfGravity->setAngularVelocityEarthAxes(initialAngularVelocityEarthAxes);

	vector<double> currentTrimVector = buildTrimVector();
	vector<double> perturbedTrimVector = currentTrimVector;
	applyTrimVector(perturbedTrimVector);

	vector<double> currentAccelerationVector = buildAccelerationVector();
	vector<double> perturbedAccelerationVector = currentAccelerationVector;
	std::cout << "acc : " << currentAccelerationVector << std::endl;

	double accelerationVectorNorm = Utils::vectorNorm(perturbedAccelerationVector);

	matrix<double> jacobianMatrix(6,6);
	matrix<double> accelerationMatrixMinusPerturbation(6,6);
	matrix<double> accelerationMatrixPlusPerturbation(6,6);

	int numberOfIterationLeft = 20;
	const double PERTURBATION_SIZE = 0.01;
	vector<double> trimGradient = Constants::ZERO_VEC_3;

	while (accelerationVectorNorm > 0.001 && numberOfIterationLeft > 0)
	{

		for(int jacobianRowIndex = 0; jacobianRowIndex < 6; jacobianRowIndex++)
		{
			for(int jacobianColumnIndex = 0; jacobianColumnIndex < 6; jacobianColumnIndex++)
			{
				perturbedTrimVector(jacobianColumnIndex) = currentTrimVector(jacobianColumnIndex) + PERTURBATION_SIZE;
				applyTrimVector(perturbedTrimVector);
				printForces();
				printWashes();
				perturbedAccelerationVector = buildAccelerationVector();
				accelerationMatrixMinusPerturbation(jacobianRowIndex, jacobianColumnIndex) = perturbedAccelerationVector(jacobianRowIndex);

				perturbedTrimVector(jacobianColumnIndex) = currentTrimVector(jacobianColumnIndex) - PERTURBATION_SIZE;
				applyTrimVector(perturbedTrimVector);
				printForces();
				printWashes();
				perturbedAccelerationVector = buildAccelerationVector();
				accelerationMatrixPlusPerturbation(jacobianRowIndex, jacobianColumnIndex) = perturbedAccelerationVector(jacobianRowIndex);

//				applyTrimVector(currentTrimVector);
				perturbedTrimVector = currentTrimVector;

			}
		}

		jacobianMatrix = (accelerationMatrixPlusPerturbation - accelerationMatrixMinusPerturbation) / (2 * PERTURBATION_SIZE);

		using namespace std;

//		cout << "jac : " << jacobianMatrix << endl;

		trimGradient = prod(Utils::invertMatrix(jacobianMatrix), currentAccelerationVector);

		currentTrimVector -= trimGradient;

		applyTrimVector(currentTrimVector);
		currentAccelerationVector = buildAccelerationVector();

		cout << "grad : " << trimGradient << endl;
		cout << "trim : " << perturbedTrimVector << endl;
		cout << "acc : " << currentAccelerationVector << endl;

		numberOfIterationLeft--;
		accelerationVectorNorm = Utils::vectorNorm(currentAccelerationVector);

		cout << "norm = " << accelerationVectorNorm << endl;

	}

	return accelerationVectorNorm;
}


/*
Copyright Benjamin Perret ESIEE Paris 2017

benjamin.perret@esiee.fr

This software is a computer program whose purpose is to compute isotonic/
monotonic regression on tree ordering.

This software is governed by the CeCILL-B license under French law and
abiding by the rules of distribution of free software.  You can  use,
modify and/ or redistribute the software under the terms of the CeCILL
license as circulated by CEA, CNRS and INRIA at the following URL
"http://www.cecill.info".

As a counterpart to the access to the source code and  rights to copy,
modify and redistribute granted by the license, users are provided only
with a limited warranty  and the software's author,  the holder of the
economic rights,  and the successive licensors  have only  limited
liability.

In this respect, the user's attention is drawn to the risks associated
with loading,  using,  modifying and/or developing or reproducing the
software by the user in light of its specific status of free software,
that may mean  that it is complicated to manipulate,  and  that  also
therefore means  that it is reserved for developers  and  experienced
professionals having in-depth computer knowledge. Users are therefore
encouraged to load and test the software's suitability as regards their
requirements in conditions enabling the security of their systems and/or
data to be ensured and,  more generally, to use and operate it in the
same conditions as regards security.

The fact that you are presently reading this means that you have had
knowledge of the CeCILL-B license and that you accept its terms.

 */

#include <vector>
#include <boost/heap/binomial_heap.hpp>

#include "morto.h"

namespace morto {

	using namespace std;

	void testOrDie(bool b, string msg)
	{
		if (!b)
			throw MortoException{ msg };
	}

	/**
	 * Generic UnionFind with path compression and union by rank on integer indicis.
	 */
	class UnionFind {
	private:
		vector<size_t> parent;
		vector<int> rank;

	public:
		/**
		Create a new family of size singleton sets.
		*/
		UnionFind(size_t size = 0)
		{
			if (size > 0)
			{
				for (size_t i = 0; i < size; ++i)
					parent.push_back(i);
				rank.resize(size, 0);
			}
		}

		/**
		Create a new singleton and return the index of its (single and canonical) node.
		Complexity O(1) (amortized)
		*/
		/*
		size_t createSet()
		{
			size_t size = parent.size();
			parent.push_back(size);
			rank.push_back(0);
			return size;
		}
		*/

		/**
		Finds and return the canonical node of the given nodeIndex and performs path compression.
		Complexity O(a(n)) (amortized)
		*/
		size_t findCanonical(size_t nodeIndex)
		{
			size_t save = nodeIndex;
			while (parent[nodeIndex] != nodeIndex)
				nodeIndex = parent[nodeIndex];
			while (parent[save] != save)
			{
				size_t tmp = save;
				save = parent[save];
				parent[tmp] = nodeIndex;
			}
			return nodeIndex;
		}

		/**
		Performs the union of two sets (given by their canonical nodes) with rank optimisation.
		Returns the index of the canonical node of the union.
		Complexity O(1)
		*/
		auto setUnion(size_t canonicalNodeIndex1, size_t canonicalNodeIndex2)
		{
			if (rank[canonicalNodeIndex1] > rank[canonicalNodeIndex2])
			{
				swap(canonicalNodeIndex1, canonicalNodeIndex2);
			}
			else if (rank[canonicalNodeIndex1] == rank[canonicalNodeIndex2])
			{
				rank[canonicalNodeIndex2]++;
			}
			parent[canonicalNodeIndex1] = canonicalNodeIndex2;
			return make_pair(canonicalNodeIndex2, canonicalNodeIndex1);
		}
	};

	//forward declaration
	struct HeapNode;

	using heap_t = boost::heap::binomial_heap<HeapNode>;

	struct HeapNode {
		double value;
		size_t nodeIndex;
	};

	bool operator<(const HeapNode & lo, const HeapNode & ro)
	{
		return lo.value < ro.value;
	}

	/**
	* Internal tree node, merges information on nodes, blocks and heaps...
	*/
	struct TreeNode {
		/**
		 * Index of the parent node
		 */
		size_t parent = 0;

		/**
		 * Weight
		 */
		double weight = 1;

		/**
		 * Input value
		 */
		double value = 0;

		/**
		 * Handle on the HeapNode associated to this node in the heap of its parent
		 */
		heap_t::handle_type handle;

		/**
		 * Heap of HeapNodes associated to every child (only valid if the node is the canonical node associated to its block)
		 */
		heap_t heap;

		/**
		 * Sum of weights of the nodes in the block (only valid if the node is the canonical node associated to its block)
		 */
		double blockTotalWeight = 1;

		/**
		* Weighted sum of the nodes in the block (only valid if the node is the canonical node associated to its block)
		*/
		double blockWeightedSum = 0;

		/**
		 * \brief Average weight of the block (only valid if the node is the canonical node associated to its block)
		 * \return Average weight of the block
		 */
		double getAverageWeight() const {
			return blockWeightedSum / blockTotalWeight;
		}

		void mergeWith(TreeNode * node)
		{
			heap.merge(node->heap);
			blockTotalWeight += node->blockTotalWeight;
			blockWeightedSum += node->blockWeightedSum;

		}
	};

	/**
	 * Internal class to represent the tree.
	 * Essentially a constructor.
	 */
	class Tree {
	private:
		vector<TreeNode> nodes;

	public:
		/**
		 * \brief Create a new tree from agnostic representation.
		 * \param parents Parent relation between nodes.
		 * Preconditions :
		 *		- parents.size() > 0
		 *		- "node i is the parent of node j" <=> parents[j] == i
		 *		- "node i is the root" <=> parents[i] == i
		 *		- nodes are in topological order : parents[i] >= i
		 * \param values Input data associated to nodes.
		 * The observed value at node i is values[i].
		 * Preconditions :
		 *		- values.size() == parents.size()
		 * \param weights Weights associated to nodes.
		 * If weights.size() == 0 , every node is assumed to have the same weight.
		 * Else, the weight of node i is weights[i].
		 * Preconditions :
		 *		- weights.size() == parents.size() || weights.empty()
		 */
		Tree(const vector<size_t> & parents, const vector<double> & values, const vector<double> & weights)
		{

			size_t nnodes = parents.size();
			nodes.resize(nnodes);

			if (!weights.empty())
			{
				for (size_t i = 0; i < nnodes; ++i)
				{
					nodes[i].weight = weights[i];
					nodes[i].blockTotalWeight = weights[i];
				}
			}

			for (size_t i = 0; i < nnodes; ++i)
			{
				nodes[i].parent = parents[i];
				nodes[i].value = values[i];
				nodes[i].blockWeightedSum = nodes[i].weight * nodes[i].value;
				if (parents[i] != i)
				{
					//nodes[parents[i]].children.push_back(i);
					nodes[i].handle = nodes[parents[i]].heap.push({ values[i], i });
				}

			}
		}

		/**
		 * \brief Get the node of index i
		 * \param i A valid node index (Precondition 0 <= i < tree.size())
		 * \return A reference to node i
		 */
		TreeNode * getNode(size_t i)
		{
			return &nodes[i];
		}

		/**
		 * \brief Number of nodes in the tree
		 * \return Number of nodes in the tree
		 */
		size_t size() const
		{
			return nodes.size();
		}
	};

	/**
	 * \brief Compute isotonic regression on the given tree.
	 * \param tree Input Tree, properly initialized.
	 * \return vector<double> containing the regression values for each node
	 */
	vector<double> IRT_BIN(Tree & tree)
	{
		size_t nnodes = tree.size();

		UnionFind uf(nnodes); // Block maintenance

		for (size_t i = 0; i < nnodes; ++i) // from leaves to root
		{

			size_t canonicali = uf.findCanonical(i); // index of the representative tree node for the block containing node i
			TreeNode * canonicalNode = tree.getNode(canonicali);

			// while we have violators among our children, fuse current block with the block of the most important violator
			while (!canonicalNode->heap.empty() && canonicalNode->getAverageWeight() < canonicalNode->heap.top().value)
			{
				size_t k = canonicalNode->heap.top().nodeIndex; // index of violator child k
				canonicalNode->heap.pop();

				size_t canonicalk = uf.findCanonical(k); // index of the representative tree node for the block containing node k

				auto resUnion = uf.setUnion(canonicali, canonicalk); // merge blocks containing i and k
				auto newCanonicalIndex = resUnion.first; // index of the new representative node (either i or k)
				auto otherIndex = resUnion.second; // index of the node that is not representative (either i or k)

				// update local variable after merge
				canonicalNode = tree.getNode(newCanonicalIndex);
				canonicali = newCanonicalIndex;

				// merge block information
				TreeNode * otherNode = tree.getNode(otherIndex);
				canonicalNode->mergeWith(otherNode);

			}

			// update parent heap to reflect the new weight of the block containing node i
			TreeNode * currentNode = tree.getNode(i);
			if (currentNode->parent != i)
			{
				TreeNode * node = tree.getNode(currentNode->parent);
				node->heap.update(currentNode->handle, { canonicalNode->getAverageWeight(), i });
			}
		}

		vector<double> res;
		for (size_t i = 0; i < nnodes; ++i)
		{
			res.push_back(tree.getNode(uf.findCanonical(i))->getAverageWeight());
		}
		return res;
	}

	bool testRelationDomain(const vector<size_t> & parents)
	{
		size_t maxV = parents.size() - 1;
		for (size_t i = 0; i < parents.size(); ++i)
		{
			if (parents[i]<0 || parents[i]>maxV) // size_t should be unsigned but well
				return false;
		}
		return true;
	}

	bool testTopologicalOrder(const vector<size_t> & parents)
	{
		for (size_t i = 0; i < parents.size(); ++i)
		{
			if (parents[i] < i)
				return false;
		}
		return true;
	}

    bool testPositiveWeights(const vector<double> & weights)
    {
        for (auto v: weights)
            if(v<=0)
                return false;
        return true;
    }
	/*bool testSingleRoot(const vector<int> & parents)
	{
		int nparents = 0;
		for (int i = 0; i<parents.size(); ++i)
		{
			if (parents[i] == i)
			{
				nparents++;
				if (nparents > 1)
					return false;
			}
		}
		return true;
	}*/

	vector<double> monotonicRegressionOnTree(const vector<size_t> & parents, const vector<double> & values, const vector<double> & weights)
	{
		testOrDie(parents.size() > 0, "Size of parents must be strictily greater than 0.");
		testOrDie(values.size() == parents.size(), "values and parents must have the same size.");
		testOrDie(weights.empty() || weights.size() == parents.size(), "weights must either be empty or have the same size as parents.");
		testOrDie(testRelationDomain(parents), "The domain of the parents relation is invalid: there exists i such that parents[i]<0 or parents[i]>=parents.size()");
		testOrDie(testTopologicalOrder(parents), "The parent relation is not given in a topological order: there exists i such that parents[i]<i");
		if(weights.size()>0)
			testOrDie(testPositiveWeights(weights),"Weights must be strictly positive.");
        //testOrDie(testSingleRoot(parents), "The parent relationcontains more than one root : there exists several i such that parents[i]==i");

		Tree tree = Tree(parents, values, weights);
		return IRT_BIN(tree);
	}

} // namespace morto
/*

using namespace morto;
using namespace std;

template<typename T>
ostream & operator<<(ostream & o, const vector<T> & vec)
{
	o << "{";
	for (size_t i = 0; i < vec.size() - 1; ++i)
		o << vec[i] << ", ";
	if (!vec.empty())
		o << vec.back();
	o << "}";
	return o;
}

void testIncreasing()
{
	vector<size_t> parents = { 4,4,6,6,7,7,7,7 };
	//vector<double> values = { 0,1,2,3,4,5,6,7 };
	vector<double> values = { 13,14,6,8,11,7,5,10 };
	vector<double> result = monotonicRegressionOnTree(parents, values);
	cout << result << endl;
}

int main(int argc, char ** argv)
{
	testIncreasing();
	getchar();
	return 0;
}
*/


//
//  yael_io.h
//  OLNN
//
//  Created by jimmy on 2016-12-01.
//  Copyright (c) 2016 Nowhere Planet. All rights reserved.
//

#ifndef OLNN_yael_io_h
#define OLNN_yael_io_h

// file IO for http://corpus-texmex.irisa.fr/

#include <stdio.h>
#include <Eigen/Dense>
#include <assert.h>

using Eigen::MatrixXf;
using Eigen::Matrix;

class YaelIO
{
public:
    // n: number
    // d: dimension
    static bool read_fvecs_file(const char *file_name,
                                Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & data);
    static bool read_ivecs_file(const char *file_name,
                                Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & data);

    // .fvecs
    static bool write_fvecs_file(const char *file_name,
                                 const Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & data);
    static bool write_ivecs_file(const char *file_name,
                                 const Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & data);

private:
    static float *fvec_new (long n);
    static int *ivec_new(long n);

    static int fvecs_read(const char *fname, int d, int n, float *v);
    static int ivecs_read(const char *fname, int d, int n, int *v);

    /*!  Read the number of vectors in a file and their dimension
     (vectors of same size). Output the number of bytes of the file. */
    static long fvecs_fsize (const char * fname, int *d_out, int *n_out);
    static long ivecs_fsize (const char * fname, int *d_out, int *n_out);
    static long bvecs_fsize (const char * fname, int *d_out, int *n_out);
    static long lvecs_fsize (const char * fname, int *d_out, int *n_out);

    /*!  write a set of vectors into an open file */
    static int ivecs_fwrite(FILE *f, int d, int n, const int *v);
    static int fvecs_fwrite(FILE *fo, int d, int n, const float *vf);

    /*!  write a vector into an open file */
    static int ivec_fwrite(FILE *f, const int *v, int d);
    static int fvec_fwrite(FILE *f, const float *v, int d);
};

class YaelIOUtil
{
public:
    static void load_1k_dataset(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & base_data,
                                Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & learn_data,
                                Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & query_data,
                                Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & ground_truth);

    static void load_1k_reorder_dataset(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & base_data,
                                        Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & learn_data,
                                        Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & query_data,
                                        Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & ground_truth);

    static void load_10k_dataset(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & base_data,
                                 Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & learn_data,
                                 Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & query_data,
                                 Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & ground_truth);

    static void load_10k_32dim_dataset(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & base_data,
                                 Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & learn_data,
                                 Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & query_data,
                                 Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & ground_truth);

    static void load_100k_dataset(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & base_data,
                                 Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & learn_data,
                                 Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & query_data,
                                 Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & ground_truth);

    static void load_1m_dataset(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & base_data,
                                 Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & learn_data,
                                 Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & query_data,
                                 Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & ground_truth);

};

#endif


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2020, KU Leuven
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of KU Leuven nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Jeroen De Maeyer */

/** This file checks if the ConstrainedPlanningStateSpace can properly copy OMPL States of type
 * ompl::base::ConstrainedStateSpace::StateType into MoveIt's robot state.
 **/

#include <moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space.h>

#include <memory>
#include <string>
#include <iostream>

#include <gtest/gtest.h>
#include <Eigen/Dense>

#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit_msgs/Constraints.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/utils/robot_model_test_utils.h>

#include <ompl/util/Exception.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/base/spaces/constraint/ProjectedStateSpace.h>

#include "load_test_robot.h"

constexpr char LOGNAME[] = "test_constrained_state_validity_checker";

/** \brief Dummy constraint for testing, always satisfied. We need this to create and OMPL ConstrainedStateSpace. **/
class DummyConstraint : public ompl::base::Constraint
{
public:
  DummyConstraint(const unsigned int num_dofs) : ompl::base::Constraint(num_dofs, 1)
  {
  }
  void function(const Eigen::Ref<const Eigen::VectorXd>& /*unused*/, Eigen::Ref<Eigen::VectorXd> out) const override
  {
    out[0] = 0.0;
  }
};

/** \brief Robot indepentent implementation of the tests.  **/
class TestConstrainedStateSpace : public ompl_interface_testing::LoadTestRobot, public testing::Test
{
protected:
  TestConstrainedStateSpace(const std::string& robot_name, const std::string& group_name)
    : LoadTestRobot(robot_name, group_name)
  {
  }

  void SetUp() override
  {
    // create a constrained state space for testing
    setupMoveItStateSpace();
    setupOMPLStateSpace();
  };

  void TearDown() override
  {
  }

  void setupMoveItStateSpace()
  {
    ompl_interface::ModelBasedStateSpaceSpecification space_spec(robot_model_, group_name_);
    moveit_state_space_ = std::make_shared<ompl_interface::ConstrainedPlanningStateSpace>(space_spec);
  }

  void setupOMPLStateSpace()
  {
    // first call setupMoveItStateSpace()
    assert(moveit_state_space_ != nullptr);
    auto con = std::make_shared<DummyConstraint>(num_dofs_);
    constrained_state_space_ = std::make_shared<ompl::base::ProjectedStateSpace>(moveit_state_space_, con);
  }

  void testGetValueAddressAtIndex()
  {
    Eigen::VectorXd joint_positions = getDeterministicState();
    ompl::base::ScopedState<> ompl_state(constrained_state_space_);
    auto state_ptr = ompl_state->as<ompl::base::ConstrainedStateSpace::StateType>()->getState();
    double* out_joint_positions = dynamic_cast<ompl_interface::ModelBasedStateSpace::StateType*>(state_ptr)->values;
    EXPECT_FALSE(out_joint_positions == nullptr);
    for (std::size_t i = 0; i < num_dofs_; ++i)
    {
      *(out_joint_positions + i) = joint_positions[i];
    }

    // check getValueAddressAtIndex
    // it can only be called with an already unwrapped state,
    // this unwrapping is either done in the constrained_state_space_ (see WrapperStateSpace in OMPL),
    // or in copyJointToOMPLState in the implementation of ConstrainedPlanningStateSpace in MoveIt.
    for (std::size_t i = 0; i < num_dofs_; ++i)
    {
      EXPECT_EQ(joint_positions[i], *(constrained_state_space_->getValueAddressAtIndex(ompl_state.get(), i)));
    }
  }

  void testCopyToRobotState()
  {
    // create and OMPL state
    // The copy operation of the ConstraintStateSpace::StateType show below is not supported
    // ompl_state->as<ompl::base::ConstrainedStateSpace::StateType>()->copy(joint_positions);
    // Because the state spaces implemented in MoveIt do not support casting to Eigen::VectorXd
    Eigen::VectorXd joint_positions = getDeterministicState();
    ompl::base::ScopedState<> ompl_state(constrained_state_space_);
    auto state_ptr = ompl_state->as<ompl::base::ConstrainedStateSpace::StateType>()->getState();
    double* out_joint_positions = dynamic_cast<ompl_interface::ModelBasedStateSpace::StateType*>(state_ptr)->values;
    EXPECT_FALSE(out_joint_positions == nullptr);
    for (std::size_t i = 0; i < num_dofs_; ++i)
    {
      *(out_joint_positions + i) = joint_positions[i];
    }

    // copy into a MoveIt state
    moveit::core::RobotState moveit_state(robot_model_);
    moveit_state.setToDefaultValues();
    moveit_state_space_->copyToRobotState(moveit_state, ompl_state.get());

    // check if copy worked out as expected
    Eigen::VectorXd out_joint_position(num_dofs_);
    moveit_state.copyJointGroupPositions(joint_model_group_, out_joint_position);

    EXPECT_EQ(joint_positions.size(), out_joint_position.size());
    for (std::size_t i = 0; i < num_dofs_; ++i)
    {
      EXPECT_EQ(joint_positions[i], out_joint_position[i]);
    }
  }

  void testCopyToOMPLState()
  {
    // create a MoveIt state
    Eigen::VectorXd joint_positions = getDeterministicState();
    moveit::core::RobotState moveit_state(robot_model_);
    moveit_state.setToDefaultValues();
    moveit_state.setJointGroupPositions(joint_model_group_, joint_positions);

    // copy into an OMPL state
    ompl::base::ScopedState<> ompl_state(constrained_state_space_);
    moveit_state_space_->copyToOMPLState(ompl_state.get(), moveit_state);

    // check if copy worked out as expected
    // (Again, support for casting to Eigen::VectorXd would have been nice here.)
    auto state_ptr = ompl_state->as<ompl::base::ConstrainedStateSpace::StateType>()->getState();
    double* out_joint_positions = dynamic_cast<ompl_interface::ModelBasedStateSpace::StateType*>(state_ptr)->values;
    EXPECT_FALSE(out_joint_positions == nullptr);
    for (std::size_t i = 0; i < num_dofs_; ++i)
    {
      EXPECT_EQ(joint_positions[i], *(out_joint_positions + i));
    }
  }

  void testCopyJointToOMPLState()
  {
    EXPECT_TRUE(true);
    // create a MoveIt state
    Eigen::VectorXd joint_positions = getDeterministicState();
    moveit::core::RobotState moveit_state(robot_model_);
    moveit_state.setToDefaultValues();
    moveit_state.setJointGroupPositions(joint_model_group_, joint_positions);

    // copy into an OMPL state, one index at a time
    ompl::base::ScopedState<> ompl_state(constrained_state_space_);
    auto joint_model_names = joint_model_group_->getActiveJointModelNames();
    for (std::size_t joint_index = 0; joint_index < num_dofs_; ++joint_index)
    {
      const moveit::core::JointModel* joint_model = joint_model_group_->getJointModel(joint_model_names[joint_index]);
      EXPECT_FALSE(joint_model == nullptr);

      ROS_DEBUG_STREAM_NAMED(LOGNAME, "Joint model: " << joint_model->getName() << " index: " << joint_index);
      ROS_DEBUG_STREAM_NAMED(LOGNAME, "first index: " << joint_model->getFirstVariableIndex() * sizeof(double));
      ROS_DEBUG_STREAM_NAMED(LOGNAME, "width: " << joint_model->getVariableCount() * sizeof(double));

      moveit_state_space_->copyJointToOMPLState(ompl_state.get(), moveit_state, joint_model, joint_index);
    }

    // check if copy worked out as expected
    auto state_ptr = ompl_state->as<ompl::base::ConstrainedStateSpace::StateType>()->getState();
    double* out_joint_positions = dynamic_cast<ompl_interface::ModelBasedStateSpace::StateType*>(state_ptr)->values;
    EXPECT_FALSE(out_joint_positions == nullptr);
    for (std::size_t i = 0; i < num_dofs_; ++i)
    {
      EXPECT_EQ(joint_positions[i], *(out_joint_positions + i));
    }
  }

protected:
  ompl::base::ConstrainedStateSpacePtr constrained_state_space_;
  ompl_interface::ConstrainedPlanningStateSpacePtr moveit_state_space_;
};

// /***************************************************************************
//  * Run all tests on the Panda robot
//  * ************************************************************************/
class PandaCopyStateTest : public TestConstrainedStateSpace
{
protected:
  PandaCopyStateTest() : TestConstrainedStateSpace("panda", "panda_arm")
  {
  }
};

TEST_F(PandaCopyStateTest, testGetValueAddressAtIndex)
{
  testGetValueAddressAtIndex();
}

TEST_F(PandaCopyStateTest, testCopyToRobotState)
{
  testCopyToRobotState();
}

TEST_F(PandaCopyStateTest, testCopyToOMPLState)
{
  testCopyToOMPLState();
}

TEST_F(PandaCopyStateTest, testCopyJointToOMPLState)
{
  testCopyJointToOMPLState();
}

/***************************************************************************
 * Run all tests on the Fanuc robot
 * ************************************************************************/
class FanucCopyStateTest : public TestConstrainedStateSpace
{
protected:
  FanucCopyStateTest() : TestConstrainedStateSpace("fanuc", "manipulator")
  {
  }
};

TEST_F(FanucCopyStateTest, testGetValueAddressAtIndex)
{
  testGetValueAddressAtIndex();
}

TEST_F(FanucCopyStateTest, testCopyToRobotState)
{
  testCopyToRobotState();
}

TEST_F(FanucCopyStateTest, testCopyToOMPLState)
{
  testCopyToOMPLState();
}

TEST_F(FanucCopyStateTest, testCopyJointToOMPLState)
{
  testCopyJointToOMPLState();
}

// /***************************************************************************
//  * Run all tests on the PR2 robot its left arm
//  * ************************************************************************/
class PR2CopyStateTest : public TestConstrainedStateSpace
{
protected:
  PR2CopyStateTest() : TestConstrainedStateSpace("pr2", "left_arm")
  {
  }
};

TEST_F(PR2CopyStateTest, testGetValueAddressAtIndex)
{
  testGetValueAddressAtIndex();
}

TEST_F(PR2CopyStateTest, testCopyToRobotState)
{
  testCopyToRobotState();
}

TEST_F(PR2CopyStateTest, testCopyToOMPLState)
{
  testCopyToOMPLState();
}

TEST_F(PR2CopyStateTest, testCopyJointToOMPLState)
{
  testCopyJointToOMPLState();
}

/***************************************************************************
 * MAIN
 * ************************************************************************/
int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


/**
 * Taken with inspiration from: gazebosim.org/tutorials?tut=plugins_model
 * www.answer.gazebosim.org/question/2341/set-and-get-position-of-gazebo-model-using-ros-plugin/
 */

#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>

#include <thread>
#include "ros/ros.h"
#include "ros/callback_queue.h"
#include "ros/subscribe_options.h"
#include "std_msgs/Float32.h"
#include "geometry_msgs/Point.h"
#include "std_msgs/Float64.h"
#include "geometry_msgs/Quaternion.h"

//#include <boost/bind.hpp>
//#include <gazebo/common/common.hh>
//#include <gazebo/common/Plugin.hh>
//#include <stdio.h>

namespace gazebo {
	class QuadModel : public ModelPlugin
	{
		public: QuadModel() {}

		public: virtual void Load(physics::ModelPtr _model,  sdf::ElementPtr _sdf)
		{
			std::cerr << "Model Name is: [" << _model->GetName() << "]\n";
			printf("Loading Plugin!\n\r");
			if (!ros::isInitialized())
			{
				int argc = 0;
				char **argv = NULL;
				printf("Starting ROS!\n\r");
				ros::init(argc, argv, "gazebo_client", ros::init_options::NoSigintHandler);
				printf("ROS start success...\n\r");
			}

			this->model = _model;
			std::cerr << "Model Name is: " + this->model->GetName() << "\n";

			this->rosNode.reset(new ros::NodeHandle("gazebo_client"));
			ros::SubscribeOptions so =
                ros::SubscribeOptions::create<geometry_msgs::Point>("/" + this->model->GetName() + "/position_O",
                1, boost::bind(&QuadModel::OnRosMsg, this, _1),
                ros::VoidPtr(), &this->rosQueue);

			ros::SubscribeOptions soQuat = ros::SubscribeOptions::create<geometry_msgs::Quaternion>("/" + this->model->GetName() + "/quaternion_O", 1, boost::bind(&QuadModel::OnRosMsgQuat, this, _1), ros::VoidPtr(), &this->rosQueue);
			this->rosSub = this->rosNode->subscribe(so);
            this->rosSubQuat = this->rosNode->subscribe(soQuat);

			this->rosQueueThread = std::thread(std::bind(&QuadModel::QueueThread, this));
		}

		private: std::unique_ptr<ros::NodeHandle> rosNode;
		private: ros::Subscriber rosSub;    /** Subscriber for Point Topic */
        private: ros::Subscriber rosSubQuat;/** Subscriber for Quat Topic */
		private: ros::CallbackQueue rosQueue;
		private: std::thread rosQueueThread;
		private: physics::ModelPtr model;
        private: double quatX;
        private: double quatY;
        private: double quatZ;
        private: double quatW;

		public: void OnRosMsgQuat(const geometry_msgs::QuaternionConstPtr &_msg)
        {
			quatX = _msg->x;
			quatY = _msg->y;
			quatZ = _msg->z;
			quatW = _msg->z;
        }

		public: void OnRosMsg(const geometry_msgs::PointConstPtr &_msg)
		{
			double x = _msg->x;
			double y = _msg->y;
			double z = _msg->z;

            math::Vector3 vect = math::Vector3(x, y, z);

            /* We make the assumption that the messages are in sync most of the time, as the queue limit in Simulink is 1. */
            math::Quaternion quat = math::Quaternion(quatW, quatX, quatY, quatZ);
			math::Pose pose = math::Pose(vect, quat);
            std::cerr << x << ", " << y << ", " << z << ", | " << quatW << ", " << quatX << ", " << quatY << "\n";
			this->model->SetWorldPose(pose);
		}

		private: void QueueThread()
		{
			static const double timeout = 0.01;
			while (this->rosNode->ok()) {
				this->rosQueue.callAvailable(ros::WallDuration(timeout));
			}
		}

	};
	GZ_REGISTER_MODEL_PLUGIN(QuadModel)
}


//****************************************************************
// Copyright MIT Lincoln Laboratory
// Project:       SPAR
// Authors:       Yang
// Description:   Unit tests for circuit classes
//
// Modifications:
// Date          Name           Modification
// ----          ----           ------------
// 29 May 2012   yang           Original Version
// 09 Aug 2012   yang           Added unit tests for XOR
//****************************************************************

#define BOOST_TEST_MODULE GatesTest

#include <bitset>
#include <vector>
#include <string>
#include <math.h>

#include <boost/shared_ptr.hpp>

#include "common/logging.h"
#include "common/test-init.h"
#include "common/check.h"

#include "stealth-circuit-gates.h"

// Helper method for testing truth tables of single gates. Allocates wires and
// assigns them to the specified MultiInputGate being tested.
void InitializeLines(int num_inputs,
                     MultiGatePtr gate, std::vector<WirePtr>* wires) {
  for (int i = 0; i < num_inputs; ++i) {
    WirePtr wire(new Wire());
    wires->push_back(wire);
    gate->AddInput(wire);
  }
}

// Checks to see if Evaluate() of ConstGate produces the correct output
BOOST_AUTO_TEST_CASE(ConstGateEvaluation) {
  Wire one;
  one.SetValue(1);
  BOOST_CHECK_EQUAL(one.Evaluate(false), 1);

  Wire zero;
  zero.SetValue(0);
  BOOST_CHECK_EQUAL(zero.Evaluate(false), 0);
}

// Checks to see if Evaluate() of Not produces the correct output
BOOST_AUTO_TEST_CASE(NotEvaluation) {
  WirePtr one(new Wire());
  one->SetValue(1);
  GatePtr output_gate(new Not(one));
  BOOST_CHECK_EQUAL(output_gate->Evaluate(true), 0);

  WirePtr zero(new Wire());
  zero->SetValue(0);
  GatePtr output_gate2(new Not(zero));
  BOOST_CHECK_EQUAL(output_gate2->Evaluate(true), 1);
}

// Checks to see if Evaluate() of And produce the correct output
BOOST_AUTO_TEST_CASE(AndEvaluation) {
  const int kNumInputs = 3;
  boost::shared_ptr<And> output_gate(new And());
  std::vector<WirePtr> wires;
  InitializeLines(kNumInputs, output_gate, &wires);

  // Test the evaluate method on all possible combinations
  // of binary inputs of size kNumInputs.
  for (int i = 0; i < pow(2, kNumInputs); ++i) {
    std::bitset<kNumInputs> inputs(i);
    // Set all wires to their respective values in the bitset.
    for (unsigned int j = 0; j < wires.size(); ++j) {
      wires[j]->SetValue(inputs[j]);
    }
    BOOST_CHECK_EQUAL(output_gate->Evaluate(true), inputs.all());
    output_gate->Reset();
    BOOST_CHECK_EQUAL(output_gate->Evaluate(false), inputs.all());
    output_gate->Reset();
  }
}

// Checks to see if Evaluate of Or produces the correct output
BOOST_AUTO_TEST_CASE(OrWorks) {
  const int kNumInputs = 5;
  boost::shared_ptr<Or> output_gate(new Or());
  std::vector<WirePtr> wires;
  InitializeLines(kNumInputs, output_gate, &wires);

  // Test the evaluate method on all possible combinations
  // of binary inputs of size kNumInputs.
  for (int i = 0; i < pow(2, kNumInputs); ++i) {
    std::bitset<kNumInputs> inputs(i);
    for (unsigned int j = 0; j < wires.size(); ++j) {
       wires[j]->SetValue(inputs[j]);
    }
    BOOST_CHECK_EQUAL(output_gate->Evaluate(true), inputs.any());
    output_gate->Reset();
    BOOST_CHECK_EQUAL(output_gate->Evaluate(false), inputs.any());
    output_gate->Reset();
  }
}

// Checks to see if Evaluate() of Xor produces the correct output
BOOST_AUTO_TEST_CASE(XorWorks) {
  const int kNumInputs = 5;
  boost::shared_ptr<Xor> output_gate(new Xor());
  std::vector<WirePtr> wires;
  InitializeLines(kNumInputs, output_gate, &wires);

  // Test the evaluate method on all possible combinations
  // of binary inputs of size kNumInputs.
  for (int i = 0; i < pow(2, kNumInputs); ++i) {
    std::bitset<kNumInputs> inputs(i);
    for (unsigned int j = 0; j < wires.size(); ++j) {
      wires[j]->SetValue(inputs[j]);
    }
    BOOST_CHECK_EQUAL(output_gate->Evaluate(true), inputs.count() % 2);
    output_gate->Reset();
    BOOST_CHECK_EQUAL(output_gate->Evaluate(false), inputs.count() % 2);
    output_gate->Reset();
  }
}

// Checks to see if a circuit of multiple levels produces the correct output.
// Wires are also attached to multiple circuits to check for memory management
// when testing with --valgrind enabled.
BOOST_AUTO_TEST_CASE(SimpleCircuit) {
  boost::shared_ptr<Xor> output_gate(new Xor());
  boost::shared_ptr<And> gate1(new And());
  boost::shared_ptr<Or> gate2(new Or());
  boost::shared_ptr<Not> not1(new Not(gate1));
  output_gate->AddInput(not1);
  output_gate->AddInput(gate2);

  WirePtr in1(new Wire());
  WirePtr in2(new Wire());

  gate1->AddInput(in1);
  gate1->AddInput(in2);
  gate2->AddInput(in1);
  gate2->AddInput(in2);

  in1->SetValue(0);
  in2->SetValue(0);

  BOOST_CHECK_EQUAL(output_gate->Evaluate(true), 1);
  output_gate->Reset();

  in1->SetValue(1);
  in2->SetValue(0);

  BOOST_CHECK_EQUAL(output_gate->Evaluate(false), 0);
}


#include <boost/hana/functional/curry.hpp>


/*******************************************************************************
Copyright(c) 2015-2021 Parker Hannifin Corp. All rights reserved.

MIT Licensed. See the included LICENSE.txt for a copy of the full MIT License.
*******************************************************************************/
#include "mscl/MicroStrain/SampleRate.h"
#include "mscl/Exceptions.h"

#include <boost/test/unit_test.hpp>

using namespace mscl;

BOOST_AUTO_TEST_SUITE(SampleRate_Test)

BOOST_AUTO_TEST_CASE(SampleRate_DefaultConstructor)
{
    //build a default sample rate (1 Hz)
    SampleRate sr;

    //check that the string is "1Hz"
    BOOST_CHECK_EQUAL(sr.str(), "1-hertz");
    BOOST_CHECK_EQUAL(sr.prettyStr(), "1Hz");

    //check that the TimeSpan is 1 second
    BOOST_CHECK_EQUAL(sr.samplePeriod().getNanoseconds(), 1000000000);

    //check that the rate type is hertz
    BOOST_CHECK_EQUAL(sr.rateType(), SampleRate::rateType_hertz);

    //check that the number of samples is 1
    BOOST_CHECK_EQUAL(sr.samples(), 1);
}

BOOST_AUTO_TEST_CASE(SampleRate_Constructor_Hz)
{
    //build a sample rate of 256 Hz
    SampleRate sr(SampleRate::rateType_hertz, 256);

    //check that the string is "256Hz"
    BOOST_CHECK_EQUAL(sr.str(), "256-hertz");
    BOOST_CHECK_EQUAL(sr.prettyStr(), "256Hz");

    //check that the TimeSpan is the correct number of nanoseconds
    BOOST_CHECK_EQUAL(sr.samplePeriod().getNanoseconds(), 3906250);

    //check that the rate type is hertz
    BOOST_CHECK_EQUAL(sr.rateType(), SampleRate::rateType_hertz);

    //check that the number of samples is 256
    BOOST_CHECK_EQUAL(sr.samples(), 256);
}

BOOST_AUTO_TEST_CASE(SampleRate_Constructor_Seconds)
{
    //build a sample rate of 1 per 60 seconds
    SampleRate sr(SampleRate::rateType_seconds, 60);

    //check that the string is "Every 1 min"
    BOOST_CHECK_EQUAL(sr.str(), "60-seconds");
    BOOST_CHECK_EQUAL(sr.prettyStr(), "Every 1 min");

    //check that the TimeSpan is the correct number of nanoseconds
    BOOST_CHECK_EQUAL(sr.samplePeriod().getNanoseconds(), 60000000000);

    //check that the rate type is hertz
    BOOST_CHECK_EQUAL(sr.rateType(), SampleRate::rateType_seconds);

    //check that the number of samples is 60
    BOOST_CHECK_EQUAL(sr.samples(), 60);
}

BOOST_AUTO_TEST_CASE(SampleRate_Hertz)
{
    //build a sample rate of 512 Hz
    SampleRate sr = SampleRate::Hertz(512);

    //check that the string is "512Hz"
    BOOST_CHECK_EQUAL(sr.str(), "512-hertz");
    BOOST_CHECK_EQUAL(sr.prettyStr(), "512Hz");

    //check that the TimeSpan is the correct number of nanoseconds
    BOOST_CHECK_EQUAL(sr.samplePeriod().getNanoseconds(), 1953125);

    //check that the rate type is hertz
    BOOST_CHECK_EQUAL(sr.rateType(), SampleRate::rateType_hertz);

    //check that the number of samples is 512
    BOOST_CHECK_EQUAL(sr.samples(), 512);
}

BOOST_AUTO_TEST_CASE(SampleRate_KiloHertz)
{
    //build a sample rate of 4 kHz
    SampleRate sr = SampleRate::KiloHertz(4);

    //check that the string is "4kHz"
    BOOST_CHECK_EQUAL(sr.str(), "4000-hertz");
    BOOST_CHECK_EQUAL(sr.prettyStr(), "4kHz");

    //check that the TimeSpan is the correct number of nanoseconds
    BOOST_CHECK_EQUAL(sr.samplePeriod().getNanoseconds(), 250000);

    //check that the rate type is hertz
    BOOST_CHECK_EQUAL(sr.rateType(), SampleRate::rateType_hertz);

    //check that the number of samples is 512
    BOOST_CHECK_EQUAL(sr.samples(), 4000);
}

BOOST_AUTO_TEST_CASE(SampleRate_Event)
{
    //build a sample rate of 4 kHz
    SampleRate sr(SampleRate::rateType_event, 0);

    BOOST_CHECK_EQUAL(sr.str(), "Event");
    BOOST_CHECK_EQUAL(sr.prettyStr(), "Event");

    BOOST_CHECK_EQUAL(sr.rateType(), SampleRate::rateType_event);

    BOOST_CHECK_THROW(sr.samplePeriod(), Error_UnknownSampleRate);
}

BOOST_AUTO_TEST_CASE(SampleRate_Seconds)
{
    //build a sample rate of 1 per 30 seconds
    SampleRate sr = SampleRate::Seconds(30);

    //check that the string is "Every 30 sec"
    BOOST_CHECK_EQUAL(sr.str(), "30-seconds");
    BOOST_CHECK_EQUAL(sr.prettyStr(), "Every 30 sec");

    //check that the TimeSpan is the correct number of nanoseconds
    BOOST_CHECK_EQUAL(sr.samplePeriod().getNanoseconds(), 30000000000);

    //check that the rate type is seconds
    BOOST_CHECK_EQUAL(sr.rateType(), SampleRate::rateType_seconds);

    //check that the number of samples is 30
    BOOST_CHECK_EQUAL(sr.samples(), 30);
}

BOOST_AUTO_TEST_CASE(SampleRate_Minutes)
{
    //build a sample rate of 1 per 2 minutes
    SampleRate sr = SampleRate::Seconds(120);

    //check the string representations
    BOOST_CHECK_EQUAL(sr.str(), "120-seconds");
    BOOST_CHECK_EQUAL(sr.prettyStr(), "Every 2 min");

    //check that the TimeSpan is the correct number of nanoseconds
    BOOST_CHECK_EQUAL(sr.samplePeriod().getNanoseconds(), 120000000000);

    //check that the rate type is seconds
    BOOST_CHECK_EQUAL(sr.rateType(), SampleRate::rateType_seconds);

    //check that the number of samples is 30
    BOOST_CHECK_EQUAL(sr.samples(), 120);
}

BOOST_AUTO_TEST_CASE(SampleRate_Minutes_float)
{
    //build a sample rate of 1 per 125 seconds
    SampleRate sr = SampleRate::Seconds(125);

    //check the string representations
    BOOST_CHECK_EQUAL(sr.str(), "125-seconds");
    BOOST_CHECK_EQUAL(sr.prettyStr(), "Every 2.08333 min");

    //check that the TimeSpan is the correct number of nanoseconds
    BOOST_CHECK_EQUAL(sr.samplePeriod().getNanoseconds(), 125000000000);

    //check that the rate type is seconds
    BOOST_CHECK_EQUAL(sr.rateType(), SampleRate::rateType_seconds);

    //check that the number of samples is 30
    BOOST_CHECK_EQUAL(sr.samples(), 125);
}

BOOST_AUTO_TEST_CASE(SampleRate_Hours)
{
    //build a sample rate of 1 per 2 minutes
    SampleRate sr = SampleRate::Seconds(86400);

    //check the string representations
    BOOST_CHECK_EQUAL(sr.str(), "86400-seconds");
    BOOST_CHECK_EQUAL(sr.prettyStr(), "Every 24 hr");

    //check that the TimeSpan is the correct number of nanoseconds
    BOOST_CHECK_EQUAL(sr.samplePeriod().getNanoseconds(), 86400000000000);

    //check that the rate type is seconds
    BOOST_CHECK_EQUAL(sr.rateType(), SampleRate::rateType_seconds);

    //check that the number of samples is 30
    BOOST_CHECK_EQUAL(sr.samples(), 86400);
}

BOOST_AUTO_TEST_CASE(SampleRate_Operators)
{
    SampleRate hz_1 = SampleRate::Hertz(1);
    SampleRate hz_512 = SampleRate::Hertz(512);
    SampleRate sec_10 = SampleRate::Seconds(10);
    SampleRate sec_1 = SampleRate::Seconds(1);

    BOOST_CHECK_EQUAL(hz_1 < hz_512, true);
    BOOST_CHECK_EQUAL(hz_1 <= hz_512, true);
    BOOST_CHECK_EQUAL(hz_1 != hz_512, true);
    BOOST_CHECK_EQUAL(hz_1 == sec_1, true);
    BOOST_CHECK_EQUAL(hz_512 > sec_10, true);
    BOOST_CHECK_EQUAL(hz_512 >= sec_10, true);
    BOOST_CHECK_EQUAL(hz_512 != sec_10, true);
    BOOST_CHECK_EQUAL(hz_512 < sec_10, false);
    BOOST_CHECK_EQUAL(sec_1 > sec_10, true);
}

BOOST_AUTO_TEST_CASE(SampleRate_PlusEquals)
{
    SampleRate hz_1 = SampleRate::Hertz(1);
    SampleRate sec_10 = SampleRate::Seconds(10);

    hz_1 += 49;
    BOOST_CHECK_EQUAL(hz_1.samplesPerSecond(), SampleRate::Hertz(50).samplesPerSecond());
    hz_1 += 2;
    BOOST_CHECK_EQUAL(hz_1.samplesPerSecond(), SampleRate::Hertz(52).samplesPerSecond());

    sec_10 += 5;
    BOOST_CHECK_EQUAL(sec_10.samplesPerSecond(), SampleRate::Seconds(5).samplesPerSecond());

    sec_10 += 10;
    BOOST_CHECK_EQUAL(sec_10.samplesPerSecond(), SampleRate::Hertz(5).samplesPerSecond());
    BOOST_CHECK_EQUAL(sec_10.rateType(), SampleRate::rateType_hertz);
}

BOOST_AUTO_TEST_CASE(SampleRate_samplesPerSecond)
{
    SampleRate hz_1 = SampleRate::Hertz(1);
    SampleRate hz_512 = SampleRate::Hertz(512);
    SampleRate hz_4096 = SampleRate::Hertz(4096);
    SampleRate sec_10 = SampleRate::Seconds(10);
    SampleRate sec_1 = SampleRate::Seconds(1);

    BOOST_CHECK_EQUAL(hz_1.samplesPerSecond(), 1.0);
    BOOST_CHECK_EQUAL(hz_512.samplesPerSecond(), 512);
    BOOST_CHECK_EQUAL(hz_4096.samplesPerSecond(), 4096);
    BOOST_CHECK_EQUAL(sec_10.samplesPerSecond(), 0.1);
    BOOST_CHECK_EQUAL(sec_1.samplesPerSecond(), 1.0);
}

BOOST_AUTO_TEST_CASE(SampleRate_Multiply)
{
    SampleRate initial = SampleRate::Seconds(10);
    SampleRate rightMult1 = initial * 5;
    SampleRate leftMult1 = 5 * initial;
    SampleRate rightMult2 = initial * 10;
    SampleRate leftMult2 = 10 * initial;
    SampleRate rightMult3 = initial * 100;
    SampleRate leftMult3 = 100 * initial;

    BOOST_CHECK(rightMult1 == SampleRate::Seconds(2));
    BOOST_CHECK(leftMult1 == SampleRate::Seconds(2));
    BOOST_CHECK(rightMult2 == SampleRate::Hertz(1));
    BOOST_CHECK(leftMult2 == SampleRate::Hertz(1));
    BOOST_CHECK(rightMult3 == SampleRate::Hertz(10));
    BOOST_CHECK(leftMult3 == SampleRate::Hertz(10));
}

BOOST_AUTO_TEST_CASE(SampleRate_Divide)
{
    SampleRate initial = SampleRate::Hertz(10);
    SampleRate rightDiv1 = initial / 5;
    SampleRate leftDiv1 = 5 / initial;
    SampleRate rightDiv2 = initial / 10;
    SampleRate leftDiv2 = 10 / initial;
    SampleRate rightDiv3 = initial / 100;
    SampleRate leftDiv3 = 100 / initial;

    BOOST_CHECK(rightDiv1 == SampleRate::Hertz(2));
    BOOST_CHECK(leftDiv1 == SampleRate::Hertz(2));
    BOOST_CHECK(rightDiv2 == SampleRate::Seconds(1));
    BOOST_CHECK(leftDiv2 == SampleRate::Seconds(1));
    BOOST_CHECK(rightDiv3 == SampleRate::Seconds(10));
    BOOST_CHECK(leftDiv3 == SampleRate::Seconds(10));
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef _ARITHMETIC_HOMOMORPHINE_BACKEND_H_
#define _ARITHMETIC_HOMOMORPHINE_BACKEND_H_

#include <string>
#include <map>
#include <vector>
#include <iostream>
#include <utility>
#include <exception>
#include <boost/algorithm/string.hpp>

#include "backend.hpp"

using namespace std;

namespace homomorphine
{

  /*! /brief Backend interface
   *
   * Provides an interface that each specific arithmetic backend has to provide
   */
  class ArithmeticBackend : public Backend
  {
    protected:
      map<string, string> params; /*!< params that are consumed by specific implementations of the arithmetic homomorphic backend */

    public:

      /*!
       * Arithmetic backend cleanup
       */
      virtual ~ArithmeticBackend() { this->params.clear(); };

      /*!
       * Sets the specific homomorphic algorithm implementation
       * that arithmetic backend provides
       *
       * \param algorithm homomorphic encryption algorithm
       */
      virtual void setAlgorithm(string algorithm) = 0;

      /*!
       * Algorithm backend initialization method - initializes all the basic
       * parameters and behavior of a specific backend implementation
       */
      virtual void init() = 0;

      /*!
       * Generates the public/secret key pair
       */
      virtual void generateKeys() = 0;

      /*!
       * Returns the UUEncoded public key
       *
       * \return public key
       */
      virtual string getPublicKey() = 0;

      /*!
       * Writes the public key to a stream
       *
       * \param stream public key stream
       */
      virtual void writePublicKeyToStream(ostream& stream) = 0;

      /*!
       * Returns the UUEncoded secret key
       *
       * \return secret key
       */
      virtual string getSecretKey() = 0;

      /*!
       * Writes the secret key to a stream
       *
       * \param stream secret key stream
       */
      virtual void writeSecretKeyToStream(ostream& stream) = 0;

      /*!
       * Returns the pair of UUEncoded public and secret keys
       *
       * \return pair of public and secret keys
       */
      virtual pair<string, string> getKeys() = 0;

      /*!
       * Sets the public key
       *
       * \param public_key UUEncoded public key
       */
      virtual void setPublicKey(string public_key) = 0;

      /*!
       * Sets the public key from stream
       *
       * \param stream public key binary stream
       */
      virtual void readPublicKeyFromStream(istream &stream) = 0;

      /*!
       * Sets the secret key
       *
       * \param secret_key UUEncoded secret key
       */
      virtual void setSecretKey(string secret_key) = 0;

      /*!
       * Sets the secret key from stream
       *
       * \param stream secret key binary stream
       */
      virtual void readSecretKeyFromStream(istream &stream) = 0;

      /*!
       * Sets the both public and secret keys
       *
       * \param public_key UUEncoded public key
       * \param secret_key UUEncoded secret key
       */
      virtual void setKeys(string public_key, string secret_key) = 0;

      /*!
       * Returns the UUEncoded cipher containing ecrypted value, or vector of values
       *
       * \return UUEncoded cipher
       */
      virtual string getCipher() = 0;

      /*!
       * Writes the cipher to output stream
       *
       * \param stream output stream
       */
      virtual void writeCipherToStream(ostream& stream) = 0;

      /*!
       * Sets the UUEncoded cipher containing ecrypted value, or vector of values
       *
       * \param cipher UUEncoded cipher
       */
      virtual void setCipher(string cipher) = 0;

      /*!
       * Reads the cipher from input stream
       *
       * \param stream cipher stream
       */
      virtual void readCipherFromStream(istream &stream) = 0;

      /*!
       * Encrypts the vector of values using the public key
       *
       * \param values vector of values
       * \return UUEncoded cipher
       */
      virtual void encrypt(vector<long> values) = 0;

      /*!
       * Encrypts the single value using the public key
       *
       * \param value value
       * \return UUEncoded cipher
       */
      virtual void encrypt(long value) = 0;

      /*!
       * Decrypts the vector of values using the secret key
       *
       * \return vector of decrypted values
       */
      virtual vector<long> decryptValues() = 0;

      /*!
       * Decrypts the single value using the secret key
       *
       * \return decrypted value
       */
      virtual long decrypt() = 0;

      /*!
       * Adds the vector of values to encrypted vector of values
       *
       * \param values vector of values
       */
      virtual void add(vector<long> values) = 0;

      /*!
       * Adds the value to encrypted value
       *
       * \param value value
       */
      virtual void add(long value) = 0;

      /*!
       * Negates a single encrypted value, or a vector of encrypted values
       */
      virtual void negate() = 0;

      /*!
       * Multiplies the vector of values with the encrypted vector of values
       *
       * \param values vector of values
       */
      virtual void multiply(vector<long> values) = 0;

      /*!
       * Multiplies the value with the encrypted value
       *
       * \param value value
       */
      virtual void multiply(long value) = 0;
  };
}

#endif


#include <boost/python.hpp>

#include "python/module.hpp"

#include "python/graph.hpp"
#include "simulation.hpp"

  // // functions
  // // class_< uniform_mass<vec3>, std::shared_ptr< uniform_mass<vec3>>,
  // // 		  bases<metric_base> >("uniform_mass_vec3");

namespace python {

  // TODO move ?where ?!
  struct simulation : graph, module<simulation> {
	static void module() {
	  using namespace boost::python;
	  using ::simulation;

      static auto get_gravity = +[](simulation* self) {
        return numpy::ndarray(self->gravity.data(), {3});
      };

      static auto set_gravity = +[](simulation* self, object obj) {
        get_gravity(self)[boost::python::slice()] = obj;
      };

	  class_<simulation, std::shared_ptr<simulation> > ("simulation")
		.def("step", &simulation::step)
		.def("init", &simulation::init)
		.def("reset", &simulation::reset)
        .add_property("gravity", get_gravity, set_gravity)
		;

	}
  };

}

// // Declare the actual type
BOOST_PYTHON_MODULE(pouf) {
  python::init_module();
}