#!/usr/bin/env python3

import math
from utils.helpers import distance_2d

def point_in_polygon(px, py, polygon):
    n = len(polygon)
    inside = False
    j = n - 1
    for i in range(n):
        xi, yi = polygon[i]
        xj, yj = polygon[j]
        if ((yi > py) != (yj > py)) and (px < (xj - xi) * (py - yi) / (yj - yi) + xi):
            inside = not inside
        j = i
    return inside

def nearest_point_on_polygon(px, py, polygon):
    min_dist = float('inf')
    nearest = (px, py)
    n = len(polygon)
    for i in range(n):
        x1, y1 = polygon[i]
        x2, y2 = polygon[(i + 1) % n]
        dx, dy = x2 - x1, y2 - y1
        length_sq = dx * dx + dy * dy
        if length_sq == 0:
            proj = (x1, y1)
        else:
            t = max(0, min(1, ((px - x1) * dx + (py - y1) * dy) / length_sq))
            proj = (x1 + t * dx, y1 + t * dy)
        dist = math.sqrt((px - proj[0])**2 + (py - proj[1])**2)
        if dist < min_dist:
            min_dist = dist
            nearest = proj
    return nearest

def distance_to_polygon_edge(px, py, polygon):
    min_dist = float('inf')
    n = len(polygon)
    for i in range(n):
        x1, y1 = polygon[i]
        x2, y2 = polygon[(i + 1) % n]
        dx, dy = x2 - x1, y2 - y1
        length_sq = dx * dx + dy * dy
        if length_sq == 0:
            dist = math.sqrt((px - x1) ** 2 + (py - y1) ** 2)
        else:
            t = max(0, min(1, ((px - x1) * dx + (py - y1) * dy) / length_sq))
            proj_x = x1 + t * dx
            proj_y = y1 + t * dy
            dist = math.sqrt((px - proj_x) ** 2 + (py - proj_y) ** 2)
        min_dist = min(min_dist, dist)
    return min_dist

def clip_to_geofence(waypoints, polygon, warn_dist=3.0, stop_on_leave=False):
    safe_polygon = []
    if warn_dist > 0:
        cx = sum(p[0] for p in polygon) / len(polygon)
        cy = sum(p[1] for p in polygon) / len(polygon)
        for x, y in polygon:
            dx = x - cx
            dy = y - cy
            length = math.sqrt(dx*dx + dy*dy)
            if length > 0:
                shrink = warn_dist / length
                safe_polygon.append((x - dx * shrink, y - dy * shrink))
            else:
                safe_polygon.append((x, y))
    else:
        safe_polygon = polygon

    clipped = []
    consecutive_outside = 0
    for wx, wy in waypoints:
        if point_in_polygon(wx, wy, safe_polygon):
            clipped.append((wx, wy))
            consecutive_outside = 0
        else:
            consecutive_outside += 1
            if stop_on_leave and consecutive_outside >= 2:
                break
            nearest = nearest_point_on_polygon(wx, wy, safe_polygon)
            if not clipped or distance_2d(clipped[-1], nearest) > 0.5:
                clipped.append(nearest)
    return clipped