#!/usr/bin/env python

from __future__ import print_function
import numpy as np
import cv2, math, sys
# 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

# MESSAGES NEED TO GO TO STDERR
print ("1.HELLO FROM PYTHON stderr", file=sys.stderr)
# 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), file=sys.stderr)
                msg = OSC.OSCMessage()
                msg.setAddress("/dot")
                #msg.append(dx)
                #msg.append(dy)
                #msg.append(m)
                msg.append(normx)
                msg.append(normy)
                try:
                    client.send(msg)
                except OSC.OSCClientError:
                    print ("Unable to send OSC", file=sys.stderr)

            # 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__':

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

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

    print ("Starting main camera loop", file=sys.stderr)
    # connect to pd
    # Init OSC
    while True:
        try:
                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
                break
        except OSC.OSCClientError:
            print ("Unable to connect via OSC to pd, trying again in 5", file=sys.stderr)
            time.sleep(5)

    prevgray = None
    for frame in cam.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)
        if prevgray != None:
            flow = cv2.calcOpticalFlowFarneback(prevgray, gray, 0.5, 3, 15, 3, 5, 1.2, 0)
            send_flow0(gray, flow)
        prevgray = gray
        # clear the stream in preparation for the next frame
        rawCapture.truncate(0)

        # cv2.imshow('flow', )

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