#define BLANCO -10
#define GRIS -20
#define NEGRO -30

/* MAXIMO VALOR DE INT */
int IMAX = numeric_limits<int>::max();



struct threadParams{
    int id;
    int nodoActual;
    Grafo arbol_local;
    //Podria usarse un heap!
    vector<int> distancia; //(g->cantNodos, IMAX);
    vector<int> distanciaNodo;
    vector<int> colores;
    atomic<bool> encolado;
    mutex puedo_pintar;
    mutex listo_para_encolar;
    threadParams(int index, int cantNodos){
        nodoActual = -1;
        arbol_local = Grafo();
        id = index;
        distancia.assign(cantNodos,IMAX);
        distanciaNodo.assign(cantNodos,-1);
        colores = vector<int>(cantNodos, BLANCO);
        bool unBool = false;
        encolado.store(unBool);
    }
};

struct infoMerge{
    threadParams * emisor;
    int nodo_emisor;
    int nodo_receptor;
    int peso;
    infoMerge(threadParams* emi, int n_emisor, int n_receptor, int p){
        emisor = emi;
        nodo_emisor = n_emisor;
        nodo_receptor = n_receptor;
        peso = p;
    }
};


void restart_thread(threadParams*);

//COLORES de los nodos
vector<int> g_colores;

vector<queue<infoMerge>> g_colas_fusion;
vector<threadParams*> params;

//Grafo (variable global)
Grafo g_grafo;

//MUTEX DEL SISTEMA
vector<mutex> g_fusion_mutex;
vector<mutex> g_colores_mutex;
vector<mutex> g_colas_mutex;
//VECTOR DE ATENDIDOS
vector<bool> atendidos;
int itTotales = 0;
int itThread1 = 0;

//DISTANCIA mínima del nodo al árbol
vector<int> g_distancia;
vector<int> g_distanciaNodo;

//Mutex de debug
mutex debugMutex;

//Variable atomica
bool algunoTermino;


//Pinto el nodo de negro para marcar que fue puesto en el árbol
void pintarNodo(int num){
  g_colores[num] = NEGRO;
  //Para no volver a elegirlo
  g_distancia[num] = IMAX;
}

//Pinto los vecinos de gris para marcar que son alcanzables desde el árbol (salvo los que ya son del árbol)
void pintarVecinos(Grafo *g, int num){
  for(vector<Eje>::iterator v = g->vecinosBegin(num); v != g->vecinosEnd(num); ++v){
    //Es la primera vez que descubro el nodo
    if(g_colores[v->nodoDestino] == BLANCO){
        //Lo marco como accesible
        g_colores[v->nodoDestino] = GRIS;
        //Le pongo el peso desde donde lo puedo acceder
        g_distancia[v->nodoDestino] = v->peso;
        //Guardo que nodo me garantiza esa distancia
        g_distanciaNodo[v->nodoDestino] = num;
    }else if(g_colores[v->nodoDestino] == GRIS){
        //Si ya era accesible, veo si encontré un camino más corto
        g_distancia[v->nodoDestino] = min(v->peso,g_distancia[v->nodoDestino]);
        //Guardo que nodo me garantiza esa distancia
        if(g_distancia[v->nodoDestino] == v->peso)
        g_distanciaNodo[v->nodoDestino] = num;
    }
  }
}

void pintarVecinos(int nodo, threadParams* param){

    for(vector<Eje>::iterator v = g_grafo.vecinosBegin(nodo); v != g_grafo.vecinosEnd(nodo); ++v){
    //Es la primera vez que descubro el nodo
    if(param->colores[v->nodoDestino] == BLANCO){
        //Lo marco como accesible
        param->colores[v->nodoDestino] = GRIS;
        //Le pongo el peso desde donde lo puedo acceder
        param->distancia[v->nodoDestino] = v->peso;
        //Guardo que nodo me garantiza esa distancia
        param->distanciaNodo[v->nodoDestino] = nodo;
    }else if(param->colores[v->nodoDestino] == GRIS){
        //Si ya era accesible, veo si encontré un camino más corto
        param->distancia[v->nodoDestino] = min(v->peso,param->distancia[v->nodoDestino]);
        //Guardo que nodo me garantiza esa distancia
        if(param->distancia[v->nodoDestino] == v->peso){
            param->distanciaNodo[v->nodoDestino] = nodo;
        }

    }
  }

}

