/***************************************************************************
 *
 * Multitouch X driver
 * Copyright (C) 2008 Henrik Rydberg <rydberg@euromail.se>
 *
 * 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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 **************************************************************************/

#include "match.h"
#include <string.h>
#include <stdio.h>

/**
 * Bitmap implementation of the hungarian algorithm (GPL license)
 *
 * Copyright (C) 2008 Henrik Rydberg <rydberg@euromail.se>
 *
 * Based on code released by Markus Buehren (2004) (BSD license)
 *
 * Copyright (C) 2004, Markus Buehren. All rights reserved.
 * See CREDITS file for full license terms.
 *
 */

typedef unsigned col_t[1];
typedef unsigned mat_t[DIM_FINGER];

#define GET1(m, x) ((m[0] >> (x)) & 1U)
#define SET1(m, x) (m[0] |= (1U << (x)))
#define CLEAR1(m, x) (m[0] &= ~(1U << (x)))

#define GET2(m, row, col) ((m[col] >> (row)) & 1U)
#define SET2(m, row, col) (m[col] |= (1U << (row)))
#define CLEAR2(m, row, col) (m[col] &= ~(1U << (row)))

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

static void buildixvector(int *ix, mat_t mstar, int nrows, int ncols)
{
	int row, col;
	for (row = 0; row < nrows; row++) {
		for (col = 0; col < ncols; col++) {
			if (GET2(mstar, row, col)) {
				ix[row] = col;
				break;
			}
		}
	}
}


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

static void step2a(int *ix, int *mdist, mat_t mstar, mat_t nmstar,
		   mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols,
		   int dmin);
static void step2b(int *ix, int *mdist, mat_t mstar, mat_t nmstar,
		   mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols,
		   int dmin);
static void step3(int *ix, int *mdist, mat_t mstar, mat_t nmstar,
		  mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols,
		  int dmin);
static void step4(int *ix, int *mdist, mat_t mstar, mat_t nmstar,
		  mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols,
		  int dmin, int row, int col);
static void step5(int *ix, int *mdist, mat_t mstar, mat_t nmstar,
		  mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols,
		  int dmin);

static void ixoptimal(int *ix, int *mdist, int nrows, int ncols)
{
	int *mdistTemp, *mdistEnd, *columnEnd, value, minValue;
	int dmin, row, col;
	col_t ccol, crow;
	mat_t mstar, mprime, nmstar;

	memset(ccol, 0, sizeof(col_t));
	memset(crow, 0, sizeof(col_t));
	memset(mstar, 0, sizeof(mat_t));
	memset(mprime, 0, sizeof(mat_t));
	memset(nmstar, 0, sizeof(mat_t));

	/* initialization */
	for (row = 0; row < nrows; row++)
		ix[row] = -1;

	mdistEnd = mdist + nrows * ncols;

	/* preliminary steps */
	if (nrows <= ncols) {
		dmin = nrows;

		for (row = 0; row < nrows; row++) {
			/* find the smallest element in the row */
			mdistTemp = mdist + row;
			minValue = *mdistTemp;
			mdistTemp += nrows;
			while (mdistTemp < mdistEnd) {
				value = *mdistTemp;
				if (value < minValue)
					minValue = value;
				mdistTemp += nrows;
			}

			/* subtract the smallest element from each element
			   of the row */
			mdistTemp = mdist + row;
			while (mdistTemp < mdistEnd) {
				*mdistTemp -= minValue;
				mdistTemp += nrows;
			}
		}

		/* Steps 1 and 2a */
		for (row = 0; row < nrows; row++) {
			for (col = 0; col < ncols; col++) {
				if (mdist[row + nrows * col] != 0)
					continue;
				if (GET1(ccol, col))
					continue;
				SET2(mstar, row, col);
				SET1(ccol, col);
				break;
			}
		}
	} else {
		dmin = ncols;

		for (col = 0; col < ncols; col++) {
			/* find the smallest element in the column */
			mdistTemp = mdist + nrows*col;
			columnEnd = mdistTemp + nrows;

			minValue = *mdistTemp++;
			while (mdistTemp < columnEnd) {
				value = *mdistTemp++;
				if (value < minValue)
					minValue = value;
			}

			/* subtract the smallest element from each element
			   of the column */
			mdistTemp = mdist + nrows*col;
			while (mdistTemp < columnEnd)
				*mdistTemp++ -= minValue;
		}

		/* Steps 1 and 2a */
		for (col = 0; col < ncols; col++) {
			for (row = 0; row < nrows; row++) {
				if (mdist[row + nrows * col] != 0)
					continue;
				if (GET1(crow, row))
					continue;
				SET2(mstar, row, col);
				SET1(ccol, col);
				SET1(crow, row);
				break;
			}
		}
		memset(crow, 0, sizeof(col_t));
	}

	/* move to step 2b */
	step2b(ix, mdist, mstar, nmstar,
	       mprime, ccol, crow, nrows, ncols,
	       dmin);
}

