/*
 *  SCGlib : A C library for the spectral coarse graining of matrices
 *  as described in the paper: Shrinking Matrices while preserving their
 *  eigenpairs with Application to the Spectral Coarse Graining of Graphs.
 *  Preprint available at <http://people.epfl.ch/david.morton>
 *
 *  Copyright (C) 2008 David Morton de Lachapelle <david.morton@a3.epfl.ch>
 *
 *  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 Street, Fifth Floor, Boston, MA
 *  02110-1301 USA
 *
 *  DESCRIPTION
 *  -----------
 *    This file implements algorithm 5.8 of the above reference.
 *    The optimal_partition function returns the minimizing partition
 *    with size 'nt' of the objective function ||v-Pv||, where P is
 *    a problem-specific projector. So far, Symmetric (matrix=1),
 *    Laplacian (matrix=2) and Stochastic (matrix=3) projectors
 *    have been implemented (the cost_matrix function below).
 *    In the stochastic case, 'p' is expected to be a valid propability
 *    vector. In all other cases, 'p' is ignored and can be set to NULL.
 *    The group labels are given in 'gr' as positive consecutive integers
 *    starting from 0.
 */

#include "igraph_error.h"
#include "igraph_memory.h"
#include "igraph_matrix.h"
#include "igraph_vector.h"

#include "scg_headers.h"

int igraph_i_optimal_partition(const igraph_real_t *v, int *gr, int n,
                               int nt, int matrix, const igraph_real_t *p,
                               igraph_real_t *value) {

    int i, non_ties, q, j, l, part_ind, col;
    igraph_i_scg_indval_t *vs = igraph_Calloc(n, igraph_i_scg_indval_t);
    igraph_real_t *Cv, temp, sumOfSquares;
    igraph_vector_t ps;
    igraph_matrix_t F;
    igraph_matrix_int_t Q;

    /*-----------------------------------------------
      -----Sorts v and counts non-ties-----------------
      -----------------------------------------------*/

    if (!vs) {
        IGRAPH_ERROR("SCG error", IGRAPH_ENOMEM);
    }
    IGRAPH_FINALLY(igraph_free, vs);

    for (i = 0; i < n; i++) {
        vs[i].val = v[i];
        vs[i].ind = i;
    }

    qsort(vs, (size_t) n, sizeof(igraph_i_scg_indval_t),
          igraph_i_compare_ind_val);

    non_ties = 1;
    for (i = 1; i < n; i++) {
        if (vs[i].val < vs[i - 1].val - 1e-14 ||
            vs[i].val > vs[i - 1].val + 1e-14) {
            non_ties++;
        }
    }

    if (nt >= non_ties) {
        IGRAPH_ERROR("`Invalid number of intervals, should be smaller than "
                     "number of unique values in V", IGRAPH_EINVAL);
    }

    /*------------------------------------------------
      ------Computes Cv, the matrix of costs------------
      ------------------------------------------------*/
    Cv = igraph_i_real_sym_matrix(n);
    if (!Cv) {
        IGRAPH_ERROR("SCG error", IGRAPH_ENOMEM);
    }
    IGRAPH_FINALLY(igraph_free, Cv);

    /* if stochastic SCG orders p */
    if (matrix == 3) {
        IGRAPH_VECTOR_INIT_FINALLY(&ps, n);
        for (i = 0; i < n; i++) {
            VECTOR(ps)[i] = p[vs[i].ind];
        }
    }

    IGRAPH_CHECK(igraph_i_cost_matrix(Cv, vs, n, matrix, &ps));
    if (matrix == 3) {
        igraph_vector_destroy(&ps);
        IGRAPH_FINALLY_CLEAN(1);
    }
    /*-------------------------------------------------
      -------Fills up matrices F and Q-------------------
      -------------------------------------------------*/
    /*here j also is a counter but the use of unsigned variables
      is to be proscribed in "for (unsigned int j=...;j>=0;j--)",
      for such loops never ends!*/

    IGRAPH_MATRIX_INIT_FINALLY(&F, nt, n);
    IGRAPH_CHECK(igraph_matrix_int_init(&Q, nt, n));
    IGRAPH_FINALLY(igraph_matrix_destroy, &Q);

    for (i = 0; i < n; i++) {
        MATRIX(Q, 0, i)++;
    }
    for (i = 0; i < nt; i++) {
        MATRIX(Q, i, i) = i + 1;
    }

    for (i = 0; i < n; i++) {
        MATRIX(F, 0, i) = igraph_i_real_sym_mat_get(Cv, 0, i);
    }

    for (i = 1; i < nt; i++)
        for (j = i + 1; j < n; j++) {
            MATRIX(F, i, j) = MATRIX(F, i - 1, i - 1) + igraph_i_real_sym_mat_get(Cv, i, j);
            MATRIX(Q, i, j) = 2;

            for (q = i - 1; q <= j - 1; q++) {
                temp = MATRIX(F, i - 1, q) + igraph_i_real_sym_mat_get(Cv, q + 1, j);
                if (temp < MATRIX(F, i, j)) {
                    MATRIX(F, i, j) = temp;
                    MATRIX(Q, i, j) = q + 2;
                }
            }
        }
    igraph_i_free_real_sym_matrix(Cv);
    IGRAPH_FINALLY_CLEAN(1);

    /*--------------------------------------------------
      -------Back-tracks through Q to work out the groups-
      --------------------------------------------------*/
    part_ind = nt;
    col = n - 1;

    for (j = nt - 1; j >= 0; j--) {
        for (i = MATRIX(Q, j, col) - 1; i <= col; i++) {
            gr[vs[i].ind] = part_ind - 1;
        }
        if (MATRIX(Q, j, col) != 2) {
            col = MATRIX(Q, j, col) - 2;
            part_ind -= 1;
        } else {
            if (j > 1) {
                for (l = 0; l <= (j - 1); l++) {
                    gr[vs[l].ind] = l;
                }
                break;
            } else {
                col = MATRIX(Q, j, col) - 2;
                part_ind -= 1;
            }
        }
    }

    sumOfSquares = MATRIX(F, nt - 1, n - 1);

    igraph_matrix_destroy(&F);
    igraph_matrix_int_destroy(&Q);
    igraph_Free(vs);
    IGRAPH_FINALLY_CLEAN(3);

    if (value) {
        *value = sumOfSquares;
    }
    return 0;
}

