# include <pslib.h>
# include <pmObjects.h>

/* pars: x, sx, sky, I, */
psF64 fgaussOD (psVector *dpar, const psVector *par, const psVector *x) {

    double X, S, Z, R, f;

    X = x->data.F32[0] - par->data.F32[0];
    S = 1.0 / (par->data.F32[1]*par->data.F32[1]);
    Z = -0.5*X*X*S;
    R = exp (Z);
    f = par->data.F32[2]*R + par->data.F32[3];

    if (dpar != NULL) {
	dpar->data.F32[0] = par->data.F32[2]*R*X*S;
	dpar->data.F32[1] = dpar->data.F32[0]*X/par->data.F32[1];
	dpar->data.F32[2] = R;
	dpar->data.F32[3] = 1;
    }
    return (f);
}

/* pars: x, sx, */
psF64 fgaussODs (psVector *dpar, const psVector *par, const psVector *x) {

    double X, S, Z, R, f;

    X = x->data.F32[0] - par->data.F32[0];
    S = 1.0 / (par->data.F32[1]*par->data.F32[1]);
    Z = -0.5*X*X*S;
    f = exp (Z);

    if (dpar != NULL) {
	dpar->data.F32[0] = f*X*S;
	dpar->data.F32[1] = dpar->data.F32[0]*X/par->data.F32[1];
    }
    return (f);
}

// test psMinimizeLMChi2
int main_test (int argc, char **argv) {

    int i;
    bool rc;
    psF32 dx;
    psArray  *x;
    psVector *y;
    psVector *v;
    psVector *p;

    psTraceSetLevel (".", 3);

    psMinimization *myMin = psMinimizationAlloc (20, 0.001); // (iterations, tolerance)

    // create data vector
    x = psArrayAlloc (100);
    y = psVectorAlloc (100, PS_TYPE_F32);
    for (i = 0; i < x[0].n; i++) {
	v = psVectorAlloc (1, PS_TYPE_F32);
	v[0].data.F32[0] = i;
	x[0].data[i] = (psPtr) v;
	dx = i - 30;
	y[0].data.F32[i] = exp (-(dx*dx)/(2*3*3));
	// xo = 30.0, sigma = 3.0
    }
    p = psVectorAlloc (2, PS_TYPE_F32);
    p[0].data.F32[0] = 25.0;
    p[0].data.F32[1] = 5.0;

    rc = psMinimizeLMChi2(myMin, NULL, p, NULL, x, y, NULL, fgaussODs);
  
    for (i = 0; i < p[0].n; i++) {
	fprintf (stderr, "par %d: %f\n", i, p[0].data.F32[i]);
    }

    exit (0);
}

// test psMinimizeLMChi2
int main (int argc, char **argv) {

    int i;
    bool rc;
    psF32 dx;
    psArray  *x;
    psVector *y;
    psVector *v;
    psVector *p;
    psVector *mask;

    psTraceArguments (&argc, argv);

    psMinimization *myMin = psMinimizationAlloc (20, 0.1); // (iterations, tolerance)

    // create data vector
    x = psArrayAlloc (100);
    y = psVectorAlloc (100, PS_TYPE_F32);
    for (i = 0; i < x[0].n; i++) {
	v = psVectorAlloc (1, PS_TYPE_F32);
	v[0].data.F32[0] = i;
	x[0].data[i] = (psPtr) v;
	dx = i - 30;
	y[0].data.F32[i] = 5.0*exp (-(dx*dx)/(2*3*3)) + 4.0;
	// sigma = 3.0, xo = 30.0, A = 5.0, B = 4.0
    }
    p = psVectorAlloc (4, PS_TYPE_F32);
    p[0].data.F32[0] = 25.0;
    p[0].data.F32[1] = 5.0;
    p[0].data.F32[2] = 8.0;
    p[0].data.F32[3] = 2.0;

    mask = psVectorAlloc (4, PS_TYPE_U8);
    mask->data.U8[0] = 0;
    mask->data.U8[1] = 0;
    mask->data.U8[2] = 0;
    mask->data.U8[3] = 0;

    rc = psMinimizeLMChi2(myMin, NULL, p, mask, x, y, NULL, fgaussOD);
  
    for (i = 0; i < p[0].n; i++) {
	fprintf (stderr, "par %d: %f\n", i, p[0].data.F32[i]);
    }

    exit (0);
}
