##########################################################################
# Etch A Sketch 1.0.  Computer control of the drawing toy.               #
# Copyright (C) 2008 Neil Fraser                                         #
# http://neil.fraser.name/hardware/etch/                                 #
#                                                                        #
# This program is free software; you can redistribute it and/or          #
# modify it under the terms of the GNU General Public License            #
# as published by the Free Software Foundation.                          #
#                                                                        #
# This program is distributed in the hope that it will be useful,        #
# but WITHOUT ANY WARRANTY; without even the implied warranty of         #
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the          #
# GNU General Public License (www.gnu.org) for more details.             #
#                                                                        #
# Uses pygame for the graphics and joystick interface.                   #
# http://www.pygame.org/                                                 #
#                                                                        #
# Uses inpout32.dll for the parallel port interface.                     #
# http://neil.fraser.name/software/lpt/                                  #
##########################################################################

from pygame import *
# pygame has its own 'time', so rename Python's standard 'time' module.
import time as pytime

from ctypes import *
InpOut = windll.LoadLibrary('INPOUT32.DLL')

# The three standard port addresses
lpt1 = 0x3BC
lpt2 = 0x378
lpt3 = 0x278
# Choose the port to use
lpt = lpt1
# Data byte:     lpt + 0
# Feedback byte: lpt + 1
# Control byte:  ltp + 2

# Read one byte from the port address.  Returns 0-255
def getByte(port):
  return InpOut.Inp32(port)

# Send one byte to the port address.
def setByte(port, value):
  return InpOut.Out32(port, value)

# Width and height of Etch A Sketch in steps.
Size = (1000, 700)
# PixelArray object making up the display
Sketch = None
# Number of steps needed to compensate for lag when reversing.
Lag = 2
# Timeout reference points.
LastTiltTime = 0
LastHorizontalTime = 0
LastVerticalTime = 0
# Last known Joystick values.
JoyX = 0.0
JoyY = 0.0
JoyZ = 1.0


def clearScreen():
  bgcolor = (200, 200, 200)
  # Fast fill; Same as nested x,y loop below.
  Sketch[:][:] = bgcolor
  #for x in xrange(Size[0]):
  #  for y in xrange(Size[1]):
  #    Sketch[x][y] = bgcolor
  display.flip()

def readEvents(flipMode):
  global LastTiltTime
  global JoyX, JoyY, JoyZ
  quit = False
  for e in event.get():
    if (event.event_name(e.type) == 'Quit' or
        (event.event_name(e.type) == 'KeyDown' and
         e.dict['key'] == 27)):
      quit = True
    elif event.event_name(e.type) == 'JoyAxisMotion':
      if e.dict['axis'] == 0:
        JoyX = e.dict['value']
        if abs(JoyX) < 0.05:
          JoyX = 0
      elif e.dict['axis'] == 1:
        JoyY = e.dict['value']
        if abs(JoyY) < 0.05:
          JoyY = 0
      elif e.dict['axis'] == 2:
        # Convert from -1,1 to 0,1
        JoyZ = 1 - (e.dict['value'] + 1) / 2
    elif ((event.event_name(e.type) == 'JoyHatMotion' and
           e.dict['value'][1] == 1) or
          (event.event_name(e.type) == 'KeyDown' and
           e.dict.has_key('unicode') and e.dict['unicode'] == u'+')):
      LastTiltTime = pytime.time()
      flipMode = 1
      print "FlipMode: %d" % flipMode
    elif ((event.event_name(e.type) == 'JoyHatMotion' and
           e.dict['value'][1] == -1) or
          (event.event_name(e.type) == 'KeyDown' and
           e.dict.has_key('unicode') and e.dict['unicode'] == u'-')):
      LastTiltTime = pytime.time()
      flipMode = -1
      print "FlipMode: %d" % flipMode
    elif (event.event_name(e.type) == 'KeyDown' and e.dict['unicode'] == u' '):
      clearScreen()
      print "Cleared screen"
    elif ((event.event_name(e.type) == 'JoyHatMotion' and
          e.dict['value'][1] == 0) or
          (event.event_name(e.type) == 'KeyUp' and e.dict.has_key('unicode') and
          (e.dict['unicode'] == u'+' or e.dict['unicode'] == u'-'))):
      flipMode = 0
      print "FlipMode off."
    elif event.event_name(e.type) == 'KeyDown':
      if e.dict['key'] == 273: # Up
        JoyY = -1
      elif e.dict['key'] == 274: # Down
        JoyY = 1
      elif e.dict['key'] == 275: # Right
        JoyX = 1
      elif e.dict['key'] == 276: # Left
        JoyX = -1
      #elif e.dict['key'] == 111: # O
      #  Circle = True
    elif event.event_name(e.type) == 'KeyUp':
      if e.dict['key'] == 273: # Up
        JoyY = 0
      elif e.dict['key'] == 274: # Down
        JoyY = 0
      elif e.dict['key'] == 275: # Right
        JoyX = 0
      elif e.dict['key'] == 276: # Left
        JoyX = 0
      #else:
      #  print "Unknown key: %s" % e.dict
    #else:
    #  print "Unknown event '%s': %s" % (event.event_name(e.type), e.dict)
  #print "Joy: (%f, %f, %f) [%s]" % (joyX, joyY, joyZ, flipMode)
  deltaX = JoyX * JoyZ
  deltaY = JoyY * JoyZ
  return (deltaX, deltaY, flipMode, quit)


