#include <functional>

#include <boost/bind.hpp>

#include "Dim.h"
#include "Event.h"
#include "Shell.h"
#include "StateMachineDim.h"
#include "ConnectionUSB.h"
#include "Configuration.h"
#include "Console.h"
#include "Converter.h"
#include "PixelMap.h"

#include "tools.h"

#include "LocalControl.h"
#include "HeadersBIAS.h"

namespace ba    = boost::asio;
namespace bs    = boost::system;
namespace dummy = ba::placeholders;

using namespace std::placeholders;
using namespace std;
using namespace BIAS;

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

class ConnectionBias : public ConnectionUSB
{
    boost::asio::deadline_timer fSyncTimer;
    boost::asio::deadline_timer fRampTimer;
    boost::asio::deadline_timer fUpdateTimer;

    vector<uint8_t> fBuffer;
    vector<uint8_t> fBufferRamp;
    vector<uint8_t> fBufferUpdate;

    bool fIsVerbose;

    vector<uint16_t> fVoltCmd;     // Current command voltage in DAC units (12bit = 90V)
    vector<uint16_t> fVoltGapd;

    vector<bool>     fPresent;

    int64_t fWrapCounter;
    int64_t fSendCounter;

    int16_t fGlobalVoltCmd;      // Command value to be reached
//    uint16_t fExpertVoltRef;      // Command value to be reached

    int16_t fRampStep;
    int16_t fRampTime;

    uint16_t fUpdateTime;
    uint16_t fSyncTime;

    bool fIsInitializing;
    bool fIsRamping;
    bool fWaitingForAnswer;

    vector<uint64_t> fCounter;

protected:
    vector<uint16_t> fVolt;        // Current voltage in DAC units (12bit = 90V)
    vector<uint16_t> fVoltRef;     // Current reference voltage in DAC units (12bit = 90V)

    vector<int16_t>  fCurrent;     // Current in ADC units (12bit = 5mA)

    uint16_t fVoltMaxAbs;
    uint16_t fVoltMaxRel;

    virtual void UpdateA()
    {
    }

    virtual void UpdateV()
    {
    }

private:
    bool CheckChDac(const string &cmd, uint16_t dac, uint16_t ch=0)
    {
        ostringstream str;
        str << cmd << " - ";

        if (ch>=kNumChannels)
        {
            str << "Channel " << ch << " out of range [0,416].";
            Error(str);
            return false;
        }

        if (dac>kMaxDac)
        {
            str << "DAC value " << dac << " out of range [0,4095].";
            Error(str);
            return false;
        }
        if (dac>fVoltMaxAbs)
        {
            str << "DAC value " << dac << " exceeds allowed absolute maximum of " << fVoltMaxAbs;
            Error(str);
            return false;
        }
        if (dac>fVoltGapd[ch]+fVoltMaxRel)
        {
            str << "DAC value " << dac << " exceeds allowed channel maximum of " << fVoltGapd[ch] << " + " << fVoltMaxRel;
            Error(str);
            return false;
        }

        return true;
    }

    vector<char> GetCmd(uint16_t board, uint16_t channel, Command_t cmd, uint16_t dac=0)
    {
        vector<char> data(3);

        /*
        if (board>kNumBoards)
            return;
        if (channel>kNumChannelsPerBoard)
            return;
        if (dac>0xfff)
            return;
        */

        data[0] = (cmd<<5) | (board<<1) | (((channel&16)>>4) & 1);
        data[1] = (channel<<4) | (dac>>8);
        data[2] =  dac&0xff;

        return data;
    }

    vector<char> GetCmd(Command_t cmd, uint16_t id=0, uint16_t dac=0)
    {
        const unsigned int board   = id/kNumChannelsPerBoard;
        const unsigned int channel = id%kNumChannelsPerBoard;

        return GetCmd(board, channel, cmd, dac);
    }

    bool CheckMessageLength(int received, int expected, const string &msg)
    {
        if (received==expected)
            return true;

        ostringstream str;
        str << msg << ": Expected " << expected << " bytes in answer, but got " << received << endl;
        Error(str);

        PostClose(false);
        return false;
    }

