/*! \file eddy.cpp
    \brief Contains main() and some very high level functions for eddy
*/
#if __cplusplus >= 201103L || __clang__
 #include <array>
using std::array;
#else
 #include <tr1/array>
using std::tr1::array;
#endif
#include <cstdlib>
#include <string>
#include <vector>
#include <cmath>
#include <memory>
#include <thread>
#include "nlohmann/json.hpp"
#include "armawrap/newmat.h"
#include "newimage/newimageall.h"
#include "miscmaths/miscmaths.h"
#include "utils/stack_dump.h"
#include "utils/FSLProfiler.h"
#include "EddyHelperClasses.h"
#include "ECScanClasses.h"
#include "DiffusionGP.h"
#include "b0Predictor.h"
#include "fmriPredictor.h"
#include "EddyUtils.h"
#include "EddyCommandLineOptions.h"
#include "PostEddyCF.h"
#include "PostEddyAlignShellsFunctions.h"
#include "MoveBySuscCF.h"
#include "BiasFieldEstimator.h"
#include "eddy.h"
#ifdef COMPILE_GPU
#include "cuda/GpuPredictorChunk.h"
#include "cuda/EddyGpuUtils.h"
#endif

namespace EDDY {
/// A struct/class that gathers nlohmann::json objects from the various file writing functions.
/// These are then written out as a single .json file. Longer term the plan is to move away
/// from writing individual text files in favour of a single .json file. It is declared here
/// as part of hiding nlohmann from nvcc.
class json_out_struct
{
public:
  nlohmann::json              shell_indicies;
  nlohmann::json              outlier_report;
  std::vector<nlohmann::json> outlier_maps;
  nlohmann::json              parameters;
  nlohmann::json              movement_over_time;
  nlohmann::json              movement_rms;
  nlohmann::json              restricted_movement_rms;
  nlohmann::json              long_ec_parameters;
  void Write(const std::string& fname) const;
};
} // End namespace EDDY

using namespace std;
using namespace EDDY;

