#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 "Dim.h"
#include "Event.h"
#include "Shell.h"
#include "StateMachineDim.h"
#include "Connection.h"
#include "Configuration.h"
#include "Timers.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;

#undef FAKE

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

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

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

private:
    bool fIsVerbose;
    bool fIsHexOutput;
    bool fIsDataOutput;

    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 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:
    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 (bytes_received == sizeof(FAD::EventHeader))
        {
            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));

            return;
        }

        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());
        for (unsigned int i=0; i<FAD::kNumChannels; i++)
        {
            if (ptr+sizeof(FAD::ChannelHeader)/2 > reinterpret_cast<uint8_t*>(fBuffer.data())+fBuffer.size()*2)
            {
                Error("WRONG SIZE1");
                break;
            }

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

            // FIXME CHECK: Event Size vs ROI

            UpdateChannelHeader(i);

            if (ptr+fChannelHeader[i].fRegionOfInterest*2 > reinterpret_cast<uint8_t*>(fBuffer.data())+fBuffer.size()*2)
            {
                Error("WRONG SIZE2");
                break;
            }

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

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

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

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

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

    void HandleReadTimeout(const bs::error_code &error)
    {
        /*
        return;
        Warn("Reading header timed-out... restarting.");
        StartReadHeader();
        return;
         */
        if (error && error!=ba::error::basic_errors::operation_aborted)
        {
            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();
    }

    void PostCmd(std::vector<uint16_t> cmd)
    {
        ostringstream msg;
        msg << "Sending command:" << hex;
        msg << " 0x" << setw(4) << setfill('0') << cmd[0];
        Message(msg);

        transform(cmd.begin(), cmd.end(), cmd.begin(), htons);

        PostMessage(cmd);
    }

    void PostCmd(uint16_t cmd)
    {
        ostringstream msg;
        msg << "Sending command:" << hex;
        msg << " 0x" << setw(4) << setfill('0') << cmd;
        Message(msg);

        cmd = htons(cmd);
        PostMessage(&cmd, sizeof(uint16_t));
    }

    void PostCmd(uint16_t cmd, uint16_t data)
    {
        ostringstream msg;
        msg << "Sending command:" << hex;
        msg << " 0x" << setw(4) << setfill('0') << cmd;
        msg << " 0x" << setw(4) << setfill('0') << data;
        Message(msg);

        const uint16_t d[2] = { htons(cmd), htons(data) };
        PostMessage(d, sizeof(d));
    }

public:
    enum Enable_t
    {
        kCmdDrsEnable       = 0x0600,  // CMD_DENABLE/CMD_DISABLE
        kCmdDwrite          = 0x0800,  // CMD_DWRITE_RUN/CMD_DWRITE_STOP
        kCmdSclk            = 0x1000,  // CMD_SCLK_ON/OFF
        kCmdSrclk           = 0x1500,  // CMD_SRCLK_ON/OFF
        kCmdTriggerLine     = 0x1800,  // CMD_TRIGGERS_ON/CMD_TRIGGERS_OFF
        //kCmdContTrigger  = 0x1f00,
        kCmdContTriggerOff  = 0x2000,
        kCmdRun             = 0x2200,  // CMD_Start/Stop
        kCmdResetTriggerId  = 0x2A00,  //
        kCmdSocket          = 0x3000,  // CMD_mode_command/CMD_mode_all_sockets
        kCmdSingleTrigger   = 0xA000,  // CMD_Trigger
        kCmdContTriggerOn   = 0xB000,
    };

private:
    enum
    {
        kCmdWrite           = 0x0500,         // write to Config-RAM
        kCmdWriteRoi        = kCmdWrite|0x00, // Baseaddress ROI-Values
        kCmdWriteDac        = kCmdWrite|0x24, // Baseaddress DAC-Values

        kCmdWriteRate       = kCmdWrite|0x2c, // Continous trigger rate
        kCmdWriteRunNumber  = kCmdWrite|0x2d, // 

        /*
         kCmdRead            = 0x0a00,         // read from Config-RAM
         kCmdReadRoi         = kCmdRead|0x00,  // Baseaddress ROI-Values
         kCmdReadDac         = kCmdRead|0x24,  // Baseaddress DAC-Values
         */

        kCmdPhaseIncrease   = 0x1200,         // CMD_PS_DIRINC
        kCmdPhaseDecrease   = 0x1300,         // CMD_PS_DIRDEC
        kCmdPhaseApply      = 0x1400,         // CMD_PS_DO
        kCmdPhaseReset      = 0x1700,         // CMD_PS_RESET
    };

public:
    ConnectionFAD(ba::io_service& ioservice, MessageImp &imp) :
    Connection(ioservice, imp()),
    fIsVerbose(true), fIsHexOutput(false), fIsDataOutput(false), fCounter(0)

    {
        SetLogStream(&imp);

#ifdef FAKE
        for (int i=0; i<7; i++)
            fake[i] = new Connection(ioservice, imp());
#endif
    }
#ifdef FAKE
    Connection *fake[7];

    ~ConnectionFAD()
    {
        // WORKAROUND
        for (int i=0; i<7; i++)
            delete fake[i];
    }
    void StartConnect()
    {
        // WORKAROUND
        Connection::StartConnect();
        for (int i=0; i<7; i++)
            fake[i]->StartConnect();
    }
    void SetEndpoint(const string &addr)
    {
        // WORKAROUND
        Connection::SetEndpoint(addr);
        for (int i=0; i<7; i++)
        {
            const size_t p0 = addr.find_first_of(':');

            ostringstream p;
            p << addr.substr(0, p0+1) << atoi(addr.substr(p0+1).c_str())+i+1;
            fake[i]->SetEndpoint(p.str());
        }
        // ==========================================WORKAROUND
    }
#endif

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

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

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

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

        PostCmd(kCmdWriteRate, val);//uint8_t(1000./val/12.5));
        //PostCmd(kCmdContTriggerRate, uint8_t(80/val));

        return true;
    }

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

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

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

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

        if (val>FAD::kMaxRoiValue)
            return false;

        if (addr<0)
            for (int i=0; i<FAD::kMaxRoiAddr; i++)
                PostCmd(kCmdWriteRoi + i, val);
        else
            PostCmd(kCmdWriteRoi + addr, val);

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

};

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