/********************************************************/
static void step2a(int *ix, int *mdist, mat_t mstar, mat_t nmstar,
		   mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols,
		   int dmin)
{
	int col, row;

	/* cover every column containing a starred zero */
	for (col = 0; col < ncols; col++) {
		for (row = 0; row < nrows; row++) {
			if (!GET2(mstar, row, col))
				continue;
			SET1(ccol, col);
			break;
		}
	}

	/* move to step 3 */
	step2b(ix, mdist, mstar, nmstar,
	       mprime, ccol, crow, nrows, ncols,
	       dmin);
}

/********************************************************/
static void step2b(int *ix, int *mdist, mat_t mstar, mat_t nmstar,
		   mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols,
		   int dmin)
{
	int col, ncc;

	/* count covered columns */
	ncc = 0;
	for (col = 0; col < ncols; col++)
		if (GET1(ccol, col))
			ncc++;

	if (ncc == dmin) {
		/* algorithm finished */
		buildixvector(ix, mstar, nrows, ncols);
	} else {
		/* move to step 3 */
		step3(ix, mdist, mstar, nmstar,
		      mprime, ccol, crow, nrows, ncols,
		      dmin);
	}

}

/********************************************************/
static void step3(int *ix, int *mdist, mat_t mstar, mat_t nmstar,
		  mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols,
		  int dmin)
{
	int zerosFound;
	int row, col, cstar;

	zerosFound = 1;
	while (zerosFound) {
		zerosFound = 0;
		for (col = 0; col < ncols; col++) {
			if (GET1(ccol, col))
				continue;
			for (row = 0; row < nrows; row++) {
				if (mdist[row + nrows * col] != 0)
					continue;
				if (GET1(crow, row))
					continue;

				/* prime zero */
				SET2(mprime, row, col);

				/* find starred zero in current row */
				for (cstar = 0; cstar < ncols; cstar++)
					if (GET2(mstar, row, cstar))
						break;

				if (cstar == ncols) { /* no starred zero */
					/* move to step 4 */
					step4(ix, mdist, mstar, nmstar,
					      mprime, ccol, crow, nrows, ncols,
					      dmin, row, col);
					return;
				} else {
					SET1(crow, row);
					CLEAR1(ccol, cstar);
					zerosFound = 1;
					break;
				}
			}
		}
	}

	/* move to step 5 */
	step5(ix, mdist, mstar, nmstar,
	      mprime, ccol, crow, nrows, ncols,
	      dmin);
}

/********************************************************/
static void step4(int *ix, int *mdist, mat_t mstar, mat_t nmstar,
		  mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols,
		  int dmin, int row, int col)
{
	int n, rstar, cstar, primeRow, primeCol;

	/* generate temporary copy of mstar */
	memcpy(nmstar, mstar, sizeof(mat_t));

	/* star current zero */
	SET2(nmstar, row, col);

	/* find starred zero in current column */
	cstar = col;
	for (rstar = 0; rstar < nrows; rstar++)
		if (GET2(mstar, rstar, cstar))
			break;

	while (rstar < nrows) {
		/* unstar the starred zero */
		CLEAR2(nmstar, rstar, cstar);

		/* find primed zero in current row */
		primeRow = rstar;
		for (primeCol = 0; primeCol < ncols; primeCol++)
			if (GET2(mprime, primeRow, primeCol))
				break;

		/* star the primed zero */
		SET2(nmstar, primeRow, primeCol);

		/* find starred zero in current column */
		cstar = primeCol;
		for (rstar = 0; rstar < nrows; rstar++)
			if (GET2(mstar, rstar, cstar))
				break;
	}

	/* use temporary copy as new mstar */
	/* delete all primes, uncover all rows */
	memcpy(mstar, nmstar, sizeof(mat_t));
	memset(mprime, 0, sizeof(mat_t));
	memset(crow, 0, sizeof(col_t));

	/* move to step 2a */
	step2a(ix, mdist, mstar, nmstar,
	       mprime, ccol, crow, nrows, ncols,
	       dmin);
}

/********************************************************/
static void step5(int *ix, int *mdist, mat_t mstar, mat_t nmstar,
		  mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols,
		  int dmin)
{
	int h = 0, value;
	int row, col, found = 0;

	/* find smallest uncovered element h */
	for (row = 0; row < nrows; row++) {
		if (GET1(crow, row))
			continue;
		for (col = 0; col < ncols; col++) {
			if (GET1(ccol, col))
				continue;
			value = mdist[row + nrows * col];
			if (!found || value < h) {
				h = value;
				found = 1;
			}
		}
	}

	/* where to go if nothing uncovered? */
	if (!found)
		return;

	/* add h to each covered row */
	for (row = 0; row < nrows; row++) {
		if (!GET1(crow, row))
			continue;
		for (col = 0; col < ncols; col++)
			mdist[row + nrows * col] += h;
	}

	/* subtract h from each uncovered column */
	for (col = 0; col < ncols; col++) {
		if (GET1(ccol, col))
			continue;
		for (row = 0; row < nrows; row++)
			mdist[row + nrows * col] -= h;
	}

	/* move to step 3 */
	step3(ix, mdist, mstar, nmstar,
	      mprime, ccol, crow, nrows, ncols,
	      dmin);
}

void match_fingers(int ix[DIM_FINGER], int A[DIM2_FINGER], int nrow, int ncol)
{
	ixoptimal(ix, A, nrow, ncol);
}