% Relates the approximate parameters (q) to the exact
% parameters (p).  Adapted from video orbits code.
% Model:   u = a + bx + cy + gx^2 + hxy
%          v = d + ex + fy + gxy  + hy^2

function [p] = q_to_p(q, g)

% Find the dimensions of g
gx = size(g,2);
gy = size(g,1);

% Create matrix of original coordinates (x, y)
%OC = [ 0 0; 0 gy-1; gx-1 0; gx-1 gy-1 ];
OC = [0 0; 0 1; 1 0; 1 1];

% Once again, we have a linear equation to solve (NC = A*pvec)

% Build vector NC (new coordinates)
NC = zeros(8,1);
for m = 1:4
  x = OC(m,1);
  y = OC(m,2);
  % x' = a + bx + cy + gx^2 + hxy
  NC(2*m-1) = q(1) + q(2)*x + q(3)*y + q(7)*x^2 + q(8)*x*y;
  %NC(2*m-1) = q(1) + q(2)*y + q(3)*x + q(7)*y^2 + q(8)*x*y;
  % y' = d + ex + fy + gxy  + hy^2
  NC(2*m)   = q(4) + q(5)*x + q(6)*y + q(7)*x*y + q(8)*y^2;
  %NC(2*m)   = q(4) + q(5)*y + q(6)*x + q(7)*x*y + q(8)*x^2;
end

% Switch x and y to return to sane land
OC = fliplr(OC);
NCn = reshape(NC,2,4)';
NCn = fliplr(NCn);
NC = reshape(NCn',8,1);

% Build matrix A
A = zeros(8);
for m = 1:4
  x = OC(m,1);
  y = OC(m,2);
  xp = NC(2*m-1);
  yp = NC(2*m);
  A(2*m-1,:) = [x, y, 1, 0, 0, 0, -x*xp, -y*xp];
  A(2*m,:)   = [0, 0, 0, x, y, 1, -x*yp, -y*yp];
end

% Solve for pvec
pvec = A \ NC;

% Rearrange into the standard transformation matrix
% pvec = [a_x'x, a_x'y, b_x', a_y'x, a_y'y, b_y', c_x, c_y]';
p = [ pvec(1), pvec(2), pvec(3);
      pvec(4), pvec(5), pvec(6);
      pvec(7), pvec(8), 1        ];