"""
Algorytm zabezpieczenia odległościowego
Implementacja charakterystyki wielokątnej (quadrilateral)
"""
import numpy as np
import math


class DistanceRelay:
    """
    Algorytm zabezpieczenia odległościowego
    Implementacja charakterystyki wielokątnej (quadrilateral)
    """
    def __init__(self, Z_line_R=2.0, Z_line_X=8.0, line_angle=75.0):
        # Impedancja linii (obliczona z danych)
        self.Z_line_R = Z_line_R
        self.Z_line_X = Z_line_X
        self.Z_line_mag = np.sqrt(Z_line_R**2 + Z_line_X**2)
        self.line_angle = line_angle

        # === Nastawy stref jako % impedancji linii ===
        # Strefa 1 - 80% linii (natychmiastowa)
        self.Z1_R = self.Z_line_R * 0.8
        self.Z1_X = self.Z_line_X * 0.8
        self.tZ1 = 0       # Brak opóźnienia

        # Strefa 2 - 120% linii (koordynacja)
        self.Z2_R = self.Z_line_R * 1.2
        self.Z2_X = self.Z_line_X * 1.2
        self.tZ2 = 300     # 300ms

        # Strefa 3 - 200% linii (rezerwowa)
        self.Z3_R = self.Z_line_R * 2.0
        self.Z3_X = self.Z_line_X * 2.0
        self.tZ3 = 600     # 600ms

        # Kąt charakterystyki (na podstawie kąta linii)
        self.angle_r1 = line_angle

        # Minimalny prąd i napięcie (zabezpieczenie przed szumem)
        self.I_min = 0.5    # A
        self.U_min = 1.0    # V

        print(f"Nastawy stref (na podstawie Z linii = {self.Z_line_mag:.2f} Ohm, {line_angle:.1f} deg):")
        print(f"  Strefa 1: R={self.Z1_R:.2f} Ohm, X={self.Z1_X:.2f} Ohm (natychmiast)")
        print(f"  Strefa 2: R={self.Z2_R:.2f} Ohm, X={self.Z2_X:.2f} Ohm (300ms)")
        print(f"  Strefa 3: R={self.Z3_R:.2f} Ohm, X={self.Z3_X:.2f} Ohm (600ms)")

        # Stany wewnętrzne dla każdej fazy
        self.init_state()

    def init_state(self):
        """Inicjalizacja stanów dla każdej fazy"""
        # Timery dla każdej fazy i strefy
        self.timers = {
            'L1_Z1': 0, 'L1_Z2': 0, 'L1_Z3': 0,
            'L2_Z1': 0, 'L2_Z2': 0, 'L2_Z3': 0,
            'L3_Z1': 0, 'L3_Z2': 0, 'L3_Z3': 0,
        }
        # Flagi trip
        self.tripped = {'L1': False, 'L2': False, 'L3': False}

    def init_relay(self):
        print("Zabezpieczenie odległościowe zainicjalizowane")
        self.init_state()

    def in_polygon(self, R, X, reach_R, reach_X, angle_deg):
        """
        Sprawdza czy punkt (R, X) jest wewnątrz wielokąta
        Charakterystyka czworokątna (quadrilateral)
        """
        # Obrót punktu o -angle_deg aby wyprostować charakterystykę
        angle_rad = math.radians(angle_deg)
        cos_a = math.cos(-angle_rad)
        sin_a = math.sin(-angle_rad)

        R_rot = R * cos_a - X * sin_a
        X_rot = R * sin_a + X * cos_a

        # Sprawdź czy punkt jest wewnątrz prostokąta w układzie obróconym
        # R musi być dodatnie (kierunek forward)
        # X musi być w zakresie [-reach_X, reach_X]
        # R musi być mniejsze niż reach_R

        # Dodatkowo: nachylone linie R
        R_max = reach_R
        X_max = reach_X
        R_min = 0.1  # Minimalna wartość R (strefa aktywna)

        # Sprawdzenie podstawowych granic
        if R_rot < R_min or R_rot > R_max:
            return False
        if abs(X_rot) > X_max:
            return False

        # Sprawdzenie linii nachylonych (opcjonalnie)
        # Górna granica X
        X_upper = X_max * (1 - (R_rot / R_max) * math.tan(math.radians(90 - angle_deg + 10)))
        # Dolna granica X
        X_lower = -X_max * (1 - (R_rot / R_max) * math.tan(math.radians(90 - angle_deg + 10)))

        return True

    def check_direction(self, U1_zg_re, U1_zg_im, I1_zg_re, I1_zg_im):
        """
        Określenie kierunku na podstawie mocy
        P = Re(U * conj(I)) > 0 = forward
        """
        power = U1_zg_re * I1_zg_re + U1_zg_im * I1_zg_im
        return power > 0  # True = forward

    def step_relay(self, phase, u_re, u_im, i_re, i_im,
                   u0_re, u0_im, i0_re, i0_im,
                   u_zg_re, u_zg_im, i_zg_re, i_zg_im):
        """
        Krok algorytmu dla jednej fazy
        phase: 'L1', 'L2' lub 'L3'
        """
        # Oblicz moduł prądu
        i_mag = math.sqrt(i_re**2 + i_im**2)
        u_mag = math.sqrt(u_re**2 + u_im**2)

        # Sprawdź progi minimalne
        if i_mag < self.I_min or u_mag < self.U_min:
            return 0

        # Oblicz impedancję Z = U / I
        i_mag_sq = i_re**2 + i_im**2
        if i_mag_sq < 1e-9:
            return 0

        z_re = (u_re * i_re + u_im * i_im) / i_mag_sq
        z_x = (u_im * i_re - u_re * i_im) / i_mag_sq

        # Sprawdź kierunek (używamy składowej zgodnej)
        if i_zg_re is not None and i_zg_im is not None:
            forward = True  # Uproszczone - zakładamy forward
        else:
            forward = True

        # Jeśli już wyłączone, nie sprawdzaj dalej
        if self.tripped[phase]:
            return 1

        trip = 0

        if forward:
            # === Strefa 1 - natychmiastowa ===
            if self.in_polygon(z_re, z_x, self.Z1_R, self.Z1_X, self.angle_r1):
                self.tripped[phase] = True
                return 1

            # === Strefa 2 - opóźniona ===
            key_Z2 = f'{phase}_Z2'
            if self.in_polygon(z_re, z_x, self.Z2_R, self.Z2_X, self.angle_r1):
                if self.timers[key_Z2] < self.tZ2:
                    self.timers[key_Z2] += 1
                elif not self.tripped[phase]:
                    self.tripped[phase] = True
                    return 1
            else:
                self.timers[key_Z2] = 0

            # === Strefa 3 - rezerwowa ===
            key_Z3 = f'{phase}_Z3'
            if self.in_polygon(z_re, z_x, self.Z3_R, self.Z3_X, self.angle_r1):
                if self.timers[key_Z3] < self.tZ3:
                    self.timers[key_Z3] += 1
                elif not self.tripped[phase]:
                    self.tripped[phase] = True
                    return 1
            else:
                self.timers[key_Z3] = 0

        return 0

    def reset(self):
        """Reset stanów dla nowego uruchomienia"""
        self.init_state()