Index: trunk/MagicSoft/Mars/mextralgo/MExtralgoDigitalFilter.cc
===================================================================
--- trunk/MagicSoft/Mars/mextralgo/MExtralgoDigitalFilter.cc	(revision 7999)
+++ trunk/MagicSoft/Mars/mextralgo/MExtralgoDigitalFilter.cc	(revision 8072)
@@ -33,12 +33,11 @@
 using namespace std;
 
-Float_t MExtralgoDigitalFilter::ExtractNoise(Int_t iter, Int_t windowsize) const
-{
-    Double_t sum, dummy;
-    Eval(sum, dummy, windowsize, 0, iter-fWeightsPerBin/2);
-    return sum;
-}
-
-void MExtralgoDigitalFilter::Extract(Int_t windowsize, Float_t timeshift)
+Float_t MExtralgoDigitalFilter::ExtractNoise(Int_t iter) const
+{
+    return Eval(fWeightsAmp, 0, iter-fWeightsPerBin/2);
+}
+
+#include <iostream>
+void MExtralgoDigitalFilter::Extract()
 {
     fSignal    =  0; // default is: no pulse found
@@ -49,24 +48,22 @@
     // FIXME: How to handle saturation?
 
-    Float_t maxamp  = -FLT_MAX;
-    Float_t maxtime =  0;
-    Int_t   maxp    = -1;
+    Double_t maxamp = -FLT_MAX;
+    Int_t    maxp   = -1;
 
     //
     // Calculate the sum of the first fWindowSize slices
     //
-    for (Int_t i=0; i<fNum-windowsize+1; i++)
-    {
-        Double_t sumamp, sumtime;
-        Eval(sumamp, sumtime, windowsize, i);
-
+
+    // For the case of an even numberof weights/bin there is
+    // no central bin.So we create an artificial central bin.
+    for (Int_t i=0; i<fNum-fWindowSize+1; i++)
+    {
+        const Double_t sumamp = Eval(fWeightsAmp, i);
         if (sumamp>maxamp)
         {
-            maxamp  = sumamp;
-            maxtime = sumtime;
-            maxp    = i;
-        }
-    }
-
+            maxamp = sumamp;
+            maxp   = i;
+        }
+    }
     // The number of available slices were smaller than the
     // extraction window size of the extractor
@@ -84,49 +81,307 @@
         return;
 
-    // This is the time offset from the extraction position
-    const Float_t time = maxtime/maxamp;
+    Int_t frac = 0;
+    const Int_t shift = AlignExtractionWindow(maxp, frac, maxamp);
+
+    // For safety we do another iteration if we have
+    // shifted the extraction window
+    if (TMath::Abs(shift)>0)
+        AlignExtractionWindow(maxp, frac);
+
+    // Now we have found the "final" position: extract time and charge
+    const Double_t sumamp = Eval(fWeightsAmp, maxp, frac);
+
+    fSignal = sumamp;
+    if (sumamp == 0)
+        return;
+
+    const Double_t sumtime = Eval(fWeightsTime, maxp, frac);
 
     // This is used to align the weights to bins between
     // -0.5/fWeightsPerBin and 0.5/fWeightsPerBin instead of
     // 0 and 1./fWeightsPerBin
-    const Float_t halfres = 0.5/fWeightsPerBin;
-
-    // move the extractor by an offset number of slices
-    // determined by the extracted time
-    const Int_t integ = TMath::FloorNint(time+halfres+0.5);
-    maxp -= integ;
-
-    // Convert the residual fraction of one slice into an
-    // offset position in the extraction weights
-    Int_t frac = TMath::FloorNint((time+halfres-integ)*fWeightsPerBin);
-
-    // Align maxp into available range
-    if (maxp < 0)
-    {
-        maxp = 0;
-        frac = -fWeightsPerBin/2; // Assume peak at the beginning of the first slice
-    }
-    if (maxp+windowsize > fNum)
-    {
-        maxp = fNum-windowsize;
-        frac = fWeightsPerBin/2-1; // Assume peak at the end of the last slice
-    }
-
-    Double_t sumamp, sumtime;
-    Eval(sumamp, sumtime, windowsize, maxp, frac);
-
-    fSignal = sumamp;
-    if (sumamp == 0)
-        return;
+    const Double_t binoffset = TMath::Even(fWeightsPerBin) ? 0.5 : 0;
+
+    fTime = maxp /*- 0.5*/ -  Double_t(frac+binoffset)/fWeightsPerBin;
+
+    // To let the lowest value which can be safely extracted be>0:
+    // Take also a possible offset from timefineadjust into account
+    //  Sould it be: fTime += fWindowSize/2; ???
+    fTime += 0.5 + 0.5/fWeightsPerBin;
+    // In the ideal case the would never get <0
+    // But timefineadjust can be >0.05 (in 60% of the cases)
+
+    // Define in each extractor a lowest and highest extracted value!
 
     // here, the first high-gain slice is already included
     // in the fTimeShiftHiGain this shifts the time to the
     // start of the rising edge