    bool EvalAnswer(const uint8_t *answer, uint16_t id, int command)
    {
        answer += id*3;

        const uint16_t status = (answer[0]>>7)&1;
        const uint16_t wrap   = (answer[0]>>4)&7;
        const uint16_t ddd    = ((uint16_t(answer[0])&0xf)<<8) | answer[1];
        const uint16_t error  = (answer[2]>>4)&0xf;
        const uint16_t board  =  answer[2]&0xf;

        // 0x10 00 7f
        //   status = 0
        //   wrap   = 1
        //   ddd    = 0
        //   error  = not present
        //   board  = 15

        /*
        Out() << dec << setw(2) << board << '|' << wrap << " ";
        if (id%8==7)
            Out() << endl;
            */

        if (fWrapCounter>=0)
        {
            if ((fWrapCounter+1)%8 != wrap)
            {
                ostringstream msg;
                msg << "Corrupted answer (id=" << id << "): received wrap counter " << wrap << " doesn't match last received counter " << fWrapCounter << ".";
                Error(msg);
                return false;
            }
        }

        fWrapCounter = wrap;

        if (command==kSynchronize)
        {
            ostringstream msg;
            msg << hex << setfill('0');
            msg << "Initial answer received: 0x";
            msg << setw(2) << (int)answer[2];
            msg << setw(2) << (int)answer[1];
            msg << setw(2) << (int)answer[0];
            Message(msg);

            if (status!=0 || ddd!=0 || error!=0 || board!=0)
            {
                Warn("Initial answer doesn't seem to be a reset as naively expected.");

                //ostringstream msg;
                //msg << hex << setfill('0');
                //msg << "S=" << status << " D=" << ddd << " E=" << error << " B=" << board;
                //Message(msg);
            }

            fSendCounter = wrap;

            msg.str("");
            msg << "Setting fSendCounter to " << wrap;
            Info(msg);

            return true;
        }

        if (error==0x8) // No device
        {
            Message("Reset button on crate pressed!");
            SetZero();
            return true;
        }

        if (command==kCmdReset)
        {
            if (status==0 && ddd==0 && error==0 && board==0)
            {
                Message("Reset successfully executed.");
                return true;
            }

            Warn("Answer to 'reset' command contains unexpected data.");
            return false;
        }

        if (command==kCmdGlobalSet)
        {
            if (status==0 && ddd==0 && error==0 && board==0)
            {
                for (int i=0; i<kNumChannels; i++)
                    fVolt[i] = fGlobalVoltCmd;

                fGlobalVoltCmd = -1;

                return true;
            }

            Warn("Answer to 'global set' command contains unexpected data.");
            return false;
        }

        if ((command&0xff)==kExpertChannelSet)
            id = command>>8;

        const int cmd = command&3;

        if (cmd==kCmdRead || cmd==kCmdChannelSet)
        {
            if (board!=id/kNumChannelsPerBoard)
            {
                ostringstream out;
                out << "Talked to board " << id/kNumChannelsPerBoard << ", but got answer from board " <<  board << ".";
                Error(out);
                return false;
            }

            // Not present
            if (error==0x7 || error==0xf)
            {
                fPresent[board] = false;
                fCurrent[id]    = 0x8000;
                return true;
            }

            // There is no -0 therefore we make a trick and replace it by -1.
            // This is not harmfull, because typical zero currents are in the
            // order of one to three bits anyway and they are never stable.
            fCurrent[id]    = status ? -(ddd==0?1:ddd) : ddd;
            fPresent[board] = true;
        }

        if (cmd==kCmdChannelSet)
            fVolt[id] = fVoltCmd[id];

        return true;

    }

private:
    void HandleReceivedData(const vector<uint8_t> &buf, size_t bytes_received, int command, int send_counter)
    {
        // Now print the received message if requested by the user
        if (fIsVerbose/* && command!=kUpdate*/)
        {
            Out() << endl << kBold << dec << "Data received (size=" << bytes_received << "):" << endl;
            Out() << " Command=" << command << " fWrapCounter=" << fWrapCounter << " fSendCounter=" << fSendCounter << " fIsInitializing=" << fIsInitializing << " fIsRamping=" << fIsRamping;
            Out() << hex << setfill('0');

            for (size_t i=0; i<bytes_received/3; i++)
            {
                if (i%8==0)
                    Out() << '\n' << setw(2) << bytes_received/24 << "| ";

                Out() << setw(2) << uint16_t(buf[i*3+2]);
                Out() << setw(2) << uint16_t(buf[i*3+1]);
                Out() << setw(2) << uint16_t(buf[i*3+0]) << " ";
            }
            Out() << endl;
        }

        const int cmd = command&0xf;

        // Check the number of received_byted according to the answer expected
        if ((cmd==kSynchronize      && !CheckMessageLength(bytes_received, 3,                "Synchronization")) ||
            (cmd==kCmdReset         && !CheckMessageLength(bytes_received, 3,                "CmdReset"))        ||
            (cmd==kCmdRead          && !CheckMessageLength(bytes_received, 3*kNumChannels,   "CmdRead"))         ||
            (cmd==kCmdChannelSet    && !CheckMessageLength(bytes_received, 3*kNumChannels,   "CmdChannelSet"))   ||
            (cmd==kExpertChannelSet && !CheckMessageLength(bytes_received, 3,                "CmdExpertChannelSet")))
            return;

        // Now evaluate the whole bunch of messages
        for (size_t i=0; i<bytes_received/3; i++)
        {
            if (!EvalAnswer(buf.data(), i, command))
            {
                PostClose(false);
                return;
            }
        }

        if (command==kSynchronize)
        {
            Message("Stream successfully synchronized.");
            fIsInitializing = false;

            // Cancel sending of the next 0
            fSyncTimer.cancel();
            fCounter[0]++;

            // Start continous reading of all channels
            ScheduleUpdate(100);
            return;
        }

        if (send_counter%8 != fWrapCounter)
        {
            ostringstream msg;
            msg << "Corrupted answer: received wrap counter " << fWrapCounter  << " is not send counter " << send_counter << "%8.";
            Error(msg);
            PostClose(false);
        }


        // Check if new values have been received
        if (cmd==kCmdRead || cmd==kCmdChannelSet || cmd==kExpertChannelSet)
        {
            UpdateV();
            UpdateA();
        }

        // ----- Take action depending on what is going on -----

        if (command==kCmdReset)
        {
            Message("Reset command successfully answered... restarting automatic readout.");

            fCounter[1]++;

            // Re-start cyclic reading of values after a short time
            // to allow the currents to become stable. This ensures that
            // we get an update soon but wait long enough to get reasonable
            // values
            fUpdateTimer.cancel();
            ScheduleUpdate(100);
        }

        if (command==kResetChannels)
        {
            ExpertReset(false);
            fCounter[5]++;
        }

        if (command==kUpdate)
        {
            ScheduleUpdate(fUpdateTime);
            fCounter[2]++;
        }

        // If we are ramping, schedule a new ramp step
        if (command==kCmdChannelSet && fIsRamping)
        {
            bool oc = false;
            for (int ch=0; ch<kNumChannels; ch++)
                if (fPresent[ch/kNumChannelsPerBoard] && fCurrent[ch]<0)
                    oc = true;

            if (oc)
            {
                Warn("OverCurrent detected - ramping stopped.");
                fIsRamping = false;
            }
            else
                ScheduleRampStep();

            fCounter[3]++;
        }

        if (command==kCmdRead)
            fCounter[4]++;

        if ((command&0xff)==kExpertChannelSet)
            fCounter[6]++;

        if (command==kCmdGlobalSet)
            fCounter[7]++;

    }

    void HandleReceivedData(const bs::error_code& err, size_t bytes_received, int command, int send_counter)
    {
        // Do not schedule a new read if the connection failed.
        if (bytes_received==0 || err)
        {
            if (err==ba::error::eof)
                Warn("Connection closed by remote host (BIAS).");

            // 107: Transport endpoint is not connected (bs::error_code(107, bs::system_category))
            // 125: Operation canceled
            if (err && err!=ba::error::eof &&                     // Connection closed by remote host
                err!=ba::error::basic_errors::not_connected &&    // Connection closed by remote host
                err!=ba::error::basic_errors::operation_aborted)  // Connection closed by us
            {
                ostringstream str;
                str << "Reading from " << URL() << ": " << err.message() << " (" << err << ")";// << endl;
                Error(str);
            }
            PostClose(false);//err!=ba::error::basic_errors::operation_aborted);
            return;
        }

        // Check if the number of received bytes is correctly dividable by 3
        // This check should never fail - just for sanity
        if (bytes_received%3)
        {
            Error("Number of received bytes not a multiple of 3, can't read data.");
            PostClose(false);
            return;
        }

        // We have three different parallel streams:
        //  1) The setting of voltages due to ramping
        //  2) The cynclic request of the currents
        //  3) Answers to commands
        // For each of these three streams an own buffer is needed, otherwise
        // a buffer which is filled in the background might overwrite
        // a buffer which is currently evaluated. In all other programs
        // this is no problem because the boards don't answer and if
        // they do the answer identifies itself. Consequently,
        // there is always only one async_read in progress. Here we have
        // three streams which need to be connected somehow to the
        // commands.

        // Maybe a better possibility would be to setup a command
        // queue (each command will be queued in a buffer)
        // and whenever an answer has been received, a new async_read is
        // scheduled.
        // Build a command queue<pair<command, vector<char>>>
        ///  This replaces the send counter and the command argument
        //   in handleReceivedData

        switch (command&0xff)
        {
        case kSynchronize:
        case kCmdReset:
        case kExpertChannelSet:
        case kCmdGlobalSet:
        case kResetChannels:
        case kCmdRead:
            fWaitingForAnswer = false;
            HandleReceivedData(fBuffer, bytes_received, command, send_counter);
            return;

        case kCmdChannelSet:
            HandleReceivedData(fBufferRamp, bytes_received, command, send_counter);
            return;

        case kUpdate:
            HandleReceivedData(fBufferUpdate, bytes_received, command, send_counter);
            return;
        }
    }

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

    void HandleSyncTimer(int counter, const bs::error_code &error)
    {
        if (error==ba::error::basic_errors::operation_aborted)
        {
            if (fIsInitializing)
                Warn("Synchronization aborted...");
            else
                Info("Synchronization successfull.");
            return;
        }

        if (error)
        {
            ostringstream str;
            str << "Synchronization timer: " << error.message() << " (" << error << ")";// << endl;
            Error(str);

            PostClose(false);
            return;
        }

        if (!is_open())
        {
            Warn("Synchronization in progress, but disconnected.");
            return;
        }

        ostringstream msg;
        msg << "Synchronization time expired (" << counter << ")";
        Info(msg);

        if (fIsInitializing)
        {
            PostMessage("\0", 1);

            if (counter==2)
            {
                Error("Synchronization attempt timed out.");
                PostClose(false);
                return;
            }

            ScheduleSync(counter+1);
            return;
        }

        Info("Synchronisation successfull.");
    }

