Index: /trunk/FACT++/src/ratecontrol.cc
===================================================================
--- /trunk/FACT++/src/ratecontrol.cc	(revision 12428)
+++ /trunk/FACT++/src/ratecontrol.cc	(revision 12429)
@@ -39,4 +39,9 @@
         kStateConnecting,
         kStateConnected,
+
+        kStateWaitingForReference,
+        kStateSettingGlobalThreshold,
+        kStateGlobalThresholdSet,
+
         kStateInProgress,
     };
@@ -55,5 +60,12 @@
 //    DimDescribedService fDimProc;
 
+    float  fTargetRate;
+    float  fTriggerRate;
+
+    uint16_t fThresholdMin;
+
     bool fTriggerOn;
+    bool fVerbose;
+    bool fEnabled;
 
     pair<Time, int> GetNewState(DimStampedInfo &info) const
@@ -93,4 +105,6 @@
             return;
 
+        Out() << "Min. DAC=" << fThresholdMin << endl;
+
         int t=0;
         for (t=0; t<160; t++)
@@ -106,5 +120,5 @@
             {
                 for (int i=0; i<4; i++)
-                    if (fThresholds[i+k*4+j*16]!=300)
+                    if (fThresholds[i+k*4+j*16]!=fThresholdMin)
                         Out() << setw(3) << fThresholds[i+k*4+j*16] << " ";
                     else
@@ -120,11 +134,11 @@
     {
         uint16_t diff = fThresholds[idx]+int16_t(truncf(step));
-        if (diff<300)
-            diff=300;
+        if (diff<fThresholdMin)
+            diff=fThresholdMin;
 
         if (diff==fThresholds[idx])
             return;
 
-        if (1/*fVerbose*/)
+        if (fVerbose)
         {
             Out() << idx/40 << "|" << (idx/4)%10 << "|" << idx%4;
@@ -137,4 +151,153 @@
     }
 
+    void ProcessPatches(const FTM::DimTriggerRates &sdata)
+    {
+
+        // Caluclate Median and deviation
+        vector<float> medb(sdata.fBoardRate, sdata.fBoardRate+40);
+        vector<float> medp(sdata.fPatchRate, sdata.fPatchRate+160);
+
+        sort(medb.begin(), medb.end());
+        sort(medp.begin(), medp.end());
+
+        vector<float> devb(40);
+        for (int i=0; i<40; i++)
+            devb[i] = fabs(sdata.fBoardRate[i]-medb[i]);
+
+        vector<float> devp(160);
+        for (int i=0; i<160; i++)
+            devp[i] = fabs(sdata.fPatchRate[i]-medp[i]);
+
+        sort(devb.begin(), devb.end());
+        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];
+
+        // If any is zero there is something wrong
+        if (mb==0 || mp==0 || db==0 || dp==0)
+            return;
+
+        if (fVerbose)
+        {
+            Out() << "Patch: Median=" << mp << " Dev=" << dp << endl;
+            Out() << "Board: Median=" << mb << " Dev=" << db << endl;
+        }
+
+        for (int i=0; i<40; i++)
+        {
+            int maxi = -1;
+
+            const float dif = fabs(sdata.fBoardRate[i]-mb)/db;
+            if (dif>5)
+            {
+                if (fVerbose)
+                    Out() << "B" << i << ": " << dif << endl;
+
+                float max = sdata.fPatchRate[i*4];
+                maxi = 0;
+
+                for (int j=1; j<4; j++)
+                    if (sdata.fPatchRate[i*4+j]>max)
+                    {
+                        max = sdata.fPatchRate[i*4+j];
+                        maxi = j;
+                    }
+            }
+
+            for (int j=0; j<4; j++)
+            {
+                // For the noise pixel correct down to median+3*deviation
+                if (maxi==j)
+                {
+                    const float step = (log10(sdata.fPatchRate[i*4+j])-log10(mp+5*dp))/0.039;
+                    //  * (dif-5)/dif
+                    Step(i*4+j, step);
+                    continue;
+                }
+
+                // For pixels below the meadian correct also back to median+3*deviation
+                if (sdata.fPatchRate[i*4+j]<mp)
+                {
+                    const float step = (log10(sdata.fPatchRate[i*4+j])-log10(mp+3*dp))/0.039;
+                    Step(i*4+j, step);
+                    continue;
+                }
+
+                const float step =  -1.5*(log10(mp+dp)-log10(mp))/0.039;
+                Step(i*4+j, step);
+            }
+        }
+    }
+
+    void ProcessCamera(const FTM::DimTriggerRates &sdata)
+    {
+        // Caluclate Median and deviation
+        vector<float> medb(sdata.fBoardRate, sdata.fBoardRate+40);
+
+        sort(medb.begin(), medb.end());
+
+        vector<float> devb(40);
+        for (int i=0; i<40; i++)
+            devb[i] = fabs(sdata.fBoardRate[i]-medb[i]);
+
+        sort(devb.begin(), devb.end());
+
+        double mb = (medb[19]+medb[20])/2;
+        double db = devb[27];
+
+        // If any is zero there is something wrong
+        if (mb==0 || db==0)
+            return;
+
+        double avg = 0;
+        int    num = 0;
+
+        for (int i=0; i<40; i++)
+        {
+            if ( fabs(sdata.fBoardRate[i]-mb)<5*db)
+            {
+                avg += sdata.fBoardRate[i];
+                num++;
+            }
+        }
+
+        fTriggerRate = avg / num * 40;
+
+        if (fVerbose)
+        {
+            Out() << "Board:  Median=" << mb << " Dev=" << db << endl;
+            Out() << "Camera: " << fTriggerRate << " (" << sdata.fTriggerRate << ", n=" << num << ")" << endl;
+        }
+
+        // ----------------------
+
+
+        if (fTriggerRate<fTargetRate)
+            return;
+
+        // Is this a step to 70Hz?
+        const float step = (log10(fTriggerRate)-log10(fTargetRate))/0.039;
+
+        const uint16_t diff = fThresholds[0]+int16_t(truncf(step));
+
+        if (diff==fThresholds[0])
+            return;
+
+        if (fVerbose)
+        {
+            //Out() << idx/40 << "|" << (idx/4)%10 << "|" << idx%4;
+            Out() << fThresholds[0];
+            Out() << (step>0 ? " += " : " -= ");
+            Out() << step << " (" << diff << ")" << endl;
+        }
+
+        const uint32_t val[2] = { -1,  diff };
+        DimClient::sendCommandNB("FTM_CONTROL/SET_THRESHOLD", (void*)val, 8);
+    }
+
     void infoHandler()
     {
@@ -187,83 +350,55 @@
             const FTM::DimTriggerRates &sdata = *static_cast<FTM::DimTriggerRates*>(curr->getData());
 
-            // Caluclate Median and deviation
-            vector<float> medb(sdata.fBoardRate, sdata.fBoardRate+40);
-            vector<float> medp(sdata.fPatchRate, sdata.fPatchRate+160);
-
-            sort(medb.begin(), medb.end());
-            sort(medp.begin(), medp.end());
-
-            vector<float> devb(40);
-            for (int i=0; i<40; i++)
-                devb[i] = fabs(sdata.fBoardRate[i]-medb[i]);
-
-            vector<float> devp(160);
-            for (int i=0; i<160; i++)
-                devp[i] = fabs(sdata.fPatchRate[i]-medp[i]);
-
-            sort(devb.begin(), devb.end());
-            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];
-
-            // If any is zero there is something wrong
-            if (mb==0 || mp==0 || db==0 || dp==0)
-                return;
-
-            if (1/*fVerbose*/)
-            {
-                Out() << "Patch: Median=" << mp << " Dev=" << dp << endl;
-                Out() << "Board: Median=" << mb << " Dev=" << db << endl;
-            }
-
-            for (int i=0; i<40; i++)
-            {
-                int maxi = -1;
-
-                const float dif = fabs(sdata.fBoardRate[i]-mb)/db;
-                if (dif>5)
-                {
-                    if (1/*fVerbose*/)
-                        Out() << "B" << i << ": " << dif << endl;
-
-                    float max = sdata.fPatchRate[i*4];
-                    maxi = 0;
-
-                    for (int j=1; j<4; j++)
-                        if (sdata.fPatchRate[i*4+j]>max)
-                        {
-                            max = sdata.fPatchRate[i*4+j];
-                            maxi = j;
-                        }
-                }
-
-                for (int j=0; j<4; j++)
-                {
-                    // For the noise pixel correct down to median+3*deviation
-                    if (maxi==j)
-                    {
-                        const float step = (log10(sdata.fPatchRate[i*4+j])-log10(mp+5*dp))/0.039;
-                        //  * (dif-5)/dif
-                        Step(i*4+j, step);
-                        continue;
-                    }
-
-                    // For pixels below the meadian correct also back to median+3*deviation
-                    if (sdata.fPatchRate[i*4+j]<mp)
-                    {
-                        const float step = (log10(sdata.fPatchRate[i*4+j])-log10(mp+3*dp))/0.039;
-                        Step(i*4+j, step);
-                        continue;
-                    }
-
-                    const float step =  -1.5*(log10(mp+dp)-log10(mp))/0.039;
-                    Step(i*4+j, step);
-                }
-            }
-        }
+            if (GetCurrentState()==kStateSettingGlobalThreshold)
+                ProcessCamera(sdata);
+
+            if (GetCurrentState()==kStateInProgress)
+                ProcessPatches(sdata);
+        }
+    }
+
+    int StartDataTaking()
+    {
+        if (!fEnabled)
+            return kStateGlobalThresholdSet;
+
+        fThresholds.resize(0);
+
+        const int32_t val[2] = { -1, fThresholdMin };
+        Dim::SendCommand("FTM_CONTROL/SET_THRESHOLD", val);
+
+        return kStateWaitingForReference;
+    }
+
+    int SetEnabled(const EventImp &evt)
+    {
+        if (!CheckEventSize(evt.GetSize(), "SetEnabled", 1))
+            return kSM_FatalError;
+
+        fEnabled = evt.GetBool();
+
+        return GetCurrentState();
+    }
+
+    int SetMinThreshold(const EventImp &evt)
+    {
+        if (!CheckEventSize(evt.GetSize(), "SetMinThreshold", 4))
+            return kSM_FatalError;
+
+        // FIXME: Check missing
+
+        fThresholdMin = evt.GetUShort();
+
+        return GetCurrentState();
+    }
+
+    int SetTargetRate(const EventImp &evt)
+    {
+        if (!CheckEventSize(evt.GetSize(), "SetTargetRate", 4))
+            return kSM_FatalError;
+
+        fTargetRate = evt.GetFloat();
+
+        return GetCurrentState();
     }
 
