/* $Id: cpl_geom_img-test.c,v 1.16 2007/11/13 13:13:02 llundin Exp $
 *
 * This file is part of the ESO Common Pipeline Library
 * Copyright (C) 2001-2004 European Southern Observatory
 *
 * 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

/*
 * $Author: llundin $
 * $Date: 2007/11/13 13:13:02 $
 * $Revision: 1.16 $
 * $Name:  $
 */

/*-----------------------------------------------------------------------------
                                   Includes
 -----------------------------------------------------------------------------*/

#include <math.h>

#include <cpl_image_gen.h>
#include <cpl_imagelist_io.h>
#include <cpl_image_io.h>
#include <cpl_image_basic.h>
#include <cpl_bivector.h>
#include <cpl_vector.h>
#include <cpl_memory.h>
#include <cpl_msg.h>
#include <cpl_tools.h>

#include "cpl_apertures.h"
#include "cpl_geom_img.h"

/*-----------------------------------------------------------------------------
                                Define
 -----------------------------------------------------------------------------*/

#define     IMAGESZ             512
#define     NFRAMES             10
#define     MAX_SHIFT_ERROR1    15
#define     MAX_SHIFT_ERROR2    0.1

/*-----------------------------------------------------------------------------
                                  Main
 -----------------------------------------------------------------------------*/
