import numpy as np
from myAnfis import anfis
import timeit
import cPickle as pickle

# define the arm and leg lengths
arm = 110.0
leg = 266.7
# length from center of base to axis of rotation
base_radius = 55


def loadAnfisNetwork():
    global anf
    with open('fuzzy_log_test_table_90_270_5_epoch_30.pkl', 'rb') as f:
        anf = pickle.load(f)
    return anf


def getMotorAngles(x_pos, y_pos, z_pos):
    input_val = np.array([[x_pos, y_pos, z_pos]])

    anfis_matrix = anfis.predict(anf, input_val)

    return np.array([anfis_matrix[0,0], anfis_matrix[0, 1], anfis_matrix[0, 2]])


def getPlatformPositions(platform_normal, end_point):
    platform_radius = 25

    beta = -(90 - np.degrees(np.arctan2(np.sqrt(platform_normal[1] ** 2 + platform_normal[2] ** 2), platform_normal[0])))
    alpha = 90 - np.degrees(np.arctan2(np.sqrt(platform_normal[2] ** 2 + platform_normal[0] ** 2), platform_normal[1]))
    gamma = -(180 - np.degrees(np.arctan2(np.sqrt(platform_normal[0] ** 2 + platform_normal[1] ** 2), platform_normal[2])))


    rotate_x = np.matrix([[1, 0, 0, 0], [0, np.cos(np.radians(alpha)), -np.sin(np.radians(alpha)), 0],[0, np.sin(np.radians(alpha)), np.cos(np.radians(alpha)), 0], [0, 0, 0, 1]])
    rotate_y = np.matrix([[np.cos(np.radians(beta)), 0, np.sin(np.radians(beta)), 0], [0, 1, 0, 0],[-np.sin(np.radians(beta)), 0, np.cos(np.radians(beta)), 0], [0, 0, 0, 1]])
    rotate_z = np.matrix([[np.cos(np.radians(gamma)), -np.sin(np.radians(gamma)), 0, 0],[np.sin(np.radians(gamma)), np.cos(np.radians(gamma)), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
    translate = np.matrix([[1, 0, 0, end_point[0]], [0, 1, 0, end_point[1]], [0, 0, 1, end_point[2]], [0, 0, 0, 1]])

    # define the position where the arms meet the base
    platform_1 = np.array([platform_radius * np.cos(np.deg2rad(90)), platform_radius * np.sin(np.deg2rad(90)), 0])
    platform_2 = np.array([platform_radius * np.cos(np.deg2rad(210)), platform_radius * np.sin(np.deg2rad(210)), 0])
    platform_3 = np.array([platform_radius * np.cos(np.deg2rad(330)), platform_radius * np.sin(np.deg2rad(330)), 0])

    p1 = np.matrix([[platform_1[0]], [platform_1[1]], [platform_1[2]], [1]])
    p2 = np.matrix([[platform_2[0]], [platform_2[1]], [platform_2[2]], [1]])
    p3 = np.matrix([[platform_3[0]], [platform_3[1]], [platform_3[2]], [1]])

    new_p1 = translate * rotate_y * rotate_x * p1
    new_p2 = translate * rotate_y * rotate_x * p2
    new_p3 = translate * rotate_y * rotate_x * p3

    new_p1_array = np.array([new_p1[0, 0], new_p1[1, 0], new_p1[2, 0]])
    new_p2_array = np.array([new_p2[0, 0], new_p2[1, 0], new_p2[2, 0]])
    new_p3_array = np.array([new_p3[0, 0], new_p3[1, 0], new_p3[2, 0]])

    return new_p1_array, new_p2_array, new_p3_array


""" Returns the unit vector of the vector.  """
def unit_vector(vector):
    return vector / np.linalg.norm(vector)


def angle_between(v1, v2):
    v1_u = unit_vector(v1)
    v2_u = unit_vector(v2)
    return np.degrees(np.arccos(np.clip(np.dot(v1_u, v2_u), -1.0, 1.0)))


def euclidean_distance(P1, P2):
    return np.sqrt((P1[0] - P2[0]) ** 2 + (P1[1] - P2[1]) ** 2 + (P1[2] - P2[2]) ** 2)


def getKneePosition(base_p, new_p):
    # Sphere Circle Intersection
    # http://gamedev.stackexchange.com/questions/75756/sphere-sphere-intersection-and-circle-sphere-intersection
    arm = 110
    leg = 266.7

    c_c = base_p
    c_s = new_p
    z_vector = np.array([0, 0, 1])

    # the normal to the circle generated by the arm
    n = np.cross(unit_vector(base_p), z_vector)

    # the distance between the plane generated by the circle and the center of the sphere
    d = np.dot(c_c - c_s, n)

    if abs(d) > leg:
        print "No intersection"

    if d == leg:
        print "Only one intersection"

    # center position of sphere circle cut by plane of intersection
    c_p = c_s + (d * n)

    # radius of sphere circle cut by plane
    r_p = np.sqrt(leg ** 2 - d ** 2)

    # distance between the center of the circle and the center of the new circle cut from the sphere
    d_p = np.sqrt((c_c[0] - c_p[0]) ** 2 + (c_c[1] - c_p[1]) ** 2 + (c_c[2] - c_p[2]) ** 2)

    # the distance from the circle center and the line of intersection between the two circles
    h = (arm **2 - r_p**2 + d_p**2) / (2 * d_p)

    # center point of the line of intersection between the two circles
    c_i = base_p + (h * unit_vector(c_p - base_p))

    # the length from the center point (c_i) to the point of intersection
    r_i = np.sqrt(arm**2 - h**2)

    # the vector direction from the center point of intersection to the points of intersection
    t = unit_vector(np.cross(c_p - c_c, n))

    # the two points of intersection
    p0 = c_i - (t * r_i)
    p1 = c_i + (t * r_i)

    return p0


def inverseKinematics(pos):
    # get the estimated angle platform using ANFIS model
    motor_angles = getMotorAngles(pos[0], pos[1], pos[2])

    if motor_angles[0] > 270:
        print "MOTOR 0 LIMIT EXCEEDED!"
        motor_angles[0] = 270
    if motor_angles[1] > 270:
        print "MOTOR 1 LIMIT EXCEEDED!"
        motor_angles[1] = 270
    if motor_angles[2] > 270:
        print "MOTOR 2 LIMIT EXCEEDED!"
        motor_angles[2] = 270
    if motor_angles[0] < 90:
        print "MOTOR 0 LIMIT EXCEEDED!"
        motor_angles[0] = 90
    if motor_angles[0] < 90:
        print "MOTOR 1 LIMIT EXCEEDED!"
        motor_angles[0] = 90
    if motor_angles[0] < 90:
        print "MOTOR 2 LIMIT EXCEEDED!"
        motor_angles[0] = 90

    # print "Angles: ", theta1, theta2, theta3
    return motor_angles




