#include <boost/array.hpp>

#include <string>
#include <queue>

#include "FACT.h"
#include "Dim.h"
#include "Event.h"
#include "StateMachineDim.h"
#include "StateMachineAsio.h"
#include "Connection.h"
#include "LocalControl.h"
#include "Configuration.h"
#include "Console.h"

#include "tools.h"

#include "HeadersMiniFTM.h"

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

using namespace std;

class ConnectionMiniFTM : public Connection
{
public:
    static bool fIsFACT;

private:
    bool fIsVerbose;
    bool fDebugRx;

    uint32_t fInterval;
    uint16_t fTimerFreq;

    boost::asio::deadline_timer fRxTimeout;
    boost::asio::deadline_timer fTempTimer;

    vector<uint8_t> fBuffer;

    bool fIsInitializing;

    MiniFTM::Config fConf;
    MiniFTM::Temp   fTemp;

    virtual void UpdateConfiguration(const MiniFTM::Config &)
    {
    }

    virtual void UpdateTemperatures(uint8_t, const MiniFTM::Temp &)
    {
    }

    virtual void UpdateTrigger(uint8_t, const uint64_t &)
    {
    }

    queue<MiniFTM::BusData> fQueue;

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

        if (error)
        {
            ostringstream str;
            str << "Read timeout: " << error.message() << " (" << error << ")";
            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 (fRxTimeout.expires_at() > ba::deadline_timer::traits_type::now())
        //    return;

        Error("Timeout ("+to_simple_string(fRxTimeout.expires_from_now())+") reading data.");
        PostClose();
    }

    void TriggerTempTimer()
    {
        if (fInterval==0)
            return;

        fTempTimer.expires_from_now(boost::posix_time::milliseconds(fInterval));
        fTempTimer.async_wait(boost::bind(&ConnectionMiniFTM::HandleTempTimer, this, dummy::error));
    }

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

        if (error)
        {
            ostringstream str;
            str << "Temp timer: " << error.message() << " (" << error << ")";
            Error(str);

            PostClose();
            return;

        }

        if (!is_open())
            return;

