#!/usr/bin/env python3
import cv2
import numpy as np
import paho.mqtt.client as mqtt
from paho.mqtt import client as mqtt_client
import time
import base64
import threading
import json
import subprocess
import os
from datetime import datetime

# ====================== KONFIGURASI ======================
MQTT_BROKER = "localhost"
MQTT_PORT = 1883
PRESENCE_TOPIC = "classroom/presence"
CONTROL_TOPIC = "classroom/ac/control"
WEBCAM_TOPIC = "classroom/webcam"
STATUS_TOPIC = "classroom/ac/status"

WEIGHTS = "yolov4-tiny.weights"
CFG = "yolov4-tiny.cfg"
NAMES = "coco.names"

CONFIDENCE_THRESHOLD = 0.5
INPUT_SIZE = (256, 256)

WEBCAM_INTERVAL = 2
WEBCAM_QUALITY = 60
WEBCAM_WIDTH = 640
WEBCAM_HEIGHT = 480

# ========================================================

print("=" * 60)
print("🚀 AC CONTROL SYSTEM - DENGAN WEBCAM MONITORING")
print("=" * 60)

# ====================== CLEANUP WEBCAM ======================
print("Membersihkan proses yang menggunakan webcam...")
try:
    # Kill semua proses yang menggunakan webcam
    subprocess.run(["sudo", "fuser", "-k", "/dev/video0"], capture_output=True)
    time.sleep(1)
except:
    pass

# ====================== INISIALISASI WEBCAM ======================
print("Membuka webcam...")
cap = None

# Coba buka webcam dengan berbagai metode
for i in range(3):  # Coba 3 kali
    cap = cv2.VideoCapture(0, cv2.CAP_V4L2)
    if cap.isOpened():
        print(f"✅ Webcam terbuka pada percobaan ke-{i+1}")
        break
    time.sleep(1)

if cap is None or not cap.isOpened():
    print("❌ Gagal membuka webcam!")
    print("\nTroubleshooting:")
    print("1. Cek koneksi USB webcam")
    print("2. Jalankan: sudo fuser -k /dev/video0")
    print("3. Restart: sudo reboot")
    exit(1)

# Set properti webcam
cap.set(cv2.CAP_PROP_FRAME_WIDTH, WEBCAM_WIDTH)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, WEBCAM_HEIGHT)
cap.set(cv2.CAP_PROP_FPS, 30)

# Test baca frame
ret, test_frame = cap.read()
if not ret:
    print("❌ Webcam terbuka tapi gagal membaca frame!")
    cap.release()
    exit(1)

print(f"✅ Webcam siap: {int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))}x{int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))}")

# ====================== LOAD MODEL YOLO ======================
print("Memuat model YOLOv4-tiny...")
for file in [WEIGHTS, CFG, NAMES]:
    if not os.path.exists(file):
        print(f"❌ File {file} tidak ditemukan!")
        cap.release()
        exit(1)

net = cv2.dnn.readNet(WEIGHTS, CFG)
net.setPreferableBackend(cv2.dnn.DNN_BACKEND_OPENCV)
net.setPreferableTarget(cv2.dnn.DNN_TARGET_CPU)

with open(NAMES, "r") as f:
    classes = [line.strip() for line in f.readlines()]

layer_names = net.getLayerNames()
output_layers = [layer_names[i - 1] for i in net.getUnconnectedOutLayers()]

print("✅ Model YOLO berhasil dimuat")

# ====================== MQTT SETUP ======================
def on_connect(client, userdata, flags, reason_code, properties):
    if reason_code == 0:
        print("✅ MQTT terhubung ke broker!")
        client.subscribe("classroom/webcam/request")
    else:
        print(f"❌ MQTT gagal connect: {reason_code}")

def on_message(client, userdata, msg):
    if msg.topic == "classroom/webcam/request":
        print("📸 Request frame webcam diterima")
        send_webcam_frame()

client = mqtt.Client(mqtt_client.CallbackAPIVersion.VERSION2)
client.on_connect = on_connect
client.on_message = on_message

