#include <iostream>
#include <istream>
#include <ostream>
#include <cstdio>
#include <cmath>
#include <cstdlib>

#include "Header.hh"
#include "ElementContainer.hh"
#include "ElementContainerArray.hh"
#include "Domain.hh"
#include "ParamSet.hh"
#include "PeakData.hh"
#include "MovingAverage.hh"
#include "PeakSearch.hh"
//#include "CommonParamKeys.hh"
#include "NewLevmar.hh"
#include "Gaussian.hh"
#include "Lorentzian.hh"


Double dist(const Double x, const UInt4 nPeaks, const Double a[], const Double c[], const Double w[]) {

    Double sum=0.0;
    for (UInt4 i=0; i<nPeaks; ++i) {
        sum += a[i]*exp(-1.0*pow((x-c[i])/w[i], 2.0));
    }
    return sum;
}

ElementContainer init_src(const Double xmin, const Double xmax, const UInt4 N) {

    Double a[]={ 30000.0, 100000.0, 100000.0, 30000.0 };
    Double c[]={     3.0,      5.0,     10.0,    13.0 };
    Double w[]={     1.0,      2.5,      2.5,     1.0 };
    const UInt4 N_PEAKS=sizeof(a)/sizeof(a[0]);

    vector<Double> bin = *(new vector<Double>());
    Double delta=(xmax-xmin)/N;
    for (UInt4 i=0; i<=N; ++i) {
        bin.push_back(xmin + static_cast<Double>(i)*delta);
    }

    vector<Double> y = *(new vector<Double>());
    vector<Double> e = *(new vector<Double>());
    Double x, p, q, s, t;
    for (UInt4 i=0; i<N; ++i) {
        x=(bin.at(i) + bin.at(i+1))/2.0;
        p=dist(x, N_PEAKS, a, c, w);
        t=static_cast<Double>(rand())/static_cast<Double>(RAND_MAX);
        s=static_cast<Double>(rand())/static_cast<Double>(RAND_MAX)-0.5;
        q=(s > 0.0 ? 1.0 : (s==0.0 ? 0.0: -1.0))*(p/10.0)*sqrt(-1.0*log(t));
        y.push_back(floor(p+q));
        e.push_back(p/10.0);
    }

    ElementContainer ec = *(new ElementContainer());
    ec.AddToHeader("run number", 1);
    ec.AddToHeader("run level",  1);
    ec.AddToHeader("Inst.", "manyo");
    ec.Add("TOF",       bin, "sec."  );
    ec.Add("Intensity", y,   "count.");
    ec.Add("Error",     e,   "count.");
    ec.SetKeys("TOF", "Intensity", "Error");

    return ec;
}

void outputElementContainer(ElementContainer &ec) {

    for (UInt4 i=0; i<ec.PutSize(ec.PutYKey()); ++i) {
        std::cout << setw(5) << i;
        std::cout << " ";
        std::cout << "[";
        std::cout << setw(10) << setprecision(10) << setiosflags(std::ios::right) << ec.Put(ec.PutXKey(), i);
        std::cout << ", ";
        std::cout << setw(10) << setprecision(10) << setiosflags(std::ios::right) << ec.Put(ec.PutXKey(), i+1);
        std::cout << "]";
        std::cout << " ";
        std::cout << setw(23) << setprecision(16) << setiosflags(std::ios::right) << ec.Put(ec.PutYKey(), i);
        std::cout << " ";
        std::cout << setw(23) << setprecision(16) << setiosflags(std::ios::right) << ec.Put(ec.PutEKey(), i);
        std::cout << endl;
    }
}

