#include <iostream>
#include <map>
#include <sstream>
#include <fstream>
#include <glog/logging.h>
#include <omp.h>
#include <float.h>
#include <math.h>

#include <HFTrain.h>

#include <boost/math/constants/constants.hpp>

void HFTrain::getTrainSet(TrainSet &train_set, int &number_of_classes, int &feature_vector_length)
{

    std::ifstream finput(input_file_.c_str(), std::ios::in | std::ios::binary);
    CHECK(finput) << "Could not open file " << input_file_;

    //read number of classes and feature vector length
    finput.read((char*)&number_of_classes, sizeof(int));
    finput.read((char*)&feature_vector_length, sizeof(int));

    train_set.resize(0);

    int num_vec = 0;
    while(true){

        int class_no;
        float yaw, pitch, roll, x, y, z;

        finput.read((char*)&class_no,   sizeof(int));
        if(finput.eof())
            break;

        finput.read((char*)&yaw,        sizeof(float));
        finput.read((char*)&pitch,      sizeof(float));
        finput.read((char*)&roll,       sizeof(float));
        finput.read((char*)&x,          sizeof(float));
        finput.read((char*)&y,          sizeof(float));
        finput.read((char*)&z,          sizeof(float));

        train_set.push_back(new TrainSample());
        train_set.back()->class_no = class_no;
        train_set.back()->node = 0;
        Eigen::VectorXf dof(6);
        dof(0) = yaw;
        dof(1) = pitch;
        dof(2) = roll;
        dof(3) = x;
        dof(4) = y;
        dof(5) = z;
        train_set.back()->dof = dof;

        Eigen::VectorXf feature_vector(feature_vector_length);
        for(int i=0; i<feature_vector_length; ++i){
            float f;
            finput.read((char*)&f, sizeof(float));
            feature_vector(i) = f;
        }

        train_set.back()->feature_vector = feature_vector;

    }

    finput.close();
}

//get random train_samples from training set and put them to the front
void HFTrain::suffle_training_set(TrainSet &train_set, int train_samples, std::vector<int> &samples_per_class){

    samples_per_class.resize(number_of_classes_);
    for(int i=0; i<train_samples; ++i){
        int k = rand() % (train_set.size() - i) + i;
        TrainSample *ts;
        ts = train_set[i];
        train_set[i] = train_set[k];
        train_set[k] = ts;
        samples_per_class[train_set[i]->class_no]++;
    }
}

//takes input last leaf_id+1, returns the changed value, added the current leaves
void HFTrain::make_leafs(TrainSet &train_set, int train_samples, const std::vector<int> &samples_per_class, int &leaf_id){

    float degrees_bin_size = 15.0f;
    float coordinates_bin_size = 0.03; //1cm

    float pi = boost::math::constants::pi<float>();

    boost::unordered_map<TreeNode*, std::vector<int> > node_class_samples;
    boost::unordered_map<TreeNode*, std::vector< std::vector<Eigen::VectorXf> > > node_class_hough_votes;
//    boost::unordered_map<TreeNode*, std::vector<Hough3DMap> > node_class_houghmap;
//    boost::unordered_map<TreeNode*, std::vector<std::vector<float> > > node_class_mean;
//    boost::unordered_map<TreeNode*, std::vector<std::vector<float> > > node_class_std;

    omp_set_num_threads(threads_per_tree_);

#pragma omp parallel
    {
        boost::unordered_map<TreeNode*, std::vector<int> > node_class_samples_local;
        boost::unordered_map<TreeNode*, std::vector< std::vector<Eigen::VectorXf> > > node_class_hough_votes_local;
//        boost::unordered_map<TreeNode*, std::vector<Hough3DMap> > node_class_houghmap_local;
//        boost::unordered_map<TreeNode*, std::vector<std::vector<float> > > node_class_mean_local;

        #pragma omp for schedule(dynamic)
        for(int i=0; i<train_samples; ++i){
            if(train_set[i]->node->leaf){
                //classification info
                TreeNode *cur_node = train_set[i]->node;
                if(node_class_samples_local.count(cur_node)){
                    node_class_samples_local[cur_node][train_set[i]->class_no]++;
                } else {
                    node_class_samples_local[cur_node] = std::vector<int>(number_of_classes_, 0);
                    node_class_samples_local[cur_node][train_set[i]->class_no]++;
                }
                if(!node_class_hough_votes_local.count(cur_node))
                    node_class_hough_votes_local[cur_node] = std::vector<std::vector<Eigen::VectorXf> >(number_of_classes_);

                node_class_hough_votes_local[cur_node][train_set[i]->class_no].push_back(train_set[i]->dof);

            }
        }

        #pragma omp critical
        {
            //concatenate classification info
            for(boost::unordered_map<TreeNode*, std::vector<int> >::iterator it = node_class_samples_local.begin();
                it != node_class_samples_local.end(); ++it){
                if(node_class_samples.count(it->first)){
                    for(int i=0; i<number_of_classes_; ++i)
                        node_class_samples[it->first][i] += (it->second)[i];
                } else {
                    node_class_samples[it->first] = it->second;
                }

            }

            for(boost::unordered_map<TreeNode*, std::vector< std::vector<Eigen::VectorXf> > >::iterator it = node_class_hough_votes_local.begin();
                it != node_class_hough_votes_local.end(); ++it){

                TreeNode *cur_node = it->first;
                if(!node_class_hough_votes.count(cur_node))
                    node_class_hough_votes[cur_node] = std::vector<std::vector<Eigen::VectorXf> >(number_of_classes_);
                for(int c=0; c<number_of_classes_; ++c)
                    node_class_hough_votes[cur_node][c].insert(node_class_hough_votes[cur_node][c].end(),
                                                               node_class_hough_votes_local[cur_node][c].begin(),
                                                               node_class_hough_votes_local[cur_node][c].end());

            }

        } //critical

    } //parallel

    //normalize class probabilities and calc mean
    for(boost::unordered_map<TreeNode*, std::vector<int> >::iterator it = node_class_samples.begin();
        it != node_class_samples.end(); ++it){

        TreeNode *cur_node = it->first;
        //save id and increase id counter
        cur_node->leaf_id = leaf_id++;
        std::vector<int> *cur_class_samples = &(node_class_samples[cur_node]);
        int total_samples = 0;
        for(int i=0; i<number_of_classes_; ++i)
            total_samples += (*cur_class_samples)[i];

        //normalize class distribution according to
        //initial number of samples per class
        cur_node->class_prob.resize(number_of_classes_);
        for(int c=0; c<number_of_classes_; ++c){
            float norm = 0;
            for(int i=0; i<number_of_classes_; ++i)
                norm += (float)samples_per_class[c]/(float)samples_per_class[i]*(float)(*cur_class_samples)[i];
            cur_node->class_prob[c] = (float)(*cur_class_samples)[c] / norm ;
        }

//        //get mean by division with total samples
//        std::vector<std::vector<float> > *cur_node_class_mean = &(node_class_mean[cur_node]);
//        for(int c=0; c<number_of_classes_; ++c){
//            int cur_samples = node_class_samples[cur_node][c];
//            if(cur_samples > 0){
//                for(int f=0; f<feature_vector_length_; ++f)
//                        (*cur_node_class_mean)[c][f] /= (float)cur_samples;
//            }
//        }
    }

    for(boost::unordered_map<TreeNode*, std::vector< std::vector<Eigen::VectorXf> > >::iterator it = node_class_hough_votes.begin();
        it != node_class_hough_votes.end(); ++it){

        TreeNode *cur_node = it->first;
        cur_node->hough_votes.resize(number_of_classes_);
        for(int c=0; c<number_of_classes_; ++c)
            cur_node->hough_votes[c].insert(cur_node->hough_votes[c].end(),
                                            node_class_hough_votes[cur_node][c].begin(),
                                            node_class_hough_votes[cur_node][c].end());

    }

}

//put non-used training samples at the end and reduce train_samples accordingly
void HFTrain::clean_trainset(TrainSet& train_set, int &train_samples){
    int front = 0;
    int back = train_samples - 1;
    while(front < back){
        while(front < train_samples && !train_set[front]->node->leaf)
            front++;
        while(back >= 0 && train_set[back]->node->leaf)
            back--;
        if(front < back){
            TrainSample *t;
            t = train_set[front];
            train_set[front] = train_set[back];
            train_set[back] = t;
        }
    }
    train_samples = front;
}

//get random features, set threshold to 0 for now
void HFTrain::get_random_features(const std::vector<TreeNode*> &level_nodes,
                                  boost::unordered_map<TreeNode*, std::vector<Test> > &node_tests
                                  )
{

    int num_tests = tests_per_node_* thresholds_per_test_;
    for(int i=0; i<level_nodes.size(); ++i)
        node_tests[level_nodes[i]].resize(num_tests);

    omp_set_num_threads(threads_per_tree_);

#pragma omp parallel for schedule(dynamic)
    for(int n=0; n<level_nodes.size(); ++n){
        TreeNode *cur_node = level_nodes[n];
        std::vector<Test> *cur_node_tests = &(node_tests[cur_node]);
        for(int t=0; t<tests_per_node_; ++t){
            int mm = rand() % 2;
            int f1 = rand() % feature_vector_length_;
            int f2 = rand() % feature_vector_length_;
            for(int th=0; th<thresholds_per_test_; ++th){
                (*cur_node_tests)[t*thresholds_per_test_ + th].measure_mode = mm;
                (*cur_node_tests)[t*thresholds_per_test_ + th].feature1 = f1;
                (*cur_node_tests)[t*thresholds_per_test_ + th].feature2 = f2;
                (*cur_node_tests)[t*thresholds_per_test_ + th].threshold = 0;
            }
        }
    }

//    std::cout << "random tests: " << omp_get_wtime() - start << std::endl;

}

