import bge
from mathutils import Matrix, Vector

XAXIS = Vector((1, 0, 0))
YAXIS = Vector((0, 1, 0))
ZAXIS = Vector((0, 0, 1))


def touch(c):
    s = c.sensors[0]
    if not (s.triggered and s.positive):
        return
    touched_ob = c.sensors[0].hitObject
    character = c.owner

    align_gravity(touched_ob)
    align_object_z(character, touched_ob)


def align_gravity(target):
    zaxis = target.getAxisVect(ZAXIS)
    new_gravity = zaxis * -9.8
    bge.logic.setGravity(new_gravity)


def align_object_z(ob, target):
    target_x_axis = target.getAxisVect(XAXIS)
    target_y_axis = target.getAxisVect(YAXIS)
    target_z_axis = target.getAxisVect(ZAXIS)

    current_x_axis = ob.getAxisVect(XAXIS)
    current_y_axis = ob.getAxisVect(YAXIS)

    if current_y_axis.dot(target_z_axis) < -0.5:
        # Approaching stairs head-on
        xaxis = target_x_axis
        yaxis = target_y_axis
    elif current_y_axis.dot(target_z_axis) > 0.5:
        # Approaching stairs backwards
        xaxis = -target_x_axis
        yaxis = -target_y_axis
    elif current_x_axis.dot(target_z_axis) < -0.5:
        # Approaching stairs sideways (with stairs on right)
        xaxis = target_y_axis
        yaxis = -target_x_axis
    elif current_x_axis.dot(target_z_axis) > 0.5:
        # Approaching stairs sideways (with stairs on left)
        xaxis = -target_y_axis
        yaxis = target_x_axis
    else:
        # Don't know what to do! Realign to point head-on
        xaxis = target_x_axis
        yaxis = target_y_axis
    zaxis = target_z_axis

    mat = vectors_to_rot_matrix(xaxis, yaxis, zaxis)
    print(mat)
    ob.worldOrientation = mat


def vectors_to_rot_matrix(xaxis, yaxis, zaxis):
    return Matrix([
        [xaxis.x, yaxis.x, zaxis.x],
        [xaxis.y, yaxis.y, zaxis.y],
        [xaxis.z, yaxis.z, zaxis.z],
    ])