    void ScheduleSync(int counter=0)
    {
        fSyncTimer.expires_from_now(boost::posix_time::milliseconds(fSyncTime));
        fSyncTimer.async_wait(boost::bind(&ConnectionBias::HandleSyncTimer, this, counter, dummy::error));
    }

    // This is called when a connection was established
    void ConnectionEstablished()
    {
        // Reset everything....
        fSendCounter    = -1;
        fWrapCounter    = -1;
        fGlobalVoltCmd  = -1;
        fIsInitializing = true;

        fVolt.assign(   kNumChannels, 0);
        fVoltRef.assign(kNumChannels, 0);
        fVoltCmd.assign(kNumChannels, 0);

        // Send a single 0 (and possible two consecutive 0's
        // to make sure we are in sync with the device)
        PostMessage("\0", 1);
        AsyncRead(ba::buffer(fBuffer, 3), kSynchronize, 0);//++fSendCounter);
        fWaitingForAnswer = true;

        // Wait for some time before sending the next 0
        ScheduleSync();
    }

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

    void HandleUpdateTimer(const bs::error_code &error)
    {
        if (error==ba::error::basic_errors::operation_aborted)
        {
            Warn("Update timer aborted...");
            fIsRamping = false;
            return;
        }

        if (error)
        {
            ostringstream str;
            str << "Update timer: " << error.message() << " (" << error << ")";// << endl;
            Error(str);

            PostClose(false);
            return;
        }

        if (!is_open())
            return;

        if (fIsRamping)
            ScheduleUpdate(fUpdateTime);
        else
            ReadAllChannels(true);
    }

    void ScheduleUpdate(int millisec)
    {
        fUpdateTimer.expires_from_now(boost::posix_time::milliseconds(millisec));
        fUpdateTimer.async_wait(boost::bind(&ConnectionBias::HandleUpdateTimer, this, dummy::error));
    }

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

    void SetAllChannels(const vector<uint16_t> &dac, bool special=false)
    {
        vector<char> data;
        data.reserve(kNumChannels*3);

        for (int ch=0; ch<kNumChannels; ch++)
        {
            // FIXME: dac[ch] += calib_offset
            const vector<char> cmd = GetCmd(kCmdChannelSet, ch, dac[ch]);
            data.insert(data.end(), cmd.begin(), cmd.end());

            fVoltCmd[ch] = dac[ch];
        }

        fSendCounter += kNumChannels;

        PostMessage(data);
        AsyncRead(ba::buffer(special ? fBuffer : fBufferRamp, kNumChannels*3),
                  special ? kResetChannels : kCmdChannelSet, fSendCounter);

        if (special)
            fWaitingForAnswer = true;
    }

    uint16_t RampOneStep(uint16_t ch)
    {
        if (fVoltRef[ch]>fVolt[ch])
            return fVolt[ch]+fRampStep>fVoltRef[ch] ? fVoltRef[ch] : fVolt[ch]+fRampStep;

        if (fVoltRef[ch]<fVolt[ch])
            return fVolt[ch]-fRampStep<fVoltRef[ch] ? fVoltRef[ch] : fVolt[ch]-fRampStep;

        return fVolt[ch];
    }

    bool RampOneStep()
    {
        if (fRampTime<0)
        {
            Warn("Ramping step time not yet set... ramping not started.");
            return false;
        }
        if (fRampStep<0)
        {
            Warn("Ramping step not yet set... ramping not started.");
            return false;
        }

        vector<uint16_t> dac(kNumChannels);

        bool identical = true;
        for (int ch=0; ch<kNumChannels; ch++)
        {
            dac[ch] = RampOneStep(ch);
            if (dac[ch]!=fVolt[ch] && fPresent[ch/kNumChannelsPerBoard])
                identical = false;
        }

        SetAllChannels(dac);

        if (identical)
            Info("Ramping: target values reached.");

        return !identical;
    }

    void HandleRampTimer(const bs::error_code &error)
    {
        if (error==ba::error::basic_errors::operation_aborted)
        {
            Warn("Ramping aborted...");
            fIsRamping = false;
            return;
        }

        if (error)
        {
            ostringstream str;
            str << "Ramping timer: " << error.message() << " (" << error << ")";// << endl;
            Error(str);

            PostClose(false);
            return;
        }

        if (!is_open())
        {
            Warn("Ramping in progress, but disconnected.");
            return;
        }

        if (!fIsRamping)
        {
            Error("Ramp handler called although no ramping in progress.");
            return;
        }

        // Check whether the deadline has passed. We compare the deadline
        // against the current time since a new asynchronous operation
        // may have moved the deadline before this actor had a chance
        // to run.
        if (fRampTimer.expires_at() > ba::deadline_timer::traits_type::now())
            return;

        fIsRamping = RampOneStep();
    }

    void ScheduleRampStep()
    {
        fRampTimer.expires_from_now(boost::posix_time::milliseconds(fRampTime));
        fRampTimer.async_wait(boost::bind(&ConnectionBias::HandleRampTimer, this, dummy::error));
    }

public:
    ConnectionBias(ba::io_service& ioservice, MessageImp &imp) : ConnectionUSB(ioservice, imp()),
        fSyncTimer(ioservice),
        fRampTimer(ioservice),
        fUpdateTimer(ioservice),
        fBuffer(3*kNumChannels),
        fBufferRamp(3*kNumChannels),
        fBufferUpdate(3*kNumChannels),
        fIsVerbose(false),
        fVoltCmd(kNumChannels),
        fVoltGapd(kNumChannels),
        //fRefCurrent(kNumChannels),
        fPresent(kNumBoards),
        fRampStep(-1),
        fRampTime(-1),
        fUpdateTime(3000),
        fSyncTime(333),
        fIsRamping(false),
        fWaitingForAnswer(false),
        fCounter(8),
        fVolt(kNumChannels),
        fVoltRef(kNumChannels),
        fCurrent(kNumChannels)
    {
        SetLogStream(&imp);
    }

    void OverCurrentReset()
    {
        if (fWaitingForAnswer)
        {
            Error("Answer on last command not yet received.");
            return;
        }

        if (fIsRamping)
        {
            Warn("OverCurrentReset - Ramping in progres.");
            RampStop();
        }

        vector<uint16_t> dac(kNumChannels);

        for (int ch=0; ch<kNumChannels; ch++)
            dac[ch] = fCurrent[ch]<0 ? 0 : fVolt[ch];

        SetAllChannels(dac, true);
    }

    void ReadAllChannels(bool special = false)
    {
        if (!special && fWaitingForAnswer)
        {
            Error("Answer on last command not yet received.");
            return;
        }

        vector<char> data;
        data.reserve(kNumChannels*3);

        for (int ch=0; ch<kNumChannels; ch++)
        {
            const vector<char> cmd = GetCmd(kCmdRead, ch);
            data.insert(data.end(), cmd.begin(), cmd.end());
        }

        fSendCounter += kNumChannels;

        PostMessage(data);
        AsyncRead(ba::buffer(special ? fBufferUpdate : fBuffer, kNumChannels*3),
                  special ? kUpdate : kCmdRead, fSendCounter);

        if (!special)
            fWaitingForAnswer = true;
    }

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

    bool ChannelSetDac(uint16_t ch, uint16_t dac)
    {
        if (!CheckChDac("ChannelSetDac", dac, ch))
            return false;

        fVoltRef[ch] = dac;

        if (!fIsRamping)
            fIsRamping = RampOneStep();

        return true;
    }

