X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibr2c2%2Farducontrol.cpp;h=47584f44bb8b145496a0b6d9a52c118f4c68fcd9;hb=9afe42fc94eed6754da8401082e76121f8c66783;hp=7111f53012b5ec177e23f718dcf5fdc607286307;hpb=4b287b53e9127209864292f583b8ddbdd481d022;p=r2c2.git

diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp
index 7111f53..47584f4 100644
--- a/source/libr2c2/arducontrol.cpp
+++ b/source/libr2c2/arducontrol.cpp
@@ -19,15 +19,12 @@ ArduControl::ArduControl(const string &dev):
 	serial(dev),
 	debug(1),
 	power(false),
-	next_refresh(refresh_cycle.end()),
-	refresh_counter(0),
 	active_accessory(0),
-	n_s88_octets(0),
-	mfx_announce_serial(0),
-	next_mfx_address(1),
+	s88(*this),
+	mfx_search(*this),
 	thread(*this)
 {
-	QueuedCommand cmd;
+	PendingCommand cmd;
 	cmd.command[0] = READ_POWER_STATE;
 	cmd.length = 1;
 	push_command(cmd);
@@ -50,7 +47,7 @@ void ArduControl::set_power(bool p)
 {
 	if(power.set(p))
 	{
-		QueuedCommand cmd(POWER);
+		PendingCommand cmd(POWER);
 		cmd.tag.serial = power.serial;
 		cmd.command[0] = (p ? POWER_ON : POWER_OFF);
 		cmd.length = 1;
@@ -87,6 +84,14 @@ unsigned ArduControl::get_protocol_speed_steps(const string &proto_name) const
 	return protocol_info[map_protocol(proto_name)].max_speed;
 }
 
+const Driver::DetectedLocomotive *ArduControl::enumerate_detected_locos(unsigned i) const
+{
+	if(i>=mfx_info.size())
+		return 0;
+
+	return &mfx_info[i];
+}
+
 unsigned ArduControl::add_loco(unsigned addr, const string &proto_name, const VehicleType &)
 {
 	if(!addr)
@@ -105,7 +110,7 @@ unsigned ArduControl::add_loco(unsigned addr, const string &proto_name, const Ve
 void ArduControl::remove_loco(unsigned id)
 {
 	Locomotive &loco = get_item(locomotives, id);
-	remove_loco_from_refresh(loco);
+	refresh.remove_loco(loco);
 	locomotives.erase(id);
 }
 
@@ -117,10 +122,10 @@ void ArduControl::set_loco_speed(unsigned id, unsigned speed)
 
 	if(loco.speed.set(speed))
 	{
-		QueuedCommand cmd(loco, Locomotive::SPEED);
+		PendingCommand cmd(loco, Locomotive::SPEED);
 		push_command(cmd);
 
-		add_loco_to_refresh(loco);
+		refresh.add_loco(loco);
 	}
 }
 
@@ -129,10 +134,10 @@ void ArduControl::set_loco_reverse(unsigned id, bool rev)
 	Locomotive &loco = get_item(locomotives, id);
 	if(loco.reverse.set(rev))
 	{
-		QueuedCommand cmd(loco, Locomotive::REVERSE);
+		PendingCommand cmd(loco, Locomotive::REVERSE);
 		push_command(cmd);
 
-		add_loco_to_refresh(loco);
+		refresh.add_loco(loco);
 	}
 }
 
@@ -147,68 +152,11 @@ void ArduControl::set_loco_function(unsigned id, unsigned func, bool state)
 	{
 		if(func>0 || loco.proto!=MM)
 		{
-			QueuedCommand cmd(loco, Locomotive::FUNCTIONS, func);
+			PendingCommand cmd(loco, Locomotive::FUNCTIONS, func);
 			push_command(cmd);
 		}
 
-		add_loco_to_refresh(loco);
-	}
-}
-
-void ArduControl::add_loco_to_refresh(Locomotive &loco)
-{
-	MutexLock lock(mutex);
-	refresh_cycle.push_back(&loco);
-	if(refresh_cycle.size()>15)
-	{
-		LocomotivePtrList::iterator oldest = refresh_cycle.begin();
-		for(LocomotivePtrList::iterator i=refresh_cycle.begin(); ++i!=refresh_cycle.end(); )
-			if((*i)->last_change_age>(*oldest)->last_change_age)
-				oldest = i;
-		if(oldest==next_refresh)
-			advance_next_refresh();
-		refresh_cycle.erase(oldest);
-	}
-	if(next_refresh==refresh_cycle.end())
-		next_refresh = refresh_cycle.begin();
-}
-
-void ArduControl::remove_loco_from_refresh(Locomotive &loco)
-{
-	MutexLock lock(mutex);
-	for(LocomotivePtrList::iterator i=refresh_cycle.begin(); i!=refresh_cycle.end(); ++i)
-		if(*i==&loco)
-		{
-			if(i==next_refresh)
-			{
-				if(refresh_cycle.size()>1)
-					advance_next_refresh();
-				else
-					next_refresh = refresh_cycle.end();
-			}
-			refresh_cycle.erase(i);
-			return;
-		}
-}
-
-ArduControl::Locomotive *ArduControl::get_loco_to_refresh()
-{
-	MutexLock lock(mutex);
-	if(refresh_cycle.empty())
-		return 0;
-
-	Locomotive *loco = *next_refresh;
-	advance_next_refresh();
-	return loco;
-}
-
-void ArduControl::advance_next_refresh()
-{
-	++next_refresh;
-	if(next_refresh==refresh_cycle.end())
-	{
-		next_refresh = refresh_cycle.begin();
-		++refresh_counter;
+		refresh.add_loco(loco);
 	}
 }
 
@@ -307,9 +255,7 @@ unsigned ArduControl::add_sensor(unsigned addr)
 		throw invalid_argument("ArduControl::add_sensor");
 
 	insert_unique(sensors, addr, Sensor(addr));
-	unsigned octet_index = (addr-1)/8;
-	if(octet_index>=n_s88_octets)
-		n_s88_octets = octet_index+1;
+	s88.grow_n_octets((addr+7)/8);
 
 	return addr;
 }