        SendRead(MiniFTM::kADCs);
        TriggerTempTimer();
    }


    void HandleReceivedData(const boost::system::error_code& err, size_t bytes_received, int)
    {
        // Do not schedule a new read if the connection failed.
        if (bytes_received!=1024 || err)
        {
            // 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;
        }

        // Keep time of (async) data reception
        const Time time;

        MiniFTM::BusData &data = *reinterpret_cast<MiniFTM::BusData*>(fBuffer.data());

        // Print raw message in verbose mode
        if (fDebugRx)
            Out() << "RX|" << data << endl;

        // Some sanity checks
        if (data.fStartBits!=0xffff || data.fStopBits!=0xffff)
        {
            Error(Tools::Form("Frame bytes mismatch (%04x|%04x)",
                              data.fStartBits, data.fStopBits));
            PostClose();
            return;
        }

        if (!data.isCrcValid())
        {
            Error(Tools::Form("Checksum mismatch (Received: %d, Expected: %d)", data.fCrc, data.calcCrc()));
            PostClose();
            return;
        }

        if (data.fReadWrite==MiniFTM::kCmdError)
        {
            ostringstream msg;
            msg << "MiniFTM returned an error: ";
            switch (data.fData)
            {
            case MiniFTM::kErrFrameStart:   msg << "Start bytes wrong"; break;
            case MiniFTM::kErrFrameStop:    msg << "Stop bytes wrong"; break;
            case MiniFTM::kErrFrameCrc:     msg << "Checksum error"; break;
            case MiniFTM::kErrUnknownCmd:   msg << "Command unknown"; break;
            case MiniFTM::kErrForbiddenCmd: msg << "Command not allowed"; break;
            default: msg << "Unknwon error"; break;
            }
            msg << Tools::Form(" [%04x]", data.fData);
            Error(msg);
            PostClose();
            return;
        }

        if (fQueue.empty() && data.fReadWrite!=MiniFTM::kCmdADM)
        {
            Error(Tools::Form("Unexpected answer [%02x|%04x] received.", data.fReadWrite, data.fCommand));
            PostClose();
            return;
        }

        if (!fQueue.empty() && data.fReadWrite!=MiniFTM::kCmdADM && fQueue.front().id() != data.id())
        {
            Error(Tools::Form("Command mismatch (Received: %06x, Expected: %06x)", data.id(), fQueue.front().id()));
            PostClose();
            return;
        }

        // Requested Message received -> cancel timeout
        if (!fQueue.empty() && data.fReadWrite!=MiniFTM::kCmdADM)
            fRxTimeout.cancel();

        switch (data.fCommand)
        {
        case MiniFTM::kRegProductId:
            fConf.fProductId = data.fData;
            UpdateConfiguration(fConf);
            Info(Tools::Form("Product ID = 0x%x", data.fData));
            // FIXME: Check for validity
            break;

        case MiniFTM::kRegFirmwareId:
            fConf.fFirmwareId = data.fData;
            UpdateConfiguration(fConf);
            Info(Tools::Form("Firmware = 0x%x", data.fData));
            // FIXME: Check for validity
            break;

        case MiniFTM::kClockEnable:
            fConf.fClockState = data.fCommand>>8;
            UpdateConfiguration(fConf);
            Info("Clock enabled.");
            break;

        case MiniFTM::kClockDisable:
            fConf.fClockState = data.fCommand>>8;
            UpdateConfiguration(fConf);
            Info("Clock disabled.");
            break;

        case MiniFTM::kClockShutdown:
            fConf.fClockState = data.fCommand>>8;
            UpdateConfiguration(fConf);
            Info("Clock shut down.");
            break;

        case MiniFTM::kClockFrequency:
            {
                const uint16_t dac = (data.fData>>2) &0x3ff;
                const uint16_t oct = (data.fData>>12)&0xf;

                const double freq = pow(2, oct)*2078/(2-dac/1024.)/1000;

                fConf.fClockFrequency64 = data.fData;
                fConf.fClockFrequencyD  = freq;

                UpdateConfiguration(fConf);

                Info(Tools::Form("Clock frequency = %.2f kHz [dac=%d; oct=%d]", freq, dac, oct));
            }
            break;

        case MiniFTM::kTriggerFixedRate:
            fConf.fTriggerState = data.fCommand>>8;
            UpdateConfiguration(fConf);
            Info("Fixed rate trigger turned on ["+to_string(fConf.fTriggerState)+"]");
            break;

        case MiniFTM::kTriggerExternal:
            fConf.fTriggerState = data.fCommand>>8;
            UpdateConfiguration(fConf);
            Info("External trigger turned on ["+to_string(fConf.fTriggerState)+"]");
            break;

        case MiniFTM::kTriggerRandom:
            fConf.fTriggerState = data.fCommand>>8;
            UpdateConfiguration(fConf);
            Info("Random trigger turned on ["+to_string(fConf.fTriggerState)+"]");
            break;

        case MiniFTM::kTriggerShutdown:
            fConf.fTriggerState = data.fCommand>>8;
            UpdateConfiguration(fConf);
            Info("Trigger turned off ["+to_string(fConf.fTriggerState)+"]");
            break;

        case MiniFTM::kTriggerRS485On:
            fConf.fRS485OnOff = data.fCommand>>8;
            UpdateConfiguration(fConf);
            Info("RS485 communication turned on.");
            break;

        case MiniFTM::kTriggerRS485Off:
            fConf.fRS485OnOff = data.fCommand>>8;
            UpdateConfiguration(fConf);
            Info("RS485 communication turned off.");
            break;

        case MiniFTM::kTriggerFrequency:
            {
                const double freq = fTimerFreq/(data.fData+1.);  // old: 2*4150.

                fConf.fTriggerFrequency64 = data.fData;
                fConf.fTriggerFrequencyD  = freq;

                UpdateConfiguration(fConf);

                Info(Tools::Form("Trigger frequency = %.2f Hz (%d)", freq, data.fData));
                break;
            }

        case MiniFTM::kFadReset:
            Info("FAD reset.");
            break;

        case MiniFTM::kFadResetCycles:
            Info(Tools::Form("FAD Reset Length = %ld cycles", data.fData));
            break;

        case MiniFTM::kFadResetActiveHi:
            Info(Tools::Form("FAD Reset is Active %s (%x)", (data.fData?"HI":"LO"), data.fData));
            break;

        case MiniFTM::kADC1:
            {
                fTemp.SetADC1(data.fData);
                UpdateTemperatures(1, fTemp);
                Info(Tools::Form("ADC1 = %.1f degC (%04x)", fTemp.fTemp1, fTemp.fADC1));
                break;
            }

        case MiniFTM::kADC2:
            {
                fTemp.SetADC2(data.fData);
                UpdateTemperatures(2, fTemp);
                Info(Tools::Form("ADC2 = %.1f degC (%0x4)", fTemp.fTemp2, fTemp.fADC2));
                break;
            }

        case MiniFTM::kADCs:
            {
                fTemp.SetADC1(data.fData&0xffff);
                fTemp.SetADC2(data.fData>>16);

                UpdateTemperatures(3, fTemp);

                // if (fIsVerbose)
                Info(Tools::Form("ADC1/2 = %.1f / %.1f degC (%04x/%04x)",
                                 fTemp.fTemp1, fTemp.fTemp2, fTemp.fADC1, fTemp.fADC2));
                break;
            }

        case MiniFTM::kRS485Data:
            fConf.fRS485Data = data.fData;
            UpdateConfiguration(fConf);
            Info(Tools::Form("RS485 data = %016lx", data.fData));
            break;

        case MiniFTM::kSingleTrigger:
            Info("Single trigger.");
            break;

        case MiniFTM::kTriggerCounter:
            if (fIsVerbose)
                Info("Trigger counter: "+to_string(data.fData&0xffff)+" (busy="+to_string(data.fData>>32)+")");
            UpdateTrigger(0, data.fData);
            break;

        case MiniFTM::kConfiguration:
            {
                Info("RS485 communication "+string(data.fData&1?"on":"off"));

                ostringstream out;
                switch (data.fData&0x06)
                {
                case 0x02:
                    fConf.fTriggerState = data.fData&0x8?MiniFTM::kRandom:MiniFTM::kFixedRate;
                    out << (data.fData&0x8?"random":"fixed rate");
                    break;
                case 0x04:
                    fConf.fTriggerState = MiniFTM::kExternal;
                    out << "external";
                    break;

                case 0x06:
                    fConf.fTriggerState = MiniFTM::kShutdown;
                    out << (data.fData&0x10?"off/hi":"off/lo");
                    break;
                }
                Info("Trigger status: "+out.str());

                fConf.fRS485OnOff = data.fData&1;
                fConf.fConfiguration = data.fData;
                UpdateConfiguration(fConf);
            }
            break;

        case MiniFTM::kRegError:
            //fDimData.SetErrorReg(data.fData[0]);
            //UpdateData(time, data.id(), fDimData);
            if (data.fData)
            {
                // Automatically acknowledge the error (Good idea?)
                SendWrite(MiniFTM::kRegError);
                Error("MiniFTM reported internal error "+to_string(data.fData)+".");
            }
            else
                Info(data.fReadWrite==MiniFTM::kCmdRead?"No internal error reported by MiniFTM.":"Internal error reported by MiniFTM cleared.");
            break;

        case MiniFTM::kEnableADM:
            Info(string("Automatic data sending mode (ADM) ")+(data.fData?"enabled":"disabled"));
            //fDimConf.set(PSU::kBitADM, data[0]);
            //UpdateConfig(time, data.id(), fDimConf);
            break;

        default:
            Error(Tools::Form("Unknown command byte received (%d)", data.fCommand));
            PostClose();
            return;
        }

        // Start reading of next package
        AsyncRead(ba::buffer(fBuffer));

        // If this was an automatic package no further handling should be done
        if (data.fReadWrite==MiniFTM::kCmdADM)
            return;

        // Remove the request for which we just processed the answer from
        // the queue. This could have a check for an empty queue, but an
        // empty queue here should never happen!
        fQueue.pop();

        // If this is the answer to the last sent initialization request
        // Initialization is done
        if (fQueue.empty() && fIsInitializing)
        {
            TriggerTempTimer();
            PrintConfig();
            fIsInitializing = false;
            return;
        }

        // send next request if queue not empty
        PostCommandFromQueue();
    }

    void PostCommandFromQueue()
    {
        if (fQueue.empty())
            return;

        const MiniFTM::BusData &dat = fQueue.front();

        PostMessage(&dat, sizeof(dat));
        if (GetDebugTx())
            Out() << "TX|" << dat << endl;

        AsyncWait(fRxTimeout, 1000, &Connection::HandleReadTimeout);
    }

