#include <iostream>
#include <fstream>
#include <stdlib.h>
#include <math.h>
#include <algorithm>
#include <stdio.h>
#include <vector>
#include <set>
#include "BasicFunctions.h"

using namespace std;


//Function returns wrapped adjacency matrix representation of graph on the Mobius Strip
vector<int> GetAdjacencyMatrix(int L, int StabXPos[], int StabYPos[])
{
    int NoOfVertices = 3*L*(L+1)/2;

    int AllRLVertices = 3*NoOfVertices;

    //Adjacency matrix on triangle
    int RGBMatrix[NoOfVertices][NoOfVertices];

    //Adjacency matrix on mobius strip
    int MobiusMatrix[AllRLVertices][AllRLVertices];
    //Mobius matrix contains the restricted lattices' adjacency matrices, arranged in the followeing order
        //RB (0)
        //GB (1)
        //RG (2)

    // It looks like: 
    /*

    RB-RB |  RB-GB | RB-RG
    - - - - - - - - - - - -
    GB-RB |  GB-GB | GB-RG
    - - - - - - - - - - - -
    RG-RB |  RG-GB | RG-RG

    */



    // Initialising matrices

    for (int i = 0; i < NoOfVertices; i++)
    {
        for (int j = 0; j< NoOfVertices; j++)
        {
            RGBMatrix[i][j] = 999999; 

            if (i == j)
            {
                RGBMatrix[i][j] = 0;
            }

        }
    }

    for (int i = 0; i < AllRLVertices; i++)
    {
        for (int j = 0; j< AllRLVertices; j++)
        {
            MobiusMatrix[i][j] = 999999;

            if (i == j)
            {
                MobiusMatrix[i][j] = 0;
            }

        }
    }



    //dealing with each color at a time on each restricted lattice  (this for loop makes the bulk edges)
    for (int i = 0; i < NoOfVertices; i++)
    {

        //if blue
        if (StabYPos[i] % 3 == 0)
        {
            //cout << "Blue stablizer: " << i << " (" << StabXPos[i] << ", " << StabYPos[i] << ")" << endl;
            
            //stabs that all blues have as neighbors
            if (true)
            {
                //northeast neighbor (red)
                int NEy = StabYPos[i] + 1;
                int NE = GetStab(StabXPos[i], NEy, StabXPos,StabYPos,NoOfVertices);
                //cout << "Northeast to " << i << " is " << StabXPos[i] << "," << NEy << " with index " << NE << endl;
                RGBMatrix[i][NE] = 1;
                RGBMatrix[NE][i] = 1;
                MobiusMatrix[i][NE] = 1;
                MobiusMatrix[NE][i] = 1;

                //north neighbor (green)
                int Ny = StabYPos[i] + 2;
                int NN = GetStab(StabXPos[i], Ny, StabXPos,StabYPos,NoOfVertices);
                //cout << "North to " << i << " is " << StabXPos[i] << "," << Ny << " with index " << NN << endl;
                RGBMatrix[i][NN] = 1;
                RGBMatrix[NN][i] = 1; 
                MobiusMatrix[i+NoOfVertices][NN+NoOfVertices] = 1;
                MobiusMatrix[NN+NoOfVertices][i+NoOfVertices] = 1;  
                              
            }

            //neighbors that only blues not along bottom edge have
            if (StabYPos[i] != 0)
            {
                //southeast neighbor (green)
                int SEy = StabYPos[i] - 1;
                int SEx = StabXPos[i] + 1;
                int SE = GetStab(SEx,SEy, StabXPos, StabYPos, NoOfVertices);
                //cout << "Southeast to " << i << " is " << SEx << "," << SEy << " with index " << SE << endl;
                RGBMatrix[i][SE] = 1;
                RGBMatrix[SE][i] = 1;
                MobiusMatrix[i+NoOfVertices][SE+NoOfVertices] = 1;
                MobiusMatrix[SE+NoOfVertices][i+NoOfVertices] = 1;                 

                //south nieghbour (red)
                int Sy = StabYPos[i] - 2;
                int SN = GetStab(StabXPos[i],Sy, StabXPos, StabYPos, NoOfVertices);
                //cout << "South to " << i << " is " << StabXPos[i] << "," << Sy << " with index " << SN << endl;
                RGBMatrix[i][SN] = 1;
                RGBMatrix[SN][i] = 1;
                MobiusMatrix[i][SN] = 1;
                MobiusMatrix[SN][i] = 1;

                //southwest neighbor (green)
                int SWy = StabYPos[i] - 1;
                int SWx = StabXPos[i];
                int SW = GetStab(SWx,SWy, StabXPos, StabYPos, NoOfVertices);
                //cout << "Southwest to " << i << " is " << SWx << "," << SWy << " with index " << SW << endl;
                RGBMatrix[i][SW] = 1;
                RGBMatrix[SW][i] = 1;
                MobiusMatrix[i+NoOfVertices][SW+NoOfVertices] = 1;
                MobiusMatrix[SW+NoOfVertices][i+NoOfVertices] = 1; 
            }

            if (StabXPos[i] != 0) // neighbors that all blues except corner stab have
            {
                //northwest neighbor (red)
                int NWy = StabYPos[i] + 1;
                int NWx = StabXPos[i] -1;
                int NW = GetStab(NWx,NWy, StabXPos, StabYPos, NoOfVertices);
                //cout << "Northwest to " << i << " is " << NWx << "," << NWy << " with index " << NW << endl;
                RGBMatrix[i][NW] = 1;
                RGBMatrix[NW][i] = 1;   
                MobiusMatrix[i][NW] = 1;
                MobiusMatrix[NW][i] = 1;                             
            }

        }

        //if green
        if (StabYPos[i] % 3 == 2)
        {
            //cout << "Green stablizer: " << i << " (" << StabXPos[i] << ", " << StabYPos[i] << ")" << endl;
            
            //neighbor stabs that all greens have
            if (true)
            {
                //southeast neighbor  (red) 
                int SEy = StabYPos[i] - 1;
                int SEx = StabXPos[i];
                int SE = GetStab(SEx,SEy, StabXPos, StabYPos, NoOfVertices);
                //cout << "Southeast to " << i << " is " << SEx << "," << SEy << " with index " << SE << endl;
                RGBMatrix[i][SE] = 1;
                RGBMatrix[SE][i] = 1;
                MobiusMatrix[i+2*NoOfVertices][SE+2*NoOfVertices] = 1;
                MobiusMatrix[SE+2*NoOfVertices][i+2*NoOfVertices] = 1;                             
            }

            //nighbors that greens not along left edge have
            if (StabXPos[i] != 0)
            {

                //southwest neighbor  (red) 
                int SWy = StabYPos[i] - 1;
                int SWx = StabXPos[i] - 1;
                int SW = GetStab(SWx,SWy, StabXPos, StabYPos, NoOfVertices);
                //cout << "Southwest to " << i << " is " << SWx << "," << SWy << " with index " << SW << endl;
                RGBMatrix[i][SW] = 1;
                RGBMatrix[SW][i] = 1;
                MobiusMatrix[i+2*NoOfVertices][SW+2*NoOfVertices] = 1;
                MobiusMatrix[SW+2*NoOfVertices][i+2*NoOfVertices] = 1;    

                //north neighbor  (red) 
                int Ny = StabYPos[i] + 2;
                int Nx = StabXPos[i] - 1;
                int NN = GetStab(Nx,Ny, StabXPos, StabYPos, NoOfVertices);
                //cout << "North to " << i << " is " << Nx << "," << Ny << " with index " << NN << endl;
                RGBMatrix[i][NN] = 1;
                RGBMatrix[NN][i] = 1;
                MobiusMatrix[i+2*NoOfVertices][NN+2*NoOfVertices] = 1;
                MobiusMatrix[NN+2*NoOfVertices][i+2*NoOfVertices] = 1;                              
            }

        }


        // There's no need to deal with reds seperately. 
        // All blues have been linked to their green  and red neighbors on the same RL
        // Then all greens have been linked to their red neighbors
        // AKA reds and blues are already linked



    }

    //until now MobiusMatrix has the restricted lattices filled in seperately. Now to 'stitch' them together along the creases (create boundary and corner edges)

    for (int i = 0; i < NoOfVertices; i++)
    {

        //if blue
        if (StabYPos[i]%3 == 0)
        {


            //if half blue on bottom boundary then stitch GB and RG laatices
            if (StabYPos[i] == 0)
            {
                for (int j = 0; j < NoOfVertices; j++)
                {
                    // if the half blue has a red neighbor in the triangle (RGBMatrix), add the red from the RG lattice as a neighbor in the MS

                    if (RGBMatrix[i][j] == 1  && StabYPos[j]%3 == 1) //STRANGE BEHAVIOUR !!!!! (Works better you put the if without term after &&)
                    {
                        MobiusMatrix[i+NoOfVertices][j+2*NoOfVertices] = 2;
                        MobiusMatrix[j+2*NoOfVertices][i+NoOfVertices] = 2;
                    }
                }                
            }


            //IMP: if corner blue then stitch RB to RG and RG to GB lattices (this is done to help FlipMatrix see that the shortest path passes through RG)
            if (StabYPos[i] == 0 && StabXPos[i] == 0)
            {

                MobiusMatrix[i][i+2*NoOfVertices] = 1;
                MobiusMatrix[i+2*NoOfVertices][i] = 1;

                MobiusMatrix[i+NoOfVertices][i+2*NoOfVertices] = 2;
                MobiusMatrix[i+2*NoOfVertices][i+NoOfVertices] = 2;
                            }


        }


        //if red
        if (StabYPos[i]%3 == 1)
        {

            //if half red on right boundary stitch RB and GB
            if (StabXPos[i] == L-(StabYPos[i]/3)-1)
            {
                for (int j = 0; j < NoOfVertices; j++)
                {
                    // if the half red has a green neighbor in RGBMatrix, add the corresponding green from GB as a neighbor to the red in RB
                    if (RGBMatrix[i][j] == 1 && StabYPos[j]%3 == 2) 
                    {
                        MobiusMatrix[i][j+NoOfVertices] = 2;
                        MobiusMatrix[j+NoOfVertices][i] = 2;
                    }
                }

                //IMP: if corner red then stitch RB to GB and GB to RG
                if (StabYPos[i] == 1)
                {

                    MobiusMatrix[i][i+NoOfVertices] = 1;
                    MobiusMatrix[i+NoOfVertices][i] = 1;

                    MobiusMatrix[i+2*NoOfVertices][i+NoOfVertices] = 2;
                    MobiusMatrix[i+NoOfVertices][i+2*NoOfVertices] = 2;
                }               
            }


        }

        //if green
        if (StabYPos[i]%3 == 2)
        {
            //if half green on left boundary stitch RG and RB lattices
            if (StabXPos[i] == 0)
            {
                for (int j = 0; j < NoOfVertices; j++)
                {
                    if (RGBMatrix[i][j] == 1 && StabYPos[j]%3 == 0)
                    {
                        MobiusMatrix[i+2*NoOfVertices][j] = 2;
                        MobiusMatrix[j][i+2*NoOfVertices] = 2;
                    }
                }


                //IMP: if corner green then stitch RG to RB and RB to GB
                if (StabYPos[i] == 3*L-1)
                {
                    //cout << "Corner green dealt with " << i << endl;

                    MobiusMatrix[i][i+2*NoOfVertices] = 1;
                    MobiusMatrix[i+2*NoOfVertices][i] = 1;

                    MobiusMatrix[i+NoOfVertices][i] = 2;
                    MobiusMatrix[i][i+NoOfVertices] = 2;                  
                }                
            }

        }


    }

    /*
    for (int i =0; i < NoOfVertices; i ++)
    {
        //if half red on right boundary of RB stitch Rs together
        if (StabYPos[i]%3 == 1 && StabXPos[i] == L-(StabYPos[i]/3)-1)
        {
            for (int j = 0; j < NoOfVertices; j ++)
            {
                //if next red on boundary
                if (StabYPos[i]%3 == 1 && StabXPos[j] == L-(StabYPos[j]/3)-1 && StabYPos[j] == StabYPos[i]+3)
                {
                    MobiusMatrix[i][j] = 2;
                    MobiusMatrix[j][i] = 2;
                }
            }

        }

        //if half blue on right boundary of GB stitch Bs together
        if (StabYPos[i] == 0)
        {
            for (int j = 0; j < NoOfVertices; j ++)
            {
                //if next blue on boundary
                if (StabYPos[j] == 0 && StabXPos[j] == StabXPos[i]+1)
                {
                    MobiusMatrix[i+NoOfVertices][j+NoOfVertices] = 2;
                    MobiusMatrix[j+NoOfVertices][i+NoOfVertices] = 2;
                }
            }

        }

        //if half green on right boundary of RG stitch Gs together
        if (StabYPos[i]%3 == 2 && StabXPos[i] == 0)
        {
            for (int j = 0; j < NoOfVertices; j ++)
            {
                //if next red on boundary
                if (StabYPos[i]%3 == 2 && StabXPos[i] == 0 && StabYPos[j] == StabYPos[i]+3)
                {
                    MobiusMatrix[i+2*NoOfVertices][j+2*NoOfVertices] = 2;
                    MobiusMatrix[j+2*NoOfVertices][i+2*NoOfVertices] = 2;
                }
            }

        }

    }*/

    //ENDNEW

   
    /*
    //print RGBMAtrix (adjacency matrix of triangle graph)
    for (int i = 0; i < NoOfVertices; i++)
    {
        for (int j = 0; j< NoOfVertices; j++)
        {
            if (RGBMatrix[i][j] != 99999)
                cout << RGBMatrix[i][j] << "  ";
            else 
                cout << "   ";
        }
        cout << endl;
    }
    cout << endl;
    */
    

    
    //wrap MobiusMatrix so that it can be output

    vector<int> WrapMobius;

    for (int i = 0; i < AllRLVertices; i++)
    {
        for (int j = 0; j< AllRLVertices; j++)
        {
            WrapMobius.push_back(MobiusMatrix[i][j]);

        }

    }

    //MobiusPrinter(WrapMobius, AllRLVertices);

    return WrapMobius;

}

