#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 "PixelMap.h"

#include "tools.h"

#include "LocalControl.h"

#include "HeadersFAD.h"
#include "HeadersFSC.h"
#include "HeadersBIAS.h"
#include "HeadersFeedback.h"

#include "DimState.h"
#include "DimDescriptionService.h"

using namespace std;

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

class StateMachineFeedback : public StateMachineDim
{
private:
    enum control_t
    {
        kIdle,
        kTemp,
        kFeedback,
        kFeedbackGlobal,
        kCurrents,
    };

    control_t fControlType;

    PixelMap fMap;

    DimVersion fDim;
    DimDescribedState fDimFAD;
    DimDescribedState fDimFSC;
    DimDescribedState fDimBias;

    DimDescribedService fDimReference;
    DimDescribedService fDimDeviation;
    DimDescribedService fDimCalibration;

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

    vector<float>    fCalibration;
    vector<float>    fVoltGapd;

    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;
    double fTempOffset;
    double fCalibrationOffset;
    double fAppliedOffset;

    uint16_t fCurrentRequestInterval;
    uint16_t fNumCalibIgnore;
    uint16_t fNumCalibRequests;

    bool fOutputEnabled;

    int HandleCameraTemp(const EventImp &evt)
    {
        if (fControlType!=kTemp && fControlType!=kCurrents)
            return GetCurrentState();

        if (evt.GetSize()!=60*sizeof(float))
            return GetCurrentState();

        const float *ptr = evt.Ptr<float>();

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

        if (numt==0)
        {
            Warn("Received sensor temperatures all invalid.");
            return GetCurrentState();
        }

        avgt /= numt; // [deg C]

        fTempOffset = (avgt-25)*4./70; // [V]

        fCursorTemp++;

        return HandleCurrentControl();
    }

