#include <valarray>
#include <algorithm>

#include "Dim.h"
#include "Event.h"
#include "Shell.h"
#include "StateMachineDim.h"
#include "Connection.h"
#include "Configuration.h"
#include "Console.h"
#include "externals/PixelMap.h"

#include "tools.h"

#include "LocalControl.h"

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

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

using namespace std;

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

class StateMachineFeedback : public StateMachineDim
{
private:
    PixelMap fMap;

    bool fIsVerbose;

    DimVersion fDim;

    DimDescribedState fDimFSC;
    DimDescribedState fDimBias;

    DimDescribedService fDimCalibration;
    DimDescribedService fDimCalibration2;
    DimDescribedService fDimCalibrationR8;
    DimDescribedService fDimCurrents;
    DimDescribedService fDimOffsets;

    vector<float>    fCalibCurrentMes[6]; // Measured calibration current at six different levels
    vector<float>    fCalibVoltage[6];    // Corresponding voltage as reported by biasctrl

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

    vector<float>    fVoltGapd;     // Nominal breakdown voltage + 1.1V
    vector<float>    fBiasVolt;     // Output voltage as reported by bias crate (voltage between R10 and R8)
    vector<float>    fBiasR9;       // 
    vector<uint16_t> fBiasDac;      // Dac value corresponding to the voltage setting

    vector<float>    fCalibration;
    vector<float>    fCalibDeltaI;
    vector<float>    fCalibR8;

     int64_t fCursorCur;

    Time fTimeCalib;
    Time fTimeTemp;

    double fUserOffset;
    vector<double> fTempOffset;
    float fTempOffsetAvg;
    float fTempOffsetRms;
    double fTempCoefficient;
    double fTemp;

    vector<double> fVoltOffset;

    uint16_t fCurrentRequestInterval;
    uint16_t fNumCalibIgnore;
    uint16_t fNumCalibRequests;
    uint16_t fCalibStep;

    // ============================= Handle Services ========================

    int HandleBiasStateChange()
    {
        if (fDimBias.state()==BIAS::State::kVoltageOn && GetCurrentState()==Feedback::State::kCalibrating)
        {
            Dim::SendCommandNB("BIAS_CONTROL/REQUEST_STATUS");
            Info("Starting calibration step "+to_string(fCalibStep));
        }

        if (fDimBias.state()==BIAS::State::kVoltageOff && GetCurrentState()==Feedback::State::kInProgress)
            return Feedback::State::kCalibrated;

        return GetCurrentState();
    }
    // ============================= Handle Services ========================

    bool CheckEventSize(size_t has, const char *name, size_t size)
    {
        if (has==size)
            return true;

        // Disconnected
        if (has==0)
            return false;

        ostringstream msg;
        msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
        Fatal(msg);
        return false;
    }

    int HandleBiasNom(const EventImp &evt)
    {
        if (evt.GetSize()>=416*sizeof(float))
        {
            fVoltGapd.assign(evt.Ptr<float>(), evt.Ptr<float>()+416);
            fBiasR9.assign(evt.Ptr<float>()+2*416, evt.Ptr<float>()+3*416);
            Info("Nominal bias voltages and calibration resistor received.");
        }

        return GetCurrentState();
    }

    int HandleBiasVoltage(const EventImp &evt)
    {
        if (evt.GetSize()>=416*sizeof(float))
            fBiasVolt.assign(evt.Ptr<float>(), evt.Ptr<float>()+416);
        return GetCurrentState();
    }

    int HandleBiasDac(const EventImp &evt)
    {
        if (evt.GetSize()>=416*sizeof(uint16_t))
            fBiasDac.assign(evt.Ptr<uint16_t>(), evt.Ptr<uint16_t>()+416);
        return GetCurrentState();
    }

    int HandleCameraTemp(const EventImp &evt)
    {
        if (!CheckEventSize(evt.GetSize(), "HandleCameraTemp", 323*sizeof(float)))
        {
            fTimeTemp = Time(Time::none);
            return GetCurrentState();
        }

        //fTempOffset = (avgt-25)*0.0561765; // [V] From Hamamatsu datasheet
        //fTempOffset = (avgt-25)*0.05678; // [V] From Hamamatsu datasheet plus our own measurement (gein vs. temperature)

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

        fTimeTemp = evt.GetTime();
        fTemp     = evt.Get<float>(321*4);

        fTempOffsetAvg = (fTemp-25)*fTempCoefficient;
        fTempOffsetRms =  evt.Get<float>(322*4)*fTempCoefficient;

        fTempOffset.resize(320);
        for (int i=0; i<320; i++)
            fTempOffset[i] = (ptr[i]-25)*fTempCoefficient;

        return GetCurrentState();
    }

