
/******************************************************************************
    one component, two slopes:
    1 / (1 + z^M + z^N)
 
    params->data.F32[0] = So;
    params->data.F32[1] = Zo;
    params->data.F32[2] = Xo;
    params->data.F32[3] = Yo;
    params->data.F32[4] = sqrt(2.0) / SigmaX;
    params->data.F32[5] = sqrt(2.0) / SigmaY;
    params->data.F32[6] = Sxy;
    params->data.F32[7] = 
    params->data.F32[8] = 
*****************************************************************************/

psF32 pmModelFunc_QGAUSS(psVector *deriv,
                         const psVector *params,
                         const psVector *x)
{
    psF32 *PAR = params->data.F32;

    psF32 X  = x->data.F32[0] - PAR[2];
    psF32 Y  = x->data.F32[1] - PAR[3];
    psF32 px = PAR[4]*X;
    psF32 py = PAR[5]*Y;
    psF32 z  = 0.5*PS_SQR(px) + 0.5*PS_SQR(py) + PAR[6]*X*Y;
    if (z < 0)
        z = 0;

    psF32 zp = pow(z,1.25);
    psF32 r  = 1.0 / (1 + PAR[7]*z + z*zp);

    psF32 r1 = PAR[1]*r;
    psF32 f  = r1 + PAR[0];

    if (deriv != NULL) {
        psF32 *dPAR = deriv->data.F32;

        // note difference from a pure gaussian: q = params->data.F32[1]*r
        psF32 t = r1*r;
        psF32 q = t*(PAR[7] + 2.25*zp);

        dPAR[0] = +1.0;
        dPAR[1] = +r;
        dPAR[2] = q*(2.0*px*PAR[4] + PAR[6]*Y);
        dPAR[3] = q*(2.0*py*PAR[5] + PAR[6]*X);
        dPAR[4] = -2.0*q*px*X;
        dPAR[5] = -2.0*q*py*Y;
        dPAR[6] = -q*X*Y;
        dPAR[7] = -t*z;
    }
    return(f);
}

bool pmModelLimits_QGAUSS (psVector **beta_lim, psVector **params_min, psVector **params_max)
{

    *beta_lim   = psVectorAlloc (8, PS_TYPE_F32);
    *params_min = psVectorAlloc (8, PS_TYPE_F32);
    *params_max = psVectorAlloc (8, PS_TYPE_F32);

    beta_lim[0][0].data.F32[0] = 1000;
    beta_lim[0][0].data.F32[1] = 3e6;
    beta_lim[0][0].data.F32[2] = 5;
    beta_lim[0][0].data.F32[3] = 5;
    beta_lim[0][0].data.F32[4] = 0.5;
    beta_lim[0][0].data.F32[5] = 0.5;
    beta_lim[0][0].data.F32[6] = 0.5;
    beta_lim[0][0].data.F32[7] = 0.5;

    params_min[0][0].data.F32[0] = -1000;
    params_min[0][0].data.F32[1] = 0;
    params_min[0][0].data.F32[2] = -100;
    params_min[0][0].data.F32[3] = -100;
    params_min[0][0].data.F32[4] = 0.01;
    params_min[0][0].data.F32[5] = 0.01;
    params_min[0][0].data.F32[6] = -5.0;
    params_min[0][0].data.F32[7] = 0.1;

    params_max[0][0].data.F32[0] = 1e5;
    params_max[0][0].data.F32[1] = 1e8;
    params_max[0][0].data.F32[2] = 1e4;  // this should be set by image dimensions!
    params_max[0][0].data.F32[3] = 1e4;  // this should be set by image dimensions!
    params_max[0][0].data.F32[4] = 2.0;
    params_max[0][0].data.F32[5] = 2.0;
    params_max[0][0].data.F32[6] = +5.0;
    params_max[0][0].data.F32[7] = 10.0;

    return (TRUE);
}

bool pmModelGuess_QGAUSS (pmModel *model, pmSource *source)
{

    pmMoments *moments = source->moments;
    pmPeak    *peak    = source->peak;
    psF32     *params  = model->params->data.F32;

    params[0] = moments->Sky;
    params[1] = moments->Peak - moments->Sky;
    params[2] = peak->x;
    params[3] = peak->y;
    params[4] = 1.2 / moments->Sx;
    params[5] = 1.2 / moments->Sy;
    params[6] = 0.0;
    params[7] = 1.0;

    return(true);
}

