import numpy as np
import math
import time
from matplotlib import pyplot as plt
from Adafruit_AMG88xx import Adafruit_AMG88xx
from scipy.interpolate import griddata
import cv2
from time import sleep

# Start sensor
sensor = Adafruit_AMG88xx()
sleep(.1)

while(1):

	# Read pixels, map them to an 8x8 grid
	pixels = sensor.readPixels()
	points = [(math.floor(ix / 8), (ix % 8)) for ix in range(0, 64)]
	grid_x, grid_y = np.mgrid[0:7:32j, 0:7:32j]

	# bicubic interpolation of 8x8 grid to make a 32x32 grid
	bicubic = griddata(points, pixels, (grid_x, grid_y), method='cubic')
	image = np.array(bicubic)
	image = np.reshape(image, (32, 32))
	#print('Thermal Image has been saved!')
	#print(image)
	plt.imsave('color_img.jpg', image, vmin=20, vmax=60, cmap=plt.cm.gist_gray)

	# Read image into OpenCV
	img = cv2.imread("color_img.jpg", cv2.IMREAD_GRAYSCALE)

	# Setup SimpleBlobDetector parameters.
	params = cv2.SimpleBlobDetector_Params()

	# Activate filterByColor to only detect bright blobs (cups)
	params.filterByColor = True
	params.blobColor = 255

	# Change thresholds
	params.minThreshold = 100
	params.maxThreshold = 255
	params.thresholdStep = 2

	# Filter by Area.
	params.filterByArea = False
	params.minArea = 4

	# Filter by Circularity
	params.filterByCircularity = True
	params.minCircularity = 0.1

	# Filter by Convexity
	params.filterByConvexity = False

	# Filter by Inertia
	params.filterByInertia = True
	params.minInertiaRatio = 0.55
	params.maxInertiaRatio = 3.4028234663852886e+38 # infinity

	# Set up the detector with default parameters.
	detector = cv2.SimpleBlobDetector_create(params)

	# Detect blobs.
	keypoints = detector.detect(img)

	# If blobs exist, print  a message and the position
	for i in range (0, len(keypoints)):
		x = keypoints[i].pt[0]
		y = keypoints[i].pt[1]
		print('Tasse gefunden bei: ')
		print(x, y)
