vtree_knn_demo.cpp

#include <algorithm>
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <fstream>
#include <iostream>
#include <map>
#include <metis.h>
#include <queue>
#include <sys/time.h>
#include <vector>

using namespace std;

const bool DEBUG_ = false;
const char *Edge_File;
const int Partition_Part = 4; // fanout of the vtree
const int Naive_Split_Limit = 33; // number of leaf nodes
bool Save_Order_Matrix = false; // Whether need to support route Matrix(If  true can have additional usage)
const int INF = 0x3fffffff;
const bool RevE = false; // Whether need to do some copy, Directed Graph:false，Undirected Graph:true
const bool Distance_Offset = false; // Whether consider the offset of the vehicle.
const bool DEBUG1 = false;
bool NWFIX;

const bool Optimization_KNN_Cut = true; // Optimization for KNN
const bool Memory_Optimization = true; // Optimization for memory (Usually on)
const bool INDEX_Optimization =
    true; // Optimization of Index, if need to store more information turn off (usually no need)

struct timeval tv;
long long ts, te;
void TIME_TICK_START() {
    gettimeofday(&tv, nullptr);
    ts = tv.tv_sec * 1000000 + tv.tv_usec;
}
void TIME_TICK_END() {
    gettimeofday(&tv, nullptr);
    te = tv.tv_sec * 1000000 + tv.tv_usec;
}
void TIME_TICK_PRINT(const char *T, int N = 1) { printf("%s RESULT: \t%lld\t (us)\r\n", (T), (te - ts) / N); }
void TIME_TICK_ALL_PRINT(const char *T, int N = 1) { printf("%s RESULT: \t%lld\t (us)\r\n", (T), (te - ts)); }

template <typename A, typename B>
istream &operator>>(istream &in, pair<A, B> &p) {
    in >> p.first >> p.second;
    return in;
}
template <typename A, typename B>
ostream &operator<<(ostream &out, const pair<A, B> &p) {
    out << p.first << ' ' << p.second << ' ';
    return out;
}
template <typename T>
ostream &operator<<(ostream &out, const vector<T> &v) {
    out << v.size() << ' ';
    for (int i = 0; i < v.size(); i++)
        out << v[i] << ' ';
    return out;
}
template <typename T>
istream &operator>>(istream &in, vector<T> &v) {
    int n;
    in >> n;
    v.clear();
    while (n--) {
        T a;
        in >> a;
        v.push_back(a);
    }
    return in;
}

