# height_worker.py - EdgeTPU 가속 객체 감지

# 1️⃣ EdgeTPU 모델 로드 (하드웨어 가속)
face_interpreter = tflite.Interpreter(
    model_path=FACE_MODEL,
    experimental_delegates=[tflite.load_delegate('libedgetpu.so.1')]
)
person_interpreter = tflite.Interpreter(
    model_path=PERSON_MODEL,
    experimental_delegates=[tflite.load_delegate('libedgetpu.so.1')]
)

# 2️⃣ 2단계 폴백 전략
def track_height():
    # 우선순위 1: 얼굴 감지 (정밀 제어)
    faces = detect_faces_ssd(face_interpreter, frame, MIN_DET_CONF)

    if faces:
        # 얼굴 중심을 화면 중앙으로
        y_center = (ymin + ymax) * 0.5
        ema_y = EMA_ALPHA * y_center + (1-EMA_ALPHA) * ema_y
        diff = ema_y - target_y

        if abs(diff) <= deadband:
            state = "center"  # ✅ 안정화
        elif diff < 0:
            moveUp(WITH_FACE)    # 빠르게 조절
        else:
            moveDown(WITH_FACE)

    else:
        # 우선순위 2: 사람 전체 감지 (대략적 위치)
        person = detect_person_ssd(person_interpreter, frame)

        if person:
            if person[1] <= 0.05:      # 화면 상단
                moveUp(WITHOUT_FACE)   # 천천히 조절
            elif person[1] >= 0.95:    # 화면 하단
                moveDown(WITHOUT_FACE)

# 3️⃣ EMA 필터로 노이즈 제거
ema_y = 0.3 * new_value + 0.7 * ema_y  # 부드러운 움직임

# linear_actuator_controller.py - 액추에이터 안전 제어

# 1️⃣ 한계 검증 (파일 기반 상태 관리)
def exceed_max_height() -> bool:
    """상한 초과 여부 판단"""
    global CUR_HEIGHT_STEP
    v = _read_height_from_file()  # current_height.txt 읽기

    if v is None:  # 파일 오류/범위 외
        print("[ACTUATOR] 🚫 높이 파일 오류. 이동 차단")
        return True

    CUR_HEIGHT_STEP = v  # 전역 상태와 동기화
    return CUR_HEIGHT_STEP >= HEIGHT_MAX  # 28000 step

def exceed_min_height() -> bool:
    """하한 초과(바닥) 여부 판단"""
    global CUR_HEIGHT_STEP
    v = _read_height_from_file()

    if v is None:
        print("[ACTUATOR] 🚫 높이 파일 오류. 이동 차단")
        return True

    CUR_HEIGHT_STEP = v
    return CUR_HEIGHT_STEP <= HEIGHT_MIN  # 0 step

# 2️⃣ 위로 이동 (확장) - 다중 검증
def moveUp(steps: int = DEFAULT_STEP):
    """액추에이터 위로 이동 (얼굴이 화면 아래에 있을 때)"""
    global CUR_HEIGHT_STEP

    # 첫 번째 검증: 이동 시작 전
    if exceed_max_height():
        print("[ACTUATOR] 🚫 최대 높이 도달, 이동 중단")
        return

    GPIO.output(DIR, GPIO.LOW)   # 위쪽 방향
    GPIO.output(ENA, GPIO.HIGH)  # 모터 활성화
    print(f"[ACTUATOR] ↑ Move UP {steps} steps (Current: {CUR_HEIGHT_STEP}/{HEIGHT_MAX})")

    # 매 스텝마다 펄스 + 재검증
    for _ in range(steps):
        # 두 번째 검증: 각 스텝마다
        if exceed_max_height():
            print("[ACTUATOR] 🚫 최대 높이 도달, 이동 중단")
            break

        # 펄스 신호 (스텝 모터 구동)
        GPIO.output(PUL, GPIO.HIGH)
        sleep(STEP_DELAY)  # 0.0004초
        GPIO.output(PUL, GPIO.LOW)
        sleep(STEP_DELAY)

        # 실시간 높이 추적 및 저장
        CUR_HEIGHT_STEP += 1
        _write_height_to_file(CUR_HEIGHT_STEP)

    print(f"[ACTUATOR] ↑ Current step: {CUR_HEIGHT_STEP}/{HEIGHT_MAX}")
    GPIO.output(ENA, GPIO.LOW)  # 모터 비활성화

