source: trunk/FACT++/src/fadctrl.cc@ 11053

#include <boost/bind.hpp>
#include <boost/array.hpp>
#if BOOST_VERSION < 104400
#if (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ > 4))
#undef BOOST_HAS_RVALUE_REFS
#endif
#endif
#include <boost/thread.hpp>
//#include <boost/foreach.hpp>
#include <boost/asio/error.hpp>
#include <boost/asio/deadline_timer.hpp>
#include <boost/date_time/posix_time/posix_time_types.hpp>

#include "Dim.h"
#include "Event.h"
#include "Shell.h"
#include "StateMachineDim.h"
#include "Connection.h"
#include "Configuration.h"
#include "Console.h"
#include "Converter.h"
#include "LocalControl.h"
#include "HeadersFAD.h"

#include "tools.h"

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

using ba::ip::tcp;

using namespace std;

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

class ConnectionFAD : public Connection
{
    vector<uint16_t> fBuffer;

protected:
    FAD::EventHeader   fEventHeader;
    FAD::ChannelHeader fChannelHeader[FAD::kNumChannels];

private:
    bool fIsVerbose;
    bool fIsHexOutput;
    bool fIsDataOutput;
    bool fBlockTransmission;

    uint64_t fCounter;

protected:
    virtual void UpdateFirstHeader()
    {
    }

    virtual void UpdateEventHeader()
    {
        // emit service with trigger counter from header
        if (!fIsVerbose)
            return;

        Out() << endl << kBold << "Header received (N=" << dec << fCounter << "):" << endl;
        Out() << fEventHeader;
        if (fIsHexOutput)
            Out() << Converter::GetHex<uint16_t>(fEventHeader, 16) << endl;
    }

    /*
    virtual void UpdateChannelHeader(int i)
    {
        // emit service with trigger counter from header
        if (!fIsVerbose)
            return;

        Out() << fChannelHeader[i];
        if (fIsHexOutput)
            Out() << Converter::GetHex<uint16_t>(fChannelHeader, 16) << endl;
    }
    */

    virtual void UpdateChannelHeaders()
    {
        // emit service with trigger counter from header
        if (!fIsVerbose)
            return;

        Out() << dec << endl;

        Out() << kBold << "ID: Crate=" << fEventHeader.Crate() << " Board=" << fEventHeader.Board() << endl;
        for (unsigned int c=0; c<FAD::kNumChips; c++)
        {
            Out() << "ReadoutWin #" << c << ":";
            for (unsigned int ch=0; ch<FAD::kNumChannelsPerChip; ch++)
                Out() << " " << setw(4) << fChannelHeader[c*FAD::kNumChannelsPerChip+ch].fRegionOfInterest;
            Out() << endl;
        }

        for (unsigned int c=0; c<FAD::kNumChips; c++)
        {
            Out() << "StartCells #" << c << ":";
            for (unsigned int ch=0; ch<FAD::kNumChannelsPerChip; ch++)
                Out() << " " << setw(4) << fChannelHeader[c*FAD::kNumChannelsPerChip+ch].fStartCell;
            Out() << endl;
        }

        if (fIsHexOutput)
            Out() << Converter::GetHex<uint16_t>(fChannelHeader, 16) << endl;
    }

    virtual void UpdateData(const uint16_t *data, size_t sz)
    {
        // emit service with trigger counter from header
        if (fIsVerbose && fIsDataOutput)
            Out() << Converter::GetHex<uint16_t>(data, sz, 16, true) << endl;
    }

private:
    enum
    {
        kReadHeader = 1,
        kReadData   = 2,
    };

    void HandleReceivedData(const bs::error_code& err, size_t bytes_received, int type)
    {
        // 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 (FAD).");

            // 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(err!=ba::error::basic_errors::operation_aborted);
            return;
        }

        if (type==kReadHeader)
        {
            if (bytes_received!=sizeof(FAD::EventHeader))
            {
                ostringstream str;
                str << "Bytes received (" << bytes_received << " don't match header size " << sizeof(FAD::EventHeader);
                Error(str);
                PostClose(false);
                return;
            }

            fEventHeader = fBuffer;

            if (fEventHeader.fStartDelimiter!=FAD::kDelimiterStart)
            {
                ostringstream str;
                str << "Invalid header received: start delimiter wrong, received ";
                str << hex << fEventHeader.fStartDelimiter << ", expected " << FAD::kDelimiterStart << ".";
                Error(str);
                PostClose(false);
                return;
            }

            if (fCounter==0)
                UpdateFirstHeader();

            UpdateEventHeader();

            fCounter++;

            fBuffer.resize(fEventHeader.fPackageLength-sizeof(FAD::EventHeader)/2);
            AsyncRead(ba::buffer(fBuffer), kReadData);
            AsyncWait(fInTimeout, 500, &Connection::HandleReadTimeout);

            return;
        }

        fInTimeout.cancel();

        if (ntohs(fBuffer.back())!=FAD::kDelimiterEnd)
        {
            ostringstream str;
            str << "Invalid data received: end delimiter wrong, received ";
            str << hex << ntohs(fBuffer.back()) << ", expected " << FAD::kDelimiterEnd << ".";
            Error(str);
            PostClose(false);
            return;
        }

