# Import modules
from liblas import file
import numpy as np
import scipy.spatial

if __name__=="__main__":
    '''Read LAS file and create an array to hold X, Y, Z values'''
    # Get file
    las_file = r"E:\Testing\kd-tree_testing\LE_K20_clipped.las"
    # Read file
    f = file.File(las_file, mode='r')
    # Get number of points from header
    num_points = int(f.__len__())
    # Create empty numpy array
    PointsXYZIC = np.empty(shape=(num_points, 5))
    # Load all LAS points into numpy array
    counter = 0
    for p in f:
        newrow = [p.x, p.y, p.z, p.intensity, p.classification]
        PointsXYZIC[counter] = newrow
        counter += 1

    '''Filter array to include classes 1 and 2'''
    # the values to filter against
    unclassified = 1
    ground = 2
    # Create an array of booleans
    filter_array = np.any([PointsXYZIC[:, 4] == 1, PointsXYZIC[:, 4] == 2], axis=0)
    # Use the booleans to index the original array
    filtered_rows = PointsXYZIC[filter_array]

    '''Create a KD tree structure and segment the point cloud'''
    tree = scipy.spatial.cKDTree(filtered_rows, leafsize=10)

    '''For each point in the point cloud use the KD tree to identify nearest neighbors,
       with a K radius'''
    k = 5 #meters
    for pntIndex in range(len(filtered_rows)):
        neighbor_list = tree.query_ball_point(filtered_rows[pntIndex], k)
        zList = []
        for neighbor in neighbor_list:
            neighbor_z = filtered_rows[neighbor, 2]
            zList.append(neighbor_z)

filter_indices = [1,3,5]
print numpy.array([11,13,155,22,0xff,32,56,88])[filter_indices]