Int4 main(Int4 argc, Char *argv[]) {

    Double xmin;
    Double xmax;
    UInt4  nDiv;

    std::cout << "xmin= "; std::cin >> xmin;
    std::cout << "xmax= "; std::cin >> xmax;
    std::cout << "nDiv= "; std::cin >> nDiv;
    ElementContainer src=init_src(xmin, xmax, nDiv);
    outputElementContainer(src);
    /*
    PeakSearch peakSearch = *(new PeakSearch(&src, new MovingAverage));
    peakSearch.setParam(MovingAverage::WINDOW_WIDTH, 201U);
    peakSearch.setParam(MovingAverage::WINDOW_UPPER, 100U);
    peakSearch.execute();
    PeakData peakData = peakSearch.getPeaks();
    peakData.Dump();
    */

    NewLevmar levmar = *(new NewLevmar());

    /* domain */
    Domain domain;
    domain.setSource(src);
    domain.setRange(0, src.PutSize(src.PutYKey())-1);

    /* functions */
    vector<FuncBase*> funcList = *(new vector<FuncBase*>());
    funcList.push_back(new Gaussian());
    funcList.push_back(new Gaussian());
    funcList.push_back(new Gaussian());
    funcList.push_back(new Gaussian());

    /* default values for fittinng parameters */
    vector<Double> fittingParams; // = peakData.toVector();
    fittingParams.push_back(100000.0);
    fittingParams.push_back(5.0);
    fittingParams.push_back(2.5);
    fittingParams.push_back(100000.0);
    fittingParams.push_back(10.0);
    fittingParams.push_back(2.5);
    fittingParams.push_back(30000.0);
    fittingParams.push_back(3.0);
    fittingParams.push_back(1.0);
    fittingParams.push_back(30000.0);
    fittingParams.push_back(13.0);
    fittingParams.push_back(1.0);
    vector<Double> referenceData = *(new vector<Double>());
    vector<Double> lowerBounds   = *(new vector<Double>());
    vector<Double> upperBounds   = *(new vector<Double>());

    string yKey=src.PutYKey();
    string eKey=src.PutEKey();
    //for (UInt4 i=domain.getLowerBoundID(); i<=domain.getUpperBoundID(); ++i) {
    ////    referenceData.push_back(src.Put(yKey, i)/src.Put(eKey, i));
    //}

    for (UInt4 i=0; i < fittingParams.size(); ++i) {
        lowerBounds.push_back(fittingParams.at(i)*0.9);
        upperBounds.push_back(fittingParams.at(i)*1.1);
    }


    ParamSet param=levmar.setDefaultParam(src);
    param.add(NewLevmar::FUNCTIONS, funcList);
    //param.add(NewLevmar::REFERENCE_VALUES, referenceData);
    param.add(NewLevmar::PARAMETER_VALUES, fittingParams);
    param.replace(NewLevmar::USE_DATA_WEIGHTS, false);

    param.add(NewLevmar::LOWER_BOUNDS, lowerBounds);
    param.add(NewLevmar::UPPER_BOUNDS, upperBounds);

    param.replace(NewLevmar::CONSTRAIN, NewLevmar::NO_CONSTRAIN);
    param.replace(NewLevmar::USE_NUMERICAL_DIFF, false);
    param.replace(NewLevmar::OUTPUT_INTERVAL,  50U);
    param.dump();

    std::cerr<< "check param: " << levmar.checkParam(src, domain, param) << endl;

    levmar.toInnerForm(src, domain, param);
    levmar.fit();
    
    Int4 ct=-1;
    if (levmar.isFitting()) {
        //std::cout << "it is fitting" << endl;
        sleep(1);
        vector<Int4>   ic=levmar.getConvergenceHistoryForInt4(  NewLevmar::ITERATION_COUNT);
        vector<Double> rf=levmar.getConvergenceHistoryForDouble(NewLevmar::R_FACTOR);
        vector<Double> rn=levmar.getConvergenceHistoryForDouble(NewLevmar::RESIDUAL_ERR_NORM);
        vector<Double> gn=levmar.getConvergenceHistoryForDouble(NewLevmar::GRADIENT_NORM);
        vector<Double> pn=levmar.getConvergenceHistoryForDouble(NewLevmar::PARAM_DIFF_NORM);
        for (Int4 i=0; i<ic.size(); ++i) {
            std::cout << ic[i];
            std::cout << " ";
            std::cout << rf[i];
            std::cout << " ";
            std::cout << rn[i];
            std::cout << " ";
            std::cout << gn[i];
            std::cout << " ";
            std::cout << pn[i];
            std::cout << endl;
        }
    }
    //levmar.stopFit();
    while (levmar.isFitting()) {
        sleep(1);
    }

    levmar.eval();

    ElementContainer result;
    levmar.toElementContainer(src, result);
    result.PutHeader().Dump();
    //result.Dump();
    //outputElementContainer(result);
    ParamSet fittedParam = levmar.getFittedParam();
    fittedParam.dump();
    //ElementContainerArray ecArray=levmar.getResultComponents(src);
    //ElementContainer fc=ecArray.Put(1);
    /*
    */
}