        uint8_t *ptr = reinterpret_cast<uint8_t*>(fBuffer.data());
        uint8_t *end = ptr + fBuffer.size()*2;
        for (unsigned int i=0; i<FAD::kNumChannels; i++)
        {
            if (ptr+sizeof(FAD::ChannelHeader) > end)
            {
                Error("Channel header exceeds buffer size.");
                PostClose(false);
                return;
            }

            fChannelHeader[i] = vector<uint16_t>((uint16_t*)ptr, (uint16_t*)ptr+sizeof(FAD::ChannelHeader)/2);
            ptr += sizeof(FAD::ChannelHeader);

            //UpdateChannelHeader(i);

            if (ptr+fChannelHeader[i].fRegionOfInterest*2 > end)
            {
                Error("Data block exceeds buffer size.");
                PostClose(false);
                return;
            }

            uint16_t *data = reinterpret_cast<uint16_t*>(ptr);

            /*
            for (uint16_t *d=data; d<data+fChannelHeader[i].fRegionOfInterest; d++)
            {
                const bool sign     = *d & 0x2000;
                const bool overflow = *d & 0x1000;

                if (sign)
                    *d |= 0xf000;   // no overflow, nagative
                else
                    *d &= 0x07ff;   // no overlow,  positive

                // max = [-2047;2048]

                if (overflow)
                {
                    if (sign)
                        *d = 0xF800;   // overflow, negative
                    else
                        *d = 0x0800;   // overflow, positive
                }
            }
            */

            UpdateData(data, fChannelHeader[i].fRegionOfInterest*2);
            ptr += fChannelHeader[i].fRegionOfInterest*2;
        }

        UpdateChannelHeaders();

        fBuffer.resize(sizeof(FAD::EventHeader)/2);
        AsyncRead(ba::buffer(fBuffer), kReadHeader);
    }

    void HandleReadTimeout(const bs::error_code &error)
    {
        if (error==ba::error::basic_errors::operation_aborted)
            return;

        if (error)
        {
            ostringstream str;
            str << "Read timeout of " << URL() << ": " << error.message() << " (" << error << ")";// << endl;
            Error(str);

            PostClose();
            return;

        }

        if (!is_open())
        {
            // For example: Here we could schedule a new accept if we
            // would not want to allow two connections at the same time.
            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 (fInTimeout.expires_at() > ba::deadline_timer::traits_type::now())
            return;

        Error("Timeout reading data from "+URL());
        PostClose();
    }

    // This is called when a connection was established
    void ConnectionEstablished()
    {
        fEventHeader.clear();
        for (unsigned int i=0; i<FAD::kNumChannels; i++)
            fChannelHeader[i].clear();

        fCounter = 0;

        fBuffer.resize(sizeof(FAD::EventHeader)/2);
        AsyncRead(ba::buffer(fBuffer), kReadHeader);

//        for (int i=0; i<36; i++)
//            CmdSetRoi(i, 100);

        Cmd(FAD::kCmdTriggerLine, true);
        Cmd(FAD::kCmdSingleTrigger);
    }

public:
    void PostCmd(std::vector<uint16_t> cmd)
    {
        if (fBlockTransmission)
            return;

#ifdef DEBUG_TX
        ostringstream msg;
        msg << "Sending command:" << hex;
        msg << " 0x" << setw(4) << setfill('0') << cmd[0];
        msg << " (+ " << cmd.size()-1 << " bytes data)";
        Message(msg);
#endif
        transform(cmd.begin(), cmd.end(), cmd.begin(), htons);

        PostMessage(cmd);
    }

    void PostCmd(uint16_t cmd)
    {
        if (fBlockTransmission)
            return;

#ifdef DEBUG_TX
        ostringstream msg;
        msg << "Sending command:" << hex;
        msg << " 0x" << setw(4) << setfill('0') << cmd;
        Message(msg);
#endif
        cmd = htons(cmd);
        PostMessage(&cmd, sizeof(uint16_t));
    }

    void PostCmd(uint16_t cmd, uint16_t data)
    {
        if (fBlockTransmission)
            return;

#ifdef DEBUG_TX
        ostringstream msg;
        msg << "Sending command:" << hex;
        msg << " 0x" << setw(4) << setfill('0') << cmd;
        msg << " 0x" << setw(4) << setfill('0') << data;
        Message(msg);
#endif
        const uint16_t d[2] = { htons(cmd), htons(data) };
        PostMessage(d, sizeof(d));
    }

public:
    ConnectionFAD(ba::io_service& ioservice, MessageImp &imp) :
        Connection(ioservice, imp()),
        fIsVerbose(false), fIsHexOutput(false), fIsDataOutput(false),
        fBlockTransmission(false), fCounter(0)
    {
        // Maximum possible needed space:
        // The full header, all channels with all DRS bins
        // Two trailing shorts
        fBuffer.reserve(sizeof(FAD::EventHeader) + FAD::kNumChannels*(sizeof(FAD::ChannelHeader) + FAD::kMaxBins*sizeof(uint16_t)) + 2*sizeof(uint16_t));

        SetLogStream(&imp);
    }

    void Cmd(FAD::Enable cmd, bool on=true)
    {
        PostCmd(cmd + (on ? 0 : 0x100));
    }

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

    // IMPLEMENT: Abs/Rel
    void CmdPhaseShift(int16_t val)
    {
        vector<uint16_t> cmd(abs(val)+2, FAD::kCmdPhaseApply);
        cmd[0] = FAD::kCmdPhaseReset;
        cmd[1] = val<0 ? FAD::kCmdPhaseDecrease : FAD::kCmdPhaseIncrease;
        PostCmd(cmd);
    }

    bool CmdSetTriggerRate(int32_t val)
    {
        if (val<0 || val>0xffff)
            return false;

        PostCmd(FAD::kCmdWriteRate, val);//uint8_t(1000./val/12.5));
        //PostCmd(FAD::kCmdWriteExecute);

        return true;
    }

    void CmdSetRunNumber(uint32_t num)
    {
        PostCmd(FAD::kCmdWriteRunNumberLSW, num&0xffff);
        PostCmd(FAD::kCmdWriteRunNumberMSW, num>>16);
        PostCmd(FAD::kCmdWriteExecute);
    }

    void CmdSetRegister(uint8_t addr, uint16_t val)
    {
        // Allowed addr:  [0, MAX_ADDR]
        // Allowed value: [0, MAX_VAL]
        PostCmd(FAD::kCmdWrite + addr, val);
        PostCmd(FAD::kCmdWriteExecute);
    }

    bool CmdSetDacValue(uint8_t addr, uint16_t val)
    {
        if (addr>FAD::kMaxDacAddr) // NDAC
            return false;

        PostCmd(FAD::kCmdWriteDac + addr, val);
        PostCmd(FAD::kCmdWriteExecute);
        return true;
    }

    bool CmdSetRoi(int8_t addr, uint16_t val)
    {
        if (val>FAD::kMaxRoiValue)
            return false;

        if (addr<0)
        {
            for (unsigned int i=0; i<=FAD::kMaxRoiAddr; i++)
                PostCmd(FAD::kCmdWriteRoi + i, val);
            PostCmd(FAD::kCmdWriteExecute);
            return true;
        }

        if (uint8_t(addr)>FAD::kMaxRoiAddr)
            return false;

        PostCmd(FAD::kCmdWriteRoi + addr, val);
            PostCmd(FAD::kCmdWriteExecute);
        return true;
    }

    bool CmdSetRoi(uint16_t val) { return CmdSetRoi(-1, val); }

    void AmplitudeCalibration()
    {
        // ------------- case baseline -----------------

        CmdSetRoi(-1, FAD::kMaxBins);

        CmdSetDacValue(1, 0);
        CmdSetDacValue(2, 0);
        CmdSetDacValue(3, 0);

        // Take N events

        /*
         // ====== Part B: Baseline calibration =====

         // Loop over all channels(ch) and time-slices (t)
         T0 = TriggerCell[chip]
         Sum[ch][(t+T0) % kMaxBins] += Data[ch][t];
         // FIXME: Determine median instead of average

         Baseline[ch][slice] = MEDIAN( sum[ch][slice] )
         */

        // --------------- case gain -------------------

        // Set new DAC values and start accumulation
        CmdSetDacValue(1, 50000);
        CmdSetDacValue(2, 50000);
        CmdSetDacValue(3, 50000);

        // Take N events

        /*
         // ====== Part C: Gain calibration =====

         T0 = TriggerCell[chip]
         Sum[ch][(t+T0) % kMaxBins] += Data[ch][t];
         // FIXME: Determine median instead of average

         Gain[ch][slice] = MEDIAN( sum[ch][slice] ) - Baseline[ch][slice]
         */

        // --------------- secondary ------------------

        // FIXME: Can most probably be done together with the baseline calibration
        // FIXME: Why does the secondary baseline not influence the baseline?

        CmdSetDacValue(1, 0);
        CmdSetDacValue(2, 0);
        CmdSetDacValue(3, 0);

        // Take N events

        /*
         // ====== Part D: Secondary calibration =====

         T0 = TriggerCell[chip]
         Sum[ch][t] = Data[ch][t] - Baseline[ch][(i-T0) % kMaxBins];

         // Determine secondary baseline if integration finished
         SecondaryBaseline[ch][t] = MEDIAN( Sum[ch][t] )
         */
    }

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

    void SetHexOutput(bool b)
    {
        fIsHexOutput = b;
    }

    void SetDataOutput(bool b)
    {
        fIsDataOutput = b;
    }

    void SetBlockTransmission(bool b)
    {
        fBlockTransmission = b;
    }

    bool IsTransmissionBlocked() const
    {
        return fBlockTransmission;
    }

};

