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"
10 #include "vehicletype.h"
17 CentralStation::CentralStation(const string &host):
23 accessories_synced(false),
26 RefPtr<Net::SockAddr> addr = Net::resolve(host+":15471");
27 socket.connect(*addr);
29 IO::print("Connected to central station at %s\n", addr->str());
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)");
38 CentralStation::~CentralStation()
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))
49 void CentralStation::set_power(bool p)
52 command(format("set(1, %s)", (power ? "go" : "stop")));
55 void CentralStation::halt(bool h)
60 for(LocoMap::iterator i=locos.begin(); i!=locos.end(); ++i)
62 set_loco_speed(i->first, 0);
65 signal_halt.emit(halted);
68 const char *CentralStation::enumerate_protocols(unsigned index) const
80 unsigned CentralStation::get_protocol_speed_steps(const string &name) const
82 switch(map_protocol(name))
85 case MM_27: return 27;
91 void CentralStation::add_loco(unsigned addr, const string &proto_name, const VehicleType &type)
93 Protocol proto = map_protocol(proto_name);
95 unsigned id = map_address(locos, loco_addr, addr);
98 Locomotive &loco = locos[addr|0x10000];
99 loco.name = type.get_name();
100 loco.protocol = proto;
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;
107 if(locos_synced && proto!=MFX)
108 command("create(10)");
111 command(format("request(%d, view, control, force)", id));
114 void CentralStation::set_loco_speed(unsigned addr, unsigned speed)
119 unsigned id = map_address(locos, loco_addr, addr);
122 Locomotive &loco = locos[id];
123 if(loco.protocol==MFX && speed)
125 command(format("set(%d, speedstep[%d])", id, speed));
129 void CentralStation::set_loco_reverse(unsigned addr, bool rev)
131 unsigned id = map_address(locos, loco_addr, addr);
133 command(format("set(%d, dir[%d])", id, rev));
136 void CentralStation::set_loco_function(unsigned addr, unsigned func, bool state)
138 unsigned id = map_address(locos, loco_addr, addr);
140 command(format("set(%d, func[%d, %d])", id, func, state));
143 void CentralStation::add_turnout(unsigned addr, const TrackType &type)
145 unsigned straight = type.get_paths();
150 const vector<TrackPart> &parts = type.get_parts();
151 for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
153 TrackPoint start = i->get_point(0);
154 TrackPoint end = i->get_point(i->get_length());
155 if(end.dir>start.dir+0.01 || end.dir<start.dir-0.01)
157 (end.dir>start.dir ? left : right) = true;
158 straight &= ~(1<<i->get_path());
160 else if(start.dir<-0.01 || start.dir>0.01)
164 MagnetAccessory::Symbol symbol = MagnetAccessory::TURNOUT_LEFT;
166 symbol = MagnetAccessory::TURNOUT_DOUBLESLIP;
167 else if(left && right)
168 symbol = MagnetAccessory::TURNOUT_THREEWAY;
170 symbol = (straight ? MagnetAccessory::TURNOUT_LEFT : MagnetAccessory::TURNOUT_CURVED_LEFT);
172 symbol = (straight ? MagnetAccessory::TURNOUT_RIGHT : MagnetAccessory::TURNOUT_CURVED_RIGHT);
174 MagnetAccessory &turnout = add_accessory(addr, MagnetAccessory::TURNOUT, symbol);
175 turnout.bits = type.get_state_bits();
178 void CentralStation::set_turnout(unsigned addr, unsigned state)
180 set_accessory_state(addr, MagnetAccessory::TURNOUT, state);
183 unsigned CentralStation::get_turnout(unsigned addr) const
185 return get_accessory_state(addr, MagnetAccessory::TURNOUT);
188 void CentralStation::add_signal(unsigned addr, const SignalType &)
190 add_accessory(addr, MagnetAccessory::SIGNAL, MagnetAccessory::SEMAPHORE_HOME);
193 void CentralStation::set_signal(unsigned addr, unsigned state)
195 set_accessory_state(addr, MagnetAccessory::SIGNAL, state);
198 unsigned CentralStation::get_signal(unsigned addr) const
200 return get_accessory_state(addr, MagnetAccessory::SIGNAL);
203 CentralStation::MagnetAccessory &CentralStation::add_accessory(unsigned addr, MagnetAccessory::Type type, MagnetAccessory::Symbol symbol)
205 unsigned id = map_address(accessories, accessory_addr, addr);
210 MagnetAccessory &accessory = accessories[id];
211 accessory.address = addr;
212 accessory.type = type;
213 accessory.symbol = symbol;
215 accessory_addr[addr] = id;
217 if(accessories_synced)
218 command("create(11, append)");
224 MagnetAccessory &accessory = accessories[id];
225 command(format("request(%d, view, control)", id));
226 if(accessory.symbol!=symbol)
227 command(format("set(%d, symbol[%d])", symbol));
233 void CentralStation::set_accessory_state(unsigned addr, MagnetAccessory::Type type, unsigned state)
235 unsigned id = map_address(accessories, accessory_addr, addr);
238 MagnetAccessory &accessory = accessories[id];
239 if(accessory.type!=type)
240 throw logic_error("accessory type conflict");
242 unsigned mask = (1<<accessory.bits)-1;
244 if(((state^accessory.state)&mask)==0 || !accessory.synced)
246 accessory.state = state;
247 accessory_state_changed(accessory);
251 accessory.state = (accessory.state&mask) | (state&~mask);
253 command(format("set(%d, state[%d])", id, state&mask));
257 unsigned CentralStation::get_accessory_state(unsigned addr, MagnetAccessory::Type type) const
259 unsigned id = map_address(accessories, accessory_addr, addr);
262 AccessoryMap::const_iterator i = accessories.find(id);
263 if(i!=accessories.end() && i->second.type==type)
264 return i->second.state;
269 void CentralStation::accessory_state_changed(const MagnetAccessory &accessory) const
271 if(accessory.type==MagnetAccessory::TURNOUT)
272 signal_turnout.emit(accessory.address, accessory.state);
273 else if(accessory.type==MagnetAccessory::SIGNAL)
274 signal_signal.emit(accessory.address, accessory.state);
277 void CentralStation::add_sensor(unsigned addr)
279 sensors.insert(SensorMap::value_type(addr, Sensor()));
283 if(addr>s88.size()*16)
284 command("create(26, add[0])");
288 bool CentralStation::get_sensor(unsigned addr) const
290 SensorMap::const_iterator i = sensors.find(addr);
292 return i->second.state;
296 void CentralStation::tick()
298 while(Message msg = receive())
301 IO::print("\033[31m*** ERROR: %s: %d %s ***\033[0m\n", msg.header.value, msg.footer.code, msg.footer.value);
303 if(msg.header.type=="REPLY")
305 else if(msg.header.type=="EVENT")
310 void CentralStation::flush()
314 void CentralStation::command(const string &cmd, bool force)
316 if(pending_commands<10 || force)
318 socket.write(cmd+"\r\n");
322 cmd_queue.push_back(cmd);
325 CentralStation::Message CentralStation::receive()
327 while(IO::poll(socket, IO::P_INPUT, Time::zero))
330 unsigned len = socket.read(rbuf, sizeof(rbuf));
334 in_buffer.append(rbuf, len);
337 if(!in_buffer.empty())
339 string::iterator iter = in_buffer.begin();
340 if(Message msg = parse_message(iter, in_buffer.end()))
342 skip(iter, in_buffer.end(), "\r\n");
343 in_buffer.erase(in_buffer.begin(), iter);
345 if(msg.header.type=="REPLY" && pending_commands>0)
348 if(!cmd_queue.empty())
350 command(cmd_queue.front());
351 cmd_queue.pop_front();
362 void CentralStation::process_reply(const Message &msg)
364 if(!msg.header.value.compare(0, 4, "get("))
366 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
368 if(accessories.count(i->first))
369 accessories[i->first].synced = true;
371 process_object(i->first, i->second);
374 else if(!msg.header.value.compare(0, 16, "queryObjects(10,"))
376 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
378 LocoMap::iterator j = locos.find(i->first);
382 Message::AttribMap::const_iterator k = i->second.find("addr");
383 if(k!=i->second.end())
385 unsigned addr = lexical_cast<unsigned>(k->second);
387 j = locos.find(addr|0x10000);
390 command(format("request(%d, view, control, force)", i->first));
391 string cmd = format("get(%d, dir", i->first);
392 for(unsigned l=0; j->second.func_mask>>l; ++l)
393 if((j->second.func_mask>>l)&1)
394 cmd += format(", func[%d]", l);
398 locos.insert(LocoMap::value_type(i->first, j->second));
406 locos.insert(LocoMap::value_type(i->first, Locomotive()));
409 process_object(i->first, i->second);
414 if(locos.lower_bound(0x10000)!=locos.end())
415 command("create(10)");
417 else if(!msg.header.value.compare(0, 16, "queryObjects(11,"))
419 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
421 AccessoryMap::iterator j = accessories.find(i->first);
422 if(j==accessories.end())
425 Message::AttribMap::const_iterator k = i->second.find("addr");
426 if(k!=i->second.end())
428 unsigned addr = lexical_cast<unsigned>(k->second);
430 j = accessories.find(addr|0x10000);
431 if(j!=accessories.end())
433 command(format("request(%d, view, control)", i->first));
434 command(format("set(%d, symbol[%d])", i->first, j->second.symbol));
435 command(format("get(%d, state)", i->first));
437 accessories.insert(AccessoryMap::value_type(i->first, j->second));
438 accessories.erase(j);
445 accessories.insert(AccessoryMap::value_type(i->first, MagnetAccessory()));
448 process_object(i->first, i->second);
451 accessories_synced = true;
453 for(AccessoryMap::const_iterator i=accessories.lower_bound(0x10000); i!=accessories.end(); ++i)
454 command("create(11, append)");
456 else if(msg.header.value=="queryObjects(26)")
459 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
461 s88.push_back(i->first);
462 command(format("request(%d, view)", i->first));
463 command(format("get(%d, state)", i->first));
466 sensors_synced = true;
470 unsigned high_addr = (--sensors.end())->first;
471 if(high_addr>16*s88.size())
472 command("create(26, add[0])");
475 else if(msg.header.value=="create(10)")
477 Message::ObjectMap::const_iterator i = msg.content.find(10);
478 if(i!=msg.content.end())
480 Message::AttribMap::const_iterator j = i->second.find("id");
481 if(j!=i->second.end())
483 unsigned id = lexical_cast<unsigned>(j->second);
484 LocoMap::iterator k = locos.lower_bound(0x10000);
487 command(format("request(%d, view, control)", id));
488 command(format("set(%d, addr[%d], protocol[%s], name[\"%s\"])",
489 id, k->second.address, (k->second.protocol==MM_27 ? "MM27" : "MM14"), k->second.name));
490 command("create(10, append)");
492 locos.insert(LocoMap::value_type(id, k->second));
498 if(locos.lower_bound(0x10000)!=locos.end())
499 command("create(10)");
501 else if(!msg.header.value.compare(0, 10, "create(11,"))
503 Message::ObjectMap::const_iterator i = msg.content.find(11);
504 if(i!=msg.content.end())
506 Message::AttribMap::const_iterator j = i->second.find("id");
507 if(j!=i->second.end())
509 unsigned id = lexical_cast<unsigned>(j->second);
510 AccessoryMap::iterator k = accessories.lower_bound(0x10000);
511 if(k!=accessories.end())
513 command(format("request(%d, view, control)", id));
514 const char *label = (k->second.type==MagnetAccessory::SIGNAL ? "Signal" : "Switch");
515 command(format("set(%d, addr[%d], symbol[%d], name1[\"%s\"], name2[\"%d\"], name3[\"\"])",
516 id, k->second.address, k->second.symbol, label, k->second.address));
517 command(format("set(%d, state[%d])", id, k->second.state&((1<<k->second.bits)-1)));
519 k->second.synced = true;
520 accessories.insert(AccessoryMap::value_type(id, k->second));
521 accessories.erase(k);
526 else if(!msg.header.value.compare(0, 10, "create(26,"))
527 command("queryObjects(26)");
530 void CentralStation::process_event(const Message &msg)
532 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
533 process_object(i->first, i->second);
536 void CentralStation::process_object(unsigned id, const Message::AttribMap &attribs)
540 for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
541 if(i->first=="status")
543 power = (i->second=="GO");
544 signal_power.emit(power);
547 else if(locos.count(id))
549 Locomotive &loco = locos[id];
550 bool speed_changed = false;
551 unsigned funcs_changed = 0;
552 for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
555 loco.name = i->second.substr(1, i->second.size()-2);
556 else if(i->first=="addr")
558 loco_addr.erase(loco.address);
559 loco.address = lexical_cast<unsigned>(i->second);
560 loco_addr[loco.address] = id;
562 else if(i->first=="protocol")
566 else if(i->second=="MM27")
567 loco.protocol = MM_27;
568 else if(i->second=="MFX")
571 else if(i->first=="speedstep")
573 loco.speed = lexical_cast<unsigned>(i->second);
574 if(loco.protocol==MFX && loco.speed)
576 speed_changed = true;
578 else if(i->first=="dir")
580 loco.reverse = i->second[0]!='0';
581 speed_changed = true;
583 else if(i->first=="func")
585 vector<string> parts = split(i->second, ", ");
586 unsigned func = lexical_cast<unsigned>(parts[0]);
587 bool value = lexical_cast<unsigned>(parts[1]);
588 loco.funcs &= ~(1<<func);
590 loco.funcs |= 1<<func;
591 funcs_changed |= 1<<func;
593 else if(i->first=="msg")
595 if(i->second=="CONTROL_LOST")
596 command(format("request(%d, control, force)", id));
601 signal_loco_speed.emit(loco.address, loco.speed, loco.reverse);
602 for(unsigned i=0; funcs_changed>>i; ++i)
603 if(funcs_changed&(1<<i))
604 signal_loco_function.emit(loco.address, i, loco.funcs&(1<<i));
606 else if(accessories.count(id))
608 MagnetAccessory &accessory = accessories[id];
609 bool state_changed = false;
610 for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
614 accessory_addr.erase(accessory.address);
615 accessory.address = lexical_cast<unsigned>(i->second);
616 accessory_addr[accessory.address] = id;
618 else if(i->first=="state")
620 unsigned state = lexical_cast<unsigned>(i->second);
621 unsigned mask = (1<<accessory.bits)-1;
622 accessory.state = (accessory.state&~mask) | (state&mask);
623 state_changed = true;
628 accessory_state_changed(accessory);
630 else if(find(s88.begin(), s88.end(), id)!=s88.end())
633 for(; (base<s88.size() && s88[base]!=id); ++base) ;
635 for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
637 if(i->first=="state")
639 unsigned state = lexical_cast<unsigned>(i->second, "%i");
640 for(unsigned j=0; j<16; ++j)
642 unsigned addr = base*16+j+1;
643 Sensor &sensor = sensors[addr];
644 bool s = state&(1<<j);
648 signal_sensor.emit(addr, sensor.state);
656 CentralStation::Protocol CentralStation::map_protocol(const string &name) const
660 else if(name=="MM-27")
665 throw invalid_argument("CentralStation::map_protocol");
669 unsigned CentralStation::map_address(const map<unsigned, T> &omap, const AddressMap &amap, unsigned addr) const
675 AddressMap::const_iterator i = amap.find(addr);
683 void CentralStation::skip(string::iterator &iter, const string::iterator &end, const string &what) const
685 for(; (iter!=end && what.find(*iter)!=string::npos); ++iter) ;
688 string CentralStation::parse_token(string::iterator &iter, const string::iterator &end, const string &stop) const
694 skip(iter, end, stop);
696 for(; iter!=end; ++iter)
698 if(stop.find(*iter)!=string::npos && parens.empty() && !quote)
700 else if(*iter=='(' || *iter=='[')
701 parens.push_back(*iter);
702 else if((*iter==')' || *iter==']') && !parens.empty())
704 if((*iter==')' && parens.back()!='(') || (*iter==']' && parens.back()!='['))
705 IO::print("Mismatched parentheses\n");
717 CentralStation::Tag CentralStation::parse_tag(string::iterator &iter, const string::iterator &end) const
721 for(; (iter!=end && *iter!='<'); ++iter) ;
725 tag.type = parse_token(++iter, end, " >");
728 string code = parse_token(iter, end, " >");
729 tag.code = lexical_cast<unsigned>(code);
731 skip(iter, end, " ");
732 tag.value = parse_token(iter, end, ">");
740 CentralStation::Message CentralStation::parse_message(string::iterator &iter, const string::iterator &end) const
744 msg.header = parse_tag(iter, end);
748 skip(iter, end, "\r\n");
752 string id = parse_token(iter, end, " \r\n<");
753 Message::AttribMap &attribs = msg.content[lexical_cast<unsigned>(id)];
754 while(iter!=end && *iter!='\n' && *iter!='\r')
756 string attr = parse_token(iter, end, " \r\n<");
757 string::size_type open_bracket = attr.find('[');
758 if(open_bracket!=string::npos)
760 string::size_type close_bracket = attr.rfind(']');
761 string attr_name = attr.substr(0, open_bracket);
762 string attr_value = attr.substr(open_bracket+1, close_bracket-open_bracket-1);
763 attribs.insert(Message::AttribMap::value_type(attr_name, attr_value));
766 attribs.insert(Message::AttribMap::value_type(attr, string()));
770 msg.footer = parse_tag(iter, end);
771 if(msg.footer.type.empty())
778 CentralStation::Tag::Tag():
782 CentralStation::Tag::operator bool() const
784 return !type.empty();
788 CentralStation::Message::operator bool() const
790 return header && footer;
794 CentralStation::Locomotive::Locomotive():
804 CentralStation::MagnetAccessory::MagnetAccessory():
807 symbol(TURNOUT_LEFT),
814 CentralStation::Sensor::Sensor():