/* Copyright (c) 2011-2012, Jérémy Fix. All rights reserved. */

/* Redistribution and use in source and binary forms, with or without */
/* modification, are permitted provided that the following conditions are met: */

/* * Redistributions of source code must retain the above copyright notice, */
/* this list of conditions and the following disclaimer. */
/* * 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. */
/* * None of the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. */

/* THIS SOFTWARE IS PROVIDED BY THE AUTHOR 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. */

#ifndef UKF_NDIM_STATE_H
#define UKF_NDIM_STATE_H

#include <gsl/gsl_linalg.h> // For the Cholesky decomposition
#include <gsl/gsl_math.h>
#include <gsl/gsl_blas.h>

#include "ukf_types.h"

namespace ukf
{

  /**
   * @short UKF for state estimation, additive noise case
   * The notations follow "Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models",p108, PhD, van Der Merwe
   */
  namespace state
  {
    /**
     * @short Allocation of the vectors/matrices and initialization
     *
     */
    void ukf_init(ukf_param &p, ukf_state &s)
    {
      // Parameters for the sigma points of the process equation
      p.nbSamples = 2 * p.n + 1;
      p.lambda = p.alpha * p.alpha * (p.n + p.kpa) - p.n;
      p.gamma = sqrt(p.n + p.lambda);

      // Parameters for the sigma points of the observation equation
      p.nbSamplesMeasure = 4 * p.n + 1;
      p.lambda_aug = p.alpha * p.alpha * (2*p.n + p.kpa) - 2*p.n;
      p.gamma_aug = sqrt(2*p.n + p.lambda_aug);

      // Init the matrices used to iterate
      s.xi = gsl_vector_alloc(p.n);
      gsl_vector_set_zero(s.xi);

      s.xi_prediction = gsl_matrix_alloc(p.n, p.nbSamples);
      gsl_matrix_set_zero(s.xi_prediction);

      s.xi_mean = gsl_vector_alloc(p.n);
      gsl_vector_set_zero(s.xi_mean);

      s.Pxxi = gsl_matrix_alloc(p.n,p.n);
      gsl_matrix_set_identity(s.Pxxi);
      gsl_matrix_scale(s.Pxxi, p.prior_x);

      s.cholPxxi = gsl_matrix_alloc(p.n,p.n);
      gsl_matrix_set_zero(s.cholPxxi);

      s.Pvvi = gsl_matrix_alloc(p.n,p.n);
      s.cholPvvi = gsl_matrix_alloc(p.n,p.n);
      p.evolution_noise->init(p,s);

      s.yi_prediction = gsl_matrix_alloc(p.no, p.nbSamplesMeasure);
      gsl_matrix_set_zero(s.yi_prediction);

      s.yi_mean = gsl_vector_alloc(p.no);
      gsl_vector_set_zero(s.yi_mean);

      s.ino_yi = gsl_vector_alloc(p.no);
      gsl_vector_set_zero(s.ino_yi);

      s.Pyyi = gsl_matrix_alloc(p.no, p.no);
      gsl_matrix_set_zero(s.Pyyi);

      s.Pnni = gsl_matrix_alloc(p.no,p.no);
      gsl_matrix_set_identity(s.Pnni);
      gsl_matrix_scale(s.Pnni, p.measurement_noise);

      s.Pxyi = gsl_matrix_alloc(p.n, p.no);
      gsl_matrix_set_zero(s.Pxyi);

      s.sigmaPoint = gsl_vector_alloc(p.n);
      gsl_vector_set_zero(s.sigmaPoint);

      s.sigmaPoints = gsl_matrix_alloc(p.n, p.nbSamples);
      gsl_matrix_set_zero(s.sigmaPoints);

      s.sigmaPointMeasure = gsl_vector_alloc(p.n);
      gsl_vector_set_zero(s.sigmaPoint);

      s.sigmaPointsMeasure = gsl_matrix_alloc(p.n, p.nbSamplesMeasure);
      gsl_matrix_set_zero(s.sigmaPointsMeasure);

      // Weights used to update the statistics
      s.wm_j  = gsl_vector_alloc(p.nbSamples); // Weights used to compute the mean of the sigma points images
      s.wc_j = gsl_vector_alloc(p.nbSamples); // Weights used to update the covariance matrices

      // Set the weights
      gsl_vector_set(s.wm_j, 0, p.lambda / (p.n + p.lambda));
      gsl_vector_set(s.wc_j, 0, p.lambda / (p.n + p.lambda) + (1.0 - p.alpha*p.alpha + p.beta));
      for(int j = 1 ; j < p.nbSamples; j ++)
	{
	  gsl_vector_set(s.wm_j, j, 1.0 / (2.0 * (p.n + p.lambda)));
	  gsl_vector_set(s.wc_j, j, 1.0 / (2.0 * (p.n + p.lambda)));
	}

      // Set the weights
      s.wm_aug_j  = gsl_vector_alloc(p.nbSamplesMeasure); // Weights used to compute the mean of the sigma points images
      s.wc_aug_j = gsl_vector_alloc(p.nbSamplesMeasure); // Weights used to update the covariance matrices
      gsl_vector_set(s.wm_aug_j, 0, p.lambda_aug / (2*p.n + p.lambda_aug));
      gsl_vector_set(s.wc_aug_j, 0, p.lambda_aug / (2*p.n + p.lambda_aug) + (1.0 - p.alpha*p.alpha + p.beta));
      for(int j = 1 ; j < p.nbSamplesMeasure; j ++)
	{
	  gsl_vector_set(s.wm_aug_j, j, 1.0 / (2.0 * (2*p.n + p.lambda_aug)));
	  gsl_vector_set(s.wc_aug_j, j, 1.0 / (2.0 * (2*p.n + p.lambda_aug)));
	}

      s.Ki = gsl_matrix_alloc(p.n, p.no);
      s.Ki_T = gsl_matrix_alloc(p.no, p.n);

      // Allocate temporary matrices
      s.temp_n = gsl_vector_alloc(p.n);

      s.temp_n_1 = gsl_matrix_alloc(p.n,1);
      s.temp_1_n = gsl_matrix_alloc(1,p.n);
      s.temp_n_n = gsl_matrix_alloc(p.n, p.n);
      s.temp_n_no = gsl_matrix_alloc(p.n, p.no);
      s.temp_no_1 = gsl_matrix_alloc(p.no,1);
      s.temp_1_no = gsl_matrix_alloc(1,p.no);
      s.temp_no_no = gsl_matrix_alloc(p.no, p.no);
    }

    /**
     * @short Free of memory allocation
     *
     */
    void ukf_free(ukf_param &p, ukf_state &s)
    {
      gsl_vector_free(s.xi);
      gsl_matrix_free(s.xi_prediction);
      gsl_vector_free(s.xi_mean);
      gsl_matrix_free(s.Pxxi);
      gsl_matrix_free(s.cholPxxi);
      gsl_matrix_free(s.Pvvi);
      gsl_matrix_free(s.cholPvvi);

      gsl_matrix_free(s.yi_prediction);
      gsl_vector_free(s.yi_mean);
      gsl_vector_free(s.ino_yi);
      gsl_matrix_free(s.Pyyi);
      gsl_matrix_free(s.Pnni);

      gsl_matrix_free(s.Pxyi);

      gsl_vector_free(s.sigmaPoint);
      gsl_matrix_free(s.sigmaPoints);

      gsl_vector_free(s.sigmaPointMeasure);
      gsl_matrix_free(s.sigmaPointsMeasure);

      gsl_vector_free(s.wm_j);
      gsl_vector_free(s.wc_j);

      gsl_vector_free(s.wm_aug_j);
      gsl_vector_free(s.wc_aug_j);

      gsl_matrix_free(s.Ki);
      gsl_matrix_free(s.Ki_T);

      gsl_vector_free(s.temp_n);
      gsl_matrix_free(s.temp_n_1);
      gsl_matrix_free(s.temp_1_n);
      gsl_matrix_free(s.temp_n_n);

      gsl_matrix_free(s.temp_n_no);
      gsl_matrix_free(s.temp_no_1);
      gsl_matrix_free(s.temp_1_no);
      gsl_matrix_free(s.temp_no_no);
    }

