% This script shows the procedure to obtain the degree 2
% polynomial equation used in section 4.3 in our paper.
clear all
close all
clc

syms m1x m2x m3x m1 m2 m3 m1y m2y m3y x y
% % definition of the symbols
% % m = (m1;m2;m3); % reflection correspondences on the reference plane.
% Note that m, together with m1, m2, and m3 are functions of (x,y). 
% mix:partial derivative of mi w.r.t. x, where i\in{1,2,3}
%% %%%%%%%%% polynomial equation %%%%%%%%%%%%%%%

% reflection correspondence
m1 = sym('m1(x,y)');
m2 = sym('m2(x,y)');
m3 = sym('m3(x,y)');
m = [m1;m2;m3];

% depth
s = sym('s(x,y)');

% image point on z = 1 or visual ray corresponding to (x,y,1)
v = [x;y;1]; 

% unnormalized normal
n = sqrt(transpose(v)*v)*(m-s*v)-v*sqrt(transpose(m-s*v)*(m-s*v));

% \partial n/\partial x and \partial n/\partial y 
f_x = diff(n,x);
f_y = diff(n,y);

% x,y,z coordinates of n
nx = n(1);
ny = n(2);
nz = n(3);

f_xn = subs(f_x,'diff(s(x,y),x)','-s(x,y)*nx/(x*nx+y*ny+nz)');
f_xn = subs(f_xn,{'nx','ny','nz'},{nx,ny,nz});
f_yn = subs(f_y,'diff(s(x,y),y)','-s(x,y)*ny/(x*nx+y*ny+nz)');
f_yn = subs(f_yn,{'nx','ny','nz'},{nx,ny,nz});

p = [y*ny+nz;-y*nx;-nx];
q = [-x*ny;x*nx+nz;-ny];


% eqn = \partial^2 s/(\partial x \partial y) - \partial^2 s/(\partial y \partial x) 
eqn = transpose(p)*f_yn-transpose(q)*f_xn;
eqn_sim = subs(eqn,{'s(x,y)','diff(m1(x,y),x)','diff(m2(x,y),x)','diff(m3(x,y),x)','diff(m1(x,y),y)','diff(m2(x,y),y)','diff(m3(x,y),y)','m1(x,y)','m2(x,y)','m3(x,y)'},{'s','m1x','m2x','m3x','m1y','m2y','m3y','m1','m2','m3'});

% compute the numerator 
[nors des] = numden(eqn_sim);

eqn_s = collect(nors,'s');

% show the numerator in a nicer form
pretty(collect(nors,'s'))

%% Computation of the coefficients of the final polynomial. These coefficients rely on the values A, B, C, a, b, p, q and t in Eq. 12 of our paper.
% corresponding to #1 in the previous pretty(collect(nors,'s'))
poly1 = (x*x+y*y+1);

% corresponding to #3 in the previous pretty(collect(nors,'s'))
poly3 = 2*m3+2*x*m1+2*y*m2;

% value A
tmpA = m2x*poly1^2 - m1y*poly1^2+m2x*x^2*poly1^2-m1y*y^2*poly1^2+m2*x*poly1-m1*y*poly1-m2*x*poly1^2+m2*x^3*poly1+m3y*x*poly1^2+m1*y*poly1^2-m1*y^3*poly1-m3x*y*poly1^2-m1*x^2*y*poly1+m2*x*y^2*poly1-m1x*x*y*poly1^2+m2y*x*y*poly1^2;

% value a
tmpa = m2x*poly1^(3/2)-m1y*poly1^(3/2)+m2x*x^2*poly1^(3/2)-m1y*y^2*poly1^(3/2)+m3y*x*poly1^(3/2)-m3x*y*poly1^(3/2)-m1x*x*y*poly1^(3/2)+m2y*x*y*poly1^(3/2);

% coefficient of the degree 4 term in the derived polynomial
disp('Coefficient of s^4 in the polynomial:')
testorder4 = tmpA^2 - tmpa^2*poly1;
simplify(tmpA^2 - tmpa^2*poly1) % degree 4 coefficient
disp('Since testorder4=0, the degree 4 term in the polynomial equation disappears.')