@@ -317,7 +263,7 @@ unsigned ArduControl::add_sensor(unsigned addr)
 void ArduControl::remove_sensor(unsigned addr)
 {
 	remove_existing(sensors, addr);
-	// TODO update n_s88_octets
+	// TODO update s88.n_octets
 }
 
 bool ArduControl::get_sensor(unsigned addr) const
@@ -336,6 +282,23 @@ void ArduControl::tick()
 				if(power.commit(tag.serial))
 					signal_power.emit(power.current);
 			}
+			else if(tag.command==NEW_LOCO)
+			{
+				MfxInfo info;
+				if(mfx_search.pop_info(info))
+				{
+					MfxInfoArray::iterator i;
+					for(i=mfx_info.begin(); (i!=mfx_info.end() && i->id!=info.id); ++i) ;
+					if(i==mfx_info.end())
+					{
+						mfx_info.push_back(info);
+						i = --mfx_info.end();
+					}
+					else
+						*i = info;
+					signal_locomotive_detected.emit(*i);
+				}
+			}
 		}
 		else if(tag.type==Tag::LOCOMOTIVE)
 		{
@@ -421,7 +384,7 @@ void ArduControl::tick()
 			unsigned i;
 			for(i=0; (lowest_bit>>i)>1; ++i) ;
 			acc.state.set(acc.state^lowest_bit);
-			QueuedCommand cmd(acc, Accessory::ACTIVATE, i);
+			PendingCommand cmd(acc, Accessory::ACTIVATE, i);
 			push_command(cmd);
 		}
 		else
@@ -434,7 +397,7 @@ void ArduControl::tick()
 		if(t>off_timeout)
 		{
 			off_timeout = Time::TimeStamp();
-			QueuedCommand cmd(*active_accessory, Accessory::DEACTIVATE);
+			PendingCommand cmd(*active_accessory, Accessory::DEACTIVATE);
 			push_command(cmd);
 		}
 	}
