// C++ program for solution of M
// Coloring problem using backtracking
#include <iostream>
#include "boost/multi_array.hpp"
#include <boost/graph/adjacency_list.hpp>
#include "boost/graph/boyer_myrvold_planar_test.hpp"
#include <algorithm>
#include <random>
#include <deque>

using namespace std;

template<class T, int N>
using boostmat = typename boost::multi_array<T,N>;

using Int2D = typename std::array<int,2>;

bool notequal(Int2D coord_1, Int2D coord_2) {
    return((coord_1[0] != coord_2[0])||(coord_1[1] != coord_2[1]));
}

Int2D getcoordinate(Int2D base_coord, int direction ) {
    if (direction == 0) {
        return Int2D({(base_coord[0]-1),base_coord[1]});
    } else if (direction == 1) {
        return Int2D({base_coord[0],(base_coord[1]+1)});
    } else if (direction == 2) {
        return Int2D({base_coord[0]+1,base_coord[1]});
    } else if (direction == 3) {
        return Int2D({base_coord[0],base_coord[1]-1});
    } else {
        throw runtime_error("unknown direction!");
    }
}

vector<int> getFinalNeighboursEnd(Int2D current_loc, Int2D before, boostmat<int,2> walk_matrix) {
    Int2D down = {current_loc[0] + 1, current_loc[1]};
    Int2D up = {current_loc[0] - 1, current_loc[1]};
    Int2D right = {current_loc[0], current_loc[1]+1};
    Int2D left = {current_loc[0], current_loc[1]-1};
    
    vector<int> directional_vector;
    
    if (notequal(up,before)&&(walk_matrix[up[0]][up[1]] > -1)) {
        directional_vector.push_back(0);
    }
    
    if (notequal(right,before)&&(walk_matrix[right[0]][right[1]] > -1)) {
        directional_vector.push_back(1);
    }
    
    if (notequal(down,before)&&(walk_matrix[down[0]][down[1]] > -1)) {
        directional_vector.push_back(2);
    }
    
    if (notequal(left,before)&&(walk_matrix[left[0]][left[1]] > -1)){
        directional_vector.push_back(3);
    } 
    
    return(directional_vector);
}

vector<int> getFinalNeighboursStart(Int2D current_loc, Int2D after, boostmat<int,2> walk_matrix) {
    Int2D down = {current_loc[0] + 1, current_loc[1]};
    Int2D up = {current_loc[0] - 1, current_loc[1]};
    Int2D right = {current_loc[0], current_loc[1]+1};
    Int2D left = {current_loc[0], current_loc[1]-1};
    
    vector<int> directional_vector;
    
    if (notequal(up,after)&&(walk_matrix[up[0]][up[1]] > -1)) {
        directional_vector.push_back(0);
    }
    
    if (notequal(right,after)&&(walk_matrix[right[0]][right[1]] > -1)) {
        directional_vector.push_back(1);
    }
    
    if (notequal(down,after)&&(walk_matrix[down[0]][down[1]] > -1)) {
        directional_vector.push_back(2);
    }
    
    if (notequal(left,after)&&(walk_matrix[left[0]][left[1]] > -1)){
        directional_vector.push_back(3);
    } 
    
    return(directional_vector);
}

vector<int> getFinalNeighbours(Int2D current_loc, Int2D before, Int2D after, boostmat<int,2> walk_matrix) {
    Int2D down = {current_loc[0] + 1, current_loc[1]};
    Int2D up = {current_loc[0] - 1, current_loc[1]};
    Int2D right = {current_loc[0], current_loc[1]+1};
    Int2D left = {current_loc[0], current_loc[1]-1};
    
    vector<int> directional_vector;
    
    if (notequal(up,before)&&notequal(up,after)&&(walk_matrix[up[0]][up[1]] > -1)) {
        directional_vector.push_back(0);
    } 
    
    if (notequal(right,before)&&notequal(right,after)&&(walk_matrix[right[0]][right[1]] > -1)) {
        directional_vector.push_back(1);
    }
    
    if (notequal(down,before)&&notequal(down,after)&&(walk_matrix[down[0]][down[1]] > -1)) {
        directional_vector.push_back(2);
    }  
    
    if (notequal(left,before)&&notequal(left,after)&&(walk_matrix[left[0]][left[1]] > -1)){
        directional_vector.push_back(3);
    }
    
    return(directional_vector);
}

int  getNextLocation(Int2D current_loc, boostmat<int,2> walk_matrix) {
    Int2D down = {current_loc[0] + 1, current_loc[1]};
    Int2D up = {current_loc[0] - 1, current_loc[1]};
    Int2D right = {current_loc[0], current_loc[1]+1};
    Int2D left = {current_loc[0], current_loc[1]-1};
    
    if (walk_matrix[up[0]][up[1]] == (walk_matrix[current_loc[0]][current_loc[1]] + 1) ) {
        return(0);
    } else if (walk_matrix[down[0]][down[1]] == (walk_matrix[current_loc[0]][current_loc[1]] + 1) ) {
        return(2);
    } else if (walk_matrix[right[0]][right[1]] == (walk_matrix[current_loc[0]][current_loc[1]] + 1) ) {
        return(1);
    }  else if (walk_matrix[left[0]][left[1]] == (walk_matrix[current_loc[0]][current_loc[1]] + 1) ){
        return(3);
    } else {
        cout << "At walk location " << walk_matrix[current_loc[0]][current_loc[1]] << endl;
        throw runtime_error("No next location!");
    }
}

bool by_horizontal_first(array<Int2D,2> c1, array<Int2D,2> c2){
    int horizontal_min_1 = min(c1[0][1], c1[1][1]);
    int horizontal_min_2 = min(c2[0][1], c2[1][1]);
    
    if (horizontal_min_1 == horizontal_min_2){
        return(c1[0][0] < c2[0][0]);
    } else {
        return(horizontal_min_1<horizontal_min_2);
    }
    
}

bool by_vertical_first(array<Int2D,2> c1, array<Int2D,2> c2){
    int vertical_min_1 = min(c1[0][0], c1[1][0]);
    int vertical_min_2 = min(c2[0][0], c2[1][0]);
    
    if (vertical_min_1 == vertical_min_2){
        return(c1[0][1] < c2[0][1]);
    } else {
        return(vertical_min_1<vertical_min_2);
    }
    
}

struct PatchableCoordinates {
    array<array<Int2D,2>,2> coords;
    Int2D subcycles;
    bool horizontal;
    
    PatchableCoordinates(array<array<Int2D,2>,2> coords_in, Int2D subcycles_in, bool horizontal_in);
};

PatchableCoordinates::PatchableCoordinates(array<array<Int2D,2>,2> coords_in, Int2D subcycles_in, bool horizontal_in) {
    coords = coords_in;
    subcycles = subcycles_in;
    horizontal = horizontal_in;
}


/******************************************
    START CLASS VERTEX
******************************************/

class Vertex {
    
    bool remove_unmatched(Int2D coordinate);
    void add_unmatched(Int2D coordinate);
    bool remove_matched(Int2D coordinate);
    void add_matched(Int2D coordinate);
    
    public:
    bool color;
    int subcycle;
    
    Vertex();
    Vertex(Int2D identity_in);
    Int2D identity;
    vector<Int2D> matched_neighbours;
    vector<Int2D> unmatched_neighbours;
    void to_matched(Int2D coordinate);
    void to_unmatched(Int2D coordinate);
    
};

Vertex::Vertex(){
}

// Functions to add and remove coordinates from the list of matched or unmatched neighbours
Vertex::Vertex(Int2D identity_in){
    identity = identity_in;
    color = bool((identity_in[0]+identity_in[1]) % 2);
    subcycle = -1;
}

