/*
* LEGAL NOTICE
* This computer software was prepared by Battelle Memorial Institute,
* hereinafter the Contractor, under Contract No. DE-AC05-76RL0 1830
* with the Department of Energy ( DOE ). NEITHER THE GOVERNMENT NOR THE
* CONTRACTOR MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR ASSUMES ANY
* LIABILITY FOR THE USE OF THIS SOFTWARE. This notice including this
* sentence must appear on any copies of this computer software.
*
* EXPORT CONTROL
* User agrees that the Software will not be shipped, transferred or
* exported into any country or used in any manner prohibited by the
* United States Export Administration Act or any other applicable
* export laws, restrictions or regulations (collectively the "Export Laws").
* Export of the Software may require some form of license or other
* authority from the U.S. Government, and failure to obtain such
* export control license may result in criminal liability under
* U.S. laws. In addition, if the Software is identified as export controlled
* items under the Export Laws, User represents and warrants that User
* is not a citizen, or otherwise located within, an embargoed nation
* (including without limitation Iran, Syria, Sudan, Cuba, and North Korea)
*     and that User is not otherwise prohibited
* under the Export Laws from receiving the Software.
*
* Copyright 2011 Battelle Memorial Institute.  All Rights Reserved.
* Distributed as open-source under the terms of the Educational Community
* License version 2.0 (ECL 2.0). http://www.opensource.org/licenses/ecl2.php
*
* For further details, see: http://www.globalchange.umd.edu/models/gcam/
*
*/

/*!
* \file logbroyden.cpp
* \ingroup objects
* \brief LogBroyden class (Broyden's method solver) source file
* \author Robert Link
*/

#include "util/base/include/definitions.h"
#include <string>
#include <algorithm>
#include <iomanip>
#include <math.h>
#include <xercesc/dom/DOMNode.hpp>
#include <xercesc/dom/DOMNodeList.hpp>

#include "solution/solvers/include/solver_component.h"
#include "solution/solvers/include/logbroyden.hpp"
#include "solution/util/include/calc_counter.h"
#include "marketplace/include/marketplace.h"
#include "containers/include/world.h"
#include "solution/util/include/solution_info_set.h"
#include "solution/util/include/solution_info.h"
#include "solution/util/include/solver_library.h"
#include "util/base/include/util.h"
#include "util/base/include/configuration.h"
#include "util/logger/include/ilogger.h"
#include "util/base/include/xml_helper.h"
#include "solution/util/include/solution_info_filter_factory.h"
#include "solution/util/include/solvable_nr_solution_info_filter.h"

#include "solution/util/include/functor-subs.hpp"
#include "solution/util/include/linesearch.hpp"
#include "solution/util/include/fdjac.hpp"
#include "solution/util/include/edfun.hpp"
#include "solution/util/include/ublas-helpers.hpp"
#include "util/base/include/fltcmp.hpp"
#include "solution/util/include/jacobian-precondition.hpp"

#if USE_LAPACK
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/bindings/lapack/gesvd.hpp>
#include "solution/util/include/svd_invert_solve.hpp"
#else
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/lu.hpp>
#endif

#include "util/base/include/timer.h"

using namespace xercesc;

std::string LogBroyden::SOLVER_NAME = "broyden-solver-component";

#if USE_LAPACK
#define UBMATRIX boost::numeric::ublas::matrix<double,boost::numeric::ublas::column_major>
#else
#define UBMATRIX boost::numeric::ublas::matrix<double>
#endif
#define UBVECTOR boost::numeric::ublas::vector<double>

namespace {
  // helper functions for the std::transform algorithm
  inline double SI2lgprice (const SolutionInfo &si) {
    double p = std::max(si.getPrice(), util::getTinyNumber());
    return log( p );
  }
  inline double SI2price (const SolutionInfo &si) {return si.getPrice();}

  // read-only accessor for solutionInfoSet (used to prepare log outputs)
  const SolutionInfoSet *cSolInfo=0;

  // utility function for finding the minimimum and maximum absolute
  // value entries in a vector.
  void locate_vector_minmax(const UBVECTOR &v, double &vmax, double &vmin, int &imax, int &imin)
  {
    vmax = vmin = fabs(v[0]);
    imax = imin = 0;
    for(int i=1; i<v.size(); ++i) {
      double vabs = fabs(v[i]);
      if(vabs < vmin) {
        vmin = vabs;
        imin = i;
      }
      if(vabs > vmax) {
        vmax = vabs;
        imax = i;
      }
    }
  }
}

int LogBroyden::mLastPer = 0;
int LogBroyden::mPerIter = 0;

bool LogBroyden::XMLParse( const DOMNode* aNode ) {
    // assume we were passed a valid node.
    assert( aNode );

    // get the children of the node.
    DOMNodeList* nodeList = aNode->getChildNodes();

    // loop through the children
    for ( unsigned int i = 0; i < nodeList->getLength(); ++i ){
        DOMNode* curr = nodeList->item( i );
        std::string nodeName = XMLHelper<std::string>::safeTranscode( curr->getNodeName() );

        if( nodeName == "#text" ) {
            continue;
        }
        else if( nodeName == "max-iterations" ) {
            mMaxIter = XMLHelper<unsigned int>::getValue( curr );
        }
        else if( nodeName == "ftol" ) {
            mFTOL = XMLHelper<double>::getValue( curr );
        }
        else if( nodeName == "solution-info-filter" ) {
            mSolutionInfoFilter.reset(
                                      SolutionInfoFilterFactory::createSolutionInfoFilterFromString( XMLHelper<std::string>::getValue( curr ) ) );
        }
        else if(nodeName == "linear-price") {
          mLogPricep = false;
        }
        else if(nodeName == "log-price") {
          mLogPricep = true;    // not strictly necessary, as this is the default.
        }
        else if( SolutionInfoFilterFactory::hasSolutionInfoFilter( nodeName ) ) {
            mSolutionInfoFilter.reset( SolutionInfoFilterFactory::createAndParseSolutionInfoFilter( nodeName, curr ) );
        }
        else {
            ILogger& mainLog = ILogger::getLogger( "main_log" );
            mainLog.setLevel( ILogger::WARNING );
            mainLog << "Unrecognized text string: " << nodeName << " found while parsing "
                    << getXMLName() << "." << std::endl;
        }
    }
    return true;
}

/*! \brief Broyden's method solver.
 * \details Attempts to solve the selected markets using Broyden's
 * method (see Numerical Recipes sectn. 9.7).  Broyden's method is
 * broadly similar to the Newton-Raphson Method.  At each iteration we
 * solve B . dx = -F to get the solution step for that iteration, we
 * backtrack as necessary to ensure that F . F decreases at each step,
 * and we iterate to convergence.  The difference is that in place of
 * the exact Jacobian, J, we use an approximate Jacobian, B.  At each
 * iteration we update B using Broyden's secant condition: B(i+1) =
 * B( i ) + ((dF( i ) - B( i ) . dx( i )) X dx( i )) / (dx . dx).  Since this
 * update does not require any model evaluations, it is *much* faster
 * than computing finite-difference Jacobians.
 *
 * We still need an initial approximation to the Jacobian.  Whenever
 * possible we'll get that from estimates of derivatives that we've
 * previously squirreled away in the SolutionInfo objects.  When we
 * add a new market to the solution set, we may not have good
 * derivatives for the newcomer.  In that case we'll do finite
 * difference approximations for the new column, and we'll zero the
 * off-diagonal terms of the new row.
 *
 * The solver can run in either log-log mode or linear-linear mode.
 *
 * \author Robert Link
 * \param solnset An initial set of SolutionInfo objects representing all of the markets we will attempt to solve
 * \param period Model time period
 * \return Status code indicating whether the algorithm was successful or not.
 */
