% 09/08/2020 Makoto. Used.
% 02/10/2020 Makoto. Used.
% 02/05/2020 Makoto. Used.
% 01/29/2020 Makoto. Used.
% 01/26/2018 Makoto. Updated.
% 12/29/2017 Makoto. Created. Copy p03 folder to p04 folder because this program is to overwrite the existing files.



% % Add path to tools.
% addpath('/data/mobi/Audiomaze/makoto/code/phaseSpaceCorrectionTools')
% 
% % Define rigid body LED marker indices.
% headIdx   = [1:7];
% torsoIdx  = [8:11 21 27];
% handIdx   = [16:20];
% footR_Idx = [23:26];
% footL_Idx = [29:32];
% 
% % List all .set files.
% %allSets = dir('/data/mobi/Audiomaze/makoto/p03_interpolateWithinSubject/*.set');
% allSets = dir('/data/mobi/Audiomaze/makoto/p04_interpolateAcrossSubjects/*.set');
% 
% 
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %%% Build the grand-mean template. %%%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % Prepare template stocks.
% headTemplates   = zeros(3, length(headIdx),   length(allSets));
% torsoTemplates  = zeros(3, length(torsoIdx),  length(allSets));
% handTemplates   = zeros(3, length(handIdx),   length(allSets));
% footR_Templates = zeros(3, length(footR_Idx), length(allSets));
% footL_Templates = zeros(3, length(footL_Idx), length(allSets));
% 
% for eegIdx = 1:length(allSets)
%     
%     disp(sprintf('Processing %d/%d blocks...', eegIdx, length(allSets)));
%     
%     loadName = allSets(eegIdx).name;
%     %EEG = pop_loadset('filename', loadName, 'filepath', '/data/mobi/Audiomaze/makoto/p03_interpolateWithinSubject', 'loadmode', 'info');
%     EEG = pop_loadset('filename', loadName, 'filepath', '/data/mobi/Audiomaze/makoto/p04_interpolateAcrossSubjects', 'loadmode', 'info');
% 
%     
%     % Obtain the rigid-body fixed phaseSpace.
%     currentData = double(EEG.etc.audiomaze.phaseSpaceCorrected);
%     
%     %%%%%%%%%%%%%%%%%%%%
%     %%% Obtain head. %%%
%     %%%%%%%%%%%%%%%%%%%%
%     currentRigidBody = double(EEG.etc.audiomaze.phaseSpaceCorrected(1:3, headIdx, :));
%     numNonNanChannel = max(sum(~isnan(squeeze(currentRigidBody(1,:,:))),1)); % Find max number of channels available at a time.
%     [initialNanFrameIdx, endingNanFrameIdx] = findInitialAndEndingNanFrames(currentRigidBody, numNonNanChannel);
%     goodFrameIdx                            = setdiff(1:size(currentRigidBody,3), [initialNanFrameIdx endingNanFrameIdx]);
%     headTemplates(:,:,eegIdx)               = currentRigidBody(:,:,goodFrameIdx(1));
%     
%     %%%%%%%%%%%%%%%%%%%%%
%     %%% Obtain Torso. %%%
%     %%%%%%%%%%%%%%%%%%%%%
%     currentRigidBody = double(EEG.etc.audiomaze.phaseSpaceCorrected(1:3, torsoIdx, :));
%     numNonNanChannel = max(sum(~isnan(squeeze(currentRigidBody(1,:,:))),1)); % Find max number of channels available at a time.
%     [initialNanFrameIdx, endingNanFrameIdx] = findInitialAndEndingNanFrames(currentRigidBody, numNonNanChannel);
%     goodFrameIdx                            = setdiff(1:size(currentRigidBody,3), [initialNanFrameIdx endingNanFrameIdx]);
%     torsoTemplates(:,:,eegIdx)              = currentRigidBody(:,:,goodFrameIdx(1));
%     
%     %%%%%%%%%%%%%%%%%%%%
%     %%% Obtain hand. %%%
%     %%%%%%%%%%%%%%%%%%%%
%     currentRigidBody = double(EEG.etc.audiomaze.phaseSpaceCorrected(1:3, handIdx, :));
%     numNonNanChannel = max(sum(~isnan(squeeze(currentRigidBody(1,:,:))),1)); % Find max number of channels available at a time.
%     [initialNanFrameIdx, endingNanFrameIdx] = findInitialAndEndingNanFrames(currentRigidBody, numNonNanChannel);
%     goodFrameIdx                            = setdiff(1:size(currentRigidBody,3), [initialNanFrameIdx endingNanFrameIdx]);
%     handTemplates(:,:,eegIdx)               = currentRigidBody(:,:,goodFrameIdx(1));
%     
%     %%%%%%%%%%%%%%%%%%%%%
%     %%% Obtain footR. %%%
%     %%%%%%%%%%%%%%%%%%%%%
%     currentRigidBody = double(EEG.etc.audiomaze.phaseSpaceCorrected(1:3, footR_Idx, :));
%     numNonNanChannel = max(sum(~isnan(squeeze(currentRigidBody(1,:,:))),1)); % Find max number of channels available at a time.
%     [initialNanFrameIdx, endingNanFrameIdx] = findInitialAndEndingNanFrames(currentRigidBody, numNonNanChannel);
%     goodFrameIdx                            = setdiff(1:size(currentRigidBody,3), [initialNanFrameIdx endingNanFrameIdx]);
%     footR_Templates(:,:,eegIdx)             = currentRigidBody(:,:,goodFrameIdx(1));
% 
%     %%%%%%%%%%%%%%%%%%%%%
%     %%% Obtain footL. %%%
%     %%%%%%%%%%%%%%%%%%%%%
%     currentRigidBody = double(EEG.etc.audiomaze.phaseSpaceCorrected(1:3, footL_Idx, :));
%     numNonNanChannel = max(sum(~isnan(squeeze(currentRigidBody(1,:,:))),1)); % Find max number of channels available at a time.
%     [initialNanFrameIdx, endingNanFrameIdx] = findInitialAndEndingNanFrames(currentRigidBody, numNonNanChannel);
%     goodFrameIdx                            = setdiff(1:size(currentRigidBody,3), [initialNanFrameIdx endingNanFrameIdx]);
%     footL_Templates(:,:,eegIdx)             = currentRigidBody(:,:,goodFrameIdx(1));
% end
% %save /data/mobi/Audiomaze/makoto/p04_interpolateAcrossSubjects/rigidBodyTemplateBeforeRotation headTemplates torsoTemplates handTemplates footR_Templates footL_Templates
% save /data/mobi/Audiomaze/makoto/p04_interpolateAcrossSubjects/rigidBodyTemplateBeforeRotation_check headTemplates torsoTemplates handTemplates footR_Templates footL_Templates
% 
% 
% 
% %%
% 
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %%% Rotate the templates to the first example. %%%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% addpath('/data/mobi/Audiomaze/makoto/code/phaseSpaceCorrectionTools')
% %load /data/mobi/Audiomaze/makoto/p04_interpolateAcrossSubjects/rigidBodyTemplateBeforeRotation
% load /data/mobi/Audiomaze/makoto/p04_interpolateAcrossSubjects/rigidBodyTemplateBeforeRotation_check
% 
% 
% % Rotate head templates.
% headTemplatesRotated = nan(size(headTemplates));
% for blockIdx = 1:size(headTemplates,3)
%     goodChannelIdx = find(~isnan(sum(headTemplates(:,:,blockIdx))));
%     headTemplatesRotated(:,goodChannelIdx,blockIdx) = rigidBodyTransformation(headTemplates(:,goodChannelIdx,1), headTemplates(:,goodChannelIdx,blockIdx));
% end
%     %{
%     figure
%     for n = 1:196
%         hold on
%         scatter3(headTemplatesRotated(3,:,n), headTemplatesRotated(1,:,n), headTemplatesRotated(2,:,n))
%     end
%     %}
% 
% % Rotate torso templates.
% torsoTemplatesRotated = nan(size(torsoTemplates));
% for blockIdx = 1:size(torsoTemplates,3)
%     goodChannelIdx = find(~isnan(sum(torsoTemplates(:,:,blockIdx))));
%     torsoTemplatesRotated(:,goodChannelIdx,blockIdx) = rigidBodyTransformation(torsoTemplates(:,goodChannelIdx,1), torsoTemplates(:,goodChannelIdx,blockIdx));
% end
%     %{
%     figure
%     for n = 1:196
%         hold on
%         scatter3(torsoTemplatesRotated(3,:,n), torsoTemplatesRotated(1,:,n), torsoTemplatesRotated(2,:,n))
%     end
%     %}
% 
% 
% % Rotate hand templates.
% handTemplatesRotated = nan(size(handTemplates));
% for blockIdx = 1:size(handTemplates,3)
%     goodChannelIdx = find(~isnan(sum(handTemplates(:,:,blockIdx))));
%     handTemplatesRotated(:,goodChannelIdx,blockIdx) = rigidBodyTransformation(handTemplates(:,goodChannelIdx,1), handTemplates(:,goodChannelIdx,blockIdx));
% end
%     %{
%     figure
%     for n = 1:196
%         hold on
%         scatter3(handTemplatesRotated(3,:,n), handTemplatesRotated(1,:,n), handTemplatesRotated(2,:,n))
%     end
%     %}
% 
% % Rotate footR templates.
% footR_TemplatesRotated = nan(size(footR_Templates));
% for blockIdx = 1:size(footR_Templates,3)
%     goodChannelIdx = find(~isnan(sum(footR_Templates(:,:,blockIdx))));
%     footR_TemplatesRotated(:,goodChannelIdx,blockIdx) = rigidBodyTransformation(footR_Templates(:,goodChannelIdx,1), footR_Templates(:,goodChannelIdx,blockIdx));
% end
%     %{
%     figure
%     for n = 1:196
%         hold on
%         scatter3(footR_TemplatesRotated(3,:,n), footR_TemplatesRotated(1,:,n), footR_TemplatesRotated(2,:,n))
%     end
%     %}
% 
% % Rotate footL templates.
% footL_TemplatesRotated = nan(size(footL_Templates));
% for blockIdx = 1:size(footL_Templates,3)
%     goodChannelIdx = find(~isnan(sum(footL_Templates(:,:,blockIdx))));
%     footL_TemplatesRotated(:,goodChannelIdx,blockIdx) = rigidBodyTransformation(footL_Templates(:,goodChannelIdx,1), footL_Templates(:,goodChannelIdx,blockIdx));
% end
%     %{
%     figure
%     for n = 1:197
%         hold on
%         scatter3(footL_TemplatesRotated(3,:,n), footL_TemplatesRotated(1,:,n), footL_TemplatesRotated(2,:,n))
%     end
%     %}
% 
% %save /data/mobi/Audiomaze/makoto/p04_interpolateAcrossSubjects/rigidBodyTemplatesRotated headTemplatesRotated torsoTemplatesRotated handTemplatesRotated footR_TemplatesRotated footL_TemplatesRotated
% save /data/mobi/Audiomaze/makoto/p04_interpolateAcrossSubjects/rigidBodyTemplatesRotated_check headTemplatesRotated torsoTemplatesRotated handTemplatesRotated footR_TemplatesRotated footL_TemplatesRotated
% 
% 
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %%% Check if any channel is swapped. %%%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% headGrandMean    = nanmedian(headTemplatesRotated,3);
% headDeviationMap = nan(size(headTemplatesRotated,2),size(headTemplatesRotated,3)); 
% for blockIdx = 1:size(headTemplatesRotated,3)
%     goodChannelIdx = find(~isnan(sum(headTemplatesRotated(:,:,blockIdx))));
%     headDeviationMap(goodChannelIdx,blockIdx) = sqrt(mean((headTemplatesRotated(:,goodChannelIdx,blockIdx)-headGrandMean(:,goodChannelIdx)).^2))';
% end
% figure; imagesc(headDeviationMap)
% 
% torsoGrandMean    = nanmedian(torsoTemplatesRotated,3);
% torsoDeviationMap = nan(size(torsoTemplatesRotated,2),size(torsoTemplatesRotated,3)); 
% for blockIdx = 1:size(torsoTemplatesRotated,3)
%     goodChannelIdx = find(~isnan(sum(torsoTemplatesRotated(:,:,blockIdx))));
%     torsoDeviationMap(goodChannelIdx,blockIdx) = sqrt(mean((torsoTemplatesRotated(:,goodChannelIdx,blockIdx)-torsoGrandMean(:,goodChannelIdx)).^2))';
% end
% figure; imagesc(torsoDeviationMap, [0 0.1])
% 
% % One island is found, > 100 cm!
% currentMarkers = torsoTemplatesRotated(:,:,152);
% figure
% plot3(currentMarkers(1,:), currentMarkers(2,:), currentMarkers(3,:), 'r.', 'markersize', 20)
% hold on
% plot3(torsoGrandMean(1,:), torsoGrandMean(2,:), torsoGrandMean(3,:), 'k.', 'markersize', 20)     
% 
% 
% 
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %%% Check how many blocks have missing marker in rigid body. %%%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% headNanCounts   = sum(isnan(reshape(headTemplatesRotated, [size(headTemplatesRotated,1)*size(headTemplatesRotated,2) size(headTemplatesRotated,3)])));
% torsoNanCounts  = sum(isnan(reshape(torsoTemplatesRotated, [size(torsoTemplatesRotated,1)*size(torsoTemplatesRotated,2) size(torsoTemplatesRotated,3)])));
% handNanCounts   = sum(isnan(reshape(handTemplatesRotated, [size(handTemplatesRotated,1)*size(handTemplatesRotated,2) size(handTemplatesRotated,3)])));
% footL_NanCounts = sum(isnan(reshape(footL_TemplatesRotated, [size(footL_TemplatesRotated,1)*size(footL_TemplatesRotated,2) size(footL_TemplatesRotated,3)])));
% footR_NanCounts = sum(isnan(reshape(footR_TemplatesRotated, [size(footR_TemplatesRotated,1)*size(footR_TemplatesRotated,2) size(footR_TemplatesRotated,3)])));
% 
% nanTable = cat(1, headNanCounts, torsoNanCounts, handNanCounts, footL_NanCounts, footR_NanCounts);
% 
% figure; imagesc(nanTable)
% 
% allSets = dir('/data/mobi/Audiomaze/makoto/p03_interpolateWithinSubject/*.set');
% allSetNames = {allSets.name}';
% subjIdList  = cellfun(@(x) x(1:7), allSetNames, 'uniformoutput', false);
% unique(subjIdList) % 17 subjects.

            

