#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(TVector<r_4>& FreqMHz,TVector<r_4>& V,TVector<r_4>& EV);
  void SetNu0(double nu0Mhz) {_nu0 = nu0Mhz;}
  void SetTyp(bool law2 = false) {_law2 = law2;}
  void InitFit(MnUserParameters& upar);
  double Func(double fr,const vector<double>& par) const;
  double operator()(const vector<double>& par) const;
  double Up(void) const {return 1.;}
  //protected:
  double _nu0;
  bool _law2;
  mutable int _ndata;
  TVector<r_4> _F;
  TVector<r_4> _V;
  TVector<r_4> _EV;
};
} }  // namespace ROOT + Minuit2

//--------------------------------------------------
int main(int narg,char *arg[])
{
  int nslicemax = -1;
  int typfit = 0;
  if(narg<=1) {
    cout<<"cmvgsmcfit cubefile.ppf [nslicemax] [typfit=0,1]"<<endl;
    return -1;
  }

  if(narg>2) nslicemax = atoi(arg[2]);
  if(narg>3) typfit = atoi(arg[3]);
  cout<<"Fitting "<<arg[1]<<" , nslicemax="<<nslicemax<<" , typfit="<<typfit<<endl;

int rc = 0;
try {

  POutPersist pos("cmvgsmcfit.ppf");
  PInPersist pis(arg[1]);
  TArray<r_4> Cube;
  pis>>PPFNameTag("cube")>>Cube;
  TVector<r_4> Freq;
  pis>>PPFNameTag("freq")>>Freq;

  const int nvar = 10;
  const char *vname[nvar] = {"x2r","ok","ph","th","v0","vn","A","N","B","DN"};
  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 = "<<j<<endl;
  for(int k=0;k<Cube.SizeZ();k++) {
    //cout<<"...k = "<<k<<endl;

    TVector<r_4> V(Cube.SizeX()), EV(Cube.SizeX());
    for(int i=0;i<Cube.SizeX();i++) {
      V(i) = Cube(i,j,k);
      EV(i) = V(i) * 0.05;
    }

    MyFCN myfcn(Freq,V,EV);
    myfcn.SetNu0(Freq(0));
    //myfcn.SetNu0((Freq(0)+Freq(Freq.Size()-1))/2.);
    myfcn.SetTyp( ((typfit==1)? true: false) );
    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;
    xnt[4] = V(0); xnt[5] = V(V.Size()-1);
    for(int i=6;i<6+par.size() && i<nvar;i++) xnt[i] = par[i-6];
    nt.Fill(xnt);
    //vector<double> err = ustate.Errors();
    for(int i=0;i<Cube.SizeX();i++) Cube(i,j,k) -= myfcn.Func(Freq(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(Freq(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(nslicemax>0 && j>=nslicemax) break;
  }

  cout<<"nfit = "<<nfit<<endl;
  pos<<PPFNameTag("nt")<<nt;
  pos<<PPFNameTag("cublis")<<Cube;

//-----------------------------------------------------------------
} catch (PException& exc) {
  cerr<<"cmvgsmcfit.cc catched PException"<<exc.Msg()<<endl;
  rc = 77;
} catch (std::exception& sex) {
  cerr << "cmvgsmcfit.cc std::exception :" 
       << (string)typeid(sex).name() << "\n msg= " << sex.what() << endl;
  rc = 78;
} catch (...) {
  cerr << "cmvgsmcfit.cc catched unknown (...) exception  " << endl; 
  rc = 79;
}

return rc;
}

//--------------------------------------------------
MyFCN::MyFCN(TVector<r_4>& FreqMHz,TVector<r_4>& V,TVector<r_4>& EV)
  : _F(FreqMHz,false), _V(V,false), _EV(EV,false)
{
  if(_F.Size()==0 || _F.Size()!=_V.Size() || _F.Size()!=_EV.Size())
    throw SzMismatchError("MyFCN::MyFCN_Error: incompatible F / V / EV sizes");
  _nu0 = _F(0);
  _law2 = false;
  _ndata =_V.Size();
}

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

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

  if(!_law2) return;
  upar.Add("B",0.,a/200.);
  upar.Add("DNu",0.,fabs(ni/200.));
  
}

double MyFCN::Func(double fr,const vector<double>& par) const
// v = A*(nu/nu0)^Nu
// v = A*(nu/nu0)^Nu + B/A*(nu/nu0)^(Nu+DNu) = A*(nu/nu0)^Nu * (1 + B*(nu/nu0)^DNu)
{
 double f = par[0]*pow(fr/_nu0,par[1]);
 if(_law2) f *= 1. + par[2]*pow(fr/_nu0,par[3]);
 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(_F(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
zone 1 2
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"

zone 1 2
n/plot nt.ok%_nl ! ! "crossmarker3"
n/plot nt.x2r%_nl ! ! "crossmarker3"

zone 1 2
n/plot nt.v0%_nl ! ! "crossmarker3"
n/plot nt.vn%_nl ! ! "plusmarker3 same red"
n/plot nt.log10(v0/vn)%v0 ! ! "crossmarker3"

zone
n/plot nt.A%_nl ok>0 ! "crossmarker3"
n/plot nt.N%_nl ok>0 ! "crossmarker3"
n/plot nt.B%_nl ok>0 ! "crossmarker3"
n/plot nt.DN%_nl ok>0 ! "crossmarker3"

n/plot nt.A%v0 ok>0 ! "crossmarker3"
n/plot nt.(A-v0)/v0%_nl ok>0 ! "crossmarker3"

n/plot nt.N%A ok>0 ! "crossmarker3"
n/plot nt.B%A ok>0 ! "crossmarker3"
n/plot nt.DN%N ok>0 ! "crossmarker3"

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
*/