bool Vertex::remove_unmatched(Int2D coordinate){
    for (int i = 0; i < unmatched_neighbours.size(); i++) {
        if (coordinate[0] == unmatched_neighbours[i][0] && coordinate[1] == unmatched_neighbours[i][1]){
            unmatched_neighbours.erase(unmatched_neighbours.begin()+i);
            return(true);
        }
    }
    throw("Cannot remove non-existent neighbour!");
    return(false);
}

void Vertex::add_unmatched(Int2D coordinate){
    unmatched_neighbours.push_back(coordinate);
}

bool Vertex::remove_matched(Int2D coordinate){
    for (int i = 0; i < matched_neighbours.size(); i++) {
        if (coordinate[0] == matched_neighbours[i][0] && coordinate[1] == matched_neighbours[i][1]){
            matched_neighbours.erase(matched_neighbours.begin()+i);
            return(true);
        }
    }
    throw runtime_error(string("Cannot unmatch non-matched neighbour ") + string("(") + to_string(coordinate[0]) 
        + string(",") + to_string(coordinate[1]) + string(")") + string(" from Vertex ")
        + string("(") + to_string(identity[0]) + string(",") + to_string(identity[1]) + string(")!"));
    return(false);
}

void Vertex::add_matched(Int2D coordinate){
    matched_neighbours.push_back(coordinate);
}

void Vertex::to_matched(Int2D coordinate){
    add_matched(coordinate);
    remove_unmatched(coordinate);
}

void Vertex::to_unmatched(Int2D coordinate){
    add_unmatched(coordinate);
    remove_matched(coordinate);
}

/******************************************
    END CLASS VERTEX
******************************************/

/******************************************
    START CLASS COMPACTSAW
******************************************/

class CompactSaw{
    mt19937_64 mersenne_generator;
    
    public:
    int nrow;
    int ncol;
    // boostmat<bool,4> lattice_graph;
    boostmat<Vertex,2> vertex_mat;
    int unsaturated_count;
    // The following two variables are the extra nodes that 
        // allow us to form a hamiltonian cycle (which can later be turned
        // into a hamiltonian path)
    Vertex zero_node; // If I were to re-do this, the presence of these two nodes probably makes working with pointers
                          // the more preferable option over working with coordinates
    Vertex one_node;
    
    boostmat<int, 2> vertex_mat_locations;
    int one_location;
    int zero_location;
    
    vector<Int2D> vertex_vector;
    
    int patch_count;
    vector<PatchableCoordinates> patchable_set;
    boostmat<bool,2> coordinate_used_up;
    int patchable_set_count;
    boostmat<int,2> final_lattice;
    unsigned int rand_seed;
    
    CompactSaw(int row_size, int col_size);
    void deletePointers();
    Int2D getUnsaturatedVertex();
    Int2D chooseUnmatchedNeighbour(Int2D);
    Int2D chooseMatchedNeighbour(Int2D);
    void get2Matching();
    void create_matching(Int2D coordinate_1, Int2D coordinate_2);
    void remove_matching(Int2D coordinate_1, Int2D coordinate_2);
    void PrintNeighbours(Int2D unsat_coordinate);
    void check2Matching();
    Vertex getVertex(int x, int y);
    int getLocation(int x, int y);
    bool isSaturated(int x, int y);
    void findSubcycles();
    void ResetLocations();
    void assignSubcycle(int x, int y, int sub_cycle);
    void createBorderMatrix();
    void reviewPatchable(Int2D subcycle_set);
    bool Patch();
    bool are_neighbours(Int2D coordinate_1, Int2D coordinate_2);
    void PrintSubcycles();
    void constructLattice();
    void printLattice();
};

CompactSaw::CompactSaw(int row_size, int col_size){
    nrow = row_size;
    ncol = col_size;
    
    random_device rand_dev;
    rand_seed = rand_dev();

    mersenne_generator.seed(rand_seed);
    
    //2033892598 3537038690 2816219696 3959467474
    
    //lattice_graph.resize(boost::extents[row_size][col_size][row_size][col_size]);
    //std::fill_n(lattice_graph.data(), lattice_graph.num_elements(), 0);
    
    zero_node = Vertex({0,-1});
    one_node = Vertex({1,-1});
    
    vector<Int2D> zero_node_neighbours;
    vector<Int2D> one_node_neighbours;
    
    // Setting up -> Detemining which nodes are neighbours on the lattice (no matching takeing place yet)
    
    vertex_mat.resize(boost::extents[row_size][col_size]);
    vertex_mat_locations.resize(boost::extents[row_size][col_size]);
    for (int i = 0; i < row_size; i++){
        for (int j = 0; j < col_size; j++){
            vertex_mat[i][j] = Vertex({i,j});
            vector<Int2D> neighbours;
            if (i != 0){
               neighbours.push_back({i-1,j});
            } 
            if (i != row_size-1){
               neighbours.push_back({i+1,j});
            }
            if (j != 0){
               neighbours.push_back({i,j-1});
            }
            if (j != col_size-1){
               neighbours.push_back({i,j+1});
            }
            if (vertex_mat[i][j].color) {
               neighbours.push_back({1,-1});
               one_node_neighbours.push_back({i,j});
            } else {
               neighbours.push_back({0,-1});
               zero_node_neighbours.push_back({i,j});
            }
            vertex_mat[i][j].unmatched_neighbours = neighbours;
        }
    }
    unsaturated_count = row_size*col_size + 2;
    
    zero_node.unmatched_neighbours = zero_node_neighbours;
    one_node.unmatched_neighbours = one_node_neighbours; // It is easier to add the zero_node to one_node matching after patching rather than now. 
    
    vertex_vector.resize(nrow*ncol+2);
    
    int vector_counter = 0; //use counter instead of push_back so 
                                //that we can get the address for the pointer matrix
    
    for (int i = 0; i < vertex_mat_locations.shape()[0]; i++){
        for (int j = 0; j < vertex_mat_locations.shape()[1]; j++){
            vertex_vector[vector_counter] = {i,j};
            vertex_mat_locations[i][j] = vector_counter;
            vector_counter++;
        }
    }
    vertex_vector[vector_counter] = {0,-1};
    zero_location = vector_counter;
    vector_counter++;
    
    
    vertex_vector[vector_counter] = {1,-1};
    one_location = vector_counter;
    
    coordinate_used_up.resize(boost::extents[row_size][col_size]);
    fill_n(coordinate_used_up.data(), coordinate_used_up.num_elements(), 0);
}

Int2D CompactSaw::getUnsaturatedVertex(){
    double rand_no = uniform_real_distribution<double>(0.0,1.0)(mersenne_generator);
    
    Int2D dummy_return;
    
    int counter = 0;
    for (int i = 0; i < vertex_mat.shape()[0]; i++){
        for (int j = 0; j < vertex_mat.shape()[1]; j++){
            if (!isSaturated(i,j)) {
                counter++;
                if ( counter >= rand_no*double(unsaturated_count)){
                    dummy_return = {i, j};
                    return(dummy_return);
                }
            }
        }
    } 
    
    // Choosing either zero_node or one_node
    if (!isSaturated(0,-1)) {
    counter++;
    if ( counter >= rand_no*double(unsaturated_count)){
        dummy_return = {0, -1};
        return(dummy_return);}
    }
    
    if (!isSaturated(1,-1)){
    counter++;
    if ( counter >= rand_no*double(unsaturated_count)){
        dummy_return = {1, -1};
        return(dummy_return);}
    }
    
    throw runtime_error("Search for unsaturated coordinates returned null result!");
    dummy_return = {-1,-1};
    return(dummy_return);
}

