/*                                                                              *
 *   This file is part of the ESO UVES Pipeline                                 *
 *   Copyright (C) 2004,2005 European Southern Observatory                      *
 *                                                                              *
 *   This library 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, 51 Franklin St, Fifth Floor, Boston, MA  02111-1307  USA       *
 *                                                                              */

/*
 * $Author: amodigli $
 * $Date: 2011-12-08 14:03:53 $
 * $Revision: 1.44 $
 * $Name: not supported by cvs2svn $
 * $Log: not supported by cvs2svn $
 * Revision 1.43  2010/09/24 09:32:04  amodigli
 * put back QFITS dependency to fix problem spot by NRI on FIBER mode (with MIDAS calibs) data
 *
 * Revision 1.41  2010/05/06 14:55:29  amodigli
 * clearer error message
 *
 * Revision 1.40  2007/08/30 07:56:54  amodigli
 * fixed some doxygen warnings
 *
 * Revision 1.39  2007/08/23 08:16:40  jmlarsen
 * Indentation change
 *
 * Revision 1.38  2007/08/21 13:08:26  jmlarsen
 * Removed irplib_access module, largely deprecated by CPL-4
 *
 * Revision 1.37  2007/06/28 09:18:01  jmlarsen
 * Return actualy polynomial degree used
 *
 * Revision 1.36  2007/06/06 08:17:33  amodigli
 * replace tab with 4 spaces
 *
 * Revision 1.35  2007/05/22 14:09:56  amodigli
 * removed compilation warnings
 *
 * Revision 1.34  2007/05/14 15:57:15  jmlarsen
 * Avoid tracing orders at very edge of chip
 *
 * Revision 1.33  2007/04/12 14:02:24  jmlarsen
 * Made robust against input orders outside image
 *
 * Revision 1.32  2007/04/12 12:02:09  jmlarsen
 * Decreased verbosity
 *
 * Revision 1.31  2007/04/10 07:07:25  jmlarsen
 * Changed interface of polynomial_regression_2d()
 *
 * Revision 1.30  2007/03/30 07:07:28  jmlarsen
 * Fixed mixed code and variable definitions
 *
 * Revision 1.29  2007/03/28 14:02:21  jmlarsen
 * Removed unused parameter
 *
 * Revision 1.28  2007/03/28 11:39:09  jmlarsen
 * Killed MIDAS flag, removed dead code
 *
 * Revision 1.27  2007/03/05 10:17:03  jmlarsen
 * Support slope parameter in 1d fitting
 *
 * Revision 1.26  2007/02/26 11:56:39  jmlarsen
 * Made fitting (even) more robust against points with low sigma
 *
 * Revision 1.25  2007/01/17 13:26:18  jmlarsen
 * Added comment
 *
 * Revision 1.24  2007/01/15 08:46:25  jmlarsen
 * More robust polynomial fitting
 *
 * Revision 1.23  2006/11/23 10:04:31  jmlarsen
 * Minor message change
 *
 * Revision 1.22  2006/11/15 15:02:14  jmlarsen
 * Implemented const safe workarounds for CPL functions
 *
 * Revision 1.20  2006/11/15 14:04:08  jmlarsen
 * Removed non-const version of parameterlist_get_first/last/next which is already
 * in CPL, added const-safe wrapper, unwrapper and deallocator functions
 *
 * Revision 1.19  2006/11/13 14:23:55  jmlarsen
 * Removed workarounds for CPL const bugs
 *
 * Revision 1.18  2006/11/06 15:19:41  jmlarsen
 * Removed unused include directives
 *
 * Revision 1.17  2006/08/23 09:33:03  jmlarsen
 * Renamed local variables shadowing POSIX reserved names
 *
 * Revision 1.16  2006/08/17 14:40:06  jmlarsen
 * Added missing documentation
 *
 * Revision 1.15  2006/08/17 14:33:28  jmlarsen
 * Added missing opening bracket
 *
 * Revision 1.14  2006/08/17 13:56:53  jmlarsen
 * Reduced max line length
 *
 * Revision 1.13  2006/08/17 09:18:27  jmlarsen
 * Removed CPL2 code
 *
 * Revision 1.12  2006/08/10 10:52:41  jmlarsen
 * Removed workaround for cpl_image_get_bpm
 *
 * Revision 1.11  2006/08/08 11:27:18  amodigli
 * upgrade to CPL3
 *
 * Revision 1.10  2006/07/14 12:22:17  jmlarsen
 * Do not use uncertainties in linear fit
 *
 * Revision 1.9  2006/07/03 14:20:39  jmlarsen
 * Exclude bad pixels from order tracing
 *
 * Revision 1.8  2006/05/12 15:05:49  jmlarsen
 * Pass image bpm as extra parameter to fitting routine for efficiency reasons
 *
 * Revision 1.7  2006/04/24 09:34:26  jmlarsen
 * Adapted to new interface of gaussian fitting routine
 *
 * Revision 1.6  2006/04/10 12:38:43  jmlarsen
 * Minor layout change
 *
 * Revision 1.5  2006/04/06 08:44:16  jmlarsen
 * Renamed shadowing variables
 *
 * Revision 1.4  2006/03/24 14:12:18  jmlarsen
 * Use MIDAS default values for polynomial degree if MIDAS flag is set
 *
 * Revision 1.3  2006/03/03 13:54:11  jmlarsen
 * Changed syntax of check macro
 *
 * Revision 1.2  2006/02/15 13:19:15  jmlarsen
 * Reduced source code max. line length
 *
 * Revision 1.1  2006/02/03 07:46:30  jmlarsen
 * Moved recipe implementations to ./uves directory
 *
 * Revision 1.42  2006/01/25 16:15:59  jmlarsen
 * Changed interface of gauss.fitting routine
 *
 * Revision 1.41  2006/01/19 08:47:24  jmlarsen
 * Inserted missing doxygen end tag
 *
 * Revision 1.40  2006/01/12 15:41:14  jmlarsen
 * Moved gauss. fitting to irplib
 *
 * Revision 1.39  2005/12/19 16:17:55  jmlarsen
 * Replaced bool -> int
 *
 */

/*----------------------------------------------------------------------------*/
/**
 * @addtogroup uves_orderpos
 */
/*----------------------------------------------------------------------------*/
/**@{*/

#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif

#include <uves_orderpos_follow.h>

#include <uves_plot.h>
#include <uves_utils.h>
#include <uves_utils_wrappers.h>
#include <uves_error.h>
#include <uves_msg.h>

#include <cpl.h>
#include <math.h>
#include <float.h>

static cpl_table * trace_order(const cpl_table *ordertable, int order,
                   const cpl_image *inputimage, const cpl_image *noise,
                   const cpl_binary *image_bad,
                   int TRACESTEP, 
                   double MAXGAP);
static int            count_orders(const cpl_table *tracetable);
static double         fit_order_linear(cpl_table *singletrace, int order, double KAPPA,
                                       double *slope);
static int      get_xcenter(int nx, int ny, cpl_table *ordertab, int row);
static int      get_ycenter(int nx, int ny, cpl_table *ordertab, int row);
static int  get_orderlength(int nx, int ny, cpl_table *ordertab, int row);
static double estimate_threshold(const cpl_image *inputimage, 
                 const cpl_image *nosie, 
                 cpl_table *ordertable, 
                 int row, double relative_threshold);
static bool find_centroid(const cpl_image *inputimage, 
              const cpl_image *noise,
              const cpl_binary *image_bad, 
              double threshold, int spacing, int x, double *yguess, 
              double *dY);

