//
//  Net.cpp
//
//
//  Created by Guanglei Wang on 03/06/2017.
//

#include <gravity/Net.h>
#include <algorithm>
#include <map>
#include <set>
#define _USE_MATH_DEFINES
#include <cmath>
#include <list>
#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <string>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <queue>
#include <time.h>
//#include <armadillo>
#ifdef USE_BOOST
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <deque>
#include <iterator>
#endif

using namespace std;
using namespace gravity;

static int max_line_len;
static char* line = nullptr;

Net::Net() {}

/* returns true if an arc is already present between the given nodes */
bool Net::duplicate(std::string n1, std::string n2, int id1) {
    int id2 = get_arc(n1, n2)->_id;

    if (id2 < id1)
        return true;
    else
        return false;
}

Net* Net::clone() {
    Net* copy_net = new Net();
    Node* node = NULL;

    for (int i=0; i<nodes.size(); i++) {
        node = this->nodes[i];
        copy_net->add_node(node->clone());
    }

    Arc* arc = NULL;
    for (int i=0; i < arcs.size(); i++) {

        /* ignores if the arc is a paralel line to an already existing arc */
        //if (duplicate(arcs[i]->_src->_name, arcs[i]->_dest->_name, arcs[i]->_id)) {
//            continue;
//        }
        arc = arcs[i]->clone();

        /* Update the source and destination to the new nodes in copy_net */
        arc->_src = copy_net->get_node(arc->_src->_name);
        arc->_dest = copy_net->get_node(arc->_dest->_name);

        /* Add the new arc to the list of arcs */
        copy_net->add_arc(arc);

        /* Connects it to its source and destination */
        arc->connect();
    }
    return copy_net;
}

Net* Net::clone_undirected() {
    Net* copy_net = new Net();
    Node* node = NULL;

    for (int i=0; i<nodes.size(); i++) {
        node = this->nodes[i];
        copy_net->add_node(node->clone());
    }

    Arc* arc = NULL;
    for (int i=0; i < arcs.size(); i++) {
        if (copy_net->get_arc(arcs[i]->_src->_name, arcs[i]->_dest->_name)!=nullptr) {
            continue;
        }
        arc = arcs[i]->clone();

        /* Update the source and destination to the new nodes in copy_net */
        arc->_src = copy_net->get_node(arc->_src->_name);
        arc->_dest = copy_net->get_node(arc->_dest->_name);

        /* Add the undirected arc to the list of arcs */
        copy_net->add_undirected_arc(arc);

        /* Connects it to its source and destination */
        arc->connect();
    }
    return copy_net;
}

const bool bag_compare(const vector<Node*> & a,const vector<Node*>& b) {
    return a.size() > b.size();
}

const bool node_compare(const Node* n1, const Node* n2) {
    return n1->fill_in > n2->fill_in;
}

void Net::add_node(Node* node) {
    node->_id = (int) nodes.size();

    if (!nodeID.insert(pair<string,Node*>(node->_name, node)).second) {
        cerr << "ERROR: adding the same node twice!";
    }

    nodes.push_back(node);
}

Node* Net::get_node(string name) const{
    return nodeID.find(name)->second;
}

/* returns the undirected arc formed by node n1 and n2 */
Arc* Net::get_arc(Node* n1, Node* n2) {
    string src, dest, key, inv_key;
    src = n1->_name;
    dest = n2->_name;
    key.clear();
    inv_key.clear();
    key.append(src);
    inv_key.append(dest);
    key.append(",");
    inv_key.append(",");
    key.append(dest);
    inv_key.append(src);
    map<string, set<Arc*>*>::iterator it= arcID.find(key);
    if (it != arcID.end()) {
        for (auto a: *it->second) {
            //   if (!a->parallel) {
            return a;
            // }
        }
    }
    it = arcID.find(inv_key);
    if (it != arcID.end()) {
        for (auto a: *it->second) {
            //   if (!a->parallel) {
            return a;
            // }
        }
    }
    return nullptr;
}

/* returns the Id of the arc formed by nodes names n1 and n2 */
// this assumes that the graph is undirected.
Arc* Net::get_arc(std::string src, std::string dest) {
    std::string key, inv_key;
    key.clear();
    inv_key.clear();
    key.append(src);
    inv_key.append(dest);
    key.append(",");
    inv_key.append(",");
    key.append(dest);
    inv_key.append(src);
    map<string, set<Arc*>*>::iterator it= arcID.find(key);
    if (it != arcID.end()) {
        for (auto a: *it->second) {
            return a;
        }
    }

    it = arcID.find(inv_key);
    if (it != arcID.end()) {
        for (auto a: *it->second) {
            return a;
        }
    }

    return nullptr;
}

Arc* Net::get_directed_arc(std::string src, std::string dest) {
    std::string key;
    key.clear();
    key.append(src);
    key.append(",");
    key.append(dest);
    map<string, set<Arc*>*>::iterator it= arcID.find(key);
    if (it != arcID.end()) {
        for (auto a: *it->second) {
            return a;
        }
    }

    return nullptr;
}

bool Net::add_arc(Arc* a) {
    bool parallel = false;
    set<Arc*>* s = NULL;
    string src, dest, key;
    src = a->_src->_name;
    dest = a->_dest->_name;
    if (src == dest){
        throw invalid_argument ("It is now allowed to make a node self connected in gravity. \n");

    }

    key.clear();
    key.append(src);
    key.append(",");
    key.append(dest);

    if(arcID.find(key)==arcID.end()) {
        s = new set<Arc*>;
        s->insert(a);
        arcID.insert(pair<string, set<Arc*>*>(key,s));
    }
    else {
        if(arcID.find(key)!=arcID.end())
            s = arcID[key];
        s->insert(a);
        Warning("\nWARNING: adding another Directed line between same nodes! \n Node ID: " << src << " and Node ID: " << dest << endl);
        a->_parallel = true;
        parallel = true;
    }
    arcMap[a->_name] = a;
    arcs.push_back(a);
    return parallel;
}
// undirected
void Net::add_undirected_arc(Arc* a) {
//    bool parallel = false;
    set<Arc*>* s = NULL;
    string src, dest, key, key_inv;
    src = a->_src->_name;
    dest = a->_dest->_name;

    if (src == dest){
        throw invalid_argument ("It is now allowed to make a node self connected in gravity. \n");

    }

    key.clear();
    key.append(src);
    key.append(",");
    key.append(dest);

    key_inv.clear();
    key_inv.append(dest);
    key_inv.append(",");
    key_inv.append(src);

    if(arcID.find(key)==arcID.end()&& arcID.find(key_inv)==arcID.end()) {
        s = new set<Arc*>;
        s->insert(a);
        arcID.insert(pair<string, set<Arc*>*>(key,s));
        arcs.push_back(a);
    }
}

/** remove the arc by
1. removing it from the arcs list, but without changing the arc id.
2. removing it from the map container.
*/

void Net::remove_arc(Arc* a) {
    arcs.erase(arcs.begin()+(a->_id));
    //arcs[a->_id] = nullptr;
    arcID.erase(a->_src->_name+","+a->_dest->_name);
}

// Reading files
char* Net::readline(FILE *input)
{
    size_t len;
    // line, max_line_len have been declared
    if(std::fgets(line,max_line_len,input)==NULL) return NULL;

    while(strrchr(line,'\n') == NULL)
    {
        max_line_len *= 2;
        line = (char *)realloc(line,max_line_len);
        len = strlen(line);
        if(fgets(line+len,max_line_len-len,input) == NULL)
            break;
    }
    return line;
}

void Net::exit_input_error(int line_num) {
    fprintf(stderr,"Wrong input format at line %d\n", line_num);
    exit(1);
}

// Reading graphs with rudy format
void Net::readrudy(const char* fname) {
    int Num_nodes=0;
    int Num_edges=0;

    ifstream infile(fname);

    string sLine;

    if (infile.good())
    {
        getline(infile, sLine);
        istringstream iss(sLine);
        iss >> Num_nodes;
        iss >> Num_edges;
    }
    else{
        fprintf(stderr,"can’t open input file %s\n",fname);
        exit(1);
    }
    // get nodes

    string name;
    Node* node = nullptr;

    for (int i= 1; i< Num_nodes + 1; i++) {
        name = to_string(i);
        node = new Node(name,i-1);
        add_node(node);
    }

    // get arcs
    Arc* arc = NULL;

    // note that src, dest are names of nodes.
    string src, dest;
    double weight;
    while(getline(infile,sLine,'\n'))
    {
        istringstream iss(sLine);
        iss >> src >> dest >> weight;

        name = (int)arcs.size()+1;
        arc = new Arc(name);

        arc->_id = (int)arcs.size();

        arc->_src = get_node(src);
        arc->_dest= get_node(dest);
        arc->_weight=weight;
        add_arc(arc);
        arc->connect();
    }
    infile.close();
}

/** construct a graph by reading an adjacency matrix */
void Net::read_adjacency_matrix(const string& fname) {
    FILE *fp = fopen(fname.c_str(),"r");
    if(fp == NULL)
    {
            cout << "Can’t open input file " << fname;
            exit(1);
    }
    max_line_len = 1024;
    line = new char[max_line_len];

    vector<vector<int>> matrix;
    int temp;
    while(readline(fp)!=NULL)
    {
        vector<int> row;
        stringstream linestream(line);
        while (linestream>>temp)
            row.push_back(temp);
        matrix.push_back(row);
    }

    int n=0;
    n =matrix.size();

    string name;
    int id = 0;

    Node* node = NULL;
    for (int i= 0; i<n; i++) {
        name = to_string(i);
        node = new Node(name,i);
        add_node(node);
    }

    Arc* arc = NULL;
    string src, dest;
    unsigned index = 0;
    for (int i = 0; i <(n); i++)
        for (int j=i+1; j<n; j++) {
            if (matrix[i][j] > 0)
            {
                src = to_string(i);
                dest = to_string(j);
                id = index;
                arc = new Arc(src + "," + dest);
                arc->_id = id;
                arc->_src = get_node(src);
                arc->_dest= get_node(dest);
                add_arc(arc);
                arc->connect();
            }
            index++;
        }
    delete[] line;
    fclose(fp);
}

void Net::get_complement(const char* fname) {
    FILE *fp = fopen(fname,"r");
    if(fp == NULL)
    {
        fprintf(stderr,"can’t open input file %s\n",fname);
        exit(1);
    }

    max_line_len = 1024;
    line = new char[max_line_len];

    vector<vector<int>> matrix;
    int temp;
    while(readline(fp)!=NULL)
    {
        vector<int> row;
        stringstream linestream(line);
        while (linestream>>temp)
            row.push_back(temp);
        matrix.push_back(row);
    }
    int n=0;
    n =matrix.size();

    string name;
    int id = 0;

    Node* node = NULL;
    for (int i= 0; i<n; i++) {
        name = to_string(i);
        node = new Node(name,i);
        add_node(node);
    }

    Arc* arc = NULL;
    string src, dest;

    for (int i = 0; i <(n-1); i++)
        for (int j=i+1; j<n; j++) {
            if (matrix[i][j] == 0)
            {
                src = to_string(i);
                dest = to_string(j);
                id = (int)arcs.size();
                arc = new Arc(to_string(id));
                arc->_id = id;
                arc->_src = get_node(src);
                arc->_dest= get_node(dest);
                add_arc(arc);
                arc->connect();
            }
        }
    delete[] line;
    fclose(fp);
}

/*  @brief Remove node and all incident arcs from the network
 @note Does not remove the incident arcs from the list of arcs in the network!
 @return the id of the node removed
 */
string Net::remove_end_node() {
    Node* n = nodes.back();
    Node * nn = nullptr;
    string n_id = n->_name;
    for (auto a: n->branches) {
        nn = a->neighbour(n);
        nn->removeArc(a);
        for (auto aa: nn->branches) {
            if (!aa->neighbour(nn)->is_connected(n)) {
                nn->fill_in--;
                assert(nn->fill_in >=0);
            }
        }
    }
    nodes.pop_back();
    return n_id;
}