//get min-max for each test for each node to constraint thresholds' range
//also get samples per class per node and samples per node because code here is convenient
void HFTrain::get_min_max_count_samples(const TrainSet &train_set,
                          int train_samples,
                          const std::vector<TreeNode*> &level_nodes,
                          boost::unordered_map<TreeNode*, std::vector<Test> > &node_tests,
                          boost::unordered_map<TreeNode*, std::vector<float> > &node_test_min,
                          boost::unordered_map<TreeNode*, std::vector<float> > &node_test_max,
                          boost::unordered_map<TreeNode*, std::vector<int> > &node_class_samples,
                          boost::unordered_map<TreeNode*, int> &node_samples
                          )
{

    int num_tests = tests_per_node_ * thresholds_per_test_;
    for(int i=0; i<level_nodes.size(); ++i){
        node_test_min[level_nodes[i]] = std::vector<float>(num_tests,  FLT_MAX);
        node_test_max[level_nodes[i]] = std::vector<float>(num_tests, -FLT_MAX);
    }

    for(int i=0; i<level_nodes.size(); ++i){
        node_class_samples[level_nodes[i]] = std::vector<int>(number_of_classes_, 0);
        node_samples[level_nodes[i]] = 0;
    }

//    double start = omp_get_wtime();

    omp_set_num_threads(threads_per_tree_);

#pragma omp parallel
    {

        boost::unordered_map<TreeNode*, std::vector<float> > node_test_min_local;
        boost::unordered_map<TreeNode*, std::vector<float> > node_test_max_local;
        for(int i=0; i<level_nodes.size(); ++i){
            node_test_min_local[level_nodes[i]] = std::vector<float>(num_tests,  FLT_MAX);
            node_test_max_local[level_nodes[i]] = std::vector<float>(num_tests, -FLT_MAX);
        }
        boost::unordered_map<TreeNode*, std::vector<int> > node_class_samples_local;
        boost::unordered_map<TreeNode*, int> node_samples_local;
        for(int i=0; i<level_nodes.size(); ++i){
            node_class_samples_local[level_nodes[i]] = std::vector<int>(number_of_classes_, 0);
            node_samples_local[level_nodes[i]] = 0;
        }

        #pragma omp for schedule(dynamic)
        for(int i=0; i<train_samples; ++i){
            TreeNode* cur_node = train_set[i]->node;
            std::vector<float> *min_local = &node_test_min_local[cur_node];
            std::vector<float> *max_local = &node_test_max_local[cur_node];
            std::vector<Test> *cur_node_tests = &node_tests[cur_node];
            for(int t=0; t<num_tests; ++t){
                int f1 = (*cur_node_tests)[t].feature1;
                int f2 = (*cur_node_tests)[t].feature2;
                int mm = (*cur_node_tests)[t].measure_mode;
                float val;
                if(mm == 0)
                    val = train_set[i]->feature_vector(f1) - train_set[i]->feature_vector(f2);
                else if(mm == 1)
                    val = train_set[i]->feature_vector(f1);

                if(val > (*max_local)[t])
                    (*max_local)[t] = val;
                if(val < (*min_local)[t])
                    (*min_local)[t] = val;
            }
            node_class_samples_local[cur_node][train_set[i]->class_no]++;
            node_samples_local[cur_node]++;
        }
        #pragma omp critical
        {
            for(int n=0; n<level_nodes.size(); ++n){
                std::vector<float> *min_local = &node_test_min_local[level_nodes[n]];
                std::vector<float> *max_local = &node_test_max_local[level_nodes[n]];
                std::vector<float> *min_global = &node_test_min[level_nodes[n]];
                std::vector<float> *max_global = &node_test_max[level_nodes[n]];
                for(int t=0; t<num_tests; ++t){
                    if( (*min_global)[t] > (*min_local)[t] )
                        (*min_global)[t] = (*min_local)[t];

                    if( (*max_global)[t] < (*max_local)[t])
                        (*max_global)[t] = (*max_local)[t];

                }
                for(int c=0; c<number_of_classes_; ++c)
                    node_class_samples[level_nodes[n]][c] += node_class_samples_local[level_nodes[n]][c];

                node_samples[level_nodes[n]] += node_samples_local[level_nodes[n]];
            }
        }
    }

//    std::cout << "min - max: " << omp_get_wtime() - start << std::endl;
//    std::cout << node_min[level_nodes[0]][0] << " " << node_max[level_nodes[0]][0] << std::endl;

}

//get random tests
void HFTrain::get_random_thresholds(const std::vector<TreeNode*> &level_nodes,
                               boost::unordered_map<TreeNode*, std::vector<float> > &node_test_min,
                               boost::unordered_map<TreeNode*, std::vector<float> > &node_test_max,
                               boost::unordered_map<TreeNode*, std::vector<Test> > &node_tests
                               )
{

    int num_tests = tests_per_node_* thresholds_per_test_;

    omp_set_num_threads(threads_per_tree_);

#pragma omp parallel for schedule(dynamic)
    for(int n=0; n<level_nodes.size(); ++n){
        TreeNode *cur_node = level_nodes[n];
        std::vector<Test> *cur_node_tests = &(node_tests[cur_node]);
        std::vector<float> *cur_node_test_min = &(node_test_min[cur_node]);
        std::vector<float> *cur_node_test_max = &(node_test_max[cur_node]);
        for(int t=0; t<num_tests; ++t){
            float range = (*cur_node_test_max)[t] - (*cur_node_test_min)[t];
            (*cur_node_tests)[t].threshold = (float)rand() / (float)RAND_MAX * range + (*cur_node_test_min)[t];
        }
    }

//    std::cout << "random tests: " << omp_get_wtime() - start << std::endl;

}

// ------classification------ //
//if no split found a node is marked as leaf.
//best tests are stored to the node of each training sample
void HFTrain::find_classification_split(TrainSet &train_set,
                                        int train_samples,
                                        const std::vector<TreeNode*> &level_nodes,
                                        boost::unordered_map<TreeNode*, std::vector<Test> > &node_tests,
                                        boost::unordered_map<TreeNode*, std::vector<int> > &node_class_samples,
                                        boost::unordered_map<TreeNode*, int> &node_samples
                                        )
{

    //number of samples per class per test per node that goes to the left child
    //if we apply test
    int num_tests = tests_per_node_* thresholds_per_test_;
    boost::unordered_map<TreeNode*, std::vector< std::vector<int> > > node_test_class_samples_left;
    for(int i=0; i<level_nodes.size(); ++i){
        node_test_class_samples_left[level_nodes[i]].resize(num_tests);
        for(int t=0; t<num_tests; ++t)
            node_test_class_samples_left[level_nodes[i]][t] = std::vector<int>(number_of_classes_, 0);
    }

//    double start = omp_get_wtime();

    omp_set_num_threads(threads_per_tree_);

#pragma omp parallel
    {

        boost::unordered_map<TreeNode*, std::vector< std::vector<int> > > node_test_class_samples_left_local;
        for(int i=0; i<level_nodes.size(); ++i){
            node_test_class_samples_left_local[level_nodes[i]].resize(num_tests);
            for(int t=0; t<num_tests; ++t)
                node_test_class_samples_left_local[level_nodes[i]][t] = std::vector<int>(number_of_classes_, 0);
        }

        #pragma omp for schedule(dynamic)
        for(int i=0; i<train_samples; ++i){
            TreeNode *cur_node = train_set[i]->node;
            std::vector< std::vector<int> > *cur_left_local = &node_test_class_samples_left_local[cur_node];
            //apply all tests to current sample
            for(int t=0; t<num_tests; ++t){
                Test cur_test = node_tests[cur_node][t];
                float val;
                if(cur_test.measure_mode == 0)
                    val = train_set[i]->feature_vector(cur_test.feature1) - train_set[i]->feature_vector(cur_test.feature2);
                else if(cur_test.measure_mode == 1)
                    val = train_set[i]->feature_vector(cur_test.feature1);

                if( val < cur_test.threshold )
                    //go to left, count it
                    (*cur_left_local)[t][train_set[i]->class_no] ++;
            }

        }
        #pragma omp critical
        {

            for(int n=0; n<level_nodes.size(); ++n){
                std::vector< std::vector<int> > *cur_left_local = &node_test_class_samples_left_local[level_nodes[n]];
                std::vector< std::vector<int> > *cur_left_global = &node_test_class_samples_left[level_nodes[n]];
                for(int t=0; t<num_tests; ++t)
                    for(int c=0; c<number_of_classes_; ++c)
                        (*cur_left_global)[t][c] += (*cur_left_local)[t][c];
            }

        }

    }

//    std::cout << "class hist: " << omp_get_wtime() - start << std::endl;

    //compute best test via entropy

#pragma omp parallel
    {
    int num_leaves = 0;
    #pragma omp for schedule(dynamic)
    for(int n=0; n<level_nodes.size(); ++n){
        TreeNode *cur_node = level_nodes[n];
        std::vector< std::vector<int> > *cur_samples_left = &node_test_class_samples_left[cur_node];
        std::vector<int> *cur_class_samples = &node_class_samples[cur_node];
        int cur_samples = node_samples[cur_node];
        float min_entropy = FLT_MAX;
        Test best_test;
        bool found = false;
        for(int t=0; t<num_tests; ++t){
            float entropy_left = 0;
            float entropy_right = 0;
            float entropy = FLT_MAX;
            int total_samples_left = 0;
            for(int c=0; c<number_of_classes_; ++c)
                total_samples_left += (*cur_samples_left)[t][c];
            int total_samples_right = cur_samples - total_samples_left;
            if(total_samples_left != 0 && total_samples_right != 0){
                for(int c=0; c<number_of_classes_; ++c){
                    //add left entropy
                    float p=(float)(*cur_samples_left)[t][c] / (float)total_samples_left;
                    if(p!=0) entropy_left -= p * log(p);
                    //add right entropy
                    p = (float)((*cur_class_samples)[c] - (*cur_samples_left)[t][c]) / (float)total_samples_right;
                    if(p!=0) entropy_right -= p * log(p);
                }
                entropy = (entropy_left * (float)total_samples_left + entropy_right * (float)total_samples_right);
                if(entropy < min_entropy){
                    min_entropy = entropy;
                    best_test = node_tests[cur_node][t];
                    found = true;
                }
            }

        }
        if(found){
            cur_node->test = best_test;
            cur_node->leaf = false;
            cur_node->left = new TreeNode;
            cur_node->right = new TreeNode;
        }
        else{
            cur_node->leaf = true;
        }
    }

    }

//    std::cout << "best tests classification: " << omp_get_wtime() - start << std::endl;

}