//Function returns two concatenated vectors: {wrapped distance matrix, wrapped flip matrix} 
//See Floyd-Warshall All Pair shortes Paths algorithm, source code from GeeksForGeeks
vector<int> Floyds(vector<int> WrappedCM, int NoOfVertices)
{
    int AllRLVertices = NoOfVertices*3;

    //matrix storing distances between every set of two points
    int CostMatrix[AllRLVertices][AllRLVertices];

    //matrix storing which creases are crossed while travelling between every set of two points
    int FlipMatrix[AllRLVertices][AllRLVertices];
    int PredecessorMatrix[AllRLVertices][AllRLVertices];

    //matrix to remove double cross penalty
    //int CrossPenalty[AllRLVertices][AllRLVertices];

    //unwrap adjacency matrix
    for (int i = 0; i < AllRLVertices; i++)
    {
        for (int j = 0; j < AllRLVertices; j++)
        {
            CostMatrix[i][j] = WrappedCM[i*AllRLVertices+j];
        }
    }

    //store naive crease crosses (not along shortest paths)
    for (int i = 0; i < AllRLVertices; i++)
    {
        for (int j = 0; j < AllRLVertices; j++)
        {
            FlipMatrix[i][j] = 0;
            PredecessorMatrix[i][j] = i;
        }
    }


    // ** Begin Floyd-Warshall Algorithm for All Pairs shortest paths **

    for (int k = 0; k < AllRLVertices; k++)  
    {  
        // Pick all vertices as source one by one  
        for (int i = 0; i < AllRLVertices; i++)  
        {  
            // Pick all vertices as destination for the  
            // above picked source  
            for (int j = 0; j < AllRLVertices; j++)  
            {  
                // If vertex k is on the shortest path from  
                // i to j, then update the value of CostMatrix[i][j], and update the predecessor when path travels through k 
                if (CostMatrix[i][k] + CostMatrix[k][j] < CostMatrix[i][j])  
                {
                    CostMatrix[i][j] = CostMatrix[i][k] + CostMatrix[k][j];
                    PredecessorMatrix[i][j] = PredecessorMatrix[k][j];
                } 
            }  
        }  
    }

    // ** CostMatrix now stores the shortest distance between every two points in the MS **


    //rewrap the results to be able to output them from the function

    vector<int> WrapMobius;

    vector<int> WrapFlips;

    vector<int> WrapPred;

    vector<int> DoubleWrap;

    for (int i = 0; i < AllRLVertices; i++)
    {
        for (int j = 0; j< AllRLVertices; j++)
        {
            WrapMobius.push_back(CostMatrix[i][j]);
            WrapPred.push_back(PredecessorMatrix[i][j]);
            DoubleWrap.push_back(CostMatrix[i][j]);

        }

    }

    for (int i = 0; i < AllRLVertices; i++)
    {
        for (int j = 0; j< AllRLVertices; j++)
        {
            int a  = i;
            int b = j;

            while (a!=b)
            {
                FlipMatrix[i][j] = CreaseCross(b,PredecessorMatrix[a][b],NoOfVertices,FlipMatrix[i][j]);
                b = PredecessorMatrix[a][b];
            }


        }

    }

    for (int i = 0; i < AllRLVertices; i++)
    {
        for (int j = 0; j< AllRLVertices; j++)
        {
            WrapFlips.push_back(FlipMatrix[i][j]);
        }
    }



    for (int i = 0; i < AllRLVertices; i++)
    {
        for (int j = 0; j< AllRLVertices; j++)
        {
            DoubleWrap.push_back(FlipMatrix[i][j]);

        }

    }

    //cout << "Printing distance matrix: " << endl;
    //MobiusPrinter(WrapMobius,AllRLVertices);

    //cout << "Printing flip matrix:" << endl;
    //MobiusPrinter(WrapFlips,AllRLVertices);

    return DoubleWrap;

}

