import serial
import struct
import time
import numpy as np

class Encoders(object):
    def __init__(self, port='/dev/ttyUSB0', polenum =[194, 256], interp=[8192, 8192]):
        self.serial = serial.Serial(port=port, baudrate=9600, bytesize=8, parity='N', stopbits=1, timeout=None, xonxoff=False, rtscts=False, write_timeout=None, dsrdtr=False, inter_byte_timeout=None, exclusive=None)
        self.polenum = np.array(polenum)
        self.interp = np.array(interp)
        self.resolution = 360. / (self.polenum * self.interp)
        
    def poll(self, wait=0, raw=False):
        time.sleep(wait)
        self.serial.write('R')
        res = self.serial.read(8)
        if raw:
            return res
        res = np.array(struct.unpack('>ii', res))
        return res * self.resolution
    
    def get_max_speed(self, freq):
        ''' Return maximal angular speed in degree per second for a given frequency in Hertz
        '''
        return freq * self.resolution

    def get_resolution(self):
        ''' Return angular resolution in arcsec
        '''
        return self.resolution * 3600
    
    def polls(self, n=100, wait=0.1):
        return np.array([self.poll(wait=wait) for _i in range(n)])

if __name__ == '__main__':
    e = Encoders()
    time.sleep(10)
    while True:
        time.sleep(1)
        print(('%f %f, %f' % ((time.time(),) + tuple(e.poll()))))