//Libraries
#include <stdio.h>
#include <math.h>
#include "mpfit.h"	//Downloaded from http://www.physics.wisc.edu/~craigm/idl/cmpfit.html
//#include "idl_export.h"	


//Declarations
int model(int m, int n, double *p, double *deviates, double **derivs, void *private);

struct vars_struct{
  double *x;
  double *y;
  double *ey;
};




//Removal routine
int sofieNO1_natural(	double *ptr_signal, 
			double *ptr_altitude, 
			double* ptr_time,
			char* ptr_mode, 
			double* ptr_residual, 
			char* ptr_band, 
			int* ptr_data_size, 
			double* ptr_resChis, 
			double* ptr_maxChis, 
			int* ptr_chisFlag, 
			double* ptr_Aparameters,
			double* ptr_SigmaParameters,
			double* ptr_maxUncertainty,
			double* ptr_minUncertainty,
			int* ptr_bumpFlag, 
			double* ptr_maxBump,
			double* ptr_weight)
{

/***********************************************************************************
*
* This function removes the thermal oscillation from the signal data array (ptr_signal). 
* See the function "model" at the end of this file for details on the model definition
* used.
* The extinction is first calculated, taking the measurement at z0 = 140 km as the exoatmospheric reference. 
* It is assumed for this calculation that above 140km no attenuation due to the atmosphere is present. Therefore,
* above z0 the oscillation in the extinction is completely exoatmospheric. The oscillation is fitted with the
* model above 140km and extrapolated over the entire data. The residual (extinction data - fitted oscillation)
* is calculated, then it is transformed again to the 'photon count' original units, and finally it is returned
* to the address indicated by "ptr_residual". The model's decay and frequency are fixed parameters in the
* model definition. 
*
* Required Input: (All inputs are pointers to arrays containing the specified data)
*
*   ptr_signal............original radiometer signal array (counts) 
*   ptr_altitude..........altitude for the event measurement (km)
*   ptr_time..............time for the event measurement (sec) 
*   ptr_mode..............occultation mode, 0 = sunrise, 1 = sunset 
*   ptr_residual..........address where the residual oscillation is stored 
*   ptr_band..............band to be fitted
*   ptr_data_size.........number of datapoints in the event
*   ptr_resChis...........address where the resulting reduced Chi^2 parameter will be saved
*   ptr_maxChis...........threshold for the maximum expected reduced Chi^2 parameter (normally 3-5 are acceptable thresholds since this is an adhoc calculation)
*   ptr_chisFlag..........address where the flag indicating if the reduced Chi^2 parameter complied with the specified threshold will be stored
*   ptr_Aparameters.......address where the array for the resulting parameters will be stored
*   ptr_SigmaParameters...address where the array for the 1 sigma uncertainties of the parameters will be stored
*   ptr_maxUncertainty....address where the array for the maximum uncertainty at each altitude will be stored
*   ptr_minUncertainty....address where the array for the minimum uncertainty at each altitude will be stored
*   ptr_bumpFlag..........address where the flag indicating if there is negative extinction in the residual will be stored
*   ptr_maxBump...........threshold for the maximum expected negative extinction (in extinction units) (example: -0.0003, -0.0002)
*
*
* Optional input: 
*
*   ptr_weight............uncertainty in measurements for the fit (1/1sigma_data)^2. If the input is 0, the default estimated value is used (4.166667E9).  
*
*
* Output:
*   ptr_residual..........contains the residual signal after applying correction (counts)
*   ptr_resChis...........contains the resulting reduced Chi^2 for the fit
*   ptr_chisFlag..........address where the flag indicating if the reduced Chi2 parameter complied with the specified threshold will be stored
*			  0 if complied, 1 if not
*   ptr_Aparameters.......address where the array for the resulting parameters will be stored
*   ptr_SigmaParameters...address where the array for the 1 sigma uncertainties of the parameters will be stored
*   ptr_maxUncertainty....address where the array for the maximum uncertainty at each altitude will be stored
*   ptr_minUncertainty....address where the array for the minimum uncertainty at each altitude will be stored
*   ptr_bumpFlag..........address where the flag indicating if there is negative extinction in the residual will be stored
*			  0 if complied, 1 if not

*** IMPORTANT REMARKS ***
	- Memory space allocation for each of the variables indicated by the pointers must be done prior to the call of the function. 
	- Also scalar values must have proper memory space allocation, since they are treated as pointers.
	- The reduced chi^2 parameter is an ad hoc calculation. The error in each data point was estimated so that good fits give a 
		reduced chi^2 parameter ranging from 1 to 3 approximately. 


* Authors: David Gomez Ramirez, Hampton University
*          Dr. John McNabb, Hampton University
*	   
************************************************************************************/


	//	printf("Initializing function. \n");
//Constants
	const double d_pi = 3.14159265;
	const double i_z0 = 140.0;					//Altitude (km) at which the fitting starts
	const int i_minFitz = 140;					//Min altitude for the fit (Km)
	const int i_maxFitz = 200;					//Max altitude for the fit (Km)

	const int i_peakRange = 40;					//Max number of datapoints at end of file that could have the peak
	const float f_peakThreshold =  0.00015;				//Change between two consecutive normalized points above which it is considered a peak
	
	const int i_nParameters = 5;


//variable declaration
	double darr_extinction[*ptr_data_size];
	double darr_spikeDetection[i_peakRange];
	double darr_timeDif[*ptr_data_size];
	double darr_extrapolated[*ptr_data_size];

	
	struct vars_struct vars;					//Contains the data, x,y and ey (weight)	
	double darr_ey[*ptr_data_size];					//Weight = 1/variance = 1/dev^2
//Initialize array to default value
	int i;
	for(i = 0; i <= (*ptr_data_size)-1; i++)
	{
		if(*ptr_weight == 0.0){
			darr_ey[i] =4.166667E9;				//Default estimated error in data, used as an adhoc weight for the fit 
		}else{
			darr_ey[i] =*ptr_weight;			//Use provided error for the data
		}
	}

	double d_x0;	
	double d_t0;
	int index_z0;
	int index_minFit;
	int index_maxFit;
	int index_spikeInit;
 
//Variables required for the mpfit function
	double perror[i_nParameters];						//Error array for the resulting parameters
	for(i = 0; i < i_nParameters; i++)
	{
		perror[i] = 0.0;						//Initialize the array 
	}


	mp_par pars[i_nParameters];						//Array containing struct with restrictions
	bzero(pars, sizeof(pars));           					//Initialize constraint structure

	double parameterValues[] = {0.0004,-0.032414,0.670007,0.0,0.0};		//Initial guess. These only apply to B16
	int i_statusMPFIT;							//Stores the status of the fit


//Variables for the deviation calculation
	int iarr_deviationScale[] = {1,1,1,1,1};				//Scale factor for the sigma uncertainty of each parameter
	double maxFit[*ptr_data_size];
	double minFit[*ptr_data_size];

	double errorTau = 0.004328;						//Use the std. deviations as uncertainty for these parameters
	double errorOmega = 0.005999;

    double dzMin= 200.;
	
//Extract index for altitude Z0 = 140km
	for(i = 0; i < (*ptr_data_size); i++)
	{
//printf(" alt  %f\n", ptr_altitude[i] );

		if( fabs(ptr_altitude[i]- i_z0) < dzMin ){				//Altitude truncated to floating precision for comparison
			index_z0 = i;		
            dzMin = fabs(ptr_altitude[i]- i_z0); 
		}
	}	
//return 12;
	

//		printf("Obtained Z0 %i, %i.\n", index_z0, *ptr_data_size);
//Extract signal and time values at Z0 = 140km
	d_x0 = *(ptr_signal + index_z0);
	d_t0 = *(ptr_time + index_z0);

	//	printf("Calculated X0 and T0.\n");


//Calculate extinction		
	for(i = 0; i < *ptr_data_size; i++ )
	{
		darr_extinction[i] = 1 - (ptr_signal[i])/d_x0;  
	}	

	//	printf("Calculated Extinction.\n");
	
//Calculate time differential (tdifferential = time - time at Z0)		
	for(i = 0; i < *ptr_data_size; i++ )
	{
		darr_timeDif[i] = (ptr_time[i]) - d_t0;    
	}	


	//	printf("Calculated time differential.\n");
		
//Get fitting index
    dzMin = 1000;
	index_maxFit = *ptr_data_size;			//Default is top of the atmosphere, or the last data point available
	for(i = 0; i < (*ptr_data_size); i++)
	{
	//		printf("Altitude: %lf \n",ptr_altitude[i] );
		if(fabs( ptr_altitude[i] -i_maxFitz) < dzMin   ){
			index_maxFit = i;
            dzMin = fabs( ptr_altitude[i] -i_maxFitz);
		}
	}	

	//	printf("Obtained fitting index.\n");
    dzMin = 1000.;
	for(i = 0; i < (*ptr_data_size); i++)
	{
		if(fabs( ptr_altitude[i] - i_minFitz) < dzMin) {	//Altitude truncated to floating precision for comparison
			index_minFit = i;
            dzMin = fabs( ptr_altitude[i] - i_minFitz);
		}		
	}	
//printf("Extracted data for fit.\n");

//Spike detection (Only applies for SR events)
	if(*ptr_mode == 0)	//If SR
	{
		index_spikeInit = 0;
		for(i = 0; i<=i_peakRange;i++){
			darr_spikeDetection[i] = darr_extinction[(index_maxFit-i_peakRange)+i]  - darr_extinction[(index_maxFit-i_peakRange)+i-1];
			darr_spikeDetection[i] = fabs(darr_spikeDetection[i]);
			

			if(darr_spikeDetection[i] > f_peakThreshold){
				index_spikeInit = i_peakRange-i+1;	//This is what needs to be substracted from the index_maxFit to avoid the spike.
				break;					//Only consider the first one found			
			}		
		}
		index_maxFit = index_maxFit - index_spikeInit;

	//		printf("Spike detected.\n");

	}  else {   //  We reverse these for Sunsets
       int k = index_maxFit;
       index_maxFit = index_minFit;
       index_minFit = k; 

    }


//printf("Spike procedure completed.\n");


//Extract data for the fit
	int i_fitElements = index_maxFit-index_minFit + 1;				//The +1 is to account for the element AT maxFit
	//	printf("iFit:%i\niMax:%i\niMin:%i\n", i_fitElements,index_maxFit, index_minFit);

	double darr_timeDifFit[i_fitElements];
	double darr_extinctionFit[i_fitElements];
	double darr_eyFit[i_fitElements];

	for(i=0; i<i_fitElements;i++)
	{
		darr_timeDifFit[i] = darr_timeDif[index_minFit+i];
		darr_extinctionFit[i] = darr_extinction[index_minFit+i];
		darr_eyFit[i] = darr_ey[index_minFit+i];
	}

//printf("Fit variables initialized. %d   %d %d \n", i_fitElements, index_maxFit, index_minFit);
//return 12;
	
//Data arrangement for mpfit function
	vars.x = darr_timeDifFit;
	vars.y = darr_extinctionFit;
	vars.ey = darr_eyFit;

	mp_result result;
	bzero(&result,sizeof(result));		//Zero results structure, (initialization)
	result.xerror = perror;
	
	pars[1].fixed = 1;                      //Fix parameter Decay
	pars[2].fixed = 1;                      //Fix parameter Frequency


//Configuration for MPFIT
	mp_config config;
	bzero(&config, sizeof(config));
    config.maxfev = 1000;
    config.nofinitecheck=1;

//printf("MPFIT initialized.\n");


//Calling fitting routine
	i_statusMPFIT = mpfit(model, i_fitElements, i_nParameters, parameterValues, pars, &config, (void *) &vars, &result);

//printf("Fit performed.\n");

//Retrieve Red Chi^2 for the fit:
	*ptr_resChis = result.bestnorm/(result.nfunc-result.nfree);	//Chi^2 / DoF The best norm needs to be multiplied by the error (constant)


//Retrieve final value for parameters
	for(i = 0; i<i_nParameters; i++)
	{
		ptr_Aparameters[i] = parameterValues[i];
	}



//Extrapolation
	double poly,s,e, d_amp,d_tau,d_ome,d_phi,d_slo;
	d_amp = parameterValues[0]; 
	d_tau = parameterValues[1]; 
	d_ome = parameterValues[2]; 
	d_phi = parameterValues[3]; 
	d_slo = parameterValues[4]; 


	for(i = 0; i < *ptr_data_size; i++ )
	{
		//Evaluating model	
		poly=-d_amp*sin(d_phi)+d_slo*darr_timeDif[i];
		s=sin(darr_timeDif[i]*d_ome+d_phi);
		e=exp(darr_timeDif[i]*d_tau);

		darr_extrapolated[i] = (d_amp*e*s+poly);

		ptr_residual[i] = darr_extinction[i] - darr_extrapolated[i];    
	}	

//Complete uncertainties for fixed parameters (Tau and Omega) with their calculated standard deviations
	
	result.xerror[1] = errorTau;
	result.xerror[2] = errorOmega;
	
//Return the 1 sigma uncertainties of the parameters
	for(i = 0; i<i_nParameters; i++)
	{
		ptr_SigmaParameters[i] = result.xerror[i];
	}
		


//Initialize maxFit and minFit array to the value of the fit
	for(i = 0; i <= (*ptr_data_size)-1; i++)
	{
		maxFit[i] = darr_extrapolated[i];
		minFit[i] = darr_extrapolated[i];
	}
	
//Uncertainty calculation, varies each parameter by +, - or no uncertainty and verifies if max or min at each altitude
	double tempFit[*ptr_data_size];	
	int iAmp, iDec, iOme, iPhi, iSlo;
	double d_tempAmp,d_tempTau,d_tempOme,d_tempPhi,d_tempSlo;	
	for (iAmp = -1; iAmp <= 1; iAmp++){
	for (iDec = -1; iDec <= 1; iDec++){
	for (iOme = -1; iOme <= 1; iOme++){
	for (iPhi = -1; iPhi <= 1; iPhi++){
	for (iSlo = -1; iSlo <= 1; iSlo++){
		//Vary the parameters
		d_tempAmp = d_amp + result.xerror[0]*iAmp*iarr_deviationScale[0];	
		d_tempTau = d_tau + result.xerror[1]*iDec*iarr_deviationScale[1];	
		d_tempOme = d_ome + result.xerror[2]*iOme*iarr_deviationScale[2];	
		d_tempPhi = d_phi + result.xerror[3]*iPhi*iarr_deviationScale[3];	
		d_tempSlo = d_slo + result.xerror[4]*iSlo*iarr_deviationScale[4];	

		//Evaluating model	
		for(i = 0; i <= *ptr_data_size-1; i++ )
		{
			poly=-d_tempAmp*sin(d_tempPhi)+d_tempSlo*darr_timeDif[i];
			s=sin(darr_timeDif[i]*d_tempOme+d_tempPhi);
			e=exp(darr_timeDif[i]*d_tempTau);
	
			tempFit[i] = (d_tempAmp*e*s+poly);
		}	

		//Verify Max or Min fit		
		for(i = 0; i <= *ptr_data_size-1; i++ )
		{
			if(tempFit[i] > maxFit[i])
			{
				maxFit[i] = tempFit[i];
			}

			if(tempFit[i] < minFit[i])
			{
				minFit[i] = tempFit[i];
			}
		}
		
	}}}}}

	//	printf("Uncertainty calculated.\n");

//Return the new max and min uncertainty
	for(i = 0; i < *ptr_data_size; i++ )
	{
		//Return the new uncertainty in terms of 'photon counts' (NOT extinction)
		ptr_maxUncertainty[i] = (1-(darr_extinction[i]-maxFit[i]))*d_x0;
		ptr_minUncertainty[i] = (1-(darr_extinction[i]-minFit[i]))*d_x0;
	}


//Calculate residual		
	for(i = 0; i < *ptr_data_size; i++ )
	{
		//Return the new uncertainty in terms of 'photon counts' (NOT extinction)
		ptr_residual[i] = (1-(darr_extinction[i]-darr_extrapolated[i]))*d_x0;
	}	

//Flag calculation
	*ptr_chisFlag = 0;			//Red. Chi^2 is within spec
	//	printf("Chis: %f, %f \n",*ptr_resChis, *ptr_maxChis);

	if(*ptr_resChis > *ptr_maxChis){
		*ptr_chisFlag = 1;		//Flag activation
	}

	*ptr_bumpFlag = 0;			//Bump is within spec
//Look for bump
	for(i = 0; i < *ptr_data_size; i++ )
	{

	//		printf("Bump: %f, %f \n",darr_extinction[i]-darr_extrapolated[i], *ptr_maxBump);

		if((darr_extinction[i]-darr_extrapolated[i]) < *ptr_maxBump){	//Max bump must be in extinction units!
			*ptr_bumpFlag = 1;		//Flag activation
		}	
	}


	return 1;	//Success
}