Int2D CompactSaw::chooseUnmatchedNeighbour(Int2D neighbour_coordinates){
    double rand_no = uniform_real_distribution<double>(0.0,1.0)(mersenne_generator);
    
    if (neighbour_coordinates[1] > -1) {
        for (int i = 0; i< vertex_mat[neighbour_coordinates[0]][neighbour_coordinates[1]].unmatched_neighbours.size(); i++ ){
            if ( i+1 >= rand_no*double(vertex_mat[neighbour_coordinates[0]][neighbour_coordinates[1]].unmatched_neighbours.size())){
                        return(vertex_mat[neighbour_coordinates[0]][neighbour_coordinates[1]].unmatched_neighbours[i]);
            }
        }
    } else {
        if (neighbour_coordinates[0] == 0) {
            for (int i = 0; i< zero_node.unmatched_neighbours.size(); i++ ){
                if ( i+1 >= rand_no*double(zero_node.unmatched_neighbours.size())){
                            return(zero_node.unmatched_neighbours[i]);
                }
            } 
        } else if (neighbour_coordinates[0] == 1) {
            for (int i = 0; i< one_node.unmatched_neighbours.size(); i++ ){
                if ( i+1 >= rand_no*double(one_node.unmatched_neighbours.size())){
                            return(one_node.unmatched_neighbours[i]);
                }
            } 
        } else {
            throw runtime_error("Coordinates for neighbour selection invalid!");
        }
    }
    throw runtime_error("Search for neighbours returned null result!");
    Int2D dummy_return = {-1,-1};
    return(dummy_return);
}

Int2D CompactSaw::chooseMatchedNeighbour(Int2D neighbour_coordinates){
    double rand_no = uniform_real_distribution<double>(0.0,1.0)(mersenne_generator);
    
    if (neighbour_coordinates[1] > -1) {
        for (int i = 0; i< vertex_mat[neighbour_coordinates[0]][neighbour_coordinates[1]].matched_neighbours.size(); i++ ){
            if ( i+1 >= rand_no*double(vertex_mat[neighbour_coordinates[0]][neighbour_coordinates[1]].matched_neighbours.size())){
                        return(vertex_mat[neighbour_coordinates[0]][neighbour_coordinates[1]].matched_neighbours[i]);
            }
        }
    } else {
        if (neighbour_coordinates[0] == 0) {
            for (int i = 0; i< zero_node.matched_neighbours.size(); i++ ){
                if ( i+1 >= rand_no*double(zero_node.matched_neighbours.size())){
                            return(zero_node.matched_neighbours[i]);
                }
            } 
        } else if (neighbour_coordinates[0] == 1) {
            for (int i = 0; i< one_node.matched_neighbours.size(); i++ ){
                if ( i+1 >= rand_no*double(one_node.matched_neighbours.size())){
                            return(one_node.matched_neighbours[i]);
                }
            } 
        } else {
            throw("Coordinates for neighbour selection invalid!");
        }
    }
    
    throw("Search for neighbours returned null result!");
    Int2D dummy_return = {-1,-1};
    return(dummy_return);
}


void CompactSaw::get2Matching(){
     Int2D unmatch_coordinate;
     Int2D match_coordinate;
     Int2D next_match_coordinate;
     Int2D next_unmatch_coordinate;
     
     bool saturated = true; // variable that tells you whether an unsaturated vertex has been found or not
     bool success = true;
     
     while (unsaturated_count > 0){
         
         // Look for an unsaturated vertex
         match_coordinate = getUnsaturatedVertex();
         
         unmatch_coordinate = chooseUnmatchedNeighbour(match_coordinate);
         
         saturated = isSaturated(unmatch_coordinate[0],unmatch_coordinate[1]);
         success = true;

         while (saturated) {
             
             // Choose a new neighbour that is matched with the current node;
             next_match_coordinate = chooseMatchedNeighbour(unmatch_coordinate);
             // cout << "Matched Neighbour: (" << next_match_coordinate[0] << ", " << next_match_coordinate[1] << ")" << endl;
             // Create a match between the two coordinates
             create_matching(match_coordinate, unmatch_coordinate);
             // cout << "Unsaturated Count: " << unsaturated_count << endl;
             // update to the new coordinate
             match_coordinate = next_match_coordinate;
             
             // cout << unsaturated_count << endl;
             // cout << endl;
             // PrintNeighbours(match_coordinate);  
             /*
             for (int i = 0; i < vertex_mat.shape()[0]; i++){
                 for (int j = 0; j < vertex_mat.shape()[1]; j++){
	                 cout << isSaturated(vertex_mat[i][j].identity[0], vertex_mat[i][j].identity[1]) << " " << flush;               
                 }
                 cout << endl;
             } 
             
             cout << "zero: " << isSaturated(zero_node.identity[0], zero_node.identity[1]) << endl;
             cout << "one: " << isSaturated(one_node.identity[0], one_node.identity[1]) << endl;
             
             cout << endl;
             
             for (int i = 0; i < vertex_mat.shape()[0]; i++){
                 for (int j = 0; j < vertex_mat.shape()[1]; j++){
	                 if(!isSaturated(vertex_mat[i][j].identity[0], vertex_mat[i][j].identity[1])){
	                     PrintNeighbours(vertex_mat[i][j].identity);
	                 }               
                 }
                 cout << endl;
             } 
             
             cout << endl;*/
             
             if (getVertex(match_coordinate[0],match_coordinate[1]).unmatched_neighbours.size() > 0) {

                 next_unmatch_coordinate = chooseUnmatchedNeighbour(match_coordinate);
                 //cout << "Unmatched Neighbour: (" << next_unmatch_coordinate[0] << ", " << next_unmatch_coordinate[1] << ")" << endl;
                 // Unmatch existing coordinates 
                 remove_matching(unmatch_coordinate, match_coordinate);
                 
                 // Check if this neighbour is saturated             
                 saturated = isSaturated(next_unmatch_coordinate[0],next_unmatch_coordinate[1]);
             
                 unmatch_coordinate = next_unmatch_coordinate; 
                 
                 

             } else {
                 // This is a contingency that is called when there are no more unmatched vertices 
                 //    (can only happen at corners). Essentially, this operation will not result in a change in the number
                 //    of matched edges 
                 
                 // Still need to remove a matching to avoid accidentally creating a three-vertex system
                 remove_matching(unmatch_coordinate, match_coordinate);

                 // we set saturated to true to exit the while loop. The success flag is set to false.             
                 saturated = true;
                 success = false;

             }           
         }
         
         // cout << "RIGHT" << endl;
         if (success) { // if contingency was not called, then we can create the final edge. 
             create_matching(match_coordinate, unmatch_coordinate);
         }
     }
     // Always check that all vertices are indeed saturated
     check2Matching();
     
     zero_node.matched_neighbours.push_back({1,-1});
     one_node.matched_neighbours.push_back({0,-1});
}

void CompactSaw::check2Matching(){
    int counter = 0;
    for (int i = 0; i < vertex_mat.shape()[0]; i++){
        for (int j = 0; j < vertex_mat.shape()[1]; j++){
            if (!isSaturated(i,j)) {
                throw runtime_error("Not all Vertices are saturated");
            }
        }
    }
}

/* DEBUG FUNCTIONS:
         cout << endl;
         cout << "Before: " << endl;
         PrintNeighbours(unsat_coordinate);
         cout << endl;
         PrintNeighbours(neighbour_coordinate);
         cout << endl;
         
         cout << "After: " << endl;
         PrintNeighbours(unsat_coordinate);
         cout << endl;
         PrintNeighbours(neighbour_coordinate);
         cout << endl; */

void CompactSaw::PrintNeighbours(Int2D unsat_coordinate){
    cout << unsat_coordinate[0] << ", " << unsat_coordinate[1] << endl;
    cout << "unmatched neighbours:" << endl;
    for (int i = 0; i < getVertex(unsat_coordinate[0], unsat_coordinate[1]).unmatched_neighbours.size(); i++){
     cout << getVertex(unsat_coordinate[0],unsat_coordinate[1]).unmatched_neighbours[i][0] << ", " 
         << getVertex(unsat_coordinate[0],unsat_coordinate[1]).unmatched_neighbours[i][1] << "; ";
    }
    cout << endl;
    cout << "matched neighbours:" << endl;
    for (int i = 0; i < getVertex(unsat_coordinate[0], unsat_coordinate[1]).matched_neighbours.size(); i++){
     cout << getVertex(unsat_coordinate[0], unsat_coordinate[1]).matched_neighbours[i][0] << ", " 
         << getVertex(unsat_coordinate[0], unsat_coordinate[1]).matched_neighbours[i][1] << "; ";
    }
    cout << endl;
}