-    fTime = maxp - Float_t(frac)/fWeightsPerBin + timeshift;
+
+    // This is from MExtractTimeAndChargeDigitalFilter
+    // fTime += 0.5 + 1./fWeightsPerBin;
+
+    // Now there is a difference between the rising edge and
+    // the extracted time. This must be applied after
+    // the randomization in case of wrog values
 
     const Float_t timefineadjust = sumtime/sumamp;
 
     // FIXME: Is 3 a good value?
-    if (TMath::Abs(timefineadjust)*fWeightsPerBin < 3.)
+    //if (TMath::Abs(timefineadjust)*fWeightsPerBin < 3.)
+    //    fTime -= timefineadjust;
+
+    //    if (TMath::FloorNint(timefineadjust+0.5)==0)
+
+    //if (TMath::Abs(timefineadjust) < 0.2)
         fTime -= timefineadjust;
 }
+
+#include <TH1.h>
+#include <TH2.h>
+#include <TMatrixD.h>
+#include <TArrayF.h>
+#include <iostream>
+#include <TSpline.h>
+#include <TProfile.h>
+
+Bool_t MExtralgoDigitalFilter::CalculateWeights(TH1 &shape, const TH2 &autocorr, TArrayF &weightsamp, TArrayF &weightstime, Int_t wpb)
+{
+    const Int_t weightsperbin = wpb<=0?shape.GetNbinsX()/autocorr.GetNbinsX():wpb;
+
+    if (wpb<=0 && weightsperbin*autocorr.GetNbinsX()!=shape.GetNbinsX())
+    {
+        cout << "ERROR - Number of bins mismatch..." << endl;
+        cout << "        Shape: " << shape.GetNbinsX() << endl;
+        cout << "        ACorr: " << autocorr.GetNbinsX() << endl;
+        return kFALSE;
+    }
+
+    const TAxis &axe = *shape.GetXaxis();
+
+    const Int_t first = axe.GetFirst()/weightsperbin;
+    const Int_t last  = axe.GetLast() /weightsperbin;
+
+    const Int_t width = last-first;
+
+    cout << "Range:  " << first << " <= bin < " << last << endl;
+    cout << "Window: " << width << endl;
+    cout << "W/Bin:  " << weightsperbin << endl;
+
+    // ---------------------------------------------
+
+    const Float_t sum = shape.Integral(first*weightsperbin, last*weightsperbin-1)/weightsperbin;
+    shape.Scale(1./sum);
+
+    cout << "Sum:    " << sum << endl;
+
+//    TGraph gr(&shape);
+//    TSpline5 val("Signal", &gr);
+
+    // FIXME: DELETE!!!
+    TH1 &derivative = *static_cast<TH1*>(shape.Clone());
+    derivative.Reset();
+
+    for (int i=0; i<derivative.GetNbinsX(); i++)
+    {
+//       const Float_t x = derivative.GetBinCenter(i+1);
+//       derivative.SetBinContent(i+1, val.Derivative(x));
+
+        const Float_t binm = shape.GetBinContent(i+1-1);
+        const Float_t binp = shape.GetBinContent(i+1+1);
+
+        const Float_t der = (binp-binm)/2./shape.GetBinWidth(1);
+
+        derivative.SetBinContent(i+1, der);
+
+        if (derivative.InheritsFrom(TProfile::Class()))
+            static_cast<TProfile&>(derivative).SetBinEntries(i+1,1);
+    }
+
+    // ---------------------------------------------
+
+    TMatrixD B(width, width);
+    for (Int_t i=0; i<width; i++)
+        for (Int_t j=0; j<width; j++)
+            B[i][j]=autocorr.GetBinContent(i+1/*first*/, j+1/*first*/);
+
+    const TMatrixD Binv(TMatrixD::kInverted, B);
+
+    // ---------------------------------------------
+
+    weightsamp.Set(width*weightsperbin);
+    weightstime.Set(width*weightsperbin);
+
+    for (Int_t i=0; i<weightsperbin; i++)
+    {
+        TMatrixD g(width, 1);
+        TMatrixD d(width, 1);
+
+        for (Int_t bin=0; bin<width; bin++)
+        {
+            const Int_t idx = weightsperbin*(bin+first) + i;
+
+            g[bin][0]=shape.GetBinContent(idx+1);
+            d[bin][0]=derivative.GetBinContent(idx+1);
+        }
+
+        const TMatrixD gT(TMatrixD::kTransposed, g);
+        const TMatrixD dT(TMatrixD::kTransposed, d);
+
+        const TMatrixD denom  = (gT*(Binv*g))*(dT*(Binv*d)) - (dT*(Binv*g))*(dT*(Binv*g));
+
+        if (denom[0][0]==0)
+        {
+            cout << "ERROR - Division by zero: denom[0][0]==0 for i=" << i << "." << endl;
+            return kFALSE;
+        }
+
+        const TMatrixD w_amp  = (dT*(Binv*d))*(gT*Binv) - (gT*(Binv*d))*(dT*Binv);
+        const TMatrixD w_time = (gT*(Binv*g))*(dT*Binv) - (gT*(Binv*d))*(gT*Binv);
+
+        for (Int_t bin=0; bin<width; bin++)
+        {
+            const Int_t idx = weightsperbin*bin + i;
+
+            weightsamp[idx]  = w_amp [0][bin]/denom[0][0];
+            weightstime[idx] = w_time[0][bin]/denom[0][0];
+        }
+    }
+
+    return kTRUE;
+}
+
+void MExtralgoDigitalFilter::CalculateWeights2(TH1F &shape, const TH2F &autocorr, TArrayF &weightsamp, TArrayF &weightstime, Int_t wpb)
+{
+    const Int_t weightsperbin = wpb<=0?shape.GetNbinsX()/autocorr.GetNbinsX():wpb;
+
+    if (wpb<=0 && weightsperbin*autocorr.GetNbinsX()!=shape.GetNbinsX())
+    {
+        cout << "ERROR - Number of bins mismatch..." << endl;
+        cout << "        Shape: " << shape.GetNbinsX() << endl;
+        cout << "        ACorr: " << autocorr.GetNbinsX() << endl;
+        return;
+    }
+
+    const TAxis &axe = *shape.GetXaxis();
+
+    const Int_t first = axe.GetFirst()/weightsperbin;
+    const Int_t last  = axe.GetLast() /weightsperbin;
+
+    const Int_t width = last-first;
+
+    cout << "Range:  " << first << " <= bin < " << last << endl;
+    cout << "Window: " << width << endl;
+    cout << "W/Bin:  " << weightsperbin << endl;
+
+    // ---------------------------------------------
+
+    const Float_t sum = shape.Integral(first*weightsperbin, last*weightsperbin-1)/weightsperbin;
+    shape.Scale(1./sum);
+
+    TGraph gr(&shape);
+    TSpline5 val("Signal", &gr);
+
+    TH1F derivative(shape);
+    derivative.Reset();
+
+    for (int i=0; i<derivative.GetNbinsX(); i++)
+    {
+        const Float_t x = derivative.GetBinCenter(i+1);
+        derivative.SetBinContent(i+1, val.Derivative(x));
+
+     /*
+        const Float_t binm = shape.GetBinContent(i+1-1);
+        const Float_t binp = shape.GetBinContent(i+1+1);
+
+        const Float_t der = (binp-binm)/2./shape.GetBinWidth(1);
+
+        derivative.SetBinContent(i+1, der);
+      */
+    }
+
+    // ---------------------------------------------
+
+    TMatrixD B(width, width);
+    for (Int_t i=0; i<width; i++)
+        for (Int_t j=0; j<width; j++)
+            B[i][j]=autocorr.GetBinContent(i+first, j+first);
+    B.Invert();
+
+    // ---------------------------------------------
+
+    weightsamp.Set(width*weightsperbin);
+    weightstime.Set(width*weightsperbin);
+
+    for (Int_t i=0; i<weightsperbin; i++)
+    {
+        TMatrixD g(width, 1);
+        TMatrixD d(width, 1);
+
+        for (Int_t bin=0; bin<width; bin++)
+        {
+            const Int_t idx = weightsperbin*(bin+first) + i;
+
+            g[bin][0]=shape.GetBinContent(idx+1);
+            d[bin][0]=derivative.GetBinContent(idx+1);
+        }
+
+        const TMatrixD gT(TMatrixD::kTransposed, g);
+        const TMatrixD dT(TMatrixD::kTransposed, d);
+
+        const TMatrixD denom  = (gT*(B*g))*(dT*(B*d)) - (dT*(B*g))*(dT*(B*g));
+
+        const TMatrixD w_amp  = (dT*(B*d))*(gT*B) - (gT*(B*d))*(dT*B);
+        const TMatrixD w_time = (gT*(B*g))*(dT*B) - (gT*(B*d))*(gT*B);
+
+        for (Int_t bin=0; bin<width; bin++)
+        {
+            const Int_t idx = weightsperbin*bin + i;
+
+            weightsamp[idx]  = w_amp [0][bin]/denom[0][0];
+            weightstime[idx] = w_time[0][bin]/denom[0][0];
+        }
+    }
+}
+/*
+Int_t start = fBinningResolutionHiGain*(fSignalStartBinHiGain + 1);
+for (Int_t i = -fBinningResolutionHiGain/2+1; i<=fBinningResolutionHiGain/2; i++)
+{
+    // i = -4...5
+    for (Int_t count=0; count < fWindowSizeHiGain; count++)
+    {
+        // count=0: pos=10*(start+0) + 10 + i
+        // count=1: pos=10*(start+1) + 10 + i
+        // count=2: pos=10*(start+2) + 10 + i
+        // count=3: pos=10*(start+3) + 10 + i
+    }
+
+    for (Int_t count=0; count < fWindowSizeHiGain; count++)
+    {
+        // count=0: pos = 10*0 + 5 +i-1
+        // count=1: pos = 10*1 + 5 +i-1
+        // count=2: pos = 10*2 + 5 +i-1
+        // count=3: pos = 10*3 + 5 +i-1
+    }
+}
+
+Int_t start = fBinningResolutionLoGain*fSignalStartBinLoGain + fBinningResolutionLoGain/2;
+for (Int_t i = -fBinningResolutionLoGain/2+1; i<=fBinningResolutionLoGain/2; i++)
+{
+    // i=-4..5
+    for (Int_t count=0; count < fWindowSizeLoGain; count++)
+    {
+        // count=0: pos = 10*(start+0) + 5 + i
+        // count=1: pos = 10*(start+1) + 5 + i
+        // count=2: pos = 10*(start+2) + 5 + i
+        // count=3: pos = 10*(start+3) + 5 + i
+    }
+
+    for (Int_t count=0; count < fWindowSizeLoGain; count++)
+    {
+        // count=0: pos = 10*0 + 5 +i-1
+        // count=1: pos = 10*1 + 5 +i-1
+        // count=2: pos = 10*2 + 5 +i-1
+        // count=3: pos = 10*3 + 5 +i-1
+    }
+}
+*/
Index: trunk/MagicSoft/Mars/mextralgo/MExtralgoDigitalFilter.h
===================================================================
--- trunk/MagicSoft/Mars/mextralgo/MExtralgoDigitalFilter.h	(revision 7999)
+++ trunk/MagicSoft/Mars/mextralgo/MExtralgoDigitalFilter.h	(revision 8072)
@@ -5,4 +5,10 @@
 #include <TROOT.h>
 #endif
