def get_beam_origin(self):
        """Calculate the direct beam origin from the
           detector beam coordinate."""

        distance = self.dict["distance"]
        beam = vec3(self.dict["beam"])

        XDSdetector_X = vec3(self.dict["detector_X"])
        XDSdetector_Y = vec3(self.dict["detector_Y"])
        XDSdetector_Z = XDSdetector_X.cross(XDSdetector_Y)

        # Calculate the direct beam coordinates on the detector
        beamCx = self.dict["origin"][0]*self.dict["pixel_size"][0]
        beamCy = self.dict["origin"][1]*self.dict["pixel_size"][1]
        beamCz = beam*XDSdetector_Z

        beamX = beamCx - beam*XDSdetector_X*distance/beamCz
        beamY = beamCy - beam*XDSdetector_Y*distance/beamCz
        beamXp = beamX/self.dict["pixel_size"][0]
        beamYp = beamY/self.dict["pixel_size"][1]

        if _debug:
            if "origin" in self.dict.keys():
                print "\nDEBUG: BEAM center read from XDS in pixel:",
                print  "%9.2f %9.2f" % tuple(self.dict["origin"])
                # When given by XDS, verifies that my calculation is correct
                assert (self.dict["origin"][0] - beamXp) < 0.05
                assert (self.dict["origin"][1] - beamYp) < 0.05
            print "DEBUG: BEAM center calculated in pixel:\t%9.2f %9.2f" % (beamXp,beamYp)
            print "DEBUG: BEAM center calculated in mm:\t\t%9.2f %9.2f\n" % (beamX,beamY)
        return beamXp, beamYp

    def debut(self):
        "Do simple cristallographic calculations from XDS initial parameters"

        A = vec3(self.dict["A"])
        B = vec3(self.dict["B"])
        C = vec3(self.dict["C"])

        volum = A.cross(B)*C
        Ar = B.cross(C).__div__(volum)
        Br = C.cross(A).__div__(volum)
        Cr = A.cross(B).__div__(volum)

        """
        Ar = B.cross(C)/volum
        Br = C.cross(A)/volum
        Cr = A.cross(B)/volum
        """
        UBxds = mat3(Ar,Br,Cr)

        BEAM = vec3(self.dict["beam"])
        wavelength = 1/BEAM.length()

        self.dict["cell_volum"] = volum
        self.dict["wavelength"] = wavelength
        self.dict["Ar"] = Ar
        self.dict["Br"] = Br
        self.dict["Cr"] = Cr
        self.dict["UB"] = UBxds

