#!/bin/python
import Basda
import BasdaMoccaWheel
import Nice
import numpy.random as rnd
import time

srvName = "WHEEL_CTRL"
tcpStr = "localhost:10211"

svcPath = "SCI_WHEEL1.SVC"
endPoint = "[NAME="+srvName+",TCP="+tcpStr+",PATH="+svcPath+"]"
sciWheel1 = BasdaMoccaWheel.Wheel.connectWithEndpoint("W1", endPoint)

svcPath = "SCI_WHEEL2.SVC"
endPoint = "[NAME="+srvName+",TCP="+tcpStr+",PATH="+svcPath+"]"
sciWheel2 = BasdaMoccaWheel.Wheel.connectWithEndpoint("W2", endPoint)


print 'Sci Wheel 1 State: {:s}'.format(str(sciWheel1.state())) 
print 'Sci Wheel 2 State: {:s}'.format(str(sciWheel2.state())) 


for i in range(50):
    try:
            
        posW1 = rnd.random_integers(1, 8)
        posW2 = rnd.random_integers(1, 8)
        print 'State W1: {:s} | State W2: {:s}'.format(str(sciWheel1.state()), str(sciWheel2.state()))
        BasdaMoccaWheel.U9_LOG('Moving W1 to position {:d}, moving W2 to position {:d} ...'.format(posW1, posW2)) 
        sciWheel1.moveToNamedPositionStart(posW1)
        sciWheel2.moveToNamedPositionStart(posW2)
 
    except Basda.ServiceIsBusyException as e:
        print '{:s}: inbeamTT'.format(e)

    except Basda.ServiceException as e:
        print '{:s}: inbeamTT'.format(e)
        

    try:
        # BasdaMoccaX.U9_LOG('Wait for ONLINE state from both services ...')
        # sciWheel1.waitForState(Basda.ServiceState.ONLINE)
        # sciWheel2.waitForState(Basda.ServiceState.ONLINE)
        # BasdaMoccaX.U9_LOG('ONLINE!')
        sciWheel1.moveToNamedPositionWait()
        sciWheel2.moveToNamedPositionWait()
        BasdaMoccaWheel.U9_LOG('Done'.format(posW1, posW2))
        print 'State W1: {:s} | State W2: {:s}'.format(str(sciWheel1.state()), str(sciWheel2.state()))
    except Basda.ServiceException as e:
        print '{:s}: Wheel moveToNamedPositionWait'.format(e)

   
    print 'Cicle {:d}'.format(i)
    print '--------------'


print 'Sci Wheel 1 State: {:s}'.format(str(sciWheel1.state())) 
print 'Sci Wheel 2 State: {:s}'.format(str(sciWheel2.state())) 

