#include "TCanvas.h"
#include "TFile.h"
#include "TEllipse.h"
#include "TH2F.h"
#include "TMath.h"
#include "TMinuit.h"
#include "TRandom.h"
#include "TStyle.h"
#include "iostream"


TH2F * h2;
Float_t thresh = 20;

Double_t Ellipse(Double_t x1, Double_t y1,Double_t x2, Double_t y2, Double_t b, Double_t x,Double_t y)
{
    // look if a point x, y is inside an ellipse defined by
    // focal points (x1, y1), (x2, y2) and radius b

    Double_t a = 0.5 * TMath::Sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1));
    Double_t s = TMath::Sqrt(a*a + b*b);
    Double_t d1 = TMath::Sqrt((x-x1)*(x-x1) + (y-y1)*(y-y1));
    Double_t d2 = TMath::Sqrt((x-x2)*(x-x2) + (y-y2)*(y-y2));

    return 2 * s - (d2 + d1);  // > 0: inside
}

Double_t ThetaOfLineDegree(Double_t x1, Double_t y1, Double_t x2, Double_t y2)
{
    Double_t len = TMath::Sqrt((x2 - x1)*(x2 - x1) + (y2 - y1)*(y2 - y1));
    Double_t phi = 0;

    if (len > 0) {
        Double_t sina = (y2 - y1) / len;
        Double_t cosa = (x2 - x1) / len;
        phi = TMath::ATan2(sina, cosa);
        if (phi < 0) phi += 2. * TMath::Pi();
    }

    return 180 * phi / TMath::Pi();
}

// Wrapping
void fcn(Int_t &npar, Double_t *gin, Double_t &f, Double_t *par, Int_t iflag)
{
    Double_t sum = 0;
    TAxis *ax = h2->GetXaxis();
    TAxis *ay = h2->GetYaxis();

    for (Int_t ix = 1; ix <= h2->GetNbinsX(); ix++) {
        for (Int_t iy = 1; iy <= h2->GetNbinsY(); iy++) {
            Double_t x = ax->GetBinCenter(ix);
            Double_t y = ay->GetBinCenter(iy);
            Double_t cont = h2->GetBinContent(ix, iy);
            Double_t d = Ellipse(par[0],par[1], par[2],par[3],par[4],x, y);
            if (d > 0) {
                if (cont < thresh) sum += d * (thresh - cont);
            } else {
                if (cont > thresh) sum += -d * (cont - thresh);
            }
        }
    }
    f = sum;
}


 Double_t fitcontour(Double_t th = 200){
   TFile * f = new TFile("my2dplot.root");
   TCanvas *c = (TCanvas *)f->Get("c1");
   h2 = (TH2F *)c->GetListOfPrimitives()->FindObject("hChi2");

   TCanvas *c1 = new TCanvas();
   h2->Draw("COL");

   TVirtualFitter::SetDefaultFitter("Minuit");  //default is Minuit
   TVirtualFitter *fitter = TVirtualFitter::Fitter(0, 5);
   fitter->SetFCN(fcn);
   fitter->SetParameter(0, "x1", 25, 10, 0,0);
   fitter->SetParameter(1, "y1", 10, 5, 0,0);
   fitter->SetParameter(2, "x2", 65, 10, 0,0);
   fitter->SetParameter(3, "y2", 30, 5, 0,0);
   fitter->SetParameter(4, "b",  10, 5, 0,0);

   Double_t arglist[1] = {0};
   fitter->ExecuteCommand("MIGRAD", arglist, 0);

   Double_t x1, y1, x2, y2, b;
   x1 = fitter->GetParameter(0);
   y1 = fitter->GetParameter(1);
   x2 = fitter->GetParameter(2);
   y2 = fitter->GetParameter(3);
   b = fitter->GetParameter(4);

   // recalculate parameters needed by TEllipse
   Double_t xcenter = 0.5 * (x1 + x2);

   Double_t ycenter = 0.5 * (y1 + y2);
   Double_t r1 =  0.5 * TMath::Sqrt((x1 - x2)*(x1 - x2)+ (y1 - y2)*(y1 - y2));

   Double_t r2 = b;
   Double_t theta = ThetaOfLineDegree(x1, y1, x2, y2);

   TEllipse * el = new TEllipse(xcenter, ycenter, r1, r2, 0, 360, theta);
   // el->Draw();
   return 0;
 }