/* @(#)mosext.c	10.1 (ESO-IPG) 8/7/95 17:34:23 */

#include <tbldef.h>
#include <midas_def.h>
#include <ok.h>
#include <math.h>

#include <proto_nrutil.h>
#include <proto_mutil.h>

#define max(A,B) ((A)  > (B) ? (A) : (B))
#define min(A,B) ((A)  < (B) ? (A) : (B))
#define myabs(A) ((A)  >  0 ? (A) : (-A))

int Order, N_iter, Npix[2];
float Ron, Gain, Thres, V0;
int dunit;

#ifdef __STDC__
int horne (float[], float[], float[], float[], float[], float[], float[],
	   float[], float[], int);

void extract_objects (float[], float[], float[], float[], float[],
		      float[], float[], float[], int[],
		      int[], int);
#else
int horne ();
void extract_objects ();
#endif

main ()

{
  int iparm[5];
  int i, iav;
  int nax, kun, knul, tid, imno1, imno2, imno3;
  int ncol, nobjects, nrow, acol, arow, nsort, select;
  int npix2[2], null[3], maxslit;
  int slitlen[100], slit[100], upper[100], lower[100], object[100];
  int icol[4], inull;

  char inframe[60], table[60], outframe[60], skyframe[60];
  float *inimage, *outimage, *skyimage;
  char unit[64], inst[72], line[80];

  float moscol[4], rparm[5], rnull;
  float *y, *z;
  /* float *x; */
  float *mask, *profile, *variance;

  double start[2], step[2], start2[2], step2[2], dnull;

/*         get into Midas and get parameters             */

  SCSPRO ("mosext");

  strcpy(inst,""), strcpy(unit,"");

  SCKGETC ("IN_A", 1, 60, &iav, inframe);
  SCKGETC ("IN_B", 1, 60, &iav, skyframe);
  SCKGETC ("INPUTC", 1, 60, &iav, table);
  SCKGETC ("OUT_A", 1, 60, &iav, outframe);
  SCKRDI ("INPUTI", 1, 4, &iav, iparm, &kun, &knul);
  SCKRDR ("INPUTR", 1, 4, &iav, rparm, &kun, &knul);

  Order = iparm[0];
  N_iter = iparm[1];

  Ron = rparm[0];
  Gain = rparm[1];
  Thres = rparm[2];
  V0 = (Ron * Ron) / (Gain * Gain);

  SCTPUT ("\n ----------------------- ");
  sprintf (line, "Input image:         %s ", inframe);
  SCTPUT (line);
  sprintf (line, "sky image:           %s ", skyframe);
  SCTPUT (line);
  sprintf (line, "Input table:         %s ", table);
  SCTPUT (line);
  sprintf (line, "Output image:        %s\n ", outframe);
  SCTPUT (line);
  SCTPUT ("Input parameters: ");
  sprintf (line, "Order of fit:        %i", Order);
  SCTPUT (line);
  sprintf (line, "Number of iterations %i", N_iter);
  SCTPUT (line);
  sprintf (line, "Read-out-noise:    %8.2f", Ron);
  SCTPUT (line);
  sprintf (line, "Gain (e/ADU):      %8.2f", Gain);
  SCTPUT (line);
  sprintf (line, "Threshold (sigma): %8.2f", Thres);
  SCTPUT (line);

/*          map input files                             */

  SCIGET (inframe, D_R4_FORMAT, F_I_MODE, F_IMA_TYPE,
	  2, &nax, Npix, start, step, inst, unit, (char **)&inimage, &imno1);

  SCIGET (skyframe, D_R4_FORMAT, F_I_MODE, F_IMA_TYPE,
	  2, &nax, Npix, start, step, inst, unit, (char **)&skyimage, &imno3);

/*           read table MOS                            */

  TCMNUL (&inull, &rnull, &dnull);
  TCTOPN (table, F_I_MODE, &tid);
  TCIGET (tid, &ncol, &nrow, &nsort, &acol, &arow);
  TCCSER (tid, ":Obj_Slit", &icol[0]);
  TCCSER (tid, ":Obj_Strt", &icol[1]);
  TCCSER (tid, ":Obj_End", &icol[2]);
  nobjects = 0;
  maxslit = 0;
  for (i = 1; i <= nrow; i++)
    {
      TCSGET (tid, i, &select);
      if (select)
	{
	  TCRRDR (tid, i, 3, icol, moscol, null);
	  if (!null[0] && !null[1] && !null[2])
	    {
	      slit[nobjects] = (int) moscol[0];
	      lower[nobjects] =  (int) ((moscol[1]-start[1])/step[1]) + 1;
	      object[nobjects] =  (int) ((moscol[2]-start[1])/step[1]) + 1;
	      slitlen[nobjects] = (int) ((moscol[2] - moscol[1])/step[1]);
	      maxslit = max (slitlen[nobjects], maxslit);
	      nobjects++;
	    }
	}
    }

  npix2[0] = Npix[0];
  npix2[1] = nobjects*2;
  step2[0] = step[0];
  step2[1] = 1.0;
  start2[0] = start[0];
  start2[1] = 1.0;

  TCTCLO (tid);

/*      open output file                                */

  SCIPUT (outframe, D_R4_FORMAT, F_O_MODE, F_IMA_TYPE,
	  nax, npix2, start2, step2, inst, unit, (char **)&outimage, &imno2);

  SCDWRI (imno2, "YPOS", object, 1, nobjects, &dunit);
  SCDWRI (imno2, "SLIT", slit,   1, nobjects, &dunit);

/*      reserve storage for various arrays              */

/*  x = (float *) osmmget (Npix[0] * sizeof (float)); */
  y = (float *) osmmget ((Npix[0]+1) * sizeof (float));
  z = (float *) osmmget ((Npix[0]+1) * sizeof (float));

  mask     = (float *) osmmget (Npix[0] * maxslit     * sizeof (float));
  profile  = (float *) osmmget (Npix[0] * (maxslit+2) * sizeof (float));
  variance = (float *) osmmget (Npix[0] * maxslit     * sizeof (float));

/*      do the extraction                               */

/*  extract_objects (inimage, outimage, skyimage,
		   mask, profile, variance, x, y, z,
		   lower, slitlen, nobjects); */

  extract_objects (inimage, outimage, skyimage,
		   mask, profile, variance, y, z,
		   lower, slitlen, nobjects);

/*          epilogue ...                 */

/*  osmmfree((char *) x), osmmfree((char *) y), osmmfree((char *) z);
*/
  osmmfree((char *) mask);
  osmmfree((char *) profile), osmmfree((char *) variance);

  SCSEPI ();

}

