function [q, mot] = anc_to_mot(fnamein,fnameout,transform)

cop_at_forceplate_origin = 0;
zero_ground_moments = 1;
threshold_factor = 0.01;

filter_order = 50; % 2 for butter
filter_cutoff = 20;
filter_type = 'none'; % or 'butter' or 'fir'

% TODO: figure this out from anc file
analog_rate = 600;

% Digital range is +/- 2048
% Voltage range is +/- 10V
analogScale = 10 / 2048;

% TODO: Read this from forcepla.cal instead of hardcoding it
FPinfo(1).calibrationDiagonals = [800 800 1500 375 1200 600];
FPinfo(1).FPcenterToFPorigin_FP = [-0.24 0.8175 0]';
FPinfo(1).LabToFPcenter_lab = [0.496 -0.288 -0.0075]';
FPinfo(1).orientationMatrix = [0 -1 0; -1 0 0; 0 0 -1];
FPinfo(1).gain = 2;

FPinfo(2).calibrationDiagonals = [800 800 1500 375 1200 600];
FPinfo(2).FPcenterToFPorigin_FP = [0.24 0.8175 0]';
FPinfo(2).LabToFPcenter_lab = [0.496 0.2265 -0.0075]';
FPinfo(2).orientationMatrix = [0 -1 0; -1 0 0; 0 0 -1];
FPinfo(2).gain = 2;

% -------------------------------------------------------------------

if nargin < 3
	transform = eye(3);
end

fin = fopen(fnamein, 'r');	
if fin == -1								
	error(['unable to open ', fnamein])		
end

nextline = fgetl(fin);
if ~strcmp(nextline, sprintf('File_Type:\tAnalog R/C ASCII\tGeneration#:\t1'))
	disp('Failed to match line 1');
	return;
end

nextline = fgetl(fin);
if ~strcmp(nextline, sprintf('Board_Type:\tNational PCI-MIO-64E-1\tPolarity:\tBipolar'))
	disp('Failed to match line 2');
	return;
end

nextline = fgetl(fin);
[name, trial, duration, channels] = strread(nextline, 'Trial_Name: %s Trial#: %d Duration(Sec.): %f #Channels: %d');

q.labels = cell(1, channels);

while true
	nextline = fgetl(fin);

	[tok, rest] = strtok(nextline);
	if strcmp(tok, 'Name')
		for i=1:channels
			[q.labels{i}, rest] = strtok(rest);
		end
	elseif strcmp(tok, 'Rate')
		q.rates = sscanf(rest, '%f');
	elseif strcmp(tok, 'Range')
		q.rates = sscanf(rest, '%f');		
		break;
	end
end

numcolumns = channels+1; % including time channel

% READ
data = fscanf(fin, '%f');

if mod(length(data),numcolumns) ~= 0
	disp('Data count not multiple of #channels');
	return;
end

numrows=length(data)/numcolumns;

data = reshape(data, numcolumns, numrows)';
q.time = data(:,1);
q.data = data(:,2:numcolumns);

% Filter
% Seems to create overshooting which may be undesirable
if strcmp(filter_type,'butter')
	disp(sprintf('butterworth filter order=%f cutoff=%f', filter_order, filter_cutoff));
	[filterb, filtera] = butter(filter_order, filter_cutoff/(0.5*analog_rate));
	q.filtered_data = filtfilt(double(filterb), double(filtera), double(q.data));
elseif strcmp(filter_type,'fir')
	disp(sprintf('fir filter order=%f cutoff=%f', filter_order, filter_cutoff));
	B = fir1(filter_order, filter_cutoff/(0.5*analog_rate));
	q.filtered_data = filtfilt(B, 1, q.data);
else
	q.filtered_data = q.data;
end

for fp=1:2
	Fz = q.filtered_data(:,6*(fp-1)+3);
	threshold = threshold_factor*max(Fz);
	contactIndices = find(Fz >= threshold);