// ------Regression for x y z------ //
//if no split found a node is marked as leaf.
//best tests are stored to the node of each training sample
void HFTrain::find_regression_location_split(TrainSet &train_set,
                                            int train_samples,
                                            const std::vector<TreeNode*> &level_nodes,
                                            boost::unordered_map<TreeNode*, std::vector<Test> > &node_tests,
                                            boost::unordered_map<TreeNode*, std::vector<int> > &node_class_samples,
                                            boost::unordered_map<TreeNode*, int> &node_samples
                                            )
{

//    double start = omp_get_wtime();

    //get the mean values for all possible split-childs

    //[node][test][0-1(left-right)][0-2(x,y,z)] = mean(x, y, z) and 4->samples_of_child per child node per test per node
    int num_tests = tests_per_node_* thresholds_per_test_;
    boost::unordered_map<TreeNode*, std::vector< std::vector< std::vector<float> > > > node_test_child_mean;
    for(int i=0; i<level_nodes.size(); ++i){
        node_test_child_mean[level_nodes[i]].resize(num_tests);
        for(int t=0; t<num_tests; ++t){
            node_test_child_mean[level_nodes[i]][t].resize(2);
            for(int child=0; child<2; ++child){
                node_test_child_mean[level_nodes[i]][t][child] = std::vector<float>(4, 0); //x y z
            }
        }
    }

    omp_set_num_threads(threads_per_tree_);

#pragma omp parallel
    {

        boost::unordered_map<TreeNode*, std::vector< std::vector< std::vector<float> > > > node_test_child_mean_local;
        for(int i=0; i<level_nodes.size(); ++i){
            node_test_child_mean_local[level_nodes[i]].resize(num_tests);
            for(int t=0; t<num_tests; ++t){
                node_test_child_mean_local[level_nodes[i]][t].resize(2);
                for(int child=0; child<2; ++child){
                    node_test_child_mean_local[level_nodes[i]][t][child] = std::vector<float>(4, 0); //x y z child_samples
                }
            }
        }

        #pragma omp for schedule(dynamic)
        for(int i=0; i<train_samples; ++i){
            TreeNode *cur_node = train_set[i]->node;
            //reduce calls to hash table so to reduce execution time
            std::vector< std::vector< std::vector<float> > > *cur_mean = &(node_test_child_mean_local[cur_node]);
            //apply all tests to current sample
            for(int t=0; t<num_tests; ++t){
                Test cur_test = node_tests[cur_node][t];
                float val;
                if(cur_test.measure_mode == 0)
                    val = train_set[i]->feature_vector(cur_test.feature1) - train_set[i]->feature_vector(cur_test.feature2);
                else if(cur_test.measure_mode == 1)
                    val = train_set[i]->feature_vector(cur_test.feature1);

                int child;
                if( val < cur_test.threshold )
                    //go to the left child
                    child = 0;
                else
                    //go to the right
                    child = 1;

                (*cur_mean)[t][child][0] += train_set[i]->dof(3); //x
                (*cur_mean)[t][child][1] += train_set[i]->dof(4); //y
                (*cur_mean)[t][child][2] += train_set[i]->dof(5); //z
                (*cur_mean)[t][child][3]++;                       //count samples

            }

        }
        //get child samples in order to calculate mean values
        #pragma omp critical
        {
            for(int n=0; n<level_nodes.size(); ++n){
                std::vector< std::vector< std::vector<float> > > *cur_mean = &(node_test_child_mean[level_nodes[n]]);
                std::vector< std::vector< std::vector<float> > > *cur_mean_local = &(node_test_child_mean_local[level_nodes[n]]);
                for(int t=0; t<num_tests; ++t)
                    for(int child=0; child<2; ++child)
                        (*cur_mean)[t][child][3] +=
                            (*cur_mean_local)[t][child][3];
            }
        }

        //continue after maen is calculated
        #pragma omp barrier

        #pragma omp critical
        {
            for(int n=0; n<level_nodes.size(); ++n){
                std::vector< std::vector< std::vector<float> > > *cur_mean = &(node_test_child_mean[level_nodes[n]]);
                std::vector< std::vector< std::vector<float> > > *cur_mean_local = &(node_test_child_mean_local[level_nodes[n]]);
                for(int t=0; t<num_tests; ++t)
                    for(int child=0; child<2; ++child)
                        for(int coord=0; coord<3; ++coord) // x y z only
                            if((*cur_mean)[t][child][3] > 0)
                                (*cur_mean)[t][child][coord] +=
                                    (*cur_mean_local)[t][child][coord] / (*cur_mean)[t][child][3];
            }

        }

    }

    //get std values for all possible split-childs

    //[node][test][0-1(left-right)] = std(x) + std(y) + std(z) per child node per test per node
    boost::unordered_map<TreeNode*, std::vector< std::vector<float> > > node_test_child_std;
    for(int i=0; i<level_nodes.size(); ++i){
        node_test_child_std[level_nodes[i]].resize(num_tests);
        for(int t=0; t<num_tests; ++t){
            node_test_child_std[level_nodes[i]][t].resize(2);
            for(int child=0; child<2; ++child){
                node_test_child_std[level_nodes[i]][t][child] = 0;
            }
        }
    }

#pragma omp parallel
    {

        boost::unordered_map<TreeNode*, std::vector< std::vector<float> > > node_test_child_std_local;
        for(int i=0; i<level_nodes.size(); ++i){
            node_test_child_std_local[level_nodes[i]].resize(num_tests);
            for(int t=0; t<num_tests; ++t){
                node_test_child_std_local[level_nodes[i]][t].resize(2);
                for(int child=0; child<2; ++child){
                    node_test_child_std_local[level_nodes[i]][t][child] = 0;
                }
            }
        }

        #pragma omp for schedule(dynamic)
        for(int i=0; i<train_samples; ++i){
            TreeNode *cur_node = train_set[i]->node;
            std::vector< std::vector< std::vector<float> > > *cur_mean = &(node_test_child_mean[cur_node]);
            std::vector< std::vector<float> > *cur_std_local = &(node_test_child_std_local[cur_node]);
            //apply all tests to current sample
            for(int t=0; t<num_tests; ++t){
                Test cur_test = node_tests[cur_node][t];
                float val;
                if(cur_test.measure_mode == 0)
                    val = train_set[i]->feature_vector(cur_test.feature1) - train_set[i]->feature_vector(cur_test.feature2);
                else if(cur_test.measure_mode == 1)
                    val = train_set[i]->feature_vector(cur_test.feature1);

                int child;
                if( val < cur_test.threshold )
                    //go to left
                    child = 0;
                else
                    //go to right
                    child = 1;

                (*cur_std_local)[t][child] +=
                    pow(train_set[i]->dof(3) - (*cur_mean)[t][child][0], 2) +  //x
                    pow(train_set[i]->dof(4) - (*cur_mean)[t][child][1], 2) +  //y
                    pow(train_set[i]->dof(5) - (*cur_mean)[t][child][2], 2);   //z

            }

        }
        #pragma omp critical
        {

            for(int n=0; n<level_nodes.size(); ++n){
                std::vector< std::vector< std::vector<float> > > *cur_mean = &(node_test_child_mean[level_nodes[n]]);
                std::vector< std::vector<float> > *cur_std_local = &(node_test_child_std_local[level_nodes[n]]);
                std::vector< std::vector<float> > *cur_std = &(node_test_child_std[level_nodes[n]]);
                for(int t=0; t<num_tests; ++t)
                    for(int child=0; child<2; ++child)
                        if((*cur_mean)[t][child][3] > 0)
                            (*cur_std)[t][child] +=
                                (*cur_std_local)[t][child] / (*cur_mean)[t][child][3];
            }

        }

    }

    //compute best test using std

#pragma omp parallel
    {
    int num_leaves = 0;
    #pragma omp for schedule(dynamic)
    for(int n=0; n<level_nodes.size(); ++n){
        TreeNode *cur_node = level_nodes[n];
        std::vector< std::vector< std::vector<float> > > *cur_mean = &(node_test_child_mean[cur_node]);
        std::vector< std::vector<float> > *cur_std = &(node_test_child_std[cur_node]);
        float min_std = FLT_MAX;
        Test best_test;
        bool found = false;
        for(int t=0; t<num_tests; ++t){
            float total_samples_left  = (*cur_mean)[t][0][3];
            float total_samples_right = (*cur_mean)[t][1][3];
            if(total_samples_left > 0.1f && total_samples_right > 0.1f){
                float std = (*cur_std)[t][0]*total_samples_left + (*cur_std)[t][1]*total_samples_right;
                if(std < min_std){
                    min_std = std;
                    best_test = node_tests[cur_node][t];
                    found = true;
                }
            }

        }
        if(found){
            cur_node->test = best_test;
            cur_node->leaf = false;
            cur_node->left = new TreeNode;
            cur_node->right = new TreeNode;
        }
        else{
            cur_node->leaf = true;
        }
    }

    }

//    std::cout << "best tests regression: " << omp_get_wtime() - start << std::endl;

}

// ------Regression for yaw pitch roll------ //
//if no split found a node is marked as leaf.
//best tests are stored to the node of each training sample
void HFTrain::find_regression_pose_split(TrainSet &train_set,
                                            int train_samples,
                                            const std::vector<TreeNode*> &level_nodes,
                                            boost::unordered_map<TreeNode*, std::vector<Test> > &node_tests,
                                            boost::unordered_map<TreeNode*, std::vector<int> > &node_class_samples,
                                            boost::unordered_map<TreeNode*, int> &node_samples
                                            )
{

//    double start = omp_get_wtime();

    //get the mean values for all possible split-childs

    // [node][test][0-1(left-right)][0-2(x,y,z)] =
    // cos(yaw), sin(yaw), cos(pitch), sin(pitch), cos(roll), sin(roll), count samples
    int num_tests = tests_per_node_* thresholds_per_test_;
    boost::unordered_map<TreeNode*, std::vector< std::vector< std::vector<float> > > > node_test_child_mean;
    for(int i=0; i<level_nodes.size(); ++i){
        node_test_child_mean[level_nodes[i]].resize(num_tests);
        for(int t=0; t<num_tests; ++t){
            node_test_child_mean[level_nodes[i]][t].resize(2);
            for(int child=0; child<2; ++child){
                node_test_child_mean[level_nodes[i]][t][child] = std::vector<float>(7, 0);
            }
        }
    }

    omp_set_num_threads(threads_per_tree_);

#pragma omp parallel
    {

        boost::unordered_map<TreeNode*, std::vector< std::vector< std::vector<float> > > > node_test_child_mean_local;
        for(int i=0; i<level_nodes.size(); ++i){
            node_test_child_mean_local[level_nodes[i]].resize(num_tests);
            for(int t=0; t<num_tests; ++t){
                node_test_child_mean_local[level_nodes[i]][t].resize(2);
                for(int child=0; child<2; ++child){
                    node_test_child_mean_local[level_nodes[i]][t][child] = std::vector<float>(7, 0);
                }
            }
        }

        #pragma omp for schedule(dynamic)
        for(int i=0; i<train_samples; ++i){
            TreeNode *cur_node = train_set[i]->node;
            //reduce calls to hash table so to reduce execution time
            std::vector< std::vector< std::vector<float> > > *cur_mean = &(node_test_child_mean_local[cur_node]);
            //apply all tests to current sample
            for(int t=0; t<num_tests; ++t){
                Test cur_test = node_tests[cur_node][t];
                float val;
                if(cur_test.measure_mode == 0)
                    val = train_set[i]->feature_vector(cur_test.feature1) - train_set[i]->feature_vector(cur_test.feature2);
                else if(cur_test.measure_mode == 1)
                    val = train_set[i]->feature_vector(cur_test.feature1);

                int child;
                if( val < cur_test.threshold )
                    //go to the left child
                    child = 0;
                else
                    //go to the right
                    child = 1;

                (*cur_mean)[t][child][0] += cos(train_set[i]->dof(0)); //cos(yaw)
                (*cur_mean)[t][child][1] += sin(train_set[i]->dof(0)); //sin(yaw)
                (*cur_mean)[t][child][2] += cos(train_set[i]->dof(1)); //cos(pitch)
                (*cur_mean)[t][child][3] += sin(train_set[i]->dof(1)); //sin(pitch)
                (*cur_mean)[t][child][4] += cos(train_set[i]->dof(2)); //cos(roll)
                (*cur_mean)[t][child][5] += sin(train_set[i]->dof(2)); //sin(roll)
                (*cur_mean)[t][child][6]++;                            //count samples

            }

        }
        //get child samples in order to calculate mean values
        #pragma omp critical
        {
            for(int n=0; n<level_nodes.size(); ++n){
                std::vector< std::vector< std::vector<float> > > *cur_mean = &(node_test_child_mean[level_nodes[n]]);
                std::vector< std::vector< std::vector<float> > > *cur_mean_local = &(node_test_child_mean_local[level_nodes[n]]);
                for(int t=0; t<num_tests; ++t)
                    for(int child=0; child<2; ++child)
                        (*cur_mean)[t][child][6] +=
                            (*cur_mean_local)[t][child][6];
            }
        }

        //continue after mean is calculated
        #pragma omp barrier

        #pragma omp critical
        {
            for(int n=0; n<level_nodes.size(); ++n){
                std::vector< std::vector< std::vector<float> > > *cur_mean = &(node_test_child_mean[level_nodes[n]]);
                std::vector< std::vector< std::vector<float> > > *cur_mean_local = &(node_test_child_mean_local[level_nodes[n]]);
                for(int t=0; t<num_tests; ++t)
                    for(int child=0; child<2; ++child)
                        for(int pose=0; pose<6; ++pose)
                            if((*cur_mean)[t][child][6] > 0)
                                (*cur_mean)[t][child][pose] +=
                                    (*cur_mean_local)[t][child][pose] / (*cur_mean)[t][child][6];
            }

        }

    }

    //get std values for all possible split-childs

    //[node][test][0-1(left-right)] = std(x) + std(y) + std(z) per child node per test per node
    boost::unordered_map<TreeNode*, std::vector< std::vector<float> > > node_test_child_std;
    for(int i=0; i<level_nodes.size(); ++i){
        node_test_child_std[level_nodes[i]].resize(num_tests);
        for(int t=0; t<num_tests; ++t){
            node_test_child_std[level_nodes[i]][t].resize(2);
            for(int child=0; child<2; ++child){
                node_test_child_std[level_nodes[i]][t][child] = 0;
            }
        }
    }

#pragma omp parallel
    {

        boost::unordered_map<TreeNode*, std::vector< std::vector<float> > > node_test_child_std_local;
        for(int i=0; i<level_nodes.size(); ++i){
            node_test_child_std_local[level_nodes[i]].resize(num_tests);
            for(int t=0; t<num_tests; ++t){
                node_test_child_std_local[level_nodes[i]][t].resize(2);
                for(int child=0; child<2; ++child){
                    node_test_child_std_local[level_nodes[i]][t][child] = 0;
                }
            }
        }

        #pragma omp for schedule(dynamic)
        for(int i=0; i<train_samples; ++i){
            TreeNode *cur_node = train_set[i]->node;
            std::vector< std::vector< std::vector<float> > > *cur_mean = &(node_test_child_mean[cur_node]);
            std::vector< std::vector<float> > *cur_std_local = &(node_test_child_std_local[cur_node]);
            //apply all tests to current sample
            for(int t=0; t<num_tests; ++t){
                Test cur_test = node_tests[cur_node][t];
                float val;
                if(cur_test.measure_mode == 0)
                    val = train_set[i]->feature_vector(cur_test.feature1) - train_set[i]->feature_vector(cur_test.feature2);
                else if(cur_test.measure_mode == 1)
                    val = train_set[i]->feature_vector(cur_test.feature1);

                int child;
                if( val < cur_test.threshold )
                    //go to left
                    child = 0;
                else
                    //go to right
                    child = 1;

                (*cur_std_local)[t][child] +=
                        pow(cos(train_set[i]->dof(0)) - (*cur_mean)[t][child][0], 2) +  //cos(yaw)
                        pow(sin(train_set[i]->dof(0)) - (*cur_mean)[t][child][1], 2) +  //sin(yaw)
                        pow(cos(train_set[i]->dof(1)) - (*cur_mean)[t][child][2], 2) +  //cos(pitch)
                        pow(sin(train_set[i]->dof(1)) - (*cur_mean)[t][child][3], 2) +  //sin(pitch)
                        pow(cos(train_set[i]->dof(2)) - (*cur_mean)[t][child][4], 2) +  //cos(roll)
                        pow(sin(train_set[i]->dof(2)) - (*cur_mean)[t][child][5], 2);   //sin(roll)

            }

        }
        #pragma omp critical
        {

            for(int n=0; n<level_nodes.size(); ++n){
                std::vector< std::vector< std::vector<float> > > *cur_mean = &(node_test_child_mean[level_nodes[n]]);
                std::vector< std::vector<float> > *cur_std_local = &(node_test_child_std_local[level_nodes[n]]);
                std::vector< std::vector<float> > *cur_std = &(node_test_child_std[level_nodes[n]]);
                for(int t=0; t<num_tests; ++t)
                    for(int child=0; child<2; ++child)
                        if((*cur_mean)[t][child][6] > 0)
                            (*cur_std)[t][child] +=
                                (*cur_std_local)[t][child] / (*cur_mean)[t][child][6];
            }

        }

    }

    //compute best test using std

#pragma omp parallel
    {
    int num_leaves = 0;
    #pragma omp for schedule(dynamic)
    for(int n=0; n<level_nodes.size(); ++n){
        TreeNode *cur_node = level_nodes[n];
        std::vector< std::vector< std::vector<float> > > *cur_mean = &(node_test_child_mean[cur_node]);
        std::vector< std::vector<float> > *cur_std = &(node_test_child_std[cur_node]);
        float min_std = FLT_MAX;
        Test best_test;
        bool found = false;
        for(int t=0; t<num_tests; ++t){
            float total_samples_left  = (*cur_mean)[t][0][6];
            float total_samples_right = (*cur_mean)[t][1][6];
            if(total_samples_left > 0.1f && total_samples_right > 0.1f){
                float std = (*cur_std)[t][0]*total_samples_left + (*cur_std)[t][1]*total_samples_right;
                if(std < min_std){
                    min_std = std;
                    best_test = node_tests[cur_node][t];
                    found = true;
                }
            }

        }
        if(found){
            cur_node->test = best_test;
            cur_node->leaf = false;
            cur_node->left = new TreeNode;
            cur_node->right = new TreeNode;
        }
        else{
            cur_node->leaf = true;
        }
    }

    }

//    std::cout << "best tests regression: " << omp_get_wtime() - start << std::endl;

}