//Find Shortest distances of stabilizers on Mobius strip to the right of red edge
vector<int> GetRPerpDistances(int L, int StabXPos[], int StabYPos[])
{
    int NoOfVertices = 3*L*(L+1)/2;
    int AllRLVertices = 3*NoOfVertices;
    int MobiusMatrix[AllRLVertices][AllRLVertices];
    vector<int> PerpDistances;

    for (int i = 0; i < NoOfVertices; i++) //RBLattice
    {
    
            if (StabYPos[i] % 3 == 0) // if blue
            {
                PerpDistances.push_back(2*StabXPos[i]+1);
            }
            else if (StabYPos[i] % 3 == 1) //if red
            {
                PerpDistances.push_back(2*StabXPos[i]+2);
            }
            else
            {
                PerpDistances.push_back(999999);
            }

    }

    int GBcounter = 2*L + 1;
    int minus = 0;
    int temp = 0;

    for (int i = 0; i < NoOfVertices; i++) // GB lattice
    {
            /*if (StabYPos[i] % 3 == 0) // if blue
            {
                if (StabYPos[i] != temp)
                {
                    minus++;
                    temp = StabYPos[i];
                }
                PerpDistances.push_back(GBcounter - minus);
            }
            else if (StabYPos[i] % 3 == 2) //if green
            {
                if (StabYPos[i] != temp)
                {
                    minus++;
                    temp = StabYPos[i];
                }
                PerpDistances.push_back(GBcounter - minus);

            }
            else //if red*/
            //{
                PerpDistances.push_back(999999);
            //}            
    } 

    int RGcounter = 2*L + 3;
    int plus = 0;
    temp = 1;

    for (int i = 0; i < NoOfVertices; i++) // RG lattice
    {
            if (StabYPos[i] % 3 == 1) // if red
            {
                if (StabYPos[i] != temp)
                {
                    plus++;
                    temp = StabYPos[i];
                }
                PerpDistances.push_back(RGcounter + plus);
            }
            else if (StabYPos[i] % 3 == 2) //if green
            {
                if (StabYPos[i] != temp)
                {
                    plus++;
                    temp = StabYPos[i];
                }
                PerpDistances.push_back(RGcounter + plus);

            }
            else //if blue
            {
                PerpDistances.push_back(999999);
            }            
    } 

    return PerpDistances; 
}


