plotting.py

"""
FocalPlotter - fSpy-like camera matching tool
Vanishing point estimation and camera solving
"""

import bpy
import gpu
import math
from gpu_extras.batch import batch_for_shader
from bpy.types import Panel, Operator, PropertyGroup
from bpy.props import (
    BoolProperty,
    PointerProperty,
    FloatVectorProperty,
    StringProperty,
)
from mathutils import Vector, Matrix, Euler
from .ui import draw_checkbox


# ---------------------------------------------------------------------------
# Data Structures
# ---------------------------------------------------------------------------

class FOCALPLOTTER_PG_axis_lines(PropertyGroup):
    """Line pair data for one axis (4 points total) - stored in normalized camera coords (0-1)"""
    line1_start: FloatVectorProperty(size=2, default=(0.2, 0.4))
    line1_end: FloatVectorProperty(size=2, default=(0.6, 0.4))
    line2_start: FloatVectorProperty(size=2, default=(0.2, 0.6))
    line2_end: FloatVectorProperty(size=2, default=(0.6, 0.6))


class FOCALPLOTTER_PG_plot_settings(PropertyGroup):
    """Settings for the focal plotter"""
    is_plotting: BoolProperty(default=False)
    use_reference_image: BoolProperty(name="Add Reference Image", default=False)
    reference_image: PointerProperty(name="Reference Image", type=bpy.types.Image)
    set_render_resolution: BoolProperty(
        name="Set Render Resolution",
        description="Set render resolution to match image size (affects entire scene)",
        default=False,
    )
    use_rule_of_thirds: BoolProperty(name="Rule of Thirds", default=False)
    
    # Axis selection (fSpy-style: two selectors instead of three toggles)
    axis_1: bpy.props.EnumProperty(
        name="First Axis",
        description="World axis for the first vanishing point (typically horizontal)",
        items=[
            ('x', "X", "X axis (red)"),
            ('y', "Y", "Y axis (green)"),
            ('z', "Z", "Z axis (blue)"),
        ],
        default='x',
        update=lambda self, ctx: update_camera_from_vanishing_points(ctx) if self.is_plotting else None,
    )
    axis_2: bpy.props.EnumProperty(
        name="Second Axis",
        description="World axis for the second vanishing point (typically horizontal)",
        items=[
            ('x', "X", "X axis (red)"),
            ('y', "Y", "Y axis (green)"),
            ('z', "Z", "Z axis (blue)"),
        ],
        default='y',
        update=lambda self, ctx: update_camera_from_vanishing_points(ctx) if self.is_plotting else None,
    )
    
    auto_solve: BoolProperty(name="Auto Solve Camera", default=True)
    
    # Quality of life settings
    freeze_guides: BoolProperty(
        name="Freeze Guides",
        description="Prevent dragging to check alignment",
        default=False,
    )
    show_horizon: BoolProperty(
        name="Show Horizon Line",
        description="Display the horizon line while plotting",
        default=True,
    )
    camera_distance: bpy.props.FloatProperty(
        name="Distance",
        description="Distance from camera to target point. Affects camera position, not rotation",
        default=10.0,
        min=0.1,
        soft_max=100.0,
        unit='LENGTH',
        update=lambda self, ctx: update_camera_position_only(ctx) if self.is_plotting else None,
    )
    principal_point_x: bpy.props.FloatProperty(
        name="X",
        description="Optical center X offset. Use if the camera lens wasn't centered when photo was taken",
        default=0.0,
        min=-0.5,
        max=0.5,
        subtype='FACTOR',
        update=lambda self, ctx: update_focal_length_only(ctx) if self.is_plotting else None,
    )
    principal_point_y: bpy.props.FloatProperty(
        name="Y",
        description="Optical center Y offset. Use if the camera lens wasn't centered when photo was taken",
        default=0.0,
        min=-0.5,
        max=0.5,
        subtype='FACTOR',
        update=lambda self, ctx: update_focal_length_only(ctx) if self.is_plotting else None,
    )
    target_mode: bpy.props.EnumProperty(
        name="Target Location",
        description="Where the camera points to",
        items=[
            ('WORLD_CENTER', "World Center", "Camera targets world origin (0,0,0)"),
            ('MANUAL', "Manual", "Set target location manually"),
        ],
        default='WORLD_CENTER',
        update=lambda self, ctx: update_camera_position_only(ctx) if self.is_plotting else None,
    )
    target_location: bpy.props.FloatVectorProperty(
        name="Target",
        description="Manual target location - camera will be positioned at 'Distance' away from this point",
        default=(0.0, 0.0, 0.0),
        subtype='TRANSLATION',
        update=lambda self, ctx: update_camera_position_only(ctx) if self.is_plotting else None,
    )
    keep_bg_image: bpy.props.BoolProperty(
        name="Keep Background Image",
        description="Keep the background image on camera after stopping plotting",
        default=True,
    )
    
    # Original settings storage
    original_passepartout: FloatVectorProperty(size=2, default=(0.0, 0.5))
    original_show_name: BoolProperty(default=False)
    original_show_composition_thirds: BoolProperty(default=False)
    original_show_bg_images: BoolProperty(default=False)
    modified_camera: StringProperty(default="")


# ---------------------------------------------------------------------------
# Utility Functions
# ---------------------------------------------------------------------------

def is_in_camera_view(context):
    space = context.space_data
    if space is None or space.type != 'VIEW_3D':
        return False
    return space.region_3d.view_perspective == 'CAMERA'


def get_active_camera(context):
    return context.scene.camera


def get_closest_point_distance(context, obj, camera):
    """
    Calculate the distance from camera to the closest point on an object.
    Works with meshes and curves (including bevelled curves).
    Returns (distance, point_world) or (None, None) if not calculable.
    """
    if obj is None or camera is None:
        return None, None
    
    cam_loc = camera.location
    
    # Get evaluated object (applies modifiers, curve to mesh conversion, etc.)
    depsgraph = context.evaluated_depsgraph_get()
    
    try:
        # Get the evaluated version of the object
        obj_eval = obj.evaluated_get(depsgraph)
        
        # Try to get mesh data from the evaluated object
        # This works for meshes and curves (curves get converted to mesh)
        try:
            mesh = obj_eval.to_mesh()
        except RuntimeError:
            # Object can't be converted to mesh (e.g., empty, camera, light)
            return None, None
        
        if mesh is None or len(mesh.vertices) == 0:
            return None, None
        
        # Get the world matrix for transforming vertices
        world_matrix = obj.matrix_world
        
        min_dist = float('inf')
        closest_point = None
        
        # Check all vertices
        for vert in mesh.vertices:
            # Transform vertex to world space
            world_pos = world_matrix @ vert.co
            dist = (world_pos - cam_loc).length
            if dist < min_dist:
                min_dist = dist
                closest_point = world_pos.copy()
        
        # Clean up the temporary mesh
        obj_eval.to_mesh_clear()
        
        if min_dist == float('inf'):
            return None, None
        
        return min_dist, closest_point
        
    except Exception:
        return None, None


def format_distance_with_units(context, distance):
    """
    Format a distance value using Blender's current unit settings.
    Returns a formatted string with appropriate units.
    """
    unit_settings = context.scene.unit_settings
    
    if unit_settings.system == 'NONE':
        return f"{distance:.3f}"
    
    # Get the scale factor and unit system
    scale = unit_settings.scale_length
    scaled_distance = distance * scale
    
    if unit_settings.system == 'METRIC':
        if unit_settings.length_unit == 'ADAPTIVE':
            # Use adaptive units
            if scaled_distance >= 1000:
                return f"{scaled_distance / 1000:.3f} km"
            elif scaled_distance >= 1:
                return f"{scaled_distance:.3f} m"
            elif scaled_distance >= 0.01:
                return f"{scaled_distance * 100:.3f} cm"
            else:
                return f"{scaled_distance * 1000:.3f} mm"
        elif unit_settings.length_unit == 'KILOMETERS':
            return f"{scaled_distance / 1000:.3f} km"
        elif unit_settings.length_unit == 'METERS':
            return f"{scaled_distance:.3f} m"
        elif unit_settings.length_unit == 'CENTIMETERS':
            return f"{scaled_distance * 100:.3f} cm"
        elif unit_settings.length_unit == 'MILLIMETERS':
            return f"{scaled_distance * 1000:.3f} mm"
        elif unit_settings.length_unit == 'MICROMETERS':
            return f"{scaled_distance * 1000000:.3f} um"
        else:
            return f"{scaled_distance:.3f} m"
    
    elif unit_settings.system == 'IMPERIAL':
        # Convert meters to feet (1 meter = 3.28084 feet)
        feet = scaled_distance * 3.28084
        
        if unit_settings.length_unit == 'ADAPTIVE':
            if feet >= 5280:  # Miles threshold
                return f"{feet / 5280:.3f} mi"
            elif feet >= 1:
                return f"{feet:.3f} ft"
            else:
                return f"{feet * 12:.3f} in"
        elif unit_settings.length_unit == 'MILES':
            return f"{feet / 5280:.3f} mi"
        elif unit_settings.length_unit == 'FEET':
            return f"{feet:.3f} ft"
        elif unit_settings.length_unit == 'INCHES':
            return f"{feet * 12:.3f} in"
        elif unit_settings.length_unit == 'THOU':
            return f"{feet * 12000:.3f} thou"
        else:
            return f"{feet:.3f} ft"
    
    # Fallback
    return f"{distance:.3f}"


