#include <stdio.h>
#include <stdlib.h>
#include <gsl/gsl_sf_expint.h>
#include <math.h>

/* Calculate the exponential integral function by invoking the GNU scientific
   library */
/* Only intended to be used with R; not the most elegant integration but it
   may do for now */

/* One argument, the value (double) at which to evaluate Ei(x) */

double x, ei; /* argument to exponential integral, value */
char *program_name; /* name program is invoked under, for errors */

int main(int argc, char* argv[]) {
  void usage(void);	/* Warn users about proper usage */

  program_name = argv[0];
  if (argc != 2) {
    usage();
  }
  x = atof(&argv[1][0]);
  ei = gsl_sf_expint_E1(x);
  printf("%.18e\n",ei);
  return(0);
}

void usage(void) {
  (void) fprintf(stderr, "Usage is %s [floating-point argument]\n", program_name);
  exit(8);
}


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

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

/* * Redistributions of source code must retain the above copyright notice, */
/* this list of conditions and the following disclaimer. */
/* * Redistributions in binary form must reproduce the above copyright notice, */
/* this list of conditions and the following disclaimer in the documentation */
/* and/or other materials provided with the distribution. */
/* * None of the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. */

/* THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS "AS IS" AND */
/* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED */
/* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE */
/* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE */
/* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL */
/* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR */
/* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER */
/* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, */
/* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE */
/* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */

#ifndef UKF_MATH_H
#define UKF_MATH_H

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

#include <iostream>
#include <cassert>
#include <cmath>

namespace ukf
{
    namespace math
    {

        inline double min(double a, double b)
        {
            return a < b ? a : b;
        }

        inline double max(double a, double b)
        {
            return a < b ? b : a;
        }

        inline double signof(double x)
        {
            return x <= 0.0 ? -1.0 : 1.0;
        }

        inline bool cmp_equal(double x, double y)
        {
            return fabs(x - y) <= 1e-10;
        }
        inline bool cmp_diff(double x, double y)
        {
            return fabs(x - y) >= 1e-10;
        }

        /**
         * @brief This function performs a cholesky update according to Strange et al.(2007)
         * @author Mathieu.Geist@Supelec.fr
         *
         */
        void choleskyUpdate(gsl_matrix * sigmaTheta, double alpha, gsl_vector *x) {

            /*
                This function performs a cholesky update of the
                cholesky factorization sigmaTheta, that is it replaces
                the Cholesky factorization sigmaTheta by the cholesky factorization of

                            sigmaTheta*sigmaTheta^T + alpha * x * x^T

                The algorithm is an adaptation of a LU factorization rank one update.

                Reference is :

                Peter Strange, Andreas Griewank and Matthias Bollhöfer.
                On the Efficient Update of Rectangular LU Factorizations subject to Low Rank Modifications.
                Electronic Transactions on Numerical Analysis, 26:161-177, 2007.
                alg. is given in left part of fig.2.1.

                Perhaps a more efficient algorithm exists, however it should do the work for now. And the code is probably not optimal...
            */

            if (sigmaTheta->size1 != sigmaTheta->size2)
                std::cout<<"ERROR ukf::math::choleskyUpdate Cannot use CholeskyUpdate on non-squared matrices"<<std::endl ;

            int i,j,n= sigmaTheta->size1 ;
            double tmp ;
            gsl_matrix *U = gsl_matrix_alloc(n,n) ;
            gsl_vector *D = gsl_vector_alloc(n) ;
            gsl_vector *y = gsl_vector_alloc(n) ;

            // A first thing is to set SS' (chol factor) in a LU form, L being unitriangular
            // Compute U = L^T and D = diag(L)
            gsl_matrix_set_zero(U) ;
            for (i=0; i<n; i++){
                gsl_vector_set(D,i,gsl_matrix_get(sigmaTheta,i,i));
                for (j=0; j<=i; j++){
                    gsl_matrix_set(U,j,i,gsl_matrix_get(sigmaTheta,i,j));
                }
            }
            // Replace L by L*D^{-1} and U by D*U
            for (i=0; i<n; i++){
                for (j=0; j<=i; j++){
                    tmp = gsl_matrix_get(sigmaTheta,i,j);
                    tmp /= gsl_vector_get(D,j);
                    gsl_matrix_set(sigmaTheta,i,j,tmp);
                    tmp = gsl_matrix_get(U,j,i);
                    tmp *= gsl_vector_get(D,j);
                    gsl_matrix_set(U,j,i,tmp);
                }
            }

            // compute the y = alpha x vector
            gsl_vector_memcpy(y,x) ;
            gsl_vector_scale(y,alpha) ;

            // perform the rank 1 LU modification
            for (i=0; i<n; i++){

                // diagonal update
                tmp = gsl_matrix_get(U,i,i) +
                      gsl_vector_get(x,i)*gsl_vector_get(y,i);
                gsl_matrix_set(U,i,i,tmp);
                tmp = gsl_vector_get(y,i);
                tmp /= gsl_matrix_get(U,i,i);
                gsl_vector_set(y,i,tmp);

                for (j=i+1; j<n; j++){
                    // L (that is sigmaTheta) update
                    tmp = gsl_vector_get(x,j) -
                          gsl_vector_get(x,i)*gsl_matrix_get(sigmaTheta,j,i);
                    gsl_vector_set(x,j,tmp) ;
                    tmp = gsl_matrix_get(sigmaTheta,j,i) + gsl_vector_get(y,i) * gsl_vector_get(x,j);
                    gsl_matrix_set(sigmaTheta,j,i,tmp);
                }

                for (j=i+1; j<n; j++) {
                    // U update
                    tmp = gsl_matrix_get(U,i,j) +
                          gsl_vector_get(x,i)*gsl_vector_get(y,j);
                    gsl_matrix_set(U,i,j,tmp) ;
                    tmp = gsl_vector_get(y,j) -
                          gsl_vector_get(y,i) * gsl_matrix_get(U,i,j);
                    gsl_vector_set(y,j,tmp) ;
                }
            }

            // Now we want the chol decomposition
            // first D = sqrt(diag(U)) ;
            for (i=0; i<n; i++){
                tmp =  gsl_matrix_get(U,i,i) ;

                if (tmp<=0){std::cout<< "WARNING ukf::math::choleskyUpdate::matrix not positive definite  : " << tmp << std::endl;}
                gsl_vector_set(D,i,sqrt(tmp)) ;
            }
            // then L = L*D ;
            for (i=0; i<n; i++) {
                for (j=0; j<n; j++) {
                    tmp = gsl_matrix_get(sigmaTheta,i,j) * gsl_vector_get(D,j);
                    gsl_matrix_set(sigmaTheta,i,j,tmp);
                }
            }
            // that's all folks !

            //free memory
            gsl_matrix_free(U);
            gsl_vector_free(y);
            gsl_vector_free(D);
        }
    }  // math
}  // ukf

#endif  // UKF_MATH_H


// MILOS v2.0 (2020)
// Milne-Eddington inversion code (based on IDL MILOS code by D. Orozco)
// Manuel Cabrera, Juan P. Cobos, Luis Bellot (IAA-CSIC)
// For questions, please contact lbellot@iaa.es
//

/*
;      eta0 = line-to-continuum absorption coefficient ratio
;      B = magnetic field strength       [Gauss]
;      vlos = line-of-sight velocity     [km/s]
;      dopp = Doppler width              [Angstroms]
;      aa = damping parameter
;      gm = magnetic field inclination   [deg]
;      az = magnetic field azimuth       [deg]
;      S0 = source function constant
;      S1 = source function gradient
;      mac = macroturbulent velocity     [km/s]
;      alpha = filling factor of the magnetic component [0->1]

*/

#include <time.h>
#include "defines.h"
#include <string.h>
#include <stdio.h>
#include "/opt/local/cfitsio/cfitsio-3.350/include/fitsio.h" ///opt/local/cfitsio/cfitsio-3.350/include/
#include "utilsFits.h"
#include "milosUtils.h"
#include "lib.h"
#include "readConfig.h"
#include <unistd.h>
#include <complex.h>
#include <fftw3.h> //siempre a continuacion de complex.h
#include <gsl/gsl_spline.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_eigen.h>
#include <libgen.h>

int NTERMS = 11;
Cuantic *cuantic; // Variable global, está hecho así, de momento,para parecerse al original

PRECISION **PUNTEROS_CALCULOS_COMPARTIDOS;
int POSW_PUNTERO_CALCULOS_COMPARTIDOS;
int POSR_PUNTERO_CALCULOS_COMPARTIDOS;

REAL *dtaux, *etai_gp3, *ext1, *ext2, *ext3, *ext4;
REAL *gp1, *gp2, *dt, *dti, *gp3, *gp4, *gp5, *gp6, *etai_2;
REAL *gp4_gp2_rhoq, *gp5_gp2_rhou, *gp6_gp2_rhov;
REAL *dgp1, *dgp2, *dgp3, *dgp4, *dgp5, *dgp6, *d_dt;
REAL *d_ei, *d_eq, *d_eu, *d_ev, *d_rq, *d_ru, *d_rv;
REAL *dfi, *dshi;
REAL CC, CC_2, sin_gm, azi_2, sinis, cosis, cosis_2, cosi, sina, cosa, sinda, cosda, sindi, cosdi, sinis_cosa, sinis_sina;
REAL *fi_p, *fi_b, *fi_r, *shi_p, *shi_b, *shi_r;
REAL *etain, *etaqn, *etaun, *etavn, *rhoqn, *rhoun, *rhovn;
REAL *etai, *etaq, *etau, *etav, *rhoq, *rhou, *rhov;
REAL *parcial1, *parcial2, *parcial3;
REAL *nubB, *nupB, *nurB;
REAL **uuGlobalInicial;
REAL **HGlobalInicial;
REAL **FGlobalInicial;

PRECISION *GMAC,* GMAC_DERIV;
PRECISION * dirConvPar;
REAL *resultConv;
PRECISION * G = NULL;

REAL * opa;
int FGlobal, HGlobal, uuGlobal;

REAL *d_spectra, *spectra, *spectra_mac, *spectra_slight;

// GLOBAL variables to use for FFT calculation
fftw_complex * inSpectraFwPSF, *inSpectraBwPSF, *outSpectraFwPSF, *outSpectraBwPSF;
fftw_complex * inSpectraFwMAC, *inSpectraBwMAC, *outSpectraFwMAC, *outSpectraBwMAC;
fftw_plan planForwardPSF, planBackwardPSF;
fftw_plan planForwardMAC, planBackwardMAC;
fftw_complex * inFilterMAC, * inFilterMAC_DERIV, * outFilterMAC, * outFilterMAC_DERIV;
fftw_plan planFilterMAC, planFilterMAC_DERIV;

fftw_complex * fftw_G_PSF, * fftw_G_MAC_PSF, * fftw_G_MAC_DERIV_PSF;
fftw_complex * inPSF_MAC, * inMulMacPSF, * inPSF_MAC_DERIV, *inMulMacPSFDeriv, *outConvFilters, * outConvFiltersDeriv;
fftw_plan planForwardPSF_MAC, planForwardPSF_MAC_DERIV,planBackwardPSF_MAC, planBackwardPSF_MAC_DERIV;

//Convolutions values
int sizeG = 0;
PRECISION FWHM = 0;

ConfigControl configCrontrolFile;

// fvoigt memory consuption
 _Complex double *z,* zden, * zdiv;

gsl_vector *eval;
gsl_matrix *evec;
gsl_eigen_symmv_workspace * workspace;

