#include "igraph_lsap.h"
#include "igraph_error.h"

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <limits.h>     /* INT_MAX */
#include <float.h>      /* DBL_MAX */
#include <assert.h>
#include <time.h>

/* constants used for improving readability of code */

#define COVERED       1
#define UNCOVERED     0
#define ASSIGNED      1
#define UNASSIGNED    0
#define TRUE          1
#define FALSE         0

#define MARKED        1
#define UNMARKED      0

#define REDUCE        1
#define NOREDUCE      0

typedef struct {
    int        n;            /* order of problem             */
    double   **C;            /* cost matrix          */
    double   **c;            /* reduced cost matrix      */
    int       *s;            /* assignment                   */
    int       *f;            /* column i is assigned to f[i] */
    int       na;            /* number of assigned items;    */
    int     runs;            /* number of iterations     */
    double  cost;            /* minimum cost         */
    time_t rtime;            /* time                         */
} AP;

/* public interface */

/* constructors and destructor */
AP     *ap_create_problem(double *t, int n);
AP     *ap_create_problem_from_matrix(double **t, int n);
AP     *ap_read_problem(char *file);
void    ap_free(AP *p);

int     ap_assignment(AP *p, int *res);
int     ap_costmatrix(AP *p, double **m);
int     ap_datamatrix(AP *p, double **m);
int     ap_iterations(AP *p);
int     ap_hungarian(AP *p);
double  ap_mincost(AP *p);
void    ap_print_solution(AP *p);
void    ap_show_data(AP *p);
int     ap_size(AP *p);
int     ap_time(AP *p);

/* error reporting */
void ap_error(char *message);

/* private functions */
void    preprocess(AP *p);
void    preassign(AP *p);
int     cover(AP *p, int *ri, int *ci);
void    reduce(AP *p, int *ri, int *ci);

int ap_hungarian(AP *p) {
    int      n;            /* size of problem */
    int    *ri;            /* covered rows    */
    int    *ci;            /* covered columns */
    time_t start, end;     /* timer           */
    int i, j, ok;

    start = time(0);

    n = p->n;
    p->runs = 0;

    /* allocate memory */
    p->s = calloc(1 + n, sizeof(int));
    p->f = calloc(1 + n, sizeof(int));

    ri = calloc(1 + n, sizeof(int));
    ci = calloc(1 + n, sizeof(int));

    if (ri == NULL || ci == NULL || p->s == NULL || p->f == NULL) {
        IGRAPH_ERROR("ap_hungarian: could not allocate memory", IGRAPH_ENOMEM);
    }

    preprocess(p);
    preassign(p);

    while (p->na < n) {
        if (REDUCE == cover(p, ri, ci)) {
            reduce(p, ri, ci);
        }
        ++p->runs;
    }

    end = time(0);

    p->rtime = end - start;

    /* check if assignment is a permutation of (1..n) */
    for (i = 1; i <= n; i++) {
        ok = 0;
        for (j = 1; j <= n; j++)
            if (p->s[j] == i) {
                ++ok;
            }
        if (ok != 1)
            IGRAPH_ERROR("ap_hungarian: error in assigment, is not a permutation",
                         IGRAPH_EINVAL);
    }

    /* calculate cost of assignment */
    p->cost = 0;
    for (i = 1; i <= n; i++) {
        p->cost += p->C[i][p->s[i]];
    }

    /* reset result back to base-0 indexing */
    for (i = 1; i <= n; i++) {
        p->s[i - 1] = p->s[i] - 1;
    }

    /* free memory */

    free(ri);
    free(ci);

    return 0;
}

/* abbreviated interface */
int ap_assignment(AP *p, int *res) {
    int i;

    if (p->s == NULL) {
        ap_hungarian(p);
    }

    for (i = 0; i < p->n; i++) {
        res[i] = p->s[i];
    }

    return p->n;
}


/*******************************************************************/
/* constructors                                                    */
/* read data from file                                             */
/*******************************************************************/

