source/libr2c2/centralstation.cpp

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