import cv
from math import floor, sqrt, ceil
from numpy import array, dot, subtract, add, linalg as lin

mask_size = 9 # to have a 9 X 9 window for masking the image 
mask_size2 = mask_size / 2


def distance(centre, point): # to find the distance between the centre of mask
                                #and last point of the mask 
    dist = sqrt(
            ((centre[0]-point[0])**2) +
            ((centre[1]-point[1])**2) +
            ((centre[2]-point[2])**2)
            )
    return dist

def CalcCentre(points): # Calculates centre for a given set of points
    centre = array([0,0,0])
    count = 0
    for p in points:
        centre = add(centre, array(p[:3]))
        count += 1
    centre = dot(1./count, centre)
    print centre
    return centre

          
def addrow(data, points, x, y):# adds row to the mask
        ix = x + 1
        iy = y
        return ix ,iy
        addpoints(data, points, iy, ix)
   
def addcolumn(data, points, x, y):# adds column to the mask
        ix = x
        iy = y + 1
        return ix, iy
        addpoints(data, points, iy, ix)
   
def addpoints (data, points, iy, ix): # makes a list of relevant points 
    if 0 < ix < data.width and 0 < iy < data.height:
        point = data[iy, ix]
        if point != (0.0, 0.0, 0.0):
            print iy, ix 
            print point
            points.append(point)
    return points

def CreateMask(data, x, y):
    radius = 0.3
    points = []
    for dy in xrange(-mask_size2, mask_size2 + 1):
        for dx in xrange(-mask_size2, mask_size2 + 1):
            ix, iy = x + dx, y + dy
            addpoints(data, points, iy , ix )
            
# Here I am trying to create that mask for every 9X9 window i.e.
# 81 points but for finding the centre I am taking only point which are not (0, 0, 0)
# and making a list of these points using addpoints method 
            
    if len(points) > 3:
        centre = CalcCentre(points)
        point = data[iy, ix ]
        dist = distance(centre, point)
        
# Now after calculating the distance between centre and the last point in the list,
# I am trying to add rows and column to my mask,
# till the dist between centre and last point in the list becomes less than radius 
   
        while dist < radius:
            addrow(data, points, x, y)     # I want to add row and column in both direction
            addcolumn(data, points, x, y) # But still figuring out how to do it 
            
                        
    points = list(set(points))  # To remove duplicate points in the list
    #(because when I add row and column to the mask 4 corner points are added twice )
    
    print points # printing list for various masks created 
    
def AccessPixels(data):
    for y in range(0, data.height): # height of image is 3000
        for x in range(0, data.width): # width of image is 300
            CreateMask(data, x, y)
                       
if __name__ == "__main__":
    data = cv.Load("Z:/data/xyz_00000_300.yml") # You can imagine data as a matrix with 300 columns and 3000 rows
    print "data loaded"                         # which contains x,y,z values of an image 
    AccessPixels(data)