AP *ap_read_problem(char *file) {
    FILE *f;
    int i, j, c;
    int m, n;
    double x;
    double **t;
    int nrow, ncol;
    AP *p;

    f = fopen(file, "r");
    if (f == NULL) {
        return NULL;
    }

    t = (double **)malloc(sizeof(double*));

    m = 0;
    n = 0;

    nrow = 0;
    ncol = 0;

    while (EOF != (i = fscanf(f, "%lf", &x))) {
        if (i == 1) {
            if (n == 0) {
                t = (double **) realloc(t, (m + 1) * sizeof(double *));
                t[m] = (double *) malloc(sizeof(double));
            } else {
                t[m] = (double *) realloc(t[m], (n + 1) * sizeof(double));
            }

            t[m][n++] = x;

            ncol = (ncol < n) ? n : ncol;
            c = fgetc(f);
            if (c == '\n') {
                n = 0;
                ++m;
                nrow = (nrow < m) ? m : nrow;
            }
        }
    }
    fclose(f);

    /* prepare data */

    if (nrow != ncol) {
        /*
          fprintf(stderr,"ap_read_problem: problem not quadratic\nrows =%d, cols = %d\n",nrow,ncol);
        */
        igraph_warningf("ap_read_problem: problem not quadratic\nrows = %d, cols = %d\n",
                        __FILE__, __LINE__, -1, nrow, ncol);
        return NULL;
    }

    p = (AP*) malloc(sizeof(AP));
    p->n = ncol;

    p->C  = (double **) malloc((1 + nrow) * sizeof(double *));
    p->c  = (double **) malloc((1 + nrow) * sizeof(double *));
    if (p->C == NULL || p->c == NULL) {
        return NULL;
    }

    for (i = 1; i <= nrow; i++) {
        p->C[i] = (double *) calloc(ncol + 1, sizeof(double));
        p->c[i] = (double *) calloc(ncol + 1, sizeof(double));
        if (p->C[i] == NULL || p->c[i] == NULL) {
            return NULL;
        }
    }

    for (i = 1; i <= nrow; i++)
        for ( j = 1; j <= ncol; j++) {
            p->C[i][j] = t[i - 1][j - 1];
            p->c[i][j] = t[i - 1][j - 1];
        }

    for (i = 0; i < nrow; i++) {
        free(t[i]);
    }
    free(t);

    p->cost = 0;
    p->s = NULL;
    p->f = NULL;
    return p;
}

AP     *ap_create_problem_from_matrix(double **t, int n) {
    int i, j;
    AP *p;

    p = (AP*) malloc(sizeof(AP));
    if (p == NULL) {
        return NULL;
    }

    p->n = n;

    p->C  = (double **) malloc((n + 1) * sizeof(double *));
    p->c  = (double **) malloc((n + 1) * sizeof(double *));
    if (p->C == NULL || p->c == NULL) {
        return NULL;
    }

    for (i = 1; i <= n; i++) {
        p->C[i] = (double *) calloc(n + 1, sizeof(double));
        p->c[i] = (double *) calloc(n + 1, sizeof(double));
        if (p->C[i] == NULL || p->c[i] == NULL) {
            return NULL;
        }
    }


    for (i = 1; i <= n; i++)
        for ( j = 1; j <= n; j++) {
            p->C[i][j] = t[i - 1][j - 1];
            p->c[i][j] = t[i - 1][j - 1];
        }
    p->cost = 0;
    p->s = NULL;
    p->f = NULL;
    return p;
}

