import numpy as np
import cv2
from panda3d.core import Point2, Point3
from panda3d.core import BoundingBox, BoundingHexahedron
from panda3d.core import VirtualFileSystem


def make_bounds(lens, scale_size=None, crop_size=None):
    """
    Allocates and returns a new BoundingVolume that encloses the frustum used
    for this kind of lens, if possible.  If a suitable bounding volume cannot
    be created, returns None.

    Same as Lens's make_bounds method except that the frustrum could be smaller
    by specifying a scale_size or crop_size.

    Original implementation of make_bounds is in here:
    https://github.com/panda3d/panda3d/blob/master/panda/src/gobj/lens.cxx
    """
    fll = Point3()
    flr = Point3()
    ful = Point3()
    fur = Point3()
    nll = Point3()
    nlr = Point3()
    nul = Point3()
    nur = Point3()

    film_size = lens.getFilmSize()
    scale_size = scale_size or 1.0
    crop_size = tuple(crop_size) or film_size
    ll = Point2(-crop_size[0] / scale_size / film_size[0], -crop_size[1] / scale_size / film_size[1])
    lr = Point2(+crop_size[0] / scale_size / film_size[0], -crop_size[1] / scale_size / film_size[1])
    ul = Point2(-crop_size[0] / scale_size / film_size[0], +crop_size[1] / scale_size / film_size[1])
    ur = Point2(+crop_size[0] / scale_size / film_size[0], +crop_size[1] / scale_size / film_size[1])

    # Upper left.
    if not lens.extrude(ul, nul, ful):
        return None

    # Upper right.
    if not lens.extrude(ur, nur, fur):
        return None

    # Lower right.
    if not lens.extrude(lr, nlr, flr):
        return None

    # Lower left.
    if not lens.extrude(ll, nll, fll):
        return None

    return BoundingHexahedron(fll, flr, fur, ful, nll, nlr, nur, nul)


def is_in_view(cam_node, obj_node, scale_size=None, crop_size=None):
    """
    Returns the intersection flag between the camera's frustrum and the
    object's tight bounding box.

    https://www.panda3d.org/forums/viewtopic.php?t=11704

    Intersection flags are defined in here:
    https://github.com/panda3d/panda3d/blob/master/panda/src/mathutil/boundingVolume.h
    """
    lens_bounds = make_bounds(cam_node.node().getLens(), scale_size=scale_size, crop_size=crop_size)
    bounds = BoundingBox(*obj_node.getTightBounds())
    bounds.xform(obj_node.getParent().getMat(cam_node))
    return lens_bounds.contains(bounds)


def project(lens, points3d):
    assert lens.isLinear()
    proj_mat = np.array(lens.getProjectionMat()).T
    points3d = np.asarray(points3d)
    points3d_full = np.c_[points3d, np.ones(len(points3d))]
    points2d_full = points3d_full.dot(proj_mat.T)
    points2d = points2d_full[:, :2] / points2d_full[:, 2][:, None]
    return points2d


def extrude_depth(lens, points2d):
    """
    Uses the depth component of the 3-d result from project() to compute the
    original point in 3-d space corresponding to a particular point on the
    lens.  This exactly reverses project(), assuming the point does fall
    legitimately within the lens.

    If points2d is an iterable of 3-d points, the first two dimensions of each
    point should be in the range (-1,1) in both dimensions, where (0,0) is the
    center of the lens and (-1,-1) is the lower-left corner. The last dimension
    of each point is the depth z from the depth buffer.

    If points2d is a depth map of shape (height, width, 3), the 2d coordinates
    are implicitly defined by the position within (height, width).
    """
    assert lens.isLinear()
    proj_mat_inv = np.array(lens.getProjectionMatInv()).T
    points2d = np.asarray(points2d)
    if points2d.ndim == 2 and points2d.shape[1] == 3:
        points2d_full = np.c_[points2d, np.ones(len(points2d))]
        points3d_full = points2d_full.dot(proj_mat_inv.T)
        points3d = points3d_full[:, :3] / points3d_full[:, 3][:, None]
    elif points2d.ndim == 3 and points2d.shape == (lens.getFilmSize()[1], lens.getFilmSize()[0], 1):
        x, y = np.meshgrid(np.arange(lens.getFilmSize()[0]), np.arange(lens.getFilmSize()[1]))
        points2d = np.concatenate([x[..., None], y[..., None], points2d], axis=-1)
        points3d = extrude_depth(lens, points2d.reshape((-1, 3))).reshape(points2d.shape)
    return points3d


def xy_to_points2d(lens, xy):
    c_xy = np.array([lens.getFilmSize()[0], lens.getFilmSize()[1]]) / 2.0 - 0.5
    points2d = 2.0 * (xy - c_xy) / (np.array(lens.getFilmSize()) - 1.0)
    points2d = points2d * np.array([1.0, -1.0])
    return points2d


def points2d_to_xy(lens, points2d):
    c_xy = np.array([lens.getFilmSize()[0], lens.getFilmSize()[1]]) / 2.0 - 0.5
    points2d = points2d * np.array([1.0, -1.0])
    xy = np.round(points2d * (np.array(lens.getFilmSize()) - 1.0) / 2.0 + c_xy).astype(int)
    return xy


def xy_depth_to_XYZ(lens, points_xy, depth_image):
    # normalize between -1.0 and 1.0
    points_2d = xy_to_points2d(lens, points_xy)
    # append z depth to it
    points_z = np.array([cv2.getRectSubPix(depth_image, (1, 1), tuple(point_xy))[0][0] for point_xy in points_xy])
    points_2d = np.c_[points_2d, points_z]
    # extrude to 3d points in the camera's local frame
    points_XYZ = extrude_depth(lens, points_2d)
    return points_XYZ


def scale_crop_camera_parameters(orig_size, orig_hfov, scale_size=None, crop_size=None):
    """
    Returns the parameters (size, hfov) of the camera that renders an image
    which is equivalent to an image that is first rendered from the original
    camera and then scaled by scale_size and center-cropped by crop_size.
    """
    scale_size = scale_size if scale_size is not None else 1.0
    crop_size = crop_size if crop_size is not None else orig_size
    size = crop_size
    hfov = np.rad2deg(2 * np.arctan(np.tan(np.deg2rad(orig_hfov) / 2.) * crop_size[0] / orig_size[0] / scale_size))
    return size, hfov


# python implementation of the VirtualFileSystem method from here:
# https://github.com/panda3d/panda3d/blob/master/panda/src/express/virtualFileSystem.cxx
def parse_options(options):
    flags = 0
    pw = ''
    for option in options.split(','):
        if option == '0' or not option:
            pass
        elif option == 'ro':
            flags |= VirtualFileSystem.MFReadOnly
        elif option.startswith('pw:'):
            pw = option[3:]
        else:
            raise ValueError('Invalid option on vfs-mount: %s' % option)
    return flags, pw
