/* Copyright (c) 2014, Giuseppe Argentieri <giuseppe.argentieri@ts.infn.it>

 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */

/*
 * =====================================================================================
 *
 *       Filename:  Reg_sc.c
 *
 *    Description:
 *
 *        Version:  1.0
 *        Created:  21/04/2014 12:27:24
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Giuseppe Argentieri (ga), giuseppe.argentieri@ts.infn.it
 *   Organization:  Università degli Studi di Trieste
 *
 * =====================================================================================
 */

/*
   Re g_sc is made of 4 integrands: 2 with principal values and 2 without.

   The common factor function is k_func = alpha*k*exp(-k/omega_c)/tanh(b*k/2)

   The poles are in (omega1-O) and (omega1+O)

   The sign of the four fractions

   1/(k+(omega1+O)) , 1/(k+(omega1-O)) , 1/(k-(omega1-O)) , 1/(k-(omega1+O))

   are

   + , - , + , -
*/

#include <stdio.h>

#include "funcs.h"
#include <gsl/gsl_integration.h>

/*
 * ===  FUNCTION  ======================================================================
 *         Name:  k_func
 *  Description:  Common integrand factor
 * =====================================================================================
 */
double k_func ( double k, void* params )
{

	/* Copying parameters */
	struct f_params* pars = (struct f_params*) params ;
	double o_c, b, O, o_1, alpha ;
	assign_p( pars, &o_c, &b, &O, &o_1 ) ;
	alpha = pars->alpha ;

	double temp = alpha*k*exp(-k/o_c) ;
	double val = temp/tanh(b*k/2) ;

	return val;
}		/* -----  end of function k_func  ----- */

/*
 * ===  FUNCTION  ======================================================================
 *         Name:  k_func_1
 *  Description:  First integrand: k_func/(k+(omega1+O))
 * =====================================================================================
 */
double k_func_1 ( double k, void* params )
{
	/* Copying parameters */
	struct f_params* pars = (struct f_params*) params ;
	double o_c, b, O, o_1 ;
	assign_p( pars, &o_c, &b, &O, &o_1 ) ;

	/* Invoking k_func and returning value */
	double v , v1 ;
	v = k_func( k, pars ) ;
	v1 = v/(k+(o_1+O)) ;

	return v1 ;
}		/* -----  end of function k_func_1  ----- */

/*
 * ===  FUNCTION  ======================================================================
 *         Name:  first_int
 *  Description:  Integrating k_func/(k+(omega1+O))
 * =====================================================================================
 */
int first_int ( double* val, double* error, void* params )
{
	/* Copying parameters */
	struct f_params* pars = (struct f_params*) params ;
	double o_c, b, O, o_1 ;
	assign_p( pars, &o_c, &b, &O, &o_1 ) ;

	/* Allocating workspace for integration */
	gsl_integration_workspace* first_int_ws =
		gsl_integration_workspace_alloc(WS_SZ) ;

	gsl_function F ;
	F.function = &k_func_1 ;
	F.params = pars ;

	/* Integration over (0,+Infinity) */
	double res, err ;
	int status = gsl_integration_qagiu( &F, 0, 1e-9, 1e-3, WS_SZ, first_int_ws,
			&res, &err ) ;

	*val = res ; *error = err ;

	gsl_integration_workspace_free(first_int_ws) ;
	return status;
}		/* -----  end of function first_int  ----- */

/*
 *      FUNCTION
 *         Name:  k_func_2
 *  Description:  Second integrand: k_func/(k+omega1-O)
 *
 */
double k_func_2 ( double k , void* params )
{
	/* Copying parameters */
	struct f_params* pars = (struct f_params*) params ;
	double o_c, b, O, o_1 ;
	assign_p( pars, &o_c, &b, &O, &o_1 ) ;

	/* Invoking k_func and returning value */
	double v, v2 ;
	v = k_func( k, pars ) ;
	v2 = v/(k+(o_1-O)) ;

	return v2;
}		/* -----  end of function k_func_2  ----- */

/*
 *      FUNCTION
 *         Name:  second_int
 *  Description:  Integration k_func/(k+(omega1-O))
 *
 */
int second_int ( double* val, double* error, void* params )
{
	/* Copying parameters */
	struct f_params* pars = (struct f_params*) params ;
	double o_c, b, O, o_1 ;
	assign_p( pars, &o_c, &b, &O, &o_1 ) ;

	/* Allocating workspace for integration */
	gsl_integration_workspace* second_int_ws =
		gsl_integration_workspace_alloc(WS_SZ) ;

	gsl_function F;
	F.function = &k_func_2 ;
	F.params = pars ;

	double res, err ;
	int status = gsl_integration_qagiu( &F, 0, 1e-9, 1e-3, WS_SZ, second_int_ws,
			&res, &err ) ;
	*val = res ; *error = err ;

	gsl_integration_workspace_free(second_int_ws) ;

	return status;
}		/* -----  end of function second_int  ----- */

/*
 *      FUNCTION
 *         Name:  k_func_3
 *  Description:  k_func/(k-(omega1-O)) to be integrated on the tail +inf
 *
 */
double k_func_3 ( double k, void* params )
{
	/* Copying parameters */
	struct f_params* pars = (struct f_params*) params ;
	double o_c, b, O, o_1 ;
	assign_p( pars, &o_c, &b, &O, &o_1 ) ;

	double temp , val ;
	temp = k_func( k, pars ) ;
	val = temp/(k-(o_1-O)) ;

	return val;
}		/* -----  end of function k_func_3  ----- */

/*
 *      FUNCTION
 *         Name:  third_int
 *  Description:  Integration of the p.v. k_func/(k-(omega1-O))
 *
 */
int third_int ( double* val, double* err, void* params )
{
	/* Copying parameters */
	struct f_params* pars = (struct f_params*) params ;
	double o_c, b, O, o_1 ;
	assign_p( pars, &o_c, &b, &O, &o_1 ) ;

	/* Integration of the p.v. */
	gsl_function F ;
	F.function = &k_func ;
	F.params = pars ;

	gsl_integration_workspace* third_int_ws =
		gsl_integration_workspace_alloc(WS_SZ) ;

	double v1 , err1 ;
	int status1 = gsl_integration_qawc( &F, (o_1-O)/2, 3*(o_1-O)/2, o_1-O,
			1e-9, 1e-3, WS_SZ, third_int_ws, &v1, &err1 ) ;

	gsl_integration_workspace_free(third_int_ws) ;

	/* Integration to +infinity */
	gsl_function G ;
	G.function = &k_func_3 ;
	G.params = pars ;

	gsl_integration_workspace* third_int_ws_2 =
		gsl_integration_workspace_alloc(WS_SZ) ;

	double v2, err2 ;
	int status2 = gsl_integration_qagiu( &G, 3*(o_1-O)/2, 1e-9, 1e-3, WS_SZ,
			third_int_ws_2, &v2, &err2 ) ;

	gsl_integration_workspace_free(third_int_ws_2) ;

	/* Integration on ( 0, (o_1-O)/2 ) */
	gsl_integration_workspace* third_int_ws_3 =
		gsl_integration_workspace_alloc(WS_SZ) ;

	gsl_function H ;
	H.function = &k_func_3 ;
	H.params = pars ;

	double v3, err3 ;
	int status3 = gsl_integration_qag( &H, 0, (o_1-O)/2, 1e-9, 1e-3, WS_SZ, 6,
			third_int_ws_3, &v3, &err3 ) ;

	gsl_integration_workspace_free(third_int_ws_3) ;

	*val = v1 + v2 + v3 ; *err = err1 + err2 + err3 ;

	return status1 + status2 + status3 ;
}		/* -----  end of function third_int  ----- */

/*
 *      FUNCTION
 *         Name:  k_func_4
 *  Description:  k_func/(k-(omega1+O))
 *
 */
double k_func_4 ( double k, void* params )
{
	/* Copying parameters */
	struct f_params* pars = (struct f_params*) params ;
	double o_c, b, O, o_1 ;
	assign_p( pars, &o_c, &b, &O, &o_1 ) ;

	double temp , val ;
	temp = k_func( k, pars ) ;
	val = temp/(k-(o_1+O)) ;

	return val;
}		/* -----  end of function k_func_4  ----- */

/*
 *      FUNCTION
 *         Name:  fourth_integ
 *  Description:  Integration of the p.v. kfunc/(k-(omega1+O))
 *
 */
int fourth_int ( double* val, double* err, void* params )
{
	/* Copying parameters */
	struct f_params* pars = (struct f_params*) params ;
	double o_c, b, O, o_1 ;
	assign_p( pars, &o_c, &b, &O, &o_1 ) ;

	/* Integration of the p.v. */
	gsl_function F ;
	F.function = &k_func ;
	F.params = pars ;

	gsl_integration_workspace* fourth_int_ws =
		gsl_integration_workspace_alloc(WS_SZ) ;

	double v1 , err1 ;
	int status1 = gsl_integration_qawc( &F, (o_1+O)/2, 3*(o_1+O)/2, o_1+O,
			1e-9, 1e-3, WS_SZ, fourth_int_ws, &v1, &err1 ) ;

	gsl_integration_workspace_free(fourth_int_ws) ;

	/* Integration to +infinity */
	gsl_function G ;
	G.function = &k_func_4 ;
	G.params = pars ;

	gsl_integration_workspace* fourth_int_ws_2 =
		gsl_integration_workspace_alloc(WS_SZ) ;

	double v2, err2 ;
	int status2 = gsl_integration_qagiu( &G, 3*(o_1+O)/2, 1e-9, 1e-3, WS_SZ,
			fourth_int_ws_2, &v2, &err2 ) ;

	gsl_integration_workspace_free(fourth_int_ws_2) ;

	/* Integration on ( 0 , (O+o_1)/2 ) */
	gsl_integration_workspace* fourth_int_ws_3 =
		gsl_integration_workspace_alloc(WS_SZ) ;

	gsl_function K ;
	K.function = &k_func_4 ;
	K.params = pars ;
	double v3, err3 ;

	int status3 = gsl_integration_qag( &K, 0, (O+o_1)/2, 1e-9, 1e-3, WS_SZ,
			GSL_INTEG_GAUSS61, fourth_int_ws_3, &v3, &err3 ) ;

	gsl_integration_workspace_free(fourth_int_ws_3) ;

	*val = v1 + v2 + v3 ; *err = err1 + err2 + err3 ;

	return status1 + status2 + status3;
}		/* -----  end of function fourth_integ  ----- */

/*
 *      FUNCTION
 *         Name:  re_gsc
 *  Description:
 *
 */
int re_gsc ( double* result, double* error, void* params )
{
	/* Copying parameters */
	struct f_params* pars = (struct f_params*) params ;
	double o_c, b, O, o_1 ;
	assign_p( pars, &o_c, &b, &O, &o_1 ) ;

	double v1, v2, v3, v4, err1, err2, err3, err4 ;
	int status = first_int( &v1, &err1, pars ) + second_int( &v2, &err2, pars )
		+ third_int( &v3, &err3, pars ) + fourth_int( &v4, &err4, pars ) ;
	*result = -(v1 - v2 + v3 - v4)/4.0 ;
	*error = (err1 + err2 + err3 + err4)/4.0 ;

	return status;
}		/* -----  end of function re_gsc  ----- */

/*
 *      FUNCTION
 *         Name:  re_gcs
 *  Description:  Re g_cs is the same of Re g_sc but with O and o_1 exchanged.
 *  		  Therefore the signs are
 *
 *  		  + , + , - , -
 *
 */
int re_gcs ( double* result, double* error, void* params )
{
	/* Copying parameters */
	struct f_params* pars = (struct f_params*) params ;
	double o_c, b, O, o_1 ;
	assign_p( pars, &o_c, &b, &O, &o_1 ) ;

	double v1, v2, v3, v4, err1, err2, err3, err4 ;
	int status =  first_int( &v1, &err1, pars ) + second_int( &v2, &err2, pars )
		+ third_int( &v3, &err3, pars ) + fourth_int( &v4, &err4, pars ) ;

	*result = -(v1 + v2 - v3 - v4)/4 ; *error = (err1 + err2 + err3 + err4)/4 ;

	return status;
}		/* -----  end of function re_gcs  ----- */


/**
 * Various binning and weighting routines for aperture pixel tables.
 *
*/

#include <math.h>

#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>

#include "spc_trace_functions.h"
#include "aXe_utils.h"
#include "aXe_grism.h"
#include "spc_spc.h"
#include "spc_driz.h"
#include "aper_conf.h"
#include "spce_output.h"
#include "spc_optimum.h"

#define SQR(x) ((x)*(x))
#define MIN(x,y) (((x)<(y))?(x):(y))
#define MAX(x,y) (((x)>(y))?(x):(y))

/*
gsl_matrix *
create_weightimage(ap_pixel *ap_p, const beam actbeam,
		   const aperture_conf *conf, const double exptime,
		   const double sky_cps)
{
  drzstamp_dim  dimension;
  drzstamp     *modvar;
  gsl_matrix   *weights;

  dimension = get_resample_dims(ap_p, actbeam);

  if (!dimension.resolution)
    return get_default_weight();

  compute_model_ivar(ap_p, conf->rdnoise, exptime, sky_cps);

  modvar = compute_modvar(ap_p, actbeam, dimension);

  weights = comp_allweight(modvar);

  return weights;
}
*/

/**
 * Function: compute_modvar
 *
 *
 * Parameters:
 * @param ap_p      - the PET table
 * @param actbeam   - beam to compute the modvar structure for
 * @param dimension - dimenasion of the images
 * @param modvar    - the structure to fill
 */