/* read data from vector */
AP *ap_create_problem(double *t, int n) {
    int i, j;
    AP *p;

    p = (AP*) malloc(sizeof(AP));
    if (p == NULL) {
        return NULL;
    }

    p->n = n;

    p->C  = (double **) malloc((n + 1) * sizeof(double *));
    p->c  = (double **) malloc((n + 1) * sizeof(double *));
    if (p->C == NULL || p->c == NULL) {
        return NULL;
    }

    for (i = 1; i <= n; i++) {
        p->C[i] = (double *) calloc(n + 1, sizeof(double));
        p->c[i] = (double *) calloc(n + 1, sizeof(double));
        if (p->C[i] == NULL || p->c[i] == NULL) {
            return NULL;
        }
    }


    for (i = 1; i <= n; i++)
        for ( j = 1; j <= n; j++) {
            p->C[i][j] = t[n * (j - 1) + i - 1];
            p->c[i][j] = t[n * (j - 1) + i - 1];
        }
    p->cost = 0;
    p->s = NULL;
    p->f = NULL;
    return p;
}

/* destructor */
void ap_free(AP *p) {
    int i;

    free(p->s);
    free(p->f);

    for (i = 1; i <= p->n; i++) {
        free(p->C[i]);
        free(p->c[i]);
    }

    free(p->C);
    free(p->c);
    free(p);
}

/* set + get functions */

/*
void ap_show_data(AP *p)
{
    int i, j;

    for(i = 1; i <= p->n; i++){
    for(j = 1; j <= p->n; j++)
        printf("%6.2f ", p->c[i][j]);
    printf("\n");
    }
}
*/

double ap_mincost(AP *p) {
    if (p->s == NULL) {
        ap_hungarian(p);
    }

    return p->cost;
}

int ap_size(AP *p) {
    return p->n;
}

int ap_time(AP *p) {
    return (int) p->rtime;
}

int ap_iterations(AP *p) {
    return p->runs;
}

/*
void ap_print_solution(AP *p)
{
    int i;

    printf("%d itertations, %d secs.\n",p->runs, (int)p->rtime);
    printf("Min Cost: %10.4f\n",p->cost);

    for(i = 0; i < p->n; i++)
    printf("%4d",p->s[i]);
    printf("\n");
}
*/

int ap_costmatrix(AP *p, double **m) {
    int i, j;

    for (i = 0; i < p->n; i++)
        for (j = 0; j < p->n; j++) {
            m[i][j] = p->C[i + 1][j + 1];
        }

    return p->n;
}

int ap_datamatrix(AP *p, double **m) {
    int i, j;

    for (i = 0; i < p->n; i++)
        for (j = 0; j < p->n; j++) {
            m[i][j] = p->c[i + 1][j + 1];
        }

    return p->n;
}

/* error reporting */

/*
void ap_error(char *message)
{
    fprintf(stderr,"%s\n",message);
    exit(1);
}
*/

/*************************************************************/
/* these functions are used internally                       */
/* by ap_hungarian                                           */
/*************************************************************/

int cover(AP *p, int *ri, int *ci) {
    int *mr, i, r;
    int n;

    n = p->n;
    mr = calloc(1 + p->n, sizeof(int));

    /* reset cover indices */
    for (i = 1; i <= n; i++) {
        if (p->s[i] == UNASSIGNED) {
            ri[i] = UNCOVERED;
            mr[i] = MARKED;
        } else {
            ri[i] = COVERED;
        }
        ci[i] = UNCOVERED;
    }

    while (TRUE) {
        /* find marked row */
        r = 0;
        for (i = 1; i <= n; i++)
            if (mr[i] == MARKED) {
                r = i;
                break;
            }

        if (r == 0) {
            break;
        }
        for (i = 1; i <= n; i++)
            if (p->c[r][i] == 0 && ci[i] == UNCOVERED) {
                if (p->f[i]) {
                    ri[p->f[i]] = UNCOVERED;
                    mr[p->f[i]] = MARKED;
                    ci[i] = COVERED;
                } else {
                    if (p->s[r] == UNASSIGNED) {
                        ++p->na;
                    }

                    p->f[p->s[r]] = 0;
                    p->f[i] = r;
                    p->s[r] = i;

                    free(mr);
                    return NOREDUCE;
                }
            }
        mr[r] = UNMARKED;
    }
    free(mr);
    return REDUCE;
}

