//
//  main.cpp
//  TreeSortSolver
//
//  Created by Daniel Graf on 27.03.15.
//  Copyright (c) 2015 Daniel Graf. All rights reserved.
//

#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <string>
#include <map>
#include <set>
#include <algorithm>
#include <unordered_set>
#include <unordered_map>
#include <queue>

using namespace std;
typedef long long int in;
typedef vector<in> VI;
typedef vector<VI> VVI;
#define INF 1000000000
#define DEB(x) if(debug) {x}
bool debug = false;

char encode(in x) {
    x++;
    if(x==0) return '_';
    if(x<=9) return '0'+x;
    if(x<=9+26) return 'A'+x-10;
    if(x<=9+26+26) return 'a'+x-(10+26);
    return '?';
}

in decode(char c) {
    if('0'<=c && c<='9') return c-'0'-1;
    if('A'<=c && c<='Z') return c-'A'+10-1;
    if('a'<=c && c<='z') return c-'a'+10+26-1;
    return -1;
}

struct Cycle {
    in l, r;
    VI C;
    unordered_map<in,in> Inv;
    Cycle(VI c) {
        if(c.empty()) return;
        C = c;
        l = *std::min_element(c.begin(), c.end());;
        r = *std::max_element(c.begin(), c.end());;
        for(in i = 0; i < c.size(); i++) {
            Inv[c[i]] = i;
        }
    }
    bool has(in x) {
        return !(Inv.find(x) == Inv.end());
    }
    in succ(in x) {
        if(!has(x)) return -1;
        return C[(Inv[x]+1)%C.size()];
    }
    in pred(in x) {
        if(!has(x)) return -1;
        return C[(Inv[x]-1+C.size())%C.size()];
    }
    void print() {
        cout << "(";
        for(in j=0; j<C.size(); j++) {
            cout << encode(C[j]);
            if(j<C.size()-1) cout << " ";
        }
        cout << ") ";
    }
    in d() {
        in sum = 0;
        for (in i=0; i<C.size(); i++) {
            sum += abs(succ(C[i]) - C[i]);
        }
        return sum;
    }
};
struct Permutation {
    // i -> Pi(i) is stored in the entries of Pi.
    VI Pi;
    // List of all cycles in order.
    vector<Cycle> C;
    // List of all non-trivial cycles in order.
    vector<Cycle> Cbar;
    
    void GetCycles() {
        // Checklist of whether we have seen an entry before.
        VI S(Pi.size());
        for(in i=0; i<Pi.size(); i++) {
            if(S[i]) continue;
            VI c;
            in j = i;
            while (!S[j]) {
                c.push_back(j);
                S[j] = true;
                j = Pi[j];
            }
            C.push_back(Cycle(c));
            if(c.size()>1) {
                Cbar.push_back(Cycle(c));
            }
        }
    }
    
    void print() {
        cout << "Pi: ";
        for(Cycle c : C) {
            c.print();
        }
        cout << endl;
    }
    
    in d() {
        in sum = 0;
        for (Cycle c: C) {
            sum += c.d();
        }
        return sum;
    }
    
    Permutation(VI P) {
        Pi = P;
        GetCycles();
    }
};

struct Point {
    in x,y;
    void print() {
        cout << "(" << x << "," << y << ")" << endl;
    }
};

// Graph represents an undirected, unweighted simple graph.
// This will be used to store the connectivity of the warehouse
struct Graph {
    // N = |V|
    in N;
    // The edges of each graph djacency List
    VVI E;
    // The neighborhoods of each vertex as hash table for quick incidence checks
    vector<unordered_set<in>> NB;
    // Directed variant:
    // Parent node of each vertex.
    VI parent;
    // Children of each vertex.
    VVI children;
    // Verticies are implicitely numbered from 0 to |E|-1.
    
    void AddEdge(in from, in to) {
        E[from].push_back(to);
        E[to].push_back(from);
        NB[from].insert(to);
        NB[to].insert(from);
        children[from].push_back(to);
        parent[to] = from;
    }
    
    Graph(in N) {
        this->N = N;
        E.resize(N);
        NB.resize(N);
        parent.resize(N);
        children.resize(N);
    }
    
    bool has(in from, in to) {
        return (NB[from].find(to) != NB[from].end());
    }
    
    bool is_path() {
        for(in n=1; n<N-1; n++) {
            if(E[n][0] != n-1 || E[n][1] != n+1)
                return false;
        }
        return true;
    }
    
    // Helper information for computing the shortest paths between two nodes.
    VVI PV; // Parent tree for the BFS from each vertex. From w go to P[v][w] to get to v.
    VVI DV; // DV[v][w] = distance to v from w.
    
    void BFS() {
        PV = VVI(N,VI(N,-1));
        DV = VVI(N,VI(N,0));
        for(in v=0; v<N; v++) {
            queue<in> Q;
            Q.push(v);
            PV[v][v] = v;
            DV[v][v] = 0;
            while(!Q.empty()) {
                in x = Q.front(); Q.pop();
                for(in e=0; e<E[x].size(); e++) {
                    in y = E[x][e];
                    if(PV[v][y]==-1) {
                        PV[v][y] = x;
                        DV[v][y] = DV[v][x] + 1;
                        Q.push(y);
                    }
                }
            }
        }
    }
    