drzstamp *
compute_modvar(ap_pixel *ap_p, const beam actbeam,
	       const drzstamp_dim dimension)
{
  ap_pixel *cur_p;
  ap_pixel *tmp_p;

  quadrangle quad;

  double jacob;

  int jcen, icen;
  int jupp, iupp;
  int jlow, ilow;

  int ii, jj;
  int stpi, stpj;

  double maxarr, arr;
  //int iim, jjm;
  double value, allweig, weig;
  double stpc;
  double totweight;
  drzstamp *modvar;

  // allocate memory for the result structure
  modvar = alloc_drzstamp(dimension);

  // allocate memory
  tmp_p = (ap_pixel *) malloc(sizeof(ap_pixel));

  // go over each pixel
  for (cur_p = ap_p; cur_p->p_x != -1; cur_p++)
    {
      // Skip this pixel if it was not actually used
      if (fabs(cur_p->dist)>actbeam.width+0.5)
        continue;

      // transfer values to the temporary pixel
      tmp_p->lambda  = cur_p->xi;
      tmp_p->dist    = cur_p->dist;
      tmp_p->dxs     = cur_p->dxs;
      tmp_p->dlambda = 1.0;

      // create the quadrangle for the current pixel
      quad = get_quad_from_pixel(tmp_p, actbeam.orient, dimension);

      // get the jacobian (well, easy here)
      // the term "cos(cur_p->dxs)" must be there
      // to correct the enlargement necessary
      // to cover the whole lambda-crossdispersion area!
      // NOT COMPLETELY understood
      jacob = cos(cur_p->dxs);

      // get the central pixel (icen, jcen) of the current PET-pixel
      icen = (int) floor(cur_p->xi   - dimension.xstart+.5);
      jcen = (int) floor(cur_p->dist - dimension.ystart+.5);

      // get the uper and lower extend of the quadrangle in x
      iupp = (int)floor(quad.xmax - (double)icen + 0.5)+1;
      ilow = (int)floor(quad.xmin - (double)icen + 0.5);

      // get the uper and lower extend of the quadrangle in x
      jupp = (int)floor(quad.ymax - (double)jcen + 0.5)+1;
      jlow = (int)floor(quad.ymin - (double)jcen + 0.5);

      maxarr=0.0;
      totweight = 0.0;
      // go over the extend in x
      for (ii=ilow;ii<iupp;ii++)
	{
	  // go over the extend in x
	  for (jj=jlow;jj<jupp;jj++)
	    {

	      // get the coordinates of the current output pixel
	      stpi = icen+ii;
	      stpj = jcen+jj;

	      // check whether the current output pixel is within
	      // the stamp image; continue if not
	      if ((stpi>=dimension.xsize)||(stpi<0)
		  ||(stpj>=dimension.ysize)||(stpj<0))
		continue;

	      // get the area which falls onto the current output pixel
	      arr = boxer(stpi,stpj,quad.x,quad.y);

	      if (arr > 0.0)
		{
		  // get the already existing counts and weights
		  stpc = gsl_matrix_get(modvar->counts,stpi,stpj);
		  weig = gsl_matrix_get(modvar->weight,stpi,stpj);

		  // compute the new, total weight of the current output pixel
		  allweig = weig + arr*cur_p->weight;

		  // do a weighted sum of the count value at the current
		  // output pixel
		  value = (stpc*weig + arr*cur_p->model*cur_p->weight*jacob)
		    / (allweig);

		  // store the new count value and the new weight
		  gsl_matrix_set(modvar->counts,stpi,stpj,value);
		  gsl_matrix_set(modvar->weight,stpi,stpj,allweig);
		}
	    }
	}
    }

  // freep the temporary PET pixel
  free(tmp_p);

  // return the result
  return modvar;
}

void
shift_tracelength(ap_pixel *ap_p, const double xi_shift)
{
  ap_pixel *cur_p;

  // go over each PET pixel
  for (cur_p = ap_p; cur_p->p_x != -1; cur_p++)
    {
      // apply a shift in trace distance
      cur_p->xi += xi_shift;
    }
}

/**
 * Function: prepare_inv_variance
 * The function prepares the inverse model variances
 * for a data set containing an object and a background PET.
 * This is done in subroutines which compute the
 * theoretical inverse pixel variance following
 * different methods for the object and the background PET.
 *
 * Parameters:
 * @param ap_p     - the object PET table
 * @param bg_p     - the background PET table
 * @param dobck    - integer indicating background subtraction
 * @param conf     - the configuration structure
 * @param exptime  - the exposure time
 * @param sky_cps  - the sky background
 */
void
prepare_inv_variance(ap_pixel *ap_p, ap_pixel *bg_p, const int dobck,
		     const aperture_conf *conf, const double exptime,
		     const double sky_cps, const double xi_shift)
{

  if (dobck)
    {
      // shift both, the object and the background PET
      shift_tracelength(ap_p, xi_shift);
      shift_tracelength(bg_p, xi_shift);

      // compute the inverse variance for the object + background PET
      compute_total_ivar(bg_p, bg_p, conf->rdnoise, exptime, sky_cps);
    }
  else
    {
      // shift both, the object PET
      shift_tracelength(ap_p, xi_shift);

      // compute the inverse variance for the object PET
      compute_object_ivar(ap_p, conf->rdnoise, exptime, sky_cps);
    }
}

/**
 * Function: compute_model_ivar
 * The function computes for each PET pixel the
 * associated inverse variance value. The input
 * for variance and inverse variance are the
 * model value, the contamination value, the
 * constant sky background value and the readnoise.
 * The computed inverse variance value is stored
 * in the weight entry of the PET.
 * The function also offers the possibility to
 * shift the trace distance by a fixed amount.
 *
 * Parameters:
 * @param ap_p     - the PET table
 * @param rdnoise  - the readnoise value
 * @param exptime  - the exposure time
 * @param sky_cps  - the sky background
 * @param xi_shift - shift in trace distance
 */
void
compute_object_ivar(ap_pixel *ap_p, const double rdnoise,
		    const double exptime, const double sky_cps)
{
  double variance;
  double sqr_rdnoise;
  double sqr_exptime;
  ap_pixel *cur_p;

  // square the readnoise
  sqr_rdnoise = rdnoise*rdnoise;

  // square the exposure time
  sqr_exptime = exptime*exptime;

  // go over each PET pixel
  for (cur_p = ap_p; cur_p->p_x != -1; cur_p++)
    {
	// compute the variance
	variance = ((cur_p->model + cur_p->contam + sky_cps) * exptime
		    + sqr_rdnoise) / sqr_exptime;

      // store the inverse variance
      cur_p->weight = 1.0/variance;
    }
}

/**
 * Function: compute_value_ivar
 * The function computes for each PET pixel the
 * associated inverse variance value. The input
 * to derive the variance is the count value and
 * the readnoise.
 * The computed inverse variance value is stored
 * in the weight entry of the PET.
 * The function also offers the possibility to
 * shift the trace distance by a fixed amount.
 *
 * Parameters:
 * @param ap_p    - the PET table
 * @param rdnoise  - the readnoise value
 * @param exptime - the exposure time
 * @param sky_cps - the sky background
 * @param xi_shift - shift in trace distance
 */
void
compute_total_ivar(ap_pixel *ap_p, const ap_pixel *bg_p, const double rdnoise,
		   const double exptime, const double sky_cps)
{
  double variance;
  double sqr_rdnoise;
  double sqr_exptime;

  ap_pixel *cur_p;
  const ap_pixel *bac_p;

  // square the readnoise
  sqr_rdnoise = rdnoise*rdnoise;

  // square the exposure time
  sqr_exptime = exptime*exptime;

  // go over each PET pixel
  bac_p = bg_p;
  for (cur_p = ap_p; cur_p->p_x != -1; cur_p++)
    {
      // check that the foreground and background
      // PET element describe the same pixel
      if (bac_p->x != cur_p->x || bac_p->y != cur_p->y)
	aXe_message (aXe_M_FATAL, __FILE__, __LINE__,
		     "aXe_PET2SPC: Background PET and Object PET "
		     "have different pixel orders in PET's.\n");
      else
      // compute the variance
      variance = ((cur_p->count + bac_p->count + sky_cps) * exptime + sqr_rdnoise) / sqr_exptime;

      // store the inverse variance
      cur_p->weight = 1.0/variance;

      // increase the background pointer
      bac_p++;
    }
}

/**
 * Function: alloc_drzstamp
 * Allocates memory for a drizzle stamp structure
 * according the the dimension specified in the input.
 *
 * Parameters:
 * @param dimension - the dimesions to allocate
 *
 * Returns:
 * @return ret  - the drizzle stamp image allocated
 */
drzstamp *
alloc_drzstamp(const drzstamp_dim dimension)
{
  drzstamp   *ret;
  gsl_matrix *counts;
  gsl_matrix *weight;

  ret = (drzstamp *) malloc(sizeof(drzstamp));

  /* Allocate the stamp matrix*/
  /* Fill stamp with NaN values */
  counts = gsl_matrix_alloc(dimension.xsize,dimension.ysize);
  //  gsl_matrix_set_all(counts, GSL_NAN);
  gsl_matrix_set_all(counts, 0.0);

  /* Allocate the weight matrix*/
  /* Fill weight with 0.0 values */
  weight = gsl_matrix_alloc(dimension.xsize,dimension.ysize);
  gsl_matrix_set_all(weight, 0.0);

  // fill the output structure
  ret->counts = counts;
  ret->weight = weight;

  // return the allocated structure
  return ret;
}

/**
 * Function: get_all_dims
 * The function derives the dimensional information
 * on the foreground and, if available, on the
 * background PET. The dimensions are compared, and
 * in case of inequality an error is thrown.
 * If the dimensions are equal, one of them is
 * returned.
 *
 * Parameters:
 * @param ap_p    - the table of aperture pixels
 * @param bg_p    - the table of background pixels
 * @param actbeam - the beam to compute the dimensions for
 * @param dobck   - integer indicating background subtraction
 *
 * Returns:
 * @return ret  - the dimension structure
 */
drzstamp_dim
get_all_dims(const ap_pixel *ap_p, const ap_pixel *bg_p, const beam actbeam,
	     const int dobck)
{
  drzstamp_dim ret;
  drzstamp_dim bck;

  // get the aperture PET diemnsion
  ret = get_resample_dims(ap_p, actbeam);

  if (dobck)
    {
      // get the background PET dimension
      bck = get_resample_dims(bg_p, actbeam);

      // check whether the two dimensions
      // are equal, throw an error if not
      if (ret.resolution != bck.resolution
	  || ret.xstart  != bck.xstart
	  || ret.ystart  != bck.ystart
	  || ret.xsize   != bck.xsize
	  || ret.ysize   != bck.ysize)
	aXe_message (aXe_M_FATAL, __FILE__, __LINE__,
		     "aXe_PET2SPC: Background PET and Object PET "
		     "do not have the same dimension.\n");
    }

  // return one of the dimensional infos
  return ret;
}

/**
 * Function: get_resample_dims
 * The function parses through a PET table and determines
 * the dimension of the image in the coordinates
 * trace distance - cross-dispersion direction. Those
 * dimensions are stored in a special structure and
 * returned to the calling routine.
 *
 * Parameters:
 * @param ap_p    - the table of aperture pixels
 * @param actbeam - the beam to compute the dimensions for
 *
 * Returns:
 * @return ret  - the dimension structure
 */
drzstamp_dim
get_resample_dims(const ap_pixel *ap_p, const beam actbeam)
{
  drzstamp_dim ret;

  const ap_pixel *cur_p;

  int i_xint, i_dist;
  int imin_xint=0, imin_dist=0;
  int imax_xint=0, imax_dist=0;

  int npixel = 0;

  // immediately return empty PET's
  if (ap_p==NULL)
    return get_default_dim();

  // set the tmp pixel to the table start
  cur_p = ap_p;

  // initialize some relevant numbers
  // based on the first pixel
  i_xint = floor(cur_p->xi   + 0.5);
  i_dist = floor(cur_p->dist + 0.5);
  imin_xint = i_xint;
  imax_xint = i_xint;
  imin_dist = i_dist;
  imax_dist = i_dist;

  // go over all pixels, check
  // for new MIN's ans MAX's
  for (cur_p = ap_p; cur_p->p_x != -1; cur_p++)
    {
      // Skip the pixel if it was not actually used
      if (fabs(cur_p->dist)>actbeam.width+.5)
	continue;

      // compute the rounded value in (x,y)
      i_xint = floor(cur_p->xi   + 0.5);
      i_dist = floor(cur_p->dist + 0.5);

      // check for extrema in x
      imin_xint = MIN(imin_xint, i_xint);
      imax_xint = MAX(imax_xint, i_xint);

      // check for extrema in y
      imin_dist = MIN(imin_dist, i_dist);
      imax_dist = MAX(imax_dist, i_dist);

      // enhance the pixel counter
      npixel++;
    }

  // in case there were no valid pixels,
  // return a dummy structure
  if (!npixel)
    return get_default_dim();

  // fill the structure with the correct values
  ret.resolution = 1.0;
  ret.xstart     = imin_xint - NSPARE_PIX;
  ret.ystart     = imin_dist - NSPARE_PIX;
  ret.xsize      = imax_xint - imin_xint + 2 * NSPARE_PIX;
  ret.ysize      = imax_dist - imin_dist + 2 * NSPARE_PIX;

  // return the structure
  return ret;
}

/**
 * Function: get_default_dim
 * The function creates and returns the default
 * dimension structure. The dewfault dimension
 * structure contains a 0.0 in all fields.
 *
 * Returns:
 * @return res  - the dimension structure
 */
drzstamp_dim
get_default_dim()
{
  drzstamp_dim ret;

  // just set everything to zero
  ret.resolution=0.0;
  ret.xstart = 0;
  ret.ystart = 0;
  ret.xsize  = 0;
  ret.ysize  = 0;

  // return the result
  return ret;
}

