

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

# define SQ(A)((A)*(A))
psF64 psImageEllipseContour (EllipseAxes axes, double xc, double yc, psImage *image);
psF64 p_psImageGetElementF64(psImage *a, int i, int j);

psF32 pmModelFunc_SGAUSS(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  = PS_MAX((0.5*PS_SQR(px) + 0.5*PS_SQR(py) + PAR[6]*X*Y), 1e-8);
    // note that if z -> 0, dPAR[7] -> -inf
    // also z^(PAR[7]-1) -> Inf

    psF32 pr = z*PAR[8];
    psF32 pr3 = pr*pr*pr;
    psF32 p  = pow(z, PAR[7] - 1.0);
    psF32 r  = 1.0 / (1 + z*p + pr*pr3);
    psF32 f  = PAR[1]*r + PAR[0];

    if (deriv != NULL) {
        // note difference from a pure gaussian: q = params->data.F32[1]*r
        psF32 t = PAR[1]*r*r;
        psF32 q = t*(PAR[7]*p + 4*PAR[8]*pr3);

        deriv->data.F32[0] = +1.0;
        deriv->data.F32[1] = +r;
        deriv->data.F32[2] = q*(2.0*px*PAR[4] + PAR[6]*Y);
        deriv->data.F32[3] = q*(2.0*py*PAR[5] + PAR[6]*X);
        deriv->data.F32[4] = -2.0*q*px*X;
        deriv->data.F32[5] = -2.0*q*py*Y;
        deriv->data.F32[6] = -q*X*Y;
        deriv->data.F32[7] = -2*t*log(z)*z*p;
        deriv->data.F32[8] = -2*t*4*z*pr3;
    }
    return(f);
}

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

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

    beta_lim[0][0].data.F32[0] = 1000;
    beta_lim[0][0].data.F32[1] = 10000;
    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;
    beta_lim[0][0].data.F32[8] = 0.05;

    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.5;
    params_min[0][0].data.F32[8] = 0.001;

    params_max[0][0].data.F32[0] = 1e5;
    params_max[0][0].data.F32[1] = 1e6;
    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] = +3.0;
    params_max[0][0].data.F32[7] = 5.0;
    params_max[0][0].data.F32[8] = 0.5;

    return (TRUE);
}

// measure the flux for the elliptical contour
psF64 psImageEllipseContour (EllipseAxes axes, double xc, double yc, psImage *image)
{

    double t, dt, ct, st, xo, yo, value;
    int N, Nt, x, y;

    // choose dt to uniformly divide contour, with ~1 pix spacing at most
    dt = asin (1 / axes.minor);
    Nt = (int)(2*M_PI / dt) + 1;
    dt = 2*M_PI / Nt;

    ct = cos(axes.theta);
    st = sin(axes.theta);
    xo = xc - image->col0;
    yo = yc - image->row0;

    psVector *contour = psVectorAlloc (Nt, PS_TYPE_F32);
    for (t = 0, N = 0; (t < 2*M_PI) && (N < Nt); t += dt) {
        x = ct*axes.major*cos(t) + st*axes.minor*sin(t) + xo;
        y = ct*axes.minor*sin(t) + st*axes.major*cos(t) + yo;
        value = p_psImageGetElementF64(image, x, y);
        if (isfinite(value)) {
            contour->data.F32[N] = value;
            N++;
        }
    }
    contour->n = N;
    // accept every pixel: double counting is not so problematic here...

    psStats *stats = psStatsAlloc (PS_STAT_SAMPLE_MEDIAN);
    psVectorStats (stats, contour, NULL, NULL, 0);
    value = stats->sampleMedian;

    psFree (stats);
    psFree (contour);

    return (value);
}

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

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

    EllipseAxes axes;
    EllipseShape shape;
    EllipseMoments moments;

    moments.x2 = PS_SQR(sMoments->Sx);
    moments.y2 = PS_SQR(sMoments->Sy);
    moments.xy = sMoments->Sxy;

    // solve the math to go from Moments To Shape
    axes = EllipseMomentsToAxes(moments);
    shape = EllipseAxesToShape(axes);

    params[0] = sMoments->Sky;
    params[1] = sMoments->Peak - sMoments->Sky;
    params[2] = peak->x;
    params[3] = peak->y;
    params[4] = 1.0 / shape.sx;
    params[5] = 1.0 / shape.sy;
    params[6] = shape.sxy;

    # if (0)

        f1 = psImageEllipseContour (axes, peak->x, peak->y, source->pixels);
    axes.major *= 2.0;
    axes.minor *= 2.0;
    f2 = psImageEllipseContour (axes, peak->x, peak->y, source->pixels);

    if (f1 > f2) {
        params[7] = PS_MIN (3.0, PS_MAX (0.5, log(2.0*(f1/f2) - 1.0) / log(2.0)));
    } else {
        params[7] = 0.5;
    }
    # endif

    params[7] = 1.8;
    params[8] = 0.1;


    return(true);
}