SolverComponent::ReturnCode LogBroyden::solve(SolutionInfoSet &solnset, int period) {
  ReturnCode code = SolverComponent::ORIGINAL_STATE;

    // If all markets are solved, then return with success code.
    if( solnset.isAllSolved() ){
        return code = SolverComponent::SUCCESS;
    }

    startMethod();
    if(period != mLastPer) {
        // reset our internal counters
        mPerIter = 0;
        mLastPer = period;
    }

    // Update the solution vector for the correct markets to solve.
    // Need to update solvable status before starting solution (Ignore return code)
    solnset.updateSolvable( mSolutionInfoFilter.get() );

    ILogger& solverLog = ILogger::getLogger( "solver_log" );
    solverLog.setLevel( ILogger::NOTICE );
    solverLog << "Beginning Broyden solution for period " << period
              << "Solving " << solnset.getNumSolvable() << "markets.\n";
    if( mLogPricep ) {
      solverLog << "Log price in effect\n";
    }
    else {
      solverLog << "Linear price in effect\n";
    }

    ILogger& worstMarketLog = ILogger::getLogger( "worst_market_log" );
    worstMarketLog.setLevel( ILogger::DEBUG );
    ILogger& singleLog = ILogger::getLogger( "single_market_log" );
    singleLog.setLevel( ILogger::DEBUG );

    size_t nsolv = solnset.getNumSolvable();
    if( nsolv == 0 ){
      solverLog << "No markets were assigned to this solver.  Exiting." << std::endl;
        return SUCCESS;
    }

    solverLog << "Initial market state:\nmkt    \tprice   \tsupply  \tdemand\n";
    std::vector<SolutionInfo> solvables = solnset.getSolvableSet();
    for(size_t i=0; i<solvables.size(); ++i) {
        solverLog << std::setw( 8 ) << i << "\t"
                  << std::setw( 8 ) << solvables[i].getPrice() << "\t"
                  << std::setw( 8 ) << solvables[i].getSupply() << "\t"
                  << std::setw( 8 ) << solvables[i].getDemand()
                  << "\t\t" << solvables[i].getName() << "\n";
    }

    Timer& solverTimer = TimerRegistry::getInstance().getTimer( TimerRegistry::SOLVER );
    solverTimer.start();

    UBVECTOR x( nsolv ), fx( nsolv );
    int neval = 0;

    // set our initial x from the solutionInfoSet
    std::vector<SolutionInfo> smkts(solnset.getSolvableSet());
    if( mLogPricep ) {
      std::transform(smkts.begin(), smkts.end(), x.begin(), SI2lgprice);
    }
    else {
      std::transform(smkts.begin(), smkts.end(), x.begin(), SI2price);
    }

    // This is the closure that will evaluate the ED function
    LogEDFun F(solnset, world, marketplace, period, mLogPricep);
    // check the assumptions:  narg==nrtn==nsolv
    if(F.narg() != nsolv || F.nrtn() != nsolv) {
      solverLog.setLevel(ILogger::SEVERE);
      solverLog << "size mismatch in logbroyden:  nsolv= " << F.narg()
                << "  nrtn= " << F.nrtn()
                << "  nsolv= " << nsolv
                << std::endl;
      abort();
    }

    // scale the initial guess for use in the solver algorithm
    F.scaleInitInputs( x );

    // Call F( x ), store the result in fx
    F(x,fx);

    solverLog.setLevel(ILogger::DEBUG);
    solverLog << "Initial guess:\n" << x << "\nInitial F( x ):\n" << fx << "\n";
    solnset.printMarketInfo("Broyden-initial", calcCounter->getPeriodCount(), singleLog);

    // Precondition the x values to avoid singular columns in the Jacobian
    solverLog.setLevel(ILogger::DEBUG);
    UBMATRIX J(F.narg(), F.nrtn());
    fdjac(F, x, fx, J, true);

    solverLog << ">>>> Main loop jacobian called.\n";
    int pcfail = jacobian_precondition(x, fx, J, F, &solverLog, mLogPricep);

    if( pcfail ) {
      solverLog.setLevel(ILogger::WARNING);
      solverLog << "Unable to find nonsingular initial guess for one or more markets.  bsolve() will probably fail.\n";
      solverLog.setLevel(ILogger::DEBUG);
    }
    else {
      solverLog << "Revised guess:\n" << x << "\nRevised F( x ):\n" << fx << "\n";
    }
    solnset.printMarketInfo("Broyden-preconditioned", calcCounter->getPeriodCount(), singleLog);
    cSolInfo = &solnset;        // make available for log outputs

    // call the solver
    int bstatus = bsolve(F, x, fx, J, neval);
    mPerIter++;                 // increment the iteration count.  This should produce a visible gap in the trace plots.

    solverTimer.stop();

    solverLog.setLevel(ILogger::NOTICE);
    solverLog << "Broyden solver:  neval= " << neval << "\nResult:  ";
    if(bstatus == 0) {
        solverLog << "Broyden solution success.\n";
        code = SUCCESS;
    }
    else if(bstatus == -1) {
        code = FAILURE_ITER_MAX_REACHED;
        solverLog << "Broyden solution failed: Iteration max reached.\n";
    }
    else if(bstatus == -3) {
        code = FAILURE_ZERO_GRADIENT;
        solverLog << "Broyden solution failed:  Encountered zero gradient in F*F.\n";
    }
    else if(bstatus == -4) {
        code = FAILURE_POOR_PROGRESS;
        solverLog << "Broyden solution failed:  repeated poor progress.\n";
    }
    else if(bstatus > 0) {
        code = FAILURE_SINGULAR_MATRIX;
        int singrow = bstatus-1; // L-U decomp returns row number as 1..N numbering
        solverLog << "Broyden solution failed:  Encountered singular matrix (row " << singrow << ").\n";
        solverLog << smkts[singrow] << std::endl;
    }
    else {
        code = FAILURE_UNKNOWN;
        solverLog << "Broyden solution failed for unknown reason.\n";
    }
    if(!solnset.isAllSolved()) {
        solverLog << "The following markets were not solved:\n";
        solnset.printUnsolved( solverLog );
    }

    solverLog << std::endl;

    // log some final debugging info
    const SolutionInfo* maxred = solnset.getWorstSolutionInfo();
    addIteration(maxred->getName(), maxred->getRelativeED());
    if( mLogPricep ) {
      worstMarketLog << "###Broyden-end-logPrice:  " << *maxred << std::endl;
    }
    else {
      worstMarketLog << "###Broyden-end-linearPrice:  " << *maxred << std::endl;
    }

    solnset.printMarketInfo("Broyden-end ", calcCounter->getPeriodCount(), singleLog);
    singleLog << std::endl;

    return code;
}

