#include <iostream>
#include <istream>
#include <ostream>
#include <cassert>
//#include <stdexcept>

#include <vector>

#include "AdvVectorArrayConv.hh"
#include "AdvLevmarFunc.hh"
#include "AdvFuncComb.hh"
#include "AdvAdditionalData.hh"

#undef USE_POINTER
#undef USE_VECTOR

/*#define USE_POINTER */
#define USE_VECTOR /**/

/**
 *  evaluate the values of the fitting function
 *
 *  \param[in]  p              the values of fitting parameters
 *  \param[out] y              the values of fitting functions
 *  \param[in]  m              the size   of the fitting parameters array
 *  \param[in]  n              the size   of the array for the evaluated values of the fitting function
 *  \param[in]  additionalData the additional data to evaluate the values of the fitting functions
 */



void evalFunc(double *p, double *y, int m, int n, void *additionalData) {
    const string memberName=string("evalFunc(Double*, Double*, Int4, Int4, void*)");
#ifdef DEBUG
    std::cerr << "Debug: " << memberName << __FILE__ << ", " << __LINE__ << ": enter" << endl;
#endif

    assert(p != NULL);
    assert(y != NULL);
    assert(m > 0);
    assert(n > 0);
    assert(additionalData != NULL);

    //AdditionalData *adata=static_cast<AdditionalData*>(additionalData);
    AdvAdditionalData *adata=static_cast<AdvAdditionalData*>(additionalData);
    assert(! adata->empty());
    assert(adata->check());
    //assert(adata->w != NULL);
    //assert(adata->funcList != NULL);
#ifdef DEBUG
    std::cerr << "Debug: " << memberName << "success cast" << endl;
#endif

#ifdef USE_POINTER
      FuncComb *func;
      UInt4 ndata;
      Double *seriesX;
      vector<Double> *seriesW
      Double *seriesV;

      UInt4 offset=0;
      for (UInt4 i=0; i<adata->getNumberOfSeries; ++i) {
          func=adata->getFunc(i);
          seriesN=adata->getNumberOfData(i);
          seriesX=adata->getX(i);
          seriesW=adata->getWeight(i);

          func->set(p, m);
          seriesV=func->eval(seriesX, n);
          for (UInt4 j=0; j<seriesN; ++j) {
              y[offset+j]=v[j]*w[j];
          }
          offset += seriesN;
      }
//    Double sum;
//    Int4  funcOffset;
//    for (Int4 k=0; k<n; ++k) {
//        Double xk=adata->x->at(k);
//        sum=0.0;
//        funcOffset=0;
//        //std::cout << "evalFunc x=" << xk << " (k, i, sum, funcOffset)=" ;
//        for (UInt4 i=0; i<adata->funcList->size(); ++i) {
//            fUNCbAse *fp = adata->funcList->at(i);
//            Int4 nFuncParam=static_cast<Int4>(fp->getNumberOfParam());
//            //sum        += fp->eval(xk, arrayToVector(p+funcOffset, nFuncParam));
//            sum        +=  fp->eval(xk, p+funcOffset);
//            funcOffset += nFuncParam;
//            //std::cout << " (" << k << ", " << i << ", " << sum << ", " << funcOffset <<")";
//         }
//         //std::cout << endl;
//         y[k] = adata->useWeight ? sum/adata->w->at(k) : sum;
//         if ( funcOffset != m ) {
//             fprintf(stderr, "%s: funcOffset (%d) is not equal to %d\n", memberName.c_str(), funcOffset ,m);
//         }
//    }
#endif // USE_POINTER

#ifdef USE_VECTOR
    AdvVectorArrayConv *conv = new AdvVectorArrayConv();
    vector<Double> *pp = conv->arrayToVector(p, m);
    //std::cerr << "Debug: " << memberName << " convert array to vector" << endl;

//cout << "koko-1-,"<<adata->linklist[0] << endl;

    adata->set(*pp);
    adata->eval(n, y);

//    f = new FuncComb(*(adata->funcList), *pp);
//    //f->dump();
//    v  =  f->eval(*(adata->x));
//    if (adata->useWeight) {
//        for (Int4 i=0; i<n; ++i) {
//            y[i] = v.at(i)/adata->w->at(i);
//        }
//    } else {
//        for (Int4 i=0; i<n; ++i) {
//            y[i] = v[i];
//        }
//    }
//    delete f;  f=NULL;
//    pp->clear(), delete pp; pp=NULL;
//    seriesV.clear();
//    delete conv; conv=NULL;
#endif // USE_VECTOR
#ifdef DEBUG
    std::cerr << "Debug: " << memberName << __FILE__ << ", " << __LINE__ << ": exit" << endl;
#endif
}