/*----------------------------------------------------------------------------*/
/**
   @brief    Trace all orders
   @param    inputimage                The input echelle image
   @param    noise                     Noise of input image
   @param    ordertable                The basic order table (one row pr. order)
   @param    TRACESTEP                 See @c trace_order()
   @param    MINTHRESH                 See @c trace_order()
   @param    MAXGAP                    See @c trace_order()
   @param    MAXRMS                    Individual orders are rejected if
                                       RMS of a linear fit of the order is larger than
                       @em MAXRMS * @em mRMS, where @em mRMS is the median
                       of all order's RMSs.
   @param    DEFPOL1                   Degree of global fit (as function of x)
                                       If negative, will be set to actual degree used
   @param    DEFPOL2                   Degree of global fit (as function of order number)
                                       If negative, will be set to actual degree used
   @param    KAPPA                     Kappa used for kappa-sigma clipping of linear 
                                       fits and final global fit
   @param    bivariate_fit             (output) The global fit of order positions
   @param    orders_traced             (output) Number of orders detected
   @return   The order trace table, or NULL on error

   This function creates the order trace table starting from the basic 
   @em ordertable which contains the (guess) order line slopes and intersepts.

   - First, all orders are traced in both directions starting from the center which is
     inferred from the @em 'Slope' and @em 'Intersept' columns of the @em ordertable .
     The detection threshold used for a certain order is
     @em min + @em MINTHRESH * (@em max - @em min), where @em min and @em max are
     the minimum and maximum pixel values at the center column of that order.
     See also @c trace_order() and @c estimate_threshold() .

   - Then each order is fitted with a straight line, and the entire order is rejected
     if the RMS is too large.

   - A global polynomial of degree (@em DEFPOL1, @em DEFPOL2) is fitted to all orders,
     and individual points are rejected using kappa-sigma clipping.
   
 */
