'''
library for 3D operations such as rotations.
'''

import numpy, pickle
from numpy import pi, sin, cos, arctan2, arcsin, arccos, dot, linspace, mean
from numpy.linalg import norm
dotProduct = numpy.dot
crossProduct = numpy.cross

def arcsin2( v, allowableNumericalError=10**-1 ):
    if -1 <= v and v <= 1:
        return arcsin(v)
    elif abs(v) -1 < allowableNumericalError:
        return pi/2 if v > 0 else -pi/2
    else:
        raise ValueError("arcsin2 called with invalid input of %s" % v)

def arccos2( v, allowableNumericalError=10**-1 ):
    if -1 <= v and v <= 1:
        return arccos(v)
    elif abs(v) -1 < allowableNumericalError:
        return 0 if v > 0 else pi
    else:
        raise ValueError("arccos2 called with invalid input of %s" % v)

def normalize( v ):
    return v / norm(v)


def quaternion(theta, u_x, u_y, u_z ):
    '''http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation 
    returns q_1, q_2, q_3, q_0 as to match FreeCads, if wikipedias naming is used'''
    return ( u_x*sin(theta/2), u_y*sin(theta/2), u_z*sin(theta/2), cos(theta/2) )
def quaternion2(theta, u_x, u_y, u_z ):
    ''' returns in wikipedia order, i.e. cos(theta/2) element first '''
    return ( cos(theta/2), u_x*sin(theta/2), u_y*sin(theta/2), u_z*sin(theta/2) )

def quaternion_to_euler( q_1, q_2, q_3, q_0): #order to match FreeCads, naming to match wikipedias
    '''
    http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions 
    for conversion to 3-1-3 Euler angles (dont know about this one, seems to me to be 3-2-1...)
    '''
    psi = arctan2( 2*(q_0*q_1 + q_2*q_3), 1 - 2*(q_1**2 + q_2**2) ) 
    phi =   arcsin2( 2*(q_0*q_2 - q_3*q_1) )
    theta =   arctan2( 2*(q_0*q_3 + q_1*q_2), 1 - 2*(q_2**2 + q_3**2) )
    return theta, phi, psi # gives same answer as FreeCADs toEuler function

def quaternion_to_axis_and_angle(  q_1, q_2, q_3, q_0): 
    'http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions'
    q =  numpy.array( [q_1, q_2, q_3])
    if norm(q) > 0:
        return q/norm(q), 2*arccos2(q_0)
    else:
        return numpy.array([1.0,0,0]), 2*arccos2(q_0)

def azimuth_and_elevation_angles_to_axis( a, e):
    u_z = sin(e)
    u_x = cos(e)*cos(a)
    u_y = cos(e)*sin(a)
    return numpy.array([ u_x, u_y, u_z ])
def axis_to_azimuth_and_elevation_angles( u_x, u_y, u_z ):
    return arctan2( u_y, u_x), arcsin2(u_z)

def quaternion_multiply( q1, q2 ):
    'http://en.wikipedia.org/wiki/Quaternion#Hamilton_product'
    a_1, b_1, c_1, d_1 = q1
    a_2, b_2, c_2, d_2 = q2
    return numpy.array([
           a_1*a_2 - b_1*b_2 - c_1*c_2 - d_1*d_2,
           a_1*b_2 + b_1*a_2 + c_1*d_2 - d_1*c_2,
           a_1*c_2 - b_1*d_2 + c_1*a_2 + d_1*b_2,
           a_1*d_2 + b_1*c_2 - c_1*b_2 + d_1*a_2
           ])

def euler_to_quaternion(angle1, angle2, angle3, axis1=3, axis2=2, axis3=1):
    '''http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles '''
    Q = []
    for angle,axis in zip([angle1,angle2,angle3],[axis1,axis2,axis3]):
        q = numpy.array( [cos(angle/2),0,0,0 ] )
        q[axis] = sin(angle/2)
        Q.append(q)
    q = quaternion_multiply( Q[0], quaternion_multiply( Q[1], Q[2] ) )
    return q[1], q[2], q[3], q[0]

