% 6 August 2008 16:51:34 PDT
% drsParseExperiment.m
% code to parse limit surface test using the min power method
% will add my own optimizations as well as
% generate a crapload of plots

% force plate and direction coordinates
% x - transverse direction
% y - shear direction
% z - preload direction


clear all
close all

% start timer for total calculation time
tic

% flag to write plots or not
outputPlots = 0; 
outputPlots = 1;

run ./configure.m

FxPulloffAll = [];
FyPulloffAll = [];
FzPulloffAll = [];

for i = 1 : length(distancePreloadMicron)
	for j = 1 : length(anglePulloffDegree)
  	for k = 1 : numTrials;
  	
			% get and load trajectory file
			string = sprintf(trajFormatString,...
											 distancePreloadMicron(i),...
											 anglePulloffDegree(j),k);
			trajData = load(fullfile(trajectoryDirectory,string));

			% get and load data file
			dataFilename = sprintf(testFormatString,...
									 distancePreloadMicron(i),...
									 anglePulloffDegree(j),k);
			data = load(fullfile(dataDirectory,dataFilename));
			[time Dx Dy Dz Ax Ay Az Fx Fy Fz Mx My Mz] = getTraces ( data );
			
			% output message to console
			fprintf('Analyzing File : %s\n',dataFilename);

			% get times from trajectory data file
			t0         = trajData(1,1);
			tStart     = trajData(2,1);
			tPreload   = trajData(3,1);
			tPulloff   = trajData(4,1);
			tClearance = trajData(5,1);
			tZero      = trajData(6,1);
			tEnd       = trajData(7,1);
% vertex 0 home/tare position 

% vertex 1
% this allows for initial zero force data
% tare length is currently 1 second
% this is used in the parseExperiment script

% vertex 2
% bottom of preload

% vertex 3
% top of pulloff

% vertex 4 end of vertical retraction

% vertex 5 back to home

% vertex 6 settling time

