#include "sopnamsp.h"
#include "machdefs.h"
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <unistd.h>

#include "fioarr.h"
#include "ntuple.h"

#include "Minuit2/FunctionMinimum.h"
#include "Minuit2/MnMigrad.h"
#include "Minuit2/MnUserParameters.h"
#include "Minuit2/MnPrint.h"
#include "Minuit2/MnMinos.h"
#include "Minuit2/MinosError.h"
#include "Minuit2/FCNBase.h"
using namespace ROOT::Minuit2;

//--------------------------------------------------
namespace ROOT { namespace Minuit2 {
class MyFCN : public FCNBase {
public:
  MyFCN(double nu0,double dnu,TVector<r_8>& V,TVector<r_8>& EV);
  void InitFit(MnUserParameters& upar);
  double Func(int ix,const vector<double>& par) const;
  double operator()(const vector<double>& par) const;
  double Up(void) const {return 1.;}
  //protected:
  mutable int _ndata;
  double _nu0, _dnu;
  TVector<r_8> _V;
  TVector<r_8> _EV;
};
} }  // namespace ROOT + Minuit2

//--------------------------------------------------
int main(int narg,char *arg[])
{
  double nu0 = 820.0, dnu = 0.5; // MHz
  if(narg<=1) {
    cout<<"cmvgsmcfit cubefile.ppf"<<endl;
    return -1;
  }

  POutPersist pos("cmvgsmcfit.ppf");
  PInPersist pis(arg[1]);
  TArray<r_4> Cube;
  pis>>PPFNameTag("cube")>>Cube;
  TVector<r_8> V(Cube.SizeX()), EV(Cube.SizeX());

  const int nvar = 6;
  const char *vname[nvar] = {"x2r","ok","ph","th","A","N"};
  NTuple nt(nvar,vname);
  double xnt[nvar]; for(int i=0;i<nvar;i++) xnt[i] = 0.;

  int nfit = 0;
  for(int j=0;j<Cube.SizeY();j++) {
    cout<<"..."<<j<<endl;
  for(int k=0;k<Cube.SizeZ();k++) {
    for(int i=0;i<Cube.SizeX();i++) {
      V(i) = Cube(i,j,k);
      EV(i) = V(i) * 0.05;
    }

    MyFCN myfcn(nu0,dnu,V,EV);
    MnUserParameters upar;
    myfcn.InitFit(upar);
    if(nfit==0) cout<<upar;

    unsigned int nfcall = 10000;
    double tolerance = 0.0001;
    MnMigrad migrad(myfcn,upar,MnStrategy(1));
    FunctionMinimum fmini = migrad(nfcall,tolerance);

    MnUserParameterState ustate = fmini.UserState();
    bool okfit = fmini.IsValid();
    double xi2r = ustate.Fval()/(V.Size()-ustate.VariableParameters());
    vector<double> par = ustate.Params();
    if(nfit==0) {
      cout<<ustate;
      // cout<<ustate.Parameters(); cout<<ustate.Covariance(); cout<<ustate.GlobalCC();
      cout<<"nFCN = "<<ustate.NFcn()<<", FVal = "<<ustate.Fval()<<", Edm = "<<ustate.Edm()<<endl;
      cout<<"xi2r = "<<xi2r<<" ndata="<<myfcn._ndata<<" nvarp="<<ustate.VariableParameters()<<endl;
    }

    xnt[0] = xi2r; xnt[1] = (okfit)? 1.: 0.;
    xnt[2] = j; xnt[3] = k;
    for(unsigned int i=0;i<par.size();i++) xnt[4+i] = par[i];
    nt.Fill(xnt);
    //vector<double> err = ustate.Errors();
    for(int i=0;i<Cube.SizeX();i++) Cube(i,j,k) -= myfcn.Func(i,par);

    if(nfit<10) {
      TVector<r_8> F(Cube.SizeX()), R(Cube.SizeX());
      for(int i=0;i<Cube.SizeX();i++) {
        F(i) = myfcn.Func(i,par);
        R(i) = V(i) - F(i);
      }
      char str[64];
      sprintf(str,"v_%d_%d",j,k);
      pos<<PPFNameTag(str)<<V;
      sprintf(str,"f_%d_%d",j,k);
      pos<<PPFNameTag(str)<<F;
      sprintf(str,"r_%d_%d",j,k);
      pos<<PPFNameTag(str)<<R;
    }
    nfit++;
    if(nfit>10) break;
  }
  }

  pos<<PPFNameTag("nt")<<nt;
  pos<<PPFNameTag("cublis")<<Cube;

}

//--------------------------------------------------
MyFCN::MyFCN(double nu0, double dnu,TVector<r_8>& V,TVector<r_8>& EV)
  : _V(V,false), _EV(EV,false)
{
  _ndata =_V.Size();
  _nu0 = nu0;
  _dnu = dnu;
}

void MyFCN::InitFit(MnUserParameters& upar)
{
  int n = _V.Size()-1;
  double a = _V(0);
  double ni = log(_V(n)/a) / log(1.+n*_dnu/_nu0);

  upar.Add("A",a,a/50.);
  upar.Add("N",ni,fabs(ni/50.));
}

double MyFCN::Func(int ix,const vector<double>& par) const
// f = A*(nu/nu0)^N   avec  nu = nu0 + i*dnu
{
 double x = (_nu0 + ix*_dnu)/_nu0;
 double f = par[0]*pow(x,par[1]);
 return f;
}

double MyFCN::operator()(const vector<double>& par) const
{
 double xi2 = 0.;
 _ndata = 0;
 for(int i=0;i<_V.Size();i++) {
   double e2 = _EV(i)*_EV(i);
   if(e2<=0.) continue;
   double v = _V(i);
   double f = Func(i,par);
   xi2 += (v-f)*(v-f)/e2;
   _ndata++;
 }
 //cout<<"par: "<<par[0]<<" "<<par[1]<<endl;
 return xi2;
}

/*
openppf cmvgsmcfit.ppf

set j 0
set k 0
n/plot v_${j}_${k}.val%n ! ! "nsta cpts"
n/plot f_${j}_${k}.val%n ! ! "nsta cpts same red"
n/plot r_${j}_${k}.val%n ! ! "nsta cpts"

n/plot nt.ok%_nl ! ! "cpts"
n/plot nt.x2r%_nl ! ! "cpts"

n/plot nt.A%_nl ! ! "cpts"
n/plot nt.N%_nl ! ! "cpts"

set n 0
objaoper cublis slicexz $n
disp slicexz_$n

n/plot cublis.val%x fabs(y-5)<0.1&&fabs(z-5)<0.1
*/