#!/usr/bin/env python

import numpy as np
import cv2, math
import video

help_message = '''
USAGE: opt_flow.py [<video_source>]

Keys:
 1 - toggle HSV flow visualization
 2 - toggle glitch

'''

# 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

import OSC
# from pythonosc import osc_message_builder
# from pythonosc import udp_client
import time

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)


    # for (x1, y1), (x2, y2) in lines:
    #     # print ("y1", y1)
    #     if (y1 == 38 or y1 == 46 or y1 == 54 or y1 == 70 or y1 == 86) and x1 in range(38, 118, 8):
    #         flines.append(((x1,y1),(x2,y2)))
    #         dx = x2-x1
    #         dy = y2 - y1
    #         m = int(math.sqrt( (dx*dx) + (dy*dy) ))
    #         if m>2:
    #             print ("x", (dx, dy, m, x1, y1))
    #             msg = OSC.OSCMessage()
    #             msg.setAddress("/x")
    #             msg.append(dx)
    #             msg.append(dy)
    #             msg.append(m)
    #             msg.append(x1)
    #             msg.append(y1)
    #             client.send(msg)


                # Here goes BPM


                

    # for (x1, y1), (x2, y2) in lines:
    #     # print ("y1", y1)
    #     if (y1 == 10 or y1 == 110) and x1 in range(90, 150, 4):
    #         flines.append(((x1,y1),(x2,y2)))
    #         dx = x2-x1
    #         dy = y2 - y1
    #         m = int(math.sqrt( (dx*dx) + (dy*dy) ))
    #         if m>2:
    #             print ("l", (dx, dy, m, x1, y1))
    #             msg = OSC.OSCMessage()
    #             msg.setAddress("/left")
    #             msg.append(dx)
    #             msg.append(dy)
    #             msg.append(m)
    #             msg.append(x1)
    #             msg.append(y1)
    #             client.send(msg)


    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


    # 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
    #data="hello"
    # client = udp_client.SimpleUDPClient("127.0.0.1", 9001)

    # connect camera
    # cam = video.create_capture(fn)
    cam = video.create_capture("0:size=160x120") #canvas size in pixels
    ret, prev = cam.read()
    prevgray = cv2.cvtColor(prev, cv2.COLOR_BGR2GRAY)
    cur_glitch = prev.copy()

    while True:
       # print "GRAB FRAME"
        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

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

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