/* truecluster/src/munkres.c
 * C implementation of the Hungarian Method for non-square matrices
 * (C) 2005-2007 Dr. Jens Oehlschlägel
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include <R_ext/Arith.h>        /* NA handling */
#include <Rmath.h>              /* fmin2 */
#include <R_ext/Random.h>       /* ..RNGstate */

#include <R_ext/Applic.h>       /* NA handling */
#include <R_ext/Print.h>        /* RPrintf */

#include <R_ext/Utils.h>      /* sort routines et al */

#define RELTOL 1e-5


int rn;                   // no. of rows
int cn;                   // no. of cols
double *C;                // double cost  matrix with cn>=rn
int *M;                   // integer mask matrix with cn>=rn 1=stared zero 2=primed zero
int path0_row, path0_col;  // zero found in step 4
int *path_row, *path_col;  // path in step 5
int *R_cov,*C_cov;        // integer vectors for covering

void PrintMat(char mess[30])
{
  int r,c;
  Rprintf(mess);
  Rprintf("\n");
  for (r=0; r<rn; r++){
    for (c=0; c<cn; c++){
      Rprintf("%d ", M[r + c * rn]);
    }
    Rprintf("| %d", R_cov[r]);
    Rprintf("  -  ");
    for (c=0; c<cn; c++){
      Rprintf("%f ", C[r + c * rn]);
    }
    Rprintf("\n");
  }
  for (c=0; c<cn; c++)
    Rprintf("- ");
  Rprintf("\n");
  for (c=0; c<cn; c++)
    Rprintf("%d ", C_cov[c]);
  Rprintf(" column covering\n");
  Rprintf("Z0 %d,%d\n", path0_row, path0_col);
  Rprintf("\n ");
}

void clear_covers()
{
  int r,c;
  for (r=0; r<rn; r++)
    R_cov[r]=0;
  for (c=0; c<cn; c++)
    C_cov[c]=0;
}


int step1()
{
  int r,c;
  double minval;
  for (r=0; r<rn; r++){
    minval = C[r];
    for (c=1; c<cn; c++){
      if (minval>C[r + c * rn])
        minval = C[r + c * rn];
    }
    for (c=0; c<cn; c++){
      C[r + c * rn] = C[r + c * rn] - minval;
    }
  }
  return(2);
}

int step2()
{
  int r,c;
  for (r=0; r<rn; r++){
    for (c=0; c<cn; c++){
      if (C[r + c * rn]==0 && C_cov[c]==0 && R_cov[r]==0){
        M[r + c * rn] = 1;
        R_cov[r] = 1;
        C_cov[c] = 1;
      }
    }
  }
  clear_covers();
  return(3);
}

int step3()
{
  int r,c,count=0;
  for (c=0; c<cn; c++){
    for (r=0; r<rn; r++){
      if (M[r + c * rn] == 1){
        C_cov[c] = 1;
      }
    }
    count += C_cov[c];
  }
  if (count>=rn)
    return(7);
  else
    return(4);
}


int step4()
{
  Rboolean done2, done=FALSE;
  int step=4;
  int r,c, row, col, col_old;
  while (!done){
    // find_an_uncovered_zero
    r=0;
    done2=FALSE;
    row=-1;
    col=-1;
    while(1){
      c=0;
      while(1){
        if (C[r + c * rn]==0 && R_cov[r]==0 && C_cov[c]==0){
          row=r;
          col=c;
          done2=TRUE;
        }
        //Rprintf("row,col %d,%d r,c %d,%d rn,cn %d,%d\n", row, col, r,c, rn, cn  );
        c+=1;
        if (c==cn)
         break;
      }
      r+=1;
      if (r==rn)
        done2=TRUE;
      if (done2)
        break;
    }
    // end find_an_uncovered_zero
    if (row==-1){
      done=TRUE;
      step=6;
    }else{
      M[row + col * rn] = 2;
      col_old = col;
      // try to find stared zero in row, if successfull then col>-1
      col=-1;
      for (c=0; c<cn; c++){
        if (M[row + c * rn]==1 /* && C[row + c * rn]==0 */ ){
          // xx Can we have stared non-zero
          if (C[row + c * rn]!=0){
            PrintMat("Found stared non-zero");
          }
          col=c;
          break;
        }
      }
      // end try to find stared zero in row, if successfull then col>-1
      if (col==-1){
        path0_row=row;
        path0_col=col_old;
        done=TRUE;
        step=5;
      }else{
        R_cov[row]=1;
        C_cov[col]=0;
      }
    }
  }
  return(step);
}


