#include <functional>

#include <boost/bind.hpp>

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

#include "tools.h"

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

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

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

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

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

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

    bool fIsVerbose;

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

    vector<bool>     fPresent;

    int64_t fWrapCounter;
    int64_t fSendCounter;

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

    int16_t fRampStep;
    int16_t fRampTime;

    uint16_t fUpdateTime;
    uint16_t fSyncTime;

    bool fIsInitializing;
    bool fIsRamping;
    bool fWaitingForAnswer;

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

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

    uint16_t fVoltMax;

    virtual void UpdateA()
    {
    }

    virtual void UpdateV()
    {
    }

private:
    bool CheckChDac(const string &cmd, uint16_t dac, uint16_t ch=0)
    {
        if (dac>kMaxDac)
        {
            Error(cmd+" - DAC value out of range.");
            return false;
        }
        if (dac>fVoltMax)
        {
            Error(cmd+" - DAC value exceeds fVoltMax.");
            return false;
        }

        if (ch>=kNumChannels)
        {
            Error(cmd+" - Channel out of range.");
            return false;
        }

        return true;
    }

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

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

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

        return data;
    }

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

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

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

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

        PostClose(false);
        return false;
    }

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

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

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

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

        fWrapCounter = wrap;

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

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

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

            fSendCounter = wrap;

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

            return true;
        }

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

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

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

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

                fGlobalVoltCmd = -1;

                return true;
            }

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

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

        const int cmd = command&3;

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

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

            fCurrent[id]    = status ? -ddd : ddd;
            fPresent[board] = true;
        }

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

        return true;

    }

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

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

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

        const int cmd = command&0xf;

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

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

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

            // Cancel sending of the next 0
            fSyncTimer.cancel();

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

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


        // Take action depending on what is going on
        if (command==kCmdReset)
            Message("Reset command successfully answered.");

        if (cmd==kCmdRead || cmd==kCmdChannelSet || cmd==kExpertChannelSet)
        {
            UpdateV();
            UpdateA();
        }

        if (cmd==kCmdReset || command==kResetChannels)
        {
            // Re-start cyclic reading of values after a short time
            // to allow the currents to become stable
            fUpdateTimer.cancel();
            ScheduleUpdate(100);
        }

        if (command==kUpdate)
            ScheduleUpdate(fUpdateTime);

        // If we are ramping, schedule a new ramp step
        if (command==kCmdChannelSet && fIsRamping)
        {
            ScheduleRampStep();
            return;
        }
    }

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

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

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

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

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

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

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

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

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

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

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

            PostClose(false);
            return;
        }

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

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

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

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

            ScheduleSync(counter+1);
            return;
        }

        Info("Synchronisation successfull.");
    }

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

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

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

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

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

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

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

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

            PostClose(false);
            return;
        }

        if (!is_open())
            return;

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

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

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

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

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

            fVoltCmd[ch] = dac[ch];
        }

        fSendCounter += kNumChannels;

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

        if (special)
            fWaitingForAnswer = true;
    }

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

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

        return fVolt[ch];
    }

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

        vector<uint16_t> dac(kNumChannels);

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

        SetAllChannels(dac);

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

        return !identical;
    }

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

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

            PostClose(false);
            return;
        }

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

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

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

        fIsRamping = RampOneStep();
    }

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

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

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

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

        vector<uint16_t> dac(kNumChannels);

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

        SetAllChannels(dac, true);
    }

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

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

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

        fSendCounter += kNumChannels;

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

        if (!special)
            fWaitingForAnswer = true;
    }

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

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

        fVoltRef[ch] = dac;

        if (!fIsRamping)
            fIsRamping = RampOneStep();

        return true;
    }

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

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

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

        if (!fIsRamping)
            fIsRamping = RampOneStep();

        return true;
    }

    bool GlobalSetVolt(float volt)
    {
        return GlobalSetDac(volt*4096/90);
    }

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

    bool SetGapdVoltage()
    {
        for (size_t ch=0; ch<kNumChannels; ch++)
            if (fVoltGapd[ch]>kMaxDac)
            {
                Error("SetGapdVoltage - Voltage reference for G-APD channel out of range.");
                return false;
            }

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

        if (!fIsRamping)
            fIsRamping = RampOneStep();

        return true;
    }

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

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

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

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

        return true;
    }

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

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

        Message("Ramping stopped.");
    }

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

        fIsRamping = RampOneStep();
    }

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

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

    bool IsRamping() const { return fIsRamping; }

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

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

        Warn("EXPERT MODE: Sending reset.");
        PostMessage(GetCmd(kCmdReset));
        AsyncRead(ba::buffer(fBuffer, 3), kCmdReset, ++fSendCounter);
        fWaitingForAnswer = true;
    }


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

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

        fVoltCmd[ch] = dac;

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

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

        return true;
    }

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

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

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

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

        fGlobalVoltCmd = dac;

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

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

        return true;
    }

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

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

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

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

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

    }

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

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

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

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

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

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

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

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

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

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

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

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

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

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


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

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

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

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

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

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

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

        if (fIsInitializing)
            return BIAS::kInitializing;

        if (fIsRamping)
            return BIAS::kRamping;

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

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

        return BIAS::kAtReference;
    }
};

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