    // Helper information for cycle coverage and hitting.
    VVI hit; // Hit[i][v] is true, if the ith non-trivial cycle hits vertex v.
    VVI cov; // Cov[i][v] is true, if the ith non-trivial cycle hits vertex v.
    
    void HitCovPrecompute(const Permutation &Pi) {
        hit = VVI(Pi.Cbar.size(), VI(N,false));
        cov = VVI(Pi.Cbar.size(), VI(N,false));
        in i = 0;
        for(Cycle C : Pi.Cbar) {
            in r = C.C[0];
            for(in v : C.C) {
                hit[i][v] = true;
                in w = v;
                do {
                    cov[i][w] = true;
                    w = PV[r][w];
                } while(w!=r);
            }
            i++;
        }
    }
    
    // Helper information for distances between vertex v and cycle coverage cov(C).
    VVI PC; // Parent tree for the BFS from each cycle. From w go to P[C][w] to get to cov(C).
    VVI DC; // DC[C][w] = distance from cov(C) to w in down-steps.
    VVI AC; // AC[C][w] = anchor point for reaching w from cov(C) using only d[C][w] down-steps.
    
    void CyclceBFS(const Permutation &Pi) {
        PC = VVI(Pi.Cbar.size(),VI(N,-1));
        AC = VVI(Pi.Cbar.size(),VI(N,0));
        DC = VVI(Pi.Cbar.size(),VI(N,0));
        in c = 0;
        for(Cycle C : Pi.Cbar) {
            queue<in> Q;
            // Fill the queue with all vertices in cov(C)
            for(in v=0; v<N; v++) {
                if(!cov[c][v]) continue;
                Q.push(v);
                PC[c][v] = v;
                AC[c][v] = v;
                DC[c][v] = 0;
                
            }
            while(!Q.empty()) {
                in x = Q.front(); Q.pop();
                for(in e=0; e<E[x].size(); e++) {
                    in y = E[x][e];
                    if(PC[c][y]==-1) {
                        PC[c][y] = x;
                        // Add 1 to the distance if it is a down-step.
                        bool down_step = DV[0][x] < DV[0][y];
                        DC[c][y] = DC[c][x] + down_step;
                        // Propagate the anchor point as soon as the number of down-steps is non-zero.
                        // Before that (on the up part of the path), each point is its own anchor.
                        if(DC[c][y]) AC[c][y] = AC[c][x];
                        else AC[c][y] = y;
                        Q.push(y);
                    }
                }
            }
            c++;
        }
    }
    
    void Preprocess(const Permutation &Pi) {
        BFS();
        HitCovPrecompute(Pi);
        CyclceBFS(Pi);
    }
    
    in d(const Permutation &P) {
        in sum = 0;
        for(in i = 0; i < N; i++) {
            sum += DV[P.Pi[i]][i];
        }
        return sum;
    }
};


// Witness edge that keeps as additional information:
// If you want to work on cycle (to) while working at cycle (from),
// then use vertex (anchor) to branch off and start sorting (to) at (hit).
// There will be (cost) down-steps between (anchor) and (hit)
struct WEdge {
    in from, to, cost, anchor, hit;
    
//    WEdge() {
//        cost = INF;
//    }
    void print() {
        cout << "(" << from << "->" << to << "[";
        if(cost == INF) cout << "%";
        else cout << cost;
        cout << ",(" << encode(anchor) <<"->" << encode(hit) << ")]) ";
    }
    bool is_loop() {
        return from==to;
    }
};

struct WEdgeCycle {
    vector<WEdge> C;
    unordered_set<in> S;
    
    WEdgeCycle(vector<WEdge> c) {
        C = c;
        for(WEdge e:c) S.insert(e.from);
    }
    
    bool has(in x) {
//        cout << "has " << x << endl;
        return (S.find(x) != S.end());
    }
    
    void print() {
        for(WEdge e : C) e.print();
    }
};
struct CycleWitnessTree {
    in N;
    vector<WEdge> P;
    
    CycleWitnessTree(in N) {
        this->N = N;
        P.resize(N);
    }
    
    in cost() {
        in sum = 0;
        for (WEdge e : P) {
            if(e.from != e.to) sum += e.cost;
        }
        return sum;
    }
    
    void print() {
        for(WEdge e : P) e.print();
    }
    
    WEdgeCycle find_cycle() {
        // Try walking back from every vertex until you hit the root or end in a loop.
        
        for(in i = 0; i<N; i++) {
            vector<WEdge> path;
            unordered_set<in> seen;
            path.push_back(P[i]);
            seen.insert(P[i].to);
            // while not a loop and while not ending at the root
            while(seen.find(path.back().from) == seen.end() &&
                  !path.back().is_loop()) {
                seen.insert(path.back().from);
                path.push_back(P[path.back().from]);
            }
            if(!path.back().is_loop()) {
                // Erase the potential beginning of the path that does not belong to the cycle:
                while(path.front().to != path.back().from)
                    path.erase(path.begin());
                return path;
            }
        }
        return WEdgeCycle({});
    }
    
    // List of children for each vertex. Used in the DFS traversal when generating the solution.
    vector<vector<WEdge>> children;
    
