/*
  NAME:
     calc_splitnmerge
  PURPOSE:
     calculates the split and merge hierarchy after an proj_EM convergence
  CALLING SEQUENCE:
     calc_splitnmerge(struct datapoint * data,int N,
     struct gaussian * gaussians, int K, gsl_matrix * qij,
     int * snmhierarchy){
  INPUT:
     data      - the data
     N         - number of data points
     gaussians - model gaussians
     K         - number of gaussians
     qij       - matrix of log(posterior likelihoods)
     bs        - bs from previous EM estimate
  OUTPUT:
     snmhierarchy - the hierarchy, first row has the highest prioriry,
                    goes down from there
  REVISION HISTORY:
     2008-09-21 - Written Bovy
*/
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <proj_gauss_mixtures.h>

void calc_splitnmerge(struct datapoint * data,int N,
		      struct gaussian * gaussians, int K,
		      gsl_matrix * qij, int * snmhierarchy){
  gsl_matrix * tempqij = gsl_matrix_alloc(N,K);
  gsl_matrix_memcpy(tempqij,qij);
  gsl_matrix * Jmerge = gsl_matrix_alloc(K,K);
  gsl_matrix_set_all(Jmerge,-1.);
  int kk1, kk2, kk, ii,maxsnm= K*(K-1)*(K-2)/2;
  int d = (gaussians->VV)->size1;//dim of mm
  double temp1,temp2,temp;
  for (kk1 = 0; kk1 != K; ++kk1)
    for (kk2 = kk1+1; kk2 != K; ++kk2){
      temp = 0.;
      for (ii=0; ii != N; ++ii){
	//make them all exps
	temp1 = exp(gsl_matrix_get(qij,ii,kk1));
	temp2 = exp(gsl_matrix_get(qij,ii,kk2));
	temp += temp1*temp2;
      }
      gsl_matrix_set(Jmerge,kk1,kk2,temp);
    }

  //Then calculate Jsplit
  gsl_vector * Jsplit = gsl_vector_alloc(K);
  gsl_vector * Jsplit_temp = gsl_vector_alloc(K);
  gsl_vector_set_all(Jsplit,-1.);
  //if there is missing data, fill in the missing data
  struct missingdatapoint{
    gsl_vector *ww;
  };
  struct missingdatapoint * missingdata;
  missingdata = (struct missingdatapoint *) malloc(N * sizeof (struct missingdatapoint) );
  for (ii=0; ii != N; ++ii){
    missingdata->ww = gsl_vector_alloc(d);
    ++missingdata;
  }
  missingdata -= N;

  gsl_matrix * tempRR,* tempVV;
  gsl_vector * tempSS,* tempwork;
  gsl_vector * expectedww = gsl_vector_alloc(d);
  //gsl_vector_view tempUcol;
  double lambda;
  int di,signum;
  for (ii=0; ii != N; ++ii){
    //First check whether there is any missing data
    if ((data->ww)->size == d){
      gsl_vector_memcpy(missingdata->ww,data->ww);
      ++missingdata;
      ++data;
      continue;
    }

    /*AS IT STANDS THE MISSING DATA PART IS *NOT* IMPLEMENTED CORRECTLY:
      A CORRECT IMPLEMENTATION NEEDS THE NULL SPACE OF THE PROJECTION
      MATRIX WHICH CAN BE FOUND FROM THE FULL SINGULAR VALUE DECOMPOSITIIN,
      UNFORTUNATELY GSL DOES NOT COMPUTE THE FULL SVD, BUT ONLY THE THIN SVD.
      LAPACK MIGHT DO, BUT MIGHT NOT BE INSTALLED (?) AND THIS MIGHT BE HARD TO IMPLEMENT.

      INDICATED BELOW ARE THE SECTION THAT WOULD HAVE TO BE FIXED TO MAKE THIS WORK
     */

    //calculate expectation, for this we need to calculate the bbijs (EXACTLY THE SAME AS IN PROJ_EM, SHOULD WRITE GENERAL FUNCTION TO DO THIS)
    gsl_vector_set_zero(expectedww);
    for (kk = 0; kk != K; ++kk){
      //prepare...
      di = (data->SS)->size1;
      p = gsl_permutation_alloc (di);
      wminusRm = gsl_vector_alloc (di);
      gsl_vector_memcpy(wminusRm,data->ww);
      TinvwminusRm = gsl_vector_alloc (di);
      Tij = gsl_matrix_alloc(di,di);
      gsl_matrix_memcpy(Tij,data->SS);
      Tij_inv = gsl_matrix_alloc(di,di);
      VRT = gsl_matrix_alloc(d,di);
      VRTTinv = gsl_matrix_alloc(d,di);
      Rtrans = gsl_matrix_alloc(d,di);
      //Calculate Tij
      gsl_matrix_transpose_memcpy(Rtrans,data->RR);
      gsl_blas_dsymm(CblasLeft,CblasUpper,1.0,gaussians->VV,Rtrans,0.0,VRT);//Only the upper right part of VV is calculated
      gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,data->RR,VRT,1.0,Tij);//This is Tij
      //Calculate LU decomp of Tij and Tij inverse
      gsl_linalg_LU_decomp(Tij,p,&signum);
      gsl_linalg_LU_invert(Tij,p,Tij_inv);
      //Calculate Tijinv*(w-Rm)
      gsl_blas_dgemv(CblasNoTrans,-1.0,data->RR,gaussians->mm,1.0,wminusRm);
      gsl_blas_dsymv(CblasUpper,1.0,Tij_inv,wminusRm,0.0,TinvwminusRm);
      //Now calculate bij and Bij
      gsl_vector_memcpy(bs->bbij,gaussians->mm);
      gsl_blas_dgemv(CblasNoTrans,1.0,VRT,TinvwminusRm,1.0,bs->bbij);
      //..and add the result to expectedww
      gsl_vector_scale(bs->bbij,exp(gsl_matrix_get(qij,ii,kk)));
      gsl_vector_add(expectedww,bs->bbij);
      //Clean up
      gsl_permutation_free (p);
      gsl_vector_free(wminusRm);
      gsl_vector_free(TinvwminusRm);
      gsl_matrix_free(Tij);
      gsl_matrix_free(Tij_inv);
      gsl_matrix_free(VRT);
      gsl_matrix_free(VRTTinv);
      gsl_matrix_free(Rtrans);
      ++gaussians;
    }
    gaussians -= K;
    //if missing, fill in the missing data
    tempRR = gsl_matrix_alloc((data->RR)->size2,(data->RR)->size1);//will hold the transpose of RR
    //tempVV = gsl_matrix_alloc((data->RR)->size1,(data->RR)->size1);
    //tempSS = gsl_vector_alloc((data->RR)->size1);
    //tempwork = gsl_vector_alloc((data->RR)->size1);
    gsl_matrix_transpose_memcpy(tempRR,data->RR);
    gsl_blas_dgemv(CblasNoTrans,1.,tempRR,data->ww,0.,missingdata->ww);
    //gsl_linalg_SV_decomp(tempRR,tempVV,tempSS,tempwork);
    //compute the missing data THIS PART IS NOT IMPLEMENTED CORRECTLY
    //for (kk = 0; kk != d-(data->ww)->size; ++kk){
      //tempUcol = gsl_matrix_column(tempRR,d-1-kk);
    //gsl_blas_ddot(&(tempUcol.vector),expectedww,&lambda);
    //gsl_vector_scale(&(tempUcol.vector),lambda);
    //gsl_vector_add(missingdata->ww,&(tempUcol.vector));
    //}
    ++missingdata;
    ++data;
    //free
    gsl_matrix_free(tempRR);
    //gsl_matrix_free(tempVV);
    //gsl_vector_free(tempSS);
    //gsl_vector_free(tempwork);
  }
  data -= N;
  missingdata -= N;

  //then for every gaussian, calculate the KL divergence between the local data density and the l-th gaussian
  double tempsplit;
  p = gsl_permutation_alloc (d);
  tempVV= gsl_matrix_alloc(d,d);
  tempRR= gsl_matrix_alloc(d,d);
  tempSS= gsl_vector_alloc(d);
  tempwork= gsl_vector_alloc(d);
  for (kk = 0; kk != K; ++kk){
    //calculate qil/ql factors
    normalize_row(tempqij,kk,false,true,0.);
    //calculate inverse of V and det(V)
    gsl_matrix_memcpy(tempVV,gaussians->VV);
    gsl_linalg_LU_decomp(tempVV,p,&signum);
    gsl_linalg_LU_invert(tempVV,p,tempRR);//tempRR now has the inverse of VV
    tempsplit = d * halflogtwopi + 0.5 * gsl_linalg_LU_lndet(tempVV);
    for (ii=0; ii != N; ++ii){
      if (exp(gsl_matrix_get(tempqij,ii,kk)) == 0.){
	++missingdata;
	continue;
      }
      tempsplit += gsl_matrix_get(tempqij,ii,kk) * exp(gsl_matrix_get(tempqij,ii,kk));
      gsl_vector_memcpy(tempSS,gaussians->mm);
      gsl_vector_scale(tempSS,-1.);
      gsl_vector_add(tempSS,missingdata->ww);
      gsl_blas_dgemv(CblasNoTrans,1.0,tempRR,tempSS,0.,tempwork);
      gsl_blas_ddot(tempSS,tempwork,&lambda);
      tempsplit += 0.5 * exp(gsl_matrix_get(tempqij,ii,kk)) * lambda;
      ++missingdata;
    }
    gsl_vector_set(Jsplit,kk,tempsplit);
    //printf("Jsplit for gaussian %i = %f\n",kk,tempsplit);
    missingdata -= N;
    ++gaussians;
  }
  gaussians -= K;

  //free
  gsl_permutation_free(p);
  gsl_matrix_free(tempRR);
  gsl_matrix_free(tempVV);
  gsl_vector_free(tempSS);
  gsl_vector_free(tempwork);

  //and put everything in the hierarchy
  size_t maxj, maxk, maxl;
  for (kk1 = 0; kk1 != maxsnm; kk1 += (K-2)){
    gsl_matrix_max_index(Jmerge,&maxj,&maxk);
    gsl_vector_memcpy(Jsplit_temp,Jsplit);
    gsl_vector_set(Jsplit_temp,maxj,-1.);
    gsl_vector_set(Jsplit_temp,maxk,-1.);
    for (kk2=0; kk2 != K-2; ++kk2){
      maxl = gsl_vector_max_index(Jsplit_temp);
      gsl_vector_set(Jsplit_temp,maxl,-1.);
      *(snmhierarchy++)= maxj;
      *(snmhierarchy++)= maxk;
      *(snmhierarchy++)= maxl;
      //printf("j = %i, k = %i, l = %i\n",(int)maxj,(int)maxk,(int)maxl);
    }
    //then set it to zero and find the next
    gsl_matrix_set(Jmerge,maxj,maxk,-1.);
  }
  snmhierarchy -= 3*maxsnm;

  //clean up
  gsl_matrix_free(Jmerge);
  gsl_vector_free(Jsplit);
  gsl_vector_free(Jsplit_temp);

  return ;
}


/**
 * \author Sylvain Marsat, University of Maryland - NASA GSFC
 *
 * \brief C code for the implementation of the Fourier domain response for LIGO-VIRGO detectors.
 *
 */

#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 "LLVgeometry.h"
#include "struct.h"
#include "LLVFDresponse.h"
#include "timeconversion.h"

/************************************************************/
/********* Functions setting detector geometry **************/

/* Function setting the response matrix of a given detector, in cartesian coordinates */
void SetMatrixD(
  gsl_matrix* D,                       /* Output: matrix of the detector response Dij */
  const Detectortag tag)               /* Tag identifying the detector */
{
  /* Allocating and defining the cartesian unit vectors along the two arms */
  gsl_vector* nx = gsl_vector_alloc(3);
  gsl_vector* ny = gsl_vector_alloc(3);
  if(tag==LHO) {
    gsl_vector_set(nx, 0, LAL_LHO_4K_ARM_X_DIRECTION_X);
    gsl_vector_set(nx, 1, LAL_LHO_4K_ARM_X_DIRECTION_Y);
    gsl_vector_set(nx, 2, LAL_LHO_4K_ARM_X_DIRECTION_Z);
    gsl_vector_set(ny, 0, LAL_LHO_4K_ARM_Y_DIRECTION_X);
    gsl_vector_set(ny, 1, LAL_LHO_4K_ARM_Y_DIRECTION_Y);
    gsl_vector_set(ny, 2, LAL_LHO_4K_ARM_Y_DIRECTION_Z);
  }
  else if(tag==LLO) {
    gsl_vector_set(nx, 0, LAL_LLO_4K_ARM_X_DIRECTION_X);
    gsl_vector_set(nx, 1, LAL_LLO_4K_ARM_X_DIRECTION_Y);
    gsl_vector_set(nx, 2, LAL_LLO_4K_ARM_X_DIRECTION_Z);
    gsl_vector_set(ny, 0, LAL_LLO_4K_ARM_Y_DIRECTION_X);
    gsl_vector_set(ny, 1, LAL_LLO_4K_ARM_Y_DIRECTION_Y);
    gsl_vector_set(ny, 2, LAL_LLO_4K_ARM_Y_DIRECTION_Z);
  }
  else if(tag==VIRGO) {
    gsl_vector_set(nx, 0, LAL_VIRGO_ARM_X_DIRECTION_X);
    gsl_vector_set(nx, 1, LAL_VIRGO_ARM_X_DIRECTION_Y);
    gsl_vector_set(nx, 2, LAL_VIRGO_ARM_X_DIRECTION_Z);
    gsl_vector_set(ny, 0, LAL_VIRGO_ARM_Y_DIRECTION_X);
    gsl_vector_set(ny, 1, LAL_VIRGO_ARM_Y_DIRECTION_Y);
    gsl_vector_set(ny, 2, LAL_VIRGO_ARM_Y_DIRECTION_Z);
  }
  else {
    printf("Error: invalid detector tag\n");
    exit(1);
  }

  /* Setting the components of the matrix D = 1/2 (nx nx - ny ny) */
  for(int i=0; i<3; i++) {
    for(int j=0; j<3; j++) {
      gsl_matrix_set(D, i, j, 1./2*(gsl_vector_get(nx, i)*gsl_vector_get(nx, j) - gsl_vector_get(ny, i)*gsl_vector_get(ny, j)));
    }
  }
  gsl_vector_free(nx);
  gsl_vector_free(ny);
}