// use greedy fill-in algorithm.
void Net::get_tree_decomp_bags() {
    Node* n = nullptr;
    Node* u = nullptr;
    Node* nn = nullptr;
    Arc* arc = nullptr;
    set<vector<Node*>> unique_bags;
    string name="";
    Net* graph_clone = clone_undirected(); //
    int nb = 0;
    unsigned max_size = 0;

    /** cliques with less than 1 nodes are useless for us.*/
    while (graph_clone->nodes.size()> 2) {
        sort(graph_clone->nodes.begin(), graph_clone->nodes.end(),node_compare);

        // last element has the minimum fill-in.
        n = graph_clone->nodes.back();
        if(!n->_active) {
            graph_clone->remove_end_node();
            continue;
        }
        Debug(n->_name << endl);
        Debug(graph_clone->nodes.size() << endl);
        vector<Node*> bag_copy;
        vector<Node*> bag;
        DebugOff("new bag = { ");
        for (auto nn: n->get_neighbours()) {
            if(!nn->_active) continue;
            bag_copy.push_back(nn);
            bag.push_back(get_node(nn->_name)); // Note it takes original node.
            DebugOff(nn->_name << ", ");
        }
        DebugOff(n->_name << "}\n");
        graph_clone->remove_end_node();
        bag_copy.push_back(n);
        bag.push_back(get_node(n->_name)); // node in this graph
        sort(bag_copy.begin(), bag_copy.end(), [](const Node* a, const Node* b) -> bool{return a->_id < b->_id;});
        sort(bag.begin(), bag.end(), [](const Node* a, const Node* b) -> bool{return a->_id < b->_id;});

        // update clone_graph and construct chordal extension.
        for (int i = 0; i < bag_copy.size(); i++) {
            u = bag_copy.at(i);
            for (int j = i+1; j<bag_copy.size(); j++) {
                nn = bag_copy.at(j);
                if (u->is_connected(nn)) {
                    if(get_arc(u,nn) && !get_arc(u,nn)->_active) {
                        Arc* off_arc = get_arc(u,nn);
                        off_arc->_imaginary = true;
                        off_arc->_free = true;
                    }
                    continue;
                }
                name = to_string((int) graph_clone->arcs.size()+1);
                arc = new Arc(name);

                arc->_id = arcs.size();
                arc->_src = u;
                arc->_dest = nn;
                arc->_imaginary = true;
                arc->_free = true;
                arc->connect();
                graph_clone->add_undirected_arc(arc);
            }
        }
        if(unique_bags.insert(bag).second){
            _bags.push_back(bag); // bag original
            if (bag_copy.size()==3) {
                nb++;
            }
        }

        if (bag_copy.size()>max_size) {
            max_size = bag_copy.size();
        }
        delete n;
    }

    DebugOn("\n Number of 3D bags = " << nb << endl);
    DebugOn("\n Max clique size = " << max_size << endl);
    if(max_size==2){
        this->_tree = true;
    }

    delete graph_clone;

}

std::vector<std::vector<Node*>> Net::decompose_bags_3d(bool print_bags){
    set<vector<Node*>> unique_bags;
    vector<std::vector<Node*>> res;
    for (auto &bag_copy:_bags) {
        if(bag_copy.size()==3){
            if(unique_bags.insert(bag_copy).second){
                res.push_back(bag_copy);
            }
        }
        else if(bag_copy.size()>3){
            DebugOff("Decomposing bigger bag into 3d bags\n");

            for (auto i = 0; i<bag_copy.size()-2; i++) {
                for (auto j = i+1; j<bag_copy.size()-1; j++) {
                    for (auto k = j+1; k<bag_copy.size(); k++) {
                        vector<Node*> new_bag;
                        new_bag.push_back(bag_copy[i]);
                        new_bag.push_back(bag_copy[j]);
                        new_bag.push_back(bag_copy[k]);
                        DebugOff("new bag = {");
                        //                        for (int i=0; i<new_bag.size();     i++) {
                        //                            cout << new_bag.at(i)->_name << " ";
                        //                        }
                        DebugOff("}" << endl);
                        if(unique_bags.insert(new_bag).second){
                            res.push_back(new_bag);
                        }
                    }
                }
            }
        }
    }
    return res;
}

std::vector<std::vector<Node*>> Net::decompose_bags_4d(bool print_bags)
{
    set<vector<Node*>> unique_bags;
    vector<std::vector<Node*>> res;
    for (auto &bag_copy:_bags) {
        if(bag_copy.size()==4){
            if(unique_bags.insert(bag_copy).second){
                res.push_back(bag_copy);
            }
        }
        else if(bag_copy.size()>4){
            DebugOff("Decomposing bigger bag into 4d bags\n");

            for (auto i = 0; i<bag_copy.size()-3; i++) {
                for (auto j = i+1; j<bag_copy.size()-2; j++) {
                    for (auto k = j+1; k<bag_copy.size()-1; k++) {
                        for (auto l = k+1; l<bag_copy.size(); l++) {
                            vector<Node*> new_bag;
                            new_bag.push_back(bag_copy[i]);
                            new_bag.push_back(bag_copy[j]);
                            new_bag.push_back(bag_copy[k]);
                            new_bag.push_back(bag_copy[l]);
                            DebugOff("new bag = {");
                            //                        for (int i=0; i<new_bag.size();     i++) {
                            //                            cout << new_bag.at(i)->_name << " ";
                            //                        }
                            DebugOff("}" << endl);
                            if(unique_bags.insert(new_bag).second){
                                res.push_back(new_bag);
                            }
                        }
                    }
                }
            }
        }
    }
    return res;
}

/** Return the vector of arcs ignoring parallel lines **/
indices Net::get_bus_pairs(){
    if(!bus_pairs.empty()){
        return bus_pairs;
    }
    for (auto a: arcs) {
        if (!a->_parallel) {
            bus_pairs.add(a->_src->_name+","+a->_dest->_name);
        }
    }
    return bus_pairs;
}

void Net::Fast_Horton(){

    Net* copy_net = clone();
    copy_net->horton_net = new Net();

    Fast_Horton(copy_net);
    delete(copy_net);
}

/* Compare function for Dijkstra's pripority queue based on the distance from the source */
struct comparator {
    bool operator() (Node* arg1, Node* arg2) {
        return (*arg1).distance > (*arg2).distance;
    }
};

/* Reset all distances to max value */
void Net::resetDistance(){
    for (vector<Node*>::iterator it = nodes.begin(); it != nodes.end(); it++){
        (*it)->distance = (int)nodes.size()+1;
        (*it)->predecessor = NULL;
    }
}

/* sets the cycle member fo all odes to be false */
void Net::reset_nodeCycle(){
    vector<Node*>::iterator it = nodes.begin();
    while (it != nodes.end()) {
        (*it)->cycle = false;
        it++;
    }
}

/* sets all nodesto unexplored */
void Net::reset_nodeExplored(){
    vector<Node*>::iterator it = nodes.begin();
    while (it != nodes.end()) {
        (*it)->explored = false;
        it++;
    }
}

/* Computes and returns the shortest path between src and dest in net */
Path* Net::Dijkstra(Node* src, Node* dest, Net* net){
    priority_queue<Node*, vector<Node*> , comparator> p_queue;
    net->resetDistance();
    src->distance = 0;
    p_queue.push(src);
    Node* current = NULL;
    Arc* arc = NULL;
    while (!p_queue.empty() && current != dest) {
        current = p_queue.top();
        p_queue.pop();
        if (current != dest) {
            for (vector<Arc*>::iterator it = current->branches.begin(); it != current->branches.end(); ++it) {
                arc = (*it);
                if (arc->neighbour(current)->distance > current->distance + 1) {
                    arc->neighbour(current)->distance = current->distance + 1;
                    arc->neighbour(current)->predecessor = current;
                    p_queue.push(arc->neighbour(current));
                }
            }
        }
        /* Stop when reaching destination */
        else
            break;
    }

    /* If there is no path between src and dest */
    if(dest->predecessor==NULL)
        return NULL;
    /* Reconstruct path backward */
    Path* p = new Path();
    current = dest;
    while (current) {
        /* The path should contain a pointer to the original nodes not the current copy */
        Node* n = get_node(current->_name);
        p->nodes.push_front(n);
        current = current->predecessor;
    }
    return p;
}

void Net::add_horton_nodes(Net* net){
    Node* copy = NULL;
    /* Add all neighbours of the end node (lowest degree) */
    Node* end_n = net->nodes.back();
    vector<Arc*>::iterator it = end_n->branches.begin();
    while(it != end_n->branches.end()){
        copy = (*it)->neighbour(end_n)->clone();
        net->horton_net->add_node(copy);
        it++;
    }
}

void Net::add_horton_branches(Net* net){
    Node* src;
    Node* dest;
    Path* shortest;
    Arc* arc;
    for (int i=0; i<net->horton_net->nodes.size()-1; i++) {
        src = net->horton_net->nodes[i];
        for (int j=i+1; j<net->horton_net->nodes.size(); j++) {
            dest = net->horton_net->nodes[j];
            /* computing shortest paths between neighbours of x in G-x */
            shortest = Dijkstra(net->get_node(src->_name), net->get_node(dest->_name), net);
            if (shortest!=NULL) {
                arc = new Arc(src, dest);
                arc->horton_path = shortest;
                arc->weight = shortest->length();
                net->horton_net->add_arc(arc);
            }
        }
    }
}

/* Compare function for fast_horton algorithm, ranking nodes in decreasing degree */
bool compareNodes(Node* n1, Node* n2){
    return n1->degree() > n2->degree();
}

/* Compare function for minimal_spanning_tree algorithm, ranking arcs in decreasing wheight */
bool compareArcs(Arc* a1, Arc* a2){
    return a1->weight > a2->weight;
}

/* Sort nodes in decreasing degree */
void Net::orderNodes(Net* net){
    if(!net->nodes.empty())
        sort(net->nodes.begin(), net->nodes.end(), compareNodes);
}

/* Sort nodes in decreasing degree */
void Net::orderArcs(Net* net){
    if(!net->arcs.empty())
        sort(net->arcs.begin(), net->arcs.end(), compareArcs);
}

/* Erase Horton network and free memory for cloned nodes and created arcs */
void Net::clear_horton_net(){
    if (!horton_net->nodes.empty()) {
        for (vector<Node*>::iterator it = horton_net->nodes.begin(); it != horton_net->nodes.end(); ++it) {
            delete(*it);
        }
        horton_net->nodes.clear();
    }
    if (!horton_net->arcs.empty()) {
        for (vector<Arc*>::iterator it = horton_net->arcs.begin(); it != horton_net->arcs.end(); ++it) {
            delete(*it);
        }
        horton_net->arcs.clear();
    }

    horton_net->nodeID.clear();

}

/*  @brief Computes a spanning tree of minimal weight in horton's network, then adds the corresponding cycles to the original network by connecting to src
 @note Implements Kruskal’s greedy algorithm
 */
void Net::minimal_spanning_tree(Node* src, Net* net){

    /* Check if the network has atleast one line */
    if (net->arcs.empty()) {
        return;
    }

    Path* min_tree = new Path();
    int n = (int)net->nodes.size();
    orderArcs(net);
    Arc* a = NULL;
    a = net->arcs.back();
    min_tree->nodes.push_back(a->_src);
    while (min_tree->nodes.size()<n) {
        a = net->arcs.back();
        /* Check if at least one node of the arc is not already in the path, in order to avoid creating a cycle */
        if(find(min_tree->nodes.begin(), min_tree->nodes.end(), a->_src) == min_tree->nodes.end() && find(min_tree->nodes.begin(), min_tree->nodes.end(), a->_dest) == min_tree->nodes.end()){
            min_tree->nodes.push_back(a->_src);
            min_tree->nodes.push_back(a->_dest);
            combine(src, a->horton_path);
            cycle_basis.push_back(a->horton_path);
        }
        else if(find(min_tree->nodes.begin(), min_tree->nodes.end(), a->_src) == min_tree->nodes.end()){
            min_tree->nodes.push_back(a->_src);
            combine(src, a->horton_path);
            cycle_basis.push_back(a->horton_path);
        }
        else if (find(min_tree->nodes.begin(), min_tree->nodes.end(), a->_dest) == min_tree->nodes.end()){
            min_tree->nodes.push_back(a->_dest);
            combine(src, a->horton_path);
            cycle_basis.push_back(a->horton_path);
        }
        net->arcs.pop_back();
    }
    delete min_tree;
}

void Net::Fast_Horton(Net *net){

    /* Arranging nodes in descending order of degree */
    orderNodes(net);

    /* Removing all nodes of degree 1 */
    while (!net->nodes.empty() && net->nodes.back()->degree()<2) {
        net->remove_end_node();
        orderNodes(net);
    }

    /* net has no cycles */
    if(net->nodes.size()<3)
        return;

    /* Erase Horton network */
    net->clear_horton_net();

    /* Adding nodes to Horton network */
    add_horton_nodes(net);

    /* Removing the end node from the original network */
    string n_id = net->remove_end_node();

    /* Computing the shortest paths among neighbours of n and creating the corresponding horton network */
    add_horton_branches(net);

    /* Computing the minimal spanning tree on the corresponding Horton network */

    minimal_spanning_tree(get_node(n_id), net->horton_net);

    while (net->nodes.size()>2)
        Fast_Horton(net);
}