/*----------------------------------------------------------------------------*/
cpl_table *
uves_locate_orders(const cpl_image *inputimage, 
                   const cpl_image *noise,
                   cpl_table *ordertable, 
                   int TRACESTEP, 
                   double MINTHRESH,
                   double MAXGAP,
                   double MAXRMS, 
                   int *DEFPOL1, 
                   int *DEFPOL2, 
                   double KAPPA, 
                   polynomial **bivariate_fit, 
                   int *orders_traced)
{
    cpl_table *tracetable  = NULL;  /* The result */
    cpl_table *singletrace = NULL;  /* Location of one order */
    cpl_table *temp        = NULL;  /* Temporary  */
    const cpl_mask *image_badmap = NULL;
    const cpl_binary *image_bad  = NULL;
    int N;  /* Initial number of orders detected */

    double mse, red_chisq;
    int order;

    /* Check input */
    assure_nomsg( inputimage != NULL, CPL_ERROR_NULL_INPUT);
    assure_nomsg( noise != NULL, CPL_ERROR_NULL_INPUT);
    assure( cpl_image_get_size_x(inputimage) == cpl_image_get_size_x(noise) &&
            cpl_image_get_size_y(inputimage) == cpl_image_get_size_y(noise),
            CPL_ERROR_INCOMPATIBLE_INPUT, 
            "Image sizes are %" CPL_SIZE_FORMAT "x%" CPL_SIZE_FORMAT " and %" CPL_SIZE_FORMAT "x%" CPL_SIZE_FORMAT "",
            cpl_image_get_size_x(inputimage), cpl_image_get_size_x(noise),
            cpl_image_get_size_y(inputimage), cpl_image_get_size_y(noise));

    assure_nomsg( ordertable != NULL, CPL_ERROR_NULL_INPUT);
    assure( cpl_table_get_ncol(ordertable) == 4, 
                  CPL_ERROR_ILLEGAL_INPUT,
                  "%" CPL_SIZE_FORMAT " columns found. 4 expected",
                  cpl_table_get_ncol(ordertable));
    assure( cpl_table_has_column(ordertable, "Intersept"),
                  CPL_ERROR_DATA_NOT_FOUND,
                  "Missing column Intersept");
    assure( cpl_table_has_column(ordertable, "Slope"),
                  CPL_ERROR_DATA_NOT_FOUND,
                  "Missing column Slope");
    assure( cpl_table_has_column(ordertable, "Order"),
                  CPL_ERROR_DATA_NOT_FOUND,
                  "Missing column Order");
    assure( cpl_table_has_column(ordertable, "Spacing"),
                  CPL_ERROR_DATA_NOT_FOUND,
                  "Missing column Spacing");
    assure_nomsg( DEFPOL1 != NULL, CPL_ERROR_NULL_INPUT );
    assure_nomsg( DEFPOL2 != NULL, CPL_ERROR_NULL_INPUT );

    image_badmap = cpl_image_get_bpm_const(inputimage);
    image_bad    = cpl_mask_get_data_const(image_badmap);

    N = cpl_table_get_nrow(ordertable);

    *bivariate_fit = NULL;
    
    /* Initialise result table */
    check(( tracetable = cpl_table_new(0),
        cpl_table_new_column(tracetable, "Order"          , CPL_TYPE_INT),
        cpl_table_new_column(tracetable, "X"              , CPL_TYPE_INT),
        cpl_table_new_column(tracetable, "Y"              , CPL_TYPE_DOUBLE),
        cpl_table_new_column(tracetable, "dY"             , CPL_TYPE_DOUBLE),
        cpl_table_new_column(tracetable, "Residual_Square", CPL_TYPE_DOUBLE),
        cpl_table_new_column(tracetable, "OrderRMS"       , CPL_TYPE_DOUBLE),
            cpl_table_new_column(tracetable, "OrderSlope"     , CPL_TYPE_DOUBLE)),  
      /* The order's RMS (from linear fit) */
      "Could not initialize order trace table");
    
    /* Info about the order */
    check(( cpl_table_new_column(ordertable, "Xcenter",      CPL_TYPE_INT),
        cpl_table_new_column(ordertable, "Ycenter",      CPL_TYPE_INT),
        cpl_table_new_column(ordertable, "OrderLength",  CPL_TYPE_INT),
        cpl_table_new_column(ordertable, "Threshold",    CPL_TYPE_DOUBLE),
        cpl_table_new_column(ordertable, "MinThreshold", CPL_TYPE_DOUBLE),
        cpl_table_new_column(ordertable, "RMS",          CPL_TYPE_DOUBLE),
        cpl_table_new_column(ordertable, "TraceSlope",   CPL_TYPE_DOUBLE)),
        "Could not add columns to order table");
    
    *orders_traced = 0;

    /* Trace all orders and make a linear fit */
    for (order = 1; order <= N; order++)
    {
            /* Calculate parameters used for tracing */
        int nx = cpl_image_get_size_x(inputimage);
        int ny = cpl_image_get_size_y(inputimage);
        int points_traced = 0;
            int xc = get_xcenter (nx, ny, ordertable, order - 1);
            int yc = get_ycenter (nx, ny, ordertable, order - 1);
            
            check(( cpl_table_set_int(ordertable, "Xcenter"     , order - 1, xc),
                    /* Order n is at row n-1 */
                    cpl_table_set_int(ordertable, "Ycenter"     , order - 1, yc),
                    cpl_table_set_int(ordertable, "OrderLength" , order - 1, 
                                      get_orderlength (nx, ny, ordertable, order - 1))),
                  "Could not calculate order line geometry");
            
            if (!(1 <= xc && xc <= nx && 1 <= yc && yc <= ny))
                {
                    uves_msg_warning("Order %d: Center of order (%d, %d) is outside image "
                                     "(intersept = %.2f, slope = %f)",
                                     order, xc, yc, 
                                     cpl_table_get_double(ordertable, "Intersept", order-1, NULL),
                                     cpl_table_get_double(ordertable, "Slope", order-1, NULL));
                }
            else
                {
                    check( cpl_table_set_double(
                               ordertable, "Threshold"   , order - 1, 
                               estimate_threshold(inputimage, noise, ordertable, order - 1, -1/*not used*/)),
                           "Could not calculate max. threshold");
                    check( cpl_table_set_double(
                               ordertable, "MinThreshold", order - 1,
                               estimate_threshold(inputimage, noise, ordertable, order - 1, MINTHRESH)),
                           "Could not calculate min. threshold");
                }
        
        /* Trace this order */
        uves_free_table(&singletrace);
        check( singletrace = trace_order(ordertable,
                         order,
                         inputimage,
                         noise,
                         image_bad,
                         TRACESTEP,
                                             MAXGAP),
           "Error occured while tracing order #%d", order);

        check(  points_traced = cpl_table_get_nrow(singletrace), "Could not read table size");

        passure( cpl_table_get_ncol(singletrace) == 3, "%" CPL_SIZE_FORMAT "", cpl_table_get_ncol(singletrace));
        passure( cpl_table_has_column(singletrace, "X"), " ");
        passure( cpl_table_has_column(singletrace, "Y"), " ");
        passure( cpl_table_has_column(singletrace, "dY"), " ");
        
        /* If no points could be located, issue a warning
           and continue with the next order  */
        if (points_traced == 0)
        {
            uves_msg_warning("Could not trace order #%d", order);
            check( cpl_table_set_invalid(ordertable, "RMS", order - 1),
               "Could not flag order %d RMS as invalid", order);
        }
        else 
        { /* At least one x-position of this order was traced */
            double rms=0;
                    double slope=0;
            
            /* Fit order (linear) and write RMS to trace 
               table and to (hough) order table */
            check( rms = fit_order_linear(singletrace, order, KAPPA, &slope),
               "Creating linear fit of order #%d failed", order);

            check(( cpl_table_set_double(ordertable, "RMS", order - 1, rms),
                cpl_table_fill_column_window_double(singletrace, "OrderRMS", 
                                0, points_traced, rms)),
              "Could not write RMS of order #%d to tables", order);
            
            check(( cpl_table_set_double(ordertable, "TraceSlope", order - 1, slope),
                cpl_table_fill_column_window_double(singletrace, "OrderSlope", 
                                0, points_traced, slope)),
              "Could not write slope of order #%d to tables", order);
            

            passure( cpl_table_get_ncol(singletrace) == 7, "%" CPL_SIZE_FORMAT "",
                 cpl_table_get_ncol(singletrace));
            passure( cpl_table_has_column(singletrace, "X"), " ");
            passure( cpl_table_has_column(singletrace, "Y"), " ");
            passure( cpl_table_has_column(singletrace, "dY"), " ");
            passure( cpl_table_has_column(singletrace, "Linear fit"), " ");
            passure( cpl_table_has_column(singletrace, "Residual_Square"), " ");
            passure( cpl_table_has_column(singletrace, "OrderRMS"), " ");
            passure( cpl_table_has_column(singletrace, "OrderSlope"), " ");

            /* Remove unnecessary column before appending */
            check( cpl_table_erase_column(singletrace, "Linear fit"),
               "Could not delete column 'Linear fit'");
            
            /* Write current order number to single order table */
            check(( cpl_table_new_column(singletrace, "Order", CPL_TYPE_INT),
                cpl_table_fill_column_window_int(
                singletrace, "Order", 
                0, cpl_table_get_nrow(singletrace), order)
                  ),
              "Could not create new column 'Order'");
            
            /* The two tables now contain the same columns */
            passure( cpl_table_compare_structure(singletrace, tracetable) == 0, " ");
            
            /* Append to 'tracetable' */
            check( cpl_table_insert(tracetable, singletrace,
                        cpl_table_get_nrow(tracetable)), 
               "Could not append single order #%d to trace table", order);
            
            *orders_traced += 1;
        }
        
    }/*  for ... order */
    
    /* Plot initial (before rejection) order tracing */
    check( uves_plot_table(tracetable, "X", "Y",
               "Initial trace (%d orders)", *orders_traced),
       "Plotting failed");

    /* The trace table now contains these columns */
    passure( cpl_table_get_ncol(tracetable) == 7, "%" CPL_SIZE_FORMAT "", cpl_table_get_ncol(tracetable));
    passure( cpl_table_has_column(tracetable, "X"), " ");
    passure( cpl_table_has_column(tracetable, "Order"), " ");
    passure( cpl_table_has_column(tracetable, "Y"), " ");
    passure( cpl_table_has_column(tracetable, "dY"), " ");
    passure( cpl_table_has_column(tracetable, "Residual_Square"), " ");
    passure( cpl_table_has_column(tracetable, "OrderRMS"), " ");
    passure( cpl_table_has_column(tracetable, "OrderSlope"), " ");
    
    assure(*orders_traced >= 1, CPL_ERROR_ILLEGAL_OUTPUT, "No orders could be traced");
    
    /* Remove badly traced orders from 'tracetable' */
    {
    double maxrms;
    int orders_rejected;
    check( maxrms = 
           uves_max_double(0.05, MAXRMS * cpl_table_get_column_median(ordertable, "RMS")),
           "Could not read median RMS");
    
    uves_msg_debug("Maximum admissible RMS is %.2f pixels", maxrms);
    
    /* Select orders with RMS > maxrms */
    check( orders_rejected = uves_select_table_rows(
           ordertable, "RMS", CPL_GREATER_THAN, maxrms),
           "Could not select rows in order table");
    
    /* Delete rows from trace table */
    if (orders_rejected > 0) 
        {
        uves_msg_warning("%d order(s) rejected because RMS "
                 "(from linear fit) was too large", orders_rejected);
        
        /* Delete rejected orders from 'tracetable' */
        check(  uves_erase_table_rows(tracetable, "OrderRMS", 
                          CPL_GREATER_THAN, maxrms),
            "Could not erase bad orders from trace table");
        
        /* Don't remove from 'ordertable' */
        }
    else
        {
        uves_msg_debug("All RMSs are less than %.2f", maxrms);
        }


        /* Reject based on line slope 
           (this is not the slope from a Hough transform
           but the slope measured after tracing the order)
        */
        check_nomsg( orders_rejected = 
               uves_select_table_rows(
                   ordertable, "TraceSlope", CPL_GREATER_THAN, 0.5) +
               uves_select_table_rows(
                   ordertable, "TraceSlope", CPL_LESS_THAN, -0.5));
        
        if (orders_rejected > 0) {
            uves_msg_warning("%d order(s) rejected because slope was outside [-0.5 ; 0.5]",
                             orders_rejected);

            check_nomsg( uves_erase_table_rows(tracetable, "OrderSlope",
                                         CPL_GREATER_THAN, 0.5));
            check_nomsg( uves_erase_table_rows(tracetable, "OrderSlope",
                                         CPL_LESS_THAN, -0.5));
        }
        else {
            uves_msg_debug("All line slopes are within [-0.5 ; 0.5]");
        }
    }

    /* Remove points with too low 'dY', 
     * they would have too much weight in fit.
     */
    {
    double dy_median = cpl_table_get_column_median(tracetable, "dY");
        double threshold = 0.40*dy_median;
        int nreject;

    check_nomsg( nreject = uves_erase_table_rows(tracetable, "dY", CPL_LESS_THAN, 
                                                     threshold) );

        uves_msg_debug("Rejected %d points with dY less than %f pixels (median = %f pixels)",
                       nreject, threshold, dy_median);
    }

    /* Auto-detect optimal pol. degree if it is negative 
     * (i.e. not specified)
     */
    if (*DEFPOL1 < 0 || *DEFPOL2 < 0)
    {
        int deg1, deg2;            /* Current degrees                            */
        int new_deg1, new_deg2;    /* New degrees                                */
        double red_chisq1, mse1;   /* Reduced chi^sq, mse  for  (DEG1+1, DEG2  ) */
        double red_chisq2, mse2;   /* Reduced chi^sq, mse  for  (DEG1  , DEG2+1) */
        double red_chisq3, mse3;   /* Reduced chi^sq, mse  for  (DEG1+1, DEG2+1) */
        bool adjust1 = (*DEFPOL1 < 0);   /* Flags indicating if DEFPOL1/DEFPOL2
                           should be adjusted */
        bool adjust2 = (*DEFPOL2 < 0);   /*   (or is held constant)               */
        int finished;                   /* 0 = finished, 
                           1 = moved to (DEG1+1, DEG2  )
                           2 = moved to (DEG1  , DEG2+1)
                           3 = moved to (DEG1+1, DEG2+1)         */
        int number_of_orders  = 0;      /* The number of order lines left after 
                           kappa-sigma clipping */
        int number_of_orders1 = 0;
        int number_of_orders2 = 0;
        int number_of_orders3 = 0;

        if (adjust1)
        {
            /* Initialize */
            *DEFPOL1 = 1;
            deg1 = 1; 
        }
        else
        {
            /* Don't modify */
            deg1 = *DEFPOL1;
        }
        if (adjust2)
        {
            /* Initialize */
            *DEFPOL2 = 1;
            deg2 = 1; 
        }
        else
        {
            /* Don't modify */
            deg2 = *DEFPOL2;
        }

        uves_free_table(&temp);
        temp = cpl_table_duplicate(tracetable);
        uves_polynomial_delete(bivariate_fit);
        check( *bivariate_fit = uves_polynomial_regression_2d(
               temp,
               "X", "Order", "Y", "dY",
               deg1,
               deg2,
               NULL, NULL, NULL,            /* No extra columns       */
               &mse, &red_chisq,
               NULL,                        /* No variance polynomial */
               KAPPA, -1),
           "Error fitting orders");

        check( number_of_orders = count_orders(temp),
           "Error counting orders");
        
        uves_msg_low("(%d, %d)-degree: RMS = %.3f pixels. "
             "Red.chi^2 = %.2f (%d orders) *",
             deg1,
             deg2,
             sqrt(mse),
             red_chisq,
             number_of_orders);

        /* Find best values of deg1, deg2 less than or equal to 8,8
           (the fitting algorithm is unstable after this point, anyway) 

        */
        do
        {
            int maxdegree = 6;
            finished = 0;
            
            adjust1 = adjust1 && (deg1 + 1 <= maxdegree);
            adjust2 = adjust2 && (deg2 + 1 <= maxdegree);
            
            /* Try (deg1+1, deg2) */
            if (adjust1)
            {
                uves_free_table(&temp);
                temp = cpl_table_duplicate(tracetable);
                uves_polynomial_delete(bivariate_fit);
                *bivariate_fit = uves_polynomial_regression_2d(
                temp,
                "X", "Order", "Y", "dY",
                deg1 + 1,
                deg2,
                NULL, NULL, NULL,  /* extra columns */
                &mse1, &red_chisq1,
                NULL,              /* variance polynomial */
                KAPPA, -1);

                if (cpl_error_get_code() == CPL_ERROR_SINGULAR_MATRIX)
                {
                    uves_error_reset();
                    mse1 = -1;
                    red_chisq1 = DBL_MAX/2;
                }
                else
                {
                    assure( cpl_error_get_code() == CPL_ERROR_NONE,
                        cpl_error_get_code(),
                        "Error fitting orders");

                    check( number_of_orders1 = count_orders(temp),
                   "Error counting orders");
                }
            }

            /* Try (deg1, deg2+1) */
            if (adjust2)
            {
                uves_free_table(&temp);
                temp = cpl_table_duplicate(tracetable);
                uves_polynomial_delete(bivariate_fit);
                *bivariate_fit = uves_polynomial_regression_2d(
                temp,
                "X", "Order", "Y", "dY",
                deg1,
                deg2 + 1,
                NULL, NULL, NULL,            /* No extra columns       */
                &mse2, &red_chisq2,
                NULL,                        /* No variance polynomial */
                KAPPA, -1);

                if (cpl_error_get_code() == CPL_ERROR_SINGULAR_MATRIX)
                {
                    uves_error_reset();
                    mse2 = -1;
                    red_chisq2 = DBL_MAX/2;
                }
                else
                {
                    assure( cpl_error_get_code() == CPL_ERROR_NONE,
                        cpl_error_get_code(),
                        "Error fitting orders");
                    
                    check( number_of_orders2 = count_orders(temp),
                       "Error counting orders");            
                }
            }
            
            /* Try (deg1+1, deg2+1) */
            if (adjust1 && adjust2)
            {
                uves_free_table(&temp);
                temp = cpl_table_duplicate(tracetable);
                uves_polynomial_delete(bivariate_fit);
                *bivariate_fit = uves_polynomial_regression_2d(
                temp,
                "X", "Order", "Y", "dY",
                deg1 + 1,
                deg2 + 1,
                NULL, NULL, NULL,       /* extra columns       */
                &mse3, &red_chisq3,
                NULL,                   /* variance polynomial */
                KAPPA, -1);

                if (cpl_error_get_code() == CPL_ERROR_SINGULAR_MATRIX)
                {
                    uves_error_reset();
                    mse3 = -1;
                    red_chisq3 = DBL_MAX/2;
                }
                else
                {
                    assure( cpl_error_get_code() == CPL_ERROR_NONE,
                        cpl_error_get_code(),
                        "Error fitting orders");
                    
                    check( number_of_orders3 = count_orders(temp),
                       "Error counting orders");
                }
            }
            
            /* If fit is significantly better (say, 10% improvement
             * in chi^2) in either direction, (in (degree,degree)-space) 
             * then move in that direction.
             *
             * First try to move one step horizontal/vertical, 
             * otherwise try to move
             * diagonally (i.e. increase both degrees)
             *
             * Assign to DEFPOL1/2 only if enough orders were detected.
             */
            
            new_deg1 = deg1;
            new_deg2 = deg2;
            if (adjust1 && mse1 >= 0 && (red_chisq - red_chisq1)/red_chisq > 0.1 &&
            red_chisq1 <= red_chisq2)
            {
                new_deg1++;
                mse = mse1;
                red_chisq = red_chisq1;
                finished = 1;

                if (number_of_orders1 >= number_of_orders)
                {
                    *DEFPOL1 = new_deg1;
                    *DEFPOL2 = new_deg2;
                    number_of_orders = number_of_orders1;
                }
            }
            else if (adjust2 && mse2 >= 0 && (red_chisq - red_chisq2)/red_chisq > 0.1 && 
                 red_chisq2 < red_chisq1)
            {
                new_deg2++;
                mse = mse2;
                red_chisq = red_chisq2;
                finished = 2;

                if (number_of_orders2 >= number_of_orders)
                {
                    *DEFPOL1 = new_deg1;
                    *DEFPOL2 = new_deg2;
                    number_of_orders = number_of_orders2;
                }
            }
            else if (adjust1 && adjust2 && 
                 mse3 >= 0 && (red_chisq - red_chisq3)/red_chisq > 0.1)
            {
                new_deg1++;
                new_deg2++;
                mse = mse3;
                red_chisq = red_chisq3;
                finished = 3;

                if (number_of_orders3 >= number_of_orders)
                {
                    *DEFPOL1 = new_deg1;
                    *DEFPOL2 = new_deg2;
                    number_of_orders = number_of_orders3;
                }
            }

            /* Print mse, chi^2, ...
             * Add a star '*' at the better solution (if any).
             */
            if (adjust1)
            {
                if (mse1 >= 0)
                {
                    uves_msg_low("(%d, %d)-degree: RMS = %.3f pixels. "
                         "Red.chi^2 = %.3f (%d orders)%s",
                         deg1 + 1,
                         deg2,
                         sqrt(mse1),
                         red_chisq1,
                         number_of_orders1,
                         (finished == 1) ? " *" : "");
                }
                else
                {
                    uves_msg_low("(%d, %d)-degree: Singular matrix",
                         deg1 + 1,
                         deg2);
                }
            }

            if (adjust2)
            {
                if (mse2 >= 0)
                {
                    uves_msg_low("(%d, %d)-degree: RMS = %.3f pixels. "
                         "Red.chi^2 = %.3f (%d orders)%s",
                         deg1,
                         deg2 + 1,
                         sqrt(mse2),
                         red_chisq2,
                         number_of_orders2,
                         (finished == 2) ? " *" : "");
                }
                else
                {
                    uves_msg_low("(%d, %d)-degree: Singular matrix",
                         deg1,
                         deg2 + 1);
                }
            }
            
            if (adjust1 && adjust2)
            {
                if (mse3 >= 0)
                {
                    uves_msg_low("(%d, %d)-degree: RMS = %.3f pixels. "
                         "Red.chi^2 = %.3f (%d orders)%s",
                         deg1 + 1,
                         deg2 + 1,
                         sqrt(mse3),
                         red_chisq3,
                         number_of_orders3,
                         (finished == 3) ? " *" : "");
                }
                else
                {
                    uves_msg_low("(%d, %d)-degree: Singular matrix",
                         deg1 + 1,
                         deg2 + 1);
                }
            }
            
            if (finished != 0) 
            {
                uves_msg_debug("Moved to degree (%d, %d), finished = %d, "
                                           "DEFPOL = %d, %d", 
                       new_deg1, new_deg2, finished, *DEFPOL1, *DEFPOL2);
            }
            
            deg1 = new_deg1;
            deg2 = new_deg2;
            
        } while (finished != 0);
        
        uves_msg_low("Using degree (%d, %d)", *DEFPOL1, *DEFPOL2);

        }/* endif auto degree */

    /* Make the final fit */
    uves_polynomial_delete(bivariate_fit);
    check( *bivariate_fit = uves_polynomial_regression_2d(tracetable,
                              "X", "Order", "Y", "dY",
                              *DEFPOL1,
                              *DEFPOL2,
                              "Yfit", NULL, "dYfit_Square",
                              &mse, &red_chisq,
                              NULL,  /* variance polynomial */
                              KAPPA, -1),
       "Error fitting orders");

    uves_msg("RMS error of (%d, %d)-degree fit is %.3f pixels. Reduced chi^2 is %.3f",
         *DEFPOL1,
         *DEFPOL2,
         sqrt(mse),
         red_chisq);
    
    /* Warn about bad fit */
    if (sqrt(mse) > 0.3)
    {
        uves_msg_warning("RMS of bivariate fit (%.2f pixels) "
                 "is larger than 0.3 pixels", sqrt(mse));
    }
    if (red_chisq < .01)
    {
        uves_msg_warning("Reduced chi^2 of fit is less than 1/100: %f", red_chisq);
    }
    if (red_chisq > 100)
    {
        uves_msg_warning("Reduced chi^2 of fit is greater than 100: %f", red_chisq);
    }
    
    /* Create residual column  'Residual' := 'Y' - 'Yfit' */
    check(( cpl_table_duplicate_column(tracetable, "Residual", tracetable, "Y"),
        cpl_table_subtract_columns(tracetable, "Residual", "Yfit")),
        "Error calculating residuals of fit");

    /* Show how many orders were traced */
    {
    check( *orders_traced =  count_orders(tracetable),
           "Error counting orders");
    
    uves_msg("%d order(s) were traced", *orders_traced);
    if (*orders_traced < N)
        {
        uves_msg_warning("Rejected %d order(s)", N - *orders_traced);
        }
    }

    /* Plot results */
    check( uves_plot_table(tracetable, "X", "Yfit", "%d orders detected", *orders_traced),
       "Plotting failed");
    check( uves_plot_table(tracetable, "X", "Residual", 
               "Residual of fit (RMS = %.3f pixels; red.chi^2 = %f)",
               sqrt(mse), red_chisq), "Plotting failed");
    check( uves_plot_table(tracetable, "Y", "Residual", 
               "Residual of fit (RMS = %.3f pixels; red.chi^2 = %f)",
               sqrt(mse), red_chisq), "Plotting failed");
    
  cleanup:
    uves_free_table(&temp);
    uves_free_table(&singletrace);
    if (cpl_error_get_code() != CPL_ERROR_NONE)    
    {
        uves_free_table(&tracetable);
    }

    return tracetable;
}