def quaternion_rotation(p, q_1, q_2, q_3, q_0 ):
    '''
    rotate the vector p using the quaternion u
    http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
    '''
    q =     numpy.array( [q_0,  q_1,  q_2,  q_3] ) 
    q_inv = numpy.array( [q_0, -q_1, -q_2, -q_3] ) 
    p_q =   numpy.array( [  0, p[0], p[1], p[2]] ) #p as a quaternion
    p_q_rotated = quaternion_multiply( q, quaternion_multiply( p_q, q_inv ) )
    #print( p_q_rotated )
    return p_q_rotated[1:]

def euler_rotation(p, angle1, angle2, angle3, axis1=3, axis2=2, axis3=3 ):
    ''' http://en.wikipedia.org/wiki/Rotation_matrix ,
    axis1=1, axis2=2, axis3=3 is the same as euler_ZYX_rotation'''
    R = numpy.eye(3)
    for angle,axis in zip([angle1,angle2,angle3],[axis1,axis2,axis3]):
        s = sin(angle)
        c = cos(angle)
        if axis == 1: #x rotation
            R_i = numpy.array([ [ 1, 0, 0], [ 0, c,-s], [ 0, s, c] ])
        elif axis == 2: # y rotation
            R_i = numpy.array([ [ c, 0, s], [ 0, 1, 0], [-s, 0, c] ])
        else: #z rotation
            R_i = numpy.array([ [ c,-s, 0], [ s, c, 0], [ 0, 0, 1] ])
        #print(R_i)
        R = dotProduct(R_i, R)
        #print(R)
    #print('generic euler_rotation R')
    #print(R)
    return dotProduct(R, p)

def euler_ZYX_rotation_matrix( angle1, angle2, angle3 ):
    ''' http://en.wikipedia.org/wiki/Rotation_matrix '''
    c_1, s_1 = cos(angle1), sin(angle1)
    c_2, s_2 = cos(angle2), sin(angle2)
    c_3, s_3 = cos(angle3), sin(angle3)
    return numpy.array( [
        [ c_1*c_2 , c_1*s_2*s_3 - c_3*s_1 , s_1*s_3 + c_1*c_3*s_2 ],
        [ c_2*s_1 , c_1*c_3 + s_1*s_2*s_3 , c_3*s_1*s_2 - c_1*s_3 ],
        [ - s_2   , c_2*s_3 , c_2*c_3 ]
    ])
    
def euler_ZYX_rotation(p, angle1, angle2, angle3 ):
    return dotProduct(euler_ZYX_rotation_matrix( angle1, angle2, angle3 ), p)

def axis_rotation_matrix( theta, u_x, u_y, u_z ):
    ''' http://en.wikipedia.org/wiki/Rotation_matrix '''
    return numpy.array( [
            [ cos(theta) + u_x**2 * ( 1 - cos(theta)) , u_x*u_y*(1-cos(theta)) - u_z*sin(theta) ,  u_x*u_z*(1-cos(theta)) + u_y*sin(theta) ] ,
            [ u_y*u_x*(1-cos(theta)) + u_z*sin(theta) , cos(theta) + u_y**2 * (1-cos(theta))    ,  u_y*u_z*(1-cos(theta)) - u_x*sin(theta )] ,
            [ u_z*u_x*(1-cos(theta)) - u_y*sin(theta) , u_z*u_y*(1-cos(theta)) + u_x*sin(theta) ,              cos(theta) + u_z**2*(1-cos(theta))   ]
            ])

def axis_rotation( p, theta, u_x, u_y, u_z ):
    return dotProduct(axis_rotation_matrix( theta, u_x, u_y, u_z ), p)

def azimuth_elevation_rotation_matrix(azi, ela, theta ):
    #print('azimuth_and_elevation_angles_to_axis(azi, ela) %s' % azimuth_and_elevation_angles_to_axis(azi, ela))
    return axis_rotation_matrix( theta, *azimuth_and_elevation_angles_to_axis(azi, ela))

def azimuth_elevation_rotation( p, azi, ela, theta ):
    return dotProduct(azimuth_elevation_rotation_matrix( azi, ela, theta ), p)

