#!/usr/bin/python
import time,hal
import serial
try:
    port = '/dev/ttyUSB0'
    ser = serial.Serial(
    port,
    baudrate=9600,
    parity=serial.PARITY_NONE,
    stopbits=serial.STOPBITS_ONE,
    bytesize=serial.EIGHTBITS
)
    ser.open()
    ser.isOpen()
except:
    print "ERROR : sx_drive - No serial interface found at %s"% port
    raise SystemExit

# disable limits and set default speed/direction
ser.write('LD3\n')
ser.write('MC\n')
ser.write('MR11\n')# 25000 steps/rev
ser.write('V0\n')
ser.write('H-\n')
ser.write('A25\nAD25\n')

# set up HAL pins
h = hal.component("sx_drive")
h.newpin("enable", hal.HAL_BIT, hal.HAL_IN)
h.newpin("debug", hal.HAL_BIT, hal.HAL_IN)
h.newpin("position-mode", hal.HAL_BIT, hal.HAL_IN)
h.newpin("up-to-speed", hal.HAL_BIT, hal.HAL_OUT)
h.newpin("velocity", hal.HAL_FLOAT, hal.HAL_IN)
h.newpin("position", hal.HAL_S32, hal.HAL_IN)
h.newpin("acceleration", hal.HAL_FLOAT, hal.HAL_IN)

h.newpin("resolution", hal.HAL_FLOAT, hal.HAL_IN)
h.newpin("position-scale", hal.HAL_FLOAT, hal.HAL_IN)
h.ready()
enable = direction = running = False;
position_mode = False
scale = position = velocity =  0.0; out = ''
accel = 25
ready_time = 0.0

try:
    while 1:
        if h['position-scale'] > 0: scale = h['position-scale']
        else: scale = 25000
        if not h['enable'] == enable:
            if h['enable']:
                ser.write('ON\n')
            else:
                ser.write('OFF\n')
                running = False

        if not h['position-mode'] == position_mode:
            if h['position-mode']:
                print "Position mode"
                ser.write('S\nMN\nPZ\n')
                ser.write('1PR\n')
            else:
                print "continuous Mode"
                ser.write('S\nMC\n')
                velocity = running = 0
            position_mode = h['position-mode']

        if  h['position-mode']:
            # Absolute position mode
            if not h['position'] == position:
                if not h['velocity']:
                    continue
                ser.write('D%s\n'% (int(h['position']*(25000/scale)) ) )
                ser.write('1PR\n')
                ser.write('V%s\n'%(abs(h['velocity']) ) )
                ser.write('G\n')
                ser.write('1PR\n')
                position = h['position']
        else:
            # continuous rotation mode
            if not h['velocity'] == velocity or (not h['enable'] == enable and h['enable']):
                if h['velocity'] < 0 and direction:
                    ser.write('S\nH-\n')
                    direction = running = False
                elif h['velocity'] > 0 and not direction:
                    ser.write('S\n')
                    ser.write('H+\n')
                    direction = True
                    running = False
                if not h['acceleration'] == accel and not h['acceleration'] == 0 and not running:
                    accel = h['acceleration']
                    ser.write('A%f\nAD%f\n'%(accel,accel) )
                if running: command = "IV"
                else: command = "V"
                ser.write('%s%s\n'%(command,abs(h['velocity']) ) )
                delta_speed = abs(0 - abs(h['velocity']) - abs(velocity))
                if h['velocity'] < 0 and velocity > 0 or h['velocity'] > 0 and velocity > 0:
                    delta_speed = delta_speed * 2
                ready_time = time.time() + delta_speed / accel
                print ready_time
                if h['velocity'] == 0:
                    ser.write('S\n')
                    running = False
                elif not running and h['enable']: 
                    ser.write('G\n')
                    running = True
    
        enable = h['enable']
        velocity = h['velocity']
        if time.time() > ready_time: h['up-to-speed'] = True
        else:h['up-to-speed'] = False
        time.sleep(.1)
        if h['debug']:
            while ser.inWaiting() > 0:
                out += ser.read(1)
            if out != '':
                print out
                out = ''
            
except KeyboardInterrupt:
    raise SystemExit