/*===========================================================================
  Copyright (C) 2001 European Southern Observatory (ESO)
 
  This program is free software; you can redistribute it and/or 
  modify it under the terms of the GNU General Public License as 
  published by the Free Software Foundation; either version 2 of 
  the License, or (at your option) any later version.
 
  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.
 
  You should have received a copy of the GNU General Public 
  License along with this program; if not, write to the Free 
  Software Foundation, Inc., 675 Massachusetss Ave, Cambridge, 
  MA 02139, USA.
 
  Corresponding concerning ESO-MIDAS should be addressed as follows:
  Internet e-mail: midas@eso.org
  Postal address: European Southern Observatory
  Data Management Division 
  Karl-Schwarzschild-Strasse 2
  D 85748 Garching bei Muenchen 
  GERMANY
  ===========================================================================*/


/*--------------------------------------------------------------------------*/
/**
 * @defgroup flames_Stand_Extract   Substep: Flames Standard Extraction
 *
 */
/*--------------------------------------------------------------------------*/

/*----------------------------------------------------------------------------
  Includes
  ---------------------------------------------------------------------------*/


#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif

#include <flames_midas_def.h>
/* FLAMES-UVES include files */ 
#include <flames_dgaussj.h>
#include <flames_uves.h>
#include <flames_Stand_Extract.h>
#include <flames_def_drs_par.h>
#include <flames_newmatrix.h>

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

/**@{*/

/*---------------------------------------------------------------------------
  Implementation
  ---------------------------------------------------------------------------*/