//El emisor le pasa su data al receptor
void fusionarArboles(infoMerge& emisor_info, threadParams* receptor){

    threadParams * emisor = emisor_info.emisor;
  emisor->listo_para_encolar.lock();

    printf("[%d] tiene %d nodos, %d ejes y %d peso, el emisor es %d y tiene %d nodos, %d ejes y %d peso\n",
        receptor->id, receptor->arbol_local.numVertices, receptor->arbol_local.numEjes, receptor -> arbol_local.pesoTotal(),
        emisor->id, emisor->arbol_local.numVertices, emisor->arbol_local.numEjes, emisor -> arbol_local.pesoTotal());

    for(int i = 0; i < g_colores.size(); i++){
        if(emisor->colores[i] == emisor->id){
            assert(receptor -> colores[i] != receptor->id);
            receptor->colores[i] = receptor->id;
            receptor->distancia[i] = IMAX;
            receptor->distanciaNodo[i] = emisor->distanciaNodo[i];

            //cambio el color en el grafo
            g_colores_mutex[i].lock();
            g_colores[i] = receptor->id;
            g_colores_mutex[i].unlock();
        }
        if(emisor->colores[i] == GRIS && receptor->colores[i] != receptor->id){
            receptor->distancia[i] = receptor->distancia[i] < emisor->distancia[i]
                                ? receptor->distancia[i] : emisor->distancia[i];
            receptor->distanciaNodo[i] = receptor->distancia[i] < emisor->distancia[i]
                                ? receptor->distanciaNodo[i] : emisor->distanciaNodo[i];
            receptor->colores[i] = GRIS;

        }
    }

    auto cantEjes = 0;
    for(auto vertice : emisor->arbol_local.listaDeAdyacencias){
        for(auto eje : vertice.second){
            if(receptor->arbol_local.listaDeAdyacencias.count(vertice.first) != 0){
                receptor->arbol_local.listaDeAdyacencias.at(vertice.first).push_back(eje);
                cantEjes++;
            }else{
                receptor->arbol_local.listaDeAdyacencias.insert(make_pair(vertice.first, vector<Eje>()));
                receptor->arbol_local.listaDeAdyacencias.at(vertice.first).push_back(eje);
                cantEjes++;
            }
        }
    }
    receptor->arbol_local.numEjes += cantEjes / 2;

    if(receptor-> arbol_local.numVertices  > 0 && emisor -> arbol_local.numVertices  > 0){
        //Agregamos el eje sobre el que se hizo la fusion
        int nodo = emisor_info.nodo_receptor;
        int otroNodo = emisor_info.nodo_emisor;
        int pesoArista = emisor_info.peso;

        debugMutex.lock();
        printf("[%d] esta uniendo su arbol con el de %d con el eje (%d, %d p:%d)\n",
            receptor -> id, emisor -> id, nodo, otroNodo, pesoArista);
        printf("emisor\n");
        imprimirVector(emisor -> distanciaNodo);

        assert(nodo != -1);
        assert(otroNodo != -1);
        debugMutex.unlock();

        receptor->arbol_local.insertarEje(nodo, otroNodo, pesoArista);
    }
    receptor->arbol_local.numVertices += emisor->arbol_local.numVertices;
    auto numEjesPost = receptor->arbol_local.numEjes;

    g_colas_mutex[emisor->id].lock();

    //Fusiono las colas
    while(!g_colas_fusion[emisor->id].empty()){
        auto thread = g_colas_fusion[emisor->id].front();
        g_colas_fusion[emisor->id].pop();
        g_colas_fusion[receptor->id].push(thread);
    }
    g_colas_mutex[emisor->id].unlock();
}



