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


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

double modelH2O(int n, double *p, double x );

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

//Removal routine
int sofieH2O(   double *ptr_signal, 
    			double *ptr_altitude, 
    			double* ptr_time,
    			char* ptr_mode, 
    			double* ptr_residual, 
    			int* ptr_data_size, 
    			double* ptr_startAlt, 
    			double* ptr_decayConstant, 
    			double* ptr_Aparameters,
    			double* ptr_weight,
    			double* ptr_startBA,
    			double* ptr_endBA)
{

/***********************************************************************************
*
* This function corrects the response problem from the signal data array (ptr_signal). 
* See the function "modelH2O" at the end of this file for details on the model definition
* used.
* The extinction is first calculated, taking the measurement at z0 = 100 km as the exoatmospheric reference. 
* It is assumed for this calculation that above 100km no attenuation due to the atmosphere is present. Therefore,
* above z0 the oscillation in the extinction is completely exoatmospheric. The response decay is fitted with the
* model above 100km and extrapolated over the entire data. The residual (extinction data - fitted response decay)
* is calculated and returned to the address indicated by "ptr_residual". The model's decay is a
* fixed parameter 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) (convention followed is that lowest index is lowest altitude)
*   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_data_size.........number of datapoints in the event
*   ptr_startAlt..........starting altitude of the fit
*   ptr_decayConstant.....fixed decay constant for the response model
*   ptr_Aparameters.......address where the array for the resulting parameters will be stored
*   ptr_startBA...........time at which the Balance Adjustment (BA) procedure starts (in s). Set to 0.0 if the function should calculate the start of the
*		..........BA procedure. 
*   ptr_endBA.............time at which the BA procedure ends (in s). Set to 0.0 if the function should calculate the end of the BA procedure.
*		..........WARNING: The given BA position must be outside the BA, otherwise the fit will not converge (it will take data during the BA procedure).
*
* Optional input: 
*
*   ptr_weight............uncertainty in measurements for the fit (error of each measurement). If the input is 0, the default estimated value is used (0.504).
*
* Output:
*   ptr_residual..........contains the residual signal after applying correction (counts)
*   ptr_Aparameters.......address where the array for the resulting parameters will be stored

*** 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.
	- Time is assumed to increase with index for both sunset (SS) and sunrise (SR) events

* Authors: Greg Paxton, GATS, Inc.
*          David Gomez Ramirez, Hampton University
*          Dr. John McNabb, Hampton University
*	   
************************************************************************************/
/*********CHANGE LOG************/
/*******************************/

	//	printf("Initializing function. \n");
//Constants
	const int i_nParameters = 3;					//Number of parameters in the model

	//initialGuess =                [amplitude, t0, decay] Look at model for description.
	const double initialGuess[3] = {20000.0,0.0,*ptr_decayConstant};	//Initial guess for the 3 parameters.

//Variable declaration
	double d_startAlt = *ptr_startAlt;				//Min altitude for the fit (Km)
	double d_startBA  = *ptr_startBA;
	double d_endBA    = *ptr_endBA;

    int startIndex     = 0;                         //Starting index for the fit
    int endIndex       = 0;                         //Ending index for the fit
    int z0Index        = 0;                         //Index for the starting altitude

	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] = 1.0;				//Default estimated error in data. Simply a one which should have no
			                                //effect on the output
		}else{
			darr_ey[i] =*ptr_weight;			//Use provided error for the data
		}
	}

//Variables required for the mpfit function
	struct vars_struct vars;						//Contains the data, x,y and ey (weight)	

	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

	int i_statusMPFIT;							//Stores the status of the fit

//Variables for the type of event
	int mode = *ptr_mode;

//////////////////////////////////////////////
///////////////PROGRAM START//////////////////
//////////////////////////////////////////////