//Auto glue function. IDL gives the option of having the parameters passed in the form argc, argv
//Calling convention, denominated AUTO_GLUE in IDL documentation
int sofieNO1(int argc, void* argv[])
{
	if(argc != 17) return 0;

	double* signal = (double*) argv[0];
	double* altitude = (double*) argv[1];
	double* time = (double*) argv[2];
	char* mode = (char*) argv[3];
	double* residual = (double*) argv[4];
	char* band = (char*) argv[5];
	int* data_size = (int*) argv[6];
	double* ptr_resChis = (double*) argv[7];
	double* ptr_maxChis= (double*) argv[8];
	int* ptr_chisFlag= (int*) argv[9] ;
	double* ptr_Aparameters= (double*) argv[10]; 
	double* ptr_SigmaParameters = (double*) argv[11];
	double* ptr_maxUncertainty= (double*) argv[12];
	double* ptr_minUncertainty= (double*) argv[13];
	int* ptr_bumpFlag= (int*) argv[14];
	double* ptr_maxBump= (double*) argv[15];
	double* ptr_weight=(double*) argv[16];


	return sofieNO1_natural(signal,
			 	altitude, 
				time,
				mode, 
				residual, 
				band, 
				data_size, 
				ptr_resChis,
				ptr_maxChis,
				ptr_chisFlag,
				ptr_Aparameters,
				ptr_SigmaParameters,
				ptr_maxUncertainty,
				ptr_minUncertainty,
				ptr_bumpFlag,
				ptr_maxBump,
				ptr_weight);
}