/**
 *  evaluate Jaccobian of the object function for Levmar
 *
 * \param[in] p              the pointer to parameters (array) for the object function, p[m]
 * \param[in] jac            the pointer to Jaccobian of the object function, size j[n*m], access j[m*i + k] (0<=i<n, 0<=k<m)
 * \param[in] m              the number of parameters
 * \param[in] n              the number of values of the object functions
 * \param[in] additionalData the additional data to evaluate the values of the object function
 */
void evalJaccobian(double *p, double *jac, int m, int n, void *additionalData) {

    const string memberName=string("evalJaccobian(Double*, Double*, Int4, Int4, void*)");
#ifdef DEBUG
    std::cerr << "Debug: " << memberName << __FILE__ << ", " << __LINE__ <<": enter" << endl;
#endif

    assert(p != NULL);
    assert(jac != NULL);
    assert(m > 0);
    assert(n > 0);
    assert(additionalData != NULL);

    struct AdvAdditionalData *adata=static_cast<AdvAdditionalData*>(additionalData);
#ifdef DEBUG
    std::cerr << "Debug: " << memberName << __FILE__ << ", " << __LINE__ <<": empty: " << adata->empty() << endl;
#endif
    assert(! adata->empty());
#ifdef DEBUG
    std::cerr << "Debug: " << memberName << __FILE__ << ", " << __LINE__ <<": check: " << adata->check() << endl;
#endif
    assert(adata->check());
    //assert(adata->w != NULL);
    //assert(adata->funcList != NULL);
#ifdef DEBUG
    std::cerr << "Debug: " << memberName << " (s, m, n)=(" << adata->getNumberOfSeries() << ", " << m << ", " << n << ")" << endl;
#endif

#ifdef USE_POINTER
    for (Int4 k=0; k<n; ++k) {
        VectorArratConv *conv = new VectorArrayConv();
        Double xk = adata->x->at(k);
        Double wk = adata->w->at(k);
        Int4  funcOffset=0;
        //std::cout << memberName << "  x=" << xk << " (k, i, l, funcOffset)=" << endl;
        for (UInt4 i=0; i<adata->funcList->size(); ++i) {
            FuncBase *fp=(adata->funcList->at(i));
            Int4 nFuncParam=static_cast<Int4>(fp->getNumberOfParam());
            Int4 offset=m*k+funcOffset;
            Double *g = new Double[nFuncParam];
            g=fp->gradient(xk, p+funcOffset);
            for (Int4 l=0; l<nFuncParam; ++l) {
                jac[offset+l] = adata->useWeight ? g[l]/wk : g[l];
            }
            delete[] g; g=NULL;
            //vector<Double> g= fp->gradient(xk, arrayToVector(p+funcOffset, nFuncParam));
            //for (UInt4 l=0; l<nFuncParam); ++l) {
            //    jac[offset+l] = adata->useWeight ? g.at(l)/wk : g.at(l);
            //    //std::cout << " (" << k << ", " << i << ", " << l << ", " << funcOffset <<")";
            //}
            //g.clear();
            funcOffset += nFuncParam;
        }
        if ( funcOffset != m ) {
            fprintf(stderr, "%s: funcOffset (%d) is not equal to %d\n", memberName.c_str(), funcOffset ,m);
        }
    }
#endif // USE_POINTER

#ifdef USE_VECTOR
    AdvVectorArrayConv *conv=new AdvVectorArrayConv();
    vector<Double> *pp = conv->arrayToVector(p, m);

#ifdef DEBUG
    std::cerr << "Debug: " << memberName << __FILE__ << ", " << __LINE__ << ": x colr" << endl;
#endif

    adata->set(*pp);
#ifdef DEBUG
    std::cerr << "Debug: " << memberName << __FILE__ << ", " << __LINE__ << ": x colr" << endl;
#endif
    adata->evalJacobian(n, m, jac);
#ifdef DEBUG
    std::cerr << "Debug: " << memberName << __FILE__ << ", " << __LINE__ << ": x colr" << endl;
#endif


//    f = new FuncComb(*(adata->funcList), *pp);
//    //f->dump();
//    vector< vector<Double> > jj =  f->gradient(*(adata->x));
//    if (adata->useWeight) {
//        for (Int4 i=0; i<n; ++i) {
//            Double wi=adata->w->at(i);
//            for (Int4 j=0; j<m; ++j) {
//                jac[i*m+j] = jj.at(i).at(j)/wi;
//            }
//        }
//    } else {
//        for (Int4 i=0; i<n; ++i) {
//            for (Int4 j=0; j<m; ++j) {
//                jac[i*m+j] = jj.at(i).at(j);
//            }
//        }
//    }
//    delete f;  f=NULL;
//    pp->clear(); delete pp; pp=NULL;
//    for (Int4 i=0; i<n; ++i) {
//        jj.at(i).clear();
//    }
//    jj.clear();
#endif //  USE_VECTOR
#ifdef DEBUG
    std::cerr << "Debug: " << memberName << __FILE__ << ", " << __LINE__ <<": exit" << endl;
#endif

}

