#include "Level1Data.h"
#include "NonLinearity.h"
#include "ReferenceAero.h"
#include "GATS_Utilities.hpp"
#include "fortranfunctions.h"

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



#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_min.h>

struct params_den {
    std::vector<float> ncep_Z;
    std::vector<float> ncep_D;
    std::vector<float> sunsensor_Z;
    std::vector<float> sunsensor_D;
};


extern "C" double alt_diff_rms(double delta, void *params)
{
    params_den* p = (params_den* ) params;

    std::vector<float> zs = p->sunsensor_Z,  d_out = p->ncep_D;
    
    std::transform(zs.begin(), zs.end(), zs.begin(), 
    	std::bind2nd( std::plus<float>(), delta) );
    
    int n1= (int)zs.size(), n2 = (int)d_out.size();
    
    ::liner_(&zs[0], &(p->sunsensor_D[0]),  &(p->ncep_Z[0]), &d_out[0], &n1, &n2);
    
	double sum = 0.0;
    	for(int il=0; il< n2; ++il)
            sum += std::pow( (d_out[il]- p->ncep_D[il]) ,2 );

//	std::cout << std::sqrt(sum / n2 ) << std::endl; exit(23); 

    return  std::sqrt(sum / n2  );

}



extern "C" void adjust_l1_alts( float* za, float* density, int* nl,
	float* ztm, float* ptm, float* ttm, int* ntm)

{
//    return;
	std::cout << "Inside adjust_l1_alts...." << std::endl;

//  calculate density of the input NCEP profile
	std::vector<float> NCEP_D, NCEP_Z;
	for(int i=0; i< *ntm; ++i) {
	   if(ztm[i] <= 30.2 && ztm[i] >= 19.8) {
		NCEP_D.push_back (  100.*ptm[i]/(287.058*ttm[i])   );
		NCEP_Z.push_back( ztm[i] );
//std::cout << NCEP_D.back() << "  " << NCEP_Z.back() << std::endl;
	   }
	}
//std::cout <<"&" << std::endl;

//for (int i = 0; i< *nl; ++i) 
//std::cout << density[i] << "  "<< za[i] << std::endl;

//exit(23);



    int status, iter=0;
    const static int max_iter=50;


    double mm, a= -5 , b= 5, guess = 0.0;
        const gsl_min_fminimizer_type *T;
        gsl_min_fminimizer *solver;
    gsl_function F;

   params_den params ={ 
		NCEP_Z, 
		NCEP_D, 
		std::vector<float>(za,za+ *nl),
		std::vector<float>(density, density+ *nl)
		};

    F.params = (void*)&params;
    F.function = &alt_diff_rms;

        T = gsl_min_fminimizer_brent;
        solver = gsl_min_fminimizer_alloc (T);
    assert(solver);
        status = gsl_min_fminimizer_set (solver, &F, guess, a, b );
    assert(status != GSL_EINVAL);

       do
         {
           iter++;
           status = gsl_min_fminimizer_iterate (solver);

           mm = gsl_min_fminimizer_x_minimum (solver);
           a = gsl_min_fminimizer_x_lower (solver);
           b = gsl_min_fminimizer_x_upper (solver);

           status
             = gsl_min_test_interval (a, b, 1.e-3, 0.0);

         }
       while (status == GSL_CONTINUE && iter < max_iter);
       gsl_min_fminimizer_free (solver);

      assert(status == GSL_SUCCESS );

      std::cout << " altitude converge diff " << mm << "  km " << std::endl;
// only accept a shift of no more than 500 meters
      assert(std::fabs(mm) <= 0.5); 
// OK now lets apply this difference to the Level 1 altitudes... effectively re-registering a little       
       
       
       std::vector<double> Z = ::gLevel1Data.GetImpactAlts();
       std::transform(Z.begin(), Z.end(), Z.begin(), std::bind2nd( std::minus<double>(), mm) );
       
       ::gLevel1Data.Set_TP_Alts(Z);
       
//       exit(23);

}