source/libr2c2/centralstation.cpp

   1 #include <algorithm>
   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"
  12
  13 using namespace std;
  14 using namespace Msp;
  15
  16 namespace R2C2 {
  17
  18 CentralStation::CentralStation(const Options &opts):
  19         socket(Net::INET),
  20         pending_commands(0),
  21         power(false),
  22         halted(false),
  23         locos_synced(false),
  24         accessories_synced(false),
  25         sensors_synced(false)
  26 {
  27         RefPtr<Net::SockAddr> addr = Net::resolve(opts.get<string>(string())+":15471");
  28         socket.connect(*addr);
  29
  30         IO::print("Connected to central station at %s\n", addr->str());
  31
  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)");
  37 }
  38
  39 CentralStation::~CentralStation()
  40 {
  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))
  47                 while(receive()) ;
  48 }
  49
  50 void CentralStation::set_power(bool p)
  51 {
  52         power = p;
  53         command(format("set(1, %s)", (power ? "go" : "stop")));
  54 }
  55
  56 void CentralStation::halt(bool h)
  57 {
  58         halted = h;
  59         if(halted)
  60         {
  61                 for(LocoMap::iterator i=locos.begin(); i!=locos.end(); ++i)
  62                         if(i->second.speed)
  63                                 set_loco_speed(i->first, 0);
  64         }
  65
  66         signal_halt.emit(halted);
  67 }
  68
  69 const char *CentralStation::enumerate_protocols(unsigned index) const
  70 {
  71         if(index==MM)
  72                 return "MM";
  73         else if(index==MM_27)
  74                 return "MM-27";
  75         else if(index==MFX)
  76                 return "MFX";
  77         else
  78                 return 0;
  79 }
  80
  81 unsigned CentralStation::get_protocol_speed_steps(const string &name) const
  82 {
  83         switch(map_protocol(name))
  84         {
  85         case MM: return 14;
  86         case MM_27: return 27;
  87         case MFX: return 126;
  88         default: return 0;
  89         }
  90 }
  91
  92 unsigned CentralStation::add_loco(unsigned addr, const string &proto_name, const VehicleType &type)
  93 {
  94         Protocol proto = map_protocol(proto_name);
  95
  96         unsigned id = map_address(locos, loco_addr, addr);
  97         if(!id)
  98         {
  99                 Locomotive &loco = locos[addr|0x10000];
 100                 loco.name = type.get_name();
 101                 loco.protocol = proto;
 102                 loco.address = addr;
 103
 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;
 107
 108                 if(locos_synced && proto!=MFX)
 109                         command("create(10)");
 110         }
 111         else
 112                 command(format("request(%d, view, control, force)", id));
 113
 114         return addr;
 115 }
 116
 117 void CentralStation::remove_loco(unsigned addr)
 118 {
 119         unsigned id = map_address(locos, loco_addr, addr);
 120         if(id)
 121                 command(format("release(%d, view, control)", id));
 122 }
 123
 124 void CentralStation::set_loco_speed(unsigned addr, unsigned speed)
 125 {
 126         if(speed && halted)
 127                 return;
 128
 129         unsigned id = map_address(locos, loco_addr, addr);
 130         if(id)
 131         {
 132                 Locomotive &loco = locos[id];
 133                 if(loco.protocol==MFX && speed)
 134                         ++speed;
 135                 command(format("set(%d, speedstep[%d])", id, speed));
 136         }
 137 }
 138
 139 void CentralStation::set_loco_reverse(unsigned addr, bool rev)
 140 {
 141         unsigned id = map_address(locos, loco_addr, addr);
 142         if(id)
 143                 command(format("set(%d, dir[%d])", id, rev));
 144 }
 145
 146 void CentralStation::set_loco_function(unsigned addr, unsigned func, bool state)
 147 {
 148         unsigned id = map_address(locos, loco_addr, addr);
 149         if(id)
 150                 command(format("set(%d, func[%d, %d])", id, func, state));
 151 }
 152
 153 unsigned CentralStation::add_turnout(unsigned addr, const TrackType &type)
 154 {
 155         unsigned straight = type.get_paths();
 156         bool left = false;
 157         bool right = false;
 158         bool cross = false;
 159
 160         const vector<TrackPart> &parts = type.get_parts();
 161         for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
 162         {
 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)
 166                 {
 167                         (end.rotation>start.rotation ? left : right) = true;
 168                         straight &= ~(1<<i->get_path());
 169                 }
 170                 else if(abs(start.rotation).radians()>0.01)
 171                         cross = true;
 172         }
 173
 174         MagnetAccessory::Symbol symbol = MagnetAccessory::TURNOUT_LEFT;
 175         if(cross)
 176                 symbol = MagnetAccessory::TURNOUT_DOUBLESLIP;
 177         else if(left && right)
 178                 symbol = MagnetAccessory::TURNOUT_THREEWAY;
 179         else if(left)
 180                 symbol = (straight ? MagnetAccessory::TURNOUT_LEFT : MagnetAccessory::TURNOUT_CURVED_LEFT);
 181         else if(right)
 182                 symbol = (straight ? MagnetAccessory::TURNOUT_RIGHT : MagnetAccessory::TURNOUT_CURVED_RIGHT);
 183
 184         MagnetAccessory &turnout = add_accessory(addr, MagnetAccessory::TURNOUT, symbol);
 185         turnout.bits = type.get_state_bits();
 186
 187         return addr;
 188 }
 189
 190 void CentralStation::remove_turnout(unsigned addr)
 191 {
 192         remove_accessory(addr);
 193 }
 194
 195 void CentralStation::set_turnout(unsigned addr, unsigned state)
 196 {
 197         set_accessory_state(addr, MagnetAccessory::TURNOUT, state);
 198 }
 199
 200 unsigned CentralStation::get_turnout(unsigned addr) const
 201 {
 202         return get_accessory_state(addr, MagnetAccessory::TURNOUT);
 203 }
 204
 205 unsigned CentralStation::add_signal(unsigned addr, const SignalType &)
 206 {
 207         add_accessory(addr, MagnetAccessory::SIGNAL, MagnetAccessory::SEMAPHORE_HOME);
 208         return addr;
 209 }
 210
 211 void CentralStation::remove_signal(unsigned addr)
 212 {
 213         remove_accessory(addr);
 214 }
 215
 216 void CentralStation::set_signal(unsigned addr, unsigned state)
 217 {
 218         set_accessory_state(addr, MagnetAccessory::SIGNAL, state);
 219 }
 220
 221 unsigned CentralStation::get_signal(unsigned addr) const
 222 {
 223         return get_accessory_state(addr, MagnetAccessory::SIGNAL);
 224 }
 225
 226 CentralStation::MagnetAccessory &CentralStation::add_accessory(unsigned addr, MagnetAccessory::Type type, MagnetAccessory::Symbol symbol)
 227 {
 228         unsigned id = map_address(accessories, accessory_addr, addr);
 229         if(!id)
 230         {
 231                 id = addr|0x10000;
 232
 233                 MagnetAccessory &accessory = accessories[id];
 234                 accessory.address = addr;
 235                 accessory.type = type;
 236                 accessory.symbol = symbol;
 237
 238                 accessory_addr[addr] = id;
 239
 240                 if(accessories_synced)
 241                         command("create(11, append)");
 242
 243                 return accessory;
 244         }
 245         else
 246         {
 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));
 251
 252                 return accessory;
 253         }
 254 }
 255
 256 void CentralStation::remove_accessory(unsigned addr)
 257 {
 258         unsigned id = map_address(accessories, accessory_addr, addr);
 259         if(id)
 260                 command(format("release(%d, view, control)", id));
 261 }
 262
 263 void CentralStation::set_accessory_state(unsigned addr, MagnetAccessory::Type type, unsigned state)
 264 {
 265         unsigned id = map_address(accessories, accessory_addr, addr);
 266         if(id)
 267         {
 268                 MagnetAccessory &accessory = accessories[id];
 269                 if(accessory.type!=type)
 270                         throw logic_error("accessory type conflict");
 271
 272                 unsigned mask = (1<<accessory.bits)-1;
 273
 274                 if(((state^accessory.state)&mask)==0 || !accessory.synced)
 275                 {
 276                         accessory.state = state;
 277                         accessory_state_changed(accessory);
 278                         return;
 279                 }
 280
 281                 accessory.state = (accessory.state&mask) | (state&~mask);
 282
 283                 command(format("set(%d, state[%d])", id, state&mask));
 284         }
 285 }
 286
 287 unsigned CentralStation::get_accessory_state(unsigned addr, MagnetAccessory::Type type) const
 288 {
 289         unsigned id = map_address(accessories, accessory_addr, addr);
 290         if(id)
 291         {
 292                 AccessoryMap::const_iterator i = accessories.find(id);
 293                 if(i!=accessories.end() && i->second.type==type)
 294                         return i->second.state;
 295         }
 296         return 0;
 297 }
 298
 299 void CentralStation::accessory_state_changed(const MagnetAccessory &accessory) const
 300 {
 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);
 305 }
 306
 307 unsigned CentralStation::add_sensor(unsigned addr)
 308 {
 309         sensors.insert(SensorMap::value_type(addr, Sensor()));
 310
 311         if(sensors_synced)
 312         {
 313                 if(addr>s88.size()*16)
 314                         command("create(26, add[0])");
 315         }
 316
 317         return addr;
 318 }
 319
 320 void CentralStation::remove_sensor(unsigned)
 321 {
 322 }
 323
 324 bool CentralStation::get_sensor(unsigned addr) const
 325 {
 326         SensorMap::const_iterator i = sensors.find(addr);
 327         if(i!=sensors.end())
 328                 return i->second.state;
 329         return false;
 330 }
 331
 332 float CentralStation::get_telemetry_value(const string &name) const
 333 {
 334         throw key_error(name);
 335 }
 336
 337 void CentralStation::tick()
 338 {
 339         while(Message msg = receive())
 340         {
 341                 if(msg.footer.code)
 342                         IO::print("\033[31m*** ERROR: %s: %d %s ***\033[0m\n", msg.header.value, msg.footer.code, msg.footer.value);
 343
 344                 if(msg.header.type=="REPLY")
 345                         process_reply(msg);
 346                 else if(msg.header.type=="EVENT")
 347                         process_event(msg);
 348         }
 349 }
 350
 351 void CentralStation::flush()
 352 {
 353 }
 354
 355 void CentralStation::command(const string &cmd, bool force)
 356 {
 357         if(pending_commands<10 || force)
 358         {
 359                 socket.write(cmd+"\r\n");
 360                 ++pending_commands;
 361         }
 362         else
 363                 cmd_queue.push_back(cmd);
 364 }
 365
 366 CentralStation::Message CentralStation::receive()
 367 {
 368         while(IO::poll(socket, IO::P_INPUT, Time::zero))
 369         {
 370                 char rbuf[1024];
 371                 unsigned len = socket.read(rbuf, sizeof(rbuf));
 372                 if(!len)
 373                         return Message();
 374
 375                 in_buffer.append(rbuf, len);
 376         }
 377
 378         if(!in_buffer.empty())
 379         {
 380                 string::iterator iter = in_buffer.begin();
 381                 if(Message msg = parse_message(iter, in_buffer.end()))
 382                 {
 383                         skip(iter, in_buffer.end(), "\r\n");
 384                         in_buffer.erase(in_buffer.begin(), iter);
 385
 386                         if(msg.header.type=="REPLY" && pending_commands>0)
 387                         {
 388                                 --pending_commands;
 389                                 if(!cmd_queue.empty())
 390                                 {
 391                                         command(cmd_queue.front());
 392                                         cmd_queue.pop_front();
 393                                 }
 394                         }
 395
 396                         return msg;
 397                 }
 398         }
 399
 400         return Message();
 401 }
 402
 403 void CentralStation::process_reply(const Message &msg)
 404 {
 405         if(!msg.header.value.compare(0, 4, "get("))
 406         {
 407                 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
 408                 {
 409                         if(accessories.count(i->first))
 410                                 accessories[i->first].synced = true;
 411
 412                         process_object(i->first, i->second);
 413                 }
 414         }
 415         else if(!msg.header.value.compare(0, 16, "queryObjects(10,"))
 416         {
 417                 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
 418                 {
 419                         LocoMap::iterator j = locos.find(i->first);
 420                         if(j==locos.end())
 421                         {
 422                                 bool found = false;
 423                                 Message::AttribMap::const_iterator k = i->second.find("addr");
 424                                 if(k!=i->second.end())
 425                                 {
 426                                         unsigned addr = lexical_cast<unsigned>(k->second);
 427
 428                                         j = locos.find(addr|0x10000);
 429                                         if(j!=locos.end())
 430                                         {
 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);
 436                                                 cmd += ')';
 437                                                 command(cmd);
 438
 439                                                 locos.insert(LocoMap::value_type(i->first, j->second));
 440                                                 locos.erase(j);
 441
 442                                                 found = true;
 443                                         }
 444                                 }
 445
 446                                 if(!found)
 447                                         locos.insert(LocoMap::value_type(i->first, Locomotive()));
 448                         }
 449
 450                         process_object(i->first, i->second);
 451                 }
 452
 453                 locos_synced = true;
 454
 455                 if(locos.lower_bound(0x10000)!=locos.end())
 456                         command("create(10)");
 457         }
 458         else if(!msg.header.value.compare(0, 16, "queryObjects(11,"))
 459         {
 460                 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
 461                 {
 462                         AccessoryMap::iterator j = accessories.find(i->first);
 463                         if(j==accessories.end())
 464                         {
 465                                 bool found = false;
 466                                 Message::AttribMap::const_iterator k = i->second.find("addr");
 467                                 if(k!=i->second.end())
 468                                 {
 469                                         unsigned addr = lexical_cast<unsigned>(k->second);
 470
 471                                         j = accessories.find(addr|0x10000);
 472                                         if(j!=accessories.end())
 473                                         {
 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));
 477
 478                                                 accessories.insert(AccessoryMap::value_type(i->first, j->second));
 479                                                 accessories.erase(j);
 480
 481                                                 found = true;
 482                                         }
 483                                 }
 484
 485                                 if(!found)
 486                                         accessories.insert(AccessoryMap::value_type(i->first, MagnetAccessory()));
 487                         }
 488
 489                         process_object(i->first, i->second);
 490                 }
 491
 492                 accessories_synced = true;
 493
 494                 for(AccessoryMap::const_iterator i=accessories.lower_bound(0x10000); i!=accessories.end(); ++i)
 495                         command("create(11, append)");
 496         }
 497         else if(msg.header.value=="queryObjects(26)")
 498         {
 499                 s88.clear();
 500                 for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
 501                 {
 502                         s88.push_back(i->first);
 503                         command(format("request(%d, view)", i->first));
 504                         command(format("get(%d, state)", i->first));
 505                 }
 506
 507                 sensors_synced = true;
 508
 509                 if(!sensors.empty())
 510                 {
 511                         unsigned high_addr = (--sensors.end())->first;
 512                         if(high_addr>16*s88.size())
 513                                 command("create(26, add[0])");
 514                 }
 515         }
 516         else if(msg.header.value=="create(10)")
 517         {
 518                 Message::ObjectMap::const_iterator i = msg.content.find(10);
 519                 if(i!=msg.content.end())
 520                 {
 521                         Message::AttribMap::const_iterator j = i->second.find("id");
 522                         if(j!=i->second.end())
 523                         {
 524                                 unsigned id = lexical_cast<unsigned>(j->second);
 525                                 LocoMap::iterator k = locos.lower_bound(0x10000);
 526                                 if(k!=locos.end())
 527                                 {
 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)");
 532
 533                                         locos.insert(LocoMap::value_type(id, k->second));
 534                                         locos.erase(k);
 535                                 }
 536                         }
 537                 }
 538
 539                 if(locos.lower_bound(0x10000)!=locos.end())
 540                         command("create(10)");
 541         }
 542         else if(!msg.header.value.compare(0, 10, "create(11,"))
 543         {
 544                 Message::ObjectMap::const_iterator i = msg.content.find(11);
 545                 if(i!=msg.content.end())
 546                 {
 547                         Message::AttribMap::const_iterator j = i->second.find("id");
 548                         if(j!=i->second.end())
 549                         {
 550                                 unsigned id = lexical_cast<unsigned>(j->second);
 551                                 AccessoryMap::iterator k = accessories.lower_bound(0x10000);
 552                                 if(k!=accessories.end())
 553                                 {
 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)));
 559
 560                                         k->second.synced = true;
 561                                         accessories.insert(AccessoryMap::value_type(id, k->second));
 562                                         accessories.erase(k);
 563                                 }
 564                         }
 565                 }
 566         }
 567         else if(!msg.header.value.compare(0, 10, "create(26,"))
 568                 command("queryObjects(26)");
 569 }
 570
 571 void CentralStation::process_event(const Message &msg)
 572 {
 573         for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
 574                 process_object(i->first, i->second);
 575 }
 576
 577 void CentralStation::process_object(unsigned id, const Message::AttribMap &attribs)
 578 {
 579         if(id==1)
 580         {
 581                 for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
 582                         if(i->first=="status")
 583                         {
 584                                 power = (i->second=="GO");
 585                                 signal_power.emit(power);
 586                         }
 587         }
 588         else if(locos.count(id))
 589         {
 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)
 594                 {
 595                         if(i->first=="name")
 596                                 loco.name = i->second.substr(1, i->second.size()-2);
 597                         else if(i->first=="addr")
 598                         {
 599                                 loco_addr.erase(loco.address);
 600                                 loco.address = lexical_cast<unsigned>(i->second);
 601                                 loco_addr[loco.address] = id;
 602                         }
 603                         else if(i->first=="protocol")
 604                         {
 605                                 if(i->second=="MM")
 606                                         loco.protocol = MM;
 607                                 else if(i->second=="MM27")
 608                                         loco.protocol = MM_27;
 609                                 else if(i->second=="MFX")
 610                                         loco.protocol = MFX;
 611                         }
 612                         else if(i->first=="speedstep")
 613                         {
 614                                 loco.speed = lexical_cast<unsigned>(i->second);
 615                                 if(loco.protocol==MFX && loco.speed)
 616                                         --loco.speed;
 617                                 speed_changed = true;
 618                         }
 619                         else if(i->first=="dir")
 620                         {
 621                                 loco.reverse = i->second[0]!='0';
 622                                 speed_changed = true;
 623                         }
 624                         else if(i->first=="func")
 625                         {
 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);
 630                                 if(value)
 631                                         loco.funcs |= 1<<func;
 632                                 funcs_changed |= 1<<func;
 633                         }
 634                         else if(i->first=="msg")
 635                         {
 636                                 if(i->second=="CONTROL_LOST")
 637                                         command(format("request(%d, control, force)", id));
 638                         }
 639                 }
 640
 641                 if(speed_changed)
 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));
 646         }
 647         else if(accessories.count(id))
 648         {
 649                 MagnetAccessory &accessory = accessories[id];
 650                 bool state_changed = false;
 651                 for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
 652                 {
 653                         if(i->first=="addr")
 654                         {
 655                                 accessory_addr.erase(accessory.address);
 656                                 accessory.address = lexical_cast<unsigned>(i->second);
 657                                 accessory_addr[accessory.address] = id;
 658                         }
 659                         else if(i->first=="state")
 660                         {
 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;
 665                         }
 666                 }
 667
 668                 if(state_changed)
 669                         accessory_state_changed(accessory);
 670         }
 671         else if(find(s88.begin(), s88.end(), id)!=s88.end())
 672         {
 673                 unsigned base = 0;
 674                 for(; (base<s88.size() && s88[base]!=id); ++base) ;
 675
 676                 for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
 677                 {
 678                         if(i->first=="state")
 679                         {
 680                                 unsigned state = lexical_cast<unsigned>(i->second, "%i");
 681                                 for(unsigned j=0; j<16; ++j)
 682                                 {
 683                                         unsigned addr = base*16+j+1;
 684                                         Sensor &sensor = sensors[addr];
 685                                         bool s = state&(1<<j);
 686                                         if(s!=sensor.state)
 687                                         {
 688                                                 sensor.state = s;
 689                                                 signal_sensor.emit(addr, sensor.state);
 690                                         }
 691                                 }
 692                         }
 693                 }
 694         }
 695 }
 696
 697 CentralStation::Protocol CentralStation::map_protocol(const string &name) const
 698 {
 699         if(name=="MM")
 700                 return MM;
 701         else if(name=="MM-27")
 702                 return MM_27;
 703         else if(name=="MFX")
 704                 return MFX;
 705         else
 706                 throw invalid_argument("CentralStation::map_protocol");
 707 }
 708
 709 template<typename T>
 710 unsigned CentralStation::map_address(const map<unsigned, T> &omap, const AddressMap &amap, unsigned addr) const
 711 {
 712         if(omap.count(addr))
 713                 return addr;
 714         else
 715         {
 716                 AddressMap::const_iterator i = amap.find(addr);
 717                 if(i!=amap.end())
 718                         return i->second;
 719                 else
 720                         return 0;
 721         }
 722 }
 723
 724 void CentralStation::skip(string::iterator &iter, const string::iterator &end, const string &what) const
 725 {
 726         for(; (iter!=end && what.find(*iter)!=string::npos); ++iter) ;
 727 }
 728
 729 string CentralStation::parse_token(string::iterator &iter, const string::iterator &end, const string &stop) const
 730 {
 731         vector<char> parens;
 732         bool quote = false;
 733         string token;
 734
 735         skip(iter, end, stop);
 736
 737         for(; iter!=end; ++iter)
 738         {
 739                 if(stop.find(*iter)!=string::npos && parens.empty() && !quote)
 740                         break;
 741                 else if(*iter=='(' || *iter=='[')
 742                         parens.push_back(*iter);
 743                 else if((*iter==')' || *iter==']') && !parens.empty())
 744                 {
 745                         if((*iter==')' && parens.back()!='(') || (*iter==']' && parens.back()!='['))
 746                                 IO::print("Mismatched parentheses\n");
 747                         parens.pop_back();
 748                 }
 749                 else if(*iter=='"')
 750                         quote = !quote;
 751
 752                 token += *iter;
 753         }
 754
 755         return token;
 756 }
 757
 758 CentralStation::Tag CentralStation::parse_tag(string::iterator &iter, const string::iterator &end) const
 759 {
 760         Tag tag;
 761
 762         for(; (iter!=end && *iter!='<'); ++iter) ;
 763         if(iter==end)
 764                 return Tag();
 765
 766         tag.type = parse_token(++iter, end, " >");
 767         if(tag.type=="END")
 768         {
 769                 string code = parse_token(iter, end, " >");
 770                 tag.code = lexical_cast<unsigned>(code);
 771         }
 772         skip(iter, end, " ");
 773         tag.value = parse_token(iter, end, ">");
 774         if(iter==end)
 775                 return Tag();
 776         ++iter;
 777
 778         return tag;
 779 }
 780
 781 CentralStation::Message CentralStation::parse_message(string::iterator &iter, const string::iterator &end) const
 782 {
 783         Message msg;
 784
 785         msg.header = parse_tag(iter, end);
 786
 787         while(iter!=end)
 788         {
 789                 skip(iter, end, "\r\n");
 790                 if(*iter=='<')
 791                         break;
 792
 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')
 796                 {
 797                         string attr = parse_token(iter, end, " \r\n<");
 798                         string::size_type open_bracket = attr.find('[');
 799                         if(open_bracket!=string::npos)
 800                         {
 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));
 805                         }
 806                         else
 807                                 attribs.insert(Message::AttribMap::value_type(attr, string()));
 808                 }
 809         }
 810
 811         msg.footer = parse_tag(iter, end);
 812         if(msg.footer.type.empty())
 813                 return Message();
 814
 815         return msg;
 816 }
 817
 818
 819 CentralStation::Tag::Tag():
 820         code(0)
 821 { }
 822
 823 CentralStation::Tag::operator bool() const
 824 {
 825         return !type.empty();
 826 }
 827
 828
 829 CentralStation::Message::operator bool() const
 830 {
 831         return header && footer;
 832 }
 833
 834
 835 CentralStation::Locomotive::Locomotive():
 836         address(0),
 837         speed(0),
 838         reverse(false),
 839         func_mask(0),
 840         funcs(0),
 841         control(false)
 842 { }
 843
 844
 845 CentralStation::MagnetAccessory::MagnetAccessory():
 846         address(0),
 847         type(TURNOUT),
 848         symbol(TURNOUT_LEFT),
 849         state(0),
 850         bits(1),
 851         synced(false)
 852 { }
 853
 854
 855 CentralStation::Sensor::Sensor():
 856         state(false)
 857 { }
 858
 859 } // namespace R2C2