import cv2
import mediapipe as mp
import numpy as np
import gradio as gr

# Initialize MediaPipe Pose and drawing utils.
mp_pose = mp.solutions.pose
pose = mp_pose.Pose(min_detection_confidence=0.5, min_tracking_confidence=0.5)
mp_drawing = mp.solutions.drawing_utils

# Calculate angle between three points.
def calculate_angle(a, b, c):
    a, b, c = np.array(a), np.array(b), np.array(c)
    radians = np.arctan2(c[1]-b[1], c[0]-b[0]) - np.arctan2(a[1]-b[1], a[0]-b[0])
    angle = np.abs(radians * 180.0 / np.pi)
    return angle

# Process the video and overlay lunge feedback.
def analyze_lunges(video_path):
    cap = cv2.VideoCapture(video_path)
    frame_width = int(cap.get(3))
    frame_height = int(cap.get(4))
    fps = cap.get(cv2.CAP_PROP_FPS) if cap.get(cv2.CAP_PROP_FPS) > 0 else 30

    output_video = "output_lunges.mp4"
    fourcc = cv2.VideoWriter_fourcc(*'mp4v')
    out = cv2.VideoWriter(output_video, fourcc, fps, (frame_width, frame_height))

    # Check lunge form based on leg angles.
    def check_lunge_feedback(landmarks):
        left_hip = [landmarks[mp_pose.PoseLandmark.LEFT_HIP.value].x,
                    landmarks[mp_pose.PoseLandmark.LEFT_HIP.value].y]
        left_knee = [landmarks[mp_pose.PoseLandmark.LEFT_KNEE.value].x,
                     landmarks[mp_pose.PoseLandmark.LEFT_KNEE.value].y]
        left_ankle = [landmarks[mp_pose.PoseLandmark.LEFT_ANKLE.value].x,
                      landmarks[mp_pose.PoseLandmark.LEFT_ANKLE.value].y]
        right_hip = [landmarks[mp_pose.PoseLandmark.RIGHT_HIP.value].x,
                     landmarks[mp_pose.PoseLandmark.RIGHT_HIP.value].y]
        right_knee = [landmarks[mp_pose.PoseLandmark.RIGHT_KNEE.value].x,
                      landmarks[mp_pose.PoseLandmark.RIGHT_KNEE.value].y]
        right_ankle = [landmarks[mp_pose.PoseLandmark.RIGHT_ANKLE.value].x,
                       landmarks[mp_pose.PoseLandmark.RIGHT_ANKLE.value].y]
        left_leg_angle = calculate_angle(left_hip, left_knee, left_ankle)
        right_leg_angle = calculate_angle(right_hip, right_knee, right_ankle)
        left_accuracy = max(0, min(100, (1 - abs(left_leg_angle - 90) / 30) * 100))
        right_accuracy = max(0, min(100, (1 - abs(right_leg_angle - 90) / 30) * 100))
        feedback = "Correct Lunge"
        if left_leg_angle < 70 or right_leg_angle < 70:
            feedback = "Incorrect Lunge - Deep enough"
        elif left_leg_angle > 110 or right_leg_angle > 110:
            feedback = "Incorrect Lunge - Lower hips"
        return feedback, left_accuracy, right_accuracy

    # Draw separate accuracy bars for the left and right legs.
    def draw_accuracy_bar(image, left_accuracy, right_accuracy):
        bar_x, bar_y = 50, 400
        bar_width, bar_height = 200, 20
        left_fill_width = int((left_accuracy / 100) * bar_width)
        cv2.rectangle(image, (bar_x, bar_y), (bar_x + bar_width, bar_y + bar_height), (200, 200, 200), 2)
        cv2.rectangle(image, (bar_x, bar_y), (bar_x + left_fill_width, bar_y + bar_height), (0, 255, 0), -1)
        cv2.putText(image, f"Left Leg Accuracy: {left_accuracy}%", (bar_x, bar_y - 10),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2)
        right_bar_y = bar_y + 50
        right_fill_width = int((right_accuracy / 100) * bar_width)
        cv2.rectangle(image, (bar_x, right_bar_y), (bar_x + bar_width, right_bar_y + bar_height), (200, 200, 200), 2)
        cv2.rectangle(image, (bar_x, right_bar_y), (bar_x + right_fill_width, right_bar_y + bar_height), (0, 255, 0), -1)
        cv2.putText(image, f"Right Leg Accuracy: {right_accuracy}%", (bar_x, right_bar_y - 10),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2)

    while cap.isOpened():
        ret, frame = cap.read()
        if not ret:
            break
        # Process frame with MediaPipe.
        image = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
        results = pose.process(image)
        image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
        if results.pose_landmarks:
            mp_drawing.draw_landmarks(image, results.pose_landmarks, mp_pose.POSE_CONNECTIONS)
            feedback, left_accuracy, right_accuracy = check_lunge_feedback(results.pose_landmarks.landmark)
            draw_accuracy_bar(image, left_accuracy, right_accuracy)
            color = (0, 255, 0) if "Correct" in feedback else (0, 0, 255)
            cv2.putText(image, feedback, (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, color, 3)
        out.write(image)
    cap.release()
    out.release()
    return output_video

# Gradio Interface for lunge analysis.
gr.Interface(
    fn=analyze_lunges,
    inputs=gr.Video(),
    outputs=gr.Video(),
    title="Lunge Form Analyzer",
    description="Upload a video of your lunges, and get feedback on your form!",
).launch()