import cv2
import mediapipe as mp
import csv

# Initialize MediaPipe Hand Tracking
mp_hands = mp.solutions.hands
hands = mp_hands.Hands(
    static_image_mode=False,       # For video input
    max_num_hands=2,               # Maximum number of hands to detect
    min_detection_confidence=0.5,  # Minimum confidence for detection
    min_tracking_confidence=0.5    # Minimum confidence for tracking
)
mp_drawing = mp.solutions.drawing_utils  # For drawing hand landmarks

# Open the input video
cap = cv2.VideoCapture('input_video.mp4')

# Get video properties
width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))   # Video frame width
height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) # Video frame height
fps = cap.get(cv2.CAP_PROP_FPS)                  # Original frames per second

# Desired processing frame rate (2 frames per second)
desired_fps = 2  
frame_skip = int(fps / desired_fps)  # Calculate how many frames to skip
frame_count = 0  # To keep track of the frame number

# Initialize Video Writer for output video
output_video = cv2.VideoWriter(
    'output_video.mp4',
    cv2.VideoWriter_fourcc(*'mp4v'),
    desired_fps,  # Set output video FPS to desired FPS
    (width, height)
)

# Prepare to write to CSV file
csv_file = open('landmarks_with_velocity.csv', mode='w', newline='')
csv_writer = csv.writer(csv_file)

# Create CSV header
header = ['Frame', 'Hand']
for i in range(21):  # 21 landmarks in MediaPipe Hands
    header.extend([f'Landmark_{i}_x', f'Landmark_{i}_y', f'Landmark_{i}_z'])
    header.extend([f'Velocity_{i}_x', f'Velocity_{i}_y', f'Velocity_{i}_z'])  # Add velocity columns
# Write header to CSV
csv_writer.writerow(header)

# To store the landmarks from the previous frame
previous_landmarks = {}

while cap.isOpened():
    ret, frame = cap.read()
    if not ret:
        break

    # Process every 'frame_skip' frames to achieve desired FPS
    if frame_count % frame_skip == 0:
        # Convert frame to RGB for MediaPipe processing
        frame_rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
        result = hands.process(frame_rgb)

        # Create a copy of the frame for drawing
        annotated_frame = frame.copy()

        # Check if hand landmarks are detected
        if result.multi_hand_landmarks:
            for hand_index, hand_landmarks in enumerate(result.multi_hand_landmarks):
                # Draw the hand landmarks on the frame
                mp_drawing.draw_landmarks(
                    annotated_frame, hand_landmarks, mp_hands.HAND_CONNECTIONS)

                # Get the wrist landmark as reference (landmark index 0)
                wrist = hand_landmarks.landmark[mp_hands.HandLandmark.WRIST]
                wrist_x = wrist.x
                wrist_y = wrist.y
                wrist_z = wrist.z

                # Initialize a list to store adjusted landmarks and velocities
                landmarks_row = [frame_count, hand_index]

                # For velocity calculation: check if the current hand existed in the previous frame
                if hand_index in previous_landmarks:
                    prev_landmarks = previous_landmarks[hand_index]
                else:
                    # If no previous landmarks, initialize with zeros
                    prev_landmarks = [(0.0, 0.0, 0.0) for _ in range(21)]

                for i, landmark in enumerate(hand_landmarks.landmark):
                    # Adjust landmarks relative to wrist
                    x = landmark.x - wrist_x
                    y = landmark.y - wrist_y
                    z = landmark.z - wrist_z
                    landmarks_row.extend([x, y, z])

                    # Calculate velocity: (current - previous) / time
                    prev_x, prev_y, prev_z = prev_landmarks[i]
                    velocity_x = (x - prev_x) / (1 / desired_fps)  # Change in position / time interval
                    velocity_y = (y - prev_y) / (1 / desired_fps)
                    velocity_z = (z - prev_z) / (1 / desired_fps)
                    landmarks_row.extend([velocity_x, velocity_y, velocity_z])

                # Write the landmarks and velocities to the CSV
                csv_writer.writerow(landmarks_row)

                # Update previous landmarks for the current hand
                previous_landmarks[hand_index] = [(landmark.x - wrist_x, landmark.y - wrist_y, landmark.z - wrist_z) for landmark in hand_landmarks.landmark]

        # Write the annotated frame to output video
        output_video.write(annotated_frame)

    frame_count += 1  # Increment frame count regardless of processing
    
    cv2.imshow('Annotated Frame', annotated_frame)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

# Release resources
cap.release()
output_video.release()
hands.close()
csv_file.close()
cv2.destroyAllWindows()
rewrite this code to add a kalman filter for landmark x,y,z, also explain, How can i add more features like angles or else, the video is created using hand held camera? which ml or dl algorithms would work best for activity recogniton on these landmarks?