######################################### telescope_control.py ###################################
#  
import sys
mypath=sys.path[0]
if mypath.find('my_python') == -1:       #If true, then "my_python" could not be found
  print "Sorry.  You need to be in my_python for Halverson's program to work."
  print "(That''s because I am assuming that the tgraphlix.py and ezu3.py libraries are there.)"
  exit()
sys.path[0]=mypath[0:mypath.find('my_python')]+'my_python'

import pyaudio      #This is to make a beep sound
# Open the stream required, mono mode only...
stream=pyaudio.PyAudio().open(format=pyaudio.paInt8,channels=1,rate=16000,output=True)

print "Use ^C to kill this program"
import u3
from ezu3_two_devices import *
from time import sleep


n_of_devices=u3.deviceCount(3)
print "Number of Labjack devices=",n_of_devices
devdict = u3.openAllU3()
device_number = 1
for i in devdict.items():
  print "Device #",device_number," has serial number ",i[0]
  device_number=device_number+1

print "If the program crashes now, its because you need to put the right serial numbers in the code."
print "The serial numbers must match the ones just printed out."

motors=devdict['320066905']    #Telescope motor controller
controller=devdict['320066924']    #Boris's hand controller
u3setup(motors,['ain','ain','dout','din','dout','dout','dout','dout'], \
  u3channelsE=['dout','dout','dout','dout','dout','dout','dout','dout'], \
u3channelsC=['din','din','dout','dout'])
sleep(0.25)
u3setup_2(controller,['din','din','ain','ain','dout','dout','dout','dout'])
sleep(0.25)

#bits=input("Give pattern for MS 1,2,3: ")
#If bits = 0, then 1 pulses per step 
#          1,      32 pulses per step
#          2,      16 pulses per step
#          3,      8 pulses per step
#          4,      4 pulses per step for the Polulu DRV8825
#		   5,	   2 pulses per step
#		
#          5, 6, 7     32 for the Polulu DRV8825
#          7       16 for the A4988

bits = 3   #medium speed 8 pulses per step
dout5(motors,bits & 1 != 0)     #microstep select 1
dout6(motors,bits & 2 != 0)     #microstep select 2
dout7(motors,bits & 4 != 0)     #microstep select 3
doutE5(motors,bits & 1 != 0)     #microstep select 1
doutE6(motors,bits & 2 != 0)     #microstep select 2
doutE7(motors,bits & 4 != 0)     #microstep select 3


#Keystroke example from 
#https://stackoverflow.com/questions/13207678/whats-the-simplest-way-of-detecting-keyboard-input-in-python-from-the-terminal

global isWindows

isWindows = False
try:
    from win32api import STD_INPUT_HANDLE
    from win32console import GetStdHandle, KEY_EVENT, ENABLE_ECHO_INPUT, ENABLE_LINE_INPUT, ENABLE_PROCESSED_INPUT
    isWindows = True
except ImportError as e:
    import sys
    import select
    import termios


class KeyPoller():
    def __enter__(self):
        global isWindows
        if isWindows:
            self.readHandle = GetStdHandle(STD_INPUT_HANDLE)
            self.readHandle.SetConsoleMode(ENABLE_LINE_INPUT|ENABLE_ECHO_INPUT|ENABLE_PROCESSED_INPUT)

            self.curEventLength = 0
            self.curKeysLength = 0

            self.capturedChars = []
        else:
            # Save the terminal settings
            self.fd = sys.stdin.fileno()
            self.new_term = termios.tcgetattr(self.fd)
            self.old_term = termios.tcgetattr(self.fd)

            # New terminal setting unbuffered
            self.new_term[3] = (self.new_term[3] & ~termios.ICANON & ~termios.ECHO)
            termios.tcsetattr(self.fd, termios.TCSAFLUSH, self.new_term)

        return self

    def __exit__(self, type, value, traceback):
        if isWindows:
            pass
        else:
            termios.tcsetattr(self.fd, termios.TCSAFLUSH, self.old_term)

    def poll(self):
        if isWindows:
            if not len(self.capturedChars) == 0:
                return self.capturedChars.pop(0)

            eventsPeek = self.readHandle.PeekConsoleInput(10000)

            if len(eventsPeek) == 0:
                return None

            if not len(eventsPeek) == self.curEventLength:
                for curEvent in eventsPeek[self.curEventLength:]:
                    if curEvent.EventType == KEY_EVENT:
                        if ord(curEvent.Char) == 0 or not curEvent.KeyDown:
                            pass
                        else:
                            curChar = str(curEvent.Char)
                            self.capturedChars.append(curChar)
                self.curEventLength = len(eventsPeek)

            if not len(self.capturedChars) == 0:
                return self.capturedChars.pop(0)
            else:
                return None
        else:
            dr,dw,de = select.select([sys.stdin], [], [], 0)
            if not dr == []:
                return sys.stdin.read(1)
            return None
            
