/*===========================================================================
 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_opt_extract   Substep: Flames Optimal Extraction
 *
 */
/*--------------------------------------------------------------------------*/
#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif
/*----------------------------------------------------------------------------
 Includes
 ---------------------------------------------------------------------------*/
/* C functions include files */
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
/* MIDAS include files */
#include <flames_midas_def.h>
/* FLAMES-UVES include files */
#include <flames_gauss_jordan.h>
#include <flames_uves.h>
#include <flames_Opt_Extract.h>
#include <flames_newmatrix.h>

/**@{*/

#include <uves_msg.h>
#include <string.h>             //memset
#include <flames_def_drs_par.h> //DRS_VERBOSITY


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

/**
 @name  flames_Opt_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 j            pixel position in the science frame
 @param mask         input mask
 @param aa           array of size [arraysize,arraysize]
 @param xx           array of size [1,arraysize]
 @param arraysize    array size = norders*nfibres
 @param fibrestosolve vector of size arraysize
 @param orderstosolve vector of size arraysize
 @param numslices    output number of slices
 @param normcover    array to hold fibre normalization factors
 @param orderoffset  order offset

 DRS function called:
 Pseudocode:
 @ doc:

 -Determine the actual size of the problem to be solved and which are the slices to be included
 - Loop over orders
 Loop over fibres

 check if a given fibre should be extracted at a given slice.
 skip not-extract-able slices, since they would invariably lead to
 singular matrices, being ill-conditioned problems

 To be extracted a fibre should be least half good=>check fibre coverage.
 for good pixels
 -add their contribution to the fibre coverage factor
 -then divide by the normalisation fraction
 -if the fraction of collected light for the current fibre exceeds a given threshold
 it is worth to extract that slice
 else mark the fibre slice as bad
 else
 this slice is bad, mark it as such in the mask associated
 count how many good slices we found:
 if there are no good slices just free allocated memory and exit
 else
 performs optimal extraction
 Create matrices and vectors to be used in matrix inversion
 if a fibre is at least half good, go ahead and extract it
 use a pixel only if it is good in the overall mask

 Invert matrix aa using Gauss-Jordan elimination getting in xx the de-blended spectrum

 @note

 In the following is supposed that:

 flatdata[n]   -> n-th fibre FF
 flatdata[n+1] -> upper half slit FF
 flatdata[n+2] -> lower half slit FF


 */