//Net* Net::get_chordal_extension() {
//    Node* n = nullptr;
//    Node* u = nullptr;
//    Node* nn = nullptr;
//    Arc* arc = nullptr;
//    Arc* arc_chordal = nullptr;
//
//    Node* u_chordal = nullptr;
//    Node* nn_chordal = nullptr;
//
//    string name="";
//    string name_chordal="";
//    Net* chordal_extension = clone();
//    Net* graph_clone = clone_undirected();
//    int nb = 0;
//
//    /** cliques with less than 1 nodes are useless for us.*/
//    while (graph_clone->nodes.size() > 1) {
//        sort(graph_clone->nodes.begin(), graph_clone->nodes.end(),node_compare);
//        // last element has the minimum fill-in.
//        n = graph_clone->nodes.back();
//        Debug(n->_name << endl);
//        Debug(_clone->nodes.size() << endl);
//        vector<Node*> bag_copy;
//        vector<Node*> bag;
//        Debug("new bag_copy = { ");
//
//        for (auto nn: n->get_neighbours()) {
//            bag_copy.push_back(nn);
//            bag.push_back(get_node(nn->_name));
//            Debug(nn->_name << ", ");
//        }
//
//        graph_clone->remove_end_node();
//        bag_copy.push_back(n);
//        bag.push_back(get_node(n->_name)); // node in this graph
//        sort(bag_copy.begin(), bag_copy.end(),[](const Node* a, const Node* b) -> bool{return a->_id < b->_id;});
//        sort(bag.begin(), bag.end(),[](const Node* a, const Node* b) -> bool{return a->_id < b->_id;});
//
//        // update graph_graph and construct chordal extension.
//        for (int i = 0; i < bag_copy.size() - 1; i++) {
//            u = bag_copy.at(i);
//            u_chordal = chordal_extension->get_node(u->_name);
//            for (int j = i+1; j<bag_copy.size(); j++) {
//                nn = bag_copy.at(j);
//                nn_chordal=chordal_extension->get_node(nn->_name);
//                if (u->is_connected(nn)) {
//                    continue;
//                }
//                name = to_string((int) graph_clone->arcs.size()+1);
//                name_chordal = to_string((int)chordal_extension->arcs.size()+1);
//
//                arc = new Arc(name);
//                arc_chordal = new Arc(name_chordal);
//
//                arc->_id = arcs.size();
//                arc->_src = u;
//                arc->_dest = nn;
//                arc->connect();
//                graph_clone->add_undirected_arc(arc);
//
//                arc_chordal->_id = chordal_extension->arcs.size();
//                arc_chordal->_src = u_chordal;
//                arc_chordal->_dest = nn_chordal;
//                arc_chordal->connect();
//                chordal_extension->add_undirected_arc(arc_chordal);
//            }
//        }
//        _bags_copy.push_back(bag_copy);
//        _bags.push_back(bag);
//        if (bag_copy.size()==3) {
//            nb++;
//        }
//        delete n;
//    }
//    // sort the bags by its size (descending order)
//    sort(_bags.begin(), _bags.end(), bag_compare);
//    printf("With greedy fill-in algirithm, the chordal graph added  %lu edges \n", (chordal_extension->arcs.size() - arcs.size()));
//
//    delete graph_clone;
//    return chordal_extension;
//}

// get cliques from the tree decomposition
// Two methods
// first one: check the inclusion relationship
// second one: use the RIP property of the tree decomposition, thus just need to check every leaf..
// One need to execute either get_tree_decomposition or get_chordal_extension first, then run get_clique_tree.

// use _bags instead of bag_copy
//void Net::get_cliquebags (bool print) {
//    for (unsigned i = 0; i < _bags.size(); i++) {
//        for (unsigned j = i+1; j < _bags.size();) {
//            if (std::includes(_bags[i].begin(),_bags[i].end(),
//                              _bags[j].begin(), _bags[j].end()))
//            {
//                _bags.erase(_bags.begin()+j);
//            }
//            else
//                j++;
//        }
//    }
//    cout << "Number of maximal cliques of the chordal extension = " << _bags.size() << endl <<endl;
//}

/* Destructors */
Net::~Net() {
    if (!nodes.empty()) {
        for (vector<Node*>::iterator it = nodes.begin(); it != nodes.end(); it++) {
            delete (*it);
        }
        nodes.clear();
    }
    if(!arcs.empty()) {
        for (vector<Arc*>::iterator it = arcs.begin(); it != arcs.end(); it++) {
            if(*it)
                delete (*it);
        }
        arcs.clear();
    }

    if(!cycle_basis.empty()) {
        for (vector<Path*>::iterator it = cycle_basis.begin(); it != cycle_basis.end(); it++) {
            delete (*it);
        }
        arcs.clear();
    }
    if (horton_net!=NULL)
        delete horton_net;
    for (pair<string,set<Arc*>*> it:arcID) {
        delete it.second;
    }
}

/* Combines the src node with the path p to form a cycle */
void Net::combine(Node* src, Path* p){
    p->nodes.insert(p->nodes.begin(), src);
    p->nodes.push_back(src);
}

//Net* Net::get_clique_tree(){
//    Net* cliquetree = new Net();
//    Node* node = nullptr;
//    Arc*  a = nullptr;
//    string name;
//    get_cliquebags(true);
//#ifdef USE_BOOST
//    /** Note that we also need the edge information of the clique tree **/
//    /** boost graph library or implement the expanded version of MCS algorithm by Blair and Peyton */
//    typedef boost::adjacency_list <boost::vecS,
//    boost::vecS,
//    boost::undirectedS,
//    boost::no_property,
//    boost::property < boost::edge_weight_t, int >> Graph;
//    typedef boost::graph_traits <Graph>::edge_descriptor Edge;
//    //typedef boost::graph_traits <Graph>::vertex_descriptor Vertex;
//
//    // BUILD THE INTERSECTION GRAPH OF THE CLIQUES
//    typedef std::pair<int, int> E;
//    std::vector<E> edges;
//    std::vector<int> weights;
//    int nb_cliques = this->_bags.size();
//    for (int i = 0; i < nb_cliques; i++) {
//        DebugOn("bag " << i << " has " << this->_bags[i].size() << " nodes." <<endl);
//        sort(this->_bags[i].begin(), this->_bags[i].end());
//        for (int j = i +1; j < nb_cliques; j++) {
//            vector<Node*> v3;
//            sort(this->_bags[j].begin(), this->_bags[j].end());
//            set_intersection(this->_bags[i].begin(), this->_bags[i].end(), this->_bags[j].begin(), this->_bags[j].end(), back_inserter(v3));
//            if (v3.size() > 0) {
//                edges.push_back(E(i, j));
//                weights.push_back(-v3.size());
//            }
//        }
//    }
//    //size_t num_edges = edges.size();
//
//#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
//    Graph g(num_nodes);
//    boost::property_map<Graph, edge_weight_t>::type weightmap = get(edge_weight, g);
//    for (std::size_t j = 0; j < num_edges; ++j) {
//        Edge e;
//        bool inserted;
//        boost::tie(e, inserted) = boost::add_edge(edges[j].first, edges[j].second, g);
//        boost::weightmap[e] = weights[j];
//    }
//#else
//    Graph g(edges.begin(), edges.end(), weights.begin(), nb_cliques);
//#endif
//    boost::property_map < Graph, boost::edge_weight_t >::type weight = get(boost::edge_weight, g);
//    std::vector < Edge > spanning_tree;
//    boost::kruskal_minimum_spanning_tree(g, std::back_inserter(spanning_tree));
//
//    DebugOn("Print the total " << spanning_tree.size() << " edges in the clique tree:" << endl);
//
//    //////////CLIQUE TREE /////////////////////////////
//    for (int i = 0; i < nb_cliques; i++) {
//        node= new Node(to_string(i), i);
//        cliquetree->add_node(node);
//    }
//
//    for (std::vector < Edge >::iterator ei = spanning_tree.begin();
//         ei != spanning_tree.end(); ++ei) {
//        int u = source(*ei, g);
//        int v = target(*ei, g);
//        DebugOn(u << " <--> " << v
//                << " with weight of " << -weight[*ei]
//                << endl);
//        name = (int) cliquetree->arcs.size();
//        a = new Arc(name);
//        a->_id = cliquetree->arcs.size();
//
//        // intersection
//        vector<Node*> v3;
//        sort(this->_bags[u].begin(), this->_bags[u].end());
//        sort(this->_bags[v].begin(), this->_bags[v].end());
//        set_intersection(this->_bags[u].begin(), this->_bags[u].end(),
//                         this->_bags[v].begin(), this->_bags[v].end(),
//                         back_inserter(v3));
//        a->_src = cliquetree->get_node(to_string(u));
//        a->_dest = cliquetree->get_node(to_string(v));
//        a->_weight = -weight[*ei];
//        a->_intersection = v3;
//        cliquetree->add_arc(a);
//        a->connect();
//
//        for (int i = 0; i < v3.size(); i++){
//                auto  node = v3.at(i);
//            for (int j = i+1; j < v3.size(); j++){
//                auto arc = get_arc(node, v3.at(j));
//                if (arc != nullptr){
//                    a->_intersection_clique.push_back(new index_pair(index_(arc->_src->_name), index_(arc->_dest->_name), arc->_active));
//                }
//             //   else
//               //     a->_intersection_clique.push_back(new index_pair(index_(node->_name), index_(v3.at(j)->_name), true));
//            }
//        }
//    }
//#endif
//    return cliquetree;
//}

//void Net::chol_decompose(bool print){
//    arma::mat adjacency_matrix = arma::zeros(nodes.size(), nodes.size());
//    for (auto &arc: arcs){
//        adjacency_matrix(arc->_src->_id, arc->_dest->_id) = 1;
//        adjacency_matrix(arc->_dest->_id, arc->_src->_id) = 1;
//
//    }
//    arma::mat pertubation = arma::zeros(nodes.size(),nodes.size());
//    // if fails,  make epsilon larger.
//    unsigned epsilon = 2;
//    arma::mat adjacency_matrix_psd = adjacency_matrix + epsilon*pertubation.eye();
//    arma::mat chordal_sparsity = arma::zeros(nodes.size(),nodes.size());
//    chordal_sparsity = arma::chol(adjacency_matrix_psd);
//    if (print){
//        cout << "chordal sparsity matrix: \n " << chordal_sparsity << endl;
//    }
//    unsigned num_nonzeros = 0;
//    for (int i = 0; i < nodes.size(); i++)
//        for (int j = i+1; j< nodes.size(); j++){
//            if (chordal_sparsity(i, j) != 0){
//                num_nonzeros +=1;
//            }
//        }
//    printf("With cholesky decomposition, the chordal graph added  %lu edges \n", (num_nonzeros - arcs.size()));
//}

//std::vector<gravity::index_pair*> Net::get_bus_pairs_all(){
//    vector<gravity::index_pair*> res;
//    string ni, nj;
//    for(int i = 0; i < nodes.size()-1; i++) {
//        for(int j = i+1; j < nodes.size(); j++) {
//            ni = nodes[i]->_name;
//            nj = nodes[j]->_name;
//            res.push_back(new index_pair(index_(ni), index_(nj), 1));
//        }
//    }
//    return res;
//}


// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is 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.
// __END_LICENSE__

#include <vw/Core/Exception.h>
#include <vw/Math/Vector.h>
#include <asp/IsisIO/RPNEquation.h>

#include <iomanip>
#include <stack>
#include <vector>

#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>

using namespace vw;
using namespace asp;

// Constructors
//-----------------------------------------------------
RPNEquation::RPNEquation() {
  m_x_eq.clear();
  m_x_consts.clear();
  m_y_eq.clear();
  m_y_consts.clear();
  m_z_eq.clear();
  m_z_consts.clear();
  m_cached_time = -1;
  m_time_offset = 0;
}
RPNEquation::RPNEquation( std::string x_eq,
                          std::string y_eq,
                          std::string z_eq ) {
  string_to_eqn( x_eq, m_x_eq, m_x_consts );
  string_to_eqn( y_eq, m_y_eq, m_y_consts );
  string_to_eqn( z_eq, m_z_eq, m_z_consts );
  m_cached_time = -1;
  m_time_offset = 0;
}

// Update
//-----------------------------------------------------
void RPNEquation::update( double const& t ) {
  m_cached_time = t;
  double delta_t = t - m_time_offset;
  m_cached_output[0] = evaluate( m_x_eq,
                                 m_x_consts,
                                 delta_t );
  m_cached_output[1] = evaluate( m_y_eq,
                                 m_y_consts,
                                 delta_t );
  m_cached_output[2] = evaluate( m_z_eq,
                                 m_z_consts,
                                 delta_t );
}
void RPNEquation::string_to_eqn( std::string& str,
                                 std::vector<std::string>& commands,
                                 std::vector<double>& consts ) {
  // Breaks a string into the equation format used internally
  commands.clear();
  consts.clear();
  boost::split( commands, str, boost::is_any_of(" ="));

  // Cleaning out any tokens that are just ""
  for(std::vector<std::string>::iterator iter = commands.begin();
      iter != commands.end(); ++iter ) {
    if ( (*iter) == "" ) {
      iter = commands.erase(iter);
      iter--;
    }
  }

  // Pulling out the numbers
  for(std::vector<std::string>::iterator iter = commands.begin();
      iter != commands.end(); ++iter ) {
    if ( isdigit( (*iter)[(*iter).size()-1] ) ) {
      consts.push_back( atof( iter->c_str() ) );
      *iter = "c";
    }
  }
}
double RPNEquation::evaluate( std::vector<std::string>& commands,
                              std::vector<double>& consts,
                              double const& t ) {
  // Evaluates an equation in the internal format
  if ( commands.empty() )
    return 0;
  int consts_index = 0;
  std::stack<double> rpn_stack;
  double buffer;
  for ( std::vector<std::string>::iterator iter = commands.begin();
        iter != commands.end(); ++iter ) {
    if ( *iter == "c" ) {
      rpn_stack.push( consts[consts_index] );
      consts_index++;
    } else if ( *iter == "t" ) {
      rpn_stack.push( t );
    } else if ( rpn_stack.size() < 1 ) {
      vw_throw( IOErr() << "Insufficient arguments for RPN command: "
                << *iter << "\n" );
    } else if ( *iter == "sin" ) {
      buffer = sin( rpn_stack.top() );
      rpn_stack.pop();
      rpn_stack.push( buffer );
    } else if ( *iter == "cos" ) {
      buffer = cos( rpn_stack.top() );
      rpn_stack.pop();
      rpn_stack.push( buffer );
    } else if ( *iter == "tan" ) {
      buffer = tan( rpn_stack.top() );
      rpn_stack.pop();
      rpn_stack.push( buffer );
    } else if ( *iter == "abs" ) {
      buffer = fabs( rpn_stack.top() );
      rpn_stack.pop();
      rpn_stack.push( buffer );
    } else if ( rpn_stack.size() < 2 ) {
      vw_throw( IOErr() << "Insufficient arguments for command: "
                << *iter << "\n" );
    } else if ( *iter == "*" ) {
      buffer = rpn_stack.top();
      rpn_stack.pop();
      buffer *= rpn_stack.top();
      rpn_stack.pop();
      rpn_stack.push( buffer );
    } else if ( *iter == "/" ) {
      buffer = rpn_stack.top();
      rpn_stack.pop();
      buffer = rpn_stack.top() / buffer;
      rpn_stack.pop();
      rpn_stack.push( buffer );
    } else if ( *iter == "-" ) {
      buffer = rpn_stack.top();
      rpn_stack.pop();
      buffer = rpn_stack.top() - buffer;
      rpn_stack.pop();
      rpn_stack.push( buffer );
    } else if ( *iter == "+" ) {
      buffer = rpn_stack.top();
      rpn_stack.pop();
      buffer += rpn_stack.top();
      rpn_stack.pop();
      rpn_stack.push( buffer );
    } else if ( *iter == "^" ) {
      buffer = rpn_stack.top();
      rpn_stack.pop();
      buffer = pow( rpn_stack.top(), buffer );
      rpn_stack.pop();
      rpn_stack.push( buffer );
    } else {
      vw_throw( IOErr() << "Unknown RPN operator: " << *iter << "\n" );
    }
  } // End of calculator

  if ( rpn_stack.size() != 1 )
    vw_throw( IOErr() << "Unbalanced RPN equation! More constants than need by operators.\n" );

  return rpn_stack.top();
}