psF64 pmModelFlux_QGAUSS(const psVector *params)
{
    float z;
    float norm;

    psF32 *PAR = params->data.F32;

    psF64 A1   = PS_SQR(PAR[4]);
    psF64 A2   = PS_SQR(PAR[5]);
    psF64 A3   = PS_SQR(PAR[6]);
    psF64 Area = 2.0 * M_PI / sqrt(A1*A2 - A3);
    // Area is equivalent to 2 pi sigma^2

    // the area needs to be multiplied by the integral of f(z)
    // XXX this integral can be done faster and more accurately
    norm = 0.0;

    # define DZ 0.25

    # if 0

    float f;
float zp;
for (z = 0.5*DZ; z < 50; z += DZ) {
        zp = pow(z,2.25);
        f = 1.0 / (1 + PAR[7]*z + zp);
        norm += f;
    }
    norm *= DZ;
    # else

        float f0 = 1.0;
    float f1, f2;
    for (z = DZ; z < 50; z += DZ) {
        f1 = 1.0 / (1 + PAR[7]*z + pow(z, 2.25));
        z += DZ;
        f2 = 1.0 / (1 + PAR[7]*z + pow(z, 2.25));
        norm += f0 + 4*f1 + f2;
        f0 = f2;
    }
    norm *= DZ / 3.0;
    # endif

    psF64 Flux = PAR[1] * Area * norm;

    return(Flux);
}

// define this function so it never returns Inf or NaN
// return the radius which yields the requested flux
psF64 pmModelRadius_QGAUSS  (const psVector *params, psF64 flux)
{
    psF64 z, f;
    psF32 *PAR = params->data.F32;
    int Nstep = 0;

    if (flux <= 0)
        return (1.0);
    if (PAR[1] <= 0)
        return (1.0);
    if (flux >= PAR[1])
        return (1.0);

    // if Sx == Sy, sigma = Sx == Sy
    psF64 sigma = hypot (1.0 / PAR[4], 1.0 / PAR[5]) / sqrt(2.0);
    psF64 limit = flux / PAR[1];

    # if 0
    /* test example will just use both, printing both */
    psF64 dz = 1.0 / (2.0 * sigma*sigma);

    // we can do this much better with intelligent choices here
    for (z = 0.0; z < 30.0; z += dz) {
        Nstep ++;
        f = 1.0 / (1 + PAR[7]*z + pow(z, 2.25));
        // test: f = 1.0 / (1 + PAR[7]*z + PS_SQR(z));
        if (f < limit)
            break;
    }
    // fprintf (stderr, "rad 1: %f, want: %f, got: %f (%d steps)\n", z, limit, f, Nstep);

    # else

        /* use the fact that f is monotonically decreasing */
        z = 0;
    Nstep = 0;

    // choose a z value guaranteed to be beyond our limit
    float z0 = pow((1.0 / limit), (1.0 / 2.25));
    float z1 = (1.0 / limit) / PAR[7];
    z1 = PS_MAX (z0, z1);
    z0 = 0.0;

    float f0 = 1.0 / (1 + PAR[7]*z0 + pow(z0, 2.25));
    float f1 = 1.0 / (1 + PAR[7]*z1 + pow(z1, 2.25));
    while ((Nstep < 10) && (fabs(z1 - z0) > 0.5)) {
        z = 0.5*(z0 + z1);
        f = 1.0 / (1 + PAR[7]*z + pow(z, 2.25));
        // fprintf (stderr, "%f  %f  %f   :   %f  %f  %f\n", f0, f, f1, z0, z, z1);
        if (f > limit) {
            z0 = z;
            f0 = f;
        } else {
            z1 = z;
            f1 = f;
        }
        Nstep ++;
    }
    // fprintf (stderr, "rad 2: %f, want: %f, got: %f (%d steps)\n", z, limit, f, Nstep);
    # endif

    psF64 radius = sigma * sqrt (2.0 * z);
    if (isnan(radius)) {
        fprintf (stderr, "error in code\n");
    }
    return (radius);
}

bool pmModelFromPSF_QGAUSS (pmModel *modelPSF, pmModel *modelFLT, pmPSF *psf)
{

    psF32 *out = modelPSF->params->data.F32;
    psF32 *in  = modelFLT->params->data.F32;

    out[0] = in[0];
    out[1] = in[1];
    out[2] = in[2];
    out[3] = in[3];

    for (int i = 4; i < 8; i++) {
        psPolynomial2D *poly = psf->params->data[i-4];
        // XXX: Verify this (from EAM change)
        //out[i] = Polynomial2DEval_EAM(poly, out[2], out[3]);
        out[i] = psPolynomial2DEval(poly, out[2], out[3]);
    }
    return(true);
}

bool pmModelFitStatus_QGAUSS (pmModel *model)
{

    psF32 dP;
    bool  status;

    psF32 *PAR  = model->params->data.F32;
    psF32 *dPAR = model->dparams->data.F32;

    dP = 0;
    dP += PS_SQR(dPAR[4] / PAR[4]);
    dP += PS_SQR(dPAR[5] / PAR[5]);
    dP = sqrt (dP);

    status = true;
    status &= (dP < 0.5);
    status &= (PAR[1] > 0);
    status &= ((dPAR[1]/PAR[1]) < 0.5);

    if (!status)
        return false;
    return true;
}