    pair<vector<float>, vector<float>> AverageCurrents(const int16_t *ptr, int n)
    {
        if (fCursorCur++>=0)
        {
            for (int i=0; i<BIAS::kNumChannels; i++)
            {
                fCurrentsAvg[i] += ptr[i];
                fCurrentsRms[i] += ptr[i]*ptr[i];
            }
        }

        if (fCursorCur<n)
            return make_pair(vector<float>(), vector<float>());

        const double conv = 5e-3/4096;

        vector<float> rms(BIAS::kNumChannels);
        vector<float> avg(BIAS::kNumChannels);
        for (int i=0; i<BIAS::kNumChannels; i++)
        {
            avg[i]  = double(fCurrentsAvg[i])/fCursorCur * conv;
            rms[i]  = double(fCurrentsRms[i])/fCursorCur * conv * conv;
            rms[i] -= avg[i]*avg[i];
            rms[i]  = rms[i]<0 ? 0 : sqrt(rms[i]);
        }

        return make_pair(avg, rms);
    }

    int HandleCalibration(const EventImp &evt)
    {
        if (fDimBias.state()!=BIAS::State::kVoltageOn)
            return GetCurrentState();

        const uint16_t dac = 256+512*fCalibStep; // Command value

        // Only the channels which are no spare channels are ramped
        if (std::count(fBiasDac.begin(), fBiasDac.end(), dac)!=320)
            return GetCurrentState();

        const auto rc = AverageCurrents(evt.Ptr<int16_t>(), fNumCalibRequests);
        if (rc.first.size()==0)
        {
            Dim::SendCommandNB("BIAS_CONTROL/REQUEST_STATUS");
            return GetCurrentState();
        }

        const vector<float> &avg = rc.first;
        const vector<float> &rms = rc.second;

        // Current through resistor R8
        fCalibCurrentMes[fCalibStep] = avg;       // [A]
        fCalibVoltage[fCalibStep]    = fBiasVolt; // [V]

        // ------------------------- Update calibration data --------------------

        struct cal_data
        {
            uint32_t dac;
            float    U[416];
            float    Iavg[416];
            float    Irms[416];

            cal_data() { memset(this, 0, sizeof(cal_data)); }
        } __attribute__((__packed__));

        cal_data cal;
        cal.dac = dac;
        memcpy(cal.U,    fBiasVolt.data(), 416*sizeof(float));
        memcpy(cal.Iavg, avg.data(),       416*sizeof(float));
        memcpy(cal.Irms, rms.data(),       416*sizeof(float));

        fDimCalibration2.setData(cal);
        fDimCalibration2.Update(fTimeCalib);

        // -------------------- Start next calibration steo ---------------------

        if (++fCalibStep<6)
        {
            fCursorCur  = -fNumCalibIgnore;
            fCurrentsAvg.assign(BIAS::kNumChannels, 0);
            fCurrentsRms.assign(BIAS::kNumChannels, 0);

            Dim::SendCommandNB("BIAS_CONTROL/SET_GLOBAL_DAC", uint32_t(256+512*fCalibStep));

            return GetCurrentState();
        }

        // --------------- Calculate old style calibration ----------------------

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

        float *pavg  = fCalibration.data();
        float *prms  = fCalibration.data()+BIAS::kNumChannels;
        float *pres  = fCalibration.data()+BIAS::kNumChannels*2;
        float *pUmes = fCalibration.data()+BIAS::kNumChannels*3;

        for (int i=0; i<BIAS::kNumChannels; i++)
        {
            const double I = fCalibCurrentMes[5][i]; // [A]
            const double U = fBiasVolt[i];           // [V]

            pavg[i]  = I*1e6;                        // [uA]
            prms[i]  = rms[i]*1e6;                   // [uA]
            pres[i]  = U/I;                          // [Ohm]
            pUmes[i] = U;                            // [V]
        }

        fDimCalibration.setData(fCalibration);
        fDimCalibration.Update(fTimeCalib);

        // -------------------- New style calibration --------------------------

        fCalibDeltaI.resize(BIAS::kNumChannels);
        fCalibR8.resize(BIAS::kNumChannels);

        // Linear regression of the values at 256+512*N for N={ 3, 4, 5 }
        for (int i=0; i<BIAS::kNumChannels; i++)
        {
            // x: Idac
            // y: Iadc

            double x  = 0;
            double y  = 0;
            double xx = 0;
            double xy = 0;

            const int beg = 3;
            const int end = 5;
            const int len = end-beg+1;

            for (int j=beg; j<=end; j++)
            {
                const double Idac = (256+512*j)*1e-3/4096;

                x  += Idac;
                xx += Idac*Idac;
                y  += fCalibCurrentMes[j][i];
                xy += fCalibCurrentMes[j][i]*Idac;
            }

            const double m1 = xy - x*y / len;
            const double m2 = xx - x*x / len;

            const double m = m2==0 ? 0 : m1/m2;

            const double t = (y - m*x) / len;

            fCalibDeltaI[i] = t;     // [A]
            fCalibR8[i]     = 100/m; // [Ohm]
        }

        vector<float> v;
        v.reserve(BIAS::kNumChannels*2);
        v.insert(v.end(), fCalibDeltaI.begin(), fCalibDeltaI.end());
        v.insert(v.end(), fCalibR8.begin(),     fCalibR8.end());

        fDimCalibrationR8.setData(v);
        fDimCalibrationR8.Update(fTimeCalib);

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

        Info("Calibration successfully done.");
        Dim::SendCommandNB("BIAS_CONTROL/SET_ZERO_VOLTAGE");

        return Feedback::State::kCalibrated;
    }