Vertex CompactSaw::getVertex(int x, int y) {
    if (y > -1) {
        return(vertex_mat[x][y]);
    } else if (x == 0) {
        return(zero_node);
    } else if (x == 1) {
        return(one_node);
    } else {
        throw("Vertex not found!");
    }
}

void CompactSaw::assignSubcycle(int x, int y, int sub_cycle) {
    if (y > -1) {
        vertex_mat[x][y].subcycle = sub_cycle;
    } else if (x == 0) {
        zero_node.subcycle = sub_cycle;
    } else if (x == 1) {
        one_node.subcycle = sub_cycle;
    } else {
        throw runtime_error("Vertex not found!");
    }
}

bool CompactSaw::isSaturated(int x, int y) {
    if (y > -1) {
        return(getVertex(x,y).matched_neighbours.size() == 2);
    } else if (x == 0) {
        return(zero_node.matched_neighbours.size() == 1);
    } else if (x == 1) {
        return(one_node.matched_neighbours.size() == 1);
    } else {
        throw("Vertex not found!");
    }
}


void CompactSaw::ResetLocations(){
    
    for (int i = 0; i < vertex_vector.size(); i++){
        if (vertex_vector[i][1] > -1) {
            vertex_mat_locations[vertex_vector[i][0]][vertex_vector[i][1]] = i;
        } else if (vertex_vector[i][0] == 0) {
            zero_location = i;
        } else if (vertex_vector[i][0] == 1){
            one_location = i;
        } else {
            throw runtime_error("Reset Location Failed");
        }
    }
}

void CompactSaw::create_matching(Int2D coordinate_1, Int2D coordinate_2){
    
    Vertex *Vertex_1_pointer; // Create pointers to simplify match creation
    Vertex *Vertex_2_pointer;
    int threshold_1;
    int threshold_2;
    if (coordinate_1[1] == -1) {
        if (coordinate_1[0] == 0) {
            Vertex_1_pointer = &zero_node; 
            threshold_1 = 0;
        } else if (coordinate_1[0] == 1) {
            Vertex_1_pointer = &one_node; 
            threshold_1 = 0;
        } else {
            throw("Invalid coordinate 1 for match creation!");
        }
    } else {
        Vertex_1_pointer = &vertex_mat[coordinate_1[0]][coordinate_1[1]];
        threshold_1 = 1;
    }
    
    if (coordinate_2[1] == -1) {
        if (coordinate_2[0] == 0) {
            Vertex_2_pointer = &zero_node; 
            threshold_2 = 0;
        } else if (coordinate_2[0] == 1) {
            Vertex_2_pointer = &one_node; 
            threshold_2 = 0;
        } else {
            throw("Invalid coordinate 2 for match creation!");
        }
    } else {
        Vertex_2_pointer = &vertex_mat[coordinate_2[0]][coordinate_2[1]];
        threshold_2 = 1;
    }
    
    int sz_1 = (*Vertex_1_pointer).matched_neighbours.size();
    int sz_2 = (*Vertex_2_pointer).matched_neighbours.size();
    
    (*Vertex_1_pointer).to_matched(coordinate_2);
    (*Vertex_2_pointer).to_matched(coordinate_1);
    
    // NEED TO CHANGE!!
    if (sz_1 == threshold_1 ) {
        unsaturated_count--;
    } 
    
    if (sz_2 == threshold_2 ) {
        unsaturated_count--;
    } 

}

int CompactSaw::getLocation(int x, int y) {
    if (y > -1) {
        return(vertex_mat_locations[x][y]);
    } else if (x == 0) {
        return(zero_location);
    } else if (x == 1) {
        return(one_location);
    } else {
        throw("Vertex not found!");
    }
}

void CompactSaw::findSubcycles(){
    
    Int2D cycle_starting_point;
    Int2D last_coordinate;
    Int2D current_coordinate;
    Int2D next_coordinate;
    Int2D temp_coordinate;
    bool success;
    int sub_cycle = 0;
    
    while (vertex_vector.size() != 0) {
        cycle_starting_point = vertex_vector[0];
        current_coordinate = chooseMatchedNeighbour(cycle_starting_point);
        assignSubcycle(current_coordinate[0], current_coordinate[1], sub_cycle);
        int offset = getLocation(cycle_starting_point[0],cycle_starting_point[1]);
        vertex_vector.erase(vertex_vector.begin() + offset);
        ResetLocations();
        last_coordinate = cycle_starting_point;
        while ((current_coordinate[0] != cycle_starting_point[0]) || (current_coordinate[1] != cycle_starting_point[1])) {
            success = false;
            for (int i = 0; i < 2 ;i++){
                temp_coordinate = getVertex(current_coordinate[0],current_coordinate[1]).matched_neighbours[i];
                if( (temp_coordinate[0] != last_coordinate[0])
                       ||  (temp_coordinate[1] != last_coordinate[1])){
                    next_coordinate = temp_coordinate;
                    last_coordinate = current_coordinate;
                    success = true;
                    break;
                }
            }     
            
            int offset = getLocation(current_coordinate[0],current_coordinate[1]);
            vertex_vector.erase(vertex_vector.begin() + offset);
            ResetLocations();
            current_coordinate = next_coordinate;
            assignSubcycle(current_coordinate[0], current_coordinate[1], sub_cycle);
        }
        sub_cycle++;
    }
    
    patch_count = sub_cycle;
}