%%
        
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Interpolate subject-dead rigid body channels with subject-mean template. %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

load /data/common/mobi/Experiments/Audiomaze/makoto/p04_interpolateAcrossSubjects/rigidBodyTemplatesRotated

% Define rigid body LED marker indices.
headIdx   = [1:7];
torsoIdx  = [8:11 21 27];
handIdx   = [16:20];
footR_Idx = [23:26];
footL_Idx = [29:32];

allSets = dir('/data/mobi/Audiomaze/makoto/p03_interpolateWithinSubject/*.set');

for rigidBodyType = 1:5
    switch rigidBodyType
        case 1
            rigidBodyIdx = headIdx;
            rigidBodyTemplatesRotated = headTemplatesRotated;
        case 2
            rigidBodyIdx = torsoIdx;
            rigidBodyTemplatesRotated = torsoTemplatesRotated;
        case 3
            rigidBodyIdx = handIdx;
            rigidBodyTemplatesRotated = handTemplatesRotated;
        case 4
            rigidBodyIdx = footR_Idx;
            rigidBodyTemplatesRotated = footR_TemplatesRotated;
        case 5
            rigidBodyIdx = footL_Idx;
            rigidBodyTemplatesRotated = footL_TemplatesRotated;
    end
    
    %{

    % Plot rigid body template.
    figure
    hold on
    scatter3(squeeze(rigidBodyTemplatesRotated(3,1,:)), squeeze(rigidBodyTemplatesRotated(1,1,:)), squeeze(rigidBodyTemplatesRotated(2,1,:)), 8, [0.8 0.8 1])
    scatter3(squeeze(rigidBodyTemplatesRotated(3,2,:)), squeeze(rigidBodyTemplatesRotated(1,2,:)), squeeze(rigidBodyTemplatesRotated(2,2,:)), 8, [0.8 0.8 1])
    scatter3(squeeze(rigidBodyTemplatesRotated(3,3,:)), squeeze(rigidBodyTemplatesRotated(1,3,:)), squeeze(rigidBodyTemplatesRotated(2,3,:)), 8, [0.8 0.8 1])
    scatter3(squeeze(rigidBodyTemplatesRotated(3,4,:)), squeeze(rigidBodyTemplatesRotated(1,4,:)), squeeze(rigidBodyTemplatesRotated(2,4,:)), 8, [0.8 0.8 1])
    scatter3(squeeze(rigidBodyTemplatesRotated(3,5,:)), squeeze(rigidBodyTemplatesRotated(1,5,:)), squeeze(rigidBodyTemplatesRotated(2,5,:)), 8, [0.8 0.8 1])
    scatter3(squeeze(rigidBodyTemplatesRotated(3,6,:)), squeeze(rigidBodyTemplatesRotated(1,6,:)), squeeze(rigidBodyTemplatesRotated(2,6,:)), 8, [0.8 0.8 1])
    %scatter3(squeeze(rigidBodyTemplatesRotated(3,7,:)), squeeze(rigidBodyTemplatesRotated(1,7,:)), squeeze(rigidBodyTemplatesRotated(2,7,:)), 8, [0.8 0.8 1])
    
    subjectMeanRigidBody = nanmean(rigidBodyTemplatesRotated,3);
    scatter3(subjectMeanRigidBody(3,:), subjectMeanRigidBody(1,:), subjectMeanRigidBody(2,:),   40, [1 0 0], 'fill')

    %}
    
    % Check if channels are missing in each subject.
    rigidBodyNanIdx = isnan(squeeze(sum(sum(rigidBodyTemplatesRotated,1),2)));
    
    if any(rigidBodyNanIdx)
        interpolationIdx = find(rigidBodyNanIdx);
        groupMeanRigidBody = nanmean(rigidBodyTemplatesRotated,3);
        
        for eegIdx = 1:length(interpolationIdx)
            
            % Load data.
            loadName = allSets(interpolationIdx(eegIdx)).name;
            
            if isempty(strfind(loadName, 'subj829'))
                continue
            end
            
            EEG = pop_loadset('filename', loadName, 'filepath', '/data/mobi/Audiomaze/makoto/p03_interpolateWithinSubject', 'loadmode', 'info');
            currentDataForCorrection = EEG.etc.audiomaze.phaseSpaceCorrected(1:3, rigidBodyIdx, :);
            
            % NaN-out the right shoulder.
            if any(strfind(loadName, 'subj829')) & rigidBodyType == 2
                currentDataForCorrection(:,[1 5 6],:) = NaN;
            end
            
            
            % Detect initial NaN block.
            numNonNanChannel = max(sum(~isnan(squeeze(currentDataForCorrection(1,:,:))),1)); % Find max number of channels available at a time.
            [initialNanFrameIdx, endingNanFrameIdx] = findInitialAndEndingNanFrames(currentDataForCorrection, numNonNanChannel);
            nonNanFrameIdx                          = setdiff(1:size(currentDataForCorrection,3), [initialNanFrameIdx endingNanFrameIdx]);
            
            % Obtain indices for available channels and those to be fixed.
            subjectMeanRigidBodyAvailabilityIdx = find(~isnan(groupMeanRigidBody(1,:,1)));
            currentRigidBodyAvailabilityIdx     = find(~isnan(currentDataForCorrection(1,:,nonNanFrameIdx(1))));
            commonChannelIdx    = intersect(subjectMeanRigidBodyAvailabilityIdx, currentRigidBodyAvailabilityIdx);
            channelToBeFixedIdx = setdiff(subjectMeanRigidBodyAvailabilityIdx, commonChannelIdx);
            
            % Perform rigid-body fitting.
            tStart = tic;
            for frameIdxIdx = 1:length(nonNanFrameIdx)
                
                frameIdx = nonNanFrameIdx(frameIdxIdx);
                
                % Show progress.
                if mod(frameIdxIdx,1000)==0
                    tElapsed = toc(tStart);
                    disp(sprintf('%d/%d(%.0f s left)...', frameIdxIdx, length(nonNanFrameIdx), round(tElapsed*(length(nonNanFrameIdx)/1000 - frameIdxIdx/1000))))
                    tStart = tic;
                end
                
                % Obtain the current rigid body.
                currentRigidBody = currentDataForCorrection(:,:,frameIdx);
                
                % Skip frames with all NaN channels.
                if sum(isnan(currentRigidBody(1,:))) == size(currentRigidBody,2)
                    continue
                end
                
                % Fit the template to the current frame.
                [~, ~, rotationMatrix] = rigidBodyTransformation(currentRigidBody(:,commonChannelIdx), groupMeanRigidBody(:,commonChannelIdx));
                identicallyRotatedData = rotationMatrix*groupMeanRigidBody;
                transitionParameter = mean(currentRigidBody(:,commonChannelIdx),2) - mean(identicallyRotatedData(:,commonChannelIdx),2);
                templateFitData     = bsxfun(@plus, identicallyRotatedData, transitionParameter);
                
                        % % Fit rigid body.
                        % [~, ~, rotationMatrix, centroidOffset] = rigidBodyTransformation(currentRigidBody(:,commonChannelIdx), subjectMeanRigidBody(:,commonChannelIdx));
                        % templateFitData = rotationMatrix*subjectMeanRigidBodyCentered;
                        % templateFitData = bsxfun(@plus, templateFitData, centroidOffset);
              
                % Add the missing channel taken from the fit block-mean template.
                currentDataForCorrection(:,channelToBeFixedIdx,frameIdx) = templateFitData(:,channelToBeFixedIdx);
            
                %{
            
                figure
                hold on
                scatter3(currentRigidBody(3,:), currentRigidBody(1,:), currentRigidBody(2,:), 24, [1 0 0], 'fill')
                scatter3(templateFitData(3,:),  templateFitData(1,:),  templateFitData(2,:),  24, [0 0 1], 'fill')
            
                %}
            
            end
            
            % Put it back to EEG.
            EEG.etc.audiomaze.phaseSpaceCorrected(1:3, rigidBodyIdx, :) = currentDataForCorrection;
            
            % Save .set file.
            pop_saveset(EEG, 'filename', EEG.filename, 'filepath', '/data/mobi/Audiomaze/makoto/p04_interpolateAcrossSubjects');
        end
    end
