#include "AdvSimulatedAnnealing.hh"
#include "AdvModelSpecular.hh"
#include <unistd.h>
#include <cfloat>

/**  class name  */
const string AdvSimulatedAnnealing::className=string("AdvSimulatedAnnealing");


/**
 *  default constructor
 */
AdvSimulatedAnnealing::AdvSimulatedAnnealing() {
    methodName=string("Simulated Annealing");

    args = new AdvSimulatedAnnealingArgs();
    status = CLEAR;
    fmode = SingleFit;
}


/**
 *  destructor
 */
AdvSimulatedAnnealing::~AdvSimulatedAnnealing() {
    delete args;
}

/**
 *  set default values of parameters for the method
 */
AdvParamSet AdvSimulatedAnnealing::setDefaultParam(ElementContainer &src) {
    fmode = SingleFit;
    return args->setDefaultParam(src);
}

AdvParamSet AdvSimulatedAnnealing::setDefaultParam(ElementContainerArray &src) {
    fmode = MultiFit;
    ElementContainer s = src.Put(0);  // only use first ElementContainer in Array
    return args->setDefaultParam(s);
}


/**
 *  check parameterset
 */
Bool AdvSimulatedAnnealing::checkParam(ElementContainer &src, AdvDomain &domain, AdvParamSet &param) {
    fmode = SingleFit;
    return args->checkParam(src, domain, param);
}

Bool AdvSimulatedAnnealing::checkParam(ElementContainerArray &src, vector<AdvDomain> &domainArray, AdvParamSet &param) {
    fmode = MultiFit;
    return args->checkParam(src, domainArray, param);
}


/**
 *  to inner form:  convert into internal data
 */
