/* @(#)matchorders    */
/*===========================================================================
  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  : matchorders.c                                                */
/* Author   : I. Porceddu  -  ITAL_FLAMES Consortium                       */
/* Date     :                                                              */
/*                                                                         */
/* Purpose  :                                                              */
/*                                                                         */
/* Input    :                                                              */
/*      ORDTAB     Table originated by HOUGH/ECHELLE. Three further columns 
               have been added                                         */
/*    Now middummr.tbl                                                   */
/*      DEFPOL     Degree in x and m domain as computed during format check 
               loop                                                    */
/*      COEFFD     A descriptor containing the coeffs of the bivariate poly 
               as extracted from the format check application          */
/*      ECHORD     Number of the first guess detected orders               */
/*                                                                         */
/* Output:                                                                 */
/*      OUTTAB     middummo.tbl with :NEWORD and :FIBRE filled             */ 
/*                                                                         */
/* SEQUENCE:
   - flames_preordpos (preordpos.c) 
    creates middummd table 
   - flames_ordpos (hough/echelle - matchorders.c - tracing.c - fitting.c )
    hough/echelle: creates middummr table
    matchorders: adds :NEWORD and :FIBRE values to middummr table
 */
/*                                                                         */
/* DRS Functions called:                                                   */
/* none                                                                    */ 
/*                                                                         */ 
/* Pseudocode:                                                             */
/* Missing                                                                 */ 
/*                                                                         */ 
/* Version  :                                                              */
/* Last modification date: 2002/08/05                                      */
/* Who     When        Why                Where                            */
/* AMo     02-08-05   Add header         header                            */
/*-------------------------------------------------------------------------*/
#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif

#include <flames_matchorders.h>

#include <flames_midas_tblsys.h>
#include <flames_midas_tblerr.h>
#include <flames_midas_tbldef.h>
#include <flames_midas_macrogen.h>
#include <flames_midas_atype.h>
#include <flames_midas_def.h>
#include <flames_def_drs_par.h>
#include <flames_uves.h>
#include <flames_newmatrix.h>
#include <uves_msg.h>

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

static int 
deltacompare(const void *pos1, const void *pos2);

