
/******************************************************************************
    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] = length;
    params->data.F32[8] = theta;
*****************************************************************************/

//# include "psEllipse.h"

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

    psF32 length = PAR[7];
    psF32 theta = PAR[8];

    psF32 x0 = PAR[2];  //streak center
    psF32 y0 = PAR[3];  //streak center

    psF32 sx = PAR[4];  
    psF32 sy = PAR[5];  
    psF32 sxy = PAR[6]; 

    psF32 sinT=sin(theta);
    psF32 cosT=cos(theta);

    //    printf("Trying object at %4.1f,%4.1f with length %3.1f and angle %1.3f\r", x0, y0, length, theta);

    psF32 X  = x->data.F32[0] - x0;
    psF32 Y  = x->data.F32[1] - y0;

    psF32 xs = X*cosT + Y*sinT;
    psF32 ys = -1.0*X*sinT + Y*cosT;

    //initialize variables to be changed below
    psF32 x1 = 0;
    psF32 y1 = 0;
    psF32 XX = 0;
    psF32 YY = 0;
    psF32 px = 0;
    psF32 py = 0;
    psF32 z = 0;
    psF32 t = 0;
    psF32 r = 0;
    psF32 f = 0;
    //    psF32 Rx = 0;
    //    psF32 Ry = 0;
    //    psF32 Rxy = 0;


    if (xs < length/(-2.0)) {
      x1 = (xs+length/2.0)*cosT - ys*sinT; //Endpoint1
      y1 = (xs+length/2.0)*sinT + ys*cosT; //Endpoint1
      //      XX  = x->data.F32[0] - x1;
      //      YY  = x->data.F32[1] - y1;
      XX=x1;
      YY=y1;
      px = sx*XX;
      py = sy*YY;
      z  = 0.5*PS_SQR(px) + 0.5*PS_SQR(py) + sxy*XX*YY;
      t  = 1 + z + z*z/2.0;
      r  = 1.0 / (t + z*z*z/6.0); /* exp (-Z) */
      f  = PAR[1]*r + PAR[0];
    } else if (xs > length/2.0){
      x1 = (xs-length/2.0)*cosT - ys*sinT; //Endpoint2
      y1 = (xs-length/2.0)*sinT + ys*cosT; //Endpoint2
      //      XX  = x->data.F32[0] - x1;
      //      YY  = x->data.F32[1] - y1;
      XX=x1;
      YY=y1;
      px = sx*XX;
      py = sy*YY;
      z  = 0.5*PS_SQR(px) + 0.5*PS_SQR(py) + sxy*XX*YY;
      t  = 1 + z + z*z/2.0;
      r  = 1.0 / (t + z*z*z/6.0); /* exp (-Z) */
      f  = PAR[1]*r + PAR[0];
    } else {
      x1 = -ys*sinT;
      y1 = ys*cosT; 
      XX=x1;
      YY=y1;
      px = sx*XX;
      py = sy*YY;
      z  = 0.5*PS_SQR(px) + 0.5*PS_SQR(py) + sxy*XX*YY;
      t  = 1 + z + z*z/2.0;
      r  = 1.0 / (t + z*z*z/6.0); /* exp (-Z) */
      f  = PAR[1]*r + PAR[0];
    }

    //ok...so this is df/dPAR[X]
    if (deriv != NULL) {
        // note difference from a pure gaussian: q = PAR[1]*r
        psF32 q = PAR[1]*r*r*t;
	//stable
        deriv->data.F32[0] = +1.0;
        deriv->data.F32[1] = +r;

	//initialize variables definied in the if statements
	//	psF32 XXX=0;
	//	psF32 YYY=0;

	  
	//variable over piecewise func
	if (xs < length/(-2.0)) {

	  deriv->data.F32[2] = q*(PS_SQR(sx)*x1 + sxy*y1);
	  deriv->data.F32[3] = q*(PS_SQR(sy)*y1 + sxy*x1);
	  deriv->data.F32[4] = -q*sx*PS_SQR(x1);
	  deriv->data.F32[5] = -q*sy*PS_SQR(y1);
	  deriv->data.F32[6] = -q*x1*y1;
	  //multiply be 0.9 to make it guess slightly bigger lengths since it seems to underestimate them
	  deriv->data.F32[7] = -q*(PS_SQR(sx)*x1*cosT/2.0 + PS_SQR(sy)*y1*sinT/2.0 + sxy*x1*sinT/2.0 + sxy*y1*cosT/2.0);
	  deriv->data.F32[8] = -q*(-1*PS_SQR(sx)*x1*length/2.0*sinT + PS_SQR(sy)*y1*length/2.0*cosT + sxy*x1*length/2.0*cosT - sxy*y1*length/2.0*sinT);

	} else if (xs > length/2.0){

	  deriv->data.F32[2] = q*(PS_SQR(sx)*x1 + sxy*y1);
	  deriv->data.F32[3] = q*(PS_SQR(sy)*y1 + sxy*x1);
	  deriv->data.F32[4] = -q*sx*PS_SQR(x1);
	  deriv->data.F32[5] = -q*sy*PS_SQR(y1);
	  deriv->data.F32[6] = -q*x1*y1;
	  //multiply be 0.9 to make it guess slightly bigger lengths since it seems to underestimate them
	  deriv->data.F32[7] = q*(PS_SQR(sx)*x1*cosT/2.0 + PS_SQR(sy)*y1*sinT/2.0 + sxy*x1*sinT/2.0 + sxy*y1*cosT/2.0);
	  deriv->data.F32[8] = -q*(PS_SQR(sx)*x1*length/2.0*sinT - PS_SQR(sy)*y1*length/2.0*cosT - sxy*x1*length/2.0*cosT + sxy*y1*length/2.0*sinT);

	} else {
	  deriv->data.F32[2] = q*(PS_SQR(sx*sinT)*x1 - PS_SQR(sy)*y1*cosT*sinT - sxy*x1*sinT*cosT + sxy*y1*PS_SQR(sinT));
	  deriv->data.F32[3] = q*(-1*PS_SQR(sx)*x1*sinT*cosT + PS_SQR(sy*cosT)*y1 + sxy*x1*PS_SQR(cosT) - sxy*y1*sinT*cosT);
	  deriv->data.F32[4] = -q*sx*PS_SQR(x1);
	  deriv->data.F32[5] = -q*sy*PS_SQR(y1);
	  deriv->data.F32[6] = -q*x1*y1;
	  deriv->data.F32[7] = 0;
	  deriv->data.F32[8] = -q*( PS_SQR(sx)*x1*(xs*sinT - ys*cosT) + PS_SQR(sy)*y1*(xs*cosT - ys*sinT) + sxy*x1*(xs*cosT - ys*sinT) + sxy*y1*(xs*sinT - ys*cosT) );

	}
    }
    return(f);
}

