/*                                                                            *
 *   This file is part of the ESO X-shooter Pipeline                          *
 *   Copyright (C) 2006 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: 2012-10-16 15:31:47 $
 * $Revision: 1.3 $
 * $Name: not supported by cvs2svn $
 */
#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif

/*--------------------------------------------------------------------------*/
/**
 * @defgroup test_xsh_tests_create_map Test Create SLITMAP and WAVEMAP
 * @ingroup unit_tests
 *
 */
/*--------------------------------------------------------------------------*/
/**@{*/

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


#include <xsh_error.h>
#include <xsh_msg.h>
#include <xsh_utils.h>
#include <xsh_dfs.h>
#include <tests.h>
#include <cpl.h>
#include <math.h>
#include <getopt.h>
#include <string.h>
/*---------------------------------------------------------------------------
                            Defines
 ---------------------------------------------------------------------------*/
#define MODULE_ID "XSH_CORREL_GAUSSIANS"

enum {
  DEBUG_OPT, HELP_OPT
};

static struct option LongOptions[] = {
  {"debug", required_argument, 0, DEBUG_OPT},
  {"help", 0, 0, HELP_OPT},
  {NULL, 0, 0, 0}
};

static void Help( void )
{
  puts ("Unitary test : Create two Gaussians one shifted to the other of a given quantity, then correlate them to check if correlation returns expected shift");
  puts( "Usage : ./tetst_xsh_correl_gaussians [options]");

  puts( "Options" ) ;
  puts( " --debug=<n>        : Level of debug LOW | MEDIUM | HIGH [MEDIUM]" );
  puts( " --help             : What you see" ) ;

  puts( "The input files argument MUST be in this order:" );
  puts( " 1. PRE frame");
  puts( " 2. SOF a) MODEL     : [XSH_MOD_CFG_TAB_UVB]");
  puts( "        b) POLYNOMIAL: [DISP_TAB, ORDER_TAB_EDGES]");
  
  TESTS_CLEAN_WORKSPACE(MODULE_ID);
  TEST_END();
  exit(0);
}

static void HandleOptions( int argc, char ** argv)
{
  int opt ;
  int option_index = 0;

  while( (opt = getopt_long( argc, argv, "debug:help",
                             LongOptions, &option_index )) != EOF){
    switch( opt ) {
    case DEBUG_OPT:
      if ( strcmp( optarg, "LOW")==0){
        xsh_debug_level_set( XSH_DEBUG_LEVEL_LOW);
      }
      else if ( strcmp( optarg, "HIGH")==0){
        xsh_debug_level_set( XSH_DEBUG_LEVEL_HIGH);
      }
      break;
    case HELP_OPT:
      Help();
      break;
    default:
      break;
    }
  }
}

