import numpy as np
from scipy import sqrt
from math import acos, sqrt, pi, cos, sin, radians


def triangle_area(x, y, z):
    """
    Computes the area of a triangle represented by its three points.

    :param x: The first 2D point of the triangle.
    :type x: tuple
    :param y: The second 2D point of the triangle.
    :type y: tuple
    :param z: The third 2D point of the triangle.
    :type z: tuple
    :return: The area of the triangle.
    :rtype: float

    """
    return x[0] * (y[1] - z[1]) + y[0] * (z[1] - x[1]) + z[0] * (x[1] - y[1])


def distance(p, q):
    """
    Computes the distance between two points in a plane.

    :param p: The first point.
    :type p: tuple
    :param q: The second point.
    :type q: tuple
    :return: The distance between the two points.
    :rtype: float

    """
    return sqrt(pow(abs(p[0] - q[0]), 2) + pow(abs(p[1] - q[1]), 2))


def line_equation(l, m, j):
    """
    Perpendicular Distance of a Point J from line formed by Points L and M; Equation of the line ax+by+c=0
    Given 3 points, the function derives the line equation of the first two points,
    computes and returns the perpendicular distance of the the 3rd point from this line.

    :param l: The first point of the line.
    :type l: tuple
    :param m: The second point of the line.
    :type m: tuple
    :param j: The single point.
    :type j: tuple
    :return: The line equation.
    :rtype: float

    """
    a = -((m[1] - l[1]) / (m[0] - l[0]))
    b = 1.0
    c = (((m[1] - l[1]) / (m[0] - l[0])) * l[0]) - l[1]

    # Now that we have a, b, c from the equation ax + by + c, time to substitute (x,y) by values from the Point J
    p_dist = (a * j[0] + (b * j[1]) + c) / sqrt((a * a) + (b * b))
    return p_dist


def line_slope(l, m):
    """
    Slope of a line by two Points L and M on it; Slope of line, S = (x1 -x2) / (y1- y2)
    The function returns the slope of the line formed by given 2 points, the alignement flag
    indicates the line is vertical and the slope is infinity.

    :param l: The first point.
    :type l: tuple
    :param m: The second point.
    :type m: tuple
    :return: float, int

    """
    dx = m[0] - l[0]
    dy = m[1] - l[1]
    if dy != 0:
        return (dy / dx), 1
    else:  # Make sure we are not dividing by zero; so use 'alignment' flag
        return 0.0, 0


def cross(v1, v2):
    """
    Auxiliary function that computes the cross between two points.

    :param v1: The first point.
    :type v1: tuple
    :param v2: The second point.
    :type v2: tuple
    :return: int

    """
    return v1[0] * v2[1] - v1[1] * v2[0]


def get_intersection_point(a1, a2, b1, b2):
    """
    Returns the Intersection Point of the lines formed by sets of two points

    :param a1: The first point of the first set.
    :type a1: tuple
    :param a2: The second point of the first set.
    :type a2: tuple
    :param b1: The first point of the second set.
    :type b1: tuple
    :param b2: The second point of the second set.
    :type b2: tuple
    :returns: float

    """
    p = np.array(a1)
    q = np.array(b1)
    r = np.array(a2) - p
    s = np.array(b2) - q

    if cross(r, s) == 0:
        return np.zeros(p.shape)

    t = cross(q - p, s) / cross(r, s)

    return p + t * r


