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

#include "PerfectMatching.h"
#include "GeomPerfectMatching.h"

using namespace std;



// Decoder function
vector<int> MatchingDecoder(bool StabilizersZ[], int L, vector<int> Distances, vector<int> Flips, int StabXPos[], int StabYPos[])
{

    struct PerfectMatching::Options options;
    

    vector<int> DecoderPairs;

    int V = 9*L*(L+1)/2;
    int NoOfVertices = V/3;
    int d = 2*L+1;



    //unwrap the vectors storing shortest paths and crease crosses

    int CostMatrix[V][V];
    int FlipMatrix[V][V];

    for (int i = 0; i < V; i++)
    {
        for (int j = 0; j < V; j++)
        {
            CostMatrix[i][j] = Distances[i*V+j];
            FlipMatrix[i][j] = Flips[i*V+j];
        }
    }


    
    int TotalAnyons = 0;
    

    for(int j = 0; j < V; j++)
    {      
        if(StabilizersZ[j])
            TotalAnyons++;       
    }
    
    
    int Locations[TotalAnyons];
    

    int v = 0;   
    for(int j = 0; j < V; j++)
    {
        if(StabilizersZ[j])
        {
            Locations[v] = j;
            v++;
        }
        
    }

 
    //cout << "Pairs" << endl; 
    int WeightCost = 0;

    int PairElement1[TotalAnyons / 2];
    int PairElement2[TotalAnyons / 2];
    
    
    for(int MatchingNo = 0; MatchingNo < 1; MatchingNo++)
    {
    
        
        // What follows can be regarded as a black box where minimum weight perfect matching is conducted
        // ====================================================
    
        int node_num = TotalAnyons;
        int edge_num = TotalAnyons*(TotalAnyons - 1) / 2;
    
        int edges[2*edge_num];
        int weights[edge_num];
    
        for(int j = 0; j < edge_num; j++)
        {
            edges[2*j] = 0;
            edges[2*j+1] = 0;
            weights[j] = 0;
        }
    
        int CurrentEdge = 0;
    
    
        for(int j = 0; j < TotalAnyons; j++)
        {
            int Connections = TotalAnyons - 1 - j;
        
        
        
            for(int k = 0; k < Connections; k++)
            {
            
                int Vertex1 = j;
                int Vertex2 = j + k + 1;
            
                edges[2*CurrentEdge] = Vertex1;
                edges[2*CurrentEdge+1] = Vertex2;
            
                int Vertex1Location = Locations[Vertex1];
                int Vertex2Location = Locations[Vertex2];

                int sameLoc = 0;
                int crossBorder = 0;

            
                weights[CurrentEdge] = CostMatrix[Vertex1Location][Vertex2Location];

               CurrentEdge++;
            }
        }


        PerfectMatching *pm = new PerfectMatching(node_num, edge_num);
        for (int e=0; e<edge_num; e++)
        {
            pm->AddEdge(edges[2*e], edges[2*e+1], weights[e]);
        }
    
        pm->options = options;
        pm->Solve();
    
        int SolutionNumber = 0;
    
        for (int i=0; i<node_num; i++)
        {
            int j = pm->GetMatch(i);
            if (i < j)
            {
                PairElement1[SolutionNumber] = Locations[i];
                PairElement2[SolutionNumber] = Locations[j];

                DecoderPairs.push_back(Locations[i]);
                DecoderPairs.push_back(Locations[j]);
                DecoderPairs.push_back(CostMatrix[PairElement2[SolutionNumber]][PairElement1[SolutionNumber]]);
                DecoderPairs.push_back(FlipMatrix[PairElement2[SolutionNumber]][PairElement1[SolutionNumber]]);

                //cout << "(" << PairElement1[SolutionNumber] << ", " << PairElement2[SolutionNumber] << ")  w=" << CostMatrix[PairElement2[SolutionNumber]][PairElement1[SolutionNumber]] << ",  f=" << FlipMatrix[PairElement2[SolutionNumber]][PairElement1[SolutionNumber]] << endl;
                WeightCost = WeightCost + CostMatrix[PairElement2[SolutionNumber]][PairElement1[SolutionNumber]];
                SolutionNumber++;
            }
        }
    
        delete pm;
    
    }

    //PairElement1 and PairElement 2 store the indices of the stabilzers  used in the final edges after MWPM
    //FlipMatrix already stores which creases these final edges cross. All that's left is to fuse these crease charges to get the final charge
    //cout << "OG matching Weight = " << WeightCost << endl;
       
    
    return DecoderPairs; // each set of 4 in this vector is {V1, V2, weight of string, parity of string}
}