int main(int argc, char **argv)
{
	int i; // for indexes
	PRECISION *wlines;
	int nlambda;
	Init_Model *vModels;
	float chisqrf, * vChisqrf;
	int * vNumIter; // to store the number of iterations used to converge for each pixel
	int indexLine, free_params; // index to identify central line to read it

	//*****
	Init_Model INITIAL_MODEL;
	PRECISION * deltaLambda, * PSF;
	PRECISION initialLambda, step, finalLambda;
	int N_SAMPLES_PSF;
	int posWL=0;
	//----------------------------------------------

	float * slight = NULL;
	int nl_straylight, ns_straylight, nx_straylight=0,ny_straylight=0;
	const char  * nameInputFileSpectra ;
	char nameOutputFilePerfiles [4096];
	const char	* nameInputFileLines;
	const char	* nameInputFilePSF ;
    FitsImage * fitsImage;
	PRECISION  dat[7];

	/********************* Read data input from file ******************************/

	/* Read data input from file */

	loadInitialValues(&configCrontrolFile);
	readTrolFile(argv[1],&configCrontrolFile,1);

	nameInputFileSpectra = configCrontrolFile.ObservedProfiles;
	nameInputFileLines = configCrontrolFile.AtomicParametersFile;

	nameInputFilePSF = configCrontrolFile.PSFFile;
	FWHM = configCrontrolFile.FWHM;

	/***************** READ INIT MODEL ********************************/
	if(configCrontrolFile.InitialGuessModel[0]!='\0' && !readInitialModel(&INITIAL_MODEL,configCrontrolFile.InitialGuessModel)){
		printf("\nERROR READING GUESS MODEL 1 FILE\n");
		exit(EXIT_FAILURE);
	}
	checkInitialModel(&INITIAL_MODEL);

	if(INITIAL_MODEL.alfa<1 && access(configCrontrolFile.StrayLightFile,F_OK)){
		printf("\nERROR. Filling factor in Initial model is less than 1 and Stray Light file  %s can not be accessed\n",configCrontrolFile.StrayLightFile);
		exit(EXIT_FAILURE);
	}

	if(configCrontrolFile.fix[10]==0) NTERMS--;
	if(INITIAL_MODEL.mac ==0 && configCrontrolFile.fix[9]==0){
		 NTERMS--;
	}

	// allocate memory for eigen values
	eval = gsl_vector_alloc (NTERMS);
  	evec = gsl_matrix_alloc (NTERMS, NTERMS);
	workspace = gsl_eigen_symmv_alloc (NTERMS);

	/***************** READ WAVELENGHT FROM GRID OR FITS ********************************/
	PRECISION * vLambda, *vOffsetsLambda;

	if(configCrontrolFile.useMallaGrid){ // read lambda from grid file
		printf("\n--------------------------------------------------------------------------------");
		printf("\nMALLA GRID FILE READ: %s",configCrontrolFile.MallaGrid);
		printf("\n--------------------------------------------------------------------------------");
		indexLine = readMallaGrid(configCrontrolFile.MallaGrid, &initialLambda, &step, &finalLambda, 1);
		printf("--------------------------------------------------------------------------------\n");
		nlambda = ((finalLambda-initialLambda)/step)+1;
		vOffsetsLambda = calloc(nlambda,sizeof(PRECISION));
		vOffsetsLambda[0] = initialLambda;
		for(i=1;i<nlambda;i++){
			vOffsetsLambda[i] = vOffsetsLambda[i-1]+step;
		}
		// pass to armstrong
		initialLambda = initialLambda/1000.0;
		step = step/1000.0;
		finalLambda = finalLambda/1000.0;
		vLambda = calloc(nlambda,sizeof(PRECISION));

		printf("Number of wavelengths in the wavelength grid: %d",nlambda);
		printf("\n--------------------------------------------------------------------------------\n");
		printf("\n--------------------------------------------------------------------------------");
		printf("\nATMOSPHERE LINES FILE READ: %s",nameInputFileLines);
		configCrontrolFile.CentralWaveLenght = readFileCuanticLines(nameInputFileLines,dat,indexLine,1);
		if(configCrontrolFile.CentralWaveLenght==0){
			printf("\n QUANTUM LINE NOT FOUND, REVIEW IT. INPUT CENTRAL WAVE LENGHT: %f",configCrontrolFile.CentralWaveLenght);
			exit(1);
		}
		vLambda[0]=configCrontrolFile.CentralWaveLenght+(initialLambda);
		for(i=1;i<nlambda;i++){
			vLambda[i]=vLambda[i-1]+step;
		}
		/******************* CREATE CUANTINC AND INITIALIZE DINAMYC MEMORY*******************/
		cuantic = create_cuantic(dat,1);

	}
	else{ // read lambda from fits file
		printf("\n--------------------------------------------------------------------------------");
		printf("\nWAVELENGTH FILE READ: %s",configCrontrolFile.WavelengthFile);
		vLambda = readFitsLambdaToArray(configCrontrolFile.WavelengthFile,&indexLine,&nlambda);
		if(vLambda==NULL){
			printf("\n FILE WITH WAVELENGHT HAS NOT BEEN READ PROPERLY, please check it.\n");
			free(vLambda);
			exit(EXIT_FAILURE);
		}
		printf("--------------------------------------------------------------------------------\n");
		printf("Number of wavelengths in the wavelength file: %d",nlambda);
		printf("\n--------------------------------------------------------------------------------\n");
		printf("\n--------------------------------------------------------------------------------");
		printf("\nATMOSPHERE LINES FILE READ: %s",nameInputFileLines);
		configCrontrolFile.CentralWaveLenght = readFileCuanticLines(nameInputFileLines,dat,indexLine,1);
		if(configCrontrolFile.CentralWaveLenght==0){
			printf("\n QUANTUM LINE NOT FOUND, REVIEW IT. INPUT CENTRAL WAVE LENGHT: %f",configCrontrolFile.CentralWaveLenght);
			exit(1);
		}
		/******************* CREATE CUANTINC AND INITIALIZE DINAMYC MEMORY*******************/
		cuantic = create_cuantic(dat,1);

	}

	/*********************************************** INITIALIZE VARIABLES  *********************************/
	REAL * vSigma = malloc((nlambda*NPARMS)*sizeof(REAL));
	for(i=0;i<nlambda*NPARMS;i++){
		vSigma[i] = configCrontrolFile.noise;
	}

	CC = PI / 180.0;
	CC_2 = CC * 2;

	wlines = (PRECISION *)calloc(2, sizeof(PRECISION));
	wlines[0] = 1;
	wlines[1] = configCrontrolFile.CentralWaveLenght;

	// count how many free param we have
	free_params=0;
	for(i=0;i<11;i++){
		if(configCrontrolFile.fix[i])
			free_params++;
	}

	/****************************************************************************************************/
	int numln=nlambda;
	// MACROTURBULENCE PLANS
	inFilterMAC = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * numln);
	outFilterMAC = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * numln);
	planFilterMAC = fftw_plan_dft_1d(numln, inFilterMAC, outFilterMAC, FFT_FORWARD, FFTW_MEASURE );
	inFilterMAC_DERIV = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * numln);
	outFilterMAC_DERIV = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * numln);
	planFilterMAC_DERIV = fftw_plan_dft_1d(numln, inFilterMAC_DERIV, outFilterMAC_DERIV, FFT_FORWARD, FFTW_MEASURE );

	inSpectraFwMAC = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * numln);
	outSpectraFwMAC = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * numln);
	planForwardMAC = fftw_plan_dft_1d(numln, inSpectraFwMAC, outSpectraFwMAC, FFT_FORWARD, FFTW_MEASURE );
	inSpectraBwMAC = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * numln);
	outSpectraBwMAC = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * numln);
	planBackwardMAC = fftw_plan_dft_1d(numln, inSpectraBwMAC, outSpectraBwMAC, FFT_BACKWARD, FFTW_MEASURE );

	// ********************************************* IF PSF HAS BEEN SELECTED IN TROL READ PSF FILE OR CREATE GAUSSIAN FILTER ***********//
	if(configCrontrolFile.ConvolveWithPSF){

		if(configCrontrolFile.FWHM > 0){
			G = fgauss_WL(FWHM,vLambda[1]-vLambda[0],vLambda[0],vLambda[nlambda/2],nlambda,&sizeG);
			//char nameAux [4096];
			//char obsAux [4096];
			//if(configCrontrolFile.ObservedProfiles[0]!='\0'){
			//	strcpy(obsAux,configCrontrolFile.ObservedProfiles);
			//	strcpy(nameAux,dirname(obsAux));
			//}
			//else{
			//	strcpy(obsAux,configCrontrolFile.InitialGuessModel);
			//	strcpy(nameAux,dirname(obsAux));
			//}
			//strcat(nameAux,"/gaussian.psf");
			//FILE *fptr = fopen(nameAux, "w");

			FILE *fptr = fopen("gaussian.psf", "w");

			if(fptr!=NULL){
			        printf("\n Gaussian PSF will be saved to file gaussian.psf");
				int kk;
				for (kk = 0; kk < nlambda; kk++)
				{
					fprintf(fptr,"\t%f\t%e\n", (vLambda[kk]-configCrontrolFile.CentralWaveLenght)*1000, G[kk]);
				}
				fclose(fptr);
			}
			else{
			  //	printf("\n ERROR !!! The output file cannot be opened: %s",nameAux);
				printf("\n ERROR !!! The output file cannot be opened: gaussian.psf");
			}
		}else{
			// read the number of lines
			FILE *fp;
			char ch;
			N_SAMPLES_PSF=0;
			//open file in read more
			fp=fopen(nameInputFilePSF,"r");
			if(fp==NULL)
			{
				printf("File \"%s\" does not exist!!!\n",nameInputFilePSF);
				return 0;
			}

			//read character by character and check for new line
			while((ch=fgetc(fp))!=EOF)
			{
				if(ch=='\n')
					N_SAMPLES_PSF++;
			}

			//close the file
			fclose(fp);
			if(N_SAMPLES_PSF>0){
				deltaLambda = calloc(N_SAMPLES_PSF,sizeof(PRECISION));
				PSF = calloc(N_SAMPLES_PSF,sizeof(PRECISION));
				readPSFFile(deltaLambda,PSF,nameInputFilePSF,configCrontrolFile.CentralWaveLenght);
				// CHECK if values of deltaLambda are in the same range of vLambda. For do that we truncate to 4 decimal places
				if( (trunc(vOffsetsLambda[0])) < (trunc(deltaLambda[0]))  || (trunc(vOffsetsLambda[nlambda-1])) > (trunc(deltaLambda[N_SAMPLES_PSF-1])) ){
					printf("\n\n ERROR: The wavelength range given in the PSF file is smaller than the range in the mesh file [%lf,%lf] [%lf,%lf]  \n\n",deltaLambda[0],vOffsetsLambda[0],deltaLambda[N_SAMPLES_PSF-1],vOffsetsLambda[nlambda-1]);
					exit(EXIT_FAILURE);
				}
				G = malloc(nlambda * sizeof(PRECISION));

				double offset=0;
				for(i=0;i<nlambda && !posWL;i++){
					if( fabs(trunc(vOffsetsLambda[i]))==0)
						posWL = i;
				}
				if(posWL!= (nlambda/2) ){ // move center to the middle of samples
					offset = ((  ((nlambda)/2) - posWL)*step)*1000;
				}

				interpolationLinearPSF(deltaLambda,  PSF, vOffsetsLambda ,N_SAMPLES_PSF, G, nlambda,offset);

				sizeG = nlambda;
			}
			else{
				printf("\n****************** ERROR THE PSF FILE is empty or damaged.******************\n");
				exit(EXIT_FAILURE);
			}
			printf("\n--------------------------------------------------------------------------------");
			printf("\nPSF FILE READ: %s", nameInputFilePSF);
			printf("\n--------------------------------------------------------------------------------\n");
		}

		//PSF FILTER PLANS
		inSpectraFwPSF = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * numln);
		outSpectraFwPSF = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * numln);
		planForwardPSF = fftw_plan_dft_1d(numln, inSpectraFwPSF, outSpectraFwPSF, FFT_FORWARD, FFTW_MEASURE );
		inSpectraBwPSF = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * numln);
		outSpectraBwPSF = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * numln);
		planBackwardPSF = fftw_plan_dft_1d(numln, inSpectraBwPSF, outSpectraBwPSF, FFT_BACKWARD, FFTW_MEASURE );

		fftw_complex * in = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * numln);
		int i;
		for (i = 0; i < numln; i++)
		{
			in[i] = G[i] + 0 * _Complex_I;
		}
		fftw_G_PSF = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * numln);
		fftw_plan p = fftw_plan_dft_1d(numln, in, fftw_G_PSF, FFT_FORWARD, FFTW_MEASURE );
		fftw_execute(p);
		for (i = 0; i < numln; i++)
		{
			fftw_G_PSF[i] = fftw_G_PSF[i] / numln;
		}
		fftw_destroy_plan(p);
		fftw_free(in);

		inPSF_MAC = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * numln);
		fftw_G_MAC_PSF = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * numln);
		planForwardPSF_MAC = fftw_plan_dft_1d(numln, inPSF_MAC, fftw_G_MAC_PSF, FFT_FORWARD, FFTW_MEASURE );
		inMulMacPSF = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * numln);
		outConvFilters = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * numln);
		planBackwardPSF_MAC = fftw_plan_dft_1d(numln, inMulMacPSF, outConvFilters, FFT_BACKWARD, FFTW_MEASURE );

		inPSF_MAC_DERIV = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * numln);
		fftw_G_MAC_DERIV_PSF = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * numln);
		planForwardPSF_MAC_DERIV = fftw_plan_dft_1d(numln, inPSF_MAC_DERIV, fftw_G_MAC_DERIV_PSF, FFT_FORWARD, FFTW_MEASURE );
		inMulMacPSFDeriv = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * numln);
		outConvFiltersDeriv = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * numln);
		planBackwardPSF_MAC_DERIV = fftw_plan_dft_1d(numln, inMulMacPSFDeriv, outConvFiltersDeriv, FFT_BACKWARD, FFTW_MEASURE );

	}
	/****************************************************************************************************/
	//  IF NUMBER OF CYCLES IS LES THAN 0 THEN --> WE USE CLASSICAL ESTIMATES
	//  IF NUMBER OF CYCLES IS 0 THEN -->  DO SYNTHESIS FROM THE INIT MODEL
	//  IF NUMBER OF CYCLES IS GREATER THAN 0 --> READ FITS FILE OR PER FILE AND PROCESS DO INVERSION WITH N CYCLES

	if(configCrontrolFile.NumberOfCycles<0){
		// read fits or per

		AllocateMemoryDerivedSynthesis(nlambda);
		if(strcmp(file_ext(configCrontrolFile.ObservedProfiles),PER_FILE)==0){ // invert only per file
			float * spectroPER = calloc(nlambda*NPARMS,sizeof(float));
			FILE * fReadSpectro;
			char * line = NULL;
			size_t len = 0;
			ssize_t read;
			fReadSpectro = fopen(configCrontrolFile.ObservedProfiles, "r");

			int contLine=0;
			if (fReadSpectro == NULL)
			{
				printf("Error opening the file of parameters, it's possible that the file doesn't exist. Please verify it. \n");
				printf("\n ******* THIS IS THE NAME OF THE FILE RECEVIED : %s \n", configCrontrolFile.ObservedProfiles);
				fclose(fReadSpectro);
				exit(EXIT_FAILURE);
			}
			float aux1, aux2,aux3,aux4,aux5,aux6;
			while ((read = getline(&line, &len, fReadSpectro)) != -1 && contLine<nlambda) {
				sscanf(line,"%e %e %e %e %e %e",&aux1,&aux2,&aux3,&aux4,&aux5,&aux6);
				spectroPER[contLine] = aux3;
				spectroPER[contLine + nlambda] = aux4;
				spectroPER[contLine + nlambda * 2] = aux5;
				spectroPER[contLine + nlambda * 3] = aux6;
				contLine++;
			}
			fclose(fReadSpectro);
			printf("\n--------------------------------------------------------------------------------");
			printf("\nOBSERVED PROFILES FILE READ: %s ", configCrontrolFile.ObservedProfiles);
			printf("\n--------------------------------------------------------------------------------\n");
			Init_Model initModel;
			initModel.eta0 = 0;
			initModel.mac = 0;
			initModel.dopp = 0;
			initModel.aa = 0;
			initModel.alfa = 0; //0.38; //stray light factor
			initModel.S1 = 0;
			//invert with classical estimates
			estimacionesClasicas(wlines[1], vLambda, nlambda, spectroPER, &initModel,0);
			// save model to file
			char nameAuxOutputModel [4096];
			if(configCrontrolFile.ObservedProfiles[0]!='\0')
				strcpy(nameAuxOutputModel,get_basefilename(configCrontrolFile.ObservedProfiles));
			else
				strcpy(nameAuxOutputModel,get_basefilename(configCrontrolFile.InitialGuessModel));

			strcat(nameAuxOutputModel,"_model_ce");
			strcat(nameAuxOutputModel,MOD_FILE);
			FILE * fptr = fopen(nameAuxOutputModel, "w");
			if(fptr!=NULL){
				fprintf(fptr,"eta_0               :%lf\n",initModel.eta0);
				fprintf(fptr,"magnetic field [G]  :%lf\n",initModel.B);
				fprintf(fptr,"LOS velocity[km/s]  :%lf\n",initModel.vlos);
				fprintf(fptr,"Doppler width [A]   :%lf\n",initModel.dopp);
				fprintf(fptr,"damping             :%lf\n",initModel.aa);
				fprintf(fptr,"gamma [deg]         :%lf\n",initModel.gm);
				fprintf(fptr,"phi  [deg]          :%lf\n",initModel.az);
				fprintf(fptr,"S_0                 :%lf\n",initModel.S0);
				fprintf(fptr,"S_1                 :%lf\n",initModel.S1);
				fprintf(fptr,"v_mac               :%lf\n",initModel.mac);
				fprintf(fptr,"filling factor      :%lf\n",initModel.alfa);
				fprintf(fptr,"# Iterations        :%d\n",0);
				fprintf(fptr,"chisqr              :%le\n",0.0);
				fprintf(fptr,"\n\n");
				fclose(fptr);
			}
			else{
				printf("\n ¡¡¡¡¡ ERROR: OUTPUT MODEL FILE CAN NOT BE OPENED: %s \n !!!!!",nameAuxOutputModel);
			}
			free(spectroPER);
		}
		else if(strcmp(file_ext(configCrontrolFile.ObservedProfiles),FITS_FILE)==0){ // invert image from fits file
			fitsImage = readFitsSpectroImage(configCrontrolFile.ObservedProfiles,0,nlambda);
			printf("\n--------------------------------------------------------------------------------");
			printf("\nOBSERVED PROFILES FILE READ: %s", configCrontrolFile.ObservedProfiles);
			printf("\n--------------------------------------------------------------------------------\n");
			// ALLOCATE MEMORY FOR STORE THE RESULTS
			int indexPixel = 0;
			vModels = calloc (fitsImage->numPixels , sizeof(Init_Model));
			vChisqrf = calloc (fitsImage->numPixels , sizeof(float));
			vNumIter = calloc (fitsImage->numPixels , sizeof(int));
			for(indexPixel = 0; indexPixel < fitsImage->numPixels; indexPixel++){

				//Initial Model
				Init_Model initModel;
				initModel.eta0 = INITIAL_MODEL.eta0;
				initModel.B = INITIAL_MODEL.B; //200 700
				initModel.gm = INITIAL_MODEL.gm;
				initModel.az = INITIAL_MODEL.az;
				initModel.vlos = INITIAL_MODEL.vlos; //km/s 0
				initModel.mac = INITIAL_MODEL.mac;
				initModel.dopp = INITIAL_MODEL.dopp;
				initModel.aa = INITIAL_MODEL.aa;
				initModel.alfa = INITIAL_MODEL.alfa; //0.38; //stray light factor
				initModel.S0 = INITIAL_MODEL.S0;
				initModel.S1 = INITIAL_MODEL.S1;
				estimacionesClasicas(wlines[1],vLambda, nlambda, fitsImage->pixels[indexPixel].spectro, &initModel,0);
				vModels[indexPixel] = initModel;

			}
			char nameAuxOutputModel [4096];
			if(configCrontrolFile.ObservedProfiles[0]!='\0')
				strcpy(nameAuxOutputModel,get_basefilename(configCrontrolFile.ObservedProfiles));
			else
				strcpy(nameAuxOutputModel,get_basefilename(configCrontrolFile.InitialGuessModel));
			strcat(nameAuxOutputModel,"_model_ce");
			strcat(nameAuxOutputModel,FITS_FILE);
			if(!writeFitsImageModels(nameAuxOutputModel,fitsImage->rows,fitsImage->cols,vModels,vChisqrf,vNumIter,configCrontrolFile.saveChisqr)){
					printf("\n ERROR WRITING FILE OF MODELS: %s",nameAuxOutputModel);
			}
			free(vModels);
			free(vChisqrf);
			free(vNumIter);
		}
		else{
			printf("\n OBSERVED PROFILES DOESN'T HAVE CORRECT EXTENSION  .PER or .FITS ");
			exit(EXIT_FAILURE);
		}
	}
	else if(configCrontrolFile.NumberOfCycles==0){ // synthesis

		if(access(configCrontrolFile.StrayLightFile,F_OK)!=-1){ //  IF NOT EMPTY READ stray light file
			if(strcmp(file_ext(configCrontrolFile.StrayLightFile),PER_FILE)==0){
				slight = readPerStrayLightFile(configCrontrolFile.StrayLightFile,nlambda,vOffsetsLambda);
				printf("\n--------------------------------------------------------------------------------");
				printf("\nSTRAY LIGHT FILE READ: %s ", configCrontrolFile.StrayLightFile);
				printf("\n--------------------------------------------------------------------------------\n");
			}
			else if(strcmp(file_ext(configCrontrolFile.StrayLightFile),FITS_FILE)==0){
				slight= readFitsStrayLightFile(&configCrontrolFile,&nl_straylight,&ns_straylight,&nx_straylight, &ny_straylight);
				if(nx_straylight!=0 || ny_straylight!=0){
					printf("\n Stray light file has 4 dimensions and for Synthesis only 2 dimensiones file is accepted, henceforth, stray light will not used for synthesis. \n");
					free(slight);
					slight= NULL;
				}
				if(nl_straylight!=nlambda){
					printf("\n The number of wavelengths is different in the stray light file: %d and malla grid file %d. \n. Stray light will not used for synthesis.", nl_straylight,nlambda);
					free(slight);
					slight= NULL;
				}
				printf("\n--------------------------------------------------------------------------------");
				printf("\nSTRAY LIGHT FILE READ: %s ", configCrontrolFile.StrayLightFile);
				printf("\n--------------------------------------------------------------------------------\n");
			}
			else{
				printf("\n Stray light file hasn't extension .PER or .FITS, review it. \n. Stray light will not used for synthesis.\n");
				free(slight);
				slight= NULL;
			}
		}

		Init_Model initModel;
		initModel.eta0 = INITIAL_MODEL.eta0;
		initModel.B = INITIAL_MODEL.B; //200 700
		initModel.gm = INITIAL_MODEL.gm;
		initModel.az = INITIAL_MODEL.az;
		initModel.vlos = INITIAL_MODEL.vlos; //km/s 0
		initModel.mac = INITIAL_MODEL.mac;
		initModel.dopp = INITIAL_MODEL.dopp;
		initModel.aa = INITIAL_MODEL.aa;
		initModel.alfa = INITIAL_MODEL.alfa; //0.38; //stray light factor
		initModel.S0 = INITIAL_MODEL.S0;
		initModel.S1 = INITIAL_MODEL.S1;
		printf("\n--------------------------------------------------------------------------------");
		printf("\nATMOSPHERE MODEL FILE READ: %s ",configCrontrolFile.InitialGuessModel);
		printf("\n--------------------------------------------------------------------------------");
		printf("\nINITAL MODEL ATMOSPHERE: \n\n");
		printf("eta_0               :%lf\n",initModel.eta0);
		printf("magnetic field [G]  :%lf\n",initModel.B);
		printf("LOS velocity[km/s]  :%lf\n",initModel.vlos);
		printf("Doppler width [A]   :%lf\n",initModel.dopp);
		printf("damping             :%lf\n",initModel.aa);
		printf("gamma [deg]         :%lf\n",initModel.gm);
		printf("phi   [deg]         :%lf\n",initModel.az);
		printf("S_0                 :%lf\n",initModel.S0);
		printf("S_1                 :%lf\n",initModel.S1);
		printf("v_mac [km/s]        :%lf\n",initModel.mac);
		printf("filling factor      :%lf\n",initModel.alfa);
		printf("--------------------------------------------------------------------------------\n");

		AllocateMemoryDerivedSynthesis(nlambda);

		if(configCrontrolFile.ConvolveWithPSF && initModel.mac>0){
			printf("\n--------------------------------------------------------------------------------");
			printf("\nThe program needs to use convolution. Filter PSF activated and macroturbulence greater than zero. ");
			printf("\n--------------------------------------------------------------------------------\n");
		}
		else if(configCrontrolFile.ConvolveWithPSF){
			printf("\n--------------------------------------------------------------------------------");
			printf("\nThe program needs to use convolution. Filter PSF activated. ");
			printf("\n--------------------------------------------------------------------------------\n");
		}
		else if(initModel.mac>0){
			printf("\n--------------------------------------------------------------------------------");
			printf("\nThe program needs to use convolution. Macroturbulence in initial atmosphere model greater than zero.");
			printf("\n--------------------------------------------------------------------------------\n");
		}
		// synthesis
		mil_sinrf(cuantic, &initModel, wlines, vLambda, nlambda, spectra, configCrontrolFile.mu, slight,spectra_mac,spectra_slight, configCrontrolFile.ConvolveWithPSF);
		me_der(cuantic, &initModel, wlines, vLambda, nlambda, d_spectra, spectra_mac, spectra_slight, configCrontrolFile.mu, slight, configCrontrolFile.ConvolveWithPSF,configCrontrolFile.fix);

		// in this case basenamefile is from initmodel
		char nameAux [4096];

		if(configCrontrolFile.ObservedProfiles[0]!='\0')
			strcpy(nameAux,get_basefilename(configCrontrolFile.ObservedProfiles));
		else
			strcpy(nameAux,get_basefilename(configCrontrolFile.InitialGuessModel));
		strcat(nameAux,PER_FILE);
		FILE *fptr = fopen(nameAux, "w");
		if(fptr!=NULL){
			int kk;
			for (kk = 0; kk < nlambda; kk++)
			{
				fprintf(fptr,"%d\t%f\t%e\t%e\t%e\t%e\n", indexLine, (vLambda[kk]-configCrontrolFile.CentralWaveLenght)*1000, spectra[kk], spectra[kk + nlambda], spectra[kk + nlambda * 2], spectra[kk + nlambda * 3]);
			}
			fclose(fptr);
			printf("\n--------------------------------------------------------------------------------");
			printf("\n------------------SYNTHESIS DONE: %s",nameAux);
			printf("\n--------------------------------------------------------------------------------\n");
		}
		else{
			printf("\n ERROR !!! The output file can not be open: %s",nameAux);
		}

		/*int number_parametros = 0;
		for (number_parametros = 0; number_parametros < NTERMS; number_parametros++)
		{
			strcpy(nameAux,get_basefilename(configCrontrolFile.InitialGuessModel));
			strcat(nameAux,"_C_");
			char extension[10];
			sprintf(extension, "%d%s", number_parametros,".per");
			strcat(nameAux,extension);
			FILE *fptr = fopen(nameAux, "w");
			//printf("\n FUNCION RESPUESTA: %d \n",number_parametros);
			int kk;
			for (kk = 0; kk < nlambda; kk++)
			{
			fprintf(fptr,"1\t%lf\t%le\t%le\t%le\t%le\n", vLambda[kk],
			d_spectra[kk + nlambda * number_parametros],
			d_spectra[kk + nlambda * number_parametros + nlambda * NTERMS],
			d_spectra[kk + nlambda * number_parametros + nlambda * NTERMS * 2],
			d_spectra[kk + nlambda * number_parametros + nlambda * NTERMS * 3]);
			}
			fclose(fptr);
		}
		printf("\n");*/

	}
	else{ // INVERT PIXEL FROM PER FILE OR IMAGE FROM FITS FILE

		if(strcmp(file_ext(configCrontrolFile.ObservedProfiles),PER_FILE)==0){ // invert only per file
			float * spectroPER = calloc(nlambda*NPARMS,sizeof(float));
			FILE * fReadSpectro;
			char * line = NULL;
			size_t len = 0;
			ssize_t read;
			fReadSpectro = fopen(configCrontrolFile.ObservedProfiles, "r");

			int contLine=0;
			if (fReadSpectro == NULL)
			{
				printf("Error opening the file of parameters, it's possible that the file doesn't exist. Please verify it. \n");
				printf("\n ******* THIS IS THE NAME OF THE FILE RECEVIED : %s \n", configCrontrolFile.ObservedProfiles);
				fclose(fReadSpectro);
				exit(EXIT_FAILURE);
			}

			float aux1, aux2, aux3, aux4, aux5, aux6;
			while ((read = getline(&line, &len, fReadSpectro)) != -1) {
				sscanf(line,"%e %e %e %e %e %e",&aux1,&aux2,&aux3,&aux4,&aux5,&aux6);
				if(contLine<nlambda){
					spectroPER[contLine] = aux3;
					spectroPER[contLine + nlambda] = aux4;
					spectroPER[contLine + nlambda * 2] = aux5;
					spectroPER[contLine + nlambda * 3] = aux6;
				}
				contLine++;
			}
			fclose(fReadSpectro);
			printf("\n--------------------------------------------------------------------------------");
			printf("\nOBSERVED PROFILES FILE READ: %s ", configCrontrolFile.ObservedProfiles);
			printf("\n--------------------------------------------------------------------------------");
			printf("\nNumber of wavelengths in the observed profiles: %d",contLine);
			printf("\n--------------------------------------------------------------------------------\n");
			if(nlambda!=contLine){
				printf("\n--------------------------------------------------------------------------------\n");
				printf("\nERROR: The number of wavelenghts in observed profiles file  %d is different to number of wavelengths in malla grid %d\n",contLine,nlambda);
				exit(EXIT_FAILURE);
			}
			if(configCrontrolFile.fix[10] &&  access(configCrontrolFile.StrayLightFile,F_OK)!=-1){ //  IF NOT EMPTY READ stray light file
				if(strcmp(file_ext(configCrontrolFile.StrayLightFile),PER_FILE)==0){
					slight = readPerStrayLightFile(configCrontrolFile.StrayLightFile,nlambda,vOffsetsLambda);
					printf("\n--------------------------------------------------------------------------------");
					printf("\nSTRAY LIGHT FILE READ: %s ", configCrontrolFile.StrayLightFile);
					printf("\n--------------------------------------------------------------------------------\n");
				}
				else if(strcmp(file_ext(configCrontrolFile.StrayLightFile),FITS_FILE)==0){
					slight= readFitsStrayLightFile(&configCrontrolFile,&nl_straylight,&ns_straylight,&nx_straylight, &ny_straylight);
					if(nx_straylight!=0 || ny_straylight!=0){
						printf("\n Stray light file has 4 dimensions and for Inversion pixel only 2 dimensiones file is accepted, henceforth, stray light will not used for inversion pixel. \n");
						free(slight);
						slight= NULL;
					}
					if(nl_straylight!=nlambda){
						printf("\n The number of wavelengths is different in the stray light file: %d and malla grid file %d. \n. Stray light will not used for inversion pixel.", nl_straylight,nlambda);
						free(slight);
						slight= NULL;
					}
					printf("\n--------------------------------------------------------------------------------");
					printf("\nSTRAY LIGHT FILE READ: %s ", configCrontrolFile.StrayLightFile);
					printf("\n--------------------------------------------------------------------------------\n");
				}
				else{
					printf("\n Stray light file hasn't extension .PER or .FITS, review it. \n. Stray light will not used for inversion pixel.\n");
					free(slight);
					slight= NULL;
				}
			}

			AllocateMemoryDerivedSynthesis(nlambda);
			Init_Model initModel;
			initModel.eta0 = INITIAL_MODEL.eta0;
			initModel.B = INITIAL_MODEL.B; //200 700
			initModel.gm = INITIAL_MODEL.gm;
			initModel.az = INITIAL_MODEL.az;
			initModel.vlos = INITIAL_MODEL.vlos; //km/s 0
			initModel.mac = INITIAL_MODEL.mac;
			initModel.dopp = INITIAL_MODEL.dopp;
			initModel.aa = INITIAL_MODEL.aa;
			initModel.alfa = INITIAL_MODEL.alfa; //0.38; //stray light factor
			initModel.S0 = INITIAL_MODEL.S0;
			initModel.S1 = INITIAL_MODEL.S1;
			printf("\n--------------------------------------------------------------------------------");
			printf("\nATMOSPHERE MODEL FILE READ: %s ",configCrontrolFile.InitialGuessModel);
			printf("\n--------------------------------------------------------------------------------");
			printf("\nINITAL MODEL ATMOSPHERE: \n\n");
			printf("eta_0               :%lf\n",initModel.eta0);
			printf("magnetic field [G]  :%lf\n",initModel.B);
			printf("LOS velocity[km/s]  :%lf\n",initModel.vlos);
			printf("Doppler width [A]   :%lf\n",initModel.dopp);
			printf("damping             :%lf\n",initModel.aa);
			printf("gamma [deg]         :%lf\n",initModel.gm);
			printf("phi   [deg]         :%lf\n",initModel.az);
			printf("S_0                 :%lf\n",initModel.S0);
			printf("S_1                 :%lf\n",initModel.S1);
			printf("v_mac [km/s]        :%lf\n",initModel.mac);
			printf("filling factor      :%lf\n",initModel.alfa);
			printf("--------------------------------------------------------------------------------\n");

			if(configCrontrolFile.ConvolveWithPSF && initModel.mac>0){
				printf("\n--------------------------------------------------------------------------------");
				printf("\nThe program needs to use convolution. Filter PSF activated and macroturbulence greater than zero. ");
				printf("\n--------------------------------------------------------------------------------\n");
			}
			else if(configCrontrolFile.ConvolveWithPSF){
				printf("\n--------------------------------------------------------------------------------");
				printf("\nThe program needs to use convolution. Filter PSF activated. ");
				printf("\n--------------------------------------------------------------------------------\n");
			}
			else if(initModel.mac>0){
				printf("\n--------------------------------------------------------------------------------");
				printf("\nThe program needs to use convolution. Macroturbulence in initial atmosphere model greater than zero.");
				printf("\n--------------------------------------------------------------------------------\n");
			}
      		int numIter =0;
			printf("\n--------------------------------------------------------------------------------");
			printf("\nNumber of free parameters for inversion: %d", free_params);
			printf("\n--------------------------------------------------------------------------------\n");
      		lm_mils(cuantic, wlines, vLambda, nlambda, spectroPER, nlambda, &initModel, spectra, &chisqrf, slight, configCrontrolFile.toplim, configCrontrolFile.NumberOfCycles,
               configCrontrolFile.WeightForStokes, configCrontrolFile.fix, vSigma, configCrontrolFile.noise, configCrontrolFile.InitialDiagonalElement,&configCrontrolFile.ConvolveWithPSF,&numIter,configCrontrolFile.mu, configCrontrolFile.logclambda);

			// SAVE OUTPUT MODEL
			char nameAuxOutputModel [4096];

			if(configCrontrolFile.ObservedProfiles[0]!='\0')
				strcpy(nameAuxOutputModel,get_basefilename(configCrontrolFile.ObservedProfiles));
			else
				strcpy(nameAuxOutputModel,get_basefilename(configCrontrolFile.InitialGuessModel));

			strcat(nameAuxOutputModel,"_model");
			strcat(nameAuxOutputModel,MOD_FILE);

			FILE *fptr = fopen(nameAuxOutputModel, "w");
			if(fptr!=NULL){
				fprintf(fptr,"eta_0               :%lf\n",initModel.eta0);
				fprintf(fptr,"magnetic field [G]  :%lf\n",initModel.B);
				fprintf(fptr,"LOS velocity[km/s]  :%lf\n",initModel.vlos);
				fprintf(fptr,"Doppler width [A]   :%lf\n",initModel.dopp);
				fprintf(fptr,"damping             :%lf\n",initModel.aa);
				fprintf(fptr,"gamma [deg]         :%lf\n",initModel.gm);
				fprintf(fptr,"phi   [deg]         :%lf\n",initModel.az);
				fprintf(fptr,"S_0                 :%lf\n",initModel.S0);
				fprintf(fptr,"S_1                 :%lf\n",initModel.S1);
				fprintf(fptr,"v_mac [km/s]        :%lf\n",initModel.mac);
				fprintf(fptr,"filling factor      :%lf\n",initModel.alfa);
				fprintf(fptr,"# Iterations        :%d\n",numIter);
				fprintf(fptr,"chisqr              :%le\n",chisqrf);
				fprintf(fptr,"\n\n");
				fclose(fptr);
				printf("\n\n--------------------------------------------------------------------------------");
				printf("\nINVERTED MODEL SAVED IN FILE: %s",nameAuxOutputModel);
				printf("\n--------------------------------------------------------------------------------\n");
			}
			else{
				printf("\n ¡¡¡¡¡ ERROR: OUTPUT MODEL FILE CAN NOT BE OPENED\n !!!!! ");
			}

			// SAVE OUTPUT ADJUST SYNTHESIS PROFILES
			if(configCrontrolFile.SaveSynthesisAdjusted){
				char nameAuxOutputStokes [4096];
				if(configCrontrolFile.ObservedProfiles[0]!='\0')
					strcpy(nameAuxOutputStokes,get_basefilename(configCrontrolFile.ObservedProfiles));
				else
					strcpy(nameAuxOutputStokes,get_basefilename(configCrontrolFile.InitialGuessModel));
				strcat(nameAuxOutputStokes,STOKES_PER_EXT);
				FILE *fptr = fopen(nameAuxOutputStokes, "w");
				if(fptr!=NULL){
			      //printf("\n valores de spectro sintetizado\n");
					int kk;
					for (kk = 0; kk < nlambda; kk++)
					{
						fprintf(fptr,"%d\t%f\t%e\t%e\t%e\t%e\n", indexLine, (vLambda[kk]-configCrontrolFile.CentralWaveLenght)*1000, spectra[kk], spectra[kk + nlambda], spectra[kk + nlambda * 2], spectra[kk + nlambda * 3]);
					}
					//printf("\nVALORES DE LAS FUNCIONES RESPUESTA \n");
					fclose(fptr);
					printf("\n--------------------------------------------------------------------------------");
					printf("\nOutput profiles: %s",nameAuxOutputStokes);
					printf("\n--------------------------------------------------------------------------------\n");
				}
				else{
					printf("\n ¡¡¡¡¡ ERROR: OUTPUT SYNTHESIS PROFILE ADJUSTED FILE CAN NOT BE OPENED\n !!!!! ");
				}
			}
			free(spectroPER);
		}
		else if(strcmp(file_ext(configCrontrolFile.ObservedProfiles),FITS_FILE)==0){ // invert image from fits file

			// check if read stray light
			if(configCrontrolFile.fix[10] && access(configCrontrolFile.StrayLightFile,F_OK)!=-1){ //  IF NOT EMPTY READ stray light file
				if(strcmp(file_ext(configCrontrolFile.StrayLightFile),PER_FILE)==0){
					slight = readPerStrayLightFile(configCrontrolFile.StrayLightFile,nlambda,vOffsetsLambda);
					printf("\n--------------------------------------------------------------------------------");
					printf("\nSTRAY LIGHT FILE READ: %s ", configCrontrolFile.StrayLightFile);
					printf("\n--------------------------------------------------------------------------------\n");
				}
				else if(strcmp(file_ext(configCrontrolFile.StrayLightFile),FITS_FILE)==0){
					slight = readFitsStrayLightFile(&configCrontrolFile,&nl_straylight,&ns_straylight,&nx_straylight, &ny_straylight);
					if(nl_straylight!=nlambda){
						printf("\n The number of wavelengths is different in the stray light file: %d and malla grid file %d. \n. Stray light will not used for inversion.", nl_straylight,nlambda);
						free(slight);
						slight= NULL;
						exit(EXIT_FAILURE);
					}
					printf("\n--------------------------------------------------------------------------------");
					printf("\nSTRAY LIGHT FILE READ: %s", configCrontrolFile.StrayLightFile);
					printf("\n--------------------------------------------------------------------------------\n");
				}
				else{
					printf("\n Stray light file hasn't extension .PER or .FITS, review it. \n. Stray light will not used for inversion.\n");
					free(slight);
					slight= NULL;
					exit(EXIT_FAILURE);
				}
			}
			// READ PIXELS FROM IMAGE
			PRECISION timeReadImage;
			clock_t t;
			t = clock();
			fitsImage = readFitsSpectroImage(nameInputFileSpectra,0,nlambda);
			t = clock() - t;
			timeReadImage = ((PRECISION)t)/CLOCKS_PER_SEC; // in seconds
			printf("\n--------------------------------------------------------------------------------");
			printf("\nOBSERVED PROFILES FILE READ: %s", nameInputFileSpectra);
			printf("\n--------------------------------------------------------------------------------");
			printf("\nTIME TO READ FITS IMAGE:  %f seconds to execute ", timeReadImage);
			printf("\n--------------------------------------------------------------------------------\n");

			if(fitsImage!=NULL){
				FitsImage * imageStokesAdjust = NULL;
				if(configCrontrolFile.SaveSynthesisAdjusted){
					imageStokesAdjust = malloc(sizeof(FitsImage));
					imageStokesAdjust->rows = fitsImage->rows;
					imageStokesAdjust->cols = fitsImage->cols;
					imageStokesAdjust->nLambdas = fitsImage->nLambdas;
					imageStokesAdjust->numStokes = fitsImage->numStokes;
					imageStokesAdjust->pos_col = fitsImage->pos_col;
					imageStokesAdjust->pos_row = fitsImage->pos_row;
					imageStokesAdjust->pos_lambda = fitsImage->pos_lambda;
					imageStokesAdjust->pos_stokes_parameters = fitsImage->pos_stokes_parameters;
					imageStokesAdjust->numPixels = fitsImage->numPixels;
					imageStokesAdjust->pixels = calloc(imageStokesAdjust->numPixels, sizeof(vpixels));
					imageStokesAdjust->naxes = fitsImage->naxes;
					imageStokesAdjust->vCard = fitsImage->vCard;
					imageStokesAdjust->vKeyname = fitsImage->vKeyname;
					imageStokesAdjust->nkeys = fitsImage->nkeys;
					imageStokesAdjust->naxis = fitsImage->naxis;
					imageStokesAdjust->bitpix = fitsImage->bitpix;
					for( i=0;i<imageStokesAdjust->numPixels;i++){
						imageStokesAdjust->pixels[i].spectro = calloc ((imageStokesAdjust->numStokes*imageStokesAdjust->nLambdas),sizeof(float));
					}
				}
				printf("\n--------------------------------------------------------------------------------");
				printf("\nATMOSPHERE MODEL FILE READ: %s ",configCrontrolFile.InitialGuessModel);
				printf("\n--------------------------------------------------------------------------------");
				printf("\nINITAL MODEL ATMOSPHERE: \n\n");
				printf("eta_0               :%lf\n",INITIAL_MODEL.eta0);
				printf("magnetic field [G]  :%lf\n",INITIAL_MODEL.B);
				printf("LOS velocity[km/s]  :%lf\n",INITIAL_MODEL.vlos);
				printf("Doppler width [A]   :%lf\n",INITIAL_MODEL.dopp);
				printf("damping             :%lf\n",INITIAL_MODEL.aa);
				printf("gamma [deg]         :%lf\n",INITIAL_MODEL.gm);
				printf("phi   [deg]         :%lf\n",INITIAL_MODEL.az);
				printf("S_0                 :%lf\n",INITIAL_MODEL.S0);
				printf("S_1                 :%lf\n",INITIAL_MODEL.S1);
				printf("v_mac [km/s]        :%lf\n",INITIAL_MODEL.mac);
				printf("filling factor      :%lf\n",INITIAL_MODEL.alfa);
				printf("--------------------------------------------------------------------------------\n");

				if(configCrontrolFile.ConvolveWithPSF && INITIAL_MODEL.mac>0){
					printf("\n--------------------------------------------------------------------------------");
					printf("\nThe program needs to use convolution. Filter PSF activated and macroturbulence greater than zero. ");
					printf("\n--------------------------------------------------------------------------------\n");
				}
				else if(configCrontrolFile.ConvolveWithPSF){
					printf("\n--------------------------------------------------------------------------------");
					printf("\nThe program needs to use convolution. Filter PSF activated. ");
					printf("\n--------------------------------------------------------------------------------\n");
				}
				else if(INITIAL_MODEL.mac>0){
					printf("\n--------------------------------------------------------------------------------");
					printf("\nThe program needs to use convolution. Macroturbulence in initial atmosphere model greater than zero.");
					printf("\n--------------------------------------------------------------------------------\n");
				}
				printf("\n--------------------------------------------------------------------------------");
				printf("\nNumber of free parameters for inversion: %d", free_params);
				printf("\n--------------------------------------------------------------------------------\n");
				//***************************************** INIT MEMORY WITH SIZE OF LAMBDA ****************************************************//
				AllocateMemoryDerivedSynthesis(nlambda);
				int indexPixel = 0;

				// ALLOCATE MEMORY FOR STORE THE RESULTS

				vModels = calloc (fitsImage->numPixels , sizeof(Init_Model));
				vChisqrf = calloc (fitsImage->numPixels , sizeof(float));
				vNumIter = calloc (fitsImage->numPixels , sizeof(int));
				t = clock();
				printf("\n--------------------------------------------------------------------------------");
				printf("\n----------------------- IMAGE INVERSION IN PROGRESS ----------------------------");
				printf("\n--------------------------------------------------------------------------------\n");

				for(indexPixel = 0; indexPixel < fitsImage->numPixels; indexPixel++){

					//Initial Model
					Init_Model initModel;
					initModel.eta0 = INITIAL_MODEL.eta0;
					initModel.B = INITIAL_MODEL.B; //200 700
					initModel.gm = INITIAL_MODEL.gm;
					initModel.az = INITIAL_MODEL.az;
					initModel.vlos = INITIAL_MODEL.vlos; //km/s 0
					initModel.mac = INITIAL_MODEL.mac;
					initModel.dopp = INITIAL_MODEL.dopp;
					initModel.aa = INITIAL_MODEL.aa;
					initModel.alfa = INITIAL_MODEL.alfa; //0.38; //stray light factor
					initModel.S0 = INITIAL_MODEL.S0;
					initModel.S1 = INITIAL_MODEL.S1;

					// CLASSICAL ESTIMATES TO GET B, GAMMA
					estimacionesClasicas(wlines[1], vLambda, nlambda, fitsImage->pixels[indexPixel].spectro, &initModel,1);
					if (isnan(initModel.B))
						initModel.B = 1;
					if (isnan(initModel.vlos))
						initModel.vlos = 1e-3;
					if (isnan(initModel.gm))
						initModel.gm = 1;
					if (isnan(initModel.az))
						initModel.az = 1;
					// INVERSION RTE

					float * slightPixel;
					if(slight==NULL)
						slightPixel = NULL;
					else{
						if(nx_straylight && ny_straylight){
							slightPixel = slight+ (nlambda*NPARMS*indexPixel);
						}
						else {
							slightPixel = slight;
						}
					}
					vNumIter[indexPixel] = indexPixel;
					lm_mils(cuantic, wlines, vLambda, nlambda, fitsImage->pixels[indexPixel].spectro, nlambda, &initModel, spectra, &vChisqrf[indexPixel], slightPixel, configCrontrolFile.toplim, configCrontrolFile.NumberOfCycles,
							configCrontrolFile.WeightForStokes, configCrontrolFile.fix, vSigma,  configCrontrolFile.noise,configCrontrolFile.InitialDiagonalElement,&configCrontrolFile.ConvolveWithPSF,&vNumIter[indexPixel],configCrontrolFile.mu,configCrontrolFile.logclambda);

					vModels[indexPixel] = initModel;
					if(configCrontrolFile.SaveSynthesisAdjusted){
						int kk;
						for (kk = 0; kk < (nlambda * NPARMS); kk++)
						{
							imageStokesAdjust->pixels[indexPixel].spectro[kk] = spectra[kk] ;
						}
					}
				}
				t = clock() - t;
				timeReadImage = ((PRECISION)t)/CLOCKS_PER_SEC; // in seconds
				printf("\n\n--------------------------------------------------------------------------------");
				printf("\nFINISH EXECUTION OF INVERSION: %f seconds to execute ", timeReadImage);
				printf("\n--------------------------------------------------------------------------------");

				char nameAuxOutputModel [4096];
				if(configCrontrolFile.ObservedProfiles[0]!='\0')
					strcpy(nameAuxOutputModel,get_basefilename(configCrontrolFile.ObservedProfiles));
				else
					strcpy(nameAuxOutputModel,get_basefilename(configCrontrolFile.InitialGuessModel));

				strcat(nameAuxOutputModel,MOD_FITS);
				if(!writeFitsImageModels(nameAuxOutputModel,fitsImage->rows,fitsImage->cols,vModels,vChisqrf,vNumIter,configCrontrolFile.saveChisqr)){
						printf("\n ERROR WRITING FILE OF MODELS: %s",nameAuxOutputModel);
				}
				// PROCESS FILE OF SYNTETIC PROFILES

				if(configCrontrolFile.SaveSynthesisAdjusted){
					// WRITE SINTHETIC PROFILES TO FITS FILE
					char nameAuxOutputStokes [4096];
					if(configCrontrolFile.ObservedProfiles[0]!='\0')
						strcpy(nameAuxOutputStokes,get_basefilename(configCrontrolFile.ObservedProfiles));
					else
						strcpy(nameAuxOutputStokes,get_basefilename(configCrontrolFile.InitialGuessModel));
					strcat(nameAuxOutputStokes,STOKES_FIT_EXT);
					if(!writeFitsImageProfiles(nameAuxOutputStokes,nameInputFileSpectra,imageStokesAdjust)){
						printf("\n ERROR WRITING FILE OF SINTHETIC PROFILES: %s",nameOutputFilePerfiles);
					}
				}
				if(configCrontrolFile.SaveSynthesisAdjusted)
					free(imageStokesAdjust);
				free(vModels);
				free(vChisqrf);
				free(vNumIter);
			}
			else{
				printf("\n\n ***************************** FITS FILE WITH THE SPECTRO IMAGE CAN NOT BE READ IT ******************************\n");
			}

			freeFitsImage(fitsImage);
		}
		else{
			printf("\n OBSERVED PROFILES DOESN'T HAVE CORRECT EXTENSION  .PER or .FITS ");
			exit(EXIT_FAILURE);
		}
	}

	fftw_free(inFilterMAC);
	fftw_free(outFilterMAC);
	fftw_destroy_plan(planFilterMAC);
	fftw_free(inFilterMAC_DERIV);
	fftw_free(outFilterMAC_DERIV);
	fftw_destroy_plan(planFilterMAC_DERIV);
	fftw_free(inSpectraFwMAC);
	fftw_free(outSpectraFwMAC);
	fftw_destroy_plan(planForwardMAC);
	fftw_free(inSpectraBwMAC);
	fftw_free(outSpectraBwMAC);
	fftw_destroy_plan(planBackwardMAC);

	if(configCrontrolFile.ConvolveWithPSF){
		fftw_free(inSpectraFwPSF);
		fftw_free(outSpectraFwPSF);
		fftw_destroy_plan(planForwardPSF);
		fftw_free(inSpectraBwPSF);
		fftw_free(outSpectraBwPSF);
		fftw_destroy_plan(planBackwardPSF);

		fftw_free(fftw_G_PSF);
		fftw_free(fftw_G_MAC_PSF);
		fftw_free(fftw_G_MAC_DERIV_PSF);

		fftw_free(inPSF_MAC);
		fftw_free(inMulMacPSF);
		fftw_free(inPSF_MAC_DERIV);
		fftw_free(inMulMacPSFDeriv);
		fftw_free(outConvFilters);
		fftw_free(outConvFiltersDeriv);

		fftw_destroy_plan(planForwardPSF_MAC);
		fftw_destroy_plan(planForwardPSF_MAC_DERIV);
		fftw_destroy_plan(planBackwardPSF_MAC);
		fftw_destroy_plan(planBackwardPSF_MAC_DERIV);
	}

	free(cuantic);
	free(wlines);
	free(vSigma);
	FreeMemoryDerivedSynthesis();
	if(G!=NULL) free(G);
	gsl_eigen_symmv_free (workspace);
	gsl_vector_free(eval);
	gsl_matrix_free(evec);

	return 0;
}


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

