/* @(#)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 output[200];
  char outtab[CATREC_LEN+5];
  int upointer = 0;
  int actvals = 0;
  int no_elem_ff = 0;
  int irr = 0;
  int irrp1 = 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++) {
    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? */
  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;
}