// ------------------------------------------------------------------------
#include "DimDescriptionService.h"
#include "EventBuilderWrapper.h"

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

template <class T>
class StateMachineFAD : public T, public EventBuilderWrapper, public ba::io_service, public ba::io_service::work
{
private:
    typedef pair<tcp::endpoint, ConnectionFAD*> Connection;
    typedef map<uint8_t, Connection>            BoardList;

    BoardList fBoards;

    bool fIsVerbose;
    bool fIsHexOutput;
    bool fIsDataOutput;

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

    int Cmd(FAD::Enable command)
    {
        for (BoardList::iterator i=fBoards.begin(); i!=fBoards.end(); i++)
            i->second.second->Cmd(command);

        return T::GetCurrentState();
    }

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

        if (evt.GetUInt()>0xffff)
        {
            T::Warn("Command value out of range (0-65535).");
            return T::GetCurrentState();
        }

        for (BoardList::iterator i=fBoards.begin(); i!=fBoards.end(); i++)
            i->second.second->PostCmd(evt.GetUInt());

        return T::GetCurrentState();
    }

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

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

        if (ptr[0]>0xffff)
        {
            T::Warn("Command value out of range (0-65535).");
            return T::GetCurrentState();
        }

        if (ptr[1]>0xffff)
        {
            T::Warn("Data value out of range (0-65535).");
            return T::GetCurrentState();
        }

        for (BoardList::iterator i=fBoards.begin(); i!=fBoards.end(); i++)
            i->second.second->PostCmd(ptr[0], ptr[1]);

        return T::GetCurrentState();
    }

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

        for (BoardList::iterator i=fBoards.begin(); i!=fBoards.end(); i++)
            i->second.second->Cmd(command, evt.GetBool());

        return T::GetCurrentState();
    }

    bool Check(const uint32_t *dat, uint32_t maxaddr, uint32_t maxval)
    {
        if (dat[0]>maxaddr)
        {
            ostringstream msg;
            msg << hex << "Address " << dat[0] << " out of range, max=" << maxaddr << ".";
            T::Error(msg);
            return false;
        }

        if (dat[1]>maxval)
        {
            ostringstream msg;
            msg << hex << "Value " << dat[1] << " out of range, max=" << maxval << ".";
            T::Error(msg);
            return false;
        }

        return true;
    }

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

        const uint32_t *dat = evt.Ptr<uint32_t>();

        if (!Check(dat, FAD::kMaxRegAddr, FAD::kMaxRegValue))
            return T::GetCurrentState();

        for (BoardList::iterator i=fBoards.begin(); i!=fBoards.end(); i++)
            i->second.second->CmdSetRegister(dat[0], dat[1]);

        return T::GetCurrentState();
    }

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

        const int32_t *dat = evt.Ptr<int32_t>();

        for (BoardList::iterator i=fBoards.begin(); i!=fBoards.end(); i++)
            if (!i->second.second->CmdSetRoi(dat[0], dat[1]))
            {
                ostringstream msg;
                msg << hex << "Channel " << dat[0] << " or Value " << dat[1] << " out of range.";
                T::Error(msg);
                return false;
            }


        return T::GetCurrentState();
    }

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

        const uint32_t *dat = evt.Ptr<uint32_t>();

        if (!Check(dat, FAD::kMaxDacAddr, FAD::kMaxDacValue))
            return T::GetCurrentState();

        for (BoardList::iterator i=fBoards.begin(); i!=fBoards.end(); i++)
            i->second.second->CmdSetDacValue(dat[0], dat[1]);

        return T::GetCurrentState();
    }

    int Trigger(int n)
    {
        for (int nn=0; nn<n; nn++)
            for (BoardList::iterator i=fBoards.begin(); i!=fBoards.end(); i++)
                i->second.second->Cmd(FAD::kCmdSingleTrigger);

        return T::GetCurrentState();
    }

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

        Trigger(evt.GetUInt());

        return T::GetCurrentState();
    }

    int StartRun(const EventImp &evt, bool start)
    {
        if (!CheckEventSize(evt.GetSize(), "StartRun", 0))
            return T::kSM_FatalError;

        for (BoardList::iterator i=fBoards.begin(); i!=fBoards.end(); i++)
            i->second.second->Cmd(FAD::kCmdRun, start);

        return T::GetCurrentState();
    }

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

        for (BoardList::iterator i=fBoards.begin(); i!=fBoards.end(); i++)
            i->second.second->CmdPhaseShift(evt.GetShort());

        return T::GetCurrentState();
    }

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

        if (evt.GetUShort()>0xff)
        {
            ostringstream msg;
            msg << hex << "Value " << evt.GetUShort() << " out of range, max=" << 0xff << "(?)";
            T::Error(msg);
            return false;
        }

        for (BoardList::iterator i=fBoards.begin(); i!=fBoards.end(); i++)
            i->second.second->CmdSetTriggerRate(evt.GetUInt());

        return T::GetCurrentState();
    }

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

        const uint64_t num = evt.GetUXtra();

        if (num>FAD::kMaxRunNumber)
        {
            ostringstream msg;
            msg << hex << "Value " << num << " out of range, max=" << FAD::kMaxRunNumber;
            T::Error(msg);
            return false;
        }

        for (BoardList::iterator i=fBoards.begin(); i!=fBoards.end(); i++)
            i->second.second->CmdSetRunNumber(num);

        return T::GetCurrentState();
    }

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


        SetMode(evt.GetShort());

        return T::GetCurrentState();
    }


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

        for (BoardList::iterator i=fBoards.begin(); i!=fBoards.end(); i++)
            i->second.second->SetVerbose(evt.GetBool());

        return T::GetCurrentState();
    }

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

        for (BoardList::iterator i=fBoards.begin(); i!=fBoards.end(); i++)
            i->second.second->SetHexOutput(evt.GetBool());

        return T::GetCurrentState();
    }

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

       for (BoardList::iterator i=fBoards.begin(); i!=fBoards.end(); i++)
            i->second.second->SetDataOutput(evt.GetBool());

        return T::GetCurrentState();
    }

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

        const int32_t slot = evt.Get<int32_t>();

        const BoardList::iterator it=fBoards.find(slot);
        if (it==fBoards.end())
        {
            ostringstream str;
            str << "Slot " << slot << " not found.";
            T::Warn(str);
            return T::GetCurrentState();
        }

        it->second.second->SetBlockTransmission(evt.Get<uint8_t>(4));

        return T::GetCurrentState();
    }

    int AddAddress(const EventImp &evt)
    {
        const string addr = Tools::Trim(evt.GetText());

        const tcp::endpoint endpoint = GetEndpoint(addr);
        if (endpoint==tcp::endpoint())
            return T::GetCurrentState();

        for (BoardList::const_iterator i=fBoards.begin(); i!=fBoards.end(); i++)
        {
            if (i->second.first==endpoint)
            {
               T::Warn("Address "+addr+" already known.... ignored.");
               return T::GetCurrentState();
            }
        }

        AddEndpoint(endpoint);

        return T::GetCurrentState();
    }

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

        const int16_t slot = evt.GetShort();

        const BoardList::iterator it = fBoards.find(slot);

        if (it==fBoards.end())
        {
            ostringstream str;
            str << "Slot " << slot << " not found.";
            T::Warn(str.str());
            return T::GetCurrentState();
        }

        delete it->second.second;
        fBoards.erase(it);

        return T::GetCurrentState();
    }

    int ListSlots()
    {
        for (BoardList::iterator i=fBoards.begin(); i!=fBoards.end(); i++)
        {
            const int        &idx  = i->first;
            const Connection &slot = i->second;

            ostringstream str;
            str << "Slot " << setw(2) << idx << ": " << slot.first;

            const ConnectionFAD *c = slot.second;

            if (c->IsConnecting())
                str << " (0:connecting, ";
            else
            {
                if (c->IsClosed())
                    str << " (0:disconnected, ";
                if (c->IsConnected())
                    str << " (0:connected, ";
            }

            switch (fStatus2[idx])
            {
            case 0:  str << "1-7:not connected)"; break;
            case 7:  str << "1-7:connected)";     break;
            default: str << "1-7:connecting [" << fStatus2[idx] << "])"; break;
            }

            if (c->IsTransmissionBlocked())
                str << " [blocked]";

            T::Out() << str.str() << endl;
        }

        T::Out() << "Event builder thread:";
        if (!IsThreadRunning())
            T::Out() << " not";
        T::Out() << " running" << endl;

        // FIXME: Output state

        return T::GetCurrentState();
    }

    void EnableSlot(Connection &c, bool enable=true)
    {
        ConnectionFAD *ptr = c.second;
        if (!ptr)
            return;

        if (!enable)
            ptr->PostClose(false);
        else
        {
            ostringstream str;
            str << c.first;
            ptr->SetEndpoint(str.str());
            ptr->StartConnect();
        }
    }

    void EnableAll(bool enable=true)
    {
        for (BoardList::iterator i=fBoards.begin(); i!=fBoards.end(); i++)
            EnableSlot(i->second, enable);
    }

    int Disconnect()
    {
        Exit();
        EnableAll(false);
        return T::GetCurrentState();
    }

    int ForceDisconnect()
    {
        Abort();
        EnableAll(false);
        return T::GetCurrentState();
    }

    int CloseOpenFiles()
    {
        EventBuilderWrapper::CloseOpenFiles();
        return T::GetCurrentState();
    }

    int Connect()
    {
        vector<tcp::endpoint> addr(40);

        for (BoardList::iterator i=fBoards.begin(); i!=fBoards.end(); i++)
            addr[i->first] = i->second.first;

        Start(addr);
        EnableAll(true);

        return T::GetCurrentState();
    }

    vector<uint8_t> fStatus1;
    vector<uint8_t> fStatus2;

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

        // ===== Evaluate connection status =====

        uint16_t nconnecting1 = 0;
        uint16_t nconnecting2 = 0;
        uint16_t nconnected1  = 0;
        uint16_t nconnected2  = 0;

        vector<uint8_t> stat1(40);
        vector<uint8_t> stat2(40);

        int cnt = 0; // counter for enbled board

        for (int idx=0; idx<40; idx++)
        {
            // ----- Command socket -----
            const BoardList::const_iterator &slot = fBoards.find(idx);
            if (slot!=fBoards.end())
            {
                const ConnectionFAD *c = slot->second.second;
                if (c->IsConnecting())
                {
                    stat1[idx] = 1;
                    nconnecting1++;
                }
                if (c->IsConnected())
                {
                    stat1[idx] = 2;
                    nconnected1++;
                }

                cnt++;
            }

            // ----- Event builder -----
            stat2[idx] = GetNumConnected(idx);

            if (!IsConnected(idx) && !IsDisconnected(idx))
                nconnecting2++;

            if (IsConnected(idx))
                nconnected2++;
        }

        // ===== Send connection status via dim =====

        if (fStatus1!=stat1 || fStatus2!=stat2)
        {
            fStatus1 = stat1;
            fStatus2 = stat2;
            UpdateConnectionStatus(stat1, stat2);
        }

        // ===== Return connection status =====

        // fadctrl:       Always connecting if not disabled
        // event builder:

       // FIXME: Evaluate event builder status
         if (nconnected1==cnt && nconnected2==cnt && cnt>0)
            return FAD::kConnected;

        if (nconnected1!=0 || nconnected2!=0)
            return FAD::kConnecting;

        // nconnected1 == nconnected2 == 0
        return IsThreadRunning() ? FAD::kDisconnected : FAD::kOffline;
    }

    void AddEndpoint(const tcp::endpoint &addr)
    {
        int i=0;
        while (i<40)
        {
            if (fBoards.find(i)==fBoards.end())
                break;
            i++;
        }

        if (i==40)
        {
            T::Warn("Not more than 40 slots allowed.");
            return;
        }

        fBoards[i] = make_pair(addr, new ConnectionFAD(*this, *this));
        fBoards[i].second->SetVerbose(fIsVerbose);
        fBoards[i].second->SetHexOutput(fIsHexOutput);
        fBoards[i].second->SetDataOutput(fIsDataOutput);
    }


    DimDescribedService fDimConnection;
 /*
    template<class T>
        void Update(DimDescribedService &svc, const T &data) const
    {
        //cout << "Update: " << svc.getName() << " (" << sizeof(T) << ")" << endl;
        svc.setData(const_cast<T*>(&data), sizeof(T));
        svc.updateService();
    }
 */
    void UpdateConnectionStatus(const vector<uint8_t> &stat1, const vector<uint8_t> &stat2)
    {
        vector<uint8_t> stat(40);

        for (int i=0; i<40; i++)
            stat[i] = stat1[i]+stat2[i];

        fDimConnection.setData(stat.data(), 40);
        fDimConnection.updateService();
    }

