#!/usr/bin/env python

import cv2
import numpy as np
import math
import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

frame = None
bridge = None
def nothing(x):
    pass

def trackbars():
    cv2.namedWindow('Tune')
    cv2.moveWindow('Tune',500,400)
    cv2.createTrackbar('HL','Tune',0,255,nothing)
    cv2.createTrackbar('HH','Tune',0,255,nothing)
    cv2.createTrackbar('SL','Tune',0,255,nothing)
    cv2.createTrackbar('SH','Tune',0,255,nothing)
    cv2.createTrackbar('VL','Tune',0,255,nothing)
    cv2.createTrackbar('VH','Tune',0,255,nothing)
    cv2.setTrackbarPos('HL','Tune',0)
    cv2.setTrackbarPos('HH','Tune',180)
    cv2.setTrackbarPos('SL','Tune',0)
    cv2.setTrackbarPos('SH','Tune',28)
    cv2.setTrackbarPos('VL','Tune',136)
    cv2.setTrackbarPos('VH','Tune',255)

def captureNextFrame(data):
    global frame,bridge
    try:
        frame = bridge.imgmsg_to_cv2(data, "bgr8")
    except CvBridgeError as e:
        print(e)
    print "callback"
    #frame=cv2.cvtColor(cv_image,cv2.COLOR_BGR2RGB)

def findDirection(roi,lines,side):
    if lines == None:
        if side == 0:
            return 10000, 0
        else:
            return 1,-10000
    avgDistX = avgDistY = avgSlope = 0
    for x in range(len(lines)):
        for x1,y1,x2,y2 in lines[x]:
            avgDistX += ((x1+x2)/2)
            avgDistY += ((y1+y2)/2)
            if(x2-x1):
                avgSlope += (y2-y1)*1.0/(x2-x1)
            else:
                avgSlope += (y2-y1)*1.0/0.0001
    avgDistX = avgDistX/len(lines)
    if side:
        avgDistX += roi.shape[1]*0.5
    avgDistY = avgDistY/len(lines)
    avgSlope = avgSlope/len(lines)
    print avgSlope
    if avgSlope == 0:
        avgSlope = 0.001
    if side == 0:
        cv2.circle(roi,(int(avgDistX),int(avgDistY)),5,(0,0,255))
        cv2.line(roi,(int(-avgDistY/avgSlope + avgDistX),0),(int((roi.shape[0] - avgDistY)/avgSlope + avgDistX),roi.shape[0]),(255,0,255),2)
    else:
        cv2.line(roi,(int(-avgDistY/avgSlope + avgDistX),0),(int((roi.shape[0] - avgDistY)/avgSlope + avgDistX),roi.shape[0]),(255,255,0),2)
        # cv2.line(roi,(int(roi.shape[1]*0.5)+int(-avgDistY/avgSlope + avgDistX),0),(int(roi.shape[1]*0.5)+int(roi.shape[0] - avgDistY/avgSlope + avgDistX),roi.shape[0]),(0,255,0),2)
    return avgSlope,-avgDistX/avgSlope + avgDistY

