#!/usr/bin/python
import cv2
import time
import numpy as np
import rospy
from std_msgs.msg import String
from nsra_odrive_interface.srv import *
import multiprocessing as mp
import yaml
import sys
from threading import Thread

rospy.init_node('corner_detect_right')

corner_left_x = 0.0
corner_left_y = 0.0

corner_right_x = 0.0
corner_right_y = 0.0

criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

class camera():

    def __init__(self,rtsp_url):        
        #load pipe for data transmittion to the process
        self.parent_conn, child_conn = mp.Pipe()
        #load process
        self.p = mp.Process(target=self.update, args=(child_conn,rtsp_url))        
        #start process
        self.p.daemon = True
        self.p.start()

    def end(self):
        #send closure request to process

        self.parent_conn.send(2)

    def update(self,conn,rtsp_url):
        #load cam into seperate process

        print("Cam Loading...")
        cap = cv2.VideoCapture(rtsp_url,cv2.CAP_FFMPEG)   
        print("Cam Loaded...")
        run = True

        while run:

            #grab frames from the buffer
            cap.grab()

            #recieve input data
            rec_dat = conn.recv()


            if rec_dat == 1:
                #if frame requested
                ret,frame = cap.read()
                conn.send(frame)

            elif rec_dat ==2:
                #if close requested
                cap.release()
                run = False

        print("Camera Connection Closed")        
        conn.close()

    def get_frame(self,resize=None):
        ###used to grab frames from the cam connection process

        ##[resize] param : % of size reduction or increase i.e 0.65 for 35% reduction  or 1.5 for a 50% increase

        #send request
        self.parent_conn.send(1)
        frame = self.parent_conn.recv()

        #reset request 
        self.parent_conn.send(0)

        #resize if needed
        if resize == None:            
            return frame
        else:
            return self.rescale_frame(frame,resize)

    def rescale_frame(self,frame, percent=65):

        return cv2.resize(frame,None,fx=percent,fy=percent) 

cam_left = camera(str(sys.argv[1]))
cam_right = camera(str(sys.argv[2]))

img_left = cam_left.get_frame(0.65)
img_right = cam_right.get_frame(0.65)

def findCBC_LEFT(img):
    global corner_left_x
    global corner_left_y
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, (6,9), None)
    if ret == True:
        corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
        corner_left_x = corners2[0,0,0]
        corner_left_y = corners2[0,0,1]
    else:
        print("No corners detected in the left image!")    

def findCBC_RIGHT(img):
    global corner_right_x
    global corner_right_y
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, (6,9), None)
    if ret == True:
        corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
        corner_right_x = corners2[0,0,0]
        corner_right_y = corners2[0,0,1]
    else:
        print("No corners detected in the left image!")   

def get2dcoords(req):
    t1 = Thread(target=findCBC_LEFT,args=([img_left]))
    t2 = Thread(target=findCBC_RIGHT,args=([img_right]))

    t1.start()
    t2.start()

    t1.join()
    t2.join()

    return coordsResponse(corner_left_x, corner_left_y, corner_right_x, corner_right_y)
    
s = rospy.Service('get2dcoords', coords, get2dcoords)

while True:
    img_left = cam_left.get_frame(0.65)
    img_right = cam_right.get_frame(0.65)

    cv2.imshow('left camera', img_left)
    cv2.imshow('right camera', img_right)

    key = cv2.waitKey(1)
    if key == 27:
        break

cv2.destroyAllWindows()     

cam_left.end()
cam_right.end()