def initJoystick():
  print "Searching for joysticks."
  joystick.init()
  joystickCount = joystick.get_count()
  joystickId = 0
  if joystickCount == 0:
    print "No joystick found."
  elif joystickCount == 1:
    print "Joystick found."
    joystickId = 1
  else:
    try:
      joystickId = input("Multiple joysticks found. Pick one.\n(1-%d):" %
                         joystickCount)
    except:
      # Bad user input
      joystickId = 1

  if joystickId:
    joystickObj = joystick.Joystick(joystickId - 1)
    joystickObj.init()
    print "Joystick initialized: %s" % joystickObj.get_name()
    return joystickObj
  return None


def outputCommand(flipMode, horizontalQuad, verticalQuad):
  commandByte = 0
  if flipMode == 1:
    commandByte += 128
  elif flipMode == -1:
    commandByte += 64

  if pytime.time() > LastHorizontalTime + 0.5:
    # It's been over half a second without movement, idle the motor
    commandByte += 0+0+4
  else:
    # Keep the quad 0-3.
    horizontalQuad = horizontalQuad % 4
    if horizontalQuad == 0:
      commandByte += 0+0+0
    elif horizontalQuad == 1:
      commandByte += 0+2+0
    elif horizontalQuad == 2:
      commandByte += 1+2+0
    elif horizontalQuad == 3:
      commandByte += 1+0+0
  if pytime.time() > LastVerticalTime + 0.5:
    # It's been over half a second without movement, idle the motor
    commandByte += 0+0+32
  else:
    # Keep the quad 0-3.
    verticalQuad = verticalQuad % 4
    if verticalQuad == 0:
      commandByte += 0+0+0
    elif verticalQuad == 1:
      commandByte += 8+0+0
    elif verticalQuad == 2:
      commandByte += 8+16+0
    elif verticalQuad == 3:
      commandByte += 0+16+0

  setByte(lpt, commandByte)


def checkFlip(flipMode):
  if flipMode != 0:
    if pytime.time() > LastTiltTime + 8:
      flipMode = 0
      print "Flip motor stalled?"

    statusByte = getByte(lpt + 1)
    #print statusByte
    #print "Mercury: %d %d" % ((statusByte & 16), (statusByte & 32))
    if (flipMode == 1) and not (statusByte & 16) and (statusByte & 32):
      flipMode = 0
      print "Upper limit"

    if (flipMode == -1) and (statusByte & 16) and (statusByte & 32):
      flipMode = 0
      print "Lower limit"
      clearScreen()
  return flipMode


def main():
  global Sketch
  global LastHorizontalTime, LastVerticalTime
  
  # Initialize pygame.
  init()  
  initJoystick()
  # Initializing screen.
  screen = display.set_mode(Size)
  Sketch = PixelArray(screen)
  clearScreen()

  # Actual location of the stylus.
  # Assume stylus starts in the middle of the screen.
  etchX = int(Size[0] / 2)
  etchY = int(Size[1] / 2)
  # Soak up a few steps during reversals.
  # Assume stylus starts relaxed.
  lagX = 0
  lagY = 0
  # Motion smaller than one pixel
  bufferX = 0
  bufferY = 0
  # Are we tilting down (-1) up (1) or not moving (0)
  flipMode = 0
  # Current step position for motor (0-3)
  horizontalQuad = 0
  verticalQuad = 0
  # Last known position of the joystick.
  deltaX = 0.0
  deltaY = 0.0
  speedZ = 1.0

  while True:
    # Read events from joystick and keyboard
    (deltaX, deltaY, flipMode, quit) = readEvents(flipMode)

    if quit:
      commandByte = 0x24 # Switch off the tilt, idle the stepper motors
      setByte(lpt, commandByte);
      display.quit()
      print "Program End\n"
      return

    # Modify flipMode based on timeouts and limit sensors.
    flipMode = checkFlip(flipMode)

    bufferX += deltaX
    bufferY += deltaY

    if bufferX > 1:
      step = True
      if lagX >= Lag:
        etchX += 1
        if etchX > Size[0] - 1:
          etchX = Size[0] - 1
          step = False
        bufferX =- 1
      else:
        lagX += 1
      if step:
        horizontalQuad += 1
        LastHorizontalTime = pytime.time()
    elif bufferX < -1:
      step = True
      if lagX <= -Lag:
        etchX -= 1
        if etchX < 0:
          etchX = 0
          step = False
        bufferX =+ 1
      else:
        lagX -= 1
      if step:
        horizontalQuad -= 1
        LastHorizontalTime = pytime.time()
    if bufferY > 1:
      step = True
      if lagY >= Lag:
        etchY += 1
        if etchY > Size[1] - 1:
          etchY = Size[1] - 1
          step = False
        bufferY =- 1
      else:
        lagY += 1
      if step:
        verticalQuad += 1
        LastVerticalTime = pytime.time()
    elif bufferY < -1:
      step = True
      if lagY <= -Lag:
        etchY -= 1
        if etchY < 0:
          etchY = 0
          step = False
        bufferY =+ 1
      else:
        lagY -= 1
      if step:
        verticalQuad -= 1
        LastVerticalTime = pytime.time()

    # Draw a pixel on the screen.
    Sketch[etchX][etchY] = (0, 0, 0)
    display.flip()

    #time.wait(5)

    outputCommand(flipMode, horizontalQuad, verticalQuad)


if __name__ == '__main__': main()