/**
 * Function: get_default_modvar
 * The function computes the default drizzle stamp
 * structure, which in this case is used for
 * tha calculation of the resampled variance and
 * profile image.
 *
 * Returns:
 * @return res  - the resulting drzstamp structure
 */
drzstamp *
get_default_modvar()
{
  gsl_matrix *counts;
  gsl_matrix *weight;
  drzstamp *res;

  res = (drzstamp *) malloc(sizeof(drzstamp));

  /* Create a dummy stamp image */
  counts = gsl_matrix_alloc(10,10);
  weight = gsl_matrix_alloc(10,10);
  /* Fill stamp with 0.0 values */
  gsl_matrix_set_all(counts, 0.0);
  gsl_matrix_set_all(weight, 0.0);

  // fill the output
  res->counts = counts;
  res->weight = weight;

  // return the output
  return res;
}

gsl_matrix *
get_default_weight()
{
  gsl_matrix *res;

  res = gsl_matrix_alloc(10,10);
  gsl_matrix_set_all(res, 0.0);

  // return the output
  return res;
}

/*
 * Function: comp_opt_weight
 * The function uses the model map and and the variance
 * map and computes optimale weigths according to the
 * Hoorne method.
 * Pixels outside of the extraction
 * area are set to 1000.0
 *
 * Parameters:
 * @param mod_map - the matrix with the model values
 * @param var_map - the matrix with the variance values
 * @param ob      - the object structure
 *
 * Returns:
 * @return weight - the matrix with the exposure time weights
 */
gsl_matrix *
comp_allweight(drzstamp *modvar)
{
  gsl_matrix *weight;

  double mod_sum, weight_sum;
  double contr, norm, allweight;
  double mod_val;
  double act_weight=0;

  //int beamInt = 0;
  int i, j;

  // allocate the weight matrix and set the default
  weight = gsl_matrix_alloc(modvar->counts->size1, modvar->counts->size2);
  gsl_matrix_set_all(weight, 1000.0);

  //* go over all columns
  for (i=0; i < (int)modvar->counts->size1; i++)
    {
      mod_sum = 0.0;
      contr = 0.0;
      allweight = 0.0;
      weight_sum=0.0;

      // determine for each column the total model counts
      // and the number of pixels with non-zero model_counts
      for (j=0; j < (int)modvar->counts->size2; j++)
	{
	  mod_sum = mod_sum + gsl_matrix_get(modvar->counts, i, j);
	  contr = contr + 1.0;
	}

      // check whether the column has model values.
      // normalize the model values and compute
      // optimal weights if yes.
      if (mod_sum > 0.0)
	{
	  //* determine the mean model value
	  //	  norm = mod_sum / contr;
	  norm = mod_sum;

	  // go over each row
	  for (j=0; j < (int)modvar->counts->size2; j++)
	    {
	      // normalize the model counts
	      mod_val = gsl_matrix_get(modvar->counts, i, j)/norm;

	      // store the normalized model counts
	      gsl_matrix_set(modvar->counts, i, j, mod_val);

	      // add up the normalization value for the optimal weights
	      weight_sum = weight_sum
		+ mod_val*mod_val*gsl_matrix_get(modvar->weight, i, j);
	    }

	  // finally compute and write the weights:
	  // go over each pixel
	  for (j=0; j < (int)modvar->counts->size2; j++)
	    {
	      if (gsl_matrix_get(modvar->counts, i, j) > 0.0 )
		{
		  // compute and set the individual pixel weight
		  act_weight = gsl_matrix_get(modvar->counts,i,j)
		    *gsl_matrix_get(modvar->weight,i,j)/weight_sum;
		  //		  gsl_matrix_set(weight,i,j,gsl_matrix_get(modvar->counts,i,j)
		  //		 *gsl_matrix_get(modvar->weight,i,j)/weight_sum);
		  gsl_matrix_set(weight, i, j, act_weight);
		  allweight = allweight + gsl_matrix_get(weight, i, j);
		}
	      else
		{
		  // set the default value if no model value is zero
		  gsl_matrix_set(weight, i, j,0.0);
		}
	    }
	}
      // if the column does not have model values at all:
      else
	{
	  // go over each row
	  for (j=0; j < (int)modvar->counts->size2; j++)
	    {
	      // set the inside default value
	      gsl_matrix_set(weight, i,j,1.0);
	      allweight = allweight + gsl_matrix_get(modvar->counts, i, j);

	    }
	}
    }

  // return the weight matrix
  return weight;
}


/*! \file Function.h
 *  \brief header file that defines several function structures
 */
// Function.h

#ifndef NBODYFUNCTION_H
#define NBODYFUNCTION_H

#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>

namespace Math
{

/// \struct Math::gsl_math_function
/// \brief gsl function wrapper
struct gsl_math_function
{
  int (* function) (const gsl_vector *params, void * data, gsl_vector *output);
};

/// \struct Math::gsl_math_function
/// \brief gsl function wrapper differentiating wrt params, storing info in Jacobian matrix
struct gsl_math_function_df
{
  int (* function) (const gsl_vector * params, void * data, gsl_matrix * outputJacobian);
};

/// \struct Math::gsl_fitting_data
struct gsl_fitting_data {
  size_t n;
  double * x;
  double * y;
  size_t npar;
  int * iparindex;
  size_t nallparams;
  double *allparams;
};

/// \struct Math::math_function
/// \brief single dependent variable function
struct math_function
{
  Double_t (* function) (Double_t x, void * params);
  int (* gsl_function) (const gsl_vector *params, void * data, gsl_vector *output);
  int (* gsl_function_df) (const gsl_vector *params, void * data, gsl_matrix * outputJacobian);
  void * params;
};

/// \struct Math::math_multidim_function
/// \brief multi-dimensional function
struct math_multidim_function
{
  Double_t (* function) (Double_t *x, int ndim, void * params);
  int ndim;
  void * params;
};
}
#endif


#ifndef __LISAUTILS_H__
#define __LISAUTILS_H__ 1

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <gsl/gsl_cdf.h>

#include "constants.h"
#include "struct.h"
#include "EOBNRv2HMROMstruct.h"
#include "EOBNRv2HMROM.h"
#include "waveform.h"
#include "wip.h"
#include "splinecoeffs.h"
#include "fresnel.h"
#include "likelihood.h"
#include "LISAFDresponse.h"
#include "LISAnoise.h"

#if defined(__cplusplus)
extern "C" {
#define complex _Complex
#elif 0
} /* so that editors will match preceding brace */
#endif

/***************** Enumerator to choose what masses/time set to sample for *****************/

typedef enum SampleMassParamstag {
  m1m2,
  Mchirpeta
} SampleMassParamstag;
/* Superseded by sampleLframe */
// typedef enum SampleTimeParamtag {
//   tSSB,
//   tL
// } SampleTimeParamtag;

/* Function to convert string input SampleMassParams to tag */
SampleMassParamstag ParseSampleMassParamstag(char* string);
/* Function to convert string input SampleTimeParams to tag */
//SampleTimeParamtag ParseSampleTimeParamtag(char* string);

/***************** Structure definitions *****************/

/* Parameters for the generation of a LISA waveform (in the form of a list of modes) */
typedef struct tagLISAParams {
  double tRef;               /* reference time (s) - GPS time at the frequency representing coalescence */
  double phiRef;             /* reference phase (rad) - phase at the frequency representing coalescence (or at fRef if specified) */
  double m1;                 /* mass of companion 1 (solar masses, default 2e6) */
  double m2;                 /* mass of companion 2 (solar masses, default 1e6) */
  double distance;           /* distance of source (Mpc, default 1e3) */
  double lambda;             /* first angle for the position in the sky (rad, default 0) */
  double beta;               /* second angle for the position in the sky (rad, default 0) */
  double inclination;        /* inclination of L relative to line of sight (rad, default PI/3) */
  double polarization;       /* polarization angle (rad, default 0) */
  int nbmode;                /* number of modes to generate (starting with 22) - defaults to 5 (all modes) */
} LISAParams;

/* Global parameters for the waveform generation and overlap computation */
typedef struct tagLISAGlobalParams {
  double fRef;               /* reference frequency (Hz, default 0 which is interpreted as Mf=0.14) */
  double deltatobs;          /* max duration of observation (years, default 2) - the start of the signals might be cut in time instead of cut in frequency */
  double minf;               /* Minimal frequency (Hz, default=0) - when set to 0, use the lowest frequency where the detector noise model is trusted __LISASimFD_Noise_fLow (set somewhat arbitrarily)*/
  double maxf;               /* Maximal frequency (Hz, default=0) - when set to 0, use the highest frequency where the detector noise model is trusted __LISASimFD_Noise_fHigh (set somewhat arbitrarily)*/
  int tagextpn;              /* Tag to allow PN extension of the waveform at low frequencies */
  int tagtRefatLISA;         /* Tag to signal time to be referenced to arrival at LISA rather than at SSB */
  double Mfmatch;            /* When PN extension allowed, geometric matching frequency: will use ROM above this value. If <=0, use ROM down to the lowest covered frequency */
  int setphiRefatfRef;       /* Flag for adjusting the FD phase at phiRef at the given fRef, which depends also on tRef - if false, treat phiRef simply as an orbital phase shift (minus an observer phase shift) (default=1) */
  int nbmodeinj;             /* number of modes to include in the injection (starting with 22) - defaults to 5 (all modes) */
  int nbmodetemp;            /* number of modes to include in the templates (starting with 22) - defaults to 5 (all modes) */
  int tagint;                /* Tag choosing the integrator: 0 for wip (default), 1 for linear integration */
  TDItag tagtdi;             /* Tag choosing the TDI variables to use */
  int nbptsoverlap;          /* Number of points to use in loglinear overlaps (default 32768) */
  LISAconstellation *variant;  /* A structure defining the LISA constellation features */
  int zerolikelihood;        /* Tag to zero out the likelihood, to sample from the prior for testing purposes (default 0) */
  int frozenLISA;            /* Freeze the orbital configuration to the time of peak of the injection (default 0) */
  ResponseApproxtag responseapprox;    /* Approximation in the GAB and orb response - choices are full (full response, default), lowfL (keep orbital delay frequency-dependence but simplify constellation response) and lowf (simplify constellation and orbital response) - WARNING : at the moment noises are not consistent, and TDI combinations from the GAB are unchanged */
  int tagsimplelikelihood22; /* Tag to use simplified, frozen-LISA and lowf likelihood where mode overlaps are precomputed - 22-mode only - can only be used when the masses and time (tL) are pinned to injection values (Note: when using --snr, distance adjustment done using responseapprox, not the simple response) */
  int tagsimplelikelihoodHM; /* Tag to use simplified, frozen-LISA and lowf likelihood where mode overlaps are precomputed - set of modes - can only be used when the masses and time (tL) are pinned to injection values (Note: when using --snr, distance adjustment done using responseapprox, not the simple response) */
} LISAGlobalParams;

typedef struct tagLISASignalCAmpPhase
{
  struct tagListmodesCAmpPhaseFrequencySeries* TDI1Signal;   /* Signal in the TDI channel 1, in the form of a list of the contribution of each mode */
  struct tagListmodesCAmpPhaseFrequencySeries* TDI2Signal;   /* Signal in the TDI channel 2, in the form of a list of the contribution of each mode */
  struct tagListmodesCAmpPhaseFrequencySeries* TDI3Signal;   /* Signal in the TDI channel 3, in the form of a list of the contribution of each mode */
  double TDI123hh;                                           /* Combined Inner product (h|h) for TDI channels 123 */
} LISASignalCAmpPhase;

typedef struct tagLISAInjectionCAmpPhase
{
  struct tagListmodesCAmpPhaseSpline* TDI1Splines;   /* Signal in the TDI channel 1, in the form of a list of splines for the contribution of each mode */
  struct tagListmodesCAmpPhaseSpline* TDI2Splines;   /* Signal in the TDI channel 2, in the form of a list of splines for the contribution of each mode */
  struct tagListmodesCAmpPhaseSpline* TDI3Splines;   /* Signal in the TDI channel 3, in the form of a list of splines for the contribution of each mode */
  double TDI123ss;                                   /* Combined Inner product (s|s) for TDI channels 123 */
} LISAInjectionCAmpPhase;

typedef struct tagLISASignalReIm /* We don't store the SNRs here, as we will use -1/2(h-s|h-s) for the likelihood */
{
  struct tagReImFrequencySeries* TDI1Signal;   /* Signal in the TDI channel 1, in the form of a Re/Im frequency series where the modes have been summed */
  struct tagReImFrequencySeries* TDI2Signal;   /* Signal in the TDI channel 2, in the form of a Re/Im frequency series where the modes have been summed */
  struct tagReImFrequencySeries* TDI3Signal;   /* Signal in the TDI channel 3, in the form of a Re/Im frequency series where the modes have been summed */
} LISASignalReIm;

typedef struct tagLISAInjectionReIm /* Storing the vectors of frequencies and noise values - We don't store the SNRs here, as we will use -1/2(h-s|h-s) for the likelihood */
{
  struct tagReImFrequencySeries* TDI1Signal;   /* Signal in the TDI channel 1, in the form of a Re/Im frequency series where the modes have been summed */
  struct tagReImFrequencySeries* TDI2Signal;   /* Signal in the TDI channel 2, in the form of a Re/Im frequency series where the modes have been summed */
  struct tagReImFrequencySeries* TDI3Signal;   /* Signal in the TDI channel 3, in the form of a Re/Im frequency series where the modes have been summed */
  gsl_vector* freq;                            /* Vector of frequencies of the injection (assumed to be the same for A,E,T) */
  gsl_vector* noisevalues1;                    /* Vector of noise values on freq for TDI channel 1 */
  gsl_vector* noisevalues2;                    /* Vector of noise values on freq for TDI channel 2 */
  gsl_vector* noisevalues3;                    /* Vector of noise values on freq for TDI channel 3 */
} LISAInjectionReIm;

