/*  Diffusion model fitting

    Timothy Behrens, Saad Jbabdi, Stam Sotiropoulos  - FMRIB Image Analysis Group
 
    Copyright (C) 2005 University of Oxford  */

/*  Part of FSL - FMRIB's Software Library
    http://www.fmrib.ox.ac.uk/fsl
    fsl@fmrib.ox.ac.uk
    
    Developed at FMRIB (Oxford Centre for Functional Magnetic Resonance
    Imaging of the Brain), Department of Clinical Neurology, Oxford
    University, Oxford, UK
    
    
    LICENCE
    
    FMRIB Software Library, Release 4.0 (c) 2007, The University of
    Oxford (the "Software")
    
    The Software remains the property of the University of Oxford ("the
    University").
    
    The Software is distributed "AS IS" under this Licence solely for
    non-commercial use in the hope that it will be useful, but in order
    that the University as a charitable foundation protects its assets for
    the benefit of its educational and research purposes, the University
    makes clear that no condition is made or to be implied, nor is any
    warranty given or to be implied, as to the accuracy of the Software,
    or that it will be suitable for any particular purpose or for use
    under any specific conditions. Furthermore, the University disclaims
    all responsibility for the use which is made of the Software. It
    further disclaims any liability for the outcomes arising from using
    the Software.
    
    The Licensee agrees to indemnify the University and hold the
    University harmless from and against any and all claims, damages and
    liabilities asserted by third parties (including claims for
    negligence) which arise directly or indirectly from the use of the
    Software or the sale of any products based on the Software.
    
    No part of the Software may be reproduced, modified, transmitted or
    transferred in any form or by any means, electronic or mechanical,
    without the express permission of the University. The permission of
    the University is not required if the said reproduction, modification,
    transmission or transference is done without financial return, the
    conditions of this Licence are imposed upon the receiver of the
    product, and all original and amended source code is included in any
    transmitted product. You may be held legally responsible for any
    copyright infringement that is caused or encouraged by your failure to
    abide by these terms and conditions.
    
    You are not permitted under this Licence to use this Software
    commercially. Use for which any financial return is received shall be
    defined as commercial use, and includes (1) integration of all or part
    of the source code or the Software into a product for sale or license
    by or on behalf of Licensee to third parties or (2) use of the
    Software or any derivative of it for research with the final aim of
    developing software products for sale or license to a third party or
    (3) use of the Software or any derivative of it for research with the
    final aim of developing non-software products for sale or license to a
    third party, or (4) use of the Software to provide any service to an
    external organisation for which payment is received. If you are
    interested in using the Software commercially, please contact Isis
    Innovation Limited ("Isis"), the technology transfer company of the
    University, to negotiate a licence. Contact details are:
    innovation@isis.ox.ac.uk quoting reference DE/1112. */

#include "diffmodels.h"



////////////////////////////////////////////////
//       DIFFUSION TENSOR MODEL
////////////////////////////////////////////////
void DTI::linfit(){
  ColumnVector logS(npts);
  ColumnVector Dvec(7);
  for (int i=1;i<=npts; i++){
    if(Y(i)>0)
      logS(i)=log(Y(i));
    else
      logS(i)=0;
  }
  Dvec = -iAmat*logS;
  if(Dvec(7)>-23)
    m_s0=exp(-Dvec(7));
  else
    m_s0=Y.MaximumAbsoluteValue();
  for (int i=1;i<=Y.Nrows();i++){
    if(m_s0<Y.Sum()/Y.Nrows()){ m_s0=Y.MaximumAbsoluteValue();  }
    logS(i)=(Y(i)/m_s0)>0.01 ? log(Y(i)):log(0.01*m_s0);
  }
  Dvec = -iAmat*logS;
  m_sse = (Amat*Dvec+logS).SumSquare();
  m_s0=exp(-Dvec(7));
  if(m_s0<Y.Sum()/Y.Nrows()){ m_s0=Y.Sum()/Y.Nrows();  }
  vec2tens(Dvec);
  calc_tensor_parameters();

  m_covar.ReSize(7);
  float dof=logS.Nrows()-7;
  float sig2=m_sse/dof;
  m_covar << sig2*(Amat.t()*Amat).i();
}
ColumnVector DTI::calc_md_grad(const ColumnVector& _tens)const{
  ColumnVector g(6);
  g = 0;
  g(1) = 1/3.0;
  g(4) = 1/3.0;
  g(6) = 1/3.0;
  return g;
}
// this will only work if the determinant is strictly positive
ReturnMatrix DTI::calc_fa_grad(const ColumnVector& _intens)const{
  ColumnVector gradv(6),ik(6),k(6);
  float m = (_intens(1)+_intens(4)+_intens(6))/3.0;
  SymmetricMatrix K(3),iK(3),M(3);

  // rescale input matrix
  vec2tens(_intens,M);
  //M /=m;

  m = M.Trace()/3.0;

  K = M - m*IdentityMatrix(3);
  tens2vec(K,k);
  iK << K.i();
  tens2vec(iK,ik);

  float p   = K.SumSquare()/6.0;
  float q   = K.Determinant()/2.0;
  float h   = std::sqrt(p*p*p-q*q)/q;
  float phi = std::atan(h)/3.0;
  if(q<0)phi+=M_PI;


  float _l1 = m + 2.0*std::sqrt(p)*std::cos(phi);
  float _l2 = m - std::sqrt(p)*(std::cos(phi)+std::sqrt(3.0)*std::sin(phi));
  float _l3 = m - std::sqrt(p)*(std::cos(phi)-std::sqrt(3.0)*std::sin(phi));

  float t  = 6.0/9.0*(_l1*_l1+_l2*_l2+_l3*_l3 - _l1*_l2-_l1*_l3-_l2*_l3);
  float b  = _l1*_l1+_l2*_l2+_l3*_l3;

  float _fa  = std::sqrt(3.0/2.0)*std::sqrt(t/b);
  

  float dfadl1 = 3.0/4.0/_fa * ( 6.0/9.0*(2.0*_l1-_l2-_l3)/b - t/b/b*2.0*_l1 );
  float dfadl2 = 3.0/4.0/_fa * ( 6.0/9.0*(2.0*_l2-_l1-_l3)/b - t/b/b*2.0*_l2 );
  float dfadl3 = 3.0/4.0/_fa * ( 6.0/9.0*(2.0*_l3-_l1-_l2)/b - t/b/b*2.0*_l3 );

  

  // determine dkdx
  ColumnVector dkdx(6);
  dkdx << 2.0/3.0 << 1.0 << 1.0 << 2.0/3.0 << 1.0 << 2.0/3.0;

  for(int i=1;i<=6;i++){
    float dL1dx=0,dL2dx=0,dL3dx=0;
    if(i==1||i==4||i==6){
      dL1dx=1.0/3.0;dL2dx=1.0/3.0;dL3dx=1.0/3.0;
    }
    //
    float p_p = k(i)/3.0 * dkdx(i);
    float q_p = q*ik(i) * dkdx(i);
    float h_p = (3.0*p*p*p_p - 2.0*q_p*q*(1+h*h))/2.0/h/q/q;

    float phi_p = h_p/(1+h*h)/3.0;

    dL1dx += p_p/std::sqrt(p)*std::cos(phi) - 2.0*std::sqrt(p)*phi_p*std::sin(phi);
    dL2dx -= std::sqrt(p)*(.5*p_p*(m-_l2)+phi_p*(std::sin(phi)-std::sqrt(3.0)*std::cos(phi)));
    dL3dx -= std::sqrt(p)*(.5*p_p*(m-_l3)+phi_p*(std::sin(phi)+std::sqrt(3.0)*std::cos(phi)));

    //
    gradv(i) = dfadl1*dL1dx + dfadl2*dL2dx + dfadl3*dL3dx;
  }
  gradv.Release();
  return gradv;
}
float DTI::calc_fa_var()const{
  ColumnVector grd;
  ColumnVector vtens;
  tens2vec(m_tens,vtens);
  grd = calc_fa_grad(vtens);
  ColumnVector g(7);
  g.SubMatrix(1,6,1,1) = grd;
  g(7) = 0;
  
  return((g.t()*m_covar*g).AsScalar());
}

void DTI::rot2angles(const Matrix& rot,float& th1,float& th2,float& th3)const{
  if(rot(3,1)!=1 && rot(3,1)!=-1){
    th2 = -asin(rot(3,1));
    float c=std::cos(th2);
    th1 = atan2(rot(3,2)/c,rot(3,3)/c);
    th3 = atan2(rot(2,1)/c,rot(1,1)/c);
  }
  else{
    th1 = atan2(rot(1,2),rot(1,3));
    th2 = -sign(rot(3,1))*M_PI/2;
    th3 = 0;
  }
}
void DTI::angles2rot(const float& th1,const float& th2,const float& th3,Matrix& rot)const{
  float c1=std::cos(th1),s1=std::sin(th1);
  float c2=std::cos(th2),s2=std::sin(th2);
  float c3=std::cos(th3),s3=std::sin(th3);

  rot(1,1) = c2*c3;    rot(1,2) = s1*s2*c3-c1*s3;    rot(3,1) = c1*s2*c3+s1*s3;
  rot(2,1) = c2*s3;    rot(2,2) = s1*s2*s3+c1*c3;    rot(3,2) = c1*s2*s3-s1*c3;
  rot(3,1) = -s2;      rot(3,2) = s1*c2;             rot(3,3) = c1*c2;
}