flames_err Opt_Extract(flames_frame *ScienceFrame, allflats *SingleFF,
                       orderpos *Order, int32_t ordsta, int32_t ordend, int32_t j,
                       frame_mask **mask, double **aa, double **xx, int32_t arraysize,
                       int32_t *fibrestosolve, int32_t *orderstosolve, int32_t *numslices,
                       frame_data **normcover, int32_t orderoffset) {
    int32_t i = 0, m = 0, n = 0, k = 0;
    int32_t ilow = 0, ihigh = 0;
    int32_t ordern, fibren, framen, orderk, fibrek, framek, ifibre, iframe;
    frame_data ffcoverage = 0;
    frame_data fdbuf1 = 0;

    int32_t *lvecbuf1 = 0;
    int32_t *lvecbuf2 = 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_mask *fmvecbuf1 = 0;
    frame_mask *fmvecbuf2 = 0;
    frame_mask *fmvecbuf3 = 0;
    frame_mask *fmvecbuf4 = 0;
    double *dvecbuf1 = 0;
    double *dvecbuf2 = 0;
    int32_t offsetm = 0;
    int32_t offsetmjindex = 0;
    int32_t mifibreoffset = 0;
    int32_t mifibreindex = 0;
    int32_t mifibrejindex = 0;
    int32_t ijindex = 0;
    int32_t orderoffsetmjindex = 0;
    int32_t upnlimit = 0;
    int32_t noffset = 0;
    int32_t noffset1 = 0;
    int32_t nkindex = 0;
    int32_t ordernfibrenjindex = 0;
    int32_t nnindex = 0;
    int32_t koffset = 0;
    int32_t koffset1 = 0;
    int32_t knindex = 0;
    int32_t orderkfibrekjindex = 0;

    int32_t kbsubcols;
    frame_data kbthreshold;

    /* Here the variance of the spectra is computed too. This calculation is
   carried out at first order of approximation.
   i.e. the following formulae hold
   A*X=B
   which gives the spectra and
   (A+dA)*(X+dX)=B+dB
   which with the approximation
   dA*dX=0
   and the first formula becomes
   A*dX=dB+dA*X
   and so we have the variances for the each spectrum.
   Moreover, we assume the variance in the FF frames to be negligible with
   respect to that in the Science Frame, which in turn means that dA is
   negligible with respect to dB. If all of this holds, it can be shown
   that, to a very decent first order approximation,
   cov(Xi,Xj) = (Inv(A))ij
   A more accurate estimate involves some heavier loops which are better
   done elsewhere, to avoid unnecessarily cluttering the extraction loop,
   which is supposed to be executed many times. For this reason, get
   Inv(A), the extracted X, fibrestosolve and numslices out of this
   function, for further processing elsewhere.
     */

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

    (*numslices) = 0;

    lvecbuf1 = SingleFF->lowfibrebounds[0][0] + j;
    lvecbuf2 = SingleFF->highfibrebounds[0][0] + j;
    fmvecbuf1 = SingleFF->goodfibres[0][0] + j;
    fmvecbuf2 = mask[0] + j;
    fmvecbuf3 = ScienceFrame->specmask[j][0];
    fdvecbuf1 = normcover[0] + j;
    fdvecbuf2 = ScienceFrame->frame_array[0] + j;
    fdvecbuf3 = ScienceFrame->frame_sigma[0] + j;

    kbsubcols = SingleFF->subcols;
    m = ordsta - Order->firstorder;
    offsetm = m - orderoffset;
    offsetmjindex = offsetm * kbsubcols;
    mifibreoffset = m * SingleFF->maxfibres;

    kbthreshold = fdvecbuf1[orderoffsetmjindex] * SingleFF->minfibrefrac;

    for (m = ordsta - Order->firstorder; m <= ordend - Order->firstorder; m++) {

        for (n = 0; n <= (ScienceFrame->num_lit_fibres - 1); n++) {
            ifibre = ScienceFrame->ind_lit_fibres[n];
            mifibreindex = mifibreoffset + ifibre;
            mifibrejindex = (mifibreindex) * kbsubcols;
            iframe = SingleFF->fibre2frame[ifibre];
            fdvecbuf4 = SingleFF->flatdata[iframe].data[0] + j;
            /* should I try to extract this fibre at this slice? It is crucial to
       skip not-extract-able slices, since they would invariably lead to
       singular matrices, being ill-conditioned problems */
            if (fmvecbuf1[mifibrejindex] != BADSLICE) {
                /* it is at least half good, is coverage good enough for this fibre? */
                ffcoverage = 0;

                ijindex = lvecbuf1[mifibrejindex] * kbsubcols;

                for (i = lvecbuf1[mifibrejindex]; i <= lvecbuf2[mifibrejindex]; i++) {
                    /* is this pixel good? */
                    if (fmvecbuf2[ijindex] == 0) {
                        /* add its contribution to the fibre coverage factor */
                        ffcoverage += fdvecbuf4[ijindex];
                    }
                    ijindex += kbsubcols;
                }

                /* divide by the normalisation fraction
         ffcoverage /= fdvecbuf1[orderoffsetmjindex];  */
                /* does the fraction of collected light for this fibre exceed the
         threshold making it worth extracting?
         if(ffcoverage<SingleFF->minfibrefrac) */
                if (ffcoverage < kbthreshold) {
                    /* no, forget it and mark this fact where it belongs */
                    fmvecbuf1[mifibrejindex] = BADSLICE;
                } else {
                    (*numslices)++;
                    fibrestosolve[*numslices] = ifibre;
                    orderstosolve[*numslices] = m;
                }
            } else {
                /* this slice is bad, mark it as such in the specmask */
                fmvecbuf3[mifibreindex] = 0;
            }
        }

        offsetm++;
        offsetmjindex += kbsubcols;
        mifibreoffset += SingleFF->maxfibres;

    }

    /* if there are no good slices just free allocated memory and return right
   now */
    if ((*numslices) == 0)
        return (NOERR);

    /* Initialize xx and aa */

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

    dvecbuf1 = aa[1];
    noffset1 = 0;
    for (noffset = 0; noffset < (*numslices); noffset++) {
        for (k = 1; k <= (*numslices); k++) {
            dvecbuf1[noffset1 + k] = 0;
        }
        noffset1 += arraysize;
    }

    /* Create matrices and vectors to be used in matrix inversion */
    /* nm=0;*/

    for (n = 1; n <= (*numslices); n++) {
        /* it is at least half good, go ahead and extract it */
        noffset = n - 1;
        ordern = orderstosolve[n];
        fibren = fibrestosolve[n];
        ordernfibrenjindex = ((ordern * SingleFF->maxfibres) + fibren) * kbsubcols;
        framen = SingleFF->fibre2frame[fibren];
        fdvecbuf4 = SingleFF->flatdata[framen].data[0] + j;
        for (i = lvecbuf1[ordernfibrenjindex]; i <= lvecbuf2[ordernfibrenjindex];
                        i++) {
            ijindex = i * SingleFF->subcols;
            /* use this pixel only if it is good in the overall mask */
            if (fmvecbuf2[ijindex] == 0) {
                dvecbuf2[noffset] += (double) (fdvecbuf2[ijindex] * fdvecbuf4[ijindex]
                                                                              / fdvecbuf3[ijindex]);
            }
        }
    }

    for (n = 1; n <= (*numslices); n++) {
        noffset = n - 1;
        noffset1 = noffset * arraysize;
        nnindex = noffset1 + n;
        ordern = orderstosolve[n];
        fibren = fibrestosolve[n];
        ordernfibrenjindex = ((ordern * SingleFF->maxfibres) + fibren)
                        * SingleFF->subcols;
        framen = SingleFF->fibre2frame[fibren];
        fdvecbuf4 = SingleFF->flatdata[framen].data[0] + j;
        /* compute diagonal terms first */
        ijindex = lvecbuf1[ordernfibrenjindex] * kbsubcols;
        for (i = lvecbuf1[ordernfibrenjindex]; i <= lvecbuf2[ordernfibrenjindex];
                        i++) {
            if (fmvecbuf2[ijindex] == 0) {
                fdbuf1 = fdvecbuf4[ijindex];
                dvecbuf1[nnindex] += fdbuf1 * fdbuf1 / fdvecbuf3[ijindex];
            }
            ijindex += kbsubcols;
        }
        /* now go for off-diagonal terms */
        for (k = n + 1; k <= (*numslices); k++) {
            koffset = k - 1;
            koffset1 = koffset * arraysize;
            nkindex = noffset1 + k;
            knindex = koffset1 + n;
            orderk = orderstosolve[k];
            fibrek = fibrestosolve[k];
            orderkfibrekjindex = ((orderk * SingleFF->maxfibres) + fibrek)
                          * SingleFF->subcols;
            framek = SingleFF->fibre2frame[fibrek];
            fdvecbuf5 = SingleFF->flatdata[framek].data[0] + j;
            /* the y loop must run only where the two fibres overlap */
            ilow =
                            lvecbuf1[ordernfibrenjindex] < lvecbuf1[orderkfibrekjindex] ?
                                            lvecbuf1[orderkfibrekjindex] : lvecbuf1[ordernfibrenjindex];
            ihigh =
                            lvecbuf2[ordernfibrenjindex] < lvecbuf2[orderkfibrekjindex] ?
                                            lvecbuf2[ordernfibrenjindex] : lvecbuf2[orderkfibrekjindex];
            ijindex = ilow * kbsubcols;
            for (i = ilow; i <= ihigh; i++) {
                if (fmvecbuf2[ijindex] == 0) {
                    dvecbuf1[nkindex] += (double) (fdvecbuf5[ijindex] * fdvecbuf4[ijindex]
                                                                                  / fdvecbuf3[ijindex]);
                }
                ijindex += kbsubcols;
            }
            /* use symmetry to set values on the other side of the diagonal */
            dvecbuf1[knindex] = dvecbuf1[nkindex];
        }
    }

    /* Invert matrix aa using Gauss-Jordan elimination getting in xx the
   deblended spectrum */

    cpl_matrix* m_aa=cpl_matrix_new(*numslices+1,*numslices+1);
    cpl_matrix* m_xx=cpl_matrix_new(*numslices+1,*numslices+1);
    cpl_matrix_set(m_aa,0,0,0);
    cpl_matrix_set(m_xx,0,0,0);


    flames_gauss_jordan(aa, (*numslices), xx, 1);
    fdvecbuf6 = ScienceFrame->spectrum[j][0];
    fmvecbuf4 = ScienceFrame->specmask[j][0];
    /* Retrieving spectrum from the temporary vector */
    /* AMO: repeat this setting to be sure */
    dvecbuf2 = xx[1] + 1;

    //printf("ok6\n");
    for (n = 1; n <= (*numslices); n++) {
        noffset = n - 1;
        ordern = orderstosolve[n];
        fibren = fibrestosolve[n];
        ordernfibrenjindex = (ordern * ScienceFrame->maxfibres) + fibren;
        fdvecbuf6[ordernfibrenjindex] = (frame_data) dvecbuf2[noffset];
        fmvecbuf4[ordernfibrenjindex] = 1;
    }
    cpl_matrix_delete(m_aa);
    cpl_matrix_delete(m_xx);
    /*
   SCTDIS("Exiting Opt_Extract\n",0);
     */
    return NOERR;
}
/**@}*/