# 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
from piCameraPipeline.VideoSource import VideoSource

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("-e", "--with-ev3", type=bool, default=False,
	help="Framerate in frame/s")
ap.add_argument("-f", "--filename", type=str, default='piCamera',
	help="Choose video source")
args = vars(ap.parse_args())

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

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

	def sendBrick(self, errorX, errorY):
		if self.brick is not None:
			data = struct.pack("f", errorX)
			ev3.system_command.write_mailbox(self.brick, 'ErrorX', data)
			data = struct.pack("f", errorY)
			ev3.system_command.write_mailbox(self.brick, 'ErrorY', data)

	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:
					if not self.isVisible:
						print ("Visible")
					self.isVisible = True
					img1_objects = cv2.rectangle(img1_objects,(x-5,y-5),(x+5,y+5),(0,0,255),2)
					errorX = float(x)-160
					errorY = float(y)-120
					self.sendBrick(errorX/2, errorY/2)
#					print(maxLoc)
				else:
					if self.isVisible:
						print ("NOT Visible")
						self.sendBrick(0, 0)
					self.isVisible = False
			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

if with_ev3:
	with ev3.EV3() as brick:

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

		print("[INFO] sampling THREADED frames from `" + videoFile + "` at " + str(framerate) + " frame/s")
		time.sleep(seconds)
		videoSource.stop()
		proc.stop()
else:
	videoSource = VideoSource(videoFile, (width,height,3), framerate)
	proc = MyRgbProcess('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()

# do a bit of cleanup
cv2.destroyAllWindows()