#include "DimDescriptionService.h"

class ConnectionDimBias : public ConnectionBias
{
private:

    DimDescribedService fDimCurrent;
    DimDescribedService fDimVoltage;

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

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


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

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

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

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

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

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

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

private:
    S fBias;

    bool fExpertMode;

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

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

        if (!fBias.GlobalSetVolt(evt.GetFloat()))
            T::Error("Supplied voltage out of range (0-90)");

        return T::GetCurrentState();
    }

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

        if (!fBias.GlobalSetDac(evt.GetUShort()))
            T::Error("Supplied voltage out of range (0-90)");

        return T::GetCurrentState();
    }

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

        if (!fBias.ChannelSetVolt(evt.GetUShort(), evt.Get<float>(2)))
            T::Error("Value out of range");

        return T::GetCurrentState();
    }

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

        if (!fBias.ChannelSetDac(evt.Get<uint16_t>(), evt.Get<uint16_t>(2)))
            T::Error("Value out of range");

        return T::GetCurrentState();
    }

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

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

        if (!fBias.ExpertGlobalSetVolt(evt.GetFloat()))
            T::Error("Supplied voltage out of range (0-90)");

        return T::GetCurrentState();
    }

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

        if (!fBias.ExpertGlobalSetDac(evt.GetUShort()))
            T::Error("Supplied voltage out of range (0-90)");

        return T::GetCurrentState();
    }

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

        if (!fBias.ExpertChannelSetVolt(evt.GetUShort(), evt.Get<float>(2)))
            T::Error("Value out of range");

        return T::GetCurrentState();
    }

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

        if (!fBias.ExpertChannelSetDac(evt.Get<uint16_t>(), evt.Get<uint16_t>(2)))
            T::Error("Value out of range");

        return T::GetCurrentState();
    }

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

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

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

        return T::GetCurrentState();
    }

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

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

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

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

        return T::GetCurrentState();
    }

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

        fBias.SetVerbose(evt.GetBool());

        return T::GetCurrentState();
    }

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

        fExpertMode = evt.GetBool();

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

        return T::GetCurrentState();
    }

    int Execute()
    {
        // Dispatch (execute) at most one handler from the queue. In contrary
        // to run_one(), it doesn't wait until a handler is available
        // which can be dispatched, so poll_one() might return with 0
        // handlers dispatched. The handlers are always dispatched/executed
        // synchronously, i.e. within the call to poll_one()
        poll_one();

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

public:
    StateMachineBias(ostream &out=cout) :
        T(out, "BIAS_CONTROL"), ba::io_service::work(static_cast<ba::io_service&>(*this)),
        fBias(*this, *this), fExpertMode(false)
    {
        // ba::io_service::work is a kind of keep_alive for the loop.
        // It prevents the io_service to go to stopped state, which
        // would prevent any consecutive calls to run()
        // or poll() to do nothing. reset() could also revoke to the
        // previous state but this might introduce some overhead of
        // deletion and creation of threads and more.

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

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

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

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

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

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

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

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

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

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

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



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

        T::AddEvent("RESET_OVER_CURRENT_STATUS", kOverCurrent)
            (Wrapper(bind(&ConnectionBias::OverCurrentReset, &fBias)))
            ("NOT YET TESTED");



        T::AddEvent("SET_GLOBAL_VOLTAGE", "F:1", kConnected, kAtReference, kOverCurrent)
            (bind(&StateMachineBias::SetGlobalVolt, this, placeholders::_1))
            ("Set all channels to a new reference voltage. Starts ramping if necessary. (This command is not realized with the GLOBAL SET command.)");

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

        T::AddEvent("SET_CHANNEL_VOLTAGE", "S:1;F:1", kConnected, kAtReference, kOverCurrent)
            (bind(&StateMachineBias::SetChannelVolt, this, placeholders::_1))
            ("Set a single channel a new reference voltage. Starts ramping if necessary.");

        T::AddEvent("SET_CHANNEL_DAC", "S:1;S:1", kConnected, kAtReference, kOverCurrent)
            (bind(&StateMachineBias::SetChannelDac, this, placeholders::_1))
            ("Set a single channel a new DAC reference value. Starts ramping if necessary.");

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

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



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

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



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


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

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

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

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

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

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

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

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

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

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

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

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

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

        fBias.SetVoltMax(maxv);

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

        ifstream fin("FACTmapV5.txt");

        int l = 0;

        vector<float> vec(kNumChannels);

        string buf;
        while (getline(fin, buf, '\n'))
        {
            if (l>1439)
                break;

            buf = Tools::Trim(buf);
            if (buf[0]=='#')
                continue;

            stringstream str(buf);

            int   idummy, board, channel;
            float fdummy, volt;

            str >> idummy >> idummy >> idummy >> idummy >> idummy;
            str >> volt;
            str >> board;
            str >> channel;
            str >> fdummy >> fdummy >> fdummy;

            if (channel+32*board>=kNumChannels)
            {
                T::Error("Invalid board/channel read from FACTmapV5.txt.");
                return 4;
            }

            vec[channel+32*board] = volt;
            l++;
        }

        if (l!=1440)
        {
            T::Error("Reading reference voltages from FACTmapV5.txt failed.");
            return 5;
        }

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

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

        fBias.Connect();

        return -1;
    }
};

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