    int HandleCurrentControl()
    {
        const double dUt = fTempOffset; // [V]

        if (GetCurrentState()==Feedback::State::kCalibrating && fBiasOffset>dUt-1.2)
        {
            fCursorTemp = 0;

            ostringstream msg;
            msg << " (applied calibration offset " << fBiasOffset << "V exceeds temperature correction " << fTempOffset << "V - 1.2V.";
            Warn("Trying to calibrate above G-APD breakdown volatge!");
            Warn(msg);
            return GetCurrentState();
        }

        // FIXME: If calibrating do not wait for the temperature!
        fAppliedOffset = fBiasOffset;
        if (GetCurrentState()!=Feedback::State::kCalibrating)
            fAppliedOffset += dUt;

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

        vec[BIAS::kNumChannels*2]   = dUt;
        vec[BIAS::kNumChannels*2+1] = fBiasOffset;

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

            // Pixel  583: 5 31 == 191 (5)  C2 B3 P3
            // Pixel  830: 2  2 ==  66 (4)  C0 B8 P1
            // Pixel 1401: 6  1 == 193 (5)  C2 B4 P0

            // Convert from DAC counts to uA
            const double conv = 5000./4096;

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

            const float *Iavg = fCalibration.data();                      // Offset at U=fCalibrationOffset
            const float *Ravg = fCalibration.data()+BIAS::kNumChannels*2; // Measured resistance

            // U0 = fCalibrationOffset
            // dT = fAppliedVoltage

            // Ifeedback = Im[i] - (U[i]-U0)/Ravg[i] - Iavg[i];
            // dUapplied[i] + dUneu[i] = R[g] * (Im[i] - (dUapplied[i]+dUneu[i]-U0+dT)/Ravg[i] - Iavg[i])

            // The assumption here is that the offset calculated from the temperature
            // does not significanly change within a single step

            // dU[i] := dUtotal[i] = dUapplied[i] + dUneu[i]
            // dU[i] / R[g]               = Im[i] - (dU[i]+dT-U0)/Ravg[i] - Iavg[i]
            // dU[i]/R[g] + dU[i]/Ravg[i] = Im[i] + U0/Ravg[i] - dT/Ravg[i] - Iavg[i]
            // dU[i]*(1/R[g]+1/Ravg[i])   = Im[i] - Iavg[i] + U0/Ravg[i] - dT/Ravg[i]
            // dU[i]   = (Im[i] - Iavg[i] + U0/Ravg[i] - dT/Ravg[i]) / (1/R[g]+1/Ravg[i])
            // dU[i] = { Im[i] - Iavg[i] + (U0-dT)/Ravg[i] } * r    with   r := 1 / (1/R[g]+1/Ravg[i])

            const double U0 = fAppliedOffset-fCalibrationOffset;

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

                // Average measured current
                const double Im = double(fCurrentsAvg[i])/fCursorCur * conv; // [uA]

                // Group index (0 or 1) of the of the pixel (4 or 5 pixel patch)
                const int g = hv.group();

                // Serial resistors in front of the G-APD
                double Rg = R[g];

                // This is assuming that the broken pixels have a 390 Ohm instead of 3900 Ohm serial resistor
                if (i==66)                // Pixel 830(66)
                    Rg = 2400;            // 2400 = (3/3900 + 1/390) + 1000 + 1100
                if (i==191 || i==193)     // Pixel 583(191) / Pixel 1401(193)
                    Rg = 2379;            // 2379 = (4/3900 + 1/390) + 1000 + 1100

                const double r = 1./(1./Rg + 1./Ravg[i]); // [Ohm]

                // Offset induced by the voltage above the calibration point
                const double dI = U0/Ravg[i]; // [V/Ohm]
 
                // Offset at the calibration point (make sure that the calibration is
                // valid (Im[i]>Iavg[i]) and we operate above the calibration point)
                const double I = Im>Iavg[i] ? Im - Iavg[i] : 0; // [A]

                // Make sure that the averaged resistor is valid
                const double dU = Ravg[i]>10000 ? r*(I*1e-6 - dI) : 0;

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

                // Angelegte Spannung:    U0+dU
                // Gemessener Strom:      Im - Iavg
                // Strom offset:          (U0+dU) / Ravg
                // Fliessender Strom:     Im-Iavg - (U0+dU)/Ravg
                // Korrektur:             [ Im-Iavg - (U0+dU)/Ravg ] * Rg

                // Aufgeloest nach dU:    dU =  ( Im-Iavg - dU/Ravg ) / ( 1/Rg + 1/Ravg )
                // Equivalent zu:         dU  = ( I*Ravg - U0 ) / ( Ravg/Rg+1 )

                // Calculate statistics only for channels with a valid calibration
                if (Iavg[i]>0)
                {
                    med[g][num[g]] = dU;
                    avg[g] += dU;
                    num[g]++;

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

            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) || fDimBias.state()!=BIAS::State::kVoltageOn)
            return GetCurrentState();

        // Trigger calibration
        if (GetCurrentState()==Feedback::State::kCalibrating && fCursorTemp==2)
        {
            DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
            return GetCurrentState();
        }

        ostringstream msg;
        msg << setprecision(4) << "Sending new absolute offset (" << fAppliedOffset << "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));