def compareSolutions(solutions1, solutions2, _epsilon=0.1):
    "Check if both solution matchs, with a tolerated difference of _epsilon."
    l = 0
    allMatchs = True
    for s1 in solutions1:
        match = False
        minRMSdiff = 1000
        for s2 in solutions2:
            l += 1
            vecDiff =  vec3(s1[2],s1[1],s1[0]) - vec3(s2)
            RMSdiff = rootSquareSum(vecDiff)/3.
            #print vecDiff, RMSdiff

        if RMSdiff < minRMSdiff:
            minRMSdiff = RMSdiff

        if RMSdiff < _epsilon:
                match = True
                break
    if match:
            solutions2.remove(s2)
            print "Good match for solution: %9.3f%9.3f%9.3f" % tuple(s1),
            print " Minimum RMSdiff = %.3f" % minRMSdiff
    else:
            allMatchs = False
            print "Warning: no match for solution: %9.3f%9.3f%9.3f" %tuple(s1),
            print " Minimum RMSdiff = %.3f" % minRMSdiff
    print l
    return allMatchs

    def UBxds_to_dnz(self):
        """ Convert the XDS direct space Orientation Matrix to a mosflm OM
        
        Denzo CAMERA coordinate frame has orthonormal axes with:

          y // to the rotation (spindel) axis
          z // to the beam
          x perpendicular to z and to the beam
        
        For more details see the denzo documentation:
        http://www.ccp4.ac.uk/dist/x-windows/Mosflm/doc/mosflm_user_guide.html#a3
        """
            
        if "UB" not in self.dict.keys():
            self.debut()
            
        BEAM = vec3(self.dict["beam"])
        ROT = vec3(self.dict["rot"])
        UBxds = self.dict["UB"]
                
        CAMERA_y = ROT.normalize()
        CAMERA_x = CAMERA_y.cross(BEAM).normalize()
        CAMERA_z = CAMERA_x.cross(CAMERA_y)
        CAMERA = mat3(CAMERA_x,CAMERA_y,CAMERA_z).transpose()
                    
        return  CAMERA * UBxds

 def UBxds_to_mos(self):
     """ Convert the XDS direct space Orientation Matrix to a mosflm OM
     
     Mosflm CAMERA coordinate frame has orthonormal axes with:
     
       z // rotation axis
       y perpendicular to z and to the beam
       x perpendicular to y and z (along the beam)
     
     For more details see the mosflm documentation:
     http://www.ccp4.ac.uk/dist/x-windows/Mosflm/doc/mosflm_user_guide.html#a3
     """
     
     if "UB" not in self.dict.keys():
         self.debut()
         
     BEAM = vec3(self.dict["beam"])
     ROT = vec3(self.dict["rot"])
     UBxds = self.dict["UB"]
     
     CAMERA_z = ROT.normalize()
     CAMERA_y = CAMERA_z.cross(BEAM).normalize()
     CAMERA_x = CAMERA_y.cross(CAMERA_z)
     CAMERA = mat3(CAMERA_x,CAMERA_y,CAMERA_z).transpose()
     
     return  CAMERA * UBxds * self.dict["wavelength"]

 def __init__(self, filename=None):
     self.DNZAxes = ey, -ex, -ez
     self.verticalAxis = vec3(1, 0, 0)
     self.spindleAxis = vec3(0, 0, 1)
     self.motorAxis = [0.,1.,0.]
     self.info = "Denzo Parser"
     self.fileType = "Denzo"
     if filename:
         self.parse(filename)
         self.spaceGroupName = self.spg.upper()
         self.spaceGroupNumber = SPGlib2[self.spg.lower()]

    def getOmega(self):
        """Calculate an Omega value (in radian) wich defines how the fast (X) and
        slow (Y) axis of detector files are  orientated toward the camera frame.
        The calculation of this omega value is supposed to reflect the Mosflm
        definition...

        But it seems that I get different values from the mosflm defaults... This
        may be due to: A) My missanderstanding of the mosflm documentation, B)
        Some tricks in the image reading routines.

        Nonetheless, this calculated value works for translating
        correctly the beam coordinates from XDS to mosflm [at least in the tested
        cases of MARCCD, MAR345 and ADSC detector images].

        Reference:
        http://www.ccp4.ac.uk/dist/x-windows/Mosflm/doc/mosflm_user_guide.html#a3
        """
        # Xd = CAMERA_y = beam
        # Yd = CAMERA_z = rot
        Xd =  vec3(self.dict["beam"]).normalize()
        Yd =  vec3(self.dict["rot"]).normalize()
        CAMERA_x = Xd.cross(Yd)
        CAMERA = mat3(CAMERA_x, Xd, Yd).transpose()
            
        # This is the definition of the fast:X and slow:Y axis for the detector files.
        XDSdetector_X = vec3(self.dict["detector_X"])
        XDSdetector_Y = vec3(self.dict["detector_Y"])
                
        # Now this axes are translated in the mosflm Camera frame
        Xs = XDSdetector_X*CAMERA
        Ys = XDSdetector_Y*CAMERA
        
        # Both angles should be identical.
        omegaX = Xd.angle(Xs)
        omegaY = Yd.angle(Ys)
        
        if _debug:
            print "DEBUG: X xds: fast =",XDSdetector_X
            print "DEBUG: Y xds: slow =",XDSdetector_Y
            print "DEBUG: Xs: fast =",XDSdetector_X,"->", Xs
            print "DEBUG: Ys: slow =",XDSdetector_Y,"->", Ys
            print "DEBUG: Xd: ", Xd
            print "DEBUG: Yd: ", Yd
            print "DEBUG: OmegaX:   %8.2f" % (omegaX*r2d)
            print "DEBUG: OmegaY:   %8.2f" % (omegaY*r2d)
            
        return omegaX

    def get_U0(self, rcell=None, vertical=None, spindle=None, clean=False):
        "Calculate denzo U0 from spindle, verctical"
        if not rcell: rcell = self.cell_r
        if not vertical: vertical = self.verticalAxis
        if not spindle: spindle = self.spindleAxis

        Bmat = self.get_B(rcell)
        vertical = vec3(vertical)
        spindle = vec3(spindle)

        U0y = (Bmat * spindle).normalize()
        U0xi = Bmat * vertical
        U0x = (U0xi - (U0xi * U0y) * U0y).normalize()

        U0 = mat3(U0x, U0y, U0x.cross(U0y)).transpose()

        # cleaning... Just cosmetic, not realy needed.
        if clean: U0 = cleanU0(U0)
        return U0

