#!/usr/bin/env python
# coding: utf-8

# In[5]:


from __future__ import division
import sympy
from sympy import *
import numpy as np
import matplotlib.pyplot as plt
from sympy import pprint as pp

# Enable LaTeX-based rendering
#%load_ext sympy.interactive.ipythonprinting
# New way of setting printing:
from sympy import init_printing
init_printing()

from IPython.display import display
from IPython.display import Image


# # 1 - Design parameters
# <img src="https://raw.githubusercontent.com/ubuntuslave/omnistereo_sensor_design/master/images/geometric_model.png" title='Geometric Model' width='300px' alt='Omnistereo Geometric Model' />

# In[ ]:


w, h, b, d, c1, c2, k1, k2, r_sys, r_ref = symbols("w, h, b, d, c_1, c_2, k_1, k_2, r_{sys}, r_{ref}", real=True)
# Constraints for hyperboloids:
k1_constraint = k1 > 2
k2_constraint = k2 > 2
c1_constraint = c1 > 0
c2_constraint = c2 > 0
xw, yw, zw = symbols("x_w, y_w, z_w", real=True)
# Local image:
# Image(filename='../images/geometric_model.png')


# ## System Height

# From `Geometry Expressions`, we obtain:
# 
# $h=
# \left\lvert{\dfrac{c_{1}}{2} + \dfrac{c_{2}}{2} - d + \sqrt{\dfrac{c_{1}^{2} \left(k_{1} - 2\right)}{4 k_{1}} + w^{2} \left(\dfrac{k_{1}}{2} + 1\right)} + \sqrt{\dfrac{c_{2}^{2} \left(k_{2} - 2\right)}{4 k_{2}} + w^{2} \left(\dfrac{k_{2}}{2} + 1\right)}}\right\rvert
# $

# In[322]:


h_eqn = abs((c1/2)+(c2/2) - d + sqrt((k1-2)*(c1)**(2)/(4*k1)+ (w**2)*(1+k1/2))+ sqrt((k2-2)*(c2)**(2)/(4*k2)+ (w**2)*(1+k2/2)))
h_eqn
display(simplify(h_eqn))
baseline_eqn = b-(c1 + c2 - d)
pp(baseline_eqn)
#print(sympy.latex(h_eqn))
#h_eqn
#print(sympy.latex(baseline_eqn))
baseline_sln = solve(baseline_eqn, b)
display(baseline_sln)


# ## Lens Hole Radius

# From `Geometry Expressions`, we obtain:
# 
# $\beta = arctan\left (\dfrac{r_{sys}}{\dfrac{c_{1}}{2}+\sqrt{\dfrac{c_{1}^{2}\cdot \left (-2+k_{1}\right )}{4\cdot k_{1}}+r_{sys}^{2}\cdot \left (-1+\dfrac{k_{1}}{2}\right )}}\right )$
# 
# $r_{\beta} =\begin{array}{l}\dfrac{\sqrt{k_{1}}\cdot r_{sys}\cdot \left (2\cdot d\cdot \sqrt{k_{2}}-c_{2}\cdot \sqrt{k_{2}}-c_{2}\cdot \sqrt{-2+k_{2}}\right )}{\sqrt{k_{2}}\cdot \left (c_{1}\cdot \sqrt{k_{1}}+\sqrt{-2\cdot c_{1}^{2}+c_{1}^{2}\cdot k_{1}-4\cdot k_{1}\cdot r_{sys}^{2}+2\cdot k_{1}^{2}\cdot r_{sys}^{2}}\right )}\\d-\dfrac{c_{2}}{2}-\dfrac{c_{2}\cdot \sqrt{-2+k_{2}}}{2\cdot \sqrt{k_{2}}}>0\end{array}$

# # 2 - Mirror profiles (expressions)

# ## Mirror 1 (Top)

# \begin{align}
#  {\left(z_1 - \frac{{{c_1}}}{2} \right)^2} - ({x_1^2} + {y_1^2})\left(\frac{{{k_1}}}{2} - 1\right) &= \frac{{c_1^2}}{4}\left(\frac{{{k_1} - 2}}{{{k_1}}}\right)
# \end{align}