def get_plot_settings(context):
    return context.scene.focalplotter_plot


def get_axis_data(context, axis):
    return getattr(context.scene, f'focalplotter_axis_{axis}')


def get_camera_frame_bounds(context):
    """
    Get the camera frame bounds in screen coordinates using Blender's native API.
    Returns (min_x, min_y, width, height) or None.
    """
    from bpy_extras.view3d_utils import location_3d_to_region_2d
    
    region = context.region
    rv3d = context.space_data.region_3d if context.space_data else None
    camera = get_active_camera(context)
    
    if region is None or rv3d is None or camera is None:
        return None
    
    # Get camera view frame corners (in camera local space)
    frame = camera.data.view_frame(scene=context.scene)
    
    # Transform corners to world space
    cam_matrix = camera.matrix_world
    corners_world = [cam_matrix @ corner for corner in frame]
    
    # Project to screen coordinates
    corners_screen = []
    for corner in corners_world:
        screen_co = location_3d_to_region_2d(region, rv3d, corner)
        if screen_co is None:
            return None
        corners_screen.append((screen_co.x, screen_co.y))
    
    # Find bounding box from corners (handles any order)
    xs = [c[0] for c in corners_screen]
    ys = [c[1] for c in corners_screen]
    
    min_x, max_x = min(xs), max(xs)
    min_y, max_y = min(ys), max(ys)
    
    return (min_x, min_y, max_x - min_x, max_y - min_y)


def normalized_to_screen(context, norm_pos):
    """
    Convert normalized camera coords (0-1) to screen pixel coords.
    (0,0) = bottom-left of camera frame, (1,1) = top-right.
    """
    bounds = get_camera_frame_bounds(context)
    if bounds is None:
        return None
    
    min_x, min_y, width, height = bounds
    
    x = min_x + norm_pos[0] * width
    y = min_y + norm_pos[1] * height
    
    return (x, y)


def screen_to_normalized(context, screen_pos):
    """
    Convert screen pixel coords to normalized camera coords (0-1).
    """
    bounds = get_camera_frame_bounds(context)
    if bounds is None:
        return None
    
    min_x, min_y, width, height = bounds
    
    if width < 1 or height < 1:
        return None
    
    norm_x = (screen_pos[0] - min_x) / width
    norm_y = (screen_pos[1] - min_y) / height
    
    return (norm_x, norm_y)


def is_point_on_screen(context, screen_pos, margin=0):
    """Check if a screen position is visible in the region."""
    region = context.region
    if region is None:
        return False
    
    x, y = screen_pos
    return (-margin <= x <= region.width + margin and 
            -margin <= y <= region.height + margin)


def get_enabled_axes(settings):
    """Get the two enabled axes as a list [axis1, axis2]"""
    return [settings.axis_1, settings.axis_2]


def is_axis_enabled(settings, axis):
    """Check if an axis is one of the two selected axes"""
    return axis in (settings.axis_1, settings.axis_2)


def get_third_axis(settings):
    """Get the axis that is NOT selected (computed from the other two)"""
    all_axes = {'x', 'y', 'z'}
    selected = {settings.axis_1, settings.axis_2}
    return list(all_axes - selected)[0] if len(selected) == 2 else None


def line_intersection_2d(p1, p2, p3, p4):
    """
    Find intersection of two infinite lines defined by point pairs.
    Returns intersection point or None if lines are parallel.
    """
    x1, y1 = p1
    x2, y2 = p2
    x3, y3 = p3
    x4, y4 = p4
    
    denom = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4)
    if abs(denom) < 0.0001:
        return None  # Parallel lines
    
    t = ((x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)) / denom
    
    ix = x1 + t * (x2 - x1)
    iy = y1 + t * (y2 - y1)
    
    return (ix, iy)


def is_point_in_forward_direction(start, end, point):
    """Check if point is in the forward direction from start through end."""
    # Direction vector from start to end
    dx = end[0] - start[0]
    dy = end[1] - start[1]
    
    # Vector from end to point
    px = point[0] - end[0]
    py = point[1] - end[1]
    
    # Dot product: positive if point is beyond end in the direction start->end
    dot = dx * px + dy * py
    return dot > 0


def point_to_segment_distance(point, seg_start, seg_end):
    """
    Calculate distance from point to line segment.
    Returns (distance, t) where t is parameter along segment (0=start, 1=end).
    Only returns valid distance if point projects onto the segment (0 <= t <= 1).
    """
    px, py = point
    x1, y1 = seg_start
    x2, y2 = seg_end
    
    dx = x2 - x1
    dy = y2 - y1
    
    seg_len_sq = dx * dx + dy * dy
    if seg_len_sq < 0.0001:
        # Segment is a point
        dist = math.sqrt((px - x1)**2 + (py - y1)**2)
        return dist, 0.5
    
    # Project point onto line, clamped to segment
    t = ((px - x1) * dx + (py - y1) * dy) / seg_len_sq
    
    if t < 0 or t > 1:
        # Point projects outside segment
        return float('inf'), t
    
    # Closest point on segment
    closest_x = x1 + t * dx
    closest_y = y1 + t * dy
    
    dist = math.sqrt((px - closest_x)**2 + (py - closest_y)**2)
    return dist, t


def get_vanishing_point_any_direction(context, axis):
    """Get vanishing point regardless of direction (for camera solving) - in screen coords"""
    p1, p2, p3, p4 = get_screen_coords_for_axis(context, axis)
    
    if None in (p1, p2, p3, p4):
        return None
    
    return line_intersection_2d(p1, p2, p3, p4)


# ---------------------------------------------------------------------------
# Camera Solving
# ---------------------------------------------------------------------------

def solve_camera_from_vanishing_points(context, vp1, vp2, axis1, axis2):
    """
    Solve camera rotation and focal length from two vanishing points.
    
    Uses the standard pinhole camera model where perpendicular world axes
    have vanishing points satisfying: VP1·VP2 = -f² (relative to principal point)
    """
    # Get camera frame bounds to find the correct center and size
    bounds = get_camera_frame_bounds(context)
    if bounds is None:
        return None, None
    
    min_x, min_y, frame_w, frame_h = bounds
    
    if frame_w < 1 or frame_h < 1:
        return None, None
    
    # Camera frame center (in screen pixels), with principal point offset
    settings = get_plot_settings(context)
    pp_offset_x = settings.principal_point_x * frame_w
    pp_offset_y = settings.principal_point_y * frame_h
    
    cx = min_x + frame_w / 2 + pp_offset_x
    cy = min_y + frame_h / 2 + pp_offset_y
    
    # Vanishing points relative to (offset) optical center
    # Screen coords: X right, Y up (Blender convention)
    u1, v1 = vp1[0] - cx, vp1[1] - cy
    u2, v2 = vp2[0] - cx, vp2[1] - cy
    
    # Focal length from orthogonality constraint: d1·d2 = 0
    # For directions (u1,v1,f) and (u2,v2,f): u1*u2 + v1*v2 + f² = 0
    dot_uv = u1 * u2 + v1 * v2
    
    if dot_uv >= 0:
        f_pixels = frame_w * 0.8  # Default if constraint not satisfied
    else:
        f_pixels = math.sqrt(-dot_uv)
    
    f_pixels = max(frame_w * 0.25, min(f_pixels, frame_w * 2.5))
    
    camera = get_active_camera(context)
    if camera is None:
        return None, None
    
    # Convert to mm
    cam_data = camera.data
    sensor_width = cam_data.sensor_width
    render_w = context.scene.render.resolution_x
    
    scale = render_w / frame_w
    focal_length_mm = (f_pixels * scale / render_w) * sensor_width
    focal_length_mm = max(15, min(focal_length_mm, 150))
    
    # Build direction vectors in a camera-centric coordinate system:
    # - cam_x: points RIGHT in the image
    # - cam_y: points UP in the image
    # - cam_z: points TOWARD VIEWER (out of screen) - this matches Blender's camera +Z
    #
    # A vanishing point at (u, v) on screen corresponds to a world direction
    # that projects to that point. The ray direction FROM camera is (u, v, -f)
    # because the image plane is in front of the camera.
    # But we want the direction TO the vanishing point which is (u, v, f) 
    # interpreted as pointing away from camera into the scene.
    #
    # In our system where +Z is toward viewer:
    # direction into scene = (u, v, -f) normalized
    
    d1 = Vector((u1, v1, -f_pixels)).normalized()
    d2 = Vector((u2, v2, -f_pixels)).normalized()
    
    # d1 and d2 are the directions of world axes axis1 and axis2 
    # as seen from the camera (pointing into the scene)
    # However, we don't know if they point in the + or - direction of each axis
    
    # Apply sign conventions based on typical camera orientations:
    # - Z axis (vertical) should point "up" in camera view (positive camera Y component)
    # - For horizontal axes (X, Y), positive directions typically have positive camera X or Y
    
    # Fix Z axis orientation: if Z is selected, it should point up in camera space
    if axis1 == 'z' and d1.y < 0:
        d1 = -d1
    if axis2 == 'z' and d2.y < 0:
        d2 = -d2
    
    # For horizontal axes, ensure consistent orientation
    # X axis should generally point right (positive camera X)
    # Y axis direction depends on the scene
    if axis1 == 'x' and d1.x < 0:
        d1 = -d1
    if axis2 == 'x' and d2.x < 0:
        d2 = -d2
    
    # Figure out third world axis
    all_axes = ['x', 'y', 'z']
    third_axis = [a for a in all_axes if a != axis1 and a != axis2][0]
    
    # World axis vectors  
    world_vecs = {
        'x': Vector((1, 0, 0)),
        'y': Vector((0, 1, 0)),
        'z': Vector((0, 0, 1)),
    }
    
    # Gram-Schmidt orthogonalization preserving d1 and keeping d2 close
    e1 = d1.normalized()
    e2_temp = d2 - d2.dot(e1) * e1
    if e2_temp.length < 0.001:
        return focal_length_mm, None
    e2 = e2_temp.normalized()
    
    # Third axis via cross product with correct handedness
    # For right-handed coordinates: X = Y × Z, Y = Z × X, Z = X × Y
    e3 = e1.cross(e2)
    
    # Ensure correct handedness for the third axis
    expected_cross = world_vecs[axis1].cross(world_vecs[axis2])
    if expected_cross.dot(world_vecs[third_axis]) < 0:
        e3 = -e3
    
    # Map each world axis to its camera-space direction
    dirs = {axis1: e1, axis2: e2, third_axis: e3}
    
    # Build rotation matrix from camera to world
    # If world axis W appears in direction D in camera space,
    # then the camera's axes in world space are given by the inverse mapping
    #
    # Camera basis vectors in world coords:
    # cam_right (X) should map world directions to camera X
    # cam_up (Y) should map world directions to camera Y  
    # cam_forward (-Z for Blender) points into scene
    #
    # The matrix with dirs as rows transforms camera coords to world coords
    cam_x_world = Vector((dirs['x'][0], dirs['y'][0], dirs['z'][0]))
    cam_y_world = Vector((dirs['x'][1], dirs['y'][1], dirs['z'][1]))
    cam_z_world = Vector((dirs['x'][2], dirs['y'][2], dirs['z'][2]))
    
    # This gives us where camera's X, Y, Z axes point in world space
    # For Blender camera: X=right, Y=up, -Z=forward (into scene)
    # Our cam_z_world points toward viewer, so -cam_z_world points into scene
    
    # Check if camera is upside down: camera's Y (up in image) should have
    # positive Z component in world space (i.e., "up" in image = "up" in world)
    # If not, we need to flip the camera 180 degrees around its view axis
    if cam_y_world.z < 0:
        # Camera is upside down - flip by negating X and Y axes
        cam_x_world = -cam_x_world
        cam_y_world = -cam_y_world
    
    # Construct rotation matrix (camera to world)
    R = Matrix((cam_x_world, cam_y_world, cam_z_world)).transposed()
    
    try:
        euler = R.to_euler('XYZ')
    except Exception:
        euler = Euler((0, 0, 0))
    
    return focal_length_mm, euler