+
+class TH1;
+class TH2;
+class TH1F;
+class TH2F;
+class TArrayF;
 
 class MExtralgoDigitalFilter
@@ -13,8 +19,9 @@
     Int_t    fNum;
 
-    Float_t *fWeightsAmp;
-    Float_t *fWeightsTime;
+    Float_t const *fWeightsAmp;
+    Float_t const *fWeightsTime;
 
-    Int_t   fWeightsPerBin; // Number of weights per data bin
+    const Int_t fWeightsPerBin; // Number of weights per data bin
+    const Int_t fWindowSize;
 
     // Result
@@ -24,5 +31,8 @@
     Float_t fSignalDev;
 
-    inline void Eval(Double_t &sumamp, Double_t &sumtime, const Int_t windowsize, const Int_t startv, const Int_t startw=0) const
+    // Weights: Weights to evaluate
+    // Startv: Index of first bin of data
+    // startw: Offset on the weights
+    inline Double_t Eval(Float_t const *weights, const Int_t startv, const Int_t startw=0) const
     {
         //
@@ -30,31 +40,87 @@
         // and multiply the entries with the corresponding weights
         //
-        sumamp = 0;
-        sumtime = 0;
+        Double_t sum = 0;
 
         // Shift the start of the weight to the center of sample 0
-        Float_t const *wa  = fWeightsAmp  + fWeightsPerBin/2 + startw;
-        Float_t const *wt  = fWeightsTime + fWeightsPerBin/2 + startw;
+        Float_t const *w = weights + startw;
+
         Float_t *const beg = fVal+startv;
+        for (Float_t const *pex=beg; pex<beg+fWindowSize; pex++)
+        {
+            sum += *w * *pex;
+            w += fWeightsPerBin;
+        }
+        return sum;
+    }
 
