function varargout = MotionAnalyst3D(varargin)
% MOTIONANALYST3D M-file for MotionAnalyst3D.fig
% Author:	Robb Colbrunn, Cleveland Clinic BioRobotics and Mechanical Testing Core
% Date:		2-10-10
% Last change:  3-11-10
% Version:	1.2
% ______________Change Log_________________________________________________
% 2-11-10:  Added display of residuals
% 3-11-10:  Fixed display of figure so that it does not reorient and
% rescale as the frames slider is moved.
%__________________________________________________________________________

%License
%     Copyright (C) 2010  Colbrunn
% 
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     any later version.
% 
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
% 
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.

%      MOTIONANALYST3D, by itself, creates a new MOTIONANALYST3D or raises the existing
%      singleton*.
%
%      H = MOTIONANALYST3D returns the handle to a new MOTIONANALYST3D or the handle to
%      the existing singleton*.
%
%      MOTIONANALYST3D('CALLBACK',hObject,eventData,handles,...) calls the local
%      function named CALLBACK in MOTIONANALYST3D.M with the given input arguments.
%
%      MOTIONANALYST3D('Property','Value',...) creates a new MOTIONANALYST3D or raises the
%      existing singleton*.  Starting from the left, property value pairs are
%      applied to the GUI before MotionAnalyst3D_OpeningFcn gets called.  An
%      unrecognized property name or invalid value makes property application
%      stop.  All inputs are passed to MotionAnalyst3D_OpeningFcn via varargin.
%
%      *See GUI Options on GUIDE's Tools menu.  Choose "GUI allows only one
%      instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES

% Edit the above text to modify the response to help MotionAnalyst3D

% Last Modified by GUIDE v2.5 04-Mar-2010 23:14:19

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @MotionAnalyst3D_OpeningFcn, ...
                   'gui_OutputFcn',  @MotionAnalyst3D_OutputFcn, ...
                   'gui_LayoutFcn',  [] , ...
                   'gui_Callback',   []);
if nargin && ischar(varargin{1})
    gui_State.gui_Callback = str2func(varargin{1});
end

if nargout
    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
    gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT


% --- Executes just before MotionAnalyst3D is made visible.
function MotionAnalyst3D_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% varargin   command line arguments to MotionAnalyst3D (see VARARGIN)
% global ext_fig;
% Choose default command line output for MotionAnalyst3D
handles.output = hObject;

% Update handles structure
guidata(hObject, handles);
axes(handles.analysticon);
imshow('analyst.png');
set(handles.axes2, 'Visible', 'Off');
%create and hide external figure
% ext_fig = figure;
% set(ext_fig, 'Visible', 'Off');
% UIWAIT makes MotionAnalyst3D wait for user response (see UIRESUME)
% uiwait(handles.figure1);


% --- Outputs from this function are returned to the command line.
function varargout = MotionAnalyst3D_OutputFcn(hObject, eventdata, handles) 
% varargout  cell array for returning output args (see VARARGOUT);
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure
varargout{1} = handles.output;



function path1_Callback(hObject, eventdata, handles)
% hObject    handle to path1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of path1 as text
%        str2double(get(hObject,'String')) returns contents of path1 as a double


% --- Executes during object creation, after setting all properties.
function path1_CreateFcn(hObject, eventdata, handles)
% hObject    handle to path1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end


% --- Executes on button press in BrowseCam1.
function BrowseCam1_Callback(hObject, eventdata, handles)
% hObject    handle to BrowseCam1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

[fname,pname] = uigetfile('*.*','Enter data file');
if (fname ~= 0)
pathname = sprintf('%s%s',pname,fname);
filename = pathname; 
datafolder = [pname '/'];
else
    return
end
read2Ddata(filename, handles, 1);

set(handles.path1,'TooltipString', pathname);
%[pathstr, name, ext, versn] = fileparts(pathname);
set(handles.path1,'String', fname);