/**
   @name  flames_Std_Extract()          
   @author G. Mulas  -  ITAL_FLAMES Consortium. Ported to CPL by A. Modigliani  

   @param ScienceFrame input science frame to be extracted
   @param SingleFF     input all flat field frame base structure
   @param Order        input order traces structure
   @param ordsta       input order start
   @param ordend       input order end
   @param ix
   @param mask         input mask
   @param aa
   @param varaa
   @param xx
   @param xx2
   @param covarxx
   @param covariance
   @param fibretosolve
   @param ordertosolve
   @param numslices
   @param pfibrecentre
   @param phalfwinsize 
   @param ylow
   @param pyup 
   @param ylow 
   @param yup 
   @param arraysize

   Purpose:
   DRS function called:                              
   Pseudocode:                       

   @note
*/
flames_err 
Std_Extract(flames_frame *ScienceFrame, 
            allflats *SingleFF, 
            orderpos *Order, 
            int32_t ordsta, 
            int32_t ordend, 
            int32_t ix, 
            frame_mask **mask, 
            double **aa, 
            double **varaa, 
            double **xx, 
            double *xx2, 
            double **covarxx, 
            double **covariance, 
            int32_t *fibrestosolve, 
            int32_t *orderstosolve, 
            int32_t *numslices, 
            double ***pfibrecentre, 
            double phalfwinsize, 
            double *pylow, 
            double *pyup, 
            int32_t *ylow, 
            int32_t *yup, 
            int32_t arraysize)
{ 
  int32_t i=0, j=0, m=0, n=0, k=0, o=0, p=0, iy=0;
  double pylown=0, pyupn=0, pylowk=0, pyupk=0;
  int32_t ylown=0, yupn=0, ylowk=0, yupk=0;
  int32_t fibrelowi=0, fibrehighi=0, fibrelowj=0, fibrehighj=0;
  int32_t fibrelowm=0, fibrehighm=0, fibrelown=0, fibrehighn=0;
  int32_t fibrelowo=0, fibrehigho=0;
  int32_t fibrei=0, orderi=0, fibrej=0, orderj=0;
  int32_t ordern=0, fibren=0, orderk=0, fibrek=0, framek=0;
  int32_t ifibre=0;
  double dreduce=0;
  frame_data fractionn=0, fractionk=0;
 
  char output[70]; /* only for testing purposes */

  singleflat *myflat=0;
  double *dvecbuf1=0;
  double *dvecbuf2=0;
  double *dvecbuf3=0;
  double *dvecbuf4=0;
  double *dvecbuf5=0;
  double *dvecbuf6=0;
  frame_data *fdvecbuf1=0;
  frame_data *fdvecbuf2=0;
  frame_data *fdvecbuf3=0;
  frame_data *fdvecbuf4=0;
  frame_data *fdvecbuf5=0;
  frame_data *fdvecbuf6=0;
  frame_data *fdvecbuf7=0;
  frame_mask *fmvecbuf1=0;
  frame_mask *fmvecbuf2=0;
  frame_mask *fmvecbuf3=0;
  int32_t *lvecbuf1=0;
  int32_t *lvecbuf2=0;
  int32_t mifibreixoffsetstep=0;
  int32_t mifibreixoffset=0;
  int32_t mifibreixindex=0;
  int32_t upnlimit=0;
  int32_t ioffset=0;
  int32_t ioffset1=0;
  int32_t ioffset2=0;
  int32_t joffset=0;
  int32_t joffset1=0;
  int32_t joffset2=0;
  int32_t moffset=0;
  int32_t moffset1=0;
  int32_t moffset2=0;
  int32_t noffset=0;
  int32_t noffset1=0;
  int32_t noffset2=0;
  int32_t ooffset=0;
  int32_t ooffset1=0;
  int32_t ooffset2=0;
  int32_t poffset=0;
  int32_t poffset1=0;
  int32_t iiindex=0;
  int32_t ijindex=0;
  int32_t imindex=0;
  int32_t ioindex=0;
  int32_t jiindex=0;
  int32_t jnindex=0;
  int32_t joindex=0;
  int32_t mnindex=0;
  int32_t nkindex=0;
  int32_t npindex=0;
  int32_t opindex=0;
  int32_t pmindex=0;
  int32_t ymax=0;
  int32_t orderifibreiindex=0;
  int32_t ordernfibrenixindex=0;
  int32_t orderkfibrekixindex=0;
  int32_t iyixindex=0;



  int actvals=0;
  char drs_verbosity[10];
  int status=0;
  memset(drs_verbosity, 0, 10);
  if ((status=SCKGETC(DRS_VERBOSITY, 1, 3, &actvals, drs_verbosity))
      != 0) {
    /* the keyword seems undefined, protest... */
    return(MAREMMA);
  }

  /* Determine here the actual size of the problem to be solved and which
     are the slices to be included */

  (*numslices) = 0;

  dvecbuf6 = pfibrecentre[0][0]+ix;
  lvecbuf1 = SingleFF->lowfibrebounds[0][0]+ix;
  lvecbuf2 = SingleFF->highfibrebounds[0][0]+ix;
  fmvecbuf1 = SingleFF->goodfibres[0][0]+ix;
  fmvecbuf2 = mask[0]+ix;
  fdvecbuf1 = ScienceFrame->frame_array[0]+ix;
  fdvecbuf2 = ScienceFrame->frame_sigma[0]+ix;
  fdvecbuf5 = ScienceFrame->spectrum[ix][0];
  fdvecbuf6 = ScienceFrame->specsigma[ix][0];
  fdvecbuf7 = ScienceFrame->speccovar[ix][0];
  fmvecbuf3 = ScienceFrame->specmask[ix][0];
  mifibreixoffsetstep=SingleFF->subcols*SingleFF->maxfibres;

  for (m=ordsta-Order->firstorder; m<=ordend-Order->firstorder; m++) {
    mifibreixoffset = m*mifibreixoffsetstep;
    for (n=0; n<=(ScienceFrame->num_lit_fibres-1); n++) {
      ifibre = ScienceFrame->ind_lit_fibres[n];
      mifibreixindex = mifibreixoffset+(ifibre*SingleFF->subcols);
      /* should I try to extract this fibre at this slice? It is crucial to 
	 skip unextractible slices, since they would invariably lead to 
	 singular matrices, being ill-conditioned problems */
      if(fmvecbuf1[mifibreixindex]!=BADSLICE){
	/* it is at least half good, extract it */
	(*numslices)++;
	fibrestosolve[*numslices] = ifibre;
	orderstosolve[*numslices] = m;
      }
    }
  }

  /* if there are no good slices just free allocated memory and return right
     now */
  if ((*numslices)==0) {
    if ( strcmp(drs_verbosity,"LOW") == 0 ){
    } else {
      sprintf(output,"bad slice at %d-th column", ix);
      SCTPUT(output);
    }
    return NOERR;
  }

  /* Initialize xx, aa and covariance matrices*/

  dvecbuf5 = xx[1]+1;
  upnlimit = (*numslices)-1;
  for (n=0; n<=upnlimit; n++) dvecbuf5[n]=0;

  dvecbuf1 = aa[1];
  dvecbuf2 = varaa[1];
  dvecbuf3 = covarxx[1];
  dvecbuf4 = covariance[1];
  for (n=1; n<=(*numslices); n++) {
    noffset = n-1;
    noffset1 = noffset*arraysize;
    for (k=1; k<=(*numslices); k++) {
      nkindex = noffset1+k;
      dvecbuf1[nkindex] = 0;
      dvecbuf2[nkindex] = 0;
      dvecbuf3[nkindex] = 0;
      dvecbuf4[nkindex] = 0;
    }
  }

  /* Create matrices and vectors to be used in matrix inversion */

  ymax = ScienceFrame->subrows-1;
  for (n=1; n<=(*numslices); n++) {
    noffset = n-1;
    noffset1 = noffset*arraysize;
    ordern = orderstosolve[n];
    fibren = fibrestosolve[n];
    ordernfibrenixindex = 
      ((ordern*SingleFF->maxfibres)+fibren)*SingleFF->subcols;
    /* we will need these integration limits over and over, compute them once */
    pylow[n] = pylown = dvecbuf6[ordernfibrenixindex]-phalfwinsize;
    pyup[n] = pyupn = dvecbuf6[ordernfibrenixindex]+phalfwinsize;
    ylown = (int32_t) floor(pylown+.5);
    if (ylown<0) ylown = 0;
    ylow[n] = ylown;
    yupn = (int32_t) floor(pyupn+.5);
    if (yupn>ymax) yup[n] = yupn = ymax;
    yup[n] = yupn;
  }

  for (n=1; n<=(*numslices); n++) {
    noffset = n-1;
    noffset1 = noffset*arraysize;
    ordern = orderstosolve[n];
    fibren = fibrestosolve[n];
    ordernfibrenixindex = 
      ((ordern*SingleFF->maxfibres)+fibren)*SingleFF->subcols;
    /* reduce the y loop to the intervals to be used in the
       integration */
    pylown = pylow[n];
    pyupn = pyup[n];
    if (ylow[n]<lvecbuf1[ordernfibrenixindex]) 
      ylown = lvecbuf1[ordernfibrenixindex];
    else ylown = ylow[n];
    if (yup[n]>lvecbuf2[ordernfibrenixindex])
      yupn = lvecbuf2[ordernfibrenixindex];
    else yupn = yup[n];
    /* integrate the science frame over the interval centered around the 
       nth fibre */
    for (iy=ylown; iy<=yupn; iy++) {
      iyixindex = iy*ScienceFrame->subcols;
      /* use this pixel only if it is good in the overall mask */
      if (fmvecbuf2[iyixindex]==0) {
	/* take care of fractional pixels */
	fractionn = 1;
	if ((dreduce=pylown+.5-(double)iy)>0) fractionn -= (frame_data) dreduce;
	if ((dreduce=.5-pyupn+(double)iy)>0) fractionn -= (frame_data) dreduce;
	dvecbuf5[noffset] += fractionn*fdvecbuf1[iyixindex];      
      }
    }
    /* in standard extraction, the aa matrix is not symmetric by construction,
       therefore I must loop over all indices */
    for (k=1; k<=(*numslices); k++) {
      nkindex = noffset1+k;
      orderk = orderstosolve[k];
      fibrek = fibrestosolve[k];
      orderkfibrekixindex = 
	((orderk*SingleFF->maxfibres)+fibrek)*SingleFF->subcols;
      framek = SingleFF->fibre2frame[fibrek];
      myflat = SingleFF->flatdata+framek;
      fdvecbuf3 = myflat->data[0]+ix;
      fdvecbuf4 = myflat->sigma[0]+ix;
      /* the y loop must run only where integration interval overlaps with
	 the kth fibre */
      if (ylow[n]<lvecbuf1[orderkfibrekixindex])
	ylowk = lvecbuf1[orderkfibrekixindex];
      else ylowk = ylow[n];
      if (yup[n]>lvecbuf2[orderkfibrekixindex])
	yupk = lvecbuf2[orderkfibrekixindex];
      else yupk = yup[n];
      /* integrate the kth fibre over the interval centered around the nth 
	 fibre */
      for (iy=ylowk; iy<=yupk; iy++) {
	iyixindex = iy*ScienceFrame->subcols;
	if (fmvecbuf2[iyixindex]==0) {
	  /* take care of fractional pixels */
	  fractionn = 1;
	  if ((dreduce=pylown+.5-(double)iy)>0) 
	    fractionn -= (frame_data)dreduce;
	  if ((dreduce=.5-pyupn+(double)iy)>0) fractionn -= (frame_data)dreduce;
	  dvecbuf1[nkindex] += fractionn*fdvecbuf3[iyixindex];
	  dvecbuf2[nkindex] += fractionn*fractionn*fdvecbuf4[iyixindex];
	}
      }
      /* now find the overlap between the integration interval around nth 
	 fibre n and kth fibre */
      if (ylow[k]<lvecbuf1[orderkfibrekixindex])
	ylowk = lvecbuf1[orderkfibrekixindex];
      else ylowk = ylow[k];
      if (yup[k]>lvecbuf2[orderkfibrekixindex])
	yupk = lvecbuf2[orderkfibrekixindex];
      else yupk = yup[k];
      pylowk = pylow[k];
      pyupk = pyup[k];
      if (ylowk<ylown) ylowk=ylown;
      if (yupk>yupn) yupk=yupn;
      /* compute the covariances of the elements of the xx vector */
      for (iy=ylowk; iy<=yupk; iy++) {
	iyixindex = iy*ScienceFrame->subcols;
	if (fmvecbuf2[iyixindex]==0) {
	  /* take care of fractional pixels */
	  fractionn = fractionk = 1;
	  if ((dreduce=pylown+.5-(double)iy)>0) fractionn-=(frame_data)dreduce;
	  if ((dreduce=.5-pyupn+(double)iy)>0) fractionn-=(frame_data)dreduce;
	  if ((dreduce=pylowk+.5-(double)iy)>0) fractionk-=(frame_data)dreduce;
	  if ((dreduce=.5-pyupk+(double)iy)>0) fractionk-=(frame_data)dreduce;
	  dvecbuf3[nkindex] += fractionn*fractionk*fdvecbuf2[iyixindex];
	}
      }
    }
    /* make a second copy of xx, to keep it after dgaussj */
    xx2[n] = dvecbuf5[noffset];
  }

  /* Invert matrix aa using Gauss-Jordan elimination getting in xx the 
     deblended spectrum */
  dgaussj(aa, (*numslices), xx, 1);

  /* now compute the errors */
  for (i=1; i<=(*numslices); i++) {
    ioffset = i-1;
    ioffset2 = i+1;
    ioffset1 = ioffset*arraysize;
    fibrelowi = i>2 ? ioffset : 1;
    fibrehighi = ioffset2<(*numslices) ? ioffset2 : (*numslices);
    fibrei = fibrestosolve[i];
    orderi = orderstosolve[i];
    for (j=i; j<=fibrehighi; j++) {
      joffset = j-1;
      joffset2 = j+1;
      joffset1 = joffset*arraysize;
      ijindex = ioffset1+j;
      fibrelowj = joffset>fibrelowi ? joffset : fibrelowi;
      fibrehighj = joffset2<fibrehighi ? joffset2 : fibrehighi;
      fibrej = fibrestosolve[j];
      orderj = orderstosolve[j];
      /* here go the internal loops to compute the errors */
      for (m=fibrelowj; m<=fibrehighj; m++) {
	moffset = m-1;
	moffset2 = m+1;
	imindex = ioffset1+m;
	moffset1 = moffset*arraysize;
	fibrelowm = moffset>fibrelowj ? moffset : fibrelowj;
	fibrehighm = moffset2<fibrehighj ? moffset2 : fibrehighj;
	for (n=fibrelowm; n<=fibrehighm; n++) {
	  noffset = n-1;
	  noffset2 = n+1;
	  mnindex = moffset1+n;
	  jnindex = joffset1+n;
	  /* start with the first, presumably largest, component */
	  if (dvecbuf3[mnindex]>0) 
	    dvecbuf4[ijindex] += dvecbuf1[imindex]*dvecbuf1[jnindex]*
	      dvecbuf3[mnindex];
	  fibrelown = noffset>fibrelowm ? noffset : fibrelowm;
	  fibrehighn = noffset2<fibrehighm ? noffset2 : fibrehighm;
	  for (o=fibrelown; o<=fibrehighn; o++) {
	    ooffset = o-1;
	    ooffset2 = o+1;
	    ooffset1 = ooffset*arraysize;
	    ioindex = ioffset1+o;
	    joindex = joffset1+o;
	    fibrelowo = ooffset>fibrelown ? ooffset : fibrelown;
	    fibrehigho = ooffset2<fibrehighn ? ooffset2 : fibrehighn;
	    for (p=fibrelowo; p<=fibrehigho; p++) {
	      poffset = p-1;
	      poffset1 = poffset*arraysize;
	      npindex = noffset1+p;
	      opindex = ooffset1+p;
	      pmindex = poffset1+m;
	      if (dvecbuf2[opindex]>0)
		dvecbuf4[ijindex] += dvecbuf1[ioindex]*dvecbuf1[pmindex]*
		  dvecbuf1[joindex]*dvecbuf1[npindex]*xx2[m]*xx2[n]*
		  dvecbuf2[opindex];
	    }
	  }
	}
      }
    }
    /* exploit the symmetry of the covariance */
    for (j=i+1; j<=fibrehighi; j++) {
      jiindex = ((j-1)*arraysize)+i;
      ijindex = ioffset1+j;
      dvecbuf4[jiindex] = dvecbuf4[ijindex];
    }
    /* put the spectrum, variance and mask where they belong */
    iiindex = ioffset1+i;
    orderifibreiindex = (orderi*ScienceFrame->maxfibres)+fibrei;
    fdvecbuf5[orderifibreiindex] = dvecbuf5[ioffset];
    fmvecbuf3[orderifibreiindex] = 1;
    fdvecbuf6[orderifibreiindex] = dvecbuf4[iiindex];
    /* if the following covariance is between this fibre and the following one, 
       put it in speccovar */
    if ((ioffset2<=(*numslices)) && (fibrestosolve[ioffset2]==fibrei+1))
      fdvecbuf7[orderifibreiindex] = dvecbuf4[iiindex+1];
  }

  return NOERR;
}
/**@}*/