%	contactIndices=1:100;

	numindices=length(contactIndices);

	% Convert 'reaction' forces and moments to 'action' forces and moments
	F = -1 * q.filtered_data(contactIndices,6*(fp-1)+(1:3));
	M = -1 * q.filtered_data(contactIndices,6*(fp-1)+(4:6));

	% Convert units (voltages to Newtons, Newtons-m)
	F = (analogScale / FPinfo(fp).gain) * (F * diag(FPinfo(fp).calibrationDiagonals(1:3)));
	M = (analogScale / FPinfo(fp).gain) * (M * diag(FPinfo(fp).calibrationDiagonals(4:6)));

	% Compute COP and Mz
	if cop_at_forceplate_origin
		COP = zeros(numindices, 3);
		T = M;
	else
		COP = [-1*M(:,2)./F(:,3) M(:,1)./F(:,3) zeros(numindices,1)];
		T = [zeros(numindices,1) zeros(numindices,1) M(:,3)+COP(:,2).*F(:,1)-COP(:,1).*F(:,2)];
	end

	% Convert to Lab CS
	% Rotate
	COP = COP * FPinfo(fp).orientationMatrix';
	F = F * FPinfo(fp).orientationMatrix';
	T = T * FPinfo(fp).orientationMatrix';

	% Translate
	FPcenterToFPorigin_lab =  FPinfo(fp).orientationMatrix * FPinfo(fp).FPcenterToFPorigin_FP;
	% not sure why we subtract the second vector instead of add, but seems to give answer consistent with EVaRT
	LabToFPorigin_lab = FPinfo(fp).LabToFPcenter_lab - FPcenterToFPorigin_lab;
	COP = COP + ones(numindices,1)*LabToFPorigin_lab';

	% Apply transform passed in (taking it from the Lab CS to the CS we're using in our simulation)
	COP = COP * transform';
	F = F * transform';
	T = T * transform';

	q.newdata(fp).F = zeros(numrows, 3);
	q.newdata(fp).F(contactIndices,:) = F;

	q.newdata(fp).T = zeros(numrows, 3);
	q.newdata(fp).T(contactIndices,:) = T;

	if cop_at_forceplate_origin
		q.newdata(fp).COP = (ones(numrows,1)*LabToFPorigin_lab') * transform';
	else
		q.newdata(fp).COP = zeros(numrows, 3);
		q.newdata(fp).COP(contactIndices,:) = COP;
	end

	q.newdata(fp).indices = contactIndices;

	if zero_ground_moments
		% TODO: figure out twist moments; for now we zero it
		q.newdata(fp).T = zeros(numrows, 3);
	end

	% debugging -- these are both close to the Mz produced by EVaRT
	M = M * FPinfo(fp).orientationMatrix';
	M = M * transform';
	q.newdata(fp).finalM = zeros(numrows,3);
	q.newdata(fp).finalM(contactIndices,:) = M;

	q.newdata(fp).finalM2 = zeros(numrows,3);
	q.newdata(fp).finalM2(contactIndices,:) = M - cross(COP, F);
end

fout = fopen(fnameout, 'wt');
if fout == -1								
	error(['unable to open ', fnameout])		
end

% WRITE MOTION FILE

mot.labels = { 'time', ...
					'ground_force_vx', 'ground_force_vy', 'ground_force_vz', ...
					'ground_force_px', 'ground_force_py', 'ground_force_pz', ...
					'ground_force_vx', 'ground_force_vy', 'ground_force_vz', ...
					'ground_force_px', 'ground_force_py', 'ground_force_pz', ...
					'ground_torque_x', 'ground_torque_y', 'ground_torque_z', ...
					'ground_torque_x', 'ground_torque_y', 'ground_torque_z' };
mot.data = [ q.time q.newdata(1).F q.newdata(1).COP q.newdata(2).F q.newdata(2).COP q.newdata(1).T q.newdata(2).T ];
write_motionFile(mot, fnameout);

mot_subsampled.labels = mot.labels;
mot_subsampled.data = mot.data(1:10:length(q.time),:);
mot_subsampled.data(:,1) = round(mot_subsampled.data(:,1)*1000)/1000;
write_motionFile(mot_subsampled, ['subsampled_' fnameout]);
