#ifdef HAVE_CONFIG_H
# include "config.h"
#endif

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <unistd.h>

#include "psMemory.h"
#include "psError.h"
#include "psLogMsg.h"
#include "psVector.h"
#include "psVectorSmooth.h"
#include "psAssert.h"


psVector *psVectorSmooth(psVector *output,
                         const psVector *input,
                         double sigma,
                         double Nsigma
                        )
{
    PS_ASSERT_VECTOR_NON_NULL(input, NULL);
    PS_ASSERT_FLOAT_LARGER_THAN(sigma, 0.0, NULL);
    PS_ASSERT_FLOAT_LARGER_THAN(Nsigma, 0.0, NULL);

    // relevant terms
    long Nrange = sigma*Nsigma + 0.5;   // Extent of smoothing
    long Npixel = 2*Nrange + 1;         // Total size of smoothing kernel
    long Nbin = input->n;               // Number of elements
    double factor = -0.5/(sigma*sigma); // Factor for Gaussian

    #define VECTOR_SMOOTH_CASE(TYPE) \
case PS_TYPE_##TYPE: { \
        output = psVectorRecycle(output, Nbin, PS_TYPE_##TYPE); \
        /* generate normalized gaussian */ \
        psVector *gaussnorm = psVectorAlloc(Npixel, PS_TYPE_##TYPE); \
        double sum = 0.0; \
        for (long i = -Nrange; i < Nrange + 1; i++) { \
            gaussnorm->data.TYPE[i+Nrange] = exp(factor*i*i); \
            sum += gaussnorm->data.TYPE[i+Nrange]; \
        } \
        for (long i = -Nrange; i < Nrange + 1; i++) { \
            gaussnorm->data.TYPE[i+Nrange] /= sum; \
        } \
        ps##TYPE *gauss = &gaussnorm->data.TYPE[Nrange]; \
        \
        /* smooth vector */ \
        psVector *temp = psVectorAlloc(Nbin, PS_TYPE_##TYPE); \
        ps##TYPE *vi = input->data.TYPE; \
        ps##TYPE *vo = temp->data.TYPE; \
        /* smooth first Nrange pixels, with renorm */ \
        /* XXX need to check that this does not run over end for narrow vectors */ \
        for (long i = 0; i < Nrange; i++, vi++, vo++) { \
            ps##TYPE *vr = vi - i; \
            ps##TYPE *vg = gauss - i; \
            double g = 0; \
            double s = 0; \
            for (int n = -i; n < Nrange + 1; n++, vr++, vg++) { \
                s += *vg * *vr; \
                g += *vg; \
            } \
            *vo = s / g; \
        } \
        /* smooth middle pixels */ \
        for (long i = Nrange; i < Nbin - Nrange; i++, vi++, vo++) { \
            ps##TYPE *vr = vi - Nrange; \
            ps##TYPE *vg = gauss - Nrange; \
            double s = 0; \
            for (int n = -Nrange; n < Nrange + 1; n++, vr++, vg++) { \
                s += *vg * *vr; \
            } \
            *vo = s; \
        } \
        /* smooth last Nrange pixels, with renorm */ \
        /* XXX does this miss the last column? */ \
        for (long i = Nbin - Nrange; i < Nbin; i++, vi++, vo++) { \
            ps##TYPE *vr = vi - Nrange; \
            ps##TYPE *vg = gauss - Nrange; \
            double g = 0; \
            double s = 0; \
            for (int n = -Nrange; n < Nbin - i - 1; n++, vr++, vg++) { \
                s += *vg * *vr; \
                g += *vg; \
            } \
            *vo = s / g; \
        } \
        memcpy(output->data.TYPE, temp->data.TYPE, Nbin*sizeof(ps##TYPE)); \
        psFree(temp); \
        psFree(gaussnorm); \
        break; \
    }

    switch (input->type.type) {
        VECTOR_SMOOTH_CASE(F32);
        VECTOR_SMOOTH_CASE(F64);
    default: {
            char* typeStr;
            PS_TYPE_NAME(typeStr, input->type.type);
            psError(PS_ERR_BAD_PARAMETER_TYPE, true, "Type %s is not valid.", typeStr);
            return NULL;
        }
    }
    return output;
}
