# Copyright 3D Control Systems, Inc. All Rights Reserved 2017-2019.
# Built in San Francisco.

# This software is distributed under a commercial license for personal,
# educational, corporate or any other use.
# The software as a whole or any parts of it is prohibited for distribution or
# use without obtaining a license from 3D Control Systems, Inc.

# All software licenses are subject to the 3DPrinterOS terms of use
# (available at https://www.3dprinteros.com/terms-and-conditions/),
# and privacy policy (available at https://www.3dprinteros.com/privacy-policy/)

import os
import logging
import time

try:
    if os.environ.get("OPENCV_LOG_LEVEL") != "FATAL":
        os.environ["OPENCV_LOG_LEVEL"] = "FATAL"
except:
    pass

import cv2

import base_cam
import config
import log
import paths
import platforms
import printer_settings_and_id


class OpenCVCaptureWrapper(base_cam.BaseCaptureWrapper):

    ID = 'CV'

    ENCODE_PARAMS = ( cv2.IMWRITE_JPEG_QUALITY, base_cam.BaseCaptureWrapper.QUALITY )
    IMAGE_EXT = ".jpg"
    SYS_VIDEO4LINUX_PATH_TEMPL = "/sys/class/video4linux/{video_n}/name"
    # DEFAULT_MODE = config.get_settings()['camera'].get('persistent_capture', False)
    ALLOW_HARDWARE_RESIZE = config.get_settings()["camera"]["hardware_resize"]
    FOCUS = config.get_settings()["camera"].get("focus")
    EXPOSURE = config.get_settings()["camera"].get("exposure")
    SKIP_FPS_SET = config.get_settings()["camera"].get("skip_fps_set")
    MAX_FAILS = 3
    RETRY_SLEEP = 1
    FOCUS = None
    EXPOSURE = None

    # EMPTY_FRAME = np.zeros((480, 640, 3), np.uint8)

    @staticmethod
    def get_backend():
        try:
            if platforms.get_platform() in ('rpi', 'linux'):
                return cv2.CAP_V4L2 #prevents choose of GSTREAMER 
        except (ValueError, TypeError):
            pass

    @staticmethod
    def make_camera_name(cap_id, camera_number):
        if isinstance(cap_id, str) and OpenCVCaptureWrapper.SCHEMA_STRING in cap_id:
            if OpenCVCaptureWrapper.AMPERSANT in cap_id:
                return "IP:" + str(cap_id.split(OpenCVCaptureWrapper.AMPERSANT)[1])
            else:
                return "IP:" + str(cap_id.split(OpenCVCaptureWrapper.SCHEMA_STRING)[1])
        if platforms.get_platform() in ('rpi', 'linux'):
            try:
                if isinstance(cap_id, str):
                    if os.path.islink(cap_id):
                        video_n_path = os.readlink(cap_id)
                    else:
                        video_n_path = cap_id
                    video_n = str(os.path.basename(video_n_path))
                elif isinstance(cap_id, int):
                    video_n = 'video' + str(cap_id)
                else:
                    return 'CV:' + str(cap_id)
                name_filepath = OpenCVFFMPEGCaptureWrapper.SYS_VIDEO4LINUX_PATH_TEMPL.format(video_n=video_n)
                if os.path.exists(name_filepath):
                    with open(name_filepath) as f:
                        name = f.read().strip()
                        parts = name.split(":")
                        if len(parts) == 2:
                            if parts[0].strip() == parts[1].strip():
                                name = parts[0]
                        return name
            except (OSError, AttributeError):
                pass
            except:
                logging.getLogger('CVWrapper').exception('Exception on make_camera_name:')
        return 'CV:' + str(cap_id)

    def __init__(self, parent, cap_id):
        super().__init__(parent, cap_id)
        self.fps = 0
        self.dynamic_skip = config.get_settings()['camera']['dynamic_skip'] and not self.SKIP_FPS_SET
        self.frame_skip = config.get_settings()['camera']['frame_skip']
        if self.camera_name == 'camera0': # a hacky solution for RPi camera that requires no frame skip
            self.dynamic_skip = False
            self.frame_skip = 0
        self.backend = self.get_backend()
        self.need_software_resize = True
        self.current_resolution = (0, 0)
        self.resize_resolution = (self.X_RESOLUTION, self.Y_RESOLUTION)
        # measured by first several grabs, but only for USB cameras, since FFMPEG backend got a dummy in grab code
        self.avg_grab_time = 0.0

    def set_fps(self, capture):
        if self.FPS and not self.SKIP_FPS_SET:
            try:
                capture.set(cv2.CAP_PROP_FPS, self.FPS)
            except:
                self.logger.warning('Unable to set FPS for: ' + str(capture))
        try:
            fps = capture.get(cv2.CAP_PROP_FPS)
            self.logger.info("Camera FPS: " + str(self.fps))
        except:
            self.logger.warning('Unable to get FPS for: ' + str(capture))

    def set_resolution(self, capture):
        try:
            w = capture.get(cv2.CAP_PROP_FRAME_WIDTH)
            h = capture.get(cv2.CAP_PROP_FRAME_HEIGHT)
            if not w or not h or w < self.REAL_RES_THRESHOLD or h < self.REAL_RES_THRESHOLD:
                raise ValueError(f'Got invalid resolution values {w}x{h} for {self.cap_id}')
            self.current_resolution = (w, h)
        except Exception as e:
            self.logger.debug(f'Error getting resolution of {self.cap_id}: {e}')
        else:
            if w == self.X_RESOLUTION and h == self.Y_RESOLUTION:
                return True
            if self.ALLOW_HARDWARE_RESIZE:
                try:
                    capture.set(cv2.CAP_PROP_FRAME_WIDTH, self.X_RESOLUTION)
                    capture.set(cv2.CAP_PROP_FRAME_HEIGHT, self.Y_RESOLUTION)
                except Exception as e:
                    self.logger.debug(f'Error setting resolution of {self.cap_id}: {e}')
        try:
            w = capture.get(cv2.CAP_PROP_FRAME_WIDTH)
            h = capture.get(cv2.CAP_PROP_FRAME_HEIGHT)
            self.current_resolution = (w, h)
            return w == self.X_RESOLUTION and h == self.Y_RESOLUTION
        except Exception as e:
            self.logger.debug(f'Error getting resolution of {self.cap_id}: {e}')
        return False

    def set_aux_capture_props(self, capture):
        if self.FOCUS is not None:
            try:
                capture.set(cv2.CAP_PROP_AUTOFOCUS, 0)
            except:
                self.logger.error(f'Error disabling autofocus for {self.cap_id}')
            try:
                capture.set(cv2.CAP_PROP_FOCUS, self.FOCUS)
            except:
                self.logger.error(f'Error setting focus({self.FOCUS}) for {self.cap_id}')
            try:
                self.logger.info("Camera focus: " + str(capture.get(cv2.CAP_PROP_FOCUS)))
            except:
                pass
        if self.EXPOSURE is not None:
            try:
                capture.set(cv2.CAP_PROP_AUTO_EXPOSURE, 3) # 3 is manual exposure
            except:
                self.logger.error(f'Error setting exposure({self.FPS}) for {self.cap_id}')
            try:
                capture.set(cv2.CAP_PROP_EXPOSURE, self.EXPOSURE)
            except:
                self.logger.error(f'Error setting exposure({self.EXPOSURE}) for {self.cap_id}')
            try:
                self.logger.info("Camera exposure: " + str(capture.get(cv2.CAP_PROP_EXPOSURE)))
            except:
                pass

    def create_capture(self):
        args = []
        if self.backend:
            args = [self.backend]
        self.logger.debug(f"Probing capture {self.cap_id}")
        try:
            capture = cv2.VideoCapture(self.cap_id, *args)
            capture.setExceptionMode(False)
        except Exception as e:
            if self.parent and self.parent.debug:
                self.logger.exception('Exception on capture creation: ' + str(e))
            else:
                self.logger.error('Exception on capture creation: ' + str(e))
        else:
            if capture.isOpened():
                self.logger.warning('Created OpenCV capture for ' + str(self.cap_id))
                self.operational = True
                self.set_fps(capture)
                self.need_software_resize = self.set_resolution(capture)
                if self.need_software_resize:
                    self.resize_resolution = self.get_resize_resolution(*self.current_resolution)
                self.set_aux_capture_props(capture)
                return capture
        self.operational = False
        self.logger.warning('Unable to create OpenCV capture for ' + str(self.cap_id))
    
    @log.log_exception
    def frame_loop(self):
        if not self.capture:
            self.capture = self.create_capture()
        while not self.stop_flag and self.capture:
            if self.fails > self.MAX_FAILS:
                self.logger.error(f"Reached max failed count for {self.cap_id}. Stopping")
                self.fails = 0
                break
            try:
                frame_skip = self.frame_skip
                grab_success = False
                avg_grab_time = None
                while frame_skip > -1:
                    if self.dynamic_skip and not self.avg_grab_time:
                        grab_start_time = time.monotonic()
                        grab_success |= self.capture.grab()
                        if not avg_grab_time:
                            avg_grab_time = time.monotonic() - grab_start_time
                        else:
                            avg_grab_time = (avg_grab_time + time.monotonic() - grab_start_time) / 2.0
                        if frame_skip > -1:
                            frame_skip -= 1
                        else:
                            self.avg_grab_time = avg_grab_time
                            self.frame_skip = int(1/self.FPS/avg_grab_time) - 1
                    else:
                        grab_success |= self.capture.grab()
                        frame_skip -= 1
                if grab_success:
                    success, frame = self.capture.retrieve()
                    # self.frame_consumed = False
                    if success:
                        # on kernel 6.12rpi picamera returns a single dimension array
                        if frame.shape[0] == 1:
                            if frame.shape[1] == 921600: # 640x480x3:
                                frame = frame[0].reshape(480, 640, 3)
                            elif frame.shape[1] == 2764800: # 1280x720x3:
                                frame = frame[0].reshape(720, 1280, 3)
                            elif frame.shape[1] == 6220800: # 1920x1080x3:
                                frame = frame[0].reshape(1080, 1920, 3)
                        with self.frame_lock:
                            self.frame = frame
                            self.fails = 0
                        if self.avg_grab_time:
                            time.sleep(self.avg_grab_time / 2.0)
                        continue
                self.logger.error(f"Failed to read frame from {self.cap_id}. Retrying...")
            except Exception as e:
                self.logger.error(f"Exception occurred while capturing frame from {self.cap_id}: {e}")
            self.fails += 1
            with self.frame_lock:
                self.frame = None
            time.sleep(self.RETRY_SLEEP)
        if self.capture is not None:
            try:
                self.capture.release()
                self.capture = None
                self.logger.info(f'Capture {self.cap_id} released')
            except Exception as e:
                self.logger.error(f"Exception occurred while releasing capture from {self.cap_id}: {e}")
        self.deactivation_cleanup()

    def get_resize_resolution(self, x, y):
        if x > self.X_RESOLUTION or y > self.Y_RESOLUTION:
            if x / y != self.KOEF_RESOLUTION:
                proportion = min(self.X_RESOLUTION / x, self.Y_RESOLUTION / y)
                return round(x * proportion), round(y * proportion)
        return self.X_RESOLUTION, self.Y_RESOLUTION

    def software_resize(self, frame):
        self.logger.debug("Resizing frame " + str(self.camera_name))
        try:
            frame = cv2.resize(frame, self.resize_resolution, interpolation=cv2.INTER_NEAREST)
        except Exception as e:
            self.logger.error("Error while software resize of frame: " + str(e))
            self.fails += 1
        return frame

    def get_frame(self):
        with self.frame_lock:
            self.frame_consumed = True
            if self.frame is not None and self.frame.any():
                if self.need_software_resize:
                    self.frame = self.software_resize(self.frame)
                try:
                    encode_success, frame_nparray = cv2.imencode(self.IMAGE_EXT, self.frame, self.ENCODE_PARAMS)
                except Exception as e:
                    self.logger.warning('Frame encoding exception: ' + str(e))
                else:
                    if encode_success: 
                        return frame_nparray.tobytes()
                self.logger.warning('Frame encode failed. Len: ' + str(len(self.frame)))
            return b""

    def close(self):
        self.stop_flag = True
        with self.frame_lock:
            if self.capture:
                try:
                    self.capture.release()
                except:
                    self.logger.warning('Error on capture release ' + str(self.camera_name))