/* Function setting the position of a detector, in cartesian coordinates */
void SetVectorXd(
  gsl_vector* Xd,                      /* Output: position vector of the detector */
  const Detectortag tag)               /* Tag identifying the detector */
{
  if(tag==LHO) {
    gsl_vector_set(Xd, 0, LAL_LHO_4K_VERTEX_LOCATION_X_SI);
    gsl_vector_set(Xd, 1, LAL_LHO_4K_VERTEX_LOCATION_Y_SI);
    gsl_vector_set(Xd, 2, LAL_LHO_4K_VERTEX_LOCATION_Z_SI);
  }
  else if(tag==LLO) {
    gsl_vector_set(Xd, 0, LAL_LLO_4K_VERTEX_LOCATION_X_SI);
    gsl_vector_set(Xd, 1, LAL_LLO_4K_VERTEX_LOCATION_Y_SI);
    gsl_vector_set(Xd, 2, LAL_LLO_4K_VERTEX_LOCATION_Z_SI);
  }
  else if(tag==VIRGO) {
    gsl_vector_set(Xd, 0, LAL_VIRGO_VERTEX_LOCATION_X_SI);
    gsl_vector_set(Xd, 1, LAL_VIRGO_VERTEX_LOCATION_Y_SI);
    gsl_vector_set(Xd, 2, LAL_VIRGO_VERTEX_LOCATION_Z_SI);
  }
  else {
    printf("Error: invalid detector tag\n");
    exit(1);
  }
}

/* Function setting the cartesian coordinates of the wave frame vectors (X,Y,Z), given the position in the sky and polarization */
void SetVectorsXYZ(
  gsl_vector* X,                       /* Output: cartesian vector of the wave frame unit vector X */
  gsl_vector* Y,                       /* Output: cartesian vector of the wave frame unit vector Y */
  gsl_vector* Z,                       /* Output: cartesian vector of the wave frame unit vector Z */
  const double theta,                  /* First angle for the position in the sky (Earth-based spherical angle) */
  const double phi,                    /* Second angle for the position in the sky (Earth-based spherical angle) */
  const double psi)                    /* Polarization angle */
{
  double cospsi = cos(psi);
  double sinpsi = sin(psi);
  double costheta = cos(theta);
  double sintheta = sin(theta);
  double cosphi = cos(phi);
  double sinphi = sin(phi);
  /* Unit vector X */
  gsl_vector_set(X, 0, -cospsi*sinphi+sinpsi*costheta*cosphi);
  gsl_vector_set(X, 1, cospsi*cosphi+sinpsi*costheta*sinphi);
  gsl_vector_set(X, 2, -sinpsi*sintheta);
  /* Unit vector Y */
  gsl_vector_set(Y, 0, sinpsi*sinphi+cospsi*costheta*cosphi);
  gsl_vector_set(Y, 1, -sinpsi*cosphi+cospsi*costheta*sinphi);
  gsl_vector_set(Y, 2, -cospsi*sintheta);
  /* Unit vector Z */
  gsl_vector_set(Z, 0, -sintheta*cosphi);
  gsl_vector_set(Z, 1, -sintheta*sinphi);
  gsl_vector_set(Z, 2, -costheta);
}

/***************************************/
/********* Core functions **************/

/* Function to convert string input network string to Networktag */
Networktag ParseNetworktag(char* string) {
  Networktag tag;
  if(strcmp(string, "L")==0) tag = L;
  else if(strcmp(string, "H")==0) tag = H;
  else if(strcmp(string, "V")==0) tag = V;
  else if(strcmp(string, "LH")==0) tag = LH;
  else if(strcmp(string, "LV")==0) tag = LV;
  else if(strcmp(string, "HV")==0) tag = HV;
  else if(strcmp(string, "LHV")==0) tag = LHV;
  else {
    printf("Error in ParseNetworktag: string not recognized.\n");
    exit(1);
  }
  return tag;
}

/* Function evaluating the Fourier-domain factors for LHV detectors */
/* Note: in case only one or two detectors are considered, amplitudes for the missing ones are set to 0 */
/* Contrarily to the LISA case, here returns trivially 0 or 1 */
/* (allows minimal changes from the old structure that assumed LHV - but not optimal) */
static int EvaluateDetectorFactor3Det(
  double* factor1,                       /* Output for factor for detector 1 */
  double* factor2,                       /* Output for factor for detector 2 */
  double* factor3,                       /* Output for factor for detector 3 */
  const Networktag networktag)                  /* Selector for the detector network */
{
  switch(networktag) {
    case L:
    *factor1 = 1.;
    *factor2 = 0;
    *factor3 = 0;
    break;
    case H:
    *factor1 = 0;
    *factor2 = 1.;
    *factor3 = 0;
    break;
    case V:
    *factor1 = 0;
    *factor2 = 0;
    *factor3 = 1.;
    break;
    case LH:
    *factor1 = 1.;
    *factor2 = 1.;
    *factor3 = 0;
    break;
    case LV:
    *factor1 = 1.;
    *factor2 = 0;
    *factor3 = 1.;
    break;
    case HV:
    *factor1 = 0;
    *factor2 = 1.;
    *factor3 = 1.;
    break;
    case LHV:
    *factor1 = 1.;
    *factor2 = 1.;
    *factor3 = 1.;
    break;
    default:
    printf("Error in EvaluateDetectorFactor3Det: networktag not recognized.\n");
    exit(1);
  }
  return SUCCESS;
}

/* Core function processing a signal (in the form of a list of modes) through the Fourier-domain LLV response (for a given detector), for given values of the inclination, position in the sky and polarization angle */
int LLVSimFDResponse(
  struct tagListmodesCAmpPhaseFrequencySeries **listhlm,  /* Input: list of modes in Frequency-domain amplitude and phase form as produced by the ROM */
  struct tagListmodesCAmpPhaseFrequencySeries **lists,    /* Output: list of contribution of each mode in the detector signal, in Frequency-domain amplitude and phase form, for the given detector and sky position */
  const double gpstime,                                   /* GPS time (s) when the signal at coalescence reaches geocenter */
  const double ra,                                        /* Position in the sky: J2000.0 right ascension (rad) */
  const double dec,                                       /* Position in the sky: J2000.0 declination (rad) */
  const double inclination,                               /* Inclination of the source (rad) */
  const double psi,                                       /* Polarization angle (rad) */
  const Detectortag tag)                                  /* Tag identifying the detector */
{
  /* Define matrix D and position vector Xd of the detector */
  gsl_matrix* D = gsl_matrix_alloc(3,3);
  gsl_vector* Xd = gsl_vector_alloc(3);
  SetVectorXd(Xd, tag);
  SetMatrixD(D, tag);

  /* Conversion from (ra, dec) to the Earth-based spherical angles (theta, phi) - neglecting nutation and precession, and identifying UT1 and UTC, so accurate roughly to a second of time */
  double gmst_angle = gmst_angle_from_gpstime(gpstime);
  double theta = PI/2 - dec;
  double phi = ra - gmst_angle;

  /* Define waveframe unit vectors (X,Y,Z) */
  gsl_vector* X = gsl_vector_alloc(3);
  gsl_vector* Y = gsl_vector_alloc(3);
  gsl_vector* Z = gsl_vector_alloc(3);
  SetVectorsXYZ(X, Y, Z, theta, phi, psi);

  /* Compute the delay from geocenter to the detector */
  double delaylength;
  gsl_blas_ddot(Xd, Z, &delaylength);
  double twopidelay = 2*PI*delaylength/C_SI;

  /* Compute the value of pattern functions Fplus, Fcross */
  gsl_vector* DX = gsl_vector_calloc(3); /* Temporary vector D.X, initialized to 0 */
  gsl_vector* DY = gsl_vector_calloc(3); /* Temporary vector D.Y, initialized to 0 */
  gsl_blas_dgemv( CblasNoTrans, 1., D, X, 0, DX);
  gsl_blas_dgemv( CblasNoTrans, 1., D, Y, 0, DY);
  double XDX;
  double XDY;
  double YDY;
  gsl_blas_ddot(X, DX, &XDX);
  gsl_blas_ddot(X, DY, &XDY);
  gsl_blas_ddot(Y, DY, &YDY);
  double Fplus = XDX - YDY;
  double Fcross = 2*XDY;

  /* Main loop over the modes - goes through all the modes present, stopping when encountering NULL */
  ListmodesCAmpPhaseFrequencySeries* listelement = *listhlm;
  while(listelement) {

    /* Definitions: l,m, frequency series and length */
    int l = listelement->l;
    int m = listelement->m;
    CAmpPhaseFrequencySeries* freqseries = listelement->freqseries;
    gsl_vector* freq = freqseries->freq;
    gsl_vector* amp_real = freqseries->amp_real;
    gsl_vector* amp_imag = freqseries->amp_imag;
    gsl_vector* phase = freqseries->phase;
    int len = (int) freq->size;
    double f;
    double complex camp;
    double complex camps;

    /* Computing the Ylm combined factors for plus and cross for this mode */
    /* Capital Phi is set to 0 by convention */
    double complex Yfactorplus;
    double complex Yfactorcross;
    if (!(l%2)) {
      Yfactorplus = 1./2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) + conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
      Yfactorcross = I/2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) - conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
    }
    else {
      Yfactorplus = 1./2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) - conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
      Yfactorcross = I/2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) + conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
    }

    /* Initializing frequency series structure for this mode, for the signal s = F+ h+ + Fx hx */
    CAmpPhaseFrequencySeries *modefreqseriess = NULL;
    CAmpPhaseFrequencySeries_Init(&modefreqseriess, len);
    gsl_vector* freqs = modefreqseriess->freq;
    gsl_vector* amp_reals = modefreqseriess->amp_real;
    gsl_vector* amp_imags = modefreqseriess->amp_imag;
    gsl_vector* phases = modefreqseriess->phase;

    /* Loop over the frequencies - multiplying by F+, Fx, and add phase due to the delay from geocenter to the detector */
    double complex factorcamp = Fplus*Yfactorplus + Fcross*Yfactorcross;
    for(int j=0; j<len; j++) {
      f = gsl_vector_get(freq, j);
      camp = gsl_vector_get(amp_real, j) + I*gsl_vector_get(amp_imag, j);
      camps = camp * factorcamp;
      gsl_vector_set(amp_reals, j, creal(camps));
      gsl_vector_set(amp_imags, j, cimag(camps));
      gsl_vector_set(phases, j, gsl_vector_get(phase, j) + twopidelay*f);
    }
    /* Copying the vectors of frequencies */
    gsl_vector_memcpy(freqs, freq);

    /* Append the modes to the ouput list-of-modes structures */
    *lists = ListmodesCAmpPhaseFrequencySeries_AddModeNoCopy(*lists, modefreqseriess, l, m);

    /* Going to the next mode in the list */
    listelement = listelement->next;
  }

  /* Cleaning */
  gsl_matrix_free(D);
  gsl_vector_free(Xd);
  gsl_vector_free(X);
  gsl_vector_free(Y);
  gsl_vector_free(Z);
  gsl_vector_free(DX);
  gsl_vector_free(DY);

  return SUCCESS;
}

