WayTileNode FindClosestLoadedNode(GeoCoord point)
{
WayTileNode result = null;
double closest = double.MaxValue;
foreach (var node in _nodeLut.Values)
{
double distanceX = node.Point.Lon - point.Lon;
double distanceY = node.Point.Lat - point.Lat;
double distanceSqr = (distanceX * distanceX) + (distanceY * distanceY);
if (distanceSqr < closest)
{
closest = distanceSqr;
result = node;
}
}
return(result);
}
// Will auto-load connected tiles
public async Task <List <WayTileNode> > GetNodeConnections(WayTileNode node, bool updateVisitCount = true)
{
// If node is outside bounds load the linked tile
if (!node.Inside.HasValue)
{
await LoadTileAtPoint(node.Point);
// If node was a link to a new tile the id will now be replaced in the LUT, but the "real" node.
node = _nodeLut[node.Id];
}
if (updateVisitCount)
{
node.VisitCount++;
}
return(node.Conn.Select(id => _nodeLut[id]).ToList());
}
public async Task UpdateAgent(string agentId, AgentCommand command)
{
_logger.LogInformation($"Updating agent {agentId}");
string agentJson = await _blobStoreService.GetText($"agents/{agentId}.json").ConfigureAwait(false);
if (string.IsNullOrWhiteSpace(agentJson))
{
throw new ArgumentException($"Unknown agent: {agentId}");
}
var agent = JsonSerializer.Deserialize <MapAgent>(agentJson);
string pathJson = await _blobStoreService.GetText(BuildPathPath(agentId), throwIfNotFound : false).ConfigureAwait(false);
var oldPath = string.IsNullOrWhiteSpace(pathJson) ? new MapAgentPath() : JsonSerializer.Deserialize <MapAgentPath>(pathJson);
var newPath = new MapAgentPath();
long pathUnixMs = GeoMath.UnixMs();
long pathMs = 0;
double pathMeters = 0;
// Keep all points that are in the future - and one point in the past so we can interpolate the line segment
List <GeoCoord> visitedPoints = new List <GeoCoord>();
long msPrev = -1;
for (int i = 0; i < oldPath.PointAbsTimestampMs.Count; ++i)
{
bool pointIsInTheFuture = oldPath.PointAbsTimestampMs[i] >= pathUnixMs;
bool nextPointIsInTheFuture = i < oldPath.PointAbsTimestampMs.Count - 1 && oldPath.PointAbsTimestampMs[i + 1] >= pathUnixMs;
bool keepPoint = pointIsInTheFuture || nextPointIsInTheFuture;
if (keepPoint)
{
pathUnixMs = oldPath.PointAbsTimestampMs[i];
newPath.PointAbsTimestampMs.Add(pathUnixMs);
newPath.Points.Add(oldPath.Points[i]);
long msCurrent = oldPath.PointAbsTimestampMs[i];
if (msPrev != -1)
{
long segmentMs = msCurrent - msPrev;
pathMs += segmentMs;
}
msPrev = msCurrent;
}
else
{
visitedPoints.Add(oldPath.Points[i]);
}
}
// Make sure all visited lines are drawn by adding the points that are cut off.
if (newPath.Points.Count > 1)
{
visitedPoints.Add(newPath.Points[0]);
visitedPoints.Add(newPath.Points[1]);
}
await _mapCoverage.UpdateCoverage(visitedPoints).ConfigureAwait(false);
{
// If stuck, clear newPath and add a single point at or near a valid location. Then run update once from Cmd. A new valid path should now be written.
//newPath.Points.Clear();
//newPath.Points.Add(new GeoCoord(6.598364, 50.078052));
//newPath.PointAbsTimestampMs.Clear();
//newPath.PointAbsTimestampMs.Add(GeoMath.UnixMs());
}
if (newPath.Points.Count == 0)
{
// Path is out of date, nothing to keep. Reuse latest known position.
int oldCount = oldPath.Points.Count;
newPath.Points.Clear();
newPath.PointAbsTimestampMs.Clear();
newPath.Points.Add(oldPath.Points[oldCount - 1]);
newPath.Points.Add(oldPath.Points[oldCount - 1]);
newPath.PointAbsTimestampMs.Add(GeoMath.UnixMs());
}
// Start path here
var startPoint = newPath.Points.Last();
var startNode = await _worldGraph.GetNearbyNode(startPoint).ConfigureAwait(false);
List <GeoCoord> deadEndDebugSegments = new List <GeoCoord>();
List <GeoCoord> possibleWayoutDebugSegments = new List <GeoCoord>();
double prevBearing = double.MaxValue;
List <WayTileNode> conn = null;
WayTileNode prevNode = null;
WayTileNode node = startNode;
WayTileNode nextNode = null;
while (true)
{
conn = await _worldGraph.GetNodeConnections(node).ConfigureAwait(false);
var options = conn.Select(x => new Option {
Node = x
}).ToList();
// Don't go back if we can go forward
if (options.Count > 1)
{
options = options.Where(x => x.Node != prevNode).ToList();
}
foreach (var option in options)
{
var(deadEndFound, unexploredNodeFound, visitedNodesFound, unvisitedNodesFound, totalVisitCount, exploredSegments) = await _graphPeek.Peek(node, option.Node).ConfigureAwait(false);
double bearing = GeoMath.CalculateBearing(node.Point, option.Node.Point);
option.BearingDiff = Math.Abs(bearing - prevBearing);
option.IsDeadEnd = deadEndFound;
option.UnvisitedCount = unvisitedNodesFound;
option.VisitedCount = visitedNodesFound;
option.TotalVisitCount = totalVisitCount;
option.UnvisitedPct = visitedNodesFound == 0 ? 100 : (double)unvisitedNodesFound / visitedNodesFound;
// Absolute score - number of new nodes scores high.
option.Score = (unvisitedNodesFound - totalVisitCount) + _rnd.Next(0, 5); // Add a little randomness
// Percentage score - small dead ends scores high.
//option.Score = ((unvisitedNodesFound + visitedNodesFound) / ((double)totalVisitCount + 1)) + _rnd.NextDouble() * 0.1; // Add a little randomness
}
// Scores should reflect the direction with the most unexplored nodes. Higher = better.
// If nothing was explored score = unvisitedNodesFound + visitedNodesFound
// If everything was explored exactly once, score = 1. Will go below 1 when visitcount gets bigger than 1.
// If there are no detected unexplored areas nearby, prefer to go straight ahead to cover some ground quickly.
bool preferStraight = false;
bool onlyAlreadyExploredOptions = !options.Any(x => x.Score > 1.0);
if (onlyAlreadyExploredOptions)
{
long minVisitCount = options.Min(x => x.Node.VisitCount ?? 0);
bool allEqual = options.All(x => x.Node.VisitCount == minVisitCount);
if (allEqual)
{
preferStraight = onlyAlreadyExploredOptions && _rnd.NextDouble() < 0.9;
}
else
{
// All are already explored, but some have higher count than others. Pick from nodes with lowest count.
options = options.Where(x => x.Node.VisitCount == minVisitCount).ToList();
}
}
else
{
// Pick best score
options = options.OrderByDescending(x => x.Score).ToList();
}
// Most of the time select element 0, but sometimes randomly select another
int maxIdx = options.Count > 1 ? 1 : 0;
int selectedIdx = _rnd.NextDouble() < 0.75 ? 0 : _rnd.Next(0, maxIdx + 1);
nextNode = options[selectedIdx].Node;
prevNode = node;
node = nextNode;
prevBearing = GeoMath.CalculateBearing(prevNode.Point, node.Point);
double segmentMeters = GeoMath.MetersDistanceTo(prevNode.Point, node.Point);
long segmentMs = (long)((segmentMeters / agent.MetersPerSecond) * 1000);
newPath.Points.Add(node.Point);
pathUnixMs += segmentMs;
newPath.PointAbsTimestampMs.Add(pathUnixMs);
pathMeters += segmentMeters;
pathMs += segmentMs;
if (pathMs >= 60 * 10 * 1000) // 10 minutes per path
{
break;
}
}
//if (deadEndDebugSegments.Count > 0)
//{
// string geo = GeoJsonBuilder.Segments(deadEndDebugSegments);
// await _blobStoreService.StoreText($"debug/skipped-deadends/deadends-{deadEndDebugSegments.Count}-{DateTime.UtcNow.Ticks}.json", geo).ConfigureAwait(false);
//}
//if (possibleWayoutDebugSegments.Count > 0)
//{
// string geo = GeoJsonBuilder.Segments(possibleWayoutDebugSegments);
// await _blobStoreService.StoreText($"debug/possible-way-out/wayout-{possibleWayoutDebugSegments.Count}-{DateTime.UtcNow.Ticks}.json", geo).ConfigureAwait(false);
//}
await _worldGraph.StoreUpdatedVisitCounts().ConfigureAwait(false);
newPath.PathMs = pathMs;
newPath.TileIds = _worldGraph.GetLoadedTiles().Select(tile => tile.Id).ToList();
newPath.EncodedPolyline = GooglePolylineConverter.Encode(newPath.Points);
string newPathJson = JsonSerializer.Serialize(newPath);
await _blobStoreService.StoreText(BuildPathPath(agentId), newPathJson, overwriteExisting : true).ConfigureAwait(false);
//string geoJson = GeoJsonBuilder.AgentPath(newPath);
//await _blobStoreService.StoreText(BuildGeoJsonPathPath(agentId), geoJson, overwriteExisting: true).ConfigureAwait(false);
var clientPath = new AgentClientPath
{
MsStart = newPath.PointAbsTimestampMs.First(),
MsEnd = newPath.PointAbsTimestampMs.Last(),
EncodedPolyline = newPath.EncodedPolyline
};
string clientPathJson = JsonSerializer.Serialize(clientPath);
await _blobStoreService.StoreText(BuildClientPathPath(agentId), clientPathJson, overwriteExisting : true).ConfigureAwait(false);
await TryTakeSelfie(oldPath.Points).ConfigureAwait(false);
}
public async Task <(bool deadEndFound, bool unexploredNodeFound, long visitedNodesFound, long unvisitedNodesFound, long totalVisitCount, List <GeoCoord> explored)> Peek(WayTileNode root, WayTileNode first)
{
_visitedList.Clear();
_pendingList.Clear();
_visitedList.Add(root);
_pendingList.Add(first);
var explored = new List <GeoCoord>();
const int MaxSteps = 200;
bool unexploredNodeFound = false;
long visitedNodesFound = 0;
long unvisitedNodesFound = 0;
long totalVisitCount = 0;
int stepCount = 0;
while (true)
{
if (_pendingList.Count == 0)
{
break;
}
var current = _pendingList.First();
if (current.VisitCount == 0)
{
unexploredNodeFound = true;
}
_pendingList.RemoveAt(0);
bool alreadyTested = _visitedList.Any(x => x.Id == current.Id);
if (alreadyTested)
{
continue;
}
stepCount++;
if (stepCount >= MaxSteps)
{
break;
}
var connections = await _worldGraph.GetNodeConnections(current, updateVisitCount : false).ConfigureAwait(false);
_pendingList.AddRange(connections);
_visitedList.Add(current);
foreach (var conn in connections)
{
visitedNodesFound += conn.VisitCount > 0 ? 1 : 0;
unvisitedNodesFound += conn.VisitCount == 0 ? 1 : 0;
totalVisitCount += conn.VisitCount ?? 0;
explored.Add(current.Point);
explored.Add(conn.Point);
}
}
bool deadEndFound = stepCount < MaxSteps;
return(deadEndFound, unexploredNodeFound, visitedNodesFound, unvisitedNodesFound, totalVisitCount, explored);
}
