/*
* criterion_sift.c
*
* Copyright (C) 2019, Tristan Dagobert, CMLA, École Normale Supérieure Paris-Saclay.
*
* This software is a computer program.
* It is a part of a code which estimates, given a satellite time series
* the visibility of each image. Given a gray level time series, it provides a
* Boolean times series indicating the visibility (mainly the opaque clouds,
* haze and shadow).
*
*
* This software is governed by the CeCILL-C license under French law and
* abiding by the rules of distribution of free software.  You can  use,
* modify and/ or redistribute the software under the terms of the CeCILL-C
* license as circulated by CEA, CNRS and INRIA at the following URL
* "http://www.cecill.info".
*
* As a counterpart to the access to the source code and  rights to copy,
* modify and redistribute granted by the license, users are provided only
* with a limited warranty  and the software's author,  the holder of the
* economic rights,  and the successive licensors  have only  limited
* liability.
*
* In this respect, the user's attention is drawn to the risks associated
* with loading,  using,  modifying and/or developing or reproducing the
* software by the user in light of its specific status of free software,
* that may mean  that it is complicated to manipulate,  and  that  also
* therefore means  that it is reserved for developers  and  experienced
* professionals having in-depth computer knowledge. Users are therefore
* encouraged to load and test the software's suitability as regards their
* requirements in conditions enabling the security of their systems and/or
* data to be ensured and,  more generally, to use and operate it in the
* same conditions as regards security.
*
* The fact that you are presently reading this means that you have had
* knowledge of the CeCILL-C license and that you accept its terms.
*
*/

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <limits.h>
#include <float.h>
#include <assert.h>

#include "iio.h"
#include "lib_sift.h"
#include "utilities.h"
#include "criterion_sift.h"
#include "bilinear_interpolation.h"

/*****************************************************************************/
#define SAMPLE_FACTOR 4
#define SIFT_NB_CLASSES 128

#define MAXI(a,b) ({ __typeof__ (a) _a = (a); __typeof__ (b) _b = (b); \
     _a > _b ? _a : _b; })

