# Author: Youxing Wang
import sys
import numpy as np
import matplotlib.pyplot as plt
import cv2

def main():
    """Implement a robotic depth perception system for human avoidance.
       Input: depth image
       Output: safer side for robot to pass and the clearance.
    """

    ## Import data
    image = np.loadtxt(str(sys.argv[1]))

    ## Filter out the outliers and get the depth
    depth = image/2
    depth[depth > 5] = 5

    ## "background" is generated by stitching image0 and image2 together to
    ## cut off the human in the image
    background = np.loadtxt('background.txt')
    human = depth - background

    ## Filter the noise points and generate the mask of human
    kernel = np.ones((2,2), np.uint8)
    opening = cv2.morphologyEx(human, cv2.MORPH_OPEN, kernel)
    human_mask = cv2.erode(opening, kernel, iterations = 1)
    human_mask[human_mask > -2.1] = 0
    human_mask[human_mask < -3.3] = 0
    human_mask[human_mask < 0] = 1

    ## Use sliding window to detect human in the mask to get rid of the rest
    ## of the noise points
    filtered = np.zeros((58, 152))
    window_x = 75
    window_y = 25
    for x in range (human_mask.shape[0] - window_x):
        for y in range(human_mask.shape[1] - window_y):
            filtered[x][y] = np.sum(human_mask[x:x+window_x, y:y+window_y])

    ## From the mask of human and the depth image, calculating the depth of
    ## human
    human_x = np.where(filtered == np.max(filtered))[0][0]
    human_y = np.where(filtered == np.max(filtered))[1][0]
    human_depth_store = []
    for i in range(human_x, human_x + window_x):
        for j in range(human_y, human_y + window_y):
            if human_mask[i][j] == 1:
                human_depth_store.append(depth[i][j])
    human_depth = np.average(human_depth_store)

    ## Decide the safer side and calculate the clearance
    store = np.where((depth >= human_depth - 0.005) & (depth <= human_depth + 0.005))
    corridor_left = np.min(store[1])
    corridor_right = np.max(store[1])
    left = np.abs(human_y - corridor_left)
    right = np.abs(corridor_right - (human_y + window_y))
    corridor = corridor_right - corridor_left
    if left > right:
        distance = (1.5/corridor) * left
        print("left %.3f" % distance)
    else:
        distance = (1.5/corridor) * right
        print("right %.3f" % distance)

if __name__ == "__main__":
    main()