public:
    void SendCommand(uint8_t rw, uint16_t cmd, uint64_t d0=0)
    {
        fQueue.emplace(rw, cmd, d0);
        if (fQueue.size()==1)
            PostCommandFromQueue();
    }

    void SendWrite(uint16_t cmd, uint64_t val=0)
    {
        SendCommand(MiniFTM::kCmdWrite, cmd, val);
    }

    void SendRead(uint16_t cmd, uint64_t val=0)
    {
        SendCommand(MiniFTM::kCmdRead, cmd, val);
    }

    // This is called when a connection was established
    void ConnectionEstablished()
    {
        Info("Connection established to "+URL()+"...");

        fQueue = queue<MiniFTM::BusData>();

        SendRead(MiniFTM::kRegProductId);
        SendRead(MiniFTM::kRegFirmwareId);
        SendRead(MiniFTM::kRegError);

        SendRead(MiniFTM::kClockFrequency);

        SendWrite(MiniFTM::kClockEnable);
        SendRead(MiniFTM::kClockFrequency);

        SendWrite(MiniFTM::kTriggerShutdown);
        SendRead(MiniFTM::kTriggerFrequency);

        //SendWrite(MiniFTM::kTriggerRS485On); // Done when trigger is truned on

        SendRead(MiniFTM::kFadResetCycles);
        SendRead(MiniFTM::kFadResetActiveHi);

        SendRead(MiniFTM::kConfiguration);
        SendRead(MiniFTM::kADCs);

        SendWrite(MiniFTM::kEnableADM, true);

        fIsInitializing = true;

        AsyncRead(ba::buffer(fBuffer));
    }