def create_line_iterator(p1, p2, img):
    """
    Produces and array that consists of the coordinates and intensities of each pixel in a line between two points

    :param p1: The first point.
    :type p1: numpy.ndarray
    :param p2: The second point.
    :type p2: numpy.ndarray
    :param img: The image being processed.
    :type img: numpy.ndarray
    :return: numpy.ndarray, numpy.ndarray

    """
    # define local variables for readability
    image_h = img.shape[0]
    image_w = img.shape[1]
    p1_x = p1[0]
    p1_y = p1[1]
    p2_x = p2[0]
    p2_y = p2[1]

    # difference and absolute difference between points
    # used to calculate slope and relative location between points
    d_x = p2_x - p1_x
    d_y = p2_y - p1_y
    d_xa = np.abs(d_x)
    d_ya = np.abs(d_y)

    # predefine numpy array for output based on distance between points
    it_buffer = np.empty(shape=(np.maximum(d_ya, d_xa), 3), dtype=np.float32)
    it_buffer.fill(np.nan)

    # Obtain coordinates along the line using a form of Bresenham's algorithm
    neg_y = p1_y > p2_y
    neg_x = p1_x > p2_x
    if p1_x == p2_x:  # vertical line segment
        it_buffer[:, 0] = p1_x
        if neg_y:
            it_buffer[:, 1] = np.arange(p1_y - 1, p1_y - d_ya - 1, -1)
        else:
            it_buffer[:, 1] = np.arange(p1_y + 1, p1_y + d_ya + 1)
    elif p1_y == p2_y:  # horizontal line segment
        it_buffer[:, 1] = p1_y
        if neg_x:
            it_buffer[:, 0] = np.arange(p1_x - 1, p1_x - d_xa - 1, -1)
        else:
            it_buffer[:, 0] = np.arange(p1_x + 1, p1_x + d_xa + 1)
    else:  # diagonal line segment
        steep_slope = d_ya > d_xa
        if steep_slope:
            slope = d_x.astype(np.float32) / d_y.astype(np.float32)
            if neg_y:
                it_buffer[:, 1] = np.arange(p1_y - 1, p1_y - d_ya - 1, -1)
            else:
                it_buffer[:, 1] = np.arange(p1_y + 1, p1_y + d_ya + 1)
            it_buffer[:, 0] = (slope * (it_buffer[:, 1] - p1_y)).astype(np.int) + p1_x
        else:
            slope = d_y.astype(np.float32) / d_x.astype(np.float32)
            if neg_x:
                it_buffer[:, 0] = np.arange(p1_x - 1, p1_x - d_xa - 1, -1)
            else:
                it_buffer[:, 0] = np.arange(p1_x + 1, p1_x + d_xa + 1)
            it_buffer[:, 1] = (slope * (it_buffer[:, 0] - p1_x)).astype(np.int) + p1_y

    # Remove points outside of image
    col_x = it_buffer[:, 0]
    col_y = it_buffer[:, 1]
    it_buffer = it_buffer[(col_x >= 0) & (col_y >= 0) & (col_x < image_w) & (col_y < image_h)]

    # Get intensities from img ndarray
    line_color = img[it_buffer[:, 1].astype(np.uint), it_buffer[:, 0].astype(np.uint)]

    return it_buffer[:, 0:2].astype(np.uint64), line_color.astype(np.uint64)


def collinear(p0, p1, p2):
    x1, y1 = p1[0] - p0[0], p1[1] - p0[1]
    x2, y2 = p2[0] - p0[0], p2[1] - p0[1]
    return x1 * y2 - x2 * y1 < 1e-12


def length(v):
    """
    Computes the length of the line.

    :param v: The point defining the line.
    :type v: list
    :return: The length of the line.
    :rtype: float

    """
    return sqrt(v[0]**2 + v[1]**2)


def rotate_point(point, angle, length):
    '''
    point - Tuple (x, y)
    angle - Angle you want your end point at in degrees.
    length - Length of the line you want to plot.

    Will return the new point.
    '''

    # unpack the first point
    x, y = point[0], point[1]

    # find the end point
    endy = length * sin(radians(angle*-1))
    endx = length * cos(radians(angle*-1))

    return np.array([round(endx+x), round(endy+y)], np.int32)


def rot90_point(point, k, shape):
    """

    :param point:
    :param k:
    :param shape:
    :return:
    """
    image_h, image_w = shape
    x, y = point
    for ii in range(k):
        x, y = -y, x

    print(x, y)
    return np.array((x, y), dtype=int) + np.array((image_w - 1 if k == 1 or k == 2 else 0,
                                                   image_h - 1 if k == 2 or k == 3 else 0), dtype=int)


def corner_scan(image_threshold, corner=0):

    assert isinstance(image_threshold, np.ndarray)
    assert len(image_threshold.shape) == 2
    assert 0 <= corner < 4

    image_threshold = np.rot90(image_threshold, k=corner)

    image_h, image_w = image_threshold.shape
    index = 2
    while index < image_h and index < image_w:
        p1 = np.ndarray(shape=(2,), buffer=np.array([index, 0]), dtype=int)
        p2 = np.ndarray(shape=(2,), buffer=np.array([0, index]), dtype=int)
        points, line = create_line_iterator(p1, p2, image_threshold)
        i = 0
        while i < len(line)-2:
            if line[i] == line[i + 1] == line[i + 2] == 0:
                point = np.array(points[i + 1], dtype=np.int64)
                rotated_point = rot90_point(point, k=corner, shape=image_threshold.shape)
                return rotated_point  # we append the middle point to the list of found points
            i += 1
        index += 1