void AdvSimulatedAnnealing::toInnerForm (ElementContainer &src, AdvDomain &domain, AdvParamSet &param) {
    string memberName=string("toInnerForm(ElementContainer &, AdvDomain &, AdvParamSet &)");
    DebugMessage(className, memberName, "enter\n");

    if (status == FITTING) {
        cout << "Cannot call toInnerForm(): Fitting is now running.\n";
        return;
    }
    if (status == FIT_SUSPENDED) {
        cout << "Cannot call toInnerForm: Stop fitting before calling toInnerForm().\n";
        return;
    }

    fmode = SingleFit;
    this->args->toInnerForm(src, domain, param);

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

void AdvSimulatedAnnealing::toInnerForm (ElementContainerArray &src, vector<AdvDomain> &domainArray, AdvParamSet &param) {
    string memberName=string("toInnerForm(ElementContainerArray &, AdvDomain &, AdvParamSet &)");
    DebugMessage(className, memberName, "enter\n");

    if (status == FITTING) {
        cout << "Cannot call toInnerForm(): Fitting is now running.\n";
        return;
    }
    if (status == FIT_SUSPENDED) {
        cout << "Cannot call toInnerForm: Stop fitting before calling toInnerForm().\n";
        return;
    }

    fmode = MultiFit;
    this->args->toInnerForm(src, domainArray, param);

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


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

    //  if toInnerForm is not called yet, do not start fitting
    if (status == CLEAR) {
        cout << "Fit can not start: parameters are not set.\n";
        return;
    }

    if (status == FITTING) {
        cout << "Fit can not start: fitting is now running.\n";
        return;
    }

    if (command == SUSPEND && status != FIT_SUSPENDED) status = FIT_SUSPENDED;

    if (status == FIT_SUSPENDED) {
        cout << "Resume fitting.\n";
        command = CONTINUE;
        return;
    }

    //  Start() will create a new thread whose starting point is Run()
    //  these functions are defined in ThreadBase class
    Start();

    Detach();

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

/**
 *  \return true if fitting is running.
 */
Bool AdvSimulatedAnnealing::isFitting() {
    cout << "current = " << args->ncall << endl;
    if (status == FITTING) return true;
    if (status == FIT_SUSPENDED) return true;
    return false;
}

/**
 *  command to stop fitting
 */
void AdvSimulatedAnnealing::stopFit() {
    command = STOP;
}
void AdvSimulatedAnnealing::abortFit() {
    command = STOP;
}

/**
 *  command to stop fitting
 */
void AdvSimulatedAnnealing::suspendFit() {
    command = SUSPEND;
}

void AdvSimulatedAnnealing::eval() {
    cout << "eval() is not implemented.\n";
}


void AdvSimulatedAnnealing::toElementContainer(ElementContainer &src, ElementContainer &dest) const {

    if (fmode != SingleFit) {
      cout << " Not Single Fit." << endl;
      return;
    }

    string xKey=src.PutXKey();
    string yKey=src.PutYKey();
    string eKey=src.PutEKey();

    dest.Add(xKey, args->srcBin.at(0), src.PutUnit(xKey));
    dest.Add(yKey, args->bestY.at(0),  src.PutUnit(yKey));
    dest.Add(eKey, src.Put(eKey),      src.PutUnit(eKey));
    dest.SetKeys(xKey, yKey, eKey);

    // method
    dest.AddToHeader("method", this->getMethodName());
}

void AdvSimulatedAnnealing::toElementContainer(ElementContainerArray &src, UInt4 id, ElementContainer &dest) const {

    if (fmode != MultiFit) {
      cout << " Not Multi Fit." << endl;
      return;
    }

    ElementContainer *srcElem = src.PutPointer(id);
    string xKey=srcElem->PutXKey();
    string yKey=srcElem->PutYKey();
    string eKey=srcElem->PutEKey();

    dest.Add(xKey, args->srcBin.at(0), srcElem->PutUnit(xKey));
    dest.Add(yKey, args->bestY.at(0),  srcElem->PutUnit(yKey));
    dest.Add(eKey, srcElem->Put(eKey), srcElem->PutUnit(eKey));
    dest.SetKeys(xKey, yKey, eKey);

    // method
    dest.AddToHeader("method", this->getMethodName());
}

void AdvSimulatedAnnealing::toElementContainerArray(ElementContainerArray &src, ElementContainerArray &dest) const {

    if (fmode != MultiFit) {
      cout << " Not Multi Fit." << endl;
      return;
    }

    Int4 nsize = src.PutSize();

    for (Int4 id = 0; id < nsize; ++id) {
      ElementContainer *destElem = dest.PutPointer(id);
      toElementContainer(src, id, *destElem);
    }
}


AdvParamSet AdvSimulatedAnnealing::getFittedParam() const {
    return args->getBestParam();
}


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

/**
 *  run the fitting by siman
 *  Start() will create a new thread whose starting point is Run()
 */
void AdvSimulatedAnnealing::Run() {

    if (fmode == SingleFit && args->funcList.empty()) {
      cout << "No function list is given" << endl;
      cout << "Cannot start fitting" << endl;
      return;
    }

    if (fmode == MultiFit && args->funcMatrix.empty()) {
      cout << "No function matrix is given" << endl;
      cout << "Cannot start fitting" << endl;
      return;
    }

    // START thread
    status = FITTING;

    simanXP* xp = new simanXP;
    xp->siman = this;

    xp->np = args->nParam;
    xp->p = new Double[xp->np];
    for (Int4 i=0; i<xp->np; ++i) {
      xp->p[i] = args->param[i];
    }

    args->ncall = 0;
    args->min_chi2 = DBL_MAX;

    if (fmode == MultiFit) {

      //  variable-size mode
      gsl_siman_solve(
        args->rand,                         //  random number generator
        xp,                                 //  The starting configuration of the system
        AdvSimulatedAnnealing::evalFunc_Multi, //  return the energy of a configuration xp.
        AdvSimulatedAnnealing::RANDOMWALK,     //  modify the configuration xp using a random step.
        AdvSimulatedAnnealing::DISTANCE,       //  return the distance between two configurations xp and yp.
        NULL,                               //  print the contents of the configuration xp.
        AdvSimulatedAnnealing::XPCOPY,         //  copy the configuration source into dest.
        AdvSimulatedAnnealing::XPNEW,          //  create a new copy of the configuration xp.
        AdvSimulatedAnnealing::XPDELATE,       //  destroy the configuration xp, freeing its memory.
        0,
        args->siman_params                  //  the parameters that control a run of gsl_siman_solve
      );

      AdvSimulatedAnnealing::evalFunc_Multi(xp);

    } else if (fmode == SingleFit) {

      //  variable-size mode
      gsl_siman_solve(
        args->rand,                         //  random number generator
        xp,                                 //  The starting configuration of the system
        AdvSimulatedAnnealing::evalFunc,       //  return the energy of a configuration xp.
        AdvSimulatedAnnealing::RANDOMWALK,     //  modify the configuration xp using a random step.
        AdvSimulatedAnnealing::DISTANCE,       //  return the distance between two configurations xp and yp.
        NULL,                               //  print the contents of the configuration xp.
        AdvSimulatedAnnealing::XPCOPY,         //  copy the configuration source into dest.
        AdvSimulatedAnnealing::XPNEW,          //  create a new copy of the configuration xp.
        AdvSimulatedAnnealing::XPDELATE,       //  destroy the configuration xp, freeing its memory.
        0,
        args->siman_params                  //  the parameters that control a run of gsl_siman_solve
      );

      AdvSimulatedAnnealing::evalFunc(xp);

    } else {
      cout << " Function is not given?" << endl;
      return;
    }

    status = FIT_DONE;
    cout << " ** Fit done **" << endl;

    // END thread
}

void AdvSimulatedAnnealing::XPCOPY(void *source, void *dest) {
    simanXP *xp1 = (simanXP*) source;
    simanXP *xp2 = (simanXP*) dest;
    xp2->siman = xp1->siman;
    xp2->np = xp1->np;
    int n = xp1->np;
    if (xp2->p == NULL) xp2->p = new Double [ n ];
    for (int i=0; i< n; i++) {
        xp2->p[i] = xp1->p[i];
    }
}

void* AdvSimulatedAnnealing::XPNEW(void *xp) {
    simanXP *xp_new = new simanXP;
    XPCOPY ( xp, xp_new );
    return xp_new;
}

void AdvSimulatedAnnealing::XPDELATE(void *xp) {
    simanXP *xp1 = (simanXP*) xp;
//    if (xp1->p) { delete [] xp1->p; xp1-p==NULL; }
    delete xp1;
}

void AdvSimulatedAnnealing::RANDOMWALK(const gsl_rng *r, void *xp, double step_size) {

    simanXP *xp1 = (simanXP*) xp;
    AdvSimulatedAnnealing* sa = xp1->siman;

    while (sa->command == SUSPEND) { 
        sa->status = FIT_SUSPENDED;
        sleep (1);
    }
    sa->status = FITTING;

    if (sa->command == STOP) {
        pthread_exit(NULL);
    }

    Int4                np  = xp1->np;

    Bool ub = (Int4)sa->args->ub.size() >= np ? true : false;
    Bool lb = (Int4)sa->args->lb.size() >= np ? true : false;

    Double dp, d, upper, lower;
    for ( Int4 i = 0; i < np; i++ ) {
        dp = step_size;

        upper = xp1->p[i] + dp;
        lower = xp1->p[i] - dp;

        if (ub && upper > sa->args->ub[i]) upper = sa->args->ub[i];
        if (lb && lower < sa->args->lb[i]) lower = sa->args->lb[i];

        d = upper - lower;

        xp1->p[i] = lower + gsl_rng_uniform(r) * d;
	if (xp1->p[i] < 0.0) {
           cout << "ERROR ";
           cout << "  ";
           cout << i;
           cout << "  ";
           cout << xp1->p[i];
           cout << "  ";
           cout << sa->args->lb[i];
           cout << endl;
        }
    }
}

double AdvSimulatedAnnealing::DISTANCE(void* xp, void* yp) {
    //  not implemented...
    return 0.0;
}

double AdvSimulatedAnnealing::evalFunc(void *xp) {
    simanXP *xp1 = (simanXP*) xp;
    AdvSimulatedAnnealing* sa = xp1->siman;
    AdvSimulatedAnnealingArgs* args = sa->args;

    vector<Double> result;

    Int4 nparam = 0;
    Int4 nf = (Int4) args->funcList.size();
    for (Int4 i=0; i<nf; ++i) {
        nparam += args->funcList[i]->getNumberOfParam();
    }

    args->SetCurParam(xp1->p, nparam);

    vector<Double> param ( nparam );
    for (Int4 i=0; i<nparam; ++i) {
      param[i] = xp1->p[i];
    }

    Double ret = AdvSimulatedAnnealing::evalF(args->funcList, args->srcX[0], args->ref[0], 
					   args->err[0], param, result,
                                           args->chi2mode);

    args->ncall++;

    // printf ( " Eval: %g , ncall = %d\n" , ret, args->ncall);

    if (ret < args->min_chi2) {
      // printf ( " Eval: %g , ncall = %d\n" , ret, ncall);

      // save current best result to file.
      ofstream ofs1( "./.siman_current_param.txt" );
      ofstream ofs2( "./.siman_current_data.txt" );
      if (ofs1) {
        ofs1 << "# Current parameter in Simulated Annealing" << endl;
        ofs1 << "#   ncall = " << args->ncall << endl;
        ofs1 << "#   chi2  = " << ret << endl << endl;
        Int4 offset=0;
        for (Int4 i=0; i<nf; ++i) {
            ofs1 << "# Function " << i << endl;
            Int4 n = args->funcList[i]->getNumberOfParam();
            for (Int4 j=0;j<n;++j) {
                ofs1 << *(xp1->p + offset + j) << endl;
            }
           offset += n;
        }
      }
      if (ofs2) {
        ofs2 << "# Current fit in Simulated Annealing" << endl;
        ofs2 << "#   ncall = " << args->ncall << endl;
        ofs2 << "#   chi2  = " << ret << endl << endl;
        Int4 nd = args->srcX[0].size();
        for (Int4 k=0; k<nd; ++k) {
            ofs2 << args->srcX[0][k] << "\t" << result[k] << endl;
        }
      }
      args->min_chi2 = ret;
      args->SetCurParamBest();
    }

    return ret;
}

double AdvSimulatedAnnealing::evalF(vector<AdvFuncBase*> funcList, vector<Double> srcX, vector<Double> ref, 
                                 vector<Double> error, vector<Double> param, vector<Double>& result,
                                 AdvSimulatedAnnealingArgs::CHI2MODE chi2mode) {

    Int4     nd  = (Int4) srcX.size();
    Int4     nf  = (Int4) funcList.size();

    result.assign( nd, 0.0 );

    Double chi2, diff;
    Int4  offset=0, count;

    for (Int4 i=0; i<nf; ++i) {
        Int4 n = funcList[i]->getNumberOfParam();
        vector<Double> p ( n );
        for (Int4 j=0;j<n;++j) {
            p[j] = param[offset+j];
        }

        for (Int4 k=0; k<nd; ++k) {
            result[k] += funcList[i]->eval(srcX[k], p);
        }
        offset += n;
    }
 
    chi2 = 0.0;
    count = 0;

    for (Int4 k=0; k<nd; ++k) {
        switch (chi2mode) {
        case AdvSimulatedAnnealingArgs::USE_GIVEN_ERRORS:
            if (error[k] > 0.0 ) {
              Double c = (result[k] - ref[k]) / error[k];
              chi2 += c * c;
              count++;
            }
            break;
        case AdvSimulatedAnnealingArgs::USE_Y_INVERSE:
            if (ref[k] > 0.0 ) {
              Double c = (result[k] - ref[k]) / ref[k];
              chi2 += c * c;
              count++;
            }
            break;
        case AdvSimulatedAnnealingArgs::USE_LOG:
            if (ref[k] > 0.0 && result[k] > 0.0 ) {
              Double c = log(result[k]) - log(ref[k]);
              chi2 += c * c;
              count++;
            }
            break;
        case AdvSimulatedAnnealingArgs::NO_WEIGHT:
            diff = ( result[k] - ref[k] );
            chi2 += diff * diff;
            count++;
            break;
        }
    }

    Double ret = count > 0 ? ( chi2 / count ) : 0.0;
    return ret;
}

double AdvSimulatedAnnealing::evalFunc_Multi(void *xp) {
    simanXP *xp1 = (simanXP*) xp;
    AdvSimulatedAnnealing* sa = xp1->siman;
    AdvSimulatedAnnealingArgs* args = sa->args;

    vector<vector<Double> > result;

    Int4 nm = (Int4) args->funcMatrix.size();
    Int4 nparam_save = -1;

    Double ret = 0.0;

    result.resize(nm);

    for (Int4 im=0; im<nm; ++im) {

      Int4 nparam = 0;
      Int4 nf = args->funcMatrix[im].size();
      for (Int4 i=0; i<nf; ++i) {
        nparam += args->funcMatrix[im][i]->getNumberOfParam();
      }

      if (nparam_save < 0) {
        nparam_save = nparam;
        args->SetCurParam(xp1->p, nparam);
      }

      if (nparam_save != nparam) {
        cout << "Error: Number of params is different" << endl;
        return DBL_MAX;
      }

      vector<Double> param ( nparam );
      for (Int4 i=0; i<nparam; ++i) {
        param[i] = xp1->p[i];
      }

      result[im].resize(args->srcX[im].size());

      ret += AdvSimulatedAnnealing::evalF(args->funcMatrix[im], args->srcX[im], args->ref[im], 
		                       args->err[im], param, result[im],
                                       args->chi2mode);

    // printf ( " Eval: %g , data = %d, ncall = %d\n" , ret, im, args->ncall);

    }

    args->ncall++;

    if ( args->ncall % 10000 == 0) {
      printf ( " Eval: %g , ncall = %d\n" , ret, args->ncall);
    }

    if (ret < args->min_chi2) {
      // printf ( " Eval: %g , ncall = %d\n" , ret, args->ncall);

      // save current best result to file.
      ofstream ofs1( "./.siman_current_param.txt" );
      ofstream ofs2( "./.siman_current_data.txt" );
      if (ofs1) {
        ofs1 << "# Current parameter in Simulated Annealing" << endl;
        ofs1 << "#   ncall = " << args->ncall << endl;
        ofs1 << "#   chi2  = " << ret << endl << endl;
        Int4 offset=0;
        Int4 nf = args->funcMatrix[0].size();
        for (Int4 i=0; i<nf; ++i) {
            ofs1 << "# Function " << i << endl;
            Int4 n = args->funcMatrix[0][i]->getNumberOfParam();
            for (Int4 j=0;j<n;++j) {
                ofs1 << *(xp1->p + offset + j) << endl;
            }
           offset += n;
        }
      }
      if (ofs2) {
        ofs2 << "# Current fit in Simulated Annealing" << endl;
        ofs2 << "#   ncall = " << args->ncall << endl;
        ofs2 << "#   chi2  = " << ret << endl << endl;
        for (Int4 im=0; im<nm; ++im) {
          Int4 nd = args->srcX[im].size();
          ofs2 << endl;
          ofs2 <<"# Fit "<< im << endl;
          for (Int4 k=0; k<nd; ++k) {
            ofs2 << args->srcX[im][k] << "\t" << result[im][k] << endl;
          }
	}
      }
      args->min_chi2 = ret;
      args->SetCurParamBest();
    }

    return ret;
}

void AdvSimulatedAnnealing::PrintStatus() {
    switch (status) {
      case CLEAR:
        cout << "Waiting for parameters" << endl;
        break;
      case READY:
        cout << "Parameters are set. Ready for fitting." << endl;
        break;
      case FITTING:
        cout << "Now fitting." << endl;
        cout << "  Number of evaluation = " << args->ncall << endl;
        cout << "  best chi2 = " << args->min_chi2 << endl;
        break;
      case FIT_SUSPENDED:
        cout << "Fitting is suspended. To resume fitting, call fit()." << endl;
        cout << "  Number of evaluation = " << args->ncall << endl;
        cout << "  best chi2 = " << args->min_chi2 << endl;
        break;
      case FIT_DONE:
        cout << "Fitting is done." << endl;
        cout << "  Number of evaluation = " << args->ncall << endl;
        cout << "  best chi2 = " << args->min_chi2 << endl;
        break;
    }

}

int AdvSimulatedAnnealing::GetStatus() {
    switch (status) {
      case CLEAR:
        return 1;
      case READY:
        return 2;
      case FITTING:
        return 3;
      case FIT_SUSPENDED:
        return 4;
      case FIT_DONE:
        return 5;
    }
return 0;
}