#include <cblas.h>
#include <clBlas.h>

void
cblas_dgemm(const enum CBLAS_ORDER Order, const enum CBLAS_TRANSPOSE TransA,
	    const enum CBLAS_TRANSPOSE TransB, const int M, const int N,
	    const int K, const double alpha, const double *A,
	    const int lda, const double *B, const int ldb,
	    const double beta, double *C, const int ldc)
{
	cl_int		err;
	cl_platform_id	platform = 0;
	cl_device_id	device = 0;
	cl_context_properties props[3] = {CL_CONTEXT_PLATFORM, 0, 0};
	cl_context	ctx = 0;
	cl_command_queue queue = 0;
	cl_mem		bufA   , bufB, bufC;
	cl_event	event = NULL;
	cl_uint available = 0;
	int		ret = 0;

	// HACK: ignore order / trans
    clblasOrder order = clblasColumnMajor;
    clblasTranspose transA = clblasNoTrans;
    clblasTranspose transB = clblasNoTrans;

	/* Setup OpenCL environment. */
	err = clGetPlatformIDs(1, &platform, &available);
	if (err != CL_SUCCESS) {
		printf("clGetPlatformIDs() failed with %d and %d\n", err, available);
		exit(1);
	}
	printf("found %d OpenCL platforms\n", available);

	// CL_DEVICE_TYPE_GPU forces GPU use
	err = clGetDeviceIDs(platform, CL_DEVICE_TYPE_DEFAULT, 1, &device, &available);
	if (err != CL_SUCCESS) {
		printf("clGetDeviceIDs() failed with %d\n", err);
		exit(1);
	}
	printf("found %d OpenCL devices\n", available);

	props[1] = (cl_context_properties) platform;
	ctx = clCreateContext(props, 1, &device, NULL, NULL, &err);
	if (err != CL_SUCCESS) {
		printf("clCreateContext() failed with %d\n", err);
		exit(1);
	}
	printf("created context\n");

	queue = clCreateCommandQueue(ctx, device, 0, &err);
	if (err != CL_SUCCESS) {
		printf("clCreateCommandQueue() failed with %d\n", err);
		clReleaseContext(ctx);
		exit(1);
	}
	printf("created command queue\n");

	/* Setup clblas. */
	err = clblasSetup();
	if (err != CL_SUCCESS) {
		printf("clblasSetup() failed with %d\n", err);
		clReleaseCommandQueue(queue);
		clReleaseContext(ctx);
		exit(1);
	}
	printf("setup clblas\n");

	/* Prepare OpenCL memory objects and place matrices inside them. */
	bufA = clCreateBuffer(ctx, CL_MEM_READ_ONLY, M * K * sizeof(*A), NULL, &err);
	bufB = clCreateBuffer(ctx, CL_MEM_READ_ONLY, K * N * sizeof(*B), NULL, &err);
	bufC = clCreateBuffer(ctx, CL_MEM_READ_WRITE, M * N * sizeof(*C), NULL, &err);
	printf("created buffers\n");

	err = clEnqueueWriteBuffer(queue, bufA, CL_TRUE, 0, M * K * sizeof(double), A, 0, NULL, NULL);
	err = clEnqueueWriteBuffer(queue, bufB, CL_TRUE, 0, K * N * sizeof(double), B, 0, NULL, NULL);
	err = clEnqueueWriteBuffer(queue, bufC, CL_TRUE, 0, M * N * sizeof(double), C, 0, NULL, NULL);
	printf("enqueud buffers\n");

	/*
	 * Call clblas extended function. Perform gemm for the lower right
	 * sub-matrices
	 */
	err = clblasDgemm(order, transA, transB, M, N, K,
                         alpha, bufA, 0, lda,
                         bufB, 0, ldb, beta,
                         bufC, 0, ldc,
                         1, &queue, 0, NULL, &event);
	if (err != CL_SUCCESS) {
		printf("clblasSgemmEx() failed with %d\n", err);
		ret = 1;
	} else {
		printf("no errors for calculation\n");
		fflush(stdout);
		/* Wait for calculations to be finished. */
		err = clWaitForEvents(1, &event);

		/* Fetch results of calculations from GPU memory. */
		err = clEnqueueReadBuffer(queue, bufC, CL_TRUE, 0,
					  M * N * sizeof(double),
					  C, 0, NULL, NULL);
		printf("got result\n");
	}

	/* Release OpenCL memory objects. */
	clReleaseMemObject(bufC);
	clReleaseMemObject(bufB);
	clReleaseMemObject(bufA);
	printf("released\n");

	/* Finalize work with clblas. */
	clblasTeardown();
	printf("teardown clblas\n");

	/* Release OpenCL working objects. */
	clReleaseCommandQueue(queue);
	printf("release command queue\n");
	clReleaseContext(ctx);
	printf("release context\n");
}


