--- /dev/null
+#include <msp/time/timer.h>
+#include <msp/time/units.h>
+#include "command.h"
+#include "constants.h"
+#include "control.h"
+#include "locomotive.h"
+
+using namespace std;
+using namespace Msp;
+
+namespace Marklin {
+
+Locomotive::Locomotive(Control &c, unsigned a):
+ control(c),
+ addr(a),
+ speed(0),
+ reverse(false),
+ funcs(0)
+{
+ control.add_locomotive(this);
+
+ refresh_status();
+}
+
+void Locomotive::set_speed(unsigned spd)
+{
+ speed=min(spd, 14U);
+
+ send_command(false);
+}
+
+void Locomotive::set_reverse(bool rev)
+{
+ if(rev==reverse)
+ return;
+
+ if(speed)
+ {
+ (new Time::Timer((500+speed*150)*Time::msec))->signal_timeout.connect(sigc::mem_fun(this, &Locomotive::reverse_timeout));
+ set_speed(0);
+ }
+ else
+ {
+ reverse=rev;
+ send_command(false);
+ }
+}
+
+void Locomotive::set_function(unsigned func, bool state)
+{
+ if(state)
+ funcs|=1<<func;
+ else
+ funcs&=~(1<<func);
+
+ send_command(true);
+}
+
+void Locomotive::refresh_status()
+{
+ char cmd[3];
+ cmd[0]=CMD_LOK_STATUS;
+ cmd[1]=addr&0xFF;
+ cmd[2]=(addr>>8)&0xFF;
+ control.command(string(cmd,3)).signal_done.connect(sigc::mem_fun(this,&Locomotive::status_reply));
+}
+
+void Locomotive::send_command(bool setf)
+{
+ char cmd[16];
+ cmd[0]=CMD_LOK;
+ cmd[1]=addr&0xFF;
+ cmd[2]=(addr>>8)&0xFF;
+
+ if(speed==0)
+ cmd[3]=0;
+ else if(speed==1)
+ cmd[3]=2;
+ else
+ cmd[3]=(speed*19-18)/2;
+
+ cmd[4]=(reverse?0:0x20) | ((funcs&1)?0x10:0);
+
+ if(setf)
+ {
+ cmd[4]|=0x80;
+ for(unsigned i=0; i<4; ++i)
+ if((funcs>>i)&2)
+ cmd[4]|=(1<<i);
+ }
+ control.command(string(cmd,5));
+}
+
+void Locomotive::status_reply(Error err, const string &reply)
+{
+ if(err==ERR_NO_ERROR)
+ {
+ if((unsigned char)reply[0]<=1)
+ speed=0;
+ else
+ speed=(unsigned char)reply[0]*2/19+1;
+ reverse=(reply[1]&0x20)?false:true;
+ funcs=(reply[1]&0xF)<<1;
+ if(reply[1]&0x10)
+ funcs|=1;
+ }
+}
+
+bool Locomotive::reverse_timeout()
+{
+ reverse=!reverse;
+ send_command(true);
+ return false;
+}
+
+} // namespace Marklin