#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

}


/*
// Copyright 2018 University of Liege
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Authors:
// - Adrien Crovato
*/

//// Interpolation from center to vertices
// Interpolate linearly surface singularities from adjacent panel centers to panel vertices. If panel is on an edge of a
// network, linear extrapolation is used instead
//
// Inputs:
// - idG: current panel global index
// - idC: current panel chordwise index
// - idS: current panel spanwise index
// - bPan: body panels (structure)
//
// Output:
// - vis: Matrix of interpolate singularities (row = vertex number; col 0 = doublet, col 1 = source)

#include <iostream>
#include <Eigen/Dense>
#include "interp_ctv.h"
#include <interp.h>

#define NDIM 3
#define NV 4

using namespace std;
using namespace Eigen;

MatrixXd interp_ctv(int idG, int idC, int idS, Network &bPan) {

    //// Initialization
    // Temporary
    MatrixXd v; // vertices coordinates
    v.resize(NV, NDIM);
    MatrixXi c; // centers (corner of interpolation)
    c.resize(NV, 4);
    MatrixXd vis; // interpolated singularities on vertices
    vis.resize(NV, 2);

    // Store vertices coordinates
    v.row(0) = bPan.v0.row(idG);
    v.row(1) = bPan.v1.row(idG);
    v.row(2) = bPan.v2.row(idG);
    v.row(3) = bPan.v3.row(idG);

    //// Compute panel center indices
    // If panel is on the last row/column, linear extrapolation is used
    if (idS == 0) {
        if (idC == 0) {
            c(0,0) = idG;
            c(0,1) = idG + 1;
            c(0,2) = idG + 1 + bPan.nC_;
            c(0,3) = idG + bPan.nC_;
            c(1,0) = idG;
            c(1,1) = idG + 1;
            c(1,2) = idG + 1 + bPan.nC_;
            c(1,3) = idG + bPan.nC_;
            c(2,0) = idG;
            c(2,1) = idG + 1;
            c(2,2) = idG + 1 + bPan.nC_;
            c(2,3) = idG + bPan.nC_;
            c(3,0) = idG;
            c(3,1) = idG + 1;
            c(3,2) = idG + 1 + bPan.nC_;
            c(3,3) = idG + bPan.nC_;
        }
        else if (idC == bPan.nC_ - 1) {
            c(0,0) = idG - 1;
            c(0,1) = idG;
            c(0,2) = idG + bPan.nC_;
            c(0,3) = idG - 1 + bPan.nC_;
            c(1,0) = idG - 1;
            c(1,1) = idG;
            c(1,2) = idG + bPan.nC_;
            c(1,3) = idG - 1 + bPan.nC_;
            c(2,0) = idG - 1;
            c(2,1) = idG;
            c(2,2) = idG + bPan.nC_;
            c(2,3) = idG - 1 + bPan.nC_;
            c(3,0) = idG - 1;
            c(3,1) = idG;
            c(3,2) = idG + bPan.nC_;
            c(3,3) = idG - 1 + bPan.nC_;
        }
        else {
            c(0,0) = idG - 1;
            c(0,1) = idG;
            c(0,2) = idG + bPan.nC_;
            c(0,3) = idG - 1 + bPan.nC_;
            c(1,0) = idG;
            c(1,1) = idG + 1;
            c(1,2) = idG + 1 + bPan.nC_;
            c(1,3) = idG + bPan.nC_;
            c(2,0) = idG;
            c(2,1) = idG + 1;
            c(2,2) = idG + 1 + bPan.nC_;
            c(2,3) = idG + bPan.nC_;
            c(3,0) = idG - 1;
            c(3,1) = idG;
            c(3,2) = idG + bPan.nC_;
            c(3,3) = idG - 1 + bPan.nC_;
        }
    }
    else if (idS == bPan.nS_ - 1) {
        if (idC == 0) {
            c(0,0) = idG - bPan.nC_;
            c(0,1) = idG - bPan.nC_ + 1;
            c(0,2) = idG + 1;
            c(0,3) = idG;
            c(1,0) = idG - bPan.nC_;
            c(1,1) = idG - bPan.nC_ + 1;
            c(1,2) = idG + 1;
            c(1,3) = idG;
            c(2,0) = idG - bPan.nC_;
            c(2,1) = idG - bPan.nC_ + 1;
            c(2,2) = idG + 1;
            c(2,3) = idG;
            c(3,0) = idG - bPan.nC_;
            c(3,1) = idG - bPan.nC_ + 1;
            c(3,2) = idG + 1;
            c(3,3) = idG;
        }
        else if (idC == bPan.nC_ - 1) {
            c(0,0) = idG - 1 - bPan.nC_;
            c(0,1) = idG - bPan.nC_;
            c(0,2) = idG;
            c(0,3) = idG - 1;
            c(1,0) = idG - 1 - bPan.nC_;
            c(1,1) = idG - bPan.nC_;
            c(1,2) = idG;
            c(1,3) = idG - 1;
            c(2,0) = idG - 1 - bPan.nC_;
            c(2,1) = idG - bPan.nC_;
            c(2,2) = idG;
            c(2,3) = idG - 1;
            c(3,0) = idG - 1 - bPan.nC_;
            c(3,1) = idG - bPan.nC_;
            c(3,2) = idG;
            c(3,3) = idG - 1;
        }
        else {
            c(0,0) = idG - 1 - bPan.nC_;
            c(0,1) = idG - bPan.nC_;
            c(0,2) = idG;
            c(0,3) = idG - 1;
            c(1,0) = idG - bPan.nC_;
            c(1,1) = idG - bPan.nC_ + 1;
            c(1,2) = idG + 1;
            c(1,3) = idG;
            c(2,0) = idG - bPan.nC_;
            c(2,1) = idG - bPan.nC_ + 1;
            c(2,2) = idG + 1;
            c(2,3) = idG;
            c(3,0) = idG - 1 - bPan.nC_;
            c(3,1) = idG - bPan.nC_;
            c(3,2) = idG;
            c(3,3) = idG - 1;
        }
    }
    else {
        c(0,0) = idG - 1 - bPan.nC_;
        c(0,1) = idG - bPan.nC_;
        c(0,2) = idG;
        c(0,3) = idG - 1;
        c(1,0) = idG - bPan.nC_;
        c(1,1) = idG - bPan.nC_ + 1;
        c(1,2) = idG + 1;
        c(1,3) = idG;
        c(2,0) = idG;
        c(2,1) = idG + 1;
        c(2,2) = idG + 1 + bPan.nC_;
        c(2,3) = idG + bPan.nC_;
        c(3,0) = idG - 1;
        c(3,1) = idG;
        c(3,2) = idG + bPan.nC_;
        c(3,3) = idG - 1 + bPan.nC_;
    }

    //// Interpolate each vertex
    for (int l = 0; l < NV; l++) {
        vis(l,0) = interp(bPan.CG(c(l,0),0), bPan.CG(c(l,0),1), bPan.CG(c(l,0),2),
                          bPan.CG(c(l,1),0), bPan.CG(c(l,1),1), bPan.CG(c(l,1),2),
                          bPan.CG(c(l,2),0), bPan.CG(c(l,2),1), bPan.CG(c(l,2),2),
                          bPan.CG(c(l,3),0), bPan.CG(c(l,3),1), bPan.CG(c(l,3),2),
                          bPan.mu(c(l,0)), bPan.mu(c(l,1)), bPan.mu(c(l,2)), bPan.mu(c(l,3)),
                          v(l,0), v(l,1), v(l,2));

        vis(l,1) = interp(bPan.CG(c(l,0),0), bPan.CG(c(l,0),1), bPan.CG(c(l,0),2),
                          bPan.CG(c(l,1),0), bPan.CG(c(l,1),1), bPan.CG(c(l,1),2),
                          bPan.CG(c(l,2),0), bPan.CG(c(l,2),1), bPan.CG(c(l,2),2),
                          bPan.CG(c(l,3),0), bPan.CG(c(l,3),1), bPan.CG(c(l,3),2),
                          bPan.tau(c(l,0)), bPan.tau(c(l,1)), bPan.tau(c(l,2)), bPan.tau(c(l,3)),
                          v(l,0), v(l,1), v(l,2));
    }
    return vis;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2018 John Maddock
//  Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef BOOST_MATH_HYPERGEOMETRIC_1F1_CF_HPP
#define BOOST_MATH_HYPERGEOMETRIC_1F1_CF_HPP

#include <boost/math/tools/fraction.hpp>

//
// Evaluation of 1F1 by continued fraction
// see http://functions.wolfram.com/HypergeometricFunctions/Hypergeometric1F1/10/0002/
//
// This is not terribly useful, as like the series we're adding a something to 1,
// so only really useful when we know that the result will be > 1.
//

  namespace boost { namespace math { namespace detail {

     template <class T>
     struct hypergeometric_1F1_cf_func
     {
        typedef std::pair<T, T>  result_type;
        hypergeometric_1F1_cf_func(T a_, T b_, T z_) : a(a_), b(b_), z(z_), k(0) {}
        std::pair<T, T> operator()()
        {
           ++k;
           return std::make_pair(-(((a + k) * z) / ((k + 1) * (b + k))), 1 + ((a + k) * z) / ((k + 1) * (b + k)));
        }
        T a, b, z;
        unsigned k;
     };

     template <class T, class Policy>
     T hypergeometric_1F1_cf(const T& a, const T& b, const T& z, const Policy& pol, const char* function)
     {
        hypergeometric_1F1_cf_func<T> func(a, b, z);
        boost::uintmax_t max_iter = boost::math::policies::get_max_series_iterations<Policy>();
        T result = boost::math::tools::continued_fraction_a(func, boost::math::policies::get_epsilon<T, Policy>(), max_iter);
        boost::math::policies::check_series_iterations<T>(function, max_iter, pol);
        return 1 + a * z / (b * (1 + result));
     }

  } } } // namespaces

#endif // BOOST_MATH_HYPERGEOMETRIC_1F1_BESSEL_HPP


#include <map>
#include <exception>
#include <string>
#include <cstdint>
#include <iostream>
#include <math.h>
#include <fc/exception/exception.hpp>
/*
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/rational.hpp>

#include <sg14/fixed_point>
#include "fixed.hpp"
*/

#include <fc/time.hpp>
#include <fc/log/logger.hpp>

//#include "bfp/lib/posit.h"

using namespace std;

typedef long double real_type;
typedef double token_type;

/*
struct margin_position {
   account_name owner;
   uint64_t     exchange_id;
   asset        lent;
   asset        collateral;
   uint64_t     open_time;

   uint64_t     primary_key()const{ return owner; }
   uint256_t    by_owner_ex_lent_collateral()const {

   }

   real_type    by_call_price()const {
      return collateral.amount / real_type(lent.amount);
   }
};
*/

template<typename Real>
Real Abs(Real Nbr)
{
 if( Nbr >= 0 )
  return Nbr;
 else
  return -Nbr;
}

template<typename Real>
Real sqrt_safe( const Real Nbr)
{
  return sqrt(Nbr);
// cout << " " << Nbr << "\n";;
 Real Number = Nbr / Real(2.0);
 const Real Tolerance = Real(double(1.0e-12));
 //cout << "tol: " << Tolerance << "\n";

 Real Sq;
 Real Er;
 do {
  auto tmp = Nbr / Number;
  tmp += Number;
  tmp /= real_type(2.0);
  if( Number == tmp ) break;
  Number = tmp;
  Sq = Number * Number;
  Er = Abs(Sq - Nbr);
// wdump((Er.getDouble())(1.0e-8)(Tolerance.getDouble()));
//  wdump(((Er - Tolerance).getDouble()));
 }while( Er >= Tolerance );

 return Number;
}

typedef __uint128_t uint128_t;
typedef string  account_name;
typedef string  symbol_type;

static const symbol_type exchange_symbol = "EXC";

struct asset {
   token_type amount;
   symbol_type symbol;
};

struct margin_key {
   symbol_type lent;
   symbol_type collat;
};

struct margin {
   asset       lent;
   symbol_type collateral_symbol;
   real_type   least_collateralized_rate;
};

struct user_margin {
   asset lent;
   asset collateral;

   real_type call_price()const {
      return collateral.amount / real_type(lent.amount);
   }
};

struct exchange_state;
struct connector {
   asset      balance;
   real_type  weight = 0.5;
   token_type total_lent; /// lent from maker to users
   token_type total_borrowed; /// borrowed from users to maker
   token_type total_available_to_lend; /// amount available to borrow
   token_type interest_pool; /// total interest earned but not claimed,
                             /// each user can claim user_lent

   void  borrow( exchange_state& ex, const asset& amount_to_borrow );
   asset convert_to_exchange( exchange_state& ex, const asset& input );
   asset convert_from_exchange( exchange_state& ex, const asset& input );
};

struct balance_key {
   account_name owner;
   symbol_type  symbol;

   friend bool operator < ( const balance_key& a, const balance_key& b ) {
      return std::tie( a.owner, a.symbol ) < std::tie( b.owner, b.symbol );
   }
   friend bool operator == ( const balance_key& a, const balance_key& b ) {
      return std::tie( a.owner, a.symbol ) == std::tie( b.owner, b.symbol );
   }
};

real_type fee = 1;//.9995;

int64_t maxtrade = 20000ll;

struct exchange_state {
   token_type  supply;
   symbol_type symbol = exchange_symbol;

   connector  base;
   connector  quote;

   void transfer( account_name user, asset q ) {
      output[balance_key{user,q.symbol}] += q.amount;
   }
   map<balance_key, token_type> output;
   vector<margin>               margins;
};

/*
void  connector::borrow( exchange_state& ex, account_name user,
                         asset amount_to_borrow,
                         asset collateral,
                         user_margin& marg ) {
   FC_ASSERT( amount_to_borrow.amount < balance.amount, "attempt to borrow too much" );
   lent.amount += amount_to_borrow.amount;
   balance.amount -= amount_to_borrow.amount;
   ex.transfer( user, amount_to_borrow );

   marg.collateral.amount += collateral.amount;
   marg.lent.amount += amount_to_borrow.amount;
   auto p = marg.price();

   if( collateral.symbol == ex.symbol ) {
      if( p > ex_margin.least_collateralized_rate )
         ex_margin.least_collateralized_rate = p;
   }
   else if( collateral.symbol == peer_margin.collateral.symbol ) {
      if( p > peer_margin.least_collateralized_rate )
         peer_margin.least_collateralized_rate = p;
   }
}
*/

asset connector::convert_to_exchange( exchange_state& ex, const asset& input ) {

   real_type R(ex.supply);
   real_type S(balance.amount+input.amount);
   real_type F(weight);
   real_type T(input.amount);
   real_type ONE(1.0);

   auto E = R * (ONE - std::pow( ONE + T / S, F) );

   //auto real_issued = real_type(ex.supply) * (sqrt_safe( 1.0 + (real_type(input.amount) / (balance.amount+input.amount))) - 1.0);
   //auto real_issued = real_type(ex.supply) * (std::pow( 1.0 + (real_type(input.amount) / (balance.amount+input.amount)), weight) - real_type(1.0));
   //auto real_issued = R * (std::pow( ONE + (T / S), F) - ONE);

   //wdump((double(E))(double(real_issued)));
   token_type issued = -E; //real_issued;

   ex.supply      += issued;
   balance.amount += input.amount;

   return asset{ issued, exchange_symbol };
}

asset connector::convert_from_exchange( exchange_state& ex, const asset& input ) {

   real_type R(ex.supply - input.amount);
   real_type S(balance.amount);
   real_type F(weight);
   real_type E(input.amount);
   real_type ONE(1.0);

   real_type T = S * (std::pow( ONE + E/R, ONE/F) - ONE);

   /*
   real_type base = real_type(1.0) + ( real_type(input.amount) / real_type(ex.supply-input.amount));
   auto out = (balance.amount * ( std::pow(base,1.0/weight) - real_type(1.0) ));
   */
   auto out = T;

//   edump((double(out-T))(double(out))(double(T)));

   ex.supply -= input.amount;
   balance.amount -= token_type(out);
   return asset{ token_type(out), balance.symbol };
}

void besio_assert( bool test, const string& msg ) {
   if( !test ) throw std::runtime_error( msg );
}

void print_state( const exchange_state& e );

/**
 *  Given the current state, calculate the new state
 */
exchange_state convert( const exchange_state& current,
                        account_name user,
                        asset        input,
                        asset        min_output,
                        asset*       out = nullptr) {

  besio_assert( min_output.symbol != input.symbol, "cannot convert" );

  exchange_state result(current);

  asset initial_output = input;

  if( input.symbol != exchange_symbol ) {
     if( input.symbol == result.base.balance.symbol ) {
        initial_output = result.base.convert_to_exchange( result, input );
     }
     else if( input.symbol == result.quote.balance.symbol ) {
        initial_output = result.quote.convert_to_exchange( result, input );
     }
     else besio_assert( false, "invalid symbol" );
  } else {
     if( min_output.symbol == result.base.balance.symbol ) {
        initial_output = result.base.convert_from_exchange( result, initial_output );
     }
     else if( min_output.symbol == result.quote.balance.symbol ) {
        initial_output= result.quote.convert_from_exchange( result, initial_output );
     }
     else besio_assert( false, "invalid symbol" );
  }

  asset final_output = initial_output;

//  std::cerr << "\n\nconvert " << input.amount << " "<< input.symbol << "  =>  " << final_output.amount << " " << final_output.symbol << "  final: " << min_output.symbol << " \n";

  result.output[ balance_key{user,final_output.symbol} ] += final_output.amount;
  result.output[ balance_key{user,input.symbol} ] -= input.amount;

  if( min_output.symbol != final_output.symbol ) {
    return convert( result, user, final_output, min_output, out );
  }

  if( out ) *out = final_output;
  return result;
}

/*  VALIDATE MARGIN ALGORITHM
 *
 *  Given an initial condition, verify that all margin positions can be filled.
 *
 *  Assume 3 assets, B, Q, and X and the notation LENT-COLLAT we get the following
 *  pairs:
 *
 *  B-X
 *  B-A
 *  A-X
 *  A-B
 *  X-A
 *  X-B
 *
 *  We assume that pairs of the same lent-type have to be simultainously filled,
 *  as filling one could make it impossible to fill the other.
 *
 *
void validate_margin( exchange_state& e ) {
   for( const auto& pos : e.margins ) {
      token_type min_collat = pos.lent.amount * pos.least_collateralized_rate;
      asset received;
      e = convert( e, "user", asset{ min_collat, pos.first.collat }, pos.lent, &received );
      FC_ASSERT( received > pos.lent.amount, "insufficient collateral" );

      received.amount -= pos.lent.amount;
      e = convert( e, "user", received, asset{ token_type(0), pos.collateral_symbol} );
   }
}
*/

/**
 *  A user has Collateral C and wishes to borrow B, so we give user B
 *  provided that C is enough to buy B back after removing it from market and
 *  that no margin calls would be triggered.
 */
exchange_state borrow( const exchange_state& current, account_name user,
                       asset amount_to_borrow,
                       asset collateral_provided ) {
    FC_ASSERT( amount_to_borrow.symbol != collateral_provided.symbol );

    /// lookup the margin position for user
    ///   update user's margin position
    ///   update least collateralized margin position on state
    ///   remove amount_to_borrow from exchange
    ///   lock collateral for user
    /// simulate complete margin calls
    return exchange_state();
}

exchange_state cover( const exchange_state& current, account_name user,
                      asset amount_to_cover, asset collateral_to_cover_with )
{
   /// lookup existing position for user/debt/collat
   /// verify collat > collateral_to_cover_with
   /// sell collateral_to_cover_with for debt on market
   /// reduce debt by proceeds
   /// add proceeds to connector
   //     - if borrowed from user, reduce borrowed from user
   /// calculate new call price and update least collateralized position
   /// simulate complete margin calls
   return exchange_state();
}

exchange_state lend( const exchange_state& current, account_name lender,
                     asset asset_to_lend ) {
   /// add to pool of funds available for lending and buy SHARES in
   ///  interest pool at current rate.
   return exchange_state();
}

exchange_state unlend( const exchange_state& current, account_name lender,
                     asset asset_to_lend ) {
    /// sell shares in interest pool at current rate
    /// this is permitable so long as total borrowed from users remains less than
    /// total available to lend. Otherwise, margin is called on the least
    /// collateralized position.
   return exchange_state();
}

void print_state( const exchange_state& e ) {
   std::cerr << "\n-----------------------------\n";
   std::cerr << "supply: " <<  e.supply  << "\n";
   std::cerr << "base: " <<  e.base.balance.amount << " " << e.base.balance.symbol << "\n";
   std::cerr << "quote: " <<  e.quote.balance.amount << " " << e.quote.balance.symbol << "\n";

   for( const auto& item : e.output ) {
      cerr << item.first.owner << "  " << item.second << " " << item.first.symbol << "\n";
   }
   std::cerr << "\n-----------------------------\n";
}

int main( int argc, char** argv ) {
 //  std::cerr << "root: " << double(root.numerator())/root.denominator() << "\n";

   exchange_state state;
   state.supply = 100000000000ll;
   //state.base.weight  = state.total_weight / 2.;
   state.base.balance.amount = 100000000;
   state.base.balance.symbol = "USD";
   state.base.weight = .49;
   //state.quote.weight = state.total_weight / 2.;
   state.quote.balance.amount = state.base.balance.amount;
   state.quote.balance.symbol = "BTC";
   state.quote.weight = .51;

   print_state( state );

   //state = convert( state, "dan", asset{ 100, "USD"}, asset{ 0, "BTC" } );

   auto start = fc::time_point::now();
   for( uint32_t i = 0; i < 10000; ++i ) {
     if( rand() % 2 == 0 )
        state = convert( state, "dan", asset{ token_type(uint32_t(rand())%maxtrade), "USD"}, asset{ 0, "BTC" } );
     else
        state = convert( state, "dan", asset{ token_type(uint32_t(rand())%maxtrade), "BTC"}, asset{ 0, "USD" } );
   }
   for( const auto& item : state.output ) {
      if( item.second > 0 ) {
         if( item.first.symbol == "USD" )
           state = convert( state, "dan", asset{ item.second, item.first.symbol}, asset{ 0, "BTC" } );
         else
           state = convert( state, "dan", asset{ item.second, item.first.symbol}, asset{ 0, "USD" } );
        break;
      }
   }
   print_state( state );

   auto end = fc::time_point::now();
   wdump((end-start));
   /*
   auto new_state = convert( state, "dan", asset{ 100, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 100, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 100, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 100, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 100, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 100, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 100, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 100, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 100, "USD"}, asset{ 0, "BTC" } );

        new_state = convert( new_state, "dan", asset{ 100, "BTC"}, asset{ 0, "USD" } );
        new_state = convert( new_state, "dan", asset{ 100, "BTC"}, asset{ 0, "USD" } );
        new_state = convert( new_state, "dan", asset{ 100, "BTC"}, asset{ 0, "USD" } );
        new_state = convert( new_state, "dan", asset{ 100, "BTC"}, asset{ 0, "USD" } );
        new_state = convert( new_state, "dan", asset{ 100, "BTC"}, asset{ 0, "USD" } );
        new_state = convert( new_state, "dan", asset{ 100, "BTC"}, asset{ 0, "USD" } );
        new_state = convert( new_state, "dan", asset{ 100, "BTC"}, asset{ 0, "USD" } );
        new_state = convert( new_state, "dan", asset{ 92.5-0.08-.53, "BTC"}, asset{ 0, "USD" } );
        new_state = convert( new_state, "dan", asset{ 100, "BTC"}, asset{ 0, "USD" } );
        */

        //new_state = convert( new_state, "dan", asset{ 442+487-733+280+349+4.493+62.9, "BTC"}, asset{ 0, "USD" } );
   /*
   auto new_state = convert( state, "dan", asset{ 500, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 500, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 442+487, "BTC"}, asset{ 0, "USD" } );
        */
        /*
        new_state = convert( new_state, "dan", asset{ 487, "BTC"}, asset{ 0, "USD" } );
        new_state = convert( new_state, "dan", asset{ 442, "BTC"}, asset{ 0, "USD" } );
        */
        //new_state = convert( new_state, "dan", asset{ 526, "BTC"}, asset{ 0, "USD" } );
        //new_state = convert( new_state, "dan", asset{ 558, "BTC"}, asset{ 0, "USD" } );
        //new_state = convert( new_state, "dan", asset{ 1746, "BTC"}, asset{ 0, "USD" } );
        /*
        new_state = convert( new_state, "dan", asset{ 526, "BTC"}, asset{ 0, "USD" } );
        new_state = convert( new_state, "dan", asset{ 500, "USD"}, asset{ 0, "EXC" } );
        new_state = convert( new_state, "dan", asset{ 500, "BTC"}, asset{ 0, "EXC" } );
        new_state = convert( new_state, "dan", asset{ 10, "EXC"}, asset{ 0, "USD" } );
        new_state = convert( new_state, "dan", asset{ 10, "EXC"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 500, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 500, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 500, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 500, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 2613, "BTC"}, asset{ 0, "USD" } );
        */

   /*
   auto new_state = convert( state, "dan", asset{ 10, "EXC"}, asset{ 0, "USD" } );

   print_state( new_state );

   new_state = convert( state, "dan", asset{ 10, "EXC"}, asset{ 0, "BTC" } );
   print_state( new_state );
   new_state = convert( new_state, "dan", asset{ 10, "EXC"}, asset{ 0, "USD" } );
   print_state( new_state );

   //new_state = convert( new_state, "dan", asset{ 52, "USD"}, asset{ 0, "EXC" } );
   */

   return 0;
}

#if 0

0. if( margin_fault )
     Convert Least Collateral
     if( margin fault ))
        defer

