/*
 * Project: MoleCuilder
 * Description: creates and alters molecular systems
 * Copyright (C)  2012 University of Bonn. All rights reserved.
 * Please see the COPYING file or "Copyright notice" in builder.cpp for details.
 *
 *
 *   This file is part of MoleCuilder.
 *
 *    MoleCuilder is free software: you can redistribute it and/or modify
 *    it under the terms of the GNU General Public License as published by
 *    the Free Software Foundation, either version 2 of the License, or
 *    (at your option) any later version.
 *
 *    MoleCuilder is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *    GNU General Public License for more details.
 *
 *    You should have received a copy of the GNU General Public License
 *    along with MoleCuilder.  If not, see <http://www.gnu.org/licenses/>.
 */

/*
 * FunctionApproximation.cpp
 *
 *  Created on: 02.10.2012
 *      Author: heber
 */

// include config.h
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include "CodePatterns/MemDebug.hpp"

#include "FunctionApproximation.hpp"

#include <algorithm>
#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <iostream>
#include <iterator>
#include <numeric>
#include <sstream>

#include <levmar.h>

#include "CodePatterns/Assert.hpp"
#include "CodePatterns/Log.hpp"

#include "FunctionApproximation/FunctionModel.hpp"

void FunctionApproximation::setTrainingData(const inputs_t &input, const outputs_t &output)
{
  ASSERT( input.size() == output.size(),
      "FunctionApproximation::setTrainingData() - the number of input and output tuples differ: "+toString(input.size())+"!="
      +toString(output.size())+".");
  if (input.size() != 0) {
    ASSERT( input[0].size() == input_dimension,
        "FunctionApproximation::setTrainingData() - the dimension of the input tuples and input dimension differ: "+toString(input[0].size())+"!="
        +toString(input_dimension)+".");
    input_data = input;
    ASSERT( output[0].size() == output_dimension,
        "FunctionApproximation::setTrainingData() - the dimension of the output tuples and output dimension differ: "+toString(output[0].size())+"!="
        +toString(output_dimension)+".");
    output_data = output;
  } else {
    ELOG(2, "Given vectors of training data are empty, clearing internal vectors accordingly.");
    input_data.clear();
    output_data.clear();
  }
}

void FunctionApproximation::setModelFunction(FunctionModel &_model)
{
  model= _model;
}


/* Meyer's (reformulated) problem, minimum at (2.48, 6.18, 3.45) */
void meyer(double *p, double *x, int m, int n, void *data)
{
register int i;
double ui;

        for(i=0; i<n; ++i){
                ui=0.45+0.05*i;
                x[i]=p[0]*exp(10.0*p[1]/(ui+p[2]) - 13.0);
        }
}

void jacmeyer(double *p, double *jac, int m, int n, void *data)
{
register int i, j;
double ui, tmp;

  for(i=j=0; i<n; ++i){
          ui=0.45+0.05*i;
          tmp=exp(10.0*p[1]/(ui+p[2]) - 13.0);

          jac[j++]=tmp;
          jac[j++]=10.0*p[0]*tmp/(ui+p[2]);
          jac[j++]=-10.0*p[0]*p[1]*tmp/((ui+p[2])*(ui+p[2]));
  }
}

/** Callback to circumvent boost::bind, boost::function and function pointer problem.
 *
 * See here (second answer!) to the nature of the problem:
 * http://stackoverflow.com/questions/282372/demote-boostfunction-to-a-plain-function-pointer
 *
 * We cannot use a boost::bind bounded boost::function as a function pointer.
 * boost::function::target() will just return NULL because the signature does not
 * match. We have to use a C-style callback function and our luck is that
 * the levmar signature provides for a void* additional data pointer which we
 * can cast back to our FunctionApproximation class, as we need access to the
 * data contained, e.g. the FunctionModel reference FunctionApproximation::model.
 *
 */
void FunctionApproximation::LevMarCallback(double *p, double *x, int m, int n, void *data)
{
  FunctionApproximation *approximator = static_cast<FunctionApproximation *>(data);
  ASSERT( approximator != NULL,
      "LevMarCallback() - received data does not represent a FunctionApproximation object.");
  boost::function<void(double*,double*,int,int,void*)> function =
      boost::bind(&FunctionApproximation::evaluate, approximator, _1, _2, _3, _4, _5);
  function(p,x,m,n,data);
}

