/******************************************************************************
 * this file defines the SERSIC source shape model.  Note that these model functions are loaded
 * by pmModelGroup.c using 'include', and thus need no 'include' statements of their own.  The
 * models use a psVector to represent the set of parameters, with the sequence used to specify
 * the meaning of the parameter.  The meaning of the parameters may thus vary depending on the
 * specifics of the model.  All models which are used a PSF representations share a few
 * parameters, for which # define names are listed in pmModel.h:
 
   f = exp(-z^n)
 
   * PM_PAR_SKY 0   - local sky : note that this is unused and may be dropped in the future
   * PM_PAR_I0 1    - central intensity
   * PM_PAR_XPOS 2  - X center of object
   * PM_PAR_YPOS 3  - Y center of object
   * PM_PAR_SXX 4   - X^2 term of elliptical contour (sqrt(2) / SigmaX)
   * PM_PAR_SYY 5   - Y^2 term of elliptical contour (sqrt(2) / SigmaY)
   * PM_PAR_SXY 6   - X*Y term of elliptical contour
   * PM_PAR_7   7   - normalized sersic parameter

   note that a standard sersic model uses exp(-K*(z^(1/n) - 1).  the additional elements (K,
   the -1 offset) are absorbed in this model by the normalization, the exponenet, and the
   radial scale.  We fit the elements in this form, then re-normalize them on output.
   *****************************************************************************/

# define PM_MODEL_FUNC       	  pmModelFunc_SERSIC
# define PM_MODEL_FLUX       	  pmModelFlux_SERSIC
# define PM_MODEL_GUESS      	  pmModelGuess_SERSIC
# define PM_MODEL_LIMITS     	  pmModelLimits_SERSIC
# define PM_MODEL_RADIUS     	  pmModelRadius_SERSIC
# define PM_MODEL_FROM_PSF   	  pmModelFromPSF_SERSIC
# define PM_MODEL_PARAMS_FROM_PSF pmModelParamsFromPSF_SERSIC
# define PM_MODEL_FIT_STATUS      pmModelFitStatus_SERSIC

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

    psF32 X  = pixcoord->data.F32[0] - PAR[PM_PAR_XPOS];
    psF32 Y  = pixcoord->data.F32[1] - PAR[PM_PAR_YPOS];
    psF32 px = X / PAR[PM_PAR_SXX];
    psF32 py = Y / PAR[PM_PAR_SYY];
    psF32 z  = PS_SQR(px) + PS_SQR(py) + PAR[PM_PAR_SXY]*X*Y;

    // XXX if the elliptical contour is defined in valid way, this step should not be required.
    // other models (like PGAUSS) don't use fractional powers, and thus do not have NaN values
    // for negative values of z
    // XXX use an assert here to force the elliptical parameters to be correctly determined
    // if (z < 0) z = 0;
    assert (z >= 0);

    psF32 f2 = pow(z,PAR[PM_PAR_7]);
    psF32 f1 = exp(-f2);
    psF32 z0 = PAR[PM_PAR_I0]*f1;
    psF32 f0 = PAR[PM_PAR_SKY] + z0;

    assert (isfinite(f2));
    assert (isfinite(f1));
    assert (isfinite(z0));
    assert (isfinite(f0));

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

	// gradient is infinite for z = 0; saturate at z = 0.01
	psF32 z1 = (z < 0.01) ? z0*PAR[PM_PAR_7]*pow(0.01,PAR[PM_PAR_7] - 1.0) : z0*PAR[PM_PAR_7]*pow(z,PAR[PM_PAR_7] - 1.0);

        dPAR[PM_PAR_SKY]  = +1.0;
        dPAR[PM_PAR_I0]   = +f1;
        dPAR[PM_PAR_7]    = (z == 0.0) ? 0.0 : -z0*f2*log(z);

	assert (isfinite(z1));
	assert (isfinite(dPAR[PM_PAR_7]));

        dPAR[PM_PAR_XPOS] = +1.0*z1*(2.0*px/PAR[PM_PAR_SXX] + Y*PAR[PM_PAR_SXY]);
        dPAR[PM_PAR_YPOS] = +1.0*z1*(2.0*py/PAR[PM_PAR_SYY] + X*PAR[PM_PAR_SXY]);
        dPAR[PM_PAR_SXX]  = +2.0*z1*px*px/PAR[PM_PAR_SXX];
        dPAR[PM_PAR_SYY]  = +2.0*z1*py*py/PAR[PM_PAR_SYY];
        dPAR[PM_PAR_SXY]  = -1.0*z1*X*Y;
        dPAR[PM_PAR_SXY]  = -1.0*z1*X*Y;
    }
    return (f0);
}