// nonlinear tensor fitting 
void DTI::nonlinfit(){
  // initialise with linear fitting
  linfit();

  print();

  // set starting parameters
  // params = s0, log(l1),log(l2), log(l3), th1, th2, th3
  ColumnVector start(nparams);

  start(1) = m_s0;
  // eigenvalues
  start(2) = m_l1>0?std::log(m_l1):std::log(1e-5);
  start(3) = m_l2>0?std::log(m_l2):std::log(1e-5);
  start(4) = m_l3>0?std::log(m_l3):std::log(1e-5);
  // angles
  float th1,th2,th3;
  Matrix rot(3,3);
  rot.Row(1) = m_v1.t();
  rot.Row(2) = m_v2.t();
  rot.Row(3) = m_v3.t();
  rot2angles(rot,th1,th2,th3);
  start(5) = th1;
  start(6) = th2;
  start(7) = th3;


  // do the fit
  NonlinParam  lmpar(start.Nrows(),NL_LM); 
  lmpar.SetGaussNewtonType(LM_L);
  lmpar.SetStartingEstimate(start);


  NonlinOut status;
  status = nonlin(lmpar,(*this));
  ColumnVector final_par(nparams);
  final_par = lmpar.Par();


  // finalise parameters
  m_s0 = final_par(1);
  m_l1 = exp(final_par(2));
  m_l2 = exp(final_par(3));
  m_l3 = exp(final_par(4));

  angles2rot(final_par(5),final_par(6),final_par(7),rot);
  m_v1 = rot.Row(1).t();
  m_v2 = rot.Row(2).t();
  m_v3 = rot.Row(3).t();

  sort();

  m_tens << m_l1*m_v1*m_v1.t() + m_l2*m_v2*m_v2.t() + m_l3*m_v3*m_v3.t();
  calc_tensor_parameters();

  print();
  //exit(1);

}
void DTI::sort(){
  vector< pair<float,int> > ls(3);
  vector<ColumnVector> vs(3);
  ls[0].first=m_l1;
  ls[0].second=0;
  ls[1].first=m_l2;
  ls[1].second=1;
  ls[2].first=m_l3;
  ls[2].second=2;
  vs[0]=m_v1;vs[1]=m_v2;vs[2]=m_v3;
  
  std::sort(ls.begin(),ls.end());

  m_l1 = ls[2].first;
  m_v1 = vs[ ls[2].second ];
  m_l2 = ls[1].first;
  m_v2 = vs[ ls[1].second ];
  m_l3 = ls[0].first;
  m_v3 = vs[ ls[0].second ];
  
}
void DTI::calc_tensor_parameters(){
  Matrix Vd;
  DiagonalMatrix Dd(3);
  // mean, eigenvalues and eigenvectors
  EigenValues(m_tens,Dd,Vd);
  m_md = Dd.Sum()/Dd.Nrows();
  m_l1 = Dd(3,3);
  m_l2 = Dd(2,2);
  m_l3 = Dd(1,1);
  m_v1 = Vd.Column(3);
  m_v2 = Vd.Column(2);
  m_v3 = Vd.Column(1);
  // mode
  float e1=m_l1-m_md, e2=m_l2-m_md, e3=m_l3-m_md;
  float n = (e1 + e2 - 2*e3)*(2*e1 - e2 - e3)*(e1 - 2*e2 + e3);
  float d = (e1*e1 + e2*e2 + e3*e3 - e1*e2 - e2*e3 - e1*e3);
  d = sqrt(bigger(0, d));
  d = 2*d*d*d;
  m_mo = smaller(bigger(d ? n/d : 0.0, -1),1);
  // fa
  float numer=1.5*((m_l1-m_md)*(m_l1-m_md)+(m_l2-m_md)*(m_l2-m_md)+(m_l3-m_md)*(m_l3-m_md));
  float denom=(m_l1*m_l1+m_l2*m_l2+m_l3*m_l3);
  if(denom>0) m_fa=numer/denom;
  else m_fa=0;
  if(m_fa>0){m_fa=sqrt(m_fa);}
  else{m_fa=0;}
}
// now the nonlinear fitting routines
double DTI::cf(const NEWMAT::ColumnVector& p)const{
  //cout << "CF" << endl;
  //OUT(p.t());
  double cfv = 0.0;
  double err = 0.0;
  ////////////////////////////////////
  ColumnVector ls(3);
  Matrix rot(3,3);
  angles2rot(p(5),p(6),p(7),rot);
  for(int k=2;k<=4;k++){
    ls(k-1) = exp(p(k));
  }
  ////////////////////////////////////
  for(int i=1;i<=Y.Nrows();i++){
    err = p(1)*anisoterm(i,ls,rot) - Y(i); 
    cfv += err*err; 
  }  
  //OUT(cfv);
  //cout<<"--------"<<endl;
  return(cfv);
}

NEWMAT::ReturnMatrix DTI::grad(const NEWMAT::ColumnVector& p)const{
  NEWMAT::ColumnVector gradv(p.Nrows());

  cout<<"grad"<<endl;
  OUT(p.t());

  gradv = 0.0;
  ////////////////////////////////////
  ColumnVector ls(3);
  Matrix rot(3,3);
  Matrix rot1(3,3),rot2(3,3),rot3(3,3);
  angles2rot(p(5),p(6),p(7),rot);

  angles2rot(p(5)+M_PI/2.0,p(6),p(7),rot1);
  angles2rot(p(5),p(6)+M_PI/2.0,p(7),rot2);
  angles2rot(p(5),p(6),p(7)+M_PI/2.0,rot3);
  for(int k=2;k<=4;k++){
    ls(k-1) = exp(p(k));
  }
  ////////////////////////////////////
  Matrix J(npts,nparams);
  ColumnVector x(3);
  ColumnVector diff(npts);
  float sig;
  for(int i=1;i<=Y.Nrows();i++){
    sig = p(1)*anisoterm(i,ls,rot);
    
    J(i,1) = sig/p(1);

    x = rotproduct(bvecs.Column(i),rot);
    J(i,2) = -bvals(1,i)*x(1)*sig*ls(1);
    J(i,3) = -bvals(1,i)*x(2)*sig*ls(2);
    J(i,4) = -bvals(1,i)*x(3)*sig*ls(3);

    x = rotproduct(bvecs.Column(i),rot1,rot);
    J(i,5) = -2.0*bvals(1,i)*(ls(1)*x(1)+ls(2)*x(2)+ls(3)*x(3))*sig;
    x = rotproduct(bvecs.Column(i),rot2,rot);
    J(i,6) = -2.0*bvals(1,i)*(ls(1)*x(1)+ls(2)*x(2)+ls(3)*x(3))*sig;
    x = rotproduct(bvecs.Column(i),rot3,rot);
    J(i,7) = -2.0*bvals(1,i)*(ls(1)*x(1)+ls(2)*x(2)+ls(3)*x(3))*sig;

    diff(i) = sig - Y(i);
  }

  OUT(diff.t());
  OUT(J.t());
  
  gradv = 2.0*J.t()*diff;

  OUT(gradv.t());
  cout<<"------"<<endl;

  gradv.Release();
  return gradv;
}

//this uses Gauss-Newton approximation
boost::shared_ptr<BFMatrix> DTI::hess(const NEWMAT::ColumnVector& p,boost::shared_ptr<BFMatrix> iptr)const{
  boost::shared_ptr<BFMatrix>   hessm;
  if (iptr && iptr->Nrows()==(unsigned int)p.Nrows() && iptr->Ncols()==(unsigned int)p.Nrows()) hessm = iptr;
  else hessm = boost::shared_ptr<BFMatrix>(new FullBFMatrix(p.Nrows(),p.Nrows()));

  cout<<"hess"<<endl;
  OUT(p.t());
  
  ////////////////////////////////////
  ColumnVector ls(3);
  Matrix rot(3,3);
  Matrix rot1(3,3),rot2(3,3),rot3(3,3);
  angles2rot(p(5),p(6),p(7),rot);

  angles2rot(p(5)+M_PI/2,p(6),p(7),rot1);
  angles2rot(p(5),p(6)+M_PI/2,p(7),rot2);
  angles2rot(p(5),p(6),p(7)+M_PI/2,rot3);
  for(int k=2;k<=4;k++){
    ls(k-1) = exp(p(k));
  }
  ////////////////////////////////////
  Matrix J(npts,nparams);
  ColumnVector x(3);
  float sig;
  for(int i=1;i<=Y.Nrows();i++){
    sig = p(1)*anisoterm(i,ls,rot);
    
    J(i,1) = sig/p(1);

    x = rotproduct(bvecs.Column(i),rot);
    J(i,2) = -bvals(1,i)*x(1)*sig*ls(1);
    J(i,3) = -bvals(1,i)*x(2)*sig*ls(2);
    J(i,4) = -bvals(1,i)*x(3)*sig*ls(3);

    x = rotproduct(bvecs.Column(i),rot1,rot);
    J(i,5) = -2.0*bvals(1,i)*(ls(1)*x(1)+ls(2)*x(2)+ls(3)*x(3))*sig;
    x = rotproduct(bvecs.Column(i),rot2,rot);
    J(i,6) = -2.0*bvals(1,i)*(ls(1)*x(1)+ls(2)*x(2)+ls(3)*x(3))*sig;
    x = rotproduct(bvecs.Column(i),rot3,rot);
    J(i,7) = -2.0*bvals(1,i)*(ls(1)*x(1)+ls(2)*x(2)+ls(3)*x(3))*sig;
  }
  

  for (int i=1; i<=p.Nrows(); i++){
    for (int j=i; j<=p.Nrows(); j++){
      sig = 0.0;
      for(int k=1;k<=J.Nrows();k++)
	sig += 2.0*(J(k,i)*J(k,j));
      hessm->Set(i,j,sig);
    }
  }
  for (int j=1; j<=p.Nrows(); j++) {
    for (int i=j+1; i<=p.Nrows(); i++) {
      hessm->Set(i,j,hessm->Peek(j,i));
    }
  }

  hessm->Print();
  cout<<"------"<<endl;

  return(hessm);
}

