import numpy as np
import json
import pyrealsense2 as rs

from .exceptions import InvalidSpecError

DEPTH_MIN = 0.11
DEPTH_MAX = 10.0
EXPANSION_FACTOR = 0.7


def _read_spec_value(model_loc, key):
    try:
        with open("{}/spec.json".format(model_loc), "r") as f:
            spec = json.load(f)

        return spec[key]
    except (FileNotFoundError, KeyError):
        raise InvalidSpecError("Invalid spec file. Try upgrading your model.")


def model_location():
    try:
        import paravision_models.liveness

        return paravision_models.liveness.location()
    except ImportError:
        raise ImportError("You need to install Paravision Liveness Models package")


def expand_bbox_to_edge_and_crop(depth_frame, proj_depth_bb):
    h, w = depth_frame.shape[:2]
    exp_bbox = _expand_bbox_to_edges(h, w, proj_depth_bb)
    cropped = _crop(depth_frame, exp_bbox)
    return cropped


def estimate_depth_bounding_box(bb, camera_params):
    height = camera_params.depth_intr.height
    width = camera_params.depth_intr.width

    left_corner = [bb.top_left.x, bb.top_left.y]
    right_corner = [bb.bottom_right.x, bb.bottom_right.y]

    left_depth_corner = _compute_epipolar_midpoint(
        height, width, left_corner, camera_params
    )
    right_depth_corner = _compute_epipolar_midpoint(
        height, width, right_corner, camera_params
    )
    proj_bb = np.hstack([left_depth_corner, right_depth_corner])
    return proj_bb


def _get_depth_point(height, width, pt, scale, camera_params):
    color_world_pt = rs.rs2_deproject_pixel_to_point(
        camera_params.color_intr, pt, scale
    )
    depth_world_pt = rs.rs2_transform_point_to_point(
        camera_params.color_to_depth_extr, color_world_pt
    )
    depth_pt = rs.rs2_project_point_to_pixel(camera_params.depth_intr, depth_world_pt)
    depth_pt = rs.adjust_2D_point_to_boundary(depth_pt, width, height)
    return depth_pt


def _compute_epipolar_midpoint(height, width, pt, camera_params):
    # define depth endpoints of epipolar line to search
    start_depth_pt = _get_depth_point(height, width, pt, DEPTH_MIN, camera_params)
    end_depth_pt = _get_depth_point(height, width, pt, DEPTH_MAX, camera_params)

    mid_pt = (np.array(start_depth_pt) + np.array(end_depth_pt)) / 2
    return mid_pt


def _expand_bbox_to_edges(h, w, bbox):
    x1, y1, x2, y2 = bbox
    dx = (x2 - x1) * EXPANSION_FACTOR / 2
    dy = (y2 - y1) * EXPANSION_FACTOR / 2
    x1_ = max(0, x1 - dx)
    y1_ = max(0, y1 - dy)
    x2_ = min(w, x2 + dx)
    y2_ = min(h, y2 + dy)
    return _round(np.array([x1_, y1_, x2_, y2_]))


def _crop(np_img, bbox):
    bbox = _round(bbox)
    x1, y1, x2, y2 = bbox
    h, w = np_img.shape[:2]
    x1 = max(x1, 0)
    y1 = max(y1, 0)
    x2 = min(x2, w)
    y2 = min(y2, h)

    return np_img[y1:y2, x1:x2]


def _round(bbox):
    return np.rint(bbox).astype(np.int32)