import cv2
import face_recognition
from pyfirmata import Arduino, util

# Arduino setup
board = Arduino('/dev/cu.usbmodem1101')  # Replace with your Arduino's port
servo_pin = board.get_pin('d:11:s')  # Servo connected to pin 11
servo_pin2 = board.get_pin('d:10:s')  # Servo connected to pin 10
logic_gate_pin = board.get_pin('a:0:i')  # Logic gate output connected to analog pin A0

# Start the iterator thread for analog pins
it = util.Iterator(board)
it.start()
logic_gate_pin.enable_reporting()

# Initialize servo positions
servo_angle = 90  # Servo 1 starts at the middle position
servo_pin.write(servo_angle)

servo_angle2 = 90  # Servo 2 starts at the middle position
servo_pin2.write(servo_angle2)

# Wait for logic gate signal to start
print("Waiting for logic gate signal (1) on pin A0...")
while True:
    logic_gate_value = logic_gate_pin.read()
    if logic_gate_value is not None and logic_gate_value > 0.5:  # Adjust threshold as needed
        print("Logic gate signal detected. Starting system...")
        break

# Load your known face encodings and their names
known_face_encodings = []
known_face_names = []

# Load your image and create an encoding
your_image = face_recognition.load_image_file("/Users/aniketsethi/Documents/Coding/facial_recognition/your_image.jpg")
your_face_encoding = face_recognition.face_encodings(your_image)[0]

# Add your encoding to the known face list
known_face_encodings.append(your_face_encoding)
known_face_names.append("Aniket")  # Replace with your name

# Initialize webcam
video_capture = cv2.VideoCapture(1)

# Optionally set a lower resolution for the webcam
video_capture.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
video_capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)

# Camera field of view (adjust based on your camera specs)
camera_fov_horizontal = 70  # Degrees
camera_tilt_offset = 10  # Degrees (adjust for physical tilt of the camera)

while True:
    # Capture frame-by-frame
    ret, frame = video_capture.read()
    if not ret:
        break

    # Flip the frame horizontally (to fix mirroring)
    frame = cv2.flip(frame, 1)  # 1 means flipping around the vertical axis

    # Resize frame for faster processing
    scaling_factor = 2
    small_frame = cv2.resize(frame, (0, 0), fx=1 / scaling_factor, fy=1 / scaling_factor)
    rgb_small_frame = small_frame[:, :, ::-1].astype('uint8')

    # Detect faces and face locations
    face_locations = face_recognition.face_locations(rgb_small_frame)
    face_encodings = face_recognition.face_encodings(rgb_small_frame, face_locations)

    for (top, right, bottom, left), face_encoding in zip(face_locations, face_encodings):
        # Compare with known faces
        matches = face_recognition.compare_faces(known_face_encodings, face_encoding)
        face_distances = face_recognition.face_distance(known_face_encodings, face_encoding)

        name = "Unknown"
        if len(face_distances) > 0:
            best_match_index = face_distances.argmin()
            if matches[best_match_index]:
                name = known_face_names[best_match_index]

        # Scale back face locations
        top *= scaling_factor
        right *= scaling_factor
        bottom *= scaling_factor
        left *= scaling_factor

        # Draw face box and label
        cv2.rectangle(frame, (left, top), (right, bottom), (0, 255, 0), 2)
        cv2.putText(frame, name, (left + 6, top - 6), cv2.FONT_HERSHEY_DUPLEX, 0.5, (255, 255, 255), 1)

        # Check if the detected person is "Aniket"
        if name == "Aniket":
            # Calculate forehead position
            forehead_x = (left + right) // 2
            forehead_y = top  # Approximate forehead position as the top of the face box

            # Map forehead_x to servo 1 angle
            frame_width = video_capture.get(cv2.CAP_PROP_FRAME_WIDTH)
            normalized_x = (forehead_x / frame_width) - 0.5  # Normalize to range -0.5 to 0.5
            new_servo_angle = int((normalized_x * camera_fov_horizontal / 180) * 180 + 90)

            # Clamp to valid servo range
            new_servo_angle = max(0, min(180, new_servo_angle))

            # Update servo 1 position if significantly different
            if abs(new_servo_angle - servo_angle) > 5:  # Threshold to avoid jitter
                servo_angle = new_servo_angle
                servo_pin.write(servo_angle)
                print(f"Servo 1 angle updated to: {servo_angle}")

            # Map forehead_y to servo 2 angle with offset
            frame_height = video_capture.get(cv2.CAP_PROP_FRAME_HEIGHT)
            normalized_y = forehead_y / frame_height  # Normalize to range 0 to 1
            new_servo_angle2 = int(normalized_y * 180 + camera_tilt_offset)

            # Clamp to valid servo range
            new_servo_angle2 = max(0, min(180, new_servo_angle2))

            # Update servo 2 position if significantly different
            if abs(new_servo_angle2 - servo_angle2) > 5:  # Threshold to avoid jitter
                servo_angle2 = new_servo_angle2
                servo_pin2.write(servo_angle2)
                print(f"Servo 2 angle updated to: {servo_angle2}")
        else:
            print(f"Face detected but not Aniket. Name: {name}")

    # Show the frame
    cv2.imshow("Video", frame)

    # Exit on 'q' key press
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

# Release resources
video_capture.release()
cv2.destroyAllWindows()
servo_pin.write(90)  # Center servo 1 before exiting
servo_pin2.write(90)  # Center servo 2 before exiting
board.exit()