    void prepare_for_traversal() {
        children.resize(N);
        for(WEdge e : P) {
            if(e.from != e.to) children[e.from].push_back(e);
        }
    }
};

struct CycleWitnessGraph {
    // N = maximal vertex number
    in N;
    // Full adjacency matrix
    vector<vector<WEdge>> E;
    //
    VVI A;

    CycleWitnessGraph() {};
    
    CycleWitnessGraph(const Graph &G, const Permutation &Pi) {
        N = Pi.Cbar.size()+1;
        E.resize(N, vector<WEdge>(N));
        // Initialize all edges with infinity.
        for(in from=0; from<N; from++) {
            for(in to=0; to<N; to++) {
                E[from][to] = {from, to, INF, -1, -1};
            }
        }
        // Find the shortest down-step distance between any pair of cycles
        for(in i = 0; i<Pi.Cbar.size(); i++) {
            for(in j = 0; j<Pi.Cbar.size(); j++) {
                if(i==j) continue;
                // Check all hitting points of C_j for a potential closer distance
                for(in v : Pi.Cbar[j].C) {
                    if(E[i][j].cost > G.DC[i][v]) {
                        E[i][j].cost = G.DC[i][v];
                        E[i][j].anchor = G.AC[i][v];
                        E[i][j].hit = v;
                    }
                }
            }
        }
        // Add the edges from the root to any cycle:
        in root = N-1;
        for(in i = 0; i<Pi.Cbar.size(); i++) {
            for(in v : Pi.Cbar[i].C) {
                if(E[root][i].cost > G.DV[v][0]) {
                    E[root][i].cost = G.DV[v][0];
                    E[root][i].hit = v;
                    E[root][i].anchor = 0;
                }
            }
        }
    }
    
    CycleWitnessTree MDST() {
        // Use Edmond's recursive algorithm to find the minimum directed spanning tree in this CWG.
        
        // Start with an empty tree and let each vertex select its cheapest incoming edge.
        CycleWitnessTree T(N);
        for(in i=0; i<N; i++) {
            // Initialize with the self-loop.
            T.P[i] = E[i][i];
//            if(i==N-1) continue; // Don't select anything for the root (all incoming edges are infinite)
            for(in j = 0; j < N; j++) {
                if(T.P[i].cost > E[j][i].cost) T.P[i] = E[j][i];
            }
        }
        
        // Find a cycle to contract.
        WEdgeCycle C = T.find_cycle();
        
        DEB(
            cout << "min-in-tree "; T.print(); cout << endl;
            cout << "cycle: "; C.print(); cout << endl;
        )
        // If there is no cycle, we are already done.
        if(C.C.empty()) return T;
        // Otherwise we contract the cycle to a single.
        CycleWitnessGraph Gbar;
        // Add one new vertex: the super vertex for the cycle
        Gbar.N = N+1;
        Gbar.E = E;
        // Add an extra row and column to the new adjacency matrix
        Gbar.E = vector<vector<WEdge>>(Gbar.N, vector<WEdge>(Gbar.N));
        // Set all edges to INF initially.
        for(in i=0; i<Gbar.N; i++) {
            for(in j=0; j<Gbar.N; j++) {
                Gbar.E[i][j].cost = INF;
            }
        }
        // Transfer all the edges:
        for(in i=0; i<N; i++) {
            for(in j=0; j<N; j++) {
                // Ignore infinite edges.
                if(E[i][j].cost == INF) continue;
                // If it does not belong to the cycle, we just copy it.
                if(!C.has(i) && !C.has(j)) Gbar.E[i][j] = E[i][j];
                // If it comes out of the cycle we just change its source.
                if(C.has(i) && !C.has(j)) {
                    // Only copy it if it is cheaper than the ones seen so far.
                    if(Gbar.E[N][j].cost > E[i][j].cost) {
                        Gbar.E[N][j] = E[i][j];
                        Gbar.E[N][j].from = N;
                    }
                }
                // If it goes into the cycle we change its destination and adopt the cost.
                if(!C.has(i) && C.has(j)) {
                    // New edge cost = old edge cost - cost of cycle-edge that ends at the same vertex
                    in cost = E[i][j].cost - T.P[j].cost;
                    if(Gbar.E[i][N].cost > cost) {
                        Gbar.E[i][N] = E[i][j];
                        Gbar.E[i][N].to = N;
                        Gbar.E[i][N].cost = cost;
                    }
                }
                
            }
        }
        DEB(
            cout << "Contracted the cycle to get Gbar" << endl;
            Gbar.print();
        )
        // Recursive call to solve the problem with at least one vertex less.
        CycleWitnessTree Tbar = Gbar.MDST();
        
        DEB(
            cout << "\nExpand cycle "; C.print(); cout << endl;
        )
        // Reconstruct the tree for G from Tbar in Gbar.
        for(WEdge e : Tbar.P) {
            if(e.cost == INF) continue;
            // For edges out of the cycle find its uncontracted source.
            if(e.from == N) {
                DEB(cout << "out-edge "; e.print(); cout << endl;)
                for(in from = 0; from<N; from++) {
                    if(!C.has(from)) continue;
                    if(E[from][e.to].cost == e.cost) {
                        T.P[e.to] = E[from][e.to];
                        break;
                    }
                }
            // For the single edge into the cycle we find its uncontracted target to break the cycle.
            } else if(e.to == N) {
                for(in to = 0; to<N; to++) {
                    if(!C.has(to)) continue;
                    // Check if the cost of the edge in Gbar plus the cost of the edge on the cycle are equal
                    // to the cost of the edge in G
                    if(e.cost + T.P[to].cost == E[e.from][to].cost) {
                        T.P[to] = E[e.from][to];
                        break;
                    }
                }
            // Otherwise the same edge exists in G
            } else {
                T.P[e.to] = e;
            }
        }
        
        DEB(cout << "fixed-tree "; T.print(); cout << endl;)
        return T;
    }
    