class ConnectionDimFAD : public ConnectionFAD
{
private:

    DimDescribedService fDimPassport;
    DimDescribedService fDimTemperatures;
    DimDescribedService fDimSetup;
    DimDescribedService fDimEventHeader;

    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 UpdateFirstHeader()
    {
        ConnectionFAD::UpdateFirstHeader();

        const FAD::DimPassport data(fEventHeader);
        Update(fDimPassport, data);
    }

    void UpdateEventHeader()
    {
        ConnectionFAD::UpdateEventHeader();

        const FAD::DimTemperatures data0(fEventHeader);
        const FAD::DimSetup        data1(fEventHeader);
        const FAD::DimEventHeader  data2(fEventHeader);

        Update(fDimTemperatures, data0);
        Update(fDimSetup,        data1);
        Update(fDimEventHeader,  data2);
    }

public:
    ConnectionDimFAD(ba::io_service& ioservice, MessageImp &imp) :
        ConnectionFAD(ioservice, imp),
        fDimPassport    ("FAD_CONTROL/PASSPORT",     "I:1;S:2;X:1", ""),
        fDimTemperatures("FAD_CONTROL/TEMPERATURES", "I:1;F:4",     ""),
        fDimSetup       ("FAD_CONTROL/SETUP",        "I:2;S:12",    ""),
        fDimEventHeader ("FAD_CONTROL/EVENT_HEADER", "C",           "")
    {
    }

    // 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
};
*/
// ------------------------------------------------------------------------
extern "C"
{
    extern void *readFAD(void*);
    extern void *procEvt(void*);
    extern void *writeEvt(void*);
    extern void initReadFAD();
};

#include "EventBuilder.h"

class EventBuilderWrapper
{
public:
    // FIXME
    static EventBuilderWrapper *This;

private:
    boost::thread fThread;

