# !/usr/bin/python
#
# Example code to go through the hokuyo_4m.bin file, read timestamps and the hits
# in each packet, and plot them.
#
# To call:
#
#   python read_hokuyo_4m.py hokuyo_4m.bin
#

import sys
import struct
import numpy as np
import matplotlib.pyplot as plt

def convert(x_s):

    scaling = 0.005 # 5 mm
    offset = -100.0

    x = x_s * scaling + offset

    return x

def main(args):

    if len(sys.argv) < 2:
        print "Please specifiy input bin file"
        return 1

    # hokuyo_4 always has 726 hits
    num_hits = 726

    # angles for each range observation
    rad0 = -2.0862138
    radstep = 0.0061359233
    angles = np.linspace(rad0, rad0 + (num_hits-1)*radstep, num_hits)

    f_bin = open(sys.argv[1], "r")

    plt.ion()

    while True:

        # Read timestamp
        utime = struct.unpack('<Q', f_bin.read(8))[0]

        print 'Timestamp', utime

        r = np.zeros(num_hits)

        for i in range(num_hits):

            s = struct.unpack('<H', f_bin.read(2))[0]
            r[i] = convert(s)

            #print s

        x = r * np.cos(angles)
        y = r * np.sin(angles)

        plt.clf()
        plt.plot(x, y, '.')
        plt.title(utime)
        plt.draw()

    f_bin.close()

    return 0

if __name__ == '__main__':
    sys.exit(main(sys.argv))