/* Core function processing a signal (in the form of a list of modes) through the Fourier-domain LLV response for a given detector network, for given values of the inclination, position in the sky and polarization angle */
/* Note: as for now, asssumes the three detectors are L,H,V - amplitudes simply set to 0 in those that are not selected by networktag */
int LLVSimFDResponse3Det(
  struct tagListmodesCAmpPhaseFrequencySeries **list1,    /* Output: list of contributions of each mode in the signal of detector 1, in Frequency-domain amplitude and phase form */
  struct tagListmodesCAmpPhaseFrequencySeries **list2,    /* Output: list of contributions of each mode in the signal of detector 1, in Frequency-domain amplitude and phase form */
  struct tagListmodesCAmpPhaseFrequencySeries **list3,    /* Output: list of contributions of each mode in the signal of detector 1, in Frequency-domain amplitude and phase form */
  struct tagListmodesCAmpPhaseFrequencySeries **listhlm,  /* Input: list of modes in Frequency-domain amplitude and phase form as produced by the ROM */
  const double gpstime,                                   /* GPS time (s) when the signal at coalescence reaches geocenter */
  const double ra,                                        /* Position in the sky: J2000.0 right ascension (rad) */
  const double dec,                                       /* Position in the sky: J2000.0 declination (rad) */
  const double inclination,                               /* Inclination of the source (rad) */
  const double psi,                                       /* Polarization angle (rad) */
  const Networktag tag)                                   /* Tag identifying the network to use */
{
  /* Read which detectors are to be included in the network */
  double factor1 = 0;
  double factor2 = 0;
  double factor3 = 0;
  EvaluateDetectorFactor3Det(&factor1, &factor2, &factor3, tag);

  /* Define matrix D and position vector Xd for the 3 detectors */
  gsl_matrix* D1 = gsl_matrix_alloc(3,3);
  gsl_matrix* D2 = gsl_matrix_alloc(3,3);
  gsl_matrix* D3 = gsl_matrix_alloc(3,3);
  gsl_vector* Xd1 = gsl_vector_alloc(3);
  gsl_vector* Xd2 = gsl_vector_alloc(3);
  gsl_vector* Xd3 = gsl_vector_alloc(3);
  SetVectorXd(Xd1, LHO);
  SetVectorXd(Xd2, LLO);
  SetVectorXd(Xd3, VIRGO);
  SetMatrixD(D1, LHO);
  SetMatrixD(D2, LLO);
  SetMatrixD(D3, VIRGO);

  /* Conversion from (ra, dec) to the Earth-based spherical angles (theta, phi) - neglecting nutation and precession, and identifying UT1 and UTC, so accurate roughly to a second of time */
  double gmst_angle = gmst_angle_from_gpstime(gpstime);
  double theta = PI/2 - dec;
  double phi = ra - gmst_angle;

  /* Define waveframe unit vectors (X,Y,Z) */
  gsl_vector* X = gsl_vector_alloc(3);
  gsl_vector* Y = gsl_vector_alloc(3);
  gsl_vector* Z = gsl_vector_alloc(3);
  SetVectorsXYZ(X, Y, Z, theta, phi, psi);

  /* Compute the delays from geocenter to each detector */
  double delaylength1 = 0;
  double delaylength2 = 0;
  double delaylength3 = 0;
  gsl_blas_ddot(Xd1, Z, &delaylength1);
  gsl_blas_ddot(Xd2, Z, &delaylength2);
  gsl_blas_ddot(Xd3, Z, &delaylength3);
  double twopidelay1 = 2*PI*delaylength1/C_SI;
  double twopidelay2 = 2*PI*delaylength2/C_SI;
  double twopidelay3 = 2*PI*delaylength3/C_SI;

  /* Compute the value of pattern functions Fplus, Fcross for each detector */
  gsl_vector* DX1 = gsl_vector_calloc(3); /* Temporary vector D.X, initialized to 0 */
  gsl_vector* DY1 = gsl_vector_calloc(3); /* Temporary vector D.Y, initialized to 0 */
  gsl_vector* DX2 = gsl_vector_calloc(3); /* Temporary vector D.X, initialized to 0 */
  gsl_vector* DY2 = gsl_vector_calloc(3); /* Temporary vector D.Y, initialized to 0 */
  gsl_vector* DX3 = gsl_vector_calloc(3); /* Temporary vector D.X, initialized to 0 */
  gsl_vector* DY3 = gsl_vector_calloc(3); /* Temporary vector D.Y, initialized to 0 */
  gsl_blas_dgemv( CblasNoTrans, 1., D1, X, 0, DX1);
  gsl_blas_dgemv( CblasNoTrans, 1., D1, Y, 0, DY1);
  gsl_blas_dgemv( CblasNoTrans, 1., D2, X, 0, DX2);
  gsl_blas_dgemv( CblasNoTrans, 1., D2, Y, 0, DY2);
  gsl_blas_dgemv( CblasNoTrans, 1., D3, X, 0, DX3);
  gsl_blas_dgemv( CblasNoTrans, 1., D3, Y, 0, DY3);
  double XDX1 = 0.;
  double XDY1 = 0.;
  double YDY1 = 0.;
  double XDX2 = 0.;
  double XDY2 = 0.;
  double YDY2 = 0.;
  double XDX3 = 0.;
  double XDY3 = 0.;
  double YDY3 = 0.;
  gsl_blas_ddot(X, DX1, &XDX1);
  gsl_blas_ddot(X, DY1, &XDY1);
  gsl_blas_ddot(Y, DY1, &YDY1);
  gsl_blas_ddot(X, DX2, &XDX2);
  gsl_blas_ddot(X, DY2, &XDY2);
  gsl_blas_ddot(Y, DY2, &YDY2);
  gsl_blas_ddot(X, DX3, &XDX3);
  gsl_blas_ddot(X, DY3, &XDY3);
  gsl_blas_ddot(Y, DY3, &YDY3);
  double Fplus1 = XDX1 - YDY1;
  double Fcross1 = 2*XDY1;
  double Fplus2 = XDX2 - YDY2;
  double Fcross2 = 2*XDY2;
  double Fplus3 = XDX3 - YDY3;
  double Fcross3 = 2*XDY3;

  /* Main loop over the modes - goes through all the modes present, stopping when encountering NULL */
  ListmodesCAmpPhaseFrequencySeries* listelement = *listhlm;
  while(listelement) {

    /* Definitions: l,m, frequency series and length */
    int l = listelement->l;
    int m = listelement->m;
    CAmpPhaseFrequencySeries* freqseries = listelement->freqseries;
    gsl_vector* freq = freqseries->freq;
    gsl_vector* amp_real = freqseries->amp_real;
    gsl_vector* amp_imag = freqseries->amp_imag;
    gsl_vector* phase = freqseries->phase;
    int len = (int) freq->size;
    double f;
    double complex camp;
    double complex camp1;
    double complex camp2;
    double complex camp3;

    /* Computing the Ylm combined factors for plus and cross for this mode */
    /* Capital Phi is set to 0 by convention */
    double complex Yfactorplus;
    double complex Yfactorcross;
    if (!(l%2)) {
      Yfactorplus = 1./2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) + conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
      Yfactorcross = I/2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) - conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
    }
    else {
      Yfactorplus = 1./2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) - conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
      Yfactorcross = I/2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) + conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
    }

    /* Initializing frequency series structure for this mode, for the signal s = F+ h+ + Fx hx in each detector */
    CAmpPhaseFrequencySeries *modefreqseries1 = NULL;
    CAmpPhaseFrequencySeries *modefreqseries2 = NULL;
    CAmpPhaseFrequencySeries *modefreqseries3 = NULL;
    CAmpPhaseFrequencySeries_Init(&modefreqseries1, len);
    CAmpPhaseFrequencySeries_Init(&modefreqseries2, len);
    CAmpPhaseFrequencySeries_Init(&modefreqseries3, len);
    gsl_vector* freq1 = modefreqseries1->freq;
    gsl_vector* freq2 = modefreqseries2->freq;
    gsl_vector* freq3 = modefreqseries3->freq;
    gsl_vector* amp_real1 = modefreqseries1->amp_real;
    gsl_vector* amp_real2 = modefreqseries2->amp_real;
    gsl_vector* amp_real3 = modefreqseries3->amp_real;
    gsl_vector* amp_imag1 = modefreqseries1->amp_imag;
    gsl_vector* amp_imag2 = modefreqseries2->amp_imag;
    gsl_vector* amp_imag3 = modefreqseries3->amp_imag;
    gsl_vector* phase1 = modefreqseries1->phase;
    gsl_vector* phase2 = modefreqseries2->phase;
    gsl_vector* phase3 = modefreqseries3->phase;

    /* Loop over the frequencies - multiplying by F+, Fx, and add phase due to the delay from geocenter to the detector */
    double complex factorcamp1 = Fplus1*Yfactorplus + Fcross1*Yfactorcross;
    double complex factorcamp2 = Fplus2*Yfactorplus + Fcross2*Yfactorcross;
    double complex factorcamp3 = Fplus3*Yfactorplus + Fcross3*Yfactorcross;
    for(int j=0; j<len; j++) {
      f = gsl_vector_get(freq, j);
      camp = gsl_vector_get(amp_real, j) + I*gsl_vector_get(amp_imag, j);
      /* Note: include factor123 as detector selectors, which are simpy 0 or 1 whether the detector is in the array or not */
      camp1 = factor1 * camp * factorcamp1;
      camp2 = factor2 * camp * factorcamp2;
      camp3 = factor3 * camp * factorcamp3;
      gsl_vector_set(amp_real1, j, creal(camp1));
      gsl_vector_set(amp_real2, j, creal(camp2));
      gsl_vector_set(amp_real3, j, creal(camp3));
      gsl_vector_set(amp_imag1, j, cimag(camp1));
      gsl_vector_set(amp_imag2, j, cimag(camp2));
      gsl_vector_set(amp_imag3, j, cimag(camp3));
      gsl_vector_set(phase1, j, gsl_vector_get(phase, j) + twopidelay1*f);
      gsl_vector_set(phase2, j, gsl_vector_get(phase, j) + twopidelay2*f);
      gsl_vector_set(phase3, j, gsl_vector_get(phase, j) + twopidelay3*f);
    }
    /* Copying the vectors of frequencies */
    gsl_vector_memcpy(freq1, freq);
    gsl_vector_memcpy(freq2, freq);
    gsl_vector_memcpy(freq3, freq);

    /* Append the modes to the ouput list-of-modes structures */
    *list1 = ListmodesCAmpPhaseFrequencySeries_AddModeNoCopy(*list1, modefreqseries1, l, m);
    *list2 = ListmodesCAmpPhaseFrequencySeries_AddModeNoCopy(*list2, modefreqseries2, l, m);
    *list3 = ListmodesCAmpPhaseFrequencySeries_AddModeNoCopy(*list3, modefreqseries3, l, m);

    /* Going to the next mode in the list */
    listelement = listelement->next;
  }

  /* Cleaning */
  gsl_matrix_free(D1);
  gsl_matrix_free(D2);
  gsl_matrix_free(D3);
  gsl_vector_free(Xd1);
  gsl_vector_free(Xd2);
  gsl_vector_free(Xd3);
  gsl_vector_free(X);
  gsl_vector_free(Y);
  gsl_vector_free(Z);
  gsl_vector_free(DX1);
  gsl_vector_free(DX2);
  gsl_vector_free(DX3);
  gsl_vector_free(DY1);
  gsl_vector_free(DY2);
  gsl_vector_free(DY3);

  return SUCCESS;
}


/*! @copyright (c) 2017 King Abdullah University of Science and
 *                      Technology (KAUST). All rights reserved.
 *
 * STARS-H is a software package, provided by King Abdullah
 *             University of Science and Technology (KAUST)
 *
 * @file testing/rndtiled.c
 * @version 1.3.0
 * @author Aleksandr Mikhalev
 * @date 2017-11-07
 * */

#ifdef MKL
    #include <mkl.h>
#else
    #include <cblas.h>
    #include <lapacke.h>
#endif
#include <stdio.h>
#include <stdlib.h>
#include <omp.h>
#include <starsh.h>
#include <starsh-randtlr.h>

int main(int argc, char **argv)
{
    if(argc != 6)
    {
        printf("%d arguments provided, but 5 are needed\n", argc-1);
        printf("randtlr N NB decay maxrank tol\n");
        return 1;
    }
    int N = atoi(argv[1]);
    int block_size = atoi(argv[2]);
    double decay = atof(argv[3]);
    int maxrank = atoi(argv[4]);
    double tol = atof(argv[5]);
    int onfly = 0;
    char symm = 'N', dtype = 'd';
    STARSH_int shape[2] = {N, N};
    int info;
    srand(0);
    // Init STARS-H
    info = starsh_init();
    if(info != 0)
        return info;
    // Generate problem by random matrices
    STARSH_randtlr *data;
    STARSH_kernel *kernel;
    info = starsh_application((void **)&data, &kernel, N, dtype,
            STARSH_RANDTLR, STARSH_RANDTLR_KERNEL1,
            STARSH_RANDTLR_NB, block_size,
            STARSH_RANDTLR_DECAY, decay, STARSH_RANDTLR_DIAG, (double)N, 0);
    if(info != 0)
    {
        printf("Problem was NOT generated (wrong parameters)\n");
        return info;
    }
    // Init problem with given data and kernel and print short info
    STARSH_problem *P;
    info = starsh_problem_new(&P, 2, shape, symm, dtype, data, data, kernel,
            "Randomly generated tiled blr-matrix");
    if(info != 0)
        return info;
    starsh_problem_info(P);
    // Init plain clusterization and print info
    STARSH_cluster *C;
    info = starsh_cluster_new_plain(&C, data, N, block_size);
    if(info != 0)
        return info;
    starsh_cluster_info(C);
    // Init tlr division into admissible blocks and print short info
    STARSH_blrf *F;
    info = starsh_blrf_new_tlr(&F, P, symm, C, C);
    if(info != 0)
        return info;
    starsh_blrf_info(F);
    // Approximate each admissible block
    STARSH_blrm *M;
    double time1 = omp_get_wtime();
    info = starsh_blrm_approximate(&M, F, maxrank, tol, onfly);
    if(info != 0)
        return info;
    time1 = omp_get_wtime()-time1;
    // Print info about updated format and approximation
    starsh_blrf_info(F);
    starsh_blrm_info(M);
    printf("TIME TO APPROXIMATE: %e secs\n", time1);
    // Measure approximation error
    time1 = omp_get_wtime();
    double rel_err = starsh_blrm__dfe_omp(M);
    time1 = omp_get_wtime()-time1;
    printf("TIME TO MEASURE ERROR: %e secs\nRELATIVE ERROR: %e\n",
            time1, rel_err);
    if(rel_err/tol > 10.)
    {
        printf("Resulting relative error is too big\n");
        return 1;
    }
    // Free block low-rank matrix
    starsh_blrm_free(M);
    // Free block low-rank format
    starsh_blrf_free(F);
    // Free clusterization info
    starsh_cluster_free(C);
    // Free STARS_Problem instance
    starsh_problem_free(P);
    // Free randomly generated data
    starsh_randtlr_free(data);
    return 0;
}