typedef struct tagLISAPrior {
  SampleMassParamstag samplemassparams;   /* Choose the set of mass params to sample from - options are m1m2 and Mchirpeta (default m1m2) */
  //SampleTimeParamtag sampletimeparam;     /* Choose the time param to sample from - options are tSSB and tL (default tSSB) */
  int sampleLframe;          /* flag to sample L-frame params tL, lambdaL, betaL, psiL instead of SSB-frame params -- priors are interpreted for those L-frame params - no phase transformation */
  double deltaT;             /* width of time prior centered on injected value (s) (default 1e5) */
  double comp_min;           /* minimum component mass (solar masses) (default 1e4) */
  double comp_max;           /* maximum component mass (solar masses) (default 1e8) */
  double mtot_min;           /* minimum total mass (solar masses) (default 5*1e4) */
  double mtot_max;           /* maximum total mass (solar masses) (default 1e8) */
  double qmax;               /* maximum asymmetric mass ratio (>=1) (default 11.98) */
  double Mchirp_min;         /* Minimum chirp mass in Solar masses - when sampling Mchirpeta (default=2e4) */
  double Mchirp_max;         /* Maximum chirp mass in Solar masses - when sampling Mchirpeta (default=4e7) */
  double eta_min;            /* Minimum symmetric mass ratio eta - when sampling Mchirpeta (default=0.072) */
  double eta_max;            /* Maximum symmetric mass ratio eta - when sampling Mchirpeta (default=0.25) */
  double dist_min;           /* minimum distance of source (Mpc) (default 100) */
  double dist_max;           /* maximum distance of source (Mpc) (default 40*1e3) */
  double lambda_min;         /* minimum lambda (rad, default 0) - for testing */
  double lambda_max;         /* maximum lambda (rad, default 2pi) - for testing */
  double beta_min;           /* minimum beta (rad, default 0) - for testing */
  double beta_max;           /* maximum beta (rad, default pi) - for testing */
  double phase_min;          /* minimum phase (rad, default 0) - for testing */
  double phase_max;          /* maximum phase (rad, default 2pi) - for testing */
  double pol_min;            /* minimum polarization (rad, default 0) - for testing */
  double pol_max;            /* maximum polarization (rad, default 2pi) - for testing */
  double inc_min;            /* minimum inclination (rad, default 0) - for testing */
  double inc_max;            /* maximum inclination (rad, default pi) - for testing */
  double fix_m1;
  double fix_m2;
  double fix_Mchirp;
  double fix_eta;
  double fix_time;
  double fix_lambda;
  double fix_beta;
  double fix_phase;
  double fix_pol;
  double fix_dist;
  double fix_inc;
  int pin_m1;
  int pin_m2;
  int pin_Mchirp;
  int pin_eta;
  int pin_time;
  int pin_lambda;
  int pin_beta;
  int pin_phase;
  int pin_pol;
  int pin_dist;
  int pin_inc;
  double snr_target;
  int rescale_distprior;
  int flat_distprior;
  int logflat_massprior;
} LISAPrior;

typedef struct tagLISARunParams {
  double eff;                /* target efficiency (default 0.1) */
  double tol;                /* logZ tolerance (default 0.5) */
  int    consteff;           /* constant efficiency mode (default 0) */
  int    nlive;              /* number of live points (default 1000) */
  int    writeparams;        /* Write params - if 1, write run parameters to file (default 1) */
  char   outroot[200];       /* output root (default "chains/LISAinference_") */
  int    bambi;              /* run BAMBI? (default 0) */
  int    resume;             /* resume form previous run? (default 0) */
  int    maxiter;            /* max number of iterations (default 0 - ignore) */
  char   netfile[200];       /* NN settings file (default "LISAinference.inp") */
  int    mmodal;             /* use multimodal decomposition ? */
  int    maxcls;             /* max number of modes in multimodal decomposition */
  int    nclspar;            /* number of parameters to use for multimodal decomposition - in the order of the cube */
  double ztol;               /* in multimodal decomposition, modes with lnZ lower than Ztol are ignored */
  int    seed;               /* seed the inference by setting one of the live points to the injection ? */
} LISARunParams;

/* Parameters for the generation of a LISA waveform (in the form of a list of modes) */
typedef struct tagLISAAddParams {
  double tRef;               /* reference time (s) - GPS time at the frequency representing coalescence */
  double phiRef;             /* reference phase (rad) - phase at the frequency representing coalescence (or at fRef if specified) */
  double m1;                 /* mass of companion 1 (solar masses, default 2e6) */
  double m2;                 /* mass of companion 2 (solar masses, default 1e6) */
  double distance;           /* distance of source (Mpc, default 1e3) */
  double lambda;             /* first angle for the position in the sky (rad, default 0) */
  double beta;               /* second angle for the position in the sky (rad, default 0) */
  double inclination;        /* inclination of L relative to line of sight (rad, default PI/3) */
  double polarization;       /* polarization angle (rad, default 0) */
  int loadparamsfile;        /* Option to load physical parameters from file for LISAlikelihood and to output resulting likelihoods to file (default 0) */
  int nlinesparams;          /* Number of lines in params file for LISAlikelihood */
  char indir[256];           /* Input directory for LISAlikelihood */
  char infile[256];          /* Input file for LISAlikelihood */
  char outdir[256];          /* Output directory for LISAlikelihood */
  char outfile[256];         /* Output file for LISAlikelihood */
} LISAAddParams;

/* Saved precomputed values for the injection , when using the simplified frozenLISA lowf likelihood - here for now assumes 22 mode only, old convention */
/* NOTE: difference in convention, new convention has a factor 3 in the integrand defining <lm|l'm'> */
typedef struct tagSimpleLikelihoodPrecomputedValues22 {
  double normalization;
  double complex sa;
  double complex se;
} SimpleLikelihoodPrecomputedValues22;
/* Saved precomputed values for the injection , when using the simplified frozenLISA lowf likelihood - here for set of modes, new convention */
/* NOTE: difference in convention, new convention has a factor 3 in the integrand defining <lm|l'm'> */
typedef struct tagSimpleLikelihoodPrecomputedValuesHM {
  gsl_matrix* Lambda_lm_lpmp; /* Matrix <lm|l'm'> */
  gsl_vector* sa_lm_real;         /* Vector s_a^{lm}, real part*/
  gsl_vector* sa_lm_imag;         /* Vector s_a^{lm}, imaginary part */
  gsl_vector* se_lm_real;         /* Vector s_e^{lm}, real part*/
  gsl_vector* se_lm_imag;         /* Vector s_e^{lm}, imaginary part */
} SimpleLikelihoodPrecomputedValuesHM;

/************ Functions for LISA parameters, injection, likelihood, prior ************/

/* Parse command line to initialize LISAParams, LISAGlobalParams, LISAPrior, and LISARunParams objects */
/* Masses are input in solar masses and distances in Mpc - converted in SI for the internals */
void parse_args_LISA(ssize_t argc, char **argv,
  LISAParams* params,
  LISAGlobalParams* globalparams,
  LISAPrior* prior,
  LISARunParams* run,
  LISAAddParams* addparams);

/* Functions to print the parameters of the run in files for reference */
int print_parameters_to_file_LISA(
  LISAParams* params,
  LISAGlobalParams* globalparams,
  LISAPrior* prior,
  LISARunParams* run);
int print_rescaleddist_to_file_LISA(
  LISAParams* params,
  LISAGlobalParams* globalparams,
  LISAPrior* prior,
  LISARunParams* run);
int print_snrlogZ_to_file_LISA(LISARunParams* run, double SNR, double logZ);
/* Function printing injection/signal parameters to stdout */
void report_LISAParams(LISAParams* params);

/* Initialization and clean-up for LISASignal structures */
void LISASignalCAmpPhase_Cleanup(LISASignalCAmpPhase* signal);
void LISASignalCAmpPhase_Init(LISASignalCAmpPhase** signal);
void LISAInjectionCAmpPhase_Cleanup(LISAInjectionCAmpPhase* signal);
void LISAInjectionCAmpPhase_Init(LISAInjectionCAmpPhase** signal);
void LISASignalReIm_Cleanup(LISASignalReIm* signal);
void LISASignalReIm_Init(LISASignalReIm** signal);
void LISAInjectionReIm_Cleanup(LISAInjectionReIm* signal);
void LISAInjectionReIm_Init(LISAInjectionReIm** signal);

//Function to restrict range of the signal/injection to within desired limits.
int listmodesCAmpPhaseTrim(ListmodesCAmpPhaseFrequencySeries* listSeries);

/* Function generating a LISA signal as a list of modes in CAmp/Phase form, from LISA parameters */
int LISAGenerateSignalCAmpPhase(
  struct tagLISAParams* params,                 /* Input: set of LISA parameters of the signal */
  struct tagLISASignalCAmpPhase* signal);  /* Output: structure for the generated signal */
/* Function generating a LISA injection as a list of modes, given as preinterpolated splines, from LISA parameters */
int LISAGenerateInjectionCAmpPhase(
  struct tagLISAParams* injectedparams,    /* Input: set of LISA parameters of the signal */
  struct tagLISAInjectionCAmpPhase* signal);  /* Output: structure for the generated signal */
/* Function generating a LISA signal as a frequency series in Re/Im form where the modes have been summed, from LISA parameters - takes as argument the frequencies on which to evaluate */
int LISAGenerateSignalReIm(
  struct tagLISAParams* params,       /* Input: set of LISA parameters of the template */
  gsl_vector* freq,                   /* Input: frequencies on which evaluating the waveform (from the injection) */
  struct tagLISASignalReIm* signal);  /* Output: structure for the generated signal */
/* Function generating a LISA injection as a frequency series in Re/Im form where the modes have been summed, from LISA parameters - frequencies on which to evaluate are to be determined internally */
int LISAGenerateInjectionReIm(
  struct tagLISAParams* injectedparams,       /* Input: set of LISA parameters of the injection */
  double fLow,                                /* Input: starting frequency */
  int nbpts,                                  /* Input: number of frequency samples */
  int tagsampling,                            /* Input: tag for using linear (0) or logarithmic (1) sampling */
  struct tagLISAInjectionReIm* signal);       /* Output: structure for the generated signal */

/*Wrapper for waveform generation with possibly a combination of EOBNRv2HMROM and TaylorF2*/
/* Note: GenerateWaveform accepts masses and distances in SI units, whereas LISA params is in solar masses and Mpc */
int GenerateWaveform(
  struct tagListmodesCAmpPhaseFrequencySeries **listhlm,  /* Output: list of modes in Frequency-domain amplitude and phase form */
  int nbmode,                                    /* Number of modes to generate (starting with the 22) */
  double f_match,                                /* Minimum frequency using EOBNRv2HMROM */
  double f_min,                                  /* Minimum frequency required */
  double deltatRef,                              /* Time shift so that the peak of the 22 mode occurs at deltatRef */
  double phiRef,                                 /* Phase at reference frequency */
  double fRef,                                   /* Reference frequency (Hz); 0 defaults to fLow */
  double m1SI,                                   /* Mass of companion 1 (kg) */
  double m2SI,                                   /* Mass of companion 2 (kg) */
  double distance                               /* Distance of source (m) */
		     );

/* Non-spinning merger TaylorF2 waveform, copied and condensed from LAL */
void TaylorF2nonspin(
		double *amp,                            /**< FD waveform amplitude (modulus)*/
		double *phase,                          /**< FD waveform phase */
		const double *freqs,                    /**< frequency points at which to evaluate the waveform (Hz) */
		const int size,                         /** number of freq samples */
		const double m1_SI,                     /**< mass of companion 1 (kg) */
		const double m2_SI,                     /**< mass of companion 2 (kg) */
		const double distance,                  /** distance (m) */
		const double imatch                     /**< index at which to match phase;
							   assumes arrays are preloaded at imatch and imatch+1
							   with the required result */
		     );

/* checks prior boundaires */
int PriorBoundaryCheckm1m2(LISAPrior *prior, double *Cube);
int PriorBoundaryCheckMchirpeta(LISAPrior *prior, double *Cube);

/* Prior functions from Cube to physical parameters
   x1 is min, x2 is max when specified
   r is Cube value */
double CubeToFlatPrior(double r, double x1, double x2);
double CubeToLogFlatPrior(double r, double x1, double x2);
double CubeToPowerPrior(double p, double r, double x1, double x2);
double CubeToGaussianPrior(double r, double mean, double sigma);
double CubeToSinPrior(double r, double x1, double x2);
double CubeToCosPrior(double r, double x1, double x2);
/* Prior functions from physical parameters to Cube
   x1 is min, x2 is max when specified
   y is physical value */
double FlatPriorToCube(double y, double x1, double x2);
double LogFlatPriorToCube(double y, double x1, double x2);
double PowerPriorToCube(double p, double y, double x1, double x2);
double SinPriorToCube(double y, double x1, double x2);
double CosPriorToCube(double y, double x1, double x2);

/* log-Likelihood functions */
double CalculateLogLCAmpPhase(LISAParams *params, LISAInjectionCAmpPhase* injection);
double CalculateLogLReIm(LISAParams *params, LISAInjectionReIm* injection);

/* Functions for simplified likelihood using precomputing relevant values */
int LISAComputeSimpleLikelihoodPrecomputedValues22(SimpleLikelihoodPrecomputedValues22* simplelikelihoodvals22, LISAParams* params);
int LISAComputeSimpleLikelihoodPrecomputedValuesHM(SimpleLikelihoodPrecomputedValuesHM* simplelikelihoodvalsHM, LISAParams* params);
double CalculateLogLSimpleLikelihood22(SimpleLikelihoodPrecomputedValues22* simplelikelihoodvals22, LISAParams* params);
double CalculateLogLSimpleLikelihoodHM(SimpleLikelihoodPrecomputedValuesHM* simplelikelihoodvalsHM, LISAParams* params);

