// $Id$
//-----------------------------------------------------------------------
//       The GSI Online Offline Object Oriented (Go4) Project
//         Experiment Data Processing at EE department, GSI
//-----------------------------------------------------------------------
// Copyright (C) 2000- GSI Helmholtzzentrum fuer Schwerionenforschung GmbH
//                     Planckstr. 1, 64291 Darmstadt, Germany
// Contact:            http://go4.gsi.de
//-----------------------------------------------------------------------
// This software can be used under the license agreements as stated
// in Go4License.txt file which is part of the distribution.
//-----------------------------------------------------------------------

#include "TGo4FitPeakFinder.h"

#include "TMath.h"
#include "TH1D.h"
#include "TMatrixD.h"
#include "TVectorD.h"
#include "TArrayD.h"
#include "TArrayI.h"
#include "TSpectrum.h"

#include "TGo4Fitter.h"
#include "TGo4FitData.h"

TGo4FitPeakFinder::TGo4FitPeakFinder() : TGo4FitterAction() {}

TGo4FitPeakFinder::TGo4FitPeakFinder(const char *Name, const char *DataName, Bool_t ClearModels, Int_t PolOrder)
   : TGo4FitterAction(Name, "Peak finder action"), fiPeakFinderType(0), fxDataName(DataName),
     fbClearModels(ClearModels), fbUsePolynom(PolOrder >= 0), fiPolynomOrder(PolOrder >= 0 ? PolOrder : 0),
     fd0MinWidth(5.), fd0MaxWidth(30.), fd0MaxAmplFactor(0.2), fd1LineWidth(5.), fd2NoiseFactor(2.),
     fd2NoiseMinimum(5.), fi2ChannelSum(2)
{
}

TGo4FitPeakFinder::~TGo4FitPeakFinder() {}

void TGo4FitPeakFinder::SetupPolynomialBackground(Int_t PolynomOrder)
{
   if (PolynomOrder < 0)
      SetUsePolynom(kFALSE);
   else {
      SetUsePolynom(kTRUE);
      SetPolynomOrder(PolynomOrder);
   }
}

void TGo4FitPeakFinder::SetupForFirst(Double_t MaxAmplFactor, Double_t MinWidth, Double_t MaxWidth)
{
   SetPeakFinderType(0);
   Set0MaxAmplFactor(MaxAmplFactor);
   Set0MinWidth(MinWidth);
   Set0MaxWidth(MaxWidth);
}

void TGo4FitPeakFinder::SetupForSecond(Double_t LineWidth)
{
   SetPeakFinderType(1);
   Set1LineWidth(LineWidth);
}

void TGo4FitPeakFinder::SetupForThird(Double_t NoiseFactor, Double_t NoiseMinimum, Int_t ChannelSum)
{
   SetPeakFinderType(2);
   Set2NoiseFactor(NoiseFactor);
   Set2NoiseMinimum(NoiseMinimum);
   Set2ChannelSum(ChannelSum);
}

void TGo4FitPeakFinder::DoAction(TGo4FitterAbstract *Fitter)
{
   TGo4Fitter *fitter = dynamic_cast<TGo4Fitter *>(Fitter);
   if (!fitter)
      return;

   TGo4FitData *data = fitter->FindData(GetDataName());
   if (!data)
      return;

   if (GetClearModels())
      fitter->DeleteModelsAssosiatedTo(data->GetName());

   if (GetPeakFinderType() == 0)
      SergeyLinevPeakFinder(fitter, data, GetUsePolynom() ? GetPolynomOrder() : -1, Get0MaxAmplFactor(), Get0MinWidth(),
                            Get0MaxWidth());

   if (GetPeakFinderType() == 1)
      ROOTPeakFinder(fitter, data, GetUsePolynom() ? GetPolynomOrder() : -1, Get1LineWidth());

   if (GetPeakFinderType() == 2)
      HansEsselPeakFinder(fitter, data, 500, Get2ChannelSum(), Get2NoiseFactor(), Get2NoiseMinimum(),
                          GetUsePolynom() ? GetPolynomOrder() : -1);
}

void TGo4FitPeakFinder::Print(Option_t *option) const
{
   TGo4FitterAction::Print(option);
}