/// The entry point of eddy.
int main(int argc, char *argv[]) try
{
  StackDump::Install(); // Gives us informative stack dump if/when program crashes

  // Parse comand line input
  EddyCommandLineOptions clo(argc,argv); // Command Line Options

  // Set range of b-values considered to be in the same shell
  if (clo.IsDiffusion()) EddyUtils::SetbRange(clo.BValueRange());

  // Prime profiler if requested by user
  if (clo.LogTimings()) Utilities::FSLProfiler::SetProfilingOn(clo.LoggerFname());
  Utilities::FSLProfiler prof("_"+string(__FILE__)+"_"+string(__func__));
  double total_key = prof.StartEntry("Begins eddy run");

  EDDY::json_out_struct json_out; // Structure for gathering json output objects.

  // Read all available info
  if (clo.Verbose()) cout << "Reading images" << endl;
  ECScanManager sm = (clo.IsDiffusion()) ?  // Conditional assignment because ECScanManager has no default constructor
                                            ECScanManager(clo.ImaFname(),clo.MaskFname(),clo.AcqpFname(),clo.TopupFname(),
							  clo.FieldFname(),clo.FieldMatFname(),clo.BVecsFname(),clo.BValsFname(),
							  clo.BDeltasFname(),clo.RepetitionTime(),clo.EchoTime(),clo.FirstLevelModel(),
							  clo.b0_FirstLevelModel(),clo.LongECModel(),clo.Indicies(),clo.SessionIndicies(),
							  clo.PolationParameters(),clo.MultiBand(),clo.DontCheckShelling())  // Scan Manager for diffusion
                                         :  // If clo.IsDiffusion() is false
                                            ECScanManager(clo.ImaFname(),clo.MaskFname(),clo.AcqpFname(),clo.TopupFname(),
							  clo.FieldFname(),clo.FieldMatFname(),clo.RepetitionTime(),
							  clo.EchoTime(),clo.Indicies(),clo.SessionIndicies(),
							  clo.PolationParameters(),clo.MultiBand()); // Scan Manager for fmri
  if (clo.FillEmptyPlanes()) { if (clo.Verbose()) cout << "Filling empty planes" << endl; sm.FillEmptyPlanes(); }
  if (clo.ResamplingMethod() == FinalResamplingType::LSR) {
    if (!sm.CanDoLSRResampling()) throw EddyException("These data do not support least-squares resampling");
  }
  if (clo.UseB0sToAlignShellsPostEddy() && !sm.B0sAreUsefulForPEAS()) {
    throw EddyException("These data do not support using b0s for Post Eddy Alignment of Shells");
  }
  if (clo.RefScanNumber()) sm.SetLocationReference(clo.RefScanNumber());

  // Write .json file with shell indicies for use by EddyQC and debugging
  if (clo.IsDiffusion()) json_out.shell_indicies = sm.WriteShellIndicies(clo.ShellIndexOutFname());

  // Write topup-field if debug flag is set
  if (clo.DebugLevel() && sm.HasSuscHzOffResField()) {
    std::string fname = "EDDY_DEBUG_susc_00_0000"; NEWIMAGE::write_volume(*(sm.GetSuscHzOffResField()),fname);
  }

  // Set initial parameters. This option is only for testing/debugging/personal use
  if (clo.InitFname() != std::string("")) {
    if (clo.RegisterDWI() && clo.Registerb0()) sm.SetParameters(clo.InitFname(),ScanType::Any);
    else if (clo.RegisterDWI()) sm.SetParameters(clo.InitFname(),ScanType::DWI);
    else sm.SetParameters(clo.InitFname(),ScanType::b0);
  }

  // Do the registration

  //////////////////////////////////////////////////////////////////////
  // The first, and possibly only, registration step is volume_to_volume
  //////////////////////////////////////////////////////////////////////

  double vol_key = prof.StartEntry("Calling DoVolumeToVolumeRegistration");
  if (clo.Verbose()) cout << "Performing volume-to-volume registration" << endl;
  ReplacementManager *dwi_rm=NULL;
  if (clo.EstimateMoveBySusc()) { // Restrict EC if we are to eventually estimate MBS
    EDDY::SecondLevelECModelType b0_slm = clo.b0_SecondLevelModel();
    EDDY::SecondLevelECModelType dwi_slm = clo.SecondLevelModel();
    if (clo.VeryVerbose()) cout << "Setting linear second level model" << endl;
    clo.Set_b0_SecondLevelModel(SecondLevelECModelType::Linear);
    clo.SetSecondLevelModel(SecondLevelECModelType::Linear);
    dwi_rm = DoVolumeToVolumeRegistration(clo,sm);
    if (clo.VeryVerbose()) cout << "Resetting second level model" << endl;
    clo.Set_b0_SecondLevelModel(b0_slm);
    clo.SetSecondLevelModel(dwi_slm);
  }
  else dwi_rm = DoVolumeToVolumeRegistration(clo,sm);
  prof.EndEntry(vol_key);

  // The remaining steps we only run if registration was actually run,
  // i.e. not if we read an initialisation file and did zero iterations.
  if (clo.NIter() > 0) {
    sm.ApplyLocationReference();

    // Write text-file with MI values if requested (testing/debugging)
    if (clo.PrintMIValues()) {
      if (clo.Verbose()) cout << "Writing MI values between shells" << endl;
      PrintMIValues(clo,sm,clo.MIPrintFname(),clo.PrintMIPlanes());
    }

    // Check for residual position differences between shells
    if (clo.RegisterDWI()) {
      double peas_key = prof.StartEntry("Calling PostEddyAlignShells");
      if (!clo.SeparateOffsetFromMovement() && !clo.AlignShellsPostEddy()) {
	if (clo.Verbose()) cout << "Checking shell alignment along PE-direction (running PostEddyAlignShellsAlongPE)" << endl;
	PEASUtils::PostEddyAlignShellsAlongPE(clo,false,sm);
	if (clo.Verbose()) cout << "Checking shell alignment (running PostEddyAlignShells)" << endl;
	PEASUtils::PostEddyAlignShells(clo,false,sm);
      }
      else if (clo.SeparateOffsetFromMovement() && !clo.AlignShellsPostEddy()) {
	if (clo.Verbose()) cout << "Aligning shells along PE-direction (running PostEddyAlignShellsAlongPE)" << endl;
	PEASUtils::PostEddyAlignShellsAlongPE(clo,true,sm);
	if (clo.Verbose()) cout << "Checking shell alignment (running PostEddyAlignShells)" << endl;
	PEASUtils::PostEddyAlignShells(clo,false,sm);
      }
      else if (clo.AlignShellsPostEddy()) {
	if (clo.Verbose()) cout << "Checking shell alignment along PE-direction (running PostEddyAlignShellsAlongPE)" << endl;
	PEASUtils::PostEddyAlignShellsAlongPE(clo,false,sm);
	if (clo.Verbose()) cout << "Aligning shells (running PostEddyAlignShells)" << endl;
	PEASUtils::PostEddyAlignShells(clo,true,sm);
      }
      prof.EndEntry(peas_key);
    }
  }

  //////////////////////////////////////////////////////////////////////
  // Next, estimate long time-constant EC if requested
  //////////////////////////////////////////////////////////////////////

  bool original_sep_offs_move = clo.SeparateOffsetFromMovement();
  if (clo.EstimateLongEC()) {
    double longEC_key = prof.StartEntry("Calling EstimateLongEC");
    if (clo.Verbose()) {
      cout << "Estimating long time-constant eddy currents" << endl;
      cout << "Using model: " << clo.LongECModelString() << endl;
      if (clo.ReestimateECWhenEstimatingLongEC()) {
	if (clo.Verbose()) cout << "Also jointly re-estimating EC and movement parameters" << endl;
      }
    }
    // The long-EC estimates _should_ break the very strong covariance between estimates
    // of EC-DC-component and subject movement along PE-direction. There is therefore an
    // option to "turn off" the attempted separation of EC-DC and subject movement at this
    // point. N.B. that it will remain turned off after this point. But we will still align
    // different shells along PE. Messy and ugly, and should be refactored at some point.
    // I leave it like this for now to avoid changing the interface (breaking scripts).
    if (!clo.SeparateOffsetFromMovementWhenEstimatingLongEC()) {
      clo.SetSeparateOffsetFromMovement(false);
    }
    dwi_rm = EstimateLongEC(clo,sm,dwi_rm);
    prof.EndEntry(longEC_key);
    sm.ApplyLocationReference();

    // Check for residual position differences between shells
    if (clo.RegisterDWI()) {
      double peas_key = prof.StartEntry("Calling PostEddyAlignShells");
      if (!original_sep_offs_move && !clo.AlignShellsPostEddy()) {
	if (clo.Verbose()) cout << "Checking shell alignment along PE-direction (running PostEddyAlignShellsAlongPE)" << endl;
	PEASUtils::PostEddyAlignShellsAlongPE(clo,false,sm);
	if (clo.Verbose()) cout << "Checking shell alignment (running PostEddyAlignShells)" << endl;
	PEASUtils::PostEddyAlignShells(clo,false,sm);
      }
      else if (original_sep_offs_move && !clo.AlignShellsPostEddy()) {
	if (clo.Verbose()) cout << "Aligning shells along PE-direction (running PostEddyAlignShellsAlongPE)" << endl;
	PEASUtils::PostEddyAlignShellsAlongPE(clo,true,sm);
	if (clo.Verbose()) cout << "Checking shell alignment (running PostEddyAlignShells)" << endl;
	PEASUtils::PostEddyAlignShells(clo,false,sm);
      }
      else if (clo.AlignShellsPostEddy()) {
	if (clo.Verbose()) cout << "Checking shell alignment along PE-direction (running PostEddyAlignShellsAlongPE)" << endl;
	PEASUtils::PostEddyAlignShellsAlongPE(clo,false,sm);
	if (clo.Verbose()) cout << "Aligning shells (running PostEddyAlignShells)" << endl;
	PEASUtils::PostEddyAlignShells(clo,true,sm);
      }
      prof.EndEntry(peas_key);      
    }
  }

  //////////////////////////////////////////////////////////////////////
  // Next do a first round of estimating a bias field if requested
  //////////////////////////////////////////////////////////////////////
  // EstimateBiasField(clo,10.0,1.0,sm);
  // std::shared_ptr<const NEWIMAGE::volume<float> > bfield = sm.GetBiasField();
  // exit(EXIT_SUCCESS);

  // Set initial slice-to-vol parameters. This option is only for testing/debugging/personal use
  if (clo.IsSliceToVol() && clo.InitS2VFname() != std::string("")) {
    sm.SetMovementModelOrder(clo.MovementModelOrder(0));
    if (clo.RegisterDWI() && clo.Registerb0()) sm.SetS2VMovement(clo.InitS2VFname(),ScanType::Any);
    else if (clo.RegisterDWI()) sm.SetS2VMovement(clo.InitS2VFname(),ScanType::DWI);
    else sm.SetS2VMovement(clo.InitS2VFname(),ScanType::b0);
  }

  //////////////////////////////////////////////////////////////////////
  // Next do the slice-to-vol registration if requested.
  //////////////////////////////////////////////////////////////////////
  if (clo.IsSliceToVol() && clo.S2V_NIter(0) > 0 && clo.InitS2VFname() == std::string("")) {
    double s2v_key = prof.StartEntry("Calling DoSliceToVolumeRegistration");
    if (clo.Verbose()) cout << "Performing slice-to-volume registration" << endl;
    if (clo.EstimateMoveBySusc()) { // Restrict EC if we are to eventually estimate MBS
      SecondLevelECModelType b0_slm = clo.b0_SecondLevelModel();
      SecondLevelECModelType dwi_slm = clo.SecondLevelModel();
      clo.Set_b0_SecondLevelModel(SecondLevelECModelType::Linear);
      clo.SetSecondLevelModel(SecondLevelECModelType::Linear);
      for (unsigned int i=0; i<clo.NumOfNonZeroMovementModelOrder(); i++) {
	if (clo.Verbose()) cout << "Setting slice-to-volume order to " << clo.MovementModelOrder(i) << endl;
	sm.SetMovementModelOrder(clo.MovementModelOrder(i));
	sm.Set_S2V_Lambda(clo.S2V_Lambda(i));
	dwi_rm = DoSliceToVolumeRegistration(clo,i,false,sm,dwi_rm);
	sm.ApplyLocationReference();
      }
      clo.Set_b0_SecondLevelModel(b0_slm);
      clo.SetSecondLevelModel(dwi_slm);
    }
    else {
      for (unsigned int i=0; i<clo.NumOfNonZeroMovementModelOrder(); i++) {
	if (clo.Verbose()) cout << "Setting slice-to-volume order to " << clo.MovementModelOrder(i) << endl;
	sm.SetMovementModelOrder(clo.MovementModelOrder(i));
	sm.Set_S2V_Lambda(clo.S2V_Lambda(i));
	dwi_rm = DoSliceToVolumeRegistration(clo,i,false,sm,dwi_rm);
	sm.ApplyLocationReference();
      }
    }
    prof.EndEntry(s2v_key);

    // Do another check for residual position differences in case s2v wrecked things
    if (clo.RegisterDWI()) {
      double peas_key = prof.StartEntry("Calling PostEddyAlignShells after S2V");
      if (!original_sep_offs_move && !clo.AlignShellsPostEddy()) {
	if (clo.Verbose()) cout << "Checking shell alignment along PE-direction (running PostEddyAlignShellsAlongPE)" << endl;
	PEASUtils::PostEddyAlignShellsAlongPE(clo,false,sm);
	if (clo.Verbose()) cout << "Checking shell alignment (running PostEddyAlignShells)" << endl;
	PEASUtils::PostEddyAlignShells(clo,false,sm);
      }
      else if (original_sep_offs_move && !clo.AlignShellsPostEddy()) {
	if (clo.Verbose()) cout << "Aligning shells along PE-direction (running PostEddyAlignShellsAlongPE)" << endl;
	PEASUtils::PostEddyAlignShellsAlongPE(clo,true,sm);
	if (clo.Verbose()) cout << "Checking shell alignment (running PostEddyAlignShells)" << endl;
	PEASUtils::PostEddyAlignShells(clo,false,sm);
      }
      else if (clo.AlignShellsPostEddy()) {
	if (clo.Verbose()) cout << "Checking shell alignment along PE-direction (running PostEddyAlignShellsAlongPE)" << endl;
	PEASUtils::PostEddyAlignShellsAlongPE(clo,false,sm);
	if (clo.Verbose()) cout << "Aligning shells (running PostEddyAlignShells)" << endl;
	PEASUtils::PostEddyAlignShells(clo,true,sm);
      }
      prof.EndEntry(peas_key);
    }
  }

  // Set initial MBS derivative fields. This option is only for testing/debugging/personal use
  if (clo.EstimateMoveBySusc() && clo.InitMBSFname() != std::string("")) {
    NEWIMAGE::volume4D<float> mbs_init;
    NEWIMAGE::read_volume4D(mbs_init,clo.InitMBSFname());
    for (unsigned int i=0; i<clo.MoveBySuscParam().size(); i++) {
      sm.SetDerivSuscField(clo.MoveBySuscParam()[i],mbs_init[i]);
    }
  }

  //////////////////////////////////////////////////////////////////////
  // Estimate movement-by-susceptibility interaction if requested.
  //////////////////////////////////////////////////////////////////////
  if (clo.EstimateMoveBySusc() && clo.MoveBySuscNiter()>0) {
    sm.SetUseB0sToInformDWIRegistration(false); // So as to preserve current estimates
    std::vector<unsigned int> b0s;
    std::vector<unsigned int> dwis;
    if (clo.IsSliceToVol()) {           // Use only "steady" volumes if we know who they are
      EDDY::s2vQuant s2vq(sm,1.0,1.0);  // "Steadiness hardcoded to 1mm and 1degree
      if (clo.Registerb0()) b0s = s2vq.FindStillVolumes(ScanType::b0,clo.MoveBySuscParam());
      if (clo.RegisterDWI()) dwis = s2vq.FindStillVolumes(ScanType::DWI,clo.MoveBySuscParam());
    }
    else { // Otherwise use all volumes
      if (clo.Registerb0()) { b0s.resize(sm.NScans(ScanType::b0)); for (unsigned int i=0; i<sm.NScans(ScanType::b0); i++) b0s[i] = i; }
      if (clo.RegisterDWI()) { dwis.resize(sm.NScans(ScanType::DWI)); for (unsigned int i=0; i<sm.NScans(ScanType::DWI); i++) dwis[i] = i; }
    }
    if (clo.RegisterDWI()) { // Do interleaved MBS and EC/movement estimation
      unsigned int mbs_niter = (clo.MoveBySuscNiter() / clo.N_MBS_Interleaves()) + 1;
      unsigned int niter, s2vi=0;
      if (clo.IsSliceToVol()) {
	s2vi = clo.NumOfNonZeroMovementModelOrder()-1;
	niter = clo.S2V_NIter(s2vi);
	sm.SetMovementModelOrder(clo.MovementModelOrder(s2vi));
	sm.Set_S2V_Lambda(clo.S2V_Lambda(s2vi));
	clo.SetS2VParam(clo.MovementModelOrder(s2vi),clo.S2V_Lambda(s2vi),0.0,(niter/clo.N_MBS_Interleaves())+1);
      }
      else { niter = clo.NIter(); clo.SetNIterAndFWHM((niter/clo.N_MBS_Interleaves())+1,std::vector<float>(1,0.0)); }
      NEWMAT::ColumnVector spar;
      EDDY::MoveBySuscCF cf(sm,clo,b0s,dwis,clo.MoveBySuscParam(),clo.MoveBySuscOrder(),clo.MoveBySuscKsp());

      for (unsigned int i=0; i<clo.N_MBS_Interleaves(); i++) {
	if (clo.Verbose()) cout << "Running interleave " << i+1 << " of MBS" << endl;
	if (!i) spar = cf.Par(); // Start guesses all zeros for first iteration
	cf.SetLambda(clo.MoveBySuscLambda());
	MISCMATHS::NonlinParam nlp(cf.NPar(),MISCMATHS::NL_LM,spar);
	nlp.SetMaxIter(mbs_niter);
	nlp.SetGaussNewtonType(MISCMATHS::LM_GN);
	double mbs_key = prof.StartEntry("Calling nonlin for MBS");
	MISCMATHS::nonlin(nlp,cf);
	prof.EndEntry(mbs_key);
	spar = cf.Par(); // Save for next iteration
	if (clo.IsSliceToVol()) {
	  if (clo.Verbose()) cout << "Running slice-to-vol interleaved with MBS" << endl;
	  double s2v_key = prof.StartEntry("Calling DoSliceToVolumeRegistration as part of MBS");
	  dwi_rm = DoSliceToVolumeRegistration(clo,s2vi,false,sm,dwi_rm);
	  sm.ApplyLocationReference();
	  prof.EndEntry(s2v_key);
	}
	else {
	  if (clo.Verbose()) cout << "Running vol-to-vol interleaved with MBS" << endl;
	  double v2v_key = prof.StartEntry("Calling DoVolumeToVolumeRegistration as part of MBS");
	  dwi_rm = DoVolumeToVolumeRegistration(clo,sm);
	  sm.ApplyLocationReference();
	  prof.EndEntry(v2v_key);
	}
      }
      cf.WriteFirstOrderFields(clo.MoveBySuscFirstOrderFname());
      if (clo.MoveBySuscOrder() > 1) cf.WriteSecondOrderFields(clo.MoveBySuscSecondOrderFname());
    }
    else { // Just do a straightforward MBS estimation
      // Make cost-function object for movement-by-susceptibility
      EDDY::MoveBySuscCF cf(sm,clo,b0s,dwis,clo.MoveBySuscParam(),clo.MoveBySuscOrder(),clo.MoveBySuscKsp());
      NEWMAT::ColumnVector spar = cf.Par(); // Start guesses (all zeros);
      cf.SetLambda(clo.MoveBySuscLambda());
      MISCMATHS::NonlinParam nlp(cf.NPar(),MISCMATHS::NL_LM,spar);
      nlp.SetMaxIter(clo.MoveBySuscNiter());
      nlp.SetGaussNewtonType(MISCMATHS::LM_GN);
      double mbs_key = prof.StartEntry("Calling nonlin for MBS");
      MISCMATHS::nonlin(nlp,cf);            // Estimate
      prof.EndEntry(mbs_key);
      cf.WriteFirstOrderFields(clo.MoveBySuscFirstOrderFname());
      if (clo.MoveBySuscOrder() > 1) cf.WriteSecondOrderFields(clo.MoveBySuscSecondOrderFname());
    }

    // Do another check for residual position differences in case MBS wrecked things
    if (clo.RegisterDWI()) {
      double peas_key = prof.StartEntry("Calling PostEddyAlignShells after MBS");
      if (!original_sep_offs_move && !clo.AlignShellsPostEddy()) {
	if (clo.Verbose()) cout << "Checking shell alignment along PE-direction (running PostEddyAlignShellsAlongPE)" << endl;
	PEASUtils::PostEddyAlignShellsAlongPE(clo,false,sm);
	if (clo.Verbose()) cout << "Checking shell alignment (running PostEddyAlignShells)" << endl;
	PEASUtils::PostEddyAlignShells(clo,false,sm);
      }
      else if (original_sep_offs_move && !clo.AlignShellsPostEddy()) {
	if (clo.Verbose()) cout << "Aligning shells along PE-direction (running PostEddyAlignShellsAlongPE)" << endl;
	PEASUtils::PostEddyAlignShellsAlongPE(clo,true,sm);
	if (clo.Verbose()) cout << "Checking shell alignment (running PostEddyAlignShells)" << endl;
	PEASUtils::PostEddyAlignShells(clo,false,sm);
      }
      else if (clo.AlignShellsPostEddy()) {
	if (clo.Verbose()) cout << "Checking shell alignment along PE-direction (running PostEddyAlignShellsAlongPE)" << endl;
	PEASUtils::PostEddyAlignShellsAlongPE(clo,false,sm);
	if (clo.Verbose()) cout << "Aligning shells (running PostEddyAlignShells)" << endl;
	PEASUtils::PostEddyAlignShells(clo,true,sm);
      }
      prof.EndEntry(peas_key);
    }
  }

  //////////////////////////////////////////////////////////////////////
  // Do final check (and possibly replacement) of outliers with ff=1
  // (very little Q-space smoothing).
  //////////////////////////////////////////////////////////////////////
  if (clo.RegisterDWI()) {
    if (clo.Verbose()) cout << "Performing final outlier check" << endl;
    double old_hypar_ff = 1.0;
    if (clo.HyParFudgeFactor() != 1.0) { old_hypar_ff = clo.HyParFudgeFactor(); clo.SetHyParFudgeFactor(1.0); }
    double folc_key = prof.StartEntry("Calling FinalOLCheck");
    dwi_rm = FinalOLCheck(clo,dwi_rm,sm);
    prof.EndEntry(folc_key);
    if (old_hypar_ff != 1.0) clo.SetHyParFudgeFactor(old_hypar_ff);
    // Write outlier information
    double wol_key  = prof.StartEntry("Writing outlier information");
    std::vector<unsigned int> i2i = sm.GetDwi2GlobalIndexMapping();
    json_out.outlier_report = dwi_rm->WriteReport(i2i,clo.OLReportFname(),!clo.SupressOldStyleTextFiles());
    json_out.outlier_maps = dwi_rm->WriteMatrixReport(i2i,sm.NScans(),clo.OLMapReportFname(),clo.OLNStDevMapReportFname(),
						      clo.OLNSqrStDevMapReportFname(),!clo.SupressOldStyleTextFiles());
    if (clo.WriteOutlierFreeData()) {
      if (clo.Verbose()) cout << "Running sm.WriteOutlierFreeData" << endl;
      sm.WriteOutlierFreeData(clo.OLFreeDataFname());
    }
    prof.EndEntry(wol_key);
  }

  // Add rotation. Hidden function. ONLY to be used to
  // test that rotation of b-vecs does the right thing.
  if (clo.DoTestRot()) {
    if (clo.Verbose()) cout << "Running sm.AddRotation" << endl;
    sm.AddRotation(clo.TestRotAngles());
  }

  // Write registration parameters
  if (clo.Verbose()) cout << "Running sm.WriteParameterFile" << endl;
  if (clo.RegisterDWI() && clo.Registerb0()) 
    json_out.parameters = sm.WriteParameterFile(clo.ParOutFname(),ScanType::Any,!clo.SupressOldStyleTextFiles());
  else if (clo.RegisterDWI()) 
    json_out.parameters = sm.WriteParameterFile(clo.ParOutFname(),ScanType::DWI,!clo.SupressOldStyleTextFiles());
  else json_out.parameters = sm.WriteParameterFile(clo.ParOutFname(),ScanType::b0,!clo.SupressOldStyleTextFiles());

  if (clo.EstimateLongEC()) {
    if (clo.Verbose()) cout << "Running sm.LongTimeConstantECModel().WriteReport" << endl;
    json_out.long_ec_parameters = sm.LongTimeConstantECModel().WriteReport(sm,clo.LongECOutFname(),!clo.SupressOldStyleTextFiles());
  }

  // Write movement-over-time file if SliceToVol registration was performed
  if (sm.IsSliceToVol()) {
    if (clo.Verbose()) cout << "Running sm.WriteMovementOverTimeFile" << endl;
    if (clo.RegisterDWI() && clo.Registerb0()) 
      json_out.movement_over_time = sm.WriteMovementOverTimeFile(clo.MovementOverTimeOutFname(),ScanType::Any,!clo.SupressOldStyleTextFiles());
    else if (clo.RegisterDWI()) 
      json_out.movement_over_time = sm.WriteMovementOverTimeFile(clo.MovementOverTimeOutFname(),ScanType::DWI,!clo.SupressOldStyleTextFiles());
    else json_out.movement_over_time = sm.WriteMovementOverTimeFile(clo.MovementOverTimeOutFname(),ScanType::b0,!clo.SupressOldStyleTextFiles());
  }

  // Write registered images
  if (clo.Verbose()) cout << "Running sm.WriteRegisteredImages" << endl;
  double wri_key = prof.StartEntry("Writing registered images");
  if (!clo.ReplaceOutliers()) { if (clo.Verbose()) { cout << "Running sm.RecycleOutliers" << endl; } sm.RecycleOutliers(); } // Bring back original data
  if (sm.IsSliceToVol()) { // If we need to get predictions to support the resampling
    NEWIMAGE::volume4D<float> pred;
    ScanType st;
    if (clo.RegisterDWI() && clo.Registerb0()) st=ScanType::Any; else if (clo.RegisterDWI()) st=ScanType::DWI; else st=ScanType::b0;
    GetPredictionsForResampling(clo,st,sm,pred);
    // Set resampling in slice direction to spline.
    EDDY::PolationPara old_pp = sm.GetPolation();
    EDDY::PolationPara new_pp = old_pp;
    if (old_pp.GetS2VInterp() != NEWIMAGE::spline) { new_pp.SetS2VInterp(NEWIMAGE::spline); sm.SetPolation(new_pp); }
    if (clo.RegisterDWI() && clo.Registerb0()) sm.WriteRegisteredImages(clo.IOutFname(),clo.OutMaskFname(),clo.ResamplingMethod(),clo.LSResamplingLambda(),clo.MaskOutput(),pred,clo.NumberOfThreads());
    else if (clo.RegisterDWI()) sm.WriteRegisteredImages(clo.IOutFname(),clo.OutMaskFname(),clo.ResamplingMethod(),clo.LSResamplingLambda(),clo.MaskOutput(),pred,clo.NumberOfThreads(),ScanType::DWI);
    else sm.WriteRegisteredImages(clo.IOutFname(),clo.OutMaskFname(),clo.ResamplingMethod(),clo.LSResamplingLambda(),clo.MaskOutput(),pred,clo.NumberOfThreads(),ScanType::b0);
    if (old_pp.GetS2VInterp() != NEWIMAGE::spline) sm.SetPolation(old_pp);
  }
  else {
    if (clo.RegisterDWI() && clo.Registerb0()) sm.WriteRegisteredImages(clo.IOutFname(),clo.OutMaskFname(),clo.ResamplingMethod(),clo.LSResamplingLambda(),clo.MaskOutput(),clo.NumberOfThreads());
    else if (clo.RegisterDWI()) sm.WriteRegisteredImages(clo.IOutFname(),clo.OutMaskFname(),clo.ResamplingMethod(),clo.LSResamplingLambda(),clo.MaskOutput(),clo.NumberOfThreads(),ScanType::DWI);
    else sm.WriteRegisteredImages(clo.IOutFname(),clo.OutMaskFname(),clo.ResamplingMethod(),clo.LSResamplingLambda(),clo.MaskOutput(),clo.NumberOfThreads(),ScanType::b0);
  }
  prof.EndEntry(wri_key);

  // Optionally write out a "data set" that consists of the GP predictions. This
  // was added to demonstrate to Kings that it makes b***er all difference.
  if (clo.WritePredictions() || clo.WriteScatterBrainPredictions()) {
    // First write predictions in model space
    NEWIMAGE::volume4D<float> pred;
    EDDY::ScanType st;
    std::vector<double> hypar;
    if (clo.RegisterDWI() && clo.Registerb0()) st=ScanType::Any; else if (clo.RegisterDWI()) st=ScanType::DWI; else st=ScanType::b0;
    if (clo.WritePredictions()) { // Do the "regular" fitting to the GP, but ensure spline interpolation in z if s2v
      if (clo.Verbose()) cout << "Running EDDY::GetPredictionsForResampling" << endl;
      double wpred_key = prof.StartEntry("Calculating and writing predictions");
      EDDY::PolationPara old_pol = sm.GetPolation();
      EDDY::PolationPara new_pol = old_pol;
      if (clo.IsSliceToVol() && new_pol.GetS2VInterp() != NEWIMAGE::spline) new_pol.SetS2VInterp(NEWIMAGE::spline);
      sm.SetPolation(new_pol);
      EddyCommandLineOptions tmp_clo = clo;
      if (!tmp_clo.RotateBVecsDuringEstimation()) tmp_clo.SetRotateBVecsDuringEstimation(true);
      hypar = GetPredictionsForResampling(tmp_clo,st,sm,pred);
      pred /= sm.ScaleFactor();
      NEWIMAGE::write_volume(pred,clo.PredictionsOutFname());
      sm.SetPolation(old_pol);
      // Next write predictions in scan space(s)
      pred = 0.0;
      hypar = GetPredictionsForResampling(clo,st,sm,pred);
      pred /= sm.ScaleFactor();
      for (int s=0; s<pred.tsize(); s++) {
	pred[s] = EddyUtils::TransformModelToScanSpace(pred[s],sm.Scan(s,st),sm.GetSuscHzOffResField(s,st));
      }
      NEWIMAGE::write_volume(pred,clo.PredictionsInScanSpaceOutFname());
      prof.EndEntry(wpred_key);
    }
    if (clo.WriteScatterBrainPredictions()) { // Write predictions using "scattered data approach"
      if (clo.Verbose()) cout << "Running EDDY::GetScatterBrainPredictions" << endl;
      double wspred_key = prof.StartEntry("Calculating and writing scatter brain predictions");
      GetScatterBrainPredictions(clo,st,sm,hypar,pred);
      pred /= sm.ScaleFactor();
      NEWIMAGE::write_volume(pred,clo.ScatterBrainPredictionsOutFname());
      prof.EndEntry(wspred_key);
    }
    if (clo.WriteVolumeScatterBrainPredictions()) { // Write predictions using "scattered data approach" but with volume based bvec rotation
      if (clo.Verbose()) cout << "Running EDDY::GetScatterBrainPredictions with volume based bvec rotation" << endl;
      double wspred_key = prof.StartEntry("Calculating and writing scatter brain predictions");
      GetScatterBrainPredictions(clo,st,sm,hypar,pred,true);
      pred /= sm.ScaleFactor();
      NEWIMAGE::write_volume(pred,clo.VolumeScatterBrainPredictionsOutFname());
      prof.EndEntry(wspred_key);
    }
  }

  // Optionally write an additional set of registered images, this time with outliers retained.
  // This was added for the benefit of the HCP.
  if (clo.ReplaceOutliers() && clo.WriteAdditionalResultsWithOutliersRetained()) {
    double wri_key = prof.StartEntry("Writing registered images with outliers retained");
    if (clo.Verbose()) cout << "Running sm.WriteRegisteredImages" << endl;
    if (clo.Verbose()) { cout << "Running sm.RecycleOutliers" << endl; }
    sm.RecycleOutliers(); // Bring back original data
    if (sm.IsSliceToVol()) { // If we need to get predictions to support the resampling
      NEWIMAGE::volume4D<float> pred;
      ScanType st;
      if (clo.RegisterDWI() && clo.Registerb0()) st=ScanType::Any; else if (clo.RegisterDWI()) st=ScanType::DWI; else st=ScanType::b0;
      GetPredictionsForResampling(clo,st,sm,pred);
      if (clo.RegisterDWI() && clo.Registerb0()) sm.WriteRegisteredImages(clo.AdditionalWithOutliersOutFname(),std::string(""),clo.ResamplingMethod(),clo.LSResamplingLambda(),clo.MaskOutput(),pred,clo.NumberOfThreads());
      else if (clo.RegisterDWI()) sm.WriteRegisteredImages(clo.AdditionalWithOutliersOutFname(),std::string(""),clo.ResamplingMethod(),clo.LSResamplingLambda(),clo.MaskOutput(),pred,clo.NumberOfThreads(),ScanType::DWI);
      else sm.WriteRegisteredImages(clo.AdditionalWithOutliersOutFname(),std::string(""),clo.ResamplingMethod(),clo.LSResamplingLambda(),clo.MaskOutput(),pred,clo.NumberOfThreads(),ScanType::b0);
    }
    else {
      if (clo.RegisterDWI() && clo.Registerb0()) sm.WriteRegisteredImages(clo.AdditionalWithOutliersOutFname(),std::string(""),clo.ResamplingMethod(),clo.LSResamplingLambda(),clo.MaskOutput(),clo.NumberOfThreads());
      else if (clo.RegisterDWI()) sm.WriteRegisteredImages(clo.AdditionalWithOutliersOutFname(),std::string(""),clo.ResamplingMethod(),clo.LSResamplingLambda(),clo.MaskOutput(),clo.NumberOfThreads(),ScanType::DWI);
      else sm.WriteRegisteredImages(clo.AdditionalWithOutliersOutFname(),std::string(""),clo.ResamplingMethod(),clo.LSResamplingLambda(),clo.MaskOutput(),clo.NumberOfThreads(),ScanType::b0);
    }
    prof.EndEntry(wri_key);
  }

  // Write EC fields
  if (clo.WriteFields()) {
    if (clo.Verbose()) cout << "Running sm.WriteECFields" << endl;
    if (clo.RegisterDWI() && clo.Registerb0()) sm.WriteECFields(clo.ECFOutFname());
    else if (clo.RegisterDWI()) sm.WriteECFields(clo.ECFOutFname(),ScanType::DWI);
    else sm.WriteECFields(clo.ECFOutFname(),ScanType::b0);
  }

  // Write rotated b-vecs
  if (clo.WriteRotatedBVecs()) {
    if (clo.Verbose()) cout << "Running sm.WriteRotatedBVecs" << endl;
    sm.WriteRotatedBVecs(clo.RotatedBVecsOutFname());
  }

  // Write b-vecs for LSR resampling
  if (clo.ResamplingMethod() == FinalResamplingType::LSR) {
    if (clo.Verbose()) cout << "Running sm.WriteBVecsForLSR" << endl;
    sm.WriteBVecsForLSR(clo.BVecsLSROutFname(),false);
    if (clo.WriteRotatedBVecs()) {
      if (clo.Verbose()) cout << "Running sm.WriteBVecsForLSR with rotation" << endl;
      sm.WriteBVecsForLSR(clo.RotatedBVecsLSROutFname(),true);
    }
  }


  // Write movement RMS
  if (clo.WriteMovementRMS()) {
    double rms_key = prof.StartEntry("Writing RMS");
    if (clo.Verbose()) cout << "Running sm.WriteMovementRMS" << endl;
    if (clo.RegisterDWI() && clo.Registerb0()) { 
      json_out.movement_rms = sm.WriteMovementRMS(clo.RMSOutFname(),ScanType::Any,!clo.SupressOldStyleTextFiles()); 
      json_out.restricted_movement_rms = sm.WriteRestrictedMovementRMS(clo.RestrictedRMSOutFname(),ScanType::Any,!clo.SupressOldStyleTextFiles()); 
    }
    else if (clo.RegisterDWI()) { 
      json_out.movement_rms = sm.WriteMovementRMS(clo.RMSOutFname(),ScanType::DWI,!clo.SupressOldStyleTextFiles()); 
      json_out.restricted_movement_rms = sm.WriteRestrictedMovementRMS(clo.RestrictedRMSOutFname(),ScanType::DWI,!clo.SupressOldStyleTextFiles()); 
    }
    else { 
      json_out.movement_rms = sm.WriteMovementRMS(clo.RMSOutFname(),ScanType::b0,!clo.SupressOldStyleTextFiles()); 
      json_out.restricted_movement_rms = sm.WriteRestrictedMovementRMS(clo.RestrictedRMSOutFname(),ScanType::b0,!clo.SupressOldStyleTextFiles()); 
    }
    prof.EndEntry(rms_key);
  }

  // Write the .json file
  json_out.Write(clo.JsonOutFname());

  // Write CNR maps
  if (clo.WriteCNRMaps() || clo.WriteRangeCNRMaps() || clo.WriteResiduals()) {
    double cnr_key = prof.StartEntry("Writing CNR maps");
    double old_hypar_ff = 1.0;
    if (clo.HyParFudgeFactor() != 1.0) { old_hypar_ff = clo.HyParFudgeFactor(); clo.SetHyParFudgeFactor(1.0); }
    if (clo.Verbose()) cout << "Running EDDY::WriteCNRMaps" << endl;
    WriteCNRMaps(clo,sm,clo.CNROutFname(),clo.RangeCNROutFname(),clo.ResidualsOutFname());
    if (old_hypar_ff != 1.0) clo.SetHyParFudgeFactor(old_hypar_ff);
    prof.EndEntry(cnr_key);
  }

  // Write 3D displacement fields
  if (clo.WriteDisplacementFields()) {
    if (clo.Verbose()) cout << "Running sm.WriteDisplacementFields" << endl;
    if (clo.RegisterDWI() && clo.Registerb0()) sm.WriteDisplacementFields(clo.DFieldOutFname());
    else if (clo.RegisterDWI()) sm.WriteDisplacementFields(clo.DFieldOutFname(),ScanType::DWI);
    else sm.WriteDisplacementFields(clo.DFieldOutFname(),ScanType::b0);
  }

  prof.EndEntry(total_key);
  return(EXIT_SUCCESS);
}
catch(const std::exception& e)
{
  cout << "EDDY::: Eddy failed with message " << e.what() << endl;
  return(EXIT_FAILURE);
}
catch(...)
{
  cout << "EDDY::: Eddy failed" << endl;
  return(EXIT_FAILURE);
}