int LogBroyden::bsolve(VecFVec<double,double> &F, UBVECTOR &x, UBVECTOR &fx,
                       UBMATRIX & B, int &neval)
{
#if !USE_LAPACK
  using boost::numeric::ublas::permutation_matrix;
  using boost::numeric::ublas::lu_factorize;
  using boost::numeric::ublas::lu_substitute;
#endif
  using boost::numeric::ublas::axpy_prod;
  using boost::numeric::ublas::inner_prod;
  int nrow = B.size1(), ncol = B.size2();
  int ageB = 0;   // number of iterations since the last reset on B
  // svd decomposition elements (note nrow == ncol)
#if USE_LAPACK
  UBMATRIX Usv(nrow,ncol),VTsv(ncol,ncol);
  UBVECTOR Ssv( ncol );
#else
  permutation_matrix<int> p(F.narg()); // permutation vector for pivoting in L-U decomposition
#endif

  UBMATRIX Btmp(nrow, ncol);
  ILogger &solverLog = ILogger::getLogger("solver_log");
  ILogger& worstMarketLog = ILogger::getLogger( "worst_market_log" );
  worstMarketLog.setLevel( ILogger::DEBUG );
  ILogger& singleLog = ILogger::getLogger( "single_market_log" );
  singleLog.setLevel( ILogger::DEBUG );

  // market ids and solvable flag for just the solvable markets
  std::vector<int> mktids_solv;
  cSolInfo->getMarketIDs(mktids_solv,true);
  std::vector<bool> issolvable_solv(mktids_solv.size(), true);
  // market ids and solvable flag for all markets (solvable and unsolvable)
  std::vector<int> mktids_all;
  cSolInfo->getMarketIDs(mktids_all, false);
  std::vector<bool> issolvable_all(mktids_all.size());
  for(unsigned i=0; i<issolvable_solv.size(); ++i) { // solvable markets are at the beginning; set their flag to true
      issolvable_all[i] = true;
  }
  for(unsigned i=issolvable_solv.size(); i<issolvable_all.size(); ++i) {
      // unsolvable markets at the end of the array; set their flag to false
      issolvable_all[i] = false;
  }
  // working space for variables that will be printed for solvable and unsolvable markets
  UBVECTOR rptvec_all(mktids_all.size());

  F(x,fx);

  solverLog.setLevel(ILogger::DEBUG);

  neval += 1 + x.size();        // initial function evaluation + jacobian calculations

  const double FTINY = mFTOL*mFTOL;

  UBVECTOR dx(F.narg());
  UBVECTOR xnew(F.narg());
  UBVECTOR gx(F.narg());
  assert(F.nrtn() == F.narg());
  assert(x.size() == F.narg());
  assert(fx.size() == F.nrtn());
  UBVECTOR jdiag(F.narg());
  // do the asserts manually, since we can't turn on normal assertions
  // (GCAM is riddled with asserts that fail under normal
  // circumstances).
  if(F.nrtn() != F.narg() || x.size() != F.narg() || fx.size() != F.nrtn()) {
    solverLog.setLevel(ILogger::SEVERE);
    solverLog << "size mismatch:  nrtn= " << F.nrtn()
              << "  narg= " << F.narg()
              << "  x.size= " << x.size()
              << "  fx.size= " << fx.size()
              << std::endl;
    abort();
  }

  // We create a functor that computes f( x ) = F( x )*F( x ).  It also
  // stores the value of F that it produces as an intermediate.
  FdotF<double,double> fnorm( F );
  double f0 = inner_prod(fx,fx); // already have a value of F on input, so no need to call fnorm yet
  if(f0 < FTINY) {
    // Guard against F=0 since it can cause a NaN in our solver.  This
    // is a more stringent test than our regular convergence test
    return 0;
  }

  bool lsfail = false;        // flag indicating whether we have had a line-search failure
  for(int iter=0; iter<mMaxIter; ++iter) {
    // log some debug info

    solverLog << "Broyden iter= " << iter << "\tneval= " << neval << "\n";
    solverLog << "Internal iteration count ( mPerIter )= " << mPerIter << "\n";
    cSolInfo->printMarketInfo("Broyden ", calcCounter->getPeriodCount(), singleLog);
    for(int j=0;j<F.narg();++j) {
      // double bjj= B(j,j);
      // jdiag[j] = bjj;
      jdiag[j] = B(j,j);
    }
    static_cast<LogEDFun&>(F).setSlope(jdiag);
    double jdmax=0.0, jdmin=0.0;
    int jdjmax=0, jdjmin=0;
    locate_vector_minmax(jdiag, jdmax, jdmin, jdjmax, jdjmin);
    solverLog << "diag( B ):\n" << jdiag << "\n";
    solverLog << "maxval= " << jdmax << " jmax= " << jdjmax << "  "
              << "minval= " << jdmin << "  jmin= " << jdjmin << "\n";

    axpy_prod(fx,B,gx);         // compute the gradient of F*F (= fx^T * B == B^T * fx)
                                // axpy_prod clears gx on entry, so we don't have to do it.
                                // NB: the order of fx and B in that last call is significant!

    // Check for zero gradient.  This indicates a local minimum in f,
    // from which we are unlikely to escape.  We will need to try
    // again with a different initial guess.  (This should be very
    // uncommon)
    double gmag2 = inner_prod(gx,gx);
    if(gmag2 / (f0+FTINY) < mFTOL) {
      solverLog << "**** ||gx|| = 0.  Returning.\n";
      return -3;
    }

    Btmp = B;                   // save the jacobian approximant
#if USE_LAPACK /* Solve using SVD */
    int ierr = boost::numeric::bindings::lapack::gesvd('O','A','A', // control parameters
                                                       B,           // input matrix
                                                       Ssv,Usv,VTsv); // outputs
    if(ierr>0) {
      // svd failed.  It's not even clear under what circumstances
      // this can happen
      solverLog.setLevel(ILogger::SEVERE);
      solverLog << "****************SVD failed.  This shouldn't happen.  It can't mean anything good.\n";
      return ierr;
    }

    // At this point, U, S, and VT contain the SVD of the original Jacobian
    solverLog.setLevel(ILogger::DEBUG);
    dx = -1.0*fx;
    int nsing = svdInvertSolve(Usv,Ssv,VTsv,dx, solverLog);

    solverLog << "\nIteration " << iter << "\nf0= " << f0
              << "\tnsing= " << nsing
              << "\nx: " << x << "\nF( x ): " << fx << "\ndx: " << dx << "\n";

#else /* No USE_LAPACK.  Solve using L-U decomposition */
    int itrial = 0;
    /* If the L-U decomposition fails the first time around, we will
       invoke the jacobian preconditioner and try again.  If it fails
       a second time, we bail out */
    do {
      for(size_t i=0; i<p.size(); ++i) {
        p[i] = i;
      }
      int sing = lu_factorize(B,p);
      if(sing>0) {
        int fail=1;
        B = Btmp;           // restore Jacobian
        if(itrial == 0) {
            solverLog << "Salvaging Jacobian.\n";
            fail = jacobian_precondition(x, fx, B, F, &solverLog, mLogPricep);
            f0 = inner_prod(fx,fx);

            // log the diagonal of the new jacobian
            for(int j=0; j<F.narg(); ++j) {
                jdiag[j] = B(j,j);
            }
            solverLog << "After jacobian salvage.  diag( B )=\n" << jdiag << "\n";

        }

        if( fail ) {
            solverLog.setLevel(ILogger::WARNING);
            solverLog << "Singular Jacobian:\n" << B << "\n";
            return sing;
        }
      }
      else {
        // L-U decomp was successful.  Continue with the next phase of the algorithm.
        break;
      }
    } while(++itrial < 2);

    // J now holds the L-U decomposition of the Jacobian.  Attempt backsubstitution
    dx = -1.0*fx;
    try {
      lu_substitute(B,p,dx);    // solve dx = J^-1 F
    }
    catch (const boost::numeric::ublas::internal_logic &err) {
      // This error seems to be thrown when the Jacobian is
      // ill-conditioned.  We let it go because often the solver will
      // muddle through to a solution.  If not, then it will
      // eventually stop with a genuinely singular matrix.
    }
    solverLog << "dx: " << dx << "\n";
#endif /* USE_LAPACK */

    // log the proposal step
    solverLog << "Proposal step magnitude dxmag= " << sqrt(inner_prod(dx,dx)) << "\n\n";
    reportVec("dxprop", dx, mktids_solv, issolvable_solv);

    // dx now holds the newton step.  Execute the line search along
    // that direction.
    double fnew;
    int lserr = linesearch(fnorm,x,f0,gx,dx, xnew,fnew, neval, &solverLog);

    if(lserr != 0) {
      // line search failed.  There are a couple of things that could
      // be happening here.

      // 1) B is not a descent direction.  If this is the first
      // failure starting from this x value, try a finite difference
      // jacobian
      if(!lsfail) {
        solverLog << "**Failed line search. Evaluating fdjac\n";
        lsfail = true;
        // call fdjac such that it re-calculates the model at x as linesearch will
        // have left off on some other price vector thus we could have bad state
        // data from which we calculate derivatives
        fdjac(F,x,B);
        neval += x.size();
        ageB = 0;  // reset the age on B

        // Log the diagonal of the new jacobian after the failed line search
        for(int j=0; j<F.narg(); ++j) {
            jdiag[j] = B(j,j);
        }
        solverLog << "New Jacobian: diag( B )=\n" << jdiag << "\n";
        static_cast<LogEDFun&>(F).setSlope(jdiag);

        // start the next iteration *without* updating x
        continue;
      }

      // 2) The descent only continues for a very short distance
      // (roughly TOL*x0).  Maybe we're really close to a solution and
      // generating very tiny dx values.  Make a relaxed convergence
      // test and return if we have a "close enough" solution.
      double msf = f0/fx.size();
      if(msf < mFTOL) {
        // basically, we're letting ourselves converge to the sqrt of
        // our intended tolerance.
        return 0;
      }

      // 3) Neither of the above.  The most likely way for this to
      // happen is if we're close to a discontinuity in (probably
      // several components of) the Jacobian.  Using smoother supply
      // and/or demand functions might help here.

      // We're not close enough to the solution, and we don't have a
      // good descent direction.  There are no good options at this point
      // so kick out and hope that the preconditioner can set us straight.
      solverLog << "linesearch failure\n";
      return -4;
    }
    else {
      // reset the line search fail flag
      lsfail = false;
    }

    UBVECTOR xstep(xnew-x);    // step in x eventually taken
    double lambda = fabs(dx[0]) > 0.0 ? xstep[0] / dx[0] : 0.0;
    solverLog << "################Return from linesearch\nfold= " << f0 << "\tfnew= " << fnew
              << "\tlambda= " << lambda << "\n";

    UBVECTOR fxnew(fx.size());
    fnorm.lastF( fxnew );            // get the last value of big-F
    solverLog << "\nxnew: " << xnew << "\nfxnew: " << fxnew << "\n";
    UBVECTOR fxstep(fxnew -fx); // change in F( x ).  We will need this for the secant update

    // log the worst market info
    const SolutionInfo* maxred = cSolInfo->getWorstSolutionInfo();
    addIteration(maxred->getName(), maxred->getRelativeED());
    if( mLogPricep ) {
      worstMarketLog << "Broyden-logPrice:  " << *maxred << "\n";
    }
    else {
      worstMarketLog << "Broyden-linearPrice:  " << *maxred << "\n";
    }

    // test for convergence
    double maxval = fabs(fxnew[0]);
    double imaxval = 0;
    for(size_t i=1; i<fxnew.size(); ++i) {
      double val = fabs(fxnew[i]);
      if(val > maxval) {
        maxval = val;
        imaxval = i;
      }
    }

    solverLog << "Convergence test maxval: " << maxval << "  imaxval= " << imaxval << "\n";
    solverLog << "\tx[i]= " << xnew[imaxval] << "  dx[i]= " << dx[imaxval] << "  xstep[i]= "
              << xstep[imaxval] << "\n";
    if(maxval <= mFTOL) {
      solverLog << "Solution successful.\n";
      x = xnew;
      fx = fxnew;
      return 0;                 // SUCCESS
    }

#if 0
    // secondary convergence test based on an estimated change in the
    // price vector.  Only test this if we have a "fresh" jacobian
    if(ageB == 0)  {
        maxval = fabs(fxnew[0]) / (util::getSmallNumber() + fabs(Btmp(0,0)));
        imaxval = 0;
        for(size_t i=1; i<fxnew.size(); ++i) {
            double val = fabs(fxnew[i]) / (util::getSmallNumber() + fabs(Btmp(i,i)));
            if(val > maxval) {
                maxval = val;
                imaxval = i;
            }
        }
        solverLog << "Secondary convergence test maxval:  " << maxval << "  imaxval= " << imaxval << "\n";
        if(maxval <= mFTOL) {   // XXX should put an x-tolerance in here
            solverLog << "Solved using secondary criterion.\n";
            x = xnew;
            fx = fxnew;
            return 0;
        }
    }
#endif

    // update B for next iteration
    double fratio_cutoff = 1.0 - 1.0/nrow;
    if(fnew/f0 < fratio_cutoff) { // making adequate progress with the Broyden formula
      double dx2 = inner_prod(xstep,xstep);
      UBVECTOR Bdx(F.nrtn());
      B = Btmp;
      fxstep -= axpy_prod(B, xstep, Bdx);
      fxstep /= dx2;
      B += outer_prod(fxstep, xstep);
      ageB++;                // increment the age of B
    }
    else {
      // Progress using the Broyden formula is anemic.  This usually
      // happens near discontinuities in the Jacobian matrix.  If B is
      // old, try a finite-difference jacobian to get us back on track.
      if(ageB > 0) {
        solverLog << "Insufficient progress with Broyden formula.  Resetting the Jacobian.\n(f0= " << f0 << ", fnew= " << fnew << ")\n";
        // just in case call fdjac such that it re-calculates the model at xnew
        // otherwise we could have bad state data from which we calculate derivatives
        fdjac(F,xnew,B);
        neval += x.size();
        ageB = 0;

        // Log the results of the Jacobian reset
        for(int j=0; j<F.narg(); ++j) {
            jdiag[j] = B(j,j);
        }

        solverLog << "New Jacobian:  diag( B )=\n" << jdiag << "\n";
        static_cast<LogEDFun&>(F).setSlope(jdiag);

      }
      else {
        // just did a reset, and it didn't help us.  Probably we've
        // got a very ill-behaved value in one of the variables.  Kick
        // it out and see if the bracketing routine can fix it.
        solverLog << "Repeated poor progress in Broyden solver.  Returning.\n";
        return -4;
      }
    }

    // log the data trace before we do the update
    reportVec("x", xnew, mktids_solv, issolvable_solv);
    reportVec("fx", fxnew, mktids_solv, issolvable_solv);
    reportVec("deltax", xnew-x, mktids_solv, issolvable_solv);    // xstep may have been modified above
    reportVec("deltafx", fxnew-fx, mktids_solv, issolvable_solv); // fxstep definitely modified above
    reportVec("diagB", jdiag, mktids_solv, issolvable_solv);
    reportPSD(rptvec_all, mktids_all, issolvable_all);                // report price, supply, and demand.
    mPerIter++;

    // update x, fx, f0 for next iteration
    f0 = fnew;
    x  = xnew;
    fx = fxnew;

  }

  // if we get here, then we didn't converge in the number of
  // iterations allowed us.  Return an error code
  solverLog << "\n****************Maximum solver iterations exceeded.\nlastx: " << x
            << "\nlastF: " << fx << "\n";
  return -1;
}