def axis_and_angle(mat_3):
    """From a rotation matrix return a corresponding rotation as an
       axis (a normalized vector) and angle (in radians).
       The angle is in the interval (-pi, pi]
    """
    asym = -asymmetrical_part(mat_3)
    axis = vec3(asym[1, 2], asym[2, 0], asym[0, 1])
    sine = axis.length()
    if abs(sine) > 1.e-10:
        axis = axis/sine
        projector = dyadic_product(axis, axis)
        cosine = trace((mat_3-projector))/(3.-axis*axis)
        angle = angle_from_sine_and_cosine(sine, cosine)
    else:
        tsr = 0.5*(mat_3+mat3(1))
        diag = tsr[0, 0], tsr[1, 1], tsr[3, 3] 
        i = tsr.index(max(diag))
        axis = vec3(tsr.getRow(i)/(tsr[i, i])**0.5)
        angle = 0.
        if trace(tsr) < 2.:
            angle = math.pi
    return axis, angle

    def getTwoTheta(self):
        """Tries to calculate the 2theta angle (in radian) of the detector.
        I am not completely sure of this calculation. How 2theta is precisely
        geometricaly defined in mosflm ?
        I need to look in the mosflm code where it is taken into account.
        """
        BEAM = vec3(self.dict["beam"])
        ROT  = vec3(self.dict["rot"]).normalize()
        camY = ROT.cross(BEAM)

        XDSdetector_X = vec3(self.dict["detector_X"]).normalize()
        XDSdetector_Y = vec3(self.dict["detector_Y"]).normalize()
        #XDSdetector_Z = XDSdetector_X.cross(XDSdetector_Y)

        #print beam.angle(XDSdetector_Z)*r2d
        if abs(ROT * XDSdetector_X) - 1 <= 0.05:
            detecorVector = -XDSdetector_Y
            #print 1
        elif abs(ROT * XDSdetector_Y) - 1 <= 0.05:
            detecorVector = XDSdetector_X
            #print 2
        else:
            raise Exception, "Can't calculate TwoTheta angle"    
        return camY.angle(detecorVector)    

def kappaVectorVertical(alpha):
    return vec3([0, -cos(alpha), sin(alpha)])

def kappaVector(alpha):
    return vec3([-sin(alpha), 0, cos(alpha)])

from math import cos, sin, pi
from pycgtypes import vec3, mat3

r2d = 180/pi
ex, ey, ez = vec3(1,0,0), vec3(0,1,0), vec3(0,0,1)
def kappaVector(alpha):
    return vec3([-sin(alpha), 0, cos(alpha)])

def kappaVectorVertical(alpha):
    return vec3([0, -cos(alpha), sin(alpha)])

# Frame definition used: Cambridge as used in Mosflm
# X = Beam_vector direction of the X-ray photons
# Z = Spindle axis, such that looking down this axis
#     towards the sample, positive phi is anti-clockwise
# Y = Defined to give a right handed coordinate system


# -- SOLEIL's PX1 CrystalLogic Goniometer definitions --
GONIOMETER_NAME = "SOLEIL PROXIMA-1 CrystalLogic"
GONIOMETER_AXES_NAMES = ("Omega","Kappa","Phi")
GONIOMETER_AXES = [ez, kappaVector(49.64/r2d), ez]
GONIOMETER_DATUM = (0,0,0)  # in degree

