#! /usr/bin/env python
"""Controller should execute control for a given effector"""
import os

# import rospy
import numpy as np
from naoqi import ALProxy


FRAME_TORSO = 0
K = 0.1
mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)


def get_transform(joint):
    return np.array(
        mp.getTransform(joint, FRAME_TORSO, False)
    ).reshape((4, 4))


def xyz(joint):
    return np.array(mp.getPosition(joint, FRAME_TORSO, False))[:3]


def jacobian():
    # get current positions
    # according to control figure these values should actually come
    # from the integration step in the previous first control loop

    end_position = xyz('LArm')
    shoulder_position = xyz('LShoulderPitch')
    bicep_position = xyz('LShoulderRoll')
    elbow_position = xyz('LElbowYaw')
    forearm_position = xyz('LElbowRoll')

    # get transformed rotation axes, transformation to torso frame

    x_axis = np.array([[1, 0, 0, 1]]).T
    y_axis = np.array([[0, 1, 0, 1]]).T
    z_axis = np.array([[0, 0, 1, 1]]).T

    shoulder_axis = get_transform('LShoulderPitch').dot(y_axis)
    bicep_axis = get_transform('LShoulderRoll').dot(z_axis)
    elbow_axis = get_transform('LElbowYaw').dot(x_axis)
    forearm_axis = get_transform('LElbowRoll').dot(z_axis)

    xyzs = np.array([shoulder_position, bicep_position, elbow_position,
                     forearm_position])

    axes = np.concatenate(
        [shoulder_axis, bicep_axis, elbow_axis, forearm_axis],
        axis=1
    )[:3].T

    jacobian = np.cross(axes, end_position - xyzs).T
    return jacobian


def pseudo_inverse(jacobian):
    return np.linalg.pinv(jacobian)


def reference_generator(xyz):
    delta = K * (xyz - xyz('LArm'))
    return np.linalg.pinv(jacobian()).dot(delta).flatten()


def movement(xyz):
    ref = reference_generator(np.array(xyz)).tolist()
    mp.changeAngles(['LShoulderPitch', 'LShoulderRoll',
                     'LElbowYaw', 'LElbowRoll'], ref, 0.2)