def rotation_matrix_to_euler_ZYX(R, debug=False, checkAnswer=False, tol=10**-6, tol_XZ_same_axis=10**-9 ):
    'better way available at http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Rotation_matrix_.E2.86.94_Euler_angles'
    if 1.0 - abs(R[2,0]) > tol_XZ_same_axis :
        s_2 = -R[2,0]
        for angle2 in [ arcsin2(s_2), pi - arcsin2(s_2)]:#two options
            if debug: print('         angle2 %f' % angle2)
            c_2 = cos(angle2)
            s_3 = R[2,1] / c_2
            c_3 = R[2,2] / c_2
            for angle3 in [ arcsin2(s_3),  pi - arcsin2(s_3)]:
                if debug: print('         angle2 %f, angle3 %f' % (angle2, angle3))
                if abs(cos(angle3) - c_3) < tol:
                    c_1 = max( min( R[0,0] / c_2, 1), -1)
                    #c_1 = R[0,0] / c_2
                    s_1 = R[1,0] / c_2
                    for angle1 in [arccos2(c_1), -arccos2(c_1)]:
                        if debug: print('         angle2 %f, angle3 %f, angle1 %f' % (angle2, angle3, angle1))
                        if abs(s_1 - sin(angle1)) < tol:
                            if checkAnswer: rotation_matrix_to_euler_ZYX_check_answer( R, angle1, angle2, angle3)
                            return angle1, angle2, angle3
        #otherwise try axis orientated approach
        if debug: print('rotation_matrix_to_euler_ZYX - direct appoarch failed. Parsing to rotation_matrix_to_euler_ZYX_2')
        return  rotation_matrix_to_euler_ZYX_2(R, debug)
    else:
        s_2 = -R[2,0]
        angle2 = arcsin2(s_2)
        c_2 = 0
        debug = False
        #return  rotation_matrix_to_euler_ZYX_2(R, debug)
        # euler_ZYX_rotation_matrix reduces to numpy.array( [
        #   [ c_1*c_2 , c_1*s_2*s_3 - c_3*s_1 , s_1*s_3 + c_1*c_3*s_2 ],
        #   [ c_2*s_1 , c_1*c_3 + s_1*s_2*s_3 , c_3*s_1*s_2 - c_1*s_3 ],
        #   [ - s_2   , c_2*s_3 , c_2*c_3 ]
        #which reduces to 
        #   [ 0     , s_2*c_1*s_3 -     s_1*c_3 ,     s_1*s_3 + s_2*c_1*c_3 ],
        #   [ 0     ,     c_1*c_3 + s_2*s_1*s_3 , s_1*c_3*s_2 -     c_1*s_3 ],
        #   [ - s_2,                           0,                          0]
        # triometric indeties
        #   sin (angle1 + angle3) = s_1 c_3 + c_1 s_3
        #   cos (angle1 + angle3)=  c_1 c_3 - s_1 s_3
        # making angle3 negative:
        #   sin (angle1 - angle3)=  s_1 c_3 - c_1 s_3 
        #   cos (angle1 - angle3)=  c_1 c_3 + s_1 s_3
        # let a = angle1 + angle3
        # let b = angle1 - angle3
        # elif s_2 == -1, R[1:,1:] reduces to
        #   [   sin(a),    -cos(a) ],
        #   [   cos(a),     sin(a) ], so
        # WTF are angle1 and angle3, about the same axis!? 
        # Which makes sense since Y-axis rotation, mean x-angle and z-angle are applied about the same axis. so let
        angle3 = 0 #s_3 -> 0 c_3 -> 1
        # euler_ZYX_rotation_matrix reduces
        #   [     0  , -s_1  , c_1*s_2 ],
        #   [     0  ,  c_1  , s_1*s_2 ],
        #   [ - s_2  ,    0  ,       0 ]
        for angle1 in [ arcsin2(-R[0,1]), pi - arcsin2(-R[0,1]) ]:
                if debug: print('         angle2 %f, angle1 %f, angle3 %f' % (angle2, angle1, angle3))
                #if debug: print('         cos(angle1) %f, R[0,2] %f' % (cos(angle1), R[0,2]))
                if abs(cos(angle1) - R[0,2]/s_2) < tol:
                    return angle1, angle2, angle3
        if debug: print('rotation_matrix_to_euler_ZYX - direct appoarch failed. Parsing to rotation_matrix_to_euler_ZYX_2')
        return  rotation_matrix_to_euler_ZYX_2(R, debug)
