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

import numpy as np

from masterloop import mp


FRAME_TORSO = 0
AXIS_XYZ = 7
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 = {
    'L': _xxx['lsh'],
    'R': _xxx['rsh']
}
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 inv_jacobian(j):
    u, s, vt = np.linalg.svd(j)
    s_inv = np.zeros((vt.shape[0], u.shape[1]))
    np.fill_diagonal(s_inv, 1 // s)
    s_inv[np.abs(s_inv) > 30] = 0
    return vt.T.dot(s_inv).dot(u.T)


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


def _elbow(arm_):
    quot = min(1.0, arm_ / 0.70)
    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 = -sign * radians(40)
    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)


def nao_cart_movement(my_arm_xyz, side, *_):
    nao_xyz = to_nao(my_arm_xyz, side)
    mp.setPositions('{}Arm'.format(side), FRAME_TORSO,
                    tuple(nao_xyz.tolist()) + (0, 0, 0),
                    1.0, AXIS_XYZ)