# -*- coding: utf-8 -*-
"""
Created on Wed Jan  5 21:11:28 2022

@author: 04566
"""


import socket
import numpy as np
import numba as nb
import open3d as o3d
import time

@nb.jit()
def ones_rslidar(x,udp_socket):
    num=x
    temp_data=np.zeros(100)
    temp_data=temp_data[np.newaxis,:]
    for k in range(int(num*10)):
        (recv_data,addr) = udp_socket.recvfrom(1290)
        temp=np.array(list(recv_data))
        temp_data=np.vstack((temp_data,temp[42:1242].reshape(12,100)))
    alpha=np.add(temp_data[:,2]*256,temp_data[:,3])/100*3.1416/180
    dises=np.zeros([int(temp_data.shape[0]),16])
    intenses=np.zeros([int(temp_data.shape[0]),16])
    for i in range(16):
        dises[:,i]=(temp_data[:,i*3+4]*256+temp_data[:,i*3+5])/100
        intenses[:,i]=temp_data[:,i*3+6]
    return (dises,intenses,alpha)


@nb.jit()
def dis(dis_alpha,dis_sphere_angle,dis_dises):
    x=np.sin(dis_alpha)@np.cos(dis_sphere_angle)
    x=np.multiply(x,dis_dises)
    y=np.cos(dis_alpha)@np.cos(dis_sphere_angle)
    y=np.multiply(y,dis_dises)
    z=np.ones_like(x)*np.sin(dis_sphere_angle)
    z=np.multiply(z,dis_dises)
    x=x.reshape(-1,1)
    y=y.reshape(-1,1)
    z=z.reshape(-1,1)
    points=np.hstack((x,y,z))
    return points


vis = o3d.visualization.Visualizer()
vis.create_window( window_name='Open3D', width=960, height=540)
render_option: o3d.visualization.RenderOption = vis.get_render_option()	#设置点云渲染参数
render_option.point_size = 1.0	#设置渲染点的大小
point_cloud = o3d.geometry.PointCloud()
vis.add_geometry(point_cloud)
udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 
udp_socket.bind(('192.168.1.102', 6699))
#channel=np.arange(8)
#sphere_angle=(channel*2-15)*3.1416/180
#sphere_angle=np.hstack((sphere_angle,(15-channel*2)*3.1416/180))
sphere_angle=np.array([-14.9927,-12.9868,-10.9859,-8.9785,\
                       -6.9945,-5.0006,-2.9838,-0.9918,\
                       15.0061,12.9936,11.0066,9.0065,\
                       7.0016,5.0006,2.9981,1.0026])
sphere_angle=sphere_angle*3.1416/180
sphere_angle=sphere_angle[np.newaxis,:]
to_reset = True
while True:
    thistime=time.time()
    (dises,intenses,alpha)=ones_rslidar(10,udp_socket)
    alpha=alpha[:,np.newaxis]
    dises=np.where(dises>600,0,dises)
    points=dis(alpha[1:],sphere_angle,dises[1:])
    point_cloud.points = o3d.utility.Vector3dVector(points)
    vis.update_geometry(point_cloud)
    if to_reset:
        vis.reset_view_point(True)
        to_reset = False
    vis.poll_events()
    vis.update_renderer()
    #time.sleep(0.1)
    print(time.time()-thistime)
udp_socket.close()
vis.destroy_window()