import math
import numpy as np

class Geolocation:
    """ Класс геопозиции """
    x: float
    y: float
    z: float # Масштаб (Высота)
    angle: float # Угол направления движения

    def __init__(self, x: float = 0, y: float = 0, z: float = 1, angle: float = 0):
        self.x = x
        self.y = y
        self.z = z
        self.angle = angle

    def __str__(self) -> str:
        return (f"Geolocation(x={self.x:.2f}, y={self.y:.2f}, "
                f"z={self.z:.2f}, angle={self.angle:.1f}°)")

    def __matmul__(self, mat: np.array) -> 'Geolocation':
        return self.copy().apply(mat)

    def __imatmul__(self, mat: np.array) -> 'Geolocation':
        return self.apply(mat)

    def apply(self, mat: np.ndarray) -> 'Geolocation':
        """ На основе матрицы трансформации вычисляет новую позицию и направление """
        
        tx, ty = -mat[0, 2], -mat[1, 2]
        
        # Вычисляем угол поворота
        rotation = -np.arctan2(mat[1, 0], mat[0, 0])
        scale = np.hypot(mat[0, 0], mat[0, 1])
        
        # Координаты уже отцентрированы, поэтому используем их напрямую
        dx_meters = tx
        dy_meters = ty

        angle_global = self.angle + rotation
        
        # Применяем поворот к смещению (учитываем текущий угол БПЛА)
        cos_angle = math.cos(angle_global)
        sin_angle = math.sin(angle_global)
        
        # Поворачиваем смещение в глобальные координаты
        # Обратите внимание: dy_meters инвертирован, так как в изображениях Y направлен вниз
        dx_global = dx_meters * cos_angle - dy_meters * sin_angle
        dy_global = dx_meters * sin_angle + dy_meters * cos_angle

        # Обновляем координаты БПЛА
        self.z /= scale
        self.x = self.x + dx_global * self.z
        self.y = self.y + dy_global * self.z

        # Нормализуем угол в диапазоне [-π, π]
        self.angle = math.atan2(math.sin(angle_global), math.cos(angle_global))

        return self

    def copy(self) -> 'Geolocation':
        return Geolocation(self.x, self.y, self.z, self.angle)

    @staticmethod
    def transform(g: 'Geolocation', mat: np.ndarray) -> 'Geolocation | None':
        return g.copy().apply(mat)