#!/usr/bin/env python
# coding: utf-8

# # Multi-Scanner simulation with the Velodyne® VLP-16 ("Puck")
# 
# Notebook: Lukas Winiwarter, 2022
# 
# In this demo, we demonstrate how a scanner with multiple scanlines, such as the Velodyne® VLP-16, can be configured in HELIOS++. The demo contains two surveys, one for a static simulation (scanner at a single location) and one for a dynamic simulation (scanner moving between waypoints).

# In[1]:


import sys, os
from pathlib import Path
from IPython.display import Code

current_folder = globals()["_dh"][0]
helios_path = str(Path(current_folder).parent)
sys.path.append(helios_path)  # add helios-plusplus directory to PATH
import pyhelios

from pyhelios.util.xmldisplayer import display_xml, find_playback_dir


# ## Scanner
# 
# As the scanner is the main component in this demo, we investigate the **scanner** XML file `tls_scanners.xml`. Here, we find a predefined *Velodyne VLP-16*. Other scanners, such as the *Velodyne Puck LITE*, or the *Ouster OS2* can be implemented in similar ways.

# In[2]:


os.chdir(helios_path)
Code(display_xml('data/scanners_tls.xml', 'vlp16'))


# The scanner is defined with `rotating` optics, but the values for `scanFreqMin_Hz` and `scanFreqMax_Hz` (both `"0"`) show that this feature is not used in this implementation. Additionaly, the `scanAngleMax_deg` is set to a value of `1`. Subsequently, 16 `channel`s are defined, each with their individual rotations. As the scan plane is not used (default would be the y/z-Plane, see [the wiki](https://github.com/3dgeo-heidelberg/helios/wiki/Scanners)) and only a single direction per pulse is given, a simple rotation upwards or downwards about the `x` axis is sufficient. The values for the angles are taken from the [user manual of the VLP-16](https://velodynelidar.com/wp-content/uploads/2019/12/63-9243-Rev-E-VLP-16-User-Manual.pdf) (pages 54-55).
# 
# The `pulseFreq_hz` in the `<scannerSettings >` is coming from the max. measurement rate of the *VLP-16* (300,000 pts/sec). The `headRotatePerSecMax_deg` value corresponds to what Velodyne refers to as *scan frequency* - this is not to be confused with the *within-scan line* `scanFreq_hz` we use for e.g. *RIEGL*-type TLS sensors. The user manual gives a value of `1200 rpm` as a maximum rotation speed of the motor, equivalent to 20 rotations per seconds (20 Hz *scan frequency*), or a rotation speed `headRotatePerSecMax_deg` of `7200` (20 * 360deg). 
# 
# 
# Let us now use a simple scene (just containing a cube with 100 m side length, centered around the origin) to carry out two surveys:
# 
# ## Static survey

# In[3]:


Code(display_xml('data/surveys/demo/box_survey_static_puck.xml'))


# Here, we note two things:
# 1) the value of `headRotatePerSec_deg`, which here corresponds to 10 rotations per second, or 600 rotations per minute. The user manual explains that values between 300 and 1200 rpm, in increments of 60 rpm, are permissible.
# 
# 2) the range between `headRotateStart_deg` and `headRotateStop_deg`. Combined with the rotation speed, this gives the sensor's integration time. In this example, 10 full rotations (10 * 360 deg) are carried out, which takes (at a rotation speed of 10 rotations per second or 3600 deg per second) one second. We are now ready to 
# 
# ## Execute the simulation
# 

# In[4]:


get_ipython().system('"run/helios" data/surveys/demo/box_survey_static_puck.xml -q')


# ## Results
# 
# Now let's find the output files and display a 3D plot for all points recorded in the first 1/10th second (one rotation). You can see how the 'zero' location of the scanner is towards the positive Y-axis.

# In[5]:


import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
import matplotlib as mpl

output_path = find_playback_dir("data/surveys/demo/box_survey_static_puck.xml")

print("Loading points from", Path(output_path).relative_to(helios_path).as_posix())
SP1 = pd.read_csv(Path(output_path) / 'leg000_points.xyz', 
                  names="X Y Z intensity echoWidth returnNumber numberOfReturns fullwaveIndex hitObjectId class gpsTime".split(' '),
                  delimiter=' ')

SP_filtered = SP1[SP1['gpsTime'] <= min(SP1['gpsTime']) + 0.1]  # select all points recorded in the first 1/10 second

fig = plt.figure(figsize=(15,10))
ax = fig.add_subplot(projection='3d')

ax.scatter(SP_filtered['X'], SP_filtered['Y'], SP_filtered['Z'], s=0.1, c=SP_filtered['gpsTime']) 
ax.scatter([0], [0], [0], s=50, c='red') 
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')

ax.tick_params(labelsize=16)
plt.show()


# 
# ## Dynamic survey
# 
# We now want to simulate a moving scanner (e.g. a driving car). We use the same scene as before, but move the scanner from `(-40, 0, 0)` to `(40, 0, 0)` over a course of 10 seconds (`8 m/s`). Note how we have to adapt the `headRotateStop_deg` to `36000` to allow for 10 seconds of operation (at a rotation of `10` Hz) before the survey exits.

# In[6]:


Code(display_xml('data/surveys/demo/box_survey_moving_puck.xml'))


# ## Execute the simulation
# 

# In[7]:


get_ipython().system('"run/helios" data/surveys/demo/box_survey_moving_puck.xml')


# ## Results
# 
# Now let's find the output files and display a 3D plot for all points recorded in the first 1/10th second (one rotation). In a second plot, we plot the points recorded in the last 1/10th second (last rotation).

# In[8]:


import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
import matplotlib as mpl

output_path = find_playback_dir(r"data/surveys/demo/box_survey_moving_puck.xml")

print("Loading points from", Path(output_path).relative_to(helios_path).as_posix())
SP1 = pd.read_csv(Path(output_path) / 'leg000_points.xyz', 
                  names="X Y Z intensity echoWidth returnNumber numberOfReturns fullwaveIndex hitObjectId class gpsTime".split(' '),
                  delimiter=' ')

for fun in (min, max):

    SP_filtered = SP1[abs(SP1['gpsTime']- fun(SP1['gpsTime'])) <= 0.1]  # select all points recorded in the first/last 1/10 second

    fig = plt.figure(figsize=(15,10))
    ax = fig.add_subplot(projection='3d')

    ax.scatter(SP_filtered['X'], SP_filtered['Y'], SP_filtered['Z'], s=0.1, c=SP_filtered['gpsTime']) 
    ax.scatter([-40 if fun is min else 40], [0], [0], s=50, c='red') 

    ax.tick_params(labelsize=16)
    plt.title("Points recorded in the "+
             ("first " if fun is min else "last ")+
             "full rotation")
    
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')
    plt.show()


# In contrast to the static case, the 'zero' point now appears towards the positive x-Axis! What happened? The `linearpath` platform oriented the sensor such that the default axis (which is `y` for the scanners) is pointing *forward* in the direction of movement. As the platform (`simple_linearpath`) does not provide any additional rotations (e.g. in contrast to the `sr22` airborne platform), the scanner faces towards the direction of movement at the beginning of the scan.
# 
# The gaps in the 'far' corners are due to the maximum range of the scanner, given as 100 m.