/// <summary>
/// Helper for notification handlers. This relies on being called from
/// an "exclusive" handler - it modifies the state directly!
/// </summary>
/// <param name="autoDef"></param>
/// <param name="values"></param>
/// <param name="timestamp"></param>
private void autoSubscribeNotificationHelper(AutoDefinition autoDef, List <Double> values, DateTime timestamp, bool keysChanged)
{
int useableValueCount =
((autoDef.subsetStart + (autoDef.subsetCount >= 1 ? autoDef.subsetCount : Int32.MaxValue)) <= values.Count) ?
autoDef.subsetCount :
(values.Count - autoDef.subsetStart);
if (useableValueCount < 0)
{
useableValueCount = 0;
}
int vectorPartCount =
(autoDef.subsetCount >= 1) ?
autoDef.subsetCount :
useableValueCount;
//Console.WriteLine("Vector: for " + autoDef.infoAsPartner.Name.Name + " got " + values.Count + " values with " + useableCount + " useable");
_state.Timestamp = timestamp;
// If the number of values has changed, we need to rearrange the vector
if (modifyStateSubset(autoDef, values))
{
_state.Validate();
// Reapply keys
applyKeysFromSubsets();
// Send replace notification, a lot changed
LogInfo("Vector length changed");
SendNotification(new Replace(_state));
}
else
{
if (keysChanged)
{
applyKeysFromSubsets();
}
// Send notification
var indices = new List <int>(autoDef.count);
for (int i = autoDef.startIndex; i < autoDef.startIndex + autoDef.count; i++)
{
indices.Add(i);
}
SendNotification(new SetByIndex(new SetByIndexRequestType()
{
Indices = indices,
Values = _state.Values.GetRange(autoDef.startIndex, autoDef.count),
Timestamp = timestamp
}));
}
}
/// <summary>
/// Helper for notification handlers. This relies on being called from
/// an "exclusive" handler - it modifies the state directly!
/// </summary>
/// <param name="autoDef"></param>
/// <param name="index"></param>
/// <param name="value"></param>
/// <param name="timestamp"></param>
private void autoSubscribeNotificationUpdateHelper(AutoDefinition autoDef, int index, double value, DateTime timestamp)
{
if ((index - autoDef.subsetStart) < autoDef.count && index >= autoDef.subsetStart)
{
_state.Set(index - autoDef.subsetStart + autoDef.startIndex, value, timestamp);
//Console.WriteLine("Updating " + index + " with start " + autoDef.startIndex + " and subset start " + autoDef.subsetStart + ". State index " + (index - autoDef.subsetStart + autoDef.startIndex));
}
SendNotification(new SetByIndex(new SetByIndexRequestType()
{
Indices = new List <int>()
{
autoDef.startIndex + index
}, Values = new List <double>()
{
value
}, Timestamp = timestamp
}));
}
private bool modifyStateSubset(AutoDefinition autoDef, List <Double> values)
{
// Figure out how many values we are actually using from this
// publisher, based on the subset specification for this
// subscription
int useableValueCount =
((autoDef.subsetStart + (autoDef.subsetCount >= 1 ? autoDef.subsetCount : Int32.MaxValue)) <= values.Count) ?
autoDef.subsetCount :
(values.Count - autoDef.subsetStart);
if (useableValueCount < 0)
{
useableValueCount = 0;
}
int vectorPartCount =
(autoDef.subsetCount >= 1) ?
autoDef.subsetCount :
useableValueCount;
if (vectorPartCount != autoDef.count)
{
// Shift Vector elements (not calling SetAll because we'll call SetKeys later to rebuild index cache)
var newValues = new List <double>(_state.Values.Count + vectorPartCount - autoDef.count);
newValues.AddRange(_state.Values.GetRange(0, autoDef.startIndex));
if (useableValueCount > 0)
{
newValues.AddRange(values.GetRange(autoDef.subsetStart, useableValueCount));
}
for (int i = useableValueCount; i < vectorPartCount; i++)
{
newValues.Add(0.0);
}
newValues.AddRange(_state.Values.GetRange(autoDef.startIndex + autoDef.count, _state.Values.Count - (autoDef.startIndex + autoDef.count)));
if (newValues.Count != (_state.Values.Count + vectorPartCount - autoDef.count))
{
LogError("Vector length consistency check No. 1 failed!!!");
}
_state.Values = newValues;
// Update AutoDefinitions startIndex's
int firstChangedDef = autoDefs.IndexOf(autoDef) + 1;
autoDef.count = vectorPartCount;
int lastIndex = autoDef.startIndex + vectorPartCount;
for (int i = firstChangedDef; i < autoDefs.Count; i++)
{
autoDefs[i].startIndex = lastIndex;
lastIndex += autoDefs[i].count;
}
// Consistency check
if (lastIndex != _state.Values.Count)
{
LogError("Vector length consistency check No. 2 failed!!!");
}
return(true);
}
else
{
// Just modify the affected elements
for (int i = 0; i < useableValueCount; i++)
{
_state.Values[autoDef.startIndex + i] = values[autoDef.subsetStart + i];
}
return(false);
}
}
/// <summary>
/// Tries to subscribe to an auto partner single contract, or throws an exception
/// if we don't support the contract. Also starts a 1-second poll that continues
/// until publishers send non-zero-length arrays.
/// </summary>
/// <param name="def"></param>
/// <param name="serviceInfo"></param>
private void subscribeAutoSingle(AutoDefinition def, ServiceInfoType serviceInfo)
{
// Check for each contract we know about and subscribe.
////////////////// Analog Sensor
if (serviceInfo.Contract.Equals(analog.Contract.Identifier))
{
var partnerPort = ServiceForwarder <analog.AnalogSensorOperations>(new Uri(serviceInfo.Service));
var notifyPort = new Port <analog.Replace>();
Activate(Arbiter.Choice(partnerPort.Subscribe(notifyPort, typeof(analog.Replace)),
delegate(SubscribeResponseType success)
{
MainPortInterleave.CombineWith(new Interleave(
new ExclusiveReceiverGroup(
Arbiter.Receive(true, notifyPort,
delegate(analog.Replace replace)
{ autoSubscribeNotificationHelper(def, new List <double>(1)
{
replace.Body.RawMeasurement
}, replace.Body.TimeStamp, false); }
)),
new ConcurrentReceiverGroup()));
},
delegate(Fault failure)
{
LogError("Fault while subscribing to auto partner", failure);
}));
}
////////////////// Analog Sensor Array
else if (serviceInfo.Contract.Equals(analogArray.Contract.Identifier))
{
var partnerPort = ServiceForwarder <analogArray.AnalogSensorOperations>(new Uri(serviceInfo.Service));
var notifyPort = new Port <analogArray.Replace>();
var pollPort = base.TimeoutPort(1000);
bool gotNonzeroState = false;
Activate(Arbiter.Receive(true, pollPort,
delegate(DateTime time)
{
if (gotNonzeroState == false)
{
Activate(Arbiter.Choice(partnerPort.Get(),
delegate(analogArray.AnalogSensorArrayState state) { notifyPort.Post(new analogArray.Replace(state)); },
delegate(Fault failure) { LogError("Vector got fault while trying to get polled state", failure); }));
TaskQueue.EnqueueTimer(TimeSpan.FromMilliseconds(1000.0), pollPort);
}
}));
Activate(Arbiter.Choice(partnerPort.Subscribe(notifyPort, typeof(analogArray.Replace)),
delegate(SubscribeResponseType success)
{
MainPortInterleave.CombineWith(new Interleave(
new ExclusiveReceiverGroup(
Arbiter.Receive(true, notifyPort,
delegate(analogArray.Replace replace)
{
if (replace.Body.Sensors.Count > 0)
{
gotNonzeroState = true;
}
autoSubscribeNotificationHelper(def,
new List <double>(from s in replace.Body.Sensors select s.RawMeasurement),
(replace.Body.Sensors.Count > 0 ? replace.Body.Sensors[0].TimeStamp : DateTime.Now), false);
})),
new ConcurrentReceiverGroup()));
},
delegate(Fault failure)
{
LogError("Fault while subscribing to auto partner", failure);
}));
}
////////////////// Contact Sensor
else if (serviceInfo.Contract.Equals(contact.Contract.Identifier))
{
var partnerPort = ServiceForwarder <contact.ContactSensorArrayOperations>(new Uri(serviceInfo.Service));
var notifyPort = new PortSet <contact.Replace, contact.Update>();
// Post an initial Replace notification to update the HW ID map (no, this is now done automatically)
//notifyPort.Post(new contact.Replace(RSUtils.ReceiveSync(TaskQueue, partnerPort.Get(), Params.defaultRecieveTimeout)));
var pollPort = base.TimeoutPort(1000);
bool gotNonzeroState = false;
Activate(Arbiter.Receive(true, pollPort,
delegate(DateTime time)
{
if (gotNonzeroState == false)
{
Activate(Arbiter.Choice(partnerPort.Get(),
delegate(contact.ContactSensorArrayState state) { notifyPort.Post(new contact.Replace(state)); },
delegate(Fault failure) { LogError("Vector got fault while trying to get polled state", failure); }));
TaskQueue.EnqueueTimer(TimeSpan.FromMilliseconds(1000.0), pollPort);
}
}));
Activate(Arbiter.Choice(partnerPort.Subscribe(notifyPort, typeof(contact.Replace), typeof(contact.Update)),
delegate(SubscribeResponseType success)
{
var contactHWIDMap = new Dictionary <int, int>();
MainPortInterleave.CombineWith(new Interleave(
new ExclusiveReceiverGroup(
Arbiter.Receive <contact.Replace>(true, notifyPort,
delegate(contact.Replace replace)
{
if (replace.Body.Sensors.Count > 0)
{
gotNonzeroState = true;
}
if (def.hwKeys.Count < replace.Body.Sensors.Count)
{
def.hwKeys.Capacity = replace.Body.Sensors.Count;
while (def.hwKeys.Count < replace.Body.Sensors.Count)
{
def.hwKeys.Add("");
}
}
// Refresh the HW ID map
contactHWIDMap.Clear();
for (int i = 0; i < replace.Body.Sensors.Count; i++)
{
contactHWIDMap.Add(replace.Body.Sensors[i].HardwareIdentifier, i);
//if (def.hwKeys[i] == null || def.hwKeys[i].Length <= 0)
def.hwKeys[i] = replace.Body.Sensors[i].Name;
}
autoSubscribeNotificationHelper(def,
new List <double>(from s in replace.Body.Sensors select(s.Pressed ? 1.0 : 0.0)),
(replace.Body.Sensors.Count > 0 ? replace.Body.Sensors[0].TimeStamp : DateTime.Now), true);
}),
Arbiter.Receive <contact.Update>(true, notifyPort,
delegate(contact.Update update)
{
gotNonzeroState = true;
try
{
int i = contactHWIDMap[update.Body.HardwareIdentifier];
autoSubscribeNotificationUpdateHelper(def, i, (update.Body.Pressed ? 1.0 : 0.0), update.Body.TimeStamp);
}
catch (KeyNotFoundException)
{
LogError("Vector got update for contact sensor hardware identifier " + update.Body.HardwareIdentifier + " and doesn't know about this hardware ID. Trying to fetch entire state.");
Activate(Arbiter.Choice(partnerPort.Get(),
delegate(contact.ContactSensorArrayState state) { notifyPort.Post(new contact.Replace(state)); },
delegate(Fault failure) { LogError("Vector got fault while trying to get contact state", failure); }));
}
})),
new ConcurrentReceiverGroup()));
},
delegate(Fault failure)
{
LogError("Fault while subscribing to auto partner", failure);
}));
}
else
{
throw new UnrecognizedContractException();
}
}
///// <summary>
///// </summary>
//private void subscribeAutos()
//{
// // Get the list of partners whose names start with "auto:"
// //var request = new partnerList.GetOperation()
// //{
// // Body = new GetRequestType(),
// // ResponsePort = new DsspResponsePort<PartnerListType>()
// //};
// //base.PartnerListManagerPort.Post(request);
// //Activate(Arbiter.Choice<PartnerListType, Fault>(
// // request.ResponsePort,
// // delegate(PartnerListType partners)
// // {
// // string autoPrefix = "Auto_";
// // List<PartnerType> autoPartners = new List<PartnerType>(
// // from partner in partners.PartnerList
// // where partner.Name.Name.StartsWith(autoPrefix)
// // select partner);
// // foreach (var p in autoPartners)
// // Console.WriteLine(p.Name);
// // // This method will take care of subscribing and updating state
// // //if (autoPartners.Count > 0)
// // // subscribeAutos2(autoPartners, autoPrefix.Length);
// // },
// // delegate(Fault failure)
// // {
// // LogError("Fault while getting partner list to subscribe to autos", failure);
// // }));
// //List<PartnerType> partnerList = new List<PartnerType>();
// //var responsePort = new DsspResponsePort<DefaultUpsertResponseType>();
// //Activate(Arbiter.MultipleItemReceive(responsePort, _state.AutoSubsets.Count,
// // delegate(ICollection<DefaultUpsertResponseType> successes, ICollection<Fault> failures)
// // {
// // foreach (Fault failure in failures)
// // LogError("Inserting partner failed", failure);
// subscribeAutos2();
// //}));
//}
/// <summary>
/// Subscribes to any services specified in the partner list to
/// automatically update the state.
/// Once we have a list of auto partners, this method creates AutoDefiniton
/// objects for each auto partner. It then waits for each partner to be
/// found, and calls subscribeAutoSingle to subscribe to each one.
/// </summary>
/// <param name="partList"></param>
/// <param name="removeFromName"></param>
private void subscribeAutos()
{
//foreach (var subset in _state.AutoSubsets)
//{
// PartnerType partner = new PartnerType() { Name = new XmlQualifiedName(subset.PartnerName, Contract.Identifier) };
// partnerList.Add(partner);
// //base.PartnerListManagerPort.Post(new partnerList.Upsert() { Body = partner, ResponsePort = responsePort });
//}
// Create AutoDefinition objects
autoDefs = new List <AutoDefinition>(_state.AutoSubsets.Count);
//int lastIndex = 0;
//List<string> allKeys = new List<string>();
List <double> allValues = new List <double>();
foreach (var subset in _state.AutoSubsets)
{
// Create AutoDefinition object and add to list
//string[] keys = part.Name.Name.Substring(removeFromName).Split('_');
//AutoSubset subset = _state.AutoSubsets.Find((s => part.Name.Name.Equals(s.PartnerName, StringComparison.OrdinalIgnoreCase)));
//if (subset != default(AutoSubset))
//{
//var newKeys = new List<string>();
//for (int i = 0; i < subset.startIndex; i++)
// newKeys.Add("");
//newKeys.AddRange(subset.keys);
//}
AutoDefinition autoDef = new AutoDefinition()
{
partnerName = subset.PartnerName,
startIndex = 0,
count = 0,
subsetStart = (subset == default(AutoSubset) ? 0 : subset.StartIndex),
subsetCount = (subset == default(AutoSubset) ? 0 : subset.Count),
keys = (subset == default(AutoSubset) ? new List <string>() : subset.Keys)
};
autoDefs.Add(autoDef);
}
foreach (var autoDef in autoDefs)
{
if (autoDef.subsetCount >= 1)
{
// Also build a default vector
var values = new List <double>(autoDef.subsetCount);
for (int i = 0; i < autoDef.subsetCount; i++)
{
values.Add(0.0);
}
modifyStateSubset(autoDef, values);
}
}
// Set values, and take keys provided in subset defs by user
//_state.SetAll(allValues, DateTime.Now);
//applyKeysFromSubsets();
//var upsertRespPort = new DsspResponsePort<DefaultUpsertResponseType>();
//PartnerListManagerPort.Post(new partnerList.Upsert() { Body = new PartnerType() { Name = new XmlQualifiedName("iRobotGenericDrive", Microsoft.Dss.ServiceModel.Dssp.Contract.Identifier) }, ResponsePort = upsertRespPort });
//Activate(Arbiter.Choice(upsertRespPort,
// delegate(DefaultUpsertResponseType suc)
// {
// //var queryRespPort = new DsspResponsePort<Microsoft.Dss.Schemas.PartnerListManager.QueryResponseType>();
// //PartnerListManagerPort.Post(new partnerList.Query() { Body = new partnerListS.QueryRequestType() { Name = new XmlQualifiedName("iRobotGenericDrive", Contract.Identifier) }, ResponsePort = queryRespPort });
// //Activate(Arbiter.Choice(queryRespPort,
// // delegate(partnerListS.QueryResponseType qsuc)
// // {
// // Console.WriteLine(qsuc.Partner.Name + " : " + qsuc.Partner.Contract + " : " + qsuc.Partner.Service);
// // },
// // delegate(Fault failure)
// // {
// // Console.WriteLine("Fault querying");
// // }));
// var partsRespPort = base.IssuePartnerSubscribe(new XmlQualifiedName("iRobotGenericDrive", Microsoft.Dss.ServiceModel.Dssp.Contract.Identifier));
// Activate(Arbiter.Choice(partsRespPort,
// delegate(PartnerListType partListResponse)
// {
// foreach (var partner in partListResponse.PartnerList)
// {
// Console.WriteLine(partner.Name + " : " + partner.Contract + " : " + partner.Service);
// }
// },
// delegate(Fault failure)
// {
// Console.WriteLine("Fault querying");
// }));
// },
// delegate(Fault fail)
// {
// Console.WriteLine("Fault upserting");
// }));
// Create the new vector state reflecting the keys
//List<double> allValues = new List<double>(from k in allKeys select 0.0);
//VectorState newState = new VectorState(allValues, allKeys, DateTime.Now);
//var responsePort = new DsspResponsePort<DefaultReplaceResponseType>();
//OperationsPort.Post(new Replace(newState, responsePort));
//Activate(Arbiter.Choice(responsePort,
// delegate(DefaultReplaceResponseType r) { },
// delegate(Fault f) { LogError("Fault while replacing initial vector state", f); }));
// Try to subscribe to compatible contracts with the AutoDefinition objects
foreach (var autoDef in autoDefs)
{
AutoDefinition myAutoDef = autoDef;
//Console.WriteLine("Waiting 10 seconds before subscribing to partner list...");
//Thread.Sleep(10000);
var partRespPort = base.IssuePartnerSubscribe(new XmlQualifiedName(myAutoDef.partnerName, Contract.Identifier));
//Console.WriteLine("Looking for " + myAutoDef.infoAsPartner.Name);
Activate(Arbiter.Choice(partRespPort,
delegate(PartnerListType partListResponse)
{
//Console.WriteLine("Found " + partListResponse.PartnerList.Count + " for " + myAutoDef.infoAsPartner.Name);
if (partListResponse.PartnerList.Count > 1)
{
LogWarning("More than one partner found for " + myAutoDef.partnerName);
}
else if (partListResponse.PartnerList.Count < 1)
{
LogWarning("No partners found for " + myAutoDef.partnerName);
}
foreach (var partner in partListResponse.PartnerList)
{
if (myAutoDef.infoAsConnected == null)
{
//Console.WriteLine("Searching for a contract for " + partner.Service);
Activate(Arbiter.Choice(RSUtils.FindCompatibleContract(TaskQueue, new Uri(partner.Service), new List <string>()
{
analog.Contract.Identifier, analogArray.Contract.Identifier, contact.Contract.Identifier
}),
delegate(ServiceInfoType serviceInfoResponse)
{
try
{
subscribeAutoSingle(myAutoDef, serviceInfoResponse);
myAutoDef.infoAsConnected = serviceInfoResponse;
LogInfo("Vector service " + base.ServiceInfo.Service + " subscribed auto update to " + serviceInfoResponse.Service + " with contract " + serviceInfoResponse.Contract);
}
catch (Exception e)
{
LogError("Exception while subscribing to auto partner", e);
}
},
delegate(Fault failure)
{
Exception e = RSUtils.ExceptionOfFault(failure);
if (e is NoContractFoundException)
{
LogError("Could not subscribe to auto partner " + myAutoDef.partnerName + ". Could not find a supported contract.");
}
else if (e is Exception)
{
LogError("Fault while searching for compatible contract", e);
}
}));
}
}
},
delegate(Fault failure)
{
Console.WriteLine("Fault for " + myAutoDef.partnerName);
LogError("Fault from subscription to partner list service", failure);
}));
//Thread.Sleep(2000);
}
}