/* ----------------------------------------------------------------------------
  JAM_AXI_RMS_WMMT

    Calculates weighted second moment.

    INPUTS
      xp    : projected x' [pc]
      yp    : projected y' [pc]
      nxy   : number of x' and y' values given
      incl  : inclination [radians]
      lum   : projected luminous MGE
      pot   : projected potential MGE
      beta  : velocity anisotropy (1 - vz^2 / vr^2)
      vv    : velocity integral selector (1=xx, 2=yy, 3=zz, 4=xy, 5=xz, 6=yz)

    NOTES
    * Based on janis2_weighted_second_moment_squared IDL code by Michele
      Cappellari.

  Mark den Brok
  Laura L Watkins [lauralwatkins@gmail.com]

  This code is released under a BSD 2-clause license.
  If you use this code for your research, please cite:
  Watkins et al. 2013, MNRAS, 436, 2598
  "Discrete dynamical models of omega Centauri"
  http://adsabs.harvard.edu/abs/2013MNRAS.436.2598W
---------------------------------------------------------------------------- */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>
#include "jam.h"
#include "../mge/mge.h"
#include "../tools/tools.h"

double *jam_axi_rms_wmmt( double *xp, double *yp, int nxy, double incl, \
        struct multigaussexp *lum, struct multigaussexp *pot, \
        double *beta, int vv , int *gslFlag_rms) {

    struct params_rmsint p;
    struct multigaussexp ilum, ipot;
    double ci, si, *kani, *s2l, *q2l, *s2q2l, *s2p, *e2p;
    double result, error, *sb_mu2;
    int i;
    int status = 0;

    // convert from projected MGEs to intrinsic MGEs
    ilum = mge_deproject( lum, incl );
    ipot = mge_deproject( pot, incl );

    // angles
    ci = cos( incl );
    si = sin( incl );

    // mge component combinations

    kani = (double *) malloc ( lum->ntotal * sizeof( double ) );
    s2l = (double *) malloc( lum->ntotal * sizeof( double ) );
    q2l = (double *) malloc( lum->ntotal * sizeof( double ) );
    s2q2l = (double *) malloc( lum->ntotal * sizeof( double ) );
    s2p = (double *) malloc( pot->ntotal * sizeof( double ) );
    e2p = (double *) malloc( pot->ntotal * sizeof( double ) );

    for ( i = 0; i < ilum.ntotal; i++ ) {
        kani[i] = 1. / ( 1. - beta[i] );
        s2l[i] = pow( ilum.sigma[i], 2. );
        q2l[i] = pow( ilum.q[i], 2. );
        s2q2l[i] = s2l[i] * q2l[i];
    }

    for ( i = 0; i < ipot.ntotal; i++ ) {
        s2p[i] = pow( ipot.sigma[i], 2. );
        e2p[i] = 1. - pow( ipot.q[i], 2. );
    }

    // parameters for the integrand function
    p.ci2 = ci * ci;
    p.si2 = si * si;
    p.cisi = ci * si;
    p.lum = &ilum;
    p.pot = &ipot;
    p.kani = kani;
    p.s2l = s2l;
    p.q2l = q2l;
    p.s2q2l = s2q2l;
    p.s2p = s2p;
    p.e2p = e2p;
    p.vv = vv;

    // perform integration

    gsl_integration_workspace *w = gsl_integration_workspace_alloc( 1000 );
    gsl_function F;
    gsl_set_error_handler_off();
    F.function = &jam_axi_rms_mgeint;

    sb_mu2 = (double *) malloc( nxy * sizeof( double ) );
    for ( i = 0; i < nxy; i++ ) {
        p.x2 = xp[i] * xp[i];
        p.y2 = yp[i] * yp[i];
        p.xy = xp[i] * yp[i];
        F.params = &p;
        status += gsl_integration_qag( &F, 0., 1., 0., 1e-5, 1000, 6, w, \
            &result, &error );
        sb_mu2[i] = result;
    }

    /*// if you want to test the gsl flagging thing uncomment these two lines
    int test = 1;
    status += test;
    */
    if (status > 0) { *gslFlag_rms += 1;   }

    gsl_integration_workspace_free( w );

    free( kani );
    free( s2l );
    free( q2l );
    free( s2q2l );
    free( s2p );
    free( e2p );

    return sb_mu2;

}


/* multifit/multiwlinear.c
 *
 * Copyright (C) 2015 Patrick Alken
 *
 * 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 <gsl/gsl_errno.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>

#include "linear_common.c"

/* General weighted case */

static int
multifit_wlinear_svd (const gsl_matrix * X,
                      const gsl_vector * w,
                      const gsl_vector * y,
                      double tol,
                      int balance,
                      size_t * rank,
                      gsl_vector * c,
                      gsl_matrix * cov,
                      double *chisq, gsl_multifit_linear_workspace * work)
{
  if (X->size1 != y->size)
    {
      GSL_ERROR
        ("number of observations in y does not match rows of matrix X",
         GSL_EBADLEN);
    }
  else if (X->size2 != c->size)
    {
      GSL_ERROR ("number of parameters c does not match columns of matrix X",
                 GSL_EBADLEN);
    }
  else if (w->size != y->size)
    {
      GSL_ERROR ("number of weights does not match number of observations",
                 GSL_EBADLEN);
    }
  else if (cov->size1 != cov->size2)
    {
      GSL_ERROR ("covariance matrix is not square", GSL_ENOTSQR);
    }
  else if (c->size != cov->size1)
    {
      GSL_ERROR
        ("number of parameters does not match size of covariance matrix",
         GSL_EBADLEN);
    }
  else if (X->size1 != work->n || X->size2 != work->p)
    {
      GSL_ERROR
        ("size of workspace does not match size of observation matrix",
         GSL_EBADLEN);
    }
  else
    {
      const size_t n = X->size1;
      const size_t p = X->size2;

      size_t i, j, p_eff;

      gsl_matrix *A = work->A;
      gsl_matrix *Q = work->Q;
      gsl_matrix *QSI = work->QSI;
      gsl_vector *S = work->S;
      gsl_vector *t = work->t;
      gsl_vector *xt = work->xt;
      gsl_vector *D = work->D;

      /* Scale X,  A = sqrt(w) X */

      gsl_matrix_memcpy (A, X);

      for (i = 0; i < n; i++)
        {
          double wi = gsl_vector_get (w, i);

          if (wi < 0)
            wi = 0;

          {
            gsl_vector_view row = gsl_matrix_row (A, i);
            gsl_vector_scale (&row.vector, sqrt (wi));
          }
        }

      /* Balance the columns of the matrix A if requested */

      if (balance)
        {
          gsl_linalg_balance_columns (A, D);
        }
      else
        {
          gsl_vector_set_all (D, 1.0);
        }

      /* Decompose A into U S Q^T */

      gsl_linalg_SV_decomp_mod (A, QSI, Q, S, xt);

      /* Solve sqrt(w) y = A c for c, by first computing t = sqrt(w) y */

      for (i = 0; i < n; i++)
        {
          double wi = gsl_vector_get (w, i);
          double yi = gsl_vector_get (y, i);
          if (wi < 0)
            wi = 0;
          gsl_vector_set (t, i, sqrt (wi) * yi);
        }

      gsl_blas_dgemv (CblasTrans, 1.0, A, t, 0.0, xt);

      /* Scale the matrix Q,  Q' = Q S^-1 */

      gsl_matrix_memcpy (QSI, Q);

      {
        double s0 = gsl_vector_get (S, 0);
        p_eff = 0;

        for (j = 0; j < p; j++)
          {
            gsl_vector_view column = gsl_matrix_column (QSI, j);
            double sj = gsl_vector_get (S, j);
            double alpha;

            if (sj <= tol * s0)
              {
                alpha = 0.0;
              }
            else
              {
                alpha = 1.0 / sj;
                p_eff++;
              }

            gsl_vector_scale (&column.vector, alpha);
          }

        *rank = p_eff;
      }

      gsl_vector_set_zero (c);

      /* solution */
      gsl_blas_dgemv (CblasNoTrans, 1.0, QSI, xt, 0.0, c);

      /* unscale the balancing factors */
      gsl_vector_div (c, D);

      /* compute chisq, from residual r = y - X c */
      {
        double r2 = 0;

        for (i = 0; i < n; i++)
          {
            double yi = gsl_vector_get (y, i);
            double wi = gsl_vector_get (w, i);
            gsl_vector_const_view row = gsl_matrix_const_row (X, i);
            double y_est, ri;
            gsl_blas_ddot (&row.vector, c, &y_est);
            ri = yi - y_est;
            r2 += wi * ri * ri;
          }

        *chisq = r2;

        /* Form covariance matrix cov = (X^T W X)^-1 = (Q S^-1) (Q S^-1)^T */

        for (i = 0; i < p; i++)
          {
            gsl_vector_view row_i = gsl_matrix_row (QSI, i);
            double d_i = gsl_vector_get (D, i);

            for (j = i; j < p; j++)
              {
                gsl_vector_view row_j = gsl_matrix_row (QSI, j);
                double d_j = gsl_vector_get (D, j);
                double s;

                gsl_blas_ddot (&row_i.vector, &row_j.vector, &s);

                gsl_matrix_set (cov, i, j, s / (d_i * d_j));
                gsl_matrix_set (cov, j, i, s / (d_i * d_j));
              }
          }
      }

      return GSL_SUCCESS;
    }
}

int
gsl_multifit_wlinear (const gsl_matrix * X,
                      const gsl_vector * w,
                      const gsl_vector * y,
                      gsl_vector * c,
                      gsl_matrix * cov,
                      double *chisq, gsl_multifit_linear_workspace * work)
{
  size_t rank;
  int status = gsl_multifit_wlinear_tsvd(X, w, y, GSL_DBL_EPSILON, c, cov, chisq, &rank, work);

  return status;
}

int
gsl_multifit_wlinear_tsvd (const gsl_matrix * X,
                           const gsl_vector * w,
                           const gsl_vector * y,
                           const double tol,
                           gsl_vector * c,
                           gsl_matrix * cov,
                           double * chisq,
                           size_t * rank,
                           gsl_multifit_linear_workspace * work)
{
  const size_t n = X->size1;
  const size_t p = X->size2;

  if (y->size != n)
    {
      GSL_ERROR("number of observations in y does not match matrix",
                GSL_EBADLEN);
    }
  else if (w->size != n)
    {
      GSL_ERROR("number of weights in w does not match matrix",
                GSL_EBADLEN);
    }
  else if (p != c->size)
    {
      GSL_ERROR ("number of parameters c does not match matrix",
                 GSL_EBADLEN);
    }
  else if (tol <= 0)
    {
      GSL_ERROR ("tolerance must be positive", GSL_EINVAL);
    }
  else
    {
      int status;
      double rnorm, snorm;
      gsl_matrix_view A = gsl_matrix_submatrix(work->A, 0, 0, n, p);
      gsl_vector_view b = gsl_vector_subvector(work->t, 0, n);

      /* compute A = sqrt(W) X, b = sqrt(W) y */
      status = gsl_multifit_linear_applyW(X, w, y, &A.matrix, &b.vector);
      if (status)
        return status;

      /* compute SVD of A */
      status = gsl_multifit_linear_bsvd(&A.matrix, work);
      if (status)
        return status;

      status = multifit_linear_solve(X, &b.vector, tol, 0.0, rank,
                                     c, &rnorm, &snorm, work);
      if (status)
        return status;

      *chisq = rnorm * rnorm;

      /* variance-covariance matrix cov = s2 * (Q S^-1) (Q S^-1)^T */
      {
        const size_t p = X->size2;
        size_t i, j;
        gsl_matrix_view QSI = gsl_matrix_submatrix(work->QSI, 0, 0, p, p);
        gsl_vector_view D = gsl_vector_subvector(work->D, 0, p);

        for (i = 0; i < p; i++)
          {
            gsl_vector_view row_i = gsl_matrix_row (&QSI.matrix, i);
            double d_i = gsl_vector_get (&D.vector, i);

            for (j = i; j < p; j++)
              {
                gsl_vector_view row_j = gsl_matrix_row (&QSI.matrix, j);
                double d_j = gsl_vector_get (&D.vector, j);
                double s;

                gsl_blas_ddot (&row_i.vector, &row_j.vector, &s);

                gsl_matrix_set (cov, i, j, s / (d_i * d_j));
                gsl_matrix_set (cov, j, i, s / (d_i * d_j));
              }
          }
      }
    }

  return GSL_SUCCESS;
}