template <typename A, typename B>
ostream &operator<<(ostream &out, const map<A, B> &h) {
    out << h.size() << ' ';
    typename map<A, B>::const_iterator iter;
    for (iter = h.begin(); iter != h.end(); iter++)
        out << iter->first << ' ' << iter->second << ' ';
    return out;
}
template <typename A, typename B>
istream &operator>>(istream &in, map<A, B> &h) {
    int n;
    in >> n;
    h.clear();
    A a;
    B b;
    while (n--) {
        in >> a >> b;
        h[a] = b;
    }
    return in;
}
template <typename T>
void save(ostream &out, T a) {
    out.write((char *)&a, sizeof(a));
}
template <typename T>
void save(ostream &out, vector<T> &a) {
    save(out, (int)a.size());
    for (int i = 0; i < a.size(); i++)
        save(out, a[i]);
}
template <typename A, typename B>
void save(ostream &out, const pair<A, B> &p) {
    save(out, p.first);
    save(out, p.second);
}
template <typename A, typename B>
void save(ostream &out, const map<A, B> &h) {

    save(out, (int)h.size());
    typename map<A, B>::const_iterator iter;
    for (iter = h.begin(); iter != h.end(); iter++) {
        save(out, iter->first);
        save(out, iter->second);
    }
}
template <typename T>
void read(istream &in, T &a) {
    in.read((char *)&a, sizeof(a));
}
template <typename T>
void read(istream &in, vector<T> &a) {
    int n;
    read(in, n);
    a = vector<T>(n);
    for (int i = 0; i < n; i++)
        read(in, a[i]);
}
template <typename A, typename B>
void read(istream &in, pair<A, B> &p) {
    read(in, p.first);
    read(in, p.second);
}
template <typename A, typename B>
void read(istream &in, map<A, B> &h) {
    int n;
    read(in, n);
    h.clear();
    A a;
    B b;
    while (n--) {
        read(in, a);
        read(in, b);
        h[a] = b;
    }
}
struct Graph // Struct of the graph
{
    int n, m; // n vertices and m edges, the number starts from 0 to n-1
    int tot;
    vector<int> id; // id[i] is the real number of vertex [i]
    vector<int> head, list, next, cost; // Adjacent Table
    Graph() { clear(); }
    ~Graph() { clear(); }
    friend ostream &operator<<(ostream &out, const Graph &G) // Storage the structure(stdout)
    {
        out << G.n << ' ' << G.m << ' ' << G.tot << ' ' << G.id << ' ' << G.head << ' ' << G.list << ' ' << G.next
            << ' ' << G.cost << ' ';
        return out;
    }
    friend istream &operator>>(istream &in, Graph &G) // Read Instruction(stdout)
    {
        in >> G.n >> G.m >> G.tot >> G.id >> G.head >> G.list >> G.next >> G.cost;
        return in;
    }
    void add_D(int a, int b, int c) // add a edge a->b with weight c (directed)
    {
        tot++;
        list[tot] = b;
        cost[tot] = c;
        next[tot] = head[a];
        head[a] = tot;
    }
    void add(int a, int b, int c) // add a edge with weight c in undirected graph a<->b
    {
        add_D(a, b, c);
        add_D(b, a, c);
    }
    void init(int N, int M, int t = 1) {
        clear();
        n = N;
        m = M;
        tot = t;
        head = vector<int>(N);
        id = vector<int>(N);
        list = vector<int>(M * 2 + 2);
        next = vector<int>(M * 2 + 2);
        cost = vector<int>(M * 2 + 2);
    }
    void clear() {
        n = m = tot = 0;
        head.clear();
        list.clear();
        next.clear();
        cost.clear();
        id.clear();
    }
    // Graph Partition
    vector<int> color; // 01 color vector
    vector<int> con; // Whether is connected
    vector<int> Split(Graph *G[], int nparts) // Partition the graph to two parts, and stores to G1, G2,2 METIS
                                              // algorithm,npart is the number of parts
    {

        vector<int> color(n);
        int i;
        /*if(n<Naive_Split_Limit)
        {
            return Split_Naive(*G[0],*G[1]);
        }*/

        if (DEBUG1)
            printf("Begin-Split\n");
        if (n == nparts) {
            for (i = 0; i < n; i++)
                color[i] = i;
        }
        else {
            idx_t options[METIS_NOPTIONS];
            memset(options, 0, sizeof(options));
            {
                METIS_SetDefaultOptions(options);
                options[METIS_OPTION_PTYPE] = METIS_PTYPE_KWAY; // _RB
                options[METIS_OPTION_OBJTYPE] = METIS_OBJTYPE_CUT; // _VOL
                options[METIS_OPTION_CTYPE] = METIS_CTYPE_SHEM; // _RM
                options[METIS_OPTION_IPTYPE] = METIS_IPTYPE_RANDOM; // _GROW _EDGE _NODE
                options[METIS_OPTION_RTYPE] = METIS_RTYPE_FM; // _GREEDY _SEP2SIDED _SEP1SIDED
                // options[METIS_OPTION_NCUTS] = 1;
                // options[METIS_OPTION_NITER] = 10;
                /* balance factor, used to be 500 */
                options[METIS_OPTION_UFACTOR] = 500;
                // options[METIS_OPTION_MINCONN];
                options[METIS_OPTION_CONTIG] = 1;
                // options[METIS_OPTION_SEED];
                options[METIS_OPTION_NUMBERING] = 0;
                // options[METIS_OPTION_DBGLVL] = 0;
            }
            idx_t nvtxs = n;
            idx_t ncon = 1;
            // transform
            int *xadj = new idx_t[n + 1];
            int *adj = new idx_t[n + 1];
            int *adjncy = new idx_t[tot - 1];
            int *adjwgt = new idx_t[tot - 1];
            int *part = new idx_t[n];


            int xadj_pos = 1;
            int xadj_accum = 0;
            int adjncy_pos = 0;

            // xadj, adjncy, adjwgt
            xadj[0] = 0;
            int i = 0;
            for (int i = 0; i < n; i++) {

                for (int j = head[i]; j; j = next[j]) {
                    int enid = list[j];
                    xadj_accum++;
                    adjncy[adjncy_pos] = enid;
                    adjwgt[adjncy_pos] = cost[j];
                    adjncy_pos++;
                }
                xadj[xadj_pos++] = xadj_accum;
            }


            // adjust nodes number started by 0

            // adjwgt -> 1
            for (int i = 0; i < adjncy_pos; i++) {
                adjwgt[i] = 1;
            }

            // nparts
            int objval = 0;
            // METIS
            METIS_PartGraphKway(&nvtxs, &ncon, xadj, adjncy, nullptr, nullptr, adjwgt, &nparts, nullptr, nullptr, options, &objval,
                                part);
            for (int i = 0; i < n; i++)
                color[i] = part[i];
            delete[] xadj;
            delete[] adj;
            delete[] adjncy;
            delete[] adjwgt;
            delete[] part;
        }
        // Partation
        int j;
        vector<int> new_id;
        vector<int> tot(nparts, 0), m(nparts, 0);
        for (i = 0; i < n; i++)
            new_id.push_back(tot[color[i]]++);
        for (i = 0; i < n; i++)
            for (j = head[i]; j; j = next[j])
                if (color[list[j]] == color[i])
                    m[color[i]]++;
        for (int t = 0; t < nparts; t++) {
            (*G[t]).init(tot[t], m[t]);
            for (i = 0; i < n; i++)
                if (color[i] == t)
                    for (j = head[i]; j; j = next[j])
                        if (color[list[j]] == color[i])
                            (*G[t]).add_D(new_id[i], new_id[list[j]], cost[j]);
        }
        for (i = 0; i < tot.size(); i++)
            tot[i] = 0;
        for (i = 0; i < n; i++)
            (*G[color[i]]).id[tot[color[i]]++] = id[i];
        if (DEBUG1)
            printf("Split_over\n");
        return color;
    }
} G;
struct Matrix // Distance Matrix
{
    Matrix() : n(0), a(nullptr) {}
    ~Matrix() { clear(); }
    int n; // n*n Matrix
    int *a;
    friend ostream &operator<<(ostream &out, const Matrix &M) {
        out << M.n << ' ';
        for (int i = 0; i < M.n; i++)
            for (int j = 0; j < M.n; j++)
                out << M.a[i * M.n + j] << ' ';
        return out;
    }
    friend istream &operator>>(istream &in, Matrix &M) {
        in >> M.n;
        M.a = new int[M.n * M.n];
        for (int i = 0; i < M.n; i++)
            for (int j = 0; j < M.n; j++)
                in >> M.a[i * M.n + j];
        return in;
    }
    void save_binary(ostream &out) {
        save(out, n);
        for (int i = 0; i < n; i++)
            for (int j = 0; j < n; j++)
                save(out, a[i * n + j]);
    }
    void read_binary(istream &in) {
        read(in, n);
        a = new int[n * n];
        for (int i = 0; i < n; i++)
            for (int j = 0; j < n; j++)
                read(in, a[i * n + j]);
    }
    void cover(int x) {
        for (int i = 0; i < n; i++)
            for (int j = 0; j < n; j++)
                a[i * n + j] = x;
    }
    void init(int N) {
        clear();
        n = N;
        a = new int[n * n];
        for (int i = 0; i < n * n; i++)
            a[i] = INF;
        for (int i = 0; i < n; i++)
            a[i * n + i] = 0;
    }
    void clear() { delete[] a; }
    void floyd() // Using floyd to calculate the matrix
    {
        int i, j, k;
        for (k = 0; k < n; k++)
            for (i = 0; i < n; i++)
                for (j = 0; j < n; j++)
                    if (a[i * n + j] > a[i * n + k] + a[k * n + j])
                        a[i * n + j] = a[i * n + k] + a[k * n + j];
    }
    void floyd(Matrix &order) // Calculate the matrix with floyd algorithm, and record result to order(route found)
    {
        int i, j, k;
        for (k = 0; k < n; k++)
            for (i = 0; i < n; i++)
                for (j = 0; j < n; j++)
                    if (a[i * n + j] > a[i * n + k] + a[k * n + j]) {
                        a[i * n + j] = a[i * n + k] + a[k * n + j];
                        order.a[i * n + j] = k;
                    }
    }
    Matrix &operator=(const Matrix &m) {
        if (this != (&m)) {
            init(m.n);
            for (int i = 0; i < n; i++)
                for (int j = 0; j < n; j++)
                    a[i * n + j] = m.a[i * n + j];
        }
        return *this;
    }
    int *operator[](int x) { return a + x * n; }
};
struct Node
{
    Node() { clear(); }
    int part; // number of son nodes
    int n, father,
        deep; // n: the number of subgraph, the id of father node,son[2]left son and right son,deep: current deepth
    vector<int> son;
    Graph G; // subgraph
    vector<int> color; // which subgraph the node belongs to
    Matrix dist, order; // border distance matrix ,border pass route with floyd k,order=(-1:connected
                        // directly)|(-2:connected in parents)|(-3:connected in son nodes)|(-INF:none)
    map<int, pair<int, int>>
        borders; // first:real id of border ;second:<number of border in borders, corresponding nodes(0~n-1)>
    vector<int> border_in_father, border_in_son, border_id, border_id_innode; //
    // the number of borders in father node and son node, raw id in graph G, number of border in sub node
    vector<int> path_record; // temp record for route
    int cache_vertex_id, cache_bound; // the start id stored in cache, the updated bound in cache(bound had updated end)
    vector<int> cache_dist;
    // cache_dist stores the distance from cache_id to all the borders, only be correct when it small or equal to
    // cache_bound
    vector<int> border_son_id; // the number of son node which border belongs to
    int min_border_dist; // min cached distance of current border in the cache (for KNN cut)
    vector<pair<int, int>> min_car_dist; // the nearest car and the node_id to the border <car_dist,node_id>
    friend ostream &operator<<(ostream &out, const Node &N) {
        out << N.n << ' ' << N.father << ' ' << N.part << ' ' << N.deep << ' ' << N.cache_vertex_id << ' '
            << N.cache_bound << ' ' << N.min_border_dist << ' ';
        for (int i = 0; i < N.part; i++)
            out << N.son[i] << ' ';
        if (INDEX_Optimization) {
            out << N.color << ' ' << N.dist << ' ' << N.borders << ' ';
            if (Save_Order_Matrix)
                out << N.order << ' ';
        }
        else {
            out << N.color << ' ' << N.dist << ' ' << N.borders << ' ' << N.border_in_father << ' ' << N.border_in_son
                << ' ' << N.border_id << ' ' << N.border_id_innode << ' ' << N.path_record << ' ' << N.cache_dist << ' '
                << N.min_car_dist << ' ' << N.border_son_id << ' ';
            if (Save_Order_Matrix)
                out << N.order << ' ';
        }
        return out;
    }
    friend istream &operator>>(istream &in, Node &N) {
        in >> N.n >> N.father >> N.part >> N.deep >> N.cache_vertex_id >> N.cache_bound >> N.min_border_dist;
        N.son.clear();
        N.son = vector<int>(N.part);
        for (int i = 0; i < N.part; i++)
            in >> N.son[i];
        if (INDEX_Optimization) {
            in >> N.color >> N.dist >> N.borders;
            if (Save_Order_Matrix)
                in >> N.order;
        }
        else {
            in >> N.color >> N.dist >> N.borders >> N.border_in_father >> N.border_in_son >> N.border_id >>
                N.border_id_innode >> N.path_record >> N.cache_dist >> N.min_car_dist >> N.border_son_id;
            if (Save_Order_Matrix)
                in >> N.order;
        }
        return in;
    }
    void save_binary(ostream &out) {
        save(out, n);
        save(out, father);
        save(out, part);
        save(out, deep);
        save(out, cache_vertex_id);
        save(out, cache_bound);
        save(out, min_border_dist);
        for (int i = 0; i < part; i++)
            save(out, son[i]);
        if (INDEX_Optimization) {
            save(out, color);
            dist.save_binary(out);
            if (Save_Order_Matrix)
                order.save_binary(out);
            save(out, borders);
        }
        else {
            save(out, color);
            dist.save_binary(out);
            if (Save_Order_Matrix)
                order.save_binary(out);
            save(out, borders);
            save(out, border_in_father);
            save(out, border_in_son);
            save(out, border_id);
            save(out, border_id_innode);
            save(out, path_record);
            save(out, cache_dist);
            save(out, min_car_dist);
            save(out, border_son_id);
        }
    }
    void read_binary(istream &in) {
        read(in, n);
        read(in, father);
        read(in, part);
        read(in, deep);
        read(in, cache_vertex_id);
        read(in, cache_bound);
        read(in, min_border_dist);
        son.clear();
        son = vector<int>(part);
        for (int i = 0; i < part; i++)
            read(in, son[i]);
        // printf("read_binary Node n=%d father=%d part=%d deep=%d\n",n,father,part,deep);
        if (INDEX_Optimization) {
            read(in, color);
            dist.read_binary(in);
            if (Save_Order_Matrix)
                order.read_binary(in);
            read(in, borders);
        }
        else {
            read(in, color);
            dist.read_binary(in);
            if (Save_Order_Matrix)
                order.read_binary(in);
            read(in, borders);
            read(in, border_in_father);
            read(in, border_in_son);
            read(in, border_id);
            read(in, border_id_innode);
            read(in, path_record);
            read(in, cache_dist);
            read(in, min_car_dist);
            read(in, border_son_id);
        }
    }
    void init(int n) {
        part = n;
        son = vector<int>(n);
        for (int i = 0; i < n; i++)
            son[i] = 0;
    }
    void clear() {
        part = n = father = deep = 0;
        son.clear();
        dist.clear();
        order.clear();
        G.clear();
        color.clear();
        borders.clear();
        border_in_father.clear();
        border_in_son.clear();
        border_id.clear();
        border_id_innode.clear();
        cache_dist.clear();
        cache_vertex_id = -1;
        path_record.clear();
        cache_dist.clear();
        border_son_id.clear();
        min_car_dist.clear();
    }
    void make_border_edge() // update the direct connected edge of border to dist(build_dist1)
    {
        int i, j;
        map<int, pair<int, int>>::iterator iter;
        for (iter = borders.begin(); iter != borders.end(); iter++) {
            i = iter->second.second;
            for (j = G.head[i]; j; j = G.next[j])
                if (color[i] != color[G.list[j]]) {
                    int id1, id2;
                    id1 = iter->second.first;
                    id2 = borders[G.id[G.list[j]]].first;
                    if (dist[id1][id2] > G.cost[j]) {
                        dist[id1][id2] = G.cost[j];
                        order[id1][id2] = -1;
                    }
                }
        }
    }
};
struct G_Tree
{
    int root;
    vector<int> id_in_node; // which node the vertex belongs to
    vector<vector<int>> car_in_node; // Record the vehicles in the vertex.
    vector<int> car_offset; // The offset of the vehicle in the vertex.

