%% calculate the angle between line from start to zero target and hand location at maximum speed

clearvars -except SUold  ThetaInDegreeold
currentFolder = pwd;
path = currentFolder(1:end-8);
%% Would you like to do extra plotting
p = 1;
%% loading in the summarized data:
data = 'data\processed\';
NS = [1:10,12:24];
if ~exist('SU')
    for si = 1:length(NS)
        s = NS(si);
        clear subject
        % change here to data sample selection
        load(sprintf('%s%ss%03d%s',path,data,s,'.mat'))
        SU{s} = subject;
      end
end
%%
ThetaInDeg = NaN(24,3000);
for s = NS
    clear subject j
    % index of force field trials:
    ind = find(SU{s}.TT ==1|SU{s}.TT ==2|SU{s}.TT ==0);
    x = SU{s}.posx(ind,:)-SU{s}.posx(ind,1);                                % x position of force field trials
    y = SU{s}.posy(ind,:)-SU{s}.posy(ind,1);                                % y position of force field trials
    
    j = NaN(size(ind));
    %% looping over all force field trials
    for i = 1:length(ind)
        speed = SU{s}.speed{ind(i)};
        [~,k] = findpeaks(speed,'MinPeakHeight',15,'MinPeakDistance',50);
        if ~isempty(k)
            %% take first peak
            j(i) = k(1);
        else
            %% take maximum speed
            [~,k] =find(max(speed));
            j(i) = k;
        end
        

    end
    %% calculate the update angle
    u = [0,10,0;0,10,0;0,10,0]'; 
    v = [diag(x(:,j)),diag(y(:,j)),zeros(size(diag(y(:,j))))]'; 
    Theta = NaN(3,length(ind));
    for i = 1:length(ind)
        
        Theta(:,i) = atan2(cross(u(:,1),v(:,i)),dot(u(:,1),v(:,i)));
        ThetaInDeg(s,ind(i)) = rad2deg(Theta(3,i));
       
    end
    if p == 1
        figure(s)
        plot(ThetaInDeg(s,:),'o')
    end

        
end
save(sprintf('%s%sUpdateAngle.mat',path,data),'ThetaInDeg')
