/*****************************************************************************
This routine must ensure that the psVectorFitPolynomial2D works correctly.
We create a vectors of data points (x and y), and populate them with the
values from an arbitrary function setData().  We then call
psVectorFitPolynomial2D() with a regular polynomial data structure.  We then
evaluate the polynomial with the coefficients generated above and determine
if they are within an error tolerance of the expected values.
 
    t00(): all input vectors are non-NULL.
    t01(): yErr is NULL.
    t02(): x, yErr is NULL.
    t03(): x, y, yErr is NULL.
 *****************************************************************************/
#include <stdio.h>
#include "pslib_strict.h"
#include "psTest.h"
#include "psMemory.h"
#include "psVector.h"
#include "psImage.h"
#include "psMinimize.h"
#include <math.h>
#define NUM_DATA 10
#define POLY_ORDER 5
#define A 3.0
#define B 2.0
#define C 3.0
#define D 3.0
#define E 3.0
#define F 3.0
#define ERROR_TOLERANCE 0.10
#define FERR 1.0
#define PRINT_RESULTS 0

double setData(double x, double y)
{
    return(A + (B * x) + (C * x * x) + (D * y) + (E * y * y) + (E * x * y));
}

psS32 t00()
{
    psS32 currentId = psMemGetId();
    psS32 testStatus = true;
    psS32 memLeaks = 0;
    double expectData;
    double actualData;

    psPolynomial2D *myPoly = psPolynomial2DAlloc(POLY_ORDER, POLY_ORDER, PS_POLYNOMIAL_ORD);
    psVector *x = psVectorAlloc(NUM_DATA, PS_TYPE_F64);
    psVector *y = psVectorAlloc(NUM_DATA, PS_TYPE_F64);
    psVector *f = psVectorAlloc(NUM_DATA, PS_TYPE_F64);
    psVector *fErr = psVectorAlloc(NUM_DATA, PS_TYPE_F64);

    for (psS32 i=0;i<NUM_DATA;i++) {
        x->data.F64[i] = (double) i;
        y->data.F64[i] = (double) i;
        f->data.F64[i] = setData(x->data.F64[i], y->data.F64[i]);
        fErr->data.F64[i] = FERR;
    }

    printPositiveTestHeader(stdout,
                            "psMinimize functions",
                            "psVectorFitPolynomial2D(): equal difference in variable fErr");

    printf("Calling psVectorFitPolynomial2D() with f, fErr, x, y as F32 vectors.\n");
    psVectorFitPolynomial2D(myPoly, NULL, 0, f, fErr, x, y);

    //    for (i=0;i<POLY_ORDER+1;i++) {
    //        printf("Polynomial coefficient %d is %0.1f\n", i, myPoly->coeff[i]);
    //    }

    for (psS32 i=0;i<NUM_DATA;i++) {
        expectData = setData(x->data.F64[i],  y->data.F64[i]);
        actualData = psPolynomial2DEval(myPoly, x->data.F64[i], y->data.F64[i]);
        if (fabs(actualData-expectData) > fabs(ERROR_TOLERANCE * expectData)) {
            printf("ERROR: Fitted data %d: (%.1f %.1f %.1f), expected was (%.1f)\n",
                   i, x->data.F64[i], y->data.F64[i], actualData, expectData);
            testStatus = false;
        }
        if (PRINT_RESULTS) {
            printf("Data point [%d] is %f, should be %f.\n", i, actualData, expectData);
        }
    }


    printf("Calling psVectorFitPolynomial2D() with fErr set to NULL.\n");
    printf("Running test with fErr set to NULL.\n");
    psVectorFitPolynomial2D(myPoly, NULL, 0, f, NULL, x, y);
    for (psS32 i=0;i<NUM_DATA;i++) {
        expectData = setData(x->data.F64[i],  y->data.F64[i]);
        actualData = psPolynomial2DEval(myPoly, x->data.F64[i], y->data.F64[i]);
        if (fabs(actualData-expectData) > fabs(ERROR_TOLERANCE * expectData)) {
            printf("ERROR: Fitted data %d: (%.1f %.1f %.1f), expected was (%.1f)\n",
                   i, x->data.F64[i], y->data.F64[i], actualData, expectData);
            testStatus = false;
        }
        if (PRINT_RESULTS) {
            printf("Data point [%d] is %f, should be %f.\n", i, actualData, expectData);
        }
    }
    psFree(myPoly);

    printf("Running test with x, y set to NULL.  Should generate error.\n");
    myPoly = psVectorFitPolynomial2D(NULL, NULL, 0, f, fErr, NULL, NULL);
    if (myPoly != NULL) {
        printf("TEST ERROR: psVectorFitPolynomial2D() returned non-NULL.\n");
        testStatus = false;
    }

    psMemCheckCorruption(1);
    psFree(x);
    psFree(y);
    psFree(f);
    psFree(fErr);
    psMemCheckCorruption(1);
    memLeaks = psMemCheckLeaks(currentId,NULL,stderr,false);
    if (0 != memLeaks) {
        psAbort(__func__,"Memory Leaks! (%d leaks)", memLeaks);
    }

    printFooter(stdout,
                "psMinimize functions",
                "psVectorFitPolynomial2D(): equal differences in variable fErr",
                testStatus);

    return (!testStatus);
}

psS32 main()
{
    psLogSetFormat("HLNM");
    t00();
}
