import cv2
import mediapipe as mp
import csv
import numpy as np

# Define a 1D Kalman Filter class with velocity
class KalmanFilter1D:
    def __init__(self, initial_position=0.0, initial_velocity=0.0):
        # State vector [position, velocity]
        self.state = np.array([[initial_position],
                               [initial_velocity]], dtype=np.float32)
        # State covariance matrix
        self.covariance = np.eye(2, dtype=np.float32)
        # State transition matrix (assuming constant velocity)
        self.F = np.array([[1, 1],
                           [0, 1]], dtype=np.float32)
        # Measurement matrix (we only measure position)
        self.H = np.array([[1, 0]], dtype=np.float32)
        # Process noise covariance
        self.Q = np.array([[1e-4, 0],
                           [0, 1e-4]], dtype=np.float32)
        # Measurement noise covariance
        self.R = np.array([[1e-1]], dtype=np.float32)
        # Kalman gain
        self.K = np.zeros((2, 1), dtype=np.float32)
        # Initialization flag
        self.initialized = False

    def initialize(self, measurement):
        self.state = np.array([[measurement],
                               [0.0]], dtype=np.float32)
        self.initialized = True

    def predict(self):
        # Predict the next state
        self.state = self.F @ self.state
        # Update the state covariance matrix
        self.covariance = self.F @ self.covariance @ self.F.T + self.Q

    def update(self, measurement):
        if not self.initialized:
            self.initialize(measurement)
            return self.state[0][0], self.state[1][0]  # Position, Velocity

        # Measurement residual
        y = np.array([[measurement]]) - (self.H @ self.state)
        # Measurement covariance
        S = self.H @ self.covariance @ self.H.T + self.R
        # Kalman gain
        self.K = self.covariance @ self.H.T @ np.linalg.inv(S)
        # Update the state estimate
        self.state = self.state + self.K @ y
        # Update the covariance estimate
        I = np.eye(self.F.shape[0], dtype=np.float32)
        self.covariance = (I - self.K @ self.H) @ self.covariance

        return self.state[0][0], self.state[1][0]  # Position, Velocity

# 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('Test_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
desired_fps = 10
frame_skip = max(int(fps / desired_fps), 1)  # Ensure at least 1
frame_count = 0  # To keep track of the frame number

# Time difference between frames
delta_t = 1 / desired_fps

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