// FileIO
//-----------------------------------------------------
void RPNEquation::write( std::ofstream &f ) {
  for ( int i = 0; i < 3; i++ ) {
    std::vector<std::string>* eq_ptr;
    std::vector<double>* cs_ptr;
    switch(i) {
    case 0:
      eq_ptr = &m_x_eq;
      cs_ptr = &m_x_consts;
      break;
    case 1:
      eq_ptr = &m_y_eq;
      cs_ptr = &m_y_consts;
      break;
    case 2:
      eq_ptr = &m_z_eq;
      cs_ptr = &m_z_consts;
      break;
    }

    f << std::setprecision( 15 );
    int cs_idx = 0;
    for ( unsigned j = 0; j < eq_ptr->size(); j++ ) {
      if ( (*eq_ptr)[j] == "c" ) {
        f << (*cs_ptr)[cs_idx] << " ";
        cs_idx++;
      } else {
        f << (*eq_ptr)[j] << " ";
      }
    }
    f << "\n";
  }
}
void RPNEquation::read( std::ifstream &f ) {
  std::string buffer;
  m_cached_time = -1;

  buffer = "";
  std::getline( f, buffer );
  string_to_eqn( buffer, m_x_eq, m_x_consts );
  buffer = "";
  std::getline( f, buffer );
  string_to_eqn( buffer, m_y_eq, m_y_consts );
  buffer = "";
  std::getline( f, buffer );
  string_to_eqn( buffer, m_z_eq, m_z_consts );
}

// Constant Access
//-----------------------------------------------------
double& RPNEquation::operator[]( size_t const& n ) {
  m_cached_time = -1;
  if ( n >= m_x_consts.size() + m_y_consts.size()
       + m_z_consts.size() )
    vw_throw( ArgumentErr() << "RPNEquation: invalid index." );
  if ( n < m_x_consts.size() )
    return m_x_consts[n];
  else if ( n < m_x_consts.size() + m_y_consts.size() )
    return m_y_consts[n-m_x_consts.size()];
  return m_z_consts[n-m_x_consts.size()-m_y_consts.size()];
}


/////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Ion Gaztanaga  2007-2013
//
// 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)
//
// See http://www.boost.org/libs/intrusive for documentation.
//
/////////////////////////////////////////////////////////////////////////////
// The implementation of splay trees is based on the article and code published
// in C++ Users Journal "Implementing Splay Trees in C++" (September 1, 2005).
//
// The splay code has been modified and (supposedly) improved by Ion Gaztanaga.
//
// Here is the copyright notice of the original file containing the splay code:
//
//  splay_tree.h -- implementation of a STL compatible splay tree.
//
//  Copyright (c) 2004 Ralf Mattethat
//
//  Permission to copy, use, modify, sell and distribute this software
//  is granted provided this copyright notice appears in all copies.
//  This software is provided "as is" without express or implied
//  warranty, and with no claim as to its suitability for any purpose.
//
/////////////////////////////////////////////////////////////////////////////

#ifndef BOOST_INTRUSIVE_SPLAYTREE_ALGORITHMS_HPP
#define BOOST_INTRUSIVE_SPLAYTREE_ALGORITHMS_HPP

#include <boost/intrusive/detail/config_begin.hpp>
#include <boost/intrusive/detail/assert.hpp>
#include <boost/intrusive/intrusive_fwd.hpp>
#include <boost/intrusive/pointer_traits.hpp>
#include <cstddef>
#include <boost/intrusive/detail/utilities.hpp>
#include <boost/intrusive/bstree_algorithms.hpp>

namespace boost {
namespace intrusive {

/// @cond
namespace detail {

template<class NodeTraits>
struct splaydown_rollback
{
   typedef typename NodeTraits::node_ptr node_ptr;
   splaydown_rollback( const node_ptr *pcur_subtree, const node_ptr & header
                     , const node_ptr & leftmost           , const node_ptr & rightmost)
      : pcur_subtree_(pcur_subtree)  , header_(header)
      , leftmost_(leftmost)   , rightmost_(rightmost)
   {}

   void release()
   {  pcur_subtree_ = 0;  }