    bool ChannelSetVolt(uint16_t ch, double volt)
    {
        if (volt<0 || volt>90)
        {
            Error("ChannelSetVolt - Voltage out of range [0V,90V].");
            return false;
        }

        return ChannelSetDac(ch, volt*4096/90.);
    }
/*
    bool GlobalSetDac(uint16_t dac)
    {
        if (!CheckChDac("GlobalSetDac", dac))
            return false;

        for (size_t ch=0; ch<kNumChannels; ch++)
            fVoltRef[ch] = dac;

        if (!fIsRamping)
            fIsRamping = RampOneStep();

        return true;
    }

    bool GlobalSetVolt(float volt)
    {
        if (volt<0 || volt>90)
        {
            Error("GlobalSetVolt - Voltage out of range [0V,90V].");
            return false;
        }

        return GlobalSetDac(volt*4096/90);
    }
*/
    bool AddDac(const vector<int16_t> &dac)
    {
        if (dac.size()!=kNumChannels)
        {
            Error("AddDac - Wrong size of array.");
            return false;
        }

        for (size_t ch=0; ch<kNumChannels; ch++)
        {
            if (fVoltRef[ch]+dac[ch]>kMaxDac)
            {
                Error("AddDac - New voltage reference out of range.");
                return false;
            }

            if (fVoltRef[ch]+dac[ch]<0)
                fVoltRef[ch] = 0;
            else
                fVoltRef[ch] += dac[ch];
        }

        if (!fIsRamping)
            fIsRamping = RampOneStep();

        return true;
    }

    bool AddVolt(const vector<float> &offset)
    {
        vector<int16_t> dac(offset.size());

        for (size_t ch=0; ch<offset.size(); ch++)
        {
            if (offset[ch]<-90 || offset[ch]>90)
            {
                Error("AddVolt - Offset out of range [-90V,90V].");
                return false;
            }
            dac[ch] = offset[ch]*4096/90;
        }

        return AddDac(dac);
    }

    bool GlobalAddDac(int16_t offset)
    {
        return AddDac(vector<int16_t>(kNumChannels, offset));
    }

    bool GlobalAddVolt(float offset)
    {
        return AddVolt(vector<float>(kNumChannels, offset));
    }

    bool SetDac(const vector<int16_t> &dac)
    {
        if (dac.size()!=kNumChannels)
        {
            Error("SetDac - Wrong size of array.");
            return false;
        }

        for (size_t ch=0; ch<kNumChannels; ch++)
        {
            if (!CheckChDac("SetDac", dac[ch]))
                return false;

            fVoltRef[ch] = dac[ch];
        }

        if (!fIsRamping)
            fIsRamping = RampOneStep();

        return true;
    }

    bool SetVolt(const vector<float> &volt)
    {
        vector<int16_t> dac(volt.size());

        for (size_t ch=0; ch<volt.size(); ch++)
        {
            if (volt[ch]<0 || volt[ch]>90)
            {
                Error("SetVolt - Voltage out of range [0V,90V].");
                return false;
            }
            dac[ch] = volt[ch]*4096/90;
        }

        return SetDac(dac);
    }

    bool GlobalSetDac(int16_t dac)
    {
        return SetDac(vector<int16_t>(kNumChannels, dac));
    }

    bool GlobalSetVolt(float volt)
    {
        return SetVolt(vector<float>(kNumChannels, volt));
    }


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

    bool SetGapdVoltage(float offset)
    {
        if (offset<-90 || offset>90)
        {
            Error("SetGapdVoltage - Offset out of range [-90V,90V].");
            return false;
        }

        const int16_t dac = offset*4096/90;

        for (size_t ch=0; ch<kNumChannels; ch++)
            if (fVoltGapd[ch]+dac>kMaxDac)
            {
                Error("SetGapdVoltage - New voltage reference out of range.");
                return false;
            }

        for (size_t ch=0; ch<kNumChannels; ch++)
            fVoltRef[ch] = fVoltGapd[ch]+dac<0 ? 0 : fVoltGapd[ch]+dac;

        if (!fIsRamping)
            fIsRamping = RampOneStep();

        return true;
    }

    bool SetGapdReferenceCh(uint16_t ch)
    {
        if (!CheckChDac("SetGapdReferenceCh", fVoltGapd[ch], ch))
            return false;

        fVoltRef[ch] = fVoltGapd[ch];

        if (!fIsRamping)
            fIsRamping = RampOneStep();

        return true;
    }


    void SetZero()
    {
        for (size_t ch=0; ch<kNumChannels; ch++)
            fVoltRef[ch] = 0;

        if (!fIsRamping)
            fIsRamping = RampOneStep();
    }

    bool SetNewGapdVoltage(const vector<float> &volt)
    {
        if (volt.size()!=kNumChannels)
        {
            ostringstream out;
            out << "SetNewGapdVoltage - Given vector has " << volt.size() << " elements - expected " << kNumChannels << endl;
            Error(out);
            return false;
        }

        for (size_t i=0; i<kNumChannels; i++)
            fVoltGapd[i] = volt[i]*4096/90;

        return true;
    }

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

    void RampStop()
    {
        fRampTimer.cancel();
        fIsRamping = false;

        Message("Ramping stopped.");
    }

    void RampStart()
    {
        if (fIsRamping)
        {
            Warn("RampStart - Ramping in progress... ignored.");
            return;
        }

        fIsRamping = RampOneStep();
    }

    void SetRampTime(uint16_t val)
    {
        fRampTime = val;
    }

    void SetRampStep(uint16_t val)
    {
        fRampStep = val;
    }

    uint16_t GetRampStepVolt() const
    {
        return fRampStep*90./4096;
    }

    bool IsRamping() const { return fIsRamping; }

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

    void ExpertReset(bool expert_mode=true)
    {
        if (fWaitingForAnswer)
        {
            Error("Answer on last command not yet received.");
            return;
        }

        if (expert_mode)
            Warn("EXPERT MODE: Sending reset.");

        PostMessage(GetCmd(kCmdReset));
        AsyncRead(ba::buffer(fBuffer, 3), kCmdReset, ++fSendCounter);
        fWaitingForAnswer = true;
    }


    bool ExpertChannelSetDac(uint16_t ch, uint16_t dac)
    {
        if (fWaitingForAnswer)
        {
            Error("Answer on last command not yet received.");
            return false;
        }

        if (!CheckChDac("ExpertChannelSetDac", dac, ch))
            return false;

        fVoltCmd[ch] = dac;

        ostringstream msg;
        msg << "EXPERT MODE: Sending 'ChannelSet' (set ch " << ch << " to DAC=" << dac << ")";
        Warn(msg);

        // FIXME: dac += calib_offset
        PostMessage(GetCmd(kCmdChannelSet, ch, dac));
        AsyncRead(ba::buffer(fBuffer, 3), kExpertChannelSet|(ch<<8), ++fSendCounter);
        fWaitingForAnswer = true;

        return true;
    }

    bool ExpertChannelSetVolt(uint16_t ch, double volt)
    {
        return ExpertChannelSetDac(ch, volt*4096/90.);
    }

    bool ExpertGlobalSetDac(uint16_t dac)
    {
        if (fWaitingForAnswer)
        {
            Error("Answer on last command not yet received.");
            return false;
        }

        if (!CheckChDac("ExpertGlobalSetDac", dac))
            return false;

        if (fGlobalVoltCmd>=0)
        {
            Error("ExpertGlobalSetDac - Still waiting for previous global-set's answer.");
            return false;
        }

        fGlobalVoltCmd = dac;

        ostringstream msg;
        msg << "EXPERT MODE: Sending 'GlobalSet' (DAC=" << dac << ")";
        Warn(msg);

        PostMessage(GetCmd(kCmdGlobalSet, 0, dac));
        AsyncRead(ba::buffer(fBuffer, 3), kCmdGlobalSet, ++fSendCounter);
        fWaitingForAnswer = true;

        return true;
    }

