/**CFile***********************************************************************

  FileName    [bvn.cc]

  PackageName [bvn]

  Synopsis    [Routines for creating random doubly (sub)stochastic matrices
		and computing Birkhoff decompositions for them.]

  Author      [Adnan Aziz]

******************************************************************************/


#include "algo.h"
#include "queue"
#include "map.h"
#include "vector.h"
#include "stdio.h"
#include "stdlib.h"

/*---------------------------------------------------------------------------*/
/* Class declarations                                                        */
/*---------------------------------------------------------------------------*/

/**Class***********************************************************************

  Synopsis    [Matrix class, includes constructor for random matrices and 
               routine for computing BvN decomposition.]

******************************************************************************/

class randomDSSMatrix {
  public:
    randomDSSMatrix(int n);
    int testIsDSMatrix();
    void convertToDSMatrix();
    void print();
    void printDecomp();
    static const double EPSILON = 0.0000000001;
    int *computeMatching();
    void decompose();
    int dimension;
    double **matrix;
    vector<randomDSSMatrix *> decompMatrices;
    vector<double> decompWeights;
};

/**Class***********************************************************************

  Synopsis    [Used in conjunction with STL's find_if - it's playing the role
               of a function object, if you are familiar with the concept.]

******************************************************************************/

class 
myPred {
  public:
    myPred(int j) {data = j;}; 
    int data;
    bool operator()(pair<int, int>foo) {return foo.first == data;} ;
};


// forward reference, should be put in a header file
extern int BFStoAugmentingVertex( int, randomDSSMatrix *, map<int, int>&, int *);

/**Function********************************************************************

  Synopsis    [Constructor, creates a random doubly substochastic matrix of dimension n.]

  Description [Constructor, creates a random doubly substochastic matrix of dimension n.
		Starts by creating a random 0/1 valued matrix, then normalizes
		the rows and columns.  Not an especially good way to produce
		a random, since the values are dependent on the order in which the
		rows/cols are processed.]

  SideEffects [Allocates n + 1 arrays each n long. One consists of pointers to floats, 
		the remainder of floats.]

******************************************************************************/


randomDSSMatrix::randomDSSMatrix(int n) {

  dimension = n;
  matrix = new (double*)[n];
  
  for(int i=0; i < dimension; i++ ) {
    matrix[i] = new (double)[n];
  }
  for (int i=0; i < dimension; i++ ) {
    double rowWeight=0.0;
    for (int j=0; j < dimension; j++ ) {
      matrix[i][j] = rand() % 2;
      rowWeight += matrix[i][j];
    }
    if ( rowWeight > 1.0 ) {
      for (int j=0; j < dimension; j++ ) {
        matrix[i][j] /= rowWeight;
      }
    }
  }

  for (int j=0; j < dimension; j++ ) {
    double columnWeight=0.0;
    for (int i=0; i < dimension; i++ ) {
      columnWeight += matrix[i][j];
    }
    if ( columnWeight > 1.0 ) {
      for (int i=0; i < dimension; i++ ) {
        matrix[i][j] /= columnWeight;
      } 
    }
  }
}


/**Function********************************************************************

  Synopsis    [Tests to see if object is a doubly stochastic matrix.]

  Description [Tests to see if object is a doubly stochastic matrix. Using
		test to within EPSILON of 1 for numerical stability; try
 		the code with EPSILON = 0.]

******************************************************************************/

int randomDSSMatrix::testIsDSMatrix() 
{
  for(int i=0; i < dimension; i++ ) {
    double rowSum=0.0;
    for (int j=0; j < dimension; j++ ) {
      rowSum += matrix[i][j];
    }
    if ( rowSum < 1.0 - EPSILON ) {
      return 0;
    }
  }
  for(int j=0; j < dimension; j++ ) {
    double colSum=0.0;
    for (int i=0; i < dimension; i++ ) {
      colSum += matrix[i][j];
    }
    if ( colSum < 1.0 - EPSILON ) {
      return 0;
    }
  }
  return 1;
}


/**Function********************************************************************

  Synopsis    [Print the matrix to cout.]

  Description [Print the matrix to cout.]

******************************************************************************/

void randomDSSMatrix::print() 
{
  double rowSum;
  double colSum;

  for(int i=0; i < dimension; i++ ) {
    rowSum = 0.0;
    for (int j=0; j < dimension; j++ ) {
      rowSum += matrix[i][j];
      printf("%.4f  ", matrix[i][j]);
    }
    printf(" (%.4f)\n", rowSum);
  }
  for(int j=0; j  < dimension; j++ ) {
    colSum = 0.0;
    for(int i=0; i < dimension; i++ ) {
      colSum += matrix[i][j];
    }
    printf("(%.4f) ", colSum);
  }
  printf("\n");
}

