
% DEMCMU35VARGPLVMRECONSTRUCTTAYLOR Reconstruct right leg and body of CMU 35.
%
%	Description:
%	
%	DESC
%	Load a model learned with vargplvm and reconstruct the missing indices
%	using the Taylor framework developed for fgplvm (angle errors)
%	Based on demCmu35fgplvmReconstructTaylor.m
% 	demCmu35vargplvmReconstructTaylor.m SVN version 1767
% 	last update 2011-11-21T23:01:18.121221Z
% See also: vargplvmTaylorAngleErrors
clear
randn('seed', 1e5);
rand('seed', 1e5);
onlyJog = 1;
onlyWalk = 0;
% dynUsed=1;
%
% % Get the sequence numbers.
% [Y, lbls] = lvmLoadData('cmu35WalkJog');
% seq = cumsum(sum(lbls)) - [1:31];

dataSetName = 'cmu35gplvm';

if ~exist('experimentNo') experimentNo = 27; end

% % load data
% [Y, lbls, Ytest, lblstest] = lvmLoadData(dataSetName);



dataSetName = 'cmu35gplvm';

capName = dataSetName;
capName(1) = upper(capName(1));

fileName=['dem' capName 'Varmgplvm' num2str(experimentNo)];
% load data


% [Y, lbls, Ytest, lblstest] = lvmLoadData(dataSetName);

if exist('onlyJog')&& onlyJog
    load([dataSetName 'JogSub']);
elseif exist('onlyWalk') && onlyWalk
    load([dataSetName 'Walk']);
end
% %%% Remove some sequences
% seqFrom=2;
% seqEnd=4;
%
%
% if seqFrom ~= 1
%     Yfrom = seq(seqFrom-1)+1;
% else
%     Yfrom = 1;
% end
% Yend=seq(seqEnd);
% Y=Y(Yfrom:Yend,:);
% seq=seq(seqFrom:seqEnd);
% seq=seq - ones(1,length(seq)).*(Yfrom-1);


origBias = mean(Y);
origScale = 1./sqrt(var(Y));
% %scale = ones(size(scale));
% Y = Y - repmat(origBias, size(Y, 1), 1);
% Ytest = Ytest - repmat(origBias, size(Ytest, 1), 1);
% Y = Y.*repmat(origScale, size(Y, 1), 1);
% Ytest = Ytest.*repmat(origScale, size(Ytest, 1), 1);

% REMOVE LEG TEST
% Indices associated with right leg.
legInd = [8:14];

% Indices associated with upper body.
bodyInd = [21:50];


dt=0.05;
Ytest = Ytests{1};
timeStampsTest = ([0:size(Ytest,1)-1].*dt)';


%for experimentNo = 1:3;

% Load saved model.
%load(['dem' capName num2str(experimentNo) '.mat']);
load(fileName); % From demCmu35gplvmVargplvm1.m
fprintf(1,'# Loaded %s\n',fileName);
if isfield(model, 'dynamics') & ~isempty(model.dynamics)
    fprintf(1,'# Dynamics kernel:');
    kernDisplay(model.dynamics.kern);
    model.dynamics.t_star = timeStampsTest;
    kernName = model.dynamics.kern.comp{1}.type;
else
    kernName = 'static';
end
fprintf(1,'# experimentNo=%d\n',experimentNo);
fprintf(1,'# Reconstruction Iters=%d\n',model.reconstrIters);

model.reconstrIters = 2000; %%%TEMP


% legInd=[1:50];
startInd = 63;

if exist('onlyJog')&& onlyJog
    startInd=10;
end
if exist('onlyWalk')&& onlyWalk
    startInd=1;
end
LegErrs = vargplvmTaylorAngleErrors(model, Y, Ytest, startInd, origBias, origScale, legInd, [dataSetName ...
    'Leg'], experimentNo);

save([fileName 'results.mat'], 'LegErrs');

%----
figPath = ['Results/CMU/' num2str(experimentNo) kernName '/Leg/'];
try
    saveAllOpenFigures(figPath, [],1)
catch 
    %
end
%----

LegErrs



% bodyErrs = vargplvmTaylorAngleErrors(model, Y, Ytest, startInd, origBias, origScale, bodyInd, [dataSetName ...
%     'Body'], experimentNo);
% 
% save(['dem' capName 'ReconstructBody' num2str(experimentNo) '.mat'], 'bodyErrs');
% 
% 
% figPath = ['Results/CMU/' num2str(experimentNo) kernName '/Body/'];
% try
%     saveAllOpenFigures(figPath, [],1)
% catch 
%     %
% end
% 
% %----
% bodyErrs

% bar(model.kern.comp{1}.inputScales)


%end