    int HandleBiasCurrent(const EventImp &evt)
    {
        if (!CheckEventSize(evt.GetSize(), "HandleBiasCurrent", BIAS::kNumChannels*sizeof(uint16_t)))
            return Feedback::State::kConnected;

        if (GetCurrentState()<Feedback::State::kCalibrating)
            return GetCurrentState();

        // ------------------------------- HandleCalibration -----------------------------------
        if (GetCurrentState()==Feedback::State::kCalibrating)
            return HandleCalibration(evt);

        // ---------------------- Calibrated, WaitingForData, InProgress -----------------------

        // We are waiting but no valid temperature yet, go on waiting
        if (GetCurrentState()==Feedback::State::kWaitingForData &&
            (!fTimeTemp.IsValid() || Time()-fTimeTemp>boost::posix_time::minutes(5)))
            return GetCurrentState();

        // We are already in progress but no valid temperature update anymore
        if (GetCurrentState()==Feedback::State::kInProgress &&
            (!fTimeTemp.IsValid() || Time()-fTimeTemp>boost::posix_time::minutes(5)))
        {
            Warn("Current control in progress, but last received temperature older than 5min... switching voltage off.");
            Dim::SendCommandNB("BIAS_CONTROL/SET_ZERO_VOLTAGE");
            return Feedback::State::kCalibrated;
        }

        // ---------------------- Calibrated, WaitingForData, InProgress -----------------------

        const int Navg = fDimBias.state()!=BIAS::State::kVoltageOn ? 1 : 3;

        const vector<float> &Imes = AverageCurrents(evt.Ptr<int16_t>(), Navg).first;
        if (Imes.size()==0)
            return GetCurrentState();

        fCurrentsAvg.assign(416, 0);
        fCurrentsRms.assign(416, 0);
        fCursorCur = 0;

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

        // Nominal overvoltage (w.r.t. the bias setup values)
        const double overvoltage = GetCurrentState()<Feedback::State::kWaitingForData ? 0 : fUserOffset;

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

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

        struct dim_data
        {
            float I[416];
            float Iavg;
            float Irms;
            float Imed;
            float Idev;
            uint32_t N;
            float Tdiff;
            float Uov[416];
            float Unom;
            float dUtemp;

            dim_data() { memset(this, 0, sizeof(dim_data)); }
        } __attribute__((__packed__));

        int Ndev[3] = { 0, 0, 0 };

        dim_data data;

        data.Unom   = overvoltage;
        data.dUtemp = fTempOffsetAvg;

        vector<float> vec(416);

        /*
        if (fEnableOldAlgorithm)
        {
            // ================================= old =======================
            // 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

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

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

                // Average measured current
                const double Im = Imes[i] * 1e6; // [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 Ubd = fVoltGapd[i] + fTempOffsets[i];
                const double U0  = Ubd + overvoltage - fCalibVoltage[5][i]; // appliedOffset-fCalibrationOffset;
                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; // [uA]

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

                vec[i] = Ubd + overvoltage + dU;

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

                    data.I[i]  = Imes[i]*1e6 - fBiasVolt[i]/Ravg[i]*1e6;
                    data.I[i] /= hv.count();

                    if (i==66)
                        data.I[i] /= 1.3;
                    if (i==191 || i==193)
                        data.I[i] /= 1.4;

                    data.Iavg += data.I[i];
                    data.Irms += data.I[i]*data.I[i];

                    med[2][num[2]++] = data.I[i];
                }
            }
        }
        */

        double UdrpAvg = 0;
        double UdrpRms = 0;

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

            // Number of G-APDs in this patch
            const int N = hv.count();

            // Average measured ADC value for this channel
            const double adc = Imes[i]/* * (5e-3/4096)*/; // [A]

            // Current through ~100 Ohm measurement resistor
            //const double I8 = (adc-fCalibDeltaI[i])*fCalibR8[i]/100;
            const double I8 = adc-fCalibDeltaI[i];

            // Current through calibration resistors (R9)
            // This is uncalibrated, but since the corresponding calibrated
            // value I8 is subtracted, the difference should yield a correct value
            const double I9 = fBiasDac[i] * (1e-3/4096);//U9/R9;   [A]

            // Current in R4/R5 branch
            //const double Iout = I8 - I9;//I8>I9 ? I8 - I9 : 0;
            const double Iout = I8 - I9*100/fCalibR8[i];//I8>I9 ? I8 - I9 : 0;

            // Applied voltage at calibration resistors, according to biasctrl
            const double U9 = fBiasVolt[i];

            // Serial resistors (one 1kOhm at the output of the bias crate, one 1kOhm in the camera)
            const double R4 = 2000;

            // Serial resistor of the individual G-APDs
            double R5 = 3900./N;

            // This is assuming that the broken pixels have a 390 Ohm instead of 3900 Ohm serial resistor
            if (i==66 || i==193)               // Pixel 830(66) / Pixel 583(191)
                R5 = 1./((N-1)/3900.+1/1000.);
            if (i==191)                        // Pixel 1399(193)
                R5 = 1./((N-1)/3900.+1/390.);
            if (i==17 || i==206)               // dead pixel 923(80) / dead pixel 424(927)
                R5 = 3900./(N-1);              // cannot identify third dead pixel in light-pulser data

            // The measurement resistor
            const double R8 = 0;

            // Total resistance of branch with diodes (R4+R5)
            // Assuming that the voltage output of the OpAMP is linear
            // with the DAC setting and not the voltage at R9, the
            // additional voltage drop at R8 must be taken into account
            const double R = R4 + R5 + R8;

            // For the patches with a broken resistor - ignoring the G-APD resistance -
            // we get:
            //
            // I[R=3900] =  Iout *      1/(10+(N-1))  = Iout        /(N+9)
            // I[R= 390] =  Iout * (1 - 1/(10+(N-1))) = Iout * (N+8)/(N+9)
            //
            // I[R=390] / I[R=3900] = N+8
            //
            // Udrp = Iout*3900/(N+9) + Iout*1000 + Iout*1000 = Iout * R

            // Voltage drop in R4/R5 branch (for the G-APDs with correct resistor)
            // The voltage drop should not be <0, otherwise an unphysical value
            // would be amplified when Uset is calculated.
            const double Udrp = Iout<0 ? 0 : R*Iout;

            // Nominal breakdown voltage with correction for temperature dependence
            const double Ubd = fVoltGapd[i] + fVoltOffset[i] + fTempOffset[i];

            // Current overvoltage (at a G-APD with the correct 3900 Ohm resistor)
            //const double Uov = U9-Udrp-Ubd>0 ? U9-Udrp-Ubd : 0;
            const double Uov = U9-Udrp-Ubd>-0.44/*-0.34*/ ? U9-Udrp-Ubd : -0.44/*-0.34*/;

            // Iout linear with U9 above Ubd
            //
            //  Rx = (U9-Ubd)/Iout
            //  I' = (U9'-Ubd) / Rx
            //  Udrp' = R*I'
            //  Uov = U9' - Udrp' - Ubd
            //  Uov = overvoltage
            //
            //  overvoltage = U9' - Udrp' - Ubd
            //  overvoltage = U9' - R*I' - Ubd
            //  overvoltage = U9' - R*((U9'-Ubd)/Rx) - Ubd
            //  overvoltage = U9' - U9'*R/Rx + Ubd*R/Rx - Ubd
            //  overvoltage = U9'*(1 - R/Rx) + Ubd*R/Rx - Ubd
            //  overvoltage - Ubd*R/Rx +Ubd = U9'*(1 - R/Rx)
            //  U9' = [ overvoltage - Ubd*R/Rx +Ubd ] / (1 - R/Rx)
            //

            // The current through one G-APD is the sum divided by the number of G-APDs
            // (assuming identical serial resistors)
            double Iapd = Iout/N;

            // Rtot = Uapd/Iout
            // Ich  = Uapd/Rch = (Rtot*Iout) / Rch = Rtot/Rch * Iout
            //
            // Rtot = 3900/N
            // Rch  = 3900
            //
            // Rtot = 1./((N-1)/3900 + 1/X)       X=390 or X=1000
            // Rch  = 3900
            //
            // Rtot/Rch =   1/((N-1)/3900 + 1/X)/3900
            // Rtot/Rch =   1/( [ X*(N-1) + 3900 ] / [ 3900 * X ])/3900
            // Rtot/Rch =   X/( [ X*(N-1)/3900 + 1 ] )/3900
            // Rtot/Rch =   X/( [ X*(N-1) + 3900 ] )
            // Rtot/Rch =   1/( [ (N-1) + 3900/X ] )
            //
            // Rtot/Rch[390Ohm]  =  1/( [ N + 9.0 ] )
            // Rtot/Rch[1000Ohm] =  1/( [ N + 2.9 ] )
            //
            // In this and the previosu case we neglect the resistance of the G-APDs, but we can make an
            // assumption: The differential resistance depends more on the NSB than on the PDE,
            // thus it is at least comparable for all G-APDs in the patch. In addition, although the
            // G-APD with the 390Ohm serial resistor has the wrong voltage applied, this does not
            // significantly influences the ohmic resistor or the G-APD because the differential
            // resistor is large enough that the increase of the overvoltage does not dramatically
            // increase the current flow as compared to the total current flow.
            if (i==66 || i==193)           // Iout/13 15.8   / Iout/14  16.8
                Iapd = Iout/(N+2.9);
            if (i==191)                    // Iout/7.9  38.3
                Iapd = Iout/(N+9);
            if (i==17 || i==206)
                Iapd = Iout/(N-1);


            // 3900   +   Rapd  = I0   ->  Uapd = Utot - 3900*I0
            // 3900   +   Rapd  = I0   ->  Uapd = Utot - 3900*I0
            // 3900   +   Rapd  = I0   ->  Uapd = Utot - 3900*I0
            // 390    +   Rx    = Ix   ->  Uapd = Utot -  390*Ix

            // Iout = N*I0 + Ix

            // The differential resistance of the G-APD, i.e. the dependence of the
            // current above the breakdown voltage, is given by
            //const double Rapd = Uov/Iapd;
            // This allows us to estimate the current Iov at the overvoltage we want to apply
            //const double Iov = overvoltage/Rapd;

            // Estimate set point for over-voltage (voltage drop at the target point)
            //const double Uset = Ubd + overvoltage + R*Iov*N;
            //const double Uset = Uov<0.3 ? Ubd + overvoltage + Udrp : Ubd + overvoltage + Udrp*pow(overvoltage/Uov, 1.66);
            const double Uset = 

                // Uov<0 ?
                //   Ubd + overvoltage + Udrp*pow(overvoltage/0.44/*0.34*/+1, 1.85/*1.66*/) :
                //   Ubd + overvoltage + Udrp*pow((overvoltage+0.44/*0.34*/)/(Uov+0.44/*0.34*/), 1.85/*1.66*/);

                // Uov<0 ?
                //   Ubd + overvoltage + Udrp*pow(overvoltage+0.44, 1.3213+0.2475*(overvoltage+0.44)) :
                //   Ubd + overvoltage + Udrp*pow(overvoltage+0.44, 1.3213+0.2475*(overvoltage+0.44))/pow(Uov+0.44, 1.3213+0.2475*(Uov+0.44));

                // This estimation is based on the linear increase of the
                // gain with voltage and the increase of the crosstalk with
                // voltage, as measured with the overvoltage-tests (OVTEST)

                Uov+0.44<0.022 ?
            Ubd + overvoltage + Udrp*
                exp(0.6*(overvoltage-Uov))*pow((overvoltage+0.44), 0.6) :
            Ubd + overvoltage + Udrp*
                exp(0.6*(overvoltage-Uov))*pow((overvoltage+0.44)/(Uov+0.44), 0.6);




            if (fabs(overvoltage-Uov)>0.033)
                Ndev[0]++;
            if (fabs(overvoltage-Uov)>0.022)
                Ndev[1]++;
            if (fabs(overvoltage-Uov)>0.011)
                Ndev[2]++;

            // Voltage set point
            vec[i] = Uset;

            /*
             if (fDimBias.state()==BIAS::State::kVoltageOn && GetCurrentState()==Feedback::State::kInProgress &&
             fabs(Uov-overvoltage)>0.033)
             cout << setprecision(4) << setw(3) << i << ": Uov=" << Uov << " Udrp=" << Udrp << " Iapd=" << Iapd*1e6 << endl;
             */

            // Calculate statistics only for channels with a valid calibration
            //if (Uov>0)
            {
                const int g = hv.group();

                med[g][num[g]] = Uov;
                avg[g] += Uov;
                num[g]++;

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

                const double iapd = Iapd*1e6; // A --> uA

                data.I[i]  = iapd;
                data.Iavg += iapd;
                data.Irms += iapd*iapd;

                data.Uov[i] = Uov;

                med[2][num[2]++] = iapd;

                UdrpAvg += Udrp;
                UdrpRms += Udrp*Udrp;
            }
        }