ColumnVector DTI::rotproduct(const ColumnVector& x,const Matrix& R)const{
  ColumnVector ret(3);
  
  for(int i=1;i<=3;i++)
    ret(i) = x(1)*x(1)*R(1,i)*R(1,i)+x(2)*x(2)*R(2,i)*R(2,i)+x(3)*x(3)*R(3,i)*R(3,i)
      +2.0*( x(1)*R(1,i)*(x(2)*R(2,i)+x(3)*R(3,i)) +x(2)*x(3)*R(2,i)*R(3,i) );   
  
  return ret;
}
ColumnVector DTI::rotproduct(const ColumnVector& x,const Matrix& R1,const Matrix& R2)const{
  ColumnVector ret(3);
  
  for(int i=1;i<=3;i++)
    ret(i) = x(1)*x(1)*R1(1,i)*R2(1,i)+x(2)*x(2)*R1(2,i)*R2(2,i)+x(3)*x(3)*R1(3,i)*R2(3,i)
      +( x(1)*R1(1,i)*(x(2)*R2(2,i)+x(3)*R2(3,i)) +x(2)*x(3)*R1(2,i)*R2(3,i) )
      +( x(1)*R2(1,i)*(x(2)*R1(2,i)+x(3)*R1(3,i)) +x(2)*x(3)*R2(2,i)*R1(3,i) );   
  
  return ret;
}

float DTI::anisoterm(const int& pt,const ColumnVector& ls,const Matrix& rot)const{
  ColumnVector x(3);
  x = rotproduct(bvecs.Column(pt),rot);

  return exp(-bvals(1,pt)*(ls(1)*x(1)+ls(2)*x(2)+ls(3)*x(3)));
}




/////////////////////////////////////////////////////////////////////////
//       PARTIAL VOLUME MODEL - SINGLE SHELL 
// Constrained Optimization for the diffusivity, fractions and their sum<1
//////////////////////////////////////////////////////////////////////////

void PVM_single_c::fit(){

  // initialise with a tensor
  DTI dti(Y,bvecs,bvals);
  dti.linfit();

  // set starting parameters for nonlinear fitting
  float _th,_ph;
  cart2sph(dti.get_v1(),_th,_ph);

  ColumnVector start(nparams);
  //Initialize the non-linear fitter. Use the DTI estimates for most parameters, apart from the volume fractions
  start(1) = dti.get_s0();
  start(2) = d2lambda(dti.get_md()>0?dti.get_md()*2:0.001); // empirically found that d~2*MD
  start(4) = _th;
  start(5) = _ph;
  for(int ii=2,i=6;ii<=nfib;ii++,i+=3){
    cart2sph(dti.get_v(ii),_th,_ph);
    start(i+1) = _th;
    start(i+2) = _ph;
  }
  
  // do a better job for initializing the volume fractions
  fit_pvf(start);

  // do the fit
  NonlinParam  lmpar(start.Nrows(),NL_LM); 
  lmpar.SetGaussNewtonType(LM_L);
  lmpar.SetStartingEstimate(start);

  NonlinOut status;
  status = nonlin(lmpar,(*this));
  ColumnVector final_par(nparams);
  final_par = lmpar.Par();
  
  // finalise parameters
  m_s0 = final_par(1);
  m_d  = lambda2d(final_par(2));
  for(int k=1;k<=nfib;k++){
    int kk = 3 + 3*(k-1);

    m_f(k)  = beta2f(final_par(kk))*partial_fsum(m_f,k-1);
    m_th(k) = final_par(kk+1);
    m_ph(k) = final_par(kk+2);
  }
  if (m_include_f0)
    m_f0=beta2f(final_par(nparams))*partial_fsum(m_f,nfib);
  sort();
  //  print();
}

void PVM_single_c::sort(){
  vector< pair<float,int> > fvals(nfib);
  ColumnVector ftmp(nfib),thtmp(nfib),phtmp(nfib);
  ftmp=m_f;thtmp=m_th;phtmp=m_ph;
  for(int i=1;i<=nfib;i++){
    pair<float,int> p(m_f(i),i);
    fvals[i-1] = p;
  }
  std::sort(fvals.begin(),fvals.end());
  for(int i=1,ii=nfib-1;ii>=0;i++,ii--){
    m_f(i)  = ftmp(fvals[ii].second);
    m_th(i) = thtmp(fvals[ii].second);
    m_ph(i) = phtmp(fvals[ii].second);
  }
}


//If the sum of the fractions is >1, then zero as many fractions
//as necessary, so that the sum becomes smaller than 1.
void PVM_single_c::fix_fsum(ColumnVector& fs)const{
  float sumf=0;
  for(int i=1;i<=nfib;i++){
    sumf+=fs(i);
    if(sumf>=1){
      for(int j=i;j<=nfib;j++) 
	fs(j)=0;
      break;
    }
  }
}


//Find the volume fractions given all the other model 
//parameters using Linear Least Squares
void PVM_single_c::fit_pvf(ColumnVector& x)const{
  ColumnVector fs(nfib);
  ColumnVector Y_I(npts);
  Matrix       M(npts,nfib),dir(3,nfib);
  float s0=x(1),d=lambda2d(x(2)), f0;
  for(int k=1;k<=nfib;k++){
    int kk = 3+3*(k-1);
    dir(1,k) = sin(x(kk+1))*cos(x(kk+2));
    dir(2,k) = sin(x(kk+1))*sin(x(kk+2));
    dir(3,k) = cos(x(kk+1));
  }
  ////////////////////////////////////
  for(int i=1;i<=npts;i++){
    float Iso_term=isoterm(i,d);
    Y_I(i) = Y(i)-s0*Iso_term;
    for(int k=1;k<=nfib;k++){
      M(i,k)=s0*(anisoterm(i,d,dir.Column(k))-Iso_term);
    }
    //if (m_include_f0)
    //M(i,f_num)=s0*(1-Iso_term);
  }
  fs = pinv(M)*Y_I;
  if (m_include_f0){
    f0=0.001; fs(1)-=f0;  //Initialize f0 with a very small value
  }

  for(int k=1;k<=nfib;k++)
    fs(k)=fabs(fs(k));    //make sure that the initial values for the fractions are positive
  
  fix_fsum(fs);

  for(int k=1;k<=nfib;k++)
    x(3+3*(k-1))=f2beta(fs(k)/partial_fsum(fs,k-1));

  if (m_include_f0)
    x(nparams)=f2beta(f0/partial_fsum(fs,nfib));
}


ReturnMatrix PVM_single_c::get_prediction()const{
  ColumnVector pred(npts);
  ColumnVector p(nparams);
  ColumnVector fs(nfib);
  
  fs=m_f;
  p(1) = m_s0;
  p(2) = d2lambda(m_d);
  for(int i=3,ii=1;ii<=nfib;i+=3,ii++){
    p(i)   = f2beta(m_f(ii)/partial_fsum(fs,ii-1));
    p(i+1) = m_th(ii);
    p(i+2) = m_ph(ii);
  }
  if (m_include_f0)
    p(nparams)=f2beta(m_f0/partial_fsum(fs,nfib));
  pred = forwardModel(p);
  pred.Release();
  return pred;
}


NEWMAT::ReturnMatrix PVM_single_c::forwardModel(const NEWMAT::ColumnVector& p)const{
  ColumnVector pred(npts);
  pred = 0;
  float val;
  float _d = lambda2d(p(2));
  ////////////////////////////////////
  ColumnVector fs(nfib);
  Matrix x(nfib,3);
  float sumf=0;
  for(int k=1;k<=nfib;k++){
    int kk = 3+3*(k-1);
    fs(k) = beta2f(p(kk))*partial_fsum(fs,k-1);
    sumf += fs(k);
    x(k,1) = sin(p(kk+1))*cos(p(kk+2));
    x(k,2) = sin(p(kk+1))*sin(p(kk+2));
    x(k,3) = cos(p(kk+1));
  }
  ////////////////////////////////////
  for(int i=1;i<=Y.Nrows();i++){
    val = 0.0;
    for(int k=1;k<=nfib;k++){
      val += fs(k)*anisoterm(i,_d,x.Row(k).t());
    }
    if (m_include_f0){
      float temp_f0=beta2f(p(nparams))*partial_fsum(fs,nfib);
      pred(i) = p(1)*(temp_f0+(1-sumf-temp_f0)*isoterm(i,_d)+val);
    } 
    else
      pred(i) = p(1)*((1-sumf)*isoterm(i,_d)+val); 
  }  
  pred.Release();
  return pred;
}


//Cost Function, sum of squared residuals
double PVM_single_c::cf(const NEWMAT::ColumnVector& p)const{
  //cout<<"CF"<<endl;
  //OUT(p.t());
  double cfv = 0.0;
  double err;
  float _d = lambda2d(p(2));
  ////////////////////////////////////
  ColumnVector fs(nfib);
  Matrix x(nfib,3);
  float sumf=0;
  for(int k=1;k<=nfib;k++){
    int kk = 3+3*(k-1);
    fs(k) = beta2f(p(kk))*partial_fsum(fs,k-1);
    sumf += fs(k);
    x(k,1) = sin(p(kk+1))*cos(p(kk+2));
    x(k,2) = sin(p(kk+1))*sin(p(kk+2));
    x(k,3) = cos(p(kk+1));
  }
  ////////////////////////////////////
  for(int i=1;i<=Y.Nrows();i++){
    err = 0.0;
    for(int k=1;k<=nfib;k++){
      err += fs(k)*anisoterm(i,_d,x.Row(k).t());
    }
    if (m_include_f0){
      float temp_f0=beta2f(p(nparams))*partial_fsum(fs,nfib);
      err = (p(1)*(temp_f0+(1-sumf-temp_f0)*isoterm(i,_d)+err) - Y(i)); 
    }
    else
      err = (p(1)*((1-sumf)*isoterm(i,_d)+err) - Y(i)); 
    cfv += err*err; 
  }  
  return(cfv);
}

