#!/usr/bin/env python3

'''
rpslam.py : BreezySLAM Python with SLAMTECH RP A1 Lidar
                 
Copyright (C) 2016 Simon D. Levy

This code is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as 
published by the Free Software Foundation, either version 3 of the 
License, or (at your option) any later version.

This code is distributed in the hope that it will be useful,     
but WITHOUT ANY WARRANTY without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU Lesser General Public License 
along with this code.  If not, see <http://www.gnu.org/licenses/>.
'''

MAP_SIZE_PIXELS         = 500
MAP_SIZE_METERS         = 10
LIDAR_DEVICE            = '/dev/ttyUSB0'

from breezyslam.algorithms import RMHC_SLAM
#from breezyslam.sensors import XVLidar as LaserModel

from rplidar import RPLidar as Lidar

from pltslamshow import SlamShow

if __name__ == '__main__':

    # Connect to Lidar unit
    lidar = Lidar(LIDAR_DEVICE)

    # Create an RMHC SLAM object with a laser model and optional robot model
    #slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)

    # Set up a SLAM display
    #display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM')

    # Initialize an empty trajectory
    trajectory = []

    # Initialize empty map
    mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)

    # Create an iterator to collect data from the RPLidar
    iterator = lidar.iter_scans()

    while True:

        try:
            print(next(iterator))

        except KeyboardInterrupt:
            break


        # Update SLAM with current Lidar scan, using first element of (scan, quality) pairs
        #slam.update([pair[0] for pair in lidar.getScan()])

        # Get current robot position
        #x, y, theta = slam.getpos()

        # Get current map bytes as grayscale
        #slam.getmap(mapbytes)

        #display.displayMap(mapbytes)

        #display.setPose(x, y, theta)

        # Exit on ESCape
        #key = display.refresh()
        #if key != None and (key&0x1A):
        #    exit(0)

    lidar.stop()
    lidar.disconnect()