def adjust_corners(image, corner, center, rad):
    """
    Adjusts the previously found corner to a one more distant to the center of the image. If the previously found
    is the farthest one, it will be returned as well.

    :param image: The Black/White image.
    :type image: numpy.ndarray
    :param corner: The previously found corner.
    :type corner: numpy.ndarray
    :param center: The center of the image.
    :type center: numpy.ndarray
    :param rad: The radious in which the comparison will be done. (For example, if it is set to 5, this method will
    only check an area fo radious 5, taking the `corner` as center.
    :type rad: int
    :return: The new corner, which will be a farthest one from the center (if it exists) or the same as before (if
    there are none that are farthest).
    :rtype numpy.ndarray

    """

    # import matplotlib.pyplot as plt

    corner = np.array(corner, dtype=int)
    sup_lim = corner+rad+1
    inf_lim = corner-rad

    crop = image[inf_lim[1]:sup_lim[1], inf_lim[0]:sup_lim[0]]

    # plt.figure()
    # plt.imshow(crop, interpolation="none")

    xx, yy = np.meshgrid(np.arange(1+rad*2), np.arange(1+rad*2))
    xy = np.swapaxes(np.array([xx, yy]), 0, 2)[:, :, ::-1]

    crop_indices = inf_lim + xy

    distances = np.linalg.norm(crop_indices.astype(np.float) - center.astype(np.float), axis=2)
    distances = distances/distances.max()

    distances[crop == 255] = 0

    # plt.figure()
    # plt.imshow(distances, interpolation="none")

    maximum_point = np.unravel_index(distances.argmax(), distances.shape)

    # TODO fix [::-1] patch!!!
    maximum_point = np.array(maximum_point)[::-1] + inf_lim

    print(maximum_point)

    return maximum_point


def line_attachment(image, point, axis="y"):
    """
    Performs the line attachment procedure using the `point` argument as fixed point and finds the p.

    :param axis:
    :param point: The fixed point to be used.
    :type point: numpy.ndarray
    :return: The first point in which the percentage of black points is inferior to the threshold value.
    :rtype: numpy.ndarray

    """

    # # Obtaining canny image from image_processor:
    # image = self.image_processor.thresholds.get("canny")

    # In case "x" border of QR code wants to be found, swap axes, due to symmetry.
    if axis == "x":
        image = np.swapaxes(image, 0, 1)
        point = point[::-1]

    image_float = image.astype(np.float) / 255

    # Extract geometry and point:
    image_y_max, image_x_max = image.shape
    point_a = np.array(point)

    # Define the extreme points of the angular sweep:
    point_0 = np.array([0, point_a[1]])
    point_180 = np.array([image_x_max - 1, point_a[1]])

    # Some magic numpy to create all the points of the sweep:
    left = np.vstack(
        (np.zeros(image_y_max - point_0[1] - 1),
         np.arange(point_0[1], image_y_max - 1)
         )
    )
    left = np.swapaxes(left, 0, 1).astype(np.uint64)

    bottom = np.vstack(
        (np.arange(0, image_x_max),
         image_y_max - 1 + np.zeros(image_x_max)
         )
    )
    bottom = np.swapaxes(bottom, 0, 1).astype(np.uint64)

    right = np.vstack(
        (point_180[0] + np.zeros(image_y_max - point_0[1] - 1),
         np.arange(point_0[1], image_y_max - 1)[::-1],
         )
    )
    right = np.swapaxes(right, 0, 1).astype(np.uint64)

    # Stack all 3 direction of the search:
    search_points = np.vstack((left, bottom, right)).astype(np.uint64)

    # Compute all vectors and angles for each point probed:
    vector_a = point_0 - point_a
    vectors_b = search_points - point_a

    vectors_cosines = np.dot(vectors_b, vector_a) / np.linalg.norm(vector_a) / np.linalg.norm(vectors_b, axis=1)
    lines_angles = np.arccos(np.clip(vectors_cosines, -1, 1))*180/np.pi

    # Define empty list to save found values:
    lines_pixels = []
    lines_indices = []

    for ii, point_b in enumerate(search_points):

        # vector_b = point_b - point_a
        # vector_cosines.append(np.dot(vector_b, vector_a) / np.linalg.norm(vector_a) / np.linalg.norm(vector_b))

        line_indices, values = create_line_iterator(point_a, point_b, image_float)
        lines_indices.append(line_indices)
        lines_pixels.append(values)

    lines_length = np.array([len(line) for line in lines_pixels])
    lines_border_counts = np.array([np.array(line).sum() for line in lines_pixels])

    # Compute density with numpy magic:
    density_of_borders = lines_border_counts / lines_length
    density_of_borders = density_of_borders - density_of_borders.min()
    density_of_borders = density_of_borders / density_of_borders.max()

    # To avoid detect the 0º maxim:
    density_of_borders[lines_angles < 30] = 0
    density_of_borders[lines_angles > 150] = 0

    max_index = density_of_borders.argmax()
    point_attachment = search_points[max_index]

    # In case "x" border of QR code wants to be found, swap axes, due to symmetry.
    if axis == "x":
        point_attachment = point_attachment[::-1]

    return point_attachment, (density_of_borders, lines_angles)