# Prepare to write to CSV file
with open('landmarks_with_velocity_and_kalman.csv', mode='w', newline='') as csv_file:
    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',
            f'Landmark_{i}_velocity_x', f'Landmark_{i}_velocity_y', f'Landmark_{i}_velocity_z',
            f'Landmark_{i}_kalman_x', f'Landmark_{i}_kalman_y', f'Landmark_{i}_kalman_z'
        ])
    # Write header to CSV
    csv_writer.writerow(header)

    # Initialize Kalman filters for all 21 landmarks (for each hand)
    kalman_filters = {
        'Left': [{'x': KalmanFilter1D(), 'y': KalmanFilter1D(), 'z': KalmanFilter1D()} for _ in range(21)],
        'Right': [{'x': KalmanFilter1D(), 'y': KalmanFilter1D(), 'z': KalmanFilter1D()} for _ in range(21)]
    }

    # Store previous landmarks to compute velocity
    previous_landmarks = {'Left': None, 'Right': None}

    # Initialize frames since detection counters for left and right hands
    frames_since_detection = {'Left': 0, 'Right': 0}

    # Maximum allowed frames without detection before resetting filters
    max_frames_without_detection = 10

    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:
            # Initialize hand_present flags for this frame
            hand_present = {'Left': False, 'Right': False}

            # 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()

            # Initialize an empty list to store CSV rows for this frame
            csv_rows = []

            # Check if hand landmarks are detected
            if result.multi_hand_landmarks and result.multi_handedness:
                # Update hand presence flags
                for handedness in result.multi_handedness:
                    hand_label = handedness.classification[0].label
                    hand_present[hand_label] = True
                    # Reset frames_since_detection for this hand
                    frames_since_detection[hand_label] = 0

                for hand_landmarks, handedness in zip(result.multi_hand_landmarks, result.multi_handedness):
                    # Get hand label ('Left' or 'Right')
                    hand_label = handedness.classification[0].label

                    # 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
                    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
                    landmarks_row = [frame_count, hand_label]
                    current_landmarks = []  # To store current frame's landmarks

                    for i, landmark in enumerate(hand_landmarks.landmark):
                        # Original normalized coordinates
                        x_raw = landmark.x
                        y_raw = landmark.y
                        z_raw = landmark.z

                        # Adjust landmarks relative to wrist
                        x = x_raw - wrist_x
                        y = y_raw - wrist_y
                        z = z_raw - wrist_z

                        # Calculate velocity using previous landmarks
                        if previous_landmarks[hand_label] is not None:
                            previous_x, previous_y, previous_z = previous_landmarks[hand_label][i]
                            velocity_x = (x - previous_x) / delta_t
                            velocity_y = (y - previous_y) / delta_t
                            velocity_z = (z - previous_z) / delta_t
                        else:
                            velocity_x = 0.0
                            velocity_y = 0.0
                            velocity_z = 0.0

                        current_landmarks.append((x, y, z))

                        # Apply Kalman filter to the x, y, and z coordinates
                        kalman_x = kalman_filters[hand_label][i]['x']
                        kalman_y = kalman_filters[hand_label][i]['y']
                        kalman_z = kalman_filters[hand_label][i]['z']

                        # Check if filters need to be re-initialized after occlusion
                        if frames_since_detection[hand_label] == 0:
                            if not kalman_x.initialized:
                                # Initialize filters with the current measurement
                                kalman_x.initialize(x)
                                kalman_y.initialize(y)
                                kalman_z.initialize(z)
                        elif frames_since_detection[hand_label] > max_frames_without_detection:
                            # Reset Kalman filters
                            kalman_x = KalmanFilter1D()
                            kalman_y = KalmanFilter1D()
                            kalman_z = KalmanFilter1D()
                            kalman_filters[hand_label][i]['x'] = kalman_x
                            kalman_filters[hand_label][i]['y'] = kalman_y
                            kalman_filters[hand_label][i]['z'] = kalman_z

                        # Prediction step
                        kalman_x.predict()
                        kalman_y.predict()
                        kalman_z.predict()

                        # Update step with measurement
                        kalman_x_estimated, kalman_velocity_x = kalman_x.update(x)
                        kalman_y_estimated, kalman_velocity_y = kalman_y.update(y)
                        kalman_z_estimated, kalman_velocity_z = kalman_z.update(z)

                        # Visualization of Raw and Kalman-Filtered Landmarks
                        # Convert normalized coordinates to pixel coordinates for visualization
                        raw_x_pixel = int(x_raw * width)
                        raw_y_pixel = int(y_raw * height)
                        kalman_x_abs = kalman_x_estimated + wrist_x
                        kalman_y_abs = kalman_y_estimated + wrist_y
                        kalman_x_pixel = int(kalman_x_abs * width)
                        kalman_y_pixel = int(kalman_y_abs * height)

                        # Ensure pixel values are within image bounds
                        raw_x_pixel = np.clip(raw_x_pixel, 0, width - 1)
                        raw_y_pixel = np.clip(raw_y_pixel, 0, height - 1)
                        kalman_x_pixel = np.clip(kalman_x_pixel, 0, width - 1)
                        kalman_y_pixel = np.clip(kalman_y_pixel, 0, height - 1)

                        # Draw raw landmark (green)
                        cv2.circle(annotated_frame, (raw_x_pixel, raw_y_pixel), 3, (0, 255, 0), -1)

                        # Draw Kalman-filtered landmark (red)
                        cv2.circle(annotated_frame, (kalman_x_pixel, kalman_y_pixel), 3, (0, 0, 255), -1)

                        # Append data to CSV row
                        landmarks_row.extend([
                            x, y, z,
                            velocity_x, velocity_y, velocity_z,
                            kalman_x_estimated, kalman_y_estimated, kalman_z_estimated
                        ])

                    # Add the row to CSV rows
                    csv_rows.append(landmarks_row)

                    # Update previous landmarks
                    previous_landmarks[hand_label] = current_landmarks

            else:
                # When hands are not detected
                for hand_label in ['Left', 'Right']:
                    if frames_since_detection[hand_label] < max_frames_without_detection:
                        # Continue predicting using Kalman filters
                        landmarks_row = [frame_count, hand_label]
                        for i in range(21):
                            # Access Kalman filters
                            kalman_x = kalman_filters[hand_label][i]['x']
                            kalman_y = kalman_filters[hand_label][i]['y']
                            kalman_z = kalman_filters[hand_label][i]['z']

                            if kalman_x.initialized:
                                # Prediction step
                                kalman_x.predict()
                                kalman_y.predict()
                                kalman_z.predict()

                                # Use predicted state as current estimate (no update step)
                                kalman_x_estimated = kalman_x.state[0][0]
                                kalman_y_estimated = kalman_y.state[0][0]
                                kalman_z_estimated = kalman_z.state[0][0]

                                # Estimate velocity from state
                                kalman_velocity_x = kalman_x.state[1][0]
                                kalman_velocity_y = kalman_y.state[1][0]
                                kalman_velocity_z = kalman_z.state[1][0]

                                # Append data to CSV row
                                landmarks_row.extend([
                                    '', '', '',  # No measurement
                                    '', '', '',  # No measurement for velocity
                                    kalman_x_estimated, kalman_y_estimated, kalman_z_estimated
                                ])
                            else:
                                # If Kalman filter not initialized, append empty data
                                landmarks_row.extend([
                                    '', '', '',
                                    '', '', '',
                                    '', '', ''
                                ])

                        # Add the row to CSV rows
                        csv_rows.append(landmarks_row)

                    # Increment frames since detection
                    frames_since_detection[hand_label] += 1

                    # Reset filters if the hand has been missing for too long
                    if frames_since_detection[hand_label] > max_frames_without_detection:
                        # Reset Kalman filters for this hand
                        for i in range(21):
                            kalman_filters[hand_label][i]['x'] = KalmanFilter1D()
                            kalman_filters[hand_label][i]['y'] = KalmanFilter1D()
                            kalman_filters[hand_label][i]['z'] = KalmanFilter1D()
                        # Reset previous landmarks
                        previous_landmarks[hand_label] = None

            # Write all rows for this frame to the CSV
            for row in csv_rows:
                csv_writer.writerow(row)

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

        frame_count += 1  # Increment frame count regardless of processing

    # Release resources
    cap.release()
    output_video.release()
    hands.close()
rewrite the code to remove all duplicate or unused kind of lines or functionalities, and optimize the code?