#!/usr/bin/env python
#coding=utf8
'''
Headmouse!
'''

import serial
import time
import threading
import sys

import cv2

import hmCam
import hmFilterData as filter
import arduinoSerial

CAMERA_ID = 0
ARDUINO_PORT = 'COM7'
INPUT_VISUALIZER = True

def main():
    # output driver setup
    arduino = arduinoSerial.get_serial_link(ARDUINO_PORT, 115200, timeout=1, async=False, slices=8)

    # input driver setup
    hmCam.displayWindow = INPUT_VISUALIZER
    hmCam.bind(CAMERA_ID)
    
    def input_source():
    	while True:
    		yield hmCam.popAndAnalyze()

    # signal proc chain setup
    velocity_gen = filter.relative_movement()
    sub_pix_gen = filter.sub_pix_trunc()
    stateful_smooth_gen = filter.stateful_smoother()
    input_smoother_gen = filter.ema_smoother(.90)
    slow_smoother_gen = filter.slow_smoother(.6)
    acceleration_gen = filter.accelerate_exp(p=2, accel=1.4, sensitivity=6.5)

    # main loop setup
    startTime = time.time()
    timeC = 0
    loops = 0

    # main loop
    for coords in input_source():
        loops += 1
        timeC += time.time() - startTime
        if loops == 100:
            loops = 0
            print "fps is around:", 100. / timeC
            timeC = 0
        #print "time took:", time.time() - startTime
        startTime = time.time()
        # Capture frame-by-frame

        ### Filter Section ###
        #Take absolute position return relative position
        v = velocity_gen.send(coords)
        v = filter.killOutliers(v, 20)


        v = slow_smoother_gen.send((v, 6))
        v = input_smoother_gen.send(v)
        v = acceleration_gen.send(v)
        #v = filter.accelerate(v)

        v = sub_pix_gen.send(v)

        #Mirror image on x-axis
        x = -v[0]
        y = v[1]

        #Duplicate in Arduino
        #print "coords are:" + x + ", " + y

        arduino.move_mouse(x,y)

        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

    # input driver cleanup
    # When everything done, release the capture
    hmCam.cleanup()

    return 0

if __name__ == "__main__":
    sys.exit(main())


@@ -216,16 +216,38 @@
  'COM7'%0A
+ARDUINO_BAUD = 115200%0A
 INPUT_VI
@@ -271,23 +271,408 @@
 def 
-main():%0A    # o
+consumer(func):%0A    '''%0A    Decorator taking care of initial next() call to %22sending%22 generators%0A%0A    From PEP-342%0A    http://www.python.org/dev/peps/pep-0342/%0A    '''%0A    def wrapper(*args,**kw):%0A        gen = func(*args, **kw)%0A        next(gen)%0A        return gen%0A    wrapper.__name__ = func.__name__%0A    wrapper.__dict__ = func.__dict__%0A    wrapper.__doc__  = func.__doc__%0A    return wrapper%0A%0A## O
 utpu
@@ -675,30 +675,154 @@
 utput driver
- setup
+s%0A%0A@consumer%0Adef arduino_output(port=ARDUINO_PORT, baud=ARDUINO_BAUD):%0A    '''Write mouse coordinates out to Arduino via serial'''
 %0A    arduino
@@ -858,28 +858,18 @@
 ink(
-ARDUINO_PORT, 115200
+port, baud
 , ti
@@ -899,16 +899,386 @@
 lices=8)
+%0A    while True:%0A        x,y = yield%0A        #arduino.move_mouse(x,y)%0A        print(%22%7B:d%7D, %7B:d%7D%22.format(x, y))%0A%0A@consumer%0Adef print_output():%0A    '''Write mouse coordinates out to stdout'''%0A    while True:%0A        x,y = yield%0A        print(%22%7B:d%7D, %7B:d%7D%22.format(x, y))%0A%0Adef main():%0A    '''Headmouse main loop'''%0A    # output driver setup%0A    output_driver = print_output()
 %0A%0A    # 
@@ -1398,17 +1398,20 @@
 ():%0A    
-%09
+    
 while Tr
@@ -1422,10 +1422,16 @@
     
-%09%09
+        
 yiel
@@ -2734,39 +2734,41 @@
         
-arduino.move_mouse
+output_driver.send(
 (x,y)
+)
 %0A%0A      

Break output drivers out into generators