void FunctionApproximation::operator()()
{
  // let levmar optimize
  register int i, j;
  int ret;
  double *p;
  double *x; // we set zero vector by giving NULL
  int m, n;
  double opts[LM_OPTS_SZ], info[LM_INFO_SZ];

  opts[0]=LM_INIT_MU; opts[1]=1E-15; opts[2]=1E-15; opts[3]=1E-20;
  opts[4]= LM_DIFF_DELTA; // relevant only if the Jacobian is approximated using finite differences; specifies forward differencing
  //opts[4]=-LM_DIFF_DELTA; // specifies central differencing to approximate Jacobian; more accurate but more expensive to compute!

  m = model.getParameterDimension();
  n = output_data.size();
  const FunctionModel::parameters_t params = model.getParameters();
  {
    p = new double[m];
    size_t index = 0;
    for(FunctionModel::parameters_t::const_iterator paramiter = params.begin();
        paramiter != params.end();
        ++paramiter, ++index) {
      p[index] = *paramiter;
    }
  }
  {
    x = new double[n];
    size_t index = 0;
    for(outputs_t::const_iterator outiter = output_data.begin();
        outiter != output_data.end();
        ++outiter, ++index) {
      x[index] = (*outiter)[0];
    }
  }

  {
    double *work, *covar;
    work=(double *)malloc((LM_DIF_WORKSZ(m, n)+m*m)*sizeof(double));
    if(!work){
      ELOG(0, "FunctionApproximation::operator() - memory allocation request failed.");
      return;
    }
    covar=work+LM_DIF_WORKSZ(m, n);

    // give this pointer as additional data to construct function pointer in
    // LevMarCallback and call
    ret=dlevmar_dif(&FunctionApproximation::LevMarCallback, p, x, m, n, 1000, opts, info, work, covar, this); // no Jacobian, caller allocates work memory, covariance estimated

    {
      std::stringstream covar_msg;
      covar_msg << "Covariance of the fit:\n";
      for(i=0; i<m; ++i){
        for(j=0; j<m; ++j)
          covar_msg << covar[i*m+j] << " ";
        covar_msg << std::endl;
      }
      covar_msg << std::endl;
      LOG(1, "INFO: " << covar_msg.str());
    }

    free(work);
  }

//  {
//   double err[16];
//   dlevmar_chkjac(meyer, jacmeyer, p, m, n, NULL, err);
//   for(i=0; i<n; ++i) printf("gradient %d, err %g\n", i, err[i]);
//  }

  {
   std::stringstream result_msg;
   result_msg << "Levenberg-Marquardt returned " << ret << " in " << info[5] << " iter, reason " << info[6] << "\nSolution: ";
   for(i=0; i<m; ++i)
     result_msg << p[i] << " ";
   result_msg << "\n\nMinimization info:\n";
   std::vector<std::string> infonames(LM_INFO_SZ);
   infonames[0] = std::string("||e||_2 at initial p");
   infonames[1] = std::string("||e||_2");
   infonames[2] = std::string("||J^T e||_inf");
   infonames[3] = std::string("||Dp||_2");
   infonames[4] = std::string("mu/max[J^T J]_ii");
   infonames[5] = std::string("# iterations");
   infonames[6] = std::string("reason for termination");
   infonames[7] = std::string(" # function evaluations");
   infonames[8] = std::string(" # Jacobian evaluations");
   infonames[9] = std::string(" # linear systems solved");
   for(i=0; i<LM_INFO_SZ; ++i)
      result_msg << infonames[i] << ": " << info[i] << " ";
    result_msg << std::endl;
    LOG(1, "INFO: " << result_msg.str());
  }

  delete[] p;
  delete[] x;
}

double SquaredDifference(const double res1, const double res2)
{
  return (res1-res2)*(res1-res2);
}

void FunctionApproximation::evaluate(double *p, double *x, int m, int n, void *data)
{
  // first set parameters
  ASSERT( (size_t)m == model.getParameterDimension(),
      "FunctionApproximation::evaluate() - LevMar expects "+toString(m)
      +" parameters but the model function expects "+toString(model.getParameterDimension())+".");
  ASSERT( (size_t)n == output_data.size(),
      "FunctionApproximation::evaluate() - LevMar expects "+toString(n)
      +" outputs but we provide "+toString(output_data.size())+".");
  FunctionModel::parameters_t params(m, 0.);
  std::copy(p, p+m, params.begin());
  model.setParameters(params);

  // then evaluate
  if (!output_data.empty()) {
    inputs_t::const_iterator initer = input_data.begin();
    outputs_t::const_iterator outiter = output_data.begin();
    size_t index = 0;
    for (; initer != input_data.end(); ++initer, ++outiter, ++index) {
      // result may be a vector, calculate L2 norm
      const FunctionModel::results_t functionvalue =
          model(*initer);
      std::vector<double> differences(functionvalue.size(), 0.);
      std::transform(
          functionvalue.begin(), functionvalue.end(), outiter->begin(),
          differences.begin(),
          &SquaredDifference);
      x[index] = std::accumulate(differences.begin(), differences.end(), 0.);
    }
  }
}