// code for fitting the OU spectrum to a given periodogram
#ifndef FIT_OUT
#define FIT_OU


#include "Config.h"
#include "InternalDefs.h"
#include "Auxiliary.cpp"
#include "../ALGLIB/src/dataanalysis.h"
#include "../ALGLIB/src/interpolation.h"
#include "../ALGLIB/src/stdafx.h"
using namespace std;



double fitted_RSS_OU(	const alglib::real_2d_array	&x,
				const alglib::real_1d_array	&y,
				const alglib::real_1d_array &c,
				void (*func)(const alglib::real_1d_array &c, const alglib::real_1d_array &x, double &func, void *ptr)){
	double RSS=0, ty;
	void *ptr;
	long n;
	alglib::real_1d_array tx; tx.setlength(1);
	for(n=0; n<y.length(); ++n){
		tx[0] = x[n][0];
		func(c,tx,ty,ptr);
		RSS += SQ(ty - y[n]);
	}
	return RSS;
}


inline double fit_PS_OU_aux(const alglib::real_1d_array &c, double x){
	// s_o=SQ(c[0]), lambda=c[1]
	return SQ(c[0]) * SQ(c[1])/(SQ(2*PI*x) + SQ(c[1]));
}

inline void fit_PS_OU_aux(const alglib::real_1d_array &c, const alglib::real_1d_array &x, double &func, void *ptr){
	func = 	fit_PS_OU_aux(c, x[0]);
}


void fit_grad_PS_OU_aux(const alglib::real_1d_array &c, const alglib::real_1d_array &x, double &func, alglib::real_1d_array &grad, void *ptr){
    // this callback calculates PS(c,x) and gradient = nabla_c PS(c,x)
	// s_o=SQ(c[0]), lambda=c[1]
    func = fit_PS_OU_aux(c,x[0]);
    grad[0] = 2*c[0] * SQ(c[1])/(SQ(2*PI*x[0]) + SQ(c[1]));
    grad[1] = 2 * SQ(c[0]) *c[1]*SQ(2*PI*x[0])/SQ(SQ(2*PI*x[0]) + SQ(c[1]));

}

inline void fit_log_PS_OU_aux(const alglib::real_1d_array &c, const alglib::real_1d_array &x, double &func, void *ptr){
	func = 	log(fit_PS_OU_aux(c, x[0]));
}


void fit_grad_log_PS_OU_aux(const alglib::real_1d_array &c, const alglib::real_1d_array &x, double &func, alglib::real_1d_array &grad, void *ptr){
	fit_grad_PS_OU_aux(c,x,func,grad,NULL);
	grad[0] /= func;
	grad[1] /= func;
	func 	 = log(func);
}







bool fit_PS_OU(	double 					df,
				const vector<double> 	&spectrum,
				double					suspected_power_o,
				double					suspected_lambda,
				double					&fitted_power_o,
				double					&fitted_lambda){
	long count = spectrum.size()-1;
	long n,m,k,q,p;
	alglib::real_2d_array x;
	x.setlength(count, 1);
	for(n=1; n<spectrum.size(); ++n){ x[n-1][0] = n*df; }
	
    alglib::real_1d_array y;
    y.setlength(count);
    for(n=1; n<spectrum.size(); ++n){ y[n-1] = (PS_OU_FIT_LOGARITHMICALLY ? log(spectrum[n]) : spectrum[n]); }
    
    alglib::real_1d_array c,oc;
    oc.setlength(2);
	oc[0] = sqrt(abs(suspected_power_o));	//power_o
	oc[1] = abs(suspected_lambda);			//lambda
	
	alglib::real_1d_array scales;
	scales.setlength(oc.length());
	for(n=0; n<oc.length(); ++n) scales[n] = oc[n];

	
    double epsf = PS_FIT_RELATIVE_ACCURACY*(PS_OU_FIT_LOGARITHMICALLY ? log(maximum(spectrum,1,spectrum.size()-1))-log(minimum(spectrum,1,spectrum.size()-1)) : maximum(spectrum,1,spectrum.size()-1));
    double epsx = df/10000;
    alglib::ae_int_t maxits = PS_OU_ALGLIB_FIT_MAX_ITERATIONS;
    alglib::ae_int_t info;
    alglib::lsfitstate state;
    alglib::lsfitreport rep;
    double diffstep = min(df/10000, suspected_lambda/10000);
    
    vector<double> trials_lambda, trials_power_o;
    for(n=-PS_OU_FIT_PARAMS_TRY_PLUSMINUS_ORDERDOFMAGNITUDE; n<=PS_OU_FIT_PARAMS_TRY_PLUSMINUS_ORDERDOFMAGNITUDE; ++n){
    	trials_power_o.push_back(oc[0]*pow(3, 1.0*n));
    }
    for(n=-PS_OU_FIT_PARAMS_TRY_PLUSMINUS_ORDERDOFMAGNITUDE; n<=PS_OU_FIT_PARAMS_TRY_PLUSMINUS_ORDERDOFMAGNITUDE; ++n){
    	trials_lambda.push_back(oc[1]*pow(3, 1.0*n));
    }
    vector<double> trials_RSS;
    vector<alglib::real_1d_array> trials_c;
    
	for(q=0; q<trials_lambda.size(); ++q){
		for(p=0; p<trials_power_o.size(); ++p){
			c = oc;
			c[0] = trials_power_o[p];
			c[1] = trials_lambda[q];
			
			try{
				if(FIT_PS_USE_GRADIENT){ alglib::lsfitcreatefg(x, y, c, true, state); //define data and function family
				}else{ alglib::lsfitcreatef(x, y, c, diffstep, state); }
				alglib::lsfitsetcond(state, epsf, epsx, maxits); //stoping conditions
				alglib::lsfitsetscale(state, scales); //define parameter scales								
				//fit:
				if(FIT_PS_USE_GRADIENT){
					if(PS_OU_FIT_LOGARITHMICALLY){ alglib::lsfitfit(state, fit_log_PS_OU_aux, fit_grad_log_PS_OU_aux); }
					else{ alglib::lsfitfit(state, fit_PS_OU_aux, fit_grad_PS_OU_aux); }
				}else{ 
					if(PS_OU_FIT_LOGARITHMICALLY){ alglib::lsfitfit(state, fit_log_PS_OU_aux); }
					else{ alglib::lsfitfit(state, fit_PS_OU_aux); }
				}
				alglib::lsfitresults(state, info, c, rep);   //read fit results						
			}
			catch (...) { return false; }
			if(int(info)>0){
				trials_c.push_back(c);
				if(PS_OU_FIT_LOGARITHMICALLY){	trials_RSS.push_back(fitted_RSS_OU(x, y, c, fit_log_PS_OU_aux)); }
				else{ trials_RSS.push_back(fitted_RSS_OU(x, y, c, fit_PS_OU_aux)); }
			}
		}
	}	
	if(trials_RSS.empty()) return false;
	long bestTrial=minimumIndex(trials_RSS);
	c = trials_c[bestTrial];
	
    fitted_power_o 	= SQ(c[0]);
    fitted_lambda 	= abs(c[1]);
    return true;
}


#endif