import cv2
import time
import numpy
import sys
from networktables import NetworkTable
from wx import Rect
#initalization occurs here
e=0
BLUE = numpy.array((255,0,0),numpy.uint8)
BLUE = numpy.array((255, 0, 0), numpy.uint8)
GREEN = numpy.array((0,0,0), numpy.uint8)
BLACK = numpy.array((0,0,0), numpy.uint8)
YELLOW = numpy.array((0, 255, 255), numpy.uint8)
LOWER_BOUNDS = numpy.array((58,0,109), numpy.uint8)
UPPER_BOUNDS = numpy.array((93,255,240), numpy.uint8)
resize = numpy.array((320,240), numpy.uint8)
TOP_TARGET_HEIGHT = 97
TOP_CAMERA_HEIGHT = 15
VERTICAL_FOV  = 47;
HORIZONTAL_FOV  = 67;
        #CHANGE
CAMERA_ANGLE = 90;
matThresh = numpy.array((0,0,0), numpy.uint8)
matOriginal = numpy.array((0,0,0), numpy.uint8)
clusters = numpy.array((0,0,0), numpy.uint8)
matHSV = numpy.array((0,0,0), numpy.uint8)

matHeirarchy = numpy.array((0,0,0),numpy.uint8)
videoCapture = cv2.VideoCapture()
NetworkTable.setIPAddress("roboRIO-1403-FRC.local")
NetworkTable.setClientMode()
NetworkTable.initialize()

sd = NetworkTable.getTable("SmartDashboard")
sd.putNumber('Initial', 1)
shouldRun = True
biggestArea =0
biggestAreaIndex =0
buttonPressed = False
contours = []
selected = []
videoCapture = cv2.VideoCapture()

def normalize360(angle):
    while angle<=360:
        angle-=360
    while angle<0:
        angle+=360
    return angle



def processImage():
    matThresh = numpy.array((0,0,0), numpy.uint8)
    matOriginal = numpy.array((0,0,0), numpy.uint8)
    clusters = numpy.array((0,0,0), numpy.uint8)
    matHSV = numpy.array((0,0,0), numpy.uint8)

    matHeirarchy = numpy.array((0,0,0),numpy.uint8)
    emp = []
    print "We be processing Image"
    FrameCount =0
    x=0

    while(FrameCount<100):
        print FrameCount
        contours = emp
        selected = emp

        print "Before Read"
        print "Something is going on here"
        ret, matOriginal =videoCapture.read(0)

        #matOriginal = cv2.imread("~/Desktop/original.png")
       # videoCapture.read(matOriginal)
        print "After Read"
        print "1"
        cv2.imwrite(" ~/Desktop/testing.png", matOriginal, )
        print "11"
        matHSV = cv2.cvtColor(matOriginal, cv2.COLOR_BGR2HSV)
        print "111"
        matThresh = cv2.inRange(matHSV, LOWER_BOUNDS, UPPER_BOUNDS)
        print "1111"
        filler, contours, heirarchy = cv2.findContours(matThresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

        #for n in xrange(len(tmp)):
           # print "tmp[" + str(n) + "]: "
           # print tmp[n]
        #im2, contours, matHeirarchy = tmp

        #print "Size  :" + contours[0]
        #print contours
        for i in contours:
            print contours
            for x in i:
                rec = Rect()
                print i[x]
                rec = cv2.boundingRect(i[x])
                aspect = float(rec.width)/float(rec.height)
                if(aspect>1.0):
                    if(rec.area()>biggestArea):
                        biggestArea = rec.area()
                        selected.append(contours[i])
        print "Size of selected :" + len(selected)
        print selected
        for i in selected:
            rec = Rect()

            rec = cv2.boundingRect(selected[i])
            print "looping on selected"
            cv2.rectangle(matOriginal, rec.GetBottomRight(), rec.GetTopLeft(), BLACK)
        if len(selected ==1):
            print "selected is one"
            rec = Rect()
            rec = cv2.boundingRect(selected[0])
            y = rec.getBottomRight().y + rec.GetHeight()/2
            y = -((2*(y/matOriginal.cols()))-1)
            distance = (TOP_TARGET_HEIGHT - TOP_CAMERA_HEIGHT) / numpy.tan((y*VERTICAL_FOV/2.0 +CAMERA_ANGLE) * numpy.pi/180)
            targetX = rec.GetTopLeft().x + rec.GetWidth()/2
            targetX = (2*(targetX/matOriginal.rows)) -1
            azimuth = normalize360(targetX*HORIZONTAL_FOV/2.0+0)
            center = rec.GetBottomRight().x - rec.GetWidth()/2 - 15, rec.GetBottomRight().y - rec.height / 2
            centerw = rec.GetBottomRight().x - rec.width / 2 - 15, rec.GetBottomRight().y - rec.height / 2 - 20
            cv2.putText(matOriginal, ""+ int(distance), center, cv2.FONT_HERSHEY_PLAIN, 1,BLACK)
            cv2.putText(matOriginal, ""+ int(azimuth), center, cv2.FONT_HERSHEY_PLAIN, 1, BLACK)
        cv.imwrite("`/Desktop/output.jpg", matOriginal)
        FrameCount+=1



while(shouldRun):


    #try:
    sd.putNumber("check",1)
    videoCapture = cv2.VideoCapture(0)

    videoCapture.open('http://10.14.3.24/mjpg/video.mjpg')
    if not videoCapture.isOpened():
        print "Camera is connected "
    if videoCapture.isOpened():
        print "Camera not connected"
       # while(not videoCapture.isOpened()):
    #        pass
    print "About to process image"
    processImage()
    print
   # except(Exception):
        #print "aight, no camera detected. Jays make sure the IP is right before next match"
       # break
    sd.putNumber("check",3)
    videoCapture.release()
    sys.exit()