#include "AdvNewLevmar.hh"
#include <unistd.h>
#include <sstream>

const string AdvNewLevmar::className=string("AdvNewLevmar");


/**
 *  default constructor
 */
AdvNewLevmar::AdvNewLevmar() {
    methodName=string("Levenberg-Marquardt");
}


/**
 *  destructor
 */
AdvNewLevmar::~AdvNewLevmar() {
    //delete im;
    //delete stat;
    //delete history;
}

/**
 *  set default values of parameters for the method
 */
AdvParamSet AdvNewLevmar::setDefaultParam(ElementContainer &src) {
    string memberName = string("setDefaultParam(ElementContainer &)");
    DebugMessage(className, memberName, "enter\n");

    AdvParamSet *param = new AdvParamSet();

    /* constrain type */
    param->add(CONSTRAIN,          DEFAULT_CONSTRAIN);

    /* numerical differentiation */
    param->add(USE_NUMERICAL_DIFF, DEFAULT_USE_NUMERICAL_DIFF);
    param->add(DIFF_METHOD,        DEFAULT_DIFF_METHOD);
    param->add(DIFF_DELTA,         DEFAULT_DIFF_DELTA);
    param->add(USE_DATA_WEIGHTS,   DEFAULT_USE_DATA_WEIGHTS);

    /* function base */
    //param->add(FUNCTIONS, *(new vector<FuncBase*>()));

    /* paramter and reference values */
    //param->add(PARAMETER_VALUES, *(new vector<Double>()));
    //param->add(REFERENCE_VALUES, *(new vector<Double>()));

    /* constrain */
    //param->add(LOWER_BOUNDS, *(new vector<Double>()));
    //param->add(UPPER_BOUNDS, *(new vector<Double>()));

    /* maximum of iterations */
    param->add(MAX_ITERATIONS,     DEFAULT_MAX_ITERATIONS);
    /* output interval */
    param->add(OUTPUT_INTERVAL,    DEFAULT_OUTPUT_INTERVAL);

    /* the capacity of convergence history buffer */
    param->add(HISTORY_CAPACITY,   DEFAULT_HISTORY_CAPACITY);

    /* scaling factor */
    param->add(SCALING_FACTOR,     DEFAULT_SCALING_FACTOR);

    /* stopping thresholds, deprecated */
    param->add(TOLERANCE,          DEFAULT_TOLERANCE);
    param->add(RELATIVE_TOLERANCE, DEFAULT_RELATIVE_TOLERANCE);
    param->add(GRADIENT_TOLERANCE, DEFAULT_GRADIENT_TOLERANCE);

    /* stopping thresholds */
    param->add(GRADIENT_THRESH,   DEFAULT_GRADIENT_THRESH);
    param->add(PARAM_DIFF_THRESH, DEFAULT_PARAM_DIFF_THRESH);
    param->add(RESIDU_ERR_THRESH, DEFAULT_RESIDU_ERR_THRESH);

    //PeakSearch *peakSearch = new PeakSearch(src, new MovingAverage());

    DebugMessage(className, memberName, "exit\n");
    return *param;
}

Bool AdvNewLevmar::checkParam(ElementContainer &src, AdvDomain &AdvDomain, AdvParamSet &param) {
    string memberName = string("checkParam(ElementContainer &, AdvDomain &, AdvParamSet &)");

    DebugMessage(className, memberName, "enter\n");
    Bool retval=true;
    retval = retval && this->im->checkParam(src, AdvDomain, param);

    DebugMessage(className, memberName, "exit\n");
    return retval;
}


void AdvNewLevmar::toInnerForm (ElementContainer &src, AdvDomain &AdvDomain, AdvParamSet &param) {
    string memberName=string("toInnerForm(ElementContainer &, AdvDomain &, AdvParamSet &)");
    DebugMessage(className, memberName, "enter\n");

    this->resultBin=*(AdvDomain.getBin());
    this->resultX  =*(AdvDomain.createXC());

    this->im=new AdvLevmarImmutables();
    this->im->toInnerForm(src, AdvDomain, param);

    this->command = new AdvFitCommand();
    this->stat    = new AdvConvergenceStat(this->im->control.historyCapacity);
    this->history = new AdvConvergenceStat(this->im->control.historyCapacity);

    //Int4 n=this->im->nParam;
    //stat->levmarStat = AdvNewLevmar::CONTINUE;
    //stat->param      = this->getFittingParam(param);;
    //stat->errors     = *(new vector<Double>(n, 0.0));
    //stat->covar      = *(new vector< vector<Double> >(n, *(new vector<Double>(n, 0.0))));

    //this->fittingInitial(this->im);
    //this->fittingResult(this->im, stat);

    DebugMessage(className, memberName, "exit\n");
}