bool chequeoDeCola(threadParams* thread){
    g_colas_mutex[thread->id].lock();
    while(!g_colas_fusion[thread->id].empty() && !algunoTermino){
        infoMerge otroThread = g_colas_fusion[thread->id].front();
        g_colas_fusion[thread->id].pop();

        debugMutex.lock();
        printf("[%d] voy comer a %d, nodo actual es %d\n", thread -> id,(otroThread.emisor)->id, thread->nodoActual);
        debugMutex.unlock();

        fusionarArboles(otroThread, thread);

        debugMutex.lock();
        printf("[%d] comi a %d, nodo actual es %d\n", thread -> id,(otroThread.emisor)->id,thread->nodoActual);
        debugMutex.unlock();

        atendidos[(otroThread.emisor)->id] = true;
    (otroThread.emisor)->puedo_pintar.unlock();
    }
    g_colas_mutex[thread->id].unlock();
    return false;
}

bool meEstanEncolando(threadParams* param) {
  if (param->encolado.load()) {
    while(!atendidos[param->id]){}
    return true;
  }
  return false;
}

//retorno si pude pintar el nodo de mi color o no
bool pintarNodo(int num, threadParams* param){
    g_colores_mutex[num].lock();
    int colorFusion = g_colores[num];

    debugMutex.lock();
    printf("[%d] queria pintar %d y era de %d, tiene %d nodos\n", param -> id, num, colorFusion, param -> arbol_local.numVertices);
    debugMutex.unlock();

    assert(colorFusion != param->id);
    bool expected = false;
  bool value = true;
    if(colorFusion != BLANCO){
        //Si mi id es mayor al del thread con el que colisione, me encolo y espero
        if(colorFusion < param->id && param->encolado.compare_exchange_strong(expected, value)){

            //Lockeo la cola a la cual me voy a pushear
      param->puedo_pintar.unlock();
      if(param -> arbol_local.numVertices >0){

            g_colas_mutex[g_colores[num]].lock();
            //Me pusheo a la cola
            infoMerge emisor(param, param->distanciaNodo[num], num,param->distancia[num]);
            g_colas_fusion[g_colores[num]].push(emisor);
            //Unlockeo la cola a la cual me pushie

            debugMutex.lock();
            printf("[%d] encontre a %d en el eje (%d, %d p:%d) y me encole\n",
                param -> id, colorFusion, param->distanciaNodo[num], num,
                param->distancia[num]);
            if(num == -1 || param -> distanciaNodo[num] == -1){
                printf("[%d] la distancia de los nodos (%lu) es:\n", param -> id, (param -> distanciaNodo).size());
                for(uint i = 0; i < param -> distanciaNodo.size(); i++){
                    printf("(%d, %d) ", param -> distanciaNodo[i], i);
                }
                printf("\n[%d] los colores (%lu) son:\n", param -> id, (param -> colores).size());
                for(uint i = 0; i < param -> colores.size(); i++){
                    printf("(%d, %d) ", i, param -> colores[i]);
                }
            }
            debugMutex.unlock();

            g_colas_mutex[g_colores[num]].unlock();
      }
      else{
        atendidos[param->id] = true;
      }

            // Espero a que me atiendan
            //Unlockeo el nodo por si otro tambien lo quiere pintar
            g_colores_mutex[num].unlock();
            while(!atendidos[param->id]){}
                return false;
        } else if (colorFusion < param->id) {
      while(!atendidos[param->id]){}
      return false;
    }
        else{
            //Unlockeo el nodo por si otro tambien lo quiere pintar
      threadParams* threadAComer = params[colorFusion];
        g_colores_mutex[num].unlock();
      threadAComer->puedo_pintar.lock();
      if (threadAComer->encolado.compare_exchange_strong(expected, value)) {
        if(g_colores[num] != colorFusion){
        //NO HACE FALTA LOCKEAR ACA, POR QUE UN THREAD NO PUEDE REINICIAR Y VOLVER A TENER EL MISMO NODO, COMO MUCHO LO TIENE ALGUIEN DE MENOR NIVEL Y NO ME IMPORTA QUIEN
            bool reverted = false;
            threadAComer->encolado = reverted;

            threadAComer->puedo_pintar.unlock();

            debugMutex.lock();
                    printf("[%d] encontre a %d en el eje (%d, %d p:%d) pero ya habia reseteado\n", param -> id,threadAComer->id , num, param->distanciaNodo[num], param->distancia[num]);
                    debugMutex.unlock();

        }
        else{
                //Pusheo al que voy a comer a la cola

            g_colas_mutex[param->id].lock();
                    infoMerge emisor(threadAComer, num, param->distanciaNodo[num],param->distancia[num]);
                    g_colas_fusion[param->id].push(emisor);


                    debugMutex.lock();
                    printf("[%d] encontre a %d en el eje (%d, %d p:%d) y lo encole\n", param -> id,threadAComer->id , num, param->distanciaNodo[num], param->distancia[num]);
                    debugMutex.unlock();

                    g_colas_mutex[param->id].unlock();
        }
            //Unlockeo la cola a la cual me pushie
      } else {
            threadAComer->puedo_pintar.unlock();
      }
      param->listo_para_encolar.lock();
            chequeoDeCola(param);
            return true;
        }
    } else if(!meEstanEncolando(param)){
      //Como voy a pintar, tengo que esperar a marcar mis vecinos antes de dejar que me coman
      param->listo_para_encolar.lock();
            param->arbol_local.numVertices += 1;
            g_colores[num] = param->id;
            param->colores[num] = param->id;
            if(param->arbol_local.numVertices > 1){
                param->arbol_local.insertarEje(num,param->distanciaNodo[num],param->distancia[num]);
            }
            else{
                param -> distanciaNodo[num] = -2;//es la raiz
            }
            param->distancia[num] = IMAX;
      g_colores_mutex[num].unlock();
      return true;
        }
  g_colores_mutex[num].unlock();
    return false;
}

