source: trunk/FACT++/src/ratecontrol.cc@ 13746

#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,

        kStateSettingGlobalThreshold,
        kStateGlobalThresholdSet,

        kStateInProgress,
    };

    bool fTriggerOn;

    vector<bool> fBlock;

    DimServiceInfoList fNetwork;

    pair<Time, int> fStatusDim;
    pair<Time, int> fStatusFTM;
    pair<Time, int> fStatusRS;

    DimStampedInfo fDim;
    DimStampedInfo fFTM;
    DimStampedInfo fRates;
    DimStampedInfo fStatic;
    DimStampedInfo fRateScan;

//    DimDescribedService fDimData;
//    DimDescribedService fDimProc;

    float  fTargetRate;
    float  fTriggerRate;

    uint16_t fThresholdMin;
    uint16_t fThresholdReference;

    bool fVerbose;
    bool fEnabled;

    uint64_t fCounter;

    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;

        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++)
        {
            for (int k=0; k<4; k++)
            {
                for (int i=0; i<4; i++)
                    if (fThresholds[i+k*4+j*16]!=fThresholdMin)
                        Out() << setw(3) << fThresholds[i+k*4+j*16] << " ";
                    else
                        Out() << " -  ";
                Out() << "   ";
            }
            Out() << endl;
        }
        Out() << endl;
    }

    void Step(int idx, float step)
    {
        uint16_t diff = fThresholds[idx]+int16_t(truncf(step));
        if (diff<fThresholdMin)
            diff=fThresholdMin;

        if (diff==fThresholds[idx])
            return;

        if (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);

        fBlock[idx] = true;
    }

    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;
                    }
            }

            vector<bool> block(160);

            for (int j=0; j<4; j++)
            {
                if (fBlock[i*4+j])
                {
                    fBlock[i*4+j] = false;
                    continue;
                }

                // For the noise pixel correct down to median+3*deviation
                if (maxi==j)
                {
                    // This is the step which has to be performed to go from
                    // a NSB rate of sdata.fPatchRate[i*4+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)
    {
        if (fCounter++==0)
            return;

        // 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)
        {
            Warn("The median or the deviation of all board rates is zero... cannot calibrate.");
            return;
        }

        double avg = 0;
        int    num = 0;

        for (int i=0; i<40; i++)
        {
            if ( fabs(sdata.fBoardRate[i]-mb)<2.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;
            Out() << "Target: " << fTargetRate << endl;
        }

        if (sdata.fTriggerRate<fTriggerRate)
            fTriggerRate = sdata.fTriggerRate;

        // ----------------------

        /*
        if (avg>0 && avg<fTargetRate)
        {
            // I am assuming here (and at other places) the the answer from the FTM when setting
            // the new threshold always arrives faster than the next rate update.
            fThresholdMin = fThresholds[0];
            Out() << "Setting fThresholdMin to " << fThresholds[0] << endl;
        }
        */

        if (fTriggerRate>0 && fTriggerRate<fTargetRate)
        {
            fThresholds.assign(160, fThresholdMin);
            return;
        }

        // This is a step towards a threshold at which the NSB rate is equal the target rate
        // +1 to avoid getting a step of 0
        const float step = (log10(fTriggerRate)-log10(fTargetRate))/0.039 + 1;

        const uint16_t diff = fThresholdMin+int16_t(truncf(step));
        if (diff<=fThresholdMin)
            return;

        if (fVerbose)
        {
            //Out() << idx/40 << "|" << (idx/4)%10 << "|" << idx%4;
            Out() << fThresholdMin;
            Out() << (step>0 ? " += " : " -= ");
            Out() << step << " (" << diff << ")" << endl;
        }

        const uint32_t val[2] = { -1,  diff };
        DimClient::sendCommandNB("FTM_CONTROL/SET_THRESHOLD", (void*)val, 8);

        fThresholdMin = diff;
    }

    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;
        }
        if (curr==&fRateScan)
        {
            fStatusRS = GetNewState(fRateScan);
            fStatusRS.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());
            fTriggerOn = sdata.HasTrigger();
 
            PrintThresholds(sdata);

            fThresholds.assign(sdata.fThreshold, sdata.fThreshold+160);
            return;
        }

        if (curr==&fRates)
        {
            if (fThresholds.size()==0)
                return;

            if (!fTriggerOn && !fEnabled)
                return;

            if (fStatusRS.second==5)
                return;

            if (!CheckEventSize(curr->getSize(), "infoHandler[DimTriggerRates]", sizeof(FTM::DimTriggerRates)))
                return;

            const FTM::DimTriggerRates &sdata = *static_cast<FTM::DimTriggerRates*>(curr->getData());

            if (GetCurrentState()==kStateSettingGlobalThreshold)
                ProcessCamera(sdata);

            if (GetCurrentState()==kStateInProgress)
                ProcessPatches(sdata);
        }
    }

    int Calibrate()
    {
        if (!fTriggerOn)
        {
            Info("Trigger not switched on... CALIBRATE command ignored.");
            return kStateGlobalThresholdSet;
        }

        const int32_t val[2] = { -1, fThresholdReference };
        Dim::SendCommand("FTM_CONTROL/SET_THRESHOLD", val);

        fThresholds.assign(160, fThresholdReference);

        fThresholdMin = fThresholdReference;
        fTriggerRate  = -1;
        fEnabled      = true;
        fCounter      = 0;

        ostringstream out;
        out << "Rate calibration started at a threshold of " << fThresholdReference << " with a target rate of " << fTargetRate << " Hz";
        Info(out);

        return kStateSettingGlobalThreshold;
    }

    int StartRC()
    {
        fEnabled = true;
        return GetCurrentState();
    }

    int StopRC()
    {
        fEnabled = false;
        return GetCurrentState();
    }

    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

        fThresholdReference = evt.GetUShort();

        return GetCurrentState();
    }

    int SetTargetRate(const EventImp &evt)
    {
        if (!CheckEventSize(evt.GetSize(), "SetTargetRate", 4))
            return kSM_FatalError;

        fTargetRate = evt.GetFloat();

        return GetCurrentState();
    }

    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(fStatusRS,  "RATE_SCAN");
        PrintState(fStatusFTM, "FTM_CONTROL");

        return GetCurrentState();
    }

    int SetVerbosity(const EventImp &evt)
    {
        if (!CheckEventSize(evt.GetSize(), "SetVerbosity", 1))
            return kSM_FatalError;

        fVerbose = evt.GetBool();

        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;

        if (GetCurrentState()==kStateSettingGlobalThreshold)
        {
            if (fTriggerRate<0 || fTriggerRate>fTargetRate)
                return kStateSettingGlobalThreshold;

            return kStateGlobalThresholdSet;
        }

        if (GetCurrentState()==kStateGlobalThresholdSet)
        {
            if (!fTriggerOn)
                return kStateGlobalThresholdSet;
            //return kStateInProgress;
        }

        // At least one subsystem is not connected
        //        if (fStatusFTM.second>=FTM::kConnected)
        return fTriggerOn && fEnabled && fStatusRS.second!=5 ? kStateInProgress : kStateConnected;
    }

public:
    StateMachineRateControl(ostream &out=cout) : StateMachineDim(out, "RATE_CONTROL"),
        fTriggerOn(false), fBlock(160),
        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),
        fRateScan("RATE_SCAN/STATE", (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")*/
    {
        // 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(kStateSettingGlobalThreshold, "Calibrating", "");
        AddStateName(kStateGlobalThresholdSet, "GlobalThresholdSet", "");

        AddStateName(kStateInProgress, "InProgress",
                     "Rate scan in progress.");

        AddEvent("CALIBRATE")
            (bind(&StateMachineRateControl::Calibrate, this))
            ("");

        AddEvent("START", "")
            (bind(&StateMachineRateControl::StartRC, this))
            ("");

        AddEvent("STOP", "")
            (bind(&StateMachineRateControl::StopRC, this))
            ("");

        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)
    {
        fVerbose = !conf.Get<bool>("quiet");

        fThresholdReference = 300;
        fTargetRate         =  75;

        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 control options");
    control.add_options()
        ("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);
}

/*
 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 ratecontrol program is a keep the rate reasonable low.\n"
        "\n"
        "Usage: ratecontrol [-c type] [OPTIONS]\n"
        "  or:  ratecontrol [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;
}