//
// Initialize statistics.
//

void stat_init (PSTAT p)
{
    memset(p, 0x0, sizeof *p);
    p->count = 0;
    p->cells = 1024;
    p->data = (double *)malloc(p->cells * sizeof(double));
    p->offset = 0.0;
    p->scale = 1.0;
}

//
// Free statistics array.
//

void stat_free (PSTAT p)
{
    free(p->data);
    p->cells = 0;
    p->count = 0;
}

//
// Add new datum to statistics array.
//

void stat_data(PSTAT p, double v)
{
    if ((long)p->count == p->cells) {
        p->cells *= 2;
        p->data = (double *) realloc(p->data, p->cells * sizeof(double));
    }
    p->data[(long)p->count] = (v + p->offset) * p->scale;
    p->count += 1;
}

//
// Calculate Allan Deviation.
//

double stat_adev (double phase[], long count)
{
    long i;
    double sum, y0, y, dy;

    if (count < 3) {
        return 0.0;
    }

    sum = 0.0;
    for (i = 0; i < (count - 1); i += 1) {
        y = phase[i + 1] - phase[i];
        if (i > 0) {
            dy = y - y0;
            sum += dy * dy;
        }
        y0 = y;
    }

    return sqrt(sum / (2.0 * (count - 1)));
}

//
// Calculate basic statistics.
//

void stat_calc (PSTAT p)
{
    double data;
    long i, n;
    double sum;

    n = (long)p->count;

    // Get first, last, min, max, and mean value.

    sum = 0.0;
    for (i = 0; i < n; i += 1) {
        data = p->data[i];
        if (i == 0 || data < p->min) {
            p->min = data;
        }
        if (i == 0 || data > p->max) {
            p->max = data;
        }
        sum += p->data[i];
    }
    if (n > 0) {
        p->mean = sum / p->count;
        p->range = p->max - p->min;
    }

    p->xmean = (p->count - 1) / 2.0;

    // Compute slope and correlation coefficient.

    if (n > 1) {
        double dx, dy;
        double Sdx2, Sdy2;
        double Sdxdy;

        Sdx2 = Sdy2 = Sdxdy = 0.0;
        for (i = 0; i < n; i += 1) {
            dx = i - p->xmean;
            dy = p->data[i] - p->mean;
            Sdx2 += dx * dx;
            Sdy2 += dy * dy;
            Sdxdy += dx * dy;
        }

        p->sdev = sqrt(Sdy2 / (p->count - 1));

        if (Sdx2 == 0.0 || Sdy2 == 0.0) {
            p->slope = 0.0;
            p->r = 0.0;

        } else {
            p->slope = Sdxdy / Sdx2;
            p->r = Sdxdy / sqrt(Sdx2 * Sdy2);
        }

        // Standard deviation about regression

        p->steyx = sqrt((Sdy2 - p->slope * Sdxdy) / (p->count - 2));

        // Standard deviation of slope

        if (Sdx2 == 0.0) {
            p->slope_sdev = 0.0;

        } else {
            p->slope_sdev = p->steyx / sqrt(Sdx2);
        }

        //
        // S/N "slope to noise ratio" [tvb special]
        // - half height of fitted line divided by sdev of residuals
        // - append * p->r for good meansure?
        //

        if (p->steyx == 0.0) {
            p->sn = 0.0;

        } else {
            p->sn = (p->xmean * p->slope) / p->steyx;
        }
    }

    // Compute Allan deviation.

    p->adev = stat_adev(p->data, (long)p->count);

    // Compute first and last related values.

    if (n > 0) {
        p->first = p->data[0];
        p->last = p->data[n - 1];
        p->span = p->last - p->first;
        p->ramp = (p->last - p->first) / p->count;
    }
}

//
// Linear least squares fit, normalize, and create residuals.
//

void stat_line_fit (PSTAT p, PSTAT r)
{
    long i, n;

    n = (long)p->count;
    r->cells = n;
    r->data = (double *)malloc(r->cells * sizeof(double));

    for (i = 0; i < n; i += 1) {
        r->data[i] = p->data[i] - p->mean - p->slope * (i - p->xmean);
    }

    r->count = p->count;
    r->xmean = p->xmean;
    r->units = p->units;
}

//
// Quadratic least squares fit.
//