    struct Interface
    {
        // int cnt1,cnt2;
        G_Tree *tree; // point to the G_Tree(var)
        int tot, size, id_tot; // tot: up bound of vir subscript, size:size of vector,id_tot up bound of memory
        int node_size; // size of vector node
        Node *node;
        vector<int> id; // id of node in vector id
        // sub node information
        vector<int> father; // father node of vir node
        vector<int> border_in_father; // vir node border_in_father
        vector<int> G_id; // real id of the leaf which n=1 in node[0]
        void build_node0(int x) // init node[0] with x
        {
            // cnt2++;
            node[0].borders.clear();
            node[0].borders[G_id[x]] = make_pair(0, 0);
            node[0].border_id[0] = G_id[x];
            node[0].border_in_father[0] = border_in_father[x];
            node[0].father = father[x];
            node[0].deep = node[id[father[x]]].deep + 1;
            {
                int node_id = G_id[x];
                if (tree->car_in_node[node_id].size() > 0)
                    node[0].min_car_dist[0] = make_pair(0, node_id);
                else
                    node[0].min_car_dist[0] = make_pair(INF, -1);
            }
        }
        Node &operator[](int x) {
            // cnt1++;
            if (id[x] == 0 && Memory_Optimization)
                build_node0(x);
            return node[id[x]];
        }
        friend ostream &operator<<(ostream &out, Interface &I) {
            out << I.tot << ' ' << I.size << ' ' << I.id_tot << ' ' << I.node_size << ' ' << Save_Order_Matrix << ' ';
            for (int i = 0; i < I.node_size; i++)
                out << I.node[i] << ' ';
            out << I.id << ' ' << I.father << ' ' << I.border_in_father << ' ' << I.G_id << ' ';
            return out;
        }
        friend istream &operator>>(istream &in, Interface &I) {
            in >> I.tot >> I.size >> I.id_tot >> I.node_size >> Save_Order_Matrix;
            delete[] I.node;
            I.node = new Node[I.node_size];
            for (int i = 0; i < I.node_size; i++)
                in >> I.node[i];
            in >> I.id >> I.father >> I.border_in_father >> I.G_id;
            // Initialize node[0]
            I.node[0].border_id.clear();
            I.node[0].border_id.push_back(0);
            I.node[0].border_in_father.clear();
            I.node[0].border_in_father.push_back(0);
            I.node[0].min_car_dist.clear();
            I.node[0].min_car_dist.push_back(make_pair(0, 0));
            return in;
        }
        void save_binary(ostream &out) {
            save(out, tot);
            save(out, size);
            save(out, id_tot);
            save(out, node_size);
            save(out, Save_Order_Matrix);
            for (int i = 0; i < node_size; i++) {
                node[i].save_binary(out);
            }
            save(out, id);
            save(out, father);
            save(out, border_in_father);
            save(out, G_id);
        }
        void read_binary(istream &in) {
            read(in, tot);
            read(in, size);
            read(in, id_tot);
            read(in, node_size);
            read(in, Save_Order_Matrix);
            delete[] node;
            node = new Node[node_size];
            for (int i = 0; i < node_size; i++) {
                node[i].read_binary(in);
            }
            read(in, id);
            read(in, father);
            read(in, border_in_father);
            read(in, G_id);
            // initialize  node[0]
            node[0].border_id.clear();
            node[0].border_id.push_back(0);
            node[0].border_in_father.clear();
            node[0].border_in_father.push_back(0);
            node[0].min_car_dist.clear();
            node[0].min_car_dist.push_back(make_pair(0, 0));
        }
        ~Interface() { delete[] node; }
    } node;
    friend ostream &operator<<(ostream &out, G_Tree &T) {
        out << T.root << ' ' << T.id_in_node << ' ' << T.car_in_node << ' ' << T.car_offset << ' ';
        out << T.node << ' ';
        return out;
    }
    friend istream &operator>>(istream &in, G_Tree &T) {
        printf("load_begin\n");
        fflush(stdout);
        in >> T.root >> T.id_in_node >> T.car_in_node >> T.car_offset;
        in >> T.node;
        T.node.tree = &T;
        if (INDEX_Optimization) {
            printf("build_border_in_father_son\n");
            fflush(stdout);
            T.build_border_in_father_son();
        }
        return in;
    }
    void save_binary(ostream &out) {
        save(out, root);
        save(out, id_in_node);
        save(out, car_in_node);
        save(out, car_offset);
        node.save_binary(out);
    }
    void read_binary(istream &in) {
        read(in, root);
        read(in, id_in_node);
        read(in, car_in_node);
        read(in, car_offset);
        node.read_binary(in);
        node.tree = this;
        if (INDEX_Optimization) {
            printf("build_border_in_father_son\n");
            fflush(stdout);
            build_border_in_father_son();
        }
    }
    void add_border(int x, int id, int id2)
    // add a real id to the boundary set of x, in vir is id2, its corresponding id of border
    {
        map<int, pair<int, int>>::iterator iter;
        iter = node[x].borders.find(id);
        if (iter == node[x].borders.end()) {
            pair<int, int> second = make_pair((int)node[x].borders.size(), id2);
            node[x].borders[id] = second;
        }
    }
    void make_border(int x, const vector<int> &color) // calculate the set of x
    {
        for (int i = 0; i < node[x].G.n; i++) {
            int id = node[x].G.id[i];
            for (int j = node[x].G.head[i]; j; j = node[x].G.next[j])
                if (color[i] != color[node[x].G.list[j]]) {
                    add_border(x, id, i);
                    break;
                }
        }
    }
    void build(int x = 1, int f = 1,
               const Graph &g = G) // Build tree，current x,number of subgraphs f,current subgraph g
    {
        if (x == 1) // x root
        {
            node.tree = this;
            node.size = G.n * 2 + 2;
            printf("malloc!\n");
            fflush(stdout);
            node.node = new Node[node.size];
            printf("malloc end!\n");
            fflush(stdout);
            node.id = vector<int>(node.size);
            for (int i = 0; i < node.size; i++)
                node.id[i] = i;
            node.tot = 2;
            root = 1;
            node[x].deep = 1;
            node[1].G = g;
            node.node_size = node.size;
        }
        else // x not root
        {
            node[x].deep = node[node[x].father].deep + 1;
        }
        node[x].n = node[x].G.n;
        if (node[x].G.n < Naive_Split_Limit)
            node[x].init(node[x].n);
        else
            node[x].init(Partition_Part);
        if (node[x].n > 50)
            printf("x=%d deep=%d n=%d ", x, node[x].deep, node[x].G.n);
        if (node[x].n > f) {
            // id of sub node
            int top = node.tot;
            for (int i = 0; i < node[x].part; i++) {
                node[x].son[i] = top + i;
                node[top + i].father = x;
            }
            node.tot += node[x].part;
            // add border between two graph
            Graph **graph;
            graph = new Graph *[node[x].part];
            for (int i = 0; i < node[x].part; i++)
                graph[i] = &node[node[x].son[i]].G;
            node[x].color = node[x].G.Split(graph, node[x].part);
            delete[] graph;
            make_border(x, node[x].color);
            if (node[x].n > 50)
                printf("border=%zu\n", node[x].borders.size());
            // give the value of border to subgraph
            map<int, pair<int, int>>::iterator iter;
            for (iter = node[x].borders.begin(); iter != node[x].borders.end(); iter++) {
                // printf("(%d,%d,%d)",iter->first,iter->second.first,iter->second.second);
                node[x].color[iter->second.second] = -node[x].color[iter->second.second] - 1;
            }
            // cout<<endl;
            vector<int> tot(node[x].part, 0);
            for (int i = 0; i < node[x].n; i++) {
                if (node[x].color[i] < 0) {
                    node[x].color[i] = -node[x].color[i] - 1;
                    add_border(node[x].son[node[x].color[i]], node[x].G.id[i], tot[node[x].color[i]]);
                }
                tot[node[x].color[i]]++;
            }
            // iterate subnode
            for (int i = 0; i < node[x].part; i++)
                build(node[x].son[i]);
        }
        else if (node[x].n > 50)
            cout << endl;
        node[x].dist.init(node[x].borders.size());
        node[x].order.init(node[x].borders.size());
        node[x].order.cover(-INF);
        if (x == 1) // construct dist by root of x
        {
            for (int i = 1; i < min(1000, node.tot - 1); i++)
                if (node[i].n > 50) {
                    printf("x=%d deep=%d n=%d ", i, node[i].deep, node[i].G.n);
                }
            printf("begin_build_border_in_father_son\n");
            build_border_in_father_son();
            printf("begin_build_dist\n");
            build_dist1(root);
            printf("begin_build_dist2\n");
            build_dist2(root);
            // calculate the leaf node id of vertex in
            id_in_node.clear();
            for (int i = 0; i < node[root].G.n; i++)
                id_in_node.push_back(-1);
            for (int i = 1; i < node.tot; i++)
                if (node[i].G.n == 1)
                    id_in_node[node[i].G.id[0]] = i;
            {
                // 建立car_in_node;
                vector<int> empty_vector;
                empty_vector.clear();
                car_in_node.clear();
                for (int i = 0; i < G.n; i++)
                    car_in_node.push_back(empty_vector);
            }
            if (Memory_Optimization)
                Memory_Compress();
        }
    }
    void build_dist1(int x = 1) // from down to top merge dist in Graph
    {
        // Calculate dist in subgraph and set to x
        for (int i = 0; i < node[x].part; i++)
            if (node[x].son[i])
                build_dist1(node[x].son[i]);
        if (node[x].son[0]) // not leaf
        {
            // construct the edge between x subnode
            node[x].make_border_edge();
            // construct the real dist inside
            node[x].dist.floyd(node[x].order);
        }
        else
            ; // leaf
        // give the inside weight to father
        if (node[x].father) {
            int y = node[x].father, i, j;
            map<int, pair<int, int>>::iterator x_iter1, y_iter1;
            vector<int> id_in_fa(node[x].borders.size());
            // calculate the id of border in father border,  no exist:-1
            for (x_iter1 = node[x].borders.begin(); x_iter1 != node[x].borders.end(); x_iter1++) {
                y_iter1 = node[y].borders.find(x_iter1->first);
                if (y_iter1 == node[y].borders.end())
                    id_in_fa[x_iter1->second.first] = -1;
                else
                    id_in_fa[x_iter1->second.first] = y_iter1->second.first;
            }
            // tell the all connection weight to father
            for (i = 0; i < (int)node[x].borders.size(); i++)
                for (j = 0; j < (int)node[x].borders.size(); j++)
                    if (id_in_fa[i] != -1 && id_in_fa[j] != -1) {
                        int *p = &node[y].dist[id_in_fa[i]][id_in_fa[j]];
                        if ((*p) > node[x].dist[i][j]) {
                            (*p) = node[x].dist[i][j];
                            node[y].order[id_in_fa[i]][id_in_fa[j]] = -3;
                        }
                    }
        }
        return;
    }
    void build_dist2(int x = 1) // Calculate from top to down as in paper
    {
        if (x != root)
            node[x].dist.floyd(node[x].order);
        if (node[x].son[0]) {
            // calculate the border id in subgraph of current border
            vector<int> id_(node[x].borders.size());
            vector<int> color_(node[x].borders.size());
            map<int, pair<int, int>>::iterator iter1, iter2;
            for (iter1 = node[x].borders.begin(); iter1 != node[x].borders.end(); iter1++) {
                int c = node[x].color[iter1->second.second];
                color_[iter1->second.first] = c;
                int y = node[x].son[c];
                id_[iter1->second.first] = node[y].borders[iter1->first].first;
            }
            // Recalculate the subgraph weight
            for (int i = 0; i < (int)node[x].borders.size(); i++)
                for (int j = 0; j < (int)node[x].borders.size(); j++)
                    if (color_[i] == color_[j]) {
                        int y = node[x].son[color_[i]];
                        int *p = &node[y].dist[id_[i]][id_[j]];
                        if ((*p) > node[x].dist[i][j]) {
                            (*p) = node[x].dist[i][j];
                            node[y].order[id_[i]][id_[j]] = -2;
                        }
                    }
            // Recursive sub nodes
            for (int i = 0; i < node[x].part; i++)
                if (node[x].son[i])
                    build_dist2(node[x].son[i]);
        }
    }
    void build_border_in_father_son()
    // calculate the id of border in father/son
    {
        int i, j, x, y;
        // Construct cache
        for (x = 1; x < node.tot; x++) {
            // printf("x=%d node.id[x]=%d size=%d\n",x,node.id[x],node.node_size);fflush(stdout);
            // node[x].write();fflush(stdout);
            node[x].cache_dist.clear();
            node[x].min_car_dist.clear();
            node[x].cache_vertex_id = -1;
            for (int j = 0; j < node[x].borders.size(); j++) {
                node[x].cache_dist.push_back(0);
                node[x].min_car_dist.push_back(make_pair(INF, -1));
            }
            node[x].border_id.clear();
            node[x].border_id_innode.clear();
            for (i = 0; i < node[x].borders.size(); i++)
                node[x].border_id.push_back(0);
            for (i = 0; i < node[x].borders.size(); i++)
                node[x].border_id_innode.push_back(0);
            for (map<int, pair<int, int>>::iterator iter = node[x].borders.begin(); iter != node[x].borders.end();
                 iter++) {
                node[x].border_id[iter->second.first] = iter->first;
                node[x].border_id_innode[iter->second.first] = iter->second.second;
            }
            node[x].border_in_father.clear();
            node[x].border_in_son.clear();
            for (i = 0; i < node[x].borders.size(); i++) {
                node[x].border_in_father.push_back(-1);
                node[x].border_in_son.push_back(-1);
            }
            if (node[x].father) {
                y = node[x].father;
                map<int, pair<int, int>>::iterator iter;
                for (iter = node[x].borders.begin(); iter != node[x].borders.end(); iter++) {
                    map<int, pair<int, int>>::iterator iter2;
                    iter2 = node[y].borders.find(iter->first);
                    if (iter2 != node[y].borders.end())
                        node[x].border_in_father[iter->second.first] = iter2->second.first;
                }
            }
            if (node[x].son[0]) {
                map<int, pair<int, int>>::iterator iter;
                for (iter = node[x].borders.begin(); iter != node[x].borders.end(); iter++) {
                    y = node[x].son[node[x].color[iter->second.second]];
                    map<int, pair<int, int>>::iterator iter2;
                    iter2 = node[y].borders.find(iter->first);
                    if (iter2 != node[y].borders.end())
                        node[x].border_in_son[iter->second.first] = iter2->second.first;
                }
                node[x].border_son_id.clear();
                for (int i = 0; i < node[x].borders.size(); i++)
                    node[x].border_son_id.push_back(node[x].son[node[x].color[node[x].border_id_innode[i]]]);
            }
        }
    }
    void Memory_Compress() // Compress the tree in memory make the node with n=1 visual
    {
        printf("Begin Memory Compress! node_size=%d\n", node.node_size);
        int cnt = 0;
        for (int i = 0; i < node.tot; i++)
            if (i == 0 || node.node[i].n > 1)
                cnt++;
        Node *new_node = new Node[cnt + 1];
        node.node_size = cnt + 1;
        node.id_tot = 1;
        node.border_in_father = node.G_id = node.father = vector<int>(node.tot);
        for (int i = 0; i < node.tot; i++) {
            if (node.node[i].n > 1 || i == 0) {
                node.id[i] = (node.id_tot++);
                new_node[node.id[i]] = node.node[i];
            }
            else {
                // node.node[i].write();
                new_node[node.id[i] = 0] = node.node[i];
                node.G_id[i] = node.node[i].G.id[0];
                node.father[i] = node.node[i].father;
                node.border_in_father[i] = node.node[i].border_in_father[0];
            }
        }
        delete[] node.node;
        node.node = new_node;
        printf("End Memory Compress! node_size=%d\n", node.node_size);
        fflush(stdout);
    }
    void push_borders_up(int x, vector<int> &dist1, int type)
    // put the shortest distance from S to x in dist1, then calculate the distance from S to x.father and update dist
    // type = 0 is up, type = 1 is down
    {
        if (node[x].father == 0)
            return;
        int y = node[x].father;
        vector<int> dist2(node[y].borders.size(), INF);
        for (int i = 0; i < node[x].borders.size(); i++)
            if (node[x].border_in_father[i] != -1)
                dist2[node[x].border_in_father[i]] = dist1[i];
        Matrix &dist = node[y].dist;
        int *begin, *end;
        begin = new int[node[x].borders.size()];
        end = new int[node[y].borders.size()];
        int tot0 = 0, tot1 = 0;
        for (int i = 0; i < dist2.size(); i++) {
            if (dist2[i] < INF)
                begin[tot0++] = i;
            else if (node[y].border_in_father[i] != -1)
                end[tot1++] = i;
        }
        if (type == 0) {
            for (int i = 0; i < tot0; i++) {
                int i_ = begin[i];
                for (int j = 0; j < tot1; j++) {
                    int j_ = end[j];
                    if (dist2[j_] > dist2[i_] + dist[i_][j_])
                        dist2[j_] = dist2[i_] + dist[i_][j_];
                }
            }
        }
        else {
            for (int i = 0; i < tot0; i++) {
                int i_ = begin[i];
                for (int j = 0; j < tot1; j++) {
                    int j_ = end[j];
                    if (dist2[j_] > dist2[i_] + dist[j_][i_])
                        dist2[j_] = dist2[i_] + dist[j_][i_];
                }
            }
        }
        dist1 = dist2;
        delete[] begin;
        delete[] end;
    }
    void push_borders_up_cache(int x, int bound = INF)
    // cache S to vertex in x.cache_dist, and calculate the distance from S to x.father, and update x.father.cache
    {
        if (node[x].father == 0)
            return;
        int y = node[x].father;
        if (node[x].cache_vertex_id == node[y].cache_vertex_id && bound <= node[y].cache_bound)
            return;
        node[y].cache_vertex_id = node[x].cache_vertex_id;
        node[y].cache_bound = bound;
        vector<int> *dist1 = &node[x].cache_dist, *dist2 = &node[y].cache_dist;
        for (int i = 0; i < (*dist2).size(); i++)
            (*dist2)[i] = INF;
        for (int i = 0; i < node[x].borders.size(); i++)
            if (node[x].border_in_father[i] != -1) {
                if (node[x].cache_dist[i] < bound) // begin in the bound
                    (*dist2)[node[x].border_in_father[i]] = (*dist1)[i];
                else
                    (*dist2)[node[x].border_in_father[i]] = -1; // begin out of the bound
            }
        Matrix &dist = node[y].dist;
        int *begin, *end; // calculated id and un-calculated id
        begin = new int[node[x].borders.size()];
        end = new int[node[y].borders.size()];
        int tot0 = 0, tot1 = 0;
        for (int i = 0; i < (*dist2).size(); i++) {
            if ((*dist2)[i] == -1)
                (*dist2)[i] = INF;
            else if ((*dist2)[i] < INF)
                begin[tot0++] = i;
            else if (node[y].border_in_father[i] != -1)
                end[tot1++] = i;
        }
        for (int i = 0; i < tot0; i++) {
            int i_ = begin[i];
            for (int j = 0; j < tot1; j++) {
                if ((*dist2)[end[j]] > (*dist2)[i_] + dist[i_][end[j]])
                    (*dist2)[end[j]] = (*dist2)[i_] + dist[i_][end[j]];
            }
        }
        delete[] begin;
        delete[] end;
        node[y].min_border_dist = INF;
        for (int i = 0; i < node[y].cache_dist.size(); i++)
            if (node[y].border_in_father[i] != -1)
                node[y].min_border_dist = min(node[y].min_border_dist, node[y].cache_dist[i]);
    }
    void push_borders_down_cache(int x, int y, int bound = INF)
    // calculate distance from S to the son of x, update y.cache by S to borders in x.cache_dist
    {
        if (node[x].cache_vertex_id == node[y].cache_vertex_id && bound <= node[y].cache_bound)
            return;
        node[y].cache_vertex_id = node[x].cache_vertex_id;
        node[y].cache_bound = bound;
        vector<int> *dist1 = &node[x].cache_dist, *dist2 = &node[y].cache_dist;
        for (int i = 0; i < (*dist2).size(); i++)
            (*dist2)[i] = INF;
        for (int i = 0; i < node[x].borders.size(); i++)
            if (node[x].son[node[x].color[node[x].border_id_innode[i]]] == y) {
                if (node[x].cache_dist[i] < bound) // bound in ,begin
                    (*dist2)[node[x].border_in_son[i]] = (*dist1)[i];
                else
                    (*dist2)[node[x].border_in_son[i]] = -1; // begin out of bound
            }
        Matrix &dist = node[y].dist;
        int *begin, *end; // calculated id and un-calculated id
        begin = new int[node[y].borders.size()];
        end = new int[node[y].borders.size()];
        int tot0 = 0, tot1 = 0;
        for (int i = 0; i < (*dist2).size(); i++) {
            if ((*dist2)[i] == -1)
                (*dist2)[i] = INF;
            else if ((*dist2)[i] < INF)
                begin[tot0++] = i;
            else
                end[tot1++] = i;
        }
        for (int i = 0; i < tot0; i++) {
            int i_ = begin[i];
            for (int j = 0; j < tot1; j++) {
                if ((*dist2)[end[j]] > (*dist2)[i_] + dist[i_][end[j]])
                    (*dist2)[end[j]] = (*dist2)[i_] + dist[i_][end[j]];
            }
        }
        delete[] begin;
        delete[] end;
        node[y].min_border_dist = INF;
        for (int i = 0; i < node[y].cache_dist.size(); i++)
            if (node[y].border_in_father[i] != -1)
                node[y].min_border_dist = min(node[y].min_border_dist, node[y].cache_dist[i]);
    }
    void push_borders_brother_cache(int x, int y, int bound = INF)
    // from S to vertex x stored in x.cache_dist,calculate the distance from S to brother vertex y and update y.cache
    {
        int S = node[x].cache_vertex_id, LCA = node[x].father, i, j;
        if (node[y].cache_vertex_id == S && node[y].cache_bound >= bound)
            return;
        int p;
        node[y].cache_vertex_id = S;
        node[y].cache_bound = bound;
        vector<int> id_LCA[2], id_now[2];
        // id of Candidate border in LCA, id of the Candidate border inside border
        for (int t = 0; t < 2; t++) {
            if (t == 0)
                p = x;
            else
                p = y;
            for (i = j = 0; i < (int)node[p].borders.size(); i++)
                if (node[p].border_in_father[i] != -1)
                    if ((t == 1) || (t == 0 && node[p].cache_dist[i] < bound)) {
                        id_LCA[t].push_back(node[p].border_in_father[i]);
                        id_now[t].push_back(i);
                    }
        }
        for (int i = 0; i < node[y].cache_dist.size(); i++)
            node[y].cache_dist[i] = INF;
        for (int i = 0; i < id_LCA[0].size(); i++)
            for (int j = 0; j < id_LCA[1].size(); j++) {
                int k = node[x].cache_dist[id_now[0][i]] + node[LCA].dist[id_LCA[0][i]][id_LCA[1][j]];
                if (k < node[y].cache_dist[id_now[1][j]])
                    node[y].cache_dist[id_now[1][j]] = k;
            }
        Matrix &dist = node[y].dist;
        int *begin, *end; // calculated id and un-calculated id
        begin = new int[node[y].borders.size()];
        end = new int[node[y].borders.size()];
        int tot0 = 0, tot1 = 0;
        for (int i = 0; i < node[y].cache_dist.size(); i++) {
            if (node[y].cache_dist[i] < bound)
                begin[tot0++] = i;
            else if (node[y].cache_dist[i] == INF)
                end[tot1++] = i;
        }
        for (int i = 0; i < tot0; i++) {
            int i_ = begin[i];
            for (int j = 0; j < tot1; j++) {
                if (node[y].cache_dist[end[j]] > node[y].cache_dist[i_] + dist[i_][end[j]])
                    node[y].cache_dist[end[j]] = node[y].cache_dist[i_] + dist[i_][end[j]];
            }
        }
        delete[] begin;
        delete[] end;
        node[y].min_border_dist = INF;
        for (int i = 0; i < node[y].cache_dist.size(); i++)
            if (node[y].border_in_father[i] != -1)
                node[y].min_border_dist = min(node[y].min_border_dist, node[y].cache_dist[i]);
    }
    void push_borders_up_path(int x, vector<int> &dist1)
    // Cache S to border of x in dist1
    // calculate the distance from s to x.father, update dist1
    // record x.father to x.father.path_record(>=0 means node, < 0 means where come, -INF non pre)
    {
        if (node[x].father == 0)
            return;
        int y = node[x].father;
        vector<int> dist3(node[y].borders.size(), INF);
        vector<int> *order = &node[y].path_record;
        (*order).clear();
        for (int i = 0; i < node[y].borders.size(); i++)
            (*order).push_back(-INF);
        for (int i = 0; i < node[x].borders.size(); i++)
            if (node[x].border_in_father[i] != -1) {
                dist3[node[x].border_in_father[i]] = dist1[i];
                (*order)[node[x].border_in_father[i]] = -x;
            }
        // printf("dist3:");save_vector(dist3);
        Matrix &dist = node[y].dist;
        int *begin, *end; // calculated id and un-calculated id
        begin = new int[node[x].borders.size()];
        end = new int[node[y].borders.size()];
        int tot0 = 0, tot1 = 0;
        for (int i = 0; i < dist3.size(); i++) {
            if (dist3[i] < INF)
                begin[tot0++] = i;
            else if (node[y].border_in_father[i] != -1)
                end[tot1++] = i;
        }
        for (int i = 0; i < tot0; i++) {
            int i_ = begin[i];
            for (int j = 0; j < tot1; j++) {
                if (dist3[end[j]] > dist3[i_] + dist[i_][end[j]]) {
                    dist3[end[j]] = dist3[i_] + dist[i_][end[j]];
                    (*order)[end[j]] = i_;
                }
            }
        }
        dist1 = dist3;
        delete[] begin;
        delete[] end;
    }
    int find_LCA(int x, int y) // LCA of x and y
    {
        if (node[x].deep < node[y].deep)
            swap(x, y);
        while (node[x].deep > node[y].deep)
            x = node[x].father;
        while (x != y) {
            x = node[x].father;
            y = node[y].father;
        }
        return x;
    }
    int search(int S, int T) // Shortest distance from S to T
    {
        if (S == T)
            return 0;
        // Calculate LCA
        int i, j, k, p;
        int LCA, x = id_in_node[S], y = id_in_node[T];
        if (node[x].deep < node[y].deep)
            swap(x, y);
        while (node[x].deep > node[y].deep)
            x = node[x].father;
        while (x != y) {
            x = node[x].father;
            y = node[y].father;
        }
        LCA = x;
        vector<int> dist[2], dist_;
        dist[0].push_back(0);
        dist[1].push_back(0);
        x = id_in_node[S], y = id_in_node[T];
        // naive method as GTree
        for (int t = 0; t < 2; t++) {
            if (t == 0)
                p = x;
            else
                p = y;
            while (node[p].father != LCA) {
                push_borders_up(p, dist[t], t);
                p = node[p].father;
            }
            if (t == 0)
                x = p;
            else
                y = p;
        }
        vector<int> id[2]; // current border id in sequence of LCA border vector
        for (int t = 0; t < 2; t++) {
            if (t == 0)
                p = x;
            else
                p = y;
            for (i = j = 0; i < (int)dist[t].size(); i++)
                if (node[p].border_in_father[i] != -1) {
                    id[t].push_back(node[p].border_in_father[i]);
                    dist[t][j] = dist[t][i];
                    j++;
                }
            while (dist[t].size() > id[t].size()) {
                dist[t].pop_back();
            }
        }
        // matched
        int MIN = INF;
        for (i = 0; i < dist[0].size(); i++) {
            int i_ = id[0][i];
            for (j = 0; j < dist[1].size(); j++) {
                k = dist[0][i] + dist[1][j] + node[LCA].dist[i_][id[1][j]];
                if (k < MIN)
                    MIN = k;
            }
        }
        return MIN;
    }
    int search_cache(int S, int T, int bound = INF)
    // Search Shortest distance from S to T, and store them in cache, skip weight >=bound, if no return INF
    {
        // Naive calculate and cache
        if (S == T)
            return 0;
        // LCA
        int i, j, k, p;
        int x = id_in_node[S], y = id_in_node[T];
        int LCA = find_LCA(x, y);

        // Get LCA path of two nodes
        vector<int> node_path[2]; // the id before x/y arrived LCA
        for (int t = 0; t < 2; t++) {
            if (t == 0)
                p = x;
            else
                p = y;
            while (node[p].father != LCA) {
                node_path[t].push_back(p);
                p = node[p].father;
            }
            node_path[t].push_back(p);
            if (t == 0)
                x = p;
            else
                y = p;
        }

        // push from leaf to the next level of LCA
        node[id_in_node[S]].cache_vertex_id = S;
        node[id_in_node[S]].cache_bound = bound;
        node[id_in_node[S]].min_border_dist = 0;
        node[id_in_node[S]].cache_dist[0] = 0;
        for (i = 0; i + 1 < node_path[0].size(); i++) {
            if (node[node_path[0][i]].min_border_dist >= bound)
                return INF;
            push_borders_up_cache(node_path[0][i]);
        }

        // calculate T distance in the layer under LCA and cache
        if (node[x].min_border_dist >= bound)
            return INF;
        push_borders_brother_cache(x, y);
        // push T under LCA to bottom node T
        for (int i = node_path[1].size() - 1; i > 0; i--) {
            if (node[node_path[1][i]].min_border_dist >= bound)
                return INF;
            push_borders_down_cache(node_path[1][i], node_path[1][i - 1]);
        }

        // Final Result
        return node[id_in_node[T]].cache_dist[0];
    }
    void add_car(int node_id, int car_id) // add a car_id car which belongs to  node_id to car set.
    {
        car_in_node[node_id].push_back(car_id);
        if (car_in_node[node_id].size() == 1) {
            // add node_id and update border LNAV
            int S = id_in_node[node_id]; // get the id of leaf node which it in
            node[S].min_car_dist[0] = make_pair(0, node_id); // make pair for LNAV
            for (int p = S; push_borders_up_add_min_car_dist(p, node_id); p = node[p].father)
                ; // add it to some node
        }
    }
    void del_car(int node_id, int car_id) // remove a car_id car which belongs to node_id from car set
    {
        int i;
        for (i = 0; i < car_in_node[node_id].size(); i++)
            if (car_in_node[node_id][i] == car_id)
                break;
        if (i == car_in_node[node_id].size())
            printf("Error: del_car find none car!");
        while (i + 1 < car_in_node[node_id].size()) {
            swap(car_in_node[node_id][i], car_in_node[node_id][i + 1]);
            i++;

        } // drop all in car_in_node

        car_in_node[node_id].pop_back();
        if (car_in_node[node_id].size() == 0) {
            // delete from the LNAV
            int S = id_in_node[node_id];
            node[S].min_car_dist[0] = make_pair(INF, -1); // INF,-1 Set to INF, -1
            for (int p = S; push_borders_up_del_min_car_dist(p, node_id); p = node[p].father)
                ;
        }
        //
    }
    void change_car_offset(int car_id, int dist) // update the offset of car_id to the vertex
    {
        while (car_offset.size() <= car_id)
            car_offset.push_back(0);
        car_offset[car_id] = dist;
    }
    int get_car_offset(int car_id) // get the offset of car_id
    {
        while (car_offset.size() <= car_id)
            car_offset.push_back(0);
        return car_offset[car_id];
    }

