#!/usr/bin/python

# 3D Mouse Driver
# Copyright (C) 2003 Neil Fraser, Scotland
# http://neil.fraser.name/

# This program is free software; you can redistribute it and/or modify it under the terms of version 2 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 for more details.  http://www.gnu.org/

# Assumes the inputs are arranged in the following pattern:
#      X
#
# Y    Z

# Requires pySerial to receive data from the mouse:
#   http://pyserial.sourceforge.net/

print "Which serial port is the 3D mouse plugged into?"
serialport = raw_input("(1/2/3/4): ")
if serialport != '2' and serialport != '3' and serialport != '4':
  serialport = '1'
serialport = "com"+serialport

print "Listening to serial port '%s'..." % serialport
import serial
ser = serial.Serial(serialport, 1200, timeout=1, bytesize=serial.SEVENBITS, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, rtscts=1)
# Eat through anything already sitting in the serial buffer.
while ser.inWaiting():
  ser.read(1)

import msvcrt
# Eat through anything sitting in the keyboard buffer.
while msvcrt.kbhit():
  msvcrt.getch()

# Flag to let the serial port thread know when to exit.
shuttingdown = False

# Depth/width in mm.  Height is assumed to be equal to width.
aspectratio = 150.0/250.0

# How many ticks does one horizontal crossing take for each sensor?
unitx = 3800 # Original x roller in Genius mouse (upper-right corner)
unity = 1627 # Shaft encoder mapped into y roller (lower-left corner)
unitz = 434 # Shaft encoder mapped into scroll wheel (lower-right corner)

import math # Some things are inevitable...

# The maximum value is the diagonal to the far corner of the rectangular prisim.
maxx = unitx * math.sqrt(1 + 1 + aspectratio*aspectratio)
maxy = unity * math.sqrt(1 + 1 + aspectratio*aspectratio)
maxz = unitz * math.sqrt(1 + 1 + aspectratio*aspectratio)

def init_scrolls():
  # Reset the pointer position (flat on the ground, in the center).
  global scrollx, scrolly, scrollz
  scrollx = math.sqrt(unitx*unitx + float(unitx*unitx)*aspectratio*aspectratio)/2
  scrolly = math.sqrt(unity*unity + float(unity*unity)*aspectratio*aspectratio)/2
  scrollz = math.sqrt(unitz*unitz + float(unitz*unitz)*aspectratio*aspectratio)/2

init_scrolls()

# Initial button states.
buttonl = False
buttonm = False
buttonr = False

def serial_thread():
  # Read data from the mouse as it arrives, one byte at a time.
  # Protocol for the Genius Scroll Mouse:
  # byte   d6   d5    d4    d3    d2    d1    d0
  #    0   1    lb    rb    dy7   dy6   dx7   dx6
  #    1   0    dx5   dx4   dx3   dx2   dx1   dx0
  #    2   0    dy5   dy4   dy3   dy2   dy1   dy0
  #    3   0    0     mb    dz3   dz2   dz1   dz0
  global scrollx, scrolly, scrollz
  global buttonl, buttonm, buttonr
  global shuttingdown
  n = 0
  scrollbitsx = [0, 0, 0, 0, 0, 0, 0, 0]
  scrollbitsy = [0, 0, 0, 0, 0, 0, 0, 0]
  scrollbitsz = [0, 0, 0, 0]
  while not shuttingdown:
    # Attempt to read one byte.
    # If there is no byte waiting, task is blocked for a one second timeout.
    # This could be avoided by checking 'ser.inWaiting()'.
    # But actually the timeout is useful since it stops this task from busy-waiting
    # when nothing's happening.
    byte = ser.read(1)

    if byte:
      byte = ord(byte)
      if byte & 64:
        n = 0
        # print "---"
      else:
        n = n + 1
      if n == 0:
        buttonl = not not (byte & 32)
        buttonr = not not (byte & 16)
        scrollbitsx[6] = not not (byte & 1)
        scrollbitsx[7] = not not (byte & 2)
        scrollbitsy[6] = not not (byte & 4)
        scrollbitsy[7] = not not (byte & 8)
      elif n == 1:
        scrollbitsx[0] = not not (byte & 1)
        scrollbitsx[1] = not not (byte & 2)
        scrollbitsx[2] = not not (byte & 4)
        scrollbitsx[3] = not not (byte & 8)
        scrollbitsx[4] = not not (byte & 16)
        scrollbitsx[5] = not not (byte & 32)
      elif n == 2:
        scrollbitsy[0] = not not (byte & 1)
        scrollbitsy[1] = not not (byte & 2)
        scrollbitsy[2] = not not (byte & 4)
        scrollbitsy[3] = not not (byte & 8)
        scrollbitsy[4] = not not (byte & 16)
        scrollbitsy[5] = not not (byte & 32)
      elif n == 3:
        scrollbitsz[0] = not not (byte & 1)
        scrollbitsz[1] = not not (byte & 2)
        scrollbitsz[2] = not not (byte & 4)
        scrollbitsz[3] = not not (byte & 8)
        buttonm = not not (byte & 16)
      # print byte & 64, byte & 32, byte & 16, byte & 8, byte & 4, byte & 2, byte & 1, "("+str(byte)+")"
      if n == 3:
        dx = scrollbitsx[6]*64 + scrollbitsx[5]*32 + scrollbitsx[4]*16 + scrollbitsx[3]*8 + scrollbitsx[2]*4 + scrollbitsx[1]*2 + scrollbitsx[0]
        if scrollbitsx[7]:
          scrollx = scrollx - dx + 128
          if scrollx > maxx:
            scrollx = maxx
        else:
          scrollx = scrollx - dx
          if scrollx < 0:
            scrollx = 0
        dy = scrollbitsy[6]*64 + scrollbitsy[5]*32 + scrollbitsy[4]*16 + scrollbitsy[3]*8 + scrollbitsy[2]*4 + scrollbitsy[1]*2 + scrollbitsy[0]
        if scrollbitsy[7]:
          scrolly = scrolly + dy - 128
          if scrolly < 0:
            scrolly = 0
        else:
          scrolly = scrolly + dy
          if scrolly > maxy:
            scrolly = maxy
        dz = scrollbitsz[2]*4 + scrollbitsz[1]*2 + scrollbitsz[0]
        if scrollbitsz[3]:
          scrollz = scrollz + dz - 8
          if scrollz < 0:
            scrollz = 0
        else:
          scrollz = scrollz + dz
          if scrollz > maxz:
            scrollz = maxz

