//
// This file is part of MorphoGraphX - http://www.MorphoGraphX.org
// Copyright (C) 2012-2015 Richard S. Smith and collaborators.
//
// If you use MorphoGraphX in your work, please cite:
//   http://dx.doi.org/10.7554/eLife.05864
//
// MorphoGraphX is free software, and is licensed under under the terms of the 
// GNU General (GPL) Public License version 2.0, http://www.gnu.org/licenses.
// 
#include <GraphAlgorithms.hpp>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_cblas.h>

#include <Progress.hpp>

namespace mgx
{

	std::map<IntInt, double> generateWeights(std::map<IntInt, double> neighborMap, bool constWeight)
  {
    std::map<IntInt, double> weights;

    if(constWeight){
      forall(const IntIntDouble& p, neighborMap){
        weights[p.first] = 1;
      }
    } else {
      forall(const IntIntDouble& p, neighborMap){
        weights[p.first] = 1./p.second;
      }
    }

    return weights;
  }


  void bfsForBetw(const cellGraph &C, const cell& s, vector<cell>& S, unordered_map<cell, double>& sig, unordered_map<cell, std::vector<cell> >& P)
  {

    unordered_map<cell, double> d;

    forall(const cell& k, C){
      d[k] = -1;
      sig[k] = 0;
    }
    d[s] = 0;
    sig[s] = 1;
    std::queue<cell> Q;
    Q.push(s);

    // bfs for shortest path
    while(Q.size() > 0){
      cell v = Q.front();
      Q.pop();
      S.push_back(v);
      forall(const cell& w, C.neighbors(v)){
        // w found for the first time?
        if(d[w]<0){
          Q.push(w);
          d[w] = d[v] + 1;
        }
        // shortest path to w via v?
        if(d[w] == d[v]+1){
          sig[w] += sig[v];
          P[w].push_back(v);
        }
      }
    }
  }


  bool dijkstraForBetw(const cellGraph &C, const cell& s, vector<cell>& S, unordered_map<cell, double>& sig, 
    unordered_map<cell, std::vector<cell> >& P, std::map<IntInt, double>& weights)
  {

    unordered_map<cell, double> d;

    forall(const cell& k, C){
      d[k] = -1;
      sig[k] = 0;
    }
    d[s] = 0;
    sig[s] = 1;
    // ordered map (key = distance) with value.first = id and value.second = previous id
    std::multimap<double, std::pair<cell, cell> > Q;

    std::set<cell> visited;

    Q.insert(make_pair(0, make_pair(s, s)));
    
    while(!Q.empty()){
      std::multimap<double, std::pair<cell, cell> >::iterator it = Q.begin();
      pair<cell, cell> p = it->second;
      cell v = p.first;
      cell pre = p.second;
      Q.erase(it);

      if(visited.find(v) != visited.end()) continue;
      visited.insert(v);

      S.push_back(v);
      sig[v] += sig[pre];

      // for all neighbors
      forall(const cell& w, C.neighbors(v)){
        if(visited.find(w) == visited.end()){ // not yet visited
          // calc new dis
          double dis = weights[make_pair(w->label, v->label)];
          if(dis == 0){ cout << "no conn " << w->label << "/" << v->label << endl; return false; }
          if(dis != 0){
            double altDis = d[v] + dis;
  
            if(d[w] < 0){ // not yet visited
              d[w] = altDis;
              Q.insert(make_pair(d[w], make_pair(w, v)));
              sig[w] = 0;
              vector<cell> newV;
              newV.push_back(v);
              P[w] = newV;
            } else {
              if(altDis < d[w]){ // if shorter: update best value
                Q.insert(make_pair(altDis, make_pair(w, v)));
                vector<cell> newV;
                newV.push_back(v);
                P[w] = newV;
                d[w] = altDis;
                sig[w] = 0;
              } else if(altDis == d[w]){
          	    sig[w] += sig[v];
          	    P[w].push_back(v);
              }
            }
          }
        
          // if equal 
        }
      }
    }
    return true;
  }