public:
    StateMachineFAD(ostream &out=cout) :
        T(out, "FAD_CONTROL"), EventBuilderWrapper(*static_cast<MessageImp*>(this)), ba::io_service::work(static_cast<ba::io_service&>(*this)),
        fStatus1(40), fStatus2(40),
        fDimConnection("FAD_CONTROL/CONNECTIONS", "C:41", "")
    {
        // 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(FAD::kOffline, "Offline",
                        "All enabled FAD boards are disconnected and the event-builer thread is not running.");

        T::AddStateName(FAD::kDisconnected, "Disconnected",
                        "All enabled FAD boards are disconnected, but the event-builder thread is running.");

        T::AddStateName(FAD::kConnected, "Connected",
                        "All enabled FAD boards are connected..");

        T::AddStateName(FAD::kConnecting, "Connecting",
                        "Only some enabled FAD boards are connected.");

        // FAD Commands
        T::AddEvent("SEND_CMD", "I:1")
            (boost::bind(&StateMachineFAD::SendCmd, this, _1))
            ("Send a command to the FADs. Values between 0 and 65535 are allowed."
             "|command[uint16]:Command to be transmittted.");
        T::AddEvent("SEND_DATA", "I:2")
            (boost::bind(&StateMachineFAD::SendCmdData, this, _1))
            ("Send a command with data to the FADs. Values between 0 and 65535 are allowed."
             "|command[uint16]:Command to be transmittted."
             "|data[uint16]:Data to be sent with the command.");

        T::AddEvent("ENABLE_SRCLK", "B:1")
            (boost::bind(&StateMachineFAD::CmdEnable, this, _1, FAD::kCmdSrclk))
            ("Set SRCLK");
        T::AddEvent("ENABLE_SCLK", "B:1")
            (boost::bind(&StateMachineFAD::CmdEnable, this, _1, FAD::kCmdSclk))
            ("Set SCLK");
        T::AddEvent("ENABLE_DRS", "B:1")
            (boost::bind(&StateMachineFAD::CmdEnable, this, _1, FAD::kCmdDrsEnable))
            ("Switch Domino wave");
        T::AddEvent("ENABLE_DWRITE", "B:1")
            (boost::bind(&StateMachineFAD::CmdEnable, this, _1, FAD::kCmdDwrite))
            ("Set Dwrite (possibly high / always low)");
        T::AddEvent("SET_DEBUG_MODE", "B:1")
            (boost::bind(&StateMachineFAD::CmdEnable, this, _1, FAD::kCmdSocket))
            ("Set debug mode (yes: dump events through command socket, no=dump events through other sockets)");
        T::AddEvent("ENABLE_TRIGGER_LINE", "B:1")
            (boost::bind(&StateMachineFAD::CmdEnable, this, _1, FAD::kCmdTriggerLine))
            ("Incoming triggers can be accepted/will not be accepted");
        T::AddEvent("SET_TRIGGER_RATE", "I:1")
            (boost::bind(&StateMachineFAD::SetTriggerRate, this, _1))
            ("Enable continous trigger");
        T::AddEvent("SEND_SINGLE_TRIGGER")
            (boost::bind(&StateMachineFAD::Trigger, this, 1))
            ("Issue software triggers");
        T::AddEvent("SEND_N_TRIGGERS", "I")
            (boost::bind(&StateMachineFAD::SendTriggers, this, _1))
            ("Issue software triggers");
        T::AddEvent("START", "")
            (boost::bind(&StateMachineFAD::StartRun, this, _1, true))
            ("Set FAD DAQ mode. when started, no configurations must be send.");
        T::AddEvent("STOP")
            (boost::bind(&StateMachineFAD::StartRun, this, _1, false))
            ("");
        T::AddEvent("PHASE_SHIFT", "S:1")
            (boost::bind(&StateMachineFAD::PhaseShift, this, _1))
            ("Adjust ADC phase (in 'steps')");

        T::AddEvent("CONTINOUS_TRIGGER_ON")
            (boost::bind(&StateMachineFAD::Cmd, this, FAD::kCmdContTriggerOn))
            ("");
        T::AddEvent("CONTINOUS_TRIGGER_OFF")
            (boost::bind(&StateMachineFAD::Cmd, this, FAD::kCmdContTriggerOff))
            ("");

        T::AddEvent("RESET_TRIGGER_ID")
            (boost::bind(&StateMachineFAD::Cmd, this, FAD::kCmdResetTriggerId))
            ("");

        T::AddEvent("SET_RUN_NUMBER", "X:1")
            (boost::bind(&StateMachineFAD::SetRunNumber, this, _1))
            ("");

        T::AddEvent("SET_REGISTER", "I:2")
            (boost::bind(&StateMachineFAD::SetRegister, this, _1))
            ("set register to value"
            "|addr[short]:Address of register"
            "|val[short]:Value to be set");

        // FIXME:  Maybe add a mask which channels should be set?
        T::AddEvent("SET_REGION_OF_INTEREST", "I:2")
            (boost::bind(&StateMachineFAD::SetRoi, this, _1))
            ("Set region-of-interest to value"
            "|addr[short]:Address of register"
            "|val[short]:Value to be set");

        // FIXME:  Maybe add a mask which channels should be set?
        T::AddEvent("SET_DAC_VALUE", "I:2")
            (boost::bind(&StateMachineFAD::SetDac, this, _1))
            ("Set DAC numbers in range to value"
            "|addr[short]:Address of register"
            "|val[short]:Value to be set");

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

        T::AddEvent("SET_HEX_OUTPUT", "B")
            (boost::bind(&StateMachineFAD::SetHexOutput, this, _1))
            ("enable or disable hex output for received data"
             "|hexout[bool]:disable or enable hex output for received data (yes/no)");

        T::AddEvent("SET_DATA_OUTPUT", "B")
            (boost::bind(&StateMachineFAD::SetDataOutput, this, _1))
            ("");

        T::AddEvent("BLOCK_TRANSMISSION", "I:1;B:1")
            (boost::bind(&StateMachineFAD::SetBlockTransmission, this, _1))
            ("Blocks the transmission of commands to the given slot. Use with care! For debugging pupose only!"
             "|slot[int]:Slot to which the command transmission should be blocked (0-39)"
             "|enable[bool]:Whether the command transmission should be blockes (yes) or allowed (no)");

        // Conenction commands
        /*
        T::AddEvent("ENABLE", "S:1;B:1", FAD::kDisconnected)
            (boost::bind(&StateMachineFAD::Enable, this, _1))
            ("");*/

        T::AddEvent("CONNECT", FAD::kOffline)
            (boost::bind(&StateMachineFAD::Connect, this))
            ("");

        T::AddEvent("DISCONNECT", FAD::kDisconnected, FAD::kConnecting, FAD::kConnected)
            (boost::bind(&StateMachineFAD::Disconnect, this))
            ("");

        T::AddEvent("FORCE_DISCONNECT", FAD::kDisconnected, FAD::kConnecting, FAD::kConnected)
            (boost::bind(&StateMachineFAD::ForceDisconnect, this))
            ("");

        T::AddEvent("CLOSE_OPEN_FILES", FAD::kConnecting, FAD::kConnected)
            (boost::bind(&StateMachineFAD::CloseOpenFiles, this))
            ("");

        T::AddEvent("TEST", "S:1")
            (boost::bind(&StateMachineFAD::Test, this, _1))
            ("");

        T::AddEvent("ADD_ADDRESS", "C", FAD::kOffline)
            (boost::bind(&StateMachineFAD::AddAddress, this, _1))
            ("Add the address of a DRS4 board to the first free slot"
             "|IP[string]:address in the format <address:port>");
        T::AddEvent("REMOVE_SLOT", "S:1", FAD::kOffline)
            (boost::bind(&StateMachineFAD::RemoveSlot, this, _1))
            ("Remove the Iaddress in slot n. For a list see LIST"
             "|slot[int]:Remove the address in slot n from the list");
        T::AddEvent("LIST_SLOTS")
            (boost::bind(&StateMachineFAD::ListSlots, this))
            ("Print a list of all available board addressesa and whether they are enabled");
    }

    ~StateMachineFAD()
    {
        for (BoardList::const_iterator i=fBoards.begin(); i!=fBoards.end(); i++)
            delete i->second.second;
        fBoards.clear();
    }

    tcp::endpoint GetEndpoint(const string &base)
    {
        const size_t p0 = base.find_first_of(':');
        const size_t p1 = base.find_last_of(':');

        if (p0==string::npos || p0!=p1)
        {
           T::Out() << kRed << "GetEndpoint - Wrong format ('host:port' expected)" << endl;
           return tcp::endpoint();
        }

        tcp::resolver resolver(get_io_service());

        boost::system::error_code ec;

        const tcp::resolver::query query(base.substr(0, p0), base.substr(p0+1));
        const tcp::resolver::iterator iterator = resolver.resolve(query, ec);

        if (ec)
        {
           T::Out() << kRed << "GetEndpoint - Couldn't resolve endpoint '" << base << "': " << ec.message();
           return tcp::endpoint();
        }

        return *iterator;
    }

    bool SetConfiguration(const Configuration &conf)
    {
        fIsVerbose = !conf.Get<bool>("quiet");
        fIsHexOutput = conf.Get<bool>("hex-out");
        fIsDataOutput = conf.Get<bool>("data-out");

        SetMaxMemory(conf.Get<unsigned int>("max-mem"));

        // vvvvv for debugging vvvvv
        if (conf.Has("debug-addr"))
        {
            const string addr = conf.Get<string>("debug-addr");
            const int    num  = conf.Get<unsigned int>("debug-num");

            const tcp::endpoint endpoint = GetEndpoint(addr);
            if (endpoint==tcp::endpoint())
                return false;

            for (int i=0; i<num; i++)
                AddEndpoint(tcp::endpoint(endpoint.address(), endpoint.port()+8*i));

            Connect();
            return true;
        }
        // ^^^^^ for debugging ^^^^^

        if (!(conf.Has("base-addr") ^ conf.Has("addr")))
        {
           T::Out() << kRed << "SetConfiguration - Only --base-addr or --addr allowed." << endl;
           return false;
        }

        if (conf.Has("base-addr"))
        {
            const string base = conf.Get<string>("base-addr");

            const tcp::endpoint endpoint = GetEndpoint(base);
            if (endpoint==tcp::endpoint())
                return false;

            const ba::ip::address_v4::bytes_type ip = endpoint.address().to_v4().to_bytes();

            if (ip[2]>250 || ip[3]>244)
            {
                T::Out() << kRed << "SetConfiguration - IP address given by --base-addr out-of-range." << endl;
                return false;
            }

            for (int crate=0; crate<4; crate++)
                for (int board=0; board<10; board++)
                {
                    ba::ip::address_v4::bytes_type target = endpoint.address().to_v4().to_bytes();
                    target[2] += crate;
                    target[3] += board;

                    AddEndpoint(tcp::endpoint(ba::ip::address_v4(target), endpoint.port()));
                }
        }

        if (conf.Has("addr"))
        {
            const vector<string> addrs = conf.Get<vector<string>>("addr");
            for (vector<string>::const_iterator i=addrs.begin(); i<addrs.end(); i++)
            {
                const tcp::endpoint endpoint = GetEndpoint(*i);
                if (endpoint==tcp::endpoint())
                    return false;

                AddEndpoint(endpoint);
            }
        }

        Connect();

        return true;
    }

};

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


