"""
     pygl/util.py: CCP4MG Molecular Graphics Program
     Copyright (C) 2001-2005 University of York, CCLRC

     This library is free software: you can redistribute it and/or
     modify it under the terms of the GNU Lesser General Public License
     version 3, modified in accordance with the provisions of the 
     license to address the requirements of UK law.
 
     You should have received a copy of the modified GNU Lesser General 
     Public License along with this library.  If not, copies may be 
     downloaded from http://www.ccp4.ac.uk/ccp4license.php
 
     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU Lesser General Public License for more details.
"""


from math import *

"""
Some simple mathematical helper functions.
"""

PI = 3.141592653589793238462643
RAD_TO_DEG = 180.0 / PI

def calc_cylinder_vertices(v1,v2,size):
     """
     Calculate position equation of
     line between two points and vertices
     of cuboid approximation to cylinder.
     No longer used by MG which uses a C
     version.
     """
     line = []
     line.append(v2[0] - v1[0])
     line.append(v2[1] - v1[1])
     line.append(v2[2] - v1[2])

     zaxis = [0,0,1]
     a = vcross(line,zaxis)

     if(a[0]*a[0] + a[1]*a[1] + a[2]*a[2])  < 0.000000001:
        yaxis = [0,1,0]
        a = vcross(line,yaxis)
        if(a[0]*a[0] + a[1]*a[1] + a[2]*a[2])  < 0.000000001:
          xaxis = [1,0,0]
          a = vcross(line,xaxis)
          if(a[0]*a[0] + a[1]*a[1] + a[2]*a[2])  < 0.000000001:
             print "Error cannot find suitable axis. Coincident points?"

     a = normalize(a)
     b = vcross(line,a)

     c = normalize(a,radius=size)
     d = normalize(b,radius=size)

     return c,d,line

def calc_line(v1,v2):
     """
     Work out how to align along a particular line.
     """
     x1,y1,z1 = v1[:3]
     x2,y2,z2 = v2[:3]
     rx = x2 - x1
     ry = y2 - y1
     rz = z2 - z1
     radius = sqrt(rx*rx + ry*ry + rz*rz)
     if radius < 0.000000001:
        vx = x1
        vy = y1
        vz = z1
     else:
        vx = rx / radius
        vy = ry / radius
        vz = rz / radius

     v1 = [vx, vy, vz]
     v2 = [0.0, 0.0, 1.0]
     q = vcross(v1,v2)
     s = vdot(v1,v2)
     omega = acos(s) * RAD_TO_DEG

     return (q,omega,radius)

def vcross(v1,v2):
     """
     Vector cross product. For 3d vectors.
     """
     q = []
     q.append(v1[1] * v2[2] - v2[1] * v1[2])
     q.append(v1[2] * v2[0] - v2[2] * v1[0])
     q.append(v1[0] * v2[1] - v2[0] * v1[1])
     return q

def vdot(v1,v2):
     """
     Vector dot product.
     """
     order1 = len(v1)
     order2 = len(v2)
     if order1 != order2:
          print "Error, trying to find dot product"
          print "of differet length vectors."
          return 0.0
     
     s = 0.0
     for i in range(order1):
          s = s + v1[i]*v2[i]
     return s

def normalize(v,radius=1.0):
    """
    Normalize an nD vector
    """
    order = len(v)
    dsq = 0.0
    for i in range(order):
         dsq = dsq + v[i] *v[i]
    d = sqrt(dsq)

    if d < 0.0000000001:
       print "zero length vector in normalize", v
       vnew = []
       for i in range(order):
            vnew.append(0.0)
       return vnew

    vnew = []
    for i in range(order):
         vnew.append( v[i] * radius / d)

    return vnew

def get_normalize_factor(v):
    order = len(v)
    dsq = 0.0
    for i in range(order):
         dsq = dsq + v[i] *v[i]
    d = sqrt(dsq)
    if d < 0.0000000001:
      print "zero length vector in get_normalize_factor"
      return 0
    return 1.0/d

def vaddv(v1,v2):
     v = []
     for i in range(len(v1)):
          if i >= len(v2):
               add = 0.0
          else:
               add = v2[i]
          v.append(v1[i]+add)
     return v

def vsubv(v1,v2):
     v = []
     for i in range(len(v1)):
          if i >= len(v2):
               add = 0.0
          else:
               add = v2[i]
          v.append(v1[i]-add)
     return v

def matrixXvector(matrix,vector):

  while len(matrix) > len(vector):
       vector.append(0.0)
  result = [0,0,0,0]
  for i in range(len(vector)):
    for j in range(len(matrix)):
       result[i] = result[i] + vector[j] * matrix[i][j]

  return result