def rotation_matrix_to_euler_ZYX_check_answer( R, angle1, angle2, angle3, tol=10**-8, disp=False):
    R_out =  euler_ZYX_rotation_matrix( angle1, angle2, angle3)
    error = numpy.linalg.norm(R - R_out)
    if disp:
        print('rotation_matrix_to_euler_ZYX_check_answer:')
        print('    norm(R - euler_ZYX_rotation_matrix( angle1, angle2, angle3)) %e' % error)
    if error > tol:
         raise RuntimeError('rotation_matrix_to_euler_ZYX check failed!. locals %s' % locals())
def rotation_matrix_to_euler_ZYX_2(R, debug=False):
    axis, angle = rotation_matrix_axis_and_angle_2(R)
    q_1, q_2, q_3, q_0 = quaternion(angle, *axis)
    return quaternion_to_euler( q_1, q_2, q_3, q_0)

def rotation_matrix_axis_and_angle(R, debug=False, checkAnswer=True, errorThreshold=10**-7, angle_pi_tol = 10**-5):
    'http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Rotation_matrix_.E2.86.94_Euler_axis.2Fangle'
    a = arccos2( 0.5 * ( R[0,0]+R[1,1]+R[2,2] - 1) )
    if abs(a % pi) > angle_pi_tol and abs(pi - (a % pi)) > angle_pi_tol:
        msg='checking angles sign, angle %f' % a
        for angle in [a, -a]:
            u_x = 0.5* (R[2,1]-R[1,2]) / sin(angle) 
            u_y = 0.5* (R[0,2]-R[2,0]) / sin(angle) 
            u_z = 0.5* (R[1,0]-R[0,1]) / sin(angle)
            if abs( (1-cos(angle))*u_x*u_y - u_z*sin(angle) - R[0,1] ) < errorThreshold:
                msg = 'abs( (1-cos(angle))*u_x*u_y - u_z*sin(angle) - R[0,1] ) < 10**-6 check passed' 
                break
        axis = numpy.array([u_x, u_y, u_z])
        error  = norm(axis_rotation_matrix(angle, *axis) - R)
        if debug: print('  norm(axis_rotation_matrix(angle, *axis) - R) %1.2e' % error)
        if error > errorThreshold:
            axis, angle = rotation_matrix_axis_and_angle_2(R, errorThreshold=errorThreshold, debug=debug, msg=msg)
    else:
        msg = 'abs(a % pi) > angle_pi_tol and abs(pi - (a % pi)) > angle_pi_tol'
        axis, angle = rotation_matrix_axis_and_angle_2( R, errorThreshold=errorThreshold, debug=debug, msg=msg)
    if numpy.isnan( angle ):
        raise RuntimeError('locals %s' % locals() )
    return axis, angle
def rotation_matrix_axis_and_angle_2(R, debug=False, errorThreshold=10**-7, msg=None):
    w, v = numpy.linalg.eig(R) #this method is not used at the primary method as numpy.linalg.eig does not return answers in high enough precision
    angle, axis = None, None
    eigErrors = abs(w -1) #errors from 1+0j
    i = (eigErrors == min(eigErrors)).tolist().index(True)
    axis = numpy.real(v[:,i])
    if i != 1:
        angle = arccos2(  numpy.real( w[1] ) )
    else:
        angle = arccos2(  numpy.real( w[0] ) )
    error  = norm(axis_rotation_matrix(angle, *axis) - R)
    if debug: print('rotation_matrix_axis_and_angle error %1.1e' % error)
    if error > errorThreshold:
        angle = -angle
        error = norm(axis_rotation_matrix(angle, *axis) - R)
        if error > errorThreshold:
            R_pickle_str = pickle.dumps(R)
            #R_abs_minus_identity = abs(R) - numpy.eye(3)
            print(R*R.transpose())
            raise ValueError( 'rotation_matrix_axis_and_angle_2: no solution found! locals %s' % str(locals()))
    return axis, angle