void CompactSaw::createBorderMatrix(){
    boostmat<vector<array<Int2D,2>>,2> horizontal_border_matrix; // The borders run along the horizontal dimension, 
                                                                   //so there is no change in horizontal dimension between 
                                                                      // neighbours from different subcycles   
    horizontal_border_matrix.resize(boost::extents[patch_count][patch_count]);               
    boostmat<vector<array<Int2D,2>>,2> vertical_border_matrix;
    vertical_border_matrix.resize(boost::extents[patch_count][patch_count]);
    
    Int2D neighbour_coord;
    int neighbour_subcycle;
    int subcycle_1;
    int subcycle_2;
    array<Int2D,2> temp_array;
    array<array<Int2D,2>,2> temp_temp_array;
    
    for (int i = 0; i < vertex_mat.shape()[0]; i++){
        for (int j = 0; j < vertex_mat.shape()[1]; j++){
            for (int k = 0; k < vertex_mat[i][j].unmatched_neighbours.size(); k++) {
                neighbour_coord = vertex_mat[i][j].unmatched_neighbours[k];
                neighbour_subcycle = getVertex(neighbour_coord[0], neighbour_coord[1]).subcycle;
                if((neighbour_subcycle != vertex_mat[i][j].subcycle) && (neighbour_coord[1] != -1) && 
                            (neighbour_coord[0]+neighbour_coord[1] < i+j)) {
                        subcycle_1 = min(vertex_mat[i][j].subcycle, neighbour_subcycle);
                        subcycle_2 = max(vertex_mat[i][j].subcycle, neighbour_subcycle);
                        
                        if (neighbour_coord[0] - i == 0) {
                            if (j < neighbour_coord[1]) {
                                temp_array[0] = {i,j};
                                temp_array[1] = neighbour_coord;
                            } else {
                                temp_array[0] = neighbour_coord;
                                temp_array[1] = {i,j};
                            }
                            
                            vertical_border_matrix[subcycle_1][subcycle_2].push_back(temp_array);
                        }
                        
                        if (neighbour_coord[1] - j == 0) {
                            if (i < neighbour_coord[0]) {
                                temp_array[0] = {i,j};
                                temp_array[1] = neighbour_coord;
                            } else {
                                temp_array[0] = neighbour_coord;
                                temp_array[1] = {i,j};
                            }
                            horizontal_border_matrix[subcycle_1][subcycle_2].push_back(temp_array);
                        }
                        
                    }
            }
        }
    } 
    
    for (int i = 0; i < horizontal_border_matrix.shape()[0]; i++) {
        for (int j = 0; j < horizontal_border_matrix.shape()[1]; j++) {
            sort (horizontal_border_matrix[i][j].begin(), horizontal_border_matrix[i][j].end(), by_vertical_first);
            for (int k = 0; k < horizontal_border_matrix[i][j].size(); k++) {
                if (k != horizontal_border_matrix[i][j].size()-1){
                    if ((horizontal_border_matrix[i][j][k][0][0] == horizontal_border_matrix[i][j][k+1][0][0]) &&
                            (horizontal_border_matrix[i][j][k][0][1]+1 == horizontal_border_matrix[i][j][k+1][0][1])
                            && are_neighbours(horizontal_border_matrix[i][j][k][0], horizontal_border_matrix[i][j][k+1][0])
                            && are_neighbours(horizontal_border_matrix[i][j][k][1], horizontal_border_matrix[i][j][k+1][1])){
                         temp_temp_array[0] = horizontal_border_matrix[i][j][k];
                         temp_temp_array[1] = horizontal_border_matrix[i][j][k+1];
                         
                         patchable_set.push_back(PatchableCoordinates(temp_temp_array,{i,j},0));
                    }
                }
            }
        }
    }
    for (int i = 0; i < vertical_border_matrix.shape()[0]; i++) {
        for (int j = 0; j < vertical_border_matrix.shape()[1]; j++) {
            sort (vertical_border_matrix[i][j].begin(), vertical_border_matrix[i][j].end(), by_horizontal_first);
            for (int k = 0; k < vertical_border_matrix[i][j].size(); k++) {
                if (k != vertical_border_matrix[i][j].size()-1){
                    if ((vertical_border_matrix[i][j][k][0][0] == vertical_border_matrix[i][j][k+1][0][0]) &&
                            (vertical_border_matrix[i][j][k][0][1]+1 == vertical_border_matrix[i][j][k+1][0][1])
                            && are_neighbours(vertical_border_matrix[i][j][k][0], vertical_border_matrix[i][j][k+1][0])
                            && are_neighbours(vertical_border_matrix[i][j][k][1], vertical_border_matrix[i][j][k+1][1])){
                         temp_temp_array[0] = vertical_border_matrix[i][j][k];
                         temp_temp_array[1] = vertical_border_matrix[i][j][k+1];
                         patchable_set.push_back(PatchableCoordinates(temp_temp_array,{i,j},0));
                    }
                }
            }
        }
    }
    
}

bool CompactSaw::Patch(){
    double rand_no = uniform_real_distribution<double>(0.0,1.0)(mersenne_generator);
    int running_patch = patch_count;
    bool complete = false;
    vector<set<int>> subcycle_equivalence_classes; // each sub-vector contains all the 
                                                         // subcycles that have already been combined
                                                            // into that new massive subcycle.
    int subcycle_equiv_1;
    int subcycle_equiv_2;
    
    Int2D current_subcycle;
    vector<PatchableCoordinates> ones_to_patch; 
    
    for(int m = 0; m < patch_count; m++) {
    
        int counter = 0;                        
        
        for (int i = 0; i < patchable_set.size(); i++){
            counter++;    
            if ( counter >= rand_no*double(patchable_set.size())){
                
                create_matching(patchable_set[i].coords[0][0],patchable_set[i].coords[0][1]);
                create_matching(patchable_set[i].coords[1][0],patchable_set[i].coords[1][1]);
                remove_matching(patchable_set[i].coords[0][0],patchable_set[i].coords[1][0]);
                remove_matching(patchable_set[i].coords[0][1],patchable_set[i].coords[1][1]);
          
                coordinate_used_up[patchable_set[i].coords[0][0][0]][patchable_set[i].coords[0][0][1]] = true;
                coordinate_used_up[patchable_set[i].coords[0][1][0]][patchable_set[i].coords[0][1][1]] = true;
                coordinate_used_up[patchable_set[i].coords[1][0][0]][patchable_set[i].coords[1][0][1]] = true;
                coordinate_used_up[patchable_set[i].coords[1][1][0]][patchable_set[i].coords[1][1][1]] = true;

                subcycle_equiv_1 = -1;
                subcycle_equiv_2 = -1;
                
                for (int n = 0; n < subcycle_equivalence_classes.size(); n++){
                    // cout << subcycle_equivalence_classes[n].count(patchable_set[i].subcycles[0]) << endl;
                    if (subcycle_equivalence_classes[n].count(patchable_set[i].subcycles[0]) 
                        > 0){
                        // subcycle_equivalence_classes[n].end()
                        subcycle_equiv_1 = n;    
                        /*cout << patchable_set[i].subcycles[0] << " was found in " << n << endl;
                        cout << n << " has the following members: " << endl;
                        for (auto sub: subcycle_equivalence_classes[n]) {
                            cout << sub << ", ";
                        }
                        cout << endl;*/
                    }
                    // cout << subcycle_equivalence_classes[n].count(patchable_set[i].subcycles[1]) << endl;
                    if (subcycle_equivalence_classes[n].count(patchable_set[i].subcycles[1]) 
                        > 0) {
                        subcycle_equiv_2 = n;
                        /*cout << patchable_set[i].subcycles[1] << " was found in " << n << endl;
                        cout << n << " has the following members: " << endl;
                        for (auto sub: subcycle_equivalence_classes[n]) {
                            cout << sub << ", ";
                        }
                        cout << endl;*/
                    }                   
                    
                }
                
                current_subcycle = patchable_set[i].subcycles;
                reviewPatchable(current_subcycle);
                
                // cout << subcycle_equiv_1 << "/" << (subcycle_equiv_1 > -1) << endl;
                // cout << subcycle_equiv_2 << "/" << (subcycle_equiv_2 > -1) << endl;
                
                if ((subcycle_equiv_1 == subcycle_equiv_2) && (subcycle_equiv_1 > -1)) {
                    throw runtime_error(string("Joining two of the same subcycles ") + to_string(subcycle_equiv_1) 
                        + string(" and ") + to_string(subcycle_equiv_2));
                }
                
                if (subcycle_equiv_1 > -1 && subcycle_equiv_2 >-1) {
                    for (auto sub_1 : subcycle_equivalence_classes[subcycle_equiv_1]){
                        for (auto sub_2 : subcycle_equivalence_classes[subcycle_equiv_2]){
                            reviewPatchable({min(sub_1,sub_2),max(sub_1,sub_2)});
                        }
                    }
                    set<int> tempset;
                    set_union(subcycle_equivalence_classes[subcycle_equiv_1].begin(), subcycle_equivalence_classes[subcycle_equiv_1].end(),
                        subcycle_equivalence_classes[subcycle_equiv_2].begin(), subcycle_equivalence_classes[subcycle_equiv_2].end(),
                        std::inserter(tempset, tempset.begin()));
                    // cout << subcycle_equiv_1 << ", " << subcycle_equiv_2 << endl;
                    for (auto sub : tempset){
                       // cout << sub << endl;
                    }
                    subcycle_equivalence_classes[subcycle_equiv_1] = tempset;
                    // cout << "An erasure occurs" << endl;
                    subcycle_equivalence_classes.erase(subcycle_equivalence_classes.begin()+subcycle_equiv_2);    
                } else if (subcycle_equiv_1 > -1) {
                    for (auto sub : subcycle_equivalence_classes[subcycle_equiv_1]){
                        reviewPatchable({min(sub, current_subcycle[1]), max(sub, current_subcycle[1])});
                    }
                    // cout << "Insert: " << patchable_set[i].subcycles[1] << " to " << subcycle_equiv_1 << endl;
                    subcycle_equivalence_classes[subcycle_equiv_1].insert(current_subcycle[1]);
                } else if (subcycle_equiv_2 > -1) {
                    for (auto sub: subcycle_equivalence_classes[subcycle_equiv_2]){
                        reviewPatchable({min(sub, current_subcycle[0]), max(sub, current_subcycle[0])});
                    }
                    // cout << "Insert: " << patchable_set[i].subcycles[0] << " to " << subcycle_equiv_2 << endl;
                    subcycle_equivalence_classes[subcycle_equiv_2].insert(current_subcycle[0]);
                } else {
                    set<int> tempset;
                    tempset.insert(current_subcycle[0]);
                    // cout << current_subcycle[0] << " was inserted." << endl;
                    tempset.insert(current_subcycle[1]);
                    // cout << current_subcycle[1] << " was inserted."<< endl;
                    subcycle_equivalence_classes.push_back(tempset);
                }
                
                break;
            }
        }
   }
   return( (subcycle_equivalence_classes.size() == 1) && (subcycle_equivalence_classes[0].size() == patch_count) );
}

