"""Cython implementation of basic linear algebra operations."""

from libc.math cimport cos, sin, acos, log, exp, sqrt

cdef add3x3(double[:,:] a, double[:,:] b, double[:,:] r):
    """Compute r = a + b, assuming 3x3 matrices."""

    cdef:
        int i, j

    for i in range(3):
        for j in range(3):
            r[i, j] = a[i, j] + b[i, j]


cdef subtract3x3(double[:,:] a, double[:,:] b, double[:,:] r):
    """Compute r = a - b, assuming 3x3 matrices."""

    cdef:
        int i, j

    for i in range(3):
        for j in range(3):
            r[i, j] = a[i, j] - b[i, j]

cdef cross(double[:] a, double[:] b, double[:] r):
    """Compute the cross product."""

    r[0] = a[1]*b[2] - a[2]*b[1]
    r[1] = a[2]*b[0] - a[0]*b[2]
    r[2] = a[0]*b[1] - a[1]*b[0]

cdef to_skew_symmetric(double[:] a, double[:, :] m):
    """Convert a to a matrix such that a x b = mb for all b."""
    
    m[0, 0] = 0
    m[0, 1] = -a[2]
    m[0, 2] = a[1]
    m[1, 0] = a[2]
    m[1, 1] = 0
    m[1, 2] = -a[0]
    m[2, 0] = -a[1]
    m[2, 1] = a[0]
    m[2, 2] = 0
    

cdef norm3d(double[:] x):
    return sqrt(x[0]*x[0] + x[1]*x[1] + x[2]*x[2])

cdef double norm_squared(double[:] x):
    cdef:
        int i, N
        double r
    N = x.shape[0]
    r = 0.0
    for i in range(N):
        r += x[i]**2
    return r

cdef norm_squared3d(double[:] x):
    return x[0]*x[0] + x[1]*x[1] + x[2]*x[2]

cdef scale3x3(double[:, :] x, double s):
    """Scales x in place."""

    cdef int i, j
    
    for i in range(3):
        for j in range(3):
            x[i, j] = s * x[i, j]

cdef multiply3x3(double[:, :] a, double[:, :] b, double[:, :] r):
    """Multiply two 3x3 matrices."""

    cdef int i, j

    for i in range(3):
        for j in range(3):
            r[i, j] = (a[i, 0] * b[0, j] +
                       a[i, 1] * b[1, j] +
                       a[i, 2] * b[2, j])


cdef copy3x3(double[:, :] a, double[:, :] r):
    """Set contents of r equal to a."""

    cdef int i, j

    for i in range(3):
        for j in range(3):
            r[i, j] = a[i, j]

cdef double hadamard3x3(double[:, :] a, double[:, :] b):
    """Return entrywise product of a and b."""

    cdef:
        int i, j
        double r = 0.0
    
    for i in range(3):
        for j in range(3):
            r += a[i, j] * b[i, j]

    return r
