import matplotlib.image as mpimg
import matplotlib.pyplot as plt
import numpy as np
from matplotlib.patches import Circle, Rectangle
from matplotlib.collections import PolyCollection

def pixel_loc_to_collection_verts(pixel_loc):
    if isinstance(pixel_loc[0], int) or isinstance(pixel_loc[0], float):
        vrts = np.zeros((1, 4, 2))
        vrts[0, 0] = np.array([pixel_loc[0], pixel_loc[1]])
        vrts[0, 1] = np.array([pixel_loc[0]+1, pixel_loc[1]])
        vrts[0, 2] = np.array([pixel_loc[0]+1, pixel_loc[1]+1])
        vrts[0, 3] = np.array([pixel_loc[0], pixel_loc[1]+1])
    else:
        num_pts = pixel_loc[0].shape[0]
        # print(num_pts)
        # print(pixel_loc)
        vrts = np.zeros((num_pts, 4, 2))
        i = 0
        for x,y in zip(pixel_loc[0],pixel_loc[1]):
            vrts[i, 0] = np.array([x, y])
            vrts[i, 1] = np.array([x+1, y])
            vrts[i, 2] = np.array([x+1, y+1])
            vrts[i, 3] = np.array([x, y+1])
            i += 1
    return vrts

def plot_correspondences(images, uv_a, uv_b, use_previous_plot=None, circ_color='g', show=True, draw_circ=False):
    radius = 2.5
    linewidth = 1.0

    if use_previous_plot is None:
        fig, axes = plt.subplots(nrows=2, ncols=2)
    else:
        fig, axes = use_previous_plot[0], use_previous_plot[1]

    fig.set_figheight(10)
    fig.set_figwidth(15)
    pixel_locs = [uv_a, uv_b, uv_a, uv_b]
    axes = axes.flat[0:]
    if use_previous_plot is not None:
        axes = [axes[1], axes[3]]
        images = [images[1], images[3]]
        pixel_locs = [pixel_locs[1], pixel_locs[3]]
    for ax, img, pixel_loc in zip(axes[0:], images, pixel_locs):
        # print(type(img))
        ax.set_aspect('equal')
        if draw_circ:
            # use circle to visualize
            if isinstance(pixel_loc[0], int) or isinstance(pixel_loc[0], float):
                circ = Circle(pixel_loc, radius=radius, facecolor=circ_color, edgecolor='white', fill=True ,linewidth = linewidth, linestyle='solid')
                ax.add_patch(circ)
            else:
                for x,y in zip(pixel_loc[0],pixel_loc[1]):
                    circ = Circle((x,y), radius=radius, facecolor=circ_color, edgecolor='white', fill=True ,linewidth = linewidth, linestyle='solid')
                    ax.add_patch(circ)
        else:
            # use rect to visualize
            vrts = pixel_loc_to_collection_verts(pixel_loc)
            coll = PolyCollection(vrts, facecolors=circ_color, edgecolor=None, linewidth = None)
            ax.add_collection(coll)
        ax.imshow(img)
    if show:
        plt.show()
        return None
    else:
        return fig, axes

def plot_correspondences_from_dir(log_dir, img_a, img_b, uv_a, uv_b, use_previous_plot=None, circ_color='g', show=True):
    img1_filename = log_dir+"/images/"+img_a+"_rgb.png"
    img2_filename = log_dir+"/images/"+img_b+"_rgb.png"
    img1_depth_filename = log_dir+"/images/"+img_a+"_depth.png"
    img2_depth_filename = log_dir+"/images/"+img_b+"_depth.png"
    images = [img1_filename, img2_filename, img1_depth_filename, img2_depth_filename]
    images = [mpimg.imread(x) for x in images]
    return plot_correspondences(images, uv_a, uv_b, use_previous_plot=use_previous_plot, circ_color=circ_color, show=show)

def plot_correspondences_direct(img_a_rgb, img_a_depth, img_b_rgb, img_b_depth, uv_a, uv_b, use_previous_plot=None, circ_color='g', show=True, draw_circ=False):
    """

    Plots rgb and depth image pair along with circles at pixel locations
    :param img_a_rgb: PIL.Image.Image
    :param img_a_depth: PIL.Image.Image
    :param img_b_rgb: PIL.Image.Image
    :param img_b_depth: PIL.Image.Image
    :param uv_a: (u,v) pixel location, or list of pixel locations
    :param uv_b: (u,v) pixel location, or list of pixel locations
    :param use_previous_plot:
    :param circ_color: str
    :param show:
    :return:
    """
    images = [img_a_rgb, img_b_rgb, img_a_depth, img_b_depth]
    return plot_correspondences(images, uv_a, uv_b, use_previous_plot=use_previous_plot, circ_color=circ_color, show=show, draw_circ=draw_circ)
    

import matplotlib.image as mpimg
import matplotlib.pyplot as plt
import numpy as np
from matplotlib.patches import Circle, Rectangle
from matplotlib.collections import PolyCollection