/*
static double
xsh_correlate_julian(const double* prof1, const double* prof2, 
		     double* correl,const int , const int hsize) {


  double *correl = cpl_calloc(length, sizeof(double));
  for(i=

}
*/
static double
xsh_correlate_profiles2(const double* prof1, const double* prof2, 
			double* correl,const int length, const int hsize) {

  XSH_ASSURE_NOT_NULL_MSG(prof1,"NULL input prof1");
  XSH_ASSURE_NOT_NULL_MSG(prof2,"NULL input prof2");
  XSH_ASSURE_NOT_NULL_MSG(correl,"NULL input correl");
  XSH_ASSURE_NOT_ILLEGAL_MSG(length>2*hsize,"length<=2*hsize");

  double *profile1 = cpl_calloc(length, sizeof(double));
  double *profile2 = cpl_calloc(length, sizeof(double));
  memset( profile1,0,length);
  memset( profile2,0,length);

  int i=0;
  double norm=0;

  /*
   * Profiles normalisation
   */


  for (i = 0; i < length; i++)
    if (norm < prof1[i]){
      norm = prof1[i];
      //xsh_msg("found norm: %g",norm);
    }    
  if (i < length && norm < prof2[i]){
    norm = prof2[i];
    //xsh_msg("found norm: %g",norm);
  }    

  for (i = 0; i < length; i++) {
    profile1[i] = prof1[i]/norm;
    profile2[i] = prof2[i]/norm;
    //xsh_msg("profile1[%d]: %g",i,profile1[i]);
    //xsh_msg("profile2[%d]: %g",i,profile2[i]);
  }
  int     short_length = length - 2*hsize - 1;
  xsh_msg("mk4 short_len=%d",short_length);

  /*
   * Cross-correlation
   */

  int step_min=-hsize;
  int step_max=+hsize;
  int nsteps=step_max-step_min+1;
  double max=0;
  double value=0;
  int maxpos=0;
  int j=0;
  int k=0;

  max = -1;
  for (i = step_min; i <= step_max; i++) {
    value = 0;
    k=i-step_min;
    if (k >= length) break;
    for (j = 0; j < length && k+j < length; j++) {
      value += profile1[k] * profile2[k+j];
    }
    correl[k]=value;
    //xsh_msg("correl value: %g",value);
    if (max < value) {
      //xsh_msg("found max: %g",max);
      max = value;
      maxpos = i;
    }
  }
  max=-1;
  for (i=0 ; i<nsteps ; i++) {
    if (correl[i]>max) {
      maxpos = i ;
      max = correl[i];
    }
  }



  /*
   * search for correlation maximum
   */
  xsh_msg("correl max[%d]=%g",maxpos,max);
  xsh_msg("nsteps %d",nsteps);

  double a=correl[maxpos-1];
  double b=correl[maxpos+1];
  double c=correl[maxpos];
  double fraction=(a-b)/(2.*a+2.*b-4.*c);
  double shift=0;
  xsh_msg("fraction=%g",fraction);
  shift=maxpos+fraction;
 cleanup:
  cpl_free(profile1); 
  cpl_free(profile2);

  return shift;

}

/*
static double
xsh_correlate_profiles(const double* prof1, const double* prof2, 
		       double* correl,const int length, const int hsize) {

  int     short_length = length - 2*hsize - 1;
  int i=0;
  int j=0;
  int k=0;
  double shift=0;
  int maxpos=0;
  double norm=0;
  double max=0;
  double value=0;

  double *profile1 = cpl_calloc(length, sizeof(double));
  double *profile2 = cpl_calloc(length, sizeof(double));

  
   // Profiles normalisation
   


  for (i = 0; i < length; i++)
    if (norm < prof1[i]){
      norm = prof1[i];
      //xsh_msg("found norm: %g",norm);
    }    
  if (norm < prof2[i]){
    norm = prof2[i];
    //xsh_msg("found norm: %g",norm);
  }    

  for (i = 0; i < length; i++) {
    profile1[i] = prof1[i]/norm;
    profile2[i] = prof2[i]/norm;
    //xsh_msg("profile1[%d]: %g",i,profile1[i]);
    //xsh_msg("profile2[%d]: %g",i,profile2[i]);
  }
  xsh_msg("mk4 short_len=%d",short_length);

  
   // Cross-correlation
   

  max = -11;
  for (i = 0; i <= hsize; i++) {
    value = 0;
    for (j = 0; j < short_length; j++) {
      k = hsize+j;
      value += profile1[k] * profile2[k+i];
    }
    correl[i]=value;
    //xsh_msg("correl value: %g",value);
    if (max < value) {
      //xsh_msg("found max: %g",max);
      max = value;
      maxpos = i;
    }
  }


  
   // search for correlation maximum
   
  xsh_msg("correl max[%d]=%g",maxpos,max);
  cpl_free(profile1); 
  cpl_free(profile2);

  double a=correl[maxpos-1];
  double b=correl[maxpos+1];
  double c=correl[maxpos];
  double fraction=(a-b)/(2.*a+2.*b-4.*c);
  xsh_msg("fraction=%g",fraction);
  shift=maxpos+fraction;

  return shift;

}
*/