/*! \brief Write a vector into the solver data log
 *
 *  \details We write the solver data log in "long" format; i.e., with
 *           one completely-specified data item per line.  This allows
 *           us to simply skip variables that we don't have data for
 *           (vs. filling in NaN values), and we don't have to commit
 *           to a fixed number of variables; e.g., we could have more
 *           variables for price, supply, and demand.
 *
 *           The output columns are:
 *           period, iteration, variable name, market id, solvable (T/F), value
 *
 */
void LogBroyden::reportVec(const std::string &aname, const UBVECTOR &av, const std::vector<int> &amktids,
                           const std::vector<bool> &aissolvable)
{
    ILogger &datalog = ILogger::getLogger("solver-data-log");
    datalog.setLevel(ILogger::DEBUG);
    // Skip preparing the output if it won't be printed
    if(!datalog.wouldPrint(ILogger::DEBUG)) {
        return;
    }

    for(unsigned int i=0; i<av.size(); ++i) {
        datalog << mLastPer <<  " , " << mPerIter
                << ",\"" << aname << "\""
                << ", " << amktids[i]
                << (aissolvable[i] ? ", T" : ", F")
                << ", " << av[i] << "\n";
    }
}

void LogBroyden::reportPSD(UBVECTOR &arptvec, const std::vector<int> &amktids, const std::vector<bool> &aissolvable)
{
    // Skip preparing the log output if we're not going to print it anyhow
    if(!ILogger::getLogger("solver-data-log").wouldPrint(ILogger::DEBUG)) {
        return;
    }

    if(!cSolInfo) {
        ILogger &solverlog = ILogger::getLogger("solver_log");
        ILogger::WarningLevel olvl = solverlog.setLevel(ILogger::ERROR);
        solverlog << "cSolInfo is not set.  Data log will not include price, supply, or demand." << std::endl;
        solverlog.setLevel( olvl );
        return;
    }

    std::vector<SolutionInfo> solvable = cSolInfo->getSolvableSet();
    std::vector<SolutionInfo> unsolvable = cSolInfo->getUnsolvableSet();

    unsigned int i;
    unsigned int j;
    // log prices
    for(i=0; i<solvable.size(); ++i) {
        arptvec[i] = solvable[i].getPrice();
    }
    for(i=0,j=solvable.size(); i<unsolvable.size(); ++i,++j) {
        arptvec[j] = unsolvable[i].getPrice();
    }

    reportVec("price", arptvec, amktids, aissolvable);

    // log supply
    for(i=0; i<solvable.size(); ++i) {
        arptvec[i] = solvable[i].getSupply();
    }
    for(i=0,j=solvable.size(); i<unsolvable.size(); ++i,++j) {
        arptvec[j] = unsolvable[i].getSupply();
    }
    reportVec("supply", arptvec, amktids, aissolvable);

    // log demand
    for(i=0; i<solvable.size(); ++i) {
        arptvec[i] = solvable[i].getDemand();
    }
    for(i=0,j=solvable.size(); i<unsolvable.size(); ++i,++j) {
        arptvec[j] = unsolvable[i].getDemand();
    }
    reportVec("demand", arptvec, amktids, aissolvable);

}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2011 - 2013   MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_FORWARD_ALIGNED_ARRAY_HPP_INCLUDED
#define BOOST_SIMD_FORWARD_ALIGNED_ARRAY_HPP_INCLUDED

#include <boost/simd/sdk/config/arch.hpp>
#include <boost/simd/meta/prev_power_of_2.hpp>

#if !defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  template< class T
          , std::size_t N
          , std::size_t Align = (BOOST_SIMD_ARCH_ALIGNMENT > (N*sizeof(T)))
                              ? meta::prev_power_of_2_c< N*sizeof(T) >::value
                              : BOOST_SIMD_ARCH_ALIGNMENT
          >
  struct aligned_array;
} }

#endif

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2020 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_PIECE_BORDER_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_PIECE_BORDER_HPP

#include <boost/array.hpp>
#include <boost/core/addressof.hpp>

#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/config.hpp>

#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/algorithms/comparable_distance.hpp>
#include <boost/geometry/algorithms/equals.hpp>
#include <boost/geometry/algorithms/expand.hpp>
#include <boost/geometry/algorithms/detail/buffer/buffer_box.hpp>
#include <boost/geometry/algorithms/detail/buffer/buffer_policies.hpp>
#include <boost/geometry/algorithms/detail/expand_by_epsilon.hpp>
#include <boost/geometry/strategies/cartesian/turn_in_ring_winding.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/segment.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{
template <typename It, typename T, typename Compare>
inline bool get_range_around(It begin, It end, T const& value, Compare const& compare, It& lower, It& upper)
{
    lower = end;
    upper = end;

    // Get first element not smaller than value
    if (begin == end)
    {
        return false;
    }
    if (compare(value, *begin))
    {
        // The value is smaller than the first item, therefore not in range
        return false;
    }
    // *(begin + std::distance(begin, end) - 1))
    if (compare(*(end - 1), value))
    {
        // The last item is larger than the value, therefore not in range
        return false;
    }

    // Assign the iterators.
    // lower >= begin and lower < end
    // upper > lower and upper <= end
    // lower_bound points to first element NOT LESS than value - but because
    // we want the first value LESS than value, we decrease it
    lower = std::lower_bound(begin, end, value, compare);
    // upper_bound points to first element of which value is LESS
    upper = std::upper_bound(begin, end, value, compare);

    if (lower != begin)
    {
        --lower;
    }
    if (upper != end)
    {
        ++upper;
    }
    return true;
}

}