    void print() {
        cout << "Complete Witness Graph" << endl;
        cout << "Adjacency matrix" <<endl;
        for(auto row : E) {
            for(auto e : row) {
                e.print();
            }
            cout << endl;
        }
        cout << "All non-INF edges: ";
        for(auto row : E) {
            for(auto e : row) {
                if(e.cost != INF) e.print();
            }
        }
        cout << endl;
        
    }
};

struct Problem {
    in N;
    Permutation Pi;
    Graph T;
    
    void setup(const in N,
               const VI &parent,
               const VI &permutation) {
        this->N = N;
        T = Graph(N);
        for(in i=1; i<N; i++) {
            // cout << "add edge " << parent[i-1] << " -> " << i << endl;
            T.AddEdge(parent[i-1],i);
        }
        Pi = Permutation(permutation);
    }
    void readFromStdin() {
        // Read the number of vertices and boxes.
        in N; cin >> N;
        VI parent, permutation;
        // Read the tree.
        for(in i=1; i<N; i++) {
            // Read in parent node (in [1,n]) of node i;
            in p; cin >> p; parent.push_back(p-1);
        }
        // Read the initial permutation.
        for(in i=0; i<N; i++) {
            in a; cin >> a; permutation.push_back(a-1);
        }
        setup(N, parent, permutation);
    }
    void readFromFile(const string filepath) {
        ifstream file;
        file.open(filepath);
        if(file.fail()) {
            cout << "File '" << filepath << "' does not exist" << endl;
            return;
        }
        in N; file >> N;
        cout << "Read problem of size: " << N << endl;
        VI parent, permutation;
        for(in i=1; i<N; i++) {
            in p; file >> p;
            parent.push_back(p-1);
        }
        for(in i=0; i<N; i++) {
            in a; file >> a;
            permutation.push_back(a-1);
        }
        setup(N, parent, permutation);
        file.close();
    }
    void createRandomTree(in N) {
        VI parent;
        for(in i=1; i<N; i++) parent.push_back(rand()%i);
        VI permutation;
        // Knuths algorithm for getting a random permutation.
        for(in i=0; i<N; i++) permutation.push_back(i);
        for(in i=0; i<N-1; i++) swap(permutation[i], permutation[i+(rand()%(N-i))]);
        setup(N, parent, permutation);
    }
    void createRandomPath(in N) {
        VI parent;
        for(in i=1; i<N; i++) parent.push_back(i-1);
        VI permutation;
        // Knuths algorithm for getting a random permutation.
        for(in i=0; i<N; i++) permutation.push_back(i);
        for(in i=0; i<N-1; i++) swap(permutation[i], permutation[i+(rand()%(N-i))]);
        setup(N, parent, permutation);
    }
    // The number of characters drawn per node (including whitespace up to the next node.
    const in layout_node_width = 7;
    const in layout_node_height = 4;
    // Recursively position vertex v at depth d starting at x-offset ox
    void layout(in v, in d, in optx, map<in,in> &M, vector<Point> &position) {
        // Ideal position of vertex v. It might get moved to the right later in order to be centered among its children.
        Point p = {max(optx, M[d]), layout_node_height*d};
        // try to center the children below the node
        in minChildx = p.x - (layout_node_width * (T.children[v].size()-1))/2;
        
        for(in i=0; i< T.children[v].size(); i++) {
            in w = T.children[v][i];
            in optxw = minChildx + i*layout_node_width;
            layout(w, d+1, optxw, M, position);
        }
        // Center v above its children.
        if(T.children[v].size()) {
            in minx = position[T.children[v].front()].x;
            in maxx = position[T.children[v].back()].x;
            p.x = (minx + maxx)/2;
        }
        position[v] = p;
        M[d] = p.x + layout_node_width;
    }
    
    void print() {

    }

    Problem() : Pi({}), T(0) { }
};

struct Step {
    in p; // New position of the robot.
    in b; // Box moved during this step.
    void print() {
        cout << "(" << p+1 << "," << b+1 << ")";
    }
};

struct State {
    in N;
    in p; // robot position.
    in b; // box on the robot (-1 if empty).
    // boxes (not necessarily a permutation as some slots might be empty (marked by -1).
    // sigma[i] = target position of the box that is currently at position i.
    VI sigma;
    Problem *P;
    