NEWMAT::ReturnMatrix PVM_single_c::grad(const NEWMAT::ColumnVector& p)const{
  //cout<<"GRAD"<<endl;
  //OUT(p.t());
  NEWMAT::ColumnVector gradv(p.Nrows());
  gradv = 0.0;
  float _d = lambda2d(p(2));
  ////////////////////////////////////
  ColumnVector fs(nfib);
  ColumnVector bs(nfib);
  Matrix x(nfib,3);ColumnVector xx(3); ColumnVector yy(3);
  float sumf=0;
  
  for(int k=1;k<=nfib;k++){
    int kk = 3+3*(k-1);
    bs(k)=p(kk);
    fs(k) = beta2f(p(kk))*partial_fsum(fs,k-1);
    sumf += fs(k);
    x(k,1) = sin(p(kk+1))*cos(p(kk+2));
    x(k,2) = sin(p(kk+1))*sin(p(kk+2));
    x(k,3) = cos(p(kk+1));
  }

  ////////////////////////////////////
  Matrix f_deriv;
  //Compute the derivatives with respect to betas, i.e the transformed volume fraction variables
  f_deriv=fractions_deriv(nfib, fs, bs);  

  Matrix J(npts,nparams);
  ColumnVector diff(npts);
  float sig, Iso_term; 
  ColumnVector Aniso_terms(nfib);

  for(int i=1;i<=Y.Nrows();i++){
    Iso_term=isoterm(i,_d);  //Precompute some terms for this datapoint
    for(int k=1;k<=nfib;k++){
      xx = x.Row(k).t();
      Aniso_terms(k)=anisoterm(i,_d,xx);
    }
    sig = 0;
    J.Row(i)=0;
    for(int k=1;k<=nfib;k++){
      int kk = 3+3*(k-1);
      xx = x.Row(k).t();
      sig += fs(k)*Aniso_terms(k); //Total signal
      // other stuff for derivatives
      // lambda (i.e. d)
      J(i,2) += p(1)*fs(k)*anisoterm_lambda(i,p(2),xx);
      
      // beta (i.e. f)
      J(i,kk)=0;
      for (int j=1; j<=nfib; j++){
	if (f_deriv(j,k)!=0)
	  J(i,kk) += p(1)*(Aniso_terms(j)-Iso_term)*f_deriv(j,k);
      }
      // th
      J(i,kk+1) = p(1)*fs(k)*anisoterm_th(i,_d,xx,p(kk+1),p(kk+2));
      // ph
      J(i,kk+2) = p(1)*fs(k)*anisoterm_ph(i,_d,xx,p(kk+1),p(kk+2));
    }
    if (m_include_f0){
      float temp_f0=beta2f(p(nparams))*partial_fsum(fs,nfib);
      //derivative with respect to f0
      J(i,nparams)= p(1)*(1-Iso_term)*sin(2*p(nparams))*partial_fsum(fs,nfib);
      sig=p(1)*(temp_f0+(1-sumf-temp_f0)*Iso_term+sig);
      J(i,2) += p(1)*(1-sumf-temp_f0)*isoterm_lambda(i,p(2));
    }
    else{
      sig = p(1)*((1-sumf)*Iso_term+sig);
      J(i,2) += p(1)*(1-sumf)*isoterm_lambda(i,p(2)); //lambda
    }
    diff(i) = sig - Y(i);
    J(i,1) = sig/p(1);  //S0
  }
  
  gradv = 2*J.t()*diff;
  gradv.Release();
  return gradv;


}
//this uses Gauss-Newton approximation
boost::shared_ptr<BFMatrix> PVM_single_c::hess(const NEWMAT::ColumnVector& p,boost::shared_ptr<BFMatrix> iptr)const{
  //cout<<"HESS"<<endl;
  //OUT(p.t());
  boost::shared_ptr<BFMatrix>   hessm;
  if (iptr && iptr->Nrows()==(unsigned int)p.Nrows() && iptr->Ncols()==(unsigned int)p.Nrows()) hessm = iptr;
  else hessm = boost::shared_ptr<BFMatrix>(new FullBFMatrix(p.Nrows(),p.Nrows()));

  float _d = lambda2d(p(2));
  ////////////////////////////////////
  ColumnVector fs(nfib);
  ColumnVector bs(nfib);
  Matrix x(nfib,3);ColumnVector xx(3); ColumnVector yy(3);
  float sumf=0;
  for(int k=1;k<=nfib;k++){
    int kk = 3+3*(k-1);
    bs(k)=p(kk);
    fs(k) = beta2f(p(kk))*partial_fsum(fs,k-1);
    sumf += fs(k);
    x(k,1) = sin(p(kk+1))*cos(p(kk+2));
    x(k,2) = sin(p(kk+1))*sin(p(kk+2));
    x(k,3) = cos(p(kk+1));
  }
  ////////////////////////////////////
  Matrix f_deriv;
  f_deriv=fractions_deriv(nfib, fs, bs);

  Matrix J(npts,nparams);
  float sig, Iso_term; 
  ColumnVector Aniso_terms(nfib);

  for(int i=1;i<=Y.Nrows();i++){
    Iso_term=isoterm(i,_d);  //Precompute some terms for this datapoint
    for(int k=1;k<=nfib;k++){
      xx = x.Row(k).t();
      Aniso_terms(k)=anisoterm(i,_d,xx);
    }
    sig = 0;
    J.Row(i)=0;
    for(int k=1;k<=nfib;k++){
      int kk = 3+3*(k-1);
      xx = x.Row(k).t();
      sig += fs(k)*Aniso_terms(k); //Total signal
      // other stuff for derivatives
      // lambda (i.e. d)
      J(i,2) += p(1)*fs(k)*anisoterm_lambda(i,p(2),xx);
      
      // beta (i.e. f)
      J(i,kk)=0;
      for (int j=1; j<=nfib; j++){
	if (f_deriv(j,k)!=0)
	  J(i,kk) += p(1)*(Aniso_terms(j)-Iso_term)*f_deriv(j,k);
      }
      // th
      J(i,kk+1) = p(1)*fs(k)*anisoterm_th(i,_d,xx,p(kk+1),p(kk+2));
      // ph
      J(i,kk+2) = p(1)*fs(k)*anisoterm_ph(i,_d,xx,p(kk+1),p(kk+2));
    }
    if (m_include_f0){
      float temp_f0=beta2f(p(nparams))*partial_fsum(fs,nfib);
      //derivative with respect to f0
      J(i,nparams)= p(1)*(1-Iso_term)*sin(2*p(nparams))*partial_fsum(fs,nfib);
      sig=p(1)*(temp_f0+(1-sumf-temp_f0)*Iso_term+sig);
      J(i,2) += p(1)*(1-sumf-temp_f0)*isoterm_lambda(i,p(2));
    }
    else{
      sig = p(1)*((1-sumf)*Iso_term+sig);
      J(i,2) += p(1)*(1-sumf)*isoterm_lambda(i,p(2)); //lambda
    }
    J(i,1) = sig/p(1);  //S0
  }

  for (int i=1; i<=p.Nrows(); i++){
    for (int j=i; j<=p.Nrows(); j++){
      sig = 0.0;
      for(int k=1;k<=J.Nrows();k++)
	sig += 2*(J(k,i)*J(k,j));
      hessm->Set(i,j,sig);
    }
  }
  for (int j=1; j<=p.Nrows(); j++) {
    for (int i=j+1; i<=p.Nrows(); i++) {
      hessm->Set(i,j,hessm->Peek(j,i));
    }
  }
  return(hessm);
}



////////////////////////////////////////////////
//       PARTIAL VOLUME MODEL - SINGLE SHELL (OLD)
////////////////////////////////////////////////

void PVM_single::fit(){

  // initialise with a tensor
  DTI dti(Y,bvecs,bvals);
  dti.linfit();

  // set starting parameters for nonlinear fitting
  float _th,_ph;
  cart2sph(dti.get_v1(),_th,_ph);

  ColumnVector start(nparams);
  start(1) = dti.get_s0();
  start(2) = dti.get_md()>0?dti.get_md()*2:0.001; // empirically found that d~2*MD
  start(3) = dti.get_fa()<1?f2x(dti.get_fa()):f2x(0.95); // first pvf = FA 
  start(4) = _th;
  start(5) = _ph;
  float sumf=x2f(start(3));
  float tmpsumf=sumf;
  for(int ii=2,i=6;ii<=nfib;ii++,i+=3){
    float denom=2;
    do{
      start(i) = f2x(x2f(start(i-3))/denom);
      denom *= 2;
      tmpsumf = sumf + x2f(start(i));
    }while(tmpsumf>=1);
    sumf += x2f(start(i));
    cart2sph(dti.get_v(ii),_th,_ph);
    start(i+1) = _th;
    start(i+2) = _ph;
  }
  if (m_include_f0)
    start(nparams)=f2x(0.001);
 
  // do the fit
  NonlinParam  lmpar(start.Nrows(),NL_LM); 
  lmpar.SetGaussNewtonType(LM_L);
  lmpar.SetStartingEstimate(start);

  NonlinOut status;
  status = nonlin(lmpar,(*this));
  ColumnVector final_par(nparams);
  final_par = lmpar.Par();


  // finalise parameters
  m_s0 = final_par(1);
  m_d  = std::abs(final_par(2));
  for(int k=1;k<=nfib;k++){
    int kk = 3 + 3*(k-1);
    m_f(k)  = x2f(final_par(kk));
    m_th(k) = final_par(kk+1);
    m_ph(k) = final_par(kk+2);
  }
  if (m_include_f0)
    m_f0=x2f(final_par(nparams));
  sort();
  fix_fsum();
}