// define the parameter limits
// AR_MAX is the maximum allowed axis ratio
// AR_RATIO is ((1-R)/(1+R))^2 where R = AR_MAX^(-2)
# define AR_MAX 20.0
# define AR_RATIO 0.99
bool PM_MODEL_LIMITS (psMinConstraintMode mode, int nParam, float *params, float *beta)
{
    float beta_lim = 0, params_min = 0, params_max = 0;
    float f1 = 0, f2 = 0, q1 = 0, q2 = 0;

    // we need to calculate the limits for SXY specially
    if (nParam == PM_PAR_SXY) {
        f1 = 1.0 / PS_SQR(params[PM_PAR_SYY]) + 1.0 / PS_SQR(params[PM_PAR_SXX]);
        f2 = 1.0 / PS_SQR(params[PM_PAR_SYY]) - 1.0 / PS_SQR(params[PM_PAR_SXX]);
        q1 = PS_SQR(f1)*AR_RATIO - PS_SQR(f2);
	q1 = (q1 < 0.0) ? 0.0 : q1;
	// if q1 < 0.0, f2 ~ f1, we have a very large axis ratio near 45deg..  Saturate at that
	// angle and let f2,f1 fight it out
        q2  = 0.5*sqrt (q1);
    }

    switch (mode) {
    case PS_MINIMIZE_BETA_LIMIT:
        switch (nParam) {
        case PM_PAR_SKY:
            beta_lim = 1000;
            break;
        case PM_PAR_I0:
            beta_lim = 3e6;
            break;
        case PM_PAR_XPOS:
            beta_lim = 5;
            break;
        case PM_PAR_YPOS:
            beta_lim = 5;
            break;
        case PM_PAR_SXX:
            beta_lim = 1.0;
            break;
        case PM_PAR_SYY:
            beta_lim = 1.0;
            break;
        case PM_PAR_SXY:
            beta_lim =  0.5*q2;
            break;
        case PM_PAR_7:
            beta_lim = 2.0;
            break;
        default:
            psAbort("invalid parameter %d for beta test", nParam);
        }
        if (fabs(beta[nParam]) > fabs(beta_lim)) {
            beta[nParam] = (beta[nParam] > 0) ? fabs(beta_lim) : -fabs(beta_lim);
	    psTrace ("psModules.objects", 5, "|beta[nParam==%d]| > |beta_lim|; %g v. %g",
		     nParam, beta[nParam], beta_lim);
            return false;
        }
        return true;
    case PS_MINIMIZE_PARAM_MIN:
        switch (nParam) {
        case PM_PAR_SKY:
            params_min = -1000;
            break;
        case PM_PAR_I0:
            params_min =     0;
            break;
        case PM_PAR_XPOS:
            params_min =  -100;
            break;
        case PM_PAR_YPOS:
            params_min =  -100;
            break;
        case PM_PAR_SXX:
            params_min =   0.25;
            break;
        case PM_PAR_SYY:
            params_min =   0.25;
            break;
        case PM_PAR_SXY:
            params_min =  -q2;
            break;
        case PM_PAR_7:
            params_min =   0.1;
            break;
        default:
            psAbort("invalid parameter %d for param min test", nParam);
        }
        if (params[nParam] < params_min) {
            params[nParam] = params_min;
	    psTrace ("psModules.objects", 5, "params[nParam==%d] < params_min; %g v. %g",
		     nParam, params[nParam], params_min);
            return false;
        }
        return true;
    case PS_MINIMIZE_PARAM_MAX:
        switch (nParam) {
        case PM_PAR_SKY:
            params_max =   1e5;
            break;
        case PM_PAR_I0:
            params_max =   1e8;
            break;
        case PM_PAR_XPOS:
            params_max =   1e4;
            break;
        case PM_PAR_YPOS:
            params_max =   1e4;
            break;
        case PM_PAR_SXX:
            params_max =   100;
            break;
        case PM_PAR_SYY:
            params_max =   100;
            break;
        case PM_PAR_SXY:
            params_max =  +q2;
            break;
        case PM_PAR_7:
            params_max =   4.0;
            break;
        default:
            psAbort("invalid parameter %d for param max test", nParam);
        }
        if (params[nParam] > params_max) {
            params[nParam] = params_max;
	    psTrace ("psModules.objects", 5, "params[nParam==%d] > params_max; %g v. %g",
		     nParam, params[nParam], params_max);
            return false;
        }
        return true;
    default:
        psAbort("invalid choice for limits");
    }
    psAbort("should not reach here");
    return false;
}