void HFTrain::apply_tests_to_train_samples(TrainSet &train_set, int train_samples, boost::unordered_map<TreeNode*, int> &new_node_samples)
{
    //apply splitting with best test found and count new node samples

    omp_set_num_threads(threads_per_tree_);

#pragma omp parallel
    {
        boost::unordered_map<TreeNode*, int> new_node_samples_local;

        #pragma omp for schedule(dynamic)
        for(int i=0; i<train_samples; ++i){
            if(!train_set[i]->node->leaf){
                //set node number for new level
                Test cur_test = train_set[i]->node->test;
                float val;
                if(cur_test.measure_mode == 0)
                    val = train_set[i]->feature_vector(cur_test.feature1) - train_set[i]->feature_vector(cur_test.feature2);
                else if(cur_test.measure_mode == 1)
                    val = train_set[i]->feature_vector(cur_test.feature1);

                if(val < cur_test.threshold)
                    train_set[i]->node = train_set[i]->node->left;
                else
                    train_set[i]->node = train_set[i]->node->right;

                if(new_node_samples_local.count(train_set[i]->node))
                    new_node_samples_local[train_set[i]->node]++;
                else
                    new_node_samples_local[train_set[i]->node] = 1;

            }
        }

        #pragma omp critical
        {

            for(boost::unordered_map<TreeNode*, int>::iterator t = new_node_samples_local.begin(); t != new_node_samples_local.end(); ++t){
                if(new_node_samples.count(t->first))
                    new_node_samples[t->first] += t->second;
                else
                    new_node_samples[t->first] = t->second;
            }

        }

    }

//    std::cout << "New node samples: " << new_node_samples[train_set[0]->node] << " " << new_node_samples[level_nodes[0]->right] << std::endl;

}

void HFTrain::optimize_level(TrainSet &train_set,
                             int train_samples,
                             int cur_level,
                             const std::vector<TreeNode*> &level_nodes,
                             boost::unordered_map<TreeNode*, int> &new_node_samples
                             )
{

    std::cout << "Depth: " << cur_level << "  nodes: " << level_nodes.size() << std::endl;
//    double start = omp_get_wtime();

    //get random test features, not thresholds yet
    boost::unordered_map<TreeNode*, std::vector<Test> > node_tests;
    get_random_features(level_nodes, node_tests);

    //get min-max for each feature for each node to constraint thresholds' range
    boost::unordered_map<TreeNode*, std::vector<float> > node_test_min;
    boost::unordered_map<TreeNode*, std::vector<float> > node_test_max;
    boost::unordered_map<TreeNode*, std::vector<int> > node_class_samples;
    boost::unordered_map<TreeNode*, int> node_samples;

    get_min_max_count_samples(train_set,
                              train_samples,
                              level_nodes,
                              node_tests,
                              node_test_min,
                              node_test_max,
                              node_class_samples,
                              node_samples);

    get_random_thresholds(level_nodes, node_test_min, node_test_max, node_tests);

    //0->classification
    //1->regression of x,y,z
    //2->refression of yaw,pitch,roll
    int split_method;
    if(cur_level < 4)
        split_method = 0;
    else
        split_method = rand() % 3;

//    std::cout << "Before split: " << omp_get_wtime() - start << std::endl;

    if(split_method == 0){
        //do classification
        find_classification_split(train_set,
                                  train_samples,
                                  level_nodes,
                                  node_tests,
                                  node_class_samples,
                                  node_samples);
    } else if(split_method == 1){
        //do regression of object coordinates
        find_regression_location_split(train_set,
                                  train_samples,
                                  level_nodes,
                                  node_tests,
                                  node_class_samples,
                                  node_samples);
    } else if(split_method == 2){
        //do regression yaw pitch roll
        find_regression_pose_split(train_set,
                                  train_samples,
                                  level_nodes,
                                  node_tests,
                                  node_class_samples,
                                  node_samples);
    }

    //apply best tests that were stored to the nodes of each train sample
    apply_tests_to_train_samples(train_set, train_samples, new_node_samples);

//    std::cout << "After split " << split_method << ": " << omp_get_wtime() - start << std::endl;

}

//train depth by depth, all nodes together!
void HFTrain::train_tree(TreeNode *root, TrainSet& train_set){

    //every training sample is set to the root
    for(int i=0; i<train_set.size(); ++i)
        train_set[i]->node = root;

    //get 66.6% of training samples at beginning
    int train_samples = 2.0f/3.0f*(float)train_set.size();
    std::vector<int> samples_per_class;
    suffle_training_set(train_set, train_samples, samples_per_class);

    //add the root
    std::vector<TreeNode*> cur_level_nodes;
    cur_level_nodes.push_back(root);

    int cur_level = 0;
    int cur_leaf_id = 0;
    while(cur_level_nodes.size() > 0) {

        boost::unordered_map<TreeNode*, int> new_node_samples;
        optimize_level(train_set, train_samples, cur_level, cur_level_nodes, new_node_samples);

        std::vector<TreeNode*> new_level_nodes;
        for(int i=0; i<cur_level_nodes.size(); ++i){
            if(!cur_level_nodes[i]->leaf){
                if(new_node_samples[cur_level_nodes[i]->left] > min_samples_)
                    new_level_nodes.push_back(cur_level_nodes[i]->left);
                else{
                    //make leaf left
                    num_leaves_++;
                    cur_level_nodes[i]->left->leaf = true;
                }
                if(new_node_samples[cur_level_nodes[i]->right] > min_samples_)
                    new_level_nodes.push_back(cur_level_nodes[i]->right);
                else{
                    //make leaf right
                    num_leaves_++;
                    cur_level_nodes[i]->right->leaf = true;
                }

            }
        }
//        double start = omp_get_wtime();
        make_leafs(train_set, train_samples, samples_per_class, cur_leaf_id);
//        std::cout << "Leafs made in: " << omp_get_wtime() - start << std::endl;

        //put train samples that belong to leaf nodes to the end and reduce the size
        //of training samples accordingly
        clean_trainset(train_set, train_samples);
//        std::cout << "Train samples after clean: :" << train_samples << std::endl;

        cur_level_nodes = new_level_nodes;
        cur_level++;
//        std::cout << "Current level: " << cur_level << std::endl;
//        std::cout << "Level nodes: " << cur_level_nodes.size() << std::endl;
//        std::cout << "Total Leaf nodes: " << num_leaves_ << std::endl;

    }

}