        // ------------------------------- Update voltages ------------------------------------

        if (GetCurrentState()!=Feedback::State::kCalibrated) // WaitingForData, InProgress
        {
            if (fDimBias.state()!=BIAS::State::kRamping)
            {
                DimClient::sendCommandNB("BIAS_CONTROL/SET_ALL_CHANNELS_VOLTAGE",
                                         vec.data(), BIAS::kNumChannels*sizeof(float));

                UdrpAvg /= 320;
                UdrpRms /= 320;
                UdrpRms -= UdrpAvg*UdrpAvg;
                UdrpRms  = UdrpRms<0 ? 0 : sqrt(UdrpRms);

                ostringstream msg;
                msg << fixed;
                msg << setprecision(2) << "Sending U: dU(" << fTemp << "degC)="
                    << setprecision(3) << fTempOffsetAvg << "V+-" << fTempOffsetRms << "  Udrp="
                    << UdrpAvg << "V+-" << UdrpRms;
                msg.unsetf(ios_base::floatfield);
                msg << " Unom=" << overvoltage << "V Uov=" << (num[0]+num[1]>0?(avg[0]+avg[1])/(num[0]+num[1]):0) << " [N=" << Ndev[0] << "/" << Ndev[1] << "/" << Ndev[2] << "]";
                Info(msg);
            }
        }
        else
        {
            if (fDimBias.state()==BIAS::State::kVoltageOn)
            {
                ostringstream msg;
                msg << setprecision(4) << "Current status: dU(" << fTemp << "degC)=" << fTempOffsetAvg << "V+-" << fTempOffsetRms << ", Unom=" << overvoltage << "V, Uov=" << (num[0]+num[1]>0?(avg[0]+avg[1])/(num[0]+num[1]):0) << " [N=" << Ndev[0] << "/" << Ndev[1] << "/" << Ndev[2] << "]";
                Info(msg);
            }

        }

