import cv2
import numpy as np
import mediapipe as mp
from mediapipe.tasks import python
from mediapipe.tasks.python import vision
import time
import random
import winsound  # Windows用の音源（Mac/Linuxの場合は別ライブラリが必要）
import csv
from datetime import datetime

# --- 設定 ---
MODEL_PATH = 'face_landmarker.task'
MIN_SOUND_INTERVAL = 5.0  # 音を鳴らす最小間隔（秒）
MAX_SOUND_INTERVAL = 10.0  # 音を鳴らす最大間隔（秒）
HISTORY_LENGTH = 100  # 画面に表示するグラフのデータ点数

# --- グローバル変数 ---
last_sound_time = time.time()
next_interval = random.uniform(MIN_SOUND_INTERVAL, MAX_SOUND_INTERVAL)
sound_triggered_flag = False

# データ保存用バッファ [(記録日時, 経過時間[s], Yaw値, 音フラグ)]
data_log = []
yaw_history = []
start_time = time.time()

# --- MediaPipe セットアップ ---
BaseOptions = python.BaseOptions
FaceLandmarker = vision.FaceLandmarker
FaceLandmarkerOptions = vision.FaceLandmarkerOptions
VisionRunningMode = vision.RunningMode

options = FaceLandmarkerOptions(
    base_options=BaseOptions(model_asset_path=MODEL_PATH),
    running_mode=VisionRunningMode.VIDEO,
    output_facial_transformation_matrixes=True,
    num_faces=1
)

cap = cv2.VideoCapture(0)

print("プログラムを開始します。'q' キーで終了し、CSVへ保存します。")

with FaceLandmarker.create_from_options(options) as landmarker:
    while cap.isOpened():
        success, frame = cap.read()
        if not success:
            break

        current_time = time.time()
        elapsed_time = current_time - start_time

        # 1. ランダムなタイミングで音を鳴らす制御
        is_sound_now = 0
        if current_time - last_sound_time > next_interval:
            # 2000Hzの音を200ミリ秒鳴らす（スレッドをブロックしないよう、短い音に設定）
            winsound.Beep(2000, 200)

            print(f"[{datetime.now().strftime('%H:%M:%S')}] ♪ 音を鳴らしました")
            last_sound_time = current_time
            next_interval = random.uniform(MIN_SOUND_INTERVAL, MAX_SOUND_INTERVAL)
            is_sound_now = 1  # CSV記録用のフラグ

        # 2. 顔検出とYaw値の計算
        rgb_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
        frame_timestamp_ms = int(cap.get(cv2.CAP_PROP_POS_MSEC))
        if frame_timestamp_ms == 0:
            frame_timestamp_ms = int(elapsed_time * 1000)

        mp_image = mp.Image(image_format=mp.ImageFormat.SRGB, data=rgb_frame)
        result = landmarker.detect_for_video(mp_image, frame_timestamp_ms)

        yaw = None
        if result.facial_transformation_matrixes:
            for matrix in result.facial_transformation_matrixes:
                rot_mat = matrix[:3, :3]
                angles, _, _, _, _, _ = cv2.RQDecomp3x3(rot_mat)
                yaw = angles[1]  # 左右の向き (Yaw)

        # 3. データの記録 (顔が検出できない場合は None または 0 として記録)
        current_yaw = yaw if yaw is not None else 0.0

        # メモリに時系列データを蓄積
        data_log.append([
            datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f')[:-3],
            round(elapsed_time, 3),
            round(current_yaw, 2),
            is_sound_now
        ])

        # リアルタイムグラフ表示用の履歴更新
        yaw_history.append(current_yaw)
        if len(yaw_history) > HISTORY_LENGTH:
            yaw_history.pop(0)

        # 4. 視覚化（カメラ映像へのテキスト描画）
        if yaw is not None:
            cv2.putText(frame, f"Yaw: {current_yaw:.1f}", (20, 40),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
        else:
            cv2.putText(frame, "Face Lost", (20, 40),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)

        if is_sound_now:
            cv2.putText(frame, "SOUND DETECTED!", (20, 80),
                        cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255), 3)

        # 5. 簡易リアルタイムグラフ（波形）の描画
        # 画面下部に黒いグラフエリアを作成
        graph_h, graph_w = 150, frame.shape[1]
        graph = np.zeros((graph_h, graph_w, 3), dtype=np.uint8)
        cv2.line(graph, (0, graph_h // 2), (graph_w, graph_h // 2), (100, 100, 100), 1)  # 中心線(0度)

        if len(yaw_history) > 1:
            point_interval = graph_w / HISTORY_LENGTH
            for i in range(1, len(yaw_history)):
                # 角度をグラフのY座標にマッピング（上下に最大±45度を想定）
                y1 = int(graph_h / 2 - (yaw_history[i - 1] * (graph_h / 90)))
                y2 = int(graph_h / 2 - (yaw_history[i] * (graph_h / 90)))

                x1 = int((i - 1) * point_interval)
                x2 = int(i * point_interval)

                # 範囲外のクリッピング
                y1 = max(0, min(graph_h - 1, y1))
                y2 = max(0, min(graph_h - 1, y2))

                cv2.line(graph, (x1, y1), (x2, y2), (0, 255, 255), 2)

        # カメラ映像とグラフを縦に連結して表示
        combined_view = np.vstack((frame, graph))
        cv2.imshow('Face Orientation & Timeline', combined_view)

        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

cap.release()
cv2.destroyAllWindows()

# --- 6. CSVファイルへのデータ書き出し ---
filename = f"turning_detection_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv"
with open(filename, 'w', newline='', encoding='utf-8') as f:
    writer = csv.writer(f)
    # ヘッダー行
    writer.writerow(["Timestamp", "Elapsed_Seconds", "Yaw_Angle", "Sound_Triggered"])
    writer.writerows(data_log)

print(f"\nデータが正常に保存されました: {filename}")