void HFTrain::train()
{

    omp_set_nested(1);
    omp_set_num_threads(threads_for_parallel_trees_);
    #pragma omp parallel
    {
        //each thread should have different seed
        //otherwise random numbers would be the same in all tress
        srand( int(time(NULL)) ^ omp_get_thread_num() );

        //each thread should read its own input to avoid confict
        TrainSet train_set;
        int feature_vector_length, number_of_classes;
        std::stringstream msg;
        msg << "Thread " << omp_get_thread_num() << ": Reading input..." << std::endl;
        std::cout << msg.str();
        getTrainSet(train_set, number_of_classes, feature_vector_length);
        msg.str("");
        msg << "Thread " << omp_get_thread_num() << ": Finished reading " << train_set.size() << " train samples." << std::endl;
        std::cout << msg.str();

        if(omp_get_thread_num() == 0){
            number_of_classes_ = number_of_classes;
            feature_vector_length_ = feature_vector_length;
            //write forest.txt
            std::ofstream finfo((output_folder_ + "/forest.txt").c_str());
            finfo << number_of_trees_ << " " <<
                     number_of_classes_ << " " <<
                     feature_vector_length_ << " " <<
                     patch_size_in_voxels_ << " " <<
                     voxel_size_in_m_ << std::endl;
            finfo.close();
        }
        //wait until member variables are written
        #pragma omp barrier

        //start training
        #pragma omp for schedule(dynamic)
        for(int t=start_tree_no_; t< start_tree_no_ + number_of_trees_; ++t){

            std::stringstream s;
            s << output_folder_ << "/tree" << t << ".dat";
            std::ofstream f(s.str().c_str(), std::ios::out | std::ios::binary);
            CHECK(f) << "Could not write to file " << s.str();

            TreeNode *tree = new TreeNode;

            msg.str("");
            msg << "Training tree: " << t << std::endl;
            std::cout << msg.str();
            double t0 = omp_get_wtime();
            train_tree(tree, train_set);
            double t1 = omp_get_wtime();

            saveTreeNode(tree, f);
            msg.str("");
            msg << "Tree " << t << " saved - " << t1-t0 << "sec" << std::endl;
            std::cout << msg.str();

            deleteTreeNode(tree);
            delete tree;
        }

    } //omp parallel

}


#include "gpuNlcgSenseGadget.h"
#include "cuNDArray_operators.h"
#include "cuNDArray_elemwise.h"
#include "cuNDArray_blas.h"
#include "cuNDArray_utils.h"
#include "cuNDArray_reductions.h"
#include "GadgetMRIHeaders.h"
#include "b1_map.h"
#include "GPUTimer.h"
#include "vector_td_utilities.h"
#include "hoNDArray_fileio.h"
#include "ismrmrd/xml.h"
#include <boost/thread/mutex.hpp>

namespace Gadgetron{

#define max_number_of_gpus 10
  static boost::mutex _mutex[max_number_of_gpus];

  gpuNlcgSenseGadget::gpuNlcgSenseGadget()
    : is_configured_(false)
    , prepared_(false)
    , channels_(0)
    , frame_counter_(0)
  {
    matrix_size_ = uint64d2(0,0);
    matrix_size_os_ = uint64d2(0,0);
    matrix_size_seq_ = uint64d2(0,0);
  }

  gpuNlcgSenseGadget::~gpuNlcgSenseGadget() {}

  int gpuNlcgSenseGadget::process_config( ACE_Message_Block* mb )
  {
    GDEBUG("gpuNlcgSenseGadget::process_config\n");

    device_number_ = deviceno.value();

    int number_of_devices = 0;
    if (cudaGetDeviceCount(&number_of_devices)!= cudaSuccess) {
      GDEBUG( "Error: unable to query number of CUDA devices.\n" );
      return GADGET_FAIL;
    }

    if (number_of_devices == 0) {
      GDEBUG( "Error: No available CUDA devices.\n" );
      return GADGET_FAIL;
    }

    if (device_number_ >= number_of_devices) {
      GDEBUG("Adjusting device number from %d to %d\n", device_number_,  (device_number_%number_of_devices));
      device_number_ = (device_number_%number_of_devices);
    }

    if (cudaSetDevice(device_number_)!= cudaSuccess) {
      GDEBUG( "Error: unable to set CUDA device.\n" );
      return GADGET_FAIL;
    }

    set_number_ = setno.value();
    slice_number_ = sliceno.value();

    number_of_cg_iterations_ = number_of_cg_iterations.value();
    cg_limit_ = cg_limit.value();
    oversampling_factor_ = oversampling_factor.value();
    kernel_width_ = kernel_width.value();

    lambda_ = lambda.value();
    alpha_ = alpha.value();
    rotations_to_discard_ = rotations_to_discard.value();
    output_convergence_ = output_convergence.value();
    exclusive_access_ = exclusive_access.value();

    if( (rotations_to_discard_%2) == 1 ){
      GDEBUG("#rotations to discard must be even.\n");
      return GADGET_FAIL;
    }

    // Get the Ismrmrd header
    //
    ISMRMRD::IsmrmrdHeader h;
    ISMRMRD::deserialize(mb->rd_ptr(),h);

    if (h.encoding.size() != 1) {
      GDEBUG("This Gadget only supports one encoding space\n");
      return GADGET_FAIL;
    }

    // Get the encoding space and trajectory description
    ISMRMRD::EncodingSpace e_space = h.encoding[0].encodedSpace;
    ISMRMRD::EncodingSpace r_space = h.encoding[0].reconSpace;
    ISMRMRD::EncodingLimits e_limits = h.encoding[0].encodingLimits;

    matrix_size_seq_ = uint64d2( r_space.matrixSize.x, r_space.matrixSize.y );

    if (!is_configured_) {

      if (h.acquisitionSystemInformation) {
	channels_ = h.acquisitionSystemInformation->receiverChannels ? *h.acquisitionSystemInformation->receiverChannels : 1;
      } else {
	channels_ = 1;
      }

      // Allocate encoding operator for non-Cartesian Sense
      E_ = boost::shared_ptr< cuNonCartesianSenseOperator<float,2> >( new cuNonCartesianSenseOperator<float,2>() );

		// Allocate preconditioner
      D_ = boost::shared_ptr< cuCgPreconditioner<float_complext> >( new cuCgPreconditioner<float_complext>() );

      TV_ = boost::shared_ptr<cuTvOperator<float_complext,3> >(new cuTvOperator<float_complext,3>);
      PICS_ = boost::shared_ptr<cuTvPicsOperator<float_complext,3> >(new cuTvPicsOperator<float_complext,3>);

      // Setup NLCG solver
      solver_ = cuNlcgSolver<float_complext>();
      solver_.set_encoding_operator( E_ );

      solver_.set_output_mode( (output_convergence_) ? cuNlcgSolver<float_complext>::OUTPUT_VERBOSE : cuNlcgSolver<float_complext>::OUTPUT_SILENT );
      solver_.set_max_iterations( number_of_cg_iterations_ );
      solver_.set_tc_tolerance(cg_limit_);
      solver_.set_preconditioner( D_ );

      is_configured_ = true;
    }

    GDEBUG("gpuNlcgSenseGadget::end of process_config\n");

    return GADGET_OK;
  }

  int gpuNlcgSenseGadget::process(GadgetContainerMessage<ISMRMRD::ImageHeader> *m1, GadgetContainerMessage<GenericReconJob> *m2)
  {
    // Is this data for this gadget's set/slice?
    //

    if( m1->getObjectPtr()->set != set_number_ || m1->getObjectPtr()->slice != slice_number_ ) {
      // No, pass it downstream...
      return this->next()->putq(m1);
    }

    //GDEBUG("gpuNlcgSenseGadget::process\n");
    //GPUTimer timer("gpuNlcgSenseGadget::process");

    if (!is_configured_) {
      GDEBUG("\nData received before configuration complete\n");
      return GADGET_FAIL;
    }

    GenericReconJob* j = m2->getObjectPtr();

    // Let's first check that this job has the required data...
    if (!j->csm_host_.get() || !j->dat_host_.get() || !j->tra_host_.get() || !j->dcw_host_.get()) {
      GDEBUG("Received an incomplete Sense job\n");
      return GADGET_FAIL;
    }

    unsigned int samples = j->dat_host_->get_size(0);
    unsigned int channels = j->dat_host_->get_size(1);
    unsigned int rotations = samples / j->tra_host_->get_number_of_elements();
    unsigned int frames = j->tra_host_->get_size(1)*rotations;

    if( samples%j->tra_host_->get_number_of_elements() ) {
      GDEBUG("Mismatch between number of samples (%d) and number of k-space coordinates (%d).\nThe first should be a multiplum of the latter.\n",
                    samples, j->tra_host_->get_number_of_elements());
      return GADGET_FAIL;
    }

    boost::shared_ptr< cuNDArray<floatd2> > traj(new cuNDArray<floatd2> (j->tra_host_.get()));
    boost::shared_ptr< cuNDArray<float> > dcw(new cuNDArray<float> (j->dcw_host_.get()));
    boost::shared_ptr< cuNDArray<float_complext> > csm(new cuNDArray<float_complext> (j->csm_host_.get()));
    boost::shared_ptr< cuNDArray<float_complext> > device_samples(new cuNDArray<float_complext> (j->dat_host_.get()));

    if( !prepared_){

      // Take the reconstruction matrix size from the regulariaztion image.
      // It could be oversampled from the sequence specified size...

      matrix_size_ = uint64d2( j->reg_host_->get_size(0), j->reg_host_->get_size(1) );

      cudaDeviceProp deviceProp;
      if( cudaGetDeviceProperties( &deviceProp, device_number_ ) != cudaSuccess) {
        GDEBUG( "\nError: unable to query device properties.\n" );
        return GADGET_FAIL;
      }

      unsigned int warp_size = deviceProp.warpSize;

      matrix_size_os_ =
        uint64d2(((static_cast<unsigned int>(std::ceil(matrix_size_[0]*oversampling_factor_))+warp_size-1)/warp_size)*warp_size,
                 ((static_cast<unsigned int>(std::ceil(matrix_size_[1]*oversampling_factor_))+warp_size-1)/warp_size)*warp_size);

      GDEBUG("Matrix size    : [%d,%d] \n", matrix_size_[0], matrix_size_[1]);
      GDEBUG("Matrix size OS : [%d,%d] \n", matrix_size_os_[0], matrix_size_os_[1]);

      std::vector<size_t> image_dims = to_std_vector(matrix_size_);
      image_dims.push_back(frames);

      E_->set_domain_dimensions(&image_dims);
      E_->set_codomain_dimensions(device_samples->get_dimensions().get());

      reg_image_ = boost::shared_ptr< cuNDArray<float_complext> >(new cuNDArray<float_complext>(&image_dims));

      // These operators need their domain/codomain set before being added to the solver
      //

      // Add "TV" regularization
      //

      if( lambda_ > 0.0 ){
      	TV_->set_weight((1.0-alpha_)*lambda_);
      	solver_.add_nonlinear_operator(TV_);
      }

      // Add "PICCS" regularization
      //

      if( alpha_ > 0.0 ){
        PICS_->set_prior(reg_image_);
        PICS_->set_weight(alpha_*lambda_);
        solver_.add_nonlinear_operator(PICS_);
      }

      prepared_ = true;
    }

    E_->set_dcw(dcw);
    E_->set_csm(csm);
    E_->setup( matrix_size_, matrix_size_os_, static_cast<float>(kernel_width_) );
    E_->preprocess(traj.get());

    // Expand the average image to the number of frames
    //

    {
      cuNDArray<float_complext> tmp(*j->reg_host_);
      *reg_image_ = expand( tmp, frames );
    }

    // Define preconditioning weights
    //

    boost::shared_ptr< cuNDArray<float> > _precon_weights = sum(abs_square(csm.get()).get(), 2);
    reciprocal_sqrt_inplace(_precon_weights.get());
    boost::shared_ptr< cuNDArray<float_complext> > precon_weights = real_to_complex<float_complext>( _precon_weights.get() );
    _precon_weights.reset();
    D_->set_weights( precon_weights );
    precon_weights.reset();

    //Apply weights
    *device_samples *= *dcw;

    // Invoke solver
    //

    boost::shared_ptr< cuNDArray<float_complext> > result;
    {
      GDEBUG("Running NLCG solver\n");
      GPUTimer timer("Running NLCG solver");

      // Optionally, allow exclusive (per device) access to the solver
      // This may not matter much in terms of speed, but it can in terms of memory consumption
      //

      if( exclusive_access_ )
        _mutex[device_number_].lock();

      result = solver_.solve(device_samples.get());

      if( exclusive_access_ )
        _mutex[device_number_].unlock();
    }

    // Provide some info about the scaling between the regularization and reconstruction.
    // If it is not close to one, PICCS does not work optimally...
    //

    if( alpha_ > 0.0 ){
      cuNDArray<float_complext> gpureg(j->reg_host_.get());
      boost::shared_ptr< cuNDArray<float_complext> > gpurec = sum(result.get(),2);
      *gpurec /= float(result->get_size(2));
      float scale = abs(dot(gpurec.get(), gpurec.get())/dot(gpurec.get(),&gpureg));
      GDEBUG("Scaling factor between regularization and reconstruction is %f.\n", scale);
    }

    if (!result.get()) {
      GDEBUG("\nNon-linear conjugate gradient solver failed\n");
      return GADGET_FAIL;
    }

    /*
      static int counter = 0;
      char filename[256];
      sprintf((char*)filename, "recon_sb_%d.cplx", counter);
      write_nd_array<float_complext>( sbresult->to_host().get(), filename );
      counter++; */

    // If the recon matrix size exceeds the sequence matrix size then crop
    if( matrix_size_seq_ != matrix_size_ )
      *result = crop<float_complext,2>( (matrix_size_-matrix_size_seq_)>>1, matrix_size_seq_, *result );

    // Now pass on the reconstructed images
    //

    unsigned int frames_per_rotation = frames/rotations;

    if( rotations == 1 ){ // this is the case for golden ratio
      rotations = frames;
      frames_per_rotation = 1;
    }

    for( unsigned int frame=0; frame<frames; frame++ ){

      unsigned int rotation_idx = frame/frames_per_rotation;

      // Check if we should discard this frame
      if( rotation_idx < (rotations_to_discard_>>1) || rotation_idx >= rotations-(rotations_to_discard_>>1) )
        continue;

      GadgetContainerMessage< hoNDArray< std::complex<float> > > *cm =
        new GadgetContainerMessage< hoNDArray< std::complex<float> > >();

      GadgetContainerMessage<ISMRMRD::ImageHeader> *m =
        new GadgetContainerMessage<ISMRMRD::ImageHeader>();

      *m->getObjectPtr() = j->image_headers_[frame];
      m->getObjectPtr()->matrix_size[0] = matrix_size_seq_[0];
      m->getObjectPtr()->matrix_size[1] = matrix_size_seq_[1];
      m->cont(cm);

      std::vector<size_t> img_dims(2);
      img_dims[0] = matrix_size_seq_[0];
      img_dims[1] = matrix_size_seq_[1];

      cm->getObjectPtr()->create(img_dims);

      size_t data_length = prod(matrix_size_seq_);

      cudaMemcpy(cm->getObjectPtr()->get_data_ptr(),
                 result->get_data_ptr()+frame*data_length,
                 data_length*sizeof(std::complex<float>),
                 cudaMemcpyDeviceToHost);

      cudaError_t err = cudaGetLastError();
      if( err != cudaSuccess ){
        GDEBUG("\nUnable to copy result from device to host: %s", cudaGetErrorString(err));
        m->release();
        return GADGET_FAIL;
      }

      m->getObjectPtr()->matrix_size[0] = img_dims[0];
      m->getObjectPtr()->matrix_size[1] = img_dims[1];
      m->getObjectPtr()->matrix_size[2] = 1;
      m->getObjectPtr()->channels       = 1;
      m->getObjectPtr()->image_index    = frame_counter_ + frame;

      if (this->next()->putq(m) < 0) {
        GDEBUG("\nFailed to result image on to Q\n");
        m->release();
        return GADGET_FAIL;
      }
    }

    frame_counter_ += frames;
    m1->release();
    return GADGET_OK;
  }