/*----------------------------------------------------------------------------*/
/**
   @brief    Count orders in trace table
   @param    tracetable         The order trace table
   @return   Number of different values present in the column @em 'Order'
   
/*/
/*----------------------------------------------------------------------------*/
static int
count_orders(const cpl_table *tracetable)
{
    int number = 0;
    int previous = -1;
    int row;
   
    passure( tracetable != NULL, " ");
    passure( cpl_table_has_column(tracetable, "Order"), " ");
 
    for (row = 0; row < cpl_table_get_nrow(tracetable); row++)
    {
    int current;
    current = cpl_table_get_int(tracetable, "Order", row, NULL);
    if (current != previous)
        {
        number++;
        }
    previous = current;
    }
    
  cleanup:
    return number;

}


/*----------------------------------------------------------------------------*/
/**
   @brief    Make a linear fit of an order trace
   @param    singletrace         Order table containing a single order
   @param    order               (Relative) number of the order to fit (used only for
                                 messaging)
   @param    KAPPA               Value used for kappa-sigma clipping
   @param    slope               (output) from linear fit
   @return   The RMS error of the fit, or undefined on error.
   
   The function fits a straight line to the specified order and returns
   the RMS error of the fit. The columns @em 'Polynomial fit', @em 'Residual_Square'
   and @em 'OrderRMS' (containing the RMS at every row) are added to the table.    

*/
/*----------------------------------------------------------------------------*/

