#!/usr/bin/env python

import numpy as np
import cv2, math
# import video
from picamera.array import PiRGBArray
from picamera import PiCamera
import OSC
# from pythonosc import osc_message_builder
# from pythonosc import udp_client
import time


# def draw_flow(img, flow, step=4): # size grid 
#     h, w = img.shape[:2]
#     y, x = np.mgrid[step/2:h:step, step/2:w:step].reshape(2,-1)
#     fx, fy = flow[y,x].T
#     lines = np.vstack([x, y, x+fx, y+fy]).T.reshape(-1, 2, 2)
#     lines = np.int32(lines + 0.5)
#     vis = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
#     cv2.polylines(vis, lines, 0, (0, 0, 255)) # BGR
#     for (x1, y1), (x2, y2) in lines:
#         cv2.circle(vis, (x1, y1), 1, (0, 255, 0), -1)
#     return vis


def send_flow0(img, flow, step=4): # size grid 
    h, w = img.shape[:2]
    y, x = np.mgrid[step/2:h:step, step/2:w:step].reshape(2,-1)
    fx, fy = flow[y,x].T
    #print "fx, fy", fx, fy
    lines = np.vstack([x, y, x+fx, y+fy]).T.reshape(-1, 2, 2)
    lines = np.int32(lines + 0.5)
    vis = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)


    flines = []
    for (x1, y1), (x2, y2) in lines:
        # print ("y1", y1)
        if (x1 == 38 or x1 == 46 or x1 == 54 or x1 == 62 or x1 == 70 or x1 == 78 or x1 == 86 or x1 == 94 or x1 == 102 or x1 == 110 or x1 == 118) and y1 in range(38, 90, 8):
            flines.append(((x1,y1),(x2,y2)))
            normx = x1 / 8 - 4
            normy = 1 - ((y1 / 8 - 4) / 3.0)
            dx = x2-x1
            dy = y2 - y1
            m = int(math.sqrt( (dx*dx) + (dy*dy) ))
            if m>2:
                print ("dot", (normx, normy))
                msg = OSC.OSCMessage()
                msg.setAddress("/dot")
                #msg.append(dx)
                #msg.append(dy)
                #msg.append(m)
                msg.append(normx)
                msg.append(normy)
                client.send(msg)
            # client.send_message("/franc", m)


    flines = np.int32(flines)
    cv2.polylines(vis, flines, 0, (0, 40, 255)) # BGR
    for (x1, y1), (x2, y2) in flines:
        cv2.circle(vis, (x1, y1), 1, (0, 255, 0), -1)
    return vis

    flines = np.int32(flines)
    cv2.polylines(vis, flines, 0, (0, 40, 255)) # BGR
    for (x1, y1), (x2, y2) in flines:
        cv2.circle(vis, (x1, y1), 1, (0, 255, 0), -1)
    return vis

    # cv2.rectangle(img, pt1, pt2, color[, thickness[, lineType[, shift]]])

if __name__ == '__main__':
    import sys
    print help_message
    try: fn = sys.argv[1]
    except: fn = 0


    #data="hello"
    # client = udp_client.SimpleUDPClient("127.0.0.1", 9001)

    # connect camera
    # cam = video.create_capture("0:size=160x120") #canvas size in pixels
    cam = PiCamera()
    framesize = (160, 120)
    camera.resolution = framesize
    camera.framerate = 32
    rawCapture = PiRGBArray(camera, size=framesize)
    # allow the camera to warmup
    time.sleep(0.25)

    # connect to pd
    # Init OSC
    client = OSC.OSCClient()
    client.connect(('127.0.0.1', 9001)) # first argument is the IP of the host, second argument is the port to use

    prevgray = None
    for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
    # while True:
       # print "GRAB FRAME"
        img = frame.array
        # ret, img = cam.read()
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        flow = cv2.calcOpticalFlowFarneback(prevgray, gray, 0.5, 3, 15, 3, 5, 1.2, 0)
        prevgray = gray
        # clear the stream in preparation for the next frame
        rawCapture.truncate(0)

        # cv2.imshow('flow', send_flow0(gray, flow))

        #ch = 0xFF & cv2.waitKey(5)
        #if ch == 27:
        #    break
    # cv2.destroyAllWindows()