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