@@ -444,13 +407,13 @@ void ArduControl::flush()
 {
 }
 
-void ArduControl::push_command(const QueuedCommand &cmd)
+void ArduControl::push_command(const PendingCommand &cmd)
 {
 	MutexLock lock(mutex);
 	command_queue.push_back(cmd);
 }
 
-bool ArduControl::pop_command(QueuedCommand &cmd)
+bool ArduControl::pop_command(PendingCommand &cmd)
 {
 	MutexLock lock(mutex);
 	if(command_queue.empty())
@@ -583,296 +546,521 @@ ArduControl::Sensor::Sensor(unsigned a):
 { }
 
 
-ArduControl::ControlThread::ControlThread(ArduControl &c):
-	control(c),
-	done(false)
+ArduControl::PendingCommand::PendingCommand():
+	length(0),
+	repeat_count(1)
+{ }
+
+ArduControl::PendingCommand::PendingCommand(GeneralCommand cmd):
+	length(0),
+	repeat_count(1)
 {
-	launch();
+	tag.type = Tag::GENERAL;
+	tag.command = cmd;
 }
 
-void ArduControl::ControlThread::exit()
+ArduControl::PendingCommand::PendingCommand(Locomotive &loco, Locomotive::Command cmd, unsigned index):
+	repeat_count(8)
 {
-	done = true;
-	join();
+	tag.type = Tag::LOCOMOTIVE;
+	tag.command = cmd;
+	tag.id = loco.id;
+	if(cmd==Locomotive::SPEED)
+	{
+		tag.serial = loco.speed.serial;
+		length = loco.create_speed_dir_command(command);
+	}
+	else if(cmd==Locomotive::REVERSE)
+	{
+		tag.serial = loco.reverse.serial;
+		length = loco.create_speed_dir_command(command);
+	}
+	else if(cmd==Locomotive::FUNCTIONS)
+	{
+		tag.serial = loco.funcs.serial;
+		length = loco.create_speed_func_command(index, command);
+	}
+	else
+		throw invalid_argument("PendingCommand");
 }
 
-void ArduControl::ControlThread::main()
+ArduControl::PendingCommand::PendingCommand(Accessory &acc, Accessory::Command cmd, unsigned index):
+	repeat_count(1)
 {
-	char command[15];
-	unsigned length = 0;
-	unsigned repeat_count = 0;
-	Tag tag;
-	Locomotive *loco = 0;
-	unsigned phase = 0;
-	unsigned s88_octets_remaining = 0;
+	tag.type = Tag::ACCESSORY;
+	tag.command = cmd;
+	tag.id = acc.address;
+	if(cmd==Accessory::ACTIVATE || cmd==Accessory::DEACTIVATE)
+	{
+		tag.serial = acc.state.serial;
+		length = acc.create_state_command(index, (cmd==Accessory::ACTIVATE), command);
+	}
+	else
+		throw invalid_argument("PendingCommand");
+}
 
-	Msp::Time::TimeStamp next_mfx_announce;
-	unsigned mfx_search_size = 0;
-	unsigned mfx_search_bits = 0;
-	Msp::Time::TimeStamp next_mfx_search;
 
-	while(!done)
+ArduControl::RefreshTask::RefreshTask():
+	next(cycle.end()),
+	round(0),
+	loco(0),
+	phase(0)
+{ }
+
+bool ArduControl::RefreshTask::get_work(PendingCommand &cmd)
+{
+	if(loco && loco->proto==MM && phase==0)
 	{
-		Time::TimeStamp t = Time::now();
+		cmd.length = loco->create_speed_func_command(round%4+1, cmd.command);
+		cmd.repeat_count = 2;
+		++phase;
+		return true;
+	}
+
+	loco = get_next_loco();
+	if(!loco)
+		return false;
+
+	phase = 0;
+	if(loco->proto==MM)
+	{
+		cmd.length = loco->create_speed_dir_command(cmd.command);
+		cmd.repeat_count = 2;
+	}
+	else if(loco->proto==MFX)
+		cmd.length = loco->create_speed_func_command(0, cmd.command);
+	else
+		return false;
+
+	return true;
+}
 
-		if(!repeat_count)
+void ArduControl::RefreshTask::add_loco(Locomotive &l)
+{
+	MutexLock lock(mutex);
+	cycle.push_back(&l);
+	if(cycle.size()>15)
+	{
+		LocomotivePtrList::iterator oldest = cycle.begin();
+		for(LocomotivePtrList::iterator i=cycle.begin(); ++i!=cycle.end(); )
+			if((*i)->last_change_age>(*oldest)->last_change_age)
+				oldest = i;
+		if(oldest==next)
+			advance();
+		cycle.erase(oldest);
+	}
+	if(next==cycle.end())
+		next = cycle.begin();
+}
+
+void ArduControl::RefreshTask::remove_loco(Locomotive &l)
+{
+	MutexLock lock(mutex);
+	for(LocomotivePtrList::iterator i=cycle.begin(); i!=cycle.end(); ++i)
+		if(*i==&l)
 		{
-			tag = Tag();
-			length = 0;
-			repeat_count = 1;
-			QueuedCommand qcmd;
-			if(control.pop_command(qcmd))
-			{
-				length = qcmd.length;
-				copy(qcmd.command, qcmd.command+length, command);
-				if(qcmd.tag.type==Tag::LOCOMOTIVE)
-					repeat_count = 8;
-				tag = qcmd.tag;
-			}
-			else if(loco && phase==0)
-			{
-				length = loco->create_speed_func_command(control.refresh_counter%4+1, command);
-				repeat_count = 2;
-				++phase;
-			}
-			else if(t>=next_mfx_announce)
-			{
-				command[0] = MFX_ANNOUNCE;
-				command[1] = control.mfx_announce_serial>>8;
-				command[2] = control.mfx_announce_serial;
-				length = 3;
-				next_mfx_announce = t+400*Time::msec;
-			}
-			else if(t>=next_mfx_search)
-			{
-				command[0] = MFX_SEARCH;
-				for(unsigned i=0; i<4; ++i)
-					command[1+i] = mfx_search_bits>>(24-i*8);
-				command[5] = mfx_search_size;
-				length = 6;
-				next_mfx_search = t+200*Time::msec;
-				if(control.debug>=1)
-					IO::print("Search %08X/%d\n", mfx_search_bits, mfx_search_size);
-			}
-			else if(!s88_octets_remaining && control.n_s88_octets)
+			if(i==next)
 			{
-				command[0] = S88_READ;
-				command[1] = control.n_s88_octets;
-				length = 2;
-				s88_octets_remaining = control.n_s88_octets;
-			}
-			else if((loco = control.get_loco_to_refresh()))
-			{
-				if(loco->proto==MM)
-				{
-					length = loco->create_speed_dir_command(command);
-					repeat_count = 2;
-					phase = 0;
-				}
-				else if(loco->proto==MFX)
-				{
-					length = loco->create_speed_func_command(0, command);
-					phase = 1;
-				}
-			}
-			else
-			{
-				// Send an idle packet for the MM protocol
-				command[0] = MOTOROLA_SPEED;
-				command[1] = 80;
-				command[2] = 0;
-				command[3] = 0;
-				length = 4;
+				if(cycle.size()>1)
+					advance();
+				else
+					next = cycle.end();
 			}
+			cycle.erase(i);
+			return;
 		}
+}
 
-		if(control.debug>=2)
+ArduControl::Locomotive *ArduControl::RefreshTask::get_next_loco()
+{
+	MutexLock lock(mutex);
+	if(cycle.empty())
+		return 0;
+
+	Locomotive *l = *next;
+	advance();
+	return l;
+}
+
+void ArduControl::RefreshTask::advance()
+{
+	++next;
+	if(next==cycle.end())
+	{
+		next= cycle.begin();
+		++round;
+	}
+}
+
+
+ArduControl::S88Task::S88Task(ArduControl &c):
+	control(c),
+	n_octets(0),
+	octets_remaining(0)
+{ }
+
+bool ArduControl::S88Task::get_work(PendingCommand &cmd)
+{
+	if(octets_remaining || !n_octets)
+		return false;
+
+	octets_remaining = n_octets;
+	cmd.command[0] = S88_READ;
+	cmd.command[1] = octets_remaining;
+	cmd.length = 2;
+
+	return true;
+}
+
+void ArduControl::S88Task::process_reply(const char *reply, unsigned length)
+{
+	unsigned char type = reply[0];
+	if(type==S88_DATA && length>2)
+	{
+		unsigned offset = static_cast<unsigned char>(reply[1]);
+		unsigned count = length-2;
+
+		SensorMap::iterator begin = control.sensors.lower_bound(offset*8+1);
+		SensorMap::iterator end = control.sensors.upper_bound((offset+count)*8);
+		for(SensorMap::iterator i=begin; i!=end; ++i)
 		{
-			string cmd_hex;
-			for(unsigned i=0; i<length; ++i)
-				cmd_hex += format(" %02X", static_cast<unsigned char>(command[i]));
-			IO::print("< %02X%s\n", length^0xFF, cmd_hex);
+			unsigned bit_index = i->first-1-offset*8;
+			bool state = (reply[2+bit_index/8]>>(7-bit_index%8))&1;
+			i->second.state.set(state);
+
+			Tag tag;
+			tag.type = Tag::SENSOR;
+			tag.command = Sensor::STATE;
+			tag.serial = i->second.state.serial;
+			tag.id = i->first;
+			control.push_completed_tag(tag);
 		}
 
-		control.serial.put(length^0xFF);
-		control.serial.write(command, length);
-		--repeat_count;
+		if(count>octets_remaining)
+			octets_remaining = 0;
+		else
+			octets_remaining -= count;
+	}
+}
 
-		bool got_reply = false;
-		bool got_data = false;
-		while(!got_reply || got_data)
-		{
-			if(got_reply)
-				got_data = IO::poll(control.serial, IO::P_INPUT, Time::zero);
-			else
-				got_data = IO::poll(control.serial, IO::P_INPUT);
+void ArduControl::S88Task::set_n_octets(unsigned n)
+{
+	n_octets = n;
+}
 
-			if(got_data)
-			{
-				unsigned rlength = control.serial.get()^0xFF;
-				if(rlength>15)
-				{
-					IO::print("Invalid length %02X\n", rlength);
-					continue;
-				}
+void ArduControl::S88Task::grow_n_octets(unsigned n)
+{
+	if(n>n_octets)
+		n_octets = n;
+}
 
-				char reply[15];
-				unsigned pos = 0;
-				while(pos<rlength)
-					pos += control.serial.read(reply+pos, rlength-pos);
 
-				if(control.debug>=2)
-				{
-					string reply_hex;
-					for(unsigned i=0; i<rlength; ++i)
-						reply_hex += format(" %02X", static_cast<unsigned char>(reply[i]));
-					IO::print("> %02X%s\n", rlength^0xFF, reply_hex);
-				}
+ArduControl::MfxAnnounceTask::MfxAnnounceTask():
+	serial(0)
+{ }
 
-				unsigned char type = reply[0];
-				if((type&0xE0)==0x80)
-				{
-					got_reply = true;
-					if(type!=COMMAND_OK)
-						IO::print("Error %02X\n", type);
-					else if(tag && !repeat_count)
-						control.push_completed_tag(tag);
-				}
-				else if(type==POWER_STATE && rlength==2)
-				{
-					control.power.set(reply[1]);
+bool ArduControl::MfxAnnounceTask::get_work(PendingCommand &cmd)
+{
+	Time::TimeStamp t = Time::now();
+	if(t<next)
+		return false;
 
-					Tag ptag;
-					ptag.type = Tag::GENERAL;
-					ptag.command = POWER;
-					ptag.serial = control.power.serial;
-					control.push_completed_tag(ptag);
-				}
-				else if(type==MFX_FEEDBACK && rlength==2)
-				{
-					if(command[0]==MFX_SEARCH)
-					{
-						bool finished = true;
-						if(reply[1])
-						{
-							if(mfx_search_size==32)
-							{
-								if(control.debug>=1)
-									IO::print("Assigning MFX address %d to decoder %08X\n", control.next_mfx_address, mfx_search_bits);
-
-								QueuedCommand qcmd;
-								qcmd.command[0] = MFX_ASSIGN_ADDRESS;
-								qcmd.command[1] = control.next_mfx_address>>8;
-								qcmd.command[2] = control.next_mfx_address;
-								for(unsigned i=0; i<4; ++i)
-									qcmd.command[3+i] = mfx_search_bits>>(24-i*8);
-								qcmd.length = 7;
-								control.push_command(qcmd);
-								++control.next_mfx_address;
-							}
-							else
-							{
-								++mfx_search_size;
-								finished = false;
-							}
-						}
-						else if(mfx_search_size>0)
-						{
-							unsigned mask = 1<<(32-mfx_search_size);
-							if(!(mfx_search_bits&mask))
-							{
-								mfx_search_bits |= mask;
-								finished = false;
-							}
-						}
-
-						if(finished)
-						{
-							next_mfx_search = t+2*Time::sec;
-							mfx_search_bits = 0;
-							mfx_search_size = 0;
-						}
-					}
-				}
-				else if(type==S88_DATA && rlength>2)
-				{
-					unsigned offset = static_cast<unsigned char>(reply[1]);
-					unsigned count = rlength-2;
+	cmd.command[0] = MFX_ANNOUNCE;
+	cmd.command[1] = serial>>8;
+	cmd.command[2] = serial;
+	cmd.length = 3;
+	next = t+400*Time::msec;
 
-					SensorMap::iterator begin = control.sensors.lower_bound(offset*8+1);
-					SensorMap::iterator end = control.sensors.upper_bound((offset+count)*8);
-					for(SensorMap::iterator i=begin; i!=end; ++i)
-					{
-						unsigned bit_index = i->first-1-offset*8;
-						bool state = (reply[2+bit_index/8]>>(7-bit_index%8))&1;
-						i->second.state.set(state);
-
-						Tag stag;
-						stag.type = Tag::SENSOR;
-						stag.command = Sensor::STATE;
-						stag.serial = i->second.state.serial;
-						stag.id = i->first;
-						control.push_completed_tag(stag);
-					}
+	return true;
+}
 
-					if(count>s88_octets_remaining)
-						s88_octets_remaining = 0;
-					else
-						s88_octets_remaining -= count;
-				}
-			}
+void ArduControl::MfxAnnounceTask::set_serial(unsigned s)
+{
+	serial = s;
+}
+
+
+ArduControl::MfxSearchTask::MfxSearchTask(ArduControl &c):
+	control(c),
+	next_address(1),
+	size(0),
+	bits(0),
+	misses(0)
+{ }
+
+bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd)
+{
+	if(size>32)
+	{
+		if(control.debug>=1)
+			IO::print("Assigning MFX address %d to decoder %08X\n", next_address, bits);
+
+		MfxInfo info;
+		info.protocol = "MFX";
+		info.address = next_address;
+		info.name = format("%08X", bits);
+		info.id = bits;
+		push_info(info);
+
+		cmd.command[0] = MFX_ASSIGN_ADDRESS;
+		cmd.command[1] = next_address>>8;
+		cmd.command[2] = next_address;
+		for(unsigned i=0; i<4; ++i)
+			cmd.command[3+i] = bits>>(24-i*8);
+		cmd.length = 7;
+
+		cmd.tag.type = Tag::GENERAL;
+		cmd.tag.command = NEW_LOCO;
+		cmd.tag.id = bits;
+
+		size = 0;
+		bits = 0;
+		++next_address;
+
+		return true;
+	}
+
+	Time::TimeStamp t = Time::now();
+	if(t<next)
+		return false;
+
+	cmd.command[0] = MFX_SEARCH;
+	for(unsigned i=0; i<4; ++i)
+		cmd.command[1+i] = bits>>(24-i*8);
+	cmd.command[5] = size;
+	cmd.length = 6;
+
+	next = t+200*Time::msec;
+
+	if(control.debug>=1)
+		IO::print("Search %08X/%d\n", bits, size);
+
+	return true;
+}
+
+void ArduControl::MfxSearchTask::process_reply(const char *reply, unsigned length)
+{
+	unsigned char type = reply[0];
+	if(type==MFX_SEARCH_FEEDBACK && length==2)
+	{
+		if(reply[1])
+		{
+			misses = 0;
+			++size;
+		}
+		else if(size>0 && misses<6)
+		{
+			++misses;
+			bits ^= 1<<(32-size);
+		}
+		else
+		{
+			next = Time::now()+2*Time::sec;
+			bits = 0;
+			size = 0;
+			misses = 0;
 		}
 	}
 }
 
+void ArduControl::MfxSearchTask::push_info(const MfxInfo &info)
+{
+	MutexLock lock(mutex);
+	queue.push_back(info);
+}
+
+bool ArduControl::MfxSearchTask::pop_info(MfxInfo &info)
+{
+	MutexLock lock(mutex);
+	if(queue.empty())
+		return false;
+	info = queue.back();
+	queue.pop_back();
+	return true;
+}
 
-ArduControl::QueuedCommand::QueuedCommand():
-	length(0)
-{ }
 
-ArduControl::QueuedCommand::QueuedCommand(GeneralCommand cmd):
-	length(0)
+ArduControl::ControlThread::ControlThread(ArduControl &c):
+	control(c),
+	done(false)
 {
-	tag.type = Tag::GENERAL;
-	tag.command = cmd;
+	tasks.push_back(&control.mfx_announce);
+	tasks.push_back(&control.mfx_search);
+	tasks.push_back(&control.s88);
+	tasks.push_back(&control.refresh);
+
+	launch();
 }
 
-ArduControl::QueuedCommand::QueuedCommand(Locomotive &loco, Locomotive::Command cmd, unsigned index)
+void ArduControl::ControlThread::exit()
 {
-	tag.type = Tag::LOCOMOTIVE;
-	tag.command = cmd;
-	tag.id = loco.id;
-	if(cmd==Locomotive::SPEED)
+	done = true;
+	join();
+}
+
+void ArduControl::ControlThread::main()
+{
+	init_baud_rate();
+
+	while(!done)
 	{
-		tag.serial = loco.speed.serial;
-		length = loco.create_speed_dir_command(command);
+		PendingCommand cmd;
+		if(get_work(cmd))
+		{
+			bool success = true;
+			for(unsigned i=0; (success && i<cmd.repeat_count); ++i)
+				success = (do_command(cmd)==COMMAND_OK);
+			if(success && cmd.tag)
+				control.push_completed_tag(cmd.tag);
+		}
+		else
+			Time::sleep(10*Time::msec);
 	}
-	else if(cmd==Locomotive::REVERSE)
+}
+
+void ArduControl::ControlThread::init_baud_rate()
+{
+	static unsigned rates[] = { 57600, 9600, 19200, 38400, 0 };
+	unsigned rate = 0;
+	control.serial.set_data_bits(8);
+	control.serial.set_parity(IO::Serial::NONE);
+	control.serial.set_stop_bits(1);
+	for(unsigned i=0; rates[i]; ++i)
 	{
-		tag.serial = loco.reverse.serial;
-		length = loco.create_speed_dir_command(command);
+		control.serial.set_baud_rate(rates[i]);
+		control.serial.put('\xFF');
+		if(IO::poll(control.serial, IO::P_INPUT, 500*Time::msec))
+		{
+			int c = control.serial.get();
+			if(c==0xFF)
+			{
+				rate = rates[i];
+				break;
+			}
+		}
 	}
-	else if(cmd==Locomotive::FUNCTIONS)
+
+	if(control.debug>=1)
+		IO::print("ArduControl detected at %d bits/s\n", rate);
+
+	if(rate!=rates[0])
 	{
-		tag.serial = loco.funcs.serial;
-		length = loco.create_speed_func_command(index, command);
+		PendingCommand cmd;
+		cmd.command[0] = SET_BAUD_RATE;
+		cmd.command[1] = rates[0]>>8;
+		cmd.command[2] = rates[0];
+		cmd.length = 3;
+		if(do_command(cmd)==COMMAND_OK)
+		{
+			control.serial.set_baud_rate(rates[0]);
+			if(do_command(cmd)==COMMAND_OK)
+			{
+				Time::sleep(Time::sec);
+				if(control.debug>=1)
+					IO::print("Rate changed to %d bits/s\n", rates[0]);
+			}
+		}
 	}
-	else
-		throw invalid_argument("QueuedCommand");
 }
 
-ArduControl::QueuedCommand::QueuedCommand(Accessory &acc, Accessory::Command cmd, unsigned index)
+bool ArduControl::ControlThread::get_work(PendingCommand &cmd)
 {
-	tag.type = Tag::ACCESSORY;
-	tag.command = cmd;
-	tag.id = acc.address;
-	if(cmd==Accessory::ACTIVATE || cmd==Accessory::DEACTIVATE)
+	if(control.pop_command(cmd))
+		return true;
+
+	for(vector<Task *>::iterator i=tasks.begin(); i!=tasks.end(); ++i)
+		if((*i)->get_work(cmd))
+			return true;
+
+	// As fallback, send an idle packet for the MM protocol
+	cmd.command[0] = MOTOROLA_SPEED;
+	cmd.command[1] = 80;
+	cmd.command[2] = 0;
+	cmd.command[3] = 0;
+	cmd.length = 4;
+
+	return true;
+}
+
+unsigned ArduControl::ControlThread::do_command(const PendingCommand &cmd)
+{
+	if(control.debug>=2)
 	{
-		tag.serial = acc.state.serial;
-		length = acc.create_state_command(index, (cmd==Accessory::ACTIVATE), command);
+		string cmd_hex;
+		for(unsigned i=0; i<cmd.length; ++i)
+			cmd_hex += format(" %02X", static_cast<unsigned char>(cmd.command[i]));
+		IO::print("< %02X%s\n", cmd.length^0xFF, cmd_hex);
+	}
+
+	control.serial.put(cmd.length^0xFF);
+	control.serial.write(cmd.command, cmd.length);
+
+	unsigned result = 0;
+	while(1)
+	{
+		bool got_data;
+		if(result)
+			got_data = IO::poll(control.serial, IO::P_INPUT, Time::zero);
+		else
+			got_data = IO::poll(control.serial, IO::P_INPUT);
+
+		if(!got_data)
+			break;
+
+		unsigned rlength = control.serial.get()^0xFF;
+		if(rlength>15)
+		{
+			IO::print("Invalid length %02X\n", rlength);
+			continue;
+		}
+
+		char reply[15];
+		unsigned pos = 0;
+		while(pos<rlength)
+			pos += control.serial.read(reply+pos, rlength-pos);
+
+		if(control.debug>=2)
+		{
+			string reply_hex;
+			for(unsigned i=0; i<rlength; ++i)
+				reply_hex += format(" %02X", static_cast<unsigned char>(reply[i]));
+			IO::print("> %02X%s\n", rlength^0xFF, reply_hex);
+		}
+
+		unsigned r = process_reply(reply, rlength);
+		if(r && !result)
+			result = r;
+	}
+
+	return result;
+}
+
+unsigned ArduControl::ControlThread::process_reply(const char *reply, unsigned rlength)
+{
+	unsigned char type = reply[0];
+	if((type&0xE0)==0x80)
+	{
+		if(type!=COMMAND_OK)
+			IO::print("Error %02X\n", type);
+		return type;
+	}
+	else if(type==POWER_STATE && rlength==2)
+	{
+		control.power.set(reply[1]);
+
+		Tag tag;
+		tag.type = Tag::GENERAL;
+		tag.command = POWER;
+		tag.serial = control.power.serial;
+		control.push_completed_tag(tag);
 	}
 	else
-		throw invalid_argument("QueuedCommand");
+	{
+		for(vector<Task *>::iterator i=tasks.begin(); i!=tasks.end(); ++i)
+			(*i)->process_reply(reply, rlength);
+	}
+
+	return 0;
 }
 
 } // namespace R2C2