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

# Define an enhanced 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],  # Position
                               [initial_velocity]]) # Velocity
        # State covariance matrix
        self.covariance = np.eye(2) * 1.0
        # 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)
        # Flag to check if initialized
        self.initialized = False
        # Residual (innovation)
        self.residual = None

    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
        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 prediction
        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 with measurement
        y = np.array([[measurement]]) - (self.H @ self.state)
        self.state = self.state + self.K @ y
        # Update the covariance
        I = np.eye(self.F.shape[0])
        self.covariance = (I - self.K @ self.H) @ self.covariance
        # Store residual
        self.residual = y[0][0]
        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('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
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
csv_file = open('landmarks_with_velocity_and_kalman.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',
        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 x, y, and z separately)
# Structure: kalman_filters[hand_label][landmark_index]['x'/'y'/'z']
kalman_filters = {
    'Left': [ {'x': KalmanFilter1D(), 'y': KalmanFilter1D(), 'z': KalmanFilter1D()} for _ in range(21) ],
    'Right': [ {'x': KalmanFilter1D(), 'y': KalmanFilter1D(), 'z': KalmanFilter1D()} for _ in range(21) ]
}

# To store previous frame's landmarks and velocities for velocity calculation
previous_landmarks = {'Left': None, 'Right': None}
previous_velocities = {'Left': None, 'Right': None}

# Data structures for plotting
plot_data = {
    'Left': [ {'frames': [], 'positions': [], 'velocities': [], 'kalman_positions': [], 'residuals': [], 'error_covariances': []} for _ in range(21) ],
    'Right': [ {'frames': [], 'positions': [], 'velocities': [], 'kalman_positions': [], 'residuals': [], 'error_covariances': []} for _ in range(21) ]
}

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 and result.multi_handedness:
            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  # 'Left' or 'Right'

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

                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

                        # Optionally, apply a moving average filter for smoothing
                        if previous_velocities[hand_label] is not None:
                            prev_velocity_x, prev_velocity_y, prev_velocity_z = previous_velocities[hand_label][i]
                            smoothed_velocity_x = (velocity_x + prev_velocity_x) / 2
                            smoothed_velocity_y = (velocity_y + prev_velocity_y) / 2
                            smoothed_velocity_z = (velocity_z + prev_velocity_z) / 2
                        else:
                            smoothed_velocity_x = velocity_x
                            smoothed_velocity_y = velocity_y
                            smoothed_velocity_z = velocity_z

                    else:
                        velocity_x = 0.0
                        velocity_y = 0.0
                        velocity_z = 0.0
                        smoothed_velocity_x = 0.0
                        smoothed_velocity_y = 0.0
                        smoothed_velocity_z = 0.0

                    current_landmarks.append((x, y, z))
                    current_velocities.append((velocity_x, velocity_y, velocity_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']

                    # 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)  # Green for raw landmark

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

                    # Append landmarks, velocity, and filtered coordinates to the row
                    landmarks_row.extend([
                        x, y, z,
                        velocity_x, velocity_y, velocity_z,
                        kalman_x_estimated, kalman_y_estimated, kalman_z_estimated
                    ])

                    # Collect data for plotting
                    plot_data[hand_label][i]['frames'].append(frame_count)
                    plot_data[hand_label][i]['positions'].append((x, y, z))
                    plot_data[hand_label][i]['velocities'].append((velocity_x, velocity_y, velocity_z))
                    plot_data[hand_label][i]['kalman_positions'].append((kalman_x_estimated, kalman_y_estimated, kalman_z_estimated))
                    plot_data[hand_label][i]['residuals'].append(kalman_x.residual)
                    plot_data[hand_label][i]['error_covariances'].append(kalman_x.covariance[0, 0])

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

                # Update previous landmarks and velocities
                previous_landmarks[hand_label] = current_landmarks
                previous_velocities[hand_label] = current_velocities

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

    frame_count += 1  # Increment frame count regardless of processing

    # Optional: Display the annotated frame (uncomment to use)
    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 remove all irrelevant lines of codes, 
explain the code in detail? 
also tell what changes i ahve to detect black gloves in video? 
tell what changes i have to do to enhance teh performance. 
For what more purposes i can use kalman filter, for hand activity recognition purposes?