  bool dijkstraForBetw(std::map<IntInt, double>& weights, const int& s, vector<int>& S, unordered_map<int, double>& sig, 
    unordered_map<int, std::vector<int> >& P)
  {

    unordered_map<int, double> d;

    std::set<int> allLabels;
    std::map<int, std::set<int> > labelNeighborsMap;

    forall(auto p, weights){
      if(p.second < 1E-5) continue;
      allLabels.insert(p.first.first);
      allLabels.insert(p.first.second);
      labelNeighborsMap[p.first.first].insert(p.first.second);
      labelNeighborsMap[p.first.second].insert(p.first.first);
    }
    
    // initialize maps
    forall(int k, allLabels){
      d[k] = -1;
      sig[k] = 0;
    }
    d[s] = 0;
    sig[s] = 1;
    // ordered map (key = distance) with value.first = id and value.second = previous id
    std::multimap<double, std::pair<int, int> > Q;

    std::set<int> visited;

    Q.insert(make_pair(0, make_pair(s, s)));
    
    while(!Q.empty()){
      std::multimap<double, std::pair<int, int> >::iterator it = Q.begin();
      pair<int, int> p = it->second;
      int v = p.first;
      int pre = p.second;
      Q.erase(it);

      if(visited.find(v) != visited.end()) continue;
      visited.insert(v);

      S.push_back(v);
      sig[v] += sig[pre];

      // for all neighbors
      forall(int w, labelNeighborsMap[v]){
        if(visited.find(w) != visited.end()) continue; // not yet visited
          // calc new dis
          double dis = weights[make_pair(w, v)];
          if(dis == 0){ cout << "no conn " << w << "/" << v << endl; return false; }
           if(dis != 0){
            double altDis = d[v] + dis;
  
            if(d[w] < 0){ // not yet visited
              d[w] = altDis;
              Q.insert(make_pair(d[w], make_pair(w, v)));
              sig[w] = 0;
              vector<int> newV;
              newV.push_back(v);
              P[w] = newV;
            } else {
              if(altDis < d[w]){ // if shorter: update best value
                Q.insert(make_pair(altDis, make_pair(w, v)));
                vector<int> newV;
                newV.push_back(v);
                P[w] = newV;
                d[w] = altDis;
                sig[w] = 0;
              } else if(altDis == d[w]){
                sig[w] += sig[v];
                P[w].push_back(v);
              }
            }
          }
        
          // if equal 
        
      }
    }
    return true;
  }



  // calculate the betweenness centrality for the cell graph (without weights)
  void calcBetweenness(cellGraph &C, std::map<cell, double>& cellBetweenessMap)
  {
    std::map<cell, double> res;

    //typedef std::pair<cell, std::vector<cell> > VtxVecVtx;

    forall(const cell& s, C){
      res[s] = 0;
    }

    forall(const cell& s, C){
      std::vector<cell> S;
      std::unordered_map<cell, std::vector<cell> > P;
      std::unordered_map<cell, double> sig;

      bfsForBetw(C, s, S, sig, P);

      std::unordered_map<cell, double> delta;

      // S return vertices in order of non-increasing distance from s
      while(S.size() > 0){
        cell w = S.back();
        S.pop_back();
        double coef = (1+delta[w])/sig[w];
        forall(const cell& v, P[w]){
          delta[v] += sig[v]*coef;
        }
        if(w != s) res[w] += delta[w];
      }

    }

    double scaling = 0.5*2/double((C.size()-1)*(C.size()-2));
    forall(const cell& c, C){
      res[c] *= scaling;
    }
    
    cellBetweenessMap = res;
  }


  // calculate the betweenness centrality for the cell graph (with weights)
  bool calcBetweenness(cellGraph &C, std::map<IntInt, double>& weights, std::map<cell, double>& cellBetweenessMap)
  {
    std::map<cell, double> res;

    //typedef std::pair<cell, std::vector<cell> > VtxVecVtx;

    forall(const cell& s, C){
      res[s] = 0;
    }

    forall(const cell& s, C){
      std::vector<cell> S;
      std::unordered_map<cell, std::vector<cell> > P;
      std::unordered_map<cell, double> sig;

      if(!dijkstraForBetw(C, s, S, sig, P, weights)) return false;

      std::unordered_map<cell, double> delta;

      // S return vertices in order of non-increasing distance from s
      while(S.size() > 0){
        cell w = S.back();
        S.pop_back();
        double coef = (1+delta[w])/sig[w];
        forall(const cell& v, P[w]){
          delta[v] += sig[v]*coef;
        }
        if(w != s) res[w] += delta[w];
      }
    }

    double scaling = 0.5*2/double((C.size()-1)*(C.size()-2));
    forall(const cell& c, C){
      res[c] *= scaling;
    }
    
    cellBetweenessMap = res;
    return true;
  }


  // calculate the betweenness centrality for the cell graph (with weights)
  bool calcBetweenness(std::map<IntInt, double>& neighborhoodGraph, std::map<int, double>& cellBetweenessMap)
  {
    std::map<int, double> res;

    //typedef std::pair<int, std::vector<int> > VtxVecVtx;

    

    std::set<int> allLabels;
    std::map<int, std::set<int> > labelNeighborsMap;

    forall(auto p, neighborhoodGraph){
      if(p.second < 1E-5) continue;
      allLabels.insert(p.first.first);
      allLabels.insert(p.first.second);
      labelNeighborsMap[p.first.first].insert(p.first.second);
      labelNeighborsMap[p.first.second].insert(p.first.first);
    }


    forall(const int& s, allLabels){
      std::vector<int> S;
      std::unordered_map<int, std::vector<int> > P;
      std::unordered_map<int, double> sig;

      if(!dijkstraForBetw(neighborhoodGraph, s, S, sig, P)) return false;

      std::unordered_map<int, double> delta;

      // S return vertices in order of non-increasing distance from s
      while(S.size() > 0){
        int w = S.back();
        S.pop_back();
        double coef = (1+delta[w])/sig[w];
        forall(const int& v, P[w]){
          delta[v] += sig[v]*coef;
        }
        if(w != s) res[w] += delta[w];
      }
    }

    double scaling = 0.5*2/double((allLabels.size()-1)*(allLabels.size()-2));
    forall(const int& c, allLabels){
      res[c] *= scaling;
    }
    
    cellBetweenessMap = res;
    return true;
  }