if( margin_fault ) assert( false, "busy calling" );

1. Fill Incoming Order
2. Check Counter Order
3. if( margin fault )
   Defer Trx to finish margin call

#endif


#include <stdio.h>
#include <rrt_planners/RandomNumbers.h>
#include <boost/random/lagged_fibonacci.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread/once.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/math/constants/constants.hpp>

/// @cond IGNORE
namespace
{
/// We use a different random number generator for the seeds of the
/// other random generators. The root seed is from the number of
/// nano-seconds in the current time, or given by the user.
class RNGSeedGenerator
{
public:
  RNGSeedGenerator()
    : someSeedsGenerated_(false)
    , firstSeed_((boost::uint32_t)(boost::posix_time::microsec_clock::universal_time() -
                                   boost::posix_time::ptime(boost::date_time::min_date_time))
                     .total_microseconds())
    , sGen_(firstSeed_)
    , sDist_(1, 1000000000)
    , s_(sGen_, sDist_)
  {
  }

  boost::uint32_t firstSeed()
  {
    boost::mutex::scoped_lock slock(rngMutex_);
    return firstSeed_;
  }

  void setSeed(boost::uint32_t seed)
  {
    boost::mutex::scoped_lock slock(rngMutex_);
    if (seed > 0)
    {
      if (someSeedsGenerated_)
      {
        printf("ERROR. Random number generation already started. Changing seed now will not lead to deterministic sampling.");
      }
      else
      {
        // In this case, since no seeds have been generated yet, so we remember this seed
        // as the first one.
        firstSeed_ = seed;
      }
    }
    else
    {
      if (someSeedsGenerated_)
      {
        printf("WARN. Random generator seed cannot be 0. Ignoring seed.");
        return;
      }
      else
      {
        printf("WARN. Random generator seed cannot be 0. Using 1 instead.");
        seed = 1;
      }
    }
    sGen_.seed(seed);
  }