void* mstThread(void* p_param){
    auto pesoEjes = 0;
    threadParams* param = ((threadParams*)p_param);
    param->nodoActual = rand() % g_grafo.numVertices;
     while(param->arbol_local.numVertices != g_grafo.numVertices){
        auto numEjes = param->arbol_local.numEjes;
        if(algunoTermino) goto end;
        int numeroVertices = param->arbol_local.numVertices;
        //Lo pinto de NEGRO para marcar que lo agregué al árbol y borro la distancia
        param->puedo_pintar.lock();
        if(meEstanEncolando(param) || !pintarNodo(param->nodoActual,param)){
            goto restart;
        }
        param->puedo_pintar.unlock();
        //QUIERO ACTUALIZAR MIS VECINOS SOLO SI PINTE ALGO O ME FUSIONE, SI NO, NO!
        if(numeroVertices < param->arbol_local.numVertices) {
            //Descubrir vecinos: los pinto y calculo distancias

            if(param->colores[param->nodoActual] != param->id){
                debugMutex.lock();
                printf("[%d] busco pintar vecinos y actualizar nodo actual %d que no es mio, es de %d\n", param->id, param->nodoActual,param->colores[param->nodoActual]);
                debugMutex.unlock();
            }

            if(param->colores[param->nodoActual] == param->id){
                pintarVecinos(param->nodoActual, param);
        //Una vez que termino de marcar mis vecinos estoy liste para que me coman
            //Busco el nodo más cercano que no esté en el árbol, pero sea alcanzable
            param->nodoActual = min_element(param->distancia.begin(),param->distancia.end()) - param->distancia.begin();
            }
        }
        param->listo_para_encolar.unlock();
      }

    algunoTermino = true;
    //param.arbol_local.imprimirGrafo();
    assert(param->arbol_local.numEjes == (g_grafo.numVertices - 1));
    assert(g_grafo.numVertices == param->arbol_local.numVertices);
    printf("termino peso: %d\n",param->arbol_local.pesoTotal());
    for(int i = 0; i < atendidos.size(); i++){
        atendidos[i] = true;
    }

  end:
  return nullptr;
  restart:
    restart_thread( param);
}