void PVM_single::sort(){
  vector< pair<float,int> > fvals(nfib);
  ColumnVector ftmp(nfib),thtmp(nfib),phtmp(nfib);
  ftmp=m_f;thtmp=m_th;phtmp=m_ph;
  for(int i=1;i<=nfib;i++){
    pair<float,int> p(m_f(i),i);
    fvals[i-1] = p;
  }
  std::sort(fvals.begin(),fvals.end());
  for(int i=1,ii=nfib-1;ii>=0;i++,ii--){
    m_f(i)  = ftmp(fvals[ii].second);
    m_th(i) = thtmp(fvals[ii].second);
    m_ph(i) = phtmp(fvals[ii].second);
  }
}

void PVM_single::fix_fsum(){
  float sumf=0;
  if (m_include_f0) 
    sumf=m_f0;
  for(int i=1;i<=nfib;i++){
    sumf+=m_f(i);
    if(sumf>=1){for(int j=i;j<=nfib;j++)m_f(j)=0;break;}
  }
}

ReturnMatrix PVM_single::get_prediction()const{
  ColumnVector pred(npts);
  ColumnVector p(nparams);
  p(1) = m_s0;
  p(2) = m_d;
  for(int i=3,ii=1;ii<=nfib;i+=3,ii++){
    p(i)   = f2x(m_f(ii));
    p(i+1) = m_th(ii);
    p(i+2) = m_ph(ii);
  }
  if (m_include_f0)
    p(nparams)=f2x(m_f0);
  pred = forwardModel(p);

  pred.Release();
  return pred;
}

NEWMAT::ReturnMatrix PVM_single::forwardModel(const NEWMAT::ColumnVector& p)const{
  //cout<<"FORWARD"<<endl;
  //OUT(p.t());
  ColumnVector pred(npts);
  pred = 0;
  float val;
  float _d = std::abs(p(2));
  
  ////////////////////////////////////
  ColumnVector fs(nfib);
  Matrix x(nfib,3);
  float sumf=0;
  for(int k=1;k<=nfib;k++){
    int kk = 3+3*(k-1);
    fs(k) = x2f(p(kk));
    sumf += fs(k);
    x(k,1) = sin(p(kk+1))*cos(p(kk+2));
    x(k,2) = sin(p(kk+1))*sin(p(kk+2));
    x(k,3) = cos(p(kk+1));
  }
  ////////////////////////////////////
  for(int i=1;i<=Y.Nrows();i++){
    val = 0.0;
    for(int k=1;k<=nfib;k++){
      val += fs(k)*anisoterm(i,_d,x.Row(k).t());
    }
    if (m_include_f0){
      float temp_f0=x2f(p(nparams));
      pred(i) = p(1)*(temp_f0+(1-sumf-temp_f0)*isoterm(i,_d)+val);
    } 
    else
      pred(i) = p(1)*((1-sumf)*isoterm(i,_d)+val); 
  }  
  pred.Release();
  //cout<<"----"<<endl;
  return pred;
}


double PVM_single::cf(const NEWMAT::ColumnVector& p)const{
  //cout<<"CF"<<endl;
  //OUT(p.t());
  double cfv = 0.0;
  double err;
  float _d = std::abs(p(2));
  ////////////////////////////////////
  ColumnVector fs(nfib);
  Matrix x(nfib,3);
  float sumf=0;
  for(int k=1;k<=nfib;k++){
    int kk = 3+3*(k-1);
    fs(k) = x2f(p(kk));
    sumf += fs(k);
    x(k,1) = sin(p(kk+1))*cos(p(kk+2));
    x(k,2) = sin(p(kk+1))*sin(p(kk+2));
    x(k,3) = cos(p(kk+1));
  }
  ////////////////////////////////////
  for(int i=1;i<=Y.Nrows();i++){
    err = 0.0;
    for(int k=1;k<=nfib;k++){
      err += fs(k)*anisoterm(i,_d,x.Row(k).t());
    }
    if (m_include_f0){
      float temp_f0=x2f(p(nparams));
      err = (p(1)*(temp_f0+(1-sumf-temp_f0)*isoterm(i,_d)+err) - Y(i)); 
    }
    else
      err = (p(1)*((1-sumf)*isoterm(i,_d)+err) - Y(i)); 
    cfv += err*err; 
  }  
  //OUT(cfv);
  //cout<<"----"<<endl;
  return(cfv);
}


NEWMAT::ReturnMatrix PVM_single::grad(const NEWMAT::ColumnVector& p)const{
  //cout<<"GRAD"<<endl;
  //OUT(p.t());
  NEWMAT::ColumnVector gradv(p.Nrows());
  gradv = 0.0;
  float _d = std::abs(p(2));
  ////////////////////////////////////
  ColumnVector fs(nfib);
  Matrix x(nfib,3);ColumnVector xx(3);
  float sumf=0;
  for(int k=1;k<=nfib;k++){
    int kk = 3+3*(k-1);
    fs(k) = x2f(p(kk));
    sumf += fs(k);
    x(k,1) = sin(p(kk+1))*cos(p(kk+2));
    x(k,2) = sin(p(kk+1))*sin(p(kk+2));
    x(k,3) = cos(p(kk+1));
  }
  ////////////////////////////////////
  Matrix J(npts,nparams);
  ColumnVector diff(npts);
  float sig;
  for(int i=1;i<=Y.Nrows();i++){
    sig = 0;
    J.Row(i)=0;
    for(int k=1;k<=nfib;k++){
      int kk = 3+3*(k-1);
      xx = x.Row(k).t();
      sig += fs(k)*anisoterm(i,_d,xx);
      // other stuff for derivatives
      // d
      J(i,2) += (p(2)>0?1.0:-1.0)*p(1)*fs(k)*anisoterm_d(i,_d,xx);
      // f
      J(i,kk) = p(1)*(anisoterm(i,_d,xx)-isoterm(i,_d)) * two_pi*sign(p(kk))*1/(1+p(kk)*p(kk));
      // th
      J(i,kk+1) = p(1)*fs(k)*anisoterm_th(i,_d,xx,p(kk+1),p(kk+2));
      // ph
      J(i,kk+2) = p(1)*fs(k)*anisoterm_ph(i,_d,xx,p(kk+1),p(kk+2));
    }
    if (m_include_f0){
      float temp_f0=x2f(p(nparams));
      //derivative with respect to f0
      J(i,nparams)= p(1)*(1-isoterm(i,_d)) * two_pi*sign(p(nparams))*1/(1+p(nparams)*p(nparams));
      sig=p(1)*(temp_f0+(1-sumf-temp_f0)*isoterm(i,_d)+sig);
      J(i,2) += (p(2)>0?1.0:-1.0)*p(1)*(1-sumf-temp_f0)*isoterm_d(i,_d);
    }
    else{
      sig = p(1)*((1-sumf)*isoterm(i,_d)+sig);
      J(i,2) += (p(2)>0?1.0:-1.0)*p(1)*(1-sumf)*isoterm_d(i,_d);
    }
    diff(i) = sig - Y(i);
    J(i,1) = sig/p(1);
  }
  
  gradv = 2*J.t()*diff;
  //OUT(gradv.t());
  //cout<<"----"<<endl;
  gradv.Release();
  return gradv;


}

