import time
import numpy as np
from typing import List, Tuple, Dict
from .config import Config

class FlightController:
    """
    Verantwortlich für die Berechnung der Flugvektoren basierend auf KI-Ergebnissen.
    Unterstützt Normal-Modus (sequenziell) und Sport-Modus (simultan/LR).
    """
    def __init__(self):
        self.last_sent_rc = [0, 0, 0, 0]
        self.smooth_face = None
        self.search_start = time.time()
        self.status = "INITIALIZING"
        
        # Speicher für verloren gegangene Ziele
        self.last_target_side = 0
        self.lost_time = 0

    def calculate(self, 
                  faces: List[Tuple], 
                  is_manual: bool, 
                  is_sport: bool, # NEU: Sport-Modus Flag
                  emergency_stop: bool, 
                  is_locked: bool, 
                  locked_person: Tuple,
                  current_height: float, 
                  target_altitude: float,
                  tof: int, 
                  zones: Dict[str, bool], 
                  zone_scores: Dict[str, float],
                  manual_rc: Tuple[int, int, int, int]) -> Tuple[Tuple[int, int, int, int], str]:
        
        lr, fb, ud, yv = 0, 0, 0, 0
        
        if len(faces) > 0:
            target = max(faces, key=lambda f: f[2] * f[3])
            if self.smooth_face is None: self.smooth_face = target
            else:
                self.smooth_face = tuple(int(self.smooth_face[i]*0.8 + target[i]*0.2) for i in range(4))
        else:
            self.smooth_face = None

        if emergency_stop:
            self.status = "EMERGENCY STOP"
            return (0, 0, 0, 0), self.status

        # Hindernisvermeidung
        center_blocked = zones["CENTER"] or tof < Config.OBSTACLE_TOF_CM
        if center_blocked:
            self.status = "AVOIDING OBSTACLE"
            yv = 80 if zone_scores["LEFT"] < zone_scores["RIGHT"] else -80
            fb = -30
            return self._smooth(0, fb, 0, yv)

        if is_manual:
            self.status = "MANUAL CONTROL"
            lr, fb, m_ud, yv = manual_rc
            if abs(m_ud) > 0: ud = m_ud
            return self._smooth(lr, fb, ud, yv)

        # AI LOGIC
        if is_locked:
            if locked_person is not None:
                self.search_start = time.time()
                self.lost_time = 0
                (x, y, w, h) = locked_person
                center_x = x + w // 2
                err_x = center_x - (Config.WIDTH // 2)
                
                self.last_target_side = 1 if err_x > 0 else -1
                
                if is_sport:
                    # SPORT MODUS: Alles gleichzeitig + LR-Strafing
                    yv = int(np.clip(Config.SPORT_YAW_GAIN * err_x, -100, 100))
                    fb = int(np.clip(Config.SPORT_FB_GAIN * (Config.TARGET_PERSON_SIZE - w), -100, 100))
                    lr = int(np.clip(Config.SPORT_LR_GAIN * err_x, -60, 60))
                    self.status = "SPORT PURSUIT: FULL AXIS"
                else:
                    # NORMAL MODUS: Sequenziell (Drehen ODER Fliegen)
                    if abs(err_x) > Config.FACE_DEADZONE:
                        yv = int(np.clip(Config.YAW_GAIN * err_x, -50, 50))
                        fb = 0 
                        self.status = "PURSUIT: AIMING"
                    else:
                        yv = 0
                        fb = int(np.clip(Config.FORWARD_GAIN * (Config.TARGET_PERSON_SIZE - w), -80, 80))
                        self.status = "PURSUIT: APPROACHING"
            else:
                # Target verloren
                if self.lost_time == 0: self.lost_time = time.time()
                elapsed = time.time() - self.lost_time
                
                search_speed = 60 if is_sport else 40
                if elapsed < 10.0:
                    yv = search_speed * self.last_target_side
                    self.status = f"LOST TARGET: SCANNING {'RIGHT' if self.last_target_side > 0 else 'LEFT'}"
                else:
                    self.status = "TARGET LOST: PATROL"
                    yv = 30
        
        elif self.smooth_face is not None:
            (x, y, w, h) = self.smooth_face
            err_x = (x + w // 2) - (Config.WIDTH // 2)
            yv = int(np.clip(Config.YAW_GAIN * err_x, -40, 40))
            self.status = "AWAITING LOCK"
        else:
            # Patrouille
            elapsed = (time.time() - self.search_start) % 8.0
            if elapsed < 3.0:
                self.status = "PATROL: ADVANCE"
                fb = 35
            else:
                self.status = "PATROL: SCAN"
                yv = 35

        return self._smooth(lr, fb, ud, yv)

    def _smooth(self, lr, fb, ud, yv):
        alpha = Config.SMOOTHING_ALPHA
        slr = int(self.last_sent_rc[0] * (1-alpha) + lr * alpha)
        sfb = int(self.last_sent_rc[1] * (1-alpha) + fb * alpha)
        sud = int(self.last_sent_rc[2] * (1-alpha) + ud * alpha)
        syv = int(self.last_sent_rc[3] * (1-alpha) + yv * alpha)
        
        if abs(slr) < 3: slr = 0
        if abs(sfb) < 3: sfb = 0
        if abs(sud) < 3: sud = 0
        if abs(syv) < 3: syv = 0
        
        self.last_sent_rc = [slr, sfb, sud, syv]
        return (slr, sfb, sud, syv), self.status