/*===========================================================================
  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_uves.h>
#include <flames_Stand_Extract.h>
#include <flames_def_drs_par.h>
#include <flames_newmatrix.h>
#include <flames_gauss_jordan.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;
    //int32_t orderj=0, fibrej=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;



    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 (0 != SCKGETC(DRS_VERBOSITY, 1, 3, &actvals, drs_verbosity)) {
        /* 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 {
        	char output[70]; /* only for testing purposes */
            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 gauss-jordan */
        xx2[n] = dvecbuf5[noffset];
    }

    /* Invert matrix aa using Gauss-Jordan elimination getting in xx the
     deblended spectrum */
    flames_gauss_jordan(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;
}
/**@}*/