    bool ExpertGlobalSetVolt(float volt)
    {
        return ExpertGlobalSetDac(volt*4096/90);
    }

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

    void SetVerbose(bool b)
    {
        fIsVerbose = b;
    }

    void PrintInfo()
    {
        Out() << endl << kBold << dec << '\n';
        Out() << "fWrapCounter    = " << fWrapCounter << '\n';
        Out() << "fSendCounter    = " << fSendCounter%8 << " (" << fSendCounter << ")" << '\n';
        Out() << "fIsInitializing = " << fIsInitializing << '\n';
        Out() << "fIsRamping      = " << fIsRamping << '\n';
        Out() << "Answer counter:" << '\n';
        Out() << " - Synchronization: " << fCounter[0] << '\n';
        Out() << " - Reset:           " << fCounter[1] << '\n';
        Out() << " - Request update:  " << fCounter[2] << '\n';
        Out() << " - Ramp step:       " << fCounter[3] << '\n';
        Out() << " - Read:            " << fCounter[4] << '\n';
        Out() << " - Reset channels:  " << fCounter[5] << '\n';
        Out() << " - Global set:      " << fCounter[7] << '\n';
        Out() << " - Channel set:     " << fCounter[6] << '\n' << endl;
    }

    void PrintLineA(int b, int ch)
    {
        Out() << setw(2) << b << "|";

        for (int c=ch; c<ch+8; c++)
        {
            const int id = c+kNumChannelsPerBoard*b;
            Out() << (fCurrent[id]<0?kRed:kGreen);
            Out() << " " << setw(7) << abs(fCurrent[id])*5000/4096.;
        }
        Out() << endl;

    }

    void PrintA()
    {
        Out() << dec << setprecision(2) << fixed << setfill(' ');
        for (int b=0; b<kNumBoards; b++)
        {
            if (!fPresent[b])
            {
                Out() << setw(2) << b << "-" << endl;
                continue;
            }

            PrintLineA(b,  0);
            PrintLineA(b,  8);
            PrintLineA(b, 16);
            PrintLineA(b, 24);
        }
    }

    void PrintLineV(int b, int ch)
    {
        Out() << setw(2) << b << "|";

        for (int c=ch; c<ch+4; c++)
        {
            const int id = c+kNumChannelsPerBoard*b;
            Out() << " ";
            Out() << (fVolt[id]==fVoltRef[id]?kGreen:kRed);
            Out() << setw(5) << fVolt[id]*90/4096. << '/';
            Out() << setw(5) << fVoltRef[id]*90/4096.;
        }
        Out() << endl;
    }

    void PrintV()
    {
        Out() << dec << setprecision(2) << fixed << setfill(' ');
        for (int b=0; b<kNumBoards; b++)
        {
            if (!fPresent[b])
            {
                Out() << setw(2) << b << "-" << endl;
                continue;
            }

            PrintLineV(b,  0);
            PrintLineV(b,  4);
            PrintLineV(b,  8);
            PrintLineV(b, 12);
            PrintLineV(b, 16);
            PrintLineV(b, 20);
            PrintLineV(b, 24);
            PrintLineV(b, 28);
        }
    }

    void PrintLineGapd(int b, int ch)
    {
        Out() << setw(2) << b << "|";

        for (int c=ch; c<ch+8; c++)
        {
            const int id = c+kNumChannelsPerBoard*b;
            Out() << " " << setw(5) << fVoltGapd[id]*90/4096.;
        }
        Out() << endl;
    }

    void PrintGapd()
    {
        Out() << dec << setprecision(2) << fixed << setfill(' ');
        for (int b=0; b<kNumBoards; b++)
        {
            if (!fPresent[b])
            {
                Out() << setw(2) << b << "-" << endl;
                continue;
            }

            PrintLineGapd(b,  0);
            PrintLineGapd(b,  8);
            PrintLineGapd(b, 16);
            PrintLineGapd(b, 24);
        }
    }

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

    void SetUpdateInterval(uint16_t val)
    {
        fUpdateTime = val;
    }

    void SetSyncDelay(uint16_t val)
    {
        fSyncTime = val;
    }

    void SetVoltMaxAbs(float max)
    {
        fVoltMaxAbs = max*4096/90;
        if (fVoltMaxAbs>4095)
            fVoltMaxAbs = 4095;
        if (max<0)
            fVoltMaxAbs = 0;
    }

    void SetVoltMaxRel(float max)
    {
        fVoltMaxRel = max*4096/90;
        if (fVoltMaxRel>4095)
            fVoltMaxRel = 4095;
        if (max<0)
            fVoltMaxRel = 0;
    }

    uint16_t GetVoltMaxAbs() const
    {
        return fVoltMaxAbs * 90./4096;
    }

    uint16_t GetVoltMaxRel() const
    {
        return fVoltMaxRel * 90./4096;
    }

/*
    void AdaptVoltages()
    {
        // Correct voltages according to current
        for (int i=0; i<kNumChannels; i++)
        {
            if (fVoltRef[i]==0 || fCurrent[i]<0 || fRefCurrent[i]<0)
                continue;

            // Calculate difference and convert ADC units to Amp
            // const double diffcur = (fRefCurrent[i]-fCurrent[i])*5000/4096
            //const int32_t diffcur = int32_t(fRefCurrent[i]-fCurrent[i])*5000;

            // Calculate voltage difference
            // #define RESISTOR 1000 // Ohm
            //const double diffvolt = diffcur*RESISTOR/1e6;

            // Calculate new vlaue by onverting voltage difference to DAC units
            //const int32_t dac = fRefVolt[i] + diffvolt*4096/90.0;
            SetVoltage(i, fRefVolt[i] + (fRefCurrent[i]-fCurrent[i])/18);
        }
    }

    void SetReferenceCurrent()
    {
        fRefCurrent = fCurrent;
    }
    */

    States_t GetStatus()
    {
        if (!IsConnected())
            return BIAS::kDisconnected;

        if (IsConnecting())
            return BIAS::kConnecting;

        if (fIsInitializing)
            return BIAS::kInitializing;

        if (fIsRamping)
            return BIAS::kRamping;

        for (int ch=0; ch<kNumChannels; ch++)
            if (fPresent[ch/kNumChannelsPerBoard] && fCurrent[ch]<0)
                return BIAS::kOverCurrent;

        for (int ch=0; ch<kNumChannels; ch++)
            if (fPresent[ch/kNumChannelsPerBoard] && fVolt[ch]!=fVoltRef[ch])
                return BIAS::kConnected;

        return BIAS::kAtReference;
    }
};

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

#include "DimDescriptionService.h"

class ConnectionDimBias : public ConnectionBias
{
private:

    DimDescribedService fDimCurrent;
    DimDescribedService fDimVoltage;

    void UpdateA()
    {
        fDimCurrent.Update(fCurrent);
    }

    void UpdateV()
    {
        vector<uint16_t> vec;
        vec.insert(vec.end(), fVolt.begin(),    fVolt.end());
        vec.insert(vec.end(), fVoltRef.begin(), fVoltRef.end());
        fDimVoltage.Update(vec);
    }


public:
    ConnectionDimBias(ba::io_service& ioservice, MessageImp &imp) :
        ConnectionBias(ioservice, imp),
        fDimCurrent("BIAS_CONTROL/CURRENT", "S:416", ""),
        fDimVoltage("BIAS_CONTROL/VOLTAGE", "S:416;S:416", "")
    {
    }

    // A B [C] [D] E [F] G H [I] J K [L] M N O P Q R [S] T U V W [X] Y Z
};

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

template <class T, class S>
class StateMachineBias : public T, public ba::io_service, public ba::io_service::work
{
    int Wrap(boost::function<void()> f)
    {
        f();
        return T::GetCurrentState();
    }