/**Function********************************************************************

  Synopsis    [Convert a doubly substochastic matrix to a doubly stochastic matrix.]

  Description [Convert a doubly substochastic matrix to a doubly stochastic matrix,
   		i.e., each entry in original matrix is replaced by an entry that is at
		least as large, and the resulting matrix is doubly stochastic.  We use
		von Neumann's trick of selecting a row and a column each summing to less
		than 1, and adding just enough to their common element to boost
		the larger of the two to sum to 1.]

******************************************************************************/

void randomDSSMatrix::convertToDSMatrix() 
{
  int boostRow = -1;
  int boostCol = -1;
  double boostRowSum;
  double boostColSum;

  while ( !this->testIsDSMatrix() ) {

    // find a row to boost
    for(int i=0; i < dimension; i++ ) {
      double rowSum=0.0;
      for (int j=0; j < dimension; j++ ) {
        rowSum += matrix[i][j];
      }
      if ( rowSum < 1.0 - EPSILON ) {
        boostRow = i;
        boostRowSum = rowSum;
        break;
      }
    }

    // find a col to boost
    for(int j=0; j < dimension; j++ ) {
      double colSum=0.0;
      for (int i=0; i < dimension; i++ ) {
        colSum += matrix[i][j];
      }
      if ( colSum < 1.0 - EPSILON ) {
        boostCol = j;
        boostColSum = colSum;
        break;
      }
    }

    // do the von Neumann trick
    matrix[boostRow][boostCol] += (1.0 - (boostColSum > boostRowSum ?  boostColSum : boostRowSum) );
    printf("Incrementing entry %d, %d by %f\n", boostRow, boostCol, 
             (1.0 - (boostColSum > boostRowSum ?  boostColSum : boostRowSum) ) );
  }

  // got here => matrix is doubly stochastic
  return;
}


/**Function********************************************************************

  Synopsis    [Compute the Birkhoff decomposition of the matrix.]

  Description [Compute the Birkhoff decomposition of the matrix.  This is
		done by computing perfect matchings (actually maximum matchings
		which we know must be perfect, thanks to Hall's theorem), and
		then pulling out the corresponding permutation matrix. 
 		The decomposition is stored in decompMatrices and decompWeights 
		which are STL vectors.]

******************************************************************************/

void 
randomDSSMatrix::decompose()
{
  while ( 1 ) {

    printf("\n---- Beginning one more decomposition\n");

    // LRmatches[i] holds the vertex on the right that vetex i on 
    // the left is matched with
    int *LRmatches = computeMatching();

    randomDSSMatrix *permMatrix = new randomDSSMatrix( dimension );
    for ( int i = 0; i < dimension; i++ ) {
      for ( int j = 0; j < dimension; j++ ) {
        permMatrix->matrix[i][j] = 0;
      }
    }
    for ( int i = 0 ; i < dimension ; i++ ) {
      permMatrix->matrix[i][LRmatches[i]] = 1.0;
    }

    double phi = 1.0;
    for ( int i = 0 ; i < dimension ; i++ ) {
      phi = ( phi > matrix[i][LRmatches[i]] ) ? matrix[i][LRmatches[i]] : phi;
    }


    // importnant: the weight of the i-th permutation matrix will actually be a function
    // of the weights we've stored in decompWeights from 0 to i because
    // of the normalization, and not just decompWeights[i]

    decompMatrices.push_back( permMatrix );
    decompWeights.push_back( phi );

    for ( int i = 0 ; i < dimension ; i++ ) {
      matrix[i][LRmatches[i]] = matrix[i][LRmatches[i]] - phi;
    }

    // terminating condition --- we've gotten the matrix down to a permutation matrix.
    // this happens iff phi = 1 (within EPSILON of 1 to avoid endless cycling because
    // of floating point roundoff issues).
    if ( phi > 1 - EPSILON ) {
      break;
    }

    for ( int i = 0; i < dimension; i++ ) {
      for ( int j = 0; j < dimension; j++ ) {
        matrix[i][j] /= (1 - phi);
      }
    }
  }
}

/**Function********************************************************************

  Synopsis    [Compute a maximum matching for the graph derived from this
		matrix.]

  Description [Compute a maximum matching for the graph derived from this
                matrix.  Not particularly carefully coded, and uses a bare bones augmenting 
                path algorithm, but is plenty quick for our application.]

******************************************************************************/