  // ordering of the cells with a given starting cell (minCell)
  std::vector<cell> cuthillMckeeOrdering(cellGraph &C, std::map<cell, int>& cellOrder, const cell &minCell)
  {

  //std::map<cell, int> cellOrder;

  std::map<cell, int> cellNrOfNeighborsMap;
  std::map<cell, bool> visitedCells;

  std::vector<cell> orderedCells;

  forall(const cell& c, C){
    forall(const cell& c2, C.neighbors(c)){
      cellNrOfNeighborsMap[c]++;
    }
  }
  
  std::queue<cell> Q;
  Q.push(minCell);
  visitedCells[minCell] = true;

  while(Q.size() > 0){

    cell parent = Q.front();
    cellOrder[parent] = orderedCells.size();
    orderedCells.push_back(parent);

    //cout << "cell " << parent->pos << endl;
    Q.pop();

    std::vector<cell> newCells;
    // add new vertices
    forall(const cell& c, C.neighbors(parent)){
      if(!visitedCells[c]){
        newCells.push_back(c);
      }
    }
    // sort them
    forall(const cell& c, newCells){
      int minNeigh = 1E9;
      cell minCell;
      forall(const cell& c2, newCells){
        if(!visitedCells[c2] and minNeigh > cellNrOfNeighborsMap[c2]){
          minCell = c;
          minNeigh = cellNrOfNeighborsMap[c2];
        }
      }
      visitedCells[minCell] = true;
      Q.push(minCell);
    }


  }

  return orderedCells;
  }

  // orders cells of a cell graph
  std::vector<cell> cuthillMckeeOrdering(cellGraph &C, std::map<cell, int>& cellOrder)
  {

  std::map<cell, int> cellNrOfNeighborsMap;

  int minNeigh = 1E9;
  cell minCell;

  // find starting vertex
  forall(const cell& c, C){
    forall(const cell& c2, C.neighbors(c)){
      cellNrOfNeighborsMap[c]++;
    }
    if(cellNrOfNeighborsMap[c] < minNeigh){
      minCell = c;
      minNeigh = cellNrOfNeighborsMap[c];
    } 
  }
  
    return cuthillMckeeOrdering(C, cellOrder, minCell);
  }

  // calculates the inverse of an input matrix and write it to the output matrix
  void matrixInverseGSL(std::vector<std::vector<double> >& inputM, std::vector<std::vector<double> >& outputM)
  {

  uint n = inputM.size();

  if(n>0){

  int s;

  // Define all the used matrices
  gsl_matrix * m = gsl_matrix_alloc (n, n);
  gsl_matrix * inverse = gsl_matrix_alloc (n, n);
  gsl_permutation * perm = gsl_permutation_alloc (n);

    for(uint i = 0; i<n; i++){
      for(uint j = 0; j<n; j++){
        gsl_matrix_set (m, i, j, inputM[i][j]);
      }
    }

  // Make LU decomposition of matrix m
  gsl_linalg_LU_decomp (m, perm, &s);

  // Invert the matrix m
  gsl_linalg_LU_invert (m, perm, inverse);

  for (uint i = 0; i < n; i++){
    for (uint j = 0; j < n; j++){
      outputM[i][j] = gsl_matrix_get (inverse, i, j);
    }
  }

  } else {
    std::cout << "matrix too small, n: " << n << std::endl;
  }

  }


//Input: M - matrix to solve, x - rhs
//Function overwrites M and x, where x is the solution to the system of equations
 void systemSolveGSL(std::vector<std::vector<double> >& inputM, std::vector<double>& x)
  {

  uint n = inputM.size();

  if(n>0){

  int s;

  // Define all the used matrices
  gsl_matrix * m = gsl_matrix_alloc (n, n);
//  gsl_matrix * inverse = gsl_matrix_alloc (n, n);
  gsl_permutation * perm = gsl_permutation_alloc (n);
  gsl_vector * v = gsl_vector_alloc(n);

    for(uint i = 0; i<n; i++){
      for(uint j = 0; j<n; j++){
        gsl_matrix_set (m, i, j, inputM[i][j]);
      }
    }
    for(uint i=0;i<n;i++)
        gsl_vector_set(v,i,x[i]);

  gsl_linalg_LU_decomp (m, perm, &s);
  gsl_linalg_LU_svx(m,perm,v);
//    gsl_linalg_HH_svx(m,v);


  for (uint i = 0; i < n; i++){
    x[i] = gsl_vector_get(v,i);
  }

  } else {
    std::cout << "matrix too small, n: " << n << std::endl;
  }

  }



  // reverse a vector
  void reverseVector(std::vector<cell>& input)
  {
    std::vector<cell> reversed (input.size());

    for(uint i=0; i<input.size(); i++){
      reversed[input.size()-i-1] = input[i];
    }
    input = reversed;
  }


}