static double
fit_order_linear(cpl_table *singletrace, 
                 int order, 
                 double KAPPA,
                 double *slope)
{
    double mse = 0;              /* mean square error of the fit             */
    double intersept;
    cpl_table *temp = NULL;      /* Don't remove rows from the input table   */
    polynomial *pol = NULL;      /* The 1d polynomial                        */   

    passure( slope != NULL, " ");
    passure( cpl_table_get_ncol(singletrace) == 3, "%" CPL_SIZE_FORMAT "", cpl_table_get_ncol(singletrace));
    passure( cpl_table_has_column(singletrace, "X"), " ");
    passure( cpl_table_has_column(singletrace, "Y"), " ");
    passure( cpl_table_has_column(singletrace, "dY")," ");

    check( temp = cpl_table_duplicate(singletrace),
       "Error cloning table");

    if (cpl_table_get_nrow(temp) == 1)
    {
        /* Only one point: create another point at next table row (1) to 
           make linear fitting is possible. */
        check(( cpl_table_set_size(temp, 2),
            cpl_table_set_int   (temp, "X",  1, uves_max_int(
                         cpl_table_get_int   (temp, "X", 0, NULL) - 1, 1)),
            cpl_table_set_double(temp, "Y",  1,
                     cpl_table_get_double(temp, "Y", 0, NULL)),
            cpl_table_set_double(temp, "dY", 1,
                     cpl_table_get_double(temp, "dY",0, NULL))),
            "Could not add point");
    }
    
    /* Make the linear fit. When kappa-sigma clipping, rows
       are removed. Therefore, use a copy of the input table */
    check( pol = uves_polynomial_regression_1d(temp,
                           "X", "Y", NULL,/* Unweighted fit 
                                 for robustness */
                           1,             /* Degree         */
                           NULL, NULL,    /* Fit, residual  */
                           &mse, 
                           KAPPA),
       "Fitting of order %d failed. You may have to increase value of kappa", 
           order);

    intersept = uves_polynomial_get_coeff_1d(pol, 0);
    *slope = uves_polynomial_get_coeff_1d(pol, 1);

    uves_msg_debug("The RMS error of order #%d is %.2f pixels; "
                   "slope = %f; intersept = %f",
                   order, sqrt(mse),
                   *slope, intersept);
    
    /* Write results of fit to input table */
    {
    int i;

    check(( cpl_table_new_column(singletrace, "Linear fit", CPL_TYPE_DOUBLE),
        cpl_table_new_column(singletrace, "Residual_Square", CPL_TYPE_DOUBLE)),
        "Error adding table columns");
    
    for (i = 0; i < cpl_table_get_nrow(singletrace); i++)
        {
        int    x = cpl_table_get_int   (singletrace, "X", i, NULL);
        double y = cpl_table_get_double(singletrace, "Y", i, NULL);

        double linear_fit, residual;

        check (linear_fit = uves_polynomial_evaluate_1d(pol, x),
               "Error evaluating polynomial");

        residual = y - linear_fit;

        check(( cpl_table_set_double(singletrace, "Linear fit", i, linear_fit),
            cpl_table_set_double(singletrace, "Residual_Square",
                         i, residual*residual)),
              "Error updating table");
        }
    }

    /* Add info about the order's RMS+slope for each point */
    check(( cpl_table_new_column(singletrace, "OrderRMS", CPL_TYPE_DOUBLE),
            cpl_table_new_column(singletrace, "OrderSlope", CPL_TYPE_DOUBLE),
        cpl_table_fill_column_window_double(
        singletrace, "OrderRMS", 0, cpl_table_get_nrow(singletrace), sqrt(mse)),
            cpl_table_fill_column_window_double(
        singletrace, "OrderSlope", 0, cpl_table_get_nrow(singletrace), *slope)),
          "Could not create columns OrderRMS and OrderSlope");

    passure( cpl_table_get_ncol(singletrace) == 7, "%" CPL_SIZE_FORMAT "", cpl_table_get_ncol(singletrace));
    passure( cpl_table_has_column(singletrace, "X"), " ");
    passure( cpl_table_has_column(singletrace, "Y"), " ");
    passure( cpl_table_has_column(singletrace, "dY")," ");
    passure( cpl_table_has_column(singletrace, "Linear fit"), " ");
    passure( cpl_table_has_column(singletrace, "Residual_Square"), " ");
    passure( cpl_table_has_column(singletrace, "OrderRMS"), " ");
    passure( cpl_table_has_column(singletrace, "OrderSlope"), " ");
    
  cleanup:
    uves_free_table(&temp);
    uves_polynomial_delete(&pol);
    return sqrt(mse);
        
}