/* #ifdef __STDC__
void 
extract_objects (float inframe[], float outframe[], float skyframe[],
		 float mask[], float profile[], float variance[], float x[],
		 float y[], float z[], int lower[],
		 int slitlen[], int nobjects)
#else
void 
extract_objects ( inframe, outframe, skyframe, mask, profile, variance, 
                  x, y, z, lower, slitlen, nobjects)
  float inframe[],outframe[],skyframe[],mask[],profile[];
  float variance[],y[],z[];
  int   lower[],slitlen[],nobjects;
#endif */

#ifdef __STDC__
void 
extract_objects (float inframe[], float outframe[], float skyframe[],
		 float mask[], float profile[], float variance[], 
		 float y[], float z[], int lower[],
		 int slitlen[], int nobjects)
#else
void 
extract_objects ( inframe, outframe, skyframe, mask, profile, variance, 
                  y, z, lower, slitlen, nobjects)
  float inframe[],outframe[],skyframe[],mask[],profile[];
  float variance[],y[],z[];
  int   lower[],slitlen[],nobjects;
#endif
{

  int nobject, k, jj, jj2, jj3;
  char line[80];

  SCTPUT ("\n ----------------------- ");
  SCTPUT (" object from to   ");

/*          loop over all objects                         */

  for (nobject = 0; nobject < nobjects; nobject++)
    {
      for (k = 0; k < Npix[0]; k++)
	{
	  jj2 = nobject * Npix[0] + k;
	  jj3 = (nobject+nobjects) * Npix[0];
	  outframe[jj2] = 0.0;
	}
      sprintf (line, " %4i   %4i %4i", nobject + 1, lower[nobject],
	       lower[nobject]+slitlen[nobject]);
      SCTPUT (line);

      jj2 = nobject * Npix[0];
      jj = lower[nobject] * Npix[0];

/*      optimum extraction for one object               */

/*      horne (&inframe[jj], &skyframe[jj], &outframe[jj2], &outframe[jj3],
	     mask, profile, variance, x, y, z, slitlen[nobject]); */
      horne (&inframe[jj], &skyframe[jj], &outframe[jj2], &outframe[jj3],
	     mask, profile, variance, y, z, slitlen[nobject]); 

    }
  SCTPUT (" ----------------------- ");

}

/* #ifdef __STDC__
int 
horne (float frame[], float sky[], float outframe[], float varframe[], 
       float mask[],float profile[], float variance[], float x[], float y[],
       float z[], int slitlen)
#else
int 
horne ( frame, sky, outframe, varframe, mask, profile, variance, 
       y, z, slitlen)
  float frame[],sky[],outframe[],varframe[],mask[],profile[],variance[],
     y[],z[];
  int   slitlen;
#endif */
#ifdef __STDC__
int 
horne (float frame[], float sky[], float outframe[], float varframe[], 
       float mask[],float profile[], float variance[], float y[],
       float z[], int slitlen)