% value B
tmpB = m1*y*poly3 - m2*x*poly3+m2*m3x*poly1-m1*m3y*poly1-m2*x^3*poly3+m1*y^3*poly3-m2x*poly1*poly3+m1y*poly1*poly3+m2*x*poly1*poly3-m3y*x*poly1*poly3-m1*y*poly1*poly3+m3x*y*poly1*poly3+m2*m1x*x^3*poly1+m2*m3x*x^2*poly1...
    +m3*m1y*x^2*poly1-m3*m2x*y^2*poly1-m1*m2y*y^3*poly1-m1*m3y*y^2*poly1-m2x*x^2*poly1*poly3+m1y*y^2*poly1*poly3+m2*m1x*x*poly1-m1*m1y*x*poly1+m3*m3y*x*poly1+m2*m2x*y*poly1-m3*m3x*y*poly1-m1*m2y*y*poly1+m1*x^2*y*poly3...
    -m2*x*y^2*poly3-m1*m3x*x*y*poly1-m3*m1x*x*y*poly1+m2*m3y*x*y*poly1+m3*m2y*x*y*poly1+m1x*x*y*poly1*poly3-m2y*x*y*poly1*poly3-m1*m1x*x^2*y*poly1-m1*m2x*x*y^2*poly1+m2*m2x*x^2*y*poly1-m1*m1y*x*y^2*poly1+m2*m1y*x^2*y*poly1...
    +m2*m2y*x*y^2*poly1;

% value b
tmpb = m2*m3x*poly1^(3/2)-m3*m2x*poly1^(3/2)-m1*m3y*poly1^(3/2)+m3*m1y*poly1^(3/2)-m1*m2x*x*poly1^(3/2)+m2*m1x*x*poly1^(3/2)-m1*m2y*y*poly1^(3/2)+m2*m1y*y*poly1^(3/2);

% coefficient of s in #2
tmpQ = -poly3;

% coefficient of the degree 3 item in the derived polynomial
testorder3 = 2*tmpA*tmpB-(tmpa^2*tmpQ+2*tmpa*tmpb*poly1);

disp('Coefficient of s^3 in the polynomial:')
simplify(testorder3)
disp('Since testorder3=0, the degree 3 term in the polynomial equation disappears.')

%% Obtaining the oefficients of the degree 2, 1, and 0 terms in the polynomial

% corresponding to #4
poly4 = m1^2 + m2^2 + m3^2;

% constant in #2
tmpT = poly4;

% coefficient of s^2 in #2
tmpP = poly1;

% value C
tmpC = (m2*x -m1*y)*poly4+(m2*x^3 - m1*y^3)*poly4+(-m2^2*m2x+m1^2*m1y)*poly1+(m2x-m1y)*poly1*poly4+(-m1*x+m2*y)*x*y*poly4+(-m3y*x+m3x*y)*m3^2*poly1+(-m2*x+m3y*x+m1*y-m3x*y)*poly1*poly4+(-m2^2*m2x*x^2+m1^2*m1y*y^2)*poly1+...
    (m2x*x^2-m1y*y^2)*poly1*poly4+(-m1*m1x-m3*m3x)*m2*poly1+(m2*m2y+m3*m3y)*m1*poly1+(-m1*m1y-m2*m2y)*m3*x*poly1+(m1*m1x+m2*m2x)*m3*y*poly1+(-m1*m1x-m3*m3x)*m2*x^2*poly1+(m2*m2y+m3*m3y)*m1*y^2*poly1+(m1^2*m1x-m2^2*m2y)*x*y*poly1+...
    (-m1x+m2y)*x*y*poly1*poly4+(m2*m2x+m3*m3x)*m1*x*y*poly1+(-m1*m1y-m3*m3y)*m2*x*y*poly1;

% coefficient of s^2
coefdegree2 = tmpB^2+2*tmpA*tmpC-tmpb^2*tmpP-2*tmpa*tmpb*tmpQ-tmpa^2*tmpT;

% coefficient of s
coefdegree1 = 2*tmpB*tmpC - tmpb^2*tmpQ - 2*tmpa*tmpb*tmpT;

% constant
coefdegree0 = tmpC^2 - tmpb^2*tmpT;

% solutions for the depth s (2 possible solutions, only one of them is valid, as shown in the example below)
s1 = (-coefdegree1+sqrt(coefdegree1*coefdegree1 - 4*coefdegree2*coefdegree0))/(2*coefdegree2);
s2 = (-coefdegree1-sqrt(coefdegree1*coefdegree1 - 4*coefdegree2*coefdegree0))/(2*coefdegree2);

