# 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, name='RgbProcess', resolution=(320, 240, 3), numEntries=2, next=None):
		super(RgbProcess, self).__init__((numEntries, ) + resolution)
		self.name = name
		self.res = resolution
		self.next = next
		self.thread = Thread(target=self.run)
		self.cancel = False
		self.fps = None
		self.frames = 0

	# Interface
	# should be called to start thread
	def start(self):
		if not self.thread.is_alive():
			self.thread.start()

	# Interface
	# should be called to exit thread
	def stop(self):
		if self.thread.is_alive():
			if self.next is not None:
				self.next.stop()
			self.cancel = True
			self.thread.join()

	# overideable method
	# called on construction
	def onConstruct(self):
		pass

	# overideable method
	# called on first frame
	def onFirstFrame(self):
		pass

	# overideable method
	# called on last frame
	def onLastFrame(self):
		pass

	# overideable method
	def onProcess(self):
		pass

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

	# private
	# thread main loop
	def run(self):
		self.started()
		while not self.cancel:
			self.onProcess()
			self.frames += 1

		self.stopped()

	# private
	def started(self):
		print(self.name + ": started()")
		self.onFirstFrame()
		self.fps = FPS().start()
		if self.next is not None:
			self.next.start()

	# private
	def stopped(self):
		self.fps.stop()
		self.onLastFrame()
		print(self.name + ": stopped()")
		print(self.name + ": Processed " + str(self.frames) + " frames")
		print(self.name + ": FPS = " + str(self.frames/self.fps.elapsed()))

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.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 data is None:
			return

		if self.size == 0:
			for proc in self.processors:
				proc.start()

			self.fps = FPS().start()

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

		if self.needRgbConversion:
			rgbData = array.bytes_to_rgb(data, res)
		else:
			rgbData = data
		
		self.size += rgbData.size

		for proc in self.processors:
			proc.onData(rgbData)

	def size(self):
		return self.size

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

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

	# Unit test with to threaded processors
	class MyRgbProcess1(RgbProcess):
		def __init__(self, name, resolution=(320, 240, 3), numEntries=2, next=None):
			super(MyRgbProcess1, self).__init__(name, resolution, numEntries, next)

		def onProcess(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, name, resolution=(320, 240, 3), numEntries=2, next=None):
			super(MyRgbProcess2, self).__init__(name, resolution, numEntries, next)

		def onProcess(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('Process 2', (240, 320, 3), 4)
	myProcessor1 = MyRgbProcess1('Process 1', (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()