  GADGET_FACTORY_DECLARE(gpuNlcgSenseGadget)
}


#include "core/smpl/smpl.h"
#include <cilantro/point_cloud.hpp>
#include <cilantro/icp_common_instances.hpp>
#include <vector>
#include <Eigen/Eigen>

namespace surfelwarp {
    void SMPL::Transform(
            const surfelwarp::DeviceArrayView<float4> &live_vertex,
            cudaStream_t stream
    ) {
        std::vector<float3> smpl_host;
        m_smpl_vertices.download(smpl_host);
        std::vector<float3> smpl_norm_host;
        m_smpl_normals.download(smpl_norm_host);

        int num = 0;
        for (auto i = 0; i < smpl_host.size(); i++)
            if (smpl_host[i].z <= 0)
                num++;
        cilantro::PointCloud3f smpl_cloud;
        smpl_cloud.points.resize(cilantro::PointCloud3f::Dimension, num);
        smpl_cloud.normals.resize(cilantro::PointCloud3f::Dimension, num);
        num = 0;
        for (auto i = 0; i < smpl_host.size(); i++) {
            if (smpl_host[i].z <= 0) {
                smpl_cloud.points.col(num) = Eigen::Vector3f(smpl_host[i].x, smpl_host[i].y, smpl_host[i].z);
                smpl_cloud.normals.col(num++) = Eigen::Vector3f(smpl_norm_host[i].x, smpl_norm_host[i].y,
                                                                smpl_norm_host[i].z);
            }
        }

        std::vector<float4> live_host;
        live_vertex.Download(live_host);
        cilantro::PointCloud3f live_cloud;
        live_cloud.points.resize(cilantro::PointCloud3f::Dimension, live_host.size());
        for (auto i = 0; i < live_host.size(); i++)
            live_cloud.points.col(i) = Eigen::Vector3f(live_host[i].x, live_host[i].y, live_host[i].z);

        cilantro::SimpleCombinedMetricRigidICP3f icp(smpl_cloud.points, smpl_cloud.normals, live_cloud.points);

        // Parameter setting
        icp.setMaxNumberOfOptimizationStepIterations(10).setPointToPointMetricWeight(1.0f).setPointToPlaneMetricWeight(
                0.0f);
        icp.correspondenceSearchEngine().setMaxDistance(3.0f);
        icp.setConvergenceTolerance(0.0001f).setMaxNumberOfIterations(500);

        cilantro::RigidTransform3f tf_est = icp.estimate().getTransform().inverse();
        init_mat = mat34(tf_est.matrix());
        transform(stream);
        cudaStreamSynchronize(stream);
    }
}


/**
 * This is code is released under the
 * Apache License Version 2.0 http://www.apache.org/licenses/.
 *
 * (c) Daniel Lemire, http://lemire.me/en/
 */
#include <memory>
#include <iomanip>
#include <time.h>
#include  <util/compression/int/fastpfor/codecfactory.h>
#include  <util/compression/int/fastpfor/util.h>
#include  <util/compression/int/fastpfor/bitpacking.h>
#include  <util/compression/int/fastpfor/synthetic.h>

#include <boost/test/unit_test.hpp>
#include <boost/shared_ptr.hpp>

using namespace std;