def findLines(roi,img,side):
    edges = cv2.Canny(img,200,200,apertureSize = 3)
    # cv2.imshow('Edges',edges)
    # cv2.moveWindow('Edges',100,400)

    # lines = cv2.HoughLines(edges,1,np.pi/180,50)
    # print lines
    # if lines!=None:
    #     for x in range(len(lines)):
    #         for rho,theta in lines[x]:
    #             a = np.cos(theta)
    #             b = np.sin(theta)
    #             x0 = a*rho
    #             y0 = b*rho
    #             x1 = int(x0 + 1000*(-b))
    #             y1 = int(y0 + 1000*(a))
    #             x2 = int(x0 - 1000*(-b))
    #             y2 = int(y0 - 1000*(a))
    #
    #             if side==0:
    #                 cv2.line(roi,(x1,y1),(x2,y2),(0,255,0),2)
    #             else:
    #                 cv2.line(roi,(int(roi.shape[1]*0.5)+x1,y1),(int(roi.shape[1]*0.5)+x2,y2),(0,0,255),2)

    minLineLength = 10
    maxLineGap = 10
    lines = cv2.HoughLinesP(edges,1,np.pi/180,1,minLineLength,maxLineGap)
    if lines!=None:
        for x in range(len(lines)):
            for x1,y1,x2,y2 in lines[x]:
                if side==0:
                    cv2.line(roi,(x1,y1),(x2,y2),(0,255,0),2)
                else:
                    cv2.line(roi,(int(roi.shape[1]*0.5)+x1,y1),(int(roi.shape[1]*0.5)+x2,y2),(0,0,255),2)
    return lines
    # lowRanges = [cv2.getTrackbarPos('HL','Tune'), cv2.getTrackbarPos('SL','Tune'), cv2.getTrackbarPos('VL','Tune') ]
    # highRanges = [cv2.getTrackbarPos('HH','Tune'), cv2.getTrackbarPos('SH','Tune'), cv2.getTrackbarPos('VH','Tune') ]
    # grayROI = cv2.cvtColor(roi,cv2.COLOR_BGR2GRAY)
    # ret2,binary = cv2.threshold(grayROI,0,255,cv2.THRESH_BINARY+cv2.THRESH_OTSU)
    # roiHSV = cv2.cvtColor(roi,cv2.COLOR_BGR2HSV)
    # binary = cv2.inRange(roiHSV,np.array(lowRanges),np.array(highRanges))
    # cv2.imshow('Binary',edges)
    # cv2.moveWindow('Binary',100,400)

def main():
    global frame, bridge
    pause = False
    rospy.init_node("MotorControlPanel")
    velocityPub = rospy.Publisher("/cmd_vel",Twist,queue_size=1)
    rospy.Subscriber("/rrbot/camera1/image_raw",Image, captureNextFrame)
    twistMsg = Twist()
    bridge = CvBridge()
    trackbars()
    #vid = cv2.VideoCapture('1.avi')
    while(True):
        #if not pause:
        #    ret, frame = vid.read()
        if frame==None:
            continue
        frameTemp = frame[int(0.5*frame.shape[0]):frame.shape[0],0:frame.shape[1]]
        cv2.imshow('Feed',frameTemp)
        cv2.moveWindow('Feed',100,100)
        roi = frameTemp.copy()
        roi = roi[int(0.5*roi.shape[0]):int(roi.shape[0]*0.8),0:roi.shape[1]]
        leftImage = roi[0:roi.shape[0],0:roi.shape[1]*0.5]
        rightImage =  roi[0:roi.shape[0],roi.shape[1]*0.5:roi.shape[1]]
        cv2.imshow('Left',leftImage)
        linesLeft = findLines(roi,leftImage,0)
        linesRight = findLines(roi,rightImage,1)
        m1,c1 = findDirection(roi,linesLeft,0)
        m2,c2 = findDirection(roi,linesRight,1)
        print m1,c1,m2,c2
        if(m1!=m2):
            x = (c2-c1)/(m1-m2)
        if x > roi.shape[1]*0.40 and x < roi.shape[1]*0.60:
            twistMsg.linear.x = 0.2
            twistMsg.angular.z = 0
            print "Forward",  x/roi.shape[1]
        elif x <= roi.shape[1]*0.40:
            twistMsg.linear.x = 0.2
            twistMsg.angular.z = 0.5
            print "Left", x/roi.shape[1]
        else:
            twistMsg.linear.x = 0.2
            twistMsg.angular.z = -0.5
            print "Right", x/roi.shape[1]

        velocityPub.publish(twistMsg)

        cv2.imshow('ROI',roi)
        cv2.moveWindow('ROI',100,500)
        c = cv2.waitKey(10)
        if c == 27:
            break
        elif c == 112:
            pause = not pause



if __name__=='__main__':
    main()