%% example:  we provide one example to compute the depth of one point on the synthetic UCBS surface. t* denotes the numerical values corresponding to its synbolic item for computing the depth for one point on the mirror surface.
tx = -0.0675;
ty = -0.0947;
tm1 = 0.7923;
tm2 = -5.2947;
tm3 = -6.0000;
tm1x = -9.0420;
tm2x = -12.2681;
tm3x = 0;
tm1y = -11.6616;
tm2y = -19.8379;
tm3y = 0;

poly1val = vpa(subs(poly1,{'x','y'},{tx,ty}));
poly3val = vpa(subs(poly3,{'x','y','m1','m2','m3','m1x','m1y','m2x','m2y','m3x','m3y'},{tx,ty,tm1,tm2,tm3,tm1x,tm1y,tm2x,tm2y,tm3x,tm3y}));

s1val = vpa(subs(s1,{'x','y','m1','m2','m3','m1x','m1y','m2x','m2y','m3x','m3y'},{tx,ty,tm1,tm2,tm3,tm1x,tm1y,tm2x,tm2y,tm3x,tm3y}));
s2val = vpa(subs(s2,{'x','y','m1','m2','m3','m1x','m1y','m2x','m2y','m3x','m3y'},{tx,ty,tm1,tm2,tm3,tm1x,tm1y,tm2x,tm2y,tm3x,tm3y}));

%% test the solutions on (As^2+Bs+C+sqrt(ps^2+qs+t)=0 and As^2+Bs+C-sqrt(ps^2+qs+t)=0 to remove the solution introduced by the squaring strategy.
orgpoly = tmpA*s*s+tmpB*s+tmpC+(tmpa*s+tmpb)*sqrt(tmpP*s*s+tmpQ*s+tmpT);% our derived polynomial (As^2+Bs+C+sqrt(ps^2+qs+t)=0)
orgpolyAdd = tmpA*s*s+tmpB*s+tmpC-(tmpa*s+tmpb)*sqrt(tmpP*s*s+tmpQ*s+tmpT);% polynomial used to remove the square root As^2+Bs+C-sqrt(ps^2+qs+t)=0

% verify validity of the solutions for the original polynomial:As^2+Bs+C+sqrt(ps^2+qs+t)=0
orgpolyval_s1 = vpa(subs(orgpoly,{'x','y','m1','m2','m3','m1x','m1y','m2x','m2y','m3x','m3y','s'},{tx,ty,tm1,tm2,tm3,tm1x,tm1y,tm2x,tm2y,tm3x,tm3y,s1val}))
orgpolyval_s2 = vpa(subs(orgpoly,{'x','y','m1','m2','m3','m1x','m1y','m2x','m2y','m3x','m3y','s'},{tx,ty,tm1,tm2,tm3,tm1x,tm1y,tm2x,tm2y,tm3x,tm3y,s2val}))

disp('Only s2 is a root of the original polynomial.');

% verify the validity of the solutions for the introduced polynomial: As^2+Bs+C-sqrt(ps^2+qs+t)=0
orgpolyAddval_s1 = vpa(subs(orgpolyAdd,{'x','y','m1','m2','m3','m1x','m1y','m2x','m2y','m3x','m3y','s'},{tx,ty,tm1,tm2,tm3,tm1x,tm1y,tm2x,tm2y,tm3x,tm3y,s1val}))
orgpolyAddval_s2 = vpa(subs(orgpolyAdd,{'x','y','m1','m2','m3','m1x','m1y','m2x','m2y','m3x','m3y','s'},{tx,ty,tm1,tm2,tm3,tm1x,tm1y,tm2x,tm2y,tm3x,tm3y,s2val}))

disp('The solution s1 is a root of the polynomial used to remove the square root.');

% depsol: the true solution satisfies (As^2+Bs+C+sqrt(ps^2+qs+t)=0.
depsol = [];
if abs(double(orgpolyval_s1)) < 10e-10,
    depsol = [depsol double(s1val)];
end

if abs(double(orgpolyval_s2)) < 10e-10,
    depsol = [depsol double(s2val)];
end

groundtruthDepth = 5.2840; % ground truth depth
errdepth = abs(depsol - groundtruthDepth); % estimation error for the depth of the seed point

disp('"errpnt" shows the reconstruction error between two 3D points');
errpnt = norm(depsol*[tx ty 1]' - groundtruthDepth*[tx ty 1]')



