use alloc::vec::Vec;
use crate::corridor::AdmissibleTangents;
use crate::decomp::sweep::decompose_into;
use crate::error::PathPlanError;
use crate::funnel::cell_search::search;
use crate::funnel::string_pull::string_pull;
use crate::geom::point::Point2D;
use crate::router::scratch::Scratch;
use crate::router::RouteRequest;
use crate::spline::containment::clamp_and_repair;
use crate::spline::solver::solve_tangents;
use crate::spline::CubicBezierSegment;
pub fn route_smooth(
scratch: &mut Scratch,
req: &RouteRequest,
) -> Result<Vec<CubicBezierSegment>, PathPlanError> {
req.validate()?;
let free_space = req.free_space()?;
let clearance = req.clearance();
let Scratch { decomp, search: search_scratch, .. } = scratch;
decompose_into(
decomp,
&free_space,
req.config.margin,
&[req.start.point.x, req.goal.point.x],
)?;
let corridor = search(decomp, req.start.point, req.goal.point, search_scratch)?;
let path = string_pull(decomp, &corridor, req.start.point, req.goal.point)?;
let admissible = AdmissibleTangents::build(decomp, &corridor.cells, &path.knots)?;
let seeds = seed_tangents(&path.knots, &req.start, &req.goal);
let mut tangents = solve_tangents(&seeds, req.config.tangent_bias);
clamp_and_repair(
&admissible,
&path.knots,
&mut tangents,
&clearance,
req.config.max_repair_iters,
req.config.repair_dampening,
);
admissible.control_points(&path.knots, &tangents, &clearance)
}
fn seed_tangents(
knots: &[Point2D],
start: &crate::router::PortConstraint,
goal: &crate::router::PortConstraint,
) -> Vec<Point2D> {
let n = knots.len();
let mut seeds: Vec<Point2D> = Vec::with_capacity(n);
for i in 0..n {
let incoming = if i > 0 { knots[i] - knots[i - 1] } else { Point2D::ZERO };
let outgoing = if i + 1 < n { knots[i + 1] - knots[i] } else { Point2D::ZERO };
let span = if incoming.length() >= outgoing.length() { incoming } else { outgoing };
let requested = match i {
0 => start.direction,
j if j + 1 == n => goal.direction,
_ => None,
};
seeds.push(match requested.and_then(|d| d.normalize()) {
Some(unit) => unit * (span.length() / 3.0),
None => span / 3.0,
});
}
seeds
}