"""The picture's camera as a pinhole in numpy (no bpy), in Blender's conventions so the fitted camera can be put in the file.

Model frame (the Kazah's and the Mako's): 1 unit = 1 m, nose to -Y, port +X, Z up.
Camera params p = [cx, cy, cz, rx, ry, rz, log f_px]: location, Blender Euler XYZ rotation (camera looks down its -Z, +Y up),
focal length in picture pixels. The principal point is the picture's centre (no lens shift).
"""
import math
import json
from pathlib import Path
import numpy as np

W_IMG, H_IMG = 827, 803
PP = (W_IMG / 2.0, H_IMG / 2.0)
HERE = Path(__file__).resolve().parent


def euler_xyz(rx, ry, rz):
    cx, sx, cy, sy, cz, sz = math.cos(rx), math.sin(rx), math.cos(ry), math.sin(ry), math.cos(rz), math.sin(rz)
    Rx = np.array([[1, 0, 0], [0, cx, -sx], [0, sx, cx]])
    Ry = np.array([[cy, 0, sy], [0, 1, 0], [-sy, 0, cy]])
    Rz = np.array([[cz, -sz, 0], [sz, cz, 0], [0, 0, 1]])
    return Rz @ Ry @ Rx                       # camera to world, as mathutils Euler('XYZ').to_matrix()


def project(p, X3):
    """picture pixels (n x 2) and camera-space depth (n) of world points X3 (n x 3)"""
    X3 = np.atleast_2d(np.asarray(X3, float))
    C = np.asarray(p[:3], float)
    R = euler_xyz(*p[3:6])
    pc = (X3 - C) @ R
    f = math.exp(p[6])
    z = -pc[:, 2]
    x = PP[0] + f * pc[:, 0] / z
    y = PP[1] - f * pc[:, 1] / z
    return np.stack([x, y], 1), z


def ray(p, u, v):
    """world origin and unit direction of the ray through picture pixel (u, v)"""
    R = euler_xyz(*p[3:6])
    f = math.exp(p[6])
    d = R @ np.array([(u - PP[0]) / f, -(v - PP[1]) / f, -1.0])
    return np.asarray(p[:3], float), d / np.linalg.norm(d)


def on_plane(p, u, v, n, d0):
    """picture pixel cast onto the plane n . X = d0"""
    o, d = ray(p, u, v)
    n = np.asarray(n, float)
    t = (d0 - n @ o) / (n @ d)
    return o + t * d


def look_at(cam, tgt, roll=0.0):
    """Blender Euler XYZ for a camera at cam looking at tgt (up = +Z), with an extra roll about the view axis"""
    cam, tgt = np.asarray(cam, float), np.asarray(tgt, float)
    f = tgt - cam
    f /= np.linalg.norm(f)
    r = np.cross(f, [0, 0, 1.0])
    r /= np.linalg.norm(r)
    u = np.cross(r, f)
    if roll:
        c, s = math.cos(roll), math.sin(roll)
        r, u = c * r + s * u, -s * r + c * u
    R = np.stack([r, u, -f], 1)               # columns: camera x, y, z in world
    # matrix -> Euler XYZ (R = Rz Ry Rx)
    ry = math.asin(-max(-1.0, min(1.0, R[2, 0])))
    rx = math.atan2(R[2, 1], R[2, 2])
    rz = math.atan2(R[1, 0], R[0, 0])
    return [rx, ry, rz]


def lens_mm(p):
    return math.exp(p[6]) * 36.0 / W_IMG      # sensor width 36 mm, horizontal fit (W > H)


def load(name="cam.json"):
    return json.loads((HERE / name).read_text())