def plane_degrees_of_freedom( normalVector, debug=False, checkAnswer=False ):
    a,e = axis_to_azimuth_and_elevation_angles(*normalVector)
    dof1 = azimuth_and_elevation_angles_to_axis( a, e - pi/2)
    dof2 = azimuth_and_elevation_angles_to_axis( a+pi/2, 0)
    if checkAnswer: plane_degrees_of_freedom_check_answer( normalVector, dof1, dof2, debug )
    return dof1, dof2
def plane_degrees_of_freedom_check_answer( normalVector, d1, d2, disp=False, tol=10**-12):
    if disp: 
        print('checking plane_degrees_of_freedom result')
        print('  plane normal vector   %s' % normalVector)
        print('  plane dof1            %s' % d1)
        print('  plane dof2            %s' % d2)
    Q = numpy.array([normalVector,d1,d2])
    P = dotProduct(Q,Q.transpose())
    error = norm(P - numpy.eye(3))
    if disp: 
        print('  dotProduct( array([normalVector,d1,d2]), array([normalVector,d1,d2]).transpose():')
        print(P)
        print(' error norm from eye(3) : %e' % error)
    if error > tol:
        raise RuntimeError('plane_degrees_of_freedom check failed!. locals %s' % locals())

def planeIntersection( normalVector1, normalVector2, debug=False, checkAnswer=False ):
    return normalize ( crossProduct(normalVector1, normalVector2) )

def planeIntersection_check_answer( normalVector1, normalVector2, d,  disp=False, tol=10**-12):
    if disp:
        print('checking planeIntersection result')
        print('  plane normal vector 1 : %s' % normalVector1 )
        print('  plane normal vector 2 : %s' % normalVector2 )
        print('  d  : %s' % d )
    for t in [-3, 7, 12]:
        error1 = abs(dotProduct( normalVector1, d*t ))
        error2 = abs(dotProduct( normalVector2, d*t ))
        if disp:print('    d*(%1.1f) -> error1 %e, error2 %e' % (t, error1, error2) )
        if error1 > tol or error2 > tol:
            raise RuntimeError(' planeIntersection check failed!. locals %s' % locals())