end



%% Check all rigid body interpolation is done.

% 01/05/2017 Makoto. Confirmed to be done successfully!

% Define rigid body LED marker indices.
headIdx   = [1:7];
torsoIdx  = [8:11 21 27];
handIdx   = [16:20];
footR_Idx = [23:26];
footL_Idx = [29:32];

% cd to the data folder.
cd /data/mobi/Audiomaze/makoto/p04_interpolateAcrossSubjects

% List all .set files.
allSets = dir('/data/mobi/Audiomaze/makoto/p04_interpolateAcrossSubjects/*.set');
for eegIdx = 1:length(allSets)
    loadName = allSets(eegIdx).name;
    EEG = pop_loadset('filename', loadName, 'filepath', '/data/mobi/Audiomaze/makoto/p04_interpolateAcrossSubjects', 'loadmode', 'info');
    
    % Check all the rigid bodies.
    headCheck   = any(squeeze(isnan(EEG.etc.audiomaze.phaseSpaceCorrected(1,headIdx,  480*3:end-(480*3)))));
    torsoCheck  = any(squeeze(isnan(EEG.etc.audiomaze.phaseSpaceCorrected(1,torsoIdx, 480*3:end-(480*3)))));
    handCheck   = any(squeeze(isnan(EEG.etc.audiomaze.phaseSpaceCorrected(1,handIdx,  480*3:end-(480*3)))));
    footR_Check = any(squeeze(isnan(EEG.etc.audiomaze.phaseSpaceCorrected(1,footR_Idx,480*3:end-(480*3)))));
    footL_Check = any(squeeze(isnan(EEG.etc.audiomaze.phaseSpaceCorrected(1,footL_Idx,480*3:end-(480*3)))));