class OpenCVFFMPEGCaptureWrapper(OpenCVCaptureWrapper):

    ID = 'FFMPEG'

    @staticmethod
    def get_backend():
        return None # force backend autodetection

    def __init__(self, parent, cap_id):
        super().__init__(parent, cap_id)
        self.dynamic_skips = False
        self.frame_skips = 0


class CvUsbDetector(base_cam.BaseCamDetector):

    CAPTURE_WRAPPER_CLASS = OpenCVCaptureWrapper
    KEEP_LIVE_CAPS = True
    MAX_CAMERA_INDEX = 10
    DEV_PATH = '/dev'
    DEV_BYPATH_PATH = DEV_PATH + '/v4l/by-path/'
    DEV_BYID_PATH = DEV_PATH + '/v4l/by-id/'

    @staticmethod
    def get_camera_video_devices():
        return [ os.path.join(CvUsbDetector.DEV_PATH, filename) for filename in os.listdir(CvUsbDetector.DEV_PATH) if filename.startswith('video') ]

    def check_capture(self, cap_id, backend):
        try:
            capture = cv2.VideoCapture(cap_id, backend)
            if capture.isOpened():
                self.logger.debug("...got camera " + str(cap_id))
                capture.release()
                return True
            else:
                self.logger.debug(f"Camera at index {cap_id} can't be opened")
        except: #opencv exceptions are wild and hard to predict
            self.logger.debug(f"Camera at index {cap_id} can't be opened")
        return False

    def get_videon_from_bypath(self, path):
        try:
            return os.readlink(path)
        except:
            pass

    def get_capture_ids(self):
        capture_ids = []
        platform = platforms.get_platform()
        if platform in ('rpi', 'linux'):
            backend = cv2.CAP_V4L2
        else:
            backend = None
        if config.get_settings()['camera'].get('usb_input'):
            if platform in ('rpi', 'linux'):
                used_video_n_devices = set()
                failed_or_dubl_n_devices = set()
                all_video_n_devices = self.get_camera_video_devices()
                bcm_in_devices = False
                if config.get_settings()['camera'].get('id_by_path'):
                    try:
                        # picamera got invalid by-path leading to a useless for us devices that are not cameras
                        # any by-path device can point to an unusable capture
                        # or be just duplication of existing with usb/usbv2 
                        # suffix of -indexN usually means another mode for a camera, like MJPEG or H264 mode
                        if os.path.isdir(self.DEV_BYPATH_PATH):
                            for filename in os.listdir(self.DEV_BYPATH_PATH):
                                if 'bcm2835' in filename or 'mailbox-video' in filename: # those all are not captures
                                    # we have picamera
                                    bcm_in_devices = True
                                    failed_or_dubl_n_devices.add(self.get_videon_from_bypath(filename))
                                    continue
                                if not self.KEEP_LIVE_CAPS or filename not in self.parent.captures:
                                    path = os.path.join(self.DEV_BYPATH_PATH, filename)
                                    videon = self.get_videon_from_bypath(path)
                                    if videon not in used_video_n_devices and videon not in failed_or_dubl_n_devices:
                                        if self.check_capture(path, backend):
                                            used_video_n_devices.add(videon)
                                            capture_ids.append(path)
                                        else:
                                            failed_or_dubl_n_devices.add(videon)
                    except OSError:
                        self.logger.warning('Error on reading v4l paths')
                    # search for picamera
                    if bcm_in_devices and platform == 'rpi':
                        for dev in all_video_n_devices:
                            video_n = os.path.basename(dev)
                            name_path = OpenCVFFMPEGCaptureWrapper.SYS_VIDEO4LINUX_PATH_TEMPL.format(video_n=video_n)
                            try:
                                with open(name_path) as f:
                                    if 'bcm2835' in f.read(): # those all are not captures
                                        continue
                            except OSError:
                                pass
                            if dev not in used_video_n_devices and dev not in failed_or_dubl_n_devices:
                                if self.check_capture(dev, backend):
                                    capture_ids.append(dev)
                                    break #only one picamera
                    return capture_ids
            for index in range(0, self.MAX_CAMERA_INDEX):
                if not self.KEEP_LIVE_CAPS or index not in self.parent.captures:
                    if self.check_capture(index, backend):
                        capture_ids.append(index)
            self.logger.info("Got cameras: " + str(capture_ids))
            self.captures_ids = capture_ids
        if not self.inited:
            self.logger.info(self.__class__.__name__ + " detected: " + str(capture_ids))
            self.inited = True
        return capture_ids


