# coding=utf-8
import numpy
cimport numpy

DTYPE = numpy.float 
ctypedef numpy.float_t DTYPE_t

cdef extern from "math.h":
    float cosf(float theta)
    float sinf(float theta)

cpdef rotx(float angle):
    """
    Homogeneous matrix of a rotation around the x-axis
   
    example:

    >>> rotx(3.14/6)
    array([[ 1.        ,  0.        ,  0.        ,  0.        ],
           [ 0.        ,  0.86615809, -0.4997701 ,  0.        ],
           [ 0.        ,  0.4997701 ,  0.86615809,  0.        ],
           [ 0.        ,  0.        ,  0.        ,  1.        ]])

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

cdef _rotz(float angle):
    """Rotation around the z-axis

    """
    cdef float ca = cosf(angle)
    cdef float sa = sinf(angle)
    cdef numpy.ndarray[DTYPE_t, ndim=2] H = numpy.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

def rotz(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.        ]])

    """

    return _rotz(angle)