/*****************************************************************************
    This routine must ensure that the psMinimize function correctly
    minimizes an arbitrary function.
 *****************************************************************************/
#include <stdio.h>
#include "pslib.h"
#include "psTest.h"
#include "psMemory.h"
#include "psVector.h"
#include "psImage.h"
#include "psFunctions.h"
#include "psMinimize.h"
#include <math.h>
#define DATA_WIDTH 2
#define NUM_PARAMS 4

float myFunc0(const psVector *restrict myParams,
              const psVector *restrict myCoords)
{
    float P0 = myCoords->data.F32[0];
    float P1 = myCoords->data.F32[1];
    float x = myParams->data.F32[0];
    float y = myParams->data.F32[1];
    float tmp = 0.0;

    tmp = 10.0 * (x - P0) * (x - P0) + 20.0 * (y - P1) * (y - P1) + 30.0;
    //    printf("--------- myFunc((%.1f %.1f) %.1f %.1f) is %.1f ---------\n",
    //    printf("--------- myFunc() params: (%.1f %.1f) coords: (%.1f %.1f) is %.1f ---------\n",
    //           x, y, P0, P1, tmp);

    return(tmp);
}

float myFuncDeriv0(const psVector *restrict myParams,
                   const psVector *restrict myCoords,
                   psS32 whichParamDeriv)
{
    float P0 = myCoords->data.F32[0];
    float P1 = myCoords->data.F32[1];
    float x = myParams->data.F32[0];
    float y = myParams->data.F32[1];
    float tmp = 0.0;

    if (whichParamDeriv == 0) {
        tmp = 20.0 * (x - P0);
    } else
        if (whichParamDeriv == 1) {
            tmp = 40.0 * (y - P1);
        }

    //    printf("--------- myFuncDeriv((%.1f %.1f) %.1f %.1f (%d)) is %.1f ---------\n",
    //            x, y, myParams->data.F32[0], myParams->data.F32[1], whichParamDeriv, tmp);
    return(tmp);
}


float myFunc(const psVector *restrict myParams,
             const psVector *restrict myCoords)
{
    float x = myCoords->data.F32[0];
    float y = myCoords->data.F32[1];
    float A = myParams->data.F32[0];
    float B = myParams->data.F32[1];
    float C = myParams->data.F32[2];
    float D = myParams->data.F32[3];
    float tmp = 0.0;

    tmp = (A * x) + (B*x*x) + (C*y*y) + (D*x*x*y);
    printf("-- myFunc() params: (%.1f %.1f %.1f %.1f) coords: (%.1f %.1f) is %.1f ---------\n",
           A, B, C, D, x, y, tmp);

    return(tmp);
}

float myFuncDeriv(const psVector *restrict myParams,
                  const psVector *restrict myCoords,
                  psS32 whichParamDeriv)
{
    float x = myCoords->data.F32[0];
    float y = myCoords->data.F32[1];
    float tmp = 0.0;

    if (whichParamDeriv == 0) {
        tmp = 1.0;
        tmp = x;
    } else
        if (whichParamDeriv == 1) {
            tmp = x;
            tmp = x*x;
        } else
            if (whichParamDeriv == 2) {
                tmp = y;
                tmp = y*y;
            } else
                if (whichParamDeriv == 3) {
                    tmp = x * x * y;
                }

    printf("--------- myFuncDeriv((%.1f %.1f) %.1f %.1f %.1f %.1f (%d)) is %.1f ---------\n",
           x, y, myParams->data.F32[0], myParams->data.F32[1],
           myParams->data.F32[2], myParams->data.F32[3], whichParamDeriv, tmp);
    return(tmp);
}


psS32 main()
{
    psVector *initialGuess = NULL;
    psVector *paramMask = NULL;
    psVector *coord = NULL;
    psS32 i = 0;
    psS32 currentId = psMemGetId();
    psS32 testStatus = true;
    psS32 memLeaks = 0;
    psVector *theParams = NULL;

    initialGuess = psVectorAlloc(NUM_PARAMS, PS_TYPE_F32);
    paramMask = psVectorAlloc(NUM_PARAMS, PS_TYPE_F32);
    coord = psVectorAlloc(DATA_WIDTH, PS_TYPE_F32);


    //--------- myFunc((0.0 1.0) -8.6 -7.6 14.6 5.0) is 6.0 ---------

    // Build the data.
    initialGuess->data.F32[0] = 2.0;
    initialGuess->data.F32[1] = 3.0;
    initialGuess->data.F32[2] = 4.0;
    initialGuess->data.F32[3] = 5.0;

    for (i=0;i<DATA_WIDTH;i++) {
        coord->data.F32[i] = (float) i;
    }
    coord->data.F32[0] = 1.0;
    coord->data.F32[1] = 2.0;

    printPositiveTestHeader(stdout,
                            "psMinimize functions",
                            "psMinimize(): no masks");

    for (i=0;i<NUM_PARAMS;i++)
        printf("Initial parameter %d is: %.1f\n", i, initialGuess->data.F32[i]);

    theParams = psMinimize(initialGuess,
                           myFunc,
                           myFuncDeriv,
                           coord,
                           NULL);

    for (i=0;i<NUM_PARAMS;i++)
        printf("Parameter %d is: %.1f\n", i, theParams->data.F32[i]);

    printf("Function is %.1f\n", myFunc(theParams, coord));

    psMemCheckCorruption(1);
    printFooter(stdout,
                "psMinimize functions",
                "psMinimize(): no masks",
                testStatus);

    psMemCheckCorruption(1);
    psFree(initialGuess);
    psFree(paramMask);
    psFree(coord);

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

    return (!testStatus);
}
