# -*- coding: utf-8 -*-

import math
import numpy as np
from numpy import linalg

class ExtMath:
    def  __init__(self):
        pass

    def getElevation(self, x, y, z):
        val = math.sqrt(x*x + y*y)
        ele  = math.atan2(z, val)
        return ele

    def getAzimuth(self, x, y):
        az = math.atan2(y, x)# *180.0/math.pi
        return az

    def getAngle(self, vec1, vec2):
        val = np.dot(vec1, vec2)

        #norm1 = np.linalg.nomr(vec1)  # crash
        norm1 = math.sqrt(vec1[0]*vec1[0]+vec1[1]*vec1[1]+vec1[2]*vec1[2])
        norm2 = math.sqrt(vec2[0]*vec2[0]+vec2[1]*vec2[1]+vec2[2]*vec2[2])

        # domain error ?

        v = val/norm1/norm2

        if v > 1.0:
            v = 1.0
        elif v < -1.0:
            v = -1.0
        #angle = math.acos(v)
        angle = np.arccos(v)

        return angle

    def getRodriguesMatrix(self, vec, rad):
        len = math.sqrt(vec[0]*vec[0]+vec[1]*vec[1]+vec[2]*vec[2])

        if len == 0.0:
            return np.mat(np.eye(3))

        n1 = vec[0]/len
        n2 = vec[1]/len
        n3 = vec[2]/len

        mat = np.mat(np.zeros((3, 3)))

        cosA = math.cos(rad)
        sinA = math.sin(rad)
        mat[0, 0] = cosA + n1*n1*(1-cosA)
        mat[0, 1] = n1*n2*(1-cosA)-n3*sinA
        mat[0, 2] = n1*n2*(1-cosA)+n2*sinA
        mat[1, 0] = n2*n1*(1-cosA)+n3*sinA
        mat[1, 1] = cosA+n2*n2*(1-cosA)
        mat[1, 2] = n2*n3*(1-cosA)-n1*sinA
        mat[2, 0] = n3*n1*(1-cosA)-n2*sinA
        mat[2, 1] = n3*n2*(1-cosA)+n1*sinA
        mat[2, 2] = cosA + n3*n3*(1-cosA)

        return mat

    def rotateAroundVector(self, vec, rad, point):
        mat = self.getRodriguesMatrix(vec, rad)
        newPoint = mat.dot(point)

        return newPoint
