import cv2
import numpy as np
import sys

SPEED = 100         # um/s
REAL_DIM = 100.0    # um
Z = 0.0             # um

np.set_printoptions(threshold=sys.maxsize)

def row(x, y, z, on, t):
    return "1\t{:.3f}\t{:.3f}\t{:.3f}\t{:.3f}\t{:d}\n".format(x, y, z, on, t)


im = cv2.imread('test_img/test_img_1.png', cv2.IMREAD_GRAYSCALE)
dim = im.shape

if (dim[0] != dim[1]):
    print("Error: image must be square")
    sys.exit()

output = ""

# Go to origin point (laser off)
output += row(0.0, 0.0, Z, 0.0, SPEED)

result_fill = np.ones(dim, np.uint8) * 255
result_borders = np.zeros(dim, np.uint8)

# the '[:-1]' is used to skip the contour at the outer border of the image
contours = cv2.findContours(im, cv2.RETR_LIST,
                            cv2.CHAIN_APPROX_SIMPLE)[0][:-1]

print(len(contours))
print(contours[-1][:10])
print(contours[0].shape)

cv2.drawContours(result_fill, contours, -1, 0, -1)
cv2.drawContours(result_borders, contours, -1, 255, 1)

for contour in contours:
    for i, point in enumerate(contour):
        point_real = point[0] / dim[0] * REAL_DIM
        laser_on = 0.0 if i == 0 else 1.0
        output += row(point_real[0], point_real[1], Z, laser_on, SPEED)
        

# xor the filled result and the borders to recreate the original image
#result = result_fill ^ result_borders
#
#cv2.imshow('image', result_borders)
#cv2.waitKey(0)

output += row(0.0, 0.0, Z, 0.0, SPEED)

with open('path.txt', 'w') as f:
    f.write(output)


# Questions
# Thickness of lines
# Stage uncertainty/increment
# Sample dimensions
# Dwell time