#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,
        kStateConnectedFSC,
        kStateConnectedFAD,
        kStateConnected,
        kStateTempCtrlIdle,      // 7
        kStateFeedbackCtrlIdle,  // 8
        kStateCurrentCtrlIdle,  // 9
        kStateTempCtrlRunning,  // 9->10
        kStateFeedbackCtrlRunning, // 10->11
        kStateCurrentCtrlRunning,  // 12
        kStateCalibrating,         // 11->13
    };

    enum control_t
    {
        kIdle,
        kTemp,
        kFeedback,
        kFeedbackGlobal,
        kCurrents,
    };

    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<float>    fCalibration;

    vector<vector<float>> fData;

     int64_t fCursorCur;
    uint64_t fCursorAmpl;
    uint64_t fCursorTemp;

    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;

    uint16_t fCurrentRequestInterval;
    uint16_t fNumCalibIgnore;
    uint16_t fNumCalibRequests;

    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 HandleCameraTemp()
    {
        if (fCameraTemp.getSize()!=60*sizeof(float))
            return;

        const float *ptr = static_cast<float*>(fCameraTemp.getData());

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

        if (numt==0)
            return;

        avgt /= numt;


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

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

        double avg[2] = {   0,   0 };
        double min[2] = {  90,  90 };
        double max[2] = { -90, -90 };
        int    num[2] = {   0,   0 };

        vector<double> med[2];
        med[0].resize(416);
        med[1].resize(416);

        if (fControlType==kCurrents)
        {
            if (fCursorCur==0)
            {
                //DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
                return;
            }

            // Pixel  583: 5 31 == 191 (5)
            // Pixel  830: 2  2 ==  66 (4)
            // Pixel 1401: 6  1 == 193 (5)

            // Convert from DAC counts to uA
            const double conv = 5000e-6/4096;

            // 3900 Ohm/n + 1000 Ohm + 1100 Ohm  (with n=4 or n=5)
            const double R[2] = { 3075, 2870 };

            for (int i=0; i<BIAS::kNumChannels; i++)
            {
                const PixelMapEntry &hv = fMap.hv(i);
                if (!hv)
                    continue;

                const int g = hv.group();

                const double Im = double(fCurrentsAvg[i])/fCursorCur;
                const double I  = Im>fCalibration[i] ? Im-fCalibration[i] : 0;

                double U  = R[g] * I*conv;

                // 510 / 390    1.30 ^1.66 =  1.55
                // 470 / 380    1.23       =  1.41
                // 450 / 360    1.25       =  1.45

                // This is assuming that the broken pixels
                // have a 1kOhm instead of 390 Ohm serial resistor
                if (i==66)
                    U *= 2400./R[0];    // (1k)2665 / (390)2400 / (~0)2100
                if (i==191)
                    U *= 2320./R[1];    // (1k)2794 / (390)2320 / (~0)2100
                if (i==193)
                    U *= 2320./R[1];    // (1k)2665 / (390)2400 / (~0)2100

                vec[i+BIAS::kNumChannels] += U;

                if (fCalibration[i]>0)
                {
                    med[g][num[g]] = U;
                    avg[g] += U;
                    num[g]++;

                    if (U<min[g])
                        min[g] = U;
                    if (U>max[g])
                        max[g] = U;
                }
            }

            sort(med[0].begin(), med[0].begin()+num[0]);
            sort(med[1].begin(), med[1].begin()+num[1]);

            fCurrentsAvg.assign(BIAS::kNumChannels, 0);
            fCursorCur = 0;
        }

        fDimDeviation.setQuality(fControlType);
        fDimDeviation.Update(vec);

        if (!fOutputEnabled || fStatusBias.second!=BIAS::kVoltageOn)
            return;

        // Trigger calibration
        if (GetCurrentState()==kStateCalibrating && fCursorTemp==1)
        {
            DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
            return;
        }

        ostringstream msg;
        msg << setprecision(4) << "Sending new absolute offset (" << diff << "V+" << (num[0]+num[1]>0?(avg[0]+avg[1])/(num[0]+num[1]):0) << "V) to biasctrl.";
        Info(msg);

        if (fControlType==kCurrents && num[0]>0 && num[1]>0)
        {
            msg.str("");
            msg << "   Avg0=" << setw(7) << avg[0]/num[0]    << "  |  Avg1=" << setw(7) << avg[1]/num[1];
            Debug(msg);

            msg.str("");
            msg << "   Med0=" << setw(7) << med[0][num[0]/2] << "  |  Med1=" << setw(7) << med[1][num[1]/2];
            Debug(msg);

            msg.str("");
            msg << "   Min0=" << setw(7) << min[0]           << "  |  Min1=" << setw(7) << min[1];
            Debug(msg);

            msg.str("");
            msg << "   Max0=" << setw(7) << max[0]           << "  |  Max1=" << setw(7) << max[1];
            Debug(msg);
        }

        DimClient::sendCommandNB("BIAS_CONTROL/SET_ALL_CHANNELS_OFFSET",
                                 vec.data()+BIAS::kNumChannels, BIAS::kNumChannels*sizeof(float));

        fCursorTemp++;
    }

    int AverageCurrents()
    {
        if (fBiasA.getSize()!=BIAS::kNumChannels*sizeof(int16_t))
            return -1;

        if (fStatusBias.second!=BIAS::kVoltageOn)
            return false;

        if (fCursorCur++<0)
            return true;

        const int16_t *ptr = static_cast<int16_t*>(fBiasA.getData());

        for (int i=0; i<BIAS::kNumChannels; i++)
        {
            fCurrentsAvg[i] += ptr[i];
            fCurrentsRms[i] += ptr[i]*ptr[i];
        }

        return true;
    }


    void HandleCalibration()
    {
        const int rc = AverageCurrents();
        if (rc<0)
            return;

        if (fCursorCur<fNumCalibRequests)
        {
            if (fStatusBias.second==BIAS::kVoltageOn)
                DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
            return;
        }

        if (rc==0)
            return;

        fCalibration.resize(BIAS::kNumChannels*2);
        for (int i=0; i<BIAS::kNumChannels; i++)
        {
            fCalibration[i]                    = double(fCurrentsAvg[i])/fCursorCur;
            fCalibration[i+BIAS::kNumChannels] = sqrt(double(fCurrentsRms[i])/fCursorCur-fCalibration[i]*fCalibration[i]);
        }

        fDimCalibration.Update(fCalibration);

        fOutputEnabled = false;
        fControlType = kIdle;

        Info("Calibration successfully done.");

        if (fStatusBias.second==BIAS::kVoltageOn)
            DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
    }

    void HandleFeedback()
    {
        if (fBiasData.getSize()!=1440*sizeof(float))
            return;

        // -------- Check age of last stored event --------

        // Must be called in this order
        const int tsec = fBiasData.getTimestamp();
        const int tms  = fBiasData.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[fCursorAmpl%fData.size()].assign(reinterpret_cast<float*>(fBiasData.getData()),
                                               reinterpret_cast<float*>(fBiasData.getData())+1440);

        if (++fCursorAmpl<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 (fCursorAmpl%fData.size()>0)
            return;

        // 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: " << fCursorAmpl << " / " << setprecision(3) << T21 << "s";
        Info(out);

        if (fPV[0].size()==0)
        {
            fPV[0].resize(avg.size());
            fPV[0] = valarray<double>(avg.data(), avg.size());
            return;
        }

        if (fPV[1].size()==0)
        {
            fPV[1].resize(avg.size());
            fPV[1] = valarray<double>(avg.data(), avg.size());
            return;
        }

        if (fPV[2].size()==0)
        {
            fPV[2].resize(avg.size());
            fPV[2] = valarray<double>(avg.data(), avg.size());
            return;
        }

        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 = 1./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+BIAS::kNumChannels] = avg[i]<5*2.5 ? 0 : correction[i];

        fDimDeviation.setQuality(fControlType);
        fDimDeviation.Update(vec);

        if (!fOutputEnabled || fStatusBias.second!=BIAS::kVoltageOn)
            return;

        Info("Sending new relative offset to biasctrl.");

        DimClient::sendCommandNB("BIAS_CONTROL/INCREASE_ALL_CHANNELS_VOLTAGE",
                                 vec.data()+BIAS::kNumChannels, BIAS::kNumChannels*sizeof(float));
    }

    void HandleGlobalFeedback()
    {
        if (fBiasData.getSize()!=1440*sizeof(float))
            return;

        // -------- Store new event --------

        vector<float> arr(reinterpret_cast<float*>(fBiasData.getData()),
                          reinterpret_cast<float*>(fBiasData.getData())+1440);

        sort(arr.begin(), arr.end());

        const float med = arr[arr.size()/2];

        fData[fCursorAmpl%fData.size()].resize(1); //assign(&med, &med);
        fData[fCursorAmpl%fData.size()][0] = med;  //assign(&med, &med);

        if (++fCursorAmpl<fData.size())
            return;

        // -------- Calculate statistics --------

        double avg=0;
        double rms=0;
        for (size_t i=0; i<fData.size(); i++)
        {
            avg += fData[i][0];
            rms += fData[i][0]*fData[i][0];
        }

        avg /= fData.size();
        rms /= fData.size();

        rms  = sqrt(rms-avg*avg);

        // -------- Calculate correction --------

        if (fCursorAmpl%fData.size()!=0)
            return;

        Out() << "Amplitude: " << avg << " +- " << rms << endl;

        // FIXME: Take out broken / dead boards.

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

        if (fPV[0].size()==0)
        {
            fPV[0].resize(1);
            fPV[0] = valarray<double>(&avg, 1);
            return;
        }

        if (fPV[1].size()==0)
        {
            fPV[1].resize(1);
            fPV[1] = valarray<double>(&avg, 1);
            return;
        }

        if (fPV[2].size()==0)
        {
            fPV[2].resize(1);
            fPV[2] = valarray<double>(&avg, 1);
            return;
        }

        fPV[0] = fPV[1];
        fPV[1] = fPV[2];

        fPV[2].resize(1);
        fPV[2] = valarray<double>(&avg, 1);

        // ----- Calculate average currents -----

        vector<float> A(BIAS::kNumChannels);
        for (int i=0; i<BIAS::kNumChannels; i++)
            A[i] = double(fCurrentsAvg[i]) / fCursorCur;

        fCurrentsAvg.assign(BIAS::kNumChannels, 0);
        fCursorCur = 0;

        // -------- Calculate correction --------

        // correction = (fSP[0]-fPV[2])*fKi
        /*
        const double T21 = 1; // feedback is  1s
        const double T10 = 1; // feedback is 20s

        const valarray<double> correction = 1./fGain/1000*
            (
             - (fKp+fKd/T21)*(fPV[2] - fPV[1])
             +  fKi*T21*(fSP[0]-fPV[2])
             +  fKd/T10*(fPV[1]-fPV[0])
            );
        */

        // pow of 1.6 comes from the non-linearity of the
        // amplitude vs bias voltage
        const valarray<double> correction = 1./fGain/1000*
            (
             //fKi*(pow(fSP[0], 1./1.6)-pow(fPV[2], 1./1.6))
             fKi*(fSP[0]-fPV[2])
            );

        Out() << "Correction: " << correction[0] << "V (" << fSP[0] << ")" << endl;

        const int nch = BIAS::kNumChannels;

        // FIXME: Sanity check!

        vector<float> vec;
        vec.reserve(2*nch);
        vec.insert(vec.begin(),     nch, fPV[2][0]-fSP[0]);
        vec.insert(vec.begin()+nch, nch, correction[0]);

        fDimDeviation.setQuality(fControlType);
        fDimDeviation.Update(vec);

        if (!fOutputEnabled || fStatusBias.second!=BIAS::kVoltageOn)
            return;

        Info("Sending new global relative offset to biasctrl.");

        DimClient::sendCommandNB("BIAS_CONTROL/INCREASE_ALL_CHANNELS_VOLTAGE",
                                 vec.data()+BIAS::kNumChannels, BIAS::kNumChannels*sizeof(float));
    }

    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 && (fControlType==kTemp || fControlType==kCurrents))
            HandleCameraTemp();

        if (curr==&fBiasA && fControlType==kTemp && GetCurrentState()==kStateCalibrating)
            HandleCalibration();

        if (curr==&fBiasA && (fControlType==kFeedbackGlobal || fControlType==kCurrents))
            AverageCurrents();

        if (curr==&fBiasData && fControlType==kFeedback)
            HandleFeedback();

        if (curr==&fBiasData && fControlType==kFeedbackGlobal)
            HandleGlobalFeedback();
    }

    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 GetCurrentState();
        }

        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::Form(" %6.1f+-%4.1f", fCalibration[k*32+j*4+i], fCalibration[k*32+j*4+i+BIAS::kNumChannels]);
                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();
    }

    void ResetData(int16_t n=-1)
    {
        fData.assign(n>0 ? n : fData.size(), vector<float>(0));

        fCursorAmpl = 0;
        fCursorCur  = 0;
        fCursorTemp = 0;

        fStartTime = Time();

        fSP = valarray<double>(0., BIAS::kNumChannels);

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

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

        fCurrentsAvg.assign(BIAS::kNumChannels, 0);
        fCurrentsRms.assign(BIAS::kNumChannels, 0);

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

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

        ResetData(evt.GetShort());

        fControlType = kFeedback;

        return GetCurrentState();
    }

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

        ResetData(evt.GetShort());

        fControlType = kFeedbackGlobal;

        return GetCurrentState();
    }

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

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

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

        if (fStatusBias.second==BIAS::kVoltageOn)
            DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);

        return GetCurrentState();
    }

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

        if (fCalibration.size()==0)
        {
            Warn("Current control needs a bias crate calibration first... command ignored.");
            return GetCurrentState();
        }

        ResetData(0);

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

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

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

        Out() << "New global reference value: " << val << "mV" << endl;

        return GetCurrentState();
    }

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

        if (fStatusBias.second==BIAS::kRamping)
        {
            Warn("Calibration cannot be started when biasctrl is in state Ramping.");
            return GetCurrentState();
        }

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

        fBiasOffset = -2;
        fControlType = kTemp;
        fCursorCur  = -fNumCalibIgnore;
        fCursorTemp = 0;
        fCurrentsAvg.assign(BIAS::kNumChannels, 0);
        fCurrentsRms.assign(BIAS::kNumChannels, 0);
        fCalibration.resize(0);
        fStartTime = Time();
        fOutputEnabled = true;

        return kStateCalibrating;
    }

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

        fCurrentRequestInterval = evt.GetUShort();

        Out() << "New current request interval: " << fCurrentRequestInterval << "ms" << endl;

        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;

        const bool bias = fStatusBias.second >= BIAS::kConnecting;
        const bool fad  = fStatusFAD.second  >= FAD::kConnected;
        const bool fsc  = fStatusFSC.second  >= 2;

        // All subsystems are not connected
        if (!bias && !fad && !fsc)
            return kStateDisconnected;

        // At least one subsystem apart from bias is connected
        if (bias && !fad && !fsc)
            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;
        }
        */

        // Needs connection of FAD and BIAS
        if (bias && fad)
        {
            if (fControlType==kFeedback || fControlType==kFeedbackGlobal)
                return fOutputEnabled ? kStateFeedbackCtrlRunning : kStateFeedbackCtrlIdle;
        }

        // Needs connection of FSC and BIAS
        if (bias && fsc)
        {
            if (fControlType==kTemp)
            {
                if (GetCurrentState()==kStateCalibrating && fCursorCur<fNumCalibRequests)
                    return GetCurrentState();

                return fOutputEnabled ? kStateTempCtrlRunning : kStateTempCtrlIdle;
            }
            if (fControlType==kCurrents)
            {
                static Time past;
                if (fCurrentRequestInterval>0 && Time()-past>boost::posix_time::milliseconds(fCurrentRequestInterval))
                {
                    if (fStatusBias.second==BIAS::kVoltageOn)
                        DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
                    past = Time();
                }

                return fOutputEnabled ? kStateCurrentCtrlRunning : kStateCurrentCtrlIdle;
            }
        }

        if (bias && fad && !fsc)
            return kStateConnectedFAD;

        if (bias && fsc && !fad)
            return kStateConnectedFSC;

        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",
                      "Amplitude reference value(s)"
                      "Vref[mV]:Amplitude reference"),
        fDimDeviation("FEEDBACK/DEVIATION", "F:416;F:416",
                      "Control loop information"
                      "|DeltaAmpl[mV]:Amplitude offset measures"
                      "|DeltaBias[mV]:Correction value calculated"),
        fDimCalibration("FEEDBACK/CALIBRATION", "F:416;F:416",
                        "Current offsets"
                        "|Avg[dac]:Average offset (5000uA/4096dac)"
                        "|Rms[dac]:Rms of offset (5000uA/4096dac)"),
        fSP(BIAS::kNumChannels),
        fKp(0), fKi(0), fKd(0), fT(-1),
        fCurrentRequestInterval(0),
        fNumCalibIgnore(30),
        fNumCalibRequests(300),
        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",
                     "Only biasctrl is available and connected with its hardware.");

        AddStateName(kStateConnectedFSC, "ConnectedFSC",
                     "biasctrl and fscctrl are available and connected with their hardware.");
        AddStateName(kStateConnectedFAD, "ConnectedFAD",
                     "biasctrl and fadctrl are available and connected with their hardware.");
        AddStateName(kStateConnected, "Connected",
                     "biasctrl, fadctrl and fscctrl are available and connected with their hardware.");

        AddStateName(kStateFeedbackCtrlIdle, "FeedbackIdle",
                     "Feedback control activated, but voltage output disabled.");
        AddStateName(kStateTempCtrlIdle, "TempCtrlIdle",
                     "Temperature control activated, but voltage output disabled.");
        AddStateName(kStateCurrentCtrlIdle, "CurrentCtrlIdle",
                     "Current 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(kStateCurrentCtrlRunning, "CurrentControl",
                     "Current/Temp control activated and voltage output enabled.");
        AddStateName(kStateCalibrating, "Calibrating",
                     "Calibrating current offsets.");

        AddEvent("START_FEEDBACK_CONTROL", "S:1", kStateConnectedFAD, kStateConnected)
            (bind(&StateMachineFeedback::StartFeedback, this, placeholders::_1))
            ("Start the feedback control loop"
             "|Num[short]:Number of events 'medianed' to calculate the correction value");

        AddEvent("START_GLOBAL_FEEDBACK", "S:1", kStateConnectedFAD, kStateConnected)
            (bind(&StateMachineFeedback::StartFeedbackGlobal, this, placeholders::_1))
            ("Start the global feedback control loop"
             "Num[short]:Number of events averaged to calculate the correction value");

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

        AddEvent("START_CURRENT_CONTROL", "F:1", kStateConnectedFSC, kStateConnected)
            (bind(&StateMachineFeedback::StartCurrentCtrl, this, placeholders::_1))
            ("Start the current/temperature control loop"
             "|offset[V]:Offset from the nominal current/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", kStateConnectedFSC, kStateConnected)//, kStateIdle)
            (bind(&StateMachineFeedback::CalibrateCurrents, this))
            ("");

        AddEvent("SET_CURRENT_REQUEST_INTERVAL", kStateConnectedFSC, kStateConnected)//, kStateIdle)
            (bind(&StateMachineFeedback::SetCurrentRequestInterval, this, placeholders::_1))
            ("|interval[ms]:Interval between two current requests in modes which need that.");

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

        fGain = 0.1; // V(Amplitude) / V(Bias)

        // 148 -> 248

        // 33 : 10s  < 2%
        // 50 :  5s  < 2%
        // 66 :  3s  < 2%
        // 85 :  2s  < 2%

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

        // Is that independent of the aboslute real amplitude of
        // the light pulser?

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

        Message(msg);

        fCurrentRequestInterval = conf.Get<uint16_t>("current-request-interval");
        fNumCalibIgnore   = conf.Get<uint16_t>("num-calib-ignore");
        fNumCalibRequests = conf.Get<uint16_t>("num-calib-average");

        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.")
        ("current-request-interval",  var<uint16_t>(1000), "Interval between two current requests.")
        ("num-calib-ignore",  var<uint16_t>(30), "Number of current requests to be ignored before averaging")
        ("num-calib-average",  var<uint16_t>(300), "Number of current requests to be averaged")
        ;

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