try:
    client.connect(MQTT_BROKER, MQTT_PORT, 60)
    client.loop_start()
    print("✅ MQTT client siap")
except Exception as e:
    print(f"❌ Gagal konek MQTT: {e}")
    cap.release()
    exit(1)

# ====================== FUNGSI CAPTURE ======================
def get_frame():
    """Ambil frame dari webcam"""
    if cap is None or not cap.isOpened():
        return False, None
    return cap.read()

def send_webcam_frame():
    """Mengirim frame webcam via MQTT"""
    try:
        ret, frame = get_frame()
        if ret and frame is not None:
            frame = cv2.resize(frame, (WEBCAM_WIDTH, WEBCAM_HEIGHT))
            _, buffer = cv2.imencode('.jpg', frame, [cv2.IMWRITE_JPEG_QUALITY, WEBCAM_QUALITY])
            frame_base64 = base64.b64encode(buffer).decode('utf-8')
            client.publish(WEBCAM_TOPIC, frame_base64)
            print(f"📸 Frame terkirim ({len(frame_base64) // 1024} KB)")
            return True
        return False
    except Exception as e:
        print(f"❌ Error: {e}")
        return False

def send_webcam_periodic():
    while True:
        send_webcam_frame()
        time.sleep(WEBCAM_INTERVAL)

# ====================== DETEKSI KEHADIRAN ======================
def detect_person(frame):
    """Deteksi orang dalam frame"""
    blob = cv2.dnn.blobFromImage(frame, 0.00392, INPUT_SIZE, (0, 0, 0), True, crop=False)
    net.setInput(blob)
    outs = net.forward(output_layers)
    
    for out in outs:
        for detection in out:
            scores = detection[5:]
            class_id = np.argmax(scores)
            confidence = scores[class_id]
            if confidence > CONFIDENCE_THRESHOLD and classes[class_id] == "person":
                return True
    return False

# ====================== MAIN LOOP ======================
def main():
    # Start thread webcam
    webcam_thread = threading.Thread(target=send_webcam_periodic, daemon=True)
    webcam_thread.start()
    print("✅ Thread webcam dimulai")
    
    last_presence = None
    frame_count = 0
    detection_skip = 2
    
    print("\n" + "=" * 60)
    print("🚀 Sistem deteksi kehadiran berjalan...")
    print(f"   - Webcam streaming: setiap {WEBCAM_INTERVAL} detik")
    print(f"   - Deteksi kehadiran: setiap {detection_skip} frame")
    print("   - Tekan Ctrl+C untuk berhenti")
    print("=" * 60 + "\n")
    
    try:
        while True:
            ret, frame = get_frame()
            if not ret or frame is None:
                print("⚠️ Gagal membaca frame, mencoba reset...")
                time.sleep(1)
                continue
            
            frame_count += 1
            
            if frame_count % detection_skip == 0:
                person_detected = detect_person(frame)
                current_presence = "ada" if person_detected else "tidak ada"
                
                client.publish(PRESENCE_TOPIC, current_presence)
                
                if current_presence != last_presence:
                    command = "on" if person_detected else "off"
                    client.publish(CONTROL_TOPIC, command)
                    
                    status_msg = {
                        "ac_state": command,
                        "temperature": 24,
                        "mode": "cool",
                        "presence": current_presence,
                        "timestamp": datetime.now().isoformat()
                    }
                    client.publish(STATUS_TOPIC, json.dumps(status_msg))
                    
                    print(f"👥 {current_presence} → AC {command.upper()}")
                    last_presence = current_presence
                
                status_indicator = "👥 ADA" if person_detected else "🚪 KOSONG"
                print(f"[{datetime.now().strftime('%H:%M:%S')}] {status_indicator} | Frame: {frame_count}")
            
            time.sleep(0.05)
            
    except KeyboardInterrupt:
        print("\n\n🛑 Berhenti...")
    
    finally:
        if cap:
            cap.release()
        client.disconnect()
        print("✅ Selesai")

if __name__ == "__main__":
    main()