import pyrealsense2 as rs
import numpy as np
import cv2
import matplotlib.pyplot as plt

def get_image():
    # Initialize pipeline
    pipeline = rs.pipeline()
    config = rs.config()

    config.enable_stream(rs.stream.color, 1920, 1080, rs.format.bgr8, 30)

    # Start streaming
    pipeline.start(config)

    try:
        for _ in range(5):
            pipeline.wait_for_frames()

        # Capture a frame
        frames = pipeline.wait_for_frames()
        color_frame = frames.get_color_frame()

        if not color_frame:
            raise RuntimeError("Could not acquire color frame.")

        # Convert to np array
        image = np.asanyarray(color_frame.get_data())
        image = cv2.convertScaleAbs(image, alpha=1.5, beta=30)

        # Save image
        # cv2.imwrite("image.jpg", image)

    finally:
        # Stop streaming
        pipeline.stop()

    return image


def image_and_crop(image, show_image=False):
    #grayscale
    image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
    #crop image to needed two blocks
    black = image[179:685,857:1211]
    white = image[201:688, 1244:1752]
    if show_image:
        plt.imshow(image)
        plt.show()
    return black, white