int igraph_i_cost_matrix(igraph_real_t*Cv, const igraph_i_scg_indval_t *vs,
                         int n,  int matrix, const igraph_vector_t *ps) {

    /* if symmetric of Laplacian SCG -> same Cv */
    if (matrix == 1 || matrix == 2) {
        int i, j;
        igraph_vector_t w, w2;

        IGRAPH_VECTOR_INIT_FINALLY(&w, n + 1);
        IGRAPH_VECTOR_INIT_FINALLY(&w2, n + 1);

        VECTOR(w)[1] = vs[0].val;
        VECTOR(w2)[1] = vs[0].val * vs[0].val;

        for (i = 2; i <= n; i++) {
            VECTOR(w)[i] = VECTOR(w)[i - 1] + vs[i - 1].val;
            VECTOR(w2)[i] = VECTOR(w2)[i - 1] + vs[i - 1].val * vs[i - 1].val;
        }

        for (i = 0; i < n; i++) {
            for (j = i + 1; j < n; j++) {
                igraph_real_t v = (VECTOR(w2)[j + 1] - VECTOR(w2)[i]) -
                                  (VECTOR(w)[j + 1] - VECTOR(w)[i]) * (VECTOR(w)[j + 1] - VECTOR(w)[i]) /
                                  (j - i + 1);
                igraph_i_real_sym_mat_set(Cv, i, j, v);
            }
        }

        igraph_vector_destroy(&w);
        igraph_vector_destroy(&w2);
        IGRAPH_FINALLY_CLEAN(2);
    }
    /* if stochastic */
    /* TODO: optimize it to O(n^2) instead of O(n^3) (as above) */
    if (matrix == 3) {
        int i, j, k;
        igraph_real_t t1, t2;
        for (i = 0; i < n; i++) {
            for (j = i + 1; j < n; j++) {
                t1 = t2 = 0;
                for (k = i; k < j; k++) {
                    t1 += VECTOR(*ps)[k];
                    t2 += VECTOR(*ps)[k] * vs[k].val;
                }
                t1 = t2 / t1;
                t2 = 0;
                for (k = i; k < j; k++) {
                    t2 += (vs[k].val - t1) * (vs[k].val - t1);
                }
                igraph_i_real_sym_mat_set(Cv, i, j, t2);
            }
        }
    }

    return 0;
}