   ~splaydown_rollback()
   {
      if(pcur_subtree_){
         //Exception can only be thrown by comp, but
         //tree invariants still hold. *pcur_subtree is the current root
         //so link it to the header.
         NodeTraits::set_parent(*pcur_subtree_, header_);
         NodeTraits::set_parent(header_, *pcur_subtree_);
         //Recover leftmost/rightmost pointers
         NodeTraits::set_left (header_, leftmost_);
         NodeTraits::set_right(header_, rightmost_);
      }
   }
   const node_ptr *pcur_subtree_;
   node_ptr header_, leftmost_, rightmost_;
};

}  //namespace detail {
/// @endcond

//!   A splay tree is an implementation of a binary search tree. The tree is
//!   self balancing using the splay algorithm as described in
//!
//!      "Self-Adjusting Binary Search Trees
//!      by Daniel Dominic Sleator and Robert Endre Tarjan
//!      AT&T Bell Laboratories, Murray Hill, NJ
//!      Journal of the ACM, Vol 32, no 3, July 1985, pp 652-686
//!
//! splaytree_algorithms is configured with a NodeTraits class, which encapsulates the
//! information about the node to be manipulated. NodeTraits must support the
//! following interface:
//!
//! Typedefs:
//!
//! <tt>node</tt>: The type of the node that forms the binary search tree
//!
//! <tt>node_ptr</tt>: A pointer to a node
//!
//! <tt>const_node_ptr</tt>: A pointer to a const node
//!
//! Static functions:
//!
//! <tt>static node_ptr get_parent(const_node_ptr n);</tt>
//!
//! <tt>static void set_parent(node_ptr n, node_ptr parent);</tt>
//!
//! <tt>static node_ptr get_left(const_node_ptr n);</tt>
//!
//! <tt>static void set_left(node_ptr n, node_ptr left);</tt>
//!
//! <tt>static node_ptr get_right(const_node_ptr n);</tt>
//!
//! <tt>static void set_right(node_ptr n, node_ptr right);</tt>
template<class NodeTraits>
class splaytree_algorithms
   #ifndef BOOST_INTRUSIVE_DOXYGEN_INVOKED
   : public bstree_algorithms<NodeTraits>
   #endif
{
   /// @cond
   private:
   typedef bstree_algorithms<NodeTraits> bstree_algo;
   /// @endcond

   public:
   typedef typename NodeTraits::node            node;
   typedef NodeTraits                           node_traits;
   typedef typename NodeTraits::node_ptr        node_ptr;
   typedef typename NodeTraits::const_node_ptr  const_node_ptr;

   //! This type is the information that will be
   //! filled by insert_unique_check
   typedef typename bstree_algo::insert_commit_data insert_commit_data;

   public:
   #ifdef BOOST_INTRUSIVE_DOXYGEN_INVOKED
   //! @copydoc ::boost::intrusive::bstree_algorithms::get_header(const const_node_ptr&)
   static node_ptr get_header(const const_node_ptr & n);

   //! @copydoc ::boost::intrusive::bstree_algorithms::begin_node
   static node_ptr begin_node(const const_node_ptr & header);

   //! @copydoc ::boost::intrusive::bstree_algorithms::end_node
   static node_ptr end_node(const const_node_ptr & header);

   //! @copydoc ::boost::intrusive::bstree_algorithms::swap_tree
   static void swap_tree(const node_ptr & header1, const node_ptr & header2);

   //! @copydoc ::boost::intrusive::bstree_algorithms::swap_nodes(const node_ptr&,const node_ptr&)
   static void swap_nodes(const node_ptr & node1, const node_ptr & node2);

   //! @copydoc ::boost::intrusive::bstree_algorithms::swap_nodes(const node_ptr&,const node_ptr&,const node_ptr&,const node_ptr&)
   static void swap_nodes(const node_ptr & node1, const node_ptr & header1, const node_ptr & node2, const node_ptr & header2);

   //! @copydoc ::boost::intrusive::bstree_algorithms::replace_node(const node_ptr&,const node_ptr&)
   static void replace_node(const node_ptr & node_to_be_replaced, const node_ptr & new_node);

   //! @copydoc ::boost::intrusive::bstree_algorithms::replace_node(const node_ptr&,const node_ptr&,const node_ptr&)
   static void replace_node(const node_ptr & node_to_be_replaced, const node_ptr & header, const node_ptr & new_node);

   //! @copydoc ::boost::intrusive::bstree_algorithms::unlink(const node_ptr&)
   static void unlink(const node_ptr & node);

   //! @copydoc ::boost::intrusive::bstree_algorithms::unlink_leftmost_without_rebalance
   static node_ptr unlink_leftmost_without_rebalance(const node_ptr & header);

   //! @copydoc ::boost::intrusive::bstree_algorithms::unique(const const_node_ptr&)
   static bool unique(const const_node_ptr & node);

   //! @copydoc ::boost::intrusive::bstree_algorithms::size(const const_node_ptr&)
   static std::size_t size(const const_node_ptr & header);

   //! @copydoc ::boost::intrusive::bstree_algorithms::next_node(const node_ptr&)
   static node_ptr next_node(const node_ptr & node);

   //! @copydoc ::boost::intrusive::bstree_algorithms::prev_node(const node_ptr&)
   static node_ptr prev_node(const node_ptr & node);

   //! @copydoc ::boost::intrusive::bstree_algorithms::init(const node_ptr&)
   static void init(const node_ptr & node);

   //! @copydoc ::boost::intrusive::bstree_algorithms::init_header(const node_ptr&)
   static void init_header(const node_ptr & header);

   #endif   //#ifdef BOOST_INTRUSIVE_DOXYGEN_INVOKED

   //! @copydoc ::boost::intrusive::bstree_algorithms::erase(const node_ptr&,const node_ptr&)
   //! Additional notes: the previous node of z is splayed. The "splay" parameter which indicated if splaying
   //! should be performed, it's deprecated and will disappear in future versions.
   static void erase(const node_ptr & header, const node_ptr & z, bool splay = true)
   {
      //posibility 1
      if(splay && NodeTraits::get_left(z)){
         splay_up(bstree_algo::prev_node(z), header);
      }
      /*
      //possibility 2
      if(splay && NodeTraits::get_left(z)){
         node_ptr l = NodeTraits::get_left(z);
         splay_up(l, header);
      }*//*
      if(splay && NodeTraits::get_left(z)){
         node_ptr l = bstree_algo::prev_node(z);
         splay_up_impl(l, z);
      }*/
      /*
      //possibility 4
      if(splay){
         splay_up(z, header);
      }*/

      //if(splay)
         //splay_up(z, header);
      bstree_algo::erase(header, z);
   }

   #ifdef BOOST_INTRUSIVE_DOXYGEN_INVOKED
   //! @copydoc ::boost::intrusive::bstree_algorithms::clone(const const_node_ptr&,const node_ptr&,Cloner,Disposer)
   template <class Cloner, class Disposer>
   static void clone
      (const const_node_ptr & source_header, const node_ptr & target_header, Cloner cloner, Disposer disposer);

   //! @copydoc ::boost::intrusive::bstree_algorithms::clear_and_dispose(const node_ptr&,Disposer)
   template<class Disposer>
   static void clear_and_dispose(const node_ptr & header, Disposer disposer);

   #endif   //#ifdef BOOST_INTRUSIVE_DOXYGEN_INVOKED
   //! @copydoc ::boost::intrusive::bstree_algorithms::count(const const_node_ptr&,const KeyType&,KeyNodePtrCompare)
   //! Additional notes: the first node of the range is splayed.
   template<class KeyType, class KeyNodePtrCompare>
   static std::size_t count
      (const node_ptr & header, const KeyType &key, KeyNodePtrCompare comp)
   {
      std::pair<node_ptr, node_ptr> ret = equal_range(header, key, comp);
      std::size_t n = 0;
      while(ret.first != ret.second){
         ++n;
         ret.first = next_node(ret.first);
      }
      return n;
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::count(const const_node_ptr&,const KeyType&,KeyNodePtrCompare)
   //! Additional note: no splaying is performed
   template<class KeyType, class KeyNodePtrCompare>
   static std::size_t count
      (const const_node_ptr & header, const KeyType &key, KeyNodePtrCompare comp)
   {  return bstree_algo::count(header, key, comp);  }

   //! @copydoc ::boost::intrusive::bstree_algorithms::lower_bound(const const_node_ptr&,const KeyType&,KeyNodePtrCompare)
   //! Additional notes: the first node of the range is splayed. The "splay" parameter which indicated if splaying
   //! should be performed, it's deprecated and will disappear in future versions.
   template<class KeyType, class KeyNodePtrCompare>
   static node_ptr lower_bound
      (const node_ptr & header, const KeyType &key, KeyNodePtrCompare comp, bool splay = true)
   {
      //splay_down(detail::uncast(header), key, comp);
      node_ptr y = bstree_algo::lower_bound(header, key, comp);
      if(splay) splay_up(y, detail::uncast(header));
      return y;
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::lower_bound(const const_node_ptr&,const KeyType&,KeyNodePtrCompare)
   //! Additional note: no splaying is performed
   template<class KeyType, class KeyNodePtrCompare>
   static node_ptr lower_bound
      (const const_node_ptr & header, const KeyType &key, KeyNodePtrCompare comp)
   {  return bstree_algo::lower_bound(header, key, comp);  }

   //! @copydoc ::boost::intrusive::bstree_algorithms::upper_bound(const const_node_ptr&,const KeyType&,KeyNodePtrCompare)
   //! Additional notes: the first node of the range is splayed. The "splay" parameter which indicated if splaying
   //! should be performed, it's deprecated and will disappear in future versions.
   template<class KeyType, class KeyNodePtrCompare>
   static node_ptr upper_bound
      (const node_ptr & header, const KeyType &key, KeyNodePtrCompare comp, bool splay = true)
   {
      //splay_down(detail::uncast(header), key, comp);
      node_ptr y = bstree_algo::upper_bound(header, key, comp);
      if(splay) splay_up(y, detail::uncast(header));
      return y;
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::upper_bound(const const_node_ptr&,const KeyType&,KeyNodePtrCompare)
   //! Additional note: no splaying is performed
   template<class KeyType, class KeyNodePtrCompare>
   static node_ptr upper_bound
      (const const_node_ptr & header, const KeyType &key, KeyNodePtrCompare comp)
   {  return bstree_algo::upper_bound(header, key, comp);  }

   //! @copydoc ::boost::intrusive::bstree_algorithms::find(const const_node_ptr&, const KeyType&,KeyNodePtrCompare)
   //! Additional notes: the found node of the lower bound is splayed. The "splay" parameter which indicated if splaying
   //! should be performed, it's deprecated and will disappear in future versions.
   template<class KeyType, class KeyNodePtrCompare>
   static node_ptr find
      (const node_ptr & header, const KeyType &key, KeyNodePtrCompare comp, bool splay = true)
   {
      if(splay) splay_down(detail::uncast(header), key, comp);
      node_ptr end = detail::uncast(header);
      node_ptr y = bstree_algo::lower_bound(header, key, comp);
      node_ptr r = (y == end || comp(key, y)) ? end : y;
      return r;
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::find(const const_node_ptr&, const KeyType&,KeyNodePtrCompare)
   //! Additional note: no splaying is performed
   template<class KeyType, class KeyNodePtrCompare>
   static node_ptr find
      (const const_node_ptr & header, const KeyType &key, KeyNodePtrCompare comp)
   {  return bstree_algo::find(header, key, comp);  }

   //! @copydoc ::boost::intrusive::bstree_algorithms::equal_range(const const_node_ptr&,const KeyType&,KeyNodePtrCompare)
   //! Additional notes: the first node of the range is splayed. The "splay" parameter which indicated if splaying
   //! should be performed, it's deprecated and will disappear in future versions.
   template<class KeyType, class KeyNodePtrCompare>
   static std::pair<node_ptr, node_ptr> equal_range
      (const node_ptr & header, const KeyType &key, KeyNodePtrCompare comp, bool splay = true)
   {
      //splay_down(detail::uncast(header), key, comp);
      std::pair<node_ptr, node_ptr> ret = bstree_algo::equal_range(header, key, comp);
      if(splay) splay_up(ret.first, detail::uncast(header));
      return ret;
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::equal_range(const const_node_ptr&,const KeyType&,KeyNodePtrCompare)
   //! Additional note: no splaying is performed
   template<class KeyType, class KeyNodePtrCompare>
   static std::pair<node_ptr, node_ptr> equal_range
      (const const_node_ptr & header, const KeyType &key, KeyNodePtrCompare comp)
   {  return bstree_algo::equal_range(header, key, comp);  }

   //! @copydoc ::boost::intrusive::bstree_algorithms::bounded_range(const const_node_ptr&,const KeyType&,const KeyType&,KeyNodePtrCompare,bool,bool)
   //! Additional notes: the first node of the range is splayed. The "splay" parameter which indicated if splaying
   //! should be performed, it's deprecated and will disappear in future versions.
   template<class KeyType, class KeyNodePtrCompare>
   static std::pair<node_ptr, node_ptr> bounded_range
      (const node_ptr & header, const KeyType &lower_key, const KeyType &upper_key, KeyNodePtrCompare comp
      , bool left_closed, bool right_closed, bool splay = true)
   {
      std::pair<node_ptr, node_ptr> ret =
         bstree_algo::bounded_range(header, lower_key, upper_key, comp, left_closed, right_closed);
      if(splay) splay_up(ret.first, detail::uncast(header));
      return ret;
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::bounded_range(const const_node_ptr&,const KeyType&,const KeyType&,KeyNodePtrCompare,bool,bool)
   //! Additional note: no splaying is performed
   template<class KeyType, class KeyNodePtrCompare>
   static std::pair<node_ptr, node_ptr> bounded_range
      (const const_node_ptr & header, const KeyType &lower_key, const KeyType &upper_key, KeyNodePtrCompare comp
      , bool left_closed, bool right_closed)
   {  return bstree_algo::bounded_range(header, lower_key, upper_key, comp, left_closed, right_closed);  }

   //! @copydoc ::boost::intrusive::bstree_algorithms::insert_equal_upper_bound(const node_ptr&,const node_ptr&,NodePtrCompare)
   //! Additional note: the inserted node is splayed
   template<class NodePtrCompare>
   static node_ptr insert_equal_upper_bound
      (const node_ptr & header, const node_ptr & new_node, NodePtrCompare comp)
   {
      splay_down(header, new_node, comp);
      return bstree_algo::insert_equal_upper_bound(header, new_node, comp);
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::insert_equal_lower_bound(const node_ptr&,const node_ptr&,NodePtrCompare)
   //! Additional note: the inserted node is splayed
   template<class NodePtrCompare>
   static node_ptr insert_equal_lower_bound
      (const node_ptr & header, const node_ptr & new_node, NodePtrCompare comp)
   {
      splay_down(header, new_node, comp);
      return bstree_algo::insert_equal_lower_bound(header, new_node, comp);
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::insert_equal(const node_ptr&,const node_ptr&,const node_ptr&,NodePtrCompare)
   //! Additional note: the inserted node is splayed
   template<class NodePtrCompare>
   static node_ptr insert_equal
      (const node_ptr & header, const node_ptr & hint, const node_ptr & new_node, NodePtrCompare comp)
   {
      splay_down(header, new_node, comp);
      return bstree_algo::insert_equal(header, hint, new_node, comp);
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::insert_before(const node_ptr&,const node_ptr&,const node_ptr&)
   //! Additional note: the inserted node is splayed
   static node_ptr insert_before
      (const node_ptr & header, const node_ptr & pos, const node_ptr & new_node)
   {
      bstree_algo::insert_before(header, pos, new_node);
      splay_up(new_node, header);
      return new_node;
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::push_back(const node_ptr&,const node_ptr&)
   //! Additional note: the inserted node is splayed
   static void push_back(const node_ptr & header, const node_ptr & new_node)
   {
      bstree_algo::push_back(header, new_node);
      splay_up(new_node, header);
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::push_front(const node_ptr&,const node_ptr&)
   //! Additional note: the inserted node is splayed
   static void push_front(const node_ptr & header, const node_ptr & new_node)
   {
      bstree_algo::push_front(header, new_node);
      splay_up(new_node, header);
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::insert_unique_check(const const_node_ptr&,const KeyType&,KeyNodePtrCompare,insert_commit_data&)
   //! Additional note: nodes with the given key are splayed
   template<class KeyType, class KeyNodePtrCompare>
   static std::pair<node_ptr, bool> insert_unique_check
      (const node_ptr & header, const KeyType &key
      ,KeyNodePtrCompare comp, insert_commit_data &commit_data)
   {
      splay_down(header, key, comp);
      return bstree_algo::insert_unique_check(header, key, comp, commit_data);
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::insert_unique_check(const const_node_ptr&,const node_ptr&,const KeyType&,KeyNodePtrCompare,insert_commit_data&)
   //! Additional note: nodes with the given key are splayed
   template<class KeyType, class KeyNodePtrCompare>
   static std::pair<node_ptr, bool> insert_unique_check
      (const node_ptr & header, const node_ptr &hint, const KeyType &key
      ,KeyNodePtrCompare comp, insert_commit_data &commit_data)
   {
      splay_down(header, key, comp);
      return bstree_algo::insert_unique_check(header, hint, key, comp, commit_data);
   }

   #ifdef BOOST_INTRUSIVE_DOXYGEN_INVOKED
   //! @copydoc ::boost::intrusive::bstree_algorithms::insert_unique_commit(const node_ptr&,const node_ptr&,const insert_commit_data&)
   static void insert_unique_commit
      (const node_ptr & header, const node_ptr & new_value, const insert_commit_data &commit_data);

   //! @copydoc ::boost::intrusive::bstree_algorithms::is_header
   static bool is_header(const const_node_ptr & p);

   //! @copydoc ::boost::intrusive::bstree_algorithms::rebalance
   static void rebalance(const node_ptr & header);

   //! @copydoc ::boost::intrusive::bstree_algorithms::rebalance_subtree
   static node_ptr rebalance_subtree(const node_ptr & old_root);

   #endif   //#ifdef BOOST_INTRUSIVE_DOXYGEN_INVOKED

   // bottom-up splay, use data_ as parent for n    | complexity : logarithmic    | exception : nothrow
   static void splay_up(const node_ptr & node, const node_ptr & header)
   {
      // If (node == header) do a splay for the right most node instead
      // this is to boost performance of equal_range/count on equivalent containers in the case
      // where there are many equal elements at the end
      node_ptr n((node == header) ? NodeTraits::get_right(header) : node);
      node_ptr t(header);

      if( n == t ) return;

      for( ;; ){
         node_ptr p(NodeTraits::get_parent(n));
         node_ptr g(NodeTraits::get_parent(p));

         if( p == t )   break;

         if( g == t ){
            // zig
            rotate(n);
         }
         else if ((NodeTraits::get_left(p) == n && NodeTraits::get_left(g) == p)    ||
                  (NodeTraits::get_right(p) == n && NodeTraits::get_right(g) == p)  ){
            // zig-zig
            rotate(p);
            rotate(n);
         }
         else{
            // zig-zag
            rotate(n);
            rotate(n);
         }
      }
   }

   // top-down splay | complexity : logarithmic    | exception : strong, note A
   template<class KeyType, class KeyNodePtrCompare>
   static node_ptr splay_down(const node_ptr & header, const KeyType &key, KeyNodePtrCompare comp)
   {
      if(!NodeTraits::get_parent(header))
         return header;
      //Most splay tree implementations use a dummy/null node to implement.
      //this function. This has some problems for a generic library like Intrusive:
      //
      // * The node might not have a default constructor.
      // * The default constructor could throw.
      //
      //We already have a header node. Leftmost and rightmost nodes of the tree
      //are not changed when splaying (because the invariants of the tree don't
      //change) We can back up them, use the header as the null node and
      //reassign old values after the function has been completed.
      node_ptr t = NodeTraits::get_parent(header);
      //Check if tree has a single node
      if(!NodeTraits::get_left(t) && !NodeTraits::get_right(t))
         return t;
      //Backup leftmost/rightmost
      node_ptr leftmost (NodeTraits::get_left(header));
      node_ptr rightmost(NodeTraits::get_right(header));
      {
         //Anti-exception rollback, recovers the original header node if an exception is thrown.
         detail::splaydown_rollback<NodeTraits> rollback(&t, header, leftmost, rightmost);
         node_ptr null_node = header;
         node_ptr l = null_node;
         node_ptr r = null_node;

         for( ;; ){
            if(comp(key, t)){
               if(NodeTraits::get_left(t) == node_ptr() )
                  break;
               if(comp(key, NodeTraits::get_left(t))){
                  t = bstree_algo::rotate_right(t);

                  if(NodeTraits::get_left(t) == node_ptr())
                     break;
                  link_right(t, r);
               }
               else if(comp(NodeTraits::get_left(t), key)){
                  link_right(t, r);

                  if(NodeTraits::get_right(t) == node_ptr() )
                     break;
                  link_left(t, l);
               }
               else{
                  link_right(t, r);
               }
            }
            else if(comp(t, key)){
               if(NodeTraits::get_right(t) == node_ptr() )
                  break;

               if(comp(NodeTraits::get_right(t), key)){
                     t = bstree_algo::rotate_left( t );

                     if(NodeTraits::get_right(t) == node_ptr() )
                        break;
                     link_left(t, l);
               }
               else if(comp(key, NodeTraits::get_right(t))){
                  link_left(t, l);

                  if(NodeTraits::get_left(t) == node_ptr())
                     break;

                  link_right(t, r);
               }
               else{
                  link_left(t, l);
               }
            }
            else{
               break;
            }
         }

         assemble(t, l, r, null_node);
         rollback.release();
      }

      //Now recover the original header except for the
      //splayed root node.
      //t is the current root
      NodeTraits::set_parent(header, t);
      NodeTraits::set_parent(t, header);
      //Recover leftmost/rightmost pointers
      NodeTraits::set_left (header, leftmost);
      NodeTraits::set_right(header, rightmost);
      return t;
   }

   private:

   /// @cond

   // assemble the three sub-trees into new tree pointed to by t    | complexity : constant        | exception : nothrow
   static void assemble(const node_ptr &t, const node_ptr & l, const node_ptr & r, const const_node_ptr & null_node )
   {
      NodeTraits::set_right(l, NodeTraits::get_left(t));
      NodeTraits::set_left(r, NodeTraits::get_right(t));

      if(NodeTraits::get_right(l) != node_ptr()){
         NodeTraits::set_parent(NodeTraits::get_right(l), l);
      }

      if(NodeTraits::get_left(r) != node_ptr()){
         NodeTraits::set_parent(NodeTraits::get_left(r), r);
      }

      NodeTraits::set_left (t, NodeTraits::get_right(null_node));
      NodeTraits::set_right(t, NodeTraits::get_left(null_node));

      if( NodeTraits::get_left(t) != node_ptr() ){
         NodeTraits::set_parent(NodeTraits::get_left(t), t);
      }

      if( NodeTraits::get_right(t) ){
         NodeTraits::set_parent(NodeTraits::get_right(t), t);
      }
   }

   // break link to left child node and attach it to left tree pointed to by l   | complexity : constant | exception : nothrow
   static void link_left(node_ptr & t, node_ptr & l)
   {
      NodeTraits::set_right(l, t);
      NodeTraits::set_parent(t, l);
      l = t;
      t = NodeTraits::get_right(t);
   }

   // break link to right child node and attach it to right tree pointed to by r | complexity : constant | exception : nothrow
   static void link_right(node_ptr & t, node_ptr & r)
   {
      NodeTraits::set_left(r, t);
      NodeTraits::set_parent(t, r);
      r = t;
      t = NodeTraits::get_left(t);
   }

   // rotate n with its parent                     | complexity : constant    | exception : nothrow
   static void rotate(const node_ptr & n)
   {
      node_ptr p = NodeTraits::get_parent(n);
      node_ptr g = NodeTraits::get_parent(p);
      //Test if g is header before breaking tree
      //invariants that would make is_header invalid
      bool g_is_header = bstree_algo::is_header(g);

      if(NodeTraits::get_left(p) == n){
         NodeTraits::set_left(p, NodeTraits::get_right(n));
         if(NodeTraits::get_left(p) != node_ptr())
            NodeTraits::set_parent(NodeTraits::get_left(p), p);
         NodeTraits::set_right(n, p);
      }
      else{ // must be ( p->right == n )
         NodeTraits::set_right(p, NodeTraits::get_left(n));
         if(NodeTraits::get_right(p) != node_ptr())
            NodeTraits::set_parent(NodeTraits::get_right(p), p);
         NodeTraits::set_left(n, p);
      }

      NodeTraits::set_parent(p, n);
      NodeTraits::set_parent(n, g);

      if(g_is_header){
         if(NodeTraits::get_parent(g) == p)
            NodeTraits::set_parent(g, n);
         else{//must be ( g->right == p )
            BOOST_INTRUSIVE_INVARIANT_ASSERT(false);
            NodeTraits::set_right(g, n);
         }
      }
      else{
         if(NodeTraits::get_left(g) == p)
            NodeTraits::set_left(g, n);
         else  //must be ( g->right == p )
            NodeTraits::set_right(g, n);
      }
   }

   /// @endcond
};

/// @cond

template<class NodeTraits>
struct get_algo<SplayTreeAlgorithms, NodeTraits>
{
   typedef splaytree_algorithms<NodeTraits> type;
};

/// @endcond

} //namespace intrusive
} //namespace boost

#include <boost/intrusive/detail/config_end.hpp>

#endif //BOOST_INTRUSIVE_SPLAYTREE_ALGORITHMS_HPP


#include "NuiKinfuCPUDepthTracker.h"

#include "NuiKinfuCPUUtilities.h"
#include "NuiKinfuCPUFrame.h"
#include "NuiKinfuCPUFeedbackFrame.h"
#include "../NuiKinfuCameraState.h"

#include "Foundation/NuiDebugMacro.h"
#include "Foundation/NuiCholesky.h"

#include <iostream>
#include <boost/smart_ptr.hpp>

#define KINFU_ICP_CORESPS_NUM 29
#define WORK_GROUP_SIZE 128

using Eigen::AngleAxisf;

NuiKinfuCPUDepthTracker::NuiKinfuCPUDepthTracker(const NuiTrackerConfig& config, UINT nWidth, UINT nHeight)
	: m_configuration(config)
	, m_error(0.0f)
	, m_numValidPoints(0)
{
	AcquireBuffers(nWidth, nHeight);
}

NuiKinfuCPUDepthTracker::~NuiKinfuCPUDepthTracker()
{
	ReleaseBuffers();
}

void	NuiKinfuCPUDepthTracker::AcquireBuffers( UINT nWidth, UINT nHeight)
{
	const NuiTrackerConfig::ITERATION_CLASS& iterations = m_configuration.iterations;
	for (UINT i = 1; i < iterations.size(); i ++)
	{
		NuiFloatImage* depths = new NuiFloatImage();
		depths->AllocateBuffer(nWidth>>i, nHeight>>i);
		m_depthsHierarchy.push_back(depths);

		NuiFloat3Image* vertices = new NuiFloat3Image();
		vertices->AllocateBuffer(nWidth>>i, nHeight>>i);
		m_verticesHierarchy.push_back(vertices);
	}
}

void	NuiKinfuCPUDepthTracker::ReleaseBuffers()
{
	for (UINT i = 1; i < m_depthsHierarchy.size(); i ++)
	{
		NuiFloatImage* depths = m_depthsHierarchy.at(i);
		SafeDelete(depths);

		NuiFloat3Image* vertices = m_verticesHierarchy.at(i);
		SafeDelete(vertices);
	}
	m_depthsHierarchy.clear();
	m_verticesHierarchy.clear();
}

void NuiKinfuCPUDepthTracker::log(const std::string& fileName) const
{
	m_configuration.log(fileName);
}

bool	NuiKinfuCPUDepthTracker::EstimatePose(
	NuiKinfuFrame* pFrame,
	NuiKinfuFeedbackFrame* pFeedbackFrame,
	NuiKinfuCameraState* pCameraState,
	Eigen::Affine3f *hint
	)
{
	if(!pFrame)
		return false;
	NuiKinfuCPUFrame* pCPUFrame = dynamic_cast<NuiKinfuCPUFrame*>(pFrame);
	if(!pCPUFrame)
		return false;

	// half sample the input depth maps into the pyramid levels
	SubSampleDepths(pCPUFrame->GetFilteredDepthBuffer());

	if(!pCameraState)
		return false;

	HierarchyDepth2vertex(pCameraState->GetCameraPos().getIntrinsics());

	if(!pFeedbackFrame)
		return false;
	NuiKinfuCPUFeedbackFrame* pCPUFeedbackFrame = dynamic_cast<NuiKinfuCPUFeedbackFrame*>(pFeedbackFrame);
	if(!pCPUFeedbackFrame)
		return false;

	return IterativeClosestPoint(
		pCPUFrame->GetVertexBuffer(),
		pCPUFeedbackFrame->GetVertexBuffer(),
		pCPUFeedbackFrame->GetNormalsBuffer(),
		pCPUFrame->GetWidth(),
		pCPUFrame->GetHeight(),
		pCameraState,
		hint);
}

void NuiKinfuCPUDepthTracker::SubSampleDepths(float* filteredDepths)
{
	// Sub sample
	float depthThreshold = m_configuration.depth_threshold;
	const int subSampleRadius = 1;
	UINT nBufferSize = (UINT)m_depthsHierarchy.size();
	for (UINT i = 0; i < nBufferSize; ++i)
	{
		NuiFloatImage* depthsDstImg = m_depthsHierarchy.at(i);
		if(!depthsDstImg)
			continue;

		float* depthsSrcBuffer = (i > 0) ? m_depthsHierarchy.at(i-1)->GetBuffer() : filteredDepths;
		float* depthsDstBuffer = depthsDstImg->GetBuffer();
		if(!depthsSrcBuffer || !depthsDstBuffer)
			continue;

		UINT rangeX = depthsDstImg->GetWidth();
		UINT rangeY = depthsDstImg->GetHeight();
#ifdef WITH_OPENMP
		#pragma omp parallel for
#endif
		for (UINT y = 0; y < rangeY; y++)
		{
			for (UINT x = 0; x < rangeX; x++)
			{
				const UINT dstId = y * rangeX + x;

				const UINT src_x = x << 1;
				const UINT src_y = y << 1;
				const UINT src_size_x = rangeX << 1;
				const UINT srcId = src_y * src_size_x + src_x;
				float center = depthsSrcBuffer[srcId];

				float sumDepth = 0.0f;
				int sumWeight = 0;
				for(int cy = -subSampleRadius; cy <= subSampleRadius; ++ cy)
				{
					for(int cx = -subSampleRadius; cx <= subSampleRadius; ++ cx)
					{
						const int nearX = x + cx;
						const int nearY = y + cy;
						if( nearX>=0 && nearX<(int)rangeX && nearY>=0 && nearY<(int)rangeY )
						{
							const int nearId = nearY * rangeX + nearX;
							float near = depthsSrcBuffer[nearId];
							if(near > 0.0f && (center == NAN_FLOAT || fabs(center - near) < depthThreshold))
							{
								sumDepth += near;
								sumWeight += 1;
							}
						}
					}
				}
				depthsDstBuffer[dstId] = (sumWeight > 0) ? sumDepth/sumWeight : NAN_FLOAT;
			}
		}
	}
}

void NuiKinfuCPUDepthTracker::HierarchyDepth2vertex(NuiCameraIntrinsics cameraIntrics)
{
	for (UINT i = 0; i < m_depthsHierarchy.size(); ++i)
	{
		int div = 1 << (i+1);

		// depth2vertex
		NuiFloatImage* depthsImg = m_depthsHierarchy.at(i);
		NuiFloat3Image* verticesImg = m_verticesHierarchy.at(i);
		if(!depthsImg || !verticesImg)
			continue;

		float* depthsBuffer = depthsImg->GetBuffer();
		Vector3f* verticesBuffer = verticesImg->GetBuffer();
		if(!depthsBuffer || !verticesBuffer)
			continue;

		UINT rangeX = depthsImg->GetWidth();
		UINT rangeY = depthsImg->GetHeight();
#ifdef WITH_OPENMP
		#pragma omp parallel for
#endif
		for (UINT y = 0; y < rangeY; y++)
		{
			for (UINT x = 0; x < rangeX; x++)
			{
				const UINT id = y * rangeX + x;
				float dp = depthsBuffer[id];

				if(dp != NAN_FLOAT)
				{
					const float intr_fx_inv = div / cameraIntrics.m_fx;
					const float intr_fy_inv = div / cameraIntrics.m_fy;
					const float intr_cx = cameraIntrics.m_cx / div;
					const float intr_cy = cameraIntrics.m_cy / div;
					verticesBuffer[id][0] = dp * ((float)x - intr_cx) * intr_fx_inv;
					verticesBuffer[id][1] = dp * ((float)y - intr_cy) * intr_fy_inv;
					verticesBuffer[id][2] = dp;
				}
				else
				{
					verticesBuffer[id] = Vector3f(NAN_FLOAT, NAN_FLOAT, NAN_FLOAT);
				}
			}
		}
	}
}

Vector3f NuiKinfuCPUDepthTracker::InterpolateBilinear_withHoles(const Vector3f* source, Vector2f position, UINT nWidth)
{
	Vector3f a, b, c, d;
	Vector3f result;
	Eigen::Vector2i p; Vector2f delta;

	p[0] = (int)floor(position[0]); p[1] = (int)floor(position[1]);
	delta[0] = position[0] - (float)p[0]; delta[1] = position[1] - (float)p[1];

	a = source[p[0] + p[1] * nWidth];
	b = source[(p[0] + 1) + p[1] * nWidth];
	c = source[p[0] + (p[1] + 1) * nWidth];
	d = source[(p[0] + 1) + (p[1] + 1) * nWidth];

	if (_IsNan(a))
		delta[0] = 1.0f;
	if (_IsNan(b))
		delta[0] = 0.0f;
	if (_IsNan(c))
		delta[1] = 1.0f;
	if (_IsNan(d))
		delta[1] = 0.0f;

	result[0] = ((float)a[0] * (1.0f - delta[0]) * (1.0f - delta[1]) + (float)b[0] * delta[0] * (1.0f - delta[1]) +
		(float)c[0] * (1.0f - delta[0]) * delta[1] + (float)d[0] * delta[0] * delta[1]);
	result[1] = ((float)a[1] * (1.0f - delta[0]) * (1.0f - delta[1]) + (float)b[1] * delta[0] * (1.0f - delta[1]) +
		(float)c[1] * (1.0f - delta[0]) * delta[1] + (float)d[1] * delta[0] * delta[1]);
	result[2] = ((float)a[2] * (1.0f - delta[0]) * (1.0f - delta[1]) + (float)b[2] * delta[0] * (1.0f - delta[1]) +
		(float)c[2] * (1.0f - delta[0]) * delta[1] + (float)d[2] * delta[0] * delta[1]);

	return result;
}

///////////////////////////////////////////////////////////////////////////////////////////
// Iterative Closest Point
bool NuiKinfuCPUDepthTracker::IterativeClosestPoint(
	Vector3f* verticesBuffer,
	Vector3f* verticesPrevBuffer,
	Vector3f* normalsPrevBuffer,
	UINT	nWidth,
	UINT	nHeight,
	NuiKinfuCameraState* pCameraState,
	Eigen::Affine3f *hint
	)
{
	if(!pCameraState)
		return false;

	const NuiCameraIntrinsics& cameraIntrinsics = pCameraState->GetCameraPos().getIntrinsics();
	const Matrix3frm& Rprev = pCameraState->GetCameraPos().getRotation();
	const Vector3f& tprev = pCameraState->GetCameraPos().getLocalTranslation();

	Matrix3frm Rinv;
	Vector3f tcurr;
	if(hint)
	{
		Rinv = hint->rotation().inverse();
		tcurr = hint->translation().matrix();
	}
	else
	{
		Rinv = Rprev.inverse(); // tranform to global coo for ith camera pose
		tcurr = tprev;
	}

	/** \brief array with IPC iteration numbers for each pyramid level */
	const NuiTrackerConfig::ITERATION_CLASS& iterations = m_configuration.iterations;
	int LEVELS = (int)iterations.size();
	float distThreshStep = m_configuration.dist_threshold / LEVELS;
	float distThresh = m_configuration.dist_threshold + distThreshStep;

	//ScopeTime time("icp-all");
	for (int level_index = LEVELS-1; level_index>=0; --level_index)
	{
		Vector3f* vertices = verticesBuffer;
		UINT rangeX = nWidth;
		UINT rangeY = nHeight;

		if(level_index > 0)
		{
			vertices = m_verticesHierarchy.at(level_index-1)->GetBuffer();
			rangeX = m_verticesHierarchy.at(level_index-1)->GetWidth();
			rangeY = m_verticesHierarchy.at(level_index-1)->GetHeight();
		}
		if(!vertices)
			continue;

		//Vector3f* normalsBuffer = m_normals.GetBuffer();

		int iter_num = iterations[level_index].m_num;
		NuiTrackerConfig::TrackerIterationType iter_type = iterations[level_index].m_type;
		bool bShortIteration = (NuiTrackerConfig::eTracker_Iteration_Both != iter_type);
		const int numPara = bShortIteration ? 3 : 6;
		const int numParaSQ = bShortIteration ? 3 + 2 + 1 : 6 + 5 + 4 + 3 + 2 + 1;

		// Reset params
		m_error = 1e20f;
		m_numValidPoints = 0;
		float lambda = 1.0;
		Matrix3frm lastGoodRotInv = Rinv;
		Vector3f lastGoodTrans = tcurr;
		distThresh = distThresh - distThreshStep;

		for (int iter = 0; iter < iter_num; ++iter)
		{
			float sumF = 0.0f;
			float sumHessian[6 * 6], sumNabla[6];
			memset(sumHessian, 0, sizeof(float) * numParaSQ);
			memset(sumNabla, 0, sizeof(float) * numPara);
			for (UINT y = 0; y < rangeY; y ++)
			{
				for (UINT x = 0; x < rangeX; x ++)
				{
					const UINT id = y * rangeX + x;
					Vector3f vert = vertices[id];
					if(_IsNan(vert))
						continue;

					Vector3f projectedVert = Rinv * (vert - tcurr);
					Vector3f projectedPos = Rprev * projectedVert + tprev;
					Vector2f projPixel( projectedPos[0] * cameraIntrinsics.m_fx / projectedPos[2] + cameraIntrinsics.m_cx,
										projectedPos[1] * cameraIntrinsics.m_fy / projectedPos[2] + cameraIntrinsics.m_cy);
					if(projPixel[0] < 0 || (UINT)projPixel[0] >= nWidth-1 || projPixel[1] < 0 || (UINT)projPixel[1] >= nHeight-1)
						continue;

					Vector3f referenceVert = InterpolateBilinear_withHoles(verticesPrevBuffer, projPixel, nWidth);
					if(_IsNan(referenceVert))
						continue;

					Vector3f ptDiff = referenceVert - projectedVert;
					float dist = ptDiff.norm();
					if (dist > distThresh)
						continue;

					Vector3f referenceNorm = InterpolateBilinear_withHoles(normalsPrevBuffer, projPixel, nWidth);
					if(_IsNan(referenceNorm))
						continue;

					/*if (normalsBuffer)
					{
						Vector3f norm = normalsBuffer[id];
						float sine = norm.cross(referenceNorm).norm();
						if(sine < m_configuration.normal_threshold)
							continue;
					}*/

					float b = referenceNorm.dot(ptDiff);

					Vector3f row0 = projectedVert.cross(referenceNorm);
					float A[6];
					if (NuiTrackerConfig::eTracker_Iteration_Rotation == iter_type)
					{
						A[0] = row0[0];
						A[1] = row0[1];
						A[2] = row0[2];
					}
					else if (NuiTrackerConfig::eTracker_Iteration_Translation == iter_type)
					{
						A[0] = referenceNorm[0];
						A[1] = referenceNorm[1];
						A[2] = referenceNorm[2];
					}
					else
					{
						A[0] = row0[0];
						A[1] = row0[1];
						A[2] = row0[2];
						A[3] = referenceNorm[0];
						A[4] = referenceNorm[1];
						A[5] = referenceNorm[2];
					}

					float localF = b * b;
					float localHessian[6 + 5 + 4 + 3 + 2 + 1], localNabla[6];
					for (int r = 0, counter = 0; r < numPara; r++)
					{
						localNabla[r] = b * A[r];
						for (int c = 0; c <= r; c++, counter++)
							localHessian[counter] = A[r] * A[c];
					}

					// sum
					m_numValidPoints ++;
					sumF += localF;
					for (int i = 0; i < numPara; i++)
						sumNabla[i] += localNabla[i];
					for (int i = 0; i < numParaSQ; i++)
						sumHessian[i] += localHessian[i];
				}
			}

			// build hessian
			float hessian[6 * 6];
			for (int r = 0, counter = 0; r < numPara; r++)
				for (int c = 0; c <= r; c++, counter++)
					hessian[r + c * 6] = sumHessian[counter];
			for (int r = 0; r < numPara; ++r)
				for (int c = r + 1; c < numPara; c++)
					hessian[r + c * 6] = hessian[c + r * 6];
			float nabla[6];
			memcpy(nabla, sumNabla, numPara * sizeof(float));
			float current_error = (m_numValidPoints > 100) ? sqrt(sumF) / m_numValidPoints : 1e5f;
			if ((m_numValidPoints <= 0) || (current_error > m_error))
			{
				Rinv = lastGoodRotInv;
				tcurr = lastGoodTrans;
				lambda *= 10.0f;
			}
			else
			{
				lastGoodRotInv = Rinv;
				lastGoodTrans = tcurr;
				m_error = current_error;

				for (int i = 0; i < 6*6; ++i) hessian[i] = hessian[i] / m_numValidPoints;
				for (int i = 0; i < 6; ++i) nabla[i] = nabla[i] / m_numValidPoints;
				lambda /= 10.0f;
			}
			for (int i = 0; i < 6; ++i)
				hessian[i+i*6] *= 1.0f + lambda;

			if(!m_numValidPoints)
				break;

			// ComputeDelta
			float step[6];
			for (int i = 0; i < 6; i++)
				step[i] = 0;
			if (bShortIteration)
			{
				float smallHessian[3 * 3];
				for (int r = 0; r < 3; r++) for (int c = 0; c < 3; c++) smallHessian[r + c * 3] = hessian[r + c * 6];

				NuiCholesky cholA(smallHessian, 3);
				cholA.Backsub(step, nabla);
			}
			else
			{
				NuiCholesky cholA(hessian, 6);
				cholA.Backsub(step, nabla);
			}

			// ApplyDelta
			switch (iter_type)
			{
			case NuiTrackerConfig::eTracker_Iteration_Rotation:
				{
					Eigen::Matrix3f Rinc;
					Rinc.setIdentity();
					Rinc(0,1) = - step[2];
					Rinc(1,0) = step[2];
					Rinc(0,2) = step[1];
					Rinc(2,0) = - step[1];
					Rinc(1,2) = - step[0];
					Rinc(2,1) = step[0];
					Rinv = Rinc * Rinv;
					break;
				}
			case NuiTrackerConfig::eTracker_Iteration_Translation:
				{
					tcurr += Vector3f(step[0], step[1], step[2]);
					break;
				}
			default:
			case NuiTrackerConfig::eTracker_Iteration_Both:
				{
					Eigen::Matrix3f Rinc;
					Rinc.setIdentity();
					Rinc(0,1) = - step[2];
					Rinc(1,0) = step[2];
					Rinc(0,2) = step[1];
					Rinc(2,0) = - step[1];
					Rinc(1,2) = - step[0];
					Rinc(2,1) = step[0];
					Vector3f tinc(step[3], step[4], step[5]);
					Rinv = Rinc * Rinv;
					tcurr = Rinc * tcurr + tinc;
					break;
				}
			}

			// HasConverged
			float stepLength = 0.0f;
			for (int i = 0; i < 6; i++)
				stepLength += step[i] * step[i];

			if (sqrt(stepLength) / 6 < m_configuration.track_threshold)
				break;
		}
	}

	pCameraState->UpdateCameraTransform(Rinv.inverse(), tcurr);

#ifdef _DEBUG
	//For debug
	//std::cout << "t:" << tcurr[0] << "\t" << tcurr[1] << "\t" << tcurr[2] << std::endl;
#endif

	return true;
}


#pragma comment(lib,"winmm.lib")
#pragma comment(lib,"wsock32.lib")

#include "rtklib/rtklib.h"
#include <Eigen/Dense>

#include "utilities.h"
#include "WeightLeastSquare.h"
#include "SelectEphemeris.h"

#include "shadowMatching.h"

//#define TARGETWEEK 1997
//#define STARTTIME 218012
//#define ENDTIME   218014

std::string main_directory = "result\\";
std::string data_name = "v2v107M";

//read the rinex obx and nav(ephemeris) using rtklib function
extern int readobsnav(gtime_t ts, gtime_t te, double ti, char **infile,
	const int *index, int n, const prcopt_t *prcopt,
	obs_t *obs, nav_t *nav, sta_t *sta)
{
	int nepoch = 0;            /* number of observation epochs */
	int i, j, ind = 0, nobs = 0, rcv = 1;
	printf("%d %d %d %d\n", index[0], index[1], index[2], index[3]); // to read two obs file
	obs->data = NULL; obs->n = obs->nmax = 0;
	nav->eph = NULL; nav->n = nav->nmax = 0;
	nav->geph = NULL; nav->ng = nav->ngmax = 0;
	nav->seph = NULL; nav->ns = nav->nsmax = 0;

	for (i = 0; i < n; i++) {

		if (index[i] != ind) {
			if (obs->n > nobs) rcv++;
			ind = index[i]; nobs = obs->n;
		}
		//read rinex obs and nav file
		if (readrnxt(infile[i], rcv, ts, te, ti, prcopt->rnxopt[rcv <= 1 ? 0 : 1], obs, nav,
			rcv <= 2 ? sta + rcv - 1 : NULL) < 0) {
			return 0;
		}
	}

	if (obs->n <= 0) {
		return 0;
	}

	nepoch = sortobs(obs);

	/*
	if (nav->n<=0&&nav->ng<=0&&nav->ns<=0) {
	return 0;
	}
	*/
	// delete duplicated ephemeris
	//uniqnav(nav);

	// set time span for progress display
	if (ts.time == 0 || te.time == 0)
	{
		for (i = 0; i < obs->n; i++)
			if (obs->data[i].rcv == 1)
				break;
		for (j = obs->n - 1; j >= 0; j--)
			if (obs->data[j].rcv == 1)
				break;
		if (i < j) {
			if (ts.time == 0) ts = obs->data[i].time;
			if (te.time == 0) te = obs->data[j].time;
			settspan(ts, te);
		}
	}

	return 1;
}

//read the precise ephemeris (not used in 3D-GNSS code)
static void readpreceph(char **infile, int n, const prcopt_t *prcopt,
	nav_t *nav, sbs_t *sbs, lex_t *lex)
{
	seph_t seph0 = { 0 };
	int i;
	//    char *ext;

	trace(3, "readpreceph: n=%d\n", n);

	nav->ne = nav->nemax = 0;
	nav->nc = nav->ncmax = 0;
	sbs->n = sbs->nmax = 0;
	lex->n = lex->nmax = 0;

	/* read precise ephemeris files */
	for (i = 0; i < n; i++) {
		if (strstr(infile[i], "%r") || strstr(infile[i], "%b")) continue;
		readsp3(infile[i], nav, 0);
	}
	/* read precise clock files */
	for (i = 0; i < n; i++) {
		if (strstr(infile[i], "%r") || strstr(infile[i], "%b")) continue;
		readrnxc(infile[i], nav);
	}
	/* read sbas message files */
	for (i = 0; i < n; i++) {
		if (strstr(infile[i], "%r") || strstr(infile[i], "%b")) continue;
		sbsreadmsg(infile[i], prcopt->sbassatsel, sbs);
	}
	/* read lex message files */
	for (i = 0; i < n; i++) {
		if (strstr(infile[i], "%r") || strstr(infile[i], "%b")) continue;
		lexreadmsg(infile[i], 0, lex);
	}
	/* allocate sbas ephemeris */
	nav->ns = nav->nsmax = NSATSBS * 2;
	if (!(nav->seph = (seph_t *)malloc(sizeof(seph_t)*nav->ns))) {
		showmsg("error : sbas ephem memory allocation");
		trace(1, "error : sbas ephem memory allocation");
		return;
	}
	for (i = 0; i < nav->ns; i++) nav->seph[i] = seph0;

}

/* main function */
int main(int argc, char * argv[])
{

#ifdef STARTTIME
	gtime_t ts = gpst2time(TARGETWEEK, STARTTIME);
#else
	gtime_t ts = { 0 }; ts.time = 0;							// start time (0:all)
#endif

#ifdef 	ENDTIME
	gtime_t te = gpst2time(TARGETWEEK, ENDTIME);
#else
	gtime_t te = { 0 }; te.time = 0;							// end time (0:all)
#endif

		/* Configuration of RTKLIB ------------------------*/
	int n = 0, i, stat;

	double ti = 0.0;						// processing interval  (s) (0:all)
	double tu = 0.0;						// unit time (s) (0:all)

	/* options */
	prcopt_t prcopt = prcopt_default;	// processing option
	solopt_t solopt = solopt_default;	// output solution option
	filopt_t filopt = { "" };	            // file option

	prcopt.mode = 2;				// 0: SPP 1: DGNSS 2:Kinematic RTK
	prcopt.navsys = SYS_ALL;
	prcopt.nf = 1;						// frequency (1:L1,2:L1+L2,3:L1+L2+L5)
	prcopt.soltype = 0;					// 0:forward,1:backward,2:combined
	prcopt.elmin = 10.0*D2R;				// elevation mask (rad)
	prcopt.tidecorr = 0;					// earth tide correction (0:off,1-:on)
	prcopt.posopt[4] = 0;               // use RAIM FDE (qmo)
	prcopt.tropopt = TROPOPT_SAAS;        // troposphere option: Saastamoinen model
	prcopt.ionoopt = IONOOPT_BRDC;		// ionosphere option: Broad cast
	prcopt.sateph = EPHOPT_BRDC;			// ephemeris option: broadcast ephemeris

	prcopt.modear = 1;					// AR mode (0:off,1:continuous,2:instantaneous,3:fix and hold)

	// HKSC
	prcopt.rb[0] = -2414266.9197;           // base position for relative mode {x,y,z} (ecef) (m)
	prcopt.rb[1] = 5386768.9868;			// base position for relative mode {x,y,z} (ecef) (m)
	prcopt.rb[2] = 2407460.0314;			// base position for relative mode {x,y,z} (ecef) (m)

	solopt.outopt = 1;					// output processing options (0:no,1:yes)
	solopt.timef = 0;						// time format (0:sssss.s,1:yyyy/mm/dd hh:mm:ss.s)
	solopt.timeu = 3;						// time digits under decimal point
	solopt.sep[0] = ',';					// field separator
	solopt.sstat = 0;						// solution statistics level (0:off,1:states,2:residuals)
	solopt.trace = 0;						// debug trace level (0:off,1-5:debug)
	solopt.sstat = 0;						// get the solution file
	solopt.posf = SOLF_XYZ;
	solopt.height = 0;

	char *rov = "", *base = "";
	char infile_[5][1024] = { "" }, *infile[5];
	char outfile[1024];

	shadowMatching SM_;

	/* set input files */
	for (i = 0; i < 5; i++) infile[i] = infile_[i];
	strcpy(infile[n++], "data\\v2v107M2.obs");
	strcpy(infile[n++], "data\\hksc107m.18n");
	strcpy(infile[n++], "data\\hksc107m.18g");

	//strcpy(infile[n++], "data\\TSTb120L.obs");
	//strcpy(infile[n++], "data\\hksc1200.18n");
	//strcpy(infile[n++], "data\\hksc1200.18g");

	/**************************************************/

	/*starting to read the obs and nav rinex data*/
	int index[4] = { 0 };

	obs_t obss = { 0 };          /* observation data */
	nav_t navs = { 0 };          /* navigation data */
	sta_t stas[MAXRCV];      /* station infomation */
	lex_t lexs = { 0 };          /* lex messages */

	if (prcopt.mode == PMODE_KINEMA || prcopt.mode == PMODE_DGPS) {
		index[1] = 1;
		index[2] = 2;
		index[3] = 0;
	}

	// Step1: rinex obs and nav to variable obss and navs! @@@
	if (!readobsnav(ts, te, ti, infile, index, n, &prcopt, &obss, &navs, stas)) return 0;
	printf("obss.n = %d\n", obss.n);
	printf("navs.n navs.ng = %d %d\n", navs.n, navs.ng);

	// show raw measurement obss nav

	/*Initializa the output file in RTKLIB format*/
	FILE * fp;
	//std::string rtklibName2 = main_directory + data_name + ".pos";
	std::string rtklibName2 = main_directory + "SM_" + ".pos";
	fopen_s(&fp, rtklibName2.c_str(), "w");
	solopt.posf = SOLF_LLH;
	if (fp)
	{
		outheader_Q(fp, infile, n, &prcopt, &solopt, &obss);
		fclose(fp);
	}
	/*---------------------------*/

	/**************************************************/

	/* Step2: starting to use obs and nav rinex data @@@*/

	int i_obs, j_obs; // (i_obs,j_obs) are the (start,end) of rover obs.
	//for (i_obs = 0; i_obs<obss.n; i_obs++)
	//	printf("%d %d \t", i_obs, obss.data[i_obs].rcv);
	for (i_obs = 0; i_obs < obss.n; i_obs++)
		if (obss.data[i_obs].rcv == 1)
			break;

	for (j_obs = obss.n - 1; j_obs >= 0; j_obs--)
		if (obss.data[j_obs].rcv == 1)
			break;

	bool bFindSameData = false;

	int IndexStart = i_obs, IndexEnd = 0;
	int idx_epoch = 0;
	// note that the for is not every epoch of gps time
	for (int idx = i_obs; idx < j_obs + 1; idx++)
	{
		if (idx == obss.n + 1)
		{
			break;
		}

		int current_week1, current_week2;
		double current_tow1, current_tow2;
		current_tow1 = time2gpst(obss.data[idx].time, &current_week1);
		current_tow2 = time2gpst(obss.data[idx + 1].time, &current_week2);

		if ((current_week1 != current_week2) || fabs(current_tow1 - current_tow2) >0.49) // assuming 1Hz data rate. (if data rate higher than 2 Hz this does not valid)
		{
			IndexEnd = idx; // for current epoch
			bFindSameData = true;
		}
		//printf("IndexStart = %d, IndexEnd = %d\n", IndexStart, IndexEnd);

		/* Step3:find the measurements at the same epoch @@@*/

		if (bFindSameData)
		{
			idx_epoch++;
			int current_week;
			double current_tow;
			current_tow = time2gpst(obss.data[IndexStart].time, &current_week);
			printf("current_week = %4d - current_tow = %6.0f\n", current_week, current_tow);

			/* Step3.1:construct measurement matrix  @@@*/
			Eigen::MatrixXd eAllMeasurement; // (n,3) PRN CNO Pseudorange
			eAllMeasurement = ConstructMeasurementMatrix(IndexStart, IndexEnd, &obss);

			/* Step3.2: Select Ephemeris*/
			int target_idx[100], target_glo_idx[100]; // maximun number of satellite is 100. (GPS and GLONASS)
			int iNumValidSV = 0, iNumValidSV_glo = 0;
			SelectEphemeris(&navs, current_week, current_tow, target_idx, target_glo_idx, iNumValidSV, iNumValidSV_glo);

			/* Step3.3: caculating satellite positions @@@*/
			// caculating satellite positions (I suggest to make a structure about satellite position related information)
			Eigen::MatrixXd eAllSVPositions; // (n,5) prn, sx, sy, sz,
			eAllSVPositions = SatellitePosition(&navs, current_week, current_tow, target_idx, target_glo_idx, iNumValidSV, iNumValidSV_glo);

			/* Step3.4: WLS @@@
			initial guess for shadow Matching is availalble,
			*/
			Eigen::MatrixXd  eWLSSolutionECEF; // 6 unknowns with two clock bias variables
			eWLSSolutionECEF = WeightLeastSquare(eAllSVPositions, eAllMeasurement);

			// example of coordinate transformation from ECEF to LLH (please see more in rtklib.h. hint:F12 can trace back the function)
			Eigen::MatrixXd eWLSSolutionLLH(3, 1);
			double pos[3], r[3];
			for (int idx = 0; idx < 3; idx++) {
				pos[idx] = eWLSSolutionECEF(idx);
				//printf("%d %f %f\n", idx, eWLSSolutionECEF(idx), pos[idx]);
			}
			ecef2pos(pos, r);
			for (int idx = 0; idx < 3; idx++) {
				eWLSSolutionLLH(idx) = r[idx];
				//printf("%d %f %f\n", idx, eWLSSolutionLLH(idx), r[idx]);
			}

			// example to calculate azimuth and angle 2018/05/17 by LT Hsu
			Eigen::MatrixXd eELAZAngles;
			eELAZAngles = CalcELAZAngle(eWLSSolutionLLH, eAllSVPositions);

			// Add by Weisong WEN
			clock_t begin_time = clock();
			vector<shadowMatching::particle> particlesMain;
			//cout << "eAllMeasurement.size-> " << eAllMeasurement.rows() << "  eELAZAngles.size->  " << eELAZAngles.rows() << "\n";
			MatrixXd llh_;
			MatrixXd ecef_;
			for (int index = 0; index < 2; index++)
			{
				particlesMain = SM_.generateParticles(eWLSSolutionECEF, 50, 2, eAllMeasurement, eELAZAngles);
				llh_ = SM_.weightSM_();
				ecef_ = SM_.llh2ecef(llh_);
				SM_.particles.clear();

			}
			std::cout << "shadow matching  result: \n" << llh_ << "\n\n";
			std::cout << "one circle  used  time ---------------------------------------------------------------------------------------------> " << float(clock() - begin_time) / CLOCKS_PER_SEC << "\n\n";
			//Output as RTKLIB format
			fopen_s(&fp, rtklibName2.c_str(), "a");
			if (fp)
			{
				//fprintf(fp, "%4d, %9.3f,  %11.9f, %12.9f,  %9.4f, %d\n", current_week, current_tow, eWLSSolutionLLH(0)*R2D, eWLSSolutionLLH(1)*R2D, eWLSSolutionLLH(2),1);
				fprintf(fp, "%4d, %9.3f,  %11.9f, %12.9f,  %9.4f, %d\n", current_week, current_tow, llh_(1), llh_(0), llh_(2), 1);
				fclose(fp);
			}

			// End of an epoch
			IndexStart = idx + 1; // don't touch this InderStart
			//printf("\n");
			bFindSameData = false;

			// coordinate convertion.

		}

	}
	/**************************************************/

	//shadowMatching SM_;
	system("pause");
	return 0;
}

/* dummy application functions for shared library ----------------------------*/
extern int showmsg(char *format, ...) {
	va_list arg;
	char buff[1024];
	if (*format) {
		va_start(arg, format);
		vsprintf(buff, format, arg);
		va_end(arg);
		printf("%s\n", buff);
	}
	return 0;
}
extern void settspan(gtime_t ts, gtime_t te) {}
extern void settime(gtime_t time) {}