#include "Header.hh"
#include "ElementContainer.hh"
#include "ElementContainerArray.hh"

#include "FuncParser.hh"
#include "FuncComb.hh"

#include "Domain.hh"
#include "ParamSet.hh"

#include "LevmarConsts.hh"
#include "LevmarControl.hh"

#include "AdditionalData.hh"
#include "LevmarFunc.hh"

#include <cmath>

Double sign(const Double s) {
    return (s > 0.0 ? 1.0 : (s < 0.0 ? -1.0 : 0.0));
}

Double uniformNoise(const Double c, const Double p) {
    Double s, v;
    s=rand()/static_cast<Double>(RAND_MAX)-0.5;
    v=rand()/static_cast<Double>(RAND_MAX);
    return c*(1.0+sign(s)*p*sqrt(-1.0*log(v)));
}

ElementContainer *initElementContainer(string &expr, vector<Double> &param, vector<Double> &bin, Double noiseCenter, Double noiseWidth) {

    FuncParser *parser=new FuncParser(expr);
    FuncComb *func=new FuncComb(parser->parse(), param);

    vector<Double> *y=new vector<Double>(bin.size()-1);
    vector<Double> *e=new vector<Double>(bin.size()-1);
    Double x, p, q;
    for (UInt4 i=0; i<bin.size()-1; ++i) {
        x=(bin.at(i)+bin.at(i+1))/2.0;
        p=func->eval(x);
        q=uniformNoise(noiseCenter, noiseWidth);
        y->at(i)=p+q;
        e->at(i)=noiseCenter*noiseWidth;
    }

    ElementContainer *ec = new ElementContainer();
    ec->AddToHeader("run number", 1);
    ec->AddToHeader("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;
   
}

ElementContainerArray *initElementContainerArray(vector<string> &expr, vector< vector<Double> > &param, Double xmin, Double xmax, UInt4 div) {

    vector<Double> *bin=new vector<Double>(div+1);
    Double delta=(xmax-xmin)/div;
    for (UInt4 i=0; i<bin->capacity(); ++i) {
        bin->at(i)=xmin+delta*i;
    }
    //for (UInt4 i=0; i<bin->size(); i+=100) {
    //    std::cout << "i=" << i << ", " << bin->at(i) << endl;
    //}

    ElementContainerArray *eca = new ElementContainerArray();
    for (UInt4 i=0; i<expr.size(); ++i) {
        eca->AddPointer( initElementContainer(expr.at(i), param.at(i), *bin, 10000.0, 0.4) );
    }
    return eca;
}

ParamSet *initParamSet() {

    ParamSet *p=new ParamSet();
    p->add(LevmarConsts::CONSTRAIN,          LevmarConsts::NO_CONSTRAIN);
    p->add(LevmarConsts::USE_NUMERICAL_DIFF, true);
    p->add(LevmarConsts::DIFF_METHOD,        LevmarConsts::FORWARD);
    p->add(LevmarConsts::USE_DATA_WEIGHTS,   false);
    p->add(LevmarConsts::MAX_ITERATIONS,     1000u);
    p->add(LevmarConsts::OUTPUT_INTERVAL,    50u);
    p->add(LevmarConsts::HISTORY_CAPACITY,   10u);

    vector<string> *exprs = new vector<string>();
    exprs->push_back("g g g");
    exprs->push_back("l l l");
    FuncParser *parser;
    vector< vector<FuncBase*> > *funcLists = new vector< vector<FuncBase*> >();
    for (UInt4 i=0; i<exprs->size(); ++i) {
        parser = new FuncParser(exprs->at(i));
        funcLists->push_back(parser->parse());
    }
    p->add(LevmarConsts::FUNCTIONS, *funcLists);

    /*
    vector<Double> *w=new vector<Double>(exprs->size());
    for (UInt4 i=0; i<w->capacity(); ++i) {
        w->at(i)=1.0/w->capacity();
    } 
    p->add(LevmarConsts::MULTI_DATA_WEIGHTS, *w);
    */

    return p;
}

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

    vector<string> *expr = new vector<string>();
    expr->push_back(string("g g g"));
    expr->push_back(string("l l l"));

    vector< vector<Double> > *paramArray = new vector< vector<Double> >(2, *(new vector<Double>(9)));
    paramArray->at(0).at(0)= 30000.0;
    paramArray->at(0).at(1)=     2.8;
    paramArray->at(0).at(2)=     1.0;
    paramArray->at(0).at(3)=100000.0;
    paramArray->at(0).at(4)=     5.0;
    paramArray->at(0).at(5)=     1.5;
    paramArray->at(0).at(6)=100000.0;
    paramArray->at(0).at(7)=    10.0;
    paramArray->at(0).at(8)=     1.5;

    paramArray->at(1).at(0)= 30000.0;
    paramArray->at(1).at(1)=     2.8;
    paramArray->at(1).at(2)=     1.0;
    paramArray->at(1).at(3)=100000.0;
    paramArray->at(1).at(4)=     5.0;
    paramArray->at(1).at(5)=     1.5;
    paramArray->at(1).at(6)=100000.0;
    paramArray->at(1).at(7)=    10.0;
    paramArray->at(1).at(8)=     1.5;

    Double xmin= 0.0;
    Double xmax=15.0;
    UInt4  div =150;

    ElementContainerArray *eca=initElementContainerArray(*expr, *paramArray, xmin, xmax, div);

    vector<Domain> *domainArray = new vector<Domain>();
    for (UInt4 i=0; i<eca->PutSize(); ++i) {
        //eca->PutPointer(i)->Dump();
        ElementContainer ec=eca->Put(i);
        domainArray->push_back(*(new Domain(ec, 1.0, 14.0)));
    }

    ParamSet paramSet = *(initParamSet());
    //paramSet.dump();
    LevmarControl *control=new LevmarControl();
    control->toInnerForm(paramSet);

    AdditionalData *additionalData=new AdditionalData(*control, *eca, *domainArray, paramSet);
    additionalData->output(8);

    
    Double p[]={10000.0,  3.0, 1.0,
                50000.0,  7.5, 1.0,
                10000.0, 12.0, 1.0};

    UInt4 n=0;
    for (UInt4 i=0; i<domainArray->size(); ++i) {
        n += domainArray->at(i).getNumberOfBin();
    }
    std::cerr << "n=" << n << endl;
    Double *y=new Double[n];
    Double *jac=new Double[n*9];
    std::cerr << "Debug: success to allocate y and jac" << endl;

    void *adata=static_cast<void*>(additionalData);
    evalFunc(p, y, 9, n, adata);
    std::cout << additionalData->getNumberOfSeries();
    //additionalData->output(8);
    //evalFunc(p, y, 9, n, static_cast<void*>(additionalData));
    //std::cout << additionalData->getNumberOfSeries();
    //additionalData->output(8);
    evalJaccobian(p, jac, 9, n, adata);

    UInt4 offset=0;
    for (UInt4 i=0; i<domainArray->size(); ++i) {
        vector<Double> *x=domainArray->at(i).createXC();
        for (UInt4 j=0; j<domainArray->at(i).getNumberOfBin(); ++j) {
            std::cout << setw(5) << j;
            std::cout << " ";
            std::cout << x->at(j);
            std::cout << " ";
            std::cout << y[offset+j];
            for (UInt4 k=0; k<9; ++k) {
                std::cout << " ";
                std::cout << jac[(offset + j)*9 +k];
            }
            std::cout << endl;
        }
        std::cout << endl;
        offset += domainArray->at(i).getNumberOfBin();
    }
    std::cout << "e" << endl;

    return 0;
}