//get numerical value from decoder MWPM
int DecoderOutcome(vector<int> DecoderPairs, int L)
{
    int DecoderOutcome = 0;


    int FinalEdges = DecoderPairs.size() / 4;

    int RC = 0;  

    int prev = 0;

    for (int i = 0; i < FinalEdges; i++)
    {
        prev = ChargeFusion(DecoderPairs[4*i+3], prev);

        if ((DecoderPairs[4*i+3] / 2) % 2 == 1)
        {
            RC++;
        }
          
    }
  
    DecoderOutcome = prev;

    return DecoderOutcome + 8*RC;

}


// DummyDecoder function
//Alternate MWPM done, returning alternatue pairs of defects going the other way around the MS
vector<int> DummyMatchingDecoder(bool ReducedZ[], int L, vector<int> WrappedAltDistances, vector<int> WrappedAltFlips, vector<int> RBdist, vector<int> RGdist, vector<int> DecoderPairs, int StabXPos[], int StabYPos[])
{

    struct PerfectMatching::Options options;
    
    vector<int> DummyPairs;

    int V = 9*L*(L+1)/2;
    int NoOfVertices = V/3;
    int d = 2*L+1;

    int VD = V + 2;


    //unwrap the vectors storing shortest paths and crease crosses

    int CostMatrix[VD][VD];
    int FlipMatrix[VD][VD];

    for (int i = 0; i < V; i++)
    {
        for (int j = 0; j < V; j++)
        {
            CostMatrix[i][j] = WrappedAltDistances[i*V+j];
            FlipMatrix[i][j] = WrappedAltFlips[i*V + j];
        }
    }



    // IMP: ReducedZ does not show the dummies yet

    bool StabilizersZ[VD];

    StabilizersZ[VD-1] = 1; //RG dummy
    StabilizersZ[VD-2] = 1; //RB dummy

    CostMatrix[V][V] = 0;
    CostMatrix[V+1][V+1] = 0;
    CostMatrix[V][V+1] = d+d*d;
    CostMatrix[V+1][V] = d+d*d;
    FlipMatrix[V][V] = 0;
    FlipMatrix[V+1][V+1] = 0;
    FlipMatrix[V][V+1] = 2;
    FlipMatrix[V+1][V] = 2;



    for (int i = 0; i < V; i++)
    {
        StabilizersZ[i] = ReducedZ[i];

        CostMatrix[V][i] = RBdist[i];
        CostMatrix[V+1][i] = RGdist[i];

        if (i < NoOfVertices)
        {
            FlipMatrix[V][i] = 0;
            FlipMatrix[V+1][i] = 5;
        }

        else if (i < 2*NoOfVertices)
        {
            FlipMatrix[V][i] = 1;
            FlipMatrix[V+1][i] = 4;
        }

        else
        {
            FlipMatrix[V][i] = 5;
            FlipMatrix[V+1][i] = 0;            
        }

        CostMatrix[i][V] = RBdist[i];
        CostMatrix[i][V+1] = RGdist[i];
        FlipMatrix[i][V] = FlipMatrix[V][i];
        FlipMatrix[i][V+1] = FlipMatrix[V+1][i];
    }

    
    int TotalAnyons = 0;
    

    for(int j = 0; j < VD; j++)
    {      
        if(StabilizersZ[j])
        {
            TotalAnyons++;  
        }         
    }
    
    
    int Locations[TotalAnyons];
    

    int v = 0;   
    for(int j = 0; j < VD; j++)
    {
        if(StabilizersZ[j])
        {
            Locations[v] = j;
            v++;
        }
        
    }

 
    int WeightCost = 0; 

    int PairElement1[TotalAnyons / 2];
    int PairElement2[TotalAnyons / 2];
    
    
    for(int MatchingNo = 0; MatchingNo < 1; MatchingNo++)
    {
    
        
        // What follows can be regarded as a black box where minimum weight perfect matching is conducted
        // ====================================================
    
        int node_num = TotalAnyons;
        int edge_num = TotalAnyons*(TotalAnyons - 1) / 2;
    
        int edges[2*edge_num];
        int weights[edge_num];
    
        for(int j = 0; j < edge_num; j++)
        {
            edges[2*j] = 0;
            edges[2*j+1] = 0;
            weights[j] = 0;
        }
    
        int CurrentEdge = 0;
    
    
        for(int j = 0; j < TotalAnyons; j++)
        {
            int Connections = TotalAnyons - 1 - j;
                
        
            for(int k = 0; k < Connections; k++)
            {
            
                int Vertex1 = j;
                int Vertex2 = j + k + 1;
            
                edges[2*CurrentEdge] = Vertex1;
                edges[2*CurrentEdge+1] = Vertex2;
            
                int Vertex1Location = Locations[Vertex1];
                int Vertex2Location = Locations[Vertex2];
            
                weights[CurrentEdge] = CostMatrix[Vertex1Location][Vertex2Location];
                          
                CurrentEdge++;
            }
        }


        PerfectMatching *pm = new PerfectMatching(node_num, edge_num);
        for (int e=0; e<edge_num; e++)
        {
            pm->AddEdge(edges[2*e], edges[2*e+1], weights[e]);
        }
    
        pm->options = options;
        pm->Solve();
    
        int SolutionNumber = 0;
    
        for (int i=0; i<node_num; i++)
        {
            int j = pm->GetMatch(i);
            if (i < j)
            {
                PairElement1[SolutionNumber] = Locations[i];
                PairElement2[SolutionNumber] = Locations[j];

                DummyPairs.push_back(Locations[i]);
                DummyPairs.push_back(Locations[j]);
                DummyPairs.push_back(CostMatrix[PairElement2[SolutionNumber]][PairElement1[SolutionNumber]]);
                DummyPairs.push_back(FlipMatrix[PairElement2[SolutionNumber]][PairElement1[SolutionNumber]]);

                //cout << "(" << PairElement1[SolutionNumber] << ", " << PairElement2[SolutionNumber] << ")  w=" << CostMatrix[PairElement2[SolutionNumber]][PairElement1[SolutionNumber]] << ",  f=" << FlipMatrix[PairElement2[SolutionNumber]][PairElement1[SolutionNumber]] << endl;
                WeightCost = WeightCost + CostMatrix[PairElement2[SolutionNumber]][PairElement1[SolutionNumber]];
                SolutionNumber++;
            }
        }
    
        delete pm;
    
    }

    //PairElement1 and PairElement 2 store the indices of the stabilzers  used in the final edges after MWPM
    //FlipMatrix already stores which creases these final edges cross. All that's left is to fuse these crease charges to get the final charge
    //cout << "Dummy Matching Weight = " << WeightCost << endl;

    int FinalEdges = TotalAnyons / 2;    
       
    
    return DummyPairs; // each set of 4 in this vector is {V1, V2, weight of string, parity of string}
}


int DummyOutcome(vector<int> DecoderPairs,  int L)
{
    int DecoderOutcome = 0;


    int FinalEdges = DecoderPairs.size() / 4;

    int prev = 2;

    for (int i = 0; i < FinalEdges; i++)
    {
        prev = ChargeFusion(DecoderPairs[4*i+3], prev);             
    }

  
    DecoderOutcome = prev;

    return DecoderOutcome;

}


//gets matching weights of each correction and returns in a vector {original, alternate}
vector<int> GetMatchingWeights(vector<int> DecoderPairs, vector<int> DummyPairs)
{
    int NoOfDecoderEdges = DecoderPairs.size() /4;
    int NoOfDummyEdges = DummyPairs.size() /4;

    vector<int> ans;
    ans.push_back(0);
    ans.push_back(0);
    
    for (int i = 0; i < NoOfDecoderEdges; i++)
    {

        ans[0] = ans[0] + (DecoderPairs[4*i+2]);
    } 

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

        ans[1] = ans[1] + (DummyPairs[4*i+2]);
    }  

    return ans;     

}