import numpy as np
import matplotlib.pyplot as plt
from scipy import ndimage as ndi
import math
from skimage import feature
from skimage import io
from skimage.filters import roberts, sobel, scharr, prewitt
from matplotlib import pyplot as plt
from skimage import data
from skimage.feature import corner_harris, corner_subpix, corner_peaks
from skimage.transform import warp, AffineTransform
from skimage.draw import ellipse
from skimage.draw import circle
from skimage.draw import line
from matplotlib import cm
from skimage.transform import (hough_line, hough_line_peaks,
                               probabilistic_hough_line)
from numpy import ones,vstack
from numpy.linalg import lstsq


def line_form(point1,point2):
    points = [point1,point2]
    x_coords, y_coords = zip(*points)
    A = vstack([x_coords,ones(len(x_coords))]).T
    m, c = lstsq(A, y_coords)[0]
    return m,c

line_form((0,1),(0,2))

def is_line(point1, point2, matrix, r=5):
    a, b = line_form(point1, point2)
    start, end = point1, point2
    if (point2[0] == point1[0] and point2[1] < point1[1]) or (point2[0] < point1[0]):
        start, end = end, start
    current = start
    while current[0] < end[0] or current[1] < end[1]:
        pass

image = io.imread("C:/Users/Wojtek/PycharmProjects/paper/set6/21.png")
edges = scharr(image)


coords = corner_peaks(corner_harris(edges), min_distance=4)
coords_subpix = corner_subpix(edges, coords, window_size=15)

print(coords)

for coord1 in range(len(coords)):
    rr, cc = circle(coords[coord1][0], coords[coord1][1], 5)
    edges[rr, cc] = 0.5
    #for coord2 in range(coord1+1, len(coords)):
        #image[rr, cc] = 127
        #rr, cc = line(coords[coord1][0],  coords[coord1][1], coords[coord2][0], coords[coord2][1])
        #image[rr, cc] = 25
        #edges[rr, cc] = 0.5



fig, ax = plt.subplots()
ax.imshow(edges, interpolation='nearest', cmap=plt.cm.gray)
ax.plot(coords[:, 1], coords[:, 0], '.b', markersize=3)
ax.plot(coords_subpix[:, 1], coords_subpix[:, 0], '+r', markersize=15)



plt.show()


# Classic straight-line Hough transform
h, theta, d = hough_line(edges)

# Generating figure 1
fig2, axes = plt.subplots(1, 3, figsize=(15, 6))
ax = axes.ravel()
ax[2].imshow(edges, cmap=cm.gray)
for _, angle, dist in zip(*hough_line_peaks(h, theta, d)):
    y0 = (dist - 0 * np.cos(angle)) / np.sin(angle)
    y1 = (dist - edges.shape[1] * np.cos(angle)) / np.sin(angle)
    ax[2].plot((0, edges.shape[1]), (y0, y1), '-r')
ax[2].set_xlim((0, edges.shape[1]))
ax[2].set_ylim((edges.shape[0], 0))
ax[2].set_axis_off()
ax[2].set_title('Detected lines')
plt.show()