Navigation Messages

use horus::prelude::*;

// Create navigation goal
let target = Pose2D::new(5.0, 3.0, 1.57);  // x, y, theta
let goal = NavGoal::new(target, 0.1, 0.05);   // 10cm position, 0.05rad angle tolerance

// With timeout and priority
let goal = NavGoal::new(target, 0.1, 0.05)
    .with_timeout(30.0)  // 30 second timeout
    .with_priority(0);   // Highest priority

// Check if goal reached
let current_pose = Pose2D::new(5.05, 3.02, 1.55);
if goal.is_reached(&current_pose) {
    println!("Goal reached!");
}

// Check position and orientation separately
if goal.is_position_reached(&current_pose) {
    println!("Position reached, adjusting orientation...");
}
if goal.is_orientation_reached(&current_pose) {
    println!("Orientation reached");
}

use horus_library::messages::navigation::GoalStatus;

let status = GoalStatus::Active;

match status {
    GoalStatus::Pending => println!("Waiting to start"),
    GoalStatus::Active => println!("Moving to goal"),
    GoalStatus::Succeeded => println!("Goal reached!"),
    GoalStatus::Aborted => println!("Navigation failed"),
    GoalStatus::Cancelled => println!("Goal cancelled by user"),
    GoalStatus::Preempted => println!("Higher priority goal received"),
    GoalStatus::TimedOut => println!("Goal timed out"),
}

use horus_library::messages::navigation::{GoalResult, GoalStatus};

// Create success result
let result = GoalResult::new(42, GoalStatus::Succeeded);

// Create failure result with error
let error_result = GoalResult::new(42, GoalStatus::Aborted)
    .with_error("Obstacle blocking path");

// Update progress
let mut in_progress = GoalResult::new(42, GoalStatus::Active);
in_progress.distance_to_goal = 2.5;  // 2.5m remaining
in_progress.eta_seconds = 5.0;       // 5s estimated
in_progress.progress = 0.75;         // 75% complete

println!("Goal {}: status={}, {:.1}m to go, ETA {:.1}s",
    in_progress.goal_id, in_progress.status,
    in_progress.distance_to_goal, in_progress.eta_seconds);

use horus::prelude::*;
use horus_library::messages::navigation::Waypoint;

// Simple waypoint
let wp = Waypoint::new(Pose2D::new(1.0, 2.0, 0.0));

// Waypoint with velocity profile
let wp = Waypoint::new(Pose2D::new(1.0, 2.0, 0.0))
    .with_velocity(Twist::new_2d(0.5, 0.0));  // 0.5 m/s forward

// Waypoint requiring stop (e.g., for pickup)
let stop_wp = Waypoint::new(Pose2D::new(3.0, 4.0, 1.57))
    .with_stop();

// Access properties
println!("Position: ({:.1}, {:.1})", wp.pose.x, wp.pose.y);
println!("Curvature: {:.3}", wp.curvature);
println!("Stop required: {}", wp.stop_required);  // 0 = no, 1 = yes

use horus::prelude::*;
use horus_library::messages::navigation::Waypoint;

// Create empty path
let mut path = NavPath::new();

// Add waypoints
path.add_waypoint(Waypoint::new(Pose2D::new(0.0, 0.0, 0.0)))?;
path.add_waypoint(Waypoint::new(Pose2D::new(1.0, 0.0, 0.0)))?;
path.add_waypoint(Waypoint::new(Pose2D::new(2.0, 1.0, 0.785)))?;

// Get path info
println!("Waypoints: {}", path.waypoint_count);
println!("Total length: {:.2}m", path.total_length);

// Get valid waypoints slice
let waypoints = path.waypoints();

// Find closest waypoint to current position
let current = Pose2D::new(1.2, 0.3, 0.0);
if let Some(idx) = path.closest_waypoint_index(&current) {
    println!("Closest waypoint: {}", idx);
}

// Calculate progress along path
let progress = path.calculate_progress(&current);
println!("Path progress: {:.0}%", progress * 100.0);

use horus::prelude::*;

// Create path plan from waypoints
let waypoints = &[
    [0.0f32, 0.0, 0.0],      // [x, y, theta]
    [1.0, 0.0, 0.0],
    [2.0, 0.5, 0.5],
    [3.0, 1.0, 0.785],
];
let goal = [3.0f32, 1.0, 0.785];

let plan = PathPlan::from_waypoints(waypoints, goal);

// Or build incrementally
let mut plan = PathPlan::new();
plan.add_waypoint(0.0, 0.0, 0.0);
plan.add_waypoint(1.0, 0.5, 0.2);
plan.goal_pose = [1.0, 0.5, 0.2];

println!("Path has {} waypoints", plan.waypoint_count);
println!("Empty: {}", plan.is_empty());

// Access individual waypoints
if let Some(wp) = plan.get_waypoint(0) {
    println!("First waypoint: x={}, y={}, theta={}", wp[0], wp[1], wp[2]);
}

use horus::prelude::*;