//Find Shortest distances of stabilizers on Mobius strip to the left of red edge
vector<int> GetRPerpDistancesOtherDir(int L, int StabXPos[], int StabYPos[])
{
    int NoOfVertices = 3*L*(L+1)/2;
    int AllRLVertices = 3*NoOfVertices;
    int MobiusMatrix[AllRLVertices][AllRLVertices];
    vector<int> PerpDistances;

    int RBcounter = 4*L + 2;


    for (int i = 0; i < NoOfVertices; i++) //RBLattice
    {
    
            if (StabYPos[i] % 3 == 0) // if blue
            {
                PerpDistances.push_back(RBcounter - 2*StabXPos[i] - 2*(StabYPos[i]/3));
            }
            else if (StabYPos[i] % 3 == 1) //if red
            {
                PerpDistances.push_back(RBcounter - 2*StabXPos[i] - 2*(StabYPos[i]/3) -1);
            }
            else
            {
                PerpDistances.push_back(999999);
            }

    }

    int GBcounter = 2;


    for (int i = 0; i < NoOfVertices; i++) // GB lattice
    {
            /*if (StabYPos[i] % 3 == 0) // if blue
            {
                PerpDistances.push_back(GBcounter + 2*StabXPos[i] + 2*(StabYPos[i]/3));
            }
            else if (StabYPos[i] % 3 == 2) //if green
            {

                PerpDistances.push_back(GBcounter + 1 + 2*StabXPos[i] + 2*(StabYPos[i]/3));

            }
            else //if red
            {*/
                PerpDistances.push_back(999999);
            //}            
    } 


    for (int i = 0; i < NoOfVertices; i++) // GB lattice
    {
            if (StabYPos[i] % 3 == 1) // if red
            {
                PerpDistances.push_back(2*StabXPos[i] + 2);
            }
            else if (StabYPos[i] % 3 == 2) //if green
            {
        
                PerpDistances.push_back(2*StabXPos[i] + 1);

            }
            else //if blue
            {
                PerpDistances.push_back(999999);
            }            
    }

    for (int i = 0; i < AllRLVertices; i++)
    {
        //cout << i << " : " << PerpDistances[i] << endl;    
    }

    return PerpDistances; 
}