int *
randomDSSMatrix::computeMatching()
{
  int* LRmatches = new int[dimension];
  int* RLmatches = new int[dimension];
  vector<int> augmentingPath;
  
  for (int i=0; i<dimension; i++ ) {
    LRmatches[i] = -1;
    RLmatches[i] = -1;
  }

  print();

  // greedy phase
  for ( int i = 0; i < dimension; i++ ) {
    for ( int j = 0; j < dimension; j++ ) {
      // is there an edge from i to j?
      if ( matrix[i][j] ) {
        // is j currently unmatched?
        if ( RLmatches[j] == -1 ) {
          // match i to j
          LRmatches[i] = j;
          RLmatches[j] = i;
          break;
        }
      }
    }
  }

  // augment paths
  for ( int i = 0; i < dimension; i++ ) {
    if ( LRmatches[i] != -1 ) {
      // already matched, nothing to do
      continue;
    }
 
    // search for an augmenting path from this vtx

    map<int, int> BFSshells; // holds reachable vertices on the right

    int augmentingVertex = BFStoAugmentingVertex( i, this, BFSshells, RLmatches );
    assert( augmentingVertex != -1);
   
    int tmpVertex=augmentingVertex;
    while( 1 ) {
      augmentingPath.push_back( tmpVertex );
      // old code for debugging
      // if ( tmpVertex == 2 ) {
      //   printf("In aug path code, we have %d mapping to %d\n", tmpVertex, (*BFSshells.find(tmpVertex)).second );
      // }
      augmentingPath.push_back( (*BFSshells.find(tmpVertex)).second);
      tmpVertex = LRmatches[ (*BFSshells.find(tmpVertex)).second ] ;
      
      if ( tmpVertex == -1 ) {
        break;
      }
    }
    reverse( augmentingPath.begin(), augmentingPath.end() );
  
    // perform augmentation
    for (unsigned int k=0; k < augmentingPath.size() ; k = k + 2 ) {
      LRmatches[ augmentingPath[k] ] = augmentingPath[k+1];
      RLmatches[ augmentingPath[k+1] ] = augmentingPath[k];
    }
  }

  printf("Final matching is\n");
  for (int i=0; i < dimension; i++ ) {
    printf("\t%d is matched to %d\n", i, LRmatches[i] );
  }
  
  return LRmatches;
}

/**Function********************************************************************

  Synopsis    [Find an augmenting path, starting at an unmatched vertex.]

  Description [Find an augmenting path, starting at an unmatched vertex.]

******************************************************************************/

int 
BFStoAugmentingVertex(
  int startingVertex,
  randomDSSMatrix *A,
  map<int, int>& BFSshells,
  int *RLmatches )
{
  queue<int> bfsQ;
  bfsQ.push( startingVertex );

  while ( bfsQ.size() > 0 ) {
    int tmpVertex = bfsQ.front();
    bfsQ.pop();
    printf("Size of BFSshells = %d\n", BFSshells.size() ); 
    for( int j=0; j < A->dimension; j++ ) {
      if ( A->matrix[tmpVertex][j] > randomDSSMatrix::EPSILON &&  BFSshells.end() == find_if( BFSshells.begin(), BFSshells.end(), myPred(j) ) ) {
        BFSshells[j] = tmpVertex;
        if ( j == 2 ) {
          printf("Mapped %d to %d\n", j, (*BFSshells.find(j)).second );
          assert( tmpVertex == (*BFSshells.find(j)).second );
        }
        if ( RLmatches[j] == -1 ) {
          return j;
        }
        else { 
          bfsQ.push(RLmatches[j]);
        }
      }
    }
  } 
  return -1;
}


/**Function********************************************************************

  Synopsis    [Write the decomposition to cout.]

  Description [Write the decomposition to cout. It would be a good idea to sort
		the decomposition based on the weights, get a better idea
		of the relative importance of the constituents.]

******************************************************************************/

void randomDSSMatrix::printDecomp() 
{
  double rowSum;
  double colSum;

  for(int id=0; id < decompMatrices.size(); id++ ) {
    printf("Perm matrix %d is: (weight is %.6f)\n", id, decompWeights[id] );
    for ( int i = 0 ; i < dimension; i++ ) {
      for (int j=0; j < dimension; j++ ) {
        printf("%.4f  ", (decompMatrices[id])->matrix[i][j]);
      }
      printf("\n");
    }
    printf("\n");
  }
}
  
    
/**Function********************************************************************

  Synopsis    [main driver routine.]

  Description [main driver routine.  Creates a random (?) doubly substochastic
		matrix of dimension 4, and then converts it to a doubly
		stochastic matrix, and then performs the Birkhoff decomposition.] 
		
******************************************************************************/

int 
main() {
  
  randomDSSMatrix tmpMatrix(4);

  tmpMatrix.print();
  printf("\n\n\n");

  tmpMatrix.convertToDSMatrix();
  tmpMatrix.print();
  tmpMatrix.decompose();
  tmpMatrix.printDecomp();
  
  return 0;
}