    State(Problem& P) {
        this->P = &P;
        p = 0;
        b = -1;
        N = P.N;
        // Initially at vertex i is box pi(i).
        sigma = P.Pi.Pi;
//        P.N;
//        sigma = VI(N);
//        // Initially box i is at position i.
//        for(in i=0; i<N; i++) sigma[i]=i;
    }
    
    bool is_valid_step(Step s) {
        if(!P->T.has(p,s.p) && !(p==s.p)) {
            cout << "This step ";
            s.print();
            cout << " is not valid as it does not move the robot along an edge of the graph." << endl;
            return false;
        }
        if(s.b != b && s.b != sigma[p]) {
            cout << "This step ";
            s.print();
            cout << " is not valid as it moves a box that is not available." << endl;
            return false;
        }
        return true;
    }
    
    bool apply_step(const Step s) {
        if(!is_valid_step(s)) return false;
        if(b != s.b) { // This step performed a swap.
            sigma[p] = b;
        }
        p = s.p;
        b = s.b;
        return true;
    }
    
    bool is_sorted() {
        // Check whether the boxes are all at their target position.
        normalize();
        for(in i=0; i<N; i++) {
            if(sigma[i] != i) return false;
        }
        return true;
    }
    
    void normalize() {
        // Put the box back into the slot if possible
        if(b != -1 && sigma[p] == -1) {
            sigma[p] = b;
            b = -1;
        }
    }
    
};

struct SortingWalk {
    vector<Step> s;
    SortingWalk() {};
    SortingWalk(vector<Step> s) {
        this->s = s;
    }
    void print() {
        for (Step step : s) step.print();
        cout << endl;
    }
    
    in firstStepTo(in v) {
        for(in i = 0; i < s.size(); i++) {
            if(s[i].p == v) return i;
        }
        return -1;
    }
    void insertAfter(in after, SortingWalk other) {
        s.insert(s.begin()+after+1, other.s.begin(), other.s.end());
    }
    void append(SortingWalk other) {
        s.insert(s.end(), other.s.begin(), other.s.end());
    }
};

// Interpolate from the previous state
void printTreeState(State t, Problem P, in r_before, double rt) {
    // Grid Layout Option: store one position per vertex.
    vector<Point> position(P.N);
    map<in,in> M;
    P.layout(0,0,0,M,position);
    // Determine dimensions of the tree.
    in mx=0, my=0;
    for (auto p : position) {
        mx = max(mx, p.x);
        my = max(my, p.y);
    }
    // Initialize an empty screen buffer:
    vector<string> B(my+2, string(mx+10, ' '));
    
    for (int i=0; i<P.N; i++) {
        Point p = position[i];
        B[p.y][p.x] = encode(i);
        B[p.y][p.x+2] = '[';
        if(t.sigma[i] == -1) B[p.y][p.x+3] = '_';
        else B[p.y][p.x+3] = encode(t.sigma[i]);
        B[p.y][p.x+4] = ']';
        // Draw an edge if
        if(P.T.parent[i] != i) {
            Point pp = position[P.T.parent[i]];
            // Draw the edge character in the middle.
            in dx = p.x - pp.x;
            in sdx = (dx>0) - (dx<0);
            
            char ce = (p.x==pp.x) ? '|' : ((p.x < pp.x) ? '/' : '\\');
            B[pp.y+1][pp.x+sdx] = ce;
            B[pp.y+3][p.x-sdx] = ce;
            // If the edge is wider than three steps, then we draw a horizontal line
            if(abs(dx)>4) {
                for(in x = min(p.x,pp.x)+2; x<=max(p.x,pp.x)-2; x++) {
                    B[pp.y+2][x] = '-';
                }
            } else if(abs(dx)<=3) {
                B[pp.y+2][pp.x+sdx] = '|';
            } else { // |x| = 3
                B[pp.y+2][pp.x+2*sdx] = ce;
            }
        }
        // Draw the robot.
        Point r_start = position[r_before];
        Point r_end = position[t.p];
        p = Point{(in)((1.0-rt)*r_start.x + rt*r_end.x),
            (in)((1.0-rt)*r_start.y + rt*r_end.y)};
        B[p.y+1][p.x+2] = '\\';
        if(t.b == -1) B[p.y+1][p.x+3] = '_';
        else B[p.y+1][p.x+3] = encode(t.b);
        B[p.y+1][p.x+4] = '/';
        
    }
    for (auto s : B) {
        cout << s << endl;
    }
}

void printPathState(State t, Problem P) {
    for(in n=0; n<P.N; n++) {
        cout << " " << encode(n) << "   ";
    }
    cout << endl;
    for(in n=0; n<P.N; n++) {
        if(t.sigma[n]==-1) cout << "[_]  ";
        else cout << "[" << encode(t.sigma[n]) << "]  ";
    }
    cout << endl;
    for(in n=0; n<P.N; n++) {
        if(n== t.p) {
            cout << "\\";
            if(t.b == -1) cout << "_";
            else cout << encode(t.b);
            cout << "/  ";
        } else {
            cout << "     ";
        }
    }
    cout << endl;
}