/*----------------------------------------------------------------------------*/
/**
   @brief    Trace a single order line
   @param    ordertable         Basic order table
   @param    order              (Relative) number of order to trace
   @param    inputimage         The input image
   @param    image_bad          image bad pixel map
   @param    noise              Noise of input image
   @param    TRACESTEP          The step size (i.e. x-increment) used when tracing
   @param    MAXGAP             The maximum gap length to jump (in units of image width)
   @return   A table defining the order trace, or NULL on error

   The order is traced in both directions starting from the center of the order (which
   is inferred from the slopes and intersepts read from the provided @em ordertable).

   The order line is sampled at x-positions seperated by the parameter @em TRACESTEP. 
   At each x-position,
   the y-centroid of the order is calculated. The trace stops if the intensity of the
   order line is below a certain threshold value in a range determined by the 
   parameter @em MAXGAP.

   The order is traced as far as possible using the lowest threshold value. 

   The returned table contains three columns, @em 'X', @em 'Y' (describing 
   the trace of the order) and @em 'dY' (uncertainty of @em 'Y').

*/
/*----------------------------------------------------------------------------*/

static cpl_table *
trace_order(const cpl_table *ordertable, int order,
        const cpl_image *inputimage, const cpl_image *noise,
        const cpl_binary *image_bad,
        int TRACESTEP, 
        double MAXGAP)
{
    cpl_table *singletrace = NULL;
    int tracerow;                  /* pointing to the next empty row in the tracetable */
    int DIRECTION;
    double slope;
    double threshold;
    double minthreshold;
    int nx;
    int xcenter;
    int ycenter;
    int orderlength;     /* x-distance between endpoints */
    int order_spacing;   /* approximate distance to next order(s) */
    int xmax, xmin;

    nx = cpl_image_get_size_x(inputimage);
    /* Initialize result */
    check(( singletrace = 
        cpl_table_new(nx/TRACESTEP + 2),
        cpl_table_new_column(singletrace, "X", CPL_TYPE_INT),
        cpl_table_new_column(singletrace, "Y", CPL_TYPE_DOUBLE),
        cpl_table_new_column(singletrace, "dY",CPL_TYPE_DOUBLE),
        tracerow = 0),
        "Could not initialize tracetable");
    
    /* Trace the order */
    /* While less than TRACEITER of the order is traced 
       lower threshold, and try again
       But don't try more than three times    */
    
    /* Order number n is in ordertable row n-1 */
    check((xcenter      = cpl_table_get_int   (ordertable, "Xcenter"     , order - 1, NULL),
           ycenter      = cpl_table_get_int   (ordertable, "Ycenter"     , order - 1, NULL),
           orderlength  = cpl_table_get_int   (ordertable, "OrderLength" , order - 1, NULL),
           order_spacing= cpl_table_get_int   (ordertable, "Spacing"     , order - 1, NULL),
           threshold    = cpl_table_get_double(ordertable, "Threshold"   , order - 1, NULL),
           minthreshold = cpl_table_get_double(ordertable, "MinThreshold", order - 1, NULL)),
      "Reading order table failed");
    
    /* Trace once using the minimum threshold */
    threshold = minthreshold;
    
    
    /*  Clear the trace table */
    tracerow = 0;
        
    xmax = xmin = xcenter;
        
    /* Trace it to the left, trace it to the right */
    for (DIRECTION = -1; DIRECTION <= 1; DIRECTION += 2)  {
        /* Start tracing at this position */
        int x = xcenter;
        double y = (double) ycenter;
        double dy = 0;
        int gap_size = 0;    /* gap size (for jumping) in pixels */
            
        check( slope = cpl_table_get_double(
                   ordertable, "Slope", order - 1, NULL), 
               "Could not read slope from table");
            
        if (xcenter < nx/10 || xcenter > (nx*99)/100) {
            /* Order at very edge of chip. Give up */
            x = 0;

            /* The numbers chosen here: 10% left and 1% right
               are finetuned to the blaze-function of UVES */
        }

        while(1 <= x && x <= nx && gap_size < MAXGAP*nx) {
            bool found;

            check( found = find_centroid(
                       inputimage, noise, image_bad, threshold, 
                       order_spacing, x, &y, &dy),
                   "Could not get order line position");

            /* If found and if
               new slope when including this detection is
               inside [-1;1] */
            if (found && 
                (y - ycenter)/(x - xcenter) > -1 &&
                (y - ycenter)/(x - xcenter) < 1) {
                
                /* Update xmax, xmin */
                xmax = uves_max_int(xmax, x);
                xmin = uves_min_int(xmin, x);
                
                uves_msg_debug("(Order, x, y, dy, threshold) = "
                               "(%d, %d, %f, %f, %f)", 
                               order, x, y, dy, threshold);
                
                if (!(x == xcenter && DIRECTION == 1)) 
                    /* Update table */
                    /* When tracing right, don't insert the 
                       center point again */
                    
                    {
                        cpl_table_set_int   (
                            singletrace, "X", tracerow, x);
                        cpl_table_set_double(
                            singletrace, "Y", tracerow, y);
                        if (dy > 0) {
                            cpl_table_set_double(
                                singletrace, "dY", tracerow, dy);
                        }
                        else {
                            cpl_table_set_invalid(
                                singletrace, "dY", tracerow);
                        }
                        tracerow++;
                    }
                
                gap_size = 0;
                
            }/* If order found */
            else {
                gap_size += TRACESTEP;
            }
            
            /* Initial 'slope' will be the Hough slope */
            x = x + DIRECTION * TRACESTEP;
            y = y + slope*DIRECTION * TRACESTEP;
            
            slope = (y - ycenter)/(x - xcenter);
            
        }/*  while */
    
    }/*  for... DIRECTION */
        
    /* Now width of the trace is (xmax - xmin + 1) */

    uves_msg_debug("%d points were traced in order %d", tracerow, order);
    
    /* Remove the last part of the table (garbage) */
    check( cpl_table_set_size(singletrace, tracerow), "Could not resize tracetable");
    
    /* Set the undetermined 'dY' column values to some value that is not completely off
       (such as the median of all other dY). If there are no other points, set dY
       to 1.0 pixel which effectively excludes the point from later fits. */
    {
    double dy_median;
    
    if (cpl_table_has_valid(singletrace, "dY"))
        {
        /* Invalid column values are excluded from the computation */
        dy_median = cpl_table_get_column_median(singletrace, "dY");
        }
    else
        {
        dy_median = 1.0;
        }

    /* Write median value to all invalid rows */
    cpl_table_select_all(singletrace);
    cpl_table_and_selected_invalid(singletrace, "dY");
    {
            int i;
        for (i = 0; i < cpl_table_get_nrow(singletrace); i++)
        {
            if (cpl_table_is_selected(singletrace, i))
            {
                cpl_table_set_double(singletrace, "dY", i, dy_median);
            }
        }
    }
    }
    
    /* Finally, sort the single order table by X */
    check( uves_sort_table_1(singletrace, "X", false), "Could not sort order table");    
    
  cleanup:
    if (cpl_error_get_code() != CPL_ERROR_NONE)
    {
        uves_free_table(&singletrace);
    }
    
    return singletrace;
}