# -- DLS's MiniKappa Goniometer definitions --
#GONIOMETER_NAME = "DLS's MiniKappa"
#GONIOMETER_AXES_NAMES = ("Omega","Kappa","Phi")
#GONIOMETER_AXES = [ez, kappaVector(-24/r2d), ez]
#GONIOMETER_DATUM = (0,0,45)  # in degree
#GONIOMETER_AXES = [[0.00211, 0.00143, 1.], [0.28907, 0.28990, 0.91236], [0.00691, -0.00364, 0.99997]]
#GONIOMETER_DATUM = (0,0,0)  # in degree

    return axis, angle

# Test code
if __name__ == '__main__':

    from Scientific.Geometry import Vector ##.Transformation import *
    from Scientific.Geometry.Transformation import Rotation
    from random import random
    #
    Q = mat3(0.36, 0.48, -0.8, -0.8, 0.6, 0, 0.48, 0.64, 0.60)
    axis_q, angle_q = axis_and_angle(Q)
    print "Axis_q:  %9.6f%9.6f%9.6f" % tuple(axis_q),
    print "Angle_q: %10.5f" % (angle_q*R2D)
    #
    for iii in range(1e6):
        axis_i = list(vec3([random(), random(), random()]).normalize())
        angle_i = 3*random()
        rme = mat3().rotation(angle_i, vec3(axis_i))
        axis_1, angle_1 = axis_and_angle(rme)

        v = Vector(axis_i)
        r = Rotation(v, angle_i)
        axis_2, angle_2 = r.axisAndAngle()
        axis_d = (axis_1 - vec3(tuple(axis_2))).length()
        angle_d = abs(angle_1 - angle_2)
        if (angle_d  > 1e-13) or (axis_d > 1e-13):
            print "Angle_d:  %.3e" % (angle_d*R2D),
            print "  Axis_length_diff:  %.3e" % axis_d 
            print "Axis_i:  %9.6f%9.6f%9.6f" % tuple(axis_i),
            print "Angle_i: %10.5f" % (angle_i*R2D)
            print "Axis_1:  %9.6f%9.6f%9.6f" % tuple(axis_1),

    def parse(self, filename):
        "Denzo x-file parser"
        try:
            xfile = open(filename,"r").read().splitlines()
            xhead = xfile[:7]
            xtail = xfile[-30:]

        except:
            raise ParserError, "Error, Can't read file: %s" % filename

        if xhead[0][:6] == "HEADER":
            xhead =  xhead[1:]
        self.title = xhead[0]
        mats = map(str2floats, xhead[1:4])
        self.UB = mat3(mats[0][:3], mats[1][:3], mats[2][:3]).transpose()
        self.U =  mat3(mats[0][3:], mats[1][3:], mats[2][3:]).transpose()

        if len(xhead[4].split()) == 4: line1, line2 = xhead[4], xhead[5][:40]
        else: line1, line2 = xhead[4][:48], xhead[4][48:88]
        self.phi0, self.phi1, self.xtod, self.wavel = str2floats(line1)
        self.rotz, self.roty, self.rotx, self.mosaic = str2floats(line2)
        self.crystal_setting = self.rotz, self.roty, self.rotx

        # Extract reciprocal unit cell vectors
        self.Ar = vec3(self.UB.getColumn(0))
        self.Br = vec3(self.UB.getColumn(1))
        self.Cr = vec3(self.UB.getColumn(2))

        # Extract reciprocal cell parameters
        self.cell_r = UB_to_cellParam(self.UB)
        self.cell = reciprocal(self.cell_r)

        # Calculate direct unit cell vectors
        self.volum_r = self.Ar.cross(self.Br)*self.Cr
        self.volum = 1/self.volum_r

        self.A = self.Br.cross(self.Cr)*self.volum
        self.B = self.Cr.cross(self.Ar)*self.volum
        self.C = self.Ar.cross(self.Br)*self.volum

        for line in xtail:
            lineSplit = line.split()
            if line.upper().count("SPACE GROUP"):
                self.spg = lineSplit[2]
            elif line.upper().count("SPINDLE AXIS"):
                self.spindleAxis = map(int,lineSplit[2:5])
                self.verticalAxis = map(int,lineSplit[7:])
            elif line.upper().count("MOTOR AXIS"):
                self.motorAxis = map(float,lineSplit[2:5])
            elif line.upper().count("DISTANCE"):
                self.distance = float(lineSplit[1])
            elif line.upper().count("X BEAM"):
                self.beam_x = float(lineSplit[2])
                self.beam_y = float(lineSplit[5])
            elif line.upper().count("SECTOR"):
                self.sector = int(lineSplit[1])
            elif line.upper().count("RAW DATA FILE"):
                self.template = str(lineSplit[-1]).replace("'","")
            elif line.upper().count("UNIT CELL"):
                self.cell2 = map(float,lineSplit[2:])
                # Verify that the cell extracted from UB correspond
                # to the cell read from the xfile tail
                assert abs(self.cell2[0] - self.cell[0]) < 1e-2 and \
                       abs(self.cell2[1] - self.cell[1]) < 1e-2 and \
                       abs(self.cell2[2] - self.cell[2]) < 1e-2 and \
                       abs(self.cell2[3] - self.cell[3]) < 2e-2 and \
                       abs(self.cell2[4] - self.cell[4]) < 2e-2 and \
                       abs(self.cell2[5] - self.cell[5]) < 2e-2 

        # Verify that the calculation method for UB_to_Rotxyz works correctly
        _rotx, _roty, _rotz = self.UB_to_Rotxyz()
        assert abs(_rotx - self.rotx) < 2e-2 and \
               abs(_roty - self.roty) < 2e-2 and \
               abs(_rotz - self.rotz) < 2e-2

        # Verify that the calculation method for Adnz_to_Udnz works correctly
        _U = self.Adnz_to_Udnz()
        print diffMAT(_U, self.U)
        assert diffMAT(_U, self.U) < 5e-6