def set_microstep_bits(bits):
        dout5(motors,bits & 1 != 0)     #microstep select 1 motor 1  Elevation motor
        dout6(motors,bits & 2 != 0)     #microstep select 2 motor 1
        dout7(motors,bits & 4 != 0)     #microstep select 3 motor 1
        doutE5(motors,bits & 1 != 0)     #microstep select 1 motor 2  Azimuth motor
        doutE6(motors,bits & 2 != 0)     #microstep select 2 motor 2
        doutE7(motors,bits & 4 != 0)     #microstep select 3 motor 2

with KeyPoller() as keyPoller:
  print "f=forward, b = backwards, s=stop, 1,2,3,4,5,6 are speeds,  q to quit."
  go_forward1 = False
  go_backward1 = False
  go_forward2 = False
  go_backward2 = False
  sleeptime=0.0001
  # Handcontroller variables
  deadband_V=0.1   # This is the half width of the "deadband" inside which there is no motion
  center_V = 2.44/2.0
  activeband_V= 2.44/2.0 - deadband_V  #Width of active bands around the deadband
  elevation_control_active1 = False   #Handcontrol is disabled until unocked by two step process
  elevation_control_active2 = False
  elevation_sleepmax = 10
  elevation_sleepcount = 0
  azimuth_control_active1 = False   #Handcontrol is disabled until unocked by two step process
  azimuth_control_active2 = False
  azimuth_sleepmax = 10
  azimuth_sleepcount = 0

  while True:
    #  Device 2: Controller built by Boris Bonillo ======================================
    elevation_controller_V=ain2_2(controller)       #Read analog inputs
    azimuth_controller_V=ain3_2(controller)
    turbo_switch=din0_2(controller)
    emergency_stop=din1_2(controller)
    #print "Hand control:",emergency_stop," ",turbo_switch," ",azimuth_controller_V," ",elevation_controller_V
    #----------------- elevation control
    if elevation_control_active1 & elevation_control_active2:
      #Yes, the control is unlocked
      elevation_speed_V = abs(elevation_controller_V - (deadband_V + center_V))
      if elevation_speed_V > deadband_V:
        elevation_sleepmax = activeband_V/elevation_speed_V #So for a small speed, the sleep will be big
        if elevation_controller_V > center_V:
          go_forward1 = True
          go_backward1 = False
          dout2(motors,1)
        else:  
          go_forward1 = False
          go_backward1 = True          
          dout2(motors,0)
      else:
        go_forward1 = False
        go_backward1 = False
    else:
      if elevation_control_active1:
        if (elevation_controller_V > deadband_V - center_V) | (elevation_controller_V < deadband_V + center_V):
          elevation_control_active2 = True
          print "Unlocking elevation hand control"
      else:
        if (elevation_controller_V < deadband_V - center_V) | (elevation_controller_V > deadband_V + center_V):
          elevation_control_active1 = True
    #----------------- azimuth control
    if azimuth_control_active1 & azimuth_control_active2:
      #Yes, the control is unlocked
      azimuth_speed_V = abs(azimuth_controller_V - (deadband_V + center_V))
      if azimuth_speed_V > deadband_V:
        azimuth_sleepmax = activeband_V/azimuth_speed_V #So for a small speed, the sleep will be big
        if azimuth_controller_V > center_V:
          go_forward2 = True
          go_backward2 = False
          doutE2(motors,1)  
        else:  
          go_forward2 = False
          go_backward2 = True          
          doutE2(motors,0)  
      else:
        go_forward2 = False
        go_backward2 = False
    else:
      if azimuth_control_active1:
        if (azimuth_controller_V > deadband_V - center_V) | (azimuth_controller_V < deadband_V + center_V):
          azimuth_control_active2 = True
          print "Unlocking azimuth hand control"
      else:
        if (azimuth_controller_V < deadband_V - center_V) | (azimuth_controller_V > deadband_V + center_V):
          azimuth_control_active1 = True
    
    #Keyboard control
    c = keyPoller.poll()
    if not c is None:
      print c," which is ascii code ",ord(c),
      if c == "f":     #go forward
        go_forward1 = True
        go_backward1 = False
        dout2(motors,1)
        print("Motor 1 Forward")
      elif c == "b":     #go backward
        go_forward1 = False
        go_backward1 = True
        dout2(motors,0)
        print("Motor 1 Backward")
      elif c == "F":     #go forward  MOTOR 2
        go_forward2 = True
        go_backward2 = False
        doutE2(motors,1)   
        print("Motor 2 Forward")
      elif c == "B":     #go backward  MOTOR 2
        go_forward2 = False
        go_backward2 = True
        doutE2(motors,0)  
        print("Motor 2 Backward")
      elif (c == "s") | (c == "S"):      #stop
        go_forward1 = False
        go_backward1 = False
        go_forward2 = False
        go_backward2 = False
        print("STOP")
      elif c == "1":
        bits = 5
        set_microstep_bits(bits)
        print(" Bits = 5, 1/32 steps")
      elif c == "2":
        bits = 4
        set_microstep_bits(bits)
        print(" Bits = 4, 1/16 steps")
      elif c == "3":
        bits = 3
        set_microstep_bits(bits)
        print(" Bits = 3, 1/8 steps")
      elif c == "4":
        bits = 2
        set_microstep_bits(bits)
        print(" Bits = 2, 1/4 steps")
      elif c == "5":
        bits = 1
        set_microstep_bits(bits)
        print(" Bits = 1, 1/2 steps")
      elif c == "6":
        bits = 0
        set_microstep_bits(bits)
        print(" Bits = 0, 1/1 steps")
      elif (c == "q") | (c == "Q"):
        break
      else:
        print("Invalid command") 
        #1 kHz tone for 0.1 seconds   I got this from http://code.activestate.com/recipes/578301-platform-independent-1khz-pure-audio-sinewave-gene/
        for n in range(0,100,1): stream.write("\x00\x30\x5a\x76\x7f\x76\x5a\x30\x00\xd0\xa6\x8a\x80\x8a\xa6\xd0")
    #------------------- elevation motor
    if elevation_sleepcount < elevation_sleepmax:
      #Don't move the motor yet
      elevation_sleepcount += 1
    else:
      elevation_sleepcount = 0
      #Yes move the motor now
      if go_forward1 | go_backward1:
        dout4(motors,True)         #step command
    #------------------- azimuth motor
    if azimuth_sleepcount < azimuth_sleepmax:
      #Don't move the motor yet
      azimuth_sleepcount += 1
    else:
      azimuth_sleepcount = 0
      #Yes move the motor now
      if go_forward2 | go_backward2:
        doutE4(motors,True)         #step command
    sleep(sleeptime)
    if go_forward1 | go_backward1:
      dout4(motors,False)         #step command
    if go_forward2 | go_backward2:
      doutE4(motors,False)         #step command
    #sleep(sleeptime)
print "Quitting"
stream.close()
pyaudio.PyAudio().terminate()