int
gsl_multifit_wlinear_svd (const gsl_matrix * X,
                          const gsl_vector * w,
                          const gsl_vector * y,
                          double tol,
                          size_t * rank,
                          gsl_vector * c,
                          gsl_matrix * cov,
                          double *chisq, gsl_multifit_linear_workspace * work)
{
  int status  = multifit_wlinear_svd (X, w, y, tol, 1, rank, c,
                                      cov, chisq, work);
  return status;

}

int
gsl_multifit_wlinear_usvd (const gsl_matrix * X,
                           const gsl_vector * w,
                           const gsl_vector * y,
                           double tol,
                           size_t * rank,
                           gsl_vector * c,
                           gsl_matrix * cov,
                           double *chisq, gsl_multifit_linear_workspace * work)
{
  int status  = multifit_wlinear_svd (X, w, y, tol, 0, rank, c,
                                      cov, chisq, work);
  return status;

}


#if !defined(SM_H)
#define SM_H

#include <gsl/gsl_matrix.h>

/**
* @brief Class describing the Standard Model
*
* This class holds all the parameters specifying the Standard Model, and
* methods to access them in convenient ways.
*
*/
class SM {

 public:

  /// Default value for EM coupling \f$ \alpha_{\rm{EM}}(M_Z) \f$
  const static double alpha    = 1./127.934;

  /// Default value for EM coupling at low energy
  const static double alpha0   = 1./137.0359997;

  /// Default value for the Fermi constant \f$ G_F \f$ (\f$ \rm{GeV}^{-2} \f$)
  const static double GF      = 1.16637E-5;

  /// Default value for \f$ M_Z \f$ (GeV)
  const static double MZ      = 91.15349;

  /// Default value for \f$ M_W \f$ (GeV)
  const static double MW      = 80.36951;

  /// Default value for the strong coupling \f$ \alpha_s(M_Z) \f$
  const static double alpha_s = 0.119;

  /// Default value for total width \f$ \Gamma_Z \f$ (GeV)
  const static double GammaZ  = 2.49581;

  /// Default value for total width \f$ \Gamma_W \f$ (GeV)
  const static double GammaW  = 2.08856;

  /// Default value for quark pole mass \f$ m_d \f$ (GeV)
  const static double md_p    = 0.0;

  /// Default value for quark pole mass \f$ m_u \f$ (GeV)
  const static double mu_p    = 0.0;

  /// Default value for quark pole mass \f$ m_s \f$ (GeV)
  const static double ms_p    = 0.1;

  // Scale for strange quark mass (GeV)
  const static double Q_ms    = 2.0;

  /// Default value for quark pole mass \f$ m_c \f$ (GeV)
  const static double mc_p    = 1.42;

  /// Default value for quark pole mass \f$ m_b \f$ (GeV)
  const static double mb_p    = 4.75;

  /// Default value for quark pole mass \f$ m_t \f$ (GeV)
  const static double mt_p    = 172.5;

  /// Default value for electron pole mass \f$ m_e \f$ (GeV)
  const static double me_p    = 0.510998918E-3;

  /// Default value for muon pole mass \f$ m_\mu \f$ (GeV)
  const static double mmu_p   = 0.105658367;

  /// Default value for tau pole mass \f$ m_\tau \f$ (GeV)
  const static double mtau_p  = 1.77684;

  // PDG booklet 2012
  /// Default value for CKM matrix element \f$ V_{ud} \f$
  const static double Vud     = 0.97427;
  /// Default value for CKM matrix element \f$ V_{us} \f$
  const static double Vus     = 0.22534;
  /// Default value for CKM matrix element \f$ V_{ub} \f$
  const static double Vub     = 0.00351;
  /// Default value for CKM matrix element \f$ V_{cd} \f$
  const static double Vcd     = 0.22520;
  /// Default value for CKM matrix element \f$ V_{cs} \f$
  const static double Vcs     = 0.97334;
  /// Default value for CKM matrix element \f$ V_{cb} \f$
  const static double Vcb     = 0.0412;
  /// Default value for CKM matrix element \f$ V_{td} \f$
  const static double Vtd     = 0.00867;
  /// Default value for CKM matrix element \f$ V_{ts} \f$
  const static double Vts     = 0.0404;
  /// Default value for CKM matrix element \f$ V_{tb} \f$
  const static double Vtb     = 0.999146;

  /**
  * @brief Default constructor initializing the Standard Model with default parameter
  *        values
  *
  * This constructor initializes a new SM object with default parameters. The
  * values used as defaults are specified by public constants in this class.
  */
  SM();

  /**
  * @brief Sets the EM coupling \f$ \alpha \f$
  *
  * @param alpha_in New value for \f$ \alpha(M_Z) \f$
  */
  void set_alpha(double alpha_in);

  /**
  * @brief Sets the EM coupling \f$ \alpha_0 \f at low energy$
  *
  * @param alpha_in New value for \f$ \alpha(Q^2=0) \f$
  */
  void set_alpha0(double alpha_in);

  /**
  * @brief Sets the Fermi constant \f$ G_F \f$
  *
  * @param GF_in New value for \f$ G_F \f$ (\f$ GeV^{-2} \f$)
  */
  void set_GF(double GF_in);

  /**
  * @brief Sets \f$ M_Z \f$
  *
  * @param MZ_in New value for \f$ M_Z \f$
  */
  void set_MZ(double MZ_in);

  /**
  * @brief Sets \f$ M_W \f$
  *
  * @param MW_in New value for \f$ M_W \f$
  */
  void set_MW(double MW_in);

  /**
  * @brief Sets \f$ \gamma_Z \f$
  *
  * @param GammaZ_in New value for \f$ \Gamma_Z \f$
  */
  void set_gamma_Z(double GammaZ_in);

  /**
  * @brief Sets \f$ \gamma_W \f$
  *
  * @param GammaW_in New value for \f$ \Gamma_W \f$
  */
  void set_gamma_W(double GammaW_in);

  /**
  * @brief Sets the strong coupling \f$ \alpha_s \f$
  *
  * @param alpha_s_in New value for \f$ \alpha_s(M_Z) \f$
  */
  void set_alpha_s(double alpha_s_in);

  /**
  * @brief Current value of \f$ \alpha \f$
  *
  * @returns Value of \f$ \alpha \f$
  */
  double get_alpha();
  double get_alpha0();

  /**
  * @brief Current value of \f$ G_F \f$
  *
  * @returns Value of \f$ G_F \f$ (\f$ \rm{GeV}^{-2} \f$)
  */
  double get_GF();

  /**
  * @brief Current value of \f$ \sin\theta_W \f$
  *
  * @returns Value of \f$ \sin\theta_W \f$
  */
  double get_sintw();

  /**
  * @brief Current value of \f$ \cos\theta_W \f$
  *
  * @returns Value of \f$ \cos\theta_W \f$
  */
  double get_costw();

  /**
  * @brief Current value of weak SU(2) coupling \f$ g \f$
  *
  * @returns Value of \f$ g \f$
  */
  double get_g();

  /**
  * @brief Current value of hypercharge U(1) coupling \f$ g' \f$
  *
  * @returns Value of \f$ g' \f$
  */
  double get_gprime();

  /**
  * @brief Current value of EM coupling \f$ e \f$
  *
  * @returns Value of \f$ g' \f$
  */
  double get_e();

  /**
  * @brief Current value of \f$ \alpha_s \f$
  *
  * @returns Value of \f$ \alpha_s \f$
  */
  double get_alpha_s();

  /**
  * @brief Current value of \f$ M_W \f$
  *
  * @returns Value of \f$ M_W \f$ (GeV)
  */
  double get_MW();

  /**
  * @brief Current value of \f$ \Gamma_W \f$
  *
  * @returns Value of \f$ \Gamma_W \f$ (GeV)
  */
  double get_gamma_W();

  /**
  * @brief Current value of \f$ M_Z \f$
  *
  * @returns Value of \f$ M_Z \f$ (GeV)
  */
  double get_MZ();

  /**
  * @brief Current value of \f$ \Gamma_Z \f$
  *
  * @returns Value of \f$ \Gamma_Z \f$ (GeV)
  */
  double get_gamma_Z();

  /**
  * @brief Current value of the vev \f$ v=1/\sqrt{\sqrt{2}G_F} \f$
  *
  * @returns Value of \f$ v \f$ (GeV)
  */
  double get_v();

  /**
  * @brief Current value of the vev squared \f$ v^2=1/(\sqrt{2}G_F) \f$
  *
  * @returns Value of \f$ v^2 \f$ (\f$ \rm{GeV}^2 \f$)
  */
  double get_v2();

  /**
  * @brief Vector boson mass
  *
  * This method returns the mass of a given vector boson. The numbering
  * convention used throughout 2HDMC is such that (1 = photon, 2 = Z0, 3 = W+).
  *
  * @param v Index (1-3) for the vector boson
  *
  * @returns Value of \f$ m_V \f$ (GeV)
  */
  double get_vmass(int v);

  /**
  * @brief Vector boson width
  *
  * This method returns the decay width of a given vector boson. The numbering
  * convention is such that (1 = photon, 2 = Z0, 3 = W+).
  *
  * @param v Index (1-3) for the vector boson
  *
  * @returns Value of \f$ \Gamma_V \f$ (GeV)
  */
  double get_gamma_V(int v);

 /**
  * @brief Sets the CKM matrix to a 3x3 unit matrix
  *
  * This method sets the CKM matrix to a 3x3 unit matrix, i.e. eliminating
  * all charged current interactions between different generations.
  */
  void set_diagonal_CKM();

  /**
  * @brief Full CKM matrix
  *
  * This method can be used to retrieve the full CKM matrix as a gsl_matrix.
  *
  * @returns CKM matrix
  *
  * @see get_MD, get_MU, get_ML, get_CKM_element
  */
  gsl_matrix* get_CKM_matrix();

  /**
  * @brief Element of the CKM matrix
  *
  * This method can be used to retrieve a certain element of the CKM matrix.
  *
  * @param i Row index (1,2,3 = u,c,t) of CKM matrix element
  * @param j Column index (1,2,3 = d,s,b) of CKM matrix element
  * @returns The requested CKM element
  *
  * @see get_CKM
  */
  double get_CKM_element(int i, int j);

  /**
  * @brief Sets the pole mass of a lepton
  *
  * This method sets the pole mass of lepton \a l to the supplied value.
  *
  * @param l Index of lepton (1,2,3 = \f$ e,\mu,\tau \f$)
  * @param lmass_in New mass of lepton
  */
  void set_lmass_pole(int l, double lmass_in);

  /**
  * @brief Sets the pole mass of a quark
  *
  * This method sets the pole mass of quark \a q to the supplied value.
  *
  * @param q Index of quark (1,2,3,4,5,6 = \f$ d,u,s,c,b,t \f$)
  * @param qmass_in New mass of quark
  */
  void set_qmass_pole(int q, double qmass_in);

  void set_qmass_msbar(int flav, double qmass_in);

  /**
  * @brief Sets the pole mass of a down-type quark
  *
  * This method sets the pole mass of quark \a d to the supplied value.
  *
  * @param d Index of quark (1,2,3 = \f$ d,s,b \f$)
  * @param dmass_in New mass of quark
  */
  void set_dmass_pole(int d, double dmass_in);

  /**
  * @brief Sets the pole mass of an up-type quark
  *
  * This method sets the pole mass of quark \a u to the supplied value.
  *
  * @param u Index of quark (1,2,3 = \f$ u,c,t \f$)
  * @param umass_in New mass of quark
  */
  void set_umass_pole(int u, double umass_in);

  /**
  * @brief Quark pole mass
  *
  * @param q Index (1,2,3,4,5,6 = \f$ d,u,s,c,b,t \f$) of quark
  *
  * @returns The pole mass \f$ m_q \f$
  */
  double get_qmass_pole(int q);

  /**
  * @brief Down-type quark pole mass
  *
  * @param d Index (1,2,3 = \f$ d,s,b \f$) of down-type quark
  *
  * @returns Quark pole mass \f$ m_d \f$
  */
  double get_dmass_pole(int d);

  /**
  * @brief Up-type quark pole mass
  *
  * @param u Index (1,2,3 = \f$ u,c,t \f$) of up-type quark
  *
  * @returns Quark pole mass \f$ m_u \f$
  */
  double get_umass_pole(int u);

  /**
  * @brief Lepton pole mass
  *
  * @param l Index (1,2,3 = \f$ e,\mu,\tau \f$) of lepton
  *
  * @returns The pole mass \f$ m_l \f$
  */
  double get_lmass_pole(int l);

  /**
  * @brief Mass matrix for down-type quarks
  *
  * This method can be used to retrieve, in matrix form, the current pole masses
  * used for the down-type quarks. The masses are returned as the diagonal
  * elements of a 3x3 gsl_matrix that must be allocated by the user.
  *
  * @returns Diagonal mass matrix for the down-type quarks
  *
  * @see get_MU, get_ML, get_CKM
  */
  gsl_matrix* get_MD();