""" XIO plugin for the export parameters as an XDS.INP format
    See http://xds.mpimf-heidelberg.mpg.de/html_doc/xds_prepare.html
"""

__version__ = "0.3.4"
__author__ = "Pierre Legrand ([email protected])"
__date__ = "15-12-2009"
__copyright__ = "Copyright (c) 2007-2009 Pierre Legrand"
__license__ = "New BSD, http://www.opensource.org/licenses/bsd-license.php"

import time

from pycgtypes import vec3
from pycgtypes import mat3

EX, EY, EZ = vec3(1, 0, 0), vec3(0, 1, 0), vec3(0, 0, 1)
V3FMT = "%9.6f %9.6f %9.6f"
PI = 3.1415926535897931
D2R = PI/180.

def det_dist(distance, dettype):
    "Return the disance with the proper sign."
    detori = XDS_DETECTOR_DICT["orient"][dettype]
    return distance*detori[2]

def det_spindle(dettype):
    "Return the spindle axis vector."
    return V3FMT % tuple(XDS_DETECTOR_DICT["orient"][dettype][3])

def polarization(wavelength):
    "Guess the polarization fraction from the wavelength."

       Original fortran version:
           Phil Evans MRC LMB, Cambridge

""" % _progname

from XOconv import mat3T, printmat, is_orthogonal, spg_num2symb, BusingLevy, \
                   SPGlib, map_r2d, PGequiv, openWriteClose, openReadClose,  \
                   rootSquareSum, random_3axes, kappaVector, SPGlib2
from XOconv import MosflmParser, DenzoParser, XDSParser

VERBOSE = True
r2d = 180/math.pi
radian2degree = lambda a: a*r2d
degree2radian = lambda a: a/r2d

ex, ey, ez = vec3(1,0,0), vec3(0,1,0), vec3(0,0,1)
X, Y, Z = ex, ey, ez
Qdnz2mos = mat3T(ez, ex, ey)

class CrystalVector(vec3):
    """ Define a crystal vector to represent reciprocal or direct space vectors

    NOTE that it can accept fractional coordinates like
         CrystalVector("(1.2 1.22 4.9)")

    NOTE that as it inherit from the Vector class, CrystalVectors support the
    usual arithmetic operations ('v1', 'v2': vectors, 's': scalar):

    -  'v1+v2'           (addition)
    -  'v1-v2'           (subtraction)
    -  'v1*v2'           (scalar product)