/************ Global Parameters ************/

extern LISAParams* injectedparams;
extern LISAGlobalParams* globalparams;
extern LISAPrior* priorParams;
extern LISAAddParams* addparams;
extern double logZdata; /* TODO: not used */
extern SimpleLikelihoodPrecomputedValues22* simplelikelihoodinjvals22;
extern SimpleLikelihoodPrecomputedValuesHM* simplelikelihoodinjvalsHM;

#if 0
{ /* so that editors will match succeeding brace */
#elif defined(__cplusplus)
}
#endif

#endif


/*
 Copyright (C) 2004-2007 M. Oliveira, F. Nogueira

 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, 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.
*/

#include <config.h>
#include <string_f.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_sf.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_odeiv.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_multiroots.h>

/* This file contains interface routines that allow Fortran 90/95
routines to call GSL routines. The names of these routines are
compiler dependent: Fortran 90/95 compilers have different
name-mangling schemes.  For a description of the routines please refer
to the GSL documentation. */

          /* Error Handling */

void FC_FUNC_(gsl_strerror, GSL_STRERROR)
     (int *err, STR_F_TYPE res STR_ARG1)
{
  char *c;

  c = gsl_strerror(*err);
  /*TO_F_STR1(c, res);!!not too sure what this does*/
}

          /* Mathematical Functions */

double FC_FUNC_(gsl_asinh, GSL_ASINH)
     (const double *x)
{
  return gsl_asinh(*x);
}

          /* Special Functions */

double FC_FUNC_(gsl_sf_bessel_knu_scaled, GSL_SF_BESSEL_KNU_SCALED)
     (double *nu, double *x)
{
  return gsl_sf_bessel_Knu_scaled(*nu,*x);
}

          /* Vectors and Matrices */

void FC_FUNC_(gsl_vector_alloc, GSL_VECTOR_ALLOC)
     (int *n, void **v)
{
  *v = (void*) gsl_vector_alloc (*n);
}

void FC_FUNC_(gsl_vector_free, GSL_VECTOR_FREE)
     (void **v)
{
  gsl_vector_free ((gsl_vector *)(*v));
}

double FC_FUNC_(gsl_vector_get, GSL_VECTOR_GET) (void **v, int *i)
{
  return gsl_vector_get ((gsl_vector *)(*v), *i);
}

void FC_FUNC_(gsl_vector_set, GSL_VECTOR_SET)
     (void **v, int *i, double *x)
{
  gsl_vector_set ((gsl_vector *)(*v), *i, *x);
}

void FC_FUNC_(gsl_matrix_alloc, GSL_MATRIX_ALLOC)
     (int *n1, int *n2, void **m)
{
  *m = (void*) gsl_matrix_alloc (*n1, *n2);
}

void FC_FUNC_(gsl_matrix_calloc, GSL_MATRIX_CALLOC)
     (int *n1, int *n2, void **m)
{
  *m = (void*) gsl_matrix_calloc (*n1, *n2);
}

void FC_FUNC_(gsl_matrix_free, GSL_MATRIX_FREE)
     (void **m)
{
  gsl_matrix_free ((gsl_matrix *)(*m));
}

double FC_FUNC_(gsl_matrix_get, GSL_MATRIX_GET)
     (void **m, int *i, int *j)
{
  return gsl_matrix_get ((gsl_matrix *)(*m), *i, *j);
}

void FC_FUNC_(gsl_matrix_set, GSL_MATRIX_SET)
     (void **m, int *i, int *j, double *x)
{
  gsl_matrix_set ((gsl_matrix *)(*m), *i, *j, *x);
}

          /* Permutations */

void FC_FUNC_(gsl_permutation_alloc, GSL_PERMUTATION_ALLOC)
     (int *n, void **p)
{
  *p = (void*) gsl_permutation_alloc (*n);
}

void FC_FUNC_(gsl_permutation_free, GSL_PERMUTATION_FREE)
     (void **p)
{
  gsl_permutation_free ((gsl_permutation *)(*p));
}

          /* Linear Algebra */

int FC_FUNC_(gsl_linalg_lu_decomp, GSL_LINALG_LU_DECOMP)
     (void **m, void **p, int *signum)
{
  int s,ierr;
  ierr = gsl_linalg_LU_decomp ((gsl_matrix *)(*m) , (gsl_permutation *)(*p), &s);
  *signum = s;
  return (ierr);
}

int FC_FUNC_(gsl_linalg_lu_invert, GSL_LINALG_LU_INVERT)
     (void **LU, void **p, void **inverse)
{

  return gsl_linalg_LU_invert ((gsl_matrix *)(*LU),(gsl_permutation *)(*p), (gsl_matrix *)(*inverse));
}

int FC_FUNC_(gsl_linalg_lu_solve, GSL_LINALG_LU_SOLVE)
     (void **LU, void **p, void **b, void **x)
{
  return gsl_linalg_LU_solve ((gsl_matrix *)(*LU), (gsl_permutation *)(*p), (gsl_vector *)(*b), (gsl_vector *)(*x));
}

          /* Ordinary Differential Equations */

void FC_FUNC_(gsl_odeiv_step_alloc, GSL_ODEIV_STEP_ALLOC)
     (const int *stepping_func, const int *dim, void **stp)
{
  /* WARNING: This routine has a small modification: the user needs to
   pass an integer in order to choose the stepping function instead of
   a variable of type gsl_odeiv_step_type. */

  switch(*stepping_func) {
  case 1 : // Embedded 2nd order Runge-Kutta with 3rd order error estimate
    *stp = (void *)gsl_odeiv_step_alloc(gsl_odeiv_step_rk2, *dim);
    break;
  case 2 : // 4th order (classical) Runge-Kutta
    *stp = (void *)gsl_odeiv_step_alloc(gsl_odeiv_step_rk4, *dim);
    break;
  case 3 : // Embedded 4th order Runge-Kutta-Fehlberg method with 5th order error estimate. This method is a good general-purpose integrator
    *stp = (void *)gsl_odeiv_step_alloc(gsl_odeiv_step_rkf45, *dim);
    break;
  case 4 : // Embedded 4th order Runge-Kutta Cash-Karp method with 5th order error estimate
    *stp = (void *)gsl_odeiv_step_alloc(gsl_odeiv_step_rkck, *dim);
    break;
  case 5 : // Embedded 8th order Runge-Kutta Prince-Dormand method with 9th order error estimate
    *stp = (void *)gsl_odeiv_step_alloc(gsl_odeiv_step_rk8pd, *dim);
    break;
  case 6 : // Implicit 2nd order Runge-Kutta at Gaussian points
    *stp = (void *)gsl_odeiv_step_alloc(gsl_odeiv_step_rk2imp, *dim);
    break;
  case 7 : // Implicit 4th order Runge-Kutta at Gaussian points
    *stp = (void *)gsl_odeiv_step_alloc(gsl_odeiv_step_rk4imp, *dim);
    break;
  case 8 : // Implicit Bulirsch-Stoer method of Bader and Deuflhard. This algorithm requires the Jacobian
    *stp = (void *)gsl_odeiv_step_alloc(gsl_odeiv_step_bsimp, *dim);
    break;
  case 9 : // M=1 implicit Gear method
    *stp = (void *)gsl_odeiv_step_alloc(gsl_odeiv_step_gear1, *dim);
    break;
  case 10 : // M=2 implicit Gear method
    *stp = (void *)gsl_odeiv_step_alloc(gsl_odeiv_step_gear2, *dim);
    break;
  }
}

void FC_FUNC_(gsl_odeiv_step_free, GSL_ODEIV_STEP_FREE)
     (void **stp)
{
  gsl_odeiv_step_free((gsl_odeiv_step *)(*stp));
}

void FC_FUNC_(gsl_odeiv_evolve_alloc, GSL_ODEIV_EVOLVE_ALLOC)
     (const int *dim, void **evl)
{
  *evl = (void *)gsl_odeiv_evolve_alloc (*dim);
}

int FC_FUNC_(gsl_odeiv_evolve_reset, GSL_ODEIV_EVOLVE_RESET)
     (void **evl)
{
  return gsl_odeiv_evolve_reset ((gsl_odeiv_evolve *)(*evl));
}

void FC_FUNC_(gsl_odeiv_evolve_free, GSL_ODEIV_EVOLVE_FREE)
     (void **evl)
{
  gsl_odeiv_evolve_free((gsl_odeiv_evolve *)(*evl));
}

void FC_FUNC_(gsl_odeiv_control_standart_new, GSL_ODEIV_CONTROL_STANDART_NEW)
     (void **ctrl, double *epsabs, double *epsrel, double *ay, double *adydt)
{
  *ctrl = (void*) gsl_odeiv_control_standard_new (*epsabs, *epsrel, *ay, *adydt);
}

void FC_FUNC_(gsl_odeiv_control_free, GSL_ODEIV_CONTROL_FREE)
     (void **ctrl)
{
  gsl_odeiv_control_free((gsl_odeiv_control *)(*ctrl));
}

          /* Interpolation */

void FC_FUNC_(gsl_interp_accel_alloc, GSL_INTERP_ACCEL_ALLOC)
     (void **acc)
{
     *acc = (void *)gsl_interp_accel_alloc();
}

void FC_FUNC_(gsl_spline_alloc, GSL_SPLINE_ALLOC)
     (void **spl, int *n, short int *opt)
{
  /* WARNING: This routine has a small modification: the user needs to
     pass an integer in order to choose the interpolation type instead
     of a variable of type gsl_interp_type. */

  switch (*opt) {
  case 1 : // linear interpolation
    *spl = (void *)gsl_spline_alloc(gsl_interp_linear, *n);
    break;
  case 2 : // polynomial interpolation
    *spl = (void *)gsl_spline_alloc(gsl_interp_polynomial, *n);
    break;
  case 3 : // cubic spline with natural boundary conditions
    *spl = (void *)gsl_spline_alloc(gsl_interp_cspline, *n);
    break;
  case 4 : // cubic spline with periodic boundary conditions
    *spl = (void *)gsl_spline_alloc(gsl_interp_cspline_periodic, *n);
    break;
  case 5 : // Akima spline with natural boundary conditions
    *spl = (void *)gsl_spline_alloc(gsl_interp_akima, *n);
    break;
  case 6 : // Akima spline with periodic boundary conditions
    *spl = (void *)gsl_spline_alloc(gsl_interp_akima_periodic, *n);
    break;
  }
}

void FC_FUNC_(gsl_spline_init, GSL_SPLINE_INIT)
     (void **spl, int *n, double *x, double *f)
{
  gsl_spline_init((gsl_spline *)(*spl), x, f, *n);
}

double FC_FUNC_(gsl_spline_eval, GSL_SPLINE_EVAL)
     (double *x, void **spl, void **acc)
{
  return gsl_spline_eval((gsl_spline *)(*spl), *x, (gsl_interp_accel *)(*acc));
}

double FC_FUNC_(gsl_spline_eval_deriv, GSL_SPLINE_EVAL_DERIV)
     (double *x, void **spl, void **acc)
{
  return gsl_spline_eval_deriv((gsl_spline *)(*spl), *x, (gsl_interp_accel *)(*acc));
}

double FC_FUNC_(gsl_spline_eval_deriv2, GSL_SPLINE_EVAL_DERIV2)
     (double *x, void **spl, void **acc)
{
  return gsl_spline_eval_deriv2((gsl_spline *)(*spl), *x, (gsl_interp_accel *)(*acc));
}

void FC_FUNC_(gsl_spline_free, GSL_SPLINE_FREE)
     (void **spl)
{
  gsl_spline_free((gsl_spline *)(*spl));

}

void FC_FUNC_(gsl_interp_accel_free, GSL_INTERP_ACCEL_FREE)
     (void **acc)
{
  gsl_interp_accel_free((gsl_interp_accel *)(*acc));
}

double FC_FUNC_(gsl_spline_eval_integ, GSL_SPLINE_EVAL_INTEG)
     (double *a, double *b, void **spl, void **acc)
{
  return gsl_spline_eval_integ((gsl_spline *)(*spl), *a, *b, (gsl_interp_accel *)(*acc));
}

          /* Multidimensional Root-Finding */

 void FC_FUNC_(gsl_multiroot_fsolver_alloc, GSL_MULTIROOT_FSOLVER_ALLOC)
     (void **s, const int *solver_type, const int *n)
{
  switch(*solver_type) {
  case 1 : // hybrid algorithm with internal scaling.
    *s = (void *)gsl_multiroot_fsolver_alloc(gsl_multiroot_fsolver_hybrids, *n);
    break;
  case 2 : // hybrid algorithm without internal scaling.
    *s = (void *)gsl_multiroot_fsolver_alloc(gsl_multiroot_fsolver_hybrid, *n);
    break;
  case 3 : // discrete Newton algorithm
    *s = (void *)gsl_multiroot_fsolver_alloc(gsl_multiroot_fsolver_dnewton, *n);
    break;
  case 4 : // Broyden algorithm
    *s = (void *)gsl_multiroot_fsolver_alloc(gsl_multiroot_fsolver_broyden, *n);
    break;
  }
}

void FC_FUNC_(gsl_multiroot_fsolver_free, GSL_MULTIROOT_FSOLVER_FREE)
     (void **s)
{
  gsl_multiroot_fsolver_free((gsl_multiroot_fsolver *)(*s));
}


/*
** compute correlation matrix
**
** G.Lohmann, Feb 2011
*/

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

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_sort_vector.h>
#include <gsl/gsl_histogram.h>

#include "viaio/Vlib.h"
#include "viaio/VImage.h"
#include "viaio/mu.h"

#ifdef _OPENMP
#include <omp.h>
#endif /*_OPENMP*/

