Index: /trunk/Mars/mcore/DrsCalib.h
===================================================================
--- /trunk/Mars/mcore/DrsCalib.h	(revision 12923)
+++ /trunk/Mars/mcore/DrsCalib.h	(revision 12924)
@@ -155,48 +155,4 @@
     }
 
-    void AddT(const int16_t *val, const int16_t *start)
-    {
-        // 1440 without tm, 1600 with tm
-        for (size_t ch=0; ch<fNumChannels; ch++)
-        {
-            const int16_t spos = start[ch];
-            if (spos<0)
-                continue;
-
-            const size_t pos = ch*fNumSamples;
-
-            uint32_t nperiods = 0;
-
-            for (size_t i=0; i<fNumSamples; i++)
-            {
-                const size_t abs0 = pos + (spos+i  )%1024;
-                const size_t abs1 = pos + (spos+i+1)%1024;
-
-                const float &v0 = val[abs0];
-                const float &v1 = val[abs1];
-
-                // Has sign changed?
-                if (v0*v1>0)
-                {
-                    // Sign has not changed
-                    fSum[abs0]  += nperiods;
-                    fSum2[abs0] += nperiods*nperiods;
-                    continue;
-                }
-
-                const double p = v0==v1 ? 1 : v0/(v0-v1);
-
-                const double value = nperiods*p + (nperiods+1)*(1-p);
-
-                fSum[abs0]  += value;
-                fSum2[abs0] += value*value;
-
-                nperiods++;
-            }
-        }
-
-        fNumEntries++;
-    }
-
     static void ApplyCh(float *vec, const int16_t *val, int16_t start, uint32_t roi,
                         const int32_t *offset, const uint32_t scaleabs,
@@ -385,5 +341,4 @@
             max[i] = *pmax;
         }
-
     }
 
@@ -391,4 +346,230 @@
 
     uint64_t GetNumEntries() const { return fNumEntries; }