    /**
     * @short UKF-additive (zero-mean) noise case, "Kalman Filtering and Neural Networks", p.233
     *
     */
    template<typename FFUNC,
      typename HFUNC>
    void ukf_iterate(ukf_param &p, ukf_state &s,
		     FFUNC f,
		     HFUNC h,
		     gsl_vector* yi)
    {
      int i,j,k;

      // ************************************************** //
      // ************ Compute the sigma points ************ //
      // ************************************************** //

      // 0 - Compute the Cholesky decomposition of s.Pxxi
      gsl_matrix_memcpy(s.cholPxxi, s.Pxxi);
      gsl_linalg_cholesky_decomp(s.cholPxxi);
      // Set all the elements of cholPvvi strictly above the diagonal to zero
      for(j = 0 ; j < p.n ; j++)
	for(k = j+1 ; k < p.n ; k++)
	  gsl_matrix_set(s.cholPxxi,j,k,0.0);

      // 1- Compute the sigma points,
      // Equation (3.170)
      // sigmapoint_j = x_(i-1)
      // sigmapoint_j = x_(i-1) + gamma * sqrt(P_i-1)_j for 1 <= j <= n
      // sigmapoint_j = x_(i-1) - gamma * sqrt(P_i-1)_(j-(n+1)) for n+1 <= j <= 2n
      gsl_matrix_set_col(s.sigmaPoints, 0, s.xi);
      for(j = 1 ; j < p.n+1 ; ++j)
	for(i = 0 ; i < p.n ; ++i)
	  {
	    gsl_matrix_set(s.sigmaPoints,i,j, s.xi->data[i] + p.gamma * gsl_matrix_get(s.cholPxxi, i, j-1));
	    gsl_matrix_set(s.sigmaPoints,i,j+p.n, s.xi->data[i] - p.gamma * gsl_matrix_get(s.cholPxxi, i, j-1));
	  }

      /**********************************/
      /***** Time update equations  *****/
      /**********************************/

      // Time update equations
      // 0 - Compute the image of the sigma points and the mean of these images
      gsl_vector_set_zero(s.xi_mean);
      gsl_vector_view vec_view;
      for(j = 0 ; j < p.nbSamples ; ++j)
	{
	  gsl_matrix_get_col(s.sigmaPoint, s.sigmaPoints, j);
	  vec_view = gsl_matrix_column(s.xi_prediction,j);
	  f(s.params, s.sigmaPoint, &vec_view.vector);

	  // Update the mean, Eq (3.172)
	  for(i = 0 ; i < p.n ; ++i)
	    s.xi_mean->data[i] += s.wm_j->data[j] * gsl_matrix_get(s.xi_prediction,i,j);
	}

      // 1 - Compute the covariance of the images and add the process noise,
      // Equation (3.173)
      // Warning, s.Pxxi will now hold P_xk^-
      gsl_matrix_set_zero(s.Pxxi);
      for(j = 0 ; j < p.nbSamples ; ++j)
	{
	  for(i = 0 ; i < p.n ; ++i)
	    s.temp_n_1->data[i] = gsl_matrix_get(s.xi_prediction,i,j) - s.xi_mean->data[i];

	  gsl_blas_dgemm(CblasNoTrans, CblasTrans, s.wc_j->data[j] , s.temp_n_1, s.temp_n_1, 0, s.temp_n_n);
	  gsl_matrix_add(s.Pxxi, s.temp_n_n);
	}
      // Add the covariance of the evolution noise
      gsl_matrix_add(s.Pxxi, s.Pvvi);

      // Augment sigma points
      // Equation 3.174
      // First put the images of the initial sigma points
      gsl_matrix_view mat_view;
      mat_view = gsl_matrix_submatrix(s.sigmaPointsMeasure, 0, 0, p.n, p.nbSamples);
      gsl_matrix_memcpy(&mat_view.matrix, s.xi_prediction);
      // And add the additional sigma points eq. (7.56)
      for(j = 0 ; j < p.n ; ++j)
	{
	  for(i = 0 ; i < p.n ; ++i)
	    {
	      gsl_matrix_set(s.sigmaPointsMeasure, i, j+p.nbSamples, gsl_matrix_get(s.xi_prediction,i,0)+p.gamma_aug*gsl_matrix_get(s.cholPvvi,i,j));
	      gsl_matrix_set(s.sigmaPointsMeasure, i, j+p.nbSamples+p.n, gsl_matrix_get(s.xi_prediction,i,0)-p.gamma_aug*gsl_matrix_get(s.cholPvvi,i,j));
	    }
	}

      // Compute the image of the sigma points through the observation equation
      // eq (3.175)
      gsl_vector_set_zero(s.yi_mean);
      for(j = 0 ; j < p.nbSamplesMeasure ; ++j)
	{
	  gsl_matrix_get_col(s.sigmaPointMeasure, s.sigmaPointsMeasure, j);
	  vec_view = gsl_matrix_column(s.yi_prediction,j);
	  h(s.sigmaPointMeasure, &vec_view.vector);

	  // Update the mean , eq (3.176)
	  for(i = 0 ; i < p.no ; ++i)
	    s.yi_mean->data[i] += s.wm_aug_j->data[j] * gsl_matrix_get(s.yi_prediction,i,j);
	}

      /*****************************************/
      /***** Measurement update equations  *****/
      /*****************************************/

      // Compute the covariance of the observations
      // Eq. (3.177)
      // Initialize with the observation noise covariance
      gsl_matrix_memcpy(s.Pyyi, s.Pnni);
      for(j = 0 ; j < p.nbSamplesMeasure ; ++j)
	{
	  for(i = 0 ; i < p.no ; ++i)
	    s.temp_no_1->data[i] = gsl_matrix_get(s.yi_prediction,i,j) - s.yi_mean->data[i];

	  gsl_blas_dgemm(CblasNoTrans, CblasTrans, s.wc_aug_j->data[j] , s.temp_no_1, s.temp_no_1, 0, s.temp_no_no);
	  gsl_matrix_add(s.Pyyi, s.temp_no_no);
	}

      // Compute the state/observation covariance
      // Eq (3.178)
      gsl_matrix_set_zero(s.Pxyi);
      for(j = 0 ; j < p.nbSamplesMeasure ; ++j)
	{
	  for(i = 0 ; i < p.n ; ++i)
	    s.temp_n_1->data[i] = gsl_matrix_get(s.sigmaPointsMeasure,i,j) - s.xi_mean->data[i];

	  for(i = 0 ; i < p.no ; ++i)
	    s.temp_1_no->data[i] = gsl_matrix_get(s.yi_prediction,i,j) - s.yi_mean->data[i];

	  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, s.wc_aug_j->data[j] , s.temp_n_1, s.temp_1_no, 0, s.temp_n_no);
	  gsl_matrix_add(s.Pxyi, s.temp_n_no);
	}

      // Compute the Kalman gain, eq (3.179)
      // 0- Compute the inverse of Pyyi
      gsl_matrix_memcpy(s.temp_no_no, s.Pyyi);
      gsl_linalg_cholesky_decomp(s.temp_no_no);
      gsl_linalg_cholesky_invert(s.temp_no_no);

      // 1- Compute the Kalman gain
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0 , s.Pxyi, s.temp_no_no, 0, s.Ki);

      // Correction : correct the estimation of the state
      // Eq. 3.180
      // Compute the innovations
      for(i = 0 ; i < p.no ; ++i)
	s.ino_yi->data[i] = gsl_vector_get(yi, i) - gsl_vector_get(s.yi_mean, i);
      gsl_vector_memcpy(s.xi, s.xi_mean);
      gsl_blas_dgemv(CblasNoTrans, 1.0 , s.Ki, s.ino_yi, 1.0, s.xi);

      // Correction : Update the covariance matrix Pk
      // Eq. 3.181
      gsl_matrix_transpose_memcpy(s.Ki_T, s.Ki);
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0 , s.Ki, s.Pyyi, 0, s.temp_n_no);
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0 , s.temp_n_no, s.Ki_T, 1.0, s.Pxxi);

      // Update of the process noise
      p.evolution_noise->updateEvolutionNoise(p,s);
      //            switch(p.process_noise_type)
      //            {
      //            case ukf::UKF_PROCESS_FIXED:
      //                //nothing to do
      //                break;
      //            case ukf::UKF_PROCESS_RLS:
      //                gsl_matrix_memcpy(s.Pvvi, s.Pxxi);
      //                gsl_matrix_scale(s.Pvvi, 1.0/p.process_noise-1.0);
      //                gsl_matrix_memcpy(s.cholPvvi, s.Pvvi);
      //                gsl_linalg_cholesky_decomp(s.cholPvvi);
      //                for(j = 0 ; j < p.n ; j++)
      //                    for(k = j+1 ; k < p.n ; k++)
      //                        gsl_matrix_set(s.cholPvvi,j,k,0.0);
      //                break;
      //            default:
      //                printf("Warning : Unrecognized process noise type\n");
      //            }

    }

    /**
     * @short Evaluation of the output from the sigma points
     *
     */
    void ukf_evaluate(ukf_param &p, ukf_state &s,
		      void (*f)(gsl_vector*, gsl_vector *, gsl_vector *),
		      void (*h)(gsl_vector*, gsl_vector *), gsl_vector* yi)
    {

      int i,j,k;
      gsl_matrix_view mat_view;
      gsl_vector_view vec_view;
      // ************************************************** //
      // ************ Compute the sigma points ************ //
      // ************************************************** //

      // 0 - Compute the Cholesky decomposition of s.Pxxi
      gsl_matrix_memcpy(s.cholPxxi, s.Pxxi);
      gsl_linalg_cholesky_decomp(s.cholPxxi);
      // Set all the elements of cholPvvi strictly above the diagonal to zero
      for(j = 0 ; j < p.n ; j++)
	for(k = j+1 ; k < p.n ; k++)
	  gsl_matrix_set(s.cholPxxi,j,k,0.0);

      // 1- Compute the sigma points,
      // Equation (3.170)
      // sigmapoint_j = x_(i-1)
      // sigmapoint_j = x_(i-1) + gamma * sqrt(P_i-1)_j for 1 <= j <= n
      // sigmapoint_j = x_(i-1) - gamma * sqrt(P_i-1)_(j-(n+1)) for n+1 <= j <= 2n
      gsl_matrix_set_col(s.sigmaPoints, 0, s.xi);
      for(j = 1 ; j < p.n+1 ; ++j)
	for(i = 0 ; i < p.n ; ++i)
	  {
	    gsl_matrix_set(s.sigmaPoints,i,j, s.xi->data[i] + p.gamma * gsl_matrix_get(s.cholPxxi, i, j-1));
	    gsl_matrix_set(s.sigmaPoints,i,j+p.n, s.xi->data[i] - p.gamma * gsl_matrix_get(s.cholPxxi, i, j-1));
	  }

      /**********************************/
      /***** Time update equations  *****/
      /**********************************/

      // Time update equations
      // 0 - Compute the image of the sigma points and the mean of these images
      gsl_vector_set_zero(s.xi_mean);
      for(j = 0 ; j < p.nbSamples ; ++j)
	{
	  gsl_matrix_get_col(s.sigmaPoint, s.sigmaPoints, j);
	  vec_view = gsl_matrix_column(s.xi_prediction,j);
	  f(s.params, s.sigmaPoint, &vec_view.vector);

	  // Update the mean, Eq (3.172)
	  for(i = 0 ; i < p.n ; ++i)
	    s.xi_mean->data[i] += s.wm_j->data[j] * gsl_matrix_get(s.xi_prediction,i,j);
	}

      // 1 - Compute the covariance of the images and add the process noise,
      // Equation (3.173)
      // Warning, s.Pxxi will now hold P_xk^-
      gsl_matrix_set_zero(s.Pxxi);
      for(j = 0 ; j < p.nbSamples ; ++j)
	{
	  for(i = 0 ; i < p.n ; ++i)
	    s.temp_n_1->data[i] = gsl_matrix_get(s.xi_prediction,i,j) - s.xi_mean->data[i];

	  gsl_blas_dgemm(CblasNoTrans, CblasTrans, s.wc_j->data[j] , s.temp_n_1, s.temp_n_1, 0, s.temp_n_n);
	  gsl_matrix_add(s.Pxxi, s.temp_n_n);
	}
      // Add the covariance of the evolution noise
      gsl_matrix_add(s.Pxxi, s.Pvvi);

      // Augment sigma points
      // Equation 3.174
      // First put the images of the initial sigma points
      mat_view = gsl_matrix_submatrix(s.sigmaPointsMeasure, 0, 0, p.n, p.nbSamples);
      gsl_matrix_memcpy(&mat_view.matrix, s.xi_prediction);
      // And add the additional sigma points eq. (7.56)
      for(j = 0 ; j < p.n ; ++j)
	{
	  for(i = 0 ; i < p.n ; ++i)
	    {
	      gsl_matrix_set(s.sigmaPointsMeasure, i, j+p.nbSamples, gsl_matrix_get(s.xi_prediction,i,0)+p.gamma_aug*gsl_matrix_get(s.cholPvvi,i,j));
	      gsl_matrix_set(s.sigmaPointsMeasure, i, j+p.nbSamples+p.n, gsl_matrix_get(s.xi_prediction,i,0)-p.gamma_aug*gsl_matrix_get(s.cholPvvi,i,j));
	    }
	}

      // Compute the image of the sigma points through the observation equation
      // eq (3.175)
      gsl_vector_set_zero(yi);
      for(j = 0 ; j < p.nbSamplesMeasure ; ++j)
	{
	  gsl_matrix_get_col(s.sigmaPointMeasure, s.sigmaPointsMeasure, j);
	  vec_view = gsl_matrix_column(s.yi_prediction,j);
	  h(s.sigmaPointMeasure, &vec_view.vector);

	  // Update the mean , eq (3.176)
	  for(i = 0 ; i < p.no ; ++i)
	    yi->data[i] += s.wm_aug_j->data[j] * gsl_matrix_get(s.yi_prediction,i,j);
	}
    }
  } // state
} // ukf