  /**
  * @brief Mass matrix for up-type quarks
  *
  * This method can be used to retrieve, in matrix form, the current pole masses
  * used for the up-type quarks. The masses are returned as the diagonal
  * elements of a 3x3 gsl_matrix that must be allocated by the user.
  *
  * @returns Diagonal mass matrix for the up-type quarks
  *
  * @see get_MD, get_ML, get_CKM
  */
  gsl_matrix* get_MU();

  /**
  * @brief Mass matrix for leptons
  *
  * This method can be used to retrieve, in matrix form, the current pole masses
  * used for the leptons. The masses are returned as the diagonal
  * elements of a 3x3 gsl_matrix that must be allocated by the user.
  *
  * @returns Diagonal mass matrix for the leptons
  *
  * @see get_MD, get_MU, get_CKM
  */
  gsl_matrix* get_ML();

  /**
  * @brief Quark \f$ \overline{\rm{MS}}\f$ mass
  *
  * This method determines the \f$ \overline{\rm{MS}}\f$ mass \f$ \overline{m}_q(\overline{m}_q) \f$
  *
  * @param q Index (1,2,3,4,5,6 = \f$ d,u,s,c,b,t \f$) of quark
  *
  * @returns Quark \f$ \overline{\rm{MS}}\f$ mass \f$ \overline{m}_q(\overline{m}_q) \f$
  *
  * @see run_qmass_MSbar
  */
  double get_qmass_MSbar(int q);

  /**
  * @brief Down-type quark \f$ \overline{\rm{MS}}\f$ mass
  *
  * This method determines the \f$ \overline{\rm{MS}}\f$ mass \f$ \overline{m}_d(\overline{m}_d) \f$
  *
  * @param d Index (1,2,3 = \f$ d,s,b \f$) of down-type quark
  *
  * @returns Quark \f$ \overline{\rm{MS}}\f$ mass \f$ \overline{m}_d(\overline{m}_d) \f$
  *
  * @see get_umass_MSbar, run_qmass_MSbar
  */
  double get_dmass_MSbar(int d);

  /**
  * @brief Up-type quark \f$ \overline{\rm{MS}}\f$ mass
  *
  * This method determines the \f$ \overline{\rm{MS}}\f$ mass \f$ \overline{m}_u(\overline{m}_u) \f$
  *
  * @param u Index (1,2,3 = \f$ u,c,t \f$) of up-type quark
  *
  * @returns Quark \f$ \overline{\rm{MS}}\f$ mass \f$ \overline{m}_u(\overline{m}_u) \f$
  *
  * @see get_dmass_MSbar, run_qmass_MSbar
  */
  double get_umass_MSbar(int u);

  /**
  * @brief Evaluates running quark masses
  *
  * This method evaluates the running quark_mass \a quark_mass, initially specified
  * at the input scale \a Qinit, at the new scale \a Qfin. The calculation is performed
  * in the \f$ \overline{\rm{MS}} \f$ scheme with variable number of active
  * flavours and threshold matching.
  *
  * @param quark_mass The input value for the running quark mass
  * @param Qinit      Starting scale
  * @param Qfin       Final scale at which the mass is to be evaluated
  * @param mtop       \f$ \overline{\rm{MS}} \f$ top mass (for thresholds)
  * @param mbot       \f$ \overline{\rm{MS}} \f$ bottom mass (for thresholds)
  *
  * @returns The running mass at the scale \a Qfin
  */
  double run_qmass_MSbar(double quark_mass, double Qinit, double Qfin, double mtop, double mbot);

  /**
  * @brief Evaluates the running strong coupling
  *
  * This method evaluates the running strong coupling \f$ \alpha_s \f$ in the
  * \f$ \overline{\rm{MS}} \f$ scheme at the scale specified by \a Q.
  *
  * @param Q          Scale at which to evaluate the running coupling
  * @param mtop       \f$ \overline{\rm{MS}} \f$ top mass (for thresholds)
  * @param mbot       \f$ \overline{\rm{MS}} \f$ bottom mass (for thresholds)
  *
  * @returns The strong coupling \f$ \alpha_s \f$ at the scale \a Q
  */
  double run_alphas_MSbar(double Q, double mtop, double mbot);

  /**
  * @brief Number of active flavours
  *
  * This method returns the number of active quark flavours to be used at
  * a certain mass scale when decoupling the heavier quarks. The thresholds
  * are determined from the quark \f$ \overline{\rm{MS}} \f$ masses.
  *
  * @param M The scale for which the number of flavours is desired
  *
  * @returns The number of quark flavours active at this scale
  */
  int get_Nactivef(double M);

  /**
  * @brief %SM width of the top quark
  *
  * Calculates the width of the top quark in the %SM, i.e. no contributions from
  * \f$ t\to H^+ b \f$ decays etc. are included.
  *
  * @returns The top width \f$ \Gamma_t \f$
  */
  double get_gamma_top();
  double get_gamma_tWd(int d);

  // --- These masses are for compatibility test with HDECAY ---
  // Scale for HD quark masses (HD)
  const static double Q_HD    = 5.0;

  // switch for HD quark masses (HD)
  const static bool b_HD    = false;

  /// Default value for running quark mass at 5 GeV \f$ m_s(5) \f$ (GeV)
  const static double ms_5    = 0.08160951656;

  /// Default value for running quark mass at 5 GeV \f$ m_c(5) \f$ (GeV)
  const static double mc_5    = 0.8960809463;

  /// Default value for running quark mass at 5 GeV \f$ m_b(5) \f$ (GeV)
  const static double mb_5    = 4.310679848;

  /// Default value for running quark mass at 5 GeV \f$ m_t(5) \f$ (GeV)
  const static double mt_5    = 246.5552058;
  // -------------------------------------------------------------

 private:

  // Internal variables which store the current values of the SM parameters
  double m_alpha;
  double m_alpha0;
  double m_GF;
  double m_s2tW;
  double m_alpha_s;
  double m_md_p[3];
  double m_mu_p[3];
  double m_ml_p[3];
  double m_MW;
  double m_MZ;
  double m_GammaW;
  double m_GammaZ;
  double m_CKM[3][3];

  double m_qmass_ms[7];

	// Support functions for the running mass calculation
  double m_threshold(double as);
  double RQ(double as, int nf, int loops);
  double QCD_beta(int c, int nf);

  void clear_lookup();

};

#endif


#include <cblas.h>

#include "tasks.h"

void syrk_task_seq(void *ptr)
{
    struct syrk_task_arg *arg = (struct syrk_task_arg*) ptr;

    int n       = arg->n;
    int k       = arg->k;
    double *A21 = arg->A21;
    double *A22 = arg->A22;
    int ldA     = arg->ldA;

    // Compute A22 := A21 - A21 * A21'.
    cblas_dsyrk(CblasColMajor, CblasLower, CblasNoTrans,
                n, k,
                -1.0, A21, ldA,
                 1.0, A22, ldA);
}


#ifndef _params_h_
#define _params_h_

#include <stdio.h>
#include <time.h>
#include <unistd.h> //For getpid() function
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <fftw3.h>

#include <gsl/gsl_spline.h>
#include <gsl/gsl_interp.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_matrix.h>

//****Simulation Parameters:
#define N_ps (int) 64 //Number of points in PS file
#define N_gen (int) 4 //To generate the 21cm field
#define LEN (double) 8.0 //Length of simulation box (in Mpc h^{-1})
#define freq_width (double) 2.0 //Width of frequency channel (in MHz)

//****Foreground Removal Parameters:
double *expo;
int expo_array;
int count(int);

//****Function declarations:
double interp_ps_lin(double);
double read_21cm_cube(char *);
double sim_21cm_cube();
double generate_full_signal();
double count_freq_channels();
double noise_removal();

//****Multifrequency fitting declarations:
double *y, *x, *N, *Ni, *xtNi, *xtNix, *xtNix_i, *big_matrix, *a, *xa;

//****GSL rng declarations:
const gsl_rng_type *T;
gsl_rng *rand1, *rand2;
long seed;
double a_q, b_q;

//****Others:
int SimON, XY_pix, Z_pix, Npol; //XY_pix and Z_pix are the box dimensions in pixels
double frequency, original_21cm_mean, total_21cm_mean;
double *original_21cm_field, *total_21cm_field, *detector_noise_field, *total_noise_field , *recovered_21cm_field, *parameter_a_cube;
double k[N_ps], Pk[N_ps];
double *freq_array;
double sigma, nu, tau, offset;
double XY_LEN, Z_LEN; //Length of the box

FILE *file;
char cmnd[1000], header[100], polystr[100];
//****polystr is to store the exponents of variables of the fitting polynomial

#endif


/*==========================================================
 * pca.c - Principal component analysis of diffraction data
 *
 * Calculates the two main components of the data
 * by Principal Component Analysis, as detailed in
 * Bernhardt et.al (2016), Biophys. J.
 *
 * The calling syntax is:
 *
 *		pca(data, mask, qx, qy, w, phi, m, n)
 *
 * Copyright 2017 Institute for X-ray Physics (University of Göttingen)
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included
 * in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
 * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE
 * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 *
 *========================================================*/

#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_eigen.h>

#define PI 3.14159265

/* The computational routine */
void pca(double *x, double *mask, double *qx, double *qy, double *w, double *phi, int m, int n)
{
    int col;
    int row;
    double tr = 0;

    /* total sum of x */
    for (row=0; row<m; row++) {
        for (col=0; col<n; col++) {
            x[row*m + col] = x[row*m + col] * mask[row*m + col];
            tr += x[row*m + col];
        }
    }

    /* covariance matrix */
    double cov_qx = 0;
    double cov_mixed = 0;
    double cov_qy = 0;

    /* total sum of x */
    for (row=0; row<m; row++) {
        for (col=0; col<n; col++) {
            cov_qx += x[row*m + col] * pow(qx[row*m + col],2);
            cov_qy += x[row*m + col] * pow(qy[row*m + col],2);
            cov_mixed += x[row*m + col] * qx[row*m + col] * qy[row*m + col];
        }
    }

    double data[] = {cov_qx/tr   , cov_mixed/tr,
                     cov_mixed/tr, cov_qy/tr   };

    gsl_matrix_view mv
        = gsl_matrix_view_array (data, 2, 2);

    gsl_vector *eval = gsl_vector_alloc (2);
    gsl_matrix *evec = gsl_matrix_alloc (2, 2);

    gsl_eigen_symmv_workspace * ws
        = gsl_eigen_symmv_alloc (2);

    gsl_eigen_symmv (&mv.matrix, eval, evec, ws);

    gsl_eigen_symmv_free (ws);

    gsl_eigen_symmv_sort (eval, evec,
                        GSL_EIGEN_SORT_ABS_DESC);

    /* get eigenvalues and eigenvectors (assuming they are sorted) */
    double eval_i = gsl_vector_get (eval, 0);
    double eval_j = gsl_vector_get (eval, 1);

    gsl_vector_view evec_i
       = gsl_matrix_column (evec, 0);

    double xcomp = gsl_vector_get (&evec_i.vector, 0);
    double ycomp = gsl_vector_get (&evec_i.vector, 1);

    /* anisotropy */
    *w = (eval_i - eval_j) / (eval_i + eval_j);

    /* orientation */
    *phi = atan2(ycomp,xcomp) * 180.0 / PI;
    if (*phi < 0){
        *phi += 180;
    }

    gsl_vector_free (eval);
    gsl_matrix_free (evec);

    /* debugging information */
    /*
    printf ("%g\t%g\t%g\n", cov_qx/tr, cov_mixed/tr, cov_qy/tr );

    printf ("eigenvalue 1 = %g\n", eval_i);
    printf ("eigenvalue 2 = %g\n", eval_j);

    printf ("eigenvector = \n");
    gsl_vector_fprintf (stdout,
                        &evec_i.vector, "%g");

    printf ("x = %g\n", xcomp);
    printf ("y = %g\n", ycomp);
    printf ("w = %g\n", *w);
    printf ("phi = %g\n", *phi);
    */
}


#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_blas.h>

#include "su3_matrix.c"

#ifndef gauge_config
#include "config.h"
#define gauge_config 1

#define sq(x) ((x)*(x))
#define cb(x) ((x)*(x)*(x))
#endif

int acc, tot; // acceptance rate

#define FORWARD 1 //true
#define BACKWARD 0 //false

// Access pattern methods --------------------------------------------------

// index of link site=(t,x,y,z) in direction dir
int index(const int* site, int dir){
  return dir+site[0]*4+site[1]*N*4+site[2]*N*N*4+site[3]*N*N*N*4;
}

// shift site array by one to direction dir
void shift(int* site, int dir, int forward){
  if(forward == 1){
    site[dir] = (site[dir]+1)%N;
  }else{
    site[dir] = (site[dir]-1+N)%N;
  }
}

// spatial rotation by 90deg of shape array
void rotate(int n, int* shape){
  for(int i=0;i<n;i++){
    if(shape[i]==1) shape[i]=2;
    if(shape[i]==2) shape[i]=3;
    if(shape[i]==3) shape[i]=1;
  }
}

// end of access pattern methods -------------------------------------------

// Plaquette methods -------------------------------------------------------

