import numpy as np
import matplotlib.pyplot as plt
import sympy as sp

d=lambda vec,var: [sp.diff(comp,var) for comp in vec]

t=sp.var('t')

r_curve=[7*sp.cos(t),4*sp.sin(t)]
tmin=0
tmax=2*np.pi

plt.xlim(-10,10)
plt.ylim(-10,10)

curve_count=200
curve_times=np.linspace(tmin,tmax,curve_count)

t0=np.pi/4

def table(expr,var,inputs):
    return([sp.N(expr.subs(var,i)) for i in inputs])

def xyt_table(expr,inputs):
    return([table(expr[i],t,inputs) for i in (0,1)])

vel=d(r_curve,t)
accel=d(vel,t)

dot=lambda u,v:sum([a*b for (a,b) in zip(u,v)])
mag=lambda u:sp.sqrt(dot(u,u))
scale=lambda c,u:[c*a for a in u]
plus=lambda u,v:[x+y for (x,y) in zip(u,v)]
unit=lambda u:scale(1/mag(u),u)
proj=lambda u,v:scale(dot(u,v)/dot(u,u),u)
at=lambda t0,v:[a.subs(t,t0) for a in v]

aperp=plus(accel,scale(-1,proj(vel,accel)))
unit_tan=unit(vel)
unit_nor=unit(aperp)
radius=dot(vel,vel)/mag(aperp)

angle_count=200
angles=np.linspace(0,2*np.pi,angle_count)

def osc_circ(t0):
    r0=at(t0,r_curve)
    T=at(t0,unit_tan)
    N=at(t0,unit_nor)
    R=radius.subs(t,t0)
    print(R)
    r=plus(r0,scale(R,plus(scale(sp.cos(t),T),scale(1+sp.sin(t),N))))
    return xyt_table(r,angles)

x,y=osc_circ(t0)
plt.plot(x,y)

x,y=xyt_table(r_curve,curve_times)
plt.plot(x,y)

plt.show()