/**
 *  start fitting
 */
void AdvNewLevmar::fit() {
    string memberName=string("fit()");
    DebugMessage(className, memberName, "enter\n");

    AdvLevmarFit *obj = new AdvLevmarFit(this->im, this->stat, this->command);
    AdvReportConvergenceProcess *report = new AdvReportConvergenceProcess(this->im, this->stat, this->history);

    Int4 retval=obj->Start();
    DebugMessage(className, memberName, "LevamrFit Start: %d\n", retval);
    obj->Detach();
    DebugMessage(className, memberName, "LevamrFit Detached\n");

    
    //report->Run();
    report->Start();
    DebugMessage(className, memberName, "ReportConvergenceProcess Start: %d\n", retval);
    report->Detach();
    DebugMessage(className, memberName, "ReportConvergenceProcess Detached\n");

    /*
    AdvParamSet *p;
    p = this->history->referLatestStat();
    while (static_cast<AdvLevmarConsts::LevmarStat>(p->getInt4(AdvLevmarConsts::TERMINATION_STAT)) == AdvLevmarConsts::CONTINUE) {
        DebugMessage(className, memberName, "fit sleep\n");
        sleep(1);
        p = this->history->referLatestStat();
    }
    */

    DebugMessage(className, memberName, "exit\n");
}

/**
 *  \return true if fitting is running.
 */
Bool AdvNewLevmar::isFitting() {
    Bool retval=false;
    AdvParamSet *pp;
    if ( this->history !=NULL ) {
        pp=this->history->referLatestStat();
        retval = static_cast<AdvLevmarConsts::LevmarStat>(pp->getInt4(AdvLevmarConsts::TERMINATION_STAT)) == AdvLevmarConsts::CONTINUE;
    }
    return retval;
}

/**
 *  command to stop fitting
 */
void AdvNewLevmar::stopFit() {
    if (this->command != NULL) {
        this->command->setStop();
    }
}


void AdvNewLevmar::eval() {
    string memberName = string("eval()");
    DebugMessage(className, memberName, "enter\n");

    //AdvParamSet *AdvParamSet=this->stat->referLatestStat();
    AdvParamSet *AdvParamSet=this->history->referLatestStat();

    for (UInt4 i=0; i<AdvParamSet->getVectorSize(AdvNewLevmar::PARAMETER_VALUES); ++i) {
        DebugMessage(className, memberName, "%u %e %e\n", i, AdvParamSet->getDouble(AdvNewLevmar::PARAMETER_VALUES, i), AdvParamSet->getDouble(AdvNewLevmar::PARAM_ERRORS, i));
    }

    if ( AdvParamSet != NULL ) {
        AdvFuncComb f=*(new AdvFuncComb(this->im->args.funcList.at(0), AdvParamSet->getVector(AdvNewLevmar::PARAMETER_VALUES), AdvParamSet->getVector(AdvNewLevmar::PARAM_ERRORS)));
        this->resultY = f.eval(this->resultX);
        this->resultE = f.evalError(this->resultX);
        DebugMessage(className, memberName, "size of Bin: %u\n", this->resultBin.size());
        DebugMessage(className, memberName, "size of X  : %u\n", this->resultX.size()  );
        DebugMessage(className, memberName, "size of Y  : %u\n", this->resultY.size()  );
        DebugMessage(className, memberName, "size of E  : %u\n", this->resultE.size()  );
    }
    DebugMessage(className, memberName, "exit\n");
}


vector< vector<Double> > AdvNewLevmar::getTrend() {
    //AdvParamSet *AdvParamSet=this->stat->referLatestStat();
    AdvParamSet *AdvParamSet=this->history->referLatestStat();

    vector< vector<Double> > *retval= new vector< vector<Double> >();
    if ( AdvParamSet != NULL ) {
        AdvFuncComb f=*(new AdvFuncComb(this->im->args.funcList.at(0), AdvParamSet->getVector(AdvNewLevmar::PARAMETER_VALUES), AdvParamSet->getVector(AdvNewLevmar::PARAM_ERRORS)));
        retval->push_back(f.eval(this->resultX));
        retval->push_back(f.der1st(this->resultX));
        retval->push_back(f.der2nd(this->resultX));
    }
    return *retval;
}