public:
    ConnectionMiniFTM(ba::io_service& ioservice, MessageImp &imp) : Connection(ioservice, imp()),
        fIsVerbose(true), fDebugRx(false),
        fRxTimeout(ioservice), fTempTimer(ioservice),
        fBuffer(1024), fIsInitializing(false)
    {
        SetLogStream(&imp);
    }

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

    void SetDebugRx(bool b)
    {
        fDebugRx = b;
        Connection::SetVerbose(b);
    }

    void SetDebugTx(bool b)
    {
        Connection::SetDebugTx(b);
    }

    int GetState() const
    {
        if (!IsConnected())
            return MiniFTM::State::kDisconnected;

        if (fIsInitializing)
            return MiniFTM::State::kConnected;

        return MiniFTM::State::kValid;
    }

    size_t GetQueueSize() const
    {
        return fQueue.size();
    }

    uint8_t GetTriggerState() const
    {
        return fConf.fTriggerState;
    }

    uint8_t IsTriggerOn() const
    {
        return fConf.fTriggerState!=MiniFTM::kShutdown;
    }

    void PrintConfig()
    {
        Out() << fConf;
    }

    void SetInterval(uint32_t i)
    {
        fTempTimer.cancel();
        fInterval = i;
        if (IsConnected() && !fIsInitializing)
            TriggerTempTimer();
    }

    void SetTimerFreq(uint16_t i)
    {
        fTimerFreq = i;
    }
};

bool ConnectionMiniFTM::fIsFACT = true;

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

#include "DimDescriptionService.h"

class ConnectionDimMiniFTM : public ConnectionMiniFTM
{
private:
    DimDescribedService fDimConfig;
    DimDescribedService fDimTemp;
    DimDescribedService fDimTrigger;

public:
    ConnectionDimMiniFTM(ba::io_service& ioservice, MessageImp &imp) :
        ConnectionMiniFTM(ioservice, imp),
        fDimConfig(fIsFACT?"MINIFTM_CONTROL/CONFIGURATION":"FTM_CONTROL/CONFIGURATION",
                   "X:1;X:1;C:1;X:1;D:1;C:1;C:1;X:1;D:1;X:1;X:1",
                   "|firmware[uint64]:Firmware ID"
                   "|product[uint64]:Product ID"
                   "|clk_state[uint8]:Clock state"
                   "|clk_freq_raw[uint64]:Clock frequency (raw)"
                   "|clk_freq[Hz]:Clock frequency"
                   "|trg_mode[uint8]:Trigger Mode"
                   "|rs485_state[uint8]:RS485 state"
                   "|trg_freq_raw[uint64]:Trigger frequency (raw)"
                   "|trg_freq[Hz]:Trigger frequency"
                   "|configuration[uint64]:Trigger and RS485 configuration bits"
                   "|rs485_data[uint64]:RS485 data"),
        fDimTemp(fIsFACT?"MINIFTM_CONTROL/TEMPERATURES":"FTM_CONTROL/TEMPERATURES",
                 "S:2;F:2",
                   "|adc[uint16]:ADC counts"
                   "|temp[degC]:Corresponding temperatures"),
        fDimTrigger(fIsFACT?"MINIFTM_CONTROL/TRIGGER_COUNTER":"FTM_CONTROL/TRIGGER_COUNTER",
                    "S:1;S:1",
                   "|counter[uint32]:Trigger counter (incoming and internal)"
                   "|busy[uint32]:Counter of suppressed triggers")
    {
    }

    void UpdateConfiguration(const MiniFTM::Config &conf)
    {
        //fDim.setQuality(status.GetVal());
    	fDimConfig.setData(conf);
        fDimConfig.Update();
    }

    void UpdateTemperatures(uint8_t qos, const MiniFTM::Temp &temp)
    {
        fDimTemp.setQuality(qos);
    	fDimTemp.setData(temp);
        fDimTemp.Update();
    }

    void UpdateTrigger(uint8_t qos, const uint64_t &data)
    {
        fDimTrigger.setQuality(qos);
        fDimTrigger.setData(data);
        fDimTrigger.Update();
    }
};

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

template <class T, class S>
class StateMachineMiniFTM : public StateMachineAsio<T>
{
private:
    S fFTM;
    Time fLastCommand;

    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 SetVerbosity(const EventImp &evt)
    {
        if (!CheckEventSize(evt.GetSize(), "SetVerbosity", 1))
            return T::kSM_FatalError;

        fFTM.SetVerbose(evt.GetBool());

        return T::GetCurrentState();
    }

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

        fFTM.SetDebugRx(evt.GetBool());