-        for (Float_t const *pex=beg; pex<beg+windowsize; pex++)
+    inline void AlignIntoLimits(Int_t &maxp, Int_t &frac) const
+    {
+        // Align maxp into available range (TO BE CHECKED)
+        if (maxp < 0)
         {
-            sumamp  += *wa * *pex;
-            sumtime += *wt * *pex;
-
-            // Step forward by one bin
-            wa += fWeightsPerBin;
-            wt += fWeightsPerBin;
+            maxp = 0;
+            frac = fWeightsPerBin/2-1; // Assume peak at the end of the last slice
+        }
+        if (maxp > fNum-fWindowSize)
+        {
+            maxp = fNum-fWindowSize;
+            frac = -fWeightsPerBin/2; // Assume peak at the beginning of the first slice
         }
     }
 
+    inline Int_t AlignExtractionWindow(Int_t &maxp, Int_t &frac, const Double_t ampsum)
+    {
+        // Align extraction window to signal position
+
+        const Double_t timesum = Eval(fWeightsTime, maxp, frac);
+
+        // Because fWeightsPerBin/2 doesn't correspond to the center
+        // of a bin the time-values extracted are slightly positive.
+        // They are roughly between -0.45 and 0.55
+        const Double_t binoffset = TMath::Even(fWeightsPerBin) ? 0.5 : 0;
+
+        // This is the time offset from the extraction position
+        Double_t tmoffset = (frac+binoffset)/fWeightsPerBin + timesum/ampsum;
+
+        // Convert the residual fraction of one slice into an
+        // offset position in the extraction weights
+        const Int_t integ = TMath::FloorNint(tmoffset+0.5);
+
+        /*
+        if (integ>0)
+            tmoffset=0.49-0.05;
+        if (integ<0)
+            tmoffset=-0.49-0.05;
+        integ=0;
+        */
+
+        // move the extractor by an offset number of slices
+        // determined by the extracted time
+        maxp -= integ;
+
+        frac  = TMath::FloorNint((tmoffset-integ)*fWeightsPerBin);
+
+        // Align maxp into available range (TO BE CHECKED)
+        AlignIntoLimits(maxp, frac);
+
+        return integ;
+    }
+
+    inline void AlignExtractionWindow(Int_t &maxp, Int_t &frac)
+    {
+        const Double_t amp = Eval(fWeightsAmp, maxp, frac);
+        if (amp!=0)
+            AlignExtractionWindow(maxp, frac, amp);
+    }
+
 public:
