Index: /trunk/FACT++/src/biasctrl.cc
===================================================================
--- /trunk/FACT++/src/biasctrl.cc	(revision 11313)
+++ /trunk/FACT++/src/biasctrl.cc	(revision 11313)
@@ -0,0 +1,783 @@
+#include <boost/bind.hpp>
+#include <boost/array.hpp>
+#if BOOST_VERSION < 104400
+#if (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ > 4))
+#undef BOOST_HAS_RVALUE_REFS
+#endif
+#endif
+#include <boost/thread.hpp>
+#include <boost/asio/error.hpp>
+#include <boost/asio/deadline_timer.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"
+
+namespace ba    = boost::asio;
+namespace bs    = boost::system;
+namespace dummy = ba::placeholders;
+
+using namespace std;
+
+// ------------------------------------------------------------------------
+
+class ConnectionBias : public ConnectionUSB
+{
+    vector<char> fBuffer;
+
+    bool fIsVerbose;
+
+    enum
+    {
+        kNumBoards           = 13,
+        kNumChannelsPerBoard = 32,
+        kNumChannels = kNumBoards*kNumChannelsPerBoard
+    };
+
+    enum Command_t
+    {
+        kCmdReset = 0,
+        kCmdRead  = 1,
+        kCmdWrite = 3
+    };
+
+
+     // Resistance in Ohm for voltage correction
+#define RESISTOR float(1000)
+
+
+    vector<uint16_t> fVolt;        // Voltage in DAC units
+    vector<uint16_t> fRefVolt;
+
+    vector<uint16_t> fCurrent;     // Current in ADC units
+    vector<uint16_t> fRefCurrent;
+
+    vector<bool>     fOC;
+    vector<bool>     fPresent;
+
+    bool fResetHit;
+
+private:
+    void HandleReceivedData(const bs::error_code& err, size_t bytes_received, int /*type*/)
+    {
+        // Do not schedule a new read if the connection failed.
+        if (bytes_received==0 || err)
+        {
+            if (err==ba::error::eof)
+                Warn("Connection closed by remote host (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(err!=ba::error::basic_errors::operation_aborted);
+            return;
+        }
+
+        if (bytes_received%3)
+        {
+            Error("Number of received bytes not a multiple of 3, can't read data.");
+            PostClose(true);
+            return;
+        }
+
+        if (fIsVerbose)
+        {
+            Out() << endl << kBold << "Data received:" << endl;
+            Out() << Converter::GetHex<uint8_t>(fBuffer, 32) << endl;
+        }
+
+        int wrapcnt = -1;
+
+        // === Check/update all wrap counter ===
+        for (unsigned int i=0; i<fBuffer.size(); i += 3)
+        {
+            const int old = wrapcnt;
+            wrapcnt = (fBuffer[i]>>4)&7;
+
+            if (wrapcnt==-1 || (old+1)%8 == wrapcnt)
+                continue;
+
+            Error("WrapCnt wrong");
+            // Error receiving proper answer!
+            return;
+        }
+
+        // Success with received answer
+
+        if (fBuffer.size()!=kNumChannels*3)
+            return;
+
+        /*
+         data[0] = (cmd<<5) | (board<<1) | (((channel&16)>>4) & 1);
+         data[1] = (channel<<4) | (cmdval>>8);
+         data[2] = val&0xff;
+         */
+
+        // ################## Read all channels status ##################
+
+        // Evaluate data returned from crate
+        for (int i=0; i<kNumChannels; i++)
+        {
+            fOC[i]      =  fBuffer[i*3]&0x80;
+            fCurrent[i] =  fBuffer[i*3+1] | ((fBuffer[i*3]&0xf)<<8);
+            fPresent[i] =  fBuffer[i*3+2]&0x70 ? false : true;
+
+            // FIXME FIXME FIXME
+            fResetHit   = fBuffer[i*3+2] & 0x80;
+
+            if (i==2*kNumChannelsPerBoard+19)
+                fOC[i] = false;
+        }
+    }
+
+    void HandleTransmittedData(size_t n)
+    {
+        fBuffer.resize(n);
+        AsyncRead(ba::buffer(fBuffer));
+        AsyncWait(fInTimeout, 50, &ConnectionUSB::HandleReadTimeout);
+    }
+
+    // This is called when a connection was established
+    void ConnectionEstablished()
+    {
+    }
+
+    void HandleReadTimeout(const bs::error_code &error)
+    {
+        if (error==ba::error::basic_errors::operation_aborted)
+            return;
+
+        if (error)
+        {
+            ostringstream str;
+            str << "Read timeout of " << URL() << ": " << error.message() << " (" << error << ")";// << endl;
+            Error(str);
+
+            PostClose();
+            return;
+
+        }
+
+        if (!is_open())
+        {
+            // For example: Here we could schedule a new accept if we
+            // would not want to allow two connections at the same time.
+            return;
+        }
+
+        // Check whether the deadline has passed. We compare the deadline
+        // against the current time since a new asynchronous operation
+        // may have moved the deadline before this actor had a chance
+        // to run.
+        if (fInTimeout.expires_at() > ba::deadline_timer::traits_type::now())
+            return;
+
+        Error("Timeout reading data from "+URL());
+
+        PostClose();
+    }
+
+
+    void SystemReset()
+    {
+        PostMessage(GetCmd(0, kCmdReset));
+    }
+
+    vector<char> GetCmd(uint16_t id, Command_t cmd, uint16_t dac=0)
+    {
+        const unsigned int board   = id/kNumChannelsPerBoard;
+        const unsigned int channel = id%kNumChannelsPerBoard;
+
+        return GetCmd(board, channel, cmd, dac);
+    }
+
+    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;
+    }
+
+    void ReadAllChannelsStatus()
+    {
+        vector<char> data;
+        data.reserve(kNumChannels*3);
+
+        // Prepare command to read all channels
+        for (int i=0; i<kNumChannels; i++)
+            {
+                const vector<char> cmd = GetCmd(i, kCmdRead);
+                data.insert(data.end(), cmd.begin(), cmd.end());
+            }
+
+        PostMessage(data);
+    }
+
+    void GlobalSetDac(uint16_t dac)
+    {
+        PostMessage(GetCmd(0, kCmdWrite, dac));
+        /*
+        // On success
+        if (fBuffer.size() == 3)
+        {
+            for (int i=0; i<MAX_NUM_BOARDS; i++)
+                for (int j=0; j<NUM_CHANNELS; j++)
+                {
+                    DAC[i][j]     = SetPoint;
+                    Volt[i][j]    = Voltage;
+                    RefVolt[i][j] = Voltage;
+                }
+        }
+        */
+    }
+
+
+    void SetChannels(const map<uint16_t, uint16_t> &vals)
+    {
+        if (vals.empty())
+            return;
+
+        vector<char> data;
+
+        // Build and execute commands
+        for (map<uint16_t, uint16_t>::const_iterator it=vals.begin();
+             it!=vals.end(); it++)
+        {
+            //const uint16_t dac = it->second/90.0*0xfff;
+
+            // If DAC value unchanged, do not send command
+            if (fVolt[it->first] == it->second)
+                continue;
+
+            const vector<char> cmd = GetCmd(it->first, kCmdWrite, it->second);
+            data.insert(data.end(), cmd.begin(), cmd.end());
+        }
+
+        PostMessage(data);
+
+        /*
+        // On success
+        if (Data.size() == Buf.size())
+        {
+            for (map<unsigned int, double>::const_iterator it = V.begin(); it != V.end(); ++it) {
+                DAC[it->first/NUM_CHANNELS][it->first%NUM_CHANNELS] = (unsigned int) (it->second/90.0*0x0fff);
+                Volt[it->first/NUM_CHANNELS][it->first%NUM_CHANNELS] = it->second;
+                RefVolt[it->first/NUM_CHANNELS][it->first%NUM_CHANNELS] = it->second;
+        }
+        */
+    }
+
+    /*
+    // ***** Synchronize board *****
+    bool Crate::Synch()
+    {
+        //############################################################
+        int Trial = 0;
+        vector<unsigned char> Data;
+
+        while(++Trial <= 3) {
+            Data = Communicate(string(1, 0));
+            if (Data.size() == 3) return true;
+        }
+        return false;
+        //############################################################
+    }
+    */
+
+    void SetReferenceCurrent()
+    {
+        fRefCurrent = fCurrent;
+    }
+
+    void GlobalSet(double voltage)
+    {
+        if (voltage>90)
+            return;
+
+        GlobalSetDac(voltage/90.0*0xfff);
+    }
+
+    // ***** Correct voltages according to current *****
+    void AdaptVoltages()
+    {
+        map<uint16_t, uint16_t> values;
+
+        for (int i=0; i<kNumChannels; i++)
+        {
+            if (fRefVolt[i]==0)
+                continue;
+
+            // Calculate difference and convert ADC units to Amps
+            const double diffcur = (fRefCurrent[i]-fCurrent[i])*1.22;
+
+            // Calculate voltage difference
+            const double diffvolt = diffcur*RESISTOR/1e6;
+
+            // Calculate new vlaue by onverting voltage difference to DAC units
+            const int32_t dac = fRefVolt[i] + diffvolt/90.0*0xfff;
+
+            if (dac<0 || dac>0xfff)
+                continue;
+
+            values[i] = fRefVolt[i] + dac;
+        }
+
+        SetChannels(values);
+
+        /*
+        static int LastUpdate = 0;
+        if (time(NULL)-LastUpdate > 5)
+        {
+            LastUpdate = time(NULL);
+            UpdateDIM();
+        }*/
+    }
+
+public:
+    ConnectionBias(ba::io_service& ioservice, MessageImp &imp) : ConnectionUSB(ioservice, imp()),
+        fIsVerbose(true),
+        fVolt(kNumChannels),
+        fRefVolt(kNumChannels),
+        fCurrent(kNumChannels),
+        fRefCurrent(kNumChannels),
+        fOC(kNumChannels),
+        fPresent(kNumChannels)
+    {
+        SetLogStream(&imp);
+    }
+
+    void SetVerbose(bool b)
+    {
+        fIsVerbose = b;
+    }
+};
+
+// ------------------------------------------------------------------------
+
+#include "DimDescriptionService.h"
+
+class ConnectionDimBias : public ConnectionBias
+{
+private:
+
+    DimDescribedService fDimCurrent;
+
+    void Update(DimDescribedService &svc, vector<float> data, float time) const
+    {
+        data.insert(data.begin(), time);
+        svc.setData(data.data(), data.size()*sizeof(float));
+        svc.updateService();
+    }
+
+    void UpdateCur(float time, const vector<float> &curr)
+    {
+        Update(fDimCurrent, curr, time);
+    }
+
+public:
+    ConnectionDimBias(ba::io_service& ioservice, MessageImp &imp) :
+        ConnectionBias(ioservice, imp),
+        fDimCurrent("BIAS_CONTROL/CURRENT", "F:1;F:4", "")
+    {
+    }
+
+    // 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();
+    }
+
+    boost::function<int(const EventImp &)> Wrapper(boost::function<void()> func)
+    {
+        return boost::bind(&StateMachineBias::Wrap, this, func);
+    }
+
+private:
+    S fBias;
+
+    enum states_t
+    {
+        kStateDisconnected = 1,
+        kStateConnected    = 2,
+    };
+
+    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 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 fBias.IsConnected() ? kStateConnected : kStateDisconnected;
+    }
+
+    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;
+
+        fBias.SetVerbose(evt.GetBool());
+
+        return T::GetCurrentState();
+    }
+
+public:
+    StateMachineBias(ostream &out=cout) :
+        T(out, "BIAS_CONTROL"), ba::io_service::work(static_cast<ba::io_service&>(*this)),
+        fBias(*this, *this)
+    {
+        // 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
+        AddStateName(kStateDisconnected, "Disconnected",
+                     "Bias-power supply not connected via USB.");
+
+        AddStateName(kStateConnected, "Connected",
+                     "USB connection to bias-power supply established.");
+
+        // Verbosity commands
+        T::AddEvent("SET_VERBOSE", "B")
+            (boost::bind(&StateMachineBias::SetVerbosity, this, _1))
+            ("set verbosity state"
+             "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");
+
+        // Conenction commands
+        AddEvent("DISCONNECT", kStateConnected)
+            (boost::bind(&StateMachineBias::Disconnect, this))
+            ("disconnect from ethernet");
+
+        AddEvent("RECONNECT", "O", kStateDisconnected, kStateConnected)
+            (boost::bind(&StateMachineBias::Reconnect, this, _1))
+            ("(Re)connect ethernet connection to FTM, a new address can be given"
+             "|[host][string]:new ethernet address in the form <host:port>");
+    }
+
+    void SetEndpoint(const string &url)
+    {
+        fBias.SetEndpoint(url);
+    }
+
+    int EvalConfiguration(const Configuration &conf)
+    {
+        SetEndpoint(conf.Get<string>("addr"));
+
+        fBias.SetVerbose(!conf.Get<bool>("quiet"));
+
+        fBias.Connect();
+
+        return -1;
+    }
+};
+
+// ------------------------------------------------------------------------
+
+void RunThread(StateMachineImp *io_service)
+{
+    // This is necessary so that the StateMachien Thread can signal the
+    // Readline to exit
+    io_service->Run();
+    Readline::Stop();
+}
+
+/*
+template<class S, class T>
+int RunDim(Configuration &conf)
+{
+    WindowLog wout;
+
+    ReadlineColor::PrintBootMsg(wout, conf.GetName(), false);
+
+
+    if (conf.Has("log"))
+        if (!wout.OpenLogFile(conf.Get<string>("log")))
+            wout << kRed << "ERROR - Couldn't open log-file " << conf.Get<string>("log") << ": " << strerror(errno) << endl;
+
+    // Start io_service.Run to use the StateMachineImp::Run() loop
+    // Start io_service.run to only use the commandHandler command detaching
+    StateMachineBias<S, T> io_service(wout);
+    if (!io_service.EvalConfiguration(conf))
+        return -1;
+
+    io_service.Run();
+
+    return 0;
+}
+*/
+
+template<class T, class S, class R>
+int RunShell(Configuration &conf)
+{
+    static T shell(conf.GetName().c_str(), conf.Get<int>("console")!=1);
+
+    WindowLog &win  = shell.GetStreamIn();
+    WindowLog &wout = shell.GetStreamOut();
+
+    if (conf.Has("log"))
+        if (!wout.OpenLogFile(conf.Get<string>("log")))
+            win << kRed << "ERROR - Couldn't open log-file " << conf.Get<string>("log") << ": " << strerror(errno) << endl;
+
+    StateMachineBias<S, R> io_service(wout);
+    if (!io_service.EvalConfiguration(conf))
+        return -1;
+
+    shell.SetReceiver(io_service);
+
+    boost::thread t(boost::bind(RunThread, &io_service));
+    // boost::thread t(boost::bind(&StateMachineBias<S>::Run, &io_service));
+
+    if (conf.Has("cmd"))
+    {
+        const vector<string> v = conf.Get<vector<string>>("cmd");
+        for (vector<string>::const_iterator it=v.begin(); it!=v.end(); it++)
+            shell.ProcessLine(*it);
+    }
+
+    if (conf.Has("exec"))
+    {
+        const vector<string> v = conf.Get<vector<string>>("exec");
+        for (vector<string>::const_iterator it=v.begin(); it!=v.end(); it++)
+            shell.Execute(*it);
+    }
+
+    if (conf.Get<bool>("quit"))
+        shell.Stop();
+
+    shell.Run();                 // Run the shell
+    io_service.Stop();           // Signal Loop-thread to stop
+    // io_service.Close();       // Obsolete, done by the destructor
+
+    // Wait until the StateMachine has finished its thread
+    // before returning and destroying the dim objects which might
+    // still be in use.
+    t.join();
+
+    return 0;
+}
+
+void SetupConfiguration(Configuration &conf)
+{
+    const string n = conf.GetName()+".log";
+
+    po::options_description config("Program options");
+    config.add_options()
+        ("dns",       var<string>("localhost"), "Dim nameserver host name (Overwites DIM_DNS_NODE environment variable)")
+        ("log,l",     var<string>(n), "Write log-file")
+        ("no-dim,d",  po_bool(),      "Disable dim services")
+        ("console,c", var<int>(),     "Use console (0=shell, 1=simple buffered, X=simple unbuffered)")
+        ("cmd",       vars<string>(), "Execute one or more commands at startup")
+        ("exec,e",    vars<string>(), "Execute one or more scrips at startup")
+        ("quit",      po_switch(),    "Quit after startup");
+        ;
+
+    po::options_description control("FTM control options");
+    control.add_options()
+        ("addr,a",        var<string>("ttysS0"),  "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.")
+        ;
+
+    conf.AddEnv("dns", "DIM_DNS_NODE");
+
+    conf.AddOptions(config);
+    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);
+    SetupConfiguration(conf);
+
+    po::variables_map vm;
+    try
+    {
+        vm = conf.Parse(argc, argv);
+    }
+#if BOOST_VERSION > 104000
+    catch (po::multiple_occurrences &e)
+    {
+        cerr << "Program options invalid due to: " << e.what() << " of '" << e.get_option_name() << "'." << endl;
+        return -1;
+    }
+#endif
+    catch (exception& e)
+    {
+        cerr << "Program options invalid due to: " << e.what() << endl;
+        return -1;
+    }
+
+    if (conf.HasVersion() || conf.HasPrint())
+        return -1;
+
+    if (conf.HasHelp())
+    {
+        PrintHelp();
+        return -1;
+    }
+
+    Dim::Setup(conf.Get<string>("dns"));
+
+    //try
+    {
+        // No console access at all
+        if (!conf.Has("console"))
+        {
+            if (conf.Get<bool>("no-dim"))
+                return 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;
+}