/*
 * 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.
 *
 * Written (W) 1999-2009 Soeren Sonnenburg
 * Written (W) 1999-2008 Gunnar Raetsch
 * Written (W) 2006-2007 Mikio L. Braun
 * Written (W) 2008 Jochen Garcke
 * Copyright (C) 1999-2009 Fraunhofer Institute FIRST and Max-Planck-Society
 */

#ifndef _LAPACK_H__
#define _LAPACK_H__

#include <shogun/lib/config.h>
#include <shogun/lib/common.h>

#ifdef HAVE_LAPACK

extern "C" {

#ifdef HAVE_MKL
#include <mkl_cblas.h>
#include <mkl_lapack.h>
#elif defined(HAVE_MVEC)
//FIXME: Accelerate framework's vForce.h forward declares
// std::complex<> classes that causes major errors
// in c++11 mode and Eigen3
// this define basically disables the include of vForce.h
#ifdef HAVE_CXX11
#define __VFORCE_H 1
#endif
#include <Accelerate/Accelerate.h>
#else
#include <cblas.h>
#endif

#ifdef HAVE_ACML
#include <acml.h>
#endif

#ifdef HAVE_ATLAS
#include <clapack.h>
#else
// ACML and MKL do not provide clapack_* routines
// double precision
int clapack_dpotrf(const CBLAS_ORDER Order, const CBLAS_UPLO Uplo,
		const int N, double *A, const int lda);
int clapack_dposv(const CBLAS_ORDER Order, const CBLAS_UPLO Uplo,
		const int N, const int NRHS, double *A, const int lda,
		double *B, const int ldb);
int clapack_dpotri(const CBLAS_ORDER Order, const CBLAS_UPLO Uplo,
		const int N, double *A, const int LDA);
int clapack_dpotrs(const CBLAS_ORDER Order, const CBLAS_UPLO Uplo,
                   const int N, const int NRHS, double *A, const int lda,
                   double *B, const int ldb);
int clapack_dgetrf(const CBLAS_ORDER Order, const int M, const int N,
                   double *A, const int lda, int *ipiv);
int clapack_dgetri(const CBLAS_ORDER Order, const int N, double *A,
                   const int lda, int *ipiv);
int clapack_dgetrs(const CBLAS_ORDER Order, const CBLAS_TRANSPOSE Transpose,
                   const int N, const int NRHS, double *A, const int lda,
                   int *ipiv, double *B, const int ldb);
#endif

namespace shogun
{
#ifndef DOXYGEN_SHOULD_SKIP_THIS
// double precision
void wrap_dsyev(char jobz, char uplo, int n, double *a, int lda,
		double *w, int *info);
void wrap_dgesvd(char jobu, char jobvt, int m, int n, double *a, int lda,
		double *sing, double *u, int ldu, double *vt, int ldvt,
		int *info);
void wrap_dgeqrf(int m, int n, double *a, int lda, double *tau, int *info);
void wrap_dorgqr(int m, int n, int k, double *a, int lda, double *tau, int *info);
void wrap_dsyevr(char jobz, char uplo, int n, double *a, int lda, int il, int iu,
                 double *eigenvalues, double *eigenvectors, int *info);
void wrap_dsygvx(int itype, char jobz, char uplo, int n, double *a, int lda, double *b,
                 int ldb, int il, int iu, double *eigenvalues, double *eigenvectors, int *info);
void wrap_dstemr(char jobz, char range, int n, double* d__, double *e, double vl, double vu,
		int il,	int iu, int* m, double* w, double* z__, int ldz, int nzc, int *isuppz,
		int tryrac, int *info);
#endif
}

// only MKL, ACML and Mac OS vector library provide a header file for the lapack routines
#if !defined(HAVE_ACML) && !defined(HAVE_MKL) && !defined(HAVE_MVEC)
// double precision
int dsyev_(char*, char*, int*, double*, int*, double*, double*, int*, int*);
int dgesvd_(char* jobu, char* jobvt, int* m, int* n, double* a, int* lda,
		double* s, double* u, int* ldu, double* vt, int* ldvt, double* work,
		int* lwork, int* info);
int dposv_(const char *uplo, const int *n, const int *nrhs, double *a, const int *lda, double *b, const int *ldb, int *info);
int dpotrf_(const char *uplo, int *n, double *a, int * lda, int *info);
int dpotri_(const char *uplo, int *n, double *a, int * lda, int *info);
int dgetri_(int *n, double *a, int *lda, int *ipiv, double *work, int *lwork, int *info);
int dgetrf_(int *m, int *n, double *a, int *lda, int *ipiv, int *info);
int dgeqrf_(int*, int*, double*, int*, double*, double*, int*, int*);
int dorgqr_(int*, int*, int*, double*, int*, double*, double*, int*, int*);
int dsyevr_(const char*, const char*, const char*, int*, double*, int*,
            double*, double*, int*, int*, double*, int*, double*, double*,
            int*, int*, double*, int*, int*, int*, int*);
int dgetrs_(const char*, int*, int*, double*, int*, int*, double*, int*, int*);
int dpotrs_(const char*, int*, int*, double*, int*, double*, int*, int*);
int dsygvx_(int*, const char*, const char*, const char*, int*, double*, int*,
            double*, int*, double* , double*, int*, int*, double*,
            int*, double*, double*, int*, double*, int*, int*, int*, int*);
int dstemr_(char*, char*, int*, double*, double*, double*, double*, int*,
	int*, int*, double*, double*, int*, int*, int*, int*, double*,
	int*, int*, int*, int*);
#endif
}

