from math import *
import matplotlib.pyplot as plt
import numpy as np
	
# alpha is half of vertical fov
# sigma is angle between laser plane and camera center vector
# phi is angle between camera center vector and target pixel vector
# d is distance between laser plane and camera
# h is vertical pixel count
alpha = pi/180*21.5
sigma = pi/180*51
r = 0.05
d = r*tan(sigma)*.65
#d = r
h = 3120
phi_r = atan(d/r)-sigma

def get_phi(y, h, alpha):
	# get phi of pixel row y
	return atan(tan(alpha)*(2*y/h - 1))

def crunch(alpha, sigma, phi, d, h):
	# depth delta approximation
	da = 2*d*tan(alpha)*cos(phi)/(h*sin(sigma+phi)**2)
	# te is tan(epsilon)
	# epsilon is angle increment in phi across one pixel
	te = (2*tan(alpha)*cos(phi)**2/(h + 2*tan(alpha)*sin(phi)*cos(phi)))
	# exact depth delta
	#de = d/sin(sigma+phi)*te/(sin(sigma+phi) + te*sin(sigma+phi))
	de = 2*d*tan(alpha)*cos(phi)*(cos(phi) - sin(phi)*te)/(h*sin(sigma+phi)*(sin(sigma+phi) + cos(sigma+phi)*te))
	return {'da':da, 'te':te, 'de':de, 'ea':de-da, 'er':(de-da)/de}

def nprint(phi_text, phi):
	nums = crunch(alpha, sigma, phi, d, h)
	print(phi_text)
	print(f"Phi:              {phi}")
	print(f"Delta approx.:    {nums['da']}")
	print(f"Tan(eps):         {nums['te']}")
	print(f"Delta exact:      {nums['de']}")
	print(f"Error (absolute): {nums['ea']}")
	print(f"Error (relative): {nums['er']}")
	print(f"Height:           {d/tan(sigma+phi)}")
	print("")

print(f"alpha is {alpha} ({alpha/pi} pi)")
print(f"sigma is {sigma} ({sigma/pi} pi)")
print(f"d at r={r} is {d}")

nprint('Maximal phi', alpha)
nprint('Zero phi', 0)
nprint('phi_r', phi_r)
nprint('Minimal phi', -alpha)
nprint('y=h/2', get_phi(h/2, h, alpha))

#for i in range(0,10):
#	nprint(f'phi = {i}/10*alpha', i/10*alpha)

print(f"phi at y = 50 is {get_phi(50, h, alpha)}")

# make data
xpoints = []
dapoints = []
depoints = []
epoints = []
aspan = 0
espan = 0
tspan = d*(1/tan(sigma-alpha) - 1/tan(sigma+alpha))
for y in range(h):
	xpoints.append(y)
	cronch = crunch(alpha, sigma, get_phi(y, h, alpha), d, h)
	dapoints.append(cronch['da'])
	aspan += cronch['da']
	depoints.append(cronch['de'])
	espan += cronch['de']
	epoints.append(cronch['er'])

print('approximate segment sum', aspan)
print('exact segment sum', espan)
print('expected sum', tspan)

xpoints = np.array(xpoints)
dapoints = np.array(dapoints)
depoints = np.array(depoints)
plt.plot(xpoints, dapoints, label = 'approx')
plt.plot(xpoints, depoints, label = 'exact')
plt.legend()
plt.show()
plt.plot(xpoints, epoints, label = 'error')
plt.legend()
plt.show()