import os
import queue
from tkinter import N
import cv2
import numpy as np
from PIL import Image, ImageDraw
import csv
import sys

try:
	input_video_path = sys.argv[1]
	input_csv_path = sys.argv[2]
	#output_video_path = sys.argv[3]
	if (not input_video_path) or (not input_csv_path):
		raise ''
except:
	print('usage: python show_trajectory.py <input_video_path> <input_csv_path>')
	exit(1)

cout = 0
with open(input_csv_path) as csvfile:
	readCSV = csv.reader(csvfile, delimiter=',')
	frames = []
	x, y = [], []
	list1 = []
	for row in readCSV:
		list1.append(row)
	n_frame = len(list1)
for i in range(1 , n_frame):
	if int(float(list1[i][2])) != 0 or int(float(list1[i][3])) != 0:
		first_tra = i
		break
for i in range(1 , n_frame):
	frames += [int(list1[i][0])]
	if int(float(list1[i][2])) != 0 or int(float(list1[i][3])) != 0:
		x.append(int(float(list1[i][2])))
		y.append(int(float(list1[i][3])))
		cout += 1


curve_fit2 = np.polyfit(x, y, 2)  # 拟合为二次曲线
plotx = np.linspace(x[0], x[-1], abs(x[-1]-x[0])+1)  # 按步长为1，设置点的x坐标
ploty2 = curve_fit2[0]*plotx**2 + curve_fit2[1]*plotx + curve_fit2[2]  # 得到二次曲线对应的y坐标
curve_fited2 = np.array([np.transpose(np.vstack([plotx, ploty2]))])  # 得到二次曲线对应的点集

curve_per_frame = int(abs(plotx[-1] - plotx[0] + 1) / (n_frame - 1))
# print(curve_per_frame)
# print(curve_fited2[::,:1 * curve_per_frame])
video = cv2.VideoCapture(input_video_path)
'''
fps = 15
output_width = int(video.get(cv2.CAP_PROP_FRAME_WIDTH))
output_height = int(video.get(cv2.CAP_PROP_FRAME_HEIGHT))
output_video_path = input_video_path[:-4]+'_predict_666'+input_video_path[-4:]
fourcc = cv2.VideoWriter_fourcc(*'mp4v')
output_video = cv2.VideoWriter(output_video_path,fourcc, fps, (output_width,output_height))
'''
for i in range(1, n_frame):
	ret, img1 = video.read()
	if not ret: 
		break
	cv2.polylines(img1, np.int_([curve_fited2[::,:i * curve_per_frame]]), False, (0, 0, 255), 5)  # 红色 二次曲线上的散点构成的折线图，近似为曲线
	cv2.namedWindow('Curve', flags=cv2.WINDOW_NORMAL |
					cv2.WINDOW_KEEPRATIO | cv2.WINDOW_GUI_EXPANDED)
	cv2.imshow('Curve', img1)
	# output_video.write(img1)
	key = cv2.waitKey(600)
	if key & 0xFF == ord('q') or key == 27:
		cv2.destroyAllWindows()
		break
# output_video.release()