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

# # Lidar Test
# 
# 11 May 2019
# 
# ydlidar data collected using gecko. `the_collector` was setup to capture data
# using `pickle` so the data is easily readable.
# 
# **Note:** this is messed up a little. The lidar is tethered by USB cables and power 
# to my desktop, so I cannot really move it nicely and I am constantly in the
# way ... but it sort of works given these *terrible* conditions.

# In[1]:


get_ipython().run_line_magic('matplotlib', 'inline')

import cv2
import numpy as np
from matplotlib import pyplot as plt 


# In[59]:


import pickle

# gecko robotics library
from pygecko.messages import lidar_st

# simple slam library based on breezy slam
from sslam import RMHC_SLAM
from sslam.sensors_models import Laser_Model


# In[69]:


get_ipython().run_line_magic('ls', '../../gecko')


# In[71]:


# fname = "../../gecko/lidar.2019-05-11-20:08:58.014623.pickle.bag"
fname = "../../gecko/lidar.2019-05-11-21:11:33.853614.pickle.bag"
with open(fname, 'rb') as fd:
    data = pickle.load(fd)


# In[96]:


class YDLIDAR_Model(Laser_Model):
    '''
    A class for
    scan_size, scan_rate_hz, detection_angle_degrees, distance_no_detection_mm, detection_margin=0, offset_mm=0
    '''
    def __init__(self, detectionMargin = 0, offsetMillimeters = 0):
        Laser_Model.__init__(self, SCAN_SIZE, 5, 360-8, 120, detectionMargin, offsetMillimeters)


# In[104]:


SCAN_SIZE = 620
MAP_SIZE_METERS = 8
MAP_SIZE_PIXELS = 500


# In[105]:


slam = RMHC_SLAM(YDLIDAR_Model(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)


# In[106]:


mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)


# In[107]:


save = []
for p in data['lidar']:
    try:
        pts = [1000*x[1] for x in p.data[:SCAN_SIZE]]
        slam.update(pts)
        save.append(slam.getpos())
    except:
        print(len(pts))
            


# In[108]:


slam.getmap(mapbytes)


# In[111]:


mapimg = np.reshape(np.frombuffer(mapbytes, dtype=np.uint8), (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))

plt.imshow(mapimg, cmap='gray')
plt.title('This really is a square room');


# In[112]:


plt.subplot(2,1,1)
plt.plot([x[0] for x in save], [x[1] for x in save])
plt.grid(True)

# the angle looks the best, I just basically turned from
# side to side trying to get different data
plt.subplot(2,1,2)
plt.plot([x[2] for x in save])
plt.grid(True)
plt.ylabel('Theta');


# In[ ]: