# import the necessary packages
from __future__ import print_function
from imutils.video import FPS
from picamera import PiCamera
from picamera import array
import imutils
import time
import numpy as np
from threading import Thread
from threading import Lock
from threading import Semaphore

class Fifo(object):
	def __init__(self, dim=(4, 320, 240, 3), dtype=np.uint8):
		self.size = 0
		self.capacity = dim[0]
		self.wi = 0
		self.ri = 0
		self.buffer = np.empty(dim, dtype=dtype)
		self.lock = Lock()
		self.sema = Semaphore(value=0)

	def write(self, data):
		shouldNotify = False
		
		self.lock.acquire()

		self.buffer[self.wi] = data
		self.wi += 1

		if self.wi == self.capacity:
			self.wi = 0

		if self.size < self.capacity:
			self.size += 1
			shouldNotify = True

		self.lock.release()

		if shouldNotify:
			self.sema.release()

	def read(self, timeout=None):
		data = None
		
		if self.sema.acquire(blocking=True, timeout=timeout):

			self.lock.acquire()
	
			data = self.buffer[self.ri]
			self.ri += 1

			if self.ri == self.capacity:
				self.ri = 0

			self.size -= 1

			self.lock.release()

		return data
		
class RgbProcess(Fifo):
	def __init__(self, resolution=(320, 240, 3), numEntries=2, next=None):
		super(RgbProcess, self).__init__((numEntries, ) + resolution)
		self.next = next
		self.thread = Thread(target=self.run)
		self.cancel = False
		self.fps = None
		self.frames = 0

	# called on construction
	def onConstruct(self):
		pass

	# called on first frame
	def onFirstFrame(self):
		self.thread.start()
		if self.next is not None:
			self.next.onFirstFrame()

	# may be overriden to process in caller context
	def onData(self, data):
		self.write(data)

	# thread main loop
	def run(self):
		self.fps = FPS().start()
		while not self.cancel:
			self.process()
			self.frames += 1

	# abstractmethod
	def process(self):
		pass

	# should be called to exit thread
	def stop(self):
		self.fps.stop()
		print("Processed " + str(self.frames) + " frames")
		print("FPS = " + str(self.frames/self.fps.elapsed()))
		self.cancel = True
		self.thread.join()
		if self.next is not None:
			self.next.stop()

class RgbProcessorAdapter(object):
	def __init__(self, resolution=(320, 240, 3)):
		self.resolution = resolution
		self.frameSize = resolution[0]*resolution[1]*resolution[2]
		self.size = 0
		self.frames = 0
		self.fps = None
		self.processors = []
		self.needRgbConversion = True

	def processorAdd(self, processor):
		self.processors.append(processor)		
		processor.onConstruct()

	def writable(self):
		return True

	def write(self, data):
		if self.size == 0:
			for proc in self.processors:
				proc.onFirstFrame()

			self.fps = FPS().start()
		
		if data is None:
			return

		self.size += len(data)
		self.frames += len(data)/self.frameSize

		res = (self.resolution[0], self.resolution[1])

		if self.needRgbConversion:
			rgbData = array.bytes_to_rgb(data, res)
		else:
			rgbData = data
		
		for proc in self.processors:
			proc.onData(rgbData)

	def size(self):
		return self.size

	def flush(self):
		if self.size > 0:
			self.fps.stop()
			print("Recorded " + str(self.frames) + " frames (" + str(self.size) + " bytes)")
			print("FPS = " + str(self.frames/self.fps.elapsed()))

if ("__main__" == __name__):
	import cv2

	# Unit test with to threaded processors
	class MyRgbProcess1(RgbProcess):
		def __init__(self, resolution=(320, 240, 3), numEntries=2, next=None):
			super(MyRgbProcess1, 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)
				gray_blurred = cv2.medianBlur(gray,5)

				cv2.imshow('displayThread1', gray_blurred)
				cv2.waitKey(1)

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

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

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

				# Canny edge detection
				canny = cv2.Canny(gray, 100, 50)
				cv2.imshow('displayThread2', canny)
				cv2.waitKey(1)

	# initialize the camera and stream
	camera = PiCamera()
	camera.resolution = (320, 240)
	camera.framerate = 60
	frameDim = (camera.resolution[0], camera.resolution[1], 3)
	myProcessor2 = MyRgbProcess2((240, 320, 3), 4)
	myProcessor1 = MyRgbProcess1((240, 320, 3), 4, myProcessor2)
	myRgbProcessorAdapter = RgbProcessorAdapter(frameDim)
	myRgbProcessorAdapter.processorAdd(myProcessor1)
	camera.start_recording(myRgbProcessorAdapter, format="bgr")

	dictFifo = Fifo(dim=(8,), dtype=dict)
	dictFifo.write({'a':1, 'b':2})
	print(dictFifo.read())

	# allow the camera to warmup and start the FPS counter
	print("[INFO] sampling frames from `picamera` module...")
	time.sleep(10.0)
	camera.stop_recording()
	myProcessor1.stop()

	# do a bit of cleanup
	cv2.destroyAllWindows()
	camera.close()