#!/usr/bin/env python
# goniometer_helpers.py
#   Copyright (C) 2011 Diamond Light Source, Graeme Winter
#
#   This code is distributed under the BSD license, a copy of which is
#   included in the root directory of this package.
#
# Helper functions for goniometer

from scitbx import matrix
from scitbx.math import r3_rotation_axis_and_angle_from_matrix

def cbf_gonio_to_effective_axis_fixed(cbf_gonio):
    '''Given a cbf goniometer handle, first determine the real rotation
    axis, then determine the fixed component of rotation which is rotated
    about this axis.'''

    # First construct the real rotation axis, as the difference in rotating
    # the identity matrix at the end of the scan and the beginning.

    x = cbf_gonio.rotate_vector(0.0, 1, 0, 0)
    y = cbf_gonio.rotate_vector(0.0, 0, 1, 0)
    z = cbf_gonio.rotate_vector(0.0, 0, 0, 1)

    R = matrix.rec(x + y + z, (3, 3)).transpose()

    x1 = cbf_gonio.rotate_vector(1.0, 1, 0, 0)
    y1 = cbf_gonio.rotate_vector(1.0, 0, 1, 0)
    z1 = cbf_gonio.rotate_vector(1.0, 0, 0, 1)

    R1 = matrix.rec(x1 + y1 + z1, (3, 3)).transpose()

    RA = R1 * R.inverse()

    rot = r3_rotation_axis_and_angle_from_matrix(RA)

    # Then, given this, determine the component of the scan which is fixed -
    # which will need to be with respect to the unrotated axis. N.B. this
    # will not be unique, but should be correct modulo a free rotation about
    # the shifted axis.

    start = cbf_gonio.get_rotation_range()[0]

    axis = matrix.col(rot.axis)

    S = axis.axis_and_angle_as_r3_rotation_matrix(start, deg = True)

    return axis, S.inverse() * R