#! /usr/bin/env python
import os 

import rospy
import numpy as np

import sys
from naoqi import ALProxy
import motion

motionProxy = 0

def get_transform(joint):
    frame = motion.FRAME_TORSO
    useSensorValues = True
    result = motionProxy.getTransform(joint,frame,useSensorValues)
    result = np.matrix(result)
    print result
    result = np.reshape(result, (4,4))
    print result
    return result


def cartesian_position(joint):
    print 'function'
    frame = motion.FRAME_TORSO
    useSensorValues = True
    result = motionProxy.getPosition(joint, frame, useSensorValues)
    #print result
    return np.array(result[:3])


def jacobian():

    # get current positions/ accordint to control figure these values should actually come from the
    # integration step in the previous first control loop

    end_position = cartesian_position('LArm')

    shoulder_position = cartesian_position('LShoulderPitch')
    bicep_position = cartesian_position('LShoulderRoll')
    elbow_position = cartesian_position('LElbowYaw')
    forearm_position = cartesian_position('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)

    # get basis vectors of jacobian
    
    shoulder_basis = np.cross(shoulder_axis[:3].flatten(), end_position - shoulder_position)
    bicep_basis = np.cross(bicep_axis[:3].flatten(), end_position - bicep_position)
    elbow_basis = np.cross(elbow_axis[:3].flatten(), end_position - elbow_position)
    forearm_basis = np.cross(forearm_axis[:3].flatten(), end_position - forearm_position)

    # build jacobian matrix
    jacobian = np.concatenate([shoulder_basis, bicep_basis, elbow_basis, forearm_basis], axis=0).T

    return jacobian


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


def reference_generator(p_d)

    # calculate jacobian
    jac_mat = jacobian()   

    # use jacobian to compute desired joint speed
    
    derivative_speed = (p_d - end_position) / 5
    inv_jac = pseudo_inverse(jac_mat)
    angular_velocity = derivative_speed * inv_jac
    
        
    # integrate over desired speed to get desired joint position
    
    names         = "LArm"
    useSensors    = False
    commandAngles = motionProxy.getAngles(names, useSensors)

    names         = "RArm"
    useSensors    = False
    commandAngles = motionProxy.getAngles(names, useSensors)

    goal_angle = mom_angle + (angular_velocity * 5)
    # return desired joint position and speed
    
    

    return


def movement(e)

    # scale joint states with matrix K

    # add desired joint speed

    # move robot arm    

    return
 

if __name__ == '__main__':

    motionProxy = ALProxy("ALMotion", os.environ['NAO_IP'], 9559)
    jacob = jacobian()
    print jacob
    jacob = pseudo_inverse(jacob)
    print(jacob)

    # given new desired coordinates

    e = 1;
    """ 
    while e bigger some value

        # run reference generator to get desired joint postion and speed

        # subtract current joint states

        # movement
 
    """    
    #rospy.init_node('cartesian_controller')
    #rospy.spin()