        return GetCurrentState();
    }

    int AverageCurrents(const EventImp &evt)
    {
        if (evt.GetSize()!=BIAS::kNumChannels*sizeof(int16_t))
            return -1;

        if (fDimBias.state()!=BIAS::State::kVoltageOn)
            return false;

        if (fCursorCur++<0)
            return true;

        const int16_t *ptr = evt.Ptr<int16_t>();

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

        return true;
    }


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

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

        if (rc==0)
            return;

        fCalibration.resize(BIAS::kNumChannels*3);

        float *avg = fCalibration.data();
        float *rms = fCalibration.data()+BIAS::kNumChannels;
        float *res = fCalibration.data()+BIAS::kNumChannels*2;

        const double conv = 5000./4096;

        for (int i=0; i<BIAS::kNumChannels; i++)
        {
            const double I = double(fCurrentsAvg[i])/fCursorCur;

            res[i] = (fVoltGapd[i]+fCalibrationOffset)/I / conv * 1e6;
            avg[i] = conv * I;
            rms[i] = conv * sqrt(double(fCurrentsRms[i])/fCursorCur-I*I);
        }

        fDimCalibration.Update(fCalibration);

        fOutputEnabled = false;
        fControlType = kIdle;

        Info("Calibration successfully done.");

        if (fDimBias.state()==BIAS::State::kVoltageOn)
            DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
    }

    void HandleFeedback(const EventImp &evt)
    {
        if (evt.GetSize()!=1440*sizeof(float))
            return;

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

        const Time tm(evt.GetTime());

        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(evt.Ptr<float>(), evt.Ptr<float>()+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+2);
        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 || fDimBias.state()!=BIAS::State::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(const EventImp &evt)
    {
        if (evt.GetSize()!=1440*sizeof(float))
            return;

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

        vector<float> arr(evt.Ptr<float>(), evt.Ptr<float>()+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+2);
        vec.insert(vec.begin(),     nch, fPV[2][0]-fSP[0]);
        vec.insert(vec.begin()+nch, nch, correction[0]);
        vec.push_back(0);
        vec.push_back(0);

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

        if (!fOutputEnabled || fDimBias.state()!=BIAS::State::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));
    }

    int HandleBiasCurrent(const EventImp &evt)
    {
        if (fControlType==kTemp && GetCurrentState()==Feedback::State::kCalibrating)
            HandleCalibration(evt);

        if (fControlType==kFeedbackGlobal || fControlType==kCurrents)
            AverageCurrents(evt);

        if (curr==&fBiasA && fControlType==kCurrents &&
            fCursorTemp>0 && fCursorCur>0)
        {
            // fCursorTemp: 1 2 3 4 5 6 7 8
            // fCursor%x:   1 1 1 2 2 2 3 3    // 9 steps in ~15s
            if (fCursorTemp<3 && fCursorCur%(fCursorTemp/3+1)==0)
                HandleCurrentControl();
        }

        return GetCurrentState();
    }

    int HandleBiasData(const EventImp &evt)
    {
        if (fControlType==kFeedback)
            HandleFeedback(evt);

        if (fControlType==kFeedbackGlobal)
            HandleGlobalFeedback(evt);

        return GetCurrentState();
    }

    int HandleBiasNom(const EventImp &evt)
    {
        fVoltGapd.assign(evt.Ptr<float>(), evt.Ptr<float>()+416);

        Info("Nominal bias voltages received.");

        return GetCurrentState();
    }

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

    int Print() const
    {
        Out() << fDim << endl;
        Out() << fDimFAD << endl;
        Out() << fDimFSC << endl;
        Out() << fDimBias << endl;

        return GetCurrentState();
    }

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

        const float *avg = fCalibration.data();
        const float *rms = fCalibration.data()+BIAS::kNumChannels;
        const float *res = fCalibration.data()+BIAS::kNumChannels*2;

        Out() << "Average current at " << fCalibrationOffset << "V below G-APD operation voltage:\n";

        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", avg[k*32+j*4+i], rms[k*32+j*4+i]);
                Out() << '\n';
            }
        Out() << '\n';

        Out() << "Measured calibration resistor:\n";
        for (int k=0; k<13; k++)
            for (int j=0; j<4; j++)
            {
                Out() << setw(2) << k << "|" << setw(2) << j*8 << "|";
                for (int i=0; i<8; i++)
                    Out() << Tools::Form(" %5.0f", res[k*32+j*8+i]);
                Out() << '\n';
            }

        Out() << flush;

        return GetCurrentState();
    }

    void WarnState(bool needfsc, bool needfad)
    {
        const bool bias = fDimBias.state() >= BIAS::State::kConnecting;
        const bool fsc  = fDimFSC.state()  >= FSC::State::kConnected;
        const bool fad  = fDimFAD.state()  >= FAD::State::kConnected;

        if (!bias)
            Warn("Bias control not yet ready.");
        if (needfsc && !fsc)
            Warn("FSC control not yet ready.");
        if (needfad && !fad)
            Warn("FAD control not yet ready.");
    }

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

        if (fControlType==kCurrents)
            if (fCursorTemp>1)
                fCursorTemp = 1;

        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+2, fBiasOffset);
        vec[2*BIAS::kNumChannels]   = 0;
        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;

        WarnState(false, true);

        fBiasOffset = 0;
        ResetData(evt.GetShort());

        fControlType = kFeedback;

        return GetCurrentState();
    }

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

        WarnState(false, true);

        fBiasOffset = 0;
        ResetData(evt.GetShort());

        fControlType = kFeedbackGlobal;

        return GetCurrentState();
    }

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

        WarnState(true, false);

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

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

        if (fDimBias.state()==BIAS::State::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();
        }

        WarnState(true, false);

        fBiasOffset = evt.GetFloat();
        fTempOffset = -3;
        ResetData(0);
        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 (fDimBias.state()==BIAS::State::kRamping)
        {
            Warn("Calibration cannot be started when biasctrl is in state Ramping.");
            return GetCurrentState();
        }

        if (fVoltGapd.size()==0)
        {
            Error("No G-APD reference voltages received yet (BIAS_CONTROL/NOMINAL).");
            return GetCurrentState();
        }

        WarnState(true, false);

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

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

        return Feedback::State::kCalibrating;
    }

    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 (!fDim.online())
            return Feedback::State::kDimNetworkNA;

        const bool bias = fDimBias.state() >= BIAS::State::kConnecting;
        const bool fad  = fDimFAD.state()  >= FAD::State::kConnected;
        const bool fsc  = fDimFSC.state()  >= FSC::State::kConnected;

        // All subsystems are not connected
        if (!bias && !fad && !fsc)
            return Feedback::State::kDisconnected;

        // At least one subsystem apart from bias is connected
        if (bias && !fad && !fsc)
            return Feedback::State::kConnecting;

