# import the necessary packages
from __future__ import print_function

from imutils.video import FPS

import sys
import numpy as np
import argparse
import time
import cv2
import pprint
import math
import struct
import ev3.ev3 as ev3
from threading import Thread

from matplotlib import pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

from picamera import PiCamera
from piCameraPipeline import PiCameraPipeline
from piCameraPipeline.PiCameraPipeline import RgbProcess
from piCameraPipeline.PiCameraPipeline import RgbProcessorAdapter

width = 320
height = 240

# construct the argument parse and parse the arguments
ap = argparse.ArgumentParser()
ap.add_argument("-n", "--seconds", type=int, default=10,
	help="# of seconds to run")
ap.add_argument("-r", "--framerate", type=int, default=30,
	help="Framerate in frame/s")
ap.add_argument("-f", "--filename", type=str, default='piCamera',
	help="Whether or not frames should be displayed")
args = vars(ap.parse_args())

videoFile = args["filename"]
framerate = args["framerate"]
seconds = args["seconds"]

class VideoSource(RgbProcessorAdapter):
	def __init__(self, fileName, resolution, framerate):
		super(VideoSource, self).__init__(resolution)
		self.fileName = fileName
		self.resolution = resolution
		self.framerate = framerate
		self.thread = None
		self.camera = None
		self.cancel = False
		if fileName == 'piCamera':
			self.camera = PiCamera()
			self.camera.resolution = (self.resolution[0], self.resolution[1])
			self.camera.framerate = self.framerate
		else:
			self.needRgbConversion = False
			self.thread = Thread(target=self.fileReadThread)

	def fileReadThread(self):
		pass

	def start(self):
		if self.thread is not None:
			self.thread.start()
			pass
		else:
			self.camera.start_recording(self, format="bgr")
		
	def stop(self):
		if self.thread is not None:
			self.cancel = True
			self.thread.join()
		else:
			self.camera.stop_recording()

	def fileReadThread(self):
		cap = cv2.VideoCapture(self.fileName)
		while not self.cancel:
			ret, frame = cap.read()
			self.write(frame)
			time.sleep(1.0/self.framerate)


class MyRgbProcess(RgbProcess):
	def __init__(self, resolution=(320, 240, 3), numEntries=2, next=None):
		super(MyRgbProcess, self).__init__(resolution, numEntries, next)

	def process(self):
		data = self.read(1.0)
		if data is not None:
			gray = cv2.cvtColor(data,cv2.COLOR_BGR2GRAY)

			img1_objects = data.copy()
			if 1:
				# Min / max approach
				gray_blurred = cv2.medianBlur(gray,5)
				minVal, maxVal, minLoc, maxLoc = cv2.minMaxLoc(gray_blurred)
				x = maxLoc[0]
				y = maxLoc[1]
				if maxVal > 100*minVal:
					img1_objects = cv2.rectangle(img1_objects,(x-5,y-5),(x+5,y+5),(0,0,255),2)
					print(maxLoc)
			else:
				# Contour approach
				gray_blurred = cv2.GaussianBlur(gray,(5,5),0)
				ret,thresh = cv2.threshold(gray_blurred,0,255,cv2.THRESH_BINARY+cv2.THRESH_OTSU)
				# Contours
				(_, contours, hierachy) = cv2.findContours(thresh.copy(),cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
				img1_contours = np.zeros((height,width,3), np.uint8)
				img1_contours = cv2.drawContours(img1_contours, contours, -1, (0,255,0), 1)
				for contour in contours:
					x,y,w,h = cv2.boundingRect(contour)
					roi = gray[y:y+h, x:x+w]
					mean_color = cv2.mean(gray)
					mean_color_roi = cv2.mean(roi)
					if mean_color_roi[0] > 2*mean_color[0]:
						print (mean_color)
						print (mean_color_roi)
						img1_objects = cv2.rectangle(img1_objects,(x,y),(x+w,y+h),(0,0,255),2)

				cv2.imshow('Contours', img1_contours)

			cv2.imshow('Objects', img1_objects)
			cv2.waitKey(1)

			if self.next is not None:
				self.next.write(cv2.cvtColor(gray_blurred,cv2.COLOR_GRAY2BGR))

# Connect to brick
#with ev3.EV3() as brick:

# created a *threaded *video stream, allow the camera sensor to warmup,
# and start the FPS counter

videoSource = VideoSource(videoFile, (width,height,3), framerate)
proc = MyRgbProcess((240, 320, 3), 4)
videoSource.processorAdd(proc)
videoSource.start()

print("[INFO] sampling THREADED frames from `" + videoFile + "` at " + str(framerate) + " frame/s")
time.sleep(seconds)
videoSource.stop()
proc.stop()

# do a bit of cleanup
cv2.destroyAllWindows()