#endif // UKF_NDIM_STATE_H


/* fit/gsl_fit.h
 *
 * Copyright (C) 2000 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 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

#ifndef __GSL_FIT_H__
#define __GSL_FIT_H__

#include <stdlib.h>
#include <gsl/gsl_math.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

int gsl_fit_linear (const double * x, const size_t xstride,
                    const double * y, const size_t ystride,
                    const size_t n,
                    double * c0, double * c1,
                    double * cov00, double * cov01, double * cov11,
                    double * sumsq);

int gsl_fit_wlinear (const double * x, const size_t xstride,
                     const double * w, const size_t wstride,
                     const double * y, const size_t ystride,
                     const size_t n,
                     double * c0, double * c1,
                     double * cov00, double * cov01, double * cov11,
                     double * chisq);

int
gsl_fit_linear_est (const double x,
                    const double c0, const double c1,
                    const double c00, const double c01, const double c11,
                    double *y, double *y_err);

int gsl_fit_mul (const double * x, const size_t xstride,
                 const double * y, const size_t ystride,
                 const size_t n,
                 double * c1,
                 double * cov11,
                 double * sumsq);

int gsl_fit_wmul (const double * x, const size_t xstride,
                  const double * w, const size_t wstride,
                  const double * y, const size_t ystride,
                  const size_t n,
                  double * c1,
                  double * cov11,
                  double * sumsq);

int
gsl_fit_mul_est (const double x,
                 const double c1,
                 const double c11,
                 double *y, double *y_err);

/* choose better names!! */

int gsl_fit_poly (const double * x,
                  const double * w,
                  const double * y,
                  size_t n,
                  double * c, size_t m,
                  double * chisq);

int gsl_fit_fns (const double * A,
                 const double * w,
                 const double * y,
                 size_t n,
                 double * c, size_t m,
                 double * chisq);

int gsl_fit_linear_nd (double * m, double * y, double * w);

__END_DECLS

#endif /* __GSL_FIT_H__ */


/**
 * \author Sylvain Marsat, University of Maryland - NASA GSFC
 *
 * \brief C code for the implementation of the Fourier-domain overlaps, likelihoods.
 *
 */

#define _XOPEN_SOURCE 500

#ifdef __GNUC__
#define UNUSED __attribute__ ((unused))
#else
#define UNUSED
#endif

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <unistd.h>
#include <getopt.h>
#include <stdbool.h>
#include <string.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_complex.h>

#include "constants.h"
#include "struct.h"
#include "splinecoeffs.h"
#include "fresnel.h"
#include "likelihood.h"

#include "wip.h"

#include <time.h> /* for testing */

/* Number of points to be used in linear integration - hardcoded for now */
#define nbptsintdefault 32768 /* Default number of points to use for linear overlaps */

/********************************* Utilities ****************************************/

/* Function to evaluate a Noise function  */
void EvaluateNoise(
  gsl_vector* noisevalues,                         /* Output: vector of the noise values */
  gsl_vector* freq,                                /* Input: vector of frequencies on which to evaluate */
  ObjectFunction * Snoise,                  /* Noise function */
  double fLow,                                     /* Lower bound of the frequency window for the detector */
  double fHigh)                                    /* Upper bound of the frequency window for the detector */
{
  int nbpts = (int) freq->size;

  /* Checking the length */
  if( freq->size != noisevalues->size) {
    printf("Error: incompatible sizes in EvaluateNoise.\n");
    exit(1);
  }

  /* Checking the boundary frequencies */
  if( gsl_vector_get(freq, 0) - fLow < -1e-15 || gsl_vector_get(freq, nbpts-1) > fHigh + 1e-15 ) {
    printf("Error: incompatible frequency range in EvaluateNoise.\n");
    printf("freq[0]=%g vs fLow=%g, freq[max]=%g vs fHigh=%g\n",gsl_vector_get(freq, 0), fLow, gsl_vector_get(freq, nbpts-1), fHigh);
    printf(" %i, %i\n",gsl_vector_get(freq, 0) < fLow , gsl_vector_get(freq, nbpts-1) > fHigh);
    printf("%g\n",gsl_vector_get(freq, 0) - fLow);
    exit(1);
  }

  for(int i=0; i<nbpts; i++) {
    gsl_vector_set(noisevalues, i, ObjectFunctionCall(Snoise,gsl_vector_get(freq, i)));
  }
}

/* Function building a frequency vector with linear or logarithmic sampling */
void SetLinearFrequencies(
  gsl_vector* freqvector,    /* Output pointer to gsl_vector, already allocated */
  const double fmin,         /* Lower bound of the frequency interval */
  const double fmax,         /* Upper bound of the frequency interval */
  const int nbpts)           /* Number of points */
{
  /* Vector of frequencies with logarithmic spacing */
  double stepf = (fmax-fmin)/(nbpts-1.);
  for(int i=0; i<nbpts; i++) {
    gsl_vector_set(freqvector, i, fmin + i * stepf);
  }
}
void SetLogFrequencies(
  gsl_vector* freqvector,    /* Output pointer to gsl_vector, already allocated */
  const double fmin,         /* Lower bound of the frequency interval */
  const double fmax,         /* Upper bound of the frequency interval */
  const int nbpts)           /* Number of points */
{
  /* Vector of frequencies with logarithmic spacing */
  double lnratio = log(fmax/fmin);
  double lnfmin = log(fmin);
  for(int i=0; i<nbpts; i++) {
    gsl_vector_set(freqvector, i, exp(lnfmin + (double) i/(nbpts-1.) * lnratio));
  }
}

/* Function determining logarithmically spaced frequencies from a waveform given as a list of modes, a minimal frequency and a number of points */
void ListmodesSetFrequencies(
  struct tagListmodesCAmpPhaseFrequencySeries *list,     /* Waveform, list of modes in amplitude/phase form */
  double fLow,                                           /* Additional lower frequency limit */
  double fHigh,                                          /* Additional upper frequency limit */
  int nbpts,                                             /* Number of frequency samples */
  int tagsampling,                                       /* Tag for linear (0) or logarithmic (1) sampling */
  gsl_vector* freqvector)                                /* Output: vector of frequencies, already allocated with nbpts */
{
  /* Checking the length */
  if((int) freqvector->size != nbpts) {
    printf("Error: incompatible sizes in ListmodesSetFrequencies.\n");
    exit(1);
  }

  /* Determining the frequency interval - from the lowest frequency of the 22 mode to the highest frequency covered by at least one mode */
  double minf, maxf;
  ListmodesCAmpPhaseFrequencySeries* listelementmode22 = ListmodesCAmpPhaseFrequencySeries_GetMode(list, 2, 2);
  minf = gsl_vector_get(listelementmode22->freqseries->freq, 0);
  maxf = gsl_vector_get(list->freqseries->freq, (int) list->freqseries->freq->size - 1);
  ListmodesCAmpPhaseFrequencySeries* listelement = list;
  while(listelement) {
    maxf = fmax(maxf, gsl_vector_get(listelement->freqseries->freq, (int) listelement->freqseries->freq->size - 1));
    listelement = listelement->next;
  }

  /* Checking that the waveform covers the fLow */
  if(fLow < minf) printf("Warning: in ListmodesSetFrequencies, fLow not covered by the wave data.\n");

  /* Actual boundaries of the frequency vector - cuts what is below fLow and above fHigh */
  minf = fmax(minf, fLow);
  maxf = fmin(maxf, fHigh);

  /* Setting values of the vector of frequencies, linear or logarithmic sampling */
  if(tagsampling==0) SetLinearFrequencies(freqvector, minf, maxf, nbpts);
  else if(tagsampling==1) SetLogFrequencies(freqvector, minf, maxf, nbpts);
  else {
    printf("Error: incorrect tagsampling in ListmodesSetFrequencies.");
    exit(1);
  }
}

/* Function computing a simple trapeze integration for real data */
static double TrapezeIntegrate(const gsl_vector* x, const gsl_vector* y)
{
  if(x->size!=y->size) {
    printf("Error: trying to apply TrapezeIntegrate on vectors of different lengths.\n");
    exit(1);
  }
  int N = ((int) x->size) - 1;
  double result = 0.;
  for(int i=0; i<N; i++){
    result += (gsl_vector_get(x, i+1) - gsl_vector_get(x, i)) * (gsl_vector_get(y, i) + gsl_vector_get(y, i+1))/2.;
  }
  return result;
}

/***************************** Functions for overlaps using linear integration ******************************/

