#/usr/local/bin/octave -qf
max_xy_noise = 3;
max_xy = 100;
max_T = 10;
num_points = 5; % number of points
do_inverse = 0;
global tmpdir;
global binary;
tmpdir = "/tmp/fid";
binary = "../../bin/fiducial_alignment";

function [rotation, translation] = svd_transform(pts1, pts2)
  if nargin != 2, error("Missing parameters"); end
  assert(size(pts1) == size(pts2));
  centre1 = mean(pts1);
  centre2 = mean(pts2);
  N = size(pts1,1);
  H = (pts1 - repmat(centre1, N, 1))' * (pts2 - repmat(centre2, N, 1));
  [U,S,V] = svd(H);
  R = V*U';
  if det(R) < 0, V(:,2) *= -1; R = V*U'; end
  translation = -R * centre1' + centre2';
  rotation = atan2(R(2,1),R(2,2));
end

function [rotation, translation, refs, fids, refs_svd, fids_svd, ret_rotation, ret_translation ...
    ] = mk_test_data(num_points, max_xy, max_T, max_xy_noise, do_inverse)
  rotation = rand(1) * 2 * pi;
  R = [ cos(rotation), -sin(rotation) ; sin(rotation), cos(rotation) ];
  translation = max_T * rand(2,1);
  refs = max_xy * rand(num_points, 2);
  fids = R * refs' + repmat(translation, 1, num_points);
  fids = fids';
  fids = fids + max_xy_noise * rand(num_points, 2);
  if(do_inverse),
    [ret_rotation, ret_translation] = svd_transform(refs, fids);
    ret_R = [ cos(ret_rotation), -sin(ret_rotation) ; sin(ret_rotation), cos(ret_rotation) ];
    ret_T = ret_translation;
    refs_svd = ((ret_R * refs') + repmat(ret_T, 1, num_points))';
    fids_svd = fids;
  else
    [ret_rotation, ret_translation] = svd_transform(fids, refs);
    ret_R = [ cos(ret_rotation), -sin(ret_rotation) ; sin(ret_rotation), cos(ret_rotation) ];
    ret_T = ret_translation;
    fids_svd = ((ret_R * fids') + repmat(ret_T, 1, num_points))';
    refs_svd = refs;
  end
end

function [ok, rotation, translation, max_err, avg_err, refs_bin, fids_bin] = run_binary(refs, fids, do_inverse)
  global tmpdir;
  global binary;
  global max_xy_noise;
  tolerance = 0;
  ref_file = strcat(tmpdir, "/refs.csv");
  fid_file = strcat(tmpdir, "/fids.csv");
  if(do_inverse), ii=" --inverse"; else ii=""; end;
  cmd = sprintf("%s -v --csv%s --tolerance='%f' '%s' '%s'", binary, ii, tolerance, ref_file, fid_file);
  csvwrite(ref_file, refs);
  csvwrite(fid_file, fids);
  [status, text] = system(cmd);
  if(status ~= 0)
    ok = 0;
    rotation = 0;
    translation = [ 0 ; 0 ];
    max_err = 0;
    avg_err = 0;
    fids_bin = fids;
    refs_bin = refs;
  else
    ok = 1;
    c = textscan(text, "%f,%f,%f,%f,%f");
    rotation = c{1};
    translation = [ c{2} ; c{3} ];
    max_err = c{4};
    avg_err = c{5};
    ret_R = [ cos(rotation), -sin(rotation) ; sin(rotation), cos(rotation) ];
    ret_T = translation;
    if(do_inverse)
      fids_bin = fids;
      refs_bin = ((ret_R * refs') + repmat(ret_T, 1, rows(refs)))';
    else
      fids_bin = ((ret_R * fids') + repmat(ret_T, 1, rows(fids)))';
      refs_bin = refs;
    end
  end;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if(exist(binary, "file") == 0), error("Binary to test not found"); end;
if(exist(tmpdir, "dir") == 0), error("Temp dir not found"); end;

for(i=1:25)
  [r, t, refs, fids, refs_svd, fids_svd, r1, t1] = mk_test_data(num_points, max_xy, max_T, max_xy_noise, do_inverse);
  [ok, r2, t2, max_err, avg_err, refs_bin, fids_bin] = run_binary(refs, fids, do_inverse);
  if(ok), break; end;
end

if(ok == 0), error("25 trys done, stopping here"); end;

err1 = refs_svd - fids_svd;
err1 = err1 .* err1;
err1 = sum(err1(:));
err_sq_svd = sqrt(err1/num_points)

err2 = refs_bin - fids_bin;
err2 = err2 .* err2;
err2 = sum(err2(:));
err_sq_bin = sqrt(err2/num_points)

figure();
if(do_inverse)
  plot(fids(:,1), fids(:,2), 'ok');
  hold on;
  plot(refs_svd(:,1),refs_svd(:,2), 'xr');
  plot(refs_bin(:,1),refs_bin(:,2), '*b');
else
  plot(refs(:,1), refs(:,2), 'ok');
  hold on;
  plot(fids_svd(:,1),fids_svd(:,2), 'xr');
  plot(fids_bin(:,1),fids_bin(:,2), '*b');
end

saveas(gcf(), sprintf("%s/plot.png", tmpdir));

sleep(3);