def update_camera_position_only(context):
    """Update camera position based on current rotation, distance and target (no re-solve)"""
    settings = get_plot_settings(context)
    camera = get_active_camera(context)
    
    if camera is None:
        return
    
    target = Vector(settings.target_location) if settings.target_mode == 'MANUAL' else Vector((0, 0, 0))
    distance = settings.camera_distance
    
    # Use existing camera rotation
    rot_matrix = camera.rotation_euler.to_matrix()
    forward = rot_matrix @ Vector((0, 0, -1))
    
    # Position camera at distance from target
    camera.location = target - forward * distance


def update_focal_length_only(context):
    """Update only focal length based on principal point, without changing rotation"""
    settings = get_plot_settings(context)
    camera = get_active_camera(context)
    
    if camera is None:
        return
    
    # Get selected axes
    axis1, axis2 = settings.axis_1, settings.axis_2
    if axis1 == axis2:
        return
    
    vp1 = get_vanishing_point_any_direction(context, axis1)
    vp2 = get_vanishing_point_any_direction(context, axis2)
    
    if vp1 is None or vp2 is None:
        return
    
    # Get camera frame bounds
    bounds = get_camera_frame_bounds(context)
    if bounds is None:
        return
    
    min_x, min_y, frame_w, frame_h = bounds
    
    if frame_w < 1 or frame_h < 1:
        return
    
    # Camera frame center with principal point offset
    pp_offset_x = settings.principal_point_x * frame_w
    pp_offset_y = settings.principal_point_y * frame_h
    
    cx = min_x + frame_w / 2 + pp_offset_x
    cy = min_y + frame_h / 2 + pp_offset_y
    
    # Vanishing points relative to (offset) optical center
    u1, v1 = vp1[0] - cx, vp1[1] - cy
    u2, v2 = vp2[0] - cx, vp2[1] - cy
    
    # Focal length from orthogonality constraint
    dot_uv = u1 * u2 + v1 * v2
    
    if dot_uv >= 0:
        f_pixels = frame_w * 0.8
    else:
        f_pixels = math.sqrt(-dot_uv)
    
    f_pixels = max(frame_w * 0.25, min(f_pixels, frame_w * 2.5))
    
    # Convert to mm
    cam_data = camera.data
    sensor_width = cam_data.sensor_width
    render_w = context.scene.render.resolution_x
    
    scale = render_w / frame_w
    focal_length_mm = (f_pixels * scale / render_w) * sensor_width
    focal_length_mm = max(15, min(focal_length_mm, 150))
    
    # Only update focal length, keep rotation unchanged
    camera.data.lens = focal_length_mm


def update_camera_from_vanishing_points(context, force=False):
    """Update camera based on current vanishing points"""
    settings = get_plot_settings(context)
    camera = get_active_camera(context)
    
    if camera is None:
        return
    
    # Skip if auto_solve is disabled, unless forced
    if not settings.auto_solve and not force:
        return
    
    # Get the two selected axes
    axis1, axis2 = settings.axis_1, settings.axis_2
    
    # Skip if same axis selected for both (invalid)
    if axis1 == axis2:
        return
    
    # Use any direction for camera solving (VP can be behind lines)
    vp1 = get_vanishing_point_any_direction(context, axis1)
    vp2 = get_vanishing_point_any_direction(context, axis2)
    
    if vp1 is None or vp2 is None:
        return
    
    focal_length, rotation = solve_camera_from_vanishing_points(
        context, vp1, vp2, axis1, axis2
    )
    
    if focal_length is not None:
        camera.data.lens = focal_length
    
    if rotation is not None:
        camera.rotation_euler = rotation
        
        # Apply camera distance and target location
        target = Vector(settings.target_location) if settings.target_mode == 'MANUAL' else Vector((0, 0, 0))
        distance = settings.camera_distance
        
        # Camera forward direction (looks down -Z local)
        rot_matrix = rotation.to_matrix()
        forward = rot_matrix @ Vector((0, 0, -1))
        
        # Position camera at distance from target
        camera.location = target - forward * distance


# ---------------------------------------------------------------------------
# Drawing
# ---------------------------------------------------------------------------

_draw_handler = None

AXIS_COLORS = {
    'x': (1.0, 0.2, 0.3, 0.9),
    'y': (0.5, 0.8, 0.2, 0.9),
    'z': (0.3, 0.5, 1.0, 0.9),
}

DOT_RADIUS = 8

# Global drag state
_drag_state = {
    'active': False,
    'drag_type': 'point',  # 'point' or 'line'
    'axis': None,
    'line_index': 0,
    'point_index': 0,
    'init_pos': None,
    'init_mouse': None,
    'last_mouse': None,
    'current_pos': None,
    # For line dragging - store both endpoints
    'init_pos_start': None,
    'init_pos_end': None,
    'current_pos_start': None,
    'current_pos_end': None,
    'precise_mode': False,
}


def draw_circle_2d(center, radius, color, shader, filled=True):
    segments = 20
    
    if filled:
        verts = [center]
        for i in range(segments + 1):
            angle = 2 * math.pi * i / segments
            verts.append((center[0] + radius * math.cos(angle),
                         center[1] + radius * math.sin(angle)))
        
        indices = [(0, i, i + 1 if i < segments else 1) for i in range(1, segments + 1)]
        batch = batch_for_shader(shader, 'TRIS', {"pos": verts}, indices=indices)
    else:
        verts = []
        for i in range(segments + 1):
            angle = 2 * math.pi * i / segments
            verts.append((center[0] + radius * math.cos(angle),
                         center[1] + radius * math.sin(angle)))
        batch = batch_for_shader(shader, 'LINE_STRIP', {"pos": verts})
    
    shader.bind()
    shader.uniform_float("color", color)
    batch.draw(shader)


def get_screen_coords_for_axis(context, axis):
    """Get screen coordinates for an axis, converting from normalized storage."""
    data = get_axis_data(context, axis)
    
    p1 = normalized_to_screen(context, tuple(data.line1_start))
    p2 = normalized_to_screen(context, tuple(data.line1_end))
    p3 = normalized_to_screen(context, tuple(data.line2_start))
    p4 = normalized_to_screen(context, tuple(data.line2_end))
    
    return p1, p2, p3, p4