/* Function computing the overlap (h1|h2) between two given modes, for a given noise function - uses simple trapeze integration on logarithmically sampled frequencies  */
double FDSinglemodeLogLinearOverlap(
  struct tagCAmpPhaseFrequencySeries *freqseries1, /* First mode h1, in amplitude/phase form */
  struct tagCAmpPhaseFrequencySeries *freqseries2, /* Second mode h2, in amplitude/phase form */
  ObjectFunction * Snoise,                  /* Noise function */
  double fLow,                                     /* Lower bound of the frequency window for the detector */
  double fHigh)                                    /* Upper bound of the frequency window for the detector */
{
  double res;
  int size1 = (int) freqseries1->freq->size;
  int size2 = (int) freqseries2->freq->size;
  int nbpts = nbptsintdefault;

  /* Minimal, maximal frequencies */
  double fmin1 = gsl_vector_get(freqseries1->freq, 0);
  double fmin2 = gsl_vector_get(freqseries2->freq, 0);
  double fmax1 = gsl_vector_get(freqseries1->freq, size1 - 1);
  double fmax2 = gsl_vector_get(freqseries2->freq, size2 - 1);
  double fmin0 = fmax(fLow, fmax(fmin1, fmin2));
  double fmax0 = fmin(fHigh, fmin(fmax1, fmax2));

  /* Vector of frequencies with logarithmic spacing */
  gsl_vector* freqvector = gsl_vector_alloc(nbpts);
  SetLogFrequencies(freqvector, fmin0, fmax0, nbpts);

  /* Initializing the splines */
  /* Note: since this must also apply to mode contribution after processing, real and imaginary parts of the amplitude are present - but since they should differ for LLV detectors by a constant factor, they can be interpolated */
  gsl_interp_accel* accel_amp1real = gsl_interp_accel_alloc();
  gsl_interp_accel* accel_amp2real = gsl_interp_accel_alloc();
  gsl_interp_accel* accel_amp1imag = gsl_interp_accel_alloc();
  gsl_interp_accel* accel_amp2imag = gsl_interp_accel_alloc();
  gsl_interp_accel* accel_phase1 = gsl_interp_accel_alloc();
  gsl_interp_accel* accel_phase2 = gsl_interp_accel_alloc();
  gsl_spline* amp1real = gsl_spline_alloc(gsl_interp_cspline, size1);
  gsl_spline* amp2real = gsl_spline_alloc(gsl_interp_cspline, size2);
  gsl_spline* amp1imag = gsl_spline_alloc(gsl_interp_cspline, size1);
  gsl_spline* amp2imag = gsl_spline_alloc(gsl_interp_cspline, size2);
  gsl_spline* phase1 = gsl_spline_alloc(gsl_interp_cspline, size1);
  gsl_spline* phase2 = gsl_spline_alloc(gsl_interp_cspline, size2);
  gsl_vector* valuesvector = gsl_vector_alloc(nbpts);
  gsl_spline_init(amp1real, gsl_vector_const_ptr(freqseries1->freq,0), gsl_vector_const_ptr(freqseries1->amp_real,0), size1);
  gsl_spline_init(amp1imag, gsl_vector_const_ptr(freqseries1->freq,0), gsl_vector_const_ptr(freqseries1->amp_imag,0), size1);
  gsl_spline_init(amp2real, gsl_vector_const_ptr(freqseries2->freq,0), gsl_vector_const_ptr(freqseries2->amp_real,0), size2);
  gsl_spline_init(amp2imag, gsl_vector_const_ptr(freqseries2->freq,0), gsl_vector_const_ptr(freqseries2->amp_imag,0), size2);
  gsl_spline_init(phase1, gsl_vector_const_ptr(freqseries1->freq,0), gsl_vector_const_ptr(freqseries1->phase,0), size1);
  gsl_spline_init(phase2, gsl_vector_const_ptr(freqseries2->freq,0), gsl_vector_const_ptr(freqseries2->phase,0), size2);

  /* Main loop - vector of values to be evaluated */
  double f, phi1, phi2, Sn;
  double complex A1;
  double complex A2;
  double* freqvectordata = freqvector->data;
  double* valuesvectordata = valuesvector->data;
  int i=0;
  for(i=0; i<nbpts; i++){
    if(i==0) {f = fmax(freqvectordata[i], fmin0);}
    else if(i==nbpts-1) {f = fmin(freqvectordata[i], fmax0);}
    else {f = freqvectordata[i];}
    A1 = gsl_spline_eval(amp1real, f, accel_amp1real) + I*gsl_spline_eval(amp1imag, f, accel_amp1imag);
    A2 = gsl_spline_eval(amp2real, f, accel_amp2real) + I*gsl_spline_eval(amp2imag, f, accel_amp2imag);
    phi1 = gsl_spline_eval(phase1, f, accel_phase1);
    phi2 = gsl_spline_eval(phase2, f, accel_phase2);
    Sn = ObjectFunctionCall(Snoise,f);
    valuesvectordata[i] = 4.*creal(A1*conj(A2)*cexp(I*(phi1-phi2))/Sn);
  }

  /* Trapeze integration */
  res = TrapezeIntegrate(freqvector, valuesvector);

  /* Clean up */
  gsl_vector_free(freqvector);
  gsl_vector_free(valuesvector);
  gsl_interp_accel_free(accel_amp1real);
  gsl_interp_accel_free(accel_amp2real);
  gsl_interp_accel_free(accel_amp1imag);
  gsl_interp_accel_free(accel_amp2imag);
  gsl_interp_accel_free(accel_phase1);
  gsl_interp_accel_free(accel_phase2);
  gsl_spline_free(amp1real);
  gsl_spline_free(amp2real);
  gsl_spline_free(amp1imag);
  gsl_spline_free(amp2imag);
  gsl_spline_free(phase1);
  gsl_spline_free(phase2);

  return res;
}

/* Function computing the overlap (h1|h2) between two waveforms given as lists of mode contributions (factos sYlm already included), for a given noise function - uses simple trapeze integration on logarithmically sampled frequencies - generates the frequency series in Re/Im form by summing the mode contributions first, then computes the overlap */
double FDListmodesLogLinearOverlap(
  struct tagListmodesCAmpPhaseFrequencySeries *list1,    /* First waveform, list of modes in amplitude/phase form */
  struct tagListmodesCAmpPhaseFrequencySeries *list2,    /* Second waveform, list of modes in amplitude/phase form */
  ObjectFunction * Snoise,                        /* Noise function */
  double fLow,                                           /* Lower bound of the frequency window for the detector */
  double fHigh,                                          /* Upper bound of the frequency window for the detector */
  double fstartobs1,                                     /* Starting frequency for the 22 mode of wf 1 - as determined from a limited duration of the observation - set to 0 to ignore */
  double fstartobs2)                                     /* Starting frequency for the 22 mode of wf 2 - as determined from a limited duration of the observation - set to 0 to ignore */
{
  /* Number of points to use in the trapeze integration */
  int nbpts = nbptsintdefault;

  /* Determining the frequency interval - from the lowest frequency of the 22 mode to the highest frequency covered by at least one mode */
  double minf1, maxf1, minf2, maxf2, minf, maxf;
  ListmodesCAmpPhaseFrequencySeries* listelement1mode22 = ListmodesCAmpPhaseFrequencySeries_GetMode(list1, 2, 2);
  ListmodesCAmpPhaseFrequencySeries* listelement2mode22 = ListmodesCAmpPhaseFrequencySeries_GetMode(list2, 2, 2);
  minf1 = gsl_vector_get(listelement1mode22->freqseries->freq, 0);
  minf2 = gsl_vector_get(listelement2mode22->freqseries->freq, 0);
  maxf1 = ListmodesCAmpPhaseFrequencySeries_maxf(list1);
  maxf2 = ListmodesCAmpPhaseFrequencySeries_maxf(list2);
  /* Taking into account fLow, fHigh -- fstartobs, determined from 22 mode, is also used as a fLow */
  if(!(fHigh==0.)) {
    maxf1 = fmin(maxf1, fHigh);
    maxf2 = fmin(maxf2, fHigh);
  }
  if(!(fLow==0.)) {
    minf1 = fmax(minf1, fLow);
    minf2 = fmax(minf2, fLow);
  }
  if(!(fstartobs1==0.)) minf1 = fmax(minf1, fstartobs1);
  if(!(fstartobs2==0.)) minf1 = fmax(minf2, fstartobs2);
  /* Actual boundaries to be used in the overlap - intersection of freqs covered by wf 1 and 2 */
  minf = fmax(minf1, minf2);
  maxf = fmin(maxf1, maxf2);

  /* Vector of frequencies used for the overlap */
  gsl_vector* freqoverlap = gsl_vector_alloc(nbpts);
  SetLogFrequencies(freqoverlap, minf, maxf, nbpts);

  /* Evaluating each frequency series by interpolating and summing the mode contributions */
  ReImFrequencySeries* freqseries1 = NULL;
  ReImFrequencySeries_Init(&freqseries1, nbpts);
  ReImFrequencySeries_SumListmodesCAmpPhaseFrequencySeries(freqseries1, list1, freqoverlap, fLow, fHigh, fstartobs1);
  ReImFrequencySeries* freqseries2 = NULL;
  ReImFrequencySeries_Init(&freqseries2, nbpts);
  ReImFrequencySeries_SumListmodesCAmpPhaseFrequencySeries(freqseries2, list2, freqoverlap, fLow, fHigh, fstartobs2);

  /* Compute the integrand */
  gsl_vector* valuesoverlap = gsl_vector_alloc(nbpts);
  double* hreal1data = freqseries1->h_real->data;
  double* himag1data = freqseries1->h_imag->data;
  double* hreal2data = freqseries2->h_real->data;
  double* himag2data = freqseries2->h_imag->data;
  double* freqdata = freqoverlap->data;
  for(int i=0; i<nbpts; i++) {
    gsl_vector_set(valuesoverlap, i, 4.*creal( (hreal1data[i] + I*himag1data[i]) * (hreal2data[i] - I*himag2data[i]) / ObjectFunctionCall(Snoise,freqdata[i])));
  }

  /* Final trapeze integration */
  double overlap = TrapezeIntegrate(freqoverlap, valuesoverlap);

  /* Clean up */
  ReImFrequencySeries_Cleanup(freqseries1);
  ReImFrequencySeries_Cleanup(freqseries2);
  gsl_vector_free(freqoverlap);
  gsl_vector_free(valuesoverlap);

  return overlap;
}

/* Function computing the overlap (h1|h2) between h1 given in Re/Im form and h2 given as a list of mode contributions (factors sYlm already included), for a given vector of noise values - uses simple trapeze integration - generates the frequency series of h2 in Re/Im form by summing the mode contributions first, then computes the overlap */
double FDOverlapReImvsListmodesCAmpPhase(
  struct tagReImFrequencySeries *freqseries1,            /* First waveform, in Re/Im form */
  struct tagListmodesCAmpPhaseFrequencySeries *list2,    /* Second waveform, list of modes in amplitude/phase form */
  gsl_vector* noisevalues,                               /* Vector for the noise values on the freq of h1 */
  double fLow,                                           /* Minimal frequency - set to 0 to ignore */
  double fHigh,                                          /* Maximal frequency - set to 0 to ignore */
  double fstartobs2)                                     /* Starting frequency for the 22 mode of wf 2 - as determined from a limited duration of the observation - set to 0 to ignore */
{
  /* Check the lengths */
  if(freqseries1->freq->size != noisevalues->size) {
    printf("Error: inconsistent lengths in FDOverlapReImvsListmodesCAmpPhase.\n");
    exit(1);
  }

  /* Frequencies used for the overlap */
  int nbpts = (int) freqseries1->freq->size;
  gsl_vector* freqoverlap = freqseries1->freq;

  /* Evaluating frequency series 2 by interpolating and summing the mode contributions */
  ReImFrequencySeries* freqseries2 = NULL;
  ReImFrequencySeries_Init(&freqseries2, nbpts);
  ReImFrequencySeries_SumListmodesCAmpPhaseFrequencySeries(freqseries2, list2, freqoverlap, fLow, fHigh, fstartobs2);

  /* Compute the integrand */
  gsl_vector* valuesoverlap = gsl_vector_alloc(nbpts);
  double* hreal1data = freqseries1->h_real->data;
  double* himag1data = freqseries1->h_imag->data;
  double* hreal2data = freqseries2->h_real->data;
  double* himag2data = freqseries2->h_imag->data;
  double* noisedata = noisevalues->data;
  for(int i=0; i<nbpts; i++) {
    gsl_vector_set(valuesoverlap, i, 4.*creal( (hreal1data[i] + I*himag1data[i]) * (hreal2data[i] - I*himag2data[i]) / noisedata[i]));
  }

  /* Final trapeze integration */
  double overlap = TrapezeIntegrate(freqoverlap, valuesoverlap);

  /* Clean up */
  ReImFrequencySeries_Cleanup(freqseries2);
  gsl_vector_free(valuesoverlap);

  return overlap;
}