#else
int 
horne ( frame, sky, outframe, varframe, mask, profile, variance, 
        y, z, slitlen)
  float frame[],sky[],outframe[],varframe[],mask[],profile[],variance[],
     y[],z[];
  int   slitlen;
#endif

{

  int i, j, index, index1, index2, kk;
  float ratio, diff, diff2, rmax, sum1, sum2, sum3, mp;
  double stst[2];
  stst[0] = 1.0;
  stst[1] = 1.0;


/*                 init arrays                            */

  for (j = 0; j < slitlen; j++)
    {
      for (i = 0; i < Npix[0]; i++)
	{
	  index = j * Npix[0] + i;
	  mask[index] = 1.0;
	  profile[index] = 0.0;
	  variance[index] = 0.0;
	  outframe[i] += (frame[index] - sky[index]);
	  varframe[i] = 0.0;
	}
    }

/*              return with standard extraction (equal weights)     */

  if (Order < 0)
    return (0);

/*                construct spatial profile               */

  for (i = 0; i < N_iter; i++)
    {

      for (i = 0; i < Npix[0]; i++)
	outframe[i] = max (outframe[i], 1.0);

      for (j = 0; j < slitlen; j++)
	{
	  for (i = 0; i < Npix[0]; i++)
	    {
	      index = j * Npix[0] + i;
	      y[i] = (frame[index] - sky[index]) / outframe[i];
	    }

/*   filter spatial profile                              */

	  for (i = 2; i < Npix[0] - 2; i++)
	    {
	      index = j * Npix[0] + i;
/*	      profile[index] = heap_median (5, &y[i]); */
	      profile[index] = heap_median (5, &y[i-2]); 

	    }

/*      treat edges                                   */

	  index1 = j * Npix[0] + 2;
	  index2 = (j + 1) * Npix[0];
	  for (i = 0; i < 2; i++)
	    {
	      index = j * Npix[0] + i;
	      profile[index] = profile[index1];
	    }
	  for (i = Npix[0] - 2; i < Npix[0]; i++)
	    {
	      index = j * Npix[0] + i;
	      profile[index] = profile[index2];
	    }

/*        fit a polynomial to spatial profile          */

	  index = j * Npix[0];
	  fit_poly (&profile[index], z, stst[0],
		    stst[1], Npix[0], Order);

	  for (i = 0; i < Npix[0]; i++)
	    {
	      index = j * Npix[0] + i;
	      profile[index] = z[i];
	    }
	}

/*      normalize spatial profile, enforce positivity         */

      for (i = 0; i < Npix[0]; i++)
	z[i] = 0.0;
      for (j = 0; j < slitlen; j++)
	{
	  for (i = 0; i < Npix[0]; i++)
	    {
	      index = j * Npix[0] + i;

/*               THIS HAS TO BE INVESTIGATED !!                */

	      profile[index] = max (profile[index], 0.01);
	      z[i] += profile[index];
	    }
	}

      for (j = 0; j < slitlen; j++)
	{
	  for (i = 0; i < Npix[0]; i++)
	    {
	      index = j * Npix[0] + i;
	      profile[index] = profile[index] / z[i];

	    }
	}

/*             compute  mask and variance                        */


      for (j = 0; j < slitlen; j++)
	{
	  for (i = 0; i < Npix[0]; i++)
	    {
	      index = j * Npix[0] + i;
	      variance[index] = V0 + myabs (outframe[i] *
		  	profile[index] + sky[index]) / Gain;

	    }
	}
      for (i = 0; i < Npix[0]; i++)
	{
	  rmax = 1.0;
	  kk = -1;
	  for (j = 0; j < slitlen; j++)
	    {
	      index = j * Npix[0] + i;
	      diff = frame[index] - sky[index] - outframe[i] *
		profile[index];
	      diff2 = diff * diff;
	      ratio = (diff2 / (Thres * variance[index])) *
		mask[index];
	      if (ratio > rmax)
		{
		  rmax = ratio;
		  kk = index;
		}
	    }

/*      now extract the weighted spectrum               */

	  if (kk >= 0)
	    mask[kk] = 0.0;
	  sum1 = 0.0;
	  sum2 = 0.0;
	  sum3 = 0.0;
	  for (j = 0; j < slitlen; j++)
	    {
	      index = j * Npix[0] + i;
	       mp = mask[index] * profile[index];
	      sum1 += mp * (frame[index] - sky[index]) / variance[index];
	      sum2 += mp * (profile[index] / variance[index]);
	      sum3 += mp;
	    }


	  /*
	  outframe[i] = sum1 / sum2;
	  varframe[i] = (float) sqrt( (double) (sum3/sum2));
	  */

          if (sum2 > 1.e-9) {  
	     outframe[i] = sum1 / sum2;
	     varframe[i] = (float) sqrt( (double) (sum3/sum2));
          } 
          else {
             outframe[i] = 0.;
             varframe[i] = 0.;
	       }



	}

    }

}