def draw_focalplotter():
    """Main draw callback"""
    context = bpy.context
    
    if not is_in_camera_view(context):
        return
    
    settings = get_plot_settings(context)
    if not settings.is_plotting:
        return
    
    region = context.region
    if region is None:
        return
    
    shader = gpu.shader.from_builtin('UNIFORM_COLOR')
    gpu.state.blend_set('ALPHA')
    gpu.state.line_width_set(2.0)
    
    # Draw the two selected axes
    axes_to_draw = [settings.axis_1, settings.axis_2]
    
    for axis in axes_to_draw:
        color = AXIS_COLORS[axis]
        
        # Convert normalized coords to screen coords
        p1, p2, p3, p4 = get_screen_coords_for_axis(context, axis)
        
        if None in (p1, p2, p3, p4):
            continue
        
        # Get vanishing point (intersection of the two lines) in screen coords
        vp = line_intersection_2d(p1, p2, p3, p4)
        
        # Draw line 1: always draw segment between dots
        batch = batch_for_shader(shader, 'LINES', {"pos": [p1, p2]})
        shader.bind()
        shader.uniform_float("color", color)
        batch.draw(shader)
        
        # Draw line 2: always draw segment between dots
        batch = batch_for_shader(shader, 'LINES', {"pos": [p3, p4]})
        shader.bind()
        shader.uniform_float("color", color)
        batch.draw(shader)
        
        # If vanishing point exists, draw rays from segment endpoints to VP
        if vp:
            ray_color = (*color[:3], 0.4)  # Dimmer for the ray extension
            
            # For line 1: check which endpoint the VP is beyond
            # VP beyond p2 (in direction p1->p2) OR VP beyond p1 (in direction p2->p1)
            vp_beyond_p2 = is_point_in_forward_direction(p1, p2, vp)
            vp_beyond_p1 = is_point_in_forward_direction(p2, p1, vp)
            
            if vp_beyond_p2:
                batch = batch_for_shader(shader, 'LINES', {"pos": [p2, vp]})
                shader.bind()
                shader.uniform_float("color", ray_color)
                batch.draw(shader)
            elif vp_beyond_p1:
                batch = batch_for_shader(shader, 'LINES', {"pos": [p1, vp]})
                shader.bind()
                shader.uniform_float("color", ray_color)
                batch.draw(shader)
            
            # For line 2: check which endpoint the VP is beyond
            vp_beyond_p4 = is_point_in_forward_direction(p3, p4, vp)
            vp_beyond_p3 = is_point_in_forward_direction(p4, p3, vp)
            
            if vp_beyond_p4:
                batch = batch_for_shader(shader, 'LINES', {"pos": [p4, vp]})
                shader.bind()
                shader.uniform_float("color", ray_color)
                batch.draw(shader)
            elif vp_beyond_p3:
                batch = batch_for_shader(shader, 'LINES', {"pos": [p3, vp]})
                shader.bind()
                shader.uniform_float("color", ray_color)
                batch.draw(shader)
            
            # Draw vanishing point marker if VP is beyond both segments (not inside them)
            line1_has_ray = vp_beyond_p2 or vp_beyond_p1
            line2_has_ray = vp_beyond_p4 or vp_beyond_p3
            if line1_has_ray and line2_has_ray:
                draw_circle_2d(vp, DOT_RADIUS + 4, (*color[:3], 0.6), shader, filled=False)
        
        # Draw dots (skip dragged dot in precise mode, skip off-screen dots)
        for point in [(p1, 1, 0), (p2, 1, 1), (p3, 2, 0), (p4, 2, 1)]:
            pos, li, pi = point
            
            # Skip if this is the dragged dot in precise mode
            if (_drag_state['precise_mode'] and _drag_state['active'] and
                _drag_state['axis'] == axis and
                _drag_state['line_index'] == li and
                _drag_state['point_index'] == pi):
                continue
            
            # Skip if dot is off-screen (rays still render, dot position preserved)
            if not is_point_on_screen(context, pos, margin=DOT_RADIUS):
                continue
            
            draw_circle_2d(pos, DOT_RADIUS, color, shader)
    
    # Draw horizon line (only when both X and Y are selected - the true horizon)
    # Horizon is the vanishing line of the ground plane (X-Y), doesn't apply to Z combinations
    has_horizon = 'x' in (settings.axis_1, settings.axis_2) and 'y' in (settings.axis_1, settings.axis_2)
    
    if settings.show_horizon and has_horizon:
        x_p1, x_p2, x_p3, x_p4 = get_screen_coords_for_axis(context, 'x')
        y_p1, y_p2, y_p3, y_p4 = get_screen_coords_for_axis(context, 'y')
        
        if None not in (x_p1, x_p2, x_p3, x_p4, y_p1, y_p2, y_p3, y_p4):
            vp_x = line_intersection_2d(x_p1, x_p2, x_p3, x_p4)
            vp_y = line_intersection_2d(y_p1, y_p2, y_p3, y_p4)
            
            if vp_x and vp_y:
                # Draw horizon line through both vanishing points
                horizon_color = (1.0, 1.0, 0.0, 0.5)  # Yellow
                gpu.state.line_width_set(1.0)
                
                # Extend the line across the screen
                dx = vp_y[0] - vp_x[0]
                dy = vp_y[1] - vp_x[1]
                length = math.sqrt(dx*dx + dy*dy)
                if length > 0.001:
                    # Normalize and extend
                    dx, dy = dx/length, dy/length
                    extend = 5000  # Extend far beyond screen
                    p_start = (vp_x[0] - dx * extend, vp_x[1] - dy * extend)
                    p_end = (vp_y[0] + dx * extend, vp_y[1] + dy * extend)
                    
                    batch = batch_for_shader(shader, 'LINES', {"pos": [p_start, p_end]})
                    shader.bind()
                    shader.uniform_float("color", horizon_color)
                    batch.draw(shader)
    
    gpu.state.blend_set('NONE')
    gpu.state.line_width_set(1.0)


def register_draw_handler():
    global _draw_handler
    if _draw_handler is None:
        _draw_handler = bpy.types.SpaceView3D.draw_handler_add(
            draw_focalplotter, (), 'WINDOW', 'POST_PIXEL'
        )


def unregister_draw_handler():
    global _draw_handler
    if _draw_handler is not None:
        bpy.types.SpaceView3D.draw_handler_remove(_draw_handler, 'WINDOW')
        _draw_handler = None


# ---------------------------------------------------------------------------
# Interactive Modal Operator
# ---------------------------------------------------------------------------

