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 turnouts_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(TurnoutMap::iterator i=turnouts.begin(); (i!=turnouts.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)
149 const vector<TrackPart> &parts = type.get_parts();
150 for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
152 TrackPoint start = i->get_point(0);
153 TrackPoint end = i->get_point(i->get_length());
154 if(end.dir>start.dir+0.01)
156 else if(end.dir<start.dir-0.01)
158 else if(start.dir<-0.01 || start.dir>0.01)
162 unsigned symbol = Turnout::LEFT;
164 symbol = Turnout::DOUBLESLIP;
165 else if(left && right)
166 symbol = Turnout::THREEWAY;
168 symbol = Turnout::LEFT;
170 symbol = Turnout::RIGHT;
172 unsigned id = map_address(turnouts, turnout_addr, addr);
177 Turnout &turnout = turnouts[id];
178 turnout.address = addr;
179 turnout.bits = type.get_state_bits();
180 turnout.symbol = symbol;
182 turnout_addr[addr] = id;
185 command("create(11, append)");
189 Turnout &turnout = turnouts[id];
190 command(format("request(%d, view, control)", id));
191 if(turnout.symbol!=symbol)
192 command(format("set(%d, symbol[%d])", symbol));
196 void CentralStation::set_turnout(unsigned addr, unsigned state)
198 unsigned id = map_address(turnouts, turnout_addr, addr);
201 Turnout &turnout = turnouts[id];
202 unsigned mask = (1<<turnout.bits)-1;
204 if(((state^turnout.state)&mask)==0 || !turnout.synced)
206 turnout.state = state;
207 signal_turnout.emit(addr, turnout.state);
211 turnout.state = (turnout.state&mask) | (state&~mask);
213 command(format("set(%d, state[%d])", id, state&mask));
217 unsigned CentralStation::get_turnout(unsigned addr) const
219 unsigned id = map_address(turnouts, turnout_addr, addr);
222 TurnoutMap::const_iterator i = turnouts.find(id);
223 if(i!=turnouts.end())
224 return i->second.state;
229 void CentralStation::add_sensor(unsigned addr)
231 sensors.insert(SensorMap::value_type(addr, Sensor()));
235 if(addr>s88.size()*16)
236 command("create(26, add[0])");
240 bool CentralStation::get_sensor(unsigned addr) const
242 SensorMap::const_iterator i = sensors.find(addr);
244 return i->second.state;
248 void CentralStation::tick()
250 while(Message msg = receive())
253 IO::print("\033[31m*** ERROR: %s: %d %s ***\033[0m\n", msg.header.value, msg.footer.code, msg.footer.value);
255 if(msg.header.type=="REPLY")
257 else if(msg.header.type=="EVENT")
262 void CentralStation::flush()
266 void CentralStation::command(const string &cmd, bool force)
268 if(pending_commands<10 || force)
270 socket.write(cmd+"\r\n");
274 cmd_queue.push_back(cmd);
277 CentralStation::Message CentralStation::receive()
279 while(IO::poll(socket, IO::P_INPUT, Time::zero))
282 unsigned len = socket.read(rbuf, sizeof(rbuf));
286 in_buffer.append(rbuf, len);
289 if(!in_buffer.empty())
291 string::iterator iter = in_buffer.begin();
292 if(Message msg = parse_message(iter, in_buffer.end()))
294 skip(iter, in_buffer.end(), "\r\n");
295 in_buffer.erase(in_buffer.begin(), iter);
297 if(msg.header.type=="REPLY" && pending_commands>0)
300 if(!cmd_queue.empty())
302 command(cmd_queue.front());
303 cmd_queue.pop_front();
314 void CentralStation::process_reply(const Message &msg)
316 if(!msg.header.value.compare(0, 4, "get("))
318 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
320 if(turnouts.count(i->first))
321 turnouts[i->first].synced = true;
323 process_object(i->first, i->second);
326 else if(!msg.header.value.compare(0, 16, "queryObjects(10,"))
328 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
330 LocoMap::iterator j = locos.find(i->first);
334 Message::AttribMap::const_iterator k = i->second.find("addr");
335 if(k!=i->second.end())
337 unsigned addr = lexical_cast<unsigned>(k->second);
339 j = locos.find(addr|0x10000);
342 command(format("request(%d, view, control, force)", i->first));
343 string cmd = format("get(%d, dir", i->first);
344 for(unsigned l=0; j->second.func_mask>>l; ++l)
345 if((j->second.func_mask>>l)&1)
346 cmd += format(", func[%d]", l);
350 locos.insert(LocoMap::value_type(i->first, j->second));
358 locos.insert(LocoMap::value_type(i->first, Locomotive()));
361 process_object(i->first, i->second);
366 if(locos.lower_bound(0x10000)!=locos.end())
367 command("create(10)");
369 else if(!msg.header.value.compare(0, 16, "queryObjects(11,"))
371 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
373 TurnoutMap::iterator j = turnouts.find(i->first);
374 if(j==turnouts.end())
377 Message::AttribMap::const_iterator k = i->second.find("addr");
378 if(k!=i->second.end())
380 unsigned addr = lexical_cast<unsigned>(k->second);
382 j = turnouts.find(addr|0x10000);
383 if(j!=turnouts.end())
385 command(format("request(%d, view, control)", i->first));
386 command(format("set(%d, symbol[%d])", i->first, j->second.symbol));
387 command(format("get(%d, state)", i->first));
389 turnouts.insert(TurnoutMap::value_type(i->first, j->second));
397 turnouts.insert(TurnoutMap::value_type(i->first, Turnout()));
400 process_object(i->first, i->second);
403 turnouts_synced = true;
405 for(TurnoutMap::const_iterator i=turnouts.lower_bound(0x10000); i!=turnouts.end(); ++i)
406 command("create(11, append)");
408 else if(msg.header.value=="queryObjects(26)")
411 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
413 s88.push_back(i->first);
414 command(format("request(%d, view)", i->first));
415 command(format("get(%d, state)", i->first));
418 sensors_synced = true;
422 unsigned high_addr = (--sensors.end())->first;
423 if(high_addr>16*s88.size())
424 command("create(26, add[0])");
427 else if(msg.header.value=="create(10)")
429 Message::ObjectMap::const_iterator i = msg.content.find(10);
430 if(i!=msg.content.end())
432 Message::AttribMap::const_iterator j = i->second.find("id");
433 if(j!=i->second.end())
435 unsigned id = lexical_cast<unsigned>(j->second);
436 LocoMap::iterator k = locos.lower_bound(0x10000);
439 command(format("request(%d, view, control)", id));
440 command(format("set(%d, addr[%d], protocol[%s], name[\"%s\"])",
441 id, k->second.address, (k->second.protocol==MM_27 ? "MM27" : "MM14"), k->second.name));
442 command("create(10, append)");
444 locos.insert(LocoMap::value_type(id, k->second));
450 if(locos.lower_bound(0x10000)!=locos.end())
451 command("create(10)");
453 else if(!msg.header.value.compare(0, 10, "create(11,"))
455 Message::ObjectMap::const_iterator i = msg.content.find(11);
456 if(i!=msg.content.end())
458 Message::AttribMap::const_iterator j = i->second.find("id");
459 if(j!=i->second.end())
461 unsigned id = lexical_cast<unsigned>(j->second);
462 TurnoutMap::iterator k = turnouts.lower_bound(0x10000);
463 if(k!=turnouts.end())
465 command(format("request(%d, view, control)", id));
466 command(format("set(%d, addr[%d], symbol[%d], name1[\"Switch\"], name2[\"%d\"], name3[\"\"])",
467 id, k->second.address, k->second.symbol, k->second.address));
468 command(format("set(%d, state[%d])", id, k->second.state&((1<<k->second.bits)-1)));
470 k->second.synced = true;
471 turnouts.insert(TurnoutMap::value_type(id, k->second));
477 else if(!msg.header.value.compare(0, 10, "create(26,"))
478 command("queryObjects(26)");
481 void CentralStation::process_event(const Message &msg)
483 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
484 process_object(i->first, i->second);
487 void CentralStation::process_object(unsigned id, const Message::AttribMap &attribs)
491 for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
492 if(i->first=="status")
494 power = (i->second=="GO");
495 signal_power.emit(power);
498 else if(locos.count(id))
500 Locomotive &loco = locos[id];
501 bool speed_changed = false;
502 unsigned funcs_changed = 0;
503 for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
506 loco.name = i->second.substr(1, i->second.size()-2);
507 else if(i->first=="addr")
509 loco_addr.erase(loco.address);
510 loco.address = lexical_cast<unsigned>(i->second);
511 loco_addr[loco.address] = id;
513 else if(i->first=="protocol")
517 else if(i->second=="MM27")
518 loco.protocol = MM_27;
519 else if(i->second=="MFX")
522 else if(i->first=="speedstep")
524 loco.speed = lexical_cast<unsigned>(i->second);
525 if(loco.protocol==MFX && loco.speed)
527 speed_changed = true;
529 else if(i->first=="dir")
531 loco.reverse = i->second[0]!='0';
532 speed_changed = true;
534 else if(i->first=="func")
536 vector<string> parts = split(i->second, ", ");
537 unsigned func = lexical_cast<unsigned>(parts[0]);
538 bool value = lexical_cast<unsigned>(parts[1]);
539 loco.funcs &= ~(1<<func);
541 loco.funcs |= 1<<func;
542 funcs_changed |= 1<<func;
544 else if(i->first=="msg")
546 if(i->second=="CONTROL_LOST")
547 command(format("request(%d, control, force)", id));
552 signal_loco_speed.emit(loco.address, loco.speed, loco.reverse);
553 for(unsigned i=0; funcs_changed>>i; ++i)
554 if(funcs_changed&(1<<i))
555 signal_loco_function.emit(loco.address, i, loco.funcs&(1<<i));
557 else if(turnouts.count(id))
559 Turnout &turnout = turnouts[id];
560 bool state_changed = false;
561 for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
565 turnout_addr.erase(turnout.address);
566 turnout.address = lexical_cast<unsigned>(i->second);
567 turnout_addr[turnout.address] = id;
569 else if(i->first=="state")
571 unsigned state = lexical_cast<unsigned>(i->second);
572 unsigned mask = (1<<turnout.bits)-1;
573 turnout.state = (turnout.state&~mask) | (state&mask);
574 state_changed = true;
579 signal_turnout.emit(turnout.address, turnout.state);
581 else if(find(s88.begin(), s88.end(), id)!=s88.end())
584 for(; (base<s88.size() && s88[base]!=id); ++base) ;
586 for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
588 if(i->first=="state")
590 unsigned state = lexical_cast<unsigned>(i->second, "%i");
591 for(unsigned j=0; j<16; ++j)
593 unsigned addr = base*16+j+1;
594 Sensor &sensor = sensors[addr];
595 bool s = state&(1<<j);
599 signal_sensor.emit(addr, sensor.state);
607 CentralStation::Protocol CentralStation::map_protocol(const string &name) const
611 else if(name=="MM-27")
616 throw invalid_argument("CentralStation::map_protocol");
620 unsigned CentralStation::map_address(const map<unsigned, T> &omap, const AddressMap &amap, unsigned addr) const
626 AddressMap::const_iterator i = amap.find(addr);
634 void CentralStation::skip(string::iterator &iter, const string::iterator &end, const string &what) const
636 for(; (iter!=end && what.find(*iter)!=string::npos); ++iter) ;
639 string CentralStation::parse_token(string::iterator &iter, const string::iterator &end, const string &stop) const
645 skip(iter, end, stop);
647 for(; iter!=end; ++iter)
649 if(stop.find(*iter)!=string::npos && parens.empty() && !quote)
651 else if(*iter=='(' || *iter=='[')
652 parens.push_back(*iter);
653 else if((*iter==')' || *iter==']') && !parens.empty())
655 if((*iter==')' && parens.back()!='(') || (*iter==']' && parens.back()!='['))
656 IO::print("Mismatched parentheses\n");
668 CentralStation::Tag CentralStation::parse_tag(string::iterator &iter, const string::iterator &end) const
672 for(; (iter!=end && *iter!='<'); ++iter) ;
676 tag.type = parse_token(++iter, end, " >");
679 string code = parse_token(iter, end, " >");
680 tag.code = lexical_cast<unsigned>(code);
682 skip(iter, end, " ");
683 tag.value = parse_token(iter, end, ">");
691 CentralStation::Message CentralStation::parse_message(string::iterator &iter, const string::iterator &end) const
695 msg.header = parse_tag(iter, end);
699 skip(iter, end, "\r\n");
703 string id = parse_token(iter, end, " \r\n<");
704 Message::AttribMap &attribs = msg.content[lexical_cast<unsigned>(id)];
705 while(iter!=end && *iter!='\n' && *iter!='\r')
707 string attr = parse_token(iter, end, " \r\n<");
708 string::size_type open_bracket = attr.find('[');
709 if(open_bracket!=string::npos)
711 string::size_type close_bracket = attr.rfind(']');
712 string attr_name = attr.substr(0, open_bracket);
713 string attr_value = attr.substr(open_bracket+1, close_bracket-open_bracket-1);
714 attribs.insert(Message::AttribMap::value_type(attr_name, attr_value));
717 attribs.insert(Message::AttribMap::value_type(attr, string()));
721 msg.footer = parse_tag(iter, end);
722 if(msg.footer.type.empty())
729 CentralStation::Tag::Tag():
733 CentralStation::Tag::operator bool() const
735 return !type.empty();
739 CentralStation::Message::operator bool() const
741 return header && footer;
745 CentralStation::Locomotive::Locomotive():
755 CentralStation::Turnout::Turnout():
764 CentralStation::Sensor::Sensor():