void CompactSaw::reviewPatchable(Int2D subcycle_set){

    patchable_set.erase(
        remove_if(
            patchable_set.begin(), 
            patchable_set.end(),
            [c = coordinate_used_up, s = subcycle_set](PatchableCoordinates const & p) { 
                return (c[p.coords[0][0][0]][p.coords[0][0][1]] || c[p.coords[0][1][0]][p.coords[0][1][1]]
                    || c[p.coords[1][0][0]][p.coords[1][0][1]] || c[p.coords[1][1][0]][p.coords[1][1][1]] || 
                    (p.subcycles[0] == s[0] && p.subcycles[1] == s[1])); 
            }
         ), 
         patchable_set.end()
    ); 
}

void CompactSaw::remove_matching(Int2D coordinate_1, Int2D coordinate_2){
    int threshold_1;
    int threshold_2;
    
    bool one_flag = false; // error flags that raise an error if both coordinates are 
                               // either zero_node or one_node (the matching between them should never be destroyed)
    bool two_flag = false;
    
    int sz_1;
    int sz_2;
    
    if (coordinate_1[1] == -1) {
        if (coordinate_1[0] == 0) {
            sz_1 = zero_node.matched_neighbours.size();
            zero_node.to_unmatched(coordinate_2);
            threshold_1 = 1;
        } else if (coordinate_1[0] == 1) {
            sz_1 = one_node.matched_neighbours.size();
            one_node.to_unmatched(coordinate_2);
            threshold_1 = 1;
        } else {
            throw runtime_error("Invalid coordinate 1 for match creation!");
        }
        one_flag = true;
    } else {
        sz_1 = vertex_mat[coordinate_1[0]][coordinate_1[1]].matched_neighbours.size();
        vertex_mat[coordinate_1[0]][coordinate_1[1]].to_unmatched(coordinate_2);
        threshold_1 = 2;
    }

    if (coordinate_2[1] == -1) {
        if (coordinate_2[0] == 0) {
            sz_2 = zero_node.matched_neighbours.size();
            zero_node.to_unmatched(coordinate_1);
            threshold_2 = 1;
        } else if (coordinate_2[0] == 1) {
            sz_2 = one_node.matched_neighbours.size();
            one_node.to_unmatched(coordinate_1);
            threshold_2 = 1; 
        } else {
            throw runtime_error("Invalid coordinate 2 for match creation!");
        }
        two_flag = true;
    } else {
        sz_2 = vertex_mat[coordinate_2[0]][coordinate_2[1]].matched_neighbours.size();
        vertex_mat[coordinate_2[0]][coordinate_2[1]].to_unmatched(coordinate_1);
        threshold_2 = 2;
    }
    
    if (one_flag && two_flag) {
        throw("Removal of the match between one_node and two_node not permitted!");
    }

    if (sz_1 == threshold_1 ) {
        unsaturated_count++;
    } 
    
    if (sz_2 == threshold_2 ) {
        unsaturated_count++;
    } 
}

bool CompactSaw::are_neighbours(Int2D coordinate_1, Int2D coordinate_2){
    vector<Int2D> neighbours_vec = getVertex(coordinate_1[0], coordinate_1[1]).matched_neighbours;
    for (int i = 0; i < neighbours_vec.size() ; i++) {
        if ((neighbours_vec[i][0] == coordinate_2[0]) && (neighbours_vec[i][1] == coordinate_2[1])) {
            return(true);
        }
    }
    return(false);
}


void CompactSaw::PrintSubcycles(){
     for (int i = 0; i < vertex_mat.shape()[0]; i++){
        for (int j = 0; j < vertex_mat.shape()[1]; j++){
            if (vertex_mat[i][j].subcycle < 10) {
	            cout << " " << vertex_mat[i][j].subcycle << " " << flush;
	        } else {
	            cout << vertex_mat[i][j].subcycle << " " << flush;
	        }                   
        }
        cout << endl;
    } 
}

void CompactSaw::constructLattice(){
     final_lattice.resize(boost::extents[nrow+2][ncol+2]);
     std::fill_n(final_lattice.data(), final_lattice.num_elements(), -1);
     Int2D point = zero_node.identity;
     Int2D next_point;
     Int2D last_point = {1,-1};
     for(int i = 0; i< nrow*ncol; i++) {
         next_point = getVertex(point[0],point[1]).matched_neighbours[0];
         // cout << "Next point guess: (" << next_point[0] << ", " << next_point[1] << ")" << endl;
         if ( (next_point[0] != last_point[0]) || (next_point[1] != last_point[1]) ) {
             last_point = point;
         } else {
             next_point = getVertex(point[0],point[1]).matched_neighbours[1];
             last_point = point;
         }
         // cout << endl;
         // cout << "Last point: (" << last_point[0] << ", " << last_point[1] << ")" << endl;
         // PrintNeighbours(next_point);
         final_lattice[next_point[0]+1][next_point[1]+1] = i;
         point = next_point;
     }
}

void CompactSaw::printLattice(){
    int max_digits = 1;
    int digits;
    // find max_Digits
    for (int i = 0; i != final_lattice.shape()[0]; i++){
        for (int j = 0; j != final_lattice.shape()[1]; j++){
            if (final_lattice[i][j] > 0) {
                if ((int(log10(final_lattice[i][j])) + 1) > max_digits){
                    max_digits = (int(log10(final_lattice[i][j])) + 1);
                }
            }
        }
    }
    
    for (int i = 0; i != final_lattice.shape()[0]; i++){
        for (int j = 0; j != final_lattice.shape()[1]; j++){
            if(final_lattice[i][j] > 0) {
                digits = max_digits - (int(log10(final_lattice[i][j])) + 1);
            } else if (final_lattice[i][j] == 0){
                digits = max_digits - 1;
            } else {
                digits = max_digits - 2;
            }
            for (int i = 0; i < digits; i++) {
                cout << " " << flush;
            }
	        cout << final_lattice[i][j] << " " << flush;
        }
        cout << endl;
    }
}

/******************************************
    END CLASS COMPACTSAW
******************************************/

/******************************************
    START CLASS BLOCK
******************************************/

class Block {
    
    public:
    vector<int> face_identities;
    vector<int> non_zero_open_faces;
    bool filled;
    
    Block();
    void start_algorithm();
    bool check_consistency(vector<int> filled, Block next_block);
    int get_identifier(); // get a 4 digit integer that is invariant to block orientation
};

Block::Block(){
    // {up,right,down,left}
    face_identities = {-1,-1,-1,-1};
    filled = false; 
}

