public bool DoSearchBuoy(Color color, IShape containerZone = null)
{
List <RealPoint> pts = AllDevices.LidarGround.GetPoints();
pts = pts.Where(o => GameBoard.IsInside(o, 50)).ToList();
if (containerZone != null)
{
pts = pts.Where(o => containerZone.Contains(o)).ToList();
}
List <List <RealPoint> > groups = pts.GroupByDistance(80);
List <Tuple <Circle, Color> > buoys = new List <Tuple <Circle, Color> >();
bool output = false;
for (int i = 0; i < groups.Count; i++)
{
if (groups[i].Count > 4)
{
RealPoint center = groups[i].GetBarycenter();
double var = Math.Sqrt(groups[i].Average(p => p.Distance(center) * p.Distance(center))) * 2;
buoys.Add(Tuple.Create(new Circle(center, var), var > 35 ? Buoy.Green : Buoy.Red));
}
}
if (buoys.Count > 0 && buoys.Exists(b => b.Item2 == color))
{
Circle buoy = buoys.OrderBy(b => b.Item1.Distance(Robots.MainRobot.Position.Coordinates)).First(b => b.Item2 == color).Item1;
RealPoint entryFrontPoint = GetEntryFrontPoint();
RealPoint entryBackPoint = GetEntryBackPoint();
AngleDelta bestAngle = 0;
double bestError = int.MaxValue;
for (AngleDelta i = 0; i < 360; i++)
{
Segment inter = new Segment(entryBackPoint.Rotation(i, Robots.MainRobot.Position.Coordinates), buoy.Center);
double error = inter.Distance(entryFrontPoint.Rotation(i, Robots.MainRobot.Position.Coordinates));
if (error < bestError)
{
bestError = error;
bestAngle = i;
}
}
bestAngle = -bestAngle.Modulo();
if (bestAngle < 0)
{
Robots.MainRobot.PivotRight(-bestAngle);
}
else
{
Robots.MainRobot.PivotLeft(bestAngle);
}
DoGrabOpen();
int dist = (int)GetEntryFrontPoint().Distance(buoy.Center) + 50;
Robots.MainRobot.Move(dist);
DoSequencePickupColor(color); // TODO Détecter la couleur au lidar ?
//DoSequencePickup();// ... ou pas...
DoGrabClose();
Robots.MainRobot.Move(-dist);
//DoSearchBuoy();
output = true;
if (Robots.Simulation)
{
GameBoard.RemoveVirtualBuoy(buoy.Center);
}
}
else
{
output = false;
}
return(output);
}