static double
xsh_function1d_xcorrelate2(
			  double *    line_i,
			  int         width_i,
			  double *    line_t,
			  int         width_t,
			  int         half_search,
			  double *    delta
			  )
{
  double * xcorr ;
  double   xcorr_max ;
  //double   mean_i, double   mean_t ;
  //double   rms_i, rms_t ;
  //double   sum, sqsum ;
  double   norm = 0;
  int      maxpos ;
  int      nsteps ;
  int      i ;
  int      step ;
  int      nval ;
  int STEP_MIN=-half_search;
  int STEP_MAX=half_search;


  /* Compute normalization factors */

  for (i = 0; i < width_i; i++)
    if (norm < line_i[i]){
      norm = line_i[i];
      //xsh_msg("found norm: %g",norm);
    }    

  for (i = 0; i < width_t; i++)
  if (norm < line_t[i]){
    norm = line_t[i];
    //xsh_msg("found norm: %g",norm);
  }    

  for (i = 0; i < width_i; i++) {
    line_i[i] /= norm;
    line_t[i] /= norm;
  }

  for (i = 0; i < width_t; i++) {
    line_t[i] /= norm;
  }


  /* compute cross correlation */
  nsteps = (STEP_MAX - STEP_MIN) +1 ;
  xcorr = cpl_malloc(nsteps * sizeof(double));
  for (step=STEP_MIN ; step<=STEP_MAX ; step++) {
    xcorr[step-STEP_MIN] = 0.00 ;
    nval = 0 ;
    for (i=0 ; i<width_t ; i++) {
      if ((i+step > 0) &&
	  (i+step < width_i)) {
	xcorr[step-STEP_MIN] += line_t[i] * line_i[i+step];
	nval++ ;
      }
    }
    xcorr[step-STEP_MIN] /= (double) nval ;
  }
  xcorr_max = xcorr[0] ;
  maxpos    = 0 ;
  for (i=0 ; i<nsteps ; i++) {
    if (xcorr[i]>xcorr_max) {
      maxpos = i ;
      xcorr_max = xcorr[i];
    }
  }
  cpl_vector* vcor=cpl_vector_wrap(nsteps,xcorr);
  double a=xcorr[maxpos-1];
  double b=xcorr[maxpos+1];
  double c=xcorr[maxpos];
  double fraction=(a-b)/(2.*a+2.*b-4.*c);
  cpl_vector_save(vcor,"vcor.fits",CPL_BPP_IEEE_FLOAT,NULL,CPL_IO_DEFAULT);
  cpl_vector_unwrap(vcor);
  cpl_free(xcorr);
  xsh_msg("STEP_MIN=%d maxpos=%d",STEP_MIN,maxpos);
  (*delta) =  (double)STEP_MIN + (double)maxpos;
  xsh_msg("a=%g b=%g c=%g fraction=%g",a,b,c,fraction);
  xsh_msg("maxpos=%d width_i=%d",maxpos,width_i);
  xsh_msg("fractionary delta=%g",*delta+fraction);
  return xcorr_max ;
}





cpl_error_code
xsh_gauss_gen(double* data,const double center,const double sigma, const int size)
{

  int i=0;
  double x=0;
  double inv_2_c2=0.5/sigma/sigma;
  double norm=sigma*sqrt(2*CPL_MATH_PI);
  double a=1./norm;

  for(i=0;i<size;i++) {
    x=i;
    data[i]=a*exp(-(x-center)*(x-center)*inv_2_c2);
  }

  return cpl_error_get_code();
}

/*---------------------------------------------------------------------------
                         Functions prototypes
 ---------------------------------------------------------------------------*/

/*--------------------------------------------------------------------------*/
/**
  @brief    Create a SLITMAP and a WAVEMAP from Set Of Files (SOF)
  @return   0 if success

  Test the Data Reduction Library function XSH_CREATE_MAP
 */
/*--------------------------------------------------------------------------*/

