from math import atan2, cos, hypot, pi, sin, tan  # + atan2

import numpy as np
from scipy.optimize import least_squares

from .calc_utils import *
from .topaze_utils import TopazeUtils


class TopazeCalculator:
    @staticmethod
    def wrap_gr_0_400(g: float) -> float:
        return g % 400.0

    @staticmethod
    def wrap_gr_m200_200(g: float) -> float:
        # Wrap grads into (-200, 200]
        return (g + 200.0) % 400.0 - 200.0

    @staticmethod
    def bearing_radian(x1, y1, x2, y2):
        """
        Azimuth in radians, 0 = North, increasing clockwise.
        """
        return atan2(x2 - x1, y2 - y1)

    @staticmethod
    def bearing_gon(x1, y1, x2, y2):
        """
        Azimuth in grads (0..400), 0g = North, increasing clockwise.
        """
        bea = TopazeCalculator.bearing_radian(x1, y1, x2, y2) * 200.0 / pi
        return TopazeCalculator.wrap_gr_0_400(bea)

    @staticmethod
    def gon_0_400(alpha):
        return alpha % 400.0

    @classmethod
    def obs_to_xyz(cls, station, obs):
        x = y = z = None
        if not obs.x:
            if obs.di and obs.av:
                dist = obs.di * sin(obs.av / 200.0 * pi)
                dz = obs.di * cos(obs.av / 200.0 * pi)
            elif obs.dh and obs.dz:
                dist = obs.dh
                dz = obs.dz
            dx = dist * sin((obs.ah + station.v0) / 200.0 * pi)
            dy = dist * cos((obs.ah + station.v0) / 200.0 * pi)
            x = station.x + dx
            y = station.y + dy
            z = station.z + dz
        elif obs.x and obs.y:
            x = obs.x
            y = obs.y
            if obs.z:
                z = obs.z

        return x, y, z

    @staticmethod
    def compute_bearing_pq(station, references):
        """
        Legacy helper (kept for compatibility). Returns mean bearing difference.
        """
        bpq_array = []
        v0 = -10
        idx = 0
        for ref in references:
            bpq = 0
            try:
                if ref.av and ref.av > 200:
                    ah = TopazeCalculator.gon_0_400(ref.ah - 200)
                else:
                    ah = ref.ah
                bpq = (
                    TopazeCalculator.bearing_gon(station.x, station.y, ref.x, ref.y)
                    - ah
                )
                bpq = TopazeCalculator.gon_0_400(bpq)
            except Exception:
                pass

            if v0 >= 0.0:
                if bpq > v0 and (bpq - v0) > 400:
                    v0 += 400
                elif bpq < v0 and (v0 - bpq) > 400:
                    bpq += 400

            v0 = (v0 * idx + bpq) / (idx + 1)
            idx += 1
            bpq_array.append(bpq)

        bearing_pq = sum(bpq_array) / max(len(bpq_array), 1)
        return bearing_pq

    @staticmethod
    def all_v0_from_measures(measures):
        """
        ORIGINAL function kept for backward-compat.
        Computes V0 per station as distance-weighted mean of bearing - AH. (Grads)
        """
        stations = []
        for m in measures:
            if m.x and m.y and m.type == "H":
                stations.append(m.id)
        stations = list(set(stations))
        v0_stations = []
        for st in stations:
            visees = []
            all_v0 = []
            all_dis = []
            v0_stations.append(
                {
                    "st": st,
                    "v0": None,
                    "visees": visees,
                    "all_v0": all_v0,
                    "all_dis": all_dis,
                }
            )
            for m in measures:
                if (
                    m.id == st
                    and m.type == "H"
                    and m.x
                    and m.y
                    and m.x_target
                    and m.y_target
                ):
                    visees.append(m)

        for v0_st in v0_stations:
            for visee in v0_st["visees"]:
                g = TopazeCalculator.bearing_gon(
                    visee.x, visee.y, visee.x_target, visee.y_target
                )
                v0 = TopazeCalculator.wrap_gr_m200_200(
                    g - (visee.value if visee.value is not None else 0.0)
                )
                v0_st["all_v0"].append(v0)
                d = float(np.hypot(visee.x - visee.x_target, visee.y - visee.y_target))
                v0_st["all_dis"].append(d if d > 0 else 1.0)
            if v0_st["all_v0"]:
                v0_moyen = sum(
                    v * d for v, d in zip(v0_st["all_v0"], v0_st["all_dis"])
                ) / sum(v0_st["all_dis"])
                v0_st["v0"] = TopazeCalculator.wrap_gr_0_400(v0_moyen)
            else:
                v0_st["v0"] = None
        return v0_stations