void AdvNewLevmar::addExpressionsToHeader(ElementContainer &dest, const string &keyBase, vector< vector<Double> > *expressions) const {
    Int4 ct=0;
    for (std::vector< vector<Double> >::const_iterator expr=expressions->begin(); expr !=expressions->end(); ++expr) {
        ostringstream ss;
        ss << keyBase << " " << ct;
        string key=ss.str();
        dest.AddToHeader(key, *expr);
        ++ct;
    }
}

void AdvNewLevmar::toElementContainer(ElementContainer &src, ElementContainer &dest) const {
    string xKey=src.PutXKey();
    string yKey=src.PutYKey();
    string eKey=src.PutEKey();

    dest.Add(xKey, this->resultBin, src.PutUnit(xKey));
    dest.Add(yKey, this->resultY,   src.PutUnit(yKey));
    dest.Add(eKey, this->resultE,   src.PutUnit(eKey));
    dest.SetKeys(xKey, yKey, eKey);

    //AdvParamSet *p=this->stat->referLatestStat();
    AdvParamSet *p=this->history->referLatestStat();
    // method
    dest.AddToHeader("method",                         this->getMethodName());
    // control
    dest.AddToHeader(AdvLevmarConsts::CONSTRAIN,          AdvLevmarConsts::CONSTRAIN_STR[this->im->control.constrain]);
    dest.AddToHeader(AdvLevmarConsts::USE_DATA_WEIGHTS,   this->im->control.useDataWeights);
    dest.AddToHeader(AdvLevmarConsts::USE_NUMERICAL_DIFF, this->im->control.useNumericalDiff);
    if (this->im->control.useNumericalDiff) {
        dest.AddToHeader(AdvLevmarConsts::DIFF_METHOD,    this->im->control.diffMethod);
    }

    // argments
    dest.AddToHeader(AdvLevmarConsts::FUNCTIONS,            *(this->im->args.getFunctionStr()));
    dest.AddToHeader(AdvLevmarConsts::INITIAL_PARAM_VALUES, *(this->im->args.getInitialParam()));
    switch (this->im->control.constrain) {
        case AdvLevmarConsts::BOX:
#ifdef HAVE_LAPACK
        case AdvLevmarConsts::BLEC:
        case AdvLevmarConsts::BLIC:
        case AdvLevmarConsts::BLEIC:
#endif // HAVE_LAPACK
            if (this->im->args.lb != NULL) {
                dest.AddToHeader(AdvLevmarConsts::LOWER_BOUNDS, *(this->im->args.getLowerBounds()));
            }
            if (this->im->args.ub != NULL) {
                dest.AddToHeader(AdvLevmarConsts::UPPER_BOUNDS, *(this->im->args.getUpperBounds()));
            }
#ifdef HAVE_LAPACK
            if (this->im->control.constrain == AdvLevmarConsts::BLEC) {
                if (this->im->args.wghts != NULL) {
                    dest.AddToHeader(AdvLevmarConsts::BOX_WEIGHTS, *(this->im->args.getBoxWeights()));
                }
            }
#endif // HAVE_LAPACK
            break;
        default:
            break;
    }
#ifdef HAVE_LAPACK
    switch (this->im->control.constrain) {
        case AdvLevmarConsts::LEC:
        case AdvLevmarConsts::BLEC:
        case AdvLevmarConsts::LEIC:
        case AdvLevmarConsts::BLEIC:
            this->addExpressionsToHeader(dest, AdvLevmarConsts::EQUATIONS, this->im->args.getEqMatrix());
            break;
        default:
            break;
    }
    switch (this->im->control.constrain) {
        case AdvLevmarConsts::LIC:
        case AdvLevmarConsts::BLIC:
        case AdvLevmarConsts::LEIC:
        case AdvLevmarConsts::BLEIC:
            this->addExpressionsToHeader(dest, AdvLevmarConsts::INEQUALITIES, this->im->args.getIneqMatrix());
            break;
        default:
            break;
    }
#endif // HAVE_LAPACK
/*
*/
    dest.AddToHeader(AdvLevmarConsts::SCALING_FACTOR,    this->im->args.opts[0]);
    dest.AddToHeader(AdvLevmarConsts::RESIDU_ERR_THRESH, this->im->args.opts[1]);
    dest.AddToHeader(AdvLevmarConsts::GRADIENT_THRESH,   this->im->args.opts[2]);
    dest.AddToHeader(AdvLevmarConsts::PARAM_DIFF_THRESH, this->im->args.opts[3]);
    /*
    */
    /*
    */
    if (this->im->control.useNumericalDiff) {
        dest.AddToHeader(AdvLevmarConsts::DIFF_DELTA,     this->im->args.opts[4]);
    }
    /*
    */

    vector<Double> *v=this->im->args.additionalData->getDomainBounds(0);
    //vector<Double> *v=new vector<Double>();
    //v->push_back(*(this->im->args.additionalData->x->begin()));
    //v->push_back(*(this->im->args.additionalData->x->end()-1));
    dest.AddToHeader("AdvDomain", *v);
    /*
    */

    // results
    dest.AddToHeader(AdvLevmarConsts::ITERATION_COUNT,    p->getInt4(AdvLevmarConsts::ITERATION_COUNT));
    dest.AddToHeader(AdvLevmarConsts::TERMINATION_STAT,   AdvLevmarConsts::TERMINATION_REASON[p->getInt4(AdvLevmarConsts::TERMINATION_STAT)]);
    dest.AddToHeader(AdvLevmarConsts::PARAMETER_VALUES,   p->getVector(AdvLevmarConsts::PARAMETER_VALUES));
    dest.AddToHeader(AdvLevmarConsts::PARAM_ERRORS,       p->getVector(AdvLevmarConsts::PARAM_ERRORS));
    /*
    */
    dest.AddToHeader(AdvLevmarConsts::R_FACTOR,           p->getDouble(AdvLevmarConsts::R_FACTOR));
    dest.AddToHeader(AdvLevmarConsts::RESIDUAL_ERR_NORM,  p->getDouble(AdvLevmarConsts::RESIDUAL_ERR_NORM));
    dest.AddToHeader(AdvLevmarConsts::GRADIENT_NORM,      p->getDouble(AdvLevmarConsts::GRADIENT_NORM));
    dest.AddToHeader(AdvLevmarConsts::PARAM_DIFF_NORM,    p->getDouble(AdvLevmarConsts::PARAM_DIFF_NORM));
    /*
    */
}


