/* Header File Including ------------------------------------------------------------------------ */#include <stdint.h>
#include <string.h>
#include "m_munkres.h"

//#include "loc_vivo_pdr.h"
/* Macro Definition ----------------------------------------------------------------------------- */
#define COST(i, j)              cost[(i) + (j) * m]
#define MARK(i, j)              mark[(i) + (j) * m]
#define PATH(i, j)              path[(i) + (j) * 64]
#define CLR_ALL(flag)           (*flag##_cover = 0)
#define SET_FLG(flag, index)    (*flag##_cover |= (1 << (index)))
#define CLR_FLG(flag, index)    (*flag##_cover &= ~(1 << (index)))
#define TST_FLG(flag, index)    (*flag##_cover & (1 << (index)))

/* Global Variable Definition ------------------------------------------------------------------- */
static float   g_cost[64];
static int     g_mark[64];
static uint8_t g_row_cover;
static uint8_t g_col_cover;



/* Function Declaration ------------------------------------------------------------------------- */
static int step_one(float *cost, int m, int n);
static int step_two(float *cost, int *mark, int m, int n, uint8_t *col_cover);
static int step_three(int *mark, int m, int n, uint8_t *col_cover);
static int step_four(float *cost, int *mark, int m, int n, uint8_t *row_cover, uint8_t *col_cover, int *path_start_i, int *path_start_j);
static int step_five(int *mark, int m, int n, uint8_t *row_cover, uint8_t *col_cover, int path_start_i, int path_start_j);
static int step_six(float *cost, int m, int n, uint8_t *row_cover, uint8_t *col_cover);

#ifdef __MATLAB_DBG
void display_cost_matrix_and_cover(float *cost, int m, int n, uint8_t row_cover, uint8_t col_cover);
void display_mark_matrix_and_cover(int *mark, int m, int n, uint8_t row_cover, uint8_t col_cover);
#endif

/* Function Definition -------------------------------------------------------------------------- */
int MUNKRES_get_assignment(int *assignment, float *cost, int m, int n) {
    int i = 0;
    int j = 0;
    int step;

	#if PRINT_ALGO_RUN_TIME
    uint64 time_algo_1 = 0l;
    uint64 time_algo_2 = 0l;
    uint64 time_algo_3 = 0l;
	uint64 time_algo_4 = 0l;

    #endif
	
    if (m > 8 || n > 8) {
        return MUNKRES_EXCESSIVE_MATRIX;
    }

    memcpy(g_cost, cost, m * n * sizeof(*g_cost));
    memset(g_mark, 0, m * n * sizeof(*g_mark));
    g_row_cover = 0;
    g_col_cover = 0;

    step = 0;
	#if PRINT_ALGO_RUN_TIME
    time_algo_1 = uTimetick_Get();
    #endif
    while (7 != step) {
#ifdef __MATLAB_DBG
        mexPrintf("step %d:\n", step);
#endif
        switch (step) {
            case 0:
            case 1:
                step = step_one(g_cost, m, n);
                break;
            case 2:
                step = step_two(g_cost, g_mark, m, n, &g_col_cover);
                break;
            case 3:
                step = step_three(g_mark, m, n, &g_col_cover);
                break;
            case 4:
                step = step_four(g_cost, g_mark, m, n, &g_row_cover, &g_col_cover, &i, &j);
				#if PRINT_ALGO_RUN_TIME
    time_algo_3 = uTimetick_Get();
	time_algo_3 = (time_algo_3-time_algo_1);
   
    #if defined(LOCATION_PLATFORM_QCOM_MODEM)
    Geek_PDR_LOGH_NMEA("step_4 run time ms %d",(int)time_algo_3,0,0,0);
	#endif
    #endif
                break;
            case 5:
                step = step_five(g_mark, m, n, &g_row_cover, &g_col_cover, i, j);
				#if PRINT_ALGO_RUN_TIME
    time_algo_4 = uTimetick_Get();
	time_algo_4 = (time_algo_4-time_algo_1);
   
    #if defined(LOCATION_PLATFORM_QCOM_MODEM)
    Geek_PDR_LOGH_NMEA("step_5 run time ms %d",(int)time_algo_4,0,0,0);
	#endif
    #endif
                break;
            case 6:
                step = step_six(g_cost, m, n, &g_row_cover, &g_col_cover);
                break;
            default:
                step = 7;
                break;
        }
#ifdef __MATLAB_DBG
        mexPrintf("  cost:\n", step);
        display_cost_matrix_and_cover(g_cost, m, n, g_row_cover, g_col_cover);
        mexPrintf("  mark:\n", step);
        display_mark_matrix_and_cover(g_mark, m, n, g_row_cover, g_col_cover);
#endif
    }
	#if PRINT_ALGO_RUN_TIME
    time_algo_2 = uTimetick_Get();
    #endif
	 #if PRINT_ALGO_RUN_TIME
    
    time_algo_2 = (time_algo_2-time_algo_1);
   // time_algo_3 = (time_algo_3-time_algo_2);
   
    #if defined(LOCATION_PLATFORM_QCOM_MODEM)
    Geek_PDR_LOGH_NMEA("munkres run time ms %d",(int)time_algo_2,0,0,0);
	#endif
   // Geek_PDR_LOGH_NMEA("pdr2 run time ms %d %d",(int)time_algo_5,time_algo_6,0,0);
    #endif

    for (i = 0; i < m; ++i) {
        for (j = 0; j < n; ++j) {
            if (0 != g_mark[(i) + (j) * m]) {
                assignment[i] = j;
                break;
            }
        }
    }

    return MUNKRES_NO_ERROR;
}

