linear loess in C++

#include <iostream>
#include <iterator>
#include <vector>
#include <cmath>
#include <gsl_fit.h>
#include "NumericUtils.h"

double tricube(double u, double t)
{
    // In our case, u represents a distance and hence should be strictly positive.
    if (u < 0) {
        std::cout << "In tricube(u, t), u < 0" << std::endl;
        std::abort();
    }

    // 0 <= u < t
    if ( (closeAtTolerance(u, 0.0) || 0.0 < u) && (u < t) ) {
        // (1 - (u/t)^3)^3
        return pow( ( 1.0 - pow(u/t, 3)), 3 );
    }
    // u >= t
    else {
        return 0.0;
    }
}

double getDistance(double x, double xi)
{
    return fabs(x - xi);
}

int main (int argc, char* argv[])
{
    // Loess algorithm parameters.
    double alpha = 0.33;

    // Create test datasets.
    const unsigned n = 21;

    double xValues[n] = { 0.5578196, 2.0217271, 2.5773252, 3.4140288, 4.3014084, 4.7448394, 5.1073781,
                         6.5411662, 6.7216176, 7.2600583, 8.1335874, 9.1224379, 11.9296663, 12.3797674,
                         13.2728619, 14.2767453, 15.3731026, 15.6476637, 18.5605355, 18.5866354, 18.7572812 };

    double yValues[n] = { 18.63654, 103.49646, 150.35391, 190.51031, 208.70115, 213.71135, 228.49353,
                         233.55387, 234.55054, 223.89225, 227.68339, 223.91982, 168.01999, 164.95750,
                         152.61107, 160.78742, 168.55567, 152.42658, 221.70702, 222.69040, 243.18828 };

    // Loess neighborhood parameter.
    const unsigned q = floor(n * alpha);

    // Working arrays. Yes we need both distances and sortedDistances.
    double distances[n];
    double sortedDistances[n];
    double weights[n];
    double ySmooth[n];
    double c0, c1, cov00, cov01, cov11, chisq, y, yErr;

    for (unsigned i = 0; i < n; ++i) {
        // Compute distances.
        for (unsigned j = 0; j < n; ++j) {
            distances[j] = getDistance(xValues[i], xValues[j]);
            sortedDistances[j] = distances[j];
        }

        // Sort distances in order from smallest to largest.
        std::sort(sortedDistances, sortedDistances + n);

        // Compute weights.
        for (unsigned j = 0; j < n; ++j) {
            weights[j] = tricube(distances[j], sortedDistances[q]);
        }

        // Compute the best-fit linear regression coefficients (c0, c1) of the model Y = c_0 + c_1 X for the weighted
        // dataset (x, y).
        gsl_fit_wlinear(xValues, 1, weights, 1, yValues, 1, n, &c0, &c1, &cov00, &cov01, &cov11, &chisq);

        // Compute the fitted function y and its standard deviation yErr for the model Y = c_0 + c_1 X at the point x.
        gsl_fit_linear_est(xValues[i], c0, c1, cov00, cov01, cov11, &y, &yErr);

        ySmooth[i] = y;
    }

    // Compare to: http://www.itl.nist.gov/div898/handbook/pmd/section1/dep/dep144.htm
    std::cout.setf(std::ios::fixed, std::ios::floatfield);
    for (unsigned i = 0; i < n; ++i) {
        std::cout << xValues[i] << " "
                  << yValues[i] << " "
                  << ySmooth[i]
                  << std::endl;
    }

    return 0;
}