/* Function computing the overlap (h1|h2) between two waveforms given as Re/Im frequency series (common freq values assumed), for a given vector of noise values - uses simple trapeze integration */
double FDOverlapReImvsReIm(
  struct tagReImFrequencySeries *freqseries1,  /* First waveform, frequency series in Re/Im form */
  struct tagReImFrequencySeries *freqseries2,  /* Second waveform, frequency series in Re/Im form */
  gsl_vector* noisevalues)                     /* Vector for the noise values on common freq of the freqseries */
{
  /* Check the lengths */
  if(freqseries1->freq->size != noisevalues->size || freqseries2->freq->size != noisevalues->size) {
    printf("Error: inconsistent lengths in FDOverlapReImvsReIm.\n");
    exit(1);
  }

  /* Frequency vector - assuming they match beyond their mere lengths */
  gsl_vector* freqoverlap = freqseries1->freq;
  int nbpts = (int) freqoverlap->size;

  /* Compute the integrand */
  gsl_vector* valuesoverlap = gsl_vector_alloc((int) freqoverlap->size);
  double* hreal1data = freqseries1->h_real->data;
  double* himag1data = freqseries1->h_imag->data;
  double* hreal2data = freqseries2->h_real->data;
  double* himag2data = freqseries2->h_imag->data;
  double* noisedata = noisevalues->data;
  for(int i=0; i<nbpts; i++) {
    gsl_vector_set(valuesoverlap, i, 4.*creal( (hreal1data[i] + I*himag1data[i]) * (hreal2data[i] - I*himag2data[i]) / noisedata[i]));
  }

  /* Final trapeze integration */
  double overlap = TrapezeIntegrate(freqoverlap, valuesoverlap);

  /* Clean up */
  gsl_vector_free(valuesoverlap);

  return overlap;
}

/***************************** Functions for overlaps using amplitude/phase (wip) ******************************/

/* Function computing the overlap (h1|h2) between two given modes in amplitude/phase form, for a given noise function - uses the amplitude/phase representation (wip) */
double FDSinglemodeWIPOverlap(
  struct tagCAmpPhaseFrequencySeries *freqseries1, /* First mode h1, in amplitude/phase form */
  struct tagCAmpPhaseFrequencySeries *freqseries2, /* Second mode h2, in amplitude/phase form */
  ObjectFunction * Snoise,                  /* Noise function */
  double fLow,                                     /* Lower bound of the frequency window for the detector */
  double fHigh)                                    /* Upper bound of the frequency window for the detector */
{
  /* Should add some error checking */
  CAmpPhaseFrequencySeries* h1 = freqseries1;
  CAmpPhaseFrequencySeries* h2 = freqseries2;

  double *f1 = h1->freq->data;
  int n1 = h1->freq->size;
  double *h1Ar = h1->amp_real->data;
  double *h1Ai = h1->amp_imag->data;
  double *h1p = h1->phase->data;

  double *f2 = h2->freq->data;
  int n2 = h2->freq->size;
  double *h2Ar = h2->amp_real->data;
  double *h2Ai = h2->amp_imag->data;
  double *h2p = h2->phase->data;

  /* fLow or fHigh <= 0 means use intersection of signal domains  */
  /* NOTE: factor 4 was previously missing */
  double overlap = 4.*wip_phase(f1, n1, f2, n2, h1Ar, h1Ai, h1p, h2Ar, h2Ai, h2p, Snoise, 1.0, fLow, fHigh);
  return overlap;
}

/* Function computing the overlap (h1|h2) between two waveforms given as list of modes, for a given noise function - two additional parameters for the starting 22-mode frequencies (then properly scaled for the other modes) for a limited duration of the observations */
double FDListmodesWIPOverlap(
  struct tagListmodesCAmpPhaseFrequencySeries *listh1, /* First waveform, list of modes in amplitude/phase form */
  struct tagListmodesCAmpPhaseFrequencySeries *listh2, /* Second waveform, list of modes in amplitude/phase form */
  ObjectFunction * Snoise,                  /* Noise function */
  double fLow,                                         /* Lower bound of the frequency window for the detector */
  double fHigh,                                        /* Upper bound of the frequency window for the detector */
  double fstartobs1,                                   /* Starting frequency for the 22 mode of wf 1 - as determined from a limited duration of the observation - set to 0 to ignore */
  double fstartobs2)                                   /* Starting frequency for the 22 mode of wf 2 - as determined from a limited duration of the observation - set to 0 to ignore */
{
  double overlap = 0;

  /* Main loop over the modes - goes through all the modes present */
  ListmodesCAmpPhaseFrequencySeries* listelementh1 = listh1;
  while(listelementh1) {
    ListmodesCAmpPhaseFrequencySeries* listelementh2 = listh2;
    while(listelementh2) {
      /* Scaling fstartobs1/2 with the appropriate factor of m (for the 21 mode we use m=2) - setting fmin in the overlap accordingly */
      int mmax1 = max(2, listelementh1->m);
      int mmax2 = max(2, listelementh2->m);
      double fcutLow = fmax(fLow, fmax(((double) mmax1)/2. * fstartobs1, ((double) mmax2)/2. * fstartobs2));
      overlap += FDSinglemodeWIPOverlap(listelementh1->freqseries, listelementh2->freqseries, Snoise, fcutLow, fHigh);

      listelementh2 = listelementh2->next;
    }
    listelementh1 = listelementh1->next;
  }
  return overlap;
}

/************** Functions for overlap/likelihood allowing to switch between wip and loglinear integration *****************/

/* Wrapping of FDListmodesWIPOverlap or FDListmodesLogLinearOverlap according to tagint */
double FDListmodesOverlap(
  struct tagListmodesCAmpPhaseFrequencySeries *listh1, /* First mode h1, list of modes in amplitude/phase form */
  struct tagListmodesCAmpPhaseFrequencySeries *listh2, /* Second mode h2, list of modes in amplitude/phase form */
  ObjectFunction * Snoise,                  /* Noise function */
  double fLow,                                         /* Lower bound of the frequency window for the detector */
  double fHigh,                                        /* Upper bound of the frequency window for the detector */
  double fstartobs1,                                   /* Starting frequency for the 22 mode of wf 1 - as determined from a limited duration of the observation - set to 0 to ignore */
  double fstartobs2,                                   /* Starting frequency for the 22 mode of wf 2 - as determined from a limited duration of the observation - set to 0 to ignore */
  int tagint)                                          /* Tag choosing the integrator: 0 for wip, 1 for log linear integration */
{
  double overlap;
  if(tagint==0) {
    overlap = FDListmodesWIPOverlap(listh1, listh2, Snoise, fLow, fHigh, fstartobs1, fstartobs2);
  }
  else if(tagint==1) {
    overlap = FDListmodesLogLinearOverlap(listh1, listh2, Snoise, fLow, fHigh, fstartobs1, fstartobs2);
  }
  return overlap;
}

/* Function computing the log likelihood (h|s) - 1/2 (h|h) - 1/2 (s|s), with s the signal, h the template, and where we keep the constant term (s|s) - passed to the function as a parameter - all the cross-products between modes are taken into account - two additionals parameters for the starting 22-mode frequencies (then properly scaled for the other modes) for a limited duration of the observations */
double FDLogLikelihood(
  struct tagListmodesCAmpPhaseFrequencySeries *lists,  /* Input: list of modes for the signal s, in Frequency-domain amplitude and phase form */
  struct tagListmodesCAmpPhaseFrequencySeries *listh,  /* Input: list of modes for the template, in Frequency-domain amplitude and phase form */
  ObjectFunction * Snoise,                      /* Noise function */
  double fLow,                                         /* Lower bound of the frequency window for the detector */
  double fHigh,                                        /* Upper bound of the frequency window for the detector */
  double ss,                                           /* Inner product (s|s), constant to be computed elsewhere and passed as an argument */
  double hh,                                           /* Inner product (h|h), constant to be computed elsewhere and passed as an argument */
  double fstartobss,                                   /* Starting frequency for the 22 mode of s - as determined from a limited duration of the observation - set to 0 to ignore */
  double fstartobsh,                                   /* Starting frequency for the 22 mode of h - as determined from a limited duration of the observation - set to 0 to ignore */
  int tagint)                                          /* Tag choosing the integrator: 0 for wip, 1 for log linear integration */
{
  double lnL;
  if(tagint==0) {
    lnL = FDListmodesWIPOverlap(lists, listh, Snoise, fLow, fHigh, fstartobss, fstartobsh) - 1./2 * hh - 1./2 * ss;
  }
  else if(tagint==1) {
    lnL = FDListmodesLogLinearOverlap(lists, listh, Snoise, fLow, fHigh, fstartobss, fstartobsh) - 1./2 * hh - 1./2 * ss;
  }
  return lnL;
}

/************** Functions for overlap/likelihood specific to loglinear integration *****************/

/* Function computing the log likelihood -1/2(h-s|h-s), with s the signal, h the template (both given as frequency series in Re/Im form) - h, s assumed to be given on the same set of frequencies - same for the vector of noise values - fstartobs for s, h has already been taken into account */
double FDLogLikelihoodReIm(
  struct tagReImFrequencySeries *s,    /* First waveform (injection), frequency series in Re/Im form */
  struct tagReImFrequencySeries *h,    /* Second waveform (template), frequency series in Re/Im form */
  gsl_vector* noisevalues)             /* Vector for the noise values on common freq of the freqseries */
{
  /* Check the lengths */
  if(s->freq->size != noisevalues->size || h->freq->size != noisevalues->size) {
    printf("Error: inconsistent lengths in FDLogLikelihoodReIm.\n");
    exit(1);
  }

  /* Taking the difference between the frequency series: diff = h-s */
  int nbpts = (int) s->freq->size;
  ReImFrequencySeries* diff = NULL;
  ReImFrequencySeries_Init(&diff, nbpts);
  gsl_vector_memcpy(diff->freq, s->freq);
  gsl_vector_memcpy(diff->h_real, h->h_real);
  gsl_vector_memcpy(diff->h_imag, h->h_imag);
  gsl_vector_sub(diff->h_real, s->h_real);
  gsl_vector_sub(diff->h_imag, s->h_imag);

  /* Likelihood lnL = -1/2(h-s|h-s) */
  double lnL;
  lnL = -1./2 * FDOverlapReImvsReIm(diff, diff, noisevalues);

  /* Clean up */
  ReImFrequencySeries_Cleanup(diff);

  return lnL;
}

/***************************** Functions for overlaps using amplitude/phase (Fresnel) ******************************/

/*  Moved these to splinecoeffs.c

// Note: for the spines in matrix form, the first column contains the x values, so the coeffs start at 1
static double EvalCubic(
  gsl_vector* coeffs,
  double eps,
  double eps2,
  double eps3)
{
  double p0 = gsl_vector_get(coeffs, 1);
  double p1 = gsl_vector_get(coeffs, 2);
  double p2 = gsl_vector_get(coeffs, 3);
  double p3 = gsl_vector_get(coeffs, 4);
  return p0 + p1*eps + p2*eps2 + p3*eps3;
}
static double EvalQuad(
  gsl_vector* coeffs,
  double eps,
  double eps2)
{
  double p0 = gsl_vector_get(coeffs, 1);
  double p1 = gsl_vector_get(coeffs, 2);
  double p2 = gsl_vector_get(coeffs, 3);
  return p0 + p1*eps + p2*eps2;
}
*/