bool Block::check_consistency(vector<int> filled, Block next_block) {
    // This does all checks EXCEPT checking that all neighbouring faces match
    
    // Check that the one and two are consistent with each other
    int two_place;
    
    if (std::find(face_identities.begin(), face_identities.end(), 1) == face_identities.end()) {
        cout << "No face 1!" << endl;
        return(false);
    } 
    
    if (std::find(face_identities.begin(), face_identities.end(), 2) == face_identities.end()) {
        cout << "No face 2!" << endl;
        return(false);
    } else {
        two_place = find(face_identities.begin(), face_identities.end(), 2) - face_identities.begin();
    }
    
    int new_block_one_place = flip_direction(two_place);
    
    if (next_block.face_identities[new_block_one_place] != 1) {
        cout << "Face 1 and face 2 locations not matching!" << endl; 
        return(false);
    }
    
    //count to ensure there are only exctly two faces equal to 1 or 2 
    // other conditions check if an unfilled side is present in the next
    // as well as if 0 faces are facing empty 
    
    int count = 0;
    for (int i = 0; i < 4 ;i++) {
        if (face_identities[i] > 2) {
             count = count + 1;
            //need to check if it is in the filled vector
            if (std::find(filled.begin(), filled.end(), i) == filled.end()) {
               // If it is NOT in the filled vector
                // check if this side is present in the next
                    if (std::find(next_block.face_identities.begin(), next_block.face_identities.end(), face_identities[i]) != next_block.face_identities.end()) {
                        cout << "Face " << i << " with identity " << face_identities[i] << " is open and repeated." << endl; 
                        return(false);
                    }
            }
        } else if (face_identities[i] == 0) {
            count = count + 1;
            // need to check that any 0 faces have no neighbour
            
            //filled contains all those with a non-zero neighbour
            // return false if i is found (which is !=)
            if (std::find(filled.begin(), filled.end(), i) != filled.end()) {
                cout << "A face 0 has a neighbour" << endl;
                return(false);
            }
        }
    }
    
    return(true);
    
    if (count != 2) {
        return(false);
    }
}

int Block::get_identifier(){
    if ( (find(face_identities.begin(),face_identities.end(),-1) != face_identities.end()) && ((face_identities[0] + face_identities[1] + face_identities[2] + face_identities[3]) > -4) ) {
        throw runtime_error("Some unlabelled faces in non-empty blocks");
    }
    
    vector<int> orientations(4);
    
    int top_neighbour = face_identities[0];
    int right_neighbour = face_identities[1];
    int bot_neighbour = face_identities[2];
    int left_neighbour = face_identities[3];
    
    orientations[0] = top_neighbour*1000+right_neighbour*100+bot_neighbour*10+left_neighbour;
    orientations[1] = left_neighbour*1000+top_neighbour*100+right_neighbour*10+bot_neighbour;      
	orientations[2] = bot_neighbour*1000+left_neighbour*100+top_neighbour*10+right_neighbour;
	orientations[3] = right_neighbour*1000+bot_neighbour*100+left_neighbour*10+top_neighbour;  
    
	int maximum
	 = -9999;
	for (int i = 0; i <4; i++) {
	    if (orientations[i] > maximum){
		    maximum = orientations[i];
	    }
	}
	
	return(maximum);
}

/******************************************
    END CLASS BLOCK
******************************************/

boostmat<int,2> generateWalk(int row_size, int col_size){
    boostmat<int,2> walk_matrix;

    bool correct = false;

    walk_matrix.resize(boost::extents[row_size+2][col_size+2]);
    
    while (!correct) {
        CompactSaw compact_saw(row_size,col_size);
        // cout << " with seed: " << compact_saw.rand_seed << endl;
        compact_saw.get2Matching();
        compact_saw.findSubcycles();
        compact_saw.createBorderMatrix();
        // compact_saw.PrintSubcycles();
        correct = compact_saw.Patch();
        // cout << "ARCHENEMY" << endl;
        if (correct) {
            // cout << "Patching successful" << endl;
            compact_saw.constructLattice();
            walk_matrix = compact_saw.final_lattice;
            // compact_saw.printLattice();
            break;
        } 
        
        /*else {
            cout << "Patching failed" << endl;
        }*/
    }
   
   return(walk_matrix);
}

