/**
  \file

  \brief SED fitting routines.

  If `INIT_SAMPLES` is defined, the solver will randomize fitter initial guess
  (instead of flux) with the -G/-R/-S options.

  If `MEDIAN_FIT` is defined, map output for -G/-R/-S runs is replaced with the
  best-fit median solution. This is the *single* fit whose parameters came
  closest to the median parameters values, not the median values themselves
  (which represent no solution in particular).

  \who \kpr
*/
#include <algorithm>
#include <ctime>
#include <thread>

// Random deviates and statistics.
#include <gsl/gsl_randist.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_statistics_double.h>

// Non-linear solver.
#include <ceres/ceres.h>
#include <glog/logging.h>

#include "Graybody.h"
#include "PACS.h"
#include "SPIRE.h"

namespace mu = mutils;


/// Package namespace.
namespace manticore {

const double  mu_g = (2.75*m_H),  ///< Mean molecular weight.
             imu_g = 1.0/mu_g;    ///< Inverse mean molecular weight.

/// Observation fitting data structure.
struct GrayData {
  /// Constructor.
  GrayData(Graybody *g, std::vector<mapDataType> *o, size_t numBands) :
    gray(g), outMap(o), nBands(numBands)
  {
     Fobs.resize(nBands);
    iFerr.resize(nBands);
     band.resize(nBands);
  }

  Graybody *gray;              ///< Graybody emission model.

  std::vector<long> axis;      ///< Image dimensions.

  std::vector<double> Fobs,    ///< Observed flux in each band.
                     iFerr,    ///< Inverse flux uncertainty in each band.
                      dnu0,    ///< Effective bandwidth.
                      fixed;   ///< Fixed parameters (optional).

  std::vector<const Detector *>
                      band;    ///< Band detectors.

  std::vector<mapDataType>
                     *outMap;  ///< Output maps.

  unsigned nBands;             ///< Number of observation bands.
};


/**
  \brief Ceres solver graybody cost model.

  The model consists of up to four parameters (two gas components, core and
  halo, each with a temperature \f$T\f$ and mass surface density \f$S\f$),
  with the halo temperature sometimes held fixed as shown below.

  NP | Active parameters | Fixed parameters
  -- | ----------------- | ----------------
  1  | Sh                | Th
  2  | Th, Sh            | -
  3  | Sh, Tc, Sc        | Th
  4  | Th, Sh, Tc, Sc    | -

  \note
  This function assumes surface densities have been scaled by #scale for
  fitting (to help normalize their magnitude relative to temperature).

  \tparam NB Number of bands.
  \tparam NP Number of parameters.
*/
template <unsigned NB, unsigned NP>
class Fcost_gray : public ceres::SizedCostFunction<NB,NP> {
public:
  /// Surface density scaling factor.
  static constexpr double scale = 128.0;

  /// Constructor.
  Fcost_gray(const GrayData &gd) : gd_(gd) { clearCache(); }

  /// Evaluate the fitting function and its Jacobian.
  virtual bool Evaluate(double const* const* parameters, double* residuals,
                        double** jacobians) const final {
    const char *const fn = "Fcost_gray";
    const double *p = parameters[0];
    double *jac = (jacobians && jacobians[0] ? jacobians[0] : nullptr);

    // Clear cache.
    if (!residuals) { clearCache(); return true; }

    // Check cache.
    if (memcmp(p, pcache_, sizeof(pcache_)) == 0) {
      MU_DEBUG(fn, "Using cache...");
      memcpy(residuals, rcache_, sizeof(rcache_));
      if (jac) { memcpy(jac, jcache_, sizeof(jcache_)); }
      return true;
    }

    constexpr unsigned P0 = NP&1;  // Parameter index offset.
    const double
      rescale = 1.0/scale,
      Th = (P0==0 ? p[0] : gd_.fixed[0]),
      Sh = p[1-P0] * rescale,
      Tc = (NP > 2 ? p[2-P0] : 0.0),
      Sc = (NP > 2 ? p[3-P0] : 0.0) * rescale;

    MU_DEBUG(fn, "Th = %7.6f, Sh = %9.8f, Tc = %7.6f, Sc = %9.8f%s",
             Th, Sh, Tc, Sc, jac ? " *" : "");

    const auto &Fobs = gd_.Fobs, &iFerr = gd_.iFerr;
    const auto &gray = *gd_.gray;
    const auto band = gd_.band;
    for (unsigned i=0; i != NB; i++) {
      auto Di = band[i];
      double w = iFerr[i], F = (NP < 3 ? gray.F(Th, Sh, Di) :
                                         gray.F(Tc, Sc, Th, Sh, Di));
      residuals[i] = w*(F - Fobs[i]);

      if (jac) {
        if (NP <= 2) {
          if (NP == 2) {
            jac[NP*i+0]  = w*gray.dF_dT(Th, Sh, Di);
          }
          jac[NP*i+1-P0] = w*gray.dF_dS(Th, Sh, Di)*rescale;
        } else {
          if (NP == 4) {
            jac[NP*i+0]  = w*gray.dF_dTh(Tc, Sc, Th, Sh, Di);
          }
          jac[NP*i+1-P0] = w*gray.dF_dSh(Tc, Sc, Th, Sh, Di)*rescale;
          jac[NP*i+2-P0] = w*gray.dF_dTc(Tc, Sc, Th, Sh, Di);
          jac[NP*i+3-P0] = w*gray.dF_dSc(Tc, Sc, Th, Sh, Di)*rescale;
        }

        // Cache results.
        memcpy(pcache_,         p, sizeof(pcache_));
        memcpy(rcache_, residuals, sizeof(rcache_));
        memcpy(jcache_,       jac, sizeof(jcache_));
      }
    }

    return true;
  }

private:
  // Observation fitting data.
  const GrayData &gd_;

  // Latest results are cached as Ceres recomputes values needlessly.
  // For cache coherency, this is done only when the Jacobian is requested.
  mutable double
    pcache_[NP],     ///< Parameter cache.
    rcache_[NB],     ///< Residual  cache.
    jcache_[NB*NP];  ///< Jacobian  cache.