static int step_one(float *cost, int m, int n) {
    int i;
    int j;
	float min;
    for (i = 0; i < m; ++i) {
        min = COST(i, 0);
        for (j = 1; j < n; ++j) {
            if (min > COST(i, j)) {
                min = COST(i, j);
            }
        }
        for (j = 0; j < n; ++j) {
            COST(i, j) -= min;
        }
    }

    return 2;
}

static int step_two(float *cost, int *mark, int m, int n, uint8_t *col_cover) {
    int i;
    int j;

    for (i = 0; i < m; ++i) {
        for (j = 0; j < n; ++j) {
            if (0 == COST(i, j) && 0 == TST_FLG(col, j)) {
                MARK(i, j) = 1;
                SET_FLG(col, j);
                break;
            }
        }
    }

    CLR_ALL(col);

    return 3;
}

static int step_three(int *mark, int m, int n, uint8_t *col_cover) {
    int i;
    int j;
    int count;

    CLR_ALL(col);
    for (i = 0; i < m; ++i) {
        for (j = 0; j < n; ++j) {
            if (1 == MARK(i, j)) {
                SET_FLG(col, j);
            }
        }
    }

    count = 0;
    for (j = 0; j < n; ++j) {
        count += (0 != TST_FLG(col, j) ? 1 : 0);
    }

    return (count >= m || count >= n) ? 7 : 4;
}

static void find_a_noncovered_zero(int *i, int *j, float *cost, int m, int n, uint8_t *row_cover, uint8_t *col_cover) {
    for (*i = 0; *i < m; ++*i) {
        if (0 != TST_FLG(row, *i)) {
            continue;
        }
        for (*j = 0; *j < n; ++*j) {
            if (0 == COST(*i, *j) && 0 == TST_FLG(col, *j)) {
                break;
            }
        }
        if (n != *j) {
            break;
        }
    }
}

static void find_a_star_zero_in_row(int *j, int i, int *mark, int m, int n) {
    for (*j = 0; *j < n; ++*j) {
        if (1 == MARK(i, *j)) {
            break;
        }
    }
}

static int step_four(float *cost, int *mark, int m, int n, uint8_t *row_cover, uint8_t *col_cover, int *path_start_i, int *path_start_j) {
    int i;
    int j;
	int jj;
    while (1) {
        find_a_noncovered_zero(&i, &j, cost, m, n, row_cover, col_cover);
        if (m == i) {
            return 6;
        }
        else {
            MARK(i, j) = 2;
            find_a_star_zero_in_row(&jj, i, mark, m, n);
            if (m != jj) {
                SET_FLG(row, i);
                CLR_FLG(col, jj);
                j = jj;
            }
            else {
                *path_start_i = i;
                *path_start_j = j;
                return 5;
            }
        }
    }
}

static void find_a_star_zero_in_col(int *i, int j, int *mark, int m) {
    for (*i = 0; *i < m; ++*i) {
        if (1 == MARK(*i, j)) {
            break;
        }
    }
}

static void find_a_prime_zero_in_row(int *j, int i, int *mark, int m, int n) {
    for (*j = 0; *j < n; ++*j) {
        if (2 == MARK(i, *j)) {
            break;
        }
    }
}

static int step_five(int *mark, int m, int n, uint8_t *row_cover, uint8_t *col_cover, int path_start_i, int path_start_j) {
    static int path[64 * 2];
    int i;
    int j;
    int k;

    k = 0;
    PATH(k, 0) = path_start_i;
    PATH(k, 1) = path_start_j;
    while (1) {
        find_a_star_zero_in_col(&i, PATH(k, 1), mark, m);
        if (m == i) {
            goto STEP_FIVE_EXIT;
        }
        else {
            PATH(k + 1, 0) = i;
            PATH(k + 1, 1) = PATH(k, 1);
            ++k;
            find_a_prime_zero_in_row(&j, PATH(k, 0), mark, m, n);
            PATH(k + 1, 0) = PATH(k, 0);
            PATH(k + 1, 1) = j;
            ++k;
        }
    }

STEP_FIVE_EXIT:
    /* augment path */
    for(; k >= 0; --k) {
        MARK(PATH(k, 0), PATH(k, 1)) = (1 == MARK(PATH(k, 0), PATH(k, 1)) ? 0 : 1);
    }
    /* clear covers */
    CLR_ALL(row);
    CLR_ALL(col);
    /* erease primes */
    for (i = 0; i < m; ++i) {
        for (j = 0; j < n; ++j) {
            if (2 == MARK(i, j)) {
                MARK(i, j) = 0;
                break;
            }
        }
    }

    return 3;
}

static int step_six(float *cost, int m, int n, uint8_t *row_cover, uint8_t *col_cover) {
    float min;
    int i;
    int j;

    min = 1e6;
    for (i = 0; i < m; ++i) {
        for (j = 0; j < n; ++j) {
            if (0 == TST_FLG(row, i) && 0 == TST_FLG(col, j)) {
                if (min > COST(i, j)) {
                    min = COST(i, j);
                }
            }
        }
    }

    for (i = 0; i < m; ++i) {
        for (j = 0; j < n; ++j) {
            if (0 != TST_FLG(row, i)) {
                COST(i, j) += min;
            }
            if (0 == TST_FLG(col, j)) {
                COST(i, j) -= min;
            }
        }
    }

    return 4;
}