#include "AdvLevmarFit.hh"

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

/**
 *  constructor
 */
AdvLevmarFit::AdvLevmarFit(AdvLevmarImmutables *im, AdvConvergenceStat *stat, AdvFitCommand *command) {
    string memberName = string("AdvLevmarFit");
    DebugMessage(className, memberName, "enter\n");

    this->setLevmarImmutables(im);
    this->setConvergenceStat(stat);
    this->setFitCommand(command);

    this->initialise(this->im);

    this->remainder=this->im->control.maxIterations % this->im->control.outputInterval;
    this->iterationCounts=0;

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

/**
 *  destructor
 */
AdvLevmarFit::~AdvLevmarFit() {

    delete[] this->info;  this->info=NULL;
    delete[] this->param; this->param=NULL;
    delete[] this->covar; this->covar=NULL;
    delete[] this->work;  this->work=NULL;
    delete[] this->linkids;  this->linkids=NULL;
}


/**
 *  set immutables for Levmar to the given value
 *
 *  \param[in] im immutabless for levmar
 */
void AdvLevmarFit::setLevmarImmutables(AdvLevmarImmutables *im) {
    this->im=im;
}

/**
 *  set convargence stat queue (buffer) to the given value
 *
 *  \param[in] satat convergence stat queue
 */
void AdvLevmarFit::setConvergenceStat(AdvConvergenceStat *stat) {
    this->stat=stat;
}

/**
 *  set command queue for fitting
 *
 *  \param[in] command a command buffer for fitting
 */
void AdvLevmarFit::setFitCommand(AdvFitCommand *command) {
    this->command=command;
}

/**
 *  allocate infomation buffer for a Levmar rotuine
 */
void AdvLevmarFit::allocInfo() {
    this->info = new Double[LM_INFO_SZ];
    for (UInt4 i=0; i<LM_INFO_SZ; ++i) {
        this->info[i]=0.0;
    }
}

/**
 *  allocate a read/write buffer for fitting parameter
 *
 *  \param[in] im the immutables for Levmar
 */
void AdvLevmarFit::allocFittingParam(AdvLevmarImmutables *im) {
    this->param = new Double[im->args.nParam];
    for (Int4 i=0; i<im->args.nParam; ++i) {
        this->param[i]=im->args.param[i];
    }
}

/**
 *  allocate covariance matrix for fitting parameters
 *
 * \param[in] im the immutables for Levmar
 */
void AdvLevmarFit::allocCovar(AdvLevmarImmutables *im) {
    Int4 n;
    n = im->args.nParam;
    n = n*n;
    this->covar = new Double[n];
    for (Int4 i=0; i<n; ++i) {
        this->covar[i]=0.0;
    }
}


/**
 *  allocate a read/write buffer for linkid
 *
 *  \param[in] im the immutables for Levmar
 */
void AdvLevmarFit::allocLinkIds(AdvLevmarImmutables *im) {
    this->linkids = new Double[im->args.nParam];
    for (Int4 i=0; i<im->args.nParam; ++i) {
        this->linkids[i]=im->args.linkids[i];
    }
}


/**
 *  allocate work area for Levmar
 *
 *  \param[in] im the immutables for Levmar
 */
void AdvLevmarFit::allocWorkArea(AdvLevmarImmutables *im) {
    switch (im->control.constrain) {
        case AdvLevmarConsts::NO_CONSTRAIN:
            this->work = im->control.useNumericalDiff ? new Double[LM_DIF_WORKSZ(im->args.nParam, im->args.nRef)] :
                                                        new Double[LM_DER_WORKSZ(im->args.nParam, im->args.nRef)];
            break;
        case AdvLevmarConsts::BOX:
            this->work = im->control.useNumericalDiff ? new Double[LM_BC_DIF_WORKSZ(im->args.nParam, im->args.nRef)] :
                                                        new Double[LM_BC_DER_WORKSZ(im->args.nParam, im->args.nRef)];
            break;
#ifdef HAVE_LAPACK
        case AdvLevmarConsts::LEC:
            this->work = im->control.useNumericalDiff ? new Double[LM_LEC_DIF_WORKSZ(im->args.nParam, im->args.nRef, im->args.nEq)] :
                                                        new Double[LM_LEC_DER_WORKSZ(im->args.nParam, im->args.nRef, im->args.nEq)];
            break;
        case AdvLevmarConsts::LIC:
            this->work = im->control.useNumericalDiff ? new Double[LM_BLEIC_DIF_WORKSZ(im->args.nParam, im->args.nRef, 0, im->args.nIneq)] :
                                                        new Double[LM_BLEIC_DER_WORKSZ(im->args.nParam, im->args.nRef, 0, im->args.nIneq)];
            break;
        case AdvLevmarConsts::BLEC:
            this->work = im->control.useNumericalDiff ? new Double[LM_BLEC_DIF_WORKSZ(im->args.nParam, im->args.nRef, im->args.nEq)] :
                                                        new Double[LM_BLEC_DER_WORKSZ(im->args.nParam, im->args.nRef, im->args.nEq)];
            break;
        case AdvLevmarConsts::BLIC:
            this->work = im->control.useNumericalDiff ? new Double[LM_BLEIC_DIF_WORKSZ(im->args.nParam, im->args.nRef, 0, im->args.nIneq)] :
                                                        new Double[LM_BLEIC_DER_WORKSZ(im->args.nParam, im->args.nRef, 0, im->args.nIneq)];
            break;
        case AdvLevmarConsts::LEIC:
            this->work = im->control.useNumericalDiff ? new Double[LM_BLEIC_DIF_WORKSZ(im->args.nParam, im->args.nRef, im->args.nEq, im->args.nIneq)] :
                                                        new Double[LM_BLEIC_DER_WORKSZ(im->args.nParam, im->args.nRef, im->args.nEq, im->args.nIneq)];
            break;
        case AdvLevmarConsts::BLEIC:
            this->work = im->control.useNumericalDiff ? new Double[LM_BLEIC_DIF_WORKSZ(im->args.nParam, im->args.nRef, im->args.nEq, im->args.nIneq)] :
                                                        new Double[LM_BLEIC_DER_WORKSZ(im->args.nParam, im->args.nRef, im->args.nEq, im->args.nIneq)];
            break;
#endif /* HAVE_LAPACK */

    }
}

/**
 *  allocate woring areas for Levmar fitting
 *
 *  \param[in] im the immutables for Levmar
 */
void AdvLevmarFit::initialise(AdvLevmarImmutables *im) {
    this->allocInfo();
    this->allocFittingParam(im);
    this->allocCovar(im);
    this->allocWorkArea(im);
    this->allocLinkIds(im);
}//    args.additionalData->setLinkList(linkids);



/**
 *  do Levmar fitting without constrain.
 *
 *  \param[in] useNumericalDiff the flag for using Numerical differential
 *  \param[in] args             arguments for Levmar
 *  \param[in] iterations       maximum of iterations for small fitting loop
 *  \retval iterations when small fitting loop ends regularlly
 *  \retval LM_ERROR   when a error occurs (into[6] == 4 or 7)
 */
Int4 AdvLevmarFit::doNoConstrain(const Bool useNumericalDiff, AdvLevmarArgs &args, const Int4 iterations) {
    return useNumericalDiff ? \
        dlevmar_dif(evalFunc, this->param, args.ref, args.nParam, args.nRef,
                    iterations, args.opts, this->info, this->work, this->covar, args.additionalData) : \
        dlevmar_der(evalFunc, evalJaccobian, this->param, args.ref, args.nParam, args.nRef,
                     iterations, args.opts, this->info, this->work, this->covar, args.additionalData);
}


/**
 *  do Levmar fitting with the box constrain.
 *
 *  \param[in] useNumericalDiff the flag for using Numerical differential
 *  \param[in] args             arguments for Levmar
 *  \param[in] iterations       maximum of iterations for small fitting loop
 *  \retval iterations when small fitting loop ends regularlly
 *  \retval LM_ERROR   when a error occurs (into[6] == 4 or 7)
 */
Int4 AdvLevmarFit::doBC(const Bool useNumericalDiff, AdvLevmarArgs &args, const Int4 iterations) {
    return useNumericalDiff ? \
        dlevmar_bc_dif(evalFunc, this->param, args.ref, args.nParam, args.nRef, args.lb, args.ub,
                       iterations, args.opts, this->info, this->work, this->covar, args.additionalData) :
        dlevmar_bc_der(evalFunc, evalJaccobian, this->param, args.ref, args.nParam, args.nRef, args.lb, args.ub,
                       iterations, args.opts, this->info, this->work, this->covar, args.additionalData);
}

#ifdef HAVE_LAPACK
/**
 *  do Levmar fitting with the linear equations constrain.
 *
 *  \param[in] useNumericalDiff the flag for using Numerical differential
 *  \param[in] args             arguments for Levmar
 *  \param[in] iterations       maximum of iterations for small fitting loop
 *  \retval iterations when small fitting loop ends regularlly
 *  \retval LM_ERROR   when a error occurs (into[6] == 4 or 7)
 */
Int4 AdvLevmarFit::doLEC(const Bool useNumericalDiff, AdvLevmarArgs &args, const Int4 iterations) {
    return useNumericalDiff ?
        dlevmar_lec_dif(evalFunc, this->param, args.ref, args.nParam, args.nRef, args.A, args.b, args.nEq,
                         iterations, args.opts, this->info, this->work, this->covar, args.additionalData) :
        dlevmar_lec_der(evalFunc, evalJaccobian, this->param, args.ref, args.nParam, args.nRef, args.A, args.b, args.nEq,
                         iterations, args.opts, this->info, this->work, this->covar, args.additionalData);

}

/**
 *  do Levmar fitting with the linear inequalities constrain.
 *
 *  \param[in] useNumericalDiff the flag for using Numerical differential
 *  \param[in] args             arguments for Levmar
 *  \param[in] iterations       maximum of iterations for small fitting loop
 *  \retval iterations when small fitting loop ends regularlly
 *  \retval LM_ERROR   when a error occurs (into[6] == 4 or 7)
 */
Int4 AdvLevmarFit::doLIC(const Bool useNumericalDiff, AdvLevmarArgs &args, const Int4 iterations) {
    return useNumericalDiff ?
        dlevmar_lic_dif(evalFunc, this->param, args.ref, args.nParam, args.nRef, args.C, args.d, args.nIneq,
                         iterations, args.opts, this->info, this->work, this->covar, args.additionalData) :
        dlevmar_lic_der(evalFunc, evalJaccobian, this->param, args.ref, args.nParam, args.nRef, args.C, args.d, args.nIneq,
                         iterations, args.opts, this->info, this->work, this->covar, args.additionalData);
}

/**
 *  do Levmar fitting with the boxies and linear equations constrain.
 *
 *  \param[in] useNumericalDiff the flag for using Numerical differential
 *  \param[in] args             arguments for Levmar
 *  \param[in] iterations       maximum of iterations for small fitting loop
 *  \retval iterations when small fitting loop ends regularlly
 *  \retval LM_ERROR   when a error occurs (into[6] == 4 or 7)
 */
Int4 AdvLevmarFit::doBLEC(const Bool useNumericalDiff, AdvLevmarArgs &args, const Int4 iterations) {
    return useNumericalDiff ?
        dlevmar_blec_dif(evalFunc, this->param, args.ref, args.nParam, args.nRef,
                           args.lb, args.ub, args.A, args.b, args.nEq, args.wghts,
                           iterations, args.opts, this->info, this->work, this->covar, args.additionalData) :
        dlevmar_blec_der(evalFunc, evalJaccobian, this->param, args.ref, args.nParam, args.nRef,
                           args.lb, args.ub, args.A, args.b, args.nEq, args.wghts,
                           iterations, args.opts, this->info, this->work, this->covar, args.additionalData) ;

}

/**
 *  do Levmar fitting with the boxies and linear inequalities constrain.
 *
 *  \param[in] useNumericalDiff the flag for using Numerical differential
 *  \param[in] args             arguments for Levmar
 *  \param[in] iterations       maximum of iterations for small fitting loop
 *  \retval iterations when small fitting loop ends regularlly
 *  \retval LM_ERROR   when a error occurs (into[6] == 4 or 7)
 */
Int4 AdvLevmarFit::doBLIC(const Bool useNumericalDiff, AdvLevmarArgs &args, const Int4 iterations) {
    return useNumericalDiff ?
        dlevmar_blic_dif(evalFunc, this->param, args.ref, args.nParam, args.nRef,
                           args.lb, args.ub, args.C, args.d, args.nIneq,
                           iterations, args.opts, this->info, this->work, this->covar, args.additionalData) :
        dlevmar_blic_der(evalFunc, evalJaccobian, this->param, args.ref, args.nParam, args.nRef,
                           args.lb, args.ub, args.C, args.d, args.nIneq,
                           iterations, args.opts, this->info, this->work, this->covar, args.additionalData);
}

/**
 *  do Levmar fitting with the linear equations and linear inequalities constrain.
 *
 *  \param[in] useNumericalDiff the flag for using Numerical differential
 *  \param[in] args             arguments for Levmar
 *  \param[in] iterations       maximum of iterations for small fitting loop
 *  \retval iterations when small fitting loop ends regularlly
 *  \retval LM_ERROR   when a error occurs (into[6] == 4 or 7)
 */
Int4 AdvLevmarFit::doLEIC(const Bool useNumericalDiff, AdvLevmarArgs &args, const Int4 iterations) {
    return useNumericalDiff ?
        dlevmar_leic_dif(evalFunc, this->param, args.ref, args.nParam, args.nRef,
                           args.A, args.b, args.nEq, args.C, args.d, args.nIneq,
                           iterations, args.opts, this->info, this->work, this->covar, args.additionalData):
        dlevmar_leic_der(evalFunc, evalJaccobian, this->param, args.ref, args.nParam, args.nRef,
                           args.A, args.b, args.nEq, args.C, args.d, args.nIneq,
                           iterations, args.opts, this->info, this->work, this->covar, args.additionalData);
}

/**
 *  do Levmar fitting with the boxies, linear equations and linear inequalities constrain.
 *
 *  \param[in] useNumericalDiff the flag for using Numerical differential
 *  \param[in] args             arguments for Levmar
 *  \param[in] iterations       maximum of iterations for small fitting loop
 *  \retval iterations when small fitting loop ends regularlly
 *  \retval LM_ERROR   when a error occurs (into[6] == 4 or 7)
 */
Int4 AdvLevmarFit::doBLEIC(const Bool useNumericalDiff, AdvLevmarArgs &args, const Int4 iterations) {
    return useNumericalDiff ?
        dlevmar_bleic_dif(evalFunc, this->param, args.ref, args.nParam, args.nRef,
                           args.lb, args.ub, args.A, args.b, args.nEq, args.C, args.d, args.nIneq,
                           iterations, args.opts, this->info, this->work, this->covar, args.additionalData):
        dlevmar_bleic_der(evalFunc, evalJaccobian, this->param, args.ref, args.nParam, args.nRef,
                           args.lb, args.ub, args.A, args.b, args.nEq, args.C, args.d, args.nIneq,
                           iterations, args.opts, this->info, this->work, this->covar, args.additionalData);
}
#endif //  HAVE_LAPACK

/**
 *  get the R-factor of the  fitted functions
 *
 *  \param[in] control a set of control variables for Levmar
 *  \param[in] args    a set of input variables for Levmar
 *  \param[in] param   the read/write buffer for fitting parametes
 */
Double AdvLevmarFit::getRFactor(AdvLevmarControl &control, AdvLevmarArgs &args, Double *param) {
    string memberName=string("getRFactor(AdvLevmarControl &, AdvLevmarArgs &, Double *)");
    
//    Double denominator, numerator;
//    denominator=0.0;
//    for (Int4 i=0; i<args.nRef; ++i) {
//        denominator += abs(args.ref[i]);
//    }

    /*
//    if ( control.useDataWeights ) {
//        args.additionalData->useWeight=true;
//    }
    */
//    Double *tmpY=new Double[args.nRef];
//    evalFunc(param, tmpY, args.nParam, args.nRef, args.additionalData);
//    numerator=0.0;
//    for (Int4 i=0; i<args.nRef; ++i) {
//        numerator += abs(abs(tmpY[i]) - abs(args.ref[i]));
//    }
//    delete[] tmpY; tmpY=NULL;

//    return numerator/denominator;

    Double *tmpY = new Double[args.nRef];
    UInt4 nSeries=args.additionalData->funcEvalBaseList->size();

    evalFunc(param,tmpY, args.nParam, args.nRef, args.additionalData);

    Double sum=0.0;
    UInt4 offset=0;
    for (UInt4 i=0; i<nSeries; ++i) {
        UInt4 n=args.additionalData->funcEvalBaseList->at(i).x->size();
        vector<Double> *w=args.additionalData->funcEvalBaseList->at(i).w;
        Double denominator=0.0;
        for (UInt4 j=offset; j<offset+n; ++j) {
            denominator += abs(args.ref[j])/w->at(j-offset);
            //denominator += abs(args.ref[j]);
        }
        Double numerator=0.0;
        Double perror=0.0;
        for (UInt4 j=offset; j<offset+n; ++j) {
            numerator += abs(abs(tmpY[j]) - abs(args.ref[j]))/w->at(j-offset);
            //numerator += abs(abs(tmpY[j]) - abs(args.ref[j]));
            perror    += pow(tmpY[j] - args.ref[j], 2.0);
            //DebugMessage(className, memberName, __FILE__, __LINE__, "j=%5d eval=%23.15e ref=%23.15e weight=%23.15e\n", j, tmpY[j], args.ref[j], w->at(j-offset));
        }
        //std::cout<< "i=" << i << " R-factor=" << numerator/denominator << " resdu. err.=" << perror << endl;
        sum += numerator/denominator;
        offset += n;
    }
    sum /= nSeries;
   
    return sum;
}

/**
 *  get norms for resisual error (L2) and gradient (infinity)
 *
 *  \param[in]  evalFunc       the pointer to the fitting function that evaluate its values
 *  \param[in]  evalJaccobian  the pointer to the functions that evaluate the Jacobi matrix for the fitting function
 *  \param[in]  control        a set of control variables for Levmar
 *  \param[in]  args           a set of input variables for Levmar
 *  \param[in]  param          the read/write buffer for fitting parameters
 *  \param[out] errNorm        the L2 norm of residuar error
 *  \param[out] gradNorm       the infinity norm of the gradient
 */
void AdvLevmarFit::getNorms(
    void (*evalFunc)(Double *p, Double *hx, Int4 m, Int4 n, void *),
    void (*evalJaccobian)(Double *p, Double *j, Int4 m, Int4 n, void *),
    const AdvLevmarControl &control, const AdvLevmarArgs &args, Double *param,
    Double *errNorm, Double *gradNorm) {
    string memberName = string("getNorm");
    DebugMessage(className, memberName, "enter\n");

    Double *hx=new Double[args.nRef];              // function values
    Double *jac=new Double[args.nRef*args.nParam]; // jacobian
    Double *e =new Double[args.nRef];              // residual error
    Double *grad=new Double[args.nParam];          // gradient

    Double squareSum, tmp;
    Double grad_inf, d_max;

    /*  the value of the function and its jacobian */
    evalFunc(param, hx, args.nParam, args.nRef, (void *)args.additionalData);
    if ( control.useNumericalDiff ) {
        if ( control.diffMethod == AdvLevmarConsts::FORWARD ) {
            Double *hxx = new Double[args.nRef];
            dlevmar_fdif_forw_jac_approx(evalFunc, param, hx, hxx, abs(args.opts[4]),
                                         jac, args.nParam, args.nRef, (void *)args.additionalData);
            delete[] hxx; hxx=NULL;
        } else { //control.diffMethod == AdvLevmarConsts::CENTRAL ) {
             Double *hxm = new Double[args.nRef]; /* work array for evaluating func(p-delta), nx1 */
             Double *hxp = new Double[args.nRef]; /* work array for evaluating func(p+delta), nx1 */
             dlevmar_fdif_cent_jac_approx(evalFunc, param, hxm, hxp, abs(args.opts[4]),
                                          jac, args.nParam, args.nRef, (void *)args.additionalData);
             delete[] hxm; hxm=NULL;
             delete[] hxp; hxp=NULL;
        }
    } else {
        evalJaccobian(param, jac, args.nParam, args.nRef, (void *)args.additionalData);
    }

    /* residual error and square sum of them */
    squareSum = 0.0;
    for (Int4 i=0; i<args.nRef; ++i) {
        e[i]=tmp=args.ref[i]-hx[i];
        squareSum += tmp*tmp;
    }
    *errNorm=squareSum;
    DebugMessage(className, memberName, "square sum of residual error: %23.15e\n", squareSum);

    /*  g=J^T e */
    for (Int4 i=0; i<args.nParam; ++i) {
        grad[i]=0.0;
    }
    for (Int4 j=0, jm=0; j<args.nRef; ++j, jm += args.nParam) {
        //DebugMessage(className, memberName, "j=%5d r=%23.15e %s", j, e[j], "Jac");
        //for (Int4 i=0; i<args.nParam; ++i) {
        //    fprintf(stderr, " %23.15e", jac[jm+i]);
        //}
        //fprintf(stderr, "\n");
        for (Int4 i=0; i<args.nParam; ++i) {
            grad[i] += e[j]*jac[jm+i] ;
            //DebugMessage(className, memberName, "i=%d j=%d jac=%23.15e e=%23.15e g=%23.15e\n", i, j, jac[jm+i], e[j], grad[i]);
        }
    }
    for (Int4 i=0; i<args.nParam; ++i) {
        DebugMessage(className, memberName, "%d g=%23.15e \n", i, grad[i]);
    }

    grad_inf=0.0;
    d_max=0.0;
    switch (control.constrain) {
        case AdvLevmarConsts::NO_CONSTRAIN:
#ifdef HAVE_LAPACK
        case AdvLevmarConsts::LEC:
        case AdvLevmarConsts::LIC:
        case AdvLevmarConsts::BLEC:
        case AdvLevmarConsts::BLIC:
        case AdvLevmarConsts::LEIC:
        case AdvLevmarConsts::BLEIC:
#endif //  HAVE_LAPACK
            for (Int4 i=0; i<args.nParam; ++i) {
                grad_inf = max(grad_inf, abs(grad[i]));
            }
            break;
        case AdvLevmarConsts::BOX:
            for (Int4 i=0, j=0, numactive=0; i<args.nParam; ++i) {
                if      (args.ub && param[i]==args.ub[i]) { ++numactive; if (grad[i]>0.0) ++j;      }
                else if (args.lb && param[i]==args.lb[i]) { ++numactive; if (grad[i]<0.0) ++j;      }
                else                                      { grad_inf = max(grad_inf, abs(grad[i])); }
                //grad_inf = max(grad_inf, abs(grad[i]));
            }
            break;
    }
    //debugMessage(className, memberName, "inf. norm of gradient: %23.15e\n", grad_inf);
    //debugMessage(className, memberName, "max of diagnal part of J~t J: %23.15e\n", d_max);

    *gradNorm=grad_inf;

    delete[] grad; grad=NULL;
    delete[] hx;   hx  =NULL;
    delete[] jac;  jac =NULL;
    delete[] e;    e   =NULL;

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


/**
 *  get parameter errors from a covariance matrix
 *
 *  \param[in] covar the covariance matrix for the fitting parameters
 *  \param[in] n     the size of the covariance matrix
 */
vector<Double> *AdvLevmarFit::covarToErrorVector(const Double *covar, const Int4 n) {
    string memberName = string("covarToErrorVector(Double*, UInt4)");
    DebugMessage(className, memberName, "enter\n");

    vector<Double> *e = new vector<Double>(n);
    for (Int4 i=0; i<n; ++i) {
        e->at(i)=sqrt(covar[i*(n+1)]);
        DebugMessage(className, memberName, "param error: %d %e\n", i, e->at(i));
    }
    DebugMessage(className, memberName, "exit\n");
    return e;
}


/**
 *  create the current convergence status from the inner data
 *
 *  \param[in] control a set of contril variables for Levmar
 *  \param[in] args    the input variables for Levmar
 *  \param[in] stat    termination stat of Levmar
 *  \param[in] info    infomation buffers from Levmar
 *  \param[in] param   the read/write buffer for the fitting parameters
 *  \param[in] covar   the covariance matrix for the fitting parameters
 */
AdvParamSet *AdvLevmarFit::createStat( AdvLevmarControl &control,  AdvLevmarArgs &args, AdvLevmarConsts::LevmarStat stat,  Double *info,  Double *param,  Double *covar,Double *linkids, Int4 time) {
    string memberName = string("createStat(AdvLevmarArgs&)");
    DebugMessage(className, memberName, "enter\n");

    AdvParamSet *ps = new AdvParamSet();
    /* termination stat */
    ps->add(AdvLevmarConsts::ITERATION_COUNT,       iterationCounts);
    ps->add(AdvLevmarConsts::TERMINATION_STAT,      static_cast<Int4>(stat));
    /* norms */
    DebugMessage(className, memberName, "info[0]=%e\n", info[0]);
    ps->add(AdvLevmarConsts::R_FACTOR,              this->getRFactor(control, args, param));
    ps->add(AdvLevmarConsts::RESIDUAL_ERR_NORM,     info[1]);
    ps->add(AdvLevmarConsts::GRADIENT_NORM,         info[2]);
    ps->add(AdvLevmarConsts::PARAM_DIFF_NORM,       info[3]);
    /* scaling factor */
    ps->add(AdvLevmarConsts::MU_RATIO,              info[4]);
    /* the number times of evaluations */
    ps->add(AdvLevmarConsts::FUNCTION_EVALUATIONS,  static_cast<Int4>(info[7]));
    ps->add(AdvLevmarConsts::JACOBIAN_EVALUATIONS,  static_cast<Int4>(info[8]));
    ps->add(AdvLevmarConsts::LINEAR_SYSTEMS_SOLVED, static_cast<Int4>(info[9]));
    ps->add(AdvLevmarConsts::ITERATION_TIME,        static_cast<Int4>(time));
    DebugMessage(className, memberName, "info[7-9]=%e %e %e\n", info[7], info[8], info[9]);

    /* paremter values */

    Double *newParam;
    newParam = param;
    Double *newCovar;
    newCovar = covar;

//cout <<"size " << sizeof(covar) <<endl;
for(UInt4 i=0;i<args.nParam; i++ )
    {
    newParam[i]=param[int(linkids[i]-1)];
    //cout << "linkid "<<linkids[i] <<","<<param[i]<<","<<covar[i]<< endl;

    for(UInt4 j=0;j<args.nParam; j++ )
            {

            if(i!=j)
                    {
                newCovar[int(i*args.nParam+j)]=covar[int(i*args.nParam+j)];
                    }
            else
                    {
                newCovar[int(i*args.nParam+j)]=covar[int(linkids[i]-1)*int(args.nParam)+int(linkids[i]-1)];
                    }

            }
    }

    ps->add(AdvLevmarConsts::PARAMETER_VALUES, *(this->arrayToVector(newParam, args.nParam)));
    DebugMessage(className, memberName, "size of fitting parameters: %u\n", ps->getVectorSize(AdvLevmarConsts::PARAMETER_VALUES));

    ps->add(AdvLevmarConsts::LINK_IDS, *(this->arrayToVector(linkids, args.nParam)));
    DebugMessage(className, memberName, "size of fitting parameters: %u\n", ps->getVectorSize(AdvLevmarConsts::LINK_IDS));

    /* parameter errors */
    ps->add(AdvLevmarConsts::PARAM_ERRORS, *covarToErrorVector(newCovar, args.nParam));
    DebugMessage(className, memberName, "size of parameter errors: %u\n", ps->getVectorSize(AdvLevmarConsts::PARAM_ERRORS));

    /* covariance matrix */
    ps->add(AdvLevmarConsts::COVARIANCE_MATRIX, *(this->arrayToMatrix(newCovar, args.nParam, args.nParam)));
    DebugMessage(className, memberName, "size of covariance matrix: %u*%u\n",
        ps->getRowSize(AdvLevmarConsts::COVARIANCE_MATRIX), ps->getColumnSize(AdvLevmarConsts::COVARIANCE_MATRIX));

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

/**
 *  output stat value
 *
 *  \param[in] control a set of control variables
 *  \param[in] args    a input variables 
 *  \param[in] stat    termination stat of fitting
 *  \param[in] info    fitting infomations
 *  \param[in] param   a read/write buffer for fitting parameters
 */
//void AdvLevmarFit::outputStat(AdvLevmarControl &control, AdvLevmarArgs &args, AdvLevmarConsts::LevmarStat stat, Double *info, Double *param) {
//    string memberName=string("outputStat");
//    DebugMessage(className, memberName, "enter\n");
//
//    char intFmt[] ="%4s%4s%-19s : %d\n";
//    char statFmt[]="%4s%4s%-19s : %s\n";
//    char normFmt[]="%4s%4s%-19s : %e\n";
//    message(intFmt,  " ", " ", AdvLevmarConsts::ITERATION_COUNT.c_str(),   this->iterationCounts);
//    message(statFmt, " ", " ", AdvLevmarConsts::TERMINATION_STAT.c_str(),  AdvLevmarConsts::TERMINATION_REASON[stat].c_str());
//    message(normFmt, " ", " ", AdvLevmarConsts::R_FACTOR.c_str(),          this->getRFactor(control, args, param));
//    message(normFmt, " ", " ", AdvLevmarConsts::RESIDUAL_ERR_NORM.c_str(), info[1]);
//    message(normFmt, " ", " ", AdvLevmarConsts::GRADIENT_NORM.c_str(),     info[2]);
//    message(normFmt, " ", " ", AdvLevmarConsts::PARAM_DIFF_NORM.c_str(),   info[3]);
//    message("\n");
//    std::cout.flush();
//
//    DebugMessage(className, memberName, "exit\n");
//}

/**
 *  output fitted parametes ant their errors
 *
 *  \param[in] control a set of control variables for Levmar
 *  \param[in] args    the input arguments for Levmar
 *  \param[in] param   the read/write buffer for fitting parameters
 *  \param[in] param   the covariance matrix for fitting parameters
 */
//void AdvLevmarFit::outputFittedParam(AdvLevmarControl &constrol, AdvLevmarArgs &args, Double *param, Double *covar) {
//    string memberName=string("outputResult");
//    DebugMessage(className, memberName, "enter\n");
//
//    UInt4 ct=0;
//    message("%4s%4s%5s %23s %23s %23s %s\n", " ", " ", "No.", "initial", "result", "estimated error", "function");
//    for (std::vector<FuncBase*>::const_iterator fp=args.funcList.begin(); fp != args.funcList.end(); ++fp) {
//        for (UInt4 i=0; i<(*fp)->getNumberOfParam(); ++i) {
//            message("%4s%4s%5d %23.15e %23.15e %23.15e", " ", " ", ct, args.param[ct], param[ct], sqrt(covar[ct*(args.nParam+1)]));
//            if (i==0) {
//                message(" %s\n", (*fp)->getName().c_str());
//            } else {
//                message("\n");
//            }
//            ++ct;
//        }
//        message("\n");
//    }
//    std::cout.flush();
//
//
//    DebugMessage(className, memberName, "exit\n");
//}


/**
 *  output covariance matrix
 *
 *  \param[in] args  the input arguments for levmar
 *  \param[in] covar the covariance matrix for the fitting parametes
 */
//void AdvLevmarFit::outputCovarianceMatrix(AdvLevmarArgs &args, Double *covar) {
//    string memberName=string("outputCovarianceMatr");
//    DebugMessage(className, memberName, "enter\n");
//
//    if ( args.nParam <=6 ) {
//        message("%4s%4s%5s", " ", " ", "No.");
//        for (Int4 j=0; j<args.nParam; ++j) {
//            message(" %23d", j);
//        }
//        message("\n");
//        for (Int4 i=0; i<args.nParam; ++i) {
//            message("%4s%4s%5d", " ", " ", i);
//            for (Int4 j=0; j<args.nParam; ++j) {
//                message(" %23.15e", covar[i*args.nParam+j]);
//            }
//            message("\n");
//        }
//        message("\n");
//    } else {
//        message("%4s%4s%13s %23s\n", " ", " ", "compoments", "value");
//        for (Int4 i=0; i<args.nParam; ++i) {
//            for (Int4 j=0; j<args.nParam; ++j) {
//                message("%4s%4s(%5d %5d) %23.15e\n", " ", " ", i, j, covar[i*args.nParam+j]);
//            }
//        }
//        message("\n");
//    }
//    std::cout.flush();
//    DebugMessage(className, memberName, "exit\n");
//}

/**
 *  output
 *
 *  \param[in] control
 *  \param[in] args
 *  \param[in] stat
 *  \param[in] info
 *  \param[in] param
 *  \param[in] covar
 */
//void AdvLevmarFit::output(AdvLevmarControl &control, AdvLevmarArgs &args, AdvLevmarConsts::LevmarStat stat, Double *info, Double *param, Double *covar) {
//
//    message("%s\n", "Fitting Results");
//    message("%4s%s\n", " ", "Termination Stat");
//    this->outputStat(control, args, stat, info, param);
//    message("%4s%s\n", " ", "Fitted Parameters and Their Errors");
//    this->outputFittedParam(control, args, param, covar);
//    message("%4s%s\n", " ", "Covariance Matix");
//    this->outputCovarianceMatrix(args, covar);
//}


/**
 *  run the fitting by Levmar
 */
void AdvLevmarFit::Run() {
    string memberName = string("Run");
    DebugMessage(className, memberName, "enter\n");

    Int4 retval;
    clock_t startTime, stopTime, lapTime;
    lapTime=0;

    this->terminationId=AdvLevmarConsts::CONTINUE;
    this->info[3]=0.0;
    this->info[4]=this->im->args.opts[0];
    this->info[7]=0.0;
    this->info[8]=0.0;
    this->info[9]=0.0;
    this->getNorms(evalFunc, evalJaccobian, this->im->control, this->im->args, this->param, this->info+1, this->info+2);

    this->stat->push(this->createStat(this->im->control, this->im->args, this->terminationId, this->info, this->param, this->covar,this->linkids, lapTime));

    while ( this->iterationCounts < this->im->control.maxIterations && this->terminationId == AdvLevmarConsts::CONTINUE) {
        DebugMessage(className, memberName, "check user command:\n");
        if ( this->command->checkStop() ) {
            this->terminationId=AdvLevmarConsts::FORCE_QUIT_BY_USER;
            this->stat->push(this->createStat(this->im->control, this->im->args, this->terminationId, this->info, this->param, this->covar,this->linkids, lapTime));
            //message("%4s%s\n", AdvLevmarConsts::TERMINATION_REASON[this->terminationId].c_str());
            warningMessage(className, memberName, "%4s%s\n", AdvLevmarConsts::TERMINATION_REASON[this->terminationId].c_str());
            break;
        }
        smallLoop=(this->im->control.maxIterations >= this->iterationCounts + this->im->control.outputInterval ?
                   this->im->control.outputInterval : this->remainder);
        DebugMessage(className, memberName, "iteration for small loop: %u\n", smallLoop);

        startTime=clock();
        switch (this->im->control.constrain) {
            case AdvLevmarConsts::NO_CONSTRAIN:
                DebugMessage(className, memberName, "no cobnstrain\n");
                retval=doNoConstrain(this->im->control.useNumericalDiff, this->im->args, smallLoop);
                break;
            case AdvLevmarConsts::BOX:
                DebugMessage(className, memberName, "box\n");
                retval=doBC(this->im->control.useNumericalDiff, this->im->args, smallLoop);
                break;
#ifdef HAVE_LAPACK
            case AdvLevmarConsts::LEC:
                DebugMessage(className, memberName, "lec\n");
                retval=doLEC(this->im->control.useNumericalDiff, this->im->args, smallLoop);
                break;
            case AdvLevmarConsts::LIC:
                DebugMessage(className, memberName, "lic\n");
                retval=doLIC(this->im->control.useNumericalDiff, this->im->args, smallLoop);
                break;
            case AdvLevmarConsts::BLEC:
                DebugMessage(className, memberName, "blec\n");
                retval=doBLEC(this->im->control.useNumericalDiff, this->im->args, smallLoop);
                break;
            case AdvLevmarConsts::BLIC:
                DebugMessage(className, memberName, "blic\n");
                retval=doBLIC(this->im->control.useNumericalDiff, this->im->args, smallLoop);
                break;
            case AdvLevmarConsts::LEIC:
                DebugMessage(className, memberName, "leic\n");
                retval=doLEIC(this->im->control.useNumericalDiff, this->im->args, smallLoop);
                break;
            case AdvLevmarConsts::BLEIC:
                DebugMessage(className, memberName, "bleic\n");
                retval=doBLEIC(this->im->control.useNumericalDiff, this->im->args, smallLoop);
                break;
#endif /* HAVE_LAPACK */
        }
        stopTime=clock();
        lapTime=stopTime-startTime;
        DebugMessage(className, memberName, "small loop finish: %u\n", retval);

        /* count current iteration */
        this->iterationCounts += static_cast<Int4>(info[5]);

        this->terminationId=static_cast<AdvLevmarConsts::LevmarStat>(info[6]);
        if ( this->terminationId == AdvLevmarConsts::REACH_MAX_ITERATION && this->iterationCounts < this->im->control.maxIterations ) this->terminationId=AdvLevmarConsts::CONTINUE;

        this->stat->push(this->createStat(this->im->control, this->im->args, this->terminationId, this->info, this->param, this->covar,this->linkids, lapTime));
        DebugMessage(className, memberName, "size of the stat queue %u\n", this->stat->size());
    }

    DebugMessage(className, memberName, "exit\n");
    //pthread_exit(&(this->ThreadDescriptor));
}

