/*
* This file is part of libmunkres, a library to solve the
* assignment problem.
*
* input : N x N matrix with costs of N workers solving N tasks
* output: assigmentmatrix M with minimal cost
*
* based on c# implementation from http://csclab.murraystate.edu/bob.pilgrim/445/munkres.html
* "Algorithms for Assignment and Transportation Problems", James Munkres, 
*	Journal of the Society for Industrial and Applied Mathematics Volume 5, Number 1, March, 1957
* original author: Robert A. Pilgrim - Murray State University - rpilgirm <a> murraystate <dot> edu
*
* Copyright(c) sschulz <AT> techfak.uni-bielefeld.de
* http://opensource.cit-ec.de/projects/libmunkres
*
* This file may be licensed under the terms of the
* GNU Lesser General Public License Version 3 (the ``LGPL''),
* or (at your option) any later version.
*
* Software distributed under the License is distributed
* on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
* express or implied. See the LGPL for the specific language
* governing rights and limitations.
*
* You should have received a copy of the LGPL along with this
* program. If not, go to http://www.gnu.org/licenses/lgpl.html
* or write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
* The development of this software was supported by the
* Excellence Cluster EXC 277 Cognitive Interaction Technology.
* The Excellence Cluster EXC 277 is a grant of the Deutsche
* Forschungsgemeinschaft (DFG) in the context of the German
* Excellence Initiative.
*/

#include "munkres.h"

#define DEBUG_MUNKRES_STEPS 0

Munkres::Munkres(){
	init_matrix(0,0,NULL);
}

bool Munkres::init_matrix(int rows, int cols, int *data){
	if ((rows >= 30) || (cols >= 30)){
		printf("> ERROR: munkres maximum matrix size exceeded (30x30), inputsize is (%dx%d)\n",rows,cols);
		return false;
	}
	
	nrow = rows;
	ncol = cols;
	for(int x=0; x<rows; x++){
		for(int y=0; y<cols; y++){
			C[x][y] = *data;
			data++;
		}
	
	}
	
	path_count = 0;
	path_row_0 = 0;
	path_col_0 = 0;
	asgn = 0;
	step = 1;
	
	resetMaskandCovers();
	return true;
}


int Munkres::get_col_match_for_row(int row){
	for (int c = 0; c < ncol; c++){
		if (M[row][c] == 1){
			return c;
		}
	}
	return -1;
}

int Munkres::get_row_match_for_col(int col){
	for (int r = 0; r < nrow; r++){
		if (M[r][col] == 1){
			return r;
		}
	}
	return -1;
}

void Munkres::resetMaskandCovers(){
	for (int r = 0; r < nrow; r++){
		RowCover[r] = 0;
		for (int c = 0; c < ncol; c++){
			M[r][c] = 0;
		}
	}
	for (int c = 0; c < ncol; c++){
                ColCover[c] = 0;
	}
}

//For each row of the cost matrix, find the smallest element and subtract
//it from every element in its row.  When finished, Go to Step 2.
void Munkres::step_one(int *step){
	int min_in_row;

	for (int r = 0; r < nrow; r++){
		min_in_row = C[r][0];
		for (int c = 0; c < ncol; c++){
			if (C[r][c] < min_in_row){
				min_in_row = C[r][c];
			}
		}
		for (int c = 0; c < ncol; c++){
			C[r][c] -= min_in_row;
		}
	}
	*step = 2;
}

//Find a zero (Z) in the resulting matrix.  If there is no starred 
//zero in its row or column, star Z. Repeat for each element in the 
//matrix. Go to Step 3.
void Munkres::step_two(int *step){
	for (int r = 0; r < nrow; r++){
		for (int c = 0; c < ncol; c++){
			if (C[r][c] == 0 && RowCover[r] == 0 && ColCover[c] == 0){
				M[r][c] = 1;
				RowCover[r] = 1;
				ColCover[c] = 1;
			}
		}
	}
	for (int r = 0; r < nrow; r++){
		RowCover[r] = 0;
	}
	for (int c = 0; c < ncol; c++){
		ColCover[c] = 0;
	}
	
	*step = 3;
}


//Cover each column containing a starred zero.  If K columns are covered, 
//the starred zeros describe a complete set of unique assignments.  In this 
//case, Go to DONE, otherwise, Go to Step 4.
void Munkres::step_three(int *step){
	int colcount;
	for (int r = 0; r < nrow; r++){
		for (int c = 0; c < ncol; c++){
			if (M[r][c] == 1){
				ColCover[c] = 1;
			}
		}
	}
	
	colcount = 0;
	for (int c = 0; c < ncol; c++){
		if (ColCover[c] == 1){
			colcount += 1;
		}
	}
	if (colcount >= ncol || colcount >=nrow){
		*step = 7;
	}else{
		*step = 4;
	}
}

//methods to support step 4
void Munkres::find_a_zero(int *row, int *col){
	int r = 0;
	int c;
	bool done;
	*row = -1;
	*col = -1;
	done = false;
	
	while (!done){
		c = 0;
		while (true){
			if ((C[r][c] == 0) && (RowCover[r] == 0) && (ColCover[c] == 0)){
				*row = r;
				*col = c;
				done = true;
			}
			c += 1;
			if (c >= ncol || done){
				break;
			}
		}
		r += 1;
		if (r >= nrow){
			done = true;
		}
	}
}

bool Munkres::star_in_row(int row){
	bool tmp = false;
	for (int c = 0; c < ncol; c++){
		if (M[row][c] == 1){
			tmp = true;
		}
	}
	return tmp;
}


void Munkres::find_star_in_row(int row, int *col){
	*col = -1;
	for (int c = 0; c < ncol; c++){
		if (M[row][c] == 1){
			*col = c;
		}
	}
}