    function<int(const EventImp &)> Wrapper(function<void()> func)
    {
        return bind(&StateMachineBias::Wrap, this, func);
    }

    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 << ".";
        T::Fatal(msg);
        return false;
    }

private:
    S fBias;

    bool fExpertMode;

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

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

        fBias.SetGapdVoltage(evt.GetFloat());

        return T::GetCurrentState();
    }

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

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

        fBias.GlobalSetVolt(evt.GetFloat());

        return T::GetCurrentState();
    }

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

        fBias.GlobalSetDac(evt.GetUShort());

        return T::GetCurrentState();
    }

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

        fBias.GlobalAddVolt(evt.GetFloat());

        return T::GetCurrentState();
    }

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

        fBias.GlobalAddDac(evt.GetShort());

        return T::GetCurrentState();
    }

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

        fBias.GlobalAddVolt(-evt.GetFloat());

        return T::GetCurrentState();
    }

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

        fBias.GlobalAddDac(-evt.GetShort());

        return T::GetCurrentState();
    }

    int SetChannelVolt(const EventImp &evt)
    {
        if (!CheckEventSize(evt.GetSize(), "SetChannelVolt", 6))
            return false;

        fBias.ChannelSetVolt(evt.GetUShort(), evt.Get<float>(2));

        return T::GetCurrentState();
    }

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

        fBias.ChannelSetDac(evt.Get<uint16_t>(), evt.Get<uint16_t>(2));

        return T::GetCurrentState();
    }

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

        fBias.SetGapdReferenceCh(evt.GetUShort());

        return T::GetCurrentState();
    }


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

    int AddReferenceDac(const EventImp &evt)
    {
        if (!CheckEventSize(evt.GetSize(), "AddReferenceDac", 2*kNumChannels))
            return false;

        const int16_t *ptr = evt.Ptr<int16_t>();
        fBias.AddDac(vector<int16_t>(ptr, ptr+416));

        return T::GetCurrentState();
    }

    int AddReferenceVolt(const EventImp &evt)
    {
        if (!CheckEventSize(evt.GetSize(), "AddReferenceVolt", 4*kNumChannels))
            return false;

        const float_t *ptr = evt.Ptr<float>();
        fBias.AddVolt(vector<float>(ptr, ptr+416));

        return T::GetCurrentState();
    }

    int SetReferenceDac(const EventImp &evt)
    {
        if (!CheckEventSize(evt.GetSize(), "SetReferenceDac", 2*kNumChannels))
            return false;

        const int16_t *ptr = evt.Ptr<int16_t>();
        fBias.SetDac(vector<int16_t>(ptr, ptr+416));

        return T::GetCurrentState();
    }

    int SetReferenceVolt(const EventImp &evt)
    {
        if (!CheckEventSize(evt.GetSize(), "SetReferenceVolt", 4*kNumChannels))
            return false;

        const float_t *ptr = evt.Ptr<float>();
        fBias.SetVolt(vector<float>(ptr, ptr+416));

        return T::GetCurrentState();
    }

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

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

        fBias.ExpertGlobalSetVolt(evt.GetFloat());

        return T::GetCurrentState();
    }

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

        fBias.ExpertGlobalSetDac(evt.GetUShort());

        return T::GetCurrentState();
    }

    int ExpertSetChannelVolt(const EventImp &evt)
    {
        if (!CheckEventSize(evt.GetSize(), "ExpertSetChannelVolt", 6))
            return false;

        fBias.ExpertChannelSetVolt(evt.GetUShort(), evt.Get<float>(2));

        return T::GetCurrentState();
    }

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

        fBias.ExpertChannelSetDac(evt.Get<uint16_t>(), evt.Get<uint16_t>(2));

        return T::GetCurrentState();
    }

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

    int Disconnect()
    {
        // Close all connections
        fBias.PostClose(false);

        /*
         // Now wait until all connection have been closed and
         // all pending handlers have been processed
         poll();
         */

        return T::GetCurrentState();
    }

    int Reconnect(const EventImp &evt)
    {
        // Close all connections to supress the warning in SetEndpoint
        fBias.PostClose(false);

        // Now wait until all connection have been closed and
        // all pending handlers have been processed
        poll();

        if (evt.GetBool())
            fBias.SetEndpoint(evt.GetString());

        // Now we can reopen the connection
        fBias.PostClose(true);

        return T::GetCurrentState();
    }

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

        fBias.SetVerbose(evt.GetBool());

        return T::GetCurrentState();
    }

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

        fExpertMode = evt.GetBool();

        if (fExpertMode)
            T::Warn("Expert commands enabled -- please ensure that you EXACTLY know what you do. These commands can destroy the system.");

        return T::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();

        return fExpertMode && fBias.GetStatus()>=kConnected ?
            kExpertMode : fBias.GetStatus();
    }