        return T::GetCurrentState();
    }

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

        fFTM.SetDebugTx(evt.GetBool());

        return T::GetCurrentState();
    }

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

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

        return T::GetCurrentState();
    }

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

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

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

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

        return T::GetCurrentState();
    }

    int PrintConfig()
    {
        fFTM.PrintConfig();
        return T::GetCurrentState();
    }

    uint16_t fTriggerMode; // Kommand to be sent to turn the trigger on

    int Configure(const EventImp &evt)
    {
        const string name = evt.GetText();

        auto it = fRunTypes.find(name);
        if (it==fRunTypes.end())
        {
            T::Info("Configure - Run-type '"+name+"' not found... trying 'default'.");

            it = fRunTypes.find("default");
            if (it==fRunTypes.end())
            {
                T::Error("Configure - Run-type 'default' not found.");
                return T::GetCurrentState();
            }
        }

        Dim::SendCommand("FTU_CONTROL/ENABLE_PRESCALING", uint8_t(0));

        fFTM.SendWrite(MiniFTM::kTriggerShutdown);
        fFTM.SendWrite(MiniFTM::kTriggerRS485On);
        fFTM.SendWrite(MiniFTM::kTriggerFrequency, it->second.fTriggerRate);
        fFTM.SendWrite(MiniFTM::kRS485Data);

        fTriggerMode = MiniFTM::kTriggerShutdown;
        if (it->second.fTriggerType=="fixedrate")
            fTriggerMode = MiniFTM::kTriggerFixedRate;
        if (it->second.fTriggerType=="external")
            fTriggerMode = MiniFTM::kTriggerExternal;
        if (it->second.fTriggerType=="random")
            fTriggerMode = MiniFTM::kTriggerRandom;

        return MiniFTM::State::kConfiguring;
    }

    int ResetConfig()
    {
        return fFTM.GetState();
    }

    int StartTrigger()
    {
        fFTM.SendWrite(fTriggerMode);
        return T::GetCurrentState();//MiniFTM::State::kTriggerOn;
    }

    int ReadRegister(uint16_t cmd)
    {
        fFTM.SendRead(cmd);

        return T::GetCurrentState();
    }

    int WriteRegister(uint16_t cmd)
    {
        fFTM.SendWrite(cmd);

        return T::GetCurrentState();
    }

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

        fFTM.SendWrite(cmd, evt.Get<uint64_t>());

        return T::GetCurrentState();
    }

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

        fFTM.SendWrite(MiniFTM::kTriggerShutdown, evt.GetBool());

        return T::GetCurrentState();
    }

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

        const uint16_t m = evt.Get<uint16_t>(0);
        const uint16_t e = evt.Get<uint16_t>(2);

        if (m>0x3ff)
        {
            T::Warn("Clock frequency matinsse exceeds allowed range (10 bit)... ignored.");
            return T::GetCurrentState();
        }

        if (e>0xf)
        {
            T::Warn("Clock frequency exponent exceeds allowed range (4 bit)... ignored.");
            return T::GetCurrentState();
        }

        fFTM.SendWrite(MiniFTM::kClockFrequency, (e<<12)|(m<<2));

        return T::GetCurrentState();
    }

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

        if (evt.GetUInt()<5)
        {
            T::Warn("Trigger frequency too high... ignored.");
            return T::GetCurrentState();
        }
        if (evt.GetUInt()>0xffff)
        {
            T::Warn("Trigger frequency exceeds allowed range (16 bit)... ignored.");
            return T::GetCurrentState();
        }

        fFTM.SendWrite(MiniFTM::kTriggerFrequency, evt.GetUInt());

        return T::GetCurrentState();
    }
/*
    int SetRS485Mode(const EventImp &evt)
    {
        if (!CheckEventSize(evt.GetSize(), "SetRS485Mode", 8))
            return T::kSM_FatalError;

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

        uint64_t data = 0;
        data |= uint64_t(ptr[0])<<40;          // baud (word2)
        data |= uint64_t(ptr[1])<<32;          // baud (word2)
        data |= uint64_t(ptr[2])<<24;          // baud (word3)
        data |= uint64_t(ptr[3])<<16;          // baud (word3)
        data |= uint64_t(ptr[4]&1)<<(40+15);   // PEN
        data |= uint64_t(ptr[5]&1)<<(40+14);   // PAR
        data |= uint64_t(ptr[6]&1)<<(40+13);   // SPB
        data |= uint64_t(ptr[7]&1)<<(40+11);   // MSB

        fFTM.SendWrite(MiniFTM::kRS485Mode, data);

        return T::GetCurrentState();
    }
*/
    int SetInterval(const EventImp &evt)
    {
        if (!CheckEventSize(evt.GetSize(), "SetInterval", 8))
            return T::kSM_FatalError;

        if (evt.GetUXtra()>0xffffffff)
        {
            T::Warn("Interval out of allowed range [32 bit]... ignored.");
            return T::GetCurrentState();
        }

        fFTM.SetInterval(evt.GetUXtra());

        return T::GetCurrentState();
    }

    int Execute()
    {
        if (fFTM.GetState()<MiniFTM::State::kValid)
            return fFTM.GetState();

        switch (T::GetCurrentState())
        {
        case MiniFTM::State::kConfiguring:
            return fFTM.GetQueueSize()==0 ? MiniFTM::State::kConfigured : MiniFTM::State::kConfiguring;

        case MiniFTM::State::kConfigured:
            return fFTM.IsTriggerOn() ? MiniFTM::State::kTriggerOn : MiniFTM::State::kConfigured;

        case MiniFTM::State::kTriggerOn:
            return fFTM.IsTriggerOn() ? MiniFTM::State::kTriggerOn : MiniFTM::State::kValid;
        }

        return fFTM.GetState();
    }

