import numpy as np
import cv2
import time
from skimage.metrics import structural_similarity as ssim

# 体全体のカスケードファイル
fullbody_detector = cv2.CascadeClassifier("./haarcascade_fullbody.xml")
# サンプル画像
cap = cv2.VideoCapture('merged_camera.mp4')

# Shi-Tomasiのコーナー検出パラメータ
feature_params = dict( maxCorners = 100,
                       qualityLevel = 0.3,
                       minDistance = 7,
                       blockSize = 7 )

# Lucas-Kanade法のパラメータ
lk_params = dict( winSize  = (15,15),
                  maxLevel = 2,
                  criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))

# ランダムに色を100個生成（値0～255の範囲で100行3列のランダムなndarrayを生成）
color = np.random.randint(0, 255, (100, 3))

# 最初のフレームの処理
end_flag, frame = cap.read()
# グレースケール変換
gray_prev = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# 追跡に向いた特徴
feature_prev = cv2.goodFeaturesToTrack(gray_prev, mask = None, **feature_params)
# 元の配列と同じ形にして0を代入
mask = np.zeros_like(frame)
frame_number = 0  # フレーム番号を追跡するための変数を追加します
prev_img = None
threshold = 0.98  # Set your desired threshold

while(end_flag):
    # グレースケールに変換
    gray_next = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    start = time.time()  
    # 全身の人を検出 
    body = fullbody_detector.detectMultiScale(gray_next,scaleFactor=1.1, minNeighbors=3, minSize=(80, 80))
    end = time.time()  

    # オプティカルフロー検出
    feature_next, status, err = cv2.calcOpticalFlowPyrLK(gray_prev, gray_next, feature_prev, None, **lk_params)
    good_prev = feature_prev[status == 1]
    good_next = feature_next[status == 1]

    # オプティカルフローを描画
    for i, (next_point, prev_point) in enumerate(zip(good_next, good_prev)):
        prev_x, prev_y = prev_point.ravel()
        prev_x = int(prev_x)
        prev_y = int(prev_y)
        next_x, next_y = next_point.ravel()
        next_x = int(next_x)
        next_y = int(next_y)
        mask = cv2.line(mask, (next_x, next_y), (prev_x, prev_y), color[i].tolist(), 2)
        frame = cv2.circle(frame, (next_x, next_y), 5, color[i].tolist(), -1)
    img = cv2.add(frame, mask)
    
    # 人検出した数表示のため変数初期化
    human_cnt = 0
    # 人検出した部分を長方形で囲う
    for (x, y, w, h) in body:
        cv2.rectangle(img, (x, y),(x+w, y+h),(255,0,0),2)
        print(f"Detected human at: x={x}, y={y}, w={w}, h={h}")
        human_cnt += 1

    # 人検出した数を表示
    if human_cnt > 0:
        print(f"frame: {frame_number}")
        print(f"Human Count: {human_cnt}")

        # フレーム間の差異が小さい場合、画像を描き出さない
        if prev_img is not None:
            grayA = cv2.cvtColor(prev_img, cv2.COLOR_BGR2GRAY)
            grayB = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
            score = ssim(grayA, grayB)
            if score > threshold:
                print(f"Skipping this frame due to low difference with previous frame (SSIM: {score})")
            else:
                # 人間が検出された場合、画像を保存します
                cv2.imwrite(f'output/frame_{frame_number}.png', img)
                prev_img = img.copy()  # 保存した画像を比較用として保持
        else:
            # 人間が検出された場合、画像を保存します
            cv2.imwrite(f'output/frame_{frame_number}.png', img)
            prev_img = img.copy()  # 保存した画像を比較用として保持

    # ESCキー
    k = cv2.waitKey(1)
    if k == 27:
        break

    # 次のフレーム、ポイントの準備
    gray_prev = gray_next.copy()
    feature_prev = good_next.reshape(-1, 1, 2)
    end_flag, frame = cap.read()
    frame_number += 1  # フレーム番号をインクリメントします

cap.release()