// Definitions of global functions used by fnirt
//
// fnirtfns.cpp
//
// Jesper Andersson, FMRIB Image Analysis Group
//
//

#include <cstdlib>
#include <cmath>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <string>
#include <vector>
#include <algorithm>
#include <boost/shared_ptr.hpp>
#include "newmat.h"
#include "newmatio.h"
#include "utils/options.h"
#include "miscmaths/miscmaths.h"
#include "newimage/newimageall.h"
#include "warpfns/fnirt_file_reader.h"
#include "warpfns/point_list.h"
#include "intensity_mappers.h"
#include "fnirtfns.h"

using namespace std;
using namespace BASISFIELD;

namespace FNIRT {

fnirt_clp::fnirt_clp(const Utilities::Option<string>&                     pref,
                     const Utilities::Option<string>&                     pobj,
                     const Utilities::Option<string>&                     paff,
                     const Utilities::Option<string>&                     pinwarp,
                     const Utilities::Option<string>&                     pin_int,
                     const Utilities::Option<string>&                     pcoef,
                     const Utilities::Option<string>&                     pobjo,
                     const Utilities::Option<string>&                     pfieldo,
                     const Utilities::Option<string>&                     pjaco,
                     const Utilities::Option<string>&                     prefo,
                     const Utilities::Option<string>&                     pinto,
                     const Utilities::Option<string>&                     plogo,
                     const Utilities::Option<string>&                     prefm,
                     const Utilities::Option<string>&                     pobjm,
                     const Utilities::Option<string>&                     pref_pl,
                     const Utilities::Option<string>&                     pobj_pl,
                     const Utilities::Option<vector<int> >&               puserefm,
                     const Utilities::Option<vector<int> >&               puseobjm,
		     const Utilities::Option<int>&                        primf,
		     const Utilities::Option<int>&                        poimf,
		     const Utilities::Option<float>&                      primv,
		     const Utilities::Option<float>&                      poimv,
                     const Utilities::Option<string>&                     pcf,
                     const Utilities::Option<string>&                     pbf,
                     const Utilities::Option<string>&                     pnlm,
                     const Utilities::Option<vector<int> >&               pmi,
                     const Utilities::Option<vector<int> >&               pss,
                     const Utilities::Option<vector<float> >&             pwres,
                     const Utilities::Option<int>&                        pspordr,
	             const Utilities::Option<vector<float> >&             pofwhm,
                     Utilities::Option<vector<float> >&                   prfwhm,
		     const Utilities::Option<string>&                     pregmod,
                     Utilities::Option<vector<float> >&                   plambda,
                     const Utilities::Option<int>&                        pssqlambda,
                     const Utilities::Option<float>&                      pmpl_lambda,
		     const Utilities::Option<vector<float> >&             pjacrange,
                     const Utilities::Option<int>&                        puserefderiv,
                     const Utilities::Option<string>&                     pintmod,
		     const Utilities::Option<vector<int> >&               pestint,
                     const Utilities::Option<int>&                        pintord,
                     const Utilities::Option<vector<float> >&             pbiasres,
                     const Utilities::Option<string>&                     pbiasregmod,
                     const Utilities::Option<float>&                      pbiaslambda,
                     const Utilities::Option<bool>&                       pverbose,
                     const Utilities::Option<int>&                        pdebug,
                     const Utilities::Option<string>&                     p_hess_prec)
  : ref(pref.value()), obj(pobj.value()), inwarp(pinwarp.value()), in_int(pin_int.value()), coef(pcoef.value()), objo(pobjo.value()), 
    fieldo(pfieldo.value()), jaco(pjaco.value()), refo(prefo.value()), into(pinto.value()), logo(plogo.value()), 
    refm(prefm.value()), objm(pobjm.value()), ref_pl(pref_pl.value()), obj_pl(pobj_pl.value()), rimf((primf.value()==0) ? false : true), 
    rimv(primv.value()), oimf((poimf.value()==0) ? false : true), oimv(poimv.value()), spordr(static_cast<unsigned int>(pspordr.value())), 
    ssqlambda((pssqlambda.value()==0) ? false : true), jacrange(pjacrange.value()), userefderiv((puserefderiv.value()==0) ? false : true), 
    verbose(pverbose.value()), debug(static_cast<unsigned int>(pdebug.value()))
{
  // Parse and assert input

  // Check that image files exist and are valid
  NEWIMAGE::volume<float> vref, vobj, vany;
  if (!(ref = existing_ref_fname(ref)).size()) throw fnirt_error(string("fnirt_clp: No file matching --ref=")+ref+string(" found"));
  read_volume_hdr_only(vref,ref);
  if (refm.length()) {
    if (!(refm = existing_ref_fname(refm)).size()) throw fnirt_error(string("fnirt_clp: No file matching --refmask=")+refm+string(" found"));
    read_volume_hdr_only(vany,refm);
    if (vref.xsize() != vany.xsize() || vref.ysize() != vany.ysize() || vref.zsize() != vany.zsize() ||
        vref.xdim() != vany.xdim() || vref.ydim() != vany.ydim() || vref.zdim() != vany.zdim()) {
      throw fnirt_error(string("fnirt_clp: --ref=")+ref+string(" and --refmask=")+refm+string(" have different dimensions."));
    }
  }
  NEWIMAGE::read_volume_hdr_only(vobj,obj);     // Throws if there is a problem
  if (objm.length()) {
    NEWIMAGE::read_volume_hdr_only(vany,objm);  // Throws if there is a problem
    if (vobj.xsize() != vany.xsize() || vobj.ysize() != vany.ysize() || vobj.zsize() != vany.zsize() ||
        vobj.xdim() != vany.xdim() || vobj.ydim() != vany.ydim() || vobj.zdim() != vany.zdim()) {
      throw fnirt_error(string("fnirt_clp: --in=")+obj+string(" and --inmask: ")+objm+string(" have different dimensions."));
    }
  }

  // Check that either two or zero point-lists have been defined.
  if ((ref_pl.length() && !obj_pl.length()) || (!ref_pl.length() && obj_pl.length())) {
    throw fnirt_error("fnirt_clp: Must define zero or two point lists");
  }
  if (ref_pl.length()) {  // This means there are two point lists
    try {
      NEWIMAGE::PointList rpl(ref_pl,ref);
      NEWIMAGE::PointList opl(obj_pl,obj);
      if (rpl.NPoints() != opl.NPoints()) {
        throw fnirt_error(string("fnirt_clp: Mismatch in number of points in lists ")+ref_pl+string(" and ")+obj_pl);
      }
    }
    catch (std::exception& error) {
      throw fnirt_error(string("fnirt_clp: Error when reding point-list. Message was: ")+string(error.what()));
    }
  }

  // Read and assert affine matrix
  if (paff.value().empty()) aff = NEWMAT::IdentityMatrix(4);
  else {
    ifstream  ifs(paff.value().c_str());
    if (!ifs.fail()) {
      aff = MISCMATHS::read_ascii_matrix(ifs);
    }
    if (ifs.fail() || aff.Nrows() != 4 || aff.Ncols() != 4) {
      throw fnirt_error(string("fnirt_clp: Invalid affine matrix ")+paff.value()+string(" passed as argument to --aff"));
    }
  }
  // Assert the categorical parameters
  if (p_hess_prec.value() == "float") hess_prec = BFMatrixFloatPrecision;
  else if (p_hess_prec.value() == "double") hess_prec = BFMatrixDoublePrecision;
  else throw fnirt_error("fnirt_clp: --numprec takes values float or double");
  if (debug > 3) throw fnirt_error("fnirt_clp: --debug takes values 0, 1, 2 or 3");
  if (pcf.value() == "ssd") cf = SSD;
  else throw fnirt_error("fnirt_clp: Invalid cost-function option");
  if (pbf.value() == "spline") bf = Spline;
  else if (pbf.value() == "dct") bf = DCT;
  else throw fnirt_error("fnirt_clp: Invalid basis option");
  if (pnlm.value() == "lm") nlm = NL_LM;
  else if (pnlm.value() == "scg") nlm = NL_SCG;
  else if (pnlm.value() == "cg") nlm = NL_CG;
  else throw fnirt_error("fnirt_clp: Invalid minimisation option");
  if (pregmod.value() == "membrane_energy") regmod = MembraneEnergy;
  else if (pregmod.value() == "bending_energy") regmod = BendingEnergy;
  else throw fnirt_error("fnirt_clp: Invalid regularisation type "+pregmod.value()+string(" passed as argument to --regmod"));
  if (spordr < 2 || spordr > 3) throw fnirt_error("fnirt_clp: Spline order (--splineorder) must be 2 or 3");

  // Make sure jacobian range isn't silly
  if (jacrange.size() == 1) {
    if (jacrange[0] == -1) { // -1 used to indicate no Jacobian constraints
      jacrange[0] = -1e6;
      jacrange.push_back(1e6);
    }
    else throw fnirt_error("fnirt_clp: --jacrange takes 2 arguments, or a single -1");
  }
  else if (jacrange.size() == 2 && (jacrange[0] >= jacrange[1])) throw fnirt_error("fnirt_clp: Invalid jacobian range");
  else if (jacrange.size() != 2) throw fnirt_error("fnirt_clp: --jacrange takes 2 arguments, or a single -1");

  // Make sure subsampling is by a power of 2
  for (int i=0; i<int(pss.value().size()); i++) {
    bool valid = false;
    for (int j=1; j<64; j*=2) {
      if ((pss.value())[i] == j) valid = true;
    }
    if (!valid) throw fnirt_error("fnirt_clp: Subsampling must be by a power of 2 at all levels");
  }
  // Make sure we have correspondence between subsampling
  // and the various parameters that should be defined for
  // each level of subsampling
  ss.resize(nlev = pss.value().size());
  for (unsigned int i=0; i<pss.value().size(); i++) ss[i] = static_cast<unsigned int>(pss.value()[i]);
  if (pmi.value().size() != nlev && pmi.value().size() != 1) {
    throw fnirt_error("fnirt_clp: Mismatch between --subsamp and --miter options");
  }
  if (pofwhm.value().size() != nlev && pofwhm.value().size() != 1) {
    throw fnirt_error("fnirt_clp: Mismatch between --subsamp and --infwhm options");
  }
  if (prfwhm.value().size() != nlev && prfwhm.value().size() != 1 && prfwhm.value().size() != 0) {
    throw fnirt_error("fnirt_clp: Mismatch between --subsamp and --reffwhm options");
  }
  if (plambda.value().size() != nlev && plambda.value().size() != 1 && plambda.value().size() != 0) {
    throw fnirt_error("fnirt_clp: Mismatch between --subsamp and --lambda options");
  }
  if (pestint.value().size() != nlev && pestint.value().size() != 1) {
    throw fnirt_error("fnirt_clp: Mismatch between --subsamp and --estint options");
  }
  if (puserefm.value().size() != nlev && puserefm.value().size() != 1) {
    throw fnirt_error("fnirt_clp: Mismatch between --subsamp and --applyrefmask options");
  }
  if (puseobjm.value().size() != nlev && puseobjm.value().size() != 1) {
    throw fnirt_error("fnirt_clp: Mismatch between --subsamp and --applyinmask options");
  }

  // Assign
  if (pmi.value().size() == nlev) {
    mi.resize(pmi.value().size()); 
    for (unsigned int i=0; i<pmi.value().size(); i++) mi[i] = static_cast<unsigned int>(pmi.value()[i]);
  }
  else {mi = vector<unsigned int>(nlev); for (unsigned int i=0; i<nlev; i++) mi[i] = (pmi.value())[0];}

  if (pofwhm.value().size() == nlev) ofwhm = pofwhm.value();
  else {ofwhm = vector<float>(nlev); for (unsigned int i=0; i<nlev; i++) ofwhm[i] = (pofwhm.value())[0];}

  if (prfwhm.value().size() == nlev) rfwhm = prfwhm.value();
  else if (prfwhm.value().size() == 1) {rfwhm = vector<float>(nlev); for (unsigned int i=0; i<nlev; i++) rfwhm[i] = (prfwhm.value())[0];}
  else {
    rfwhm = ofwhm;
    prfwhm.set_T(pofwhm.value());
  }

  if (plambda.value().size() == nlev) lambda = plambda.value();
  else {
    lambda = vector<float>(nlev);
    if (plambda.value().size() == 1) {for (unsigned int i=0; i<nlev; i++) lambda[i] = (plambda.value())[0];}
    else {
      // Here we have to assign values for lambda
      // based on what regularisation model has been
      // specified and wether to weight by ssq
      if (ssqlambda) {
        if (regmod == BendingEnergy) {for (unsigned int i=0; i<nlev; i++) lambda[i] = ss[i] * 30.0;}
	else if (regmod == MembraneEnergy) {for (unsigned int i=0; i<nlev; i++) lambda[i] = ss[i] * 2.5;}
      }
      else {
	if (regmod == BendingEnergy) {for (unsigned int i=0; i<nlev; i++) lambda[i] = ss[i] * 15000.0;} 
	else if (regmod == MembraneEnergy) {for (unsigned int i=0; i<nlev; i++) lambda[i] = ss[i] * 1500;}
      }
      plambda.set_T(lambda);
    }
  }

  estint.resize(nlev);
  if (pestint.value().size() == nlev) for (unsigned int i=0; i<nlev; i++) estint[i] = (pestint.value()[i] ? true : false);
  else if (pestint.value().size() == 1) for (unsigned int i=0; i<nlev; i++) estint[i] = (pestint.value()[0] ? true : false);

  userefmask.resize(nlev);
  if (puserefm.value().size() == nlev) for (unsigned int i=0; i<nlev; i++) userefmask[i] = (puserefm.value()[i] ? true : false);
  else if (puserefm.value().size() == 1) for (unsigned int i=0; i<nlev; i++) userefmask[i] = (puserefm.value()[0] ? true : false);

  useobjmask.resize(nlev);
  if (puseobjm.value().size() == nlev) for (unsigned int i=0; i<nlev; i++) useobjmask[i] = (puseobjm.value()[i] ? true : false);
  else if (puseobjm.value().size() == 1) for (unsigned int i=0; i<nlev; i++) useobjmask[i] = (puseobjm.value()[0] ? true : false);

  //
  // Assert and parse parameters pertaining to intensity mapping.
  //
  if (!pintmod.value().compare(string("none"))) imt = NONE;
  else if (!pintmod.value().compare(string("global_linear"))) imt = GLOBAL_LINEAR;
  else if (!pintmod.value().compare(string("global_non_linear"))) imt = GLOBAL_NON_LINEAR;
  else if (!pintmod.value().compare(string("local_linear"))) imt = LOCAL_LINEAR;
  else if (!pintmod.value().compare(string("global_non_linear_with_bias"))) imt = LOCAL_BIAS_WITH_GLOBAL_NON_LINEAR;
  else if (!pintmod.value().compare(string("local_non_linear"))) imt = LOCAL_NON_LINEAR;
  else throw fnirt_error(string("fnirt_clp: Argument to --intmod is unknown scaling option: ")+pintmod.value());

  if (pbiasregmod.value() == string("bending_energy")) biasregmod = BendingEnergy;
  else throw fnirt_error(string("fnirt_clp: Argument to --biasregmod is unknown option: ")+pbiasregmod.value());

  biaslambda = vector<float>(nlev,pbiaslambda.value());
  mpl_lambda = vector<float>(nlev,pmpl_lambda.value());

  if (imt == GLOBAL_NON_LINEAR && (intord = static_cast<unsigned int>(pintord.value())) > 10) {
    throw fnirt_error("fnirt_clp: Argument to --intorder, cannot use polynomial of order > 10 for intensity mapping");
  }
  else if (imt == LOCAL_BIAS_WITH_GLOBAL_NON_LINEAR && (intord = static_cast<unsigned int>(pintord.value())) > 5) {
    throw fnirt_error("fnirt_clp: Argument to --intorder, cannot use polynomial of order > 5 for intensity mapping when also modelling intensity bias");
  }

  
  double pxs[] = {vref.xdim(), vref.ydim(), vref.zdim()};
  int    dim[] = {vref.xsize(), vref.ysize(), vref.zsize()};

  //
  // Convert mm to voxels/order for splines/DCT for displacement fields
  //
  vector<float>  tmpwres(3,0);
  if (pwres.value().size() == 3) tmpwres = pwres.value();
  else if (pwres.value().size() == 1) {for (int i=0; i<3; i++) tmpwres[i] = (pwres.value())[0];}
  else throw fnirt_error("fnirt_clp: --warpres option must be 3x1 vector or scalar");
  if (bf == Spline) {
    vector<unsigned int>     tmpksp(3,0);
    for (int i=0; i<3; i++) {
      tmpksp[i] = max(static_cast<unsigned int>(1),static_cast<unsigned int>(floor(tmpwres[i]/pxs[i] + 0.5)));
      // cout << "tmpwres[" << i << "]=" << tmpwres[i] << ", pxs[" << i << "]=" << pxs[i] << ", tmpksp[" << i << "]=" << tmpksp[i] << endl;
    }
    ksp = vector<vector<unsigned int> >(nlev);
    for (unsigned int i=0; i<nlev; i++) ksp[i] = tmpksp;
    if (!(tmpksp[0]/ss[nlev-1])) throw fnirt_error("fnirt_clp: Incompatible combination of --subsamp, --warpres and voxel size in x-direction");
    if (!(tmpksp[1]/ss[nlev-1])) throw fnirt_error("fnirt_clp: Incompatible combination of --subsamp, --warpres and voxel size in y-direction");
    if (!(tmpksp[2]/ss[nlev-1])) throw fnirt_error("fnirt_clp: Incompatible combination of --subsamp, --warpres and voxel size in z-direction");
  }
  else if (bf == DCT) {
    vector<int> tmpdco(3,0);
    for (int i=0; i<3; i++) tmpdco[i] = static_cast<int>(floor(pxs[i]*dim[i]/tmpwres[i] + 0.5));
    dco = vector<vector<unsigned int> >(nlev);
    for (unsigned int i=0; i<nlev; i++) {
      for (int j=0; j<3; j++) {
        dco[i][j] = static_cast<unsigned int>(floor(float(tmpdco[j])/float(ss[i]/ss[nlev-1]) + 0.5));
      }
    }
  }

  //
  // Convert mm to voxels/order for splines/DCT for bias-fields,
  // taking into account that we want to keep the resolution (in mm)
  // constant across the subsamplings.
  //
  vector<float>  tmpbres(3,0);
  if (pbiasres.value().size() == 3) tmpbres = pbiasres.value();
  else if (pbiasres.value().size() == 1) {for (int i=0; i<3; i++) tmpbres[i] = (pbiasres.value())[0];}
  else throw fnirt_error("fnirt_clp: --biasres option must be 3x1 vector or scalar");
  if (bf == Spline) {
    vector<unsigned int>  tmpksp(3,0);       // Knot-spacing in voxels for lowest resolution (most sub-sampling)
    for (int i=0; i<3; i++) tmpksp[i] = static_cast<unsigned int>(floor(double(tmpbres[i] / double(pxs[i] * ss[0])) + 0.5));
    bias_ksp.resize(nlev);
    for (unsigned int l=0; l<nlev; l++) {
      bias_ksp[l].resize(3);
      for (int i=0; i<3; i++) {
        bias_ksp[l][i] = (ss[0]/ss[l]) * tmpksp[i];
      }
    }
  }
  else if (bf == DCT) {
    throw fnirt_error("Sorry, not yet implemented");
  }  
}

boost::shared_ptr<fnirt_clp> parse_fnirt_command_line(unsigned int   narg,
                                                      char           *args[])
{
  // These are the specifications of all the command-line options

  Utilities::Option<string> refname(string("--ref"), string(""),
      string("\tname of reference image"), true, Utilities::requires_argument);

  Utilities::Option<string> objname(string("--in"), string(""),
      string("\tname of input image"), true, Utilities::requires_argument);

  Utilities::Option<string> affname(string("--aff"), string(""),
      string("\tname of file containing affine transform"), false, Utilities::requires_argument);

  Utilities::Option<string> inwarpname(string("--inwarp"), string(""),
      string("name of file containing initial non-linear warps"), false, Utilities::requires_argument);

  Utilities::Option<string> in_intname(string("--intin"), string(""),
      string("\tname of file/files containing initial intensity maping"), false, Utilities::requires_argument);

  Utilities::Option<string> coefname(string("--cout"), string(""),
      string("\tname of output file with field coefficients"), false, Utilities::requires_argument);

  Utilities::Option<string> imoutname(string("--iout"), string(""),
      string("\tname of output image"), false, Utilities::requires_argument);

  Utilities::Option<string> fieldoutname(string("--fout"), string(""),
      string("\tname of output file with field"), false, Utilities::requires_argument);

  Utilities::Option<string> jacname(string("--jout"), string(""),
      string("\tname of file for writing out the Jacobian of the field (for diagnostic or VBM purposes)"),
      false,Utilities::requires_argument);

  Utilities::Option<string> refoutname(string("--refout"), string(""),
      string("name of file for writing out intensity modulated --ref (for diagnostic purposes)"),
      false,Utilities::requires_argument);

  Utilities::Option<string> intoutname(string("--intout"), string(""),
      string("name of files for writing information pertaining to intensity mapping"),false,Utilities::requires_argument);

  Utilities::Option<string> logoutname(string("--logout"),string(""),
			    string("Name of log-file"),false,Utilities::requires_argument);

  Utilities::Option<string> configname(string("--config"),string(""),
			    string("Name of config file specifying command line arguments"),false,Utilities::requires_argument);

  Utilities::Option<string> costfunction(string("--cf"), string("ssd"),
      string("cost-function to minimise"), false, Utilities::requires_argument);

  Utilities::Option<string> basis(string("--basis"), string("spline"),
      string("\tbasis used to model field [spline | dct]"), false, Utilities::requires_argument);

  Utilities::Option<string> refmaskname(string("--refmask"), string(""),
      string("name of file with mask in reference space"), false, Utilities::requires_argument);

  Utilities::Option<string> objmaskname(string("--inmask"), string(""),
      string("name of file with mask in input image space"),false, Utilities::requires_argument);

  // The point-lists are hidden for the time being
  Utilities::HiddenOption<string> refpointlistname(string("--refpointlist"), string(""),
      string("name of file with points/coordinates in reference space"), false, Utilities::requires_argument);

  Utilities::HiddenOption<string> objpointlistname(string("--inpointlist"), string(""),
      string("name of file with points/coordinates in input image space"),false, Utilities::requires_argument);

  Utilities::HiddenOption<float> mpl_lambda(string("--pointlistlambda"),1.0,
      string("Weight of landmark distances relative to intensities, default 1"), 
      false, Utilities::requires_argument);

  vector<int> applyrefmaskdefault(4,1);
  Utilities::Option<vector<int> > applyrefmask(string("--applyrefmask"),applyrefmaskdefault,
      string("Use specified refmask if set, deafult 1 (true)"),false,Utilities::requires_argument);

  vector<int> applyobjmaskdefault(4,1);
  Utilities::Option<vector<int> > applyobjmask(string("--applyinmask"),applyobjmaskdefault,
      string("Use specified inmask if set, deafult 1 (true)"),false,Utilities::requires_argument);

  Utilities::Option<int> imprefflag(string("--imprefm"), 1,
      string("If =1, use implicit masking based on value in --ref image. Default =1"),false, Utilities::requires_argument);

  Utilities::Option<int> impobjflag(string("--impinm"), 1,
      string("If =1, use implicit masking based on value in --in image, Default =1"),false, Utilities::requires_argument);

  Utilities::Option<float> imprefval(string("--imprefval"), 0.0,
      string("Value to mask out in --ref image. Default =0.0"),false, Utilities::requires_argument);

  Utilities::Option<float> impobjval(string("--impinval"), 0.0,
      string("Value to mask out in --in image. Default =0.0"),false, Utilities::requires_argument);

  Utilities::Option<string> minimisationmethod(string("--minmet"), string("lm"),
      string("non-linear minimisation method [lm | scg | cg] (Leveberg-Marquardt, Scaled Conjugate Gradient or Conjugate Gradient)"), 
      false, Utilities::requires_argument);

  vector<int> maxiterdefault(4,0);
  maxiterdefault[0] = 5; maxiterdefault[1] = 5; maxiterdefault[2] = 5; maxiterdefault[3] = 5; 
  Utilities::Option<vector<int> > maxiter(string("--miter"), maxiterdefault,
      string("\tMax # of non-linear iterations, default 5,5,5,5"), false, Utilities::requires_argument);

  vector<int> subsamplingdefault(4,0);
  subsamplingdefault[0] = 4; subsamplingdefault[1] = 2; subsamplingdefault[2] = 1; subsamplingdefault[3] = 1; 
  Utilities::Option<vector<int> > subsampling(string("--subsamp"), subsamplingdefault,
      string("sub-sampling scheme, default 4,2,1,1"), false, Utilities::requires_argument);

  vector<float> warpresdefault(3,10.0);
  Utilities::Option<vector<float> > warpres(string("--warpres"), warpresdefault,
      string("(approximate) resolution (in mm) of warp basis in x-, y- and z-direction, default 10,10,10"), false, Utilities::requires_argument);

  Utilities::Option<int> splineorder(string("--splineorder"), 3,
      string("Order of spline, 2->Qadratic spline, 3->Cubic spline. Default=3"),false, Utilities::requires_argument);

  vector<float> objsmoothdefault(4,0);
  objsmoothdefault[0] = 6.0; objsmoothdefault[1] = 4.0; objsmoothdefault[2] = 2.0; objsmoothdefault[3] = 2.0;
  Utilities::Option<vector<float> > objsmoothing(string("--infwhm"),objsmoothdefault,
      string("FWHM (in mm) of gaussian smoothing kernel for input volume, default 6,4,2,2"), false, Utilities::requires_argument);

  vector<float> refsmoothdefault(4,0);
  refsmoothdefault[0] = 4.0; refsmoothdefault[1] = 2.0; refsmoothdefault[2] = 0.0; refsmoothdefault[3] = 0.0;
  Utilities::Option<vector<float> > refsmoothing(string("--reffwhm"),refsmoothdefault,
      string("FWHM (in mm) of gaussian smoothing kernel for ref volume, default 4,2,0,0"), false, Utilities::requires_argument);

  Utilities::Option<string> regularisationmodel(string("--regmod"),string("bending_energy"),
				                string("Model for regularisation of warp-field [membrane_energy bending_energy], default bending_energy"),
				                false,Utilities::requires_argument); 

  vector<float> lambdadefault(0);
  Utilities::Option<vector<float> > lambda(string("--lambda"),lambdadefault,
      string("Weight of regularisation, default depending on --ssqlambda and --regmod switches. See user documetation."), 
      false, Utilities::requires_argument);

  Utilities::Option<int> ssqlambda(string("--ssqlambda"),true,string("If set (=1), lambda is weighted by current ssq, default 1"),
      false,Utilities::requires_argument);

  vector<float> jacrangedefault(2,0);
  jacrangedefault[0] = 0.01; jacrangedefault[1] = 100.0;   // Basically non-negativity
  Utilities::Option<vector<float> > jacrange(string("--jacrange"),jacrangedefault,
      string("Allowed range of Jacobian determinants, default 0.01,100.0"), 
      false, Utilities::requires_argument);

  Utilities::Option<int> userefderiv(string("--refderiv"),0,string("If =1, ref image is used to calculate derivatives. Default =0"),
      false,Utilities::requires_argument);

  Utilities::Option<string> intensitymodel(string("--intmod"),string("global_non_linear_with_bias"),
      string("Model for intensity-mapping [none global_linear global_non_linear local_linear global_non_linear_with_bias local_non_linear]"),
      false,Utilities::requires_argument); 

  int intensityorderdefault = 5;
  Utilities::Option<int> intensityorder(string("--intorder"),intensityorderdefault,
      string("Order of poynomial for mapping intensities, default 5"),false,Utilities::requires_argument);
   
  vector<float> biasresdefault(3,50.0);
  Utilities::Option<vector<float> > biasfieldres(string("--biasres"),biasresdefault,
      string("Resolution (in mm) of bias-field modelling local intensities, default 50,50,50"),false,Utilities::requires_argument);

  Utilities::Option<float> biasfieldlambda(string("--biaslambda"),1e4,
      string("Weight of regularisation for bias-field, default 10000"),false,Utilities::requires_argument);

  Utilities::Option<string> biasfieldregmod(string("--biasregmod"),string("bending_energy"),
      string("Model for regularisation of biasfield, default bending_energy"),false,Utilities::requires_argument);

  vector<int> estintdefault(4,1);
  estintdefault[3] = 0;
  Utilities::Option<vector<int> > estimateintensity(string("--estint"),estintdefault,
      string("Estimate intensity-mapping if set, deafult 1 (true)"),false,Utilities::requires_argument);

  Utilities::Option<string> numprec(string("--numprec"),string("double"),
      string("Precision for representing Hessian, double or float. Default double"),false,Utilities::requires_argument);

  Utilities::Option<string> configfile(string("--config"),string(""),
      string("Name of configuration field with settings for some/all fnirt parameters"),false,Utilities::requires_argument);

  Utilities::Option<bool> help(string("-h,--help"), false,
      string("display help info"), false, Utilities::no_argument);

  Utilities::Option<bool> verbose(string("-v,--verbose"), false,
      string("Print diagonostic information while running"), false, Utilities::no_argument);

  Utilities::HiddenOption<int> debug(string("--debug"), 0,
      string("Save debug information while running, levels 0 (no info), 1 (some info), 3 (little more info) or 3 (LOTS of info)"), false, Utilities::requires_argument);

  // Some explanatory text

  string title = "fnirt";
  string examples = string("fnirt --ref=<some template> --in=<some image>\n") +
                    string("fnirt --ref=<some template> --in=<some image> --infwhm=8,4,2 --subsamp=4,2,1 --warpres=8,8,8");

  // Create and load options-parser

  Utilities::OptionParser options(title, examples);

  try {
    // Load parser

    options.add(refname);
    options.add(objname);
    options.add(affname);
    options.add(inwarpname);
    options.add(in_intname);
    options.add(coefname);
    options.add(imoutname);
    options.add(fieldoutname);
    options.add(jacname);
    options.add(refoutname);
    options.add(intoutname);
    options.add(logoutname);
    options.add(configname);
    options.add(refmaskname);
    options.add(objmaskname);
    options.add(refpointlistname);
    options.add(objpointlistname);
    options.add(applyrefmask);
    options.add(applyobjmask);
    options.add(imprefflag);
    options.add(impobjflag);
    options.add(imprefval);
    options.add(impobjval);
    // options.add(costfunction);         Cost-function option hidden for now
    // options.add(basis);                Basis-set hidden for now
    // options.add(minimisationmethod);   Minimisation-method option hidden for now
    options.add(maxiter);
    options.add(subsampling);
    options.add(warpres);
    options.add(splineorder);
    options.add(objsmoothing);
    options.add(refsmoothing);
    options.add(regularisationmodel);
    options.add(lambda);
    options.add(ssqlambda);
    options.add(mpl_lambda);
    options.add(jacrange);
    options.add(userefderiv);
    options.add(intensitymodel);
    options.add(intensityorder);
    options.add(biasfieldres);
    // options.add(biasfieldregmod);      Regularisation model for bias field hidden for now
    options.add(biasfieldlambda);
    options.add(estimateintensity);
    options.add(numprec);
    options.add(verbose);
    options.add(debug);
    options.add(help);

    // Parse command line

    unsigned int i = options.parse_command_line(narg,args);
    if (i < narg) {
      for (; i<narg; i++) {
        cerr << "Unknown input: " << args[i] << endl;
      }
      exit(EXIT_FAILURE);
    }

    // Then parse config file if one present

    string   final_configname;
    if (configname.set()) {
      final_configname = existing_conf_file(configname.value());
      if (!final_configname.length()) {
        cerr << "Cannot find config-file " << configname.value() << endl;
        exit(EXIT_FAILURE);
      }
      options.parse_config_file(final_configname);
    }

    if (help.value() || !options.check_compulsory_arguments()) {
      options.usage();
      exit(EXIT_FAILURE);
    }
  }
  catch(Utilities::X_OptionError& e) {
    options.usage();
    cerr << "\n" << e.what() << "\n";
    exit(EXIT_FAILURE);
  }
  catch(std::exception &e) {
    cerr << "\n" << e.what() << "\n";
    exit(EXIT_FAILURE);
  }

  // Do the higher-level parsing, i.e. make sure this particular combination
  // of options make sense. If it does, create a fnirt_clp object that can
  // be interrogated about the different options.

  boost::shared_ptr<fnirt_clp>   clp;
  try {
    clp = boost::shared_ptr<fnirt_clp>(new fnirt_clp(refname,objname,affname,inwarpname,in_intname,coefname,imoutname,fieldoutname,
                                                     jacname,refoutname,intoutname,logoutname,refmaskname,objmaskname,refpointlistname,
                                                     objpointlistname,applyrefmask,applyobjmask,imprefflag,impobjflag,imprefval,impobjval,costfunction,
                                                     basis,minimisationmethod,maxiter,subsampling,warpres,splineorder,objsmoothing,
                                                     refsmoothing,regularisationmodel,lambda,ssqlambda,mpl_lambda,jacrange,userefderiv,intensitymodel,
                                                     estimateintensity,intensityorder,biasfieldres,biasfieldregmod,
                                                     biasfieldlambda,verbose,debug,numprec));
  }
  catch(fnirt_error& e) {
    options.usage();
    cerr << endl << e.what() << endl;
    exit(EXIT_FAILURE);
  }

  // Write out a log-file containing all the parsed/expanded parameters
  try {
    std::ofstream logfs(clp->LogFname().c_str());
    logfs << refname << endl;
    logfs << objname << endl;
    if (affname.set()) logfs << affname << endl;
    if (inwarpname.set()) logfs << inwarpname << endl;
    if (in_intname.set()) logfs << in_intname << endl;
    if (coefname.set()) logfs << coefname << endl;
    if (imoutname.set()) logfs << imoutname << endl;
    if (fieldoutname.set()) logfs << fieldoutname << endl;
    if (jacname.set()) logfs << jacname << endl;
    if (refoutname.set()) logfs << refoutname << endl;
    if (intoutname.set()) logfs << intoutname << endl;
    if (logoutname.set()) logfs << logoutname << endl;
    if (refmaskname.set()) logfs << refmaskname << endl;
    if (objmaskname.set()) logfs << objmaskname << endl;
    // Uncomment these when pointlist functionality is released
    // if (refpointlistname.set()) logfs << refpointlistname << endl;
    // if (objpointlistname.set()) logfs << objpointlistname << endl;
    // logfs << mpl_lambda << endl;    
    logfs << imprefflag << endl;
    logfs << impobjflag << endl;
    logfs << imprefval << endl;
    logfs << impobjval << endl;
    logfs << subsampling << endl;
    logfs << maxiter << endl;
    logfs << objsmoothing << endl;
    logfs << refsmoothing << endl;
    logfs << lambda << endl;
    logfs << estimateintensity << endl;
    logfs << warpres << endl;
    logfs << splineorder << endl;
    logfs << ssqlambda << endl;
    logfs << jacrange << endl;
    logfs << regularisationmodel << endl;
    logfs << intensitymodel << endl;
    logfs << intensityorder << endl;
    logfs << biasfieldres << endl;
    logfs << biasfieldlambda << endl;
    logfs << numprec << endl;
    logfs << userefderiv << endl;    
    logfs.close();
  }
  catch (...) {
    cerr << "Error when writing log-file: " << clp->LogFname() << endl;
    throw;
  }

  return(clp);  
}

bool constrain_warpfield(const SSD_fnirt_CF&   cf,
                         const fnirt_clp&      clp,
                         unsigned int          max_try)
{
  std::pair<double,double> range = cf.JacobianRange();
  if (clp.Verbose()) cout << "Jacobian range is " << range.first << " -- " << range.second << endl;

  unsigned int n_try=0;
  while ((range.first < clp.JacLowerBound() || range.second > clp.JacUpperBound()) && n_try < max_try) {
    if (clp.Verbose()) cout << "Forcing Jacobian range to " << clp.JacLowerBound() << " -- " << clp.JacUpperBound() << endl;
    cf.ForceJacobianRange(clp.JacLowerBound(),clp.JacUpperBound());
    range = cf.JacobianRange();
    if (clp.Verbose()) cout << "Jacobian range is " << range.first << " -- " << range.second << endl;
    n_try++;
  } 

  if (range.first < clp.JacLowerBound() || range.second > clp.JacUpperBound()) return(false);

  return(true);  
}

vector<boost::shared_ptr<basisfield> > init_warpfield(const fnirt_clp&  clp)
{
  vector<boost::shared_ptr<basisfield> >    field(3);

  if (clp.InWarp().size()) {  // If there is a starting guess for the field
    NEWIMAGE::FnirtFileReader    reader;
    try {
      reader.Read(clp.InWarp());
    }
    catch (...) {
      cerr << "Problems reading initial warp file " << clp.InWarp() << endl;
      exit(EXIT_FAILURE);
    }
    if (clp.RefSize() != reader.FieldSize()) {
      cerr << "Field in file " << clp.InWarp() << " not of same size as image in " << clp.Ref() << endl;
      exit(EXIT_FAILURE);
    }
    if (clp.RefVxs() != reader.VoxelSize()) {
      cerr << "Field in file " << clp.InWarp() << " has different voxel size from image in " << clp.Ref() << endl;
      exit(EXIT_FAILURE);
    }
    if (clp.Basis() == FNIRT::Spline) {
      for (int i=0; i<3; i++) field[i] = boost::shared_ptr<splinefield>(new splinefield(reader.FieldAsSplinefield(i,clp.FullResKsp())));
    }
    else if (clp.Basis() == FNIRT::DCT) {  
      for (int i=0; i<3; i++) field[i] = boost::shared_ptr<dctfield>(new dctfield(reader.FieldAsDctfield(i,clp.DCTOrder(clp.NoLevels()))));
    }      
  }
  else { // If we are starting from scratch
    if (clp.Basis() == FNIRT::Spline) {
      for (int i=0; i<3; i++) field[i] = boost::shared_ptr<splinefield>(new splinefield(clp.RefSize(),clp.RefVxs(),clp.FullResKsp(),clp.SplineOrder()));
    }
    else if (clp.Basis() == FNIRT::DCT) {  
      for (int i=0; i<3; i++) field[i] = boost::shared_ptr<dctfield>(new dctfield(clp.RefSize(),clp.RefVxs(),clp.DCTOrder(clp.NoLevels())));
    }
  }
  return(field);
}

boost::shared_ptr<IntensityMapper> init_intensity_mapper(const fnirt_clp&  clp)
{
  boost::shared_ptr<IntensityMapper>  mymapper;  // Null pointer
  IntensityMapperReader               mapreader;

  if (clp.InInt().size()) mapreader.Read(clp.InInt());    // If initialisation file name has been specified

  if (clp.IntMapType() == NONE) {
    mymapper = boost::shared_ptr<IntensityMapper>(new SSDIntensityMapper);
  }
  else if (clp.IntMapType() == GLOBAL_LINEAR) {
    double se = 1.0;
    if (mapreader.HasGlobal()) {
      vector<double>  old_global = mapreader.GetGlobal();
      if (old_global.size()==1) se = old_global[0];
      else if (old_global.size()>1) se = old_global[1];
    }
    mymapper = boost::shared_ptr<IntensityMapper>(new SSDIntensityMapper(se));
  }
  else if (clp.IntMapType() == GLOBAL_NON_LINEAR) {
    vector<double>  se(clp.IntMapOrder(),0.0);       // Set all components to zero
    se[1] = 1.0;                                     // Set linear component to 1
    if (mapreader.HasGlobal()) {
      vector<double> old_global = mapreader.GetGlobal();
      if (old_global.size()==1) se[1] = old_global[0];
      else if (old_global.size()>1) for (unsigned int i=0; i<Min(se.size(),old_global.size()); i++) se[i] = old_global[i];
    }
    mymapper = boost::shared_ptr<IntensityMapper>(new SSDIntensityMapper(se));
  }
  else if (clp.IntMapType() == LOCAL_LINEAR) {
    boost::shared_ptr<basisfield>  fld;  // Null-pointer
    if (clp.Basis() == Spline) {
      if (mapreader.HasLocal()) {
        if (mapreader.LocalFieldSize()!=clp.RefSize()) {
          throw fnirt_error("init_intensity_mapper: Intensity Mapper file has different matrix-size from --ref");
	}
        if (mapreader.LocalFieldVoxelSize()!=clp.RefVxs()) {
	  throw fnirt_error("init_intensity_mapper: Intensity Mapper file has different voxel-size from --ref");
	}
	if (mapreader.NLocalFields()==1) {
          fld = boost::shared_ptr<basisfield>(new splinefield(mapreader.GetLocalAsSingleSplinefield(clp.FullResIntKsp())));
        }
        else if (mapreader.NLocalFields()>1) {
          fld = boost::shared_ptr<basisfield>(new splinefield(mapreader.GetLocalAsSingleSplinefield(clp.FullResIntKsp(),1)));
	}
      }
      else {
        fld = boost::shared_ptr<basisfield>(new splinefield(clp.RefSize(),clp.RefVxs(),clp.FullResIntKsp()));
        fld->SetToConstant(1.0);  // Make it fields of ones to start out with
      }
    }
    else if (clp.Basis() == DCT) {
      cout << "NYI" << endl;
    }
    mymapper = boost::shared_ptr<IntensityMapper>(new SSDIntensityMapper(fld,clp.IntLambda()));
  }
  else if (clp.IntMapType() == LOCAL_BIAS_WITH_GLOBAL_NON_LINEAR) {
    boost::shared_ptr<BASISFIELD::basisfield>  fld;  // Null-pointer
    if (clp.Basis() == Spline) {
      if (mapreader.HasLocal()) {
        if (mapreader.LocalFieldSize()!=clp.RefSize()) {
          throw fnirt_error("init_intensity_mapper: Intensity Mapper file has different matrix-size from --ref");
	}
        if (mapreader.LocalFieldVoxelSize()!=clp.RefVxs()) {
	  throw fnirt_error("init_intensity_mapper: Intensity Mapper file has different voxel-size from --ref");
	}
	if (mapreader.NLocalFields()==1) {
          fld = boost::shared_ptr<basisfield>(new splinefield(mapreader.GetLocalAsSingleSplinefield(clp.FullResIntKsp())));
        }
        else if (mapreader.NLocalFields()>1) {
          fld = boost::shared_ptr<basisfield>(new splinefield(mapreader.GetLocalAsSingleSplinefield(clp.FullResIntKsp(),1)));
	}
      }
      else {
        fld = boost::shared_ptr<BASISFIELD::basisfield>(new BASISFIELD::splinefield(clp.RefSize(),clp.RefVxs(),clp.FullResIntKsp()));
        fld->SetToConstant(1.0);  // Make it fields of ones to start out with
      }
    }
    else if (clp.Basis() == DCT) {
      cout << "NYI" << endl;
    }
    vector<double> se(clp.IntMapOrder(),0.0);
    se[1] = 1.0;                // Set linear component to 1
    if (mapreader.HasGlobal()) {
      vector<double> old_global = mapreader.GetGlobal();
      if (old_global.size()==1) se[1] = old_global[0];
      else if (old_global.size()>1) for (unsigned int i=0; i<Min(se.size(),old_global.size()); i++) se[i] = old_global[i];
    }
    mymapper = boost::shared_ptr<IntensityMapper>(new SSDIntensityMapper(se,fld,clp.IntLambda()));
  }
  else if (clp.IntMapType() == LOCAL_NON_LINEAR) {
    throw fnirt_error("init_intensity_mapper: Local non-linear not yet implemented");
  }
  return(mymapper);
}

void set_nlpars(NonlinParam&  nlp)
{
  if (nlp.Method() == NL_LM) {
    nlp.SetEquationSolverMaxIter(500);
    nlp.SetEquationSolverTol(1.0e-3);
  }
  else if (nlp.Method() == NL_CG) {
  }
  else if (nlp.Method() == NL_SCG) {
  }
}

// Combine an inclusive/exclusive explicit mask with an implicit mask

boost::shared_ptr<NEWIMAGE::volume<char> > make_mask(const string&                  mfname, 
                                                     MaskType                       mt, 
                                                     const NEWIMAGE::volume<float>& ima, 
                                                     bool                           impf, 
                                                     double                         impv)
{
  boost::shared_ptr<NEWIMAGE::volume<char> >  maskp;
  if (!((mfname.length() && mt!=IgnoreMask) || impf)) return(maskp); // Return null-pointer
  
  if (mfname.length() && mt!=IgnoreMask) { // If there is an explicit mask
    maskp = boost::shared_ptr<NEWIMAGE::volume<char> >(new NEWIMAGE::volume<char>);
    NEWIMAGE::volume<char>&  mask = *maskp;
    read_volume(mask,mfname);
    for (int k=0; k<mask.zsize(); k++) {
      for (int j=0; j<mask.ysize(); j++) {
        for (int i=0; i<mask.xsize(); i++) {
	  if (mask(i,j,k)!=1 && mask(i,j,k)!=0) throw fnirt_error("make_mask: Mask must be binary");
	  if (mt == ExclusiveMask) mask(i,j,k) = (mask(i,j,k)) ? 0 : 1;  // Turn exclusive to inclusive
	}
      }
    }
  }
  else { // No explicit mask then, but we know impf is true
    maskp = boost::shared_ptr<NEWIMAGE::volume<char> >(new NEWIMAGE::volume<char>(ima.xsize(),ima.ysize(),ima.zsize()));
    maskp->setdims(ima.xdim(),ima.ydim(),ima.zdim());
    *maskp = 1;
  }

  if (impf) { // Add implicit mask 
    NEWIMAGE::volume<char>&  mask = *maskp;
    for (int k=0; k<mask.zsize(); k++) {
      for (int j=0; j<mask.ysize(); j++) {
        for (int i=0; i<mask.xsize(); i++) {
          mask(i,j,k) = (fabs(double(ima(i,j,k))-impv) < 1e-16) ? 0 : mask(i,j,k);
	}
      }
    }
  }
  
  return(maskp);  
}

double spmlike_mean(NEWIMAGE::volume<float>&  ima)
{
  double mean = 0.0;
  // First pass to get mean of all voxels
  for (int k=0; k<ima.zsize(); k++) {
    for (int j=0; j<ima.ysize(); j++) {
      for (int i=0; i<ima.xsize(); i++) mean += ima(i,j,k);
    }
  }
  mean /= (ima.xsize()*ima.ysize()*ima.zsize());
  double thres = 0.125*mean;  // 1/8
  mean = 0.0;
  // Now take the mean of all voxels > 1/8 * mean
  int n = 0;
  for (int k=0; k<ima.zsize(); k++) {
    for (int j=0; j<ima.ysize(); j++) {
      for (int i=0; i<ima.xsize(); i++) {
        if (ima(i,j,k) > thres) {mean += ima(i,j,k); n++;}
      }
    }
  }
  mean /= n;
  
  return(mean);
}


////////////////////////////////////////////////////////////////////////////
//
// Check for attempt to register to self
//
////////////////////////////////////////////////////////////////////////////

bool trying_to_register_to_self(const string&                    ref_fname,
				const NEWIMAGE::volume<float>&   ref,
				const string&                    obj_fname,
				const NEWIMAGE::volume<float>&   obj,
                                const NEWMAT::Matrix&            aff)
{
  if (is_identity(aff)) {
    if (ref_fname == obj_fname) return(true);
    if (ref == obj) return(true);
  }
  
  return(false);
}

bool is_identity(const NEWMAT::Matrix&   A,
                 double                  prec)
{
  if (A.Nrows() != A.Ncols()) return(false);
  if ((A - IdentityMatrix(A.Nrows())).MaximumAbsoluteValue() < prec) return(true);
  return(false);
}

////////////////////////////////////////////////////////////////////////////
//
// Write results relevant to self-registration
//
////////////////////////////////////////////////////////////////////////////

void write_self_results(const fnirt_clp&                clp,
                        const NEWIMAGE::volume<float>&  ref)
{
  // Create a cf-object to do the job for us
  std::vector<boost::shared_ptr<basisfield> >   field = init_warpfield(clp);
  boost::shared_ptr<IntensityMapper>            intmap = init_intensity_mapper(clp);
  boost::shared_ptr<SSD_fnirt_CF>   cf = boost::shared_ptr<SSD_fnirt_CF>(new SSD_fnirt_CF(ref,ref,IdentityMatrix(4),field,intmap));

  cf->SaveDefCoefs(clp.CoefFname());                                      // Coefficients
  if (clp.FieldFname().length()) cf->SaveDefFields(clp.FieldFname());     // Field
  if (clp.JacFname().length()) cf->SaveJacobian(clp.JacFname());          // Jacobian
  if (clp.RefOutFname().length()) cf->SaveScaledRef(clp.RefOutFname());   // Intensity modulated ref scan
  if (clp.ObjOutFname().length()) cf->SaveScaledRef(clp.ObjOutFname());   // Warped object image
  // Intensity-mapping
  if (clp.IntensityMappingFname().length()) cf->SaveIntensityMapping(clp.IntensityMappingFname());  
}

////////////////////////////////////////////////////////////////////////////
//
// Try to find existing file matching the name ref_fname
// It is needed because when we read the filenames from 
// the configuration file the shell will not be able to
// do variable substitutions for us.
//
// It will
// 1. Check to see if there is an absolute path,
//    if so it will check for existence of that.
// 2. If no path explicitly given it will look 
//    in the current directory.
// 3. If no path given and not in the current 
//    directory it will look in ${FSLDIR}/data/standard
//
////////////////////////////////////////////////////////////////////////////

string existing_ref_fname(const string& ref_fname)
{
  if (FNIRT::path(ref_fname).length()) {      // If there is an explicit path
    if (NEWIMAGE::fsl_imageexists(ref_fname)) return(string(ref_fname));
    else return(string(""));
  }
  else {        // If no explicit path
    if (NEWIMAGE::fsl_imageexists(ref_fname)) { // Try to open it in current directory
      return(string(ref_fname));
    }
    else {
      const char *fsldir_ptr = getenv("FSLDIR");
      string eref_fname = string(fsldir_ptr) + string("/data/standard/") + ref_fname;
      if (NEWIMAGE::fsl_imageexists(eref_fname)) return(eref_fname);
      else return(string(""));
    }
  }
}
/*
string existing_ref_fname(const string& ref_fname)
{
  string   eref_fname;
  if (FNIRT::path(ref_fname).length()) {      // If there is an explicit path
    try {
      NEWIMAGE::volume<float> vref;
      NEWIMAGE::read_volume_hdr_only(vref,ref_fname); // Throws if file dont exist
      eref_fname = ref_fname;
    }
    catch(...) {
      return(string(""));     // Return empty string
    }
  }
  else {        // If no explicit path
    NEWIMAGE::volume<float> vref;
    try {       // Try to open it in current directory
      NEWIMAGE::read_volume_hdr_only(vref,ref_fname); // Throws if file dont exist
      eref_fname = ref_fname;
    }
    catch(...) { // Didn't exist in current directory, try in ${FSLDIR}/data/standard
      const char *fsldir_ptr = getenv("FSLDIR");
      eref_fname = string(fsldir_ptr) + string("/data/standard/") + ref_fname;
      try {
        cout << "Could not find " << ref_fname << ", now checking " << eref_fname << endl;
        NEWIMAGE::read_volume_hdr_only(vref,eref_fname); // Throws if file dont exist
      }
      catch(...) {
        return(string(""));
      }
    }
  }
  return(eref_fname);
}
*/
////////////////////////////////////////////////////////////////////////////
//
// Try to find existing file matching the name cfname. It will look for
// (in turn) i) cfname , ii) cfname + ".cnf" , 
// iii) $FSLDIR + "/etc/flirtsch/" + cfname and
// iv) $FSLDIR + "/etc/flirtsch/" + cfname + ".cnf"
// and returns the first one that exists.
//
////////////////////////////////////////////////////////////////////////////

string existing_conf_file(const string& cfname)
{
  string    ecfname;
  ifstream  ins;

  ecfname = cfname;
  if (check_exist(ecfname)) return(ecfname);
  if (!FNIRT::extension(ecfname).length()) {        // If no extension explicitly given
    ecfname += string(".cnf");
    if (check_exist(ecfname)) return(ecfname);
  }
  if (!FNIRT::path(cfname).length()) {              // If no path explicitly given
    const char *fsldir_ptr = getenv("FSLDIR");
    ecfname = string(fsldir_ptr) + string("/etc/flirtsch/") + cfname;
    if (check_exist(ecfname)) return(ecfname);
    else if (!FNIRT::extension(ecfname).length()) { // If no path _and_ no extension given
      ecfname += string(".cnf");
      if (check_exist(ecfname)) return(ecfname);
    }
  }
  return(string(""));
}

// New version that _might_ help a little

bool check_exist(const string& fname)
{
  // cout << "Attempting to open file named " << fname << endl;

  std::ifstream  ins(fname.c_str(),std::ios::in);
  return((ins) ? true : false);
}

/* Old version (used for first release that had a problem 
bool check_exist(const string& fname)
{
  std::ifstream  ins;

  cout << "Attempting to open file named " << fname << endl;
  ins.open(fname.c_str(),std::ios::in);
  ins.close();
  return((ins.fail()) ? false : true);
}
*/

string path(const string& fullname)
{
  string             pathv;
  string::size_type  idx = fullname.find_last_of("/");
  if (idx == string::npos) pathv = "";
  else pathv = fullname.substr(0,idx+1);

  return(pathv);
}

string filename(const string& fullname)
{
  string             fnamev;

  string::size_type  idx = fullname.find_last_of("/");
  if (idx == string::npos) fnamev = fullname;
  else fnamev = fullname.substr(idx+1);

  idx = fnamev.find_first_of(".");
  if (idx != string::npos) fnamev = fnamev.substr(0,idx);

  return(fnamev);
}

string extension(const string& fullname)
{
  string   extv;

  string::size_type  dotidx = fullname.find_last_of(".");
  if (dotidx != string::npos) {    // If there is a dot
    string::size_type  eopidx = fullname.find_last_of("/");
    if (eopidx != string::npos) {  // If there is an explicit path
      if (dotidx > eopidx) extv = fullname.substr(dotidx);
    }
    else extv = fullname.substr(dotidx);
  }
  else extv = "";

  return(extv);
}


} // End namespace FNIRT
