import sys
import cv2
import matplotlib.pyplot as plt
import time
import numpy as np
import pandas as pd
from pyntcloud import PyntCloud
from detector import ParticleDetector

search_range = 75
search_offset = 700
patch_size = 10
mm_per_pix = 0.00112
focal_length = 21/mm_per_pix # pixels
base_line = 20 # mm
baseline_offset = 0 # pixels

def get_patch(img, x, y, path_size):

    x = int(x)
    y = int(y)
    of = int(patch_size/2)

    y_min = y - of
    if y_min < 0:
        y_min = 0
    y_max = y + of
    if y_max > img.shape[0]:
        y_max = img.shape[0]

    x_min = x - of
    if x_min < 0:
        x_min = 0
    x_max = x + of
    if x_max > img.shape[1]:
        x_max = img.shape[1]

    return img[y_min:y_max, x_min:x_max]


if __name__ == "__main__":

    img_left = cv2.imread(sys.argv[1])
    pd_left = ParticleDetector(img_left)
    pd_left.detect()

    img_right = cv2.imread(sys.argv[2])
    pd_right = ParticleDetector(img_right)
    pd_right.detect()

    # simple matching using rectilinear images and feature minimum error

    right_x_vals = pd_right.particles[:, 0]
    right_y_vals = pd_right.particles[:, 1]

    cloud = []

    for particle in pd_left.particles:

        x_val = particle[0]
        y_val = particle[1]
        x_max = int(x_val-search_offset)
        x_min = int(x_val-search_offset-search_range)

        mask = np.logical_and(
            right_x_vals >= x_min,
            np.logical_and(right_x_vals < x_max, np.abs(right_y_vals-y_val) <= 5)
        )

        candidates = pd_right.particles[mask, :]

        left_patch = get_patch(img_left, x_val, y_val, patch_size)

        iml = img_left.copy()
        imr = img_right.copy()

        if candidates.shape[0] > 0:

            epi_error = []

            for c in range(candidates.shape[0]):

                r_x_val = candidates[c, 0]
                r_y_val = candidates[c, 1]

                right_patch = get_patch(img_right, r_x_val, r_y_val, patch_size)

                # scale patches to match (pixel errors)
                right_patch = cv2.resize(right_patch, (left_patch.shape[1], left_patch.shape[0]), interpolation=cv2.INTER_CUBIC)

                #cv2.imshow('left patch', left_patch)
                #cv2.imshow('right patch', right_patch)
                err = np.sum((right_patch - left_patch) ** 2)
                #print(err)
                #cv2.waitKey()
                epi_error.append(err)

            #print(candidates)
            #diff_mat = candidates[:, [1,3,4,5,6]]-np.tile(particle[[1,3,4,5,6]], (candidates.shape[0],1))
            #print(diff_mat)
            #epi_error = np.sum(abs(diff_mat), axis=1)

            #print(epi_error)
            match_ind = np.squeeze(np.argwhere(epi_error == np.min(epi_error)))

            disparity = x_val - candidates[match_ind, 0]

            depth = base_line*focal_length / (disparity + baseline_offset) / 1000 # in meters

            #print(depth)

            # save 3-point point in left-camera coordinates
            p = [(x_val-img_left.shape[1]/2)*depth/focal_length -base_line/2/1000, y_val*depth/focal_length, depth]
            print(p)
            cloud.append(p)

            #cv2.drawMarker(iml, (int(x_val), int(y_val)), (0,255,0),  cv2.MARKER_DIAMOND, 10, 2)
            #cv2.drawMarker(imr, (int(candidates[match_ind, 0]), int(candidates[match_ind, 1])), (0, 0, 255), cv2.MARKER_DIAMOND, 10, 2)
            #print([x_val, y_val, candidates[match_ind, 0:2]])

            #plt.imshow(np.hstack((iml,imr)))
            #plt.show()
            #cv2.imshow('points left', iml)
            #cv2.imshow('points right', imr)
            #cv2.waitKey()

    cloud = np.array(cloud)
    print(cloud.shape)

    ptcloud = PyntCloud(pd.DataFrame(
        # same arguments that you are passing to visualize_pcl
        data=np.array(cloud), columns=["x", "y", "z"])
    )
    ptcloud.to_file("test.ply")