#include <stdio.h>
#include <pslib.h>

#if 0
psVector *gauss(const psVector *params, const psArray *coords)
{
    float norm = params->data.F32[0];
    float mean = params->data.F32[1];
    float width = params->data.F32[2];

    psVector *results = psVectorAlloc(coords->n, PS_TYPE_F32);
    for (int i = 0; i < coords->n; i++) {
        psVector *values = coords->data[i];
        float x = (values->data.F32[0] - mean) / width;
        results->data.F32[i] = norm * expf(-0.5 * PS_SQR(x));
    }

    return results;
}
#else
float gauss(psVector *deriv, const psVector *params, const psVector *coords)
{
    float norm = params->data.F32[0];
    float mean = params->data.F32[1];
    float width = params->data.F32[2];

    float x = coords->data.F32[0];
    float dx = x - mean;
    float value = dx / width;
    float func = norm * expf(-0.5 * PS_SQR(value));

    deriv->data.F32[0] = func / norm;
    deriv->data.F32[1] = func * value / width;
    deriv->data.F32[2] = func * PS_SQR(value) / width;

    return func;
}
#endif


int main(int argc, char *argv[])
{
    psTraceSetLevel("psLib.math", 2);

    const char *name = argv[1];
    psArray *input = psVectorsReadFromFile(name, "%f %d\n");
    psVector *x = input->data[0];
    psVector *n = input->data[1];

    psVector *nums = psVectorCopy(NULL, n, PS_TYPE_F32);

    int bins = x->n;

    float max = -INFINITY;
    int maxBin = -1;
    for (int i = 0; i < bins; i++) {
        int num = n->data.S32[i];
        if (num > max) {
            max = num;
            maxBin = i;
        }
    }
    assert(maxBin != -1 && isfinite(max));

    int loHalf = -1, hiHalf = -1;
    for (loHalf = maxBin; loHalf >= 0 && n->data.S32[loHalf] > max / 2.0; loHalf--);
    for (hiHalf = maxBin; hiHalf < bins && n->data.S32[hiHalf] > max / 2.0; hiHalf++);
    float width = 2 * (x->data.F32[hiHalf] - x->data.F32[loHalf]) / 2.35;

    psMinimization *min = psMinimizationAlloc(1000, 1.0e-9);

    psVector *params = psVectorAlloc(3, PS_TYPE_F32);
    params->data.F32[0] = 0.5 * max;
    params->data.F32[1] = x->data.F32[maxBin + 5];
    params->data.F32[2] = width;

    psArray *coords = psArrayAlloc(bins);
    for (int i = 0; i < bins; i++) {
        psVector *coord = coords->data[i] = psVectorAlloc(1, PS_TYPE_F32);
        coord->data.F32[0] = x->data.F32[i];
    }


#if 0
    if (!psMinimizeChi2Powell(min, params, NULL, coords, nums, NULL, gauss)) {
        psErrorStackPrint(stderr, "Failed to minimize");
        exit(PS_EXIT_DATA_ERROR);
    }
#else
    psImage *covar = NULL;
    if (!psMinimizeLMChi2(min, covar, params, NULL, coords, nums, NULL, gauss)) {
        psErrorStackPrint(stderr, "Failed to minimize");
        exit(PS_EXIT_DATA_ERROR);
    }
    psFree(covar);
#endif

    printf("# %f %f %f\n", params->data.F32[0], params->data.F32[1], params->data.F32[2]);

    float norm = params->data.F32[0];
    float mean = params->data.F32[1];
    float sigma = params->data.F32[2];
    for (int i = 0; i < bins; i++) {
        printf("%f %f\n", x->data.F32[i], norm * expf(-0.5 * PS_SQR((x->data.F32[i] - mean) / sigma)));
    }

    psFree(nums);
    psFree(min);
    psFree(coords);
    psFree(params);
    psFree(input);

    return PS_EXIT_SUCCESS;
}
