Index: /trunk/FACT++/src/feedback.cc
===================================================================
--- /trunk/FACT++/src/feedback.cc	(revision 12028)
+++ /trunk/FACT++/src/feedback.cc	(revision 12028)
@@ -0,0 +1,653 @@
+#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 "HeadersFAD.h"
+#include "HeadersBIAS.h"
+
+namespace ba    = boost::asio;
+namespace bs    = boost::system;
+namespace dummy = ba::placeholders;
+
+using namespace std;
+
+// ------------------------------------------------------------------------
+
+#include "DimDescriptionService.h"
+
+// ------------------------------------------------------------------------
+
+class StateMachineFeedback : public StateMachineDim, public DimInfoHandler
+{
+    /*
+    int Wrap(boost::function<void()> f)
+    {
+        f();
+        return T::GetCurrentState();
+    }
+
+    boost::function<int(const EventImp &)> Wrapper(boost::function<void()> func)
+    {
+        return bind(&StateMachineMCP::Wrap, this, func);
+    }*/
+
+private:
+    enum states_t
+    {
+        kStateDimNetworkNA = 1,
+        kStateDisconnected,
+        kStateConnecting,
+        kStateConnected,
+        kStateRunning,
+    };
+
+    PixelMap fMap;
+
+    DimServiceInfoList fNetwork;
+
+    pair<Time, int> fStatusDim;
+    pair<Time, int> fStatusFAD;
+    pair<Time, int> fStatusBias;
+
+    DimStampedInfo fDim;
+    DimStampedInfo fFAD;
+    DimStampedInfo fBias;
+    DimStampedInfo *fBiasData;
+
+    DimDescribedService fDimReference;
+    DimDescribedService fDimDeviation;
+
+    vector<vector<float>> fData;
+
+    int fCursor;
+
+    Time fBiasLast;
+    Time fStartTime;
+
+    valarray<double> fPV[3];  // Process variable (intgerated/averaged amplitudes)
+    valarray<double> fSP;     // Set point        (target amplitudes)
+
+    double fT;
+    double fKp;               // Proportional constant
+    double fKi;               // Integral     constant
+    double fKd;               // Derivative   constant
+
+    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());
+    }
+
+    void ResetData()
+    {
+        fData.clear();
+        fData.resize(500);
+        fCursor = 0;
+        fStartTime = Time();
+
+        fSP.resize(416);
+        fSP = valarray<double>(0., 416);
+
+        fPV[0].resize(0);
+        fPV[1].resize(0);
+        fPV[2].resize(0);
+    }
+
+    void infoHandler()
+    {
+        DimInfo *curr = getInfo(); // get current DimInfo address
+        if (!curr)
+            return;
+
+        if (curr==&fBias)
+        {
+            fStatusBias = GetNewState(fBias);
+            return;
+        }
+
+        if (curr==&fFAD)
+        {
+            fStatusFAD = GetNewState(fFAD);
+            return;
+        }
+
+        if (curr==&fDim)
+        {
+            fStatusDim = GetNewState(fDim);
+            fStatusDim.second = curr->getSize()==4 ? curr->getInt() : 0;
+            return;
+        }
+
+        if (curr==fBiasData)
+        {
+            if (curr->getSize()==0)
+                return;
+            if (curr->getSize()!=1440*sizeof(float))
+                return;
+
+            // -------- Check age of last stored event --------
+
+            // Must be called in this order
+            const int tsec = curr->getTimestamp();
+            const int tms  = curr->getTimestampMillisecs();
+
+            const Time tm(tsec, tms*1000);
+
+            if (Time()-fBiasLast>boost::posix_time::seconds(30))
+            {
+                Warn("Last received event data older than 30s... resetting average calculation.");
+                ResetData();
+            }
+            fBiasLast = tm;
+
+            // -------- Store new event --------
+
+            fData[fCursor%fData.size()].assign(reinterpret_cast<float*>(curr->getData()),
+                                               reinterpret_cast<float*>(curr->getData())+1440);
+
+
+            ostringstream out;
+            out << "New event received: " << fCursor;
+            Info(out);
+
+            fCursor++;
+
+            if (fCursor<fData.size())
+                return;
+
+            // -------- Calculate statistics --------
+
+            valarray<double> med(1440);
+
+            for (int ch=0; ch<1440; ch++)
+            {
+                vector<float> arr(fData.size());
+                for (size_t i=0; i<fData.size(); i++)
+                    arr[i] = fData[i][ch];
+
+                sort(arr.begin(), arr.end());
+
+                med[ch] = arr[arr.size()/2];
+            }
+
+            /*
+            vector<float> med(1440);
+            vector<float> rms(1440);
+            for (size_t i=0; i<fData.size(); i++)
+            {
+                if (fData[i].size()==0)
+                    return;
+
+                for (int j=0; j<1440; j++)
+                {
+                    med[j] += fData[i][j];
+                    rms[j] += fData[i][j]*fData[i][j];
+                }
+            }
+            */
+
+            vector<double> avg(BIAS::kNumChannels);
+            vector<int>    num(BIAS::kNumChannels);
+            for (int i=0; i<1440; i++)
+            {
+                const PixelMapEntry &ch = fMap.hw(i);
+
+                // FIXME: Add a consistency check if the median makes sense...
+                // FIXME: Add a consistency check to remove pixels with bright stars (median?)
+
+                avg[ch.hv()] += med[i];
+                num[ch.hv()]++;
+            }
+
+            for (int i=0; i<BIAS::kNumChannels; i++)
+            {
+                if (num[i])
+                    avg[i] /= num[i];
+
+            }
+
+            // -------- Calculate correction --------
+
+            // http://bestune.50megs.com/typeABC.htm
+
+            // CO: Controller output
+            // PV: Process variable
+            // SP: Set point
+            // T:  Sampling period (loop update period)
+            // e = SP - PV
+            //
+            // Kp : No units
+            // Ki : per seconds
+            // Kd : seconds
+
+            // CO(k)-CO(k-1) = - Kp[ PV(k) - PV(k-1) ] + Ki * T * (SP(k)-PV(k)) - Kd/T [ PV(k) - 2PV(k-1) + PV(k-2) ]
+
+            if (fCursor%fData.size()==0)
+            {
+                // FIXME: Take out broken / dead boards.
+
+                const Time tm0 = Time();
+
+                const double T21 = (tm0-fStartTime).total_microseconds()/1000000.;
+                const double T10 = fT;
+                fT = T21;
+
+                fStartTime = tm0;
+
+                cout << "Step..." << endl;
+
+                if (fPV[0].size()==0)
+                {
+                    fPV[0].resize(avg.size());
+                    fPV[0] = valarray<double>(avg.data(), avg.size());
+                }
+                else
+                    if (fPV[1].size()==0)
+                    {
+                        fPV[1].resize(avg.size());
+                        fPV[1] = valarray<double>(avg.data(), avg.size());
+                    }
+                    else
+                        if (fPV[2].size()==0)
+                        {
+                            fPV[2].resize(avg.size());
+                            fPV[2] = valarray<double>(avg.data(), avg.size());
+                        }
+                        else
+                        {
+                            fPV[0] = fPV[1];
+                            fPV[1] = fPV[2];
+
+                            fPV[2].resize(avg.size());
+                            fPV[2] = valarray<double>(avg.data(), avg.size());
+
+                            if (T10<=0 || T21<=0)
+                                return;
+
+                            cout << "Calculating (" << fCursor << ")... " << endl;
+
+                            // fKi[j] = response[j]*gain;
+                            // Kp = 0;
+                            // Kd = 0;
+
+                            //                                                                                  (PV[2]-PV[1] + PV[0]-PV[1])
+                            //valarray<double> correction = - Kp*(PV[2] - PV[1]) + Ki * dT * (SP-PV[2]) - Kd/dT * (PV[2] - 2*PV[1] + PV[0]);
+                            //valarray<double> correction =
+                            //    - Kp*(PV[2] - PV[1]) + Ki * T21 * (SP-PV[2]) - Kd*(PV[2]-PV[1])/T21 - Kd*(PV[0]-PV[1])/T01;
+                            const valarray<double> correction =
+                                - (fKp+fKd/T21)*(fPV[2] - fPV[1]) + fKi*T21*(fSP-fPV[2]) + fKd/T10*(fPV[1]-fPV[0]);
+
+                            vector<float> vec(2*BIAS::kNumChannels);
+                            for (int i=0; i<BIAS::kNumChannels; i++)
+                                vec[i] = fSP[i]-fPV[2][i];
+
+                            for (int i=0; i<BIAS::kNumChannels; i++)
+                                vec[i+416] = correction[i];
+
+                            fDimDeviation.Update(vec);
+                        }
+
+            }
+
+            /*
+            vector<float> correction(416);
+            vector<float> response(416);
+            vector<float> dev(416);
+
+            double gain = 0;
+
+            for (int j=0; j<416; j++)
+            {
+                //avg[j] /= fData.size();
+                //rms[j] /= sqrt((rms[j]*fData.size() - avg[j]*avg[j]))/fData.size();
+
+                dev[j] = avg[j] - target[j];
+
+                // Determine correction from response maxtrix and change voltages
+                correction[j] = -dev[j]*response[j]*gain;
+
+                if (correction[j]==0)
+                    continue;
+
+                // Limit voltage steps // Limit changes to 100 mV
+//                if (fabs(correction[j]) > 0.1)
+//                    correction[j] = 0.1*fabs(correction[j])/correction[j];
+
+                // Add voltage change command if not too noisy
+//                if (fabs(Average[i]) > 2*Sigma[i])
+//                    Cmd << fIDTable[i] << " " << std::showpos << Correction/Multiplicity[i] << " ";
+
+            }
+            */
+            return;
+        }
+
+    }
+
+    bool CheckEventSize(size_t has, const char *name, size_t size)
+    {
+        if (has==size)
+            return true;
+
+        ostringstream msg;
+        msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
+        Fatal(msg);
+        return false;
+    }
+
+    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(fStatusFAD,  "FAD_CONTROL");
+        PrintState(fStatusBias, "BIAS_CONTROL");
+
+        return GetCurrentState();
+    }
+
+    int SetConstant(const EventImp &evt, int constant)
+    {
+        if (!CheckEventSize(evt.GetSize(), "SetConstant", 8))
+            return T::kSM_FatalError;
+
+        switch (constant)
+        {
+        case 0: fKi = evt.GetDouble(); break,
+        case 1: fKp = evt.GetDouble(); break;
+        case 2: fKd = evt.GetDouble(); break;
+        default:
+            Fatal("SetConstant got an unexpected constant id -- this is a program bug!");
+        }
+
+        return GetCurrentState();
+    }
+
+    int StartFeedback()
+    {
+        fBiasData = new DimStampedInfo("FAD_CONTROL/FEEDBACK_DATA", (void*)NULL, 0, this);
+
+        ResetData();
+
+        return GetCurrentState();
+    }
+
+    int StopFeedback()
+    {
+        delete fBiasData;
+        fBiasData = 0;
+
+        return GetCurrentState();
+    }
+
+    int StoreReference()
+    {
+        if (!fPV[0].size() && !fPV[1].size() && !fPV[2].size())
+        {
+            Warn("No values in memory. Take enough events first!");
+            return GetCurrentState();
+        }
+
+        // FIXME: Check age
+
+        if (!fPV[1].size() && !fPV[2].size())
+            fSP = fPV[0];
+
+        if (!fPV[2].size())
+            fSP = fPV[1];
+        else
+            fSP = fPV[2];
+
+        vector<float> vec(BIAS::kNumChannels);
+        for (int i=0; i<BIAS::kNumChannels; i++)
+            vec[i] = fSP[i];
+        fDimReference.Update(vec);
+
+        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;
+
+        if (fStatusFAD.second<FAD::kConnected && fStatusBias.second<BIAS::kConnecting)
+            return kStateDisconnected;
+
+        if (fStatusFAD.second<FAD::kConnected && fStatusBias.second<BIAS::kConnected)
+            return kStateConnecting;
+
+        return fBiasData ? kStateRunning : kStateConnected;
+    }
+
+public:
+    StateMachineFeedback(ostream &out=cout) : StateMachineDim(out, "FEEDBACK"),
+        fStatusDim(make_pair(Time(), -2)),
+        fStatusFAD(make_pair(Time(), -2)),
+        fStatusBias(make_pair(Time(), -2)),
+        fDim("DIS_DNS/VERSION_NUMBER",  (void*)NULL, 0, this),
+        fFAD("FAD_CONTROL/STATE",       (void*)NULL, 0, this),
+        fBias("BIAS_CONTROL/STATE",     (void*)NULL, 0, this),
+        fDimReference("FEEDBACK/REFERENCE", "F:416",        ""),
+        fDimDeviation("FEEDBACK/DEVIATION", "F:416;F:416",  ""),
+        fBiasData(NULL)
+    {
+        // 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",
+                     ".");
+
+        AddStateName(kStateDisconnected, "Disconnected",
+                     ".");
+
+        AddStateName(kStateConnecting, "Connecting",
+                     ".");
+
+        AddStateName(kStateConnected, "Connected",
+                     ".");
+
+        AddStateName(kStateRunning, "Running",
+                     ".");
+
+        AddEvent("START")//, kStateIdle)
+            (bind(&StateMachineFeedback::StartFeedback, this))
+            ("");
+
+        AddEvent("STOP")//, kStateIdle)
+            (bind(&StateMachineFeedback::StopFeedback, this))
+            ("");
+
+        AddEvent("STORE_REFERENCE")//, kStateIdle)
+            (bind(&StateMachineFeedback::StoreReference, this))
+            ("Stored the last (averaged) value as new reference (for debug purpose only)");
+
+        AddEvent("SET_Ki", "D:1")//, kStateIdle)
+            (bind(&StateMachineFeedback::SetConstant, this, 0))
+            ("Set integral constant Ki");
+
+        AddEvent("SET_Kp", "D:1")//, kStateIdle)
+            (bind(&StateMachineFeedback::SetConstant, this, 1))
+            ("Set proportional constant Kp");
+
+        AddEvent("SET_Kd", "D:1")//, kStateIdle)
+            (bind(&StateMachineFeedback::SetConstant, this, 2))
+            ("Set derivative constant Kd");
+
+        // Verbosity commands
+//        AddEvent("SET_VERBOSE", "B:1")
+//            (bind(&StateMachineMCP::SetVerbosity, this, placeholders::_1))
+//            ("set verbosity state"
+//             "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");
+
+        AddEvent("PRINT")
+            (bind(&StateMachineFeedback::Print, this))
+            ("");
+    }
+
+    int EvalOptions(Configuration &)
+    {
+        //SetEndpoint(conf.Get<string>("addr"));
+
+        //fFSC.SetVerbose(!conf.Get<bool>("quiet"));
+
+        if (!fMap.Read("FACTmapV5.txt"))
+        {
+            Error("Reading mapping table from FACTmapV5.txt failed.");
+            return 1;
+        }
+
+        /*
+        if (!fBias.SetNewGapdVoltage(map.Vgapd()))
+        {
+            T::Error("Setting reference voltages failed.");
+            return 6;
+        }*/
+
+        return -1;
+    }
+};
+
+// ------------------------------------------------------------------------
+
+#include "Main.h"
+
+template<class T>
+int RunShell(Configuration &conf)
+{
+    return Main::execute<T, StateMachineFeedback>(conf);
+}
+
+/*
+ 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 ftmctrl controls the FSC (FACT Slow Control) board.\n"
+        "\n"
+        "The default is that the program is started without user intercation. "
+        "All actions are supposed to arrive as DimCommands. Using the -c "
+        "option, a local shell can be initialized. With h or help a short "
+        "help message about the usuage can be brought to the screen.\n"
+        "\n"
+        "Usage: fscctrl [-c type] [OPTIONS]\n"
+        "  or:  fscctrl [OPTIONS]\n";
+    cout << endl;
+}
+
+void PrintHelp()
+{
+    /* 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);
+
+    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;
+}