# Sign Language Robot - Main Application
import cv2
import mediapipe as mp
import numpy as np
import tensorflow as tf
from adafruit_servokit import ServoKit
import threading
import time
import json

class SignLanguageRobot:
    def __init__(self):
        print("🤖 Initializing Sign Language Robot...")
        
        # Initialize MediaPipe
        self.mp_hands = mp.solutions.hands
        self.hands = self.mp_hands.Hands(
            static_image_mode=False,
            max_num_hands=2,
            min_detection_confidence=0.7,
            min_tracking_confidence=0.5
        )
        self.mp_draw = mp.solutions.drawing_utils
        
        # Initialize cameras
        self.cap1 = cv2.VideoCapture(0)  # Left camera
        self.cap2 = cv2.VideoCapture(1)  # Right camera
        
        # Initialize servo controllers
        self.left_kit = ServoKit(channels=16, address=0x40)
        self.right_kit = ServoKit(channels=16, address=0x41)
        
        # Load AI model
        self.model = tf.keras.models.load_model('sign_language_model.h5')
        
        # Sign vocabulary
        self.signs = self.load_sign_vocabulary()
        
        # Hand tracking data
        self.left_hand_data = None
        self.right_hand_data = None
        self.current_sign = "NONE"
        
        print("✅ Robot initialized successfully!")
    
    def load_sign_vocabulary(self):
        """โหลดคำศัพท์ภาษามือ"""
        try:
            with open('sign_vocabulary.json', 'r', encoding='utf-8') as f:
                return json.load(f)
        except FileNotFoundError:
            print("⚠️ Sign vocabulary file not found, using default")
            return {
                0: "HELLO", 1: "THANK_YOU", 2: "PLEASE", 3: "SORRY",
                4: "YES", 5: "NO", 6: "GOOD", 7: "BAD", 8: "HELP", 9: "STOP"
            }
    
    def extract_hand_features(self, landmarks):
        """สกัดฟีเจอร์จาก Hand Landmarks"""
        if not landmarks:
            return np.zeros(63)  # 21 landmarks * 3 coordinates
        
        features = []
        for landmark in landmarks.landmark:
            features.extend([landmark.x, landmark.y, landmark.z])
        
        return np.array(features)
    
    def predict_sign(self, left_features, right_features):
        """ทำนายภาษามือจากฟีเจอร์"""
        try:
            # Combine left and right hand features
            combined_features = np.concatenate([left_features, right_features])
            combined_features = combined_features.reshape(1, -1)
            
            # Predict using AI model
            prediction = self.model.predict(combined_features, verbose=0)
            predicted_class = np.argmax(prediction[0])
            confidence = np.max(prediction[0])
            
            if confidence > 0.8:  # Confidence threshold
                return self.signs.get(predicted_class, "UNKNOWN"), confidence
            else:
                return "UNCERTAIN", confidence
                
        except Exception as e:
            print(f"❌ Prediction error: {e}")
            return "ERROR", 0.0
    
    def control_servo(self, hand, finger, angle):
        """ควบคุม Servo Motor"""
        try:
            angle = max(0, min(180, angle))  # Clamp angle
            
            if hand == "left":
                self.left_kit.servo[finger].angle = angle
            elif hand == "right":
                self.right_kit.servo[finger].angle = angle
                
        except Exception as e:
            print(f"❌ Servo control error: {e}")
    
    def mirror_hand_pose(self, landmarks, hand_type):
        """สะท้อนท่าทางมือไปยังหุ่นยนต์"""
        if not landmarks:
            return
        
        try:
            # Calculate finger angles from landmarks
            angles = self.calculate_finger_angles(landmarks)
            
            # Control servos
            for i, angle in enumerate(angles):
                if i < 10:  # 10 servos per hand
                    self.control_servo(hand_type, i, angle)
                    
        except Exception as e:
            print(f"❌ Hand mirroring error: {e}")
    
    def calculate_finger_angles(self, landmarks):
        """คำนวณมุมนิ้วจาก Landmarks"""
        angles = []
        
        # Finger tip and base landmarks
        finger_tips = [4, 8, 12, 16, 20]  # Thumb, Index, Middle, Ring, Pinky
        finger_bases = [2, 5, 9, 13, 17]
        
        for tip, base in zip(finger_tips, finger_bases):
            try:
                tip_pos = landmarks.landmark[tip]
                base_pos = landmarks.landmark[base]
                
                # Calculate angle based on position difference
                dy = tip_pos.y - base_pos.y
                dx = tip_pos.x - base_pos.x
                
                angle = np.degrees(np.arctan2(dy, dx))
                angle = (angle + 180) % 180  # Normalize to 0-180
                
                angles.append(angle)
                
                # Add intermediate joint angle
                if len(angles) < 10:
                    angles.append(angle * 0.7)  # Approximate intermediate joint
                    
            except Exception as e:
                print(f"❌ Angle calculation error: {e}")
                angles.extend([90, 90])  # Default neutral position
        
        return angles[:10]  # Return exactly 10 angles
    
    def process_frame(self, frame, camera_id):
        """ประมวลผลเฟรมจากกล้อง"""
        rgb_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
        results = self.hands.process(rgb_frame)
        
        if results.multi_hand_landmarks:
            for idx, hand_landmarks in enumerate(results.multi_hand_landmarks):
                # Determine hand type
                hand_type = results.multi_handedness[idx].classification[0].label.lower()
                
                # Extract features
                features = self.extract_hand_features(hand_landmarks)
                
                # Store hand data
                if hand_type == "left":
                    self.left_hand_data = features
                else:
                    self.right_hand_data = features
                
                # Mirror hand pose to robot
                self.mirror_hand_pose(hand_landmarks, hand_type)
                
                # Draw landmarks
                self.mp_draw.draw_landmarks(
                    frame, hand_landmarks, self.mp_hands.HAND_CONNECTIONS
                )
        
        return frame
    
    def run(self):
        """เริ่มการทำงานหลัก"""
        print("🚀 Starting Sign Language Robot...")
        
        try:
            while True:
                # Read frames from both cameras
                ret1, frame1 = self.cap1.read()
                ret2, frame2 = self.cap2.read()
                
                if not ret1 or not ret2:
                    print("❌ Camera error")
                    break
                
                # Process frames
                frame1 = self.process_frame(frame1, 0)
                frame2 = self.process_frame(frame2, 1)
                
                # Predict sign if both hands detected
                if self.left_hand_data is not None and self.right_hand_data is not None:
                    sign, confidence = self.predict_sign(
                        self.left_hand_data, self.right_hand_data
                    )
                    
                    if sign != self.current_sign:
                        self.current_sign = sign
                        print(f"🤟 Detected Sign: {sign} (Confidence: {confidence:.2f})")
                
                # Display frames
                combined_frame = np.hstack([frame1, frame2])
                
                # Add text overlay
                cv2.putText(combined_frame, f"Current Sign: {self.current_sign}", 
                           (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
                
                cv2.imshow('Sign Language Robot', combined_frame)
                
                # Exit on 'q' key
                if cv2.waitKey(1) & 0xFF == ord('q'):
                    break
                    
        except KeyboardInterrupt:
            print("\n⏹️ Robot stopped by user")
        except Exception as e:
            print(f"❌ Runtime error: {e}")
        finally:
            self.cleanup()
    
    def cleanup(self):
        """ทำความสะอาดทรัพยากร"""
        print("🧹 Cleaning up resources...")
        
        # Release cameras
        self.cap1.release()
        self.cap2.release()
        cv2.destroyAllWindows()
        
        # Reset servos to neutral position
        for i in range(10):
            self.control_servo("left", i, 90)
            self.control_servo("right", i, 90)
        
        print("✅ Cleanup completed")

# การใช้งาน
if __name__ == "__main__":
    robot = SignLanguageRobot()
    robot.run()