public:
    StateMachineMiniFTM(ostream &out=cout) :
        StateMachineAsio<T>(out, ConnectionMiniFTM::fIsFACT?"MINIFTM_CONTROL":"FTM_CONTROL"),
        fFTM(*this, *this)
    {
        // State names
        T::AddStateName(MiniFTM::State::kDisconnected, "Disconnected",
                     "No ethernet connection established");

        T::AddStateName(MiniFTM::State::kConnected, "Connected",
                     "Connection established, requesting configuration");

        T::AddStateName(MiniFTM::State::kValid, "Valid",
                     "Connection established, valid configuration received");

        T::AddStateName(MiniFTM::State::kConfiguring, "Configuring",
                     "Configuring FTM for data taking");

        T::AddStateName(MiniFTM::State::kConfigured, "Configured",
                     "Ready for data taking, ready to enable trigger");

        T::AddStateName(MiniFTM::State::kTriggerOn, "TriggerOn",
                     "Trigger enabled");


        // Verbosity commands
        T::AddEvent("SET_VERBOSE", "B:1")
            (bind(&StateMachineMiniFTM::SetVerbosity, this, placeholders::_1))
            ("Set verbosity state"
             "|verbosity[bool]:disable or enable verbosity for interpreted data (yes/no)");

        T::AddEvent("SET_DEBUG_RX", "B:1")
            (bind(&StateMachineMiniFTM::SetDebugRx, this, placeholders::_1))
            ("Set debux-rx state"
             "|debug[bool]:dump received message to console (yes/no)");

        T::AddEvent("SET_DEBUG_TX", "B:1")
            (bind(&StateMachineMiniFTM::SetDebugTx, this, placeholders::_1))
            ("Set debux-tx state"
             "|debug[bool]:dump outgoing message to console (yes/no)");


        // Device control
        T::AddEvent("READ_PRODUCT_ID", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::ReadRegister, this, MiniFTM::kRegProductId))
            ("Read product identification");

        T::AddEvent("READ_FIRMWARE_ID", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::ReadRegister, this, MiniFTM::kRegFirmwareId))
            ("Read firmware version");

        T::AddEvent("READ_CLOCK_STATE", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::ReadRegister, this, MiniFTM::kClockState))
            ("Read clock state");

        T::AddEvent("READ_CLOCK_FREQUENCY", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::ReadRegister, this, MiniFTM::kClockFrequency))
            ("Read clock frequency");

        //T::AddEvent("READ_TRIGGER_MODE", MiniFTM::State::kValid)
        //    (bind(&StateMachineMiniFTM::ReadRegister, this, MiniFTM::kTriggerState))
        //    ("Read trigger mode");

        T::AddEvent("READ_TRIGGER_FREQUENCY", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::ReadRegister, this, MiniFTM::kTriggerFrequency))
            ("Read trigger frequency");

        T::AddEvent("READ_CONFIGURATION", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::ReadRegister, this, MiniFTM::kConfiguration))
            ("Read some configuration bits");

        T::AddEvent("RESET_FAD", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::ReadRegister, this, MiniFTM::kFadReset))
            ("Send FAD reset");

        T::AddEvent("READ_ADC1", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::ReadRegister, this, MiniFTM::kADC1))
            ("Read ADC1 (Temp1)");

        T::AddEvent("READ_ADC2", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::ReadRegister, this, MiniFTM::kADC2))
            ("Read ADC2 (Temp2)");

        T::AddEvent("READ_TEMPERATURES", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::ReadRegister, this, MiniFTM::kADCs))
            ("Read both temperatures (ADCs)");


        T::AddEvent("READ_FAD_RESET_CYCLES", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::ReadRegister, this, MiniFTM::kFadResetCycles))
            ("Read number of cycles of FAD reset");

        T::AddEvent("READ_FAD_RESET_ACTIVE_HI", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::ReadRegister, this, MiniFTM::kFadResetActiveHi))
            ("Set when FAD reset is active hi");



        T::AddEvent("SET_CLOCK_FREQUENCY", "S:1;S:1", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::SetClockFrequency, this, placeholders::_1))
            ("Set clock frequency 2^oct*2078Hz/(2-dac/1024)"
             "|dac[uint16]:Value DAC (10 bit)"
             "|oct[uint16]:Value OCT (4 bit)");

        T::AddEvent("SET_TRIGGER_FREQUENCY", "I:1", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::SetTriggerFrequency, this, placeholders::_1))
            ("Set trigger frequency"
             "|dac[Hz]:Clock frequency (16 bit): DAC = 2*4150Hz/f - 1");



        T::AddEvent("ENABLE_CLOCK", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::WriteRegister, this, MiniFTM::kClockEnable))
            ("Enable clock");

        T::AddEvent("DISABLE_CLOCK", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::WriteRegister, this, MiniFTM::kClockDisable))
            ("Disable clock");

        T::AddEvent("SHUTDOWN_CLOCK", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::WriteRegister, this, MiniFTM::kClockShutdown))
            ("Shutdown clock");


        T::AddEvent("ENABLE_FIXED_RATE_TRIGGER", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::WriteRegister, this, MiniFTM::kTriggerFixedRate))
            ("Enable fixed rate trigger");

        T::AddEvent("ENABLE_EXTERNAL_TRIGGER", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::WriteRegister, this, MiniFTM::kTriggerExternal))
            ("Enable external trigger");

        T::AddEvent("ENABLE_RANDOM_TRIGGER", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::WriteRegister, this, MiniFTM::kTriggerRandom))
            ("Enable random trigger");

        T::AddEvent("SHUTDOWN_TRIGGER", "B:1", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::ShutdownTrigger, this, placeholders::_1))
            ("Shutdown trigger"
             "|hilo[bool]:Set hi or lo state after shutdown");