#endif //HAVE_LAPACK
#endif //_LAPACK_H__


#if !defined(DECAY_H)
#define DECAY_H

#include "SM.h"
#include "THDM.h"
#include <complex>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_integration.h>

using namespace std;

/**
* @brief Calculates the decay modes of 2HDM Higgs bosons
*
* Given a THDM object, a DecayTable can be generated. From this table, the
* Higgs boson decay widths and branching ratios are obtained. For the complete
* list of available decay modes, we refer to the complete documentation, or
* the list of member methods below.
*/
class DecayTable {

 public:

  /**
  * @brief Default constructor
  *
  * This default constructor takes a THDM object as argument for which
  * the decays are to be calculated. %SM properties are taken from the SM
  * object in the THDM.
  *
  * @param mod Two-Higgs doublet model for which to calculate decay modes
  */
  DecayTable(THDM mod);

  /**
  * @brief Sets underlying 2HDM
  *
  * This method sets the THDM underlying the DecayTable.
  *
  * @param model Two-Higgs doublet model for which to calculate decay modes
  */
	void set_model(THDM model);

  /**
  * @brief Underlying 2HDM
  *
  * Use to obtain the underlying THDM object
  *
  * @returns The THDM object on which this DecayTable operates
  */
	THDM get_model();

  /**
  * @brief Decay width \f$\Gamma(h\to u_1\overline{u}_2) \f$
  *
  * This method calculates the on-shell decay width for the decay of Higgs
  * boson \a h to a pair of up-type quarks. QCD corrections have been included.
  *
  * @param h  Index of Higgs boson (1,2,3 = h,H,A)
  * @param u1 Index of up-type quark (1,2,3 = \f$ u,c,t \f$)
  * @param u2 Index of up-type antiquark (1,2,3 = \f$ \bar{u},\bar{c},\bar{t} \f$)
  *
  * @returns The decay width in GeV
  */
  double  get_gamma_huu(int h, int u1, int u2);