  boost::uint32_t nextSeed()
  {
    boost::mutex::scoped_lock slock(rngMutex_);
    someSeedsGenerated_ = true;
    return s_();
  }

private:
  bool someSeedsGenerated_;
  boost::uint32_t firstSeed_;
  boost::mutex rngMutex_;
  boost::lagged_fibonacci607 sGen_;
  boost::uniform_int<> sDist_;
  boost::variate_generator<boost::lagged_fibonacci607&, boost::uniform_int<> > s_;
};

static boost::once_flag g_once = BOOST_ONCE_INIT;
static boost::scoped_ptr<RNGSeedGenerator> g_RNGSeedGenerator;

void initRNGSeedGenerator()
{
  g_RNGSeedGenerator.reset(new RNGSeedGenerator());
}

RNGSeedGenerator& getRNGSeedGenerator()
{
  boost::call_once(&initRNGSeedGenerator, g_once);
  return *g_RNGSeedGenerator;
}
}  // namespace
/// @endcond

boost::uint32_t RNG::getSeed()
{
  return getRNGSeedGenerator().firstSeed();
}

void RNG::setSeed(boost::uint32_t seed)
{
  getRNGSeedGenerator().setSeed(seed);
}

RNG::RNG()
  : generator_(getRNGSeedGenerator().nextSeed())
  , uniDist_(0, 1)
  , normalDist_(0, 1)
  , uni_(generator_, uniDist_)
  , normal_(generator_, normalDist_)
{
}