function path2_Callback(hObject, eventdata, handles)
% hObject    handle to path2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of path2 as text
%        str2double(get(hObject,'String')) returns contents of path2 as a double


% --- Executes during object creation, after setting all properties.
function path2_CreateFcn(hObject, eventdata, handles)
% hObject    handle to path2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end


% --- Executes on button press in BrowseCam2.
function BrowseCam2_Callback(hObject, eventdata, handles)
% hObject    handle to BrowseCam2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
[fname,pname] = uigetfile('*.*','Enter data file');
if (fname ~= 0)
pathname = sprintf('%s%s',pname,fname);
filename = pathname; 
datafolder = [pname '/'];
end
read2Ddata(filename, handles, 2);

set(handles.path2,'TooltipString', pathname);
%[pathstr, name, ext, versn] = fileparts(pathname);
set(handles.path2,'String', fname);


function calpath_Callback(hObject, eventdata, handles)
% hObject    handle to calpath (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of calpath as text
%        str2double(get(hObject,'String')) returns contents of calpath as a double


% --- Executes during object creation, after setting all properties.
function calpath_CreateFcn(hObject, eventdata, handles)
% hObject    handle to calpath (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end


% --- Executes on button press in BrowseCal.
function BrowseCal_Callback(hObject, eventdata, handles)
% hObject    handle to BrowseCal (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
[fname,pname] = uigetfile('*.*','Enter data file');
if (fname ~= 0)
pathname = sprintf('%s%s',pname,fname);
filename = pathname; 
datafolder = [pname '/'];
else
    return
end
readCaldata(filename, handles);

set(handles.calpath,'TooltipString', pathname);
%[pathstr, name, ext, versn] = fileparts(pathname);
set(handles.calpath,'String', fname);

% --- Executes on button press in Compute.
function Compute_Callback(hObject, eventdata, handles)
% hObject    handle to Compute (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
CoefMatrix = get(handles.calpath, 'UserData');
Cam1points = get(handles.path1, 'UserData');
Cam2points = get(handles.path2, 'UserData');

%reshape array to have 3rd dimension coorespond to marker#
nums1 = size(Cam1points);
nums2 = size(Cam2points);
numframes1 = nums1(1);
numframes2 = nums2(1);
Cam1 = reshape(Cam1points, numframes1, 2,  nums1(2)/2);
Cam2 = reshape(Cam2points, numframes2, 2,  nums2(2)/2);

if numframes1 ~= numframes2
    fprintf(1,'Number of markers in each file do not match');
    return
end
if (numframes1 <=1) || (numframes2 <=1)
    numframes1 = 1;
    numframes2 = 1;
end
set(handles.slider1, 'Max',numframes1);
set(handles.slider1, 'Min',1);
%need to reconfu for each marker.
%CamCombined is [xcam1,ycam1,xcam2,ycam2...; at time 1]
%               [xcam1,ycam1,xcam2,ycam2...; at time 2]
%               [xcam1,ycam1,xcam2,ycam2...; at time 3]
%H = reconfu(CoefMatrix,CamCombined)
for i=1:(nums1(2)/2) %for each marker
    CamCombined = [Cam1(:,:,i) Cam2(:,:,i)];
    H(:,:,i) = reconfu(CoefMatrix,CamCombined);
end

%get x,y,z data.  (time, NA, marker#)
x = H(:,1,:);
y = H(:,2,:);
z = H(:,3,:);

%Configure residue for plotting
residue = H(:,4,:); %get residuals for each frame
res_siz = size(residue);
residuals = reshape(residue, res_siz(1), res_siz(3));%reshape to 2d array
set(handles.Residuals,'UserData',residuals);%store data

%reshape to 2D. rows are change in time.  columns are change in marker#
sizes = size(x);
dispx = (reshape(x,sizes(1),sizes(3)))';
dispy = (reshape(y,sizes(1),sizes(3)))';
dispz = (reshape(z,sizes(1),sizes(3)))';

Data3D = struct('x', dispx, 'y', dispy, 'z', dispz);
setappdata(handles.axes2, 'Data', Data3D);

displayd(handles);

%build path to 3D output file. Assume it is in same folder as coefficients file
path = get(handles.calpath,'TooltipString');
[pathstr, name, ext, versn] = fileparts(path);
outpath = [pathstr '\*_Results3D.mat'];
    
[fname,pname] = uiputfile(outpath,'Enter Output Data File');
if (fname ~= 0)
pathname = sprintf('%s%s',pname,fname);
filename = pathname; 
datafolder = [pname '/'];
writedata(filename, handles);
end

% --- Executes on slider movement.
function slider1_Callback(hObject, eventdata, handles)
% hObject    handle to slider1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

%%this snaps the slider to an integer frame number.  This is annoying when
%%there are only a few frames so I commented it out.
% sliderval = round(get(hObject, 'Value'));
% set(hObject, 'Value', sliderval);
displayd(handles);
% set(handles.path,'String', file);

% --- Executes during object creation, after setting all properties.
function slider1_CreateFcn(hObject, eventdata, handles)
% hObject    handle to slider1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end

function read2Ddata(filename,handles,camnum)
    %load data from 2D results.mat files resulting from 2D tracking
    load(filename);
    %set variables
    if camnum == 1 
        PathHandle = handles.path1;
        BrowseHandle = handles.BrowseCam1;
    else
        PathHandle = handles.path2;
        BrowseHandle = handles.BrowseCam2;
    end
    set(BrowseHandle, 'UserData', Res2DFileHeader);
    set(PathHandle, 'UserData', xypoints);
    
function readCaldata(filename,handles)
    %load data from 3D coefficients.mat file resulting from 2D-3D calibration
    load(filename);
    %set variables
    set(handles.BrowseCal, 'UserData', CoefFileHeader);
    set(handles.calpath, 'UserData', CoefMatrix);
    set(handles.CalLbl, 'UserData', units);
    unitstip = strcat('Units = ', char(units));
    set(handles.CalLbl, 'TooltipString', unitstip);
    
function displayd(handles)
%global ext_fig;
%display 3D data
set(handles.axes2, 'Visible', 'On');
frame = round(get(handles.slider1, 'Value'));
Data = getappdata(handles.axes2, 'Data');
xpoints = Data.x(:,frame);
ypoints = Data.y(:,frame);
zpoints = Data.z(:,frame);

nums = size(Data.x);
numframes = nums(2);
frmstring = [num2str(frame) ' of ' num2str(numframes)];
set(handles.FrameNums, 'String', frmstring);

MarkerSize = round(get(handles.MarkSize, 'Value'));

S = [];
C = [];
for i = 1:numel(xpoints)
    S(i) = MarkerSize;
    C(i) = i;
end

% plot in current figure
pos = campos; %get current camera position to reset after redraw
axes(handles.axes2);
scatter3(xpoints, ypoints, zpoints, S, C, 'filled');
rotate3d on
% set labels
xlabel('x axis')
ylabel('y axis')
zlabel('z axis')
%set limits to max and min values of all frames so that it never has to rescale
xlim([min(min(Data.x)) max(max(Data.x))]);
ylim([min(min(Data.y)) max(max(Data.y))]);
zlim([min(min(Data.z)) max(max(Data.z))]);
campos(pos); % reset camera position

% create external figure
ext_disp = get(handles.AdvPlot,'Value');
if ext_disp == 1 
% set(ext_fig, 'Visible', 'On');
% set(0,'CurrentFigure',ext_fig)
%axes(ext_fig);
temp_string = ['Frame ' frmstring];
figure('Name', temp_string);% always make new figure each time so no changes are made once user modifes it
scatter3(xpoints, ypoints, zpoints, S, C, 'filled');
rotate3d on
xlabel('x axis')
ylabel('y axis')
zlabel('z axis')
end
   

function FrameNums_Callback(hObject, eventdata, handles)
% hObject    handle to FrameNums (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of FrameNums as text
%        str2double(get(hObject,'String')) returns contents of FrameNums as a double


% --- Executes during object creation, after setting all properties.
function FrameNums_CreateFcn(hObject, eventdata, handles)
% hObject    handle to FrameNums (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.92549 .913725 .84705]);
end


% --- Executes on button press in Help.
function Help_Callback(hObject, eventdata, handles)
% hObject    handle to Help (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
open('MotionAnalyst3D_help.pdf');

% --- Executes on slider movement.
function MarkSize_Callback(hObject, eventdata, handles)
% hObject    handle to MarkSize (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider

displayd(handles);

% --- Executes during object creation, after setting all properties.
function MarkSize_CreateFcn(hObject, eventdata, handles)
% hObject    handle to MarkSize (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end


% --- Executes on button press in Save.
function Save_Callback(hObject, eventdata, handles)
% hObject    handle to Save (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

%build path to 3D output file. Assume it is in same folder as coefficients file
path = get(handles.calpath,'TooltipString');
[pathstr, name, ext, versn] = fileparts(path);
outpath = [pathstr '\*_Results3D.mat'];

[fname,pname] = uiputfile(outpath,'Enter output data file');
if (fname ~= 0)
pathname = sprintf('%s%s',pname,fname);
filename = pathname; 
datafolder = [pname '/'];
writedata(filename, handles);
end


% --- Executes on button press in Load.
function Load_Callback(hObject, eventdata, handles)
% hObject    handle to Load (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
[fname,pname] = uigetfile('*.mat','Select data file');
if (fname ~= 0)
pathname = sprintf('%s%s',pname,fname);
filename = pathname ;
datafolder = [pname '/'];
end

load(filename);%load data and header info
setappdata(handles.axes2, 'Data' ,Data3D);%put 3D data in memory
nums = size(Data3D.x(:,:));%number of markers (1) and # of frames(2)
set(handles.slider1, 'Max',nums(2)); %set slider dimensions

% get and load data from camera 1
[pathstr, name, ext, versn] = fileparts(ResFile3DHeader{2,2});
set(handles.path1, 'TooltipString',ResFile3DHeader{2,2});
set(handles.path1, 'String',name);
read2Ddata(ResFile3DHeader{2,2},handles,1);

% get and load data from camera 2
[pathstr, name, ext, versn] = fileparts(ResFile3DHeader{3,2});
set(handles.path2, 'TooltipString',ResFile3DHeader{3,2});
set(handles.path2, 'String',name);
read2Ddata(ResFile3DHeader{3,2},handles,2);

% get and load data from calibration
[pathstr, name, ext, versn] = fileparts(ResFile3DHeader{4,2});
set(handles.calpath, 'TooltipString',ResFile3DHeader{4,2});
set(handles.calpath, 'String',name);
readCaldata(ResFile3DHeader{4,2},handles);

displayd(handles); %display loaded data

%===========================================================
function writedata(filename, handles)
	% write the data to the 3D output file
    
    %get all data required for output file
    date = datestr(now);
    path1 = get(handles.path1, 'TooltipString');
    path2 = get(handles.path2, 'TooltipString');
    calpath = get(handles.calpath, 'TooltipString');
    units = get(handles.CalLbl, 'UserData');
    
    ResFile3DHeader = {'Date/Time', datestr(now);
                    'Camera 1 Data', path1;
                    'Camera 2 Data', path2;
                    'Calibration Data', calpath;
                    'Data format', 'struct of 2D arrays of dimensions(j) x, y, and z';
                    'Data format.j row 1', 'mrk1j-frm1 mrk1j-frm2 mrk1j-frm3...';
                    'Data format.j row 2', 'mrk2j-frm1 mrk2j-frm2 mrk2j-frm3...';
                    'Data units', units};
       
    Data3D = getappdata(handles.axes2, 'Data');%get output matrix
    Residuals = get(handles.Residuals, 'UserData');%get residuals
    save(filename, 'ResFile3DHeader', 'Data3D', 'Residuals');%write to .mat file
    
    %manipulate data to make .csv file
    %change header
    ResFile3DHeader_ASCII = {'Date/Time', datestr(now);
                    'Camera 1 Data', path1;
                    'Camera 2 Data', path2;
                    'Calibration Data', calpath;
                    'Data format', 'mrk1x-frm1 mrk1y-frm1 mrk1z-frm1 mrk2x-frm1 mrk2y-frm1 mrk2z-frm1...';
                    'Data format - line2 etc...', 'mrk1x-frm2 mrk1y-frm2 mrk1z-frm2 mrk2x-frm2 mrk2y-frm2 mrk2z-frm2...';
                    'Data units', units'};
    headsize = size(ResFile3DHeader_ASCII);
   %
    %change data
    nums = size(Data3D.x(:,:));%number of markers (1) and # of frames(2)
    for i = 1:nums(2)
    xyz(:,:,i) = [Data3D.x(:,i) Data3D.y(:,i) Data3D.z(:,i)];
    end
    for i = 1:nums(2)
        xyzout(i,:) = reshape(xyz(:,:,i)', numel(xyz(:,:,i)),1);
        dataout(i,:) = [xyzout(i,:) Residuals(i,:)];
    end
    
    %change file extension
    [pathstr, name, ext, versn] = fileparts(filename);
    ASCIIfile = [pathstr '\' name '.csv'];
    fid = fopen(ASCIIfile,'w');
    % write header to output file
    for i = 1:headsize(1)
        for j = 1:headsize(2) 
            temp_string = char(ResFile3DHeader_ASCII(i,j));
            fprintf(fid,'%s',temp_string);
            if (j < headsize(2))
                fprintf(fid,',');
            end;
        end;
        fprintf(fid,'\n');
    end;
    fprintf(fid,'\n');
    % write column headings to output file 
    %xyz headers
    for i=1:nums(1) % number of markers
        temp_string = strcat('x',num2str(i),',');
        fprintf(fid,temp_string);
        temp_string = strcat('y',num2str(i),',');
        fprintf(fid,temp_string);
        temp_string = strcat('z',num2str(i));
        fprintf(fid,temp_string);
        if (i < nums(1))
            fprintf(fid,',');
        end;
    end;
    fprintf(fid,',');
    %residuals headers
    for i=1:nums(1) % number of markers
        temp_string = strcat('Residuals-mrk',num2str(i));
        fprintf(fid,temp_string);
        if (i < nums(1))
            fprintf(fid,',');
        end;
    end;
    fprintf(fid,'\n');
    
    % write data to output file    
    for i = 1:nums(2) %number of frames
        numcols = nums(1)*4; % number of markers * 4 components (x,y,z,residual)
        for j = 1:numcols
            fprintf(fid,'%f',dataout(i,j));
            if (j < numcols)
                fprintf(fid,',');
            end;
        end;
        fprintf(fid,'\n');
    end;    
    fclose(fid);

% --- Executes on button press in AdvPlot.
function AdvPlot_Callback(hObject, eventdata, handles)
% hObject    handle to AdvPlot (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
displayd(handles);
%displays the 3D data in a new plot window


% --- Executes on button press in Residuals.
function Residuals_Callback(hObject, eventdata, handles)
% hObject    handle to Residuals (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
residuals = get(hObject, 'UserData');
figure('Name', 'Residuals');% always make new figure each time so no changes are made once user modifes it
plot(residuals);
xlabel('Frames')
ylabel('Residue')
nums = size(residuals);
for i = 1:nums(2) % 1 to number of markers
    tempname = strcat('Marker-',num2str(i));
    labels(i) = {tempname};
end
legend(labels);