// plaquette operator at site (t,x,y,z) in plane (mu,nu)
// normally mu>nu
double plaquette(const su3_matrix* links, int mu, int nu, int* site){
  su3_matrix A,B,C,D;
  A = links[index(site,mu)];
  shift(site,mu,FORWARD);
  B = links[index(site,nu)];
  shift(site,mu,BACKWARD);
  shift(site,nu,FORWARD);
  C = su3_inv(links[index(site,mu)]);
  shift(site,nu,BACKWARD); // restore site position
  D = su3_inv(links[index(site,nu)]);

  A = su3_mul(A,B);
  A = su3_mul(A,C);
  A = su3_mul(A,D);

  return su3_trace(A)/3.0;
}

// plaquette spatial mean
double plaq_mean_s(int t, const su3_matrix* links){
  double mean = 0;
  int site[4];
  for(int z=0;z<N;z++){
    for(int y=0;y<N;y++){
      for(int x=0;x<N;x++){
          site[0]=t; site[1]=x; site[2]=y; site[3]=z;
          for(int mu=1;mu<4;mu++){
            for(int nu=mu+1;nu<4;nu++){
              mean += plaquette(links,mu,nu,site);
            }
          }
      }
    }
  }
  return mean/ (3*N*N*N); // 3 directions & N^3 sites
}

// end of plaquette methods ------------------------------------------------

// Metropolis update --------------------------------------------------------

void do_hb_update(su3_matrix* links, const gsl_rng* r, const su3_matrix* rands, int* site, int dir);
double action(su3_matrix* links, int* site, int dir);

// the big loop
void hb_update(su3_matrix* links, const gsl_rng* r, const su3_matrix* rands){
  int site[4];
  for(int z=0;z<N;z++){
    for(int y=0;y<N;y++){
      for(int x=0;x<N;x++){
        for(int t=0;t<N;t++){
          for(int dir=0;dir<4;dir++){
            site[0]=t; site[1]=x; site[2]=y; site[3]=z;
            do_hb_update(links,r,rands,site,dir);
          }
        }
      }
    }
  }
}

// update
void do_hb_update(su3_matrix* links, const gsl_rng* r, const su3_matrix* rands, int* site, int dir){
  if(verbose) {printf("Updating {%d, %d, %d, %d} direction %d",site[0],site[1],site[2],site[3],dir);getchar();}
  double dS,action_old;
  su3_matrix link_old;
  for(int times=0; times<Nk; times++){
    action_old = action(links,site,dir);
    link_old = links[index(site,dir)];
    links[index(site,dir)]=su3_mul(links[index(site,dir)],rands[gsl_rng_uniform_int(r, 50)]);

    dS = action(links,site,dir)-action_old;

    if(verbose) {printf("Old action = %f, ",action_old); su3_print(link_old); getchar();}
    if(verbose) {printf("New action = %f, ",action_old+dS); su3_print(links[index(site,dir)]); getchar();}
    acc++;tot++;
    if(dS>0){
      if(exp(-dS)<gsl_rng_uniform(r)){
        acc--;
        links[index(site,dir)]=link_old; // revert update
        if(verbose) {printf("Reverted"); getchar();}
      }
    }
  }

}

// calculating the action
// for each direction we need to find plaquette at 2 sites in each of the 3 planes
// mu > nu
double action(su3_matrix* links, int* site, int dir){
  int mu,nu;
  double result=0;
  for(int k=0; k<4; k++){
    if(k==dir) continue; // loop over planes
    if(k>dir){mu=k; nu=dir;} // mu=larger number
    if(k<dir){mu=dir; nu=k;}

    result += plaquette(links, mu,nu,site);
    shift(site,k,BACKWARD); // shift to the perpendicular direction backwards
    result += plaquette(links, mu,nu,site);
    shift(site,k,FORWARD);  // shift it back
  }

  return -beta*result;
}

// end of heat bath update -------------------------------------------------

// Loops -------------------------------------------------------------------
// Generic loop ------------------------------------------------------------

// TODO: not working
double gloop(int n, int* shape, const su3_matrix* links, int* site){
  su3_matrix A[2*n];

  // forward
  for(int i=0;i<n;i++){
    A[i]=links[index(site,shape[i])];
    shift(site,shape[i],FORWARD);
  }

  // backward
  for(int i=0;i<n;i++){
    shift(site,shape[i],BACKWARD);
    A[n+i]=su3_inv(links[index(site,shape[i])]);
  }

  // multiply
  for(int i=1;i<2*n;i++){
    A[0]=su3_mul(A[0],A[i]);
  }

  return su3_trace(A[0])/3.0;
}

double loop(int r, int t, int mu, int nu, const su3_matrix* links, int* site){
  int shape[r+t];
  for(int i=0;i<t;i++)shape[i]=nu;
  for(int i=0;i<r;i++)shape[t+i]=mu;

  return gloop(r+t,shape,links,site);
  /*
  su3_matrix A[(r+t)*2];

  // bottom side
  for(int i=0;i<t;i++){
    A[i]=links[index(site,nu)];
    shift(site,nu,FORWARD);
  }

  // right side
  for(int i=0;i<r;i++){
    A[t+i]=links[index(site,mu)];
    shift(site,mu,FORWARD);
  }

  // top side
  for(int i=0;i<t;i++){
    shift(site,nu,BACKWARD);
    A[t+r+i]=su3_inv(links[index(site,nu)]);
  }

  // left side
  for(int i=0;i<r;i++){
    shift(site,mu,BACKWARD);
    A[t+r+t+i]=su3_inv(links[index(site,mu)]);
  }

  // multiply
  for(int i=1;i<2*(r+t);i++){
    A[0]=su3_mul(A[0],A[i]);
  }

  return su3_trace(A[0])/3.0;*/
}

// end of Loop -------------------------------------------------------------
// Wilson loop -------------------------------------------------------------

double wloop(int r, int t, int mu,const su3_matrix* links, int* site){
  return loop(r, t, mu, 0, links, site);
}

double wloop_mean(int wR, int wT, const su3_matrix* links){
  double mean = 0;
  int site[4];
  for(int z=0;z<N;z++){
    for(int y=0;y<N;y++){
      for(int x=0;x<N;x++){
        for(int t=0;t<N;t++){
            site[0]=t; site[1]=x; site[2]=y; site[3]=z;
            for(int mu=1;mu<4;mu++){
              mean += wloop(wR,wT,mu,links,site);
            }
        }
      }
    }
  }
  return mean/ (3*N*N*N*N); // 3 directions & N^3 sites
}
// end of Wilson loop ------------------------------------------------------
// Polyakov loop -----------------------------------------------------------
double polyakov(const su3_matrix* links, int* site){
  su3_matrix A[N];

  for(int i=0;i<N;i++){
    A[i]=links[index(site,0)];
    shift(site,0,FORWARD);
  }

  // multiply
  for(int i=1;i<N;i++){
    A[0]=su3_mul(A[0],A[i]);
  }

  return su3_trace(A[0])/3.0;
}

double polyakov_mean(const su3_matrix* links){
  double mean = 0;
  int site[4];
  site[0]=0;
  for(int z=0;z<N;z++){
    for(int y=0;y<N;y++){
      for(int x=0;x<N;x++){
        site[1]=x; site[2]=y; site[3]=z;
        mean += polyakov(links,site);
      }
    }
  }
  return mean/ (N*N*N);
}
// end of Polyakov loop ----------------------------------------------------

// Twisted Wilson loop -----------------------------------------------------
// Ref: Lepage p.33

// r=sqrt(2)
double twloop2(int t, int mu, int nu ,const su3_matrix* links, int* site){
  su3_matrix A[(2+t)*2];

  // bottom side
  for(int i=0;i<t;i++){
    A[i]=links[index(site,0)];
    shift(site,0,FORWARD);
  }

  // right side, twisted
  A[t+0]=links[index(site,mu)];
  shift(site,mu,FORWARD);
  A[t+1]=links[index(site,nu)];
  shift(site,nu,FORWARD);

  // top side
  for(int i=0;i<t;i++){
    shift(site,0,BACKWARD);
    A[t+2+i]=su3_inv(links[index(site,0)]);
  }

  // left side, twisted
  shift(site,mu,BACKWARD);
  A[t+2+t+0]=su3_inv(links[index(site,mu)]);
  shift(site,nu,BACKWARD);
  A[t+2+t+1]=su3_inv(links[index(site,nu)]);

  // multiply
  for(int i=1;i<2*(2+t);i++){
    A[0]=su3_mul(A[0],A[i]);
  }

  return su3_trace(A[0])/3.0;
}

double twloop2_mean(int wT, const su3_matrix* links){
  double mean = 0;
  int site[4];
  for(int z=0;z<N;z++){
    for(int y=0;y<N;y++){
      for(int x=0;x<N;x++){
        for(int t=0;t<N;t++){
            site[0]=t; site[1]=x; site[2]=y; site[3]=z;
            for(int mu=1;mu<4;mu++){
              for(int nu=1;nu<4;nu++){
                if(mu!=nu){
                  mean += twloop2(wT,mu,nu,links,site);
                }
              }
            }
        }
      }
    }
  }
  return mean/ (6*N*N*N*N); // 3 x 2 directions & N^3 sites
}

// r=sqrt(3)
double twloop3(int t, const su3_matrix* links, int* site){
  su3_matrix A[(3+t)*2];

  // bottom side
  for(int i=0;i<t;i++){
    A[i]=links[index(site,0)];
    shift(site,0,FORWARD);
  }

  // right side, twisted
  A[t+0]=links[index(site,1)];
  shift(site,1,FORWARD);
  A[t+1]=links[index(site,2)];
  shift(site,2,FORWARD);
  A[t+2]=links[index(site,3)];
  shift(site,3,FORWARD);

  // top side
  for(int i=0;i<t;i++){
    shift(site,0,BACKWARD);
    A[t+3+i]=su3_inv(links[index(site,0)]);
  }

  // left side, twisted
  shift(site,1,BACKWARD);
  A[t+3+t+0]=su3_inv(links[index(site,1)]);
  shift(site,2,BACKWARD);
  A[t+3+t+1]=su3_inv(links[index(site,2)]);
  shift(site,3,BACKWARD);
  A[t+3+t+2]=su3_inv(links[index(site,3)]);

  // multiply
  for(int i=1;i<2*(3+t);i++){
    A[0]=su3_mul(A[0],A[i]);
  }

  return su3_trace(A[0])/3.0;
}

double twloop3_mean(int wT, const su3_matrix* links){
  double mean = 0;
  int site[4];
  for(int z=0;z<N;z++){
    for(int y=0;y<N;y++){
      for(int x=0;x<N;x++){
        for(int t=0;t<N;t++){
            site[0]=t; site[1]=x; site[2]=y; site[3]=z;
            mean += twloop3(wT,links,site);
        }
      }
    }
  }
  return mean/ (N*N*N*N); // N^3 sites
}
// end of twisted Wilson loop --------------------------------------------

// Plaquette correlator
double plaq_corr(int T, const su3_matrix* links){
  double mean=0;
  for(int i=0;i<N;i++){
    mean+=plaq_mean_s(i,links)*plaq_mean_s(i+T,links);
  }
  return mean/N;
}
// end of plaquette correlator

// TODO: vvvvvvvv

// Smeared Wilson loop ---------------------------------------------------

// end of smeared Wilson loop --------------------------------------------


#include <float.h>

#include <gsl/gsl_blas.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_spmatrix.h>
#include <gsl/gsl_vector.h>
#include <osqp/osqp.h>

#include "qdm.h"

#define QDM_THETA_MIN_THRESHOLD 0.0001

int
qdm_theta_optimize(
    gsl_vector *result,
    gsl_vector *emperical_quantiles,
    gsl_matrix *ix
)
{
  int status = 0;

  gsl_matrix *p = gsl_matrix_alloc(ix->size2, ix->size2);
  gsl_matrix *a = gsl_matrix_alloc(ix->size2, ix->size2);
  gsl_vector *q = gsl_vector_alloc(ix->size2);
  gsl_vector *l = gsl_vector_alloc(ix->size2);
  gsl_vector *u = gsl_vector_alloc(ix->size2);

  // FIXME check c_malloc...
  OSQPData *data         = c_malloc(sizeof(OSQPData));
  OSQPSettings *settings = c_malloc(sizeof(OSQPSettings));
  OSQPWorkspace *work    = NULL;

  /* Ensure all data fields are initialized before use. */
  *data = (OSQPData) {
    .n = 0,
    .m = 0,
    .P = NULL,
    .A = NULL,
    .q = NULL,
    .l = NULL,
    .u = NULL,
  };

  status = qdm_matrix_tmm(ix, p);
  if (status != 0) {
    goto cleanup;
  }

  qdm_matrix_select_upper_triangle(p);

  gsl_matrix_set_identity(a);

  gsl_vector_set_all(l, QDM_THETA_MIN_THRESHOLD);
  gsl_vector_set_all(u, INFINITY);

  status = gsl_blas_dgemv(CblasTrans, -1.0, ix, emperical_quantiles, 0, q);
  if (status != 0) {
    goto cleanup;
  }

  /* Solve... */
  data->n = ix->size2;
  data->m = ix->size2;

  status = qdm_matrix_to_csc_matrix(&data->P, p);
  if (status != 0) {
    goto cleanup;
  }

  status = qdm_matrix_to_csc_matrix(&data->A, a);
  if (status != 0) {
    goto cleanup;
  }

  data->q = q->data;
  data->l = l->data;
  data->u = u->data;

  osqp_set_default_settings(settings);
  settings->verbose = 0;

  status = osqp_setup(&work, data, settings);
  if (status != 0) {
    goto cleanup;
  }

  status = osqp_solve(work);
  if (status != 0) {
    goto cleanup;
  }

  gsl_vector_view beta0 = gsl_vector_view_array(work->solution->x, ix->size2);
  status = gsl_vector_memcpy(result, &beta0.vector);
  if (status != 0) {
    goto cleanup;
  }

cleanup:
  if (data != NULL) {
    if (data->A != NULL) {
      free(data->A->p);
      free(data->A->i);
      free(data->A->x);
    }

    c_free(data->A);

    if (data->P != NULL) {
      free(data->P->p);
      free(data->P->i);
      free(data->P->x);
    }

    c_free(data->P);

    c_free(data);
  }

  c_free(settings);
  osqp_cleanup(work);

  gsl_vector_free(u);
  gsl_vector_free(l);
  gsl_vector_free(q);
  gsl_matrix_free(a);
  gsl_matrix_free(p);

  return status;
}