AdvParamSet AdvNewLevmar::getFittedParam() const {
    AdvParamSet *fittedParam = new AdvParamSet();
    //AdvParamSet *latestStat  = this->stat->referLatestStat();
    AdvParamSet *latestStat  = this->history->referLatestStat();
    vector<Double> v;

    fittedParam->add(AdvNewLevmar::CONSTRAIN,          this->im->control.constrain);
    fittedParam->add(AdvNewLevmar::USE_NUMERICAL_DIFF, this->im->control.useNumericalDiff);
    fittedParam->add(AdvNewLevmar::DIFF_METHOD,        this->im->control.diffMethod);
    fittedParam->add(AdvNewLevmar::USE_DATA_WEIGHTS,   this->im->control.useDataWeights);

    fittedParam->add(AdvNewLevmar::MAX_ITERATIONS,     this->im->control.maxIterations );
    fittedParam->add(AdvNewLevmar::OUTPUT_INTERVAL,    this->im->control.outputInterval);

    fittedParam->add(AdvNewLevmar::FUNCTIONS, this->im->args.funcList.at(0));

    fittedParam->add(AdvNewLevmar::INITIAL_PARAM_VALUES, *(this->im->args.getInitialParam()));
    v= latestStat->getVector(AdvNewLevmar::PARAMETER_VALUES);
    fittedParam->add(AdvNewLevmar::PARAMETER_VALUES, v);

    switch ( this->im->control.constrain) {
        case AdvNewLevmar::BOX:
#ifdef HAVE_LAPACK
        case AdvNewLevmar::BLEC:
        case AdvNewLevmar::BLIC:
        case AdvNewLevmar::BLEIC:
#endif
            if (this->im->args.lb != NULL) {
                fittedParam->add(AdvNewLevmar::LOWER_BOUNDS, *(this->im->args.getLowerBounds()));
            }
            if (this->im->args.ub != NULL) {
                fittedParam->add(AdvNewLevmar::UPPER_BOUNDS, *(this->im->args.getUpperBounds()));
            }
#ifdef HAVE_LAPACK
            if (this->im->control.constrain == AdvLevmarConsts::BLEC) {
                if (this->im->args.wghts != NULL) {
                    fittedParam->add(AdvLevmarConsts::BOX_WEIGHTS, *(this->im->args.getBoxWeights()));
                }
            }
#endif
            break;
        default:
            break;
    }
#ifdef HAVE_LAPACK
    switch ( this->im->control.constrain) {
        case AdvNewLevmar::LEC:
        case AdvNewLevmar::BLEC:
        case AdvNewLevmar::LEIC:
        case AdvNewLevmar::BLEIC:
            fittedParam->add(AdvNewLevmar::EQUATIONS, *(this->im->args.getEqMatrix()));
            break;
        default:
            break;
    }
    switch ( this->im->control.constrain) {
        case AdvNewLevmar::LIC:
        case AdvNewLevmar::BLIC:
        case AdvNewLevmar::LEIC:
        case AdvNewLevmar::BLEIC:
            fittedParam->add(AdvNewLevmar::INEQUALITIES, *(this->im->args.getEqMatrix()));
            break;
        default:
            break;
    }
#endif

    fittedParam->add(AdvNewLevmar::GRADIENT_TOLERANCE, this->im->args.opts[1]);
    fittedParam->add(AdvNewLevmar::TOLERANCE,          this->im->args.opts[2]);
    fittedParam->add(AdvNewLevmar::RELATIVE_TOLERANCE, this->im->args.opts[3]);

    fittedParam->add(AdvNewLevmar::GRADIENT_THRESH,    this->im->args.opts[1]);
    fittedParam->add(AdvNewLevmar::PARAM_DIFF_THRESH,  this->im->args.opts[2]);
    fittedParam->add(AdvNewLevmar::RESIDU_ERR_THRESH,  this->im->args.opts[3]);
    if (this->im->control.useNumericalDiff) {
        fittedParam->add(AdvNewLevmar::DIFF_DELTA, this->im->args.opts[4]);
    }

    v=latestStat->getVector(AdvNewLevmar::PARAM_ERRORS);
    fittedParam->add(AdvNewLevmar::PARAM_ERRORS, v);
    fittedParam->add(AdvNewLevmar::COVARIANCE_MATRIX, latestStat->getMatrix(AdvNewLevmar::COVARIANCE_MATRIX));

    fittedParam->add(AdvNewLevmar::FUNCTION_EVALUATIONS,  latestStat->getInt4(AdvNewLevmar::FUNCTION_EVALUATIONS));
    fittedParam->add(AdvNewLevmar::JACOBIAN_EVALUATIONS,  latestStat->getInt4(AdvNewLevmar::JACOBIAN_EVALUATIONS));
    fittedParam->add(AdvNewLevmar::LINEAR_SYSTEMS_SOLVED, latestStat->getInt4(AdvNewLevmar::LINEAR_SYSTEMS_SOLVED));
    
    return *fittedParam;
}


