X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibr2c2%2Fcentralstation.cpp;fp=source%2Flibr2c2%2Fcentralstation.cpp;h=69bb8a9cf48c78745bf882f764cf0c7270087152;hb=e61a3ee0847b957152d98ba5945e42325821c357;hp=0000000000000000000000000000000000000000;hpb=9a4aeac8f8278eea6cbd67addfb76ace8c937e3c;p=r2c2.git

diff --git a/source/libr2c2/centralstation.cpp b/source/libr2c2/centralstation.cpp
new file mode 100644
index 0000000..69bb8a9
--- /dev/null
+++ b/source/libr2c2/centralstation.cpp
@@ -0,0 +1,770 @@
+/* $Id$
+
+This file is part of R²C²
+Copyright © 2010  Mikkosoft Productions, Mikko Rasa
+Distributed under the GPL
+*/
+
+#include <algorithm>
+#include <msp/core/refptr.h>
+#include <msp/io/print.h>
+#include <msp/net/resolve.h>
+#include <msp/strings/utils.h>
+#include <msp/time/units.h>
+#include <msp/time/utils.h>
+#include "centralstation.h"
+#include "tracktype.h"
+#include "vehicletype.h"
+
+using namespace std;
+using namespace Msp;
+
+namespace R2C2 {
+
+CentralStation::CentralStation(const string &host):
+	socket(Net::INET),
+	pending_commands(0),
+	power(false),
+	halted(false),
+	locos_synced(false),
+	turnouts_synced(false),
+	sensors_synced(false)
+{
+	RefPtr<Net::SockAddr> addr = Net::resolve(host+":15471");
+	socket.connect(*addr);
+
+	IO::print("Connected to central station at %s\n", addr->str());
+
+	command("get(1, status)");
+	command("request(1, view)");
+	command("queryObjects(10, addr, name)");
+	command("queryObjects(11, addr)");
+	command("queryObjects(26)");
+}
+
+CentralStation::~CentralStation()
+{
+	command("release(1, view)", true);
+	for(LocoMap::iterator i=locos.begin(); (i!=locos.end() && !(i->first&0x10000)); ++i)
+		command(format("release(%d, view, control)", i->first));
+	for(TurnoutMap::iterator i=turnouts.begin(); (i!=turnouts.end() && !(i->first&0x10000)); ++i)
+		command(format("release(%d, view, control)", i->first));
+	while(IO::poll(socket, IO::P_INPUT, 100*Time::msec))
+		while(receive()) ;
+}
+
+void CentralStation::set_power(bool p)
+{
+	power = p;
+	command(format("set(1, %s)", (power ? "go" : "stop")));
+}
+
+void CentralStation::halt(bool h)
+{
+	halted = h;
+	if(halted)
+	{
+		for(LocoMap::iterator i=locos.begin(); i!=locos.end(); ++i)
+			if(i->second.speed)
+				set_loco_speed(i->first, 0);
+	}
+
+	signal_halt.emit(halted);
+}
+
+const char *CentralStation::enumerate_protocols(unsigned index) const
+{
+	if(index==MM)
+		return "MM";
+	else if(index==MM_27)
+		return "MM-27";
+	else if(index==MFX)
+		return "MFX";
+	else
+		return 0;
+}
+
+unsigned CentralStation::get_protocol_speed_steps(const string &name) const
+{
+	switch(map_protocol(name))
+	{
+	case MM: return 14;
+	case MM_27: return 27;
+	case MFX: return 127;
+	default: return 0;
+	}
+}
+
+void CentralStation::add_loco(unsigned addr, const string &proto_name, const VehicleType &type)
+{
+	Protocol proto = map_protocol(proto_name);
+
+	unsigned id = map_address(locos, loco_addr, addr);
+	if(!id)
+	{
+		Locomotive &loco = locos[addr|0x10000];
+		loco.name = type.get_name();
+		loco.protocol = proto;
+		loco.address = addr;
+
+		if(locos_synced && proto!=MFX)
+			command("create(10)");
+	}
+	else
+		command(format("request(%d, view, control, force)", id));
+}
+
+void CentralStation::set_loco_speed(unsigned addr, unsigned speed)
+{
+	if(speed && halted)
+		return;
+
+	unsigned id = map_address(locos, loco_addr, addr);
+	if(id)
+		command(format("set(%d, speedstep[%d])", id, speed));
+}
+
+void CentralStation::set_loco_reverse(unsigned addr, bool rev)
+{
+	unsigned id = map_address(locos, loco_addr, addr);
+	if(id)
+		command(format("set(%d, dir[%d])", id, rev));
+}
+
+void CentralStation::set_loco_function(unsigned addr, unsigned func, bool state)
+{
+	unsigned id = map_address(locos, loco_addr, addr);
+	if(id)
+		command(format("set(%d, func[%d, %d])", id, func, state));
+}
+
+void CentralStation::add_turnout(unsigned addr, const TrackType &type)
+{
+	bool left = false;
+	bool right = false;
+	bool cross = false;
+
+	const vector<TrackPart> &parts = type.get_parts();
+	for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
+	{
+		TrackPoint start = i->get_point(0);
+		TrackPoint end = i->get_point(i->get_length());
+		if(end.dir>start.dir+0.01)
+			left = true;
+		else if(end.dir<start.dir-0.01)
+			right = true;
+		else if(start.dir<-0.01 || start.dir>0.01)
+			cross = true;
+	}
+
+	unsigned symbol;
+	if(cross)
+		symbol = Turnout::DOUBLESLIP;
+	else if(left && right)
+		symbol = Turnout::THREEWAY;
+	else if(left)
+		symbol = Turnout::LEFT;
+	else if(right)
+		symbol = Turnout::RIGHT;
+
+	unsigned id = map_address(turnouts, turnout_addr, addr);
+	if(!id)
+	{
+		id = addr|0x10000;
+
+		Turnout &turnout = turnouts[id];
+		turnout.address = addr;
+		turnout.bits = type.get_state_bits();
+		turnout.symbol = symbol;
+
+		turnout_addr[addr] = id;
+
+		if(turnouts_synced)
+			command("create(11, append)");
+	}
+	else
+	{
+		Turnout &turnout = turnouts[id];
+		command(format("request(%d, view, control)", id));
+		if(turnout.symbol!=symbol)
+			command(format("set(%d, symbol[%d])", symbol));
+	}
+}
+
+void CentralStation::set_turnout(unsigned addr, unsigned state)
+{
+	unsigned id = map_address(turnouts, turnout_addr, addr);
+	if(id)
+	{
+		Turnout &turnout = turnouts[id];
+		unsigned mask = (1<<turnout.bits)-1;
+
+		if(((state^turnout.state)&mask)==0 || !turnout.synced)
+		{
+			turnout.state = state;
+			signal_turnout.emit(addr, turnout.state);
+			return;
+		}
+
+		turnout.state = (turnout.state&mask) | (state&~mask);
+
+		command(format("set(%d, state[%d])", id, state&mask));
+	}
+}
+
+unsigned CentralStation::get_turnout(unsigned addr) const
+{
+	unsigned id = map_address(turnouts, turnout_addr, addr);
+	if(id)
+	{
+		TurnoutMap::const_iterator i = turnouts.find(id);
+		if(i!=turnouts.end())
+			return i->second.state;
+	}
+	return 0;
+}
+
+void CentralStation::add_sensor(unsigned addr)
+{
+	sensors.insert(SensorMap::value_type(addr, Sensor()));
+
+	if(sensors_synced)
+	{
+		if(addr>s88.size()*16)
+			command("create(26, add[0])");
+	}
+}
+
+bool CentralStation::get_sensor(unsigned addr) const
+{
+	SensorMap::const_iterator i = sensors.find(addr);
+	if(i!=sensors.end())
+		return i->second.state;
+	return false;
+}
+
+void CentralStation::tick()
+{
+	Time::TimeStamp t = Time::now();
+	for(SensorMap::iterator i=sensors.begin(); i!=sensors.end(); ++i)
+		if(i->second.off_timeout && t>i->second.off_timeout)
+		{
+			i->second.state = false;
+			i->second.off_timeout = Time::TimeStamp();
+			signal_sensor.emit(i->first, i->second.state);
+		}
+
+	while(Message msg = receive())
+	{
+		if(msg.footer.code)
+			IO::print("\033[31m*** ERROR: %s: %d %s ***\033[0m\n", msg.header.value, msg.footer.code, msg.footer.value);
+
+		if(msg.header.type=="REPLY")
+			process_reply(msg);
+		else if(msg.header.type=="EVENT")
+			process_event(msg);
+	}
+}
+
+void CentralStation::flush()
+{
+}
+
+void CentralStation::command(const string &cmd, bool force)
+{
+	if(pending_commands<10 || force)
+	{
+		socket.write(cmd+"\r\n");
+		++pending_commands;
+	}
+	else
+		cmd_queue.push_back(cmd);
+}
+
+CentralStation::Message CentralStation::receive()
+{
+	while(IO::poll(socket, IO::P_INPUT, Time::zero))
+	{
+		char rbuf[1024];
+		unsigned len = socket.read(rbuf, sizeof(rbuf));
+		if(!len)
+			return Message();
+
+		in_buffer.append(rbuf, len);
+	}
+
+	if(!in_buffer.empty())
+	{
+		string::iterator iter = in_buffer.begin();
+		if(Message msg = parse_message(iter, in_buffer.end()))
+		{
+			skip(iter, in_buffer.end(), "\r\n");
+			in_buffer.erase(in_buffer.begin(), iter);
+
+			if(msg.header.type=="REPLY" && pending_commands>0)
+			{
+				--pending_commands;
+				if(!cmd_queue.empty())
+				{
+					command(cmd_queue.front());
+					cmd_queue.pop_front();
+				}
+			}
+
+			return msg;
+		}
+	}
+
+	return Message();
+}
+
+void CentralStation::process_reply(const Message &msg)
+{
+	if(!msg.header.value.compare(0, 4, "get("))
+	{
+		for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
+		{
+			if(turnouts.count(i->first))
+				turnouts[i->first].synced = true;
+
+			process_object(i->first, i->second);
+		}
+	}
+	else if(!msg.header.value.compare(0, 16, "queryObjects(10,"))
+	{
+		for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
+		{
+			LocoMap::iterator j = locos.find(i->first);
+			if(j==locos.end())
+			{
+				bool found = false;
+				Message::AttribMap::const_iterator k = i->second.find("addr");
+				if(k!=i->second.end())
+				{
+					unsigned addr = lexical_cast<unsigned>(k->second);
+
+					j = locos.find(addr|0x10000);
+					if(j!=locos.end())
+					{
+						command(format("request(%d, view, control, force)", i->first));
+						command(format("get(%d, dir, func[0])", i->first));
+
+						locos.insert(LocoMap::value_type(i->first, j->second));
+						locos.erase(j);
+
+						found = true;
+					}
+				}
+
+				if(!found)
+					locos.insert(LocoMap::value_type(i->first, Locomotive()));
+			}
+
+			process_object(i->first, i->second);
+		}
+
+		locos_synced = true;
+
+		if(locos.lower_bound(0x10000)!=locos.end())
+			command("create(10)");
+	}
+	else if(!msg.header.value.compare(0, 16, "queryObjects(11,"))
+	{
+		for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
+		{
+			TurnoutMap::iterator j = turnouts.find(i->first);
+			if(j==turnouts.end())
+			{
+				bool found = false;
+				Message::AttribMap::const_iterator k = i->second.find("addr");
+				if(k!=i->second.end())
+				{
+					unsigned addr = lexical_cast<unsigned>(k->second);
+
+					j = turnouts.find(addr|0x10000);
+					if(j!=turnouts.end())
+					{
+						command(format("request(%d, view, control)", i->first));
+						command(format("set(%d, symbol[%d])", i->first, j->second.symbol));
+						command(format("get(%d, state)", i->first));
+
+						turnouts.insert(TurnoutMap::value_type(i->first, j->second));
+						turnouts.erase(j);
+
+						found = true;
+					}
+				}
+
+				if(!found)
+					turnouts.insert(TurnoutMap::value_type(i->first, Turnout()));
+			}
+
+			process_object(i->first, i->second);
+		}
+
+		turnouts_synced = true;
+
+		for(TurnoutMap::const_iterator i=turnouts.lower_bound(0x10000); i!=turnouts.end(); ++i)
+			command("create(11, append)");
+	}
+	else if(msg.header.value=="queryObjects(26)")
+	{
+		s88.clear();
+		for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
+		{
+			s88.push_back(i->first);
+			command(format("request(%d, view)", i->first));
+		}
+
+		sensors_synced = true;
+
+		if(!sensors.empty())
+		{
+			unsigned high_addr = (--sensors.end())->first;
+			if(high_addr>16*s88.size())
+				command("create(26, add[0])");
+		}
+	}
+	else if(msg.header.value=="create(10)")
+	{
+		Message::ObjectMap::const_iterator i = msg.content.find(10);
+		if(i!=msg.content.end())
+		{
+			Message::AttribMap::const_iterator j = i->second.find("id");
+			if(j!=i->second.end())
+			{
+				unsigned id = lexical_cast<unsigned>(j->second);
+				LocoMap::iterator k = locos.lower_bound(0x10000);
+				if(k!=locos.end())
+				{
+					command(format("request(%d, view, control)", id));
+					command(format("set(%d, addr[%d], protocol[%s], name[\"%s\"])",
+						id, k->second.address, (k->second.protocol==MM_27 ? "MM27" : "MM14"), k->second.name));
+					command("create(10, append)");
+
+					locos.insert(LocoMap::value_type(id, k->second));
+					locos.erase(k);
+				}
+			}
+		}
+
+		if(locos.lower_bound(0x10000)!=locos.end())
+			command("create(10)");
+	}
+	else if(!msg.header.value.compare(0, 10, "create(11,"))
+	{
+		Message::ObjectMap::const_iterator i = msg.content.find(11);
+		if(i!=msg.content.end())
+		{
+			Message::AttribMap::const_iterator j = i->second.find("id");
+			if(j!=i->second.end())
+			{
+				unsigned id = lexical_cast<unsigned>(j->second);
+				TurnoutMap::iterator k = turnouts.lower_bound(0x10000);
+				if(k!=turnouts.end())
+				{
+					command(format("request(%d, view, control)", id));
+					command(format("set(%d, addr[%d], symbol[%d], name1[\"Switch\"], name2[\"%d\"], name3[\"\"])",
+						id, k->second.address, k->second.symbol, k->second.address));
+					command(format("set(%d, state[%d])", id, k->second.state&((1<<k->second.bits)-1)));
+
+					k->second.synced = true;
+					turnouts.insert(TurnoutMap::value_type(id, k->second));
+					turnouts.erase(k);
+				}
+			}
+		}
+	}
+	else if(!msg.header.value.compare(0, 10, "create(26,"))
+		command("queryObjects(26)");
+}
+
+void CentralStation::process_event(const Message &msg)
+{
+	for(Message::ObjectMap::const_iterator i=msg.content.begin(); i!=msg.content.end(); ++i)
+		process_object(i->first, i->second);
+}
+
+void CentralStation::process_object(unsigned id, const Message::AttribMap &attribs)
+{
+	if(id==1)
+	{
+		for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
+			if(i->first=="status")
+			{
+				power = (i->second=="GO");
+				signal_power.emit(power);
+			}
+	}
+	else if(locos.count(id))
+	{
+		Locomotive &loco = locos[id];
+		bool speed_changed = false;
+		unsigned funcs_changed = 0;
+		for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
+		{
+			if(i->first=="name")
+				loco.name = i->second.substr(1, i->second.size()-2);
+			else if(i->first=="addr")
+			{
+				loco_addr.erase(loco.address);
+				loco.address = lexical_cast<unsigned>(i->second);
+				loco_addr[loco.address] = id;
+			}
+			else if(i->first=="protocol")
+			{
+				if(i->second=="MM")
+					loco.protocol = MM;
+				else if(i->second=="MM27")
+					loco.protocol = MM_27;
+				else if(i->second=="MFX")
+					loco.protocol = MFX;
+			}
+			else if(i->first=="speedstep")
+			{
+				loco.speed = lexical_cast<unsigned>(i->second);
+				speed_changed = true;
+			}
+			else if(i->first=="dir")
+			{
+				loco.reverse = i->second[0]!='0';
+				speed_changed = true;
+			}
+			else if(i->first=="func")
+			{
+				vector<string> parts = split(i->second, ", ");
+				unsigned func = lexical_cast<unsigned>(parts[0]);
+				bool value = lexical_cast<unsigned>(parts[1]);
+				loco.funcs &= ~(1<<func);
+				if(value)
+					loco.funcs |= 1<<func;
+				funcs_changed |= 1<<func;
+			}
+			else if(i->first=="msg")
+			{
+				if(i->second=="CONTROL_LOST")
+					command(format("request(%d, control, force)", id));
+			}
+		}
+
+		if(speed_changed)
+			signal_loco_speed.emit(loco.address, loco.speed, loco.reverse);
+		for(unsigned i=0; funcs_changed>>i; ++i)
+			if(funcs_changed&(1<<i))
+				signal_loco_function.emit(loco.address, i, loco.funcs&(1<<i));
+	}
+	else if(turnouts.count(id))
+	{
+		Turnout &turnout = turnouts[id];
+		bool state_changed = false;
+		for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
+		{
+			if(i->first=="addr")
+			{
+				turnout_addr.erase(turnout.address);
+				turnout.address = lexical_cast<unsigned>(i->second);
+				turnout_addr[turnout.address] = id;
+			}
+			else if(i->first=="state")
+			{
+				unsigned state = lexical_cast<unsigned>(i->second);
+				unsigned mask = (1<<turnout.bits)-1;
+				turnout.state = (turnout.state&~mask) | (state&mask);
+				state_changed = true;
+			}
+		}
+
+		if(state_changed)
+			signal_turnout.emit(turnout.address, turnout.state);
+	}
+	else if(find(s88.begin(), s88.end(), id)!=s88.end())
+	{
+		unsigned base = 0;
+		for(; (base<s88.size() && s88[base]!=id); ++base) ;
+
+		for(Message::AttribMap::const_iterator i=attribs.begin(); i!=attribs.end(); ++i)
+		{
+			if(i->first=="state")
+			{
+				unsigned state = lexical_cast<unsigned>(i->second, "%i");
+				for(unsigned j=0; j<16; ++j)
+				{
+					unsigned addr = base*16+j+1;
+					Sensor &sensor = sensors[addr];
+					bool s = state&(1<<j);
+					if(s)
+					{
+						sensor.off_timeout = Time::TimeStamp();
+						if(!sensor.state)
+						{
+							sensor.state = true;
+							signal_sensor.emit(addr, sensor.state);
+						}
+					}
+					else if(sensor.state)
+						sensor.off_timeout = Time::now()+700*Time::msec;
+				}
+			}
+		}
+	}
+}
+
+CentralStation::Protocol CentralStation::map_protocol(const string &name) const
+{
+	if(name=="MM")
+		return MM;
+	else if(name=="MM-27")
+		return MM_27;
+	else if(name=="MFX")
+		return MFX;
+	else
+		throw InvalidParameterValue("Unknown protocol");
+}
+
+template<typename T>
+unsigned CentralStation::map_address(const map<unsigned, T> &omap, const AddressMap &amap, unsigned addr) const
+{
+	if(omap.count(addr))
+		return addr;
+	else
+	{
+		AddressMap::const_iterator i = amap.find(addr);
+		if(i!=amap.end())
+			return i->second;
+		else
+			return 0;
+	}
+}
+
+void CentralStation::skip(string::iterator &iter, const string::iterator &end, const string &what) const
+{
+	for(; (iter!=end && what.find(*iter)!=string::npos); ++iter) ;
+}
+
+string CentralStation::parse_token(string::iterator &iter, const string::iterator &end, const string &stop) const
+{
+	vector<char> parens;
+	bool quote = false;
+	string token;
+
+	skip(iter, end, stop);
+
+	for(; iter!=end; ++iter)
+	{
+		if(stop.find(*iter)!=string::npos && parens.empty() && !quote)
+			break;
+		else if(*iter=='(' || *iter=='[')
+			parens.push_back(*iter);
+		else if((*iter==')' || *iter==']') && !parens.empty())
+		{
+			if((*iter==')' && parens.back()!='(') || (*iter==']' && parens.back()!='['))
+				IO::print("Mismatched parentheses\n");
+			parens.pop_back();
+		}
+		else if(*iter=='"')
+			quote = !quote;
+
+		token += *iter;
+	}
+
+	return token;
+}
+
+CentralStation::Tag CentralStation::parse_tag(string::iterator &iter, const string::iterator &end) const
+{
+	Tag tag;
+
+	for(; (iter!=end && *iter!='<'); ++iter) ;
+	if(iter==end)
+		return Tag();
+
+	tag.type = parse_token(++iter, end, " >");
+	if(tag.type=="END")
+	{
+		string code = parse_token(iter, end, " >");
+		tag.code = lexical_cast<unsigned>(code);
+	}
+	skip(iter, end, " ");
+	tag.value = parse_token(iter, end, ">");
+	if(iter==end)
+		return Tag();
+	++iter;
+
+	return tag;
+}
+
+CentralStation::Message CentralStation::parse_message(string::iterator &iter, const string::iterator &end) const
+{
+	Message msg;
+
+	msg.header = parse_tag(iter, end);
+
+	while(iter!=end)
+	{
+		skip(iter, end, "\r\n");
+		if(*iter=='<')
+			break;
+
+		string id = parse_token(iter, end, " \r\n<");
+		Message::AttribMap &attribs = msg.content[lexical_cast<unsigned>(id)];
+		while(iter!=end && *iter!='\n' && *iter!='\r')
+		{
+			string attr = parse_token(iter, end, " \r\n<");
+			string::size_type open_bracket = attr.find('[');
+			if(open_bracket!=string::npos)
+			{
+				string::size_type close_bracket = attr.rfind(']');
+				attribs[attr.substr(0, open_bracket)] = attr.substr(open_bracket+1, close_bracket-open_bracket-1);
+			}
+			else
+				attribs[attr];
+		}
+	}
+
+	msg.footer = parse_tag(iter, end);
+	if(msg.footer.type.empty())
+		return Message();
+
+	return msg;
+}
+
+
+CentralStation::Tag::Tag():
+	code(0)
+{ }
+
+CentralStation::Tag::operator bool() const
+{
+	return !type.empty();
+}
+
+
+CentralStation::Message::operator bool() const
+{
+	return header && footer;
+}
+
+
+CentralStation::Locomotive::Locomotive():
+	address(0),
+	speed(0),
+	reverse(false),
+	funcs(0),
+	control(false)
+{ }
+
+
+CentralStation::Turnout::Turnout():
+	address(0),
+	symbol(0),
+	state(0),
+	bits(0),
+	synced(false)
+{ }
+
+
+CentralStation::Sensor::Sensor():
+	state(false)
+{ }
+
+} // namespace R2C2