function f = geo_project()
% prototype code for closed-form geometric arc projection
% 2016/02/01 marin@zoox.com: original version using symbolic output  
% 2016/07/01 marin@zoox.com: optimized using precomputed A,b terms

syms x0 y0 a0 wz vx vy x y t k real

g0 = se2_g([a0;x0;y0]);

%wz = 0

if wz==0
  g = se2_g(t*[0; vx; vy])
else
  g = se2_exp(t*[wz; vx; vy])
end

e = g(1:2,3) - [x; y]

de = simplify(jacobian(e, t))

s = simplify(de'*e)

% try to solve for t
t=solve(s, t)

% if needed use Grobner basis
%s = subs(s, {'cos(t*wz)', 'sin(t*wz)'}, {'(1 - k * k) / (1 + k * k)', '2 * k / (1 + k * k)'})
%k = solve(s, k)

% turns out in this case the original equation is linear in cos and sin so can be solved directly:
%s = (vx^2*sin(t*wz) + vy^2*sin(t*wz) - vx*wz*x*cos(t*wz) - vy*wz*y*cos(t*wz) + vy*wz*x*sin(t*wz) - ...
% vx*wz*y*sin(t*wz))/wz
% => (vx^2 + vy^2 + vy*wz*x - vx*wz*y)*sin(t*wz) + (- vx*wz*x - vy*wz*y)*cos(t*wz) = 0
% tan(t*wz) = (vx*wz*x + vy*wz*y)/(vx^2 + vy^2 + vy*wz*x - vx*wz*y);

t = atan((vx*wz*x + vy*wz*y)/(vx^2 + vy^2 + vy*wz*x - vx*wz*y))/wz;

gt=se2_exp(t*[wz;vx;vy]);

simplify(gt(1,3))
simplify(gt(2,3))


% example setup:

xi = [1.2; 5; 1];
g0 = eye(3);
gf = se2_exp(xi);

hold off
ps = arc(eye(3), xi, 0:.1:1);
plot(ps(1,:), ps(2,:), '-.')
hold on

for i=1:20
  p = 18*rand(2,1)-[6; 6];
  [g, t, d] = closest(g0, gf, p, 1e-10);
  plot(p(1), p(2), '*');
  plot([p(1),g(1,3)]', [p(2), g(2,3)]' ,'-')
end
axis equal



function [g, t, d] = closest(g0, gf, p, eps)
% finds the closest pose g on a geodesic between two poses g0 and gf
% closest to a given point p
% t satisfies 0<=t<=1 and returs the flow offset along the geodesic of the closest pose
% g is the closest pose
% d is the closest distance

g0i = se2_inv(g0);
xi = se2_log(g0i*gf);

% xi above defines the geodesic vector field
% note that for a regular arc, this vector field is of the form
% xi = [kappa; 1; 0]; 
% where kappa is the cuvature
% in general the third (lateral) component can be non-zero
% in which case the geodesic is a section of an ellipse

% convert into g0's frame in homogeneous coordinates
ph = g0i*[p; 1];
ph=ph(1:2);

x = ph(1); 
y = ph(2);
wz=xi(1); vx = xi(2); vy = xi(3);

if abs(wz) < eps
  t = (vx*x + vy*y)/(vx^2 + vy^2);
 else
   a1 = vy/wz;
   a2 = vx/wz;
   A= [a1, a2; -a2, a1];
   b = [-a1; a2];
   
   c = wz*A'*ph - wz*A'*b;   
   c = c/norm(c);
   
   % direction is (c(1),c(2))
   t = atan2(c(2), c(1))/wz;
   % NOTE: the above is equivalent to 
   % t = atan((vx*wz*x + vy*wz*y)/(vx^2 + vy^2 + vy*wz*x -
   % vx*wz*y))/wz;
   % but one could precompute A,b, and reuse for many lookups
   % also computing the closest position p is super fast with A,b
end

% if the projection is outside of the arc b/n the two poses
% then we use the closest pose as the projected pose
if (t<0 || t>1)  
  pc0 = g0(1:2,3);
  pcf = gf(1:2,3);
  d0 = norm(p - pc0);
  df = norm(p - pcf);
  
  if d0 < df
    t = 0;
    d = d0;
    g = g0;
  else
    t = 1;
    d = df;
    g = gf;
  end
    return    
end

% the fastest way to compute g is to call
% g = g0*se2_exp(t*xi);


% but it is more efficient avoid calling exp a second time
% so instead follow the procedure below:

% closest position
pc = A*c + b;

% tangent to arc
dpc = A*[-c(2); c(1)];
dpcu = dpc/norm(dpc);

g = g0*[dpcu(1), -dpcu(2), pc(1);
        dpcu(2), dpcu(1), pc(2);
        0, 0, 1];

d = norm(p - g(1:2,3));


function ps = arc(g0, xi, ts)
ps = [];
for i=1:length(ts)
  g = g0*se2_exp(ts(i)*xi);
  ps = [ps, g(1:2,3)];
end