def distance_between_axes( p1, u1, p2, u2):
    '''
    returns the shortest distance between to axes (or lines) in 3D space,
    where p1 is a point which line 1 goes through, and u1 is the direction vector for line 1.

    prob.
       minize d**2
       where d**2 = (p1_x + u1_x*t1 - p2_x - u2_x*t2)**2 + (p1_y + u1_y*t1 - p2_y - u2_y*t2)**2  + (p1_z + u1_z*t1 - p2_z - u2_z*t2)**2 
       giving a quadratic 2 varaiable (X = [t1,t2]) problem in the form of
           0.5 X^T A X + C^T B 
       differenting it gives
          0 = Q X + C
    
    using sympy to expand the abover expression
    > from sympy import *
    > x,y = symbols('x y')
    > expand( (x +y )**2)
      x**2 + 2*x*y + y**2
       
    > t1, t2, p1_x, p1_y, p1_z, p2_x, p2_y, p2_z, u1_x, u1_y, u1_z, u2_x, u2_y, u2_z = symbols('t1, t2, p1_x, p1_y, p1_z, p2_x, p2_y, p2_z, u1_x, u1_y, u1_z, u2_x, u2_y, u2_z')
    > d_sqrd = (p1_x + u1_x*t1 - p2_x - u2_x*t2)**2 + (p1_y + u1_y*t1 - p2_y - u2_y*t2)**2  + (p1_z + u1_z*t1 - p2_z - u2_z*t2)**2
    > expand(d_sqrd)
    > collect( expand(d_sqrd), [t1 , t2] )
    '''
    p1_x, p1_y, p1_z = p1
    u1_x, u1_y, u1_z = u1
    p2_x, p2_y, p2_z = p2
    u2_x, u2_y, u2_z = u2

    if numpy.array_equal( u1, u2 ) or numpy.array_equal( u1, -u2 ): #then
        assert numpy.linalg.norm( u1 ) != 0
        # generated using sympy 
        # > t, p1_x, p1_y, p1_z, p2_x, p2_y, p2_z, u1_x, u1_y, u1_z = symbols('t, p1_x, p1_y, p1_z, p2_x, p2_y, p2_z, u1_x, u1_y, u1_z')
        # > d_sqrd = (p1_x + u1_x*t - p2_x)**2 + (p1_y + u1_y*t - p2_y)**2  + (p1_z + u1_z*t - p2_z)**2
        # > solve( diff( d_sqrd, t ), t ) # gives the expresssion for t_opt
        t = (-p1_x*u1_x - p1_y*u1_y - p1_z*u1_z + p2_x*u1_x + p2_y*u1_y + p2_z*u1_z)/(u1_x**2 + u1_y**2 + u1_z**2)
        d_sqrd = (p1_x - p2_x + t*u1_x)**2 + (p1_y - p2_y + t*u1_y)**2 + (p1_z - p2_z + t*u1_z)**2 
    else:
        t1_t1_coef = u1_x**2 + u1_y**2 + u1_z**2 #collect( expand(d_sqrd), [t1 , t2] )
        t1_t2_coef = -2*u1_x*u2_x - 2*u1_y*u2_y - 2*u1_z*u2_z # collect( expand(d_sqrd), [t1*t2] )
        t2_t2_coef = u2_x**2 + u2_y**2 + u2_z**2
        t1_coef    = 2*p1_x*u1_x + 2*p1_y*u1_y + 2*p1_z*u1_z - 2*p2_x*u1_x - 2*p2_y*u1_y - 2*p2_z*u1_z
        t2_coef    =-2*p1_x*u2_x - 2*p1_y*u2_y - 2*p1_z*u2_z + 2*p2_x*u2_x + 2*p2_y*u2_y + 2*p2_z*u2_z

        A = numpy.array([ [ 2*t1_t1_coef , t1_t2_coef ] , [ t1_t2_coef, 2*t2_t2_coef ] ])
        b = numpy.array([ t1_coef, t2_coef])
        try:
            t1, t2 = numpy.linalg.solve(A,-b)
        except numpy.linalg.LinAlgError:
            print('distance_between_axes, failed to solve problem due to LinAlgError, using numerical solver instead')
            print('  variables : ')
            print('    p1 : %s' % p1 )
            print('    u1 : %s' % u1 )
            print('    p2 : %s' % p2 )
            print('    u2 : %s' % u2 )
            return distance_between_axes_fmin(p1, u1, p2, u2)
            
        d_sqrd = t1_t1_coef * t1**2 + t1_t2_coef * t1*t2 + t2_t2_coef * t2**2 + t1_coef*t1 + t2_coef*t2 + p1_x**2 - 2*p1_x*p2_x + p1_y**2 - 2*p1_y*p2_y + p1_z**2 - 2*p1_z*p2_z + p2_x**2 + p2_y**2 + p2_z**2 
    return d_sqrd ** 0.5

def distance_between_axes_fmin( p1, u1, p2, u2):
    from scipy.optimize import fmin_bfgs
    def distance(T):
        t1, t2 = T
        return numpy.linalg.norm( p1 + u1*t1 - (p2 + u2*t2) )
    T_opt = fmin_bfgs( distance, [0 , 0], disp=False)
    return distance(T_opt)

def distance_between_two_axes_3_points(p1,u1,p2,u2):
    ''' used for axial and circular edge constraints  '''
    # generated using sympy 
    # > t, p1_x, p1_y, p1_z, p2_x, p2_y, p2_z, u1_x, u1_y, u1_z = symbols('t, p1_x, p1_y, p1_z, p2_x, p2_y, p2_z, u1_x, u1_y, u1_z')
    # > d_sqrd = (p1_x + u1_x*t - p2_x)**2 + (p1_y + u1_y*t - p2_y)**2  + (p1_z + u1_z*t - p2_z)**2
    # > solve( diff( d_sqrd, t ), t ) # gives the expresssion for t_opt
    assert numpy.linalg.norm( u1 ) != 0
    p1_x, p1_y, p1_z = p1
    u1_x, u1_y, u1_z = u1
    #if not (u1_x**2 + u1_y**2 + u1_z**2) == 1:
    #    raise ValueError, "(u1_x**2 + u1_y**2 + u1_z**2) !=1 but rather %f  " % ( u1_x**2 + u1_y**2 + u1_z**2 )
    dist = 0
    for axis2_t in [-10, 0, 10]: #find point on axis 1 which is closest
        p2_x, p2_y, p2_z = p2 + axis2_t*u2
        t = (-p1_x*u1_x - p1_y*u1_y - p1_z*u1_z + p2_x*u1_x + p2_y*u1_y + p2_z*u1_z)/(u1_x**2 + u1_y**2 + u1_z**2) #should be able to drop this last term as it will equal 1...
        d_sqrd = (p1_x - p2_x + t*u1_x)**2 + (p1_y - p2_y + t*u1_y)**2 + (p1_z - p2_z + t*u1_z)**2
        dist = dist + d_sqrd ** 0.5
    return dist