    int begin[10000], end[10000]; // push_borders_up_add/del_min_car_dist vector of calculated vertex and un-calculated
    int buffer[10000];
    bool push_borders_up_add_min_car_dist(int x, int start_id)
    // update x.father by min_car_dist in x, update which node_id=start_id, if no exist return false, of return true.
    {
        // leaf node and the position of the car
        int re = false;
        if (node[x].father == 0)
            return re;
        int y = node[x].father;
        vector<pair<int, int>> &dist1 = node[x].min_car_dist, &dist2 = node[y].min_car_dist;
        int nodex_borders_size = node[x].borders.size();
        vector<int> &nodex_border_in_father = node[x].border_in_father;
        for (int i = 0; i < nodex_borders_size; i++) {
            if (dist1[i].second == start_id) // border with new comer
            {
                re = true; // true
                if (nodex_border_in_father[i] != -1) // if border belongs to its father, update
                {
                    if (dist2[nodex_border_in_father[i]].first > dist1[i].first) // first distance，second car_id
                        dist2[nodex_border_in_father[i]] = dist1[i]; // update the pair
                }
            }
        }

        if (y != root) // do till arrive root
        {
            int tot0 = 0, tot1 = 0; // dp，0begin size，tot1 end size
            for (int i = 0; i < dist2.size(); i++) {
                if (dist2[i].second == start_id) { // if StartID contained，means will influence
                    begin[tot0++] = i; // second  leaf node ID
                }
                else
                    end[tot1++] = i;
            }
            int *dist;
            int n_ = node[y].dist.n;


            int *nodey_dist_a = node[y].dist.a;
            for (int i = 0; i < tot0; i++) {
                dist = begin[i] * n_ + nodey_dist_a;
                for (int j = 0; j < n_; j++) {
                    buffer[j] = dist2[begin[i]].first + dist[j];
                }
                for (int j = 0; j < n_; j++) {
                    if (dist2[j].first > buffer[j]) {
                        dist2[j].first = buffer[j];
                        dist2[j].second = dist2[begin[i]].second;
                    }
                }
            }
        }
        return re;
    }
    bool push_borders_up_del_min_car_dist(int x, int start_id) // del  min_car_dist which node_id=start_id from, then
                                                               // update. no exist return false, otherwise return true
    {
        if (node[x].father == 0)
            return false; // till top
        bool re = false;
        int y = node[x].father;
        vector<pair<int, int>> &dist1 = node[x].min_car_dist, &dist2 = node[y].min_car_dist;
        int tot0 = 0, tot1 = 0;
        int nodex_borders_size = node[x].borders.size();
        vector<int> &nodex_border_in_father = node[x].border_in_father;
        if (y == root) // del node_id in x.father
        {
            for (int i = 0; i < nodex_borders_size; i++)
                if (nodex_border_in_father[i] != -1) {
                    if (dist2[nodex_border_in_father[i]].second == start_id) {
                        dist2[nodex_border_in_father[i]] = make_pair(INF, -1);
                        re = true;
                    }
                }
            for (int i = 0; i < nodex_borders_size; i++) {
                if (nodex_border_in_father[i] != -1) {
                    if (dist2[nodex_border_in_father[i]].first > dist1[i].first)
                        dist2[nodex_border_in_father[i]] = dist1[i];
                }
            }
        }
        else {
            int SIZE = node[y].borders.size();
            vector<int> &nodey_border_son_id = node[y].border_son_id;
            vector<int> &nodey_border_in_son = node[y].border_in_son;
            for (int i = 0; i < SIZE; i++) {
                if (dist2[i].second == start_id) {
                    dist2[i] = node[nodey_border_son_id[i]].min_car_dist[nodey_border_in_son[i]];
                    end[tot1++] = i; // color END needs DP
                    re = true; // yes it is found
                }
                else
                    begin[tot0++] = i;
            }
        }


        if (re) // If  vertex is be delete（not root） in this node, recalculate
        {
            if (y != root) // root is connected all
            {
                int *dist;
                int n_ = node[y].dist.n;
                int *nodey_dist_a = node[y].dist.a;
                for (int j = 0; j < tot1; j++) {
                    dist = end[j] * n_ + nodey_dist_a;
                    for (int i = 0; i < tot0; i++) {
                        if (dist2[end[j]].first > dist2[begin[i]].first + dist[begin[i]]) {
                            dist2[end[j]].first = dist2[begin[i]].first + dist[begin[i]];
                            dist2[end[j]].second = dist2[begin[i]].second;
                        }
                    }
                }
                // DP
                // first fot start, then calculte end

                dist = node[y].dist.a;
                while (tot1) {
                    for (int i = 0; i < tot1 - 1; i++)
                        if (dist2[end[i]].first < dist2[end[i + 1]].first)
                            swap(end[i], end[i + 1]); // sort from small to big
                    tot1--;
                    // use end[tot1]to calculate dist2
                    int i_ = end[tot1];
                    for (int j = 0; j < tot1; j++) {
                        if (dist2[end[j]].first > dist2[i_].first + dist[i_ * n_ + end[j]]) {
                            dist2[end[j]].first = dist2[i_].first + dist[i_ * n_ + end[j]];
                            dist2[end[j]].second = dist2[i_].second;
                        }
                    }
                }
            }
        }
        return re;
    }
    int push_borders_up_cache_KNN_min_dist_car(int x)
    // calculate S to x.father and cache
    {
        // DP
        if (node[x].father == 0)
            return INF;
        int re = INF + 1;

        int y = node[x].father; // 向上push到父图
        node[y].cache_vertex_id = node[x].cache_vertex_id; // int cache_id,向上一层子图的cache所保存的起点编号更新
        node[y].cache_bound = -1;
        vector<int> *dist1 = &node[x].cache_dist, *dist2 = &node[y].cache_dist;

        for (int i = 0; i < (*dist2).size(); i++)
            (*dist2)[i] = INF; // init
        for (int i = 0; i < node[x].borders.size(); i++) {
            if (node[x].border_in_father[i] != -1)
                (*dist2)[node[x].border_in_father[i]] = (*dist1)[i];
        } // if border same，direct copy

        Matrix &dist = node[y].dist;
        int tot0 = 0, tot1 = 0;

        for (int i = 0; i < (*dist2).size(); i++) {
            if ((*dist2)[i] < INF)
                begin[tot0++] = i; // calculated
            else
                end[tot1++] = i; // un-calculated
        }

        for (int i = 0; i < tot0; i++) {
            int i_ = begin[i];
            for (int j = 0; j < tot1; j++) {
                if ((*dist2)[end[j]] > (*dist2)[i_] + dist[i_][end[j]]) {
                    (*dist2)[end[j]] = (*dist2)[i_] + dist[i_][end[j]];
                }
            }
        }

        if (y == root) {
            re = INF + 1;
        }
        else {
            for (int i = 0; i < node[y].borders.size(); i++)
                if (node[y].border_in_father[i] != -1) {
                    re = min(re, (*dist2)[i]);
                }
        }
        return re;
    }