-    MExtralgoDigitalFilter(Float_t *val, Int_t n, Float_t *wa, Float_t *wt)
-        : fVal(val), fNum(n),
-        fWeightsAmp(wa), fWeightsTime(wt), fTime(0), fTimeDev(-1), fSignal(0), fSignalDev(-1)
+    MExtralgoDigitalFilter(Int_t res, Int_t windowsize, Float_t *wa, Float_t *wt)
+        : fVal(0), fNum(0), fWeightsAmp(wa+res/2), fWeightsTime(wt+res/2),
+        fWeightsPerBin(res), fWindowSize(windowsize),
+        fTime(0), fTimeDev(-1), fSignal(0), fSignalDev(-1)
     {
     }
 
-    void SetWeightsPerBin(Int_t res) { fWeightsPerBin=res; }
+    void SetData(Int_t n, Float_t *val) { fNum=n; fVal=val; }
 
     Float_t GetTime() const          { return fTime; }
@@ -67,6 +133,9 @@
     void GetTime(Float_t &sig, Float_t &dsig) const   { sig=fTime; dsig=fTimeDev; }
 
-    Float_t ExtractNoise(Int_t iter, Int_t windowsize) const;
-    void Extract(Int_t windowsize, Float_t timeshift);
+    Float_t ExtractNoise(Int_t iter) const;
+    void Extract();
+
+    static Bool_t CalculateWeights(TH1 &shape, const TH2 &autocorr, TArrayF &wa, TArrayF &wt, Int_t wpb=-1);
+    static void CalculateWeights2(TH1F &shape, const TH2F &autocorr, TArrayF &wa, TArrayF &wt, Int_t wpb=-1);
 };