boostmat<Block,2> GetBlockIdentities(boostmat<int,2> walk_matrix, int structure_size) {

    int row_size = walk_matrix.shape()[0] - 2;
    int col_size = walk_matrix.shape()[1] - 2;

    boostmat<Block,2> block_matrix;
    block_matrix.resize(boost::extents[row_size+2][col_size+2]);
    //find_zero_location
    Int2D location;
    Int2D start_loc;
    Int2D next_location;
    Int2D previous_location;
    int next_direction;
    int previous_direction;
    vector<int> directions;

    for (int i = 0; i < row_size+2; i++) {
        for (int j = 0; j < col_size+2; j++) {
            if (walk_matrix[i][j] == 0) {
                start_loc = {i,j};
                break;
            }
        }
    }

    location = start_loc;
    next_direction = getNextLocation(start_loc, walk_matrix);
    next_location = getcoordinate(location,next_direction);
    directions = getFinalNeighboursStart(location,next_location,walk_matrix);
    for (int i = 0; i < 4; i++) {
       if (std::find(directions.begin(), directions.end(), i) != directions.end()) {
            block_matrix[location[0]][location[1]].face_identities[i] = 3;
        }
        else if (i == next_direction){
            block_matrix[location[0]][location[1]].face_identities[i] = 2;
        } else {
            block_matrix[location[0]][location[1]].face_identities[i] = 0;
        }
    }

    int maximum = 3;
    for (int k = 1; k < structure_size-1; k++) {

       vector<int> unknown_faces;
       // Recentering reference frame to current location
       previous_direction = flip_direction(next_direction);
       previous_location = location;
       location = next_location;
       next_direction = getNextLocation(location, walk_matrix);
       next_location = getcoordinate(location,next_direction); 
       // Decide what face to use  
       directions = getFinalNeighbours(location, previous_location, next_location, walk_matrix);
       /*cout << k << ": {" << flush;
       for (int i = 0; i < directions.size(); i++) {
           cout << directions[i] << " " << flush;
       }
       cout << "}" << endl;
       */
       for (int i = 0; i < 4; i++) {
            if (std::find(directions.begin(), directions.end(), i) != directions.end()) {
                // get the direction to the current block from the frame of the neighbouring block
                int opposite_direction = flip_direction(i);
                //convert the direction to a coordinate location of the neighbour
                Int2D neighbour_coord = getcoordinate(location,i);
                if (block_matrix[neighbour_coord[0]][neighbour_coord[1]].face_identities[opposite_direction] > -1) {
                    block_matrix[location[0]][location[1]].face_identities[i] = block_matrix[neighbour_coord[0]][neighbour_coord[1]].face_identities[opposite_direction];
                } else {
                    unknown_faces.push_back(i);
                }
            }
            else if (i == next_direction){
                block_matrix[location[0]][location[1]].face_identities[i] = 2;
            } else if (i == previous_direction) {
                block_matrix[location[0]][location[1]].face_identities[i] = 1;
            } else {
                block_matrix[location[0]][location[1]].face_identities[i] = 0;
            }
       }
       
       set<int> unallowable_faces;
       
       // GET A LIST OF UNALLOWABLE FACES FROM PREVIOUS BLOCK
       //cout << k <<  " (previous forbiddens) : " << flush;
       for (int i = 0; i < 4; i++) {
           if (block_matrix[previous_location[0]][previous_location[1]].face_identities[i] > 2) {
               Int2D neighbour = getcoordinate(previous_location,i); 
               int opposite_direction = flip_direction(i);
               if (block_matrix[neighbour[0]][neighbour[1]].face_identities[opposite_direction] == -1) {
                   unallowable_faces.insert(block_matrix[previous_location[0]][previous_location[1]].face_identities[i]);
                   //cout << block_matrix[previous_location[0]][previous_location[1]].face_identities[i] << ", " << flush;
               }
           }
       }
       //cout << endl;
       
       // GET A LIST OF UNALLOWABLE FACES FROM THE NEXT BLOCK
       Int2D next_next_location;
       if (k != structure_size-2) {
           next_next_location = getcoordinate(next_location,getNextLocation(next_location, walk_matrix));
       } else {
           next_next_location = {0,0}; // This coordinate is inaccesible for folding, 
           // and is simply used to 'trick' the program into thinking there is no next next location  
       }
       vector<int> next_neighbour_directions = getFinalNeighbours(next_location, location, next_next_location, walk_matrix);
       /*for (int i = 0; i < next_neighbour_directions.size(); i++) {
           cout << next_neighbour_directions[i] << endl;
       }*/
       
       //Find Neighbours
       for (int i = 0; i < next_neighbour_directions.size(); i++) {
           int opposite_direction = flip_direction(next_neighbour_directions[i]);
           Int2D neighbour_coord = getcoordinate(next_location,next_neighbour_directions[i]);
           if (block_matrix[neighbour_coord[0]][neighbour_coord[1]].face_identities[opposite_direction] > 2) {
               unallowable_faces.insert(block_matrix[neighbour_coord[0]][neighbour_coord[1]].face_identities[opposite_direction]);
           }
       }
       
       bool found = false;
       //cout << k << " (conclusion) : " << flush;
       for (int i = 3; i < 7; i++) {
           if (find(unallowable_faces.begin(), unallowable_faces.end(), i) == unallowable_faces.end()) {
               //cout << i << endl;
               for (int j = 0; j < unknown_faces.size(); j++) {
                   block_matrix[location[0]][location[1]].face_identities[unknown_faces[j]] = i;
               }
               
               if (i > maximum) {
                   maximum = i;
               }
               
               found = true;
               break;
           }
       }
       
       if (!found) {
           throw runtime_error("Proof Failed: More than 4 non-backbone bonds are needed");
       }
   
    }

    //The final location need special treatment
    vector<int> unknown_faces;
    previous_location = location;
    location = next_location;
    previous_direction = flip_direction(next_direction);
    directions = getFinalNeighboursEnd(location,previous_location,walk_matrix);
    for (int i = 0; i < 4; i++) {
       if (std::find(directions.begin(), directions.end(), i) != directions.end()) {
           int opposite_direction = flip_direction(i);
                //convert the direction to a coordinate location of the neighbour
           Int2D neighbour_coord = getcoordinate(location,i);
           if (block_matrix[neighbour_coord[0]][neighbour_coord[1]].face_identities[opposite_direction] > -1) {
               block_matrix[location[0]][location[1]].face_identities[i] = block_matrix[neighbour_coord[0]][neighbour_coord[1]].face_identities[opposite_direction];
           } else {
               unknown_faces.push_back(i);
           }
       } else if (i == previous_direction){
           block_matrix[location[0]][location[1]].face_identities[i] = 1;
       } else {
           block_matrix[location[0]][location[1]].face_identities[i] = 0;
       }
    }

    set<int> unallowable_faces;
    // GET A LIST OF UNALLOWABLE FACES FROM PREVIOUS BLOCK
    for (int i = 0; i < 4; i++) {
       if (block_matrix[previous_location[0]][previous_location[1]].face_identities[i] > 2) {
           Int2D neighbour = getcoordinate(previous_location,i); 
           int opposite_direction = flip_direction(i);
           if (block_matrix[neighbour[0]][neighbour[1]].face_identities[opposite_direction] == -1) {
               unallowable_faces.insert(block_matrix[previous_location[0]][previous_location[1]].face_identities[i]);
           }
       }
    }

    bool found = false;
    for (int i = 3; i < 7; i++) {
      if (find(unallowable_faces.begin(), unallowable_faces.end(), i) == unallowable_faces.end()) {
          for (int j = 0; j < unknown_faces.size(); j++) {
              block_matrix[location[0]][location[1]].face_identities[unknown_faces[j]] = i;
          }
          found = true;
          break;
      }
    }
    
    return(block_matrix);

    //Printing to see there are no nasty surprises

    /*location = start_loc;
    for (int k = 0; k < structure_size; k++) {
       cout << k << ": " << "{" << flush;
       for (int i = 0; i < 3; i ++) {
           cout << block_matrix[location[0]][location[1]].face_identities[i] << ", " << flush;
       } 
       cout << block_matrix[location[0]][location[1]].face_identities[3] << "}" << endl;
       if (k < structure_size - 1) {
           next_direction = getNextLocation(location, walk_matrix);
           location = getcoordinate(location,next_direction); 
       }
    }
    */

    // cout << "The maximum face is " << maximum << endl;
}

void ValidateWalk(boostmat<int,2> walk_matrix, boostmat<Block,2> block_matrix, int structure_size) {
    int row_size = walk_matrix.shape()[0] - 2;
    int col_size = walk_matrix.shape()[1] - 2;
    
    Int2D start_loc;
    for (int i = 0; i < row_size+2; i++) {
        for (int j = 0; j < col_size+2; j++) {
            if (walk_matrix[i][j] == 0) {
                start_loc = {i,j};
                break;
            }
        }
    }
    
    boostmat<Block,2> block_validator_matrix;
    block_validator_matrix.resize(boost::extents[row_size+2][col_size+2]);
    // We test out assembly and make sure that everything assembles correctly.
    Int2D location = start_loc;
    block_validator_matrix[location[0]][location[1]] =  block_matrix[location[0]][location[1]];
    int next_direction = getNextLocation(location, walk_matrix);
    Int2D next_location = getcoordinate(location,next_direction); 
    block_validator_matrix[next_location[0]][next_location[1]] =  block_matrix[next_location[0]][next_location[1]];
    for (int k = 1; k < structure_size-1; k++) {
       location = next_location;
       next_direction = getNextLocation(location, walk_matrix);
       next_location = getcoordinate(location,next_direction);
       block_validator_matrix[next_location[0]][next_location[1]] =  block_matrix[next_location[0]][next_location[1]];
       vector<int> filled;
       
       for (int i = 0; i < 4; i++) {
           Int2D neighbour_coord = getcoordinate(location,i);
           int opposite = flip_direction(i);
           if ( block_validator_matrix[neighbour_coord[0]][neighbour_coord[1]].face_identities[opposite] > 2 ) {
               filled.push_back(i);
               if ( block_validator_matrix[location[0]][location[1]].face_identities[i] !=
                       block_validator_matrix[neighbour_coord[0]][neighbour_coord[1]].face_identities[opposite] ) {
                   throw runtime_error("Neighbouring faces do no match at k = " + to_string(k) );
               }
           }
       }
       
       bool answer = block_validator_matrix[location[0]][location[1]].check_consistency(filled, block_validator_matrix[next_location[0]][next_location[1]]);
       
       if (!answer) {
           throw runtime_error("Error at k = " + to_string(k));
       }
       
       
       if (k < structure_size - 1) {
           next_direction = getNextLocation(location, walk_matrix);
           location = getcoordinate(location,next_direction); 
       }
    }
}

int GetNumberofBlocks(boostmat<Block,2> block_matrix){
    int row_size = block_matrix.shape()[0] - 2;
    int col_size = block_matrix.shape()[1] - 2;
    
    set<int> block_set;
    
    cout << "Total Size: " << (row_size - 2)*(col_size - 2) << endl;
    
    for (int i = 0; i < row_size+2; i++) {
        for (int j = 0; j < col_size+2; j++) {
            int test_int = block_matrix[i][j].get_identifier();
            if (test_int >= 0) {
                // cout << test_int << endl;
                block_set.insert(test_int);
            }
        }
    }
    
    cout << block_set.size() << " blocks were found" << endl;
    
    return(block_set.size());
}