public:
    StateMachineBias(ostream &out=cout) :
        T(out, "BIAS_CONTROL"), ba::io_service::work(static_cast<ba::io_service&>(*this)),
        fBias(*this, *this), fExpertMode(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.

        // State names
        T::AddStateName(kDisconnected, "Disconnected",
                        "Bias-power supply not connected via USB.");

        T::AddStateName(kConnecting, "Connecting",
                        "Trying to establish USB connection to bias-power supply.");

        T::AddStateName(kInitializing, "Initializing",
                        "USB connection to bias-power supply established, synchronizing USB stream.");

        T::AddStateName(kConnected, "Connected",
                        "USB connection to bias-power supply established.");

        T::AddStateName(kAtReference, "Referenced",
                        "Internal reference voltage matches last sent voltage.");

        T::AddStateName(kOverCurrent, "OverCurrent",
                        "At least one channel is in over current state.");

        T::AddStateName(kExpertMode, "ExpertMode",
                        "Special (risky!) mode to directly send command to the bias-power supply.");

        T::AddStateName(kRamping, "Ramping",
                        "Voltage ramping in progress.");

        // Verbosity commands
        T::AddEvent("SET_VERBOSE", "B")
            (bind(&StateMachineBias::SetVerbosity, this, placeholders::_1))
            ("set verbosity state"
             "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");

        // Conenction commands
        T::AddEvent("DISCONNECT", kConnected, kAtReference)
            (bind(&StateMachineBias::Disconnect, this))
            ("disconnect from USB");

        T::AddEvent("RECONNECT", "O", kDisconnected, kConnected, kAtReference)
            (bind(&StateMachineBias::Reconnect, this, placeholders::_1))
            ("(Re)connect USB connection to Bias power supply, a new address can be given"
             "|tty[string]:new USB address");



        T::AddEvent("REQUEST_STATUS", kConnected, kAtReference, kOverCurrent)
            (Wrapper(bind(&ConnectionBias::ReadAllChannels, &fBias, false)))
            ("Asynchronously request the status (current) of all channels.");

        T::AddEvent("RESET_OVER_CURRENT_STATUS", kOverCurrent)
            (Wrapper(bind(&ConnectionBias::OverCurrentReset, &fBias)))
            ("Set all channels in over current state to 0V and send a system reset to reset the over current flags.");



        T::AddEvent("SET_GLOBAL_VOLTAGE", "F:1", kConnected, kAtReference, kOverCurrent)
            (bind(&StateMachineBias::SetGlobalVolt, this, placeholders::_1))
            ("Set all channels to a new reference voltage. Starts ramping if necessary. (This command is not realized with the GLOBAL SET command.)"
             "|voltage[V]:Global target voltage in volts (will be converted to DAC units)");

        T::AddEvent("SET_GLOBAL_DAC", "S:1", kConnected, kAtReference, kOverCurrent)
            (bind(&StateMachineBias::SetGlobalDac, this, placeholders::_1))
            ("Set all channels to a new DAC reference. Starts ramping if necessary. (This command is not realized with the GLOBAL SET command.)"
             "|voltage[dac]:Global target voltage as DAC counts.");

        T::AddEvent("INCREASE_GLOBAL_VOLTAGE", "F:1", kConnected, kAtReference, kOverCurrent)
            (bind(&StateMachineBias::IncGlobalVolt, this, placeholders::_1))
            ("Set all channels to a new reference voltage. Starts ramping if necessary. (This command is not realized with the GLOBAL SET command.)"
             "|offset[V]:Offset to be added to all channels (will be converted to DAC counts)");

        T::AddEvent("INCREASE_GLOBAL_DAC", "S:1", kConnected, kAtReference, kOverCurrent)
            (bind(&StateMachineBias::IncGlobalDac, this, placeholders::_1))
            ("Set all channels to a new DAC reference. Starts ramping if necessary. (This command is not realized with the GLOBAL SET command.)"
             "|offset[dac]:Offset to be added to all channels as DAC counts");

        T::AddEvent("DECREASE_GLOBAL_VOLTAGE", "F:1", kConnected, kAtReference, kOverCurrent)
            (bind(&StateMachineBias::DecGlobalVolt, this, placeholders::_1))
            ("Set all channels to a new reference voltage. Starts ramping if necessary. (This command is not realized with the GLOBAL SET command.)"
             "|offset[V]:Offset to be subtracted from all channels (will be converted to DAC counts)");

        T::AddEvent("DECREASE_GLOBAL_DAC", "S:1", kConnected, kAtReference, kOverCurrent)
            (bind(&StateMachineBias::DecGlobalDac, this, placeholders::_1))
            ("Set all channels to a new DAC reference. Starts ramping if necessary. (This command is not realized with the GLOBAL SET command.)"
             "|offset[dac]:Offset to be subtracted from all channels as DAC counts");

        T::AddEvent("SET_CHANNEL_VOLTAGE", "S:1;F:1", kConnected, kAtReference, kOverCurrent)
            (bind(&StateMachineBias::SetChannelVolt, this, placeholders::_1))
            ("Set a single channel to a new reference voltage. Starts ramping if necessary."
             "|channel[short]:Channel for which to set the target voltage [0-416]"
             "|voltage[V]:Target voltage in volts for the given channel (will be converted to DAC units)");

        T::AddEvent("SET_CHANNEL_DAC", "S:1;S:1", kConnected, kAtReference, kOverCurrent)
            (bind(&StateMachineBias::SetChannelDac, this, placeholders::_1))
            ("Set a single channel to a new DAC reference value. Starts ramping if necessary."
             "|channel[short]:Channel for which to set the target voltage [0-416]"
             "|voltage[dac]:Target voltage in DAC units for the given channel");

        T::AddEvent("SET_GLOBAL_GAPD_REFERENCE_VOLTAGE", kConnected, kAtReference, kOverCurrent)
            (Wrapper(bind(&ConnectionBias::SetGapdVoltage, &fBias, 0.)))
            ("Set all channels to their G-APD reference voltage. Starts ramping if necessary.");

        T::AddEvent("SET_CHANNEL_GAPD_REFERENCE_VOLTAGE", "S:1", kConnected, kAtReference, kOverCurrent)
            (bind(&StateMachineBias::SetGapdReferenceCh, this, placeholders::_1))
            ("Set a single channel channels to its G-APD reference voltage. Starts ramping if necessary."
             "|channel[short]:Channel for which to set the target voltage [0-416]");

        T::AddEvent("SET_GAPD_REFERENCE_OFFSET", "F:1", kConnected, kAtReference, kOverCurrent)
            (bind(&StateMachineBias::SetGapdVoltage, this, placeholders::_1))
            ("Set all channels to their G-APD reference voltage plus the given offset. Starts ramping if necessary."
             "|offset[V]:Offset to be added to teh G-APD reference voltage globally");

        T::AddEvent("SET_ZERO_VOLTAGE", kConnected, kAtReference, kOverCurrent)
            (Wrapper(bind(&ConnectionBias::SetZero, &fBias)))
            ("Set all channels to a zero reference voltage. Starts ramping if necessary.");

        T::AddEvent("SET_REFERENCE_VOLTAGES", "F:416", kConnected, kAtReference, kOverCurrent)
            (bind(&StateMachineBias::SetReferenceVolt, this, placeholders::_1))
            ("Set all channels to the given new reference voltage. Starts ramping if necessary."
             "voltage[V]:New reference voltage for all channels");

        T::AddEvent("SET_REFERENCE_DACS", "S:416", kConnected, kAtReference, kOverCurrent)
            (bind(&StateMachineBias::SetReferenceDac, this, placeholders::_1))
            ("Set all channels to the given new reference voltage. Starts ramping if necessary."
             "voltage[dac]:New reference voltage for all channels in DAC units");

        T::AddEvent("ADD_REFERENCE_VOLTAGES", "F:416", kConnected, kAtReference, kOverCurrent)
            (bind(&StateMachineBias::AddReferenceVolt, this, placeholders::_1))
            ("Add the given voltages to the current reference voltages. Starts ramping if necessary."
             "offset[V]:Offsets to be added to the reference voltage of all channels in volts");

        T::AddEvent("ADD_REFERENCE_DACS", "S:416", kConnected, kAtReference, kOverCurrent)
            (bind(&StateMachineBias::AddReferenceDac, this, placeholders::_1))
            ("Add the given voltages to the current reference voltages. Starts ramping if necessary."
             "offset[dac]:Offsets to be added to the reference voltage of all channels in DAC units");




        T::AddEvent("STOP", kConnected, kRamping, kAtReference, kOverCurrent)
            (Wrapper(bind(&ConnectionBias::RampStop, &fBias)))
            ("Stop an on-going ramping");

        T::AddEvent("START", kConnected, kOverCurrent)
            (Wrapper(bind(&ConnectionBias::RampStart, &fBias)))
            ("Start a ramping if no ramping is in progress and if reference values differ from current voltages");



        T::AddEvent("PRINT_INFO")
            (Wrapper(bind(&ConnectionBias::PrintInfo, &fBias)))
            ("Print a table with all current read back with the last request operation");
        T::AddEvent("PRINT_CURRENTS")
            (Wrapper(bind(&ConnectionBias::PrintA, &fBias)))
            ("Print a table with all current read back with the last request operation");
        T::AddEvent("PRINT_VOLTAGES")
            (Wrapper(bind(&ConnectionBias::PrintV, &fBias)))
            ("Print a table with all voltages (current and reference voltages as currently in memory)");
        T::AddEvent("PRINT_GAPD_REFERENCE_VOLTAGES")
            (Wrapper(bind(&ConnectionBias::PrintGapd, &fBias)))
            ("Print the G-APD reference values obtained from file");


        T::AddEvent("EXPERT_MODE", "B:1")
            (bind(&StateMachineBias::SetExpertMode, this, placeholders::_1))
            ("Enable usage of expert commands (note that for safty reasons the are exclusive with the standard commands)");

        T::AddEvent("EXPERT_RESET", kExpertMode)
            (Wrapper(bind(&ConnectionBias::ExpertReset, &fBias, true)))
            ("Send the RESET command (note that this is possibly harmfull command)");

        T::AddEvent("EXPERT_SET_GLOBAL_VOLTAGE", "F:1", kExpertMode)
            (bind(&StateMachineBias::ExpertSetGlobalVolt, this, placeholders::_1))
            ("Send the global set command. The given voltage is converted to DAC counts.");

        T::AddEvent("EXPERT_SET_GLOBAL_DAC", "S:1", kExpertMode)
            (bind(&StateMachineBias::ExpertSetGlobalDac, this, placeholders::_1))
            ("Send the global set command.");

        T::AddEvent("EXPERT_SET_CHANNEL_VOLTAGE", "S:1;F:1", kExpertMode)
            (bind(&StateMachineBias::ExpertSetChannelVolt, this, placeholders::_1))
            ("Send a single channel set command. The given voltage is converted to DAC commands.");

        T::AddEvent("EXPERT_SET_CHANNEL_DAC", "S:1;S:1", kExpertMode)
            (bind(&StateMachineBias::ExpertSetChannelDac, this, placeholders::_1))
            ("Send a single channel set command.");
    }

    ~StateMachineBias() { T::Warn("TODO: Implement rampming at shutdown!"); }

    int EvalOptions(Configuration &conf)
    {
        // FIXME: Read calib_offset
        // FIXME: Check calib offset being smaller than +/-0.25V

        fBias.SetVerbose(!conf.Get<bool>("quiet"));

        fBias.SetEndpoint(conf.Get<string>("dev"));
        T::Message("Setting device to "+fBias.URL());

        const uint16_t step = conf.Get<uint16_t>("ramp-step");
        const uint16_t time = conf.Get<uint16_t>("ramp-time");

        if (step>230) // 5V
        {
            T::Error("ramp-step exceeds allowed range.");
            return 1;
        }

        fBias.SetRampStep(step);
        fBias.SetRampTime(time);
        fBias.SetUpdateInterval(conf.Get<uint16_t>("update-interval"));
        fBias.SetSyncDelay(conf.Get<uint16_t>("sync-delay"));

        ostringstream str1, str2;
        str1 << "Ramping in effective steps of " << fBias.GetRampStepVolt() << "V";
        str2 << "Ramping with a delay per step of " << time << "ms";
        T::Message(str1);
        T::Message(str2);

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

        const float maxabsv = conf.Get<float>("volt-max-abs");
        const float maxrelv = conf.Get<float>("volt-max-rel");
        if (maxabsv>90)
        {
            T::Error("volt-max exceeds 90V.");
            return 2;
        }
        if (maxabsv>75)
            T::Warn("volt-max exceeds 75V.");
        if (maxabsv<70)
            T::Warn("volt-max below 70V.");
        if (maxabsv<0)
        {
            T::Error("volt-max negative.");
            return 3;
        }

        fBias.SetVoltMaxAbs(maxabsv);
        fBias.SetVoltMaxRel(maxrelv);

        ostringstream str3, str4;
        str3 << "Effective maximum allowed absolute voltage: " << fBias.GetVoltMaxAbs() << "V";
        str4 << "Effective maximum difference w.r.t to G-APD reference: " << fBias.GetVoltMaxRel() << "V";
        T::Message(str3);
        T::Message(str4);

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

        PixelMap map;
        if (!map.Read(conf.Get<string>("pixel-map-file")))
        {
            T::Error("Reading reference voltages from "+conf.Get<string>("pixel-map-file")+" failed.");
            return 5;
        }

        if (!fBias.SetNewGapdVoltage(map.Vgapd()))
        {
            T::Error("Setting reference voltages failed.");
            return 6;
        }

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

        fBias.Connect();

        return -1;
    }
};

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