namespace detail { namespace buffer
{

//! Contains the border of the piece, consisting of 4 parts:
//! 1: the part of the offsetted ring (referenced, not copied)
//! 2: the part of the original (one or two points)
//! 3: the left part (from original to offsetted)
//! 4: the right part (from offsetted to original)
//! Besides that, it contains some properties of the piece(border);
//!   - convexity
//!   - envelope
//!   - monotonicity of the offsetted ring
//!   - min/max radius of a point buffer
//!   - if it is a "reversed" piece (linear features with partly negative buffers)
template <typename Ring, typename Point>
struct piece_border
{
    typedef typename geometry::coordinate_type<Point>::type coordinate_type;
    typedef typename default_comparable_distance_result<Point>::type radius_type;
    typedef typename geometry::strategy::buffer::turn_in_ring_winding<coordinate_type>::state_type state_type;

    bool m_reversed;

    // Points from the offsetted ring. They are not copied, this structure
    // refers to those points
    Ring const* m_ring;
    std::size_t m_begin;
    std::size_t m_end;

    // Points from the original (one or two, depending on piece shape)
    // Note, if there are 2 points, they are REVERSED w.r.t. the original
    // Therefore here we can walk in its order.
    boost::array<Point, 2> m_originals;
    std::size_t m_original_size;

    geometry::model::box<Point> m_envelope;
    bool m_has_envelope;

    // True if piece is determined as "convex"
    bool m_is_convex;

    // True if offsetted part is monotonically changing in x-direction
    bool m_is_monotonic_increasing;
    bool m_is_monotonic_decreasing;

    radius_type m_min_comparable_radius;
    radius_type m_max_comparable_radius;

    piece_border()
        : m_reversed(false)
        , m_ring(NULL)
        , m_begin(0)
        , m_end(0)
        , m_original_size(0)
        , m_has_envelope(false)
        , m_is_convex(false)
        , m_is_monotonic_increasing(false)
        , m_is_monotonic_decreasing(false)
        , m_min_comparable_radius(0)
        , m_max_comparable_radius(0)
    {
    }

    // Only used for debugging (SVG)
    Ring get_full_ring() const
    {
        Ring result;
        if (ring_or_original_empty())
        {
            return result;
        }
        std::copy(m_ring->begin() + m_begin,
                  m_ring->begin() + m_end,
                  std::back_inserter(result));
        std::copy(m_originals.begin(),
                  m_originals.begin() + m_original_size,
                  std::back_inserter(result));
        // Add the closing point
        result.push_back(*(m_ring->begin() + m_begin));

        return result;
    }

    void get_properties_of_border(bool is_point_buffer, Point const& center)
    {
        m_has_envelope = calculate_envelope(m_envelope);
        if (m_has_envelope)
        {
            // Take roundings into account, enlarge box
            geometry::detail::expand_by_epsilon(m_envelope);
        }
        if (! ring_or_original_empty() && is_point_buffer)
        {
            // Determine min/max radius
            calculate_radii(center, m_ring->begin() + m_begin, m_ring->begin() + m_end);
        }
    }

    template <typename SideStrategy>
    void get_properties_of_offsetted_ring_part(SideStrategy const& strategy)
    {
        if (! ring_or_original_empty())
        {
            m_is_convex = is_convex(strategy);
            check_monotonicity(m_ring->begin() + m_begin, m_ring->begin() + m_end);
        }
    }

    void set_offsetted(Ring const& ring, std::size_t begin, std::size_t end)
    {
        BOOST_GEOMETRY_ASSERT(begin <= end);
        BOOST_GEOMETRY_ASSERT(begin < boost::size(ring));
        BOOST_GEOMETRY_ASSERT(end <= boost::size(ring));

        m_ring = boost::addressof(ring);
        m_begin = begin;
        m_end = end;
    }

    void add_original_point(Point const& point)
    {
        BOOST_GEOMETRY_ASSERT(m_original_size < 2);
        m_originals[m_original_size++] = point;
    }

    template <typename Box>
    bool calculate_envelope(Box& envelope) const
    {
        geometry::assign_inverse(envelope);
        if (ring_or_original_empty())
        {
            return false;
        }
        expand_envelope(envelope, m_ring->begin() + m_begin, m_ring->begin() + m_end);
        expand_envelope(envelope, m_originals.begin(), m_originals.begin() + m_original_size);
        return true;
    }

    // Whatever the return value, the state should be checked.
    template <typename TurnPoint, typename State>
    bool point_on_piece(TurnPoint const& point,
                        bool one_sided, bool is_linear_end_point,
                        State& state) const
    {
        if (ring_or_original_empty())
        {
            return false;
        }

        // Walk over the different parts of the ring, in clockwise order
        // For performance reasons: start with the helper part (one segment)
        // then the original part (one segment, if any), then the other helper
        // part (one segment), and only then the offsetted part
        // (probably more segments, check monotonicity)
        geometry::strategy::buffer::turn_in_ring_winding<coordinate_type> tir;

        Point const offsetted_front = *(m_ring->begin() + m_begin);
        Point const offsetted_back = *(m_ring->begin() + m_end - 1);

        // For onesided buffers, or turns colocated with linear end points,
        // the place on the ring is changed to offsetted (because of colocation)
        geometry::strategy::buffer::place_on_ring_type const por_original
            = adapted_place_on_ring(geometry::strategy::buffer::place_on_ring_original,
                                    one_sided, is_linear_end_point);
        geometry::strategy::buffer::place_on_ring_type const por_from_offsetted
            = adapted_place_on_ring(geometry::strategy::buffer::place_on_ring_from_offsetted,
                                    one_sided, is_linear_end_point);
        geometry::strategy::buffer::place_on_ring_type const por_to_offsetted
            = adapted_place_on_ring(geometry::strategy::buffer::place_on_ring_to_offsetted,
                                    one_sided, is_linear_end_point);

        bool continue_processing = true;
        if (m_original_size == 1)
        {
            // One point. Walk from last offsetted to point, and from point to first offsetted
            continue_processing = step(point, offsetted_back, m_originals[0], tir, por_from_offsetted, state)
                && step(point, m_originals[0], offsetted_front, tir, por_to_offsetted, state);
        }
        else if (m_original_size == 2)
        {
            // Two original points. Walk from last offsetted point to first original point,
            // then along original, then from second oginal to first offsetted point
            continue_processing = step(point, offsetted_back, m_originals[0], tir, por_from_offsetted, state)
                    && step(point, m_originals[0], m_originals[1], tir, por_original, state)
                    && step(point, m_originals[1], offsetted_front, tir, por_to_offsetted, state);
        }

        if (continue_processing)
        {
            // Check the offsetted ring (in rounded joins, these might be
            // several segments)
            walk_offsetted(point, m_ring->begin() + m_begin, m_ring->begin() + m_end,
                           tir, state);
        }

        return true;
    }

    //! Returns true if empty (no ring, or no points, or no original)
    bool ring_or_original_empty() const
    {
        return m_ring == NULL || m_begin >= m_end || m_original_size == 0;
    }

private :

    static geometry::strategy::buffer::place_on_ring_type
        adapted_place_on_ring(geometry::strategy::buffer::place_on_ring_type target,
                              bool one_sided, bool is_linear_end_point)
    {
        return one_sided || is_linear_end_point
               ? geometry::strategy::buffer::place_on_ring_offsetted
               : target;
    }

    template <typename TurnPoint, typename Iterator, typename Strategy, typename State>
    bool walk_offsetted(TurnPoint const& point, Iterator begin, Iterator end, Strategy const & strategy, State& state) const
    {
        Iterator it = begin;
        Iterator beyond = end;

        // Move iterators if the offsetted ring is monotonic increasing or decreasing
        if (m_is_monotonic_increasing)
        {
            if (! get_range_around(begin, end, point, geometry::less<Point, 0>(), it, beyond))
            {
                return true;
            }
        }
        else if (m_is_monotonic_decreasing)
        {
            if (! get_range_around(begin, end, point, geometry::greater<Point, 0>(), it, beyond))
            {
                return true;
            }
        }

        for (Iterator previous = it++ ; it != beyond ; ++previous, ++it )
        {
            if (! step(point, *previous, *it, strategy,
                 geometry::strategy::buffer::place_on_ring_offsetted, state))
            {
                return false;
            }
        }
        return true;
    }