/* Quadratic Legendre approximation to compute values at minf and maxf when they do not fall on the grid of a freqseries, using the two first/last intervals */
static double EstimateBoundaryLegendreQuad(
  gsl_vector* vectx,   /**/
  gsl_vector* vecty,   /**/
  int j,               /**/
  double xvalue)       /**/
{
  double x0 = 0;
  double x1 = gsl_vector_get(vectx, j+1) - gsl_vector_get(vectx, j);
  double x2 = gsl_vector_get(vectx, j+2) - gsl_vector_get(vectx, j);
  double x = xvalue - gsl_vector_get(vectx, j);
  double y0 = gsl_vector_get(vecty, j);
  double y1 = gsl_vector_get(vecty, j+1);
  double y2 = gsl_vector_get(vecty, j+2);
  if(!(x>=x0 && x<=x2)) {
    printf("Error: value out of bounds in EstimateBoundaryLegendreQuad.\n");
    exit(1);
  }
  return y0*(x-x1)*(x-x2)/(x0-x1)/(x0-x2) + y1*(x-x0)*(x-x2)/(x1-x0)/(x1-x2) + y2*(x-x0)*(x-x1)/(x2-x0)/(x2-x1);
}

/* Function computing the integrand values */
void ComputeIntegrandValues(
  CAmpPhaseFrequencySeries** integrand,     /* Output: values of the integrand on common frequencies (initialized in the function) */
  CAmpPhaseFrequencySeries* freqseries1,    /* Input: frequency series for wf 1 */
  CAmpPhaseSpline* splines2,                /* Input: splines in matrix form for wf 2 */
  ObjectFunction * Snoise,           /* Noise function */
  double fLow,                              /* Lower bound of the frequency - 0 to ignore */
  double fHigh)                             /* Upper bound of the frequency - 0 to ignore */
{
  gsl_set_error_handler(&Err_Handler);

  /* Determining the boundaries of indices */
  gsl_vector* freq1 = freqseries1->freq;
  int imin1 = 0;
  int imax1 = freq1->size - 1;
  double* f1 = freq1->data;
  double f2min = gsl_matrix_get(splines2->quadspline_phase, 0, 0);
  double f2max = gsl_matrix_get(splines2->quadspline_phase, splines2->quadspline_phase->size1 - 1, 0);
  if((fLow>0 && (f1[imax1]<=fLow || f2max<=fLow)) || (fHigh>0 && (f1[imin1]>=fHigh || f2min>=fHigh))) {
    printf("Error: range of frequencies incompatible with fLow, fHigh in IntegrandValues.\n");
    printf("need one of {%g, %g} <= %g and one of {%g, %g}>=%g\n",f1[imax1],f2max,fLow,f1[imin1],f2min,fHigh);
    exit(1);
  }
  /* If starting outside, move the ends of the frequency series to be just outside the final minf and maxf */
  double minf = fmax(f1[imin1], f2min);
  double maxf = fmin(f1[imax1], f2max);
  if(fLow>0) {minf = fmax(fLow, minf);}
  if(fHigh>0) {maxf = fmin(fHigh, maxf);}
  while(f1[imin1+1]<=minf) imin1++;
  while(f1[imax1-1]>=maxf) imax1--;
  /* Estimate locally values for freqseries1 at the boundaries */
  double areal1minf = EstimateBoundaryLegendreQuad(freq1, freqseries1->amp_real, imin1, minf);
  double aimag1minf = EstimateBoundaryLegendreQuad(freq1, freqseries1->amp_imag, imin1, minf);
  double phi1minf = EstimateBoundaryLegendreQuad(freq1, freqseries1->phase, imin1, minf);
  double areal1maxf = EstimateBoundaryLegendreQuad(freq1, freqseries1->amp_real, imax1-2, maxf); /* Note the imax1-2 */
  double aimag1maxf = EstimateBoundaryLegendreQuad(freq1, freqseries1->amp_imag, imax1-2, maxf); /* Note the imax1-2 */
  double phi1maxf = EstimateBoundaryLegendreQuad(freq1, freqseries1->phase, imax1-2, maxf); /* Note the imax1-2 */

  /* Initializing output structure */
  int nbpts = imax1 + 1 - imin1;
  CAmpPhaseFrequencySeries_Init(integrand, nbpts);

  /* Loop computing integrand values */
  gsl_vector* freq = (*integrand)->freq;
  gsl_vector* ampreal = (*integrand)->amp_real;
  gsl_vector* ampimag = (*integrand)->amp_imag;
  gsl_vector* phase = (*integrand)->phase;
  double f, eps, eps2, eps3, ampreal1, ampimag1, phase1, ampreal2, ampimag2, phase2, invSn;
  double complex camp;
  double* areal1 = freqseries1->amp_real->data;
  double* aimag1 = freqseries1->amp_imag->data;
  double* phi1 = freqseries1->phase->data;
  gsl_matrix* splineAreal2 = splines2->spline_amp_real;
  gsl_matrix* splineAimag2 = splines2->spline_amp_imag;
  gsl_matrix* quadsplinephase2 = splines2->quadspline_phase;
  int i2 = 0; int j = 0;
  for(int i=imin1; i<=imax1; i++) {
    /* Distinguish the case where we are at minf or maxf */
    if(i==imin1) {
      f = minf;
      ampreal1 = areal1minf;
      ampimag1 = aimag1minf;
      phase1 = phi1minf;
    }
    else if(i==imax1) {
      f = maxf;
      ampreal1 = areal1maxf;
      ampimag1 = aimag1maxf;
      phase1 = phi1maxf;
    }
    else {
      f = gsl_vector_get(freq1, i);
      ampreal1 = areal1[i];
      ampimag1 = aimag1[i];
      phase1 = phi1[i];
    }
    /* Adjust the index in the spline if necessary and compute */
    while(gsl_matrix_get(splines2->quadspline_phase, i2+1, 0)<f) i2++;
    eps = f - gsl_matrix_get(splines2->quadspline_phase, i2, 0);
    eps2 = eps*eps;
    eps3 = eps2*eps;
    gsl_vector_view coeffsampreal2 = gsl_matrix_row(splineAreal2, i2);
    gsl_vector_view coeffsampimag2 = gsl_matrix_row(splineAimag2, i2);
    gsl_vector_view coeffsphase2 = gsl_matrix_row(quadsplinephase2, i2);
    ampreal2 = EvalCubic(&coeffsampreal2.vector, eps, eps2, eps3);
    ampimag2 = EvalCubic(&coeffsampimag2.vector, eps, eps2, eps3);
    phase2 = EvalQuad(&coeffsphase2.vector, eps, eps2);
    invSn = 1./ObjectFunctionCall(Snoise,f);
    camp = invSn * (ampreal1 + I*ampimag1) * (ampreal2 - I*ampimag2);
    gsl_vector_set(freq, j, f);
    gsl_vector_set(ampreal, j, creal(camp));
    gsl_vector_set(ampimag, j, cimag(camp));
    gsl_vector_set(phase, j, phase1 - phase2);
    j++;
  }
}