    vector<int> KNN_min_dist_car(int S, int K) // Most import one, calculate the KNN of S, return the set of result
    {
        // optimization as in paper, only need to calculate some layer, only K is farther than current or non car in
        // current layer.
        int Now_Cache_Node_Number = id_in_node[S], Now_Cache_Dist = 0;
        priority_queue<pair<int, pair<int, int>>> q; // save K minimal <-dist,<node_id,border_id>>
        { // 构建S的cache
            node[id_in_node[S]].cache_vertex_id = S; // cache cache_id
            node[id_in_node[S]].cache_bound = INF;
            node[id_in_node[S]].min_border_dist = 0; // the distance from S to nearest border, prepare prune
            node[id_in_node[S]].cache_dist[0] = 0; // the distance from S to all borders
            /*for(int p=id_in_node[S];p!=root;p=node[p].father)
                push_borders_up_cache_KNN_min_dist_car(p);*/
        }
        // PQ
        for (int i = 0; i < node[Now_Cache_Node_Number].borders.size(); i++) {
            q.push(make_pair(
                -(node[Now_Cache_Node_Number].cache_dist[i] + node[Now_Cache_Node_Number].min_car_dist[i].first),
                make_pair(Now_Cache_Node_Number, i)));
        }

        vector<int> ans, ans2; // car_id, node_id(car_id in )
        if (Distance_Offset == false) // no offset
        {
            while (K) {
                int Dist = q.top().first;
                int node_id = q.top().second.first;
                int border_id = q.top().second.second;
                int real_Dist = -(node[node_id].cache_dist[border_id] + node[node_id].min_car_dist[border_id].first);
                // real distance
                if (Dist != real_Dist) { // no changed, no computing
                    q.pop();
                    q.push(make_pair(real_Dist, make_pair(node_id, border_id))); // put the right one
                    continue;
                }

                if (-Dist > Now_Cache_Dist &&
                    Now_Cache_Node_Number != root) // Now_Catch_Dist ，distance from s to up graph
                                                   // if the distance is bigger than Dist, no need to explore
                {
                    Now_Cache_Dist = push_borders_up_cache_KNN_min_dist_car(Now_Cache_Node_Number);
                    Now_Cache_Node_Number = node[Now_Cache_Node_Number].father;
                    for (int i = 0; i < node[Now_Cache_Node_Number].borders.size(); i++) {
                        q.push(make_pair(-(node[Now_Cache_Node_Number].cache_dist[i] +
                                           node[Now_Cache_Node_Number].min_car_dist[i].first),
                                         make_pair(Now_Cache_Node_Number, i)));
                    }
                    continue;
                }

                int real_node_id = node[node_id].min_car_dist[border_id].second;
                if (real_node_id == -1)
                    break; // if not larger than k, get all
                int car_id = car_in_node[real_node_id][0];

                q.pop();
                ans.push_back(car_id);
                ans2.push_back(real_node_id);
                del_car(real_node_id, car_id);

                q.push(make_pair(-(node[node_id].cache_dist[border_id] + node[node_id].min_car_dist[border_id].first),
                                 make_pair(node_id, border_id)));
                // put s again, for changed
                K--;
            }
            for (int i = 0; i < ans.size(); i++)
                add_car(ans2[i], ans[i]); // roll back delete
        }
        else // stop
        {
            priority_queue<int> KNN_Dist;
            vector<int> ans3;
            while (KNN_Dist.size() < K || KNN_Dist.top() > -q.top().first)
            // the k smaller, as the smallest will bigger and bigger

            {
                int Dist = q.top().first;
                int node_id = q.top().second.first;
                int border_id = q.top().second.second;
                int real_Dist = -(node[node_id].cache_dist[border_id] + node[node_id].min_car_dist[border_id].first);
                if (Dist != real_Dist) {
                    q.pop();
                    q.push(make_pair(real_Dist, make_pair(node_id, border_id)));
                    continue;
                }
                if (-Dist > Now_Cache_Dist && Now_Cache_Node_Number != root) {
                    Now_Cache_Dist = push_borders_up_cache_KNN_min_dist_car(Now_Cache_Node_Number);
                    Now_Cache_Node_Number = node[Now_Cache_Node_Number].father;
                    for (int i = 0; i < node[Now_Cache_Node_Number].borders.size(); i++)
                        q.push(make_pair(-(node[Now_Cache_Node_Number].cache_dist[i] +
                                           node[Now_Cache_Node_Number].min_car_dist[i].first),
                                         make_pair(Now_Cache_Node_Number, i)));
                    continue;
                }
                int real_node_id = node[node_id].min_car_dist[border_id].second;

                if (real_node_id == -1)
                    break;
                int car_id = car_in_node[real_node_id][0];
                q.pop();
                del_car(real_node_id, car_id);
                q.push(make_pair(-(node[node_id].cache_dist[border_id] + node[node_id].min_car_dist[border_id].first),
                                 make_pair(node_id, border_id)));
                int car_dist = get_car_offset(car_id) - real_Dist;
                if (KNN_Dist.size() < K)
                    KNN_Dist.push(car_dist);
                else if (KNN_Dist.top() > car_dist) {
                    KNN_Dist.pop();
                    KNN_Dist.push(car_dist);
                }
                ans.push_back(car_id);
                ans2.push_back(real_node_id);
                ans3.push_back(car_dist);
            }
            for (int i = 0; i < ans.size(); i++)
                add_car(ans2[i], ans[i]);
            int j = 0;
            for (int i = 0; i < ans.size(); i++)
                if (ans3[i] <= KNN_Dist.top())
                    ans[j++] = ans[i];
            while (ans.size() > K)
                ans.pop_back();
        }
        return ans;
    }
} tree;
void init() { srand(747929791); }
void read() {
    printf("begin read\n");
    FILE *in = nullptr;
    in = fopen(Edge_File, "r");
    cout << "correct1" << endl;
    fscanf(in, "%d %d\n", &G.n, &G.m);
    cout << G.n << " " << G.m << endl;
    cout << "correct2" << endl;
    G.init(G.n, G.m);
    for (int i = 0; i < G.n; i++)
        G.id[i] = i;
    cout << "correct3" << endl;
    int i, j, k, l;
    for (i = 0; i < G.m; i++) // Load edge
    {
        // int temp;
        fscanf(in, "%d %d %d\n", &j, &k, &l);
        if (NWFIX == 1) {
            if (RevE == false)
                G.add_D(j, k, l); // Undirected
            else
                G.add(j, k, l); // Directed
        }
        else {
            if (RevE == false)
                G.add_D(j - 1, k - 1, l); // Undirected
            else
                G.add(j - 1, k - 1, l); // Directed
        }
    }
    cout << "correct4" << endl;
    fclose(in);
}
void save(string file_name) {
    printf("begin save\n");
    ofstream out(file_name.c_str());
    out << G.n << ' ';
    out << tree;
    out.close();
    printf("save_over\n");
}
void save_binary(string file_name) {
    printf("begin save_binary\n");
    ofstream out(file_name.c_str());
    save(out, G.n);
    tree.save_binary(out);
    out.close();
    printf("save_binary_over\n");
}
void load(string file_name) {
    ifstream in(file_name.c_str());
    in >> G.n;
    in >> tree;
}
void load_binary(string file_name) {
    printf("begin load_binary\n");
    ifstream in(file_name.c_str());
    read(in, G.n);
    printf("load_binary G.n=%d\n", G.n);
    tree.read_binary(in);
    in.close();
    printf("load_binary_over\n");
}