void TGo4FitPeakFinder::ROOTPeakFinder(TGo4Fitter *fitter, TGo4FitData *data, Int_t PolynomOrder, Double_t Sigma)
{
   if (!fitter || !data)
      return;

   auto iter = data->MakeIter();
   if (!iter)
      return;

   Int_t size = iter->CountPoints();

   if ((size < 10) || (iter->ScalesSize() != 1))
      return;

   TArrayD Bins(size), Scales(size), HScales(size + 1);

   Int_t nbin = 0;
   if (iter->Reset())
      do {
         Bins[nbin] = data->GetAmplValue() * iter->Value();
         Double_t pos = iter->x();
         Double_t width = iter->HasWidths() ? iter->Widths()[0] : 1.;
         Scales[nbin] = pos;
         HScales[nbin] = pos - width / 2.;
         HScales[nbin + 1] = pos + width / 2;
         nbin++;
      } while (iter->Next());

   if (PolynomOrder >= 0)
      fitter->AddPolynomX(data->GetName(), "Pol", PolynomOrder);

   TH1D histo("ROOTPeakFinder", "ROOTPeakFinder spectrum", size, HScales.GetArray());
   histo.SetDirectory(nullptr);
   for (Int_t i = 0; i < size; i++)
      histo.SetBinContent(i + 1, Bins[i]);

   TSpectrum sp(100);
   sp.Search(&histo, Sigma);
   //   sp.Search1(Bins.GetArray(), size, Sigma);

   for (Int_t n = 0; n < sp.GetNPeaks(); n++) {
      Double_t pos = sp.GetPositionX()[n];
      Double_t width = Sigma;

      //   this is old code for ROOT 3.*, may be usefull for somebody
      /*
             Double_t dindx = sp.GetPositionX()[n];
            Int_t left = TMath::Nint(dindx-Sigma/2.);
            Int_t right = TMath::Nint(dindx+Sigma/2.);
            if (left<0) left = 0;
            if (right>=size) right = size-1;

            Double_t pos = Scales[TMath::Nint(dindx)];
            Double_t width = TMath::Abs(Scales[right]-Scales[left]);
      */
      fitter->AddGauss1(data->GetName(), fitter->FindNextName("Gauss", n), pos, width);
   }
}

Double_t TGo4FitPeakFinder::CalcPolynom(const TArrayD &Coef, Double_t x)
{
   Double_t power = 1., sum = 0.;
   for (Int_t n = 0; n < Coef.GetSize(); n++) {
      sum += Coef[n] * power;
      power *= x;
   }
   return sum;
}

void TGo4FitPeakFinder::DefinePolynom(Int_t size, Double_t *bins, Double_t *scales, TArrayD &Coef, Double_t *weight,
                                      Double_t *backgr, Char_t *use)
{
   Int_t maxorder = Coef.GetSize() - 1;

   TMatrixD matr(maxorder + 1, maxorder + 1);
   TVectorD vector(maxorder + 1);

   for (Int_t order1 = 0; order1 <= maxorder; order1++)
      for (Int_t order2 = order1; order2 <= maxorder; order2++) {
         Double_t sum = 0;
         for (Int_t i = 0; i < size; i++)
            if (!use || use[i]) {
               Double_t zn = TMath::Power(scales[i], order1 + order2);
               if (weight)
                  zn *= weight[i];
               sum += zn;
            }
         matr(order1, order2) = sum;
         matr(order2, order1) = sum;
      }

   for (Int_t order1 = 0; order1 <= maxorder; order1++) {
      Double_t sum = 0;
      for (Int_t i = 0; i < size; i++)
         if (!use || use[i]) {
            Double_t zn = bins[i];
            if (backgr)
               zn -= backgr[i];
            zn *= TMath::Power(scales[i], order1);
            if (weight)
               zn *= weight[i];
            sum += zn;
         }
      vector(order1) = sum;
   }

   matr.Invert();
   vector *= matr;

   for (Int_t order = 0; order <= maxorder; order++)
      Coef[order] = vector(order);
}

void TGo4FitPeakFinder::DefinePolynomEx(Int_t size, Double_t *bins, Double_t *scales, Double_t *weight,
                                        Double_t *backgr, Int_t lbound, Int_t rbound, TArrayD &Coef)
{
   TArrayC use(size);
   use.Reset(0);
   if (lbound < 0)
      lbound = 0;
   if (rbound >= size)
      rbound = size - 1;
   for (Int_t i = lbound; i <= rbound; i++)
      use[i] = 1;
   DefinePolynom(size, bins, scales, Coef, weight, backgr, use.GetArray());
}

