# cython: language_level=3
# Copyright (c) 2020-2024, Manfred Moitzi
# License: MIT License
from typing import Sequence, Iterable, Tuple, TYPE_CHECKING, Iterator
from itertools import chain
import math
import numpy as np
import cython

from .vector cimport (
Vec2, Vec3, v3_normalize, v3_isclose, v3_cross, v3_dot,
)
from .vector import X_AXIS, Y_AXIS, Z_AXIS, NULLVEC

from libc.math cimport fabs, sin, cos, tan

if TYPE_CHECKING:
    from ezdxf.math import UVec

cdef extern from "constants.h":
    const double ABS_TOL
    const double REL_TOL

cdef double[16] IDENTITY = [
    1.0, 0.0, 0.0, 0.0,
    0.0, 1.0, 0.0, 0.0,
    0.0, 0.0, 1.0, 0.0,
    0.0, 0.0, 0.0, 1.0
]

cdef void set_floats(double *m, object values) except *:
    cdef int i = 0
    for v in values:
        if i < 16:  # Do not write beyond array bounds
            m[i] = v
        i += 1
    if i != 16:
        raise ValueError("invalid argument count")

cdef class Matrix44:
    def __cinit__(self, *args):
        cdef int nargs = len(args)
        if nargs == 0:  # default constructor Matrix44(): fastest setup
            self.m = IDENTITY  # memcopy!
        elif nargs == 1:  # 16 numbers: slow setup
            set_floats(self.m, args[0])
        elif nargs == 4:  # 4 rows of 4 numbers: slowest setup
            set_floats(self.m, chain(*args))
        else:
            raise ValueError("invalid argument count: 4 row vectors or "
                             "iterable of 16 numbers")

    def __reduce__(self):
        return Matrix44, (tuple(self),)

    def __getitem__(self, tuple index) -> float:
        cdef int row = index[0]
        cdef int col = index[1]
        cdef int i = row * 4 + col

        if 0 <= i < 16 and 0 <= col < 4:
            return self.m[i]
        else:
            raise IndexError(f'index out of range: {index}')

    def __setitem__(self, tuple index, double value):
        cdef int row = index[0]
        cdef int col = index[1]
        cdef int i = row * 4 + col

        if 0 <= i < 16 and 0 <= col < 4:
            self.m[i] = value
        else:
            raise IndexError(f'index out of range: {index}')

    def __iter__(self):
        cdef int i
        for i in range(16):
            yield self.m[i]

    def __repr__(self) -> str:
        def format_row(row):
            return "(%s)" % ", ".join(str(value) for value in row)

        return "Matrix44(%s)" % \
               ", ".join(format_row(row) for row in self.rows())

    def get_2d_transformation(self) -> Tuple[float, ...]:
        cdef double *m = self.m
        return m[0], m[1], 0.0, m[4], m[5], 0.0, m[12], m[13], 1.0

    @staticmethod
    def from_2d_transformation(components: Sequence[float]) -> Matrix44:
        if len(components) != 6:
            raise ValueError(
                "First 2 columns of a 3x3 matrix required: m11, m12, m21, m22, m31, m32"
            )

        m44 = Matrix44()
        m44.m[0] = components[0]
        m44.m[1] = components[1]
        m44.m[4] = components[2]
        m44.m[5] = components[3]
        m44.m[12] = components[4]
        m44.m[13] = components[5]
        return m44

    def get_row(self, int row) -> Tuple[float, ...]:
        cdef int index = row * 4
        if 0 <= index < 13:
            return self.m[index], self.m[index + 1], self.m[index + 2], self.m[
                index + 3]
        else:
            raise IndexError(f'invalid row index: {row}')

    def set_row(self, int row, values: Sequence[float]) -> None:
        cdef Py_ssize_t count = len(values)
        cdef Py_ssize_t start = row * 4
        cdef Py_ssize_t i
        if 0 <= row < 4:
            if count > 4:
                count = 4
            for i in range(count):
                self.m[start + i] = values[i]
        else:
            raise IndexError(f'invalid row index: {row}')

    def get_col(self, int col) -> Tuple[float, ...]:
        if 0 <= col < 4:
            return self.m[col], self.m[col + 4], \
                   self.m[col + 8], self.m[col + 12]
        else:
            raise IndexError(f'invalid col index: {col}')

    def set_col(self, int col, values: Sequence[float]):
        cdef Py_ssize_t count = len(values)
        cdef Py_ssize_t i
        if 0 <= col < 4:
            if count > 4:
                count = 4
            for i in range(count):
                self.m[col + i * 4] = values[i]
        else:
            raise IndexError(f'invalid col index: {col}')

    def rows(self) -> Iterator[Tuple[float, ...]]:
        return (self.get_row(index) for index in (0, 1, 2, 3))

    def columns(self) -> Iterator[Tuple[float, ...]]:
        return (self.get_col(index) for index in (0, 1, 2, 3))

    def copy(self) -> Matrix44:
        cdef Matrix44 _copy = Matrix44()
        _copy.m = self.m
        return _copy

    __copy__ = copy

    @property
    def origin(self) -> Vec3:
        cdef Vec3 v = Vec3()
        v.x = self.m[12]
        v.y = self.m[13]
        v.z = self.m[14]
        return v

    @origin.setter
    def origin(self, v: UVec) -> None:
        cdef Vec3 origin = Vec3(v)
        self.m[12] = origin.x
        self.m[13] = origin.y
        self.m[14] = origin.z

    @property
    def ux(self) -> Vec3:
        return self.get_ux()

    cdef Vec3 get_ux(self):
        cdef Vec3 v = Vec3()
        v.x = self.m[0]
        v.y = self.m[1]
        v.z = self.m[2]
        return v

    @property
    def uy(self) -> Vec3:
        return self.get_uy()

    cdef Vec3 get_uy(self):
        cdef Vec3 v = Vec3()
        v.x = self.m[4]
        v.y = self.m[5]
        v.z = self.m[6]
        return v

    @property
    def uz(self) -> Vec3:
        return self.get_uz()

    cdef Vec3 get_uz(self):
        cdef Vec3 v = Vec3()
        v.x = self.m[8]
        v.y = self.m[9]
        v.z = self.m[10]
        return v

    @property
    def is_cartesian(self) -> bool:
        cdef Vec3 x_axis = v3_cross(self.get_uy(), self.get_uz())
        return v3_isclose(x_axis, self.get_ux(), REL_TOL, ABS_TOL)

    @property
    def is_orthogonal(self) -> bool:
        cdef Vec3 ux = v3_normalize(self.get_ux(), 1.0)
        cdef Vec3 uy = v3_normalize(self.get_uy(), 1.0)
        cdef Vec3 uz = v3_normalize(self.get_uz(), 1.0)
        return fabs(v3_dot(ux, uy)) < 1e-9 and \
               fabs(v3_dot(ux, uz)) < 1e-9 and \
               fabs(v3_dot(uy, uz)) < 1e-9

    @staticmethod
    def scale(double sx, sy = None, sz = None) -> Matrix44:
        cdef Matrix44 mat = Matrix44()
        mat.m[0] = sx
        mat.m[5] = sx if sy is None else sy
        mat.m[10] = sx if sz is None else sz
        return mat

    @staticmethod
    def translate(double dx, double dy, double dz) -> Matrix44:
        cdef Matrix44 mat = Matrix44()
        mat.m[12] = dx
        mat.m[13] = dy
        mat.m[14] = dz
        return mat

    @staticmethod
    def x_rotate(double angle) -> Matrix44:
        cdef Matrix44 mat = Matrix44()
        cdef double cos_a = cos(angle)
        cdef double sin_a = sin(angle)
        mat.m[5] = cos_a
        mat.m[6] = sin_a
        mat.m[9] = -sin_a
        mat.m[10] = cos_a
        return mat

    @staticmethod
    def y_rotate(double angle) -> Matrix44:
        cdef Matrix44 mat = Matrix44()
        cdef double cos_a = cos(angle)
        cdef double sin_a = sin(angle)
        mat.m[0] = cos_a
        mat.m[2] = -sin_a
        mat.m[8] = sin_a
        mat.m[10] = cos_a
        return mat

    @staticmethod
    def z_rotate(double angle) -> Matrix44:
        cdef Matrix44 mat = Matrix44()
        cdef double cos_a = cos(angle)
        cdef double sin_a = sin(angle)
        mat.m[0] = cos_a
        mat.m[1] = sin_a
        mat.m[4] = -sin_a
        mat.m[5] = cos_a
        return mat

    @staticmethod
    def axis_rotate(axis: UVec, double angle) -> Matrix44:
        cdef Matrix44 mat = Matrix44()
        cdef double cos_a = cos(angle)
        cdef double sin_a = sin(angle)
        cdef double one_m_cos = 1.0 - cos_a
        cdef Vec3 _axis = Vec3(axis).normalize()
        cdef double x = _axis.x
        cdef double y = _axis.y
        cdef double z = _axis.z

        mat.m[0] = x * x * one_m_cos + cos_a
        mat.m[1] = y * x * one_m_cos + z * sin_a
        mat.m[2] = x * z * one_m_cos - y * sin_a

        mat.m[4] = x * y * one_m_cos - z * sin_a
        mat.m[5] = y * y * one_m_cos + cos_a
        mat.m[6] = y * z * one_m_cos + x * sin_a

        mat.m[8] = x * z * one_m_cos + y * sin_a
        mat.m[9] = y * z * one_m_cos - x * sin_a
        mat.m[10] = z * z * one_m_cos + cos_a

        return mat

    @staticmethod
    def xyz_rotate(double angle_x, double angle_y,
                   double angle_z) -> Matrix44:
        cdef Matrix44 mat = Matrix44()
        cdef double cx = cos(angle_x)
        cdef double sx = sin(angle_x)
        cdef double cy = cos(angle_y)
        cdef double sy = sin(angle_y)
        cdef double cz = cos(angle_z)
        cdef double sz = sin(angle_z)
        cdef double sxsy = sx * sy
        cdef double cxsy = cx * sy

        mat.m[0] = cy * cz
        mat.m[1] = sxsy * cz + cx * sz
        mat.m[2] = -cxsy * cz + sx * sz
        mat.m[4] = -cy * sz
        mat.m[5] = -sxsy * sz + cx * cz
        mat.m[6] = cxsy * sz + sx * cz
        mat.m[8] = sy
        mat.m[9] = -sx * cy
        mat.m[10] = cx * cy
        return mat

    @staticmethod
    def shear_xy(double angle_x = 0, double angle_y = 0) -> Matrix44:
        cdef Matrix44 mat = Matrix44()
        cdef double tx = tan(angle_x)
        cdef double ty = tan(angle_y)
        mat.m[1] = ty
        mat.m[4] = tx
        return mat

    @staticmethod
    def perspective_projection(double left, double right, double top,
                               double bottom, double near,
                               double far) -> Matrix44:
        cdef Matrix44 mat = Matrix44()
        mat.m[0] = (2. * near) / (right - left)
        mat.m[5] = (2. * near) / (top - bottom)
        mat.m[8] = (right + left) / (right - left)
        mat.m[9] = (top + bottom) / (top - bottom)
        mat.m[10] = -((far + near) / (far - near))
        mat.m[11] = -1
        mat.m[14] = -((2. * far * near) / (far - near))
        return mat

    @staticmethod
    def perspective_projection_fov(fov: float, aspect: float, near: float,
                                   far: float) -> Matrix44:
        vrange = near * math.tan(fov / 2.)
        left = -vrange * aspect
        right = vrange * aspect
        bottom = -vrange
        top = vrange
        return Matrix44.perspective_projection(left, right, bottom, top, near,
                                               far)

    @staticmethod
    def chain(*matrices: Matrix44) -> Matrix44:
        cdef Matrix44 transformation = Matrix44()
        for matrix in matrices:
            transformation *= matrix
        return transformation

    def __imul__(self, Matrix44 other) -> Matrix44:
        cdef double[16] m1 = self.m
        cdef double *m2 = other.m

        self.m[0] = m1[0] * m2[0] + m1[1] * m2[4] + m1[2] * m2[8] + \
                    m1[3] * m2[12]
        self.m[1] = m1[0] * m2[1] + m1[1] * m2[5] + m1[2] * m2[9] + \
                    m1[3] * m2[13]
        self.m[2] = m1[0] * m2[2] + m1[1] * m2[6] + m1[2] * m2[10] + \
                    m1[3] * m2[14]
        self.m[3] = m1[0] * m2[3] + m1[1] * m2[7] + m1[2] * m2[11] + \
                    m1[3] * m2[15]

        self.m[4] = m1[4] * m2[0] + m1[5] * m2[4] + m1[6] * m2[8] + \
                    m1[7] * m2[12]
        self.m[5] = m1[4] * m2[1] + m1[5] * m2[5] + m1[6] * m2[9] + \
                    m1[7] * m2[13]
        self.m[6] = m1[4] * m2[2] + m1[5] * m2[6] + m1[6] * m2[10] + \
                    m1[7] * m2[14]
        self.m[7] = m1[4] * m2[3] + m1[5] * m2[7] + m1[6] * m2[11] + \
                    m1[7] * m2[15]

        self.m[8] = m1[8] * m2[0] + m1[9] * m2[4] + m1[10] * m2[8] + \
                    m1[11] * m2[12]
        self.m[9] = m1[8] * m2[1] + m1[9] * m2[5] + m1[10] * m2[9] + \
                    m1[11] * m2[13]
        self.m[10] = m1[8] * m2[2] + m1[9] * m2[6] + m1[10] * m2[10] + \
                     m1[11] * m2[14]
        self.m[11] = m1[8] * m2[3] + m1[9] * m2[7] + m1[10] * m2[11] + \
                     m1[11] * m2[15]

        self.m[12] = m1[12] * m2[0] + m1[13] * m2[4] + m1[14] * m2[8] + \
                     m1[15] * m2[12]
        self.m[13] = m1[12] * m2[1] + m1[13] * m2[5] + m1[14] * m2[9] + \
                     m1[15] * m2[13]
        self.m[14] = m1[12] * m2[2] + m1[13] * m2[6] + m1[14] * m2[10] + \
                     m1[15] * m2[14]
        self.m[15] = m1[12] * m2[3] + m1[13] * m2[7] + m1[14] * m2[11] + \
                     m1[15] * m2[15]
        return self

    def __mul__(self, Matrix44 other) -> Matrix44:
        cdef Matrix44 res_matrix = self.copy()
        return res_matrix.__imul__(other)

    # __matmul__ = __mul__ does not work!

    def __matmul__(self, Matrix44 other) -> Matrix44:
        cdef Matrix44 res_matrix = self.copy()
        return res_matrix.__imul__(other)

    def transpose(self) -> None:
        swap(&self.m[1], &self.m[4])
        swap(&self.m[2], &self.m[8])
        swap(&self.m[3], &self.m[12])
        swap(&self.m[6], &self.m[9])
        swap(&self.m[7], &self.m[13])
        swap(&self.m[11], &self.m[14])

    def determinant(self) -> float:
        cdef double *m = self.m
        return m[0] * m[5] * m[10] * m[15] - m[0] * m[5] * m[11] * m[14] + \
               m[0] * m[6] * m[11] * m[13] - m[0] * m[6] * m[9] * m[15] + \
               m[0] * m[7] * m[9] * m[14] - m[0] * m[7] * m[10] * m[13] - \
               m[1] * m[6] * m[11] * m[12] + m[1] * m[6] * m[8] * m[15] - \
               m[1] * m[7] * m[8] * m[14] + m[1] * m[7] * m[10] * m[12] - \
               m[1] * m[4] * m[10] * m[15] + m[1] * m[4] * m[11] * m[14] + \
               m[2] * m[7] * m[8] * m[13] - m[2] * m[7] * m[9] * m[12] + \
               m[2] * m[4] * m[9] * m[15] - m[2] * m[4] * m[11] * m[13] + \
               m[2] * m[5] * m[11] * m[12] - m[2] * m[5] * m[8] * m[15] - \
               m[3] * m[4] * m[9] * m[14] + m[3] * m[4] * m[10] * m[13] - \
               m[3] * m[5] * m[10] * m[12] + m[3] * m[5] * m[8] * m[14] - \
               m[3] * m[6] * m[8] * m[13] + m[3] * m[6] * m[9] * m[12]

    def inverse(self) -> None:
        cdef double[16] m = self.m  # memcopy
        cdef double det = self.determinant()
        cdef double f = 1. / det
        self.m[0] = (m[6] * m[11] * m[13] - m[7] * m[10] * m[13] + m[7] * m[9] *
                     m[14] - m[5] * m[11] * m[14] - m[6] * m[9] * m[15] + m[5] *
                     m[10] * m[15]) * f

        self.m[1] = (m[3] * m[10] * m[13] - m[2] * m[11] * m[13] - m[3] * m[9] *
                     m[14] + m[1] * m[11] * m[14] + m[2] * m[9] * m[15] -
                     m[1] * m[10] * m[15]) * f
        self.m[2] = (m[2] * m[7] * m[13] - m[3] * m[6] * m[13] + m[3] * m[5] *
                     m[14] - m[1] * m[7] * m[14] - m[2] * m[5] * m[15] +
                     m[1] * m[6] * m[15]) * f

        self.m[3] = (m[3] * m[6] * m[9] - m[2] * m[7] * m[9] - m[3] * m[5] *
                     m[10] + m[1] * m[7] * m[10] + m[2] * m[5] * m[11] - m[1] *
                     m[6] * m[11]) * f

        self.m[4] = (m[7] * m[10] * m[12] - m[6] * m[11] * m[12] - m[7] * m[8] *
                     m[14] + m[4] * m[11] * m[14] + m[6] * m[8] * m[15] -
                     m[4] * m[10] * m[15]) * f

        self.m[5] = (m[2] * m[11] * m[12] - m[3] * m[10] * m[12] + m[3] * m[8] *
                     m[14] - m[0] * m[11] * m[14] - m[2] * m[8] * m[15] +
                     m[0] * m[10] * m[15]) * f

        self.m[6] = (m[3] * m[6] * m[12] - m[2] * m[7] * m[12] - m[3] * m[4] *
                     m[14] + m[0] * m[7] * m[14] + m[2] * m[4] * m[15] -
                     m[0] * m[6] * m[15]) * f

        self.m[7] = (m[2] * m[7] * m[8] - m[3] * m[6] * m[8] + m[3] * m[4] *
                     m[10] - m[0] * m[7] * m[10] - m[2] * m[4] * m[11] +
                     m[0] * m[6] * m[11]) * f

        self.m[8] = (m[5] * m[11] * m[12] - m[7] * m[9] * m[12] + m[7] * m[8] *
                     m[13] - m[4] * m[11] * m[13] - m[5] * m[8] * m[15] +
                     m[4] * m[9] * m[15]) * f

        self.m[9] = (m[3] * m[9] * m[12] - m[1] * m[11] * m[12] - m[3] *
                     m[8] * m[13] + m[0] * m[11] * m[13] + m[1] * m[8] *
                     m[15] - m[0] * m[9] * m[15]) * f

        self.m[10] = (m[1] * m[7] * m[12] - m[3] * m[5] * m[12] + m[3] *
                      m[4] * m[13] - m[0] * m[7] * m[13] - m[1] * m[4] *
                      m[15] + m[0] * m[5] * m[15]) * f

        self.m[11] = (m[3] * m[5] * m[8] - m[1] * m[7] * m[8] - m[3] * m[4] *
                      m[9] + m[0] * m[7] * m[9] + m[1] * m[4] * m[11] -
                      m[0] * m[5] * m[11]) * f

        self.m[12] = (m[6] * m[9] * m[12] - m[5] * m[10] * m[12] - m[6] *
                      m[8] * m[13] + m[4] * m[10] * m[13] + m[5] * m[8] *
                      m[14] - m[4] * m[9] * m[14]) * f

        self.m[13] = (m[1] * m[10] * m[12] - m[2] * m[9] * m[12] + m[2] *
                      m[8] * m[13] - m[0] * m[10] * m[13] - m[1] * m[8] *
                      m[14] + m[0] * m[9] * m[14]) * f

        self.m[14] = (m[2] * m[5] * m[12] - m[1] * m[6] * m[12] - m[2] *
                      m[4] * m[13] + m[0] * m[6] * m[13] + m[1] * m[4] *
                      m[14] - m[0] * m[5] * m[14]) * f

        self.m[15] = (m[1] * m[6] * m[8] - m[2] * m[5] * m[8] + m[2] * m[4] *
                      m[9] - m[0] * m[6] * m[9] - m[1] * m[4] * m[10] +
                      m[0] * m[5] * m[10]) * f

    @staticmethod
    def ucs(ux=X_AXIS, uy=Y_AXIS, uz=Z_AXIS, origin=NULLVEC) -> Matrix44:
        cdef Matrix44 mat = Matrix44()
        cdef Vec3 _ux = Vec3(ux)
        cdef Vec3 _uy = Vec3(uy)
        cdef Vec3 _uz = Vec3(uz)
        cdef Vec3 _origin = Vec3(origin)

        mat.m[0] = _ux.x
        mat.m[1] = _ux.y
        mat.m[2] = _ux.z

        mat.m[4] = _uy.x
        mat.m[5] = _uy.y
        mat.m[6] = _uy.z

        mat.m[8] = _uz.x
        mat.m[9] = _uz.y
        mat.m[10] = _uz.z

        mat.m[12] = _origin.x
        mat.m[13] = _origin.y
        mat.m[14] = _origin.z

        return mat

    def transform(self, vector: UVec) -> Vec3:
        cdef Vec3 res = Vec3(vector)
        cdef double x = res.x
        cdef double y = res.y
        cdef double z = res.z
        cdef double *m = self.m

        res.x = x * m[0] + y * m[4] + z * m[8] + m[12]
        res.y = x * m[1] + y * m[5] + z * m[9] + m[13]
        res.z = x * m[2] + y * m[6] + z * m[10] + m[14]
        return res

    def transform_direction(self, vector: UVec, normalize=False) -> Vec3:
        cdef Vec3 res = Vec3(vector)
        cdef double x = res.x
        cdef double y = res.y
        cdef double z = res.z
        cdef double *m = self.m

        res.x = x * m[0] + y * m[4] + z * m[8]
        res.y = x * m[1] + y * m[5] + z * m[9]
        res.z = x * m[2] + y * m[6] + z * m[10]
        if normalize:
            return v3_normalize(res, 1.0)
        else:
            return res

    ocs_to_wcs = transform_direction

    def transform_vertices(self, vectors: Iterable[UVec]) -> Iterator[Vec3]:
        cdef double *m = self.m
        cdef Vec3 res
        cdef double x, y, z

        for vector in vectors:
            res = Vec3(vector)
            x = res.x
            y = res.y
            z = res.z

            res.x = x * m[0] + y * m[4] + z * m[8] + m[12]
            res.y = x * m[1] + y * m[5] + z * m[9] + m[13]
            res.z = x * m[2] + y * m[6] + z * m[10] + m[14]
            yield res

    def fast_2d_transform(self, points: Iterable[UVec]) -> Iterator[Vec2]:
        cdef double m0 = self.m[0]
        cdef double m1 = self.m[1]
        cdef double m4 = self.m[4]
        cdef double m5 = self.m[5]
        cdef double m12 = self.m[12]
        cdef double m13 = self.m[13]
        cdef double x, y
        cdef Vec2 res

        for pnt in points:
            res = Vec2(pnt)
            x = res.x
            y = res.y
            res.x = x * m0 + y * m4 + m12
            res.y = x * m1 + y * m5 + m13
            yield res

    def transform_array_inplace(self, array: np.ndarray, ndim: int) -> None:
        """Transforms a numpy array inplace, the argument `ndim` defines the dimensions
        to transform, this allows 2D/3D transformation on arrays with more columns
        e.g. a polyline array which stores points as (x, y, start_width, end_width,
        bulge) values.

        """
        cdef int _ndim = ndim
        if _ndim == 2:
            assert array.shape[1] > 1
            transform_2d_array_inplace(self.m, array, array.shape[0])
        elif _ndim == 3:
            assert array.shape[1] > 2
            transform_3d_array_inplace(self.m, array, array.shape[0])
        else:
            raise ValueError("ndim has to be 2 or 3")


    def transform_directions(
        self, vectors: Iterable[UVec], normalize=False
    ) -> Iterator[Vec3]:
        cdef double *m = self.m
        cdef Vec3 res
        cdef double x, y, z
        cdef bint _normalize = normalize

        for vector in vectors:
            res = Vec3(vector)
            x = res.x
            y = res.y
            z = res.z

            res.x = x * m[0] + y * m[4] + z * m[8]
            res.y = x * m[1] + y * m[5] + z * m[9]
            res.z = x * m[2] + y * m[6] + z * m[10]
            yield v3_normalize(res, 1.0) if _normalize else res

    def ucs_vertex_from_wcs(self, wcs: Vec3) -> Vec3:
        return self.ucs_direction_from_wcs(wcs - self.origin)

    def ucs_direction_from_wcs(self, wcs: UVec) -> Vec3:
        cdef double *m = self.m
        cdef Vec3 res = Vec3(wcs)
        cdef double x = res.x
        cdef double y = res.y
        cdef double z = res.z

        res.x = x * m[0] + y * m[1] + z * m[2]
        res.y = x * m[4] + y * m[5] + z * m[6]
        res.z = x * m[8] + y * m[9] + z * m[10]
        return res

    ocs_from_wcs = ucs_direction_from_wcs


@cython.boundscheck(False)
@cython.wraparound(False)
cdef void transform_2d_array_inplace(double *m, double [:, ::1] array, Py_ssize_t size):
    cdef double m0 = m[0]
    cdef double m1 = m[1]
    cdef double m4 = m[4]
    cdef double m5 = m[5]
    cdef double m12 = m[12]
    cdef double m13 = m[13]
    cdef double x, y
    cdef Py_ssize_t i

    for i in range(size):
        x = array[i, 0]
        y = array[i, 1]
        array[i, 0] = x * m0 + y * m4 + m12
        array[i, 1] = x * m1 + y * m5 + m13

@cython.boundscheck(False)
@cython.wraparound(False)
cdef void transform_3d_array_inplace(double *m, double [:, ::1] array, Py_ssize_t size):
    cdef double x, y, z
    cdef Py_ssize_t i

    for i in range(size):
        x = array[i, 0]
        y = array[i, 1]
        z = array[i, 2]

        array[i, 0] = x * m[0] + y * m[4] + z * m[8] + m[12]
        array[i, 1] = x * m[1] + y * m[5] + z * m[9] + m[13]
        array[i, 2] = x * m[2] + y * m[6] + z * m[10] + m[14]