######################################### 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)
#If the line above gives an error, you need to install pyaudio.  It is available on
#Halverson's web site in the Python Program Files area

def beep_1khz():
  #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")

beep_1khz()   #make a beep sound to say the program is starting

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.001
  # 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
  switches = 0
  switches_old = 999

  while True:
    #  Device 2: Controller built by Boris Bonillo ======================================
    elevation_controller_V=ain2_2(controller)       #Read analog inputs
    azimuth_controller_V=ain3_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)    #Direction bit
        else:  
          go_forward1 = False
          go_backward1 = True          
          dout2(motors,0)         #Direction bit
      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"
          #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")
      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)         #Direction bit
        else:  
          go_forward2 = False
          go_backward2 = True          
          doutE2(motors,0)         #Direction bit
      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"
          #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")
      else:
        if (azimuth_controller_V < deadband_V - center_V) | (azimuth_controller_V > deadband_V + center_V):
          azimuth_control_active1 = True
    #Control the speed depending on the hand controller switch settings
    switch0=din0_2(controller)
    switch1=din1_2(controller)
    if not switch0:
      switches = 1
    else:
      switches = 0  
    if not switch1:
      switches = switches + 2
    if switches != switches_old:   #Check if the switches have changed
      switches_old = switches
      if switches == 0:
        set_microstep_bits(5)
        print(" Bits = 5, 1/32 steps")
      elif switches == 1:
        set_microstep_bits(4)
        print(" Bits = 4, 1/16 steps")
      elif switches == 2:
        set_microstep_bits(3)
        print(" Bits = 3, 1/8 steps")
      else:
        set_microstep_bits(1)
        print(" Bits = 1, 1/2 steps")
        
    #Keyboard control
    c = keyPoller.poll()
    if not c is None:
      print c," which is ascii code ",ord(c),
      if c == "1":
        set_microstep_bits(5)
        print(" Bits = 5, 1/32 steps")
      elif c == "2":
        set_microstep_bits(4)
        print(" Bits = 4, 1/16 steps")
      elif c == "3":
        set_microstep_bits(3)
        print(" Bits = 3, 1/8 steps")
      elif c == "4":
        set_microstep_bits(2)
        print(" Bits = 2, 1/4 steps")
      elif c == "5":
        set_microstep_bits(1)
        print(" Bits = 1, 1/2 steps")
      elif c == "6":
        set_microstep_bits(0)
        print(" Bits = 0, whole 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:
      dout6(controller,go_forward1)   #Going up LED
      dout7(controller,go_backward1) #Going down LED
      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:
      dout4(controller,go_backward2)   #Going left LED
      dout5(controller,go_forward2) #Going right LED
      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()