        if (GetCurrentState()==Feedback::State::kInProgress &&
            fDimBias.state()==BIAS::State::kRamping)
            return GetCurrentState();

        // --------------------------------- Console out --------------------------------------

        if (num[0]>0 && num[1]>0 && fIsVerbose && !fDimBias.state()==BIAS::State::kRamping)
        {
            sort(med[0].begin(), med[0].begin()+num[0]);
            sort(med[1].begin(), med[1].begin()+num[1]);

            ostringstream msg;
            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);
        }

        // ---------------------------- Calibrated Currents -----------------------------------

        if (num[2]>0)
        {
            data.Iavg /= num[2];
            data.Irms /= num[2];
            data.Irms -= data.Iavg*data.Iavg;

            data.N = num[2];
            data.Irms = data.Irms<0 ? 0: sqrt(data.Irms);

            sort(med[2].data(), med[2].data()+num[2]);

            data.Imed = num[2]%2 ? med[2][num[2]/2] : (med[2][num[2]/2-1]+med[2][num[2]/2])/2;

            for (int i=0; i<num[2]; i++)
                med[2][i] = fabs(med[2][i]-data.Imed);

            sort(med[2].data(), med[2].data()+num[2]);

            data.Idev = med[2][uint32_t(0.682689477208650697*num[2])];

            data.Tdiff = evt.GetTime().UnixTime()-fTimeCalib.UnixTime();

            // FIXME:
            //  + Current overvoltage
            //  + Temp offset
            //  + User offset
            //  + Command overvoltage
            fDimCurrents.setQuality(GetCurrentState());
            fDimCurrents.setData(&data, sizeof(dim_data));
            fDimCurrents.Update(evt.GetTime());
        }

        return GetCurrentState()==Feedback::State::kCalibrated ? Feedback::State::kCalibrated : Feedback::State::kInProgress;
    }

    // ======================================================================

    int Print() const
    {
        Out() << fDim << 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();
    }

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

        fIsVerbose = evt.GetBool();

        return GetCurrentState();
    }

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

        fCurrentRequestInterval = evt.GetUShort();

        Info("New current request interval: "+to_string(fCurrentRequestInterval)+"ms");

        return GetCurrentState();
    }

    int Calibrate()
    {
        if (fDimBias.state()!=BIAS::State::kVoltageOff)
        {
            Warn("Calibration can only be started when biasctrl is in state VoltageOff.");
            return GetCurrentState();
        }

        Message("Starting calibration (ignore="+to_string(fNumCalibIgnore)+", N="+to_string(fNumCalibRequests)+")");

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

        fBiasDac.assign(BIAS::kNumChannels, 0);

        fCalibStep = 3;
        fTimeCalib = Time();

        Dim::SendCommandNB("BIAS_CONTROL/SET_GLOBAL_DAC", uint32_t(256+512*fCalibStep));

        return Feedback::State::kCalibrating;
    }

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

        if (fDimBias.state()==BIAS::State::kRamping)
        {
            Warn("Feedback can not be started when biasctrl is in state Ramping.");
            return GetCurrentState();
        }

        fUserOffset = evt.GetFloat();

        fCursorCur = 0;

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

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

        return Feedback::State::kWaitingForData;
    }

    int StopFeedback()
    {
        if (GetCurrentState()==Feedback::State::kCalibrating)
            return Feedback::State::kConnected;

        if (GetCurrentState()>Feedback::State::kCalibrated)
            return Feedback::State::kCalibrated;

        return GetCurrentState();
    }

    bool LoadOffsets(const string &file)
    {
        vector<double> data(416);

        ifstream fin(file);

        int cnt = 0;
        while (fin && cnt<320)
            fin >> data[cnt++];

        if (cnt!=320)
        {
            Error("Reading offsets from "+file+" failed [N="+to_string(cnt-1)+"]");
            return false;
        }

        fVoltOffset = data;

        fDimOffsets.Update(fVoltOffset);

        Info("New voltage offsets loaded from "+file);
        return true;

    }

    int LoadOffset(const EventImp &evt)
    {
        LoadOffsets(evt.GetText());
        return GetCurrentState();
    }

    int ResetOffset()
    {
        fVoltOffset.assign(416, 0);

        fDimOffsets.Update(fVoltOffset);

        Info("Voltage offsets resetted.");
        return GetCurrentState();
    }


    int Execute()
    {
        if (!fDim.online())
            return Feedback::State::kDimNetworkNA;

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

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

        // Not all subsystems are yet connected
        if (!bias || !fsc)
            return Feedback::State::kConnecting;

        if (GetCurrentState()<Feedback::State::kCalibrating)
            return Feedback::State::kConnected;

        if (GetCurrentState()==Feedback::State::kConnected)
            return GetCurrentState();
        if (GetCurrentState()==Feedback::State::kCalibrating)
            return GetCurrentState();

        // kCalibrated, kWaitingForData, kInProgress

        if (fDimBias.state()==BIAS::State::kVoltageOn || (fDimBias.state()==BIAS::State::kVoltageOff && GetCurrentState()==Feedback::State::kWaitingForData))
        {
            static Time past;
            if (fCurrentRequestInterval>0 && Time()-past>boost::posix_time::milliseconds(fCurrentRequestInterval))
            {
                Dim::SendCommandNB("BIAS_CONTROL/REQUEST_STATUS");
                past = Time();
            }
        }

        return GetCurrentState();
    }