#define SQR(x) ((x) * (x))
#define ABS(x) ((x) > 0 ? (x) : -(x))

extern void VNormalize(float *data,int nt,VBoolean stddev);
extern long GetAddr(VImage mapimage,int bi,int ri,int ci,int m,int k,int l);

int VNumNeigbours(size_t id,VImage map,VImage mapimage,int adjdef)
{
  int nslices = VImageNBands(mapimage);
  int nrows = VImageNRows(mapimage);
  int ncols = VImageNColumns(mapimage);

  int bi = VPixel(map,0,0,id,VShort);
  int ri = VPixel(map,0,1,id,VShort);
  int ci = VPixel(map,0,2,id,VShort);
  if (bi < 1 || bi >= nslices-2) return 0;
  if (ri < 1 || ri >= nrows-2) return 0;
  if (ci < 1 || ci >= ncols-2) return 0;

  int wn=1,rad2=0;
  if (adjdef == 3) {
    wn = 2;
    rad2 = 2*2;
  }

  int n=0;
  int k,l,m;
  for (m=-wn; m<=wn; m++) {
    for (k=-wn; k<=wn; k++) {
      for (l=-wn; l<=wn; l++) {
      	long jj = GetAddr(mapimage,bi,ri,ci,m,k,l);
      	if (jj < 0) continue;  /* outside of brain mask */

      	if (adjdef == 0) {     /* 6 adjacency */
      	  if (ABS(m)+ABS(k)+ABS(l) > 1) continue;
      	}
      	if (adjdef == 1) {     /* 18-adjacency */
      	  if (ABS(m) > 0 && ABS(k) > 0 && ABS(l) > 0) continue;
      	}
	if (adjdef == 3) {     /* sphere */
	  if (m*m + k*k + l*l > rad2) continue;
	}
      	n++;
      }
    }
  }
  return n;
}

/* convert to ranks */
void GetRank(float *data,gsl_vector *v,gsl_permutation *perm,gsl_permutation *rank)
{
  size_t i;
  size_t n = v->size;
  for (i=0; i<n; i++) gsl_vector_set(v,i,(double)data[i]);
  gsl_sort_vector_index (perm, v);
  gsl_permutation_inverse (rank, perm);
  for (i=0; i<n; i++) data[i] = (float)rank->data[i];
}

double Spearman(const float *data1,const float *data2,int n)
{
  int i;
  double nx = (double)n;
  double kx = nx*(nx*nx-1.0);
  double sxy=0.0;
  for (i=0; i<n; i++) {
    const double u = (double)data1[i];
    const double v = (double)data2[i];
    const double d = (u-v);
    sxy += d*d;
  }
  double rho = 1.0 - 6.0*sxy/kx;
  return rho;
}

double Correlation(const float *data1,const float *data2,int n)
{
  int i;
  double corr=0;
  for (i=0; i<n; i++) {
    const double u = (double)data1[i];
    const double v = (double)data2[i];
    corr += u*v;
  }
  corr /= (double)n;
  return corr;
}

float EdgeCorr(const float *data1,const float *data2,int n,int metric)
{
  float corr=0.0;
  if (metric == 0) corr = Correlation(data1,data2,n);
  if (metric == 1) corr = Spearman(data1,data2,n);
  if (corr > 0) return corr;
  else return 0.0;
}

void GetSNR(gsl_matrix_float **X1,gsl_matrix_float **X2,int *table,int n,gsl_matrix_float *SNR,int metric)
{
  long j,nvox=X1[0]->size1;
  long k,s,nt=X1[0]->size2;

  if (n < 3) VError(" n: %d",n);
  double nx = (double)n;
  double *sum1 = (double *)VCalloc(nt,sizeof(double));
  double *sum2 = (double *)VCalloc(nt,sizeof(double));

  gsl_vector *vec = NULL;
  gsl_permutation *perm = NULL;
  gsl_permutation *rank = NULL;
  if (metric == 1) {  /* only needed for spearman correlation */
    vec = gsl_vector_calloc(nt);
    perm = gsl_permutation_alloc(nt);
    rank = gsl_permutation_alloc(nt);
  }

  double ave=0,var=0,snr=0;

  for (j=0; j<nvox; j++) {
    memset(sum1,0,nt*sizeof(double));
    memset(sum2,0,nt*sizeof(double));
    const float *pp=NULL;
    for (s=0; s<n; s++) {
      if (table[s] == 0)
	pp = gsl_matrix_float_const_ptr(X1[s],j,0);
      else
	pp = gsl_matrix_float_const_ptr(X2[s],j,0);
      for (k=0; k<nt; k++) {
	const double x = (double)(*pp++);
	sum1[k] += x;
	sum2[k] += x*x;
      }
    }
    for (k=0; k<nt; k++) {
      ave = (sum1[k]/nx);
      var = (sum2[k] - nx*ave*ave) / (nx - 1.0);
      snr = 0;
      if (var > 0) snr = ave / sqrt(var);
      gsl_matrix_float_set(SNR,j,k,snr);
    }

    if (metric == 0) {  /* pearson correlation */
      float *qq = gsl_matrix_float_ptr(SNR,j,0);
      VNormalize(qq,nt,TRUE);
    }
    if (metric == 1) {  /* spearman ranks      */
      float *qq = gsl_matrix_float_ptr(SNR,j,0);
      GetRank(qq,vec,perm,rank);
    }
  }
  VFree(sum1);
  VFree(sum2);
}

float ZMatrix(gsl_histogram *histogram,gsl_matrix_float *SNR1,gsl_matrix_float *SNR2,int n1,int n2,
	      VImage roi,VImage map,VImage mapimage,int adjdef,float elength,float quantile,int step,int metric)
{
  size_t i;
  size_t nvox=SNR1->size1;
  int nt=SNR1->size2;
  size_t progress=0;
  int rad2 = (int)(elength*elength);
  double tiny=1.0e-8;
  gsl_set_error_handler_off ();

  fprintf(stderr," Computing matrix...\n");

  gsl_histogram_reset(histogram);
  size_t nbins = gsl_histogram_bins (histogram);
  double hmax = gsl_histogram_max (histogram);
  double hmin = gsl_histogram_min (histogram);
  double zmin = 99999.0;
  double zmax = -99999.0;
  int minadj = 1;

#pragma omp parallel for shared(progress,histogram) schedule(dynamic) firstprivate(SNR1,SNR2)
  for (i=0; i<nvox; i+=step) {
    if (i%1000 == 0) fprintf(stderr," %ld000\r",(long)progress++);
    int bi = (int)VPixel(map,0,0,i,VShort);
    int ri = (int)VPixel(map,0,1,i,VShort);
    int ci = (int)VPixel(map,0,2,i,VShort);
    int nadjx = VNumNeigbours(i,map,mapimage,adjdef);
    if (nadjx < minadj) continue;

    int roiflagi = 0;
    if (roi != NULL) {
      if (VGetPixel(roi,bi,ri,ci) > 0.5) roiflagi = 1;
    }

    gsl_histogram *tmphist = gsl_histogram_alloc (nbins);
    gsl_histogram_set_ranges_uniform (tmphist,hmin,hmax);
    gsl_histogram_reset(tmphist);

    const float *datax1 = gsl_matrix_float_const_ptr(SNR1,i,0);
    const float *datax2 = gsl_matrix_float_const_ptr(SNR2,i,0);

    size_t j=0;
    for (j=0; j<i; j+=step) {
      int bj = (int)VPixel(map,0,0,j,VShort);
      int rj = (int)VPixel(map,0,1,j,VShort);
      int cj = (int)VPixel(map,0,2,j,VShort);
      int d = SQR(bi-bj) + SQR(ri-rj) + SQR(ci-cj);
      if (d < rad2) continue;
      int nadjy = VNumNeigbours(j,map,mapimage,adjdef);
      if (nadjy < minadj) continue;

      int roiflagj = 0;
      if (roi != NULL) {
	if (VGetPixel(roi,bj,rj,cj) > 0.5) roiflagj = 1;
	if (roiflagi + roiflagj != 1) continue;
      }
      const float *datay1 = gsl_matrix_float_const_ptr(SNR1,j,0);
      const float *datay2 = gsl_matrix_float_const_ptr(SNR2,j,0);

      /* edge z-value */
      double z1 = EdgeCorr(datax1,datay1,nt,metric);
      double z2 = EdgeCorr(datax2,datay2,nt,metric);
      double z = (z1-z2);
      if (z < zmin) zmin = z;
      if (z > zmax) zmax = z;

      if (z < hmin) z = hmin;
      if (z > hmax-tiny) z = hmax-tiny;
      gsl_histogram_increment (tmphist,z);
    }

#pragma omp critical
    {
      gsl_histogram_add (histogram,tmphist);
    }
    gsl_histogram_free (tmphist);
  }

  /* get quantile cutoff */
  gsl_histogram_pdf *pdf = gsl_histogram_pdf_alloc(nbins);
  gsl_histogram_pdf_init(pdf,histogram);
  double lower=0,upper=0;

  size_t i0=0;
  for (i=nbins-1; i>=0; i--) {
    gsl_histogram_get_range (histogram,i,&lower,&upper);
    if (pdf->sum[i] < quantile) {
      if (gsl_histogram_get_range (histogram,i,&lower,&upper) == GSL_EDOM) VError(" err hist");
      i0 = i;
      break;
    }
  }
  double sum=0;
  for (i=nbins-1; i>i0; i--) {
    sum += gsl_histogram_get(histogram,i);
  }
  return (float)upper;
}


/* randist/pascal.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 James Theiler, Brian Gough
 *
 * 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 3 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

/* The Pascal distribution is a negative binomial with valued integer n

   prob(k) =  (n - 1 + k)!/(n!(k - 1)!) *  p^n (1-p)^k for k = 0, 1, ..., n

   */

unsigned int
gsl_ran_pascal (const gsl_rng * r, double p, unsigned int n)
{
  /* This is a separate interface for the pascal distribution so that
     it can be optimized differently from the negative binomial in
     future.

     e.g. if n < 10 it might be faster to generate the Pascal
     distributions as the sum of geometric variates directly.  */

  unsigned int k = gsl_ran_negative_binomial (r, p, (double) n);
  return k;
}

double
gsl_ran_pascal_pdf (const unsigned int k, const double p, unsigned int n)
{
  double P = gsl_ran_negative_binomial_pdf (k, p, (double) n);
  return P;
}


#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <dc1394/dc1394.h>
#include <math.h>
#include <fitsio.h>
#include <gsl/gsl_statistics.h>
#include <xpa.h>
#include <time.h>
#include <sys/time.h>
#include <string.h>
#include <inttypes.h>

#define NXPA 10

typedef struct _Box {
    double  x;
    double  y;
    double  fwhm;
    double  cenx;
    double  ceny;
    double  counts;
    double  background;
    double  noise;
    double  sigmaxythresh;
    double  snr;
    double  sigmaxy;
    double  sigmafwhmthresh;
    double  sigmafwhm;
    double  strehl;
    double  max;
    int     r;
} Box;

typedef struct _Back {
    double x;
    double y;
    double background;
    double sigma;
    int r;
    int width;
} Back;

Box box[3];
Back back;
long nelements, naxes[2], fpixel;
int boxsize = 17;

/* MASSDIMM values */
/*  20101228:  determined DIMM pixel scale to be 1.78"/pixel using HR1866
    and HR8895 as astrometric double stars
*/
double pixel_scale = 1.78;
double d = 0.080;
double r = 0.170;

double lambda = 0.6e-6;

double stardist(int i, int j) {
    return( sqrt( (box[i].cenx-box[j].cenx)*(box[i].cenx-box[j].cenx) +
		  (box[i].ceny-box[j].ceny)*(box[i].ceny-box[j].ceny) ) );
}

/* this routine uses a. tokovinin's modified equation given in
   2002, PASP, 114, 1156
 */
double seeing(double var) {
    double b, K, seeing;

    b = r/d;

    /* pixel scale in "/pixel, convert to rad */
    var = var * pow(pixel_scale/206265.0, 2);

    K = 0.364 * (1.0 - 0.532 * pow(b, -1/3) - 0.024 * pow(b, -7/3));

    seeing = 206265.0 * 0.98 * pow(d/lambda, 0.2) * pow(var/K, 0.6);

    return seeing;
}

/* this routine uses the classic DIMM equation */
double old_seeing(double var) {
    double r0;

    /* pixel scale in "/pixel, convert to rad */
    var = var * pow(pixel_scale/206265.0, 2);

    r0 = pow(2.0 * (lambda * lambda) * (
		 ( 0.1790 * pow(d, -1.0/3.0) -
		   0.0968 * pow(r, -1.0/3.0) ) / var
		 ), 0.6);

    // return 206265.0*0.98*lambda/r0;
    return r0;
}

/* measure the background in an annulus around the spot pattern */
int background(unsigned char *image, int imwidth, int imheight) {
    int i, j, backpix;
    int low_y, up_y, low_x, up_x;
    double dist, sum, sumsq;

    backpix = 0;
    sum = 0.0;
    sumsq = 0.0;

    low_y = back.y - back.r - back.width;
    up_y  = back.y + back.r + back.width;
    low_x = back.x - back.r - back.width;
    up_x  = back.x + back.r + back.width;
    if (low_y < 0) {
	low_y = 0;
    }
    if (up_y >= imheight) {
	up_y = imheight;
    }
    if (low_x < 0) {
	low_x = 0;
    }
    if (up_x >= imwidth) {
	up_x = imwidth;
    }

    for (i=low_y; i<up_y; i++) {
	for (j=low_x; j<up_x; j++) {
	    dist = sqrt(pow(back.x-j, 2) + pow(back.y-i, 2));
	    if (dist >= back.r && dist <= back.r+back.width) {
		sum += image[i*naxes[0]+j];
		backpix++;
	    }
	}
    }

    back.background = sum/backpix;
    //back.background = 0.0;

    for (i=low_y; i<up_y; i++) {
	for (j=low_x; j<up_x; j++) {
	    dist = sqrt(pow(back.x-j, 2) + pow(back.y-i, 2));
	    if (dist >= back.r && dist <= back.r+back.width) {
		sumsq += (image[i*naxes[0]+j]-back.background)*
		    (image[i*naxes[0]+j]-back.background);
	    }
	}
    }

    back.sigma = sqrt(sumsq/backpix);
    //back.sigma = 1.0;

    return 1;
}

