public void AddToIABuffer(List <Point> path)
{
NXTAction action;
Point oldPos = vehicule.Position;
Point direction = vehicule.Direction;
NXTCase currentCase = circuit.getCase(oldPos);
if (path.Count > 0)
{
if (!((currentCase.goThrough(new NXTAction(NXTMovement.STRAIGHT), direction) + oldPos).Equals(path[0]) ||
(currentCase.goThrough(new NXTAction(NXTMovement.INTER_LEFT), direction) + oldPos).Equals(path[0]) ||
(currentCase.goThrough(new NXTAction(NXTMovement.INTER_RIGHT), direction) + oldPos).Equals(path[0])))
{
this.buffer.Add(new NXTAction(NXTMovement.UTURN), false);
direction = Rotate90Clockwise(Rotate90Clockwise(direction));
}
}
foreach (Point p in path)
{
action = MovementToAction(circuit.getCase(oldPos), oldPos, direction, p, out direction);
if (circuit.hasPatient(p) && vehicule.Patients < vehicule.MAX_PATIENTS)
{
action.Action = NXTAction.TAKE;
}
else if (circuit.hasHopital(p) && vehicule.Patients > 0)
{
action.Action = NXTAction.DROP;
}
this.buffer.Add(action, false);
oldPos = p;
}
}
protected NXTAction MovementToAction(NXTCase currentCase, Point currentPosition, Point currentDirection, Point destination, out Point newDirection)
{
NXTAction outInstance = null;
newDirection = currentDirection;
Point deplacement = destination - currentPosition;
Point caseDirection = OrientationToDirection(currentCase.CaseOrientation);
//Si le robot doit marquer une pause
if (destination.Equals(currentPosition))
{
outInstance = new NXTAction(NXTMovement.PAUSE);
}
// Si destination derriere direction actuelle, demi tour
if (destination.Equals(currentPosition - currentDirection))
{
outInstance = new NXTAction(NXTMovement.UTURN);
}
// Sinon si virage ou tout droit envoyer straight
else if (currentCase.TypeCase == Case.STRAIGHT || currentCase.TypeCase == Case.VIRAGE)
{
outInstance = new NXTAction(NXTMovement.STRAIGHT);
if (currentCase.TypeCase == Case.VIRAGE)
{
newDirection = currentCase.goThrough(new NXTAction(NXTMovement.STRAIGHT), currentDirection);
}
}
// sinon (intersection)
else
{
//Console.WriteLine("[INTERSECTION] Deplacement= " + deplacement);
// Si la case est dans la meme direction que le vehicule
if (caseDirection.Equals(currentDirection))
{
if (deplacement.Equals(Rotate90Clockwise(currentDirection)))
{
outInstance = new NXTAction(NXTMovement.INTER_RIGHT);
newDirection = Rotate90Clockwise(currentDirection);
}
else
{
outInstance = new NXTAction(NXTMovement.INTER_LEFT);
newDirection = Rotate90AntiClockwise(currentDirection);
}
}
else if (caseDirection.Equals(Rotate90Clockwise(currentDirection))) // r = tout droit
{
if (deplacement.Equals(Rotate90AntiClockwise(currentDirection)))
{
outInstance = new NXTAction(NXTMovement.INTER_LEFT);
newDirection = Rotate90AntiClockwise(currentDirection);
}
else
{
outInstance = new NXTAction(NXTMovement.INTER_RIGHT);
}
}
else if (caseDirection.Equals(Rotate90AntiClockwise(currentDirection))) // l = tout droit
{
if (deplacement.Equals(Rotate90Clockwise(currentDirection)))
{
outInstance = new NXTAction(NXTMovement.INTER_RIGHT);
newDirection = Rotate90Clockwise(currentDirection);
}
else
{
outInstance = new NXTAction(NXTMovement.INTER_LEFT);
}
}
}
//Console.WriteLine(currentCase + " @ " + currentPosition + " -> " + destination + " _ " + DirectionToOrientation(newDirection) + "\nResult= " + outInstance);
return(outInstance);
}