Index: /trunk/FACT++/src/ratecontrol.cc
===================================================================
--- /trunk/FACT++/src/ratecontrol.cc	(revision 16785)
+++ /trunk/FACT++/src/ratecontrol.cc	(revision 16786)
@@ -94,10 +94,10 @@
     }
 
-    vector<uint16_t> fThresholds;
+    vector<uint32_t> fThresholds;
 
     void PrintThresholds(const FTM::DimStaticData &sdata)
     {
-        //if (!fVerbose)
-        //    return;
+        if (!fVerbose)
+            return;
 
         if (fThresholds.empty())
@@ -105,12 +105,4 @@
 
         Out() << "Min. DAC=" << fThresholdMin << endl;
-
-        int t=0;
-        for (t=0; t<160; t++)
-            if (sdata.fThreshold[t]!=fThresholds[t])
-                break;
-
-        if (t==160)
-            return;
 
         for (int j=0; j<10; j++)
@@ -119,8 +111,18 @@
             {
                 for (int i=0; i<4; i++)
-                    if (fThresholds[i+k*4+j*16]!=fThresholdMin)
-                        Out() << setw(3) << fThresholds[i+k*4+j*16] << " ";
+                {
+                    const int p = i + k*4 + j*16;
+
+                    if (fThresholds[p]!=fThresholdMin)
+                        Out() << setw(3) << fThresholds[p];
                     else
-                        Out() << " -  ";
+                        Out() << " - ";
+
+                    if (fThresholds[p]!=sdata.fThreshold[p])
+                        Out() << "!";
+                    else
+                        Out() << " ";
+                }
+
                 Out() << "   ";
             }
@@ -131,26 +133,26 @@
 
     // RETUNR VALUE
-    int32_t Step(int idx, float step)
-    {
-        uint16_t diff = fThresholds[idx]+int16_t(truncf(step));
+    bool Step(int idx, float step)
+    {
+        uint32_t diff = fThresholds[idx]+int16_t(truncf(step));
         if (diff<fThresholdMin)
             diff=fThresholdMin;
+        if (diff>0xffff)
+            diff = 0xffff;
 
         if (diff==fThresholds[idx])
-            return -1;
+            return false;
 
         if (fVerbose)
         {
-            Out() << idx/40 << "|" << (idx/4)%10 << "|" << idx%4;
+            Out() << "Apply: Patch " << setw(3) << idx << " [" << idx/40 << "|" << (idx/4)%10 << "|" << idx%4 << "]";
             Out() << (step>0 ? " += " : " -= ");
-            Out() << fabs(step) << " (" << diff << ")" << endl;
-        }
-
-        //const uint32_t val[2] = { idx,  diff };
-        //Dim::SendCommandNB("FTM_CONTROL/SET_THRESHOLD", val);
-
+            Out() << fabs(step) << " (old=" << fThresholds[idx] << ", new=" << diff << ")" << endl;
+        }
+
+        fThresholds[idx] = diff;
         fBlock[idx/4] = true;
 
-        return diff;
+        return true;
     }
 
@@ -176,9 +178,9 @@
         sort(devp.begin(), devp.end());
 
-        double mb = (medb[19]+medb[20])/2;
-        double mp = (medp[79]+medp[80])/2;
-
-        double db = devb[27];
-        double dp = devp[109];
+        const double mb = (medb[19]+medb[20])/2;
+        const double mp = (medp[79]+medp[80])/2;
+
+        const double db = devb[27];
+        const double dp = devp[109];
 
         // If any is zero there is something wrong
@@ -187,7 +189,7 @@
 
         if (fVerbose)
-            Out() << Tools::Form("Board: Med=%3.1f +- %3.1f   Patch: Med=%3.1f +- %3.1f", mb, db, mp, dp) << endl;
-
-        vector<int32_t> newth(160, -1);
+            Out() << Tools::Form("Boards: Med=%3.1f +- %3.1f Hz   Patches: Med=%3.1f +- %3.1f Hz", mb, db, mp, dp) << endl;
+
+        bool changed = false;
 
         for (int i=0; i<40; i++)
@@ -205,5 +207,5 @@
             {
                 if (fVerbose)
-                    Out() << "B" << i << ": " << dif << endl;
+                    Out() << "Board " << setw(3) << i << ": " << dif << " dev away from med" << endl;
 
                 float max = sdata.fPatchRate[i*4];
@@ -229,5 +231,5 @@
                     const float step = (log10(sdata.fPatchRate[i*4+j])-log10(mp+3.5*dp))/0.039;
                     //  * (dif-5)/dif
-                    newth[i*4+j] = Step(i*4+j, step);
+                    changed |= Step(i*4+j, step);
                     continue;
                 }
@@ -237,19 +239,15 @@
                 {
                     const float step = (log10(sdata.fPatchRate[i*4+j])-log10(mp+3.5*dp))/0.039;
-                    newth[i*4+j] = Step(i*4+j, step);
+                    changed |= Step(i*4+j, step);
                     continue;
                 }
 
                 const float step =  -1.5*(log10(mp+dp)-log10(mp))/0.039;
-                newth[i*4+j] = Step(i*4+j, step);
+                changed |= Step(i*4+j, step);
             }
         }
 
-        // WARNING: By _not_ updating fThresholds here, we assume
-        // the the returned static data block is received before the
-        // next update. If not, we will just skip these patches (block
-        // condition) and send the same update next time again.
-        if (std::count(newth.begin(), newth.end(), -1)!=160)
-            Dim::SendCommandNB("FTM_CONTROL/SET_SELECTED_THRESHOLDS", newth);
+        if (changed)
+            Dim::SendCommandNB("FTM_CONTROL/SET_SELECTED_THRESHOLDS", fThresholds);
     }
 
@@ -532,7 +530,7 @@
             avg2 += vec[i];
 
-            if (vec[i]-avg>3.5*avg_dev)
+            if (vec[i]>avg+3.5*avg_dev)
             {
-                fThresholds[i] = max(uint16_t(40.5*pow(vec[i], 0.642)+164), fThresholdReference);
+                fThresholds[i] = max(uint16_t(40.5*pow(vec[i], 0.642)+164), fThresholdMin);
 
                 counter++;
@@ -542,7 +540,5 @@
 
 
-        vector<int32_t> val(160, fThresholdMin);
-        std::copy(fThresholds.begin(), fThresholds.end(), val.begin());
-        Dim::SendCommandNB("FTM_CONTROL/SET_ALL_THRESHOLDS", val);
+        Dim::SendCommandNB("FTM_CONTROL/SET_ALL_THRESHOLDS", fThresholds);