void RunThread(StateMachineImp *io_service)
{
    // This is necessary so that the StateMachien Thread can signal the
    // Readline to exit
    io_service->Run();
    Readline::Stop();
}

template<class S>
int RunDim(Configuration &conf)
{
    /*
     initscr();               // Start curses mode
     cbreak();                // Line buffering disabled, Pass on
     intrflush(stdscr, FALSE);
     start_color();            // Initialize ncurses colors
     use_default_colors();     // Assign terminal default colors to -1
     for (int i=1; i<8; i++)
        init_pair(i, i, -1);  // -1: def background
        scrollok(stdscr, true);
        */

    WindowLog wout;

    ReadlineColor::PrintBootMsg(wout, conf.GetName(), false);

    //log.SetWindow(stdscr);
    if (conf.Has("log"))
        if (!wout.OpenLogFile(conf.Get<string>("log")))
            wout << kRed << "ERROR - Couldn't open log-file " << conf.Get<string>("log") << ": " << strerror(errno) << endl;

    // Start io_service.Run to use the StateMachineImp::Run() loop
    // Start io_service.run to only use the commandHandler command detaching
    StateMachineFAD<S> io_service(wout);
    if (!io_service.SetConfiguration(conf))
        return -1;

    io_service.Run();

    return 0;
}