#include "Main.h"

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

void SetupConfiguration(Configuration &conf)
{
    po::options_description control("BIAS control options");
    control.add_options()
        ("no-dim,d",        po_bool(),  "Disable dim services")
        ("dev",             var<string>("FTE00FOH"),  "Device address of USB port to bias-power supply")
        ("quiet,q",         po_bool(),           "Disable printing contents of all received messages (except dynamic data) in clear text.")
        ("ramp-time",       var<uint16_t>(15),   "Delay between the answer of one ramping steps and sending the next ramp command to all channels in milliseconds.")
        ("ramp-step",       var<uint16_t>(46),   "Maximum step in DAC counts during ramping (Volt = DAC*90/4096)")
        ("update-interval", var<uint16_t>(3000), "Interval between two current requests in milliseconds")
        ("sync-delay",      var<uint16_t>(333),  "Delay between sending the inital 0's after a newly established connection to synchronize the output stream in milliseconds")
        ("volt-max",        var<float>(75),      "Upper limit for the voltage which can be applied in Volts")
        ;
    // FIXME: Make sure ramping / request and commands are
    //        not sent at the same time.

    conf.AddOptions(control);
}

/*
 Extract usage clause(s) [if any] for SYNOPSIS.
 Translators: "Usage" and "or" here are patterns (regular expressions) which
 are used to match the usage synopsis in program output.  An example from cp
 (GNU coreutils) which contains both strings:
  Usage: cp [OPTION]... [-T] SOURCE DEST
    or:  cp [OPTION]... SOURCE... DIRECTORY
    or:  cp [OPTION]... -t DIRECTORY SOURCE...
 */
void PrintUsage()
{
    cout <<
        "The biasctrl controls the bias-power supply 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: biasctrl [-c type] [OPTIONS]\n"
        "  or:  biasctrl [OPTIONS]\n";
    cout << endl;
}

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

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

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

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

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

    return 0;
}