# coding=utf-8
"""
Functions for working with homogeneous matrices.

"""
__author__ = ("Sébastien BARTHÉLEMY <sebastien.barthelemy@gmail.com>")

from misc_c cimport asym, sin, cos
import numpy
from numpy import array, zeros, empty, dot

DTYPE = numpy.float
cdef DTYPE_t tol = 1e-12

cpdef rotz(DTYPE_t angle):
    """
    Rotation around the z-axis
    example:
    >>> rotz(3.14/6)
    array([[ 0.86615809, -0.4997701 ,  0.        ,  0.        ],
           [ 0.4997701 ,  0.86615809,  0.        ,  0.        ],
           [ 0.        ,  0.        ,  1.        ,  0.        ],
           [ 0.        ,  0.        ,  0.        ,  1.        ]])

    """
    cdef DTYPE_t ca = cos(angle)
    cdef DTYPE_t sa = sin(angle)
    cdef numpy.ndarray[DTYPE_t, ndim=2] H = zeros((4,4), dtype=DTYPE)
    H[0,0] = ca
    H[0,1] = -sa
    H[1,0] = sa
    H[1,1] = ca
    H[2,2] = 1.
    H[3,3] = 1.

    return H

cpdef adjoint(numpy.ndarray H):
    """
    Return the adjoint of the homogeneous matrix

    >>> H = array(
    ...     [[ 0.70738827,  0.        , -0.70682518,  3.        ],
    ...      [ 0.61194086,  0.50045969,  0.61242835,  4.        ],
    ...      [ 0.35373751, -0.86575984,  0.35401931,  5.        ],
    ...      [ 0.        ,  0.        ,  0.        ,  1.        ]])
    >>> adjoint(H)
    array([[ 0.70738827,  0.        , -0.70682518,  0.        ,  0.        ,
             0.        ],
           [ 0.61194086,  0.50045969,  0.61242835,  0.        ,  0.        ,
             0.        ],
           [ 0.35373751, -0.86575984,  0.35401931,  0.        ,  0.        ,
             0.        ],
           [-1.64475426, -5.96533781, -1.64606451,  0.70738827,  0.        ,
            -0.70682518],
           [ 2.47572882,  2.59727952, -4.59618383,  0.61194086,  0.50045969,
             0.61242835],
           [-0.9937305 ,  1.50137907,  4.66458577,  0.35373751, -0.86575984,
             0.35401931]])

    """
    cdef int i,j
    cdef numpy.ndarray[DTYPE_t, ndim=2] Ad = zeros((6,6), dtype=DTYPE)
    Ad[0:3,0:3] = H[0:3,0:3]
    Ad[0:3,3:6] = 0.
    Ad[3:6,0:3] = dot(asym(H[0,3],H[1,3],H[2,3]), H[0:3,0:3])
    Ad[3:6,3:6] = H[0:3,0:3]
    return Ad