class FOCALPLOTTER_OT_interactive(Operator):
    """Interactive mode - click and drag dots directly"""
    bl_idname = "focalplotter.interactive"
    bl_label = "FocalPlotter Interactive"
    bl_options = {'REGISTER'}

    def find_nearest_point(self, context, mouse_pos):
        """Find nearest dot to mouse position. Returns (axis, line_idx, point_idx, screen_pos) or None."""
        settings = get_plot_settings(context)
        
        # Use the two selected axes
        axes = [settings.axis_1, settings.axis_2]
        
        nearest = None
        min_dist = DOT_RADIUS + 10
        
        for axis in axes:
            # Get screen coordinates
            p1, p2, p3, p4 = get_screen_coords_for_axis(context, axis)
            if None in (p1, p2, p3, p4):
                continue
            
            points = [
                (p1, axis, 1, 0),
                (p2, axis, 1, 1),
                (p3, axis, 2, 0),
                (p4, axis, 2, 1),
            ]
            
            for pos, ax, line_idx, point_idx in points:
                dist = math.sqrt((pos[0] - mouse_pos[0])**2 + (pos[1] - mouse_pos[1])**2)
                if dist < min_dist:
                    min_dist = dist
                    nearest = (ax, line_idx, point_idx, pos)
        
        return nearest

    def find_nearest_line(self, context, mouse_pos):
        """Find if mouse is near a line segment (not dots, not rays). Returns (axis, line_idx, start, end) in screen coords or None."""
        settings = get_plot_settings(context)
        
        # Use the two selected axes
        axes = [settings.axis_1, settings.axis_2]
        
        nearest = None
        min_dist = 8  # Pixel threshold for line detection
        
        for axis in axes:
            # Get screen coordinates
            p1, p2, p3, p4 = get_screen_coords_for_axis(context, axis)
            if None in (p1, p2, p3, p4):
                continue
            
            lines = [
                (p1, p2, axis, 1),
                (p3, p4, axis, 2),
            ]
            
            for start, end, ax, line_idx in lines:
                dist, t = point_to_segment_distance(mouse_pos, start, end)
                if dist < min_dist:
                    min_dist = dist
                    nearest = (ax, line_idx, start, end)
        
        return nearest

    def get_prop_name(self, line_index, point_index):
        if line_index == 1:
            return "line1_start" if point_index == 0 else "line1_end"
        else:
            return "line2_start" if point_index == 0 else "line2_end"

    def invoke(self, context, event):
        settings = get_plot_settings(context)
        if not settings.is_plotting:
            return {'CANCELLED'}
        
        context.window_manager.modal_handler_add(self)
        return {'RUNNING_MODAL'}

    def modal(self, context, event):
        global _drag_state
        
        settings = get_plot_settings(context)
        
        # Stop if not plotting or left camera view
        if not settings.is_plotting:
            _drag_state['active'] = False
            context.window.cursor_set('DEFAULT')
            return {'FINISHED'}
        
        if not is_in_camera_view(context):
            # Pause dragging when not in camera view, but keep plotting active
            _drag_state['active'] = False
            context.window.cursor_set('DEFAULT')
            return {'PASS_THROUGH'}
        
        mouse = (event.mouse_region_x, event.mouse_region_y)
        
        # Check freeze state - don't allow new drags when frozen
        is_frozen = settings.freeze_guides
        
        # Check if mouse is over a dot (has priority over line)
        nearest_point = self.find_nearest_point(context, mouse)
        is_over_dot = nearest_point is not None and not is_frozen
        
        # Check if mouse is over a line segment (only if not over dot)
        nearest_line = None
        if not is_over_dot and not is_frozen:
            nearest_line = self.find_nearest_line(context, mouse)
        is_over_line = nearest_line is not None
        
        if event.type == 'MOUSEMOVE':
            if _drag_state['active']:
                # Currently dragging
                _drag_state['precise_mode'] = event.shift
                
                if event.shift:
                    context.window.cursor_set('NONE')
                else:
                    context.window.cursor_set('SCROLL_XY' if _drag_state['drag_type'] == 'line' else 'HAND')
                
                data = get_axis_data(context, _drag_state['axis'])
                
                # Calculate incremental delta from last mouse position (in screen coords)
                delta = Vector(mouse) - Vector(_drag_state['last_mouse'])
                
                # Apply precision factor only to this frame's movement
                if event.shift:
                    delta *= 0.1
                
                if _drag_state['drag_type'] == 'line':
                    # Line dragging - move both endpoints (in screen space)
                    new_start = Vector(_drag_state['current_pos_start']) + delta
                    new_end = Vector(_drag_state['current_pos_end']) + delta
                    _drag_state['current_pos_start'] = (new_start.x, new_start.y)
                    _drag_state['current_pos_end'] = (new_end.x, new_end.y)
                    _drag_state['last_mouse'] = mouse
                    
                    # Convert screen coords to normalized and store
                    norm_start = screen_to_normalized(context, (new_start.x, new_start.y))
                    norm_end = screen_to_normalized(context, (new_end.x, new_end.y))
                    if norm_start and norm_end:
                        start_prop = self.get_prop_name(_drag_state['line_index'], 0)
                        end_prop = self.get_prop_name(_drag_state['line_index'], 1)
                        setattr(data, start_prop, norm_start)
                        setattr(data, end_prop, norm_end)
                else:
                    # Point dragging (in screen space)
                    new_pos = Vector(_drag_state['current_pos']) + delta
                    _drag_state['current_pos'] = (new_pos.x, new_pos.y)
                    _drag_state['last_mouse'] = mouse
                    
                    # Convert screen coords to normalized and store
                    norm_pos = screen_to_normalized(context, (new_pos.x, new_pos.y))
                    if norm_pos:
                        prop_name = self.get_prop_name(_drag_state['line_index'], _drag_state['point_index'])
                        setattr(data, prop_name, norm_pos)
                
                update_camera_from_vanishing_points(context)
                context.area.tag_redraw()
                
                return {'RUNNING_MODAL'}  # Consume event while dragging
            else:
                # Not dragging - update cursor
                if is_over_dot:
                    context.window.cursor_set('HAND')
                elif is_over_line:
                    context.window.cursor_set('SCROLL_XY')
                else:
                    context.window.cursor_set('DEFAULT')
                return {'PASS_THROUGH'}
        
        elif event.type == 'LEFTMOUSE' and event.value == 'PRESS':
            if is_over_dot:
                # Start point drag - pos is in screen coords
                axis, line_idx, point_idx, pos = nearest_point
                # Store normalized initial position for cancel
                data = get_axis_data(context, axis)
                prop_name = self.get_prop_name(line_idx, point_idx)
                init_norm = tuple(getattr(data, prop_name))
                
                _drag_state['active'] = True
                _drag_state['drag_type'] = 'point'
                _drag_state['axis'] = axis
                _drag_state['line_index'] = line_idx
                _drag_state['point_index'] = point_idx
                _drag_state['init_pos'] = init_norm  # Normalized for cancel
                _drag_state['init_mouse'] = mouse
                _drag_state['last_mouse'] = mouse
                _drag_state['current_pos'] = pos  # Screen coords for dragging
                _drag_state['precise_mode'] = event.shift
                context.window.cursor_set('HAND')
                return {'RUNNING_MODAL'}  # Consume to prevent box select
            elif is_over_line:
                # Start line drag - start/end are in screen coords
                axis, line_idx, start, end = nearest_line
                # Store normalized initial positions for cancel
                data = get_axis_data(context, axis)
                start_prop = self.get_prop_name(line_idx, 0)
                end_prop = self.get_prop_name(line_idx, 1)
                init_norm_start = tuple(getattr(data, start_prop))
                init_norm_end = tuple(getattr(data, end_prop))
                
                _drag_state['active'] = True
                _drag_state['drag_type'] = 'line'
                _drag_state['axis'] = axis
                _drag_state['line_index'] = line_idx
                _drag_state['init_pos_start'] = init_norm_start  # Normalized for cancel
                _drag_state['init_pos_end'] = init_norm_end  # Normalized for cancel
                _drag_state['init_mouse'] = mouse
                _drag_state['last_mouse'] = mouse
                _drag_state['current_pos_start'] = start  # Screen coords for dragging
                _drag_state['current_pos_end'] = end  # Screen coords for dragging
                _drag_state['precise_mode'] = event.shift
                context.window.cursor_set('SCROLL_XY')
                return {'RUNNING_MODAL'}  # Consume to prevent box select
            return {'PASS_THROUGH'}
        
        elif event.type == 'LEFTMOUSE' and event.value == 'RELEASE':
            if _drag_state['active']:
                # End drag
                _drag_state['active'] = False
                _drag_state['precise_mode'] = False
                context.window.cursor_set('DEFAULT')
                context.area.tag_redraw()
                return {'RUNNING_MODAL'}
            return {'PASS_THROUGH'}
        
        elif event.type in {'RIGHTMOUSE', 'ESC'} and event.value == 'PRESS':
            if _drag_state['active']:
                # Cancel drag - restore original position(s)
                data = get_axis_data(context, _drag_state['axis'])
                if _drag_state['drag_type'] == 'line':
                    # Restore both endpoints
                    start_prop = self.get_prop_name(_drag_state['line_index'], 0)
                    end_prop = self.get_prop_name(_drag_state['line_index'], 1)
                    setattr(data, start_prop, _drag_state['init_pos_start'])
                    setattr(data, end_prop, _drag_state['init_pos_end'])
                else:
                    # Restore single point
                    prop_name = self.get_prop_name(_drag_state['line_index'], _drag_state['point_index'])
                    setattr(data, prop_name, _drag_state['init_pos'])
                _drag_state['active'] = False
                _drag_state['precise_mode'] = False
                context.window.cursor_set('DEFAULT')
                context.area.tag_redraw()
                return {'RUNNING_MODAL'}
            # ESC without dragging - exit interactive mode but keep plotting
            context.window.cursor_set('DEFAULT')
            return {'FINISHED'}
        
        return {'PASS_THROUGH'}


# ---------------------------------------------------------------------------
# Main Operators
# ---------------------------------------------------------------------------