template<class T, class S>
int RunShell(Configuration &conf)
{
    static T shell(conf.GetName().c_str(), conf.Get<int>("console")!=1);

    WindowLog &win  = shell.GetStreamIn();
    WindowLog &wout = shell.GetStreamOut();

    if (conf.Has("log"))
        if (!wout.OpenLogFile(conf.Get<string>("log")))
            win << kRed << "ERROR - Couldn't open log-file " << conf.Get<string>("log") << ": " << strerror(errno) << endl;

    StateMachineFAD<S> io_service(wout);
    if (!io_service.SetConfiguration(conf))
        return -1;

    shell.SetReceiver(io_service);

    boost::thread t(boost::bind(RunThread, &io_service));
    //boost::thread t(boost::bind(&StateMachineFAD<S>::Run, &io_service));

    if (conf.Has("exec"))
    {
        const vector<string> v = conf.Get<vector<string>>("exec");
        for (vector<string>::const_iterator it=v.begin(); it!=v.end(); it++)
            shell.Execute(*it);
    }

    shell.Run();                 // Run the shell
    io_service.Stop();           // Signal Loop-thread to stop

    // Wait until the StateMachine has finished its thread
    // before returning and destroying the dim objects which might
    // still be in use.
    t.join();

    return 0;
}

void SetupConfiguration(Configuration &conf)
{
    const string n = conf.GetName()+".log";

    po::options_description config("Program options");
    config.add_options()
        ("dns",       var<string>("localhost"),       "Dim nameserver host name (Overwites DIM_DNS_NODE environment variable)")
        ("log,l",     var<string>(n), "Write log-file")
        ("no-dim,d",  po_switch(),    "Disable dim services")
        ("console,c", var<int>(),     "Use console (0=shell, 1=simple buffered, X=simple unbuffered)")
        ("exec,e",    vars<string>(), "Execute one or more scrips at startup")
        ;

    po::options_description control("FAD control options");
    control.add_options()
        ("quiet,q",    po_bool(),  "Disable printing contents of all received messages in clear text.")
        ("hex-out",    po_bool(),  "Enable printing contents of all printed messages also as hex data.")
        ("data-out",   po_bool(),  "Enable printing received event data.")
        ;

    po::options_description builder("Event builder options");
    builder.add_options()
        ("max-mem,m",  var<unsigned int>(100), "Maximum memory the event builder thread is allowed to consume for its event buffer")
        ;

    po::options_description connect("FAD connection options");
    connect.add_options()
        ("addr",        vars<string>(),     "Network address of FAD")
        ("base-addr",   var<string>(),      "Base address of all FAD")
        ("debug-num,n", var<unsigned int>(40),  "Sets the number of fake boards to be connected locally")
        ("debug-addr",  var<string>(),    "")
        ;

    conf.AddEnv("dns", "DIM_DNS_NODE");

    conf.AddOptions(config);
    conf.AddOptions(control);
    conf.AddOptions(builder);
    conf.AddOptions(connect);
}

void PrintUsage()
{
    cout <<
        "The fadctrl controls the FAD boards.\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: fadctrl [-c type] [OPTIONS]\n"
        "  or:  fadctrl [OPTIONS]\n";
    cout << endl;
}

void PrintHelp()
{
    /* Additional help text which is printed after the configuration
     options goes here */
}

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

    po::variables_map vm;
    try
    {
        vm = conf.Parse(argc, argv);
    }
#if BOOST_VERSION > 104000
    catch (po::multiple_occurrences &e)
    {
        cerr << "Program options invalid due to: " << e.what() << " of '" << e.get_option_name() << "'." << endl;
        return -1;
    }
#endif
    catch (exception& e)
    {
        cerr << "Program options invalid due to: " << e.what() << endl;
        return -1;
    }

    if (conf.HasVersion() || conf.HasPrint())
        return -1;

    if (conf.HasHelp())
    {
        PrintHelp();
        return -1;
    }

    Dim::Setup(conf.Get<string>("dns"));

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

    return 0;
}