/*****************************************************************************
    This routine must ensure that psMinimizeLM() works correctly.
 *****************************************************************************/
#include <stdio.h>
#include "pslib.h"
#include "psTest.h"
#include <math.h>
#define NUM_ITERATIONS 10000
#define ERR_TOL 0.0
#define N 5
#define MIN_VALUE 5.0
#define NUM_PARAMS 3
float expectedParm[NUM_PARAMS];
int testStatus = true;

/*****************************************************************************
myFunc(): This routine subtracts the associate value in expectedParm[] from
each parameter and then squares it, then sums that for all parameters, then
adds MIN_VALUE to it.  The minimum for this function will be MIN_VALUE, and
will occur when each parameter equals the associated value in expectedParm[].
 
This procedure ignores the coordinates, other than to ensure that they were
passed correctly from psMinimizePowell().
 
 *****************************************************************************/
float myFunc(psVector *myDeriv,
             psVector *myParams,
             psVector *myCoords)
{
    float sum = 0.0;
    //    float coordData = 0.0;
    //    float expData = 0.0;
    int i;

    if (myDeriv == NULL) {
        myDeriv = psVectorAlloc(myParams->n, PS_TYPE_F32);
        psError(__func__, "myDeriv is NULL.\n");
    }

    // Simply test that coords were passed in correctly.
    /*
        for (i=0;i<N;i++) {
            coordData = myCoords->data.F32[0];
            expData = (float) (i+10);
            if (fabs(coordData - expData) > FLT_EPSILON) {
                printf("ERROR(1): coordinate data was incorrectly passed to myFunc()\n");
                printf("ERROR(1): was (%f) should be (%f)\n", coordData, expData);
                testStatus = false;
            }
            coordData = myCoords->data.F32[1];
            expData = (float) (i+3);
            if (fabs(coordData - expData) > FLT_EPSILON) {
                printf("ERROR(2): coordinate data was incorrectly passed to myFunc()\n");
                printf("ERROR(2): was (%f) should be (%f)\n", coordData, expData);
                testStatus = false;
            }
        }
    */

    sum = 0.0;
    for (i=0;i<NUM_PARAMS;i++) {
        sum+= (myParams->data.F32[i] - expectedParm[i]) *
              (myParams->data.F32[i] - expectedParm[i]);
        myDeriv->data.F32[i] = (2.0 * myParams->data.F32[i]) -
                               (2.0 * expectedParm[i]);
    }
    //    for (i=0;i<NUM_PARAMS;i++)
    //        printf("HMMM: myParams->data.F32[%d] is %f\n", i, myParams->data.F32[i]);
    //    for (i=0;i<NUM_PARAMS;i++)
    //        printf("HMMM: myDeriv->data.F32[%d] is %f\n", i, myDeriv->data.F32[i]);

    sum+= MIN_VALUE;
    return(sum);
}


int main()
{
    int currentId = psMemGetId();
    int memLeaks = 0;
    int i = 0;
    psArray *myCoords;
    psVector *myParams;
    psVector *myParamMask;
    psImage *myCovar;
    psMinimization *min;
    psVector *x = psVectorAlloc(N, PS_TYPE_F32);
    psVector *y = psVectorAlloc(N, PS_TYPE_F32);

    psTraceSetLevel(".psLib", 0);
    /**************************************************************************
     *************************************************************************/
    myParams = psVectorAlloc(NUM_PARAMS, PS_TYPE_F32);
    myParamMask = psVectorAlloc(NUM_PARAMS, PS_TYPE_F32);
    min = psMinimizationAlloc(NUM_ITERATIONS, ERR_TOL);
    myCovar = psImageAlloc(NUM_PARAMS, NUM_PARAMS, PS_TYPE_F32);

    myCoords = psArrayAlloc(N);
    for (i=0;i<N;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);
        x->data.F32[i] = (float) i;
        y->data.F32[i] = (float) i;
    }
    for (i=0;i<NUM_PARAMS;i++) {
        expectedParm[i] = 2.42 + (float) (2 * i);
        myParams->data.F32[i] = (float) i;
        myParams->data.F32[i] = expectedParm[i] * 1.3;
        myParams->data.F32[i] = (float) (5 + i);
        myParams->data.F32[i] = 0.0;
        myParamMask->data.U8[i] = 0;
    }

    psMinimizeLMChi2(min,
                     myParams,
                     NULL,
                     myCovar,
                     myCoords,
                     y,
                     (psMinimizeLMChi2Func) myFunc);

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

    psFree(myCoords);
    psFree(myParams);
    psFree(myParamMask);
    psFree(min);
    psFree(x);
    psFree(y);
    psFree(myCovar);

    psMemCheckCorruption(1);
    memLeaks = psMemCheckLeaks(currentId,NULL,NULL);
    if (0 != memLeaks) {
        psAbort(__func__,"Memory Leaks! (%d leaks)", memLeaks);
    }
    return (!testStatus);
}