/*
        T::AddEvent("SET_RS485_MODE", "S:2;B:4", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::SetRS485Mode, this, placeholders::_1))
            ("Set the RS485 mode"
             "|BAUD0[uint16]:Baud rate (word 2)"
             "|BAUD1[uint16]:Baud rate (word 3)"
             "|PEN[bool]:Parity enabled (0: disabled, 1: enabled)"
             "|PAR[bool]:Parity even (0: odd, 1: even)"
             "|SPB[bool]:Stop bits (0: one, 1: two)"
             "|MSB[bool]:Most Significant Bit First (MSB) (0: LSB, 1: MSB)");
*/

        T::AddEvent("ENABLE_RS485", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::WriteRegister, this, MiniFTM::kTriggerRS485On))
            ("Enable RS485 communication.");

        T::AddEvent("DISABLE_RS485", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::WriteRegister, this, MiniFTM::kTriggerRS485Off))
            ("Disable RS485 communication.");



        T::AddEvent("READ_RS485_DATA", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::ReadRegister, this, MiniFTM::kRS485Data))
            ("Read RS485 data");

        T::AddEvent("SET_RS485_DATA", "X:1", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::WriteRegister64, this, placeholders::_1, MiniFTM::kRS485Data))
            ("Set RS485 data");



        T::AddEvent("SET_FAD_RESET_CYCLES", "X:1", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::WriteRegister64, this, placeholders::_1, MiniFTM::kFadResetCycles))
            ("Set number of Cycles of FAD reset"
             "|cycles[uint16]:Number of cycles (min: 10, 16 bit)");


        T::AddEvent("SET_FAD_RESET_ACTIVE_HI", "X:1", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::WriteRegister64, this, placeholders::_1, MiniFTM::kFadResetActiveHi))
            ("Set whether FAD reset is active hi"
             "|hi[bool]:Active hi");



        T::AddEvent("SINGLE_TRIGGER", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::WriteRegister, this, MiniFTM::kSingleTrigger))
            ("Issue single trigger");



        T::AddEvent("ERROR", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::WriteRegister, this, 0x9999))
            ("Send an errorneous command (debugging purpose)");



        T::AddEvent("PRINT_CONFIGURATION")
            (bind(&StateMachineMiniFTM::PrintConfig, this))
            ("Print the current configuration as available in memory");


        // A new configure will first stop the FTM this means
        // we can allow it in idle _and_ taking data
        T::AddEvent("CONFIGURE", "C", MiniFTM::State::kValid, MiniFTM::State::kTriggerOn)
            (bind(&StateMachineMiniFTM::Configure, this, placeholders::_1))
            ("Configure a new run");

        T::AddEvent("RESET_CONFIGURE", MiniFTM::State::kConfigured)
            (bind(&StateMachineMiniFTM::ResetConfig, this))
            ("Reset states during a configuration or in case of configuration error");

        T::AddEvent("START_TRIGGER", MiniFTM::State::kConfigured)
            (bind(&StateMachineMiniFTM::StartTrigger, this))
            ("Start trigger as configured by CONFIGURE");

        T::AddEvent("STOP_TRIGGER", MiniFTM::State::kTriggerOn)
            (bind(&StateMachineMiniFTM::WriteRegister, this, MiniFTM::kTriggerShutdown))
            ("Disable all triggers");



        T::AddEvent("SET_INTERVAL", "X:1", MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::SetInterval, this, placeholders::_1))
            ("Set temperature request interval"
             "|dt[uint32]:Interval in ms (0=off)");



        // Conenction commands
        T::AddEvent("DISCONNECT", MiniFTM::State::kConnected, MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::Disconnect, this))
            ("Disconnect from ethernet");

        T::AddEvent("RECONNECT", "O", MiniFTM::State::kDisconnected, MiniFTM::State::kConnected, MiniFTM::State::kValid)
            (bind(&StateMachineMiniFTM::Reconnect, this, placeholders::_1))
            ("(Re)connect ethernet connection, a new address can be given"
             "|[host][string]:new ethernet address in the form <host:port>");
    }

    map<string, MiniFTM::RunType> fRunTypes;

    template<typename _t>
    bool GetConfig(Configuration &conf, const string &name, const string &sub, _t &rc)
    {
        if (conf.HasDef(name, sub))
        {
            rc = conf.GetDef<_t>(name, sub);
            return true;
        }

        T::Error("Neither "+name+"default nor "+name+sub+" found.");
        return false;
    }

    int EvalOptions(Configuration &conf)
    {
        fFTM.SetVerbose(!conf.Get<bool>("quiet"));
        fFTM.SetDebugTx(conf.Get<bool>("debug-tx"));
        fFTM.SetDebugRx(conf.Get<bool>("debug-rx"));
        fFTM.SetEndpoint(conf.Get<string>("addr"));
        fFTM.SetInterval(conf.Get<uint32_t>("interval"));
        fFTM.SetTimerFreq(conf.Get<uint16_t>("timer-frequency"));

        // ---------- Setup run types ---------
        const vector<string> types = conf.Vec<string>("run-type");
        if (types.empty())
            T::Warn("No run-types defined.");
        else
            T::Message("Defining run-types");

        for (auto it=types.begin(); it!=types.end(); it++)
        {
            T::Message(" -> "+ *it);

            if (fRunTypes.count(*it)>0)
            {
                T::Error("Run-type "+*it+" defined twice.");
                return 1;
            }

            MiniFTM::RunType &c = fRunTypes[*it];
            if (!GetConfig(conf, "trigger-rate.", *it, c.fTriggerRate) ||
                !GetConfig(conf, "trigger-type.", *it, c.fTriggerType))
                return 2;
        }

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

        fFTM.StartConnect();

        return -1;
    }
};

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

