function [mag_left phase_left mag_right phase_right] = process( data, frequency )
% recover magnitude and phase from data
tim = data(:,1);
figure(1)
plot(diff(tim)) % plot time intervals to check servo timing is ok

% get position in radians
pos_left = 2*pi*(data(:,2) - mean(data(:,2)))/(1560);
pos_right = 2*pi*(data(:,3) - mean(data(:,3)))/(1560);
% sample times for plotting
tt = 0.002*(0:(length(pos_left) - 1));
% get torque in raw units.
trq = data(:,4) - mean(data(:,4));
figure(2)
hold off
plot(tt,0.014*trq) % plot scaled torque and position on same scale
hold on
plot(tt,pos_left)
plot(tt,pos_right)

% Take ffts of the data
[ ft ftt fttt ] = fft2cga( trq );            
[ fpl fppl fpppl ] = fft2cga( pos_left );
[ fpr fppr fpppr ] = fft2cga( pos_right );

% find index of closest frequency
min = 1e8;
min_i = -1;
for i = 1:length(fpl)
  d2 = (fpl(i) - frequency)*(fpl(i) - frequency);
  if min >= d2
    min = d2;
    min_i = i;
  end
end
[ min_i fpl(min_i) frequency ]

max_i_search = 2*min_i
figure(3)
hold off
plot(ft(1:max_i_search),0.014*ftt(1:max_i_search));
hold on
plot(fpl(1:max_i_search),fppl(1:max_i_search));
plot(fpr(1:max_i_search),fppr(1:max_i_search));

figure(4)
hold off
plot(0.014*ftt(1:max_i_search));
hold on
plot(fppl(1:max_i_search));
plot(fppr(1:max_i_search));

mag_left = abs(fpppl(min_i))/abs(fttt(min_i));
mag_right = abs(fpppr(min_i))/abs(fttt(min_i));
phase_left = angle(fttt(min_i)) - angle(fpppl(min_i));
phase_right = angle(fttt(min_i)) - angle(fpppr(min_i));
if phase_left < 0
  phase_left = phase_left + 2*pi;
end
if phase_right < 0
  phase_right = phase_right + 2*pi;
end
[mag_left mag_right phase_left phase_right]
drawnow
