Index: branches/FACT++_lidctrl_usb/src/lidctrl_usb.cc
===================================================================
--- branches/FACT++_lidctrl_usb/src/lidctrl_usb.cc	(revision 18788)
+++ branches/FACT++_lidctrl_usb/src/lidctrl_usb.cc	(revision 18788)
@@ -0,0 +1,326 @@
+#include <functional>
+#include <boost/bind.hpp>
+#include <cstdlib>
+
+#include "Dim.h"
+#include "Event.h"
+#include "Shell.h"
+#include "StateMachineDim.h"
+#include "StateMachineAsio.h"
+#include "ConnectionUSB.h"
+#include "Configuration.h"
+#include "Console.h"
+
+#include "tools.h"
+
+#include "LocalControl.h"
+#include "HeadersLid.h"
+
+namespace ba    = boost::asio;
+namespace bs    = boost::system;
+namespace dummy = ba::placeholders;
+
+using namespace std::placeholders;
+using namespace std;
+
+using namespace Lid;
+
+#include "DimDescriptionService.h"
+
+class ConnectionLid : public ConnectionUSB
+{
+public:
+    vector<char> fRecievedBytes;
+    vector<char> fCollectedBytes;
+    State::states_t fStateOfArduino;
+    State::states_t fLastStateOfArduino;
+
+    DimDescribedService fDim;
+    message_t fLastMessage;
+    boost::asio::deadline_timer fReadTimer;
+
+    void PrintMessage(const message_t msg)
+    {
+        Out() << "LastMessage:" << endl;
+        Out() << "    OuterMotor:" << endl;
+        Out() << "        Speed:    " << msg.outer_motor_speed << endl;
+        Out() << "        Current:  " << msg.outer_motor_current.mean << endl;
+        Out() << "        Position: " << msg.outer_motor_position.mean << endl;
+        Out() << "    InnerMotor:" << endl;
+        Out() << "        Speed:    " << msg.inner_motor_speed << endl;
+        Out() << "        Current:  " << msg.inner_motor_current.mean << endl;
+        Out() << "        Position: " << msg.inner_motor_position.mean << endl;
+        Out() << "    LastCommand: " << msg.current_cmd << endl;
+        Out() << "    State: " << (int)msg.system_state << endl;
+    }
+
+    void CollectBytes(size_t bytes_received)
+    {
+        // append fRecievedBytes to fCollectedBytes
+        std::copy(
+            fRecievedBytes.begin(),
+            fRecievedBytes.begin() + bytes_received,
+            std::back_inserter(fCollectedBytes)
+        );
+    }
+
+    void TruncateUntilStartFlagFound()
+    {
+        // search for the 1st occurence of "start:" in the whole fCollectedBytes
+        const string search_string("start:");
+        string s(fCollectedBytes.begin(), fCollectedBytes.end());
+        int pos=s.find(search_string);
+	if (pos != 0){
+            Out() << "TruncateUntilStartFlagFound: pos=" << pos << endl; 
+	}
+        // cut off the first 'pos' bytes from fCollectedBytes
+        vector<decltype(fCollectedBytes)::value_type>(
+            fCollectedBytes.begin() + pos,
+            fCollectedBytes.end()
+        ).swap(fCollectedBytes);
+    }
+
+    complete_message_t
+    PopCompleteMessage(){
+
+        complete_message_t tmp;
+        memcpy(&tmp, fCollectedBytes.data(), sizeof(complete_message_t));
+        // cut off the bytes we just copied into tmp
+        std::vector<decltype(fCollectedBytes)::value_type>(
+            fCollectedBytes.begin() + sizeof(complete_message_t),
+            fCollectedBytes.end()
+        ).swap(fCollectedBytes);
+        return tmp;
+    }
+
+    void UpdateDimService(const message_t& msg){
+        dim_service_t update;
+
+        update.inner_current_mean = (float)(msg.inner_motor_current.mean) * 3.4;
+        update.outer_current_mean = (float)(msg.outer_motor_current.mean) * 3.4;
+        update.inner_position_mean = (float)(msg.inner_motor_position.mean);
+        update.outer_position_mean = (float)(msg.outer_motor_position.mean);
+        update.inner_speed = (int32_t)msg.inner_motor_speed;
+        update.outer_speed = (int32_t)msg.outer_motor_speed;
+
+        // update only if some interesting happened, i.e.
+        //  - if the fStateOfArduino is not one of the steady states: kOpen or kclosed
+        //  - if the fStateOfArduino just changed.
+        if ( (fLastStateOfArduino != fStateOfArduino) || !((fStateOfArduino == State::kOpen) || (fStateOfArduino == State::kClosed)) )
+        {
+            fDim.Update(update);
+        }
+        fLastStateOfArduino = fStateOfArduino;
+    }
+
+    void HandleReceivedData(const boost::system::error_code&, size_t bytes_received)
+    {
+        CollectBytes(bytes_received);
+        if (fCollectedBytes.size() > 2*sizeof(complete_message_t))
+        {
+            TruncateUntilStartFlagFound();
+            complete_message_t complete_message = PopCompleteMessage();
+            message_t msg = complete_message.msg;
+
+            if (Tools::Fletcher16(&msg, 1) == complete_message.checksum)
+            {
+                fLastMessage = msg;
+                fStateOfArduino = static_cast<State::states_t>(msg.system_state + 1);
+                UpdateDimService(msg);
+            }
+        }
+	ConnectionEstablished();
+    }
+
+    void DoAsyncRead(){
+        async_read(
+            *this,
+            boost::asio::buffer(fRecievedBytes, 2 * sizeof(complete_message_t)),
+            boost::bind(
+                &ConnectionLid::HandleReceivedData,
+                this,
+                dummy::error,
+                dummy::bytes_transferred
+            )
+        );
+    }
+
+    void ConnectionEstablished()
+    {
+        fReadTimer.expires_from_now(boost::posix_time::milliseconds(50));
+	fReadTimer.async_wait(boost::bind(&ConnectionLid::DoAsyncRead, this));
+    }
+
+    ConnectionLid(ba::io_service& ioservice, MessageImp &imp) :
+        ConnectionUSB(ioservice, imp()),
+        fRecievedBytes(2 * sizeof(complete_message_t)),
+        fCollectedBytes(0),
+        fStateOfArduino(State::kOpen),
+        fLastStateOfArduino(State::kOpen),
+        fDim("LID_CONTROL/MOTORS", "F:1;F:1;F:1;F:1;I:1;I:1",
+            "|inner_current_mean[mA]: "
+            "|outer_current_mean[mA]: "
+            "|inner_position_mean[LSB]: "
+            "|outer_position_mean[LSB]: "
+            "|inner_speed[LSB]: "
+            "|outer_speed[LSB]: "),
+        fLastMessage(),
+	fReadTimer(ioservice)
+    {
+        SetLogStream(&imp);
+    }
+
+    State::states_t GetStatus()
+    {
+        return fStateOfArduino;
+    }
+};
+
+// ------------------------------------------------------------------------
+
+template <class T, class S>
+class StateMachineLid : public StateMachineAsio<T>
+{
+    int Wrap(boost::function<void()> f)
+    {
+        f();
+        return T::GetCurrentState();
+    }
+
+    function<int(const EventImp &)> Wrapper(function<void()> func)
+    {
+        return bind(&StateMachineLid::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 fLid;
+
+    int Execute()
+    {
+        return fLid.GetStatus();
+    }
+
+    int Open()
+    {
+        std::array<char, 1> cmd = {{'o'}};
+        fLid.PostMessage(cmd);
+        return T::GetCurrentState();
+    }
+
+    int Close()
+    {
+        std::array<char, 1> cmd = {{'c'}};
+        fLid.PostMessage(cmd);
+        return T::GetCurrentState();
+    }
+
+    int Print()
+    {
+        fLid.PrintMessage(fLid.fLastMessage);
+        return T::GetCurrentState();
+    }
+
+public:
+    StateMachineLid(ostream &out=cout) :
+        StateMachineAsio<T>(out, "LID_CONTROL"), fLid(*this, *this)
+    {
+        T::AddStateName(State::kOpen, "Open", "Lid is open");
+        T::AddStateName(State::kClosed, "Closed", "Lid is closed");
+        T::AddStateName(State::kUpperOpening, "UpperOpening", "upper(right) lid is opening");
+        T::AddStateName(State::kHalfOpen, "HalfOpen", "upper(right) lid open, lower(left) lid closed");
+        T::AddStateName(State::kLowerOpening, "LowerOpening", "lower(left) lid is opening");
+        T::AddStateName(State::kLowerClosing, "LowerClosing", "lower(left) lid is closing");
+        T::AddStateName(State::kUpperClosing, "UpperClosing", "upper(right) lid is closing");
+        T::AddStateName(State::kUnknown, "Unknown", "Position unknown - arduino just restarted");
+
+        T::AddEvent("OPEN")(bind(&StateMachineLid::Open, this))("Open Lid");
+        T::AddEvent("CLOSE")(bind(&StateMachineLid::Close, this))("Close Lid");
+        T::AddEvent("PRINT")(bind(&StateMachineLid::Print, this))("Print the last message");
+    }
+
+    ~StateMachineLid()
+    {
+    }
+
+    int EvalOptions(Configuration &conf)
+    {
+        if (conf.Has("dev"))
+        {
+            fLid.SetEndpoint(conf.Get<string>("dev"));
+            T::Message("Setting device to "+fLid.URL());
+            fLid.Connect();
+        }
+        return -1;
+    }
+};
+
+// ------------------------------------------------------------------------
+
+#include "Main.h"
+
+template<class T, class S, class R>
+int RunShell(Configuration &conf)
+{
+    return Main::execute<T, StateMachineLid<S, R>>(conf);
+}
+
+void SetupConfiguration(Configuration &conf)
+{
+    po::options_description control("Lid control options");
+    control.add_options()
+        ("no-dim,d",        po_bool(),  "Disable dim services")
+        ("dev",             var<string>("lid-arduino"),       "Device address of USB port to lid-arduino")
+        ("quiet,q",         po_bool(true),       "Disable printing contents of all received messages (except dynamic data) in clear text.")
+        ;
+
+    conf.AddOptions(control);
+}
+
+void PrintUsage()
+{
+    cout <<
+        "This lidctrl_usb program controls the Lid.\n"
+        "Usage: lidctrl_usb [-c type] [OPTIONS]\n"
+        "  or:  biaslidctrl_usbctrl [OPTIONS]\n";
+    cout << endl;
+}
+
+void PrintHelp()
+{
+    Main::PrintHelp<StateMachineLid<StateMachine,ConnectionLid>>();
+}
+
+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"))
+        {
+            return RunShell<LocalStream, StateMachineDim, ConnectionLid>(conf);
+        }
+        if (conf.Get<int>("console")==0)
+            return RunShell<LocalShell, StateMachineDim, ConnectionLid>(conf);
+        else
+            return RunShell<LocalConsole, StateMachineDim, ConnectionLid>(conf);
+    }
+    return 0;
+}