BOOST_AUTO_TEST_CASE(fastpfor)
{
    CODECFactory fac;
    vector < boost::shared_ptr<IntegerCODEC> > myalgos = fac.allSchemes();
    for (uint32_t b = 0; b <= 28; ++b) {

        cout << "testing... b = " << b << endl;

        for (size_t length = 128; length < (1U << 12); length += 128) {

            vector<uint32_t, cacheallocator> data(length);
            data.reserve(length + 1024);
            for (size_t i = 0; i < data.size(); ++i) {
                data[i] = (i + (24 - i) * (12 - i)) % (1U << b);
            }
            for (vector<boost::shared_ptr<IntegerCODEC> >::iterator i =
                    myalgos.begin(); i != myalgos.end(); ++i) {
                IntegerCODEC & c = **i;
                //cout<<c.name() <<" ";
                //cout.flush();
                vector<uint32_t, cacheallocator> out(2 * data.size() + 1024);
                if (c.name() != "VSEncoding") // VSEncoding is fragile, output must be zero
                    for (uint32_t k = 0; k < out.size(); ++k)
                        out[k] = rand();
                size_t nvalue = out.size();
                // next magic is to satisfy cachepacking aligment requirements; should not hurt others
                uint32_t *aligned_out = &out[0];
                assert(!needPaddingTo64bytes(aligned_out));
                   //     reinterpret_cast<uint32_t *> (padTo64bytes(
                     //           reinterpret_cast<uint8_t *> (&out[0]))); // ofk
                c.encodeArray(&data[0], data.size(), aligned_out, nvalue);
                out.resize(nvalue);
                vector<uint32_t, cacheallocator> recover(data.size() + 1024);

                for (uint32_t k = 0; k < recover.size(); ++k)
                    recover[k] = rand();
                size_t recoveredsize = recover.size();
                c.decodeArray(aligned_out, out.size(), &recover[0],
                        recoveredsize);
                recover.resize(recoveredsize);
                if (recover != data) {
                    if (recoveredsize != data.size()) {
                        cerr << "lengths don't match!" << endl;
                        cerr << recoveredsize << endl;
                        cerr << data.size() << endl;
                    }
                    uint32_t howmany = 0;
                    for (size_t k = 0; k < data.size(); ++k)
                        if (recover[k] != data[k]) {
                            cout << "recovered[" << k << "] = " << recover[k]
                                    << endl;
                            cout << "data[" << k << "] = " << data[k] << endl;
                            howmany++;
                            if (howmany > 15)
                                break;
                        }
                    cerr << c.name() << endl;
                    cerr << length << endl;
                    throw logic_error("we have a bug");
                }
                //cout<<"ok."<<endl;
            }
        }

        for (size_t length = 0; length < (1U << 9); ++length) {
            vector<uint32_t, cacheallocator> data(length);
            data.reserve(length + 1024);
            for (size_t i = 0; i < data.size(); ++i) {
                data[i] = (33231 - i + i * i) % (1U << b);
            }
            for (vector<boost::shared_ptr<IntegerCODEC> >::iterator i =
                    myalgos.begin(); i != myalgos.end(); ++i) {
                IntegerCODEC & c = **i;
                vector<uint32_t, cacheallocator> out(2 * data.size() + 1024);
                if (c.name() != "VSEncoding") // VSEncoding is fragile, output must be zero
                    for (uint32_t k = 0; k < out.size(); ++k)
                        out[k] = rand();
                size_t nvalue = out.size();
                uint32_t *aligned_out = &out[0];
                      //  reinterpret_cast<uint32_t *> (padTo64bytes(
                        //        reinterpret_cast<uint8_t *> (&out[0]))); // ofk
                assert(!needPaddingTo64bytes(aligned_out));
                c.encodeArray(&data[0], data.size(), aligned_out, nvalue);
                out.resize(nvalue);
                vector<uint32_t, cacheallocator> recover(data.size() + 1024);
                //if(c.name() != "VSEncoding")
                for (uint32_t k = 0; k < recover.size(); ++k)
                    recover[k] = rand();
                size_t recoveredsize = recover.size();
                c.decodeArray(aligned_out, out.size(), &recover[0],
                        recoveredsize);
                recover.resize(recoveredsize);
                if (recoveredsize != data.size()) {
                    cerr << "lengths don't match!" << endl;
                    cerr << recoveredsize << endl;
                    cerr << data.size() << endl;
                    throw logic_error("we have a bug");
                }
                if (!equal(recover.begin(), recover.end(), data.begin())) {

                    uint32_t howmany = 0;
                    for (size_t k = 0; k < data.size(); ++k)
                        if (recover[k] != data[k]) {
                            cout << "recovered[" << k << "] = " << recover[k]
                                    << endl;
                            cout << "data[" << k << "] = " << data[k] << endl;
                            howmany++;
                            if (howmany > 15)
                                break;
                        }
                    cerr << c.name() << endl;
                    cerr << length << endl;
                    throw logic_error("we have a bug");
                }
            }
        }
    }
    cout << " testing large zipfian arrays..." << endl;
    for (size_t lengthb = 5; lengthb <= 25; lengthb += 5) {
        size_t length = 1U << lengthb;
        cout << endl << "length = " << length << endl;
        vector<uint32_t, cacheallocator> data = generateZipfianArray32(length,
                2, (1U << 9) - 1);
        data.reserve(data.size() + 1024);
        for (vector<boost::shared_ptr<IntegerCODEC> >::iterator i = myalgos.begin(); i
                != myalgos.end(); ++i) {
            IntegerCODEC & c = **i;
            cout << c.name();
            cout.flush();
            vector < uint32_t, cacheallocator> out(2 * data.size() + 1024);
            if (c.name() != "VSEncoding") // VSEncoding is fragile, output must be zero
                for (uint32_t k = 0; k < out.size(); ++k)
                    out[k] = rand();
            size_t nvalue = out.size();
            cout << " encoding ...";
            cout.flush();
            uint32_t *aligned_out = &out[0]; //reinterpret_cast<uint32_t *> (padTo64bytes(
                   // reinterpret_cast<uint8_t *> (&out[0]))); // ofk
            assert(!needPaddingTo64bytes(aligned_out));
            c.encodeArray(&data[0], data.size(), aligned_out, nvalue);
            out.resize(nvalue);
            assert(out.size() == nvalue); // paranoid
            vector<uint32_t, cacheallocator> recover(data.size() + 1024);
            assert(recover.size() == data.size()+1024); // being paranoid

            for (uint32_t k = 0; k < recover.size(); ++k)
                recover[k] = rand();
            size_t recoveredsize = recover.size();
            cout << " decoding ...";
            cout.flush();
            c.decodeArray(aligned_out, out.size(), &recover[0], recoveredsize);
            recover.resize(recoveredsize);
            if (recoveredsize != data.size()) {
                cerr << "lengths don't match!" << endl;
                cerr << recoveredsize << endl;
                cerr << data.size() << endl;
                throw logic_error("we have a bug");
            }
            if (!equal(recover.begin(), recover.end(), data.begin())) {
                uint32_t howmany = 0;
                for (size_t k = 0; k < data.size(); ++k)
                    if (recover[k] != data[k]) {
                        cout << "recovered[" << k << "] = " << recover[k]
                                << endl;
                        cout << "data[" << k << "] = " << data[k] << endl;
                        howmany++;
                        if (howmany > 15)
                            break;
                    }
                cerr << c.name() << endl;
                cerr << length << endl;
                throw logic_error("we have a bug");
            }
            cout << " ok!" << endl;
        }
    }

    cout << endl << endl << "testing...ok. Your code is good." << endl;
}


#ifndef INCLUDE_SWIFT_VIO_POINT_LANDMARK_MODELS_HPP_
#define INCLUDE_SWIFT_VIO_POINT_LANDMARK_MODELS_HPP_

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <ceres/ceres.h>
namespace swift_vio {
// Expressed in an anchor camera frame [\alpha, \beta, 1, \rho] = [x, y, z, w]/z.
class InverseDepthParameterization final: public ::ceres::LocalParameterization
{
public:
  static const int kModelId = 1;
  static const int kGlobalDim = 4;
  static const int kLocalDim = 3;

  // Generalization of the addition operation,
  //
  //   x_plus_delta = Plus(x, delta)
  //
  // with the condition that Plus(x, 0) = x.
  bool Plus(const double* x, const double* delta, double* x_plus_delta) const {
    return plus(x, delta, x_plus_delta);
  }

  // The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
  //
  // jacobian is a row-major GlobalSize() x LocalSize() matrix.
  bool ComputeJacobian(const double* /*x*/, double* jacobian) const {
    Eigen::Map<Eigen::Matrix<double, kGlobalDim, kLocalDim, Eigen::RowMajor>> j(jacobian);
    j.setZero();
    j(0, 0) = 1.0;
    j(1, 1) = 1.0;
    j(3, 2) = 1.0;
    return true;
  }

  // Size of x.
  int GlobalSize() const final {
    return kGlobalDim;
  }

  // Size of delta.
  int LocalSize() const final {
    return kLocalDim;
  }

  static bool plus(const double* x, const double* delta, double* x_plus_delta);

  static bool minus(const double* x, const double* x_plus_delta, double* delta);

  static bool liftJacobian(const double* /*x*/, double* jacobian) {
    Eigen::Map<Eigen::Matrix<double, kLocalDim, kGlobalDim, Eigen::RowMajor>> j(jacobian);
    j.setZero();
    j(0, 0) = 1.0;
    j(1, 1) = 1.0;
    j(2, 3) = 1.0;
    return true;
  }
};

// [x, y, z, w, c, s]
// [x, y, z, w] is the quaternion underlying unit bearing vector n such that
// n = q(w, x, y, z) * [0, 0, 1]'.
// c, s are cos(theta) and sin(theta) where \theta is the parallax angle.
class ParallaxAngleParameterization final: public ::ceres::LocalParameterization {
public:
  static const int kModelId = 2;
  static const int kGlobalDim = 6;
  static const int kLocalDim = 3;

  // Generalization of the addition operation,
  //
  //   x_plus_delta = Plus(x, delta)
  //
  // with the condition that Plus(x, 0) = x.
  bool Plus(const double* x, const double* delta, double* x_plus_delta) const {
    return plus(x, delta, x_plus_delta);
  }

  // The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
  //
  // jacobian is a row-major GlobalSize() x LocalSize() matrix.
  bool ComputeJacobian(const double* /*x*/, double* jacobian) const {
    Eigen::Map<Eigen::Matrix<double, kGlobalDim, kLocalDim, Eigen::RowMajor>> j(jacobian);
    j.setIdentity();
    return true;
  }

  // Size of x.
  int GlobalSize() const final {
    return kGlobalDim;
  }

  // Size of delta.
  int LocalSize() const final {
    return kLocalDim;
  }

  /// \brief Generalization of the addition operation,
  ///        x_plus_delta = Plus(x, delta)
  ///        with the condition that Plus(x, 0) = x.
  /// @param[in] x Variable.
  /// @param[in] delta Perturbation.
  /// @param[out] x_plus_delta Perturbed x.
  static bool plus(const double* x, const double* delta, double* x_plus_delta);

  /// \brief Computes the Jacobian from minimal space to naively overparameterised space as used by ceres.
  /// @param[in] x Variable.
  /// @param[out] jacobian the Jacobian (dimension minDim x dim).
  /// \return True on success.
  static bool liftJacobian(const double* /*x*/, double* jacobian) {
    Eigen::Map<Eigen::Matrix<double, kLocalDim, kGlobalDim, Eigen::RowMajor>> j(jacobian);
    j.setIdentity();
    return true;
  }
};

// add the model switch functions
} // namespace swift_vio

#endif // INCLUDE_SWIFT_VIO_POINT_LANDMARK_MODELS_HPP_


//  (C) Copyright 2014 Alvaro J. Genial (http://alva.ro)
//  Use, modification and distribution are subject to the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt).

#ifndef AJG_SYNTH_ADAPTERS_NUMERIC_ADAPTER_HPP_INCLUDED
#define AJG_SYNTH_ADAPTERS_NUMERIC_ADAPTER_HPP_INCLUDED

#include <iomanip>

#include <boost/io/ios_state.hpp>
#include <boost/type_traits/is_integral.hpp>

#include <ajg/synth/detail/is_integer.hpp>
#include <ajg/synth/adapters/concrete_adapter.hpp>

namespace ajg {
namespace synth {

//
// numeric_adapter:
//     Base adapter implementation for primitive numeric types
////////////////////////////////////////////////////////////////////////////////////////////////////

template <class Behavior, class Adapted>
struct numeric_adapter : concrete_adapter<Behavior, Adapted> {
    numeric_adapter(Adapted const& adapted) : concrete_adapter<Behavior, Adapted>(adapted) {}

    AJG_SYNTH_ADAPTER_TYPEDEFS(Behavior);

    boolean_type  is_numeric()  const { return true; }
    floating_type to_floating() const { return static_cast<floating_type>(this->adapted()); }
    boolean_type  to_boolean()  const { return this->adapted() != static_cast<Adapted>(0); }

    void input (istream_type& in)        { in >> this->adapted(); }
    void output(ostream_type& out) const {
        // The fast, happy, integral path:
        if (boost::is_integral<Adapted>::value) {
            out << this->adapted();
            return;
        }

        // The slow, sad, fractional path:
        boost::io::basic_ios_all_saver<char_type> saver(out);

        if (detail::is_integer(this->adapted())) {
            out << std::fixed << std::setprecision(0);
        }

        if (out.flags() & out.hex) {
        // If they hex is desired, we convert to an
        // integral first, and then output as hex.
            out << static_cast<integer_type>(this->adapted());
        }
        else {
            out << this->adapted();
        }
    }
};

}} // namespace ajg::synth

#endif // AJG_SYNTH_ADAPTERS_NUMERIC_ADAPTER_HPP_INCLUDED


/*! *****************************************************************************
 *   \file libSampler.cpp
 *   \author moennen
 *   \brief
 *   \date 2018-04-06
 *   *****************************************************************************/

#include "sampleBuffDataset/libBuffDatasetSampler.h"

#include "utils/cv_utils.h"
#include "utils/imgFileLst.h"
#include "utils/Hop.h"

#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>

#include <glm/glm.hpp>

#include <fstream>
#include <string>
#include <vector>
#include <array>

#include <ctime>
#include <iostream>
#include <map>

using namespace std;
using namespace cv;
using namespace glm;

#define TRACE cout << __PRETTY_FUNCTION__ << "@" << __LINE__ << endl;

namespace
{
struct Sampler final
{
   boost::random::mt19937 _rng;
   boost::random::uniform_int_distribution<> _dataGen;
   boost::random::uniform_real_distribution<> _tsGen;

   static constexpr float maxDsScaleFactor = 3.5;

   const ivec3 _sampleSz;

   enum
   {
      nBuffers = 2
   };
   ImgNFileLst<nBuffers> _data;

   inline static unsigned getBufferDepth( const unsigned buffId ) { return buffId == 0 ? 3 : 1; }

