import numpy as np
import logging

log = logging.getLogger(__name__)

class Intrinsic_Spherical:

    def __init__(self, width, height):
        if (height * 2) != width:
            log.error("the ERP image width {} is not two time of height {}".format(width, height))

        self.width = width
        self.height = height

    def ima2cam(self, ima_points):
        ima_points_x = ima_points[0]
        ima_points_y = ima_points[1]

        # 1) point location to uv coordinates
        cam_points_x = (ima_points_x - self.width / 2.0) / self.width
        cam_points_y = (ima_points_y - self.height / 2.0) / self.width

        return [cam_points_x, cam_points_y]

    def bearing(self, ima_points):
        cam_points = self.ima2cam(ima_points)

        lon = cam_points[0] * 2.0 * np.pi
        lat = -cam_points[1] * 2.0 * np.pi

        lon = torch.from_numpy(lon)
        lat = torch.from_numpy(lat)

        bearing_x = torch.cos(lat) * torch.sin(lon)
        bearing_y = torch.cos(lat) * torch.cos(lon)
        bearing_z = torch.sin(lat)

        bearing_x = bearing_x.to(torch.float64)
        bearing_y = bearing_y.to(torch.float64)
        bearing_z = bearing_z.to(torch.float64)

        return [bearing_x, bearing_y, bearing_z]

    def cam2ima(self, cam_points):
        cam_points_x = cam_points[0]
        cam_points_y = cam_points[1]

        ima_points_x = cam_points_x * self.width + self.width / 2.0
        ima_points_y = cam_points_y * self.width + self.height / 2.0

        return [ima_points_x, ima_points_y]

    def project(self, cam_3dpoints):
        cam_3dpoints_x = cam_3dpoints[0]
        cam_3dpoints_y = cam_3dpoints[1]
        cam_3dpoints_z = cam_3dpoints[2]

        lon = torch.atan2(cam_3dpoints_x, cam_3dpoints_y)
        lat = torch.atan2(cam_3dpoints_z, torch.hypot(cam_3dpoints_x, cam_3dpoints_y))

        cam_2dpoints_x = lon / (2.0 * np.pi)
        cam_2dpoints_y = -lat / (2.0 * np.pi)

        return self.cam2ima([cam_2dpoints_x, cam_2dpoints_y])


class Intrinsic_Spherical_NP:

    def __init__(self, width, height):
        if (height * 2) != width:
            log.error("the ERP image width {} is not two time of height {}".format(width, height))

        self.width = width
        self.height = height

    def ima2cam(self, ima_points):
        ima_points_x = ima_points[0]
        ima_points_y = ima_points[1]

        # 1) point location to uv coordinates
        cam_points_x = (ima_points_x - self.width / 2.0) / self.width
        cam_points_y = (ima_points_y - self.height / 2.0) / self.width

        return [cam_points_x, cam_points_y]

    def bearing(self, ima_points):
        cam_points = self.ima2cam(ima_points)

        lon = cam_points[0] * 2.0 * np.pi
        lat = -cam_points[1] * 2.0 * np.pi

        bearing_x = np.cos(lat) * np.sin(lon)
        bearing_y = np.cos(lat) * np.cos(lon)
        bearing_z = np.sin(lat)

        return [bearing_x, bearing_y, bearing_z]

    def cam2ima(self, cam_points):
        cam_points_x = cam_points[0]
        cam_points_y = cam_points[1]

        ima_points_x = cam_points_x * self.width + self.width / 2.0
        ima_points_y = cam_points_y * self.width + self.height / 2.0

        return [ima_points_x, ima_points_y]

    def project(self, cam_3dpoints):
        cam_3dpoints_x = cam_3dpoints[0]
        cam_3dpoints_y = cam_3dpoints[1]
        cam_3dpoints_z = cam_3dpoints[2]

        lon = np.arctan2(cam_3dpoints_x, cam_3dpoints_y)
        lat = np.arctan2(cam_3dpoints_z, np.hypot(cam_3dpoints_x, cam_3dpoints_y))

        cam_2dpoints_x = lon / (2.0 * np.pi)
        cam_2dpoints_y = -lat / (2.0 * np.pi)

        return self.cam2ima([cam_2dpoints_x, cam_2dpoints_y])