from PIL import Image
import numpy as np
import cv2
import cv2.aruco as aruco
import matplotlib.pyplot as plt
import PIL
import pyglet


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

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([[1, 1, 1, 0], [1, 0, 0, 1], [0, 1, 0, 1], [1, 0, 1, 1]], dtype = np.uint8)
mybits2 = np.array([[1, 1, 1, 1], [1, 0, 0, 1], [0, 1, 0, 1], [1, 0, 1, 0]], dtype = np.uint8)
mybits3 = np.array([[0, 0, 1, 0], [0, 1, 0, 0], [0, 1, 1, 0], [1, 0, 1, 0]], dtype = np.uint8)
mybits4 = np.array([[0, 0, 0, 0], [0, 1, 0, 0], [1, 1, 1, 1], [0, 0, 0, 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
cap = cv2.VideoCapture(0)

while True:
    ret, img = cap.read()
    cv2.imshow("camera", img)
    if cv2.waitKey(10) == 27: # Клавиша Esc
        break
cap.release()
cv2.destroyAllWindows()

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)
plt.imshow(opening)

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)



# 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.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)