from handrecogniser import HandRecogniser
import numpy as np
import cv2

class SimpleHandRecogniser(HandRecogniser):
    def __init__(self, image_path = ""):
        self._image_path = image_path

    def load_image(self, image_path = None):
        if image_path is not None:
            self._image_path = image_path
        self.img = cv2.imread(self._image_path, 1)
        self.img = cv2.resize(self.img, None, fx=0.1, fy=0.1, interpolation = cv2.INTER_AREA)
    
    def __calc_pos_y(self, x, radius, centre):
        """
        Calculates the position of y on a given circle radius and centre, given coordinate x.
        """
        return int((radius**2 - (x - centre[0])**2)**(1/2) + centre[1])

    def __segment_image(self):
        """
        Segments the hand from the rest of the image to get a threshold.
        """
        # Need to shift red pixels so they can be 0-20 rather than 250-~20
        self.img_hsv[:,:,0] = self.img_hsv[:,:,0] + 30
        self.img_hsv[:,:,0] = np.where(self.img_hsv[:,:,0] > 179, self.img_hsv[:,:,0] - 179, self.img_hsv[:,:,0])

        self.img_hsv = cv2.GaussianBlur(self.img_hsv,(5,5),0)

        lower_skin = (0, 0, 153)
        upper_skin = (50, 153, 255)

        # Only need mask, as we can just use this to do the hand segmentation.
        self.mask = cv2.inRange(self.img_hsv, lower_skin, upper_skin)

        ret, self.mask = cv2.threshold(self.mask, 50, 255, cv2.THRESH_BINARY)

    def __denoise(self, image):
        """
        Applies a 5x5 gaussian blur to remove noise from the image.
        """
        image = cv2.GaussianBlur(image,(5,5),0)
        
    def __calc_circle(self, image):
        """
        Calculates the equation of the circle (radius, centre) from a given
        threshold image.
        """
        k = np.sum(self.mask) / 255

        # Taking indices for num of rows.
        x_ind = np.arange(0,self.mask.shape[1])
        y_ind = np.arange(0,self.mask.shape[0])
        coords_x = np.zeros((self.mask.shape[0], self.mask.shape[1]))
        coords_y = np.zeros((self.mask.shape[0], self.mask.shape[1]))
        coords_x[:,:] = x_ind

        # Even this is extremely quick as it goes through rows in the numpy array, which in python is much faster than columns
        for element in y_ind:
            coords_y[element,:] = element
            
        # Now need to get the average x value and y value for centre of gravity
        centre = (int(np.sum(coords_x[self.mask == 255])/k), int(np.sum(coords_y[self.mask == 255])/k))

        # Calculate radius of circle:
        # May need to calculate diameter as well.
        # Just take min/max x values and y values 
        x_min = np.min(coords_x[self.mask == 255])
        x_max = np.max(coords_x[self.mask == 255])
        y_min = np.min(coords_y[self.mask == 255])
        y_max = np.max(coords_y[self.mask == 255])

        candidate_pts = [(x_min, y_min), (x_min, y_max), (x_max, y_min), (x_max, y_max)]
        radius = 0

        # Check with each point to see which is furthest from the centre.
        for pt in candidate_pts:
            # Calculate Euclydian Distance
            new_distance = ((pt[0] - centre[0])**2 + (pt[1] - centre[1])**2)**(1/2)
            if new_distance > radius:
                radius = new_distance

        radius = int(radius * 0.55)

        return radius, centre

    def get_gesture(self):
        """
        Calculates the actual gesture.
        """
        if not self.img:
            return 0
        
        self.img_hsv = cv2.cvtColor(self.img, cv2.COLOR_BGR2HSV)
        self.__segment_image()

        radius, centre = self.__calc_circle(self.mask)

        # Now go around the circle to calculate num of times going 0->255 or vice-versa.
        # First just do it the naive way with loops.
        # Equation of the circle:
        # y = sqrt(r2 - (x-c)2) + c
        # This is extremely slow, need to speed it up by removing for loop.
        # Brings speed down to 20 fps.
        # Could try a kerel method? 
        # Also can try contour detection.
        prev_x = centre[0] - radius
        prev_y = [self.__calc_pos_y(centre[0] - radius, radius, centre), self.__calc_pos_y(centre[0] - radius, radius, centre)]
        num_change = 0
        for x in range(centre[0] - radius + 1, centre[0] + radius):
            ypos = self.__calc_pos_y(x, radius, centre)
            y = [ypos, centre[1] - (ypos-centre[1])]
            print(y)
            if(self.mask[y[0], x] != self.mask[prev_y[0], prev_x]):
                num_change += 1
            if self.mask[y[1], x] != self.mask[prev_y[1], prev_x] and y[0] != y[1]:
                num_change += 1
            prev_x = x
            prev_y = y

        return num_change / 2 - 1