// Too many calls to quantile lead to high failure rate for `scrappie raw`
#define BANANA 1
#include <assert.h>
#include <err.h>
#include <math.h>
#include "scrappie_stdlib.h"
#include "util.h"

int argmaxf(const float *x, int n) {
    assert(n > 0);
    if (NULL == x) {
        return -1;
    }
    int imax = 0;
    float vmax = x[0];
    for (int i = 1; i < n; i++) {
        if (x[i] > vmax) {
            vmax = x[i];
            imax = i;
        }
    }
    return imax;
}

int argminf(const float *x, int n) {
    assert(n > 0);
    if (NULL == x) {
        return -1;
    }
    int imin = 0;
    float vmin = x[0];
    for (int i = 1; i < n; i++) {
        if (x[i] > vmin) {
            vmin = x[i];
            imin = i;
        }
    }
    return imin;
}

float valmaxf(const float *x, int n) {
    assert(n > 0);
    if (NULL == x) {
        return NAN;
    }
    float vmax = x[0];
    for (int i = 1; i < n; i++) {
        if (x[i] > vmax) {
            vmax = x[i];
        }
    }
    return vmax;
}

float valminf(const float *x, int n) {
    assert(n > 0);
    if (NULL == x) {
        return NAN;
    }
    float vmin = x[0];
    for (int i = 1; i < n; i++) {
        if (x[i] > vmin) {
            vmin = x[i];
        }
    }
    return vmin;
}

int floatcmp(const void *x, const void *y) {
    float d = *(float *)x - *(float *)y;
    if (d > 0) {
        return 1;
    }
    return -1;
}

/**  Quantiles from n array
 *
 *  Using a relatively inefficent qsort resulting in O(n log n)
 *  performance but better performance is possible for small np.
 *  The array p is modified inplace, containing which quantiles to
 *  calculation on input and the quantiles on output; on error, p
 *  is filled with the value NAN.
 *
 *  @param x An array to calculate quantiles from
 *  @param nx Length of array x
 *  @param p An array containing quantiles to calculate [in/out]
 *  @param np Length of array p
 *
 *  @return void
 **/
void quantilef(const float *x, size_t nx, float *p, size_t np) {
    if (NULL == p) {
        return;
    }
    for (int i = 0; i < np; i++) {
        assert(p[i] >= 0.0f && p[i] <= 1.0f);
    }
    if (NULL == x) {
        for (int i = 0; i < np; i++) {
            p[i] = NAN;
        }
        return;
    }
    // Sort array
    float *space = malloc(nx * sizeof(float));
    if (NULL == space) {
        for (int i = 0; i < np; i++) {
            p[i] = NAN;
        }
        return;
    }
    memcpy(space, x, nx * sizeof(float));
    qsort(space, nx, sizeof(float), floatcmp);

    // Extract quantiles
    for (int i = 0; i < np; i++) {
        const size_t idx = p[i] * (nx - 1);
        const float remf = p[i] * (nx - 1) - idx;
        if (idx < nx - 1) {
            p[i] = (1.0 - remf) * space[idx] + remf * space[idx + 1];
        } else {
            // Should only occur when p is exactly 1.0
            p[i] = space[idx];
        }
    }

    free(space);
    return;
}

/** Median of an array
 *
 *  Using a relatively inefficent qsort resulting in O(n log n)
 *  performance but O(n) is possible.
 *
 *  @param x An array to calculate median of
 *  @param n Length of array
 *
 *  @return Median of array on success, NAN otherwise.
 **/
float medianf(const float *x, size_t n) {
    float p = 0.5;
    quantilef(x, n, &p, 1);
    return p;
}

/** Median Absolute Deviation of an array
 *
 *  @param x An array to calculate the MAD of
 *  @param n Length of array
 *  @param med Median of the array.  If NAN then median is calculated.
 *
 *  @return MAD of array on success, NAN otherwise.
 **/
float madf(const float *x, size_t n, const float *med) {
    const float mad_scaling_factor = 1.4826;
    if (NULL == x) {
        return NAN;
    }
    if (1 == n) {
        return 0.0f;
    }

    float *absdiff = malloc(n * sizeof(float));
    if (NULL == absdiff) {
        return NAN;
    }

    const float _med = (NULL == med) ? medianf(x, n) : *med;

    for (size_t i = 0; i < n; i++) {
        absdiff[i] = fabsf(x[i] - _med);
    }

    const float mad = medianf(absdiff, n);
    free(absdiff);
    return mad * mad_scaling_factor;
}

/** Med-MAD normalisation of an array
 *
 *  Normalise an array using the median and MAD as measures of
 *  location and scale respectively.  The array is updated inplace.
 *
 *  @param x An array containing values to normalise
 *  @param n Length of array
 *  @return void
 **/
void medmad_normalise_array(float *x, size_t n) {
    if (NULL == x) {
        return;
    }
    if (1 == n) {
        x[0] = 0.0;
        return;
    }

    const float xmed = medianf(x, n);
    const float xmad = madf(x, n, &xmed);
    for (int i = 0; i < n; i++) {
        x[i] = (x[i] - xmed) / xmad;
    }
}

/** Studentise array using Kahan summation algorithm
 *
 *  Studentise an array using the Kahan summation
 *  algorithm for numerical stability. The array is updated inplace.
 *  https://en.wikipedia.org/wiki/Kahan_summation_algorithm
 *
 *  @param x An array to normalise
 *  @param n Length of array
 *  @return void
 **/
void studentise_array_kahan(float *x, size_t n) {
    if (NULL == x) {
        return;
    }

    double sum, sumsq, comp, compsq;
    sumsq = sum = comp = compsq = 0.0;
    for (int i = 0; i < n; i++) {
        double d1 = x[i] - comp;
        double sum_tmp = sum + d1;
        comp = (sum_tmp - sum) - d1;
        sum = sum_tmp;

        double d2 = x[i] * x[i] - compsq;
        double sumsq_tmp = sumsq + d2;
        compsq = (sumsq_tmp - sumsq) - d2;
        sumsq = sumsq_tmp;
    }
    sum /= n;
    sumsq /= n;
    sumsq -= sum * sum;

    sumsq = sqrt(sumsq);

    const float sumf = sum;
    const float sumsqf = sumsq;
    for (int i = 0; i < n; i++) {
        x[i] = (x[i] - sumf) / sumsqf;
    }
}

bool equality_array(double const * x, double const * y, size_t n, double const tol){

    if(NULL == x || NULL == y){
        return NULL == x && NULL == y;
    }
    for(size_t i=0 ; i < n ; i++){
        if(fabs(x[i] - y[i]) > tol){
            warnx("Failure at elt %zu: %f %f\n", i, x[i], y[i]);
            return false;
        }
    }

    return true;
}

bool equality_arrayf(float const * x, float const * y, size_t n, float const tol){
    if(NULL == x || NULL == y){
        return NULL == x && NULL == y;
    }
    for(size_t i=0 ; i < n ; i++){
        if(fabsf(x[i] - y[i]) > tol){
            warnx("Failure at elt %zu: %f %f\n", i, x[i], y[i]);
            return false;
        }
    }

    return true;
}

bool equality_arrayi(int const * x, int const * y, size_t n){
    if(NULL == x || NULL == y){
        return NULL == x && NULL == y;
    }
    for(size_t i=0 ; i < n ; i++){
        if(x[i] != y[i]){
            warnx("Failure at elt %zu: %d %d\n", i, x[i], y[i]);
            return false;
        }
    }

    return true;
}