//Find a noncovered zero and prime it.  If there is no starred zero 
//in the row containing this primed zero, Go to Step 5.  Otherwise, 
//cover this row and uncover the column containing the starred zero. 
//Continue in this manner until there are no uncovered zeros left. 
//Save the smallest uncovered value and Go to Step 6.
void Munkres::step_four(int *step){
	int row = -1;
	int col = -1;
	bool done;

	done = false;
	while (!done){
		find_a_zero(&row, &col);
		if (row == -1){
			done = true;
			*step = 6;
		}else{
			M[row][col] = 2;
			if (star_in_row(row)){
				find_star_in_row(row, &col);
				RowCover[row] = 1;
				ColCover[col] = 0;
			}else{
				done = true;
				*step = 5;
				path_row_0 = row;
				path_col_0 = col;
			}
		}
	}
}

// methods to support step 5
void Munkres::find_star_in_col(int c, int *r){
	*r = -1;
	for (int i = 0; i < nrow; i++){
		if (M[i][c] == 1){
			*r = i;
		}
	}
}

void Munkres::find_prime_in_row(int r, int *c){
	for (int j = 0; j < ncol; j++){
		if (M[r][j] == 2){
			*c = j;
		}
        }
}

void Munkres::augment_path(){
	for (int p = 0; p < path_count; p++){
		if (M[path[p][0]][path[p][1]] == 1){
			M[path[p][0]][path[p][1]] = 0;
		}else{
			M[path[p][0]][path[p][1]] = 1;
		}
	}
}

void Munkres::clear_covers(){
	for (int r = 0; r < nrow; r++){
		RowCover[r] = 0;
	}
	for (int c = 0; c < ncol; c++){
		ColCover[c] = 0;
	}
}

void Munkres::erase_primes(){
	for (int r = 0; r < nrow; r++){
		for (int c = 0; c < ncol; c++){
			if (M[r][c] == 2){
				M[r][c] = 0;
			}
		}
	}
}


//Construct a series of alternating primed and starred zeros as follows.  
//Let Z0 represent the uncovered primed zero found in Step 4.  Let Z1 denote 
//the starred zero in the column of Z0 (if any). Let Z2 denote the primed zero 
//in the row of Z1 (there will always be one).  Continue until the series 
//terminates at a primed zero that has no starred zero in its column.  
//Unstar each starred zero of the series, star each primed zero of the series, 
//erase all primes and uncover every line in the matrix.  Return to Step 3.
void Munkres::step_five(int *step){
	bool done;
	int r = -1;
	int c = -1;

	path_count = 1;
	path[path_count - 1][0] = path_row_0;
	path[path_count - 1][1] = path_col_0;
	done = false;
	
	while (!done){
		find_star_in_col(path[path_count - 1][1], &r);
		if (r > -1){
			path_count += 1;
			path[path_count - 1][0] = r;
			path[path_count - 1][1] = path[path_count - 2][1];
		}else{
			done = true;
		}
		
		if (!done){
			find_prime_in_row(path[path_count - 1][0], &c);
			path_count += 1;
			path[path_count - 1][0] = path[path_count - 2][0];
			path[path_count - 1][1] = c;
		}
	}
	augment_path();
	clear_covers();
	erase_primes();
	*step = 3;
}

//methods to support step 6
void Munkres::find_smallest(int *minval){
	for (int r = 0; r < nrow; r++){
		for (int c = 0; c < ncol; c++){
			if (RowCover[r] == 0 && ColCover[c] == 0){
				if (*minval > C[r][c]){
					*minval = C[r][c];
				}
			}
		}
	}
}

//Add the value found in Step 4 to every element of each covered row, and subtract 
//it from every element of each uncovered column.  Return to Step 4 without 
//altering any stars, primes, or covered lines.
void Munkres::step_six(int *step){
	int minval = std::numeric_limits<int>::max();
	find_smallest(&minval);
	
	for (int r = 0; r < nrow; r++){
		for (int c = 0; c < ncol; c++){
			if (RowCover[r] == 1){
				C[r][c] += minval;
			}
			if (ColCover[c] == 0){
				C[r][c] -= minval;
			}
		}
	}
	*step = 4;
}

void Munkres::step_seven(int *step){
	if(DEBUG_MUNKRES_STEPS) printf("\n> completed\n");
}


void Munkres::run(){
	bool done = false;
	while (!done){
		if(DEBUG_MUNKRES_STEPS) show_cost_matrix();
		if(DEBUG_MUNKRES_STEPS) show_mask_matrix();
		
		switch (step){
			case 1:
				step_one(&step);
				break;
			case 2:
				step_two(&step);
				break;
			case 3:
				step_three(&step);
				break;
			case 4:
				step_four(&step);
				break;
			case 5:
				step_five(&step);
				break;
			case 6:
				step_six(&step);
				break;
			case 7:
				step_seven(&step);
				done = true;
				break;
		}
	}
// show_mask_matrix();
// show_cost_matrix();
}

void Munkres::show_cost_matrix(){
	printf("\n\n> STEP %d\n",step);
	for (int r = 0; r < nrow; r++){
		printf("\n    ");
		for (int c = 0; c < ncol; c++){
			printf("%5d ",C[r][c]);
		}
	}
}

void Munkres::show_mask_matrix(){
	printf("\n\n      ");
	for (int c = 0; c < ncol; c++){
		printf(" %4d",ColCover[c]);
	}
	for (int r = 0; r < nrow; r++){
		printf("\n %4d  ",RowCover[r]);
		for (int c = 0; c < ncol; c++){
			printf("%4d ",M[r][c]);
		}
	}
}