   Sampler( const char* dataSetPath, const char* dataPath, const ivec3 sampleSz, const int seed )
       : _rng( seed ), _tsGen( 0.0, 1.0 ), _sampleSz( sampleSz )
   {
      HOP_PROF_FUNC();

      _data.open( dataSetPath, dataPath );

      if ( _data.size() )
      {
         std::cout << "Read dataset " << dataSetPath << " (" << _data.size() << ") " << std::endl;
      }
      _dataGen = boost::random::uniform_int_distribution<>( 0, _data.size() - 1 );
   }

   bool sample( float* buff )
   {
      HOP_PROF_FUNC();

      const unsigned depthBuffSz = _sampleSz.y * _sampleSz.z;
      const unsigned imgBuffSz = depthBuffSz * 3;

      float* currBuffImg = buff;
      float* currBuffDepth = buff + imgBuffSz * _sampleSz.x ;

      for ( size_t s = 0; s < _sampleSz.x; ++s )
      {
         const ImgNFileLst<nBuffers>::Data& data = _data[_dataGen( _rng )];

         Mat currImg = cv_utils::imread32FC3( data[0], false/*toLinear*/, true/*toRGB*/ );
         Mat currDepth = cv_utils::imread32FC1( data[1] );

         ivec2 imgSz( currImg.cols, currImg.rows );

         // ignore too small samples
         if ( ( imgSz.x < _sampleSz.y ) || ( imgSz.y < _sampleSz.z ) )
         {
            --s;
            continue;
         }

         // bad dataset : the samples o be of the same size
         if ( ( currDepth.cols != imgSz.x ) || ( currDepth.rows != imgSz.y ) ) return false;

         // random rescale
         const float minDs = std::max( (float)_sampleSz.z / imgSz.y, (float)_sampleSz.y / imgSz.x );
         const float ds = mix( std::min( 1.0f, maxDsScaleFactor * minDs ), minDs, _tsGen( _rng ) );
         resize( currImg, currImg, Size(), ds, ds, CV_INTER_AREA );
         resize( currDepth, currDepth, Size(), ds, ds, CV_INTER_AREA );
         imgSz = ivec2( currImg.cols, currImg.rows );

         // random translate
         const ivec2 trans(
             std::floor( _tsGen( _rng ) * ( imgSz.x - _sampleSz.y ) ),
             std::floor( _tsGen( _rng ) * ( imgSz.y - _sampleSz.z ) ) );

         // crop
         currImg = currImg( Rect( trans.x, trans.y, _sampleSz.y, _sampleSz.z ) );
         currDepth = currDepth( Rect( trans.x, trans.y, _sampleSz.y, _sampleSz.z ) );

         // random small blur to remove artifacts + copy to destination
         Mat imgSple( _sampleSz.z, _sampleSz.y, CV_32FC3, currBuffImg );
         GaussianBlur( currImg, imgSple, Size( 5, 5 ), 1.5 * _tsGen( _rng ) );
         Mat depthSple( _sampleSz.z, _sampleSz.y, CV_32FC1, currBuffDepth );
         GaussianBlur( currDepth, depthSple, Size( 5, 5 ), 1.5 * _tsGen( _rng ) );
         //cv_utils::normalizeMeanStd(depthSple);
         normalize( depthSple, depthSple, 0, 1, NORM_MINMAX );

         currBuffImg += imgBuffSz;
         currBuffDepth += depthBuffSz;
      }

      return true;
   }

   size_t nSamples() const { return _data.size(); }
   ivec3 sampleSizes() const { return _sampleSz; }
};

array<unique_ptr<Sampler>, 33> g_samplers;
};

extern "C" int getNbBuffers( const int /*sidx*/ ) { return Sampler::nBuffers; }

extern "C" int getBuffersDim( const int sidx, float* dims )
{
   HOP_PROF_FUNC();

   if ( !g_samplers[sidx].get() ) return ERROR_UNINIT;

   const ivec3 sz = g_samplers[sidx]->sampleSizes();
   float* d = dims;
   for ( size_t i = 0; i < Sampler::nBuffers; ++i )
   {
      d[0] = sz.y;
      d[1] = sz.z;
      d[2] = Sampler::getBufferDepth( i );
      d += 3;
   }

   return SUCCESS;
}

extern "C" int initBuffersDataSampler(
    const int sidx,
    const char* datasetPath,
    const char* dataPath,
    const int nParams,
    const float* params,
    const int seed )
{
   HOP_PROF_FUNC();

   // check input
   if ( ( nParams < 3 ) || ( sidx > g_samplers.size() ) ) return ERROR_BAD_ARGS;

   // parse params
   const ivec3 sz( params[0], params[1], params[2] );
   g_samplers[sidx].reset( new Sampler( datasetPath, dataPath, sz, seed ) );

   return g_samplers[sidx]->nSamples() ? SUCCESS : ERROR_BAD_DB;
}

extern "C" int getBuffersDataSample( const int sidx, float* buff )
{
   HOP_PROF_FUNC();

   if ( !g_samplers[sidx].get() ) return ERROR_UNINIT;

   // sample
   if ( !g_samplers[sidx]->sample( buff ) ) return ERROR_GENERIC;

   return SUCCESS;
}


/*

Copyright (c) 2005-2018, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

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 the University of Oxford 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 HOLDER 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.

*/

#ifndef BUSKEADHESIVEFORCE_HPP_
#define BUSKEADHESIVEFORCE_HPP_

#include "AbstractTwoBodyInteractionForce.hpp"

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>

/**
 * A force law employed by Buske et al (2011) in their overlapping spheres
 * model of the intestinal crypt (doi:10.1371/journal.pcbi.1001045).
 *
 * Length is scaled by natural length. \todo does this mean natural radius of a cell? If so at what age? (#1764)
 * Time is in hours.
 */
template<unsigned DIM>
class BuskeAdhesiveForce : public AbstractTwoBodyInteractionForce<DIM>
{
    friend class TestForcesNotForRelease;
private:

    /** Needed for serialization. */
    friend class boost::serialization::access;
    /**
     * Archive the object and its member variables.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractTwoBodyInteractionForce<DIM> >(*this);
        archive & mAdhesionEnergyParameter;
    }

    /**
     * Adhesion energy parameter.
     *
     * Represented by the parameter epsilon in the model by Buske et al (2011) in
     * their off-lattice model of the intestinal crypt
     * (doi:10.1371/journal.pcbi.1001045).
     */
    double mAdhesionEnergyParameter;

public:

    /**
     * Constructor.
     */
    BuskeAdhesiveForce();

    /**
     * @return #mAdhesionEnergyParameter.
     */
    double GetAdhesionEnergyParameter();

    /**
     * Set mAdhesionEnergyParameter.
     *
     * @param adhesionEnergyParameter the new value of mAdhesionEnergyParameter
     */
    void SetAdhesionEnergyParameter(double adhesionEnergyParameter);

    /**
     * @return the force between two nodes.
     *
     * Note that this assumes they are connected and is called by rCalculateVelocitiesOfEachNode()
     *
     * @param nodeAGlobalIndex index of one neighbouring node
     * @param nodeBGlobalIndex index of the other neighbouring node
     * @param rCellPopulation the cell population
     */
    c_vector<double, DIM> CalculateForceBetweenNodes(unsigned nodeAGlobalIndex, unsigned nodeBGlobalIndex, AbstractCellPopulation<DIM>& rCellPopulation);

    /**
     * @return Calculated magnitude of the force between two nodes that are a given distance apart and
     * are associated with given cell radii.
     *
     * @param distanceBetweenNodes the distance between two nodes
     * @param radiusOfCellOne radius of a cell
     * @param radiusOfCellTwo radius of a cell
     */
    double GetMagnitudeOfForce(double distanceBetweenNodes, double radiusOfCellOne, double radiusOfCellTwo);

    /**
     * Overridden OutputForceParameters() method.
     *
     * @param rParamsFile the file stream to which the parameters are output
     */
    virtual void OutputForceParameters(out_stream& rParamsFile);
};

#include "SerializationExportWrapper.hpp"
EXPORT_TEMPLATE_CLASS_SAME_DIMS(BuskeAdhesiveForce)

#endif /*BUSKEADHESIVEFORCE_HPP_*/


/******************************************************************************
 * Copyright (C) 2013 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 *                                                                            *
 * This program is free software; you can redistribute it and/or modify       *
 * it under the terms of the Lesser GNU General Public License as published by*
 * the Free Software Foundation; either version 3 of the License, or          *
 * (at your option) any later version.                                        *
 *                                                                            *
 * This program is distributed in the hope that it will be useful,            *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of             *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              *
 * Lesser GNU General Public License for more details.                        *
 *                                                                            *
 * You should have received a copy of the Lesser GNU General Public License   *
 * along with this program. If not, see <http://www.gnu.org/licenses/>.       *
 ******************************************************************************/

#include "aslam/calibration/error-terms/ErrorTermWheelsZ.h"

#include <Eigen/Dense>

using namespace aslam::backend;

namespace aslam {
  namespace calibration {

/******************************************************************************/
/* Constructors and Destructor                                                */
/******************************************************************************/

  ErrorTermWheelsZ::ErrorTermWheelsZ(const aslam::backend::EuclideanExpression& v_w_mwl,
                                     const aslam::backend::EuclideanExpression& v_w_mwr,
                                     const Covariance& sigma2_vert_vel) :
        _v_w_mwl(v_w_mwl),
        _v_w_mwr(v_w_mwr),
        _sigma2_vert_vel(sigma2_vert_vel) {
      setInvR(_sigma2_vert_vel.inverse());
      DesignVariable::set_t dv;
      _v_w_mwl.getDesignVariables(dv);
      _v_w_mwr.getDesignVariables(dv);
      setDesignVariablesIterator(dv.begin(), dv.end());
    }

  ErrorTermWheelsZ::ErrorTermWheelsZ(const ErrorTermWheelsZ& other) :
        ErrorTermFs<1>(other),
        _v_w_mwl(other. _v_w_mwl),
        _v_w_mwr(other. _v_w_mwr),
        _sigma2_vert_vel(other._sigma2_vert_vel){
    }

  ErrorTermWheelsZ& ErrorTermWheelsZ::operator =
        (const ErrorTermWheelsZ& other) {
      if (this != &other) {
        ErrorTermFs<1>::operator=(other);
        _v_w_mwl = other._v_w_mwl;
        _v_w_mwr = other._v_w_mwr;
       _sigma2_vert_vel = other._sigma2_vert_vel;
      }
      return *this;
    }

    ErrorTermWheelsZ::~ErrorTermWheelsZ() {
    }

/******************************************************************************/
/* Methods                                                                    */
/******************************************************************************/

    double ErrorTermWheelsZ::evaluateErrorImplementation() {
      error_t error;
      const double vzl = _v_w_mwl.toEuclidean()(2);
      const double vzr = _v_w_mwr.toEuclidean()(2);

      error(0) = vzl + vzr;

      setError(error);
      return evaluateChiSquaredError();
    }

    void ErrorTermWheelsZ::evaluateJacobiansImplementation(JacobianContainer&
        jacobians) {
      Eigen::Matrix<double, 1, 3> Jl = Eigen::Matrix<double, 1, 3>::Zero();
      Eigen::Matrix<double, 1, 3> Jr = Eigen::Matrix<double, 1, 3>::Zero();

      Jl(2) = 1.0;
      Jr(2) = 1.0;

      _v_w_mwl.evaluateJacobians(jacobians, Jl);
      _v_w_mwr.evaluateJacobians(jacobians,Jr);
    }

  }
}