public WaypointTask(MyGridProgram program, IAutopilot pilotSubsystem, Waypoint waypoint, AvoidObstacleMode avoidObstacleMode = AvoidObstacleMode.SmartEnter, IMyTerminalBlock moveReference = null)
        {
            Autopilot          = pilotSubsystem;
            Program            = program;
            Destination        = waypoint;
            IntelScratchpad    = new List <IFleetIntelligence>(64);
            PositionScratchpad = new List <Vector3>(64);
            ObstacleMode       = avoidObstacleMode;
            MoveReference      = moveReference;
            Cleared            = false;

            WaypointHelper.PlotPath(new Dictionary <MyTuple <IntelItemType, long>, IFleetIntelligence>(), TimeSpan.FromSeconds(1), Autopilot, Destination, IntelScratchpad, PositionScratchpad, Program, ObstacleMode);
        }
        public void Do(Dictionary <MyTuple <IntelItemType, long>, IFleetIntelligence> IntelItems, TimeSpan canonicalTime, Profiler profiler)
        {
            if (!Cleared)
            {
                Autopilot.Clear();
                Cleared = true;
            }
            Autopilot.Move(ObstacleMode == AvoidObstacleMode.DoNotAvoid ? Destination.Position : WaypointHelper.PlotPath(IntelItems, canonicalTime, Autopilot, Destination, IntelScratchpad, PositionScratchpad, Program, ObstacleMode));
            Autopilot.Turn(Destination.Direction);
            Autopilot.Spin(Destination.DirectionUp);
            Autopilot.Drift(Destination.Velocity);
            Autopilot.SetMaxSpeed(Destination.MaxSpeed);
            Autopilot.Reference = MoveReference;

            if (canonicalTime == TimeSpan.Zero)
            {
                Autopilot.AtWaypoint(Destination);
                Autopilot.Clear();
            }
        }