/*----------------------------------------------------------------------------*/
/**
   @brief    Calculate the length of an order line
   @param    nx                 Width of input image
   @param    ny                 Height of input image
   @param    ordertable         Basic order table
   @param    row                Row of order table to use
   @return   Order line length

   The function returns (@em x1 - @em x0) where @em x1 and @em x0 are the x-coordinates
   of the line's endpoints.

*/
/*----------------------------------------------------------------------------*/
static int 
get_orderlength(int nx, int ny, cpl_table *ordertable, int row)
{
    int x0 = 0, y_0, x1 = 0, y_1;
    double intersept, slope;

    check(( intersept = cpl_table_get_double(ordertable, "Intersept", row, NULL),
        slope     = cpl_table_get_double(ordertable, "Slope", row, NULL)),
        "Could not read line from ordertable");
    
    /* The left endpoint of the order line is... */
    x0 = 1;
    y_0 = uves_round_double(intersept + slope*x0);
    
    /* However, if... */
    if (y_0 < 1)
    {
        y_0 = 1;
        x0 = uves_round_double((y_0 - intersept)/slope); /* y = intersept + slope*x */
    }
    
    /* The right endpoint */
    x1 = nx;
    y_1 = uves_round_double(intersept + slope*nx);
    if (y_1 > ny)
    {
        y_1 = ny;
        x1 = uves_round_double((y_1 - intersept)/slope);
    }
    
  cleanup:
    return (x1 - x0);
}


/*----------------------------------------------------------------------------*/
/**
   @brief    Calculate the x-center of an order
   @param    nx                 Width of input image
   @param    ny                 Height of input image
   @param    ordertable         Basic order table
   @param    row                Row of order table to use
   @return   X-coordinate of the order line center
   
   Note that the line center is different from @em nx / 2, if the order line
   crosses the upper or low boundary of the image.

*/
/*----------------------------------------------------------------------------*/
static int 
get_xcenter(int nx, int ny, cpl_table *ordertable, int row)
{
    int x0, y_0, x1, y_1, xc = 0;
    double intersept, slope;
    check(( intersept = cpl_table_get_double(ordertable, "Intersept", row, NULL),
        slope     = cpl_table_get_double(ordertable, "Slope", row, NULL)),
        "Could not read line from ordertable");
    
    /* The left endpoint of the order line */
    x0 = 1;
    y_0 = uves_round_double(intersept + slope*x0);
        
    /* However, if... */
    if (y_0 < 1)
    {
        y_0 = 1;
        x0 = uves_round_double((y_0 - intersept)/slope); 
            /* y = intersept + slope*x */
    }

    
    /* The right endpoint */
    x1 = nx;
    y_1 = uves_round_double(intersept + slope*nx);

    /* However, if ... */
    if (y_1 > ny)
    {
        y_1 = ny;
        x1 = uves_round_double((y_1 - intersept)/slope);
    }
    
    xc = (x0 + x1)/2;
       
  cleanup:
    return xc;
}

/*----------------------------------------------------------------------------*/
/**
   @brief    Calculate the y-center of an order
   @param    nx                 Width of input image
   @param    ny                 Height of input image
   @param    ordertable         Basic order table
   @param    row                Row of order table to use
   @return   Y-coordinate of the order line center

   See @c get_xcenter() .

*/
/*----------------------------------------------------------------------------*/
static int 
get_ycenter(int nx, int ny, cpl_table *ordertable, int row)
{
    int xc = 0;
    int yc = 0;
    check( xc = get_xcenter(nx, ny, ordertable, row), "Could not find x-center of order");
    
    check( yc = uves_round_double(
           cpl_table_get_double(ordertable, "Slope"    , row, NULL)*xc +
           cpl_table_get_double(ordertable, "Intersept", row, NULL)
           ), "Could not read line from ordertable");
    
  cleanup:
    return yc;
}

/*----------------------------------------------------------------------------*/
/**
   @brief    Calculate threshold for order tracing algorithm
   @param    inputimage         The input image
   @param    noise              Noise of input image
   @param    ordertable         Basic order table
   @param    row                Row of order table to use
   @param    relative_threshold Parameter defining the threshold to calculate (see text)      
   @return   The calculated threshold

   This function calculates the threshold levels used by the order tracing algorithm. 
   The threshold computed is  @em min + @em relative_threshold * (@em max - @em min), 
   where @em min and @em max are the minimum and maximum pixel values at the
   center column of the order. The threshold is always above the noise level.

*/
/*----------------------------------------------------------------------------*/
static double
estimate_threshold(const cpl_image *inputimage, const cpl_image *noise, 
           cpl_table *ordertable, int row, double relative_threshold)
{
    int yupper = 0;
    int ylower = 0;
    int xc, yc;
    int N;
    int ny;
    double returnvalue = 0;
    cpl_stats *stats = NULL;
    
    passure( inputimage != NULL, " ");
    passure( ordertable != NULL, " ");
    passure( cpl_table_get_int(ordertable, "Order", row, NULL) == row+1, "%d %d", 
         cpl_table_get_int(ordertable, "Order", row, NULL), row);

    check( ny = cpl_image_get_size_y(inputimage), "Could not read input image dimension");
    
    check( N = cpl_table_get_nrow(ordertable), "Could not read size of ordertable");
    assure(N > 1, CPL_ERROR_ILLEGAL_INPUT, 
       "Cannot calculate orderspacing with less than 2 (i.e. %d) orders.", N);
    check( xc = cpl_table_get_int(ordertable, "Xcenter", row, NULL), 
       "Could not read x-center of order #%d", row+1);
    check( yc = cpl_table_get_int(ordertable, "Ycenter", row, NULL), 
       "Could not find y-center of order #%d", row+1);
    
    
    /* Set yupper and ylower midway between this and the adjacent orders
     * The y-location of the surrounding orders must be calculated at the center,
     * xc, of the current order
     */
    if (row < N - 1)
    {
        double ynext;
        check(ynext =
          cpl_table_get_double(ordertable, "Slope"    , row + 1, NULL)*xc +
          cpl_table_get_double(ordertable, "Intersept", row + 1, NULL),
          "Could not read line from ordertable row %d", row + 1);
        
        yupper = (int)((yc + (uves_round_double(ynext)-1))/2);  
        /* Midway between this and the next order */
    }

    if (row > 0)
    {
        double yprev;
        check( yprev =
           cpl_table_get_double(ordertable, "Slope"    , row - 1, NULL)*xc +
           cpl_table_get_double(ordertable, "Intersept", row - 1, NULL),
           "Could not read line from ordertable row %d", row - 1);
        
        ylower = (int)((yc + uves_round_double(yprev)-1)/2);
        /* Midway between this and the previous order */
    }

    /* We need to manually set yupper for the highest order
       and ylower for the lowest order */
    if (row == N-1)
    {
        yupper = yc + (yc - ylower);
    }
    if (row == 0)
    {
        ylower = yc - (yupper - yc);
    }
    yupper = uves_min_int(uves_max_int(yupper, 1), ny);
    ylower = uves_min_int(uves_max_int(ylower, 1), ny);

    /* Order lines were originally sorted with respect to intersept. This does not
       necessarily mean that their centers are also sorted (if the Hough algorithm
       detected wrong slopes (which happens if trying to detect too many lines)).
       So check this. */
    assure(yupper > ylower, CPL_ERROR_ILLEGAL_INPUT, 
       "Initially detected order lines intersept!");

    /* Find max and min pixel values between ylower and yupper, then calculate threshold */
    {
    double minval = 0;
    double maxval = 0;
    double noise_level = 0;
    
    /* Find maximum and minimum pixels along center column */
    check( stats = cpl_stats_new_from_image_window(
           inputimage,
           CPL_STATS_MIN | CPL_STATS_MAX | CPL_STATS_MINPOS,
           xc, ylower,   /* Corners of window (FITS convention) (included) */
           xc, yupper),
           "Could not get statistics on image sub-window (%d,%d)-(%d,%d)", 
           xc, ylower, xc, yupper);
    
    check(( minval = cpl_stats_get_min(stats),
        maxval = cpl_stats_get_max(stats)),
           "Could not get minimum and maximum pixel values");

    /* Get noise level at the location of 'minval' */
    {
        int xpos, ypos, pis_rejected;
        xpos = cpl_stats_get_min_x(stats);
        ypos = cpl_stats_get_min_y(stats);
        noise_level = cpl_image_get(noise, xpos, ypos, &pis_rejected);
    }
    
    /* Calculate threshold */
    returnvalue = uves_max_double(minval + relative_threshold * (maxval - minval),
                 (minval + noise_level) + noise_level);
    
    uves_msg_debug("Order: %d \tThreshold: %f \tMinimum: %f \tMaximum: %f"
               " \tNoise: %f \tWindow: (%d, %d)-(%d, %d)",
              row+1, returnvalue, minval, maxval, noise_level, xc, ylower, xc, yupper);
    }
    
  cleanup:
    uves_free_stats(&stats);
    return returnvalue;
}