#include "Main.h"


template<class T, class S, class R>
int RunShell(Configuration &conf)
{
#if BOOST_VERSION < 104600
    const string fname = boost::filesystem::path(conf.GetName()).filename();
#else
    const string fname = boost::filesystem::path(conf.GetName()).filename().string();
#endif

    ConnectionMiniFTM::fIsFACT = fname!="ftmctrl";

    return Main::execute<T, StateMachineMiniFTM<S, R>>(conf);
}

void SetupConfiguration(Configuration &conf)
{
    po::options_description control("Interlock control");
    control.add_options()
        ("no-dim,d",   po_switch(),     "Disable dim services")
        ("addr,a",     var<string>(""), "Network address of the lid controling Arduino including port")
        ("quiet,q",    po_bool(true),   "Disable printing contents of all received messages (except dynamic data) in clear text.")
        ("debug-tx",   po_bool(),       "Enable debugging of ethernet transmission.")
        ("debug-rx",   po_bool(),       "Enable debugging for received data.")
        ("interval",   var<uint32_t>(15000), "Interval in which temperatures are requested [ms]")
        ("timer-frequency", var<uint16_t>(32768), "Frequency of internal timer module [Hz]")
        ;

    po::options_description runtype("Run type configuration");
    runtype.add_options()
        ("run-type",        vars<string>(),  "Name of run-types (replace the * in the following configuration by the case-sensitive names defined here)")
        ("trigger-type.*",  var<string>(),   "Calibration type ('fixedrate', 'random', 'external', 'off')")
        ("trigger-rate.*",  var<uint16_t>(), "Target rate for calibration by rate")
    ;

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

/*
 Extract usage clause(s) [if any] for SYNOPSIS.
 Translators: "Usage" and "or" here are patterns (regular expressions) which
 are used to match the usage synopsis in program output.  An example from cp
 (GNU coreutils) which contains both strings:
  Usage: cp [OPTION]... [-T] SOURCE DEST
    or:  cp [OPTION]... SOURCE... DIRECTORY
    or:  cp [OPTION]... -t DIRECTORY SOURCE...
 */
void PrintUsage()
{
    cout <<
        "The ftmctrl is a hardware interface to the MiniFTM board built for FAMOUS"
        "\n"
        "The default is that the program is started with 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: ftmctrl [-c type] [OPTIONS]\n"
        "  or:  ftmctrl [OPTIONS]\n";
    cout << endl;
}

void PrintHelp()
{
    Main::PrintHelp<StateMachineMiniFTM<StateMachine, ConnectionMiniFTM>>();

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

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

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

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

    // No console access at all
    if (!conf.Has("console"))
    {
        if (conf.Get<bool>("no-dim"))
            return RunShell<LocalStream, StateMachine, ConnectionMiniFTM>(conf);
        else
            return RunShell<LocalStream, StateMachineDim, ConnectionDimMiniFTM>(conf);
    }
    // Cosole access w/ and w/o Dim
    if (conf.Get<bool>("no-dim"))
    {
        if (conf.Get<int>("console")==0)
            return RunShell<LocalShell, StateMachine, ConnectionMiniFTM>(conf);
        else
            return RunShell<LocalConsole, StateMachine, ConnectionMiniFTM>(conf);
    }
    else
    {
        if (conf.Get<int>("console")==0)
            return RunShell<LocalShell, StateMachineDim, ConnectionDimMiniFTM>(conf);
        else
            return RunShell<LocalConsole, StateMachineDim, ConnectionDimMiniFTM>(conf);
    }

    return 0;
}