/* measure centroid using center-of-mass algorithm */
int centroid(unsigned char *image, int imwidth, int imheight, int num) {
    int i, j;
    double  max    = 0.0;
    double  sum    = 0.0;
    double  sumx   = 0.0;
    double  sumxx  = 0.0;
    double  sumy   = 0.0;
    double  sumyy  = 0.0;
    double  val = 0.0;
    double  gain = 0.9;
    double  rmom;
    double  dist, dx;
    double  nsigma = 3.0;
    int low_y, up_y, low_x, up_x;
    int sourcepix = 0;

    dx = pixel_scale/206265.0;

    low_y = box[num].y - box[num].r;
    up_y  = box[num].y + box[num].r;
    low_x = box[num].x - box[num].r;
    up_x  = box[num].x + box[num].r;
    if (low_y < 0) {
	low_y = 0;
    }
    if (up_y >= imheight) {
	up_y = imheight;
    }
    if (low_x < 0) {
	low_x = 0;
    }
    if (up_x >= imwidth) {
	up_x = imwidth;
    }

    for (i=low_y; i<up_y; i++) {
	for (j=low_x; j<up_x; j++) {
	    dist = sqrt(pow(box[num].x-j, 2) + pow(box[num].y-i, 2));
	    if (dist <= box[num].r) {
		val = image[i*naxes[0]+j] - back.background;
		if (val >= nsigma*back.sigma) {
		    if (val > max) {
			max = val;
		    }
		    sum   += val;
		    sumx  += val*j;
		    sumxx += val*j*j;
		    sumy  += val*i;
		    sumyy += val*i*i;
		    sourcepix++;
		}
	    }
	}
    }

    if ( sum <= 0.0 ) {
	box[num].sigmaxy = -1.0;
	box[num].sigmafwhm = -1.0;
	box[num].snr = 0.0;
	box[num].fwhm = -1.0;
	box[num].counts = 1.0;
	box[num].strehl = 0.0;
    } else {
	box[num].max = max;
	box[num].strehl = (max/sum)*(4.0/3.14159)*pow( lambda/(d*dx), 2);
	rmom = ( sumxx - sumx * sumx / sum + sumyy - sumy * sumy / sum ) / sum;

	if ( rmom <= 0 ) {
	    box[num].fwhm = -1.0;
    } else {
	    box[num].fwhm = sqrt(rmom)  * 2.354 / sqrt(2.0);
    }

    box[num].counts = sum;
    box[num].cenx   = sumx / sum;
    box[num].ceny   = sumy / sum;
    box[num].noise  = back.sigma*sourcepix;
    box[num].x += gain*(box[num].cenx - box[num].x);
    box[num].y += gain*(box[num].ceny - box[num].y);
    box[num].snr = sum/sqrt(box[num].noise*box[num].noise + sum);
    box[num].sigmaxy = 1.0 / box[num].snr / sqrt(6.0);
    box[num].sigmafwhm = back.sigma * pow(sourcepix,1.5) / 10.
	/ box[num].fwhm / box[num].counts
	* 2.354 * 2.354 / 2.0;
  }

  return 1;

}

int grab_frame(dc1394camera_t *c, unsigned char *buf, int nbytes) {
    dc1394video_frame_t *frame=NULL;
    dc1394error_t err;

    err = dc1394_capture_dequeue(c, DC1394_CAPTURE_POLICY_WAIT, &frame);
    if (err != DC1394_SUCCESS) {
	dc1394_log_error("Unable to capture.");
	dc1394_capture_stop(c);
	dc1394_camera_free(c);
	exit(1);
    }
    memcpy(buf, frame->image, nbytes);
    dc1394_capture_enqueue(c, frame);
    return 1;
}

int add_gaussian(unsigned char *buffer, float cenx, float ceny, float a, float sigma) {
    float gauss, rsq;
    int i, j, low_x, up_x, low_y, up_y, size;
    double xoff, yoff;

    xoff = 2.0*drand48() - 1.0;
    yoff = 2.0*drand48() - 1.0;

    cenx += 0.5*xoff;
    ceny += 0.5*yoff;

    size = 30;
    low_x = (int)(cenx-size);
    up_x = (int)(cenx+size);
    low_y = (int)(ceny-size);
    up_y = (int)(ceny+size);

    for (i=low_y; i<up_y; i++) {
	for (j=low_x; j<up_x; j++) {
	    rsq = (cenx - j)*(cenx - j) + (ceny - i)*(ceny - i);
	    gauss = a*expf(-1.0*rsq/(sigma*sigma));
	    if (gauss > 255)
		gauss = 255;
	    buffer[i*naxes[0]+j] += (unsigned char)gauss;
	}
    }

    return 1;
}

