"""
비례(P) 제어기 모듈

차선 중심 오차를 기반으로 조향각을 계산합니다.
"""


class ProportionalController:
    """하이브리드 제어기 (위치 + 각도)"""

    def __init__(self, kp=0.4, k_position=0.3, k_angle=1.5, base_speed=120, steering_min=25, steering_max=155, steering_neutral=90):
        """
        Args:
            kp (float): 비례 상수 (하위 호환성용, k_position과 동일)
            k_position (float): 위치 오차 계수
            k_angle (float): 각도 오차 계수
            base_speed (int): 기본 속도 (0-255)
            steering_min (int): 최소 조향각 (좌회전 한계)
            steering_max (int): 최대 조향각 (우회전 한계)
            steering_neutral (int): 중립 조향각 (직진)
        """
        self.kp = kp  # 하위 호환성
        self.k_position = k_position
        self.k_angle = k_angle
        self.base_speed = base_speed
        self.steering_min = steering_min
        self.steering_max = steering_max
        self.steering_neutral = steering_neutral

    def calculate_steering(self, lane_center_x, frame_center_x, lane_angle=None):
        """
        조향각 계산 (하이브리드: 위치 + 각도)

        Args:
            lane_center_x (int): 차선 중심 x 좌표 (None: 미검출)
            frame_center_x (int): 프레임 중심 x 좌표
            lane_angle (float): 차선 각도 (degrees, None: 각도 미검출)

        Returns:
            int: 조향각 (steering_min ~ steering_max)
        """
        if lane_center_x is None:
            # 차선 미검출 시 중립 유지
            return self.steering_neutral

        # 1. 위치 오차 (Position Error) - 좌우 반전으로 인해 부호 반전
        position_error = frame_center_x - lane_center_x

        # 2. 각도 오차 (Angle Error) - 좌우 반전으로 인해 부호 반전
        angle_error = 0
        if lane_angle is not None:
            angle_error = -lane_angle  # 각도 자체가 오차 (0도 = 직진)

        # 3. 하이브리드 제어: 위치 + 각도
        steering_adjustment = (self.k_position * position_error) + (self.k_angle * angle_error)

        # 4. 조향각 계산 (중립 + 조정값)
        steering_angle = self.steering_neutral + steering_adjustment

        # 5. 제한 범위 적용
        steering_angle = max(self.steering_min, min(self.steering_max, steering_angle))

        return int(steering_angle)

    def calculate_speed(self, steering_angle=None):
        """
        속도 계산 (조향각 기반 선택적 조정)

        Args:
            steering_angle (int): 현재 조향각 (None: 기본 속도 사용)

        Returns:
            int: 속도 (0-255)
        """
        if steering_angle is None:
            return self.base_speed

        # 급격한 회전 시 속도 감소 (선택적)
        steering_deviation = abs(steering_angle - self.steering_neutral)

        if steering_deviation > 30:
            # 큰 조향 → 속도 감소
            speed = int(self.base_speed * 0.8)
        elif steering_deviation > 20:
            speed = int(self.base_speed * 0.9)
        else:
            speed = self.base_speed

        return max(0, min(255, speed))

    def update_kp(self, kp):
        """Kp 값 동적 조정"""
        self.kp = kp
        print(f"Kp 업데이트: {kp}")

    def get_control_info(self, lane_center_x, frame_center_x):
        """
        제어 정보 반환 (디버깅용)

        Returns:
            dict: 제어 정보
        """
        steering = self.calculate_steering(lane_center_x, frame_center_x)
        speed = self.calculate_speed(steering)

        error = 0 if lane_center_x is None else lane_center_x - frame_center_x

        return {
            'error': error,
            'steering': steering,
            'speed': speed,
            'kp': self.kp
        }


# ===== 테스트 코드 =====
if __name__ == "__main__":
    # 제어기 생성
    controller = ProportionalController(kp=0.4, base_speed=120)

    # 테스트 시나리오
    frame_center = 160  # 320x240 프레임 중심

    test_cases = [
        ("차선 중심 (직진)", 160),
        ("차선 약간 오른쪽", 180),
        ("차선 많이 오른쪽", 220),
        ("차선 약간 왼쪽", 140),
        ("차선 많이 왼쪽", 100),
        ("차선 미검출", None),
    ]

    print("=== 비례 제어기 테스트 ===")
    print(f"Kp: {controller.kp}, 기본 속도: {controller.base_speed}")
    print(f"조향 범위: {controller.steering_min}° ~ {controller.steering_max}°")
    print(f"프레임 중심: {frame_center}px\n")

    for description, lane_center in test_cases:
        info = controller.get_control_info(lane_center, frame_center)

        print(f"{description}:")
        print(f"  차선 중심: {lane_center if lane_center else 'None'}")
        print(f"  오차: {info['error']}px")
        print(f"  조향각: {info['steering']}° (90° = 중립)")
        print(f"  속도: {info['speed']}")
        print()