int main( int argc, char** argv)
{
  /* Initialize libraries */
  TESTS_INIT_WORKSPACE(MODULE_ID);
  TESTS_INIT( MODULE_ID);
  cpl_msg_set_level( CPL_MSG_DEBUG);
  xsh_debug_level_set( XSH_DEBUG_LEVEL_MEDIUM) ;

  HandleOptions( argc, argv);

  /* Analyse parameters */
  if ( (argc-optind) >= 2) {
    Help();
  }

  int ret=0;
  double shift_i=5.15;
  double shift_o=0;
  double gauss_c=50.;
  double gauss_s=10.;
  double* gauss_d1=NULL;
  double* gauss_d2=NULL;

  int size=100;
  cpl_vector* gauss_v1=cpl_vector_new(size);
  cpl_vector* gauss_v2=cpl_vector_new(size);

  gauss_d1=cpl_vector_get_data(gauss_v1);
  gauss_d2=cpl_vector_get_data(gauss_v2);

  check(xsh_gauss_gen(gauss_d1,gauss_c,gauss_s,size));
  check(xsh_gauss_gen(gauss_d2,gauss_c+shift_i,gauss_s,size));


  int half_search=(int)(2*gauss_s+1);
  //half_search=7;
  //int len_corr=1+2*half_search;
  int len_corr=199;



  double corr=0;
  corr=xsh_function1d_xcorrelate2(gauss_d1,size,gauss_d2,size,half_search,&shift_o);
  xsh_msg("function1d corerel: shift %g",shift_o);

  double moses_shift=0;
  double* moses_correl=NULL;

  moses_correl = cpl_calloc(len_corr, sizeof(double));
  check(moses_shift=xsh_correlate_profiles2(gauss_d1, gauss_d2,moses_correl,size,half_search));
  xsh_msg("moses_shift=%g",moses_shift);
  cpl_vector* mc=cpl_vector_wrap(len_corr,moses_correl);
  cpl_vector_save(mc,"moses_correl.fits",CPL_BPP_IEEE_FLOAT,NULL,CPL_IO_DEFAULT);
  xsh_free_vector(&mc);

  cpl_vector_save(gauss_v1,"gauss_v1.fits",CPL_BPP_IEEE_FLOAT,NULL,CPL_IO_DEFAULT);
  cpl_vector_save(gauss_v2,"gauss_v2.fits",CPL_BPP_IEEE_FLOAT,NULL,CPL_IO_DEFAULT);




  /* CPL VECTOR */

  cpl_vector* correl=cpl_vector_new(len_corr);
  check(cpl_vector_fill(correl, 0.0));
  double shift=cpl_vector_correlate(correl,gauss_v1,gauss_v2);

  cpl_vector_save(correl,"correl.fits",CPL_BPP_IEEE_FLOAT,NULL,CPL_IO_DEFAULT);
  xsh_msg("shift=%g",shift);
  //cpl_vector_dump(correl,stdout);
  int maxpos=(int)(shift-1);
  xsh_msg("correl max=%g",cpl_vector_get(correl,shift));
  xsh_msg( "Shift: expected: %g  computed: %g accuracy:%g ",
        shift_i,shift_o,fabs((shift_i-shift_o)/shift_i));
  double max=0;
  //int maxpos=0;
  check(max = cpl_vector_get_max(correl));
  //check(maxpos=cpl_vector_find(correl,shift+1));
  xsh_msg("max=%g maxpos=%d",max,maxpos);
  double* pvec=NULL;
  pvec=cpl_vector_get_data(correl);
  max=-100;
  int i=0;
  for(i=1;i<len_corr;i++) {
    if(max<pvec[i] ) {
      max=pvec[i];
      maxpos=i;
    }
  }
  xsh_msg("maxpos my determination: %d",maxpos);

  double a=0;
  double b=0;
  double c=0;
  double fraction=0;
  //maxpos +=1;
  a=cpl_vector_get(correl,maxpos-1);
  b=cpl_vector_get(correl,maxpos+1);
  c=cpl_vector_get(correl,maxpos);
  fraction=(a-b)/(2.*a+2.*b-4.*c);
  xsh_msg("len_corr=%d",len_corr);
  xsh_msg("a=%g b=%g c=%g fraction=%g",a,b,c,fraction);


  
 cleanup: 
  xsh_free_vector(&gauss_v1);
  xsh_free_vector(&gauss_v2);
  xsh_free_vector(&correl);


    if (cpl_error_get_code() != CPL_ERROR_NONE) {
      xsh_error_dump( CPL_MSG_ERROR);
      ret=1;
    } 
    TESTS_CLEAN_WORKSPACE(MODULE_ID);
    TEST_END();
    return ret;
}

/**@}*/