  /**
  * @brief Decay width \f$ \Gamma(h\to d_1\overline{d}_2) \f$
  *
  * This method calculates the on-shell decay width for the decay of Higgs
  * boson \a h to a pair of down-type quarks. QCD corrections have been included.
  *
  * @param h  Index of Higgs boson (1,2,3 = h,H,A)
  * @param d1 Index of down-type quark (1,2,3 = \f$ d,s,b \f$)
  * @param d2 Index of down-type antiquark (1,2,3 = \f$ \bar{d},\bar{s},\bar{b} \f$)
  *
  * @returns The decay width in GeV
  */
  double  get_gamma_hdd(int h, int d1, int d2);

  /**
  * @brief Decay width \f$ \Gamma(h\to l_1\overline{l}_2) \f$
  *
  * This method calculates the on-shell decay width for the decay of Higgs
  * boson \a h to a pair of charged leptons.
  *
  * @param h  Index of Higgs boson (1,2,3 = h,H,A)
  * @param l1 Index of lepton (1,2,3 = \f$ e,\mu,\tau \f$)
  * @param l2 Index of antilepton (1,2,3 = \f$ \bar{e},\bar{\mu},\bar{\tau} \f$)
  *
  * @returns The decay width in GeV
  */
  double  get_gamma_hll(int h, int l1, int l2);

  /**
  * @brief Decay width \f$ \Gamma(h\to u\overline{d}) \f$
  *
  * This method calculates the on-shell decay width for the decay of Higgs
  * boson \a h to a pair of quarks. QCD corrections have been included.
  *
  * @param h  Index of Higgs boson (4 = H+)
  * @param d  Index of down-type antiquark (1,2,3 = \f$ \bar{d},\bar{s},\bar{b} \f$)
  * @param u  Index of up-type quark (1,2,3 = \f$ u,c,t \f$)
  *
  * @returns The decay width in GeV
  */
  double  get_gamma_hdu(int h, int d, int u);

  /**
  * @brief Decay width \f$ \Gamma(h\to l\overline{\nu}_{l'}) \f$
  *
  * This method calculates the on-shell decay width for the decay of Higgs
  * boson \a h to lepton-neutrino.
  *
  * @param h  Index of Higgs boson (4 = H+)
  * @param l  Index of charged lepton (1,2,3 = \f$ e^+,\mu^+,\tau^+ \f$)
  * @param n  Index of neutrino (1,2,3 = \f$ \nu_e,\nu_\mu,\nu_\tau \f$)
  *
  * @returns The decay width in GeV
  */
  double  get_gamma_hln(int h, int l, int n);

  /**
  * @brief Decay width \f$ \Gamma(h\to H_1 H_2) \f$
  *
  * This method calculates the on-shell decay width for the decay of Higgs
  * boson \a h to a pair of Higgs bosons
  *
  * @param h  Index of decaying Higgs boson (1,2,3,4 = h,H,A,H+)
  * @param h1 Index of first Higgs boson (1,2,3,4 = h,H,A,H+)
  * @param h2 Index of second Higgs boson (1,2,3,4 = h,H,A,H+)
  *
  * @returns The decay width in GeV
  */
  double  get_gamma_hhh(int h, int h1, int h2);

  /**
  * @brief Decay width \f$ \Gamma(h\to VV) \f$
  *
  * This method calculates the decay width for the Higgs boson \a h
  * to a pair of vector bosons. The decay mode with one vector
  * boson off-shell is included.
  *
  * @param h  Index of decaying Higgs boson (1,2,3,4 = h,H,A,H+)
  * @param v  Index of vector bosons (1,2,3 = \f$\gamma \f$,Z,W)
  *
  * @returns The decay width in GeV
  */
  double  get_gamma_hvv(int h, int v);

  /**
  * @brief Decay width \f$ \Gamma(h\to VH) \f$
  *
  * This method calculates the decay width for the Higgs boson \a h
  * to one massive vector and one Higgs boson. Decay with the vector
  * boson off-shell is included.
  *
  * @param h  Index of decaying Higgs boson (1,2,3,4 = h,H,A,H+)
  * @param V  Index of vector boson (2,3 = \f$\gamma \f$,Z,W)
  * @param H  Index of final-state Higgs boson (1,2,3,4 = h,H,A,H+)
	*
  * @returns The decay width in GeV
  */
  double  get_gamma_hvh(int H, int V, int h);

  /**
  * @brief Decay width \f$ \Gamma(h\to gg) \f$
  *
  * This method calculates the decay width for the Higgs boson \a h
  * to a pair of gluons. LO QCD corrections are included.
  *
  * @param h  Index of decaying Higgs boson (1,2,3,4 = h,H,A,H+)
	*
  * @returns The decay width in GeV
  */
  double  get_gamma_hgg(int h);

  /**
  * @brief Decay width \f$ \Gamma(h\to \gamma\gamma) \f$
  *
  * This method calculates the decay width for the neutral
  * Higgs boson \a h to a pair of photons.
  *
  * @param h  Index of decaying Higgs boson (1,2,3 = h,H,A)
	*
  * @returns The decay width in GeV
  */
	double  get_gamma_hgaga(int h);

  /**
  * @brief Decay width \f$ \Gamma(h\to Z\gamma) \f$
  *
  * This method calculates the decay width for the neutral
  * Higgs boson \a h to a Z and a photon.
  *
  * @param h  Index of decaying Higgs boson (1,2,3 = h,H,A)
	*
  * @returns The decay width in GeV
  */
	double  get_gamma_hZga(int h);

  /**
  * @brief Decay width \f$ \Gamma(h\to WZ) \f$
  *
  * This method calculates the decay width for the neutral
  * Higgs boson \a h to a Z and a photon.
  *
  * @param h  Index of decaying Higgs boson (4 = H+-)
    *
  * @returns The decay width in GeV
  */
    double  get_gamma_hWZ(int h);

  /**
  * @brief Decay width \f$ \Gamma(h\to W\gamma) \f$
  *
  * This method calculates the decay width for the neutral
  * Higgs boson \a h to a Z and a photon.
  *
  * @param h  Index of decaying Higgs boson (4 = H+-)
    *
  * @returns The decay width in GeV
  */
    double  get_gamma_hWga(int h);

  /**
  * @brief Total width \f$ \Gamma_h \f$
  *
  * Calculates the total decay width of Higgs boson \a h
  *
  * @param h  Index of Higgs boson (1,2,3,4 = h,H,A,H+)
	*
  * @returns Total width in GeV
  */
  double  get_gammatot_h(int h);

  /**
  * @brief Total width \f$ \Gamma_V \f$
  *
  * Returns the total decay width of vector boson \a v
  *
  * @param v  Index of vector boson (2,3 = Z,W)
	*
  * @returns Total width in GeV
  */
  double  get_gammatot_v(int v);