/* Function computing the integrand values, combining three non-correlated channels */
int ComputeIntegrandValues3Chan(
  CAmpPhaseFrequencySeries** integrand,     /* Output: values of the integrand on common frequencies (initialized in the function) */
  CAmpPhaseFrequencySeries* freqseries1chan1,    /* Input: frequency series for wf 1, channel 1 */
  CAmpPhaseFrequencySeries* freqseries1chan2,    /* Input: frequency series for wf 1, channel 2 */
  CAmpPhaseFrequencySeries* freqseries1chan3,    /* Input: frequency series for wf 1, channel 3 */
  CAmpPhaseSpline* splines2chan1,                /* Input: splines in matrix form for wf 2, channel 1 */
  CAmpPhaseSpline* splines2chan2,                /* Input: splines in matrix form for wf 2, channel 2 */
  CAmpPhaseSpline* splines2chan3,                /* Input: splines in matrix form for wf 2, channel 3 */
  ObjectFunction * Snoise1,                /* Noise function */
  ObjectFunction * Snoise2,                /* Noise function */
  ObjectFunction * Snoise3,                /* Noise function */
  double fLow,                              /* Lower bound of the frequency - 0 to ignore */
  double fHigh)                             /* Upper bound of the frequency - 0 to ignore */
{
  gsl_set_error_handler(&Err_Handler);

  /* Determining the boundaries of indices - frequency vectors assumed to be the same for channels 1,2,3 */
  gsl_vector* freq1 = freqseries1chan1->freq;
  int imin1 = 0;
  int imax1 = freq1->size - 1;
  double* f1 = freq1->data;
  double f2min = gsl_matrix_get(splines2chan1->quadspline_phase, 0, 0);
  double f2max = gsl_matrix_get(splines2chan1->quadspline_phase, splines2chan1->quadspline_phase->size1 - 1, 0);
  if((fLow>0 && (f1[imax1]<=fLow || f2max<=fLow)) || (fHigh>0 && (f1[imin1]>=fHigh || f2min>=fHigh))) {
    //printf("Error: range of frequencies incompatible with fLow, fHigh in IntegrandValues.\n");
    //printf("need both {%g, %g} > %g and both {%g, %g} < %g\n",f1[imax1],f2max,fLow,f1[imin1],f2min,fHigh);
    return -1;
  }
  /* If starting outside, move the ends of the frequency series to be just outside the final minf and maxf */
  double minf = fmax(f1[imin1], f2min);
  double maxf = fmin(f1[imax1], f2max);
  if(fLow>0) {minf = fmax(fLow, minf);}
  if(fHigh>0) {maxf = fmin(fHigh, maxf);}
  while(f1[imin1+1]<=minf) imin1++;
  while(f1[imax1-1]>=maxf) imax1--;
  //printf("imin=%i, imax=%i\n",imin1,imax1);
  int nbpts = imax1 + 1 - imin1;
  //printf("nbpts=%i\n",nbpts);
  if(nbpts<4)return -1;
  /* Estimate locally values for freqseries1 at the boundaries - phase vectors assumed to be the same for channels 1,2,3 - this is still true now that the response-processed phase includes the signal phase + R-delay phase, which is the same for all channels */
  double areal1chan1minf = EstimateBoundaryLegendreQuad(freq1, freqseries1chan1->amp_real, imin1, minf);
  double aimag1chan1minf = EstimateBoundaryLegendreQuad(freq1, freqseries1chan1->amp_imag, imin1, minf);
  double areal1chan2minf = EstimateBoundaryLegendreQuad(freq1, freqseries1chan2->amp_real, imin1, minf);
  double aimag1chan2minf = EstimateBoundaryLegendreQuad(freq1, freqseries1chan2->amp_imag, imin1, minf);
  double areal1chan3minf = EstimateBoundaryLegendreQuad(freq1, freqseries1chan3->amp_real, imin1, minf);
  double aimag1chan3minf = EstimateBoundaryLegendreQuad(freq1, freqseries1chan3->amp_imag, imin1, minf);
  double phi1minf = EstimateBoundaryLegendreQuad(freq1, freqseries1chan1->phase, imin1, minf);
  double areal1chan1maxf = EstimateBoundaryLegendreQuad(freq1, freqseries1chan1->amp_real, imax1-2, maxf); /* Note the imax1-2 */
  double aimag1chan1maxf = EstimateBoundaryLegendreQuad(freq1, freqseries1chan1->amp_imag, imax1-2, maxf); /* Note the imax1-2 */
  double areal1chan2maxf = EstimateBoundaryLegendreQuad(freq1, freqseries1chan2->amp_real, imax1-2, maxf); /* Note the imax1-2 */
  double aimag1chan2maxf = EstimateBoundaryLegendreQuad(freq1, freqseries1chan2->amp_imag, imax1-2, maxf); /* Note the imax1-2 */
  double areal1chan3maxf = EstimateBoundaryLegendreQuad(freq1, freqseries1chan3->amp_real, imax1-2, maxf); /* Note the imax1-2 */
  double aimag1chan3maxf = EstimateBoundaryLegendreQuad(freq1, freqseries1chan3->amp_imag, imax1-2, maxf); /* Note the imax1-2 */
  double phi1maxf = EstimateBoundaryLegendreQuad(freq1, freqseries1chan1->phase, imax1-2, maxf); /* Note the imax1-2 */

  /* Initializing output structure */
  CAmpPhaseFrequencySeries_Init(integrand, nbpts);

  /* Loop computing integrand values - phases are the same for chan1, chan2 and chan3 */
  gsl_vector* freq = (*integrand)->freq;
  gsl_vector* ampreal = (*integrand)->amp_real;
  gsl_vector* ampimag = (*integrand)->amp_imag;
  gsl_vector* phase = (*integrand)->phase;
  double f, eps, eps2, eps3, ampreal1chan1, ampimag1chan1, ampreal1chan2, ampimag1chan2, ampreal1chan3, ampimag1chan3, phase1, ampreal2chan1, ampimag2chan1, ampreal2chan2, ampimag2chan2, ampreal2chan3, ampimag2chan3, phase2, invSnchan1, invSnchan2, invSnchan3;
  double complex camp;
  double* areal1chan1 = freqseries1chan1->amp_real->data;
  double* aimag1chan1 = freqseries1chan1->amp_imag->data;
  double* areal1chan2 = freqseries1chan2->amp_real->data;
  double* aimag1chan2 = freqseries1chan2->amp_imag->data;
  double* areal1chan3 = freqseries1chan3->amp_real->data;
  double* aimag1chan3 = freqseries1chan3->amp_imag->data;
  double* phi1 = freqseries1chan1->phase->data;
  gsl_matrix* splinechan1real2chan1 = splines2chan1->spline_amp_real;
  gsl_matrix* splinechan1imag2chan1 = splines2chan1->spline_amp_imag;
  gsl_matrix* splinechan1real2chan2 = splines2chan2->spline_amp_real;
  gsl_matrix* splinechan1imag2chan2 = splines2chan2->spline_amp_imag;
  gsl_matrix* splinechan1real2chan3 = splines2chan3->spline_amp_real;
  gsl_matrix* splinechan1imag2chan3 = splines2chan3->spline_amp_imag;
  gsl_matrix* quadsplinephase2 = splines2chan1->quadspline_phase;
  int i2 = 0; int j = 0;
  for(int i=imin1; i<=imax1; i++) {
    /* Distinguish the case where we are at minf or maxf */
    if(i==imin1) {
      f = minf;
      ampreal1chan1 = areal1chan1minf;
      ampimag1chan1 = aimag1chan1minf;
      ampreal1chan2 = areal1chan2minf;
      ampimag1chan2 = aimag1chan2minf;
      ampreal1chan3 = areal1chan3minf;
      ampimag1chan3 = aimag1chan3minf;
      phase1 = phi1minf;
    }
    else if(i==imax1) {
      f = maxf;
      ampreal1chan1 = areal1chan1maxf;
      ampimag1chan1 = aimag1chan1maxf;
      ampreal1chan2 = areal1chan2maxf;
      ampimag1chan2 = aimag1chan2maxf;
      ampreal1chan3 = areal1chan3maxf;
      ampimag1chan3 = aimag1chan3maxf;
      phase1 = phi1maxf;
    }
    else {
      f = gsl_vector_get(freq1, i);
      ampreal1chan1 = areal1chan1[i];
      ampimag1chan1 = aimag1chan1[i];
      ampreal1chan2 = areal1chan2[i];
      ampimag1chan2 = aimag1chan2[i];
      ampreal1chan3 = areal1chan3[i];
      ampimag1chan3 = aimag1chan3[i];
      phase1 = phi1[i];
    }
    /* Adjust the index in the spline if necessary and compute */
    while(gsl_matrix_get(splines2chan1->quadspline_phase, i2+1, 0)<f) i2++;
    eps = f - gsl_matrix_get(splines2chan1->quadspline_phase, i2, 0);
    eps2 = eps*eps;
    eps3 = eps2*eps;
    gsl_vector_view coeffsampreal2chan1 = gsl_matrix_row(splinechan1real2chan1, i2);
    gsl_vector_view coeffsampimag2chan1 = gsl_matrix_row(splinechan1imag2chan1, i2);
    gsl_vector_view coeffsampreal2chan2 = gsl_matrix_row(splinechan1real2chan2, i2);
    gsl_vector_view coeffsampimag2chan2 = gsl_matrix_row(splinechan1imag2chan2, i2);
    gsl_vector_view coeffsampreal2chan3 = gsl_matrix_row(splinechan1real2chan3, i2);
    gsl_vector_view coeffsampimag2chan3 = gsl_matrix_row(splinechan1imag2chan3, i2);
    gsl_vector_view coeffsphase2 = gsl_matrix_row(quadsplinephase2, i2);
    ampreal2chan1 = EvalCubic(&coeffsampreal2chan1.vector, eps, eps2, eps3);
    ampimag2chan1 = EvalCubic(&coeffsampimag2chan1.vector, eps, eps2, eps3);
    ampreal2chan2 = EvalCubic(&coeffsampreal2chan2.vector, eps, eps2, eps3);
    ampimag2chan2 = EvalCubic(&coeffsampimag2chan2.vector, eps, eps2, eps3);
    ampreal2chan3 = EvalCubic(&coeffsampreal2chan3.vector, eps, eps2, eps3);
    ampimag2chan3 = EvalCubic(&coeffsampimag2chan3.vector, eps, eps2, eps3);
    phase2 = EvalQuad(&coeffsphase2.vector, eps, eps2);
    invSnchan1 = 1./ObjectFunctionCall(Snoise1,f);
    invSnchan2 = 1./ObjectFunctionCall(Snoise2,f);
    invSnchan3 = 1./ObjectFunctionCall(Snoise3,f);

    camp = invSnchan1 * (ampreal1chan1 + I*ampimag1chan1) * (ampreal2chan1 - I*ampimag2chan1) + invSnchan2 * (ampreal1chan2 + I*ampimag1chan2) * (ampreal2chan2 - I*ampimag2chan2) + invSnchan3 * (ampreal1chan3 + I*ampimag1chan3) * (ampreal2chan3 - I*ampimag2chan3);
    //dump
    /*
    printf("j=%i, im=%g\n a11=(%g,%g),  a12=(%g,%g),  a13=(%g,%g)\n a11=(%g,%g),  a12=(%g,%g),  a13=(%g,%g)\n",j,cimag(camp),
	   ampreal1chan1,ampimag1chan1,ampreal1chan2,ampimag1chan2,ampreal1chan3,ampimag1chan3,
	   ampreal2chan1,ampimag2chan1,ampreal2chan2,ampimag2chan2,ampreal2chan3,ampimag2chan3);
    printf("in1=%g, in2=%g, in3=%g\n",invSnchan1,invSnchan2,invSnchan3);
    */

    gsl_vector_set(freq, j, f);
    gsl_vector_set(ampreal, j, creal(camp));
    gsl_vector_set(ampimag, j, cimag(camp));
    gsl_vector_set(phase, j, phase1 - phase2);
    j++;
  }
  return 0;
}

/* Function computing the overlap (h1|h2) between two given modes in amplitude/phase form, one being already interpolated, for a given noise function - uses the amplitude/phase representation (Fresnel) */
double FDSinglemodeFresnelOverlap(
  struct tagCAmpPhaseFrequencySeries *freqseries1, /* First mode h1, in amplitude/phase form */
  struct tagCAmpPhaseSpline *splines2,             /* Second mode h2, already interpolated in matrix form */
  ObjectFunction * Snoise,                  /* Noise function */
  double fLow,                                     /* Lower bound of the frequency window for the detector */
  double fHigh)                                    /* Upper bound of the frequency window for the detector */
{
  /* Computing the integrand values, on the frequency grid of h1 */
  CAmpPhaseFrequencySeries* integrand = NULL;
  ComputeIntegrandValues(&integrand, freqseries1, splines2, Snoise, fLow, fHigh);

  /* Rescaling the integrand */
  double scaling = 10./gsl_vector_get(integrand->freq, integrand->freq->size-1);
  gsl_vector_scale(integrand->freq, scaling);
  gsl_vector_scale(integrand->amp_real, 1./scaling);
  gsl_vector_scale(integrand->amp_imag, 1./scaling);

  /* Interpolating the integrand */
  CAmpPhaseSpline* integrandspline = NULL;
  BuildSplineCoeffs(&integrandspline, integrand);

  /* Computing the integral - including here the factor 4 and the real part */
  double overlap = 4.*creal(ComputeInt(integrandspline->spline_amp_real, integrandspline->spline_amp_imag, integrandspline->quadspline_phase));

  /* Clean up */
  CAmpPhaseSpline_Cleanup(integrandspline);
  CAmpPhaseFrequencySeries_Cleanup(integrand);

  return overlap;
}

/* Function computing the overlap (h1|h2) between two given modes in amplitude/phase form for each non-correlated channel 1,2,3, one being already interpolated, for a given noise function - uses the amplitude/phase representation (Fresnel) */
double FDSinglemodeFresnelOverlap3Chan(
  struct tagCAmpPhaseFrequencySeries *freqseries1chan1, /* First mode h1 for channel 1, in amplitude/phase form */
  struct tagCAmpPhaseFrequencySeries *freqseries1chan2, /* First mode h1 for channel 2, in amplitude/phase form */
  struct tagCAmpPhaseFrequencySeries *freqseries1chan3, /* First mode h1 for channel 3, in amplitude/phase form */
  struct tagCAmpPhaseSpline *splines2chan1,             /* Second mode h2 for channel 1, already interpolated in matrix form */
  struct tagCAmpPhaseSpline *splines2chan2,             /* Second mode h2 for channel 2, already interpolated in matrix form */
  struct tagCAmpPhaseSpline *splines2chan3,             /* Second mode h2 for channel 3, already interpolated in matrix form */
  ObjectFunction * Snoisechan1,                  /* Noise function */
  ObjectFunction * Snoisechan2,                  /* Noise function */
  ObjectFunction * Snoisechan3,                  /* Noise function */
  double fLow,                                      /* Lower bound of the frequency window for the detector */
  double fHigh)                                     /* Upper bound of the frequency window for the detector */
{
  /* Computing the integrand values, on the frequency grid of h1 */
  CAmpPhaseFrequencySeries* integrand = NULL;
  if(0>ComputeIntegrandValues3Chan(&integrand, freqseries1chan1, freqseries1chan2, freqseries1chan3, splines2chan1, splines2chan2, splines2chan3, Snoisechan1, Snoisechan2, Snoisechan3, fLow, fHigh))return 0;//if allowed freq range does not exist, return 0 for overlap

  /* Rescaling the integrand */
  double scaling = 10./gsl_vector_get(integrand->freq, integrand->freq->size-1);

//TEST
//scaling = 1.;

  gsl_vector_scale(integrand->freq, scaling);
  gsl_vector_scale(integrand->amp_real, 1./scaling);
  gsl_vector_scale(integrand->amp_imag, 1./scaling);

  //dump
  /*
  for(int ii=0;ii<integrand->freq->size;ii++)
    printf("ii=%i, f=%g, integrand = ( %g, %g, %g )\n",ii,gsl_vector_get(integrand->freq,ii),gsl_vector_get(integrand->amp_real,ii),gsl_vector_get(integrand->amp_imag,ii),gsl_vector_get(integrand->phase,ii));
  */

  /* Interpolating the integrand */
  CAmpPhaseSpline* integrandspline = NULL;
  BuildSplineCoeffs(&integrandspline, integrand);

  /* Computing the integral - including here the factor 4 and the real part */
  double overlap = 4.*creal(ComputeInt(integrandspline->spline_amp_real, integrandspline->spline_amp_imag, integrandspline->quadspline_phase));

  /* Clean up */
  CAmpPhaseSpline_Cleanup(integrandspline);
  CAmpPhaseFrequencySeries_Cleanup(integrand);

  return overlap;
}