int * HISTOGRAMME_DIST;
float * INDEX_REF;
/*===========================================================================*/
void
get_sift_image_size(int srow[1], int scol[1], int nrow, int ncol)
{
    scol[0] = (ncol % SAMPLE_FACTOR == 0) ?
              ncol / SAMPLE_FACTOR + 1 : ncol / SAMPLE_FACTOR + 2;

    srow[0] = (nrow % SAMPLE_FACTOR == 0) ?
              nrow / SAMPLE_FACTOR + 1 : nrow / SAMPLE_FACTOR + 2;

    return;
}
/*===========================================================================*/
static float *
extract_partition (int nl, int nc, float *im, int ncol, int dl, int dc)
{
    int l, c;
    float *part = malloc (nl * nc * sizeof (float));

    for (l = 0; l < nl; l++)
    {
        for (c = 0; c < nc; c++)
        {
            part[l * nc + c] = im[(l + dl) * ncol + (c + dc)];
        }
    }
    return part;
}
/*****************************************************************************/
static void
merge_partition (float *im, int ncol, float *part, int nl, int nc,
                 int dl, int dc)
{
    int l, c;
    for (l = 0; l < nl; l++)
    {
        for (c = 0; c < nc; c++)
        {
            im[(l + dl) * ncol + (c + dc)] = part[l * nc + c];
        }
    }

    return;
}
/*===========================================================================*/
static float *
zoom_in_sift (float *im, int ncol, int nrow, int factor, int zoom_ncol,
              int zoom_nrow)
{
    /*     x0−…−x1−…−x2−−x3
     *     |         |   |
     *     …         …   …
     *     |   a     | b |
     *     x4−…−x5−…−x6−−x7
     *     |   c     | d |
     *     x8−…−x9−…−xA−−xB
     *
     * The image to zoom was sampled at the x positions regularly, except at the
     * extrema if the sampling factor kappa is not a divider of the image size.
     * There exists 4 possible partitions of the image to resample: {a}, {b},
     * {c} and {d}. One has to adequately interpolate when resampling.
     */

    int nl, nc;
    int dl, dc;            /* biases */

    int a_ncol, a_nrow;
    int A_ncol, A_nrow;
    int B_ncol, B_nrow;
    int C_ncol, C_nrow;
    int D_ncol, D_nrow;

    a_ncol =
        (zoom_ncol % factor == 0) ? zoom_ncol / factor : zoom_ncol / factor + 1;
    a_nrow =
        (zoom_nrow % factor == 0) ? zoom_nrow / factor : zoom_nrow / factor + 1;

    /* aggregation of partitions {A, B, C, D} */
    float *neo;
    neo = calloc (zoom_nrow * zoom_ncol, sizeof (float));

    /* processing partition a */
    float *part_a = NULL, *part_A = NULL;
    nl = a_nrow;
    nc = a_ncol;
    /* copy of {x0, …, x1, …, x2, …, …, …, x4, …, x5, …, x6} */
    dl = 0;
    dc = 0;
    part_a = extract_partition (nl, nc, im, ncol, dl, dc);
    /* zoom of {a} to {A} */

    A_ncol = factor * (a_ncol - 1) + 1;
    A_nrow = factor * (a_nrow - 1) + 1;
    part_A = (float *) malloc (A_nrow * A_ncol * sizeof (float));
    bilinear_interpolant_vec (part_A, A_ncol, A_nrow, part_a, nc, nl, 1);

    /* copy of {A} */
    dl = 0;
    dc = 0;
    merge_partition (neo, zoom_ncol, part_A, A_nrow, A_ncol, dl, dc);

    /* processing partition b */
    float *part_b = NULL, *part_B = NULL;
    nl = a_nrow;
    nc = ncol - a_ncol + 1;
    if (nc > 1)
    {
        /* copy of {x2, x3, …, …, x6, x7} */
        dl = 0;
        dc = a_ncol - 1;
        part_b = extract_partition (nl, nc, im, ncol, dl, dc);
        /* zoom of {b} to {B} */

        B_nrow = A_nrow;
        B_ncol = zoom_ncol - A_ncol + 1;
        part_B = (float *) malloc (B_nrow * B_ncol * sizeof (float));
        bilinear_interpolant_vec (part_B, B_ncol, B_nrow, part_b, nc, nl, 1);

        /* copy of {B} */
        dl = 0;
        dc = A_ncol - 1;
        merge_partition (neo, zoom_ncol, part_B, B_nrow, B_ncol, dl, dc);

    }

    /* processing partition c */
    float *part_c = NULL, *part_C = NULL;
    nl = nrow - a_nrow + 1;
    nc = a_ncol;
    if (nl > 1)
    {
        /* copy of {x4, …, x5, …, x6, …, …, …, x8, …, x9, …, xA} */
        dl = a_nrow - 1;
        dc = 0;
        part_c = extract_partition (nl, nc, im, ncol, dl, dc);
        /* zoom of {c} to {C} */
        C_nrow = zoom_nrow - A_nrow + 1;
        C_ncol = A_ncol;
        part_C = (float *) malloc (C_nrow * C_ncol * sizeof (float));
        bilinear_interpolant_vec (part_C, C_ncol, C_nrow, part_c, nc, nl, 1);

        /* copy of {C} */
        dl = A_nrow - 1;
        dc = 0;
        merge_partition (neo, zoom_ncol, part_C, C_nrow, C_ncol, dl, dc);

    }

    /* processing partition d */
    float *part_d = NULL, *part_D = NULL;
    nl = nrow - a_nrow + 1;
    nc = ncol - a_ncol + 1;
    if (nl > 1 && nc > 1)
    {
        /* copy of {x6, x7, xA, xB} */
        dl = a_nrow - 1;
        dc = a_ncol - 1;
        part_d = extract_partition (nl, nc, im, ncol, dl, dc);
        /* zoom of {d} to {D} */
        D_nrow = zoom_nrow - A_nrow + 1;
        D_ncol = zoom_ncol - A_ncol + 1;
        part_D = (float *) malloc (D_nrow * D_ncol * sizeof (float));
        bilinear_interpolant_vec (part_D, D_ncol, D_nrow, part_d, nc, nl, 1);

        /* copy of {D} */
        dl = A_nrow - 1;
        dc = A_ncol - 1;
        merge_partition (neo, zoom_ncol, part_D, D_nrow, D_ncol, dl, dc);

    }

    free (part_a);
    free (part_b);
    free (part_c);
    free (part_d);
    free (part_A);
    free (part_B);
    free (part_C);
    free (part_D);
    return neo;
}
/*****************************************************************************/
static sift_point_t *
create_sift_descriptors (float *im, int nrow, int ncol, int *nb_pts_key,
                         int *nb_pts_row, int *nb_pts_col)
{
    int i, j, l, c;
    long int p;

    /* determine the number of SIFT descriptors to compute according the
     * sampling factor and the image size */
    *nb_pts_col =
        (ncol % SAMPLE_FACTOR ==
         0) ? ncol / SAMPLE_FACTOR + 1 : ncol / SAMPLE_FACTOR + 2;
    *nb_pts_row =
        (nrow % SAMPLE_FACTOR ==
         0) ? nrow / SAMPLE_FACTOR + 1 : nrow / SAMPLE_FACTOR + 2;
    *nb_pts_key = *nb_pts_col * *nb_pts_row;

    sift_point_t *pts_sift;

    pts_sift = (sift_point_t *) malloc (*nb_pts_key * sizeof (sift_point_t));

    /* The SIFT key points are computed in a regular way at positions "x"
     * according the sampling factor kappa, except close to the right and
     * bottom boundaries :
     *                     x−−−x−−−x−x
     *                     |       | |
     *                     |   a   |b|
     *                     x−−−x−−−x−x
     *                     |   c   |d|
     *                     x−−−x−−−x−x
     */

    for (p = 0, l = 0, i = 0; i < *nb_pts_row;
            i++, l = (l + SAMPLE_FACTOR < nrow - 1) ? l + SAMPLE_FACTOR : nrow - 1)
    {
        for (c = 0, j = 0; j < *nb_pts_col;
                j++, p++, c =
                    (c + SAMPLE_FACTOR < ncol - 1) ? c + SAMPLE_FACTOR : ncol - 1)
        {
            pts_sift[p].x = l;
            pts_sift[p].y = c;
            pts_sift[p].scale = 1;
            pts_sift[p].orientation = 0;
        }
    }

    /* compute the SIFT descriptors of the image */
    sift_fill_descriptors (im, ncol, nrow, pts_sift, *nb_pts_key);

    return pts_sift;
}