@@ -292,4 +427,14 @@
     }
 
+    int SetVerbosity(const EventImp &evt)
+    {
+        if (!CheckEventSize(evt.GetSize(), "SetVerbosity", 1))
+            return kSM_FatalError;
+
+        fVerbose = evt.GetBool();
+
+        return GetCurrentState();
+    }
+
     int Execute()
     {
@@ -307,4 +452,26 @@
         if (fStatusFTM.second<FTM::kConnected)
             return kStateDisconnected;
+
+        if (GetCurrentState()==kStateWaitingForReference)
+        {
+            if (fThresholds.size()==0)
+                return kStateWaitingForReference;
+
+            return kStateSettingGlobalThreshold;
+        }
+
+        if (GetCurrentState()==kStateSettingGlobalThreshold)
+        {
+            if (fTriggerRate>fTargetRate)
+                return kStateSettingGlobalThreshold;
+
+            return kStateGlobalThresholdSet;
+        }
+
+        if (GetCurrentState()==kStateGlobalThresholdSet)
+        {
+            // FIXME: What if it changes?
+            return kStateGlobalThresholdSet;
+        }
 
         // At least one subsystem is not connected
@@ -353,15 +520,34 @@
 //            ("Stop a ratescan in progress");
 
+        AddEvent("START_DATA_TAKING")
+            (bind(&StateMachineRateControl::StartDataTaking, this))
+            ("");
+
+        AddEvent("ENABLE_RATE_CONTROL", "B:1")
+            (bind(&StateMachineRateControl::SetEnabled, this, placeholders::_1))
+            ("");
+
+        AddEvent("SET_MIN_THRESHOLD", "I:1")
+            (bind(&StateMachineRateControl::SetMinThreshold, this, placeholders::_1))
+            ("");
+
+        AddEvent("SET_TARGET_RATE", "F:1")
+            (bind(&StateMachineRateControl::SetTargetRate, this, placeholders::_1))
+            ("");
+
         AddEvent("PRINT")
             (bind(&StateMachineRateControl::Print, this))
             ("");
+
+        AddEvent("SET_VERBOSE", "B")
+            (bind(&StateMachineRateControl::SetVerbosity, this, placeholders::_1))
+            ("set verbosity state"
+             "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");
+
     }
 
     int EvalOptions(Configuration &conf)
     {
-        /*
-        fSecondsMax = conf.Get<uint16_t>("max-wait");
-        fResolution = conf.Get<double>("resolution");
-        */
+        fVerbose = !conf.Get<bool>("quiet");
         return -1;
     }
@@ -380,12 +566,12 @@
 void SetupConfiguration(Configuration &conf)
 {
-    /*
     po::options_description control("Rate scan options");
     control.add_options()
-        ("max-wait",   var<uint16_t>(150), "The maximum number of seconds to wait to get the anticipated resolution for a point.")
-        ("resolution", var<double>(0.05) , "The minimum resolution required for a single data point.")
+        ("quiet,q",       po_bool(),  "Disable printing more informations during rate control.")
+       //("max-wait",   var<uint16_t>(150), "The maximum number of seconds to wait to get the anticipated resolution for a point.")
+       // ("resolution", var<double>(0.05) , "The minimum resolution required for a single data point.")
         ;
 
-    conf.AddOptions(control);*/
+    conf.AddOptions(control);
 }