3 This file is part of R²C²
4 Copyright © 2010 Mikkosoft Productions, Mikko Rasa
5 Distributed under the GPL
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"
24 CentralStation::CentralStation(const string &host):
30 turnouts_synced(false),
33 RefPtr<Net::SockAddr> addr = Net::resolve(host+":15471");
34 socket.connect(*addr);
36 IO::print("Connected to central station at %s\n", addr->str());
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)");
45 CentralStation::~CentralStation()
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))
56 void CentralStation::set_power(bool p)
59 command(format("set(1, %s)", (power ? "go" : "stop")));
62 void CentralStation::halt(bool h)
67 for(LocoMap::iterator i=locos.begin(); i!=locos.end(); ++i)
69 set_loco_speed(i->first, 0);
72 signal_halt.emit(halted);
75 const char *CentralStation::enumerate_protocols(unsigned index) const
87 unsigned CentralStation::get_protocol_speed_steps(const string &name) const
89 switch(map_protocol(name))
92 case MM_27: return 27;
98 void CentralStation::add_loco(unsigned addr, const string &proto_name, const VehicleType &type)
100 Protocol proto = map_protocol(proto_name);
102 unsigned id = map_address(locos, loco_addr, addr);
105 Locomotive &loco = locos[addr|0x10000];
106 loco.name = type.get_name();
107 loco.protocol = proto;
110 if(locos_synced && proto!=MFX)
111 command("create(10)");
114 command(format("request(%d, view, control, force)", id));
117 void CentralStation::set_loco_speed(unsigned addr, unsigned speed)
122 unsigned id = map_address(locos, loco_addr, addr);
125 Locomotive &loco = locos[id];
126 if(loco.protocol==MFX && speed)
128 command(format("set(%d, speedstep[%d])", id, speed));
132 void CentralStation::set_loco_reverse(unsigned addr, bool rev)
134 unsigned id = map_address(locos, loco_addr, addr);
136 command(format("set(%d, dir[%d])", id, rev));
139 void CentralStation::set_loco_function(unsigned addr, unsigned func, bool state)
141 unsigned id = map_address(locos, loco_addr, addr);
143 command(format("set(%d, func[%d, %d])", id, func, state));
146 void CentralStation::add_turnout(unsigned addr, const TrackType &type)
152 const vector<TrackPart> &parts = type.get_parts();
153 for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
155 TrackPoint start = i->get_point(0);
156 TrackPoint end = i->get_point(i->get_length());
157 if(end.dir>start.dir+0.01)
159 else if(end.dir<start.dir-0.01)
161 else if(start.dir<-0.01 || start.dir>0.01)
165 unsigned symbol = Turnout::LEFT;
167 symbol = Turnout::DOUBLESLIP;
168 else if(left && right)
169 symbol = Turnout::THREEWAY;
171 symbol = Turnout::LEFT;
173 symbol = Turnout::RIGHT;
175 unsigned id = map_address(turnouts, turnout_addr, addr);
180 Turnout &turnout = turnouts[id];
181 turnout.address = addr;
182 turnout.bits = type.get_state_bits();
183 turnout.symbol = symbol;
185 turnout_addr[addr] = id;
188 command("create(11, append)");
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));
199 void CentralStation::set_turnout(unsigned addr, unsigned state)
201 unsigned id = map_address(turnouts, turnout_addr, addr);
204 Turnout &turnout = turnouts[id];
205 unsigned mask = (1<<turnout.bits)-1;
207 if(((state^turnout.state)&mask)==0 || !turnout.synced)
209 turnout.state = state;
210 signal_turnout.emit(addr, turnout.state);
214 turnout.state = (turnout.state&mask) | (state&~mask);
216 command(format("set(%d, state[%d])", id, state&mask));
220 unsigned CentralStation::get_turnout(unsigned addr) const
222 unsigned id = map_address(turnouts, turnout_addr, addr);
225 TurnoutMap::const_iterator i = turnouts.find(id);
226 if(i!=turnouts.end())
227 return i->second.state;
232 void CentralStation::add_sensor(unsigned addr)
234 sensors.insert(SensorMap::value_type(addr, Sensor()));
238 if(addr>s88.size()*16)
239 command("create(26, add[0])");
243 bool CentralStation::get_sensor(unsigned addr) const
245 SensorMap::const_iterator i = sensors.find(addr);
247 return i->second.state;
251 void CentralStation::tick()
253 Time::TimeStamp t = Time::now();
254 for(SensorMap::iterator i=sensors.begin(); i!=sensors.end(); ++i)
255 if(i->second.off_timeout && t>i->second.off_timeout)
257 i->second.state = false;
258 i->second.off_timeout = Time::TimeStamp();
259 signal_sensor.emit(i->first, i->second.state);
262 while(Message msg = receive())
265 IO::print("\033[31m*** ERROR: %s: %d %s ***\033[0m\n", msg.header.value, msg.footer.code, msg.footer.value);
267 if(msg.header.type=="REPLY")
269 else if(msg.header.type=="EVENT")
274 void CentralStation::flush()
278 void CentralStation::command(const string &cmd, bool force)
280 if(pending_commands<10 || force)
282 socket.write(cmd+"\r\n");
286 cmd_queue.push_back(cmd);
289 CentralStation::Message CentralStation::receive()
291 while(IO::poll(socket, IO::P_INPUT, Time::zero))
294 unsigned len = socket.read(rbuf, sizeof(rbuf));
298 in_buffer.append(rbuf, len);
301 if(!in_buffer.empty())
303 string::iterator iter = in_buffer.begin();
304 if(Message msg = parse_message(iter, in_buffer.end()))
306 skip(iter, in_buffer.end(), "\r\n");
307 in_buffer.erase(in_buffer.begin(), iter);
309 if(msg.header.type=="REPLY" && pending_commands>0)
312 if(!cmd_queue.empty())
314 command(cmd_queue.front());
315 cmd_queue.pop_front();
326 void CentralStation::process_reply(const Message &msg)
328 if(!msg.header.value.compare(0, 4, "get("))
330 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
332 if(turnouts.count(i->first))
333 turnouts[i->first].synced = true;
335 process_object(i->first, i->second);
338 else if(!msg.header.value.compare(0, 16, "queryObjects(10,"))
340 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
342 LocoMap::iterator j = locos.find(i->first);
346 Message::AttribMap::const_iterator k = i->second.find("addr");
347 if(k!=i->second.end())
349 unsigned addr = lexical_cast<unsigned>(k->second);
351 j = locos.find(addr|0x10000);
354 command(format("request(%d, view, control, force)", i->first));
355 command(format("get(%d, dir, func[0])", i->first));
357 locos.insert(LocoMap::value_type(i->first, j->second));
365 locos.insert(LocoMap::value_type(i->first, Locomotive()));
368 process_object(i->first, i->second);
373 if(locos.lower_bound(0x10000)!=locos.end())
374 command("create(10)");
376 else if(!msg.header.value.compare(0, 16, "queryObjects(11,"))
378 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
380 TurnoutMap::iterator j = turnouts.find(i->first);
381 if(j==turnouts.end())
384 Message::AttribMap::const_iterator k = i->second.find("addr");
385 if(k!=i->second.end())
387 unsigned addr = lexical_cast<unsigned>(k->second);
389 j = turnouts.find(addr|0x10000);
390 if(j!=turnouts.end())
392 command(format("request(%d, view, control)", i->first));
393 command(format("set(%d, symbol[%d])", i->first, j->second.symbol));
394 command(format("get(%d, state)", i->first));
396 turnouts.insert(TurnoutMap::value_type(i->first, j->second));
404 turnouts.insert(TurnoutMap::value_type(i->first, Turnout()));
407 process_object(i->first, i->second);
410 turnouts_synced = true;
412 for(TurnoutMap::const_iterator i=turnouts.lower_bound(0x10000); i!=turnouts.end(); ++i)
413 command("create(11, append)");
415 else if(msg.header.value=="queryObjects(26)")
418 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
420 s88.push_back(i->first);
421 command(format("request(%d, view)", i->first));
424 sensors_synced = true;
428 unsigned high_addr = (--sensors.end())->first;
429 if(high_addr>16*s88.size())
430 command("create(26, add[0])");
433 else if(msg.header.value=="create(10)")
435 Message::ObjectMap::const_iterator i = msg.content.find(10);
436 if(i!=msg.content.end())
438 Message::AttribMap::const_iterator j = i->second.find("id");
439 if(j!=i->second.end())
441 unsigned id = lexical_cast<unsigned>(j->second);
442 LocoMap::iterator k = locos.lower_bound(0x10000);
445 command(format("request(%d, view, control)", id));
446 command(format("set(%d, addr[%d], protocol[%s], name[\"%s\"])",
447 id, k->second.address, (k->second.protocol==MM_27 ? "MM27" : "MM14"), k->second.name));
448 command("create(10, append)");
450 locos.insert(LocoMap::value_type(id, k->second));
456 if(locos.lower_bound(0x10000)!=locos.end())
457 command("create(10)");
459 else if(!msg.header.value.compare(0, 10, "create(11,"))
461 Message::ObjectMap::const_iterator i = msg.content.find(11);
462 if(i!=msg.content.end())
464 Message::AttribMap::const_iterator j = i->second.find("id");
465 if(j!=i->second.end())
467 unsigned id = lexical_cast<unsigned>(j->second);
468 TurnoutMap::iterator k = turnouts.lower_bound(0x10000);
469 if(k!=turnouts.end())
471 command(format("request(%d, view, control)", id));
472 command(format("set(%d, addr[%d], symbol[%d], name1[\"Switch\"], name2[\"%d\"], name3[\"\"])",
473 id, k->second.address, k->second.symbol, k->second.address));
474 command(format("set(%d, state[%d])", id, k->second.state&((1<<k->second.bits)-1)));
476 k->second.synced = true;
477 turnouts.insert(TurnoutMap::value_type(id, k->second));
483 else if(!msg.header.value.compare(0, 10, "create(26,"))
484 command("queryObjects(26)");
487 void CentralStation::process_event(const Message &msg)
489 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
490 process_object(i->first, i->second);
493 void CentralStation::process_object(unsigned id, const Message::AttribMap &attribs)
497 for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
498 if(i->first=="status")
500 power = (i->second=="GO");
501 signal_power.emit(power);
504 else if(locos.count(id))
506 Locomotive &loco = locos[id];
507 bool speed_changed = false;
508 unsigned funcs_changed = 0;
509 for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
512 loco.name = i->second.substr(1, i->second.size()-2);
513 else if(i->first=="addr")
515 loco_addr.erase(loco.address);
516 loco.address = lexical_cast<unsigned>(i->second);
517 loco_addr[loco.address] = id;
519 else if(i->first=="protocol")
523 else if(i->second=="MM27")
524 loco.protocol = MM_27;
525 else if(i->second=="MFX")
528 else if(i->first=="speedstep")
530 loco.speed = lexical_cast<unsigned>(i->second);
531 if(loco.protocol==MFX && loco.speed)
533 speed_changed = true;
535 else if(i->first=="dir")
537 loco.reverse = i->second[0]!='0';
538 speed_changed = true;
540 else if(i->first=="func")
542 vector<string> parts = split(i->second, ", ");
543 unsigned func = lexical_cast<unsigned>(parts[0]);
544 bool value = lexical_cast<unsigned>(parts[1]);
545 loco.funcs &= ~(1<<func);
547 loco.funcs |= 1<<func;
548 funcs_changed |= 1<<func;
550 else if(i->first=="msg")
552 if(i->second=="CONTROL_LOST")
553 command(format("request(%d, control, force)", id));
558 signal_loco_speed.emit(loco.address, loco.speed, loco.reverse);
559 for(unsigned i=0; funcs_changed>>i; ++i)
560 if(funcs_changed&(1<<i))
561 signal_loco_function.emit(loco.address, i, loco.funcs&(1<<i));
563 else if(turnouts.count(id))
565 Turnout &turnout = turnouts[id];
566 bool state_changed = false;
567 for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
571 turnout_addr.erase(turnout.address);
572 turnout.address = lexical_cast<unsigned>(i->second);
573 turnout_addr[turnout.address] = id;
575 else if(i->first=="state")
577 unsigned state = lexical_cast<unsigned>(i->second);
578 unsigned mask = (1<<turnout.bits)-1;
579 turnout.state = (turnout.state&~mask) | (state&mask);
580 state_changed = true;
585 signal_turnout.emit(turnout.address, turnout.state);
587 else if(find(s88.begin(), s88.end(), id)!=s88.end())
590 for(; (base<s88.size() && s88[base]!=id); ++base) ;
592 for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
594 if(i->first=="state")
596 unsigned state = lexical_cast<unsigned>(i->second, "%i");
597 for(unsigned j=0; j<16; ++j)
599 unsigned addr = base*16+j+1;
600 Sensor &sensor = sensors[addr];
601 bool s = state&(1<<j);
604 sensor.off_timeout = Time::TimeStamp();
608 signal_sensor.emit(addr, sensor.state);
611 else if(sensor.state)
612 sensor.off_timeout = Time::now()+700*Time::msec;
619 CentralStation::Protocol CentralStation::map_protocol(const string &name) const
623 else if(name=="MM-27")
628 throw InvalidParameterValue("Unknown protocol");
632 unsigned CentralStation::map_address(const map<unsigned, T> &omap, const AddressMap &amap, unsigned addr) const
638 AddressMap::const_iterator i = amap.find(addr);
646 void CentralStation::skip(string::iterator &iter, const string::iterator &end, const string &what) const
648 for(; (iter!=end && what.find(*iter)!=string::npos); ++iter) ;
651 string CentralStation::parse_token(string::iterator &iter, const string::iterator &end, const string &stop) const
657 skip(iter, end, stop);
659 for(; iter!=end; ++iter)
661 if(stop.find(*iter)!=string::npos && parens.empty() && !quote)
663 else if(*iter=='(' || *iter=='[')
664 parens.push_back(*iter);
665 else if((*iter==')' || *iter==']') && !parens.empty())
667 if((*iter==')' && parens.back()!='(') || (*iter==']' && parens.back()!='['))
668 IO::print("Mismatched parentheses\n");
680 CentralStation::Tag CentralStation::parse_tag(string::iterator &iter, const string::iterator &end) const
684 for(; (iter!=end && *iter!='<'); ++iter) ;
688 tag.type = parse_token(++iter, end, " >");
691 string code = parse_token(iter, end, " >");
692 tag.code = lexical_cast<unsigned>(code);
694 skip(iter, end, " ");
695 tag.value = parse_token(iter, end, ">");
703 CentralStation::Message CentralStation::parse_message(string::iterator &iter, const string::iterator &end) const
707 msg.header = parse_tag(iter, end);
711 skip(iter, end, "\r\n");
715 string id = parse_token(iter, end, " \r\n<");
716 Message::AttribMap &attribs = msg.content[lexical_cast<unsigned>(id)];
717 while(iter!=end && *iter!='\n' && *iter!='\r')
719 string attr = parse_token(iter, end, " \r\n<");
720 string::size_type open_bracket = attr.find('[');
721 if(open_bracket!=string::npos)
723 string::size_type close_bracket = attr.rfind(']');
724 attribs[attr.substr(0, open_bracket)] = attr.substr(open_bracket+1, close_bracket-open_bracket-1);
731 msg.footer = parse_tag(iter, end);
732 if(msg.footer.type.empty())
739 CentralStation::Tag::Tag():
743 CentralStation::Tag::operator bool() const
745 return !type.empty();
749 CentralStation::Message::operator bool() const
751 return header && footer;
755 CentralStation::Locomotive::Locomotive():
764 CentralStation::Turnout::Turnout():
773 CentralStation::Sensor::Sensor():