+};
+
+class DrsCalibrateTime
+{
+public:
+    uint64_t fNumEntries;
+
+    size_t fNumSamples;
+    size_t fNumChannels;
+
+    std::vector<std::pair<double, double>> fStat;
+
+public:
+    DrsCalibrateTime() : fNumEntries(0), fNumSamples(0), fNumChannels(0)
+    {
+        InitSize(160, 1024);
+    }
+
+    DrsCalibrateTime(const DrsCalibrateTime &p) : fNumEntries(p.fNumEntries), fNumSamples(p.fNumSamples), fNumChannels(p.fNumChannels), fStat(p.fStat)
+    {
+    }
+
+    double Sum(uint32_t i) const { return fStat[i].first; }
+    double W(uint32_t i) const { return fStat[i].second; }
+
+    void InitSize(uint16_t channels, uint16_t samples)
+    {
+        fNumChannels = channels;
+        fNumSamples  = samples;
+
+        fNumEntries  = 0;
+
+        fStat.clear();
+
+        fStat.resize(samples*channels);
+    }
+
+    void AddT(const float *val, const int16_t *start, signed char edge=0)
+    {
+        if (fNumSamples!=1024 || fNumChannels!=160)
+            return;
+
+        // Rising or falling edge detection has the advantage that
+        // we are much less sensitive to baseline shifts
+
+        for (size_t ch=0; ch<160; ch++)
+        {
+            const size_t tm = ch*9+8;
+
+            const int16_t spos = start[tm];
+            if (spos<0)
+                continue;
+
+            const size_t pos = ch*1024;
+
+            double  p_prev =  0;
+            int32_t i_prev = -1;
+
+            for (size_t i=0; i<1024-1; i++)
+            {
+                const size_t rel = tm*1024 + i;
+
+                const float &v0 = val[rel];  //-avg;
+                const float &v1 = val[rel+1];//-avg;
+
+                // Is rising edge?
+                if (edge>0 && v0>0)
+                    continue;
+
+                // Is falling edge?
+                if (edge<0 && v0<0)
+                    continue;
+
+                // Has sign changed?
+                if ((v0<0 && v1<0) || (v0>0 && v1>0))
+                    continue;
+
+                const double p = v0==v1 ? 0.5 : v0/(v0-v1);
+
+                const double l = i+p - (i_prev+p_prev);
+
+                if (i_prev>=0)
+                {
+                    const double w0 = 1-p_prev;
+                    const double w1 = p;
+
+                    fStat[pos+(spos+i_prev)%1024].first  += w0*l;
+                    fStat[pos+(spos+i_prev)%1024].second += w0;
+
+                    for (size_t k=i_prev+1; k<i; k++)
+                    {
+                        fStat[pos+(spos+k)%1024].first  += l;
+                        fStat[pos+(spos+k)%1024].second += 1;
+                    }
+
+                    fStat[pos+(spos+i)%1024].first  += w1*l;
+                    fStat[pos+(spos+i)%1024].second += w1;
+                }
+
+                p_prev = p;
+                i_prev = i;
+            }
+        }
+        fNumEntries++;
+    }
+
+    void FillEmptyBins()
+    {
+        for (int ch=0; ch<160; ch++)
+        {
+            const auto beg = fStat.begin() + ch*1024;
+            const auto end = beg + 1024;
+
+            double   avg = 0;
+            uint32_t num = 0;
+            for (auto it=beg; it!=end; it++)
+            {
+                if (it->second<fNumEntries-0.5)
+                    continue;
+
+                avg += it->first / it->second;
+                num++;
+            }
+            avg /= num;
+
+            for (auto it=beg; it!=end; it++)
+            {
+                if (it->second>=fNumEntries-0.5)
+                    continue;
+
+                // {
+                //     result[i+1].first  = *is2;
+                //     result[i+1].second = *iw2;
+                // }
+                // else
+                // {
+                it->first  = avg*fNumEntries;
+                it->second = fNumEntries;
+                // }
+            }
+        }
+    }
+
+    DrsCalibrateTime GetComplete() const
+    {
+        DrsCalibrateTime rc(*this);
+        rc.FillEmptyBins();
+        return rc;
+    }
+
+    void CalcResult()
+    {
+        for (int ch=0; ch<160; ch++)
+        {
+            const auto beg = fStat.begin() + ch*1024;
+            const auto end = beg + 1024;
+
+            // Calculate mean
+            double s = 0;
+            double w = 0;
+
+            for (auto it=beg; it!=end; it++)
+            {
+                s += it->first;
+                w += it->second;
+            }
+
+            s /= w;
+
+            double sumw = 0;
+            double sumv = 0;
+            int n = 0;
+
+            // Sums about many values are numerically less stable than
+            // just sums over less. So we do the exercise from both sides.
+            for (auto it=beg; it!=end-512; it++, n++)
+            {
+                const double v = it->first;
+                const double w = it->second;
+
+                it->first = sumv>0 ? n*(1-s*sumw/sumv) :0;
+
+                sumw += w;
+                sumv += v;
+            }
+
+            sumw = 0;
+            sumv = 0;
+            n = 1;
+
+            for (auto it=end-1; it!=beg-1+512; it--, n++)
+            {
+                const double v = it->first;
+                const double w = it->second;
+
+                sumw += w;
+                sumv += v;
+
+                it->first = sumv>0 ? n*(s*sumw/sumv-1) : 0;
+            }
+        }
+    }
+
+    DrsCalibrateTime GetResult() const
+    {
+        DrsCalibrateTime rc(*this);
+        rc.CalcResult();
+        return rc;
+    }
+
+    double Offset(uint32_t ch, double pos) const
+    {
+        const auto p = fStat.begin() + ch*1024;
+
+        const uint32_t f = floor(pos);
+
+        const double v0 = p[f].first;
+        const double v1 = p[(f+1)%1024].first;
+
+        return v0 + fmod(pos, 1)*(v1-v0);
+    }
+
+    double Calib(uint32_t ch, double pos) const
+    {
+        return pos-Offset(ch, pos);
+    }
 };