//fixed 
bool pmModelLimits_STRAIL (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] = 50.;
    beta_lim[0][0].data.F32[3] = 50.;
    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] = 10.; 
    beta_lim[0][0].data.F32[8] = M_PI/6; 

    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;
    params_min[0][0].data.F32[8] = -1*M_PI;

    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] = 4.0;
    params_max[0][0].data.F32[5] = 4.0;
    params_max[0][0].data.F32[6] = +5.0;
    params_max[0][0].data.F32[7] = 150;
    params_max[0][0].data.F32[8] = M_PI;

    return (TRUE);
}

//fixed
psF64 pmModelFlux_STRAIL(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]);
    psF32 Rx=2./PS_SQR(PAR[4]); 
    psF32 Ry=2./PS_SQR(PAR[5]); 
    psF32 Rxy=PAR[6];


    psF32 theta = PAR[8]; 
    psF32 sinT=sin(theta);
    psF32 cosT=cos(theta);

    psF32 Syrot = ( PS_SQR(sinT)/Rx + PS_SQR(cosT)/Ry - Rxy*sinT*cosT );  //rotated sigma y

    psF64 A4   = Syrot*PAR[7];

    psF64 Area = 2.0 * M_PI / sqrt(A1*A2 - A3) + A4;
    // Area is equivalent to 2 pi sigma^2 + rectangle

    // the area needs to be multiplied by the integral of f(z)
    norm = 0.0;
    for (z = 0.005; z < 50; z += 0.01) {
	f = 1.0 / (1 + z + z*z/2 + z*z*z/6);
	norm += f;
    }
    norm *= 0.01;
    
    psF64 Flux = params->data.F32[1] * Area * norm;

    return(Flux);
}