def distance_between_axis_and_point( p1,u1,p2 ):
    assert numpy.linalg.norm( u1 ) != 0
    d = p2 - p1
    offset = d - dotProduct(u1,d)*u1
    #print(norm(offset))
    return norm(offset)

def distance_between_axis_and_point_old( p1, u1, p2 ):
    assert numpy.linalg.norm( u1 ) != 0
    p1_x, p1_y, p1_z = p1
    u1_x, u1_y, u1_z = u1
    p2_x, p2_y, p2_z = p2
    t = (-p1_x*u1_x - p1_y*u1_y - p1_z*u1_z + p2_x*u1_x + p2_y*u1_y + p2_z*u1_z)
    # dropped the (u1_x**2 + u1_y**2 + u1_z**2) term as it should equal 1
    d_sqrd = (p1_x - p2_x + t*u1_x)**2 + (p1_y - p2_y + t*u1_y)**2 + (p1_z - p2_z + t*u1_z)**2
    return d_sqrd ** 0.5



def rotation_required_to_rotate_a_vector_to_be_aligned_to_another_vector( v, v_ref ):
    c = crossProduct( v, v_ref)
    if norm(c) > 0:
        axis = normalize(c)
    else: #dont think this ever happens.
        axis, notUsed = plane_degrees_of_freedom( v )
    #if dof_axis == None:
    angle = arccos2( dotProduct( v, v_ref ))
    #else:
    #    axis3 = normalize ( crossProduct(v_ref, dof_axis) )
    #    a = dotProduct( v, v_ref ) #adjacent
    #    o = dotProduct( v, axis3 ) #oppersite
    #    angle = numpy.arctan2( o, a )
    return axis, angle

def rotation_required_to_rotate_a_vector_to_be_aligned_to_another_vector2( v, v_ref, dof_axis=None ):
    '''
    calculate the axis in a method other then a crossduct.
    finding axis, so that dot(axis, v) == 0 and dot(axis, v_ref) == 0, using linear alegebra.

    '''
    A_matrixs = []
    for i in range(3):
        A_matrixs.append([ 
                [v[j] for j in range(3) if j != i],
                [v_ref[j] for j in range(3) if j != i]] )
        #prettyPrintArray( A_matrixs[-1] )
    cond_number = map( numpy.linalg.cond,  A_matrixs)
    minloc = cond_number.index(min(cond_number)) 
    b = numpy.array([ -v[minloc], -v_ref[minloc] ])
    c = numpy.linalg.solve( A_matrixs[minloc], b).tolist()
    c.insert(minloc,1)
    c = numpy.array(c) #* numpy.sign(crossProduct( v, v_ref))
    if norm(c) > 0:
        axis = normalize(c)
    else: #dont think this ever happens.
        axis, notUsed = plane_degrees_of_freedom( v )
    if dof_axis == None:
        angle = arccos2( dotProduct( v, v_ref ))
        #checking if negative angle should be used
        v_rotated = dotProduct( axis_rotation_matrix( angle, *axis), v)
        #print('             v_rotated %s' % (v_rotated) )
        error = norm( v_ref - v_rotated )
        if error > 1:
            angle = -angle
    else:
        axis3 = normalize ( crossProduct(v_ref, dof_axis) )
        a = dotProduct( v, v_ref ) #adjacent
        o = dotProduct( v, axis3 ) #oppersite
        angle = numpy.arctan2( o, a )
    return axis, angle