    enum CommandStates_t // g_runStat
    {
        kAbort      = -2,  // quit as soon as possible ('abort')
        kExit       = -1,  // stop reading, quit when buffered events done ('exit')
        kInitialize =  0,  // 'initialize' (e.g. dim not yet started)
        kHybernate  =  1,  // do nothing for long time ('hybernate') [wakeup within ~1sec]
        kSleep      =  2,  // do nothing ('sleep')                   [wakeup within ~10msec]
        kModeFlush  = 10,  // read data from camera, but skip them ('flush')
        kModeTest   = 20,  // read data and process them, but do not write to disk ('test')
        kModeFlag   = 30,  // read data, process and write all to disk ('flag')
        kModeRun    = 40,  // read data, process and write selected to disk ('run')
    };

    MessageImp &fMsg;

public:
    EventBuilderWrapper(MessageImp &msg) : fMsg(msg)
    {
        if (This)
            throw runtime_error("EventBuilderWrapper cannot be instantiated twice.");

        This = this;
        Start();
    }

    void Update(ostringstream &out, int severity)
    {
        fMsg.Update(out, severity);
    }

    void Start()
    {
        if (fThread.joinable())
        {
            fMsg.Warn("Start - EventBuilder still running");
            return;
        }

        fMsg.Message("Initializing EventBuilder");
        initReadFAD();

        g_runStat = kHybernate;
        fThread = boost::thread(readFAD, (void*)NULL);

        fMsg.Message("EventBuilder started");
    }
    void Abort()
    {
        fMsg.Message("Waiting for EventBuilder to abort...");
        g_runStat = kAbort;
        fThread.join();
        fMsg.Message("EventBuilder stopped.");
    }

    void Exit()
    {
        fMsg.Message("Waiting for EventBuilder to exit - be patient...");
        g_runStat = kExit;
    }

    void Wait()
    {
        fThread.join();
        fMsg.Message("EventBuilder stopped.");
    }

    void Hybernate() { g_runStat = kHybernate; }
    void Sleep()     { g_runStat = kSleep;     }
    void FlushMode() { g_runStat = kModeFlush; }
    void TestMode()  { g_runStat = kModeTest;  }
    void FlagMode()  { g_runStat = kModeFlag;  }
    void RunMode()   { g_runStat = kModeRun;   }

    // FIXME: To be removed
    void SetMode(int mode) { g_runStat = mode; }

    bool IsConnected(int i) const     { return gi_NumConnect[i]==7; }
    bool IsDisconnected(int i) const  { return gi_NumConnect[i]==0; }
    int  GetNumConnected(int i) const { return gi_NumConnect[i]; }

    void Restart()
    {
        Abort();
        Start();
    }

    ~EventBuilderWrapper()
    {
        Abort();
    }
};
/*
extern "C" {

void Error(int severity, int errnum, const char *fmt, ...)
{
    va_list ap;
    va_start(ap, fmt);

    int n=256;

    char *ret=0;
    while (1)
    {
        ret = new char[n+1];

        const int sz = vsnprintf(ret, n, fmt, ap);
        if (sz<=n)
            break;

        n *= 2;
        delete [] ret;
    };

    va_end(ap);

    ostringstream str;
    str << ret << " (" << errnum << ":" << strerror(errnum) << ")";

    delete [] ret;

    EventBuilderWrapper::This->Update(str, severity);
}

}
*/

EventBuilderWrapper *EventBuilderWrapper::This = 0;

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

template <class T>
class StateMachineFAD : public T, public EventBuilderWrapper, public ba::io_service, public ba::io_service::work
{
private:
    typedef pair<string, ConnectionFAD*>    Connection;
    typedef pair<const uint8_t, Connection> Board;
    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(ConnectionFAD::Enable_t command)
    {
        for (BoardList::iterator i=fBoards.begin(); i!=fBoards.end(); i++)
            i->second.second->Cmd(command);

        return T::GetCurrentState();
    }