    template <typename TurnPoint, typename Strategy, typename State>
    bool step(TurnPoint const& point, Point const& p1, Point const& p2, Strategy const & strategy,
              geometry::strategy::buffer::place_on_ring_type place_on_ring, State& state) const
    {
        // A step between original/offsetted ring is always convex
        // (unless the join strategy generates points left of it -
        //  future: convexity might be added to the buffer-join-strategy)
        // Therefore, if the state count > 0, it means the point is left of it,
        // and because it is convex, we can stop

        typedef typename geometry::coordinate_type<Point>::type coordinate_type;
        typedef geometry::detail::distance_measure<coordinate_type> dm_type;
        dm_type const dm = geometry::detail::get_distance_measure(point, p1, p2);
        if (m_is_convex && dm.measure > 0)
        {
            // The point is left of this segment of a convex piece
            state.m_count = 0;
            return false;
        }
        // Call strategy, and if it is on the border, return false
        // to stop further processing.
        return strategy.apply(point, p1, p2, dm, place_on_ring, state);
    }

    template <typename It, typename Box>
    void expand_envelope(Box& envelope, It begin, It end) const
    {
        typedef typename strategy::expand::services::default_strategy
            <
                point_tag, typename cs_tag<Box>::type
            >::type expand_strategy_type;

        for (It it = begin; it != end; ++it)
        {
            geometry::expand(envelope, *it, expand_strategy_type());
        }
    }

    template <typename SideStrategy>
    bool is_convex(SideStrategy const& strategy) const
    {
        if (ring_or_original_empty())
        {
            // Convexity is undetermined, and for this case it does not matter,
            // because it is only used for optimization in point_on_piece,
            // but that is not called if the piece border is not valid
            return false;
        }

        if (m_end - m_begin <= 2)
        {
            // The offsetted ring part of this piece has only two points.
            // If this is true, and the original ring part has only one point,
            // a triangle and it is convex. If the original ring part has two
            // points, it is a rectangle and theoretically could be concave,
            // but because of the way the buffer is generated, that is never
            // the case.
            return true;
        }

        // The offsetted ring part of thie piece has at least three points
        // (this is often the case in a piece marked as "join")

        // We can assume all points of the offset ring are different, and also
        // that all points on the original are different, and that the offsetted
        // ring is different from the original(s)
        Point const offsetted_front = *(m_ring->begin() + m_begin);
        Point const offsetted_second = *(m_ring->begin() + m_begin + 1);

        // These two points will be reassigned in every is_convex call
        Point previous = offsetted_front;
        Point current = offsetted_second;

        // Verify the offsetted range (from the second point on), the original,
        // and loop through the first two points of the offsetted range
        bool const result = is_convex(previous, current, m_ring->begin() + m_begin + 2, m_ring->begin() + m_end, strategy)
            && is_convex(previous, current, m_originals.begin(), m_originals.begin() + m_original_size, strategy)
            && is_convex(previous, current, offsetted_front, strategy)
            && is_convex(previous, current, offsetted_second, strategy);

        return result;
    }

    template <typename It, typename SideStrategy>
    bool is_convex(Point& previous, Point& current, It begin, It end, SideStrategy const& strategy) const
    {
        for (It it = begin; it != end; ++it)
        {
            if (! is_convex(previous, current, *it, strategy))
            {
                return false;
            }
        }
        return true;
    }

    template <typename SideStrategy>
    bool is_convex(Point& previous, Point& current, Point const& next, SideStrategy const& strategy) const
    {
        typename SideStrategy::equals_point_point_strategy_type const
            eq_pp_strategy = strategy.get_equals_point_point_strategy();

        int const side = strategy.apply(previous, current, next);
        if (side == 1)
        {
            // Next is on the left side of clockwise ring: piece is not convex
            return false;
        }
        if (! equals::equals_point_point(current, next, eq_pp_strategy))
        {
            previous = current;
            current = next;
        }
        return true;
    }

    template <int Direction>
    inline void step_for_monotonicity(Point const& current, Point const& next)
    {
        if (geometry::get<Direction>(current) >= geometry::get<Direction>(next))
        {
            m_is_monotonic_increasing = false;
        }
        if (geometry::get<Direction>(current) <= geometry::get<Direction>(next))
        {
            m_is_monotonic_decreasing = false;
        }
    }

    template <typename It>
    void check_monotonicity(It begin, It end)
    {
        m_is_monotonic_increasing = true;
        m_is_monotonic_decreasing = true;

        if (begin == end || begin + 1 == end)
        {
            return;
        }

        It it = begin;
        for (It previous = it++; it != end; ++previous, ++it)
        {
            step_for_monotonicity<0>(*previous, *it);
        }
    }

    template <typename It>
    inline void calculate_radii(Point const& center, It begin, It end)
    {
        typedef geometry::model::referring_segment<Point const> segment_type;

        bool first = true;

        // An offsetted point-buffer ring around a point is supposed to be closed,
        // therefore walking from start to end is fine.
        It it = begin;
        for (It previous = it++; it != end; ++previous, ++it)
        {
            segment_type const s(*previous, *it);
            radius_type const d = geometry::comparable_distance(center, s);

            if (first || d < m_min_comparable_radius)
            {
                m_min_comparable_radius = d;
            }
            if (first || d > m_max_comparable_radius)
            {
                m_max_comparable_radius = d;
            }
            first = false;
        }
    }

};

}} // namespace detail::buffer
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_PIECE_BORDER_HPP


#ifdef USE_CAFFE

#include <boost/filesystem.hpp>
#include <boost/make_shared.hpp>
#include <fstream>
#include <glog/logging.h>
#include <opencv2/opencv.hpp>

#include "nexus/backend/caffe_densecap_model.h"
#include "nexus/common/image.h"
#include "nexus/common/util.h"
// Caffe headers
#include "caffe/layers/slice_layer.hpp"
#include "caffe/util/nms.hpp"