int main(int argc, char *argv[]) {
    dc1394_t * dc;
    dc1394camera_t * camera;
    dc1394camera_list_t * list;
    dc1394error_t err;
    dc1394video_mode_t mode;
    unsigned int max_height, max_width, winleft, wintop;
    uint64_t total_bytes = 0;
    unsigned char *buffer, *buffer2, *average;
    char *names[NXPA];
    char *messages[NXPA];
    char filename[256], xpastr[256];
    char *froot;
    fitsfile *fptr;
    int i, j, f, fstatus, status, nimages, nboxes, test, xsize, ysize;
    int nbad = 0, nbad_l = 0;
    FILE *init, *out, *cenfile;
    float xx = 0.0, yy = 0.0, xsum = 0.0, ysum = 0.0;
    float elapsed_time, fps, avemax;
    double *dist, *sig, *dist_l, *sig_l, *weight, *weight_l;
    double mean, var, var_l, avesig, airmass, exptime;
    double r0, seeing_short, seeing_long, seeing_ave;
    struct timeval start_time, end_time;
    struct tm ut;
    time_t start_sec, end_sec;
    suseconds_t start_usec, end_usec;

    srand48((unsigned)time(NULL));

    XPA xpa;
    xpa = XPAOpen(NULL);

    stderr = freopen("measure_seeing.log", "w", stderr);

    if (argc <= 3) {
	printf("Usage: measure_seeing <n> <airmass> <exptime>\n");
	exit(-1);
    }

    nimages = atoi(argv[1]);
    airmass = atof(argv[2]);
    exptime = atof(argv[3]);
    fstatus = 0;
    status = 0;
    xsize = 320;
    ysize = 240;
    naxes[0] = xsize;
    naxes[1] = ysize;
    fpixel = 1;

    fps = 1.0/exptime;
    if (fps > 330.0) {
	fps = 330.0;
    }

    nelements = naxes[0]*naxes[1];

    mode = DC1394_VIDEO_MODE_FORMAT7_1;
    dc = dc1394_new();
    if (!dc)
	return -1;
    err = dc1394_camera_enumerate(dc, &list);
    DC1394_ERR_RTN(err, "Failed to enumerate cameras.");

    if (list->num == 0) {
	dc1394_log_error("No cameras found.");
	return -1;
    }

    camera = dc1394_camera_new(dc, list->ids[0].guid);
    if (!camera) {
	dc1394_log_error("Failed to initialize camera with guid %"PRIx64".",
			 list->ids[0].guid);
	return -1;
    }
    dc1394_camera_free_list(list);

    printf("Using camera with GUID %"PRIx64"\n", camera->guid);

    // need to use legacy firewire400 mode for now.  800 not quite reliable.
    dc1394_video_set_iso_speed(camera, DC1394_ISO_SPEED_400);

    // configure camera for format7
    err = dc1394_video_set_mode(camera, mode);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "Can't choose video mode.");
    err = dc1394_format7_get_max_image_size(camera, mode, &max_width, &max_height);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "cannot get max image size.");

    winleft = 0;
    wintop = 0;

    err = dc1394_format7_set_roi(camera,
				 mode,
				 DC1394_COLOR_CODING_MONO8,
				 DC1394_USE_MAX_AVAIL,
				 winleft, wintop, // left, top
				 xsize, ysize);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "Can't set ROI.");

    // set the frame rate to absolute value in frames/sec
    err = dc1394_feature_set_mode(camera, DC1394_FEATURE_FRAME_RATE,
				  DC1394_FEATURE_MODE_MANUAL);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free (camera), "cannot set framerate to manual");
    err = dc1394_feature_set_absolute_control(camera,
					      DC1394_FEATURE_FRAME_RATE, DC1394_TRUE);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free (camera),
		       "cannot set framerate to absolute mode");
    err = dc1394_feature_set_absolute_value(camera, DC1394_FEATURE_FRAME_RATE, fps);
    DC1394_ERR_CLN_RTN(err,dc1394_camera_free (camera),"cannot set framerate");
    printf("I: framerate is %f fps\n", fps);

    // set the shutter speed to absolute value in seconds
    err = dc1394_feature_set_mode(camera, DC1394_FEATURE_SHUTTER, DC1394_FEATURE_MODE_MANUAL);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "cannot set shutter to manual");
    err = dc1394_feature_set_absolute_control(camera, DC1394_FEATURE_SHUTTER, DC1394_TRUE);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "cannot set shutter to absolute mode");
    err = dc1394_feature_set_absolute_value(camera, DC1394_FEATURE_SHUTTER, exptime);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "cannot set shutter");
    printf("I: exptime is %f s\n", exptime);

    // set gain manually.  use relative value here in range 48 to 730.
    err = dc1394_feature_set_mode(camera, DC1394_FEATURE_GAIN, DC1394_FEATURE_MODE_MANUAL);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "cannot set gain to manual");
    err = dc1394_feature_set_value(camera, DC1394_FEATURE_GAIN, 400);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "cannot set gain");

    // set brightness manually.  use relative value in range 0 to 1023.
    err = dc1394_feature_set_mode(camera, DC1394_FEATURE_BRIGHTNESS, DC1394_FEATURE_MODE_MANUAL);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "cannot set brightness to manual");
    err = dc1394_feature_set_value(camera, DC1394_FEATURE_BRIGHTNESS, 100);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "cannot set brightness");

    err = dc1394_format7_get_total_bytes(camera, DC1394_VIDEO_MODE_FORMAT7_1, &total_bytes);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "Can't get total bytes.");

    err = dc1394_capture_setup(camera, 16, DC1394_CAPTURE_FLAGS_DEFAULT);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "Error capturing.");

    // start the camera up
    err = dc1394_video_set_transmission(camera, DC1394_ON);
    if (err != DC1394_SUCCESS) {
	dc1394_log_error("Unable to start camera iso transmission.");
	dc1394_capture_stop(camera);
	dc1394_camera_free(camera);
	return -1;
    }

    printf("Camera successfully initialized.\n");

    out = fopen("seeing.dat", "a");
    cenfile = fopen("centroids.dat", "w");
    init = fopen("init_cen_all", "r");

    i = 0;
    while (fscanf(init, "%f %f\n", &xx, &yy) != EOF) {
	box[i].x = xx;
	box[i].cenx = xx;
	box[i].y = yy;
	box[i].ceny = yy;
	box[i].r = boxsize/2.0;
	i++;
    }
    nboxes = i;
    fclose(init);

    back.r = 60;
    back.width = 10;

    /* allocate the buffers */
    if (!(dist = calloc(nimages, sizeof(double)))) {
	printf("Couldn't allocate dist array.\n");
	exit(-1);
    }
    if (!(sig = calloc(nimages, sizeof(double)))) {
	printf("Couldn't allocate sig array.\n");
	exit(-1);
    }
    if (!(weight = calloc(nimages, sizeof(double)))) {
	printf("Couldn't allocate sig array.\n");
	exit(-1);
    }
    if (!(weight_l = calloc(nimages, sizeof(double)))) {
	printf("Couldn't allocate sig array.\n");
	exit(-1);
    }
    if (!(dist_l = calloc(nimages, sizeof(double)))) {
	printf("Couldn't allocate dist_l array.\n");
	exit(-1);
    }
    if (!(sig_l = calloc(nimages, sizeof(double)))) {
	printf("Couldn't allocate sig_l array.\n");
	exit(-1);
    }
    if (!(buffer = malloc(nelements*sizeof(char)))) {
	printf("Couldn't Allocate Image Buffer\n");
	exit(-1);
    }
    if (!(buffer2 = malloc(nelements*sizeof(char)))) {
	printf("Couldn't Allocate 2nd Image Buffer\n");
	exit(-1);
    }
    if (!(average = malloc(nelements*sizeof(char)))) {
	printf("Couldn't Allocate Average Image Buffer\n");
	exit(-1);
    }

    froot = "seeing.fits";
    avemax = 0.0;

    /* get initial frame */
    grab_frame(camera, buffer2, nelements*sizeof(char));
    grab_frame(camera, buffer2, nelements*sizeof(char));
    grab_frame(camera, buffer2, nelements*sizeof(char));
    grab_frame(camera, buffer2, nelements*sizeof(char));
    // add_gaussian(buffer2, 195.0, 130.0, 140.0, 1.5);
    // add_gaussian(buffer2, 140.0, 115.0, 140.0, 1.5);

    gettimeofday(&start_time, NULL);

    for (f=0; f<nimages; f++) {
	grab_frame(camera, buffer, nelements*sizeof(char));
	// add_gaussian(buffer, 195.0, 130.0, 140.0, 1.5);
	// add_gaussian(buffer, 140.0, 115.0, 140.0, 1.5);

	// find center of star images and calculate background
	xsum = 0.0;
	ysum = 0.0;
	for (i=0; i<nboxes; i++) {
	    xsum += box[i].cenx;
	    ysum += box[i].ceny;
	}
	back.x = xsum/nboxes;
	back.y = ysum/nboxes;
	background(buffer, naxes[0], naxes[1]);

	for (i=0; i<nboxes; i++) {
	    box[i].r = boxsize/2.0;
	    centroid(buffer, naxes[0], naxes[1], i);
	}

	/* fix both boxes to the same Y */
	ysum = (box[0].y + box[1].y)/2.0;
	box[0].y = ysum;
	box[1].y = ysum;

	if (box[0].snr < box[1].snr) {
	    weight[f] = (box[0].snr/box[0].counts)*(box[0].snr/box[0].counts);
	} else {
	    weight[f] = (box[1].snr/box[1].counts)*(box[1].snr/box[1].counts);
	}

	if (weight[f] == 0.0) {
	    nbad++;
	}

	dist[f] = stardist(0, 1);
	if (dist[f] > 55.0 || dist[f] < 15.0) {
	    weight[f] = 0.0;
	}

	if (box[0].fwhm > 0.0 && box[1].fwhm > 0.0) {
	    for (i=0; i<nboxes; i++) {
		fprintf(cenfile,
			"%6.2f %6.2f %5.2f %9.4f %8.4f %7.4f %7.4f %8.4f %5.3f\t ",
			box[i].cenx,
			box[i].ceny,
			box[i].fwhm,
			box[i].counts,
			back.background,
			box[i].noise,
			box[i].sigmaxy,
			box[i].snr,
			box[i].strehl
		    );
	    }
	    sig[f] = sqrt(box[0].sigmaxy*box[0].sigmaxy + box[1].sigmaxy*box[1].sigmaxy);
	    fprintf(cenfile, "%6.2f %6.2f %.1e\n", dist[f], sig[f], exptime);
	    if (box[0].max > box[1].max) {
		avemax += box[0].max;
	    } else {
		avemax += box[1].max;
	    }
	}

	/* now average two exposures */
	for (j=0; j<nelements; j++) {
	    test = (int)buffer[j] + (int)buffer2[j];
	    if (test <= 254) {
		average[j] = buffer[j]+buffer2[j];
	    } else {
		average[j] = 254;
	    }
	}
	memcpy(buffer2, buffer, nelements*sizeof(char));

	xsum = 0.0;
	ysum = 0.0;
	for (i=0; i<nboxes; i++) {
	    xsum += box[i].cenx;
	    ysum += box[i].ceny;
	}
	back.x = xsum/nboxes;
	back.y = ysum/nboxes;
	background(average, naxes[0], naxes[1]);

	for (i=0; i<nboxes; i++) {
	    box[i].r = boxsize/2.0;
	    //centroid(average, i);
	    //box[i].r = boxsize/3.0;
	    //centroid(average, i);
	    //box[i].r = boxsize/4.0;
	    centroid(average, naxes[0], naxes[1], i);
	}

	if (box[0].snr < box[1].snr) {
	    weight_l[f] = (box[0].snr/box[0].counts)*(box[0].snr/box[0].counts);
	} else {
	    weight_l[f] = (box[1].snr/box[1].counts)*(box[1].snr/box[1].counts);
	}

	if (weight_l[f] == 0.0) {
	    nbad_l++;
	}

	dist_l[f] = stardist(0, 1);
	sig_l[f] = sqrt(box[0].sigmaxy*box[0].sigmaxy + box[1].sigmaxy*box[1].sigmaxy);

	if (f % 160 == 0) {
	    sprintf(filename, "!%s", froot);
	    fits_create_file(&fptr, "!seeing.fits", &fstatus);
	    fits_create_img(fptr, BYTE_IMG, 2, naxes, &fstatus);
	    fits_write_img(fptr, TBYTE, fpixel, nelements, buffer, &fstatus);
	    fits_close_file(fptr, &fstatus);
	    fits_report_error(stdout, fstatus);

	    status = XPASet(xpa, "timDIMM", "array [xdim=320,ydim=240,bitpix=8]", "ack=false",
			    (char *)buffer, nelements, names, messages, NXPA);
	    sprintf(xpastr, "image; box %f %f %d %d 0.0",
		    box[0].x, box[0].y, boxsize, boxsize);
	    status = XPASet(xpa, "timDIMM", "regions", "ack=false",
			    xpastr, strlen(xpastr), names, messages, NXPA);
	    sprintf(xpastr, "image; box %f %f %d %d 0.0",
		    box[1].x, box[1].y, boxsize, boxsize);
	    status = XPASet(xpa, "timDIMM", "regions", "ack=false",
			    xpastr, strlen(xpastr), names, messages, NXPA);
	}

    }

    gettimeofday(&end_time, NULL);
    printf("End capture.\n");

    /*-----------------------------------------------------------------------
     *  stop data transmission
     *-----------------------------------------------------------------------*/
    err = dc1394_video_set_transmission(camera, DC1394_OFF);
    DC1394_ERR_RTN(err,"couldn't stop the camera?");

    start_sec = start_time.tv_sec;
    start_usec = start_time.tv_usec;
    end_sec = end_time.tv_sec;
    end_usec = end_time.tv_usec;

    elapsed_time = (float)((end_sec + 1.0e-6*end_usec) - (start_sec + 1.0e-6*start_usec));
    fps = nimages/elapsed_time;
    printf("Elapsed time = %g seconds.\n", elapsed_time);
    printf("Framerate = %g fps.\n", fps);

    avemax /= nimages;
    printf("Avemax is %.3f\n", avemax);

    /* analyze short exposure */
    printf("\t SHORT EXPOSURE\n");
    mean = gsl_stats_wmean(weight, 1, dist, 1, nimages);
    avesig = gsl_stats_wmean(weight, 1, sig, 1, nimages);
    printf("mean = %f, avesig = %f\n", mean, avesig);

    printf("\n");

    var = gsl_stats_wvariance_m(weight, 1, dist, 1, nimages, mean);
    var = var - avesig*avesig;
    if (var < 0.0) {
	var = 0.0;
    }
    seeing_short = seeing(var)/pow(airmass,0.6);
    r0 = old_seeing(var);
    printf("sigma = %f, seeing = %f\n", sqrt(var), seeing_short);

    /* analyze long exposure */
    printf("\t LONG EXPOSURE\n");
    mean = gsl_stats_wmean(weight_l, 1, dist_l, 1, nimages);
    avesig = gsl_stats_wmean(weight_l, 1, sig_l, 1, nimages);
    printf("mean_l = %f, avesig_l = %f\n", mean, avesig);

    printf("\n");

    var_l = gsl_stats_wvariance_m(weight_l, 1, dist_l, 1, nimages, mean);
    var_l = var_l - avesig*avesig;
    if (var_l < 0.0) {
	var_l = 0.0;
    }
    seeing_long = seeing(var_l)/pow(airmass,0.6);
    printf("sigma_l = %f, seeing_l = %f\n", sqrt(var_l), seeing_long);

    printf("Bad samples:  %d for short, %d for long.\n", nbad, nbad_l);

    /* reduce exposure when too bright */
    if (avemax > 100.0) {
      if (exptime > 1.0e-3) {
	exptime -= 1.0e-3;
      } else {
	exptime /= 2.0;
      }
      printf("\033[0;33mStar too bright, reducing exposure time to %.1e seconds.\033[0;39m\n", exptime);
    }

    /* increase if too faint, but max out at 8 ms */
    if (avemax < 25.0) {
      if (exptime >= 1.0e-3) {
	exptime += 1.0e-3;
      } else {
	exptime *= 2.0;
      }
      if (exptime > 8.0e-3) {
	exptime = 8.0e-3;
	printf("\033[0;33mStar too faint, but exposure time max'ed out at %.1e seconds.\033[0;39m\n",
	       exptime);
      } else {
	printf("\033[0;33mStar too faint, exposure time increased to %.1e seconds.\033[0;39m\n",
	       exptime);
      }
    }

    init = fopen("exptime", "w");
    fprintf(init, "%.2e\n", exptime);
    fclose(init);

    if (nbad < 30) {
	seeing_ave = pow(seeing_short, 1.75)*pow(seeing_long,-0.75);
	printf("\033[0;32mAirmass corrected seeing = %4.2f\"\033[0;39m\n\n", seeing_short);
	printf("\033[0;32mFried Parameter, R0 = %.2f cm\033[0;39m\n\n", 100*r0);

	gmtime_r(&end_sec, &ut);
	fprintf(out, "%d-%02d-%02d %02d:%02d:%02d %f %f %f %f %f %f\n",
		ut.tm_year+1900,
		ut.tm_mon+1,
		ut.tm_mday,
		ut.tm_hour,
		ut.tm_min,
		ut.tm_sec,
		var,
		var_l,
		seeing_short,
		seeing_long,
		seeing_ave,
		airmass);

	init = fopen("init_cen_all", "w");
	for (i=0; i<nboxes; i++) {
	    fprintf(init, "%f %f\n", box[i].cenx, box[i].ceny);
	}
	fclose(init);
	init = fopen("seeing.out", "w");
	fprintf(init, "%.2f\n", seeing_short);
	fclose(init);
	init = fopen("r0.out", "w");
	fprintf(init, "%.1f\n", 100*r0);
	fclose(init);
    } else {
	printf("\n\n\033[0;31mABORTING measurement!  Lost at least one box.\033[0;39m\n\n");
    }

    fclose(cenfile);
    fclose(out);

    return (status);
}


#include "../include/paralleltt.h"

#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <stdio.h>
#include <cblas.h>
#include <lapacke.h>
#include <unistd.h>
#include <mpi.h>

#define NUM_THREAD 1
#define min(a,b) ((a)>(b)?(b):(a))
#define VERBOSE ((int) 1)

#ifndef HEAD
#define HEAD ((int) 0)
#endif

#define BUF ((int) 2)

tensor_train* TT_init(const int d, const int* restrict n)
{
    tensor_train* sim = (tensor_train*) malloc(sizeof(tensor_train));
    sim->d = d;
    sim->n = (int*) malloc(sizeof(int)*d);
    sim->r = (int*) malloc(sizeof(int)*(d+1));
    sim->trains = (double**) malloc(sizeof(double*)*d);

    sim->r[0] = 1;
    for (int j = 0; j < d; ++j) {
        sim->n[j] = n[j];

        if (j < d-1) {
            sim->r[j+1] = n[j];
        }
    }
    sim->r[d] = 1;

    for (int j = 0; j < d; ++j) {
        sim->trains[j] = (double*) malloc(sizeof(double)*(sim->r[j])*(sim->n[j])*(sim->r[j+1]));
    }

    return sim;
}

tensor_train* TT_init_rank(const int d, const int* restrict n, const int* restrict r)
{
    tensor_train* sim = (tensor_train*) malloc(sizeof(tensor_train));
    sim->d = d;
    sim->n = (int*) malloc(sizeof(int)*d);
    sim->r = (int*) malloc(sizeof(int)*(d+1));
    sim->trains = (double**) malloc(sizeof(double*)*d);

    for (int j = 0; j < d; ++j) {
        sim->n[j] = n[j];
    }
    for (int j = 0; j < d+1; ++j) {
        sim->r[j] = r[j];
    }

    for (int j = 0; j < d; ++j) {
        sim->trains[j] = (double*) malloc(sizeof(double)*(sim->r[j])*(sim->n[j])*(sim->r[j+1]));
    }

    return sim;
}

tensor_train* TT_copy(tensor_train* X)
{
    int d = X->d;
    int* n = X->n;
    int* r = X->r;
    tensor_train* Y = TT_init_rank(d, n, r);
    for (int j = 0; j < X->d; ++j) {
        int N = r[j] * n[j] * r[j+1];
        memcpy(Y->trains[j], X->trains[j], N * sizeof(double));
    }

    return Y;
}

void TT_free(tensor_train* sim)
{
    for (int j = 0; j < sim->d; ++j) {
        free(sim->trains[j]); sim->trains[j] = NULL;
    }
    free(sim->trains); sim->trains = NULL;
    free(sim->r);      sim->r = NULL;
    free(sim->n);      sim->n = NULL;
    free(sim);
}

void TT_print(tensor_train* tt)
{

    int d = tt->d;
    int* n = tt->n;
    int* r = tt->r;
    double** trains = tt->trains;
    printf("Printing the tensor train with d = %d\n",d);

    for (int ii = 0; ii < d; ++ii){
        printf("\nTensor %d/d (r[%d] = %d, n[%d]=%d, r[%d]=%d):\n",ii,ii,r[ii],ii,n[ii],ii+1,r[ii+1]);
        matrix_tt* A = matrix_tt_wrap(r[ii], n[ii] * r[ii+1], trains[ii]);
        matrix_tt_print(A, 1);
        free(A); A = NULL;
    }
}

void tt_broadcast(MPI_Comm comm, tensor_train* tt){
    for (int ii = 0; ii < tt->d; ++ii){
        matrix_tt* A = matrix_tt_wrap((tt->r[ii]) * (tt->n[ii]) * (tt->r[ii+1]), 1, tt->trains[ii]);
        matrix_tt_broadcast(comm, A);
        free(A);
    }
}

double get_compression(const tensor_train* tt)
{
    long N = 1;
    for (int ii = 0; ii < tt->d; ++ii)
    {
        N = N * tt->n[ii];
    }

    long Ntt = 0;
    for (int ii = 0; ii < tt->d; ++ii)
    {
        Ntt = Ntt + ((tt->r[ii])*(tt->n[ii])*(tt->r[ii+1]));
    }

    double compression = (double) Ntt/N;
    return compression;
}