# include "imfit.h"

int imfit (int argc, char **argv) {

  int i, j, N, Npts, Save, VERBOSE;
  int sx, sy, nx, ny, Nx, Ny;
  float chisq, ochisq, dchisq, Gain, RDnoise, SatThreshold;
  opihi_flt *x, *y, *z, *dz;
  float *V;
  Buffer *buf;

  Save = FALSE;
  if ((N = get_argument (argc, argv, "-save"))) {
    remove_argument (N, &argc, argv);
    Save = TRUE;
  }

  // int ShapeVariation = FALSE;
  // if ((N = get_argument (argc, argv, "-shapes"))) {
  //   remove_argument (N, &argc, argv);
  //   ShapeVariation = TRUE;
  // }

  SatThreshold = 0xffff;
  if ((N = get_argument (argc, argv, "-sat"))) {
    remove_argument (N, &argc, argv);
    SatThreshold = atof (argv[N]);
    remove_argument (N, &argc, argv);
  }

  /* Gain in e/DN */
  Gain = 1.0;
  if ((N = get_argument (argc, argv, "-gain"))) {
    remove_argument (N, &argc, argv);
    Gain = atof(argv[N]);
    remove_argument (N, &argc, argv);
  }

  /* RD noise in DN */
  RDnoise = 0.0;
  if ((N = get_argument (argc, argv, "-rdnoise"))) {
    remove_argument (N, &argc, argv);
    RDnoise = atof(argv[N]);
    remove_argument (N, &argc, argv);
  }

  VERBOSE = FALSE;
  if ((N = get_argument (argc, argv, "-v"))) {
    remove_argument (N, &argc, argv);
    VERBOSE = TRUE;
  }

  /* set fitting function */
  fgauss_setup ("fgauss");
  if ((N = get_argument (argc, argv, "-func"))) {
    fitfunc = NULL;
    remove_argument (N, &argc, argv);
    fgauss_setup (argv[N]);
    pgauss_setup (argv[N]);
    pgauss_psf_setup (argv[N]);
    sgauss_setup (argv[N]);
    sgauss_psf_setup (argv[N]);
    qgauss_setup (argv[N]);
    qgauss_psf_setup (argv[N]);
    qfgauss_setup (argv[N]);
    qrgauss_setup (argv[N]);
    if (fitfunc == NULL) {
      gprint (GP_ERR, "unknown function %s\n", argv[N]);
      return (FALSE);
    }
    remove_argument (N, &argc, argv);
  }

  if (argc != 6) {
    gprint (GP_ERR, "USAGE: imfit <buffer> sx sy nx ny\n");
    return (FALSE);
  }

  /* non-optional arguments */
  if ((buf = SelectBuffer (argv[1], OLDBUFFER, TRUE)) == NULL) return (FALSE);
  sx = atof (argv[2]);
  sy = atof (argv[3]);
  nx = atof (argv[4]);
  ny = atof (argv[5]);
  Nx = buf[0].matrix.Naxis[0];
  Ny = buf[0].matrix.Naxis[1];

  /* check if region is valid */
  if (sx + 0.5*nx < 0) goto range;
  if (sy + 0.5*ny < 0) goto range;
  if (sx + 0.5*nx >= Nx) goto range;
  if (sy + 0.5*ny >= Ny) goto range;

  /* convert array z[x,y] to x[i], y[i], z[i] */
  N = 0;
  Npts = nx*ny;
  ALLOCATE (x,  opihi_flt, 2*Npts);
  ALLOCATE (y,  opihi_flt, 2*Npts);
  ALLOCATE (z,  opihi_flt, 2*Npts);
  ALLOCATE (dz, opihi_flt, 2*Npts);
  for (j = 0; j < ny; j++) {
    if (j + sy < 0) continue;
    if (j + sy >= Ny) continue;
    V = (float *)(buf[0].matrix.buffer) + (j+sy)*buf[0].matrix.Naxis[0] + sx; 
    for (i = 0; i < nx; i++) {
      if (i + sx < 0) continue;
      if (i + sx >= Nx) continue;
      if (*V > SatThreshold) goto next;
      dz[N] = (SQ(RDnoise) + *V/Gain);
      if (dz[N] <= 0) goto next;
      dz[N] = 1.0 / dz[N];
      x[N] = i + sx;
      y[N] = j + sy;
      z[N] = *V;
      N++;
    next:
      V++;
    }
  }
  Npts = N;

  /* run fit routine */
  ochisq = mrq2dinit (x, y, z, dz, Npts, par, Npar, fitfunc, VERBOSE);
  dchisq = ochisq;
  chisq  = ochisq;
  for (i = 0; (i < 25) && ((dchisq <= 0.0) || (dchisq > 0.01*(Npts - Npar))); i++) {
    chisq = mrq2dmin (x, y, z, dz, Npts, par, Npar, fitfunc, VERBOSE);
    dchisq = ochisq - chisq;
    ochisq = chisq;
  }  
  set_int_variable ("Niter",  i);

  /** create output image (keep in sky) **/
  if (Save) {
    Buffer *out;
    float *Vi, *Vo, vr, vf;

    if ((out = SelectBuffer ("out",   ANYBUFFER, TRUE)) == NULL) return (FALSE);
    free (out[0].header.buffer);
    free (out[0].matrix.buffer);

    strcpy (out[0].file, "(empty)");
    if (!CreateBuffer (out, 2*nx, 2*ny, -32, 0.0, 1.0)) return FALSE;

    /* four panels: 1) raw image. 2) fit  3) raw - fit   4) ?? */
    Vi = (float *)buf[0].matrix.buffer;
    Vo = (float *)out[0].matrix.buffer;
    for (j = 0; j < ny; j++) {
      for (i = 0; i < nx; i++) {
	vf = fitfunc ((float)(i+sx), (float)(j+sy), par, Npar, NULL);
	vr = Vi[(i+sx)+(j+sy)*Nx];
	Vo[(i   )+(j   )*2*nx] = vr;
	Vo[(i+nx)+(j   )*2*nx] = vf;
	Vo[(i   )+(j+ny)*2*nx] = vr - vf + *sky;
	Vo[(i+nx)+(j+ny)*2*nx] = fabs(vr-vf) + *sky;
      }
    }
  }

  /* save parameters to opihi variables */
  imfit_cleanup ();

  set_variable ("ChiSq", chisq/(Npts - Npar));

  if (VERBOSE) {
    for (i = 0; i < Npar; i++) {
      gprint (GP_ERR, "%g ", par[i]);
    }
    gprint (GP_ERR, "\n");
  }

  free (x);
  free (y);
  free (z);
  free (dz);
  free (par);
  free (fpar);

  mrq2dfree (Npar);
  return (TRUE);

range:
  gprint (GP_ERR, "region out of range\n");
  return (FALSE);
}