// make an initial guess for parameters
bool PM_MODEL_GUESS (pmModel *model, pmSource *source)
{
    pmMoments *moments = source->moments;
    pmPeak    *peak    = source->peak;
    psF32     *PAR  = model->params->data.F32;

    psEllipseMoments emoments;
    emoments.x2 = moments->Sx;
    emoments.y2 = moments->Sy;
    emoments.xy = moments->Sxy;

    // force the axis ratio to be < 20.0
    psEllipseAxes axes = psEllipseMomentsToAxes (emoments, 20.0);

    if (!isfinite(axes.major)) return false;
    if (!isfinite(axes.minor)) return false;
    if (!isfinite(axes.theta)) return false;

    psEllipseShape shape = psEllipseAxesToShape (axes);

    if (!isfinite(shape.sx))  return false;
    if (!isfinite(shape.sy))  return false;
    if (!isfinite(shape.sxy)) return false;

    PAR[PM_PAR_SKY]  = moments->Sky;
    PAR[PM_PAR_I0]   = moments->Peak - moments->Sky;
    PAR[PM_PAR_XPOS] = peak->x;
    PAR[PM_PAR_YPOS] = peak->y;
    PAR[PM_PAR_SXX]  = PS_MAX(0.5, M_SQRT2*shape.sx);
    PAR[PM_PAR_SYY]  = PS_MAX(0.5, M_SQRT2*shape.sy);
    PAR[PM_PAR_SXY]  = shape.sxy;
    PAR[PM_PAR_7]    = 0.5;

    return(true);
}

psF64 PM_MODEL_FLUX (const psVector *params)
{
    float z, norm;
    psEllipseShape shape;

    psF32 *PAR = params->data.F32;

    shape.sx  = PAR[PM_PAR_SXX] / M_SQRT2;
    shape.sy  = PAR[PM_PAR_SYY] / M_SQRT2;
    shape.sxy = PAR[PM_PAR_SXY];

    // Area is equivalent to 2 pi sigma^2
    psEllipseAxes axes = psEllipseShapeToAxes (shape, 20.0);
    psF64 Area = 2.0 * M_PI * axes.major * axes.minor;

    // the area needs to be multiplied by the integral of f(z)
    norm = 0.0;

    # define DZ 0.25

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

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

    return(Flux);
}

// define this function so it never returns Inf or NaN
// return the radius which yields the requested flux
psF64 PM_MODEL_RADIUS (const psVector *params, psF64 flux)
{
    psEllipseShape shape;

    psF32 *PAR = params->data.F32;

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

    shape.sx  = PAR[PM_PAR_SXX] / M_SQRT2;
    shape.sy  = PAR[PM_PAR_SYY] / M_SQRT2;
    shape.sxy = PAR[PM_PAR_SXY];

    psEllipseAxes axes = psEllipseShapeToAxes (shape, 20.0);
    psF64 sigma = axes.major;

    psF64 limit = flux / PAR[PM_PAR_I0];

    psF64 z = pow (-log(limit), (1.0 / PAR[PM_PAR_7]));

    psF64 radius = sigma * sqrt (2.0 * z);

    if (isnan(radius))
        psAbort("error in code: radius is NaN");

    return (radius);
}

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

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

    // we require these two parameters to exist
    assert (psf->params->n > PM_PAR_YPOS);
    assert (psf->params->n > PM_PAR_XPOS);

    for (int i = 0; i < psf->params->n; i++) {
        if (psf->params->data[i] == NULL) {
            out[i] = in[i];
        } else {
            psPolynomial2D *poly = psf->params->data[i];
            out[i] = psPolynomial2DEval(poly, in[PM_PAR_XPOS], in[PM_PAR_YPOS]);
        }
    }

    // the 2D PSF model fits polarization terms (E0,E1,E2)
    // convert to shape terms (SXX,SYY,SXY)
    if (!pmPSF_FitToModel (out, 0.1)) {
	psError(PM_ERR_PSF, false, "Failed to fit object at (r,c) = (%.1f,%.1f)",
		in[PM_PAR_YPOS], in[PM_PAR_XPOS]);
	return false;
    }

    // apply the model limits here: this truncates excessive extrapolation
    // XXX do we need to do this still?  should we put in asserts to test?
    for (int i = 0; i < psf->params->n; i++) {
        // apply the limits to all components or just the psf-model parameters?
        if (psf->params->data[i] == NULL)
            continue;

	bool status = true;
        status &= PM_MODEL_LIMITS(PS_MINIMIZE_PARAM_MIN, i, out, NULL);
	status &= PM_MODEL_LIMITS(PS_MINIMIZE_PARAM_MAX, i, out, NULL);
	if (!status) {
	    psTrace ("psModules.objects", 5, "Hitting parameter limits at (r,c) = (%.1f, %.1f)",
		     in[PM_PAR_XPOS], in[PM_PAR_YPOS]);
	    modelPSF->flags |= PM_MODEL_STATUS_LIMITS;
	}
    }

    return true;
}