// step5

  int find_star_in_col(int c)
  {
    int r, ret=-1;
    for (r=0; r<rn; r++){
      if (M[r + c * rn]==1)
        ret=r;
    }
    return (ret);
  }

  int find_prime_in_row(int r)
  {
    int c, ret=-1;
    for (c=0; c<cn; c++){
      if (M[r + c * rn]==2)
        ret=c;
    }
    return(ret);
  }

  void convert_path(int path_i)
  {
    int i, index;
    for (i=0; i<=path_i; i++){
      index = path_row[i] + path_col[i] * rn;
      if (M[index]==1)
        M[index]=0;
      else
        M[index]=1;
    }
  }

  void erase_primes()
  {
    int r,c;
    for (r=0; r<rn; r++)
      for (c=0; c<cn; c++)
        if (M[r + c * rn]==2)
          M[r + c * rn]=0;
  }


int step5()
{
  Rboolean done=FALSE;
  int r,c;
  int path_i=0;
  path_row[0] = path0_row;
  path_col[0] = path0_col;
  while (!done){
    r = find_star_in_col(path_col[path_i]);
    if (r>-1){
      path_i+=1;
      path_row[path_i]=r;
      path_col[path_i]=path_col[path_i-1];
      //Rprintf("Z%d %d,%d\n", path_i, path_row[path_i], path_col[path_i]);
    }else{
      done=TRUE;
      //Rprintf("Path done\n");
    }
    if (!done){
      c = find_prime_in_row(path_row[path_i]);
      path_i+=1;
      path_row[path_i]=path_row[path_i-1];
      path_col[path_i]=c;
      //Rprintf("Z%d %d,%d\n", path_i, path_row[path_i], path_col[path_i]);
    }
  }
  convert_path(path_i);
  clear_covers();
  erase_primes();

  return(3);
}


// step6

    double find_smallest()
    {
      int r,c;
      double minval = R_PosInf;
      for (r=0; r<rn; r++){
        for (c=0; c<cn; c++)
         if (R_cov[r]==0 && C_cov[c]==0){
           if (minval>C[r + c * rn])
              minval = C[r + c * rn];
         }
      }
      return (minval);
    }

int step6()
{
  int r,c;
  double minval=find_smallest();
  for (r=0; r<rn; r++)
    for (c=0; c<cn; c++){
        if (R_cov[r]==1)
          C[r + c * rn] = C[r + c * rn] + minval;
        if (C_cov[c]==0)
          C[r + c * rn] = C[r + c * rn] - minval;
    }
  return(4);
}




void R_tc_munkres(
  double *Cost      // double input matrix with cn>=rn
, int *Rn           // no. of rows
, int *Cn           // no. of cols
, int *row          // integer output rows
, int *col          // integer output cols
)
{
  int step=1;
  int r,c;
  rn= *Rn;
  cn= *Cn;
  C = Cost;
  R_cov = row;
  C_cov = col;
  M = Calloc(sizeof(int)*rn*cn, int);
  path_row = Calloc(sizeof(int)*rn*cn, int);  // xx how long can the path become??
  path_col = Calloc(sizeof(int)*rn*cn, int);  // xx how long can the path become??
  Rboolean done = FALSE;
  //PrintMat("start");
  while(!done){
    //Rprintf("pre step%d\n", step);
    switch(step){
      case 1:
        step = step1();
        break;
      case 2:
        step = step2();
        break;
      case 3:
        step = step3();
        break;
      case 4:
        step = step4();
        break;
      case 5:
        step = step5();
        break;
      case 6:
        step = step6();
        break;
      default: done = TRUE;
    }
    //PrintMat("next");
  }

  //PrintMat("Solution");

  // convert solution matrix to row/col indices
  for (r=0; r<rn; r++){
    for (c=0; c<cn; c++){
        if (M[r + c * rn]==1){
          row[r] = r;
          col[r] = c;
        }
    }
    for (; c<cn; c++){
      col[c] = NA_INTEGER;
    }
  }

  Free(path_row);
  Free(path_col);
  Free(M);
}

