import cv2
import matplotlib.pyplot as plt
import numpy as np
import csv

startind=71
ind=startind-50
scale=(1107-15)/(7*12*25.4/1000)#pixels/m


cap = cv2.VideoCapture("clipping_rate_test.mp4")
ret, frame = cap.read()

cap.set(cv2.CAP_PROP_POS_FRAMES, ind)



tracker = cv2.TrackerMIL_create()
bbox=[210,520,50,50]

ret, frame = cap.read()
#initialize tracker, set bounding box
ok=tracker.init(frame,bbox)

width  = int(cap.get(3))   # float `width`
height = int(cap.get(4))  # float `height`
print([width,height])
output = cv2.VideoWriter('output_video_from_file_nobox.avi', cv2.VideoWriter_fourcc('M','J','P','G'), 20, [width, height])

positions=[]
times=[]

while(1):
   ret, frame = cap.read()
   if cv2.waitKey(1) & 0xFF == ord('q') or ret==False :
       cap.release()
       cv2.destroyAllWindows()
       break

   ok,bbox = tracker.update(frame) #update tracking
   positions.append(-(bbox[1]+bbox[3]/2)/scale)
   times.append((ind-startind)/240)#120 fps
         
   font = cv2.FONT_HERSHEY_SIMPLEX
   cv2.putText(frame,'t='+"{:.3f}".format((ind-startind)/240)+' s',(50,150),font, 4,(255,255,255),2,cv2.LINE_AA)
   #cv2.rectangle(frame, (bbox[0], bbox[1]), (bbox[0]+bbox[2], bbox[1]+bbox[3]), (0, 255, 0), 2)#Draw Bounding Box Around Object
   for i in range(30):
      cv2.line(frame, (150,14+round(scale*i/10)),(250,14+round(scale*i/10)) , (255, 0, 255), 1)#Draw Bounding Box Around Object

   cv2.imshow('frame',frame)
   
   #print(frame.size)
   fps = cap.get(cv2.CAP_PROP_FPS)
   #print(fps)
   #print(ind)
   timer=cv2.getTickCount()
   if (ind-startind)%6==0:
      cv2.imwrite('.\\toss_high_frames\\'+str(ind)+'.jpg', frame)
   #write video frame to video
   output.write(frame)
   ind+=1
   
   if ind>320:#startind+215
      break

cv2.imwrite('bbox_toss_high'+str(ind)+'.jpg', frame)

output.release()
cap.release()

np.savetxt('Toss_high.csv', (times, positions), delimiter=',')


#fit and plot output from object tracking as time series data

thefit = np.polyfit(times,positions,2)
plt.plot(times,positions,'bx-')
plt.plot(times,np.multiply(np.power(times,2),thefit[0])+np.multiply(times,thefit[1])+thefit[2],'r')
plt.plot
plt.xlabel('times (s)')
plt.ylabel('position (m)')
print(thefit[0]*2)
plt.show()