// construct the PSF model from the FLT model and the psf
// XXX is this sufficiently general do be a global function, not a pmModelClass function?
bool PM_MODEL_PARAMS_FROM_PSF (pmModel *model, pmPSF *psf, float Xo, float Yo, float Io)
{
    psF32 *PAR = model->params->data.F32;

    // we require these two parameters to exist
    assert (psf->params->n > PM_PAR_YPOS);
    assert (psf->params->n > PM_PAR_XPOS);

    PAR[PM_PAR_SKY]  = 0.0;
    PAR[PM_PAR_I0]   = Io;
    PAR[PM_PAR_XPOS] = Xo;
    PAR[PM_PAR_YPOS] = Yo;
    
    // supply the model-fitted parameters, or copy from the input
    for (int i = 0; i < psf->params->n; i++) {
	if (i == PM_PAR_SKY) continue;
	if (i == PM_PAR_I0) continue;
	if (i == PM_PAR_XPOS) continue;
	if (i == PM_PAR_YPOS) continue;
	psPolynomial2D *poly = psf->params->data[i];
	assert (poly);
	PAR[i] = psPolynomial2DEval(poly, Xo, Yo);
    }

    // the 2D PSF model fits polarization terms (E0,E1,E2)
    // convert to shape terms (SXX,SYY,SXY)
    // XXX user-defined value for limit?
    if (!pmPSF_FitToModel (PAR, 0.1)) {
	psError(PM_ERR_PSF, false, "Failed to fit object at (r,c) = (%.1f,%.1f)", Xo, Yo);
	return false;
    }

    // apply the model limits here: this truncates excessive extrapolation
    // XXX do we need to do this still?  should we put in asserts to test?
    for (int i = 0; i < psf->params->n; i++) {
        // apply the limits to all components or just the psf-model parameters?
        if (psf->params->data[i] == NULL)
            continue;

	bool status = true;
        status &= PM_MODEL_LIMITS (PS_MINIMIZE_PARAM_MIN, i, PAR, NULL);
        status &= PM_MODEL_LIMITS (PS_MINIMIZE_PARAM_MAX, i, PAR, NULL);
	if (!status) {
	    psTrace ("psModules.objects", 5, "Hitting parameter limits at (r,c) = (%.1f, %.1f)", Xo, Yo);
	    model->flags |= PM_MODEL_STATUS_LIMITS;
	}
    }
    return(true);
}

bool PM_MODEL_FIT_STATUS (pmModel *model)
{

    psF32 dP;
    bool  status;

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

    dP = 0;
    dP += PS_SQR(dPAR[PM_PAR_SXX] / PAR[PM_PAR_SXX]);
    dP += PS_SQR(dPAR[PM_PAR_SYY] / PAR[PM_PAR_SYY]);
    dP = sqrt (dP);

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

    return status;
}

# undef PM_MODEL_FUNC
# undef PM_MODEL_FLUX
# undef PM_MODEL_GUESS
# undef PM_MODEL_LIMITS
# undef PM_MODEL_RADIUS
# undef PM_MODEL_FROM_PSF
# undef PM_MODEL_PARAMS_FROM_PSF
# undef PM_MODEL_FIT_STATUS
