# -*- coding: utf-8 -*-
from PIL import Image
import numpy as np
import cv2
import cv2.aruco as aruco
import matplotlib.pyplot as plt

import pyglet




def drawpoint(x, y, color, pic):
    pic.putpixel((int(x), int(y)), color)
    pic.putpixel((int(x) + 1, int(y) - 1), color)
    pic.putpixel((int(x) - 1, int(y) + 1), color)
    pic.putpixel((int(x) + 1, int(y) + 1), color)
    pic.putpixel((int(x) - 1, int(y) - 1), color)
    pic.putpixel((int(x) + 1, int(y)), color)
    pic.putpixel((int(x) - 1, int(y)), color)
    pic.putpixel((int(x), int(y) + 1), color)
    pic.putpixel((int(x), int(y) - 1), color)


aruco_dict = aruco.custom_dictionary(0, 4, 1)
aruco_dict.bytesList = np.empty(shape=(4, 2, 4), dtype=np.uint8)
mybits = np.array([[0, 1, 1, 0], [0, 0, 1, 0], [1, 0, 0, 1], [1, 0, 0, 1]], dtype=np.uint8)
mybits2 = np.array([[0, 0, 1, 1], [0, 0, 1, 1], [0, 0, 1, 0], [1, 1, 0, 1]], dtype=np.uint8)
mybits3 = np.array([[1, 0, 1, 1], [0, 1, 0, 1], [0, 0, 1, 1], [0, 0, 1, 0]], dtype=np.uint8)
mybits4 = np.array([[0, 1, 1, 0], [0, 1, 0, 1], [0, 1, 0, 0], [0, 1, 1, 1]], dtype=np.uint8)

aruco_dict.bytesList[0] = aruco.Dictionary_getByteListFromBits(mybits)
# print(len(aruco_dict.bytesList))
aruco_dict.bytesList[1] = aruco.Dictionary_getByteListFromBits(mybits2)
aruco_dict.bytesList[2] = aruco.Dictionary_getByteListFromBits(mybits3)
aruco_dict.bytesList[3] = aruco.Dictionary_getByteListFromBits(mybits4)

threshold = 100
# img = cv2.imread('img.png', 0)
img = None




capture = cv2.VideoCapture(1)

capture.set(3,1920)
capture.set(4,1080)

for i in range(30):
    capture.read()

ret, img = capture.read()
capture.release()
cv2.destroyAllWindows()
#img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

#plt.imshow(img)


th2 = cv2.adaptiveThreshold(img, 1, cv2.ADAPTIVE_THRESH_MEAN_C, cv2.THRESH_BINARY, 11, 3)

ret, th1 = cv2.threshold(img, 127, 255, cv2.THRESH_BINARY)

kernel = np.ones((2, 3), np.uint8)
opening = cv2.morphologyEx(th1, cv2.MORPH_OPEN, kernel)
#opening = img
plt.imshow(opening)
plt.show()

parameters = aruco.DetectorParameters_create()
corners, ids, rejectedImgPoints = aruco.detectMarkers(
    opening, aruco_dict, parameters=parameters)

#print(corners, ids, rejectedImgPoints)
print(corners)

pic = Image.new("RGB", (1500, 1500), (255, 255, 255))



minimal = [99999999, 99999999, 9999999, 9999999]
minp = [0, 0, 0, 0]
sax = [0, 0, 0, 0, 0]
say = [0, 0, 0, 0, 0]
for i in range(0, 4):
    sx = 0
    sy = 0
    for j in range(0, 4):
        sx += corners[i][0][j][0]
        sy += corners[i][0][j][1]
    sx /= 4
    sy /= 4
    sax[i] = sx
    say[i] = sy

srx = 0
sry = 0

for i in range(0, 4):
    drawpoint(sax[i], say[i], (0, 255, 0), pic)
    srx += sax[i]
    sry += say[i]

srx /= 4
sry /= 4
drawpoint(srx, sry, (0, 255, 0), pic)

for i in range(0, 4):
    for j in range(0, 4):
        drawpoint(corners[i][0][j][0], corners[i][0][j][1], (255, 0, 0), pic)

for i in range(0, 4):
    for j in range(0, 4):
        dist = ((corners[i][0][j][0] - srx) ** 2 + (corners[i][0][j][1] - sry) ** 2)
        if dist < minimal[i]:
            minimal[i] = dist
            minp[i] = j

for i in range(0, 4):
    drawpoint(corners[i][0][minp[i]][0], corners[i][0][minp[i]][1], (0, 0, 255), pic)

plt.imshow(pic)
plt.show()

# Enable blending
pyglet.gl.glEnable(pyglet.gl.GL_BLEND)
pyglet.gl.glBlendFunc(pyglet.gl.GL_SRC_ALPHA, pyglet.gl.GL_ONE_MINUS_SRC_ALPHA)

# Enable transparency
pyglet.gl.glEnable(pyglet.gl.GL_ALPHA_TEST)
pyglet.gl.glAlphaFunc(pyglet.gl.GL_GREATER, .1)

x1 = srx
x2 = srx
x3 = srx
x0 = srx
y1 = sry
y2 = sry
y3 = sry
y0 = sry

window = pyglet.window.Window(1920, 1080)

@window.event
def on_draw():
    global x1, x2, x3, x0, y1, y2, y3, y0
    window.clear()

    pyglet.gl.glColor4f(0.5, 1.0, 0.2, 0.5)

    pyglet.gl.glBegin(pyglet.gl.GL_QUADS)
    pyglet.gl.glVertex3f(x0, y0, 0)
    pyglet.gl.glVertex3f(x1, y1, 0)
    pyglet.gl.glVertex3f(x3, y3, 0)
    pyglet.gl.glVertex3f(x2, y2, 0)

    pyglet.gl.glEnd()


def update(dt):
    global x1, x2, x3, x0, y1, y2, y3, y0
    x0 -= 1
    y0 -= 1
    x1 -= 1
    y1 += 1
    x3 += 1
    y3 += 1
    x2 += 1
    y2 -= 1
    on_draw()


pyglet.clock.schedule_interval(update, 1 / 50.0)
pyglet.app.run()
pyglet.clock.unschedule(update)