namespace nexus {
namespace backend {

namespace fs = boost::filesystem;

CaffeDenseCapModel::CaffeDenseCapModel(int gpu_id,
                                       const ModelInstanceConfig& config) :
    ModelInstance(gpu_id, config) {
  CHECK(model_info_["feature_prototxt"]) << "Missing feature_prototxt in " <<
      "the config";
  CHECK(model_info_["rnn_prototxt"]) << "Missing rnn_prototxt in the config";
  CHECK(model_info_["embed_prototxt"]) << "Missing embed_prototxt in " <<
      "the config";
  CHECK(model_info_["model_file"]) << "Missing model_file in the config";
  CHECK(model_info_["vocab_file"]) << "Missing vocab_file in the config";
  CHECK(model_info_["mean_value"]) << "Missing mean_value in the config";
  CHECK_EQ(model_info_["mean_value"].size(), 3);
  // load config
  max_boxes_ = model_info_["max_boxes"].as<int>();
  max_timestep_ = model_info_["max_timestep"].as<int>();
  nms_threshold_ = model_info_["nms_threshold"].as<float>();
  score_threshold_ = model_info_["score_threshold"].as<float>();
  for (uint i = 0; i < model_info_["mean_value"].size(); ++i) {
    mean_values_.push_back(model_info_["mean_value"][i].as<float>());
  }
  for (uint i = 0; i < model_info_["bbox_mean"].size(); ++i) {
    bbox_mean_.push_back(model_info_["bbox_mean"][i].as<float>());
  }
  for (uint i = 0; i < model_info_["bbox_stds"].size(); ++i) {
    bbox_stds_.push_back(model_info_["bbox_stds"][i].as<float>());
  }
  // init gpu device
  caffe::Caffe::SetDevice(gpu_id);
  caffe::Caffe::set_mode(caffe::Caffe::GPU);
  // load caffe model
  fs::path model_dir = fs::path(model_info_["model_dir"].as<std::string>());
  fs::path feature_prototxt = model_dir / model_info_["feature_prototxt"].
                              as<std::string>();
  fs::path rnn_prototxt = model_dir / model_info_["rnn_prototxt"].
                          as<std::string>();
  fs::path embed_prototxt = model_dir / model_info_["embed_prototxt"].
                            as<std::string>();
  fs::path model_file = model_dir / model_info_["model_file"].as<std::string>();
  feature_net_.reset(new caffe::ServeNet<float>(
      feature_prototxt.string(), max_batch_));
  rnn_net_.reset(new caffe::ServeNet<float>(
      rnn_prototxt.string(), max_batch_ * max_boxes_, 1));
  embed_net_.reset(new caffe::ServeNet<float>(
      embed_prototxt.string(), max_batch_ * max_boxes_, 1));
  caffe::NetParameter caffemodel;
  caffe::ReadNetParamsFromBinaryFileOrDie(model_file.string(), &caffemodel);
  feature_net_->CopyTrainedLayersFrom(caffemodel);
  rnn_net_->CopyTrainedLayersFrom(caffemodel);
  embed_net_->CopyTrainedLayersFrom(caffemodel);
  // set up input and output size
  image_height_ = model_session_.image_height();
  image_width_ = model_session_.image_width();
  /*
  int target_size = model_info_["target_size"].as<int>();
  int max_size = model_info_["max_size"].as<int>();
  float aspect_ratio = model_info_["aspect_ratio"].as<float>();
  if (aspect_ratio > 1) {
    input_width_ = (int) round(
        std::min((float) max_size, target_size * aspect_ratio));
    input_height_ = (int) round(input_width_ / aspect_ratio);
  } else {
    input_height_ = (int) round(
        std::min((float) max_size, target_size / aspect_ratio));
    input_width_ = (int) round(input_height_ * aspect_ratio);
  }*/
  LOG(INFO) << "input shape: " << image_height_ << " x " << image_width_;

  input_shape_.set_dims({max_batch_, 3, image_height_, image_width_});
  input_size_ = input_shape_.NumElements(1);
  feature_net_input_idx_ = feature_net_->input_blob_indices()[0];
  // Reshape the input blob and feature_net according to our input size
  feature_net_->input_blobs()[0]->Reshape(input_shape_.dims());
  feature_net_->Reshape();
  // load vocabulary
  fs::path vocab_file = model_dir / model_info_["vocab_file"].as<std::string>();
  LoadVocabulary(vocab_file.string());
  // set helper buffer
  multiplier_.reset(new caffe::Blob<float>({max_boxes_}));
  caffe::caffe_gpu_set(max_boxes_, (float) 1., multiplier_->mutable_gpu_data());
  best_words_.resize(max_batch_ * max_boxes_);

  output_shapes_.emplace("rois", Shape({max_batch_, max_boxes_, 5}));
  output_shapes_.emplace("bbox_offsets", Shape({max_batch_, max_boxes_, 4}));
  output_shapes_.emplace("captions",
                        Shape({max_batch_, max_boxes_, max_timestep_}));
  output_shapes_.emplace("scores", Shape({max_batch_, max_boxes_, 2}));
}

Shape CaffeDenseCapModel::InputShape() {
  return input_shape_;
}

std::unordered_map<std::string, Shape> CaffeDenseCapModel::OutputShapes() {
  return output_shapes_;
}

ArrayPtr CaffeDenseCapModel::CreateInputGpuArray() {
  boost::shared_ptr<caffe::Blob<float> > blob;
  if (input_blobs_.empty()) {
    blob = feature_net_->blobs()[feature_net_input_idx_];
  } else {
    blob = boost::make_shared<caffe::Blob<float> >(input_shape_.dims());
  }
  size_t nfloats = max_batch_ * input_size_;
  auto buf = std::make_shared<Buffer>(blob->mutable_gpu_data(),
                                      nfloats * sizeof(float),
                                      gpu_device_);
  auto arr = std::make_shared<Array>(DT_FLOAT, nfloats, buf);
  arr->set_tag(input_blobs_.size());
  input_blobs_.push_back(blob);
  return arr;
}

std::unordered_map<std::string, ArrayPtr> CaffeDenseCapModel::GetOutputGpuArrays() {
  // Currently doesn't support in-place output in GPU memory
  return {};
}

void CaffeDenseCapModel::Preprocess(std::shared_ptr<Task> task) {
  const auto& query = task->query;
  const auto& input_data = query.input();
  if (input_data.data_type() != DT_IMAGE) {
    task->result.set_status(INPUT_TYPE_INCORRECT);
    task->result.set_error_message("Input type incorrect: " +
                                   DataType_Name(input_data.data_type()));
    return;
  }
  cv::Mat cv_img_bgr = DecodeImage(input_data.image(), CO_BGR);
  cv::Mat img;
  cv_img_bgr.convertTo(img, CV_32FC3);
  for (cv::Point3_<float>& p : cv::Mat_<cv::Point3_<float> >(img)) {
    p.x -= mean_values_[0];
    p.y -= mean_values_[1];
    p.z -= mean_values_[2];
  }
  int origin_height = cv_img_bgr.rows;
  int origin_width = cv_img_bgr.cols;
  cv::Mat resized;
  cv::resize(img, resized, cv::Size(image_width_, image_height_));
  float scale_h = float(image_height_) / origin_height;
  float scale_w = float(image_width_) / origin_width;
  // set the attributes
  task->attrs["im_height"] = origin_height;
  task->attrs["im_width"] = origin_width;
  task->attrs["scale_h"] = scale_h;
  task->attrs["scale_w"] = scale_w;
  // transpose the image
  const float* im_data = (const float*) resized.data;
  auto in_arr = std::make_shared<Array>(DT_FLOAT, input_size_, cpu_device_);
  float* input = in_arr->Data<float>();
  for (int h = 0; h < image_height_; ++h) {
    for (int w = 0; w < image_width_; ++w) {
      for (int c = 0; c < 3; ++c) {
        int idx = (c * image_height_ + h) * image_width_ + w;
        input[idx] = im_data[(h * image_width_ + w) * 3 + c];
      }
    }
  }
  task->AppendInput(in_arr);
}

void CaffeDenseCapModel::Forward(std::shared_ptr<BatchTask> batch_task) {
  // Get all output arrays
  auto rois_arr = batch_task->GetOutputArray("rois");
  auto bbox_offsets_arr = batch_task->GetOutputArray("bbox_offsets");
  auto captions_arr = batch_task->GetOutputArray("captions_arr");
  auto scores_arr = batch_task->GetOutputArray("scores");

  auto t1 = std::chrono::high_resolution_clock::now();
  // Prepare image blob
  auto blob = input_blobs_[batch_task->GetInputArray()->tag()];
  int batch = batch_task->batch_size();
  std::vector<int> input_shape = input_shape_.dims();
  input_shape[0] = batch;
  blob->Reshape(input_shape);
  feature_net_->set_blob(feature_net_input_idx_, blob);

  // Prepare im_info blob
  auto im_info_blob = feature_net_->input_blobs()[1];
  float* im_info = im_info_blob->mutable_cpu_data();
  im_info[0] = image_height_;  // input image height
  im_info[1] = image_width_;  // input image width
  im_info[2] = batch_task->inputs()[0]->task->attrs["scale_h"].as<float>();

  // set the slice points
  auto split_fc7_layer = dynamic_cast<caffe::SliceLayer<float>*>(
      feature_net_->layer_by_name("split_fc7").get());
  auto split_fc8_layer = dynamic_cast<caffe::SliceLayer<float>*>(
      feature_net_->layer_by_name("split_fc8").get());
  split_fc7_layer->SetSlicePoints({batch});
  split_fc8_layer->SetSlicePoints({batch});
  // forward feature_net_
  feature_net_->Forward();

  // get outputs from feature_net_
  auto num_proposals_blob = feature_net_->blob_by_name("num_proposals");
  auto rois_blob = feature_net_->blob_by_name("rois");
  auto scores_blob = feature_net_->blob_by_name("cls_probs");
  int total_proposals = scores_blob->shape(0);

  // Copy outputs of feature_net from GPU to CPU
  float* num_proposals = new float[batch];
  float* rois = rois_arr->Data<float>();
  float* scores = scores_arr->Data<float>();
  Memcpy(num_proposals, cpu_device_, num_proposals_blob->gpu_data(),
         gpu_device_, batch * sizeof(float));
  // add offset batch is because [0 - batch-1] rois are global rois
  Memcpy(rois, rois_arr->device(),
         rois_blob->gpu_data() + rois_blob->offset(batch), gpu_device_,
         total_proposals * 5 * sizeof(float));
  Memcpy(scores, scores_arr->device(), scores_blob->gpu_data(), gpu_device_,
         total_proposals * 2 * sizeof(float));
  auto t2 = std::chrono::high_resolution_clock::now();
  //LOG(INFO) << *num_proposals;
  //LOG(INFO) << total_proposals;

  // Get all blobs needed for RNN
  // outputs from feature_net_
  auto _global_features_blob = feature_net_->blob_by_name("global_features");
  auto region_features_blob = feature_net_->blob_by_name("region_features");
  // inputs to rnn_net_
  auto global_features_blob = rnn_net_->blob_by_name("global_features");
  auto cont_sentence_blob = rnn_net_->blob_by_name("cont_sentence");
  // outputs from rnn_net_
  auto word_probs_blob = rnn_net_->blob_by_name("probs");
  auto bbox_pred_blob = rnn_net_->blob_by_name("bbox_pred");
  // input to embed_net_
  auto input_sentence_blob = embed_net_->input_blobs()[0];
  // output from embed_net_
  auto embedded_sentence_blob = embed_net_->blob_by_name("embedded_sentence");
  int nfeat = _global_features_blob->shape(2);
  int nwords = word_probs_blob->shape(2);

  // prepare inputs to rnn_net_
  global_features_blob->Reshape({global_features_blob->shape(0),
          total_proposals, global_features_blob->shape(2)});
  int offset = 0;
  // broadcase global features to same dimension as region features
  for (int i = 0; i < batch; ++i) {
    caffe::caffe_gpu_gemm(
        CblasNoTrans, CblasNoTrans, num_proposals[i], nfeat, 1,
        (float)1., multiplier_->gpu_data(), _global_features_blob->gpu_data(),
        (float)0., global_features_blob->mutable_gpu_data() + offset);
    offset += int(num_proposals[i]) * nfeat;
  }
  rnn_net_->set_blob("input_features", region_features_blob);
  cont_sentence_blob->Reshape({1, total_proposals});
  // init cont_sentence with 0 first
  caffe::caffe_gpu_set(total_proposals, (float) 0.,
                       cont_sentence_blob->mutable_gpu_data());

  // pass the image features throught rnn_net_ first
  rnn_net_->Forward();

  // Prepare the inputs to embed_net_ and rnn_net_
  // reset cont_sentence to 1
  caffe::caffe_gpu_set(total_proposals, (float) 1.,
                       cont_sentence_blob->mutable_gpu_data());
  input_sentence_blob->Reshape({1, total_proposals});
  // start with EOS for all sentences
  caffe::caffe_gpu_set(total_proposals, (float) 0.,
                       input_sentence_blob->mutable_gpu_data());
  // hook the output of embed_net_ to rnn_net_
  rnn_net_->set_blob("input_features", embedded_sentence_blob);
  rnn_net_->set_blob("global_features", embedded_sentence_blob);

  // Get the output buffers
  float* bbox_offsets = bbox_offsets_arr->Data<float>();
  // TODO: current assume captions array allocated in CPU only
  float* captions = captions_arr->Data<float>();

  // forward rnn_net_
  for (int step = 0; step < max_timestep_; ++step) {
    embed_net_->Forward();
    rnn_net_->Forward();
    const float* word_prob = word_probs_blob->cpu_data();
    int finished = 0;
    for (int i = 0; i < total_proposals; ++i) {
      int max_idx = -1;
      float max_prob = 0.;
      for (int j = 0; j < nwords; ++j) {
        if (word_prob[j] > max_prob) {
          max_prob = word_prob[j];
          max_idx = j;
        }
      }
      // LOG(INFO) << "proposal " << i << ": " << max_idx << " " << max_prob;
      if (step == 0 || captions[i * max_timestep_ + step - 1] != 0) {
        if (max_idx == 0) {
          ++finished;
        }
        best_words_[i] = max_idx;
        captions[i * max_timestep_ + step] = max_idx;
        Memcpy(bbox_offsets + i * 4, bbox_offsets_arr->device(),
               bbox_pred_blob->gpu_data() + i * 4, gpu_device_,
               4 * sizeof(float));
      } else {
        ++finished;
        best_words_[i] = 0.;
        captions[i * max_timestep_ + step] = 0;
      }
      word_prob += nwords;
    }
    if (finished == total_proposals) {
      break;
    }
    // copy the best words to input_sentence gpu data
    Memcpy(input_sentence_blob->mutable_gpu_data(), gpu_device_,
           best_words_.data(), cpu_device_, total_proposals * sizeof(float));
  }

  auto t3 = std::chrono::high_resolution_clock::now();
  auto feature_lat = std::chrono::duration_cast<std::chrono::milliseconds>(
      t2 - t1);
  auto caption_lat = std::chrono::duration_cast<std::chrono::milliseconds>(
      t3 - t2);
  LOG(INFO) << "feature latency: " << feature_lat.count() << " ms, caption " <<
      " latency: " << caption_lat.count() << " ms";

  // Slice output
  std::unordered_map<std::string, Slice> slices{
    {"rois", Slice(batch, num_proposals, 5)},
    {"bbox_offsets", Slice(batch, num_proposals, 4)},
    {"captions", Slice(batch, num_proposals, max_timestep_)},
    {"scores", Slice(batch, num_proposals, 2)},
  };
  batch_task->SliceOutputBatch(slices);
  delete[] num_proposals;
}

void CaffeDenseCapModel::Postprocess(std::shared_ptr<Task> task) {
  const QueryProto& query = task->query;
  QueryResultProto* result = &task->result;
  std::vector<std::string> output_fields(query.output_field().begin(),
                                         query.output_field().end());
  if (output_fields.size() == 0) {
    output_fields.push_back("rect");
    output_fields.push_back("caption");
  }

  auto& output = task->outputs[0];
  int nboxes = output->arrays.at("rois")->num_elements() / 5;
  float* rois = output->arrays.at("rois")->Data<float>();
  float* boxes = rois;
  float* bbox_offsets = output->arrays.at("bbox_offsets")->Data<float>();
  float* captions = output->arrays.at("captions")->Data<float>();
  float* scores = output->arrays.at("scores")->Data<float>();
  // get attributes
  int im_height = task->attrs["im_height"].as<int>();
  int im_width = task->attrs["im_width"].as<int>();
  float scale = task->attrs["scale_h"].as<float>();
  int* order = new int[nboxes];
  for (int i = 0; i < nboxes; ++i) {
    order[i] = i;
    // transform bbox offsets
    for (int j = 0; j < 4; ++j) {
      bbox_offsets[i * 4 + j] = bbox_offsets[i * 4 + j] * bbox_stds_[j] +
                                bbox_mean_[j];
    }
  }
  TransformBbox(im_height, im_width, scale, nboxes, rois, bbox_offsets, boxes);
  std::sort(order, order + nboxes, [&](int a, int b) {
      return scores[b * 2 + 1] < scores[a * 2 + 1];
    });
  int num_out;
  int* keep_out = new int[nboxes];
  caffe::nms_cpu(boxes, order, nboxes, nms_threshold_, keep_out, &num_out);

  int total_boxes = 0;
  for (int i = 0; i < num_out; ++i) {
    int idx = keep_out[i];
    if (scores[idx * 2 + 1] <= score_threshold_) {
      continue;
    }
    std::string sentence;
    for (int step = 0; step < max_timestep_; ++step) {
      int word = (int) captions[idx * max_timestep_ + step];
      if (word == 0) {
        break;
      }
      sentence += vocabulary_[word] + " ";
    }
    float* box = boxes + idx * 4;
    // Add caption result
    auto record = result->add_output();
    for (auto field : output_fields) {
      if (field == "rect") {
        auto value = record->add_named_value();
        value->set_name("rect");
        value->set_data_type(DT_RECT);
        auto rect = value->mutable_rect();
        rect->set_left(int(round(box[0])));
        rect->set_top(int(round(box[1])));
        rect->set_right(int(round(box[2])));
        rect->set_bottom(int(round(box[3])));
      } else if (field == "caption") {
        auto value = record->add_named_value();
        value->set_name("caption");
        value->set_data_type(DT_STRING);
        value->set_s(sentence);
      } else if (field == "score") {
        auto value = record->add_named_value();
        value->set_name("score");
        value->set_data_type(DT_FLOAT);
        value->set_f(scores[idx * 2 + 1]);
      }
    }
    ++total_boxes;
  }
  //LOG(INFO) << "total boxes: " << total_boxes;
  delete[] order;
  delete[] keep_out;
}

void CaffeDenseCapModel::LoadVocabulary(const std::string& filename) {
  std::ifstream fin(filename);
  vocabulary_.push_back("<EOS>");
  std::string line;
  while (std::getline(fin, line)) {
    vocabulary_.push_back(line);
  }
  fin.close();
  LOG(INFO) << "Load " << vocabulary_.size() << " vocabs from " << filename;
}

void CaffeDenseCapModel::TransformBbox(
    int im_height, int im_width, float scale, int nboxes, const float* rois,
    const float* bbox_deltas, float* out) {
  for (int i = 0; i < nboxes; ++i) {
    float x1 = rois[i * 5 + 1] / scale;
    float y1 = rois[i * 5 + 2] / scale;
    float x2 = rois[i * 5 + 3] / scale;
    float y2 = rois[i * 5 + 4] / scale;
    float width = x2 - x1 + 1;
    float height = y2 - y1 + 1;
    float ctr_x = x1 + 0.5 * width;
    float ctr_y = y1 + 0.5 * height;
    float dx = bbox_deltas[i * 4];
    float dy = bbox_deltas[i * 4 + 1];
    float dw = bbox_deltas[i * 4 + 2];
    float dh = bbox_deltas[i * 4 + 3];
    ctr_x += dx * width;
    ctr_y += dy * height;
    width *= exp(dw);
    height *= exp(dh);
    out[i * 4] = std::max(std::min(ctr_x - 0.5 * width, im_width - 1.), 0.);
    out[i * 4 + 1] = std::max(std::min(ctr_y - 0.5 * height, im_height - 1.),
                              0.);
    out[i * 4 + 2] = std::max(std::min(ctr_x + 0.5 * width, im_width - 1.), 0.);
    out[i * 4 + 3] = std::max(std::min(ctr_y + 0.5 * height, im_height - 1.),
                              0.);
  }
}

} // namespace backend
} // namespace nexus

#endif // USE_CAFFE