    int CmdEnable(const EventImp &evt, ConnectionFAD::Enable_t 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]>FAD::kMaxRegAddr)
        {
            ostringstream msg;
            msg << hex << "Address " << dat[0] << " out of range, max=" << maxaddr << ".";
            T::Error(msg);
            return false;
        }

        if (dat[1]>FAD::kMaxRegValue)
        {
            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 = reinterpret_cast<const uint32_t*>(evt.GetData());

        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;

        // ---- was uint32_t
        const int32_t *dat = reinterpret_cast<const int32_t*>(evt.GetData());

        // ---- -1 for all
        //if (!Check(dat, FAD::kMaxRoiAddr, FAD::kMaxRoiValue))
        //            return T::GetCurrentState();

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

        return T::GetCurrentState();
    }

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

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

        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(ConnectionFAD::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(ConnectionFAD::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 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.GetText()[0]!=0);

        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.GetText()[0]!=0);

        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.GetText()[0]!=0);

        return T::GetCurrentState();
    }

    const BoardList::iterator GetSlot(int slot)
    {
        const BoardList::iterator i = fBoards.find(slot);
        if (i!=fBoards.end())
            return i;

        ostringstream str;
        str << "Slot " << slot << " not found.";
        T::Warn(str.str());
        return fBoards.end();
    }

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

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

        AddEndpoint(addr);

        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 v = GetSlot(slot);
        if (v!=fBoards.end())
        {
            delete v->second.second;
            fBoards.erase(v);
        }

        return T::GetCurrentState();
    }

    int ListSlots()
    {
        for (BoardList::const_iterator i=fBoards.begin(); i!=fBoards.end(); i++)
        {
            ostringstream str;
            str << "Slot " << setw(2) << (int)i->first << ": " << i->second.first;

            const ConnectionFAD *c = i->second.second;

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

            switch (fStatus2[i->first])
            {
            case 0: str << "1-7:disconnected)"; break;
            case 1: str << "1-7:connecting [" << GetNumConnected(i->first) << "])";   break;
            case 2: str << "1-7:connected)";    break;
            }

            T::Message(str.str());
        }

        return T::GetCurrentState();
    }

    void EnableSlot(BoardList::iterator i, bool enable=true)
    {
        if (i==fBoards.end())
            return;

        ConnectionFAD* &ptr = i->second.second;

        if (!enable)
            ptr->PostClose(false);
        else
        {
            ptr->SetEndpoint(i->second.first);
            ptr->StartConnect();
        }
    }

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

    /*
    int Enable(const EventImp &evt)
    {
        if (!CheckEventSize(evt.GetSize(), "Enable", 3))
            return T::kSM_FatalError;

        const int16_t slot   = evt.GetShort();
        const bool    enable = evt.GetText()[2]>0;

        if (slot<0)
        {
            EnableAll(enable);
            return T::GetCurrentState();
        }

        EnableSlot(GetSlot(slot), enable);

        return T::GetCurrentState();
    }*/

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

    int Connect()
    {
        T::Error("FIXME - Propagate IP Addresses to EventBuilder");

        Start();
        EnableAll(true);

        return T::GetCurrentState();
   }

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

        const int16_t slot = evt.GetShort();

        if (slot<0)
        {
            // Close all connections to supress the warning in SetEndpoint
            for (BoardList::const_iterator i=fBoards.begin(); i!=fBoards.end(); i++)
                i->second.second->PostClose(false);

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

            // Now we can reopen the connection
            for (BoardList::const_iterator i=fBoards.begin(); i!=fBoards.end(); i++)
                i->second.second->PostClose(true);

            return T::GetCurrentState();
        }

        const BoardList::const_iterator v = GetSlot(slot);
        if (v==fBoards.end())
            return T::GetCurrentState();

        // Close all connections to supress the warning in SetEndpoint
        v->second.second->PostClose(false);

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

        // Now we can reopen the connection
        v->second.second->PostClose(true);

        return T::GetCurrentState();
    }*/

    virtual void UpdateConnectionStatus()
    {
        //cout << "Connection Status changed prop to Dim." << endl;
    }

    vector<char> fStatus1;
    vector<char> 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<char> stat1(40);
        vector<char> stat2(40);
        for (BoardList::const_iterator i=fBoards.begin(); i!=fBoards.end(); i++)
        {
            const ConnectionFAD &c = *i->second.second;

            const int &idx = i->first;

            // ----- Command socket -----
            if (c.IsConnecting())
            {
                stat1[idx] = 1;
                nconnecting1++;
            }
            if (c.IsConnected())
            {
                stat1[idx] = 2;
                nconnected1++;
            }

            // ----- Event builder -----
            if (!IsConnected(idx) && !IsDisconnected(idx))
            {
                stat2[idx] = 1;
                nconnecting2++;
            }

            if (IsConnected(idx))
            {
                stat2[idx] = 2;
                nconnected2++;
            }
        }

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

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

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

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

        if (nconnected1==fBoards.size() && nconnected2==fBoards.size())
            return FAD::kConnected;

        if (nconnected1==0 && nconnected2==0)
            return FAD::kDisconnected;

        // FIXME: Evaluate event builder status
        return FAD::kConnecting;
    }

    void AddEndpoint(const string &addr)
    {
        if (fBoards.size()==40)
        {
            T::Warn("Not more than 40 slots allowed.");
            return;
        }

        int i=0;
        while (1)
        {
            const BoardList::const_iterator v = fBoards.find(i);
            if (v==fBoards.end())
                break;
            i++;
        }

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



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)
    {
        // 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::kDisconnected, "Disconnected",
                        "All enabled FAD boards are disconnected.");

        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("ENABLE_SRCLK", "B:1")
            (boost::bind(&StateMachineFAD::CmdEnable, this, _1, ConnectionFAD::kCmdSrclk))
            ("Set SRCLK");
        T::AddEvent("ENABLE_SCLK", "B:1")
            (boost::bind(&StateMachineFAD::CmdEnable, this, _1, ConnectionFAD::kCmdSclk))
            ("Set SCLK");
        T::AddEvent("ENABLE_DRS", "B:1")
            (boost::bind(&StateMachineFAD::CmdEnable, this, _1, ConnectionFAD::kCmdDrsEnable))
            ("Switch Domino wave");
        T::AddEvent("ENABLE_DWRITE", "B:1")
            (boost::bind(&StateMachineFAD::CmdEnable, this, _1, ConnectionFAD::kCmdDwrite))
            ("Set Dwrite (possibly high / always low)");
        T::AddEvent("SET_DEBUG_MODE", "B:1")
            (boost::bind(&StateMachineFAD::CmdEnable, this, _1, ConnectionFAD::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, ConnectionFAD::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, ConnectionFAD::kCmdContTriggerOn))
            ("");
        T::AddEvent("CONTINOUS_TRIGGER_OFF")
            (boost::bind(&StateMachineFAD::Cmd, this, ConnectionFAD::kCmdContTriggerOff))
            ("");

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

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

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

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

        T::AddEvent("DISCONNECT")
            (boost::bind(&StateMachineFAD::Disconnect, this))
            ("");

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

        T::AddEvent("ADD_ADDRESS", "C", FAD::kDisconnected)
            (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::kDisconnected)
            (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();
    }

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

        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 size_t p0 = base.find_first_of(':');
            const size_t p1 = base.find_last_of(':');

            if (p0==string::npos || p0!=p1)
            {
                T::Out() << kRed << "SetConfiguration - Wrong format of argument --base-addr ('host:port' expected)" << endl;
                return false;
            }

            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() << " " << ec.message() << " (" << ec << ")";
               return false;
            }

            const tcp::endpoint endpoint = *iterator;

            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<2; crate++)
                for (int board=0; board<10; board++)
                {
                    //if (crate==0 && board==2)
                    //    continue;

                    ostringstream str;
                    str << (int)ip[0] << "." << (int)ip[1] << ".";
                    str << (int)(ip[2]+crate) << "." << (int)(ip[3]+board) << ":";
                    str << endpoint.port();

                    AddEndpoint(str.str());
                }
        }

        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++)
                AddEndpoint(*i);
        }

        EnableAll();

        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;

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

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

    po::options_description control("FAD control options");
    control.add_options()
//        ("addr,a",        var<string>("localhost:5000"),  "Network address of FTM")
        ("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.")
        ("addr",       vars<string>(), "Network address of FAD")
        ("base-addr",  var<string>(),  "Base address of all FAD")
        ;

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

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

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