/*----------------------------------------------------------------------------*/
/**
   @brief    Find order line centroid
   @param    inputimage     The input image
   @param    noise          Noise of input image
   @param    image_bad      Image bad pixel map
   @param    threshold      Detection threshold
   @param    spacing        Approximate distance between orders (near this order)
   @param    x              Find centroid in this column
   @param    yguess         Guess location of centroid, will be updated with centroid location
   @param    dY             The uncertainty (which is 'returned') associated with @em yguess.
                            If this (positive number) cannot be calculated, the variable 
                is set to -1.
   @return   true if the order line could be traced at this @em x, false otherwise

   The function calculates the centroid of the vertical
   strip containing @em yguess.

   All pixels above half peak maximum contribute to the centroid calculation, 
   and the centroid position is refined by making a gaussian fit. Additionally,
   the function returns true, if the peak intensity is more than 10 @em sigma,
   where @em sigma is the RMS error of the gaussian fit.   
*/
/*----------------------------------------------------------------------------*/
static bool
find_centroid(const cpl_image *inputimage, const cpl_image *noise, 
          const cpl_binary *image_bad, 
          double threshold, int spacing, int x, double *yguess, double *dY)
{
    bool returnvalue = true;
    int nx;
    int ny;
    int y;
    double thisvalue = 0;
    int pis_rejected;
    int ylow = 0;
    int yhigh = 0;
    cpl_matrix *covariance = NULL;

    passure( inputimage != NULL, " ");

    nx = cpl_image_get_size_x(inputimage);
    ny = cpl_image_get_size_y(inputimage);

    passure( 1 <= x && x <= nx, "%d %d", x, nx);

    uves_msg_debug("Order location estimate = (%d, %f)", x, *yguess);
    
    /* Start at yguess and move to a local max */

    y = uves_round_double(*yguess);
    if (y < 1 || y > ny)
    {
        returnvalue = false;
    }
    else {
    bool cont;          /* continue? */
    
    do {
        cont = false;
        thisvalue  = cpl_image_get(inputimage, x, y    , &pis_rejected);
        /* Move up? */
        if (y < ny) {
        double uppervalue = cpl_image_get(inputimage, x, y + 1, &pis_rejected);
        if (!pis_rejected && uppervalue > thisvalue)
            {
            y += 1;
            cont = true;
            }
        }
    
        /* Move down? */
        if (y > 1) {
        double lowervalue = cpl_image_get(inputimage, x, y - 1, &pis_rejected);
        if (!pis_rejected && lowervalue > thisvalue)
            {
            y -= 1;
            cont = true;
            }
        }
    
    } while (cont);
    
    /* Now 'thisvalue' is the local maximum */
    
    uves_msg_debug("Local maximum at (%d, %d) (value = %f)\tthreshold = %f", 
               x, y, thisvalue, threshold);
    
    /* Return false if no value above threshold was found */
    if (thisvalue < threshold)
        {
        uves_msg_debug("Order not traced at (%d, %d) (value = %f)\tthreshold = %f",
                   x, y, thisvalue, threshold);
        returnvalue = false;
        }
    else
        { 
                /* Find and use pixels that are above half max */
                double minvalue;
        double sigmaY;   /* Width of peak */

                double mse, rms, chi_sq;
                double background;
                double norm;
        
                /* Threshold is half of max value at this x */
                minvalue = 0.5*thisvalue;
            
        /* Move to the lowest y above 'minvalue' */
        while(y > 1 && cpl_image_get(inputimage, x, y - 1, &pis_rejected) >= minvalue)
            {
            y--;
            }
            
        assure( cpl_error_get_code() == CPL_ERROR_NONE,
            cpl_error_get_code(), "Could not read pixel from input image" );
            
        /* Remember this place */
        ylow = y;
            
        /* Move to the highest y above 'minvalue' */
        while(y < ny && cpl_image_get(inputimage, x, y + 1, &pis_rejected) >= minvalue)
            {
            y++;
            }
            
        assure( cpl_error_get_code() == CPL_ERROR_NONE, cpl_error_get_code(), 
            "Could not read pixel from input image" );
            
        /* Also remember this place */
        yhigh = y;
            
        /* Update the order line's location to centroid of 
           strip from ylow to yhigh w.r.t. minvalue*/
        {
            double sum  = 0;
            double sumy = 0;
            double sumy2= 0;
            for (y = ylow; y <= yhigh; y++)
            {
                double flux;
                flux = cpl_image_get(inputimage, x, y, &pis_rejected) - minvalue;
                if (!pis_rejected && flux > 0)
                {
                    sum   += flux;
                    sumy  += flux * (y - *yguess*0);
                    sumy2 += flux * (y - *yguess*0) * (y - *yguess*0);
                }
            }
            if (sum > 0)
            {
                *yguess = *yguess*0 + sumy / sum;
                sigmaY = sqrt( sumy2 / sum - sumy*sumy/(sum*sum) );
                
                if ( sumy2 / sum - sumy*sumy/(sum*sum) < 0 || 
                 sigmaY < sqrt(1.0/12) )
                {
                    /* If the sum is over one pixel, sigma will be zero 
                       (or less than zero because of numerical error), 
                       so set sigma to stddev of one pixel = 1/sqrt(12)
                       in that case */
                    sigmaY = sqrt(1.0/12);
                }
                
                /* Uncertainty, dY, of mean value (yguess) is  sigma/sqrt(N)
                   where N is the total count, i.e. area under curve */
                *dY = sigmaY/sqrt(sum);
                
            }
            else
            {
                /* If all pixels were bad, don't update '*yguess' */
                sigmaY = 1.0;
                *dY = .1;
                
            }
        }
            
                /* This is a better method. Get centroid 
                   position by making a Gaussian fit. */
                
                /* Use a wide fitting window to get a well defined background level */
                ylow  = uves_max_int(1 , uves_round_double(*yguess - spacing/3));
                yhigh = uves_min_int(ny, uves_round_double(*yguess + spacing/3));
                
                assure( yhigh - ylow >= 1, CPL_ERROR_ILLEGAL_INPUT,
                        "Estimated spacing too small: %d pixel(s)", spacing);
                
                /* Fit. Save the result in 'yguess' */
                uves_fit_1d_image(inputimage, noise, 
                                  image_bad,
                                  false, false, false,
                                  ylow, yhigh, x,
                                  yguess, &sigmaY, &norm, &background, NULL,
                                  &mse, &chi_sq, &covariance,
                                  uves_gauss, uves_gauss_derivative, 4);
                
                /* Recover from specific fitting errors */
                if (cpl_error_get_code() == CPL_ERROR_NONE)
                    {
                        /* Variance is guaranteed to be positive */
                        *dY = sqrt(cpl_matrix_get(covariance, 0, 0));
                    }
                else if (cpl_error_get_code() == CPL_ERROR_CONTINUE)
                    {
                        /* Fitting failed */
                        uves_error_reset();
                        uves_msg_debug("Fitting failed at (x,y) = (%d, %e), "
                                       "using centroid", x, *yguess);
                        *dY = sigmaY / sqrt(norm);
                    }
                else if (cpl_error_get_code() == CPL_ERROR_SINGULAR_MATRIX)
                    {
                        uves_error_reset();
                        
                        /* Fitting succeeded but covariance computation failed */
                        uves_msg_debug("Covariance matrix computation failed");
                        *dY = sigmaY / sqrt(norm);
                    }
                
                assure(cpl_error_get_code() == CPL_ERROR_NONE,
                       cpl_error_get_code(), "Gaussian fitting failed");
                
                rms = sqrt(mse);
                
                uves_msg_debug("dy = %f   sigma/sqrt(N) = %f", *dY, sigmaY/(norm));
                
                /* If the peak is definitely there or definitely not there,
                   set the returnvalue appropriately */
                if ( norm > 10 * rms)
                    {
                        returnvalue = true;
                    }
                if ( norm < 2 * rms)
                    {
                        returnvalue = false;
                    }
                
            } /* signal was above threshold at this x */
    
    }/* If yguess was inside image */

  cleanup:
    cpl_matrix_delete(covariance);
    return returnvalue;
}
/**@}*/