  /// Clear result cache.
  void clearCache() const { for (auto &p: pcache_) { p = -1.0; } }
};


/// Current time a la std::asctime().
std::string asctime()
{
  char buf[64];
  struct tm tmbuf;
  auto t = std::time(nullptr);
  std::string rtn = asctime_r(localtime_r(&t, &tmbuf), buf);
  rtn.pop_back();  // Remove newline.
  return rtn;
}


/**
  \brief Replace input observations with theoretical model.

  \param[in,out] bandData  Band data  storage.
  \param[in,out] bandError Band error storage.
  \param[in]     gdata     Model parameters.
  \param[in]     nParam    Number of parameters.

  The model varies the parameters \a Tc, \a Sc, \a Th, \a Sh on a regular grid,
  core parameters varying along each row (temperature first) and the halo
  parameters varying along the columns.

  \pre
  The input maps must be at least modelMapLen on a side.
*/
void makeModelMap(std::vector<mapDataType> &bandData,
                  std::vector<mapDataType> &bandError,
                  const GrayData &gdata, unsigned nParam)
{
  const auto &gray = *gdata.gray;
  const int n = sqrt(1.0*modelMapLen);
  const double in1 = 1.0/(n-1);

  for (unsigned iMap=0; iMap != modelMapLen*modelMapLen; iMap++) {
    auto i = iMap/modelMapLen, j = iMap%modelMapLen;
    double
      Tc = 6.0*(1.0 + (j%n)*in1),           // Tc ~ [6, 12] K
      Sc = mu_g*3e21*pow(100.0, (j/n)*in1), // Nc(H2) ~ [3e21, 3e23] cm^-2
      Th = 15.0 + 3.0*(i%n)*in1,            // Th ~ [15, 18] K
      Sh = mu_g*3e19*pow(100.0, (i/n)*in1); // Nc(H2) ~ [3e19, 3e21] cm^-2
      MU_DEBUG( "makeModelMap",
                  "(%lu, %lu) Tc = %.2f, Sc = %.2e, Th = %.2f, Sh = %.2e",
                  i, j, Tc, Sc, Th, Sh);

    for (unsigned iBand=0; iBand != bandData.size(); iBand++) {
      auto &data = bandData[iBand], &err = bandError[iBand];
      auto Di = gdata.band[iBand];
      data[iMap] = (nParam == 2 ? gray.F(Th, Sh, Di) :
                                  gray.F(Tc, Sc, Th, Sh, Di)) /
                    gdata.dnu0[iBand];
       err[iMap] = 0.001*data[iMap];
      MU_DEBUG("makeModelMap", "  [%lu] %.4e", iBand, data[iMap]);
    }
  }
}


/**
  \brief Sample random data errors on a fixed grid.

  To empirically explore the effects of data uncertainty on parameter results,
  this method returns an effective data value using the sample index to cycle
  through \f$ data \pm err \f$ values for each band. For this, bit \a iBand
  of \a iCycle is used as the sign bit of error offset (\a nSamp = 2).
  For \a nSamp = 3, the unaltered data sample is also included in the cycle.

  \param[in] data   Band data sample.
  \param[in] err    Sample uncertainty.
  \param[in] iBand  Band index.
  \param[in] iCycle Cycle index.
  \param[in] nSamp  Total samples per band per cycle.
*/
double gridSample(double data, double err, unsigned iBand, unsigned iCycle,
                  unsigned nSamp)
{
  if (nSamp == 2) { return data + ((iCycle>>iBand)&1 ? -err : +err); }
  else {
    unsigned div = 1U;
    for (unsigned i=iBand; i != 0; i--) { div *= nSamp; }

    int digit = (iCycle/div) % nSamp;
    double half = 0.5*(nSamp-1);
    return data + (digit/half-1.0)*err;
  }
}


/**
  \brief Sample systematic data errors on a fixed grid.

  \ref Detector objects contain information on instrument systematic errors,
  both correlated and uncorrelated between bands. This routine cycles through
  all possibilities of \f$ data \pm err \f$---up to \f$2^{NB+NI}\f$, where `NB`
  is the number of bands and `NI` the number of unique instruments between
  them.  If any of the systematic errors is zero, the number of possibilities
  is reduced. This is indicated by setting the output fluxes to `NAN`.

  \param[in] data   Band data sample.
  \param[in] iBand  Band index.
  \param[in] iCycle Cycle index.
  \param[in] instr  Unique instruments.
  \param[in] bands  Detector bands.
*/
double sysSample(double data, unsigned iBand, unsigned iCycle,
                 const std::vector<std::string> &instr,
                 const std::vector<const Detector *> &bands)
{
  // Which instrument?
  unsigned iInstr = 0;
  for (unsigned i=0; i != instr.size(); i++) {
    if (! bands[iBand]->name().compare(0, instr[i].size(), instr[i])) {
      iInstr = i;
      break;
    }
  }

  // Correlated/uncorrelated error offsets this cycle.
  // Skip the cycle if either is zero and this iteration is a repeat.
  unsigned cBit = (iCycle>>(iInstr+bands.size()))&1, uBit = (iCycle>>iBand)&1;
  auto sErr = bands[iBand]->sysError();
  double cErr = (cBit ? -sErr.first  : +sErr.first),
         uErr = (uBit ? -sErr.second : +sErr.second);
  MU_DEBUG("sysSample", "%lu[%lu] %lu/%g %lu/%g",
           iCycle, iBand, cBit, cErr, uBit, uErr);
  return ((cBit && !cErr) || (uBit && !uErr) ? NAN : data*(1.0+cErr+uErr));
}


/**
  \brief Computes median (and arithmetic) statistics on an input vector.

  The input vector is copied, sorted and analyzed to determine the median,
  quantile-based deviation, arithmtic mean, sample standard deviation, minimum
  and maximum, which are written to \a stats in that order. Hence \a stats
  must contain at least **six** elements.

  \param[out] stats Statistics storage.
  \param[in]     x0 Data samples.
  \param[in]      n Number of data samples.
*/
void calcStats(double stats[], const double x0[], unsigned n)
{
  double x[n];
  memcpy(x, x0, sizeof(x));
  std::sort(x, x+n);
  double med  = gsl_stats_median_from_sorted_data(x, 1, n),
         qdev = gsl_stats_quantile_from_sorted_data(x, 1, n, 0.8413) -
                gsl_stats_quantile_from_sorted_data(x, 1, n, 0.1587),
         ave  = gsl_stats_mean(x, 1, n),
         sdev = sqrt(gsl_stats_variance_m(x, 1, n, ave)),
         min = x[0], max = x[n-1];

  stats[0] = med, stats[1] = 0.5*qdev;
  stats[2] = ave, stats[3] = sdev;
  stats[4] = min, stats[5] = max;
}

/**
  \brief Finds optimal model from a set of alternatives.

  The optimal model is considered the one with minimum deviation from the
  median model parameters, taking also its \f$\chi^2\f$ into account.

  \param[in] pstats  Parameters statistics from calcStats().
  \param[in] nParam  Number of parameters.
  \param[in] pdata   Model parameter storage array.
  \param[in] nModels Number of models (cannot exceed \a nCols).
  \param[in] nCols   Number of columns per row in \a pdata.

  \return Index of optimal model.
*/
unsigned findMedianModel(const double pstats[][6], unsigned nParam,
                         const double *pdata, unsigned nModels, unsigned nCols)
{
  unsigned P0 = nParam&1, imin = 0;
  double min = 1e300;
  for (unsigned i=0; i != nModels; i++) {
    double merit = 0.0;
    for (unsigned j=P0; j != P0+nParam; j++) {
      merit += fabs(pdata[nCols*j+i] - pstats[j][0]) / pstats[j][1];
    }
    merit += 0.1*fabs(pdata[nCols*4+i]-pdata[nCols*4]);  // chi2_dof
    if (merit < min) { min = merit, imin = i; }
  }
  return imin;
}

/**
  \brief Trial fit with rejection check.

  Performs a trial fit and compares it to a previous fit (if provided).
  Restores the old fit if that was better.

  \param[in,out] prob     Ceres problem.
  \param[in,out] sum      Ceres summary.
  \param[in,out] opts     Ceres options.
  \param[in]     cost     Ceres cost function.
  \param[in]     p        Parameters.
  \param[in]     NP       Number of parameters.
  \param[in]     p0       Previous parameters (can be null).
  \param[in]     sum0     Previous Ceres summary (can be null).

  \return Number of fitter iterations.
*/
int trialFit(ceres::Problem &prob, ceres::Solver::Summary &sum,
             ceres::Solver::Options &opts, const ceres::CostFunction *cost,
             double p[], unsigned NP, const double p0[] = nullptr,
             const ceres::Solver::Summary *sum0 = nullptr)
{
  // Invalidate cost cache (manticore-specific; cf. Fcost_gray).
  const double *const pp = p;
  (void )cost->Evaluate(&pp, nullptr, nullptr);

  // Calculate fit, restore previous one if superior.
  ceres::Solve(opts, &prob, &sum);
  auto nIter =  sum.num_successful_steps + sum.num_unsuccessful_steps - 1;
  //
  if (p0 && sum0 && (!sum.IsSolutionUsable() ||
        (sum0->IsSolutionUsable() && sum0->final_cost < sum.final_cost))) {
    memcpy(p, p0, NP*sizeof(double));
    sum = *sum0;
  }
  return nIter;
}

/**
  \brief Non-linear fit with robustness checking.

  If the initial fit fails by one of several criteria, or if explicitly
  requested, the parameters are perturbed and refit; if more successful,
  the refit replaces the original solution. Parameter perturbations are set by
  the \a dp[] uncertainties, with signs interleaved between component
  parameters to account for typical covariance between them. If \a dp is null,
  a relative error of 10% is used.

  \param[in,out] prob     Ceres problem.
  \param[in,out] sum      Ceres summary.
  \param[in,out] opts     Ceres options.
  \param[in]     cost     Ceres cost function.
  \param[in]     p        Parameters.
  \param[in]     dp       Parameter uncertainties (for refit use; can be null).
  \param[in]     pmin     Parameter lower bounds.
  \param[in]     pmax     Parameter upper bounds.
  \param[in]     NP       Number of parameters.
  \param[in]     minCost  Cost below which \a refit option is ignored.
  \param[in]     maxCost  Cost above which a refit is forced.
  \param[in]     refit    Whether to force refit.

  \return Total fitter iterations consumed.
*/
int robustFit(ceres::Problem &prob, ceres::Solver::Summary &sum,
              ceres::Solver::Options &opts, const ceres::CostFunction *cost,
              double p[], const double dp[],
              const double pmin[], const double pmax[],
              unsigned NP, double minCost = 0.25, double maxCost = 25.0,
              bool refit = false)
{
  // Trial solution.
  int nIter = trialFit(prob, sum, opts, cost, p, NP);

  // Check solution.
  auto needRefit = [minCost,maxCost,refit](const ceres::Solver::Summary &s) {
    return (!s.IsSolutionUsable() ||
            s.termination_type == ceres::NO_CONVERGENCE ||
            s.final_cost > maxCost || (s.final_cost > minCost && refit));
  };
  if (needRefit(sum)) {
    // Perturb initial conditions.
    auto perturb = [NP](double p[], const double dp[],
                        const double pmin[], const double pmax[]) noexcept {
      for (unsigned i=0; i != NP; i++) {
        double del = 0.1*fabs(dp[i]);
        p[i] = std::max(pmin[i]+del, std::min(p[i]+dp[i], pmax[i]-del));
      }
    };

    // Save trial solution.
    ceres::Solver::Summary sum0{sum};
    double p0[NP];
    memcpy(p0, p, sizeof(p0));

    // Use simple 10% relative error estimate if none provided.
    // Alternate signs for balance.
    double dp0[NP];
    if (dp) { memcpy(dp0, dp, sizeof(dp0)); }
    else    { for (unsigned i=0; i != NP; i++) { dp0[i] = 0.10*p[i]; } }
    //
    for (unsigned i=1; i < NP; i+=2) { dp0[i] = -dp0[i]; }

    // Perturb parameters and try again.
    perturb(p, dp0, pmin, pmax);
    nIter += trialFit(prob, sum, opts, cost, p, NP, p0, &sum0);
    if (needRefit(sum)) {
      // Reverse all signs and try one last time.
      for (unsigned i=0; i != NP; i++) { dp0[i] = -dp0[i]; }
      memcpy(p0, p, sizeof(p0)),  sum0 = sum;
      perturb(p, dp0, pmin, pmax);
      nIter += trialFit(prob, sum, opts, cost, p, NP, p0, &sum0);
    }
  }

  return nIter;
}

/**
  \brief Set unique instrument types.

  This is used to associate correlated systematic errors with the
  appropriate set of detector bands. It is assumed band names follow
  the \c INSTRUMENT-BAND convention, e.g., \c PACS-160.

  \param[in] band Detector bands.
*/
std::vector<std::string>
setInstruments(const std::vector<const Detector *> &band)
{
  std::vector<std::string> instr;
  for (auto &b: band) {
    bool found = false;
    auto iname = b->name();
    iname = iname.substr(0, iname.find('-')+1);

    for (auto &i: instr) { if (! i.compare(iname)) { found = true; break; } }
    if (! found) { instr.push_back(iname); }
  }
  return instr;
}

/**
  \brief Finds best-fit model parameters.

  Solves the non-linear least-squares problem to find the best-fit temperature
  and surface density for up to two gas components. The one-component model
  consists of a single "hot"/"halo" slab of gas. The two-component model
  consists of a "cold"/"core" slab sandwiched between two identical
  "hot"/"halo" slabs, for a total of four parameters (two temperatures and
  surface densities). Optionally, the halo temperature can be prescribed and
  held fixed (\b NP = 1 or \b NP = 3).

  \tparam NP Number of (active) parameters.

  \param[in]  bandData   Input band data.
  \param[in]  bandError  Input band (1-sigma) uncertainties.
  \param[in]  tempMap    Single-component temperature map (can be empty).
  \param[in]  chi2Map    Single-component reduced \f$\chi^2\f$ map
                         (can be empty).
  \param[in]  cli        Command line options and arguments.
  \param[in]  gdata0     Model parameters.
  \param[in]  id         Thread ID number (0-based).
  \param[in]  nThreads   Number of threads.

  \note
  When using the \c --sys-error option, the random error maps should *probably*
  be systematically rescaled the same as the data, but this has not been
  implemented.
*/
template<unsigned NP>
void findParameters(const std::vector<mapDataType> &bandData,
                    const std::vector<mapDataType> &bandError,
                    const mapDataType &tempMap,
                    const mapDataType &chi2Map,
                    const mu::CommandLine &cli,
                    const GrayData &gdata0, unsigned id, unsigned nThreads)
{
  constexpr unsigned P0 = NP&1,             // Index of first active parameter.
                     M0 = (NP > 2 ? 4 : 0); // Cold component map index offset.
  const unsigned NB = gdata0.nBands;

  const char *const fn = "findParameters";
  size_t minBands = cli.getInteger('n', NP==4 ? NP : 3);

  auto &Thalo   = gdata0.outMap->at(0),
       &dThalo  = gdata0.outMap->at(1),
       &Nhalo   = gdata0.outMap->at(2),
       &dNhalo  = gdata0.outMap->at(3),
       &Tcore   = gdata0.outMap->at(0+M0),  // Unused for NP <= 2.
       &dTcore  = gdata0.outMap->at(1+M0),  // Unused for NP <= 2.
       &Ncore   = gdata0.outMap->at(2+M0),  // Unused for NP <= 2.
       &dNcore  = gdata0.outMap->at(3+M0),  // Unused for NP <= 2.
       &Cmap    = gdata0.outMap->at(4+M0),
       &dCmap   = gdata0.outMap->at(5+M0),
       &Imap    = gdata0.outMap->at(6+M0);

  auto *diffMap = &gdata0.outMap->at(7+M0),
       *fracMap = &gdata0.outMap->at(7+M0+(NP>2 ? NB : 0));  // Unused NP <= 2.
  const bool absdiff = cli.getOption('a');
  const double relerr = cli.getDouble('A', 1e-3),
               minCost = 0.125*NB, maxCost = 4.0*NB;

  // Unique instruments (ignoring band).
  std::vector<string> instr = setInstruments(gdata0.band);

  // Data resampling option.
  const bool sysSamp = cli.getOption('S');
  const unsigned sysBits = (sysSamp ? instr.size()+NB : 0);
  const unsigned gridSamp = std::min(cli.getOption('G'), 2U),
                 randSamp = std::max(0L, cli.getInteger('R', 0)),
                 nSamp = 1+(gridSamp ? (gridSamp == 1 ? 16 : 81) :
                             sysSamp ? std::max(1U, randSamp) : randSamp) *
                           (sysSamp ? 1U<<sysBits : 1);
  gsl_rng *rng = gsl_rng_alloc(gsl_rng_mt19937);
  gsl_rng_set(rng, 12345678901234567890UL + id);

  // Single-temperature chi^2 bypass mask.
  // Pixels whose chi2Map[i] < chi2Max are skipped (NaN-filled) here.
  double chi2Max = (chi2Map.size() ? cli.getDouble('c', 1.0) : -1.0);
  if (chi2Map.size() && chi2Map.size() != bandData[0].size()) {
    MU_ERROR(fn, "Ignoring --single-temp map (size %lu, require %lu)",
             chi2Map.size(), bandData[0].size());
    chi2Max = -1.0;
  }

  // Thread-local storage.
  Graybody gray{*gdata0.gray};
  GrayData gdata{gdata0};
  gdata.gray = &gray;

  // Model flux.
  auto calcF = [NB,&gdata,&gray](double F[], double Th, double Sh,
                                             double Tc, double Sc) {
    for (unsigned j=0; j != NB; j++) {
      auto Dj = gdata.band[j];
      F[j] = (NP < 3 ? gray.F(Th, Sh, Dj) : gray.F(Tc, Sc, Th, Sh, Dj));
    }
  };

  // Model data resampling. Sigma limited to reduce fitter stress.
  // Note this biases the randF variance estimate slightly lower.
  auto sample = [rng](double mean, double sigma) {
    double dev;
    do { dev = gsl_ran_gaussian(rng, 1.0); } while (fabs(dev) > 2.5);
    return mean + dev*sigma;
  };
  auto randF = [NB,&gdata,&sample](const double F[]) {
    for (unsigned j=0; j != NB; j++) {
      if (double iFerr = gdata.iFerr[j]) {
        gdata.Fobs[j] = sample(F[j], 1.0/iFerr);
      }
    }
  };
  auto gridF = [NB,gridSamp,&gdata](const double F[], unsigned iSamp) {
    for (unsigned j=0; j != NB; j++) {
      if (double iFerr = gdata.iFerr[j]) {
        gdata.Fobs[j] = gridSample(F[j], 1.0/iFerr, j, iSamp, 1+gridSamp);
      }
    }
  };
  auto sysF = [NB,&instr, &gdata](const double F[], unsigned iSamp) {
    for (unsigned j=0; j != NB; j++) {
      gdata.Fobs[j] = sysSample(F[j], j, iSamp, instr, gdata.band);
    }
  };

  // Initial guess.
  const double T0 = cli.getDouble('T', 15.0), S0 = 0.05,
               T1 = (NP > 2 ? 10.0 : 0.0), S1 = (NP > 2 ? 0.5 : 0.0),
               iDOF = 1.0/std::max(NB-NP, 1U);
  size_t maxIter = std::max(1L, cli.getInteger('m', 50));
  gdata.fixed.push_back(T0);  // Used when Th held fixed.

  // Ceres setup.
  constexpr double Sscale = Fcost_gray<1,1>::scale, Srescale = (1.0/Sscale);
  const double Nmin = cli.getDouble('N', 0.0) * mu_g;
  double cov[4*4],
         p[] = {0.0, 0.0, 0.0, 0.0},
         p0[] = {T0, S0*Sscale, T1, S1*Sscale},
         pmin[] = { 5.0, Nmin*Sscale,  5.0,  0.0*Sscale},
         pmax[] = {50.0, 50.0*Sscale, 15.0, 50.0*Sscale};
  //
  // Bound Tc <= Th if the latter is prescribed.
  if (P0) { pmax[2] = T0; }
  if (NP == 4) { pmin[0] = pmax[2] = std::min(14.0, T0-1.0); }

  ceres::Problem prob;
  ceres::Solver::Summary sum;
  ceres::CostFunction *cost;
  switch (NB) {
    case 1U: cost = new Fcost_gray<1,NP>(gdata); break;
    case 2U: cost = new Fcost_gray<2,NP>(gdata); break;
    case 3U: cost = new Fcost_gray<3,NP>(gdata); break;
    default: cost = new Fcost_gray<4,NP>(gdata); break;
  }
  prob.AddResidualBlock(cost, nullptr, p+P0);
  for (unsigned i=0; i != NP; i++) {
    prob.SetParameterLowerBound(p+P0, i, pmin[i+P0]);
    prob.SetParameterUpperBound(p+P0, i, pmax[i+P0]);
  }

  ceres::Solver::Options opts;
  opts.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
  opts.linear_solver_type = ceres::DENSE_QR;
  opts.dense_linear_algebra_library_type = ceres::EIGEN;
  opts.initial_trust_region_radius = 1e4;
  opts.min_trust_region_radius = 1e-12;
  opts.max_trust_region_radius = 1e+12;
  opts.use_nonmonotonic_steps = false;
  opts.max_consecutive_nonmonotonic_steps = 4;
  opts.max_num_consecutive_invalid_steps = 4;
  opts.minimizer_progress_to_stdout = (mutils::Log::level >=
                                       mutils::Log::DEBUG);
  opts.max_num_iterations = maxIter;
  opts.parameter_tolerance = relerr;
  opts.function_tolerance = 0.1*relerr;

  // Debug check only.
  opts.check_gradients = false;
  opts.gradient_check_relative_precision = 1e-12;

  std::vector<std::pair<const double *, const double *>> covblocks;
  covblocks.emplace_back(p+P0, p+P0);

  ceres::Covariance::Options covopts;
  covopts.algorithm_type  = ceres::DENSE_SVD;
  covopts.null_space_rank = -1;

  ceres::Covariance covinst(covopts);

  // Set initial conditions for first pixel.
  // These are updated at the end on the loop.
  memcpy(p, p0, (P0+NP)*sizeof(p[0]));

  // Multi-simulation parameter storage for statistical analysis.
  double pdata[4+1][nSamp], pstats[4+1][6];

  // Loop over map pixels.
  // Each thread traverses individual rows, interleaved to minimize
  // differences in thread execution time across the map.
  const unsigned nRows = gdata.axis[1], nCols = gdata.axis[0],
    px = nRows*nCols, px0 = ((nRows+nThreads-1)/nThreads)*nCols,
    px10 = px0/10, iSkip = (nThreads-1)*nCols;
  unsigned iOut = (id==0 ? 0 : ~0U);
  for (unsigned i=id*nCols, iCol=0, iTot=0; i < px;
         iTot++, iCol=(iCol==nCols-1 ? 0 : iCol+1), i+=1+(iCol ? 0 : iSkip)) {
    // Progress display.
    if (iTot == iOut) {
      MU_INFO(fn, "Solution %3lu%% complete @ %s",
              (iOut == px0-1 ? 100 : 10*(iOut/px10)), asctime());
      fflush(mutils::Log::stream);
      iOut = (iOut == 9*px10 ? px0-1 : iOut+px10);
    }

    // Reset target data.
    unsigned nOk = 0;
    for (unsigned j=0; j != NB; j++) {
      auto Fobs = gdata.dnu0[j]*bandData[j][i],
           Ferr = gdata.dnu0[j]*bandError[j][i];

      if (std::isfinite(Fobs) && std::isfinite(Ferr)) {
        gdata.Fobs[j]  = Fobs;
        gdata.iFerr[j] = 1.0/Ferr;
        nOk++;
      } else {
        gdata.Fobs[j] = gdata.iFerr[j] = 0.0;  // Weight-out bad data.
      }
    }

    // Skip invalid/masked pixels.
    if (nOk < minBands || (chi2Max >= 0.0 && chi2Map[i] <= chi2Max)) {
      for (auto &omap: *gdata0.outMap) { omap[i] = NAN; }
      memcpy(p, p0, (P0+NP)*sizeof(p[0]));
      continue;
    }

    // Initial temperature override.
    if (tempMap.size()) {
      p[0] = tempMap[i];
      if (NP == 3) { prob.SetParameterUpperBound(p+P0, 1, p[0]); }
    }

    // Find best-fit parameters.
    int nIter = robustFit(prob, sum, opts, cost,
                          p+P0, nullptr, pmin+P0, pmax+P0,
                          NP, minCost, maxCost, nSamp > 1);
    double chi2_dof = 2.0*sum.final_cost*iDOF, dchi2_dof = 0.0;
    MU_DEBUG(fn, "CERES report:\n%s", sum.FullReport().c_str());

    // Check convergence.
    if (! sum.IsSolutionUsable()) {
      MU_WARN(fn, "(%ld,%ld) Solution unusable; masking pixel",
                  1+iCol, 1+i/nCols);
      for (auto &omap: *gdata0.outMap) { omap[i] = NAN; }
      memcpy(p, p0, (P0+NP)*sizeof(p[0]));
      continue;
    }
    MU_INFO_IF(sum.termination_type == ceres::NO_CONVERGENCE, fn,
               "(%ld,%ld) Iteration limit reached", 1+iCol, 1+i/nCols);

    // Fiducial best-fit parameters.
    double Th = p[0], Sh = p[1]*Srescale, dTh = 0.0, dSh = 0.0,
           Tc = p[2], Sc = p[3]*Srescale, dTc = 0.0, dSc = 0.0;

    if (nSamp == 1) {
      // Use covariance matrix to estimate parameter variance.
      if (covinst.Compute(covblocks, &prob)) {
        covinst.GetCovarianceBlock(p+P0, p+P0, cov);
      } else {
        MU_INFO(fn, "(%ld,%ld) Bad covariance matrix; masking values",
                1+iCol, 1+i/nCols);
        for (unsigned j = 0; j != NP; j++) { cov[NP*j+j] = NAN; }
      }
      dTh = (P0 == 0 ? sqrt(cov[NP*0+0]) : 0.0);
      dSh = sqrt(cov[NP*(1-P0)+(1-P0)])*Srescale;
      dTc = (NP > 2 ? sqrt(cov[NP*(2-P0)+(2-P0)]) : 0.0);
      dSc = (NP > 2 ? sqrt(cov[NP*(3-P0)+(3-P0)]) : 0.0)*Srescale;
    } else {
      // Resample data to estimate parameter variance.

      // Fiducial model flux.
      double F0[NB], Fobs0[NB];
      calcF(F0, Th, Sh, Tc, Sc);
      memcpy(Fobs0, &gdata.Fobs[0], sizeof(Fobs0));

      // Fiducial model is the first sample.
      unsigned nOk = 1;
      pdata[0][0] = p[0], pdata[1][0] = p[1];
      pdata[2][0] = p[2], pdata[3][0] = p[3];
      pdata[4][0] = chi2_dof;

      // Data resampling loop.
      for (unsigned j=1; j != nSamp; j++) {
        // Fiducial initial guess.
        p[0] = Th, p[1] = Sh*Sscale;
        p[2] = Tc, p[3] = Sc*Sscale;

#ifdef INIT_SAMPLES
        // Resample initial guess.
        if (P0 == 0) { p[0] += 2.0*(gsl_rng_uniform(rng)-0.5); }
        p[1] *= 0.5 + 1.5*gsl_rng_uniform(rng);
        if (NP > 2) {
          p[2] += 2.0*(gsl_rng_uniform(rng)-0.5);
          p[3] *= 0.5 + 1.5*gsl_rng_uniform(rng);
        }
#else
        // Resample "observed" flux applying both systematic and random errors.
        const double *Fsys = F0;
        if (sysSamp) { sysF(F0, j-1); Fsys = &gdata.Fobs[0]; }
        //
        if      (gridSamp) { gridF(Fsys, (j-1)>>sysBits); }
        else if (randSamp) { randF(Fsys); }
        MU_DEBUG(fn, "sample %2lu: dF = {%7.4f, %7.4f, %7.4f, %7.4f}", nOk,
                 (gdata.Fobs[0]-F0[0])*gdata.iFerr[0],
                 (gdata.Fobs[1]-F0[1])*gdata.iFerr[1],
                 (gdata.Fobs[2]-F0[2])*gdata.iFerr[2],
                 (gdata.Fobs[3]-F0[3])*gdata.iFerr[3]);

        // Cycle skip check.
        bool ok = true;
        for (auto f: gdata.Fobs) {
          if (! std::isfinite(f)) { ok = false; break; }
        }
        if (!ok) { MU_DEBUG(fn, "Skipping..."); continue; }
#endif  // INIT_SAMPLES

        // Find and record best-fit parameters.
        nIter += robustFit(prob, sum, opts, cost,
                           p+P0, nullptr, pmin+P0, pmax+P0,
                           NP, minCost, maxCost);
        if (sum.IsSolutionUsable() &&
            sum.termination_type != ceres::NO_CONVERGENCE) {
          pdata[0][nOk] = p[0], pdata[1][nOk] = p[1];
          pdata[2][nOk] = p[2], pdata[3][nOk] = p[3];
          pdata[4][nOk] = 2.0*sum.final_cost*iDOF;
          MU_DEBUG(fn, "sample %lu:  p = {%7.4f, %7.4f, %7.4f, %7.4f} [%.4f]",
                   nOk, p[0], p[1], p[2], p[3], pdata[4][nOk]);
          nOk++;
        }
      }

      // Analyze statistics.
      if (P0) { pstats[0][1] = pstats[0][3] = 0.0; }  // Fixed temperature.
      for (unsigned j=P0; j != P0+NP; j++) {
        calcStats(pstats[j], pdata[j]+0, nOk);
        MU_DEBUG(fn, "pstats[%lu]: %7.4f(%7.4f)  %7.4f(%7.4f) %7.4f %7.4f", j,
                 pstats[j][0], pstats[j][1], pstats[j][2],
                 pstats[j][3], pstats[j][4], pstats[j][5]);
      }
      dTh = pstats[0][3], dSh = pstats[1][3]*Srescale;
      dTc = pstats[2][3], dSc = pstats[3][3]*Srescale;
      //
      calcStats(pstats[4], pdata[4]+0, nOk);
      dchi2_dof = pstats[4][3];

      // Restore original observations.
      memcpy(&gdata.Fobs[0], Fobs0, sizeof(Fobs0));

#ifdef MEDIAN_FIT
      // Replace fiducial fit with "median" solution.
      unsigned iOpt = findMedianModel(pstats, NP, pdata[0], nOk, nSamp);
      Th = pdata[0][iOpt], Sh = pdata[1][iOpt]*Srescale;
      Tc = pdata[2][iOpt], Sc = pdata[3][iOpt]*Srescale;
      chi2_dof = pdata[4][iOpt];
#else
      // Perform trial fits using final uncertainties.
      double dp[4];
      for (unsigned j=P0; j != P0+NP; j++) {
        p[j] = pstats[j][0], dp[j] = pstats[j][3];
      }
      nIter += robustFit(prob, sum, opts, cost, p+P0, dp+P0, pmin+P0, pmax+P0,
                         NP, minCost, maxCost, true);

      // Use new solution if superior.
      double chi2_dof1 = 2.0*sum.final_cost*iDOF;
      if (chi2_dof1 < pdata[4][0]) {
        Th = p[0], Sh = p[1]*Srescale;
        Tc = p[2], Sc = p[3]*Srescale;
        chi2_dof = chi2_dof1;
      } else {
        p[0] = pdata[0][0], p[1] = pdata[1][0];
        p[2] = pdata[2][0], p[3] = pdata[3][0];
      }
#endif  // MEDIAN_FIT
    }
    MU_DEBUG(fn, "(%2ld,%2ld) Th = %5.2f(%.2f), Sh = %7.4f(%.4f), "
                 "Tc = %5.2f(%.2f), Sc = %7.4f(%.4f), chi2/DOF = %.3f(%.3f)",
             1+iCol, 1+i/nCols, Th, dTh, Sh, dSh, Tc, dTc, Sc, dSc,
             chi2_dof, dchi2_dof);

    // Map outputs.
    Thalo[i] = Th,        dThalo[i] = dTh;
    Nhalo[i] = Sh*imu_g,  dNhalo[i] = dSh*imu_g;
    if (NP > 2) {
      Tcore[i] = Tc,        dTcore[i] = dTc;
      Ncore[i] = Sc*imu_g,  dNcore[i] = dSc*imu_g;
    }
    Cmap[i] = chi2_dof,  dCmap[i] = dchi2_dof;
    Imap[i] = nIter;

    // Solution quality maps.
    for (unsigned j=0; j != NB; j++) {
      auto Dj = gdata.band[j];
      double Fj = (NP < 3 ? gray.F(Th, Sh, Dj) : gray.F(Tc, Sc, Th, Sh, Dj));

      diffMap[j][i] = (Fj - gdata.Fobs[j])*(absdiff ? 1.0/gdata.dnu0[j] :
                                                      gdata.iFerr[j]);
      if (NP > 2) { fracMap[j][i] = gray.F(Tc, Sc, Th, 0.0, Dj) / Fj; }
    }

    // Chain next initial guess to previous solution (if sane).
    // Otherwise reset to standard values.
    if (chi2_dof > 40.0 || (NP > 2 && fabs(Tc-T1) > 3.0)) {
      memcpy(p, p0, (P0+NP)*sizeof(p[0]));
    }
  }

  // Deallocate storage.
  gsl_rng_free(rng);
}


/**
  \brief Read a map plane from the `--single-temp` FITS file.

  If the `--single-temp` option was given, extract the HDU named \a hduName
  from the FITS file.

  A 'T' map can be used to define the halo temperature for 1- or 3-parameter
  fits.

  A 'Chi2/DOF' map can be used in masking evaluation of the stage 2
  (3- or 4-parameter) fitter for individual pixels. The corresponding cut-off
  is specified via the `--chi2-max` option.

  \param[in] cli     Command line options and arguments.
  \param[in] ll      Cutout lower-left  vertex.
  \param[in] ur      Cutout upper-right vertex.
  \param[in] st      Cutout stride (cutout disabled if \a st[0] = 0).
  \param[in] hduName Map HDU name to extract.

  \return Single-temperature reduced \f$\chi^2\f$ map.
*/
mapDataType getSingleMap(const mu::CommandLine &cli,
                         const std::vector<long> &ll,
                         const std::vector<long> &ur,
                         const std::vector<long> &st,
                         const std::string &hduName)
{
  const char *const fn = "getSingleMap";
  mapDataType map;
  mu::CommandLine::Arguments files;

  if (cli.getOption('s', &files, 1)) {
    try {
      std::unique_ptr<CCfits::FITS>
        fmap{new CCfits::FITS(files.front(), CCfits::Read, hduName, true)};
      if (st[0]) { fmap->extension(hduName).read(map, ll, ur, st); }
      else       { fmap->extension(hduName).read(map); }
    } catch (...) {
      MU_WARN(fn, "No HDU '%s' in file '%s'", hduName, files.front());
      map.resize(0);
    }
  }

  return map;
}


/**
  \brief Performs least-squares fitting of band SEDs.

  Given input band continuum fluxes and uncertainties, calculates the best-fit
  temperature and column density maps (including uncertainties and reduced
  chi-square information).

  For one component the output FITS file will contain the following named image
  extensions:
  - **T**      (gas temperature)
  - **dT**     (1-sigma uncertainty in **T**)
  - **N(H2)**  (N(H2) gas column density)
  - **dN(H2)** (1-sigma uncertainty in **N**)
  - **Chi2**   (fit chi-squared per degree of freedom)

  \param[out]    outMap     Output result data.
  \param[in,out] outHDU     Output FITS headers.
  \param[in,out] outFITS    Output FITS file.
  \param[in]     bandData   Input band data.
  \param[in]     bandError  Input band (1-sigma) uncertainties.
  \param[in]     bandHDU    Input band FITS headers.
  \param[in]     axis       Images dimensions.
  \param[in]     cli        Command line options and arguments.
  \param[in]     ll         Cutout lower-left  vertex.
  \param[in]     ur         Cutout upper-right vertex.
  \param[in]     st         Cutout stride (cutout disabled if \a st[0] = 0).
*/
void solve(std::vector<mapDataType> &outMap,
           std::vector<CCfits::ExtHDU *> &outHDU,
           CCfits::FITS *outFITS,
           const std::vector<mapDataType> &bandData,
           const std::vector<mapDataType> &bandError,
           const std::vector<CCfits::PHDU*> &bandHDU,
           const std::vector<long> &axis,
           const mu::CommandLine &cli,
           const std::vector<long> &ll,
           const std::vector<long> &ur,
           const std::vector<long> &st)
{
  const char *const fn = "solve";
  bool modelMap = cli.getOption('M');

  // Dust model(s).
  bool extend = !cli.getOption('p');
  auto modelh = cli.getString('D', "DL3"),
       ratioh = cli.getString('g', "100.0");

  // Different core/halo models?
  auto mpos = modelh.find(','), rpos = ratioh.find(',');
  decltype(modelh) modelc{modelh}, ratioc{ratioh};
  if (mpos != mutils::npos) {
    modelc = modelh.substr(mpos+1);
    modelh.erase(mpos);
  }
  if (rpos != mutils::npos) {
    ratioc = ratioh.substr(rpos+1);
    ratioh.erase(rpos);
  }

  Dust dusth(modelh, std::stod(ratioh)), dustc(modelc, std::stod(ratioc));
  size_t nParam = (cli.getOption('4') ? 4 : cli.getOption('3') ? 3 :
                   cli.getOption('1') ? 1 : 2);
  MU_INFO(fn, "Using %s source detector response",
          extend ? "extended" : "point");
  MU_INFO(fn, "Minimum (halo) N(H2) = %.1e cm^-2", cli.getDouble('N', 0.0));
  MU_INFO(fn, "Hot/halo  dust model: %s, gas-to-dust ratio = %g",
          dusth.name(), dusth.rho());
  MU_INFO_IF(nParam > 2, fn, "Cold/core dust model: %s, gas-to-dust ratio = %g",
          dustc.name(), dustc.rho());

  // Band detector setup.
  // Currently supports PACS/SPIRE only!
  size_t nBands = bandData.size();
  std::vector<std::unique_ptr<Detector>> detect;
  for (size_t j=0; j != nBands; j++) {
    int band = getBand(*bandHDU[j]);
    auto d = (band < 200 ? (Detector *)new PACS(band, extend)
                         : (Detector *)new SPIRE(band, extend));
    d->init();
    detect.emplace_back(d);
  }

  // Allocate result maps.
  const double relerr = cli.getDouble('A', 1e-3);
  auto px = axis[0]*axis[1];
  char comm[1024];
  snprintf(comm, sizeof(comm),
           "%s sources, N(H2) >= %.1e cm^-2,"
           " %s/%s dust, gas/dust = %g/%g, relerr = %.1e",
           (extend ? "Extended" : "Point"), cli.getDouble('N', 0.0),
           dusth.name().c_str(), dustc.name().c_str(),
           dusth.rho(), dustc.rho(), relerr);
  //
  std::vector<std::string> mapNames = {"T", "dT", "N(H2)", "dN(H2)"};
  if (nParam > 2) {
    mapNames = {"Th", "dTh", "Nh(H2)", "dNh(H2)",
                "Tc", "dTc", "Nc(H2)", "dNc(H2)"};
  }
  for (auto m: {"Chi2/DOF", "dChi2/DOF", "nIter"}) { mapNames.emplace_back(m); }
  for (auto &b: bandHDU) {
    // Band fit residuals.
    mapNames.push_back("diff" + std::to_string(getBand(*b)));
  }
  if (nParam > 2) {
    for (auto &b: bandHDU) {
      // Cold flux fraction.
      mapNames.push_back("frac" + std::to_string(getBand(*b)));
    }
  }
  //
  // CCFits API omits 'const'...
  std::vector<long> ax{axis};
  for (auto name: mapNames) {
    outMap.push_back(mapDataType(0.0, px));
    outHDU.push_back(outFITS->addImage(name, mapDataCode, ax));
    outHDU.back()->writeDate();
    outHDU.back()->writeComment(comm);
  }

  // Emission model.
  constexpr double MJy = 1e-17;  // MJy -> erg/cm^2/s/Hz
  Graybody gray{dusth, dustc, 0.1*relerr};
  GrayData gdata{&gray, &outMap, nBands};
  for (size_t j=0; j != nBands; j++) {
    gdata.band[j] = detect[j].get();
    gdata.dnu0.push_back(detect[j]->freqWidth() * MJy);
  }
  gdata.axis = axis;

  // Theoretical model map option.
  if (modelMap) {
    makeModelMap(const_cast<std::vector<mapDataType>&>(bandData),
                 const_cast<std::vector<mapDataType>&>(bandError),
                 gdata, nParam);
  }

  // Google logging for Ceres solver.
  google::InitGoogleLogging("manticore");

  // Create threads and process maps.
  unsigned nThreads = std::max(1L, cli.getInteger('t', 1));
  auto tempMap = getSingleMap(cli, ll, ur, st, "T");
  auto chi2Map = getSingleMap(cli, ll, ur, st, "Chi2/DOF");
  auto solver = (nParam == 1 ? &findParameters<1> :
                 nParam == 2 ? &findParameters<2> :
                 nParam == 3 ? &findParameters<3> :
                               &findParameters<4>);

#undef CHI2_MIN
#ifdef CHI2_MIN
// Quick hack to examine global chi^2 behavior vs. systematic errors.
auto bandData1 = bandData;
double sErr[5], sMax[5] = {0.05, 0.04, 0.015, 0.015, 0.015};
const unsigned iChi = (nParam <= 2 ? 4 : 8);
for (unsigned j=0, n=5, nSamp=n*n*n*n*n; j != nSamp; j++) {
  // Error state.
  for (unsigned k=0; k != 5; k++) {
    sErr[k] = gridSample(0.0, sMax[k], k, j, n);
  }
  for (unsigned k=0; k != px; k++) {
    bandData1[0][k] = bandData[0][k] * (1.0 + sErr[0]);
    bandData1[1][k] = bandData[1][k] * (1.0 + sErr[1] + sErr[2]);
    bandData1[2][k] = bandData[2][k] * (1.0 + sErr[1] + sErr[3]);
    bandData1[3][k] = bandData[3][k] * (1.0 + sErr[1] + sErr[4]);
  }
#else
#  define bandData1 bandData
#endif  // CHI2_MIN

  std::vector<std::thread> threads;
  for (unsigned i=1; i != nThreads; i++) {
    threads.emplace_back(solver, bandData1, bandError, tempMap, chi2Map, cli,
                         gdata, i, nThreads);
  }

  solver(bandData1, bandError, tempMap, chi2Map, cli, gdata, 0, nThreads);
  //
  for (auto &t: threads) { t.join(); }

#ifdef CHI2_MIN
  double chi2_tot = 0.0;
  for (unsigned k=0; k != px; k++) { chi2_tot += outMap[iChi][k]; }
  MU_INFO(fn, "<chi2>: %6.3f  %+6.3f %+6.3f %+6.3f %+6.3f %+6.3f\n",
          chi2_tot/px, sErr[0], sErr[1], sErr[2], sErr[3], sErr[4]);
}
#endif  // CHI2_MIN

}

}  // namespace manticore