def gram_schmidt_proj(u,v):
    return dot(v,u)/dot(u,u)*u
def gram_schmidt_orthonormalization( v1, v2, v3 ):
    'https://en.wikipedia.org/wiki/Gram%E2%80%93Schmidt_process'
    u1 = v1
    u2 = v2 - gram_schmidt_proj(u1,v2)
    u3 = v3 - gram_schmidt_proj(u1,v3) - gram_schmidt_proj(u2,v3)
    return normalize(u1), normalize(u2), normalize(u3)

def fit_plane_to_surface1( surface, n_u=3, n_v=3 ):
    uv = sum( [ [ (u,v) for u in linspace(0,1,n_u)] for v in linspace(0,1,n_v) ], [] )
    P = [ surface.value(u,v) for u,v in uv ] #positions at u,v points
    N = [ crossProduct( *surface.tangent(u,v) ) for u,v in uv ] 
    plane_norm = sum(N) / len(N) #plane's normal, averaging done to reduce error
    plane_pos = P[0]
    error = sum([ abs( dot(p - plane_pos, plane_norm) ) for p in P ])
    return plane_norm, plane_pos, error

def fit_rotation_axis_to_surface1( surface, n_u=3, n_v=3 ):
    'should work for cylinders and pssibly cones (depending on the u,v mapping)'
    uv = sum( [ [ (u,v) for u in linspace(0,1,n_u)] for v in linspace(0,1,n_v) ], [] )
    P = [ numpy.array(surface.value(u,v)) for u,v in uv ] #positions at u,v points
    N = [ crossProduct( *surface.tangent(u,v) ) for u,v in uv ] 
    intersections = []
    for i in range(len(N)-1):
        for j in range(i+1,len(N)):
            # based on the distance_between_axes( p1, u1, p2, u2) function,
            if 1 - abs(dot( N[i], N[j])) < 10**-6:
                continue #ignore parrallel case
            p1_x, p1_y, p1_z = P[i]
            u1_x, u1_y, u1_z = N[i]
            p2_x, p2_y, p2_z = P[j]
            u2_x, u2_y, u2_z = N[j]
            t1_t1_coef = u1_x**2 + u1_y**2 + u1_z**2 #should equal 1
            t1_t2_coef = -2*u1_x*u2_x - 2*u1_y*u2_y - 2*u1_z*u2_z # collect( expand(d_sqrd), [t1*t2] )
            t2_t2_coef = u2_x**2 + u2_y**2 + u2_z**2 #should equal 1 too
            t1_coef    = 2*p1_x*u1_x + 2*p1_y*u1_y + 2*p1_z*u1_z - 2*p2_x*u1_x - 2*p2_y*u1_y - 2*p2_z*u1_z
            t2_coef    =-2*p1_x*u2_x - 2*p1_y*u2_y - 2*p1_z*u2_z + 2*p2_x*u2_x + 2*p2_y*u2_y + 2*p2_z*u2_z
            A = numpy.array([ [ 2*t1_t1_coef , t1_t2_coef ] , [ t1_t2_coef, 2*t2_t2_coef ] ])
            b = numpy.array([ t1_coef, t2_coef])
            try:
                t1, t2 = numpy.linalg.solve(A,-b)
            except numpy.linalg.LinAlgError:
                continue #print('distance_between_axes, failed to solve problem due to LinAlgError, using numerical solver instead')
            pos_t1 = P[i] + numpy.array(N[i])*t1
            pos_t2 = P[j] + N[j]*t2
            intersections.append( pos_t1 )
            intersections.append( pos_t2 )
    if len(intersections) < 2:
        error = numpy.inf
        return 0, 0, error
    else: #fit vector to intersection points; http://mathforum.org/library/drmath/view/69103.html
        X = numpy.array(intersections)
        centroid = numpy.mean(X,axis=0)
        M = numpy.array([i - centroid for i in intersections ])
        A = numpy.dot(M.transpose(), M)
        U,s,V = numpy.linalg.svd(A)    #numpy docs: s : (..., K) The singular values for every matrix, sorted in descending order.
        axis_pos = centroid
        axis_dir = V[0]
        error = s[1] #dont know if this will work
        return axis_dir, axis_pos, error