%     nanTable = cat(1, headCheck, torsoCheck, handCheck, footL_Check, footR_Check);
%     figure; imagesc(nanTable)
    
    % Exit if any check fails
    if headCheck | torsoCheck | handCheck | footR_Check | footL_Check
        error([loadName ' failed!'])
    end
end


%% 09/08/2020 Makoto.
load /data/common/mobi/Experiments/Audiomaze/makoto/p04_interpolateAcrossSubjects/rigidBodyTemplatesRotated

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Check how many blocks have missing marker in rigid body. %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
headNanCounts   = sum(isnan(reshape(headTemplatesRotated, [size(headTemplatesRotated,1)*size(headTemplatesRotated,2) size(headTemplatesRotated,3)])));
torsoNanCounts  = sum(isnan(reshape(torsoTemplatesRotated, [size(torsoTemplatesRotated,1)*size(torsoTemplatesRotated,2) size(torsoTemplatesRotated,3)])));
handNanCounts   = sum(isnan(reshape(handTemplatesRotated, [size(handTemplatesRotated,1)*size(handTemplatesRotated,2) size(handTemplatesRotated,3)])));
footL_NanCounts = sum(isnan(reshape(footL_TemplatesRotated, [size(footL_TemplatesRotated,1)*size(footL_TemplatesRotated,2) size(footL_TemplatesRotated,3)])));
footR_NanCounts = sum(isnan(reshape(footR_TemplatesRotated, [size(footR_TemplatesRotated,1)*size(footR_TemplatesRotated,2) size(footR_TemplatesRotated,3)])));