// define this function so it never returns Inf or NaN
// also prevent 0 returns, and just send a v small number
// return the radius which yields the requested flux

//fixed, but need to change how it is called to accomodate 2 radii
psF64 pmModelRadius_STRAIL  (const psVector *params, psF64 flux)
{
    if (flux <= 0) return (1.0);
    if (params->data.F32[1] <= 0) return (1.0);
    if (flux >= params->data.F32[1]) return (1.0);

    psF32 *PAR = params->data.F32;
    psF32 sigma  = sqrt(2.0) * hypot (1.0 / params->data.F32[4], 1.0 / params->data.F32[5]);

    psF32 theta = PAR[8]; 
    psF32 sinT=sin(theta);
    psF32 cosT=cos(theta);
    psF32 Rx=2./PS_SQR(PAR[4]);
    psF32 Ry=2./PS_SQR(PAR[5]);
    psF32 Rxy=PAR[6];
    psF32 length=PAR[7];

    psF32 Syrot = ( PS_SQR(sinT)/Rx + PS_SQR(cosT)/Ry - Rxy*sinT*cosT );  //rotated sigma y

    psF64 radius = 0;
    if (flux > 0){
      psF64 radius0 = sigma * sqrt (2.0 * log(params->data.F32[1] / flux));
      psF64 radius1 = Syrot * sqrt (2.0 * log(params->data.F32[1] / flux));

      if (radius0 > radius1) {
	radius=radius0+length/2.0;
      } else {
	radius=radius1+length/2.0;
      } 
    } else {
      radius = 1000;
    }

    if (radius < 0.01){
      radius = 0.01;
    }

    if (isnan(radius)) {
      fprintf (stderr, "error in code\n");
    }
    return (radius);
}

//fixed I think...no good way of guessing as far as I can tell
//^*^
bool pmModelGuess_STRAIL (pmModel *model, pmSource *source) {

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

    psEllipseAxes axes;
    psEllipseShape shape;
    psEllipseMoments moments;

    moments.x2 = PS_SQR(Smoments->Sx);
    moments.y2 = PS_SQR(Smoments->Sy);
    moments.xy = Smoments->Sxy;
    //sometimes these moment inputs are zero...why?

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

    params[0] = Smoments->Sky;
    params[1] = Smoments->Peak - Smoments->Sky;
    params[2] = Smoments->x;
    params[3] = Smoments->y;
    params[7] = 2 * axes.major; 
    params[8] = axes.theta;

    if (moments.x2 == 0 || moments.y2 == 0){
      params[4] = 2.0;
      params[5] = 2.0;
      params[6] = 0.0;
    } else {
      params[4] = 1.0 / shape.sx;
      params[5] = 1.0 / shape.sy;
      params[6] = shape.sxy;
    }

    //    printf("Who is NaN? momx: %4.3f  momy: %4.3f  momxy: %4.3f\n", moments.x2,moments.y2, moments.xy);

    return(true);
}

//fixed
bool pmModelFromPSF_STRAIL (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];
    out[7] = in[7];
    out[8] = in[8];

    for (int i = 4; i < 7; i++) {
      psPolynomial2D *poly = psf->params->data[i-4];
	out[i] = psPolynomial2DEval (poly, out[2], out[3]);
    }
    return(true);
}

//done I think
bool pmModelFitStatus_STRAIL (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);
    //    status &= ((dPAR[7]/PAR[7]) < 0.5);
    //    status &= ((dPAR[8]/PAR[8]) < 0.5);

    if (status) return true;
    return false;
}