#include "Main.h"

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

void SetupConfiguration(Configuration &conf)
{
    po::options_description control("BIAS control options");
    control.add_options()
        ("no-dim,d",        po_bool(),  "Disable dim services")
        ("dev",             var<string>("FTE00FOH"),  "Device address of USB port to bias-power supply")
        ("quiet,q",         po_bool(),           "Disable printing contents of all received messages (except dynamic data) in clear text.")
        ("ramp-time",       var<uint16_t>(15),   "Delay between the answer of one ramping step and sending the next ramp command to all channels in milliseconds.")
        ("ramp-step",       var<uint16_t>(46),   "Maximum step in DAC counts during ramping (Volt = DAC*90/4096)")
        ("update-interval", var<uint16_t>(3000), "Interval between two current requests in milliseconds")
        ("sync-delay",      var<uint16_t>(500),  "Delay between sending the inital 0's after a newly established connection to synchronize the output stream in milliseconds")
        ("volt-max-abs",    var<float>(75),      "Absolte upper limit for the voltage (in Volts)")
        ("volt-max-rel",    var<float>(2.5),     "Relative upper limit for the voltage w.r.t. the G-APD reference voltage (in Volts)")
        ("pixel-map-file",  var<string>("FACTmapV5a.txt"), "Pixel mapping file. Used here to get the default reference voltage.")
        ;

    conf.AddOptions(control);
}

/*
 Extract usage clause(s) [if any] for SYNOPSIS.
 Translators: "Usage" and "or" here are patterns (regular expressions) which
 are used to match the usage synopsis in program output.  An example from cp
 (GNU coreutils) which contains both strings:
  Usage: cp [OPTION]... [-T] SOURCE DEST
    or:  cp [OPTION]... SOURCE... DIRECTORY
    or:  cp [OPTION]... -t DIRECTORY SOURCE...
 */
void PrintUsage()
{
    cout <<
        "The biasctrl program controls the bias-power supply boards.\n"
        "\n"
        "Note: At default the program is started without a command line (user) "
        "interface. In this case Actions/Commands are available via Dim "
        "exclusively.\n"
        "Use the -c option to start the program with a command line interface.\n"
        "\n"
        "In the running application:\n"
        "Use h or help to print a short help message about its usage.\n"
        "\n"
        "Usage: biasctrl [-c type] [OPTIONS]\n"
        "  or:  biasctrl [OPTIONS]\n";
    cout << endl;
}

void PrintHelp()
{
    Main::PrintHelp<StateMachineBias<StateMachine,ConnectionBias>>();

    /* Additional help text which is printed after the configuration
     options goes here */

    /*
     cout << "bla bla bla" << endl << endl;
     cout << endl;
     cout << "Environment:" << endl;
     cout << "environment" << endl;
     cout << endl;
     cout << "Examples:" << endl;
     cout << "test exam" << endl;
     cout << endl;
     cout << "Files:" << endl;
     cout << "files" << endl;
     cout << endl;
     */
}

int main(int argc, const char* argv[])
{
    Configuration conf(argv[0]);
    conf.SetPrintUsage(PrintUsage);
    Main::SetupConfiguration(conf);
    SetupConfiguration(conf);

    if (!conf.DoParse(argc, argv, PrintHelp))
        return -1;

    //try
    {
        // No console access at all
        if (!conf.Has("console"))
        {
            if (conf.Get<bool>("no-dim"))
                return RunShell<LocalStream, StateMachine, ConnectionBias>(conf);
            else
                return RunShell<LocalStream, StateMachineDim, ConnectionDimBias>(conf);
        }
        // Cosole access w/ and w/o Dim
        if (conf.Get<bool>("no-dim"))
        {
            if (conf.Get<int>("console")==0)
                return RunShell<LocalShell, StateMachine, ConnectionBias>(conf);
            else
                return RunShell<LocalConsole, StateMachine, ConnectionBias>(conf);
        }
        else
        {
            if (conf.Get<int>("console")==0)
                return RunShell<LocalShell, StateMachineDim, ConnectionDimBias>(conf);
            else
                return RunShell<LocalConsole, StateMachineDim, ConnectionDimBias>(conf);
        }
    }
    /*catch (std::exception& e)
    {
        cerr << "Exception: " << e.what() << endl;
        return -1;
    }*/

    return 0;
}