public:
    StateMachineFeedback(ostream &out=cout) : StateMachineDim(out, "FEEDBACK"),
        fIsVerbose(false), 
        //---
        fDimFSC("FSC_CONTROL"),
        fDimBias("BIAS_CONTROL"),
        //---
        fDimCalibration("FEEDBACK/CALIBRATION", "F:416;F:416;F:416;F:416",
                        "Current offsets"
                        "|Avg[uA]:Average offset at dac=256+5*512"
                        "|Rms[uA]:Rms of Avg"
                        "|R[Ohm]:Measured calibration resistor"
                        "|U[V]:Corresponding voltage reported by biasctrl"),
        fDimCalibration2("FEEDBACK/CALIBRATION_STEPS", "I:1;F:416;F:416;F:416",
                        "Calibration of the R8 resistor"
                        "|DAC[dac]:DAC setting"
                        "|U[V]:Corresponding voltages reported by biasctrl"
                        "|Iavg[uA]:Averaged measured current"
                        "|Irms[uA]:Rms measured current"),
        fDimCalibrationR8("FEEDBACK/CALIBRATION_R8", "F:416;F:416",
                          "Calibration of R8"
                          "|DeltaI[uA]:Average offset"
                          "|R8[uA]:Measured effective resistor R8"),
        fDimCurrents("FEEDBACK/CALIBRATED_CURRENTS", "F:416;F:1;F:1;F:1;F:1;I:1;F:1;F:416;F:1;F:1",
                     "Calibrated currents"
                     "|I[uA]:Calibrated currents per pixel"
                     "|I_avg[uA]:Average calibrated current (N channels)"
                     "|I_rms[uA]:Rms of calibrated current (N channels)"
                     "|I_med[uA]:Median calibrated current (N channels)"
                     "|I_dev[uA]:Deviation of calibrated current (N channels)"
                     "|N[uint16]:Number of valid values"
                     "|T_diff[s]:Time difference to calibration"
                     "|U_ov[V]:Calculated overvoltage"
                     "|U_nom[V]:Nominal overvoltage"
                     "|dU_temp[V]:Correction calculated from temperature"
                    ),
        fDimOffsets("FEEDBACK/OFFSETS", "F:416",
                    "Offsets operation voltages"
                    "|U[V]:Offset per bias channels"),
        fVoltOffset(416),
        fCurrentRequestInterval(0),
        fNumCalibIgnore(30),
        fNumCalibRequests(300)
    {
        fDim.Subscribe(*this);
        fDimFSC.Subscribe(*this);
        fDimBias.Subscribe(*this);

        fDimBias.SetCallback(bind(&StateMachineFeedback::HandleBiasStateChange, this));

        Subscribe("BIAS_CONTROL/CURRENT")
            (bind(&StateMachineFeedback::HandleBiasCurrent, this, placeholders::_1));
        Subscribe("BIAS_CONTROL/VOLTAGE")
            (bind(&StateMachineFeedback::HandleBiasVoltage, this, placeholders::_1));
        Subscribe("BIAS_CONTROL/DAC")
            (bind(&StateMachineFeedback::HandleBiasDac,     this, placeholders::_1));
        Subscribe("BIAS_CONTROL/NOMINAL")
            (bind(&StateMachineFeedback::HandleBiasNom,     this, placeholders::_1));
        Subscribe("FSC_CONTROL/BIAS_TEMP")
            (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",
                     "Either biasctrl or fscctrl not connected.");
        AddStateName(Feedback::State::kConnected, "Connected",
                     "biasctrl and fscctrl are available and connected with their hardware.");

        AddStateName(Feedback::State::kCalibrating, "Calibrating",
                     "Bias crate calibrating in progress.");
        AddStateName(Feedback::State::kCalibrated, "Calibrated",
                     "Bias crate calibrated.");

        AddStateName(Feedback::State::kWaitingForData, "WaitingForData",
                     "Current control started, waiting for valid temperature and current data.");
        AddStateName(Feedback::State::kInProgress, "InProgress",
                     "Current control in progress.");


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

        AddEvent("CALIBRATE", Feedback::State::kConnected, Feedback::State::kCalibrated)
            (bind(&StateMachineFeedback::Calibrate, this))
            ("");

        AddEvent("START", "F:1", Feedback::State::kCalibrated)
            (bind(&StateMachineFeedback::Start, this, placeholders::_1))
            ("Start the current/temperature control loop"
             "|Uov[V]:Overvoltage to be applied (standard value is 1.1V)");

        AddEvent("STOP")
            (bind(&StateMachineFeedback::StopFeedback, this))
            ("Stop any control loop");

        AddEvent("LOAD_OFFSETS", "C", Feedback::State::kConnected, Feedback::State::kCalibrated)
            (bind(&StateMachineFeedback::LoadOffset, this, placeholders::_1))
            ("");
        AddEvent("RESET_OFFSETS", Feedback::State::kConnected, Feedback::State::kCalibrated)
            (bind(&StateMachineFeedback::ResetOffset, this))
            ("");


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

        // Verbosity commands
        AddEvent("SET_VERBOSE", "B:1")
            (bind(&StateMachineFeedback::SetVerbosity, this, placeholders::_1))
            ("set verbosity state"
             "|verbosity[bool]:disable or enable verbosity when calculating overvoltage");
    }

    int EvalOptions(Configuration &conf)
    {
        fIsVerbose = !conf.Get<bool>("quiet");

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

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

        if (conf.Has("offset-file"))
            LoadOffsets(conf.Get<string>("offset-file"));

        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()
        ("quiet,q", po_bool(true), "Disable printing more information on average overvoltagecontents of all received messages (except dynamic data) in clear text.")
        ("pixel-map-file",      var<string>()->required(), "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")
        ("temp-coefficient",    var<double>()->required(), "Temp. coefficient [V/K]")
        ("offset-file",         var<string>(), "File with operation voltage offsets")
        ;

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