SortingWalk showSwitches(const SortingWalk S) {
    SortingWalk R;
    Step last = {0,-2};
    for (in i=0; i<S.s.size(); i++) {
        if (S.s[i].b != last.b) {
            R.s.push_back({last.p, S.s[i].b});
        }
        R.s.push_back(S.s[i]);
        last = S.s[i];
    }
    // Put down the final box.
    if(R.s.back().b != -1) R.s.push_back({R.s.back().p,-1});
    return R;
}


void printState(State t, Problem P, in r_before, double rt, in spacing) {
    while(spacing--) cout << endl;
    P.Pi.print();
    cout << "Graph:" << endl;
    
    if(P.T.is_path()) {
        printPathState(t,P);
    } else {
        printTreeState(t, P, r_before, rt);
    }
}
void printState(State t, Problem P, in spacing) {
    printState(t, P, t.p, 1.0, spacing);
}
in speed = 20;

void pause(double ratio) {
    in x = ratio*speed*5000000;
    while(x--);
}
void wait() {
    while(!getchar()) {}
}
void printSortingWalk(SortingWalk S) {
    cout << "Sorting Walk S: ";
    S.print();
    cout << endl;
}

void printWalk(State t, Problem P, SortingWalk S) {
    printState(t,P,50);
    S = showSwitches(S);
//    printSortingWalk(S);
    for (Step s : S.s) {
//        wait();
        in old_p = t.p;
        in new_p = s.p;
        if(old_p != new_p) {
            if(!t.apply_step(s)) return;
            in timesteps = 5;
            for(double rt=0; rt<1.0; rt+= 1./timesteps) {
                printState(t, P, old_p, rt, 50);
                pause(1./timesteps);
            }
        }
        // Animate the ride from
        else {
            printState(t,P,50);
            pause(0.5);
            if(!t.apply_step(s)) return;
            printState(t,P,50);
            pause(0.5);
        }
//        char c;
//        cin >> c;
    }
}

// Fill up multi-steps with the single steps in between.
SortingWalk fillup(in p, SortingWalk S, const Problem &P) {
    SortingWalk R;
    for(in i = 0; i<S.s.size(); i++) {
        while(p != S.s[i].p) {
            // Walk one step towards S.s[i].p.
            p = P.T.PV[S.s[i].p][p];
            R.s.push_back({p,S.s[i].b});
        }
    }
    return R;
}


void printVI(VI V, bool code) {
    cout << "[ ";
    for (in x : V) {
        if(code) cout << encode(x) << " ";
        else cout << x << " ";
    }
    cout << "]" << endl;
}

void printVVI(VVI A, bool code) {
    for (VI V: A) printVI(V, code);
}

SortingWalk solvePath(Problem P) {
    P.T.Preprocess(P.Pi);

    SortingWalk S;
//    vector<Step *> firstStep(P.N,NULL);
    Step * maxStep = NULL;
    // rightmost vertex visited so far.
    in max_visited = 0;

    for(Cycle C : P.Pi.Cbar) {
        // Generate the steps for sorting C.
        SortingWalk SC;
        for (in i = 0; i < C.C.size(); i++) {
            SC.s.push_back({C.C[(i+1)%C.C.size()],C.C[(i+1)%C.C.size()]});
        }
        SC = fillup(C.l,SC,P);
//        cout << "new sorting cycle for ";
//        C.print();
//        cout << ": ";
//        printSortingWalk(SC);
//        cout << endl;
        
        SortingWalk Snew;
        
        if(max_visited >= C.C[0]) {
            // We can insert into existing walk using only essential steps.
            
            // Search the insert position.
            in i = -1;
//            cout << "C.l " << C.l << endl;
            
            do {
                i++;
                if(i < S.s.size()) Snew.s.push_back(S.s[i]);
            } while(i < S.s.size() && S.s[i].p != C.l);

//            cout << "insert after " << Snew.size() << " steps of the old path" << endl;
            // Insert the new cycle.
            for(Step s : SC.s)
                Snew.s.push_back(s);
            
            // Finish the previous walk.
            i++;
            while(i<S.s.size()) {
                Snew.s.push_back(S.s[i]);
                i++;
            }
        } else {
            // We have to extend the current path with non-essential steps.
            
            // Search the step where max_visited is reached.
            in i = -1;
            in last_box = -1;
            do {
                i++;
                if(i < S.s.size()) {
                    Snew.s.push_back(S.s[i]);
                    last_box = S.s[i].b;
                }
            } while(i < S.s.size() && S.s[i].p != max_visited);
            
            // Insert non-essential steps to C.l
            SortingWalk SnonTo = fillup(max_visited, SortingWalk({{C.l, last_box}}),P);
            for (Step s : SnonTo.s) Snew.s.push_back(s);
            
            // Insert the new cycle.
            for(Step s : SC.s)
                Snew.s.push_back(s);

            // Insert non-essential steps back to max_visited
            SortingWalk SnonBack = fillup(C.l, SortingWalk({{max_visited, last_box}}),P);
            for (Step s : SnonBack.s) Snew.s.push_back(s);
            
            // Finish the previous walk.
            i++;
            while(i<S.s.size()) {
                Snew.s.push_back(S.s[i]);
                i++;
            }
        }
        max_visited = max(max_visited, C.r);
        S = Snew;
        
//        cout << "Walk after adding cycle ";
//        C.print();
//        printSortingWalk(S);
    }
    return S;
    
}