//Extract indices for the fit
//mode: 0 = sunrise, 1 = sunset 
    if (mode)
    {
        for(i = 0; i < (*ptr_data_size - 1); i++)
        {
            if((float)ptr_time[i] < d_endBA && (float)ptr_time[i+1] >= d_endBA)				//When time exceeds the end of the balance adjustment
            {
                startIndex = i + 1;
            }
            if((float)ptr_altitude[i] >= d_startAlt && (float)ptr_altitude[i+1] < d_startAlt)				//When altitude exceeds the exoatmospheric threshold
            {
                endIndex = i;
                z0Index = i;
                i = *ptr_data_size;
            }
        }
    }
    else
    {
        for(i = 0; i < (*ptr_data_size - 1); i++)
        {
            if((float)ptr_altitude[i] < d_startAlt && (float)ptr_altitude[i+1] >= d_startAlt)				//When altitude exceeds the exoatmospheric threshold
            {
                startIndex = i + 1;
                z0Index = i;
            }
            if((float)ptr_time[i] <= d_startBA && (float)ptr_time[i+1] > d_startBA)				//When time exceeds the start of the balance adjustment
            {
                endIndex = i;
                i = *ptr_data_size;
            }
        }
    }

    int fitPoints = endIndex - startIndex;
	double darr_fitTime[fitPoints];
	double darr_fitSignals[fitPoints];
	double darr_eyFit[fitPoints];

//Fill arrays with only the data we want to fit
	for(i = 0; i < fitPoints; i++ )
	{
	    darr_fitTime[i] = ptr_time[i+startIndex];
	    darr_fitSignals[i] = ptr_signal[i+startIndex];
	    darr_eyFit[i] = darr_ey[i+startIndex];
	}	

//Data and variables definition for the mpfit function
	vars.x = darr_fitTime;
	vars.y = darr_fitSignals;
	vars.ey = darr_eyFit;

	mp_result result;
	bzero(&result,sizeof(result));		//Zero results structure, (initialization)
	result.xerror = perror;
	
	//Format of the parameters = [x0, Cba pre, Cba post, indexBA, Amp, Decay, Frequency, Phase, Slope]
	double parameterValues[] = {initialGuess[0], initialGuess[1], initialGuess[2]};

	pars[0].fixed = 0;                      //Fix parameter Amplitude
	pars[1].fixed = 0;                      //Fix parameter t0
	pars[2].fixed = 1;                      //Fix parameter Decay


//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(residuals, fitPoints, i_nParameters, parameterValues, pars, &config, (void *) &vars, &result);
	//i_statusMPFIT = mpfit(residuals, fitPoints, i_nParameters, parameterValues, 0, &config, (void *) &vars, &result);
	printf("H2O_Response: MPFIT Status   = %d\n", i_statusMPFIT);
	//	printf("Fit performed.\n");

	printf("H2O_Response: Parameters: A = %f\tt0 = %f\ta = %f\n", parameterValues[0], parameterValues[1], parameterValues[2]);

//Extrapolation
    double z0Corrected = modelH2O(i_nParameters, parameterValues, ptr_time[z0Index]);
    double corrected = 0.;

    for (i = 0; i < i_nParameters; i++)
    {
        ptr_Aparameters[i] = parameterValues[i];
    }

	//Evaluating model
	for (i=0; i<*ptr_data_size; i++) {
		corrected = modelH2O(i_nParameters, parameterValues, ptr_time[i] );

		//Residual calculation
		ptr_residual[i] = ptr_signal[i] * (z0Corrected / corrected);
	}

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

	return 1;	//Success
}


/*************************************************/
//////////////Additional Functions/////////////////
/*************************************************/
//Residuals acquired from the modelH2O function
int residuals(int m, int n, double *p, double *deviates, double **derivs, void *ptr_vars)
{
/***************************************************************************************
* This routine returns the differences between the model and the measured signals
*
* 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 [A,x0,a]
*   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: Greg Paxton, GATS, Inc.
*          Dr. John McNabb, Hampton University
*	       David Gomez, Hampton University
*************************************************************************************/	


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

	/* Compute function deviates */
	for (i = 0; i < m; i++)
	{
		deviates[i] = (y[i] - modelH2O(n, p, x[i] )) / ey[i];
	}

	return 0;
}

//Model function
double modelH2O(int n, double *p, double x )
{
/***************************************************************************************
* This routine models the exponential decay function.  y = A{1 - exp[-(x - x0) / a]}
*
* Called from residuals().
*
* Required Input:
*   n............number of parameters
*   p............address of parameter array with the form [A,x0,a]
*   x............input measured x value
*
* Output:
*
*   y............output model value at x
*
* Author: Greg Paxton, GATS, Inc.
*************************************************************************************/	


	double A  = p[0];
	double x0 = p[1];
	double a  = p[2];
	double y  = 0.0;

    y = A * (1 - exp(-(x - x0) / a));

	return y;
}