void stat_quad_fit (PSTAT p, PSTAT r)
{
    long i, n;
    double denom, dy, sum;
    double Sx0, Sx1, Sx2, Sx3, Sx4;
    double Syx0, Syx1, Syx2;
    double c[3];

#define X(i) (i - p->xmean)
#define Y(i) (p->data[i] - p->mean)

    Sx0 = Sx1 = Sx2 = Sx3 = Sx4 = 0.0;
    Syx0 = Syx1 = Syx2 = 0.0;

    n = (long)p->count;
    for (i = 0; i < n; i += 1) {

        Sx0 += 1.0;
        Sx1 += X(i);
        Sx2 += X(i) * X(i);
        Sx3 += X(i) * X(i) * X(i);
        Sx4 += X(i) * X(i) * X(i) * X(i);

        Syx0 += Y(i);
        Syx1 += Y(i) * X(i);
        Syx2 += Y(i) * X(i) * X(i);
    }

    // WARNING: subject to massive loss of precision or overflow

    denom = Sx0 * (Sx2 * Sx4 - Sx3 * Sx3)
          - Sx1 * (Sx1 * Sx4 - Sx2 * Sx3)
          + Sx2 * (Sx1 * Sx3 - Sx2 * Sx2);

    c[0] = ( Syx0 * (Sx2 * Sx4 - Sx3 * Sx3)
           + Syx1 * (Sx2 * Sx3 - Sx1 * Sx4)
           + Syx2 * (Sx1 * Sx3 - Sx2 * Sx2) ) / denom;

    c[1] = ( Syx0 * (Sx2 * Sx3 - Sx1 * Sx4)
           + Syx1 * (Sx0 * Sx4 - Sx2 * Sx2)
           + Syx2 * (Sx1 * Sx2 - Sx0 * Sx3) ) / denom;

    c[2] = ( Syx0 * (Sx1 * Sx3 - Sx2 * Sx2)
           + Syx1 * (Sx2 * Sx1 - Sx0 * Sx3)
           + Syx2 * (Sx0 * Sx2 - Sx1 * Sx1) ) / denom;

    // Calculate residuals

    r->cells = n;
    r->data = (double *)malloc(r->cells * sizeof(double));

    r->count = p->count;
    r->xmean = p->xmean;
    r->units = p->units;

    sum = 0.0;
    for (i = 0; i < n; i += 1) {
        dy = Y(i) - (c[0] + c[1] * X(i) + c[2] * X(i) * X(i));
        r->data[i] = p->data[i] + dy;
        sum += dy * dy;
    }

    p->offset2 = c[0];
    p->slope2 = c[1];
    p->drift2 = c[2];
    p->sdev2 = sqrt(sum / (p->count - 3));
}

//
// Display all calculated statistics.
//

void stat_dump (PSTAT p)
{
    FILE *f = stdout;

    fprintf(f, "%8s %24.16le\n", "count", p->count);
    if (p->count < 1) {
        return;
    }
    fprintf(f, "%8s %24.16le %s\n", "mean", p->mean, p->units);
    fprintf(f, "%8s %24.16le %s\n", "min", p->min, p->units);
    fprintf(f, "%8s %24.16le %s\n", "max", p->max, p->units);
    fprintf(f, "%8s %24.16le %s\n", "range", p->range, p->units);
    fprintf(f, "%8s %24.16le %s\n", "(first)", p->first, p->units);
    fprintf(f, "%8s %24.16le %s\n",  "(last)", p->last, p->units);
    fprintf(f, "%8s %24.16le %s\n",  "(span)", p->span, p->units);
    fprintf(f, "%8s %24.16le %s/s\n", "(ramp)", p->ramp, p->units);
    if (p->count < 2) {
        return;
    }
    fprintf(f, "%8s %24.16le %s\n", "sdev", p->sdev, p->units);
    if (p->count < 3) {
        return;
    }
    fprintf(f, "%8s %24.16le %s\n", "adev", p->adev, p->units);
    fprintf(f, "%8s %24.16le %s/s\n", "slope", p->slope, p->units);
    fprintf(f, "%8s %24.16le\n", "r", p->r);
    fprintf(f, "%8s %24.16le\n", "r^2", p->r * p->r);
    fprintf(f, "%8s %24.16le %s\n", "steyx", p->steyx, p->units);
    fprintf(f, "%8s %24.16le %s\n", "slope_sdev", p->slope_sdev, p->units);
    fprintf(f, "%8s %24.16le\n", "sn", p->sn);

    fprintf(f, "%8s %24.16le\n", "offset2", p->offset2);
    fprintf(f, "%8s %24.16le\n", "slope2", p->slope2);
    fprintf(f, "%8s %24.16le\n", "drift2", p->drift2);
    fprintf(f, "%8s %24.16le\n", "sdev2", p->sdev2);

    fprintf(f, "\n");
}