namespace EDDY {

/****************************************************************//**
*
*  A very high-level global function that registers all the scans
*  (b0 and dwis) in sm using a volume-to-volume model.
*  \param[in] clo Carries information about the command line options
*  that eddy was invoked with.
*  \param[in,out] sm Collection of all scans. Will be updated by this call.
*  \return A ptr to ReplacementManager that details which slices in
*  which dwi scans were replaced by their expectations.
*
********************************************************************/
ReplacementManager *DoVolumeToVolumeRegistration(// Input
						 const EddyCommandLineOptions&  clo,
						 // Input/Output
						 ECScanManager&                 sm) EddyTry
{
  static Utilities::FSLProfiler prof("_"+string(__FILE__)+"_"+string(__func__));
  double total_key = prof.StartEntry("Total");
  if (clo.IsDiffusion()) {
    // Start by registering the b0 scans
    NEWMAT::Matrix b0_mss, b0_ph;
    ReplacementManager *b0_rm = nullptr;
    if (clo.NIter() && clo.Registerb0() && sm.NScans(ScanType::b0)>1) {
      double b0_key = prof.StartEntry("b0");
      if (clo.Verbose()) cout << "Running Register" << endl;
      b0_rm = Register(clo,ScanType::b0,clo.NIter(),clo.FWHM(),clo.b0_SecondLevelModel(),false,sm,b0_rm,b0_mss,b0_ph);
      if (clo.IsSliceToVol()) { // Set scan for shape reference if we are to do also slice-to-volume registration
	if (clo.ShapeReferencesSet()) { // User has specified shape-references
	  if (clo.Verbose()) cout << "Setting user requested scan " << clo.B0ShapeReference() << " as b0 shape-reference."<< endl;
	  sm.SetB0ShapeReference(clo.B0ShapeReference());
	}
	else { // We need to find best scan for shape reference
	  double minmss=1e20;
	  unsigned int mindx=0;
	  for (unsigned int i=0; i<sm.NScans(ScanType::b0); i++) {
	    if (b0_mss(b0_mss.Nrows(),i+1) < minmss) { minmss=b0_mss(b0_mss.Nrows(),i+1); mindx=i; }
	  }
	  if (clo.Verbose()) cout << "Setting scan " << sm.Getb02GlobalIndexMapping(mindx) << " as b0 shape-reference."<< endl;
	  sm.SetB0ShapeReference(sm.Getb02GlobalIndexMapping(mindx));
	}
      }
      // Apply reference for location
      if (clo.Verbose()) cout << "Running sm.ApplyB0LocationReference" << endl;
      sm.ApplyB0LocationReference();
      prof.EndEntry(b0_key);
    }
    // See if we can use b0 movement estimates to inform dwi registration
    if (sm.B0sAreInterspersed() && sm.UseB0sToInformDWIRegistration() && clo.Registerb0() && clo.RegisterDWI() && clo.NIter() > 0) {
      if (clo.Verbose()) cout << "Running sm.PolateB0MovPar" << endl;
      sm.PolateB0MovPar();
    }
    // Now register the dwi scans
    NEWMAT::Matrix dwi_mss, dwi_ph;
    ReplacementManager *dwi_rm = NULL;
    if (clo.NIter() && clo.RegisterDWI()) {
      double dwi_key = prof.StartEntry("dwi");
      if (clo.Verbose()) cout << "Running Register" << endl;
      dwi_rm = Register(clo,ScanType::DWI,clo.NIter(),clo.FWHM(),clo.SecondLevelModel(),true,sm,dwi_rm,dwi_mss,dwi_ph);
      if (clo.IsSliceToVol()) { // Set scan for shape reference if we are to do also slice-to-volume registration
	if (clo.ShapeReferencesSet()) { // User has specified shape-reference
	  for (unsigned int i=0; i<sm.NoOfShells(ScanType::DWI); i++) {
	    std::pair<int,double> ref = clo.ShellShapeReference(i);
	    if (clo.Verbose()) cout << "Setting user requested scan " << ref.first << " as shell shape-reference for shell "<< i << " with b-value= " << ref.second << endl;
	    sm.SetShellShapeReference(i,ref.first);	  
	  }
	}
	else { // We need to find best scan for shape reference
	  std::vector<double> bvals;
	  std::vector<std::vector<unsigned int> > shindx = sm.GetShellIndicies(bvals);
	  for (unsigned int shell=0; shell<shindx.size(); shell++) {
	    double minmss=1e20;
	    unsigned int mindx=0;
	    bool found_vol_with_no_outliers=false;
	    for (unsigned int i=0; i<shindx[shell].size(); i++) {
	      if (!sm.Scan(shindx[shell][i]).HasOutliers()) { // Only consider scans without outliers
		found_vol_with_no_outliers=true;
		if (dwi_mss(dwi_mss.Nrows(),sm.GetGlobal2DWIIndexMapping(shindx[shell][i])+1) < minmss) {
		  minmss=dwi_mss(dwi_mss.Nrows(),sm.GetGlobal2DWIIndexMapping(shindx[shell][i])+1);
		  mindx=shindx[shell][i];
		}
	      }
	    }
	    if (!found_vol_with_no_outliers) {
	      std::vector<unsigned int> i2i = sm.GetDwi2GlobalIndexMapping();
	      dwi_rm->WriteReport(i2i,clo.OLReportFname());
	      dwi_rm->WriteMatrixReport(i2i,sm.NScans(),clo.OLMapReportFname(true),clo.OLNStDevMapReportFname(true),clo.OLNSqrStDevMapReportFname(true));
	      std::ostringstream errtxt;
	      errtxt << "DoVolumeToVolumeRegistration: Unable to find volume with no outliers in shell " << shell << " with b-value=" << bvals[shell];
	      throw EddyException(errtxt.str());
	    }
	    if (clo.Verbose()) cout << "Setting scan " << mindx << " as shell shape-reference for shell "<< shell << " with b-value= " << bvals[shell] << endl;
	    sm.SetShellShapeReference(shell,mindx);
	  }
	}
	// Apply reference for location
      }
      if (clo.Verbose()) cout << "Running sm.ApplyDWILocationReference" << endl;
      sm.ApplyDWILocationReference();
      prof.EndEntry(dwi_key);
    }
    // Write history of cost-function and parameter estimates if requested
    if (clo.NIter() && clo.History()) {
      if (clo.RegisterDWI()) {
	MISCMATHS::write_ascii_matrix(clo.DwiMssHistoryFname(),dwi_mss);
	MISCMATHS::write_ascii_matrix(clo.DwiParHistoryFname(),dwi_ph);
      }
      if (clo.Registerb0()) {
	MISCMATHS::write_ascii_matrix(clo.B0MssHistoryFname(),b0_mss);
	MISCMATHS::write_ascii_matrix(clo.B0ParHistoryFname(),b0_ph);
      }
    }
    prof.EndEntry(total_key);
    return(dwi_rm);
  }
  else { // If it is fMRI
    // Now register the dwi scans
    NEWMAT::Matrix fmri_mss, fmri_ph;
    ReplacementManager *fmri_rm = NULL;
    if (clo.NIter()) {
      double fmri_key = prof.StartEntry("fmri");
      if (clo.Verbose()) cout << "Running Register" << endl;
      fmri_rm = Register(clo,ScanType::fMRI,clo.NIter(),clo.FWHM(),clo.SecondLevelModel(),true,sm,fmri_rm,fmri_mss,fmri_ph);
      if (clo.IsSliceToVol()) { // Set scan for shape reference if we are to do also slice-to-volume registration
	if (clo.ShapeReferencesSet()) { // User has specified shape-references
	  if (clo.Verbose()) cout << "Setting user requested scan " << clo.fMRIShapeReference() << " as shape-reference."<< endl;
	  sm.SetfMRIShapeReference(clo.fMRIShapeReference());
	}
	else { // We need to find best scan for shape reference
	  double minmss=1e20;
	  unsigned int mindx=0;
	  bool found_vol_with_no_outliers=false;
	  for (unsigned int i=0; i<sm.NScans(ScanType::fMRI); i++) {
	    if (!sm.Scan(i,ScanType::fMRI).HasOutliers()) {
	      found_vol_with_no_outliers=true;
	      if (fmri_mss(fmri_mss.Nrows(),i+1) < minmss) {
		minmss = fmri_mss(fmri_mss.Nrows(),i+1);
		mindx = i;
	      }
	    }
	  }
	  if (!found_vol_with_no_outliers) {
	    std::vector<unsigned int> one2one(sm.NScans(ScanType::fMRI));
	    std::iota(one2one.begin(),one2one.end(),0);
	    fmri_rm->WriteReport(one2one,clo.OLReportFname());
	    fmri_rm->WriteMatrixReport(one2one,sm.NScans(),clo.OLMapReportFname(true),clo.OLNStDevMapReportFname(true),clo.OLNSqrStDevMapReportFname(true));
	    throw EddyException("DoVolumeToVolumeRegistration: Unable to find volume with no outliers.");
	  }
	  if (clo.Verbose()) cout << "Setting scan " << mindx << " as shape-reference."<< endl;
	  sm.SetfMRIShapeReference(mindx);
	}
	// Apply reference for location
      }
      if (clo.Verbose()) cout << "Running sm.ApplyDWILocationReference" << endl;
      sm.ApplyfMRILocationReference();
      prof.EndEntry(fmri_key);
    }
    // Write history of cost-function and parameter estimates if requested
    if (clo.NIter() && clo.History()) {
      MISCMATHS::write_ascii_matrix(clo.fMRIMssHistoryFname(),fmri_mss);
      MISCMATHS::write_ascii_matrix(clo.fMRIParHistoryFname(),fmri_ph);
    }
    prof.EndEntry(total_key);
    return(fmri_rm);  
  }
} EddyCatch


/****************************************************************//**
*
*  A very high-level global function that estimates long
*  time-constant eddy currents. It will alternate between estimating
*  the long EC parameters and updating the EC parameters.
*  \param[in] clo Carries information about the command line options
*  that eddy was invoked with.
*  \param[in,out] sm Collection of all scans. Will be updated by this call.
*  \param[in,out] rm A ptr to ReplacementManager that details which slices 
*  in which dwi scans were replaced by their expectations.
*  \return A ptr to ReplacementManager that details which slices in
*  which dwi scans were replaced by their expectations.
*
********************************************************************/
ReplacementManager *EstimateLongEC(// Input
				   const EddyCommandLineOptions&   clo,
				   // Input/Output
				   ECScanManager&                  sm,
				   ReplacementManager              *rm) EddyTry
{
  static Utilities::FSLProfiler prof("_"+string(__FILE__)+"_"+string(__func__));
  double total_key = prof.StartEntry("Total");
  if (sm.IsDiffusion()) {
    NEWIMAGE::volume<float> mask = sm.Scan(0).GetIma(); EddyUtils::SetTrilinearInterp(mask); mask = 1.0; // FOV mask in model space
    for (unsigned int iter=0; iter<clo.LongECNIter(); iter++) {
      #ifdef COMPILE_GPU
      // Load prediction maker in model space and make all the predictions
      // Note that for this we need to have predictions both for b0 and dwi
      std::shared_ptr<DWIPredictionMaker> b0_pmp = EddyGpuUtils::LoadPredictionMaker(clo,ScanType::b0,sm,iter,0.0,mask);      
      std::shared_ptr<DWIPredictionMaker> dwi_pmp = EddyGpuUtils::LoadPredictionMaker(clo,ScanType::DWI,sm,iter,0.0,mask);      
      std::vector<NEWIMAGE::volume<float> > pred(sm.NScans());
      for (GpuPredictorChunk c(sm,ScanType::b0); c<sm.NScans(ScanType::b0); c++) {
        std::vector<unsigned int> b0i = c.Indicies();
	std::vector<NEWIMAGE::volume<float> > chunk = b0_pmp->Predict(b0i);
	for (const unsigned int& i : b0i) pred[sm.Getb02GlobalIndexMapping(i)] = chunk[i]; 
      }
      for (GpuPredictorChunk c(sm,ScanType::DWI); c<sm.NScans(ScanType::DWI); c++) {
        std::vector<unsigned int> dwii = c.Indicies();
	std::vector<NEWIMAGE::volume<float> > chunk = dwi_pmp->Predict(dwii);
	for (const unsigned int& i : dwii) pred[sm.GetDwi2GlobalIndexMapping(i)] = chunk[i]; 
      }
      // Now estimate long EC parameters
      if (clo.DebugLevel()) {
        std::vector<double> mss = EddyGpuUtils::LongECParamUpdate(pred,mask,0.0,clo.VeryVerbose(),iter,clo.DebugLevel(),clo.DebugIndicies().GetIndicies(),sm);
      }
      else {
        std::vector<double> mss = EddyGpuUtils::LongECParamUpdate(pred,mask,0.0,clo.VeryVerbose(),sm);
      }
      #else
      // Load prediction maker in model space and make all the predictions
      // Note that for this we need to have predictions both for b0 and dwi
      std::shared_ptr<DWIPredictionMaker> b0_pmp = EDDY::LoadPredictionMaker(clo,ScanType::b0,sm,iter,0.0,mask);
      std::shared_ptr<DWIPredictionMaker> dwi_pmp = EDDY::LoadPredictionMaker(clo,ScanType::DWI,sm,iter,0.0,mask);
      std::vector<NEWIMAGE::volume<float> > pred(sm.NScans());
      for (unsigned int s=0; s<sm.NScans(ScanType::b0); s++) pred[sm.Getb02GlobalIndexMapping(s)] = b0_pmp->Predict(s);
      for (unsigned int s=0; s<sm.NScans(ScanType::DWI); s++) pred[sm.GetDwi2GlobalIndexMapping(s)] = dwi_pmp->Predict(s);
      // Caclulate parameter updates
      if (clo.DebugLevel()) {
        std::vector<double> mss = EddyUtils::LongECParamUpdate(pred,mask,0.0,clo.NumberOfThreads(),clo.VeryVerbose(),iter,clo.DebugLevel(),clo.DebugIndicies().GetIndicies(),sm);
      }
      else {
        std::vector<double> mss = EddyUtils::LongECParamUpdate(pred,mask,0.0,clo.NumberOfThreads(),clo.VeryVerbose(),sm);
      }
      #endif
      if (clo.ReestimateECWhenEstimatingLongEC()) {
        // Next update the "regular" movement and/or EC parameters
        ReplacementManager *b0_rm = nullptr;
        NEWMAT::Matrix b0_mss, b0_ph;
        b0_rm = Register(clo,ScanType::b0,1,std::vector<float>(1,0.0),clo.b0_SecondLevelModel(),false,sm,b0_rm,b0_mss,b0_ph);
        NEWMAT::Matrix dwi_mss, dwi_ph;
        rm = Register(clo,ScanType::DWI,1,std::vector<float>(1,0.0),clo.SecondLevelModel(),true,sm,rm,dwi_mss,dwi_ph);
      }
    }
  }
  else throw EddyException("EstimateLongEC: Called with fMRI data");
  prof.EndEntry(total_key);
  return(rm);
} EddyCatch

/****************************************************************//**
*
*  A very high-level global function that registers all the scans
*  (b0 and dwis) in sm using a slice-to-volume model.
*  \param[in] clo Carries information about the command line options
*  that eddy was invoked with.
*  \param[in,out] sm Collection of all scans. Will be updated by this call.
*  \return A ptr to ReplacementManager that details which slices in
*  which dwi scans were replaced by their expectations.
*
********************************************************************/

ReplacementManager *DoSliceToVolumeRegistration(// Input
						const EddyCommandLineOptions&  clo,
						unsigned int                   oi,        // Order index
						bool                           dol,       // Detect outliers?
						// Input/Output
						ECScanManager&                 sm,
						ReplacementManager             *dwi_rm) EddyTry
{
  static Utilities::FSLProfiler prof("_"+string(__FILE__)+"_"+string(__func__));
  double total_key = prof.StartEntry("Total");
  if (sm.IsDiffusion()) {
    // Start by registering the b0 scans
    NEWMAT::Matrix b0_mss, b0_ph;
    ReplacementManager *b0_rm = NULL;
    if (clo.S2V_NIter(oi) && clo.Registerb0() && sm.NScans(ScanType::b0)>1) {
      double b0_key = prof.StartEntry("b0");
      if (clo.Verbose()) cout << "Running Register" << endl;
      b0_rm = Register(clo,ScanType::b0,clo.S2V_NIter(oi),clo.S2V_FWHM(oi),clo.b0_SecondLevelModel(),false,sm,b0_rm,b0_mss,b0_ph);
      // Set reference for shape
      if (clo.Verbose()) cout << "Running sm.ApplyB0ShapeReference" << endl;
      sm.ApplyB0ShapeReference();
      // Set reference for location
      if (clo.Verbose()) cout << "Running sm.ApplyB0LocationReference" << endl;
      sm.ApplyB0LocationReference();
      prof.EndEntry(b0_key);
    }
    // Now register the dwi scans
    NEWMAT::Matrix dwi_mss, dwi_ph;
    if (clo.S2V_NIter(oi) && clo.RegisterDWI()) {
      double dwi_key = prof.StartEntry("dwi");
      if (clo.Verbose()) cout << "Running Register" << endl;
      dwi_rm = Register(clo,ScanType::DWI,clo.S2V_NIter(oi),clo.S2V_FWHM(oi),clo.SecondLevelModel(),dol,sm,dwi_rm,dwi_mss,dwi_ph);
      // Set reference for shape
      if (clo.Verbose()) cout << "Running sm.ApplyShellShapeReference" << endl;
      for (unsigned int si=0; si<sm.NoOfShells(ScanType::DWI); si++) sm.ApplyShellShapeReference(si);
      // Set reference for location
      if (clo.Verbose()) cout << "Running sm.ApplyDWILocationReference" << endl;
      sm.ApplyDWILocationReference();
      prof.EndEntry(dwi_key);
    }
    // Write history of cost-function and parameter estimates if requested
    if (clo.S2V_NIter(oi) && clo.History()) {
      if (clo.RegisterDWI()) {
        MISCMATHS::write_ascii_matrix(clo.DwiMssS2VHistoryFname(),dwi_mss);
	MISCMATHS::write_ascii_matrix(clo.DwiParS2VHistoryFname(),dwi_ph);
      }
      if (clo.Registerb0()) {
	MISCMATHS::write_ascii_matrix(clo.B0MssS2VHistoryFname(),b0_mss);
        MISCMATHS::write_ascii_matrix(clo.B0ParS2VHistoryFname(),b0_ph);
      }
    }
    prof.EndEntry(total_key);
    return(dwi_rm);
  }
  else { // Assume fMRI
    NEWMAT::Matrix fmri_mss, fmri_ph;
    ReplacementManager *fmri_rm = NULL;
    double fmri_key = prof.StartEntry("fmri");
    if (clo.Verbose()) cout << "Running Register" << endl;
    fmri_rm = Register(clo,ScanType::fMRI,clo.S2V_NIter(oi),clo.S2V_FWHM(oi),clo.b0_SecondLevelModel(),false,sm,fmri_rm,fmri_mss,fmri_ph);
    // Set reference for shape
    if (clo.Verbose()) cout << "Running sm.ApplyfMRIShapeReference" << endl;
    sm.ApplyfMRIShapeReference();
    // Set reference for location
    if (clo.Verbose()) cout << "Running sm.ApplyfMRILocationReference" << endl;
    sm.ApplyfMRILocationReference();
    prof.EndEntry(fmri_key);
    // Write history of cost-function and parameter estimates if requested
    if (clo.S2V_NIter(oi) && clo.History()) {
      MISCMATHS::write_ascii_matrix(clo.fMRIMssS2VHistoryFname(),fmri_mss);
      MISCMATHS::write_ascii_matrix(clo.fMRIParS2VHistoryFname(),fmri_ph);
    }
    return(fmri_rm);
  }
} EddyCatch

/****************************************************************//**
*
*  A very high-level global function that estimates a receieve
*  bias field that is stationary in the scanner framework.
*  \param[in] clo Carries information about the command line options
*  that eddy was invoked with.
*  \param[in] ksp Knotspacing of spline representation of field.
*  Set to negative value for free-form field.
*  \param[in] lambda Weight of regularisation when estimating field.
*  \param[in,out] sm Collection of all scans. Will be updated by this call.
*
********************************************************************/
/*
void EstimateBiasField(// Input
		       const EddyCommandLineOptions&  clo,
		       double                         ksp,
		       double                         lambda,
		       // Input/output
		       ECScanManager&                 sm) EddyTry
{
  BiasFieldEstimator bfe;
  ScanType st[2] = {ScanType::b0, ScanType::DWI};
  NEWIMAGE::volume<float> mask = sm.Scan(0,ScanType::Any).GetIma(); EddyUtils::SetTrilinearInterp(mask); mask = 1.0; // FOV-mask in model space

  #ifdef COMPILE_GPU
  // Loop over b0s and DWIs
  for (unsigned int sti=0; sti<2; sti++) {
    if (sm.NScans(st[sti]) > 1) { // Will only have info on field if 2 or more locations
      std::shared_ptr<DWIPredictionMaker> pmp = EddyGpuUtils::LoadPredictionMaker(clo,st[sti],sm,0,0.0,mask);
      // First calculate the average location for this scan type to use as reference
      EDDY::ImageCoordinates mic = sm.Scan(0,st[sti]).SamplingPoints();
      for (unsigned int s=1; s<sm.NScans(st[sti]); s++) {
	mic += sm.Scan(s,st[sti]).SamplingPoints();
      }
      mic /= static_cast<float>(sm.NScans(st[sti]));
      bfe.SetRefScan(mask,mic);
      // Next loop over chunks of predictions and add scans
      for (GpuPredictorChunk c(sm,st[sti]); c<sm.NScans(st[sti]); c++) {
	std::vector<unsigned int> si = c.Indicies();
	std::vector<NEWIMAGE::volume<float> > pred = pmp->Predict(si);
	// Loop over scans within chunk
	for (unsigned int i=0; i<si.size(); i++) {
	  // EDDY::ImageCoordinates ic = EddyGpuUtils::GetCoordinatesInScannerSpace(sm.Scan(si[i],st[sti]));
	  // cout << "i = " << i << ", calling SamplingPoints()" << endl;
	  EDDY::ImageCoordinates ic = sm.Scan(si[i],st[sti]).SamplingPoints();
	  bfe.AddScan(pred[i],EddyGpuUtils::GetUnwarpedScan(sm.Scan(si[i],st[sti]),sm.GetSuscHzOffResField(si[i],st[sti]),true,sm.GetPolation()),mask,ic);
	}
      }
    }
  }
  #else
  // Loop over b0s and DWIs
  for (unsigned int sti=0; sti<2; sti++) {
    std::shared_ptr<DWIPredictionMaker> pmp = EDDY::LoadPredictionMaker(clo,st[sti],sm,0,0.0,mask);
    // Loop over scans
    #pragma omp parallel for
    for (int s=0; s<int(sm.NScans(st[sti])); s++) {
      // Get coordinates in scanner space
      EDDY::ImageCoordinates ic = sm.Scan(s,st[sti]).SamplingPoints();
      // Get prediction in model space
      NEWIMAGE::volume<float> pred = pmp->Predict(s);
      // Get original data in model space
      NEWIMAGE::volume<float> sm.Scan(s,st[sti]).GetUnwarpedIma(sm.GetSuscHzOffResField(s,st[sti]));
      // AddScan
      bfe.AddScan(pred,sm.Scan(s,st[sti]).GetUnwarpedIma(sm.GetSuscHzOffResField(s,st[sti])),mask,ic);
    }
  }
  #endif
  // Caclulate many fields
  // double iksp[] = {10.0};
  // double ilambda[] = {1e-10, 1e-6, 1e-2, 100, 1e6, 1e10, 1e14};
  // for (unsigned int i=0; i<7; i++) {
    // cout << "Calculating field with lambda = " << ilambda[i] << endl;
    // NEWIMAGE::volume<float> bfield = bfe.GetField(ilambda[i]);
    // char fname[256]; sprintf(fname,"bfield_%d",i);
    // NEWIMAGE::write_volume(bfield,fname);
  // }

  // Calculate field
  NEWIMAGE::volume<float> bfield = bfe.GetField(1e10);
  NEWIMAGE::write_volume(bfield,"bfield_1e10");

  // Set field
  sm.SetBiasField(bfield);

  // Do a second step where the unknown offset is calculated by
  // maximising the SNR/CNR of the corrected data.

  // float offset = EstimateBiasFieldOffset();

  return;
} EddyCatch
*/

/****************************************************************//**
*
*  A very high-level global function that estimates the offset
*  of an estimated bias field. It does that by finding the offset
*  that maximises a weighted sum of the SNR (b0-volumes) and CNR
*  (dwi-volumes) of the corrected data. It will return the estimated
*  offset, and also set it in the ScanManager. It will also set the
*  average of the bias field to one within the user defined mask.
*  \returns offset The estimated offset value
*  \param[in] clo Carries information about the command line options
*  that eddy was invoked with.
*  \param[in,out] sm Collection of all scans. Will be updated by this call.
*
********************************************************************/
float EstimatedBiasFieldOffset(// Input
			       const EddyCommandLineOptions&  clo,
			       // Input/output
			       ECScanManager&                 sm) EddyTry
{
  return(1.0);
} EddyCatch

/****************************************************************//**
*
*  A global function that registers the scans in sm.
*  \param[in] clo Carries information about the command line options
*  that eddy was invoked with.
*  \param[in] st Specifies if we should register the diffusion weighted
*  images or the b=0 images.
*  \param[in] niter Specifies how many iterations should be run
*  \param[in] slm Specifies if a 2nd level model should be used to
*  constrain the estimates.
*  \param[in] dol Detect outliers if true
*  \param[in,out] sm Collection of all scans. Will be updated by this call.
*  \param[in,out] rm Pointer to ReplacementManager. If NULL on input
*  one will be allocated and the new pointer value passed on return.
*  \param[out] msshist Returns the history of the mss. msshist(i,j)
*  contains the mss for the jth scan on the ith iteration.
*  \param[out] phist Returns the history of the estimated parameters.
*  phist(i,j) contains the jth parameter on the ith iteration
*  \return A ptr to ReplacementManager that details which slices in
*  which scans were replaced by their expectations.
*
********************************************************************/
ReplacementManager *Register(// Input
			     const EddyCommandLineOptions&  clo,
			     ScanType                       st,
			     unsigned int                   niter,
			     const std::vector<float>&      fwhm,
			     SecondLevelECModelType         slm,
			     bool                           dol,
			     // Input/Output
			     ECScanManager&                 sm,
			     ReplacementManager             *rm,
			     // Output
			     NEWMAT::Matrix&                msshist,
			     NEWMAT::Matrix&                phist) EddyTry
{
  static Utilities::FSLProfiler prof("_"+string(__FILE__)+"_"+string(__func__));
  double total_key = prof.StartEntry("Total");
  msshist.ReSize(niter,sm.NScans(st));
  phist.ReSize(niter,sm.NScans(st)*sm.Scan(0,st).NParam());
  double *mss_tmp = new double[sm.NScans(st)]; // Replace by vector
  if (rm == NULL) { // If no replacement manager passed in
    if (clo.OLSummaryStatistics() == OLSumStats::ShellWise) {
      std::vector<double> bvals;
      std::vector<std::vector<unsigned int> > shi = sm.GetDWIShellIndicies(bvals);
      rm = new ReplacementManager(shi,bvals,static_cast<unsigned int>(sm.Scan(0,st).GetIma().zsize()),clo.OLDef(),clo.OLErrorType(),clo.OLType(),clo.MultiBand());
    }
    else rm = new ReplacementManager(sm.NScans(st),static_cast<unsigned int>(sm.Scan(0,st).GetIma().zsize()),clo.OLDef(),clo.OLErrorType(),clo.OLType(),clo.MultiBand());
  }
  NEWIMAGE::volume<float> mask = sm.Scan(0,st).GetIma(); EddyUtils::SetTrilinearInterp(mask); mask = 1.0; // FOV-mask in model space

  for (unsigned int iter=0; iter<niter; iter++) {
    double load_key = prof.StartEntry("LoadPredictionMaker");
    // Load prediction maker in model space
    #ifdef COMPILE_GPU
    std::shared_ptr<DWIPredictionMaker> pmp = EddyGpuUtils::LoadPredictionMaker(clo,st,sm,iter,fwhm[iter],mask);
    if (clo.DebugLevel() > 2) EDDY::WritePMDebugInfo(pmp,clo,st,iter,"GPU_","before_repol");
    #else
    std::shared_ptr<DWIPredictionMaker> pmp = EDDY::LoadPredictionMaker(clo,st,sm,iter,fwhm[iter],mask);
    if (clo.DebugLevel() > 2) EDDY::WritePMDebugInfo(pmp,clo,st,iter,"","before_repol");
    #endif
    prof.EndEntry(load_key);
    // Detect outliers and replace them
    DiffStatsVector stats(sm.NScans(st));
    if (dol) {
      double dol_key = prof.StartEntry("Detecting and replacing outliers");
      std::shared_ptr<DWIPredictionMaker> od_pmp;
      #ifdef COMPILE_GPU
      od_pmp = pmp;
      if (clo.DebugLevel()) stats = EddyGpuUtils::DetectOutliers(clo,st,od_pmp,mask,sm,iter,clo.DebugLevel(),*rm);
      else stats = EddyGpuUtils::DetectOutliers(clo,st,od_pmp,mask,sm,*rm);
      if (iter) {
	EddyGpuUtils::ReplaceOutliers(clo,st,od_pmp,mask,*rm,false,sm);
	// EddyGpuUtils::UpdatePredictionMaker(clo,st,sm,rm,mask,pmp);
        pmp = EddyGpuUtils::LoadPredictionMaker(clo,st,sm,iter,fwhm[iter],mask);
	if (clo.DebugLevel() > 2) EDDY::WritePMDebugInfo(pmp,clo,st,iter,"GPU_","after_repol");
      }
      #else
      od_pmp = pmp;
      if (clo.DebugLevel()) stats = EDDY::DetectOutliers(clo,st,od_pmp,mask,sm,iter,clo.DebugLevel(),*rm);
      else stats = EDDY::DetectOutliers(clo,st,od_pmp,mask,sm,*rm);
      if (iter) {
	EDDY::ReplaceOutliers(clo,st,od_pmp,mask,*rm,false,sm);
	// EDDY::UpdatePredictionMaker(clo,st,sm,rm,mask,pmp);
	pmp = EDDY::LoadPredictionMaker(clo,st,sm,iter,fwhm[iter],mask);
	if (clo.DebugLevel() > 2) EDDY::WritePMDebugInfo(pmp,clo,st,iter,"","after_repol");
      }
      #endif
      prof.EndEntry(dol_key);
    }
    //
    // Calculate the parameter updates
    // Note that this section will proceed in three different
    // ways depending on if it is run in single processor mode,
    // multi processor mode (OpenMP) or with a GPU-box (CUDA).
    //
    if (clo.Verbose()) cout << "Calculating parameter updates" << endl;
    double update_key = prof.StartEntry("Updating parameters");
    #ifdef COMPILE_GPU
    for (GpuPredictorChunk c(sm,st); c<sm.NScans(st); c++) {
      double predict_chunk_key = prof.StartEntry("Predicting chunk");
      std::vector<unsigned int> si = c.Indicies();
      if (clo.VeryVerbose()) cout << "Making predictions for scans: " << c << endl;
      std::vector<NEWIMAGE::volume<float> > pred = pmp->Predict(si);
      prof.EndEntry(predict_chunk_key);
      if (clo.VeryVerbose()) cout << "Finished making predictions for scans: " << c << endl;
      double update_chunk_key = prof.StartEntry("Updating parameters for chunk");
      for (unsigned int i=0; i<si.size(); i++) {
        unsigned int global_indx = sm.GetGlobalIndex(si[i],st);
	if (clo.DebugLevel() && clo.DebugIndicies().IsAmongIndicies(global_indx)) {
	  mss_tmp[si[i]] = EddyGpuUtils::MovAndECParamUpdate(pred[i],sm.GetSuscHzOffResField(si[i],st),sm.GetBiasField(),mask,fwhm[iter],clo.VeryVerbose(),global_indx,iter,clo.DebugLevel(),sm.Scan(si[i],st));
	}
	else mss_tmp[si[i]] = EddyGpuUtils::MovAndECParamUpdate(pred[i],sm.GetSuscHzOffResField(si[i],st),sm.GetBiasField(),mask,fwhm[iter],clo.VeryVerbose(),global_indx,sm.Scan(si[i],st));
	if (clo.VeryVerbose()) {
	  std::cout << "Iter: " << iter << ", scan: " << global_indx << ", gpu_mss = " << mss_tmp[si[i]] << std::endl << std::flush;
	}
      }
      prof.EndEntry(update_chunk_key);
    }
    #else
    if (clo.NumberOfThreads() == 1) { // If we are to run single threaded
      for (int s=0; s<int(sm.NScans(st)); s++) {
        // Get prediction in model space
        NEWIMAGE::volume<float> pred = pmp->Predict(s);
        // Update parameters
        unsigned int global_indx = sm.GetGlobalIndex(s,st);
        if (clo.DebugLevel() && clo.DebugIndicies().IsAmongIndicies(global_indx)) {
	  mss_tmp[s] = EddyUtils::MovAndECParamUpdate(pred,sm.GetSuscHzOffResField(s,st),sm.GetBiasField(),mask,fwhm[iter],clo.VeryVerbose(),global_indx,iter,clo.DebugLevel(),sm.Scan(s,st));
        }
        else mss_tmp[s] = EddyUtils::MovAndECParamUpdate(pred,sm.GetSuscHzOffResField(s,st),sm.GetBiasField(),mask,fwhm[iter],clo.VeryVerbose(),sm.Scan(s,st));
        if (clo.VeryVerbose()) {
	  std::cout << "Iter: " << iter << ", scan: " << global_indx << ", mss = " << mss_tmp[s] << std::endl << std::flush;
	}
      }
    }
    else { // We are to run multi-threaded
      // Decide number of volumes per thread
      std::vector<unsigned int> nvols = EddyUtils::ScansPerThread(sm.NScans(st),clo.NumberOfThreads());
      // Next spawn threads to do the calculations
      std::vector<std::thread> threads(clo.NumberOfThreads()-1); // + main thread makes clo.NumberOfThreads()
      for (unsigned int i=0; i<clo.NumberOfThreads()-1; i++) {
	threads[i] = std::thread(MovAndECParamUpdateWrapper,nvols[i],nvols[i+1],std::ref(clo),
				 pmp,std::ref(mask),st,fwhm[iter],iter,std::ref(sm),mss_tmp);
      }
      MovAndECParamUpdateWrapper(nvols[clo.NumberOfThreads()-1],nvols[clo.NumberOfThreads()],clo,pmp,mask,st,fwhm[iter],iter,sm,mss_tmp);
      std::for_each(threads.begin(),threads.end(),std::mem_fn(&std::thread::join));
    }
    #endif
    prof.EndEntry(update_key);

    // Print/collect some information that can be used for diagnostics
    Diagnostics(clo,iter,st,sm,mss_tmp,stats,*rm,msshist,phist);

    // Maybe use model based EC parameters
    if (slm != SecondLevelECModelType::None) {
      if (clo.VeryVerbose()) cout << "Performing 2nd level modelling of estimated parameters" << endl;
      sm.SetPredictedECParam(st,slm);
    }

    // Maybe try and separate field-offset and translation in PE direction
    if (clo.SeparateOffsetFromMovement()) {
      if (clo.VeryVerbose()) cout << "Attempting to separate field-offset from subject movement" << endl;
      sm.SeparateFieldOffsetFromMovement(st,clo.OffsetModel());
    }
  }

  delete [] mss_tmp;
  prof.EndEntry(total_key);
  return(rm);
} EddyCatch

ReplacementManager *FinalOLCheck(// Input
				 const EddyCommandLineOptions&  clo,
				 // Input/output
				 ReplacementManager             *rm,
				 ECScanManager&                 sm) EddyTry
{
  static Utilities::FSLProfiler prof("_"+string(__FILE__)+"_"+string(__func__));
  double total_key = prof.StartEntry("Total");
  NEWIMAGE::volume<float> mask = sm.Scan(0,ScanType::DWI).GetIma(); EddyUtils::SetTrilinearInterp(mask); mask = 1.0; // FOV-mask in model space
  if (rm == NULL) {
    if (clo.OLSummaryStatistics() == OLSumStats::ShellWise) {
      std::vector<double> bvals;
      std::vector<std::vector<unsigned int> > shi = sm.GetDWIShellIndicies(bvals);
      rm = new ReplacementManager(shi,bvals,static_cast<unsigned int>(sm.Scan(0,ScanType::DWI).GetIma().zsize()),clo.OLDef(),clo.OLErrorType(),clo.OLType(),clo.MultiBand());
    }
    rm = new ReplacementManager(sm.NScans(ScanType::DWI),static_cast<unsigned int>(sm.Scan(0,ScanType::DWI).GetIma().zsize()),clo.OLDef(),clo.OLErrorType(),clo.OLType(),clo.MultiBand());
  }

  // Load prediction maker in model space
  double load_key = prof.StartEntry("LoadPredictionMaker");
  #ifdef COMPILE_GPU
  std::shared_ptr<DWIPredictionMaker> pmp = EddyGpuUtils::LoadPredictionMaker(clo,ScanType::DWI,sm,0,0.0,mask);
  #else
  std::shared_ptr<DWIPredictionMaker> pmp = EDDY::LoadPredictionMaker(clo,ScanType::DWI,sm,0,0.0,mask);
  #endif
  prof.EndEntry(load_key);
  // Detect outliers and replace them
  DiffStatsVector stats(sm.NScans(ScanType::DWI));
  bool add_noise = clo.AddNoiseToReplacements();
  double det_key = prof.StartEntry("DetectOutliers");
  #ifdef COMPILE_GPU
  stats = EddyGpuUtils::DetectOutliers(clo,ScanType::DWI,pmp,mask,sm,*rm);
  #else
  stats = EDDY::DetectOutliers(clo,ScanType::DWI,pmp,mask,sm,*rm);
  #endif
  prof.EndEntry(det_key);
  double rep_key = prof.StartEntry("ReplaceOutliers");
  #ifdef COMPILE_GPU
  EddyGpuUtils::ReplaceOutliers(clo,ScanType::DWI,pmp,mask,*rm,add_noise,sm);
  #else
  EDDY::ReplaceOutliers(clo,ScanType::DWI,pmp,mask,*rm,add_noise,sm);
  #endif
  prof.EndEntry(rep_key);
  prof.EndEntry(total_key);
  return(rm);
} EddyCatch

/****************************************************************//**
*
*  A global function that loads up a prediction maker with all scans
*  of a given type. It will load it with unwarped scans (given the
*  current estimates of the warps) as served up by sm.GetUnwarpedScan()
*  or sm.GetUnwarpedOrigScan() depending on the value of use_orig.
*  \param[in] clo Carries information about the command line options
*  that eddy was invoked with.
*  \param[in] st Specifies if we should register the diffusion weighted
*  images or the b=0 images. If it is set to DWI the function will return
*  an EDDY::DiffusionGP prediction maker and if it is set to b0 it will
*  return an EDDY::b0Predictor.
*  \param[in] sm Collection of all scans.
*  \param[out] mask Returns a mask that indicates the voxels where data
*  is present for all input scans in sm.
*  \param[in] use_orig If set to true it will load it with unwarped "original"
*  , i.e. un-smoothed, scans. Default is false.
*  \return A safe pointer to a DWIPredictionMaker that can be used to
*  make predictions about what the scans should look like in undistorted space.
*
********************************************************************/
std::shared_ptr<DWIPredictionMaker> LoadPredictionMaker(// Input
							const EddyCommandLineOptions& clo,
							ScanType                      st,
							const ECScanManager&          sm,
							unsigned int                  iter,
							float                         fwhm,
							// Output
							NEWIMAGE::volume<float>&      mask,
							// Optional input
							bool                          use_orig) EddyTry
{
  static Utilities::FSLProfiler prof("_"+string(__FILE__)+"_"+string(__func__));
  double total_key = prof.StartEntry("Total");
  std::shared_ptr<DWIPredictionMaker>  pmp;      // Prediction Maker Pointer
  std::shared_ptr<HyParCF> hpcf;                 // Hyper-parameter cost-function
  std::shared_ptr<HyParEstimator> hpe;           // Hyper-parameter estimator
  if (st==ScanType::DWI || (st==ScanType::fMRI && !clo.HyperParFixed())) { // If we actually need to estimate hyper-parameters
    if (clo.HyperParFixed()) hpe = std::shared_ptr<FixedValueHyParEstimator>(new FixedValueHyParEstimator(clo.HyperParValues()));
    else {
      if (clo.HyParCostFunction() == HyParCostFunctionType::CC) hpe = std::shared_ptr<CheapAndCheerfulHyParEstimator>(new CheapAndCheerfulHyParEstimator(clo.NVoxHp(),clo.InitRand()));
      else {
	if (clo.HyParCostFunction() == HyParCostFunctionType::MML) hpcf = std::shared_ptr<MMLHyParCF>(new MMLHyParCF);
	else if (clo.HyParCostFunction() == HyParCostFunctionType::CV) hpcf = std::shared_ptr<CVHyParCF>(new CVHyParCF);
	else if (clo.HyParCostFunction() == HyParCostFunctionType::GPP) hpcf = std::shared_ptr<GPPHyParCF>(new GPPHyParCF);
	else throw EddyException("LoadPredictionMaker: Unknown hyperparameter cost-function");
	hpe = std::shared_ptr<FullMontyHyParEstimator>(new FullMontyHyParEstimator(hpcf,clo.HyParFudgeFactor(),clo.NVoxHp(),clo.InitRand(),clo.VeryVerbose()));
      }
    }
  }
  if (st==ScanType::DWI) { // If diffusion weighted data
    std::shared_ptr<KMatrix> K;
    if (clo.CovarianceFunction() == CovarianceFunctionType::Spherical) K = std::shared_ptr<SphericalKMatrix>(new SphericalKMatrix(clo.DontCheckShelling()));
    else if (clo.CovarianceFunction() == CovarianceFunctionType::Exponential) K = std::shared_ptr<ExponentialKMatrix>(new ExponentialKMatrix(clo.DontCheckShelling()));
    else if (clo.CovarianceFunction() == CovarianceFunctionType::NewSpherical) K = std::shared_ptr<NewSphericalKMatrix>(new NewSphericalKMatrix(clo.DontCheckShelling()));
    else throw EddyException("LoadPredictionMaker: Unknown covariance function");
    pmp = std::shared_ptr<DWIPredictionMaker>(new DiffusionGP(K,hpe));  // GP
  }
  else if (st==ScanType::b0) pmp = std::shared_ptr<DWIPredictionMaker>(new b0Predictor);  // Silly mean predictor for b=0 data
  else if (st==ScanType::fMRI) {
    if (clo.CovarianceFunction() == CovarianceFunctionType::SquaredExponential) {
      if (clo.HyperParFixed()) pmp = std::shared_ptr<DWIPredictionMaker>(new b0Predictor); // Use mean predictor for now
      else {
	std::shared_ptr<KMatrix> K = std::shared_ptr<SqrExpKMatrix>(new SqrExpKMatrix());
	pmp = std::shared_ptr<DWIPredictionMaker>(new fmriPredictor(K,hpe));
      }
    }
    else throw EddyException("LoadPredictionMaker: Unknown covariance function");
  }
  else throw EddyException("LoadPredictionMaker: Invalid scan type");
  pmp->SetNoOfScans(sm.NScans(st));
  mask = sm.Scan(0,st).GetIma(); EddyUtils::SetTrilinearInterp(mask); mask = 1.0;

  double load_key = prof.StartEntry("Loading");
  if (clo.Verbose()) std::cout << "Loading prediction maker" << std::flush;
  if (clo.VeryVerbose()) std::cout << std::endl << "Scan:" << std::flush;
  if (clo.NumberOfThreads() == 1) { // If we are to run single threaded
    for (int s=0; s<int(sm.NScans(st)); s++) {
      if (clo.VeryVerbose()) std::cout << " " << sm.GetGlobalIndex(s,st) << std::flush;
      NEWIMAGE::volume<float> tmpmask = sm.Scan(s,st).GetIma();
      EddyUtils::SetTrilinearInterp(tmpmask); tmpmask = 1.0;
      if (use_orig) pmp->SetScan(sm.GetUnwarpedOrigScan(s,tmpmask,st),sm.Scan(s,st).GetDiffPara(clo.RotateBVecsDuringEstimation()),s);
      else pmp->SetScan(sm.GetUnwarpedScan(s,tmpmask,st),sm.Scan(s,st).GetDiffPara(clo.RotateBVecsDuringEstimation()),s);
      {
	mask *= tmpmask;
      }
    }
    if (clo.VeryVerbose()) std::cout << std::endl << std::flush;
  }
  else { // We are to run multi-threaded
    // Decide number of volumes per thread
    std::vector<unsigned int> nvols = EddyUtils::ScansPerThread(sm.NScans(st),clo.NumberOfThreads());
    // Next spawn threads to do the calculations
    std::vector<std::thread> threads(clo.NumberOfThreads()-1); // + main thread makes clo.NumberOfThreads()
    for (unsigned int i=0; i<clo.NumberOfThreads()-1; i++) {
      threads[i] = std::thread(SetUnwarpedScanWrapper,nvols[i],nvols[i+1],std::ref(clo),
			       std::ref(sm),st,use_orig,pmp,std::ref(mask));
    }
    SetUnwarpedScanWrapper(nvols[clo.NumberOfThreads()-1],nvols[clo.NumberOfThreads()],clo,sm,st,use_orig,pmp,mask);
    std::for_each(threads.begin(),threads.end(),std::mem_fn(&std::thread::join));
    if (clo.VeryVerbose()) std::cout << std::endl << std::flush;
  }
  prof.EndEntry(load_key);
  double eval_key = prof.StartEntry("Evaluating");
  if (clo.Verbose()) cout << endl << "Evaluating prediction maker model" << endl;
  pmp->EvaluateModel(sm.Mask()*mask,fwhm,clo.Verbose());
  prof.EndEntry(eval_key);
  if (clo.DebugLevel() > 2 && st==ScanType::DWI) {
    char fname[256];
    sprintf(fname,"EDDY_DEBUG_K_Mat_Data_%02d",iter);
    pmp->WriteMetaData(fname);
  }

  prof.EndEntry(total_key);
  return(pmp);
} EddyCatch

DiffStatsVector DetectOutliers(// Input
			       const EddyCommandLineOptions&               clo,
			       ScanType                                    st,
			       std::shared_ptr<const DWIPredictionMaker>   pmp,
			       const NEWIMAGE::volume<float>&              mask,
			       const ECScanManager&                        sm,
			       // Input/Output
			       ReplacementManager&                         rm) EddyTry
{
  return(detect_outliers(clo,st,pmp,mask,sm,0,0,rm));
} EddyCatch

DiffStatsVector DetectOutliers(// Input
			       const EddyCommandLineOptions&               clo,
			       ScanType                                    st,
			       std::shared_ptr<const DWIPredictionMaker>   pmp,
			       const NEWIMAGE::volume<float>&              mask,
			       const ECScanManager&                        sm,
			       unsigned int                                iter,
			       unsigned int                                dl, // Debug Level
			       // Input/Output
			       ReplacementManager&                         rm) EddyTry
{
  return(detect_outliers(clo,st,pmp,mask,sm,iter,dl,rm));
} EddyCatch


DiffStatsVector detect_outliers(// Input
                                const EddyCommandLineOptions&             clo,
			        ScanType                                  st,
			        std::shared_ptr<const DWIPredictionMaker> pmp,
			        const NEWIMAGE::volume<float>&            mask,
			        const ECScanManager&                      sm,
			        unsigned int                              iter,
			        unsigned int                              dl, 
			        // Input/Output
			        ReplacementManager&                       rm) EddyTry
{
  static Utilities::FSLProfiler prof("_"+string(__FILE__)+"_"+string(__func__));
  double total_key = prof.StartEntry("Total");
  if (clo.VeryVerbose()) cout << "Checking for outliers" << endl;
  // Generate slice-wise stats on difference between observation and prediction
  DiffStatsVector stats(sm.NScans(st));
  if (clo.NumberOfThreads() == 1) { // If we are to run single threaded
    for (int s=0; s<int(sm.NScans(st)); s++) {
      NEWIMAGE::volume<float> pred = pmp->Predict(s);
      stats[s] = EddyUtils::GetSliceWiseStats(clo,st,pred,sm.GetSuscHzOffResField(s,st),mask,sm,s,iter,dl);
    }
  }
  else { // We are running multi-threaded
    // Decide number of volumes per thread
    std::vector<unsigned int> nvols = EddyUtils::ScansPerThread(sm.NScans(st),clo.NumberOfThreads());
    // Next spawn threads to do the calculations
    std::vector<std::thread> threads(clo.NumberOfThreads()-1); // + main thread makes clo.NumberOfThreads()
    for (unsigned int i=0; i<clo.NumberOfThreads()-1; i++) {
      threads[i] = std::thread(GetSliceWiseStatsWrapper,nvols[i],nvols[i+1],std::ref(clo),
                               std::ref(sm),st,pmp,std::ref(mask),iter,dl,std::ref(stats));
    }
    GetSliceWiseStatsWrapper(nvols[clo.NumberOfThreads()-1],nvols[clo.NumberOfThreads()],clo,sm,st,pmp,mask,iter,dl,stats);
    std::for_each(threads.begin(),threads.end(),std::mem_fn(&std::thread::join));
  }
  // Detect outliers and update replacement manager
  rm.Update(stats);
  prof.EndEntry(total_key);
  if (dl) { // Note that we use a local "debug level" rather than clo.DebugLevel()
    std::string prefix("EDDY_DEBUG_");
    if (st==ScanType::b0) prefix += "b0";
    else if (st==ScanType::DWI) prefix += "DWI";  
    else if (st==ScanType::fMRI) prefix += "fMRI";
    char fname[256];
    sprintf(fname,"%s_repol_info_%02d.mat",prefix.c_str(),iter);
    rm.WriteDebugInfo(fname,sm.GetDwi2GlobalIndexMapping(),sm.NScans());
  }
  return(stats);
} EddyCatch

void ReplaceOutliers(// Input
		     const EddyCommandLineOptions&             clo,
		     ScanType                                  st,
		     std::shared_ptr<DWIPredictionMaker>       pmp,
		     const NEWIMAGE::volume<float>&            mask,
		     const ReplacementManager&                 rm,
		     bool                                      add_noise,
		     // Input/Output
		     ECScanManager&                            sm) EddyTry
{
  static Utilities::FSLProfiler prof("_"+string(__FILE__)+"_"+string(__func__));
  double total_key = prof.StartEntry("Total");
  // Replace outlier slices with their predictions
  if (clo.VeryVerbose()) cout << "Replacing outliers with predictions" << endl;
  if (clo.NumberOfThreads() == 1) { // If we are to run single threaded
    for (int s=0; s<int(sm.NScans(st)); s++) {
      std::vector<unsigned int> ol = rm.OutliersInScan(s); 
      if (ol.size()) { // If this scan has outlier slices
	if (clo.VeryVerbose()) {
	  std::cout << "Scan " << sm.GetGlobalIndex(s,st) << " has outlier slices:"; 
	  for (unsigned int i=0; i<ol.size(); i++) { std::cout << " " << ol[i]; }     
	  std::cout << std::endl << std::flush;
	}
	NEWIMAGE::volume<float> pred = pmp->Predict(s,true);
	if (add_noise) {
	  double vp = pmp->PredictionVariance(s,true);
	  double ve = pmp->ErrorVariance(s);
	  double stdev = std::sqrt(vp+ve) - std::sqrt(vp);
	  pred += EddyUtils::MakeNoiseIma(pred,0.0,stdev);
	}
	sm.Scan(s,st).SetAsOutliers(pred,sm.GetSuscHzOffResField(s,st),mask,ol);
      }
    }
  }
  else { // We are to run multi-threaded
    // Decide number of volumes per thread
    std::vector<unsigned int> nvols = EddyUtils::ScansPerThread(sm.NScans(st),clo.NumberOfThreads());
    // Next spawn threads to do the calculations
    std::vector<std::thread> threads(clo.NumberOfThreads()-1); // + main thread makes clo.NumberOfThreads()
    for (unsigned int i=0; i<clo.NumberOfThreads()-1; i++) {
      threads[i] = std::thread(SetAsOutliersWrapper,nvols[i],nvols[i+1],pmp,std::ref(mask),
			       std::ref(rm),st,clo.VeryVerbose(),add_noise,std::ref(sm));
    }
    SetAsOutliersWrapper(nvols[clo.NumberOfThreads()-1],nvols[clo.NumberOfThreads()],pmp,mask,rm,st,clo.VeryVerbose(),add_noise,sm);
    std::for_each(threads.begin(),threads.end(),std::mem_fn(&std::thread::join));
  }
  prof.EndEntry(total_key);
  return;
} EddyCatch

/****************************************************************//**
*
*  A global function that makes ff=1 predictions, in model space,
*  for all scans of type st.
*  \param[in] clo Carries information about the command line options
*  that eddy was invoked with.
*  \param[in] st Specifies if we should resample the diffusion weighted
*  images, the b=0 images or both.
*  \param[in] sm Collection of all scans.
*  \param[out] pred A 4D volume with predictions for all scans of the
*  type indicated by st.
*  \return The hyper-parameters used for the predictions
*
********************************************************************/
std::vector<double> GetPredictionsForResampling(// Input
						const EddyCommandLineOptions&    clo,
						ScanType                         st,
						const ECScanManager&             sm,
						// Output
						NEWIMAGE::volume4D<float>&       pred) EddyTry
{
  static Utilities::FSLProfiler prof("_"+string(__FILE__)+"_"+string(__func__));
  double total_key = prof.StartEntry("Total");
  pred.reinitialize(sm.Scan(0,st).GetIma().xsize(),sm.Scan(0,st).GetIma().ysize(),sm.Scan(0,st).GetIma().zsize(),sm.NScans(st));
  NEWIMAGE::copybasicproperties(sm.Scan(0,st).GetIma(),pred);
  NEWIMAGE::volume<float> mask = sm.Scan(0,st).GetIma(); EddyUtils::SetTrilinearInterp(mask); mask = 1.0; // FOV-mask in model space
  EddyCommandLineOptions lclo = clo;
  std::vector<double> hypar;
  if (lclo.HyParFudgeFactor() != 1.0) lclo.SetHyParFudgeFactor(1.0);
  if (st == ScanType::Any || st == ScanType::b0) {
    #ifdef COMPILE_GPU
    std::shared_ptr<DWIPredictionMaker> pmp = EddyGpuUtils::LoadPredictionMaker(lclo,ScanType::b0,sm,0,0.0,mask);
    #else
    std::shared_ptr<DWIPredictionMaker> pmp = EDDY::LoadPredictionMaker(lclo,ScanType::b0,sm,0,0.0,mask);
    #endif
    for (unsigned int s=0; s<sm.NScans(ScanType::b0); s++) {
      if (st == ScanType::b0) pred[s] = pmp->Predict(s,true);
      else pred[sm.Getb02GlobalIndexMapping(s)] = pmp->Predict(s,true);
    }
  }
  if (st == ScanType::Any || st == ScanType::DWI) {
    #ifdef COMPILE_GPU
    std::shared_ptr<DWIPredictionMaker> pmp = EddyGpuUtils::LoadPredictionMaker(lclo,ScanType::DWI,sm,0,0.0,mask);
    #else
    std::shared_ptr<DWIPredictionMaker> pmp = EDDY::LoadPredictionMaker(lclo,ScanType::DWI,sm,0,0.0,mask);
    #endif
    hypar = pmp->GetHyperPar();
    for (unsigned int s=0; s<sm.NScans(ScanType::DWI); s++) {
      if (st == ScanType::DWI) pred[s] = pmp->Predict(s,true);
      else pred[sm.GetDwi2GlobalIndexMapping(s)] = pmp->Predict(s,true);
    }
  }
  prof.EndEntry(total_key);
  return(hypar);
} EddyCatch

/****************************************************************//**
*
*  A global function that makes ff=1 predictions, in model space,
*  for all scans of type st. It differs from the "normal" way eddy
*  makes predictions in that it uses a "scattered data reconstruction"
*  approach.
*  \param[in] clo Carries information about the command line options
*  that eddy was invoked with.
*  \param[in] st Specifies if we should resample the diffusion weighted
*  images, the b=0 images or both.
*  \param[in] sm Collection of all scans.
*  \param[in] hypar Hyperparameters. Valid only for "spherical" covariance-
*  function.
*  \param[out] pred A 4D volume with predictions for all scans of the
*  type indicated by st.
*  \param[in] vwbvrot If true means that bvec rotation is per volume
*  instead of per slice/MB-group which is default.
*
********************************************************************/
void GetScatterBrainPredictions(// Input
                                const EddyCommandLineOptions&    clo,
				ScanType                         st,
				ECScanManager&                   sm,
				const std::vector<double>&       hypar,
				// Output
				NEWIMAGE::volume4D<float>&       pred,
				// Optional input
				bool                             vwbvrot) EddyTry
{
  static Utilities::FSLProfiler prof("_"+string(__FILE__)+"_"+string(__func__));
  double total_key = prof.StartEntry("Total");
  // Do some checking to ensure that the call and the input makes sense
  if (clo.CovarianceFunction() != CovarianceFunctionType::NewSpherical) {
    throw EddyException("EDDY::GetScatterBrainPredictions: Predictions only available for Spherical covariance function");
  }
  if (!clo.IsSliceToVol()) throw EddyException("EDDY::GetScatterBrainPredictions: Predictions only makes sense for slice-to-vol model");
  NEWIMAGE::volume<float> mask = sm.Scan(0,st).GetIma(); EddyUtils::SetTrilinearInterp(mask); mask = 1.0; // FOV-mask in model space
  std::vector<DiffPara> dp = sm.GetDiffParas(ScanType::DWI);
  NewSphericalKMatrix K(clo.DontCheckShelling());
  K.SetDiffusionPar(dp);
  if (hypar.size() != K.NoOfHyperPar()) throw EddyException("EDDY::GetScatterBrainPredictions: Incompatible hypar size");
  pred.reinitialize(sm.Scan(0,st).GetIma().xsize(),sm.Scan(0,st).GetIma().ysize(),sm.Scan(0,st).GetIma().zsize(),sm.NScans(st));
  NEWIMAGE::copybasicproperties(sm.Scan(0,st).GetIma(),pred);
  // The b0s predictions are just the means and unaffected by rotations
  // Should be updated to be closer to the way we calculate the predictions
  // for the diffusion weighted data.
  if (st==ScanType::b0 || st==ScanType::Any) {
    EDDY::PolationPara old_pol = sm.GetPolation();
    EDDY::PolationPara new_pol = old_pol;
    if (clo.IsSliceToVol() && new_pol.GetS2VInterp() != NEWIMAGE::trilinear) new_pol.SetS2VInterp(NEWIMAGE::trilinear);
    sm.SetPolation(new_pol);
    #ifdef COMPILE_GPU
    std::shared_ptr<DWIPredictionMaker> pmp = EddyGpuUtils::LoadPredictionMaker(clo,ScanType::b0,sm,0,0.0,mask);
    #else
    std::shared_ptr<DWIPredictionMaker> pmp = EDDY::LoadPredictionMaker(clo,ScanType::b0,sm,0,0.0,mask);
    #endif
    for (unsigned int s=0; s<sm.NScans(ScanType::b0); s++) {
      if (st == ScanType::b0) pred[s] = pmp->Predict(s,true);
      else pred[sm.Getb02GlobalIndexMapping(s)] = pmp->Predict(s,true);
    }
    sm.SetPolation(new_pol);
  }
  if (st==ScanType::DWI || st==ScanType::Any) {
    #ifdef COMPILE_GPU
    EddyGpuUtils::MakeScatterBrainPredictions(clo,sm,hypar,pred,vwbvrot);
    #else
    throw EddyException("EDDY::GetScatterBrainPredictions: Only implemented for GPU");
    #endif
  }
  prof.EndEntry(total_key);
  return;
} EddyCatch

/****************************************************************//**
*
*  A global function that calculates CNR-maps for the dwi volume,
*  SNR-maps for the b0 volumes and a 4D map of residuals.
*  for all scans of type st. All the output parameters are optional
*  and passing in a nullptr means that parameter is not calculated
*  or returned.
*  \param[in] clo Carries information about the command line options
*  that eddy was invoked with.
*  \param[in] sm Collection of all scans.
*  \param[out] std_cnr A 4D file with one CNR-map, calculated as
*  std(pred)/std(res), per shell.
*  \param[out] range_cnr A 4D file with one CNR-map,calculated as
*  range(pred)/std(res), per shell.
*  \param[out] b0_snr A 3D SNR-map for the b0-volumes
*  \param[out] residuals A 4D-map with the residuals for both dwis and b0s
*
********************************************************************/
void CalculateCNRMaps(// Input
		      const EddyCommandLineOptions&               clo,
		      const ECScanManager&                        sm,
		      // Output
		      std::shared_ptr<NEWIMAGE::volume4D<float> > std_cnr,
		      std::shared_ptr<NEWIMAGE::volume4D<float> > range_cnr,
                      std::shared_ptr<NEWIMAGE::volume<float> >   b0_snr,
		      std::shared_ptr<NEWIMAGE::volume4D<float> > residuals) EddyTry
{
  static Utilities::FSLProfiler prof("_"+string(__FILE__)+"_"+string(__func__));
  double total_key = prof.StartEntry("Total");
  if (std_cnr==nullptr && range_cnr==nullptr && b0_snr==nullptr && residuals==nullptr) {
    throw EddyException("EDDY::CalculateCNRMaps: At least one output parameter must be set.");
  }
  if (!sm.IsShelled()) {
    throw EddyException("EDDY::CalculateCNRMaps: Can only calculate CNR for shelled data.");
  }
  if ((std_cnr!=nullptr && (!NEWIMAGE::samesize(sm.Scan(0,ScanType::Any).GetIma(),(*std_cnr)[0]) || (*std_cnr).tsize() != sm.NoOfShells(ScanType::DWI))) ||
      (range_cnr!=nullptr && (!NEWIMAGE::samesize(sm.Scan(0,ScanType::Any).GetIma(),(*range_cnr)[0]) || (*range_cnr).tsize() != sm.NoOfShells(ScanType::DWI))) ||
      (b0_snr!=nullptr && !NEWIMAGE::samesize(sm.Scan(0,ScanType::Any).GetIma(),*b0_snr)) ||
      (residuals!=nullptr && (!NEWIMAGE::samesize(sm.Scan(0,ScanType::Any).GetIma(),(*residuals)[0]) || (*residuals).tsize() != sm.NScans(ScanType::Any)))) {
    throw EddyException("EDDY::CalculateCNRMaps: Size mismatch between sm and output containers.");
  }
  NEWIMAGE::volume<float> mask = sm.Scan(0,ScanType::DWI).GetIma(); // FOV-mask in model space
  EddyUtils::SetTrilinearInterp(mask); mask = 1.0;
  std::shared_ptr<DWIPredictionMaker> dwi_pmp;
  std::shared_ptr<DWIPredictionMaker> b0_pmp;
  if (std_cnr || range_cnr || residuals) {
    #ifdef COMPILE_GPU
    dwi_pmp = EddyGpuUtils::LoadPredictionMaker(clo,ScanType::DWI,sm,0,0.0,mask);
    #else
    dwi_pmp = EDDY::LoadPredictionMaker(clo,ScanType::DWI,sm,0,0.0,mask);
    #endif
  }
  if (b0_snr || residuals) {
    #ifdef COMPILE_GPU
    b0_pmp = EddyGpuUtils::LoadPredictionMaker(clo,ScanType::b0,sm,0,0.0,mask);
    #else
    b0_pmp = EDDY::LoadPredictionMaker(clo,ScanType::b0,sm,0,0.0,mask);
    #endif
  }
  if (std_cnr || range_cnr) { // Calculate and write shell-wise CNR maps
    std::vector<double>                      grpb;                                 // b-values of the different dwi shells
    std::vector<std::vector<unsigned int> >  dwi_indx = sm.GetShellIndicies(grpb); // Global indicies of dwi scans
    std::vector<NEWIMAGE::volume<float> >    mvols(grpb.size());                   // Volumes of mean predictions for the shells
    // Calculate mean volumes of predictions
    for (unsigned int i=0; i<grpb.size(); i++) {
      mvols[i] = dwi_pmp->Predict(sm.GetGlobal2DWIIndexMapping(dwi_indx[i][0]));
      for (unsigned int j=1; j<dwi_indx[i].size(); j++) {
	mvols[i] += dwi_pmp->Predict(sm.GetGlobal2DWIIndexMapping(dwi_indx[i][j]));
      }
      mvols[i] /= float(dwi_indx[i].size());
    }
    std::vector<NEWIMAGE::volume<float> >    stdvols(grpb.size());
    if (std_cnr) { // Calculate standard deviation volumes of predictions
      for (unsigned int i=0; i<grpb.size(); i++) {
	NEWIMAGE::volume<float> tmp = dwi_pmp->Predict(sm.GetGlobal2DWIIndexMapping(dwi_indx[i][0])) - mvols[i];
	stdvols[i] = tmp*tmp;
	for (unsigned int j=1; j<dwi_indx[i].size(); j++) {
	  tmp = dwi_pmp->Predict(sm.GetGlobal2DWIIndexMapping(dwi_indx[i][j])) - mvols[i];
	  stdvols[i] += tmp*tmp;
	}
	stdvols[i] /= float(dwi_indx[i].size()-1);
	stdvols[i] = NEWIMAGE::sqrt(stdvols[i]);
      }
    }
    std::vector<NEWIMAGE::volume<float> >    minvols(grpb.size());
    std::vector<NEWIMAGE::volume<float> >    maxvols(grpb.size());
    if (range_cnr) { // Caclculate range (max-min) volumes of predictions
      for (unsigned int i=0; i<grpb.size(); i++) {
	minvols[i] = dwi_pmp->Predict(sm.GetGlobal2DWIIndexMapping(dwi_indx[i][0]));
	maxvols[i] = minvols[i];
	for (unsigned int j=1; j<dwi_indx[i].size(); j++) {
	  minvols[i] = NEWIMAGE::min(minvols[i],dwi_pmp->Predict(sm.GetGlobal2DWIIndexMapping(dwi_indx[i][j])));
	  maxvols[i] = NEWIMAGE::max(maxvols[i],dwi_pmp->Predict(sm.GetGlobal2DWIIndexMapping(dwi_indx[i][j])));
	}
	maxvols[i] -= minvols[i]; // Maxvols now contain the range rather than the max
      }
    }
    // Calculate standard deviation of residuals
    std::vector<NEWIMAGE::volume<float> >  stdres(grpb.size());
    for (unsigned int i=0; i<grpb.size(); i++) {
      NEWIMAGE::volume<float> tmp = dwi_pmp->Predict(sm.GetGlobal2DWIIndexMapping(dwi_indx[i][0])) - dwi_pmp->InputData(sm.GetGlobal2DWIIndexMapping(dwi_indx[i][0]));
      stdres[i] = tmp*tmp;
      for (unsigned int j=1; j<dwi_indx[i].size(); j++) {
	tmp = dwi_pmp->Predict(sm.GetGlobal2DWIIndexMapping(dwi_indx[i][j])) - dwi_pmp->InputData(sm.GetGlobal2DWIIndexMapping(dwi_indx[i][j]));
	stdres[i] += tmp*tmp;
      }
      stdres[i] /= float(dwi_indx[i].size()-1);
      stdres[i] = NEWIMAGE::sqrt(stdres[i]);
    }
    // Divide prediction std/range with std of residuals to get CNR
    for (unsigned int i=0; i<grpb.size(); i++) {
      if (std_cnr) (*std_cnr)[i] = stdvols[i] / stdres[i];
      if (range_cnr) (*range_cnr)[i] = maxvols[i] / stdres[i];
    }
  }
  // Get the SNR of the b0s
  if (b0_snr) {
    if (sm.NScans(ScanType::b0) > 1) {
      std::vector<unsigned int>   b0_indx = sm.GetB0Indicies();
      *b0_snr = b0_pmp->Predict(0) - b0_pmp->InputData(0); // N.B. Predict(i) is mean of all b0 scans
      *b0_snr *= *b0_snr;
      for (unsigned int i=1; i<b0_indx.size(); i++) {
	NEWIMAGE::volume<float> tmp = b0_pmp->Predict(i) - b0_pmp->InputData(i);
	*b0_snr += tmp*tmp;
      }
      *b0_snr /= static_cast<float>(b0_indx.size()-1);
      *b0_snr = NEWIMAGE::sqrt(*b0_snr);
      *b0_snr = b0_pmp->Predict(0) / *b0_snr;
    }
    else (*b0_snr) = 0.0; // Set SNR to zero if we can't estimate it
  }
  // Get residuals
  if (residuals) {
    for (unsigned int i=0; i<sm.NScans(ScanType::DWI); i++) {
      (*residuals)[sm.GetDwi2GlobalIndexMapping(i)] = (dwi_pmp->InputData(i) - dwi_pmp->Predict(i)) / sm.ScaleFactor();
    }
    for (unsigned int i=0; i<sm.NScans(ScanType::b0); i++) {
      (*residuals)[sm.Getb02GlobalIndexMapping(i)] = (b0_pmp->InputData(i) - b0_pmp->Predict(i)) / sm.ScaleFactor();
    }
  }
  prof.EndEntry(total_key);

  return;
} EddyCatch

void WriteCNRMaps(// Input
		  const EddyCommandLineOptions&   clo,
		  const ECScanManager&            sm,
		  const std::string&              spatial_fname,
		  const std::string&              range_fname,
		  const std::string&              residual_fname) EddyTry
{
  static Utilities::FSLProfiler prof("_"+string(__FILE__)+"_"+string(__func__));
  double total_key = prof.StartEntry("Total");
  if (spatial_fname.empty() && residual_fname.empty()) throw EddyException("EDDY::WriteCNRMaps: At least one of spatial and residual fname must be set");

  // Allocate memory for the maps we are interested in
  std::shared_ptr<NEWIMAGE::volume4D<float> > std_cnr;
  std::shared_ptr<NEWIMAGE::volume4D<float> > range_cnr;
  std::shared_ptr<NEWIMAGE::volume<float> >   b0_snr;
  std::shared_ptr<NEWIMAGE::volume4D<float> > residuals;
  if (!spatial_fname.empty()) {
    const NEWIMAGE::volume<float>& tmp = sm.Scan(0,ScanType::Any).GetIma();
    std_cnr = std::make_shared<NEWIMAGE::volume4D<float> > (tmp.xsize(),tmp.ysize(),tmp.zsize(),sm.NoOfShells(ScanType::DWI));
    NEWIMAGE::copybasicproperties(tmp,*std_cnr);
    b0_snr = std::make_shared<NEWIMAGE::volume<float> > (tmp.xsize(),tmp.ysize(),tmp.zsize());
    NEWIMAGE::copybasicproperties(tmp,*b0_snr);
  }
  if (!range_fname.empty()) {
    const NEWIMAGE::volume<float>& tmp = sm.Scan(0,ScanType::Any).GetIma();
    range_cnr = std::make_shared<NEWIMAGE::volume4D<float> > (tmp.xsize(),tmp.ysize(),tmp.zsize(),sm.NoOfShells(ScanType::DWI));
    NEWIMAGE::copybasicproperties(tmp,*range_cnr);
    if (b0_snr==nullptr) {
      b0_snr = std::make_shared<NEWIMAGE::volume<float> > (tmp.xsize(),tmp.ysize(),tmp.zsize());
      NEWIMAGE::copybasicproperties(tmp,*b0_snr);
    }
  }
  if (!residual_fname.empty()) {
    const NEWIMAGE::volume<float>& tmp = sm.Scan(0,ScanType::Any).GetIma();
    residuals = std::make_shared<NEWIMAGE::volume4D<float> > (tmp.xsize(),tmp.ysize(),tmp.zsize(),sm.NScans(ScanType::Any));
    NEWIMAGE::copybasicproperties(tmp,*residuals);
  }
  // Calculate the maps we are interested in
  EDDY::CalculateCNRMaps(clo,sm,std_cnr,range_cnr,b0_snr,residuals);
  // Write them out
  if (!spatial_fname.empty()) {
    const NEWIMAGE::volume<float>& tmp = sm.Scan(0,ScanType::Any).GetIma();
    NEWIMAGE::volume4D<float> ovol(tmp.xsize(),tmp.ysize(),tmp.zsize(),sm.NoOfShells(ScanType::Any));
    NEWIMAGE::copybasicproperties(tmp,ovol);
    ovol[0] = *b0_snr;
    for (unsigned int i=0; i<sm.NoOfShells(ScanType::DWI); i++) ovol[i+1] = (*std_cnr)[i];
    NEWIMAGE::write_volume(ovol,spatial_fname);
  }
  if (!range_fname.empty()) {
    const NEWIMAGE::volume<float>& tmp = sm.Scan(0,ScanType::Any).GetIma();
    NEWIMAGE::volume4D<float> ovol(tmp.xsize(),tmp.ysize(),tmp.zsize(),sm.NoOfShells(ScanType::Any));
    NEWIMAGE::copybasicproperties(tmp,ovol);
    ovol[0] = *b0_snr;
    for (unsigned int i=0; i<sm.NoOfShells(ScanType::DWI); i++) ovol[i+1] = (*range_cnr)[i];
    NEWIMAGE::write_volume(ovol,range_fname);
  }
  if (!residual_fname.empty()) NEWIMAGE::write_volume(*residuals,residual_fname);
  prof.EndEntry(total_key);

  return;
} EddyCatch

/****************************************************************//**
*
*  A global function that performs an update of the movement and EC
*  parameters for a range of scans. It is a wrappper to facilitate using
*  the C++11 thread library for parallelisation.
*  \param[in] first_vol Index of first volume to process
*  \param[in] last_vol Index one past the last volume to process
*  \param[in] clo Carries information about the command line options
*  that eddy was invoked with.
*  \param[in] pmp A safe pointer to a DWIPredictionMaker that can be used to
*  make predictions about what the scans should look like in undistorted space.
*  \param[in] mask A mask specifying where predictions are valid
*  \param[in] st Specifies if we should update the diffusion weighted
*  images or the b=0 images.
*  parameters will be checked before acceptiong them.
*  \param[in] fwhm FWHM
*  \param[in] iter Specifies the iteration number.
*  \param[in/out] sm Collection of all scans.
*  \param[out] mss The mss for the volumes after applying new parameters.
*  \return None
*
********************************************************************/
void MovAndECParamUpdateWrapper(// Input
				unsigned int                              first_vol,
				unsigned int                              last_vol,
				const EddyCommandLineOptions&             clo,
				std::shared_ptr<const DWIPredictionMaker> pmp,
				const NEWIMAGE::volume<float>&            mask,
				ScanType                                  st,
				float                                     fwhm,
				unsigned int                              iter,
				// Input/output
				ECScanManager&                            sm,
				// Output
				double                                    *mss) EddyTry
{
  static std::mutex cout_mutex;

  for (unsigned int s=first_vol; s<last_vol; s++) {
    NEWIMAGE::volume<float> pred = pmp->Predict(s);
    unsigned int global_indx = sm.GetGlobalIndex(s,st);
    if (clo.DebugLevel() && clo.DebugIndicies().IsAmongIndicies(global_indx)) {
      mss[s] = EddyUtils::MovAndECParamUpdate(pred,sm.GetSuscHzOffResField(s,st),sm.GetBiasField(),mask,fwhm,clo.VeryVerbose(),global_indx,iter,clo.DebugLevel(),sm.Scan(s,st));
    }
    else {
      mss[s] = EddyUtils::MovAndECParamUpdate(pred,sm.GetSuscHzOffResField(s,st),sm.GetBiasField(),mask,fwhm,clo.VeryVerbose(),sm.Scan(s,st));
    }
    if (clo.VeryVerbose()) {
      cout_mutex.lock();
      std::cout << "Iter: " << iter << ", scan: " << global_indx << ", mss = " << mss[s] << std::endl << std::flush;
      cout_mutex.unlock();
    }
  }
} EddyCatch

/****************************************************************//**
*
*  A global function that gets an unwarped scan and loads it into a
*  prediction maker. It does this for a range of scans. It is a wrappper
*  to facilitate using the C++11 thread library for parallelisation.
*  \param[in] first_vol Index of first volume to process
*  \param[in] last_vol Index one past the last volume to process
*  \param[in] clo Carries information about the command line options
*  that eddy was invoked with.
*  \param[in] sm Collection of all scans.
*  \param[in] st Specifies if we should update the diffusion weighted
*  images or the b=0 images.
*  \param[in/out] pmp A safe pointer to a DWIPredictionMaker that is to
*  be loaded.
*  \param[in/out] mask A mask specifying where predictions are valid
*  \return None
*
********************************************************************/
void SetUnwarpedScanWrapper(// Input
			    unsigned int                              first_vol,
			    unsigned int                              last_vol,
			    const EddyCommandLineOptions&             clo,
			    const ECScanManager&                      sm,
			    ScanType                                  st,
			    bool                                      use_orig,
			    // Input/output
			    std::shared_ptr<DWIPredictionMaker>       pmp,
			    NEWIMAGE::volume<float>&                  mask) EddyTry
{
  static std::mutex cout_mutex;
  static std::mutex mask_mutex;

  for (unsigned int s=first_vol; s<last_vol; s++) {
    if (clo.VeryVerbose()) {
      cout_mutex.lock();
      std::cout << " " << sm.GetGlobalIndex(s,st) << std::flush;
      cout_mutex.unlock();
    }

    NEWIMAGE::volume<float> tmpmask = sm.Scan(s,st).GetIma();
    EddyUtils::SetTrilinearInterp(tmpmask); tmpmask = 1.0;
    if (use_orig) pmp->SetScan(sm.GetUnwarpedOrigScan(s,tmpmask,st),sm.Scan(s,st).GetDiffPara(clo.RotateBVecsDuringEstimation()),s);
    else pmp->SetScan(sm.GetUnwarpedScan(s,tmpmask,st),sm.Scan(s,st).GetDiffPara(clo.RotateBVecsDuringEstimation()),s);

    mask_mutex.lock();
    mask *= tmpmask;
    mask_mutex.unlock();
  }
} EddyCatch

/****************************************************************//**
*
*  A global function that calculates slice wise stats for a range
*  of scans. It is a wrappper to facilitate using the C++11 thread
*  library for parallelisation.
*  \param[in] first_vol Index of first volume to process
*  \param[in] last_vol Index one past the last volume to process
*  \param[in] sm Collection of all scans.
*  \param[in] pmp A safe pointer to a DWIPredictionMaker that is to
*  be loaded.
*  \param[in] mask A mask to limit the area for which to calculate stats.
*  \param[in] st Specifies if we should update the diffusion weighted
*  images or the b=0 images.
*  \param[out] stats A vector of slice-wise stats.
*  \return None
*
********************************************************************/
void GetSliceWiseStatsWrapper(// Input
			      unsigned int                              first_vol,
			      unsigned int                              last_vol,
			      const EddyCommandLineOptions&             clo,
			      const ECScanManager&                      sm,
			      ScanType                                  st,
			      std::shared_ptr<const DWIPredictionMaker> pmp,
			      const NEWIMAGE::volume<float>&            mask,
			      unsigned int                              iter,
			      unsigned int                              dl,
			      // Output
			      DiffStatsVector&                          stats) EddyTry
{
  for (unsigned int s=first_vol; s<last_vol; s++) {
    NEWIMAGE::volume<float> pred = pmp->Predict(s);
    stats[s] = EddyUtils::GetSliceWiseStats(clo,st,pred,sm.GetSuscHzOffResField(s,st),mask,sm,s,iter,dl);
  }
} EddyCatch


/****************************************************************//**
*
*  A global function that replaces outliers with predictions for a range
*  of scans. It is a wrappper to facilitate using the C++11 thread
*  library for parallelisation.
*  \param[in] first_vol Index of first volume to process
*  \param[in] last_vol Index one past the last volume to process
*  \param[in] pmp A safe pointer to a DWIPredictionMaker that is to
*  be loaded.
*  \param[i] mask
*  \param[in] st Specifies if we should update the diffusion weighted
*  images or the b=0 images.
*  \param[in] very_verbose Very Verbose flag
*  \param[in] add_noise Decides of noise should be added to predictions.
*  \param[in/out] sm Collection of all scans.
*  \return None
*
********************************************************************/
void SetAsOutliersWrapper(// Input
			  unsigned int                              first_vol,
			  unsigned int                              last_vol,
			  std::shared_ptr<DWIPredictionMaker>       pmp,
			  const NEWIMAGE::volume<float>&            mask,
			  const ReplacementManager&                 rm,
			  ScanType                                  st,
			  bool                                      very_verbose,
			  bool                                      add_noise,
			  // Input/output
			  ECScanManager&                            sm) EddyTry
{
  static std::mutex cout_mutex;

  for (unsigned int s=first_vol; s<last_vol; s++) {
    std::vector<unsigned int> ol = rm.OutliersInScan(s);
    if (ol.size()) { // If the scan has outliers
      if (very_verbose) {
	cout_mutex.lock();
	std::cout << "Scan " << sm.GetGlobalIndex(s,st) << " has outlier slices:";
	for (unsigned int i=0; i<ol.size(); i++) { std::cout << " " << ol[i]; }     
	std::cout << std::endl << std::flush;
	cout_mutex.unlock();
      }
      NEWIMAGE::volume<float> pred = pmp->Predict(s,true);
      if (add_noise) {
	double vp = pmp->PredictionVariance(s,true);
	double ve = pmp->ErrorVariance(s);
	double stdev = std::sqrt(vp+ve) - std::sqrt(vp);
	pred += EddyUtils::MakeNoiseIma(pred,0.0,stdev);
      }
      sm.Scan(s,st).SetAsOutliers(pred,sm.GetSuscHzOffResField(s,st),mask,ol);
    }
  }
} EddyCatch


/*
void WriteCNRMaps(// Input
		  const EddyCommandLineOptions&   clo,
		  const ECScanManager&            sm,
		  const std::string&              spatial_fname,
		  const std::string&              range_fname,
		  const std::string&              residual_fname) EddyTry
{
  if (spatial_fname == std::string("") && residual_fname == std::string("")) throw EddyException("EDDY::WriteCNRMaps: At least one of spatial and residual fname must be set");

  // Load prediction maker
  NEWIMAGE::volume<float> mask = sm.Scan(0,ScanType::DWI).GetIma(); EddyUtils::SetTrilinearInterp(mask); mask = 1.0; // FOV-mask in model space
  #ifdef COMPILE_GPU
  std::shared_ptr<DWIPredictionMaker> dwi_pmp = EddyGpuUtils::LoadPredictionMaker(clo,ScanType::DWI,sm,0,0.0,mask);
  std::shared_ptr<DWIPredictionMaker> b0_pmp = EddyGpuUtils::LoadPredictionMaker(clo,ScanType::b0,sm,0,0.0,mask);
  #else
  std::shared_ptr<DWIPredictionMaker> dwi_pmp = EDDY::LoadPredictionMaker(clo,ScanType::DWI,sm,0,0.0,mask);
  std::shared_ptr<DWIPredictionMaker> b0_pmp = EDDY::LoadPredictionMaker(clo,ScanType::b0,sm,0,0.0,mask);
  #endif

  if (sm.IsShelled()) {
    if (spatial_fname != std::string("") || range_fname != std::string("")) {  // Calculate and write shell-wise CNR maps
      std::vector<double>                      grpb;
      std::vector<std::vector<unsigned int> >  dwi_indx = sm.GetShellIndicies(grpb); // Global indicies of dwi scans
      std::vector<NEWIMAGE::volume<float> >    mvols(grpb.size());
      // Calculate mean volumes of predictions
      for (unsigned int i=0; i<grpb.size(); i++) {
	mvols[i] = dwi_pmp->Predict(sm.GetGlobal2DWIIndexMapping(dwi_indx[i][0]));
	for (unsigned int j=1; j<dwi_indx[i].size(); j++) {
	  mvols[i] += dwi_pmp->Predict(sm.GetGlobal2DWIIndexMapping(dwi_indx[i][j]));
	}
	mvols[i] /= float(dwi_indx[i].size());
      }
      // Calculate standard deviation volumes of predictions
      std::vector<NEWIMAGE::volume<float> >    stdvols(grpb.size());
      for (unsigned int i=0; i<grpb.size(); i++) {
	NEWIMAGE::volume<float> tmp = dwi_pmp->Predict(sm.GetGlobal2DWIIndexMapping(dwi_indx[i][0])) - mvols[i];
	stdvols[i] = tmp*tmp;
	for (unsigned int j=1; j<dwi_indx[i].size(); j++) {
	  tmp = dwi_pmp->Predict(sm.GetGlobal2DWIIndexMapping(dwi_indx[i][j])) - mvols[i];
	  stdvols[i] += tmp*tmp;
	}
	stdvols[i] /= float(dwi_indx[i].size()-1);
	stdvols[i] = NEWIMAGE::sqrt(stdvols[i]);
      }
      // Calculate range (min--max) volumes of predictions to make dHCP data seem like it has decent CNR
      std::vector<NEWIMAGE::volume<float> >    minvols(grpb.size());
      std::vector<NEWIMAGE::volume<float> >    maxvols(grpb.size());
      for (unsigned int i=0; i<grpb.size(); i++) {
	minvols[i] = dwi_pmp->Predict(sm.GetGlobal2DWIIndexMapping(dwi_indx[i][0]));
	maxvols[i] = minvols[i];
	for (unsigned int j=1; j<dwi_indx[i].size(); j++) {
	  minvols[i] = NEWIMAGE::min(minvols[i],dwi_pmp->Predict(sm.GetGlobal2DWIIndexMapping(dwi_indx[i][j])));
	  maxvols[i] = NEWIMAGE::max(maxvols[i],dwi_pmp->Predict(sm.GetGlobal2DWIIndexMapping(dwi_indx[i][j])));
	}
	maxvols[i] -= minvols[i]; // Maxvols now contain the range rather than the max
      }
      // Calculate standard deviation of residuals
      std::vector<NEWIMAGE::volume<float> >  stdres(grpb.size());
      for (unsigned int i=0; i<grpb.size(); i++) {
	NEWIMAGE::volume<float> tmp = dwi_pmp->Predict(sm.GetGlobal2DWIIndexMapping(dwi_indx[i][0])) - dwi_pmp->InputData(sm.GetGlobal2DWIIndexMapping(dwi_indx[i][0]));
	stdres[i] = tmp*tmp;
	for (unsigned int j=1; j<dwi_indx[i].size(); j++) {
	  tmp = dwi_pmp->Predict(sm.GetGlobal2DWIIndexMapping(dwi_indx[i][j])) - dwi_pmp->InputData(sm.GetGlobal2DWIIndexMapping(dwi_indx[i][j]));
	  stdres[i] += tmp*tmp;
	}
	stdres[i] /= float(dwi_indx[i].size()-1);
	stdres[i] = NEWIMAGE::sqrt(stdres[i]);
      }
      // Divide std and range of predictions with std of residuals to get CNR
      for (unsigned int i=0; i<grpb.size(); i++) {
	maxvols[i] /= stdres[i];
	stdvols[i] /= stdres[i];
      }
      // Get the SNR (since CNR is zero) of the b0s
      std::vector<unsigned int>   b0_indx = sm.GetB0Indicies();
      NEWIMAGE::volume<float>     b0_SNR = b0_pmp->Predict(0) - b0_pmp->InputData(0);
      if (b0_indx.size() > 1) {
	b0_SNR *= b0_SNR;
	for (unsigned int i=1; i<b0_indx.size(); i++) {
	  NEWIMAGE::volume<float> tmp = b0_pmp->Predict(i) - b0_pmp->InputData(i);
	  b0_SNR += tmp*tmp;
	}
	b0_SNR /= float(b0_indx.size()-1);
	b0_SNR = NEWIMAGE::sqrt(b0_SNR);
	b0_SNR = b0_pmp->Predict(0) /= b0_SNR;
      }
      else b0_SNR = 0.0; // Set it to zero if we can't assess it.
      // Put SNR and CNR maps together into 4D file with spatial CNR
      NEWIMAGE::volume4D<float> spat_cnr(stdvols[0].xsize(),stdvols[0].ysize(),stdvols[0].zsize(),stdvols.size()+1);
      NEWIMAGE::copybasicproperties(stdvols[0],spat_cnr);
      spat_cnr[0] = b0_SNR;
      if (spatial_fname != std::string("")) {
	for (unsigned int i=0; i<stdvols.size(); i++) spat_cnr[i+1] = stdvols[i];
	NEWIMAGE::write_volume(spat_cnr,spatial_fname);
      }
      // Put SNR and range maps together into 4D file with spatial "range-CNR"
      if (range_fname != std::string("")) {
	for (unsigned int i=0; i<maxvols.size(); i++) spat_cnr[i+1] = maxvols[i];
	NEWIMAGE::write_volume(spat_cnr,range_fname);
      }
    }
    if (residual_fname != std::string("")) {   // Calculate and write maps of residuals for all (b0 and DWI) scans
      NEWIMAGE::volume4D<float> residuals(dwi_pmp->InputData(0).xsize(),dwi_pmp->InputData(0).ysize(),dwi_pmp->InputData(0).zsize(),sm.NScans(ScanType::Any));
      NEWIMAGE::copybasicproperties(dwi_pmp->InputData(0),residuals);
      for (unsigned int i=0; i<sm.NScans(ScanType::DWI); i++) {
	residuals[sm.GetDwi2GlobalIndexMapping(i)] = dwi_pmp->InputData(i) - dwi_pmp->Predict(i);
      }
      for (unsigned int i=0; i<sm.NScans(ScanType::b0); i++) {
	residuals[sm.Getb02GlobalIndexMapping(i)] = b0_pmp->InputData(i) - b0_pmp->Predict(i);
      }
      NEWIMAGE::write_volume(residuals,residual_fname);
    }
  }
  else {
    throw EddyException("WriteCNRMaps: Cannot calculate CNR for non-shelled data.");
  }
} EddyCatch
*/

void Diagnostics(// Input
		 const EddyCommandLineOptions&  clo,
		 unsigned int                   iter,
		 ScanType                       st,
		 const ECScanManager&           sm,
                 const double                   *mss_tmp,
                 const DiffStatsVector&         stats,
		 const ReplacementManager&      rm,
		 // Output
		 NEWMAT::Matrix&                mss,
		 NEWMAT::Matrix&                phist) EddyTry
{
  if (clo.Verbose()) {
    double tss=0.0;
    for (unsigned int s=0; s<sm.NScans(st); s++) tss+=mss_tmp[s];
    cout << "Iter: " << iter << ", Total mss = " << tss/sm.NScans(st) << endl;
  }

  for (unsigned int s=0; s<sm.NScans(st); s++) {
    mss(iter+1,s+1) = mss_tmp[s];
    phist.SubMatrix(iter+1,iter+1,s*sm.Scan(0,st).NParam()+1,(s+1)*sm.Scan(0,st).NParam()) = sm.Scan(s,st).GetParams().t();
  }

  if (clo.WriteSliceStats()) {
    char istring[256];
    if (st==EDDY::ScanType::DWI) sprintf(istring,"%s.EddyDwiSliceStatsIteration%02d",clo.IOutFname().c_str(),iter);
    else sprintf(istring,"%s.Eddyb0SliceStatsIteration%02d",clo.IOutFname().c_str(),iter);
    stats.Write(string(istring));
    rm.DumpOutlierMaps(string(istring));
  }
} EddyCatch

void AddRotation(ECScanManager&               sm,
		 const NEWMAT::ColumnVector&  rp) EddyTry
{
  for (unsigned int i=0; i<sm.NScans(); i++) {
    NEWMAT::ColumnVector mp = sm.Scan(i).GetParams(ParametersType::Movement);
    mp(4) += rp(1); mp(5) += rp(2); mp(6) += rp(3);
    sm.Scan(i).SetParams(mp,ParametersType::Movement);
  }
} EddyCatch

void PrintMIValues(const EddyCommandLineOptions&  clo,
                   const ECScanManager&           sm,
                   const std::string&             fname,
                   bool                           write_planes) EddyTry
{
  std::vector<std::string> dir(6);
  dir[0]="xt"; dir[1]="yt"; dir[2]="zt";
  dir[3]="xr"; dir[4]="yr"; dir[5]="zr";
  // First write 1D profiles along main directions
  for (unsigned int i=0; i<6; i++) {
    std::vector<unsigned int> n(6,1); n[i] = 100;
    std::vector<double> first(6,0.0); first[i] = -2.5;
    std::vector<double> last(6,0.0); last[i] = 2.5;
    if (clo.VeryVerbose()) cout << "Writing MI values for direction " << i << endl;
    PEASUtils::WritePostEddyBetweenShellMIValues(clo,sm,n,first,last,fname+"_"+dir[i]);
  }
  // Write 2D planes if requested
  for (unsigned int i=0; i<6; i++) {
    for (unsigned int j=i+1; j<6; j++) {
      std::vector<unsigned int> n(6,1); n[i] = 20; n[j] = 20;
      std::vector<double> first(6,0.0); first[i] = -1.0; first[j] = -1.0;
      std::vector<double> last(6,0.0); last[i] = 1.0; last[j] = 1.0;
      if (clo.VeryVerbose()) cout << "Writing MI values for plane " << i << "-" << j << endl;
      PEASUtils::WritePostEddyBetweenShellMIValues(clo,sm,n,first,last,fname+"_"+dir[i]+"_"+dir[j]);
    }
  }
} EddyCatch

void json_out_struct::Write(const std::string& fname) const EddyTry
{
  nlohmann::json json_file;
  json_file["Shell indicies"] = this->shell_indicies;
  json_file["Outlier report"] = this->outlier_report;
  json_file["Outlier maps"] = this->outlier_maps;
  json_file["Parameters"] = this->parameters;
  json_file["Movement over time"] = this->movement_over_time;
  json_file["Movement RMS"] = this->movement_rms;
  json_file["Restricted movement RMS"] = this->restricted_movement_rms;
  json_file["Long time-constant EC parameters"] = this->long_ec_parameters;
  try {
    std::ofstream ofile(fname);
    ofile << std::setw(4) << json_file << std::endl;
    ofile.close();
  }
  catch(const std::exception& e) {
    throw EddyException("json_out_struct::Write: Exception thrown with message " + std::string(e.what()));
  }   
} EddyCatch

void WritePMDebugInfo(std::shared_ptr<DWIPredictionMaker> pmp,
                      const EddyCommandLineOptions&       clo,
                      EDDY::ScanType                      st,
                      unsigned int                        iter,
                      const std::string&                  arch,
                      const std::string&                  when) EddyTry
{
  std::string prefix("EDDY_DEBUG_");
  prefix += arch;
  if (st==ScanType::b0) prefix += "b0";
  else if (st==ScanType::DWI) prefix += "DWI";  
  else if (st==ScanType::fMRI) prefix += "fMRI";
  if (clo.DebugLevel() > 2) {
    char fname[256];
    sprintf(fname,"%s_K_Mat_Data_%s_%02d",prefix.c_str(),when.c_str(),iter);
    pmp->WriteMetaData(fname);
  }
  if (clo.DebugLevel() > 3) {
    char fname[256];
    sprintf(fname,"%s_K_Mat_Ima_%s_%02d",prefix.c_str(),when.c_str(),iter);
    pmp->WriteImageData(fname);    
  }
} EddyCatch

} // End namespace EDDY

/*! \mainpage
 * Here goes a description of the eddy project
 */