/*****************************************************************************
    This routine must ensure that psMinimizeLM() works correctly.
 
    XXX: This code needs a lot of additional test case work.
    XXX: Use the tst_template.
 *****************************************************************************/
#include <stdio.h>
#include "pslib_strict.h"
#include "psTest.h"
#include <math.h>
#define NUM_ITERATIONS 100
#define ERR_TOL 0.1
#define NUM_DATA_POINTS 20
#define NUM_PARAMS 5
float expectedParm[NUM_PARAMS];
psS32 testStatus = true;

/*****************************************************************************
myFunc():
    sum = param[0] + x[0] * x[1] +
          param[1] * x[0] + 
          param[2] * x[0]^2 +
          param[3] * x[1] + 
          param[4] * x[1]^2
 
 *****************************************************************************/
psF32 myFunc(psVector *deriv,
             psVector *params,
             psVector *x)
{
    if ((deriv == NULL) || (params == NULL)) {
        psError(PS_ERR_UNKNOWN, true, "deriv or params is NULL.\n");
    }

    psF32 sum = (params->data.F32[0] * x->data.F32[0] * x->data.F32[1]) +
                (params->data.F32[1] * x->data.F32[0]) +
                (params->data.F32[2] * PS_SQR(x->data.F32[0])) +
                (params->data.F32[3] * x->data.F32[1]) +
                (params->data.F32[4] * PS_SQR(x->data.F32[1]));

    deriv->data.F32[0] = x->data.F32[0] * x->data.F32[1];
    deriv->data.F32[1] = x->data.F32[0];
    deriv->data.F32[2] = PS_SQR(x->data.F32[0]);
    deriv->data.F32[3] = x->data.F32[1];
    deriv->data.F32[4] = PS_SQR(x->data.F32[1]);

    return(sum);
}

psS32 t01()
{
    psS32 currentId = psMemGetId();
    psTraceSetLevel(".psLib", 0);
    /**************************************************************************
     *************************************************************************/
    psMinimization *min = psMinimizationAlloc(NUM_ITERATIONS, ERR_TOL);
    psImage *myCovar = psImageAlloc(NUM_PARAMS, NUM_PARAMS, PS_TYPE_F32);
    psVector *myParams = psVectorAlloc(NUM_PARAMS, PS_TYPE_F32);
    psVector *myDerivs = psVectorAlloc(NUM_PARAMS, PS_TYPE_F32);
    psArray *myCoords = psArrayAlloc(NUM_DATA_POINTS);
    psVector *y = psVectorAlloc(NUM_DATA_POINTS, PS_TYPE_F32);

    for (psS32 i=0;i<NUM_DATA_POINTS;i++) {
        myCoords->data[i] = (psPtr *) psVectorAlloc(2, PS_TYPE_F32);
        ((psVector *) (myCoords->data[i]))->data.F32[0] = (float) (i+10);
        ((psVector *) (myCoords->data[i]))->data.F32[1] = (float) (i+3);
        psTrace(__func__, 5, "x[%d] is vector (%f, %f)\n", i, ((psVector *) (myCoords->data[i]))->data.F32[0],
                ((psVector *) (myCoords->data[i]))->data.F32[1]);
        y->data.F32[i] = (float) i;
    }
    for (psS32 i=0;i<NUM_PARAMS;i++) {
        expectedParm[i] = 2.42 + (float) (2 * i);
        myParams->data.F32[i] = (float) (2 * i);
    }

    psBool rc = psMinimizeLMChi2(min,
                                 NULL,
                                 myParams,
                                 NULL,
                                 myCoords,
                                 y,
                                 NULL,
                                 (psMinimizeLMChi2Func) myFunc);
    if (rc == false) {
        printf("TEST ERROR: psMinimizeLMChi2() returned FALSE.\n");
        fflush(stdout);
        testStatus = false;
    } else {
        printf("\nThe value of the function at the minimum is %f\n", min->value);
        for (psS32 i=0;i<NUM_DATA_POINTS;i++) {
            printf("The minimum for data point %d: %f\n", i, myFunc(myDerivs, myParams, (psVector *) myCoords->data[i]));
            fflush(stdout);
        }

        for (psS32 i=0;i<NUM_PARAMS;i++) {
            printf("Parameter %d at the minimum is %f (expected: %f)\n", i,
                   myParams->data.F32[i], expectedParm[i]);
            fflush(stdout);
        }
    }

    psFree(min);
    psFree(myCovar);
    psFree(myParams);
    psFree(myDerivs);
    psFree(myCoords);
    psFree(y);
    psMemCheckCorruption(1);
    psS32 memLeaks = psMemCheckLeaks(currentId, NULL, NULL, false);
    if (0 != memLeaks) {
        printf("TEST ERROR: Memory Leaks! (%d leaks).\n", memLeaks);
        fflush(stdout);
        // XXX: This is causing a seg fault
        psAbort(__func__,"Memory Leaks! (%d leaks)", memLeaks);
    }
    return (!testStatus);
}



psS32 main()
{
    psLogSetFormat("HLNM");
    psTraceSetDestination(1);
    psTraceSetLevel(".", 0);
    psTraceSetLevel(__func__, 0);
    psTraceSetLevel("t01", 0);
    psTraceSetLevel("psMinimizeLMChi2_OLD", 0);
    psTraceSetLevel("psMinimizeLMChi2", 0);
    psTraceSetLevel("psMinimizeGaussNewtonDelta", 0);
    psTraceSetLevel("psMinimizeGaussNewtonDelta_EAM", 0);
    psTraceSetLevel("p_psMinLM_GuessABP", 0);
    psTraceSetLevel("p_psMinLM_GuessABP_EAM", 0);
    psTraceSetLevel("p_psMinLM_SetABX", 0);
    psTraceSetLevel("psGaussJordan", 0);
    psTraceSetLevel("psMinimizationAlloc", 0);
    psTraceSetLevel("psMemCheckMinimization", 0);
    psTraceSetLevel("p_psDetermineBracket", 0);
    psTraceSetLevel("p_psDetermineBracket2", 0);
    psTraceSetLevel("p_psLineMin", 0);
    psTraceSetLevel("psMinimizePowell", 0);
    psTraceSetLevel("myPowellChi2Func", 0);
    psTraceSetLevel("psMinimizeChi2Powell", 0);
    psTraceSetLevel("BuildSums1D", 0);
    psTraceSetLevel("BuildSums2D", 0);
    psTraceSetLevel("Polynomial2DEvalVectorD", 0);
    psTraceSetLevel("vectorFitPolynomial1DCheby", 0);
    psTraceSetLevel("VectorFitPolynomial1DOrd", 0);
    psTraceSetLevel("psVectorFitPolynomial1D", 0);
    psTraceSetLevel("psVectorClipFitPolynomial1D", 0);

    psTrace(__func__, 2, "Calling new psMinimize().\n");
    t01();
    printf("DONE\n");
    fflush(stdout);
}