% vertex 7 zero flag for microcontroller


			% Get transition index
			index0         = find((time>=t0),1,'first');
			indexStart     = find((time>=tStart),1,'first');
			indexPreload   = find((time>=tPreload),1,'first');
			indexPulloff   = find((time>=tPulloff),1,'first');
			indexClearance = find((time>=tClearance),1,'first');
			indexZero      = find((time>=tZero),1,'first');
			indexEnd       = find((time>=tEnd),1,'first');
			indexEndTare   = find( ( time >= timeTareSecond ), 1, 'first' );			

			% desired positions
			DxMicron = Dx * 1000;
			DyMicron = Dy * 1000;
			DzMicron = Dz * 1000;

			% actual positions
			AxMicron = Ax * 1000;
			AyMicron = Ay * 1000;
			AzMicron = Az * 1000;
			
			% each of the above are column vectors
			% force is in units of Newtons
			forceXRaw = Fx;
			forceYRaw = Fy;
			forceZRaw = Fz;



			% Tare force data using initial idle time
			forceBiasX = mean ( forceXRaw( 1:indexEndTare ));
			forceBiasY = mean ( forceYRaw( 1:indexEndTare ));
			forceBiasZ = mean ( forceZRaw( 1:indexEndTare ));
			forceXTared = forceXRaw - forceBiasX;
			forceYTared = forceYRaw - forceBiasY;
			forceZTared = forceZRaw - forceBiasZ;	

			% Tare position data using initial idle time
			positionBiasX = mean ( AxMicron( 1:indexEndTare ));
			positionBiasY = mean ( AyMicron( 1:indexEndTare ));
			positionBiasZ = mean ( AzMicron( 1:indexEndTare ));
			AxMicron = AxMicron - positionBiasX;
			AyMicron = AyMicron - positionBiasY;
			AzMicron = AzMicron - positionBiasZ;	

			useRawData = 1;
			% Filter data
			if (useRawData)
			 	forceXFiltered = forceXTared;
			 	forceYFiltered = forceYTared;
			 	forceZFiltered = forceZTared;
			else 
				[numd,dend] = butter( 3, 0.05 );
				forceXFiltered = filtfilt(numd,dend,forceXTared);
				forceYFiltered = filtfilt(numd,dend,forceYTared);
				forceZFiltered = filtfilt(numd,dend,forceZTared);
			end	
			

			% use standard deviation of initial period to determine noise floor
			margin = 200;
			noiseFloorX = std ( forceXRaw( 1+margin:indexEndTare-margin ));
			noiseFloorY = std ( forceYRaw( 1+margin:indexEndTare-margin ));
			noiseFloorZ = std ( forceZRaw( 1+margin:indexEndTare-margin ));

			noiseFloorX = std ( forceXFiltered( 1+margin:indexEndTare-margin ));
			noiseFloorY = std ( forceYFiltered( 1+margin:indexEndTare-margin ));
			noiseFloorZ = std ( forceZFiltered( 1+margin:indexEndTare-margin ));


			% look for threshold crossing in the z force data
			% use filtered data to find this crossing
			threshold = - 4.0 * noiseFloorZ;
			indexStartSearch = indexStart - margin;
			indexEndSearch = length ( forceZFiltered );
			indexContact = find (forceZFiltered(indexStartSearch:indexPreload)...
			                       < threshold, 1, 'first');
			indexContact = indexContact + indexStartSearch - 1;
			%fprintf('indexContact = %d  ', indexContact);
			%fprintf('threshold = %3.3f  ', threshold);
			depthAtContact = - AzMicron(indexContact);

			
			
			% Get pre-load force and position
			[FzPreload(i,j,k),tempIndex] = ...
			                             min(forceZFiltered(indexStart:indexPulloff));
			indexMaxPreload = tempIndex + indexStart - 1;
			FxPreload(i,j,k) = forceXFiltered(indexMaxPreload);
			FyPreload(i,j,k) = forceYFiltered(indexMaxPreload);
			depthAtMaxPreload = - AzMicron(indexMaxPreload);

			% calculate actual preload depth and put in 
			depthPreloadMeasured(i,j,k) = depthAtMaxPreload - depthAtContact;
			fprintf('Preload : %3.3f\n',depthPreloadMeasured(i,j,k));

			% Get pull-off or drag
			[FzPulloff(i,j,k),tempIndex] = max(forceZFiltered(indexPreload:indexPulloff));
			samplePulloff = tempIndex + indexPreload - 1;
			FxPulloff(i,j,k) = forceXFiltered(samplePulloff);
			FyPulloff(i,j,k) = forceYFiltered(samplePulloff);

			% Get all forces during pull-off
			FxTemp = forceXFiltered(indexPreload:1:indexPulloff);
			FyTemp = forceYFiltered(indexPreload:1:indexPulloff);
			FzTemp = forceZFiltered(indexPreload:1:indexPulloff);
			FxPulloffAll(i,j,k,1:length(FxTemp)) = FxTemp;
			FyPulloffAll(i,j,k,1:length(FxTemp)) = FyTemp;
			FzPulloffAll(i,j,k,1:length(FxTemp)) = FzTemp;
			
			% Get pull-off via maximum vector between switch and plane
			v = [0;0;1];
			R1 = [1 0 0;... 
						0 cos(anglePulloffRadian(j)) -sin(anglePulloffRadian(j));... 
						0 sin(anglePulloffRadian(j))  cos(anglePulloffRadian(j))];
			v = R1 * v;
			% velocity vector of glass plate
			v = [0; -sin(anglePulloffRadian(j)); cos(anglePulloffRadian(j))];
			% force vector is which force plate or reaction or what?
			Fvector = [forceXFiltered(indexPreload:indexPulloff)...
			           forceYFiltered(indexPreload:indexPulloff)...
			           forceZFiltered(indexPreload:indexPulloff)];
			clear FvectorNorm;
			for index=1:size(Fvector,1)
					FvectorNorm(index) = dot(v,Fvector(index,:));
			end
			[garbage,tempIndex] = max(FvectorNorm);
			indexPulloffVector = tempIndex + indexPreload - 1;
			FxPulloffVector(i,j,k) = forceXFiltered(indexPulloffVector);
			FyPulloffVector(i,j,k) = forceYFiltered(indexPulloffVector);
			FzPulloffVector(i,j,k) = forceZFiltered(indexPulloffVector);

			% Get maximum and average positioning error
			Ex = Dx - Ax;
			Ey = Dy - Ay;
			Ez = Dz - Az;
			Error = sqrt(Ex.^2 + Ey.^2 + Ez.^2);
			ErrorMax(i,j,k) = max( Error );
			ErrorMean(i,j,k) = mean( Error );

% plot lines for indices

			if (outputPlots) 
				% plotting code
				figureHandle = figure;
				axesHandle = gca;
				plot(forceXFiltered,'k');
				hold on;
				plot(forceYFiltered,'r');
				plot(forceZFiltered,'b');
				plot(AzMicron(1:1100) / 1000,'g');
				legendStrings = {'Fx','Fy','Fz'};
				legend(legendStrings,'FontSize',12,'Location','EastOutside');
				title(axesHandle, dataFilename, 'Interpreter','None');
				xlabel (axesHandle, 'Time (ms)');
				ylabel (axesHandle, 'Force (N)');
				
				% in force space
				% plot point of contact
				plot(indexContact,forceZFiltered(indexContact),'bo');
				% plot max preload point
				plot(indexMaxPreload,FzPreload(i,j,k),'bo');
				% plot pull off vector point
				plot(indexPulloffVector,FzPulloffVector(i,j,k),'bo');

				% in position space
				% plot point of contact
				plot(indexContact,AzMicron(indexContact)/1000,'gx');
				% plot max preload point
				plot(indexMaxPreload,AzMicron(indexMaxPreload)/1000,'gx');
				
       
				yLimits = get(axesHandle,'YLim');
				line([indexStart indexStart],yLimits, ...
																		'Color','k',...
																		'LineStyle','-')
			

				
				hold off;
				formatPlot( figureHandle, axesHandle, 'Times New Roman', 24 );
				plotFilename = sprintf(plotFormatString, ...
											 distancePreloadMicron(i), anglePulloffDegree(j), k);
				printPlot( figureHandle, plotFilename, 8, 6);
				close(figureHandle);
			end
			
  	end
	end
end

save experiment

% end timer and output total calculation time
toc