int main(void)
{
    double              off_x_init[NFRAMES] ;
    double              off_y_init[NFRAMES] ;
    cpl_imagelist       *   iset ;
    cpl_image       *   img ;
    cpl_bivector    *   offs_est ;
    cpl_vector      *   off_vec_x ;
    cpl_vector      *   off_vec_y ;
    cpl_bivector    *   offs_ref ;
    double          *   x_offs;
    double          *   y_offs;
    cpl_apertures       *   aperts ;
    int                 naperts ;
    cpl_bivector    *   aperts_pos ;
    double          *   aperts_pos_data_x ;
    double          *   aperts_pos_data_y ;
    cpl_vector      *   correl ;
    double              psigmas[] = {5, 2, 1, 0.5};
    cpl_vector      *   sigmas;
    cpl_image       **  combined ;
    int                 i ;
    int                 pos;

    cpl_test_init(PACKAGE_BUGREPORT, CPL_MSG_WARNING);

    /* Insert tests below */

    off_x_init[0] = 0.00 ;
    off_x_init[1] = -6 ;
    off_x_init[2] = -18 ;
    off_x_init[3] = 54 ;
    off_x_init[4] = 33 ;
    off_x_init[5] = 46 ;
    off_x_init[6] = -3 ;
    off_x_init[7] = 36 ;
    off_x_init[8] = 42 ;
    off_x_init[9] = -13 ;
    off_y_init[0] = 0.00 ;
    off_y_init[1] = 13 ;
    off_y_init[2] = 3 ;
    off_y_init[3] = 8 ;
    off_y_init[4] = 32 ;
    off_y_init[5] = 22 ;
    off_y_init[6] = 18 ;
    off_y_init[7] = -56 ;
    off_y_init[8] = 3 ;
    off_y_init[9] = 10 ;

    /* Create an image set */
    img = cpl_image_fill_test_create(IMAGESZ, IMAGESZ) ;
    iset = cpl_imagelist_new() ;
    for (i=0 ; i<NFRAMES ; i++)
        cpl_imagelist_set(iset, cpl_image_duplicate(img), i);
    cpl_image_delete(img) ;
    for (i=1 ; i<NFRAMES ; i++) cpl_image_shift(cpl_imagelist_get(iset, i),
            (int)(off_x_init[i] * -1), (int)(off_y_init[i] * -1)) ;
    
    off_vec_x = cpl_vector_wrap(NFRAMES, off_x_init) ;
    off_vec_y = cpl_vector_wrap(NFRAMES, off_y_init) ;
    offs_est = cpl_bivector_wrap_vectors(off_vec_x, off_vec_y) ;

    sigmas = cpl_vector_wrap(4, psigmas);

    /* Get some cross-correlation apertures */
    cpl_msg_info("","Apply the apertures detection") ;
    aperts = cpl_apertures_extract(cpl_imagelist_get(iset, 0), sigmas, &pos);
    cpl_vector_unwrap(sigmas);
    if (aperts == NULL) {
        cpl_bivector_unwrap_vectors(offs_est) ;
        cpl_vector_unwrap(off_vec_x) ;
        cpl_vector_unwrap(off_vec_y) ;
        cpl_imagelist_delete(iset) ;
        cpl_test_end(1);
    }
    naperts = cpl_apertures_get_size(aperts) ;
    aperts_pos = cpl_bivector_new(naperts) ;
    aperts_pos_data_x = cpl_bivector_get_x_data(aperts_pos) ;
    aperts_pos_data_y = cpl_bivector_get_y_data(aperts_pos) ;
    for (i=0 ; i<naperts ; i++) {
        aperts_pos_data_x[i] = cpl_apertures_get_max_x(aperts, i+1) ;
        aperts_pos_data_y[i] = cpl_apertures_get_max_y(aperts, i+1) ;
    }
    cpl_apertures_delete(aperts) ;
   
    /* Refine the offsets with cpl_geom_img_offset_fine */
    cpl_msg_info("","Refine the offsets") ;
    correl = cpl_vector_new(cpl_imagelist_get_size(iset)) ;
    cpl_tools_get_cputime(CPL_CLOCK_STOP);
    if ((offs_ref = cpl_geom_img_offset_fine(iset, offs_est, aperts_pos,
                    15, 15, 15, 15, correl)) == NULL) {

        cpl_vector_delete(correl) ;
        cpl_bivector_delete(aperts_pos) ;
        cpl_bivector_unwrap_vectors(offs_est) ;
        cpl_vector_unwrap(off_vec_x) ;
        cpl_vector_unwrap(off_vec_y) ;
        cpl_imagelist_delete(iset) ;
        cpl_test_end(1);
    }
    cpl_tools_get_cputime(CPL_CLOCK_START);
    cpl_vector_delete(correl) ;
    cpl_bivector_unwrap_vectors(offs_est) ;
    cpl_vector_unwrap(off_vec_x) ;
    cpl_vector_unwrap(off_vec_y) ;
    cpl_bivector_delete(aperts_pos) ;
    x_offs = cpl_bivector_get_x_data(offs_ref) ;
    y_offs = cpl_bivector_get_y_data(offs_ref) ;
    for (i=0 ; i<cpl_bivector_get_size(offs_ref) ; i++) {
        if ((fabs(x_offs[i]-off_x_init[i]) > MAX_SHIFT_ERROR2) || 
                (fabs(y_offs[i]-off_y_init[i]) > MAX_SHIFT_ERROR2)) {
            cpl_bivector_delete(offs_ref) ;
            cpl_imagelist_delete(iset) ;
            cpl_test_end(1);
        }
    }

    /* Shift and add */
    cpl_msg_info("","Shift and add") ;
    cpl_tools_get_cputime(CPL_CLOCK_STOP);
    if ((combined = cpl_geom_img_offset_saa(iset, offs_ref, 
                    CPL_KERNEL_DEFAULT, 1, 1, CPL_GEOM_UNION)) == NULL) {
        cpl_bivector_delete(offs_ref) ;
        cpl_imagelist_delete(iset) ;
        cpl_test_end(1);
    }
    cpl_tools_get_cputime(CPL_CLOCK_START);
    cpl_bivector_delete(offs_ref) ;
    cpl_imagelist_delete(iset) ;
    
    /* Free and return */
    cpl_image_delete(combined[0]) ;
    cpl_image_delete(combined[1]) ;
    cpl_free(combined) ;

    /* End of tests */
    return cpl_test_end(0);
}