double RNG::halfNormalReal(double r_min, double r_max, double focus)
{
  assert(r_min <= r_max);

  const double mean = r_max - r_min;
  double v = gaussian(mean, mean / focus);

  if (v > mean)
    v = 2.0 * mean - v;
  double r = v >= 0.0 ? v + r_min : r_min;
  return r > r_max ? r_max : r;
}

int RNG::halfNormalInt(int r_min, int r_max, double focus)
{
  int r = (int)floor(halfNormalReal((double)r_min, (double)(r_max) + 1.0, focus));
  return (r > r_max) ? r_max : r;
}

// From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III,
//       pg. 124-132
void RNG::quaternion(double value[4])
{
  double x0 = uni_();
  double r1 = sqrt(1.0 - x0), r2 = sqrt(x0);
  double t1 = 2.0 * boost::math::constants::pi<double>() * uni_(),
         t2 = 2.0 * boost::math::constants::pi<double>() * uni_();
  double c1 = cos(t1), s1 = sin(t1);
  double c2 = cos(t2), s2 = sin(t2);
  value[0] = s1 * r1;
  value[1] = c1 * r1;
  value[2] = s2 * r2;
  value[3] = c2 * r2;
}

// From Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning, by James
// Kuffner, ICRA 2004
void RNG::eulerRPY(double value[3])
{
  value[0] = boost::math::constants::pi<double>() * (-2.0 * uni_() + 1.0);
  value[1] = acos(1.0 - 2.0 * uni_()) - boost::math::constants::pi<double>() / 2.0;
  value[2] = boost::math::constants::pi<double>() * (-2.0 * uni_() + 1.0);
}