#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,
        kStateTempCtrlIdle,
        kStateFeedbackCtrlIdle,
        kStateTempCtrlRunning,
        kStateFeedbackCtrlRunning,
        kStateCalibrating,
    };

    enum control_t
    {
        kIdle,
        kTemp,
        kFeedback
    };

    control_t fControlType;

    PixelMap fMap;

    DimServiceInfoList fNetwork;

    pair<Time, int> fStatusDim;
    pair<Time, int> fStatusFAD;
    pair<Time, int> fStatusFSC;
    pair<Time, int> fStatusBias;

    DimStampedInfo fDim;
    DimStampedInfo fFAD;
    DimStampedInfo fFSC;
    DimStampedInfo fBias;
    DimStampedInfo fBiasA;

    DimStampedInfo fBiasData;
    DimStampedInfo fCameraTemp;

    DimDescribedService fDimReference;
    DimDescribedService fDimDeviation;
    DimDescribedService fDimCalibration;

    vector<int64_t>  fCurrentsAvg;
    vector<int64_t>  fCurrentsRms;

    vector<double>   fCalibration;

    vector<vector<float>> fData;

    uint64_t fCursor;

    Time fBiasLast;
    Time fStartTime;

    valarray<double> fPV[3];  // Process variable (intgerated/averaged amplitudes)
    valarray<double> fSP;     // Set point        (target amplitudes)

    double fKp;    // Proportional constant
    double fKi;    // Integral     constant
    double fKd;    // Derivative   constant
    double fT;     // Time         constant (cycle time)
    double fGain;  // Gain (conversion from a DRS voltage deviation into a BIAS voltage change at G-APD reference voltage)

    double fT21;

    double fBiasOffset;

    bool fOutputEnabled;

    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 = valarray<double>(0., 416);

        vector<float> vec(2*BIAS::kNumChannels);
        fDimDeviation.Update(vec);

        fPV[0].resize(0);
        fPV[1].resize(0);
        fPV[2].resize(0);

        if (fKp==0 && fKi==0 && fKd==0)
            Warn("Control loop parameters are all set to zero.");
    }

    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==&fFSC)
        {
            fStatusFSC = GetNewState(fFSC);
            return;
        }

        if (curr==&fDim)
        {
            fStatusDim = GetNewState(fDim);
            fStatusDim.second = curr->getSize()==4 ? curr->getInt() : 0;
            return;
        }

        if (curr==&fCameraTemp)
        {
            if (curr->getSize()!=60*sizeof(float))
                return;

            const float *ptr = static_cast<float*>(curr->getData());

            double avg = 0;
            int    num = 0;
            for (int i=1; i<32; i++)
                if (ptr[i]!=0)
                {
                    avg += ptr[i];
                    num++;
                }

            if (num==0)
                return;

            avg /= num;


            const float diff = (avg-25)*4./70 + fBiasOffset;

            vector<float> vec(2*BIAS::kNumChannels);
            for (int i=0; i<BIAS::kNumChannels; i++)
                vec[i+416] = diff;

            if (fControlType!=kTemp)
                return;

            fDimDeviation.Update(vec);

            if (fOutputEnabled && fStatusBias.second==BIAS::kVoltageOn)
            {
                Info("Sending correction to feedback.");

                DimClient::sendCommandNB((char*)"BIAS_CONTROL/SET_GAPD_REFERENCE_OFFSET",
                                         (void*)&diff, sizeof(float));
            }
        }

        if (curr==&fBiasA && fControlType==kTemp && GetCurrentState()==kStateCalibrating)
        {
            if (curr->getSize()!=416*sizeof(int16_t))
                return;

            if (fStatusBias.second==BIAS::kRamping)
                return;

            const int16_t *ptr = static_cast<int16_t*>(curr->getData());

            for (int i=0; i<416; i++)
                if (ptr[i]>0)
                {
                    fCurrentsAvg[i] += ptr[i];
                    fCurrentsRms[i] += ptr[i]*ptr[i];
                }

            if (++fCursor<100)
            {
                DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
                return;
            }

            fCalibration.resize(416);
            for (int i=0; i<416; i++)
            {
                fCalibration[i]     = double(fCurrentsAvg[i])/fCursor;
                fCalibration[i+416] = sqrt(double(fCurrentsRms[i])/fCursor-val[i]*val[i]);
            }

            fDimCalibration.Update(fCalibration);

            fOutputEnabled = false;
            fControlType = kIdle;

            DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
        }

        if (curr==&fBiasData && fControlType==kFeedback)
        {
            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);


            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 = fT>0 ? fT : (tm0-fStartTime).total_microseconds()/1000000.;
                const double T10 = fT21;
                fT21 = T21;

                fStartTime = tm0;

                ostringstream out;
                out << "New " << fData.size() << " event received: " << fCursor << " / " << setprecision(3) << T21 << "s";
                Info(out);

                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 << ":" << T21 << ")... " << endl;

                            // fKi[j] = response[j]*gain;
                            // Kp = 0;
                            // Kd = 0;

                            // => Kp = 0.01 * gain     = 0.00005
                            // => Ki = 0.8  * gain/20s = 0.00025
                            // => Kd = 0.1  * gain/20s = 0.00003

                            fKp = 0;
                            fKd = 0;
                            fKi = 0.00003*20;
                            T21 = 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])
                            // + dT * Ki * (SP    - PV[2])
                            // - Kd / dT * (PV[2] - 2*PV[1] + PV[0]);
                            //
                            // - (Kp+Kd/dT1) * (PV[2] - PV[1])
                            // + dT2 * Ki * (SP    - PV[2])
                            // + Kd / dT1 * (PV[1] - PV[0]);
                            //
                            // - Kp * (PV[2] - PV[1])
                            // + Ki * (SP    - PV[2])*dT
                            // - Kd * (PV[2] - PV[1])/dT
                            // + Kd * (PV[1] - PV[0])/dT;
                            //
                            //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 = fGain/1000*
                                (
                                 - (fKp+fKd/T21)*(fPV[2] - fPV[1])
                                 +  fKi*T21*(fSP-fPV[2])
                                 +  fKd/T10*(fPV[1]-fPV[0])
                                );

                            /*
                             integral = 0
                             start:
                                 integral += (fSP - fPV[2])*dt

                                 output = Kp*(fSP - fPV[2]) + Ki*integral - Kd*(fPV[2] - fPV[1])/dt

                                 wait(dt)

                                 goto start
                             */

                            vector<float> vec(2*BIAS::kNumChannels);
                            for (int i=0; i<BIAS::kNumChannels; i++)
                                vec[i] = fPV[2][i]-fSP[i];

                            for (int i=0; i<BIAS::kNumChannels; i++)
                                vec[i+416] = avg[i]<5*2.5 ? 0 : correction[i];

                            fDimDeviation.Update(vec);

                            if (fOutputEnabled && fStatusBias.second==BIAS::kVoltageOn)
                            {
                                Info("Sending correction to feedback.");

                                DimClient::sendCommandNB((char*)"BIAS_CONTROL/ADD_REFERENCE_VOLTAGES",
                                                         (void*)(vec.data()+416), 416*sizeof(float));

                                /*
                                if (!Dim::SendCommand("BIAS_CONTROL/ADD_REFERENCE_VOLTAGES",
                                                      (const void*)(vec.data()+416), 416*sizeof(float)))
                                {
                                    Error("Sending correction to bias control failed... switching off.");
                                    fOutputEnabled=false;
                                }
                                else
                                   Info("Success!");
                                */
                            }
                        }

            }
        }

    }

    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(fStatusFSC,  "FSC_CONTROL");
        PrintState(fStatusBias, "BIAS_CONTROL");

        return GetCurrentState();
    }

    int PrintCalibration()
    {
        if (fCalibration.size()==0)
        {
            Out() << "No calibration performed so far." << endl;
            return GetCurrentStatus();
        }

        for (int k=0; k<13; k++)
            for (int j=0; j<8; j++)
            {
                Out() << setw(2) << k << "|" << setw(2) << j*4 << "|";
                for (int i=0; i<4; i++)
                    Out() << Tools::Format(" %6.1f+-4.1f", fCalibration[i], fCalibration[i+416]);
                Out() << endl;
            }
        }

        return GetCurrentState();
    }

    int SetConstant(const EventImp &evt, int constant)
    {
        if (!CheckEventSize(evt.GetSize(), "SetConstant", 8))
            return kSM_FatalError;

        switch (constant)
        {
        case 0: fKi   = evt.GetDouble(); break;
        case 1: fKp   = evt.GetDouble(); break;
        case 2: fKd   = evt.GetDouble(); break;
        case 3: fT    = evt.GetDouble(); break;
        case 4: fGain = evt.GetDouble(); break;
        default:
            Fatal("SetConstant got an unexpected constant id -- this is a program bug!");
            return kSM_FatalError;
        }

        return GetCurrentState();
    }

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

        fOutputEnabled = evt.GetBool();

        return GetCurrentState();
    }

    int StartFeedback()
    {
        ResetData();

        fControlType = kFeedback;

        return GetCurrentState();
    }

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

        ostringstream out;
        out << "Starting temperature feedback with an offset of " << fBiasOffset << "V";
        Message(out);

        fBiasOffset = evt.GetFloat();
        fControlType = kTemp;

        return GetCurrentState();
    }

    int StopFeedback()
    {
        fControlType = kIdle;

        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 SetReference(const EventImp &evt)
    {
        if (!CheckEventSize(evt.GetSize(), "SetReference", 4))
            return kSM_FatalError;

        const float val = evt.GetFloat();
        /*
        if (!fPV[0].size() && !fPV[1].size() && !fPV[2].size())
        {
            Warn("No values in memory. Take enough events first!");
            return GetCurrentState();
        }*/

        vector<float> vec(BIAS::kNumChannels);
        for (int i=0; i<BIAS::kNumChannels; i++)
            vec[i] = fSP[i] = val;
        fDimReference.Update(vec);

        return GetCurrentState();
    }

    int CalibrateCurrents()
    {
//        if (!CheckEventSize(evt.GetSize(), "StartTempCtrl", 4))
//            return kSM_FatalError;

        ostringstream out;
        out << "Starting temperature feedback with an offset of -2V";
        Message(out);

        fBiasOffset = -2;
        fControlType = kTemp;
        fCursor = 0;
        fCurrentsAvg.assign(416, 0);
        fCurrentsRms.assign(416, 0);
        fCalibration.resize(0);
        fStartTime = Time();
        fOutputEnabled = true;

        return kStateCalibrating;
    }

    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 (fStatusFAD.second<FAD::kConnected &&
            fStatusBias.second<BIAS::kConnecting &&
            fStatusFSC.second<2)
            return kStateDisconnected;

        // At least one subsystem is not connected
        if (fStatusFAD.second<FAD::kConnected ||
            fStatusBias.second<BIAS::kConnected ||
            fStatusFSC.second<2)
            return kStateConnecting;

/*
        // All subsystems are connected
        if (GetCurrentStatus()==kStateConfiguringStep1)
        {
            if (fCursor<1)
                return kStateConfiguringStep1;

            if (fCursor==1)
            {
                fStartTime = Time();
                return kStateConfiguringStep2;
            }
        }
        if (GetCurrentStatus()==kStateConfiguringStep2)
        {
            if (fCursor==1)
            {
                if ((Time()-fStartTime).total_microseconds()/1000000.<1.5)
                    return kStateConfiguringStep2;

                Dim::SendCommand("BIAS_CONTROL/REQUEST_STATUS");
            }
            if (fCursor==2)
            {

                int n=0;
                double avg = 0;
                for (size_t i=0; i<fCurrents.size(); i++)
                    if (fCurrents[i]>=0)
                    {
                        avg += fCurrents[i];
                        n++;
                    }

                cout << avg/n << endl;
            }
            return kStateConnected;
        }
*/
        if (fControlType==kFeedback)
            return fOutputEnabled ? kStateFeedbackCtrlRunning : kStateFeedbackCtrlIdle;

        if (fControlType==kTemp)
        {
            if (GetCurrentState()==kStateCalibrating && fCursor<100)
                return GetCurrentState();

            return fOutputEnabled ? kStateTempCtrlRunning : kStateTempCtrlIdle;
        }

        return 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),
        fFSC("FSC_CONTROL/STATE",       (void*)NULL, 0, this),
        fBias("BIAS_CONTROL/STATE",     (void*)NULL, 0, this),
        fBiasA("BIAS_CONTROL/CURRENT",  (void*)NULL, 0, this),
        fBiasData("FAD_CONTROL/FEEDBACK_DATA", (void*)NULL, 0, this),
        fCameraTemp("FSC_CONTROL/TEMPERATURE", (void*)NULL, 0, this),
        fDimReference("FEEDBACK/REFERENCE", "F:416",        ""),
        fDimDeviation("FEEDBACK/DEVIATION", "F:416;F:416",  ""),
        fDimCalibration("FEEDBACK/CALIBRATION", "F:416;F:416",  ""),
        fSP(416),
        fKp(0), fKi(0), fKd(0), fT(-1), fOutputEnabled(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.

//        fSP.resize(416);

        // 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(kStateConnecting, "Connecting",
                     "Not all subsystems (FAD, FSC, Bias) are connected to their hardware.");

        AddStateName(kStateConnected, "Connected",
                     "All needed subsystems are connected to their hardware, no action is performed.");

        AddStateName(kStateFeedbackCtrlIdle, "FeedbackIdle",
                     "Feedback control activated, but voltage output disabled.");

        AddStateName(kStateTempCtrlIdle, "FeedbackIdle",
                     "Temperature control activated, but voltage output disabled.");

        AddStateName(kStateFeedbackCtrlRunning, "FeedbackControl",
                     "Feedback control activated and voltage output enabled.");

        AddStateName(kStateTempCtrlRunning, "TempControl",
                     "Temperature control activated and voltage output enabled.");

        AddStateName(kStateCalibrating, "Calibrating",
                     "Calibrating current offsets.");

        AddEvent("START_FEEDBACK_CONTROL", kStateConnected)
            (bind(&StateMachineFeedback::StartFeedback, this))
            ("Start the feedback control loop");

        AddEvent("START_TEMP_CONTROL", "F:1", kStateConnected)
            (bind(&StateMachineFeedback::StartTempCtrl, this, placeholders::_1))
            ("Start the temperature control loop"
             "|offset[V]:Offset from the nominal temperature corrected value in Volts");

        // kStateTempCtrlIdle, kStateFeedbackCtrlIdle, kStateTempCtrlRunning, kStateFeedbackCtrlRunning
        AddEvent("STOP")
            (bind(&StateMachineFeedback::StopFeedback, this))
            ("Stop any control loop");

        AddEvent("ENABLE_OUTPUT", "B:1")//, kStateIdle)
            (bind(&StateMachineFeedback::EnableOutput, this, placeholders::_1))
            ("Enable sending of correction values caluclated by the control loop to the biasctrl");

        AddEvent("STORE_REFERENCE")//, kStateIdle)
            (bind(&StateMachineFeedback::StoreReference, this))
            ("Store the last (averaged) value as new reference (for debug purpose only)");

        AddEvent("SET_REFERENCE", "F:1")//, kStateIdle)
            (bind(&StateMachineFeedback::SetReference, this, placeholders::_1))
            ("Set a new global reference value (for debug purpose only)");

        AddEvent("SET_Ki", "D:1")//, kStateIdle)
            (bind(&StateMachineFeedback::SetConstant, this, placeholders::_1, 0))
            ("Set integral constant Ki");

        AddEvent("SET_Kp", "D:1")//, kStateIdle)
            (bind(&StateMachineFeedback::SetConstant, this, placeholders::_1, 1))
            ("Set proportional constant Kp");

        AddEvent("SET_Kd", "D:1")//, kStateIdle)
            (bind(&StateMachineFeedback::SetConstant, this, placeholders::_1, 2))
            ("Set derivative constant Kd");

        AddEvent("SET_T", "D:1")//, kStateIdle)
            (bind(&StateMachineFeedback::SetConstant, this, placeholders::_1, 3))
            ("Set time-constant. (-1 to use the cycle time, i.e. the time for the last average cycle, instead)");

        AddEvent("CALIBRATE_CURRENTS", kStateConnected)//, kStateIdle)
            (bind(&StateMachineFeedback::CalibrateCurrents, this))
            ("");

        // 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))
            ("");

        AddEvent("PRINT_CALIBRATION")
            (bind(&StateMachineFeedback::PrintCalibration, this))
            ("");
    }

    int EvalOptions(Configuration &conf)
    {
        if (!fMap.Read(conf.Get<string>("pixel-map-file")))
        {
            Error("Reading mapping table from "+conf.Get<string>("pixel-map-file")+" failed.");
            return 1;
        }

        //                    -110 / -110 (-23 DAC / -0.51V)
        // Reference voltage: -238 / -203
        //                    -360 / -343 ( 23 DAC /  0.51V)

        // 0.005 A/V
        // 220 Amplitude / 1V

        // Gain = 1V / 200 = 0.005

        fGain = 5; // (BIAS)V / (DRS)V     ( 1V / 0.22V )

        fKp = 0;
        fKd = 0;
        fKi = 0.12;
        fT  = 1;

        ostringstream msg;
        msg << "Control loop parameters: ";
        msg << "Kp=" << fKp << ", Kd=" << fKd << ", Ki=" << fKi << ", ";
        if (fT>0)
            msg << fT;
        else
            msg << "<auto>";
        msg << ", Gain(BIAS/DRS)=" << fGain << "V/V";

        Message(msg);

        return -1;
    }
};

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

#include "Main.h"

template<class T>
int RunShell(Configuration &conf)
{
    return Main::execute<T, StateMachineFeedback>(conf);
}

void SetupConfiguration(Configuration &conf)
{
    po::options_description control("Feedback options");
    control.add_options()
        ("pixel-map-file",  var<string>("FACTmapV5a.txt"), "Pixel mapping file. Used here to get the default reference voltage.")
        ;

    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 feedback control the BIAS voltages based on the calibration signal.\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: feedback [-c type] [OPTIONS]\n"
        "  or:  feedback [OPTIONS]\n";
    cout << endl;
}

void PrintHelp()
{
    Main::PrintHelp<StateMachineFeedback>();

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