/*===========================================================================
  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
===========================================================================*/
/* Program  : Opt_Extract.c                                                */
/* Author   : G. Mulas  -  ITAL_FLAMES Consortium                          */
/* Date     :                                                              */
/*                                                                         */
/* Purpose  : Missing                                                      */
/*                                                                         */
/*                                                                         */
/* Input:  see interface                                                   */ 
/*                                                                      */
/* Output:                                                              */
/*                                                                         */
/* DRS Functions called:                                                   */
/* none                                                                    */ 
/*                                                                         */ 
/* Pseudocode:                                                             */
/* Missing                                                                 */ 
/*                                                                         */ 
/* Version  :                                                              */
/* Last modification date: 2002/08/05                                      */
/* Who     When        Why                Where                            */
/* AMo     02-08-05   Add header         header                            */
/*-------------------------------------------------------------------------*/
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
.COPYRIGHT (c) 2001 European Southern Observatory 
.IDENT Opt_Extract.c
.LANGUAGE C
.AUTHOR P. Caldara   Osservatorio Astronomico di Palermo
.KEYWORDS Optimal Extraction
.PURPOSE 
.COMMENT It uses the structures defined in the flames_uves.h file
.VERSION 0.1
.RETURNS 
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 */
#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif
/* 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_quickoptextract.h>
#include <flames_uves.h>

/* 
   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 quickoptextract(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)
{ 
    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 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_mask *fmvecbuf1=0;
    frame_mask *fmvecbuf2=0;
    frame_mask *fmvecbuf3=0;
    frame_mask *fmvecbuf4=0;
    double *dvecbuf1=0;
    double *dvecbuf2=0;
    int32_t mjindex=0;
    int32_t mifibreoffset=0;
    int32_t mifibreindex=0;
    int32_t mifibrejindex=0;
    int32_t ijindex=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 goodpixels=0;

    int32_t kbsubcols=0;


    /* 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 = ScienceFrame->frame_array[0]+j;
    fdvecbuf2 = ScienceFrame->frame_sigma[0]+j;

    kbsubcols  = SingleFF->subcols;
    m = ordsta-Order->firstorder;
    mjindex = m * kbsubcols;
    mifibreoffset = m*SingleFF->maxfibres;

    for (m=ordsta-Order->firstorder; m<=ordend-Order->firstorder;) {
        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];
            fdvecbuf3 = SingleFF->flatdata[iframe].data[0]+j;
            /* 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[mifibrejindex]!=BADSLICE) {
                /* it is at least half good, is coverage good enough for this fibre? */
                goodpixels=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 */
                        goodpixels++;
                    }
                    ijindex += kbsubcols;
                }

                /* does the fraction of pixels in this fibre exceed the
       threshold making it worth extracting? */
                if((double)goodpixels*SingleFF->substepy /
                                (2*SingleFF->halfibrewidth) < SingleFF->minfibrefrac) {
                    /* 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;
            }
        }

        /* it is advantageous to directly increment these indices together with m,
       by the right amount, instead of recalculating them, although it is
       quite a bit less readable. At least, let's increment m here too, 
       instead of incrementing it in the for() statement, this makes the code 
       a little (tiny) bit clearer. */
        m ++;
        mjindex += 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];
        fdvecbuf3 = 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) (fdvecbuf1[ijindex]*fdvecbuf3[ijindex]/fdvecbuf2[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];
        fdvecbuf3 = 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 = fdvecbuf3[ijindex];
                dvecbuf1[nnindex] += fdbuf1*fdbuf1/fdvecbuf2[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];
            fdvecbuf4 = 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)(fdvecbuf4[ijindex]*fdvecbuf3[ijindex]/fdvecbuf2[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 */
    flames_gauss_jordan(aa, (*numslices), xx, 1);

    fdvecbuf5 = ScienceFrame->spectrum[j][0];
    fmvecbuf4 = ScienceFrame->specmask[j][0];
    /* Retrieving spectrum from the temporary vector */
    for (n=1; n<=(*numslices); n++) {
        noffset = n-1;
        ordern = orderstosolve[n];
        fibren = fibrestosolve[n];
        ordernfibrenjindex = (ordern*ScienceFrame->maxfibres)+fibren;
        fdvecbuf5[ordernfibrenjindex] = (frame_data) dvecbuf2[noffset];
        fmvecbuf4[ordernfibrenjindex] = 1;
    }

    /*
  SCTDIS("Exiting Opt_Extract\n",0);
     */

    return NOERR;

}