void CWT_traversal(in v, CycleWitnessTree & CWT, SortingWalk & S, const Problem &P) {
    DEB(cout << "CWT traversal in " << v << endl;)
    // Unless we are at the root, we first add the steps for C_v to the current sorting walk.
    if(v<P.Pi.Cbar.size()) {
        WEdge e = CWT.P[v];
        in last_step_before = S.firstStepTo(e.anchor);
        in last_box_before = -1;
        if(last_step_before != -1) {
            last_box_before = S.s[last_step_before].b;
        }
        // Generate sorting steps for this cycle
        SortingWalk SC;
        Cycle C = P.Pi.Cbar[v];
        // Find an offset, so that the cycle starts at the box hit by edge
        in j=0; while(C.C[j] != e.hit) j++;
        for (in i = 0; i < C.C.size(); i++) {
            SC.s.push_back({C.C[(j+i+1)%C.C.size()],C.C[(j+i+1)%C.C.size()]});
        }
        SC = fillup(e.hit,SC,P);
        // Insert non-essential down-steps from the anchor to hit
        SortingWalk SnonTo = fillup(e.anchor, SortingWalk({{e.hit, last_box_before}}),P);
        // Insert non-essential up-steps from hit back to anchor
        SortingWalk SnonBack = fillup(e.hit, SortingWalk({{e.anchor, last_box_before}}),P);
        
        SortingWalk Sinsert;
        Sinsert.append(SnonTo);
        Sinsert.append(SC);
        Sinsert.append(SnonBack);
        DEB(cout << "walk to insert: "; Sinsert.print();)
        
        S.insertAfter(last_step_before, Sinsert);
        DEB(cout << "S after to insert: "; S.print();)
    }
    // Now we have the sorting of C_v included and can continue with all children of C_v.
    for(in j=0; j<CWT.children[v].size(); j++) {
        in w = CWT.children[v][j].to;
        CWT_traversal(w, CWT, S, P);
    }
}

SortingWalk solveTree(Problem P) {
    P.T.Preprocess(P.Pi);

    DEB(
        cout << "\nPV\n"; printVVI(P.T.PV,true);
        cout << "\nDV\n"; printVVI(P.T.DV,false);
        cout << "\nhit\n"; printVVI(P.T.hit,false);
        cout << "\ncov\n"; printVVI(P.T.cov,false);
        cout << "\nPC\n"; printVVI(P.T.PC,true);
        cout << "\nAC\n"; printVVI(P.T.AC,true);
        cout << "\nDC\n"; printVVI(P.T.DC,false);
    )
    
    CycleWitnessGraph CWG(P.T,P.Pi);
    DEB(CWG.print();)
    
    CycleWitnessTree CWT = CWG.MDST();
    
    SortingWalk S;
    CWT.prepare_for_traversal();
    CWT_traversal(CWT.N-1, CWT, S, P);
    
    return S;
}

SortingWalk solve(Problem P) {
    if (P.T.is_path()) {
        return solvePath(P);
    } else {
        return solveTree(P);
    }
}
in path_solution_length(Problem P) {
    in sum = P.Pi.d();
    // Use a scan line approach to count the number of not-covered edges.
    // Boolean array to mark the positions where forward moving boxes arrive.
    VI E(P.N,0);
    in open = 0;
    // Find the rightmost box at position N that is part of a non-trivial cycle.
    in N=0;
    for(in i=0; i<P.N; i++) {
        if(i!=P.Pi.Pi[i]) N=i;
    }
    // Move along the warehouse from left until the rightmost non-sorted box.
    for (in i=0; i<N; i++) {
        // Act for all forward arcs of the permutation.
        if (P.Pi.Pi[i] > i) {
            open++;
            E[P.Pi.Pi[i]]++;
        }
        open -= E[i];
//        cout << i << " -> " << i+1 << "(" << open << ")" << endl;
        // If we are not at the final vertex and no arc is active across the edge
        // to the right, then this edge will be used twice for non-essential steps.
        if(!open) sum += 2;
    }
    return sum;
}

in tree_solution_length(Problem P) {
    P.T.Preprocess(P.Pi);
    CycleWitnessGraph CWG(P.T,P.Pi);
    CycleWitnessTree CWT = CWG.MDST();
    return P.T.d(P.Pi) + 2 * CWT.cost();
}

bool verify(Problem P, SortingWalk S) {
    cout << "Solution walk of length " << S.s.size() << endl;
    State tau(P);
    bool valid = true;
    in bound = 0;
    if(P.T.is_path()) {
        bound = path_solution_length(P);
        
    } else {
        bound = tree_solution_length(P);
    }
    if (bound == S.s.size()) {
        cout << "-> this matches the bound of the theorem!" << endl;
    } else {
        cout << "-> this does not match the length given by the theorem, which is " << bound << "!" << endl;
        S.print();
        printState(tau, P, 0);
        valid = false;
    }
    for (Step s: S.s) {
        if(!tau.apply_step(s)) break;
    }
    // Verify that it is solved.
    if(tau.is_sorted()) {
        cout << "-> it correctly sorts the input!" << endl;
    } else {
        cout << "-> it does not correctly sort the input and ends in state:" << endl;
        S.print();
        printState(tau, P, 0);
        valid = false;
    }
    return valid;
}