# 3️⃣ 아래로 이동 (수축) - 다중 검증
def moveDown(steps: int = DEFAULT_STEP):
    """액추에이터 아래로 이동 (얼굴이 화면 위에 있을 때)"""
    global CUR_HEIGHT_STEP

    # 첫 번째 검증: 이동 시작 전
    if exceed_min_height():
        print("[ACTUATOR] 🚫 최소 높이 도달, 이동 중단")
        return

    GPIO.output(DIR, GPIO.HIGH)  # 아래쪽 방향
    GPIO.output(ENA, GPIO.HIGH)
    print(f"[ACTUATOR] ↓ Move DOWN {steps} steps (Current: {CUR_HEIGHT_STEP}/{HEIGHT_MAX})")

    for _ in range(steps):
        # 두 번째 검증: 각 스텝마다
        if exceed_min_height():
            print("[ACTUATOR] 🚫 최소 높이 도달, 이동 중단")
            break

        GPIO.output(PUL, GPIO.HIGH)
        sleep(STEP_DELAY)
        GPIO.output(PUL, GPIO.LOW)
        sleep(STEP_DELAY)

        CUR_HEIGHT_STEP -= 1
        CUR_HEIGHT_STEP = max(HEIGHT_MIN, CUR_HEIGHT_STEP)
        _write_height_to_file(CUR_HEIGHT_STEP)

    print(f"[ACTUATOR] ↓ Current step: {CUR_HEIGHT_STEP}/{HEIGHT_MAX}")
    GPIO.output(ENA, GPIO.LOW)

# 4️⃣ 프로그램 종료 시 자동 원점 복귀
def return_to_start():
    """현재 위치만큼 하강하여 기계 원점(0) 복귀"""
    global CUR_HEIGHT_STEP

    if CUR_HEIGHT_STEP <= 0:
        print("[ACTUATOR] Already at home position (0 step)")
        return

    print(f"[ACTUATOR] 🏁 Returning to home: {CUR_HEIGHT_STEP} steps down...")
    GPIO.output(ENA, GPIO.HIGH)
    GPIO.output(DIR, GPIO.HIGH)  # 하강 방향

    for i in range(CUR_HEIGHT_STEP):
        GPIO.output(PUL, GPIO.HIGH)
        sleep(STEP_DELAY)
        GPIO.output(PUL, GPIO.LOW)
        sleep(STEP_DELAY)
        _write_height_to_file(CUR_HEIGHT_STEP - i - 1)

        if i % 100 == 0 and i > 0:
            print(f"   ↓ 진행: {i}/{CUR_HEIGHT_STEP}")

    CUR_HEIGHT_STEP = 0
    _write_height_to_file(CUR_HEIGHT_STEP)
    GPIO.output(ENA, GPIO.LOW)
    print("[ACTUATOR] ✅ Returned to home position (step=0)")

def on_shutdown():
    """비정상 종료 시에도 원점 복귀 보장"""
    print("[SYSTEM] 🔻 Returning actuator to 0...")
    return_to_start()
    GPIO.cleanup()
    print("[SYSTEM] ✅ Actuator returned to home position.")

atexit.register(on_shutdown)  # 종료 훅 등록

# eye_tracking_worker.py - 3D 기하학 기반 시선 추적

# 1️⃣ PCA로 얼굴 3D 좌표계 생성
def compute_coordinate_box(face_landmarks, indices, w, h):
    # 23개 코 랜드마크로 좌표계 구성
    points_3d = np.array([
        [face_landmarks[i].x * w,
         face_landmarks[i].y * h,
         face_landmarks[i].z * w]
        for i in nose_indices  # 23개 점
    ])

    # 주성분 분석으로 얼굴 회전 계산
    center = np.mean(points_3d, axis=0)
    cov = np.cov((points_3d - center).T)
    eigvals, eigvecs = np.linalg.eigh(cov)

    # 회전 행렬 생성 (얼굴의 방향)
    R_final = Rscipy.from_matrix(eigvecs).as_matrix()
    return center, R_final, points_3d

# 2️⃣ 양안 시선 벡터 융합
def track_gaze():
    # 좌우 눈의 시선 방향 계산
    left_gaze_dir = iris_3d_left - sphere_world_l
    right_gaze_dir = iris_3d_right - sphere_world_r

    # 두 눈의 평균으로 최종 시선 결정
    combined_direction = (left_gaze_dir + right_gaze_dir) / 2
    combined_direction /= np.linalg.norm(combined_direction)

    # 시선 방향 → 화면 좌표 변환
    screen_x, screen_y = convert_gaze_to_screen_coordinates(
        combined_direction,
        calibration_offset_yaw,   # 사용자별 보정값
        calibration_offset_pitch
    )

# 3️⃣ 사용자별 캘리브레이션
def calibrate():
    # 현재 눈 위치를 기준점으로 저장
    left_sphere_local_offset = R_final.T @ (iris - head_center)
    left_sphere_local_offset += base_radius * camera_dir_local

    # 화면 중앙을 보고 있다고 가정하고 오프셋 계산
    calibration_offset_yaw = -raw_yaw
    calibration_offset_pitch = -raw_pitch