2 #include <msp/core/maputils.h>
3 #include <msp/core/refptr.h>
4 #include <msp/io/print.h>
5 #include <msp/net/resolve.h>
6 #include <msp/strings/utils.h>
7 #include <msp/time/units.h>
8 #include <msp/time/utils.h>
9 #include "centralstation.h"
10 #include "tracktype.h"
11 #include "vehicletype.h"
18 CentralStation::CentralStation(const Options &opts):
24 accessories_synced(false),
27 RefPtr<Net::SockAddr> addr = Net::resolve(opts.get<string>(string())+":15471");
28 socket.connect(*addr);
30 IO::print("Connected to central station at %s\n", addr->str());
32 command("get(1, status)");
33 command("request(1, view)");
34 command("queryObjects(10, addr, name)");
35 command("queryObjects(11, addr)");
36 command("queryObjects(26)");
39 CentralStation::~CentralStation()
41 command("release(1, view)", true);
42 for(LocoMap::iterator i=locos.begin(); (i!=locos.end() && !(i->first&0x10000)); ++i)
43 command(format("release(%d, view, control)", i->first));
44 for(AccessoryMap::iterator i=accessories.begin(); (i!=accessories.end() && !(i->first&0x10000)); ++i)
45 command(format("release(%d, view, control)", i->first));
46 while(IO::poll(socket, IO::P_INPUT, 100*Time::msec))
50 void CentralStation::set_power(bool p)
53 command(format("set(1, %s)", (power ? "go" : "stop")));
56 void CentralStation::halt(bool h)
61 for(LocoMap::iterator i=locos.begin(); i!=locos.end(); ++i)
63 set_loco_speed(i->first, 0);
66 signal_halt.emit(halted);
69 const char *CentralStation::enumerate_protocols(unsigned index) const
81 unsigned CentralStation::get_protocol_speed_steps(const string &name) const
83 switch(map_protocol(name))
86 case MM_27: return 27;
92 unsigned CentralStation::add_loco(unsigned addr, const string &proto_name, const VehicleType &type)
94 Protocol proto = map_protocol(proto_name);
96 unsigned id = map_address(locos, loco_addr, addr);
99 Locomotive &loco = locos[addr|0x10000];
100 loco.name = type.get_name();
101 loco.protocol = proto;
104 const VehicleType::FunctionMap &type_funcs = type.get_functions();
105 for(VehicleType::FunctionMap::const_iterator i=type_funcs.begin(); i!=type_funcs.end(); ++i)
106 loco.func_mask |= 1<<i->first;
108 if(locos_synced && proto!=MFX)
109 command("create(10)");
112 command(format("request(%d, view, control, force)", id));
117 void CentralStation::remove_loco(unsigned addr)
119 unsigned id = map_address(locos, loco_addr, addr);
121 command(format("release(%d, view, control)", id));
124 void CentralStation::set_loco_speed(unsigned addr, unsigned speed)
129 unsigned id = map_address(locos, loco_addr, addr);
132 Locomotive &loco = locos[id];
133 if(loco.protocol==MFX && speed)
135 command(format("set(%d, speedstep[%d])", id, speed));
139 void CentralStation::set_loco_reverse(unsigned addr, bool rev)
141 unsigned id = map_address(locos, loco_addr, addr);
143 command(format("set(%d, dir[%d])", id, rev));
146 void CentralStation::set_loco_function(unsigned addr, unsigned func, bool state)
148 unsigned id = map_address(locos, loco_addr, addr);
150 command(format("set(%d, func[%d, %d])", id, func, state));
153 unsigned CentralStation::add_turnout(unsigned addr, const TrackType &type)
155 unsigned straight = type.get_paths();
160 const vector<TrackPart> &parts = type.get_parts();
161 for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
163 OrientedPoint start = i->get_point(0);
164 OrientedPoint end = i->get_point(i->get_length());
165 if(abs(end.rotation-start.rotation).radians()<0.01)
167 (end.rotation>start.rotation ? left : right) = true;
168 straight &= ~(1<<i->get_path());
170 else if(abs(start.rotation).radians()>0.01)
174 MagnetAccessory::Symbol symbol = MagnetAccessory::TURNOUT_LEFT;
176 symbol = MagnetAccessory::TURNOUT_DOUBLESLIP;
177 else if(left && right)
178 symbol = MagnetAccessory::TURNOUT_THREEWAY;
180 symbol = (straight ? MagnetAccessory::TURNOUT_LEFT : MagnetAccessory::TURNOUT_CURVED_LEFT);
182 symbol = (straight ? MagnetAccessory::TURNOUT_RIGHT : MagnetAccessory::TURNOUT_CURVED_RIGHT);
184 MagnetAccessory &turnout = add_accessory(addr, MagnetAccessory::TURNOUT, symbol);
185 turnout.bits = type.get_state_bits();
190 void CentralStation::remove_turnout(unsigned addr)
192 remove_accessory(addr);
195 void CentralStation::set_turnout(unsigned addr, unsigned state)
197 set_accessory_state(addr, MagnetAccessory::TURNOUT, state);
200 unsigned CentralStation::get_turnout(unsigned addr) const
202 return get_accessory_state(addr, MagnetAccessory::TURNOUT);
205 unsigned CentralStation::add_signal(unsigned addr, const SignalType &)
207 add_accessory(addr, MagnetAccessory::SIGNAL, MagnetAccessory::SEMAPHORE_HOME);
211 void CentralStation::remove_signal(unsigned addr)
213 remove_accessory(addr);
216 void CentralStation::set_signal(unsigned addr, unsigned state)
218 set_accessory_state(addr, MagnetAccessory::SIGNAL, state);
221 unsigned CentralStation::get_signal(unsigned addr) const
223 return get_accessory_state(addr, MagnetAccessory::SIGNAL);
226 CentralStation::MagnetAccessory &CentralStation::add_accessory(unsigned addr, MagnetAccessory::Type type, MagnetAccessory::Symbol symbol)
228 unsigned id = map_address(accessories, accessory_addr, addr);
233 MagnetAccessory &accessory = accessories[id];
234 accessory.address = addr;
235 accessory.type = type;
236 accessory.symbol = symbol;
238 accessory_addr[addr] = id;
240 if(accessories_synced)
241 command("create(11, append)");
247 MagnetAccessory &accessory = accessories[id];
248 command(format("request(%d, view, control)", id));
249 if(accessory.symbol!=symbol)
250 command(format("set(%d, symbol[%d])", symbol));
256 void CentralStation::remove_accessory(unsigned addr)
258 unsigned id = map_address(accessories, accessory_addr, addr);
260 command(format("release(%d, view, control)", id));
263 void CentralStation::set_accessory_state(unsigned addr, MagnetAccessory::Type type, unsigned state)
265 unsigned id = map_address(accessories, accessory_addr, addr);
268 MagnetAccessory &accessory = accessories[id];
269 if(accessory.type!=type)
270 throw logic_error("accessory type conflict");
272 unsigned mask = (1<<accessory.bits)-1;
274 if(((state^accessory.state)&mask)==0 || !accessory.synced)
276 accessory.state = state;
277 accessory_state_changed(accessory);
281 accessory.state = (accessory.state&mask) | (state&~mask);
283 command(format("set(%d, state[%d])", id, state&mask));
287 unsigned CentralStation::get_accessory_state(unsigned addr, MagnetAccessory::Type type) const
289 unsigned id = map_address(accessories, accessory_addr, addr);
292 AccessoryMap::const_iterator i = accessories.find(id);
293 if(i!=accessories.end() && i->second.type==type)
294 return i->second.state;
299 void CentralStation::accessory_state_changed(const MagnetAccessory &accessory) const
301 if(accessory.type==MagnetAccessory::TURNOUT)
302 signal_turnout.emit(accessory.address, accessory.state);
303 else if(accessory.type==MagnetAccessory::SIGNAL)
304 signal_signal.emit(accessory.address, accessory.state);
307 unsigned CentralStation::add_sensor(unsigned addr)
309 sensors.insert(SensorMap::value_type(addr, Sensor()));
313 if(addr>s88.size()*16)
314 command("create(26, add[0])");
320 void CentralStation::remove_sensor(unsigned)
324 bool CentralStation::get_sensor(unsigned addr) const
326 SensorMap::const_iterator i = sensors.find(addr);
328 return i->second.state;
332 float CentralStation::get_telemetry_value(const string &name) const
334 throw key_error(name);
337 void CentralStation::tick()
339 while(Message msg = receive())
342 IO::print("\033[31m*** ERROR: %s: %d %s ***\033[0m\n", msg.header.value, msg.footer.code, msg.footer.value);
344 if(msg.header.type=="REPLY")
346 else if(msg.header.type=="EVENT")
351 void CentralStation::flush()
355 void CentralStation::command(const string &cmd, bool force)
357 if(pending_commands<10 || force)
359 socket.write(cmd+"\r\n");
363 cmd_queue.push_back(cmd);
366 CentralStation::Message CentralStation::receive()
368 while(IO::poll(socket, IO::P_INPUT, Time::zero))
371 unsigned len = socket.read(rbuf, sizeof(rbuf));
375 in_buffer.append(rbuf, len);
378 if(!in_buffer.empty())
380 string::iterator iter = in_buffer.begin();
381 if(Message msg = parse_message(iter, in_buffer.end()))
383 skip(iter, in_buffer.end(), "\r\n");
384 in_buffer.erase(in_buffer.begin(), iter);
386 if(msg.header.type=="REPLY" && pending_commands>0)
389 if(!cmd_queue.empty())
391 command(cmd_queue.front());
392 cmd_queue.pop_front();
403 void CentralStation::process_reply(const Message &msg)
405 if(!msg.header.value.compare(0, 4, "get("))
407 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
409 if(accessories.count(i->first))
410 accessories[i->first].synced = true;
412 process_object(i->first, i->second);
415 else if(!msg.header.value.compare(0, 16, "queryObjects(10,"))
417 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
419 LocoMap::iterator j = locos.find(i->first);
423 Message::AttribMap::const_iterator k = i->second.find("addr");
424 if(k!=i->second.end())
426 unsigned addr = lexical_cast<unsigned>(k->second);
428 j = locos.find(addr|0x10000);
431 command(format("request(%d, view, control, force)", i->first));
432 string cmd = format("get(%d, dir", i->first);
433 for(unsigned l=0; j->second.func_mask>>l; ++l)
434 if((j->second.func_mask>>l)&1)
435 cmd += format(", func[%d]", l);
439 locos.insert(LocoMap::value_type(i->first, j->second));
447 locos.insert(LocoMap::value_type(i->first, Locomotive()));
450 process_object(i->first, i->second);
455 if(locos.lower_bound(0x10000)!=locos.end())
456 command("create(10)");
458 else if(!msg.header.value.compare(0, 16, "queryObjects(11,"))
460 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
462 AccessoryMap::iterator j = accessories.find(i->first);
463 if(j==accessories.end())
466 Message::AttribMap::const_iterator k = i->second.find("addr");
467 if(k!=i->second.end())
469 unsigned addr = lexical_cast<unsigned>(k->second);
471 j = accessories.find(addr|0x10000);
472 if(j!=accessories.end())
474 command(format("request(%d, view, control)", i->first));
475 command(format("set(%d, symbol[%d])", i->first, j->second.symbol));
476 command(format("get(%d, state)", i->first));
478 accessories.insert(AccessoryMap::value_type(i->first, j->second));
479 accessories.erase(j);
486 accessories.insert(AccessoryMap::value_type(i->first, MagnetAccessory()));
489 process_object(i->first, i->second);
492 accessories_synced = true;
494 for(AccessoryMap::const_iterator i=accessories.lower_bound(0x10000); i!=accessories.end(); ++i)
495 command("create(11, append)");
497 else if(msg.header.value=="queryObjects(26)")
500 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
502 s88.push_back(i->first);
503 command(format("request(%d, view)", i->first));
504 command(format("get(%d, state)", i->first));
507 sensors_synced = true;
511 unsigned high_addr = (--sensors.end())->first;
512 if(high_addr>16*s88.size())
513 command("create(26, add[0])");
516 else if(msg.header.value=="create(10)")
518 Message::ObjectMap::const_iterator i = msg.content.find(10);
519 if(i!=msg.content.end())
521 Message::AttribMap::const_iterator j = i->second.find("id");
522 if(j!=i->second.end())
524 unsigned id = lexical_cast<unsigned>(j->second);
525 LocoMap::iterator k = locos.lower_bound(0x10000);
528 command(format("request(%d, view, control)", id));
529 command(format("set(%d, addr[%d], protocol[%s], name[\"%s\"])",
530 id, k->second.address, (k->second.protocol==MM_27 ? "MM27" : "MM14"), k->second.name));
531 command("create(10, append)");
533 locos.insert(LocoMap::value_type(id, k->second));
539 if(locos.lower_bound(0x10000)!=locos.end())
540 command("create(10)");
542 else if(!msg.header.value.compare(0, 10, "create(11,"))
544 Message::ObjectMap::const_iterator i = msg.content.find(11);
545 if(i!=msg.content.end())
547 Message::AttribMap::const_iterator j = i->second.find("id");
548 if(j!=i->second.end())
550 unsigned id = lexical_cast<unsigned>(j->second);
551 AccessoryMap::iterator k = accessories.lower_bound(0x10000);
552 if(k!=accessories.end())
554 command(format("request(%d, view, control)", id));
555 const char *label = (k->second.type==MagnetAccessory::SIGNAL ? "Signal" : "Switch");
556 command(format("set(%d, addr[%d], symbol[%d], name1[\"%s\"], name2[\"%d\"], name3[\"\"])",
557 id, k->second.address, k->second.symbol, label, k->second.address));
558 command(format("set(%d, state[%d])", id, k->second.state&((1<<k->second.bits)-1)));
560 k->second.synced = true;
561 accessories.insert(AccessoryMap::value_type(id, k->second));
562 accessories.erase(k);
567 else if(!msg.header.value.compare(0, 10, "create(26,"))
568 command("queryObjects(26)");
571 void CentralStation::process_event(const Message &msg)
573 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
574 process_object(i->first, i->second);
577 void CentralStation::process_object(unsigned id, const Message::AttribMap &attribs)
581 for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
582 if(i->first=="status")
584 power = (i->second=="GO");
585 signal_power.emit(power);
588 else if(locos.count(id))
590 Locomotive &loco = locos[id];
591 bool speed_changed = false;
592 unsigned funcs_changed = 0;
593 for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
596 loco.name = i->second.substr(1, i->second.size()-2);
597 else if(i->first=="addr")
599 loco_addr.erase(loco.address);
600 loco.address = lexical_cast<unsigned>(i->second);
601 loco_addr[loco.address] = id;
603 else if(i->first=="protocol")
607 else if(i->second=="MM27")
608 loco.protocol = MM_27;
609 else if(i->second=="MFX")
612 else if(i->first=="speedstep")
614 loco.speed = lexical_cast<unsigned>(i->second);
615 if(loco.protocol==MFX && loco.speed)
617 speed_changed = true;
619 else if(i->first=="dir")
621 loco.reverse = i->second[0]!='0';
622 speed_changed = true;
624 else if(i->first=="func")
626 vector<string> parts = split(i->second, ", ");
627 unsigned func = lexical_cast<unsigned>(parts[0]);
628 bool value = lexical_cast<unsigned>(parts[1]);
629 loco.funcs &= ~(1<<func);
631 loco.funcs |= 1<<func;
632 funcs_changed |= 1<<func;
634 else if(i->first=="msg")
636 if(i->second=="CONTROL_LOST")
637 command(format("request(%d, control, force)", id));
642 signal_loco_speed.emit(loco.address, loco.speed, loco.reverse);
643 for(unsigned i=0; funcs_changed>>i; ++i)
644 if(funcs_changed&(1<<i))
645 signal_loco_function.emit(loco.address, i, loco.funcs&(1<<i));
647 else if(accessories.count(id))
649 MagnetAccessory &accessory = accessories[id];
650 bool state_changed = false;
651 for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
655 accessory_addr.erase(accessory.address);
656 accessory.address = lexical_cast<unsigned>(i->second);
657 accessory_addr[accessory.address] = id;
659 else if(i->first=="state")
661 unsigned state = lexical_cast<unsigned>(i->second);
662 unsigned mask = (1<<accessory.bits)-1;
663 accessory.state = (accessory.state&~mask) | (state&mask);
664 state_changed = true;
669 accessory_state_changed(accessory);
671 else if(find(s88.begin(), s88.end(), id)!=s88.end())
674 for(; (base<s88.size() && s88[base]!=id); ++base) ;
676 for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
678 if(i->first=="state")
680 unsigned state = lexical_cast<unsigned>(i->second, "%i");
681 for(unsigned j=0; j<16; ++j)
683 unsigned addr = base*16+j+1;
684 Sensor &sensor = sensors[addr];
685 bool s = state&(1<<j);
689 signal_sensor.emit(addr, sensor.state);
697 CentralStation::Protocol CentralStation::map_protocol(const string &name) const
701 else if(name=="MM-27")
706 throw invalid_argument("CentralStation::map_protocol");
710 unsigned CentralStation::map_address(const map<unsigned, T> &omap, const AddressMap &amap, unsigned addr) const
716 AddressMap::const_iterator i = amap.find(addr);
724 void CentralStation::skip(string::iterator &iter, const string::iterator &end, const string &what) const
726 for(; (iter!=end && what.find(*iter)!=string::npos); ++iter) ;
729 string CentralStation::parse_token(string::iterator &iter, const string::iterator &end, const string &stop) const
735 skip(iter, end, stop);
737 for(; iter!=end; ++iter)
739 if(stop.find(*iter)!=string::npos && parens.empty() && !quote)
741 else if(*iter=='(' || *iter=='[')
742 parens.push_back(*iter);
743 else if((*iter==')' || *iter==']') && !parens.empty())
745 if((*iter==')' && parens.back()!='(') || (*iter==']' && parens.back()!='['))
746 IO::print("Mismatched parentheses\n");
758 CentralStation::Tag CentralStation::parse_tag(string::iterator &iter, const string::iterator &end) const
762 for(; (iter!=end && *iter!='<'); ++iter) ;
766 tag.type = parse_token(++iter, end, " >");
769 string code = parse_token(iter, end, " >");
770 tag.code = lexical_cast<unsigned>(code);
772 skip(iter, end, " ");
773 tag.value = parse_token(iter, end, ">");
781 CentralStation::Message CentralStation::parse_message(string::iterator &iter, const string::iterator &end) const
785 msg.header = parse_tag(iter, end);
789 skip(iter, end, "\r\n");
793 string id = parse_token(iter, end, " \r\n<");
794 Message::AttribMap &attribs = msg.content[lexical_cast<unsigned>(id)];
795 while(iter!=end && *iter!='\n' && *iter!='\r')
797 string attr = parse_token(iter, end, " \r\n<");
798 string::size_type open_bracket = attr.find('[');
799 if(open_bracket!=string::npos)
801 string::size_type close_bracket = attr.rfind(']');
802 string attr_name = attr.substr(0, open_bracket);
803 string attr_value = attr.substr(open_bracket+1, close_bracket-open_bracket-1);
804 attribs.insert(Message::AttribMap::value_type(attr_name, attr_value));
807 attribs.insert(Message::AttribMap::value_type(attr, string()));
811 msg.footer = parse_tag(iter, end);
812 if(msg.footer.type.empty())
819 CentralStation::Tag::Tag():
823 CentralStation::Tag::operator bool() const
825 return !type.empty();
829 CentralStation::Message::operator bool() const
831 return header && footer;
835 CentralStation::Locomotive::Locomotive():
845 CentralStation::MagnetAccessory::MagnetAccessory():
848 symbol(TURNOUT_LEFT),
855 CentralStation::Sensor::Sensor():