// XXX EAM : test version using flux contours to guess slope
bool pmModelGuess_SGAUSS_HARD (pmModel *model, pmSource *source)
{

    pmMoments *sMoments = source->moments;
    pmPeak    *peak     = source->peak;
    psF32     *params   = model->params->data.F32;
    float f1, f2;

    EllipseAxes axes;
    EllipseShape shape;
    EllipseMoments moments;

    moments.x2 = PS_SQR(sMoments->Sx);
    moments.y2 = PS_SQR(sMoments->Sy);
    moments.xy = sMoments->Sxy;

    // solve the math to go from Moments To Shape
    axes = EllipseMomentsToAxes(moments);
    shape = EllipseAxesToShape(axes);

    params[0] = sMoments->Sky;
    params[1] = sMoments->Peak - sMoments->Sky;
    params[2] = peak->x;
    params[3] = peak->y;
    params[4] = 1.0 / shape.sx;
    params[5] = 1.0 / shape.sy;
    params[6] = shape.sxy;

    f1 = psImageEllipseContour (axes, peak->x, peak->y, source->pixels);
    axes.major *= 2.0;
    axes.minor *= 2.0;
    f2 = psImageEllipseContour (axes, peak->x, peak->y, source->pixels);

    if (f1 > f2) {
        params[7] = PS_MIN (3.0, PS_MAX (0.5, log(2.0*(f1/f2) - 1.0) / log(2.0)));
    } else {
        params[7] = 0.5;
    }
    params[8] = 0.1;

    return(true);
}

psF64 pmModelFlux_SGAUSS(const psVector *params)
{
    float f, norm, z;

    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)
    norm = 0.0;
    for (z = 0.005; z < 50; z += 0.01) {
        psF32 pr = PAR[8]*z;
        f = 1.0 / (1 + pow(z, PAR[7]) + SQ(SQ(pr)));
        norm += f;
    }
    norm *= 0.01;

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

    return(Flux);
}

// XXX need to define the radius along the major axis
// define this function so it never returns Inf or NaN
// return the radius which yields the requested flux
psF64 pmModelRadius_SGAUSS  (const psVector *params, psF64 flux)
{
    psF64 r, z, pr, f;
    psF32 *PAR = params->data.F32;

    EllipseAxes axes;
    EllipseShape shape;

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

    // convert Sx,Sy,Sxy to major/minor axes
    shape.sx = 1.0 / PAR[4];
    shape.sy = 1.0 / PAR[5];
    shape.sxy = PAR[6];

    axes = EllipseShapeToAxes (shape);
    psF64 dr = 1.0 / axes.major;
    psF64 limit = flux / PAR[1];

    // XXX : we can do this faster with an intelligent starting choice
    for (r = 0.0; r < 20.0; r += dr) {
        z = SQ(r);
        pr = PAR[8]*z;
        f = 1.0 / (1 + pow(z, PAR[7]) + SQ(SQ(pr)));
        if (f < limit)
            break;
    }
    psF64 radius = 2.0 * axes.major * sqrt (z);
    if (isnan(radius)) {
        fprintf (stderr, "error in code\n");
    }
    return (radius);
}

bool pmModelFromPSF_SGAUSS (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 < 9; 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_SGAUSS (pmModel *model)
{

    psF32 dP;
    bool  status;
    EllipseAxes axes;
    EllipseShape shape;

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

    shape.sx = 1.0 / PAR[4];
    shape.sy = 1.0 / PAR[5];
    shape.sxy = PAR[6];

    axes = EllipseShapeToAxes (shape);

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

    status = true;
    status &= (dP < 0.5);
    status &= (PAR[1] > 0);
    status &= ((dPAR[1]/PAR[1]) < 0.5);
    status &= (fabs(PAR[8]) < 0.5);
    status &= (dPAR[8] < 0.1);
    status &= (axes.major > 1.41);
    status &= (axes.minor > 1.41);
    status &= ((axes.major / axes.minor) < 5.0);
    status &= (PAR[7] > 0.5);

    if (status)
        return true;
    return false;
}
