## License: Apache 2.0. See LICENSE file in root directory.
## Copyright(c) 2015-2017 Intel Corporation. All Rights Reserved.

###############################################
##      Open CV and Numpy integration        ##
###############################################

import pyrealsense2 as rs
import numpy as np
import cv2
import socket
import math
import pickle
import sys
import time
import datetime

max_length = 65000
host =  "192.168.50.158"
port = 5000

sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)

pipeline.start(config)

try:
    while True:
        frames = pipeline.wait_for_frames()
        depth_frame = frames.get_depth_frame()
        color_frame = frames.get_color_frame()
        if not depth_frame or not color_frame:
            continue

        depth_image = np.asanyarray(depth_frame.get_data())
        color_image = np.asanyarray(color_frame.get_data())

        depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)

        text_depth = "depth value of point is " + str(
        np.round(depth_frame.get_distance(320, 240), 4)) + "meter(s)"
        color_image = cv2.circle(color_image, (320, 240), 1, (0, 255, 255), 10)
        color_image = cv2.putText(color_image, text_depth, (10, 20), cv2.FONT_HERSHEY_PLAIN, 2, (0, 0, 255), 1,
                                  cv2.LINE_AA)

        images = np.hstack((color_image, depth_colormap))
        images = cv2.resize(images, (640,320),interpolation=cv2.INTER_AREA)
        retval, buffer = cv2.imencode(".jpg", images)

        if retval:
            # convert to byte array
            buffer = buffer.tobytes()
            # get size of the frame
            buffer_size = len(buffer)

            num_of_packs = 1
            if buffer_size > max_length:
                num_of_packs = math.ceil(buffer_size / max_length)

            frame_info = {"packs": num_of_packs}
            # send the number of packs to be expected
            print("Number of packs:", num_of_packs)
            sock.sendto(pickle.dumps(frame_info), (host, port))

            left = 0
            right = max_length

            for i in range(num_of_packs):
                print("left:", left)
                print("right:", right)

                # truncate data to send
                data = buffer[left:right]
                left = right
                right += max_length

                # send the frames accordingly
                sock.sendto(data, (host, port))




finally:
    pipeline.stop()