
import open3d
import numpy as np
from datetime import datetime

import elements as el
from ransac import RanSac

_wallWidth = 5   # width of the wall to be checked for inliners
_radiusTolerance = 0.03  # translates to % difference in radius, which will be accepted
_maxbox = 225

start_time2 = datetime.now()

pointcloud = open3d.io.read_point_cloud("test_sphere - Cloud.pcd")
#open3d.visualization.draw_geometries([pointcloud])
print("podvzorkovani")
pcldownsampled = pointcloud.voxel_down_sample(voxel_size=0.05)
print("Radius oulier removal")
cl, ind = pcldownsampled.remove_radius_outlier(nb_points=16, radius=5)
pclSampleFirst = pcldownsampled.select_down_sample(ind)
print(pclSampleFirst)
print("Statistic oulier removal")
cl2, ind = pclSampleFirst.remove_statistical_outlier(nb_neighbors=20,
                                                        std_ratio=2.0)
pclSampled = pclSampleFirst.select_down_sample(ind)
print(pclSampled)

sphere = RanSac.findSphere(pclSampled, 60.5, _radiusTolerance, _wallWidth)
try:
    mesh_sphere = open3d.geometry.TriangleMesh.create_sphere(radius=el.Sphere.radius3(sphere))
    mesh_sphere.translate(el.Point.to3D(el.Sphere.center2(sphere)))
    mesh_sphere.paint_uniform_color([1, 0, 0])
    time_elapsed2 = datetime.now() - start_time2
    print('Whole time elapsed {}'.format(time_elapsed2))
    open3d.visualization.draw_geometries([pointcloud, mesh_sphere])
except:
    print("Not found")