Double AdvNewLevmar::likehood() {
    return 0.0;
}

AdvParamSet *AdvNewLevmar::getLatestConvergenceStat() const {
    return this->history->referLatestStat();
}


/**
 *
 */
deque<AdvParamSet*> AdvNewLevmar::getConvergenceHistory() const {
    deque<AdvParamSet*> *retval=new deque<AdvParamSet*>();
    for (UInt4 i=0; i<this->history->size(); ++i) {
        retval->push_back(this->history->at(i));
    }
    return *retval;
}

/**
 *  get the size (depth) of convergence history
 */
UInt4 AdvNewLevmar::getHistorySize() const {
    return this->history->size();
}

/**
 *  get a histrical list of Int4 type value
 */
vector<Int4> AdvNewLevmar::getConvergenceHistoryForInt4(const string &key) const {
    vector<Int4> *retval=new vector<Int4>();
    for (UInt4 i=0; i<this->history->size(); ++i) {
        retval->push_back(this->history->at(i)->getInt4(key));
    }
    return *retval;
}

/**
 *  get a histrical list of Double type value
 */
vector<Double> AdvNewLevmar::getConvergenceHistoryForDouble(const string &key) const {
    vector<Double> *retval=new vector<Double>();
    for (UInt4 i=0; i<this->history->size(); ++i) {
        retval->push_back(this->history->at(i)->getDouble(key));
    }
    return *retval;
}