void demo() {
    Problem P;
    State Tau(P);
    // Example of a small tree.
    P.readFromFile("sample1.in");
    Tau = State(P);
    printState(Tau,P,0);
    // Example of nine boxes on a line.
        P.readFromFile("line3.in");
//    P.createRandomPath(13);
//    P.createRandomPath(30);
    Tau = State(P);
    //    S.apply_step({1,4});
    //    print(S,P);
    // Suboptimal sorting walk that sorts cycle after cycle.
    //    SortingWalk S = {{1,4},{2,4},{3,4},{4,4},{5,7},{6,7},{7,7},{6,5}, {5,5}, {4,0}, {3,0},{2,0}, {1,0}, {0,0},
    //        {1,-1}, {2,2}, {1,1},
    //        {2,-1}, {3,-1}, {4,-1}, {5,-1}, {6,-1},{7,8}, {8,8}, {7,6}, {6,6},
    //        {5,-1},{4,-1},{3,-1},{2,-1},{1,-1},{0,-1}};
    //    printWalk(Tau, P, S);
    
    SortingWalk Sopt = solvePath(P);
    Tau = State(P);
    printState(Tau,P,0);
    printSortingWalk(Sopt);
    printWalk(Tau, P, Sopt);
    //    // Example a medium size random tree and a random permutation.
    //    P.createRandomTree(35);
    //    Tau = State(P);
    //    print(Tau,P);
}

void console() {
    Problem P;
    State tau(P);
    
    while(true) {
        cout << "$> ";
        string line; getline(cin,line);
        stringstream ss(line);
        string cmd;
        ss >> cmd;
        if (cmd == "l" || cmd == "load") {
            string filename; ss >> filename;
            P.readFromFile(filename);
            tau = State(P);
        } else if (cmd == "rp" || cmd == "randompath") {
            in n; ss >> n;
            P.createRandomPath(n);
            tau = State(P);
        } else if (cmd == "rt" || cmd == "randomtree") {
            in n; ss >> n;
            P.createRandomTree(n);
            tau = State(P);
        } else if (cmd == "s" || cmd == "step") {
            char cp, cb; ss >> cp >> cb;
            in p=decode(cp), b=decode(cb);
            if(tau.apply_step({p,b}))
                printState(tau, P, 0);
        } else if (cmd == "p" || cmd == "print") {
            printState(tau, P, 0);
        } else if (cmd == "e" || cmd == "speed") {
            ss >> speed;
        } else if (cmd == "d" || cmd == "debug") {
            debug = !debug;
        } else if (cmd == "x" || cmd == "solve") {
            SortingWalk S = solve(P);
            printSortingWalk(S);
        } else if (cmd == "a" || cmd == "animate" || cmd == "show") {
            SortingWalk S = solve(P);
            tau = State(P);
            printWalk(tau, P, S);
        } else if (cmd == "v" || cmd == "verify") {
            SortingWalk S = solve(P);
            verify(P,S);
        } else if (cmd == "t" || cmd == "test") {
            do {
                P.createRandomPath(rand()%10+1);
                if(!verify(P, solve(P))) break;
                
                P.createRandomTree(rand()%10+1);
                if(!verify(P, solve(P))) break;
            } while(true);
            cout << "TEST FAILED" << endl;
        } else if (cmd == "d" || cmd == "demo") {
            demo();
        } else if (cmd == "h" || cmd == "help") {
            cout << "====================" << endl;
            cout << "|| Tree Sort Help ||" << endl;
            cout << "====================" << endl;
            cout << endl;
            cout << "Generate or load a problem" << endl;
            cout << "--------------------------" << endl;
            cout << "l/load [filename]\n\tLoad a tree from a file." << endl;
            cout << "rp/randompath [n]\n\tGenerate a path of n boxes with a random permutation." << endl;
            cout << "rt/randomtree [n]\n\tGenerate a tree of n boxes with a random permutation." << endl;
            cout << endl;
            cout << "Solve a problem" << endl;
            cout << "--------------------------" << endl;
            cout << "s/step [p] [b]\n\tPerform a single sorting step to vertex p with box b." << endl;
            cout << "x/solve\n\tSolve the problem and print a shortest sorting walk." << endl;
            cout << "v/verify\n\tVerify that the sorting walk is valid and as short as the theorem claims it." << endl;
            cout << "a/animate\n\tAnimate the sorting walk of the robot in ASCII." << endl;
            cout << "t/test\n\tGenerate random paths and trees and check their solutions for validity and optimality (randomized bug-finding)." << endl;
            cout << endl;
            cout << "Utilities" << endl;
            cout << "--------------------------" << endl;
            cout << "d/debug\n\tToggle debug output when solving an problem (prints the contraction hiearchy and its DMSTs)." << endl;
            cout << "e/speed [v]\n\t.Set the animation speed. Default is 20." << endl;
            cout << "p/print\n\tPrint the current state of the warehouse." << endl;
            cout << endl;
            
        } else {
            cout << "[Invalid command. Use 'h' or 'help' to get a list of all commands." << endl;
        }
    }
}

int main(int argc, const char * argv[]) {
    console();
    return 0;
}