// Create 10m x 10m map at 5cm resolution
let origin = Pose2D::origin();
let mut grid = OccupancyGrid::new(
    200,   // width (200 * 0.05 = 10m)
    200,   // height
    0.05,  // resolution (5cm per cell)
    origin
);

// Set occupancy (-1=unknown, 0=free, 100=occupied)
grid.set_occupancy(100, 100, 0);    // Free cell
grid.set_occupancy(150, 150, 100);  // Occupied cell (obstacle)

// World to grid coordinate conversion
if let Some((gx, gy)) = grid.world_to_grid(5.0, 5.0) {
    println!("World (5.0, 5.0) -> Grid ({}, {})", gx, gy);
}

// Grid to world coordinate conversion
if let Some((x, y)) = grid.grid_to_world(100, 100) {
    println!("Grid (100, 100) -> World ({:.2}, {:.2})", x, y);
}

// Check cell status
let test_x = 7.5;
let test_y = 7.5;
if grid.is_free(test_x, test_y) {
    println!("({}, {}) is free", test_x, test_y);
} else if grid.is_occupied(test_x, test_y) {
    println!("({}, {}) is occupied", test_x, test_y);
}

// Get occupancy value
if let Some((gx, gy)) = grid.world_to_grid(test_x, test_y) {
    if let Some(value) = grid.get_occupancy(gx, gy) {
        println!("Occupancy: {}", value);
    }
}

use horus::prelude::*;

// Create occupancy grid first
let grid = OccupancyGrid::new(200, 200, 0.05, Pose2D::origin());

// Create costmap with inflation radius
let costmap = CostMap::from_occupancy_grid(grid, 0.55);  // 55cm inflation

// Get cost at world coordinates (0-255, 253=lethal)
if let Some(cost) = costmap.cost(5.0, 5.0) {
    if cost >= costmap.lethal_cost {
        println!("Position is in obstacle!");
    } else {
        println!("Cost: {}", cost);
    }
}

// Access underlying grid
println!("Map size: {}x{}",
    costmap.occupancy_grid.width,
    costmap.occupancy_grid.height);

use horus_library::messages::navigation::VelocityObstacle;

let obstacle = VelocityObstacle {
    position: [3.0, 2.0],        // [x, y]
    velocity: [0.5, 0.0],        // Moving at 0.5 m/s in x
    radius: 0.3,                  // 30cm radius
    time_horizon: 5.0,            // 5 second prediction
    obstacle_id: 1,
};

println!("Obstacle {} at ({:.1}, {:.1}) moving at ({:.1}, {:.1})",
    obstacle.obstacle_id,
    obstacle.position[0], obstacle.position[1],
    obstacle.velocity[0], obstacle.velocity[1]);

use horus_library::messages::navigation::{VelocityObstacles, VelocityObstacle};

let mut obstacles = VelocityObstacles::default();
obstacles.obstacles[0] = VelocityObstacle {
    position: [2.0, 1.0],
    velocity: [0.3, 0.1],
    radius: 0.25,
    time_horizon: 3.0,
    obstacle_id: 1,
};
obstacles.count = 1;

println!("Tracking {} dynamic obstacles", obstacles.count);

use horus::prelude::*;
use horus_library::messages::navigation::{GoalResult, GoalStatus, Waypoint};

struct NavigationNode {
    goal_sub: Topic<NavGoal>,
    odom_sub: Topic<Odometry>,
    map_sub: Topic<OccupancyGrid>,
    path_pub: Topic<NavPath>,
    result_pub: Topic<GoalResult>,
    current_goal: Option<NavGoal>,
    current_path: Option<NavPath>,
}

impl Node for NavigationNode {
    fn name(&self) -> &str { "Navigation" }

    fn tick(&mut self) {
        // Check for new goals
        if let Some(goal) = self.goal_sub.recv() {
            self.current_goal = Some(goal);
            // Plan path to goal
            if let Some(map) = self.map_sub.recv() {
                let path = self.plan_path(&goal, &map);
                self.path_pub.send(path);
                self.current_path = Some(path);
            }
        }

        // Check goal progress
        if let (Some(goal), Some(odom)) = (&self.current_goal, self.odom_sub.recv()) {
            let current_pose = odom.pose;  // Odometry.pose is Pose2D

            if goal.is_reached(&current_pose) {
                let result = GoalResult::new(goal.goal_id, GoalStatus::Succeeded);
                self.result_pub.send(result);
                self.current_goal = None;
            } else {
                let mut result = GoalResult::new(goal.goal_id, GoalStatus::Active);
                result.distance_to_goal = goal.target_pose.distance_to(&current_pose);
                if let Some(path) = &self.current_path {
                    result.progress = path.calculate_progress(&current_pose);
                }
                self.result_pub.send(result);
            }
        }
    }
}

impl NavigationNode {
    fn plan_path(&self, _goal: &NavGoal, _map: &OccupancyGrid) -> NavPath {
        // Path planning implementation (A*, RRT*, etc.)
        NavPath::new()
    }
}