#!/usr/bin/env python

import roslib
import rospy
 
from sensor_msgs.msg import Image   # for receiving the video feed
import cv2
import numpy as np
from std_msgs.msg import String
import os
import time
from object_detection import Vision_Processor
from image_converter import ToOpenCV
from cv_bridge import CvBridge, CvBridgeError

KINECT_MAX_DEPTH = 5.55
KINECT_MIN_DEPTH = 0.35

class Kinect_Ros ():
    def __init__ (self):
        rospy.init_node('Kinect_Ros', anonymous=True)
        self.confirm = 0 
        self.seeTarget = 0
        self.bridge = CvBridge ()
        #self.depthSub = rospy.Subscriber('/camera/depth/image', Image, self.saveDepthImage)
        #self.rgbSub = rospy.Subscriber('/camera/rgb/image_raw', Image, self.saveRgbImage)
        self.vision = Vision_Processor()
        self.logfile = open('error.log', 'w')
        self.image_sub = rospy.Subscriber('camera/rgb/image_raw', Image, self.image_callback)
        rospy.spin()

    def image_callback(self, ros_image):
        self.cv_image = np.asarray(ToOpenCV(ros_image))
        self.vision.process_target(self.cv_image)
        cv2.imshow("window", self.cv_image)
        cv2.waitKey(3)

    def saveRgbImage (self, ros_image):
        self.cv_image = np.asarray(ToOpenCV(ros_image))
    
    
    def getDepth (self,x,y):
        try:
            return self.depth_array[y][x] 
        except:
            self.logfile.write("Error occured in getDepth...\n")
            return -1


    def saveDepthImage (self, data):
        depth_image = self.bridge.imgmsg_to_cv(data, '32FC1')
        self.depth_array = np.array(depth_image, dtype=np.float32)




kinect = Kinect_Ros()