/* Function computing the overlap (h1|h2) between two waveforms given as list of modes, one being already interpolated, for a given noise function - two additional parameters for the starting 22-mode frequencies (then properly scaled for the other modes) for a limited duration of the observations */
double FDListmodesFresnelOverlap(
  struct tagListmodesCAmpPhaseFrequencySeries *listh1, /* First waveform, list of modes in amplitude/phase form */
  struct tagListmodesCAmpPhaseSpline *listsplines2,    /* Second waveform, list of modes already interpolated in matrix form */
  ObjectFunction * Snoise,                  /* Noise function */
  double fLow,                                         /* Lower bound of the frequency window for the detector */
  double fHigh,                                        /* Upper bound of the frequency window for the detector */
  double fstartobs1,                                   /* Starting frequency for the 22 mode of wf 1 - as determined from a limited duration of the observation - set to 0 to ignore */
  double fstartobs2)                                   /* Starting frequency for the 22 mode of wf 2 - as determined from a limited duration of the observation - set to 0 to ignore */
{
  double overlap = 0;

  /* Main loop over the modes - goes through all the modes present */
  ListmodesCAmpPhaseFrequencySeries* listelementh1 = listh1;
  while(listelementh1) {
    ListmodesCAmpPhaseSpline* listelementsplines2 = listsplines2;
    while(listelementsplines2) {
      /* Scaling fstartobs1/2 with the appropriate factor of m (for the 21 mode we use m=2) - setting fmin in the overlap accordingly */
      int mmax1 = max(2, listelementh1->m);
      int mmax2 = max(2, listelementsplines2->m);
      double fcutLow = fmax(fLow, fmax(((double) mmax1)/2. * fstartobs1, ((double) mmax2)/2. * fstartobs2));
      overlap += FDSinglemodeFresnelOverlap(listelementh1->freqseries, listelementsplines2->splines, Snoise, fcutLow, fHigh);

      listelementsplines2 = listelementsplines2->next;
    }
    listelementh1 = listelementh1->next;
  }
  return overlap;
}

/* Function computing the overlap (h1|h2) between two waveforms given as list of modes for each non-correlated channel 1,2,3, one being already interpolated, for a given noise function - two additional parameters for the starting 22-mode frequencies (then properly scaled for the other modes) for a limited duration of the observations */
double FDListmodesFresnelOverlap3Chan(
  struct tagListmodesCAmpPhaseFrequencySeries *listh1chan1, /* First waveform channel channel 1, list of modes in amplitude/phase form */
  struct tagListmodesCAmpPhaseFrequencySeries *listh1chan2, /* First waveform channel channel 2, list of modes in amplitude/phase form */
  struct tagListmodesCAmpPhaseFrequencySeries *listh1chan3, /* First waveform channel channel 3, list of modes in amplitude/phase form */
  struct tagListmodesCAmpPhaseSpline *listsplines2chan1,    /* Second waveform channel channel 1, list of modes already interpolated in matrix form */
  struct tagListmodesCAmpPhaseSpline *listsplines2chan2,    /* Second waveform channel channel 2, list of modes already interpolated in matrix form */
  struct tagListmodesCAmpPhaseSpline *listsplines2chan3,    /* Second waveform channel channel 3, list of modes already interpolated in matrix form */
  ObjectFunction * Snoise1,                          /* Noise function for channel 1 */
  ObjectFunction * Snoise2,                          /* Noise function for channel 1 */
  ObjectFunction * Snoise3,                          /* Noise function for channel 1 */
  double fLow,                                          /* Lower bound of the frequency window for the detector */
  double fHigh,                                         /* Upper bound of the frequency window for the detector */
  double fstartobs1,                                    /* Starting frequency for the 22 mode of wf 1 - as determined from a limited duration of the observation - set to 0 to ignore */
  double fstartobs2)                                    /* Starting frequency for the 22 mode of wf 2 - as determined from a limited duration of the observation - set to 0 to ignore */
{
  double overlap = 0;

  /* Main loop over the modes - goes through all the modes present, the same for all three channels 1,2,3 */
  ListmodesCAmpPhaseFrequencySeries* listelementh1chan1 = listh1chan1;
  while(listelementh1chan1) { /* We use the structure for channel 1 to loop through modes */
    ListmodesCAmpPhaseFrequencySeries* listelementh1chan2 = ListmodesCAmpPhaseFrequencySeries_GetMode(listh1chan2, listelementh1chan1->l, listelementh1chan1->m);
    ListmodesCAmpPhaseFrequencySeries* listelementh1chan3 = ListmodesCAmpPhaseFrequencySeries_GetMode(listh1chan3, listelementh1chan1->l, listelementh1chan1->m);
    ListmodesCAmpPhaseSpline* listelementsplines2chan1 = listsplines2chan1;
    while(listelementsplines2chan1) { /* We use the structure for channel 1 to loop through modes */
      ListmodesCAmpPhaseSpline* listelementsplines2chan2 = ListmodesCAmpPhaseSpline_GetMode(listsplines2chan2, listelementsplines2chan1->l, listelementsplines2chan1->m);
      ListmodesCAmpPhaseSpline* listelementsplines2chan3 = ListmodesCAmpPhaseSpline_GetMode(listsplines2chan3, listelementsplines2chan1->l, listelementsplines2chan1->m);
      /* Scaling fstartobs1/2 with the appropriate factor of m (for the 21 mode we use m=2) - setting fmin in the overlap accordingly */
      int mmax1 = max(2, listelementh1chan1->m);
      int mmax2 = max(2, listelementsplines2chan1->m);
      double fcutLow = fmax(fLow, fmax(((double) mmax1)/2. * fstartobs1, ((double) mmax2)/2. * fstartobs2));
      double overlapmode = FDSinglemodeFresnelOverlap3Chan(listelementh1chan1->freqseries, listelementh1chan2->freqseries, listelementh1chan3->freqseries, listelementsplines2chan1->splines, listelementsplines2chan2->splines, listelementsplines2chan3->splines, Snoise1, Snoise2, Snoise3, fcutLow, fHigh);
      overlap += overlapmode;
      listelementsplines2chan1 = listelementsplines2chan1->next;
    }
    listelementh1chan1 = listelementh1chan1->next;
  }

  return overlap;
}


#ifndef SurrogateHeatFluidDriver_h
#define SurrogateHeatFluidDriver_h

#include <memory>
#include <vector>

#include "Teuchos_ParameterList.hpp"
#include "Teuchos_RCP.hpp"

#include <gsl/gsl>

#include "Assembly_Model.h"
#include "Single_Pin_Subchannel.h"
#include "Single_Pin_Conduction.h"

namespace enrico {

//! Driver for coupled subchannel/heat conduction simulations. This driver
//! decouples the fluid and solid phase in an explicit manner (i.e. no
//! iteration between the convective heat transfer linking the phases within
//! a single solve), which is sufficient given that this project targets
//! steady-state simulations via fixed point iteration.
class SurrogateHeatFluidDriver {
public:
  //@{
  //! Typedefs
  using SP_Assembly = std::shared_ptr<Assembly_Model>;
  using RCP_PL = Teuchos::RCP<Teuchos::ParameterList>;
  //@}

  //! solve the thermal-hydraulic problem for the fluid and solid phases
  //! for a given power distribution
  void solve(const std::vector<double>& powers);

  //! fluid temperature
  virtual std::vector<double> fluid_temperature() const { return d_pin_temps; }

  //! fluid density
  virtual std::vector<double> density() const { return d_pin_densities; }

  //! solid temperature
  virtual std::vector<double> solid_temperature() const { return d_solid_temps; }

  double pressure_bc_; //! System pressure in [MPa]

private:
  // >>> DATA
  SP_Assembly d_assembly;

  // Note that for channel-centered equations, Nx and Ny are one greater
  //  than number of pins in each direction.
  int d_Nx, d_Ny, d_Nz;

  // Cross sectional area of each channel
  std::vector<double> d_areas;

  // Mass flow rate (kg/s) in each channel
  std::vector<double> d_mdots;

  //! subchannel solver for a single channel
  std::unique_ptr<Single_Pin_Subchannel> d_pin_subchannel;

  //! heat conduction solver for a single rod
  std::unique_ptr<Single_Pin_Conduction> d_pin_conduction;

  //! coolant temperature in [K] for each channel, of total length given by the
  //! product of the number of pins by the number of axial cells
  std::vector<double> d_pin_temps;

  //! coolant density in [g/cm^3] for each channel, of total length given by the
  //! product of the number of pins by the number of axial cells
  std::vector<double> d_pin_densities;

  //! solid temperature in [K]
  std::vector<double> d_solid_temps;

public:
  // Constructor
  SurrogateHeatFluidDriver(SP_Assembly assembly,
                       RCP_PL subchannel_params,
                       RCP_PL conduction_params,
                       const std::vector<double>& dz);

  //! heat source
  std::vector<double> d_pin_powers;

private:
  //! Set up the sizes of solution arrays
  void generate_arrays();

  int channel_index(int ix, int iy) const
  {
    Expects(ix < d_Nx);
    Expects(iy < d_Ny);
    return ix + d_Nx * iy;
  }

  // Solve the fluid flow equations
  void solve_fluid(const std::vector<double>& power);

  // Solve the solid equations
  void solve_heat(const std::vector<double>& power,
    const std::vector<double>& channel_temp, std::vector<double>& fuel_temp);
};

//---------------------------------------------------------------------------//
} // end namespace enrico

//---------------------------------------------------------------------------//
#endif // SurrogateHeatFluidDriver_h

//---------------------------------------------------------------------------//
// end of SurrogateHeatFluidDriver.h
//---------------------------------------------------------------------------//