/*
        // All subsystems are connected
        if (GetCurrentStatus()==Feedback::State::kConfiguringStep1)
        {
            if (fCursor<1)
                return Feedback::State::kConfiguringStep1;

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

                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 Feedback::State::kConnected;
        }
        */

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

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

                return fOutputEnabled ? Feedback::State::kTempCtrlRunning : Feedback::State::kTempCtrlIdle;
            }
            if (fControlType==kCurrents)
            {
                static Time past;
                if (fCurrentRequestInterval>0 && Time()-past>boost::posix_time::milliseconds(fCurrentRequestInterval))
                {
                    if (fDimBias.state()==BIAS::State::kVoltageOn)
                        DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
                    past = Time();
                }

                return fOutputEnabled && fCursorTemp>0 ? Feedback::State::kCurrentCtrlRunning : Feedback::State::kCurrentCtrlIdle;
            }
        }

        if (bias && fad && !fsc)
            return Feedback::State::kConnectedFAD;

        if (bias && fsc && !fad)
            return Feedback::State::kConnectedFSC;

        return Feedback::State::kConnected;
    }

public:
    StateMachineFeedback(ostream &out=cout) : StateMachineDim(out, "FEEDBACK"),
        //---
        fDimFAD("FAD_CONTROL"),
        fDimFSC("FSC_CONTROL"),
        fDimBias("BIAS_CONTROL"),
        //---
        fDimReference("FEEDBACK/REFERENCE", "F:416",
                      "Amplitude reference value(s)"
                      "Vref[mV]:Amplitude reference"),
        fDimDeviation("FEEDBACK/DEVIATION", "F:416;F:416;F:1;F:1",
                      "Control loop information"
                      "|DeltaAmpl[mV]:Amplitude offset measures"
                      "|DeltaBias[mV]:Correction value calculated"
                      "|DeltaTemp[mV]:Correction calculated from temperature"
                      "|DeltaUser[mV]:Additional offset specified by user"),
        fDimCalibration("FEEDBACK/CALIBRATION", "F:416;F:416;F:416",
                        "Current offsets"
                        "|Avg[uA]:Average offset"
                        "|Rms[uA]:Rms of offset"
                        "|R[Ohm]:Measured calibration resistor"),
        fSP(BIAS::kNumChannels),
        fKp(0), fKi(0), fKd(0), fT(-1),
        fCalibrationOffset(-3),
        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.

        fDim.Subscribe(*this);
        fDimFAD.Subscribe(*this);
        fDimFSC.Subscribe(*this);
        fDimBias.Subscribe(*this);

        Subscribe("BIAS_CONTROL/CURRENT")
            (bind(&StateMachineFeedback::HandleBiasCurrent, this, placeholders::_1));
        Subscribe("BIAS_CONTROL/FEEDBACK_DATA")
            (bind(&StateMachineFeedback::HandleBiasData,    this, placeholders::_1));
        Subscribe("FSC_CONTROL/TEMPERATURE")
            (bind(&StateMachineFeedback::HandleCameraTemp,  this, placeholders::_1));

        // State names
        AddStateName(Feedback::State::kDimNetworkNA, "DimNetworkNotAvailable",
                     "The Dim DNS is not reachable.");

        AddStateName(Feedback::State::kDisconnected, "Disconnected",
                     "The Dim DNS is reachable, but the required subsystems are not available.");

        AddStateName(Feedback::State::kConnecting, "Connecting",
                     "Only biasctrl is available and connected with its hardware.");

        AddStateName(Feedback::State::kConnectedFSC, "ConnectedFSC",
                     "biasctrl and fscctrl are available and connected with their hardware.");
        AddStateName(Feedback::State::kConnectedFAD, "ConnectedFAD",
                     "biasctrl and fadctrl are available and connected with their hardware.");
        AddStateName(Feedback::State::kConnected, "Connected",
                     "biasctrl, fadctrl and fscctrl are available and connected with their hardware.");

        AddStateName(Feedback::State::kFeedbackCtrlIdle, "FeedbackIdle",
                     "Feedback control activated, but voltage output disabled.");
        AddStateName(Feedback::State::kTempCtrlIdle, "TempCtrlIdle",
                     "Temperature control activated, but voltage output disabled.");
        AddStateName(Feedback::State::kCurrentCtrlIdle, "CurrentCtrlIdle",
                     "Current control activated, but voltage output disabled.");

        AddStateName(Feedback::State::kFeedbackCtrlRunning, "FeedbackControl",
                     "Feedback control activated and voltage output enabled.");
        AddStateName(Feedback::State::kTempCtrlRunning, "TempControl",
                     "Temperature control activated and voltage output enabled.");
        AddStateName(Feedback::State::kCurrentCtrlRunning, "CurrentControl",
                     "Current/Temp control activated and voltage output enabled.");
        AddStateName(Feedback::State::kCalibrating, "Calibrating",
                     "Calibrating current offsets.");

        AddEvent("START_FEEDBACK_CONTROL", "S:1", Feedback::State::kConnectedFAD, Feedback::State::kConnected)
            (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", Feedback::State::kConnectedFAD, Feedback::State::kConnected)
            (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", Feedback::State::kConnectedFSC, Feedback::State::kConnected)
            (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", Feedback::State::kConnectedFSC, Feedback::State::kConnected)
            (bind(&StateMachineFeedback::StartCurrentCtrl, this, placeholders::_1))
            ("Start the current/temperature control loop"
             "|offset[V]:Offset from the nominal current/temperature corrected value in Volts");

        // Feedback::State::kTempCtrlIdle, Feedback::State::kFeedbackCtrlIdle, Feedback::State::kTempCtrlRunning, Feedback::State::kFeedbackCtrlRunning
        AddEvent("STOP")
            (bind(&StateMachineFeedback::StopFeedback, this))
            ("Stop any control loop");

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

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

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

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

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

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

        AddEvent("SET_T", "D:1")//, Feedback::State::kIdle)
            (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", Feedback::State::kConnectedFSC, Feedback::State::kConnected)//, Feedback::State::kIdle)
            (bind(&StateMachineFeedback::CalibrateCurrents, this))
            ("");

        AddEvent("SET_CURRENT_REQUEST_INTERVAL", Feedback::State::kConnectedFSC, Feedback::State::kConnected)//, Feedback::State::kIdle)
            (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");
        fCalibrationOffset      = conf.Get<float>("calibration-offset");

        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")
        ("calibration-offset",  var<float>(-3), "Absolute offset relative to the G-APD operation voltage when calibrating")
        ;

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

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