stacks.jl

start_state_domain = SearchState_t(mp.q_start, [mp.q_start], VAMP.make_packed_view_volume(UnionOfGeometry(starting_visibility__Fworld)))

path_segments = [[start_state_domain]]
goal_stack = Any[
    PointEpsilonGoal(convert(SE2_MPState_t, goal.q), goal.ϵ)]

execute_move_prefix = false

struct CoverageGoal{T}
    region::T
end

while true
    @show length(goal_stack)
    this_goal = pop!(goal_stack)

    start_state = prune_visible_region_state(path_segments[end][end])

    if typeof(this_goal) <: PointEpsilonGoal
        move_alg = make_move_alg( # Create a heuristic for `this_goal`.Setup search problem.
          mp,
          this_goal,
          (s_to, s_fro) -> distance(s_to.q, s_fro.q) + move_safe_preference * vsafe_violations(s_to, s_fro), # this is the edge cost function
          move_vis_successors,
          (s_to, s_fro) -> mp.q_valid_test(s_to.q) # This is a relaxed planner. Only cares about collisions. Assuming graph edges are short, so we only need to check the s_to, not the segment between s_from and s_to
        )
        
        # Run PathVis
        move_search_result = move_search!(move_alg, start_state)

        # Compute the prefix that could be executed, and the one that violates the visibility constraint.
        move_process_result = process_move_path(move_alg, path_segments[end][end]; prune=false)

        if move_process_result.metadata.reaches_goal
            push!(path_segments, move_process_result.move_path_to_add)
            println("succeeded in reaching goal.")
            break
        end

        println("didn't reach goal in vsafe way")
        push!(goal_stack, this_goal)    # we didn't get to the goal, so we have to put it back on the stack.
        swept_volume_uncovered = visibility_violations_path(move_process_result.move_path_unsafe)
        push!(goal_stack, CoverageGoal(swept_volume_uncovered)) # this is our subgoal. if we saw this, then we would have been able to execute the plan to get to `this_goal`
        continue
    end

    if typeof(this_goal) <: CoverageGoal
        this_goal_original = this_goal
        # maybe we saw some of this already serendipitously.
        this_goal_region = difference(this_goal_original.region, start_state.visible_region)

        if length(this_goal_original.region) != length(this_goal_region)
            println("Saw some of coverage goal already.")
        end

        # I don't handle all the cases yet, so sometimes a goal gets pushed on that isn't actually a goal.
        # or maybe everything was seen serendipitously
        if length(this_goal_region) == 0; warn("this_goal is empty"); continue end

        timeouts = @NT(improvement=2.0, failure=600.0)
        
        # run PathVis
        last_search = explore_search_result = explore_search(start_state, this_goal_region, timeouts, Val{true}; log_header="\n\nexplore relaxed\n")
        explore_process_result = process_explore_path(explore_search_result.explore_alg, start_state)
        # get the part that we could execute, and the part that we could execute if only we saw some more stuff.
        (explore_safe, explore_unsafe) = vsafe_split_path(explore_process_result.explore_path_to_add);

        # how much did we actually see, and how much is left to see?
        this_goal_remaining = difference(this_goal_region, CoveragePath(unlift_q(explore_safe)))

        # execute, and add the left-overs to the goal stack.
        # also have option to not execute and add whole coverage back onto stack.
        push!(path_segments, explore_safe)
        if length(this_goal_remaining) > 0
            push!(goal_stack, CoverageGoal(this_goal_remaining))
            println("new coverage goal with n $(length(this_goal_remaining))")
        end

        # if there was some stuff we couldn't execute, compute the region we would need to see in order to enable execution.
        if length(explore_unsafe) > 0
            need_to_see = visibility_violations_path(explore_unsafe)
            push!(goal_stack, CoverageGoal(need_to_see))
        end

    end
end