class FOCALPLOTTER_OT_plot_camera(Operator):
    """Toggle focal point plotting mode"""
    bl_idname = "focalplotter.plot_camera"
    bl_label = "Plot Camera"
    bl_options = {'REGISTER', 'UNDO'}

    @classmethod
    def poll(cls, context):
        return is_in_camera_view(context) and get_active_camera(context) is not None

    def execute(self, context):
        settings = get_plot_settings(context)
        camera = get_active_camera(context)
        
        if camera is None:
            self.report({'ERROR'}, "No active camera")
            return {'CANCELLED'}
        
        cam_data = camera.data
        
        if settings.is_plotting:
            # STOP PLOTTING
            
            # If auto-solve was disabled, solve once now (force=True)
            if not settings.auto_solve:
                update_camera_from_vanishing_points(context, force=True)
            
            settings.is_plotting = False
            
            if settings.modified_camera == camera.name:
                cam_data.show_passepartout = bool(settings.original_passepartout[0])
                cam_data.passepartout_alpha = settings.original_passepartout[1]
                cam_data.show_name = settings.original_show_name
                cam_data.show_composition_thirds = settings.original_show_composition_thirds
                # Only restore bg images setting if not keeping the bg image
                if not settings.keep_bg_image:
                    cam_data.show_background_images = settings.original_show_bg_images
            
            settings.modified_camera = ""
            self.report({'INFO'}, "Plotting stopped")
        else:
            # START PLOTTING
            # Ensure different axes are selected
            if settings.axis_1 == settings.axis_2:
                settings.axis_1 = 'x'
                settings.axis_2 = 'y'
            
            if settings.use_reference_image and settings.reference_image is None:
                self.report({'WARNING'}, "Reference image is enabled but not set!")
                return {'CANCELLED'}
            
            # Auto-set resolution
            # Set render resolution if enabled
            if settings.set_render_resolution and settings.reference_image is not None:
                img = settings.reference_image
                if img.size[0] > 0 and img.size[1] > 0:
                    context.scene.render.resolution_x = img.size[0]
                    context.scene.render.resolution_y = img.size[1]
            
            # Store original settings
            settings.original_passepartout = (
                float(cam_data.show_passepartout),
                cam_data.passepartout_alpha,
            )
            settings.original_show_name = cam_data.show_name
            settings.original_show_composition_thirds = cam_data.show_composition_thirds
            settings.original_show_bg_images = cam_data.show_background_images
            settings.modified_camera = camera.name
            
            # Apply camera settings
            cam_data.show_passepartout = True
            cam_data.passepartout_alpha = 1.0
            cam_data.show_name = True
            
            if settings.use_reference_image and settings.reference_image is not None:
                cam_data.show_background_images = True
                if len(cam_data.background_images) == 0:
                    bg = cam_data.background_images.new()
                else:
                    bg = cam_data.background_images[0]
                bg.image = settings.reference_image
                bg.show_background_image = True
                bg.alpha = 1.0
            
            if settings.use_rule_of_thirds:
                cam_data.show_composition_thirds = True
            
            settings.is_plotting = True
            
            # Initialize with converging defaults (in normalized coords 0-1)
            # X axis (red) - lines pointing right, converging toward right
            x_data = get_axis_data(context, 'x')
            x_data.line1_start = (0.15, 0.55)
            x_data.line1_end = (0.55, 0.52)
            x_data.line2_start = (0.15, 0.45)
            x_data.line2_end = (0.55, 0.48)
            
            # Y axis (green) - lines pointing left, converging toward left
            y_data = get_axis_data(context, 'y')
            y_data.line1_start = (0.85, 0.55)
            y_data.line1_end = (0.45, 0.52)
            y_data.line2_start = (0.85, 0.45)
            y_data.line2_end = (0.45, 0.48)
            
            # Z axis (blue) - lines pointing up, converging toward top
            z_data = get_axis_data(context, 'z')
            z_data.line1_start = (0.45, 0.15)
            z_data.line1_end = (0.48, 0.55)
            z_data.line2_start = (0.55, 0.15)
            z_data.line2_end = (0.52, 0.55)
            
            # Start interactive mode
            bpy.ops.focalplotter.interactive('INVOKE_DEFAULT')
            
            self.report({'INFO'}, "Plotting started")
        
        for area in context.screen.areas:
            if area.type == 'VIEW_3D':
                area.tag_redraw()
        
        return {'FINISHED'}



class FOCALPLOTTER_OT_target_to_cursor(Operator):
    """Set target location to 3D cursor"""
    bl_idname = "focalplotter.target_to_cursor"
    bl_label = "Target to Cursor"
    bl_options = {'REGISTER', 'UNDO'}

    def execute(self, context):
        settings = get_plot_settings(context)
        settings.target_location = context.scene.cursor.location.copy()
        settings.target_mode = 'MANUAL'
        
        if settings.is_plotting:
            update_camera_from_vanishing_points(context)
        
        context.area.tag_redraw()
        return {'FINISHED'}


class FOCALPLOTTER_OT_flatten_horizon(Operator):
    """Rotate all points around image center to make horizon level (requires X and Y axes)"""
    bl_idname = "focalplotter.flatten_horizon"
    bl_label = "Flatten Horizon"
    bl_options = {'REGISTER', 'UNDO'}

    def execute(self, context):
        settings = get_plot_settings(context)
        
        # Horizon only exists with X and Y axes (horizontal plane)
        has_x = 'x' in (settings.axis_1, settings.axis_2)
        has_y = 'y' in (settings.axis_1, settings.axis_2)
        if not (has_x and has_y):
            self.report({'WARNING'}, "Need both X and Y axes selected (horizon = X+Y plane)")
            return {'CANCELLED'}
        
        # Get vanishing points in screen coords
        x_p1, x_p2, x_p3, x_p4 = get_screen_coords_for_axis(context, 'x')
        y_p1, y_p2, y_p3, y_p4 = get_screen_coords_for_axis(context, 'y')
        
        if None in (x_p1, x_p2, x_p3, x_p4, y_p1, y_p2, y_p3, y_p4):
            self.report({'ERROR'}, "Could not compute screen coordinates")
            return {'CANCELLED'}
        
        vp_x = line_intersection_2d(x_p1, x_p2, x_p3, x_p4)
        vp_y = line_intersection_2d(y_p1, y_p2, y_p3, y_p4)
        
        if vp_x is None or vp_y is None:
            self.report({'WARNING'}, "Vanishing points are parallel")
            return {'CANCELLED'}
        
        # Calculate angle of horizon line between the two VPs
        dx = vp_y[0] - vp_x[0]
        dy = vp_y[1] - vp_x[1]
        line_angle = math.atan2(dy, dx)
        
        # Only rotate by small amounts - limit to +/- 45 degrees to avoid flipping
        # If line is more than 45 degrees off, something is very wrong
        if abs(line_angle) > math.pi / 4:
            # Check if we should rotate the other way (closer to horizontal)
            if line_angle > math.pi / 4:
                line_angle = line_angle - math.pi
            elif line_angle < -math.pi / 4:
                line_angle = line_angle + math.pi
        
        # Clamp to reasonable range
        line_angle = max(-math.pi / 4, min(math.pi / 4, line_angle))
        
        if abs(line_angle) < 0.001:
            self.report({'INFO'}, "Line is already level")
            return {'FINISHED'}
        
        # We need to rotate all points by -line_angle around the image center
        # to make the vanishing line horizontal
        center = (0.5, 0.5)  # Normalized center
        cos_a = math.cos(-line_angle)
        sin_a = math.sin(-line_angle)
        
        def rotate_point(p, center, cos_a, sin_a):
            px, py = p[0] - center[0], p[1] - center[1]
            rx = px * cos_a - py * sin_a
            ry = px * sin_a + py * cos_a
            return (rx + center[0], ry + center[1])
        
        # Rotate the two selected axis points
        for axis in [settings.axis_1, settings.axis_2]:
            data = get_axis_data(context, axis)
            data.line1_start = rotate_point(tuple(data.line1_start), center, cos_a, sin_a)
            data.line1_end = rotate_point(tuple(data.line1_end), center, cos_a, sin_a)
            data.line2_start = rotate_point(tuple(data.line2_start), center, cos_a, sin_a)
            data.line2_end = rotate_point(tuple(data.line2_end), center, cos_a, sin_a)
        
        self.report({'INFO'}, f"Rotated {math.degrees(line_angle):.1f} degrees to flatten")
        
        if settings.is_plotting:
            update_camera_from_vanishing_points(context)
        
        context.area.tag_redraw()
        return {'FINISHED'}


class FOCALPLOTTER_OT_reset_defaults(Operator):
    """Reset all plotting settings to defaults"""
    bl_idname = "focalplotter.reset_defaults"
    bl_label = "Reset Defaults"
    bl_options = {'REGISTER', 'UNDO'}

    def execute(self, context):
        settings = get_plot_settings(context)
        
        # Reset QoL settings
        settings.freeze_guides = False
        settings.show_horizon = True
        settings.camera_distance = 10.0
        settings.principal_point_x = 0.0
        settings.principal_point_y = 0.0
        settings.target_mode = 'WORLD_CENTER'
        settings.target_location = (0.0, 0.0, 0.0)
        
        # Reset axis defaults
        x_data = get_axis_data(context, 'x')
        x_data.line1_start = (0.15, 0.55)
        x_data.line1_end = (0.55, 0.52)
        x_data.line2_start = (0.15, 0.45)
        x_data.line2_end = (0.55, 0.48)
        
        y_data = get_axis_data(context, 'y')
        y_data.line1_start = (0.85, 0.55)
        y_data.line1_end = (0.45, 0.52)
        y_data.line2_start = (0.85, 0.45)
        y_data.line2_end = (0.45, 0.48)
        
        z_data = get_axis_data(context, 'z')
        z_data.line1_start = (0.45, 0.15)
        z_data.line1_end = (0.48, 0.55)
        z_data.line2_start = (0.55, 0.15)
        z_data.line2_end = (0.52, 0.55)
        
        self.report({'INFO'}, "Settings reset to defaults")
        
        if settings.is_plotting:
            update_camera_from_vanishing_points(context)
        
        context.area.tag_redraw()
        return {'FINISHED'}


class FOCALPLOTTER_OT_set_keyframe(Operator):
    """Set keyframe for camera transform and lens"""
    bl_idname = "focalplotter.set_keyframe"
    bl_label = "Set Keyframe"
    bl_options = {'REGISTER', 'UNDO'}

    def execute(self, context):
        camera = get_active_camera(context)
        if camera is None:
            self.report({'ERROR'}, "No active camera")
            return {'CANCELLED'}
        
        frame = context.scene.frame_current
        
        camera.keyframe_insert(data_path="location", frame=frame)
        camera.keyframe_insert(data_path="rotation_euler", frame=frame)
        camera.data.keyframe_insert(data_path="lens", frame=frame)
        
        self.report({'INFO'}, f"Keyframe set at frame {frame}")
        return {'FINISHED'}