//this uses Gauss-Newton approximation
boost::shared_ptr<BFMatrix> PVM_single::hess(const NEWMAT::ColumnVector& p,boost::shared_ptr<BFMatrix> iptr)const{
  //cout<<"HESS"<<endl;
  //OUT(p.t());
  boost::shared_ptr<BFMatrix>   hessm;
  if (iptr && iptr->Nrows()==(unsigned int)p.Nrows() && iptr->Ncols()==(unsigned int)p.Nrows()) hessm = iptr;
  else hessm = boost::shared_ptr<BFMatrix>(new FullBFMatrix(p.Nrows(),p.Nrows()));

  float _d = std::abs(p(2));
  ////////////////////////////////////
  ColumnVector fs(nfib);
  Matrix x(nfib,3);ColumnVector xx(3);
  float sumf=0;
  for(int k=1;k<=nfib;k++){
    int kk = 3+3*(k-1);
    fs(k) = x2f(p(kk));
    sumf += fs(k);
    x(k,1) = sin(p(kk+1))*cos(p(kk+2));
    x(k,2) = sin(p(kk+1))*sin(p(kk+2));
    x(k,3) = cos(p(kk+1));
  }
   ////////////////////////////////////
  Matrix J(npts,nparams);
  float sig;
  for(int i=1;i<=Y.Nrows();i++){
    sig = 0;
    J.Row(i)=0;
    for(int k=1;k<=nfib;k++){
      int kk = 3+3*(k-1);
      xx = x.Row(k).t();
      sig += fs(k)*anisoterm(i,_d,xx);
      // other stuff for derivatives
      // d
      J(i,2) += (p(2)>0?1.0:-1.0)*p(1)*fs(k)*anisoterm_d(i,_d,xx);
      // f
      J(i,kk) = p(1)*(anisoterm(i,_d,xx)-isoterm(i,_d)) * two_pi*sign(p(kk))*1/(1+p(kk)*p(kk));
      // th
      J(i,kk+1) = p(1)*fs(k)*anisoterm_th(i,_d,xx,p(kk+1),p(kk+2));
      // ph
      J(i,kk+2) = p(1)*fs(k)*anisoterm_ph(i,_d,xx,p(kk+1),p(kk+2));
    }
    if (m_include_f0){
      float temp_f0=x2f(p(nparams));
      //derivative with respect to f0
      J(i,nparams)= p(1)*(1-isoterm(i,_d)) * two_pi*sign(p(nparams))*1/(1+p(nparams)*p(nparams));
      sig=p(1)*(temp_f0+(1-sumf-temp_f0)*isoterm(i,_d)+sig);
      J(i,2) += (p(2)>0?1.0:-1.0)*p(1)*(1-sumf-temp_f0)*isoterm_d(i,_d);
    }
    else{
      sig = p(1)*((1-sumf)*isoterm(i,_d)+sig);
      J(i,2) += (p(2)>0?1.0:-1.0)*p(1)*(1-sumf)*isoterm_d(i,_d);
    }
    J(i,1) = sig/p(1);
  }
  
  for (int i=1; i<=p.Nrows(); i++){
    for (int j=i; j<=p.Nrows(); j++){
      sig = 0.0;
      for(int k=1;k<=J.Nrows();k++)
	sig += 2*(J(k,i)*J(k,j));
      hessm->Set(i,j,sig);
    }
  }
  for (int j=1; j<=p.Nrows(); j++) {
    for (int i=j+1; i<=p.Nrows(); i++) {
      hessm->Set(i,j,hessm->Peek(j,i));
    }
  }
  //hessm->Print();
  //cout<<"----"<<endl;
  return(hessm);
}





////////////////////////////////////////////////
//       PARTIAL VOLUME MODEL - MULTIPLE SHELLS
////////////////////////////////////////////////

void PVM_multi::fit(){

  // initialise with simple pvm
  PVM_single pvm1(Y,bvecs,bvals,nfib,m_include_f0);
  pvm1.fit();

  float _a,_b;
  _a = 1.0; // start with d=d_std
  _b = pvm1.get_d();

  ColumnVector start(nparams);
  start(1) = pvm1.get_s0();
  start(2) = _a;
  start(3) = _b;
  for(int i=1,ii=4;i<=nfib;i++,ii+=3){
    start(ii) = f2x(pvm1.get_f(i));   //Is this a bug?
    start(ii+1) = pvm1.get_th(i);
    start(ii+2) = pvm1.get_ph(i);
  }
  if (m_include_f0)
    start(nparams)=f2x(pvm1.get_f0());

  
  // do the fit
  NonlinParam  lmpar(start.Nrows(),NL_LM); 
  lmpar.SetGaussNewtonType(LM_L);
  lmpar.SetStartingEstimate(start);

  NonlinOut status;
  status = nonlin(lmpar,(*this));
  ColumnVector final_par(nparams);
  final_par = lmpar.Par();

  // finalise parameters
  m_s0     = final_par(1);
  m_d      = std::abs(final_par(2)*final_par(3));
  m_d_std  = std::sqrt(std::abs(final_par(2)*final_par(3)*final_par(3)));
  for(int i=4,k=1;k<=nfib;i+=3,k++){
    m_f(k)  = x2f(final_par(i));
    m_th(k) = final_par(i+1);
    m_ph(k) = final_par(i+2);
  }
  if (m_include_f0)
    m_f0=x2f(final_par(nparams));
  sort();
  fix_fsum();
}

void PVM_multi::sort(){
  vector< pair<float,int> > fvals(nfib);
  ColumnVector ftmp(nfib),thtmp(nfib),phtmp(nfib);
  ftmp=m_f;thtmp=m_th;phtmp=m_ph;
  for(int i=1;i<=nfib;i++){
    pair<float,int> p(m_f(i),i);
    fvals[i-1] = p;
  }
  std::sort(fvals.begin(),fvals.end());
  for(int i=1,ii=nfib-1;ii>=0;i++,ii--){
    m_f(i)  = ftmp(fvals[ii].second);
    m_th(i) = thtmp(fvals[ii].second);
    m_ph(i) = phtmp(fvals[ii].second);
  }
}


void PVM_multi::fix_fsum(){
  float sumf=0;
  if (m_include_f0) 
    sumf=m_f0;
  for(int i=1;i<=nfib;i++){
    sumf+=m_f(i);
    if(sumf>=1){for(int j=i;j<=nfib;j++)m_f(j)=0;break;}
  }
}


ReturnMatrix PVM_multi::get_prediction()const{
  ColumnVector pred(npts);
  ColumnVector p(nparams);
  p(1) = m_s0;
  p(2) = m_d*m_d/m_d_std/m_d_std;
  p(3) = m_d_std*m_d_std/m_d; // =1/beta
  for(int k=1;k<=nfib;k++){
    int kk = 4+3*(k-1);
    p(kk)   = f2x(m_f(k));
    p(kk+1) = m_th(k);
    p(kk+2) = m_ph(k);
  }
  if (m_include_f0)
    p(nparams)=f2x(m_f0);
  pred = forwardModel(p);
  pred.Release();
  return pred;
}


NEWMAT::ReturnMatrix PVM_multi::forwardModel(const NEWMAT::ColumnVector& p)const{
  ColumnVector pred(npts);
  pred = 0;
  float val;
  float _a = std::abs(p(2));
  float _b = std::abs(p(3));
  ////////////////////////////////////
  ColumnVector fs(nfib);
  Matrix x(nfib,3);
  float sumf=0;
  for(int k=1;k<=nfib;k++){
    int kk = 4+3*(k-1);
    fs(k) = x2f(p(kk));
    sumf += fs(k);
    x(k,1) = sin(p(kk+1))*cos(p(kk+2));
    x(k,2) = sin(p(kk+1))*sin(p(kk+2));
    x(k,3) = cos(p(kk+1));
  }
  ////////////////////////////////////
  for(int i=1;i<=Y.Nrows();i++){
    val = 0.0;
    for(int k=1;k<=nfib;k++){
      val += fs(k)*anisoterm(i,_a,_b,x.Row(k).t());
    }
    if (m_include_f0){
      float temp_f0=x2f(p(nparams));
      pred(i) = std::abs(p(1))*(temp_f0+(1-sumf-temp_f0)*isoterm(i,_a,_b)+val);
    } 
    else
      pred(i) = std::abs(p(1))*((1-sumf)*isoterm(i,_a,_b)+val); 
  }  
  pred.Release();
  return pred;
}

double PVM_multi::cf(const NEWMAT::ColumnVector& p)const{
  //cout<<"CF"<<endl;
  //OUT(p.t());
  double cfv = 0.0;
  double err;
  float _a = std::abs(p(2));
  float _b = std::abs(p(3));
  ////////////////////////////////////
  ColumnVector fs(nfib);
  Matrix x(nfib,3);
  float sumf=0;
  for(int k=1;k<=nfib;k++){
    int kk = 4+3*(k-1);
    fs(k) = x2f(p(kk));
    sumf += fs(k);
    x(k,1) = sin(p(kk+1))*cos(p(kk+2));
    x(k,2) = sin(p(kk+1))*sin(p(kk+2));
    x(k,3) = cos(p(kk+1));
  }
  ////////////////////////////////////
  for(int i=1;i<=Y.Nrows();i++){
    err = 0.0;
    for(int k=1;k<=nfib;k++){
      err += fs(k)*anisoterm(i,_a,_b,x.Row(k).t());
    } 
    if (m_include_f0){
      float temp_f0=x2f(p(nparams));
      err = (std::abs(p(1))*(temp_f0+(1-sumf-temp_f0)*isoterm(i,_a,_b)+err) - Y(i)); 
    }
    else
      err = (std::abs(p(1))*((1-sumf)*isoterm(i,_a,_b)+err) - Y(i)); 
    cfv += err*err; 
  }  
  //OUT(cfv);
  //cout<<"----"<<endl;
  return(cfv);
}

