from sympy import Matrix, Identity, DotProduct, eye
from sympy import sin, cos, sqrt, asin, acos
from sympy.matrices.expressions.vee import vee, Vee
from sympy.matrices.expressions.hat import hat, Hat


I3 = Matrix.eye(3)
v0 = Matrix([0,0,0])

def V(theta):
    norm2 = (theta.T * theta)[0]
    if norm2 == 0:
        return eye(3)
    else:
        norm = sqrt(norm2)
        t_hat = theta.hat()
        c_ = cos(norm)
        s_ = sin(norm)
        return eye(3) + t_hat * ((1-c_)/norm2) + (t_hat**2)*((norm-s_)/norm**3)

def Vinv(theta):
    norm2 = (theta.T * theta)[0]
    if norm2 == 0:
        return eye(3)
    else:
        norm = sqrt(norm2)
        t_hat = theta.hat()
        c_ = cos(norm)
        s_ = sin(norm)
        return eye(3) - t_hat/2 +  (t_hat**2)*(2*s_ - norm*(1 - c_)) / (2 * (norm**2) * s_)

def Exp_R3(vec):
    if len(vec) != 3:
        raise NotImplementedError
        
    theta = sqrt((vec.T * vec)[0])
    u = vec/theta
    
    return I3 + sin(theta) * u.hat() + (1-sin(theta)) * u.hat() * u.hat()

def Log_R3(R):
    R_ = Matrix(R)
    cos_theta = (R_.trace()-1)/2
    theta = acos(cos_theta)
    sin_theta_2 = 2*sqrt(1 - cos_theta**2)
    return theta * (R_ - R_.T) * theta/sin_theta_2
    