/*****************************************************************************/
static inline float
distance_correlation_l2(unsigned char * sift1, unsigned char * sift2)
{
    /* The correlation L2, or cosine distance is expressed by :
     *
     *           ρ = < X-μx, Y-μy > / (|X-μx|₂ . |Y-μy|₂)
     *
     * It is defined natively on [-1, +1]; we update it to produce an output
     * defined on [0, +1].
     */
    float res, norm_x, norm_y;
    float x[SIFT_NB_CLASSES], y[SIFT_NB_CLASSES];
    int c;
    float mean_x, mean_y;

    /* conversion */
    for (mean_x = 0, mean_y = 0, c = 0; c < SIFT_NB_CLASSES; c++)
    {
        x[c] = sift1[c];
        y[c] = sift2[c];

        mean_x += x[c];
        mean_y += y[c];
    }
    mean_x /= SIFT_NB_CLASSES;
    mean_y /= SIFT_NB_CLASSES;

    /* normalisation */
    for (c = 0; c < SIFT_NB_CLASSES; c++)
    {
        x[c] -= mean_x;
        y[c] -= mean_y;
    }

    /* norm L2 */
    for (norm_x = 0, norm_y = 0, c = 0; c < SIFT_NB_CLASSES; c++)
    {
        norm_x += x[c] * x[c];
        norm_y += y[c] * y[c];
    }

    /* correlation L2 */
    for (res = 0.f, c = 0; c < SIFT_NB_CLASSES; c++)
    {
        res  += x[c] * y[c];
    }
    res = res / sqrt(norm_x * norm_y);

    /* [-1, +1] −> [0, +1] */
    res = (res + 1)/2.0;

    if (isnan(res)) res = 0.0;

    return res;
}
/*===========================================================================*/
static void
create_correlations_maps(float ** corrs, sift_point_t ** ims_pts_sift,
                         int nb_ims, int nrow, int ncol)
{
    /* For each pixel (x,n) at time n, we search its lowest SIFT distance between
     * it and (x, m) picked among the series (u_m(x))_{1≤m≤N} */
    int m, n, p, j, k;

    int srow, scol, spix;
    get_sift_image_size(&srow, &scol, nrow, ncol);
    spix = srow * scol;

    float ** corrs_small;
    corrs_small = malloc(nb_ims * sizeof(float *));
    for (n=0; n<nb_ims; n++) corrs_small[n] = calloc(spix, sizeof(float));

    unsigned char *histo_n, *histo_m;
    float val, mini;

    float ** dist_matrix = malloc(nb_ims * sizeof(float *));
    for (n=0; n<nb_ims; n++) dist_matrix[n] = calloc(nb_ims, sizeof(float));

    /* we compute the correlations between the images */
    for (p = 0, k = 0; k < srow; k++)
    {
        for (j = 0; j < scol; j++, p++) /* pixels */
        {
            /* we compute the distance matrix */
            for (n=0; n<nb_ims; n++)
            {
                /* we compute the distances for all pairs n≠m */
                for (m=n; m<nb_ims; m++)
                {
                    /* we do not compute autocorrelation */
                    if (m == n)
                    {
                        dist_matrix[n][m] = FLT_MAX;
                        dist_matrix[m][n] = FLT_MAX;
                        continue;
                    }

                    /* case m ≠ n */
                    histo_n = ims_pts_sift[n][p].descriptor;
                    histo_m = ims_pts_sift[m][p].descriptor;

                    /* compute the distance */
                    val = 1 - distance_correlation_l2(histo_n, histo_m);
                    dist_matrix[n][m] = val;
                    dist_matrix[m][n] = val;
                }

            }

            /* compute the series (Cn(p))_{1 ≤ n ≤N} */
            for (n=0; n<nb_ims; n++)
            {
                /* we search for the minimal distances between n≠m */
                for (m=1, mini=dist_matrix[n][0]; m<nb_ims; m++)
                {
                    mini = dist_matrix[n][m] < mini ? dist_matrix[n][m] : mini;
                }
                corrs_small[n][p] = mini;
            }
        }
    }

    /* we zoom the correlation images by a bilinear filter */
    float * neo_corr;
    for (n=0; n<nb_ims; n++)
    {
        /* interpolation of the correlation images */
        neo_corr = zoom_in_sift (corrs_small[n], scol, srow, SAMPLE_FACTOR, ncol, nrow);

        /* copy into adhoc structures */
        memcpy(corrs[n], neo_corr, ncol * nrow * sizeof(float));
        free(neo_corr);
    }

    /* free the memory */
    free_series_f(corrs_small, nb_ims, NULL);
    free_series_f(dist_matrix, nb_ims, NULL);
    return;
}
float **
compute_distance_sift_series(float **images, int nb_ims, int nrow, int ncol)
{
    /* compute the SIFT descriptors of a temporal series */

    float **corr_images = NULL;
    int n;

    int npix, spix, srow, scol;
    sift_point_t ** ims_pts_sift = NULL;

    if (images == NULL)
        return NULL;

    npix = nrow * ncol;

    printf ("[INFO] Creation of the SIFT descriptors of the %d images...\n", nb_ims);
    ims_pts_sift = malloc (nb_ims * sizeof (sift_point_t *));
    for (n = 0; n < nb_ims; n++)
    {
        ims_pts_sift[n] =
            create_sift_descriptors (images[n], nrow, ncol, &spix,
                                     &srow, &scol);
        fprintf(stdout, "...");
        fflush(stdout);
    }
    printf("\n");
    printf ("[INFO] creation of the SIFT descriptors done.\n");

    /* allocation of the correlation series */
    corr_images = malloc(nb_ims * sizeof(float *));
    for (n = 0; n < nb_ims; n++) corr_images[n] = malloc(npix * sizeof(float));

    /* compute the correlation series */
    create_correlations_maps(corr_images, ims_pts_sift, nb_ims, nrow, ncol);

    /* cleaning */
    for (n = 0; n < nb_ims; n++) free (ims_pts_sift[n]);
    free (ims_pts_sift);

    return corr_images;
}

/*===========================================================================*/