NEWMAT::ReturnMatrix PVM_multi::grad(const NEWMAT::ColumnVector& p)const{
  //cout<<"GRAD"<<endl;
  //OUT(p.t());
  NEWMAT::ColumnVector gradv(p.Nrows());
  gradv = 0.0;
  float _a = std::abs(p(2));
  float _b = std::abs(p(3));
  ////////////////////////////////////
  ColumnVector fs(nfib);
  Matrix x(nfib,3);ColumnVector xx(3);
  float sumf=0;
  for(int k=1;k<=nfib;k++){
    int kk = 4+3*(k-1);
    fs(k) = x2f(p(kk));
    sumf += fs(k);
    x(k,1) = sin(p(kk+1))*cos(p(kk+2));
    x(k,2) = sin(p(kk+1))*sin(p(kk+2));
    x(k,3) = cos(p(kk+1));
  }
  ////////////////////////////////////
  Matrix J(npts,nparams);
  ColumnVector diff(npts);
  float sig;
  for(int i=1;i<=Y.Nrows();i++){
    sig = 0;
    J.Row(i)=0;
    for(int k=1;k<=nfib;k++){
      int kk = 4+3*(k-1);
      xx = x.Row(k).t();
      sig += fs(k)*anisoterm(i,_a,_b,xx);
      // other stuff for derivatives
      // alpha
      J(i,2) += (p(2)>0?1.0:-1.0)*std::abs(p(1))*fs(k)*anisoterm_a(i,_a,_b,xx);
      // beta
      J(i,3) += (p(3)>0?1.0:-1.0)*std::abs(p(1))*fs(k)*anisoterm_b(i,_a,_b,xx);
      // f
      J(i,kk) = std::abs(p(1))*(anisoterm(i,_a,_b,xx)-isoterm(i,_a,_b)) * two_pi*sign(p(kk))*1/(1+p(kk)*p(kk));
      // th
      J(i,kk+1) = std::abs(p(1))*fs(k)*anisoterm_th(i,_a,_b,xx,p(kk+1),p(kk+2));
      // ph
      J(i,kk+2) = std::abs(p(1))*fs(k)*anisoterm_ph(i,_a,_b,xx,p(kk+1),p(kk+2));
    }
    if (m_include_f0){
      float temp_f0=x2f(p(nparams));
      //derivative with respect to f0
      J(i,nparams)= std::abs(p(1))*(1-isoterm(i,_a,_b))*two_pi*sign(p(nparams))*1/(1+p(nparams)*p(nparams));
      sig=std::abs(p(1))*(temp_f0+(1-sumf-temp_f0)*isoterm(i,_a,_b)+sig);
      J(i,2) += (p(2)>0?1.0:-1.0)*std::abs(p(1))*(1-sumf-temp_f0)*isoterm_a(i,_a,_b);
      J(i,3) += (p(3)>0?1.0:-1.0)*std::abs(p(1))*(1-sumf-temp_f0)*isoterm_b(i,_a,_b);
    }
    else{
      sig = std::abs(p(1))*((1-sumf)*isoterm(i,_a,_b)+sig);
      J(i,2) += (p(2)>0?1.0:-1.0)*std::abs(p(1))*(1-sumf)*isoterm_a(i,_a,_b);
      J(i,3) += (p(3)>0?1.0:-1.0)*std::abs(p(1))*(1-sumf)*isoterm_b(i,_a,_b);
    }
    diff(i) = sig - Y(i);
    J(i,1) = (p(1)>0?1.0:-1.0)*sig/p(1);
  }
  
  gradv = 2*J.t()*diff;
  //OUT(gradv.t());
  //cout<<"----"<<endl;
  gradv.Release();
  return gradv;


}
//this uses Gauss-Newton approximation
boost::shared_ptr<BFMatrix> PVM_multi::hess(const NEWMAT::ColumnVector& p,boost::shared_ptr<BFMatrix> iptr)const{
  //cout<<"HESS"<<endl;
  //OUT(p.t());
  boost::shared_ptr<BFMatrix>   hessm;
  if (iptr && iptr->Nrows()==(unsigned int)p.Nrows() && iptr->Ncols()==(unsigned int)p.Nrows()) hessm = iptr;
  else hessm = boost::shared_ptr<BFMatrix>(new FullBFMatrix(p.Nrows(),p.Nrows()));

  float _a = std::abs(p(2));
  float _b = std::abs(p(3));
  ////////////////////////////////////
  ColumnVector fs(nfib);
  Matrix x(nfib,3);ColumnVector xx(3);
  float sumf=0;
  for(int k=1;k<=nfib;k++){
    int kk = 4+3*(k-1);
    fs(k) = x2f(p(kk));
    sumf += fs(k);
    x(k,1) = sin(p(kk+1))*cos(p(kk+2));
    x(k,2) = sin(p(kk+1))*sin(p(kk+2));
    x(k,3) = cos(p(kk+1));
  }
  ////////////////////////////////////
  Matrix J(npts,nparams);
  float sig;
  for(int i=1;i<=Y.Nrows();i++){
    sig = 0;
    J.Row(i)=0;
    for(int k=1;k<=nfib;k++){
      int kk = 4+3*(k-1);
      xx = x.Row(k).t();
      sig += fs(k)*anisoterm(i,_a,_b,xx);
      // other stuff for derivatives
      // change of variable
      float cov = two_pi*sign(p(kk))*1/(1+p(kk)*p(kk));
      // alpha
      J(i,2) += (p(2)>0?1.0:-1.0)*std::abs(p(1))*fs(k)*anisoterm_a(i,_a,_b,xx);
      // beta
      J(i,3) += (p(3)>0?1.0:-1.0)*std::abs(p(1))*fs(k)*anisoterm_b(i,_a,_b,xx);
      // f
      J(i,kk) = std::abs(p(1))*(anisoterm(i,_a,_b,xx)-isoterm(i,_a,_b)) * cov;
      // th
      J(i,kk+1) = std::abs(p(1))*fs(k)*anisoterm_th(i,_a,_b,xx,p(kk+1),p(kk+2));
      // ph
      J(i,kk+2) = std::abs(p(1))*fs(k)*anisoterm_ph(i,_a,_b,xx,p(kk+1),p(kk+2));
    }
    if (m_include_f0){
      float temp_f0=x2f(p(nparams));
      //derivative with respect to f0
      J(i,nparams)= std::abs(p(1))*(1-isoterm(i,_a,_b))*two_pi*sign(p(nparams))*1/(1+p(nparams)*p(nparams));
      sig=std::abs(p(1))*(temp_f0+(1-sumf-temp_f0)*isoterm(i,_a,_b)+sig);
      J(i,2) += (p(2)>0?1.0:-1.0)*std::abs(p(1))*(1-sumf-temp_f0)*isoterm_a(i,_a,_b);
      J(i,3) += (p(3)>0?1.0:-1.0)*std::abs(p(1))*(1-sumf-temp_f0)*isoterm_b(i,_a,_b);
    }
    else{
      sig = std::abs(p(1))*((1-sumf)*isoterm(i,_a,_b)+sig);
      J(i,2) += (p(2)>0?1.0:-1.0)*std::abs(p(1))*(1-sumf)*isoterm_a(i,_a,_b);
      J(i,3) += (p(3)>0?1.0:-1.0)*std::abs(p(1))*(1-sumf)*isoterm_b(i,_a,_b);
    }
    J(i,1) = (p(1)>0?1.0:-1.0)*sig/p(1);
  }
  
  for (int i=1; i<=p.Nrows(); i++){
    for (int j=i; j<=p.Nrows(); j++){
      sig = 0.0;
      for(int k=1;k<=J.Nrows();k++)
	sig += 2*(J(k,i)*J(k,j));
      hessm->Set(i,j,sig);
    }
  }
  for (int j=1; j<=p.Nrows(); j++) {
    for (int i=j+1; i<=p.Nrows(); i++) {
      hessm->Set(i,j,hessm->Peek(j,i));
    }
  }
  //hessm->Print();
  //cout<<"----"<<endl;
  return(hessm);
}





///////////////////////////////////////////////////////////////////////////////////////////////
//               USEFUL FUNCTIONS TO CALCULATE DERIVATIVES
///////////////////////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////
///////Model 1 (Constrained)
/////////////////////////////////////
// functions
float PVM_single_c::isoterm(const int& pt,const float& _d)const{
  return(std::exp(-bvals(1,pt)*_d));
}
float PVM_single_c::anisoterm(const int& pt,const float& _d,const ColumnVector& x)const{
  float dp = bvecs(1,pt)*x(1)+bvecs(2,pt)*x(2)+bvecs(3,pt)*x(3);
  return(std::exp(-bvals(1,pt)*_d*dp*dp));
}
// 1st order derivatives
float PVM_single_c::isoterm_lambda(const int& pt,const float& lambda)const{
  return(-2*bvals(1,pt)*lambda*std::exp(-bvals(1,pt)*lambda*lambda));
}
float PVM_single_c::anisoterm_lambda(const int& pt,const float& lambda,const ColumnVector& x)const{
  float dp = bvecs(1,pt)*x(1)+bvecs(2,pt)*x(2)+bvecs(3,pt)*x(3);
  return(-2*bvals(1,pt)*lambda*dp*dp*std::exp(-bvals(1,pt)*lambda*lambda*dp*dp));
}
float PVM_single_c::anisoterm_th(const int& pt,const float& _d,const ColumnVector& x,const float& _th,const float& _ph)const{
  float dp  = bvecs(1,pt)*x(1)+bvecs(2,pt)*x(2)+bvecs(3,pt)*x(3);
  float dp1 = cos(_th)*(bvecs(1,pt)*cos(_ph) + bvecs(2,pt)*sin(_ph)) - bvecs(3,pt)*sin(_th);
  return(-2*bvals(1,pt)*_d*dp*dp1*std::exp(-bvals(1,pt)*_d*dp*dp));
}
float PVM_single_c::anisoterm_ph(const int& pt,const float& _d,const ColumnVector& x,const float& _th,const float& _ph)const{
  float dp  = bvecs(1,pt)*x(1)+bvecs(2,pt)*x(2)+bvecs(3,pt)*x(3);
  float dp1 = sin(_th)*(-bvecs(1,pt)*sin(_ph) + bvecs(2,pt)*cos(_ph));
  return(-2*bvals(1,pt)*_d*dp*dp1*std::exp(-bvals(1,pt)*_d*dp*dp));
}

NEWMAT::ReturnMatrix PVM_single_c::fractions_deriv(const int& nfib, const ColumnVector& fs, const ColumnVector& bs) const{
  NEWMAT::Matrix Deriv(nfib,nfib);
  float fsum;
  Deriv=0;
  for (int j=1; j<=nfib; j++)
    for (int k=1; k<=nfib; k++){
      if (j==k){
	fsum=1; 
	for (int n=1; n<=j-1; n++)
	  fsum-=fs(n);
	Deriv(j,k)=sin(2*bs(k))*fsum;
      }
      else if (j>k){
	fsum=0;
	for (int n=1; n<=j-1; n++)
	  fsum+=Deriv(n,k);
	Deriv(j,k)=-pow(sin(bs(j)),2.0)*fsum;  
      }
    } 	   
  Deriv.Release();
  return Deriv;
}