int flames_matchorders(const double *MATCH_THRES,
                       const int *LIMIT,
                       const double *TAB_IO_YSHIFT,
                       int *FIBRENUMBERS,
                       double *FIBRESHIFTS,
                       const char *LTAB,
                       const int *NUMBER,
                       const int *DY_RANGE,
                       const double *DY_STEP,
                       int *FIBRESON)
{

    char outtab[CATREC_LEN+5];
    int upointer = 0;
    int actvals = 0;
    int no_elem_ff = 0;
    int irr = 0;

    int colnum = 0;
    int colnum1 = 0;
    int colnum2[4] = {0,0,0,0};
    int no_elem_ftck = 0;
    int ir=0;
    int irp1=0;
    int irmin=0;
    int irmax=0;
    int TIN2id = 0;
    int TIN1id = 0;
    int TOUTid = 0;
    int steps = 0;
    int index = 0;
    int loop = 0;
    int idelta = 0;
    int ideltamax = 0;
    int *orders = 0;
    double **pftck = 0;
    double ftck = 0;
    double **pordpos = 0;
    double ordpos = 0;
    double matchthres = 0;
    double Residual = 0;
    double dy = 0;
    double arg = 0;
    double arg1 = 0;
    double arg2 = 0;
    double pos0 = 0;
    double pos1 = 0;
    double pos2 = 0;
    int ff = 0;
    int i = 0;
    int row_i=0;
    int dy_range =0;
    int k = 0;
    int fibers_on = 0;
    int null = 0;
    int number = 0;
    int maxfibres = 0;
    int ifibre=0;
    float    **delta=0;
    float    *maxx=0;
    float    peak = 0.0;
    float    **delta_fibers=0;
    int *fibrenumbers=0;
    int *fibremask=0;
    int32_t dy_stepnum=0, approxhalfwidth=0;
    double dy_step=0;
    double startyshift=0;
    double *fibreshifts=0;
    int nflats=0;



    char drs_verbosity[10];
    int status=0;
    int dummy;


    /* === interface to MIDAS ================================================*/

    /*Let's initialize the MIDAS environment */
    SCSPRO("matchorders");

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

    /* keyword MAXFIBRES stores the maximum number of fibres */
    SCKRDI(&MAXFIBRES, 1, 1, &actvals, &maxfibres, &upointer, &null);
    if (maxfibres <= 0) {
        SCTPUT("Error: MAXFIBRES must be > 0!");
        return flames_midas_fail();
    }

    /* initialise matchthres from keyword */
    if (SCKRDD(MATCH_THRES, 1, 1, &actvals, &matchthres, &upointer,
                    &null) != 0) {
        /* problems reading MATCH_THRES */
        SCTPUT("Error reading the MATCH_THRES keyword");
        return flames_midas_fail();
    }

    /* keyword limit stores the maximum number of frames + 2 */
    /* jmlarsen: changed to uppercase */
    SCKRDI(LIMIT, 1, 1, &actvals, &nflats, &upointer, &null);
    nflats -= 2;

    /* keyword TAB_IO_YSHIFT stores the y shift of corresponding
     orders between the guess table we are using now and the one
     which was used first to determine it (this whole procedure can
     be used recursively) */
    SCKRDD(TAB_IO_YSHIFT, 1, 1, &actvals, &startyshift, &upointer, &null);

    maxx = vector(0, maxfibres-1);
    delta_fibers = matrix(0, maxfibres-1, 0, 1);
    fibrenumbers = ivector(0, (nflats*maxfibres)-1);
    fibremask = ivector(0, maxfibres-1);
    fibreshifts = dvector(0, (nflats*maxfibres)-1);

    SCKRDI(FIBRENUMBERS, 1, nflats*maxfibres, &actvals,
           fibrenumbers, &upointer, &null);

    SCKRDD(FIBRESHIFTS, 1, nflats*maxfibres, &actvals,
           fibreshifts, &upointer, &null);

    for (row_i=0; row_i<=(maxfibres-1); row_i++){
        maxx[row_i] = 0.0;
        delta_fibers[row_i][0] = 0.0;
        delta_fibers[row_i][1] = 0.0;
    }

    /* Let's open the table created by HOUGH/ECHELLE.
     A few columns have been added. Standard name is middummr */
    TCTOPN("middummr.fits",F_IO_MODE,&TIN1id);

    /* The FIBREMASK descriptor was copied to the middummr table from the
     input frame */
    SCDRDI(TIN1id,"FIBREMASK",1,maxfibres,&actvals,fibremask,&upointer,&null);

    /* The TBLCONTR descriptor does contain the actual number of orders by fibres
     which have been detected. We use the 4th element of the array */
    // jmlarsen:  Read table geometry by using the dedicated function */
    //  SCDRDI(TIN1id,"TBLCONTR",4,1,&actvals,&no_elem_ff,&upointer,&null);
    TCIGET (TIN1id, &dummy, &no_elem_ff);

    /* We need the column number corresponding to the column labelled as
     YCENTER */
    TCLSER(TIN1id,"YQUART1",&colnum2[0]);
    TCLSER(TIN1id,"YCENTER",&colnum2[1]);
    TCLSER(TIN1id,"YQUART3",&colnum2[2]);

    /* We allocate the memory for the bidimensional double matrix pordpos.
     It has a number of rows corresponding to the number of steps used for 
     matching the FtCk and the FF frames. The columns will contain
     1) The y value in the central column of the frame;
     2) The order as being computed from the cross correlation (see below) */

    pordpos=dmatrix(0,no_elem_ff-1,0,4);

    /* We read the column  YCENTER from middummr table (Order Pos) */
    for (irr=0 ; irr<=(no_elem_ff-1); irr++) {
        int irrp1 = irr+1;
        if (TCERDD(TIN1id,irrp1,colnum2[0],&ordpos,&null)) return flames_midas_fail();
        pordpos[irr][0] = ordpos;
        if (TCERDD(TIN1id,irrp1,colnum2[1],&ordpos,&null)) return flames_midas_fail();
        pordpos[irr][1] = ordpos;
        if (TCERDD(TIN1id,irrp1,colnum2[2],&ordpos,&null)) return flames_midas_fail();
        pordpos[irr][2] = ordpos;
        pordpos[irr][3] = 0;
        pordpos[irr][4] = 0;
    }

    /* The GTAB keyword does provide the actual name for guess table */
    SCKGETC(LTAB,1,14,&actvals,outtab);

    /* Let's open the table file as created from Format Check
     which contains the y values for the mid frame position */
    TCTOPN(outtab,F_IO_MODE,&TIN2id);

    /* The TBLCONTR descriptor does contain the number of orders detected by
     FmtCk 
     We use the 4th element of the array */
    //jmlarsen: Use the dedicated function for this
    //SCDRDI(TIN2id,"TBLCONTR",4,1,&actvals,&no_elem_ftck,&upointer,&null);
    TCIGET (TIN2id, &dummy, &no_elem_ftck);


    /* Keyword NUMBER stores the total no. of fibers */
    SCKRDI(NUMBER,1,1,&actvals,&number,&upointer,&null);

    /* We need the column number corresponding to the column labelled as
     YMIDFTCK */
    TCLSER(TIN2id,"YQ1FTCK",&colnum2[0]);
    TCLSER(TIN2id,"YMIDFTCK",&colnum2[1]);
    TCLSER(TIN2id,"YQ3FTCK",&colnum2[2]);
    TCLSER(TIN2id,"ORDER",&colnum2[3]);

    /* We read the YMIDFTCK column from middummd table (Fmt Ck) - START
     Allocate memory for the array which will accept the YMIDFTCK values */ 
    pftck = dmatrix(0,no_elem_ftck-1,0,2);
    orders = ivector(0,no_elem_ftck-1);

    /* Loop over the FmtChk orders */
    for (ir=0 ; ir <= (no_elem_ftck-1) ; ir++) {
        irp1 = ir+1;
        /* Read actual values*/
        if (TCERDD(TIN2id,irp1,colnum2[0],&ftck,&null)) return flames_midas_fail();
        pftck[ir][0] = ftck;
        /* printf("pftck[ir][0]=%f\n",pftck[ir][0]); */
        if (TCERDD(TIN2id,irp1,colnum2[1],&ftck,&null)) return flames_midas_fail();
        pftck[ir][1] = ftck;
        if (TCERDD(TIN2id,irp1,colnum2[2],&ftck,&null)) return flames_midas_fail();
        pftck[ir][2] = ftck;
        if (TCERDI(TIN2id,irp1,colnum2[3],orders+ir,&null)) return flames_midas_fail();
    }

    /* Running counter steps: the number of steps' values is a function
     of the running "dy" window*/
    steps=0;

    /* dy half window */
    /* read keyword DY_RANGE */
    SCKRDI(DY_RANGE, 1, 1, &actvals, &dy_range, &upointer, &null);
    SCKRDD(DY_STEP, 1, 1, &actvals, &dy_step, &upointer, &null);
    dy_stepnum = 2*((int32_t)ceil((double)dy_range/dy_step));

    delta = matrix(0, dy_stepnum, 0, 2);

    /*Looping over dy_range. We compute the cross correlation between
    the FmtChk function and that derived from the FF frame */
    for (dy=-(double)dy_range ; dy<=(double)dy_range; dy += dy_step) {
        delta[steps][0] = 0;
        for (i=0; i<=(no_elem_ftck-1); i++) {
            pos0 = pftck[i][0]+dy;
            pos1 = pftck[i][1]+dy;
            pos2 = pftck[i][2]+dy;
            for (ff=0 ; ff<=(no_elem_ff-1); ff++ ) {
                arg1 = pos0-pordpos[ff][0];  /* YQUART1 */
                arg2 = arg1*arg1;
                arg1 = pos1-pordpos[ff][1]; /* YCENTER */
                arg2 += arg1*arg1;
                arg1 = pos2-pordpos[ff][2]; /* YQUART3 */
                arg2 += arg1*arg1;
                arg = .5*arg2;
                /*****************************************************************
                 * this check to make the code portable to Alpha-Linux            *
                 * Indeed on Alpha the argument of the exponential cannot be      *
                 * in absolute value greater than some big number > ~ 700. The    *
                 * threshold chosen (200) is to be on a safe side. Despite this   *
                 * check slow the code and is not very nice, the result is almost *
                 * unaffected and the code results more robust.            *
                 *****************************************************************/
                if(arg <= 200) {
                    delta[steps][0] += exp(-arg);
                    /*printf("arg=%f\n",arg);*/
                }
            }
        }
        delta[steps][1] = dy;
        uves_msg_debug("Cross-correlation dy = %f, val = %f", dy, delta[steps][0]);
        steps++;
    }

    /* Creating a new table for offline plotting of peaks */
    /* jmlarsen: Use F_O_MODE for new table
     old code: TCTINI("middummf",F_IO_MODE,steps,&TOUTid); */
    TCTINI("middummf.fits",F_O_MODE,steps,&TOUTid);

    /* Creating a new column */
    TCCINI(TOUTid, D_R8_FORMAT, 1, "F8.4", " ", "DELTA", &colnum);
    /* Writing table values */
    for (index=0; index <= (steps-1); index++) {
        if (TCEWRR(TOUTid, index+1, colnum, &delta[index][0]))
        {
            return flames_midas_fail();
        }
    }
    TCTCLO(TOUTid);

    /* Hunting for peak values in the cross correlation function*/
    ideltamax=0;
    for (loop=0; loop<=(maxfibres-1); loop++) {
        for (idelta =1; idelta <= (steps-1); idelta++) { /* idelta <= 1 ?????? */
            if(delta[ideltamax][0]-delta[idelta][0] < 0) {
                maxx[loop] = delta[idelta][0];
                ideltamax = idelta;
            }
        }
        delta_fibers[loop][0] = delta[ideltamax][0];
        delta_fibers[loop][1] = delta[ideltamax][1];
        approxhalfwidth = (int32_t) ceil(5/dy_step);
        if ((irmin=ideltamax-approxhalfwidth)<0) irmin=0;
        if ((irmax=ideltamax+approxhalfwidth)>(steps-1)) irmax=steps-1;
        for (ir = irmin; ir<=irmax;ir++) delta[ir][0] = 0.;

        uves_msg_debug("d_f0 %f  d_f1  %f    ideltamax %d",delta_fibers[loop][0],
                       delta_fibers[loop][1],ideltamax);
    }

    /* Selecting the right peaks (= lit fibers!) */
    peak = delta_fibers[0][0];
    for(loop=0; loop<=(maxfibres-1); loop++) {
        if (peak < delta_fibers[loop][0]) peak = delta_fibers[loop][0];
    }
    peak /= 2;  /* arbitrarily selected threshold for cutting down the
              spurious peaks. Initial walue is 2.*/
    fibers_on = 0;
    for (loop=0; loop<=(maxfibres-1); loop++) {
        if (delta_fibers[loop][0] > peak) {
            fibers_on++;
            /* fprintf(stderr, "fibers_on %d \t loop %d  delta_fibers[loop] %f peak %f \n",
          fibers_on,loop,delta_fibers[loop][0],peak );*/
        }
    }

    /* sort the fibres found in this frame in ascending shift order */
    qsort(delta_fibers[0], (size_t) fibers_on, 2*sizeof(float), deltacompare);
    /* find correspondence between lit fibres found here and lit fibres in
     FIBREMASK */
    for (ifibre=0, ir=0; ifibre<=(maxfibres-1); ifibre++) {
        if (fibremask[ifibre] == TRUE) {
            irp1 = ir+number;
            fibrenumbers[irp1] = ifibre+1;
            /* remember the fibre shifts that we found for later use referred to
     the first guess order table which was used */
            fibreshifts[irp1] = delta_fibers[ir][1]+startyshift;
            ir++;
        }
    }
    /* did I match them all? */
    char output[200];
    if (ir != fibers_on) {
        sprintf(output, "Error: %d fibres detected, %d in middummr.fits:FIBREMASK!",
                        fibers_on, ir);
        SCTPUT(output);
        free_dmatrix(pftck, 0, no_elem_ftck-1, 0, 2);
        free_dmatrix(pordpos, 0, no_elem_ff-1, 0, 4);
        free_matrix(delta, 0, dy_stepnum, 0, 2);
        free_ivector(orders, 0, no_elem_ftck-1);
        free_vector(maxx, 0, maxfibres-1);
        free_matrix(delta_fibers, 0, maxfibres-1, 0, 1);
        free_ivector(fibrenumbers, 0, (nflats*maxfibres)-1);
        free_dvector(fibreshifts, 0, (nflats*maxfibres)-1);
        free_ivector(fibremask, 0, maxfibres-1);
        return flames_midas_fail();
    }

    /* Storing fibre correspondence table as a keyword */
    SCKWRI(FIBRENUMBERS, fibrenumbers, 1, nflats*maxfibres, &null);

    /* Storing no. of fibers as keyword */
    SCKWRI(FIBRESON,&fibers_on,1,1,&null);

    /* Storing as descriptor in middummr */
    SCDWRI(TIN1id,"FIBRESON",&fibers_on,1,1,&null);

    /* Labelling fibers and orders */
    for(loop=0 ; loop<=(fibers_on-1) ; loop++) {
        for (ir=0; ir<=(no_elem_ftck-1); ir++) {
            for (ff=no_elem_ff-1; ff>=0; ff--) {
                /* If we get a position mismatch between peak in FmtCk and
       odd/even FF which is below the selected threshold, the fiber
       is marked */
                Residual = fabs(pftck[ir][0]-pordpos[ff][0]+delta_fibers[loop][1])+
                                fabs(pftck[ir][1]-pordpos[ff][1]+delta_fibers[loop][1])+
                                fabs(pftck[ir][2]-pordpos[ff][2]+delta_fibers[loop][1]);
                if(Residual <= matchthres) {
                    if (pordpos[ff][3] != 0) {
                        SCTPUT("Internal error: a fibre matches 2 orders!");
                        /* An error is returned if a fiber is assigned twice */
                        free_dmatrix(pftck, 0, no_elem_ftck-1, 0, 2);
                        free_dmatrix(pordpos, 0, no_elem_ff-1, 0, 4);
                        free_matrix(delta, 0, dy_stepnum, 0, 2);
                        free_ivector(orders, 0, no_elem_ftck-1);
                        free_vector(maxx, 0, maxfibres-1);
                        free_matrix(delta_fibers, 0, maxfibres-1, 0, 1);
                        free_ivector(fibrenumbers, 0, (nflats*maxfibres)-1);
                        free_dvector(fibreshifts, 0, (nflats*maxfibres)-1);
                        free_ivector(fibremask, 0, maxfibres-1);
                        return flames_midas_fail();
                    }
                    pordpos[ff][3] = loop+number+1;
                    pordpos[ff][4] = orders[ir];
                }
            }
        }
    }

    /* Storing fibre correspondence table as a keyword */
    SCKWRD(FIBRESHIFTS, fibreshifts, 1, nflats*maxfibres, &null);

    /* We need the column number corresponding to the column labelled as
     "FIBRE" */

    TCLSER(TIN1id,"NEWORD",&colnum);
    TCLSER(TIN1id,"FIBRE",&colnum1);

    /* Writing found orders/fibres set into middummr table */
    for (k=0; k<=(no_elem_ff-1); k++) {
        if((pordpos[k][3] !=0 ) && (pordpos[k][4]) != 0) {
            if (TCEWRD(TIN1id, k+1, colnum1, &pordpos[k][3]))
            {
                return flames_midas_fail();
            }
            if (TCEWRD(TIN1id, k+1, colnum, &pordpos[k][4]))
            {
                return flames_midas_fail();
            }                
        }
    }
    TCTCLO(TIN1id);
    TCTCLO(TIN2id);
    free_dmatrix(pftck, 0, no_elem_ftck-1, 0, 2);
    free_dmatrix(pordpos, 0, no_elem_ff-1, 0, 4);
    free_matrix(delta, 0, dy_stepnum, 0, 2);
    free_ivector(orders, 0, no_elem_ftck-1);
    free_vector(maxx, 0, maxfibres-1);
    free_matrix(delta_fibers, 0, maxfibres-1, 0, 1);
    free_ivector(fibrenumbers, 0, (nflats*maxfibres)-1);
    free_dvector(fibreshifts, 0, (nflats*maxfibres)-1);
    free_ivector(fibremask, 0, maxfibres-1);

    return SCSEPI();

}

static int 
deltacompare(const void *p1, const void *p2)
{
    const float* pos1=(const float*) p1;
    const float* pos2=(const float*) p2;

    if (*(pos1+1) < *(pos2+1)) return -1;
    else if (*(pos1+1) > *(pos2+1)) return 1;
    else return 0;
}