Bool_t FindValue(Double_t *bins, Double_t *backgr, Char_t *use, Int_t size, Int_t &p, Int_t dir, Double_t value)
{
   p += dir;
   while ((p >= 0) && (p < size) && use[p]) {
      Double_t zn = bins[p] - backgr[p];
      if (zn < value)
         return kTRUE;
      p += dir;
   }
   return kFALSE;
}

void TGo4FitPeakFinder::SergeyLinevPeakFinder(TGo4Fitter *fitter, TGo4FitData *data, Int_t PolOrder,
                                              Double_t AmplThreshold, Double_t MinWidth, Double_t MaxWidth)
{
   if (!fitter || !data)
      return;

   auto iter = data->MakeIter();
   if (!iter)
      return;

   Int_t size = iter->CountPoints();

   if ((size < 10) || (iter->ScalesSize() != 1))
      return;

   TArrayD Bins(size), Scales(size), Weights(size), Background(size);

   Weights.Reset(1.);
   Background.Reset(0.);

   Double_t *bins = Bins.GetArray();
   Double_t *scales = Scales.GetArray();
   Double_t *weights = Weights.GetArray();
   Double_t *backgr = Background.GetArray();

   Int_t nbin = 0;
   if (iter->Reset())
      do {
         bins[nbin] = data->GetAmplValue() * iter->Value();
         if (iter->StandardDeviation() > 0)
            weights[nbin] = 1. / iter->StandardDeviation();
         scales[nbin] = iter->x();
         nbin++;
      } while (iter->Next());

   if ((PolOrder >= 0) && (PolOrder < (size + 1) * 5)) {
      Int_t nsegments = (PolOrder + 1) * 2;
      TArrayC usage(size);
      usage.Reset(0);
      for (Int_t nseg = 0; nseg < nsegments; nseg++) {
         Int_t lbound = Int_t((nseg + 0.) / nsegments * size);
         Int_t rbound = Int_t((nseg + 1.) / nsegments * size - 1.);
         Int_t nselect = (rbound - lbound) / 10;
         if (nselect < 1)
            nselect = 1;
         Int_t nsel = 0;
         while (nsel++ < nselect) {
            Int_t pmin = -1;
            for (Int_t i = lbound; i <= rbound; i++)
               if (!usage[i])
                  if ((pmin < 0) || (bins[i] < bins[pmin]))
                     pmin = i;
            usage[pmin] = 1;
         }
      }

      TArrayD Coef(PolOrder + 1);
      DefinePolynom(size, bins, scales, Coef, weights, nullptr, usage.GetArray());

      for (Int_t i = 0; i < size; i++)
         backgr[i] = CalcPolynom(Coef, scales[i]);

      fitter->AddPolynomX(data->GetName(), "Pol", Coef);
   }

   Double_t dmax = 0.;
   for (Int_t i = 0; i < size; i++) {
      Double_t zn = bins[i] - backgr[i];
      if (zn > dmax)
         dmax = zn;
   }
   AmplThreshold = AmplThreshold * dmax;

   TArrayC usage(size);
   usage.Reset(1);
   Char_t *use = usage.GetArray();

   Bool_t validline = kFALSE;

   do {

      validline = kFALSE;

      Int_t pmax = -1;
      Double_t max = 0.;
      for (Int_t i = 0; i < size; i++)
         if (use[i]) {
            Double_t zn = bins[i] - backgr[i];
            if ((pmax < 0) || (zn > max)) {
               pmax = i;
               max = zn;
            }
         }

      Double_t sigma = TMath::Sqrt(1. / weights[pmax]);

      if ((max < 2. * sigma) || (max < AmplThreshold))
         break;

      Int_t lbound = pmax, rbound = pmax;

      Bool_t lok = FindValue(bins, backgr, use, size, lbound, -1, max * 0.5);
      Bool_t rok = FindValue(bins, backgr, use, size, rbound, +1, max * 0.5);

      if (lok || rok) {

         validline = kTRUE;

         Int_t w = (lok && rok) ? (rbound - lbound) / 4 : (lok ? (pmax - lbound) / 2 : (rbound - pmax) / 2);
         if (w < 2)
            w = 2;

         TArrayD Coef1(3), Coef2(2);
         DefinePolynomEx(size, bins, scales, weights, backgr, pmax - w, pmax + w, Coef1);
         Double_t middle = -0.5 * Coef1[1] / Coef1[2];
         Double_t amplitude = CalcPolynom(Coef1, middle);

         Double_t left = 0, right = 0;
         if (lok) {
            DefinePolynomEx(size, bins, scales, weights, backgr, lbound - w, lbound + w, Coef2);
            left = (amplitude * .5 - Coef2[0]) / Coef2[1];
         }

         if (rok) {
            DefinePolynomEx(size, bins, scales, weights, backgr, rbound - w, rbound + w, Coef2);
            right = (amplitude * .5 - Coef2[0]) / Coef2[1];
         }

         if (!lok)
            left = 2 * middle - right;
         else if (!rok)
            right = 2 * middle - left;

         Double_t width = (right - left) * 0.5;

         lbound = pmax - 4 * w;
         if (lbound < 0)
            lbound = 0;
         rbound = pmax + 4 * w;
         if (rbound >= size)
            rbound = size - 1;
         for (Int_t i = lbound; i <= rbound; i++)
            use[i] = 0;

         if (lok && rok && (width >= MinWidth) && (width <= MaxWidth) && (amplitude > AmplThreshold) &&
             (amplitude > 2 * sigma))
            fitter->AddGauss1(data->GetName(), fitter->FindNextName("Gauss", 0), middle, width, amplitude);
      }

   } while (validline);
}

