% get the transformation that maps the tee into a standard position
% with [0,0] at the centre of the head and with the tee stem
% in the positive direction

function [tr,tc,theta] = findteetransform(headset)

  % get rotation
  
  dir = headset(1,1:2) - headset(1,3:4);
  theta = atan2(dir(2),dir(1)) + pi/2;
  R = [ [ cos(theta),-sin(theta)]',[sin(theta),cos(theta)]'];

  % rotate all lines
  
  tmp = headset;
  for i = 1 : 4
    v1 = [headset(i,1),headset(i,2)]';
    v2 = [headset(i,3),headset(i,4)]';
    tv1 = R*v1;
    tv2 = R*v2;
    tmp(i,:) = [tv1(1),tv1(2),tv2(1),tv2(2)];
  end

  % see if need pi more rotation
  % (i.e. check for correct T orientation or not)
  
  if tmp(4,1) < tmp(1,1)
    theta = theta+pi;
    if theta > 2*pi
      theta = theta - 2*pi;
    end
    R = [ [ cos(theta),-sin(theta)]',[sin(theta),cos(theta)]'];
    for i = 1 : 4
      v1 = [headset(i,1),headset(i,2)]';
      v2 = [headset(i,3),headset(i,4)]';
      tv1 = R*v1;
      tv2 = R*v2;
      tmp(i,:) = [tv1(1),tv1(2),tv2(1),tv2(2)];
    end
  end

  % estimate translation to apply now
  
  tr = -(2*tmp(1,1) + 2*tmp(1,3) + tmp(2,1) + tmp(2,3)+ tmp(3,1) + tmp(3,3)) / 8;
  tc = -(tmp(1,2) + tmp(1,4) + min([tmp(2,2),tmp(2,4),tmp(3,2),tmp(3,4)]) ...
        + max([tmp(2,2),tmp(2,4),tmp(3,2),tmp(3,4)]) ) / 4;

  
  
