import type { WorldPosition } from "./types";

export function distance(a: WorldPosition, b: WorldPosition) {
  return Math.hypot(a[0] - b[0], a[1] - b[1]);
}

export function moveToward(
  current: WorldPosition,
  target: WorldPosition,
  maximumDistance: number,
): WorldPosition {
  const dx = target[0] - current[0];
  const dz = target[1] - current[1];
  const length = Math.hypot(dx, dz);
  if (length <= maximumDistance || length < 0.0001) return [target[0], target[1]];
  return [current[0] + (dx / length) * maximumDistance, current[1] + (dz / length) * maximumDistance];
}

export function pointToSegmentDistance(
  point: WorldPosition,
  start: WorldPosition,
  end: WorldPosition,
) {
  const dx = end[0] - start[0];
  const dz = end[1] - start[1];
  const lengthSquared = dx * dx + dz * dz;
  if (lengthSquared === 0) return distance(point, start);
  const projection = Math.max(
    0,
    Math.min(1, ((point[0] - start[0]) * dx + (point[1] - start[1]) * dz) / lengthSquared),
  );
  return Math.hypot(
    point[0] - (start[0] + projection * dx),
    point[1] - (start[1] + projection * dz),
  );
}

export function pointOutsideLane(
  point: WorldPosition,
  start: WorldPosition,
  end: WorldPosition,
  clearance: number,
  preferredSide: -1 | 1,
): WorldPosition {
  const dx = end[0] - start[0];
  const dz = end[1] - start[1];
  const lengthSquared = dx * dx + dz * dz;
  if (lengthSquared < 0.0001) return [point[0], point[1]];

  const length = Math.sqrt(lengthSquared);
  const projection = Math.max(
    0,
    Math.min(1, ((point[0] - start[0]) * dx + (point[1] - start[1]) * dz) / lengthSquared),
  );
  const nearestX = start[0] + projection * dx;
  const nearestZ = start[1] + projection * dz;
  const normalX = -dz / length;
  const normalZ = dx / length;
  const signedDistance = (point[0] - nearestX) * normalX + (point[1] - nearestZ) * normalZ;
  const side = Math.abs(signedDistance) > 0.01 ? Math.sign(signedDistance) : preferredSide;

  return [nearestX + normalX * clearance * side, nearestZ + normalZ * clearance * side];
}

export function angleTo(origin: WorldPosition, target: WorldPosition) {
  return Math.atan2(target[0] - origin[0], target[1] - origin[1]);
}

export function angularDistance(first: number, second: number) {
  return Math.abs(Math.atan2(Math.sin(first - second), Math.cos(first - second)));
}

export function pointInCone(
  point: WorldPosition,
  origin: WorldPosition,
  facingAngle: number,
  halfAngle: number,
  range: number,
) {
  return distance(point, origin) <= range && angularDistance(angleTo(origin, point), facingAngle) <= halfAngle;
}

export function pointOutsideCircle(
  point: WorldPosition,
  center: WorldPosition,
  clearance: number,
  fallbackAngle: number,
): WorldPosition {
  const dx = point[0] - center[0];
  const dz = point[1] - center[1];
  const length = Math.hypot(dx, dz);
  if (length < 0.001) {
    return [center[0] + Math.sin(fallbackAngle) * clearance, center[1] + Math.cos(fallbackAngle) * clearance];
  }
  return [center[0] + (dx / length) * clearance, center[1] + (dz / length) * clearance];
}
