/**
* Executes the trajectory
*
* @param Coordinate start the coordinate to move from
*
* @return void
*/
public void Execute(Coordinate start)
{
Trajectory t = new Trajectory(start, this.end, this.samples);
List<Coordinate> traj = t.Compute();
Console.WriteLine("Trajectory computed");
OutputThread o = new OutputThread(traj, this.period, PYT.Properties.Settings.Default.OutgoingHost, PYT.Properties.Settings.Default.OutgoingPort, ref this.iThread);
this.oThread = new Thread(new ThreadStart(o.process));
Console.WriteLine("Output initialised");
this.oThread.Start();
Console.WriteLine("Waiting for Output thread to start...");
while (!this.oThread.IsAlive);
Console.WriteLine("Execution commenced");
}
/**
* Process method to run the actual delivery
*/
public override void process()
{
Coordinate prevCoord = null;
Coordinate lastReceived = null;
int threshold = 25;
bool quit = false;
foreach (Coordinate coord in this.trajectory)
{
//if (prevCoord != null)
if (prevCoord != null)
{
lastReceived = Coordinate.fromString(coord.getCoordinateNames(), this.iThread.getLastReceived());
foreach (string c in coord.getCoordinateNames())
{
if (
(lastReceived.getCoordinate(c) > prevCoord.getCoordinate(c) + threshold) ||
(lastReceived.getCoordinate(c) < prevCoord.getCoordinate(c) - threshold)
)
{
Console.WriteLine("WARNING: Value out of range in " + c + ", " + lastReceived.getCoordinate(c).ToString() + " outside " + prevCoord.getCoordinate(c).ToString() + " with threshold " + threshold.ToString() + ". Abort!");
quit = true;
}
}
}
prevCoord = coord;
if (quit)
{
break;
}
Socket sendSocket = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);
EndPoint sendEndPoint = new IPEndPoint(IPAddress.Parse(this.host), this.port);
byte[] buffer = coord.toBytes();
sendSocket.SendTo(buffer, buffer.Length, SocketFlags.None, sendEndPoint);
Thread.Sleep(this.period);
}
if (!quit)
{
// successful execution
Console.WriteLine("Execution completed");
}
else if (this.permitReverse && lastReceived != null)
{
// something got in the way
Console.WriteLine("Inconsistency detected, reversing");
Thread.Sleep(this.period * 10);
Trajectory reverse = new Trajectory(Coordinate.fromString(this.trajectory[0].getCoordinateNames(), this.iThread.getLastReceived()), this.trajectory[0]);
this.trajectory = reverse.Compute();
Console.WriteLine("Reverse trajectory has " + this.trajectory.Count.ToString() + " steps");
this.permitReverse = false;
this.process();
}
else
{
Console.WriteLine("Inconsistency detected and unable to reverse, abort!");
}
}