class FOCALPLOTTER_OT_delete_keyframe(Operator):
    """Delete keyframe for camera at current frame"""
    bl_idname = "focalplotter.delete_keyframe"
    bl_label = "Delete Keyframe"
    bl_options = {'REGISTER', 'UNDO'}

    def execute(self, context):
        camera = get_active_camera(context)
        if camera is None:
            self.report({'ERROR'}, "No active camera")
            return {'CANCELLED'}
        
        frame = context.scene.frame_current
        
        try:
            camera.keyframe_delete(data_path="location", frame=frame)
            camera.keyframe_delete(data_path="rotation_euler", frame=frame)
            camera.data.keyframe_delete(data_path="lens", frame=frame)
            self.report({'INFO'}, f"Keyframe deleted at frame {frame}")
        except RuntimeError:
            self.report({'WARNING'}, "No keyframe at current frame")
        
        return {'FINISHED'}


class FOCALPLOTTER_OT_delete_all_keyframes(Operator):
    """Delete all keyframes for camera"""
    bl_idname = "focalplotter.delete_all_keyframes"
    bl_label = "Delete All Keyframes"
    bl_options = {'REGISTER', 'UNDO'}

    def execute(self, context):
        camera = get_active_camera(context)
        if camera is None:
            self.report({'ERROR'}, "No active camera")
            return {'CANCELLED'}
        
        camera.animation_data_clear()
        if camera.data.animation_data:
            camera.data.animation_data_clear()
        
        self.report({'INFO'}, "All camera keyframes deleted")
        return {'FINISHED'}


class FOCALPLOTTER_OT_move_along_view(Operator):
    """Move selected objects along camera rays (perspective-correct, appears like scaling)"""
    bl_idname = "focalplotter.move_along_view"
    bl_label = "Move Along View"
    bl_options = {'REGISTER', 'UNDO', 'BLOCKING'}

    @classmethod
    def poll(cls, context):
        return is_in_camera_view(context) and context.selected_objects
    
    def invoke(self, context, event):
        camera = get_active_camera(context)
        if camera is None:
            self.report({'ERROR'}, "No active camera")
            return {'CANCELLED'}
        
        # Store initial state
        self._last_mouse_x = event.mouse_x
        self._camera = camera
        self._cam_loc = camera.location.copy()
        self._objects = [obj for obj in context.selected_objects if obj != camera]
        self._initial_locations = [obj.location.copy() for obj in self._objects]
        
        # Calculate ray direction for each object (from camera through object)
        self._ray_directions = []
        for obj in self._objects:
            ray = (obj.location - self._cam_loc)
            if ray.length > 0.001:
                self._ray_directions.append(ray.normalized())
            else:
                self._ray_directions.append(Vector((0, 0, -1)))  # Fallback
        
        # Store initial distances from camera - these get updated as we drag
        self._initial_distances = [(obj.location - self._cam_loc).length for obj in self._objects]
        self._current_distances = self._initial_distances.copy()
        
        self._sensitivity = 0.005
        
        if not self._objects:
            self.report({'WARNING'}, "No objects selected (excluding camera)")
            return {'CANCELLED'}
        
        context.window_manager.modal_handler_add(self)
        context.window.cursor_set('SCROLL_X')
        self.report({'INFO'}, "Drag left/right to move, Shift for precision, Enter/LMB to confirm, Esc/RMB to cancel")
        return {'RUNNING_MODAL'}

    def modal(self, context, event):
        if event.type == 'MOUSEMOVE':
            # Calculate incremental movement since last frame
            delta_x = event.mouse_x - self._last_mouse_x
            self._last_mouse_x = event.mouse_x
            
            # Apply sensitivity (reduced when shift is held)
            sensitivity = self._sensitivity * 0.1 if event.shift else self._sensitivity
            
            # Calculate additive distance change based on average current distance
            # This makes movement feel consistent regardless of object distance
            avg_dist = sum(self._current_distances) / len(self._current_distances) if self._current_distances else 1.0
            distance_delta = delta_x * sensitivity * max(avg_dist, 0.1)
            
            # Move each object along its own ray from camera
            for i, (obj, ray_dir) in enumerate(zip(self._objects, self._ray_directions)):
                # Add to current distance (additive, not multiplicative)
                new_dist = self._current_distances[i] + distance_delta
                new_dist = max(0.01, new_dist)  # Prevent negative/zero
                self._current_distances[i] = new_dist
                obj.location = self._cam_loc + ray_dir * new_dist
            
            context.area.tag_redraw()
            return {'RUNNING_MODAL'}
        
        elif event.type in {'LEFTMOUSE', 'RET', 'NUMPAD_ENTER'} and event.value == 'PRESS':
            # Confirm
            context.window.cursor_set('DEFAULT')
            self.report({'INFO'}, f"Moved {len(self._objects)} object(s)")
            return {'FINISHED'}
        
        elif event.type in {'RIGHTMOUSE', 'ESC'} and event.value == 'PRESS':
            # Cancel - restore original positions
            for obj, init_loc in zip(self._objects, self._initial_locations):
                obj.location = init_loc
            context.window.cursor_set('DEFAULT')
            self.report({'INFO'}, "Cancelled")
            return {'CANCELLED'}
        
        return {'RUNNING_MODAL'}


class FOCALPLOTTER_OT_add_projection_object(Operator):
    """Add a cube with UV projection from camera for reference matching"""
    bl_idname = "focalplotter.add_projection_object"
    bl_label = "Add Projection Object"
    bl_options = {'REGISTER', 'UNDO'}

    @classmethod
    def poll(cls, context):
        camera = get_active_camera(context)
        return camera is not None and is_in_camera_view(context)

    def execute(self, context):
        camera = get_active_camera(context)
        if camera is None:
            self.report({'ERROR'}, "No active camera")
            return {'CANCELLED'}
        
        settings = get_plot_settings(context)
        
        # Get background image from camera
        bg_image = None
        if camera.data.background_images:
            for bg in camera.data.background_images:
                if bg.image:
                    bg_image = bg.image
                    break
        
        # Also check settings reference image
        if bg_image is None and settings.reference_image:
            bg_image = settings.reference_image
        
        if bg_image is None:
            self.report({'WARNING'}, "No background image found on camera or in settings")
        
        # 1. Add a cube
        bpy.ops.mesh.primitive_cube_add(size=2, location=(0, 0, 0))
        obj = context.active_object
        obj.name = "FocalPlotter_Projection"
        
        # 2. Move it a few meters in front of camera
        rot_matrix = camera.rotation_euler.to_matrix()
        forward = rot_matrix @ Vector((0, 0, -1))
        distance = settings.camera_distance * 0.5 if settings.camera_distance > 2 else 3.0
        obj.location = camera.location + forward * distance
        
        # 3. Add subdivision surface modifier
        subsurf = obj.modifiers.new(name="Subdivision", type='SUBSURF')
        subsurf.subdivision_type = 'SIMPLE'
        subsurf.levels = 4
        subsurf.render_levels = 6
        
        # 4. Add UV Project modifier
        uv_proj = obj.modifiers.new(name="UVProject", type='UV_PROJECT')
        uv_proj.projector_count = 1
        uv_proj.projectors[0].object = camera
        
        # 5. Set aspect ratio based on camera/render resolution
        render = context.scene.render
        aspect_x = render.resolution_x / render.resolution_y if render.resolution_y > 0 else 1.0
        uv_proj.aspect_x = aspect_x
        uv_proj.aspect_y = 1.0
        
        # 6 & 7. Create material with image texture
        mat = bpy.data.materials.new(name="FocalPlotter_Projection_Mat")
        mat.use_nodes = True
        nodes = mat.node_tree.nodes
        links = mat.node_tree.links
        
        # Clear default nodes
        nodes.clear()
        
        # Create nodes
        output_node = nodes.new(type='ShaderNodeOutputMaterial')
        output_node.location = (300, 0)
        
        diffuse_node = nodes.new(type='ShaderNodeBsdfDiffuse')
        diffuse_node.location = (0, 0)
        
        tex_node = nodes.new(type='ShaderNodeTexImage')
        tex_node.location = (-300, 0)
        tex_node.extension = 'CLIP'  # Don't repeat outside image bounds
        
        uv_node = nodes.new(type='ShaderNodeUVMap')
        uv_node.location = (-500, -150)
        uv_node.uv_map = "UVMap"
        
        # Set the image if available
        if bg_image:
            tex_node.image = bg_image
        
        # Connect nodes
        links.new(uv_node.outputs['UV'], tex_node.inputs['Vector'])
        links.new(tex_node.outputs['Color'], diffuse_node.inputs['Color'])
        links.new(diffuse_node.outputs['BSDF'], output_node.inputs['Surface'])
        
        # Assign material to object
        obj.data.materials.append(mat)
        
        self.report({'INFO'}, f"Created projection object '{obj.name}'")
        return {'FINISHED'}


# ---------------------------------------------------------------------------
# Panel
# ---------------------------------------------------------------------------