/////////////////////////////////////
////////Model 1 (Old)
/////////////////////////////////////
//functions
float PVM_single::isoterm(const int& pt,const float& _d)const{
  return(std::exp(-bvals(1,pt)*_d));
}
float PVM_single::anisoterm(const int& pt,const float& _d,const ColumnVector& x)const{
  float dp = bvecs(1,pt)*x(1)+bvecs(2,pt)*x(2)+bvecs(3,pt)*x(3);
  return(std::exp(-bvals(1,pt)*_d*dp*dp));
}
float PVM_single::bvecs_fibre_dp(const int& pt,const float& _th,const float& _ph)const{
  float angtmp = cos(_ph-beta(pt))*sinalpha(pt)*sin(_th) + cosalpha(pt)*cos(_th);
  return(angtmp*angtmp);
}
float PVM_single::bvecs_fibre_dp(const int& pt,const ColumnVector& x)const{
  float dp = bvecs(1,pt)*x(1)+bvecs(2,pt)*x(2)+bvecs(3,pt)*x(3);
  return(dp*dp);
}
// 1st order derivatives
float PVM_single::isoterm_d(const int& pt,const float& _d)const{
  return(-bvals(1,pt)*std::exp(-bvals(1,pt)*_d));
}
float PVM_single::anisoterm_d(const int& pt,const float& _d,const ColumnVector& x)const{
  float dp = bvecs(1,pt)*x(1)+bvecs(2,pt)*x(2)+bvecs(3,pt)*x(3);
  return(-bvals(1,pt)*dp*dp*std::exp(-bvals(1,pt)*_d*dp*dp));
}
float PVM_single::anisoterm_th(const int& pt,const float& _d,const ColumnVector& x,const float& _th,const float& _ph)const{
  float dp  = bvecs(1,pt)*x(1)+bvecs(2,pt)*x(2)+bvecs(3,pt)*x(3);
  float dp1 = cos(_th)*(bvecs(1,pt)*cos(_ph) + bvecs(2,pt)*sin(_ph)) - bvecs(3,pt)*sin(_th);
  return(-2*bvals(1,pt)*_d*dp*dp1*std::exp(-bvals(1,pt)*_d*dp*dp));
}
float PVM_single::anisoterm_ph(const int& pt,const float& _d,const ColumnVector& x,const float& _th,const float& _ph)const{
  float dp  = bvecs(1,pt)*x(1)+bvecs(2,pt)*x(2)+bvecs(3,pt)*x(3);
  float dp1 = sin(_th)*(-bvecs(1,pt)*sin(_ph) + bvecs(2,pt)*cos(_ph));
  return(-2*bvals(1,pt)*_d*dp*dp1*std::exp(-bvals(1,pt)*_d*dp*dp));
}
// 2nd order derivatives
float PVM_single::isoterm_dd(const int& pt,const float& _d)const{
  return(bvals(1,pt)*bvals(1,pt)*std::exp(-bvals(1,pt)*_d));
}
float PVM_single::anisoterm_dd(const int& pt,const float& _d,const ColumnVector& x)const{
  float dp = bvecs(1,pt)*x(1)+bvecs(2,pt)*x(2)+bvecs(3,pt)*x(3);
  dp *= dp;
  return(bvals(1,pt)*dp*bvals(1,pt)*dp*std::exp(-bvals(1,pt)*_d*dp));
}
float PVM_single::anisoterm_dth(const int& pt,const float& _d,const ColumnVector& x,const float& _th,const float& _ph)const{
  float dp  = bvecs(1,pt)*x(1)+bvecs(2,pt)*x(2)+bvecs(3,pt)*x(3);
  float dp1 = cos(_th)*(bvecs(1,pt)*cos(_ph) + bvecs(2,pt)*sin(_ph)) - bvecs(3,pt)*sin(_th);
  return( -2*bvals(1,pt)*dp*dp1*(1-bvals(1,pt)*_d*dp*dp)*std::exp(-bvals(1,pt)*_d*dp*dp) );
}
float PVM_single::anisoterm_dph(const int& pt,const float& _d,const ColumnVector& x,const float& _th,const float& _ph)const{
  float dp  = bvecs(1,pt)*x(1)+bvecs(2,pt)*x(2)+bvecs(3,pt)*x(3);
  float dp1 = sin(_th)*(-bvecs(1,pt)*sin(_ph) + bvecs(2,pt)*cos(_ph));
  return( -2*bvals(1,pt)*dp*dp1*(1-bvals(1,pt)*_d*dp*dp)*std::exp(-bvals(1,pt)*_d*dp*dp) );
}
float PVM_single::anisoterm_thth(const int& pt,const float& _d,const ColumnVector& x,const float& _th,const float& _ph)const{
  float dp  = bvecs(1,pt)*x(1)+bvecs(2,pt)*x(2)+bvecs(3,pt)*x(3);
  return( -2*bvals(1,pt)*_d*std::exp(-bvals(1,pt)*_d*dp*dp)* ( (1-2*bvals(1,pt)*dp*dp) -dp*dp ) );
}
float PVM_single::anisoterm_phph(const int& pt,const float& _d,const ColumnVector& x,const float& _th,const float& _ph)const{
  float dp  = bvecs(1,pt)*x(1)+bvecs(2,pt)*x(2)+bvecs(3,pt)*x(3);
  float dp1 = (1-cos(2*_th))/2.0;
  float dp2 = -bvecs(1,pt)*x(1) - bvecs(2,pt)*x(2);
  return( -2*bvals(1,pt)*_d*std::exp(-bvals(1,pt)*_d*dp*dp)* ( (1-2*bvals(1,pt)*dp*dp)*dp1 +dp*dp2 ) );
}
float PVM_single::anisoterm_thph(const int& pt,const float& _d,const ColumnVector& x,const float& _th,const float& _ph)const{
  float dp  = bvecs(1,pt)*x(1)+bvecs(2,pt)*x(2)+bvecs(3,pt)*x(3);
  float dp2 = cos(_th)*(-bvecs(1,pt)*sin(_ph) + bvecs(2,pt)*cos(_ph));
  return( -2*bvals(1,pt)*_d*std::exp(-bvals(1,pt)*_d*dp*dp)* ( dp*dp2 ) );
}



////// NOW FOR MULTISHELL
// functions
float PVM_multi::isoterm(const int& pt,const float& _a,const float& _b)const{
  return(std::exp(-_a*std::log(1+bvals(1,pt)*_b)));
}
float PVM_multi::anisoterm(const int& pt,const float& _a,const float& _b,const ColumnVector& x)const{
  float dp = bvecs(1,pt)*x(1)+bvecs(2,pt)*x(2)+bvecs(3,pt)*x(3);
  return(std::exp(-_a*std::log(1+bvals(1,pt)*_b*(dp*dp))));
}
// 1st order derivatives
float PVM_multi::isoterm_a(const int& pt,const float& _a,const float& _b)const{
    return(-std::log(1+bvals(1,pt)*_b)*std::exp(-_a*std::log(1+bvals(1,pt)*_b)));
}
float PVM_multi::isoterm_b(const int& pt,const float& _a,const float& _b)const{
      return(-_a*bvals(1,pt)/(1+bvals(1,pt)*_b)*std::exp(-_a*std::log(1+bvals(1,pt)*_b)));
}
float PVM_multi::anisoterm_a(const int& pt,const float& _a,const float& _b,const ColumnVector& x)const{
  float dp = bvecs(1,pt)*x(1)+bvecs(2,pt)*x(2)+bvecs(3,pt)*x(3);
  return(-std::log(1+bvals(1,pt)*(dp*dp)*_b)*std::exp(-_a*std::log(1+bvals(1,pt)*(dp*dp)*_b)));
}
float PVM_multi::anisoterm_b(const int& pt,const float& _a,const float& _b,const ColumnVector& x)const{
  float dp = bvecs(1,pt)*x(1)+bvecs(2,pt)*x(2)+bvecs(3,pt)*x(3);
  return(-_a*bvals(1,pt)*(dp*dp)/(1+bvals(1,pt)*(dp*dp)*_b)*std::exp(-_a*std::log(1+bvals(1,pt)*(dp*dp)*_b)));
}
float PVM_multi::anisoterm_th(const int& pt,const float& _a,const float& _b,const ColumnVector& x,const float& _th,const float& _ph)const{
  float dp = bvecs(1,pt)*x(1)+bvecs(2,pt)*x(2)+bvecs(3,pt)*x(3);
  float dp1 = cos(_th)*(bvecs(1,pt)*cos(_ph) + bvecs(2,pt)*sin(_ph)) - bvecs(3,pt)*sin(_th);
  return(-_a*_b*bvals(1,pt)/(1+bvals(1,pt)*(dp*dp)*_b)*std::exp(-_a*std::log(1+bvals(1,pt)*(dp*dp)*_b))*2*dp*dp1);
}
float PVM_multi::anisoterm_ph(const int& pt,const float& _a,const float& _b,const ColumnVector& x,const float& _th,const float& _ph)const{
  float dp = bvecs(1,pt)*x(1)+bvecs(2,pt)*x(2)+bvecs(3,pt)*x(3);
  float dp1 = sin(_th)*(-bvecs(1,pt)*sin(_ph) + bvecs(2,pt)*cos(_ph));
  return(-_a*_b*bvals(1,pt)/(1+bvals(1,pt)*(dp*dp)*_b)*std::exp(-_a*std::log(1+bvals(1,pt)*(dp*dp)*_b))*2*dp*dp1);
}
