Fix memory leaks and other bad stuff

[r2c2.git] / source / libmarklin / control.cpp

diff --git a/source/libmarklin/control.cpp b/source/libmarklin/control.cpp
index 6f7ef74dbf30bc18cd83bd111e994ef7e606c619..ae26a179a9aa2f4b51585584b4d289ff6e59d4cc 100644 (file)
--- a/source/libmarklin/control.cpp
+++ b/source/libmarklin/control.cpp
@@ -1,15 +1,15 @@
 /* $Id$
 
 This file is part of the MSP Märklin suite
-Copyright © 2007-2008  Mikkosoft Productions, Mikko Rasa
+Copyright © 2007-2009  Mikkosoft Productions, Mikko Rasa
 Distributed under the GPL
 */
 
 #include <fcntl.h>
 #include <termios.h>
 #include <sys/poll.h>
-#include <iostream>
 #include <msp/core/except.h>
+#include <msp/io/print.h>
 #include <msp/time/units.h>
 #include <msp/time/utils.h>
 #include "command.h"
@@ -36,7 +36,8 @@ Control::~Control()
                delete i->second;
        for(map<unsigned, Locomotive *>::iterator i=locomotives.begin(); i!=locomotives.end(); ++i)
                delete i->second;
-       close(serial_fd);
+       if(serial_fd>=0)
+               close(serial_fd);
 }
 
 void Control::open(const string &dev)
@@ -71,7 +72,7 @@ void Control::open(const string &dev)
                pollfd pfd = { serial_fd, POLLIN, 0 };
                if(poll(&pfd, 1, 500)>0)
                {
-                       cout<<"IB detected at "<<baud[i]<<" bits/s\n";
+                       IO::print("IB detected at %d bits/s\n", baud[i]);
                        char buf[2];
                        p50 = (read(serial_fd, buf, 2)==2);
                        ok = true;
@@ -122,6 +123,12 @@ Command &Control::command(Cmd cmd, const unsigned char *data, unsigned len)
        return queue.back();
 }
 
+void Control::flush()
+{
+       for(list<Command>::iterator i=queue.begin(); i!=queue.end(); ++i)
+               i->send(serial_fd);
+}
+
 void Control::add_turnout(Turnout &t)
 {
        turnouts[t.get_address()] = &t;
@@ -198,7 +205,7 @@ void Control::tick()
                {
                        Reply reply = Reply::read(serial_fd, queue.front().get_command());
                        if(debug)
-                               cout<<"R: "<<reply<<'\n';
+                               IO::print("R: %s\n", reply);
 
                        queue.front().signal_done.emit(reply);
                        queue.erase(queue.begin());
@@ -210,9 +217,16 @@ void Control::tick()
        if(!queue.empty())
        {
                if(debug)
-                       cout<<"W: "<<queue.front()<<'\n';
+                       IO::print("W: %s\n", queue.front());
 
-               queue.front().send(serial_fd);
+               if(serial_fd>=0)
+                       queue.front().send(serial_fd);
+               else
+               {
+                       Reply reply = Reply::simulate(queue.front().get_command());
+                       queue.front().signal_done.emit(reply);
+                       queue.erase(queue.begin());
+               }
        }
 }
 
@@ -236,7 +250,7 @@ void Control::event_query_done(const Reply &reply)
                command(CMD_EVENT_TURNOUT).signal_done.connect(sigc::mem_fun(this, &Control::turnout_event_done));
        if(data[0]&0x04)
                command(CMD_EVENT_SENSOR).signal_done.connect(sigc::mem_fun(this, &Control::sensor_event_done));
-       if(data[1]&0x40)
+       if((data[0]&0x80) && (data[1]&0x40))
                command(CMD_STATUS).signal_done.connect(sigc::mem_fun(this, &Control::status_done));
 }
 
@@ -248,7 +262,7 @@ void Control::turnout_event_done(const Reply &reply)
        {
                unsigned addr = (data[i*2+1])+((data[i*2+2]&7)<<8);
                bool status = !(data[i*2+2]&0x80);
-               cout<<"Turnout "<<addr<<", status "<<status<<'\n';
+               IO::print("Turnout %d, status %d\n", addr, status);
                signal_turnout_event.emit(addr, status);
        }
 }
@@ -260,10 +274,7 @@ void Control::sensor_event_done(const Reply &reply)
        {
                unsigned module = data[i];
 
-               cout<<"S88 module "<<module<<", status ";
-               for(unsigned j=0; j<16; ++j)
-                       cout<<((data[i+1+j/8]>>(7-j%8))&1);
-               cout<<'\n';
+               IO::print("S88 module %d, status %08b%08b\n", module, data[1], data[2]);
 
                for(unsigned j=0; j<16; ++j)
                        signal_sensor_event.emit(module*16+j-15, (data[i+1+j/8]>>(7-j%8))&1);