# In[294]:


x1, y1, z1 = symbols("x_1, y_1, z_1", real=True)


# In[324]:


mirror_1_eqn = (z1 - c1/2)**2 - (x1**2 + y1**2)*(k1/2 - 1) - (c1**2/4)*((k1 - 2)/k1) 
display(expand(mirror_1_eqn))
mirror_1_eqn_top = z1 - c1/2 - abs(sqrt((x1**2 + y1**2)*(k1/2 - 1) + (c1**2/4)*((k1 - 2)/k1)))
display(mirror_1_eqn_top)
display(expand(mirror_1_eqn_top))


# ## Line passing through points $P_w$ and $F_1$ 

# ### Line equations:

# In[296]:


lambda1, s1 = symbols("lambda_1, s_1", real=True)
pos_F1 = (0, 0, c1)

x1_on_line = xw + s1 * (pos_F1[0]-xw)
display(x1_on_line)

y1_on_line = yw + s1 * (pos_F1[1]-yw)
display(y1_on_line)

z1_on_line = zw + s1 * (pos_F1[2]-zw)
display(z1_on_line)


# Let $\lambda_1 = 1 - s_1$, so that:

# In[297]:


s1_eqn = 1 - lambda1

xr1_on_line = simplify(x1_on_line.subs({s1: s1_eqn}))
display(xr1_on_line)

yr1_on_line = simplify(y1_on_line.subs({s1: s1_eqn}))
display(yr1_on_line)

zr1_on_line = expand(z1_on_line.subs({s1: s1_eqn}))
display(zr1_on_line)


# In[298]:


mirror_1_intersection = mirror_1_eqn_top.subs({x1: xr1_on_line, y1: yr1_on_line, z1:zr1_on_line})
display(mirror_1_intersection)


# ## Solving for intersection point $P_1$ 

# In[299]:


lambda1_sln = solve([mirror_1_intersection], lambda1)
lambda1_sln_minus = simplify(lambda1_sln[0])
display(lambda1_sln_minus)
lambda1_sln_plus = together(lambda1_sln[1])
display(lambda1_sln_plus)


# In[300]:


lambda1_func_minus = lambdify([c1, k1, xw, yw, zw], lambda1_sln_minus, modules=['numpy'])
lambda1_func_plus = lambdify([c1, k1, xw, yw, zw], lambda1_sln_plus, modules=['numpy'])


# $\lambda_1$, can be simplified to be
# 
# \begin{align}
#     \lambda_1=\frac{c_1}{L_{v_1} \sqrt{k_1\cdot(k_1 - 2)} 
#     + k_1 \left(c_1 - z_w \right)}
# \end{align}
# 
# where $L_{v_1} = ||{\vec{f_1} - \vec{p_w}}|| =|\sqrt{x_w^2+y_w^2+(c_1 - z_w)^2} |$, which is the distance between $P_w$
# and mirror 1's focus point $F_1$.
# 

# In[301]:


L1w = sqrt(xw**2 + yw**2 + (c1-zw)**2)
lambda1_simple = c1/(L1w*sqrt(k1*(k1-2)) + k1*(c1-zw))
display(lambda1_simple)
lambda1_simple_den = (abs(sqrt(k1*(k1-2)*xw**2 + yw**2 + (c1-zw)**2)) + k1*(c1-zw))
den1_simple = lambda1_simple_den*lambda1_simple_den


# ## Mirror 2 (Bottom)

# \begin{align}
#   {\left(z_2 - \left(d - \frac{c_2}{2}\right) \right)^2} - ({x_2^2} + {y_2^2})\left(\frac{{{k_2}}}{2} - 1\right) &= \frac{{c_2^2}}{4}\left(\frac{{{k_2} - 2}}{{{k_2}}}\right)
# \end{align}

# In[325]:


x2, y2, z2, = symbols("x_2, y_2, z_2", real=True)
mirror_2_eqn = (z2 - (d-c2/2))**2 - (x2**2 + y2**2)*(k2/2 - 1) - (c2**2/4)*((k2 - 2)/k2)
display(expand(mirror_2_eqn))
mirror_2_eqn_bottom = z2 - d + c2/2 + abs(sqrt((x2**2 + y2**2)*(k2/2 - 1) + (c2**2/4)*((k2 - 2)/k2)))
display(mirror_2_eqn_bottom)


# ## Line passing through points $P_w$ and $F_2$ 

# ### Line equations:

# In[303]:


lambda2, s2 = symbols("lambda_2, s_2", real=True)
pos_F2 = (0, 0, d - c2)

x2_on_line = xw + s2 * (pos_F2[0]-xw)
display(x2_on_line)

y2_on_line = yw + s2 * (pos_F2[1]-yw)
display(y2_on_line)

z2_on_line = zw + s2 * (pos_F2[2] - zw) 
display(z2_on_line)


# Let $\lambda_2 = 1 - s_2$, so that:

# In[304]:


s2_eqn = 1 - lambda2

xr2_on_line = simplify(x2_on_line.subs({s2: s2_eqn}))
display(xr2_on_line)

yr2_on_line = simplify(y2_on_line.subs({s2: s2_eqn}))
display(yr2_on_line)

zr2_on_line = simplify(z2_on_line.subs({s2: s2_eqn}))
display(zr2_on_line)
display(expand(zr2_on_line))
display(zw*lambda2 + (d-c2)*(1-lambda2))


# In[305]:


mirror_2_intersection = mirror_2_eqn_bottom.subs({x2: xr2_on_line, y2: yr2_on_line, z2:zr2_on_line})
display(mirror_2_intersection)


# ## Solving for intersection point $P_2$ 

# In[306]:


lambda2_sln = solve([mirror_2_intersection], lambda2)
lambda2_sln_minus = simplify(lambda2_sln[0])
display(lambda2_sln_minus)
lambda2_sln_plus = simplify(lambda2_sln[1])
display(lambda2_sln_plus)
display(lambda1_sln_plus)


# In[307]:


lambda2_func_minus = lambdify([c2, k2, xw, yw, zw, d], lambda2_sln_minus, modules=['numpy'])
lambda2_func_plus = lambdify([c2, k2, xw, yw, zw, d], lambda2_sln_plus, modules=['numpy'])


# $\lambda_2$, can be simplified to be
# 
# \begin{align}
#     \lambda_2=\frac{c_2}{L_{v_2} \sqrt{k_2\cdot(k_2 - 2)} 
#     - k_2 \left(d-c_2-z_w \right)}
# \end{align}
# 
# where $L_{v_2} = ||{\vec{f_2} - \vec{p_w}}|| =|\sqrt{x_w^2+y_w^2+(d-c_2-z_w)^2} |$, which is the distance between $P_w$
# and mirror 2's focus point $F_2$.
# 

# In[308]:


display(expand(d - lambda2*zw - (d - c2)*(1 - lambda2)))
display(expand(c2 + lambda2*(d - c2 - zw)))


# ## Fielf of View Angles

# From `Geometry Expressions`, we obtain:
# 
# 
# \begin{align}
# \theta_{1,max} = -arctan\left (\dfrac{c_{1}\cdot \sqrt{k_{1}}-\sqrt{-2+k_{1}}\cdot \sqrt{c_{1}^{2}+2\cdot k_{1}\cdot r_{sys}^{2}}}{2\cdot \sqrt{k_{1}}\cdot r_{sys}}\right )
# \end{align}
# 
# or by `Mathematica`:
# 
# \begin{align}
# \frac{\arctan \left(\sqrt{\frac{(k_1-2) \left(c_1^2+2 k_1 r^2_{sys} \right)}{k_1}}-c_1\right)}{2 r_{sys}}
# \end{align}

# In[309]:


theta_1_max_eqn = -atan((c1* sqrt(k1)-sqrt(-2+k1)* sqrt(c1**2 + 2* k1 * r_sys**2))/(2*sqrt(k1) * r_sys))
display(ratsimp(theta_1_max_eqn))