void
qdm_theta_matrix_constrain(
    gsl_matrix *theta,
    double min
)
{
  /* First row is constrained to be greater than zero. */
  for (size_t j = 0; j < theta->size2; j++) {
    if (gsl_matrix_get(theta, 0, j) < 0) {
      gsl_matrix_set(theta, 0, j, min);
    }
  }

  /* Remaining cells are set to zero if the sum of the column is negative.
   *
   * NOTE: The first column is not modified.
   */
  for (size_t i = 1; i < theta->size1; i++) {
    for (size_t j = 1; j < theta->size2; j++) {
      gsl_vector_view column = gsl_matrix_column(theta, j);
      double sum = qdm_vector_sum(&column.vector);

      if (sum < 0) {
        gsl_matrix_set(theta, i, j, 0);
      }
    }
  }
}


// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2013.
//
// This software is released under a three-clause BSD license:
//  * 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.
//  * Neither the name of any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// 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 ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS 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.
//
// --------------------------------------------------------------------------
// $Maintainer: Clemens Groepl $
// $Authors: $
// --------------------------------------------------------------------------

#ifndef OPENMS_MATH_STATISTICS_LINEARREGRESSION_H
#define OPENMS_MATH_STATISTICS_LINEARREGRESSION_H

#include <OpenMS/CONCEPT/Types.h>
#include <OpenMS/CONCEPT/Exception.h>

#include <vector>
#include <gsl/gsl_fit.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_cdf.h>

namespace OpenMS
{
  namespace Math
  {
    /**
      @brief This class offers functions to perform least-squares fits to a straight line model, \f$ Y(c,x) = c_0 + c_1 x \f$.

      It encapsulates the GSL methods for a weighted and an unweighted linear regression.

      Next to the intercept with the y-axis and the slope of the fitted line, this class computes the:
        - squared pearson coefficient
        - value of the t-distribution
        - standard deviation of the residuals
        - standard error of the slope
        - intercept with the x-axis (useful for additive series experiments)
        - lower border of confidence interval
        - higher border of confidence interval
        - chi squared value
        - x mean

      @ingroup Math
    */
    class OPENMS_DLLAPI LinearRegression
    {
public:

      /// Constructor
      LinearRegression() :
        intercept_(0),
        slope_(0),
        x_intercept_(0),
        lower_(0),
        upper_(0),
        t_star_(0),
        r_squared_(0),
        stand_dev_residuals_(0),
        mean_residuals_(0),
        stand_error_slope_(0),
        chi_squared_(0),
        rsd_(0)
      {
      }

      /// Destructor
      virtual ~LinearRegression()
      {
      }

      /**
          @brief This function computes the best-fit linear regression coefficients \f$ (c_0,c_1) \f$
          of the model \f$ Y = c_0 + c_1 X \f$ for the dataset \f$ (x, y) \f$.

          The values in x-dimension of the dataset \f$ (x,y) \f$ are given by the iterator range [x_begin,x_end)
          and the corresponding y-values start at position y_begin.

          For a  "x %" Confidence Interval use confidence_interval_P = x/100.
          For example the 95% Confidence Interval is supposed to be an interval that has a 95% chance of
          containing the true value of the parameter.

          @return If an error occurred during the fit.

          @exception Exception::UnableToFit is thrown if fitting cannot be performed
      */
      template <typename Iterator>
      void computeRegression(double confidence_interval_P, Iterator x_begin, Iterator x_end, Iterator y_begin);

      /**
          @brief This function computes the best-fit linear regression coefficient \f$ (c_0) \f$
          of the model \f$ Y = c_1 X \f$ for the dataset \f$ (x, y) \f$.

          The values in x-dimension of the dataset \f$ (x,y) \f$ are given by the iterator range [x_begin,x_end)
          and the corresponding y-values start at position y_begin.

          For a  "x %" Confidence Interval use confidence_interval_P = x/100.
          For example the 95% Confidence Interval is supposed to be an interval that has a 95% chance of
          containing the true value of the parameter.

          @return If an error occurred during the fit.

          @exception Exception::UnableToFit is thrown if fitting cannot be performed
      */
      template <typename Iterator>
      void computeRegressionNoIntercept(double confidence_interval_P, Iterator x_begin, Iterator x_end, Iterator y_begin);

      /**
          @brief This function computes the best-fit linear regression coefficients \f$ (c_0,c_1) \f$
          of the model \f$ Y = c_0 + c_1 X \f$ for the weighted dataset \f$ (x, y) \f$.

          The values in x-dimension of the dataset \f$ (x, y) \f$ are given by the iterator range [x_begin,x_end)
          and the corresponding y-values start at position y_begin. They will be weighted by the
          values starting at w_begin.

          For a  "x %" Confidence Interval use confidence_interval_P = x/100.
          For example the 95% Confidence Interval is supposed to be an interval that has a 95% chance of
          containing the true value of the parameter.

          @return If an error occurred during the fit.

          @exception Exception::UnableToFit is thrown if fitting cannot be performed
      */
      template <typename Iterator>
      void computeRegressionWeighted(double confidence_interval_P, Iterator x_begin, Iterator x_end, Iterator y_begin, Iterator w_begin);

      /// Non-mutable access to the y-intercept of the straight line
      DoubleReal getIntercept() const;
      /// Non-mutable access to the slope of the straight line
      DoubleReal getSlope() const;
      /// Non-mutable access to the x-intercept of the straight line
      DoubleReal getXIntercept() const;
      /// Non-mutable access to the lower border of confidence interval
      DoubleReal getLower() const;
      /// Non-mutable access to the upper border of confidence interval
      DoubleReal getUpper() const;
      /// Non-mutable access to the value of the t-distribution
      DoubleReal getTValue() const;
      /// Non-mutable access to the squared pearson coefficient
      DoubleReal getRSquared() const;
      /// Non-mutable access to the standard deviation of the residuals
      DoubleReal getStandDevRes() const;
      /// Non-mutable access to the residual mean
      DoubleReal getMeanRes() const;
      /// Non-mutable access to the standard error of the slope
      DoubleReal getStandErrSlope() const;
      /// Non-mutable access to the chi squared value
      DoubleReal getChiSquared() const;
      /// Non-mutable access to relative standard deviation
      DoubleReal getRSD() const;

protected:

      /// The intercept of the fitted line with the y-axis
      double intercept_;
      /// The slope of the fitted line
      double slope_;
      /// The intercept of the fitted line with the x-axis
      double x_intercept_;
      /// The lower bound of the confidence interval
      double lower_;
      /// The upper bound of the confidence interval
      double upper_;
      /// The value of the t-statistic
      double t_star_;
      /// The squared correlation coefficient (Pearson)
      double r_squared_;
      /// The standard deviation of the residuals
      double stand_dev_residuals_;
      /// Mean of residuals
      double mean_residuals_;
      /// The standard error of the slope
      double stand_error_slope_;
      /// The value of the Chi Squared statistic
      double chi_squared_;
      /// the relative standard deviation
      double rsd_;

      /// Computes the goodness of the fitted regression line
      void computeGoodness_(double * X, double * Y, int N, double confidence_interval_P);

      /// Copies the distance(x_begin,x_end) elements starting at x_begin and y_begin into the arrays x_array and y_array
      template <typename Iterator>
      void iteratorRange2Arrays_(Iterator x_begin, Iterator x_end, Iterator y_begin, double * x_array, double * y_array);

      /// Copy the distance(x_begin,x_end) elements starting at  x_begin, y_begin and w_begin into the arrays x_array, y_array and w_array
      template <typename Iterator>
      void iteratorRange3Arrays_(Iterator x_begin, Iterator x_end, Iterator y_begin, Iterator w_begin, double * x_array, double * y_array, double * w_array);

private:

      /// Not implemented
      LinearRegression(const LinearRegression & arg);
      /// Not implemented
      LinearRegression & operator=(const LinearRegression & arg);
    };

    template <typename Iterator>
    void LinearRegression::computeRegression(double confidence_interval_P, Iterator x_begin, Iterator x_end, Iterator y_begin)
    {
      int N = int(distance(x_begin, x_end));

      double * X = new double[N];
      double * Y = new double[N];
      iteratorRange2Arrays_(x_begin, x_end, y_begin, X, Y);

      double cov00, cov01, cov11;

      // Compute the unweighted linear fit.
      // Get the intercept and the slope of the regression Y_hat=intercept_+slope_*X
      // and the value of Chi squared, the covariances of the intercept and the slope
      int error = gsl_fit_linear(X, 1, Y, 1, N, &intercept_, &slope_, &cov00, &cov01, &cov11, &chi_squared_);

      if (!error)
      {
        computeGoodness_(X, Y, N, confidence_interval_P);
      }

      delete[] X;
      delete[] Y;

      if (error)
      {
        throw Exception::UnableToFit(__FILE__, __LINE__, __PRETTY_FUNCTION__, "UnableToFit-LinearRegression", "Could not fit a linear model to the data");
      }
    }

    template <typename Iterator>
    void LinearRegression::computeRegressionNoIntercept(double confidence_interval_P, Iterator x_begin, Iterator x_end, Iterator y_begin)
    {
      int N = int(distance(x_begin, x_end));

      double * X = new double[N];
      double * Y = new double[N];
      iteratorRange2Arrays_(x_begin, x_end, y_begin, X, Y);

      double cov;

      // Compute the linear fit.
      // Get the intercept and the slope of the regression Y_hat=intercept_+slope_*X
      // and the value of Chi squared, the covariances of the intercept and the slope
      int error = gsl_fit_mul(X, 1, Y, 1, N, &slope_, &cov, &chi_squared_);
      intercept_ = 0.0;

      if (!error)
      {
        computeGoodness_(X, Y, N, confidence_interval_P);
      }

      delete[] X;
      delete[] Y;

      if (error)
      {
        throw Exception::UnableToFit(__FILE__, __LINE__, __PRETTY_FUNCTION__, "UnableToFit-LinearRegression", "Could not fit a linear model to the data");
      }
    }

    template <typename Iterator>
    void LinearRegression::computeRegressionWeighted(double confidence_interval_P, Iterator x_begin, Iterator x_end, Iterator y_begin, Iterator w_begin)
    {
      int N = int(distance(x_begin, x_end));

      double * X = new double[N];
      double * Y = new double[N];
      double * W = new double[N];
      iteratorRange3Arrays_(x_begin, x_end, y_begin, w_begin, X, Y, W);

      double cov00, cov01, cov11;

      // Compute the weighted linear fit.
      // Get the intercept and the slope of the regression Y_hat=intercept_+slope_*X
      // and the value of Chi squared, the covariances of the intercept and the slope
      int error = gsl_fit_wlinear(X, 1, W, 1, Y, 1, N, &intercept_, &slope_, &cov00, &cov01, &cov11, &chi_squared_);

      if (!error)
      {
        computeGoodness_(X, Y, N, confidence_interval_P);
      }

      delete[] X;
      delete[] Y;
      delete[] W;

      if (error)
      {
        throw Exception::UnableToFit(__FILE__, __LINE__, __PRETTY_FUNCTION__, "UnableToFit-LinearRegression", "Could not fit a linear model to the data");
      }
    }

    template <typename Iterator>
    void LinearRegression::iteratorRange2Arrays_(Iterator x_begin, Iterator x_end, Iterator y_begin, double * x_array, double * y_array)
    {
      int i = 0;
      while (x_begin < x_end)
      {
        x_array[i] = *x_begin;
        y_array[i] = *y_begin;
        ++x_begin;
        ++y_begin;
        ++i;
      }
    }

    template <typename Iterator>
    void LinearRegression::iteratorRange3Arrays_(Iterator x_begin, Iterator x_end, Iterator y_begin, Iterator w_begin, double * x_array, double * y_array, double * w_array)
    {
      int i = 0;
      while (x_begin < x_end)
      {
        x_array[i] = *x_begin;
        y_array[i] = *y_begin;
        w_array[i] = *w_begin;
        ++x_begin;
        ++y_begin;
        ++w_begin;
        ++i;
      }
    }

  } // namespace Math
} // namespace OpenMS

#endif