void reduce(AP *p, int *ri, int *ci) {
    int i, j, n;
    double min;

    n = p->n;

    /* find minimum in uncovered c-matrix */
    min = DBL_MAX;
    for (i = 1; i <= n; i++)
        for (j = 1; j <= n; j++)
            if (ri[i] == UNCOVERED && ci[j] == UNCOVERED) {
                if (p->c[i][j] < min) {
                    min = p->c[i][j];
                }
            }

    /* subtract min from each uncovered element and add it to each element */
    /* which is covered twice                                              */
    for (i = 1; i <= n; i++)
        for (j = 1; j <= n; j++) {
            if (ri[i] == UNCOVERED && ci[j] == UNCOVERED) {
                p->c[i][j] -= min;
            }
            if (ri[i] == COVERED && ci[j] == COVERED) {
                p->c[i][j] += min;
            }
        }
}

void preassign(AP *p) {
    int i, j, min, r, c, n, count;
    int *ri, *ci, *rz, *cz;

    n = p->n;
    p->na = 0;

    /* row and column markers */
    ri = calloc(1 + n, sizeof(int));
    ci = calloc(1 + n, sizeof(int));

    /* row and column counts of zeroes */
    rz = calloc(1 + n, sizeof(int));
    cz = calloc(1 + n, sizeof(int));

    for (i = 1; i <= n; i++) {
        count = 0;
        for (j = 1; j <= n; j++)
            if (p->c[i][j] == 0) {
                ++count;
            }
        rz[i] = count;
    }

    for (i = 1; i <= n; i++) {
        count = 0;
        for (j = 1; j <= n; j++)
            if (p->c[j][i] == 0) {
                ++count;
            }
        cz[i] = count;
    }

    while (TRUE) {
        /* find unassigned row with least number of zeroes > 0 */
        min = INT_MAX;
        r = 0;
        for (i = 1; i <= n; i++)
            if (rz[i] > 0 && rz[i] < min && ri[i] == UNASSIGNED) {
                min = rz[i];
                r = i;
            }
        /* check if we are done */
        if (r == 0) {
            break;
        }

        /* find unassigned column in row r with least number of zeroes */
        c = 0;
        min = INT_MAX;
        for (i = 1; i <= n; i++)
            if (p->c[r][i] == 0 && cz[i] < min && ci[i] == UNASSIGNED) {
                min = cz[i];
                c = i;
            }

        if (c) {
            ++p->na;
            p->s[r] = c;
            p->f[c] = r;

            ri[r] = ASSIGNED;
            ci[c] = ASSIGNED;

            /* adjust zero counts */
            cz[c] = 0;
            for (i = 1; i <= n; i++)
                if (p->c[i][c] == 0) {
                    --rz[i];
                }
        }
    }

    /* free memory */
    free(ri);
    free(ci);
    free(rz);
    free(cz);
}

void preprocess(AP *p) {
    int i, j, n;
    double min;

    n = p->n;

    /* subtract column minima in each row */
    for (i = 1; i <= n; i++) {
        min = p->c[i][1];
        for (j = 2; j <= n; j++)
            if (p->c[i][j] < min) {
                min = p->c[i][j];
            }
        for (j = 1; j <= n; j++) {
            p->c[i][j] -= min;
        }
    }

    /* subtract row minima in each column */
    for (i = 1; i <= n; i++) {
        min = p->c[1][i];
        for (j = 2; j <= n; j++)
            if (p->c[j][i] < min) {
                min = p->c[j][i];
            }
        for (j = 1; j <= n; j++) {
            p->c[j][i] -= min;
        }
    }
}

int igraph_solve_lsap(igraph_matrix_t *c, igraph_integer_t n,
                      igraph_vector_int_t *p) {
    AP *ap;

    IGRAPH_CHECK(igraph_vector_int_resize(p, n));
    igraph_vector_int_null(p);

    ap = ap_create_problem(&MATRIX(*c, 0, 0), n);
    ap_hungarian(ap);
    ap_assignment(ap, VECTOR(*p));
    ap_free(ap);

    return 0;
}