#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from rclpy.qos import qos_profile_sensor_data, QoSProfile, DurabilityPolicy
from std_msgs.msg import String
from geometry_msgs.msg import Twist, PoseStamped, PoseWithCovarianceStamped
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry, OccupancyGrid
from nav2_msgs.action import NavigateToPose
from action_msgs.msg import GoalStatus
import math

class ParkingControllerNode(Node):
    def __init__(self):
        super().__init__('parking_controller_node')

        # --- [Parameters] ---
        # Map waypoint to navigate to before starting ArUco parking
        self.declare_parameter('parking_waypoint_x', 0.0)      # Edit this to match map coordinate
        self.declare_parameter('parking_waypoint_y', 0.0)      # Edit this to match map coordinate
        self.declare_parameter('parking_waypoint_yaw', 0.0)    # Edit this to match map coordinate
        self.declare_parameter('parking_frame', 'map')
        
        self.declare_parameter('target_distance', 0.5)         # Stop 0.5m in front of marker
        self.declare_parameter('kp_linear', 0.25)
        self.declare_parameter('kp_bearing', 0.8)
        self.declare_parameter('kp_yaw', 0.4)

        self.waypoint_x = self.get_parameter('parking_waypoint_x').value
        self.waypoint_y = self.get_parameter('parking_waypoint_y').value
        self.waypoint_yaw = self.get_parameter('parking_waypoint_yaw').value
        self.waypoint_frame = self.get_parameter('parking_frame').value
        self.waypoint_active = False # Tracks if user has set a valid parking waypoint
        
        self.target_dist = self.get_parameter('target_distance').value
        self.kp_linear = self.get_parameter('kp_linear').value
        self.kp_bearing = self.get_parameter('kp_bearing').value
        self.kp_yaw = self.get_parameter('kp_yaw').value

        # Declare patrol waypoints as parameter (flat list of x, y, yaw)
        # No default coordinates: these were tuned for the old indoor map and are
        # meaningless (possibly unsafe) on the new Gunsan site map. Patrol falls back
        # to "go home and scan" until the user supplies real waypoints for the new
        # map via this parameter (see send_patrol_goal/advance_patrol below, which
        # already append the home position regardless of this list's length).
        default_patrol = []
        self.declare_parameter('patrol_waypoints', default_patrol)
        self.patrol_waypoints_raw = self.get_parameter('patrol_waypoints').value
        self.patrol_waypoints = []
        for i in range(0, len(self.patrol_waypoints_raw), 3):
            if i + 2 < len(self.patrol_waypoints_raw):
                self.patrol_waypoints.append({
                    'x': self.patrol_waypoints_raw[i],
                    'y': self.patrol_waypoints_raw[i+1],
                    'yaw': self.patrol_waypoints_raw[i+2]
                })

        if not self.patrol_waypoints:
            self.get_logger().warn(
                '[PATROL] patrol_waypoints 파라미터가 비어 있습니다 (신규 군산 지도용 좌표 미설정). '
                '순찰 시작 시 홈 위치에서 360도 스캔만 수행합니다. rviz2로 새 지도를 확인한 뒤 '
                'patrol_waypoints 파라미터로 실제 순찰 좌표를 설정하세요.'
            )

        self.patrol_targets = []
        self.patrol_index = 0
        self.patrol_start_x = 0.0
        self.patrol_start_y = 0.0
        self.patrol_start_yaw = 0.0

        # States: "IDLE", "NAV2_WAYPOINT", "WAITING_FOR_MARKER", "VISUAL_SERVOING", "PARKED", "PATROL_NAVIGATING", "PATROL_FINISHED"
        self.state = "IDLE"
        self.current_mode = "nav2"

        # Mock mode parameters and simulation variables
        self.declare_parameter('mock_mode', False)
        self.mock_mode = self.get_parameter('mock_mode').value
        
        self.sim_x = 0.0
        self.sim_y = 0.0
        self.sim_yaw = 0.0
        self.sim_marker_z = 2.5
        self.sim_marker_x = 0.2
        self.sim_marker_yaw = 0.3
        self.mock_scan_start_time = None
        self.virtual_pose_pub = self.create_publisher(PoseStamped, '/aruco_marker_pose', 10)
        
        # Mock Nav2 goal variables
        self.mock_nav2_active = False
        self.mock_nav2_x = 0.0
        self.mock_nav2_y = 0.0
        self.mock_nav2_yaw = 0.0
        
        # Map data for mock collision avoidance
        self.map_data = None
        self.map_info = None
        
        # Damping momentum for mock angular velocity (prevents wobble oscillations)
        self.mock_prev_w = 0.0
        
        # Backing up (undocking) state variables
        self.backup_start_x = 0.0
        self.backup_start_y = 0.0

        # Patrol 360-degree scan state variables
        self.scan_accumulated_yaw = 0.0
        self.scan_last_yaw = 0.0

        # Stuck detection and recovery variables
        self.stuck_check_time = None
        self.stuck_start_pose = None
        self.is_recovering = False
        self.recovery_state = None  # "BACKUP", "SPIN"
        self.recovery_start_time = None
        self.recovery_start_x = 0.0
        self.recovery_start_y = 0.0
        self.recovery_start_yaw = 0.0

        # Last detected marker pose
        self.last_marker_pose = None
        self.last_marker_time = self.get_clock().now()
        self.marker_timeout = 1.0 # seconds
        
        # LiDAR obstacle avoidance parameters
        self.obstacle_detected = False
        self.declare_parameter('min_safety_dist', 0.45)
        self.declare_parameter('safety_cone_angle', 15.0)
        self.min_safety_dist = self.get_parameter('min_safety_dist').value
        self.safety_cone_angle = self.get_parameter('safety_cone_angle').value

        # --- [ROS Publishers & Subscribers] ---
        self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
        self.mode_pub = self.create_publisher(String, '/robot_mode_status', 10)

        self.create_subscription(String, '/robot_mode', self.mode_callback, 10)
        self.create_subscription(Twist, '/cmd_vel_nav2', self.nav2_cmd_vel_callback, 10)
        self.create_subscription(PoseStamped, '/aruco_detector/pose', self.aruco_callback, 10)
        self.create_subscription(PoseStamped, '/aruco_marker_waypoint', self.waypoint_callback, 10)
        self.create_subscription(LaserScan, '/scan', self.scan_callback, qos_profile_sensor_data)
        self.create_subscription(Odometry, '/odom_wheels', self.odom_callback, qos_profile_sensor_data)
        self.create_subscription(PoseStamped, '/goal_pose', self.mock_nav2_goal_callback, 10)
        self.create_subscription(PoseWithCovarianceStamped, '/initialpose', self.initial_pose_callback, 10)

        # AMCL pose subscriber (Real AGV mode: map-frame accurate position)
        # Used to update sim_x/sim_y/sim_yaw with real localization data
        from rclpy.qos import QoSReliabilityPolicy
        amcl_qos = QoSProfile(depth=5,
                              reliability=QoSReliabilityPolicy.BEST_EFFORT,
                              durability=DurabilityPolicy.VOLATILE)
        self.last_amcl_time = None
        self.create_subscription(
            PoseWithCovarianceStamped, '/amcl_pose',
            self.amcl_pose_callback, amcl_qos
        )
        
        # ROS2 OccupancyGrid map server uses TRANSIENT_LOCAL durability.
        map_qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)
        self.create_subscription(OccupancyGrid, '/map', self.map_callback, map_qos)

        # Nav2 Action Client
        self.nav_to_pose_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
        self.goal_handle = None

        # Control loop timer (20Hz)
        self.create_timer(0.05, self.control_loop)

        self.get_logger().info('==========================================')
        self.get_logger().info('        FORI AGV PARKING CONTROLLER       ')
        self.get_logger().info('        Status: Ready (Idle mode)         ')
        self.get_logger().info('==========================================')

    def waypoint_callback(self, msg):
        self.waypoint_x = msg.pose.position.x
        self.waypoint_y = msg.pose.position.y
        
        # Quaternion to yaw
        q = msg.pose.orientation
        siny_cosp = 2 * (q.w * q.z + q.x * q.y)
        cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z)
        self.waypoint_yaw = math.atan2(siny_cosp, cosy_cosp)
        self.waypoint_frame = msg.header.frame_id
        
        self.waypoint_active = True
        self.get_logger().info(f'Updated parking waypoint from Web UI: x={self.waypoint_x:.2f}, y={self.waypoint_y:.2f}, yaw={self.waypoint_yaw:.2f}')
        
        # If in parking mode and waiting for a waypoint, trigger drive now
        if self.current_mode == 'parking' and self.state == "WAITING_FOR_WAYPOINT":
            self.state = "NAV2_WAYPOINT"
            self.send_nav2_goal()

    def odom_callback(self, msg):
        # Only use wheel odom if AMCL hasn't updated in the last 3 seconds
        if self.last_amcl_time is not None:
            elapsed = (self.get_clock().now() - self.last_amcl_time).nanoseconds / 1e9
            if elapsed < 3.0:
                return  # AMCL data is fresher, skip wheel odom update
        self.sim_x = msg.pose.pose.position.x
        self.sim_y = msg.pose.pose.position.y
        q = msg.pose.pose.orientation
        siny_cosp = 2 * (q.w * q.z + q.x * q.y)
        cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z)
        self.sim_yaw = math.atan2(siny_cosp, cosy_cosp)

    def amcl_pose_callback(self, msg):
        """Primary pose update for Real AGV mode (map frame, AMCL localization)."""
        self.last_amcl_time = self.get_clock().now()
        self.sim_x = msg.pose.pose.position.x
        self.sim_y = msg.pose.pose.position.y
        q = msg.pose.pose.orientation
        siny_cosp = 2 * (q.w * q.z + q.x * q.y)
        cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z)
        self.sim_yaw = math.atan2(siny_cosp, cosy_cosp)

    def initial_pose_callback(self, msg):
        self.sim_x = msg.pose.pose.position.x
        self.sim_y = msg.pose.pose.position.y
        q = msg.pose.pose.orientation
        siny_cosp = 2 * (q.w * q.z + q.x * q.y)
        cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z)
        self.sim_yaw = math.atan2(siny_cosp, cosy_cosp)
        self.get_logger().info(f'[INITIALPOSE] Reset controller sim pose to: x={self.sim_x:.2f}, y={self.sim_y:.2f}, yaw={self.sim_yaw:.2f}')

    def mock_nav2_goal_callback(self, msg):
        if not self.mock_mode:
            return
        self.mock_nav2_x = msg.pose.position.x
        self.mock_nav2_y = msg.pose.position.y
        q = msg.pose.orientation
        siny_cosp = 2 * (q.w * q.z + q.x * q.y)
        cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z)
        self.mock_nav2_yaw = math.atan2(siny_cosp, cosy_cosp)
        self.mock_nav2_active = True
        self.get_logger().info(f'[MOCK NAV2] Received target goal: x={self.mock_nav2_x:.2f}, y={self.mock_nav2_y:.2f}, yaw={self.mock_nav2_yaw:.2f}')

    def map_callback(self, msg):
        self.map_data = msg.data
        self.map_info = msg.info

    def scan_callback(self, msg):
        # Check for obstacles within frontal cone
        angle_min = msg.angle_min
        angle_increment = msg.angle_increment
        cone_rad = self.safety_cone_angle * math.pi / 180.0
        
        # Relax safety limits during fine visual docking to allow approaching the target wall (0.5m)
        safety_limit = 0.25 if self.state == "ALIGNING_AND_PARKING" else self.min_safety_dist
        
        obstacle_found = False
        for idx, dist in enumerate(msg.ranges):
            if math.isnan(dist) or math.isinf(dist) or dist <= 0.05:
                continue
            
            # Calculate angle of current ray and normalize
            angle = angle_min + idx * angle_increment
            angle = math.atan2(math.sin(angle), math.cos(angle))
            
            if abs(angle) < cone_rad:
                if dist < safety_limit:
                    obstacle_found = True
                    break
                    
        self.obstacle_detected = obstacle_found

    def mode_callback(self, msg):
        target_mode = msg.data.lower()
        
        # Handle emergency stop transition immediately
        if target_mode == 'stop':
            self.current_mode = 'stop'
            self.state = "ESTOP"
            self.cancel_nav2_goal()
            self.stop_robot()
            self.mock_nav2_active = False
            self.get_logger().warn('!!! EMERGENCY STOP DETECTED FROM WEB UI !!!')
            return

        if target_mode == self.current_mode:
            return

        self.get_logger().info(f'Mode transition requested: {self.current_mode} -> {target_mode}')
        
        # Reset mock nav2 active status
        self.mock_nav2_active = False
        
        # If transitioning to nav2 from any active parking docking state, initiate undock backing-up sequence first
        if self.state in ["WAITING_FOR_MARKER", "ALIGNING_AND_PARKING", "PARKED"] and target_mode == "nav2":
            self.get_logger().info('[UNDOCK] Initiating backing-up sequence (0.5m) to undock from charger...')
            self.state = "BACKING_UP"
            self.backup_start_x = self.sim_x
            self.backup_start_y = self.sim_y
            self.current_mode = "nav2"
            return
            
        self.current_mode = target_mode
        self.waypoint_active = False # Reset active waypoint status on mode switch

        if target_mode == 'parking':
            # Wait for user to specify a waypoint from Web UI
            self.state = "WAITING_FOR_WAYPOINT"
            self.get_logger().info('Switched to parking mode. Waiting for user to send waypoint coordinates...')
        elif target_mode == 'patrol':
            self.state = "PATROL_INIT"
            # Start position (captures current initialized pose sim_x, sim_y, sim_yaw)
            self.patrol_start_x = self.sim_x
            self.patrol_start_y = self.sim_y
            self.patrol_start_yaw = self.sim_yaw
            
            # Setup targets (predefined safe map waypoints + Home start position)
            self.patrol_targets = list(self.patrol_waypoints)
            self.patrol_targets.append({
                'x': self.patrol_start_x,
                'y': self.patrol_start_y,
                'yaw': self.patrol_start_yaw
            })
            
            self.patrol_index = 0
            self.state = "PATROL_NAVIGATING"
            self.get_logger().info(f'Starting patrol from Home ({self.patrol_start_x:.2f}, {self.patrol_start_y:.2f}). Total waypoints: {len(self.patrol_targets)}')
            self.send_patrol_goal()
        else:
            self.cancel_nav2_goal()
            self.state = "IDLE"

    def nav2_cmd_vel_callback(self, msg):
        # In normal nav2 mode, forward command directly
        if self.current_mode == 'nav2' and self.state == "IDLE":
            self.cmd_vel_pub.publish(msg)

    def aruco_callback(self, msg):
        if self.mock_mode:
            # In simulation mock mode, only forward real camera detections to update the Web UI card if we are NOT actively visual servoing
            if self.state != "ALIGNING_AND_PARKING":
                self.virtual_pose_pub.publish(msg)
            return  
        self.last_marker_pose = msg.pose
        self.last_marker_time = self.get_clock().now()
        # In real mode, act as the single publisher for /aruco_marker_pose to update the Web UI
        self.virtual_pose_pub.publish(msg)
        
        # If in real mode and currently performing 360-degree scan after patrol return, auto-transition to Parking!
        if not self.mock_mode and self.current_mode == 'patrol' and self.state == "SCANNING_FOR_MARKER":
            self.get_logger().info('!!! [PATROL -> AUTO PARKING] ArUco marker detected during 360-degree scan! Initiating Automatic Parking docking... !!!')
            self.current_mode = 'parking'
            self.state = "ALIGNING_AND_PARKING"

    def send_nav2_goal(self):
        if self.mock_mode:
            self.get_logger().info('[MOCK] Bypassing Nav2 Action Server. Simulating drive to waypoint...')
            return
        self.get_logger().info('Waiting for Nav2 NavigateToPose action server...')
        self.nav_to_pose_client.wait_for_server()

        goal_msg = NavigateToPose.Goal()
        goal_msg.pose.header.frame_id = self.waypoint_frame
        goal_msg.pose.header.stamp = self.get_clock().now().to_msg()
        
        goal_msg.pose.pose.position.x = self.waypoint_x
        goal_msg.pose.pose.position.y = self.waypoint_y
        goal_msg.pose.pose.position.z = 0.0

        # Euler to Quaternion for yaw
        cy = math.cos(self.waypoint_yaw * 0.5)
        sy = math.sin(self.waypoint_yaw * 0.5)
        goal_msg.pose.pose.orientation.w = cy
        goal_msg.pose.pose.orientation.z = sy

        self.get_logger().info(f'Sending Nav2 goal to pre-parking waypoint: x={self.waypoint_x:.2f}, y={self.waypoint_y:.2f}, yaw={self.waypoint_yaw:.2f}')
        
        send_goal_future = self.nav_to_pose_client.send_goal_async(goal_msg)
        send_goal_future.add_done_callback(self.goal_response_callback)

    def goal_response_callback(self, future):
        self.goal_handle = future.result()
        if not self.goal_handle.accepted:
            self.get_logger().warn('Nav2 goal rejected by server! Switching to WAITING_FOR_MARKER.')
            self.state = "WAITING_FOR_MARKER"
            return

        self.get_logger().info('Nav2 goal accepted. Navigating...')
        get_result_future = self.goal_handle.get_result_async()
        get_result_future.add_done_callback(self.goal_result_callback)

    def goal_result_callback(self, future):
        status = future.result().status
        if status == GoalStatus.STATUS_SUCCEEDED:
            self.get_logger().info('Successfully arrived at pre-parking waypoint. Waiting for ArUco marker...')
            self.state = "WAITING_FOR_MARKER"
        else:
            self.get_logger().warn(f'Nav2 navigation failed with status {status}. Scanning for ArUco marker directly.')
            self.state = "WAITING_FOR_MARKER"

    def cancel_nav2_goal(self):
        if self.goal_handle is not None:
            self.get_logger().info('Canceling active Nav2 goal...')
            self.goal_handle.cancel_goal_async()
            self.goal_handle = None

    def check_stuck_status(self, cmd_v, cmd_w):
        if not self.mock_mode or self.is_recovering:
            return
            
        # We only check for stuck state if we are in an active navigation mode
        is_navigating = False
        if self.current_mode == 'nav2' and self.mock_nav2_active:
            is_navigating = True
        elif self.current_mode == 'patrol' and self.state == "PATROL_NAVIGATING":
            is_navigating = True
        elif self.current_mode == 'parking' and self.state == "NAV2_WAYPOINT":
            is_navigating = True
            
        if not is_navigating:
            self.stuck_check_time = None
            self.stuck_start_pose = None
            return

        now = self.get_clock().now()
        if self.stuck_check_time is None:
            self.stuck_check_time = now
            self.stuck_start_pose = (self.sim_x, self.sim_y, self.sim_yaw)
        else:
            elapsed = (now - self.stuck_check_time).nanoseconds / 1e9
            if elapsed > 2.0:
                dx = self.sim_x - self.stuck_start_pose[0]
                dy = self.sim_y - self.stuck_start_pose[1]
                dist_moved = math.sqrt(dx*dx + dy*dy)
                
                # Recovery triggers only when attempting FORWARD linear motion (cmd_v > 0.03) but stuck at same spot (<0.05m) for 2 seconds
                if abs(cmd_v) > 0.03 and dist_moved < 0.05:
                    self.get_logger().warn('!!! [교착 감지] 2초간 전진 시도 불가(막다른 길): 0.6m 후진 탈출(BACKUP) 가동 !!!')
                    self.is_recovering = True
                    self.recovery_state = "BACKUP"
                    self.recovery_start_time = now
                    self.recovery_start_x = self.sim_x
                    self.recovery_start_y = self.sim_y
                    self.recovery_start_yaw = self.sim_yaw
                
                self.stuck_check_time = now
                self.stuck_start_pose = (self.sim_x, self.sim_y, self.sim_yaw)

    def publish_mock_cmd_vel(self, v, w):
        twist = Twist()
        twist.linear.x = v
        twist.angular.z = w
        self.cmd_vel_pub.publish(twist)
        self.check_stuck_status(v, w)

    def send_patrol_goal(self):
        target = self.patrol_targets[self.patrol_index]
        self.get_logger().info(f'[PATROL] Sending goal {self.patrol_index + 1}/{len(self.patrol_targets)}: x={target["x"]:.2f}, y={target["y"]:.2f}')
        
        if self.mock_mode:
            # In mock mode, we use potential field simulation.
            # We set the target coordinates for the potential field navigation.
            self.waypoint_x = target['x']
            self.waypoint_y = target['y']
            self.waypoint_yaw = target['yaw']
            return

        self.nav_to_pose_client.wait_for_server()
        goal_msg = NavigateToPose.Goal()
        goal_msg.pose.header.frame_id = self.waypoint_frame
        goal_msg.pose.header.stamp = self.get_clock().now().to_msg()
        
        goal_msg.pose.pose.position.x = target['x']
        goal_msg.pose.pose.position.y = target['y']
        goal_msg.pose.pose.position.z = 0.0

        cy = math.cos(target['yaw'] * 0.5)
        sy = math.sin(target['yaw'] * 0.5)
        goal_msg.pose.pose.orientation.w = cy
        goal_msg.pose.pose.orientation.z = sy

        send_goal_future = self.nav_to_pose_client.send_goal_async(goal_msg)
        send_goal_future.add_done_callback(self.patrol_goal_response_callback)

    def patrol_goal_response_callback(self, future):
        self.goal_handle = future.result()
        if not self.goal_handle.accepted:
            self.get_logger().warn('Patrol goal rejected by Nav2 server! Retrying next waypoint...')
            self.advance_patrol()
            return

        self.get_logger().info('Patrol goal accepted. Navigating...')
        get_result_future = self.goal_handle.get_result_async()
        get_result_future.add_done_callback(self.patrol_goal_result_callback)

    def patrol_goal_result_callback(self, future):
        # Prevent callback execution if goal_handle was reset (canceled)
        if self.goal_handle is None:
            return
        status = future.result().status
        if status == GoalStatus.STATUS_SUCCEEDED:
            self.get_logger().info(f'Arrived at patrol waypoint {self.patrol_index + 1}!')
            self.advance_patrol()
        else:
            self.get_logger().warn(f'Patrol navigation to waypoint {self.patrol_index + 1} failed with status {status}. Moving to next.')
            self.advance_patrol()

    def advance_patrol(self):
        self.patrol_index += 1
        if self.patrol_index < len(self.patrol_targets):
            self.send_patrol_goal()
        else:
            self.get_logger().info('Returned to starting position! Initiating 360-degree scan for ArUco marker...')
            self.state = "SCANNING_FOR_MARKER"
            self.scan_accumulated_yaw = 0.0
            self.scan_last_yaw = self.sim_yaw

    def calculate_steering_target(self, tx, ty):
        # Calculate goal direction vector
        dx = tx - self.sim_x
        dy = ty - self.sim_y
        dist = math.sqrt(dx*dx + dy*dy)
        if dist < 0.05:
            return math.atan2(dy, dx), dist
            
        att_x = dx / dist
        att_y = dy / dist
        
        # Calculate repulsive forces from map obstacles
        rep_x = 0.0
        rep_y = 0.0
        
        if self.map_data is not None and self.map_info is not None:
            res = self.map_info.resolution
            ox = self.map_info.origin.position.x
            oy = self.map_info.origin.position.y
            w = self.map_info.width
            h = self.map_info.height
            
            r_col = int((self.sim_x - ox) / res)
            r_row = int((self.sim_y - oy) / res)
            
            # Scan a 12x12 cell region around the robot (approx +/- 0.6 meters)
            scan_range = 12
            for di in range(-scan_range, scan_range + 1):
                for dj in range(-scan_range, scan_range + 1):
                    col = r_col + di
                    row = r_row + dj
                    if 0 <= col < w and 0 <= row < h:
                        idx = row * w + col
                        if self.map_data[idx] == 100: # Obstacle cell
                            obs_x = col * res + ox
                            obs_y = row * res + oy
                            odx = self.sim_x - obs_x
                            ody = self.sim_y - obs_y
                            o_dist = math.sqrt(odx*odx + ody*ody)
                            
                            d0 = 0.55 # Influence range (0.55 meters)
                            if o_dist < d0:
                                # Calculate relative bearing of the obstacle from the robot's current heading
                                vector_to_obs_x = obs_x - self.sim_x
                                vector_to_obs_y = obs_y - self.sim_y
                                angle_to_obs = math.atan2(vector_to_obs_y, vector_to_obs_x)
                                relative_angle = angle_to_obs - self.sim_yaw
                                relative_angle = math.atan2(math.sin(relative_angle), math.cos(relative_angle))
                                
                                # Continuous angle weighting: only repel if obstacle is in front semicircle (+/- 90 deg)
                                if abs(relative_angle) < (math.pi / 2.0):
                                    # Weight goes from 1.0 (directly in front) to 0.0 (at 90 degrees) continuously
                                    angle_weight = math.cos(relative_angle)
                                    
                                    o_dist = max(o_dist, 0.05)
                                    # Standard potential field force formula: force = eta * (1/d - 1/d0) * (1/d^2) * angle_weight
                                    eta = 0.40
                                    force = eta * (1.0 / o_dist - 1.0 / d0) * (1.0 / (o_dist * o_dist)) * angle_weight
                                    rep_x += (odx / o_dist) * force
                                    rep_y += (ody / o_dist) * force
                                
        # Clamp repulsive force to prevent it from completely overwhelming goal attraction (spinning in place)
        rep_mag = math.sqrt(rep_x*rep_x + rep_y*rep_y)
        max_rep = 1.3 # Slightly larger than attractive force (1.0) to allow pushing away, but limits spinning
        if rep_mag > max_rep:
            rep_x = (rep_x / rep_mag) * max_rep
            rep_y = (rep_y / rep_mag) * max_rep

        # Combined force vector
        net_x = att_x + rep_x
        net_y = att_y + rep_y
        
        return math.atan2(net_y, net_x), dist

    def control_loop(self):
        # Handle Emergency Stop (ESTOP) state first (both mock and real modes)
        if self.state == "ESTOP" or self.current_mode == "stop":
            self.stop_robot()
            status_msg = String()
            env_suffix = " (Simulation)" if self.mock_mode else " (Real)"
            status_msg.data = f"Mode: stop | State: ESTOP{env_suffix}"
            self.mode_pub.publish(status_msg)
            return

        # Handle Backing Up state first (both mock and real modes)
        if self.state == "BACKING_UP":
            dx = self.sim_x - self.backup_start_x
            dy = self.sim_y - self.backup_start_y
            dist_traveled = math.sqrt(dx*dx + dy*dy)
            
            if dist_traveled >= 0.50:
                self.get_logger().info('[UNDOCK] Backing-up completed (0.5m). Transitioning to IDLE.')
                self.stop_robot()
                self.state = "IDLE"
                return
                
            twist = Twist()
            twist.linear.x = -0.12 # Back up slowly
            twist.angular.z = 0.0
            self.cmd_vel_pub.publish(twist)
            
            # Publish BACKING_UP status
            status_msg = String()
            env_suffix = " (Simulation)" if self.mock_mode else " (Real)"
            status_msg.data = f"Mode: {self.current_mode} | State: BACKING_UP{env_suffix}"
            self.mode_pub.publish(status_msg)
            return

        # Handle Stuck Recovery (primarily for mock mode navigation)
        if self.mock_mode and self.is_recovering:
            now = self.get_clock().now()
            v = 0.0
            w = 0.0
            
            if self.recovery_state == "BACKUP":
                dx = self.sim_x - self.recovery_start_x
                dy = self.sim_y - self.recovery_start_y
                dist = math.sqrt(dx*dx + dy*dy)
                duration = (now - self.recovery_start_time).nanoseconds / 1e9
                
                # Back up 0.6 meters or for 3.0 seconds max
                if dist < 0.6 and duration < 3.0:
                    v = -0.1
                    w = 0.0
                else:
                    self.get_logger().info('[RECOVERY] Backup finished. Initiating spin to turn away...')
                    self.recovery_state = "SPIN"
                    self.recovery_start_time = now
                    self.recovery_start_yaw = self.sim_yaw
            
            elif self.recovery_state == "SPIN":
                yaw_diff = self.sim_yaw - self.recovery_start_yaw
                yaw_diff = math.atan2(math.sin(yaw_diff), math.cos(yaw_diff))
                duration = (now - self.recovery_start_time).nanoseconds / 1e9
                
                # Spin by 90 degrees (1.57 rad) or for 3.0 seconds max
                if abs(yaw_diff) < 1.57 and duration < 3.0:
                    v = 0.0
                    w = 0.5  # Rotate in place
                else:
                    self.get_logger().info('[RECOVERY] Escape maneuver finished.')
                    self.is_recovering = False
                    self.recovery_state = None
                    self.stuck_check_time = None  # Reset stuck timer
                    
                    if self.current_mode == 'patrol' and self.state == "PATROL_NAVIGATING":
                        # If the current target is the final Home return target, do NOT skip Home! Retry returning Home.
                        if self.patrol_index == len(self.patrol_targets) - 1:
                            home_target = self.patrol_targets[self.patrol_index]
                            dx = home_target['x'] - self.sim_x
                            dy = home_target['y'] - self.sim_y
                            dist_to_home = math.sqrt(dx*dx + dy*dy)
                            if dist_to_home < 0.18:
                                self.get_logger().info(f'[PATROL] Arrived near Home ({dist_to_home:.2f}m margin). Patrol completed successfully!')
                                self.advance_patrol()
                            else:
                                self.get_logger().warn('[PATROL] Obstacle on path to Home. Retrying navigation back to Home...')
                                self.send_patrol_goal()
                        else:
                            self.get_logger().warn('[PATROL] Current patrol waypoint is blocked. Skipping to the next waypoint...')
                            self.advance_patrol()
                    elif self.current_mode == 'nav2':
                        self.mock_nav2_active = False
                        self.stop_robot()
                        self.get_logger().warn('[MOCK NAV2] Goal is blocked! Stopping robot.')
                    elif self.current_mode == 'parking' and self.state == "NAV2_WAYPOINT":
                        self.state = "WAITING_FOR_WAYPOINT"
                        self.stop_robot()
                        self.get_logger().warn('[MOCK PARKING] Pre-parking waypoint is blocked! Stopping robot.')
            
            twist = Twist()
            twist.linear.x = v
            twist.angular.z = w
            self.cmd_vel_pub.publish(twist)
            
            # Publish status
            status_msg = String()
            status_msg.data = f"Mode: {self.current_mode} | State: RECOVERY_{self.recovery_state} (Simulation)"
            self.mode_pub.publish(status_msg)
            return

        # Publish current mode status
        status_msg = String()
        env_suffix = " (Simulation)" if self.mock_mode else " (Real)"
        status_msg.data = f"Mode: {self.current_mode} | State: {self.state}{env_suffix}"
        self.mode_pub.publish(status_msg)

        # In Mock Mode, perform collision checking on the OccupancyGrid map
        if self.mock_mode and self.map_data is not None and self.map_info is not None:
            obstacle_in_path = False
            # Relax safety limits during visual docking (allows approaching 0.5m target)
            safety_limit = 0.25 if self.state == "ALIGNING_AND_PARKING" else self.min_safety_dist
            check_dists = [0.15, 0.2, 0.25] if self.state == "ALIGNING_AND_PARKING" else [0.2, 0.3, 0.4, 0.5, 0.6]
            
            for check_dist in check_dists:
                check_x = self.sim_x + check_dist * math.cos(self.sim_yaw)
                check_y = self.sim_y + check_dist * math.sin(self.sim_yaw)
                
                res = self.map_info.resolution
                ox = self.map_info.origin.position.x
                oy = self.map_info.origin.position.y
                w = self.map_info.width
                h = self.map_info.height
                
                col = int((check_x - ox) / res)
                row = int((check_y - oy) / res)
                
                if 0 <= col < w and 0 <= row < h:
                    idx = row * w + col
                    val = self.map_data[idx]
                    if val == 100: # Occupied
                        obstacle_in_path = True
                        break
            
            self.obstacle_detected = obstacle_in_path

        if self.current_mode == 'nav2':
            if self.mock_mode and self.mock_nav2_active:
                # Steer around obstacles using artificial potential field
                target_heading, dist = self.calculate_steering_target(self.mock_nav2_x, self.mock_nav2_y)
                
                if dist > 0.08:
                    heading_error = target_heading - self.sim_yaw
                    heading_error = math.atan2(math.sin(heading_error), math.cos(heading_error))
                    
                    # Turn in place if heading error is large to avoid driving forward into obstacles on the side
                    if abs(heading_error) > 0.8:
                        v = 0.0
                        w = 0.8 * heading_error
                        w = max(min(w, 0.4), -0.4)
                    else:
                        v = 0.25 * dist
                        v = max(min(v, 0.15), -0.15)
                        w = 0.8 * heading_error
                        w = max(min(w, 0.4), -0.4)
                else:
                    yaw_error = self.mock_nav2_yaw - self.sim_yaw
                    yaw_error = math.atan2(math.sin(yaw_error), math.cos(yaw_error))
                    if abs(yaw_error) > 0.08:
                        v = 0.0
                        w = 0.6 * yaw_error
                        w = max(min(w, 0.3), -0.3)
                    else:
                        v = 0.0
                        w = 0.0
                        self.mock_nav2_active = False
                        self.get_logger().info('[MOCK NAV2] Arrived at Nav2 Goal!')
                        
                # Emergency safety brake: zero forward speed but allow rotation to steer away
                if self.obstacle_detected:
                    if v > 0.0:
                        self.get_logger().warn('!!! LiDAR 전방 장애물 감지: 전진 차단, 회전 허용 !!!', throttle_duration_sec=1.0)
                        v = 0.0  # Block forward motion only
                    w = w * 0.5  # Damp rotation near obstacles to prevent chattering
                        
                # Apply low-pass filter to prevent oscillations (corridor wobble)
                if v == 0.0 and w == 0.0:
                    self.mock_prev_w = 0.0
                else:
                    self.mock_prev_w = self.mock_prev_w * 0.75 + w * 0.25
                    w = self.mock_prev_w
                    
                self.publish_mock_cmd_vel(v, w)
            return

        if self.current_mode == 'patrol':
            # Handle patrol navigation state in mock mode
            if self.state == "PATROL_NAVIGATING":
                if self.mock_mode:
                    target = self.patrol_targets[self.patrol_index]
                    target_heading, dist = self.calculate_steering_target(target['x'], target['y'])
                    
                    is_home_target = (self.patrol_index == len(self.patrol_targets) - 1)
                    arrival_dist = 0.18 if is_home_target else 0.35
                    
                    if dist > arrival_dist:
                        heading_error = target_heading - self.sim_yaw
                        heading_error = math.atan2(math.sin(heading_error), math.cos(heading_error))
                        
                        if abs(heading_error) < 0.06:
                            # Deadband: drive straight without micro-oscillations (wobble)
                            w = 0.0
                            v = 0.25 * dist
                            v = max(min(v, 0.15), 0.05)
                        elif abs(heading_error) > 0.8:
                            v = 0.0
                            w = 0.6 * heading_error
                            w = max(min(w, 0.35), -0.35)
                        else:
                            v = 0.25 * dist
                            v = max(min(v, 0.15), 0.05)
                            w = 0.5 * heading_error
                            w = max(min(w, 0.3), -0.3)
                    else:
                        # Arrived at waypoint - transition smoothly to next without stopping/oscillating!
                        self.get_logger().info(f'[MOCK] Arrived at patrol waypoint {self.patrol_index + 1}/{len(self.patrol_targets)}!')
                        self.advance_patrol()
                        return

                    # Obstacle safety check: stop completely without left-right chattering
                    if self.obstacle_detected:
                        v = 0.0
                        w = 0.0  # Zero out angular velocity to stop left-right wobbling in front of obstacle
                    
                    # Low pass filter
                    if v == 0.0 and w == 0.0:
                        self.mock_prev_w = 0.0
                    else:
                        self.mock_prev_w = self.mock_prev_w * 0.75 + w * 0.25
                        w = self.mock_prev_w

                    self.publish_mock_cmd_vel(v, w)
            elif self.state == "SCANNING_FOR_MARKER":
                w = 0.30  # Smooth 360-degree rotation speed (rad/s)
                v = 0.0
                
                # Measure actual physical/simulated orientation change from self.sim_yaw
                yaw_diff = self.sim_yaw - self.scan_last_yaw
                yaw_diff = math.atan2(math.sin(yaw_diff), math.cos(yaw_diff))
                self.scan_accumulated_yaw += abs(yaw_diff)
                self.scan_last_yaw = self.sim_yaw
                
                # In real mode, check if ArUco marker is fresh and detected during the scan
                if not self.mock_mode and self.last_marker_pose is not None:
                    now = self.get_clock().now()
                    if (now - self.last_marker_time).nanoseconds / 1e9 < 1.0:
                        self.get_logger().info('!!! [PATROL -> AUTO PARKING] ArUco marker detected during 360-degree scan! Initiating Automatic Parking docking... !!!')
                        self.current_mode = 'parking'
                        self.state = "ALIGNING_AND_PARKING"
                        return

                if self.scan_accumulated_yaw >= (2.0 * math.pi - 0.05):
                    self.get_logger().info('[PATROL SCAN] 360-degree scan complete. Stopping at Home position.')
                    self.stop_robot()
                    self.state = "PATROL_FINISHED"
                    return

                # Publish direct un-damped velocity command for smooth 360-degree rotation
                twist = Twist()
                twist.linear.x = 0.0
                twist.angular.z = w
                self.cmd_vel_pub.publish(twist)
                return
            elif self.state == "PATROL_FINISHED":
                self.stop_robot()
            return

        if self.current_mode != 'parking':
            return

        # Check ArUco marker fresh status
        now = self.get_clock().now()
        marker_timeout = (now - self.last_marker_time).nanoseconds / 1e9 > self.marker_timeout
        if marker_timeout:
            self.last_marker_pose = None

        if self.state == "NAV2_WAYPOINT":
            if self.mock_mode:
                # Steer around obstacles using artificial potential field
                target_heading, dist = self.calculate_steering_target(self.waypoint_x, self.waypoint_y)
                
                if dist > 0.08:
                    heading_error = target_heading - self.sim_yaw
                    heading_error = math.atan2(math.sin(heading_error), math.cos(heading_error))
                    
                    # Turn in place if heading error is large to avoid driving forward into obstacles on the side
                    if abs(heading_error) > 0.8:
                        v = 0.0
                        w = 0.8 * heading_error
                        w = max(min(w, 0.4), -0.4)
                    else:
                        v = 0.25 * dist
                        v = max(min(v, 0.15), -0.15)
                        w = 0.8 * heading_error
                        w = max(min(w, 0.4), -0.4)
                else:
                    yaw_error = self.waypoint_yaw - self.sim_yaw
                    yaw_error = math.atan2(math.sin(yaw_error), math.cos(yaw_error))
                    if abs(yaw_error) > 0.08:
                        v = 0.0
                        w = 0.6 * yaw_error
                        w = max(min(w, 0.3), -0.3)
                    else:
                        v = 0.0
                        w = 0.0
                        self.get_logger().info('[MOCK] Waypoint reached! Scanning for ArUco marker...')
                        self.state = "WAITING_FOR_MARKER"
                        self.mock_scan_start_time = now
                        
                # Emergency safety brake: zero forward speed but allow rotation to steer away
                if self.obstacle_detected:
                    if v > 0.0:
                        self.get_logger().warn('!!! LiDAR 전방 장애물 감지: 전진 차단, 회전 허용 !!!', throttle_duration_sec=1.0)
                        v = 0.0  # Block forward motion only
                    w = w * 0.5  # Damp rotation near obstacles to prevent chattering
                        
                # Apply low-pass filter to prevent oscillations (corridor wobble)
                if v == 0.0 and w == 0.0:
                    self.mock_prev_w = 0.0
                else:
                    self.mock_prev_w = self.mock_prev_w * 0.75 + w * 0.25
                    w = self.mock_prev_w
                    
                self.publish_mock_cmd_vel(v, w)
            else:
                pass

        elif self.state == "WAITING_FOR_MARKER":
            if self.mock_mode:
                if self.mock_scan_start_time is None:
                    self.mock_scan_start_time = now
                
                # Scan for 2 seconds then "find" the virtual marker
                elapsed = (now - self.mock_scan_start_time).nanoseconds / 1e9
                if elapsed > 2.0:
                    self.sim_marker_z = 2.5
                    self.sim_marker_x = 0.2
                    self.sim_marker_yaw = 0.3
                    
                    mock_pose = PoseStamped()
                    mock_pose.header.stamp = now.to_msg()
                    mock_pose.header.frame_id = 'camera_optical_link'
                    mock_pose.pose.position.x = self.sim_marker_x
                    mock_pose.pose.position.z = self.sim_marker_z
                    mock_pose.pose.orientation.z = math.sin(self.sim_marker_yaw * 0.5)
                    mock_pose.pose.orientation.w = math.cos(self.sim_marker_yaw * 0.5)
                    self.virtual_pose_pub.publish(mock_pose)
                    
                    self.last_marker_pose = mock_pose.pose
                    self.last_marker_time = now
                    self.state = "ALIGNING_AND_PARKING"
                    self.get_logger().info('[MOCK] Virtual ArUco marker detected!')
                else:
                    self.stop_robot()
            else:
                if self.last_marker_pose is not None:
                    self.get_logger().info('ArUco marker detected! Commencing visual servoing parking sequence.')
                    self.state = "ALIGNING_AND_PARKING"
                else:
                    # Stop and scan
                    self.stop_robot()

        elif self.state == "ALIGNING_AND_PARKING":
            # LiDAR Safety Halt Check
            if self.obstacle_detected:
                self.get_logger().warn('!!! LiDAR 전방 장애물 감지: 비상 제동 가동 !!!', throttle_duration_sec=1.0)
                self.stop_robot()
                return

            if self.last_marker_pose is None:
                self.get_logger().warn('ArUco marker lost! Halting robot to scan.')
                self.state = "WAITING_FOR_MARKER"
                self.stop_robot()
                return

            # Visual Servoing Kinematics
            # Optical frame: Z forward, X right, Y down
            x = self.last_marker_pose.position.x
            z = self.last_marker_pose.position.z

            # Calculate marker yaw relative to camera
            # Normal vector in camera frame from pose quaternion
            qx = self.last_marker_pose.orientation.x
            qy = self.last_marker_pose.orientation.y
            qz = self.last_marker_pose.orientation.z
            qw = self.last_marker_pose.orientation.w
            
            # Rotation matrix components
            r02 = 2 * (qx * qz + qw * qy)
            r22 = qw * qw - qx * qx - qy * qy + qz * qz
            yaw_error = math.atan2(r02, r22)
 
            # Bearing angle to marker
            bearing_error = math.atan2(x, z)

            # Target distance error
            dist_error = z - self.target_dist

            self.get_logger().info(f'VS Feedback: dist_err={dist_error:.2f}m, bearing_err={bearing_error:.2f}rad, yaw_err={yaw_error:.2f}rad', throttle_duration_sec=0.5)

            # Check if parking is complete
            if abs(dist_error) < 0.04 and abs(bearing_error) < 0.05 and abs(yaw_error) < 0.06:
                self.get_logger().info('AUTOMATIC PARKING SUCCESSFUL! Braking applied.')
                self.state = "PARKED"
                self.stop_robot()
                return

            # Proportional speed calculations
            # Linear velocity (forward/backward)
            v = self.kp_linear * dist_error
            # Clip linear velocity (limit to max 0.15 m/s for safe parking)
            v = max(min(v, 0.15), -0.15)

            # Angular velocity (steering)
            # Combine bearing alignment and heading correction (facing parallel to marker normal)
            w = (self.kp_bearing * bearing_error) + (self.kp_yaw * yaw_error)
            w = max(min(w, 0.4), -0.4)

            if self.mock_mode:
                dt = 0.05
                self.sim_marker_z -= v * dt
                self.sim_marker_yaw -= w * dt
                self.sim_marker_x -= (v * math.sin(self.sim_marker_yaw) * dt + w * self.sim_marker_z * dt)
                
                mock_pose = PoseStamped()
                mock_pose.header.stamp = now.to_msg()
                mock_pose.header.frame_id = 'camera_optical_link'
                mock_pose.pose.position.x = self.sim_marker_x
                mock_pose.pose.position.z = self.sim_marker_z
                mock_pose.pose.orientation.z = math.sin(self.sim_marker_yaw * 0.5)
                mock_pose.pose.orientation.w = math.cos(self.sim_marker_yaw * 0.5)
                self.virtual_pose_pub.publish(mock_pose)
                
                self.last_marker_pose = mock_pose.pose
                self.last_marker_time = now

            # Publish cmd_vel
            twist = Twist()
            twist.linear.x = v
            twist.angular.z = w
            self.cmd_vel_pub.publish(twist)

        elif self.state == "PARKED":
            self.stop_robot()

    def stop_robot(self):
        twist = Twist()
        self.cmd_vel_pub.publish(twist)

def main(args=None):
    rclpy.init(args=args)
    node = ParkingControllerNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()
