Index: trunk/FACT++/src/ratecontrol.cc
===================================================================
--- trunk/FACT++/src/ratecontrol.cc	(revision 12355)
+++ trunk/FACT++/src/ratecontrol.cc	(revision 12355)
@@ -0,0 +1,476 @@
+#include <valarray>
+
+#include "Dim.h"
+#include "Event.h"
+#include "Shell.h"
+#include "StateMachineDim.h"
+#include "Connection.h"
+#include "Configuration.h"
+#include "Console.h"
+#include "Converter.h"
+#include "DimServiceInfoList.h"
+//#include "PixelMap.h"
+
+#include "tools.h"
+
+#include "LocalControl.h"
+
+#include "HeadersFTM.h"
+
+namespace ba    = boost::asio;
+namespace bs    = boost::system;
+namespace dummy = ba::placeholders;
+
+using namespace std;
+
+// ------------------------------------------------------------------------
+
+#include "DimDescriptionService.h"
+
+// ------------------------------------------------------------------------
+
+class StateMachineRateControl : public StateMachineDim, public DimInfoHandler
+{
+private:
+    enum states_t
+    {
+        kStateDimNetworkNA = 1,
+        kStateDisconnected,
+        kStateConnecting,
+        kStateConnected,
+        kStateInProgress,
+    };
+
+    DimServiceInfoList fNetwork;
+
+    pair<Time, int> fStatusDim;
+    pair<Time, int> fStatusFTM;
+
+    DimStampedInfo fDim;
+    DimStampedInfo fFTM;
+    DimStampedInfo fRates;
+    DimStampedInfo fStatic;
+
+//    DimDescribedService fDimData;
+//    DimDescribedService fDimProc;
+
+    bool fTriggerOn;
+
+    pair<Time, int> GetNewState(DimStampedInfo &info) const
+    {
+        const bool disconnected = info.getSize()==0;
+
+        // Make sure getTimestamp is called _before_ getTimestampMillisecs
+        const int tsec = info.getTimestamp();
+        const int tms  = info.getTimestampMillisecs();
+
+        return make_pair(Time(tsec, tms*1000),
+                         disconnected ? -2 : info.getQuality());
+    }
+
+    bool CheckEventSize(size_t has, const char *name, size_t size)
+    {
+        if (has==size)
+            return true;
+
+        if (has==0)
+            return false;
+
+        ostringstream msg;
+        msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
+        Fatal(msg);
+        return false;
+    }
+
+    vector<uint16_t> fThresholds;
+
+    void PrintThresholds(const FTM::DimStaticData &sdata)
+    {
+        //if (!fVerbose)
+        //    return;
+
+        if (fThresholds.size()==0)
+            return;
+
+        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++)
+        {
+            for (int k=0; k<16; k+=4)
+            {
+                for (int i=0; i<4; i++)
+                    if (fThresholds[j*k+i]!=300)
+                        Out() << setw(3) << fThresholds[j*k+i] << " ";
+                    else
+                        Out() << " -  ";
+                Out() << "   ";
+            }
+            Out() << endl;
+        }
+        Out() << endl;
+    }
+
+    void Step(int idx, float step)
+    {
+        uint16_t diff = fThresholds[idx]+int16_t(truncf(step));
+        if (diff<300)
+            diff=300;
+
+        if (diff==fThresholds[idx])
+            return;
+
+        if (1/*fVerbose*/)
+        {
+            Out() << idx/40 << "|" << (idx/4)%10 << "|" << idx%4;
+            Out() << (step>0 ? " += " : " -= ");
+            Out() << step << " (" << diff << ")" << endl;
+        }
+
+        const uint32_t val[2] = { idx,  diff };
+        DimClient::sendCommandNB("FTM_CONTROL/SET_THRESHOLD", (void*)val, 8);
+    }
+
+    void infoHandler()
+    {
+        DimInfo *curr = getInfo(); // get current DimInfo address
+        if (!curr)
+            return;
+
+        if (curr==&fFTM)
+        {
+            fStatusFTM = GetNewState(fFTM);
+            return;
+        }
+
+        if (curr==&fDim)
+        {
+            fStatusDim = GetNewState(fDim);
+            fStatusDim.second = curr->getSize()==4 ? curr->getInt() : 0;
+            return;
+        }
+
+        static vector<uint8_t> counter(160);
+
+        if (curr==&fStatic)
+        {
+            if (!CheckEventSize(curr->getSize(), "infoHandler[DimStaticData]", sizeof(FTM::DimStaticData)))
+                return;
+
+            const FTM::DimStaticData &sdata = *static_cast<FTM::DimStaticData*>(curr->getData());
+
+            PrintThresholds(sdata);
+
+            fTriggerOn = sdata.HasTrigger();
+
+            fThresholds.assign(sdata.fThreshold, sdata.fThreshold+160);
+
+            return;
+        }
+
+        if (curr==&fRates)
+        {
+            if (fThresholds.size()==0)
+                return;
+
+            if (!fTriggerOn)
+                return;
+
+            if (!CheckEventSize(curr->getSize(), "infoHandler[DimTriggerRates]", sizeof(FTM::DimTriggerRates)))
+                return;
+
+            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);
+                }
+            }
+        }
+    }
+
+    void PrintState(const pair<Time,int> &state, const char *server)
+    {
+        const State rc = fNetwork.GetState(server, state.second);
+
+        Out() << state.first.GetAsStr("%H:%M:%S.%f").substr(0, 12) << " - ";
+        Out() << kBold << server << ": ";
+        Out() << rc.name << "[" << rc.index << "]";
+        Out() << kReset << " - " << kBlue << rc.comment << endl;
+    }
+
+    int Print()
+    {
+        Out() << fStatusDim.first.GetAsStr("%H:%M:%S.%f").substr(0, 12) << " - ";
+        Out() << kBold << "DIM_DNS: ";
+        if (fStatusDim.second==0)
+            Out() << "Offline" << endl;
+        else
+            Out() << "V" << fStatusDim.second/100 << 'r' << fStatusDim.second%100 << endl;
+
+        PrintState(fStatusFTM,  "FTM_CONTROL");
+
+        return GetCurrentState();
+    }
+
+    int Execute()
+    {
+        // Dispatch (execute) at most one handler from the queue. In contrary
+        // to run_one(), it doesn't wait until a handler is available
+        // which can be dispatched, so poll_one() might return with 0
+        // handlers dispatched. The handlers are always dispatched/executed
+        // synchronously, i.e. within the call to poll_one()
+        //poll_one();
+
+        if (fStatusDim.second==0)
+            return kStateDimNetworkNA;
+
+        // All subsystems are not connected
+        if (fStatusFTM.second<FTM::kConnected)
+            return kStateDisconnected;
+
+        // At least one subsystem is not connected
+        //        if (fStatusFTM.second>=FTM::kConnected)
+        return fTriggerOn ? kStateInProgress : kStateConnected;
+    }
+
+public:
+    StateMachineRateControl(ostream &out=cout) : StateMachineDim(out, "RATE_CONTROL"),
+        fStatusDim(make_pair(Time(), -2)),
+        fStatusFTM(make_pair(Time(), -2)),
+        fDim("DIS_DNS/VERSION_NUMBER",  (void*)NULL, 0, this),
+        fFTM("FTM_CONTROL/STATE",       (void*)NULL, 0, this),
+        fRates("FTM_CONTROL/TRIGGER_RATES", (void*)NULL, 0, this),
+        fStatic("FTM_CONTROL/STATIC_DATA", (void*)NULL, 0, this)/*,
+        fDimData("RATE_SCAN/DATA", "I:1;F:1;F:1;F:1;F:40;F:160", ""),
+        fDimProc("RATE_SCAN/PROCESS_DATA", "I:1;I:1;I:1",
+                 "Rate scan process data"
+                 "|min[DAC]:Value at which scan was started"
+                 "|max[DAC]:Value at which scan will end"
+                 "|step[DAC]:Step size for scan")*/,
+        fTriggerOn(false)
+    {
+        // ba::io_service::work is a kind of keep_alive for the loop.
+        // It prevents the io_service to go to stopped state, which
+        // would prevent any consecutive calls to run()
+        // or poll() to do nothing. reset() could also revoke to the
+        // previous state but this might introduce some overhead of
+        // deletion and creation of threads and more.
+
+        // State names
+        AddStateName(kStateDimNetworkNA, "DimNetworkNotAvailable",
+                     "The Dim DNS is not reachable.");
+
+        AddStateName(kStateDisconnected, "Disconnected",
+                     "The Dim DNS is reachable, but the required subsystems are not available.");
+
+        AddStateName(kStateConnected, "Connected",
+                     "All needed subsystems are connected to their hardware, no action is performed.");
+
+        AddStateName(kStateInProgress, "InProgress",
+                     "Rate scan in progress.");
+
+//        AddEvent("STOP", kStateInProgress)
+//            (bind(&StateMachineRateControl::StopRateControl, this))
+//            ("Stop a ratescan in progress");
+
+        AddEvent("PRINT")
+            (bind(&StateMachineRateControl::Print, this))
+            ("");
+    }
+
+    int EvalOptions(Configuration &conf)
+    {
+        /*
+        fSecondsMax = conf.Get<uint16_t>("max-wait");
+        fResolution = conf.Get<double>("resolution");
+        */
+        return -1;
+    }
+};
+
+// ------------------------------------------------------------------------
+
+#include "Main.h"
+
+template<class T>
+int RunShell(Configuration &conf)
+{
+    return Main::execute<T, StateMachineRateControl>(conf);
+}
+
+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.")
+        ;
+
+    conf.AddOptions(control);*/
+}
+
+/*
+ Extract usage clause(s) [if any] for SYNOPSIS.
+ Translators: "Usage" and "or" here are patterns (regular expressions) which
+ are used to match the usage synopsis in program output.  An example from cp
+ (GNU coreutils) which contains both strings:
+  Usage: cp [OPTION]... [-T] SOURCE DEST
+    or:  cp [OPTION]... SOURCE... DIRECTORY
+    or:  cp [OPTION]... -t DIRECTORY SOURCE...
+ */
+void PrintUsage()
+{
+    cout <<
+        "The ratescan program is a tool for automation of rate scans.\n"
+        "\n"
+        "Usage: ratescan [-c type] [OPTIONS]\n"
+        "  or:  ratescan [OPTIONS]\n";
+    cout << endl;
+}
+
+void PrintHelp()
+{
+    Main::PrintHelp<StateMachineRateControl>();
+
+    /* Additional help text which is printed after the configuration
+     options goes here */
+
+    /*
+     cout << "bla bla bla" << endl << endl;
+     cout << endl;
+     cout << "Environment:" << endl;
+     cout << "environment" << endl;
+     cout << endl;
+     cout << "Examples:" << endl;
+     cout << "test exam" << endl;
+     cout << endl;
+     cout << "Files:" << endl;
+     cout << "files" << endl;
+     cout << endl;
+     */
+}
+
+int main(int argc, const char* argv[])
+{
+    Configuration conf(argv[0]);
+    conf.SetPrintUsage(PrintUsage);
+    Main::SetupConfiguration(conf);
+    SetupConfiguration(conf);
+
+    if (!conf.DoParse(argc, argv, PrintHelp))
+        return -1;
+
+    //try
+    {
+        // No console access at all
+        if (!conf.Has("console"))
+        {
+//            if (conf.Get<bool>("no-dim"))
+//                return RunShell<LocalStream, StateMachine, ConnectionFSC>(conf);
+//            else
+                return RunShell<LocalStream>(conf);
+        }
+        // Cosole access w/ and w/o Dim
+/*        if (conf.Get<bool>("no-dim"))
+        {
+            if (conf.Get<int>("console")==0)
+                return RunShell<LocalShell, StateMachine, ConnectionFSC>(conf);
+            else
+                return RunShell<LocalConsole, StateMachine, ConnectionFSC>(conf);
+        }
+        else
+*/        {
+            if (conf.Get<int>("console")==0)
+                return RunShell<LocalShell>(conf);
+            else
+                return RunShell<LocalConsole>(conf);
+        }
+    }
+    /*catch (std::exception& e)
+    {
+        cerr << "Exception: " << e.what() << endl;
+        return -1;
+    }*/
+
+    return 0;
+}