import cv2
import numpy as np
import pandas as pd
from filterpy.kalman import UnscentedKalmanFilter, MerweScaledSigmaPoints
from ultralytics import YOLO

model = YOLO('/Users/surabhi/Documents/kalman/best.pt')
def fx(x, dt):
# This function predicts the state of the system at time t+1
# based on the state at time t and the time step dt.
# The state vector x has 10 elements: [pos_x, pos_y, vel_x, vel_y, acc_x, acc_y, jerk_x, jerk_y, snap_x, snap_y]
    F = np.array([[1, 0, dt, 0, 0.5*dt**2, 0, (1/6)*dt**3, 0, (1/24)*dt**4, 0],
                    [0, 1, 0, dt, 0, 0.5*dt**2, 0, (1/6)*dt**3, 0, (1/24)*dt**4],
                    [0, 0, 1, 0, dt, 0, 0.5*dt**2, 0, (1/6)*dt**3, 0],
                    [0, 0, 0, 1, 0, dt, 0, 0.5*dt**3, 0, (1/6)*dt**4],
                    [0, 0, 0, 0, 1, 0, dt, 0, 0.5*dt**2, 0],
                    [0, 0, 0, 0, 0, 1, 0, dt, 0, 0.5*dt**3],
                    [0, 0, 0, 0, 0, 0, 1, 0, dt, 0],
                    [0, 0, 0, 0, 0, 0, 0, 1, 0, dt],
                    [0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
                    [0, 0, 0, 0, 0, 0, 0, 0, 0, 1]])
    B = np.array([[0.5*dt**2, 0, (1/6)*dt**3, 0],
                [0, 0.5*dt**2, 0, (1/6)*dt**3],
                [dt, 0, 0.5*dt**2, 0],
                [0, dt, 0, 0.5*dt**2],
                [0, 0, dt, 0],
                [0, 0, 0, dt],
                [0, 0, 0, 0],
                [0, 0, 0, 0],
                [0, 0, 0, 0],
                [0, 0, 0, 0]])
    return np.dot(F, x)+ np.dot(B, u)
dt = 1.0
kf = UnscentedKalmanFilter(dim_x=10, dim_z=2, dt=dt, hx=None, fx=fx, points=MerweScaledSigmaPoints(n=10, alpha=0.1, beta=2., kappa=-1))
kf.x = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) # initial state estimate
kf.P = np.eye(10) * 1000 # initial error covariance matrix
kf.R = np.diag([0.1, 0.1]) # measurement noise covariance matrix

kf.Q = lambda dt, v: np.diag([dt**4/4, dt**4/4, dt**2/2, dt**2/2, dt, dt, 0.1*dt**4/4, 0.1*dt**4/4, 0.1*dt**2/2, 0.1*dt**2/2])
kf.sigma_points = MerweScaledSigmaPoints(n=10, alpha=0.1, beta=2., kappa=-1)

u = np.zeros((4, 1))

cap = cv2.VideoCapture("1_1.mp4")
frame_num = 0
predicted_points = []
bounce_detected = False
last_bounce_frame = -10
test_df = pd.DataFrame(columns=['frame', 'x', 'y', 'vx', 'vy', 'ax', 'ay', 'V'])

while True:
    ret, frame = cap.read()
    if ret is False:
        break
    bbox = model(frame, show=True)
    frame_num += 1
    for boxes_1 in bbox:
        result = boxes_1.boxes.xyxy
        if len(result) == 0:
            print("not detected")
        else:
            cx = int((result[0][0] + result[0][2]) / 2)
            cy = int((result[0][1] + result[0][3]) / 2)
            centroid = np.array([cx, cy])
            kf.predict(dt=dt, control_input=u)
            kf.update(centroid)
            next_point = (kf.x).tolist()
            predicted_points.append((int(next_point[0]), int(next_point[1])))
            if len(predicted_points) > 10:
                predicted_points.pop(0)
            print("next_point", next_point)
            print("frame_number", frame_num)
            if next_point[2] > 0:
                vx = "positive"
            else:
                vx = "negative"
            if next_point[3] > 0:
                vy = "positive"
            else:
                vy = "negative"
            test_df = test_df.append({
                'frame': frame_num,
                'x': next_point[0],
                'y': next_point[1],
                'vx': next_point[2],
                'vy': next_point[3],
                'ax': next_point[4],
                'ay': next_point[5],
                'V': np.sqrt(kf.x[2]**2 + kf.x[3]**2)
            }, ignore_index=True)
            cv2.putText(frame, f'Frame: {frame_num}', (10, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
            cv2.circle(frame, (cx, cy), 5, (0,0,255), 5)
            cv2.circle(frame, (int(next_point[0]), int(next_point[1])), 5, (255, 0, 0), 10)
            for i, p in enumerate(predicted_points):
                color = (255,255,255)
                cv2.circle(frame, p, 5, color, 2)
                print(kf.x[2])
            if not bounce_detected and frame_num - last_bounce_frame > 50:
                if abs(next_point[2]) < 1 and test_df.shape[0] > 1 and test_df.shape[1] > 3 and np.sign(test_df.iloc[-2, 3]) == np.sign(kf.x[2]) and np.sqrt(kf.x[2]**2 + kf.x[3]**2) < 5:
                    bounce_detected = True
                    last_bounce_frame = frame_num
                    print("Bounce detected")

            if bounce_detected:
                cv2.putText(frame, 'Bounce Detected', (10, 350), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
            if kf.x[5] > 0:
                bounce_detected = False
                print(test_df)
                test_df.to_csv('file.csv')

cv2.imshow('raw', frame)

cap.release()
cv2.destroyAllWindows()