#! /usr/bin/env python
"""Controller should execute control for a given effector"""
import os
import json
from math import asin, atan, radians

import numpy as np

from masterloop import mp


FRAME_TORSO = 0
K = 0.1

_here = os.path.dirname(os.path.realpath(__file__))
with open('{}/../config/default.yaml'.format(_here)) as f:
    _xxx = json.load(f)

ARM = _xxx['arm']
MY_SHOULDERS = _xxx['should']
JOINTS = ('ShoulderPitch', 'ShoulderRoll', 'ElbowYaw', 'ElbowRoll')
NAO_ARM = 0.22


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(side):
    end_xyz = xyz('{}Arm'.format(side))
    xyzs = np.array([xyz(side + j) for j in JOINTS])

    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

    ax_order = (y_axis, z_axis, x_axis, z_axis)

    axes = np.concatenate(
        [get_transform(side + j).dot(ax) for j, ax in zip(JOINTS, ax_order)],
        axis=1
    )[:3].T

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


def to_nao(my_xyz, side):
    sh_offs = np.array(MY_SHOULDERS[side])
    my_sh_xyz = my_xyz - sh_offs
    nao_sh_xyz = my_sh_xyz / ARM * NAO_ARM
    nao_xyz = nao_sh_xyz + xyz(side + 'Shoulder')
    return nao_xyz


def our_cartesian(my_xyz, side):
    nao_xyz = to_nao(my_xyz, side)
    delta_r = K * (nao_xyz - xyz('{}Arm'.format(side)))
    crt_q = mp.getAngles([side + j for j in JOINTS], False)
    delta_q =  np.linalg.pinv(jacobian(side)).dot(delta_r).flatten()
    return crt_q + delta_q


def nao_cartesian(my_xyz, side):
    pass


def _elbow(arm_):
    quot = min(1.0, arm_ / ARM)
    return radians(180 * (1 - quot))


def dumb(my_xyz, side):
    sign = 1 if side == 'L' else -1
    roll = asin(my_xyz[1] / np.linalg.norm(my_xyz))
    roll -= sign * radians(25)
    pitch = atan(-my_xyz[2] / np.abs(my_xyz[0]))

    eroll = -sign * _elbow(np.linalg.norm(my_xyz))
    eyaw = mp.getAngles(['{}ElbowYaw'.format(side)], False)[0]
    return np.array([pitch, roll, eyaw, eroll])


def movement(my_xyz, side, control):
    ref = control(np.array(my_xyz), side).tolist()
    mp.setAngles([side + j for j in JOINTS], ref, 0.3)