def pixel_loc_to_collection_verts(pixel_loc):
    if isinstance(pixel_loc[0], int) or isinstance(pixel_loc[0], float):
        vrts = np.zeros((1, 4, 2))
        vrts[0, 0] = np.array([pixel_loc[0], pixel_loc[1]])
        vrts[0, 1] = np.array([pixel_loc[0]+1, pixel_loc[1]])
        vrts[0, 2] = np.array([pixel_loc[0]+1, pixel_loc[1]+1])
        vrts[0, 3] = np.array([pixel_loc[0], pixel_loc[1]+1])
    else:
        num_pts = pixel_loc[0].shape[0]
        # print(num_pts)
        # print(pixel_loc)
        vrts = np.zeros((num_pts, 4, 2))
        i = 0
        for x,y in zip(pixel_loc[0],pixel_loc[1]):
            vrts[i, 0] = np.array([x, y])
            vrts[i, 1] = np.array([x+1, y])
            vrts[i, 2] = np.array([x+1, y+1])
            vrts[i, 3] = np.array([x, y+1])
            i += 1
    return vrts

def plot_correspondences(images, uv_a, uv_b, use_previous_plot=None, circ_color='g', show=True, draw_circ=False):
    radius = 2.5
    linewidth = 1.0

    if use_previous_plot is None:
        fig, axes = plt.subplots(nrows=2, ncols=2)
    else:
        fig, axes = use_previous_plot[0], use_previous_plot[1]

    fig.set_figheight(10)
    fig.set_figwidth(15)
    pixel_locs = [uv_a, uv_b, uv_a, uv_b]
    axes = axes.flat[0:]
    if use_previous_plot is not None:
        axes = [axes[1], axes[3]]
        images = [images[1], images[3]]
        pixel_locs = [pixel_locs[1], pixel_locs[3]]
    for ax, img, pixel_loc in zip(axes[0:], images, pixel_locs):
        # print(type(img))
        ax.set_aspect('equal')
        if draw_circ:
            # use circle to visualize
            if isinstance(pixel_loc[0], int) or isinstance(pixel_loc[0], float):
                circ = Circle(pixel_loc, radius=radius, facecolor=circ_color, edgecolor='white', fill=True ,linewidth = linewidth, linestyle='solid')
                ax.add_patch(circ)
            else:
                for x,y in zip(pixel_loc[0],pixel_loc[1]):
                    circ = Circle((x,y), radius=radius, facecolor=circ_color, edgecolor='white', fill=True ,linewidth = linewidth, linestyle='solid')
                    ax.add_patch(circ)
        else:
            # use rect to visualize
            vrts = pixel_loc_to_collection_verts(pixel_loc)
            coll = PolyCollection(vrts, facecolors=circ_color, edgecolor=None, linewidth = None)
            ax.add_collection(coll)
        ax.imshow(img)
    if show:
        plt.show()
        return None
    else:
        return fig, axes

def plot_correspondences_from_dir(log_dir, img_a, img_b, uv_a, uv_b, use_previous_plot=None, circ_color='g', show=True):
    img1_filename = log_dir+"/images/"+img_a+"_rgb.png"
    img2_filename = log_dir+"/images/"+img_b+"_rgb.png"
    img1_depth_filename = log_dir+"/images/"+img_a+"_depth.png"
    img2_depth_filename = log_dir+"/images/"+img_b+"_depth.png"
    images = [img1_filename, img2_filename, img1_depth_filename, img2_depth_filename]
    images = [mpimg.imread(x) for x in images]
    return plot_correspondences(images, uv_a, uv_b, use_previous_plot=use_previous_plot, circ_color=circ_color, show=show)

def plot_correspondences_direct(img_a_rgb, img_a_depth, img_b_rgb, img_b_depth, uv_a, uv_b, use_previous_plot=None, circ_color='g', show=True, draw_circ=False):
    """

    Plots rgb and depth image pair along with circles at pixel locations
    :param img_a_rgb: PIL.Image.Image
    :param img_a_depth: PIL.Image.Image
    :param img_b_rgb: PIL.Image.Image
    :param img_b_depth: PIL.Image.Image
    :param uv_a: (u,v) pixel location, or list of pixel locations
    :param uv_b: (u,v) pixel location, or list of pixel locations
    :param use_previous_plot:
    :param circ_color: str
    :param show:
    :return:
    """
    images = [img_a_rgb, img_b_rgb, img_a_depth, img_b_depth]
    return plot_correspondences(images, uv_a, uv_b, use_previous_plot=use_previous_plot, circ_color=circ_color, show=show, draw_circ=draw_circ)
    

# print(num_pts) # print(pixel_loc) # print(type(img)) # use circle to visualize # use rect to visualize Plots rgb and depth image pair along with circles at pixel locations     :param img_a_rgb: PIL.Image.Image     :param img_a_depth: PIL.Image.Image     :param img_b_rgb: PIL.Image.Image     :param img_b_depth: PIL.Image.Image     :param uv_a: (u,v) pixel location, or list of pixel locations     :param uv_b: (u,v) pixel location, or list of pixel locations     :param use_previous_plot:     :param circ_color: str     :param show:     :return: