+ArduControl::PendingCommand::PendingCommand():
+ length(0),
+ repeat_count(1)
+{ }
+
+ArduControl::PendingCommand::PendingCommand(GeneralCommand cmd):
+ length(0),
+ repeat_count(1)
+{
+ tag.type = Tag::GENERAL;
+ tag.command = cmd;
+}
+
+ArduControl::PendingCommand::PendingCommand(Locomotive &loco, Locomotive::Command cmd, unsigned index):
+ repeat_count(8)
+{
+ 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");
+}
+
+ArduControl::PendingCommand::PendingCommand(Accessory &acc, Accessory::Command cmd, unsigned index):
+ repeat_count(1)
+{
+ 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");
+}
+
+
+template<typename T>
+void ArduControl::Queue<T>::push(const T &item)
+{
+ MutexLock lock(mutex);
+ items.push_back(item);
+}
+
+template<typename T>
+bool ArduControl::Queue<T>::pop(T &item)
+{
+ MutexLock lock(mutex);
+ if(items.empty())
+ return false;
+
+ item = items.front();
+ items.pop_front();
+ return true;
+}
+
+
+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)
+ {
+ 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;
+}
+
+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)
+ {
+ if(i==next)
+ {
+ if(cycle.size()>1)
+ advance();
+ else
+ next = cycle.end();
+ }
+ cycle.erase(i);
+ return;
+ }
+}
+
+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)
+ {
+ 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.completed_commands.push(tag);
+ }
+
+ if(count>octets_remaining)
+ octets_remaining = 0;
+ else
+ octets_remaining -= count;
+ }
+}
+
+void ArduControl::S88Task::set_n_octets(unsigned n)
+{
+ n_octets = n;
+}
+
+void ArduControl::S88Task::grow_n_octets(unsigned n)
+{
+ if(n>n_octets)
+ n_octets = n;
+}
+
+
+ArduControl::MfxAnnounceTask::MfxAnnounceTask():
+ serial(0)
+{ }
+
+bool ArduControl::MfxAnnounceTask::get_work(PendingCommand &cmd)
+{
+ Time::TimeStamp t = Time::now();
+ if(t<next)
+ return false;
+
+ cmd.command[0] = MFX_ANNOUNCE;
+ cmd.command[1] = serial>>8;
+ cmd.command[2] = serial;
+ cmd.length = 3;
+ next = t+400*Time::msec;
+
+ return true;
+}
+
+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;
+ queue.push(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::set_next_address(unsigned a)
+{
+ next_address = a;
+}
+
+bool ArduControl::MfxSearchTask::pop_info(MfxInfo &info)
+{
+ return queue.pop(info);
+}
+
+