  /**
  * @brief Total width \f$ \Gamma_t \f$
  *
  * Returns the total decay width of the top quark
  *
  * @returns Total width in GeV
  */
  double  get_gammatot_top();

  /**
  * @brief Decay width for \f$ t \to H^+X \f$
  *
  * Returns the decay width of the top quark in the charged Higgs mode
  *
  * @returns Decay width in GeV
  *
  * @param u Index of decaying quark (1,2,3 = \f$ u,c,t \f$)
  * @param h Index of Higgs boson (4 = H+)
	* @param d Index of down-type quark (1,2,3 = \f$ d,s,b \f$)
  */
  double  get_gamma_uhd(int u, int h, int d);

  double  get_gamma_uhu(int u1, int h, int u2);

  /**
  *	@brief Prints the decay modes of a Higgs boson
  *
  * The decay modes of Higgs boson \a h are printed to stdout
  *
  * @param h Index of Higgs boson (1,2,3,4 = h,H,A,H+)
  */
  void    print_decays(int h);

  /**
  *	@brief Prints the total width of a Higgs boson
  *
  * The total decay width of Higgs boson \a h are printed to stdout
  *
  * @param h Index of Higgs boson (1,2,3,4 = h,H,A,H+)
  */
  void    print_width(int h);

  /**
  *	@brief Prints the decay modes of the top quark
  *
  * The decay modes of the top quark are printed to stdout
  */
  void    print_top_decays();

  /**
  *	@brief Prints decay information for a Higgs boson in LesHouches format
  *
  * The decay information for Higgs boson \a h are printed to a file in
  * LesHouches format
  *
  * @param output The name of the output file to write
  * @param h 			Index of Higgs boson (1,2,3,4 = h,H,A,H+)
  * @param full 	If \a true, all decay modes are printed, if \a false only the total width
  */
  void    print_decays_LesHouches(FILE* output, int h, bool full);

  /**
  *	@brief Prints decay information for the top quark in LesHouches format
  *
  * The decay information for the top quark are printed to a file in
  * LesHouches format
  *
  * @param output The name of the output file to write
  * @param full 	If \a true, all decay modes are printed, if \a false only the total width
  */
  void    print_top_decays_LesHouches(FILE* output, bool full);

  /**
  *     @brief Turns QCD corrections on or off
  *
  * This method is used to turn QCD corrections on or off. If the output is
  * meant to be used with the MadGraph/MadEvent 2HDMC model
  * QCD corrections should be turned off to get a consistent result.
  *
  * @param set  If \a true QCD corrections are turned on,
  *             if \a false QCD corrections are turned off
  */
  void    set_qcd(bool set);

  static double  DHp(double ui, double uj, double xi, double xj, double sqL);
  static double  DHm(double ui, double uj, double xi, double xj, double sqL);
  static double  BHp(double ui, double uj, double xi, double xj, double sqL);

 private:
  THDM model;
  SM sm;

  complex <double> F_sf(double t);
  complex <double> F_pf(double t);
  complex <double> F_0(double t);
  complex <double> F_1(double t);
  complex <double> ftau(double t);
  complex <double> gtau(double t);
  complex <double> I_1(double tau, double lambda);
  complex <double> I_2(double tau, double lambda);
  complex <double> FF_s(double tau, double lambda);
  complex <double> FF_p(double tau, double lambda);
  complex <double> FW(double tau, double lambda);
  complex <double> FHp(double tau, double lambda);

  double gammatot_h[5];
  double gamma_uhd[5][5][5];
  double gamma_uhu[5][5][5];
  double gamma_hdd[5][5][5];
  double gamma_huu[5][5][5];
  double gamma_hdu[5][5][5];
  double gamma_hll[5][5][5];
  double gamma_hln[5][5][5];
  double gamma_hgg[5];
  double gamma_hgaga[5];
  double gamma_hZga[5];
  double gamma_hWga[5]; // For H+- only
  double gamma_hWZ[5]; // For H+- only
  double gamma_hvv[5][5];
  double gamma_hvh[5][5][5];
  double gamma_hhh[5][5][5];

  double  br(double dG, double G);

  double  hvv_onshell(int h, int V, double M);
  double  hvv_offshell(int h, int V, double M);
  double  hvv_all(int h, int V, double M);
  double  hff_onshell(double M, double m1, double m1run, double m2, double m2run, complex<double> cs, complex<double> cp, int Nc, int h, bool tt);
  double  hpff_onshell(double M, double m1, double m1run, double m2, double m2run, complex<double> cs, complex<double> cp, int Nc, int h);
  double  htt_onshell(double M, double m1, double m2, int Nc, int h);
  double  htt_offshell(double M, double m1, double m2, int Nc, int h);
  double  htb_offshell(double M, double m1, double m1run, double m2, double m2run, complex<double> cs, complex<double> cp, int Nc);

  double  hvh_onshell(int H, int V, int h, double M);
  double  hvh_offshell(int H, int V, int h, double M);
  double  hdu_offshell(int h, int d, int u, double M);
  double  hgaga(int h);
  double  hZga(int h);
  double  hWga(int h);
  double  hWZ(int h);
  double  hgg(int h);
  double  PS2(double M, double m1, double m2);
  double  LAM(double m12, double m22, double m32);

  double  interp(double R, double x, double y, double c);

  bool    qcd_on;
  int     err_code;

  void    print_decay_LesHouches(FILE* output, double br, int id1, int id2);
  void    print_decay(const char *h, const char *id1, const char *id2, double g, double br);
  void    print_decays(FILE* output, int h, bool full, bool lh);

};

#endif


//
// This file is subject to the terms and conditions defined in
// file 'LICENSE.txt', which is part of this source code package.
//
// Principal developer : Felipe Bordeu (Felipe.Bordeu@ec-nantes.fr)
//

#include <iostream>

#ifdef MATLAB_MEX_FILE
#include "mex.h"
#ifdef __cplusplus
    extern "C" bool utIsInterruptPending();
    extern "C" void utSetInterruptPending(bool);
#else
    extern bool utIsInterruptPending();
    extern void utSetInterruptPending(bool);
#endif
#endif /* MATLAB_MEX_FILE */

//if in matlab we use the matlab blas library
#ifdef MATLAB_MEX_FILE
    #include <blas.h>
    inline double cblas_ddot( ptrdiff_t& a, double*& b,const int& c, double* d,const int& e){
        ptrdiff_t cc = c;
        ptrdiff_t ee = e;
        return ddot(&a,b,&cc,d,&ee);
   }
   enum CBLAS_ORDER {CblasRowMajor=101, CblasColMajor=102 };
   enum CBLAS_TRANSPOSE {CblasNoTrans=111, CblasTrans=112, CblasConjTrans=113,
                         AtlasConj=114};

template<typename T5,typename T6,typename T7,typename T8,typename T9,typename T12>
void cblas_dgemv(const CBLAS_ORDER& a, const CBLAS_TRANSPOSE& b, const ptrdiff_t & c, const ptrdiff_t& d,const T5& e,T6 f,T7& g,T8 h,const T9& i,const double& j,double* k,const T12& l){
    char bb= 'n';
    if (b ==CblasTrans) bb = 't';
    ptrdiff_t cc = c;
    ptrdiff_t dd = d;
    double ee = e;
    ptrdiff_t ii = i;
    double jj = j;
    ptrdiff_t ll  =  l;
    dgemv( &bb,  &cc,    &dd,    &ee,    f,    &g,    h,    &ii,    &jj,    k,   &ll);
};

template<typename T5,typename T6,typename T7, typename T8,typename T9,typename T12>
void cblas_sgemv(const CBLAS_ORDER& a, const CBLAS_TRANSPOSE& b, const ptrdiff_t& c, const ptrdiff_t& d, const T5& e,T6 f,T7& g,T8 h,const  T9& i,const float& j,float* k,const T12& l){
    char bb= 'n';
    if (b ==CblasTrans) bb = 't';
    ptrdiff_t cc = c;
    ptrdiff_t dd = d;
    float ee = e;
    ptrdiff_t ii = i;
    float jj = j;
    ptrdiff_t ll  =  l;

    sgemv( &bb,  &cc,    &dd,    &ee,    f,    &g,    h,    &ii,    &jj,    k,   &ll);
};

 void cblas_dgemm(const CBLAS_ORDER& a, const CBLAS_TRANSPOSE& b, const CBLAS_TRANSPOSE& c, ptrdiff_t d, ptrdiff_t e, ptrdiff_t f ,double g, double* h, ptrdiff_t i,double* j , ptrdiff_t k,double l,double* m,ptrdiff_t n){
    char bb= 'n';
    if (b ==CblasTrans) bb = 't';
    char cc= 'n';
    if (c ==CblasTrans) cc = 't';

    dgemm( &bb, &cc, &d, &e, &f, &g, h, &i, j, &k, &l,m, &n);
}

template <typename pdiff>
void  cblas_dscal (pdiff& a,double& b, double* c,const pdiff& d){
    pdiff dd = d;
    dscal(&a,&b,c,&dd);
}

template <typename pdiff>
void  cblas_sscal (pdiff& a,float& b, float* c,const pdiff& d){
    pdiff dd = d;
    sscal(&a,&b,c,&dd);
}

template <typename pdiff>
inline float cblas_sdot( pdiff& a, float*& b,const pdiff& c, float* d,const pdiff& e){
    pdiff cc = c;
    pdiff ee = e;
    return sdot(&a,b,&cc,d,&ee);
}
#else
// if not matlab we use the 'system' blas

#ifdef __APPLE__
   #include   <Accelerate/Accelerate.h>
#else
#if defined(_WIN32)
   /// for now no compilation can be done in windows without matlab
#else
   /// linux
   #include <cblas.h>
   #include <vector>
#endif
#endif
#endif /* MATLAB_MEX_FILE */

template <typename ptrdiff, typename T>
T cblas_Xdot( ptrdiff&, T*&,const ptrdiff&, T*,const ptrdiff& );

template <typename ptrdiff>
double cblas_Xdot( ptrdiff& a, double*& b,const ptrdiff& c, double* d,const ptrdiff& e){
return cblas_ddot(a,b,c,d,e);
}

template <typename ptrdiff>
float cblas_Xdot( ptrdiff& a, float*& b,const ptrdiff& c, float* d,const ptrdiff& e){
return cblas_sdot(a,b,c,d,e);
}

//template<typename T>
//void cblas_Xgemv(const CBLAS_ORDER&  , const CBLAS_TRANSPOSE&  ,int&   ,int&  ,const T&       ,     T*  , int&  ,T*      , const int&   ,const T&      , T*       ,const int&);
//

template <typename ptrdiff>
void cblas_Xgemv(const CBLAS_ORDER& a, const CBLAS_TRANSPOSE& b,const int& c,const int& d,const double& e,double* f, ptrdiff& g,double* h,const int& i ,const double& j,double* k,const int& l){
            cblas_dgemv(a,b,c,d,e,f,g,h,i,j,k,l);
};

template <typename ptrdiff>
void cblas_Xgemv(const CBLAS_ORDER& a,const CBLAS_TRANSPOSE& b,const int& c,const int& d,const float& e,float* f,ptrdiff& g,float* h,const  int& i,const float& j,float* k,const int& l){
            cblas_sgemv(a,b,c,d,e,f,g,h,i,j,k,l);
};

//template<typename T1,typename T2, typename T3,typename T4>
//void cblas_Xscal(T1&,T2&,T3,const T4&);

//template<typename T1>
void  cblas_Xscal(ptrdiff_t& a,double& b,double* c,const ptrdiff_t& d){
        cblas_dscal(a,b,c ,d);
}

//template<typename T1>
void  cblas_Xscal(ptrdiff_t& a,float& b, float* c,const ptrdiff_t& d){
        cblas_sscal(a,b,c,d);
}

template <typename T>
void update_C(MatLabDataMatrix <T >& C,T*  r, MatLabDataMatrix <T >& MM,ptrdiff_t& N, const ptrdiff_t& dim);

template<>
void update_C(MatLabDataMatrix <double >& C,double*  r, MatLabDataMatrix <double >& MM,ptrdiff_t& NN, const ptrdiff_t& dim) {
    int N = NN;
    int M = MM.dsizes[0];
    int INCY = C.dsizes[0];
    cblas_dgemv(CblasColMajor,CblasTrans, M, N, 1.0, &MM.Data[0], MM.dsizes[0],      r, 1,      0, &C.Data[dim], C.dsizes[0] );
    #ifdef USE_MPI
       MPI_Allreduce(MPI_IN_PLACE, &C.Data[dim], NN, everydim, myOp, MPI_COMM_WORLD);
    #endif /* USE_MPI */
};

template<>
void update_C(MatLabDataMatrix <float >& C,float*  r, MatLabDataMatrix <float >& MM,ptrdiff_t& NN, const ptrdiff_t& dim) {
    int N = NN;
    int M = MM.dsizes[0];
    int INCY = C.dsizes[0];
    cblas_sgemv(CblasColMajor,CblasTrans, M, N, (float)1.0, &MM.Data[0], MM.dsizes[0],      r, 1,      0, &C.Data[dim], C.dsizes[0] );
    #ifdef USE_MPI
       MPI_Allreduce(MPI_IN_PLACE, &C.Data[dim], NN, everydim, myOp, MPI_COMM_WORLD);
    #endif /* USE_MPI */
};

template<typename T>
void update_D(MatLabDataMatrix <T >& D, MatLabDataMatrix <T >& FF, T* r){
  int cpt = -1;
  for(int j =0; j < FF.dsizes[1]; ++j){
    for(int i =0; i < FF.dsizes[0]; ++i){
        D.Data[++cpt] = FF(i,j)*r[i];
    }
  }
};

//template <typename T>
//void update_CWithFF2(MatLabDataMatrix <T >& C,T*  r, MatLabDataMatrix <T >& MM);
//,unsigned& N, const unsigned& dim, MatLabDataMatrix <T >& DD, std::vector<T> &workingplace);