def resolve(x, y, z):
  # Given the raw x/y/z data, calculate the height/width/depth coordinates.
  # First, convert x/y/z into the same unit (the sensors have different ticks/cm).
  global unitx, unity, unitz
  global aspectratio
  if x <= 0:
    x = 0.0001
  else:
    x = x / float(unitx)
  if y <= 0:
    y = 0.0001
  else:
    y = y / float(unity)
  if z <= 0:
    z = 0.0001
  else:
    z = z / float(unitz)
  #print "X: %f\tY: %f\tZ: %f\r" % (x, y, z)

  # Second, calculate the angle from y>z>p (Cosine rule)
  angle1 = (1*1 + z*z - y*y)/(2*z*1)
  angle1 = min(max(0, angle1), 1)
  angle1 = math.acos(angle1)

  # Third, calculate the height of the previous triangle (sine law)
  height1 = z*math.sin(angle1)

  # Fourth, calculate the width coordinate (Pythagorean theorem)
  width = y*y - height1*height1
  if width < 0:
    width = -math.sqrt(-width)
  else:
    width = math.sqrt(width)
  #print "Width: %f\t%f\t%f\r" % (width, height1, angle1/math.pi*180)

  # Fifth, calculate the angle from x>z>p (Cosine rule)
  angle2 = (aspectratio*aspectratio + z*z - x*x)/(2*z*aspectratio)
  angle2 = min(max(0, angle2), 1)
  angle2 = math.acos(angle2)

  # Sixth, calculate the height of the previous triangle (sine law)
  height2 = z*math.sin(angle2)

  # Seventh, calculate the depth coordinate (Pythagorean theorem)
  depth = z*z - height2*height2
  if depth < 0:
    depth = -math.sqrt(-depth)
  else:
    depth = math.sqrt(depth)
  #print "Depth: %f\t%f\t%f\r" % (depth, height2, angle2/math.pi*180)

  # Eighth, calculate the height coordinate (Pythagorean theorem)
  height = height1*height1 - depth*depth
  if height >= 0:
    height = math.sqrt(height)
  else:
    height = 0.0

  # Ninth, calculate the height coordinate from the other direction
  # Commented out.  I hereby declare the math to be correct.
  #height_check = height2*height2 - (1-width)*(1-width)
  #if height_check >= 0:
  #  height_check = math.sqrt(height_check)
  #else:
  #  height_check = 0.0
  #if abs(height-height_check) > 0.001:
  #  print "Calculation error!  Height check failed: ", height, height_check

  return (width, depth, height)


import thread
thread.start_new_thread(serial_thread, ())

print "Driver started."
print
print "* Type 'a' to get current X/Y/Z raw data."
print "* Type 'b' to get current button positions."
print "* Type 'c' to get current width/depth/height coordinates."
print "* Type ' ' to reset position to the middle of the board."
print "* Type 'x' to terminate driver."
while not shuttingdown:
  # Check for Keyboard commands.
  kb_command = msvcrt.getch()

  if kb_command:
    if kb_command == 'a':
      print "X: %d\tY: %d\tZ: %d\r\n" % (scrollx, scrolly, scrollz)
    elif kb_command == 'b':
      print "L: %r\tM: %r\tR: %r\r\n" % (buttonl, buttonm, buttonr)
    elif kb_command == 'c':
      (width, depth, height) = resolve(scrollx, scrolly, scrollz)
      print "W: %f\tD: %f\tH: %f\r\n" % (width, depth, height)
    elif kb_command == ' ':
      init_scrolls()
    elif kb_command == 'x':
      shuttingdown = True

ser.close()
print
print "Driver shutdown."