/*************************************************/
//////////////Additional Functions/////////////////
/*************************************************/
//Model to be fitted
int model(int m, int n, double *p, double *deviates, double **derivs, void *ptr_vars)
{
/***************************************************************************************
* This routine models the thermal oscillation in the extinction. The following model definition 
* is used: f(X) = Amp*e^(X*Tao)*sin(X*Omega + phi) - Amp*sin(phi) + Slope*X 
* Directly called by MPFIT.
*
* Required Input (following convention required by MPFIT.c):
*   m............number of data points 
*   n............number of parameters
*   p............address of parameter array with the form [Amp,Tao,Omega,Phi,Slope]
*   deviates.....address of array where the deviates will be stored
*   derivs.......address of array where derivatives are calculated.
*   ptr_vars.....structure of the form ptr_vars containing the data for the model (independent, dependent variables, error, etc)
*	    .....for more information, consult the documentation of mpfit.c

* Output:
*
*   deviates.....deviation from required value is returned, this value will be minimized
*   derivs.......derivative calculation, if not specified, then the algorithm uses a finite difference approximation
*
* Authors: Dr. John McNabb, Hampton University
*	   David Gomez, Hampton University
*************************************************************************************/	



	double amp = p[0];
	double tau = p[1];
	double ome = p[2];
	double phi = p[3];
	double slo = p[4];

	//Extract data from the vars structure
	struct vars_struct *v = (struct vars_struct *) ptr_vars;
	double *x, *y, *ey;
	x = v->x;
	y = v->y;
	ey = v->ey;


	int i;
	double poly, s, e;
	/* Compute function deviates */
	for (i=0; i<m; i++) {
		poly=-amp*sin(phi)+slo*x[i];
		s=sin(x[i]*ome+phi);
		e=exp(x[i]*tau);
		deviates[i] = (y[i] - (amp*e*s+poly))*sqrt(ey[i]);
	}

	return 0;
}