def buildmatrix(M,n,m):
    import point_funcs
    matrix =  point_funcs.doublea(n*m)

    for i in range(n):
      for j in range(m):
         matrix[i*n+j]=M[i][j]

    return matrix

def define_plane(vertices):
     """
     Given three points in 3D space
     it returns A,B,C,D in the plane
     equation Ax+By+Cz+D=0
     """
     x1,y1,z1 = vertices[0][:3]
     x2,y2,z2 = vertices[1][:3]
     x3,y3,z3 = vertices[2][:3]
     A = y1 * (z2 - z3) + y2 * (z3 - z1) + y3 * (z1 - z2) 
     B = z1 * (x2 - x3) + z2 * (x3 - x1) + z3 * (x1 - x2) 
     C = x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2) 
     D = -(x1 * (y2 * z3 - y3 * z2) + x2 * (y3 * z1 - y1 * z3) + x3 * (y1 * z2 - y2 * z1))

     return (A,B,C,D)

def line_plane_intersection(plane,line):
     """
     Calculate the point where plane
     and line intersect.
     """
     p0 = line[0]; p1 = line[1]
     a = plane[0]; b = plane[1]; c = plane[2]; d = plane[3];
     num = a*p0[0] + b*p0[1] + c*p0[2] + d
     den = a*(p0[0]-p1[0]) + b*(p0[1]-p1[1]) + c*(p0[2]-p1[2]) 
     if abs(den) < 0.0000000001:
        print "Error: Line parallel with plane in intersection"
        return (0,0,0)
     u = float(num)/float(den)
     p = []
     p.append(p0[0] + u*(p1[0]-p0[0]))
     p.append(p0[1] + u*(p1[1]-p0[1]))
     p.append(p0[2] + u*(p1[2]-p0[2]))

     return p

def find_points_on_plane(plane):
     """
     Find three (totally arbitrary!) points on a plane.
     """
     p0 = line_plane_intersection(plane,[(0,0,0),plane[:3]])
     xaxis = (1.0,0.0,0.0); yaxis = (0.0,1.0,0.0); zaxis = (0.0,0.0,1.0)
     ax = acos(vdot(normalize(plane[:3]),xaxis))
     ay = acos(vdot(normalize(plane[:3]),yaxis))
     az = acos(vdot(normalize(plane[:3]),zaxis))
     if ax > pi:
          ax = 2*pi - ax
     if ay > pi:
          ay = 2*pi - ay
     if az > pi:
          az = 2*pi - az
     if ax > pi*0.5:
          ax = pi - ax
     if ay > pi*0.5:
          ay = pi - ay
     if az > pi*0.5:
          az = pi - az
     maxa = ax
     axis = xaxis[:]
     if ay > maxa:
          maxa = ay
          axis = yaxis[:]
     if az > maxa:
          maxa = az
          axis = zaxis[:]

     v = vcross(normalize(plane[:3]),axis)
     p1 = []
     p1.append(p0[0]+v[0])
     p1.append(p0[1]+v[1])
     p1.append(p0[2]+v[2])
     v = vcross(normalize(plane[:3]),v)
     p2 = []
     p2.append(p0[0]+v[0])
     p2.append(p0[1]+v[1])
     p2.append(p0[2]+v[2])
     
     """
     Now some tests ...
     """

     new_plane = define_plane([p0,p1,p2])

     opfac = get_normalize_factor(plane[:3])
     npfac = get_normalize_factor(new_plane[:3])

     if abs(plane[0]*opfac > 0.0000000001):
          if abs(plane[1]*opfac > 0.0000000001):
               if abs(((plane[0]*opfac)/(new_plane[0]*npfac))/((plane[1]*opfac)/(new_plane[1]*npfac))) < 0.9:
                    print "Error finding plane points"
          if abs(plane[2]*opfac > 0.0000000001):
               if abs(((plane[0]*opfac)/(new_plane[0]*npfac))/((plane[2]*opfac)/(new_plane[2]*npfac))) < 0.9:
                    print "Error finding plane points"

     if abs(plane[1]*opfac > 0.0000000001):
          if abs(plane[2]*opfac > 0.0000000001):
               if abs(((plane[0]*opfac)/(new_plane[0]*npfac))/((plane[2]*opfac)/(new_plane[2]*npfac))) < 0.9:
                    print "Error finding plane points"

     if opfac/npfac < 0.0:
          return (p1,p0,p2)
     else:
          return (p0,p1,p2)