nanTable = cat(1, headNanCounts, torsoNanCounts, handNanCounts, footL_NanCounts, footR_NanCounts);

figure; imagesc(nanTable)

allSets = dir('/data/mobi/Audiomaze/makoto/p03_interpolateWithinSubject/*.set');
allSetNames = {allSets.name}';
subjIdList  = cellfun(@(x) x(1:7), allSetNames, 'uniformoutput', false);
unique(subjIdList) % 17 subjects.







currentFootL = squeeze(sum(abs(footL_TemplatesRotated)));
currentFootR = squeeze(sum(abs(footR_TemplatesRotated)));
currentHand  = squeeze(sum(abs(handTemplatesRotated)));
currentHead  = squeeze(sum(abs(headTemplatesRotated)));
currentTorso = squeeze(sum(abs(torsoTemplatesRotated)));

sum(isnan(currentFootL(:)))
sum(isnan(currentFootR(:)))
sum(isnan(currentHand(:)))
sum(isnan(currentHead(:)))
sum(isnan(currentTorso(:)))



allSets = dir('/data/common/mobi/Experiments/Audiomaze/makoto/p04_interpolateAcrossSubjects/*.set');
allSetNames  = {allSets.name}';
allSubjNames = cellfun(@(x) x(1:7), allSetNames, 'uniformoutput', false);
uniqueSubjNames = unique(allSubjNames);
subjWiseMissingMarkerMatrix = zeros(length(uniqueSubjNames), 5);
for uniqueSubjIdx = 1:length(uniqueSubjNames)
    
    currentSubjTrialIdx = find(contains(allSubjNames, uniqueSubjNames(uniqueSubjIdx)));
    
    currentFootL = squeeze(mean(mean(footL_TemplatesRotated(:,:,currentSubjTrialIdx),1),3));
    currentFootR = squeeze(mean(mean(footR_TemplatesRotated(:,:,currentSubjTrialIdx),1),3));
    currentHand  = squeeze(mean(mean(handTemplatesRotated(:,:,currentSubjTrialIdx),1),3));
    currentHead  = squeeze(mean(mean(headTemplatesRotated(:,:,currentSubjTrialIdx),1),3));
    currentTorso = squeeze(mean(mean(torsoTemplatesRotated(:,:,currentSubjTrialIdx),1),3));
    
    subjWiseMissingMarkerMatrix(uniqueSubjIdx,:) = [sum(isnan(currentHead)) sum(isnan(currentTorso)) sum(isnan(currentHand)) sum(isnan(currentFootL)) sum(isnan(currentFootR))];
end

subjWiseMissingMarkerProb = bsxfun(@rdivide, subjWiseMissingMarkerMatrix, [7 6 5 4 4]);

subjWiseMissingMarkerProb(16,:) = [];

sum(subjWiseMissingMarkerProb)/size(subjWiseMissingMarkerProb,1) % This is subject-wise missing! I need trial-wise missinig.