//template<>
void update_CU(std::vector<double >& C,MatLabDataMatrix <double >&  sol, MatLabDataMatrix <double >& D) {

    if( sol.dsizes[1] == 0) return;

    int Sr,Sc,Dr,Dc;
    Sr = sol.dsizes[0];
    Sc = sol.dsizes[1];

    Dr = D.dsizes[0];
    Dc = D.dsizes[1];

    ///C(Cr,Cc) = sol(Sr,Sc)'*D(Dr,Dc);

    //printf (" Initializing data for matrix multiplication C=sol'*D for matrix \n"
    //        " sol(%ix%i) and matrix D(%ix%i)\n\n", Sr, Sc, Dr, Dc);

    //std::cout << "sol.size() : " << sol.fullsize << " =? " <<Sr*Sc << std::endl;
    //std::cout << "D.size() : " << D.fullsize << " =? " <<Dr*Dc << std::endl;

   //std::cout << "--------------------------------------" << std::endl;

    cblas_dgemm(CblasColMajor, CblasTrans, CblasNoTrans,
                Sc,Dc, Dr, 1.0, sol.Data, sol.LD(), D.Data , D.LD(), 0, &C[0], sol.dsizes[1]);

    //std::cout << " ------------------------- DONE ------------------------" << std::endl;
    #ifdef USE_MPI
       MPI_Allreduce(MPI_IN_PLACE, &C[0], C.size(), everydim, myOp, MPI_COMM_WORLD);
    #endif /* USE_MPI */
};
//
void update_CU(std::vector<float >& C,MatLabDataMatrix <float >&  sol, MatLabDataMatrix <float >& D) {
//#warning to correct for the double implementation
    //cblas_sgemm(CblasColMajor, CblasTrans, CblasNoTrans, sol.dsizes[1], D.dsizes[1], sol.dsizes[0], 1.0, sol.Data, sol.dsizes[0], D.Data , D.dsizes[0], 0, &C[0],sol.dsizes[0]);
    #ifdef USE_MPI
       MPI_Allreduce(MPI_IN_PLACE, &C[0], C.size(), everydim, myOp, MPI_COMM_WORLD);
    #endif /* USE_MPI */
};

#define PrintM(X) std::cout << #X " : "  << std::endl << "  "; \
                  for(int ii =0; ii < X.dsizes[0] ; ++ii){\
                    for(int jj =0; jj < X.dsizes[1] ; ++jj){\
                      std::cout << X(ii,jj) << " ";\
                    }\
                    std::cout << std::endl << "  "; \
                  }\
                  std::cout << std::endl;

#define PrintV(X) std::cout << #X " : "  << std::endl << "  "; \
                  for(int ii=0; ii < X.size(); ++ii){\
                    std::cout << X[ii] << " "; \
                  } \
                  std::cout << std::endl;

// template<>
// void update_CWithFF2(MatLabDataMatrix <float >& C,float*  r, MatLabDataMatrix <float >& MM,unsigned& NN, const unsigned& dim, MatLabDataMatrix <float >& DD, std::vector<float> &workingplace) {    int N = NN;
//     ///TODO function to be coded
//     int M = MM.dsizes[0];
//     int LDA = MM.dsizes[0];
//     int INCY = C.dsizes[0];
//     cblas_sgemv(CblasColMajor,CblasTrans, M, N, 1.0, &MM.Data[0], LDA,      r, 1,      0, &C.Data[dim], C.dsizes[0] );
//     #ifdef USE_MPI
//        MPI_Allreduce(MPI_IN_PLACE, &C.Data[dim], NN, everydim, myOp, MPI_COMM_WORLD);
//     #endif /* USE_MPI */
//
// };


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

#include "../csm_all.h"
#include "gpm.h"

#include <egsl/egsl_macros.h>

void sm_gpm(struct sm_params*params, struct sm_result*res) {
	res->valid = 0;
	/* Check for well-formedness of the input data */
	if(!ld_valid_fields(params->laser_ref) ||
	   !ld_valid_fields(params->laser_sens)) {
		return;
	}

	LDP laser_ref  = params->laser_ref;
	LDP laser_sens = params->laser_sens;

	/* We need to compute cartesian points */
	ld_compute_cartesian(laser_ref);
	/* ... and orientation */
	ld_simple_clustering(laser_ref, params->clustering_threshold);
	ld_compute_orientation(laser_ref, params->orientation_neighbourhood, params->sigma);
	/* ... for both scans. */
	ld_compute_cartesian(laser_sens);
	ld_simple_clustering(laser_sens, params->clustering_threshold);
	ld_compute_orientation(laser_sens, params->orientation_neighbourhood, params->sigma);

	/* Create an histogram whose bin is large `theta_bin_size` */
	double theta_bin_size = deg2rad(params->gpm_theta_bin_size_deg);
	double hist_min = -M_PI-theta_bin_size; /* be robust */
	double hist_max = +M_PI+theta_bin_size;
	size_t nbins = (size_t) ceil(  (hist_max-hist_min) / theta_bin_size);
	gsl_histogram*hist = gsl_histogram_alloc(nbins);
	gsl_histogram_set_ranges_uniform(hist, hist_min, hist_max);

	/* Fill the histogram with samples */
	double u[3]; copy_d(params->first_guess, 3, u);
	sm_debug("gpm 1/2: old u = : %s \n", friendly_pose(u) );

	int interval = params->gpm_interval;

	int num_correspondences_theta=-1;

	ght_find_theta_range(laser_ref, laser_sens,
		u, params->max_linear_correction,
		params->max_angular_correction_deg, interval, hist, &num_correspondences_theta);

	if(num_correspondences_theta < laser_ref->nrays) {
		sm_error("sm_gpm(): I found only %d correspondences in the first pass of GPM. I consider it a failure.\n",
			num_correspondences_theta);
		return;
	}

	/* Find the bin with most samples */
	size_t max_bin = gsl_histogram_max_bin(hist);

	/* Around that value will be the range admissible for theta */
	double min_range, max_range;
	gsl_histogram_get_range(hist,max_bin,&min_range,&max_range);

	/* Extend the range of the search */
	double extend_range = deg2rad(params->gpm_extend_range_deg);
	min_range += -extend_range;
	max_range += +extend_range;

	/*	if(jf()) fprintf(jf(), "iteration 0\n");
	journal_pose("x_old", u);*/

	/*	if(jf()) fprintf(jf(), "iteration 1\n");
	journal_pose("x_old", u);*/

	/* Now repeat the samples generation with a smaller domain */
	u[2] = 0.5 * (max_range + min_range);
	double new_range_deg = rad2deg( 0.5*(max_range - min_range) );

	double x_new[3];
	int num_correspondences=-1;
	ght_one_shot(laser_ref, laser_sens,
			u, params->max_linear_correction*2,
			new_range_deg, interval, x_new, &num_correspondences) ;

	if(num_correspondences < laser_ref->nrays) {
		sm_error("sm_gpm(): I found only %d correspondences in the second pass of GPM. I consider it a failure.\n",
			num_correspondences);
		return;
	}

	/* Et voila, in x_new we have the answer */

	{
		sm_debug("gpm : max_correction_lin %f def %f\n", params->max_linear_correction, 		params->max_angular_correction_deg);
		sm_debug("gpm : acceptable range for theta: [%f, %f]\n", min_range,max_range);
		sm_debug("gpm : 1) Num correspondences for theta: %d\n", num_correspondences_theta);

		sm_debug("gpm 1/2: new u = : %s \n", friendly_pose(u) );
		sm_debug("gpm 1/2: New range: %f to %f\n",rad2deg(min_range),rad2deg(max_range));

		sm_debug("gpm 2/2: Solution: %s \n", friendly_pose(x_new));
	/*	if(jf()) fprintf(jf(), "iteration 2\n");
		journal_pose("x_old", x_new);	*/
	}

	/* Administrivia */

	res->valid = 1;
	copy_d(x_new, 3, res->x);

	res->iterations = 0;

	gsl_histogram_free(hist);
}

void ght_find_theta_range(LDP laser_ref, LDP laser_sens,
	const double*x0, double max_linear_correction,
	double max_angular_correction_deg, int interval, gsl_histogram*hist, int*num_correspondences)
{
	/** Compute laser_sens's points in laser_ref's coordinates by roto-translating by x0 */
	ld_compute_world_coords(laser_sens, x0);

	int count = 0;
	int i;
	for(i=0;i<laser_sens->nrays;i++) {
		if(!laser_sens->alpha_valid[i]) continue;
		if(i % interval) continue;

		const double * p_i = laser_sens->points[i].p;

		const double * p_i_w = laser_sens->points_w[i].p;
		int from; int to; int start_cell;
		possible_interval(p_i_w, laser_ref, max_angular_correction_deg,
			max_linear_correction, &from, &to, &start_cell);

//		printf("\n i=%d interval = [%d,%d] ", i, from, to);
		int j;
		for(j=from;j<=to;j++) {
			if(!laser_ref->alpha_valid[j]) continue;
			if(j % interval) continue;

			double theta = angleDiff(laser_ref->alpha[j], laser_sens->alpha[i]);
			double theta_diff = angleDiff(theta,x0[2]);
			if( fabs(theta_diff) > deg2rad(max_angular_correction_deg) )
				continue;
			theta = x0[2] + theta_diff; // otherwise problems near +- PI

			const double * p_j = laser_ref->points[j].p;

			double c = cos(theta); double s = sin(theta);
			double t_x = p_j[0] - (c*p_i[0]-s*p_i[1]);
			double t_y = p_j[1] - (s*p_i[0]+c*p_i[1]);
			double t_dist = sqrt( square(t_x-x0[0]) + square(t_y-x0[1]) );

			if(t_dist > max_linear_correction)
				continue;

			/*double weight = 1/(laser_sens->cov_alpha[i]+laser_ref->cov_alpha[j]);*/
			double weight = 1;
			gsl_histogram_accumulate(hist, theta, weight);
			gsl_histogram_accumulate(hist, theta+2*M_PI, weight); /* be robust */
			gsl_histogram_accumulate(hist, theta-2*M_PI, weight);
			count ++;
		}
	}
	*num_correspondences = count;
	sm_debug(" correspondences = %d\n",count);
}

void ght_one_shot(LDP laser_ref, LDP laser_sens,
		const double*x0, double max_linear_correction,
	double max_angular_correction_deg, int interval, double*x, int*num_correspondences)
{
	/** Compute laser_sens's points in laser_ref's coordinates by roto-translating by x0 */
	ld_compute_world_coords(laser_sens, x0);

	double L[3][3]  = {{0,0,0},{0,0,0},{0,0,0}};
	double z[3] = {0,0,0};

	int count = 0;
	int i;
	for(i=0;i<laser_sens->nrays;i++) {
		if(!laser_sens->alpha_valid[i]) continue;
		if(i % interval) continue;

		const double * p_i = laser_sens->points_w[i].p;

		const double * p_i_w = laser_sens->points_w[i].p;
		int from; int to; int start_cell;
		possible_interval(p_i_w, laser_ref, max_angular_correction_deg,
			max_linear_correction, &from, &to, &start_cell);
//		from = 0; to = laser_ref->nrays-1;

		int j;
		for(j=from;j<=to;j++) {
			if(j % interval) continue;
			if(!laser_ref->alpha_valid[j]) continue;

			double theta = angleDiff(laser_ref->alpha[j], laser_sens->alpha[i]);
			double theta_diff = angleDiff(theta,x0[2]);
			if( fabs(theta_diff) > deg2rad(max_angular_correction_deg) )
				continue;
			theta = x0[2] + theta_diff; // otherwise problems near +- PI

			const double * p_j = laser_ref->points[j].p;

			double c = cos(theta); double s = sin(theta);
			double t_x = p_j[0] - (c*p_i[0]-s*p_i[1]);
			double t_y = p_j[1] - (s*p_i[0]+c*p_i[1]);
			double t_dist = sqrt( square(t_x-x0[0]) + square(t_y-x0[1]) );

			if(t_dist > max_linear_correction)
				continue;

			/*double weight = 1/(laser_sens->cov_alpha[i]+laser_ref->cov_alpha[j]);
			double weight = exp( -square(t_dist) - 5 * square(theta-x0[2]) );*/

			double weight = 1;

			double alpha = laser_ref->alpha[j];
			double ca = cos(alpha); double sa=sin(alpha);

//				printf("%d ", (int) rad2deg(theta));

/*			printf("valid %d alpha %f weight %f t_x %f t_y %f\n",
				laser_ref->alpha_valid[j],alpha,weight,
				t_x, t_y); */
			z[0] += weight*(ca*ca*t_x + sa*ca*t_y);
			z[1] += weight*(sa*ca*t_x + sa*sa*t_y);
			z[2] += weight*theta;
			L[0][0] += weight* ca * ca;
			L[0][1] += weight* sa * ca;
			L[1][0] += weight* sa * ca;
			L[1][1] += weight* sa * sa;
			L[2][2] += weight;

			count += 1;
		}
	}

	*num_correspondences = count;

	if(1) {

		double weight = 0.5 * count;
		z[0] += x0[0] * weight;
		z[1] += x0[1] * weight;
		L[0][0] += weight;
		L[0][1] += 0;
		L[1][0] += 0;
		L[1][1] += weight;
	}

	egsl_push();
		val eL = egsl_alloc(3,3);
			size_t a,b;
			for(a=0;a<3;a++)
			for(b=0;b<3;b++)
				*egsl_atmp(eL,a,b) = L[a][b];

/*		egsl_print("eL", eL);*/
		val ez = egsl_vFa(3,z);

		val ex = m(inv(eL), ez);

		egsl_v2a(ex, x);

/*		egsl_print("eL", eL);
		egsl_print("ez", ez);
		egsl_print("ex", ex); */

	egsl_pop();

//	sm_debug("gpm: second step: theta = %f   %f / %d = %f \n", rad2deg(x[2]), rad2deg(z[2]), count, rad2deg(z[2]) / count);
	sm_debug("gpm: second step: found %d correspondences\n",count);

}