// logic: find waypoint and mark all previous waypoints flown
// insert current pos as first waypoint
// EXCEPTION; if it would by-pass final approach or IAF to FAF we want to go to the leg
public void DirectToWaypoint(George george, WayPoint awp, Vessel vessel, VesselData vdata)
{
Deb.Log("DirectToWaypoint: direct to {0}", next);
if (next != null)
next.ClearFlag(WPFlag.Active);
bool activateLeg = false;
if (awp.HasFlag(WPFlag.RW) || awp.HasFlag(WPFlag.FAF))
{
activateLeg = true;
}
// should not happen, but just in case
if (course.Count == 0)
{
Deb.Err("DirectToWaypoint: flight plan has no waypoint");
return;
}
// need to copy all waypoints and prune out any "current" ones that are in the middle
var newCourse = new List<WayPoint>();
int ndx = -1;
int j = 0; // in case we skip any
WayPoint prev_wp = null;
WayPoint prevToDirect = null;
for (int i = 0; i < course.Count; i++)
{
var wp = course[i];
if (System.Object.ReferenceEquals(wp, awp))
{
// may have skipped so use index of destination
prevToDirect = prev_wp;
ndx = j;
}
if (i > 0 && wp.HasFlag(WPFlag.Current))
continue;
var newWp = wp.Clone();
if (prev_wp != null)
{
prev_wp.next= newWp;
}
prev_wp = newWp;
newCourse.Add(newWp);
j += 1;
}
if (ndx < 0)
{
Deb.Err("DirectToWaypoint: wp {0} not found.", awp);
return;
}
// everything before active wp should be marked flown and inactive
for (int i = 0; i < ndx; i++)
{
var wp = newCourse[i];
wp.SetFlag(WPFlag.Flown);
wp.ClearFlag(WPFlag.Active);
}
// everything after active wp should be marked inactive and NOT flown
for (int i = ndx; i < newCourse.Count; i++)
{
var wp = newCourse[i];
wp.ClearFlag(WPFlag.Flown);
wp.ClearFlag(WPFlag.Active);
}
if (activateLeg)
{
// ignore our position
if (prevToDirect == null)
{
// should not happen, but just in case
Deb.Err("DirectToWaypoint: no previous waypoint for leg!");
return;
}
course = newCourse;
next = course[ndx];
prev = prevToDirect.Clone();
prev.next = next;
next.SetFlag(WPFlag.Active);
}
else
{
// insert a fake waypoint for our current position
course = newCourse;
var current = position.Clone();
current.SetFlag(WPFlag.Vertical);
current.SetFlag(WPFlag.Flown);
current.SetFlag(WPFlag.Current);
next = course[ndx];
course.Insert(ndx, current);
// don't set old_prev to link to current: old_prev.next = current;
prev = current;
prev.next = next;
next.SetFlag(WPFlag.Active);
}
// need fresh status
courseStatus = new CourseStatus();
UpdateWayPointValues(planet);
UpdateData(george, vessel, vdata);
george.WayPointSequenced(next);
}