class FOCALPLOTTER_PT_plot_panel(Panel):
    """Main focal plotter panel"""
    bl_label = "Focal Plotter"
    bl_idname = "FOCALPLOTTER_PT_plot_panel"
    bl_space_type = 'VIEW_3D'
    bl_region_type = 'UI'
    bl_category = "FocalPlotter"

    def draw(self, context):
        layout = self.layout
        settings = get_plot_settings(context)
        camera = get_active_camera(context)
        in_camera_view = is_in_camera_view(context)
        
        # Show warning if not in camera view
        if not in_camera_view:
            box = layout.box()
            box.alert = True
            box.label(text="Enter Camera View", icon='ERROR')
            box.label(text="Press Numpad 0 or")
            box.label(text="View > Cameras > Active Camera")
            
            if camera:
                layout.separator()
                layout.operator("view3d.view_camera", text=f"View {camera.name}", icon='CAMERA_DATA')
            return
        
        if camera:
            layout.label(text=f"Camera: {camera.name}", icon='CAMERA_DATA')
        
        layout.separator()
        
        # Settings box
        box = layout.box()
        box.label(text="Settings", icon='SETTINGS')
        
        draw_checkbox(box, settings, "use_reference_image", label="Add Reference Image")
        if settings.use_reference_image:
            row = box.row()
            row.template_ID(settings, "reference_image", open="image.open")
            if settings.reference_image is None:
                row = box.row()
                row.alert = True
                row.label(text="Select an image!", icon='ERROR')
            else:
                if settings.reference_image.size[0] > 0:
                    box.label(text=f"Size: {settings.reference_image.size[0]}x{settings.reference_image.size[1]}")
                
                draw_checkbox(box, settings, "set_render_resolution", label="Set Render Resolution")
                if settings.set_render_resolution:
                    warn_row = box.row()
                    warn_row.alert = True
                    warn_row.label(text="Affects entire scene!", icon='ERROR')
                
                draw_checkbox(box, settings, "keep_bg_image", label="Keep BG Image After Plotting")
        
        draw_checkbox(box, settings, "use_rule_of_thirds", label="Rule of Thirds")
        draw_checkbox(box, settings, "auto_solve", label="Auto Solve Camera")
        
        layout.separator()
        
        # Axes selection (fSpy-style dropdowns)
        box = layout.box()
        box.label(text="Vanishing Point Axes", icon='ORIENTATION_GLOBAL')
        
        col = box.column(align=True)
        row = col.row(align=True)
        row.prop(settings, "axis_1", text="VP 1")
        row.prop(settings, "axis_2", text="VP 2")
        
        # Warning if same axis selected
        if settings.axis_1 == settings.axis_2:
            row = box.row()
            row.alert = True
            row.label(text="Select different axes!", icon='ERROR')
        
        # Info about third axis
        third = get_third_axis(settings)
        if third:
            box.label(text=f"Third axis ({third.upper()}) computed automatically", icon='INFO')
        
        layout.separator()
        
        # Plot button
        can_plot = not (settings.use_reference_image and settings.reference_image is None)
        
        row = layout.row()
        row.scale_y = 1.6
        row.enabled = can_plot
        row.operator(
            "focalplotter.plot_camera",
            text="Plot Camera" if not settings.is_plotting else "Stop Plotting",
            icon='PLAY' if not settings.is_plotting else 'PAUSE',
            depress=settings.is_plotting,
        )
        
        # Show plotting controls when active
        if settings.is_plotting:
            # Quick info
            box = layout.box()
            col = box.column(align=True)
            col.label(text="Drag dots to adjust", icon='MOUSE_LMB')
            col.label(text="Drag lines to move both", icon='MOUSE_LMB')
            col.label(text="Shift = precision", icon='EVENT_SHIFT')
            
            if camera:
                col.separator()
                col.label(text=f"Focal: {camera.data.lens:.1f}mm")
            
            layout.separator()
            
            # Guides control
            box = layout.box()
            box.label(text="Guides", icon='ORIENTATION_VIEW')
            col = box.column(align=True)
            col.prop(settings, "freeze_guides", text="Freeze Guides (prevent dragging)")
            
            # Horizon only available when X+Y are selected
            has_x = 'x' in (settings.axis_1, settings.axis_2)
            has_y = 'y' in (settings.axis_1, settings.axis_2)
            has_horizon = has_x and has_y
            
            row = col.row()
            row.enabled = has_horizon
            row.prop(settings, "show_horizon", text="Show Horizon Line")
            
            row = box.row(align=True)
            row.enabled = has_horizon
            row.operator("focalplotter.flatten_horizon", text="Flatten Horizon", icon='REMOVE')
            row.operator("focalplotter.reset_defaults", text="Reset", icon='LOOP_BACK')
            
            if not has_horizon:
                box.label(text="Horizon needs X+Y axes", icon='INFO')
            
            layout.separator()
            
            # Camera settings
            box = layout.box()
            box.label(text="Camera Position", icon='CAMERA_DATA')
            
            col = box.column(align=True)
            col.prop(settings, "camera_distance")
            col.label(text="(distance from target)", icon='INFO')
            
            box.separator()
            box.label(text="Optical Center Offset", icon='DOT')
            col = box.column(align=True)
            row = col.row(align=True)
            row.prop(settings, "principal_point_x", text="X")
            row.prop(settings, "principal_point_y", text="Y")
            col.label(text="(0 = image center)", icon='INFO')
            
            layout.separator()
            
            # Target settings
            box = layout.box()
            box.label(text="Target", icon='OBJECT_ORIGIN')
            box.prop(settings, "target_mode", text="")
            
            if settings.target_mode == 'MANUAL':
                col = box.column(align=True)
                row = col.row(align=True)
                row.prop(settings, "target_location", index=0, text="X")
                row = col.row(align=True)
                row.prop(settings, "target_location", index=1, text="Y")
                row = col.row(align=True)
                row.prop(settings, "target_location", index=2, text="Z")
                col.operator("focalplotter.target_to_cursor", text="From 3D Cursor", icon='PIVOT_CURSOR')
        
        # Always show these in camera view (not just when plotting)
        layout.separator()
        
        # Camera Keyframes
        box = layout.box()
        box.label(text="Camera Keyframes", icon='KEYFRAME')
        row = box.row(align=True)
        row.operator("focalplotter.set_keyframe", text="Set", icon='KEYFRAME_HLT')
        row.operator("focalplotter.delete_keyframe", text="Delete", icon='KEYFRAME')
        box.operator("focalplotter.delete_all_keyframes", text="Delete All", icon='TRASH')
        
        # Object Tools (disabled in Edit mode)
        layout.separator()
        box = layout.box()
        box.label(text="Object Tools", icon='OBJECT_DATA')
        
        in_object_mode = context.mode == 'OBJECT'
        
        col = box.column()
        col.enabled = in_object_mode
        col.operator("focalplotter.move_along_view", text="Move Along Camera View", icon='VIEW_CAMERA')
        col.operator("focalplotter.add_projection_object", text="Add Projection Cube", icon='MESH_CUBE')
        
        # Distance to closest point on selected object
        col.separator()
        col.label(text="Distance to Camera", icon='DRIVER_DISTANCE')
        
        active_obj = context.active_object
        if active_obj is not None and active_obj.type in {'MESH', 'CURVE', 'SURFACE', 'FONT'}:
            distance, closest_point = get_closest_point_distance(context, active_obj, camera)
            if distance is not None:
                dist_str = format_distance_with_units(context, distance)
                sub = col.column(align=True)
                sub.label(text=f"{active_obj.name}:")
                row = sub.row()
                row.label(text=dist_str, icon='BLANK1')
            else:
                col.label(text="Could not calculate", icon='ERROR')
        elif active_obj is not None:
            col.label(text="Select a mesh or curve", icon='INFO')
        else:
            col.label(text="No object selected", icon='INFO')
        
        if not in_object_mode:
            box.label(text="Switch to Object mode", icon='INFO')


# ---------------------------------------------------------------------------
# Registration
# ---------------------------------------------------------------------------

classes = (
    FOCALPLOTTER_PG_axis_lines,
    FOCALPLOTTER_PG_plot_settings,
    FOCALPLOTTER_OT_plot_camera,
    FOCALPLOTTER_OT_interactive,
    FOCALPLOTTER_OT_target_to_cursor,
    FOCALPLOTTER_OT_flatten_horizon,
    FOCALPLOTTER_OT_reset_defaults,
    FOCALPLOTTER_OT_set_keyframe,
    FOCALPLOTTER_OT_delete_keyframe,
    FOCALPLOTTER_OT_delete_all_keyframes,
    FOCALPLOTTER_OT_move_along_view,
    FOCALPLOTTER_OT_add_projection_object,
    FOCALPLOTTER_PT_plot_panel,
)


def register():
    for cls in classes:
        bpy.utils.register_class(cls)
    
    bpy.types.Scene.focalplotter_plot = PointerProperty(type=FOCALPLOTTER_PG_plot_settings)
    bpy.types.Scene.focalplotter_axis_x = PointerProperty(type=FOCALPLOTTER_PG_axis_lines)
    bpy.types.Scene.focalplotter_axis_y = PointerProperty(type=FOCALPLOTTER_PG_axis_lines)
    bpy.types.Scene.focalplotter_axis_z = PointerProperty(type=FOCALPLOTTER_PG_axis_lines)
    
    register_draw_handler()


def unregister():
    unregister_draw_handler()
    
    del bpy.types.Scene.focalplotter_axis_z
    del bpy.types.Scene.focalplotter_axis_y
    del bpy.types.Scene.focalplotter_axis_x
    del bpy.types.Scene.focalplotter_plot
    
    for cls in reversed(classes):
        bpy.utils.unregister_class(cls)