class CvUrlListDetector(base_cam.BaseCamDetector):

    CAPTURE_WRAPPER_CLASS = OpenCVFFMPEGCaptureWrapper
    KEEP_LIVE_CAPS = False
    IS_DEFAULT_FOR_EMPTY_CLASS = True
    FILES = (paths.CAMERA_URLS_FILE, paths.USER_CAMERA_URLS_FILE)

    @classmethod
    def load_camera_urls_file(cls, files=FILES, retries=5, full_string=False):
        urls = []
        for filepath in files:
            try:
                if os.path.isfile(filepath):
                    with open(filepath) as f:
                        for camera_ulr_line in f:
                            line_words = camera_ulr_line.strip().split()
                            if line_words:
                                if (len(line_words) > 1 and line_words[1] == cls.CAPTURE_WRAPPER_CLASS.ID) or \
                                    cls.IS_DEFAULT_FOR_EMPTY_CLASS and len(line_words) == 1: 
                                    if line_words[0] not in urls:
                                        if full_string:
                                            urls.append(line_words)
                                        else:
                                            urls.append(line_words[0])
            except:
                retries -= 1
                if retries:
                    time.sleep(0.1)
                    return cls.load_camera_urls_file(files=files, retries=retries, full_string=full_string)
        return urls

    def load_cameras_from_printer_settings_files(self):
        try:
            all_printers_settings = printer_settings_and_id.load_all_printer_settings()
            for printer_settings in all_printers_settings.values():
                camera_dict = printer_settings.get('camera')
                if camera_dict:
                    try:
                        url = str(camera_dict.get('url'))
                        number = int(camera_dict.get('number'))
                        cam_type = camera_dict.get('type')
                    except (ValueError, AttributeError):
                        continue
                    if (not cam_type and self.IS_DEFAULT_FOR_EMPTY_CLASS) or \
                        camera_dict == self.CAPTURE_WRAPPER_CLASS.ID:
                        yield url, number
        except Exception as e:
            self.logger.error(f'Error reading camera urls file: {e}')

    def get_capture_ids(self):
        capture_ids = []
        if config.get_settings()['camera'].get('network_input'):
            for url in self.load_camera_urls_file():
                if url not in capture_ids:
                    capture_ids.append(url)
            # for url, _ in self.load_cameras_from_printer_settings_files():
            #     if url not in capture_ids:
            #         capture_ids.append(url)
        if not self.inited:
            self.logger.debug(self.__class__.__name__ + " detected: " + str(capture_ids))
            self.inited = True
        self.capture_ids = capture_ids
        return capture_ids
