]> git.tdb.fi Git - r2c2.git/blob - source/libr2c2/centralstation.cpp
Sync timetable to clock on first tick
[r2c2.git] / source / libr2c2 / centralstation.cpp
1 #include <algorithm>
2 #include <msp/core/refptr.h>
3 #include <msp/io/print.h>
4 #include <msp/net/resolve.h>
5 #include <msp/strings/utils.h>
6 #include <msp/time/units.h>
7 #include <msp/time/utils.h>
8 #include "centralstation.h"
9 #include "tracktype.h"
10 #include "vehicletype.h"
11
12 using namespace std;
13 using namespace Msp;
14
15 namespace R2C2 {
16
17 CentralStation::CentralStation(const Options &opts):
18         socket(Net::INET),
19         pending_commands(0),
20         power(false),
21         halted(false),
22         locos_synced(false),
23         accessories_synced(false),
24         sensors_synced(false)
25 {
26         RefPtr<Net::SockAddr> addr = Net::resolve(opts.get<string>(string())+":15471");
27         socket.connect(*addr);
28
29         IO::print("Connected to central station at %s\n", addr->str());
30
31         command("get(1, status)");
32         command("request(1, view)");
33         command("queryObjects(10, addr, name)");
34         command("queryObjects(11, addr)");
35         command("queryObjects(26)");
36 }
37
38 CentralStation::~CentralStation()
39 {
40         command("release(1, view)", true);
41         for(LocoMap::iterator i=locos.begin(); (i!=locos.end() && !(i->first&0x10000)); ++i)
42                 command(format("release(%d, view, control)", i->first));
43         for(AccessoryMap::iterator i=accessories.begin(); (i!=accessories.end() && !(i->first&0x10000)); ++i)
44                 command(format("release(%d, view, control)", i->first));
45         while(IO::poll(socket, IO::P_INPUT, 100*Time::msec))
46                 while(receive()) ;
47 }
48
49 void CentralStation::set_power(bool p)
50 {
51         power = p;
52         command(format("set(1, %s)", (power ? "go" : "stop")));
53 }
54
55 void CentralStation::halt(bool h)
56 {
57         halted = h;
58         if(halted)
59         {
60                 for(LocoMap::iterator i=locos.begin(); i!=locos.end(); ++i)
61                         if(i->second.speed)
62                                 set_loco_speed(i->first, 0);
63         }
64
65         signal_halt.emit(halted);
66 }
67
68 const char *CentralStation::enumerate_protocols(unsigned index) const
69 {
70         if(index==MM)
71                 return "MM";
72         else if(index==MM_27)
73                 return "MM-27";
74         else if(index==MFX)
75                 return "MFX";
76         else
77                 return 0;
78 }
79
80 unsigned CentralStation::get_protocol_speed_steps(const string &name) const
81 {
82         switch(map_protocol(name))
83         {
84         case MM: return 14;
85         case MM_27: return 27;
86         case MFX: return 126;
87         default: return 0;
88         }
89 }
90
91 unsigned CentralStation::add_loco(unsigned addr, const string &proto_name, const VehicleType &type)
92 {
93         Protocol proto = map_protocol(proto_name);
94
95         unsigned id = map_address(locos, loco_addr, addr);
96         if(!id)
97         {
98                 Locomotive &loco = locos[addr|0x10000];
99                 loco.name = type.get_name();
100                 loco.protocol = proto;
101                 loco.address = addr;
102
103                 const VehicleType::FunctionMap &type_funcs = type.get_functions();
104                 for(VehicleType::FunctionMap::const_iterator i=type_funcs.begin(); i!=type_funcs.end(); ++i)
105                         loco.func_mask |= 1<<i->first;
106
107                 if(locos_synced && proto!=MFX)
108                         command("create(10)");
109         }
110         else
111                 command(format("request(%d, view, control, force)", id));
112
113         return addr;
114 }
115
116 void CentralStation::remove_loco(unsigned addr)
117 {
118         unsigned id = map_address(locos, loco_addr, addr);
119         if(id)
120                 command(format("release(%d, view, control)", id));
121 }
122
123 void CentralStation::set_loco_speed(unsigned addr, unsigned speed)
124 {
125         if(speed && halted)
126                 return;
127
128         unsigned id = map_address(locos, loco_addr, addr);
129         if(id)
130         {
131                 Locomotive &loco = locos[id];
132                 if(loco.protocol==MFX && speed)
133                         ++speed;
134                 command(format("set(%d, speedstep[%d])", id, speed));
135         }
136 }
137
138 void CentralStation::set_loco_reverse(unsigned addr, bool rev)
139 {
140         unsigned id = map_address(locos, loco_addr, addr);
141         if(id)
142                 command(format("set(%d, dir[%d])", id, rev));
143 }
144
145 void CentralStation::set_loco_function(unsigned addr, unsigned func, bool state)
146 {
147         unsigned id = map_address(locos, loco_addr, addr);
148         if(id)
149                 command(format("set(%d, func[%d, %d])", id, func, state));
150 }
151
152 unsigned CentralStation::add_turnout(unsigned addr, const TrackType &type)
153 {
154         unsigned straight = type.get_paths();
155         bool left = false;
156         bool right = false;
157         bool cross = false;
158
159         const vector<TrackPart> &parts = type.get_parts();
160         for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
161         {
162                 OrientedPoint start = i->get_point(0);
163                 OrientedPoint end = i->get_point(i->get_length());
164                 if(abs(end.rotation-start.rotation).radians()<0.01)
165                 {
166                         (end.rotation>start.rotation ? left : right) = true;
167                         straight &= ~(1<<i->get_path());
168                 }
169                 else if(abs(start.rotation).radians()>0.01)
170                         cross = true;
171         }
172
173         MagnetAccessory::Symbol symbol = MagnetAccessory::TURNOUT_LEFT;
174         if(cross)
175                 symbol = MagnetAccessory::TURNOUT_DOUBLESLIP;
176         else if(left && right)
177                 symbol = MagnetAccessory::TURNOUT_THREEWAY;
178         else if(left)
179                 symbol = (straight ? MagnetAccessory::TURNOUT_LEFT : MagnetAccessory::TURNOUT_CURVED_LEFT);
180         else if(right)
181                 symbol = (straight ? MagnetAccessory::TURNOUT_RIGHT : MagnetAccessory::TURNOUT_CURVED_RIGHT);
182
183         MagnetAccessory &turnout = add_accessory(addr, MagnetAccessory::TURNOUT, symbol);
184         turnout.bits = type.get_state_bits();
185
186         return addr;
187 }
188
189 void CentralStation::remove_turnout(unsigned addr)
190 {
191         remove_accessory(addr);
192 }
193
194 void CentralStation::set_turnout(unsigned addr, unsigned state)
195 {
196         set_accessory_state(addr, MagnetAccessory::TURNOUT, state);
197 }
198
199 unsigned CentralStation::get_turnout(unsigned addr) const
200 {
201         return get_accessory_state(addr, MagnetAccessory::TURNOUT);
202 }
203
204 unsigned CentralStation::add_signal(unsigned addr, const SignalType &)
205 {
206         add_accessory(addr, MagnetAccessory::SIGNAL, MagnetAccessory::SEMAPHORE_HOME);
207         return addr;
208 }
209
210 void CentralStation::remove_signal(unsigned addr)
211 {
212         remove_accessory(addr);
213 }
214
215 void CentralStation::set_signal(unsigned addr, unsigned state)
216 {
217         set_accessory_state(addr, MagnetAccessory::SIGNAL, state);
218 }
219
220 unsigned CentralStation::get_signal(unsigned addr) const
221 {
222         return get_accessory_state(addr, MagnetAccessory::SIGNAL);
223 }
224
225 CentralStation::MagnetAccessory &CentralStation::add_accessory(unsigned addr, MagnetAccessory::Type type, MagnetAccessory::Symbol symbol)
226 {
227         unsigned id = map_address(accessories, accessory_addr, addr);
228         if(!id)
229         {
230                 id = addr|0x10000;
231
232                 MagnetAccessory &accessory = accessories[id];
233                 accessory.address = addr;
234                 accessory.type = type;
235                 accessory.symbol = symbol;
236
237                 accessory_addr[addr] = id;
238
239                 if(accessories_synced)
240                         command("create(11, append)");
241
242                 return accessory;
243         }
244         else
245         {
246                 MagnetAccessory &accessory = accessories[id];
247                 command(format("request(%d, view, control)", id));
248                 if(accessory.symbol!=symbol)
249                         command(format("set(%d, symbol[%d])", symbol));
250
251                 return accessory;
252         }
253 }
254
255 void CentralStation::remove_accessory(unsigned addr)
256 {
257         unsigned id = map_address(accessories, accessory_addr, addr);
258         if(id)
259                 command(format("release(%d, view, control)", id));
260 }
261
262 void CentralStation::set_accessory_state(unsigned addr, MagnetAccessory::Type type, unsigned state)
263 {
264         unsigned id = map_address(accessories, accessory_addr, addr);
265         if(id)
266         {
267                 MagnetAccessory &accessory = accessories[id];
268                 if(accessory.type!=type)
269                         throw logic_error("accessory type conflict");
270
271                 unsigned mask = (1<<accessory.bits)-1;
272
273                 if(((state^accessory.state)&mask)==0 || !accessory.synced)
274                 {
275                         accessory.state = state;
276                         accessory_state_changed(accessory);
277                         return;
278                 }
279
280                 accessory.state = (accessory.state&mask) | (state&~mask);
281
282                 command(format("set(%d, state[%d])", id, state&mask));
283         }
284 }
285
286 unsigned CentralStation::get_accessory_state(unsigned addr, MagnetAccessory::Type type) const
287 {
288         unsigned id = map_address(accessories, accessory_addr, addr);
289         if(id)
290         {
291                 AccessoryMap::const_iterator i = accessories.find(id);
292                 if(i!=accessories.end() && i->second.type==type)
293                         return i->second.state;
294         }
295         return 0;
296 }
297
298 void CentralStation::accessory_state_changed(const MagnetAccessory &accessory) const
299 {
300         if(accessory.type==MagnetAccessory::TURNOUT)
301                 signal_turnout.emit(accessory.address, accessory.state);
302         else if(accessory.type==MagnetAccessory::SIGNAL)
303                 signal_signal.emit(accessory.address, accessory.state);
304 }
305
306 unsigned CentralStation::add_sensor(unsigned addr)
307 {
308         sensors.insert(SensorMap::value_type(addr, Sensor()));
309
310         if(sensors_synced)
311         {
312                 if(addr>s88.size()*16)
313                         command("create(26, add[0])");
314         }
315
316         return addr;
317 }
318
319 void CentralStation::remove_sensor(unsigned)
320 {
321 }
322
323 bool CentralStation::get_sensor(unsigned addr) const
324 {
325         SensorMap::const_iterator i = sensors.find(addr);
326         if(i!=sensors.end())
327                 return i->second.state;
328         return false;
329 }
330
331 void CentralStation::tick()
332 {
333         while(Message msg = receive())
334         {
335                 if(msg.footer.code)
336                         IO::print("\033[31m*** ERROR: %s: %d %s ***\033[0m\n", msg.header.value, msg.footer.code, msg.footer.value);
337
338                 if(msg.header.type=="REPLY")
339                         process_reply(msg);
340                 else if(msg.header.type=="EVENT")
341                         process_event(msg);
342         }
343 }
344
345 void CentralStation::flush()
346 {
347 }
348
349 void CentralStation::command(const string &cmd, bool force)
350 {
351         if(pending_commands<10 || force)
352         {
353                 socket.write(cmd+"\r\n");
354                 ++pending_commands;
355         }
356         else
357                 cmd_queue.push_back(cmd);
358 }
359
360 CentralStation::Message CentralStation::receive()
361 {
362         while(IO::poll(socket, IO::P_INPUT, Time::zero))
363         {
364                 char rbuf[1024];
365                 unsigned len = socket.read(rbuf, sizeof(rbuf));
366                 if(!len)
367                         return Message();
368
369                 in_buffer.append(rbuf, len);
370         }
371
372         if(!in_buffer.empty())
373         {
374                 string::iterator iter = in_buffer.begin();
375                 if(Message msg = parse_message(iter, in_buffer.end()))
376                 {
377                         skip(iter, in_buffer.end(), "\r\n");
378                         in_buffer.erase(in_buffer.begin(), iter);
379
380                         if(msg.header.type=="REPLY" && pending_commands>0)
381                         {
382                                 --pending_commands;
383                                 if(!cmd_queue.empty())
384                                 {
385                                         command(cmd_queue.front());
386                                         cmd_queue.pop_front();
387                                 }
388                         }
389
390                         return msg;
391                 }
392         }
393
394         return Message();
395 }
396
397 void CentralStation::process_reply(const Message &msg)
398 {
399         if(!msg.header.value.compare(0, 4, "get("))
400         {
401                 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
402                 {
403                         if(accessories.count(i->first))
404                                 accessories[i->first].synced = true;
405
406                         process_object(i->first, i->second);
407                 }
408         }
409         else if(!msg.header.value.compare(0, 16, "queryObjects(10,"))
410         {
411                 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
412                 {
413                         LocoMap::iterator j = locos.find(i->first);
414                         if(j==locos.end())
415                         {
416                                 bool found = false;
417                                 Message::AttribMap::const_iterator k = i->second.find("addr");
418                                 if(k!=i->second.end())
419                                 {
420                                         unsigned addr = lexical_cast<unsigned>(k->second);
421
422                                         j = locos.find(addr|0x10000);
423                                         if(j!=locos.end())
424                                         {
425                                                 command(format("request(%d, view, control, force)", i->first));
426                                                 string cmd = format("get(%d, dir", i->first);
427                                                 for(unsigned l=0; j->second.func_mask>>l; ++l)
428                                                         if((j->second.func_mask>>l)&1)
429                                                                 cmd += format(", func[%d]", l);
430                                                 cmd += ')';
431                                                 command(cmd);
432
433                                                 locos.insert(LocoMap::value_type(i->first, j->second));
434                                                 locos.erase(j);
435
436                                                 found = true;
437                                         }
438                                 }
439
440                                 if(!found)
441                                         locos.insert(LocoMap::value_type(i->first, Locomotive()));
442                         }
443
444                         process_object(i->first, i->second);
445                 }
446
447                 locos_synced = true;
448
449                 if(locos.lower_bound(0x10000)!=locos.end())
450                         command("create(10)");
451         }
452         else if(!msg.header.value.compare(0, 16, "queryObjects(11,"))
453         {
454                 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
455                 {
456                         AccessoryMap::iterator j = accessories.find(i->first);
457                         if(j==accessories.end())
458                         {
459                                 bool found = false;
460                                 Message::AttribMap::const_iterator k = i->second.find("addr");
461                                 if(k!=i->second.end())
462                                 {
463                                         unsigned addr = lexical_cast<unsigned>(k->second);
464
465                                         j = accessories.find(addr|0x10000);
466                                         if(j!=accessories.end())
467                                         {
468                                                 command(format("request(%d, view, control)", i->first));
469                                                 command(format("set(%d, symbol[%d])", i->first, j->second.symbol));
470                                                 command(format("get(%d, state)", i->first));
471
472                                                 accessories.insert(AccessoryMap::value_type(i->first, j->second));
473                                                 accessories.erase(j);
474
475                                                 found = true;
476                                         }
477                                 }
478
479                                 if(!found)
480                                         accessories.insert(AccessoryMap::value_type(i->first, MagnetAccessory()));
481                         }
482
483                         process_object(i->first, i->second);
484                 }
485
486                 accessories_synced = true;
487
488                 for(AccessoryMap::const_iterator i=accessories.lower_bound(0x10000); i!=accessories.end(); ++i)
489                         command("create(11, append)");
490         }
491         else if(msg.header.value=="queryObjects(26)")
492         {
493                 s88.clear();
494                 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
495                 {
496                         s88.push_back(i->first);
497                         command(format("request(%d, view)", i->first));
498                         command(format("get(%d, state)", i->first));
499                 }
500
501                 sensors_synced = true;
502
503                 if(!sensors.empty())
504                 {
505                         unsigned high_addr = (--sensors.end())->first;
506                         if(high_addr>16*s88.size())
507                                 command("create(26, add[0])");
508                 }
509         }
510         else if(msg.header.value=="create(10)")
511         {
512                 Message::ObjectMap::const_iterator i = msg.content.find(10);
513                 if(i!=msg.content.end())
514                 {
515                         Message::AttribMap::const_iterator j = i->second.find("id");
516                         if(j!=i->second.end())
517                         {
518                                 unsigned id = lexical_cast<unsigned>(j->second);
519                                 LocoMap::iterator k = locos.lower_bound(0x10000);
520                                 if(k!=locos.end())
521                                 {
522                                         command(format("request(%d, view, control)", id));
523                                         command(format("set(%d, addr[%d], protocol[%s], name[\"%s\"])",
524                                                 id, k->second.address, (k->second.protocol==MM_27 ? "MM27" : "MM14"), k->second.name));
525                                         command("create(10, append)");
526
527                                         locos.insert(LocoMap::value_type(id, k->second));
528                                         locos.erase(k);
529                                 }
530                         }
531                 }
532
533                 if(locos.lower_bound(0x10000)!=locos.end())
534                         command("create(10)");
535         }
536         else if(!msg.header.value.compare(0, 10, "create(11,"))
537         {
538                 Message::ObjectMap::const_iterator i = msg.content.find(11);
539                 if(i!=msg.content.end())
540                 {
541                         Message::AttribMap::const_iterator j = i->second.find("id");
542                         if(j!=i->second.end())
543                         {
544                                 unsigned id = lexical_cast<unsigned>(j->second);
545                                 AccessoryMap::iterator k = accessories.lower_bound(0x10000);
546                                 if(k!=accessories.end())
547                                 {
548                                         command(format("request(%d, view, control)", id));
549                                         const char *label = (k->second.type==MagnetAccessory::SIGNAL ? "Signal" : "Switch");
550                                         command(format("set(%d, addr[%d], symbol[%d], name1[\"%s\"], name2[\"%d\"], name3[\"\"])",
551                                                 id, k->second.address, k->second.symbol, label, k->second.address));
552                                         command(format("set(%d, state[%d])", id, k->second.state&((1<<k->second.bits)-1)));
553
554                                         k->second.synced = true;
555                                         accessories.insert(AccessoryMap::value_type(id, k->second));
556                                         accessories.erase(k);
557                                 }
558                         }
559                 }
560         }
561         else if(!msg.header.value.compare(0, 10, "create(26,"))
562                 command("queryObjects(26)");
563 }
564
565 void CentralStation::process_event(const Message &msg)
566 {
567         for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
568                 process_object(i->first, i->second);
569 }
570
571 void CentralStation::process_object(unsigned id, const Message::AttribMap &attribs)
572 {
573         if(id==1)
574         {
575                 for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
576                         if(i->first=="status")
577                         {
578                                 power = (i->second=="GO");
579                                 signal_power.emit(power);
580                         }
581         }
582         else if(locos.count(id))
583         {
584                 Locomotive &loco = locos[id];
585                 bool speed_changed = false;
586                 unsigned funcs_changed = 0;
587                 for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
588                 {
589                         if(i->first=="name")
590                                 loco.name = i->second.substr(1, i->second.size()-2);
591                         else if(i->first=="addr")
592                         {
593                                 loco_addr.erase(loco.address);
594                                 loco.address = lexical_cast<unsigned>(i->second);
595                                 loco_addr[loco.address] = id;
596                         }
597                         else if(i->first=="protocol")
598                         {
599                                 if(i->second=="MM")
600                                         loco.protocol = MM;
601                                 else if(i->second=="MM27")
602                                         loco.protocol = MM_27;
603                                 else if(i->second=="MFX")
604                                         loco.protocol = MFX;
605                         }
606                         else if(i->first=="speedstep")
607                         {
608                                 loco.speed = lexical_cast<unsigned>(i->second);
609                                 if(loco.protocol==MFX && loco.speed)
610                                         --loco.speed;
611                                 speed_changed = true;
612                         }
613                         else if(i->first=="dir")
614                         {
615                                 loco.reverse = i->second[0]!='0';
616                                 speed_changed = true;
617                         }
618                         else if(i->first=="func")
619                         {
620                                 vector<string> parts = split(i->second, ", ");
621                                 unsigned func = lexical_cast<unsigned>(parts[0]);
622                                 bool value = lexical_cast<unsigned>(parts[1]);
623                                 loco.funcs &= ~(1<<func);
624                                 if(value)
625                                         loco.funcs |= 1<<func;
626                                 funcs_changed |= 1<<func;
627                         }
628                         else if(i->first=="msg")
629                         {
630                                 if(i->second=="CONTROL_LOST")
631                                         command(format("request(%d, control, force)", id));
632                         }
633                 }
634
635                 if(speed_changed)
636                         signal_loco_speed.emit(loco.address, loco.speed, loco.reverse);
637                 for(unsigned i=0; funcs_changed>>i; ++i)
638                         if(funcs_changed&(1<<i))
639                                 signal_loco_function.emit(loco.address, i, loco.funcs&(1<<i));
640         }
641         else if(accessories.count(id))
642         {
643                 MagnetAccessory &accessory = accessories[id];
644                 bool state_changed = false;
645                 for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
646                 {
647                         if(i->first=="addr")
648                         {
649                                 accessory_addr.erase(accessory.address);
650                                 accessory.address = lexical_cast<unsigned>(i->second);
651                                 accessory_addr[accessory.address] = id;
652                         }
653                         else if(i->first=="state")
654                         {
655                                 unsigned state = lexical_cast<unsigned>(i->second);
656                                 unsigned mask = (1<<accessory.bits)-1;
657                                 accessory.state = (accessory.state&~mask) | (state&mask);
658                                 state_changed = true;
659                         }
660                 }
661
662                 if(state_changed)
663                         accessory_state_changed(accessory);
664         }
665         else if(find(s88.begin(), s88.end(), id)!=s88.end())
666         {
667                 unsigned base = 0;
668                 for(; (base<s88.size() && s88[base]!=id); ++base) ;
669
670                 for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
671                 {
672                         if(i->first=="state")
673                         {
674                                 unsigned state = lexical_cast<unsigned>(i->second, "%i");
675                                 for(unsigned j=0; j<16; ++j)
676                                 {
677                                         unsigned addr = base*16+j+1;
678                                         Sensor &sensor = sensors[addr];
679                                         bool s = state&(1<<j);
680                                         if(s!=sensor.state)
681                                         {
682                                                 sensor.state = s;
683                                                 signal_sensor.emit(addr, sensor.state);
684                                         }
685                                 }
686                         }
687                 }
688         }
689 }
690
691 CentralStation::Protocol CentralStation::map_protocol(const string &name) const
692 {
693         if(name=="MM")
694                 return MM;
695         else if(name=="MM-27")
696                 return MM_27;
697         else if(name=="MFX")
698                 return MFX;
699         else
700                 throw invalid_argument("CentralStation::map_protocol");
701 }
702
703 template<typename T>
704 unsigned CentralStation::map_address(const map<unsigned, T> &omap, const AddressMap &amap, unsigned addr) const
705 {
706         if(omap.count(addr))
707                 return addr;
708         else
709         {
710                 AddressMap::const_iterator i = amap.find(addr);
711                 if(i!=amap.end())
712                         return i->second;
713                 else
714                         return 0;
715         }
716 }
717
718 void CentralStation::skip(string::iterator &iter, const string::iterator &end, const string &what) const
719 {
720         for(; (iter!=end && what.find(*iter)!=string::npos); ++iter) ;
721 }
722
723 string CentralStation::parse_token(string::iterator &iter, const string::iterator &end, const string &stop) const
724 {
725         vector<char> parens;
726         bool quote = false;
727         string token;
728
729         skip(iter, end, stop);
730
731         for(; iter!=end; ++iter)
732         {
733                 if(stop.find(*iter)!=string::npos && parens.empty() && !quote)
734                         break;
735                 else if(*iter=='(' || *iter=='[')
736                         parens.push_back(*iter);
737                 else if((*iter==')' || *iter==']') && !parens.empty())
738                 {
739                         if((*iter==')' && parens.back()!='(') || (*iter==']' && parens.back()!='['))
740                                 IO::print("Mismatched parentheses\n");
741                         parens.pop_back();
742                 }
743                 else if(*iter=='"')
744                         quote = !quote;
745
746                 token += *iter;
747         }
748
749         return token;
750 }
751
752 CentralStation::Tag CentralStation::parse_tag(string::iterator &iter, const string::iterator &end) const
753 {
754         Tag tag;
755
756         for(; (iter!=end && *iter!='<'); ++iter) ;
757         if(iter==end)
758                 return Tag();
759
760         tag.type = parse_token(++iter, end, " >");
761         if(tag.type=="END")
762         {
763                 string code = parse_token(iter, end, " >");
764                 tag.code = lexical_cast<unsigned>(code);
765         }
766         skip(iter, end, " ");
767         tag.value = parse_token(iter, end, ">");
768         if(iter==end)
769                 return Tag();
770         ++iter;
771
772         return tag;
773 }
774
775 CentralStation::Message CentralStation::parse_message(string::iterator &iter, const string::iterator &end) const
776 {
777         Message msg;
778
779         msg.header = parse_tag(iter, end);
780
781         while(iter!=end)
782         {
783                 skip(iter, end, "\r\n");
784                 if(*iter=='<')
785                         break;
786
787                 string id = parse_token(iter, end, " \r\n<");
788                 Message::AttribMap &attribs = msg.content[lexical_cast<unsigned>(id)];
789                 while(iter!=end && *iter!='\n' && *iter!='\r')
790                 {
791                         string attr = parse_token(iter, end, " \r\n<");
792                         string::size_type open_bracket = attr.find('[');
793                         if(open_bracket!=string::npos)
794                         {
795                                 string::size_type close_bracket = attr.rfind(']');
796                                 string attr_name = attr.substr(0, open_bracket);
797                                 string attr_value = attr.substr(open_bracket+1, close_bracket-open_bracket-1);
798                                 attribs.insert(Message::AttribMap::value_type(attr_name, attr_value));
799                         }
800                         else
801                                 attribs.insert(Message::AttribMap::value_type(attr, string()));
802                 }
803         }
804
805         msg.footer = parse_tag(iter, end);
806         if(msg.footer.type.empty())
807                 return Message();
808
809         return msg;
810 }
811
812
813 CentralStation::Tag::Tag():
814         code(0)
815 { }
816
817 CentralStation::Tag::operator bool() const
818 {
819         return !type.empty();
820 }
821
822
823 CentralStation::Message::operator bool() const
824 {
825         return header && footer;
826 }
827
828
829 CentralStation::Locomotive::Locomotive():
830         address(0),
831         speed(0),
832         reverse(false),
833         func_mask(0),
834         funcs(0),
835         control(false)
836 { }
837
838
839 CentralStation::MagnetAccessory::MagnetAccessory():
840         address(0),
841         type(TURNOUT),
842         symbol(TURNOUT_LEFT),
843         state(0),
844         bits(1),
845         synced(false)
846 { }
847
848
849 CentralStation::Sensor::Sensor():
850         state(false)
851 { }
852
853 } // namespace R2C2