import numpy as np
import matplotlib.pyplot as plt

fs = 5e6
fref = 1e6
ascan = 2e-3/633e-9
fscan = 50.0
leakage = 0.2


t = np.arange(0.0, 0.1, 1/fs)
ref = np.exp(2.0j*np.pi*fref*t)
position = ascan*np.sin(2.0*np.pi*fscan*t)
meas = np.exp(2.0j*np.pi*(fref*t + position)) + leakage*ref


demod = np.conjugate(ref)*meas
estimated_leakage = np.real(np.sum(demod)/len(demod))
estimated_position = np.unwrap(np.angle(demod - estimated_leakage))/(2.0*np.pi)


print(estimated_leakage)
plt.plot(estimated_position)
plt.plot(position)
plt.show()