from visual import *
import math
import numpy as np
from numpy.linalg import inv
# import random

# 1. Given p, e1, e2, 劃出兩個座標系統的XY軸，世界座標用紅色，視覺座標用藍色
scene.background = (1,1,1)
p = [2,1]
e1 = [a,b]=[1/sqrt(2), 1/sqrt(2)]
e2 = [c,d]=[-1/sqrt(2), 1/sqrt(2)]
pe1 = [p[i]+e1[i] for i in range(len(e1))]
pe2 = [p[i]+e2[i] for i in range(len(e2))]


curve(pos = [[0,0],[0,1]], color = color.red)
curve(pos = [[0,0],[1,0]], color = color.red)
curve(pos = [p,pe1], color = color.blue)
curve(pos = [p, pe2], color = color.blue)

#2 給定世界座標w，繪出該點(紅色)，並做視覺座標到世界座標的轉換
M = np.array([e1, e2])
w = [1,1]
points(pos = w, color = color.red)
minv = inv(M)
#print(minv)
# w, p, M represent in world coordinates
# e is in eye coordinats
def w2e(w, p, M):
    return dot([w[0]-p[0], w[1]-p[1]], inv(M))

#3 給定視覺座標，做視覺座標到世界座標的轉換，並繪出世界座標的點(藍色)
e = [1,1]
def e2w(e, p, M):
    M1 = dot(e, M)
    return [M1[0]+p[0], M1[1]+p[1]]

points(pos = e2w(e, p, M), color = color.blue)

#4 列印出轉換點
print("世界座標點w=", w, "轉換成視覺座標點e=", w2e(w, p, M))
print("視覺座標點e=", e, "轉換成世界座標點w=", e2w(w, p, M))

    
    


'''
theta = pi/4
r = 5
dad = sphere(pos = (0,0,0), color = color.yellow, opacity = 0.5, radius = r)
john = sphere(pos = (r,0,0), color = color.blue, velocity=vector(0, 1, 0),  radius=1)
arrow(pos = john.pos, axis = john.velocity)
#mary = sphere(pos = (-r,0,0), color = color.red)

scene.background = (1,1,1)

def norm(L):
    return sqrt(L[0]**2+L[1]**2+L[2]**2)

def normalize(L):
    length = norm(L)
    return (L[0]/length, L[1]/length, L[2]/length)

def inner(v1, v2):
    return v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2]

def angle(v1, v2):
    n1 = normalize(v1)
    n2 = normalize(v2)
#    print(n1)
#    print(n2)
    print("inner(n1, n2) = ", n1, n2, inner(n1,n2))
    if math.asin(inner(n1, n2)) < theta:
        return True
    else:
        return False

def random_vector(v,p):
    q = True
    while q:
        x = random.randint(1,8)
        y = random.randint(1,8)
        z = sqrt(100-x*x-y*y)
        if angle(v, (x,y,z)):
            q = false
    print([x,y,z])
    return normalize([x,y,z])

def transM(v,p):
    return [[v.y*p.z-v.z*p.y, v.z*p.x-v.x*p.z, v.x*p.y-v.y*p.x,1],v+[1], p+[1], [0,0,0,1]]

def random_vector(v,p):
    M = transM(v,p)

def on_sphere(p):
    p = normalize(p)
    return [r*p[0], r*p[1], r*p[2]]

def nextp(p, v):
    print(p)
    print(v)
    for i in range(3):
        p[i] = p[i] + v[i]
    print(p)
    p = on_sphere(p)
    print(p)
    john.velocity = random_vector(v,p)
    return p

while True:
    rate(10)
    john.pos =  nextp(john.pos, john.velocity)
    print(john.pos)
    
"""
The above random is not good base on the john wandering around.
Please modify it so that the sphere will fly smoothly
by restricting the random vector almost parallel to the current velocity
'''