bool arg_has_str(char ar[], string query_str) { // some special data NW,US from 0
    string strtemp = string(ar);
    if (strtemp.find(query_str) == std::string::npos) {
        return false;
    }
    return true;
}

void knn_test(double car_percent, double change_percent, int query_number, int K, int query_time) {

    vector<int> ans;
    vector<int> addr;
    vector<int> query_pos;
    addr.clear();

    int car_num = G.n * car_percent;
    int change_num = int(car_num * change_percent);
    int sun_change_num = 0;
    printf("KNN Test: car_percent=%lf change_percent=%lf K=%d\n", car_percent, change_percent, K);
    printf("Total Vertice Number is %d\n", G.n);
    printf("Total Car Number is %d\n", car_num);
    printf("Change number of vehicles per query: %d\n", change_num);
    printf("Query number in between two updates: %d\n", query_number);
    printf("Every test repeat %d times.\n", query_time);

    for (int i = 0; i < car_num; i++) { // generate position of car
        int S = rand() % G.n;
        addr.push_back(S);
    }

    for (int i = 0; i < query_time; i++) { // generate query position
        int te_query = rand() % G.n;
        query_pos.push_back(te_query);
    }

    for (int i = 0; i < car_num; i++) { // add cars to road network
        tree.add_car(addr[i], i);
    }

    TIME_TICK_START(); // one time knn
    for (int i = 0; i < query_time; i++) {
        ans = tree.KNN_min_dist_car(query_pos[i], K);
    }
    TIME_TICK_END();

    TIME_TICK_PRINT("ONE time knn", query_time);


    long long total_up_time = 0;
    long long total_query_time = 0;
    long long sin_up_time = 0;
    long long sin_que_time = 0;

    for (int t = 0; t < query_time; t++) {

        TIME_TICK_START();
        for (int i = 0; i < change_num; i++) { // Change the position of vehicle
            int change_car = rand() % car_num;
            int old_pos = addr[change_car];
            int new_pos = (addr[change_car] + 1) % G.n;
            addr[change_car] = new_pos;
            tree.del_car(old_pos, change_car);
            tree.add_car(addr[change_car], change_car);
        }
        TIME_TICK_END();

        sin_up_time = te - ts;
        total_up_time += sin_up_time;

        TIME_TICK_START();
        for (int i = 0; i < query_number; i++) {
            ans = tree.KNN_min_dist_car(query_pos[i], K);
        }
        TIME_TICK_END();
        sin_que_time = te - ts;
        total_query_time += sin_que_time;
    }

    long long ave_amor_time = (total_query_time + total_up_time) / (query_number * query_time);
    long long ave_query_time = total_query_time / (query_time * query_number);
    printf("Query Time \t %lld (us)\t Amortized Time \t %lld (us)", ave_query_time, ave_amor_time);
    fflush(stdout);

    // TIME_TICK_PRINT("Mutiple Time knn",query_time);
}

int main(int argc, char *argv[]) {
    string loadfile;
    if (argc < 6) {
        printf("Load VTree and run KNN test use './vtree_knn_demo  VTree_file $K(int) $car_percent(int) "
               "$change_percent(int) $query_per_update'\n");
        return 0;
    }
    loadfile = argv[1];
    int K = std::atoi(argv[2]);
    float car_per = std::atof(argv[3]);
    float change_per = std::atof(argv[4]);
    float query_number = std::atof(argv[5]);
    fflush(stdout);
    TIME_TICK_START();
    init();
    load_binary(loadfile);
    cout << "***************************************" << endl;
    cout << "Finish Load File " << loadfile << "vertice Number:\t" << G.n << endl;
    cout << "***************************************" << endl << endl;
    fflush(stdout);
    TIME_TICK_END();

    knn_test(car_per, change_per, query_number, K, 10000);
    	printf("Program End.\n");fflush(stdout);
        return 0;
}