#include "f_find_peaks.c"

void TGo4FitPeakFinder::HansEsselPeakFinder(TGo4Fitter *fitter, TGo4FitData *data, Int_t MaxNumPeaks, Int_t ChannelSum,
                                            Double_t NoiseFactor, Double_t NoiseMinimum, Int_t MinimasOrder)
{
   if (!fitter || !data || (MaxNumPeaks < 1))
      return;

   auto iter = data->MakeIter();
   if (!iter)
      return;

   Int_t size = iter->CountPoints();

   if ((size < 10) || (iter->ScalesSize() != 1))
      return;

   TArrayD Bins(size), Scales(size);
   Double_t *bins = Bins.GetArray();
   Double_t *scales = Scales.GetArray();

   Int_t nbin = 0;
   if (iter->Reset())
      do {
         bins[nbin] = data->GetAmplValue() * iter->Value();
         scales[nbin] = iter->x();
         nbin++;
      } while (iter->Next());

   int NumPeaks = 0;
   TArrayI Minimas(MaxNumPeaks), Maximas(MaxNumPeaks), MaximaWidths(MaxNumPeaks);
   TArrayI PeaksLeft(MaxNumPeaks), PeaksRight(MaxNumPeaks), PeaksMaximum(MaxNumPeaks);
   TArrayD MinimaValues(MaxNumPeaks), MinimaScales(MaxNumPeaks), MaximaIntegrals(MaxNumPeaks);

   go4fit_find_peaks(bins, TYPE__DOUBLE, 0, size, ChannelSum, NoiseFactor, NoiseMinimum, nullptr, MaxNumPeaks, &NumPeaks,
                     Minimas.GetArray(), MinimaValues.GetArray(), Maximas.GetArray(), MaximaWidths.GetArray(),
                     MaximaIntegrals.GetArray(), PeaksLeft.GetArray(), PeaksMaximum.GetArray(), PeaksRight.GetArray(),
                     nullptr, nullptr);

   if (NumPeaks <= 0)
      return;

   Int_t NumMinimas = (NumPeaks < MaxNumPeaks) ? NumPeaks + 1 : MaxNumPeaks;
   for (int n = 0; n < NumMinimas; n++) {
      MinimaScales[n] = scales[Minimas[n]];
   }

   if ((MinimasOrder >= 0) && (NumMinimas > 1)) {
      Int_t order = MinimasOrder;
      if (order >= NumMinimas)
         order = NumMinimas - 1;
      TArrayD Coef(order + 1);
      DefinePolynom(NumMinimas, MinimaValues.GetArray(), MinimaScales.GetArray(), Coef);
      fitter->AddPolynomX(data->GetName(), "Pol", Coef);
   }

   for (int n = 0; n < NumPeaks; n++) {
      Double_t pos = scales[PeaksMaximum[n]];
      Double_t width = 0.;
      if (PeaksRight[n] - PeaksLeft[n] < 1)
         width = scales[PeaksMaximum[n]] - scales[PeaksMaximum[n] - 1];
      else
         width = TMath::Abs(scales[PeaksRight[n]] - scales[PeaksLeft[n]]) / 2.;
      Double_t ampl = bins[PeaksMaximum[n]];

      fitter->AddGauss1(data->GetName(), fitter->FindNextName("Gauss", 0), pos, width, ampl);
   }
}