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 (2 frames per second)
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 the code to only save the position and velocity of landmarks, delete the kalman filter portion from it? also delete any irrelevant code and also add important checks for enhanced velocity capturing as well? also explain the modified code in detial??