# From `Geometry Expressions`, we obtain:
# 
# $$\theta_{1,min} = \arctan\left (\dfrac{\dfrac{-d}{2}+c_{1}}{r_{ref}}\right )$$

# In[310]:


theta_1_min_eqn = atan((-d/2+c1)/(r_ref))
display(cancel(theta_1_min_eqn))


# From `Geometry Expressions`, we obtain:
# 
# $\theta_{2,min} = arctan\left (\dfrac{\dfrac{-c_{2}}{2}+\sqrt{\dfrac{c_{2}^{2}\cdot \left (-2+k_{2}\right )}{4\cdot k_{2}}+r_{sys}^{2}\cdot \left (-1+\dfrac{k_{2}}{2}\right )}}{r_{sys}}\right )$

# In[311]:


theta_2_min_eqn = atan((-c2/2 + sqrt((c2**2 *(-2+k2))/(4*k2)+r_sys**2 * (-1+k2/2)))/r_sys)
display(trigsimp(theta_2_min_eqn))
display(cancel(theta_2_min_eqn))


# # Back Projections

# ## BP Mirror 1:

# In[314]:


q1, xq1, yq1, zq1, t1, theta1, phi1 = symbols("q_1, x_q1, y_q1, z_q1, t_1, theta_1, phi_1", real=True)
x1_bp_line_eqn = t1*xq1
y1_bp_line_eqn = t1*yq1
z1_bp_line_eqn = t1
mirror1_bp_intersection = mirror_1_eqn_top.subs({x1: x1_bp_line_eqn, y1: y1_bp_line_eqn, z1:z1_bp_line_eqn})
display(mirror1_bp_intersection)
t1_sln = solve([mirror1_bp_intersection], t1)
t1_sln_minus = simplify(t1_sln[0])
display(t1_sln_minus)
t1_sln_plus = together(t1_sln[1])
display(t1_sln_plus)


# Simplifying the `+`  solution of $t_1$, we get:
# \begin{align}
#     t_1=\frac{c_1}{k_1-||{q_1}||\sqrt{k_1\cdot(k_1 - 2)}}
# \end{align}

# ## BP Mirror 2:

# In[318]:


q2, xq2, yq2, zq2, t2, theta2, phi2 = symbols("q_2, x_q2, y_q2, z_q2, t_2, theta_2, phi_2", real=True)
x2_bp_line_eqn = t2*xq2
y2_bp_line_eqn = t2*yq2
z2_bp_line_eqn = d-t2
mirror2_bp_intersection = mirror_2_eqn_bottom.subs({x2: x2_bp_line_eqn, y2: y2_bp_line_eqn, z2:z2_bp_line_eqn})
display(mirror2_bp_intersection)
t2_sln = solve([mirror2_bp_intersection], t2)
t2_sln_minus = simplify(t2_sln[0])
display(t2_sln_minus)
t2_sln_plus = together(t2_sln[1])
display(t2_sln_plus)


# ## Test and Plot Forward Projections

# # Horizontal Depth from Triangulation

# From `Geometry Expressions`:
# 
# \begin{align}
# D=\left |\dfrac{-\left (-d+c_{1}+c_{2}\right )\cdot cos\left (\theta_{1}\right )\cdot cos\left (\theta_{2}\right )}{sin\left (\theta_{2}\right )\cdot cos\left (\theta_{1}\right )+sin\left (\theta_{1}\right )\cdot cos\left (\theta_{2}\right )}\right |
# \end{align}

# In[27]:


theta1, theta2 = symbols("theta_1, theta_2", real=True)
D_eqn = simplify(-B*cos(theta1)*cos(theta2)/(sin(theta2)*cos(theta1)+sin(theta1)*cos(theta2)))
display(D_eqn)
D_eqn_2 = B*sin(pi/2-theta1)*sin(pi/2-theta2)/sin(pi-theta1-theta2)
display(D_eqn_2)
D_eqn == D_eqn_2